sadmen
/
openpilot_comma
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)

Browse Source 
* Store almanac on ublox poweroff

* send current UTC time

* move message building to ublox_msg.h
old-commit-hash: ea5141d909
commatwo_master
Willem Melching 4 years ago committed by GitHub
parent d1498aa3d3
commit 4d874a90ea
6 changed files with 139 additions and 21 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. 24
      selfdrive/boardd/boardd.cc
  2. 32
      selfdrive/boardd/pigeon.cc
  3. 2
      selfdrive/boardd/pigeon.h
  4. 29
      selfdrive/common/util.h
  5. 10
      selfdrive/locationd/test/ubloxd.py
  6. 63
      selfdrive/locationd/ublox_msg.h

24
selfdrive/boardd/boardd.cc
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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 {

Write Preview
Loading…
Cancel
Save