Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 :
16 : /*
17 : * NavEKF based AHRS (Attitude Heading Reference System) interface for
18 : * ArduPilot
19 : *
20 : */
21 :
22 : #include "AP_AHRS_config.h"
23 :
24 : #if AP_AHRS_ENABLED
25 :
26 : #include <AP_HAL/AP_HAL.h>
27 : #include "AP_AHRS.h"
28 : #include "AP_AHRS_View.h"
29 : #include <AP_BoardConfig/AP_BoardConfig.h>
30 : #include <AP_ExternalAHRS/AP_ExternalAHRS.h>
31 : #include <AP_Module/AP_Module.h>
32 : #include <AP_GPS/AP_GPS.h>
33 : #include <AP_Baro/AP_Baro.h>
34 : #include <AP_Compass/AP_Compass.h>
35 : #include <AP_InternalError/AP_InternalError.h>
36 : #include <AP_Logger/AP_Logger.h>
37 : #include <AP_Notify/AP_Notify.h>
38 : #include <AP_Vehicle/AP_Vehicle_Type.h>
39 : #include <GCS_MAVLink/GCS.h>
40 : #include <AP_InertialSensor/AP_InertialSensor.h>
41 :
42 : #include <AP_Mission/AP_Mission_config.h>
43 : #if AP_MISSION_ENABLED
44 : #include <AP_Mission/AP_Mission.h>
45 : #endif
46 :
47 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
48 : #include <SITL/SITL.h>
49 : #endif
50 : #include <AP_NavEKF3/AP_NavEKF3_feature.h>
51 :
52 : #define ATTITUDE_CHECK_THRESH_ROLL_PITCH_RAD radians(10)
53 : #define ATTITUDE_CHECK_THRESH_YAW_RAD radians(20)
54 :
55 : #ifndef HAL_AHRS_EKF_TYPE_DEFAULT
56 : #define HAL_AHRS_EKF_TYPE_DEFAULT 3
57 : #endif
58 :
59 : #ifndef HAL_AHRS_OPTIONS_DEFAULT
60 : #if APM_BUILD_TYPE(APM_BUILD_Rover)
61 : // DISABLE_DCM_FALLBACK_FW | DISABLE_DCM_FALLBACK_VTOL
62 : #define HAL_AHRS_OPTIONS_DEFAULT 3
63 : #elif APM_BUILD_TYPE(APM_BUILD_ArduSub)
64 : // ENABLE_USE_RECORDED_ORIGIN
65 : #define HAL_AHRS_OPTIONS_DEFAULT 16
66 : #else
67 : #define HAL_AHRS_OPTIONS_DEFAULT 0
68 : #endif
69 : #endif
70 :
71 : // table of user settable parameters
72 : const AP_Param::GroupInfo AP_AHRS::var_info[] = {
73 : // index 0 and 1 are for old parameters that are no longer not used
74 :
75 : // @Param: GPS_GAIN
76 : // @DisplayName: AHRS GPS gain
77 : // @Description: This controls how much to use the GPS to correct the attitude. This should never be set to zero for a plane as it would result in the plane losing control in turns. For a plane please use the default value of 1.0.
78 : // @Range: 0.0 1.0
79 : // @Increment: 0.01
80 : // @User: Advanced
81 : AP_GROUPINFO("GPS_GAIN", 2, AP_AHRS, gps_gain, 1.0f),
82 :
83 : // @Param: GPS_USE
84 : // @DisplayName: AHRS use GPS for DCM navigation and position-down
85 : // @Description: This controls whether to use dead-reckoning or GPS based navigation. If set to 0 then the GPS won't be used for navigation, and only dead reckoning will be used. A value of zero should never be used for normal flight. Currently this affects only the DCM-based AHRS: the EKF uses GPS according to its own parameters. A value of 2 means to use GPS for height as well as position - both in DCM estimation and when determining altitude-above-home.
86 : // @Values: 0:Disabled,1:Use GPS for DCM position,2:Use GPS for DCM position and height
87 : // @User: Advanced
88 : AP_GROUPINFO("GPS_USE", 3, AP_AHRS, _gps_use, float(GPSUse::Enable)),
89 :
90 : // @Param: YAW_P
91 : // @DisplayName: Yaw P
92 : // @Description: This controls the weight the compass or GPS has on the heading. A higher value means the heading will track the yaw source (GPS or compass) more rapidly.
93 : // @Range: 0.1 0.4
94 : // @Increment: 0.01
95 : // @User: Advanced
96 : AP_GROUPINFO("YAW_P", 4, AP_AHRS, _kp_yaw, 0.2f),
97 :
98 : // @Param: RP_P
99 : // @DisplayName: AHRS RP_P
100 : // @Description: This controls how fast the accelerometers correct the attitude
101 : // @Range: 0.1 0.4
102 : // @Increment: 0.01
103 : // @User: Advanced
104 : AP_GROUPINFO("RP_P", 5, AP_AHRS, _kp, 0.2f),
105 :
106 : // @Param: WIND_MAX
107 : // @DisplayName: Maximum wind
108 : // @Description: This sets the maximum allowable difference between ground speed and airspeed. A value of zero means to use the airspeed as is. This allows the plane to cope with a failing airspeed sensor by clipping it to groundspeed plus/minus this limit. See ARSPD_OPTIONS and ARSPD_WIND_MAX to disable airspeed sensors.
109 : // @Range: 0 127
110 : // @Units: m/s
111 : // @Increment: 1
112 : // @User: Advanced
113 : AP_GROUPINFO("WIND_MAX", 6, AP_AHRS, _wind_max, 0.0f),
114 :
115 : // NOTE: 7 was BARO_USE
116 :
117 : // @Param: TRIM_X
118 : // @DisplayName: AHRS Trim Roll
119 : // @Description: Compensates for the roll angle difference between the control board and the frame. Positive values make the vehicle roll right.
120 : // @Units: rad
121 : // @Range: -0.1745 +0.1745
122 : // @Increment: 0.01
123 : // @User: Standard
124 :
125 : // @Param: TRIM_Y
126 : // @DisplayName: AHRS Trim Pitch
127 : // @Description: Compensates for the pitch angle difference between the control board and the frame. Positive values make the vehicle pitch up/back.
128 : // @Units: rad
129 : // @Range: -0.1745 +0.1745
130 : // @Increment: 0.01
131 : // @User: Standard
132 :
133 : // @Param: TRIM_Z
134 : // @DisplayName: AHRS Trim Yaw
135 : // @Description: Not Used
136 : // @Units: rad
137 : // @Range: -0.1745 +0.1745
138 : // @Increment: 0.01
139 : // @User: Advanced
140 : AP_GROUPINFO("TRIM", 8, AP_AHRS, _trim, 0),
141 :
142 : // @Param: ORIENTATION
143 : // @DisplayName: Board Orientation
144 : // @Description: Overall board orientation relative to the standard orientation for the board type. This rotates the IMU and compass readings to allow the board to be oriented in your vehicle at any 90 or 45 degree angle. The label for each option is specified in the order of rotations for that orientation. This option takes affect on next boot. After changing you will need to re-level your vehicle. Firmware versions 4.2 and prior can use a CUSTOM (100) rotation to set the AHRS_CUSTOM_ROLL/PIT/YAW angles for AHRS orientation. Later versions provide two general custom rotations which can be used, Custom 1 and Custom 2, with CUST_ROT1_ROLL/PIT/YAW or CUST_ROT2_ROLL/PIT/YAW angles.
145 : // @Values: 0:None,1:Yaw45,2:Yaw90,3:Yaw135,4:Yaw180,5:Yaw225,6:Yaw270,7:Yaw315,8:Roll180,9:Yaw45Roll180,10:Yaw90Roll180,11:Yaw135Roll180,12:Pitch180,13:Yaw225Roll180,14:Yaw270Roll180,15:Yaw315Roll180,16:Roll90,17:Yaw45Roll90,18:Yaw90Roll90,19:Yaw135Roll90,20:Roll270,21:Yaw45Roll270,22:Yaw90Roll270,23:Yaw135Roll270,24:Pitch90,25:Pitch270,26:Yaw90Pitch180,27:Yaw270Pitch180,28:Pitch90Roll90,29:Pitch90Roll180,30:Pitch90Roll270,31:Pitch180Roll90,32:Pitch180Roll270,33:Pitch270Roll90,34:Pitch270Roll180,35:Pitch270Roll270,36:Yaw90Pitch180Roll90,37:Yaw270Roll90,38:Yaw293Pitch68Roll180,39:Pitch315,40:Pitch315Roll90,42:Roll45,43:Roll315,100:Custom 4.1 and older,101:Custom 1,102:Custom 2
146 : // @User: Advanced
147 : AP_GROUPINFO("ORIENTATION", 9, AP_AHRS, _board_orientation, 0),
148 :
149 : // @Param: COMP_BETA
150 : // @DisplayName: AHRS Velocity Complementary Filter Beta Coefficient
151 : // @Description: This controls the time constant for the cross-over frequency used to fuse AHRS (airspeed and heading) and GPS data to estimate ground velocity. Time constant is 0.1/beta. A larger time constant will use GPS data less and a small time constant will use air data less.
152 : // @Range: 0.001 0.5
153 : // @Increment: 0.01
154 : // @User: Advanced
155 : AP_GROUPINFO("COMP_BETA", 10, AP_AHRS, beta, 0.1f),
156 :
157 : // @Param: GPS_MINSATS
158 : // @DisplayName: AHRS GPS Minimum satellites
159 : // @Description: Minimum number of satellites visible to use GPS for velocity based corrections attitude correction. This defaults to 6, which is about the point at which the velocity numbers from a GPS become too unreliable for accurate correction of the accelerometers.
160 : // @Range: 0 10
161 : // @Increment: 1
162 : // @User: Advanced
163 : AP_GROUPINFO("GPS_MINSATS", 11, AP_AHRS, _gps_minsats, 6),
164 :
165 : // NOTE: index 12 was for GPS_DELAY, but now removed, fixed delay
166 : // of 1 was found to be the best choice
167 :
168 : // 13 was the old EKF_USE
169 :
170 : // @Param: EKF_TYPE
171 : // @DisplayName: Use NavEKF Kalman filter for attitude and position estimation
172 : // @Description: This controls which NavEKF Kalman filter version is used for attitude and position estimation
173 : // @Values: 0:Disabled,2:Enable EKF2,3:Enable EKF3, 10:Sim, 11:ExternalAHRS
174 : // @User: Advanced
175 : AP_GROUPINFO("EKF_TYPE", 14, AP_AHRS, _ekf_type, HAL_AHRS_EKF_TYPE_DEFAULT),
176 :
177 : // index 15 was CUSTOM_ROLL
178 :
179 : // index 16 was CUSTOM_PIT
180 :
181 : // index 17 was CUSTOM_YAW
182 :
183 : // @Param: OPTIONS
184 : // @DisplayName: Optional AHRS behaviour
185 : // @Description: This controls optional AHRS behaviour. Setting DisableDCMFallbackFW will change the AHRS behaviour for fixed wing aircraft in fly-forward flight to not fall back to DCM when the EKF stops navigating. Setting DisableDCMFallbackVTOL will change the AHRS behaviour for fixed wing aircraft in non fly-forward (VTOL) flight to not fall back to DCM when the EKF stops navigating. Setting DontDisableAirspeedUsingEKF disables the EKF based innovation check for airspeed consistency. Setting AutoRecordOrigin will auto-save the EKF origin to parameters when it becomes valid.
186 : // @Bitmask: 0:DisableDCMFallbackFW, 1:DisableDCMFallbackVTOL, 2:DontDisableAirspeedUsingEKF, 3:RecordOrigin, 4:UseRecordedOriginForNonGPS
187 : // @User: Advanced
188 : AP_GROUPINFO("OPTIONS", 18, AP_AHRS, _options, HAL_AHRS_OPTIONS_DEFAULT),
189 :
190 : // @Param: ORIGIN_LAT
191 : // @DisplayName: AHRS last origin latitude
192 : // @Description: AHRS last origin latitude in degrees
193 : // @Range: -180 180
194 : // @Increment: 1
195 : // @User: Advanced
196 : AP_GROUPINFO("ORIGIN_LAT", 19, AP_AHRS, _origin_lat, 0),
197 :
198 : // @Param: ORIGIN_LON
199 : // @DisplayName: AHRS last origin longitude
200 : // @Description: AHRS last origin longitude in degrees
201 : // @Range: -180 180
202 : // @User: Advanced
203 : AP_GROUPINFO("ORIGIN_LON", 20, AP_AHRS, _origin_lon, 0),
204 :
205 : // @Param: ORIGIN_ALT
206 : // @DisplayName: AHRS last origin altitude
207 : // @Description: AHRS last origin altitude in meters
208 : // @Range: -200 5000
209 : // @User: Advanced
210 : AP_GROUPINFO("ORIGIN_ALT", 21, AP_AHRS, _origin_alt, 0),
211 :
212 : AP_GROUPEND
213 : };
214 :
215 : extern const AP_HAL::HAL& hal;
216 :
217 : // constructor
218 2100 : AP_AHRS::AP_AHRS(uint8_t flags) :
219 2100 : _ekf_flags(flags)
220 : {
221 2100 : _singleton = this;
222 :
223 : // load default values from var_info table
224 2100 : AP_Param::setup_object_defaults(this, var_info);
225 :
226 : // we assume active_backend is not nullptr in many places, so make
227 : // sure it is set early:
228 2100 : update_configured_ekf_type();
229 2100 : update_active_EKF_type();
230 2100 : update_secondary_backend_pointers();
231 :
232 : #if APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_ArduSub)
233 : // Copter and Sub force the use of EKF
234 1084 : _ekf_flags |= AP_AHRS::FLAG_ALWAYS_USE_EKF;
235 : #endif
236 2100 : state.dcm_matrix.identity();
237 :
238 : // initialise the controller-to-autopilot-body trim state:
239 2100 : _last_trim = _trim.get();
240 2100 : _rotation_autopilot_body_to_vehicle_body.from_euler(_last_trim.x, _last_trim.y, _last_trim.z);
241 2100 : _rotation_vehicle_body_to_autopilot_body = _rotation_autopilot_body_to_vehicle_body.transposed();
242 2100 : }
243 :
244 : // return a pointer to the backend for supplied type
245 133503282 : AP_AHRS_Backend *AP_AHRS::backend_for_type(EKFType type)
246 : {
247 133503282 : switch (type) {
248 : #if AP_AHRS_DCM_ENABLED
249 2250170 : case EKFType::DCM:
250 2250170 : return &dcm;
251 : #endif
252 : #if AP_AHRS_NAVEKF3_ENABLED
253 119632491 : case EKFType::THREE:
254 119632491 : return &ekf3;
255 : #endif
256 : #if AP_AHRS_NAVEKF2_ENABLED
257 673509 : case EKFType::TWO:
258 673509 : return &ekf2;
259 : #endif
260 : #if AP_AHRS_EXTERNAL_ENABLED
261 309843 : case EKFType::EXTERNAL:
262 309843 : return &external;
263 : #endif
264 : #if AP_AHRS_SIM_ENABLED
265 10637269 : case EKFType::SIM:
266 10637269 : return ∼
267 : #endif
268 : }
269 0 : return nullptr;
270 : }
271 :
272 : // updates the cached value for the currently configured EKF type
273 : // (i.e. the one that the user has specified with parameters). Also
274 : // sets the pointer to the allocated backend for that type.
275 41690904 : void AP_AHRS::update_configured_ekf_type()
276 : {
277 41690904 : const auto new_type = _configured_ekf_type();
278 41690904 : auto *new_backend = backend_for_type(new_type);
279 41690904 : if (new_backend != nullptr) {
280 41690904 : state.configured_ekf_type = new_type;
281 41690904 : configured_backend = new_backend;
282 41690904 : configured_estimates = estimates_for_type(new_type);
283 41690904 : return;
284 : }
285 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
286 : }
287 :
288 41690904 : void AP_AHRS::update_active_EKF_type()
289 : {
290 41690904 : const auto new_type = _active_EKF_type();
291 41690904 : auto *new_backend = backend_for_type(new_type);
292 41690904 : if (new_backend != nullptr) {
293 41690904 : state.active_EKF_type = new_type;
294 41690904 : active_backend = new_backend;
295 41690904 : active_estimates = estimates_for_type(new_type);
296 41690904 : return;
297 : }
298 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
299 : }
300 :
301 : // update the pointer to the estimates containing the secondary backend's data
302 41690904 : void AP_AHRS::update_secondary_backend_pointers()
303 : {
304 : EKFType secondary_type;
305 41690904 : if (!_get_secondary_EKF_type(secondary_type)) {
306 129619 : secondary_estimates = nullptr;
307 129619 : return;
308 : }
309 41561285 : secondary_estimates = estimates_for_type(secondary_type);
310 : }
311 :
312 : // init sets up INS board orientation
313 2088 : void AP_AHRS::init()
314 : {
315 2088 : update_orientation();
316 :
317 : // EKF1 is no longer supported - handle case where it is selected
318 2088 : if (_ekf_type.get() == 1) {
319 0 : AP_BoardConfig::config_error("EKF1 not available");
320 : }
321 : #if !HAL_NAVEKF2_AVAILABLE && HAL_NAVEKF3_AVAILABLE
322 0 : if (_ekf_type.get() == 2) {
323 0 : _ekf_type.set(EKFType::THREE);
324 0 : ekf3.EKF3.set_enable(true);
325 : }
326 : #elif !HAL_NAVEKF3_AVAILABLE && HAL_NAVEKF2_AVAILABLE
327 : if (_ekf_type.get() == 3) {
328 : _ekf_type.set(EKFType::TWO);
329 : ekf2.EKF2.set_enable(true);
330 : }
331 : #endif
332 :
333 : #if HAL_NAVEKF2_AVAILABLE && HAL_NAVEKF3_AVAILABLE
334 : // a special case to catch users who had AHRS_EKF_TYPE=2 saved and
335 : // updated to a version where EK2_ENABLE=0
336 2088 : if (_ekf_type.get() == 2 && !ekf2.EKF2.get_enable() && ekf3.EKF3.get_enable()) {
337 0 : _ekf_type.set(EKFType::THREE);
338 : }
339 : #endif
340 :
341 : // we may have updated ekf_type()'s results, so set the backend again:
342 2088 : update_configured_ekf_type();
343 2088 : update_active_EKF_type();
344 2088 : update_secondary_backend_pointers();
345 :
346 : // initialise this as no-change from the active type:
347 2088 : last_active_ekf_type = state.active_EKF_type;
348 2088 : }
349 :
350 : // has_status returns information about the EKF health and
351 : // capabilities. It is currently invalid to call this when a
352 : // backend is in charge which returns false for get_filter_status
353 : // - so this will simply return false for DCM, for example.
354 13856072 : bool AP_AHRS::has_status(Status status) const {
355 : nav_filter_status filter_status;
356 13856072 : if (!get_filter_status(filter_status)) {
357 202530 : return false;
358 : }
359 13653542 : return (filter_status.value & uint32_t(status)) != 0;
360 : }
361 :
362 : // updates matrices responsible for rotating vectors from vehicle body
363 : // frame to autopilot body frame from _trim variables
364 41686716 : void AP_AHRS::update_trim_rotation_matrices()
365 : {
366 41686716 : if (_last_trim == _trim.get()) {
367 : // nothing to do
368 41686304 : return;
369 : }
370 :
371 412 : _last_trim = _trim.get();
372 412 : _rotation_autopilot_body_to_vehicle_body.from_euler(_last_trim.x, _last_trim.y, _last_trim.z);
373 412 : _rotation_vehicle_body_to_autopilot_body = _rotation_autopilot_body_to_vehicle_body.transposed();
374 : }
375 :
376 : // return a Quaternion representing our current attitude in NED frame
377 109536 : void AP_AHRS::get_quat_body_to_ned(Quaternion &quat) const
378 : {
379 109536 : quat.from_rotation_matrix(get_rotation_body_to_ned());
380 109536 : }
381 :
382 : // convert a vector from body to earth frame
383 0 : Vector3f AP_AHRS::body_to_earth(const Vector3f &v) const
384 : {
385 0 : return get_rotation_body_to_ned() * v;
386 : }
387 :
388 : // convert a vector from earth to body frame
389 48531 : Vector3f AP_AHRS::earth_to_body(const Vector3f &v) const
390 : {
391 48531 : return get_rotation_body_to_ned().mul_transpose(v);
392 : }
393 :
394 :
395 : // reset the current gyro drift estimate
396 : // should be called if gyro offsets are recalculated
397 7 : void AP_AHRS::reset_gyro_drift(void)
398 : {
399 : // support locked access functions to AHRS data
400 7 : WITH_SEMAPHORE(_rsem);
401 :
402 42 : for (auto &backend_and_estimates : backends_and_estimates) {
403 35 : backend_and_estimates.backend.reset_gyro_drift();
404 : }
405 7 : }
406 :
407 : /*
408 : * copy results from a backend over AP_AHRS canonical results.
409 : * This updates member variables like roll and pitch, as well as
410 : * updating derived values like sin_roll and sin_pitch.
411 : */
412 41686716 : void AP_AHRS::update_state(void)
413 : {
414 41686716 : const uint8_t primary_gyro = active_estimates->primary_gyro;
415 : #if AP_INERTIALSENSOR_ENABLED
416 : // tell the IMUS about primary changes
417 41686716 : if (primary_gyro != state.primary_gyro) {
418 61 : AP::ins().set_primary(primary_gyro);
419 : }
420 : #endif
421 41686716 : state.primary_gyro = primary_gyro;
422 :
423 41686716 : state.primary_accel = active_estimates->primary_accel;
424 :
425 41686716 : state.EAS2TAS = AP_AHRS_Backend::get_EAS2TAS();
426 41686716 : state.airspeed_EAS_ok = _airspeed_EAS(state.airspeed_EAS, state.airspeed_estimate_type);
427 41686716 : state.airspeed_TAS_ok = _airspeed_TAS(state.airspeed_TAS);
428 41686716 : state.airspeed_TAS_vec_ok = _airspeed_TAS(state.airspeed_TAS_vec);
429 :
430 41686716 : roll = active_estimates->roll_rad;
431 41686716 : pitch = active_estimates->pitch_rad;
432 41686716 : yaw = active_estimates->yaw_rad;
433 :
434 41686716 : state.dcm_matrix = active_estimates->dcm_matrix;
435 :
436 41686716 : state.gyro_estimate = active_estimates->gyro_estimate;
437 41686716 : state.gyro_drift = active_estimates->gyro_drift;
438 :
439 41686716 : state.accel_ef = active_estimates->accel_ef;
440 41686716 : state.accel_bias = active_estimates->accel_bias;
441 :
442 41686716 : update_cd_values();
443 41686716 : update_trig();
444 :
445 41686716 : state.quat_ok = active_estimates->get_quaternion(state.quat);
446 41686716 : state.location_ok = _get_location(state.location);
447 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
448 41686716 : if (state.location_ok && !state.location.initialised()) {
449 0 : AP_HAL::panic("uninitialised location returned by _get_location");
450 : }
451 : #endif // CONFIG_HAL_BOARD == HAL_BOARD_SITL
452 41686716 : state.ground_speed_vec = active_estimates->velocity_NE;
453 41686716 : state.ground_speed = state.ground_speed_vec.length();
454 41686716 : state.corrected_dv_valid = _getCorrectedDeltaVelocityNED(state.corrected_dv, state.corrected_dv_dt);
455 :
456 : // check if origin has been set
457 41686716 : bool origin_ok = _get_origin(state.origin);
458 41686716 : if (origin_ok && !state.origin_ok) {
459 : // record origin from backend to AHRS state
460 964 : state.origin_ok = origin_ok;
461 :
462 : #if HAL_LOGGING_ENABLED
463 : // log origin
464 964 : Log_Write_Home_And_Origin();
465 : #endif
466 :
467 : // report origin via MAVLink
468 964 : GCS_SEND_MESSAGE(MSG_ORIGIN);
469 :
470 : // save origin to parameters
471 964 : record_origin();
472 : }
473 :
474 : // if no origin, attempt to use recorded origin
475 41686716 : if (!state.origin_ok) {
476 5099682 : use_recorded_origin_maybe();
477 : }
478 :
479 41686716 : state.velocity_NED_ok = active_estimates->get_velocity_NED(state.velocity_NED);
480 41686716 : }
481 :
482 208433580 : void AP_AHRS::try_set_common_origin(const AP_AHRS_Backend &source_backend, const AP_AHRS_Backend::Estimates &source_estimates)
483 : {
484 208433580 : if (done_common_origin) {
485 208432685 : return;
486 : }
487 :
488 : /*
489 : if we now have an origin then set in all backends
490 : */
491 32424992 : if (!source_estimates.provides_common_origin) {
492 : // e.g. DCM doesn't provide an origin which can be set
493 : // into the other backends
494 19454438 : return;
495 : }
496 12970554 : Location new_origin;
497 12970554 : if (!source_backend.get_origin(new_origin)) {
498 : // no valid origin from this backend
499 12969659 : return;
500 : }
501 : // set the origin in all backends which can take it (except
502 : // the one which supplied it). Invoking AP_AHRS::set_origin
503 : // here will cause warnings from the EKFs.
504 5370 : for (auto &dest_backend_and_estimates : backends_and_estimates) {
505 4475 : if (&dest_backend_and_estimates.estimates == &source_estimates) {
506 895 : continue;
507 : }
508 : // note that SITL and DCM ignore this set_origin call via
509 : // an empty base-class implementation:
510 3580 : dest_backend_and_estimates.backend.set_origin(new_origin);
511 : }
512 :
513 895 : done_common_origin = true;
514 : }
515 :
516 : // method responsible for updating the reset counters in the AHRS.
517 : // These can get bumped if we change backends or the count in the
518 : // current backend changes.
519 41686716 : void AP_AHRS::update_reset_counters()
520 : {
521 41686716 : if (state.active_EKF_type != last_active_ekf_type) {
522 1530 : attitude_reset_tracker.fill(active_estimates->attitude_reset_count);
523 1530 : yaw_reset_tracker.fill(active_estimates->yaw_reset_count);
524 1530 : position_NE_reset_tracker.fill(active_estimates->position_NE_reset_count);
525 1530 : position_D_reset_tracker.fill(active_estimates->position_D_reset_count);
526 1530 : LOGGER_WRITE_EVENT(LogEvent::EKF_YAW_RESET);
527 1530 : return;
528 : }
529 :
530 41685186 : attitude_reset_tracker.update(active_estimates->attitude_reset_count);
531 41685186 : if (yaw_reset_tracker.update(active_estimates->yaw_reset_count)) {
532 1586 : LOGGER_WRITE_EVENT(LogEvent::EKF_YAW_RESET);
533 : }
534 41685186 : position_NE_reset_tracker.update(active_estimates->position_NE_reset_count);
535 41685186 : position_D_reset_tracker.update(active_estimates->position_D_reset_count);
536 : }
537 :
538 : // update run at loop rate
539 41686716 : void AP_AHRS::update(bool skip_ins_update)
540 : {
541 : // periodically checks to see if we should update the AHRS
542 : // orientation (e.g. based on the AHRS_ORIENTATION parameter)
543 : // allow for runtime change of orientation
544 : // this makes initial config easier
545 41686716 : update_orientation();
546 :
547 41686716 : if (!skip_ins_update) {
548 : // tell the IMU to grab some data
549 9093585 : AP::ins().update();
550 : }
551 :
552 : // support locked access functions to AHRS data
553 41686716 : WITH_SEMAPHORE(_rsem);
554 :
555 : // see if we have to restore home after a watchdog reset:
556 41686716 : if (!_checked_watchdog_home) {
557 2086 : load_watchdog_home();
558 2086 : _checked_watchdog_home = true;
559 : }
560 :
561 : // drop back to normal priority if we were boosted by the INS
562 : // calling delay_microseconds_boost()
563 41686716 : hal.scheduler->boost_end();
564 :
565 : // update autopilot-body-to-vehicle-body from _trim parameters:
566 41686716 : update_trim_rotation_matrices();
567 :
568 : // update takeoff/touchdown flags
569 41686716 : update_flags();
570 :
571 : // update the backends, configured-first. Some backends look at
572 : // loop-time-remaining and opt-out of their full update if there
573 : // isn't enough time left. Copy back their results while we are
574 : // at it.
575 41686716 : configured_backend->update();
576 41686716 : *configured_estimates = {};
577 41686716 : configured_backend->get_results(*configured_estimates);
578 : // if we don't have an origin, maybe set one:
579 41686716 : try_set_common_origin(*configured_backend, *configured_estimates);
580 :
581 250120296 : for (auto &backend_and_estimates : backends_and_estimates) {
582 208433580 : if (&backend_and_estimates.backend == configured_backend) {
583 : // already updated
584 41686716 : continue;
585 : }
586 166746864 : backend_and_estimates.backend.update();
587 166746864 : backend_and_estimates.estimates = {};
588 166746864 : backend_and_estimates.backend.get_results(backend_and_estimates.estimates);
589 : // if we don't have an origin, maybe set one:
590 166746864 : try_set_common_origin(backend_and_estimates.backend, backend_and_estimates.estimates);
591 : }
592 :
593 41686716 : update_configured_ekf_type();
594 41686716 : update_active_EKF_type();
595 41686716 : update_secondary_backend_pointers();
596 :
597 : // update blinking lights, buzzer etc bsaed on active EKF type:
598 41686716 : update_notify_from_filter_status(active_estimates->filter_status);
599 :
600 : // update result reset counters. Note that this *must* be called
601 : // before assignment of state.active_EKF_type to
602 : // last_active_ekf_type as it uses the difference between these
603 : // two values to determine what sort of reset to do
604 41686716 : update_reset_counters();
605 :
606 41686716 : if (state.active_EKF_type != last_active_ekf_type) {
607 1530 : last_active_ekf_type = state.active_EKF_type;
608 1530 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AHRS: %s active", active_backend->shortname());
609 : }
610 :
611 : // update published state, including copying state from the active backend:
612 41686716 : update_state();
613 :
614 : #if AP_MODULE_SUPPORTED
615 : // call AHRS_update hook if any
616 0 : AP_Module::call_hook_AHRS_update(*this);
617 : #endif
618 :
619 : // push gyros if optical flow present
620 41686716 : if (hal.opticalflow) {
621 41686716 : const Vector3f &exported_gyro_bias = get_gyro_drift();
622 41686716 : hal.opticalflow->push_gyro_bias(exported_gyro_bias.x, exported_gyro_bias.y);
623 : }
624 :
625 41686716 : if (_view != nullptr) {
626 : // update optional alternative attitude view
627 36916929 : _view->update();
628 : }
629 :
630 : // update AOA and SSA
631 41686716 : update_AOA_SSA();
632 :
633 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
634 : /*
635 : add timing jitter to simulate slow EKF response
636 : */
637 41686716 : const auto *sitl = AP::sitl();
638 41686716 : if (sitl->loop_time_jitter_us > 0) {
639 0 : hal.scheduler->delay_microseconds(random() % sitl->loop_time_jitter_us);
640 : }
641 : #endif
642 41686716 : }
643 :
644 41686716 : void AP_AHRS::update_notify_from_filter_status(const nav_filter_status &status)
645 : {
646 41686716 : AP_Notify::flags.gps_fusion = status.flags.using_gps; // Drives AP_Notify flag for usable GPS.
647 41686716 : AP_Notify::flags.gps_glitching = status.flags.gps_glitching;
648 41686716 : AP_Notify::flags.have_pos_abs = status.flags.horiz_pos_abs;
649 41686716 : }
650 :
651 2115 : void AP_AHRS::reset()
652 : {
653 : // support locked access functions to AHRS data
654 2115 : WITH_SEMAPHORE(_rsem);
655 :
656 12690 : for (auto &backend_and_estimates : backends_and_estimates) {
657 10575 : backend_and_estimates.backend.reset();
658 : }
659 2115 : }
660 :
661 : // dead-reckoning support
662 41686716 : bool AP_AHRS::_get_location(Location &loc) const
663 : {
664 41686716 : if (active_estimates->get_location(loc)) {
665 38577875 : return true;
666 : }
667 :
668 : #if AP_AHRS_DCM_ENABLED
669 : // fall back to position from DCM
670 3108841 : if (!always_use_EKF()) {
671 481619 : return dcm_estimates.get_location(loc);
672 : }
673 : #endif
674 :
675 2627222 : return false;
676 : }
677 :
678 : // status reporting of estimated errors
679 1117757 : float AP_AHRS::get_error_rp(void) const
680 : {
681 : #if AP_AHRS_DCM_ENABLED
682 1117757 : return dcm.get_error_rp();
683 : #endif
684 : return 0;
685 : }
686 :
687 1117757 : float AP_AHRS::get_error_yaw(void) const
688 : {
689 : #if AP_AHRS_DCM_ENABLED
690 1117757 : return dcm.get_error_yaw();
691 : #endif
692 : return 0;
693 : }
694 :
695 : /*
696 : * Determine how aligned heading_deg is with the wind. Return result
697 : * is 1.0 when perfectly aligned heading into wind, -1 when perfectly
698 : * aligned with-wind, and zero when perfect cross-wind. There is no
699 : * distinction between a left or right cross-wind. Wind speed is ignored
700 : */
701 35442 : float AP_AHRS::wind_alignment(const float heading_deg) const
702 : {
703 35442 : Vector3f wind;
704 35442 : if (!get_wind(wind)) {
705 2288 : return 0;
706 : }
707 33154 : const float wind_heading_rad = atan2f(-wind.y, -wind.x);
708 33154 : return cosf(wind_heading_rad - radians(heading_deg));
709 : }
710 :
711 : /*
712 : * returns forward head-wind component in m/s. Negative means tail-wind.
713 : */
714 35442 : float AP_AHRS::head_wind(void) const
715 : {
716 35442 : Vector3f wind;
717 : // wind_alignment() has already returned zero if we have no valid
718 : // estimate, so the validity of the wind vector is not checked here
719 35442 : IGNORE_RETURN(get_wind(wind));
720 35442 : const float alignment = wind_alignment(get_yaw_deg());
721 35442 : return alignment * wind.xy().length();
722 : }
723 :
724 : /*
725 : return true if the current AHRS airspeed estimate is directly derived from an airspeed sensor
726 : */
727 3837229 : bool AP_AHRS::using_airspeed_sensor() const
728 : {
729 3837229 : return state.airspeed_estimate_type == AirspeedEstimateType::AIRSPEED_SENSOR;
730 : }
731 :
732 : /*
733 : Return true if a airspeed sensor should be used for the AHRS airspeed estimate
734 : */
735 41686716 : bool AP_AHRS::_should_use_airspeed_sensor(uint8_t airspeed_index) const
736 : {
737 41686716 : if (!airspeed_sensor_enabled(airspeed_index)) {
738 33452901 : return false;
739 : }
740 : nav_filter_status filter_status;
741 8233815 : if (!option_set(Options::DISABLE_AIRSPEED_EKF_CHECK) &&
742 12722549 : fly_forward &&
743 8226625 : hal.util->get_soft_armed() &&
744 19618197 : get_filter_status(filter_status) &&
745 3426834 : (filter_status.flags.rejecting_airspeed && !filter_status.flags.dead_reckoning)) {
746 : // special case for when backend is rejecting airspeed data in
747 : // an armed fly_forward state and not dead reckoning. Then the
748 : // airspeed data is highly suspect and will be rejected. We
749 : // will use the synthetic airspeed instead
750 1725 : return false;
751 : }
752 8232090 : return true;
753 : }
754 :
755 : // return an airspeed estimate if available. return true
756 : // if we have an estimate
757 41686716 : bool AP_AHRS::_airspeed_EAS(float &airspeed_ret, AirspeedEstimateType &airspeed_estimate_type) const
758 : {
759 : #if AP_AHRS_DCM_ENABLED || (AP_AIRSPEED_ENABLED && AP_GPS_ENABLED)
760 41686716 : const uint8_t idx = get_active_airspeed_index();
761 : #endif
762 : #if AP_AIRSPEED_ENABLED && AP_GPS_ENABLED
763 41686716 : if (_should_use_airspeed_sensor(idx)) {
764 8232090 : airspeed_ret = AP::airspeed()->get_airspeed(idx);
765 :
766 8232090 : if (_wind_max > 0 && AP::gps().status() >= AP_GPS_FixType::FIX_2D) {
767 : // constrain the airspeed by the ground speed
768 : // and AHRS_WIND_MAX
769 0 : const float gnd_speed = AP::gps().ground_speed();
770 0 : float true_airspeed = airspeed_ret * get_EAS2TAS();
771 0 : true_airspeed = constrain_float(true_airspeed,
772 : gnd_speed - _wind_max,
773 : gnd_speed + _wind_max);
774 0 : airspeed_ret = true_airspeed / get_EAS2TAS();
775 : }
776 8232090 : airspeed_estimate_type = AirspeedEstimateType::AIRSPEED_SENSOR;
777 8232090 : return true;
778 : }
779 : #endif
780 :
781 33454626 : if (!get_wind_estimation_enabled()) {
782 33309065 : airspeed_estimate_type = AirspeedEstimateType::NO_NEW_ESTIMATE;
783 33309065 : return false;
784 : }
785 :
786 : // estimate it via nav velocity and wind estimates
787 :
788 : // get wind estimates
789 145561 : Vector3f wind_vel;
790 145561 : bool have_wind = false;
791 :
792 145561 : switch (active_EKF_type()) {
793 : #if AP_AHRS_DCM_ENABLED
794 64486 : case EKFType::DCM:
795 64486 : airspeed_estimate_type = AirspeedEstimateType::DCM_SYNTHETIC;
796 64486 : return dcm.airspeed_EAS(idx, airspeed_ret);
797 : #endif
798 :
799 : #if AP_AHRS_SIM_ENABLED
800 598 : case EKFType::SIM:
801 598 : airspeed_estimate_type = AirspeedEstimateType::SIM;
802 598 : return sim.airspeed_EAS(airspeed_ret);
803 : #endif
804 :
805 : #if HAL_NAVEKF2_AVAILABLE
806 0 : case EKFType::TWO:
807 : #if AP_AHRS_DCM_ENABLED
808 0 : airspeed_estimate_type = AirspeedEstimateType::DCM_SYNTHETIC;
809 0 : return dcm.airspeed_EAS(idx, airspeed_ret);
810 : #else
811 : return false;
812 : #endif
813 : #endif
814 :
815 : #if HAL_NAVEKF3_AVAILABLE
816 80182 : case EKFType::THREE:
817 80182 : wind_vel = ekf3_estimates.wind;
818 80182 : have_wind = ekf3_estimates.wind_valid;
819 80182 : break;
820 : #endif
821 :
822 : #if AP_AHRS_EXTERNAL_ENABLED
823 295 : case EKFType::EXTERNAL:
824 : #if AP_AHRS_DCM_ENABLED
825 295 : airspeed_estimate_type = AirspeedEstimateType::DCM_SYNTHETIC;
826 295 : return dcm.airspeed_EAS(idx, airspeed_ret);
827 : #else
828 : return false;
829 : #endif
830 : #endif
831 : }
832 :
833 : // estimate it via nav velocity and wind estimates
834 80182 : Vector3f nav_vel;
835 80182 : if (have_wind && have_inertial_nav() && get_velocity_NED(nav_vel)) {
836 73977 : Vector3f true_airspeed_vec = nav_vel - wind_vel;
837 73977 : float true_airspeed = true_airspeed_vec.length();
838 73977 : float gnd_speed = nav_vel.length();
839 73977 : if (_wind_max > 0) {
840 0 : float tas_lim_lower = MAX(0.0f, (gnd_speed - _wind_max));
841 0 : float tas_lim_upper = MAX(tas_lim_lower, (gnd_speed + _wind_max));
842 0 : true_airspeed = constrain_float(true_airspeed, tas_lim_lower, tas_lim_upper);
843 : } else {
844 73977 : true_airspeed = MAX(0.0f, true_airspeed);
845 : }
846 73977 : airspeed_ret = true_airspeed / get_EAS2TAS();
847 73977 : airspeed_estimate_type = AirspeedEstimateType::EKF3_SYNTHETIC;
848 73977 : return true;
849 : }
850 :
851 : #if AP_AHRS_DCM_ENABLED
852 : // fallback to DCM
853 6205 : airspeed_estimate_type = AirspeedEstimateType::DCM_SYNTHETIC;
854 6205 : return dcm.airspeed_EAS(idx, airspeed_ret);
855 : #endif
856 :
857 : return false;
858 : }
859 :
860 41686716 : bool AP_AHRS::_airspeed_TAS(float &airspeed_ret) const
861 : {
862 41686716 : switch (active_EKF_type()) {
863 : #if AP_AHRS_DCM_ENABLED
864 1986578 : case EKFType::DCM:
865 1986578 : return dcm.airspeed_TAS(airspeed_ret);
866 : #endif
867 : #if HAL_NAVEKF2_AVAILABLE
868 39700138 : case EKFType::TWO:
869 : #endif
870 : #if HAL_NAVEKF3_AVAILABLE
871 0 : case EKFType::THREE:
872 : #endif
873 : #if AP_AHRS_SIM_ENABLED
874 : case EKFType::SIM:
875 : #endif
876 : #if AP_AHRS_EXTERNAL_ENABLED
877 : case EKFType::EXTERNAL:
878 : #endif
879 39700138 : break;
880 : }
881 :
882 39700138 : if (!airspeed_EAS(airspeed_ret)) {
883 32450097 : return false;
884 : }
885 7250041 : airspeed_ret *= get_EAS2TAS();
886 7250041 : return true;
887 : }
888 :
889 : // return estimate of true airspeed vector in body frame in m/s
890 : // returns false if estimate is unavailable
891 41686716 : bool AP_AHRS::_airspeed_TAS(Vector3f &vec) const
892 : {
893 41686716 : switch (active_EKF_type()) {
894 : #if AP_AHRS_DCM_ENABLED
895 1986578 : case EKFType::DCM:
896 1986578 : break;
897 : #endif
898 : #if HAL_NAVEKF2_AVAILABLE
899 135980 : case EKFType::TWO:
900 135980 : return ekf2.EKF2.getAirSpdVec(vec);
901 : #endif
902 :
903 : #if HAL_NAVEKF3_AVAILABLE
904 36087486 : case EKFType::THREE:
905 36087486 : return ekf3.EKF3.getAirSpdVec(vec);
906 : #endif
907 :
908 : #if AP_AHRS_SIM_ENABLED
909 3410661 : case EKFType::SIM:
910 3410661 : break;
911 : #endif
912 :
913 : #if AP_AHRS_EXTERNAL_ENABLED
914 66011 : case EKFType::EXTERNAL:
915 66011 : break;
916 : #endif
917 : }
918 5463250 : return false;
919 : }
920 :
921 : // 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
922 : // returns false if the data is unavailable
923 178439 : bool AP_AHRS::airspeed_health_data(uint8_t instance, float &innovation, float &innovationVariance, uint32_t &age_ms) const
924 : {
925 178439 : switch (active_EKF_type()) {
926 : #if AP_AHRS_DCM_ENABLED
927 14590 : case EKFType::DCM:
928 14590 : break;
929 : #endif
930 : #if HAL_NAVEKF2_AVAILABLE
931 56 : case EKFType::TWO:
932 56 : break;
933 : #endif
934 :
935 : #if HAL_NAVEKF3_AVAILABLE
936 152265 : case EKFType::THREE:
937 152265 : return ekf3.EKF3.getAirSpdHealthData(instance, innovation, innovationVariance, age_ms);
938 : #endif
939 :
940 : #if AP_AHRS_SIM_ENABLED
941 7742 : case EKFType::SIM:
942 7742 : break;
943 : #endif
944 :
945 : #if AP_AHRS_EXTERNAL_ENABLED
946 3786 : case EKFType::EXTERNAL:
947 3786 : break;
948 : #endif
949 : }
950 26174 : return false;
951 : }
952 :
953 : // true if compass is being used
954 133406 : bool AP_AHRS::use_compass(void)
955 : {
956 : #if AP_AHRS_EXTERNAL_ENABLED
957 133406 : if (active_backend == &external) {
958 : #if AP_AHRS_DCM_ENABLED
959 : // for external we return true if DCM is using compass..
960 67351 : return dcm.use_compass();
961 : #else
962 : return false;
963 : #endif
964 : }
965 : #endif
966 :
967 66055 : return active_backend->use_compass();
968 : }
969 :
970 124943093 : AP_AHRS_Backend::Estimates *AP_AHRS::estimates_for_type(EKFType type)
971 : {
972 124943093 : switch (type) {
973 : #if AP_AHRS_DCM_ENABLED
974 41724118 : case EKFType::DCM:
975 41724118 : return &dcm_estimates;
976 : #endif
977 :
978 : #if HAL_NAVEKF2_AVAILABLE
979 389239 : case EKFType::TWO:
980 389239 : return &ekf2_estimates;
981 : #endif
982 :
983 : #if HAL_NAVEKF3_AVAILABLE
984 75874086 : case EKFType::THREE:
985 75874086 : return &ekf3_estimates;
986 : #endif
987 :
988 : #if AP_AHRS_SIM_ENABLED
989 6821512 : case EKFType::SIM:
990 6821512 : return &sim_estimates;
991 : #endif
992 :
993 : #if AP_AHRS_EXTERNAL_ENABLED
994 134138 : case EKFType::EXTERNAL:
995 134138 : return &external_estimates;
996 : #endif
997 : }
998 0 : return nullptr;
999 : }
1000 :
1001 : // set the EKF's origin location in 10e7 degrees. This should only
1002 : // be called when the EKF has no absolute position reference (i.e. GPS)
1003 : // from which to decide the origin on its own
1004 7 : bool AP_AHRS::set_origin(const Location &loc)
1005 : {
1006 7 : WITH_SEMAPHORE(_rsem);
1007 : #if HAL_NAVEKF2_AVAILABLE
1008 7 : const bool ret2 = ekf2.set_origin(loc);
1009 : #endif
1010 : #if HAL_NAVEKF3_AVAILABLE
1011 7 : const bool ret3 = ekf3.set_origin(loc);
1012 : #endif
1013 : #if AP_AHRS_EXTERNAL_ENABLED
1014 7 : const bool ret_ext = external.set_origin(loc);
1015 : #endif
1016 :
1017 : // return success if active EKF's origin was set
1018 7 : bool success = false;
1019 7 : switch (active_EKF_type()) {
1020 : #if AP_AHRS_DCM_ENABLED
1021 3 : case EKFType::DCM:
1022 3 : break;
1023 : #endif
1024 :
1025 : #if HAL_NAVEKF2_AVAILABLE
1026 0 : case EKFType::TWO:
1027 0 : success = ret2;
1028 0 : break;
1029 : #endif
1030 :
1031 : #if HAL_NAVEKF3_AVAILABLE
1032 4 : case EKFType::THREE:
1033 4 : success = ret3;
1034 4 : break;
1035 : #endif
1036 :
1037 : #if AP_AHRS_SIM_ENABLED
1038 0 : case EKFType::SIM:
1039 : // never allow origin set in SITL. The origin is set by the
1040 : // simulation backend
1041 0 : break;
1042 : #endif
1043 : #if AP_AHRS_EXTERNAL_ENABLED
1044 0 : case EKFType::EXTERNAL:
1045 0 : success = ret_ext;
1046 0 : break;
1047 : #endif
1048 : }
1049 14 : return success;
1050 7 : }
1051 :
1052 : // Record the current valid origin to parameters
1053 : // This may save the user from having to set the origin manually when using position controlled modes without GPS
1054 964 : void AP_AHRS::record_origin()
1055 : {
1056 : // Only record origin if user has enabled using the record origin option
1057 964 : if (!option_set(Options::RECORD_ORIGIN)) {
1058 962 : return;
1059 : }
1060 2 : _origin_lat.set_and_save_ifchanged(state.origin.lat * 1.0e-7);
1061 2 : _origin_lon.set_and_save_ifchanged(state.origin.lng * 1.0e-7);
1062 2 : _origin_alt.set_and_save_ifchanged(state.origin.alt * 1.0e-2);
1063 : }
1064 :
1065 : // Set the origin to the last recorded location if option bit set and not using GPS
1066 : // This is useful for position controlled modes without GPS
1067 5099682 : void AP_AHRS::use_recorded_origin_maybe()
1068 : {
1069 : // exit immediately if origin is already set or option disabled
1070 5099682 : if (state.origin_ok || !option_set(Options::USE_RECORDED_ORIGIN_FOR_NONGPS)) {
1071 5099680 : return;
1072 : }
1073 :
1074 : // never use all zero origin
1075 364395 : if (is_zero(_origin_lat.get()) && is_zero(_origin_lon.get()) && is_zero(_origin_alt.get())) {
1076 363590 : return;
1077 : }
1078 :
1079 : // don't use recorded origin if the configured EKF uses GPS for
1080 : // position — GPS will set a correct origin when it gets
1081 : // a fix. Using the recorded origin here would prevent GPS from
1082 : // setting it later (EKF origin is immutable once set).
1083 805 : if (active_estimates->configured_to_use_gps_for_pos_XY) {
1084 803 : return;
1085 : }
1086 :
1087 : // try to set origin
1088 : const Location loc {
1089 2 : int32_t(_origin_lat.get() * 1e7),
1090 4 : int32_t(_origin_lon.get() * 1e7),
1091 4 : int32_t(_origin_alt.get() * 100),
1092 : Location::AltFrame::ABSOLUTE
1093 2 : };
1094 2 : if (set_origin(loc)) {
1095 2 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AHRS: using recorded origin:%.7f,%.7f,%.1f",
1096 : (double)_origin_lat.get(), (double)_origin_lon.get(), (double)_origin_alt.get());
1097 : }
1098 : }
1099 :
1100 : #if AP_AHRS_POSITION_RESET_ENABLED
1101 2 : bool AP_AHRS::handle_external_position_estimate(const Location &loc, float pos_accuracy, uint32_t timestamp_ms)
1102 : {
1103 : #if HAL_NAVEKF3_AVAILABLE
1104 2 : return ekf3.EKF3.setLatLng(loc, pos_accuracy, timestamp_ms);
1105 : #endif
1106 : return false;
1107 : }
1108 : #endif
1109 :
1110 : // return true if inertial navigation is active
1111 9908048 : bool AP_AHRS::have_inertial_nav(void) const
1112 : {
1113 : #if AP_AHRS_DCM_ENABLED
1114 9908048 : return active_EKF_type() != EKFType::DCM;
1115 : #endif
1116 : return true;
1117 : }
1118 :
1119 : // get velocity down in m/s. This returns get_velocity_NED.z() if available, otherwise falls back to get_vert_pos_rate_D()
1120 : // if high_vibes is true then this is equivalent to get_vert_pos_rate_D
1121 22972943 : bool AP_AHRS::get_velocity_D(float &velD, bool high_vibes) const
1122 : {
1123 22972943 : Vector3f velNED;
1124 22972943 : if (!high_vibes && get_velocity_NED(velNED)) {
1125 22957189 : velD = velNED.z;
1126 22957189 : return true;
1127 : }
1128 15754 : return get_vert_pos_rate_D(velD);
1129 : }
1130 :
1131 : // Get a derivative of the vertical position which is kinematically consistent with the vertical position is required by some control loops.
1132 : // 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.
1133 2879287 : bool AP_AHRS::get_vert_pos_rate_D(float &velocity) const
1134 : {
1135 2879287 : return active_estimates->get_vert_pos_rate_D(velocity);
1136 : }
1137 :
1138 1186861 : bool AP_AHRS::get_relative_position_NED_origin_float(Vector3f &vec) const
1139 : {
1140 1186861 : Vector3p tmp_posNED;
1141 1186861 : if (!get_relative_position_NED_origin(tmp_posNED)) {
1142 188629 : return false;
1143 : }
1144 998232 : vec = tmp_posNED.tofloat();
1145 998232 : return true;
1146 : }
1147 :
1148 : /*
1149 : return a relative ground position from home in meters
1150 : */
1151 27237 : bool AP_AHRS::get_relative_position_NED_home(Vector3f &vec) const
1152 : {
1153 27237 : Location loc;
1154 54474 : if (!_home_is_set ||
1155 27237 : !get_location(loc)) {
1156 0 : return false;
1157 : }
1158 27237 : vec = _home.get_distance_NED(loc);
1159 27237 : return true;
1160 : }
1161 :
1162 11398220 : bool AP_AHRS::get_relative_position_NE_origin_float(Vector2f &posNE) const
1163 : {
1164 11398220 : Vector2p tmp_posNE;
1165 11398220 : if (!get_relative_position_NE_origin(tmp_posNE)) {
1166 447836 : return false;
1167 : }
1168 10950384 : posNE = tmp_posNE.tofloat();
1169 10950384 : return true;
1170 : }
1171 :
1172 : /*
1173 : return a relative ground position from home in meters North/East
1174 : */
1175 267973 : bool AP_AHRS::get_relative_position_NE_home(Vector2f &posNE) const
1176 : {
1177 267973 : Location loc;
1178 533887 : if (!_home_is_set ||
1179 265914 : !get_location(loc)) {
1180 2059 : return false;
1181 : }
1182 :
1183 265914 : posNE = _home.get_distance_NE(loc);
1184 265914 : return true;
1185 : }
1186 :
1187 55259479 : bool AP_AHRS::get_relative_position_D_origin_float(float &posD) const
1188 : {
1189 : postype_t tmp_posD;
1190 55259479 : if (!get_relative_position_D_origin(tmp_posD)) {
1191 3507041 : return false;
1192 : }
1193 51752438 : posD = float(tmp_posD);
1194 51752438 : return true;
1195 : }
1196 :
1197 : /*
1198 : return relative position from home in meters
1199 : */
1200 8543040 : void AP_AHRS::get_relative_position_D_home(float &posD) const
1201 : {
1202 8543040 : if (!_home_is_set) {
1203 : // fall back to an altitude derived from barometric pressure
1204 : // differences vs a calibrated ground pressure:
1205 346133 : posD = -AP::baro().get_altitude();
1206 346133 : return;
1207 : }
1208 :
1209 8196907 : Location originLLH;
1210 : postype_t originD;
1211 16392769 : if (!get_relative_position_D_origin(originD) ||
1212 8195862 : !_get_origin(originLLH)) {
1213 : #if AP_GPS_ENABLED
1214 267126 : const auto &gps = AP::gps();
1215 267126 : if (_gps_use == GPSUse::EnableWithHeight &&
1216 0 : gps.status() >= AP_GPS_FixType::FIX_3D) {
1217 0 : posD = (_home.alt - gps.location().alt) * 0.01;
1218 0 : return;
1219 : }
1220 : #endif
1221 267126 : posD = -AP::baro().get_altitude();
1222 267126 : return;
1223 : }
1224 :
1225 7929781 : posD = originD - ((originLLH.alt - _home.alt) * 0.01f);
1226 7929781 : return;
1227 : }
1228 :
1229 : /*
1230 : canonicalise _ekf_type, forcing it to be 0, 2 or 3
1231 : type 1 has been deprecated
1232 : */
1233 41690904 : AP_AHRS::EKFType AP_AHRS::_configured_ekf_type(void) const
1234 : {
1235 41690904 : EKFType type = (EKFType)_ekf_type.get();
1236 41690904 : switch (type) {
1237 : #if AP_AHRS_SIM_ENABLED
1238 3410756 : case EKFType::SIM:
1239 3410756 : return type;
1240 : #endif
1241 : #if AP_AHRS_EXTERNAL_ENABLED
1242 67069 : case EKFType::EXTERNAL:
1243 67069 : return type;
1244 : #endif
1245 : #if HAL_NAVEKF2_AVAILABLE
1246 146417 : case EKFType::TWO:
1247 146417 : return type;
1248 : #endif
1249 : #if HAL_NAVEKF3_AVAILABLE
1250 37937043 : case EKFType::THREE:
1251 37937043 : return type;
1252 : #endif
1253 : #if AP_AHRS_DCM_ENABLED
1254 33214 : case EKFType::DCM:
1255 33214 : if (always_use_EKF()) {
1256 : #if HAL_NAVEKF2_AVAILABLE
1257 0 : return EKFType::TWO;
1258 : #elif HAL_NAVEKF3_AVAILABLE
1259 0 : return EKFType::THREE;
1260 : #endif
1261 : }
1262 33214 : return EKFType::DCM;
1263 : #endif
1264 : }
1265 : // we can get to here if the user has mis-set AHRS_EKF_TYPE - any
1266 : // value above 3 will get to here. TWO is returned here for no
1267 : // better reason than "tradition".
1268 : #if HAL_NAVEKF2_AVAILABLE
1269 96405 : return EKFType::TWO;
1270 : #elif HAL_NAVEKF3_AVAILABLE
1271 0 : return EKFType::THREE;
1272 : #elif AP_AHRS_DCM_ENABLED
1273 : return EKFType::DCM;
1274 : #else
1275 : #error "no default backend available"
1276 : #endif
1277 : }
1278 :
1279 41690904 : AP_AHRS::EKFType AP_AHRS::_active_EKF_type(void) const
1280 : {
1281 41690904 : EKFType ret = fallback_active_EKF_type();
1282 :
1283 41690904 : switch (configured_ekf_type()) {
1284 : #if AP_AHRS_DCM_ENABLED
1285 33214 : case EKFType::DCM:
1286 33214 : return EKFType::DCM;
1287 : #endif
1288 :
1289 : #if HAL_NAVEKF2_AVAILABLE
1290 242822 : case EKFType::TWO: {
1291 : // do we have an EKF2 yet?
1292 242822 : if (!ekf2.started) {
1293 102306 : return fallback_active_EKF_type();
1294 : }
1295 140516 : if (always_use_EKF()) {
1296 135211 : if (ekf2_estimates.filter_faults == 0) {
1297 135211 : ret = EKFType::TWO;
1298 : }
1299 5305 : } else if (ekf2_estimates.healthy) {
1300 5017 : ret = EKFType::TWO;
1301 : }
1302 140516 : break;
1303 : }
1304 : #endif
1305 :
1306 : #if HAL_NAVEKF3_AVAILABLE
1307 37937043 : case EKFType::THREE: {
1308 : // do we have an EKF3 yet?
1309 37937043 : if (!ekf3.started) {
1310 1227373 : return fallback_active_EKF_type();
1311 : }
1312 36709670 : if (always_use_EKF()) {
1313 28866369 : if (ekf3_estimates.filter_faults == 0) {
1314 28866369 : ret = EKFType::THREE;
1315 : }
1316 7843301 : } else if (ekf3_estimates.healthy) {
1317 7794966 : ret = EKFType::THREE;
1318 : }
1319 36709670 : break;
1320 : }
1321 : #endif
1322 :
1323 : #if AP_AHRS_SIM_ENABLED
1324 3410756 : case EKFType::SIM:
1325 3410756 : ret = EKFType::SIM;
1326 3410756 : break;
1327 : #endif
1328 : #if AP_AHRS_EXTERNAL_ENABLED
1329 67069 : case EKFType::EXTERNAL:
1330 67069 : ret = EKFType::EXTERNAL;
1331 67069 : break;
1332 : #endif
1333 : }
1334 :
1335 : #if AP_AHRS_DCM_ENABLED
1336 : // Handle fallback for fixed wing planes (including VTOL's) and ground vehicles.
1337 40328011 : if (_vehicle_class == VehicleClass::FIXED_WING ||
1338 32470491 : _vehicle_class == VehicleClass::GROUND) {
1339 8462025 : bool should_use_gps = true;
1340 8462025 : nav_filter_status filt_state {};
1341 8462025 : switch (ret) {
1342 47017 : case EKFType::DCM:
1343 : // already using DCM
1344 47017 : break;
1345 : #if HAL_NAVEKF2_AVAILABLE
1346 5017 : case EKFType::TWO:
1347 5017 : filt_state = ekf2_estimates.filter_status;
1348 5017 : should_use_gps = ekf2.EKF2.configuredToUseGPSForPosXY();
1349 5017 : break;
1350 : #endif
1351 : #if HAL_NAVEKF3_AVAILABLE
1352 7713723 : case EKFType::THREE:
1353 7713723 : filt_state = ekf3_estimates.filter_status;
1354 7713723 : should_use_gps = ekf3.EKF3.configuredToUseGPSForPosXY();
1355 7713723 : break;
1356 : #endif
1357 : #if AP_AHRS_SIM_ENABLED
1358 645420 : case EKFType::SIM:
1359 645420 : filt_state = sim_estimates.filter_status;
1360 645420 : break;
1361 : #endif
1362 : #if AP_AHRS_EXTERNAL_ENABLED
1363 50848 : case EKFType::EXTERNAL:
1364 50848 : filt_state = external_estimates.filter_status;
1365 50848 : should_use_gps = true;
1366 50848 : break;
1367 : #endif
1368 : }
1369 :
1370 : // Handle fallback for the case where the DCM or EKF is unable to provide attitude or height data.
1371 8462025 : const bool can_use_dcm = dcm.yaw_source_available() || fly_forward;
1372 : // ground vehicles only require attitude from the EKF, other vehicles also require vertical velocity and position
1373 16836589 : const bool can_use_ekf = filt_state.flags.attitude &&
1374 8374564 : ((_vehicle_class == VehicleClass::GROUND) ||
1375 7785831 : (filt_state.flags.vert_vel && filt_state.flags.vert_pos));
1376 8462025 : if (!can_use_dcm && can_use_ekf) {
1377 : // no choice - continue to use EKF
1378 1249825 : return ret;
1379 8457431 : } else if (!can_use_ekf) {
1380 : // No choice - we have to use DCM
1381 87555 : return EKFType::DCM;
1382 : }
1383 :
1384 8369876 : const bool disable_dcm_fallback = fly_forward?
1385 8369876 : option_set(Options::DISABLE_DCM_FALLBACK_FW) : option_set(Options::DISABLE_DCM_FALLBACK_VTOL);
1386 8369876 : if (disable_dcm_fallback) {
1387 : // don't fallback
1388 618800 : return ret;
1389 : }
1390 :
1391 : // Handle loss of global position when we still have a GPS fix
1392 14053457 : if (hal.util->get_soft_armed() &&
1393 12604762 : (_gps_use != GPSUse::Disable) &&
1394 6302381 : should_use_gps &&
1395 20355838 : AP::gps().status() >= AP_GPS_FixType::FIX_3D &&
1396 6296947 : (!filt_state.flags.using_gps || !filt_state.flags.horiz_pos_abs)) {
1397 : /*
1398 : If the EKF is not fusing GPS or doesn't have a 2D fix and we have a 3D GPS lock,
1399 : then plane and rover would prefer to use the GPS position from DCM unless the
1400 : fallback has been inhibited by the user.
1401 : Note: The aircraft could be dead reckoning with acceptable accuracy and rejecting a bad GPS
1402 : Note: This is a last resort fallback and makes the navigation highly vulnerable to GPS noise.
1403 : Note: When operating in a VTOL flight mode that actively controls height such as QHOVER,
1404 : the EKF gives better vertical velocity and position estimates and height control characteristics.
1405 : */
1406 3131 : return EKFType::DCM;
1407 : }
1408 :
1409 : // Handle complete loss of navigation
1410 7747945 : if (hal.util->get_soft_armed() && filt_state.flags.const_pos_mode) {
1411 : /*
1412 : Provided the EKF has been configured to use GPS, ie should_use_gps is true, then the
1413 : key difference to the case handled above is only the absence of a GPS fix which means
1414 : that DCM will not be able to navigate either so we are primarily concerned with
1415 : providing an attitude, vertical position and vertical velocity estimate.
1416 : */
1417 0 : return EKFType::DCM;
1418 : }
1419 :
1420 7747945 : if (!filt_state.flags.horiz_vel ||
1421 7744703 : (!filt_state.flags.horiz_pos_abs && !filt_state.flags.horiz_pos_rel)) {
1422 539495 : if ((!AP::compass().use_for_yaw()) &&
1423 539495 : AP::gps().status() >= AP_GPS_FixType::FIX_3D &&
1424 3750 : AP::gps().ground_speed() < 2) {
1425 : /*
1426 : special handling for non-compass mode when sitting
1427 : still. The EKF may not yet have aligned its yaw. We
1428 : accept EKF as healthy to allow arming. Once we reach
1429 : speed the EKF should get yaw alignment
1430 : */
1431 3243 : if (filt_state.flags.gps_quality_good) {
1432 320 : return ret;
1433 : }
1434 : }
1435 535425 : return EKFType::DCM;
1436 : }
1437 : }
1438 : #endif
1439 :
1440 39078186 : return ret;
1441 : }
1442 :
1443 82720877 : AP_AHRS::EKFType AP_AHRS::fallback_active_EKF_type(void) const
1444 : {
1445 : #if AP_AHRS_DCM_ENABLED
1446 82720877 : return EKFType::DCM;
1447 : #endif
1448 :
1449 : #if HAL_NAVEKF3_AVAILABLE
1450 : if (ekf3.started) {
1451 : return EKFType::THREE;
1452 : }
1453 : #endif
1454 :
1455 : #if HAL_NAVEKF2_AVAILABLE
1456 : if (ekf2.started) {
1457 : return EKFType::TWO;
1458 : }
1459 : #endif
1460 :
1461 : #if AP_AHRS_EXTERNAL_ENABLED
1462 : if (external_estimates.healthy) {
1463 : return EKFType::EXTERNAL;
1464 : }
1465 : #endif
1466 :
1467 : // so nobody is ready yet. Return something, even if it is not ready:
1468 : #if HAL_NAVEKF3_AVAILABLE
1469 : return EKFType::THREE;
1470 : #elif HAL_NAVEKF2_AVAILABLE
1471 : return EKFType::TWO;
1472 : #elif AP_AHRS_EXTERNAL_ENABLED
1473 : return EKFType::EXTERNAL;
1474 : #endif
1475 : }
1476 :
1477 : // get secondary EKF type. returns false if no secondary (i.e. only using DCM)
1478 41690904 : bool AP_AHRS::_get_secondary_EKF_type(EKFType &secondary_ekf_type) const
1479 : {
1480 :
1481 41690904 : switch (active_EKF_type()) {
1482 : #if AP_AHRS_DCM_ENABLED
1483 1990610 : case EKFType::DCM:
1484 : // EKF2, EKF3 or External is secondary
1485 : #if HAL_NAVEKF3_AVAILABLE
1486 1990610 : if ((EKFType)_ekf_type.get() == EKFType::THREE) {
1487 1849557 : secondary_ekf_type = EKFType::THREE;
1488 1849557 : return true;
1489 : }
1490 : #endif
1491 : #if HAL_NAVEKF2_AVAILABLE
1492 141053 : if ((EKFType)_ekf_type.get() == EKFType::TWO) {
1493 10437 : secondary_ekf_type = EKFType::TWO;
1494 10437 : return true;
1495 : }
1496 : #endif
1497 : #if AP_AHRS_EXTERNAL_ENABLED
1498 130616 : if ((EKFType)_ekf_type.get() == EKFType::EXTERNAL) {
1499 997 : secondary_ekf_type = EKFType::EXTERNAL;
1500 997 : return true;
1501 : }
1502 : #endif
1503 129619 : return false;
1504 : #endif
1505 : #if HAL_NAVEKF2_AVAILABLE
1506 39700294 : case EKFType::TWO:
1507 : #endif
1508 : #if HAL_NAVEKF3_AVAILABLE
1509 0 : case EKFType::THREE:
1510 : #endif
1511 : #if AP_AHRS_SIM_ENABLED
1512 : case EKFType::SIM:
1513 : #endif
1514 : #if AP_AHRS_EXTERNAL_ENABLED
1515 : case EKFType::EXTERNAL:
1516 : #endif
1517 : // DCM is secondary
1518 39700294 : secondary_ekf_type = fallback_active_EKF_type();
1519 39700294 : return true;
1520 : }
1521 :
1522 : // since there is no default case above, this is unreachable
1523 0 : return false;
1524 : }
1525 :
1526 : /*
1527 : check if the AHRS subsystem is healthy
1528 : */
1529 1167760 : bool AP_AHRS::healthy(void) const
1530 : {
1531 1167760 : if (!configured_estimates->healthy) {
1532 2869 : return false;
1533 : }
1534 :
1535 : // we must be using the configured type to be considered healthy:
1536 1164891 : if (configured_backend != active_backend) {
1537 12996 : return false;
1538 : }
1539 :
1540 1151895 : return true;
1541 : }
1542 :
1543 : // returns false if we fail arming checks, in which case the buffer will be populated with a failure message
1544 : // requires_position should be true if horizontal position configuration should be checked
1545 109571 : bool AP_AHRS::pre_arm_check(bool requires_position, char *failure_msg, uint8_t failure_msg_len) const
1546 : {
1547 109571 : bool ret = true;
1548 109571 : if (!healthy()) {
1549 : // this rather generic failure might be overwritten by
1550 : // something more specific in the "backend"
1551 7561 : hal.util->snprintf(failure_msg, failure_msg_len, "Not healthy");
1552 7561 : ret = false;
1553 : }
1554 :
1555 : #if AP_AHRS_EXTERNAL_ENABLED
1556 : // Always check external AHRS if enabled
1557 : // it is a source for IMU data even if not being used as direct AHRS replacement
1558 109571 : if (AP::externalAHRS().enabled() || (configured_ekf_type() == EKFType::EXTERNAL)) {
1559 66685 : if (!AP::externalAHRS().pre_arm_check(failure_msg, failure_msg_len)) {
1560 35 : return false;
1561 : }
1562 : }
1563 : #endif
1564 :
1565 109536 : if (!attitudes_consistent(failure_msg, failure_msg_len)) {
1566 1359 : return false;
1567 : }
1568 :
1569 : // ensure we're using the configured backend, but bypass in compass-less cases:
1570 222491 : if (_ekf_type != int8_t(configured_ekf_type()) ||
1571 114314 : (configured_ekf_type() != active_EKF_type() && AP::compass().use_for_yaw())) {
1572 6105 : hal.util->snprintf(failure_msg, failure_msg_len, "not using configured AHRS type");
1573 6105 : return false;
1574 : }
1575 :
1576 102072 : return (configured_backend->pre_arm_check(requires_position, failure_msg, failure_msg_len) && ret);
1577 : }
1578 :
1579 : // write optical flow data to EKF
1580 10128 : void AP_AHRS::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride)
1581 : {
1582 : #if HAL_NAVEKF2_AVAILABLE
1583 10128 : ekf2.EKF2.writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawGyroRates, msecFlowMeas, posOffset, heightOverride);
1584 : #endif
1585 : #if EK3_FEATURE_OPTFLOW_FUSION
1586 10128 : ekf3.EKF3.writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawGyroRates, msecFlowMeas, posOffset, heightOverride);
1587 : #endif
1588 10128 : }
1589 :
1590 : // retrieve latest corrected optical flow samples (used for calibration)
1591 11051 : bool AP_AHRS::getOptFlowSample(uint32_t& timeStamp_ms, Vector2f& flowRate, Vector2f& bodyRate, Vector2f& losPred) const
1592 : {
1593 : #if EK3_FEATURE_OPTFLOW_FUSION
1594 11051 : return ekf3.EKF3.getOptFlowSample(timeStamp_ms, flowRate, bodyRate, losPred);
1595 : #endif
1596 : return false;
1597 : }
1598 :
1599 : // write body frame odometry measurements to the EKF
1600 3633 : void AP_AHRS::writeBodyFrameOdom(float quality, const Vector3f &delPos, const Vector3f &delAng, float delTime, uint32_t timeStamp_ms, uint16_t delay_ms, const Vector3f &posOffset)
1601 : {
1602 : #if HAL_NAVEKF3_AVAILABLE
1603 3633 : ekf3.EKF3.writeBodyFrameOdom(quality, delPos, delAng, delTime, timeStamp_ms, delay_ms, posOffset);
1604 : #endif
1605 3633 : }
1606 :
1607 : // Write position and quaternion data from an external navigation system
1608 10069 : void AP_AHRS::writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint16_t delay_ms, uint32_t resetTime_ms)
1609 : {
1610 : #if HAL_NAVEKF2_AVAILABLE
1611 10069 : ekf2.EKF2.writeExtNavData(pos, quat, posErr, angErr, timeStamp_ms, delay_ms, resetTime_ms);
1612 : #endif
1613 : #if HAL_NAVEKF3_AVAILABLE
1614 10069 : ekf3.EKF3.writeExtNavData(pos, quat, posErr, angErr, timeStamp_ms, delay_ms, resetTime_ms);
1615 : #endif
1616 10069 : }
1617 :
1618 : // Writes the default equivalent airspeed and 1-sigma uncertainty in m/s to be used in forward flight if a measured airspeed is required and not available.
1619 0 : void AP_AHRS::writeDefaultAirSpeed(float airspeed, float uncertainty)
1620 : {
1621 : #if HAL_NAVEKF2_AVAILABLE
1622 0 : ekf2.EKF2.writeDefaultAirSpeed(airspeed);
1623 : #endif
1624 : #if HAL_NAVEKF3_AVAILABLE
1625 0 : ekf3.EKF3.writeDefaultAirSpeed(airspeed, uncertainty);
1626 : #endif
1627 0 : }
1628 :
1629 : // Write velocity data from an external navigation system
1630 4278 : void AP_AHRS::writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeStamp_ms, uint16_t delay_ms)
1631 : {
1632 : #if HAL_NAVEKF2_AVAILABLE
1633 4278 : ekf2.EKF2.writeExtNavVelData(vel, err, timeStamp_ms, delay_ms);
1634 : #endif
1635 : #if HAL_NAVEKF3_AVAILABLE
1636 4278 : ekf3.EKF3.writeExtNavVelData(vel, err, timeStamp_ms, delay_ms);
1637 : #endif
1638 4278 : }
1639 :
1640 : // set the terrain SRTM altitude in meters above sea level
1641 : // only used by optical flow when out of rangefinder range
1642 : // Write terrain (derived from SRTM) altitude in meters above sea level
1643 1127469 : void AP_AHRS::writeTerrainAMSL(float alt_amsl_m)
1644 : {
1645 : #if EK3_FEATURE_OPTFLOW_SRTM
1646 : // return immediately if EKF origin has not been set
1647 1127469 : if (!state.origin_ok) {
1648 62480 : return;
1649 : }
1650 : // convert from amsl alt to alt above EKF origin
1651 1064989 : const float alt_above_origin_m = alt_amsl_m - (state.origin.alt * 0.01f);
1652 1064989 : ekf3.EKF3.writeTerrainData(alt_above_origin_m);
1653 : #endif
1654 : }
1655 :
1656 : // Retrieves the NED delta velocity corrected
1657 41686716 : bool AP_AHRS::_getCorrectedDeltaVelocityNED(Vector3f& ret, float& dt) const
1658 : {
1659 41686716 : if (!AP::ins().get_delta_velocity(active_estimates->primary_accel, ret, dt)) {
1660 0 : return false;
1661 : }
1662 41686716 : ret -= active_estimates->accel_bias * dt;
1663 41686716 : ret = state.dcm_matrix * get_rotation_autopilot_body_to_vehicle_body() * ret;
1664 41686716 : ret.z += GRAVITY_MSS*dt;
1665 41686716 : return true;
1666 : }
1667 :
1668 1359 : void AP_AHRS::set_failure_inconsistent_message(const char *estimator, const char *axis, float diff_rad, char *failure_msg, const uint8_t failure_msg_len) const
1669 : {
1670 1359 : hal.util->snprintf(failure_msg, failure_msg_len, "%s %s inconsistent %d deg. Wait or reboot", estimator, axis, (int)degrees(diff_rad));
1671 1359 : }
1672 :
1673 : // check all cores providing consistent attitudes for prearm checks
1674 109536 : bool AP_AHRS::attitudes_consistent(char *failure_msg, const uint8_t failure_msg_len) const
1675 : {
1676 : // get primary attitude source's attitude as quaternion
1677 109536 : Quaternion primary_quat;
1678 109536 : get_quat_body_to_ned(primary_quat);
1679 : // only check yaw if compasses are being used
1680 109536 : const bool check_yaw = AP::compass().use_for_yaw();
1681 109536 : uint8_t total_ekf_cores = 0;
1682 :
1683 : #if HAL_NAVEKF2_AVAILABLE
1684 : // check primary vs ekf2
1685 109536 : if (configured_ekf_type() == EKFType::TWO || active_EKF_type() == EKFType::TWO) {
1686 690 : for (uint8_t i = 0; i < ekf2.EKF2.activeCores(); i++) {
1687 459 : Quaternion ekf2_quat;
1688 459 : ekf2.EKF2.getQuaternionBodyToNED(i, ekf2_quat);
1689 :
1690 : // check roll and pitch difference
1691 459 : const float rp_diff_rad = primary_quat.roll_pitch_difference(ekf2_quat);
1692 459 : if (rp_diff_rad > ATTITUDE_CHECK_THRESH_ROLL_PITCH_RAD) {
1693 0 : set_failure_inconsistent_message("EKF2", "Roll/Pitch", rp_diff_rad, failure_msg, failure_msg_len);
1694 3 : return false;
1695 : }
1696 :
1697 : // check yaw difference
1698 459 : Vector3f angle_diff;
1699 459 : primary_quat.angular_difference(ekf2_quat).to_axis_angle(angle_diff);
1700 459 : const float yaw_diff = fabsf(angle_diff.z);
1701 459 : if (check_yaw && (yaw_diff > ATTITUDE_CHECK_THRESH_YAW_RAD)) {
1702 3 : set_failure_inconsistent_message("EKF2", "Yaw", yaw_diff, failure_msg, failure_msg_len);
1703 3 : return false;
1704 : }
1705 : }
1706 231 : total_ekf_cores = ekf2.EKF2.activeCores();
1707 : }
1708 : #endif
1709 :
1710 : #if HAL_NAVEKF3_AVAILABLE
1711 : // check primary vs ekf3
1712 109533 : if (configured_ekf_type() == EKFType::THREE || active_EKF_type() == EKFType::THREE) {
1713 111229 : for (uint8_t i = 0; i < ekf3.EKF3.activeCores(); i++) {
1714 73821 : Quaternion ekf3_quat;
1715 73821 : ekf3.EKF3.getQuaternionBodyToNED(i, ekf3_quat);
1716 :
1717 : // check roll and pitch difference
1718 73821 : const float rp_diff_rad = primary_quat.roll_pitch_difference(ekf3_quat);
1719 73821 : if (rp_diff_rad > ATTITUDE_CHECK_THRESH_ROLL_PITCH_RAD) {
1720 21 : set_failure_inconsistent_message("EKF3", "Roll/Pitch", rp_diff_rad, failure_msg, failure_msg_len);
1721 792 : return false;
1722 : }
1723 :
1724 : // check yaw difference
1725 73800 : Vector3f angle_diff;
1726 73800 : primary_quat.angular_difference(ekf3_quat).to_axis_angle(angle_diff);
1727 73800 : const float yaw_diff = fabsf(angle_diff.z);
1728 73800 : if (check_yaw && (yaw_diff > ATTITUDE_CHECK_THRESH_YAW_RAD)) {
1729 771 : set_failure_inconsistent_message("EKF3", "Yaw", yaw_diff, failure_msg, failure_msg_len);
1730 771 : return false;
1731 : }
1732 : }
1733 37408 : total_ekf_cores += ekf3.EKF3.activeCores();
1734 : }
1735 : #endif
1736 :
1737 : #if AP_AHRS_DCM_ENABLED
1738 : // check primary vs dcm
1739 108741 : if (!always_use_EKF() || (total_ekf_cores == 1)) {
1740 19034 : Quaternion dcm_quat;
1741 19034 : dcm_quat.from_rotation_matrix(get_DCM_rotation_body_to_ned());
1742 :
1743 : // check roll and pitch difference
1744 19034 : const float rp_diff_rad = primary_quat.roll_pitch_difference(dcm_quat);
1745 19034 : if (rp_diff_rad > ATTITUDE_CHECK_THRESH_ROLL_PITCH_RAD) {
1746 530 : set_failure_inconsistent_message("DCM", "Roll/Pitch", rp_diff_rad, failure_msg, failure_msg_len);
1747 564 : return false;
1748 : }
1749 :
1750 : // Check vs DCM yaw if this vehicle could use DCM in flight
1751 : // and if not using an external yaw source (DCM does not support external yaw sources)
1752 18504 : bool using_noncompass_for_yaw = false;
1753 : #if HAL_NAVEKF3_AVAILABLE
1754 18504 : using_noncompass_for_yaw = (configured_ekf_type() == EKFType::THREE) && ekf3.EKF3.using_noncompass_for_yaw();
1755 : #endif
1756 18504 : if (!always_use_EKF() && !using_noncompass_for_yaw) {
1757 18370 : Vector3f angle_diff;
1758 18370 : primary_quat.angular_difference(dcm_quat).to_axis_angle(angle_diff);
1759 18370 : const float yaw_diff = fabsf(angle_diff.z);
1760 18370 : if (check_yaw && (yaw_diff > ATTITUDE_CHECK_THRESH_YAW_RAD)) {
1761 34 : set_failure_inconsistent_message("DCM", "Yaw", yaw_diff, failure_msg, failure_msg_len);
1762 34 : return false;
1763 : }
1764 : }
1765 : }
1766 : #endif
1767 :
1768 108177 : return true;
1769 : }
1770 :
1771 : // Resets the baro so that it reads zero at the current height
1772 : // Resets the EKF height to zero
1773 : // Adjusts the EKf origin height so that the EKF height + origin height is the same as before
1774 3121 : void AP_AHRS::resetHeightDatum(void)
1775 : {
1776 : // support locked access functions to AHRS data
1777 3121 : WITH_SEMAPHORE(_rsem);
1778 :
1779 18726 : for (auto &backend_and_estimates : backends_and_estimates) {
1780 15605 : backend_and_estimates.backend.resetHeightDatum();
1781 : }
1782 3121 : }
1783 :
1784 : #if HAL_GCS_ENABLED
1785 : // send a EKF_STATUS_REPORT for configured EKF
1786 1117755 : void AP_AHRS::send_ekf_status_report(GCS_MAVLINK &link) const
1787 : {
1788 : // get filter status
1789 1117755 : if (!configured_estimates->filter_status_valid) {
1790 17862 : return;
1791 : }
1792 :
1793 : // get variances
1794 1112214 : if (!configured_estimates->variances_valid) {
1795 12321 : return;
1796 : }
1797 :
1798 1099893 : if (!configured_estimates->terrain_alt_variance_valid) {
1799 0 : return;
1800 : }
1801 :
1802 : // prepare flags
1803 1099893 : uint16_t flags = 0;
1804 1099893 : if (configured_estimates->filter_status.flags.attitude) {
1805 1054318 : flags |= EKF_ATTITUDE;
1806 : }
1807 1099893 : if (configured_estimates->filter_status.flags.horiz_vel) {
1808 1052819 : flags |= EKF_VELOCITY_HORIZ;
1809 : }
1810 1099893 : if (configured_estimates->filter_status.flags.vert_vel) {
1811 1054199 : flags |= EKF_VELOCITY_VERT;
1812 : }
1813 1099893 : if (configured_estimates->filter_status.flags.horiz_pos_rel) {
1814 903642 : flags |= EKF_POS_HORIZ_REL;
1815 : }
1816 1099893 : if (configured_estimates->filter_status.flags.horiz_pos_abs) {
1817 898406 : flags |= EKF_POS_HORIZ_ABS;
1818 : }
1819 1099893 : if (configured_estimates->filter_status.flags.vert_pos) {
1820 1053503 : flags |= EKF_POS_VERT_ABS;
1821 : }
1822 1099893 : if (configured_estimates->filter_status.flags.terrain_alt) {
1823 126783 : flags |= EKF_POS_VERT_AGL;
1824 : }
1825 1099893 : if (configured_estimates->filter_status.flags.const_pos_mode) {
1826 149358 : flags |= EKF_CONST_POS_MODE;
1827 : }
1828 1099893 : if (configured_estimates->filter_status.flags.pred_horiz_pos_rel) {
1829 903644 : flags |= EKF_PRED_POS_HORIZ_REL;
1830 : }
1831 1099893 : if (configured_estimates->filter_status.flags.pred_horiz_pos_abs) {
1832 898408 : flags |= EKF_PRED_POS_HORIZ_ABS;
1833 : }
1834 1099893 : if (!configured_estimates->filter_status.flags.initalized) {
1835 40580 : flags |= EKF_UNINITIALIZED;
1836 : }
1837 1099893 : if (configured_estimates->filter_status.flags.gps_glitching) {
1838 2427 : flags |= (1<<15);
1839 : }
1840 :
1841 : const mavlink_ekf_status_report_t packet{
1842 1099893 : configured_estimates->velVar,
1843 1099893 : configured_estimates->posVar,
1844 1099893 : configured_estimates->hgtVar,
1845 1099893 : fmaxf(fmaxf(configured_estimates->magVar.x, configured_estimates->magVar.y), configured_estimates->magVar.z),
1846 1099893 : configured_estimates->terrain_alt_variance,
1847 : flags,
1848 1099893 : configured_estimates->tasVar
1849 1099893 : };
1850 :
1851 : // send message
1852 1099893 : mavlink_msg_ekf_status_report_send_struct(link.get_chan(), &packet);
1853 : }
1854 : #endif // HAL_GCS_ENABLED
1855 :
1856 : // return origin for a specified EKF type
1857 50121474 : bool AP_AHRS::_get_origin(EKFType type, Location &ret) const
1858 : {
1859 50121474 : auto *backend = backend_for_type(type);
1860 50121474 : if (backend == nullptr) {
1861 : // internal error?
1862 0 : return false;
1863 : }
1864 :
1865 50121474 : return backend->get_origin(ret);
1866 : }
1867 :
1868 : /*
1869 : return origin for the configured EKF type. If we are armed and the
1870 : configured EKF type cannot return an origin then return origin for
1871 : the active EKF type (if available)
1872 :
1873 : This copes with users force arming a plane that is running on DCM as
1874 : the EKF has not fully initialised
1875 : */
1876 49882578 : bool AP_AHRS::_get_origin(Location &ret) const
1877 : {
1878 49882578 : if (_get_origin(configured_ekf_type(), ret)) {
1879 44369083 : return true;
1880 : }
1881 5513495 : if (hal.util->get_soft_armed() && _get_origin(active_EKF_type(), ret)) {
1882 134820 : return true;
1883 : }
1884 5378675 : return false;
1885 : }
1886 :
1887 6762 : bool AP_AHRS::set_home(const Location &loc)
1888 : {
1889 6762 : WITH_SEMAPHORE(_rsem);
1890 : // check location is valid
1891 6762 : if (!loc.initialised()) {
1892 0 : return false;
1893 : }
1894 6762 : if (!loc.check_latlng()) {
1895 0 : return false;
1896 : }
1897 : // home must always be global frame at the moment as .alt is
1898 : // accessed directly by the vehicles and they may not be rigorous
1899 : // in checking the frame type.
1900 6762 : Location tmp = loc;
1901 6762 : if (!tmp.change_alt_frame(Location::AltFrame::ABSOLUTE)) {
1902 0 : return false;
1903 : }
1904 :
1905 : #if !APM_BUILD_TYPE(APM_BUILD_UNKNOWN) && HAL_LOGGING_ENABLED
1906 6758 : if (!_home_is_set) {
1907 : // record home is set
1908 1007 : AP::logger().Write_Event(LogEvent::SET_HOME);
1909 : }
1910 : #endif
1911 :
1912 6762 : _home = tmp;
1913 6762 : _home_is_set = true;
1914 :
1915 : #if HAL_LOGGING_ENABLED
1916 6762 : Log_Write_Home_And_Origin();
1917 : #endif
1918 :
1919 : // send new home and ekf origin to GCS
1920 6762 : GCS_SEND_MESSAGE(MSG_HOME);
1921 6762 : GCS_SEND_MESSAGE(MSG_ORIGIN);
1922 :
1923 6762 : AP_HAL::Util::PersistentData &pd = hal.util->persistent_data;
1924 6762 : pd.home_lat = loc.lat;
1925 6762 : pd.home_lon = loc.lng;
1926 6762 : pd.home_alt_cm = loc.alt;
1927 :
1928 : #if AP_MISSION_ENABLED
1929 : // Save home to mission
1930 6762 : AP::mission().write_home_to_storage();
1931 : #endif
1932 :
1933 6762 : return true;
1934 6762 : }
1935 :
1936 : /* if this was a watchdog reset then get home from backup registers */
1937 2086 : void AP_AHRS::load_watchdog_home()
1938 : {
1939 2086 : const AP_HAL::Util::PersistentData &pd = hal.util->persistent_data;
1940 2086 : if (hal.util->was_watchdog_reset() && (pd.home_lat != 0 || pd.home_lon != 0)) {
1941 1 : _home.lat = pd.home_lat;
1942 1 : _home.lng = pd.home_lon;
1943 1 : _home.set_alt_cm(pd.home_alt_cm, Location::AltFrame::ABSOLUTE);
1944 1 : _home_is_set = true;
1945 1 : _home_locked = true;
1946 1 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Restored watchdog home");
1947 : }
1948 2086 : }
1949 :
1950 : // Set to true if the terrain underneath is stable enough to be used as a height reference
1951 : // this is not related to terrain following
1952 3800955 : void AP_AHRS::set_terrain_hgt_stable(bool stable)
1953 : {
1954 : // avoid repeatedly setting variable in NavEKF objects to prevent
1955 : // spurious event logging
1956 3800955 : switch (terrainHgtStableState) {
1957 998 : case TriState::UNKNOWN:
1958 998 : break;
1959 831167 : case TriState::True:
1960 831167 : if (stable) {
1961 830595 : return;
1962 : }
1963 572 : break;
1964 2968790 : case TriState::False:
1965 2968790 : if (!stable) {
1966 2968021 : return;
1967 : }
1968 769 : break;
1969 : }
1970 2339 : terrainHgtStableState = (TriState)stable;
1971 :
1972 : #if HAL_NAVEKF2_AVAILABLE
1973 2339 : ekf2.EKF2.setTerrainHgtStable(stable);
1974 : #endif
1975 : #if HAL_NAVEKF3_AVAILABLE
1976 2339 : ekf3.EKF3.setTerrainHgtStable(stable);
1977 : #endif
1978 : }
1979 :
1980 : // get 1-sigma position and velocity uncertainty from the EKF state error covariance matrix P
1981 13 : bool AP_AHRS::get_pos_vel_uncertainty(float &pos_horiz_m, float &pos_vert_m, float &vel_m_s) const
1982 : {
1983 13 : switch (active_EKF_type()) {
1984 : #if HAL_NAVEKF3_AVAILABLE
1985 13 : case EKFType::THREE:
1986 13 : return ekf3.EKF3.getPosVelUncertainty(pos_horiz_m, pos_vert_m, vel_m_s);
1987 : #endif
1988 0 : default:
1989 0 : return false;
1990 : }
1991 : }
1992 :
1993 : // get a source's velocity innovations. source should be from 0 to 7 (see AP_NavEKF_Source::SourceXY)
1994 : // returns true on success and results are placed in innovations and variances arguments
1995 39 : bool AP_AHRS::get_vel_innovations_and_variances_for_source(uint8_t source, Vector3f &innovations, Vector3f &variances) const
1996 : {
1997 39 : switch (configured_ekf_type()) {
1998 : #if AP_AHRS_DCM_ENABLED
1999 0 : case EKFType::DCM:
2000 : // We are not using an EKF so no data
2001 0 : return false;
2002 : #endif
2003 :
2004 : #if HAL_NAVEKF2_AVAILABLE
2005 0 : case EKFType::TWO:
2006 : // EKF2 does not support source level variances
2007 0 : return false;
2008 : #endif
2009 :
2010 : #if HAL_NAVEKF3_AVAILABLE
2011 39 : case EKFType::THREE:
2012 : // use EKF to get variance
2013 39 : return ekf3.EKF3.getVelInnovationsAndVariancesForSource((AP_NavEKF_Source::SourceXY)source, innovations, variances);
2014 : #endif
2015 :
2016 : #if AP_AHRS_SIM_ENABLED
2017 0 : case EKFType::SIM:
2018 : // SITL does not support source level variances
2019 0 : return false;
2020 : #endif
2021 :
2022 : #if AP_AHRS_EXTERNAL_ENABLED
2023 0 : case EKFType::EXTERNAL:
2024 0 : return false;
2025 : #endif
2026 : }
2027 :
2028 0 : return false;
2029 : }
2030 :
2031 : //get the index of the active airspeed sensor, wrt the primary core
2032 83375025 : uint8_t AP_AHRS::get_active_airspeed_index() const
2033 : {
2034 : #if AP_AIRSPEED_ENABLED
2035 83375025 : const auto *airspeed = AP::airspeed();
2036 83375025 : if (airspeed == nullptr) {
2037 171422 : return 0;
2038 : }
2039 :
2040 : // we only have affinity for EKF3 as of now
2041 : #if HAL_NAVEKF3_AVAILABLE
2042 83203603 : if (active_EKF_type() == EKFType::THREE) {
2043 72438414 : uint8_t ret = ekf3.EKF3.getActiveAirspeed();
2044 72438414 : if (ret != UINT8_MAX && airspeed->healthy(ret) && airspeed->use(ret)) {
2045 13486831 : return ret;
2046 : }
2047 : }
2048 : #endif
2049 :
2050 : // for the rest, let the primary airspeed sensor be used
2051 69716772 : return airspeed->get_primary();
2052 : #else
2053 :
2054 : return 0;
2055 : #endif // AP_AIRSPEED_ENABLED
2056 : }
2057 :
2058 : #if AP_AHRS_EKF_RESET_ENABLED
2059 : // request full backend reset, currently only implemented for EKF3
2060 : // returns true if the reset was performed
2061 1 : bool AP_AHRS::reset_configured_backend(void)
2062 : {
2063 : // reset EKF3 regardless of active EKF type — if we've fallen back
2064 : // to DCM due to EKF failure, that's exactly when a bootstrap reset
2065 : // is most needed to force re-convergence
2066 1 : switch (configured_ekf_type()) {
2067 : #if AP_AHRS_NAVEKF3_ENABLED
2068 1 : case EKFType::THREE:
2069 1 : return ekf3.EKF3.InitialiseFilterBootstrap();
2070 : #endif // AP_AHRS_NAVEKF3_ENABLED
2071 0 : default:
2072 0 : break;
2073 : }
2074 :
2075 0 : return false;
2076 : }
2077 : #endif // AP_AHRS_EKF_RESET_ENABLED
2078 :
2079 : // set position, velocity and yaw sources to either 0=primary, 1=secondary, 2=tertiary
2080 16 : void AP_AHRS::set_posvelyaw_source_set(AP_NavEKF_Source::SourceSetSelection source_set_idx)
2081 : {
2082 : #if HAL_NAVEKF3_AVAILABLE
2083 16 : ekf3.EKF3.setPosVelYawSourceSet((uint8_t)source_set_idx);
2084 : #endif
2085 16 : }
2086 :
2087 : //returns active source set used, 0=primary, 1=secondary, 2=tertiary
2088 0 : uint8_t AP_AHRS::get_posvelyaw_source_set() const
2089 : {
2090 : #if HAL_NAVEKF3_AVAILABLE
2091 0 : return ekf3.EKF3.get_active_source_set();
2092 : #else
2093 : return 0;
2094 : #endif
2095 : }
2096 :
2097 2461314 : void AP_AHRS::Log_Write()
2098 : {
2099 : #if HAL_NAVEKF2_AVAILABLE
2100 2461314 : ekf2.EKF2.Log_Write();
2101 : #endif
2102 : #if HAL_NAVEKF3_AVAILABLE
2103 2461314 : ekf3.EKF3.Log_Write();
2104 : #endif
2105 :
2106 2461314 : Write_AHRS2();
2107 2461314 : Write_POS();
2108 :
2109 : #if AP_AHRS_SIM_ENABLED
2110 2461314 : AP::sitl()->Log_Write_SIMSTATE();
2111 : #endif
2112 2461314 : }
2113 :
2114 : // check if non-compass sensor is providing yaw. Allows compass pre-arm checks to be bypassed
2115 563 : bool AP_AHRS::using_noncompass_for_yaw(void) const
2116 : {
2117 : #if AP_AHRS_DCM_ENABLED && HAL_NAVEKF3_AVAILABLE
2118 : // FIXME: DCM uses EKF3's answer (which is not necessarily the
2119 : // configured type)
2120 563 : if (active_estimates == &dcm_estimates) {
2121 0 : return ekf3_estimates.using_noncompass_for_yaw;
2122 : }
2123 : #endif
2124 563 : return active_estimates->using_noncompass_for_yaw;
2125 : }
2126 :
2127 : // check if external nav is providing yaw
2128 1186 : bool AP_AHRS::using_extnav_for_yaw(void) const
2129 : {
2130 : #if AP_AHRS_DCM_ENABLED && HAL_NAVEKF3_AVAILABLE
2131 : // FIXME: DCM uses EKF3's answer (which is not necessarily the
2132 : // configured type)
2133 1186 : if (active_estimates == &dcm_estimates) {
2134 154 : return ekf3_estimates.using_extnav_for_yaw;
2135 : }
2136 : #endif
2137 1032 : return active_estimates->using_extnav_for_yaw;
2138 : }
2139 :
2140 : // set and save the alt noise parameter value
2141 82 : void AP_AHRS::set_alt_measurement_noise(float noise)
2142 : {
2143 : #if HAL_NAVEKF2_AVAILABLE
2144 82 : ekf2.EKF2.set_baro_alt_noise(noise);
2145 : #endif
2146 : #if HAL_NAVEKF3_AVAILABLE
2147 82 : ekf3.EKF3.set_baro_alt_noise(noise);
2148 : #endif
2149 82 : }
2150 :
2151 : // check if non-compass sensor is providing yaw. Allows compass pre-arm checks to be bypassed
2152 479 : const EKFGSF_yaw *AP_AHRS::get_yaw_estimator(void) const
2153 : {
2154 479 : switch (active_EKF_type()) {
2155 : #if HAL_NAVEKF2_AVAILABLE
2156 0 : case EKFType::TWO:
2157 0 : return ekf2.EKF2.get_yawEstimator();
2158 : #endif
2159 : #if AP_AHRS_DCM_ENABLED
2160 0 : case EKFType::DCM:
2161 : #if HAL_NAVEKF3_AVAILABLE
2162 0 : return ekf3.EKF3.get_yawEstimator();
2163 : #else
2164 : return nullptr;
2165 : #endif
2166 : #endif
2167 : #if HAL_NAVEKF3_AVAILABLE
2168 479 : case EKFType::THREE:
2169 479 : return ekf3.EKF3.get_yawEstimator();
2170 : #endif
2171 : #if AP_AHRS_SIM_ENABLED
2172 0 : case EKFType::SIM:
2173 : #endif
2174 : #if AP_AHRS_EXTERNAL_ENABLED
2175 0 : case EKFType::EXTERNAL:
2176 : #endif
2177 0 : return nullptr;
2178 : }
2179 : // since there is no default case above, this is unreachable
2180 0 : return nullptr;
2181 : }
2182 :
2183 : // get current location estimate
2184 89554763 : bool AP_AHRS::get_location(Location &loc) const
2185 : {
2186 89554763 : loc = state.location;
2187 89554763 : return state.location_ok;
2188 : }
2189 :
2190 : // return a wind estimation vector in "wind" (m/s); returns false on failure
2191 1943939 : bool AP_AHRS::get_wind(Vector3f &wind) const
2192 : {
2193 1943939 : wind = active_estimates->wind;
2194 1943939 : return active_estimates->wind_valid;
2195 : }
2196 :
2197 : // return an airspeed estimate if available. return true
2198 : // if we have an estimate
2199 74353099 : bool AP_AHRS::airspeed_EAS(float &airspeed_ret) const
2200 : {
2201 74353099 : airspeed_ret = state.airspeed_EAS;
2202 74353099 : return state.airspeed_EAS_ok;
2203 : }
2204 :
2205 : // return an airspeed estimate if available. return true
2206 : // if we have an estimate
2207 1310707 : bool AP_AHRS::airspeed_EAS(float &airspeed_ret, AP_AHRS::AirspeedEstimateType &type) const
2208 : {
2209 1310707 : airspeed_ret = state.airspeed_EAS;
2210 1310707 : type = state.airspeed_estimate_type;
2211 1310707 : return state.airspeed_EAS_ok;
2212 : }
2213 :
2214 : // return a true airspeed estimate (navigation airspeed) if
2215 : // available. return true if we have an estimate
2216 43 : bool AP_AHRS::airspeed_TAS(float &airspeed_ret) const
2217 : {
2218 43 : airspeed_ret = state.airspeed_TAS;
2219 43 : return state.airspeed_TAS_ok;
2220 : }
2221 :
2222 : // return estimate of true airspeed vector in body frame in m/s
2223 : // returns false if estimate is unavailable
2224 770123 : bool AP_AHRS::airspeed_vector_TAS(Vector3f &vec) const
2225 : {
2226 770123 : vec = state.airspeed_TAS_vec;
2227 770123 : return state.airspeed_TAS_vec_ok;
2228 : }
2229 :
2230 : // return the quaternion defining the rotation from NED to XYZ (body) axes
2231 30861 : bool AP_AHRS::get_quaternion(Quaternion &quat) const
2232 : {
2233 30861 : quat = state.quat;
2234 30861 : return state.quat_ok;
2235 : }
2236 :
2237 : // returns the inertial navigation origin in lat/lon/alt
2238 376767905 : bool AP_AHRS::get_origin(Location &ret) const
2239 : {
2240 376767905 : ret = state.origin;
2241 376767905 : return state.origin_ok;
2242 : }
2243 :
2244 : // return a ground velocity in meters/second, North/East/Down
2245 : // order. Must only be called if have_inertial_nav() is true
2246 86101927 : bool AP_AHRS::get_velocity_NED(Vector3f &vec) const
2247 : {
2248 86101927 : vec = state.velocity_NED;
2249 86101927 : return state.velocity_NED_ok;
2250 : }
2251 :
2252 : // return location corresponding to vector relative to the
2253 : // vehicle's origin
2254 4436 : bool AP_AHRS::get_location_from_origin_offset_NED(Location &loc, const Vector3p &offset_ned) const
2255 : {
2256 4436 : if (!get_origin(loc)) {
2257 0 : return false;
2258 : }
2259 4436 : loc.offset(offset_ned);
2260 :
2261 4436 : return true;
2262 : }
2263 :
2264 : // return location corresponding to vector relative to the
2265 : // vehicle's home location
2266 0 : bool AP_AHRS::get_location_from_home_offset_NED(Location &loc, const Vector3p &offset_ned) const
2267 : {
2268 0 : if (!home_is_set()) {
2269 0 : return false;
2270 : }
2271 0 : loc = get_home();
2272 0 : loc.offset(offset_ned);
2273 :
2274 0 : return true;
2275 : }
2276 :
2277 : /*
2278 : get EAS to TAS scaling
2279 : */
2280 99679131 : float AP_AHRS::get_EAS2TAS(void) const
2281 : {
2282 99679131 : if (is_positive(state.EAS2TAS)) {
2283 99674635 : return state.EAS2TAS;
2284 : }
2285 4489 : return 1.0;
2286 : }
2287 :
2288 : // get air density / sea level density - decreases as altitude climbs
2289 67541113 : float AP_AHRS::get_air_density_ratio(void) const
2290 : {
2291 67541113 : const float eas2tas = get_EAS2TAS();
2292 67541113 : return 1.0 / sq(eas2tas);
2293 : }
2294 :
2295 : // singleton instance
2296 : AP_AHRS *AP_AHRS::_singleton;
2297 :
2298 : namespace AP {
2299 :
2300 2903180248 : AP_AHRS &ahrs()
2301 : {
2302 2903180248 : return *AP_AHRS::get_singleton();
2303 : }
2304 :
2305 : }
2306 :
2307 : #endif // AP_AHRS_ENABLED
|
