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;
int64_t latitudeOnPZ90;
int64_t longitudeOnPZ90;
int32_t altitude;
int32_t altitudeOnWgs84;
int64_t altitudeOnPZ90;
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);
 
// Latitude conversion
latitude,
&latitudeOnPZ90);
LE_ASSERT((LE_OK == result) || (LE_UNSUPPORTED == result));
if (LE_OK == result)
{
LE_INFO("Latitude: On WGS84 %d, On PZ90 %" PRId64 ", float %f",
latitude,
latitudeOnPZ90,
(float)latitudeOnPZ90/1000000.0);
}
 
// Longitude conversion
longitude,
&longitudeOnPZ90);
LE_ASSERT((LE_OK == result) || (LE_UNSUPPORTED == result));
if (LE_OK == result)
{
LE_INFO("Longitude: On WGS84 %d, On PZ90 %" PRId64 ", float %f",
longitude,
longitudeOnPZ90,
(float)longitudeOnPZ90/1000000.0);
}
}
else
{
if (INT32_MAX != latitude)
{
LE_INFO("Latitude %f", (float)latitude/1000000.0);
}
else
{
LE_INFO("Latitude unknown %d", latitude);
}
 
if (INT32_MAX != longitude)
{
LE_INFO("Latitude %f", (float)longitude/1000000.0);
}
else
{
LE_INFO("Longitude unknown %d", longitude);
}
 
if (INT32_MAX != hAccuracy)
{
LE_INFO("Horizontal accuracy %f", (float)hAccuracy/100.0);
}
else
{
LE_INFO("Horizontal accuracy unknown %d", hAccuracy);
}
}
 
// Get altitude
LE_INFO("Test SetDataResolution() for vAccuracy parameter of le_gnss_GetAltitude() function");
 
for (dataRes=LE_GNSS_RES_ZERO_DECIMAL; dataRes<LE_GNSS_RES_UNKNOWN; dataRes++)
{
result = le_gnss_GetAltitude( positionSampleRef, &altitude, &vAccuracy);
LE_ASSERT((LE_OK == result) || (LE_OUT_OF_RANGE == result));
 
if (LE_OK == result)
{
switch(dataRes)
{
LE_INFO("Resolution: %d decimal place, altitude.%f, vAccuracy.%f",
dataRes, (float)altitude/1000.0, (float)vAccuracy);
break;
LE_INFO("Resolution: %d decimal place, altitude.%f, vAccuracy.%f",
dataRes, (float)altitude/1000.0, (float)vAccuracy/10.0);
break;
LE_INFO("Resolution: %d decimal place, altitude.%f, vAccuracy.%f",
dataRes, (float)altitude/1000.0, (float)vAccuracy/100.0);
break;
LE_INFO("Resolution: %d decimal place, altitude.%f, vAccuracy.%f",
dataRes, (float)altitude/1000.0, (float)vAccuracy/1000.0);
break;
default:
LE_INFO("Unknown resolution.");
break;
}
}
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: %f", (float)altitudeOnWgs84/1000.0);
 
altitudeOnWgs84,
&altitudeOnPZ90);
LE_ASSERT((LE_OK == result) || (LE_UNSUPPORTED == result));
if (LE_OK == result)
{
LE_INFO("Altitude: On WGS84: %d, On PZ90 %" PRId64 ", float %f",
altitudeOnWgs84,
altitudeOnPZ90,
(float)altitudeOnPZ90/1000.0);
}
}
else
{
LE_INFO("AltitudeOnWgs84 unknown [%d]", altitudeOnWgs84);
}
 
LE_INFO("Dop parameters: \n");
 
// Set the DOP resolution
if (LE_GNSS_RES_UNKNOWN == ++DopRes)
{
}
LE_INFO("Set DOP resolution: %d decimal place\n", DopRes);
 
do
{
// Get DOP parameter
result = le_gnss_GetDilutionOfPrecision(positionSampleRef, dopType, &dop);
LE_ASSERT((result == LE_OK) || (result == LE_OUT_OF_RANGE));
if (LE_OK == result)
{
switch(DopRes)
{
LE_INFO("resolution: %d decimal place, %s %.1f\n",
DopRes, tabDop[dopType], (float)dop);
break;
LE_INFO("resolution: %d decimal place, %s %.1f\n",
DopRes, tabDop[dopType], (float)(dop)/10);
break;
LE_INFO("resolution: %d decimal place, %s %.2f\n",
DopRes, tabDop[dopType], (float)(dop)/100);
break;
default:
LE_INFO("resolution: %d decimal place, %s %.3f\n",
DopRes, tabDop[dopType], (float)(dop)/1000);
break;
}
}
else
{
LE_INFO("%s invalid %d\n", tabDop[dopType], dop);
}
dopType++;
}
while (dopType != LE_GNSS_DOP_LAST);
 
