Line data Source code
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 : * AHRS (Attitude Heading Reference System) frontend interface for
20 : * ArduPilot
21 : *
22 : */
23 :
24 : #include "AP_AHRS_config.h"
25 :
26 : #include <AP_HAL/Semaphores.h>
27 :
28 : #include "AP_AHRS_Backend.h"
29 : #include "AP_AHRS_NavEKF2.h"
30 : #include "AP_AHRS_NavEKF3.h"
31 : #include <AP_NavEKF/AP_Nav_Common.h> // definitions shared by inertial and ekf nav filters
32 :
33 : #include "AP_AHRS_DCM.h"
34 : #include "AP_AHRS_SIM.h"
35 : #include "AP_AHRS_External.h"
36 :
37 : // forward declare view class
38 : class AP_AHRS_View;
39 :
40 : // fwd declare GSF estimator
41 : class EKFGSF_yaw;
42 :
43 : class AP_AHRS {
44 : friend class AP_AHRS_View;
45 : public:
46 :
47 : // copy this into our namespace
48 : using Status = NavFilterStatusBit;
49 :
50 : enum Flags {
51 : FLAG_ALWAYS_USE_EKF = 0x1,
52 : };
53 :
54 : // has_status returns information about the EKF health and
55 : // capabilities. It is currently invalid to call this when a
56 : // backend is in charge which returns false for get_filter_status
57 : // - so this will simply return false for DCM, for example.
58 : bool has_status(Status status) const;
59 :
60 : // Constructor
61 : AP_AHRS(uint8_t flags = 0);
62 :
63 : // initialise
64 : void init(void);
65 :
66 : /* Do not allow copies */
67 : CLASS_NO_COPY(AP_AHRS);
68 :
69 : // get singleton instance
70 2903402212 : static AP_AHRS *get_singleton() {
71 2903402212 : return _singleton;
72 : }
73 :
74 : #if AP_AHRS_NAVEKF2_ENABLED
75 : AP_AHRS_NavEKF2 ekf2;
76 : #endif
77 : #if AP_AHRS_NAVEKF3_ENABLED
78 : AP_AHRS_NavEKF3 ekf3;
79 : #endif
80 :
81 : // periodically checks to see if we should update the AHRS
82 : // orientation (e.g. based on the AHRS_ORIENTATION parameter)
83 : // allow for runtime change of orientation
84 : // this makes initial config easier
85 : void update_orientation();
86 :
87 : // allow threads to lock against AHRS update
88 460340 : HAL_Semaphore &get_semaphore(void) {
89 460340 : return _rsem;
90 : }
91 :
92 : // return the smoothed gyro vector corrected for drift
93 72586451 : const Vector3f &get_gyro(void) const { return state.gyro_estimate; }
94 :
95 : // return the current drift correction integrator value
96 81241648 : const Vector3f &get_gyro_drift(void) const { return state.gyro_drift; }
97 :
98 : // reset the current gyro drift estimate
99 : // should be called if gyro offsets are recalculated
100 : void reset_gyro_drift();
101 :
102 : void update(bool skip_ins_update=false);
103 : void reset();
104 :
105 : // get current location estimate
106 : bool get_location(Location &loc) const WARN_IF_UNUSED;
107 :
108 : // get latest altitude estimate above ground level in meters and validity flag
109 2473248 : bool get_hagl(float &hagl) const WARN_IF_UNUSED { return active_estimates->get_hagl(hagl); }
110 :
111 : // status reporting of estimated error
112 : float get_error_rp() const;
113 : float get_error_yaw() const;
114 :
115 : /*
116 : * wind estimation support
117 : */
118 :
119 : // enable wind estimation
120 692 : void set_wind_estimation_enabled(bool b) { wind_estimation_enabled = b; }
121 :
122 : // wind_estimation_enabled returns true if wind estimation is enabled
123 68598340 : bool get_wind_estimation_enabled() const { return wind_estimation_enabled; }
124 :
125 : // return a wind estimation vector in "wind" (m/s); returns false if
126 : // we have no valid estimate
127 : bool get_wind(Vector3f &wind) const;
128 :
129 : // Determine how aligned heading_deg is with the wind. Return result
130 : // is 1.0 when perfectly aligned heading into wind, -1 when perfectly
131 : // aligned with-wind, and zero when perfect cross-wind. There is no
132 : // distinction between a left or right cross-wind. Wind speed is ignored
133 : float wind_alignment(const float heading_deg) const;
134 :
135 : // returns forward head-wind component in m/s. Negative means tail-wind
136 : float head_wind(void) const;
137 :
138 : #if AP_AHRS_EXTERNAL_WIND_ESTIMATE_ENABLED
139 14 : void set_external_wind_estimate(float speed, float direction) {
140 14 : dcm.set_external_wind_estimate(speed, direction);
141 14 : }
142 : #endif
143 :
144 : // return the parameter AHRS_WIND_MAX in metres per second
145 9400250 : uint8_t get_max_wind() const {
146 9400250 : return _wind_max;
147 : }
148 :
149 : /*
150 : * airspeed support
151 : */
152 :
153 : // get apparent to true airspeed ratio
154 : float get_EAS2TAS(void) const;
155 :
156 : // get air density / sea level density - decreases as altitude climbs
157 : float get_air_density_ratio(void) const;
158 :
159 : // return an (equivalent) airspeed estimate if available. return
160 : // true if airspeed_ret is valid. This value may be derived from
161 : // airspeed data or synthesised from other sources.
162 : bool airspeed_EAS(float &airspeed_ret) const;
163 :
164 : enum AirspeedEstimateType : uint8_t {
165 : NO_NEW_ESTIMATE = 0,
166 : AIRSPEED_SENSOR = 1,
167 : DCM_SYNTHETIC = 2,
168 : EKF3_SYNTHETIC = 3,
169 : SIM = 4,
170 : };
171 :
172 : // return an (equivalent) airspeed estimate if available. return
173 : // true if airspeed_ret is valid. This value may be derived from
174 : // airspeed data or synthesised from other sources (the type
175 : // return parameter allows you to distinguish).
176 : bool airspeed_EAS(float &airspeed_ret, AirspeedEstimateType &type) const;
177 :
178 : // return true if the current AHRS airspeed estimate (from airspeed_estimate method) is directly derived from an airspeed sensor
179 : bool using_airspeed_sensor() const;
180 :
181 : // return a true airspeed (navigation airspeed) if
182 : // available. return true if airspeed_ret is valid. This value
183 : // may be derived from actual airspeed sensor data or synthesized
184 : // from other sources.
185 : bool airspeed_TAS(float &airspeed_ret) const;
186 :
187 : // return estimate of true airspeed vector in body frame in m/s
188 : // returns false if estimate is unavailable
189 : bool airspeed_vector_TAS(Vector3f &vec) const;
190 :
191 : // return the innovation in m/s, innovation variance in (m/s)^2 and age in msec of the last TAS measurement processed for a given sensor instance
192 : // returns false if the data is unavailable
193 : bool airspeed_health_data(uint8_t instance, float &innovation, float &innovationVariance, uint32_t &age_ms) const;
194 :
195 : // return true if a airspeed sensor is enabled
196 564382 : bool airspeed_sensor_enabled(void) const {
197 : // FIXME: make this a method on the active backend
198 564382 : return AP_AHRS_Backend::airspeed_sensor_enabled();
199 : }
200 :
201 : // return true if a airspeed from a specific airspeed sensor is enabled
202 82843001 : bool airspeed_sensor_enabled(uint8_t airspeed_index) const {
203 : // FIXME: make this a method on the active backend
204 82843001 : return AP_AHRS_Backend::airspeed_sensor_enabled(airspeed_index);
205 : }
206 :
207 : // true if compass is being used
208 : bool use_compass();
209 :
210 : // return the quaternion defining the rotation from NED to XYZ (body) axes
211 : bool get_quaternion(Quaternion &quat) const WARN_IF_UNUSED;
212 :
213 : // return secondary estimates; note that this may return nullptr!
214 1120989 : const AP_AHRS_Backend::Estimates *get_secondary_estimates() const {
215 1120989 : return secondary_estimates;
216 : }
217 :
218 : // EKF has a better ground speed vector estimate
219 1924325 : const Vector2f &groundspeed_vector() const { return state.ground_speed_vec; }
220 :
221 : // return ground speed estimate in meters/second. Used by ground vehicles.
222 5109053 : float groundspeed(void) const { return state.ground_speed; }
223 :
224 74573092 : const Vector3f &get_accel_ef() const {
225 74573092 : return state.accel_ef;
226 : }
227 :
228 : // Retrieves the corrected NED delta velocity in use by the inertial navigation
229 341176 : bool getCorrectedDeltaVelocityNED(Vector3f& ret, float& dt) const {
230 341176 : if (!state.corrected_dv_valid) {
231 0 : return false;
232 : }
233 341176 : ret = state.corrected_dv;
234 341176 : dt = state.corrected_dv_dt;
235 341176 : return true;
236 : }
237 :
238 : // set the EKF's origin location in 10e7 degrees. This should only
239 : // be called when the EKF has no absolute position reference (i.e. GPS)
240 : // from which to decide the origin on its own
241 : bool set_origin(const Location &loc) WARN_IF_UNUSED;
242 :
243 : // Set the origin to the last recorded location if option bit set and not using GPS
244 : // This is useful for position controlled modes without GPS
245 : void use_recorded_origin_maybe();
246 :
247 : #if AP_AHRS_POSITION_RESET_ENABLED
248 : // Set the EKF's NE horizontal position states and their corresponding variances from the supplied WGS-84 location
249 : // and 1-sigma horizontal position uncertainty. This can be used when the EKF is dead reckoning to periodically
250 : // correct the position. If the EKF is is still using data from a position sensor such as GPS, the position set
251 : // will not be performed.
252 : // pos_accuracy is the standard deviation of the horizontal position uncertainty in metres.
253 : // The altitude element of the location is not used.
254 : // Returns true if the set was successful.
255 : bool handle_external_position_estimate(const Location &loc, float pos_accuracy, uint32_t timestamp_);
256 : #endif
257 :
258 : // returns the inertial navigation origin in lat/lon/alt
259 : bool get_origin(Location &ret) const WARN_IF_UNUSED;
260 :
261 : bool have_inertial_nav() const;
262 :
263 : // return a ground velocity in meters/second, North/East/Down
264 : // order. Must only be called if have_inertial_nav() is true
265 : bool get_velocity_NED(Vector3f &vec) const WARN_IF_UNUSED;
266 :
267 : // return the relative position NED from home
268 : // return true if the estimate is valid
269 : bool get_relative_position_NED_home(Vector3f &vec) const WARN_IF_UNUSED;
270 : bool get_relative_position_NE_home(Vector2f &posNE) const WARN_IF_UNUSED;
271 : void get_relative_position_D_home(float &posD) const;
272 :
273 : // return the relative position NED from origin
274 : // return true if the estimate is valid
275 40790934 : bool get_relative_position_NED_origin(Vector3p &vec) const WARN_IF_UNUSED {
276 40790934 : vec.xy() = active_estimates->position_NE;
277 40790934 : vec.z = active_estimates->position_D;
278 40790934 : return (active_estimates->position_NE_valid && active_estimates->position_D_valid);
279 : }
280 : bool get_relative_position_NED_origin_float(Vector3f &vec) const WARN_IF_UNUSED;
281 :
282 : // return the relative position NE from home or origin
283 : // return true if the estimate is valid
284 11601098 : bool get_relative_position_NE_origin(Vector2p &posNE) const WARN_IF_UNUSED {
285 11601098 : posNE = active_estimates->position_NE;
286 11601098 : return active_estimates->position_NE_valid;
287 : }
288 : bool get_relative_position_NE_origin_float(Vector2f &posNE) const WARN_IF_UNUSED;
289 :
290 : // return the relative position down from home or origin
291 : // baro will be used for the _home relative one if the EKF isn't
292 63456386 : bool get_relative_position_D_origin(postype_t &posD) const WARN_IF_UNUSED {
293 63456386 : posD = active_estimates->position_D;
294 63456386 : return active_estimates->position_D_valid;
295 : }
296 :
297 : bool get_relative_position_D_origin_float(float &posD) const WARN_IF_UNUSED;
298 :
299 : // return location corresponding to vector relative to the
300 : // vehicle's origin
301 : bool get_location_from_origin_offset_NED(Location &loc, const Vector3p &offset_ned) const WARN_IF_UNUSED;
302 : bool get_location_from_home_offset_NED(Location &loc, const Vector3p &offset_ned) const WARN_IF_UNUSED;
303 :
304 : // get velocity down in m/s. This returns get_velocity_NED.z() if available, otherwise falls back to get_vert_pos_rate_D()
305 : // if high_vibes is true then this is equivalent to get_vert_pos_rate_D
306 : bool get_velocity_D(float &velD, bool high_vibes = false) const WARN_IF_UNUSED;
307 :
308 : // Get a derivative of the vertical position in m/s which is kinematically consistent with the vertical position is required by some control loops.
309 : // 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.
310 : bool get_vert_pos_rate_D(float &velocity) const;
311 :
312 : // write optical flow measurements to EKF
313 : void writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset, const float heightOverride);
314 :
315 : // retrieve latest corrected optical flow samples (used for calibration)
316 : bool getOptFlowSample(uint32_t& timeStamp_ms, Vector2f& flowRate, Vector2f& bodyRate, Vector2f& losPred) const;
317 :
318 : // write body odometry measurements to the EKF
319 : void writeBodyFrameOdom(float quality, const Vector3f &delPos, const Vector3f &delAng, float delTime, uint32_t timeStamp_ms, uint16_t delay_ms, const Vector3f &posOffset);
320 :
321 : // Writes the default equivalent airspeed and its 1-sigma uncertainty in m/s to be used in forward flight if a measured airspeed is required and not available.
322 : void writeDefaultAirSpeed(float airspeed, float uncertainty);
323 :
324 : // Write position and quaternion data from an external navigation system
325 : void writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint16_t delay_ms, uint32_t resetTime_ms);
326 :
327 : // Write velocity data from an external navigation system
328 : void writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeStamp_ms, uint16_t delay_ms);
329 :
330 : // Write terrain (derived from SRTM) altitude in meters above sea level
331 : void writeTerrainAMSL(float alt_amsl_m);
332 :
333 : // get speed limit imposed by the estimator
334 3029099 : float get_control_ground_speed_limit_ms() const { return active_estimates->control_ground_speed_limit_ms; }
335 : // get scaler used to limit response due to poor AHRS estimates
336 19264067 : float get_control_gain_scaler_XY() const { return active_estimates->control_gain_scaler_XY; }
337 51134204 : float get_control_gain_scaler_Z() const { return active_estimates->control_gain_scaler_Z; }
338 :
339 : // is the AHRS subsystem healthy?
340 : bool healthy() const;
341 :
342 : // returns false if we fail arming checks, in which case the buffer will be populated with a failure message
343 : // requires_position should be true if horizontal position configuration should be checked
344 : bool pre_arm_check(bool requires_position, char *failure_msg, uint8_t failure_msg_len) const;
345 :
346 : // true if the AHRS has completed initialisation
347 1127373 : bool initialised() const {
348 1127373 : return configured_estimates->initialised;
349 : }
350 :
351 : #if AP_AHRS_DCM_ENABLED
352 : // return true if *DCM* yaw has been initialised
353 5070 : bool dcm_yaw_initialised(void) const {
354 5070 : return dcm.yaw_initialised();
355 : }
356 : #endif
357 :
358 : // get_filter_status - returns filter status as a series of flags
359 17933104 : bool get_filter_status(nav_filter_status &status) const {
360 17933104 : status = configured_estimates->filter_status;
361 17933104 : return configured_estimates->filter_status_valid;
362 : }
363 :
364 : #if AP_COMPASS_LEARN_COPY_FROM_EKF_ENABLED
365 : // get compass offset estimates, in body frame, milligauss
366 : // true if offsets are valid
367 12 : bool getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const {
368 12 : return configured_backend->get_mag_offsets(mag_idx, magOffsets);
369 : }
370 : #endif // AP_COMPASS_LEARN_COPY_FROM_EKF_ENABLED
371 :
372 : // returns the number of times the yaw angle has been reset
373 40970678 : uint16_t get_yaw_reset_count(void) const {
374 40970678 : return yaw_reset_tracker.count();
375 : }
376 :
377 : // returns the number of times the NE position has been reset
378 23368064 : uint16_t get_position_NE_reset_count(void) const {
379 23368064 : return position_NE_reset_tracker.count();
380 : }
381 :
382 : // returns the number of times the D position has been reset
383 31751271 : uint16_t get_position_D_reset_count(void) const {
384 31751271 : return position_D_reset_tracker.count();
385 : }
386 :
387 : // returns a counter which is incremented each time the estimator's output resets
388 38789995 : uint16_t get_last_attitude_reset_count() const {
389 38789995 : return attitude_reset_tracker.count();
390 : }
391 :
392 : // Resets the baro so that it reads zero at the current height
393 : // Resets the EKF height to zero
394 : // Adjusts the EKf origin height so that the EKF height + origin height is the same as before
395 : void resetHeightDatum();
396 :
397 : // send a EKF_STATUS_REPORT for current EKF
398 : void send_ekf_status_report(class GCS_MAVLINK &link) const;
399 :
400 : // get_hgt_ctrl_limit - get maximum height to be observed by the control loops in meters and a validity flag
401 : // this is used to limit height during optical flow navigation
402 : // it will return invalid when no limiting is required
403 1430161 : bool get_hgt_ctrl_limit(float &limit) const {
404 1430161 : limit = active_estimates->control_height_limit_m;
405 1430161 : return active_estimates->control_height_limit_valid;
406 : }
407 :
408 : // Set to true if the terrain underneath is stable enough to be used as a height reference
409 : // this is not related to terrain following
410 : void set_terrain_hgt_stable(bool stable);
411 :
412 : // return the innovations for the specified instance
413 : // An out of range instance (eg -1) returns data for the primary instance
414 759512 : bool get_innovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const {
415 759512 : return configured_backend->get_innovations(velInnov, posInnov, magInnov, tasInnov, yawInnov);
416 : }
417 :
418 : // returns true when the state estimates are significantly degraded by vibration
419 759512 : bool is_vibration_affected() const {
420 759512 : return configured_estimates->is_vibration_affected;
421 : }
422 :
423 : // get_variances - provides the innovations normalised using the innovation variance where a value of 0
424 : // indicates perfect consistency between the measurement and the EKF solution and a value of 1 is the maximum
425 : // inconsistency that will be accepted by the filter
426 : // boolean false is returned if variances are not available
427 797040 : bool get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar) const {
428 797040 : velVar = configured_estimates->velVar;
429 797040 : posVar = configured_estimates->posVar;
430 797040 : hgtVar = configured_estimates->hgtVar;
431 797040 : magVar = configured_estimates->magVar;
432 797040 : tasVar = configured_estimates->tasVar;
433 797040 : return configured_estimates->variances_valid;
434 : }
435 :
436 : // get 1-sigma position and velocity uncertainty derived from the EKF state error covariance matrix P
437 : // pos_horiz_m: 2D RMS horizontal position uncertainty (m)
438 : // pos_vert_m: 1-sigma vertical position uncertainty (m)
439 : // vel_m_s: 1-sigma worst-case NED velocity uncertainty (m/s)
440 : // returns false if not available
441 : bool get_pos_vel_uncertainty(float &pos_horiz_m, float &pos_vert_m, float &vel_m_s) const;
442 :
443 : // get a source's velocity innovations
444 : // returns true on success and results are placed in innovations and variances arguments
445 : bool get_vel_innovations_and_variances_for_source(uint8_t source, Vector3f &innovations, Vector3f &variances) const WARN_IF_UNUSED;
446 :
447 : #if AP_AHRS_GET_MAG_DATA_ENABLED
448 : // returns the expected NED magnetic field
449 1890 : bool get_mag_field_NED(Vector3f& ret) const {
450 1890 : ret = active_estimates->mag_field_NED;
451 1890 : return active_estimates->mag_field_NED_valid;
452 : }
453 :
454 : // returns the estimated magnetic field offsets in body frame
455 1890 : bool get_mag_field_correction(Vector3f &ret) const {
456 1890 : ret = active_estimates->mag_field_corrections;
457 1890 : return active_estimates->mag_field_corrections_valid;
458 : }
459 : #endif // AP_AHRS_GET_MAG_DATA_ENABLED
460 :
461 : // return the index of the airspeed we should use for airspeed measurements
462 : // with multiple airspeed sensors and airspeed affinity in EKF3, it is possible to have switched
463 : // over to a lane not using the primary airspeed sensor, so AHRS should know which airspeed sensor
464 : // to use, i.e, the one being used by the primary lane. A lane switch could have happened due to an
465 : // airspeed sensor fault, which makes this even more necessary
466 : uint8_t get_active_airspeed_index() const;
467 :
468 : // get the index of the current primary accelerometer sensor
469 509697 : uint8_t get_primary_accel_index(void) const { return state.primary_accel; }
470 :
471 : // get the index of the current primary gyro sensor
472 38656363 : uint8_t get_primary_gyro_index(void) const { return state.primary_gyro; }
473 :
474 : // see if EKF lane switching is possible to avoid EKF failsafe
475 39 : void check_lane_switch(void) {
476 39 : active_backend->check_lane_switch();
477 39 : }
478 :
479 : // request EKF yaw reset to try and avoid the need for an EKF lane switch or failsafe
480 6 : void request_yaw_reset(void) {
481 6 : active_backend->request_yaw_reset();
482 6 : }
483 :
484 : #if AP_AHRS_EKF_RESET_ENABLED
485 : // request full backend reset, currently only implemented for EKF3
486 : // returns true if the reset was performed
487 : bool reset_configured_backend(void);
488 : #endif // AP_AHRS_EKF_RESET_ENABLED
489 :
490 : // set position, velocity and yaw sources to either 0=primary, 1=secondary, 2=tertiary
491 : void set_posvelyaw_source_set(AP_NavEKF_Source::SourceSetSelection source_set_idx);
492 :
493 : //returns index of active source set used, 0=primary, 1=secondary, 2=tertiary
494 : uint8_t get_posvelyaw_source_set() const;
495 :
496 : void Log_Write();
497 :
498 : // check if non-compass sensor is providing yaw. Allows compass pre-arm checks to be bypassed
499 : bool using_noncompass_for_yaw(void) const;
500 :
501 : // check if external nav is providing yaw
502 : bool using_extnav_for_yaw(void) const;
503 :
504 : // active_backend_configured_to_use_gps will be true if the
505 : // estimator will use GPS data in creating its estimate when the
506 : // data is good
507 92716 : bool active_backend_configured_to_use_gps() const { return active_estimates->configured_to_use_gps; }
508 :
509 : // set and save the ALT_M_NSE parameter value
510 : void set_alt_measurement_noise(float noise);
511 :
512 : enum class EKFType : uint8_t {
513 : #if AP_AHRS_DCM_ENABLED
514 : DCM = 0,
515 : #endif
516 : #if AP_AHRS_NAVEKF3_ENABLED
517 : THREE = 3,
518 : #endif
519 : #if AP_AHRS_NAVEKF2_ENABLED
520 : TWO = 2,
521 : #endif
522 : #if AP_AHRS_SIM_ENABLED
523 : SIM = 10,
524 : #endif
525 : #if AP_AHRS_EXTERNAL_ENABLED
526 : EXTERNAL = 11,
527 : #endif
528 : };
529 :
530 : // returns a canonicalised and valid EKFType, as opposed to the raw
531 : // parameter value which may be any value the user has set
532 134481003 : EKFType configured_ekf_type(void) const {
533 134481003 : return state.configured_ekf_type;
534 : }
535 :
536 : // set the selected ekf type, for RC aux control
537 0 : void set_ekf_type(EKFType ahrs_type) {
538 0 : _ekf_type.set(ahrs_type);
539 0 : }
540 :
541 : // for holding parameters
542 : static const struct AP_Param::GroupInfo var_info[];
543 :
544 : // create a view
545 : AP_AHRS_View *create_view(enum Rotation rotation, float pitch_trim_deg=0);
546 :
547 : // write AOA and SSA information to dataflash logs:
548 : void Write_AOA_SSA(void) const;
549 :
550 : // return AOA
551 1076619 : float getAOA(void) const { return _AOA; }
552 :
553 : // return SSA
554 1076619 : float getSSA(void) const { return _SSA; }
555 :
556 : /*
557 : * trim-related functions
558 : */
559 :
560 : // get trim
561 207903152 : const Vector3f &get_trim() const { return _trim.get(); }
562 :
563 : // set trim
564 : void set_trim(const Vector3f &new_trim);
565 :
566 : // add_trim - adjust the roll and pitch trim up to a total of 10 degrees
567 : void add_trim(float roll_in_radians, float pitch_in_radians, bool save_to_eeprom = true);
568 :
569 : // trim rotation matrices:
570 166806357 : const Matrix3f& get_rotation_autopilot_body_to_vehicle_body(void) const { return _rotation_autopilot_body_to_vehicle_body; }
571 82843001 : const Matrix3f& get_rotation_vehicle_body_to_autopilot_body(void) const { return _rotation_vehicle_body_to_autopilot_body; }
572 :
573 : // Logging functions
574 : void Log_Write_Home_And_Origin();
575 : void Write_Attitude(const Vector3f &targets) const;
576 :
577 : enum class LogOriginType {
578 : ekf_origin = 0,
579 : ahrs_home = 1
580 : };
581 : void Write_Origin(LogOriginType origin_type, const Location &loc) const;
582 : void write_video_stabilisation() const;
583 :
584 : // return a smoothed and corrected gyro vector in radians/second
585 : // using the latest ins data (which may not have been consumed by
586 : // the EKF yet)
587 : Vector3f get_gyro_latest(void) const;
588 :
589 : // get yaw rate in earth frame in radians/sec
590 8965047 : float get_yaw_rate_earth(void) const {
591 8965047 : return get_gyro() * get_rotation_body_to_ned().c;
592 : }
593 :
594 : /*
595 : * home-related functionality
596 : */
597 :
598 : // get the home location. This is const to prevent any changes to
599 : // home without telling AHRS about the change
600 1132066964 : const Location &get_home(void) const {
601 1132066964 : return _home;
602 : }
603 :
604 : // functions to handle locking of home. Some vehicles use this to
605 : // allow GCS to lock in a home location.
606 95 : void lock_home() {
607 95 : _home_locked = true;
608 95 : }
609 10017 : bool home_is_locked() const {
610 10017 : return _home_locked;
611 : }
612 :
613 : // returns true if home is set
614 96789094 : bool home_is_set(void) const {
615 96789094 : return _home_is_set;
616 : }
617 :
618 : // set the home location in 10e7 degrees. This should be called
619 : // when the vehicle is at this position. It is assumed that the
620 : // current barometer and GPS altitudes correspond to this altitude
621 : bool set_home(const Location &loc) WARN_IF_UNUSED;
622 :
623 : /*
624 : * Attitude-related public methods and attributes:
625 : */
626 :
627 : #if AP_SCRIPTING_ENABLED
628 : // deprecated functions for accessing rpy. Do not use, these will
629 : // be removed.
630 0 : float get_roll() const { return roll; }
631 0 : float get_pitch() const { return pitch; }
632 0 : float get_yaw() const { return yaw; }
633 : #endif // AP_SCRIPTING_ENABLED
634 :
635 : // roll/pitch/yaw euler angles, all in radians
636 15217260 : float get_roll_rad() const { return roll; }
637 12361258 : float get_pitch_rad() const { return pitch; }
638 3738591 : float get_yaw_rad() const { return yaw; }
639 :
640 : // roll/pitch/yaw euler angles, all in degrees
641 29833973 : float get_roll_deg() const { return rpy_deg[0]; }
642 11065002 : float get_pitch_deg() const { return rpy_deg[1]; }
643 1600820 : float get_yaw_deg() const { return rpy_deg[2]; }
644 :
645 : // helper trig value accessors
646 26662511 : float cos_roll() const {
647 26662511 : return _cos_roll;
648 : }
649 26377178 : float cos_pitch() const {
650 26377178 : return _cos_pitch;
651 : }
652 2467773 : float cos_yaw() const {
653 2467773 : return _cos_yaw;
654 : }
655 56924 : float sin_roll() const {
656 56924 : return _sin_roll;
657 : }
658 382722 : float sin_pitch() const {
659 382722 : return _sin_pitch;
660 : }
661 2467773 : float sin_yaw() const {
662 2467773 : return _sin_yaw;
663 : }
664 :
665 : // floating point Euler angles (Degrees)
666 : float rpy_deg[3];
667 :
668 : // integer Euler angles (Degrees * 100)
669 : int32_t roll_sensor;
670 : int32_t pitch_sensor;
671 : int32_t yaw_sensor;
672 :
673 96717641 : const Matrix3f &get_rotation_body_to_ned(void) const { return state.dcm_matrix; }
674 :
675 : // return a Quaternion representing our current attitude in NED frame
676 : void get_quat_body_to_ned(Quaternion &quat) const;
677 :
678 : #if AP_AHRS_DCM_ENABLED
679 : // get rotation matrix specifically from DCM backend (used for
680 : // compass calibrator)
681 615123 : const Matrix3f &get_DCM_rotation_body_to_ned(void) const {
682 615123 : return dcm_estimates.dcm_matrix;
683 : }
684 : #endif
685 :
686 : // rotate a 2D vector from earth frame to body frame
687 : // in result, x is forward, y is right
688 : Vector2f earth_to_body2D(const Vector2f &ef_vector) const;
689 :
690 : // rotate a 2D vector from body frame to earth frame
691 : // in input, x is forward, y is right
692 : Vector2f body_to_earth2D(const Vector2f &bf) const WARN_IF_UNUSED;
693 : Vector2p body_to_earth2D_p(const Vector2p &bf) const WARN_IF_UNUSED;
694 :
695 : // convert a vector from body to earth frame
696 : Vector3f body_to_earth(const Vector3f &v) const;
697 :
698 : // convert a vector from earth to body frame
699 : Vector3f earth_to_body(const Vector3f &v) const;
700 :
701 : /*
702 : * methods for the benefit of LUA bindings
703 : */
704 : // return current vibration vector for primary IMU
705 : Vector3f get_vibration(void) const;
706 :
707 : // return primary accels
708 382738 : const Vector3f &get_accel(void) const {
709 382738 : return AP::ins().get_accel(get_primary_accel_index());
710 : }
711 :
712 : // return primary accel bias. This should be subtracted from
713 : // get_accel() vector to get best current body frame accel
714 : // estimate
715 6292312 : const Vector3f &get_accel_bias(void) const {
716 6292312 : return state.accel_bias;
717 : }
718 :
719 : /*
720 : * AHRS is used as a transport for vehicle-takeoff-expected and
721 : * vehicle-landing-expected:
722 : */
723 : void set_takeoff_expected(bool b);
724 :
725 41156285 : bool get_takeoff_expected(void) const {
726 41156285 : return takeoff_expected;
727 : }
728 :
729 : void set_touchdown_expected(bool b);
730 :
731 41156285 : bool get_touchdown_expected(void) const {
732 41156285 : return touchdown_expected;
733 : }
734 :
735 : /*
736 : * fly_forward is set by the vehicles to indicate the vehicle
737 : * should generally be moving in the direction of its heading.
738 : * It is an additional piece of information that the backends can
739 : * use to provide additional and/or improved estimates.
740 : */
741 9007685 : void set_fly_forward(bool b) {
742 9007685 : fly_forward = b;
743 9007685 : }
744 124847559 : bool get_fly_forward(void) const {
745 124847559 : return fly_forward;
746 : }
747 :
748 : /* we modify our behaviour based on what sort of vehicle the
749 : * vehicle code tells us we are. This information is also pulled
750 : * from AP_AHRS by other libraries
751 : */
752 : enum class VehicleClass : uint8_t {
753 : UNKNOWN,
754 : GROUND,
755 : COPTER,
756 : FIXED_WING,
757 : SUBMARINE,
758 : };
759 41156285 : VehicleClass get_vehicle_class(void) const {
760 41156285 : return _vehicle_class;
761 : }
762 2055 : void set_vehicle_class(VehicleClass vclass) {
763 2055 : _vehicle_class = vclass;
764 2055 : }
765 :
766 : // get the view
767 3 : AP_AHRS_View *get_view(void) const { return _view; };
768 :
769 : // get access to an EKFGSF_yaw estimator
770 : const EKFGSF_yaw *get_yaw_estimator(void) const;
771 :
772 : private:
773 :
774 : // roll/pitch/yaw euler angles, all in radians
775 : float roll;
776 : float pitch;
777 : float yaw;
778 :
779 : // optional view class
780 : AP_AHRS_View *_view;
781 :
782 : static AP_AHRS *_singleton;
783 :
784 : /* we modify our behaviour based on what sort of vehicle the
785 : * vehicle code tells us we are. This information is also pulled
786 : * from AP_AHRS by other libraries
787 : */
788 : VehicleClass _vehicle_class{VehicleClass::UNKNOWN};
789 :
790 : // multi-thread access support
791 : HAL_Semaphore _rsem;
792 :
793 : /*
794 : * Parameters
795 : */
796 : AP_Int8 _wind_max;
797 : AP_Int8 _board_orientation;
798 : AP_Enum<EKFType> _ekf_type;
799 :
800 : /*
801 : * DCM-backend parameters; it takes references to these
802 : */
803 : // settable parameters
804 : AP_Float _kp_yaw;
805 : AP_Float _kp;
806 : AP_Float gps_gain;
807 :
808 : AP_Float beta;
809 :
810 : AP_Enum<GPSUse> _gps_use;
811 : AP_Int8 _gps_minsats;
812 : AP_Float _origin_lat;
813 : AP_Float _origin_lon;
814 : AP_Float _origin_alt;
815 :
816 221474167 : EKFType active_EKF_type(void) const { return state.active_EKF_type; }
817 :
818 40119486 : bool always_use_EKF() const {
819 40119486 : return _ekf_flags & FLAG_ALWAYS_USE_EKF;
820 : }
821 :
822 : // check all cores providing consistent attitudes for prearm checks
823 : bool attitudes_consistent(char *failure_msg, const uint8_t failure_msg_len) const;
824 : // convenience method for setting error string:
825 : void set_failure_inconsistent_message(const char *estimator, const char *axis, float diff_rad, char *failure_msg, const uint8_t failure_msg_len) const;
826 :
827 : /*
828 : * Attitude-related private methods and attributes:
829 : */
830 : // calculate sin/cos of roll/pitch/yaw from rotation
831 : void calc_trig(const Matrix3f &rot,
832 : float &cr, float &cp, float &cy,
833 : float &sr, float &sp, float &sy) const;
834 :
835 : // update_trig - recalculates _cos_roll, _cos_pitch, etc based on latest attitude
836 : // should be called after _dcm_matrix is updated
837 : void update_trig(void);
838 :
839 : // update roll_sensor, pitch_sensor and yaw_sensor
840 : void update_cd_values(void);
841 :
842 : // helper trig variables
843 : float _cos_roll{1.0f};
844 : float _cos_pitch{1.0f};
845 : float _cos_yaw{1.0f};
846 : float _sin_roll;
847 : float _sin_pitch;
848 : float _sin_yaw;
849 :
850 : uint8_t _ekf_flags; // bitmask from Flags enumeration
851 :
852 : /*
853 : * home-related state
854 : */
855 : void load_watchdog_home();
856 : bool _checked_watchdog_home;
857 : Location _home;
858 : bool _home_is_set :1;
859 : bool _home_locked :1;
860 :
861 : // avoid setting current state repeatedly across all cores on all EKFs:
862 : enum class TriState {
863 : False = 0,
864 : True = 1,
865 : UNKNOWN = 3,
866 : };
867 :
868 : TriState terrainHgtStableState = TriState::UNKNOWN;
869 :
870 : /*
871 : * private AOA and SSA-related state and methods
872 : */
873 : float _AOA, _SSA;
874 : uint32_t _last_AOA_update_ms;
875 : void update_AOA_SSA(void);
876 :
877 : EKFType last_active_ekf_type;
878 :
879 : /*
880 : * trim-related state and private methods:
881 : */
882 :
883 : // a vector to capture the difference between the controller and body frames
884 : AP_Vector3f _trim;
885 :
886 : // cached trim rotations
887 : Vector3f _last_trim;
888 :
889 : Matrix3f _rotation_autopilot_body_to_vehicle_body;
890 : Matrix3f _rotation_vehicle_body_to_autopilot_body;
891 :
892 : // last time orientation was updated from AHRS_ORIENTATION:
893 : uint32_t last_orientation_update_ms;
894 :
895 : // updates matrices responsible for rotating vectors from vehicle body
896 : // frame to autopilot body frame from _trim variables
897 : void update_trim_rotation_matrices();
898 :
899 : /*
900 : * AHRS is used as a transport for vehicle-takeoff-expected and
901 : * vehicle-landing-expected:
902 : */
903 : // update takeoff/touchdown flags
904 : void update_flags();
905 : bool takeoff_expected; // true if the vehicle is in a state that takeoff might be expected. Ground effect may be in play.
906 : uint32_t takeoff_expected_start_ms;
907 : bool touchdown_expected; // true if the vehicle is in a state that touchdown might be expected. Ground effect may be in play.
908 : uint32_t touchdown_expected_start_ms;
909 :
910 : /*
911 : * wind estimation support
912 : */
913 : bool wind_estimation_enabled;
914 :
915 : /*
916 : * fly_forward is set by the vehicles to indicate the vehicle
917 : * should generally be moving in the direction of its heading.
918 : * It is an additional piece of information that the backends can
919 : * use to provide additional and/or improved estimates.
920 : */
921 : bool fly_forward; // true if we can assume the vehicle will be flying forward on its X axis
922 :
923 : // poke AP_Notify based on values from status
924 : void update_notify_from_filter_status(const nav_filter_status &status);
925 :
926 : // write out secondary estimates:
927 : void Write_AHRS2(void) const;
928 : // write POS (canonical vehicle position) message out:
929 : void Write_POS(void) const;
930 :
931 : // return an airspeed estimate if available. return true
932 : // if we have an estimate
933 : bool _airspeed_EAS(float &airspeed_ret, AirspeedEstimateType &status) const;
934 :
935 : // set state.configured_ekf_type and the pointer to the configured backend
936 : void update_configured_ekf_type();
937 :
938 : // set state.active_EKF_type and the pointer to the active backend
939 : void update_active_EKF_type();
940 :
941 : // update secondary backend pointers:
942 : void update_secondary_backend_pointers();
943 :
944 : // get active EKF type
945 : EKFType _active_EKF_type(void) const;
946 :
947 : // get configured EKF type:
948 : EKFType _configured_ekf_type(void) const;
949 :
950 : // return a true airspeed estimate (navigation airspeed) if
951 : // available. return true if we have an estimate
952 : bool _airspeed_TAS(float &airspeed_ret) const;
953 :
954 : // return estimate of true airspeed vector in body frame in m/s
955 : // returns false if estimate is unavailable
956 : bool _airspeed_TAS(Vector3f &vec) const;
957 :
958 : // Retrieves the corrected NED delta velocity in use by the inertial navigation
959 : bool _getCorrectedDeltaVelocityNED(Vector3f& ret, float& dt) const WARN_IF_UNUSED;
960 :
961 : // returns the inertial navigation origin in lat/lon/alt
962 : bool _get_origin(Location &ret) const WARN_IF_UNUSED;
963 :
964 : // return origin for a specified EKF type
965 : bool _get_origin(EKFType type, Location &ret) const;
966 :
967 : // return a ground velocity in meters/second, North/East/Down
968 : // order. Must only be called if have_inertial_nav() is true
969 : bool _get_velocity_NED(Vector3f &vec) const WARN_IF_UNUSED;
970 :
971 : // get secondary EKF type. returns false if no secondary (i.e. only using DCM)
972 : bool _get_secondary_EKF_type(EKFType &secondary_ekf_type) const;
973 :
974 : // get current location estimate
975 : bool _get_location(Location &loc) const;
976 :
977 : // return true if a airspeed sensor should be used for the AHRS airspeed estimate
978 : bool _should_use_airspeed_sensor(uint8_t airspeed_index) const;
979 :
980 : /*
981 : update state structure
982 : */
983 : void update_state(void);
984 :
985 : // returns an EKF type to be used as active if we decide the
986 : // primary is not good enough.
987 : EKFType fallback_active_EKF_type(void) const;
988 :
989 : // Record the current valid origin to parameters
990 : // This may save the user from having to set the origin manually when using position controlled modes without GPS
991 : void record_origin();
992 :
993 : /*
994 : state updated at the end of each update() call
995 : */
996 : struct {
997 : EKFType active_EKF_type; // EKFType of backend these results are for
998 : EKFType configured_ekf_type; // vetted parameter-configured EKFType
999 : uint8_t primary_gyro;
1000 : uint8_t primary_accel;
1001 : Vector3f gyro_estimate;
1002 : Matrix3f dcm_matrix;
1003 : Vector3f gyro_drift;
1004 : Vector3f accel_ef;
1005 : Vector3f accel_bias;
1006 : float EAS2TAS;
1007 : bool airspeed_EAS_ok;
1008 : float airspeed_EAS;
1009 : AirspeedEstimateType airspeed_estimate_type;
1010 : bool airspeed_TAS_ok;
1011 : float airspeed_TAS;
1012 : Vector3f airspeed_TAS_vec;
1013 : bool airspeed_TAS_vec_ok;
1014 : Quaternion quat;
1015 : bool quat_ok;
1016 : Location location;
1017 : bool location_ok;
1018 : Vector2f ground_speed_vec;
1019 : float ground_speed;
1020 : bool corrected_dv_valid;
1021 : Vector3f corrected_dv;
1022 : float corrected_dv_dt;
1023 : Location origin;
1024 : bool origin_ok;
1025 : Vector3f velocity_NED;
1026 : bool velocity_NED_ok;
1027 : } state;
1028 :
1029 : /*
1030 : * backends (and their results)
1031 : */
1032 : #if AP_AHRS_DCM_ENABLED
1033 : AP_AHRS_DCM dcm{_kp_yaw, _kp, gps_gain, beta, _gps_use, _gps_minsats};
1034 : struct AP_AHRS_Backend::Estimates dcm_estimates;
1035 : #endif
1036 : #if AP_AHRS_NAVEKF3_ENABLED
1037 : struct AP_AHRS_Backend::Estimates ekf3_estimates;
1038 : #endif
1039 : #if AP_AHRS_NAVEKF2_ENABLED
1040 : struct AP_AHRS_Backend::Estimates ekf2_estimates;
1041 : #endif
1042 : #if AP_AHRS_SIM_ENABLED
1043 : #if HAL_NAVEKF3_AVAILABLE
1044 : AP_AHRS_SIM sim{ekf3.EKF3};
1045 : #else
1046 : AP_AHRS_SIM sim;
1047 : #endif
1048 : struct AP_AHRS_Backend::Estimates sim_estimates;
1049 : #endif
1050 :
1051 : #if AP_AHRS_EXTERNAL_ENABLED
1052 : AP_AHRS_External external;
1053 : struct AP_AHRS_Backend::Estimates external_estimates;
1054 : #endif
1055 :
1056 : // FIXME: C++-17 will let us do away with this construct:
1057 : #define AP_AHRS_BACKEND_COUNT \
1058 : (!!(AP_AHRS_DCM_ENABLED) + \
1059 : !!(AP_AHRS_NAVEKF2_ENABLED) + \
1060 : !!(AP_AHRS_NAVEKF3_ENABLED) + \
1061 : !!(AP_AHRS_SIM_ENABLED) + \
1062 : !!(AP_AHRS_EXTERNAL_ENABLED))
1063 :
1064 : struct {
1065 : AP_AHRS_Backend &backend;
1066 : AP_AHRS_Backend::Estimates &estimates;
1067 : } backends_and_estimates[AP_AHRS_BACKEND_COUNT] {
1068 : #if AP_AHRS_DCM_ENABLED
1069 : { dcm, dcm_estimates },
1070 : #endif // AP_AHRS_DCM_ENABLED
1071 : #if AP_AHRS_NAVEKF2_ENABLED
1072 : { ekf2, ekf2_estimates },
1073 : #endif // AP_AHRS_NAVEKF2_ENABLED
1074 : #if AP_AHRS_NAVEKF3_ENABLED
1075 : { ekf3, ekf3_estimates },
1076 : #endif // AP_AHRS_NAVEKF3_ENABLED
1077 : #if AP_AHRS_SIM_ENABLED
1078 : { sim, sim_estimates },
1079 : #endif // AP_AHRS_SIM_ENABLED
1080 : #if AP_AHRS_EXTERNAL_ENABLED
1081 : { external, external_estimates },
1082 : #endif // AP_AHRS_EXTERNAL_ENABLED
1083 : };
1084 :
1085 :
1086 : enum class Options : uint16_t {
1087 : DISABLE_DCM_FALLBACK_FW=(1U<<0),
1088 : DISABLE_DCM_FALLBACK_VTOL=(1U<<1),
1089 : DISABLE_AIRSPEED_EKF_CHECK=(1U<<2),
1090 : RECORD_ORIGIN=(1U<<3),
1091 : USE_RECORDED_ORIGIN_FOR_NONGPS=(1U<<4),
1092 : };
1093 : AP_Int16 _options;
1094 :
1095 21704337 : bool option_set(Options option) const {
1096 21704337 : return (_options & uint16_t(option)) != 0;
1097 : }
1098 :
1099 : // true when we have completed the common origin setup
1100 : bool done_common_origin;
1101 : void try_set_common_origin(const AP_AHRS_Backend &source_backend, const AP_AHRS_Backend::Estimates &source_estimates);
1102 :
1103 : // return a pointer to the backend for supplied type
1104 : AP_AHRS_Backend *backend_for_type(EKFType type);
1105 50121474 : const AP_AHRS_Backend *backend_for_type(EKFType type) const {
1106 50121474 : return const_cast<AP_AHRS*>(this)->backend_for_type(type);
1107 : }
1108 : AP_AHRS_Backend::Estimates *estimates_for_type(EKFType type);
1109 : const AP_AHRS_Backend::Estimates *estimates_for_type(EKFType type) const {
1110 : return const_cast<AP_AHRS*>(this)->estimates_for_type(type);
1111 : }
1112 :
1113 : // configured_backend is the backend the user wants to use as
1114 : // indicated by parameter values
1115 : AP_AHRS_Backend *configured_backend;
1116 : AP_AHRS_Backend::Estimates *configured_estimates;
1117 :
1118 : // active_backend is the backend which is currently providing
1119 : // results (e.g. this may be a fallback estimator if the
1120 : // configured_backend is unhealthy)
1121 : AP_AHRS_Backend *active_backend;
1122 : AP_AHRS_Backend::Estimates *active_estimates;
1123 :
1124 : // method responsible for updating the reset counters in the AHRS.
1125 : // These can get bumped if we change backends or the count in the
1126 : // current backend changes.
1127 : void update_reset_counters();
1128 : // reset counters. These are updated if the backend changes or if
1129 : // the backend results change (e.g. switching core)
1130 : AP_AHRS_ResetCounter<uint16_t> attitude_reset_tracker;
1131 : AP_AHRS_ResetCounter<uint16_t> yaw_reset_tracker;
1132 : AP_AHRS_ResetCounter<uint16_t> position_NE_reset_tracker;
1133 : AP_AHRS_ResetCounter<uint16_t> position_D_reset_tracker;
1134 :
1135 : // secondary estimates - used for reporting purposes. If the
1136 : // primary backend fails this is the backend/result pair likely to
1137 : // be used by the vehicle. In the case that the primary *has*
1138 : // failed this *continues* to be the same estimates. For now.
1139 : // Note that these pointers *must* be nullptr-checked before use!
1140 : // AP_AHRS_Backend *secondary_backend;
1141 : AP_AHRS_Backend::Estimates *secondary_estimates;
1142 : };
1143 :
1144 : namespace AP {
1145 : AP_AHRS &ahrs();
1146 : };
|
