Merge panda subtree

Vehicle Researcher 2018-08-19 20:34:21 -07:00
commit b70d75d1d0
10 changed files with 386 additions and 242 deletions

View File

@ -25,9 +25,13 @@ jobs:
command: |
docker run panda_build /bin/bash -c "cd /panda; python setup.py install"
- run:
name: Build STM image
name: Build Panda STM image
command: |
docker run panda_build /bin/bash -c "cd /panda/board; make bin"
- run:
name: Build NEO STM image
command: |
docker run panda_build /bin/bash -c "cd /panda/board; make clean; make -f Makefile.legacy obj/comma.bin"
- run:
name: Build ESP image
command: |

View File

@ -461,8 +461,6 @@ void CAN3_SCE_IRQHandler() { can_sce(CAN3); }
#endif
#include "canbitbang.h"
void can_send(CAN_FIFOMailBox_TypeDef *to_push, uint8_t bus_number) {
if (safety_tx_hook(to_push) && !can_autobaud_enabled[bus_number]) {
if (bus_number < BUS_MAX) {

View File

@ -1,203 +0,0 @@
#define MAX_BITS_CAN_PACKET (200)
// returns out_len
int do_bitstuff(char *out, char *in, int in_len) {
int last_bit = -1;
int bit_cnt = 0;
int j = 0;
for (int i = 0; i < in_len; i++) {
char bit = in[i];
out[j++] = bit;
// do the stuffing
if (bit == last_bit) {
bit_cnt++;
if (bit_cnt == 5) {
// 5 in a row the same, do stuff
last_bit = !bit;
out[j++] = last_bit;
bit_cnt = 1;
}
} else {
// this is a new bit
last_bit = bit;
bit_cnt = 1;
}
}
return j;
}
int append_crc(char *in, int in_len) {
int crc = 0;
for (int i = 0; i < in_len; i++) {
crc <<= 1;
if (in[i] ^ ((crc>>15)&1)) {
crc = crc ^ 0x4599;
}
crc &= 0x7fff;
}
for (int i = 14; i >= 0; i--) {
in[in_len++] = (crc>>i)&1;
}
return in_len;
}
int append_bits(char *in, int in_len, char *app, int app_len) {
for (int i = 0; i < app_len; i++) {
in[in_len++] = app[i];
}
return in_len;
}
int append_int(char *in, int in_len, int val, int val_len) {
for (int i = val_len-1; i >= 0; i--) {
in[in_len++] = (val&(1<<i)) != 0;
}
return in_len;
}
int get_bit_message(char *out, CAN_FIFOMailBox_TypeDef *to_bang) {
char pkt[MAX_BITS_CAN_PACKET];
char footer[] = {
1, // CRC delimiter
1, // ACK
1, // ACK delimiter
1,1,1,1,1,1,1, // EOF
1,1,1, // IFS
};
int len = 0;
// test packet
int dlc_len = to_bang->RDTR & 0xF;
len = append_int(pkt, len, 0, 1); // Start-of-frame
if (to_bang->RIR & 4) {
// extended identifier
len = append_int(pkt, len, to_bang->RIR >> 21, 11); // Identifier
len = append_int(pkt, len, 3, 2); // SRR+IDE
len = append_int(pkt, len, (to_bang->RIR >> 3) & ((1<<18)-1), 18); // Identifier
len = append_int(pkt, len, 0, 3); // RTR+r1+r0
} else {
// standard identifier
len = append_int(pkt, len, to_bang->RIR >> 21, 11); // Identifier
len = append_int(pkt, len, 0, 3); // RTR+IDE+reserved
}
len = append_int(pkt, len, dlc_len, 4); // Data length code
// append data
for (int i = 0; i < dlc_len; i++) {
unsigned char dat = ((unsigned char *)(&(to_bang->RDLR)))[i];
len = append_int(pkt, len, dat, 8);
}
// append crc
len = append_crc(pkt, len);
// do bitstuffing
len = do_bitstuff(out, pkt, len);
// append footer
len = append_bits(out, len, footer, sizeof(footer));
return len;
}
// hardware stuff below this line
#ifdef PANDA
void set_bitbanged_gmlan(int val) {
if (val) {
GPIOB->ODR |= (1 << 13);
} else {
GPIOB->ODR &= ~(1 << 13);
}
}
char pkt_stuffed[MAX_BITS_CAN_PACKET];
int gmlan_sending = -1;
int gmlan_sendmax = -1;
int gmlan_silent_count = 0;
int gmlan_fail_count = 0;
#define REQUIRED_SILENT_TIME 10
#define MAX_FAIL_COUNT 10
void TIM4_IRQHandler(void) {
if (TIM4->SR & TIM_SR_UIF && gmlan_sendmax != -1) {
int read = get_gpio_input(GPIOB, 12);
if (gmlan_silent_count < REQUIRED_SILENT_TIME) {
if (read == 0) {
gmlan_silent_count = 0;
} else {
gmlan_silent_count++;
}
} else if (gmlan_silent_count == REQUIRED_SILENT_TIME) {
int retry = 0;
// in send loop
if (gmlan_sending > 0 && // not first bit
(read == 0 && pkt_stuffed[gmlan_sending-1] == 1) && // bus wrongly dominant
gmlan_sending != (gmlan_sendmax-11)) { //not ack bit
puts("GMLAN ERR: bus driven at ");
puth(gmlan_sending);
puts("\n");
retry = 1;
} else if (read == 1 && gmlan_sending == (gmlan_sendmax-11)) { // recessive during ACK
puts("GMLAN ERR: didn't recv ACK\n");
retry = 1;
}
if (retry) {
// reset sender (retry after 7 silent)
set_bitbanged_gmlan(1); // recessive
gmlan_silent_count = 0;
gmlan_sending = 0;
gmlan_fail_count++;
if (gmlan_fail_count == MAX_FAIL_COUNT) {
puts("GMLAN ERR: giving up send\n");
}
} else {
set_bitbanged_gmlan(pkt_stuffed[gmlan_sending]);
gmlan_sending++;
}
}
if (gmlan_sending == gmlan_sendmax || gmlan_fail_count == MAX_FAIL_COUNT) {
set_bitbanged_gmlan(1); // recessive
set_gpio_mode(GPIOB, 13, MODE_INPUT);
TIM4->DIER = 0; // no update interrupt
TIM4->CR1 = 0; // disable timer
gmlan_sendmax = -1; // exit
}
}
TIM4->SR = 0;
}
void bitbang_gmlan(CAN_FIFOMailBox_TypeDef *to_bang) {
// TODO: make failure less silent
if (gmlan_sendmax != -1) return;
int len = get_bit_message(pkt_stuffed, to_bang);
gmlan_fail_count = 0;
gmlan_silent_count = 0;
gmlan_sending = 0;
gmlan_sendmax = len;
// setup for bitbang loop
set_bitbanged_gmlan(1); // recessive
set_gpio_mode(GPIOB, 13, MODE_OUTPUT);
// setup
TIM4->PSC = 48-1; // tick on 1 us
TIM4->CR1 = TIM_CR1_CEN; // enable
TIM4->ARR = 30-1; // 33.3 kbps
// in case it's disabled
NVIC_EnableIRQ(TIM4_IRQn);
// run the interrupt
TIM4->DIER = TIM_DIER_UIE; // update interrupt
TIM4->SR = 0;
}
#endif

View File

@ -0,0 +1,276 @@
#define GMLAN_TICKS_PER_SECOND 33300 //1sec @ 33.3kbps
#define GMLAN_TICKS_PER_TIMEOUT_TICKLE 500 //15ms @ 33.3kbps
#define GMLAN_HIGH 0 //0 is high on bus (dominant)
#define GMLAN_LOW 1 //1 is low on bus
#define DISABLED -1
#define BITBANG 0
#define GPIO_SWITCH 1
#define MAX_BITS_CAN_PACKET (200)
int gmlan_alt_mode = DISABLED;
// returns out_len
int do_bitstuff(char *out, char *in, int in_len) {
int last_bit = -1;
int bit_cnt = 0;
int j = 0;
for (int i = 0; i < in_len; i++) {
char bit = in[i];
out[j++] = bit;
// do the stuffing
if (bit == last_bit) {
bit_cnt++;
if (bit_cnt == 5) {
// 5 in a row the same, do stuff
last_bit = !bit;
out[j++] = last_bit;
bit_cnt = 1;
}
} else {
// this is a new bit
last_bit = bit;
bit_cnt = 1;
}
}
return j;
}
int append_crc(char *in, int in_len) {
int crc = 0;
for (int i = 0; i < in_len; i++) {
crc <<= 1;
if (in[i] ^ ((crc>>15)&1)) {
crc = crc ^ 0x4599;
}
crc &= 0x7fff;
}
for (int i = 14; i >= 0; i--) {
in[in_len++] = (crc>>i)&1;
}
return in_len;
}
int append_bits(char *in, int in_len, char *app, int app_len) {
for (int i = 0; i < app_len; i++) {
in[in_len++] = app[i];
}
return in_len;
}
int append_int(char *in, int in_len, int val, int val_len) {
for (int i = val_len-1; i >= 0; i--) {
in[in_len++] = (val&(1<<i)) != 0;
}
return in_len;
}
int get_bit_message(char *out, CAN_FIFOMailBox_TypeDef *to_bang) {
char pkt[MAX_BITS_CAN_PACKET];
char footer[] = {
1, // CRC delimiter
1, // ACK
1, // ACK delimiter
1,1,1,1,1,1,1, // EOF
1,1,1, // IFS
};
int len = 0;
// test packet
int dlc_len = to_bang->RDTR & 0xF;
len = append_int(pkt, len, 0, 1); // Start-of-frame
if (to_bang->RIR & 4) {
// extended identifier
len = append_int(pkt, len, to_bang->RIR >> 21, 11); // Identifier
len = append_int(pkt, len, 3, 2); // SRR+IDE
len = append_int(pkt, len, (to_bang->RIR >> 3) & ((1<<18)-1), 18); // Identifier
len = append_int(pkt, len, 0, 3); // RTR+r1+r0
} else {
// standard identifier
len = append_int(pkt, len, to_bang->RIR >> 21, 11); // Identifier
len = append_int(pkt, len, 0, 3); // RTR+IDE+reserved
}
len = append_int(pkt, len, dlc_len, 4); // Data length code
// append data
for (int i = 0; i < dlc_len; i++) {
unsigned char dat = ((unsigned char *)(&(to_bang->RDLR)))[i];
len = append_int(pkt, len, dat, 8);
}
// append crc
len = append_crc(pkt, len);
// do bitstuffing
len = do_bitstuff(out, pkt, len);
// append footer
len = append_bits(out, len, footer, sizeof(footer));
return len;
}
#ifdef PANDA
void setup_timer4() {
// setup
TIM4->PSC = 48-1; // tick on 1 us
TIM4->CR1 = TIM_CR1_CEN; // enable
TIM4->ARR = 30-1; // 33.3 kbps
// in case it's disabled
NVIC_EnableIRQ(TIM4_IRQn);
// run the interrupt
TIM4->DIER = TIM_DIER_UIE; // update interrupt
TIM4->SR = 0;
}
int gmlan_timeout_counter = GMLAN_TICKS_PER_TIMEOUT_TICKLE; //GMLAN transceiver times out every 17ms held high; tickle every 15ms
int can_timeout_counter = GMLAN_TICKS_PER_SECOND; //1 second
int inverted_bit_to_send = GMLAN_HIGH;
int gmlan_switch_below_timeout = -1;
int gmlan_switch_timeout_enable = 0;
void gmlan_switch_init(int timeout_enable) {
gmlan_switch_timeout_enable = timeout_enable;
gmlan_alt_mode = GPIO_SWITCH;
gmlan_switch_below_timeout = 1;
set_gpio_mode(GPIOB, 13, MODE_OUTPUT);
setup_timer4();
inverted_bit_to_send = GMLAN_LOW; //We got initialized, set the output low
}
void set_gmlan_digital_output(int to_set) {
inverted_bit_to_send = to_set;
/*
puts("Writing ");
puth(inverted_bit_to_send);
puts("\n");
*/
}
void reset_gmlan_switch_timeout(void) {
can_timeout_counter = GMLAN_TICKS_PER_SECOND;
gmlan_switch_below_timeout = 1;
gmlan_alt_mode = GPIO_SWITCH;
}
void set_bitbanged_gmlan(int val) {
if (val) {
GPIOB->ODR |= (1 << 13);
} else {
GPIOB->ODR &= ~(1 << 13);
}
}
char pkt_stuffed[MAX_BITS_CAN_PACKET];
int gmlan_sending = -1;
int gmlan_sendmax = -1;
int gmlan_silent_count = 0;
int gmlan_fail_count = 0;
#define REQUIRED_SILENT_TIME 10
#define MAX_FAIL_COUNT 10
void TIM4_IRQHandler(void) {
if (gmlan_alt_mode == BITBANG) {
if (TIM4->SR & TIM_SR_UIF && gmlan_sendmax != -1) {
int read = get_gpio_input(GPIOB, 12);
if (gmlan_silent_count < REQUIRED_SILENT_TIME) {
if (read == 0) {
gmlan_silent_count = 0;
} else {
gmlan_silent_count++;
}
} else if (gmlan_silent_count == REQUIRED_SILENT_TIME) {
int retry = 0;
// in send loop
if (gmlan_sending > 0 && // not first bit
(read == 0 && pkt_stuffed[gmlan_sending-1] == 1) && // bus wrongly dominant
gmlan_sending != (gmlan_sendmax-11)) { //not ack bit
puts("GMLAN ERR: bus driven at ");
puth(gmlan_sending);
puts("\n");
retry = 1;
} else if (read == 1 && gmlan_sending == (gmlan_sendmax-11)) { // recessive during ACK
puts("GMLAN ERR: didn't recv ACK\n");
retry = 1;
}
if (retry) {
// reset sender (retry after 7 silent)
set_bitbanged_gmlan(1); // recessive
gmlan_silent_count = 0;
gmlan_sending = 0;
gmlan_fail_count++;
if (gmlan_fail_count == MAX_FAIL_COUNT) {
puts("GMLAN ERR: giving up send\n");
}
} else {
set_bitbanged_gmlan(pkt_stuffed[gmlan_sending]);
gmlan_sending++;
}
}
if (gmlan_sending == gmlan_sendmax || gmlan_fail_count == MAX_FAIL_COUNT) {
set_bitbanged_gmlan(1); // recessive
set_gpio_mode(GPIOB, 13, MODE_INPUT);
TIM4->DIER = 0; // no update interrupt
TIM4->CR1 = 0; // disable timer
gmlan_sendmax = -1; // exit
}
}
TIM4->SR = 0;
} //bit bang mode
else if (gmlan_alt_mode == GPIO_SWITCH) {
if (TIM4->SR & TIM_SR_UIF && gmlan_switch_below_timeout != -1) {
if (can_timeout_counter == 0 && gmlan_switch_timeout_enable) {
//it has been more than 1 second since timeout was reset; disable timer and restore the GMLAN output
set_gpio_output(GPIOB, 13, GMLAN_LOW);
gmlan_switch_below_timeout = -1;
gmlan_timeout_counter = GMLAN_TICKS_PER_TIMEOUT_TICKLE;
gmlan_alt_mode = DISABLED;
}
else {
can_timeout_counter--;
if (gmlan_timeout_counter == 0) {
//Send a 1 (bus low) every 15ms to reset the GMLAN transceivers timeout
gmlan_timeout_counter = GMLAN_TICKS_PER_TIMEOUT_TICKLE;
set_gpio_output(GPIOB, 13, GMLAN_LOW);
}
else {
set_gpio_output(GPIOB, 13, inverted_bit_to_send);
gmlan_timeout_counter--;
}
}
}
TIM4->SR = 0;
} //gmlan switch mode
}
void bitbang_gmlan(CAN_FIFOMailBox_TypeDef *to_bang) {
gmlan_alt_mode = BITBANG;
// TODO: make failure less silent
if (gmlan_sendmax != -1) return;
int len = get_bit_message(pkt_stuffed, to_bang);
gmlan_fail_count = 0;
gmlan_silent_count = 0;
gmlan_sending = 0;
gmlan_sendmax = len;
// setup for bitbang loop
set_bitbanged_gmlan(1); // recessive
set_gpio_mode(GPIOB, 13, MODE_OUTPUT);
setup_timer4();
}
#endif

View File

@ -15,6 +15,7 @@
#include "drivers/uart.h"
#include "drivers/adc.h"
#include "drivers/usb.h"
#include "drivers/gmlan_alt.h"
#include "drivers/can.h"
#include "drivers/spi.h"
#include "drivers/timer.h"

View File

@ -5,6 +5,14 @@ struct sample_t {
int max;
} sample_t_default = {{0}, 0, 0};
// no float support in STM32F2 micros (cortex-m3)
#ifdef PANDA
struct lookup_t {
float x[3];
float y[3];
};
#endif
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 +24,12 @@ 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);
#ifdef PANDA
float interpolate(struct lookup_t xy, float x);
#endif
typedef void (*safety_hook_init)(int16_t param);
typedef void (*rx_hook)(CAN_FIFOMailBox_TypeDef *to_push);
@ -49,6 +60,7 @@ int controls_allowed = 0;
#include "safety/safety_gm.h"
#include "safety/safety_ford.h"
#include "safety/safety_cadillac.h"
#include "safety/safety_hyundai.h"
#include "safety/safety_elm327.h"
const safety_hooks *current_hooks = &nooutput_hooks;
@ -87,6 +99,7 @@ typedef struct {
#define SAFETY_HONDA_BOSCH 4
#define SAFETY_FORD 5
#define SAFETY_CADILLAC 6
#define SAFETY_HYUNDAI 7
#define SAFETY_TOYOTA_IPAS 0x1335
#define SAFETY_TOYOTA_NOLIMITS 0x1336
#define SAFETY_ALLOUTPUT 0x1337
@ -100,6 +113,7 @@ const safety_hook_config safety_hook_registry[] = {
{SAFETY_GM, &gm_hooks},
{SAFETY_FORD, &ford_hooks},
{SAFETY_CADILLAC, &cadillac_hooks},
{SAFETY_HYUNDAI, &hyundai_hooks},
{SAFETY_TOYOTA_NOLIMITS, &toyota_nolimits_hooks},
#ifdef PANDA
{SAFETY_TOYOTA_IPAS, &toyota_ipas_hooks},
@ -201,3 +215,31 @@ 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);
}
#ifdef PANDA
// 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];
}
}
#endif

