diff --git a/common/daq/can_config.json b/common/daq/can_config.json index fbf8cf61..365a525a 100644 --- a/common/daq/can_config.json +++ b/common/daq/can_config.json @@ -182,7 +182,6 @@ {"msg_name": "max_cell_temp"}, {"msg_name": "LWS_Standard"}, {"msg_name": "main_module_bl_cmd" , "callback": true}, - {"msg_name": "throttle_remapped"}, {"msg_name": "orion_currents_volts"}, {"msg_name": "throttle_vcu"} ] @@ -425,8 +424,7 @@ "signals":[ {"sig_name":"gps_vel_n","type":"int16_t","unit": "m/s", "scale": 0.01}, {"sig_name":"gps_vel_e", "type":"int16_t","unit": "m/s", "scale": 0.01}, - {"sig_name":"gps_vel_d", "type":"int16_t","unit": "m/s", "scale": 0.01}, - {"sig_name":"gps_vel_total", "type":"int16_t", "unit": "m/s","scale": 0.01} + {"sig_name":"gps_vel_d", "type":"int16_t","unit": "m/s", "scale": 0.01} ], "msg_period":40, "msg_hlp":3, @@ -435,9 +433,6 @@ { "msg_name":"gps_position", "msg_desc":"gps position", "signals":[ - {"sig_name":"gps_pos_x","type":"int16_t","unit": "m", "scale": 0.01}, - {"sig_name":"gps_pos_y", "type":"int16_t","unit": "m", "scale": 0.01}, - {"sig_name":"gps_pos_z", "type":"int16_t","unit": "m", "scale": 0.01}, {"sig_name":"height", "type":"int16_t", "unit": "m", "scale": 0.01} ], "msg_period":40, @@ -546,23 +541,13 @@ { "msg_name": "throttle_vcu", "msg_desc": "throttle vcu", - "signals":[ - {"sig_name": "vcu_r_rl", "type": "int16_t", "unit": "none", "scale": 1}, - {"sig_name": "vcu_r_rr", "type": "int16_t", "unit": "none", "scale": 1} - ], - "msg_period": 15, - "msg_hlp": 1, - "msg_pgn": 151 - }, - { - "msg_name": "throttle_remapped", - "msg_desc": "throttle remapped", "signals":[ {"sig_name": "vcu_k_rl", "type": "int16_t", "unit": "none", "scale": 1}, - {"sig_name": "vcu_k_rr", "type": "int16_t", "unit": "none", "scale": 1} + {"sig_name": "vcu_k_rr", "type": "int16_t", "unit": "none", "scale": 1}, + {"sig_name": "vcu_r_max", "type": "int16_t", "unit": "none", "scale": 1} ], "msg_period": 15, - "msg_hlp": 1, + "msg_hlp": 3, "msg_pgn": 150 }, { @@ -579,7 +564,7 @@ "msg_name": "vehHead", "msg_desc": "Vehicle Heading", "signals":[ - {"sig_name": "vehHead", "type": "int16_t", "unit": "none", "scale": 1} + {"sig_name": "vehHead", "type": "int16_t", "unit": "none", "scale": 0.1} ], "msg_period": 15, "msg_hlp": 3, diff --git a/common/daq/per_dbc.dbc b/common/daq/per_dbc.dbc index ef2fd870..6ed5e2d8 100644 --- a/common/daq/per_dbc.dbc +++ b/common/daq/per_dbc.dbc @@ -242,17 +242,13 @@ BO_ 2348812088 orion_errors: 4 OrionBMS SG_ charger_safety_relay : 1|1@1+ (1,0) [0|0] "" Vector__XXX SG_ discharge_limit_enforce : 0|1@1+ (1,0) [0|0] "" Vector__XXX -BO_ 2348810935 gps_velocity: 8 torque_vector - SG_ gps_vel_total : 48|16@1- (0.01,0) [0|0] "m/s" Vector__XXX +BO_ 2348810935 gps_velocity: 6 torque_vector SG_ gps_vel_d : 32|16@1- (0.01,0) [0|0] "m/s" Vector__XXX SG_ gps_vel_e : 16|16@1- (0.01,0) [0|0] "m/s" Vector__XXX SG_ gps_vel_n : 0|16@1- (0.01,0) [0|0] "m/s" Vector__XXX -BO_ 2348819255 gps_position: 8 torque_vector - SG_ height : 48|16@1- (0.01,0) [0|0] "m" Vector__XXX - SG_ gps_pos_z : 32|16@1- (0.01,0) [0|0] "m" Vector__XXX - SG_ gps_pos_y : 16|16@1- (0.01,0) [0|0] "m" Vector__XXX - SG_ gps_pos_x : 0|16@1- (0.01,0) [0|0] "m" Vector__XXX +BO_ 2348819255 gps_position: 2 torque_vector + SG_ height : 0|16@1- (0.01,0) [0|0] "m" Vector__XXX BO_ 2348819319 gps_coordinates: 8 torque_vector SG_ longitude : 32|32@1- (1,0) [0|0] "deg" Vector__XXX @@ -299,11 +295,8 @@ BO_ 2348902967 sfs_ang_vel: 6 torque_vector SG_ sfs_ang_vel_y : 16|16@1- (0.0001,0) [0|0] "" Vector__XXX SG_ sfs_ang_vel_x : 0|16@1- (0.0001,0) [0|0] "" Vector__XXX -BO_ 2214602231 throttle_vcu: 4 torque_vector - SG_ vcu_r_rr : 16|16@1- (1,0) [0|0] "none" Vector__XXX - SG_ vcu_r_rl : 0|16@1- (1,0) [0|0] "none" Vector__XXX - -BO_ 2214602167 throttle_remapped: 4 torque_vector +BO_ 2348819895 throttle_vcu: 6 torque_vector + SG_ vcu_r_max : 32|16@1- (1,0) [0|0] "none" Vector__XXX SG_ vcu_k_rr : 16|16@1- (1,0) [0|0] "none" Vector__XXX SG_ vcu_k_rl : 0|16@1- (1,0) [0|0] "none" Vector__XXX @@ -311,7 +304,7 @@ BO_ 2348820023 maxR: 2 torque_vector SG_ vcu_max_r : 0|16@1- (1,0) [0|0] "none" Vector__XXX BO_ 2348820087 vehHead: 2 torque_vector - SG_ vehHead : 0|16@1- (1,0) [0|0] "none" Vector__XXX + SG_ vehHead : 0|16@1- (0.1,0) [0|0] "none" Vector__XXX BO_ 2416010039 tv_can_stats: 4 torque_vector SG_ can_rx_overrun : 24|8@1+ (1,0) [0|0] "" Vector__XXX @@ -801,15 +794,11 @@ CM_ SG_ 2348812088 internal_hardware ""; CM_ SG_ 2348812088 charger_safety_relay ""; CM_ SG_ 2348812088 discharge_limit_enforce ""; CM_ BO_ 2348810935 "gps velocity"; -CM_ SG_ 2348810935 gps_vel_total ""; CM_ SG_ 2348810935 gps_vel_d ""; CM_ SG_ 2348810935 gps_vel_e ""; CM_ SG_ 2348810935 gps_vel_n ""; CM_ BO_ 2348819255 "gps position"; CM_ SG_ 2348819255 height ""; -CM_ SG_ 2348819255 gps_pos_z ""; -CM_ SG_ 2348819255 gps_pos_y ""; -CM_ SG_ 2348819255 gps_pos_x ""; CM_ BO_ 2348819319 "gps coordinates"; CM_ SG_ 2348819319 longitude ""; CM_ SG_ 2348819319 latitude ""; @@ -846,12 +835,10 @@ CM_ BO_ 2348902967 "orientation vel from sfs"; CM_ SG_ 2348902967 sfs_ang_vel_z ""; CM_ SG_ 2348902967 sfs_ang_vel_y ""; CM_ SG_ 2348902967 sfs_ang_vel_x ""; -CM_ BO_ 2214602231 "throttle vcu"; -CM_ SG_ 2214602231 vcu_r_rr ""; -CM_ SG_ 2214602231 vcu_r_rl ""; -CM_ BO_ 2214602167 "throttle remapped"; -CM_ SG_ 2214602167 vcu_k_rr ""; -CM_ SG_ 2214602167 vcu_k_rl ""; +CM_ BO_ 2348819895 "throttle vcu"; +CM_ SG_ 2348819895 vcu_r_max ""; +CM_ SG_ 2348819895 vcu_k_rr ""; +CM_ SG_ 2348819895 vcu_k_rl ""; CM_ BO_ 2348820023 "Maximum Allowed Throttle"; CM_ SG_ 2348820023 vcu_max_r ""; CM_ BO_ 2348820087 "Vehicle Heading"; diff --git a/source/main_module/can/can_parse.c b/source/main_module/can/can_parse.c index 5a220f0e..3bf70f9e 100644 --- a/source/main_module/can/can_parse.c +++ b/source/main_module/can/can_parse.c @@ -89,12 +89,6 @@ void canRxUpdate(void) can_data.main_module_bl_cmd.data = msg_data_a->main_module_bl_cmd.data; main_module_bl_cmd_CALLBACK(msg_data_a); break; - case ID_THROTTLE_REMAPPED: - can_data.throttle_remapped.vcu_k_rl = (int16_t) msg_data_a->throttle_remapped.vcu_k_rl; - can_data.throttle_remapped.vcu_k_rr = (int16_t) msg_data_a->throttle_remapped.vcu_k_rr; - can_data.throttle_remapped.stale = 0; - can_data.throttle_remapped.last_rx = sched.os_ticks; - break; case ID_ORION_CURRENTS_VOLTS: can_data.orion_currents_volts.pack_current = (int16_t) msg_data_a->orion_currents_volts.pack_current; can_data.orion_currents_volts.pack_voltage = msg_data_a->orion_currents_volts.pack_voltage; @@ -102,8 +96,9 @@ void canRxUpdate(void) can_data.orion_currents_volts.last_rx = sched.os_ticks; break; case ID_THROTTLE_VCU: - can_data.throttle_vcu.vcu_r_rl = (int16_t) msg_data_a->throttle_vcu.vcu_r_rl; - can_data.throttle_vcu.vcu_r_rr = (int16_t) msg_data_a->throttle_vcu.vcu_r_rr; + can_data.throttle_vcu.vcu_k_rl = (int16_t) msg_data_a->throttle_vcu.vcu_k_rl; + can_data.throttle_vcu.vcu_k_rr = (int16_t) msg_data_a->throttle_vcu.vcu_k_rr; + can_data.throttle_vcu.vcu_r_max = (int16_t) msg_data_a->throttle_vcu.vcu_r_max; can_data.throttle_vcu.stale = 0; can_data.throttle_vcu.last_rx = sched.os_ticks; break; @@ -165,9 +160,6 @@ void canRxUpdate(void) CHECK_STALE(can_data.LWS_Standard.stale, sched.os_ticks, can_data.LWS_Standard.last_rx, UP_LWS_STANDARD); - CHECK_STALE(can_data.throttle_remapped.stale, - sched.os_ticks, can_data.throttle_remapped.last_rx, - UP_THROTTLE_REMAPPED); CHECK_STALE(can_data.orion_currents_volts.stale, sched.os_ticks, can_data.orion_currents_volts.last_rx, UP_ORION_CURRENTS_VOLTS); @@ -201,22 +193,20 @@ bool initCANFilter() CAN1->sFilterRegister[2].FR1 = (ID_LWS_STANDARD << 3) | 4; CAN1->sFilterRegister[2].FR2 = (ID_MAIN_MODULE_BL_CMD << 3) | 4; CAN1->FA1R |= (1 << 3); // configure bank 3 - CAN1->sFilterRegister[3].FR1 = (ID_THROTTLE_REMAPPED << 3) | 4; - CAN1->sFilterRegister[3].FR2 = (ID_ORION_CURRENTS_VOLTS << 3) | 4; + CAN1->sFilterRegister[3].FR1 = (ID_ORION_CURRENTS_VOLTS << 3) | 4; + CAN1->sFilterRegister[3].FR2 = (ID_THROTTLE_VCU << 3) | 4; CAN1->FA1R |= (1 << 4); // configure bank 4 - CAN1->sFilterRegister[4].FR1 = (ID_THROTTLE_VCU << 3) | 4; - CAN1->sFilterRegister[4].FR2 = (ID_FAULT_SYNC_PDU << 3) | 4; + CAN1->sFilterRegister[4].FR1 = (ID_FAULT_SYNC_PDU << 3) | 4; + CAN1->sFilterRegister[4].FR2 = (ID_FAULT_SYNC_DASHBOARD << 3) | 4; CAN1->FA1R |= (1 << 5); // configure bank 5 - CAN1->sFilterRegister[5].FR1 = (ID_FAULT_SYNC_DASHBOARD << 3) | 4; - CAN1->sFilterRegister[5].FR2 = (ID_FAULT_SYNC_A_BOX << 3) | 4; + CAN1->sFilterRegister[5].FR1 = (ID_FAULT_SYNC_A_BOX << 3) | 4; + CAN1->sFilterRegister[5].FR2 = (ID_FAULT_SYNC_TORQUE_VECTOR << 3) | 4; CAN1->FA1R |= (1 << 6); // configure bank 6 - CAN1->sFilterRegister[6].FR1 = (ID_FAULT_SYNC_TORQUE_VECTOR << 3) | 4; - CAN1->sFilterRegister[6].FR2 = (ID_FAULT_SYNC_TEST_NODE << 3) | 4; + CAN1->sFilterRegister[6].FR1 = (ID_FAULT_SYNC_TEST_NODE << 3) | 4; + CAN1->sFilterRegister[6].FR2 = (ID_SET_FAULT << 3) | 4; CAN1->FA1R |= (1 << 7); // configure bank 7 - CAN1->sFilterRegister[7].FR1 = (ID_SET_FAULT << 3) | 4; - CAN1->sFilterRegister[7].FR2 = (ID_RETURN_FAULT_CONTROL << 3) | 4; - CAN1->FA1R |= (1 << 8); // configure bank 8 - CAN1->sFilterRegister[8].FR1 = (ID_DAQ_COMMAND_MAIN_MODULE << 3) | 4; + CAN1->sFilterRegister[7].FR1 = (ID_RETURN_FAULT_CONTROL << 3) | 4; + CAN1->sFilterRegister[7].FR2 = (ID_DAQ_COMMAND_MAIN_MODULE << 3) | 4; /* END AUTO FILTER */ CAN1->FA1R |= (1 << 6); // configure bank 6 CAN1->sFilterRegister[6].FR1 = (ID_LWS_STANDARD << 21); diff --git a/source/main_module/can/can_parse.h b/source/main_module/can/can_parse.h index e3dfa97e..a81360da 100644 --- a/source/main_module/can/can_parse.h +++ b/source/main_module/can/can_parse.h @@ -41,9 +41,8 @@ #define ID_MAX_CELL_TEMP 0xc04e604 #define ID_LWS_STANDARD 0x2b0 #define ID_MAIN_MODULE_BL_CMD 0x409c43e -#define ID_THROTTLE_REMAPPED 0x40025b7 #define ID_ORION_CURRENTS_VOLTS 0x140006f8 -#define ID_THROTTLE_VCU 0x40025f7 +#define ID_THROTTLE_VCU 0xc0025b7 #define ID_FAULT_SYNC_PDU 0x8cb1f #define ID_FAULT_SYNC_DASHBOARD 0x8cac5 #define ID_FAULT_SYNC_A_BOX 0x8ca44 @@ -76,9 +75,8 @@ #define DLC_MAX_CELL_TEMP 2 #define DLC_LWS_STANDARD 5 #define DLC_MAIN_MODULE_BL_CMD 5 -#define DLC_THROTTLE_REMAPPED 4 #define DLC_ORION_CURRENTS_VOLTS 4 -#define DLC_THROTTLE_VCU 4 +#define DLC_THROTTLE_VCU 6 #define DLC_FAULT_SYNC_PDU 3 #define DLC_FAULT_SYNC_DASHBOARD 3 #define DLC_FAULT_SYNC_A_BOX 3 @@ -224,7 +222,6 @@ #define UP_FILT_THROTTLE_BRAKE 15 #define UP_MAX_CELL_TEMP 500 #define UP_LWS_STANDARD 15 -#define UP_THROTTLE_REMAPPED 15 #define UP_ORION_CURRENTS_VOLTS 32 #define UP_THROTTLE_VCU 15 /* END AUTO UP DEFS */ @@ -409,17 +406,14 @@ typedef union { uint64_t cmd: 8; uint64_t data: 32; } main_module_bl_cmd; - struct { - uint64_t vcu_k_rl: 16; - uint64_t vcu_k_rr: 16; - } throttle_remapped; struct { uint64_t pack_current: 16; uint64_t pack_voltage: 16; } orion_currents_volts; struct { - uint64_t vcu_r_rl: 16; - uint64_t vcu_r_rr: 16; + uint64_t vcu_k_rl: 16; + uint64_t vcu_k_rr: 16; + uint64_t vcu_r_max: 16; } throttle_vcu; struct { uint64_t idx: 16; @@ -497,12 +491,6 @@ typedef struct { uint8_t cmd; uint32_t data; } main_module_bl_cmd; - struct { - int16_t vcu_k_rl; - int16_t vcu_k_rr; - uint8_t stale; - uint32_t last_rx; - } throttle_remapped; struct { int16_t pack_current; uint16_t pack_voltage; @@ -510,8 +498,9 @@ typedef struct { uint32_t last_rx; } orion_currents_volts; struct { - int16_t vcu_r_rl; - int16_t vcu_r_rr; + int16_t vcu_k_rl; + int16_t vcu_k_rr; + int16_t vcu_r_max; uint8_t stale; uint32_t last_rx; } throttle_vcu; diff --git a/source/main_module/car/car.c b/source/main_module/car/car.c index 48be3391..2e9b6682 100644 --- a/source/main_module/car/car.c +++ b/source/main_module/car/car.c @@ -241,10 +241,10 @@ void carPeriodic() float t_req_pedal_r = 0; if (!can_data.filt_throttle_brake.stale) t_req_pedal = (float) CLAMP(can_data.filt_throttle_brake.throttle, 0, 4095); - if (!can_data.throttle_remapped.stale) - t_req_pedal_l = (float) CLAMP(can_data.throttle_remapped.vcu_k_rl, 0, 4095); - if (!can_data.throttle_remapped.stale) - t_req_pedal_r = (float) CLAMP(can_data.throttle_remapped.vcu_k_rr, 0, 4095); + if (!can_data.throttle_vcu.stale) + t_req_pedal_l = (float) CLAMP(can_data.throttle_vcu.vcu_k_rl, 0, 4095); + if (!can_data.throttle_vcu.stale) + t_req_pedal_r = (float) CLAMP(can_data.throttle_vcu.vcu_k_rr, 0, 4095); t_req_pedal = t_req_pedal * 100.0f / 4095.0f; t_req_pedal_l = t_req_pedal_l * 100.0f / 4095.0f; diff --git a/source/torque_vector/ac/ac_ext.h b/source/torque_vector/ac/ac_ext.h index 156dadb8..e5f297fb 100644 --- a/source/torque_vector/ac/ac_ext.h +++ b/source/torque_vector/ac/ac_ext.h @@ -1,7 +1,10 @@ +#ifndef _AC_EXT_H_ +#define _AC_EXT_H_ #define NUM_ELEM_ACC_CALIBRATION 151 typedef struct { float ax[151]; float ay[151]; float az[151]; -} vec_accumulator; \ No newline at end of file +} vec_accumulator; +#endif \ No newline at end of file diff --git a/source/torque_vector/can/can_parse.h b/source/torque_vector/can/can_parse.h index 09032174..2c674a1d 100644 --- a/source/torque_vector/can/can_parse.h +++ b/source/torque_vector/can/can_parse.h @@ -32,8 +32,7 @@ #define ID_SFS_ACC 0xc0169b7 #define ID_SFS_ANG 0xc0169f7 #define ID_SFS_ANG_VEL 0xc016a37 -#define ID_THROTTLE_VCU 0x40025f7 -#define ID_THROTTLE_REMAPPED 0x40025b7 +#define ID_THROTTLE_VCU 0xc0025b7 #define ID_MAXR 0xc002637 #define ID_VEHHEAD 0xc002677 #define ID_TV_CAN_STATS 0x10016337 @@ -58,8 +57,8 @@ // Message DLC definitions /* BEGIN AUTO DLC DEFS */ -#define DLC_GPS_VELOCITY 8 -#define DLC_GPS_POSITION 8 +#define DLC_GPS_VELOCITY 6 +#define DLC_GPS_POSITION 2 #define DLC_GPS_COORDINATES 8 #define DLC_IMU_GYRO 6 #define DLC_IMU_ACCEL 6 @@ -69,8 +68,7 @@ #define DLC_SFS_ACC 6 #define DLC_SFS_ANG 8 #define DLC_SFS_ANG_VEL 6 -#define DLC_THROTTLE_VCU 4 -#define DLC_THROTTLE_REMAPPED 4 +#define DLC_THROTTLE_VCU 6 #define DLC_MAXR 2 #define DLC_VEHHEAD 2 #define DLC_TV_CAN_STATS 4 @@ -95,21 +93,17 @@ // Message sending macros /* BEGIN AUTO SEND MACROS */ -#define SEND_GPS_VELOCITY(gps_vel_n_, gps_vel_e_, gps_vel_d_, gps_vel_total_) do {\ +#define SEND_GPS_VELOCITY(gps_vel_n_, gps_vel_e_, gps_vel_d_) do {\ CanMsgTypeDef_t msg = {.Bus=CAN1, .ExtId=ID_GPS_VELOCITY, .DLC=DLC_GPS_VELOCITY, .IDE=1};\ CanParsedData_t* data_a = (CanParsedData_t *) &msg.Data;\ data_a->gps_velocity.gps_vel_n = gps_vel_n_;\ data_a->gps_velocity.gps_vel_e = gps_vel_e_;\ data_a->gps_velocity.gps_vel_d = gps_vel_d_;\ - data_a->gps_velocity.gps_vel_total = gps_vel_total_;\ canTxSendToBack(&msg);\ } while(0) -#define SEND_GPS_POSITION(gps_pos_x_, gps_pos_y_, gps_pos_z_, height_) do {\ +#define SEND_GPS_POSITION(height_) do {\ CanMsgTypeDef_t msg = {.Bus=CAN1, .ExtId=ID_GPS_POSITION, .DLC=DLC_GPS_POSITION, .IDE=1};\ CanParsedData_t* data_a = (CanParsedData_t *) &msg.Data;\ - data_a->gps_position.gps_pos_x = gps_pos_x_;\ - data_a->gps_position.gps_pos_y = gps_pos_y_;\ - data_a->gps_position.gps_pos_z = gps_pos_z_;\ data_a->gps_position.height = height_;\ canTxSendToBack(&msg);\ } while(0) @@ -185,18 +179,12 @@ data_a->sfs_ang_vel.sfs_ang_vel_z = sfs_ang_vel_z_;\ canTxSendToBack(&msg);\ } while(0) -#define SEND_THROTTLE_VCU(vcu_r_rl_, vcu_r_rr_) do {\ +#define SEND_THROTTLE_VCU(vcu_k_rl_, vcu_k_rr_, vcu_r_max_) do {\ CanMsgTypeDef_t msg = {.Bus=CAN1, .ExtId=ID_THROTTLE_VCU, .DLC=DLC_THROTTLE_VCU, .IDE=1};\ CanParsedData_t* data_a = (CanParsedData_t *) &msg.Data;\ - data_a->throttle_vcu.vcu_r_rl = vcu_r_rl_;\ - data_a->throttle_vcu.vcu_r_rr = vcu_r_rr_;\ - canTxSendToBack(&msg);\ - } while(0) -#define SEND_THROTTLE_REMAPPED(vcu_k_rl_, vcu_k_rr_) do {\ - CanMsgTypeDef_t msg = {.Bus=CAN1, .ExtId=ID_THROTTLE_REMAPPED, .DLC=DLC_THROTTLE_REMAPPED, .IDE=1};\ - CanParsedData_t* data_a = (CanParsedData_t *) &msg.Data;\ - data_a->throttle_remapped.vcu_k_rl = vcu_k_rl_;\ - data_a->throttle_remapped.vcu_k_rr = vcu_k_rr_;\ + data_a->throttle_vcu.vcu_k_rl = vcu_k_rl_;\ + data_a->throttle_vcu.vcu_k_rr = vcu_k_rr_;\ + data_a->throttle_vcu.vcu_r_max = vcu_r_max_;\ canTxSendToBack(&msg);\ } while(0) #define SEND_MAXR(vcu_max_r_) do {\ @@ -270,12 +258,8 @@ typedef union { uint64_t gps_vel_n: 16; uint64_t gps_vel_e: 16; uint64_t gps_vel_d: 16; - uint64_t gps_vel_total: 16; } gps_velocity; struct { - uint64_t gps_pos_x: 16; - uint64_t gps_pos_y: 16; - uint64_t gps_pos_z: 16; uint64_t height: 16; } gps_position; struct { @@ -323,14 +307,11 @@ typedef union { uint64_t sfs_ang_vel_y: 16; uint64_t sfs_ang_vel_z: 16; } sfs_ang_vel; - struct { - uint64_t vcu_r_rl: 16; - uint64_t vcu_r_rr: 16; - } throttle_vcu; struct { uint64_t vcu_k_rl: 16; uint64_t vcu_k_rr: 16; - } throttle_remapped; + uint64_t vcu_r_max: 16; + } throttle_vcu; struct { uint64_t vcu_max_r: 16; } maxR; diff --git a/source/torque_vector/gps/gps.c b/source/torque_vector/gps/gps.c index fc9cf81c..de0b6f8a 100644 --- a/source/torque_vector/gps/gps.c +++ b/source/torque_vector/gps/gps.c @@ -125,7 +125,7 @@ bool parseVelocity(GPS_Handle_t *GPS) iLong.bytes[2] = GPS->raw_message[44]; iLong.bytes[3] = GPS->raw_message[45]; GPS->height = iLong.iLong; /* mm is the raw data */ - GPS->height_rounded = (int16_t)((GPS->height * 100) / 10000); + GPS->height_rounded = (int16_t)(GPS->height / 10); // Collect North Velocity GPS->n_vel_bytes[0] = GPS->raw_message[54]; @@ -137,7 +137,7 @@ bool parseVelocity(GPS_Handle_t *GPS) iLong.bytes[2] = GPS->raw_message[56]; iLong.bytes[3] = GPS->raw_message[57]; GPS->n_vel = iLong.iLong; /* mm/s is the raw data */ - GPS->n_vel_rounded = (int16_t)((GPS->n_vel * 100) / 1000); + GPS->n_vel_rounded = (int16_t)(GPS->n_vel / 10); GPS->n_vel_sfs1 = (double)(GPS->n_vel); GPS->n_vel_sfs2 = (double)GPS->n_vel; @@ -151,7 +151,7 @@ bool parseVelocity(GPS_Handle_t *GPS) iLong.bytes[2] = GPS->raw_message[60]; iLong.bytes[3] = GPS->raw_message[61]; GPS->e_vel = iLong.iLong; /* mm/s is the raw data */ - GPS->e_vel_rounded = (int16_t)((GPS->e_vel * 100) / 1000); + GPS->e_vel_rounded = (int16_t)(GPS->e_vel / 10); // Collect Down Velocity GPS->d_vel_bytes[0] = GPS->raw_message[62]; @@ -163,7 +163,7 @@ bool parseVelocity(GPS_Handle_t *GPS) iLong.bytes[2] = GPS->raw_message[64]; iLong.bytes[3] = GPS->raw_message[65]; GPS->d_vel = iLong.iLong; /* mm/s is the raw data */ - GPS->d_vel_rounded = (int16_t)((GPS->d_vel * 100) / 1000); + GPS->d_vel_rounded = (int16_t)(GPS->d_vel / 10); // Collect Vehicle Heading of Motion GPS->headVeh_bytes[0] = GPS->raw_message[70]; @@ -198,10 +198,11 @@ bool parseVelocity(GPS_Handle_t *GPS) ++counter; } - SEND_GPS_VELOCITY(GPS->n_vel_rounded, GPS->e_vel_rounded, GPS->d_vel_rounded, GPS->speed_rounded); - SEND_GPS_COORDINATES(GPS->lat_rounded, GPS->lon_rounded); - SEND_GPS_POSITION(0, 0, 0, GPS->height_rounded); + SEND_GPS_VELOCITY(GPS->n_vel_rounded, GPS->e_vel_rounded, GPS->d_vel_rounded); SEND_VEHHEAD(GPS->headVeh_rounded); + + SEND_GPS_COORDINATES(GPS->lat_rounded, GPS->lon_rounded); + SEND_GPS_POSITION(GPS->height_rounded); } // Collect Magnetic Declination diff --git a/source/torque_vector/main.c b/source/torque_vector/main.c index a9e9e4e2..3c85274c 100644 --- a/source/torque_vector/main.c +++ b/source/torque_vector/main.c @@ -27,6 +27,21 @@ #include "tv.h" #include "tv_pp.h" +#include "bsxlite_interface.h" + +#include "bmi088.h" +#include "imu.h" +#include "gps.h" + +#include "ac_ext.h" +#include "ac_compute_R.h" + +#include "em.h" +#include "em_pp.h" + +#include "tv.h" +#include "tv_pp.h" + uint8_t collect_test[100] = {0}; GPIOInitConfig_t gpio_config[] = { @@ -359,7 +374,7 @@ void VCU_MAIN(void) /* If Energized -> Accumulate Acceleration Vector */ /* If R2D -> Do TV */ /* Else -> Do Nothing */ - if ((can_data.main_hb.car_state == 2) & (ac_counter <= NUM_ELEM_ACC_CALIBRATION)) { + if (((can_data.main_hb.car_state == 1) | (can_data.main_hb.car_state == 2)) & (ac_counter <= NUM_ELEM_ACC_CALIBRATION)) { /* Accumulate Acceleration Vector */ vec_mm.ax[ac_counter] = GPSHandle.acceleration.x; vec_mm.ay[ac_counter] = GPSHandle.acceleration.y; @@ -396,21 +411,17 @@ void VCU_MAIN(void) setFault(ID_NO_GPS_FIX_FAULT,!rtU_tv.F_raw[8]); /* Send Messages */ - SEND_THROTTLE_VCU((int16_t)(rtY_em.k[0]*4095),(int16_t)(rtY_em.k[1]*4095)); - SEND_THROTTLE_REMAPPED((int16_t)(rtY_em.k[0]*4095),(int16_t)(rtY_em.k[1]*4095)); - - SEND_SFS_ACC((int16_t)(rtY_tv.sig_filt[15] * 100), - (int16_t)(rtY_tv.sig_filt[16] * 100), (int16_t)(rtY_tv.sig_filt[17] * 100)); - SEND_SFS_ANG_VEL((int16_t)(rtY_tv.sig_filt[7] * 10000), - (int16_t)(rtY_tv.sig_filt[8] * 10000), (int16_t)(rtY_tv.sig_filt[9] * 10000)); + SEND_THROTTLE_VCU((int16_t)(rtY_em.k[0]*4095),(int16_t)(rtY_em.k[1]*4095),MAX((int16_t)(rtY_tv.rTVS[0]*4095),(int16_t)(rtY_tv.rTVS[1]))); + SEND_MAXR((int16_t)(rtY_tv.max_K*4095)); - SEND_MAXR((int16_t)(rtY_tv.max_K*100)); + SEND_SFS_ACC((int16_t)(rtY_tv.sig_filt[15] * 100),(int16_t)(rtY_tv.sig_filt[16] * 100), (int16_t)(rtY_tv.sig_filt[17] * 100)); + SEND_SFS_ANG_VEL((int16_t)(rtY_tv.sig_filt[7] * 10000),(int16_t)(rtY_tv.sig_filt[8] * 10000), (int16_t)(rtY_tv.sig_filt[9] * 10000)); - //SEND_SFS_POS((int16_t)(rtY.pos_VNED[0] * 100), + //SEND_SFS_POS(q_tx_can, (int16_t)(rtY.pos_VNED[0] * 100), // (int16_t)(rtY.pos_VNED[1] * 100), (int16_t)(rtY.pos_VNED[2] * 100)); - //SEND_SFS_VEL((int16_t)(rtY.vel_VNED[0] * 100), + //SEND_SFS_VEL(q_tx_can, (int16_t)(rtY.vel_VNED[0] * 100), // (int16_t)(rtY.vel_VNED[1] * 100), (int16_t)(rtY.vel_VNED[2] * 100)); - //SEND_SFS_ANG((int16_t)(rtY.ang_NED[0] * 10000), + //SEND_SFS_ANG(q_tx_can, (int16_t)(rtY.ang_NED[0] * 10000), // (int16_t)(rtY.ang_NED[1] * 10000), (int16_t)(rtY.ang_NED[2] * 10000), (int16_t)(rtY.ang_NED[3] * 10000)); } diff --git a/source/torque_vector/tv/tv.c b/source/torque_vector/tv/tv.c index dd63884b..6024e73a 100644 --- a/source/torque_vector/tv/tv.c +++ b/source/torque_vector/tv/tv.c @@ -7,9 +7,9 @@ * * Code generated for Simulink model 'tv'. * - * Model version : 1.21 + * Model version : 1.26 * Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 - * C/C++ source code generated on : Wed Mar 13 19:11:02 2024 + * C/C++ source code generated on : Thu Mar 28 16:15:49 2024 * * Target selection: ert.tlc * Embedded hardware selection: ARM Compatible->ARM Cortex-M diff --git a/source/torque_vector/tv/tv.h b/source/torque_vector/tv/tv.h index db515bd0..0a1a814f 100644 --- a/source/torque_vector/tv/tv.h +++ b/source/torque_vector/tv/tv.h @@ -7,9 +7,9 @@ * * Code generated for Simulink model 'tv'. * - * Model version : 1.21 + * Model version : 1.26 * Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 - * C/C++ source code generated on : Wed Mar 13 19:11:02 2024 + * C/C++ source code generated on : Thu Mar 28 16:15:49 2024 * * Target selection: ert.tlc * Embedded hardware selection: ARM Compatible->ARM Cortex-M @@ -60,9 +60,9 @@ typedef struct { /* External inputs (root inport signals with default storage) */ typedef struct { - real_T R[9]; /* '/R' */ - boolean_T F_raw[13]; /* '/F_raw' */ real_T D_raw[19]; /* '/D_raw' */ + boolean_T F_raw[13]; /* '/F_raw' */ + real_T R[9]; /* '/R' */ real_T dphi; /* '/dphi' */ real_T TVS_P; /* '/TVS_P' */ real_T TVS_I; /* '/TVS_I' */ @@ -72,13 +72,13 @@ typedef struct { typedef struct { real_T rTVS[2]; /* '/rTVS' */ real_T rEQUAL[2]; /* '/rEQUAL' */ + real_T max_K; /* '/max_K' */ real_T w[2]; /* '/w' */ real_T V; /* '/V' */ real_T TVS_STATE; /* '/TVS_STATE' */ boolean_T F_TVS[51]; /* '/F_TVS' */ real_T sig_trunc[19]; /* '/sig_trunc' */ real_T sig_filt[18]; /* '/sig_filt' */ - real_T max_K; /* '/max_K' */ } ExtY_tv; /* Parameters (default storage) */ diff --git a/source/torque_vector/tv/tv_data.c b/source/torque_vector/tv/tv_data.c index ee3c9d80..5dcf32c1 100644 --- a/source/torque_vector/tv/tv_data.c +++ b/source/torque_vector/tv/tv_data.c @@ -7,9 +7,9 @@ * * Code generated for Simulink model 'tv'. * - * Model version : 1.21 + * Model version : 1.26 * Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 - * C/C++ source code generated on : Wed Mar 13 19:11:02 2024 + * C/C++ source code generated on : Thu Mar 28 16:15:49 2024 * * Target selection: ert.tlc * Embedded hardware selection: ARM Compatible->ARM Cortex-M @@ -26,7 +26,7 @@ P_tv rtP_tv = { /* Variable: I_FUSE * Referenced by: '/I_FUSE' */ - 125.0, + 130.0, /* Variable: PLb * Referenced by: @@ -61,7 +61,7 @@ P_tv rtP_tv = { * '/T_m to Max Power Level' * '/T_mc to Max Power Level' */ - { 1.0, 0.0 }, + { 2.0, 0.0 }, /* Variable: r_power_sat * Referenced by: '/Gain4' @@ -82,7 +82,7 @@ P_tv rtP_tv = { * '/Constant4' * '/Saturation' */ - { 1.0, 130.0, 430.0, 1100.0, 1100.0, 30.0, 30.0, 30.0, 2.5, 2.5, 2.5, 125.0, + { 1.0, 130.0, 430.0, 1100.0, 1100.0, 30.0, 30.0, 30.0, 2.5, 2.5, 2.5, 130.0, 100.0, 100.0, 125.0, 125.0, 30.0, 30.0, 30.0 }, /* Variable: v diff --git a/source/torque_vector/tv/tv_types.h b/source/torque_vector/tv/tv_types.h index f06b6260..6ccfdf62 100644 --- a/source/torque_vector/tv/tv_types.h +++ b/source/torque_vector/tv/tv_types.h @@ -7,9 +7,9 @@ * * Code generated for Simulink model 'tv'. * - * Model version : 1.21 + * Model version : 1.26 * Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 - * C/C++ source code generated on : Wed Mar 13 19:11:02 2024 + * C/C++ source code generated on : Thu Mar 28 16:15:49 2024 * * Target selection: ert.tlc * Embedded hardware selection: ARM Compatible->ARM Cortex-M diff --git a/source/torque_vector/tv_pp/tv_pp.c b/source/torque_vector/tv_pp/tv_pp.c index 1346325f..d0ea5d79 100644 --- a/source/torque_vector/tv_pp/tv_pp.c +++ b/source/torque_vector/tv_pp/tv_pp.c @@ -31,7 +31,7 @@ void tv_pp(ExtU_tv *rtU_tv, GPS_Handle_t *GPS) rtU_tv->D_raw[8] = (GPS->gyroscope.x); /* Incoming data is rad/s */ rtU_tv->D_raw[9] = (GPS->gyroscope.y); /* Incoming data is rad/s */ rtU_tv->D_raw[10] = (GPS->gyroscope.z); /* Incoming data is rad/s */ - rtU_tv->D_raw[11] = (can_data.orion_currents_volts.pack_current*0.1); /* Incoming is A out of battery */ + rtU_tv->D_raw[11] = (can_data.orion_currents_volts.pack_current*0.1); /* Incoming is 10*A out of battery */ rtU_tv->D_raw[12] = (can_data.rear_motor_temps.left_ctrl_temp); /* Incoming is degree C */ rtU_tv->D_raw[13] = (can_data.rear_motor_temps.right_ctrl_temp); /* Incoming is degree C */ rtU_tv->D_raw[14] = (can_data.rear_motor_temps.left_mot_temp); /* Incoming is degree C */