Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 02:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF: enable airspeed and fix getLLH()

Browse Source
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 -*- /// -*- 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_HAL.h>
#include "AP_NavEKF.h" #include "AP_NavEKF.h"
@ -20,7 +20,7 @@ extern const AP_HAL::HAL& hal;
NavEKF::NavEKF(const AP_AHRS &ahrs, AP_Baro &baro) : NavEKF::NavEKF(const AP_AHRS &ahrs, AP_Baro &baro) :
_ahrs(ahrs), _ahrs(ahrs),
_baro(baro), _baro(baro),
useAirspeed(false), useAirspeed(true),
useCompass(true), useCompass(true),
fusionModeGPS(1), // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity 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 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 // Update states using GPS, altimeter, compass and airspeed observations
SelectVelPosFusion(); SelectVelPosFusion();
SelectMagFusion(); SelectMagFusion();
//SelectTasFusion(); SelectTasFusion();
} }
} }
@ -1887,14 +1887,13 @@ void NavEKF::calcposNE(float lat, float lon)
posNE[1] = RADIUS_OF_EARTH * cosf(latRef) * (lon - lonRef); posNE[1] = RADIUS_OF_EARTH * cosf(latRef) * (lon - lonRef);
} }
#if 0 bool NavEKF::getLLH(struct Location &loc)
void NavEKF::calcLLH(float &lat, float &lon, float &hgt)
{ {
lat = latRef + posNED[0] / RADIUS_OF_EARTH; loc.lat = 1.0e7 * degrees(latRef + states[7] / RADIUS_OF_EARTH);
lon = lonRef + (posNED[1] / RADIUS_OF_EARTH) / cosf(latRef); loc.lng = 1.0e7 * degrees(lonRef + (states[8] / RADIUS_OF_EARTH) / cosf(latRef));
hgt = hgtRef - posNED[2]; loc.alt = 1.0e2 * (hgtRef - states[9]);
return true;
} }
#endif
void NavEKF::OnGroundCheck() 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) Vector3f velNED; // North, East, Down velocity measurements (m/s)
Vector2 posNE; // North, East position measurements (m) Vector2 posNE; // North, East position measurements (m)
float hgtMea; // height measurement relative to reference point (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 statesAtVelTime; // States at the effective time of velNED measurements
Vector24 statesAtPosTime; // States at the effective time of posNE measurements Vector24 statesAtPosTime; // States at the effective time of posNE measurements
Vector24 statesAtHgtTime; // States at the effective time of hgtMea measurement Vector24 statesAtHgtTime; // States at the effective time of hgtMea measurement