Line data Source code
1 : #include "AP_NavEKF2_core.h"
2 :
3 : #include "AP_NavEKF2.h"
4 :
5 : #include <AP_DAL/AP_DAL.h>
6 : #include <AP_HAL/AP_HAL.h>
7 : #include <GCS_MAVLink/GCS.h>
8 : #include <AP_Logger/AP_Logger.h>
9 : #include <AP_Vehicle/AP_Vehicle_Type.h>
10 : #include <new>
11 :
12 : /*
13 : parameter defaults for different types of vehicle. The
14 : APM_BUILD_DIRECTORY is taken from the main vehicle directory name
15 : where the code is built.
16 : */
17 : #if APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_Replay)
18 : // copter defaults
19 : #define VELNE_M_NSE_DEFAULT 0.3f
20 : #define VELD_M_NSE_DEFAULT 0.5f
21 : #define POSNE_M_NSE_DEFAULT 1.0f
22 : #define ALT_M_NSE_DEFAULT 3.0f
23 : #define MAG_M_NSE_DEFAULT 0.05f
24 : #define GYRO_P_NSE_DEFAULT 3.0E-02f
25 : #define ACC_P_NSE_DEFAULT 6.0E-01f
26 : #define GBIAS_P_NSE_DEFAULT 1.0E-04f
27 : #define GSCALE_P_NSE_DEFAULT 5.0E-04f
28 : #define ABIAS_P_NSE_DEFAULT 5.0E-03f
29 : #define MAGB_P_NSE_DEFAULT 1.0E-04f
30 : #define MAGE_P_NSE_DEFAULT 1.0E-03f
31 : #define VEL_I_GATE_DEFAULT 500
32 : #define POS_I_GATE_DEFAULT 500
33 : #define HGT_I_GATE_DEFAULT 500
34 : #define MAG_I_GATE_DEFAULT 300
35 : #define MAG_CAL_DEFAULT 3
36 : #define GLITCH_RADIUS_DEFAULT 25
37 : #define FLOW_MEAS_DELAY 10
38 : #define FLOW_M_NSE_DEFAULT 0.25f
39 : #define FLOW_I_GATE_DEFAULT 300
40 : #define CHECK_SCALER_DEFAULT 100
41 : #define FLOW_USE_DEFAULT 1
42 :
43 : #elif APM_BUILD_TYPE(APM_BUILD_Rover)
44 : // rover defaults
45 : #define VELNE_M_NSE_DEFAULT 0.5f
46 : #define VELD_M_NSE_DEFAULT 0.7f
47 : #define POSNE_M_NSE_DEFAULT 1.0f
48 : #define ALT_M_NSE_DEFAULT 3.0f
49 : #define MAG_M_NSE_DEFAULT 0.05f
50 : #define GYRO_P_NSE_DEFAULT 3.0E-02f
51 : #define ACC_P_NSE_DEFAULT 6.0E-01f
52 : #define GBIAS_P_NSE_DEFAULT 1.0E-04f
53 : #define GSCALE_P_NSE_DEFAULT 5.0E-04f
54 : #define ABIAS_P_NSE_DEFAULT 5.0E-03f
55 : #define MAGB_P_NSE_DEFAULT 1.0E-04f
56 : #define MAGE_P_NSE_DEFAULT 1.0E-03f
57 : #define VEL_I_GATE_DEFAULT 500
58 : #define POS_I_GATE_DEFAULT 500
59 : #define HGT_I_GATE_DEFAULT 500
60 : #define MAG_I_GATE_DEFAULT 300
61 : #define MAG_CAL_DEFAULT 2
62 : #define GLITCH_RADIUS_DEFAULT 25
63 : #define FLOW_MEAS_DELAY 10
64 : #define FLOW_M_NSE_DEFAULT 0.25f
65 : #define FLOW_I_GATE_DEFAULT 300
66 : #define CHECK_SCALER_DEFAULT 100
67 : #define FLOW_USE_DEFAULT 1
68 :
69 : #elif APM_BUILD_TYPE(APM_BUILD_ArduPlane)
70 : // plane defaults
71 : #define VELNE_M_NSE_DEFAULT 0.5f
72 : #define VELD_M_NSE_DEFAULT 0.7f
73 : #define POSNE_M_NSE_DEFAULT 1.0f
74 : #define ALT_M_NSE_DEFAULT 3.0f
75 : #define MAG_M_NSE_DEFAULT 0.05f
76 : #define GYRO_P_NSE_DEFAULT 3.0E-02f
77 : #define ACC_P_NSE_DEFAULT 6.0E-01f
78 : #define GBIAS_P_NSE_DEFAULT 1.0E-04f
79 : #define GSCALE_P_NSE_DEFAULT 5.0E-04f
80 : #define ABIAS_P_NSE_DEFAULT 5.0E-03f
81 : #define MAGB_P_NSE_DEFAULT 1.0E-04f
82 : #define MAGE_P_NSE_DEFAULT 1.0E-03f
83 : #define VEL_I_GATE_DEFAULT 500
84 : #define POS_I_GATE_DEFAULT 500
85 : #define HGT_I_GATE_DEFAULT 500
86 : #define MAG_I_GATE_DEFAULT 300
87 : #define MAG_CAL_DEFAULT 0
88 : #define GLITCH_RADIUS_DEFAULT 25
89 : #define FLOW_MEAS_DELAY 10
90 : #define FLOW_M_NSE_DEFAULT 0.15f
91 : #define FLOW_I_GATE_DEFAULT 500
92 : #define CHECK_SCALER_DEFAULT 150
93 : #define FLOW_USE_DEFAULT 2
94 :
95 : #else
96 : // build type not specified, use copter defaults
97 : #define VELNE_M_NSE_DEFAULT 0.5f
98 : #define VELD_M_NSE_DEFAULT 0.7f
99 : #define POSNE_M_NSE_DEFAULT 1.0f
100 : #define ALT_M_NSE_DEFAULT 3.0f
101 : #define MAG_M_NSE_DEFAULT 0.05f
102 : #define GYRO_P_NSE_DEFAULT 3.0E-02f
103 : #define ACC_P_NSE_DEFAULT 6.0E-01f
104 : #define GBIAS_P_NSE_DEFAULT 1.0E-04f
105 : #define GSCALE_P_NSE_DEFAULT 5.0E-04f
106 : #define ABIAS_P_NSE_DEFAULT 5.0E-03f
107 : #define MAGB_P_NSE_DEFAULT 1.0E-04f
108 : #define MAGE_P_NSE_DEFAULT 1.0E-03f
109 : #define VEL_I_GATE_DEFAULT 500
110 : #define POS_I_GATE_DEFAULT 500
111 : #define HGT_I_GATE_DEFAULT 500
112 : #define MAG_I_GATE_DEFAULT 300
113 : #define MAG_CAL_DEFAULT 3
114 : #define GLITCH_RADIUS_DEFAULT 25
115 : #define FLOW_MEAS_DELAY 10
116 : #define FLOW_M_NSE_DEFAULT 0.25f
117 : #define FLOW_I_GATE_DEFAULT 300
118 : #define CHECK_SCALER_DEFAULT 100
119 : #define FLOW_USE_DEFAULT 1
120 :
121 : #endif // APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_Replay)
122 :
123 : // This allows boards to default to using a specified number of IMUs and EKF lanes
124 : #ifndef HAL_EKF_IMU_MASK_DEFAULT
125 : #define HAL_EKF_IMU_MASK_DEFAULT 3 // Default to using two IMUs
126 : #endif
127 :
128 : extern const AP_HAL::HAL& hal;
129 :
130 : // Define tuning parameters
131 : const AP_Param::GroupInfo NavEKF2::var_info[] = {
132 :
133 : // @Param: ENABLE
134 : // @DisplayName: Enable EKF2
135 : // @Description: This enables EKF2. Enabling EKF2 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=2. A reboot or restart will need to be performed after changing the value of EK2_ENABLE for it to take effect.
136 : // @Values: 0:Disabled, 1:Enabled
137 : // @User: Advanced
138 : // @RebootRequired: True
139 : AP_GROUPINFO_FLAGS("ENABLE", 0, NavEKF2, _enable, 0, AP_PARAM_FLAG_ENABLE),
140 :
141 : // GPS measurement parameters
142 :
143 : // @Param: GPS_TYPE
144 : // @DisplayName: GPS mode control
145 : // @Description: This controls use of GPS measurements : 0 = use 3D velocity & 2D position, 1 = use 2D velocity and 2D position, 2 = use 2D position, 3 = Inhibit GPS use - this can be useful when flying with an optical flow sensor in an environment where GPS quality is poor and subject to large multipath errors.
146 : // @Values: 0:GPS 3D Vel and 2D Pos, 1:GPS 2D vel and 2D pos, 2:GPS 2D pos, 3:No GPS
147 : // @User: Advanced
148 : AP_GROUPINFO("GPS_TYPE", 1, NavEKF2, _fusionModeGPS, 0),
149 :
150 : // @Param: VELNE_M_NSE
151 : // @DisplayName: GPS horizontal velocity measurement noise (m/s)
152 : // @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.
153 : // @Range: 0.05 5.0
154 : // @Increment: 0.05
155 : // @User: Advanced
156 : // @Units: m/s
157 : AP_GROUPINFO("VELNE_M_NSE", 2, NavEKF2, _gpsHorizVelNoise, VELNE_M_NSE_DEFAULT),
158 :
159 : // @Param: VELD_M_NSE
160 : // @DisplayName: GPS vertical velocity measurement noise (m/s)
161 : // @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.
162 : // @Range: 0.05 5.0
163 : // @Increment: 0.05
164 : // @User: Advanced
165 : // @Units: m/s
166 : AP_GROUPINFO("VELD_M_NSE", 3, NavEKF2, _gpsVertVelNoise, VELD_M_NSE_DEFAULT),
167 :
168 : // @Param: VEL_I_GATE
169 : // @DisplayName: GPS velocity innovation gate size
170 : // @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.
171 : // @Range: 100 1000
172 : // @Increment: 25
173 : // @User: Advanced
174 : AP_GROUPINFO("VEL_I_GATE", 4, NavEKF2, _gpsVelInnovGate, VEL_I_GATE_DEFAULT),
175 :
176 : // @Param: POSNE_M_NSE
177 : // @DisplayName: GPS horizontal position measurement noise (m)
178 : // @Description: This sets the GPS horizontal position observation noise. Increasing it reduces the weighting of GPS horizontal position measurements.
179 : // @Range: 0.1 10.0
180 : // @Increment: 0.1
181 : // @User: Advanced
182 : // @Units: m
183 : AP_GROUPINFO("POSNE_M_NSE", 5, NavEKF2, _gpsHorizPosNoise, POSNE_M_NSE_DEFAULT),
184 :
185 : // @Param: POS_I_GATE
186 : // @DisplayName: GPS position measurement gate size
187 : // @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.
188 : // @Range: 100 1000
189 : // @Increment: 25
190 : // @User: Advanced
191 : AP_GROUPINFO("POS_I_GATE", 6, NavEKF2, _gpsPosInnovGate, POS_I_GATE_DEFAULT),
192 :
193 : // @Param: GLITCH_RAD
194 : // @DisplayName: GPS glitch radius gate size (m)
195 : // @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.
196 : // @Range: 10 100
197 : // @Increment: 5
198 : // @User: Advanced
199 : // @Units: m
200 : AP_GROUPINFO("GLITCH_RAD", 7, NavEKF2, _gpsGlitchRadiusMax, GLITCH_RADIUS_DEFAULT),
201 :
202 : // 8 was used for GPS_DELAY
203 :
204 : // Height measurement parameters
205 :
206 : // @Param: ALT_SOURCE
207 : // @DisplayName: Primary altitude sensor source
208 : // @Description: Primary height sensor used by the EKF. If a sensor other than Baro is selected and becomes unavailable, then the Baro sensor will be used as a fallback. NOTE: The EK2_RNG_USE_HGT parameter can be used to switch to range-finder when close to the ground in conjunction with EK2_ALT_SOURCE = 0 or 2 (Baro or GPS).
209 : // @Values: 0:Use Baro, 1:Use Range Finder, 2:Use GPS, 3:Use Range Beacon
210 : // @User: Advanced
211 : // @RebootRequired: True
212 : AP_GROUPINFO("ALT_SOURCE", 9, NavEKF2, _altSource, 0),
213 :
214 : // @Param: ALT_M_NSE
215 : // @DisplayName: Altitude measurement noise (m)
216 : // @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.
217 : // @Range: 0.1 10.0
218 : // @Increment: 0.1
219 : // @User: Advanced
220 : // @Units: m
221 : AP_GROUPINFO("ALT_M_NSE", 10, NavEKF2, _baroAltNoise, ALT_M_NSE_DEFAULT),
222 :
223 : // @Param: HGT_I_GATE
224 : // @DisplayName: Height measurement gate size
225 : // @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.
226 : // @Range: 100 1000
227 : // @Increment: 25
228 : // @User: Advanced
229 : AP_GROUPINFO("HGT_I_GATE", 11, NavEKF2, _hgtInnovGate, HGT_I_GATE_DEFAULT),
230 :
231 : // @Param: HGT_DELAY
232 : // @DisplayName: Height measurement delay (msec)
233 : // @Description: This is the number of msec that the Height measurements lag behind the inertial measurements.
234 : // @Range: 0 250
235 : // @Increment: 10
236 : // @User: Advanced
237 : // @Units: ms
238 : // @RebootRequired: True
239 : AP_GROUPINFO("HGT_DELAY", 12, NavEKF2, _hgtDelay_ms, 60),
240 :
241 : // Magnetometer measurement parameters
242 :
243 : // @Param: MAG_M_NSE
244 : // @DisplayName: Magnetometer measurement noise (Gauss)
245 : // @Description: This is the RMS value of noise in magnetometer measurements. Increasing it reduces the weighting on these measurements.
246 : // @Range: 0.01 0.5
247 : // @Increment: 0.01
248 : // @User: Advanced
249 : // @Units: Gauss
250 : AP_GROUPINFO("MAG_M_NSE", 13, NavEKF2, _magNoise, MAG_M_NSE_DEFAULT),
251 :
252 : // @Param: MAG_CAL
253 : // @DisplayName: Magnetometer default fusion mode
254 : // @Description: This determines when the filter will use the 3-axis magnetometer fusion model that estimates both earth and body fixed magnetic field states, when it will use a simpler magnetic heading fusion model that does not use magnetic field states and when it will use an alternative method of yaw determination to the magnetometer. The 3-axis magnetometer fusion is only suitable for use when the external magnetic field environment is stable. EK2_MAG_CAL = 0 uses heading fusion on ground, 3-axis fusion in-flight, and is the default setting for Plane users. EK2_MAG_CAL = 1 uses 3-axis fusion only when manoeuvring. EK2_MAG_CAL = 2 uses heading fusion at all times, is recommended if the external magnetic field is varying and is the default for rovers. EK2_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. EK2_MAG_CAL = 4 uses 3-axis fusion at all times. NOTE: The fusion mode can be forced to 2 for specific EKF cores using the EK2_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 with COMPASS_ENABLE set to 1. If this is done, the EK2_GSF_RUN and EK2_GSF_USE masks must be set to the same as EK2_IMU_MASK.
255 : // @Values: 0:When flying,1:When manoeuvring,2:Never,3:After first climb yaw reset,4:Always
256 : // @User: Advanced
257 : AP_GROUPINFO("MAG_CAL", 14, NavEKF2, _magCal, MAG_CAL_DEFAULT),
258 :
259 : // @Param: MAG_I_GATE
260 : // @DisplayName: Magnetometer measurement gate size
261 : // @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.
262 : // @Range: 100 1000
263 : // @Increment: 25
264 : // @User: Advanced
265 : AP_GROUPINFO("MAG_I_GATE", 15, NavEKF2, _magInnovGate, MAG_I_GATE_DEFAULT),
266 :
267 : // Airspeed measurement parameters
268 :
269 : // @Param: EAS_M_NSE
270 : // @DisplayName: Equivalent airspeed measurement noise (m/s)
271 : // @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.
272 : // @Range: 0.5 5.0
273 : // @Increment: 0.1
274 : // @User: Advanced
275 : // @Units: m/s
276 : AP_GROUPINFO("EAS_M_NSE", 16, NavEKF2, _easNoise, 1.4f),
277 :
278 : // @Param: EAS_I_GATE
279 : // @DisplayName: Airspeed measurement gate size
280 : // @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.
281 : // @Range: 100 1000
282 : // @Increment: 25
283 : // @User: Advanced
284 : AP_GROUPINFO("EAS_I_GATE", 17, NavEKF2, _tasInnovGate, 400),
285 :
286 : // Rangefinder measurement parameters
287 :
288 : // @Param: RNG_M_NSE
289 : // @DisplayName: Range finder measurement noise (m)
290 : // @Description: This is the RMS value of noise in the range finder measurement. Increasing it reduces the weighting on this measurement.
291 : // @Range: 0.1 10.0
292 : // @Increment: 0.1
293 : // @User: Advanced
294 : // @Units: m
295 : AP_GROUPINFO("RNG_M_NSE", 18, NavEKF2, _rngNoise, 0.5f),
296 :
297 : // @Param: RNG_I_GATE
298 : // @DisplayName: Range finder measurement gate size
299 : // @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.
300 : // @Range: 100 1000
301 : // @Increment: 25
302 : // @User: Advanced
303 : AP_GROUPINFO("RNG_I_GATE", 19, NavEKF2, _rngInnovGate, 500),
304 :
305 : // Optical flow measurement parameters
306 :
307 : // @Param: MAX_FLOW
308 : // @DisplayName: Maximum valid optical flow rate
309 : // @Description: This sets the magnitude maximum optical flow rate in rad/sec that will be accepted by the filter
310 : // @Range: 1.0 4.0
311 : // @Increment: 0.1
312 : // @User: Advanced
313 : // @Units: rad/s
314 : AP_GROUPINFO("MAX_FLOW", 20, NavEKF2, _maxFlowRate, 2.5f),
315 :
316 : // @Param: FLOW_M_NSE
317 : // @DisplayName: Optical flow measurement noise (rad/s)
318 : // @Description: This is the RMS value of noise and errors in optical flow measurements. Increasing it reduces the weighting on these measurements.
319 : // @Range: 0.05 1.0
320 : // @Increment: 0.05
321 : // @User: Advanced
322 : // @Units: rad/s
323 : AP_GROUPINFO("FLOW_M_NSE", 21, NavEKF2, _flowNoise, FLOW_M_NSE_DEFAULT),
324 :
325 : // @Param: FLOW_I_GATE
326 : // @DisplayName: Optical Flow measurement gate size
327 : // @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.
328 : // @Range: 100 1000
329 : // @Increment: 25
330 : // @User: Advanced
331 : AP_GROUPINFO("FLOW_I_GATE", 22, NavEKF2, _flowInnovGate, FLOW_I_GATE_DEFAULT),
332 :
333 : // @Param: FLOW_DELAY
334 : // @DisplayName: Optical Flow measurement delay (msec)
335 : // @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.
336 : // @Range: 0 127
337 : // @Increment: 10
338 : // @User: Advanced
339 : // @Units: ms
340 : // @RebootRequired: True
341 : AP_GROUPINFO("FLOW_DELAY", 23, NavEKF2, _flowDelay_ms, FLOW_MEAS_DELAY),
342 :
343 : // State and Covariance Predition Parameters
344 :
345 : // @Param: GYRO_P_NSE
346 : // @DisplayName: Rate gyro noise (rad/s)
347 : // @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.
348 : // @Range: 0.0001 0.1
349 : // @Increment: 0.0001
350 : // @User: Advanced
351 : // @Units: rad/s
352 : AP_GROUPINFO("GYRO_P_NSE", 24, NavEKF2, _gyrNoise, GYRO_P_NSE_DEFAULT),
353 :
354 : // @Param: ACC_P_NSE
355 : // @DisplayName: Accelerometer noise (m/s^2)
356 : // @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.
357 : // @Range: 0.01 1.0
358 : // @Increment: 0.01
359 : // @User: Advanced
360 : // @Units: m/s/s
361 : AP_GROUPINFO("ACC_P_NSE", 25, NavEKF2, _accNoise, ACC_P_NSE_DEFAULT),
362 :
363 : // @Param: GBIAS_P_NSE
364 : // @DisplayName: Rate gyro bias stability (rad/s/s)
365 : // @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.
366 : // @Range: 0.00001 0.001
367 : // @User: Advanced
368 : // @Units: rad/s/s
369 : AP_GROUPINFO("GBIAS_P_NSE", 26, NavEKF2, _gyroBiasProcessNoise, GBIAS_P_NSE_DEFAULT),
370 :
371 : // @Param: GSCL_P_NSE
372 : // @DisplayName: Rate gyro scale factor stability (1/s)
373 : // @Description: This noise controls the rate of gyro scale factor learning. Increasing it makes rate gyro scale factor estimation faster and noisier.
374 : // @Range: 0.000001 0.001
375 : // @User: Advanced
376 : // @Units: Hz
377 : AP_GROUPINFO("GSCL_P_NSE", 27, NavEKF2, _gyroScaleProcessNoise, GSCALE_P_NSE_DEFAULT),
378 :
379 : // @Param: ABIAS_P_NSE
380 : // @DisplayName: Accelerometer bias stability (m/s^3)
381 : // @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.
382 : // @Range: 0.00001 0.005
383 : // @User: Advanced
384 : // @Units: m/s/s/s
385 : AP_GROUPINFO("ABIAS_P_NSE", 28, NavEKF2, _accelBiasProcessNoise, ABIAS_P_NSE_DEFAULT),
386 :
387 : // 29 previously used for EK2_MAG_P_NSE parameter that has been replaced with EK2_MAGE_P_NSE and EK2_MAGB_P_NSE
388 :
389 : // @Param: WIND_P_NSE
390 : // @DisplayName: Wind velocity process noise (m/s^2)
391 : // @Description: This state process noise controls the growth of wind state error estimates. Increasing it makes wind estimation faster and noisier.
392 : // @Range: 0.01 1.0
393 : // @Increment: 0.1
394 : // @User: Advanced
395 : // @Units: m/s/s
396 : AP_GROUPINFO("WIND_P_NSE", 30, NavEKF2, _windVelProcessNoise, 0.1f),
397 :
398 : // @Param: WIND_PSCALE
399 : // @DisplayName: Height rate to wind process noise scaler
400 : // @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.
401 : // @Range: 0.0 1.0
402 : // @Increment: 0.1
403 : // @User: Advanced
404 : AP_GROUPINFO("WIND_PSCALE", 31, NavEKF2, _wndVarHgtRateScale, 0.5f),
405 :
406 : // @Param: GPS_CHECK
407 : // @DisplayName: GPS preflight check
408 : // @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.
409 : // @Bitmask: 0:NSats,1:HDoP,2:speed error,3:position error,4:yaw error,5:pos drift,6:vert speed,7:horiz speed
410 : // @User: Advanced
411 : AP_GROUPINFO("GPS_CHECK", 32, NavEKF2, _gpsCheck, 31),
412 :
413 : // @Param: IMU_MASK
414 : // @DisplayName: Bitmask of active IMUs
415 : // @Description: 1 byte bitmap of IMUs to use in EKF2. A separate instance of EKF2 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 EKF2 will fail to start.
416 : // @Bitmask: 0:FirstIMU,1:SecondIMU,2:ThirdIMU,3:FourthIMU,4:FifthIMU,5:SixthIMU
417 : // @User: Advanced
418 : // @RebootRequired: True
419 : AP_GROUPINFO("IMU_MASK", 33, NavEKF2, _imuMask, HAL_EKF_IMU_MASK_DEFAULT),
420 :
421 : // @Param: CHECK_SCALE
422 : // @DisplayName: GPS accuracy check scaler (%)
423 : // @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.
424 : // @Range: 50 200
425 : // @User: Advanced
426 : // @Units: %
427 : AP_GROUPINFO("CHECK_SCALE", 34, NavEKF2, _gpsCheckScaler, CHECK_SCALER_DEFAULT),
428 :
429 : // @Param: NOAID_M_NSE
430 : // @DisplayName: Non-GPS operation position uncertainty (m)
431 : // @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.
432 : // @Range: 0.5 50.0
433 : // @User: Advanced
434 : // @Units: m
435 : AP_GROUPINFO("NOAID_M_NSE", 35, NavEKF2, _noaidHorizNoise, 10.0f),
436 :
437 : // 36 was LOG_MASK, used for specifying which IMUs/cores to log
438 : // replay data for
439 :
440 : // control of magnetic yaw angle fusion
441 :
442 : // @Param: YAW_M_NSE
443 : // @DisplayName: Yaw measurement noise (rad)
444 : // @Description: This is the RMS value of noise in yaw measurements from the magnetometer. Increasing it reduces the weighting on these measurements.
445 : // @Range: 0.05 1.0
446 : // @Increment: 0.05
447 : // @User: Advanced
448 : // @Units: rad
449 : AP_GROUPINFO("YAW_M_NSE", 37, NavEKF2, _yawNoise, 0.5f),
450 :
451 : // @Param: YAW_I_GATE
452 : // @DisplayName: Yaw measurement gate size
453 : // @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.
454 : // @Range: 100 1000
455 : // @Increment: 25
456 : // @User: Advanced
457 : AP_GROUPINFO("YAW_I_GATE", 38, NavEKF2, _yawInnovGate, 300),
458 :
459 : // @Param: TAU_OUTPUT
460 : // @DisplayName: Output complementary filter time constant (centi-sec)
461 : // @Description: Sets the time constant of the output complementary filter/predictor in centi-seconds.
462 : // @Range: 10 50
463 : // @Increment: 5
464 : // @User: Advanced
465 : // @Units: cs
466 : AP_GROUPINFO("TAU_OUTPUT", 39, NavEKF2, _tauVelPosOutput, 25),
467 :
468 : // @Param: MAGE_P_NSE
469 : // @DisplayName: Earth magnetic field process noise (gauss/s)
470 : // @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.
471 : // @Range: 0.00001 0.01
472 : // @User: Advanced
473 : // @Units: Gauss/s
474 : AP_GROUPINFO("MAGE_P_NSE", 40, NavEKF2, _magEarthProcessNoise, MAGE_P_NSE_DEFAULT),
475 :
476 : // @Param: MAGB_P_NSE
477 : // @DisplayName: Body magnetic field process noise (gauss/s)
478 : // @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.
479 : // @Range: 0.00001 0.01
480 : // @User: Advanced
481 : // @Units: Gauss/s
482 : AP_GROUPINFO("MAGB_P_NSE", 41, NavEKF2, _magBodyProcessNoise, MAGB_P_NSE_DEFAULT),
483 :
484 : // @Param: RNG_USE_HGT
485 : // @DisplayName: Range finder switch height percentage
486 : // @Description: Range finder can be used as the primary height source when below this percentage of its maximum range (see RNGFND*_MAX). This will not work unless Baro or GPS height is selected as the primary height source vis EK2_ALT_SOURCE = 0 or 2 respectively. 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.
487 : // @Range: -1 70
488 : // @Increment: 1
489 : // @User: Advanced
490 : // @Units: %
491 : AP_GROUPINFO("RNG_USE_HGT", 42, NavEKF2, _useRngSwHgt, -1),
492 :
493 : // @Param: TERR_GRAD
494 : // @DisplayName: Maximum terrain gradient
495 : // @Description: Specifies the maximum gradient of the terrain below the vehicle assumed when it is fusing range finder or optical flow to estimate terrain height.
496 : // @Range: 0 0.2
497 : // @Increment: 0.01
498 : // @User: Advanced
499 : AP_GROUPINFO("TERR_GRAD", 43, NavEKF2, _terrGradMax, 0.1f),
500 :
501 : // @Param: BCN_M_NSE
502 : // @DisplayName: Range beacon measurement noise (m)
503 : // @Description: This is the RMS value of noise in the range beacon measurement. Increasing it reduces the weighting on this measurement.
504 : // @Range: 0.1 10.0
505 : // @Increment: 0.1
506 : // @User: Advanced
507 : // @Units: m
508 : AP_GROUPINFO("BCN_M_NSE", 44, NavEKF2, _rngBcnNoise, 1.0f),
509 :
510 : // @Param: BCN_I_GTE
511 : // @DisplayName: Range beacon measurement gate size
512 : // @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.
513 : // @Range: 100 1000
514 : // @Increment: 25
515 : // @User: Advanced
516 : AP_GROUPINFO("BCN_I_GTE", 45, NavEKF2, _rngBcnInnovGate, 500),
517 :
518 : // @Param: BCN_DELAY
519 : // @DisplayName: Range beacon measurement delay (msec)
520 : // @Description: This is the number of msec that the range beacon 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.
521 : // @Range: 0 127
522 : // @Increment: 10
523 : // @User: Advanced
524 : // @Units: ms
525 : // @RebootRequired: True
526 : AP_GROUPINFO("BCN_DELAY", 46, NavEKF2, _rngBcnDelay_ms, 50),
527 :
528 : // @Param: RNG_USE_SPD
529 : // @DisplayName: Range finder max ground speed
530 : // @Description: The range finder will not be used as the primary height source when the horizontal ground speed is greater than this value.
531 : // @Range: 2.0 6.0
532 : // @Increment: 0.5
533 : // @User: Advanced
534 : // @Units: m/s
535 : AP_GROUPINFO("RNG_USE_SPD", 47, NavEKF2, _useRngSwSpd, 2.0f),
536 :
537 : // @Param: MAG_MASK
538 : // @DisplayName: Bitmask of active EKF cores that will always use heading fusion
539 : // @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.
540 : // @Bitmask: 0:FirstEKF,1:SecondEKF,2:ThirdEKF,3:FourthEKF,4:FifthEKF,5:SixthEKF
541 : // @User: Advanced
542 : // @RebootRequired: True
543 : AP_GROUPINFO("MAG_MASK", 48, NavEKF2, _magMask, 0),
544 :
545 : // @Param: OGN_HGT_MASK
546 : // @DisplayName: Bitmask control of EKF reference height correction
547 : // @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).
548 : // @Bitmask: 0:Correct when using Baro height,1:Correct when using range finder height,2:Apply corrections to local position
549 : // @User: Advanced
550 : // @RebootRequired: True
551 : AP_GROUPINFO("OGN_HGT_MASK", 49, NavEKF2, _originHgtMode, 0),
552 :
553 : // EXTNAV_DELAY was 50
554 :
555 : // @Param: FLOW_USE
556 : // @DisplayName: Optical flow use bitmask
557 : // @Description: Controls if the optical flow data is fused into the 24-state navigation estimator OR the 1-state terrain height estimator.
558 : // @User: Advanced
559 : // @Values: 0:None,1:Navigation,2:Terrain
560 : // @RebootRequired: True
561 : AP_GROUPINFO("FLOW_USE", 51, NavEKF2, _flowUse, FLOW_USE_DEFAULT),
562 :
563 : // @Param: MAG_EF_LIM
564 : // @DisplayName: EarthField error limit
565 : // @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.
566 : // @User: Advanced
567 : // @Range: 0 500
568 : // @Units: mGauss
569 : AP_GROUPINFO("MAG_EF_LIM", 52, NavEKF2, _mag_ef_limit, 50),
570 :
571 : // @Param: HRT_FILT
572 : // @DisplayName: Height rate filter crossover frequency
573 : // @Description: Specifies the crossover frequency of the complementary filter used to calculate the output predictor height rate derivative.
574 : // @Range: 0.1 30.0
575 : // @Units: Hz
576 : AP_GROUPINFO("HRT_FILT", 53, NavEKF2, _hrt_filt_freq, 2.0f),
577 :
578 : // @Param: GSF_RUN_MASK
579 : // @DisplayName: Bitmask of which EKF-GSF yaw estimators run
580 : // @Description: A bitmask of which EKF2 instances run an independant 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 EKF2 instance will be logged as GSF0 and GSF1 messages. Use of the yaw estimate generated by this algorithm is controlled by the EK2_GSF_USE_MASK and EK2_GSF_RST_MAX parameters. To run the EKF-GSF yaw estimator in ride-along and logging only, set EK2_GSF_USE_MASK to 0.
581 : // @Bitmask: 0:FirstEKF,1:SecondEKF,2:ThirdEKF,3:FourthEKF,4:FifthEKF,5:SixthEKF
582 : // @User: Advanced
583 : // @RebootRequired: True
584 : AP_GROUPINFO("GSF_RUN_MASK", 54, NavEKF2, _gsfRunMask, 3),
585 :
586 : // @Param: GSF_USE_MASK
587 : // @DisplayName: Bitmask of which EKF-GSF yaw estimators are used
588 : // @Description: 1 byte bitmap of which EKF2 instances will use the output from the EKF-GSF yaw estimator that has been turned on by the EK2_GSF_RUN_MASK parameter. If the inertial navigation calculation stops following the GPS, then the vehicle code can request EKF2 to attempt to resolve the issue, either by performing a yaw reset if enabled by this parameter by switching to another EKF2 instance.
589 : // @Bitmask: 0:FirstEKF,1:SecondEKF,2:ThirdEKF,3:FourthEKF,4:FifthEKF,5:SixthEKF
590 : // @User: Advanced
591 : // @RebootRequired: True
592 : AP_GROUPINFO("GSF_USE_MASK", 55, NavEKF2, _gsfUseMask, 3),
593 :
594 : // 56 was GSF_DELAY which was never released in a stable version
595 :
596 : // @Param: GSF_RST_MAX
597 : // @DisplayName: Maximum number of resets to the EKF-GSF yaw estimate allowed
598 : // @Description: Sets the maximum number of times the EKF2 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 EK2_GSF_USE_MASK parameter.
599 : // @Range: 1 10
600 : // @Increment: 1
601 : // @User: Advanced
602 : // @RebootRequired: True
603 : AP_GROUPINFO("GSF_RST_MAX", 57, NavEKF2, _gsfResetMaxCount, 2),
604 :
605 : // @Param: OPTIONS
606 : // @DisplayName: Optional EKF behaviour
607 : // @Description: optional EKF2 behaviour. Disabling external navigation prevents use of external vision data in the EKF2 solution
608 : // @Bitmask: 0:DisableExternalNavigation
609 : // @User: Advanced
610 : AP_GROUPINFO("OPTIONS", 58, NavEKF2, _options, 0),
611 :
612 : AP_GROUPEND
613 : };
614 :
615 2145 : NavEKF2::NavEKF2()
616 : {
617 2145 : AP_Param::setup_object_defaults(this, var_info);
618 2145 : }
619 :
620 :
621 : // Initialise the filter
622 39049668 : bool NavEKF2::InitialiseFilter(void)
623 : {
624 39049668 : AP::dal().start_frame(AP_DAL::FrameType::InitialiseFilterEKF2);
625 :
626 : // Return immediately if there is insufficient memory
627 39049668 : if (core_malloc_failed) {
628 : return false;
629 : }
630 :
631 39049668 : initFailure = InitFailures::UNKNOWN;
632 39049668 : if (_enable == 0) {
633 38945869 : if (AP::dal().configured_ekf_type() == 2) {
634 96405 : initFailure = InitFailures::NO_ENABLE;
635 : }
636 38945869 : return false;
637 : }
638 103799 : const auto &ins = AP::dal().ins();
639 :
640 103799 : imuSampleTime_us = AP::dal().micros64();
641 :
642 : // remember expected frame time
643 103799 : const float loop_rate = ins.get_loop_rate_hz();
644 103799 : if (!is_positive(loop_rate)) {
645 : return false;
646 : }
647 103799 : _frameTimeUsec = 1e6 / loop_rate;
648 :
649 : // expected number of IMU frames per prediction
650 103799 : _framesPerPrediction = uint8_t((EKF_TARGET_DT / (_frameTimeUsec * 1.0e-6) + 0.5));
651 :
652 103799 : if (core == nullptr) {
653 :
654 : // don't allow more filters than IMUs
655 511 : uint8_t mask = (1U<<ins.get_accel_count())-1;
656 511 : _imuMask.set_and_default(_imuMask.get() & mask);
657 :
658 : // count IMUs from mask
659 511 : num_cores = __builtin_popcount(_imuMask);
660 :
661 : // abort if num_cores is zero
662 511 : if (num_cores == 0) {
663 0 : if (ins.get_accel_count() == 0) {
664 0 : initFailure = InitFailures::NO_IMUS;
665 : } else {
666 0 : initFailure = InitFailures::NO_MASK;
667 : }
668 0 : return false;
669 : }
670 :
671 : // check if there is enough memory to create the EKF cores
672 511 : if (AP::dal().available_memory() < sizeof(NavEKF2_core)*num_cores + 4096) {
673 0 : initFailure = InitFailures::NO_MEM;
674 0 : core_malloc_failed = true;
675 0 : num_cores = 0;
676 0 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "NavEKF2: not enough memory available");
677 0 : return false;
678 : }
679 :
680 : // try to allocate from CCM RAM, fallback to Normal RAM if not available or full
681 511 : core = (NavEKF2_core*)AP::dal().malloc_type(sizeof(NavEKF2_core)*num_cores, AP_DAL::MemoryType::FAST);
682 511 : if (core == nullptr) {
683 0 : initFailure = InitFailures::NO_MEM;
684 0 : core_malloc_failed = true;
685 0 : num_cores = 0;
686 0 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "NavEKF2: memory allocation failed");
687 0 : return false;
688 : }
689 :
690 : //Call Constructors on all cores
691 1533 : for (uint8_t i = 0; i < num_cores; i++) {
692 1022 : new (&core[i]) NavEKF2_core(this);
693 : }
694 :
695 : // set the IMU index for the cores
696 511 : num_cores = 0;
697 4088 : for (uint8_t i=0; i<7; i++) {
698 3577 : if (_imuMask & (1U<<i)) {
699 1022 : if(!core[num_cores].setup_core(i, num_cores)) {
700 : // if any core setup fails, free memory, zero the core pointer and abort
701 0 : AP::dal().free_type(core, sizeof(NavEKF2_core)*num_cores, AP_DAL::MemoryType::FAST);
702 0 : core = nullptr;
703 0 : initFailure = InitFailures::NO_SETUP;
704 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "NavEKF2: core %d setup failed", num_cores);
705 0 : return false;
706 : }
707 1022 : num_cores++;
708 : }
709 : }
710 :
711 : // Set the primary initially to be the lowest index
712 511 : primary = 0;
713 : }
714 :
715 : // invalidate shared origin
716 103799 : common_origin_valid = false;
717 :
718 : // initialise the cores. We return success only if all cores
719 : // initialise successfully
720 : bool ret = true;
721 311397 : for (uint8_t i=0; i<num_cores; i++) {
722 207598 : ret &= core[i].InitialiseFilterBootstrap();
723 : }
724 :
725 : // zero the structs used capture reset events
726 103799 : memset(&yaw_reset_data, 0, sizeof(yaw_reset_data));
727 103799 : memset((void *)&pos_reset_data, 0, sizeof(pos_reset_data));
728 103799 : memset(&pos_down_reset_data, 0, sizeof(pos_down_reset_data));
729 :
730 103799 : return ret;
731 : }
732 :
733 : /*
734 : return true if a new core index has a better score than the current
735 : core
736 : */
737 40561 : bool NavEKF2::coreBetterScore(uint8_t new_core, uint8_t current_core) const
738 : {
739 40561 : const NavEKF2_core &oldCore = core[current_core];
740 40561 : const NavEKF2_core &newCore = core[new_core];
741 40561 : if (!newCore.healthy()) {
742 : // never consider a new core that isn't healthy
743 : return false;
744 : }
745 33357 : if (newCore.have_aligned_tilt() != oldCore.have_aligned_tilt()) {
746 : // tilt alignment is most critical, if one is tilt aligned and
747 : // the other isn't then use the tilt aligned lane
748 : return newCore.have_aligned_tilt();
749 : }
750 33357 : if (newCore.have_aligned_yaw() != oldCore.have_aligned_yaw()) {
751 : // yaw alignment is next most critical, if one is yaw aligned
752 : // and the other isn't then use the yaw aligned lane
753 : return newCore.have_aligned_yaw();
754 : }
755 : // if both cores are aligned then look at error scores
756 33357 : return newCore.errorScore() < oldCore.errorScore();
757 : }
758 :
759 : // Update Filter States - this should be called whenever new IMU data is available
760 2106892 : void NavEKF2::UpdateFilter(void)
761 : {
762 2106892 : AP::dal().start_frame(AP_DAL::FrameType::UpdateFilterEKF2);
763 :
764 2106892 : if (!core) {
765 0 : return;
766 : }
767 :
768 2106892 : imuSampleTime_us = AP::dal().micros64();
769 :
770 2106892 : bool statePredictEnabled[num_cores];
771 6320676 : for (uint8_t i=0; i<num_cores; i++) {
772 : // if we have not overrun by more than 3 IMU frames, and we
773 : // have already used more than 1/3 of the CPU budget for this
774 : // loop then suppress the prediction step. This allows
775 : // multiple EKF instances to cooperate on scheduling
776 8427568 : if (core[i].getFramesSincePredict() < (_framesPerPrediction+3) &&
777 4213784 : AP::dal().ekf_low_time_remaining(AP_DAL::EKFType::EKF2, i)) {
778 0 : statePredictEnabled[i] = false;
779 : } else {
780 4213784 : statePredictEnabled[i] = true;
781 : }
782 4213784 : core[i].UpdateFilter(statePredictEnabled[i]);
783 : }
784 :
785 : // If the current core selected has a bad error score or is unhealthy, switch to a healthy core with the lowest fault score
786 : // Don't start running the check until the primary core has started returned healthy for at least 10 seconds to avoid switching
787 : // due to initial alignment fluctuations and race conditions
788 2106892 : if (!runCoreSelection) {
789 : static uint64_t lastUnhealthyTime_us = 0;
790 172396 : if (!core[primary].healthy() || lastUnhealthyTime_us == 0) {
791 14594 : lastUnhealthyTime_us = imuSampleTime_us;
792 : }
793 172396 : runCoreSelection = (imuSampleTime_us - lastUnhealthyTime_us) > 1E7;
794 : }
795 2106892 : float primaryErrorScore = core[primary].errorScore();
796 2106892 : if ((primaryErrorScore > 1.0f || !core[primary].healthy()) && runCoreSelection) {
797 40560 : float lowestErrorScore = 0.67f * primaryErrorScore;
798 40560 : uint8_t newPrimaryIndex = primary; // index for new primary
799 121680 : for (uint8_t coreIndex=0; coreIndex<num_cores; coreIndex++) {
800 :
801 81120 : if (coreIndex != primary) {
802 : // an alternative core is available for selection only if healthy and if states have been updated on this time step
803 40560 : bool altCoreAvailable = statePredictEnabled[coreIndex] && coreBetterScore(coreIndex, newPrimaryIndex);
804 :
805 : // If the primary core is unhealthy and another core is available, then switch now
806 : // If the primary core is still healthy,then switching is optional and will only be done if
807 : // a core with a significantly lower error score can be found
808 40560 : float altErrorScore = core[coreIndex].errorScore();
809 40560 : if (altCoreAvailable && (!core[newPrimaryIndex].healthy() || altErrorScore < lowestErrorScore)) {
810 : newPrimaryIndex = coreIndex;
811 : lowestErrorScore = altErrorScore;
812 : }
813 : }
814 : }
815 : // update the yaw and position reset data to capture changes due to the lane switch
816 40560 : if (newPrimaryIndex != primary) {
817 6187 : updateLaneSwitchYawResetData(newPrimaryIndex);
818 6187 : updateLaneSwitchPosResetData(newPrimaryIndex);
819 6187 : updateLaneSwitchPosDownResetData(newPrimaryIndex);
820 6187 : primary = newPrimaryIndex;
821 6187 : lastLaneSwitch_ms = AP::dal().millis();
822 : }
823 : }
824 :
825 2106892 : if (primary != 0 && core[0].healthy() && !AP::dal().get_armed()) {
826 : // when on the ground and disarmed force the first lane. This
827 : // avoids us ending with with a lottery for which IMU is used
828 : // in each flight. Otherwise the alignment of the timing of
829 : // the lane updates with the timing of GPS updates can lead to
830 : // a lane other than the first one being used as primary for
831 : // some flights. As different IMUs may have quite different
832 : // noise characteristics this leads to inconsistent
833 : // performance
834 6183 : primary = 0;
835 : }
836 2106892 : }
837 :
838 : /*
839 : check if switching lanes will reduce the normalised
840 : innovations. This is called when the vehicle code is about to
841 : trigger an EKF failsafe, and it would like to avoid that by
842 : using a different EKF lane
843 : */
844 1 : void NavEKF2::checkLaneSwitch(void)
845 : {
846 1 : AP::dal().log_event2(AP_DAL::Event::checkLaneSwitch);
847 1 : const uint32_t now = AP::dal().millis();
848 1 : if (lastLaneSwitch_ms != 0 && now - lastLaneSwitch_ms < 5000) {
849 : // don't switch twice in 5 seconds
850 : return;
851 : }
852 1 : uint8_t newPrimaryIndex = primary;
853 3 : for (uint8_t coreIndex=0; coreIndex<num_cores; coreIndex++) {
854 2 : if (coreIndex != primary) {
855 1 : const NavEKF2_core &newCore = core[coreIndex];
856 : // an alternative core is available for selection only if healthy and if states have been updated on this time step
857 1 : bool altCoreAvailable = coreBetterScore(coreIndex, newPrimaryIndex);
858 1 : if (altCoreAvailable && newCore.errorScore() < 0.9) {
859 : newPrimaryIndex = coreIndex;
860 : }
861 : }
862 : }
863 :
864 : // update the yaw and position reset data to capture changes due to the lane switch
865 1 : if (newPrimaryIndex != primary) {
866 0 : updateLaneSwitchYawResetData(newPrimaryIndex);
867 0 : updateLaneSwitchPosResetData(newPrimaryIndex);
868 0 : updateLaneSwitchPosDownResetData(newPrimaryIndex);
869 0 : primary = newPrimaryIndex;
870 0 : lastLaneSwitch_ms = now;
871 0 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "NavEKF2: lane switch %u", primary);
872 : }
873 : }
874 :
875 : // Check basic filter health metrics and return a consolidated health status
876 2106892 : bool NavEKF2::healthy(void) const
877 : {
878 2106892 : if (!core) {
879 : return false;
880 : }
881 2106892 : return core[primary].healthy();
882 : }
883 :
884 : // returns false if we fail arming checks, in which case the buffer will be populated with a failure message
885 80 : bool NavEKF2::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const
886 : {
887 80 : if (!core) {
888 0 : AP::dal().snprintf(failure_msg, failure_msg_len, "no EKF2 cores");
889 0 : return false;
890 : }
891 240 : for (uint8_t i = 0; i < num_cores; i++) {
892 160 : if (!core[i].healthy()) {
893 0 : const char *failure = core[i].prearm_failure_reason();
894 0 : if (failure != nullptr) {
895 0 : AP::dal().snprintf(failure_msg, failure_msg_len, failure);
896 : } else {
897 0 : AP::dal().snprintf(failure_msg, failure_msg_len, "EKF2 core %d unhealthy", (int)i);
898 : }
899 0 : return false;
900 : }
901 : }
902 : return true;
903 : }
904 :
905 : // returns the index of the primary core
906 : // return -1 if no primary core selected
907 3673988 : int8_t NavEKF2::getPrimaryCoreIndex(void) const
908 : {
909 3673988 : if (!core) {
910 : return -1;
911 : }
912 3673988 : return primary;
913 : }
914 :
915 : // returns the index of the IMU of the primary core
916 : // return -1 if no primary core selected
917 83373432 : int8_t NavEKF2::getPrimaryCoreIMUIndex(void) const
918 : {
919 83373432 : if (!core) {
920 : return -1;
921 : }
922 4420898 : return core[primary].getIMUIndex();
923 : }
924 :
925 : // Write the last calculated NE position relative to the reference point (m).
926 : // If a calculated solution is not available, use the best available data and return false
927 : // If false returned, do not use for flight control
928 41686716 : bool NavEKF2::getPosNE(Vector2p &posNE) const
929 : {
930 41686716 : if (!core) {
931 : return false;
932 : }
933 2210449 : return core[primary].getPosNE(posNE);
934 : }
935 :
936 : // Write the last calculated D position relative to the reference point (m).
937 : // If a calculated solution is not available, use the best available data and return false
938 : // If false returned, do not use for flight control
939 41686716 : bool NavEKF2::getPosD(postype_t &posD) const
940 : {
941 41686716 : if (!core) {
942 : return false;
943 : }
944 2210449 : return core[primary].getPosD(posD);
945 : }
946 :
947 : // return NED velocity in m/s
948 41686716 : void NavEKF2::getVelNED(Vector3f &vel) const
949 : {
950 41686716 : if (core) {
951 2210449 : core[primary].getVelNED(vel);
952 : }
953 41686716 : }
954 :
955 : // return estimate of true airspeed vector in body frame in m/s for the specified instance
956 : // An out of range instance (eg -1) returns data for the primary instance
957 : // returns false if estimate is unavailable
958 135980 : bool NavEKF2::getAirSpdVec(Vector3f &vel) const
959 : {
960 135980 : if (core) {
961 135980 : return core[primary].getAirSpdVec(vel);
962 : }
963 : return false;
964 : }
965 :
966 : // Return the rate of change of vertical position in the down direction (dPosD/dt) in m/s
967 41686716 : float NavEKF2::getPosDownDerivative() const
968 : {
969 : // return the value calculated from a complementary filter applied to the EKF height and vertical acceleration
970 41686716 : if (core) {
971 2210449 : return core[primary].getPosDownDerivative();
972 : }
973 : return 0.0f;
974 : }
975 :
976 : // return body axis gyro bias estimates in rad/sec
977 41686716 : void NavEKF2::getGyroBias(Vector3f &gyroBias) const
978 : {
979 41686716 : if (core) {
980 2210449 : core[primary].getGyroBias(gyroBias);
981 : }
982 41686716 : }
983 :
984 : // reset body axis gyro bias estimates
985 7 : void NavEKF2::resetGyroBias(void)
986 : {
987 7 : AP::dal().log_event2(AP_DAL::Event::resetGyroBias);
988 :
989 7 : if (core) {
990 18 : for (uint8_t i=0; i<num_cores; i++) {
991 12 : core[i].resetGyroBias();
992 : }
993 : }
994 7 : }
995 :
996 : // Resets the baro so that it reads zero at the current height
997 : // Resets the EKF height to zero
998 : // Adjusts the EKf origin height so that the EKF height + origin height is the same as before
999 : // Returns true if the height datum reset has been performed
1000 : // If using a range finder for height no reset is performed and it returns false
1001 3121 : bool NavEKF2::resetHeightDatum(void)
1002 : {
1003 3121 : AP::dal().log_event2(AP_DAL::Event::resetHeightDatum);
1004 :
1005 : bool status = true;
1006 3121 : if (core) {
1007 4488 : for (uint8_t i=0; i<num_cores; i++) {
1008 2992 : if (!core[i].resetHeightDatum()) {
1009 : status = false;
1010 : }
1011 : }
1012 : } else {
1013 : status = false;
1014 : }
1015 3121 : return status;
1016 : }
1017 :
1018 : // return the horizontal speed limit in m/s set by optical flow sensor limits
1019 : // return the scale factor to be applied to navigation velocity gains to compensate for increase in velocity noise with height when using optical flow
1020 41686716 : void NavEKF2::getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) const
1021 : {
1022 41686716 : if (core) {
1023 2210449 : core[primary].getEkfControlLimits(ekfGndSpdLimit, ekfNavVelGainScaler);
1024 : } else {
1025 39476267 : ekfGndSpdLimit = 400.0f; //return 80% of max filter speed
1026 39476267 : ekfNavVelGainScaler = 1.0f;
1027 : }
1028 41686716 : }
1029 :
1030 : // return the individual Z-accel bias estimates in m/s^2
1031 41686716 : void NavEKF2::getAccelZBias(float &zbias) const
1032 : {
1033 41686716 : if (core) {
1034 2210449 : core[primary].getAccelZBias(zbias);
1035 : }
1036 41686716 : }
1037 :
1038 : // return the NED wind speed estimates in m/s (positive is air moving in the direction of the axis)
1039 41686716 : void NavEKF2::getWind(Vector3f &wind) const
1040 : {
1041 41686716 : if (core) {
1042 2210449 : core[primary].getWind(wind);
1043 : }
1044 41686716 : }
1045 :
1046 : // return earth magnetic field estimates in measurement units / 1000
1047 41686716 : void NavEKF2::getMagNED(Vector3f &magNED) const
1048 : {
1049 41686716 : if (core) {
1050 2210449 : core[primary].getMagNED(magNED);
1051 : }
1052 41686716 : }
1053 :
1054 : // return body magnetic field estimates in measurement units / 1000
1055 41686716 : void NavEKF2::getMagXYZ(Vector3f &magXYZ) const
1056 : {
1057 41686716 : if (core) {
1058 2210449 : core[primary].getMagXYZ(magXYZ);
1059 : }
1060 41686716 : }
1061 :
1062 : // Return estimated magnetometer offsets
1063 : // Return true if magnetometer offsets are valid
1064 0 : bool NavEKF2::getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const
1065 : {
1066 0 : if (!core) {
1067 : return false;
1068 : }
1069 : // try the primary first, else loop through all of the cores and return when one has offsets for this mag instance
1070 0 : if (core[primary].getMagOffsets(mag_idx, magOffsets)) {
1071 : return true;
1072 : }
1073 0 : for (uint8_t i=0; i<num_cores; i++) {
1074 0 : if(core[i].getMagOffsets(mag_idx, magOffsets)) {
1075 : return true;
1076 : }
1077 : }
1078 : return false;
1079 : }
1080 :
1081 : // Return the last calculated latitude, longitude and height in WGS-84
1082 : // If a calculated location isn't available, return a raw GPS measurement
1083 : // The status will return true if a calculation or raw measurement is available
1084 : // The getFilterStatus() function provides a more detailed description of data health and must be checked if data is to be used for flight control
1085 41686716 : bool NavEKF2::getLLH(Location &loc) const
1086 : {
1087 41686716 : if (!core) {
1088 : return false;
1089 : }
1090 2210449 : return core[primary].getLLH(loc);
1091 : }
1092 :
1093 : // Return the latitude and longitude and height used to set the NED origin for the specified instance
1094 : // An out of range instance (eg -1) returns data for the primary instance
1095 : // All NED positions calculated by the filter are relative to this location
1096 : // Returns false if the origin has not been set
1097 6779639 : bool NavEKF2::getOriginLLH(Location &loc) const
1098 : {
1099 6779639 : if (!core) {
1100 : return false;
1101 : }
1102 431141 : if (common_origin_valid) {
1103 129125 : loc = common_EKF_origin;
1104 129125 : return true;
1105 : }
1106 302016 : return core[primary].getOriginLLH(loc);
1107 : }
1108 :
1109 : // set the latitude and longitude and height used to set the NED origin
1110 : // All NED positions calculated by the filter will be relative to this location
1111 : // The origin cannot be set if the filter is in a flight mode (eg vehicle armed)
1112 : // Returns false if the filter has rejected the attempt to set the origin
1113 750 : bool NavEKF2::setOriginLLH(const Location &loc)
1114 : {
1115 750 : AP::dal().log_SetOriginLLH2(loc);
1116 :
1117 750 : if (!core) {
1118 : return false;
1119 : }
1120 17 : if (common_origin_valid) {
1121 : // we don't allow setting of the EKF origin if the EKF origin
1122 : // has already been set.
1123 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "EKF2 refusing set origin");
1124 0 : return false;
1125 : }
1126 : bool ret = false;
1127 51 : for (uint8_t i=0; i<num_cores; i++) {
1128 34 : ret |= core[i].setOriginLLH(loc);
1129 : }
1130 : // return true if any core accepts the new origin
1131 : return ret;
1132 : }
1133 :
1134 : // return estimated height above ground level
1135 : // return false if ground height is not being estimated.
1136 41686716 : bool NavEKF2::getHAGL(float &HAGL) const
1137 : {
1138 41686716 : if (!core) {
1139 : return false;
1140 : }
1141 2210449 : return core[primary].getHAGL(HAGL);
1142 : }
1143 :
1144 : // return the Euler roll, pitch and yaw angle in radians for the specified instance
1145 41686716 : void NavEKF2::getEulerAngles(Vector3f &eulers) const
1146 : {
1147 41686716 : if (core) {
1148 2210449 : core[primary].getEulerAngles(eulers);
1149 : }
1150 41686716 : }
1151 :
1152 : // return the transformation matrix from XYZ (body) to NED axes
1153 41686716 : void NavEKF2::getRotationBodyToNED(Matrix3f &mat) const
1154 : {
1155 41686716 : if (core) {
1156 2210449 : core[primary].getRotationBodyToNED(mat);
1157 : }
1158 41686716 : }
1159 :
1160 : // return the quaternions defining the rotation from NED to XYZ (body) axes
1161 459 : void NavEKF2::getQuaternionBodyToNED(int8_t instance, Quaternion &quat) const
1162 : {
1163 459 : if (instance < 0 || instance >= num_cores) instance = primary;
1164 459 : if (core) {
1165 459 : Matrix3f mat;
1166 459 : core[instance].getRotationBodyToNED(mat);
1167 459 : quat.from_rotation_matrix(mat);
1168 : }
1169 459 : }
1170 :
1171 : // return the quaternions defining the rotation from NED to XYZ (autopilot) axes
1172 41686716 : void NavEKF2::getQuaternion(Quaternion &quat) const
1173 : {
1174 41686716 : if (core) {
1175 2210449 : core[primary].getQuaternion(quat);
1176 : }
1177 41686716 : }
1178 :
1179 : // return the innovations for the specified instance
1180 3439 : bool NavEKF2::getInnovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const
1181 : {
1182 3439 : if (core == nullptr) {
1183 : return false;
1184 : }
1185 :
1186 3408 : return core[primary].getInnovations(velInnov, posInnov, magInnov, tasInnov, yawInnov);
1187 : }
1188 :
1189 : // return the innovation consistency test ratios for the velocity, position, magnetometer and true airspeed measurements
1190 41686716 : bool NavEKF2::getVariances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const
1191 : {
1192 41686716 : if (core == nullptr) {
1193 : return false;
1194 : }
1195 :
1196 2210449 : return core[primary].getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset);
1197 : }
1198 :
1199 : // should we use the compass? This is public so it can be used for
1200 : // reporting via ahrs.use_compass()
1201 129 : bool NavEKF2::use_compass(void) const
1202 : {
1203 129 : if (!core) {
1204 : return false;
1205 : }
1206 129 : return core[primary].use_compass();
1207 : }
1208 :
1209 : // are we using a gps?
1210 41686716 : bool NavEKF2::using_gps(void) const
1211 : {
1212 41686716 : return _fusionModeGPS != 3;
1213 : }
1214 :
1215 : // write the raw optical flow measurements
1216 : // rawFlowQuality is a measured of quality between 0 and 255, with 255 being the best quality
1217 : // rawFlowRates are the optical flow rates in rad/sec about the X and Y sensor axes.
1218 : // rawGyroRates are the sensor rotation rates in rad/sec measured by the sensors internal gyro
1219 : // The sign convention is that a RH physical rotation of the sensor about an axis produces both a positive flow and gyro rate
1220 : // msecFlowMeas is the scheduler time in msec when the optical flow data was received from the sensor.
1221 : // posOffset is the XYZ flow sensor position in the body frame in m
1222 : // heightOverride is the fixed height of the sensor above ground in m, when on rover vehicles. 0 if not used
1223 10128 : void NavEKF2::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride)
1224 : {
1225 10128 : AP::dal().writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawGyroRates, msecFlowMeas, posOffset, heightOverride);
1226 :
1227 10128 : if (core) {
1228 0 : for (uint8_t i=0; i<num_cores; i++) {
1229 0 : core[i].writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawGyroRates, msecFlowMeas, posOffset, heightOverride);
1230 : }
1231 : }
1232 10128 : }
1233 :
1234 : // Set to true if the terrain underneath is stable enough to be used as a height reference
1235 : // in combination with a range finder. Set to false if the terrain underneath the vehicle
1236 : // cannot be used as a height reference. Use to prevent range finder operation otherwise
1237 : // enabled by the combination of EK2_RNG_AID_HGT and EK2_RNG_USE_SPD parameters.
1238 2339 : void NavEKF2::setTerrainHgtStable(bool val)
1239 : {
1240 2339 : if (val) {
1241 769 : AP::dal().log_event2(AP_DAL::Event::setTerrainHgtStable);
1242 : } else {
1243 1570 : AP::dal().log_event2(AP_DAL::Event::unsetTerrainHgtStable);
1244 : }
1245 2339 : if (core) {
1246 84 : for (uint8_t i=0; i<num_cores; i++) {
1247 56 : core[i].setTerrainHgtStable(val);
1248 : }
1249 : }
1250 :
1251 2339 : }
1252 :
1253 : /*
1254 : return the filter fault status as a bitmasked integer
1255 : 0 = quaternions are NaN
1256 : 1 = velocities are NaN
1257 : 2 = badly conditioned X magnetometer fusion
1258 : 3 = badly conditioned Y magnetometer fusion
1259 : 4 = badly conditioned Z magnetometer fusion
1260 : 5 = badly conditioned airspeed fusion
1261 : 6 = badly conditioned synthetic sideslip fusion
1262 : 7 = filter is not initialised
1263 : */
1264 41686716 : void NavEKF2::getFilterFaults(uint16_t &faults) const
1265 : {
1266 41686716 : if (core) {
1267 2210449 : core[primary].getFilterFaults(faults);
1268 : } else {
1269 39476267 : faults = 0;
1270 : }
1271 41686716 : }
1272 :
1273 : /*
1274 : return filter status flags
1275 : */
1276 41686716 : void NavEKF2::getFilterStatus(nav_filter_status &status) const
1277 : {
1278 41686716 : if (core) {
1279 2210449 : core[primary].getFilterStatus(status);
1280 : } else {
1281 39476267 : memset(&status, 0, sizeof(status));
1282 : }
1283 41686716 : }
1284 :
1285 : /*
1286 : return filter gps quality check status
1287 : */
1288 0 : void NavEKF2::getFilterGpsStatus(nav_gps_status &status) const
1289 : {
1290 0 : if (core) {
1291 0 : core[primary].getFilterGpsStatus(status);
1292 : } else {
1293 0 : memset(&status, 0, sizeof(status));
1294 : }
1295 0 : }
1296 :
1297 : // send an EKF_STATUS_REPORT message to GCS
1298 41686716 : bool NavEKF2::getTerrainAltVariance(float &terrainAltVar) const
1299 : {
1300 41686716 : if (core) {
1301 2210449 : return core[primary].getTerrainAltVariance(terrainAltVar);
1302 : }
1303 : return false;
1304 : }
1305 :
1306 : // provides the height limit to be observed by the control loops
1307 : // returns false if no height limiting is required
1308 : // this is needed to ensure the vehicle does not fly too high when using optical flow navigation
1309 41686716 : bool NavEKF2::getHeightControlLimit(float &height) const
1310 : {
1311 41686716 : if (!core) {
1312 : return false;
1313 : }
1314 2210449 : return core[primary].getHeightControlLimit(height);
1315 : }
1316 :
1317 : // Returns a count of NE position reset events; incremented when the primary
1318 : // core changes and when the primary core resets its NE position
1319 2106892 : uint16_t NavEKF2::getPosNorthEastResetCount(void)
1320 : {
1321 2106892 : if (!core) {
1322 : return 0;
1323 : }
1324 2106892 : const uint16_t core_count = core[primary].getPosNorthEastResetCount();
1325 2106892 : if (core_count != pos_reset_data.last_core_count) {
1326 450 : pos_reset_data.last_core_count = core_count;
1327 450 : pos_reset_data.count++;
1328 : }
1329 2106892 : return pos_reset_data.count;
1330 : }
1331 :
1332 : // Returns a count of D position reset events; incremented when the primary
1333 : // core changes and when the primary core resets its D position
1334 2106892 : uint16_t NavEKF2::getPosDownResetCount(void)
1335 : {
1336 2106892 : if (!core) {
1337 : return 0;
1338 : }
1339 2106892 : const uint16_t core_count = core[primary].getPosDownResetCount();
1340 2106892 : if (core_count != pos_down_reset_data.last_core_count) {
1341 305 : pos_down_reset_data.last_core_count = core_count;
1342 305 : pos_down_reset_data.count++;
1343 : }
1344 2106892 : return pos_down_reset_data.count;
1345 : }
1346 :
1347 : // Returns a count of yaw reset events; incremented when the primary core
1348 : // changes and when the primary core resets its yaw
1349 2106892 : uint16_t NavEKF2::getYawResetCount(void)
1350 : {
1351 2106892 : if (!core) {
1352 : return 0;
1353 : }
1354 2106892 : const uint16_t core_count = core[primary].getYawResetCount();
1355 2106892 : if (core_count != yaw_reset_data.last_core_count) {
1356 547 : yaw_reset_data.last_core_count = core_count;
1357 547 : yaw_reset_data.count++;
1358 : }
1359 2106892 : return yaw_reset_data.count;
1360 : }
1361 :
1362 : // update the yaw reset data to capture changes due to a lane switch
1363 6187 : void NavEKF2::updateLaneSwitchYawResetData(uint8_t new_primary)
1364 : {
1365 : // the new primary's historic in-core resets are not new events
1366 : // for consumers; re-seat the count and record a single event for
1367 : // the switch itself
1368 6187 : yaw_reset_data.last_core_count = core[new_primary].getYawResetCount();
1369 6187 : yaw_reset_data.count++;
1370 6187 : }
1371 :
1372 : // update the position reset data to capture changes due to a lane switch
1373 6187 : void NavEKF2::updateLaneSwitchPosResetData(uint8_t new_primary)
1374 : {
1375 : // the new primary's historic in-core resets are not new events
1376 : // for consumers; re-seat the count and record a single event for
1377 : // the switch itself
1378 6187 : pos_reset_data.last_core_count = core[new_primary].getPosNorthEastResetCount();
1379 6187 : pos_reset_data.count++;
1380 :
1381 6187 : }
1382 :
1383 : // Update the vertical position reset data to capture changes due to a core switch
1384 : // This should be called after the decision to switch cores has been made, but before the
1385 : // new primary EKF update has been run
1386 6187 : void NavEKF2::updateLaneSwitchPosDownResetData(uint8_t new_primary)
1387 : {
1388 : // the new primary's historic in-core resets are not new events
1389 : // for consumers; re-seat the count and record a single event for
1390 : // the switch itself
1391 6187 : pos_down_reset_data.last_core_count = core[new_primary].getPosDownResetCount();
1392 6187 : pos_down_reset_data.count++;
1393 :
1394 6187 : }
1395 :
1396 : /*
1397 : * Write position and quaternion data from an external navigation system
1398 : *
1399 : * 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.
1400 : * quat : quaternion describing the rotation from navigation frame to body frame
1401 : * posErr : 1-sigma spherical position error (m)
1402 : * angErr : 1-sigma spherical angle error (rad)
1403 : * timeStamp_ms : system time the measurement was taken, not the time it was received (mSec)
1404 : * delay_ms : average delay of external nav system measurements relative to inertial measurements
1405 : * resetTime_ms : system time of the last position reset request (mSec)
1406 : *
1407 : * Sensor offsets are pulled directly from the AP_VisualOdom library
1408 : *
1409 : */
1410 10069 : void NavEKF2::writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint16_t delay_ms, uint32_t resetTime_ms)
1411 : {
1412 10069 : AP::dal().writeExtNavData(pos, quat, posErr, angErr, timeStamp_ms, delay_ms, resetTime_ms);
1413 10069 : if (!option_is_set(Option::DisableExternalNav) && core) {
1414 0 : for (uint8_t i=0; i<num_cores; i++) {
1415 0 : core[i].writeExtNavData(pos, quat, posErr, angErr, timeStamp_ms, delay_ms, resetTime_ms);
1416 : }
1417 : }
1418 10069 : }
1419 :
1420 : // check if external navigation is being used for yaw observation
1421 83373432 : bool NavEKF2::isExtNavUsedForYaw() const
1422 : {
1423 83373432 : if (core) {
1424 4420898 : return core[primary].isExtNavUsedForYaw();
1425 : }
1426 : return false;
1427 : }
1428 :
1429 : // check if configured to use GPS for horizontal position estimation
1430 41691733 : bool NavEKF2::configuredToUseGPSForPosXY(void) const
1431 : {
1432 : // 0 = use 3D velocity, 1 = use 2D velocity, 2 = use no velocity, 3 = do not use GPS
1433 41691733 : return (_fusionModeGPS.get() != 3);
1434 : }
1435 :
1436 2 : void NavEKF2::requestYawReset(void)
1437 : {
1438 2 : AP::dal().log_event2(AP_DAL::Event::requestYawReset);
1439 :
1440 6 : for (uint8_t i = 0; i < num_cores; i++) {
1441 4 : core[i].EKFGSF_requestYawReset();
1442 : }
1443 2 : }
1444 :
1445 : // Writes the default equivalent airspeed in m/s to be used in forward flight if a measured airspeed is required and not available.
1446 0 : void NavEKF2::writeDefaultAirSpeed(float airspeed)
1447 : {
1448 0 : AP::dal().log_writeDefaultAirSpeed2(airspeed,0.0f);
1449 :
1450 0 : if (core) {
1451 0 : for (uint8_t i=0; i<num_cores; i++) {
1452 0 : core[i].writeDefaultAirSpeed(airspeed);
1453 : }
1454 : }
1455 0 : }
1456 :
1457 : /* Write velocity data from an external navigation system
1458 : * vel : velocity in NED (m)
1459 : * err : velocity error (m/s)
1460 : * timeStamp_ms : system time the measurement was taken, not the time it was received (mSec)
1461 : * delay_ms : average delay of external nav system measurements relative to inertial measurements
1462 : */
1463 4278 : void NavEKF2::writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeStamp_ms, uint16_t delay_ms)
1464 : {
1465 4278 : AP::dal().writeExtNavVelData(vel, err, timeStamp_ms, delay_ms);
1466 4278 : if (!option_is_set(Option::DisableExternalNav) && core) {
1467 0 : for (uint8_t i=0; i<num_cores; i++) {
1468 0 : core[i].writeExtNavVelData(vel, err, timeStamp_ms, delay_ms);
1469 : }
1470 : }
1471 4278 : }
1472 :
1473 : // get yaw estimator instance
1474 0 : const EKFGSF_yaw *NavEKF2::get_yawEstimator(void) const
1475 : {
1476 0 : if (core) {
1477 0 : return core[primary].get_yawEstimator();
1478 : }
1479 : return nullptr;
1480 : }
|
