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

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
+12
View File
@@ -311,6 +311,15 @@ initPacketUBXNAVRELPOSNED KEYWORD2
flushNAVRELPOSNED KEYWORD2
logNAVRELPOSNED KEYWORD2
getNAVAOPSTATUS KEYWORD2
setAutoNAVAOPSTATUS KEYWORD2
setAutoNAVAOPSTATUSrate KEYWORD2
setAutoNAVAOPSTATUScallback KEYWORD2
assumeAutoNAVAOPSTATUS KEYWORD2
initPacketUBXNAVAOPSTATUS KEYWORD2
flushNAVAOPSTATUS KEYWORD2
logNAVAOPSTATUS KEYWORD2
getRXMSFRBX KEYWORD2
setAutoRXMSFRBX KEYWORD2
setAutoRXMSFRBXrate KEYWORD2
@@ -509,6 +518,9 @@ getRelPosAccN KEYWORD2
getRelPosAccE KEYWORD2
getRelPosAccD KEYWORD2
getAOPSTATUSuseAOP KEYWORD2
getAOPSTATUSstatus KEYWORD2
getESFroll KEYWORD2
getESFpitch KEYWORD2
getESFyaw KEYWORD2
+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
@@ -369,6 +369,7 @@ const uint8_t UBX_NAV_TIMELS = 0x26; //Leap second event information
const uint8_t UBX_NAV_TIMEUTC = 0x21; //UTC Time Solution
const uint8_t UBX_NAV_VELECEF = 0x11; //Velocity Solution in ECEF
const uint8_t UBX_NAV_VELNED = 0x12; //Velocity Solution in NED
const uint8_t UBX_NAV_AOPSTATUS = 0x60; //AssistNow Autonomous status
//Class: RXM
//The following are used to configure the RXM UBX messages (receiver manager messages). Descriptions from UBX messages overview (ZED_F9P Interface Description Document page 36)
@@ -1001,6 +1002,15 @@ public:
void flushNAVRELPOSNED(); //Mark all the data as read/stale
void logNAVRELPOSNED(bool enabled = true); // Log data to file buffer
bool getAOPSTATUS(uint16_t maxWait = defaultMaxWait); //Query module for latest AssistNow Autonomous status and load global vars:. If autoAOPSTATUS is disabled, performs an explicit poll and waits, if enabled does not block. Returns true if new AOPSTATUS is available.
bool setAutoAOPSTATUS(bool enabled, uint16_t maxWait = defaultMaxWait); //Enable/disable automatic AOPSTATUS reports at the navigation frequency
bool setAutoAOPSTATUS(bool enabled, bool implicitUpdate, uint16_t maxWait = defaultMaxWait); //Enable/disable automatic AOPSTATUS reports at the navigation frequency, with implicitUpdate == false accessing stale data will not issue parsing of data in the rxbuffer of your interface, instead you have to call checkUblox when you want to perform an update
bool setAutoAOPSTATUSrate(uint8_t rate, bool implicitUpdate = true, uint16_t maxWait = defaultMaxWait); //Set the rate for automatic AOPSTATUS reports
bool setAutoAOPSTATUScallback(void (*callbackPointer)(UBX_NAV_AOPSTATUS_data_t), uint16_t maxWait = defaultMaxWait); //Enable automatic AOPSTATUS reports at the navigation frequency. Data is accessed from the callback.
bool assumeAutoAOPSTATUS(bool enabled, bool implicitUpdate = true); //In case no config access to the GPS is possible and AOPSTATUS is send cyclically already
void flushAOPSTATUS(); //Mark all the AOPSTATUS data as read/stale
void logNAVAOPSTATUS(bool enabled = true); // Log data to file buffer
// Receiver Manager Messages (RXM)
bool getRXMSFRBX(uint16_t maxWait = defaultMaxWait); // RXM SFRBX
@@ -1244,6 +1254,11 @@ public:
float getRelPosAccE(uint16_t maxWait = defaultMaxWait); // Returned as m
float getRelPosAccD(uint16_t maxWait = defaultMaxWait); // Returned as m
// Helper functions for AOPSTATUS
uint8_t getAOPSTATUSuseAOP(uint16_t maxWait = defaultMaxWait); // Returns the UBX-NAV-AOPSTATUS useAOP flag. Don't confuse this with getAopCfg - which returns the aopCfg byte from UBX-CFG-NAVX5
uint8_t getAOPSTATUSstatus(uint16_t maxWait = defaultMaxWait); // Returns the UBX-NAV-AOPSTATUS status field. A host application can determine the optimal time to shut down the receiver by monitoring the status field for a steady 0.
// Helper functions for ESF
float getESFroll(uint16_t maxWait = defaultMaxWait); // Returned as degrees
@@ -1290,6 +1305,7 @@ public:
UBX_NAV_TIMELS_t *packetUBXNAVTIMELS = NULL; // Pointer to struct. RAM will be allocated for this if/when necessary
UBX_NAV_SVIN_t *packetUBXNAVSVIN = NULL; // Pointer to struct. RAM will be allocated for this if/when necessary
UBX_NAV_RELPOSNED_t *packetUBXNAVRELPOSNED = NULL; // Pointer to struct. RAM will be allocated for this if/when necessary
UBX_NAV_AOPSTATUS_t *packetUBXNAVAOPSTATUS = NULL; // Pointer to struct. RAM will be allocated for this if/when necessary
UBX_RXM_SFRBX_t *packetUBXRXMSFRBX = NULL; // Pointer to struct. RAM will be allocated for this if/when necessary
UBX_RXM_RAWX_t *packetUBXRXMRAWX = NULL; // Pointer to struct. RAM will be allocated for this if/when necessary
@@ -1371,6 +1387,7 @@ private:
bool initPacketUBXNAVTIMELS(); // Allocate RAM for packetUBXNAVTIMELS and initialize it
bool initPacketUBXNAVSVIN(); // Allocate RAM for packetUBXNAVSVIN and initialize it
bool initPacketUBXNAVRELPOSNED(); // Allocate RAM for packetUBXNAVRELPOSNED and initialize it
bool initPacketUBXNAVAOPSTATUS(); // Allocate RAM for packetUBXNAVAOPSTATUS and initialize it
bool initPacketUBXRXMSFRBX(); // Allocate RAM for packetUBXRXMSFRBX and initialize it
bool initPacketUBXRXMRAWX(); // Allocate RAM for packetUBXRXMRAWX and initialize it
bool initPacketUBXCFGRATE(); // Allocate RAM for packetUBXCFGRATE and initialize it
+47
View File
@@ -65,6 +65,8 @@ struct ubxAutomaticFlags
} flags;
};
// NAV-specific structs
// UBX-NAV-POSECEF (0x01 0x01): Position solution in ECEF
const uint16_t UBX_NAV_POSECEF_LEN = 20;
@@ -1071,6 +1073,51 @@ typedef struct
UBX_NAV_RELPOSNED_data_t *callbackData;
} UBX_NAV_RELPOSNED_t;
// UBX-NAV-AOPSTATUS (0x01 0x60): AssistNow Autonomous status
const uint16_t UBX_NAV_AOPSTATUS_LEN = 16;
typedef struct
{
uint32_t iTOW; // GPS time of week of the navigation epoch: ms
union
{
uint8_t all;
struct
{
uint8_t useAOP : 1; // AOP enabled flag
} bits;
} aopCfg; // AssistNow Autonomous configuration
uint8_t status; // AssistNow Autonomous subsystem is idle (0) or running (not 0)
uint8_t reserved1[10];
} UBX_NAV_AOPSTATUS_data_t;
typedef struct
{
union
{
uint32_t all;
struct
{
uint32_t all : 1;
uint32_t iTOW : 1;
uint32_t useAOP : 1;
uint32_t status : 1;
} bits;
} moduleQueried;
} UBX_NAV_AOPSTATUS_moduleQueried_t;
typedef struct
{
ubxAutomaticFlags automaticFlags;
UBX_NAV_AOPSTATUS_data_t data;
UBX_NAV_AOPSTATUS_moduleQueried_t moduleQueried;
void (*callbackPointer)(UBX_NAV_AOPSTATUS_data_t);
UBX_NAV_AOPSTATUS_data_t *callbackData;
} UBX_NAV_AOPSTATUS_t;
// RXM-specific structs
// UBX-RXM-SFRBX (0x02 0x13): Broadcast navigation data subframe