Skip to content
This repository was archived by the owner on Jan 28, 2021. It is now read-only.

Commit 9d59880

Browse files
edsparkedspark
committed
Adds sensor status function, changes name of imuMetric struct to imuData
* Adds more fields to the imuData struct for Raw Data * Adds new struct for all sensor status information
1 parent 7c3caab commit 9d59880

File tree

2 files changed

+104
-34
lines changed

2 files changed

+104
-34
lines changed

‎src/SparkFun_Ublox_Arduino_Library.cpp‎

Lines changed: 76 additions & 25 deletions
Original file line numberDiff line numberDiff line change
@@ -2709,7 +2709,7 @@ boolean SFE_UBLOX_GPS::getRELPOSNED(uint16_t maxWait)
27092709

27102710
return (true);
27112711
}
2712-
boolean SFE_UBLOX_GPS::getUdrStatus(uint16_t maxWait)
2712+
boolean SFE_UBLOX_GPS::getEsfStatus(uint16_t maxWait)
27132713
{
27142714
// Requesting Data from the receiver
27152715
packetCfg.cls = UBX_CLASS_ESF;
@@ -2723,16 +2723,16 @@ boolean SFE_UBLOX_GPS::getUdrStatus(uint16_t maxWait)
27232723
checkUblox();
27242724

27252725
// payload should be loaded.
2726-
imuMetric.version = extractByte(4);
2727-
imuMetric.fusionMode = extractByte(12);
2728-
imuMetric.numSens = extractByte(15);
2726+
imuData.version = extractByte(4);
2727+
imuData.fusionMode = extractByte(12);
2728+
imuData.numSens = extractByte(15);
27292729

27302730
// Individual Status Sensor in different function
27312731
return(true);
27322732
}
27332733

27342734
//
2735-
boolean SFE_UBLOX_GPS::getInsInfo(uint16_t maxWait)
2735+
boolean SFE_UBLOX_GPS::getEsfInfo(uint16_t maxWait)
27362736
{
27372737
packetCfg.cls = UBX_CLASS_ESF;
27382738
packetCfg.id = UBX_ESF_INS;
@@ -2747,26 +2747,26 @@ boolean SFE_UBLOX_GPS::getInsInfo(uint16_t maxWait)
27472747
// Validity of each sensor value below
27482748
uint32_t validity = extractLong(0);
27492749

2750-
imuMetric.xAngRateVald = (validity && 0x0080) >> 8;
2751-
imuMetric.yAngRateVald = (validity && 0x0100) >> 9;
2752-
imuMetric.zAngRateVald = (validity && 0x0200) >> 10;
2753-
imuMetric.xAccelVald = (validity && 0x0400) >> 11;
2754-
imuMetric.yAccelVald = (validity && 0x0800) >> 12;
2755-
imuMetric.zAccelVald = (validity && 0x1000) >> 13;
2750+
imuData.xAngRateVald = (validity && 0x0080) >> 8;
2751+
imuData.yAngRateVald = (validity && 0x0100) >> 9;
2752+
imuData.zAngRateVald = (validity && 0x0200) >> 10;
2753+
imuData.xAccelVald = (validity && 0x0400) >> 11;
2754+
imuData.yAccelVald = (validity && 0x0800) >> 12;
2755+
imuData.zAccelVald = (validity && 0x1000) >> 13;
27562756

2757-
imuMetric.xAngRate = extractLong(12); // deg/s
2758-
imuMetric.yAngRate = extractLong(16); // deg/s
2759-
imuMetric.zAngRate = extractLong(20); // deg/s
2757+
imuData.xAngRate = extractLong(12); // deg/s
2758+
imuData.yAngRate = extractLong(16); // deg/s
2759+
imuData.zAngRate = extractLong(20); // deg/s
27602760

2761-
imuMetric.xAccel = extractLong(24); // m/s
2762-
imuMetric.yAccel = extractLong(28); // m/s
2763-
imuMetric.zAccel = extractLong(32); // m/s
2761+
imuData.xAccel = extractLong(24); // m/s
2762+
imuData.yAccel = extractLong(28); // m/s
2763+
imuData.zAccel = extractLong(32); // m/s
27642764

27652765
return(true);
27662766
}
27672767

