PlatformIO-Libs/SparkFunGNSS
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Add auto support UBX-NAV-AOPSTATUS (AssistNow Autonomous)

Browse Source
This commit is contained in:
PaulZC
2021-11-29 18:40:12 +00:00
parent 611e086864
commit 64f830f418
4 changed files with 336 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/SparkFun_u-blox_GNSS_Arduino_Library.cpp
+260 -2
View File
@@ -244,6 +244,16 @@ void SFE_UBLOX_GNSS::end(void)
packetUBXNAVRELPOSNED = NULL; // Redundant?
}
if (packetUBXNAVAOPSTATUS != NULL)
{
if (packetUBXNAVAOPSTATUS->callbackData != NULL)
{
delete packetUBXNAVAOPSTATUS->callbackData;
}
delete packetUBXNAVAOPSTATUS;
packetUBXNAVAOPSTATUS = NULL; // Redundant?
}
if (packetUBXRXMSFRBX != NULL)
{
if (packetUBXRXMSFRBX->callbackData != NULL)
@@ -1023,6 +1033,9 @@ bool SFE_UBLOX_GNSS::checkAutomatic(uint8_t Class, uint8_t ID)
case UBX_NAV_RELPOSNED:
if (packetUBXNAVRELPOSNED != NULL) result = true;
break;
case UBX_NAV_AOPSTATUS:
if (packetUBXNAVAOPSTATUS != NULL) result = true;
break;
}
}
break;
@@ -1158,6 +1171,9 @@ uint16_t SFE_UBLOX_GNSS::getMaxPayloadSize(uint8_t Class, uint8_t ID)
case UBX_NAV_RELPOSNED:
maxSize = UBX_NAV_RELPOSNED_LEN_F9;
break;
case UBX_NAV_AOPSTATUS:
maxSize = UBX_NAV_AOPSTATUS_LEN;
break;
}
}
break;
@@ -2417,6 +2433,33 @@ void SFE_UBLOX_GNSS::processUBXpacket(ubxPacket *msg)
}
}
}
else if (msg->id == UBX_NAV_AOPSTATUS && msg->len == UBX_NAV_AOPSTATUS_LEN)
{
//Parse various byte fields into storage - but only if we have memory allocated for it
if (packetUBXNAVAOPSTATUS != NULL)
{
packetUBXNAVAOPSTATUS->data.iTOW = extractLong(msg, 0);
packetUBXNAVAOPSTATUS->data.aopCfg.all = extractByte(msg, 4);
packetUBXNAVAOPSTATUS->data.status = extractByte(msg, 5);
//Mark all datums as fresh (not read before)
packetUBXNAVAOPSTATUS->moduleQueried.moduleQueried.all = 0xFFFFFFFF;
//Check if we need to copy the data for the callback
if ((packetUBXNAVAOPSTATUS->callbackData != NULL) // If RAM has been allocated for the copy of the data
&& (packetUBXNAVAOPSTATUS->automaticFlags.flags.bits.callbackCopyValid == false)) // AND the data is stale
{
memcpy(&packetUBXNAVAOPSTATUS->callbackData->iTOW, &packetUBXNAVAOPSTATUS->data.iTOW, sizeof(UBX_NAV_AOPSTATUS_data_t));
packetUBXNAVAOPSTATUS->automaticFlags.flags.bits.callbackCopyValid = true;
}
//Check if we need to copy the data into the file buffer
if (packetUBXNAVAOPSTATUS->automaticFlags.flags.bits.addToFileBuffer)
{
storePacket(msg);
}
}
}
break;
case UBX_CLASS_RXM:
if (msg->id == UBX_RXM_SFRBX)
@@ -3756,6 +3799,17 @@ void SFE_UBLOX_GNSS::checkCallbacks(void)
packetUBXNAVRELPOSNED->automaticFlags.flags.bits.callbackCopyValid = false; // Mark the data as stale
}
if ((packetUBXNAVAOPSTATUS != NULL) // If RAM has been allocated for message storage
&& (packetUBXNAVAOPSTATUS->callbackData != NULL) // If RAM has been allocated for the copy of the data
&& (packetUBXNAVAOPSTATUS->callbackPointer != NULL) // If the pointer to the callback has been defined
&& (packetUBXNAVAOPSTATUS->automaticFlags.flags.bits.callbackCopyValid == true)) // If the copy of the data is valid
{
// if (_printDebug == true)
// _debugSerial->println(F("checkCallbacks: calling callback for NAV AOPSTATUS"));
packetUBXNAVAOPSTATUS->callbackPointer(*packetUBXNAVAOPSTATUS->callbackData); // Call the callback
packetUBXNAVAOPSTATUS->automaticFlags.flags.bits.callbackCopyValid = false; // Mark the data as stale
}
if ((packetUBXRXMSFRBX != NULL) // If RAM has been allocated for message storage
&& (packetUBXRXMSFRBX->callbackData != NULL) // If RAM has been allocated for the copy of the data
&& (packetUBXRXMSFRBX->callbackPointer != NULL) // If the pointer to the callback has been defined
@@ -7130,11 +7184,11 @@ bool SFE_UBLOX_GNSS::initPacketUBXNAVATT()
return (true);
}
//Mark all the DOP data as read/stale. This is handy to get data alignment after CRC failure
//Mark all the ATT data as read/stale. This is handy to get data alignment after CRC failure
void SFE_UBLOX_GNSS::flushNAVATT()
{
if (packetUBXNAVATT == NULL) return; // Bail if RAM has not been allocated (otherwise we could be writing anywhere!)
packetUBXNAVATT->moduleQueried.moduleQueried.all = 0; //Mark all DOPs as stale (read before)
packetUBXNAVATT->moduleQueried.moduleQueried.all = 0; //Mark all ATT data as stale (read before)
}
//Log this data in file buffer
@@ -8538,6 +8592,182 @@ void SFE_UBLOX_GNSS::logNAVRELPOSNED(bool enabled)
packetUBXNAVRELPOSNED->automaticFlags.flags.bits.addToFileBuffer = (uint8_t)enabled;
}
// ***** AOPSTATUS automatic support
bool SFE_UBLOX_GNSS::getAOPSTATUS(uint16_t maxWait)
{
if (packetUBXNAVAOPSTATUS == NULL) initPacketUBXNAVAOPSTATUS(); //Check that RAM has been allocated for the AOPSTATUS data
if (packetUBXNAVAOPSTATUS == NULL) //Bail if the RAM allocation failed
return (false);
if (packetUBXNAVAOPSTATUS->automaticFlags.flags.bits.automatic && packetUBXNAVAOPSTATUS->automaticFlags.flags.bits.implicitUpdate)
{
//The GPS is automatically reporting, we just check whether we got unread data
// if (_printDebug == true)
// {
// _debugSerial->println(F("getAOPSTATUS: Autoreporting"));
// }
checkUbloxInternal(&packetCfg, UBX_CLASS_NAV, UBX_NAV_AOPSTATUS);
return packetUBXNAVAOPSTATUS->moduleQueried.moduleQueried.bits.all;
}
else if (packetUBXNAVAOPSTATUS->automaticFlags.flags.bits.automatic && !packetUBXNAVAOPSTATUS->automaticFlags.flags.bits.implicitUpdate)
{
//Someone else has to call checkUblox for us...
// if (_printDebug == true)
// {
// _debugSerial->println(F("getAOPSTATUS: Exit immediately"));
// }
return (false);
}
else
{
// if (_printDebug == true)
// {
// _debugSerial->println(F("getAOPSTATUS: Polling"));
// }
//The GPS is not automatically reporting navigation position so we have to poll explicitly
packetCfg.cls = UBX_CLASS_NAV;
packetCfg.id = UBX_NAV_AOPSTATUS;
packetCfg.len = 0;
packetCfg.startingSpot = 0;
//The data is parsed as part of processing the response
sfe_ublox_status_e retVal = sendCommand(&packetCfg, maxWait);
if (retVal == SFE_UBLOX_STATUS_DATA_RECEIVED)
return (true);
if (retVal == SFE_UBLOX_STATUS_DATA_OVERWRITTEN)
{
// if (_printDebug == true)
// {
// _debugSerial->println(F("getAOPSTATUS: data in packetCfg was OVERWRITTEN by another message (but that's OK)"));
// }
return (true);
}
// if (_printDebug == true)
// {
// _debugSerial->print(F("getAOPSTATUS retVal: "));
// _debugSerial->println(statusString(retVal));
// }
return (false);
}
}
//Enable or disable automatic navigation message generation by the GNSS. This changes the way getAOPSTATUS
//works.
bool SFE_UBLOX_GNSS::setAutoAOPSTATUS(bool enable, uint16_t maxWait)
{
return setAutoAOPSTATUSrate(enable ? 1 : 0, true, maxWait);
}
//Enable or disable automatic navigation message generation by the GNSS. This changes the way getAOPSTATUS
//works.
bool SFE_UBLOX_GNSS::setAutoAOPSTATUS(bool enable, bool implicitUpdate, uint16_t maxWait)
{
return setAutoAOPSTATUSrate(enable ? 1 : 0, implicitUpdate, maxWait);
}
//Enable or disable automatic navigation message generation by the GNSS. This changes the way getAOPSTATUS
//works.
bool SFE_UBLOX_GNSS::setAutoAOPSTATUSrate(uint8_t rate, bool implicitUpdate, uint16_t maxWait)
{
if (packetUBXNAVAOPSTATUS == NULL) initPacketUBXNAVAOPSTATUS(); //Check that RAM has been allocated for the data
if (packetUBXNAVAOPSTATUS == NULL) //Only attempt this if RAM allocation was successful
return false;
packetCfg.cls = UBX_CLASS_CFG;
packetCfg.id = UBX_CFG_MSG;
packetCfg.len = 3;
packetCfg.startingSpot = 0;
payloadCfg[0] = UBX_CLASS_NAV;
payloadCfg[1] = UBX_NAV_AOPSTATUS;
payloadCfg[2] = rate; // rate relative to navigation freq.
bool ok = ((sendCommand(&packetCfg, maxWait)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
if (ok)
{
packetUBXNAVAOPSTATUS->automaticFlags.flags.bits.automatic = (rate > 0);
packetUBXNAVAOPSTATUS->automaticFlags.flags.bits.implicitUpdate = implicitUpdate;
}
packetUBXNAVAOPSTATUS->moduleQueried.moduleQueried.bits.all = false;
return ok;
}
//Enable automatic navigation message generation by the GNSS.
bool SFE_UBLOX_GNSS::setAutoAOPSTATUScallback(void (*callbackPointer)(UBX_NAV_AOPSTATUS_data_t), uint16_t maxWait)
{
// Enable auto messages. Set implicitUpdate to false as we expect the user to call checkUblox manually.
bool result = setAutoAOPSTATUS(true, false, maxWait);
if (!result)
return (result); // Bail if setAuto failed
if (packetUBXNAVAOPSTATUS->callbackData == NULL) //Check if RAM has been allocated for the callback copy
{
packetUBXNAVAOPSTATUS->callbackData = new UBX_NAV_AOPSTATUS_data_t; //Allocate RAM for the main struct
}
if (packetUBXNAVAOPSTATUS->callbackData == NULL)
{
if ((_printDebug == true) || (_printLimitedDebug == true)) // This is important. Print this if doing limited debugging
_debugSerial->println(F("setAutoAOPSTATUScallback: RAM alloc failed!"));
return (false);
}
packetUBXNAVAOPSTATUS->callbackPointer = callbackPointer;
return (true);
}
//In case no config access to the GNSS is possible and AOPSTATUS is send cyclically already
//set config to suitable parameters
bool SFE_UBLOX_GNSS::assumeAutoAOPSTATUS(bool enabled, bool implicitUpdate)
{
if (packetUBXNAVAOPSTATUS == NULL) initPacketUBXNAVAOPSTATUS(); //Check that RAM has been allocated for the data
if (packetUBXNAVAOPSTATUS == NULL) //Only attempt this if RAM allocation was successful
return false;
bool changes = packetUBXNAVAOPSTATUS->automaticFlags.flags.bits.automatic != enabled || packetUBXNAVAOPSTATUS->automaticFlags.flags.bits.implicitUpdate != implicitUpdate;
if (changes)
{
packetUBXNAVAOPSTATUS->automaticFlags.flags.bits.automatic = enabled;
packetUBXNAVAOPSTATUS->automaticFlags.flags.bits.implicitUpdate = implicitUpdate;
}
return changes;
}
// PRIVATE: Allocate RAM for packetUBXNAVAOPSTATUS and initialize it
bool SFE_UBLOX_GNSS::initPacketUBXNAVAOPSTATUS()
{
packetUBXNAVAOPSTATUS = new UBX_NAV_AOPSTATUS_t; //Allocate RAM for the main struct
if (packetUBXNAVAOPSTATUS == NULL)
{
if ((_printDebug == true) || (_printLimitedDebug == true)) // This is important. Print this if doing limited debugging
_debugSerial->println(F("initPacketUBXNAVAOPSTATUS: RAM alloc failed!"));
return (false);
}
packetUBXNAVAOPSTATUS->automaticFlags.flags.all = 0;
packetUBXNAVAOPSTATUS->callbackPointer = NULL;
packetUBXNAVAOPSTATUS->callbackData = NULL;
packetUBXNAVAOPSTATUS->moduleQueried.moduleQueried.all = 0;
return (true);
}
//Mark all the AOPSTATUS data as read/stale. This is handy to get data alignment after CRC failure
void SFE_UBLOX_GNSS::flushAOPSTATUS()
{
if (packetUBXNAVAOPSTATUS == NULL) return; // Bail if RAM has not been allocated (otherwise we could be writing anywhere!)
packetUBXNAVAOPSTATUS->moduleQueried.moduleQueried.all = 0; //Mark all AOPSTATUSs as stale (read before)
}
//Log this data in file buffer
void SFE_UBLOX_GNSS::logNAVAOPSTATUS(bool enabled)
{
if (packetUBXNAVAOPSTATUS == NULL) return; // Bail if RAM has not been allocated (otherwise we could be writing anywhere!)
packetUBXNAVAOPSTATUS->automaticFlags.flags.bits.addToFileBuffer = (uint8_t)enabled;
}
// ***** RXM SFRBX automatic support
bool SFE_UBLOX_GNSS::getRXMSFRBX(uint16_t maxWait)
@@ -11804,6 +12034,34 @@ float SFE_UBLOX_GNSS::getRelPosAccD(uint16_t maxWait) // Returned as m
return (((float)packetUBXNAVRELPOSNED->data.accD) / 10000.0); // Convert to m
}
// ***** AOPSTATUS Helper Functions
uint8_t SFE_UBLOX_GNSS::getAOPSTATUSuseAOP(uint16_t maxWait)
{
if (packetUBXNAVAOPSTATUS == NULL) initPacketUBXNAVAOPSTATUS(); //Check that RAM has been allocated for the AOPSTATUS data
if (packetUBXNAVAOPSTATUS == NULL) //Bail if the RAM allocation failed
return 0;
if (packetUBXNAVAOPSTATUS->moduleQueried.moduleQueried.bits.useAOP == false)
getAOPSTATUS(maxWait);
packetUBXNAVAOPSTATUS->moduleQueried.moduleQueried.bits.useAOP = false; //Since we are about to give this to user, mark this data as stale
packetUBXNAVAOPSTATUS->moduleQueried.moduleQueried.bits.all = false;
return (packetUBXNAVAOPSTATUS->data.aopCfg.bits.useAOP);
}
uint8_t SFE_UBLOX_GNSS::getAOPSTATUSstatus(uint16_t maxWait)
{
if (packetUBXNAVAOPSTATUS == NULL) initPacketUBXNAVAOPSTATUS(); //Check that RAM has been allocated for the AOPSTATUS data
if (packetUBXNAVAOPSTATUS == NULL) //Bail if the RAM allocation failed
return 0;
if (packetUBXNAVAOPSTATUS->moduleQueried.moduleQueried.bits.status == false)
getAOPSTATUS(maxWait);
packetUBXNAVAOPSTATUS->moduleQueried.moduleQueried.bits.status = false; //Since we are about to give this to user, mark this data as stale
packetUBXNAVAOPSTATUS->moduleQueried.moduleQueried.bits.all = false;
return (packetUBXNAVAOPSTATUS->data.status);
}
// ***** ESF Helper Functions
float SFE_UBLOX_GNSS::getESFroll(uint16_t maxWait) // Returned as degrees