Improve robustness of ublox init procedure (#20685)
* improve ublox init robustness * cleanup * only log if running at 1hz * not neededalbatross
Willem Melching
GitHub
parent
b3d1ef77ee
commit
a163daf5e0
|
|
@ -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) {
|
||||
|
|
|
|||
|
|
@ -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");
|
||||
while (!do_exit){
|
||||
LOGW("panda GPS start");
|
||||
|
||||
// power off pigeon
|
||||
set_power(false);
|
||||
util::sleep_for(100);
|
||||
// power off pigeon
|
||||
set_power(false);
|
||||
util::sleep_for(100);
|
||||
|
||||
// 9600 baud at init
|
||||
set_baud(9600);
|
||||
// 9600 baud at init
|
||||
set_baud(9600);
|
||||
|
||||
// power on pigeon
|
||||
set_power(true);
|
||||
util::sleep_for(500);
|
||||
// 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);
|
||||
// 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);
|
||||
// set baud rate to 460800
|
||||
set_baud(460800);
|
||||
|
||||
// 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);
|
||||
// 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");
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
Loading…
Reference in New Issue