Line data Source code
1 : #include <AP_HAL/AP_HAL.h>
2 :
3 : #include "AP_NavEKF3.h"
4 : #include "AP_NavEKF3_core.h"
5 : #include <AP_DAL/AP_DAL.h>
6 : #include <GCS_MAVLink/GCS.h>
7 :
8 : // Check basic filter health metrics and return a consolidated health status
9 112457226 : bool NavEKF3_core::healthy(void) const
10 : {
11 : uint16_t faultInt;
12 112457226 : getFilterFaults(faultInt);
13 112457226 : if (faultInt > 0) {
14 : return false;
15 : }
16 112457226 : if (velTestRatio > 1 && posTestRatio > 1 && hgtTestRatio > 1) {
17 : // all three metrics being above 1 means the filter is
18 : // extremely unhealthy.
19 : return false;
20 : }
21 : // Give the filter a second to settle before use
22 112457211 : if ((imuSampleTime_ms - ekfStartTime_ms) < 1000 ) {
23 : return false;
24 : }
25 : // position and height innovations must be within limits when on-ground and in a static mode of operation
26 110945660 : float horizErrSq = sq(innovVelPos[3]) + sq(innovVelPos[4]);
27 110945660 : if (onGround && (PV_AidingMode == AID_NONE) && ((horizErrSq > 1.0f) || (fabsF(hgtInnovFiltState) > 1.0f))) {
28 8615 : return false;
29 : }
30 :
31 : // all OK
32 : return true;
33 : }
34 :
35 : /*
36 : per-core pre-arm checks. returns false if we fail arming checks, in
37 : which case the buffer will be populated with a failure message
38 : requires_position should be true if horizontal position configuration should be checked
39 : */
40 61515 : bool NavEKF3_core::pre_arm_check(bool requires_position, char *failure_msg, uint8_t failure_msg_len) const
41 : {
42 61515 : if (requires_position) {
43 : // additional checks when position is required, used by pre-arm checks
44 : const float max_vel_innovation = 2.0;
45 37385 : const float hvel_innovation = sqrtf(sq(innovVelPos[0])+sq(innovVelPos[1]));
46 65215 : if (onGround && PV_AidingMode == AID_ABSOLUTE &&
47 93284 : frontend->sources.useVelXYSource(AP_NavEKF_Source::SourceXY::GPS, core_index) &&
48 : hvel_innovation > max_vel_innovation) {
49 : // more than 2 m/s horizontal velocity innovation on the ground
50 4 : dal.snprintf(failure_msg, failure_msg_len,
51 4 : "EKF3[%u] vel error %.1f", unsigned(core_index)+1, hvel_innovation);
52 4 : return false;
53 : }
54 : }
55 :
56 : // all OK
57 : return true;
58 : }
59 :
60 : // Return a consolidated error score where higher numbers represent larger errors
61 : // Intended to be used by the front-end to determine which is the primary EKF
62 53344536 : float NavEKF3_core::errorScore() const
63 : {
64 53344536 : float score = 0.0f;
65 53344536 : if (tiltAlignComplete && yawAlignComplete) {
66 : // Check GPS fusion performance
67 53286569 : score = MAX(score, 0.5f * (velTestRatio + posTestRatio));
68 : // Check altimeter fusion performance
69 53286569 : score = MAX(score, hgtTestRatio);
70 : // Check airspeed fusion performance - only when we are using at least 2 airspeed sensors so we can switch lanes with
71 : // a better one. This only comes into effect for a forward flight vehicle. A sensitivity factor of 0.3 is added to keep the
72 : // EKF less sensitive to innovations arising due events like strong gusts of wind, thus, prevent reporting high error scores
73 53286569 : if (assume_zero_sideslip()) {
74 7052555 : const auto *arsp = dal.airspeed();
75 7052555 : if (arsp != nullptr && arsp->get_num_sensors() >= 2 && (frontend->_affinity & EKF_AFFINITY_ARSP)) {
76 25640 : score = MAX(score, 0.3f * tasTestRatio);
77 : }
78 : }
79 : // Check magnetometer fusion performance - need this when magnetometer affinity is enabled to override the inherent compass
80 : // switching mechanism, and instead be able to move to a better lane
81 53286569 : if (frontend->_affinity & EKF_AFFINITY_MAG) {
82 682710 : score = MAX(score, 0.3f * (magTestRatio.x + magTestRatio.y + magTestRatio.z));
83 : }
84 : }
85 53344536 : return score;
86 : }
87 :
88 : // provides the height limit to be observed by the control loops
89 : // returns false if no height limiting is required
90 : // this is needed to ensure the vehicle does not fly too high when using optical flow navigation
91 40053019 : bool NavEKF3_core::getHeightControlLimit(float &height) const
92 : {
93 : // only ask for limiting if we are doing optical flow navigation
94 40053019 : if (frontend->sources.useVelXYSource(AP_NavEKF_Source::SourceXY::OPTFLOW, core_index) && (PV_AidingMode == AID_RELATIVE) && flowDataValid) {
95 :
96 : // If we are using optical flow nav with terrain alt from SRTM then there is no limit
97 : #if EK3_FEATURE_OPTFLOW_SRTM
98 150611 : if (terrain_srtm_alt_valid) {
99 : return false;
100 : }
101 : #endif
102 :
103 : // if using rangefinder, ensure the height above ground is within range finder limits after accounting for vehicle tilt and control errors
104 : #if AP_RANGEFINDER_ENABLED
105 149185 : const auto *_rng = dal.rangefinder();
106 149185 : if (_rng == nullptr) {
107 : // we really, really shouldn't be here.
108 : return false;
109 : }
110 138943 : height = MAX(float(_rng->max_distance_orient(ROTATION_PITCH_270)) * 0.7f - 1.0f, 1.0f);
111 : #else
112 : return false;
113 : #endif
114 : // If we are are not using the range finder as the height reference, then compensate for the difference between terrain and EKF origin
115 138943 : if (frontend->sources.getPosZSource(core_index) != AP_NavEKF_Source::SourceZ::RANGEFINDER) {
116 138943 : height -= terrainState;
117 : }
118 138943 : return true;
119 : } else {
120 : return false;
121 : }
122 : }
123 :
124 :
125 : // return the Euler roll, pitch and yaw angle in radians
126 44802263 : void NavEKF3_core::getEulerAngles(Vector3f &euler) const
127 : {
128 44802263 : outputDataNew.quat.to_euler(euler);
129 44802263 : euler = euler - dal.get_trim();
130 44802263 : }
131 :
132 : // return body axis gyro bias estimates in rad/sec
133 44805620 : void NavEKF3_core::getGyroBias(Vector3f &gyroBias) const
134 : {
135 44805620 : if (dtEkfAvg < 1e-6f) {
136 0 : gyroBias.zero();
137 0 : return;
138 : }
139 44805620 : gyroBias = (stateStruct.gyro_bias / dtEkfAvg).tofloat();
140 : }
141 :
142 : // return accelerometer bias in m/s/s
143 44802263 : void NavEKF3_core::getAccelBias(Vector3f &accelBias) const
144 : {
145 44802263 : if (!statesInitialised) {
146 904354 : accelBias.zero();
147 904354 : return;
148 : }
149 43897909 : accelBias = (stateStruct.accel_bias / dtEkfAvg).tofloat();
150 : }
151 :
152 : // return the transformation matrix from XYZ (body) to NED axes
153 40126840 : void NavEKF3_core::getRotationBodyToNED(Matrix3f &mat) const
154 : {
155 40126840 : outputDataNew.quat.rotation_matrix(mat);
156 40126840 : mat = mat * dal.get_rotation_vehicle_body_to_autopilot_body();
157 40126840 : }
158 :
159 : // return the quaternions defining the rotation from NED to XYZ (body) axes
160 44802263 : void NavEKF3_core::getQuaternion(Quaternion& ret) const
161 : {
162 44802263 : ret = outputDataNew.quat.tofloat();
163 44802263 : }
164 :
165 : // return the NED wind speed estimates in m/s (positive is air moving in the direction of the axis)
166 : // returns true if wind state estimation is active
167 44802263 : bool NavEKF3_core::getWind(Vector3f &wind) const
168 : {
169 44802263 : wind.x = stateStruct.wind_vel.x;
170 44802263 : wind.y = stateStruct.wind_vel.y;
171 44802263 : wind.z = 0.0f; // currently don't estimate this
172 44802263 : return !inhibitWindStates;
173 : }
174 :
175 : // return the NED velocity of the body frame origin in m/s
176 : //
177 44802263 : void NavEKF3_core::getVelNED(Vector3f &vel) const
178 : {
179 : // correct for the IMU position offset (EKF calculations are at the IMU)
180 44802263 : vel = (outputDataNew.velocity + velOffsetNED).tofloat();
181 44802263 : }
182 :
183 : // return estimate of true airspeed vector in body frame in m/s
184 : // returns false if estimate is unavailable
185 36087486 : bool NavEKF3_core::getAirSpdVec(Vector3f &vel) const
186 : {
187 36087486 : if (PV_AidingMode == AID_NONE) {
188 : return false;
189 : }
190 29202566 : vel = (outputDataNew.velocity + velOffsetNED).tofloat();
191 29202566 : if (!inhibitWindStates) {
192 3527926 : vel.x -= stateStruct.wind_vel.x;
193 3527926 : vel.y -= stateStruct.wind_vel.y;
194 : }
195 29202566 : Matrix3f Tnb; // rotation from nav to body frame
196 29202566 : outputDataNew.quat.inverse().rotation_matrix(Tnb);
197 29202566 : vel = Tnb * vel;
198 29202566 : return true;
199 : }
200 :
201 : // return the innovation in m/s, innovation variance in (m/s)^2 and age in msec of the last TAS measurement processed
202 : // returns false if the data is unavailable
203 146979 : bool NavEKF3_core::getAirSpdHealthData(float &innovation, float &innovationVariance, uint32_t &age_ms) const
204 : {
205 146979 : if (tasDataDelayed.time_ms == 0) {
206 : // no data has been processed since startup
207 : return false;
208 : }
209 141374 : innovation = (float)innovVtas;
210 141374 : innovationVariance = (float)varInnovVtas;
211 141374 : age_ms = imuSampleTime_ms - tasDataDelayed.time_ms;
212 141374 : return true;
213 : }
214 :
215 : // Return the rate of change of vertical position in the down direction (dPosD/dt) of the body frame origin in m/s
216 44802263 : float NavEKF3_core::getPosDownDerivative(void) const
217 : {
218 : // return the value calculated from a complementary filter applied to the EKF height and vertical acceleration
219 : // correct for the IMU offset (EKF calculations are at the IMU)
220 44802263 : return vertCompFiltState.vel + velOffsetNED.z;
221 : }
222 :
223 : // Write the last estimated NE position of the body frame origin relative to the reference point (m).
224 : // Return true if the estimate is valid
225 44802263 : bool NavEKF3_core::getPosNE(Vector2p &posNE) const
226 : {
227 : // There are three modes of operation, absolute position (GPS fusion), relative position (optical flow fusion) and constant position (no position estimate available)
228 44802263 : if (PV_AidingMode != AID_NONE) {
229 : // This is the normal mode of operation where we can use the EKF position states
230 : // correct for the IMU offset (EKF calculations are at the IMU)
231 35671375 : posNE = outputDataNew.position.xy().topostype() + posOffsetNED.xy().topostype() + public_origin.get_distance_NE_postype(EKF_origin);
232 35671375 : return true;
233 :
234 : } else {
235 : // In constant position mode the EKF position states are at the origin, so we cannot use them as a position estimate
236 9130888 : if(validOrigin) {
237 4642870 : auto &gps = dal.gps();
238 4642870 : if ((gps.status(selected_gps) >= AP_GPS_FixType::FIX_2D)) {
239 : // If the origin has been set and we have GPS, then return the GPS position relative to the origin
240 4499822 : const Location &gpsloc = gps.location(selected_gps);
241 4499822 : posNE = public_origin.get_distance_NE_postype(gpsloc);
242 4499822 : return false;
243 : #if EK3_FEATURE_BEACON_FUSION
244 143048 : } else if (rngBcn.alignmentStarted) {
245 : // If we are attempting alignment using range beacon data, then report the position
246 30141 : posNE.x = rngBcn.receiverPos.x;
247 30141 : posNE.y = rngBcn.receiverPos.y;
248 30141 : return false;
249 : #endif
250 : } else {
251 : // If no GPS fix is available, all we can do is provide the last known position
252 112907 : posNE = outputDataNew.position.xy().topostype();
253 112907 : return false;
254 : }
255 : } else {
256 : // If the origin has not been set, then we have no means of providing a relative position
257 4488018 : posNE.zero();
258 4488018 : return false;
259 : }
260 : }
261 : return false;
262 : }
263 :
264 : // Write the last calculated D position of the body frame origin relative to the EKF local origin
265 : // Return true if the estimate is valid
266 79950853 : bool NavEKF3_core::getPosD_local(postype_t &posD) const
267 : {
268 79950853 : posD = outputDataNew.position.z + posOffsetNED.z;
269 :
270 : // Return the current height solution status
271 79950853 : return filterStatus.flags.vert_pos;
272 :
273 : }
274 :
275 : // Write the last calculated D position of the body frame origin relative to the public origin
276 : // Return true if the estimate is valid
277 44802263 : bool NavEKF3_core::getPosD(postype_t &posD) const
278 : {
279 44802263 : bool ret = getPosD_local(posD);
280 :
281 : // adjust posD for difference between our origin and the public_origin
282 44802263 : Location local_origin;
283 44802263 : if (getOriginLLH(local_origin)) {
284 39274602 : posD += (public_origin.alt - local_origin.alt) * 0.01;
285 : }
286 :
287 44802263 : return ret;
288 : }
289 :
290 : // return the estimated height of body frame origin above ground level
291 40053019 : bool NavEKF3_core::getHAGL(float &HAGL) const
292 : {
293 : #if EK3_FEATURE_OPTFLOW_AGL_KF
294 40053019 : if (frontend->option_is_enabled(NavEKF3::Option::AglKfForOptflow) && aglKfValid) {
295 0 : HAGL = aglKfH;
296 0 : return healthy();
297 : }
298 : #endif
299 40053019 : HAGL = terrainState - outputDataNew.position.z - posOffsetNED.z;
300 : // If we know the terrain offset and altitude, then we have a valid height above ground estimate
301 40053019 : return !hgtTimeout && gndOffsetValid && healthy();
302 : }
303 :
304 : // Return the last calculated latitude, longitude and height in WGS-84
305 : // If a calculated location isn't available and position source is GPS, return a raw GPS measurement
306 : // The status will return true if a calculation or raw measurement is available
307 : // The getFilterStatus() function provides a more detailed description of data health and must be checked if data is to be used for flight control
308 40053019 : bool NavEKF3_core::getLLH(Location &loc) const
309 : {
310 40053019 : Location origin;
311 40053019 : const bool pos_from_GPS = (frontend->sources.getPosXYSource(core_index) == AP_NavEKF_Source::SourceXY::GPS);
312 40053019 : if (getOriginLLH(origin)) {
313 : postype_t posD;
314 35148590 : if (getPosD_local(posD) && PV_AidingMode != AID_NONE) {
315 : // Altitude returned is an absolute altitude relative to the WGS-84 spherioid
316 30907467 : loc.set_alt_cm(origin.alt - posD*100.0, Location::AltFrame::ABSOLUTE);
317 30907467 : if (filterStatus.flags.horiz_pos_abs || filterStatus.flags.horiz_pos_rel) {
318 : // The EKF is able to provide a position estimate
319 30838952 : loc.lat = EKF_origin.lat;
320 30838952 : loc.lng = EKF_origin.lng;
321 30838952 : loc.offset(outputDataNew.position.x + posOffsetNED.x,
322 30838952 : outputDataNew.position.y + posOffsetNED.y);
323 30838952 : return true;
324 : } else {
325 : // We have been be doing inertial dead reckoning for too long so use raw GPS if available
326 68515 : if (pos_from_GPS && getGPSLLH(loc)) {
327 : return true;
328 : } else {
329 : // Return the EKF estimate but mark it as invalid
330 62672 : loc.lat = EKF_origin.lat;
331 62672 : loc.lng = EKF_origin.lng;
332 62672 : loc.offset(outputDataNew.position.x + posOffsetNED.x,
333 62672 : outputDataNew.position.y + posOffsetNED.y);
334 62672 : return false;
335 : }
336 : }
337 : } else {
338 : // Return a raw GPS reading if available and the last recorded positon if not
339 4241123 : if (pos_from_GPS && getGPSLLH(loc)) {
340 : return true;
341 : } else {
342 137698 : loc.lat = EKF_origin.lat;
343 137698 : loc.lng = EKF_origin.lng;
344 137698 : loc.offset(lastKnownPositionNE.x + posOffsetNED.x,
345 137698 : lastKnownPositionNE.y + posOffsetNED.y);
346 137698 : loc.alt = EKF_origin.alt - lastKnownPositionD*100.0;
347 137698 : return false;
348 : }
349 : }
350 : } else {
351 : // The EKF is not navigating so use raw GPS if available
352 4904429 : return pos_from_GPS && getGPSLLH(loc);
353 : }
354 : }
355 :
356 9021535 : bool NavEKF3_core::getGPSLLH(Location &loc) const
357 : {
358 9021535 : const auto &gps = dal.gps();
359 9021535 : if ((gps.status(selected_gps) >= AP_GPS_FixType::FIX_3D)) {
360 6518562 : loc = gps.location(selected_gps);
361 6518562 : return true;
362 : }
363 : return false;
364 : }
365 :
366 : // return the horizontal speed limit in m/s set by optical flow sensor limits
367 : // return the scale factor to be applied to navigation velocity gains to compensate for increase in velocity noise with height when using optical flow
368 40053019 : void NavEKF3_core::getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) const
369 : {
370 : // If in the last 10 seconds we have received flow data and no odometry data, then we are relying on optical flow
371 40053019 : bool relyingOnFlowData = (imuSampleTime_ms - prevBodyVelFuseTime_ms > 1000)
372 40053019 : && (imuSampleTime_ms - flowValidMeaTime_ms <= 10000);
373 :
374 : // If relying on optical flow, limit speed to prevent sensor limit being exceeded and adjust
375 : // nav gains to prevent body rate feedback into flow rates destabilising the control loop
376 40053019 : if (PV_AidingMode == AID_RELATIVE && relyingOnFlowData) {
377 : // allow 1.0 rad/sec margin for angular motion
378 152587 : ekfGndSpdLimit = MAX((frontend->_maxFlowRate - 1.0f), 0.0f) * MAX((terrainState - stateStruct.position[2]), rngOnGnd);
379 : // use standard gains up to 5.0 metres height and reduce above that
380 152587 : ekfNavVelGainScaler = 4.0f / MAX((terrainState - stateStruct.position[2]),4.0f);
381 : } else {
382 39900432 : ekfGndSpdLimit = 400.0f; //return 80% of max filter speed
383 39900432 : ekfNavVelGainScaler = 1.0f;
384 : }
385 40053019 : }
386 :
387 :
388 : // return the LLH location of the filters NED origin
389 99657015 : bool NavEKF3_core::getOriginLLH(Location &loc) const
390 : {
391 99657015 : if (validOrigin) {
392 78811334 : loc = public_origin;
393 : // report internally corrected reference height if enabled
394 78811334 : if ((frontend->_originHgtMode & (1<<2)) == 0) {
395 77513900 : loc.alt = (int32_t)(100.0f * (float)ekfGpsRefHgt);
396 : }
397 : }
398 99657015 : return validOrigin;
399 : }
400 :
401 : // return earth magnetic field estimates in measurement units / 1000
402 44802263 : void NavEKF3_core::getMagNED(Vector3f &magNED) const
403 : {
404 44802263 : magNED = (stateStruct.earth_magfield * 1000.0f).tofloat();
405 44802263 : }
406 :
407 : // return body magnetic field estimates in measurement units / 1000
408 44802263 : void NavEKF3_core::getMagXYZ(Vector3f &magXYZ) const
409 : {
410 44802263 : magXYZ = (stateStruct.body_magfield*1000.0f).tofloat();
411 44802263 : }
412 :
413 : // return magnetometer offsets
414 : // return true if offsets are valid
415 20 : bool NavEKF3_core::getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const
416 : {
417 20 : const auto &compass = dal.compass();
418 20 : if (!compass.available()) {
419 : return false;
420 : }
421 :
422 : // compass offsets are valid if we have finalised magnetic field initialisation, magnetic field learning is not prohibited,
423 : // primary compass is valid and state variances have converged
424 : const float maxMagVar = 5E-6f;
425 20 : bool variancesConverged = (P[19][19] < maxMagVar) && (P[20][20] < maxMagVar) && (P[21][21] < maxMagVar);
426 25 : if ((mag_idx == magSelectIndex) &&
427 5 : finalInflightMagInit &&
428 5 : !inhibitMagStates &&
429 25 : compass.healthy(magSelectIndex) &&
430 : variancesConverged) {
431 5 : magOffsets = compass.get_offsets(magSelectIndex) - stateStruct.body_magfield.tofloat()*1000.0;
432 5 : return true;
433 : } else {
434 15 : magOffsets = compass.get_offsets(magSelectIndex);
435 15 : return false;
436 : }
437 : }
438 :
439 : // return the index for the active magnetometer
440 : // return the index for the active airspeed
441 77351818 : uint8_t NavEKF3_core::getActiveAirspeed() const
442 : {
443 77351818 : return (uint8_t)selected_airspeed;
444 : }
445 :
446 : // return the innovations for the NED Pos, NED Vel, XYZ Mag and Vtas measurements
447 5426445 : bool NavEKF3_core::getInnovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const
448 : {
449 5426445 : velInnov.x = innovVelPos[0];
450 5426445 : velInnov.y = innovVelPos[1];
451 5426445 : velInnov.z = innovVelPos[2];
452 5426445 : posInnov.x = innovVelPos[3];
453 5426445 : posInnov.y = innovVelPos[4];
454 5426445 : posInnov.z = innovVelPos[5];
455 5426445 : magInnov.x = 1e3f*innovMag[0]; // Convert back to sensor units
456 5426445 : magInnov.y = 1e3f*innovMag[1]; // Convert back to sensor units
457 5426445 : magInnov.z = 1e3f*innovMag[2]; // Convert back to sensor units
458 5426445 : tasInnov = innovVtas;
459 5426445 : yawInnov = innovYaw;
460 5426445 : return true;
461 : }
462 :
463 : // return the synthetic air data drag and sideslip innovations
464 4749244 : void NavEKF3_core::getSynthAirDataInnovations(Vector2f &dragInnov, float &betaInnov) const
465 : {
466 : #if EK3_FEATURE_DRAG_FUSION
467 4749244 : dragInnov.x = innovDrag[0];
468 4749244 : dragInnov.y = innovDrag[1];
469 4749244 : betaInnov = innovBeta;
470 : #endif
471 4749244 : }
472 :
473 : // return the innovation consistency test ratios for the velocity, position, magnetometer and true airspeed measurements
474 : // this indicates the amount of margin available when tuning the various error traps
475 : // also return the delta in position due to the last position reset
476 44802263 : bool NavEKF3_core::getVariances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const
477 : {
478 44802263 : velVar = sqrtF(velTestRatio);
479 44802263 : posVar = sqrtF(posTestRatio);
480 44802263 : hgtVar = sqrtF(hgtTestRatio);
481 : // If we are using simple compass yaw fusion, populate all three components with the yaw test ratio to provide an equivalent output
482 44802263 : magVar.x = sqrtF(MAX(magTestRatio.x,yawTestRatio));
483 44802263 : magVar.y = sqrtF(MAX(magTestRatio.y,yawTestRatio));
484 44802263 : magVar.z = sqrtF(MAX(magTestRatio.z,yawTestRatio));
485 44802263 : tasVar = sqrtF(tasTestRatio);
486 44802263 : offset = posResetNE.tofloat();
487 :
488 44802263 : return true;
489 : }
490 :
491 : // return 1-sigma position and velocity uncertainty from the EKF state error covariance matrix P
492 13 : bool NavEKF3_core::getPosVelUncertainty(float &pos_horiz_m, float &pos_vert_m, float &vel_m_s) const
493 : {
494 13 : if (!statesInitialised) {
495 : return false;
496 : }
497 : // Horizontal position: 2D RMS from the N and E position state variances P[7][7] and P[8][8].
498 : // sqrt(P[7][7] + P[8][8]) is the 2D (circular) RMS, matching the convention used by GPS
499 : // receivers when reporting horizontal accuracy (hAcc).
500 13 : pos_horiz_m = sqrtF(P[7][7] + P[8][8]);
501 : // Vertical position: 1-sigma from the D position state variance P[9][9]
502 13 : pos_vert_m = sqrtF(P[9][9]);
503 : // Velocity: worst-case 1-sigma across NED components
504 13 : vel_m_s = sqrtF(MAX(MAX(P[4][4], P[5][5]), P[6][6]));
505 13 : return true;
506 : }
507 :
508 : // get a particular source's velocity innovations
509 : // returns true on success and results are placed in innovations and variances arguments
510 28 : bool NavEKF3_core::getVelInnovationsAndVariancesForSource(AP_NavEKF_Source::SourceXY source, Vector3f &innovations, Vector3f &variances) const
511 : {
512 28 : switch (source) {
513 0 : case AP_NavEKF_Source::SourceXY::GPS:
514 : // check for timeouts
515 0 : if (dal.millis() - gpsRetrieveTime_ms > 500) {
516 : return false;
517 : }
518 0 : innovations = gpsVelInnov.tofloat();
519 0 : variances = gpsVelVarInnov.tofloat();
520 0 : return true;
521 : #if EK3_FEATURE_EXTERNAL_NAV
522 28 : case AP_NavEKF_Source::SourceXY::EXTNAV:
523 : // check for timeouts
524 28 : if (dal.millis() - extNavVelInnovTime_ms > 500) {
525 : return false;
526 : }
527 0 : innovations = extNavVelInnov.tofloat();
528 0 : variances = extNavVelVarInnov.tofloat();
529 0 : return true;
530 : #endif // EK3_FEATURE_EXTERNAL_NAV
531 0 : case AP_NavEKF_Source::SourceXY::OPTFLOW:
532 : // check for timeouts
533 0 : if (dal.millis() - flowInnovTime_ms > 500) {
534 : return false;
535 : }
536 0 : innovations.x = flowInnov[0];
537 0 : innovations.y = flowInnov[1];
538 0 : innovations.z = 0;
539 0 : variances.x = flowVarInnov[0];
540 0 : variances.y = flowVarInnov[1];
541 0 : variances.z = 0;
542 0 : return true;
543 : default:
544 : // variances are not available for this source
545 : return false;
546 : }
547 :
548 : // should never get here but just in case
549 : return false;
550 : }
551 :
552 : /*
553 : return the filter fault status as a bitmasked integer
554 : 0 = quaternions are NaN
555 : 1 = velocities are NaN
556 : 2 = badly conditioned X magnetometer fusion
557 : 3 = badly conditioned Y magnetometer fusion
558 : 4 = badly conditioned Z magnetometer fusion
559 : 5 = badly conditioned airspeed fusion
560 : 6 = badly conditioned synthetic sideslip fusion
561 : 7 = filter is not initialised
562 : */
563 157259489 : void NavEKF3_core::getFilterFaults(uint16_t &faults) const
564 : {
565 157259489 : faults = (stateStruct.quat.is_nan() * uint16_t(NavFilterFaultBit::BAD_QUATERNION) |
566 157259489 : stateStruct.velocity.is_nan() * uint16_t(NavFilterFaultBit::BAD_VELOCITY) |
567 157259489 : faultStatus.bad_xmag * uint16_t(NavFilterFaultBit::BAD_XMAG) |
568 157259489 : faultStatus.bad_ymag * uint16_t(NavFilterFaultBit::BAD_YMAG) |
569 157259489 : faultStatus.bad_zmag * uint16_t(NavFilterFaultBit::BAD_ZMAG) |
570 157259489 : faultStatus.bad_airspeed * uint16_t(NavFilterFaultBit::BAD_AIRSPEED) |
571 157259489 : faultStatus.bad_sideslip * uint16_t(NavFilterFaultBit::BAD_SIDESLIP) |
572 157259489 : !statesInitialised * uint16_t(NavFilterFaultBit::NOT_INITIALISED));
573 157259489 : }
574 :
575 : // Return the navigation filter status message
576 44802263 : void NavEKF3_core::getFilterStatus(nav_filter_status &status) const
577 : {
578 44802263 : status = filterStatus;
579 44802263 : }
580 :
581 : // return a terrain altitude variance
582 40053019 : bool NavEKF3_core::getTerrainAltVariance(float &temp) const
583 : {
584 : // Only report range finder normalised innovation levels if the EKF needs the data for primary
585 : // height estimation or optical flow operation. This prevents false alarms at the GCS if a
586 : // range finder is fitted for other applications
587 40053019 : if (((frontend->_useRngSwHgt > 0) && activeHgtSource == AP_NavEKF_Source::SourceZ::RANGEFINDER) || (PV_AidingMode == AID_RELATIVE && flowDataValid)) {
588 209842 : temp = sqrtF(auxRngTestRatio);
589 : } else {
590 39843177 : temp = 0;
591 : }
592 : // we always successfully return a value, even if that value is a
593 : // "nothing to look at here" 0 value:
594 40053019 : return true;
595 : }
596 :
597 : // report the reason for why the backend is refusing to initialise
598 662 : const char *NavEKF3_core::prearm_failure_reason(void) const
599 : {
600 662 : if (gpsGoodToAlign) {
601 : // we are not failing
602 : return nullptr;
603 : }
604 645 : return prearm_fail_string;
605 : }
606 :
607 :
608 : // report the number of frames lapsed since the last state prediction
609 : // this is used by other instances to level load
610 78317950 : uint8_t NavEKF3_core::getFramesSincePredict(void) const
611 : {
612 78317950 : return framesSincePredict;
613 : }
|
