Skip to content

Commit

Permalink
Features/dgulewic/torque vector (#120)
Browse files Browse the repository at this point in the history 
* update engine map tables

* few signals

* re-enable can messages for debugging

* few signals

* re-enable can messages for debugging

* throttle map 4_14 testing

* updates from testing

* FIX WS

* fix VCU

* update vcu code generated code

* final fixes to vcu for operation

---------

Co-authored-by: DEMETRIUS GULEWICZ <[email protected]>
Co-authored-by: AdityaAsGithub <[email protected]>
  • Loading branch information
@AdityaAsGithub @invertedparadox
3 people authored Apr 20, 2024
1 parent d17743d commit 986e75f
Show file tree
Hide file tree
Showing 19 changed files with 1,614 additions and 1,662 deletions.
74 changes: 30 additions & 44 deletions common/daq/can_config.json
View file
Original file line number Diff line number Diff line change
Expand Up @@ -463,6 +463,15 @@
"msg_hlp":3,
"msg_pgn":10
},
{ "msg_name":"gps_speed",
"msg_desc":"gps speed",
"signals":[
{"sig_name":"gps_speed","type":"int16_t","unit": "m/s", "scale": 0.01}
],
"msg_period":40,
"msg_hlp":3,
"msg_pgn":68
},
{ "msg_name":"gps_position",
"msg_desc":"gps position",
"signals":[
Expand All @@ -482,14 +491,24 @@
"msg_hlp":3,
"msg_pgn":141
},
{
"msg_name": "vehHead",
"msg_desc": "Vehicle Heading",
"signals":[
{"sig_name": "vehHead", "type": "int16_t", "unit": "none", "scale": 0.1}
],
"msg_period": 40,
"msg_hlp": 3,
"msg_pgn": 153
},
{ "msg_name":"imu_gyro",
"msg_desc":"gyroscope from imu",
"signals":[
{"sig_name":"imu_gyro_x","type":"int16_t", "scale": 0.01,"unit": "rad/s"},
{"sig_name":"imu_gyro_y","type":"int16_t", "scale": 0.01,"unit": "rad/s"},
{"sig_name":"imu_gyro_z","type":"int16_t", "scale": 0.01,"unit": "rad/s"}
],
"msg_period":40,
"msg_period":20,
"msg_hlp":3,
"msg_pgn":11
},
Expand All @@ -500,7 +519,7 @@
{"sig_name":"imu_accel_y","type":"int16_t", "scale": 0.01,"unit": "m/s^2"},
{"sig_name":"imu_accel_z","type":"int16_t", "scale": 0.01,"unit": "m/s^2"}
],
"msg_period":40,
"msg_period":20,
"msg_hlp":3,
"msg_pgn":142
},
Expand All @@ -511,29 +530,18 @@
{"sig_name":"bmm_mag_y","type":"int16_t","unit": "uT"},
{"sig_name":"bmm_mag_z","type":"int16_t","unit": "uT"}
],
"msg_period":40,
"msg_period":20,
"msg_hlp":3,
"msg_pgn":143
},
{ "msg_name":"sfs_pos",
"msg_desc":"position from sfs",
"signals":[
{"sig_name":"sfs_pos_x","type":"int16_t","unit": "m","scale": 0.01},
{"sig_name":"sfs_pos_y","type":"int16_t","unit": "m","scale": 0.01},
{"sig_name":"sfs_pos_z","type":"int16_t","unit": "m","scale": 0.01}
],
"msg_period":40,
"msg_hlp":3,
"msg_pgn":1444
},
{ "msg_name":"sfs_vel",
"msg_desc":"velocity from sfs",
"signals":[
{"sig_name":"sfs_vel_x","type":"int16_t","unit": "m/s","scale": 0.01},
{"sig_name":"sfs_vel_y","type":"int16_t","unit": "m/s","scale": 0.01},
{"sig_name":"sfs_vel_z","type":"int16_t","unit": "m/s","scale": 0.01}
],
"msg_period":40,
"msg_period":20,
"msg_hlp":3,
"msg_pgn":1445
},
Expand All @@ -544,21 +552,9 @@
{"sig_name":"sfs_acc_y","type":"int16_t","unit": "m/s^2","scale": 0.01},
{"sig_name":"sfs_acc_z","type":"int16_t","unit": "m/s^2","scale": 0.01}
],
"msg_period":40,
"msg_period":20,
"msg_hlp":3,
"msg_pgn":1446
},
{ "msg_name":"sfs_ang",
"msg_desc":"orientation from sfs",
"signals":[
{"sig_name":"sfs_ang_a","type":"int16_t","scale": 0.0001},
{"sig_name":"sfs_ang_b","type":"int16_t","scale": 0.0001},
{"sig_name":"sfs_ang_c","type":"int16_t","scale": 0.0001},
{"sig_name":"sfs_ang_d","type":"int16_t","scale": 0.0001}
],
"msg_period":40,
"msg_hlp":3,
"msg_pgn":1447
},
{ "msg_name":"sfs_ang_vel",
"msg_desc":"orientation vel from sfs",
Expand All @@ -567,7 +563,7 @@
{"sig_name":"sfs_ang_vel_y","type":"int16_t","scale": 0.0001},
{"sig_name":"sfs_ang_vel_z","type":"int16_t","scale": 0.0001}
],
"msg_period":40,
"msg_period":20,
"msg_hlp":3,
"msg_pgn":1448
},
Expand All @@ -577,9 +573,10 @@
"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_r_max", "type": "int16_t", "unit": "none", "scale": 1}
{"sig_name": "equal_k_rl", "type": "int16_t", "unit": "none", "scale": 1},
{"sig_name": "equal_k_rr", "type": "int16_t", "unit": "none", "scale": 1}
],
"msg_period": 15,
"msg_period": 20,
"msg_hlp": 3,
"msg_pgn": 150
},
Expand All @@ -589,20 +586,10 @@
"signals":[
{"sig_name": "vcu_max_r", "type": "int16_t", "unit": "none", "scale": 1}
],
"msg_period": 15,
"msg_period": 20,
"msg_hlp": 3,
"msg_pgn": 152
},
{
"msg_name": "vehHead",
"msg_desc": "Vehicle Heading",
"signals":[
{"sig_name": "vehHead", "type": "int16_t", "unit": "none", "scale": 0.1}
],
"msg_period": 15,
"msg_hlp": 3,
"msg_pgn": 153
},
{
"msg_name": "tv_can_stats",
"msg_desc": "CAN diagnostics",
Expand All @@ -625,8 +612,7 @@
{"msg_name": "dashboard_tv_parameters"},
{"msg_name": "main_hb"},
{"msg_name": "rear_wheel_speeds"},
{"msg_name": "rear_motor_temps"},
{"msg_name": "rear_motor_currents_volts"}
{"msg_name": "rear_motor_temps"}
]
},
{
Expand Down
43 changes: 15 additions & 28 deletions common/daq/per_dbc.dbc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -259,13 +259,19 @@ BO_ 2348810935 gps_velocity: 6 torque_vector
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_ 2348814647 gps_speed: 2 torque_vector
SG_ gps_speed : 0|16@1- (0.01,0) [0|0] "m/s" 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
SG_ latitude : 0|32@1- (1,0) [0|0] "deg" Vector__XXX

BO_ 2348820087 vehHead: 2 torque_vector
SG_ vehHead : 0|16@1- (0.1,0) [0|0] "none" Vector__XXX

BO_ 2348810999 imu_gyro: 6 torque_vector
SG_ imu_gyro_z : 32|16@1- (0.01,0) [0|0] "rad/s" Vector__XXX
SG_ imu_gyro_y : 16|16@1- (0.01,0) [0|0] "rad/s" Vector__XXX
Expand All @@ -281,11 +287,6 @@ BO_ 2348819447 bmm_mag: 6 torque_vector
SG_ bmm_mag_y : 16|16@1- (1,0) [0|0] "uT" Vector__XXX
SG_ bmm_mag_x : 0|16@1- (1,0) [0|0] "uT" Vector__XXX

BO_ 2348902711 sfs_pos: 6 torque_vector
SG_ sfs_pos_z : 32|16@1- (0.01,0) [0|0] "m" Vector__XXX
SG_ sfs_pos_y : 16|16@1- (0.01,0) [0|0] "m" Vector__XXX
SG_ sfs_pos_x : 0|16@1- (0.01,0) [0|0] "m" Vector__XXX

BO_ 2348902775 sfs_vel: 6 torque_vector
SG_ sfs_vel_z : 32|16@1- (0.01,0) [0|0] "m/s" Vector__XXX
SG_ sfs_vel_y : 16|16@1- (0.01,0) [0|0] "m/s" Vector__XXX
Expand All @@ -296,28 +297,20 @@ BO_ 2348902839 sfs_acc: 6 torque_vector
SG_ sfs_acc_y : 16|16@1- (0.01,0) [0|0] "m/s^2" Vector__XXX
SG_ sfs_acc_x : 0|16@1- (0.01,0) [0|0] "m/s^2" Vector__XXX

BO_ 2348902903 sfs_ang: 8 torque_vector
SG_ sfs_ang_d : 48|16@1- (0.0001,0) [0|0] "" Vector__XXX
SG_ sfs_ang_c : 32|16@1- (0.0001,0) [0|0] "" Vector__XXX
SG_ sfs_ang_b : 16|16@1- (0.0001,0) [0|0] "" Vector__XXX
SG_ sfs_ang_a : 0|16@1- (0.0001,0) [0|0] "" Vector__XXX

BO_ 2348902967 sfs_ang_vel: 6 torque_vector
SG_ sfs_ang_vel_z : 32|16@1- (0.0001,0) [0|0] "" Vector__XXX
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_ 2348819895 throttle_vcu: 6 torque_vector
SG_ vcu_r_max : 32|16@1- (1,0) [0|0] "none" Vector__XXX
BO_ 2348819895 throttle_vcu: 8 torque_vector
SG_ equal_k_rr : 48|16@1- (1,0) [0|0] "none" Vector__XXX
SG_ equal_k_rl : 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

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- (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
SG_ can_rx_overflow : 16|8@1+ (1,0) [0|0] "" Vector__XXX
Expand Down Expand Up @@ -835,11 +828,15 @@ CM_ BO_ 2348810935 "gps velocity";
CM_ SG_ 2348810935 gps_vel_d "";
CM_ SG_ 2348810935 gps_vel_e "";
CM_ SG_ 2348810935 gps_vel_n "";
CM_ BO_ 2348814647 "gps speed";
CM_ SG_ 2348814647 gps_speed "";
CM_ BO_ 2348819255 "gps position";
CM_ SG_ 2348819255 height "";
CM_ BO_ 2348819319 "gps coordinates";
CM_ SG_ 2348819319 longitude "";
CM_ SG_ 2348819319 latitude "";
CM_ BO_ 2348820087 "Vehicle Heading";
CM_ SG_ 2348820087 vehHead "";
CM_ BO_ 2348810999 "gyroscope from imu";
CM_ SG_ 2348810999 imu_gyro_z "";
CM_ SG_ 2348810999 imu_gyro_y "";
Expand All @@ -852,10 +849,6 @@ CM_ BO_ 2348819447 "magnetic from bmm";
CM_ SG_ 2348819447 bmm_mag_z "";
CM_ SG_ 2348819447 bmm_mag_y "";
CM_ SG_ 2348819447 bmm_mag_x "";
CM_ BO_ 2348902711 "position from sfs";
CM_ SG_ 2348902711 sfs_pos_z "";
CM_ SG_ 2348902711 sfs_pos_y "";
CM_ SG_ 2348902711 sfs_pos_x "";
CM_ BO_ 2348902775 "velocity from sfs";
CM_ SG_ 2348902775 sfs_vel_z "";
CM_ SG_ 2348902775 sfs_vel_y "";
Expand All @@ -864,23 +857,17 @@ CM_ BO_ 2348902839 "acceleration from sfs";
CM_ SG_ 2348902839 sfs_acc_z "";
CM_ SG_ 2348902839 sfs_acc_y "";
CM_ SG_ 2348902839 sfs_acc_x "";
CM_ BO_ 2348902903 "orientation from sfs";
CM_ SG_ 2348902903 sfs_ang_d "";
CM_ SG_ 2348902903 sfs_ang_c "";
CM_ SG_ 2348902903 sfs_ang_b "";
CM_ SG_ 2348902903 sfs_ang_a "";
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_ 2348819895 "throttle vcu";
CM_ SG_ 2348819895 vcu_r_max "";
CM_ SG_ 2348819895 equal_k_rr "";
CM_ SG_ 2348819895 equal_k_rl "";
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";
CM_ SG_ 2348820087 vehHead "";
CM_ BO_ 2416010039 "CAN diagnostics";
CM_ SG_ 2416010039 can_rx_overrun "CAN Rx FIFO overrun count";
CM_ SG_ 2416010039 can_rx_overflow "CAN Rx queue overflow count";
Expand Down
3 changes: 2 additions & 1 deletion source/main_module/can/can_parse.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -82,7 +82,8 @@ void canRxUpdate(void)
case ID_THROTTLE_VCU:
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.equal_k_rl = (int16_t) msg_data_a->throttle_vcu.equal_k_rl;
can_data.throttle_vcu.equal_k_rr = (int16_t) msg_data_a->throttle_vcu.equal_k_rr;
can_data.throttle_vcu.stale = 0;
can_data.throttle_vcu.last_rx = sched.os_ticks;
break;
Expand Down
10 changes: 6 additions & 4 deletions source/main_module/can/can_parse.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -89,7 +89,7 @@ typedef union {
#define DLC_LWS_STANDARD 5
#define DLC_MAIN_MODULE_BL_CMD 5
#define DLC_ORION_CURRENTS_VOLTS 4
#define DLC_THROTTLE_VCU 6
#define DLC_THROTTLE_VCU 8
#define DLC_FAULT_SYNC_PDU 3
#define DLC_FAULT_SYNC_DASHBOARD 3
#define DLC_FAULT_SYNC_A_BOX 3
Expand Down Expand Up @@ -250,7 +250,7 @@ typedef union {
#define UP_MAX_CELL_TEMP 500
#define UP_LWS_STANDARD 15
#define UP_ORION_CURRENTS_VOLTS 32
#define UP_THROTTLE_VCU 15
#define UP_THROTTLE_VCU 20
/* END AUTO UP DEFS */

#define CHECK_STALE(stale, curr, last, period) if(!stale && \
Expand Down Expand Up @@ -448,7 +448,8 @@ typedef union {
struct {
uint64_t vcu_k_rl: 16;
uint64_t vcu_k_rr: 16;
uint64_t vcu_r_max: 16;
uint64_t equal_k_rl: 16;
uint64_t equal_k_rr: 16;
} throttle_vcu;
struct {
uint64_t idx: 16;
Expand Down Expand Up @@ -535,7 +536,8 @@ typedef struct {
struct {
int16_t vcu_k_rl;
int16_t vcu_k_rr;
int16_t vcu_r_max;
int16_t equal_k_rl;
int16_t equal_k_rr;
uint8_t stale;
uint32_t last_rx;
} throttle_vcu;
Expand Down
25 changes: 7 additions & 18 deletions source/torque_vector/can/can_parse.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -92,13 +92,6 @@ void canRxUpdate()
can_data.rear_motor_temps.stale = 0;
can_data.rear_motor_temps.last_rx = sched.os_ticks;
break;
case ID_REAR_MOTOR_CURRENTS_VOLTS:
can_data.rear_motor_currents_volts.left_current = msg_data_a->rear_motor_currents_volts.left_current;
can_data.rear_motor_currents_volts.right_current = msg_data_a->rear_motor_currents_volts.right_current;
can_data.rear_motor_currents_volts.right_voltage = msg_data_a->rear_motor_currents_volts.right_voltage;
can_data.rear_motor_currents_volts.stale = 0;
can_data.rear_motor_currents_volts.last_rx = sched.os_ticks;
break;
case ID_FAULT_SYNC_PDU:
can_data.fault_sync_pdu.idx = msg_data_a->fault_sync_pdu.idx;
can_data.fault_sync_pdu.latched = msg_data_a->fault_sync_pdu.latched;
Expand Down Expand Up @@ -161,9 +154,6 @@ void canRxUpdate()
CHECK_STALE(can_data.rear_motor_temps.stale,
sched.os_ticks, can_data.rear_motor_temps.last_rx,
UP_REAR_MOTOR_TEMPS);
CHECK_STALE(can_data.rear_motor_currents_volts.stale,
sched.os_ticks, can_data.rear_motor_currents_volts.last_rx,
UP_REAR_MOTOR_CURRENTS_VOLTS);
/* END AUTO STALE CHECKS */
}

Expand Down Expand Up @@ -194,17 +184,16 @@ bool initCANFilter()
CAN1->sFilterRegister[3].FR1 = (ID_REAR_WHEEL_SPEEDS << 3) | 4;
CAN1->sFilterRegister[3].FR2 = (ID_REAR_MOTOR_TEMPS << 3) | 4;
CAN1->FA1R |= (1 << 4); // configure bank 4
CAN1->sFilterRegister[4].FR1 = (ID_REAR_MOTOR_CURRENTS_VOLTS << 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_MAIN_MODULE << 3) | 4;
CAN1->FA1R |= (1 << 5); // configure bank 5
CAN1->sFilterRegister[5].FR1 = (ID_FAULT_SYNC_MAIN_MODULE << 3) | 4;
CAN1->sFilterRegister[5].FR2 = (ID_FAULT_SYNC_DASHBOARD << 3) | 4;
CAN1->sFilterRegister[5].FR1 = (ID_FAULT_SYNC_DASHBOARD << 3) | 4;
CAN1->sFilterRegister[5].FR2 = (ID_FAULT_SYNC_A_BOX << 3) | 4;
CAN1->FA1R |= (1 << 6); // configure bank 6
CAN1->sFilterRegister[6].FR1 = (ID_FAULT_SYNC_A_BOX << 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->sFilterRegister[7].FR1 = (ID_RETURN_FAULT_CONTROL << 3) | 4;
/* END AUTO FILTER */

CAN1->FMR &= ~CAN_FMR_FINIT; // Enable Filters (exit filter init mode)
Expand Down
Loading

0 comments on commit 986e75f

Please sign in to comment.