Merge remote-tracking branch 'upstream/master' into port_honda_hrv_2023

pull/27731/head
Shane Smiskol 2 years ago
commit 137662ca55
  1. 2
      .github/workflows/selfdrive_tests.yaml
  2. 2
      cereal
  3. 2
      panda
  4. 2
      selfdrive/boardd/boardd.cc
  5. 28
      selfdrive/car/gm/interface.py
  6. 4
      selfdrive/car/gm/values.py
  7. 4
      selfdrive/car/honda/carcontroller.py
  8. 8
      selfdrive/car/honda/hondacan.py
  9. 3
      selfdrive/car/honda/interface.py
  10. 16
      selfdrive/car/interfaces.py
  11. 2
      selfdrive/car/toyota/values.py
  12. 10
      selfdrive/controls/lib/drive_helpers.py
  13. 6
      selfdrive/controls/lib/latcontrol_torque.py
  14. 17
      selfdrive/locationd/locationd.cc
  15. 1
      selfdrive/locationd/locationd.h
  16. 2
      selfdrive/test/process_replay/ref_commit
  17. 1
      selfdrive/ui/qt/offroad/settings.cc
  18. 2
      tools/cabana/SConscript
  19. 12
      tools/cabana/binaryview.cc
  20. 3
      tools/cabana/binaryview.h
  21. 25
      tools/cabana/chartswidget.cc
  22. 3
      tools/cabana/dbc/dbcmanager.cc
  23. 2
      tools/cabana/dbc/dbcmanager.h
  24. 2
      tools/cabana/detailwidget.h
  25. 2
      tools/cabana/settings.cc
  26. 3
      tools/cabana/settings.h
  27. 58
      tools/cabana/signalview.cc
  28. 5
      tools/cabana/signalview.h
  29. 4
      tools/cabana/util.h
  30. 13
      tools/cabana/videowidget.cc
  31. 2
      tools/ubuntu_setup.sh

@ -261,7 +261,7 @@ jobs:
name: process_replay_diff.txt
path: selfdrive/test/process_replay/diff.txt
- name: Upload reference logs
if: ${{ failure() && steps.print-diff.outcome == 'success' && github.event_name == 'pull_request' && github.repository == 'commaai/openpilot' && env.AZURE_TOKEN != '' }}
if: ${{ failure() && steps.print-diff.outcome == 'success' && github.repository == 'commaai/openpilot' && env.AZURE_TOKEN != '' }}
run: |
${{ env.RUN }} "CI=1 AZURE_TOKEN='$AZURE_TOKEN' python selfdrive/test/process_replay/test_processes.py -j$(nproc) --upload-only"
- name: "Upload coverage to Codecov"

@ -1 +1 @@
Subproject commit 1f0d21df7307228218a4b874a3fdcd819b1558af
Subproject commit d70d215de6c584f671272d2de2f46a4f778e9f14

@ -1 +1 @@
Subproject commit db6c50cd09f773c231f962fc0e31b4612c572b08
Subproject commit 12b9b65985edf7207a1dfd48de0b714192e6e55d

