sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Improve robustness of ublox init procedure (#20685)

Browse Source 
* improve ublox init robustness

* cleanup

* only log if running at 1hz

* not needed
pull/20691/head
Willem Melching 5 years ago committed by GitHub
parent b3d1ef77ee
commit a163daf5e0
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
4 changed files with 82 additions and 55 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 5
      selfdrive/boardd/boardd.cc
  2. 118
      selfdrive/boardd/pigeon.cc
  3. 2
      selfdrive/boardd/pigeon.h
  4. 12
      selfdrive/locationd/test/ubloxd.py

5
selfdrive/boardd/boardd.cc
Unescape View File

@ -31,7 +31,6 @@
#include "panda.h" #include "panda.h"
#include "pigeon.h" #include "pigeon.h"
#define MAX_IR_POWER 0.5f #define MAX_IR_POWER 0.5f
#define MIN_IR_POWER 0.0f #define MIN_IR_POWER 0.0f
#define CUTOFF_IL 200 #define CUTOFF_IL 200
@ -499,8 +498,8 @@ void pigeon_thread() {
std::unordered_map<char, uint64_t> last_recv_time; std::unordered_map<char, uint64_t> last_recv_time;
std::unordered_map<char, int64_t> cls_max_dt = { std::unordered_map<char, int64_t> cls_max_dt = {
{(char)ublox::CLASS_NAV, int64_t(250000000ULL)}, // 0.25s {(char)ublox::CLASS_NAV, int64_t(900000000ULL)}, // 0.9s
{(char)ublox::CLASS_RXM, int64_t(250000000ULL)}, // 0.25s {(char)ublox::CLASS_RXM, int64_t(900000000ULL)}, // 0.9s
}; };
while (!do_exit && panda->connected) { while (!do_exit && panda->connected) {

118
selfdrive/boardd/pigeon.cc
Unescape View File

@ -17,6 +17,11 @@
using namespace std::string_literals; 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";
Pigeon * Pigeon::connect(Panda * p){ Pigeon * Pigeon::connect(Panda * p){
PandaPigeon * pigeon = new PandaPigeon(); PandaPigeon * pigeon = new PandaPigeon();
@ -32,50 +37,77 @@ Pigeon * Pigeon::connect(const char * tty){
return pigeon; return pigeon;
} }
bool Pigeon::wait_for_ack(){
std::string s;
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) {
LOGE("No response from ublox");
return false;
}
util::sleep_for(1); // Allow other threads to be scheduled
}
return false;
}
bool Pigeon::send_with_ack(std::string cmd){
send(cmd);
return wait_for_ack();
}
void Pigeon::init() { void Pigeon::init() {
util::sleep_for(1000); while (!do_exit){
LOGW("panda GPS start"); LOGW("panda GPS start");
// power off pigeon // power off pigeon
set_power(false); set_power(false);
util::sleep_for(100); util::sleep_for(100);
// 9600 baud at init // 9600 baud at init
set_baud(9600); set_baud(9600);
// power on pigeon // power on pigeon
set_power(true); set_power(true);
util::sleep_for(500); util::sleep_for(500);
// baud rate upping // 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); 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); util::sleep_for(100);
// set baud rate to 460800 // set baud rate to 460800
set_baud(460800); set_baud(460800);
util::sleep_for(100);
// init from ubloxd
// init from ubloxd // To generate this data, run selfdrive/locationd/test/ubloxd.py
// To generate this data, run 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;
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); 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;
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); 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;
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); 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;
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); if (!send_with_ack("\xB5\x62\x06\x00\x00\x00\x06\x18"s)) continue;
send("\xB5\x62\x06\x00\x00\x00\x06\x18"s); if (!send_with_ack("\xB5\x62\x06\x00\x01\x00\x01\x08\x22"s)) continue;
send("\xB5\x62\x06\x00\x01\x00\x01\x08\x22"s); if (!send_with_ack("\xB5\x62\x06\x00\x01\x00\x03\x0A\x24"s)) continue;
send("\xB5\x62\x06\x00\x01\x00\x02\x09\x23"s); if (!send_with_ack("\xB5\x62\x06\x08\x06\x00\x64\x00\x01\x00\x00\x00\x79\x10"s)) continue;
send("\xB5\x62\x06\x00\x01\x00\x03\x0A\x24"s); 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;
send("\xB5\x62\x06\x08\x06\x00\x64\x00\x01\x00\x00\x00\x79\x10"s); 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;
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); if (!send_with_ack("\xB5\x62\x06\x24\x00\x00\x2A\x84"s)) continue;
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); if (!send_with_ack("\xB5\x62\x06\x23\x00\x00\x29\x81"s)) continue;
send("\xB5\x62\x06\x24\x00\x00\x2A\x84"s); if (!send_with_ack("\xB5\x62\x06\x1E\x00\x00\x24\x72"s)) continue;
send("\xB5\x62\x06\x23\x00\x00\x29\x81"s); if (!send_with_ack("\xB5\x62\x06\x01\x03\x00\x01\x07\x01\x13\x51"s)) continue;
send("\xB5\x62\x06\x1E\x00\x00\x24\x72"s); if (!send_with_ack("\xB5\x62\x06\x01\x03\x00\x02\x15\x01\x22\x70"s)) continue;
send("\xB5\x62\x06\x01\x03\x00\x01\x07\x01\x13\x51"s); if (!send_with_ack("\xB5\x62\x06\x01\x03\x00\x02\x13\x01\x20\x6C"s)) continue;
send("\xB5\x62\x06\x01\x03\x00\x02\x15\x01\x22\x70"s); if (!send_with_ack("\xB5\x62\x06\x01\x03\x00\x0A\x09\x01\x1E\x70"s)) continue;
send("\xB5\x62\x06\x01\x03\x00\x02\x13\x01\x20\x6C"s); if (!send_with_ack("\xB5\x62\x06\x01\x03\x00\x0A\x0B\x01\x20\x74"s)) continue;
send("\xB5\x62\x06\x01\x03\x00\x0A\x09\x01\x1E\x70"s);
send("\xB5\x62\x06\x01\x03\x00\x0A\x0B\x01\x20\x74"s); break;
}
LOGW("panda GPS on"); LOGW("panda GPS on");
} }