27682768
//
2769-
boolean SFE_UBLOX_GPS::getExternSensMeas(uint16_t maxWait)
2769+
boolean SFE_UBLOX_GPS::getEsfMeas(uint16_t maxWait)
27702770
{
27712771

27722772
packetCfg.cls = UBX_CLASS_ESF;
@@ -2787,14 +2787,23 @@ boolean SFE_UBLOX_GPS::getExternSensMeas(uint16_t maxWait)
27872787
uint8_t tagValid = (flags && 0x04) >> 3;
27882788
uint8_t numMeas = (flags && 0x1000) >> 15;
27892789

2790+
uint8_t byteOffset = 4;
2791+
2792+
for(uint8_t i=0; i<imuData.numSens; i++){
2793+
2794+
uint32_t bitField = extractLong(4 + byteOffset * i);
2795+
imuData.dataType[i] = (bitField && 0xFF000000) >> 23;
2796+
imuData.data[i] = (bitField && 0xFFFFFF);
2797+
imuData.dataTStamp[i] = extractLong(8 + byteOffset * i);
2798+
2799+
}
2800+
27902801
}
27912802

27922803
boolean SFE_UBLOX_GPS::getEsfRaw(uint16_t maxWait)
27932804
{
27942805

2795-
// Need the number of sensors to know what to sample.
2796-
getUdrStatus();
2797-
2806+
// Need to know the number of sensor to get the correct data
27982807
// Rate selected in UBX-CFG-MSG is not respected
27992808
packetCfg.cls = UBX_CLASS_ESF;
28002809
packetCfg.id = UBX_ESF_RAW;
@@ -2808,13 +2817,55 @@ boolean SFE_UBLOX_GPS::getEsfRaw(uint16_t maxWait)
28082817

28092818
uint8_t byteOffset = 8;
28102819

2811-
for(uint8_t i=0; i<imuMetric.numSens; i++){
2820+
for(uint8_t i=0; i<imuData.numSens; i++){
28122821

28132822
uint32_t bitField = extractLong(4 + byteOffset * i);
2814-
imuMetric.dataType[i] = (bitField && 0xFF000000) >> 23; // Repeating Blocks on the back burner...
2815-
imuMetric.data[i] = (bitField && 0xFFFFFF);
2816-
imuMetric.timeStamp[i] = extractLong(8 + byteOffset * i);
2823+
imuData.rawDataType[i] = (bitField && 0xFF000000) >> 23;
2824+
imuData.rawData[i] = (bitField && 0xFFFFFF);
2825+
imuData.rawTStamp[i] = extractLong(8 + byteOffset * i);
28172826

28182827
}
28192828
}
28202829

2830+
boolean SFE_UBLOX_GPS::getSensorStatus(uint8_t sensor)
2831+
{
2832+
2833+
packetCfg.cls = UBX_CLASS_ESF;
2834+
packetCfg.id = UBX_ESF_STATUS;
2835+
packetCfg.len = 0;
2836+
packetCfg.startingSpot = 0;
2837+
2838+
if (sendCommand(packetCfg, maxWait) == false)
2839+
return (false); //If command send fails then bail
2840+
2841+
uint8_t numberSens = extactByte(15)
2842+
if (sensor > numberSens)
2843+
return SFE_UBLOX_STATUS_OUT_OF_RANGE;
2844+
2845+
checkUblox();
2846+
2847+
uint8_t offset = 4;
2848+
2849+
// Only the last sensor value checked will remain.
2850+
for(uint8_t i=0; i<sensor; i++){
2851+
2852+
uint8_t sensorFieldOne = extractByte(16 + offset * i);
2853+
uint8_t sensorFieldTwo = extractByte(17 + offset * i);
2854+
ublox.freq = extractByte(18 + offset * i);
2855+
uint8_t sensorFieldThr = extractByte(19 + offset * i);
2856+
2857+
ubloxSen.senType = (sensorFieldOne && 0x10) >> 5;
2858+
ubloxSen.isUsed = (sensorFieldOne && 0x20) >> 6;
2859+
ubloxSen.isReady = (sensorFieldOne && 0x30) >> 7;
2860+
2861+
ubloxSen.calibStatus = sensorFieldTwo && 0x03;
2862+
ubloxSen.timeStatus = (sensorFieldTwo && 0xC) >> 2;
2863+
2864+
ubloxSen.badMeas = (sensorFieldThr && 0x01);
2865+
ubloxSen.badTag = (sensorFieldThr && 0x02) >> 1;
2866+
ubloxSen.missMeas = (sensorFieldThr && 0x04) >> 2;
2867+
ubloxSen.noisyMeas = (sensorFieldThr && 0x08) >> 3;
2868+
}
2869+
2870+
}
2871+

‎src/SparkFun_Ublox_Arduino_Library.h‎

Lines changed: 28 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -590,9 +590,9 @@ class SFE_UBLOX_GPS
590590
//Change the dynamic platform model using UBX-CFG-NAV5
591591
boolean setDynamicModel(dynModel newDynamicModel = DYN_MODEL_PORTABLE, uint16_t maxWait = 1100);
592592

593-
boolean getUdrStatus(uint16_t maxWait = 1100);
594-
boolean getInsInfo(uint16_t maxWait = 1100);
595-
boolean getExternSensMeas(uint16_t maxWait = 1100);
593+
boolean getEsfStatus(uint16_t maxWait = 1100);
594+
boolean getEsfInfo(uint16_t maxWait = 1100);
595+
boolean getEsfMeas(uint16_t maxWait = 1100);
596596
boolean getEsfRaw(uint16_t maxWait = 1100);
597597

598598
//Survey-in specific controls
@@ -668,11 +668,12 @@ class SFE_UBLOX_GPS
668668

669669
uint16_t rtcmFrameCounter = 0; //Tracks the type of incoming byte inside RTCM frame
670670

671-
struct udrData
671+
#define DEF_NUM_SENS 7
672+
struct deadReckData
672673
{
673674
uint8_t version;
674675
uint8_t fusionMode;
675-
uint8_t numSens;
676+
uint8_t numSens = DEF_NUM_SENS;
676677

677678
uint8_t xAngRateVald;
678679
uint8_t yAngRateVald;
@@ -690,10 +691,28 @@ class SFE_UBLOX_GPS
690691
int32_t zAccel;
691692

692693
// The array size is based on testing directly on M8U and F9R
693-
uint32_t data[7];
694-
uint32_t dataType[7];
695-
uint32_t timeStamp[7];
696-
} imuMetric;
694+
uint32_t rawData[numSens];
695+
uint32_t rawDataType[numSens];
696+
uint32_t rawTstamp[numSens];
697+
698+
uint32_t data[numSens];
699+
uint32_t dataType[numSens];
700+
uint32_t dataTstamp[numSens];
701+
} imuData;
702+
703+
struct indivImuData
704+
{
705+
uint8_t senType;
706+
bool isUsed;
707+
bool isReady;
708+
uint8_t calibStatus;
709+
uint8_t timeStatus;
710+
uint8_t freq; // Hz
711+
uint8_t badMeas;
712+
uint8_t badTag;
713+
uint8_t missMeas;
714+
uint8_t noisyMeas;
715+
} ubloxSen;
697716

698717
private:
699718
//Depending on the sentence type the processor will load characters into different arrays

0 commit comments

Comments
 (0)