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

118
selfdrive/boardd/pigeon.cc
Unescape View File

@ -17,6 +17,11 @@
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){
PandaPigeon * pigeon = new PandaPigeon();
@ -32,50 +37,77 @@ Pigeon * Pigeon::connect(const char * tty){
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() {
util::sleep_for(1000);
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);
util::sleep_for(100);
// init from ubloxd
// To generate this data, run test/ubloxd.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\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);
send("\xB5\x62\x06\x01\x03\x00\x0A\x0B\x01\x20\x74"s);
while (!do_exit){
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\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\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;
break;
}
LOGW("panda GPS on");
}

2
selfdrive/boardd/pigeon.h
Unescape View File

@ -12,6 +12,8 @@ class Pigeon {
virtual ~Pigeon(){};
void init();
bool wait_for_ack();
bool send_with_ack(std::string cmd);
virtual void set_baud(int baud) = 0;
virtual void send(const std::string &s) = 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
import struct
panda = True
baudrate = 460800
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=0, inMask=0, outMask=0) # disable DDC
if panda:
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
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
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_port(port=4, inMask=0, outMask=0) # disable SPI
dev.configure_poll_port()
dev.configure_poll_port(ublox.PORT_SERIAL1)
dev.configure_poll_port(ublox.PORT_SERIAL2)
dev.configure_poll_port(ublox.PORT_USB)
dev.configure_solution_rate(rate_ms=rate)
@ -65,7 +59,7 @@ if __name__ == "__main__":
class Device:
def write(self, 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)
configure_ublox(dev)

Write Preview
Loading…
Cancel
Save