Skip to content

Commit 9e5f77b

Browse files
PaulZCPaulZC
committed
Add Auto support for TIM TP
1 parent 466348c commit 9e5f77b

File tree

5 files changed

+423
-15
lines changed

5 files changed

+423
-15
lines changed

‎keywords.txt‎

Lines changed: 16 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -60,6 +60,7 @@ UBX_RXM_MEASX_data_t KEYWORD1
6060
UBX_RXM_QZSSL6_message_data_t KEYWORD1
6161

6262
UBX_TIM_TM2_data_t KEYWORD1
63+
UBX_TIM_TP_data_t KEYWORD1
6364

6465
UBX_ESF_ALG_data_t KEYWORD1
6566
UBX_ESF_INS_data_t KEYWORD1
@@ -491,6 +492,15 @@ assumeAutoTIMTM2 KEYWORD2
491492
flushTIMTM2 KEYWORD2
492493
logTIMTM2 KEYWORD2
493494

495+
getTIMTP KEYWORD2
496+
setAutoTIMTP KEYWORD2
497+
setAutoTIMTPrate KEYWORD2
498+
setAutoTIMTPcallback KEYWORD2
499+
setAutoTIMTPcallbackPtr KEYWORD2
500+
assumeAutoTIMTP KEYWORD2
501+
flushTIMTP KEYWORD2
502+
logTIMTP KEYWORD2
503+
494504
getEsfAlignment KEYWORD2
495505
getESFALG KEYWORD2
496506
setAutoESFALG KEYWORD2
@@ -666,6 +676,11 @@ getRelPosAccD KEYWORD2
666676
getAOPSTATUSuseAOP KEYWORD2
667677
getAOPSTATUSstatus KEYWORD2
668678

679+
getTIMTPtowMS KEYWORD2
680+
getTIMTPtowSubMS KEYWORD2
681+
getTIMTPweek KEYWORD2
682+
getTIMTPAsEpoch KEYWORD2
683+
669684
getESFroll KEYWORD2
670685
getESFpitch KEYWORD2
671686
getESFyaw KEYWORD2
@@ -904,6 +919,7 @@ UBX_RXM_SPARTN LITERAL1
904919
UBX_RXM_QZSSL6 LITERAL1
905920

906921
UBX_TIM_TM2 LITERAL1
922+
UBX_TIM_TP LITERAL1
907923

908924
UBX_RTCM_MSB LITERAL1
909925
UBX_RTCM_1005 LITERAL1

‎src/u-blox_GNSS.cpp‎

Lines changed: 301 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -399,6 +399,16 @@ void DevUBLOXGNSS::end(void)
399399
packetUBXTIMTM2 = nullptr;
400400
}
401401

