dragonpilot/selfdrive/locationd/ublox_msg.h

#pragma once

#include <cassert>
#include <cstdint>
#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;
  const uint8_t PREAMBLE2 = 0x62;

  const int UBLOX_HEADER_SIZE = 6;
  const int UBLOX_CHECKSUM_SIZE = 2;
  const int UBLOX_MAX_MSG_SIZE = 65536;

  // Boardd still uses these:
  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(const 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 {
  public:
    bool add_data(const uint8_t *incoming_data, uint32_t incoming_data_len, size_t &bytes_consumed);
    inline void reset() {bytes_in_parse_buf = 0;}
    inline int needed_bytes();
    inline std::string data() {return std::string((const char*)msg_parse_buf, bytes_in_parse_buf);}

    std::pair<std::string, kj::Array<capnp::word>> gen_msg();
    kj::Array<capnp::word> gen_nav_pvt(ubx_t::nav_pvt_t *msg);
    kj::Array<capnp::word> gen_rxm_sfrbx(ubx_t::rxm_sfrbx_t *msg);
    kj::Array<capnp::word> gen_rxm_rawx(ubx_t::rxm_rawx_t *msg);
    kj::Array<capnp::word> gen_mon_hw(ubx_t::mon_hw_t *msg);
    kj::Array<capnp::word> gen_mon_hw2(ubx_t::mon_hw2_t *msg);

  private:
    inline bool valid_cheksum();
    inline bool valid();
    inline bool valid_so_far();

    std::unordered_map<int, std::unordered_map<int, std::string>> gps_subframes;

    size_t bytes_in_parse_buf = 0;
    uint8_t msg_parse_buf[ublox::UBLOX_HEADER_SIZE + ublox::UBLOX_MAX_MSG_SIZE];

};