From b4555f30a52a789c6c60716b789f075a87a0f951 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 23 Sep 2015 10:27:56 +1000 Subject: [PATCH] AP_NavEKF2: added frontend calls to core code --- libraries/AP_NavEKF2/AP_NavEKF2.cpp | 410 ++++++++++++++++++++++++- libraries/AP_NavEKF2/AP_NavEKF2.h | 196 +++++++++++- libraries/AP_NavEKF2/AP_NavEKF2_core.h | 10 +- 3 files changed, 606 insertions(+), 10 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp index 428d57991f..61bd9ca0a2 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp @@ -5,6 +5,7 @@ #include "AP_NavEKF2_core.h" #include +#include /* parameter defaults for different types of vehicle. The @@ -347,7 +348,10 @@ const AP_Param::GroupInfo NavEKF2::var_info[] PROGMEM = { AP_GROUPEND }; -NavEKF2::NavEKF2() : +NavEKF2::NavEKF2(const AP_AHRS *ahrs, AP_Baro &baro, const RangeFinder &rng) : + _ahrs(ahrs), + _baro(baro), + _rng(rng), gpsNEVelVarAccScale(0.05f), // Scale factor applied to horizontal velocity measurement variance due to manoeuvre acceleration - used when GPS doesn't report speed error gpsDVelVarAccScale(0.07f), // Scale factor applied to vertical velocity measurement variance due to manoeuvre acceleration - used when GPS doesn't report speed error gpsPosVarAccScale(0.05f), // Scale factor applied to horizontal position measurement variance due to manoeuvre acceleration @@ -384,4 +388,408 @@ NavEKF2::NavEKF2() : AP_Param::setup_object_defaults(this, var_info); } + +// Initialise the filter +bool NavEKF2::InitialiseFilter(void) +{ + if (_enable == 0) { + return false; + } + if (core == nullptr) { + core = new NavEKF2_core(*this, _ahrs, _baro, _rng); + if (core == nullptr) { + _enable.set(0); + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, PSTR("NavEKF2: allocation failed")); + return false; + } + } + return core->InitialiseFilterBootstrap(); +} + +// Update Filter States - this should be called whenever new IMU data is available +void NavEKF2::UpdateFilter(void) +{ + if (core) { + core->UpdateFilter(); + } +} + +// Check basic filter health metrics and return a consolidated health status +bool NavEKF2::healthy(void) const +{ + if (!core) { + return false; + } + return core->healthy(); +} + +// Return the last calculated NED position relative to the reference point (m). +// If a calculated solution is not available, use the best available data and return false +// If false returned, do not use for flight control +bool NavEKF2::getPosNED(Vector3f &pos) const +{ + if (!core) { + return false; + } + return core->getPosNED(pos); +} + +// return NED velocity in m/s +void NavEKF2::getVelNED(Vector3f &vel) const +{ + if (core) { + core->getVelNED(vel); + } +} + +// This returns the specific forces in the NED frame +void NavEKF2::getAccelNED(Vector3f &accelNED) const +{ + if (core) { + core->getAccelNED(accelNED); + } +} + +// return body axis gyro bias estimates in rad/sec +void NavEKF2::getGyroBias(Vector3f &gyroBias) const +{ + if (core) { + core->getGyroBias(gyroBias); + } +} + +// return body axis gyro scale factor estimates +void NavEKF2::getGyroScale(Vector3f &gyroScale) const +{ + if (core) { + core->getGyroScale(gyroScale); + } +} + +// return tilt error convergence metric +void NavEKF2::getTiltError(float &ang) const +{ + if (core) { + core->getTiltError(ang); + } +} + +// reset body axis gyro bias estimates +void NavEKF2::resetGyroBias(void) +{ + if (core) { + core->resetGyroBias(); + } +} + +// Resets the baro so that it reads zero at the current height +// Resets the EKF height to zero +// Adjusts the EKf origin height so that the EKF height + origin height is the same as before +// Returns true if the height datum reset has been performed +// If using a range finder for height no reset is performed and it returns false +bool NavEKF2::resetHeightDatum(void) +{ + if (!core) { + return false; + } + return core->resetHeightDatum(); +} + +// Commands the EKF to not use GPS. +// This command must be sent prior to arming as it will only be actioned when the filter is in static mode +// This command is forgotten by the EKF each time it goes back into static mode (eg the vehicle disarms) +// Returns 0 if command rejected +// Returns 1 if attitude, vertical velocity and vertical position will be provided +// Returns 2 if attitude, 3D-velocity, vertical position and relative horizontal position will be provided +uint8_t NavEKF2::setInhibitGPS(void) +{ + if (!core) { + return 0; + } + return core->setInhibitGPS(); +} + +// return the horizontal speed limit in m/s set by optical flow sensor limits +// return the scale factor to be applied to navigation velocity gains to compensate for increase in velocity noise with height when using optical flow +void NavEKF2::getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) const +{ + if (core) { + core->getEkfControlLimits(ekfGndSpdLimit, ekfNavVelGainScaler); + } +} + +// return weighting of first IMU in blending function +void NavEKF2::getIMU1Weighting(float &ret) const +{ + if (core) { + core->getIMU1Weighting(ret); + } +} + +// return the individual Z-accel bias estimates in m/s^2 +void NavEKF2::getAccelZBias(float &zbias1, float &zbias2) const +{ + if (core) { + core->getAccelZBias(zbias1, zbias2); + } +} + +// return the NED wind speed estimates in m/s (positive is air moving in the direction of the axis) +void NavEKF2::getWind(Vector3f &wind) const +{ + if (core) { + core->getWind(wind); + } +} + +// return earth magnetic field estimates in measurement units / 1000 +void NavEKF2::getMagNED(Vector3f &magNED) const +{ + if (core) { + core->getMagNED(magNED); + } +} + +// return body magnetic field estimates in measurement units / 1000 +void NavEKF2::getMagXYZ(Vector3f &magXYZ) const +{ + if (core) { + core->getMagXYZ(magXYZ); + } +} + +// Return estimated magnetometer offsets +// Return true if magnetometer offsets are valid +bool NavEKF2::getMagOffsets(Vector3f &magOffsets) const +{ + if (!core) { + return false; + } + return core->getMagOffsets(magOffsets); +} + +// Return the last calculated latitude, longitude and height in WGS-84 +// If a calculated location isn't available, return a raw GPS measurement +// The status will return true if a calculation or raw measurement is available +// The getFilterStatus() function provides a more detailed description of data health and must be checked if data is to be used for flight control +bool NavEKF2::getLLH(struct Location &loc) const +{ + if (!core) { + return false; + } + return core->getLLH(loc); +} + +// return the latitude and longitude and height used to set the NED origin +// All NED positions calculated by the filter are relative to this location +// Returns false if the origin has not been set +bool NavEKF2::getOriginLLH(struct Location &loc) const +{ + if (!core) { + return false; + } + return core->getOriginLLH(loc); +} + +// set the latitude and longitude and height used to set the NED origin +// All NED positions calcualted by the filter will be relative to this location +// The origin cannot be set if the filter is in a flight mode (eg vehicle armed) +// Returns false if the filter has rejected the attempt to set the origin +bool NavEKF2::setOriginLLH(struct Location &loc) +{ + if (!core) { + return false; + } + return core->setOriginLLH(loc); +} + +// return estimated height above ground level +// return false if ground height is not being estimated. +bool NavEKF2::getHAGL(float &HAGL) const +{ + if (!core) { + return false; + } + return core->getHAGL(HAGL); +} + +// return the Euler roll, pitch and yaw angle in radians +void NavEKF2::getEulerAngles(Vector3f &eulers) const +{ + if (core) { + core->getEulerAngles(eulers); + } +} + +// return the transformation matrix from XYZ (body) to NED axes +void NavEKF2::getRotationBodyToNED(Matrix3f &mat) const +{ + if (core) { + core->getRotationBodyToNED(mat); + } +} + +// return the quaternions defining the rotation from NED to XYZ (body) axes +void NavEKF2::getQuaternion(Quaternion &quat) const +{ + if (core) { + core->getQuaternion(quat); + } +} + +// return the innovations for the NED Pos, NED Vel, XYZ Mag and Vtas measurements +void NavEKF2::getInnovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const +{ + if (core) { + core->getInnovations(velInnov, posInnov, magInnov, tasInnov, yawInnov); + } +} + +// return the innovation consistency test ratios for the velocity, position, magnetometer and true airspeed measurements +void NavEKF2::getVariances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const +{ + if (core) { + core->getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset); + } +} + +// should we use the compass? This is public so it can be used for +// reporting via ahrs.use_compass() +bool NavEKF2::use_compass(void) const +{ + if (!core) { + return false; + } + return core->use_compass(); +} + +// write the raw optical flow measurements +// rawFlowQuality is a measured of quality between 0 and 255, with 255 being the best quality +// rawFlowRates are the optical flow rates in rad/sec about the X and Y sensor axes. +// rawGyroRates are the sensor rotation rates in rad/sec measured by the sensors internal gyro +// The sign convention is that a RH physical rotation of the sensor about an axis produces both a positive flow and gyro rate +// msecFlowMeas is the scheduler time in msec when the optical flow data was received from the sensor. +void NavEKF2::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, Vector2f &rawGyroRates, uint32_t &msecFlowMeas) +{ + if (core) { + core->writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawGyroRates, msecFlowMeas); + } +} + +// return data for debugging optical flow fusion +void NavEKF2::getFlowDebug(float &varFlow, float &gndOffset, float &flowInnovX, float &flowInnovY, float &auxInnov, + float &HAGL, float &rngInnov, float &range, float &gndOffsetErr) const +{ + if (core) { + core->getFlowDebug(varFlow, gndOffset, flowInnovX, flowInnovY, auxInnov, HAGL, rngInnov, range, gndOffsetErr); + } +} + +// called by vehicle code to specify that a takeoff is happening +// causes the EKF to compensate for expected barometer errors due to ground effect +void NavEKF2::setTakeoffExpected(bool val) +{ + if (core) { + core->setTakeoffExpected(val); + } +} + +// called by vehicle code to specify that a touchdown is expected to happen +// causes the EKF to compensate for expected barometer errors due to ground effect +void NavEKF2::setTouchdownExpected(bool val) +{ + if (core) { + core->setTouchdownExpected(val); + } +} + +/* + return the filter fault status as a bitmasked integer + 0 = quaternions are NaN + 1 = velocities are NaN + 2 = badly conditioned X magnetometer fusion + 3 = badly conditioned Y magnetometer fusion + 5 = badly conditioned Z magnetometer fusion + 6 = badly conditioned airspeed fusion + 7 = badly conditioned synthetic sideslip fusion + 7 = filter is not initialised +*/ +void NavEKF2::getFilterFaults(uint8_t &faults) const +{ + if (core) { + core->getFilterFaults(faults); + } +} + +/* + return filter timeout status as a bitmasked integer + 0 = position measurement timeout + 1 = velocity measurement timeout + 2 = height measurement timeout + 3 = magnetometer measurement timeout + 5 = unassigned + 6 = unassigned + 7 = unassigned + 7 = unassigned +*/ +void NavEKF2::getFilterTimeouts(uint8_t &timeouts) const +{ + if (core) { + core->getFilterTimeouts(timeouts); + } +} + +/* + return filter status flags +*/ +void NavEKF2::getFilterStatus(nav_filter_status &status) const +{ + if (core) { + core->getFilterStatus(status); + } +} + +// send an EKF_STATUS_REPORT message to GCS +void NavEKF2::send_status_report(mavlink_channel_t chan) +{ + if (core) { + core->send_status_report(chan); + } +} + +// provides the height limit to be observed by the control loops +// returns false if no height limiting is required +// this is needed to ensure the vehicle does not fly too high when using optical flow navigation +bool NavEKF2::getHeightControlLimit(float &height) const +{ + if (!core) { + return false; + } + return core->getHeightControlLimit(height); +} + +// provides the quaternion that was used by the INS calculation to +// rotate from the previous orientation to the orientaion at the +// current time step +// returns a zero rotation quaternion if the INS +// calculation was not performed on that time step. +Quaternion NavEKF2::getDeltaQuaternion(void) const +{ + if (!core) { + return Quaternion(); + } + return core->getDeltaQuaternion(); +} + +// return the amount of yaw angle change due to the last yaw angle reset in radians +// returns true if a reset yaw angle has been updated and not queried +// this function should not have more than one client +bool NavEKF2::getLastYawResetAngle(float &yawAng) +{ + if (!core) { + return false; + } + return core->getLastYawResetAngle(yawAng); +} + #endif //HAL_CPU_CLASS diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.h b/libraries/AP_NavEKF2/AP_NavEKF2.h index 0bdf6abf8e..53dbd2ff15 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2.h @@ -25,12 +25,206 @@ #include #include #include +#include +#include +#include +#include +#include +#include + +class NavEKF2_core; +class AP_AHRS; class NavEKF2 { public: + friend class NavEKF2_core; static const struct AP_Param::GroupInfo var_info[]; - NavEKF2(); + + NavEKF2(const AP_AHRS *ahrs, AP_Baro &baro, const RangeFinder &rng); + + // Initialise the filter + bool InitialiseFilter(void); + + // Update Filter States - this should be called whenever new IMU data is available + void UpdateFilter(void); + + // Check basic filter health metrics and return a consolidated health status + bool healthy(void) const; + + // Return the last calculated NED position relative to the reference point (m). + // If a calculated solution is not available, use the best available data and return false + // If false returned, do not use for flight control + bool getPosNED(Vector3f &pos) const; + + // return NED velocity in m/s + void getVelNED(Vector3f &vel) const; + + // This returns the specific forces in the NED frame + void getAccelNED(Vector3f &accelNED) const; + + // return body axis gyro bias estimates in rad/sec + void getGyroBias(Vector3f &gyroBias) const; + + // return body axis gyro scale factor estimates + void getGyroScale(Vector3f &gyroScale) const; + + // return tilt error convergence metric + void getTiltError(float &ang) const; + + // reset body axis gyro bias estimates + void resetGyroBias(void); + + // Resets the baro so that it reads zero at the current height + // Resets the EKF height to zero + // Adjusts the EKf origin height so that the EKF height + origin height is the same as before + // Returns true if the height datum reset has been performed + // If using a range finder for height no reset is performed and it returns false + bool resetHeightDatum(void); + + // Commands the EKF to not use GPS. + // This command must be sent prior to arming as it will only be actioned when the filter is in static mode + // This command is forgotten by the EKF each time it goes back into static mode (eg the vehicle disarms) + // Returns 0 if command rejected + // Returns 1 if attitude, vertical velocity and vertical position will be provided + // Returns 2 if attitude, 3D-velocity, vertical position and relative horizontal position will be provided + uint8_t setInhibitGPS(void); + + // return the horizontal speed limit in m/s set by optical flow sensor limits + // return the scale factor to be applied to navigation velocity gains to compensate for increase in velocity noise with height when using optical flow + void getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) const; + + // return weighting of first IMU in blending function + void getIMU1Weighting(float &ret) const; + + // return the individual Z-accel bias estimates in m/s^2 + void getAccelZBias(float &zbias1, float &zbias2) const; + + // return the NED wind speed estimates in m/s (positive is air moving in the direction of the axis) + void getWind(Vector3f &wind) const; + + // return earth magnetic field estimates in measurement units / 1000 + void getMagNED(Vector3f &magNED) const; + + // return body magnetic field estimates in measurement units / 1000 + void getMagXYZ(Vector3f &magXYZ) const; + + // Return estimated magnetometer offsets + // Return true if magnetometer offsets are valid + bool getMagOffsets(Vector3f &magOffsets) const; + + // Return the last calculated latitude, longitude and height in WGS-84 + // If a calculated location isn't available, return a raw GPS measurement + // The status will return true if a calculation or raw measurement is available + // The getFilterStatus() function provides a more detailed description of data health and must be checked if data is to be used for flight control + bool getLLH(struct Location &loc) const; + + // return the latitude and longitude and height used to set the NED origin + // All NED positions calculated by the filter are relative to this location + // Returns false if the origin has not been set + bool getOriginLLH(struct Location &loc) const; + + // set the latitude and longitude and height used to set the NED origin + // All NED positions calcualted by the filter will be relative to this location + // The origin cannot be set if the filter is in a flight mode (eg vehicle armed) + // Returns false if the filter has rejected the attempt to set the origin + bool setOriginLLH(struct Location &loc); + + // return estimated height above ground level + // return false if ground height is not being estimated. + bool getHAGL(float &HAGL) const; + + // return the Euler roll, pitch and yaw angle in radians + void getEulerAngles(Vector3f &eulers) const; + + // return the transformation matrix from XYZ (body) to NED axes + void getRotationBodyToNED(Matrix3f &mat) const; + + // return the quaternions defining the rotation from NED to XYZ (body) axes + void getQuaternion(Quaternion &quat) const; + + // return the innovations for the NED Pos, NED Vel, XYZ Mag and Vtas measurements + void getInnovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const; + + // return the innovation consistency test ratios for the velocity, position, magnetometer and true airspeed measurements + void getVariances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const; + + // should we use the compass? This is public so it can be used for + // reporting via ahrs.use_compass() + bool use_compass(void) const; + + // write the raw optical flow measurements + // rawFlowQuality is a measured of quality between 0 and 255, with 255 being the best quality + // rawFlowRates are the optical flow rates in rad/sec about the X and Y sensor axes. + // rawGyroRates are the sensor rotation rates in rad/sec measured by the sensors internal gyro + // The sign convention is that a RH physical rotation of the sensor about an axis produces both a positive flow and gyro rate + // msecFlowMeas is the scheduler time in msec when the optical flow data was received from the sensor. + void writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, Vector2f &rawGyroRates, uint32_t &msecFlowMeas); + + // return data for debugging optical flow fusion + void getFlowDebug(float &varFlow, float &gndOffset, float &flowInnovX, float &flowInnovY, float &auxInnov, float &HAGL, float &rngInnov, float &range, float &gndOffsetErr) const; + + // called by vehicle code to specify that a takeoff is happening + // causes the EKF to compensate for expected barometer errors due to ground effect + void setTakeoffExpected(bool val); + + // called by vehicle code to specify that a touchdown is expected to happen + // causes the EKF to compensate for expected barometer errors due to ground effect + void setTouchdownExpected(bool val); + + /* + return the filter fault status as a bitmasked integer + 0 = quaternions are NaN + 1 = velocities are NaN + 2 = badly conditioned X magnetometer fusion + 3 = badly conditioned Y magnetometer fusion + 5 = badly conditioned Z magnetometer fusion + 6 = badly conditioned airspeed fusion + 7 = badly conditioned synthetic sideslip fusion + 7 = filter is not initialised + */ + void getFilterFaults(uint8_t &faults) const; + + /* + return filter timeout status as a bitmasked integer + 0 = position measurement timeout + 1 = velocity measurement timeout + 2 = height measurement timeout + 3 = magnetometer measurement timeout + 5 = unassigned + 6 = unassigned + 7 = unassigned + 7 = unassigned + */ + void getFilterTimeouts(uint8_t &timeouts) const; + + /* + return filter status flags + */ + void getFilterStatus(nav_filter_status &status) const; + + // send an EKF_STATUS_REPORT message to GCS + void send_status_report(mavlink_channel_t chan); + + // provides the height limit to be observed by the control loops + // returns false if no height limiting is required + // this is needed to ensure the vehicle does not fly too high when using optical flow navigation + bool getHeightControlLimit(float &height) const; + + // provides the quaternion that was used by the INS calculation to rotate from the previous orientation to the orientaion at the current time step + // returns a zero rotation quaternion if the INS calculation was not performed on that time step. + Quaternion getDeltaQuaternion(void) const; + + // return the amount of yaw angle change due to the last yaw angle reset in radians + // returns true if a reset yaw angle has been updated and not queried + // this function should not have more than one client + bool getLastYawResetAngle(float &yawAng); + +private: + NavEKF2_core *core = nullptr; + const AP_AHRS *_ahrs; + AP_Baro &_baro; + const RangeFinder &_rng; // EKF Mavlink Tuneable Parameters AP_Int8 _enable; // zero to disable EKF2 diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h index f1bc049bbe..c82756492c 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.h @@ -22,13 +22,6 @@ #define AP_NavEKF2_core #include -#include -#include -#include -#include -#include -#include -#include #include "AP_NavEKF2.h" // #define MATH_CHECK_INDEXES 1 @@ -225,10 +218,10 @@ public: // this function should not have more than one client bool getLastYawResetAngle(float &yawAng); +private: // Reference to the global EKF frontend for parameters NavEKF2 &frontend; -private: typedef float ftype; #if defined(MATH_CHECK_INDEXES) && (MATH_CHECK_INDEXES == 1) typedef VectorN Vector2; @@ -249,6 +242,7 @@ private: typedef VectorN Vector24; typedef VectorN Vector25; typedef VectorN Vector31; + typedef VectorN Vector28; typedef VectorN,3> Matrix3; typedef VectorN,24> Matrix24; typedef VectorN,50> Matrix34_50;