small logic cleanup (commaai#118)

jeankalud · Jun 1, 2018 · 83bcaa3 · 83bcaa3
1 parent 9d92bf2
commit 83bcaa3
Showing 1 changed file with 6 additions and 3 deletions.
diff --git a/board/safety/safety_toyota.h b/board/safety/safety_toyota.h
@@ -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;