Pigeon abstraction layer (#1977)
* pigeon abstraction layer
* Fix string literals
* more generic pigeon class
* add TTYpigon
* nicer tty error handling
* close tty fd on pigeon delete
* pigeon receive return std::string
* use sizeof
* max receive size to prevent infinite loop
* remove namespace
* add unistd include for usleep
* fix is pigeon
* Handle tty error in opening
* fix printing binary strings with dump.py
* fix pigeon build on macos
* Handle errors seperately
Co-authored-by: Comma Device <device@comma.ai>
old-commit-hash: 502cc665e0
commatwo_master
Willem Melching
5 years ago
committed by
GitHub
parent
e25063a0b3
commit
938ce91d02
11 changed files with 443 additions and 99 deletions
@ -0,0 +1,226 @@ |
||||
#include <cassert> |
||||
#include <unistd.h> |
||||
#include <fcntl.h> |
||||
#include <errno.h> |
||||
#include <termios.h> |
||||
|
||||
#include "common/swaglog.h" |
||||
#include "common/gpio.h" |
||||
|
||||
#include "pigeon.h" |
||||
|
||||
// Termios on macos doesn't define all baud rate constants
|
||||
#ifndef B460800 |
||||
#define B460800 0010004 |
||||
#endif |
||||
|
||||
using namespace std::string_literals; |
||||
|
||||
|
||||
Pigeon * Pigeon::connect(Panda * p){ |
||||
PandaPigeon * pigeon = new PandaPigeon(); |
||||
pigeon->connect(p); |
||||
|
||||
return pigeon; |
||||
} |
||||
|
||||
Pigeon * Pigeon::connect(const char * tty){ |
||||
TTYPigeon * pigeon = new TTYPigeon(); |
||||
pigeon->connect(tty); |
||||
|
||||
return pigeon; |
||||
} |
||||
|
||||
void Pigeon::init() { |
||||
usleep(1000*1000); |
||||
LOGW("panda GPS start"); |
||||
|
||||
// power off pigeon
|
||||
set_power(0); |
||||
usleep(100*1000); |
||||
|
||||
// 9600 baud at init
|
||||
set_baud(9600); |
||||
|
||||
// power on pigeon
|
||||
set_power(1); |
||||
usleep(500*1000); |
||||
|
||||
// baud rate upping
|
||||
send("\x24\x50\x55\x42\x58\x2C\x34\x31\x2C\x31\x2C\x30\x30\x30\x37\x2C\x30\x30\x30\x33\x2C\x34\x36\x30\x38\x30\x30\x2C\x30\x2A\x31\x35\x0D\x0A"s); |
||||
usleep(100*1000); |
||||
|
||||
// set baud rate to 460800
|
||||
set_baud(460800); |
||||
usleep(100*1000); |
||||
|
||||
// init from ubloxd
|
||||
// To generate this data, run test/ubloxd.py with the print statements enabled in the write function in panda/python/serial.py
|
||||
send("\xB5\x62\x06\x00\x14\x00\x03\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x01\x00\x01\x00\x00\x00\x00\x00\x1E\x7F"s); |
||||
send("\xB5\x62\x06\x3E\x00\x00\x44\xD2"s); |
||||
send("\xB5\x62\x06\x00\x14\x00\x00\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x19\x35"s); |
||||
send("\xB5\x62\x06\x00\x14\x00\x01\x00\x00\x00\xC0\x08\x00\x00\x00\x08\x07\x00\x01\x00\x01\x00\x00\x00\x00\x00\xF4\x80"s); |
||||
send("\xB5\x62\x06\x00\x14\x00\x04\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x1D\x85"s); |
||||
send("\xB5\x62\x06\x00\x00\x00\x06\x18"s); |
||||
send("\xB5\x62\x06\x00\x01\x00\x01\x08\x22"s); |
||||
send("\xB5\x62\x06\x00\x01\x00\x02\x09\x23"s); |
||||
send("\xB5\x62\x06\x00\x01\x00\x03\x0A\x24"s); |
||||
send("\xB5\x62\x06\x08\x06\x00\x64\x00\x01\x00\x00\x00\x79\x10"s); |
||||
send("\xB5\x62\x06\x24\x24\x00\x05\x00\x04\x03\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x5A\x63"s); |
||||
send("\xB5\x62\x06\x1E\x14\x00\x00\x00\x00\x00\x01\x03\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x3C\x37"s); |
||||
send("\xB5\x62\x06\x24\x00\x00\x2A\x84"s); |
||||
send("\xB5\x62\x06\x23\x00\x00\x29\x81"s); |
||||
send("\xB5\x62\x06\x1E\x00\x00\x24\x72"s); |
||||
send("\xB5\x62\x06\x01\x03\x00\x01\x07\x01\x13\x51"s); |
||||
send("\xB5\x62\x06\x01\x03\x00\x02\x15\x01\x22\x70"s); |
||||
send("\xB5\x62\x06\x01\x03\x00\x02\x13\x01\x20\x6C"s); |
||||
send("\xB5\x62\x06\x01\x03\x00\x0A\x09\x01\x1E\x70"s); |
||||
|
||||
LOGW("panda GPS on"); |
||||
} |
||||
|
||||
void PandaPigeon::connect(Panda * p) { |
||||
panda = p; |
||||
} |
||||
|
||||
void PandaPigeon::set_baud(int baud) { |
||||
panda->usb_write(0xe2, 1, 0); |
||||
panda->usb_write(0xe4, 1, baud/300); |
||||
} |
||||
|
||||
void PandaPigeon::send(std::string s) { |
||||
int len = s.length(); |
||||
const char * dat = s.data(); |
||||
|
||||
unsigned char a[0x20+1]; |
||||
a[0] = 1; |
||||
for (int i=0; i<len; i+=0x20) { |
||||
int ll = std::min(0x20, len-i); |
||||
memcpy(&a[1], &dat[i], ll); |
||||
|
||||
panda->usb_bulk_write(2, a, ll+1); |
||||
} |
||||
} |
||||
|
||||
std::string PandaPigeon::receive() { |
||||
std::string r; |
||||
|
||||
while (true){ |
||||
unsigned char dat[0x40]; |
||||
int len = panda->usb_read(0xe0, 1, 0, dat, sizeof(dat)); |
||||
if (len <= 0 || r.length() > 0x1000) break; |
||||
r.append((char*)dat, len); |
||||
} |
||||
|
||||
return r; |
||||
} |
||||
|
||||
void PandaPigeon::set_power(bool power) { |
||||
panda->usb_write(0xd9, power, 0); |
||||
} |
||||
|
||||
PandaPigeon::~PandaPigeon(){ |
||||
} |
||||
|
||||
void handle_tty_issue(int err, const char func[]) { |
||||
LOGE_100("tty error %d \"%s\" in %s", err, strerror(err), func); |
||||
} |
||||
|
||||
void TTYPigeon::connect(const char * tty) { |
||||
pigeon_tty_fd = open(tty, O_RDWR); |
||||
if (pigeon_tty_fd < 0){ |
||||
handle_tty_issue(errno, __func__); |
||||
assert(pigeon_tty_fd >= 0); |
||||
} |
||||
assert(tcgetattr(pigeon_tty_fd, &pigeon_tty) == 0); |
||||
|
||||
// configure tty
|
||||
pigeon_tty.c_cflag &= ~PARENB; // disable parity
|
||||
pigeon_tty.c_cflag &= ~CSTOPB; // single stop bit
|
||||
pigeon_tty.c_cflag |= CS8; // 8 bits per byte
|
||||
pigeon_tty.c_cflag &= ~CRTSCTS; // no RTS/CTS flow control
|
||||
pigeon_tty.c_cflag |= CREAD | CLOCAL; // turn on READ & ignore ctrl lines
|
||||
pigeon_tty.c_lflag &= ~ICANON; // disable canonical mode
|
||||
pigeon_tty.c_lflag &= ~ISIG; // disable interpretation of INTR, QUIT and SUSP
|
||||
pigeon_tty.c_iflag &= ~(IXON | IXOFF | IXANY); // turn off software flow ctrl
|
||||
pigeon_tty.c_iflag &= ~(IGNBRK|BRKINT|PARMRK|ISTRIP|INLCR|IGNCR|ICRNL); // disable any special handling of received bytes
|
||||
pigeon_tty.c_oflag &= ~OPOST; // prevent special interpretation of output bytes
|
||||
pigeon_tty.c_oflag &= ~ONLCR; // prevent conversion of newline to carriage return/line feed
|
||||
|
||||
// configure blocking behavior
|
||||
pigeon_tty.c_cc[VMIN] = 0; // min amount of characters returned
|
||||
pigeon_tty.c_cc[VTIME] = 0; // max blocking time in s/10 (0=inf)
|
||||
|
||||
assert(tcsetattr(pigeon_tty_fd, TCSANOW, &pigeon_tty) == 0); |
||||
} |
||||
|
||||
void TTYPigeon::set_baud(int baud){ |
||||
speed_t baud_const = 0; |
||||
switch(baud){ |
||||
case 9600: |
||||
baud_const = B9600; |
||||
break; |
||||
case 460800: |
||||
baud_const = B460800; |
||||
break; |
||||
default: |
||||
assert(false); |
||||
} |
||||
|
||||
// make sure everything is tx'ed before changing baud
|
||||
assert(tcdrain(pigeon_tty_fd) == 0); |
||||
|
||||
// change baud
|
||||
assert(tcgetattr(pigeon_tty_fd, &pigeon_tty) == 0); |
||||
assert(cfsetspeed(&pigeon_tty, baud_const) == 0); |
||||
assert(tcsetattr(pigeon_tty_fd, TCSANOW, &pigeon_tty) == 0); |
||||
|
||||
// flush
|
||||
assert(tcflush(pigeon_tty_fd, TCIOFLUSH) == 0); |
||||
} |
||||
|
||||
void TTYPigeon::send(std::string s) { |
||||
int len = s.length(); |
||||
const char * dat = s.data(); |
||||
|
||||
int err = write(pigeon_tty_fd, dat, len); |
||||
if(err < 0) { handle_tty_issue(err, __func__); } |
||||
err = tcdrain(pigeon_tty_fd); |
||||
if(err < 0) { handle_tty_issue(err, __func__); } |
||||
} |
||||
|
||||
std::string TTYPigeon::receive() { |
||||
std::string r; |
||||
|
||||
while (true){ |
||||
char dat[0x40]; |
||||
int len = read(pigeon_tty_fd, dat, sizeof(dat)); |
||||
if(len < 0) { |
||||
handle_tty_issue(len, __func__); |
||||
} else if (len == 0 || r.length() > 0x1000){ |
||||
break; |
||||
} else { |
||||
r.append(dat, len); |
||||
} |
||||
|
||||
} |
||||
return r; |
||||
} |
||||
|
||||
void TTYPigeon::set_power(bool power){ |
||||
#ifdef QCOM2 |
||||
int err = 0; |
||||
err += gpio_init(GPIO_UBLOX_RST_N, true); |
||||
err += gpio_init(GPIO_UBLOX_SAFEBOOT_N, true); |
||||
err += gpio_init(GPIO_UBLOX_PWR_EN, true); |
||||
|
||||
err += gpio_set(GPIO_UBLOX_RST_N, power); |
||||
err += gpio_set(GPIO_UBLOX_SAFEBOOT_N, power); |
||||
err += gpio_set(GPIO_UBLOX_PWR_EN, power); |
||||
assert(err == 0); |
||||
#endif |
||||
} |
||||
|
||||
TTYPigeon::~TTYPigeon(){ |
||||
close(pigeon_tty_fd); |
||||
} |
||||
@ -0,0 +1,43 @@ |
||||
#pragma once |
||||
#include <string> |
||||
#include <termios.h> |
||||
|
||||
|
||||
#include "panda.h" |
||||
|
||||
class Pigeon { |
||||
public: |
||||
static Pigeon* connect(Panda * p); |
||||
static Pigeon* connect(const char * tty); |
||||
virtual ~Pigeon(){}; |
||||
|
||||
void init(); |
||||
virtual void set_baud(int baud) = 0; |
||||
virtual void send(std::string s) = 0; |
||||
virtual std::string receive() = 0; |
||||
virtual void set_power(bool power) = 0; |
||||
}; |
||||
|
||||
class PandaPigeon : public Pigeon { |
||||
Panda * panda = NULL; |
||||
public: |
||||
~PandaPigeon(); |
||||
void connect(Panda * p); |
||||
void set_baud(int baud); |
||||
void send(std::string s); |
||||
std::string receive(); |
||||
void set_power(bool power); |
||||
}; |
||||
|
||||
|
||||
class TTYPigeon : public Pigeon { |
||||
int pigeon_tty_fd = -1; |
||||
struct termios pigeon_tty; |
||||
public: |
||||
~TTYPigeon(); |
||||
void connect(const char* tty); |
||||
void set_baud(int baud); |
||||
void send(std::string s); |
||||
std::string receive(); |
||||
void set_power(bool power); |
||||
}; |
||||
@ -0,0 +1,75 @@ |
||||
#include "gpio.h" |
||||
#include <fcntl.h> |
||||
#include <unistd.h> |
||||
#include <stdio.h> |
||||
|
||||
// We assume that all pins have already been exported on boot,
|
||||
// and that we have permission to write to them.
|
||||
|
||||
int gpio_init(int pin_nr, bool output){ |
||||
int ret = 0; |
||||
int fd, tmp; |
||||
|
||||
char pin_dir_path[50]; |
||||
int pin_dir_path_len = snprintf(pin_dir_path, sizeof(pin_dir_path), |
||||
"/sys/class/gpio/gpio%d/direction", pin_nr); |
||||
if(pin_dir_path_len <= 0){ |
||||
ret = -1; |
||||
goto cleanup; |
||||
} |
||||
|
||||
fd = open(pin_dir_path, O_WRONLY); |
||||
if(fd == -1){ |
||||
ret = -1; |
||||
goto cleanup; |
||||
} |
||||
if(output){ |
||||
tmp = write(fd, "out", 3); |
||||
if(tmp != 3){ |
||||
ret = -1; |
||||
goto cleanup; |
||||
} |
||||
} else { |
||||
tmp = write(fd, "in", 2); |
||||
if(tmp != 2){ |
||||
ret = -1; |
||||
goto cleanup; |
||||
} |
||||
} |
||||
|
||||
cleanup: |
||||
if(fd >= 0){ |
||||
close(fd); |
||||
} |
||||
return ret; |
||||
} |
||||
|
||||
int gpio_set(int pin_nr, bool high){ |
||||
int ret = 0; |
||||
int fd, tmp; |
||||
|
||||
char pin_val_path[50]; |
||||
int pin_val_path_len = snprintf(pin_val_path, sizeof(pin_val_path), |
||||
"/sys/class/gpio/gpio%d/value", pin_nr); |
||||
if(pin_val_path_len <= 0){ |
||||
ret = -1; |
||||
goto cleanup; |
||||
} |
||||
|
||||
fd = open(pin_val_path, O_WRONLY); |
||||
if(fd == -1){ |
||||
ret = -1; |
||||
goto cleanup; |
||||
} |
||||
tmp = write(fd, high ? "1" : "0", 1); |
||||
if(tmp != 1){ |
||||
ret = -1; |
||||
goto cleanup; |
||||
} |
||||
|
||||
cleanup: |
||||
if(fd >= 0){ |
||||
close(fd); |
||||
} |
||||
return ret; |
||||
} |
||||
@ -0,0 +1,35 @@ |
||||
#ifndef GPIO_H |
||||
#define GPIO_H |
||||
|
||||
#include <stdbool.h> |
||||
|
||||
// Pin definitions
|
||||
#ifdef QCOM2 |
||||
#define GPIO_HUB_RST_N 30 |
||||
#define GPIO_UBLOX_RST_N 32 |
||||
#define GPIO_UBLOX_SAFEBOOT_N 33 |
||||
#define GPIO_UBLOX_PWR_EN 34 |
||||
#define GPIO_STM_RST_N 124 |
||||
#define GPIO_STM_BOOT0 134 |
||||
#else |
||||
#define GPIO_HUB_RST_N 0 |
||||
#define GPIO_UBLOX_RST_N 0 |
||||
#define GPIO_UBLOX_SAFEBOOT_N 0 |
||||
#define GPIO_UBLOX_PWR_EN 0 |
||||
#define GPIO_STM_RST_N 0 |
||||
#define GPIO_STM_BOOT0 0 |
||||
#endif |
||||
|
||||
|
||||
#ifdef __cplusplus |
||||
extern "C" { |
||||
#endif |
||||
|
||||
int gpio_init(int pin_nr, bool output); |
||||
int gpio_set(int pin_nr, bool high); |
||||
|
||||
#ifdef __cplusplus |
||||
} // extern "C"
|
||||
#endif |
||||
|
||||
#endif |
||||
Loading…
Reference in new issue