From 2c86a490ed87e53cdaeca9eef17b3df321960b8b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 2 Jan 2014 12:15:22 +1100 Subject: [PATCH] AP_NavEKF: make it possible for NavEKF to be a AHRS member ready for AHRS integration --- libraries/AP_NavEKF/AP_NavEKF.cpp | 78 ++++++++++++++++--------------- libraries/AP_NavEKF/AP_NavEKF.h | 50 ++++++++++---------- 2 files changed, 65 insertions(+), 63 deletions(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index a1374cc282..a2c561ee08 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -9,6 +9,8 @@ #pragma GCC optimize("O3") #include "AP_NavEKF.h" +#include + //#include 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) { // 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() 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) } } -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) 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) 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) 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) 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() 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() 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() 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() 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() 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() } } -void NavEKF::calcEarthRateNED(Vector3f &omega, float latitude) +void NavEKF::calcEarthRateNED(Vector3f &omega, float latitude) const { omega.x = earthRate*cosf(latitude); omega.y = 0; diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h index 4fbe04e11c..df8527dbc9 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.h +++ b/libraries/AP_NavEKF/AP_NavEKF.h @@ -22,10 +22,8 @@ #define AP_NavEKF #include -#include #include #include -#include #include #include @@ -38,6 +36,8 @@ #endif +class AP_AHRS; + class NavEKF { 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: 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: 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: // 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();