402+
if (packetUBXTIMTP != nullptr)
403+
{
404+
if (packetUBXTIMTP->callbackData != nullptr)
405+
{
406+
delete packetUBXTIMTP->callbackData;
407+
}
408+
delete packetUBXTIMTP;
409+
packetUBXTIMTP = nullptr;
410+
}
411+
402412
#ifndef SFE_UBLOX_DISABLE_ESF
403413
if (packetUBXESFALG != nullptr)
404414
{
@@ -1397,6 +1407,12 @@ bool DevUBLOXGNSS::autoLookup(uint8_t Class, uint8_t ID, uint16_t *maxSize)
13971407
*maxSize = UBX_TIM_TM2_LEN;
13981408
return (packetUBXTIMTM2 != nullptr);
13991409
}
1410+
else if (ID == UBX_TIM_TP)
1411+
{
1412+
if (maxSize != nullptr)
1413+
*maxSize = UBX_TIM_TP_LEN;
1414+
return (packetUBXTIMTP != nullptr);
1415+
}
14001416
break;
14011417
case UBX_CLASS_ESF:
14021418
#ifndef SFE_UBLOX_DISABLE_ESF
@@ -4158,6 +4174,36 @@ void DevUBLOXGNSS::processUBXpacket(ubxPacket *msg)
41584174
}
41594175
}
41604176
}
4177+
else if (msg->id == UBX_TIM_TP && msg->len == UBX_TIM_TP_LEN)
4178+
{
4179+
// Parse various byte fields into storage - but only if we have memory allocated for it
4180+
if (packetUBXTIMTP != nullptr)
4181+
{
4182+
packetUBXTIMTP->data.towMS = extractLong(msg, 0);
4183+
packetUBXTIMTP->data.towSubMS = extractLong(msg, 4);
4184+
packetUBXTIMTP->data.qErr = extractSignedLong(msg, 8);
4185+
packetUBXTIMTP->data.week = extractInt(msg, 12);
4186+
packetUBXTIMTP->data.flags.all = extractByte(msg, 14);
4187+
packetUBXTIMTP->data.refInfo.all = extractByte(msg, 15);
4188+
4189+
// Mark all datums as fresh (not read before)
4190+
packetUBXTIMTP->moduleQueried.moduleQueried.all = 0xFFFFFFFF;
4191+
4192+
// Check if we need to copy the data for the callback
4193+
if ((packetUBXTIMTP->callbackData != nullptr) // If RAM has been allocated for the copy of the data
4194+
&& (packetUBXTIMTP->automaticFlags.flags.bits.callbackCopyValid == false)) // AND the data is stale
4195+
{
4196+
memcpy(&packetUBXTIMTP->callbackData->towMS, &packetUBXTIMTP->data.towMS, sizeof(UBX_TIM_TP_data_t));
4197+
packetUBXTIMTP->automaticFlags.flags.bits.callbackCopyValid = true;
4198+
}
4199+
4200+
// Check if we need to copy the data into the file buffer
4201+
if (packetUBXTIMTP->automaticFlags.flags.bits.addToFileBuffer)
4202+
{
4203+
addedToFileBuffer = storePacket(msg);
4204+
}
4205+
}
4206+
}
41614207
break;
41624208
#ifndef SFE_UBLOX_DISABLE_ESF
41634209
case UBX_CLASS_ESF:
@@ -5642,6 +5688,17 @@ void DevUBLOXGNSS::checkCallbacks(void)
56425688
packetUBXTIMTM2->automaticFlags.flags.bits.callbackCopyValid = false; // Mark the data as stale
56435689
}
56445690

5691+
if (packetUBXTIMTP != nullptr) // If RAM has been allocated for message storage
5692+
if (packetUBXTIMTP->callbackData != nullptr) // If RAM has been allocated for the copy of the data
5693+
if (packetUBXTIMTP->automaticFlags.flags.bits.callbackCopyValid == true) // If the copy of the data is valid
5694+
{
5695+
if (packetUBXTIMTP->callbackPointerPtr != nullptr) // If the pointer to the callback has been defined
5696+
{
5697+
packetUBXTIMTP->callbackPointerPtr(packetUBXTIMTP->callbackData); // Call the callback
5698+
}
5699+
packetUBXTIMTP->automaticFlags.flags.bits.callbackCopyValid = false; // Mark the data as stale
5700+
}
5701+
56455702
#ifndef SFE_UBLOX_DISABLE_ESF
56465703
if (packetUBXESFALG != nullptr) // If RAM has been allocated for message storage
56475704
if (packetUBXESFALG->callbackData != nullptr) // If RAM has been allocated for the copy of the data
@@ -13010,6 +13067,172 @@ void DevUBLOXGNSS::logTIMTM2(bool enabled)
1301013067
packetUBXTIMTM2->automaticFlags.flags.bits.addToFileBuffer = (uint8_t)enabled;
1301113068
}
1301213069

