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

moved interpolate function to safety header

master
Riccardo 2018-08-13 22:52:31 -07:00
parent d7bd4731fb
commit 0a480ec20e
2 changed files with 36 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
board/safety.h
View File

@ -5,6 +5,11 @@ struct sample_t {
int max;
} sample_t_default = {{0}, 0, 0};
struct lookup_t {
float x[3];
float y[3];
};
void safety_rx_hook(CAN_FIFOMailBox_TypeDef *to_push);
int safety_tx_hook(CAN_FIFOMailBox_TypeDef *to_send);
int safety_tx_lin_hook(int lin_num, uint8_t *data, int len);
@ -16,9 +21,10 @@ int max_limit_check(int val, const int MAX, const int MIN);
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 driver_limit_check(int val, int val_last, struct sample_t *val_driver,
const int MAX, const int MAX_RATE_UP, const int MAX_RATE_DOWN,
const int MAX, const int MAX_RATE_UP, const int MAX_RATE_DOWN,
const int MAX_ALLOWANCE, const int DRIVER_FACTOR);
int rt_rate_limit_check(int val, int val_last, const int MAX_RT_DELTA);
float interpolate(struct lookup_t xy, float x);
typedef void (*safety_hook_init)(int16_t param);
typedef void (*rx_hook)(CAN_FIFOMailBox_TypeDef *to_push);
@ -201,3 +207,29 @@ int rt_rate_limit_check(int val, int val_last, const int MAX_RT_DELTA) {
// check for violation
return (val < lowest_val) || (val > highest_val);
}
// interp function that holds extreme values
float interpolate(struct lookup_t xy, float x) {
int size = sizeof(xy.x) / sizeof(xy.x[0]);
// x is lower than the first point in the x array. Return the first point
if (x <= xy.x[0]) {
return xy.y[0];
} else {
// find the index such that (xy.x[i] <= x < xy.x[i+1]) and linearly interp
for (int i=0; i < size-1; i++) {
if (x < xy.x[i+1]) {
float x0 = xy.x[i];
float y0 = xy.y[i];
float dx = xy.x[i+1] - x0;
float dy = xy.y[i+1] - y0;
// dx should not be zero as xy.x is supposed ot be monotonic
if (dx <= 0.) dx = 0.0001;
return dy * (x - x0) / dx + y0;
}
}
// if no such point is found, then x > xy.x[size-1]. Return last point
return xy.y[size - 1];
}
}

36
board/safety/safety_toyota_ipas.h
View File

@ -4,11 +4,6 @@
// IPAS override
const int32_t TOYOTA_IPAS_OVERRIDE_THRESHOLD = 200; // disallow controls when user torque exceeds this value
struct lookup_t {
float x[3];
float y[3];
};
// 2m/s are added to be less restrictive
const struct lookup_t LOOKUP_ANGLE_RATE_UP = {
{2., 7., 17.},
@ -35,31 +30,6 @@ uint32_t ts_angle_last = 0;
int controls_allowed_last = 0;
// interp function that holds extreme values
float interpolate(struct lookup_t xy, float x) {
int size = sizeof(xy.x) / sizeof(xy.x[0]);
// x is lower than the first point in the x array. Return the first point
if (x <= xy.x[0]) {
return xy.y[0];
} else {
// find the index such that (xy.x[i] <= x < xy.x[i+1]) and linearly interp
for (int i=0; i < size-1; i++) {
if (x < xy.x[i+1]) {
float x0 = xy.x[i];
float y0 = xy.y[i];
float dx = xy.x[i+1] - x0;
float dy = xy.y[i+1] - y0;
// dx should not be zero as xy.x is supposed ot be monotonic
if (dx <= 0.) dx = 0.0001;
return dy * (x - x0) / dx + y0;
}
}
// if no such point is found, then x > xy.x[size-1]. Return last point
return xy.y[size - 1];
}
}
static void toyota_ipas_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
// check standard toyota stuff as well
@ -146,15 +116,15 @@ static int toyota_ipas_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
int delta_angle_down = (int) (interpolate(LOOKUP_ANGLE_RATE_DOWN, speed) * CAN_TO_DEG + 1.);
int highest_desired_angle = desired_angle_last + (desired_angle_last > 0? delta_angle_up:delta_angle_down);
int lowest_desired_angle = desired_angle_last - (desired_angle_last > 0? delta_angle_down:delta_angle_up);
if ((desired_angle > highest_desired_angle) ||
if ((desired_angle > highest_desired_angle) ||
(desired_angle < lowest_desired_angle)){
violation = 1;
controls_allowed = 0;
}
}
// desired steer angle should be the same as steer angle measured when controls are off
if ((!controls_allowed) &&
if ((!controls_allowed) &&
((desired_angle < (angle_meas.min - 1)) ||
(desired_angle > (angle_meas.max + 1)) ||
(ipas_state_cmd != 1))) {