// Get horizontal speed
LE_INFO("Test SetDataResolution() for hSpeedAccuracy parameter of le_gnss_GetHorizontalSpeed() \
function");
 
for (dataRes=LE_GNSS_RES_ZERO_DECIMAL; dataRes<LE_GNSS_RES_UNKNOWN; dataRes++)
{
result = le_gnss_GetHorizontalSpeed( positionSampleRef, &hSpeed, &hSpeedAccuracy);
LE_ASSERT((LE_OK == result) || (LE_OUT_OF_RANGE == result));
 
if (LE_OK == result)
{
switch(dataRes)
{
LE_INFO("Resolution: %d decimal place, hSpeed %u - Accuracy %.3f",
dataRes, hSpeed/100, (float)hSpeedAccuracy);
break;
LE_INFO("Resolution: %d decimal place, hSpeed %u - Accuracy %.3f",
dataRes, hSpeed/100, (float)hSpeedAccuracy/10);
break;
LE_INFO("Resolution: %d decimal place, hSpeed %u - Accuracy %.3f",
dataRes, hSpeed/100, (float)hSpeedAccuracy/100);
break;
LE_INFO("Resolution: %d decimal place, hSpeed %u - Accuracy %.3f",
dataRes, hSpeed/100, (float)hSpeedAccuracy/1000);
break;
default:
LE_INFO("Unknown resolution.");
break;
}
}
else
{
LE_INFO("hSpeed unknown [%u,%.3f]", hSpeed, (float)hSpeedAccuracy);
}
}
 
// Get vertical speed
LE_INFO("Test SetDataResolution() for vSpeedAccuracy parameter of le_gnss_GetVerticalSpeed() \
function");
 
for (dataRes=LE_GNSS_RES_ZERO_DECIMAL; dataRes<LE_GNSS_RES_UNKNOWN; dataRes++)
{
result = le_gnss_GetVerticalSpeed( positionSampleRef, &vSpeed, &vSpeedAccuracy);
LE_ASSERT((LE_OK == result) || (LE_OUT_OF_RANGE == result));
 
if (LE_OK == result)
{
switch(dataRes)
{
LE_INFO("Resolution: %d decimal place, vSpeed %d - Accuracy %.3f",
dataRes, vSpeed/100, (float)vSpeedAccuracy);
break;
LE_INFO("Resolution: %d decimal place, vSpeed %d - Accuracy %.3f",
dataRes, vSpeed/100, (float)vSpeedAccuracy/10);
break;
LE_INFO("Resolution: %d decimal place, vSpeed %d - Accuracy %.3f",
dataRes, vSpeed/100, (float)vSpeedAccuracy/100);
break;
LE_INFO("Resolution: %d decimal place, vSpeed %d - Accuracy %.3f",
dataRes, vSpeed/100, (float)vSpeedAccuracy/1000);
break;
default:
LE_INFO("Unknown resolution.");
break;
}
}
else
{
LE_INFO("vSpeed unknown [%d,%.3f]", vSpeed, (float)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 = NUM_ARRAY_MEMBERS(satIdPtr);
size_t satConstNumElements = NUM_ARRAY_MEMBERS(satConstPtr);
bool satUsedPtr[LE_GNSS_SV_INFO_MAX_LEN];
size_t satUsedNumElements = NUM_ARRAY_MEMBERS(satUsedPtr);
uint8_t satSnrPtr[LE_GNSS_SV_INFO_MAX_LEN];
size_t satSnrNumElements = NUM_ARRAY_MEMBERS(satSnrPtr);
uint16_t satAzimPtr[LE_GNSS_SV_INFO_MAX_LEN];
size_t satAzimNumElements = NUM_ARRAY_MEMBERS(satAzimPtr);
uint8_t satElevPtr[LE_GNSS_SV_INFO_MAX_LEN];
size_t satElevNumElements = NUM_ARRAY_MEMBERS(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;
result = le_gnss_SetMinElevation(minElevation);
LE_ASSERT((LE_OK == result) || (LE_OUT_OF_RANGE == result));
if (LE_OK == result)
{
LE_INFO("Set minElevation %d",minElevation);
}
 
// Test le_gnss_SetDataResolution() before starting GNSS
LE_INFO("Sanity test for le_gnss_SetDataResolution");
LE_INFO("Start GNSS");
LE_INFO("Wait 5 seconds");
sleep(5);
 
// Test le_gnss_SetDataResolution() after starting GNSS
LE_GNSS_RES_UNKNOWN));
 
// 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 EpochTime %llu",TimeAccuracy, (unsigned long long int)EpochTime);
 
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");
 
 
// first test in ConvertDop() in le_gnss.c to find the default resolution.
// Test that the chosen resolution in PositionHandlerFunction is LE_GNSS_RES_THREE_DECIMAL
 
// 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;
}