Line data Source code
1 : #include "AP_NavEKF3_core.h"
2 :
3 : #include "AP_NavEKF3.h"
4 :
5 : #include <AP_HAL/AP_HAL.h>
6 :
7 : #include <GCS_MAVLink/GCS.h>
8 : #include <AP_Logger/AP_Logger.h>
9 : #include <AP_Vehicle/AP_Vehicle_Type.h>
10 : #include <AP_BoardConfig/AP_BoardConfig.h>
11 :
12 : #include "AP_DAL/AP_DAL.h"
13 :
14 : #include <new>
15 :
16 : /*
17 : parameter defaults for different types of vehicle. The
18 : APM_BUILD_DIRECTORY is taken from the main vehicle directory name
19 : where the code is built.
20 : */
21 : #if APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_Replay)
22 : // copter defaults
23 : #define VELNE_M_NSE_DEFAULT 0.3f
24 : #define VELD_M_NSE_DEFAULT 0.5f
25 : #define POSNE_M_NSE_DEFAULT 0.5f
26 : #define ALT_M_NSE_DEFAULT 2.0f
27 : #define MAG_M_NSE_DEFAULT 0.05f
28 : #define GYRO_P_NSE_DEFAULT 1.5E-02f
29 : #define ACC_P_NSE_DEFAULT 3.5E-01f
30 : #define GBIAS_P_NSE_DEFAULT 1.0E-03f
31 : #define ABIAS_P_NSE_DEFAULT 2.0E-02f
32 : #define MAGB_P_NSE_DEFAULT 1.0E-04f
33 : #define MAGE_P_NSE_DEFAULT 1.0E-03f
34 : #define VEL_I_GATE_DEFAULT 500
35 : #define POS_I_GATE_DEFAULT 500
36 : #define HGT_I_GATE_DEFAULT 500
37 : #define MAG_I_GATE_DEFAULT 300
38 : #define MAG_CAL_DEFAULT 3
39 : #define GLITCH_RADIUS_DEFAULT 25
40 : #define FLOW_MEAS_DELAY 10
41 : #define FLOW_M_NSE_DEFAULT 0.25f
42 : #define FLOW_I_GATE_DEFAULT 300
43 : #define CHECK_SCALER_DEFAULT 100
44 : #define FLOW_USE_DEFAULT 1
45 : #define WIND_P_NSE_DEFAULT 0.2
46 :
47 : #elif APM_BUILD_TYPE(APM_BUILD_Rover)
48 : // rover defaults
49 : #define VELNE_M_NSE_DEFAULT 0.5f
50 : #define VELD_M_NSE_DEFAULT 0.7f
51 : #define POSNE_M_NSE_DEFAULT 0.5f
52 : #define ALT_M_NSE_DEFAULT 2.0f
53 : #define MAG_M_NSE_DEFAULT 0.05f
54 : #define GYRO_P_NSE_DEFAULT 1.5E-02f
55 : #define ACC_P_NSE_DEFAULT 3.5E-01f
56 : #define GBIAS_P_NSE_DEFAULT 1.0E-03f
57 : #define ABIAS_P_NSE_DEFAULT 2.0E-02f
58 : #define MAGB_P_NSE_DEFAULT 1.0E-04f
59 : #define MAGE_P_NSE_DEFAULT 1.0E-03f
60 : #define VEL_I_GATE_DEFAULT 500
61 : #define POS_I_GATE_DEFAULT 500
62 : #define HGT_I_GATE_DEFAULT 500
63 : #define MAG_I_GATE_DEFAULT 300
64 : #define MAG_CAL_DEFAULT 2
65 : #define GLITCH_RADIUS_DEFAULT 25
66 : #define FLOW_MEAS_DELAY 10
67 : #define FLOW_M_NSE_DEFAULT 0.25f
68 : #define FLOW_I_GATE_DEFAULT 300
69 : #define CHECK_SCALER_DEFAULT 100
70 : #define FLOW_USE_DEFAULT 1
71 : #define WIND_P_NSE_DEFAULT 0.1
72 :
73 : #elif APM_BUILD_TYPE(APM_BUILD_ArduPlane)
74 : // plane defaults
75 : #define VELNE_M_NSE_DEFAULT 0.5f
76 : #define VELD_M_NSE_DEFAULT 0.7f
77 : #define POSNE_M_NSE_DEFAULT 0.5f
78 : #define ALT_M_NSE_DEFAULT 3.0f
79 : #define MAG_M_NSE_DEFAULT 0.05f
80 : #define GYRO_P_NSE_DEFAULT 1.5E-02f
81 : #define ACC_P_NSE_DEFAULT 3.5E-01f
82 : #define GBIAS_P_NSE_DEFAULT 1.0E-03f
83 : #define ABIAS_P_NSE_DEFAULT 2.0E-02f
84 : #define MAGB_P_NSE_DEFAULT 1.0E-04f
85 : #define MAGE_P_NSE_DEFAULT 1.0E-03f
86 : #define VEL_I_GATE_DEFAULT 500
87 : #define POS_I_GATE_DEFAULT 500
88 : #define HGT_I_GATE_DEFAULT 500
89 : #define MAG_I_GATE_DEFAULT 300
90 : #define MAG_CAL_DEFAULT 0
91 : #define GLITCH_RADIUS_DEFAULT 25
92 : #define FLOW_MEAS_DELAY 10
93 : #define FLOW_M_NSE_DEFAULT 0.15f
94 : #define FLOW_I_GATE_DEFAULT 500
95 : #define CHECK_SCALER_DEFAULT 150
96 : #define FLOW_USE_DEFAULT 2
97 : #define WIND_P_NSE_DEFAULT 0.1
98 :
99 : #elif APM_BUILD_TYPE(APM_BUILD_ArduSub)
100 : // Sub defaults
101 : #define VELNE_M_NSE_DEFAULT 0.5f
102 : #define VELD_M_NSE_DEFAULT 0.7f
103 : #define POSNE_M_NSE_DEFAULT 0.5f
104 : #define ALT_M_NSE_DEFAULT 0.01f
105 : #define MAG_M_NSE_DEFAULT 0.05f
106 : #define GYRO_P_NSE_DEFAULT 1.5E-02f
107 : #define ACC_P_NSE_DEFAULT 3.5E-01f
108 : #define GBIAS_P_NSE_DEFAULT 1.0E-03f
109 : #define ABIAS_P_NSE_DEFAULT 2.0E-02f
110 : #define MAGB_P_NSE_DEFAULT 1.0E-04f
111 : #define MAGE_P_NSE_DEFAULT 1.0E-03f
112 : #define VEL_I_GATE_DEFAULT 500
113 : #define POS_I_GATE_DEFAULT 500
114 : #define HGT_I_GATE_DEFAULT 500
115 : #define MAG_I_GATE_DEFAULT 300
116 : #define MAG_CAL_DEFAULT 3
117 : #define GLITCH_RADIUS_DEFAULT 25
118 : #define FLOW_MEAS_DELAY 10
119 : #define FLOW_M_NSE_DEFAULT 0.25f
120 : #define FLOW_I_GATE_DEFAULT 300
121 : #define CHECK_SCALER_DEFAULT 100
122 : #define FLOW_USE_DEFAULT 1
123 : #define WIND_P_NSE_DEFAULT 0.1
124 :
125 : #else
126 : // build type not specified, use copter defaults
127 : #define VELNE_M_NSE_DEFAULT 0.5f
128 : #define VELD_M_NSE_DEFAULT 0.7f
129 : #define POSNE_M_NSE_DEFAULT 0.5f
130 : #define ALT_M_NSE_DEFAULT 2.0f
131 : #define MAG_M_NSE_DEFAULT 0.05f
132 : #define GYRO_P_NSE_DEFAULT 1.5E-02f
133 : #define ACC_P_NSE_DEFAULT 3.5E-01f
134 : #define GBIAS_P_NSE_DEFAULT 1.0E-03f
135 : #define ABIAS_P_NSE_DEFAULT 2.0E-02f
136 : #define MAGB_P_NSE_DEFAULT 1.0E-04f
137 : #define MAGE_P_NSE_DEFAULT 1.0E-03f
138 : #define VEL_I_GATE_DEFAULT 500
139 : #define POS_I_GATE_DEFAULT 500
140 : #define HGT_I_GATE_DEFAULT 500
141 : #define MAG_I_GATE_DEFAULT 300
142 : #define MAG_CAL_DEFAULT 3
143 : #define GLITCH_RADIUS_DEFAULT 25
144 : #define FLOW_MEAS_DELAY 10
145 : #define FLOW_M_NSE_DEFAULT 0.25f
146 : #define FLOW_I_GATE_DEFAULT 300
147 : #define CHECK_SCALER_DEFAULT 100
148 : #define FLOW_USE_DEFAULT 1
149 : #define WIND_P_NSE_DEFAULT 0.1
150 :
151 : #endif // APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_Replay)
152 :
153 : #ifndef EK3_PRIMARY_DEFAULT
154 : #define EK3_PRIMARY_DEFAULT 0
155 : #endif
156 :
157 : // This allows boards to default to using a specified number of IMUs and EKF lanes
158 : #ifndef HAL_EKF_IMU_MASK_DEFAULT
159 : #define HAL_EKF_IMU_MASK_DEFAULT 3 // Default to using two IMUs
160 : #endif
161 :
162 : // Define tuning parameters
163 : const AP_Param::GroupInfo NavEKF3::var_info[] = {
164 :
165 : // @Param: ENABLE
166 : // @DisplayName: Enable EKF3
167 : // @Description: This enables EKF3. Enabling EKF3 only makes the maths run, it does not mean it will be used for flight control. To use it for flight control set AHRS_EKF_TYPE=3. A reboot or restart will need to be performed after changing the value of EK3_ENABLE for it to take effect.
168 : // @Values: 0:Disabled, 1:Enabled
169 : // @User: Advanced
170 : // @RebootRequired: True
171 : AP_GROUPINFO_FLAGS("ENABLE", 0, NavEKF3, _enable, 1, AP_PARAM_FLAG_ENABLE),
172 :
173 : // GPS measurement parameters
174 :
175 : // 1 was GPS_TYPE
176 :
177 : // @Param: VELNE_M_NSE
178 : // @DisplayName: GPS horizontal velocity measurement noise (m/s)
179 : // @Description: This sets a lower limit on the speed accuracy reported by the GPS receiver that is used to set horizontal velocity observation noise. If the model of receiver used does not provide a speed accurcy estimate, then the parameter value will be used. Increasing it reduces the weighting of the GPS horizontal velocity measurements.
180 : // @Range: 0.05 5.0
181 : // @Increment: 0.05
182 : // @User: Advanced
183 : // @Units: m/s
184 : AP_GROUPINFO("VELNE_M_NSE", 2, NavEKF3, _gpsHorizVelNoise, VELNE_M_NSE_DEFAULT),
185 :
186 : // @Param: VELD_M_NSE
187 : // @DisplayName: GPS vertical velocity measurement noise (m/s)
188 : // @Description: This sets a lower limit on the speed accuracy reported by the GPS receiver that is used to set vertical velocity observation noise. If the model of receiver used does not provide a speed accurcy estimate, then the parameter value will be used. Increasing it reduces the weighting of the GPS vertical velocity measurements.
189 : // @Range: 0.05 5.0
190 : // @Increment: 0.05
191 : // @User: Advanced
192 : // @Units: m/s
193 : AP_GROUPINFO("VELD_M_NSE", 3, NavEKF3, _gpsVertVelNoise, VELD_M_NSE_DEFAULT),
194 :
195 : // @Param: VEL_I_GATE
196 : // @DisplayName: GPS velocity innovation gate size
197 : // @Description: This sets the percentage number of standard deviations applied to the GPS velocity measurement innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted. If EK3_GLITCH_RAD set to 0 the velocity innovations will be clipped instead of rejected if they exceed the gate size and a smaller value of EK3_VEL_I_GATE not exceeding 300 is recommended to limit the effect of GPS transient errors.
198 : // @Range: 100 1000
199 : // @Increment: 25
200 : // @User: Advanced
201 : AP_GROUPINFO("VEL_I_GATE", 4, NavEKF3, _gpsVelInnovGate, VEL_I_GATE_DEFAULT),
202 :
203 : // @Param: POSNE_M_NSE
204 : // @DisplayName: GPS horizontal position measurement noise (m)
205 : // @Description: This sets the GPS horizontal position observation noise. Increasing it reduces the weighting of GPS horizontal position measurements.
206 : // @Range: 0.1 10.0
207 : // @Increment: 0.1
208 : // @User: Advanced
209 : // @Units: m
210 : AP_GROUPINFO("POSNE_M_NSE", 5, NavEKF3, _gpsHorizPosNoise, POSNE_M_NSE_DEFAULT),
211 :
212 : // @Param: POS_I_GATE
213 : // @DisplayName: GPS position measurement gate size
214 : // @Description: This sets the percentage number of standard deviations applied to the GPS position measurement innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted. If EK3_GLITCH_RAD has been set to 0 the horizontal position innovations will be clipped instead of rejected if they exceed the gate size so a smaller value of EK3_POS_I_GATE not exceeding 300 is recommended to limit the effect of GPS transient errors.
215 : // @Range: 100 1000
216 : // @Increment: 25
217 : // @User: Advanced
218 : AP_GROUPINFO("POS_I_GATE", 6, NavEKF3, _gpsPosInnovGate, POS_I_GATE_DEFAULT),
219 :
220 : // @Param: GLITCH_RAD
221 : // @DisplayName: GPS glitch radius gate size (m)
222 : // @Description: This controls the maximum radial uncertainty in position between the value predicted by the filter and the value measured by the GPS before the filter position and velocity states are reset to the GPS. Making this value larger allows the filter to ignore larger GPS glitches but also means that non-GPS errors such as IMU and compass can create a larger error in position before the filter is forced back to the GPS position. If EK3_GLITCH_RAD set to 0 the GPS innovations will be clipped instead of rejected if they exceed the gate size set by EK3_VEL_I_GATE and EK3_POS_I_GATE which can be useful if poor quality sensor data is causing GPS rejection and loss of navigation but does make the EKF more susceptible to GPS glitches. If setting EK3_GLITCH_RAD to 0 it is recommended to reduce EK3_VEL_I_GATE and EK3_POS_I_GATE to 300.
223 : // @Range: 10 100
224 : // @Increment: 5
225 : // @User: Advanced
226 : // @Units: m
227 : AP_GROUPINFO("GLITCH_RAD", 7, NavEKF3, _gpsGlitchRadiusMax, GLITCH_RADIUS_DEFAULT),
228 :
229 : // 8 previously used for EKF3_GPS_DELAY parameter that has been deprecated.
230 : // The EKF now takes its GPS delay form the GPS library with the default delays
231 : // specified by the GPS_DELAY and GPS_DELAY2 parameters.
232 :
233 : // Height measurement parameters
234 :
235 : // 9 was ALT_SOURCE
236 :
237 : // @Param: ALT_M_NSE
238 : // @DisplayName: Altitude measurement noise (m)
239 : // @Description: This is the RMS value of noise in the altitude measurement. Increasing it reduces the weighting of the baro measurement and will make the filter respond more slowly to baro measurement errors, but will make it more sensitive to GPS and accelerometer errors. A larger value for EK3_ALT_M_NSE may be required when operating with EK3_SRCx_POSZ = 0. This parameter also sets the noise for the 'synthetic' zero height measurement that is used when EK3_SRCx_POSZ = 0.
240 : // @Range: 0.01 100.0
241 : // @Increment: 0.1
242 : // @User: Advanced
243 : // @Units: m
244 : AP_GROUPINFO("ALT_M_NSE", 10, NavEKF3, _baroAltNoise, ALT_M_NSE_DEFAULT),
245 :
246 : // @Param: HGT_I_GATE
247 : // @DisplayName: Height measurement gate size
248 : // @Description: This sets the percentage number of standard deviations applied to the height measurement innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted. If EK3_GLITCH_RAD set to 0 the vertical position innovations will be clipped instead of rejected if they exceed the gate size and a smaller value of EK3_HGT_I_GATE not exceeding 300 is recommended to limit the effect of height sensor transient errors.
249 : // @Range: 100 1000
250 : // @Increment: 25
251 : // @User: Advanced
252 : AP_GROUPINFO("HGT_I_GATE", 11, NavEKF3, _hgtInnovGate, HGT_I_GATE_DEFAULT),
253 :
254 : // @Param: HGT_DELAY
255 : // @DisplayName: Height measurement delay (msec)
256 : // @Description: This is the number of msec that the Height measurements lag behind the inertial measurements.
257 : // @Range: 0 250
258 : // @Increment: 10
259 : // @User: Advanced
260 : // @Units: ms
261 : // @RebootRequired: True
262 : AP_GROUPINFO("HGT_DELAY", 12, NavEKF3, _hgtDelay_ms, 60),
263 :
264 : // Magnetometer measurement parameters
265 :
266 : // @Param: MAG_M_NSE
267 : // @DisplayName: Magnetometer measurement noise (Gauss)
268 : // @Description: This is the RMS value of noise in magnetometer measurements. Increasing it reduces the weighting on these measurements.
269 : // @Range: 0.01 0.5
270 : // @Increment: 0.01
271 : // @User: Advanced
272 : // @Units: Gauss
273 : AP_GROUPINFO("MAG_M_NSE", 13, NavEKF3, _magNoise, MAG_M_NSE_DEFAULT),
274 :
275 : // @Param: MAG_CAL
276 : // @DisplayName: Magnetometer default fusion mode
277 : // @Description: This determines when the filter will use the 3-axis magnetometer fusion model that estimates both earth and body fixed magnetic field states and when it will use a simpler magnetic heading fusion model that does not use magnetic field states. The 3-axis magnetometer fusion is only suitable for use when the external magnetic field environment is stable. EK3_MAG_CAL = 0 uses heading fusion on ground, 3-axis fusion in-flight, and is the default setting for Plane users. EK3_MAG_CAL = 1 uses 3-axis fusion only when manoeuvring. EK3_MAG_CAL = 2 uses heading fusion at all times, is recommended if the external magnetic field is varying and is the default for rovers. EK3_MAG_CAL = 3 uses heading fusion on the ground and 3-axis fusion after the first in-air field and yaw reset has completed, and is the default for copters. EK3_MAG_CAL = 4 uses 3-axis fusion at all times. EK3_MAG_CAL = 7 uses 3-axis fusion on the ground and after the first in-air field and yaw reset has completed. This allows the magnetic field to be learned on the ground before takeoff (useful when swapping batteries with different magnetic signatures) while inhibiting learning during the initial climb when motor magnetic interference is strongest. While disarmed and stationary the yaw is additionally anchored to the measured magnetic heading, since a stationary vehicle gives the 3-axis fusion no way to separate a yaw error from the body field states. A body field component perpendicular to the horizontal earth field is not separable from yaw by a heading measurement, so it appears as a heading offset rather than being learned. NOTE : Use of simple heading magnetometer fusion makes vehicle compass calibration and alignment errors harder for the EKF to detect which reduces the sensitivity of the Copter EKF failsafe algorithm. NOTE: The fusion mode can be forced to 2 for specific EKF cores using the EK3_MAG_MASK parameter. NOTE: limited operation without a magnetometer or any other yaw sensor is possible by setting all COMPASS_USE, COMPASS_USE2, COMPASS_USE3, etc parameters to 0 and setting COMPASS_ENABLE to 0. If this is done, the EK3_GSF_RUN and EK3_GSF_USE masks must be set to the same as EK3_IMU_MASK. A yaw angle derived from IMU and GPS velocity data using a Gaussian Sum Filter (GSF) will then be used to align the yaw when flight commences and there is sufficient movement.
278 : // @Values: 0:When flying,1:When manoeuvring,2:Never,3:After first climb yaw reset,4:Always,5:Use external yaw sensor (Deprecated in 4.1+ see EK3_SRCn_YAW),6:External yaw sensor with compass fallback (Deprecated in 4.1+ see EK3_SRCn_YAW),7:On ground and after first climb yaw reset
279 : // @User: Advanced
280 : // @RebootRequired: True
281 : AP_GROUPINFO("MAG_CAL", 14, NavEKF3, _magCal, MAG_CAL_DEFAULT),
282 :
283 : // @Param: MAG_I_GATE
284 : // @DisplayName: Magnetometer measurement gate size
285 : // @Description: This sets the percentage number of standard deviations applied to the magnetometer measurement innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted.
286 : // @Range: 100 1000
287 : // @Increment: 25
288 : // @User: Advanced
289 : AP_GROUPINFO("MAG_I_GATE", 15, NavEKF3, _magInnovGate, MAG_I_GATE_DEFAULT),
290 :
291 : // Airspeed measurement parameters
292 :
293 : // @Param: EAS_M_NSE
294 : // @DisplayName: Equivalent airspeed measurement noise (m/s)
295 : // @Description: This is the RMS value of noise in equivalent airspeed measurements used by planes. Increasing it reduces the weighting of airspeed measurements and will make wind speed estimates less noisy and slower to converge. Increasing also increases navigation errors when dead-reckoning without GPS measurements.
296 : // @Range: 0.5 5.0
297 : // @Increment: 0.1
298 : // @User: Advanced
299 : // @Units: m/s
300 : AP_GROUPINFO("EAS_M_NSE", 16, NavEKF3, _easNoise, 1.4f),
301 :
302 : // @Param: EAS_I_GATE
303 : // @DisplayName: Airspeed measurement gate size
304 : // @Description: This sets the percentage number of standard deviations applied to the airspeed measurement innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted.
305 : // @Range: 100 1000
306 : // @Increment: 25
307 : // @User: Advanced
308 : AP_GROUPINFO("EAS_I_GATE", 17, NavEKF3, _tasInnovGate, 400),
309 :
310 : // Rangefinder measurement parameters
311 :
312 : // @Param: RNG_M_NSE
313 : // @DisplayName: Range finder measurement noise (m)
314 : // @Description: This is the RMS value of noise in the range finder measurement. Increasing it reduces the weighting on this measurement.
315 : // @Range: 0.1 10.0
316 : // @Increment: 0.1
317 : // @User: Advanced
318 : // @Units: m
319 : AP_GROUPINFO("RNG_M_NSE", 18, NavEKF3, _rngNoise, 0.5f),
320 :
321 : // @Param: RNG_I_GATE
322 : // @DisplayName: Range finder measurement gate size
323 : // @Description: This sets the percentage number of standard deviations applied to the range finder innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted.
324 : // @Range: 100 1000
325 : // @Increment: 25
326 : // @User: Advanced
327 : AP_GROUPINFO("RNG_I_GATE", 19, NavEKF3, _rngInnovGate, 500),
328 :
329 : // Optical flow measurement parameters
330 :
331 : // @Param: FLOW_MAX
332 : // @DisplayName: Optical flow rate maximum
333 : // @Description: The maximum optical flow rate in rad/sec that will be accepted by the filter. Flow rates above this value will not be fused.
334 : // @Range: 1.0 4.0
335 : // @Increment: 0.1
336 : // @User: Advanced
337 : // @Units: rad/s
338 : AP_GROUPINFO("FLOW_MAX", 20, NavEKF3, _maxFlowRate, 2.5f),
339 :
340 : // @Param: FLOW_M_NSE
341 : // @DisplayName: Optical flow measurement noise (rad/s)
342 : // @Description: This is the RMS value of noise and errors in optical flow measurements. Increasing it reduces the weighting on these measurements.
343 : // @Range: 0.05 1.0
344 : // @Increment: 0.05
345 : // @User: Advanced
346 : // @Units: rad/s
347 : AP_GROUPINFO("FLOW_M_NSE", 21, NavEKF3, _flowNoise, FLOW_M_NSE_DEFAULT),
348 :
349 : // @Param: FLOW_I_GATE
350 : // @DisplayName: Optical Flow measurement gate size
351 : // @Description: This sets the percentage number of standard deviations applied to the optical flow innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted.
352 : // @Range: 100 1000
353 : // @Increment: 25
354 : // @User: Advanced
355 : AP_GROUPINFO("FLOW_I_GATE", 22, NavEKF3, _flowInnovGate, FLOW_I_GATE_DEFAULT),
356 :
357 : // @Param: FLOW_DELAY
358 : // @DisplayName: Optical Flow measurement delay (msec)
359 : // @Description: This is the number of msec that the optical flow measurements lag behind the inertial measurements. It is the time from the end of the optical flow averaging period and does not include the time delay due to the 100msec of averaging within the flow sensor.
360 : // @Range: 0 250
361 : // @Increment: 10
362 : // @User: Advanced
363 : // @Units: ms
364 : // @RebootRequired: True
365 : AP_GROUPINFO("FLOW_DELAY", 23, NavEKF3, _flowDelay_ms, FLOW_MEAS_DELAY),
366 :
367 : // State and Covariance Predition Parameters
368 :
369 : // @Param: GYRO_P_NSE
370 : // @DisplayName: Rate gyro noise (rad/s)
371 : // @Description: This control disturbance noise controls the growth of estimated error due to gyro measurement errors excluding bias. Increasing it makes the flter trust the gyro measurements less and other measurements more.
372 : // @Range: 0.0001 0.1
373 : // @Increment: 0.0001
374 : // @User: Advanced
375 : // @Units: rad/s
376 : AP_GROUPINFO("GYRO_P_NSE", 24, NavEKF3, _gyrNoise, GYRO_P_NSE_DEFAULT),
377 :
378 : // @Param: ACC_P_NSE
379 : // @DisplayName: Accelerometer noise (m/s^2)
380 : // @Description: This control disturbance noise controls the growth of estimated error due to accelerometer measurement errors excluding bias. Increasing it makes the flter trust the accelerometer measurements less and other measurements more.
381 : // @Range: 0.01 1.0
382 : // @Increment: 0.01
383 : // @User: Advanced
384 : // @Units: m/s/s
385 : AP_GROUPINFO("ACC_P_NSE", 25, NavEKF3, _accNoise, ACC_P_NSE_DEFAULT),
386 :
387 : // @Param: GBIAS_P_NSE
388 : // @DisplayName: Rate gyro bias stability (rad/s/s)
389 : // @Description: This state process noise controls growth of the gyro delta angle bias state error estimate. Increasing it makes rate gyro bias estimation faster and noisier.
390 : // @Range: 0.00001 0.001
391 : // @User: Advanced
392 : // @Units: rad/s/s
393 : AP_GROUPINFO("GBIAS_P_NSE", 26, NavEKF3, _gyroBiasProcessNoise, GBIAS_P_NSE_DEFAULT),
394 :
395 : // 27 previously used for EK2_GSCL_P_NSE parameter that has been removed
396 :
397 : // @Param: ABIAS_P_NSE
398 : // @DisplayName: Accelerometer bias stability (m/s^3)
399 : // @Description: This noise controls the growth of the vertical accelerometer delta velocity bias state error estimate. Increasing it makes accelerometer bias estimation faster and noisier.
400 : // @Range: 0.00001 0.02
401 : // @User: Advanced
402 : // @Units: m/s/s/s
403 : AP_GROUPINFO("ABIAS_P_NSE", 28, NavEKF3, _accelBiasProcessNoise, ABIAS_P_NSE_DEFAULT),
404 :
405 : // 29 previously used for EK2_MAG_P_NSE parameter that has been replaced with EK3_MAGE_P_NSE and EK3_MAGB_P_NSE
406 :
407 : // @Param: WIND_P_NSE
408 : // @DisplayName: Wind velocity process noise (m/s^2)
409 : // @Description: This state process noise controls the growth of wind state error estimates. Increasing it makes wind estimation faster and noisier.
410 : // @Range: 0.01 2.0
411 : // @Increment: 0.1
412 : // @User: Advanced
413 : // @Units: m/s/s
414 : AP_GROUPINFO("WIND_P_NSE", 30, NavEKF3, _windVelProcessNoise, WIND_P_NSE_DEFAULT),
415 :
416 : // @Param: WIND_PSCALE
417 : // @DisplayName: Height rate to wind process noise scaler
418 : // @Description: This controls how much the process noise on the wind states is increased when gaining or losing altitude to take into account changes in wind speed and direction with altitude. Increasing this parameter increases how rapidly the wind states adapt when changing altitude, but does make wind velocity estimation noiser.
419 : // @Range: 0.0 2.0
420 : // @Increment: 0.1
421 : // @User: Advanced
422 : AP_GROUPINFO("WIND_PSCALE", 31, NavEKF3, _wndVarHgtRateScale, 1.0f),
423 :
424 : // @Param: GPS_CHECK
425 : // @DisplayName: GPS preflight check
426 : // @Description: This is a 1 byte bitmap controlling which GPS preflight checks are performed. Set to 0 to bypass all checks. Set to 255 perform all checks. Set to 3 to check just the number of satellites and HDoP. Set to 31 for the most rigorous checks that will still allow checks to pass when the copter is moving, eg launch from a boat.
427 : // @Bitmask: 0:NSats,1:HDoP,2:speed error,3:position error,4:yaw error,5:pos drift,6:vert speed,7:horiz speed
428 : // @User: Advanced
429 : AP_GROUPINFO("GPS_CHECK", 32, NavEKF3, _gpsCheck, 31),
430 :
431 : // @Param: IMU_MASK
432 : // @DisplayName: Bitmask of active IMUs
433 : // @Description: 1 byte bitmap of IMUs to use in EKF3. A separate instance of EKF3 will be started for each IMU selected. Set to 1 to use the first IMU only (default), set to 2 to use the second IMU only, set to 3 to use the first and second IMU. Additional IMU's can be used up to a maximum of 6 if memory and processing resources permit. There may be insufficient memory and processing resources to run multiple instances. If this occurs EKF3 will fail to start.
434 : // @Bitmask: 0:FirstIMU,1:SecondIMU,2:ThirdIMU,3:FourthIMU,4:FifthIMU,5:SixthIMU
435 : // @User: Advanced
436 : // @RebootRequired: True
437 : AP_GROUPINFO("IMU_MASK", 33, NavEKF3, _imuMask, HAL_EKF_IMU_MASK_DEFAULT),
438 :
439 : // @Param: CHECK_SCALE
440 : // @DisplayName: GPS accuracy check scaler (%)
441 : // @Description: This scales the thresholds that are used to check GPS accuracy before it is used by the EKF. A value of 100 is the default. Values greater than 100 increase and values less than 100 reduce the maximum GPS error the EKF will accept. A value of 200 will double the allowable GPS error.
442 : // @Range: 50 200
443 : // @User: Advanced
444 : // @Units: %
445 : AP_GROUPINFO("CHECK_SCALE", 34, NavEKF3, _gpsCheckScaler, CHECK_SCALER_DEFAULT),
446 :
447 : // @Param: NOAID_M_NSE
448 : // @DisplayName: Non-GPS operation position uncertainty (m)
449 : // @Description: This sets the amount of position variation that the EKF allows for when operating without external measurements (eg GPS or optical flow). Increasing this parameter makes the EKF attitude estimate less sensitive to vehicle manoeuvres but more sensitive to IMU errors.
450 : // @Range: 0.5 50.0
451 : // @User: Advanced
452 : // @Units: m
453 : AP_GROUPINFO("NOAID_M_NSE", 35, NavEKF3, _noaidHorizNoise, 10.0f),
454 :
455 : // @Param: BETA_MASK
456 : // @DisplayName: Bitmask controlling sidelip angle fusion
457 : // @Description: 1 byte bitmap controlling use of sideslip angle fusion for estimation of non wind states during operation of 'fly forward' vehicle types such as fixed wing planes. By assuming that the angle of sideslip is small, the wind velocity state estimates are corrected whenever the EKF is not dead reckoning (e.g. has an independent velocity or position sensor such as GPS). This behaviour is on by default and cannot be disabled. When the EKF is dead reckoning, the wind states are used as a reference, enabling use of the small angle of sideslip assumption to correct non wind velocity states (eg attitude, velocity, position, etc) and improve navigation accuracy. This behaviour is on by default and cannot be disabled. The behaviour controlled by this parameter is the use of the small angle of sideslip assumption to correct non wind velocity states when the EKF is NOT dead reckoning. This is primarily of benefit to reduce the buildup of yaw angle errors during straight and level flight without a yaw sensor (e.g. magnetometer or dual antenna GPS yaw) provided aerobatic flight maneuvers with large sideslip angles are not performed. The 'always' option might be used where the yaw sensor is intentionally not fitted or disabled. The 'WhenNoYawSensor' option might be used if a yaw sensor is fitted, but protection against in-flight failure and continual rejection by the EKF is desired. For vehicles operated within visual range of the operator performing frequent turning maneuvers, setting this parameter is unnecessary.
458 : // @Bitmask: 0:Always,1:WhenNoYawSensor
459 : // @User: Advanced
460 : // @RebootRequired: True
461 : AP_GROUPINFO("BETA_MASK", 36, NavEKF3, _betaMask, 0),
462 :
463 : // control of magnetic yaw angle fusion
464 :
465 : // @Param: YAW_M_NSE
466 : // @DisplayName: Yaw measurement noise (rad)
467 : // @Description: This is the RMS value of noise in yaw measurements from the magnetometer. Increasing it reduces the weighting on these measurements.
468 : // @Range: 0.05 1.0
469 : // @Increment: 0.05
470 : // @User: Advanced
471 : // @Units: rad
472 : AP_GROUPINFO("YAW_M_NSE", 37, NavEKF3, _yawNoise, 0.5f),
473 :
474 : // @Param: YAW_I_GATE
475 : // @DisplayName: Yaw measurement gate size
476 : // @Description: This sets the percentage number of standard deviations applied to the magnetometer yaw measurement innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted.
477 : // @Range: 100 1000
478 : // @Increment: 25
479 : // @User: Advanced
480 : AP_GROUPINFO("YAW_I_GATE", 38, NavEKF3, _yawInnovGate, 300),
481 :
482 : // @Param: TAU_OUTPUT
483 : // @DisplayName: Output complementary filter time constant (centi-sec)
484 : // @Description: Sets the time constant of the output complementary filter/predictor in centi-seconds.
485 : // @Range: 10 50
486 : // @Increment: 5
487 : // @User: Advanced
488 : // @Units: cs
489 : AP_GROUPINFO("TAU_OUTPUT", 39, NavEKF3, _tauVelPosOutput, 25),
490 :
491 : // @Param: MAGE_P_NSE
492 : // @DisplayName: Earth magnetic field process noise (gauss/s)
493 : // @Description: This state process noise controls the growth of earth magnetic field state error estimates. Increasing it makes earth magnetic field estimation faster and noisier.
494 : // @Range: 0.00001 0.01
495 : // @User: Advanced
496 : // @Units: Gauss/s
497 : AP_GROUPINFO("MAGE_P_NSE", 40, NavEKF3, _magEarthProcessNoise, MAGE_P_NSE_DEFAULT),
498 :
499 : // @Param: MAGB_P_NSE
500 : // @DisplayName: Body magnetic field process noise (gauss/s)
501 : // @Description: This state process noise controls the growth of body magnetic field state error estimates. Increasing it makes magnetometer bias error estimation faster and noisier.
502 : // @Range: 0.00001 0.01
503 : // @User: Advanced
504 : // @Units: Gauss/s
505 : AP_GROUPINFO("MAGB_P_NSE", 41, NavEKF3, _magBodyProcessNoise, MAGB_P_NSE_DEFAULT),
506 :
507 : // @Param: RNG_USE_HGT
508 : // @DisplayName: Range finder switch height percentage
509 : // @Description: Range finder can be used as the primary height source when below this percentage of its maximum range (see RNGFNDx_MAX) and the primary height source is Baro or GPS (see EK3_SRCx_POSZ). This feature should not be used for terrain following as it is designed for vertical takeoff and landing with climb above the range finder use height before commencing the mission, and with horizontal position changes below that height being limited to a flat region around the takeoff and landing point.
510 : // @Range: -1 70
511 : // @Increment: 1
512 : // @User: Advanced
513 : // @Units: %
514 : AP_GROUPINFO("RNG_USE_HGT", 42, NavEKF3, _useRngSwHgt, -1),
515 :
516 : // @Param: TERR_GRAD
517 : // @DisplayName: Maximum terrain gradient
518 : // @Description: Specifies the maximum gradient of the terrain below the vehicle when it is using range finder as a height reference
519 : // @Range: 0 0.2
520 : // @Increment: 0.01
521 : // @User: Advanced
522 : AP_GROUPINFO("TERR_GRAD", 43, NavEKF3, _terrGradMax, 0.1f),
523 :
524 : // @Param: BCN_M_NSE
525 : // @DisplayName: Range beacon measurement noise (m)
526 : // @Description: This is the RMS value of noise in the range beacon measurement. Increasing it reduces the weighting on this measurement.
527 : // @Range: 0.1 10.0
528 : // @Increment: 0.1
529 : // @User: Advanced
530 : // @Units: m
531 : AP_GROUPINFO("BCN_M_NSE", 44, NavEKF3, _rngBcnNoise, 1.0f),
532 :
533 : // @Param: BCN_I_GTE
534 : // @DisplayName: Range beacon measurement gate size
535 : // @Description: This sets the percentage number of standard deviations applied to the range beacon measurement innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted.
536 : // @Range: 100 1000
537 : // @Increment: 25
538 : // @User: Advanced
539 : AP_GROUPINFO("BCN_I_GTE", 45, NavEKF3, _rngBcnInnovGate, 500),
540 :
541 : // @Param: BCN_DELAY
542 : // @DisplayName: Range beacon measurement delay (msec)
543 : // @Description: This is the number of msec that the range beacon measurements lag behind the inertial measurements.
544 : // @Range: 0 250
545 : // @Increment: 10
546 : // @User: Advanced
547 : // @Units: ms
548 : // @RebootRequired: True
549 : AP_GROUPINFO("BCN_DELAY", 46, NavEKF3, _rngBcnDelay_ms, 50),
550 :
551 : // @Param: RNG_USE_SPD
552 : // @DisplayName: Range finder max ground speed
553 : // @Description: The range finder will not be used as the primary height source when the horizontal ground speed is greater than this value.
554 : // @Range: 2.0 6.0
555 : // @Increment: 0.5
556 : // @User: Advanced
557 : // @Units: m/s
558 : AP_GROUPINFO("RNG_USE_SPD", 47, NavEKF3, _useRngSwSpd, 2.0f),
559 :
560 : // @Param: ACC_BIAS_LIM
561 : // @DisplayName: Accelerometer bias limit
562 : // @Description: The accelerometer bias state will be limited to +- this value
563 : // @Range: 0.5 2.5
564 : // @Increment: 0.1
565 : // @User: Advanced
566 : // @Units: m/s/s
567 : AP_GROUPINFO("ACC_BIAS_LIM", 48, NavEKF3, _accBiasLim, 1.0f),
568 :
569 : // @Param: MAG_MASK
570 : // @DisplayName: Bitmask of active EKF cores that will always use heading fusion
571 : // @Description: 1 byte bitmap of EKF cores that will disable magnetic field states and use simple magnetic heading fusion at all times. This parameter enables specified cores to be used as a backup for flight into an environment with high levels of external magnetic interference which may degrade the EKF attitude estimate when using 3-axis magnetometer fusion. NOTE : Use of a different magnetometer fusion algorithm on different cores makes unwanted EKF core switches due to magnetometer errors more likely.
572 : // @Bitmask: 0:FirstEKF,1:SecondEKF,2:ThirdEKF,3:FourthEKF,4:FifthEKF,5:SixthEKF
573 : // @User: Advanced
574 : // @RebootRequired: True
575 : AP_GROUPINFO("MAG_MASK", 49, NavEKF3, _magMask, 0),
576 :
577 : // @Param: OGN_HGT_MASK
578 : // @DisplayName: Bitmask control of EKF reference height correction
579 : // @Description: When a height sensor other than GPS is used as the primary height source by the EKF, the position of the zero height datum is defined by that sensor and its frame of reference. If a GPS height measurement is also available, then the height of the WGS-84 height datum used by the EKF can be corrected so that the height returned by the getLLH() function is compensated for primary height sensor drift and change in datum over time. The first two bit positions control when the height datum will be corrected. Correction is performed using a Bayes filter and only operates when GPS quality permits. The third bit position controls where the corrections to the GPS reference datum are applied. Corrections can be applied to the local vertical position or to the reported EKF origin height (default).
580 : // @Bitmask: 0:Correct when using Baro height,1:Correct when using range finder height,2:Apply corrections to local position
581 : // @User: Advanced
582 : // @RebootRequired: True
583 : AP_GROUPINFO("OGN_HGT_MASK", 50, NavEKF3, _originHgtMode, 0),
584 :
585 : // @Param: VIS_VERR_MIN
586 : // @DisplayName: Visual odometry minimum velocity error
587 : // @Description: This is the 1-STD odometry velocity observation error that will be assumed when maximum quality is reported by the sensor. When quality is between max and min, the error will be calculated using linear interpolation between VIS_VERR_MIN and VIS_VERR_MAX.
588 : // @Range: 0.05 0.5
589 : // @Increment: 0.05
590 : // @User: Advanced
591 : // @Units: m/s
592 : AP_GROUPINFO("VIS_VERR_MIN", 51, NavEKF3, _visOdmVelErrMin, 0.1f),
593 :
594 : // @Param: VIS_VERR_MAX
595 : // @DisplayName: Visual odometry maximum velocity error
596 : // @Description: This is the 1-STD odometry velocity observation error that will be assumed when minimum quality is reported by the sensor. When quality is between max and min, the error will be calculated using linear interpolation between VIS_VERR_MIN and VIS_VERR_MAX.
597 : // @Range: 0.5 5.0
598 : // @Increment: 0.1
599 : // @User: Advanced
600 : // @Units: m/s
601 : AP_GROUPINFO("VIS_VERR_MAX", 52, NavEKF3, _visOdmVelErrMax, 0.9f),
602 :
603 : // @Param: WENC_VERR
604 : // @DisplayName: Wheel odometry velocity error
605 : // @Description: This is the 1-STD odometry velocity observation error that will be assumed when wheel encoder data is being fused.
606 : // @Range: 0.01 1.0
607 : // @Increment: 0.1
608 : // @User: Advanced
609 : // @Units: m/s
610 : AP_GROUPINFO("WENC_VERR", 53, NavEKF3, _wencOdmVelErr, 0.1f),
611 :
612 : // @Param: FLOW_USE
613 : // @DisplayName: Optical flow use bitmask
614 : // @Description: Controls if the optical flow data is fused into the 24-state navigation estimator OR the 1-state terrain height estimator.
615 : // @User: Advanced
616 : // @Values: 0:None,1:Navigation,2:Terrain
617 : // @RebootRequired: True
618 : AP_GROUPINFO("FLOW_USE", 54, NavEKF3, _flowUse, FLOW_USE_DEFAULT),
619 :
620 : // @Param: HRT_FILT
621 : // @DisplayName: Height rate filter crossover frequency
622 : // @Description: Specifies the crossover frequency of the complementary filter used to calculate the output predictor height rate derivative.
623 : // @Range: 0.1 30.0
624 : // @Units: Hz
625 : AP_GROUPINFO("HRT_FILT", 55, NavEKF3, _hrt_filt_freq, 2.0f),
626 :
627 : // @Param: MAG_EF_LIM
628 : // @DisplayName: EarthField error limit
629 : // @Description: This limits the difference between the learned earth magnetic field and the earth field from the world magnetic model tables. A value of zero means to disable the use of the WMM tables.
630 : // @User: Advanced
631 : // @Range: 0 500
632 : // @Units: mGauss
633 : AP_GROUPINFO("MAG_EF_LIM", 56, NavEKF3, _mag_ef_limit, 50),
634 :
635 : // @Param: GSF_RUN_MASK
636 : // @DisplayName: Bitmask of which EKF-GSF yaw estimators run
637 : // @Description: 1 byte bitmap of which EKF3 instances run an independent EKF-GSF yaw estimator to provide a backup yaw estimate that doesn't rely on magnetometer data. This estimator uses IMU, GPS and, if available, airspeed data. EKF-GSF yaw estimator data for the primary EKF3 instance will be logged as GSF0 and GSF1 messages. Use of the yaw estimate generated by this algorithm is controlled by the EK3_GSF_USE_MASK and EK3_GSF_RST_MAX parameters. To run the EKF-GSF yaw estimator in ride-along and logging only, set EK3_GSF_USE to 0.
638 : // @Bitmask: 0:FirstEKF,1:SecondEKF,2:ThirdEKF,3:FourthEKF,4:FifthEKF,5:SixthEKF
639 : // @User: Advanced
640 : // @RebootRequired: True
641 : AP_GROUPINFO("GSF_RUN_MASK", 57, NavEKF3, _gsfRunMask, 3),
642 :
643 : // @Param: GSF_USE_MASK
644 : // @DisplayName: Bitmask of which EKF-GSF yaw estimators are used
645 : // @Description: A bitmask of which EKF3 instances will use the output from the EKF-GSF yaw estimator that has been turned on by the EK3_GSF_RUN_MASK parameter. If the inertial navigation calculation stops following the GPS, then the vehicle code can request EKF3 to attempt to resolve the issue, either by performing a yaw reset if enabled by this parameter by switching to another EKF3 instance.
646 : // @Bitmask: 0:FirstEKF,1:SecondEKF,2:ThirdEKF,3:FourthEKF,4:FifthEKF,5:SixthEKF
647 : // @User: Advanced
648 : // @RebootRequired: True
649 : AP_GROUPINFO("GSF_USE_MASK", 58, NavEKF3, _gsfUseMask, 3),
650 :
651 : // 59 was GSF_DELAY which was never released in a stable version
652 :
653 : // @Param: GSF_RST_MAX
654 : // @DisplayName: Maximum number of resets to the EKF-GSF yaw estimate allowed
655 : // @Description: Sets the maximum number of times the EKF3 will be allowed to reset its yaw to the estimate from the EKF-GSF yaw estimator. No resets will be allowed unless the use of the EKF-GSF yaw estimate is enabled via the EK3_GSF_USE_MASK parameter.
656 : // @Range: 1 10
657 : // @Increment: 1
658 : // @User: Advanced
659 : // @RebootRequired: True
660 : AP_GROUPINFO("GSF_RST_MAX", 60, NavEKF3, _gsfResetMaxCount, 2),
661 :
662 : // @Param: ERR_THRESH
663 : // @DisplayName: EKF3 Lane Relative Error Sensitivity Threshold
664 : // @Description: lanes have to be consistently better than the primary by at least this threshold to reduce their overall relativeCoreError, lowering this makes lane switching more sensitive to smaller error differences
665 : // @Range: 0.05 1
666 : // @Increment: 0.05
667 : // @User: Advanced
668 : AP_GROUPINFO("ERR_THRESH", 61, NavEKF3, _err_thresh, 0.2),
669 :
670 : // @Param: AFFINITY
671 : // @DisplayName: EKF3 Sensor Affinity Options
672 : // @Description: These options control the affinity between sensor instances and EKF cores
673 : // @User: Advanced
674 : // @Bitmask: 0:EnableGPSAffinity,1:EnableBaroAffinity,2:EnableCompassAffinity,3:EnableAirspeedAffinity
675 : // @RebootRequired: True
676 :
677 : AP_GROUPINFO("AFFINITY", 62, NavEKF3, _affinity, 0),
678 :
679 : AP_SUBGROUPEXTENSION("", 63, NavEKF3, var_info2),
680 :
681 : AP_GROUPEND
682 : };
683 :
684 : // second table of parameters. allows us to go beyond the 64 parameter limit
685 : const AP_Param::GroupInfo NavEKF3::var_info2[] = {
686 :
687 : // @Group: SRC
688 : // @Path: ../AP_NavEKF/AP_NavEKF_Source.cpp
689 : AP_SUBGROUPINFO(sources, "SRC", 1, NavEKF3, AP_NavEKF_Source),
690 :
691 : // @Param: DRAG_BCOEF_X
692 : // @DisplayName: Ballistic coefficient for X axis drag
693 : // @Description: Ratio of mass to drag coefficient measured along the X body axis. This parameter enables estimation of wind drift for vehicles with bluff bodies and without propulsion forces in the X and Y direction (eg multicopters). The drag produced by this effect scales with speed squared. Set to a positive value > 1.0 to enable. A starting value is the mass in Kg divided by the frontal area. The predicted drag from the rotors is specified separately by the EK3_DRAG_MCOEF parameter.
694 : // @Range: 0.0 1000.0
695 : // @Units: kg/m/m
696 : // @User: Advanced
697 : AP_GROUPINFO("DRAG_BCOEF_X", 2, NavEKF3, _ballisticCoef_x, 0.0f),
698 :
699 : // @Param: DRAG_BCOEF_Y
700 : // @DisplayName: Ballistic coefficient for Y axis drag
701 : // @Description: Ratio of mass to drag coefficient measured along the Y body axis. This parameter enables estimation of wind drift for vehicles with bluff bodies and without propulsion forces in the X and Y direction (eg multicopters). The drag produced by this effect scales with speed squared. Set to a positive value > 1.0 to enable. A starting value is the mass in Kg divided by the side area. The predicted drag from the rotors is specified separately by the EK3_DRAG_MCOEF parameter.
702 : // @Range: 50.0 1000.0
703 : // @Units: kg/m/m
704 : // @User: Advanced
705 : AP_GROUPINFO("DRAG_BCOEF_Y", 3, NavEKF3, _ballisticCoef_y, 0.0f),
706 :
707 : // @Param: DRAG_M_NSE
708 : // @DisplayName: Observation noise for drag acceleration
709 : // @Description: This sets the amount of noise used when fusing X and Y acceleration as an observation that enables estimation of wind velocity for multi-rotor vehicles. This feature is enabled by the EK3_DRAG_BCOEF_X and EK3_DRAG_BCOEF_Y parameters
710 : // @Range: 0.1 2.0
711 : // @Increment: 0.1
712 : // @User: Advanced
713 : // @Units: m/s/s
714 : AP_GROUPINFO("DRAG_M_NSE", 4, NavEKF3, _dragObsNoise, 0.5f),
715 :
716 : // @Param: DRAG_MCOEF
717 : // @DisplayName: Momentum coefficient for propeller drag
718 : // @Description: This parameter is used to predict the drag produced by the rotors when flying a multi-copter, enabling estimation of wind drift. The drag produced by this effect scales with speed not speed squared and is produced because some of the air velocity normal to the rotors axis of rotation is lost when passing through the rotor disc which changes the momentum of the airflow causing drag. For unducted rotors the effect is roughly proportional to the area of the propeller blades when viewed side on and changes with different propellers. It is higher for ducted rotors. For example if flying at 15 m/s at sea level conditions produces a rotor induced drag acceleration of 1.5 m/s/s, then EK3_DRAG_MCOEF would be set to 0.1 = (1.5/15.0). Set EK3_MCOEF to a positive value to enable wind estimation using this drag effect. To account for the drag produced by the body which scales with speed squared, see documentation for the EK3_DRAG_BCOEF_X and EK3_DRAG_BCOEF_Y parameters.
719 : // @Range: 0.0 1.0
720 : // @Increment: 0.01
721 : // @Units: 1/s
722 : // @User: Advanced
723 : AP_GROUPINFO("DRAG_MCOEF", 5, NavEKF3, _momentumDragCoef, 0.0f),
724 :
725 : // @Param: OGNM_TEST_SF
726 : // @DisplayName: On ground not moving test scale factor
727 : // @Description: This parameter is adjust the sensitivity of the on ground not moving test which is used to assist with learning the yaw gyro bias and stopping yaw drift before flight when operating without a yaw sensor. Bigger values allow the detection of a not moving condition with noiser IMU data. Check the XKFM data logged when the vehicle is on ground not moving and adjust the value of OGNM_TEST_SF to be slightly higher than the maximum value of the XKFM.ADR, XKFM.ALR, XKFM.GDR and XKFM.GLR test levels.
728 : // @Range: 1.0 10.0
729 : // @Increment: 0.5
730 : // @User: Advanced
731 : AP_GROUPINFO("OGNM_TEST_SF", 6, NavEKF3, _ognmTestScaleFactor, 2.0f),
732 :
733 : // @Param: GND_EFF_DZ
734 : // @DisplayName: Baro height ground effect dead zone
735 : // @Description: This parameter sets the size of the dead zone that is applied to negative baro height spikes that can occur when taking off or landing when a vehicle with lift rotors is operating in ground effect ground effect. Set to about 0.5m less than the amount of negative offset in baro height that occurs just prior to takeoff when lift motors are spooling up. Set to 0 if no ground effect is present.
736 : // @Range: 0.0 10.0
737 : // @Increment: 0.5
738 : // @User: Advanced
739 : AP_GROUPINFO("GND_EFF_DZ", 7, NavEKF3, _baroGndEffectDeadZone, 4.0f),
740 :
741 : // @Param: PRIMARY
742 : // @DisplayName: Primary core number
743 : // @Description: The core number (index in IMU mask) that will be used as the primary EKF core on startup. While disarmed the EKF will force the use of this core. A value of 0 corresponds to the first IMU in EK3_IMU_MASK.
744 : // @Range: 0 2
745 : // @Increment: 1
746 : // @User: Advanced
747 : AP_GROUPINFO("PRIMARY", 8, NavEKF3, _primary_core, EK3_PRIMARY_DEFAULT),
748 :
749 : // @Param: LOG_LEVEL
750 : // @DisplayName: Logging Level
751 : // @Description: Determines how verbose the EKF3 streaming logging is. A value of 0 provides full logging(default), a value of 1 only XKF4 scaled innovations are logged, a value of 2 both XKF4 and GSF are logged, and a value of 3 disables all streaming logging of EKF3.
752 : // @Range: 0 3
753 : // @Increment: 1
754 : // @User: Advanced
755 : AP_GROUPINFO("LOG_LEVEL", 9, NavEKF3, _log_level, 0),
756 :
757 : // @Param: GPS_VACC_MAX
758 : // @DisplayName: GPS vertical accuracy threshold
759 : // @Description: Vertical accuracy threshold for GPS as the altitude source. The GPS will not be used as an altitude source if the reported vertical accuracy of the GPS is larger than this threshold, falling back to baro instead. Set to zero to deactivate the threshold check.
760 : // @Range: 0.0 10.0
761 : // @Increment: 0.1
762 : // @User: Advanced
763 : // @Units: m
764 : AP_GROUPINFO("GPS_VACC_MAX", 10, NavEKF3, _gpsVAccThreshold, 0.0f),
765 :
766 : // @Param: OPTIONS
767 : // @DisplayName: Optional EKF behaviour
768 : // @Description: EKF optional behaviour. Bit 0 (JammingExpected): Setting JammingExpected will change the EKF behaviour such that if dead reckoning navigation is possible it will require the preflight alignment GPS quality checks controlled by EK3_GPS_CHECK and EK3_CHECK_SCALE to pass before resuming GPS use if GPS lock is lost for more than 2 seconds to prevent bad position estimate. Bit 1 (Manual lane switching): DANGEROUS – If enabled, this disables automatic lane switching. If the active lane becomes unhealthy, no automatic switching will occur. Users must manually set EK3_PRIMARY to change lanes. No health checks will be performed on the selected lane. Use with extreme caution. Bit 2 (Optflow may use terrain alt): Terrain SRTM data will be used if the vehicle climbs above the rangefinder's range allowing optical flow to be used at higher altitudes. Bit 3 (AGL KF for optflow scaling): Use a 2-state IMU-aided AGL Kalman filter (height + vertical velocity, fused with rangefinder) to compute the height-above-ground used for optical flow velocity scaling, instead of terrainState-pd. This decouples optical flow scaling from errors in the main filter's vertical position state.
769 : // @Bitmask: 0:JammingExpected, 1:ManualLaneSwitching, 2:Optflow may use terrain alt, 3:AGL KF for optflow scaling
770 : // @User: Advanced
771 : AP_GROUPINFO("OPTIONS", 11, NavEKF3, _options, 0),
772 :
773 : AP_GROUPEND
774 : };
775 :
776 2193 : NavEKF3::NavEKF3() :
777 2193 : dal{AP::dal()}
778 : {
779 2193 : AP_Param::setup_object_defaults(this, var_info);
780 2193 : AP_Param::setup_object_defaults(this, var_info2);
781 2193 : }
782 :
783 :
784 : // Initialise the filter
785 1859466 : bool NavEKF3::InitialiseFilter(void)
786 : {
787 1859466 : if (_enable == 0 || _imuMask == 0) {
788 1103649 : return false;
789 : }
790 755817 : const auto &ins = dal.ins();
791 :
792 755817 : dal.start_frame(AP_DAL::FrameType::InitialiseFilterEKF3);
793 :
794 755817 : imuSampleTime_us = dal.micros64();
795 :
796 : // remember expected frame time
797 755817 : const float loop_rate = ins.get_loop_rate_hz();
798 755817 : if (!is_positive(loop_rate)) {
799 : return false;
800 : }
801 755817 : _frameTimeUsec = 1e6 / loop_rate;
802 :
803 : // expected number of IMU frames per prediction
804 755817 : _framesPerPrediction = uint8_t((EKF_TARGET_DT / (_frameTimeUsec * 1.0e-6) + 0.5));
805 :
806 : #if APM_BUILD_TYPE(APM_BUILD_Replay)
807 : if (ins.get_accel_count() == 0) {
808 : return false;
809 : }
810 : #endif
811 :
812 755817 : if (core == nullptr) {
813 :
814 : // don't run multiple filters for 1 IMU
815 1995 : uint8_t mask = (1U<<ins.get_accel_count())-1;
816 1995 : _imuMask.set_and_default(_imuMask.get() & mask);
817 :
818 : // initialise the setup variables
819 7980 : for (uint8_t i=0; i<MAX_EKF_CORES; i++) {
820 5985 : coreSetupRequired[i] = false;
821 5985 : coreImuIndex[i] = 0;
822 : }
823 1995 : num_cores = 0;
824 :
825 : // count IMUs from mask
826 7980 : for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
827 5985 : if (_imuMask & (1U<<i)) {
828 3989 : coreSetupRequired[num_cores] = true;
829 3989 : coreImuIndex[num_cores] = i;
830 3989 : num_cores++;
831 : }
832 : }
833 :
834 : // check if there is enough memory to create the EKF cores
835 1995 : if (dal.available_memory() < sizeof(NavEKF3_core)*num_cores + 4096) {
836 0 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "EKF3 not enough memory");
837 0 : _enable.set(0);
838 0 : num_cores = 0;
839 0 : return false;
840 : }
841 :
842 : //try to allocate from CCM RAM, fallback to Normal RAM if not available or full
843 1995 : core = (NavEKF3_core*)dal.malloc_type(sizeof(NavEKF3_core)*num_cores, AP_DAL::MemoryType::FAST);
844 1995 : if (core == nullptr) {
845 0 : _enable.set(0);
846 0 : num_cores = 0;
847 0 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "EKF3 allocation failed");
848 0 : return false;
849 : }
850 :
851 : // Call constructors on all cores
852 5984 : for (uint8_t i = 0; i < num_cores; i++) {
853 3989 : new (&core[i]) NavEKF3_core(this, dal);
854 : }
855 : }
856 :
857 : // Set up any cores that have been created
858 : // This specifies the IMU to be used and creates the data buffers
859 : // If we are unable to set up a core, return false and try again next time the function is called
860 : bool core_setup_success = true;
861 2267085 : for (uint8_t core_index=0; core_index<num_cores; core_index++) {
862 1511268 : if (coreSetupRequired[core_index]) {
863 48273 : coreSetupRequired[core_index] = !core[core_index].setup_core(coreImuIndex[core_index], core_index);
864 48273 : if (coreSetupRequired[core_index]) {
865 : core_setup_success = false;
866 : }
867 : }
868 : }
869 : // exit with failure if any cores could not be setup
870 755817 : if (!core_setup_success) {
871 : return false;
872 : }
873 :
874 : // set relative error scores for all cores to 0
875 733658 : resetCoreErrors();
876 :
877 : // Set the primary initially to be users selected primary
878 733658 : primary = uint8_t(_primary_core) < num_cores? _primary_core : 0;
879 :
880 : // invalidate shared origin
881 733658 : common_origin_valid = false;
882 :
883 : // initialise the cores. We return success only if all cores
884 : // initialise successfully
885 : bool ret = true;
886 2200608 : for (uint8_t i=0; i<num_cores; i++) {
887 1466950 : ret &= core[i].InitialiseFilterBootstrap();
888 : }
889 :
890 : // set last time the cores were primary to 0
891 733658 : memset(coreLastTimePrimary_us, 0, sizeof(coreLastTimePrimary_us));
892 :
893 : // zero the structs used capture reset events
894 733658 : memset(&yaw_reset_data, 0, sizeof(yaw_reset_data));
895 733658 : memset((void *)&pos_reset_data, 0, sizeof(pos_reset_data));
896 733658 : memset(&pos_down_reset_data, 0, sizeof(pos_down_reset_data));
897 :
898 733658 : return ret;
899 : }
900 :
901 : /*
902 : return true if a new core index has a better score than the current
903 : core
904 : */
905 26541991 : bool NavEKF3::coreBetterScore(uint8_t new_core, uint8_t current_core) const
906 : {
907 26541991 : const NavEKF3_core &oldCore = core[current_core];
908 26541991 : const NavEKF3_core &newCore = core[new_core];
909 26541991 : if (!newCore.healthy()) {
910 : // never consider a new core that isn't healthy
911 : return false;
912 : }
913 26541991 : if (newCore.have_aligned_tilt() != oldCore.have_aligned_tilt()) {
914 : // tilt alignment is most critical, if one is tilt aligned and
915 : // the other isn't then use the tilt aligned lane
916 : return newCore.have_aligned_tilt();
917 : }
918 26541991 : if (newCore.have_aligned_yaw() != oldCore.have_aligned_yaw()) {
919 : // yaw alignment is next most critical, if one is yaw aligned
920 : // and the other isn't then use the yaw aligned lane
921 : return newCore.have_aligned_yaw();
922 : }
923 : // if both cores are aligned then look at relative error scores
924 26541986 : return coreRelativeErrors[new_core] < coreRelativeErrors[current_core];
925 : }
926 :
927 : /*
928 : Update Filter States - this should be called whenever new IMU data is available
929 : Execution speed governed by SCHED_LOOP_RATE
930 : */
931 39298372 : void NavEKF3::UpdateFilter(void)
932 : {
933 39298372 : dal.start_frame(AP_DAL::FrameType::UpdateFilterEKF3);
934 :
935 39298372 : if (!core) {
936 : return;
937 : }
938 :
939 39298372 : imuSampleTime_us = dal.micros64();
940 :
941 117616322 : for (uint8_t i=0; i<num_cores; i++) {
942 : // if we have not overrun by more than 3 IMU frames, and we
943 : // have already used more than 1/3 of the CPU budget for this
944 : // loop then suppress the prediction step. This allows
945 : // multiple EKF instances to cooperate on scheduling
946 : bool allow_state_prediction = true;
947 156635900 : if (core[i].getFramesSincePredict() < (_framesPerPrediction+3) &&
948 78317950 : dal.ekf_low_time_remaining(AP_DAL::EKFType::EKF3, i)) {
949 : allow_state_prediction = false;
950 : }
951 78317950 : core[i].UpdateFilter(allow_state_prediction);
952 : }
953 :
954 : // If the current core selected has a bad error score or is unhealthy, switch to a healthy core with the lowest fault score
955 : // Don't start running the check until the primary core has started returned healthy for at least 10 seconds to avoid switching
956 : // due to initial alignment fluctuations and race conditions
957 39298372 : if (!runCoreSelection) {
958 : static uint64_t lastUnhealthyTime_us = 0;
959 3192867 : if (!core[primary].healthy() || lastUnhealthyTime_us == 0) {
960 421928 : lastUnhealthyTime_us = imuSampleTime_us;
961 : }
962 3192867 : runCoreSelection = (imuSampleTime_us - lastUnhealthyTime_us) > 1E7;
963 : }
964 :
965 39298372 : const uint8_t user_primary = uint8_t(_primary_core) < num_cores? _primary_core : 0;
966 : bool lane_switching_enabled = true;
967 39298372 : if (option_is_enabled(Option::ManualLaneSwitch)) {
968 : lane_switching_enabled = false;
969 62726 : if (primary != user_primary) {
970 : // switch back to the user-selected core
971 1 : switchLane(user_primary);
972 : }
973 : }
974 :
975 39298372 : const bool armed = dal.get_armed();
976 :
977 : // core selection is only available after the vehicle is armed, else forced to lane 0 if its healthy
978 39298372 : if (runCoreSelection && armed && lane_switching_enabled) {
979 : // update this instance's error scores for all active cores and get the primary core's error score
980 26802485 : float primaryErrorScore = updateCoreErrorScores();
981 :
982 : // update the accumulated relative error scores for all active cores
983 26802485 : updateCoreRelativeErrors();
984 :
985 : bool betterCore = false;
986 : bool altCoreAvailable = false;
987 26802485 : uint8_t newPrimaryIndex = primary;
988 :
989 : // loop through all available cores to find if an alternative core is available
990 80146961 : for (uint8_t coreIndex=0; coreIndex<num_cores; coreIndex++) {
991 53344476 : if (coreIndex != primary) {
992 26541991 : float altCoreError = coreRelativeErrors[coreIndex];
993 :
994 : // an alternative core is available for selection based on 2 conditions -
995 : // 1. healthy and states have been updated on this time step
996 : // 2. has relative error less than primary core error
997 : // 3. not been the primary core for at least 10 seconds
998 26541991 : altCoreAvailable = coreBetterScore(coreIndex, newPrimaryIndex) &&
999 66373 : imuSampleTime_us - coreLastTimePrimary_us[coreIndex] > 1E7;
1000 :
1001 : if (altCoreAvailable) {
1002 : // if this core has a significantly lower relative error to the active primary, we consider it as a
1003 : // better core and would like to switch to it even if the current primary is healthy
1004 62568 : betterCore = altCoreError <= -BETTER_THRESH; // a better core if its relative error is below a substantial level than the primary's
1005 : // handle the case where the secondary core is faster to complete yaw alignment which can happen
1006 : // in flight when oeprating without a magnetomer
1007 62568 : const NavEKF3_core &newCore = core[coreIndex];
1008 62568 : const NavEKF3_core &oldCore = core[primary];
1009 125134 : betterCore |= newCore.have_aligned_yaw() && !oldCore.have_aligned_yaw();
1010 : newPrimaryIndex = coreIndex;
1011 : }
1012 : }
1013 : }
1014 26802485 : altCoreAvailable = newPrimaryIndex != primary;
1015 :
1016 : // Switch cores if another core is available and the active primary core meets one of the following conditions -
1017 : // 1. has a bad error score
1018 : // 2. is unhealthy
1019 : // 3. is healthy, but a better core is available
1020 : // also update the yaw and position reset data to capture changes due to the lane switch
1021 26802485 : if (altCoreAvailable && (primaryErrorScore > 1.0f || !core[primary].healthy() || betterCore)) {
1022 46 : switchLane(newPrimaryIndex);
1023 46 : resetCoreErrors();
1024 46 : coreLastTimePrimary_us[primary] = imuSampleTime_us;
1025 : }
1026 : }
1027 :
1028 39298372 : if (primary != user_primary && core[user_primary].healthy() && !armed) {
1029 : // when on the ground and disarmed force the selected primary
1030 : // core. This avoids us ending with with a lottery for which
1031 : // IMU is used in each flight. Otherwise the alignment of the
1032 : // timing of the core selection updates with the timing of GPS
1033 : // updates can lead to a core other than the first one being
1034 : // used as primary for some flights. As different IMUs may
1035 : // have quite different noise characteristics this leads to
1036 : // inconsistent performance
1037 23 : primary = user_primary;
1038 : }
1039 :
1040 : // align position of inactive sources to ahrs
1041 39298372 : sources.align_inactive_sources();
1042 : }
1043 :
1044 : /*
1045 : check if switching lanes will reduce the normalised
1046 : innovations. This is called when the vehicle code is about to
1047 : trigger an EKF failsafe, and it would like to avoid that by
1048 : using a different EKF lane
1049 : */
1050 31 : void NavEKF3::checkLaneSwitch(void)
1051 : {
1052 31 : dal.log_event3(AP_DAL::Event::checkLaneSwitch);
1053 :
1054 31 : if (option_is_enabled(Option::ManualLaneSwitch)) {
1055 : return;
1056 : }
1057 :
1058 30 : uint32_t now = dal.millis();
1059 30 : if (lastLaneSwitch_ms != 0 && now - lastLaneSwitch_ms < 5000) {
1060 : // don't switch twice in 5 seconds
1061 : return;
1062 : }
1063 :
1064 30 : float primaryErrorScore = core[primary].errorScore();
1065 : float lowestErrorScore = primaryErrorScore;
1066 30 : uint8_t newPrimaryIndex = primary;
1067 90 : for (uint8_t coreIndex=0; coreIndex<num_cores; coreIndex++) {
1068 60 : if (coreIndex != primary) {
1069 30 : const NavEKF3_core &newCore = core[coreIndex];
1070 : // an alternative core is available for selection only if healthy and if states have been updated on this time step
1071 30 : bool altCoreAvailable = newCore.healthy() && newCore.have_aligned_yaw() && newCore.have_aligned_tilt();
1072 30 : float altErrorScore = newCore.errorScore();
1073 30 : if (altCoreAvailable && altErrorScore < lowestErrorScore && altErrorScore < 0.9) {
1074 : newPrimaryIndex = coreIndex;
1075 : lowestErrorScore = altErrorScore;
1076 : }
1077 : }
1078 : }
1079 :
1080 : // update the yaw and position reset data to capture changes due to the lane switch
1081 30 : switchLane(newPrimaryIndex);
1082 : }
1083 :
1084 : // switch to a new lane
1085 77 : void NavEKF3::switchLane(uint8_t new_lane_index)
1086 : {
1087 77 : if (new_lane_index >= num_cores) {
1088 : return;
1089 : }
1090 :
1091 77 : if (new_lane_index != primary) {
1092 57 : updateLaneSwitchYawResetData(new_lane_index);
1093 57 : updateLaneSwitchPosResetData(new_lane_index);
1094 57 : updateLaneSwitchPosDownResetData(new_lane_index);
1095 57 : primary = new_lane_index;
1096 57 : lastLaneSwitch_ms = dal.millis();
1097 57 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "EKF3 lane switch %u", primary);
1098 : }
1099 : }
1100 :
1101 4 : void NavEKF3::requestYawReset(void)
1102 : {
1103 4 : dal.log_event3(AP_DAL::Event::requestYawReset);
1104 :
1105 12 : for (uint8_t i = 0; i < num_cores; i++) {
1106 8 : core[i].EKFGSF_requestYawReset();
1107 : }
1108 4 : }
1109 :
1110 : /*
1111 : Update this instance error score value for all active cores
1112 : */
1113 26802485 : float NavEKF3::updateCoreErrorScores()
1114 : {
1115 80146961 : for (uint8_t i = 0; i < num_cores; i++) {
1116 53344476 : coreErrorScores[i] = core[i].errorScore();
1117 : }
1118 26802485 : return coreErrorScores[primary];
1119 : }
1120 :
1121 : /*
1122 : Update the relative error for all alternate available cores with respect to primary core's error.
1123 : A positive relative error for a core means it has been more erroneous than the existing primary.
1124 : A negative relative error indicates a core which can be switched to.
1125 : */
1126 26802485 : void NavEKF3::updateCoreRelativeErrors()
1127 : {
1128 : float error = 0;
1129 80146961 : for (uint8_t i = 0; i < num_cores; i++) {
1130 53344476 : if (i != primary) {
1131 26541991 : error = coreErrorScores[i] - coreErrorScores[primary];
1132 : // reduce error for a core only if its better than the primary lane by at least the Relative Error Threshold, this should prevent unnecessary lane changes
1133 26541991 : if (error > 0 || error < -MAX(_err_thresh, 0.05)) {
1134 15460995 : coreRelativeErrors[i] += error;
1135 15460995 : coreRelativeErrors[i] = constrain_ftype(coreRelativeErrors[i], -CORE_ERR_LIM, CORE_ERR_LIM);
1136 : }
1137 : }
1138 : }
1139 26802485 : }
1140 :
1141 : // Reset the relative error values
1142 733704 : void NavEKF3::resetCoreErrors(void)
1143 : {
1144 2200746 : for (uint8_t i = 0; i < num_cores; i++) {
1145 1467042 : coreRelativeErrors[i] = 0;
1146 : }
1147 733704 : }
1148 :
1149 : // set position, velocity and yaw sources to either 0=primary, 1=secondary, 2=tertiary
1150 16 : void NavEKF3::setPosVelYawSourceSet(uint8_t source_set_idx)
1151 : {
1152 16 : if (source_set_idx < AP_NAKEKF_SOURCE_SET_MAX) {
1153 16 : dal.log_event3(AP_DAL::Event(uint8_t(AP_DAL::Event::setSourceSet0)+source_set_idx));
1154 : }
1155 16 : sources.setPosVelYawSourceSet((AP_NavEKF_Source::SourceSetSelection)source_set_idx);
1156 16 : }
1157 :
1158 : // Check basic filter health metrics and return a consolidated health status
1159 39298372 : bool NavEKF3::healthy(void) const
1160 : {
1161 39298372 : if (!core) {
1162 : return false;
1163 : }
1164 39298372 : return core[primary].healthy();
1165 : }
1166 :
1167 : // returns false if we fail arming checks, in which case the buffer will be populated with a failure message
1168 : // requires_position should be true if horizontal position configuration should be checked
1169 31446 : bool NavEKF3::pre_arm_check(bool requires_position, char *failure_msg, uint8_t failure_msg_len) const
1170 : {
1171 : // check source configuration
1172 31446 : if (!sources.pre_arm_check(requires_position, failure_msg, failure_msg_len)) {
1173 : return false;
1174 : }
1175 :
1176 : // check if using compass (i.e. EK3_SRCn_YAW) with deprecated MAG_CAL values (5 was EXTERNAL_YAW, 6 was EXTERNAL_YAW_FALLBACK)
1177 31433 : const int8_t magCalParamVal = _magCal.get();
1178 94258 : for (uint8_t i = 0; i < num_cores; i++) {
1179 62825 : const AP_NavEKF_Source::SourceYaw yaw_source = sources.getYawSource(i);
1180 62825 : if (((magCalParamVal == 5) || (magCalParamVal == 6)) && (yaw_source != AP_NavEKF_Source::SourceYaw::GPS)) {
1181 : // yaw source is configured to use compass but MAG_CAL valid is deprecated
1182 0 : dal.snprintf(failure_msg, failure_msg_len, "EK3_MAG_CAL and EK3_SRC1_YAW inconsistent");
1183 0 : return false;
1184 : }
1185 : }
1186 :
1187 31433 : if (!core) {
1188 0 : dal.snprintf(failure_msg, failure_msg_len, "no EKF3 cores");
1189 0 : return false;
1190 : }
1191 92944 : for (uint8_t i = 0; i < num_cores; i++) {
1192 62177 : if (!core[i].healthy()) {
1193 662 : const char *failure = core[i].prearm_failure_reason();
1194 662 : if (failure != nullptr) {
1195 645 : dal.snprintf(failure_msg, failure_msg_len, failure);
1196 : } else {
1197 17 : dal.snprintf(failure_msg, failure_msg_len, "EKF3 core %d unhealthy", (int)i);
1198 : }
1199 662 : return false;
1200 : }
1201 : // run per-core pre-arm checks
1202 61515 : if (!core[i].pre_arm_check(requires_position, failure_msg, failure_msg_len)) {
1203 : return false;
1204 : }
1205 : }
1206 : return true;
1207 : }
1208 :
1209 : // returns the index of the primary core
1210 : // return -1 if no primary core selected
1211 44047616 : int8_t NavEKF3::getPrimaryCoreIndex(void) const
1212 : {
1213 44047616 : if (!core) {
1214 : return -1;
1215 : }
1216 44047616 : return primary;
1217 : }
1218 :
1219 : // returns the index of the IMU of the primary core
1220 : // return -1 if no primary core selected
1221 83373432 : int8_t NavEKF3::getPrimaryCoreIMUIndex(void) const
1222 : {
1223 83373432 : if (!core) {
1224 : return -1;
1225 : }
1226 80106038 : return core[primary].getIMUIndex();
1227 : }
1228 :
1229 : // Write the last calculated NE position relative to the reference point (m).
1230 : // If a calculated solution is not available, use the best available data and return false
1231 : // If false returned, do not use for flight control
1232 41686716 : bool NavEKF3::getPosNE(Vector2p &posNE) const
1233 : {
1234 41686716 : if (!core) {
1235 : return false;
1236 : }
1237 40053019 : return core[primary].getPosNE(posNE);
1238 : }
1239 :
1240 : // Write the last calculated D position relative to the reference point (m).
1241 : // If a calculated solution is not available, use the best available data and return false
1242 : // If false returned, do not use for flight control
1243 41686716 : bool NavEKF3::getPosD(postype_t &posD) const
1244 : {
1245 41686716 : if (!core) {
1246 : return false;
1247 : }
1248 40053019 : return core[primary].getPosD(posD);
1249 : }
1250 :
1251 : // return NED velocity in m/s
1252 41686716 : void NavEKF3::getVelNED(Vector3f &vel) const
1253 : {
1254 41686716 : if (core) {
1255 40053019 : core[primary].getVelNED(vel);
1256 : }
1257 41686716 : }
1258 :
1259 : // return estimate of true airspeed vector in body frame in m/s
1260 : // returns false if estimate is unavailable
1261 36087486 : bool NavEKF3::getAirSpdVec(Vector3f &vel) const
1262 : {
1263 36087486 : if (core) {
1264 36087486 : return core[primary].getAirSpdVec(vel);
1265 : }
1266 : return false;
1267 : }
1268 :
1269 : // 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
1270 152265 : bool NavEKF3::getAirSpdHealthData(uint8_t instance, float &innovation, float &innovationVariance, uint32_t &age_ms) const
1271 : {
1272 152265 : if (core == nullptr) {
1273 : return false;
1274 : }
1275 :
1276 : // Return primary if it is configured to use the given instance
1277 152265 : if (core[primary].getActiveAirspeed() == instance) {
1278 146109 : return core[primary].getAirSpdHealthData(innovation, innovationVariance, age_ms);
1279 : }
1280 :
1281 : // See if another core is using the given instance
1282 17181 : for (uint8_t i = 0; i < num_cores; i++) {
1283 11895 : if (core[i].getActiveAirspeed() == instance) {
1284 870 : return core[i].getAirSpdHealthData(innovation, innovationVariance, age_ms);
1285 : }
1286 : }
1287 :
1288 : return false;
1289 : }
1290 :
1291 : // Return the rate of change of vertical position in the down direction (dPosD/dt) in m/s
1292 41686716 : float NavEKF3::getPosDownDerivative() const
1293 : {
1294 : // return the value calculated from a complementary filter applied to the EKF height and vertical acceleration
1295 41686716 : if (core) {
1296 40053019 : return core[primary].getPosDownDerivative();
1297 : }
1298 : return 0.0f;
1299 : }
1300 :
1301 : // return body axis gyro bias estimates in rad/sec
1302 41686716 : void NavEKF3::getGyroBias(int8_t instance, Vector3f &gyroBias) const
1303 : {
1304 41686716 : if (instance < 0 || instance >= num_cores) instance = primary;
1305 41686716 : if (core) {
1306 40053019 : core[instance].getGyroBias(gyroBias);
1307 : }
1308 41686716 : }
1309 :
1310 : // return accelerometer bias estimate in m/s/s
1311 41686716 : void NavEKF3::getAccelBias(int8_t instance, Vector3f &accelBias) const
1312 : {
1313 41686716 : if (instance < 0 || instance >= num_cores) instance = primary;
1314 41686716 : if (core) {
1315 40053019 : core[instance].getAccelBias(accelBias);
1316 : }
1317 41686716 : }
1318 :
1319 : // returns active source set used by EKF3
1320 0 : uint8_t NavEKF3::get_active_source_set() const
1321 : {
1322 0 : return sources.get_active_source_set();
1323 : }
1324 :
1325 : // reset body axis gyro bias estimates
1326 7 : void NavEKF3::resetGyroBias(void)
1327 : {
1328 7 : dal.log_event3(AP_DAL::Event::resetGyroBias);
1329 :
1330 7 : if (core) {
1331 21 : for (uint8_t i=0; i<num_cores; i++) {
1332 14 : core[i].resetGyroBias();
1333 : }
1334 : }
1335 7 : }
1336 :
1337 : // Resets the baro so that it reads zero at the current height
1338 : // Resets the EKF height to zero
1339 : // Adjusts the EKF origin height so that the EKF height + origin height is the same as before
1340 : // Returns true if the height datum reset has been performed
1341 : // If using a range finder for height no reset is performed and it returns false
1342 3121 : bool NavEKF3::resetHeightDatum(void)
1343 : {
1344 3121 : dal.log_event3(AP_DAL::Event::resetHeightDatum);
1345 :
1346 : bool status = true;
1347 3121 : if (core) {
1348 8828 : for (uint8_t i=0; i<num_cores; i++) {
1349 5885 : if (!core[i].resetHeightDatum()) {
1350 : status = false;
1351 : }
1352 : }
1353 : } else {
1354 : status = false;
1355 : }
1356 3121 : return status;
1357 : }
1358 :
1359 : // return the horizontal speed limit in m/s set by optical flow sensor limits
1360 : // return the scale factor to be applied to navigation velocity gains to compensate for increase in velocity noise with height when using optical flow
1361 41686716 : void NavEKF3::getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) const
1362 : {
1363 41686716 : if (core) {
1364 40053019 : core[primary].getEkfControlLimits(ekfGndSpdLimit, ekfNavVelGainScaler);
1365 : } else {
1366 1633697 : ekfGndSpdLimit = 400.0f; //return 80% of max filter speed
1367 1633697 : ekfNavVelGainScaler = 1.0f;
1368 : }
1369 41686716 : }
1370 :
1371 : // return the NED wind speed estimates in m/s (positive is air moving in the direction of the axis)
1372 : // returns true if wind state estimation is active
1373 41686716 : bool NavEKF3::getWind(Vector3f &wind) const
1374 : {
1375 41686716 : if (core == nullptr) {
1376 : return false;
1377 : }
1378 40053019 : return core[primary].getWind(wind);
1379 : }
1380 :
1381 : // return earth magnetic field estimates in measurement units / 1000
1382 41686716 : void NavEKF3::getMagNED(Vector3f &magNED) const
1383 : {
1384 41686716 : if (core) {
1385 40053019 : core[primary].getMagNED(magNED);
1386 : }
1387 41686716 : }
1388 :
1389 : // return body magnetic field estimates in measurement units / 1000
1390 41686716 : void NavEKF3::getMagXYZ(Vector3f &magXYZ) const
1391 : {
1392 41686716 : if (core) {
1393 40053019 : core[primary].getMagXYZ(magXYZ);
1394 : }
1395 41686716 : }
1396 :
1397 : // return the airspeed sensor in use
1398 72438414 : uint8_t NavEKF3::getActiveAirspeed() const
1399 : {
1400 72438414 : if (core) {
1401 72438414 : return core[primary].getActiveAirspeed();
1402 : } else {
1403 : return UINT8_MAX;
1404 : }
1405 : }
1406 :
1407 : // Return estimated magnetometer offsets
1408 : // Return true if magnetometer offsets are valid
1409 9 : bool NavEKF3::getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const
1410 : {
1411 9 : if (!core) {
1412 : return false;
1413 : }
1414 : // try the primary first, else loop through all of the cores and return when one has offsets for this mag instance
1415 9 : if (core[primary].getMagOffsets(mag_idx, magOffsets)) {
1416 : return true;
1417 : }
1418 15 : for (uint8_t i=0; i<num_cores; i++) {
1419 11 : if(core[i].getMagOffsets(mag_idx, magOffsets)) {
1420 : return true;
1421 : }
1422 : }
1423 : return false;
1424 : }
1425 :
1426 : // Return the last calculated latitude, longitude and height in WGS-84
1427 : // If a calculated location isn't available, return a raw GPS measurement
1428 : // The status will return true if a calculation or raw measurement is available
1429 : // The getFilterStatus() function provides a more detailed description of data health and must be checked if data is to be used for flight control
1430 41686716 : bool NavEKF3::getLLH(Location &loc) const
1431 : {
1432 41686716 : if (!core) {
1433 : return false;
1434 : }
1435 40053019 : return core[primary].getLLH(loc);
1436 : }
1437 :
1438 : // Return the latitude and longitude and height used to set the NED origin
1439 : // All NED positions calculated by the filter are relative to this location
1440 : // Returns false if the origin has not been set
1441 52093524 : bool NavEKF3::getOriginLLH(Location &loc) const
1442 : {
1443 52093524 : if (!core) {
1444 : return false;
1445 : }
1446 49962614 : if (common_origin_valid) {
1447 39910125 : loc = common_EKF_origin;
1448 39910125 : return true;
1449 : }
1450 10052489 : return core[primary].getOriginLLH(loc);
1451 : }
1452 :
1453 : // set the latitude and longitude and height used to set the NED origin
1454 : // All NED positions calculated by the filter will be relative to this location
1455 : // The origin cannot be set if the filter is in a flight mode (eg vehicle armed)
1456 : // Returns false if the filter has rejected the attempt to set the origin
1457 219 : bool NavEKF3::setOriginLLH(const Location &loc)
1458 : {
1459 219 : dal.log_SetOriginLLH3(loc);
1460 :
1461 219 : if (!core) {
1462 : return false;
1463 : }
1464 159 : if (common_origin_valid) {
1465 : // we don't allow setting the EKF origin if it has already been set
1466 : // this is to prevent causing upsets from a shifting origin.
1467 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "EKF3: origin already set");
1468 0 : return false;
1469 : }
1470 : bool ret = false;
1471 477 : for (uint8_t i=0; i<num_cores; i++) {
1472 318 : ret |= core[i].setOriginLLH(loc);
1473 : }
1474 : // return true if any core accepts the new origin
1475 : return ret;
1476 : }
1477 :
1478 2 : bool NavEKF3::setLatLng(const Location &loc, float posAccuracy, uint32_t timestamp_ms)
1479 : {
1480 : #if EK3_FEATURE_POSITION_RESET
1481 2 : dal.log_SetLatLng(loc, posAccuracy, timestamp_ms);
1482 :
1483 2 : if (!core) {
1484 : return false;
1485 : }
1486 : bool ret = false;
1487 6 : for (uint8_t i=0; i<num_cores; i++) {
1488 4 : ret |= core[i].setLatLng(loc, posAccuracy, timestamp_ms);
1489 : }
1490 : // return true if any core accepts the new origin
1491 : return ret;
1492 : #else
1493 : return false;
1494 : #endif // EK3_FEATURE_POSITION_RESET
1495 : }
1496 :
1497 :
1498 : // return estimated height above ground level
1499 : // return false if ground height is not being estimated.
1500 41686716 : bool NavEKF3::getHAGL(float &HAGL) const
1501 : {
1502 41686716 : if (!core) {
1503 : return false;
1504 : }
1505 40053019 : return core[primary].getHAGL(HAGL);
1506 : }
1507 :
1508 : // return the Euler roll, pitch and yaw angle in radians
1509 41686716 : void NavEKF3::getEulerAngles(Vector3f &eulers) const
1510 : {
1511 41686716 : if (core) {
1512 40053019 : core[primary].getEulerAngles(eulers);
1513 : }
1514 41686716 : }
1515 :
1516 : // return the transformation matrix from XYZ (body) to NED axes
1517 41686716 : void NavEKF3::getRotationBodyToNED(Matrix3f &mat) const
1518 : {
1519 41686716 : if (core) {
1520 40053019 : core[primary].getRotationBodyToNED(mat);
1521 : }
1522 41686716 : }
1523 :
1524 : // return the quaternions defining the rotation from XYZ (body) to NED axes
1525 73821 : void NavEKF3::getQuaternionBodyToNED(int8_t instance, Quaternion &quat) const
1526 : {
1527 73821 : if (instance < 0 || instance >= num_cores) instance = primary;
1528 73821 : if (core) {
1529 73821 : Matrix3f mat;
1530 73821 : core[instance].getRotationBodyToNED(mat);
1531 73821 : quat.from_rotation_matrix(mat);
1532 : }
1533 73821 : }
1534 :
1535 : // return the quaternions defining the rotation from NED to XYZ (autopilot) axes
1536 41686716 : void NavEKF3::getQuaternion(Quaternion &quat) const
1537 : {
1538 41686716 : if (core) {
1539 40053019 : core[primary].getQuaternion(quat);
1540 : }
1541 41686716 : }
1542 :
1543 : // return the innovations
1544 686586 : bool NavEKF3::getInnovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const
1545 : {
1546 686586 : if (core == nullptr) {
1547 : return false;
1548 : }
1549 :
1550 677201 : return core[primary].getInnovations(velInnov, posInnov, magInnov, tasInnov, yawInnov);
1551 : }
1552 :
1553 : // return the innovation consistency test ratios for the velocity, position, magnetometer and true airspeed measurements
1554 41686716 : bool NavEKF3::getVariances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const
1555 : {
1556 41686716 : if (core == nullptr) {
1557 : return false;
1558 : }
1559 :
1560 40053019 : return core[primary].getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset);
1561 : }
1562 :
1563 : // return 1-sigma position and velocity uncertainty from the EKF state error covariance matrix P
1564 13 : bool NavEKF3::getPosVelUncertainty(float &pos_horiz_m, float &pos_vert_m, float &vel_m_s) const
1565 : {
1566 13 : if (core == nullptr) {
1567 : return false;
1568 : }
1569 13 : return core[primary].getPosVelUncertainty(pos_horiz_m, pos_vert_m, vel_m_s);
1570 : }
1571 :
1572 : // get a source's velocity innovations
1573 : // returns true on success and results are placed in innovations and variances arguments
1574 39 : bool NavEKF3::getVelInnovationsAndVariancesForSource(AP_NavEKF_Source::SourceXY source, Vector3f &innovations, Vector3f &variances) const
1575 : {
1576 39 : if (core) {
1577 28 : return core[primary].getVelInnovationsAndVariancesForSource(source, innovations, variances);
1578 : }
1579 : return false;
1580 : }
1581 :
1582 : // should we use the compass? This is public so it can be used for
1583 : // reporting via ahrs.use_compass()
1584 46313 : bool NavEKF3::use_compass(void) const
1585 : {
1586 46313 : if (!core) {
1587 : return false;
1588 : }
1589 46313 : return core[primary].use_compass();
1590 : }
1591 :
1592 : // are we using (aka fusing) a non-compass yaw?
1593 41704514 : bool NavEKF3::using_noncompass_for_yaw(void) const
1594 : {
1595 41704514 : if (!core) {
1596 : return false;
1597 : }
1598 40070817 : return core[primary].using_noncompass_for_yaw();
1599 : }
1600 :
1601 : // are we using (aka fusing) external nav for yaw?
1602 41686716 : bool NavEKF3::using_extnav_for_yaw() const
1603 : {
1604 41686716 : if (!core) {
1605 : return false;
1606 : }
1607 40053019 : return core[primary].using_extnav_for_yaw();
1608 : }
1609 :
1610 : // are we using a gps?
1611 41686716 : bool NavEKF3::using_gps(void) const
1612 : {
1613 41686716 : if (!core) {
1614 : return false;
1615 : }
1616 40053019 : return core[primary].using_gps();
1617 : }
1618 :
1619 : // check if configured to use GPS for horizontal position estimation
1620 49400439 : bool NavEKF3::configuredToUseGPSForPosXY(void) const
1621 : {
1622 : // 0 = use 3D velocity, 1 = use 2D velocity, 2 = use no velocity, 3 = do not use GPS
1623 49400439 : if (sources.getPosXYSource(primary) == AP_NavEKF_Source::SourceXY::GPS) {
1624 48959659 : return true;
1625 : }
1626 : return false;
1627 : }
1628 :
1629 : // check if configured to use GPS for horizontal position estimation
1630 0 : bool NavEKF3::configuredToUseGPSForPos(void) const
1631 : {
1632 0 : if (configuredToUseGPSForPosXY() || sources.getPosZSource(primary) == AP_NavEKF_Source::SourceZ::GPS) {
1633 0 : return true;
1634 : }
1635 : return false;
1636 : }
1637 :
1638 : // write the raw optical flow measurements
1639 : // rawFlowQuality is a measured of quality between 0 and 255, with 255 being the best quality
1640 : // rawFlowRates are the optical flow rates in rad/sec about the X and Y sensor axes.
1641 : // rawGyroRates are the sensor rotation rates in rad/sec measured by the sensors internal gyro
1642 : // The sign convention is that a RH physical rotation of the sensor about an axis produces both a positive flow and gyro rate
1643 : // msecFlowMeas is the scheduler time in msec when the optical flow data was received from the sensor.
1644 : // posOffset is the XYZ flow sensor position in the body frame in m
1645 : // heightOverride is the fixed height of the sensor above ground in m, when on rover vehicles. 0 if not used
1646 : #if EK3_FEATURE_OPTFLOW_FUSION
1647 10128 : void NavEKF3::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride)
1648 : {
1649 10128 : dal.writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawGyroRates, msecFlowMeas, posOffset, heightOverride);
1650 :
1651 10128 : if (core) {
1652 29964 : for (uint8_t i=0; i<num_cores; i++) {
1653 19976 : core[i].writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawGyroRates, msecFlowMeas, posOffset, heightOverride);
1654 : }
1655 : }
1656 10128 : }
1657 :
1658 : // retrieve latest corrected optical flow samples (used for calibration)
1659 11051 : bool NavEKF3::getOptFlowSample(uint32_t& timeStamp_ms, Vector2f& flowRate, Vector2f& bodyRate, Vector2f& losPred) const
1660 : {
1661 : // return optical flow samples from primary core
1662 11051 : if (core) {
1663 11051 : return core[primary].getOptFlowSample(timeStamp_ms, flowRate, bodyRate, losPred);
1664 : }
1665 : return false;
1666 : }
1667 : #endif // EK3_FEATURE_OPTFLOW_FUSION
1668 :
1669 : // write yaw angle sensor measurements
1670 0 : void NavEKF3::writeEulerYawAngle(float yawAngle, float yawAngleErr, uint32_t timeStamp_ms, uint8_t type)
1671 : {
1672 0 : dal.log_writeEulerYawAngle(yawAngle, yawAngleErr, timeStamp_ms, type);
1673 :
1674 0 : if (core) {
1675 0 : for (uint8_t i=0; i<num_cores; i++) {
1676 0 : core[i].writeEulerYawAngle(yawAngle, yawAngleErr, timeStamp_ms, type);
1677 : }
1678 : }
1679 0 : }
1680 :
1681 : /*
1682 : * Write position and quaternion data from an external navigation system
1683 : *
1684 : * pos : XYZ position (m) in a RH navigation frame with the Z axis pointing down and XY axes horizontal. Frame must be aligned with NED if the magnetomer is being used for yaw.
1685 : * quat : quaternion describing the rotation from navigation frame to body frame
1686 : * posErr : 1-sigma spherical position error (m)
1687 : * angErr : 1-sigma spherical angle error (rad)
1688 : * timeStamp_ms : system time the measurement was taken, not the time it was received (mSec)
1689 : * delay_ms : average delay of external nav system measurements relative to inertial measurements
1690 : * resetTime_ms : system time of the last position reset request (mSec)
1691 : *
1692 : */
1693 10069 : void NavEKF3::writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint16_t delay_ms, uint32_t resetTime_ms)
1694 : {
1695 10069 : dal.writeExtNavData(pos, quat, posErr, angErr, timeStamp_ms, delay_ms, resetTime_ms);
1696 :
1697 10069 : if (core) {
1698 29844 : for (uint8_t i=0; i<num_cores; i++) {
1699 19896 : core[i].writeExtNavData(pos, quat, posErr, angErr, timeStamp_ms, delay_ms, resetTime_ms);
1700 : }
1701 : }
1702 10069 : }
1703 :
1704 : /* Write velocity data from an external navigation system
1705 : * vel : velocity in NED (m)
1706 : * err : velocity error (m/s)
1707 : * timeStamp_ms : system time the measurement was taken, not the time it was received (mSec)
1708 : * delay_ms : average delay of external nav system measurements relative to inertial measurements
1709 : */
1710 4278 : void NavEKF3::writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeStamp_ms, uint16_t delay_ms)
1711 : {
1712 4278 : dal.writeExtNavVelData(vel, err, timeStamp_ms, delay_ms);
1713 :
1714 4278 : if (core) {
1715 12702 : for (uint8_t i=0; i<num_cores; i++) {
1716 8468 : core[i].writeExtNavVelData(vel, err, timeStamp_ms, delay_ms);
1717 : }
1718 : }
1719 4278 : }
1720 :
1721 : // return data for debugging optical flow fusion
1722 : /*
1723 : * Write body frame linear and angular displacement measurements from a visual odometry sensor
1724 : *
1725 : * quality is a normalised confidence value from 0 to 100
1726 : * delPos is the XYZ change in linear position measured in body frame and relative to the inertial reference at timeStamp_ms (m)
1727 : * delAng is the XYZ angular rotation measured in body frame and relative to the inertial reference at timeStamp_ms (rad)
1728 : * delTime is the time interval for the measurement of delPos and delAng (sec)
1729 : * timeStamp_ms is the timestamp of the last image used to calculate delPos and delAng (msec)
1730 : * delay_ms is the average delay of external nav system measurements relative to inertial measurements
1731 : * posOffset is the XYZ body frame position of the camera focal point (m)
1732 : */
1733 3633 : void NavEKF3::writeBodyFrameOdom(float quality, const Vector3f &delPos, const Vector3f &delAng, float delTime, uint32_t timeStamp_ms, uint16_t delay_ms, const Vector3f &posOffset)
1734 : {
1735 3633 : dal.writeBodyFrameOdom(quality, delPos, delAng, delTime, timeStamp_ms, delay_ms, posOffset);
1736 :
1737 3633 : if (core) {
1738 10743 : for (uint8_t i=0; i<num_cores; i++) {
1739 7162 : core[i].writeBodyFrameOdom(quality, delPos, delAng, delTime, timeStamp_ms, delay_ms, posOffset);
1740 : }
1741 : }
1742 3633 : }
1743 :
1744 : /*
1745 : * Write odometry data from a wheel encoder. The axis of rotation is assumed to be parallel to the vehicle body axis
1746 : *
1747 : * delAng is the measured change in angular position from the previous measurement where a positive rotation is produced by forward motion of the vehicle (rad)
1748 : * delTime is the time interval for the measurement of delAng (sec)
1749 : * timeStamp_ms is the time when the rotation was last measured (msec)
1750 : * posOffset is the XYZ body frame position of the wheel hub (m)
1751 : */
1752 4353 : void NavEKF3::writeWheelOdom(float delAng, float delTime, uint32_t timeStamp_ms, const Vector3f &posOffset, float radius)
1753 : {
1754 4353 : dal.writeWheelOdom(delAng, delTime, timeStamp_ms, posOffset, radius);
1755 :
1756 4353 : if (core) {
1757 12459 : for (uint8_t i=0; i<num_cores; i++) {
1758 8306 : core[i].writeWheelOdom(delAng, delTime, timeStamp_ms, posOffset, radius);
1759 : }
1760 : }
1761 4353 : }
1762 :
1763 : /*
1764 : * Write terrain altitude (derived from SRTM) in meters above the origin
1765 : * only used by optical flow when out of rangefinder range
1766 : */
1767 1064989 : void NavEKF3::writeTerrainData(float alt_m)
1768 : {
1769 : #if EK3_FEATURE_OPTFLOW_SRTM
1770 : // write altitude to DAL
1771 1064989 : dal.writeTerrainData(alt_m);
1772 :
1773 : // exit immediately if feature is not enabled
1774 1064989 : if (!option_is_enabled(Option::OptflowMayUseTerrainAlt)) {
1775 : return;
1776 : }
1777 :
1778 : // send to each core
1779 0 : if (core) {
1780 0 : for (uint8_t i=0; i<num_cores; i++) {
1781 0 : core[i].writeTerrainData(alt_m);
1782 : }
1783 : }
1784 : #endif
1785 : }
1786 :
1787 : // Set to true if the terrain underneath is stable enough to be used as a height reference
1788 : // in combination with a range finder. Set to false if the terrain underneath the vehicle
1789 : // cannot be used as a height reference. Use to prevent range finder operation otherwise
1790 : // enabled by the combination of EK3_RNG_USE_HGT and EK3_RNG_USE_SPD parameters.
1791 2339 : void NavEKF3::setTerrainHgtStable(bool val)
1792 : {
1793 2339 : if (val) {
1794 769 : dal.log_event3(AP_DAL::Event::setTerrainHgtStable);
1795 : } else {
1796 1570 : dal.log_event3(AP_DAL::Event::unsetTerrainHgtStable);
1797 : }
1798 :
1799 2339 : if (core) {
1800 3837 : for (uint8_t i=0; i<num_cores; i++) {
1801 2559 : core[i].setTerrainHgtStable(val);
1802 : }
1803 : }
1804 :
1805 2339 : }
1806 :
1807 : /*
1808 : return the filter fault status as a bitmasked integer
1809 : 0 = quaternions are NaN
1810 : 1 = velocities are NaN
1811 : 2 = badly conditioned X magnetometer fusion
1812 : 3 = badly conditioned Y magnetometer fusion
1813 : 4 = badly conditioned Z magnetometer fusion
1814 : 5 = badly conditioned airspeed fusion
1815 : 6 = badly conditioned synthetic sideslip fusion
1816 : 7 = filter is not initialised
1817 : */
1818 41686716 : void NavEKF3::getFilterFaults(uint16_t &faults) const
1819 : {
1820 41686716 : if (core) {
1821 40053019 : core[primary].getFilterFaults(faults);
1822 : } else {
1823 1633697 : faults = 0;
1824 : }
1825 41686716 : }
1826 :
1827 : /*
1828 : return filter status flags
1829 : */
1830 41686716 : void NavEKF3::getFilterStatus(nav_filter_status &status) const
1831 : {
1832 41686716 : if (core) {
1833 40053019 : core[primary].getFilterStatus(status);
1834 : } else {
1835 1633697 : memset(&status, 0, sizeof(status));
1836 : }
1837 41686716 : }
1838 :
1839 : // send an EKF_STATUS_REPORT message to GCS
1840 41686716 : bool NavEKF3::getTerrainAltVariance(float &terrainAltVar) const
1841 : {
1842 41686716 : if (core) {
1843 40053019 : return core[primary].getTerrainAltVariance(terrainAltVar);
1844 : }
1845 : return false;
1846 : }
1847 :
1848 : // provides the height limit to be observed by the control loops
1849 : // returns false if no height limiting is required
1850 : // this is needed to ensure the vehicle does not fly too high when using optical flow navigation
1851 41686716 : bool NavEKF3::getHeightControlLimit(float &height) const
1852 : {
1853 41686716 : if (!core) {
1854 : return false;
1855 : }
1856 40053019 : return core[primary].getHeightControlLimit(height);
1857 : }
1858 :
1859 : // Returns a count of NE position reset events; incremented when the primary
1860 : // core changes and when the primary core resets its NE position
1861 39298372 : uint16_t NavEKF3::getPosNorthEastResetCount(void)
1862 : {
1863 39298372 : if (!core) {
1864 : return 0;
1865 : }
1866 39298372 : const uint16_t core_count = core[primary].getPosNorthEastResetCount();
1867 39298372 : if (core_count != pos_reset_data.last_core_count) {
1868 2497 : pos_reset_data.last_core_count = core_count;
1869 2497 : pos_reset_data.count++;
1870 : }
1871 39298372 : return pos_reset_data.count;
1872 : }
1873 :
1874 : // Returns a count of D position reset events; incremented when the primary
1875 : // core changes and when the primary core resets its D position
1876 39298372 : uint16_t NavEKF3::getPosDownResetCount(void)
1877 : {
1878 39298372 : if (!core) {
1879 : return 0;
1880 : }
1881 39298372 : const uint16_t core_count = core[primary].getPosDownResetCount();
1882 39298372 : if (core_count != pos_down_reset_data.last_core_count) {
1883 1619 : pos_down_reset_data.last_core_count = core_count;
1884 1619 : pos_down_reset_data.count++;
1885 : }
1886 39298372 : return pos_down_reset_data.count;
1887 : }
1888 :
1889 : // Returns a count of yaw reset events; incremented when the primary core
1890 : // changes and when the primary core resets its yaw
1891 39298372 : uint16_t NavEKF3::getYawResetCount(void)
1892 : {
1893 39298372 : if (!core) {
1894 : return 0;
1895 : }
1896 39298372 : const uint16_t core_count = core[primary].getYawResetCount();
1897 39298372 : if (core_count != yaw_reset_data.last_core_count) {
1898 2140 : yaw_reset_data.last_core_count = core_count;
1899 2140 : yaw_reset_data.count++;
1900 : }
1901 39298372 : return yaw_reset_data.count;
1902 : }
1903 :
1904 : // update the yaw reset data to capture changes due to a lane switch
1905 57 : void NavEKF3::updateLaneSwitchYawResetData(uint8_t new_primary)
1906 : {
1907 : // the new primary's historic in-core resets are not new events
1908 : // for consumers; re-seat the count and record a single event for
1909 : // the switch itself
1910 57 : yaw_reset_data.last_core_count = core[new_primary].getYawResetCount();
1911 57 : yaw_reset_data.count++;
1912 57 : }
1913 :
1914 : // update the position reset data to capture changes due to a lane switch
1915 57 : void NavEKF3::updateLaneSwitchPosResetData(uint8_t new_primary)
1916 : {
1917 : // the new primary's historic in-core resets are not new events
1918 : // for consumers; re-seat the count and record a single event for
1919 : // the switch itself
1920 57 : pos_reset_data.last_core_count = core[new_primary].getPosNorthEastResetCount();
1921 57 : pos_reset_data.count++;
1922 :
1923 57 : }
1924 :
1925 : // Update the vertical position reset data to capture changes due to a core switch
1926 : // This should be called after the decision to switch cores has been made, but before the
1927 : // new primary EKF update has been run
1928 57 : void NavEKF3::updateLaneSwitchPosDownResetData(uint8_t new_primary)
1929 : {
1930 : // the new primary's historic in-core resets are not new events
1931 : // for consumers; re-seat the count and record a single event for
1932 : // the switch itself
1933 57 : pos_down_reset_data.last_core_count = core[new_primary].getPosDownResetCount();
1934 57 : pos_down_reset_data.count++;
1935 :
1936 57 : }
1937 :
1938 : // 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.
1939 0 : void NavEKF3::writeDefaultAirSpeed(float airspeed, float uncertainty)
1940 : {
1941 : // ignore any data if the EKF is not started
1942 0 : if (!core) {
1943 : return;
1944 : }
1945 :
1946 0 : dal.log_writeDefaultAirSpeed3(airspeed, uncertainty);
1947 :
1948 0 : for (uint8_t i=0; i<num_cores; i++) {
1949 0 : core[i].writeDefaultAirSpeed(airspeed, uncertainty);
1950 : }
1951 : }
1952 :
1953 : // returns true when the yaw angle has been aligned
1954 0 : bool NavEKF3::yawAlignmentComplete(void) const
1955 : {
1956 0 : if (!core) {
1957 : return false;
1958 : }
1959 0 : return core[primary].have_aligned_yaw();
1960 : }
1961 :
1962 : // returns true when the state estimates are significantly degraded by vibration
1963 41686716 : bool NavEKF3::isVibrationAffected() const
1964 : {
1965 41686716 : if (core) {
1966 40053019 : return core[primary].isVibrationAffected();
1967 : }
1968 : return false;
1969 : }
1970 :
1971 : // get a yaw estimator instance
1972 479 : const EKFGSF_yaw *NavEKF3::get_yawEstimator(void) const
1973 : {
1974 479 : if (core) {
1975 479 : return core[primary].get_yawEstimator();
1976 : }
1977 : return nullptr;
1978 : }
1979 :
1980 : // Do a reset and bootstrap alignment of all EKF cores
1981 : // return true if successful for all cores
1982 : // When on the ground and stationary, gyros are recalibrated first so
1983 : // the filter bootstraps with clean offsets. In flight the gyro
1984 : // calibration is skipped and the filter resets with existing biases.
1985 1 : bool NavEKF3::InitialiseFilterBootstrap()
1986 : {
1987 : // ignore any data if the EKF is not started
1988 1 : if (!core) {
1989 : return false;
1990 : }
1991 :
1992 : // initialise the cores. We return success only if all cores
1993 : // initialise successfully. The per-core InitialiseFilterBootstrap()
1994 : // return value is false when the IMU delay buffer is not yet full,
1995 : // which is normal during a reset. Check statesInitialised directly
1996 : // to determine whether the bootstrap alignment succeeded.
1997 : bool ret = true;
1998 3 : for (uint8_t i=0; i<num_cores; i++) {
1999 : // clear the statesInitialised status to allow a bootstrap alignment
2000 2 : core[i].clearStatesInitialised();
2001 : // perform a bootstrap alignment
2002 2 : core[i].InitialiseFilterBootstrap();
2003 2 : if (!core[i].isStatesInitialised()) {
2004 : ret = false;
2005 : }
2006 : }
2007 : return ret;
2008 : }
|
