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

small logic cleanup (#118)

master
rbiasini 2018-06-01 16:27:55 -07:00 committed by GitHub
parent 9d92bf27c8
commit 83bcaa3980
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 6 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
board/safety/safety_toyota.h
View File

@ -32,10 +32,14 @@ int cruise_engaged_last = 0; // cruise state
static void toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
// get eps motor torque (0.66 factor in dbc)
if ((to_push->RIR>>21) == 0x260) {
int16_t torque_meas_new_16 = (((to_push->RDHR) & 0xFF00) | ((to_push->RDHR >> 16) & 0xFF));
int torque_meas_new = (((to_push->RDHR) & 0xFF00) | ((to_push->RDHR >> 16) & 0xFF));
torque_meas_new = to_signed(torque_meas_new, 16);
// scale by dbc_factor
torque_meas_new *= dbc_eps_torque_factor / 100;
// increase torque_meas by 1 to be conservative on rounding
int torque_meas_new = ((int)(torque_meas_new_16) * dbc_eps_torque_factor / 100) + (torque_meas_new_16 > 0 ? 1 : -1);
torque_meas_new += (torque_meas_new > 0 ? 1 : -1);
// update array of sample
update_sample(&torque_meas, torque_meas_new);
@ -88,7 +92,6 @@ static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
if (desired_torque < -MAX_TORQUE) violation = 1;
if (desired_torque > MAX_TORQUE) violation = 1;
// *** torque rate limit check ***
int16_t highest_allowed_torque = max(desired_torque_last, 0) + MAX_RATE_UP;
int16_t lowest_allowed_torque = min(desired_torque_last, 0) - MAX_RATE_UP;