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
    le_gnss_FixState_t 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;
    le_gnss_DopType_t dopType = LE_GNSS_PDOP;
    le_gnss_Resolution_t dataRes = LE_GNSS_RES_ZERO_DECIMAL;
 
    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);
 
    LE_ASSERT_OK(le_gnss_InjectUtcTime(EpochTime , 0));
 
    // 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
        result = le_gnss_ConvertDataCoordinateSystem(LE_GNSS_COORDINATE_SYSTEM_WGS84,
                                                     LE_GNSS_COORDINATE_SYSTEM_PZ90,
                                                     LE_GNSS_POS_LATITUDE,
                                                     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
        result = le_gnss_ConvertDataCoordinateSystem(LE_GNSS_COORDINATE_SYSTEM_WGS84,
                                                     LE_GNSS_COORDINATE_SYSTEM_PZ90,
                                                     LE_GNSS_POS_LONGITUDE,
                                                     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++)
    {
        LE_ASSERT_OK(le_gnss_SetDataResolution(LE_GNSS_DATA_VACCURACY, dataRes));
        result = le_gnss_GetAltitude( positionSampleRef, &altitude, &vAccuracy);
        LE_ASSERT((LE_OK == result) || (LE_OUT_OF_RANGE == result));
 
        if (LE_OK == result)
        {
            switch(dataRes)
            {
                case LE_GNSS_RES_ZERO_DECIMAL:
                    LE_INFO("Resolution: %d decimal place, altitude.%f, vAccuracy.%f",
                            dataRes, (float)altitude/1000.0, (float)vAccuracy);
                    break;
                case LE_GNSS_RES_ONE_DECIMAL:
                    LE_INFO("Resolution: %d decimal place, altitude.%f, vAccuracy.%f",
                            dataRes, (float)altitude/1000.0, (float)vAccuracy/10.0);
                    break;
                case LE_GNSS_RES_TWO_DECIMAL:
                    LE_INFO("Resolution: %d decimal place, altitude.%f, vAccuracy.%f",
                            dataRes, (float)altitude/1000.0, (float)vAccuracy/100.0);
                    break;
                case LE_GNSS_RES_THREE_DECIMAL:
                    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);
 
        result = le_gnss_ConvertDataCoordinateSystem(LE_GNSS_COORDINATE_SYSTEM_WGS84,
                                                     LE_GNSS_COORDINATE_SYSTEM_PZ90,
                                                     LE_GNSS_POS_ALTITUDE,
                                                     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)
    {
        DopRes = LE_GNSS_RES_ZERO_DECIMAL;
    }
    LE_ASSERT_OK(le_gnss_SetDopResolution(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)
            {
                case LE_GNSS_RES_ZERO_DECIMAL:
                     LE_INFO("resolution: %d decimal place, %s %.1f\n",
                            DopRes, tabDop[dopType], (float)dop);
                     break;
                case LE_GNSS_RES_ONE_DECIMAL:
                     LE_INFO("resolution: %d decimal place, %s %.1f\n",
                            DopRes, tabDop[dopType], (float)(dop)/10);
                     break;
                case LE_GNSS_RES_TWO_DECIMAL:
                     LE_INFO("resolution: %d decimal place, %s %.2f\n",
                            DopRes, tabDop[dopType], (float)(dop)/100);
                     break;
                case LE_GNSS_RES_THREE_DECIMAL:
                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++)
    {
        LE_ASSERT_OK(le_gnss_SetDataResolution(LE_GNSS_DATA_HSPEEDACCURACY, dataRes));
        result = le_gnss_GetHorizontalSpeed( positionSampleRef, &hSpeed, &hSpeedAccuracy);
        LE_ASSERT((LE_OK == result) || (LE_OUT_OF_RANGE == result));
 
        if (LE_OK == result)
        {
            switch(dataRes)
            {
                case LE_GNSS_RES_ZERO_DECIMAL:
                    LE_INFO("Resolution: %d decimal place, hSpeed %u - Accuracy %.3f",
                            dataRes, hSpeed/100, (float)hSpeedAccuracy);
                    break;
                case LE_GNSS_RES_ONE_DECIMAL:
                    LE_INFO("Resolution: %d decimal place, hSpeed %u - Accuracy %.3f",
                            dataRes, hSpeed/100, (float)hSpeedAccuracy/10);
                    break;
                case LE_GNSS_RES_TWO_DECIMAL:
                    LE_INFO("Resolution: %d decimal place, hSpeed %u - Accuracy %.3f",
                            dataRes, hSpeed/100, (float)hSpeedAccuracy/100);
                    break;
                case LE_GNSS_RES_THREE_DECIMAL:
                    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++)
    {
        LE_ASSERT_OK(le_gnss_SetDataResolution(LE_GNSS_DATA_VSPEEDACCURACY, dataRes));
        result = le_gnss_GetVerticalSpeed( positionSampleRef, &vSpeed, &vSpeedAccuracy);
        LE_ASSERT((LE_OK == result) || (LE_OUT_OF_RANGE == result));
 
        if (LE_OK == result)
        {
            switch(dataRes)
            {
                case LE_GNSS_RES_ZERO_DECIMAL:
                    LE_INFO("Resolution: %d decimal place, vSpeed %d - Accuracy %.3f",
                            dataRes, vSpeed/100, (float)vSpeedAccuracy);
                    break;
                case LE_GNSS_RES_ONE_DECIMAL:
                    LE_INFO("Resolution: %d decimal place, vSpeed %d - Accuracy %.3f",
                            dataRes, vSpeed/100, (float)vSpeedAccuracy/10);
                    break;
                case LE_GNSS_RES_TWO_DECIMAL:
                    LE_INFO("Resolution: %d decimal place, vSpeed %d - Accuracy %.3f",
                            dataRes, vSpeed/100, (float)vSpeedAccuracy/100);
                    break;
                case LE_GNSS_RES_THREE_DECIMAL:
                    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);
    le_gnss_Constellation_t satConstPtr[LE_GNSS_SV_INFO_MAX_LEN];
    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_gnss_ConnectService();
 
    LE_INFO("======== Position Handler thread  ========");
    PositionHandlerRef = le_gnss_AddPositionHandler(PositionHandlerFunction, NULL);
    LE_ASSERT((PositionHandlerRef != NULL));
 
    le_event_RunLoop();
    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_ASSERT(LE_BAD_PARAMETER == le_gnss_SetDataResolution(LE_GNSS_DATA_UNKNOWN,
                                                            LE_GNSS_RES_ONE_DECIMAL));
    LE_INFO("Start GNSS");
    LE_ASSERT_OK(le_gnss_Start());
    LE_INFO("Wait 5 seconds");
    sleep(5);
 
    // Test le_gnss_SetDataResolution() after starting GNSS
    LE_ASSERT(LE_BAD_PARAMETER == le_gnss_SetDataResolution(LE_GNSS_DATA_VACCURACY,
                                                            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");
    LE_ASSERT_OK(le_gnss_ForceColdRestart());
 
    // 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");
 
    LE_ASSERT(LE_BAD_PARAMETER == le_gnss_SetDopResolution(LE_GNSS_RES_UNKNOWN));
 
    // 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");
    LE_ASSERT_OK(le_gnss_Stop());
    EpochTime=0;
    TimeAccuracy=0;
}