rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-01 09:24:10 +01:00
Code Issues Releases Activity

OP-116 AHRS: Made data going to the AHRS in NED format. Once the final estimate object is created I'll repeat the same calculation to populate it, although we want to avoid going back to LLA format on the OP because the CPU requirements are excessive.

Browse Source 
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1376 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124 2010-08-22 19:01:36 +00:00 committed by peabody124
parent 6adc834d9b
commit 2e705e6836
2 changed files with 22 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
flight/OpenPilot/Modules/AHRSComms/ahrs_comms.c
View File

@ -61,6 +61,7 @@
#include "positionactual.h" #include "positionactual.h"
#include "homelocation.h" #include "homelocation.h"
#include "ahrscalibration.h" #include "ahrscalibration.h"
#include "CoordinateConversions.h"
#include "pios_opahrs.h" // library for OpenPilot AHRS access functions #include "pios_opahrs.h" // library for OpenPilot AHRS access functions
#include "pios_opahrs_proto.h" #include "pios_opahrs_proto.h"
@ -364,8 +365,10 @@ static void load_gps_position(struct opahrs_msg_v1_req_update * update)
HomeLocationData home; HomeLocationData home;
HomeLocationGet(&home); HomeLocationGet(&home);
update->gps.updated = 1; update->gps.updated = 1;
if(home.RNE[0] == 0 && home.RNE[1] && home.RNE[2] == 0 && home.RNE[3] == 0)
{
update->gps.NED[0] = 0; update->gps.NED[0] = 0;
update->gps.NED[1] = 0; update->gps.NED[1] = 0;
update->gps.NED[2] = 0; update->gps.NED[2] = 0;
@ -373,6 +376,16 @@ static void load_gps_position(struct opahrs_msg_v1_req_update * update)
update->gps.heading = 0; update->gps.heading = 0;
update->gps.quality = 0; update->gps.quality = 0;
} }
else {
update->gps.groundspeed = data.Groundspeed;
update->gps.heading = data.Heading;
update->gps.quality = 0;
double LLA[3] = {(double) data.Latitude, (double) data.Longitude, (double) data.Altitude};
double ECEF[3] = {(double) home.ECEF[0], (double) home.ECEF[1], (double) home.ECEF[2]};
LLA2Base(LLA, ECEF, (float (*)[3]) home.RNE, update->gps.NED);
}
}
static void process_update(struct opahrs_msg_v1_rsp_update * update) static void process_update(struct opahrs_msg_v1_rsp_update * update)
{ {

5
flight/OpenPilot/Modules/GPS/GPS.c
View File

@ -185,16 +185,13 @@ static void setHomeLocation(PositionActualData * gpsData)
// Compute home ECEF coordinates and the rotation matrix into NED // 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 LLA[3] = {(double) home.Latitude / 10e6, (double) home.Longitude / 10e6, (double) home.Altitude};
double ECEF[3]; double ECEF[3];
float RNE[3][3]; RneFromLLA(LLA, (float (*)[3]) home.RNE);
RneFromLLA(LLA, RNE);
LLA2ECEF(LLA, ECEF); LLA2ECEF(LLA, ECEF);
// TODO: Currently UAVTalk only supports float but these conversions use double // 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 // need to find out if they require that precision and if so extend UAVTAlk
home.ECEF[0] = ECEF[0]; home.ECEF[0] = ECEF[0];
home.ECEF[1] = ECEF[1]; home.ECEF[1] = ECEF[1];
home.ECEF[2] = ECEF[2]; home.ECEF[2] = ECEF[2];
// Can't figure out how to directly cast home.RNE (float *) to a float[3][3]
memcpy(&home.RNE[0], &RNE[0][0], 9 * sizeof(RNE[0][0]));
// Compute magnetic flux direction at home location // Compute magnetic flux direction at home location
WMM_GetMagVector(LLA[0], LLA[1], LLA[2], 8, 17, 2010, &home.Be[0]); WMM_GetMagVector(LLA[0], LLA[1], LLA[2], 8, 17, 2010, &home.Be[0]);