76 lines
2.3 KiB
C
76 lines
2.3 KiB
C
/*============================ INCLUDES ======================================*/
|
|
|
|
#include <stdint.h>
|
|
#include <string.h>
|
|
#include "constants.h"
|
|
#include "ublox.h"
|
|
#include "math.h"
|
|
#include "same_endian.h"
|
|
|
|
/*=============================== MARCOS =====================================*/
|
|
|
|
|
|
/*============================================================================*/
|
|
|
|
void UbxDecodeNavPvt(UbxStruct *sol, uint8_t frame[])
|
|
{
|
|
uint8_t *p = frame + 6;
|
|
uint8_t carrSoln, diffSoln;
|
|
|
|
sol->towms = GET_U32(p);
|
|
sol->utc.year = GET_U16(p + 4);
|
|
sol->utc.month = GET_U08(p + 6);
|
|
sol->utc.day = GET_U08(p + 7);
|
|
sol->utc.hour = GET_U08(p + 8);
|
|
sol->utc.min = GET_U08(p + 9);
|
|
sol->utc.isec = GET_U08(p + 10);
|
|
sol->utc.msec = sol->towms % 1000;
|
|
sol->TimeFlag = GET_U08(p + 11);
|
|
|
|
sol->GpsType = GET_U08(p + 20);
|
|
sol->RtkType = GET_U08(p + 21);
|
|
sol->SvNum = GET_U08(p + 23);
|
|
sol->LonDeg = GET_S32(p + 24);
|
|
sol->LatDeg = GET_S32(p + 28);
|
|
sol->hgt = GET_S32(p + 32); // mm Height above ellipsoid
|
|
sol->msl = GET_S32(p + 36); // mm Height above mean sea level
|
|
sol->hacc = GET_U32(p + 40);
|
|
sol->vacc = GET_U32(p + 44);
|
|
|
|
sol->vn = GET_S32(p + 48); // mm/s
|
|
sol->ve = GET_S32(p + 52);
|
|
sol->vd = GET_S32(p + 56);
|
|
|
|
sol->GroundSpeed = GET_S32(p + 60) * B10_N3;
|
|
sol->HeadingMotionDeg = GET_S32(p + 64) * B10_N5;
|
|
sol->sacc = GET_U32(p + 68);
|
|
sol->cacc = GET_U32(p + 72) * B10_N5;
|
|
sol->pdop = GET_U16(p + 76) * B10_N2;
|
|
sol->HeadingVehichleDeg = GET_S32(p + 84) * B10_N5;
|
|
|
|
sol->DateValid = GET_BYTE_FLAG(sol->TimeFlag, 0);
|
|
sol->TimeValid = GET_BYTE_FLAG(sol->TimeFlag, 1);
|
|
sol->TimeFullyResolved = GET_BYTE_FLAG(sol->TimeFlag, 2);
|
|
|
|
carrSoln = GET_BYTE_FLAG(sol->RtkType, 7) * 2 + GET_BYTE_FLAG(sol->RtkType, 6);
|
|
diffSoln = GET_BYTE_FLAG(sol->RtkType, 1);
|
|
|
|
sol->CntNavPvt++;
|
|
sol->seq++;
|
|
}
|
|
|
|
void UbxDecodeInit(UbxStruct *sol)
|
|
{
|
|
sol->seq = 0;
|
|
sol->CntNavPvt = 0;
|
|
sol->LastCntNavPvt = 0;
|
|
}
|
|
|
|
void UbxDecodeStats(UbxStruct *sol)
|
|
{
|
|
sol->PpsNavPvt = sol->CntNavPvt - sol->LastCntNavPvt;
|
|
sol->LastCntNavPvt = sol->CntNavPvt;
|
|
}
|
|
|
|
|