Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF: enable airspeed and fix getLLH()

This commit is contained in:
Andrew Tridgell 2013-12-30 16:19:50 +11:00
parent 1fbae4dd44
commit 163cfd839d
2 changed files with 8 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -1,6 +1,6 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#define MATH_CHECK_INDEXES 1
// #define MATH_CHECK_INDEXES 1
#include <AP_HAL.h>
#include "AP_NavEKF.h"
@ -20,7 +20,7 @@ extern const AP_HAL::HAL& hal;
NavEKF::NavEKF(const AP_AHRS &ahrs, AP_Baro &baro) :
_ahrs(ahrs),
_baro(baro),
useAirspeed(false),
useAirspeed(true),
useCompass(true),
fusionModeGPS(1), // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity
covTimeStepMax(0.07f), // maximum time (sec) between covariance prediction updates
@ -172,7 +172,7 @@ void NavEKF::UpdateFilter()
// Update states using GPS, altimeter, compass and airspeed observations
SelectVelPosFusion();
SelectMagFusion();
//SelectTasFusion();
SelectTasFusion();
}
}
@ -1887,14 +1887,13 @@ void NavEKF::calcposNE(float lat, float lon)
posNE[1] = RADIUS_OF_EARTH * cosf(latRef) * (lon - lonRef);
}
#if 0
void NavEKF::calcLLH(float &lat, float &lon, float &hgt)
bool NavEKF::getLLH(struct Location &loc)
{
lat = latRef + posNED[0] / RADIUS_OF_EARTH;
lon = lonRef + (posNED[1] / RADIUS_OF_EARTH) / cosf(latRef);
hgt = hgtRef - posNED[2];
loc.lat = 1.0e7 * degrees(latRef + states[7] / RADIUS_OF_EARTH);
loc.lng = 1.0e7 * degrees(lonRef + (states[8] / RADIUS_OF_EARTH) / cosf(latRef));
loc.alt = 1.0e2 * (hgtRef - states[9]);
return true;
}
#endif
void NavEKF::OnGroundCheck()
{

1
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -171,7 +171,6 @@ private:
Vector3f velNED; // North, East, Down velocity measurements (m/s)
Vector2 posNE; // North, East position measurements (m)
float hgtMea; // height measurement relative to reference point (m)
Vector3f posNED; // North, East Down position relative to reference point (m)
Vector24 statesAtVelTime; // States at the effective time of velNED measurements
Vector24 statesAtPosTime; // States at the effective time of posNE measurements
Vector24 statesAtHgtTime; // States at the effective time of hgtMea measurement