@@ -554,7 +554,7 @@ void SFE_UBLOX_GPS::process(uint8_t incoming, ubxPacket *incomingUBX, uint8_t re
554
554
// So let's check for an HPPOSLLH message arriving when we were expecting PVT and vice versa
555
555
else if ((packetBuf.cls == requestedClass) &&
556
556
(((packetBuf.id == UBX_NAV_PVT) && (requestedID == UBX_NAV_HPPOSLLH || requestedID == UBX_NAV_DOP)) ||
557
- ((packetBuf.id == UBX_NAV_HPPOSLLH) && (requestedID == UBX_NAV_PVT || requestedID == UBX_NAV_DOP)) ||
557
+ ((packetBuf.id == UBX_NAV_HPPOSLLH) && (requestedID == UBX_NAV_PVT || requestedID == UBX_NAV_DOP)) ||
558
558
((packetBuf.id == UBX_NAV_DOP) && (requestedID == UBX_NAV_PVT || requestedID == UBX_NAV_HPPOSLLH))))
559
559
{
560
560
// This is not the message we were expecting but we start diverting data into incomingUBX (usually packetCfg) and process it anyway
@@ -827,7 +827,7 @@ void SFE_UBLOX_GPS::processUBX(uint8_t incoming, ubxPacket *incomingUBX, uint8_t
827
827
// So let's check for an HPPOSLLH message arriving when we were expecting PVT and vice versa
828
828
else if ((incomingUBX->cls == requestedClass) &&
829
829
(((incomingUBX->id == UBX_NAV_PVT) && (requestedID == UBX_NAV_HPPOSLLH || requestedID == UBX_NAV_DOP)) ||
830
- ((incomingUBX->id == UBX_NAV_HPPOSLLH) && (requestedID == UBX_NAV_PVT || requestedID == UBX_NAV_DOP)) ||
830
+ ((incomingUBX->id == UBX_NAV_HPPOSLLH) && (requestedID == UBX_NAV_PVT || requestedID == UBX_NAV_DOP)) ||
831
831
((incomingUBX->id == UBX_NAV_DOP) && (requestedID == UBX_NAV_PVT || requestedID == UBX_NAV_HPPOSLLH))))
832
832
{
833
833
// This isn't the message we are looking for...
@@ -979,25 +979,25 @@ void SFE_UBLOX_GPS::processUBXpacket(ubxPacket *msg)
979
979
gpsSecond = extractByte (10 );
980
980
gpsDateValid = extractByte (11 ) & 0x01 ;
981
981
gpsTimeValid = (extractByte (11 ) & 0x02 ) >> 1 ;
982
- gpsNanosecond = extractLong (16 ); // Includes milliseconds
982
+ gpsNanosecond = extractSignedLong (16 ); // Includes milliseconds
983
983
984
984
fixType = extractByte (20 - startingSpot);
985
985
gnssFixOk = extractByte (21 - startingSpot) & 0x1 ; // Get the 1st bit
986
986
diffSoln = extractByte (21 - startingSpot) >> 1 & 0x1 ; // Get the 2nd bit
987
987
carrierSolution = extractByte (21 - startingSpot) >> 6 ; // Get 6th&7th bits of this byte
988
988
SIV = extractByte (23 - startingSpot);
989
- longitude = extractLong (24 - startingSpot);
990
- latitude = extractLong (28 - startingSpot);
991
- altitude = extractLong (32 - startingSpot);
992
- altitudeMSL = extractLong (36 - startingSpot);
989
+ longitude = extractSignedLong (24 - startingSpot);
990
+ latitude = extractSignedLong (28 - startingSpot);
991
+ altitude = extractSignedLong (32 - startingSpot);
992
+ altitudeMSL = extractSignedLong (36 - startingSpot);
993
993
horizontalAccEst = extractLong (40 - startingSpot);
994
994
verticalAccEst = extractLong (44 - startingSpot);
995
- nedNorthVel = extractLong (48 - startingSpot);
996
- nedEastVel = extractLong (52 - startingSpot);
997
- nedDownVel = extractLong (56 - startingSpot);
995
+ nedNorthVel = extractSignedLong (48 - startingSpot);
996
+ nedEastVel = extractSignedLong (52 - startingSpot);
997
+ nedDownVel = extractSignedLong (56 - startingSpot);
998
998
999
- groundSpeed = extractLong (60 - startingSpot);
1000
- headingOfMotion = extractLong (64 - startingSpot);
999
+ groundSpeed = extractSignedLong (60 - startingSpot);
1000
+ headingOfMotion = extractSignedLong (64 - startingSpot);
1001
1001
pDOP = extractInt (76 - startingSpot);
1002
1002
1003
1003
// Mark all datums as fresh (not read before)
@@ -1036,10 +1036,10 @@ void SFE_UBLOX_GPS::processUBXpacket(ubxPacket *msg)
1036
1036
else if (msg->id == UBX_NAV_HPPOSLLH && msg->len == 36 )
1037
1037
{
1038
1038
timeOfWeek = extractLong (4 );
1039
- highResLongitude = extractLong (8 );
1040
- highResLatitude = extractLong (12 );
1041
- elipsoid = extractLong (16 );
1042
- meanSeaLevel = extractLong (20 );
1039
+ highResLongitude = extractSignedLong (8 );
1040
+ highResLatitude = extractSignedLong (12 );
1041
+ elipsoid = extractSignedLong (16 );
1042
+ meanSeaLevel = extractSignedLong (20 );
1043
1043
highResLongitudeHp = extractSignedChar (24 );
1044
1044
highResLatitudeHp = extractSignedChar (25 );
1045
1045
elipsoidHp = extractSignedChar (26 );
@@ -2973,6 +2973,19 @@ uint32_t SFE_UBLOX_GPS::extractLong(uint8_t spotToStart)
2973
2973
return (val);
2974
2974
}
2975
2975
2976
+ // Just so there is no ambiguity about whether a uint32_t will cast to a int32_t correctly...
2977
+ int32_t SFE_UBLOX_GPS::extractSignedLong (uint8_t spotToStart)
2978
+ {
2979
+ union // Use a union to convert from uint32_t to int32_t
2980
+ {
2981
+ uint32_t unsignedLong;
2982
+ int32_t signedLong;
2983
+ } unsignedSigned;
2984
+
2985
+ unsignedSigned.unsignedLong = extractLong (spotToStart);
2986
+ return (unsignedSigned.signedLong );
2987
+ }
2988
+
2976
2989
// Given a spot in the payload array, extract two bytes and build an int
2977
2990
uint16_t SFE_UBLOX_GPS::extractInt (uint8_t spotToStart)
2978
2991
{
@@ -3444,7 +3457,7 @@ boolean SFE_UBLOX_GPS::getDOP(uint16_t maxWait)
3444
3457
3445
3458
if (retVal == SFE_UBLOX_STATUS_DATA_RECEIVED)
3446
3459
return (true );
3447
-
3460
+
3448
3461
if ((retVal == SFE_UBLOX_STATUS_DATA_OVERWRITTEN) && (packetCfg.id == UBX_NAV_PVT))
3449
3462
{
3450
3463
if (_printDebug == true )
@@ -3733,7 +3746,7 @@ boolean SFE_UBLOX_GPS::getProtocolVersion(uint16_t maxWait)
3733
3746
for (uint8_t extensionNumber = 0 ; extensionNumber < 10 ; extensionNumber++)
3734
3747
{
3735
3748
// Now we need to find "PROTVER=18.00" in the incoming byte stream
3736
- if (payloadCfg[(30 * extensionNumber) + 0 ] == ' P' && payloadCfg[(30 * extensionNumber) + 6 ] == ' R' )
3749
+ if (( payloadCfg[(30 * extensionNumber) + 0 ] == ' P' ) && ( payloadCfg[(30 * extensionNumber) + 6 ] == ' R' ) )
3737
3750
{
3738
3751
versionHigh = (payloadCfg[(30 * extensionNumber) + 8 ] - ' 0' ) * 10 + (payloadCfg[(30 * extensionNumber) + 9 ] - ' 0' ); // Convert '18' to 18
3739
3752
versionLow = (payloadCfg[(30 * extensionNumber) + 11 ] - ' 0' ) * 10 + (payloadCfg[(30 * extensionNumber) + 12 ] - ' 0' ); // Convert '00' to 00
@@ -3818,6 +3831,9 @@ void SFE_UBLOX_GPS::flushDOP()
3818
3831
3819
3832
// Relative Positioning Information in NED frame
3820
3833
// Returns true if commands was successful
3834
+ // Note:
3835
+ // RELPOSNED on the M8 is only 40 bytes long
3836
+ // RELPOSNED on the F9 is 64 bytes long and contains much more information
3821
3837
boolean SFE_UBLOX_GPS::getRELPOSNED (uint16_t maxWait)
3822
3838
{
3823
3839
packetCfg.cls = UBX_CLASS_NAV;
@@ -3836,35 +3852,73 @@ boolean SFE_UBLOX_GPS::getRELPOSNED(uint16_t maxWait)
3836
3852
3837
3853
int32_t tempRelPos;
3838
3854
3839
- tempRelPos = extractLong (8 );
3840
- relPosInfo.relPosN = tempRelPos / 100.0 ; // Convert cm to m
3855
+ tempRelPos = extractSignedLong (8 );
3856
+ relPosInfo.relPosN = (( float ) tempRelPos) / 100.0 ; // Convert cm to m
3841
3857
3842
- tempRelPos = extractLong (12 );
3843
- relPosInfo.relPosE = tempRelPos / 100.0 ; // Convert cm to m
3858
+ tempRelPos = extractSignedLong (12 );
3859
+ relPosInfo.relPosE = (( float ) tempRelPos) / 100.0 ; // Convert cm to m
3844
3860
3845
- tempRelPos = extractLong (16 );
3846
- relPosInfo.relPosD = tempRelPos / 100.0 ; // Convert cm to m
3861
+ tempRelPos = extractSignedLong (16 );
3862
+ relPosInfo.relPosD = (( float ) tempRelPos) / 100.0 ; // Convert cm to m
3847
3863
3848
- relPosInfo.relPosLength = extractLong (20 );
3849
- relPosInfo.relPosHeading = extractLong (24 );
3864
+ if (packetCfg.len == 40 )
3865
+ {
3866
+ // The M8 version does not contain relPosLength or relPosHeading
3867
+ relPosInfo.relPosLength = 0 ;
3868
+ relPosInfo.relPosHeading = 0 ;
3869
+ }
3870
+ else
3871
+ {
3872
+ relPosInfo.relPosLength = extractSignedLong (20 );
3873
+ relPosInfo.relPosHeading = extractSignedLong (24 );
3874
+ }
3850
3875
3851
- relPosInfo.relPosHPN = payloadCfg[32 ];
3852
- relPosInfo.relPosHPE = payloadCfg[33 ];
3853
- relPosInfo.relPosHPD = payloadCfg[34 ];
3854
- relPosInfo.relPosHPLength = payloadCfg[35 ];
3876
+ if (packetCfg.len == 40 )
3877
+ {
3878
+ relPosInfo.relPosHPN = payloadCfg[20 ];
3879
+ relPosInfo.relPosHPE = payloadCfg[21 ];
3880
+ relPosInfo.relPosHPD = payloadCfg[22 ];
3881
+ relPosInfo.relPosHPLength = 0 ; // The M8 version does not contain relPosHPLength
3882
+ }
3883
+ else
3884
+ {
3885
+ relPosInfo.relPosHPN = payloadCfg[32 ];
3886
+ relPosInfo.relPosHPE = payloadCfg[33 ];
3887
+ relPosInfo.relPosHPD = payloadCfg[34 ];
3888
+ relPosInfo.relPosHPLength = payloadCfg[35 ];
3889
+ }
3855
3890
3856
3891
uint32_t tempAcc;
3857
3892
3858
- tempAcc = extractLong (36 );
3859
- relPosInfo.accN = tempAcc / 10000.0 ; // Convert 0.1 mm to m
3860
-
3861
- tempAcc = extractLong (40 );
3862
- relPosInfo.accE = tempAcc / 10000.0 ; // Convert 0.1 mm to m
3893
+ if (packetCfg.len == 40 )
3894
+ {
3895
+ tempAcc = extractLong (24 );
3896
+ relPosInfo.accN = ((float )tempAcc) / 10000.0 ; // Convert 0.1 mm to m
3897
+ tempAcc = extractLong (28 );
3898
+ relPosInfo.accE = ((float )tempAcc) / 10000.0 ; // Convert 0.1 mm to m
3899
+ tempAcc = extractLong (32 );
3900
+ relPosInfo.accD = ((float )tempAcc) / 10000.0 ; // Convert 0.1 mm to m
3901
+ }
3902
+ else
3903
+ {
3904
+ tempAcc = extractLong (36 );
3905
+ relPosInfo.accN = ((float )tempAcc) / 10000.0 ; // Convert 0.1 mm to m
3906
+ tempAcc = extractLong (40 );
3907
+ relPosInfo.accE = ((float )tempAcc) / 10000.0 ; // Convert 0.1 mm to m
3908
+ tempAcc = extractLong (44 );
3909
+ relPosInfo.accD = ((float )tempAcc) / 10000.0 ; // Convert 0.1 mm to m
3910
+ }
3863
3911
3864
- tempAcc = extractLong (44 );
3865
- relPosInfo.accD = tempAcc / 10000.0 ; // Convert 0.1 mm to m
3912
+ uint8_t flags;
3866
3913
3867
- uint8_t flags = payloadCfg[60 ];
3914
+ if (packetCfg.len == 40 )
3915
+ {
3916
+ flags = payloadCfg[36 ];
3917
+ }
3918
+ else
3919
+ {
3920
+ flags = payloadCfg[60 ];
3921
+ }
3868
3922
3869
3923
relPosInfo.gnssFixOk = flags & (1 << 0 );
3870
3924
relPosInfo.diffSoln = flags & (1 << 1 );
@@ -3876,6 +3930,7 @@ boolean SFE_UBLOX_GPS::getRELPOSNED(uint16_t maxWait)
3876
3930
3877
3931
return (true );
3878
3932
}
3933
+
3879
3934
boolean SFE_UBLOX_GPS::getEsfInfo (uint16_t maxWait)
3880
3935
{
3881
3936
// Requesting Data from the receiver
@@ -3914,20 +3969,20 @@ boolean SFE_UBLOX_GPS::getEsfIns(uint16_t maxWait)
3914
3969
// Validity of each sensor value below
3915
3970
uint32_t validity = extractLong (0 );
3916
3971
3917
- imuMeas.xAngRateVald = (validity && 0x0080 ) >> 8 ;
3918
- imuMeas.yAngRateVald = (validity && 0x0100 ) >> 9 ;
3919
- imuMeas.zAngRateVald = (validity && 0x0200 ) >> 10 ;
3920
- imuMeas.xAccelVald = (validity && 0x0400 ) >> 11 ;
3921
- imuMeas.yAccelVald = (validity && 0x0800 ) >> 12 ;
3922
- imuMeas.zAccelVald = (validity && 0x1000 ) >> 13 ;
3972
+ imuMeas.xAngRateVald = (validity & 0x0100 ) >> 8 ;
3973
+ imuMeas.yAngRateVald = (validity & 0x0200 ) >> 9 ;
3974
+ imuMeas.zAngRateVald = (validity & 0x0400 ) >> 10 ;
3975
+ imuMeas.xAccelVald = (validity & 0x0800 ) >> 11 ;
3976
+ imuMeas.yAccelVald = (validity & 0x1000 ) >> 12 ;
3977
+ imuMeas.zAccelVald = (validity & 0x2000 ) >> 13 ;
3923
3978
3924
- imuMeas.xAngRate = extractLong (12 ); // deg/s
3925
- imuMeas.yAngRate = extractLong (16 ); // deg/s
3926
- imuMeas.zAngRate = extractLong (20 ); // deg/s
3979
+ imuMeas.xAngRate = extractSignedLong (12 ); // 0.001 deg/s
3980
+ imuMeas.yAngRate = extractSignedLong (16 ); // 0.001 deg/s
3981
+ imuMeas.zAngRate = extractSignedLong (20 ); // 0.001 deg/s
3927
3982
3928
- imuMeas.xAccel = extractLong (24 ); // m/s
3929
- imuMeas.yAccel = extractLong (28 ); // m/s
3930
- imuMeas.zAccel = extractLong (32 ); // m/s
3983
+ imuMeas.xAccel = extractSignedLong (24 ); // 0.01 m/s^2
3984
+ imuMeas.yAccel = extractSignedLong (28 ); // 0.01 m/s^2
3985
+ imuMeas.zAccel = extractSignedLong (32 ); // 0.01 m/s^2
3931
3986
3932
3987
return (true );
3933
3988
}
@@ -3949,23 +4004,34 @@ boolean SFE_UBLOX_GPS::getEsfDataInfo(uint16_t maxWait)
3949
4004
uint32_t timeStamp = extractLong (0 );
3950
4005
uint32_t flags = extractInt (4 );
3951
4006
3952
- uint8_t timeSent = ( flags && 0x01 ) >> 1 ;
3953
- uint8_t timeEdge = (flags && 0x02 ) >> 2 ;
3954
- uint8_t tagValid = (flags && 0x04 ) >> 3 ;
3955
- uint8_t numMeas = (flags && 0x1000 ) >> 15 ;
4007
+ uint8_t timeSent = flags & 0x03 ; // timeSent is 2-bit: 0 = none, 1 = on Ext0, 2 = on Ext1
4008
+ uint8_t timeEdge = (flags & 0x04 ) >> 2 ;
4009
+ uint8_t tagValid = (flags & 0x08 ) >> 3 ;
4010
+ uint8_t numMeas = (flags & 0xF800 ) >> 11 ;
3956
4011
3957
- if (numMeas > DEF_NUM_SENS)
4012
+ if (numMeas > DEF_NUM_SENS) // Truncate numMeas if required
3958
4013
numMeas = DEF_NUM_SENS;
3959
4014
3960
4015
uint8_t byteOffset = 4 ;
3961
4016
3962
4017
for (uint8_t i = 0 ; i < numMeas; i++)
3963
4018
{
4019
+ uint32_t bitField = extractLong (8 + (byteOffset * i));
4020
+ imuMeas.dataType [i] = (bitField & 0x3F000000 ) >> 24 ;
4021
+ imuMeas.data [i] = (bitField & 0xFFFFFF );
4022
+ }
4023
+
4024
+ numMeas = (flags & 0xF800 ) >> 11 ; // Restore numMeas
3964
4025
3965
- uint32_t bitField = extractLong (4 + byteOffset * i);
3966
- imuMeas.dataType [i] = (bitField && 0xFF000000 ) >> 23 ;
3967
- imuMeas.data [i] = (bitField && 0xFFFFFF );
3968
- imuMeas.dataTStamp [i] = extractLong (8 + byteOffset * i);
4026
+ if (packetCfg.len > (8 + (4 * numMeas))) // The calibTtag is optional - only extract it if it is present
4027
+ {
4028
+ uint8_t startOfTtag = 8 + (4 * numMeas); // Calculate where the Ttag data starts
4029
+ if (numMeas > DEF_NUM_SENS) // Truncate numMeas if required
4030
+ numMeas = DEF_NUM_SENS;
4031
+ for (uint8_t i = 0 ; i < numMeas; i++)
4032
+ {
4033
+ imuMeas.dataTStamp [i] = extractLong (startOfTtag + (byteOffset * i));
4034
+ }
3969
4035
}
3970
4036
3971
4037
return (true );
@@ -3987,13 +4053,15 @@ boolean SFE_UBLOX_GPS::getEsfRawDataInfo(uint16_t maxWait)
3987
4053
checkUblox ();
3988
4054
3989
4055
uint32_t bitField = extractLong (4 );
3990
- imuMeas.rawDataType = (bitField && 0xFF000000 ) >> 23 ;
3991
- imuMeas.rawData = (bitField && 0xFFFFFF );
4056
+ imuMeas.rawDataType = (bitField & 0xFF000000 ) >> 24 ;
4057
+ imuMeas.rawData = (bitField & 0xFFFFFF );
4058
+
3992
4059
imuMeas.rawTStamp = extractLong (8 );
3993
4060
3994
4061
return (true );
3995
4062
}
3996
4063
4064
+ // Note: senor numbering starts at 1 (not 0)
3997
4065
sfe_ublox_status_e SFE_UBLOX_GPS::getSensState (uint8_t sensor, uint16_t maxWait)
3998
4066
{
3999
4067
@@ -4018,22 +4086,22 @@ sfe_ublox_status_e SFE_UBLOX_GPS::getSensState(uint8_t sensor, uint16_t maxWait)
4018
4086
for (uint8_t i = 0 ; i < sensor; i++)
4019
4087
{
4020
4088
4021
- uint8_t sensorFieldOne = extractByte (16 + offset * i);
4022
- uint8_t sensorFieldTwo = extractByte (17 + offset * i);
4023
- ubloxSen.freq = extractByte (18 + offset * i);
4089
+ uint8_t sensorFieldOne = extractByte (16 + ( offset * i) );
4090
+ uint8_t sensorFieldTwo = extractByte (17 + ( offset * i) );
4091
+ ubloxSen.freq = extractByte (18 + ( offset * i) );
4024
4092
uint8_t sensorFieldThr = extractByte (19 + offset * i);
4025
4093
4026
- ubloxSen.senType = (sensorFieldOne && 0x10 ) >> 5 ;
4027
- ubloxSen.isUsed = (sensorFieldOne && 0x20 ) >> 6 ;
4028
- ubloxSen.isReady = (sensorFieldOne && 0x30 ) >> 7 ;
4094
+ ubloxSen.senType = (sensorFieldOne & 0x3F ) ;
4095
+ ubloxSen.isUsed = (sensorFieldOne & 0x40 ) >> 6 ;
4096
+ ubloxSen.isReady = (sensorFieldOne & 0x80 ) >> 7 ;
4029
4097
4030
- ubloxSen.calibStatus = sensorFieldTwo && 0x03 ;
4031
- ubloxSen.timeStatus = (sensorFieldTwo && 0xC ) >> 2 ;
4098
+ ubloxSen.calibStatus = sensorFieldTwo & 0x03 ;
4099
+ ubloxSen.timeStatus = (sensorFieldTwo & 0xC ) >> 2 ;
4032
4100
4033
- ubloxSen.badMeas = (sensorFieldThr && 0x01 );
4034
- ubloxSen.badTag = (sensorFieldThr && 0x02 ) >> 1 ;
4035
- ubloxSen.missMeas = (sensorFieldThr && 0x04 ) >> 2 ;
4036
- ubloxSen.noisyMeas = (sensorFieldThr && 0x08 ) >> 3 ;
4101
+ ubloxSen.badMeas = (sensorFieldThr & 0x01 );
4102
+ ubloxSen.badTag = (sensorFieldThr & 0x02 ) >> 1 ;
4103
+ ubloxSen.missMeas = (sensorFieldThr & 0x04 ) >> 2 ;
4104
+ ubloxSen.noisyMeas = (sensorFieldThr & 0x08 ) >> 3 ;
4037
4105
}
4038
4106
4039
4107
return (SFE_UBLOX_STATUS_SUCCESS);
@@ -4052,13 +4120,13 @@ boolean SFE_UBLOX_GPS::getVehAtt(uint16_t maxWait)
4052
4120
4053
4121
checkUblox ();
4054
4122
4055
- vehAtt.roll = extractLong (8 );
4056
- vehAtt.pitch = extractLong (12 );
4057
- vehAtt.heading = extractLong (16 );
4123
+ vehAtt.roll = extractSignedLong (8 ); // 0.00001 deg
4124
+ vehAtt.pitch = extractSignedLong (12 ); // 0.00001 deg
4125
+ vehAtt.heading = extractSignedLong (16 ); // 0.00001 deg
4058
4126
4059
- vehAtt.accRoll = extractLong (20 );
4060
- vehAtt.accPitch = extractLong (24 );
4061
- vehAtt.accHeading = extractLong (28 );
4127
+ vehAtt.accRoll = extractLong (20 ); // 0.00001 deg
4128
+ vehAtt.accPitch = extractLong (24 ); // 0.00001 deg
4129
+ vehAtt.accHeading = extractLong (28 ); // 0.00001 deg
4062
4130
4063
4131
return (true );
4064
4132
}
0 commit comments