@@ -554,7 +554,7 @@ void SFE_UBLOX_GPS::process(uint8_t incoming, ubxPacket *incomingUBX, uint8_t re
554554 // So let's check for an HPPOSLLH message arriving when we were expecting PVT and vice versa
555555 else if ((packetBuf.cls == requestedClass) &&
556556 (((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)) ||
558558 ((packetBuf.id == UBX_NAV_DOP) && (requestedID == UBX_NAV_PVT || requestedID == UBX_NAV_HPPOSLLH))))
559559 {
560560 // 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
827827 // So let's check for an HPPOSLLH message arriving when we were expecting PVT and vice versa
828828 else if ((incomingUBX->cls == requestedClass) &&
829829 (((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)) ||
831831 ((incomingUBX->id == UBX_NAV_DOP) && (requestedID == UBX_NAV_PVT || requestedID == UBX_NAV_HPPOSLLH))))
832832 {
833833 // This isn't the message we are looking for...
@@ -979,25 +979,25 @@ void SFE_UBLOX_GPS::processUBXpacket(ubxPacket *msg)
979979 gpsSecond = extractByte (10 );
980980 gpsDateValid = extractByte (11 ) & 0x01 ;
981981 gpsTimeValid = (extractByte (11 ) & 0x02 ) >> 1 ;
982- gpsNanosecond = extractLong (16 ); // Includes milliseconds
982+ gpsNanosecond = extractSignedLong (16 ); // Includes milliseconds
983983
984984 fixType = extractByte (20 - startingSpot);
985985 gnssFixOk = extractByte (21 - startingSpot) & 0x1 ; // Get the 1st bit
986986 diffSoln = extractByte (21 - startingSpot) >> 1 & 0x1 ; // Get the 2nd bit
987987 carrierSolution = extractByte (21 - startingSpot) >> 6 ; // Get 6th&7th bits of this byte
988988 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);
993993 horizontalAccEst = extractLong (40 - startingSpot);
994994 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);
998998
999- groundSpeed = extractLong (60 - startingSpot);
1000- headingOfMotion = extractLong (64 - startingSpot);
999+ groundSpeed = extractSignedLong (60 - startingSpot);
1000+ headingOfMotion = extractSignedLong (64 - startingSpot);
10011001 pDOP = extractInt (76 - startingSpot);
10021002
10031003 // Mark all datums as fresh (not read before)
@@ -1036,10 +1036,10 @@ void SFE_UBLOX_GPS::processUBXpacket(ubxPacket *msg)
10361036 else if (msg->id == UBX_NAV_HPPOSLLH && msg->len == 36 )
10371037 {
10381038 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 );
10431043 highResLongitudeHp = extractSignedChar (24 );
10441044 highResLatitudeHp = extractSignedChar (25 );
10451045 elipsoidHp = extractSignedChar (26 );
@@ -2973,6 +2973,19 @@ uint32_t SFE_UBLOX_GPS::extractLong(uint8_t spotToStart)
29732973 return (val);
29742974}
29752975
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+
29762989// Given a spot in the payload array, extract two bytes and build an int
29772990uint16_t SFE_UBLOX_GPS::extractInt (uint8_t spotToStart)
29782991{
@@ -3444,7 +3457,7 @@ boolean SFE_UBLOX_GPS::getDOP(uint16_t maxWait)
34443457
34453458 if (retVal == SFE_UBLOX_STATUS_DATA_RECEIVED)
34463459 return (true );
3447-
3460+
34483461 if ((retVal == SFE_UBLOX_STATUS_DATA_OVERWRITTEN) && (packetCfg.id == UBX_NAV_PVT))
34493462 {
34503463 if (_printDebug == true )
@@ -3733,7 +3746,7 @@ boolean SFE_UBLOX_GPS::getProtocolVersion(uint16_t maxWait)
37333746 for (uint8_t extensionNumber = 0 ; extensionNumber < 10 ; extensionNumber++)
37343747 {
37353748 // 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' ) )
37373750 {
37383751 versionHigh = (payloadCfg[(30 * extensionNumber) + 8 ] - ' 0' ) * 10 + (payloadCfg[(30 * extensionNumber) + 9 ] - ' 0' ); // Convert '18' to 18
37393752 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()
38183831
38193832// Relative Positioning Information in NED frame
38203833// 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
38213837boolean SFE_UBLOX_GPS::getRELPOSNED (uint16_t maxWait)
38223838{
38233839 packetCfg.cls = UBX_CLASS_NAV;
@@ -3836,35 +3852,73 @@ boolean SFE_UBLOX_GPS::getRELPOSNED(uint16_t maxWait)
38363852
38373853 int32_t tempRelPos;
38383854
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
38413857
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
38443860
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
38473863
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+ }
38503875
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+ }
38553890
38563891 uint32_t tempAcc;
38573892
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+ }
38633911
3864- tempAcc = extractLong (44 );
3865- relPosInfo.accD = tempAcc / 10000.0 ; // Convert 0.1 mm to m
3912+ uint8_t flags;
38663913
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+ }
38683922
38693923 relPosInfo.gnssFixOk = flags & (1 << 0 );
38703924 relPosInfo.diffSoln = flags & (1 << 1 );
@@ -3876,6 +3930,7 @@ boolean SFE_UBLOX_GPS::getRELPOSNED(uint16_t maxWait)
38763930
38773931 return (true );
38783932}
3933+
38793934boolean SFE_UBLOX_GPS::getEsfInfo (uint16_t maxWait)
38803935{
38813936 // Requesting Data from the receiver
@@ -3914,20 +3969,20 @@ boolean SFE_UBLOX_GPS::getEsfIns(uint16_t maxWait)
39143969 // Validity of each sensor value below
39153970 uint32_t validity = extractLong (0 );
39163971
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 ;
39233978
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
39273982
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
39313986
39323987 return (true );
39333988}
@@ -3949,23 +4004,34 @@ boolean SFE_UBLOX_GPS::getEsfDataInfo(uint16_t maxWait)
39494004 uint32_t timeStamp = extractLong (0 );
39504005 uint32_t flags = extractInt (4 );
39514006
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 ;
39564011
3957- if (numMeas > DEF_NUM_SENS)
4012+ if (numMeas > DEF_NUM_SENS) // Truncate numMeas if required
39584013 numMeas = DEF_NUM_SENS;
39594014
39604015 uint8_t byteOffset = 4 ;
39614016
39624017 for (uint8_t i = 0 ; i < numMeas; i++)
39634018 {
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
39644025
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+ }
39694035 }
39704036
39714037 return (true );
@@ -3987,13 +4053,15 @@ boolean SFE_UBLOX_GPS::getEsfRawDataInfo(uint16_t maxWait)
39874053 checkUblox ();
39884054
39894055 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+
39924059 imuMeas.rawTStamp = extractLong (8 );
39934060
39944061 return (true );
39954062}
39964063
4064+ // Note: senor numbering starts at 1 (not 0)
39974065sfe_ublox_status_e SFE_UBLOX_GPS::getSensState (uint8_t sensor, uint16_t maxWait)
39984066{
39994067
@@ -4018,22 +4086,22 @@ sfe_ublox_status_e SFE_UBLOX_GPS::getSensState(uint8_t sensor, uint16_t maxWait)
40184086 for (uint8_t i = 0 ; i < sensor; i++)
40194087 {
40204088
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) );
40244092 uint8_t sensorFieldThr = extractByte (19 + offset * i);
40254093
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 ;
40294097
4030- ubloxSen.calibStatus = sensorFieldTwo && 0x03 ;
4031- ubloxSen.timeStatus = (sensorFieldTwo && 0xC ) >> 2 ;
4098+ ubloxSen.calibStatus = sensorFieldTwo & 0x03 ;
4099+ ubloxSen.timeStatus = (sensorFieldTwo & 0xC ) >> 2 ;
40324100
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 ;
40374105 }
40384106
40394107 return (SFE_UBLOX_STATUS_SUCCESS);
@@ -4052,13 +4120,13 @@ boolean SFE_UBLOX_GPS::getVehAtt(uint16_t maxWait)
40524120
40534121 checkUblox ();
40544122
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
40584126
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
40624130
40634131 return (true );
40644132}
0 commit comments