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

Safety: made rate limit check also common

master
Commaremote 2018-06-13 01:37:36 -07:00
parent dc3cc240b9
commit ef079e6da9
2 changed files with 25 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
board/safety.h
View File

@ -12,8 +12,10 @@ int safety_ignition_hook();
uint32_t get_ts_elapsed(uint32_t ts, uint32_t ts_last);
int to_signed(int d, int bits);
void update_sample(struct sample_t *sample, int sample_new);
int rt_rate_limit_check(int val, int val_last, const int MAX_RT_DELTA);
int max_limit_check(int val, const int MAX);
int dist_to_meas_check(int val, int val_last, struct sample_t *val_meas,
const int MAX_RATE_UP, const int MAX_RATE_DOWN, const int MAX_ERROR);
int rt_rate_limit_check(int val, int val_last, const int MAX_RT_DELTA);
typedef void (*safety_hook_init)(int16_t param);
typedef void (*rx_hook)(CAN_FIFOMailBox_TypeDef *to_push);
@ -144,6 +146,26 @@ void update_sample(struct sample_t *sample, int sample_new) {
}
}
int max_limit_check(int val, const int MAX) {
return (val > MAX) | (val < -MAX);
}
// check that commanded value isn't too far from measured
int dist_to_meas_check(int val, int val_last, struct sample_t *val_meas,
const int MAX_RATE_UP, const int MAX_RATE_DOWN, const int MAX_ERROR) {
// *** val rate limit check ***
int16_t highest_allowed_val = max(val_last, 0) + MAX_RATE_UP;
int16_t lowest_allowed_val = min(val_last, 0) - MAX_RATE_UP;
// if we've exceeded the meas val, we must start moving toward 0
highest_allowed_val = min(highest_allowed_val, max(val_last - MAX_RATE_DOWN, max(val_meas->max, 0) + MAX_ERROR));
lowest_allowed_val = max(lowest_allowed_val, min(val_last + MAX_RATE_DOWN, min(val_meas->min, 0) - MAX_ERROR));
// check for violation
return (val < lowest_allowed_val) || (val > highest_allowed_val);
}
// real time check, mainly used for steer torque rate limiter
int rt_rate_limit_check(int val, int val_last, const int MAX_RT_DELTA) {
@ -151,10 +173,6 @@ int rt_rate_limit_check(int val, int val_last, const int MAX_RT_DELTA) {
int16_t highest_val = max(val_last, 0) + MAX_RT_DELTA;
int16_t lowest_val = min(val_last, 0) - MAX_RT_DELTA;
// return 1 if violation
// check for violation
return (val < lowest_val) || (val > highest_val);
}
int max_limit_check(int val, const int MAX) {
return (val > MAX) | (val < -MAX);
}

12
board/safety/safety_toyota.h
View File

@ -92,17 +92,7 @@ static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
violation |= max_limit_check(desired_torque, MAX_TORQUE);
// *** 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;
// if we've exceeded the applied torque, we must start moving toward 0
highest_allowed_torque = min(highest_allowed_torque, max(desired_torque_last - MAX_RATE_DOWN, max(torque_meas.max, 0) + MAX_TORQUE_ERROR));
lowest_allowed_torque = max(lowest_allowed_torque, min(desired_torque_last + MAX_RATE_DOWN, min(torque_meas.min, 0) - MAX_TORQUE_ERROR));
// check for violation
if ((desired_torque < lowest_allowed_torque) || (desired_torque > highest_allowed_torque)) {
violation = 1;
}
violation |= dist_to_meas_check(desired_torque, desired_torque_last, &torque_meas, MAX_RATE_UP, MAX_RATE_DOWN, MAX_TORQUE_ERROR);
// used next time
desired_torque_last = desired_torque;