static void setHomeLocation(GPSPositionData * gpsData)
{
HomeLocationData home;
HomeLocationGet(&home);
GPSTimeData gps;
GPSTimeGet(&gps);
if (gps.Year >= 2000)
{
// Store LLA
home.Latitude = gpsData->Latitude;
home.Longitude = gpsData->Longitude;
home.Altitude = gpsData->Altitude + gpsData->GeoidSeparation;
// Compute home ECEF coordinates and the rotation matrix into NED
double LLA[3] = { ((double)home.Latitude) / 10e6, ((double)home.Longitude) / 10e6, ((double)home.Altitude) };
double ECEF[3];
RneFromLLA(LLA, (float (*)[3])home.RNE);
LLA2ECEF(LLA, ECEF);
// TODO: Currently UAVTalk only supports float but these conversions use double
// need to find out if they require that precision and if so extend UAVTAlk
home.ECEF[0] = (int32_t) (ECEF[0] * 100);
home.ECEF[1] = (int32_t) (ECEF[1] * 100);
home.ECEF[2] = (int32_t) (ECEF[2] * 100);
// Compute magnetic flux direction at home location
if (WMM_GetMagVector(LLA[0], LLA[1], LLA[2], gps.Month, gps.Day, gps.Year, &home.Be[0]) >= 0)
{ // calculations appeared to go OK
home.Set = HOMELOCATION_SET_TRUE;
HomeLocationSet(&home);
}
}
}
/**
* Get the current location in Longitude, Latitude Altitude (above WSG-84 ellipsoid)
* @param[in] BaseECEF the ECEF of the home location (in m)
* @param[in] NED the offset from the home location (in m)
* @param[out] position three element double for position in decimal degrees and altitude in meters
* @returns
* @arg 0 success
* @arg -1 for failure
*/
int CoordinateConversions::NED2LLA_HomeECEF(double BaseECEFm[3], double NED[3], double position[3])
{
int i;
// stored value is in cm, convert to m
double BaseLLA[3];
double ECEF[3];
double Rne [3][3];
// Get LLA address to compute conversion matrix
ECEF2LLA(BaseECEFm, BaseLLA);
RneFromLLA(BaseLLA, Rne);
/* P = ECEF + Rne' * NED */
for(i = 0; i < 3; i++)
ECEF[i] = BaseECEFm[i] + Rne[0][i]*NED[0] + Rne[1][i]*NED[1] + Rne[2][i]*NED[2];
ECEF2LLA(ECEF,position);
return 0;
}
