2018-05-31 19:49:12 -06:00
|
|
|
// sample struct that keeps 3 samples in memory
|
|
|
|
struct sample_t {
|
2018-06-03 18:39:57 -06:00
|
|
|
int values[6];
|
2018-05-31 19:49:12 -06:00
|
|
|
int min;
|
|
|
|
int max;
|
2018-06-03 18:39:57 -06:00
|
|
|
} sample_t_default = {{0}, 0, 0};
|
2018-05-31 19:49:12 -06:00
|
|
|
|
2017-07-14 22:17:32 -06:00
|
|
|
void safety_rx_hook(CAN_FIFOMailBox_TypeDef *to_push);
|
2017-07-21 00:36:06 -06:00
|
|
|
int safety_tx_hook(CAN_FIFOMailBox_TypeDef *to_send);
|
|
|
|
int safety_tx_lin_hook(int lin_num, uint8_t *data, int len);
|
2018-04-10 18:55:55 -06:00
|
|
|
int safety_ignition_hook();
|
2018-05-25 14:57:19 -06:00
|
|
|
uint32_t get_ts_elapsed(uint32_t ts, uint32_t ts_last);
|
2018-05-25 18:52:37 -06:00
|
|
|
int to_signed(int d, int bits);
|
2018-05-31 19:49:12 -06:00
|
|
|
void update_sample(struct sample_t *sample, int sample_new);
|
2018-06-13 01:56:29 -06:00
|
|
|
int rt_rate_limit_check(int val, int val_last, const int MAX_RT_DELTA);
|
2018-06-13 02:11:32 -06:00
|
|
|
int max_limit_check(int val, const int MAX);
|
2017-07-14 22:17:32 -06:00
|
|
|
|
2018-01-26 00:57:04 -07:00
|
|
|
typedef void (*safety_hook_init)(int16_t param);
|
2017-07-14 22:17:32 -06:00
|
|
|
typedef void (*rx_hook)(CAN_FIFOMailBox_TypeDef *to_push);
|
2017-07-21 00:36:06 -06:00
|
|
|
typedef int (*tx_hook)(CAN_FIFOMailBox_TypeDef *to_send);
|
|
|
|
typedef int (*tx_lin_hook)(int lin_num, uint8_t *data, int len);
|
2018-04-10 18:55:55 -06:00
|
|
|
typedef int (*ign_hook)();
|
2018-02-28 00:27:47 -07:00
|
|
|
typedef int (*fwd_hook)(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd);
|
2017-07-14 22:17:32 -06:00
|
|
|
|
|
|
|
typedef struct {
|
|
|
|
safety_hook_init init;
|
2018-04-10 18:55:55 -06:00
|
|
|
ign_hook ignition;
|
2017-07-14 22:17:32 -06:00
|
|
|
rx_hook rx;
|
|
|
|
tx_hook tx;
|
|
|
|
tx_lin_hook tx_lin;
|
2018-02-28 00:27:47 -07:00
|
|
|
fwd_hook fwd;
|
2017-07-14 22:17:32 -06:00
|
|
|
} safety_hooks;
|
|
|
|
|
2017-07-21 12:48:03 -06:00
|
|
|
// This can be set by the safety hooks.
|
|
|
|
int controls_allowed = 0;
|
|
|
|
|
2017-07-14 22:17:32 -06:00
|
|
|
// Include the actual safety policies.
|
2017-07-17 11:20:08 -06:00
|
|
|
#include "safety/safety_defaults.h"
|
|
|
|
#include "safety/safety_honda.h"
|
2017-08-24 23:31:34 -06:00
|
|
|
#include "safety/safety_toyota.h"
|
2018-04-11 21:47:29 -06:00
|
|
|
#ifdef PANDA
|
2018-04-11 15:31:45 -06:00
|
|
|
#include "safety/safety_toyota_ipas.h"
|
2018-04-11 21:47:29 -06:00
|
|
|
#endif
|
2018-02-23 01:19:22 -07:00
|
|
|
#include "safety/safety_gm.h"
|
2018-05-02 23:45:23 -06:00
|
|
|
#include "safety/safety_ford.h"
|
2018-05-25 11:36:25 -06:00
|
|
|
#include "safety/safety_cadillac.h"
|
2017-08-11 17:17:43 -06:00
|
|
|
#include "safety/safety_elm327.h"
|
2017-07-14 22:17:32 -06:00
|
|
|
|
|
|
|
const safety_hooks *current_hooks = &nooutput_hooks;
|
|
|
|
|
|
|
|
void safety_rx_hook(CAN_FIFOMailBox_TypeDef *to_push){
|
|
|
|
current_hooks->rx(to_push);
|
|
|
|
}
|
|
|
|
|
2017-07-21 00:36:06 -06:00
|
|
|
int safety_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
|
|
|
|
return current_hooks->tx(to_send);
|
2017-07-14 22:17:32 -06:00
|
|
|
}
|
|
|
|
|
2017-07-21 00:36:06 -06:00
|
|
|
int safety_tx_lin_hook(int lin_num, uint8_t *data, int len){
|
|
|
|
return current_hooks->tx_lin(lin_num, data, len);
|
2017-07-14 22:17:32 -06:00
|
|
|
}
|
|
|
|
|
2018-04-10 18:55:55 -06:00
|
|
|
// -1 = Disabled (Use GPIO to determine ignition)
|
|
|
|
// 0 = Off (not started)
|
|
|
|
// 1 = On (started)
|
|
|
|
int safety_ignition_hook() {
|
|
|
|
return current_hooks->ignition();
|
|
|
|
}
|
2018-02-28 00:27:47 -07:00
|
|
|
int safety_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
|
|
|
|
return current_hooks->fwd(bus_num, to_fwd);
|
|
|
|
}
|
|
|
|
|
2017-07-14 22:17:32 -06:00
|
|
|
typedef struct {
|
|
|
|
uint16_t id;
|
|
|
|
const safety_hooks *hooks;
|
|
|
|
} safety_hook_config;
|
|
|
|
|
2017-07-17 11:48:16 -06:00
|
|
|
#define SAFETY_NOOUTPUT 0
|
|
|
|
#define SAFETY_HONDA 1
|
2017-08-24 23:31:34 -06:00
|
|
|
#define SAFETY_TOYOTA 2
|
2018-02-23 01:19:22 -07:00
|
|
|
#define SAFETY_GM 3
|
2018-02-28 00:27:47 -07:00
|
|
|
#define SAFETY_HONDA_BOSCH 4
|
2018-05-02 23:45:23 -06:00
|
|
|
#define SAFETY_FORD 5
|
2018-05-25 11:36:25 -06:00
|
|
|
#define SAFETY_CADILLAC 6
|
2018-05-02 23:45:23 -06:00
|
|
|
#define SAFETY_TOYOTA_IPAS 0x1335
|
|
|
|
#define SAFETY_TOYOTA_NOLIMITS 0x1336
|
2017-07-17 11:48:16 -06:00
|
|
|
#define SAFETY_ALLOUTPUT 0x1337
|
2017-08-11 17:17:43 -06:00
|
|
|
#define SAFETY_ELM327 0xE327
|
2017-07-17 11:48:16 -06:00
|
|
|
|
2017-07-14 22:17:32 -06:00
|
|
|
const safety_hook_config safety_hook_registry[] = {
|
2017-07-17 11:48:16 -06:00
|
|
|
{SAFETY_NOOUTPUT, &nooutput_hooks},
|
|
|
|
{SAFETY_HONDA, &honda_hooks},
|
2018-02-28 00:27:47 -07:00
|
|
|
{SAFETY_HONDA_BOSCH, &honda_bosch_hooks},
|
2017-08-24 23:31:34 -06:00
|
|
|
{SAFETY_TOYOTA, &toyota_hooks},
|
2018-05-02 23:45:23 -06:00
|
|
|
{SAFETY_GM, &gm_hooks},
|
|
|
|
{SAFETY_FORD, &ford_hooks},
|
2018-05-25 11:36:25 -06:00
|
|
|
{SAFETY_CADILLAC, &cadillac_hooks},
|
2017-09-16 01:53:09 -06:00
|
|
|
{SAFETY_TOYOTA_NOLIMITS, &toyota_nolimits_hooks},
|
2018-04-11 21:47:29 -06:00
|
|
|
#ifdef PANDA
|
2018-04-11 15:31:45 -06:00
|
|
|
{SAFETY_TOYOTA_IPAS, &toyota_ipas_hooks},
|
2018-04-11 21:47:29 -06:00
|
|
|
#endif
|
2017-07-17 11:48:16 -06:00
|
|
|
{SAFETY_ALLOUTPUT, &alloutput_hooks},
|
2017-08-11 17:17:43 -06:00
|
|
|
{SAFETY_ELM327, &elm327_hooks},
|
2017-07-14 22:17:32 -06:00
|
|
|
};
|
|
|
|
|
|
|
|
#define HOOK_CONFIG_COUNT (sizeof(safety_hook_registry)/sizeof(safety_hook_config))
|
|
|
|
|
2018-01-26 00:57:04 -07:00
|
|
|
int safety_set_mode(uint16_t mode, int16_t param) {
|
2017-07-17 11:27:34 -06:00
|
|
|
for (int i = 0; i < HOOK_CONFIG_COUNT; i++) {
|
|
|
|
if (safety_hook_registry[i].id == mode) {
|
2017-07-14 22:17:32 -06:00
|
|
|
current_hooks = safety_hook_registry[i].hooks;
|
2018-01-26 00:57:04 -07:00
|
|
|
if (current_hooks->init) current_hooks->init(param);
|
2017-07-14 22:17:32 -06:00
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
return -1;
|
|
|
|
}
|
2017-08-23 13:49:56 -06:00
|
|
|
|
2018-05-25 14:57:19 -06:00
|
|
|
// compute the time elapsed (in microseconds) from 2 counter samples
|
|
|
|
uint32_t get_ts_elapsed(uint32_t ts, uint32_t ts_last) {
|
|
|
|
return ts > ts_last ? ts - ts_last : (0xFFFFFFFF - ts_last) + 1 + ts;
|
|
|
|
}
|
2018-05-25 18:52:37 -06:00
|
|
|
|
|
|
|
// convert a trimmed integer to signed 32 bit int
|
|
|
|
int to_signed(int d, int bits) {
|
|
|
|
if (d >= (1 << (bits - 1))) {
|
|
|
|
d -= (1 << bits);
|
|
|
|
}
|
|
|
|
return d;
|
|
|
|
}
|
2018-05-31 19:49:12 -06:00
|
|
|
|
|
|
|
// 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];
|
|
|
|
}
|
|
|
|
}
|
2018-06-13 01:56:29 -06:00
|
|
|
|
|
|
|
// 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) {
|
|
|
|
|
2018-06-13 01:57:36 -06:00
|
|
|
// *** torque real time rate limit check ***
|
|
|
|
int16_t highest_val = max(val_last, 0) + MAX_RT_DELTA;
|
|
|
|
int16_t lowest_val = min(val_last, 0) - MAX_RT_DELTA;
|
2018-06-13 01:56:29 -06:00
|
|
|
|
2018-06-13 01:57:36 -06:00
|
|
|
// return 1 if violation
|
|
|
|
return (val < lowest_val) || (val > highest_val);
|
2018-06-13 01:56:29 -06:00
|
|
|
}
|
2018-06-13 02:11:32 -06:00
|
|
|
|
|
|
|
int max_limit_check(int val, const int MAX) {
|
|
|
|
return (val > MAX) | (val < -MAX);
|
|
|
|
}
|