zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_NavEKF: make it possible for NavEKF to be a AHRS member 

ready for AHRS integration
mission-4.1.18
Andrew Tridgell 11 years ago
parent
6f31961fb5
commit
2c86a490ed
2 changed files with 65 additions and 63 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 78
      libraries/AP_NavEKF/AP_NavEKF.cpp
  2. 50
      libraries/AP_NavEKF/AP_NavEKF.h

78
libraries/AP_NavEKF/AP_NavEKF.cpp
Unescape View File

@ -9,6 +9,8 @@ @@ -9,6 +9,8 @@
#pragma GCC optimize("O3")
#include "AP_NavEKF.h"
#include <AP_AHRS.h>
//#include <stdio.h>
extern const AP_HAL::HAL& hal;
@ -16,19 +18,19 @@ extern const AP_HAL::HAL& hal; @@ -16,19 +18,19 @@ extern const AP_HAL::HAL& hal;
#define earthRate 0.000072921f
// constructor
NavEKF::NavEKF(const AP_AHRS &ahrs, AP_Baro &baro) :
NavEKF::NavEKF(const AP_AHRS *ahrs, AP_Baro &baro) :
_ahrs(ahrs),
_baro(baro),
useAirspeed(true),
useCompass(true),
fusionModeGPS(0), // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity
fuseMeNow(true), // forces fusion to occur on the IMU frame that data arrives
covTimeStepMax(0.07f), // maximum time (sec) between covariance prediction updates
covDelAngMax(0.05f), // maximum delta angle between covariance prediction updates
yawVarScale(10.0f), // scale factor applied to yaw gyro errors when on ground
TASmsecMax(333), // maximum allowed interal between airspeed measurement updates
MAGmsecMax(333), // maximum allowed interval between magnetometer measurement updates
HGTmsecMax(1000), // maximum interval between height measurement updates
fuseMeNow(true), // forces fusion to occur on the IMU frame that data arrives
msecVelDelay(230), // msec of GPS velocity delay
msecPosDelay(210), // msec of GPS position delay
msecHgtDelay(350), // msec of barometric height delay
@ -66,9 +68,9 @@ void NavEKF::InitialiseFilter(void) @@ -66,9 +68,9 @@ void NavEKF::InitialiseFilter(void)
{
// Calculate initial filter quaternion states from ahrs solution
Quaternion initQuat;
ahrsEul[0] = _ahrs.roll;
ahrsEul[1] = _ahrs.pitch;
ahrsEul[2] = _ahrs.yaw;
ahrsEul[0] = _ahrs->roll;
ahrsEul[1] = _ahrs->pitch;
ahrsEul[2] = _ahrs->yaw;
eul2quat(initQuat, ahrsEul);
// Calculate initial Tbn matrix and rotate Mag measurements into NED
@ -1685,7 +1687,7 @@ void NavEKF::FuseAirspeed() @@ -1685,7 +1687,7 @@ void NavEKF::FuseAirspeed()
float vd;
float vwn;
float vwe;
float EAS2TAS = _ahrs.get_EAS2TAS();
float EAS2TAS = _ahrs->get_EAS2TAS();
const float R_TAS = _easVar*sq(constrain_float(EAS2TAS,1.0f,10.0f));
Vector3f SH_TAS;
float SK_TAS;
@ -1860,7 +1862,7 @@ void NavEKF::RecallStates(Vector24 &statesForFusion, uint32_t msec) @@ -1860,7 +1862,7 @@ void NavEKF::RecallStates(Vector24 &statesForFusion, uint32_t msec)
}
}
void NavEKF::quat2Tnb(Matrix3f &Tnb, const Quaternion &quat)
void NavEKF::quat2Tnb(Matrix3f &Tnb, const Quaternion &quat) const
{
// Calculate the nav to body cosine matrix
float q00 = sq(quat[0]);
@ -1885,13 +1887,13 @@ void NavEKF::quat2Tnb(Matrix3f &Tnb, const Quaternion &quat) @@ -1885,13 +1887,13 @@ void NavEKF::quat2Tnb(Matrix3f &Tnb, const Quaternion &quat)
Tnb.b.z = 2*(q23 + q01);
}
void NavEKF::quat2Tbn(Matrix3f &Tbn, const Quaternion &quat)
void NavEKF::quat2Tbn(Matrix3f &Tbn, const Quaternion &quat) const
{
// Calculate the body to nav cosine matrix
quat.rotation_matrix(Tbn);
}
void NavEKF::eul2quat(Quaternion &quat, const Vector3f &eul)
void NavEKF::eul2quat(Quaternion &quat, const Vector3f &eul) const
{
float u1 = cosf(0.5f*eul[0]);
float u2 = cosf(0.5f*eul[1]);
@ -1905,27 +1907,27 @@ void NavEKF::eul2quat(Quaternion &quat, const Vector3f &eul) @@ -1905,27 +1907,27 @@ void NavEKF::eul2quat(Quaternion &quat, const Vector3f &eul)
quat[3] = u1*u2*u6-u4*u5*u3;
}
void NavEKF::quat2eul(Vector3f &y, const Quaternion &u)
void NavEKF::quat2eul(Vector3f &y, const Quaternion &u) const
{
y[0] = atan2f((2*(u[2]*u[3]+u[0]*u[1])) , (u[0]*u[0]-u[1]*u[1]-u[2]*u[2]+u[3]*u[3]));
y[1] = -asinf(2*(u[1]*u[3]-u[0]*u[2]));
y[2] = atan2f((2*(u[1]*u[2]+u[0]*u[3])) , (u[0]*u[0]+u[1]*u[1]-u[2]*u[2]-u[3]*u[3]));
}
void NavEKF::getEulerAngles(Vector3f &euler)
void NavEKF::getEulerAngles(Vector3f &euler) const
{
Quaternion q(states[0], states[1], states[2], states[3]);
quat2eul(euler, q);
}
void NavEKF::getVelNED(Vector3f &vel)
void NavEKF::getVelNED(Vector3f &vel) const
{
vel.x = states[4];
vel.y = states[5];
vel.z = states[6];
}
bool NavEKF::getPosNED(Vector3f &pos)
bool NavEKF::getPosNED(Vector3f &pos) const
{
pos.x = states[7];
pos.y = states[8];
@ -1933,35 +1935,35 @@ bool NavEKF::getPosNED(Vector3f &pos) @@ -1933,35 +1935,35 @@ bool NavEKF::getPosNED(Vector3f &pos)
return true;
}
void NavEKF::getGyroBias(Vector3f &gyroBias)
void NavEKF::getGyroBias(Vector3f &gyroBias) const
{
gyroBias.x = states[10]*60.0f*RAD_TO_DEG*dtIMUAvgInv;
gyroBias.y = states[11]*60.0f*RAD_TO_DEG*dtIMUAvgInv;
gyroBias.z = states[12]*60.0f*RAD_TO_DEG*dtIMUAvgInv;
}
void NavEKF::getAccelBias(Vector3f &accelBias)
void NavEKF::getAccelBias(Vector3f &accelBias) const
{
accelBias.x = states[13]*dtIMUAvgInv;
accelBias.y = states[14]*dtIMUAvgInv;
accelBias.z = states[15]*dtIMUAvgInv;
}
void NavEKF::getWind(Vector3f &wind)
void NavEKF::getWind(Vector3f &wind) const
{
wind.x = states[16];
wind.y = states[17];
wind.z = 0.0f; // curently don't estimate this
}
void NavEKF::getMagNED(Vector3f &magNED)
void NavEKF::getMagNED(Vector3f &magNED) const
{
magNED.x = states[18]*1000.0f;
magNED.y = states[19]*1000.0f;
magNED.z = states[20]*1000.0f;
}
void NavEKF::getMagXYZ(Vector3f &magXYZ)
void NavEKF::getMagXYZ(Vector3f &magXYZ) const
{
magXYZ.x = states[21]*1000.0f;
magXYZ.y = states[22]*1000.0f;
@ -1974,7 +1976,7 @@ void NavEKF::calcposNE(float lat, float lon) @@ -1974,7 +1976,7 @@ void NavEKF::calcposNE(float lat, float lon)
posNE[1] = RADIUS_OF_EARTH * cosf(latRef) * (lon - lonRef);
}
bool NavEKF::getLLH(struct Location &loc)
bool NavEKF::getLLH(struct Location &loc) const
{
loc.lat = 1.0e7f * degrees(latRef + states[7] / RADIUS_OF_EARTH);
loc.lng = 1.0e7f * degrees(lonRef + (states[8] / RADIUS_OF_EARTH) / cosf(latRef));
@ -1987,7 +1989,7 @@ void NavEKF::OnGroundCheck() @@ -1987,7 +1989,7 @@ void NavEKF::OnGroundCheck()
uint8_t lowAirSpd;
uint8_t lowGndSpd;
uint8_t lowHgt;
lowAirSpd = (uint8_t)(_ahrs.airspeed_estimate_true(&VtasMeas) < 8.0f);
lowAirSpd = (uint8_t)(_ahrs->airspeed_estimate_true(&VtasMeas) < 8.0f);
lowGndSpd = (uint8_t)((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f);
lowHgt = (uint8_t)(hgtMea < 15.0f);
// Go with a majority vote from three criteria
@ -2032,8 +2034,8 @@ void NavEKF::readIMUData() @@ -2032,8 +2034,8 @@ void NavEKF::readIMUData()
IMUmsec = IMUusec/1000;
dtIMU = 1.0e-6f * (IMUusec - lastIMUusec);
lastIMUusec = IMUusec;
angRate = _ahrs.get_ins().get_gyro();
accel = _ahrs.get_ins().get_accel();
angRate = _ahrs->get_ins().get_gyro();
accel = _ahrs->get_ins().get_accel();
dAngIMU = (angRate + lastAngRate) * dtIMU * 0.5f;
lastAngRate = angRate;
@ -2043,19 +2045,19 @@ void NavEKF::readIMUData() @@ -2043,19 +2045,19 @@ void NavEKF::readIMUData()
void NavEKF::readGpsData()
{
if ((_ahrs.get_gps()->last_message_time_ms() != lastFixTime) &&
(_ahrs.get_gps()->status() >= GPS::GPS_OK_FIX_3D))
if ((_ahrs->get_gps()->last_message_time_ms() != lastFixTime) &&
(_ahrs->get_gps()->status() >= GPS::GPS_OK_FIX_3D))
{
lastFixTime = _ahrs.get_gps()->last_message_time_ms();
lastFixTime = _ahrs->get_gps()->last_message_time_ms();
newDataGps = true;
RecallStates(statesAtVelTime, (IMUmsec - msecVelDelay));
RecallStates(statesAtPosTime, (IMUmsec - msecPosDelay));
velNED[0] = _ahrs.get_gps()->velocity_north(); // (rad)
velNED[1] = _ahrs.get_gps()->velocity_east(); // (m/s)
velNED[2] = _ahrs.get_gps()->velocity_down(); // (m/s)
gpsLat = DEG_TO_RAD * 1e-7f * _ahrs.get_gps()->latitude; // (rad)
gpsLon = DEG_TO_RAD * 1e-7f * _ahrs.get_gps()->longitude; //(rad)
gpsHgt = 0.01f * _ahrs.get_gps()->altitude_cm; // (m)
velNED[0] = _ahrs->get_gps()->velocity_north(); // (rad)
velNED[1] = _ahrs->get_gps()->velocity_east(); // (m/s)
velNED[2] = _ahrs->get_gps()->velocity_down(); // (m/s)
gpsLat = DEG_TO_RAD * 1e-7f * _ahrs->get_gps()->latitude; // (rad)
gpsLon = DEG_TO_RAD * 1e-7f * _ahrs->get_gps()->longitude; //(rad)
gpsHgt = 0.01f * _ahrs->get_gps()->altitude_cm; // (m)
// Convert GPS measurements to Pos NE
calcposNE(gpsLat, gpsLon);
}
@ -2073,11 +2075,11 @@ void NavEKF::readHgtData() @@ -2073,11 +2075,11 @@ void NavEKF::readHgtData()
void NavEKF::readMagData()
{
// scale compass data to improve numerical conditioning
if (_ahrs.get_compass()->last_update != lastMagUpdate) {
lastMagUpdate = _ahrs.get_compass()->last_update;
if (_ahrs->get_compass()->last_update != lastMagUpdate) {
lastMagUpdate = _ahrs->get_compass()->last_update;
magData = _ahrs.get_compass()->get_field() * 0.001f;
magBias = _ahrs.get_compass()->get_offsets() * 0.001f;
magData = _ahrs->get_compass()->get_field() * 0.001f;
magBias = _ahrs->get_compass()->get_offsets() * 0.001f;
// Recall states from compass measurement time
RecallStates(statesAtMagMeasTime, (IMUmsec - msecMagDelay));
@ -2089,10 +2091,10 @@ void NavEKF::readMagData() @@ -2089,10 +2091,10 @@ void NavEKF::readMagData()
void NavEKF::readAirSpdData()
{
const AP_Airspeed *aspeed = _ahrs.get_airspeed();
const AP_Airspeed *aspeed = _ahrs->get_airspeed();
if (aspeed &&
aspeed->last_update_ms() != lastAirspeedUpdate &&
_ahrs.airspeed_estimate_true(&VtasMeas)) {
_ahrs->airspeed_estimate_true(&VtasMeas)) {
lastAirspeedUpdate = aspeed->last_update_ms();
newDataTas = true;
RecallStates(statesAtVtasMeasTime, (IMUmsec - msecTasDelay));
@ -2101,7 +2103,7 @@ void NavEKF::readAirSpdData() @@ -2101,7 +2103,7 @@ void NavEKF::readAirSpdData()
}
}
void NavEKF::calcEarthRateNED(Vector3f &omega, float latitude)
void NavEKF::calcEarthRateNED(Vector3f &omega, float latitude) const
{
omega.x = earthRate*cosf(latitude);
omega.y = 0;

50
libraries/AP_NavEKF/AP_NavEKF.h
Unescape View File

@ -22,10 +22,8 @@ @@ -22,10 +22,8 @@
#define AP_NavEKF
#include <AP_Math.h>
#include <AP_AHRS.h>
#include <AP_InertialSensor.h>
#include <AP_Baro.h>
#include <AP_AHRS.h>
#include <AP_Airspeed.h>
#include <AP_Compass.h>
@ -38,6 +36,8 @@ @@ -38,6 +36,8 @@
#endif
class AP_AHRS;
class NavEKF
{
public:
@ -68,7 +68,7 @@ public: @@ -68,7 +68,7 @@ public:
#endif
// Constructor
NavEKF(const AP_AHRS &ahrs, AP_Baro &baro);
NavEKF(const AP_AHRS *ahrs, AP_Baro &baro);
// Initialise the filter states from the AHRS and magnetometer data (if present)
void InitialiseFilter(void);
@ -77,47 +77,47 @@ public: @@ -77,47 +77,47 @@ public:
void UpdateFilter(void);
// fill in latitude, longitude and height of the reference point
void getRefLLH(struct Location &loc);
void getRefLLH(struct Location &loc) const;
// return the last calculated NED position relative to the
// reference point (m). Return false if no position is available
bool getPosNED(Vector3f &pos);
bool getPosNED(Vector3f &pos) const;
// return NED velocity in m/s
void getVelNED(Vector3f &vel);
void getVelNED(Vector3f &vel) const;
// return bodyaxis gyro bias estimates in deg/hr
void getGyroBias(Vector3f &gyroBias);
void getGyroBias(Vector3f &gyroBias) const;
// return body axis accelerometer bias estimates in m/s^2
void getAccelBias(Vector3f &accelBias);
void getAccelBias(Vector3f &accelBias) const;
// return the NED wind speed estimates in m/s
void getWind(Vector3f &wind);
void getWind(Vector3f &wind) const;
// return earth magnetic field estimates in measurement units
void getMagNED(Vector3f &magNED);
void getMagNED(Vector3f &magNED) const;
// return body magnetic field estimates in measurement units
void getMagXYZ(Vector3f &magXYZ);
void getMagXYZ(Vector3f &magXYZ) const;
// return the last calculated latitude, longitude and height
bool getLLH(struct Location &loc);
bool getLLH(struct Location &loc) const;
// return the Euler roll, pitch and yaw angle in radians
void getEulerAngles(Vector3f &eulers);
void getEulerAngles(Vector3f &eulers) const;
// get the transformation matrix from NED to XYD (body) axes
void getRotationNEDToBody(Matrix3f &mat);
void getRotationNEDToBody(Matrix3f &mat) const;
// get the transformation matrix from XYZ (body) to NED axes
void getRotationBodyToNED(Matrix3f &mat);
void getRotationBodyToNED(Matrix3f &mat) const;
// get the quaternions defining the rotation from NED to XYZ (body) axes
void getQuaternion(Quaternion &quat);
void getQuaternion(Quaternion &quat) const;
private:
const AP_AHRS &_ahrs;
const AP_AHRS *_ahrs;
AP_Baro &_baro;
void UpdateStrapdownEquationsNED();
@ -134,7 +134,7 @@ private: @@ -134,7 +134,7 @@ private:
void zeroCols(Matrix24 &covMat, uint8_t first, uint8_t last);
void quatNorm(Quaternion &quatOut, const Quaternion &quatIn);
void quatNorm(Quaternion &quatOut, const Quaternion &quatIn) const;
// store states along with system time stamp in msces
void StoreStates(void);
@ -142,21 +142,21 @@ private: @@ -142,21 +142,21 @@ private:
// recall state vector stored at closest time to the one specified by msec
void RecallStates(Vector24 &statesForFusion, uint32_t msec);
void quat2Tnb(Matrix3f &Tnb, const Quaternion &quat);
void quat2Tnb(Matrix3f &Tnb, const Quaternion &quat) const;
void quat2Tbn(Matrix3f &Tbn, const Quaternion &quat);
void quat2Tbn(Matrix3f &Tbn, const Quaternion &quat) const;
void calcEarthRateNED(Vector3f &omega, float latitude);
void calcEarthRateNED(Vector3f &omega, float latitude) const;
void eul2quat(Quaternion &quat, const Vector3f &eul);
void eul2quat(Quaternion &quat, const Vector3f &eul) const;
void quat2eul(Vector3f &eul, const Quaternion &quat);
void quat2eul(Vector3f &eul, const Quaternion &quat) const;
void calcvelNED(Vector3f &velNED, float gpsCourse, float gpsGndSpd, float gpsVelD);
void calcvelNED(Vector3f &velNED, float gpsCourse, float gpsGndSpd, float gpsVelD) const;
void calcposNE(float lat, float lon);
void calcllh(float &lat, float &lon, float &hgt);
void calcllh(float &lat, float &lon, float &hgt) const;
void OnGroundCheck();

Write Preview
Loading…
Cancel
Save