APM:Libraries
AP_AHRS_NavEKF.h
Go to the documentation of this file.
1 #pragma once
2 
3 /*
4  This program is free software: you can redistribute it and/or modify
5  it under the terms of the GNU General Public License as published by
6  the Free Software Foundation, either version 3 of the License, or
7  (at your option) any later version.
8 
9  This program is distributed in the hope that it will be useful,
10  but WITHOUT ANY WARRANTY; without even the implied warranty of
11  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12  GNU General Public License for more details.
13 
14  You should have received a copy of the GNU General Public License
15  along with this program. If not, see <http://www.gnu.org/licenses/>.
16  */
17 
18 /*
19  * NavEKF based AHRS (Attitude Heading Reference System) interface for
20  * ArduPilot
21  *
22  */
23 
24 #include <AP_HAL/AP_HAL.h>
25 
26 #define AP_AHRS_NAVEKF_AVAILABLE 1
27 #include "AP_AHRS.h"
28 
29 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
30 #include <SITL/SITL.h>
31 #endif
32 
33 #if HAL_CPU_CLASS >= HAL_CPU_CLASS_150
34 #include <AP_NavEKF2/AP_NavEKF2.h>
35 #include <AP_NavEKF3/AP_NavEKF3.h>
36 #include <AP_NavEKF/AP_Nav_Common.h> // definitions shared by inertial and ekf nav filters
37 
38 
39 #define AP_AHRS_NAVEKF_SETTLE_TIME_MS 20000 // time in milliseconds the ekf needs to settle after being started
40 
41 class AP_AHRS_NavEKF : public AP_AHRS_DCM {
42 public:
43  enum Flags {
44  FLAG_NONE = 0,
46  };
47 
48  // Constructor
49  AP_AHRS_NavEKF(NavEKF2 &_EKF2, NavEKF3 &_EKF3, Flags flags = FLAG_NONE);
50 
51  /* Do not allow copies */
52  AP_AHRS_NavEKF(const AP_AHRS_NavEKF &other) = delete;
53  AP_AHRS_NavEKF &operator=(const AP_AHRS_NavEKF&) = delete;
54 
55  // return the smoothed gyro vector corrected for drift
56  const Vector3f &get_gyro(void) const override;
57  const Matrix3f &get_rotation_body_to_ned(void) const override;
58 
59  // return the current drift correction integrator value
60  const Vector3f &get_gyro_drift(void) const override;
61 
62  // reset the current gyro drift estimate
63  // should be called if gyro offsets are recalculated
64  void reset_gyro_drift() override;
65 
66  void update(bool skip_ins_update=false) override;
67  void reset(bool recover_eulers = false) override;
68 
69  // reset the current attitude, used on new IMU calibration
70  void reset_attitude(const float &roll, const float &pitch, const float &yaw) override;
71 
72  // dead-reckoning support
73  bool get_position(struct Location &loc) const override;
74 
75  // get latest altitude estimate above ground level in meters and validity flag
76  bool get_hagl(float &hagl) const override;
77 
78  // status reporting of estimated error
79  float get_error_rp() const override;
80  float get_error_yaw() const override;
81 
82  // return a wind estimation vector, in m/s
83  Vector3f wind_estimate() const override;
84 
85  // return an airspeed estimate if available. return true
86  // if we have an estimate
87  bool airspeed_estimate(float *airspeed_ret) const override;
88 
89  // true if compass is being used
90  bool use_compass() override;
91 
92  // we will need to remove these to fully hide which EKF we are using
94  return EKF2;
95  }
96  const NavEKF2 &get_NavEKF2_const(void) const {
97  return EKF2;
98  }
99 
101  return EKF3;
102  }
103  const NavEKF3 &get_NavEKF3_const(void) const {
104  return EKF3;
105  }
106 
107  // return secondary attitude solution if available, as eulers in radians
108  bool get_secondary_attitude(Vector3f &eulers) const override;
109 
110  // return secondary attitude solution if available, as quaternion
111  bool get_secondary_quaternion(Quaternion &quat) const override;
112 
113  // return secondary position solution if available
114  bool get_secondary_position(struct Location &loc) const override;
115 
116  // EKF has a better ground speed vector estimate
117  Vector2f groundspeed_vector() override;
118 
119  const Vector3f &get_accel_ef(uint8_t i) const override;
120  const Vector3f &get_accel_ef() const override;
121 
122  // Retrieves the corrected NED delta velocity in use by the inertial navigation
123  void getCorrectedDeltaVelocityNED(Vector3f& ret, float& dt) const override;
124 
125  // blended accelerometer values in the earth frame in m/s/s
126  const Vector3f &get_accel_ef_blended() const override;
127 
128  // set the EKF's origin location in 10e7 degrees. This should only
129  // be called when the EKF has no absolute position reference (i.e. GPS)
130  // from which to decide the origin on its own
131  bool set_origin(const Location &loc) override;
132 
133  // returns the inertial navigation origin in lat/lon/alt
134  bool get_origin(Location &ret) const override;
135 
136  bool have_inertial_nav() const override;
137 
138  bool get_velocity_NED(Vector3f &vec) const override;
139 
140  // return the relative position NED to either home or origin
141  // return true if the estimate is valid
142  bool get_relative_position_NED_home(Vector3f &vec) const override;
143  bool get_relative_position_NED_origin(Vector3f &vec) const override;
144 
145  // return the relative position NE to either home or origin
146  // return true if the estimate is valid
147  bool get_relative_position_NE_home(Vector2f &posNE) const override;
148  bool get_relative_position_NE_origin(Vector2f &posNE) const override;
149 
150  // return the relative position down to either home or origin
151  // baro will be used for the _home relative one if the EKF isn't
152  void get_relative_position_D_home(float &posD) const override;
153  bool get_relative_position_D_origin(float &posD) const override;
154 
155  // Get a derivative of the vertical position in m/s which is kinematically consistent with the vertical position is required by some control loops.
156  // 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.
157  bool get_vert_pos_rate(float &velocity) const;
158 
159  // write optical flow measurements to EKF
160  void writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, Vector2f &rawGyroRates, uint32_t &msecFlowMeas, const Vector3f &posOffset);
161 
162  // write body odometry measurements to the EKF
163  void writeBodyFrameOdom(float quality, const Vector3f &delPos, const Vector3f &delAng, float delTime, uint32_t timeStamp_ms, const Vector3f &posOffset);
164 
165  // Write position and quaternion data from an external navigation system
166  void writeExtNavData(const Vector3f &sensOffset, const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint32_t resetTime_ms) override;
167 
168  // inhibit GPS usage
169  uint8_t setInhibitGPS(void);
170 
171  // get speed limit
172  void getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) const;
173 
174  void set_ekf_use(bool setting);
175 
176  // is the AHRS subsystem healthy?
177  bool healthy() const override;
178 
179  // true if the AHRS has completed initialisation
180  bool initialised() const override;
181 
182  // get_filter_status - returns filter status as a series of flags
183  bool get_filter_status(nav_filter_status &status) const;
184 
185  // get compass offset estimates
186  // true if offsets are valid
187  bool getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const;
188 
189  // report any reason for why the backend is refusing to initialise
190  const char *prearm_failure_reason(void) const override;
191 
192  // return the amount of yaw angle change due to the last yaw angle reset in radians
193  // returns the time of the last yaw angle reset or 0 if no reset has ever occurred
194  uint32_t getLastYawResetAngle(float &yawAng) const override;
195 
196  // return the amount of NE position change in meters due to the last reset
197  // returns the time of the last reset or 0 if no reset has ever occurred
198  uint32_t getLastPosNorthEastReset(Vector2f &pos) const override;
199 
200  // return the amount of NE velocity change in meters/sec due to the last reset
201  // returns the time of the last reset or 0 if no reset has ever occurred
202  uint32_t getLastVelNorthEastReset(Vector2f &vel) const override;
203 
204  // return the amount of vertical position change due to the last reset in meters
205  // returns the time of the last reset or 0 if no reset has ever occurred
206  uint32_t getLastPosDownReset(float &posDelta) const override;
207 
208  // Resets the baro so that it reads zero at the current height
209  // Resets the EKF height to zero
210  // Adjusts the EKf origin height so that the EKF height + origin height is the same as before
211  // Returns true if the height datum reset has been performed
212  // If using a range finder for height no reset is performed and it returns false
213  bool resetHeightDatum() override;
214 
215  // send a EKF_STATUS_REPORT for current EKF
216  void send_ekf_status_report(mavlink_channel_t chan) const;
217 
218  // get_hgt_ctrl_limit - get maximum height to be observed by the control loops in meters and a validity flag
219  // this is used to limit height during optical flow navigation
220  // it will return invalid when no limiting is required
221  bool get_hgt_ctrl_limit(float &limit) const override;
222 
223  // get_location - updates the provided location with the latest
224  // calculated location including absolute altitude
225  // returns true on success (i.e. the EKF knows it's latest
226  // position), false on failure
227  bool get_location(struct Location &loc) const;
228 
229  // get_variances - provides the innovations normalised using the innovation variance where a value of 0
230  // indicates perfect consistency between the measurement and the EKF solution and a value of of 1 is the maximum
231  // inconsistency that will be accepted by the filter
232  // boolean false is returned if variances are not available
233  bool get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const override;
234 
235  // returns the expected NED magnetic field
236  bool get_mag_field_NED(Vector3f& ret) const;
237 
238  // returns the estimated magnetic field offsets in body frame
239  bool get_mag_field_correction(Vector3f &ret) const override;
240 
241  void setTakeoffExpected(bool val);
242  void setTouchdownExpected(bool val);
243 
244  bool getGpsGlitchStatus() const;
245 
246  // used by Replay to force start at right timestamp
247  void force_ekf_start(void) { _force_ekf = true; }
248 
249  // is the EKF backend doing its own sensor logging?
250  bool have_ekf_logging(void) const override;
251 
252  // get the index of the current primary accelerometer sensor
253  uint8_t get_primary_accel_index(void) const override;
254 
255  // get the index of the current primary gyro sensor
256  uint8_t get_primary_gyro_index(void) const override;
257 
258 private:
262 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
264 #endif
265  };
266  EKF_TYPE active_EKF_type(void) const;
267 
268  bool always_use_EKF() const {
270  }
271 
283  const uint16_t startup_delay_ms = 1000;
284  uint32_t start_time_ms = 0;
286 
287  uint8_t ekf_type(void) const;
288  void update_DCM(bool skip_ins_update);
289  void update_EKF2(void);
290  void update_EKF3(void);
291 
292  // get the index of the current primary IMU
293  uint8_t get_primary_IMU_index(void) const;
294 
295 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
298  void update_SITL(void);
299 #endif
300 };
301 #endif
void get_relative_position_D_home(float &posD) const override
Vector3f _gyro_estimate
bool initialised() const override
bool get_relative_position_D_origin(float &posD) const override
bool use_compass() override
float roll
Definition: AP_AHRS.h:224
void getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) const
uint8_t get_primary_IMU_index(void) const
void send_ekf_status_report(mavlink_channel_t chan) const
void update_SITL(void)
const Vector3f & get_accel_ef_blended() const override
bool resetHeightDatum() override
float get_error_yaw() const override
void writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, Vector2f &rawGyroRates, uint32_t &msecFlowMeas, const Vector3f &posOffset)
bool get_secondary_attitude(Vector3f &eulers) const override
const Vector3f & get_gyro(void) const override
bool have_inertial_nav() const override
bool healthy() const override
const NavEKF3 & get_NavEKF3_const(void) const
bool get_secondary_position(struct Location &loc) const override
float get_error_rp() const override
bool get_filter_status(nav_filter_status &status) const
void update_EKF3(void)
bool getGpsGlitchStatus() const
uint8_t ekf_type(void) const
AP_AHRS_NavEKF(NavEKF2 &_EKF2, NavEKF3 &_EKF3, Flags flags=FLAG_NONE)
bool get_hagl(float &hagl) const override
bool get_mag_field_correction(Vector3f &ret) const override
bool set_origin(const Location &loc) override
float pitch
Definition: AP_AHRS.h:225
bool getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const
void setTakeoffExpected(bool val)
bool get_location(struct Location &loc) const
float yaw
Definition: AP_AHRS.h:226
uint32_t start_time_ms
void reset_attitude(const float &roll, const float &pitch, const float &yaw) override
Vector3f _dcm_attitude
void update_EKF2(void)
bool get_origin(Location &ret) const override
Vector3f _accel_ef_ekf_blended
uint32_t getLastYawResetAngle(float &yawAng) const override
NavEKF3 & get_NavEKF3(void)
bool get_relative_position_NED_home(Vector3f &vec) const override
bool airspeed_estimate(float *airspeed_ret) const override
const char * prearm_failure_reason(void) const override
const Vector3f & get_accel_ef() const override
void reset(bool recover_eulers=false) override
void getCorrectedDeltaVelocityNED(Vector3f &ret, float &dt) const override
const NavEKF2 & get_NavEKF2_const(void) const
bool get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const override
uint8_t get_primary_accel_index(void) const override
void update(bool skip_ins_update=false) override
Vector3f _accel_ef_ekf[INS_MAX_INSTANCES]
void writeBodyFrameOdom(float quality, const Vector3f &delPos, const Vector3f &delAng, float delTime, uint32_t timeStamp_ms, const Vector3f &posOffset)
uint32_t getLastPosDownReset(float &posDelta) const override
bool get_position(struct Location &loc) const override
uint8_t setInhibitGPS(void)
bool get_relative_position_NE_origin(Vector2f &posNE) const override
bool have_ekf_logging(void) const override
SITL::SITL * _sitl
uint32_t _last_body_odm_update_ms
Matrix3f _dcm_matrix
bool get_vert_pos_rate(float &velocity) const
AP_AHRS_NavEKF & operator=(const AP_AHRS_NavEKF &)=delete
NavEKF2 & get_NavEKF2(void)
Vector3f wind_estimate() const override
uint8_t get_primary_gyro_index(void) const override
bool always_use_EKF() const
AP_HAL::AnalogSource * chan
Definition: AnalogIn.cpp:8
const Matrix3f & get_rotation_body_to_ned(void) const override
void reset_gyro_drift() override
EKF_TYPE active_EKF_type(void) const
bool get_mag_field_NED(Vector3f &ret) const
uint32_t getLastVelNorthEastReset(Vector2f &vel) const override
bool get_relative_position_NED_origin(Vector3f &vec) const override
void setTouchdownExpected(bool val)
void force_ekf_start(void)
#define INS_MAX_INSTANCES
bool get_secondary_quaternion(Quaternion &quat) const override
void set_ekf_use(bool setting)
bool get_relative_position_NE_home(Vector2f &posNE) const override
Vector2f groundspeed_vector() override
bool get_velocity_NED(Vector3f &vec) const override
const uint16_t startup_delay_ms
bool get_hgt_ctrl_limit(float &limit) const override
void writeExtNavData(const Vector3f &sensOffset, const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint32_t resetTime_ms) override
Vector3f _gyro_drift
const Vector3f & get_gyro_drift(void) const override
uint32_t getLastPosNorthEastReset(Vector2f &pos) const override
void update_DCM(bool skip_ins_update)