diff --git a/cereal b/cereal index 23abef4fdb..ab32956aaf 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 23abef4fdb18bc2b06281f9937ed3e7fc31e5c37 +Subproject commit ab32956aaf927ee6318bcf639e3a25eea72027a9 diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index 48e7dfe3c1..ef5faeb36c 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -789,6 +789,7 @@ void pigeon_init() { usleep(100*1000); // init from ubloxd + // To generate this data, run test/ubloxd.py with the print statements enabled in the write function in panda/python/serial.py pigeon_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"); pigeon_send("\xB5\x62\x06\x3E\x00\x00\x44\xD2"); pigeon_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"); @@ -807,6 +808,7 @@ void pigeon_init() { pigeon_send("\xB5\x62\x06\x01\x03\x00\x01\x07\x01\x13\x51"); pigeon_send("\xB5\x62\x06\x01\x03\x00\x02\x15\x01\x22\x70"); pigeon_send("\xB5\x62\x06\x01\x03\x00\x02\x13\x01\x20\x6C"); + pigeon_send("\xB5\x62\x06\x01\x03\x00\x0A\x09\x01\x1E\x70"); LOGW("panda GPS on"); } diff --git a/selfdrive/locationd/test/ublox.py b/selfdrive/locationd/test/ublox.py index 496207d401..d99e714696 100644 --- a/selfdrive/locationd/test/ublox.py +++ b/selfdrive/locationd/test/ublox.py @@ -475,6 +475,8 @@ msg_types = { UBloxDescriptor('AID_ALM', ' UbloxMsgParser::gen_nav_data() { return kj::Array(); } +kj::Array UbloxMsgParser::gen_mon_hw() { + mon_hw_msg *msg = (mon_hw_msg *)&msg_parse_buf[UBLOX_HEADER_SIZE]; + + capnp::MallocMessageBuilder msg_builder; + cereal::Event::Builder event = msg_builder.initRoot(); + event.setLogMonoTime(nanos_since_boot()); + auto gnss = event.initUbloxGnss(); + auto hwStatus = gnss.initHwStatus(); + hwStatus.setNoisePerMS(msg->noisePerMS); + hwStatus.setAgcCnt(msg->agcCnt); + hwStatus.setAStatus((cereal::UbloxGnss::HwStatus::AntennaSupervisorState) msg->aStatus); + hwStatus.setAPower((cereal::UbloxGnss::HwStatus::AntennaPowerStatus) msg->aPower); + hwStatus.setJamInd(msg->jamInd); + return capnp::messageToFlatArray(msg_builder); +} + bool UbloxMsgParser::add_data(const uint8_t *incoming_data, uint32_t incoming_data_len, size_t &bytes_consumed) { int needed = needed_bytes(); if(needed > 0) { diff --git a/selfdrive/locationd/ublox_msg.h b/selfdrive/locationd/ublox_msg.h index eca6186d4a..4bb103bb50 100644 --- a/selfdrive/locationd/ublox_msg.h +++ b/selfdrive/locationd/ublox_msg.h @@ -85,6 +85,27 @@ typedef struct __attribute__((packed)) { uint32_t dwrd; } rxm_sfrbx_msg_extra; +// MON_HW +typedef struct __attribute__((packed)) { + uint32_t pinSel; + uint32_t pinBank; + uint32_t pinDir; + uint32_t pinVal; + uint16_t noisePerMS; + uint16_t agcCnt; + uint8_t aStatus; + uint8_t aPower; + uint8_t flags; + uint8_t reserved1; + uint32_t usedMask; + uint8_t VP[17]; + uint8_t jamInd; + uint8_t reserved2[2]; + uint32_t pinIrq; + uint32_t pullH; + uint32_t pullL; +} mon_hw_msg; + namespace ublox { // protocol constants const uint8_t PREAMBLE1 = 0xb5; @@ -93,6 +114,7 @@ namespace ublox { // message classes const uint8_t CLASS_NAV = 0x01; const uint8_t CLASS_RXM = 0x02; + const uint8_t CLASS_MON = 0x0A; // NAV messages const uint8_t MSG_NAV_PVT = 0x7; @@ -101,6 +123,9 @@ namespace ublox { const uint8_t MSG_RXM_RAW = 0x15; const uint8_t MSG_RXM_SFRBX = 0x13; + // MON messages + const uint8_t MSG_MON_HW = 0x09; + const int UBLOX_HEADER_SIZE = 6; const int UBLOX_CHECKSUM_SIZE = 2; const int UBLOX_MAX_MSG_SIZE = 65536; @@ -113,6 +138,7 @@ namespace ublox { UbloxMsgParser(); kj::Array gen_solution(); kj::Array gen_raw(); + kj::Array gen_mon_hw(); kj::Array gen_nav_data(); bool add_data(const uint8_t *incoming_data, uint32_t incoming_data_len, size_t &bytes_consumed); diff --git a/selfdrive/locationd/ubloxd_main.cc b/selfdrive/locationd/ubloxd_main.cc index 6554ba7794..4e07932fff 100644 --- a/selfdrive/locationd/ubloxd_main.cc +++ b/selfdrive/locationd/ubloxd_main.cc @@ -97,6 +97,17 @@ int ubloxd_main(poll_ubloxraw_msg_func poll_func, send_gps_event_func send_func) } } else LOGW("Unknown rxm msg id: 0x%02X", parser.msg_id()); + } else if(parser.msg_class() == CLASS_MON) { + if(parser.msg_id() == MSG_MON_HW) { + //LOGD("MSG_MON_HW"); + auto words = parser.gen_mon_hw(); + if(words.size() > 0) { + auto bytes = words.asBytes(); + send_func(ubloxGnss, bytes.begin(), bytes.size()); + } + } else { + LOGW("Unknown mon msg id: 0x%02X", parser.msg_id()); + } } else LOGW("Unknown msg class: 0x%02X", parser.msg_class()); parser.reset();