diff --git a/can_up.sh b/can_up.sh new file mode 100755 index 0000000..b76e5c3 --- /dev/null +++ b/can_up.sh @@ -0,0 +1,20 @@ +#!/bin/bash + +# Check if can0 is up +# if ! ip link show up | grep -q can0; then +echo "Bringing up can0..." + +# Load CAN related modules +sudo modprobe can +sleep 0.1 +sudo modprobe can_raw +sleep 0.1 +# Set the bitrate for can0 +sudo ip link set can0 type can bitrate 500000 +sleep 0.5 +sudo ip link set up can0 +# Bring up can0 interfac +echo "can0 setup complete." +# else +# echo "can0 is already up." +# fi diff --git a/config/drivebrain_config.json b/config/drivebrain_config.json index e36c389..e718d88 100644 --- a/config/drivebrain_config.json +++ b/config/drivebrain_config.json @@ -1,11 +1,11 @@ { "CANDriverPrimary": { - "canbus_device": "vcan0", - "path_to_dbc": "./config/hytech.dbc" + "canbus_device": "can0", + "path_to_dbc": "config/hytech.dbc" }, "CANDriverSecondary": { "canbus_device": "vcan1", - "path_to_dbc": "./config/hytech.dbc" + "path_to_dbc": "config/hytech.dbc" }, "SimpleSpeedController": { "max_torque": 21.0, @@ -94,63 +94,70 @@ "in":2.0 }, "estimator_MatlabEstimModel": { - "steer_sensor_offset": 0.0, - "static_fz_N_fl": 0.0, - "static_fz_N_fr": 0.0, - "static_fz_N_rl": 0.0, - "static_fz_N_rr": 0.0, - "cgz_m": 0.0, - "fz_Q": 0.0, - "fz_R": 0.0, - "amk_eff_modifier": 0.0, - "vy_Q11": 0.0, - "wz_Q22": 0.0, - "vy_R11": 0.0, - "wz_R22": 0.0, - "process_noise_vx": 0.0, - "process_noise_yaw_rate": 0.0, - "process_noise_ax": 0.0, - "percent_wheel_diff_noise_gain": 0.0, - "accel_noise_coef": 0.0, - "yaw_rate_noise_coef": 0.0, - "ay_cap_m_s2": 0.0, - "des_Mz_gain": 0.0, - "drive_motor_torq_lim": 0.0, - "regen_motor_torq_lim": 0.0, - "elec_p_lim_kW": 0.0, - "motor_rpm_lim": 0.0, - "brake_deadzone": 0.0, - "accel_deadzone": 0.0, - "vel_saturation": 0.0, - "per_motor_elec_p_lim_kW": 0.0, - "use_steer_deadzone": false, - "use_intent_w_derate": false, - "amk_eff_modifier_fl":0.0, - "amk_eff_modifier_fr": 0.0, - "amk_eff_modifier_rl": 0.0, - "amk_eff_modifier_rr": 0.0, - "global_eff_modifier": 0.0 + "steer_sensor_offset": 20.0, + "static_fz_N_fl": 420.0, + "static_fz_N_fr": 383.6, + "static_fz_N_rl": 281.0, + "static_fz_N_rr": 400.7, + "cgz_m": 0.27, + "fz_Q": 120.0, + "fz_R": 70.0, + "amk_eff_modifier": 0.92, + "vy_Q11": 10.0, + "wz_Q22": 1.0, + "vy_R11": 0.1, + "wz_R22": 0.01, + "process_noise_vx": 0.2, + "process_noise_yaw_rate": 0.2, + "process_noise_ax": 0.2, + "percent_wheel_diff_noise_gain": 1.0, + "accel_noise_coef": 0.1, + "yaw_rate_noise_coef": 0.07, + "ay_cap_m_s2": 23.0, + "des_Mz_gain": 1.0, + "drive_motor_torq_lim": 21.0, + "regen_motor_torq_lim": -15.0, + "elec_p_lim_kW": 80.0, + "motor_rpm_lim": 20000.0, + "brake_deadzone": 0.02, + "accel_deadzone": 0.02, + "vel_saturation": 35.0, + "per_motor_elec_p_lim_kW": 32.0, + "use_steer_deadzone": true, + "use_intent_w_derate": true, + "amk_eff_modifier_rr": 1.0, + "amk_eff_modifier_fl": 1.0, + "amk_eff_modifier_fr": 1.0, + "amk_eff_modifier_rl": 1.0, + "global_eff_modifier": 1.0 + }, "qp_torq_allocator_MatlabModel": { - "mux": 0.0, - "high_axle_alpha": 0.0, - "high_axle_beta": 0.0, - "high_axle_lambda": 0.0, - "low_axle_alpha": 0.0, - "low_axle_beta": 0.0, - "low_axle_lambda": 0.0, - "k_opt": 0.0, - "torq_side_delta": 0.0, - "torq_long_delta": 0.0, - "w_starting": 0.0, - "w_ending": 0.0, - "coast_brake_torq": 0.0, - "estimator_intent_motor_rpm_lim": 0.0 - }, + "mux": 2.0, + "high_axle_alpha": 0.00125, + "high_axle_beta": 150000.0, + "high_axle_lambda": 2.0, + "low_axle_alpha": 0.00085, + "low_axle_beta": 1000000.0, + "low_axle_lambda": 7.0, + "k_opt": 0.1, + "torq_side_delta": 10.0, + "torq_long_delta": 15.0, + "w": 1.0, + "coast_brake_torq": -5.0, + "w_starting": 1.0, + "w_ending": 0.01, + "estimator_intent_motor_rpm_lim": 20000.0, + "w_derate_starting_kW_delta": 10.0, + "w_derate_ending_kW_delta": 5.0, + "use_qp_internal_torq_feedback": false, + "use_min_cell_derate": false + }, "use_vectornav": false, - "use_fake_vn": true, + "use_fake_vn": false, "use_surrey_aero": false, "use_scale_comms": false, - "use_laptimer": true + "use_laptimer": false + +} -} \ No newline at end of file diff --git a/config/hytech.dbc b/config/hytech.dbc index 95beb6d..a1e72ea 100644 --- a/config/hytech.dbc +++ b/config/hytech.dbc @@ -36,12 +36,6 @@ BS_: BU_: -BO_ 1220 IZZE_IR_TEMPS: 8 Peripherals - SG_ izze_brake_IR_temp_4 : 55|16@0+ (1,0) [0|0] "" Vector__XXX - SG_ izze_brake_IR_temp_3 : 39|16@0+ (1,0) [0|0] "" Vector__XXX - SG_ izze_brake_IR_temp_2 : 23|16@0+ (1,0) [0|0] "" Vector__XXX - SG_ izze_brake_IR_temp_1 : 7|16@0+ (1,0) [0|0] "" Vector__XXX - BO_ 1025 ACU_SHUNT_MEASUREMENTS: 6 ECU SG_ ts_out_filtered_read : 32|16@1+ (0.169047463702246,0) [0|0] "volts" Vector__XXX SG_ pack_filtered_read : 16|16@1+ (0.16869351172632,0) [0|0] "volts" Vector__XXX @@ -52,69 +46,6 @@ BO_ 5 Attitude: 6 ECU SG_ Pitch_angle : 16|9@1- (0.25,0) [0|0] "deg" Vector__XXX SG_ Height : 0|12@1- (0.035,0) [0|71.645] "cm" Vector__XXX -BO_ 222 BMS_BALANCING_STATUS: 8 ECU - SG_ cell_60_balancing_status : 63|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ cell_59_balancing_status : 62|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ cell_58_balancing_status : 61|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ cell_57_balancing_status : 60|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ cell_56_balancing_status : 59|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ cell_55_balancing_status : 58|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ cell_54_balancing_status : 57|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ cell_53_balancing_status : 56|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ cell_52_balancing_status : 55|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ cell_51_balancing_status : 54|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ cell_50_balancing_status : 53|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ cell_49_balancing_status : 52|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ cell_48_balancing_status : 51|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ cell_47_balancing_status : 50|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ cell_46_balancing_status : 49|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ cell_45_balancing_status : 48|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ cell_44_balancing_status : 47|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ cell_43_balancing_status : 46|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ cell_42_balancing_status : 45|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ cell_41_balancing_status : 44|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ cell_40_balancing_status : 43|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ cell_39_balancing_status : 42|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ cell_38_balancing_status : 41|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ cell_37_balancing_status : 40|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ cell_36_balancing_status : 39|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ cell_35_balancing_status : 38|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ cell_34_balancing_status : 37|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ cell_33_balancing_status : 36|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ cell_32_balancing_status : 35|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ cell_31_balancing_status : 34|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ cell_30_balancing_status : 33|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ cell_29_balancing_status : 32|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ cell_28_balancing_status : 31|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ cell_27_balancing_status : 30|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ cell_26_balancing_status : 29|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ cell_25_balancing_status : 28|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ cell_24_balancing_status : 27|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ cell_23_balancing_status : 26|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ cell_22_balancing_status : 25|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ cell_21_balancing_status : 24|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ cell_20_balancing_status : 23|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ cell_19_balancing_status : 22|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ cell_18_balancing_status : 21|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ cell_17_balancing_status : 20|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ cell_16_balancing_status : 19|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ cell_15_balancing_status : 18|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ cell_14_balancing_status : 17|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ cell_13_balancing_status : 16|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ cell_12_balancing_status : 15|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ cell_11_balancing_status : 14|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ cell_10_balancing_status : 13|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ cell_9_balancing_status : 12|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ cell_8_balancing_status : 11|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ cell_7_balancing_status : 10|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ cell_6_balancing_status : 9|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ cell_5_balancing_status : 8|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ cell_4_balancing_status : 7|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ cell_3_balancing_status : 6|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ cell_2_balancing_status : 5|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ cell_1_balancing_status : 4|1@1+ (1,0) [0|0] "" Vector__XXX - SG_ group_id : 0|4@1+ (1,0) [0|0] "" Vector__XXX - BO_ 226 BMS_COULOMB_COUNTS: 8 ECU SG_ total_discharge : 32|32@1+ (0.0001,0) [0|0] "Coulombs" Vector__XXX SG_ total_charge : 0|32@1+ (0.0001,0) [0|0] "Coulombs" Vector__XXX @@ -139,9 +70,9 @@ BO_ 214 BMS_ONBOARD_DETAILED_TEMPS: 5 ECU SG_ ic_id : 0|4@1+ (1,0) [0|0] "" Vector__XXX BO_ 213 BMS_ONBOARD_TEMPS: 6 ECU - SG_ high_temp : 32|16@1- (0.01,0) [0|0] "deg C" Vector__XXX - SG_ low_temp : 16|16@1- (0.01,0) [0|0] "" Vector__XXX - SG_ average_temp : 0|16@1- (0.01,0) [0|0] "Deg C" Vector__XXX + SG_ high_cell_temp : 32|16@1- (0.01,0) [0|0] "Deg C" Vector__XXX + SG_ low_cell_temp : 16|16@1- (0.01,0) [0|0] "Deg C" Vector__XXX + SG_ max_board_temp : 0|16@1- (0.01,0) [0|0] "Deg C" Vector__XXX BO_ 219 BMS_STATUS: 6 ECU SG_ shutdown_h_above_threshold_error : 41|1@1+ (1,0) [0|0] "" Vector__XXX @@ -159,9 +90,9 @@ BO_ 219 BMS_STATUS: 6 ECU SG_ state : 0|8@1+ (1,0) [0|0] "" Vector__XXX BO_ 217 BMS_TEMPS: 6 ECU - SG_ high_temp : 32|16@1- (0.01,0) [0|0] "deg C" Vector__XXX - SG_ low_temp : 16|16@1- (0.01,0) [0|0] "" Vector__XXX - SG_ average_temp : 0|16@1- (0.01,0) [0|0] "Deg C" Vector__XXX + SG_ high_cell_temp : 32|16@1- (0.01,0) [0|0] "Deg C" Vector__XXX + SG_ low_cell_temp : 16|16@1- (0.01,0) [0|0] "Deg C" Vector__XXX + SG_ max_board_temp : 0|16@1- (0.01,0) [0|0] "Deg C" Vector__XXX BO_ 215 BMS_VOLTAGES: 8 ECU SG_ total_voltage : 48|16@1+ (0.01,0) [0|0] "" Vector__XXX @@ -1106,7 +1037,7 @@ BO_ 210 VN_LINEAR_ACCEL_UNCOMP: 8 ECU SG_ vn_lin_uncomp_accel_x : 0|16@1- (0.001,0) [0|0] "m/s^2" Vector__XXX BO_ 225 VN_STATUS: 8 ECU - SG_ vn_gps_status : 0|16@1+ (1,0) [0|0] "" Vector__XXX + SG_ vn_gps_status : 0|8@1+ (1,0) [0|0] "" Vector__XXX BO_ 208 VN_VEL: 8 ECU SG_ vn_vel_uncertainty : 48|16@1+ (0.0005,0) [0|0] "m/s" Vector__XXX @@ -1127,15 +1058,167 @@ BO_ 4 Velocities: 8 ECU SG_ Velocity_y : 12|10@1- (0.03,0) [0|0] "m/s" Vector__XXX SG_ Velocity_x : 0|12@1- (0.02,0) [0|0] "m/s" Vector__XXX -BO_ 0 brake_temps: 8 ECU - BO_ 1280 ACU_OK: 1 ECU + SG_ latch_ok : 2|1@1+ (1,0) [0|1] "" Vector__XXX SG_ imd_ok : 1|1@1+ (1,0) [0|1] "" Vector__XXX SG_ bms_ok : 0|1@1+ (1,0) [0|1] "" Vector__XXX +BO_ 553 DRIVEBRAIN_STATE_DATA: 1 ECU + SG_ vn_gps_status : 0|8@1+ (1,0) [0|0] "" Vector__XXX + +BO_ 519 CAR_STATES: 3 ECU + SG_ drivebrain_in_control : 16|1@1+ (1,0) [0|1] "" Vector__XXX + SG_ drivetrain_state : 8|8@1+ (1,0) [0|0] "" Vector__XXX + SG_ vehicle_state : 0|8@1+ (1,0) [0|0] "" Vector__XXX + +BO_ 1220 FL_BRAKE_ROTOR_TEMP_CH1_CH4: 8 ECU + SG_ brake_temp_channel_4 : 55|16@0+ (0.1,-100) [0|0] "deg C" Vector__XXX + SG_ brake_temp_channel_3 : 39|16@0+ (0.1,-100) [0|0] "deg C" Vector__XXX + SG_ brake_temp_channel_2 : 23|16@0+ (0.1,-100) [0|0] "deg C" Vector__XXX + SG_ brake_temp_channel_1 : 7|16@0+ (0.1,-100) [0|0] "deg C" Vector__XXX + +BO_ 1221 FL_BRAKE_ROTOR_TEMP_CH5_CH8: 8 ECU + SG_ brake_temp_channel_8 : 55|16@0+ (0.1,-100) [0|0] "deg C" Vector__XXX + SG_ brake_temp_channel_7 : 39|16@0+ (0.1,-100) [0|0] "deg C" Vector__XXX + SG_ brake_temp_channel_6 : 23|16@0+ (0.1,-100) [0|0] "deg C" Vector__XXX + SG_ brake_temp_channel_5 : 7|16@0+ (0.1,-100) [0|0] "deg C" Vector__XXX + +BO_ 1222 FL_BRAKE_ROTOR_TEMP_CH9_CH12: 8 ECU + SG_ brake_temp_channel_12 : 55|16@0+ (0.1,-100) [0|0] "deg C" Vector__XXX + SG_ brake_temp_channel_11 : 39|16@0+ (0.1,-100) [0|0] "deg C" Vector__XXX + SG_ brake_temp_channel_10 : 23|16@0+ (0.1,-100) [0|0] "deg C" Vector__XXX + SG_ brake_temp_channel_9 : 7|16@0+ (0.1,-100) [0|0] "deg C" Vector__XXX + +BO_ 1224 FL_BRAKE_ROTOR_SENSOR_TEMP: 8 ECU + SG_ brake_rotor_sensor_temp : 7|16@0+ (0.1,-100) [0|0] "deg C" Vector__XXX + +BO_ 1223 FL_BRAKE_ROTOR_TEMP_CH13_CH16: 8 ECU + SG_ brake_temp_channel_16 : 55|16@0+ (0.1,-100) [0|0] "deg C" Vector__XXX + SG_ brake_temp_channel_15 : 39|16@0+ (0.1,-100) [0|0] "deg C" Vector__XXX + SG_ brake_temp_channel_14 : 23|16@0+ (0.1,-100) [0|0] "deg C" Vector__XXX + SG_ brake_temp_channel_13 : 7|16@0+ (0.1,-100) [0|0] "deg C" Vector__XXX + +BO_ 1225 FR_BRAKE_ROTOR_TEMP_CH1_CH4: 8 ECU + SG_ brake_temp_channel_4 : 55|16@0+ (0.1,-100) [0|0] "deg C" Vector__XXX + SG_ brake_temp_channel_3 : 39|16@0+ (0.1,-100) [0|0] "deg C" Vector__XXX + SG_ brake_temp_channel_2 : 23|16@0+ (0.1,-100) [0|0] "deg C" Vector__XXX + SG_ brake_temp_channel_1 : 7|16@0+ (0.1,-100) [0|0] "deg C" Vector__XXX + +BO_ 1226 FR_BRAKE_ROTOR_TEMP_CH5_CH8: 8 ECU + SG_ brake_temp_channel_8 : 55|16@0+ (0.1,-100) [0|0] "deg C" Vector__XXX + SG_ brake_temp_channel_7 : 39|16@0+ (0.1,-100) [0|0] "deg C" Vector__XXX + SG_ brake_temp_channel_6 : 23|16@0+ (0.1,-100) [0|0] "deg C" Vector__XXX + SG_ brake_temp_channel_5 : 7|16@0+ (0.1,-100) [0|0] "deg C" Vector__XXX + +BO_ 1227 FR_BRAKE_ROTOR_TEMP_CH9_CH12: 8 ECU + SG_ brake_temp_channel_12 : 55|16@0+ (0.1,-100) [0|0] "deg C" Vector__XXX + SG_ brake_temp_channel_11 : 39|16@0+ (0.1,-100) [0|0] "deg C" Vector__XXX + SG_ brake_temp_channel_10 : 23|16@0+ (0.1,-100) [0|0] "deg C" Vector__XXX + SG_ brake_temp_channel_9 : 7|16@0+ (0.1,-100) [0|0] "deg C" Vector__XXX + +BO_ 1229 FR_BRAKE_ROTOR_SENSOR_TEMP: 8 ECU + SG_ brake_rotor_sensor_temp : 7|16@0+ (0.1,-100) [0|0] "deg C" Vector__XXX + +BO_ 1228 FR_BRAKE_ROTOR_TEMP_CH13_CH16: 8 ECU + SG_ brake_temp_channel_16 : 55|16@0+ (0.1,-100) [0|0] "deg C" Vector__XXX + SG_ brake_temp_channel_15 : 39|16@0+ (0.1,-100) [0|0] "deg C" Vector__XXX + SG_ brake_temp_channel_14 : 23|16@0+ (0.1,-100) [0|0] "deg C" Vector__XXX + SG_ brake_temp_channel_13 : 7|16@0+ (0.1,-100) [0|0] "deg C" Vector__XXX + +BO_ 1219 BRAKE_ROTOR_SENSOR_COMMAND: 8 ECU + SG_ brake_temp_sensor_can_bitrate : 63|8@0+ (1,0) [0|0] "" Vector__XXX + SG_ brake_temp_sensor_ch_setting : 55|8@0+ (1,0) [0|0] "" Vector__XXX + SG_ brake_temp_sensor_sampling_freq : 47|8@0+ (1,0) [0|0] "" Vector__XXX + SG_ brake_temp_sensor_emissivity : 39|8@0+ (0.01,0) [0|0] "" Vector__XXX + SG_ brake_temp_sensor_base_can_id : 23|16@0+ (1,0) [1|2047] "" Vector__XXX + SG_ brake_temp_sensor_prog_const : 7|16@0+ (1,0) [30000|30000] "" Vector__XXX + +BO_ 1552 AERO_PRESSURE_SENSOR_11: 8 ECU + SG_ aero_channel_3 : 55|16@0+ (0.14285714285,-53000) [0|0] "uBar" Vector__XXX + SG_ aero_channel_2 : 39|16@0+ (0.14285714285,-53000) [0|0] "uBar" Vector__XXX + SG_ aero_channel_1 : 23|16@0+ (0.14285714285,-53000) [0|0] "uBar" Vector__XXX + SG_ aero_channel_0 : 7|16@0+ (0.14285714285,-53000) [0|0] "uBar" Vector__XXX + +BO_ 1584 AERO_PRESSURE_SENSOR_31: 8 ECU + SG_ aero_channel_3 : 55|16@0+ (0.14285714285,-53000) [0|0] "uBar" Vector__XXX + SG_ aero_channel_2 : 39|16@0+ (0.14285714285,-53000) [0|0] "uBar" Vector__XXX + SG_ aero_channel_1 : 23|16@0+ (0.14285714285,-53000) [0|0] "uBar" Vector__XXX + SG_ aero_channel_0 : 7|16@0+ (0.14285714285,-53000) [0|0] "uBar" Vector__XXX + +BO_ 1600 AERO_PRESSURE_SENSOR_41: 8 ECU + SG_ aero_channel_3 : 55|16@0+ (0.14285714285,-53000) [0|0] "uBar" Vector__XXX + SG_ aero_channel_2 : 39|16@0+ (0.14285714285,-53000) [0|0] "uBar" Vector__XXX + SG_ aero_channel_1 : 23|16@0+ (0.14285714285,-53000) [0|0] "uBar" Vector__XXX + SG_ aero_channel_0 : 7|16@0+ (0.14285714285,-53000) [0|0] "uBar" Vector__XXX + +BO_ 1568 AERO_PRESSURE_SENSOR_21: 8 ECU + SG_ aero_channel_3 : 55|16@0+ (0.14285714285,-53000) [0|0] "uBar" Vector__XXX + SG_ aero_channel_2 : 39|16@0+ (0.14285714285,-53000) [0|0] "uBar" Vector__XXX + SG_ aero_channel_1 : 23|16@0+ (0.14285714285,-53000) [0|0] "uBar" Vector__XXX + SG_ aero_channel_0 : 7|16@0+ (0.14285714285,-53000) [0|0] "uBar" Vector__XXX + +BO_ 784 AERO_PRESSURE_SENSOR_12: 8 ECU + SG_ aero_channel_7 : 55|16@0+ (0.14285714285,-53000) [0|0] "uBar" Vector__XXX + SG_ aero_channel_6 : 39|16@0+ (0.14285714285,-53000) [0|0] "uBar" Vector__XXX + SG_ aero_channel_5 : 23|16@0+ (0.14285714285,-53000) [0|0] "uBar" Vector__XXX + SG_ aero_channel_4 : 7|16@0+ (0.14285714285,-53000) [0|0] "uBar" Vector__XXX + +BO_ 800 AERO_PRESSURE_SENSOR_22: 8 ECU + SG_ aero_channel_7 : 55|16@0+ (0.14285714285,-53000) [0|0] "uBar" Vector__XXX + SG_ aero_channel_6 : 39|16@0+ (0.14285714285,-53000) [0|0] "uBar" Vector__XXX + SG_ aero_channel_5 : 23|16@0+ (0.14285714285,-53000) [0|0] "uBar" Vector__XXX + SG_ aero_channel_4 : 7|16@0+ (0.14285714285,-53000) [0|0] "uBar" Vector__XXX + +BO_ 816 AERO_PRESSURE_SENSOR_32: 8 ECU + SG_ aero_channel_7 : 55|16@0+ (0.14285714285,-53000) [0|0] "uBar" Vector__XXX + SG_ aero_channel_6 : 39|16@0+ (0.14285714285,-53000) [0|0] "uBar" Vector__XXX + SG_ aero_channel_5 : 23|16@0+ (0.14285714285,-53000) [0|0] "uBar" Vector__XXX + SG_ aero_channel_4 : 7|16@0+ (0.14285714285,-53000) [0|0] "uBar" Vector__XXX + +BO_ 832 AERO_PRESSURE_SENSOR_42: 8 ECU + SG_ aero_channel_7 : 55|16@0+ (0.14285714285,-53000) [0|0] "uBar" Vector__XXX + SG_ aero_channel_6 : 39|16@0+ (0.14285714285,-53000) [0|0] "uBar" Vector__XXX + SG_ aero_channel_5 : 23|16@0+ (0.14285714285,-53000) [0|0] "uBar" Vector__XXX + SG_ aero_channel_4 : 7|16@0+ (0.14285714285,-53000) [0|0] "uBar" Vector__XXX + +BO_ 310 INV1_OVERLOAD: 8 ECU + SG_ motor_overload_percentage : 16|16@1+ (0.1,0) [0|0] "" Vector__XXX + SG_ inverter_overload_percentage : 0|16@1+ (0.1,0) [0|0] "" Vector__XXX + +BO_ 320 INV2_OVERLOAD: 4 ECU + SG_ motor_overload_percentage : 16|16@1+ (0.1,0) [0|0] "" Vector__XXX + SG_ inverter_overload_percentage : 0|16@1+ (0.1,0) [0|0] "" Vector__XXX + +BO_ 330 INV3_OVERLOAD: 4 ECU + SG_ motor_overload_percentage : 16|16@1+ (0.1,0) [0|0] "" Vector__XXX + SG_ inverter_overload_percentage : 0|16@1+ (0.1,0) [0|0] "" Vector__XXX + +BO_ 340 INV4_OVERLOAD: 4 ECU + SG_ motor_overload_percentage : 16|16@1+ (0.1,0) [0|0] "" Vector__XXX + SG_ inverter_overload_percentage : 0|16@1+ (0.1,0) [0|0] "" Vector__XXX + +BO_ 269 ENERGY_METER_MEAS: 8 ECU + SG_ voltage_V : 39|32@0+ (1,0) [0|0] "" Vector__XXX + SG_ current_A : 7|32@0+ (1,0) [0|0] "" Vector__XXX + +BO_ 1037 ENERGY_METER_STATUS: 8 ECU + SG_ em_energy_w_hr : 15|32@0+ (1,0) [0|0] "" Vector__XXX + SG_ em_logging : 1|1@1+ (1,0) [0|1] "" Vector__XXX + SG_ em_violation : 0|1@1+ (1,0) [0|1] "" Vector__XXX + +BO_ 1549 ENERGY_METER_TEMP: 8 ECU + SG_ em_temp_4 : 63|8@0+ (0.5,0) [0|0] "" Vector__XXX + SG_ em_temp_3 : 55|8@0+ (0.5,0) [0|0] "" Vector__XXX + SG_ em_temp_2 : 47|8@0+ (0.5,0) [0|0] "" Vector__XXX + SG_ em_temp_1 : 39|8@0+ (0.5,0) [0|0] "" Vector__XXX + SG_ em_temp_0 : 31|8@0+ (0.5,0) [0|0] "" Vector__XXX + SG_ em_max_temp : 23|8@0+ (0.5,0) [0|0] "" Vector__XXX + SG_ em_min_temp : 15|8@0+ (0.5,0) [0|0] "" Vector__XXX + SG_ em_temp_sig_index : 2|3@0+ (1,0) [0|1] "" Vector__XXX + SG_ em_num_sensors : 7|5@0+ (1,0) [0|0] "" Vector__XXX + BO_TX_BU_ 1025 : ECU,Peripherals; BO_TX_BU_ 5 : ECU,Peripherals; -BO_TX_BU_ 222 : ECU,Peripherals; BO_TX_BU_ 226 : ECU,Peripherals; BO_TX_BU_ 218 : ECU,Peripherals; BO_TX_BU_ 216 : ECU,Peripherals; @@ -1292,8 +1375,35 @@ BO_TX_BU_ 225 : ECU,Peripherals; BO_TX_BU_ 208 : ECU,Peripherals; BO_TX_BU_ 211 : ECU,Peripherals; BO_TX_BU_ 4 : ECU,Peripherals; -BO_TX_BU_ 0 : ECU,Peripherals; BO_TX_BU_ 1280 : ECU,Peripherals; +BO_TX_BU_ 553 : ECU,Peripherals; +BO_TX_BU_ 519 : ECU,Peripherals; +BO_TX_BU_ 1220 : ECU,Peripherals; +BO_TX_BU_ 1221 : ECU,Peripherals; +BO_TX_BU_ 1222 : ECU,Peripherals; +BO_TX_BU_ 1224 : ECU,Peripherals; +BO_TX_BU_ 1223 : ECU,Peripherals; +BO_TX_BU_ 1225 : ECU,Peripherals; +BO_TX_BU_ 1226 : ECU,Peripherals; +BO_TX_BU_ 1227 : ECU,Peripherals; +BO_TX_BU_ 1229 : ECU,Peripherals; +BO_TX_BU_ 1228 : ECU,Peripherals; +BO_TX_BU_ 1219 : ECU,Peripherals; +BO_TX_BU_ 1552 : ECU,Peripherals; +BO_TX_BU_ 1584 : ECU,Peripherals; +BO_TX_BU_ 1600 : ECU,Peripherals; +BO_TX_BU_ 1568 : ECU,Peripherals; +BO_TX_BU_ 784 : ECU,Peripherals; +BO_TX_BU_ 800 : ECU,Peripherals; +BO_TX_BU_ 816 : ECU,Peripherals; +BO_TX_BU_ 832 : ECU,Peripherals; +BO_TX_BU_ 310 : ECU,Peripherals; +BO_TX_BU_ 320 : ECU,Peripherals; +BO_TX_BU_ 330 : ECU,Peripherals; +BO_TX_BU_ 340 : ECU,Peripherals; +BO_TX_BU_ 269 : ECU,Peripherals; +BO_TX_BU_ 1037 : ECU,Peripherals; +BO_TX_BU_ 1549 : ECU,Peripherals; CM_ SG_ 1025 ts_out_filtered_read "Voltage across tractive system"; @@ -1419,6 +1529,54 @@ CM_ SG_ 4 Velocity_x_CGCorrected "ISO Standard Sign, Positive is Vehicle Moving CM_ SG_ 4 Velocity_z "ISO Standard Sign, Positive is Vehicle Moving Up"; CM_ SG_ 4 Velocity_y "ISO Standard Sign, Positive is Vehicle Turning Left"; CM_ SG_ 4 Velocity_x "ISO Standard Sign, Positive is Vehicle Moving Forward"; +CM_ SG_ 553 vn_gps_status "GPS fix status"; +CM_ SG_ 1220 brake_temp_channel_4 "brake rotor temp sensor channel reading in degrees C"; +CM_ SG_ 1220 brake_temp_channel_3 "brake rotor temp sensor channel reading in degrees C"; +CM_ SG_ 1220 brake_temp_channel_2 "brake rotor temp sensor channel reading in degrees C"; +CM_ SG_ 1220 brake_temp_channel_1 "brake rotor temp sensor channel reading in degrees C"; +CM_ SG_ 1221 brake_temp_channel_8 "brake rotor temp sensor channel reading in degrees C"; +CM_ SG_ 1221 brake_temp_channel_7 "brake rotor temp sensor channel reading in degrees C"; +CM_ SG_ 1221 brake_temp_channel_6 "brake rotor temp sensor channel reading in degrees C"; +CM_ SG_ 1221 brake_temp_channel_5 "brake rotor temp sensor channel reading in degrees C"; +CM_ SG_ 1222 brake_temp_channel_12 "brake rotor temp sensor channel reading in degrees C"; +CM_ SG_ 1222 brake_temp_channel_11 "brake rotor temp sensor channel reading in degrees C"; +CM_ SG_ 1222 brake_temp_channel_10 "brake rotor temp sensor channel reading in degrees C"; +CM_ SG_ 1222 brake_temp_channel_9 "brake rotor temp sensor channel reading in degrees C"; +CM_ SG_ 1224 brake_rotor_sensor_temp "temperature internal to the sensor degrees C"; +CM_ SG_ 1223 brake_temp_channel_16 "brake rotor temp sensor channel reading in degrees C"; +CM_ SG_ 1223 brake_temp_channel_15 "brake rotor temp sensor channel reading in degrees C"; +CM_ SG_ 1223 brake_temp_channel_14 "brake rotor temp sensor channel reading in degrees C"; +CM_ SG_ 1223 brake_temp_channel_13 "brake rotor temp sensor channel reading in degrees C"; +CM_ SG_ 1225 brake_temp_channel_4 "brake rotor temp sensor channel reading in degrees C"; +CM_ SG_ 1225 brake_temp_channel_3 "brake rotor temp sensor channel reading in degrees C"; +CM_ SG_ 1225 brake_temp_channel_2 "brake rotor temp sensor channel reading in degrees C"; +CM_ SG_ 1225 brake_temp_channel_1 "brake rotor temp sensor channel reading in degrees C"; +CM_ SG_ 1226 brake_temp_channel_8 "brake rotor temp sensor channel reading in degrees C"; +CM_ SG_ 1226 brake_temp_channel_7 "brake rotor temp sensor channel reading in degrees C"; +CM_ SG_ 1226 brake_temp_channel_6 "brake rotor temp sensor channel reading in degrees C"; +CM_ SG_ 1226 brake_temp_channel_5 "brake rotor temp sensor channel reading in degrees C"; +CM_ SG_ 1227 brake_temp_channel_12 "brake rotor temp sensor channel reading in degrees C"; +CM_ SG_ 1227 brake_temp_channel_11 "brake rotor temp sensor channel reading in degrees C"; +CM_ SG_ 1227 brake_temp_channel_10 "brake rotor temp sensor channel reading in degrees C"; +CM_ SG_ 1227 brake_temp_channel_9 "brake rotor temp sensor channel reading in degrees C"; +CM_ SG_ 1229 brake_rotor_sensor_temp "temperature internal to the sensor degrees C"; +CM_ SG_ 1228 brake_temp_channel_16 "brake rotor temp sensor channel reading in degrees C"; +CM_ SG_ 1228 brake_temp_channel_15 "brake rotor temp sensor channel reading in degrees C"; +CM_ SG_ 1228 brake_temp_channel_14 "brake rotor temp sensor channel reading in degrees C"; +CM_ SG_ 1228 brake_temp_channel_13 "brake rotor temp sensor channel reading in degrees C"; +CM_ SG_ 1219 brake_temp_sensor_can_bitrate "bitrate setting for CAN on the brake temp sensor. 1 = 1M, 2 = 500k, 3 = 250k, 4 = 100k"; +CM_ SG_ 1219 brake_temp_sensor_ch_setting "40 = 4 Channels, 80 = 8 channels, 160 = 16 channels"; +CM_ SG_ 1219 brake_temp_sensor_sampling_freq "1 = 1 hz, 2 = 2hz, 3 = 4hz, 4 = 8 hz, 5=16 hz, 6=32 hz, 7 = 64hz, 8 = 100hz"; +CM_ SG_ 1219 brake_temp_sensor_emissivity "emissivity of the material (0 to 1)"; +CM_ SG_ 1219 brake_temp_sensor_prog_const "programming constant, should be set to 30000"; +CM_ SG_ 310 motor_overload_percentage "the current overload of the motor according to the i2t calc"; +CM_ SG_ 310 inverter_overload_percentage "the current overload of the inverter according to the i2t calc"; +CM_ SG_ 320 motor_overload_percentage "the current overload of the motor according to the i2t calc"; +CM_ SG_ 320 inverter_overload_percentage "the current overload of the inverter according to the i2t calc"; +CM_ SG_ 330 motor_overload_percentage "the current overload of the motor according to the i2t calc"; +CM_ SG_ 330 inverter_overload_percentage "the current overload of the inverter according to the i2t calc"; +CM_ SG_ 340 motor_overload_percentage "the current overload of the motor according to the i2t calc"; +CM_ SG_ 340 inverter_overload_percentage "the current overload of the inverter according to the i2t calc"; @@ -1430,7 +1588,6 @@ VAL_ 6 Attitude_Filtering 0 "NO_FILTERING_ATT" 1 "LIGHT_FILTERING_ATT" 2 "MEDIUM VAL_ 235 dial_state 0 "MODE_0" 1 "MODE_1" 2 "MODE_2" 3 "MODE_3" 4 "MODE_4" 5 "MODE_5" ; VAL_ 236 dial_state 0 "MODE_0" 1 "MODE_1" 2 "MODE_2" 3 "MODE_3" 4 "MODE_4" 5 "MODE_5" ; VAL_ 257 torque_controller_mux_status 0 "NO_ERROR" 1 "ERROR_SPEED_DIFF_TOO_HIGH" 2 "ERROR_TORQUE_DIFF_TOO_HIGH" 3 "ERROR_CONTROLLER_INDEX_OUT_OF_BOUNDS" 4 "ERROR_CONTROLLER_NULL_POINTER" ; -VAL_ 195 ecu_state 0 "STARTUP" 1 "TRACTIVE_SYSTEM_NOT_ACTIVE" 2 "TRACTIVE_SYSTEM_ACTIVE" 3 "ENABLING_INVERTER" 4 "WAITING_READY_TO_DRIVE_SOUND" 5 "READY_TO_DRIVE" ; VAL_ 3 Velocity_Filtering 0 "NO_FILTERING_VEL" 1 "LIGHT_FILTERING_VEL" 2 "MEDIUM_FILTERING_VEL" 3 "HEAVY_FILTERING_VEL" ; VAL_ 3 IMU_Filtering 0 "NO_FILTERING_IMU" 1 "LIGHT_FILTERING_IMU" 2 "MEDIUM_FILTERING_IMU" 3 "HEAVY_FILTERING_IMU" ; VAL_ 3 Attitude_Filtering 0 "NO_FILTERING_ATT" 1 "LIGHT_FILTERING_ATT" 2 "MEDIUM_FILTERING_ATT" 3 "HEAVY_FILTERING_ATT" ; @@ -1443,6 +1600,12 @@ VAL_ 2 sensor_2_Validity 0 "Sensor_Valid_sense_2" 1 "Response_Timeout_sense_2" 2 VAL_ 2 sensor_1_Validity 0 "Sensor_Valid_sense_1" 1 "Response_Timeout_sense_1" 2 "Bad_Optical_Signal_sense_1" 3 "Speed_Too_High_sense_1" ; VAL_ 2 sensor_0_Validity 0 "Sensor_Valid_sense_0" 1 "Response_Timeout_sense_0" 2 "Bad_Optical_Signal_sense_0" 3 "Speed_Too_High_sense_0" ; VAL_ 225 vn_gps_status 0 "NO_FIX" 1 "TIME_ONLY" 2 "FIX_2D" 3 "FIX_3D" ; +VAL_ 553 vn_gps_status 0 "NO_FIX" 1 "TIME_ONLY" 2 "FIX_2D" 3 "FIX_3D" ; +VAL_ 519 drivetrain_state 0 "NOT_CONNECTED" 1 "NOT_ENABLED_NO_HV_PRESENT" 2 "NOT_ENABLED_HV_PRESENT" 3 "INVERTERS_READY" 4 "INVERTERS_HV_ENABLED" 5 "ENABLED_DRIVE_MODE" 6 "ERROR" 7 "CLEARING_ERRORS" ; +VAL_ 519 vehicle_state 0 "TRACTIVE_SYSTEM_NOT_ACTIVE" 1 "TRACTIVE_SYSTEM_ACTIVE" 2 "WANTING_READY_TO_DRIVE" 3 "READY_TO_DRIVE" 4 "WANTING_RECALIBRATE_PEDALS" 5 "RECALIBRATING_PEDALS" ; SIG_VALTYPE_ 1055 steering_digital_raw : 1; +SIG_VALTYPE_ 269 current_A : 1; +SIG_VALTYPE_ 269 voltage_V : 1; +SIG_VALTYPE_ 1037 em_energy_w_hr : 1; diff --git a/decode.py b/decode.py new file mode 100644 index 0000000..f963185 --- /dev/null +++ b/decode.py @@ -0,0 +1,14 @@ +import cantools + +# Load DBC file +db = cantools.database.load_file('hytech_179.dbc') + +# Hardcoded CAN frame (ID and hex payload from your log) +can_id = 0x40D + +# 40D#00000000006147AF +data = bytes.fromhex('00000000006147AF') # Example from your log + +# Decode and print the result +decoded = db.decode_message(can_id, data) +print(f"Decoded 0x{can_id:X}: {decoded}") diff --git a/down.sh b/down.sh new file mode 100755 index 0000000..1c3ce44 --- /dev/null +++ b/down.sh @@ -0,0 +1,13 @@ +#!/bin/bash + +# Check if can0 is already down +if ! ip link show up | grep -q can0; then + echo "can0 is already down." +else + echo "Bringing down can0..." + + # Bring down can0 interface + sudo ip link set down can0 + + echo "can0 has been brought down." +fi diff --git a/drivebrain_app/include/DriveBrainApp.hpp b/drivebrain_app/include/DriveBrainApp.hpp index 47e686f..de6be71 100644 --- a/drivebrain_app/include/DriveBrainApp.hpp +++ b/drivebrain_app/include/DriveBrainApp.hpp @@ -99,6 +99,7 @@ class DriveBrainApp { std::shared_ptr _lap_timer_driver; bool _using_lap_timer = false; std::shared_ptr> _acu_eth_driver; + std::shared_ptr> _acu_eth_driver_core; std::shared_ptr> _vcr_eth_driver; std::shared_ptr> _vcf_eth_driver; std::shared_ptr> _fake_vn = nullptr; diff --git a/drivebrain_app/src/DriveBrainApp.cpp b/drivebrain_app/src/DriveBrainApp.cpp index 77c4924..bd7b5f6 100644 --- a/drivebrain_app/src/DriveBrainApp.cpp +++ b/drivebrain_app/src/DriveBrainApp.cpp @@ -98,6 +98,7 @@ DriveBrainApp::DriveBrainApp(const std::string& param_path, const std::string& d configurable_components.push_back(_driver_secondary_can); spdlog::info("made CAN driver"); _acu_eth_driver = std::make_shared>(_io_context, 7766); + _acu_eth_driver_core = std::make_shared>(_io_context, 7777); _vcr_eth_driver = std::make_shared>(_io_context, 9999, _state_estimator); _vcf_eth_driver = std::make_shared>(_io_context, 4444); @@ -199,6 +200,7 @@ DriveBrainApp::DriveBrainApp(const std::string& param_path, const std::string& d _fake_vn, _state_estimator, _acu_eth_driver, + _acu_eth_driver_core, _vcr_eth_driver, _vcf_eth_driver, _aero_sensor_driver, diff --git a/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp b/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp index 3f33222..6d52f83 100644 --- a/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp +++ b/drivebrain_core_impl/drivebrain_estimation/src/StateEstimator.cpp @@ -36,13 +36,61 @@ void StateEstimator::_recv_inverter_states(std::shared_ptr(msg); } else if (name == "hytech.inv1_temps") { - _handle_set_inverter_temps<0, hytech::inv1_temps>(msg); + auto in_msg = std::static_pointer_cast(msg); + { + std::unique_lock lk(_state_mutex); + _vehicle_state.dt_data.inverter_igbt_temps_c.FL = in_msg->igbt_temp(); + _vehicle_state.dt_data.inverter_temps_c.FL = in_msg->inverter_temp(); + _vehicle_state.dt_data.inverter_motor_temps_c.FL = in_msg->motor_temp(); + } } else if (name == "hytech.inv2_temps") { - _handle_set_inverter_temps<1, hytech::inv2_temps>(msg); + auto in_msg = std::static_pointer_cast(msg); + { + std::unique_lock lk(_state_mutex); + _vehicle_state.dt_data.inverter_igbt_temps_c.FR = in_msg->igbt_temp(); + _vehicle_state.dt_data.inverter_temps_c.FR = in_msg->inverter_temp(); + _vehicle_state.dt_data.inverter_motor_temps_c.FR = in_msg->motor_temp(); + } } else if (name == "hytech.inv3_temps") { - _handle_set_inverter_temps<2, hytech::inv3_temps>(msg); + auto in_msg = std::static_pointer_cast(msg); + { + std::unique_lock lk(_state_mutex); + _vehicle_state.dt_data.inverter_igbt_temps_c.RL = in_msg->igbt_temp(); + _vehicle_state.dt_data.inverter_temps_c.RL = in_msg->inverter_temp(); + _vehicle_state.dt_data.inverter_motor_temps_c.RL = in_msg->motor_temp(); + } } else if (name == "hytech.inv4_temps") { - _handle_set_inverter_temps<3, hytech::inv4_temps>(msg); + auto in_msg = std::static_pointer_cast(msg); + { + std::unique_lock lk(_state_mutex); + _vehicle_state.dt_data.inverter_igbt_temps_c.RR = in_msg->igbt_temp(); + _vehicle_state.dt_data.inverter_temps_c.RR = in_msg->inverter_temp(); + _vehicle_state.dt_data.inverter_motor_temps_c.RR = in_msg->motor_temp(); + } + } else if (name == "hytech.inv1_overload") { + auto in_msg = std::static_pointer_cast(msg); + { + std::unique_lock lk(_state_mutex); + _vehicle_state.motor_overload_percentages.FL = in_msg->motor_overload_percentage(); + } + } else if (name == "hytech.inv2_overload") { + auto in_msg = std::static_pointer_cast(msg); + { + std::unique_lock lk(_state_mutex); + _vehicle_state.motor_overload_percentages.FR = in_msg->motor_overload_percentage(); + } + } else if (name == "hytech.inv3_overload") { + auto in_msg = std::static_pointer_cast(msg); + { + std::unique_lock lk(_state_mutex); + _vehicle_state.motor_overload_percentages.RL = in_msg->motor_overload_percentage(); + } + } else if (name == "hytech.inv4_overload") { + auto in_msg = std::static_pointer_cast(msg); + { + std::unique_lock lk(_state_mutex); + _vehicle_state.motor_overload_percentages.RR = in_msg->motor_overload_percentage(); + } } else if(name == "hytech.inv1_status") { auto in_msg = std::static_pointer_cast(msg); @@ -92,6 +140,7 @@ void StateEstimator::handle_recv_process(std::shared_ptrGetTypeName() == "hytech_msgs.ACUAllData") { auto in_msg = std::static_pointer_cast(message); + // std::cout << "min cell: " << in_msg->core_data().min_cell_voltage() << std::endl; { std::unique_lock lk(_state_mutex); _vehicle_state.acc_data.min_cell_voltage = in_msg->core_data().min_cell_voltage(); @@ -174,7 +223,6 @@ void StateEstimator::_handle_set_inverter_dynamics(std::shared_ptr void StateEstimator::_handle_set_inverter_temps(std::shared_ptr msg) { - auto in_msg = std::static_pointer_cast(msg); core::DrivetrainData dt_data = {}; dt_data.inverter_igbt_temps_c.set_from_index(in_msg->igbt_temp()); @@ -259,18 +307,10 @@ core::VehicleState StateEstimator::append_state_variables_from_raw_inputs(core::VehicleState vs, core::RawInputData raw_data) { auto vehicle_state = vs; - vehicle_state.suspension_potentiometers_mm.FL = math::normalize_linear_scale( - raw_data.raw_shock_pot_values.FL, _config.fl_sus_pot_min, _config.fl_sus_pot_max, - _config.fl_sus_pot_min_mm, _config.fl_sus_pot_max_mm); - vehicle_state.suspension_potentiometers_mm.FR = math::normalize_linear_scale( - raw_data.raw_shock_pot_values.FR, _config.fr_sus_pot_min, _config.fr_sus_pot_max, - _config.fr_sus_pot_min_mm, _config.fr_sus_pot_max_mm); - vehicle_state.suspension_potentiometers_mm.RL = math::normalize_linear_scale( - raw_data.raw_shock_pot_values.RL, _config.rl_sus_pot_min, _config.rl_sus_pot_max, - _config.rl_sus_pot_min_mm, _config.rl_sus_pot_max_mm); - vehicle_state.suspension_potentiometers_mm.RR = math::normalize_linear_scale( - raw_data.raw_shock_pot_values.RR, _config.rr_sus_pot_min, _config.rr_sus_pot_max, - _config.rr_sus_pot_min_mm, _config.rr_sus_pot_max_mm); + vehicle_state.suspension_potentiometers_mm.FL = raw_data.raw_shock_pot_values.FL; + vehicle_state.suspension_potentiometers_mm.FR = raw_data.raw_shock_pot_values.FR; + vehicle_state.suspension_potentiometers_mm.RL = raw_data.raw_shock_pot_values.RL; + vehicle_state.suspension_potentiometers_mm.RR = raw_data.raw_shock_pot_values.RR; vehicle_state.loadcells.FL = raw_data.raw_load_cell_values.FL; vehicle_state.loadcells.FR = raw_data.raw_load_cell_values.FR; diff --git a/flake.lock b/flake.lock index c274c32..ffd5dfe 100644 --- a/flake.lock +++ b/flake.lock @@ -15,7 +15,6 @@ }, "original": { "owner": "hytech-racing", - "ref": "2025-06-14T04_41_35", "repo": "HT_proto", "type": "github" } @@ -71,14 +70,14 @@ "db-simulink-gen-src": { "flake": false, "locked": { - "lastModified": 1749914780, - "narHash": "sha256-u4CDvtmDn1D7VfoD/QITc7BI3leZFZDcsbVsbxsxrdo=", + "lastModified": 1757552658, + "narHash": "sha256-Z/NQFLnag40TNSacho7z187fqMkIhImSZltusTuXn3w=", "type": "tarball", - "url": "https://github.com/hytech-racing/drivebrain_simulink_models/releases/download/rel7/gen_rel.tar.gz" + "url": "https://github.com/hytech-racing/drivebrain_simulink_models/releases/download/rel13-2/gen_rel.tar.gz" }, "original": { "type": "tarball", - "url": "https://github.com/hytech-racing/drivebrain_simulink_models/releases/download/rel7/gen_rel.tar.gz" + "url": "https://github.com/hytech-racing/drivebrain_simulink_models/releases/download/rel13-2/gen_rel.tar.gz" } }, "dbcppp-src": { @@ -373,11 +372,11 @@ "utils": "utils_2" }, "locked": { - "lastModified": 1749916491, - "narHash": "sha256-YKHet4wW71yYoah2AYlHIC5n930HI0lWFkeuxzlSd54=", + "lastModified": 1750250981, + "narHash": "sha256-+qgDweOcLF2ga70jOBXWsoxYsyKd8xP51FK7UdPBvwQ=", "owner": "hytech-racing", "repo": "ht_can", - "rev": "ef8559488f5c020ecf83175e1cd12afc84e15167", + "rev": "a641eef22d37c97563fd72ec928e48888066a50f", "type": "github" }, "original": { diff --git a/flake.nix b/flake.nix index 314357d..161da71 100644 --- a/flake.nix +++ b/flake.nix @@ -16,7 +16,7 @@ nix-proto.url = "github:notalltim/nix-proto"; nix-proto.inputs.nixpkgs.follows = "nixpkgs"; - HT_proto.url = "github:hytech-racing/HT_proto/2025-06-14T04_41_35"; + HT_proto.url = "github:hytech-racing/HT_proto"; foxglove-schemas-src = { url = "github:foxglove/schemas"; @@ -30,12 +30,12 @@ vn_driver_lib.url = "github:RCMast3r/vn_driver_lib/fix/boost-compatible"; db-core-src = { - url = "github:hytech-racing/drivebrain_core"; + url = "github:hytech-racing/drivebrain_core/456fb8eec9143d2bb10d26369737e000cae2d63d"; flake = false; }; db-simulink-gen-src = { - url = "https://github.com/hytech-racing/drivebrain_simulink_models/releases/download/rel7/gen_rel.tar.gz"; + url = "https://github.com/hytech-racing/drivebrain_simulink_models/releases/download/rel16/gen_rel.tar.gz"; flake = false; }; };