@ -418,7 +418,7 @@ std::optional<bool> send_panda_states(PubMaster *pm, const std::vector<Panda *>
size_t j = 0;
for (size_t f = size_t(cereal::PandaState::FaultType::RELAY_MALFUNCTION);
f <= size_t(cereal::PandaState::FaultType::INTERRUPT_RATE_EXTI); f++) {
f <= size_t(cereal::PandaState::FaultType::SIREN_MALFUNCTION); f++) {
if (fault_bits.test(f)) {
faults.set(j, cereal::PandaState::FaultType(f));
j++;

@ -1,13 +1,14 @@
#!/usr/bin/env python3
from cereal import car
from math import fabs
from math import fabs, exp
from panda import Panda
from common.conversions import Conversions as CV
from selfdrive.car import STD_CARGO_KG, create_button_event, scale_tire_stiffness, get_safety_config
from selfdrive.car.gm.radar_interface import RADAR_HEADER_MSG
from selfdrive.car.gm.values import CAR, CruiseButtons, CarControllerParams, EV_CAR, CAMERA_ACC_CAR, CanBus
from selfdrive.car.interfaces import CarInterfaceBase
from selfdrive.car.interfaces import CarInterfaceBase, TorqueFromLateralAccelCallbackType, FRICTION_THRESHOLD
from selfdrive.controls.lib.drive_helpers import get_friction
ButtonType = car.CarState.ButtonEvent.Type
EventName = car.CarEvent.EventName
@ -44,6 +45,29 @@ class CarInterface(CarInterfaceBase):
else:
return CarInterfaceBase.get_steer_feedforward_default
@staticmethod
def torque_from_lateral_accel_bolt(lateral_accel_value: float, torque_params: car.CarParams.LateralTorqueTuning,
lateral_accel_error: float, lateral_accel_deadzone: float, friction_compensation: bool) -> float:
friction = get_friction(lateral_accel_error, lateral_accel_deadzone, FRICTION_THRESHOLD, torque_params, friction_compensation)
def sig(val):
return 1 / (1 + exp(-val)) - 0.5
# The "lat_accel vs torque" relationship is assumed to be the sum of "sigmoid + linear" curves
# An important thing to consider is that the slope at 0 should be > 0 (ideally >1)
# This has big effect on the stability about 0 (noise when going straight)
# ToDo: To generalize to other GMs, explore tanh function as the nonlinear
a, b, c, _ = [2.6531724862969748, 1.0, 0.1919764879840985, 0.009054123646805178] # weights computed offline
steer_torque = (sig(lateral_accel_value * a) * b) + (lateral_accel_value * c)
return float(steer_torque) + friction
def torque_from_lateral_accel(self) -> TorqueFromLateralAccelCallbackType:
if self.CP.carFingerprint == CAR.BOLT_EUV:
return self.torque_from_lateral_accel_bolt
else:
return self.torque_from_lateral_accel_linear
@staticmethod
def _get_params(ret, candidate, fingerprint, car_fw, experimental_long):
ret.carName = "gm"

@ -14,8 +14,8 @@ class CarControllerParams:
STEER_STEP = 3 # Active control frames per command (~33hz)
INACTIVE_STEER_STEP = 10 # Inactive control frames per command (10hz)
STEER_DELTA_UP = 10 # Delta rates require review due to observed EPS weakness
STEER_DELTA_DOWN = 25
STEER_DRIVER_ALLOWANCE = 50
STEER_DELTA_DOWN = 15
STEER_DRIVER_ALLOWANCE = 65
STEER_DRIVER_MULTIPLIER = 4
STEER_DRIVER_FACTOR = 100
NEAR_STOP_BRAKE_PHASE = 0.5 # m/s

@ -131,6 +131,7 @@ class CarController:
self.brake_last = 0.
self.apply_brake_last = 0
self.last_pump_ts = 0.
self.stopping_counter = 0
self.accel = 0.0
self.speed = 0.0
@ -232,8 +233,9 @@ class CarController:
self.gas = interp(accel, self.params.BOSCH_GAS_LOOKUP_BP, self.params.BOSCH_GAS_LOOKUP_V)
stopping = actuators.longControlState == LongCtrlState.stopping
self.stopping_counter = self.stopping_counter + 1 if stopping else 0
can_sends.extend(hondacan.create_acc_commands(self.packer, CC.enabled, CC.longActive, self.accel, self.gas,
stopping, self.CP.carFingerprint))
self.stopping_counter, self.CP.carFingerprint))
else:
apply_brake = clip(self.brake_last - wind_brake, 0.0, 1.0)
apply_brake = int(clip(apply_brake * self.params.NIDEC_BRAKE_MAX, 0, self.params.NIDEC_BRAKE_MAX - 1))

@ -51,7 +51,7 @@ def create_brake_command(packer, apply_brake, pump_on, pcm_override, pcm_cancel_
return packer.make_can_msg("BRAKE_COMMAND", bus, values)
def create_acc_commands(packer, enabled, active, accel, gas, stopping, car_fingerprint):
def create_acc_commands(packer, enabled, active, accel, gas, stopping_counter, car_fingerprint):
commands = []
bus = get_pt_bus(car_fingerprint)
min_gas_accel = CarControllerParams.BOSCH_GAS_LOOKUP_BP[0]
@ -60,8 +60,8 @@ def create_acc_commands(packer, enabled, active, accel, gas, stopping, car_finge
gas_command = gas if active and accel > min_gas_accel else -30000
accel_command = accel if active else 0
braking = 1 if active and accel < min_gas_accel else 0
standstill = 1 if active and stopping else 0
standstill_release = 1 if active and not stopping else 0
standstill = 1 if active and stopping_counter > 0 else 0
standstill_release = 1 if active and stopping_counter == 0 else 0
# common ACC_CONTROL values
acc_control_values = {
@ -72,7 +72,7 @@ def create_acc_commands(packer, enabled, active, accel, gas, stopping, car_finge
if car_fingerprint in HONDA_BOSCH_RADARLESS:
acc_control_values.update({
"CONTROL_ON": enabled,
"IDLESTOP_ALLOW": 0, # disallows idle stop
"IDLESTOP_ALLOW": stopping_counter > 200, # allow idle stop after 4 seconds (50 Hz)
})
else:
acc_control_values.update({

@ -74,8 +74,7 @@ class CarInterface(CarInterfaceBase):
ret.longitudinalTuning.kiV = [0.05]
ret.longitudinalActuatorDelayUpperBound = 0.5 # s
if candidate in HONDA_BOSCH_RADARLESS:
ret.stopAccel = -3.5 # stock allows -4.0 but limited by safety model
ret.stoppingDecelRate = 2.0 # reach brake quickly after enabling
ret.stopAccel = CarControllerParams.BOSCH_ACCEL_MIN # stock uses -4.0 m/s^2 once stopped but limited by safety model
else:
# default longitudinal tuning for all hondas
ret.longitudinalTuning.kpBP = [0., 5., 35.]

@ -8,10 +8,10 @@ from cereal import car
from common.basedir import BASEDIR
from common.conversions import Conversions as CV
from common.kalman.simple_kalman import KF1D
from common.numpy_fast import clip, interp
from common.numpy_fast import clip
from common.realtime import DT_CTRL
from selfdrive.car import apply_hysteresis, gen_empty_fingerprint, scale_rot_inertia, scale_tire_stiffness
from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, apply_center_deadzone
from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, get_friction
from selfdrive.controls.lib.events import Events
from selfdrive.controls.lib.vehicle_model import VehicleModel
@ -131,15 +131,11 @@ class CarInterfaceBase(ABC):
return self.get_steer_feedforward_default
@staticmethod
def torque_from_lateral_accel_linear(lateral_accel_value, torque_params, lateral_accel_error, lateral_accel_deadzone, friction_compensation):
def torque_from_lateral_accel_linear(lateral_accel_value: float, torque_params: car.CarParams.LateralTorqueTuning,
lateral_accel_error: float, lateral_accel_deadzone: float, friction_compensation: bool) -> float:
# The default is a linear relationship between torque and lateral acceleration (accounting for road roll and steering friction)
friction_interp = interp(
apply_center_deadzone(lateral_accel_error, lateral_accel_deadzone),
[-FRICTION_THRESHOLD, FRICTION_THRESHOLD],
[-torque_params.friction, torque_params.friction]
)
friction = friction_interp if friction_compensation else 0.0
return (lateral_accel_value / torque_params.latAccelFactor) + friction
friction = get_friction(lateral_accel_error, lateral_accel_deadzone, FRICTION_THRESHOLD, torque_params, friction_compensation)
return (lateral_accel_value / float(torque_params.latAccelFactor)) + friction
def torque_from_lateral_accel(self) -> TorqueFromLateralAccelCallbackType:
return self.torque_from_lateral_accel_linear

@ -327,6 +327,7 @@ FW_VERSIONS = {
CAR.AVALON_TSS2: {
(Ecu.abs, 0x7b0, None): [
b'\x01F152607240\x00\x00\x00\x00\x00\x00',
b'\x01F152607250\x00\x00\x00\x00\x00\x00',
b'\x01F152607280\x00\x00\x00\x00\x00\x00',
],
(Ecu.eps, 0x7a1, None): [
@ -334,6 +335,7 @@ FW_VERSIONS = {
],
(Ecu.engine, 0x700, None): [
b'\x01896630742000\x00\x00\x00\x00',
b'\x01896630743000\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x750, 0xf): [
b'\x018821F6201200\x00\x00\x00\x00',

@ -190,3 +190,13 @@ def get_lag_adjusted_curvature(CP, v_ego, psis, curvatures, curvature_rates):
current_curvature_desired + max_curvature_rate * DT_MDL)
return safe_desired_curvature, safe_desired_curvature_rate
def get_friction(lateral_accel_error: float, lateral_accel_deadzone: float, friction_threshold: float, torque_params: car.CarParams.LateralTorqueTuning, friction_compensation: bool) -> float:
friction_interp = interp(
apply_center_deadzone(lateral_accel_error, lateral_accel_deadzone),
[-friction_threshold, friction_threshold],
[-torque_params.friction, torque_params.friction]
)
friction = float(friction_interp) if friction_compensation else 0.0
return friction

@ -61,10 +61,12 @@ class LatControlTorque(LatControl):
low_speed_factor = interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y)**2
setpoint = desired_lateral_accel + low_speed_factor * desired_curvature
measurement = actual_lateral_accel + low_speed_factor * actual_curvature
error = setpoint - measurement
gravity_adjusted_lateral_accel = desired_lateral_accel - params.roll * ACCELERATION_DUE_TO_GRAVITY
pid_log.error = self.torque_from_lateral_accel(error, self.torque_params, error,
torque_from_setpoint = self.torque_from_lateral_accel(setpoint, self.torque_params, setpoint,
lateral_accel_deadzone, friction_compensation=False)
torque_from_measurement = self.torque_from_lateral_accel(measurement, self.torque_params, measurement,
lateral_accel_deadzone, friction_compensation=False)
pid_log.error = torque_from_setpoint - torque_from_measurement
ff = self.torque_from_lateral_accel(gravity_adjusted_lateral_accel, self.torque_params,
desired_lateral_accel - actual_lateral_accel,
lateral_accel_deadzone, friction_compensation=True)

@ -27,7 +27,6 @@ const double MAX_FILTER_REWIND_TIME = 0.8; // s
// They should be replaced with synced time from a real clock
const double GPS_QUECTEL_SENSOR_TIME_OFFSET = 0.630; // s
const double GPS_UBLOX_SENSOR_TIME_OFFSET = 0.095; // s
const float GPS_MUL_FACTOR = 10.0;
const float GPS_POS_STD_THRESHOLD = 50.0;
const float GPS_VEL_STD_THRESHOLD = 5.0;
const float GPS_POS_ERROR_RESET_THRESHOLD = 300.0;
@ -326,8 +325,14 @@ void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::R
VectorXd ecef_pos = this->converter->ned2ecef({ 0.0, 0.0, 0.0 }).to_vector();
VectorXd ecef_vel = this->converter->ned2ecef({ log.getVNED()[0], log.getVNED()[1], log.getVNED()[2] }).to_vector() - ecef_pos;
MatrixXdr ecef_pos_R = Vector3d::Constant(std::pow(GPS_MUL_FACTOR * log.getAccuracy(),2) + std::pow(GPS_MUL_FACTOR * log.getVerticalAccuracy(),2)).asDiagonal();
MatrixXdr ecef_vel_R = Vector3d::Constant(std::pow(GPS_MUL_FACTOR * log.getSpeedAccuracy(), 2)).asDiagonal();
float ecef_pos_std;
if (ublox_available) {
ecef_pos_std = std::sqrt(std::pow(log.getAccuracy(), 2) + std::pow(log.getVerticalAccuracy(), 2));
} else {
ecef_pos_std = std::sqrt(3 * std::pow(log.getVerticalAccuracy(), 2));
}
MatrixXdr ecef_pos_R = Vector3d::Constant(std::pow(this->gps_std_factor * ecef_pos_std, 2)).asDiagonal();
MatrixXdr ecef_vel_R = Vector3d::Constant(std::pow(this->gps_std_factor * log.getSpeedAccuracy(), 2)).asDiagonal();
this->unix_timestamp_millis = log.getUnixTimestampMillis();
double gps_est_error = (this->kf->get_x().segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START) - ecef_pos).norm();
@ -377,14 +382,14 @@ void Localizer::handle_gnss(double current_time, const cereal::GnssMeasurements:
// indexed at 0 cause all std values are the same MAE
auto ecef_pos_std = log.getPositionECEF().getStd()[0];
MatrixXdr ecef_pos_R = Vector3d::Constant(pow(GPS_MUL_FACTOR*ecef_pos_std, 2)).asDiagonal();
MatrixXdr ecef_pos_R = Vector3d::Constant(pow(this->gps_std_factor*ecef_pos_std, 2)).asDiagonal();
auto ecef_vel_v = log.getVelocityECEF().getValue();
VectorXd ecef_vel = Vector3d(ecef_vel_v[0], ecef_vel_v[1], ecef_vel_v[2]);
// indexed at 0 cause all std values are the same MAE
auto ecef_vel_std = log.getVelocityECEF().getStd()[0];
MatrixXdr ecef_vel_R = Vector3d::Constant(pow(GPS_MUL_FACTOR*ecef_vel_std, 2)).asDiagonal();
MatrixXdr ecef_vel_R = Vector3d::Constant(pow(this->gps_std_factor*ecef_vel_std, 2)).asDiagonal();
double gps_est_error = (this->kf->get_x().segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START) - ecef_pos).norm();
@ -661,8 +666,10 @@ int Localizer::locationd_thread() {
const char* gps_location_socket;
if (ublox_available) {
gps_location_socket = "gpsLocationExternal";
this->gps_std_factor = 10.0;
} else {
gps_location_socket = "gpsLocation";
this->gps_std_factor = 2.0;
}
const std::initializer_list<const char *> service_list = {gps_location_socket, "cameraOdometry", "liveCalibration",
"carState", "carParams", "accelerometer", "gyroscope"};

@ -84,4 +84,5 @@ private:
std::map<std::string, double> observation_values_invalid;
bool standstill = true;
int32_t orientation_reset_count = 0;
float gps_std_factor;
};

@ -1 +1 @@
50f1e873095fe2462d2aadb9c401bda76759c01c
1bb3f665191e1b75c1b786f60e76d51b274f98ae

@ -193,6 +193,7 @@ DevicePanel::DevicePanel(SettingsWindow *parent) : ListWidget(parent) {
connect(resetCalibBtn, &ButtonControl::clicked, [&]() {
if (ConfirmationDialog::confirm(tr("Are you sure you want to reset calibration?"), tr("Reset"), this)) {
params.remove("CalibrationParams");
params.remove("LiveTorqueParameters");
}
});
addItem(resetCalibBtn);

@ -28,7 +28,7 @@ cabana_env.Depends(assets, Glob('/assets/*', exclude=[assets, assets_src, "asset
prev_moc_path = cabana_env['QT_MOCHPREFIX']
cabana_env['QT_MOCHPREFIX'] = os.path.dirname(prev_moc_path) + '/cabana/moc_'
cabana_lib = cabana_env.Library("cabana_lib", ['mainwin.cc', 'streams/livestream.cc', 'streams/abstractstream.cc', 'streams/replaystream.cc', 'binaryview.cc', 'chartswidget.cc', 'historylog.cc', 'videowidget.cc', 'signaledit.cc',
cabana_lib = cabana_env.Library("cabana_lib", ['mainwin.cc', 'streams/livestream.cc', 'streams/abstractstream.cc', 'streams/replaystream.cc', 'binaryview.cc', 'chartswidget.cc', 'historylog.cc', 'videowidget.cc', 'signalview.cc',
'dbc/dbc.cc', 'dbc/dbcfile.cc', 'dbc/dbcmanager.cc',
'commands.cc', 'messageswidget.cc', 'route.cc', 'settings.cc', 'util.cc', 'detailwidget.cc', 'tools/findsimilarbits.cc'], LIBS=cabana_libs, FRAMEWORKS=base_frameworks)
cabana_env.Program('_cabana', ['cabana.cc', cabana_lib, assets], LIBS=cabana_libs, FRAMEWORKS=base_frameworks)

@ -11,7 +11,7 @@
#include <QToolTip>
#include "tools/cabana/commands.h"
#include "tools/cabana/signaledit.h"
#include "tools/cabana/signalview.h"
// BinaryView
@ -273,6 +273,10 @@ void BinaryViewModel::refresh() {
row_count = can->lastMessage(msg_id).dat.size();
items.resize(row_count * column_count);
}
int valid_rows = std::min(can->lastMessage(msg_id).dat.size(), row_count);
for (int i = 0; i < valid_rows * column_count; ++i) {
items[i].valid = true;
}
endResetModel();
updateState();
}
@ -307,9 +311,6 @@ void BinaryViewModel::updateState() {
items[i * column_count + 8].val = toHex(binary[i]);
items[i * column_count + 8].bg_color = last_msg.colors[i];
}
for (int i = binary.size() * column_count; i < items.size(); ++i) {
items[i].val = "-";
}
for (int i = 0; i < items.size(); ++i) {
if (i >= prev_items.size() || prev_items[i].val != items[i].val || prev_items[i].bg_color != items[i].bg_color) {
@ -376,6 +377,9 @@ void BinaryItemDelegate::paint(QPainter *painter, const QStyleOptionViewItem &op
}
}
if (!item->valid) {
painter->fillRect(option.rect, QBrush(Qt::darkGray, Qt::BDiagPattern));
}
painter->drawText(option.rect, Qt::AlignCenter, item->val);
if (item->is_msb || item->is_lsb) {
painter->setFont(small_font);

@ -42,8 +42,9 @@ public:
QColor bg_color = QColor(102, 86, 169, 0);
bool is_msb = false;
bool is_lsb = false;
QString val = "-";
QString val;
QList<const cabana::Signal *> sigs;
bool valid = false;
};
std::vector<Item> items;

@ -189,7 +189,7 @@ void ChartsWidget::setMaxChartRange(int value) {
void ChartsWidget::updateToolBar() {
title_label->setText(tr("Charts: %1").arg(charts.size()));
columns_action->setText(tr("Column: %1").arg(column_count));
range_lb->setText(QString("Range: %1:%2 ").arg(max_chart_range / 60, 2, 10, QLatin1Char('0')).arg(max_chart_range % 60, 2, 10, QLatin1Char('0')));
range_lb->setText(QString("Range: %1 ").arg(utils::formatSeconds(max_chart_range)));
range_lb_action->setVisible(!is_zoomed);
range_slider_action->setVisible(!is_zoomed);
undo_zoom_action->setVisible(is_zoomed);
@ -536,14 +536,16 @@ void ChartView::updateSeriesPoints() {
for (auto &s : sigs) {
auto begin = std::lower_bound(s.vals.begin(), s.vals.end(), axis_x->min(), xLessThan);
auto end = std::lower_bound(begin, s.vals.end(), axis_x->max(), xLessThan);
int num_points = std::max<int>(end - begin, 1);
int pixels_per_point = width() / num_points;
if (series_type == SeriesType::Scatter) {
((QScatterSeries *)s.series)->setMarkerSize(std::clamp(pixels_per_point / 3, 2, 8) * devicePixelRatioF());
} else {
s.series->setPointsVisible(pixels_per_point > 20);
if (begin != end) {
int num_points = std::max<int>((end - begin), 1);
QPointF right_pt = end == s.vals.end() ? s.vals.back() : *end;
double pixels_per_point = (chart()->mapToPosition(right_pt).x() - chart()->mapToPosition(*begin).x()) / num_points;
if (series_type == SeriesType::Scatter) {
((QScatterSeries *)s.series)->setMarkerSize(std::clamp(pixels_per_point / 2.0, 2.0, 8.0) * devicePixelRatioF());
} else {
s.series->setPointsVisible(pixels_per_point > 20);
}
}
}
}
@ -711,8 +713,7 @@ void ChartView::mouseReleaseEvent(QMouseEvent *event) {
if (rubber->width() <= 0) {
// no rubber dragged, seek to mouse position
can->seekTo(min);
} else if ((max_rounded - min_rounded) >= 0.5) {
// zoom in if selected range is greater than 0.5s
} else if (rubber->width() > 10) {
emit zoomIn(min_rounded, max_rounded);
} else {
scene()->invalidate({}, QGraphicsScene::ForegroundLayer);
@ -847,8 +848,8 @@ void ChartView::drawForeground(QPainter *painter, const QRectF &rect) {
if (s.series->useOpenGL() && s.series->isVisible() && s.series->pointsVisible()) {
auto first = std::lower_bound(s.vals.begin(), s.vals.end(), axis_x->min(), xLessThan);
auto last = std::lower_bound(first, s.vals.end(), axis_x->max(), xLessThan);
painter->setBrush(s.series->color());
for (auto it = first; it != last; ++it) {
painter->setBrush(s.series->color());
painter->drawEllipse(chart()->mapToPosition(*it), 4, 4);
}
}

@ -14,7 +14,8 @@ bool DBCManager::open(SourceSet s, const QString &dbc_file_name, QString *error)
// Check if file is already open, and merge sources
if (dbc_file->filename == dbc_file_name) {
ss |= s;
dbc_files[i] = {ss | s, dbc_file};
emit DBCFileChanged();
return true;
}

@ -1,6 +1,8 @@
#pragma once
#include <map>
#include <optional>
#include <QList>
#include <QMetaType>
#include <QObject>

@ -8,7 +8,7 @@
#include "tools/cabana/binaryview.h"
#include "tools/cabana/chartswidget.h"
#include "tools/cabana/historylog.h"
#include "tools/cabana/signaledit.h"
#include "tools/cabana/signalview.h"
class EditMessageDialog : public QDialog {
public:

@ -27,6 +27,7 @@ void Settings::save() {
s.setValue("recent_files", recent_files);
s.setValue("message_header_state", message_header_state);
s.setValue("chart_series_type", chart_series_type);
s.setValue("sparkline_range", sparkline_range);
}
void Settings::load() {
@ -44,6 +45,7 @@ void Settings::load() {
recent_files = s.value("recent_files").toStringList();
message_header_state = s.value("message_header_state").toByteArray();
chart_series_type = s.value("chart_series_type", 0).toInt();
sparkline_range = s.value("sparkline_range", 15).toInt();
}
// SettingsDlg

@ -17,8 +17,9 @@ public:
int max_cached_minutes = 30;
int chart_height = 200;
int chart_column_count = 1;
int chart_range = 3 * 60; // e minutes
int chart_range = 3 * 60; // 3 minutes
int chart_series_type = 0;
int sparkline_range = 15; // 15 seconds
QString last_dir;
QString last_route_dir;
QByteArray geometry;

@ -1,4 +1,4 @@
#include "tools/cabana/signaledit.h"
#include "tools/cabana/signalview.h"
#include <QApplication>
#include <QCompleter>
@ -62,10 +62,21 @@ void SignalModel::updateState(const QHash<MessageId, CanData> *msgs) {
auto &dat = can->lastMessage(msg_id).dat;
int row = 0;
for (auto item : root->children) {
item->sig_val = QString::number(get_raw_value((uint8_t *)dat.constData(), dat.size(), *item->sig), 'f', item->sig->precision);
double value = get_raw_value((uint8_t *)dat.constData(), dat.size(), *item->sig);
item->sig_val = QString::number(value, 'f', item->sig->precision);
// Show unit
if (!item->sig->unit.isEmpty()) {
item->sig_val += " " + item->sig->unit;
}
// Show enum string
for (auto &[val, desc] : item->sig->val_desc) {
if (std::abs(value - val.toInt()) < 1e-6) {
item->sig_val = desc;
}
}
emit dataChanged(index(row, 1), index(row, 1), {Qt::DisplayRole});
++row;
}
@ -366,17 +377,15 @@ void SignalItemDelegate::paint(QPainter *painter, const QStyleOptionViewItem &op
void SignalItemDelegate::drawSparkline(QPainter *painter, const QStyleOptionViewItem &option, const QModelIndex &index) const {
static std::vector<QPointF> points;
// TODO: get seconds from settings.
const uint64_t chart_seconds = 15; // seconds
const auto &msg_id = ((SignalView *)parent())->msg_id;
const auto &msgs = can->events().at(msg_id);
uint64_t ts = (can->lastMessage(msg_id).ts + can->routeStartTime()) * 1e9;
auto first = std::lower_bound(msgs.cbegin(), msgs.cend(), CanEvent{.mono_time = (uint64_t)std::max<int64_t>(ts - chart_seconds * 1e9, 0)});
if (first != msgs.cend()) {
auto first = std::lower_bound(msgs.cbegin(), msgs.cend(), CanEvent{.mono_time = (uint64_t)std::max<int64_t>(ts - settings.sparkline_range * 1e9, 0)});
auto last = std::upper_bound(first, msgs.cend(), CanEvent{.mono_time = ts});
if (first != last) {
double min = std::numeric_limits<double>::max();
double max = std::numeric_limits<double>::lowest();
const auto sig = ((SignalModel::Item *)index.internalPointer())->sig;
auto last = std::lower_bound(first, msgs.cend(), CanEvent{.mono_time = ts});
points.clear();
for (auto it = first; it != last; ++it) {
double value = get_raw_value(it->dat, it->size, *sig);
@ -391,7 +400,7 @@ void SignalItemDelegate::drawSparkline(QPainter *painter, const QStyleOptionView
int h_margin = option.widget->style()->pixelMetric(QStyle::PM_FocusFrameHMargin);
int v_margin = std::max(option.widget->style()->pixelMetric(QStyle::PM_FocusFrameVMargin) + 2, 4);
const double xscale = (option.rect.width() - 175.0 * option.widget->devicePixelRatioF() - h_margin * 2) / chart_seconds;
const double xscale = (option.rect.width() - 175.0 - h_margin * 2) / settings.sparkline_range;
const double yscale = (option.rect.height() - v_margin * 2) / (max - min);
const int left = option.rect.left();
const int top = option.rect.top() + v_margin;
@ -401,6 +410,13 @@ void SignalItemDelegate::drawSparkline(QPainter *painter, const QStyleOptionView
}
painter->setPen(getColor(sig));
painter->drawPolyline(points.data(), points.size());
if ((points.back().x() - points.front().x()) / points.size() > 10) {
painter->setPen(Qt::NoPen);
painter->setBrush(getColor(sig));
for (const auto &pt : points) {
painter->drawEllipse(pt, 2, 2);
}
}
}
}
@ -451,6 +467,17 @@ SignalView::SignalView(ChartsWidget *charts, QWidget *parent) : charts(charts),
filter_edit->setPlaceholderText(tr("filter signals"));
hl->addWidget(filter_edit);
hl->addStretch(1);
// WARNING: increasing the maximum range can result in severe performance degradation.
// 30s is a reasonable value at present.
const int max_range = 30; // 30s
settings.sparkline_range = std::clamp(settings.sparkline_range, 1, max_range);
hl->addWidget(sparkline_label = new QLabel());
hl->addWidget(sparkline_range_slider = new QSlider(Qt::Horizontal, this));
sparkline_range_slider->setRange(1, max_range);
sparkline_range_slider->setValue(settings.sparkline_range);
sparkline_range_slider->setToolTip(tr("Sparkline time range"));
auto collapse_btn = toolButton("dash-square", tr("Collapse All"));
collapse_btn->setIconSize({12, 12});
hl->addWidget(collapse_btn);
@ -473,8 +500,10 @@ SignalView::SignalView(ChartsWidget *charts, QWidget *parent) : charts(charts),
main_layout->setSpacing(0);
main_layout->addWidget(title_bar);
main_layout->addWidget(tree);
updateToolBar();
QObject::connect(filter_edit, &QLineEdit::textEdited, model, &SignalModel::setFilter);
QObject::connect(sparkline_range_slider, &QSlider::valueChanged, this, &SignalView::setSparklineRange);
QObject::connect(collapse_btn, &QPushButton::clicked, tree, &QTreeView::collapseAll);
QObject::connect(tree, &QAbstractItemView::clicked, this, &SignalView::rowClicked);
QObject::connect(tree, &QTreeView::viewportEntered, [this]() { emit highlight(nullptr); });
@ -521,7 +550,7 @@ void SignalView::rowsChanged() {
});
}
}
signal_count_lb->setText(tr("Signals: %1").arg(model->rowCount()));
updateToolBar();
updateChartState();
}
@ -569,6 +598,17 @@ void SignalView::signalHovered(const cabana::Signal *sig) {
}
}
void SignalView::updateToolBar() {
signal_count_lb->setText(tr("Signals: %1").arg(model->rowCount()));
sparkline_label->setText(QString("Range: %1 ").arg(utils::formatSeconds(settings.sparkline_range)));
}
void SignalView::setSparklineRange(int value) {
settings.sparkline_range = value;
updateToolBar();
model->updateState(nullptr);
}
void SignalView::leaveEvent(QEvent *event) {
emit highlight(nullptr);
QWidget::leaveEvent(event);

@ -3,6 +3,7 @@
#include <QAbstractItemModel>
#include <QLabel>
#include <QLineEdit>
#include <QSlider>
#include <QStyledItemDelegate>
#include <QTableWidget>
#include <QTreeView>
@ -109,6 +110,8 @@ signals:
private:
void rowsChanged();
void leaveEvent(QEvent *event);
void updateToolBar();
void setSparklineRange(int value);
struct TreeView : public QTreeView {
TreeView(QWidget *parent) : QTreeView(parent) {}
@ -120,6 +123,8 @@ private:
};
TreeView *tree;
QLabel *sparkline_label;
QSlider *sparkline_range_slider;
QLineEdit *filter_edit;
ChartsWidget *charts;
QLabel *signal_count_lb;

@ -4,6 +4,7 @@
#include <cmath>
#include <QByteArray>
#include <QDateTime>
#include <QColor>
#include <QFont>
#include <QRegExpValidator>
@ -97,6 +98,9 @@ public:
namespace utils {
QPixmap icon(const QString &id);
inline QString formatSeconds(int seconds) {
return QDateTime::fromTime_t(seconds).toString(seconds > 60 * 60 ? "hh:mm:ss" : "mm:ss");
}
}
QToolButton *toolButton(const QString &icon, const QString &tooltip);

@ -1,7 +1,6 @@
#include "tools/cabana/videowidget.h"
#include <QButtonGroup>
#include <QDateTime>
#include <QMouseEvent>
#include <QPainter>
#include <QStyleOptionSlider>
@ -20,10 +19,6 @@ static const QColor timeline_colors[] = {
[(int)TimelineType::AlertCritical] = QColor(199, 0, 57),
};
static inline QString formatTime(int seconds) {
return QDateTime::fromTime_t(seconds).toString(seconds > 60 * 60 ? "hh:mm:ss" : "mm:ss");
}
VideoWidget::VideoWidget(QWidget *parent) : QFrame(parent) {
setFrameStyle(QFrame::StyledPanel | QFrame::Plain);
auto main_layout = new QVBoxLayout(this);
@ -101,11 +96,11 @@ QWidget *VideoWidget::createCameraWidget() {
slider_layout->addWidget(end_time_label);
l->addLayout(slider_layout);
QObject::connect(slider, &QSlider::sliderReleased, [this]() { can->seekTo(slider->value() / 1000.0); });
QObject::connect(slider, &QSlider::valueChanged, [=](int value) { time_label->setText(formatTime(value / 1000)); });
QObject::connect(slider, &QSlider::valueChanged, [=](int value) { time_label->setText(utils::formatSeconds(value / 1000)); });
QObject::connect(cam_widget, &CameraWidget::clicked, []() { can->pause(!can->isPaused()); });
QObject::connect(can, &AbstractStream::updated, this, &VideoWidget::updateState);
QObject::connect(can, &AbstractStream::streamStarted, [this]() {
end_time_label->setText(formatTime(can->totalSeconds()));
end_time_label->setText(utils::formatSeconds(can->totalSeconds()));
slider->setRange(0, can->totalSeconds() * 1000);
});
return w;
@ -116,7 +111,7 @@ void VideoWidget::rangeChanged(double min, double max, bool is_zoomed) {
min = 0;
max = can->totalSeconds();
}
end_time_label->setText(formatTime(max));
end_time_label->setText(utils::formatSeconds(max));
slider->setRange(min * 1000, max * 1000);
}
@ -230,7 +225,7 @@ void Slider::mouseMoveEvent(QMouseEvent *e) {
}
int x = std::clamp(e->pos().x() - thumb.width() / 2, THUMBNAIL_MARGIN, rect().right() - thumb.width() - THUMBNAIL_MARGIN);
int y = -thumb.height() - THUMBNAIL_MARGIN - style()->pixelMetric(QStyle::PM_LayoutVerticalSpacing);
thumbnail_label.showPixmap(mapToGlobal({x, y}), formatTime(seconds), thumb);
thumbnail_label.showPixmap(mapToGlobal({x, y}), utils::formatSeconds(seconds), thumb);
QSlider::mouseMoveEvent(e);
}

@ -53,6 +53,8 @@ function install_ubuntu_common_requirements() {
libgles2-mesa-dev \
libglfw3-dev \
libglib2.0-0 \
libncurses5-dev \
libncursesw5-dev \
libomp-dev \
libopencv-dev \
libpng16-16 \

Loading…
Cancel
Save