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

Store almanac on ublox poweroff (#20967) 

* Store almanac on ublox poweroff

* send current UTC time

* move message building to ublox_msg.h
old-commit-hash: ea5141d909
This commit is contained in:
Willem Melching 2021-05-20 11:43:03 +02:00 committed by GitHub
parent d1498aa3d3
commit 4d874a90ea
6 changed files with 139 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
selfdrive/boardd/boardd.cc
View File

@ -12,7 +12,6 @@
#include <atomic>
#include <bitset>
#include <cassert>
#include <ctime>
#include <thread>
#include <unordered_map>
@ -41,21 +40,7 @@ std::atomic<bool> safety_setter_thread_running(false);
std::atomic<bool> ignition(false);
ExitHandler do_exit;
struct tm get_time(){
time_t rawtime;
time(&rawtime);
struct tm sys_time;
gmtime_r(&rawtime, &sys_time);
return sys_time;
}
bool time_valid(struct tm sys_time){
int year = 1900 + sys_time.tm_year;
int month = 1 + sys_time.tm_mon;
return (year > 2020) || (year == 2020 && month >= 10);
}
void safety_setter_thread() {
LOGD("Starting safety setter thread");
@ -163,10 +148,10 @@ bool usb_connect() {
if (tmp_panda->has_rtc){
setenv("TZ","UTC",1);
struct tm sys_time = get_time();
struct tm sys_time = util::get_time();
struct tm rtc_time = tmp_panda->get_rtc();
if (!time_valid(sys_time) && time_valid(rtc_time)) {
if (!util::time_valid(sys_time) && util::time_valid(rtc_time)) {
LOGE("System time wrong, setting from RTC. "
"System: %d-%02d-%02d %02d:%02d:%02d RTC: %d-%02d-%02d %02d:%02d:%02d",
sys_time.tm_year + 1900, sys_time.tm_mon + 1, sys_time.tm_mday,
@ -333,9 +318,9 @@ void panda_state_thread(bool spoofing_started) {
if ((panda->has_rtc) && !ignition && (no_ignition_cnt % 120 == 1)){
// Write time to RTC if it looks reasonable
setenv("TZ","UTC",1);
struct tm sys_time = get_time();
struct tm sys_time = util::get_time();
if (time_valid(sys_time)){
if (util::time_valid(sys_time)){
struct tm rtc_time = panda->get_rtc();
double seconds = difftime(mktime(&rtc_time), mktime(&sys_time));
@ -545,6 +530,7 @@ void pigeon_thread() {
} else if (!ignition && ignition_last) {
// power off on falling edge of ignition
LOGD("powering off pigeon\n");
pigeon->stop();
pigeon->set_power(false);
}

32
selfdrive/boardd/pigeon.cc
View File

@ -6,10 +6,12 @@
#include <unistd.h>
#include <cassert>
#include <optional>
#include "selfdrive/common/gpio.h"
#include "selfdrive/common/swaglog.h"
#include "selfdrive/common/util.h"
#include "selfdrive/locationd/ublox_msg.h"
// Termios on macos doesn't define all baud rate constants
#ifndef B460800
@ -22,7 +24,8 @@ 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_ack = "\xb5\x62\x09\x14\x08\x00\x02\x00\x00\x00\x01\x00\x00\x00";
const std::string sos_nack = "\xb5\x62\x09\x14\x08\x00\x02\x00\x00\x00\x00\x00\x00\x00";
Pigeon * Pigeon::connect(Panda * p){
PandaPigeon * pigeon = new PandaPigeon();
@ -38,7 +41,7 @@ Pigeon * Pigeon::connect(const char * tty){
return pigeon;
}
bool Pigeon::wait_for_ack(){
bool Pigeon::wait_for_ack(std::string ack, std::string nack){
std::string s;
while (!do_exit){
s += receive();
@ -59,6 +62,10 @@ bool Pigeon::wait_for_ack(){
return false;
}
bool Pigeon::wait_for_ack(){
return wait_for_ack(ack, nack);
}
bool Pigeon::send_with_ack(std::string cmd){
send(cmd);
return wait_for_ack();
@ -110,6 +117,11 @@ void Pigeon::init() {
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;
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;
@ -117,6 +129,22 @@ void Pigeon::init() {
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_ack, sos_nack)) {
LOGW("Done storing almanac");
} else {
LOGE("Error storing almanac");
}
}
void PandaPigeon::connect(Panda * p) {
panda = p;
}

2
selfdrive/boardd/pigeon.h
View File

@ -14,7 +14,9 @@ class Pigeon {
virtual ~Pigeon(){};
void init();
void stop();
bool wait_for_ack();
bool wait_for_ack(std::string ack, std::string nack);
bool send_with_ack(std::string cmd);
virtual void set_baud(int baud) = 0;
virtual void send(const std::string &s) = 0;

29
selfdrive/common/util.h
View File

@ -16,6 +16,9 @@
#include <string>
#include <thread>
#include <map>
#include <ctime>
#include <sstream>
#include <iomanip>
#ifndef sighandler_t
typedef void (*sighandler_t)(int sig);
@ -28,6 +31,23 @@ int set_core_affinity(int core);
namespace util {
// Time helpers
inline struct tm get_time(){
time_t rawtime;
time(&rawtime);
struct tm sys_time;
gmtime_r(&rawtime, &sys_time);
return sys_time;
}
inline bool time_valid(struct tm sys_time){
int year = 1900 + sys_time.tm_year;
int month = 1 + sys_time.tm_mon;
return (year > 2020) || (year == 2020 && month >= 10);
}
// ***** math helpers *****
// map x from [a1, a2] to [b1, b2]
@ -108,6 +128,15 @@ inline bool file_exists(const std::string& fn) {
return f.good();
}
inline std::string hexdump(const std::string& in) {
std::stringstream ss;
ss << std::hex << std::setfill('0');
for (size_t i = 0; i < in.size(); i++) {
ss << std::setw(2) << static_cast<unsigned int>(static_cast<unsigned char>(in[i]));
}
return ss.str();
}
}
class ExitHandler {

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

@ -61,6 +61,16 @@ def configure_ublox(dev):
dev.configure_message_rate(ublox.CLASS_MON, ublox.MSG_MON_HW, 1)
dev.configure_message_rate(ublox.CLASS_MON, ublox.MSG_MON_HW2, 1)
print("send on stop:")
# Save on shutdown
# Controlled GNSS stop and hot start
payload = struct.pack('<HBB', 0x0000, 0x08, 0x00)
dev.send_message(ublox.CLASS_CFG, ublox.MSG_CFG_RST, payload)
# UBX-UPD-SOS backup
dev.send_message(0x09, 0x14, b"\x00\x00\x00\x00")
if __name__ == "__main__":
class Device:

63
selfdrive/locationd/ublox_msg.h
View File

@ -4,11 +4,15 @@
#include <memory>
#include <string>
#include <unordered_map>
#include <ctime>
#include "cereal/messaging/messaging.h"
#include "selfdrive/common/util.h"
#include "selfdrive/locationd/generated/gps.h"
#include "selfdrive/locationd/generated/ubx.h"
using namespace std::string_literals;
// protocol constants
namespace ublox {
const uint8_t PREAMBLE1 = 0xb5;
@ -22,6 +26,65 @@ namespace ublox {
const uint8_t CLASS_NAV = 0x01;
const uint8_t CLASS_RXM = 0x02;
const uint8_t CLASS_MON = 0x0A;
struct ubx_mga_ini_time_utc_t {
uint8_t type;
uint8_t version;
uint8_t ref;
int8_t leapSecs;
uint16_t year;
uint8_t month;
uint8_t day;
uint8_t hour;
uint8_t minute;
uint8_t second;
uint8_t reserved1;
uint32_t ns;
uint16_t tAccS;
uint16_t reserved2;
uint32_t tAccNs;
} __attribute__((packed));
inline std::string ubx_add_checksum(std::string msg){
assert(msg.size() > 2);
uint8_t ck_a = 0, ck_b = 0;
for(int i = 2; i < msg.size(); i++) {
ck_a = (ck_a + msg[i]) & 0xFF;
ck_b = (ck_b + ck_a) & 0xFF;
}
std::string r = msg;
r.push_back(ck_a);
r.push_back(ck_b);
return r;
}
inline std::string build_ubx_mga_ini_time_utc(struct tm time) {
ublox::ubx_mga_ini_time_utc_t payload = {
.type = 0x10,
.version = 0x0,
.ref = 0x0,
.leapSecs = -128, // Unknown
.year = (uint16_t)(1900 + time.tm_year),
.month = (uint8_t)(1 + time.tm_mon),
.day = (uint8_t)time.tm_mday,
.hour = (uint8_t)time.tm_hour,
.minute = (uint8_t)time.tm_min,
.second = (uint8_t)time.tm_sec,
.reserved1 = 0x0,
.ns = 0,
.tAccS = 30,
.reserved2 = 0x0,
.tAccNs = 0,
};
assert(sizeof(payload) == 24);
std::string msg = "\xb5\x62\x13\x40\x18\x00"s;
msg += std::string((char*)&payload, sizeof(payload));
return ubx_add_checksum(msg);
}
}
class UbloxMsgParser {