View File

@ -0,0 +1,36 @@
int hyundai_giraffe_switch_1 = 0; // is giraffe switch 1 high?
static void hyundai_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
int bus = (to_push->RDTR >> 4) & 0xF;
// 832 is lkas cmd. If it is on bus 0, then giraffe switch 1 is high and we want stock
if ((to_push->RIR>>21) == 832 && (bus == 0)) {
hyundai_giraffe_switch_1 = 1;
}
}
static void hyundai_init(int16_t param) {
controls_allowed = 0;
hyundai_giraffe_switch_1 = 0;
}
static int hyundai_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
// forward camera to car and viceversa, excpet for lkas11 and mdps12
if ((bus_num == 0 || bus_num == 2) && !hyundai_giraffe_switch_1) {
int addr = to_fwd->RIR>>21;
bool is_lkas_msg = (addr == 832 && bus_num == 2) || (addr == 593 && bus_num == 0);
return is_lkas_msg? -1 : (uint8_t)(~bus_num & 0x2);
}
return -1;
}
const safety_hooks hyundai_hooks = {
.init = hyundai_init,
.rx = hyundai_rx_hook,
.tx = nooutput_tx_hook,
.tx_lin = nooutput_tx_lin_hook,
.ignition = default_ign_hook,
.fwd = hyundai_fwd_hook,
};

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))) {

View File

@ -2,13 +2,13 @@
Dependencies
-----
**Mac**
**Debian / Ubuntu**
```
./get_sdk.sh
```
**Debian / Ubuntu**
**Mac**
```
./get_sdk_mac.sh

View File

@ -0,0 +1,20 @@
#!/usr/bin/env python
from panda import Panda
def get_panda_password():
try:
print("Trying to connect to Panda over USB...")
p = Panda()
except AssertionError:
print("USB connection failed")
sys.exit(0)
wifi = p.get_serial()
#print('[%s]' % ', '.join(map(str, wifi)))
print("SSID: " + wifi[0])
print("Password: " + wifi[1])
if __name__ == "__main__":
get_panda_password()