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

50
libraries/AP_NavEKF/AP_NavEKF.h
Unescape View File

@ -22,10 +22,8 @@
#define AP_NavEKF #define AP_NavEKF
#include <AP_Math.h> #include <AP_Math.h>
#include <AP_AHRS.h>
#include <AP_InertialSensor.h> #include <AP_InertialSensor.h>
#include <AP_Baro.h> #include <AP_Baro.h>
#include <AP_AHRS.h>
#include <AP_Airspeed.h> #include <AP_Airspeed.h>
#include <AP_Compass.h> #include <AP_Compass.h>
@ -38,6 +36,8 @@
#endif #endif
class AP_AHRS;
class NavEKF class NavEKF
{ {
public: public:
@ -68,7 +68,7 @@ public:
#endif #endif
// Constructor // 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) // Initialise the filter states from the AHRS and magnetometer data (if present)
void InitialiseFilter(void); void InitialiseFilter(void);
@ -77,47 +77,47 @@ public:
void UpdateFilter(void); void UpdateFilter(void);
// fill in latitude, longitude and height of the reference point // 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 // return the last calculated NED position relative to the
// reference point (m). Return false if no position is available // 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 // return NED velocity in m/s
void getVelNED(Vector3f &vel); void getVelNED(Vector3f &vel) const;
// return bodyaxis gyro bias estimates in deg/hr // 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 // 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 // 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 // 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 // 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 // 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 // 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 // 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 // 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 // get the quaternions defining the rotation from NED to XYZ (body) axes
void getQuaternion(Quaternion &quat); void getQuaternion(Quaternion &quat) const;
private: private:
const AP_AHRS &_ahrs; const AP_AHRS *_ahrs;
AP_Baro &_baro; AP_Baro &_baro;
void UpdateStrapdownEquationsNED(); void UpdateStrapdownEquationsNED();
@ -134,7 +134,7 @@ private:
void zeroCols(Matrix24 &covMat, uint8_t first, uint8_t last); 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 // store states along with system time stamp in msces
void StoreStates(void); void StoreStates(void);
@ -142,21 +142,21 @@ private:
// recall state vector stored at closest time to the one specified by msec // recall state vector stored at closest time to the one specified by msec
void RecallStates(Vector24 &statesForFusion, uint32_t 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 calcposNE(float lat, float lon);
void calcllh(float &lat, float &lon, float &hgt); void calcllh(float &lat, float &lon, float &hgt) const;
void OnGroundCheck(); void OnGroundCheck();

Write Preview
Loading…
Cancel
Save