Sample code for GNSS position information

//--------------------------------------------------------------------------------------------------
/**
* Handler function for Position Notifications.
*
*/
//--------------------------------------------------------------------------------------------------
static void PositionHandlerFunction
(
le_gnss_SampleRef_t positionSampleRef,
void* contextPtr
)
{
le_result_t result;
// Date parameters
uint16_t year;
uint16_t month;
uint16_t day;
// Time parameters
uint16_t hours;
uint16_t minutes;
uint16_t seconds;
uint16_t milliseconds;
// GPS time
uint32_t gpsWeek;
uint32_t gpsTimeOfWeek;
// Leap seconds in advance
uint8_t leapSeconds;
// Position state
// Location
int32_t latitude;
int32_t longitude;
int32_t altitude;
int32_t altitudeOnWgs84;
int32_t hAccuracy;
int32_t vAccuracy;
int32_t magneticDeviation;
// DOP parameter
uint16_t dop;
// Horizontal speed
uint32_t hSpeed;
uint32_t hSpeedAccuracy;
// Vertical speed
int32_t vSpeed;
int32_t vSpeedAccuracy;
// Direction
uint32_t direction;
uint32_t directionAccuracy;
 
static const char *tabDop[] =
{
"Position dilution of precision (PDOP)",
"Horizontal dilution of precision (HDOP)",
"Vertical dilution of precision (VDOP)",
"Geometric dilution of precision (GDOP)",
"Time dilution of precision (TDOP)"
};
 
if (NULL == positionSampleRef)
{
LE_ERROR("New Position sample is NULL!");
}
else
{
LE_DEBUG("New Position sample %p", positionSampleRef);
}
 
// Get UTC date
result = le_gnss_GetDate(positionSampleRef, &year, &month, &day);
LE_ASSERT((LE_OK == result)||(LE_OUT_OF_RANGE == result));
 
// Get UTC time
result = le_gnss_GetTime(positionSampleRef, &hours, &minutes, &seconds, &milliseconds);
LE_ASSERT((LE_OK == result)||(LE_OUT_OF_RANGE == result));
 
// Get Epoch time
LE_ASSERT_OK(le_gnss_GetEpochTime(positionSampleRef, &EpochTime));
 
// Display time/date format 13:45:30 2009-06-15
LE_INFO("%02d:%02d:%02d %d-%02d-%02d,", hours, minutes, seconds, year, month, day);
 
// Display Epoch time
LE_INFO("epoch time: %llu:", (unsigned long long int) EpochTime);
 
// Get GPS time
result = le_gnss_GetGpsTime(positionSampleRef, &gpsWeek, &gpsTimeOfWeek);
LE_ASSERT((LE_OK == result)||(LE_OUT_OF_RANGE == result));
 
LE_INFO("GPS time W %02d:ToW %dms", gpsWeek, gpsTimeOfWeek);
 
// Get time accuracy
result = le_gnss_GetTimeAccuracy(positionSampleRef, &TimeAccuracy);
LE_ASSERT((LE_OK == result)||(LE_OUT_OF_RANGE == result));
 
LE_INFO("GPS time acc %d", TimeAccuracy);
 
// Get UTC leap seconds in advance
result = le_gnss_GetGpsLeapSeconds(positionSampleRef, &leapSeconds);
LE_ASSERT((LE_OK == result)||(LE_OUT_OF_RANGE == result));
 
LE_INFO("UTC leap seconds in advance %d", leapSeconds);
 
// Get position state
result = le_gnss_GetPositionState(positionSampleRef, &state);
LE_ASSERT((LE_OK == result));
LE_DEBUG("Position state: %s", (LE_GNSS_STATE_FIX_NO_POS == state)?"No Fix"
:(LE_GNSS_STATE_FIX_2D == state)?"2D Fix"
:(LE_GNSS_STATE_FIX_3D == state)?"3D Fix"
: "Unknown");
 
// Get Location
result = le_gnss_GetLocation(positionSampleRef, &latitude, &longitude, &hAccuracy);
LE_ASSERT((LE_OK == result)||(LE_OUT_OF_RANGE == result));
 
if (LE_OK == result)
{
LE_INFO("Position lat.%f, long.%f, hAccuracy.%f",
(float)latitude/1000000.0,
(float)longitude/1000000.0,
(float)hAccuracy/100.0);
}
else
{
LE_INFO("Position unknown [%d,%d,%d]", latitude, longitude, hAccuracy);
}
 
// Get altitude
result = le_gnss_GetAltitude( positionSampleRef, &altitude, &vAccuracy);
LE_ASSERT((LE_OK == result)||(LE_OUT_OF_RANGE == result));
 
if (LE_OK == result)
{
LE_INFO("Altitude.%f, vAccuracy.%f", (float)altitude/1000.0, (float)vAccuracy/10.0);
}
else
{
LE_INFO("Altitude unknown [%d,%d]", altitude, vAccuracy);
}
 
// Get altitude in meters, between WGS-84 earth ellipsoid
// and mean sea level [resolution 1e-3]
result = le_gnss_GetAltitudeOnWgs84(positionSampleRef, &altitudeOnWgs84);
LE_ASSERT((LE_OK == result)||(LE_OUT_OF_RANGE == result));
 
if (LE_OK == result)
{
LE_INFO("AltitudeOnWgs84.%d", altitudeOnWgs84/1000);
}
else
{
LE_INFO("AltitudeOnWgs84 unknown [%d]", altitudeOnWgs84);
}
 
// Get DOP parameter
do
{
result = le_gnss_GetDilutionOfPrecision(positionSampleRef,
dopType,
&dop);
LE_ASSERT((result == LE_OK)||(result == LE_OUT_OF_RANGE));
if (LE_OK == result)
{
printf("%s %.3f\n", tabDop[dopType], (float)(dop)/1000);
}
else
{
printf("%s invalid %d\n", tabDop[dopType], dop);
}
dopType++;
}
while (dopType != LE_GNSS_DOP_LAST);
 
// Get horizontal speed
result = le_gnss_GetHorizontalSpeed( positionSampleRef, &hSpeed, &hSpeedAccuracy);
LE_ASSERT((LE_OK == result)||(LE_OUT_OF_RANGE == result));
if (LE_OK == result)
{
LE_INFO("hSpeed %u - Accuracy %u", hSpeed/100, hSpeedAccuracy/10);
}
else
{
LE_INFO("hSpeed unknown [%u,%u]", hSpeed, hSpeedAccuracy);
}
 
// Get vertical speed
result = le_gnss_GetVerticalSpeed( positionSampleRef, &vSpeed, &vSpeedAccuracy);
LE_ASSERT((LE_OK == result)||(LE_OUT_OF_RANGE == result));
if (LE_OK == result)
{
LE_INFO("vSpeed %d - Accuracy %d", vSpeed/100, vSpeedAccuracy/10);
}
else
{
LE_INFO("vSpeed unknown [%d,%d]", vSpeed, vSpeedAccuracy);
}
 
// Get direction
result = le_gnss_GetDirection( positionSampleRef, &direction, &directionAccuracy);
LE_ASSERT((LE_OK == result)||(LE_OUT_OF_RANGE == result));
if (LE_OK == result)
{
LE_INFO("direction %u - Accuracy %u", direction/10, directionAccuracy/10);
}
else
{
LE_INFO("direction unknown [%u,%u]", direction, directionAccuracy);
}
 
// Get the magnetic deviation
result = le_gnss_GetMagneticDeviation( positionSampleRef, &magneticDeviation);
LE_ASSERT((LE_OK == result)||(LE_OUT_OF_RANGE == result));
if (LE_OK == result)
{
LE_INFO("magnetic deviation %d", magneticDeviation/10);
}
else
{
LE_INFO("magnetic deviation unknown [%d]",magneticDeviation);
}
 
 
/* Satellites status */
uint8_t satsInViewCount;
uint8_t satsTrackingCount;
uint8_t satsUsedCount;
result = le_gnss_GetSatellitesStatus(positionSampleRef,
&satsInViewCount,
&satsTrackingCount,
&satsUsedCount);
 
LE_ASSERT((LE_OK == result)||(LE_OUT_OF_RANGE == result));
 
LE_INFO("satsInView %d - satsTracking %d - satsUsed %d",
satsInViewCount,
satsTrackingCount,
satsUsedCount);
 
/* Satellites information */
uint16_t satIdPtr[LE_GNSS_SV_INFO_MAX_LEN];
size_t satIdNumElements = sizeof(satIdPtr);
size_t satConstNumElements = sizeof(satConstPtr);
bool satUsedPtr[LE_GNSS_SV_INFO_MAX_LEN];
size_t satUsedNumElements = sizeof(satUsedPtr);
uint8_t satSnrPtr[LE_GNSS_SV_INFO_MAX_LEN];
size_t satSnrNumElements = sizeof(satSnrPtr);
uint16_t satAzimPtr[LE_GNSS_SV_INFO_MAX_LEN];
size_t satAzimNumElements = sizeof(satAzimPtr);
uint8_t satElevPtr[LE_GNSS_SV_INFO_MAX_LEN];
size_t satElevNumElements = sizeof(satElevPtr);
int i;
 
result = le_gnss_GetSatellitesInfo(positionSampleRef,
satIdPtr,
&satIdNumElements,
satConstPtr,
&satConstNumElements,
satUsedPtr,
&satUsedNumElements,
satSnrPtr,
&satSnrNumElements,
satAzimPtr,
&satAzimNumElements,
satElevPtr,
&satElevNumElements);
 
LE_ASSERT((LE_OK == result)||(LE_OUT_OF_RANGE == result));
 
// Satellite Vehicle information
for(i=0; i<satIdNumElements; i++)
{
if ((0 != satIdPtr[i]) && (UINT16_MAX != satIdPtr[i]))
{
LE_INFO("[%02d] SVid %03d - C%01d - U%d - SNR%02d - Azim%03d - Elev%02d",
i,
satIdPtr[i],
satConstPtr[i],
satUsedPtr[i],
satSnrPtr[i],
satAzimPtr[i],
satElevPtr[i]);
 
if (LE_GNSS_SV_CONSTELLATION_SBAS == satConstPtr[i])
{
LE_INFO("SBAS category : %d", le_gnss_GetSbasConstellationCategory(satIdPtr[i]));
}
}
}
 
// Release provided Position sample reference
le_gnss_ReleaseSampleRef(positionSampleRef);
}
 