13070+
// ***** TIM TP automatic support
13071+
13072+
bool DevUBLOXGNSS::getTIMTP(uint16_t maxWait)
13073+
{
13074+
if (packetUBXTIMTP == nullptr)
13075+
initPacketUBXTIMTP(); // Check that RAM has been allocated for the TP data
13076+
if (packetUBXTIMTP == nullptr) // Bail if the RAM allocation failed
13077+
return (false);
13078+
13079+
if (packetUBXTIMTP->automaticFlags.flags.bits.automatic && packetUBXTIMTP->automaticFlags.flags.bits.implicitUpdate)
13080+
{
13081+
// The GPS is automatically reporting, we just check whether we got unread data
13082+
checkUbloxInternal(&packetCfg, 0, 0); // Call checkUbloxInternal to parse any incoming data. Don't overwrite the requested Class and ID
13083+
return packetUBXTIMTP->moduleQueried.moduleQueried.bits.all;
13084+
}
13085+
else if (packetUBXTIMTP->automaticFlags.flags.bits.automatic && !packetUBXTIMTP->automaticFlags.flags.bits.implicitUpdate)
13086+
{
13087+
// Someone else has to call checkUblox for us...
13088+
return (false);
13089+
}
13090+
else
13091+
{
13092+
// The GPS is not automatically reporting navigation position so we have to poll explicitly
13093+
packetCfg.cls = UBX_CLASS_TIM;
13094+
packetCfg.id = UBX_TIM_TP;
13095+
packetCfg.len = 0;
13096+
packetCfg.startingSpot = 0;
13097+
13098+
// The data is parsed as part of processing the response
13099+
sfe_ublox_status_e retVal = sendCommand(&packetCfg, maxWait);
13100+
13101+
if (retVal == SFE_UBLOX_STATUS_DATA_RECEIVED)
13102+
return (true);
13103+
13104+
if (retVal == SFE_UBLOX_STATUS_DATA_OVERWRITTEN)
13105+
{
13106+
return (true);
13107+
}
13108+
13109+
return (false);
13110+
}
13111+
}
13112+
13113+
// Enable or disable automatic message generation by the GNSS. This changes the way getTIMTP works.
13114+
bool DevUBLOXGNSS::setAutoTIMTP(bool enable, uint8_t layer, uint16_t maxWait)
13115+
{
13116+
return setAutoTIMTPrate(enable ? 1 : 0, true, layer, maxWait);
13117+
}
13118+
13119+
// Enable or disable automatic message generation by the GNSS. This changes the way getTIMTP works.
13120+
bool DevUBLOXGNSS::setAutoTIMTP(bool enable, bool implicitUpdate, uint8_t layer, uint16_t maxWait)
13121+
{
13122+
return setAutoTIMTPrate(enable ? 1 : 0, implicitUpdate, layer, maxWait);
13123+
}
13124+
13125+
// Enable or disable automatic message generation by the GNSS. This changes the way getTIMTP works.
13126+
bool DevUBLOXGNSS::setAutoTIMTPrate(uint8_t rate, bool implicitUpdate, uint8_t layer, uint16_t maxWait)
13127+
{
13128+
if (packetUBXTIMTP == nullptr)
13129+
initPacketUBXTIMTP(); // Check that RAM has been allocated for the data
13130+
if (packetUBXTIMTP == nullptr) // Only attempt this if RAM allocation was successful
13131+
return false;
13132+
13133+
if (rate > 127)
13134+
rate = 127;
13135+
13136+
uint32_t key = UBLOX_CFG_MSGOUT_UBX_TIM_TP_I2C;
13137+
if (_commType == COMM_TYPE_SPI)
13138+
key = UBLOX_CFG_MSGOUT_UBX_TIM_TP_SPI;
13139+
else if (_commType == COMM_TYPE_SERIAL)
13140+
{
13141+
if (!_UART2)
13142+
key = UBLOX_CFG_MSGOUT_UBX_TIM_TP_UART1;
13143+
else
13144+
key = UBLOX_CFG_MSGOUT_UBX_TIM_TP_UART2;
13145+
}
13146+
13147+
bool ok = setVal8(key, rate, layer, maxWait);
13148+
if (ok)
13149+
{
13150+
packetUBXTIMTP->automaticFlags.flags.bits.automatic = (rate > 0);
13151+
packetUBXTIMTP->automaticFlags.flags.bits.implicitUpdate = implicitUpdate;
13152+
}
13153+
packetUBXTIMTP->moduleQueried.moduleQueried.bits.all = false;
13154+
return ok;
13155+
}
13156+
13157+
// Enable automatic message generation by the GNSS.
13158+
bool DevUBLOXGNSS::setAutoTIMTPcallbackPtr(void (*callbackPointerPtr)(UBX_TIM_TP_data_t *), uint8_t layer, uint16_t maxWait)
13159+
{
13160+
// Enable auto messages. Set implicitUpdate to false as we expect the user to call checkUblox manually.
13161+
bool result = setAutoTIMTP(true, false, layer, maxWait);
13162+
if (!result)
13163+
return (result); // Bail if setAuto failed
13164+
13165+
if (packetUBXTIMTP->callbackData == nullptr) // Check if RAM has been allocated for the callback copy
13166+
{
13167+
packetUBXTIMTP->callbackData = new UBX_TIM_TP_data_t; // Allocate RAM for the main struct
13168+
}
13169+
13170+
if (packetUBXTIMTP->callbackData == nullptr)
13171+
{
13172+
#ifndef SFE_UBLOX_REDUCED_PROG_MEM
13173+
if ((_printDebug == true) || (_printLimitedDebug == true)) // This is important. Print this if doing limited debugging
13174+
_debugSerial.println(F("setAutoTIMTPcallbackPtr: RAM alloc failed!"));
13175+
#endif
13176+
return (false);
13177+
}
13178+
13179+
packetUBXTIMTP->callbackPointerPtr = callbackPointerPtr;
13180+
return (true);
13181+
}
13182+
13183+
// In case no config access to the GNSS is possible and TIM TP is send cyclically already
13184+
// set config to suitable parameters
13185+
bool DevUBLOXGNSS::assumeAutoTIMTP(bool enabled, bool implicitUpdate)
13186+
{
13187+
if (packetUBXTIMTP == nullptr)
13188+
initPacketUBXTIMTP(); // Check that RAM has been allocated for the data
13189+
if (packetUBXTIMTP == nullptr) // Only attempt this if RAM allocation was successful
13190+
return false;
13191+
13192+
bool changes = packetUBXTIMTP->automaticFlags.flags.bits.automatic != enabled || packetUBXTIMTP->automaticFlags.flags.bits.implicitUpdate != implicitUpdate;
13193+
if (changes)
13194+
{
13195+
packetUBXTIMTP->automaticFlags.flags.bits.automatic = enabled;
13196+
packetUBXTIMTP->automaticFlags.flags.bits.implicitUpdate = implicitUpdate;
13197+
}
13198+
return changes;
13199+
}
13200+
13201+
// PRIVATE: Allocate RAM for packetUBXTIMTP and initialize it
13202+
bool DevUBLOXGNSS::initPacketUBXTIMTP()
13203+
{
13204+
packetUBXTIMTP = new UBX_TIM_TP_t; // Allocate RAM for the main struct
13205+
if (packetUBXTIMTP == nullptr)
13206+
{
13207+
#ifndef SFE_UBLOX_REDUCED_PROG_MEM
13208+
if ((_printDebug == true) || (_printLimitedDebug == true)) // This is important. Print this if doing limited debugging
13209+
_debugSerial.println(F("initPacketUBXTIMTP: RAM alloc failed!"));
13210+
#endif
13211+
return (false);
13212+
}
13213+
packetUBXTIMTP->automaticFlags.flags.all = 0;
13214+
packetUBXTIMTP->callbackPointerPtr = nullptr;
13215+
packetUBXTIMTP->callbackData = nullptr;
13216+
packetUBXTIMTP->moduleQueried.moduleQueried.all = 0;
13217+
return (true);
13218+
}
13219+
13220+
// Mark all the data as read/stale
13221+
void DevUBLOXGNSS::flushTIMTP()
13222+
{
13223+
if (packetUBXTIMTP == nullptr)
13224+
return; // Bail if RAM has not been allocated (otherwise we could be writing anywhere!)
13225+
packetUBXTIMTP->moduleQueried.moduleQueried.all = 0; // Mark all datums as stale (read before)
13226+
}
13227+
13228+
// Log this data in file buffer
13229+
void DevUBLOXGNSS::logTIMTP(bool enabled)
13230+
{
13231+
if (packetUBXTIMTP == nullptr)
13232+
return; // Bail if RAM has not been allocated (otherwise we could be writing anywhere!)
13233+
packetUBXTIMTP->automaticFlags.flags.bits.addToFileBuffer = (uint8_t)enabled;
13234+
}
13235+
1301313236
#ifndef SFE_UBLOX_DISABLE_ESF
1301413237
// ***** ESF ALG automatic support
1301513238

