diff --git a/board/safety.h b/board/safety.h index 23e96cc23..a3271c59a 100644 --- a/board/safety.h +++ b/board/safety.h @@ -332,7 +332,6 @@ int set_safety_hooks(uint16_t mode, uint16_t param) { regen_braking = false; regen_braking_prev = false; cruise_engaged_prev = false; - vehicle_speed = 0; vehicle_moving = false; acc_main_on = false; cruise_button_prev = 0; @@ -345,6 +344,8 @@ int set_safety_hooks(uint16_t mode, uint16_t param) { valid_steer_req_count = 0; invalid_steer_req_count = 0; + vehicle_speed.min = 0; + vehicle_speed.max = 0; torque_meas.min = 0; torque_meas.max = 0; torque_driver.min = 0; diff --git a/board/safety/safety_ford.h b/board/safety/safety_ford.h index c1b1d8195..0a8569980 100644 --- a/board/safety/safety_ford.h +++ b/board/safety/safety_ford.h @@ -170,7 +170,7 @@ static int ford_rx_hook(CANPacket_t *to_push) { // Update vehicle speed if (addr == MSG_BrakeSysFeatures) { // Signal: Veh_V_ActlBrk - vehicle_speed = ((GET_BYTE(to_push, 0) << 8) | GET_BYTE(to_push, 1)) * 0.01 / 3.6; + update_sample(&vehicle_speed, ((GET_BYTE(to_push, 0) << 8) | GET_BYTE(to_push, 1)) * 0.01 / 3.6) } // Check vehicle speed against a second source diff --git a/board/safety/safety_nissan.h b/board/safety/safety_nissan.h index 550a78005..4f005cd45 100644 --- a/board/safety/safety_nissan.h +++ b/board/safety/safety_nissan.h @@ -65,7 +65,7 @@ static int nissan_rx_hook(CANPacket_t *to_push) { uint16_t right_rear = (GET_BYTE(to_push, 0) << 8) | (GET_BYTE(to_push, 1)); uint16_t left_rear = (GET_BYTE(to_push, 2) << 8) | (GET_BYTE(to_push, 3)); vehicle_moving = (right_rear | left_rear) != 0U; - vehicle_speed = (right_rear + left_rear) / 2.0 * 0.005 / 3.6; + update_sample(&vehicle_speed, (right_rear + left_rear) / 2.0 * 0.005 / 3.6); } // X-Trail 0x15c, Leaf 0x239 diff --git a/board/safety/safety_tesla.h b/board/safety/safety_tesla.h index b82d89646..b99bf3793 100644 --- a/board/safety/safety_tesla.h +++ b/board/safety/safety_tesla.h @@ -80,8 +80,9 @@ static int tesla_rx_hook(CANPacket_t *to_push) { if(addr == (tesla_powertrain ? 0x116 : 0x118)) { // Vehicle speed: ((0.05 * val) - 25) * MPH_TO_MPS - vehicle_speed = (((((GET_BYTE(to_push, 3) & 0x0FU) << 8) | (GET_BYTE(to_push, 2))) * 0.05) - 25) * 0.447; - vehicle_moving = ABS(vehicle_speed) > 0.1; + float speed = (((((GET_BYTE(to_push, 3) & 0x0FU) << 8) | (GET_BYTE(to_push, 2))) * 0.05) - 25) * 0.447; + vehicle_moving = speed > 0.1; + update_sample(&vehicle_speed, speed); } if(addr == (tesla_powertrain ? 0x106 : 0x108)) { diff --git a/board/safety_declarations.h b/board/safety_declarations.h index 3ea2c89f9..e768e5c32 100644 --- a/board/safety_declarations.h +++ b/board/safety_declarations.h @@ -192,7 +192,7 @@ bool brake_pressed_prev = false; bool regen_braking = false; bool regen_braking_prev = false; bool cruise_engaged_prev = false; -float vehicle_speed = 0; +struct sample_t vehicle_speed; bool vehicle_moving = false; bool acc_main_on = false; // referred to as "ACC off" in ISO 15622:2018 int cruise_button_prev = 0;