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); } } }
void CoordinateConversions::LLA2Base(double LLA[3], double BaseECEF[3], float Rne[3][3], float NED[3]) { double ECEF[3]; float diff[3]; LLA2ECEF(LLA, ECEF); diff[0] = (float)(ECEF[0] - BaseECEF[0]); diff[1] = (float)(ECEF[1] - BaseECEF[1]); diff[2] = (float)(ECEF[2] - BaseECEF[2]); NED[0] = Rne[0][0] * diff[0] + Rne[0][1] * diff[1] + Rne[0][2] * diff[2]; NED[1] = Rne[1][0] * diff[0] + Rne[1][1] * diff[1] + Rne[1][2] * diff[2]; NED[2] = Rne[2][0] * diff[0] + Rne[2][1] * diff[1] + Rne[2][2] * diff[2]; }
// ****** Express LLA in a local NED Base Frame ******** void LLA2Base(float LLA[3], float BaseECEF[3], float Rne[3][3], float NED[3]) { float ECEF[3]; float diff[3]; LLA2ECEF(LLA, ECEF); diff[0] = (float)(ECEF[0] - BaseECEF[0]); diff[1] = (float)(ECEF[1] - BaseECEF[1]); diff[2] = (float)(ECEF[2] - BaseECEF[2]); NED[0] = Rne[0][0] * diff[0] + Rne[0][1] * diff[1] + Rne[0][2] * diff[2]; NED[1] = Rne[1][0] * diff[0] + Rne[1][1] * diff[1] + Rne[1][2] * diff[2]; NED[2] = Rne[2][0] * diff[0] + Rne[2][1] * diff[1] + Rne[2][2] * diff[2]; }