retropilot
/
panda
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

WIP: op compatibility mode

master
wocsor 2021-11-15 12:41:17 -07:00
parent 60936e46e6
commit d527579b41
1 changed files with 40 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

70
board/smart_dsu/main.c
View File

@ -235,6 +235,7 @@ bool send = 0;
#define ACC_CTRL 0xF10 #define ACC_CTRL 0xF10
#define ACC_CTRL_OP 0x343 #define ACC_CTRL_OP 0x343
bool enable_acc = 0; bool enable_acc = 0;
bool op_ctrl_mode = 0;
int acc_cmd = 0; int acc_cmd = 0;
#define AEB_CTRL 0xF11 #define AEB_CTRL 0xF11
@ -284,6 +285,9 @@ void CAN1_RX0_IRQ_Handler(void) {
} }
break; break;
case ACC_CTRL_OP: // hack to allow the firmware to support stock OP case ACC_CTRL_OP: // hack to allow the firmware to support stock OP
to_fwd.RIR &= 0xFFFFFFFE; // do not fwd
op_ctrl_mode = 1;
break;
case ACC_CTRL: case ACC_CTRL:
// send this EXACTLY how ACC_CONTROL is sent // send this EXACTLY how ACC_CONTROL is sent
for (int i=0; i<8; i++) { for (int i=0; i<8; i++) {
@ -388,38 +392,44 @@ void CAN3_RX0_IRQ_Handler(void) {
switch (address) { switch (address) {
case DSU_ACC_CONTROL: // ACC_CTRL case DSU_ACC_CONTROL: // ACC_CTRL
for (int i=0; i<8; i++) { if (op_ctrl_mode){
dat[i] = GET_BYTE(&CAN3->sFIFOMailBox[0], i); to_fwd.RIR &= 0xFFFFFFFE; // do not fwd
} // op_ctrl_mode = 0;
if(dat[7] == toyota_checksum(address, dat, 8)) { // for now, just set this flag until the next reboot
// add permit_braking and recompute the checksum } else {
dat[2] &= 0x3F; // mask off the top 2 bits for (int i=0; i<8; i++) {
dat[2] |= (1 << 6U); // SET_ME_X01 dat[i] = GET_BYTE(&CAN3->sFIFOMailBox[0], i);
dat[3] |= (1 << 6U); // permit_braking
dat[7] = toyota_checksum(address, dat, 8);
if (enable_acc){
// modify this before sending to the car only if requested
dat[0] = (acc_cmd >> 8U);
dat[1] = (acc_cmd & 0xFF);
} }
to_fwd.RDLR = dat[0] | (dat[1] << 8) | (dat[2] << 16) | (dat[3] << 24); if(dat[7] == toyota_checksum(address, dat, 8)) {
to_fwd.RDHR = dat[4] | (dat[5] << 8) | (dat[6] << 16) | (dat[7] << 24); // add permit_braking and recompute the checksum
// reset the timer for seeing the DSU dat[2] &= 0x3F; // mask off the top 2 bits
timeout = 0; dat[2] |= (1 << 6U); // SET_ME_X01
state = NO_FAULT; dat[3] |= (1 << 6U); // permit_braking
} else { dat[7] = toyota_checksum(address, dat, 8);
// bad checksum if (enable_acc){
state = FAULT_BAD_CHECKSUM; // modify this before sending to the car only if requested
#ifdef DEBUG_CAN dat[0] = (acc_cmd >> 8U);
for(int ii = 0; ii<8; ii++){ dat[1] = (acc_cmd & 0xFF);
puth2(dat[ii]);
} }
puts("\n expected: "); to_fwd.RDLR = dat[0] | (dat[1] << 8) | (dat[2] << 16) | (dat[3] << 24);
puth2(toyota_checksum(address, dat, 8)); to_fwd.RDHR = dat[4] | (dat[5] << 8) | (dat[6] << 16) | (dat[7] << 24);
puts(" got: "); // reset the timer for seeing the DSU
puth2(dat[7]); timeout = 0;
puts("\n"); state = NO_FAULT;
#endif } else {
// bad checksum
state = FAULT_BAD_CHECKSUM;
#ifdef DEBUG_CAN
for(int ii = 0; ii<8; ii++){
puth2(dat[ii]);
}
puts("\n expected: ");
puth2(toyota_checksum(address, dat, 8));
puts(" got: ");
puth2(dat[7]);
puts("\n");
#endif
}
} }
break; break;
case DSU_AEB_CMD: // AEB BRAKING case DSU_AEB_CMD: // AEB BRAKING