diff --git a/.github/workflows/selfdrive_tests.yaml b/.github/workflows/selfdrive_tests.yaml index 5873e7aee6..0f782107ec 100644 --- a/.github/workflows/selfdrive_tests.yaml +++ b/.github/workflows/selfdrive_tests.yaml @@ -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" diff --git a/cereal b/cereal index 1f0d21df73..d70d215de6 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 1f0d21df7307228218a4b874a3fdcd819b1558af +Subproject commit d70d215de6c584f671272d2de2f46a4f778e9f14 diff --git a/panda b/panda index db6c50cd09..12b9b65985 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit db6c50cd09f773c231f962fc0e31b4612c572b08 +Subproject commit 12b9b65985edf7207a1dfd48de0b714192e6e55d diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index 4faa08d536..8f045c2a69 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -418,7 +418,7 @@ std::optional send_panda_states(PubMaster *pm, const std::vector 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++; diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index f501c8c7dd..6e2797ce24 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -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" diff --git a/selfdrive/car/gm/values.py b/selfdrive/car/gm/values.py index 0d13cb9827..1b814e00b2 100644 --- a/selfdrive/car/gm/values.py +++ b/selfdrive/car/gm/values.py @@ -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 diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index 91e1f04e18..5d8783245e 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -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)) diff --git a/selfdrive/car/honda/hondacan.py b/selfdrive/car/honda/hondacan.py index b7c448d1d3..1fe0a13767 100644 --- a/selfdrive/car/honda/hondacan.py +++ b/selfdrive/car/honda/hondacan.py @@ -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({ diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index a1c97c0c9b..ca0d49d664 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -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.] diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 249818369c..cf6d7280fa 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -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 diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index 38fb8dfbd1..f0e846cc54 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -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', diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 05c3897335..cd6525949c 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -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 diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 2f56094379..6550b19227 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -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) diff --git a/selfdrive/locationd/locationd.cc b/selfdrive/locationd/locationd.cc index 307626506a..87c5f644c5 100755 --- a/selfdrive/locationd/locationd.cc +++ b/selfdrive/locationd/locationd.cc @@ -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_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_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 service_list = {gps_location_socket, "cameraOdometry", "liveCalibration", "carState", "carParams", "accelerometer", "gyroscope"}; diff --git a/selfdrive/locationd/locationd.h b/selfdrive/locationd/locationd.h index b7e42a9df8..6366b84f8e 100755 --- a/selfdrive/locationd/locationd.h +++ b/selfdrive/locationd/locationd.h @@ -84,4 +84,5 @@ private: std::map observation_values_invalid; bool standstill = true; int32_t orientation_reset_count = 0; + float gps_std_factor; }; diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 556781738d..af63dab2f1 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -50f1e873095fe2462d2aadb9c401bda76759c01c +1bb3f665191e1b75c1b786f60e76d51b274f98ae diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index 63b87149d4..c22484167c 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -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); diff --git a/tools/cabana/SConscript b/tools/cabana/SConscript index d9156f229a..7d5129fc16 100644 --- a/tools/cabana/SConscript +++ b/tools/cabana/SConscript @@ -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) diff --git a/tools/cabana/binaryview.cc b/tools/cabana/binaryview.cc index 0a4f6fc999..d0576615c9 100644 --- a/tools/cabana/binaryview.cc +++ b/tools/cabana/binaryview.cc @@ -11,7 +11,7 @@ #include #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); diff --git a/tools/cabana/binaryview.h b/tools/cabana/binaryview.h index 8598fe490b..aa1f8c656b 100644 --- a/tools/cabana/binaryview.h +++ b/tools/cabana/binaryview.h @@ -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 sigs; + bool valid = false; }; std::vector items; diff --git a/tools/cabana/chartswidget.cc b/tools/cabana/chartswidget.cc index ef8524be37..09ee9f1cff 100644 --- a/tools/cabana/chartswidget.cc +++ b/tools/cabana/chartswidget.cc @@ -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(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((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); } } diff --git a/tools/cabana/dbc/dbcmanager.cc b/tools/cabana/dbc/dbcmanager.cc index 0cc51989cd..49527765a8 100644 --- a/tools/cabana/dbc/dbcmanager.cc +++ b/tools/cabana/dbc/dbcmanager.cc @@ -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; } diff --git a/tools/cabana/dbc/dbcmanager.h b/tools/cabana/dbc/dbcmanager.h index 31183e8199..f9af96516c 100644 --- a/tools/cabana/dbc/dbcmanager.h +++ b/tools/cabana/dbc/dbcmanager.h @@ -1,6 +1,8 @@ #pragma once #include +#include + #include #include #include diff --git a/tools/cabana/detailwidget.h b/tools/cabana/detailwidget.h index cc5bffd534..2794d41b99 100644 --- a/tools/cabana/detailwidget.h +++ b/tools/cabana/detailwidget.h @@ -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: diff --git a/tools/cabana/settings.cc b/tools/cabana/settings.cc index 00bbb94de5..9e829708b1 100644 --- a/tools/cabana/settings.cc +++ b/tools/cabana/settings.cc @@ -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 diff --git a/tools/cabana/settings.h b/tools/cabana/settings.h index c3ba906576..a8b6d189a5 100644 --- a/tools/cabana/settings.h +++ b/tools/cabana/settings.h @@ -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; diff --git a/tools/cabana/signaledit.cc b/tools/cabana/signalview.cc similarity index 92% rename from tools/cabana/signaledit.cc rename to tools/cabana/signalview.cc index a2455a0317..e3c19faa13 100644 --- a/tools/cabana/signaledit.cc +++ b/tools/cabana/signalview.cc @@ -1,4 +1,4 @@ -#include "tools/cabana/signaledit.h" +#include "tools/cabana/signalview.h" #include #include @@ -62,10 +62,21 @@ void SignalModel::updateState(const QHash *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 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(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(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::max(); double max = std::numeric_limits::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); diff --git a/tools/cabana/signaledit.h b/tools/cabana/signalview.h similarity index 96% rename from tools/cabana/signaledit.h rename to tools/cabana/signalview.h index 163edcad69..50ba4e06b8 100644 --- a/tools/cabana/signaledit.h +++ b/tools/cabana/signalview.h @@ -3,6 +3,7 @@ #include #include #include +#include #include #include #include @@ -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; diff --git a/tools/cabana/util.h b/tools/cabana/util.h index 7f40c42352..e90a838af8 100644 --- a/tools/cabana/util.h +++ b/tools/cabana/util.h @@ -4,6 +4,7 @@ #include #include +#include #include #include #include @@ -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); diff --git a/tools/cabana/videowidget.cc b/tools/cabana/videowidget.cc index 0e9714994e..fe9e00bc45 100644 --- a/tools/cabana/videowidget.cc +++ b/tools/cabana/videowidget.cc @@ -1,7 +1,6 @@ #include "tools/cabana/videowidget.h" #include -#include #include #include #include @@ -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); } diff --git a/tools/ubuntu_setup.sh b/tools/ubuntu_setup.sh index 71bad2e8a2..97b706de5b 100755 --- a/tools/ubuntu_setup.sh +++ b/tools/ubuntu_setup.sh @@ -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 \