2
selfdrive/boardd/pigeon.h
Unescape View File

@ -12,6 +12,8 @@ class Pigeon {
virtual ~Pigeon(){}; virtual ~Pigeon(){};
void init(); void init();
bool wait_for_ack();
bool send_with_ack(std::string cmd);
virtual void set_baud(int baud) = 0; virtual void set_baud(int baud) = 0;
virtual void send(const std::string &s) = 0; virtual void send(const std::string &s) = 0;
virtual std::string receive() = 0; virtual std::string receive() = 0;

12
selfdrive/locationd/test/ubloxd.py
Unescape View File

@ -5,7 +5,6 @@
from selfdrive.locationd.test import ublox from selfdrive.locationd.test import ublox
import struct import struct
panda = True
baudrate = 460800 baudrate = 460800
rate = 100 # send new data every 100ms rate = 100 # send new data every 100ms
@ -15,17 +14,12 @@ def configure_ublox(dev):
dev.configure_port(port=ublox.PORT_USB, inMask=1, outMask=1) # enable only UBX on USB dev.configure_port(port=ublox.PORT_USB, inMask=1, outMask=1) # enable only UBX on USB
dev.configure_port(port=0, inMask=0, outMask=0) # disable DDC dev.configure_port(port=0, inMask=0, outMask=0) # disable DDC
if panda: payload = struct.pack('<BBHIIHHHBB', 1, 0, 0, 2240, baudrate, 1, 1, 0, 0, 0)
payload = struct.pack('<BBHIIHHHBB', 1, 0, 0, 2240, baudrate, 1, 1, 0, 0, 0) dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_PRT, payload) # enable UART
dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_PRT, payload) # enable UART
else:
payload = struct.pack('<BBHIIHHHBB', 1, 0, 0, 2240, baudrate, 0, 0, 0, 0, 0)
dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_PRT, payload) # disable UART
dev.configure_port(port=4, inMask=0, outMask=0) # disable SPI dev.configure_port(port=4, inMask=0, outMask=0) # disable SPI
dev.configure_poll_port() dev.configure_poll_port()
dev.configure_poll_port(ublox.PORT_SERIAL1) dev.configure_poll_port(ublox.PORT_SERIAL1)
dev.configure_poll_port(ublox.PORT_SERIAL2)
dev.configure_poll_port(ublox.PORT_USB) dev.configure_poll_port(ublox.PORT_USB)
dev.configure_solution_rate(rate_ms=rate) dev.configure_solution_rate(rate_ms=rate)
@ -65,7 +59,7 @@ if __name__ == "__main__":
class Device: class Device:
def write(self, s): def write(self, s):
d = '"{}"s'.format(''.join('\\x{:02X}'.format(b) for b in s)) d = '"{}"s'.format(''.join('\\x{:02X}'.format(b) for b in s))
print(f"send({d});") print(f" if (!send_with_ack({d})) continue;")
dev = ublox.UBlox(Device(), baudrate=baudrate) dev = ublox.UBlox(Device(), baudrate=baudrate)
configure_ublox(dev) configure_ublox(dev)

Write Preview
Loading…
Cancel
Save