ardupilot/libraries/AP_AHRS/AP_AHRS_NavEKF.h

373 lines
14 KiB
C
Raw Normal View History

#pragma once
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/*
* NavEKF based AHRS (Attitude Heading Reference System) interface for
* ArduPilot
*
*/
#include <AP_HAL/AP_HAL.h>
#ifndef HAL_NAVEKF2_AVAILABLE
// only default to EK2 enabled on boards with over 1M flash
#define HAL_NAVEKF2_AVAILABLE (BOARD_FLASH_SIZE>1024)
#endif
#ifndef HAL_NAVEKF3_AVAILABLE
#define HAL_NAVEKF3_AVAILABLE 1
#endif
#define AP_AHRS_NAVEKF_AVAILABLE 1
#include "AP_AHRS.h"
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#include <SITL/SITL.h>
#endif
2015-09-22 21:35:02 -03:00
#include <AP_NavEKF2/AP_NavEKF2.h>
#include <AP_NavEKF3/AP_NavEKF3.h>
#include <AP_NavEKF/AP_Nav_Common.h> // definitions shared by inertial and ekf nav filters
#define AP_AHRS_NAVEKF_SETTLE_TIME_MS 20000 // time in milliseconds the ekf needs to settle after being started
2017-08-30 04:59:36 -03:00
class AP_AHRS_NavEKF : public AP_AHRS_DCM {
public:
enum Flags {
FLAG_NONE = 0,
FLAG_ALWAYS_USE_EKF = 0x1,
};
// Constructor
AP_AHRS_NavEKF(uint8_t flags = FLAG_NONE);
2017-08-30 04:59:36 -03:00
// initialise
void init(void) override;
2017-08-30 04:59:36 -03:00
/* Do not allow copies */
AP_AHRS_NavEKF(const AP_AHRS_NavEKF &other) = delete;
AP_AHRS_NavEKF &operator=(const AP_AHRS_NavEKF&) = delete;
// return the smoothed gyro vector corrected for drift
const Vector3f &get_gyro(void) const override;
const Matrix3f &get_rotation_body_to_ned(void) const override;
// return the current drift correction integrator value
const Vector3f &get_gyro_drift(void) const override;
2014-10-28 08:22:48 -03:00
// reset the current gyro drift estimate
// should be called if gyro offsets are recalculated
void reset_gyro_drift() override;
2014-10-28 08:22:48 -03:00
void update(bool skip_ins_update=false) override;
void reset(bool recover_eulers = false) override;
// reset the current attitude, used on new IMU calibration
void reset_attitude(const float &roll, const float &pitch, const float &yaw) override;
// dead-reckoning support
bool get_position(struct Location &loc) const override;
2017-01-05 14:07:14 -04:00
// get latest altitude estimate above ground level in meters and validity flag
bool get_hagl(float &hagl) const override;
// status reporting of estimated error
float get_error_rp() const override;
float get_error_yaw() const override;
// return a wind estimation vector, in m/s
2017-12-02 09:13:32 -04:00
Vector3f wind_estimate() const override;
// return an airspeed estimate if available. return true
// if we have an estimate
bool airspeed_estimate(float &airspeed_ret) const override;
// true if compass is being used
bool use_compass() override;
// we will need to remove these to fully hide which EKF we are using
#if HAL_NAVEKF2_AVAILABLE
2015-09-23 04:29:43 -03:00
NavEKF2 &get_NavEKF2(void) {
return EKF2;
}
const NavEKF2 &get_NavEKF2_const(void) const {
return EKF2;
}
#endif
#if HAL_NAVEKF3_AVAILABLE
NavEKF3 &get_NavEKF3(void) {
return EKF3;
}
const NavEKF3 &get_NavEKF3_const(void) const {
return EKF3;
}
#endif
2019-02-19 23:53:28 -04:00
2020-03-31 22:08:54 -03:00
// return the quaternion defining the rotation from NED to XYZ (body) axes
bool get_quaternion(Quaternion &quat) const override WARN_IF_UNUSED;
// return secondary attitude solution if available, as eulers in radians
2017-12-02 09:13:32 -04:00
bool get_secondary_attitude(Vector3f &eulers) const override;
// return secondary attitude solution if available, as quaternion
2017-12-02 09:13:32 -04:00
bool get_secondary_quaternion(Quaternion &quat) const override;
2019-02-19 23:53:28 -04:00
// return secondary position solution if available
2017-12-02 09:13:32 -04:00
bool get_secondary_position(struct Location &loc) const override;
// EKF has a better ground speed vector estimate
Vector2f groundspeed_vector() override;
const Vector3f &get_accel_ef(uint8_t i) const override;
const Vector3f &get_accel_ef() const override;
// Retrieves the corrected NED delta velocity in use by the inertial navigation
void getCorrectedDeltaVelocityNED(Vector3f& ret, float& dt) const override;
// blended accelerometer values in the earth frame in m/s/s
const Vector3f &get_accel_ef_blended() const override;
2017-04-19 03:28:14 -03:00
// set the EKF's origin location in 10e7 degrees. This should only
// be called when the EKF has no absolute position reference (i.e. GPS)
// from which to decide the origin on its own
bool set_origin(const Location &loc) override;
// returns the inertial navigation origin in lat/lon/alt
bool get_origin(Location &ret) const override;
bool have_inertial_nav() const override;
bool get_velocity_NED(Vector3f &vec) const override;
// return the relative position NED to either home or origin
// return true if the estimate is valid
bool get_relative_position_NED_home(Vector3f &vec) const override;
bool get_relative_position_NED_origin(Vector3f &vec) const override;
// return the relative position NE to either home or origin
// return true if the estimate is valid
bool get_relative_position_NE_home(Vector2f &posNE) const override;
bool get_relative_position_NE_origin(Vector2f &posNE) const override;
// return the relative position down to either home or origin
// baro will be used for the _home relative one if the EKF isn't
void get_relative_position_D_home(float &posD) const override;
bool get_relative_position_D_origin(float &posD) const override;
// Get a derivative of the vertical position in m/s which is kinematically consistent with the vertical position is required by some control loops.
2017-01-05 14:07:14 -04:00
// This is different to the vertical velocity from the EKF which is not always consistent with the vertical position due to the various errors that are being corrected for.
2017-12-02 09:13:32 -04:00
bool get_vert_pos_rate(float &velocity) const;
// write optical flow measurements to EKF
void writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset);
// write body odometry measurements to the EKF
void writeBodyFrameOdom(float quality, const Vector3f &delPos, const Vector3f &delAng, float delTime, uint32_t timeStamp_ms, uint16_t delay_ms, const Vector3f &posOffset);
// Writes the default equivalent airspeed in m/s to be used in forward flight if a measured airspeed is required and not available.
void writeDefaultAirSpeed(float airspeed);
// Write position and quaternion data from an external navigation system
2020-04-13 02:03:01 -03:00
void writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint16_t delay_ms, uint32_t resetTime_ms) override;
2020-05-12 03:06:24 -03:00
// Write velocity data from an external navigation system
void writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeStamp_ms, uint16_t delay_ms) override;
2017-01-05 14:07:14 -04:00
// inhibit GPS usage
uint8_t setInhibitGPS(void);
// get speed limit
2017-12-02 09:13:32 -04:00
void getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) const;
2015-03-19 02:58:13 -03:00
void set_ekf_use(bool setting);
2014-01-03 21:39:20 -04:00
// is the AHRS subsystem healthy?
bool healthy() const override;
bool prearm_healthy() const override;
// true if the AHRS has completed initialisation
bool initialised() const override;
// get_filter_status - returns filter status as a series of flags
bool get_filter_status(nav_filter_status &status) const;
// get compass offset estimates
// true if offsets are valid
2017-12-02 09:13:32 -04:00
bool getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const;
2015-09-08 02:50:22 -03:00
// report any reason for why the backend is refusing to initialise
const char *prearm_failure_reason(void) const override;
// check all cores providing consistent attitudes for prearm checks
bool attitudes_consistent(char *failure_msg, const uint8_t failure_msg_len) const override;
// return the amount of yaw angle change due to the last yaw angle reset in radians
// returns the time of the last yaw angle reset or 0 if no reset has ever occurred
uint32_t getLastYawResetAngle(float &yawAng) override;
2017-01-05 14:07:14 -04:00
// return the amount of NE position change in meters due to the last reset
// returns the time of the last reset or 0 if no reset has ever occurred
uint32_t getLastPosNorthEastReset(Vector2f &pos) override;
2017-01-05 14:07:14 -04:00
// return the amount of NE velocity change in meters/sec due to the last reset
// returns the time of the last reset or 0 if no reset has ever occurred
uint32_t getLastVelNorthEastReset(Vector2f &vel) const override;
// return the amount of vertical position change due to the last reset in meters
// returns the time of the last reset or 0 if no reset has ever occurred
uint32_t getLastPosDownReset(float &posDelta) override;
// 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() override;
// send a EKF_STATUS_REPORT for current EKF
2017-12-02 09:13:32 -04:00
void send_ekf_status_report(mavlink_channel_t chan) const;
2019-02-19 23:53:28 -04:00
2017-01-05 14:07:14 -04:00
// get_hgt_ctrl_limit - get maximum height to be observed by the control loops in meters and a validity flag
// this is used to limit height during optical flow navigation
// it will return invalid when no limiting is required
bool get_hgt_ctrl_limit(float &limit) const override;
// Set to true if the terrain underneath is stable enough to be used as a height reference
// this is not related to terrain following
void set_terrain_hgt_stable(bool stable) override;
2017-12-07 19:26:28 -04:00
// get_location - updates the provided location with the latest
// calculated location including absolute altitude
// returns true on success (i.e. the EKF knows it's latest
// position), false on failure
bool get_location(struct Location &loc) const;
2019-10-04 22:04:00 -03:00
// return the innovations for the specified instance
// An out of range instance (eg -1) returns data for the primary instance
bool get_innovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const override;
2015-10-17 02:49:50 -03:00
// get_variances - provides the innovations normalised using the innovation variance where a value of 0
2017-01-05 14:07:14 -04:00
// indicates perfect consistency between the measurement and the EKF solution and a value of of 1 is the maximum
// inconsistency that will be accepted by the filter
2015-10-17 02:49:50 -03:00
// boolean false is returned if variances are not available
bool get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const override;
2015-10-17 02:49:50 -03:00
// returns the expected NED magnetic field
bool get_mag_field_NED(Vector3f& ret) const;
// returns the estimated magnetic field offsets in body frame
bool get_mag_field_correction(Vector3f &ret) const override;
void setTakeoffExpected(bool val);
void setTouchdownExpected(bool val);
2017-12-02 09:13:32 -04:00
bool getGpsGlitchStatus() const;
2016-01-26 17:28:21 -04:00
// used by Replay to force start at right timestamp
void force_ekf_start(void) { _force_ekf = true; }
2016-05-04 01:02:12 -03:00
// is the EKF backend doing its own sensor logging?
bool have_ekf_logging(void) const override;
2020-04-21 01:39:45 -03:00
// return the index of the primary core or -1 if no primary core selected
int8_t get_primary_core_index() const override;
// get the index of the current primary accelerometer sensor
uint8_t get_primary_accel_index(void) const override;
// get the index of the current primary gyro sensor
uint8_t get_primary_gyro_index(void) const override;
2019-06-07 20:33:45 -03:00
// see if EKF lane switching is possible to avoid EKF failsafe
void check_lane_switch(void) override;
// request EKF yaw reset to try and avoid the need for an EKF lane switch or failsafe
void request_yaw_reset(void) override;
void Log_Write();
// check whether external navigation is providing yaw. Allows compass pre-arm checks to be bypassed
bool is_ext_nav_used_for_yaw(void) const override;
// set and save the ALT_M_NSE parameter value
void set_alt_measurement_noise(float noise) override;
// these are only out here so vehicles can reference them for parameters
#if HAL_NAVEKF2_AVAILABLE
NavEKF2 EKF2;
#endif
#if HAL_NAVEKF3_AVAILABLE
NavEKF3 EKF3;
#endif
private:
enum class EKFType {
NONE = 0
#if HAL_NAVEKF3_AVAILABLE
,THREE = 3
#endif
#if HAL_NAVEKF2_AVAILABLE
,TWO = 2
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
,SITL = 10
#endif
};
EKFType active_EKF_type(void) const;
2014-01-02 01:16:29 -04:00
bool always_use_EKF() const {
return _ekf_flags & FLAG_ALWAYS_USE_EKF;
}
#if HAL_NAVEKF2_AVAILABLE
void update_EKF2(void);
bool _ekf2_started;
#endif
#if HAL_NAVEKF3_AVAILABLE
bool _ekf3_started;
void update_EKF3(void);
#endif
bool _force_ekf;
// rotation from vehicle body to NED frame
Matrix3f _dcm_matrix;
Vector3f _dcm_attitude;
Vector3f _gyro_drift;
Vector3f _gyro_estimate;
Vector3f _accel_ef_ekf[INS_MAX_INSTANCES];
Vector3f _accel_ef_ekf_blended;
const uint16_t startup_delay_ms = 1000;
uint32_t start_time_ms = 0;
uint8_t _ekf_flags; // bitmask from Flags enumeration
EKFType ekf_type(void) const;
void update_DCM(bool skip_ins_update);
// get the index of the current primary IMU
uint8_t get_primary_IMU_index(void) const;
2019-02-19 23:53:28 -04:00
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
SITL::SITL *_sitl;
uint32_t _last_body_odm_update_ms = 0;
void update_SITL(void);
#endif
};