@@ -16545,6 +16768,84 @@ uint8_t DevUBLOXGNSS::getAOPSTATUSstatus(uint16_t maxWait)
1654516768
return (packetUBXNAVAOPSTATUS->data.status);
1654616769
}
1654716770

16771+
// ***** TIM TP Helper Functions
16772+
16773+
uint32_t DevUBLOXGNSS::getTIMTPtowMS(uint16_t maxWait)
16774+
{
16775+
if (packetUBXTIMTP == nullptr)
16776+
initPacketUBXTIMTP(); // Check that RAM has been allocated for the TP data
16777+
if (packetUBXTIMTP == nullptr) // Bail if the RAM allocation failed
16778+
return 0;
16779+
16780+
if (packetUBXTIMTP->moduleQueried.moduleQueried.bits.towMS == false)
16781+
getTIMTP(maxWait);
16782+
packetUBXTIMTP->moduleQueried.moduleQueried.bits.towMS = false; // Since we are about to give this to user, mark this data as stale
16783+
packetUBXTIMTP->moduleQueried.moduleQueried.bits.all = false;
16784+
return (packetUBXTIMTP->data.towMS);
16785+
}
16786+
16787+
uint32_t DevUBLOXGNSS::getTIMTPtowSubMS(uint16_t maxWait)
16788+
{
16789+
if (packetUBXTIMTP == nullptr)
16790+
initPacketUBXTIMTP(); // Check that RAM has been allocated for the TP data
16791+
if (packetUBXTIMTP == nullptr) // Bail if the RAM allocation failed
16792+
return 0;
16793+
16794+
if (packetUBXTIMTP->moduleQueried.moduleQueried.bits.towSubMS == false)
16795+
getTIMTP(maxWait);
16796+
packetUBXTIMTP->moduleQueried.moduleQueried.bits.towSubMS = false; // Since we are about to give this to user, mark this data as stale
16797+
packetUBXTIMTP->moduleQueried.moduleQueried.bits.all = false;
16798+
return (packetUBXTIMTP->data.towSubMS);
16799+
}
16800+
16801+
uint16_t DevUBLOXGNSS::getTIMTPweek(uint16_t maxWait)
16802+
{
16803+
if (packetUBXTIMTP == nullptr)
16804+
initPacketUBXTIMTP(); // Check that RAM has been allocated for the TP data
16805+
if (packetUBXTIMTP == nullptr) // Bail if the RAM allocation failed
16806+
return 0;
16807+
16808+
if (packetUBXTIMTP->moduleQueried.moduleQueried.bits.week == false)
16809+
getTIMTP(maxWait);
16810+
packetUBXTIMTP->moduleQueried.moduleQueried.bits.week = false; // Since we are about to give this to user, mark this data as stale
16811+
packetUBXTIMTP->moduleQueried.moduleQueried.bits.all = false;
16812+
return (packetUBXTIMTP->data.week);
16813+
}
16814+
16815+
// Convert TIM TP to Unix epoch including microseconds
16816+
// CAUTION! Assumes the time base is UTC and the week number is GPS
16817+
uint32_t DevUBLOXGNSS::getTIMTPAsEpoch(uint32_t &microsecond, uint16_t maxWait)
16818+
{
16819+
if (packetUBXNAVPVT == nullptr)
16820+
initPacketUBXNAVPVT(); // Check that RAM has been allocated for the PVT data
16821+
if (packetUBXNAVPVT == nullptr) // Bail if the RAM allocation failed
16822+
return 0;
16823+
16824+
if (packetUBXTIMTP->moduleQueried.moduleQueried.bits.week == false)
16825+
getTIMTP(maxWait);
16826+
packetUBXTIMTP->moduleQueried.moduleQueried.bits.week = false; // Since we are about to give this to user, mark this data as stale
16827+
packetUBXTIMTP->moduleQueried.moduleQueried.bits.towMS = false;
16828+
packetUBXTIMTP->moduleQueried.moduleQueried.bits.towSubMS = false;
16829+
packetUBXTIMTP->moduleQueried.moduleQueried.bits.all = false;
16830+
16831+
uint32_t tow = packetUBXTIMTP->data.week - SFE_UBLOX_JAN_1ST_2020_WEEK; // Calculate the number of weeks since Jan 1st 2020
16832+
tow *= SFE_UBLOX_SECS_PER_WEEK; // Convert weeks to seconds
16833+
tow += SFE_UBLOX_EPOCH_WEEK_2086; // Add the TOW for Jan 1st 2020
16834+
tow += packetUBXTIMTP->data.towMS / 1000; // Add the TOW for the next TP
16835+
16836+
uint32_t us = packetUBXTIMTP->data.towMS % 1000; // Extract the milliseconds
16837+
us *= 1000; // Convert to microseconds
16838+
16839+
double subMS = packetUBXTIMTP->data.towSubMS; // Get towSubMS (ms * 2^-32)
16840+
subMS *= pow(2.0, -32.0); // Convert to milliseconds
16841+
subMS *= 1000; // Convert to microseconds
16842+
16843+
us += (int32_t)subMS; // Add subMS
16844+
16845+
microsecond = us;
16846+
return tow;
16847+
}
16848+
1654816849
#ifndef SFE_UBLOX_DISABLE_ESF
1654916850
// ***** ESF Helper Functions
1655016851

0 commit comments

Comments
 (0)