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

Correct support for ESF RAW

Browse Source
This commit is contained in:
PaulZC
2022-09-09 01:58:53 +01:00
parent e988f05aad
commit 1593dbc1a6
5 changed files with 181 additions and 123 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/SparkFun_u-blox_GNSS_Arduino_Library.cpp
+11 -101
View File
@@ -4246,15 +4246,13 @@ void SFE_UBLOX_GNSS::processUBXpacket(ubxPacket *msg)
// Parse various byte fields into storage - but only if we have memory allocated for it
if (packetUBXESFRAW != NULL)
{
for (uint16_t i = 0; (i < DEF_NUM_SENS) && ((i * 8) < (msg->len - 4)); i++)
packetUBXESFRAW->data.numEsfRawBlocks = (msg->len - 4) / 8; // Record how many blocks were received. Could be 7 or 70 (ZED-F9R vs. NEO-M8U)
for (uint16_t i = 0; (i < (DEF_NUM_SENS * DEF_MAX_NUM_ESF_RAW_REPEATS)) && ((i * 8) < (msg->len - 4)); i++)
{
packetUBXESFRAW->data.data[i].data.all = extractLong(msg, 4 + (i * 8));
packetUBXESFRAW->data.data[i].sTag = extractLong(msg, 8 + (i * 8));
}
// Mark all datums as fresh (not read before)
packetUBXESFRAW->moduleQueried.moduleQueried.all = 0xFFFFFFFF;
// Check if we need to copy the data for the callback
if ((packetUBXESFRAW->callbackData != NULL) // If RAM has been allocated for the copy of the data
&& (packetUBXESFRAW->automaticFlags.flags.bits.callbackCopyValid == false)) // AND the data is stale
@@ -10233,7 +10231,7 @@ bool SFE_UBLOX_GNSS::getVehAtt(uint16_t maxWait)
bool SFE_UBLOX_GNSS::getNAVATT(uint16_t maxWait)
{
if (packetUBXNAVATT == NULL)
initPacketUBXNAVATT(); // Check that RAM has been allocated for the ESF RAW data
initPacketUBXNAVATT(); // Check that RAM has been allocated for the NAV ATT data
if (packetUBXNAVATT == NULL) // Only attempt this if RAM allocation was successful
return false;
@@ -10373,7 +10371,7 @@ bool SFE_UBLOX_GNSS::setAutoNAVATTcallbackPtr(void (*callbackPointerPtr)(UBX_NAV
bool SFE_UBLOX_GNSS::assumeAutoNAVATT(bool enabled, bool implicitUpdate)
{
if (packetUBXNAVATT == NULL)
initPacketUBXNAVATT(); // Check that RAM has been allocated for the ESF RAW data
initPacketUBXNAVATT(); // Check that RAM has been allocated for the NAV ATT data
if (packetUBXNAVATT == NULL) // Only attempt this if RAM allocation was successful
return false;
@@ -14738,92 +14736,23 @@ void SFE_UBLOX_GNSS::logESFMEAS(bool enabled)
// ***** ESF RAW automatic support
bool SFE_UBLOX_GNSS::getEsfRawDataInfo(uint16_t maxWait)
{
return (getESFRAW(maxWait));
}
// ESF RAW messages are output only. They cannot be polled.
bool SFE_UBLOX_GNSS::getESFRAW(uint16_t maxWait)
{
if (packetUBXESFRAW == NULL)
initPacketUBXESFRAW(); // Check that RAM has been allocated for the ESF RAW data
if (packetUBXESFRAW == NULL) // Only attempt this if RAM allocation was successful
return false;
if (packetUBXESFRAW->automaticFlags.flags.bits.automatic && packetUBXESFRAW->automaticFlags.flags.bits.implicitUpdate)
{
// The GPS is automatically reporting, we just check whether we got unread data
// if (_printDebug == true)
// {
// _debugSerial->println(F("getEsfRawDataInfo: Autoreporting"));
// }
checkUbloxInternal(&packetCfg, UBX_CLASS_ESF, UBX_ESF_RAW);
return packetUBXESFRAW->moduleQueried.moduleQueried.bits.all;
}
else if (packetUBXESFRAW->automaticFlags.flags.bits.automatic && !packetUBXESFRAW->automaticFlags.flags.bits.implicitUpdate)
{
// Someone else has to call checkUblox for us...
// if (_printDebug == true)
// {
// _debugSerial->println(F("getEsfRawDataInfo: Exit immediately"));
// }
return (false);
}
else
{
// if (_printDebug == true)
// {
// _debugSerial->println(F("getEsfRawDataInfo: Polling"));
// }
// The GPS is not automatically reporting HNR PVT so we have to poll explicitly
packetCfg.cls = UBX_CLASS_ESF;
packetCfg.id = UBX_ESF_RAW;
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("getEsfRawDataInfo: data in packetCfg was OVERWRITTEN by another message (but that's OK)"));
// }
return (true);
}
// if (_printDebug == true)
// {
// _debugSerial->print(F("getEsfRawDataInfo retVal: "));
// _debugSerial->println(statusString(retVal));
// }
return (false);
}
return (false); // Trap. We should never get here...
}
// Enable or disable automatic ESF RAW message generation by the GNSS. This changes the way getESFRawDataInfo
// works.
// Enable or disable automatic ESF RAW message generation by the GNSS.
bool SFE_UBLOX_GNSS::setAutoESFRAW(bool enable, uint16_t maxWait)
{
return setAutoESFRAWrate(enable ? 1 : 0, true, maxWait);
}
// Enable or disable automatic ESF RAW message generation by the GNSS. This changes the way getESFRawDataInfo
// works.
// Enable or disable automatic ESF RAW message generation by the GNSS.
bool SFE_UBLOX_GNSS::setAutoESFRAW(bool enable, bool implicitUpdate, uint16_t maxWait)
{
return setAutoESFRAWrate(enable ? 1 : 0, implicitUpdate, maxWait);
}
// Enable or disable automatic ESF RAW message generation by the GNSS. This changes the way getESFRawDataInfo
// works.
// Enable or disable automatic ESF RAW message generation by the GNSS.
// Note: this function can only be used to enable or disable the messages. A rate of zero disables the messages.
// A rate of 1 or more causes the messages to be generated at the full 100Hz.
bool SFE_UBLOX_GNSS::setAutoESFRAWrate(uint8_t rate, bool implicitUpdate, uint16_t maxWait)
{
if (packetUBXESFRAW == NULL)
@@ -14848,11 +14777,10 @@ bool SFE_UBLOX_GNSS::setAutoESFRAWrate(uint8_t rate, bool implicitUpdate, uint16
packetUBXESFRAW->automaticFlags.flags.bits.automatic = (rate > 0);
packetUBXESFRAW->automaticFlags.flags.bits.implicitUpdate = implicitUpdate;
}
packetUBXESFRAW->moduleQueried.moduleQueried.bits.all = false; // Mark data as stale
return ok;
}
// Enable automatic navigation message generation by the GNSS.
// Enable automatic message generation by the GNSS.
bool SFE_UBLOX_GNSS::setAutoESFRAWcallback(void (*callbackPointer)(UBX_ESF_RAW_data_t), uint16_t maxWait)
{
// Enable auto messages. Set implicitUpdate to false as we expect the user to call checkUblox manually.
@@ -14937,7 +14865,6 @@ bool SFE_UBLOX_GNSS::initPacketUBXESFRAW()
packetUBXESFRAW->callbackPointer = NULL;
packetUBXESFRAW->callbackPointerPtr = NULL;
packetUBXESFRAW->callbackData = NULL;
packetUBXESFRAW->moduleQueried.moduleQueried.all = 0;
return (true);
}
@@ -14946,7 +14873,6 @@ void SFE_UBLOX_GNSS::flushESFRAW()
{
if (packetUBXESFRAW == NULL)
return; // Bail if RAM has not been allocated (otherwise we could be writing anywhere!)
packetUBXESFRAW->moduleQueried.moduleQueried.all = 0; // Mark all datums as stale (read before)
}
// Log this data in file buffer
@@ -18141,22 +18067,6 @@ bool SFE_UBLOX_GNSS::getSensorFusionMeasurement(UBX_ESF_MEAS_sensorData_t *senso
return (true);
}
bool SFE_UBLOX_GNSS::getRawSensorMeasurement(UBX_ESF_RAW_sensorData_t *sensorData, uint8_t sensor, uint16_t maxWait)
{
if (packetUBXESFRAW == NULL)
initPacketUBXESFRAW(); // Check that RAM has been allocated for the ESF RAW data
if (packetUBXESFRAW == NULL) // Bail if the RAM allocation failed
return (false);
if ((packetUBXESFRAW->moduleQueried.moduleQueried.bits.data & (1 << sensor)) == 0)
getESFRAW(maxWait);
packetUBXESFRAW->moduleQueried.moduleQueried.bits.data &= ~(1 << sensor); // Since we are about to give this to user, mark this data as stale
packetUBXESFRAW->moduleQueried.moduleQueried.bits.all = false;
sensorData->data.all = packetUBXESFRAW->data.data[sensor].data.all;
sensorData->sTag = packetUBXESFRAW->data.data[sensor].sTag;
return (true);
}
bool SFE_UBLOX_GNSS::getRawSensorMeasurement(UBX_ESF_RAW_sensorData_t *sensorData, UBX_ESF_RAW_data_t ubxDataStruct, uint8_t sensor)
{
sensorData->data.all = ubxDataStruct.data[sensor].data.all;
src/SparkFun_u-blox_GNSS_Arduino_Library.h
-3
View File
@@ -1283,8 +1283,6 @@ public:
void flushESFMEAS(); // Mark all the data as read/stale
void logESFMEAS(bool enabled = true); // Log data to file buffer
bool getEsfRawDataInfo(uint16_t maxWait = defaultMaxWait); // ESF RAW Helper
bool getESFRAW(uint16_t maxWait = defaultMaxWait); // ESF RAW
bool setAutoESFRAW(bool enabled, uint16_t maxWait = defaultMaxWait); // Enable/disable automatic ESF RAW reports
bool setAutoESFRAW(bool enabled, bool implicitUpdate, uint16_t maxWait = defaultMaxWait); // Enable/disable automatic ESF RAW reports, 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 setAutoESFRAWrate(uint8_t rate, bool implicitUpdate = true, uint16_t maxWait = defaultMaxWait); // Set the rate for automatic RAW reports
@@ -1470,7 +1468,6 @@ public:
float getESFyaw(uint16_t maxWait = defaultMaxWait); // Returned as degrees
bool getSensorFusionMeasurement(UBX_ESF_MEAS_sensorData_t *sensorData, uint8_t sensor, uint16_t maxWait = defaultMaxWait);
bool getSensorFusionMeasurement(UBX_ESF_MEAS_sensorData_t *sensorData, UBX_ESF_MEAS_data_t ubxDataStruct, uint8_t sensor);
bool getRawSensorMeasurement(UBX_ESF_RAW_sensorData_t *sensorData, uint8_t sensor, uint16_t maxWait = defaultMaxWait);
bool getRawSensorMeasurement(UBX_ESF_RAW_sensorData_t *sensorData, UBX_ESF_RAW_data_t ubxDataStruct, uint8_t sensor);
bool getSensorFusionStatus(UBX_ESF_STATUS_sensorStatus_t *sensorStatus, uint8_t sensor, uint16_t maxWait = defaultMaxWait);
bool getSensorFusionStatus(UBX_ESF_STATUS_sensorStatus_t *sensorStatus, UBX_ESF_STATUS_data_t ubxDataStruct, uint8_t sensor);
src/u-blox_structs.h
+10 -17
View File
@@ -49,6 +49,10 @@
#define DEF_NUM_SENS 7 // The maximum number of ESF sensors
#endif
#ifndef DEF_MAX_NUM_ESF_RAW_REPEATS
#define DEF_MAX_NUM_ESF_RAW_REPEATS 10 // The NEO-M8U sends ESF RAW data in blocks / sets of ten readings. (The ZED-F9R sends them one at a time.)
#endif
// Additional flags and pointers that need to be stored with each message type
struct ubxAutomaticFlags
{
@@ -2257,7 +2261,10 @@ typedef struct
// UBX-ESF-RAW (0x10 0x03): Raw sensor measurements
// Note: length is variable
const uint16_t UBX_ESF_RAW_MAX_LEN = 4 + (8 * DEF_NUM_SENS);
// Note: The ZED-F9R sends sets of seven sensor readings one at a time
// But the NEO-M8U sends them in sets of ten (i.e. seventy readings per message)
// Note: ESF RAW data cannot be polled. It is "Output" only
const uint16_t UBX_ESF_RAW_MAX_LEN = 4 + (8 * DEF_NUM_SENS * DEF_MAX_NUM_ESF_RAW_REPEATS);
typedef struct
{
@@ -2276,28 +2283,14 @@ typedef struct
typedef struct
{
uint8_t reserved1[4];
UBX_ESF_RAW_sensorData_t data[DEF_NUM_SENS];
UBX_ESF_RAW_sensorData_t data[DEF_NUM_SENS * DEF_MAX_NUM_ESF_RAW_REPEATS];
uint8_t numEsfRawBlocks; // Note: this is not contained in the ESF RAW message. It is calculated from the message length.
} UBX_ESF_RAW_data_t;
typedef struct
{
union
{
uint32_t all;
struct
{
uint32_t all : 1;
uint32_t data : DEF_NUM_SENS;
} bits;
} moduleQueried;
} UBX_ESF_RAW_moduleQueried_t;
typedef struct
{
ubxAutomaticFlags automaticFlags;
UBX_ESF_RAW_data_t data;
UBX_ESF_RAW_moduleQueried_t moduleQueried;
void (*callbackPointer)(UBX_ESF_RAW_data_t);
void (*callbackPointerPtr)(UBX_ESF_RAW_data_t *);
UBX_ESF_RAW_data_t *callbackData;