mawei/panda-meb
1
0
Fork 0
mirror of https://github.com/infiniteCable2/panda.git synced 2026-02-18 17:23:52 +08:00
Code Issues Packages Projects Releases Wiki Activity

safety utils: add ROUND helper (#1397)

Browse Source 
* add round macro

* Update board/utils.h

* function

* one line

* misra

* use here too
This commit is contained in:
Shane Smiskol
2023-05-06 21:25:25 -07:00
committed by GitHub
parent e7f2e72c3d
commit 35609dfdce
5 changed files with 9 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
board/safety.h
View File

@@ -494,6 +494,10 @@ float interpolate(struct lookup_t xy, float x) {
return ret;
}
int ROUND(float val) {
return val + ((val > 0.0) ? 0.5 : -0.5);
}
// Safety checks for longitudinal actuation
bool longitudinal_accel_checks(int desired_accel, const LongitudinalLimits limits) {
bool accel_valid = get_longitudinal_allowed() && !max_limit_check(desired_accel, limits.max_accel, limits.min_accel);

6
board/safety/safety_ford.h
View File

@@ -170,7 +170,7 @@ static int ford_rx_hook(CANPacket_t *to_push) {
// Update vehicle speed
if (addr == MSG_BrakeSysFeatures) {
// Signal: Veh_V_ActlBrk
update_sample(&vehicle_speed, (((GET_BYTE(to_push, 0) << 8) | GET_BYTE(to_push, 1)) * 0.01 / 3.6 * VEHICLE_SPEED_FACTOR) + 0.5);
update_sample(&vehicle_speed, ROUND(((GET_BYTE(to_push, 0) << 8) | GET_BYTE(to_push, 1)) * 0.01 / 3.6 * VEHICLE_SPEED_FACTOR));
}
// Check vehicle speed against a second source
@@ -189,9 +189,7 @@ static int ford_rx_hook(CANPacket_t *to_push) {
float ford_yaw_rate = (((GET_BYTE(to_push, 2) << 8U) | GET_BYTE(to_push, 3)) * 0.0002) - 6.5;
float current_curvature = ford_yaw_rate / MAX(vehicle_speed.values[0] / VEHICLE_SPEED_FACTOR, 0.1);
// convert current curvature into units on CAN for comparison with desired curvature
int current_curvature_can = current_curvature * (float)FORD_STEERING_LIMITS.angle_deg_to_can +
((current_curvature > 0.) ? 0.5 : -0.5);
update_sample(&angle_meas, current_curvature_can);
update_sample(&angle_meas, ROUND(current_curvature * (float)FORD_STEERING_LIMITS.angle_deg_to_can));
}
// Update gas pedal

2
board/safety/safety_nissan.h
View File

@@ -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;
update_sample(&vehicle_speed, ((right_rear + left_rear) / 2.0 * 0.005 / 3.6 * VEHICLE_SPEED_FACTOR) + 0.5);
update_sample(&vehicle_speed, ROUND((right_rear + left_rear) / 2.0 * 0.005 / 3.6 * VEHICLE_SPEED_FACTOR));
}
// X-Trail 0x15c, Leaf 0x239

2
board/safety/safety_tesla.h
View File

@@ -82,7 +82,7 @@ static int tesla_rx_hook(CANPacket_t *to_push) {
// Vehicle speed: ((0.05 * val) - 25) * MPH_TO_MPS
float speed = (((((GET_BYTE(to_push, 3) & 0x0FU) << 8) | (GET_BYTE(to_push, 2))) * 0.05) - 25) * 0.447;
vehicle_moving = ABS(speed) > 0.1;
update_sample(&vehicle_speed, (speed * VEHICLE_SPEED_FACTOR) + 0.5);
update_sample(&vehicle_speed, ROUND(speed * VEHICLE_SPEED_FACTOR));
}
if(addr == (tesla_powertrain ? 0x106 : 0x108)) {

1
board/safety_declarations.h
View File

@@ -141,6 +141,7 @@ bool driver_limit_check(int val, int val_last, struct sample_t *val_driver,
bool get_longitudinal_allowed(void);
bool rt_rate_limit_check(int val, int val_last, const int MAX_RT_DELTA);
float interpolate(struct lookup_t xy, float x);
int ROUND(float val);
void gen_crc_lookup_table_8(uint8_t poly, uint8_t crc_lut[]);
void gen_crc_lookup_table_16(uint16_t poly, uint16_t crc_lut[]);
bool msg_allowed(CANPacket_t *to_send, const CanMsg msg_list[], int len);