aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF: Added constructor for measurements

Browse Source
This commit is contained in:
Paul Riseborough 2013-12-29 10:17:23 +11:00 committed by Andrew Tridgell
parent 4a7f81e50a
commit 1e993d2ef5
2 changed files with 20 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
libraries/AP_NavEKF/NavEKF.cpp
View File

@ -32,7 +32,7 @@ void InitialiseFilter()
{
latRef = gpsLat;
lonRef = gpsLon
hgtRef = barometer.get_altitude();
hgtRef = _baro->get_altitude();
}
// write to state vector
@ -1938,18 +1938,18 @@ void readGpsData()
{
static uint64_t lastFixTime=0;
if ((g_gps->last_fix_time > g_gps->last_fix_time) && (g_gps->status() >= GPS::GPS_OK_FIX_3D))
if ((_gps->last_fix_time > _gps->last_fix_time) && (_gps->status() >= GPS::GPS_OK_FIX_3D))
{
GPSstatus = g_gps->status();
GPSstatus = _gps->status();
newDataGps = true;
RecallStates(statesAtVelTime, (IMUmsec - msecVelDelay));
RecallStates(statesAtPosTime, (IMUmsec - msecPosDelay));
velNED[0] = g_gps->velocity_north(); // (rad)
velNED[1] = g_gps->velocity_east(); // (m/s)
velNED[2] = g_gps->velocity_down(); // (m/s)
gpsLat = deg2rad*1e-7*g_gps->latitude(); // (rad)
gpsLon = deg2rad*1e-7*g_gps->longitude(); //(rad)
gpsHgt = 0.01*g_gps->altitude_cm(); // (m)
velNED[0] = _gps->velocity_north(); // (rad)
velNED[1] = _gps->velocity_east(); // (m/s)
velNED[2] = _gps->velocity_down(); // (m/s)
gpsLat = deg2rad*1e-7*_gps->latitude(); // (rad)
gpsLon = deg2rad*1e-7*_gps->longitude(); //(rad)
gpsHgt = 0.01*_gps->altitude_cm(); // (m)
// Convert GPS measurements to Pos NE
calcposNE(posNE, gpsLat, gpsLon, latRef, lonRef);
}
@ -1959,7 +1959,7 @@ void readHgtData()
{
// ToDo do we check for new height data or grab a height measurement?
// Best to do this at the same time as GPS measurement fusion for efficiency
hgtMea = barometer.get_altitude() - hgtRef;
hgtMea = _baro->get_altitude() - hgtRef;
// recall states from compass measurement time
if(newDataHgt) RecallStates(statesAtVelTime, (IMUmsec - msecHgtDelay));
}
@ -1967,8 +1967,8 @@ void readHgtData()
void readMagData()
{
// scale compass data to improve numerical conditioning
magData = 0.001*compass.get_field();
magBias = 0.001*compass.get_offsets();
magData = 0.001*_compass->get_field();
magBias = 0.001*_compass->get_offsets();
// Recall states from compass measurement time
RecallStates(statesAtMagMeasTime, (IMUmsec - msecMagDelay));
}

14
libraries/AP_NavEKF/NavEKF.h
View File

@ -23,18 +23,20 @@
#include <AP_Math.h>
#include <AP_AHRS.h>
#include <AP_Airspeed.h>
#include <AP_InertialSensor.h> // ArduPilot Mega IMU Library
#include <AP_Baro.h> // ArduPilot Mega Barometer Library
#include <AP_AHRS.h>
#include <AP_Airspeed.h>
class NavEKF
{
public:
// Constructor - don't know how to do this
//AP_InertialNav( const AP_AHRS* ahrs, AP_Baro* baro, GPS*& gps) :
// _ahrs(ahrs),
// _baro(baro),
// _gps(gps)
// Constructor
AP_InertialNav( const AP_AHRS* ahrs, AP_Baro* baro, GPS*& gps) :
_ahrs(ahrs),
_baro(baro),
_gps(gps)
// Initialise the filter states from the AHRS and magnetometer data (if present)
void InitialiseFilter();