mawei/onepilot
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Pigeond (#25561)

Browse Source 
* split out pigeond from boardd

* also want to turn off power

* fix manager calls

* move to sensord folder

* release files:

* add assistnow code

* no bare except

* add test script to test TTFF

Co-authored-by: Comma Device <device@comma.ai>
old-commit-hash: 13489d092e2c09119e880022507d7331cf54c615
This commit is contained in:
Robbe Derks
2022-08-26 23:11:46 -07:00
committed by GitHub
parent d9c279aea9
commit 78881cf731
10 changed files with 314 additions and 454 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
common/params.cc
View File

@@ -84,6 +84,7 @@ private:
std::unordered_map<std::string, uint32_t> keys = {
{"AccessToken", CLEAR_ON_MANAGER_START | DONT_LOG},
{"AssistNowToken", PERSISTENT},
{"AthenadPid", PERSISTENT},
{"AthenadUploadQueue", PERSISTENT},
{"CalibrationParams", PERSISTENT},

3
release/files_common
View File

@@ -90,8 +90,6 @@ selfdrive/boardd/boardd_api_impl.pyx
selfdrive/boardd/can_list_to_can_capnp.cc
selfdrive/boardd/panda.cc
selfdrive/boardd/panda.h
selfdrive/boardd/pigeon.cc
selfdrive/boardd/pigeon.h
selfdrive/boardd/set_time.py
selfdrive/boardd/pandad.py
@@ -282,6 +280,7 @@ selfdrive/sensord/sensors_qcom2.cc
selfdrive/sensord/sensors/*.cc
selfdrive/sensord/sensors/*.h
selfdrive/sensord/sensord
selfdrive/sensord/pigeond.py
selfdrive/thermald/thermald.py
selfdrive/thermald/power_monitoring.py

2
selfdrive/boardd/SConscript
View File

@@ -1,7 +1,7 @@
Import('env', 'envCython', 'common', 'cereal', 'messaging')
libs = ['usb-1.0', common, cereal, messaging, 'pthread', 'zmq', 'capnp', 'kj']
env.Program('boardd', ['main.cc', 'boardd.cc', 'panda.cc', 'pigeon.cc'], LIBS=libs)
env.Program('boardd', ['main.cc', 'boardd.cc', 'panda.cc'], LIBS=libs)
env.Library('libcan_list_to_can_capnp', ['can_list_to_can_capnp.cc'])
envCython.Program('boardd_api_impl.so', 'boardd_api_impl.pyx', LIBS=["can_list_to_can_capnp", 'capnp', 'kj'] + envCython["LIBS"])

53
selfdrive/boardd/boardd.cc
View File

@@ -29,8 +29,6 @@
#include "common/util.h"
#include "system/hardware/hw.h"
#include "selfdrive/boardd/pigeon.h"
// -- Multi-panda conventions --
// Ordering:
// - The internal panda will always be the first panda
@@ -561,56 +559,6 @@ void peripheral_control_thread(Panda *panda) {
}
}
static void pigeon_publish_raw(PubMaster &pm, const std::string &dat) {
// create message
MessageBuilder msg;
msg.initEvent().setUbloxRaw(capnp::Data::Reader((uint8_t*)dat.data(), dat.length()));
pm.send("ubloxRaw", msg);
}
void pigeon_thread(Panda *panda) {
util::set_thread_name("boardd_pigeon");
PubMaster pm({"ubloxRaw"});
bool ignition_last = false;
std::unique_ptr<Pigeon> pigeon(Hardware::TICI() ? Pigeon::connect("/dev/ttyHS0") : Pigeon::connect(panda));
while (!do_exit && panda->connected) {
bool need_reset = false;
bool ignition_local = ignition;
std::string recv = pigeon->receive();
// Check based on null bytes
if (ignition_local && recv.length() > 0 && recv[0] == (char)0x00) {
need_reset = true;
LOGW("received invalid ublox message while onroad, resetting panda GPS");
}
if (recv.length() > 0) {
pigeon_publish_raw(pm, recv);
}
// init pigeon on rising ignition edge
// since it was turned off in low power mode
if((ignition_local && !ignition_last) || need_reset) {
pigeon_active = true;
pigeon->init();
} else if (!ignition_local && ignition_last) {
// power off on falling edge of ignition
LOGD("powering off pigeon\n");
pigeon->stop();
pigeon->set_power(false);
pigeon_active = false;
}
ignition_last = ignition_local;
// 10ms - 100 Hz
util::sleep_for(10);
}
}
void boardd_main_thread(std::vector<std::string> serials) {
PubMaster pm({"pandaStates", "peripheralState"});
LOGW("attempting to connect");
@@ -649,7 +597,6 @@ void boardd_main_thread(std::vector<std::string> serials) {
threads.emplace_back(panda_state_thread, &pm, pandas, getenv("STARTED") != nullptr);
threads.emplace_back(peripheral_control_thread, peripheral_panda);
threads.emplace_back(pigeon_thread, peripheral_panda);
threads.emplace_back(can_send_thread, pandas, getenv("FAKESEND") != nullptr);
threads.emplace_back(can_recv_thread, pandas);

333
selfdrive/boardd/pigeon.cc
View File

@@ -1,333 +0,0 @@
#include "selfdrive/boardd/pigeon.h"
#include <fcntl.h>
#include <termios.h>
#include <unistd.h>
#include <cassert>
#include <cerrno>
#include <optional>
#include "common/gpio.h"
#include "common/swaglog.h"
#include "common/util.h"
#include "selfdrive/locationd/ublox_msg.h"
// Termios on macos doesn't define all baud rate constants
#ifndef B460800
#define B460800 0010004
#endif
using namespace std::string_literals;
extern ExitHandler do_exit;
const std::string ack = "\xb5\x62\x05\x01\x02\x00";
const std::string nack = "\xb5\x62\x05\x00\x02\x00";
const std::string sos_save_ack = "\xb5\x62\x09\x14\x08\x00\x02\x00\x00\x00\x01\x00\x00\x00";
const std::string sos_save_nack = "\xb5\x62\x09\x14\x08\x00\x02\x00\x00\x00\x00\x00\x00\x00";
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;
}
bool Pigeon::wait_for_ack(const std::string &ack_, const std::string &nack_, int timeout_ms) {
std::string s;
const double start_t = millis_since_boot();
while (!do_exit) {
s += receive();
if (s.find(ack_) != std::string::npos) {
LOGD("Received ACK from ublox");
return true;
} else if (s.find(nack_) != std::string::npos) {
LOGE("Received NACK from ublox");
return false;
} else if (s.size() > 0x1000 || ((millis_since_boot() - start_t) > timeout_ms)) {
LOGE("No response from ublox");
return false;
}
util::sleep_for(1); // Allow other threads to be scheduled
}
return false;
}
bool Pigeon::wait_for_ack() {
return wait_for_ack(ack, nack);
}
bool Pigeon::send_with_ack(const std::string &cmd) {
send(cmd);
return wait_for_ack();
}
sos_restore_response Pigeon::wait_for_backup_restore_status(int timeout_ms) {
std::string s;
const double start_t = millis_since_boot();
while (!do_exit) {
s += receive();
size_t position = s.find("\xb5\x62\x09\x14\x08\x00\x03");
if (position != std::string::npos && s.size() >= (position + 11)) {
return static_cast<sos_restore_response>(s[position + 10]);
} else if (s.size() > 0x1000 || ((millis_since_boot() - start_t) > timeout_ms)) {
LOGE("No backup restore response from ublox");
return error;
}
util::sleep_for(1); // Allow other threads to be scheduled
}
return error;
}
void Pigeon::init() {
for (int i = 0; i < 10; i++) {
if (do_exit) return;
LOGW("panda GPS start");
// power off pigeon
set_power(false);
util::sleep_for(100);
// 9600 baud at init
set_baud(9600);
// power on pigeon
set_power(true);
util::sleep_for(500);
// 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);
util::sleep_for(100);
// set baud rate to 460800
set_baud(460800);
// init from ubloxd
// To generate this data, run selfdrive/locationd/test/ubloxd.py
if (!send_with_ack("\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)) continue;
if (!send_with_ack("\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)) continue;
if (!send_with_ack("\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)) continue;
if (!send_with_ack("\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)) continue;
if (!send_with_ack("\xB5\x62\x06\x00\x00\x00\x06\x18"s)) continue;
if (!send_with_ack("\xB5\x62\x06\x00\x01\x00\x01\x08\x22"s)) continue;
if (!send_with_ack("\xB5\x62\x06\x00\x01\x00\x03\x0A\x24"s)) continue;
if (!send_with_ack("\xB5\x62\x06\x08\x06\x00\x64\x00\x01\x00\x00\x00\x79\x10"s)) continue;
if (!send_with_ack("\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)) continue;
if (!send_with_ack("\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)) continue;
if (!send_with_ack("\xB5\x62\x06\x39\x08\x00\xFF\xAD\x62\xAD\x1E\x63\x00\x00\x83\x0C"s)) continue;
if (!send_with_ack("\xB5\x62\x06\x24\x00\x00\x2A\x84"s)) continue;
if (!send_with_ack("\xB5\x62\x06\x23\x00\x00\x29\x81"s)) continue;
if (!send_with_ack("\xB5\x62\x06\x1E\x00\x00\x24\x72"s)) continue;
if (!send_with_ack("\xB5\x62\x06\x39\x00\x00\x3F\xC3"s)) continue;
if (!send_with_ack("\xB5\x62\x06\x01\x03\x00\x01\x07\x01\x13\x51"s)) continue;
if (!send_with_ack("\xB5\x62\x06\x01\x03\x00\x02\x15\x01\x22\x70"s)) continue;
if (!send_with_ack("\xB5\x62\x06\x01\x03\x00\x02\x13\x01\x20\x6C"s)) continue;
if (!send_with_ack("\xB5\x62\x06\x01\x03\x00\x0A\x09\x01\x1E\x70"s)) continue;
if (!send_with_ack("\xB5\x62\x06\x01\x03\x00\x0A\x0B\x01\x20\x74"s)) continue;
// check the backup restore status
send("\xB5\x62\x09\x14\x00\x00\x1D\x60"s);
sos_restore_response restore_status = wait_for_backup_restore_status();
switch(restore_status) {
case restored:
LOGW("almanac backup restored");
// clear the backup
send_with_ack("\xB5\x62\x06\x01\x03\x00\x0A\x0B\x01\x20\x74"s);
break;
case no_backup:
LOGW("no almanac backup found");
break;
default:
LOGE("failed to restore almanac backup, status: %d", restore_status);
}
auto time = util::get_time();
if (util::time_valid(time)) {
LOGW("Sending current time to ublox");
send(ublox::build_ubx_mga_ini_time_utc(time));
}
LOGW("panda GPS on");
return;
}
LOGE("failed to initialize panda GPS");
}
void Pigeon::stop() {
LOGW("Storing almanac in ublox flash");
// Controlled GNSS stop
send("\xB5\x62\x06\x04\x04\x00\x00\x00\x08\x00\x16\x74"s);
// Store almanac in flash
send("\xB5\x62\x09\x14\x04\x00\x00\x00\x00\x00\x21\xEC"s);
if (wait_for_ack(sos_save_ack, sos_save_nack)) {
LOGW("Done storing almanac");
} else {
LOGE("Error storing almanac");
}
}
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(const 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;
r.reserve(0x1000 + 0x40);
unsigned char dat[0x40];
while (r.length() < 0x1000) {
int len = panda->usb_read(0xe0, 1, 0, dat, sizeof(dat));
if (len <= 0) 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 = HANDLE_EINTR(open(tty, O_RDWR));
if (pigeon_tty_fd < 0) {
handle_tty_issue(errno, __func__);
assert(pigeon_tty_fd >= 0);
}
int err = tcgetattr(pigeon_tty_fd, &pigeon_tty);
assert(err == 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)
err = tcsetattr(pigeon_tty_fd, TCSANOW, &pigeon_tty);
assert(err == 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
int err = tcdrain(pigeon_tty_fd);
assert(err == 0);
// change baud
err = tcgetattr(pigeon_tty_fd, &pigeon_tty);
assert(err == 0);
err = cfsetspeed(&pigeon_tty, baud_const);
assert(err == 0);
err = tcsetattr(pigeon_tty_fd, TCSANOW, &pigeon_tty);
assert(err == 0);
// flush
err = tcflush(pigeon_tty_fd, TCIOFLUSH);
assert(err == 0);
}
void TTYPigeon::send(const std::string &s) {
int err = HANDLE_EINTR(write(pigeon_tty_fd, s.data(), s.length()));
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;
r.reserve(0x1000 + 0x40);
char dat[0x40];
while (r.length() < 0x1000) {
int len = read(pigeon_tty_fd, dat, sizeof(dat));
if(len < 0) {
handle_tty_issue(len, __func__);
} else if (len == 0) {
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);
}

58
selfdrive/boardd/pigeon.h
View File

@@ -1,58 +0,0 @@
#pragma once
#include <termios.h>
#include <atomic>
#include <string>
#include "selfdrive/boardd/panda.h"
enum sos_restore_response : int {
unknown = 0,
failed = 1,
restored = 2,
no_backup = 3,
error = -1
};
class Pigeon {
public:
static Pigeon* connect(Panda * p);
static Pigeon* connect(const char * tty);
virtual ~Pigeon(){};
void init();
void stop();
bool wait_for_ack();
bool wait_for_ack(const std::string &ack, const std::string &nack, int timeout_ms = 1000);
bool send_with_ack(const std::string &cmd);
sos_restore_response wait_for_backup_restore_status(int timeout_ms = 1000);
virtual void set_baud(int baud) = 0;
virtual void send(const 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(const 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(const std::string &s);
std::string receive();
void set_power(bool power);
};

14
selfdrive/locationd/test/ubloxd.py
View File

@@ -41,13 +41,13 @@ def configure_ublox(dev):
ITMF_config2 = 25374
payload = struct.pack('<II', ITMF_config1, ITMF_config2)
dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_ITMF, payload)
#payload = struct.pack('<HHIBBBBBBBBBBH6BBB2BH4B3BB', 0, 8192, 0, 0, 0,
# 0, 0, 0, 0, 0, 0,
# 0, 0, 0, 0, 0, 0,
# 0, 0, 0, 0, 0, 0,
# 0, 0, 0, 0, 0, 0,
# 0, 0, 0, 0)
#dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_NAVX5, payload)
payload = struct.pack('<HHIBBBBBBBBBBH6BBB2BH4B3BB', 0, (1 << 10), 0, 0, 0,
0, 0, 0, 0, 0, 0,
0, 1, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0,
0, 0, 0, 0)
dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_NAVX5, payload)
dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_NAV5)
dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_NAVX5)

1
selfdrive/manager/process_config.py
View File

@@ -45,6 +45,7 @@ procs = [
PythonProcess("navd", "selfdrive.navd.navd"),
PythonProcess("pandad", "selfdrive.boardd.pandad", offroad=True),
PythonProcess("paramsd", "selfdrive.locationd.paramsd"),
PythonProcess("pigeond", "selfdrive.sensord.pigeond", enabled=TICI),
PythonProcess("plannerd", "selfdrive.controls.plannerd"),
PythonProcess("radard", "selfdrive.controls.radard"),
PythonProcess("thermald", "selfdrive.thermald.thermald", offroad=True),

255
selfdrive/sensord/pigeond.py Executable file
View File

@@ -0,0 +1,255 @@
#!/usr/bin/env python3
import sys
import time
import signal
import serial
import struct
import requests
import urllib.parse
from datetime import datetime
from cereal import messaging
from common.params import Params
from system.swaglog import cloudlog
from selfdrive.hardware import TICI
from common.gpio import gpio_init, gpio_set
from selfdrive.hardware.tici.pins import GPIO
UBLOX_TTY = "/dev/ttyHS0"
UBLOX_ACK = b"\xb5\x62\x05\x01\x02\x00"
UBLOX_NACK = b"\xb5\x62\x05\x00\x02\x00"
UBLOX_SOS_ACK = b"\xb5\x62\x09\x14\x08\x00\x02\x00\x00\x00\x01\x00\x00\x00"
UBLOX_SOS_NACK = b"\xb5\x62\x09\x14\x08\x00\x02\x00\x00\x00\x00\x00\x00\x00"
UBLOX_BACKUP_RESTORE_MSG = b"\xb5\x62\x09\x14\x08\x00\x03"
UBLOX_ASSIST_ACK = b"\xb5\x62\x13\x60\x08\x00"
def set_power(enabled):
gpio_init(GPIO.UBLOX_SAFEBOOT_N, True)
gpio_init(GPIO.UBLOX_PWR_EN, True)
gpio_init(GPIO.UBLOX_RST_N, True)
gpio_set(GPIO.UBLOX_SAFEBOOT_N, True)
gpio_set(GPIO.UBLOX_PWR_EN, enabled)
gpio_set(GPIO.UBLOX_RST_N, enabled)
def add_ubx_checksum(msg):
A = B = 0
for b in msg[2:]:
A = (A + b) % 256
B = (B + A) % 256
return msg + bytes([A, B])
def get_assistnow_messages(token):
# make request
# TODO: implement adding the last known location
r = requests.get("https://online-live2.services.u-blox.com/GetOnlineData.ashx", params=urllib.parse.urlencode({
'token': token,
'gnss': 'gps,glo',
'datatype': 'eph,alm,aux',
}, safe=':,'), timeout=5)
assert r.status_code == 200, "Got invalid status code"
dat = r.content
# split up messages
msgs = []
while len(dat) > 0:
assert dat[:2] == b"\xB5\x62"
msg_len = 6 + (dat[5] << 8 | dat[4]) + 2
msgs.append(dat[:msg_len])
dat = dat[msg_len:]
return msgs
class TTYPigeon():
def __init__(self, path):
self.path = path
self.tty = serial.VTIMESerial(UBLOX_TTY, baudrate=9600, timeout=0)
def send(self, dat):
self.tty.write(dat)
def receive(self):
dat = b''
while len(dat) < 0x1000:
d = self.tty.read(0x40)
dat += d
if len(d) == 0:
break
return dat
def set_baud(self, baud):
self.tty.baudrate = baud
def wait_for_ack(self, ack=UBLOX_ACK, nack=UBLOX_NACK, timeout=0.5):
dat = b''
st = time.monotonic()
while True:
dat += self.receive()
if ack in dat:
cloudlog.debug("Received ACK from ublox")
return True
elif nack in dat:
cloudlog.error("Received NACK from ublox")
return False
elif time.monotonic() - st > timeout:
cloudlog.error("No response from ublox")
raise TimeoutError('No response from ublox')
time.sleep(0.001)
def send_with_ack(self, dat, ack=UBLOX_ACK, nack=UBLOX_NACK):
self.send(dat)
self.wait_for_ack(ack, nack)
def wait_for_backup_restore_status(self, timeout=1):
dat = b''
st = time.monotonic()
while True:
dat += self.receive()
position = dat.find(UBLOX_BACKUP_RESTORE_MSG)
if position >= 0 and len(dat) >= position + 11:
return dat[position + 10]
elif time.monotonic() - st > timeout:
cloudlog.error("No backup restore response from ublox")
raise TimeoutError('No response from ublox')
time.sleep(0.001)
def initialize_pigeon(pigeon):
# try initializing a few times
for _ in range(10):
try:
pigeon.set_baud(9600)
# up baud rate
pigeon.send(b"\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")
time.sleep(0.1)
pigeon.set_baud(460800)
# other configuration messages
pigeon.send_with_ack(b"\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")
pigeon.send_with_ack(b"\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")
pigeon.send_with_ack(b"\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")
pigeon.send_with_ack(b"\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")
pigeon.send_with_ack(b"\xB5\x62\x06\x00\x00\x00\x06\x18")
pigeon.send_with_ack(b"\xB5\x62\x06\x00\x01\x00\x01\x08\x22")
pigeon.send_with_ack(b"\xB5\x62\x06\x00\x01\x00\x03\x0A\x24")
pigeon.send_with_ack(b"\xB5\x62\x06\x08\x06\x00\x64\x00\x01\x00\x00\x00\x79\x10")
pigeon.send_with_ack(b"\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")
pigeon.send_with_ack(b"\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")
pigeon.send_with_ack(b"\xB5\x62\x06\x39\x08\x00\xFF\xAD\x62\xAD\x1E\x63\x00\x00\x83\x0C")
pigeon.send_with_ack(b"\xB5\x62\x06\x23\x28\x00\x00\x00\x00\x04\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x01\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x56\x24")
pigeon.send_with_ack(b"\xB5\x62\x06\x24\x00\x00\x2A\x84")
pigeon.send_with_ack(b"\xB5\x62\x06\x23\x00\x00\x29\x81")
pigeon.send_with_ack(b"\xB5\x62\x06\x1E\x00\x00\x24\x72")
pigeon.send_with_ack(b"\xB5\x62\x06\x39\x00\x00\x3F\xC3")
pigeon.send_with_ack(b"\xB5\x62\x06\x01\x03\x00\x01\x07\x01\x13\x51")
pigeon.send_with_ack(b"\xB5\x62\x06\x01\x03\x00\x02\x15\x01\x22\x70")
pigeon.send_with_ack(b"\xB5\x62\x06\x01\x03\x00\x02\x13\x01\x20\x6C")
pigeon.send_with_ack(b"\xB5\x62\x06\x01\x03\x00\x0A\x09\x01\x1E\x70")
pigeon.send_with_ack(b"\xB5\x62\x06\x01\x03\x00\x0A\x0B\x01\x20\x74")
cloudlog.debug("pigeon configured")
# try restoring almanac backup
pigeon.send(b"\xB5\x62\x09\x14\x00\x00\x1D\x60")
restore_status = pigeon.wait_for_backup_restore_status()
if restore_status == 2:
cloudlog.warning("almanac backup restored")
elif restore_status == 3:
cloudlog.warning("no almanac backup found")
else:
cloudlog.error(f"failed to restore almanac backup, status: {restore_status}")
# sending time to ublox
t_now = datetime.utcnow()
if t_now >= datetime(2021, 6, 1):
cloudlog.warning("Sending current time to ublox")
# UBX-MGA-INI-TIME_UTC
msg = add_ubx_checksum(b"\xB5\x62\x13\x40\x18\x00" + struct.pack("<BBBBHBBBBBxIHxxI",
0x10,
0x00,
0x00,
0x80,
t_now.year,
t_now.month,
t_now.day,
t_now.hour,
t_now.minute,
t_now.second,
0,
30,
0
))
pigeon.send_with_ack(msg, ack=UBLOX_ASSIST_ACK)
# try getting AssistNow if we have a token
token = Params().get('AssistNowToken')
if token is not None:
try:
for msg in get_assistnow_messages(token):
pigeon.send_with_ack(msg, ack=UBLOX_ASSIST_ACK)
cloudlog.warning("AssistNow messages sent")
except Exception:
cloudlog.warning("failed to get AssistNow messages")
cloudlog.warning("Pigeon GPS on!")
break
except TimeoutError:
cloudlog.warning("Initialization failed, trying again!")
def deinitialize_and_exit(pigeon):
cloudlog.warning("Storing almanac in ublox flash")
# controlled GNSS stop
pigeon.send(b"\xB5\x62\x06\x04\x04\x00\x00\x00\x08\x00\x16\x74")
# store almanac in flash
pigeon.send(b"\xB5\x62\x09\x14\x04\x00\x00\x00\x00\x00\x21\xEC")
try:
if pigeon.wait_for_ack(ack=UBLOX_SOS_ACK, nack=UBLOX_SOS_NACK):
cloudlog.warning("Done storing almanac")
else:
cloudlog.error("Error storing almanac")
except TimeoutError:
pass
# turn off power and exit cleanly
set_power(False)
sys.exit(0)
def main():
assert TICI, "unsupported hardware for pigeond"
pm = messaging.PubMaster(['ubloxRaw'])
# power cycle ublox
set_power(False)
time.sleep(0.1)
set_power(True)
time.sleep(0.5)
pigeon = TTYPigeon(UBLOX_TTY)
initialize_pigeon(pigeon)
# register exit handler
signal.signal(signal.SIGINT, lambda sig, frame: deinitialize_and_exit(pigeon))
# start receiving data
while True:
dat = pigeon.receive()
if len(dat) > 0:
if dat[0] == 0x00:
cloudlog.warning("received invalid data from ublox, re-initing!")
initialize_pigeon(pigeon)
continue
# send out to socket
msg = messaging.new_message('ubloxRaw', len(dat))
msg.ubloxRaw = dat[:]
pm.send('ubloxRaw', msg)
if __name__ == "__main__":
main()

48
selfdrive/sensord/test/ttff_test.py Executable file
View File

@@ -0,0 +1,48 @@
#!/usr/bin/env python3
import time
import atexit
from cereal import messaging
from selfdrive.manager.process_config import managed_processes
TIMEOUT = 10*60
def kill():
for proc in ['ubloxd', 'pigeond']:
managed_processes[proc].stop(retry=True, block=True)
if __name__ == "__main__":
# start ubloxd
managed_processes['ubloxd'].start()
atexit.register(kill)
sm = messaging.SubMaster(['ubloxGnss'])
times = []
for i in range(20):
# start pigeond
st = time.monotonic()
managed_processes['pigeond'].start()
# wait for a >4 satellite fix
while True:
sm.update(0)
msg = sm['ubloxGnss']
if msg.which() == 'measurementReport' and sm.updated["ubloxGnss"]:
report = msg.measurementReport
if report.numMeas > 4:
times.append(time.monotonic() - st)
print(f"\033[94m{i}: Got a fix in {round(times[-1], 2)} seconds\033[0m")
break
if time.monotonic() - st > TIMEOUT:
raise TimeoutError("\033[91mFailed to get a fix in {TIMEOUT} seconds!\033[0m")
time.sleep(0.1)
# stop pigeond
managed_processes['pigeond'].stop(retry=True, block=True)
time.sleep(20)
print(f"\033[92mAverage TTFF: {round(sum(times) / len(times), 2)}s\033[0m")