//--------------------------------------------------------------------------------------------------
/**
* Test: Add Position Handler
*
*/
//--------------------------------------------------------------------------------------------------
static void* PositionThread
(
void* context
)
{
 
 
LE_INFO("======== Position Handler thread ========");
PositionHandlerRef = le_gnss_AddPositionHandler(PositionHandlerFunction, NULL);
LE_ASSERT((PositionHandlerRef != NULL));
 
return NULL;
}
 
//--------------------------------------------------------------------------------------------------
/**
* Test: GNSS position handler
*
*/
//--------------------------------------------------------------------------------------------------
static void TestLeGnssPositionHandler
(
void
)
{
le_result_t result;
le_thread_Ref_t positionThreadRef;
le_gnss_SampleRef_t positionSampleRef;
uint64_t epochTime;
uint32_t ttff = 0;
uint8_t minElevation;
 
LE_INFO("Start Test Testle_gnss_PositionHandlerTest");
 
// NMEA frame GPGSA is checked that no SV with elevation below 10
// degrees are given.
minElevation = 10;
LE_INFO("Set minElevation %d",minElevation);
 
LE_INFO("Start GNSS");
LE_INFO("Wait 5 seconds");
sleep(5);
 
// Add Position Handler Test
positionThreadRef = le_thread_Create("PositionThread",PositionThread,NULL);
le_thread_Start(positionThreadRef);
 
// test Cold Restart boosted by le_gnss_InjectUtcTime
// EpochTime and timeAccuracy should be valid and saved by now
sleep(2);
 
LE_INFO("Ask for a Cold restart");
 
// Last accurate epochTime and timeAccuracy are used
LE_ASSERT(0 != EpochTime);
LE_INFO("TimeAccuracy %d",TimeAccuracy);
LE_ASSERT_OK(le_gnss_InjectUtcTime(EpochTime , TimeAccuracy));
 
// Get TTFF,position fix should be still in progress for the FACTORY start
result = le_gnss_GetTtff(&ttff);
LE_ASSERT(LE_BUSY == result);
LE_INFO("TTFF is checked as not available immediatly after a Cold restart");
 
// Wait for a 3D fix
LE_INFO("Wait 60 seconds for a 3D fix");
sleep(60);
// Get TTFF
result = le_gnss_GetTtff(&ttff);
LE_ASSERT((LE_OK == result)||(LE_BUSY == result));
if(result == LE_OK)
{
LE_INFO("TTFF cold restart = %d msec", ttff);
}
else
{
LE_INFO("TTFF cold restart not available");
}
 
le_gnss_RemovePositionHandler(PositionHandlerRef);
 
LE_INFO("Wait 5 seconds");
sleep(5);
 
// stop thread
le_thread_Cancel(positionThreadRef);
 
// Get Epoch time, get last position sample
positionSampleRef = le_gnss_GetLastSampleRef();
LE_ASSERT_OK(le_gnss_GetEpochTime(positionSampleRef, &epochTime));
 
// Display epoch time
LE_INFO("epoch time: %llu:", (unsigned long long int) epochTime);
 
LE_INFO("Stop GNSS");
EpochTime=0;
TimeAccuracy=0;
}