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

Toyota: moved common functions into safety header file

master
Commaremote 2018-05-31 18:49:12 -07:00
parent 40c8ddaf7d
commit 79ab5af8c3
2 changed files with 23 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
board/safety.h
View File

@ -1,9 +1,17 @@
// sample struct that keeps 3 samples in memory
struct sample_t {
int values[3];
int min;
int max;
} sample_t_default = {{0, 0, 0}, 0, 0};
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);
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);
typedef void (*safety_hook_init)(int16_t param);
typedef void (*rx_hook)(CAN_FIFOMailBox_TypeDef *to_push);
@ -118,3 +126,18 @@ int to_signed(int d, int bits) {
}
return d;
}
// given a new sample, update the smaple_t struct
void update_sample(struct sample_t *sample, int sample_new) {
for (int i = sizeof(sample->values)/sizeof(sample->values[0]) - 1; i > 0; i--) {
sample->values[i] = sample->values[i-1];
}
sample->values[0] = sample_new;
// get the minimum and maximum measured samples
sample->min = sample->max = sample->values[0];
for (int i = 1; i < sizeof(sample->values)/sizeof(sample->values[0]); i++) {
if (sample->values[i] < sample->min) sample->min = sample->values[i];
if (sample->values[i] > sample->max) sample->max = sample->values[i];
}
}

19
board/safety/safety_toyota.h
View File

@ -1,9 +1,3 @@
// track the torque measured for limiting
struct sample_t {
int values[3];
int min;
int max;
} sample_t_default = {{0, 0, 0}, 0, 0};
struct sample_t torque_meas; // last 3 motor torques produced by the eps
// global torque limit
@ -34,19 +28,6 @@ int16_t rt_torque_last = 0; // last desired torque for real time chec
uint32_t ts_last = 0;
int cruise_engaged_last = 0; // cruise state
void update_sample(struct sample_t *sample, int sample_new) {
for (int i = sizeof(sample->values)/sizeof(sample->values[0]) - 1; i > 0; i--) {
sample->values[i] = sample->values[i-1];
}
sample->values[0] = sample_new;
// get the minimum and maximum measured torque over the last 3 frames
sample->min = sample->max = sample->values[0];
for (int i = 1; i < sizeof(sample->values)/sizeof(sample->values[0]); i++) {
if (sample->values[i] < sample->min) sample->min = sample->values[i];
if (sample->values[i] > sample->max) sample->max = sample->values[i];
}
}
static void toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
// get eps motor torque (0.66 factor in dbc)