Skip to content

Instantly share code, notes, and snippets.

@tve

tve/init.cpp

Last active November 7, 2024 21:55
Show Gist options
  • Save tve/a316f6b4a51215b5062d0b603eac0f67 to your computer and use it in GitHub Desktop.
Save tve/a316f6b4a51215b5062d0b603eac0f67 to your computer and use it in GitHub Desktop.
Download ZIP
Minimal UBX parsing
Raw
init.cpp
// Configure which GNSS constellation to use
constexpr struct PACKED {
Header hdr; uint8_t version; uint8_t layers; uint16_t resv;
cfgL c1, c2, c3;
Cksum ck;
} gpsConfigGNSS = {
MakeHeader(UBX_CFG_VALSET, sizeof(gpsConfigGNSS)-8), 0, L_RAM|L_BBR, 0,
CFG_SIGNAL_GAL_ENA_OFF, CFG_SIGNAL_BDS_ENA_OFF, CFG_SIGNAL_GLO_ENA_OFF,
{0,0}
};
// configuration for CloudLocate w/out power management
constexpr struct PACKED {
Header hdr; uint8_t version; uint8_t layers; uint16_t resv;
cfgU2 c4, c4b; cfgU1 c5, c6;
cfgU1 c10, c11, c12;
cfgU1 c15, c16, c17, c18;
cfgL c55;
cfgU1 cN;
Cksum ck;
} gpsConfigCL = {
MakeHeader(UBX_CFG_VALSET, sizeof(gpsConfigCL)-8), 0, L_RAM|L_BBR, 0,
CFG_RATE_MEAS_1s, CFG_RATE_SOLN_1, CFG_MON_RXR_1s, CFG_INFMSG_UBX_UART_WRN,
CFG_NAV_SAT_1, CFG_NAV_ORB_1, CFG_NAV_UTC_10,
CFG_NAV_MON_PVT_0, CFG_RXM_MEAS20_DIS, CFG_RXM_MEAS50_ENA, CFG_RXM_MEASX_ENA,
CFG_ANA_ENA,
CFG_PM_OPERATEMODE_FULL,
{0,0}
};
// configuration for NAV_PVT without power management
constexpr struct PACKED {
Header hdr; uint8_t version; uint8_t layers; uint16_t resv;
cfgU2 c4, c4b; cfgU1 c5, c6;
cfgU1 c10, c11, c12;
cfgU1 c15, c16, c17, c18;
cfgU2 c50; cfgL c55;
cfgU1 cN;
Cksum ck;
} gpsConfigPVT = {
MakeHeader(UBX_CFG_VALSET, sizeof(gpsConfigPVT)-8), 0, L_RAM|L_BBR, 0,
CFG_RATE_MEAS_1s, CFG_RATE_SOLN_1, CFG_MON_RXR_1s, CFG_INFMSG_UBX_UART_WRN,
CFG_NAV_SAT_1, CFG_NAV_ORB_1, CFG_NAV_UTC_10,
CFG_NAV_MON_PVT_1s, CFG_RXM_MEAS20_DIS, CFG_RXM_MEAS50_DIS, CFG_RXM_MEASX_DIS,
CFG_NAVSPG_POS_ACC_200m, CFG_ANA_ENA,
CFG_PM_OPERATEMODE_FULL,
{0,0}
};
// configuration for NAV_VT with power management
constexpr struct PACKED {
Header hdr; uint8_t version; uint8_t layers; uint16_t resv;
cfgU2 c4, c4b; cfgU1 c5, c6;
cfgU1 c10, c11, c12;
cfgU1 c15, c16, c17, c18;
cfgU2 c50; cfgL c55;
cfgU1 c20; cfgU2 c21; cfgU4 c25, c26;
cfgL c30; cfgU1 c31, c32;
Cksum ck;
} gpsConfigPVTPM = {
MakeHeader(UBX_CFG_VALSET, sizeof(gpsConfigPVTPM)-8), 0, L_RAM|L_BBR, 0,
CFG_RATE_MEAS_1s, CFG_RATE_SOLN_1, CFG_MON_RXR_1s, CFG_INFMSG_UBX_UART_WRN,
CFG_NAV_SAT_5, CFG_NAV_ORB_1, CFG_NAV_UTC_10,
CFG_NAV_MON_PVT_1s, CFG_RXM_MEAS20_DIS, CFG_RXM_MEAS50_DIS, CFG_RXM_MEASX_DIS,
CFG_NAVSPG_POS_ACC_200m, CFG_ANA_ENA,
CFG_PM_MAXACQTIME_0, CFG_PM_ONTIME_0s, CFG_PM_ACQPERIOD_0, CFG_PM_POSUPDATEPERIOD_4500s,
CFG_PM_DONOTENTEROFF_DIS, CFG_PM_UPDATEEPH_ON, CFG_PM_OPERATEMODE_OO,
{0,0}
};
constexpr struct PACKED {
Header hdr;
uint8_t version; uint8_t layers; uint16_t resv;
cfgU4 baudrate;
cfgL outnmea, outubx;
Cksum ck;
} portConfig = {
MakeHeader(UBX_CFG_VALSET, sizeof(portConfig)-8), 0, L_RAM|L_BBR, 0,
CFG_UART1_BAUDRATE_38400,
CFG_UART1_OUT_UBX_ON, CFG_UART1_OUT_NMEA_OFF,
{0,0}
};
constexpr struct PACKED {
Header hdr;
uint16_t bbrMask;
uint8_t resetMode, resv;
Cksum ck;
} warmReset = {
MakeHeader(UBX_CFG_RST, sizeof(warmReset)-8),
// 0x001F, // reset all nav info, but not time info
0x8001, // reset ephemeris and autonomous orbit info only
0x02, 0, // controlled software reset of GNSS only
{0,0}
};
constexpr struct PACKED {
Header hdr; uint32_t version;
uint32_t duration, flags, wakeup;
Cksum ck;
} gpsSleep = {
MakeHeader(UBX_RXM_PMREQ, sizeof(gpsSleep)-8), 0,
0, // wait forever
0x6, // backup mode and force (see integration manual)
0x8, // wake-up on uart edge
{0,0}
};
const struct {
uint8_t *buf; uint32_t len; char name[8];
} configs[] = {
{ (uint8_t*)&portConfig, sizeof(portConfig), "uart" },
{ (uint8_t*)&gpsConfigGNSS, sizeof(gpsConfigGNSS), "gnss" },
{ (uint8_t*)&CONFIG, sizeof(CONFIG), "config" },
{ nullptr, 0, "done" },
};
Raw
main.cpp
// Program the GPS to output an 8Mhz clock
#include <jee.h>
#include <jee/hal.h>
#include <alloca.h>
using namespace jeeh;
#include "defs.h"
#include "console.h"
#include "pin-power.h"
#include "timer.h"
#include "ublox.h"
using namespace ublox;
Uart *theUart = nullptr;
Uart *uart2 = nullptr;
auto fcnt = new Timer2<TIM2.ADDR>(ena::TIM2);
Pin fcntClk, fcntCap;
// auto lseOut = new Timer2<TIM16.ADDR>(ena::TIM16);
// Pin lsePin;
bool gpsReady = false;
// ===== Power levels
// assuming synchronous drivers: i2c/spi/uart are inactive when lowestPower() is called
uint8_t jeeh::lowestPower (uint8_t power, uint16_t ms) {
(void)power;
(void)ms;
return sys::SLEEP; // required due to bug
}
void jeeh::resumePower () {
// logf("back!");
}
uint32_t efficientClock () { return slowClock(true); } // 4Mhz
#define SysCoreClkMhz (uint8_t)(SystemCoreClock/1000000)
// return a message to call o.f(msg) in ms time
// usage: sys::send(delay<T>(...))
template< typename T >
Message& delay(Message &msg, uint16_t ms, T* o, void (T::* f)(Message&)) {
if (msg.inUse()) { logf("already delaying"); fail(); }
msg.mDst = '@'; // ticker device ID
msg.mLen = ms;
msg.mObj = (uint8_t*) o;
msg.mFun = (void (*) (void*,Message&)) ((uint32_t*) &f)[0];
assert(((uint32_t*) &f)[1] == 0); // TODO vtable support
return msg;
}
uint32_t deltaTime(uint32_t t0) { return (rtc::getMillis()-t0) & ((1<<26)-1); }
uint32_t deltaTime(uint32_t t1, uint32_t t0) { return (t1-t0) & ((1<<26)-1); }
void printBuf(uint8_t *buf, uint16_t len) {
for (uint16_t i=0; i<len; i+=1) {
uint8_t ch = buf[i];
// if (ch >= ' ' && ch < 127)
// printf("%1c", ch);
// else
printf("\\%02x", ch);
}
printf("\n");
}
// ===== Test activity just relaying the characters from the GPS to the console
//
// Kind'a assumes the GPS starts from a reset in NMEA 9600 baud mode
struct RelayGPSActivity {
Message txMsg[3] = {};
Message rxMsg{'G', 'R', 0, nullptr};
Uart uart;
// uint8_t buf[300];
RelayGPSActivity(Uart &u) : uart{u} {}
void start() {
logf("Listening to GPS");
uart.baudRate(38400);
startRx();
}
// void sendUBX(Message &m, uint8_t *ubxMsg, uint16_t len) {
// assert(!m.inUse());
// uint8_t *txBuf = (uint8_t*)malloc(len);
// memcpy(txBuf, ubxMsg, len);
// ublox::CalcCksum(txBuf+2, len-4);
// m.mDst = 'G';
// m.mTag = 'W';
// m.mPtr = txBuf;
// m.mLen = len;
// printf("UBX TX %d: ", m.mLen);
// printBuf(m.mPtr, m.mLen);
// sys::send(m);
// }
void freePtr(Message &m) {
free(m.mPtr);
m.mPtr = nullptr;
}
void recv(Message &m) {
led = 1;
logf("Got %d", m.mLen);
uint8_t *buf = (uint8_t*)malloc(m.mLen);
assert(buf != nullptr);
memcpy(buf, m.mPtr, m.mLen);
uint8_t i = 3;
while (i >= 3) {
for (i=0; i<3; i++) if (!txMsg[i].inUse()) break;
if (i >= 3) sys::recv();
}
txMsg[i].mDst = 'U';
txMsg[i].mTag = 'W';
txMsg[i].mPtr = buf;
txMsg[i].mLen = m.mLen;
sys::send(txMsg[i].setCallback(this, &RelayGPSActivity::freePtr));
startRx();
led = 0;
}
void startRx() { sys::send(rxMsg.setCallback(this, &RelayGPSActivity::recv)); }
};
void printNavPVT(ublox::UbxNavPVT *rec) {
// printf("UBX_NAV_PVT ");
if (rec->valid & 1) printf("%04d-%02d-%02d", rec->year, rec->month, rec->day);
else printf("nodate");
if (rec->valid & 2) printf(" %02d:%02d:%02d", rec->hour, rec->min, rec->sec);
else printf(" notime");
if (rec->fixType < 2 || rec->fixType > 3) {
printf(" nofix(%d)", rec->fixType);
} else {
int32_t lat = rec->lat >= 0 ? rec->lat : -rec->lat;
uint8_t latH = rec->lat >= 0 ? 'N' : 'S';
int32_t lon = rec->lon >= 0 ? rec->lon : -rec->lon;
uint8_t lonH = rec->lon >= 0 ? 'E' : 'W';
static constexpr uint32_t shift = 10'000'000;
printf(" %ld.%ld%c %ld.%ld%c %ldm",
lon/shift, (lon%shift)/100, lonH, lat/shift, (lat%shift)/100, latH, rec->hMSL/1000);
printf(" Δ%ldm/%ldm", rec->hAcc/1000, rec->vAcc/1000);
}
uint8_t f1 = rec->flags;
static constexpr char psmStates[] = "N/A\0STA\0ACQ\0TRK\0POT\0INA";
const char *psm = psmStates + 4*((f1>>2)&7);
const char *dim = rec->fixType == 2 ? "-2D" : rec->fixType == 3 ? "-3D" : "";
printf(" %c%s%s %s sv:%d\n", f1&1?'V':'I', dim, f1&2?"-diff":"", psm, rec->numSV);
}
// ===== GPS Activity
// Configure which GNSS constellation to use
constexpr struct PACKED {
Header hdr; uint8_t version; uint8_t layers; uint16_t resv;
cfgL c1, c2, c3;
Cksum ck;
} gpsConfigGNSS = {
MakeHeader(UBX_CFG_VALSET, sizeof(gpsConfigGNSS)-8), 0, L_RAM|L_BBR, 0,
CFG_SIGNAL_GAL_ENA_OFF, CFG_SIGNAL_BDS_ENA_OFF, CFG_SIGNAL_GLO_ENA_OFF,
{0,0}
};
// configuration for NAV_PVT without power management
constexpr struct PACKED {
Header hdr; uint8_t version; uint8_t layers; uint16_t resv;
cfgU2 c4, c4b; cfgU1 c5, c6;
cfgU1 c10, c11, c12;
cfgU1 c15, c16, c17, c18;
cfgU2 c50; cfgL c55;
cfgU1 cN;
Cksum ck;
} gpsConfigPVT = {
MakeHeader(UBX_CFG_VALSET, sizeof(gpsConfigPVT)-8), 0, L_RAM|L_BBR, 0,
CFG_RATE_MEAS_1s, CFG_RATE_SOLN_1, CFG_MON_RXR_1s, CFG_INFMSG_UBX_UART_WRN,
CFG_NAV_SAT_0, CFG_NAV_ORB_0, CFG_NAV_UTC_10,
CFG_NAV_MON_PVT_1s, CFG_RXM_MEAS20_DIS, CFG_RXM_MEAS50_DIS, CFG_RXM_MEASX_DIS,
CFG_NAVSPG_POS_ACC_200m, CFG_ANA_ENA,
CFG_PM_OPERATEMODE_FULL,
{0,0}
};
// configuration for the serial port to get UBX and no NMEA stanzas
constexpr struct PACKED {
Header hdr; uint8_t version; uint8_t layers; uint16_t resv;
cfgU4 baudrate;
cfgL outnmea, outubx;
Cksum ck;
} portConfig = {
MakeHeader(UBX_CFG_VALSET, sizeof(portConfig)-8), 0, L_RAM|L_BBR, 0,
CFG_UART1_BAUDRATE_38400,
CFG_UART1_OUT_UBX_ON, CFG_UART1_OUT_NMEA_OFF,
{0,0}
};
// configuration to enable 8kHz timepulse output when locked
constexpr struct PACKED {
Header hdr; uint8_t version; uint8_t layers; uint16_t resv;
cfgU1 c1, c2, c3, c4;
cfgU4 c10; cfgR8 c20, c21;
Cksum ck;
} timepulse8MhzLock = {
MakeHeader(UBX_CFG_VALSET, sizeof(timepulse8MhzLock)-8), 0, L_RAM|L_BBR, 0,
CFG_TP_ISFREQ, CFG_TP_RATIO, CFG_TP_TIMEGRID_GPS, CFG_TP_USELOCKED_ENA,
CFG_TP_8MHZ_LOCKED, CFG_TP_DUTY_1, CFG_TP_DUTY_50_LOCKED,
{0,0}
};
// configuration to enable 8kHz timepulse output (unconditionally))
constexpr struct PACKED {
Header hdr; uint8_t version; uint8_t layers; uint16_t resv;
cfgU1 c1, c2, c3, c4;
cfgU4 c10; cfgR8 c20;
Cksum ck;
} timepulse8Mhz = {
MakeHeader(UBX_CFG_VALSET, sizeof(timepulse8Mhz)-8), 0, L_RAM|L_BBR, 0,
CFG_TP_ISFREQ, CFG_TP_RATIO, CFG_TP_TIMEGRID_GPS, CFG_TP_USELOCKED_DIS,
CFG_TP_8MHZ, CFG_TP_DUTY_50,
{0,0}
};
// array of configurations to send to device
const struct {
uint8_t *buf; uint32_t len; char name[8];
} configs[] = {
{ (uint8_t*)&portConfig, sizeof(portConfig), "uart" },
{ (uint8_t*)&gpsConfigGNSS, sizeof(gpsConfigGNSS), "gnss" },
{ (uint8_t*)&gpsConfigPVT, sizeof(gpsConfigPVT), "pvt" },
{ (uint8_t*)&timepulse8Mhz, sizeof(timepulse8Mhz), "tp" },
{ nullptr, 0, "done" },
};
// =====
#define MAXSV 24
// GPS data collection structs
struct GPSFix {
// from NAV_PVT
int32_t lat, lon, alt_msl;
uint16_t hAcc, vAcc; // in meters
bool valid;
uint8_t numSV;
// from NAV_SAT
struct {
uint8_t svId;
uint8_t cno;
uint8_t qual;
bool ephAvail;
} sats[MAXSV];
};
// GPSActivity states
enum { doConfig, doInit, doRun };
struct GPSActivity {
Message txMsg{'G', 'W'};
Message txMsg2{'G', 'W'};
Message rxMsg{'G', 'R', 0, nullptr};
Message wakeMsg{};
# define TXFREE txMsg.setCallback(this, &GPSActivity::freeUBX)
uint32_t itow; // ms in week
uint8_t state = doConfig;
bool gotDateTime = false;
uint8_t configIX = 0;
uint32_t lastCnt = 0;
GPSActivity() {}
void start() {
logf("Configuring GPS");
state = doConfig;
startRx(rxMsg);
itow = 0;
sendConfig();
// logf("Done sending GPS config");
}
void switchBaudRate(bool fast) {
uart2->baudRate(fast ? 38400 : 9600);
cycles::msBusy(2);
}
void sendUBX(Message &m, uint8_t *ubxMsg, uint16_t len) {
assert(!m.inUse());
uint8_t *txBuf = (uint8_t*)malloc(len);
if (txBuf == nullptr) logf("OOPS1");
memcpy(txBuf, ubxMsg, len);
ublox::CalcCksum(txBuf+2, len-4);
m.mDst = 'G';
m.mTag = 'W';
m.mPtr = txBuf;
m.mLen = len;
#if 1
printf("UBX TX %d (0x%02x%02x)\n", m.mLen, m.mPtr[2], m.mPtr[3]);
#elif 1
printf("UBX TX %d: ", m.mLen);
printBuf(m.mPtr, m.mLen);
#endif
sys::send(m);
}
void freeUBX(Message &m) {
assert(m.mPtr != nullptr);
free(m.mPtr);
}
void nopCB(Message &) {}
// poll for GNSS systems in use, this will trigger sending the config
// void queryMonGNSS(Message &m) {
// static constexpr ublox::Poll pollMonGNSS = ublox::PollUbxMonGNSS();
// sendUBX(m, (uint8_t*)&pollMonGNSS, sizeof(pollMonGNSS));
// }
// send current config stanza
void sendConfig() {
if (configs[configIX].len == 0) return;
printf("Sending '%s' config\n", configs[configIX].name);
sendUBX(TXFREE, configs[configIX].buf, configs[configIX].len);
}
void updateTime(uint16_t off) { // offset of year value in frame
uint8_t *p = ublox::frame.payload;
auto dd = rtc::getDate();
uint16_t yr = p[off] | ((uint16_t)p[off+1]<<8);
uint8_t mo=p[off+2], dy=p[off+3];
uint8_t hh=p[off+4], mm=p[off+5], ss=p[off+6];
if (dd.yr != yr%100 || dd.mo != mo || dd.dy != dy || dd.hh != hh || dd.mm != mm || dd.ss != ss) {
printf("Adjusting time %02d-%02d-%02d %02d:%02d:%02d -> %02d-%02d-%02d %02d:%02d:%02d\n",
dd.yr, dd.mo, dd.dy, dd.hh, dd.mm, dd.ss, yr, mo, dy, hh, mm, ss);
sys::wait(2); // before messing with rtc...
rtc::set(DateTime(yr, mo, dy, hh, mm, ss));
}
gotDateTime = true;
}
void recv(Message &m) {
#if 0
uint32_t at = rtc::getMillis();
printf("\n{%ldms:%d s=%d", at%10000, m.mLen, ublox::state);
for (int i=0; i<m.mLen; i++) printf("%c%02x", m.mPtr[i]==0xB5?'*':' ', m.mPtr[i]);
printf("\n");
// if (at - lastRecv > 300) ublox::state = 0;
// if (at - lastRecv > 300) sendUBX(txMsg2.setCallback(this, &GPSActivity::freeUBX), nulls, 20);
// lastRecv = at;
#endif
// detect 9600 baud operation and cause switch
if (state == doConfig && configIX == 0 && m.mLen >= 10) {
bool ok = false;
for (int i=0; i<m.mLen; i++) {
uint8_t ch = m.mPtr[i];
if (ch != 0x80 && ch != 0xf8) ok = true;
}
if (!ok) {
printf("Looks like 9600 baud\n");
switchBaudRate(false);
sendConfig();
cycles::msBusy(40); // 30 chars sent -> 31.25ms
switchBaudRate(true);
}
}
for (int i=0; i<m.mLen; i++) {
if (!ublox::parse(m.mPtr[i])) continue;
// printf(">%04x\n", ublox::frame.classId);
switch (ublox::frame.classId) {
// primary navigation message with time & location & more
case ublox::UBX_NAV_PVT: {
// logf("UBX_NAV_PVT");
if (ublox::frame.payLen < sizeof(ublox::UbxNavPVT)) { logf("UBX_NAV_PVT too short"); break; }
ublox::UbxNavPVT *rec = (ublox::UbxNavPVT*)ublox::frame.payload;
bool datetimeValid = (rec->valid & 0x7) == 7;
if (datetimeValid && !gotDateTime) updateTime(4); // FIXME: slew...
// figure out whether it's time to sleep
printNavPVT(rec);
if (state == doInit && datetimeValid) {
updateTime(4);
state = doRun;
printf("Got datetime, init done\n");
}
uint32_t cnt = fcnt->read();
uint32_t capVal = fcnt->capVal - fcnt->capVal1;
uint32_t capCnt = fcnt->capCnt;
printf("Cnt: %lu delta: %ld | cap: #%ld at %lu\n",
cnt, cnt-lastCnt, capCnt, capVal);
uint32_t freq = (uint64_t)capCnt * 8 * 8000000000 / capVal;
int32_t err = freq-32768000;
uint32_t absErr = err >= 0 ? err : -err;
uint32_t ppb = (uint64_t)absErr * 1000000000 / 32768000;
printf("F=%lu.%03luHz err=%ld.%03luHz=%luppb\n",
freq/1000, freq%1000, err/1000, absErr%1000, ppb);
lastCnt = cnt;
break; }
// UTC time info
case ublox::UBX_NAV_TIMEUTC: {
if (ublox::frame.payLen < 20) { logf("UBX_NAV_TIMEUTC too short"); break; }
uint8_t *p = ublox::frame.payload;
bool datetimeValid = (p[19] & 0x7) == 7; // may be off by leap seconds
if (!gotDateTime) {
uint16_t yy = p[12] | ((uint16_t)p[13]<<8);
if (p[19] & 1) printf("%04d-%02d-%02d", yy, p[14], p[15]);
else printf("nodate");
if (p[19] & 2) printf(" %02d:%02d:%02d", p[16], p[17], p[18]);
else printf(" notime");
if (datetimeValid) printf(" valid\n"); else printf(" not valid (%x)\n", p[19]&0x7);
}
if (!datetimeValid) break;
if (state == doInit) {
updateTime(12);
state = doRun;
printf("Got datetime, init done\n");
} else if (!gotDateTime) updateTime(12); // FIXME: should continue sync'ing
break; }
case ublox::NMEA: {
ublox::frame.payload[ublox::frame.payLen] = 0;
printf("NMEA %d: %s\n", ublox::frame.payLen, ublox::frame.payload);
#if 1
if (!txMsg2.inUse())
sendUBX(txMsg2.setCallback(this, &GPSActivity::freeUBX), (uint8_t*)&portConfig, sizeof(portConfig));
#else
if (!txMsg.inUse())
sendUBX(TXFREE, (uint8_t*)&portConfig, sizeof(portConfig));
#endif
break; }
default:
switch (ublox::frame.classId & 0xf) {
// informational messages (debug/info/warn/err text)
case 0x04: { // UBX-INF
ublox::frame.payload[ublox::frame.payLen] = 0; // hack
printf("%s %s\n", ublox::UbxInfo[ublox::frame.classId>>8], ublox::frame.payload);
break; }
// ack-nack to our config messages, used to drive state machine
case 0x05: { // UBX-ACK
uint8_t *p = ublox::frame.payload;
bool ack = !!(ublox::frame.classId>>4);
const char *which = ack ? "ACK" : "NAK ****";
printf("UBX_ACK_%s %02x:%02x\n", which, p[0], p[1]);
uint16_t classId = p[0] | ((uint16_t)p[1]<<8);
// FIXME: handle NACK, perhaps by resetting GPS?
// if this is a response to a CFG_VALSET command then check for more to send
if (classId == UBX_CFG_VALSET && state == doConfig) {
if (ack) {
configIX++;
if (configs[configIX].len > 0) {
sendConfig();
} else {
#if WARM_RESET
sendUBX(TXFREE, (uint8_t*)&warmReset, sizeof(warmReset));
printf("Config done, sending reset\n");
#else
printf("Config done\n");
#endif
state = doInit;
}
} else {
sendConfig(); // rexmit
}
}
break; }
default:
printf("Got Ublox %02x:%02x len:%d\n",
ublox::frame.classId & 0xff, ublox::frame.classId >> 8, ublox::frame.payLen);
}
}
}
startRx(m);
}
void startRx(Message &m) {
assert(!m.inUse());
sys::send(m.setCallback(this, &GPSActivity::recv));
}
};
// ===== main
int main () {
led.mode("P"); led = 0;
fastClock(true);
cycles::init();
sys::wait(0); // init Ticker
rtc::init(true); // got 32 kHz xtal
if (rtc::getSecs() < 367*86400) rtc::set(DateTime());
// handle debug signals
#ifdef LED2
led2.mode("P"); led2 = 1;
#endif
cycles::msBusy(1000);
// the UART config comes from platformio.ini and is defined in defs.h
theUart = new Uart{'U'};
theUart->init(UART_PINS, 115'200, { UART_NAME.ADDR, ena::UART_NAME,
UART_FREQ, Irq::UART_NAME, UART_CONF });
stdout_device = 'U';
printf("\n\n***** %s: JeeGPSFreqCtr @%ld MHz", SVDNAME, SystemCoreClock / 1'000'000);
auto dd = rtc::getDate();
printf(" -- 20%02d-%02d-%02d %02d:%02d:%02d\n", dd.yr, dd.mo, dd.dy, dd.hh, dd.mm, dd.ss);
cycles::msBusy(500);
#ifdef LED2
led2 = 0;
#else
led = 1;
#endif
// fcnt = new Timer2<TIM2.ADDR>((uint16_t)ena::TIM2, 1);
fcntClk.init(TIM2_CLK);
fcntCap.init("A0:1");
fcnt->init(3); // mode 3 -> capture
fcnt->start();
uart2 = new Uart{'G'};
uart2->init(UARTGPS_PINS, 38400, { UARTGPS_NAME.ADDR, ena::UARTGPS_NAME,
UARTGPS_FREQ, Irq::UARTGPS_NAME, UARTGPS_CONF });
#if 1
GPSActivity gpsAct{};
gpsAct.start();
#else
RelayGPSActivity gpsAct{*uart2};
gpsAct.start();
#endif
while (true) sys::recv();
}
Raw
ulox.h
#include <stdint.h>
namespace ublox {
constexpr int MAX_PAYLOAD = 700;
// Message class & type as uint16_t, class in low byte due to little endianness
enum {
UBX_ACK_ACK = 0x0105,
UBX_ACK_NAK = 0x0205,
UBX_CFG_CFG = 0x0906,
UBX_CFG_RST = 0x0406,
UBX_CFG_VALSET = 0x8a06,
UBX_NAV_PVT = 0x0701,
UBX_NAV_STATUS = 0x0301,
UBX_NAV_ORB = 0x3401,
UBX_NAV_SAT = 0x3501,
UBX_NAV_TIMEUTC = 0x2101,
UBX_MON_GNSS = 0x280a,
UBX_MON_RXR = 0x210a,
UBX_RXM_MEAS20 = 0x8402,
UBX_RXM_MEAS50 = 0x8602,
UBX_RXM_MEASX = 0x1402,
UBX_RXM_PMREQ = 0x4102,
NMEA = 0xffff,
};
// ===== RX
uint8_t paybuf[MAX_PAYLOAD];
uint16_t paylen;
uint8_t rxCkA;
uint8_t cksumA, cksumB;
struct Frame {
uint16_t classId;
uint16_t payLen;
uint8_t *payload;
} frame;
struct UbxNavPVT {
uint32_t itow;
uint16_t year;
uint8_t month, day, hour, min, sec;
uint8_t valid;
uint32_t tAcc; // time accuracy
int32_t nano;
uint8_t fixType, flags, flags2, numSV;
int32_t lon, lat, height, hMSL;
uint32_t hAcc, vAcc; // horiz/vert accuracy
int32_t velN, velE, velD, gSpeed; // velocity north, east, down
int32_t headMot;
uint32_t sAcc, headAcc; // accuracies
uint16_t pDOP;
uint16_t flags3;
};
struct UbxNavOrbSV {
uint8_t gnssId;
uint8_t svId;
uint8_t svFlag;
uint8_t eph;
uint8_t alm;
uint8_t otherOrb;
};
struct UbxNavOrb {
uint32_t itow;
uint8_t version;
uint8_t numSV;
uint16_t resv;
UbxNavOrbSV sv[1];
};
struct UbxNavSatSV {
uint8_t gnssId;
uint8_t svId;
uint8_t cno;
int8_t elev;
int16_t azim;
int16_t prRes;
// uint32_t flags;
uint8_t quality:3;
bool used:1;
uint8_t health:2; // unkn,healthy,unhealthy
bool diffCorr:1;
bool smoothed:1;
uint8_t orbitSrc:3; // none,eph,alm,ANoffline,ANauto,other..
bool ephAvail:1;
bool almAvail:1;
bool anoAvail:1; // AssistNow-offline
bool aopAvail:1; // AssistNow-autonomous
bool sbasUsed:1, rtcmUsed:1, slasUsed:1, spartnUsed:1;
bool prUsed:1, crUsed:1, doUsed:1, clasUsed:1;
};
struct UbxNavSat {
uint32_t itow;
uint8_t version;
uint8_t numSV;
uint16_t resv;
UbxNavSatSV sv[1];
};
struct UbxMeasxHdr {
uint32_t version; // really U1 and 3xU1 resv; v==1 for now
uint32_t gpsTow, gloTow, bdsTow, resv1, qzssTow;
uint16_t gpsTowAcc, gloTowAcc, bdsTowAcc, resv2, qzssTowAcc;
uint8_t numSV, flags, resv3[8];
};
static_assert(sizeof (UbxMeasxHdr) == 44);
struct UbxMeasxSat {
uint8_t gnssId, svId, cNo, mPathIndic;
int32_t dopplerMS, dopplerHz;
uint16_t wholeChips, fracChips;
uint32_t codePhase;
uint8_t intCodePhase, pseuRangeRMSErr, resv[2];
};
static_assert(sizeof (UbxMeasxSat) == 24);
// parsing states (the name indicates what we expect)
enum { sync1, sync2, mclass, mid, len1, len2, pay, skip, ckA, ckB, nmea_lf, nmea_dollar, nmea_cr };
uint8_t state = sync1;
void initCksum() { cksumA = 0; cksumB = 0; }
void addCksum(uint8_t ch) { cksumA += ch; cksumB += cksumA; }
// returns true if a full frame has been received
bool parse(uint8_t ch) {
switch (state) {
case sync1:
if (ch == 0xb5) { state = sync2; }
else if (ch == '\r') { state = nmea_lf; }
return false;
case sync2:
state = ch == 0x62 ? mclass : sync1;
return false;
case mclass:
frame.classId = ch;
initCksum();
addCksum(ch);
state = mid;
return false;
case mid:
frame.classId |= ch << 8;
addCksum(ch);
state = len1;
return false;
case len1:
frame.payLen = ch;
addCksum(ch);
state = len2;
return false;
case len2:
frame.payLen += (uint16_t)ch << 8;
addCksum(ch);
paylen = 0;
state = frame.payLen == 0 ? ckA
: frame.payLen < MAX_PAYLOAD ? pay
: skip;
if (state == skip) {
logf("Frame %02x:%02x len=%d (skip)", frame.classId&0xf, frame.classId>>8, frame.payLen);
if (frame.payLen > MAX_PAYLOAD) state = sync1; // assume corruption/overrun
} // else printf("[%02x:%02x %d]", frame.classId&0xf, frame.classId>>8, frame.payLen);
return false;
case pay:
assert(paylen < MAX_PAYLOAD);
paybuf[paylen++] = ch;
addCksum(ch);
if (paylen == frame.payLen) state = ckA;
return false;
case skip:
paylen++;
if (paylen == frame.payLen+2) state = sync1; // skip checksum too
return false;
case ckA:
rxCkA = ch;
state = ckB;
return false;
case ckB:
state = sync1;
if (rxCkA == cksumA && ch == cksumB) {
frame.payload = paybuf;
return true;
}
logf("Bad checksum %02x:%02x (calc %02x%02x got %02x%02x) len=%d",
frame.classId&0xff, frame.classId>>8, cksumA, cksumB, rxCkA, ch, paylen);
// for (int i=0; i<paylen && i<100; i++) printf(" %02x", paybuf[i]);
// printf("\n");
return false;
case nmea_lf:
state = ch == '\n' ? nmea_dollar : sync1;
return false;
case nmea_dollar:
if (ch != '$') {
state = sync1;
} else {
state = nmea_cr;
paylen = 1;
paybuf[0] = ch;
}
return false;
case nmea_cr:
if (ch == '\r') {
state = nmea_lf;
frame.classId = NMEA;
frame.payLen = paylen;
frame.payload = paybuf;
return true;
} else if (paylen < MAX_PAYLOAD) {
paybuf[paylen++] = ch;
}
return false;
default:
fail();
}
}
// ===== Configuration messages
#define PACKED __attribute__((packed))
struct PACKED cfgL { uint32_t key; bool val; };
struct PACKED cfgU1 { uint32_t key; uint8_t val; };
struct PACKED cfgU2 { uint32_t key; uint16_t val; };
struct PACKED cfgU4 { uint32_t key; uint32_t val; };
struct PACKED cfgI1 { uint32_t key; int8_t val; };
struct PACKED cfgI2 { uint32_t key; int16_t val; };
struct PACKED cfgI4 { uint32_t key; int32_t val; };
constexpr cfgL CFG_UART1_OUT_UBX_ON{0x10740001, 1}; // enable UBX output
constexpr cfgL CFG_UART1_OUT_NMEA_OFF{0x10740002, 0}; // disable NMEA output
constexpr cfgU4 CFG_UART1_BAUDRATE_38400{0x40520001, 38400};
constexpr cfgL CFG_SIGNAL_GAL_ENA_OFF{0x10310021, 0}; // disable Galileo
constexpr cfgL CFG_SIGNAL_GAL_ENA_ON{0x10310021, 1}; // disable Galileo
constexpr cfgL CFG_SIGNAL_BDS_ENA_OFF{0x10310022, 0}; // disable BeiDou
constexpr cfgL CFG_SIGNAL_GLO_ENA_OFF{0x10310025, 0}; // disable Glonass
constexpr cfgL CFG_SIGNAL_GLO_ENA_ON{0x10310025, 1}; // disable Glonass
constexpr cfgU1 CFG_PM_OPERATEMODE_FULL{0x20d00001, 0}; // full power mode
constexpr cfgU1 CFG_PM_OPERATEMODE_OO{0x20d00001, 1}; // on/off mode
constexpr cfgU1 CFG_PM_OPERATEMODE_CT{0x20d00001, 2}; // cyclic tracking mode
constexpr cfgU4 CFG_PM_ACQPERIOD_0{0x40d00003, 0}; // acquisition retry, 0=never retry
constexpr cfgU4 CFG_PM_ACQPERIOD_40s{0x40d00003, 40}; // acquisition retry in s
constexpr cfgU4 CFG_PM_ACQPERIOD_120s{0x40d00003, 120}; // acquisition retry in s
constexpr cfgU4 CFG_PM_ACQPERIOD_1805s{0x40d00003, 1805}; // acquisition retry in s
constexpr cfgU4 CFG_PM_ACQPERIOD_4500s{0x40d00003, 4500}; // acquisition retry in s
constexpr cfgU2 CFG_PM_ONTIME_0s{0x30d00005, 0}; // tracking time in s
constexpr cfgU2 CFG_PM_ONTIME_2s{0x30d00005, 2}; // tracking time in s
constexpr cfgU2 CFG_PM_ONTIME_10s{0x30d00005, 10}; // tracking time in s
constexpr cfgU1 CFG_PM_MAXACQTIME_0{0x20d00007, 0}; // max time in acquisition, 0=auto
constexpr cfgU1 CFG_PM_MAXACQTIME_30s{0x20d00007, 30}; // max time in acquisition in s
constexpr cfgU1 CFG_PM_MAXACQTIME_60s{0x20d00007, 60}; // max time in acquisition in s
constexpr cfgU1 CFG_PM_MAXACQTIME_120s{0x20d00007, 120}; // max time in acquisition in s
constexpr cfgU1 CFG_PM_UPDATEEPH_ON{0x10d0000a, 1}; // update ephemeris
constexpr cfgU1 CFG_PM_UPDATEEPH_OFF{0x10d0000a, 0}; // update ephemeris
constexpr cfgU4 CFG_PM_POSUPDATEPERIOD_20s{0x40d00002, 20}; // update period in OO mode
constexpr cfgU4 CFG_PM_POSUPDATEPERIOD_60s{0x40d00002, 60}; // update period in OO mode
constexpr cfgU4 CFG_PM_POSUPDATEPERIOD_300s{0x40d00002, 300}; // update period in OO mode
constexpr cfgU4 CFG_PM_POSUPDATEPERIOD_1805s{0x40d00002, 1805}; // update period in OO mode
constexpr cfgU4 CFG_PM_POSUPDATEPERIOD_3000s{0x40d00002, 3000}; // update period in OO mode
constexpr cfgU4 CFG_PM_POSUPDATEPERIOD_4500s{0x40d00002, 4500}; // update period in OO mode
constexpr cfgL CFG_PM_DONOTENTEROFF_ENA{0x10d00008, 1}; // do not turn off until fix
constexpr cfgL CFG_PM_DONOTENTEROFF_DIS{0x10d00008, 0}; // turn off even if no fix
constexpr cfgU2 CFG_RATE_MEAS_1s{0x30210001, 1000}; // time between measurements in ms
constexpr cfgU2 CFG_RATE_MEAS_4s{0x30210001, 4000}; // time between measurements in ms
constexpr cfgU2 CFG_RATE_MEAS_12s{0x30210001, 12000}; // time between measurements in ms
constexpr cfgU2 CFG_RATE_SOLN_1{0x30210002, 1}; // measurements per solution
constexpr cfgU2 CFG_RATE_SOLN_10{0x30210002, 10}; // measurements per solution
constexpr cfgU2 CFG_RATE_SOLN_127{0x30210002, 127}; // measurements per solution
constexpr cfgU1 CFG_NAV_MON_PVT_1s{0x20910007, 1}; // per second output rate of UBX-NAV-PVT (position, velocity, time)
constexpr cfgU1 CFG_NAV_MON_PVT_0{0x20910007, 0}; // per second output rate of UBX-NAV-PVT (position, velocity, time)
constexpr cfgU1 CFG_MON_RXR_1s{0x20910188, 1}; // receiver state output
constexpr cfgU1 CFG_NAV_ORB_1{0x20910011, 1}; // satellites known
constexpr cfgU1 CFG_NAV_ORB_10{0x20910011, 10}; // satellites known
constexpr cfgU1 CFG_NAV_SAT_1{0x20910016, 1}; // satellites theoretically visible
constexpr cfgU1 CFG_NAV_SAT_2{0x20910016, 2}; // satellites theoretically visible
constexpr cfgU1 CFG_NAV_SAT_4{0x20910016, 4}; // satellites theoretically visible
constexpr cfgU1 CFG_NAV_SAT_5{0x20910016, 5}; // satellites theoretically visible
constexpr cfgU1 CFG_NAV_UTC_10{0x2091005C, 10}; // UTC time
constexpr cfgU1 CFG_RXM_MEAS20_ENA{0x20910644, 1}; // cloud locate MEAS20 every second
constexpr cfgU1 CFG_RXM_MEAS20_DIS{0x20910644, 0}; // cloud locate disabled
constexpr cfgU1 CFG_RXM_MEAS50_ENA{0x20910649, 1}; // cloud locate MEAS50 every second
constexpr cfgU1 CFG_RXM_MEAS50_DIS{0x20910649, 0}; // cloud locate disabled
constexpr cfgU1 CFG_RXM_MEASX_ENA{0x20910205, 1}; // cloud locate MEASX enabled
constexpr cfgU1 CFG_RXM_MEASX_DIS{0x20910205, 0}; // cloud locate MEASX disabled
constexpr cfgU1 CFG_INFMSG_UBX_UART_DBG{0x20920002, 0x17}; // enable all but test messages
constexpr cfgU1 CFG_INFMSG_UBX_UART_WRN{0x20920002, 0x03}; // only warn & err messages
constexpr cfgU2 CFG_NAVSPG_POS_ACC_200m{0x301100b3, 200}; // valid fix position accuracy threshold
constexpr cfgL CFG_ANA_ENA{0x10230001, 1}; // AssistNow Autonomous
constexpr cfgL CFG_ANA_DIS{0x10230001, 0}; // AssistNow Autonomous
enum { L_RAM=1, L_BBR=2, L_FLASH=4 }; // config storage layers
constexpr char UbxInfo[5][5] = { "ERR", "WARN", "NOTE", "TEST", "DEBG"};
// ===== TX
struct Header {
uint8_t s1, s2; // 0xb5 0x62
uint16_t classId;
uint16_t payLen; // little endian
};
struct Cksum { uint8_t ckA, ckB; };
struct Poll { Header h; Cksum c; }; // message with no payload
constexpr Header MakeHeader(uint16_t clsId, uint16_t len=0) {
return Header{0xb5, 0x62, clsId, len};
}
constexpr Cksum MakeCksum(Header hdr) {
uint8_t ckA = hdr.classId & 0xff, ckB = ckA;
ckA += hdr.classId >> 8; ckB += ckA;
ckA += hdr.payLen & 0xff; ckB += ckA;
ckA += hdr.payLen >> 8; ckB += ckA;
return Cksum{ckA, ckB};
}
void CalcCksum(uint8_t *buf, uint16_t len) {
uint8_t ckA = 0, ckB = 0;
for (uint16_t i=0; i<len; i++) {
ckA += buf[i];
ckB += ckA;
}
buf[len+0] = ckA;
buf[len+1] = ckB;
}
constexpr Poll PollUbxMonGNSS() {
auto hdr = MakeHeader(UBX_MON_GNSS);
return Poll{hdr, MakeCksum(hdr)};
}
constexpr Poll PollUbxNavOrb() {
auto hdr = MakeHeader(UBX_NAV_ORB);
return Poll{hdr, MakeCksum(hdr)};
}
constexpr Poll PollUbxNavUTC() {
auto hdr = MakeHeader(UBX_NAV_TIMEUTC);
return Poll{hdr, MakeCksum(hdr)};
}
// constexpr auto CfgNavMonPvt(uint8_t rate) {
// (void) rate;
// const uint16_t paylen = 4 + sizeof(CFG_NAV_MON_PVT_1s) + sizeof(CFG_RATE_MEAS_4s);
// struct PACKED {
// Header hdr;
// uint8_t version; uint8_t layers; uint16_t resv;
// cfgU1 c1; cfgU2 c2;
// Cksum ck;
// } msg{MakeHeader(UBX_CFG_VALSET, paylen), 0, L_RAM, 0, CFG_NAV_MON_PVT, CFG_RATE_MEAS, {0,0}};
// return msg;
// }
// constexpr auto CfgUartOutProt() {
// const uint16_t paylen = 4 + sizeof(CFG_UART1_OUT_UBX_ON) + sizeof(CFG_UART1_OUT_NMEA_OFF);
// struct PACKED {
// Header hdr;
// uint8_t version; uint8_t layers; uint16_t resv;
// cfgL c1, c2;
// Cksum ck;
// } msg{MakeHeader(UBX_CFG_VALSET, paylen), 0, L_RAM, 0, CFG_UART1_OUT_UBX_ON, CFG_UART1_OUT_NMEA_OFF, {0,0}};
// return msg;
// }
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment