Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 :
16 : #include "AP_NavEKF_Source.h"
17 : #include <AP_Math/AP_Math.h>
18 : #include <AP_DAL/AP_DAL.h>
19 : #include <AP_Logger/AP_Logger.h>
20 : #include <AP_HAL/AP_HAL.h>
21 : #include "AP_Nav_Common.h"
22 :
23 : extern const AP_HAL::HAL& hal;
24 :
25 : const AP_Param::GroupInfo AP_NavEKF_Source::var_info[] = {
26 :
27 : // @Param: 1_POSXY
28 : // @DisplayName: Position Horizontal Source (Primary)
29 : // @Description: Position Horizontal Source (Primary)
30 : // @Values: 0:None, 3:GPS, 4:Beacon, 6:ExternalNav
31 : // @User: Advanced
32 : AP_GROUPINFO("1_POSXY", 1, AP_NavEKF_Source, _source_set[0].posxy, (int8_t)AP_NavEKF_Source::SourceXY::GPS),
33 :
34 : // @Param: 1_VELXY
35 : // @DisplayName: Velocity Horizontal Source
36 : // @Description: Velocity Horizontal Source
37 : // @Values: 0:None, 3:GPS, 4:Beacon, 5:OpticalFlow, 6:ExternalNav, 7:WheelEncoder
38 : // @User: Advanced
39 : AP_GROUPINFO("1_VELXY", 2, AP_NavEKF_Source, _source_set[0].velxy, (int8_t)AP_NavEKF_Source::SourceXY::GPS),
40 :
41 : // @Param: 1_POSZ
42 : // @DisplayName: Position Vertical Source
43 : // @Description: Position Vertical Source
44 : // @Values: 0:None, 1:Baro, 2:RangeFinder, 3:GPS, 4:Beacon, 6:ExternalNav
45 : // @User: Advanced
46 : AP_GROUPINFO("1_POSZ", 3, AP_NavEKF_Source, _source_set[0].posz, (int8_t)AP_NavEKF_Source::SourceZ::BARO),
47 :
48 : // @Param: 1_VELZ
49 : // @DisplayName: Velocity Vertical Source
50 : // @Description: Velocity Vertical Source
51 : // @Values: 0:None, 3:GPS, 4:Beacon, 6:ExternalNav
52 : // @User: Advanced
53 : AP_GROUPINFO("1_VELZ", 4, AP_NavEKF_Source, _source_set[0].velz, (int8_t)AP_NavEKF_Source::SourceZ::GPS),
54 :
55 : // @Param: 1_YAW
56 : // @DisplayName: Yaw Source
57 : // @Description: Yaw Source
58 : // @Values: 0:None, 1:Compass, 2:GPS, 3:GPS with Compass Fallback, 6:ExternalNav, 8:GSF
59 : // @User: Advanced
60 : AP_GROUPINFO("1_YAW", 5, AP_NavEKF_Source, _source_set[0].yaw, (int8_t)AP_NavEKF_Source::SourceYaw::COMPASS),
61 :
62 : #if AP_NAKEKF_SOURCE_SET_MAX >= 2
63 : // @Param: 2_POSXY
64 : // @DisplayName: Position Horizontal Source (Secondary)
65 : // @Description: Position Horizontal Source (Secondary)
66 : // @Values: 0:None, 3:GPS, 4:Beacon, 6:ExternalNav
67 : // @User: Advanced
68 : AP_GROUPINFO("2_POSXY", 6, AP_NavEKF_Source, _source_set[1].posxy, (int8_t)AP_NavEKF_Source::SourceXY::NONE),
69 :
70 : // @Param: 2_VELXY
71 : // @DisplayName: Velocity Horizontal Source (Secondary)
72 : // @Description: Velocity Horizontal Source (Secondary)
73 : // @Values: 0:None, 3:GPS, 4:Beacon, 5:OpticalFlow, 6:ExternalNav, 7:WheelEncoder
74 : // @User: Advanced
75 : AP_GROUPINFO("2_VELXY", 7, AP_NavEKF_Source, _source_set[1].velxy, (int8_t)AP_NavEKF_Source::SourceXY::NONE),
76 :
77 : // @Param: 2_POSZ
78 : // @DisplayName: Position Vertical Source (Secondary)
79 : // @Description: Position Vertical Source (Secondary)
80 : // @Values: 0:None, 1:Baro, 2:RangeFinder, 3:GPS, 4:Beacon, 6:ExternalNav
81 : // @User: Advanced
82 : AP_GROUPINFO("2_POSZ", 8, AP_NavEKF_Source, _source_set[1].posz, (int8_t)AP_NavEKF_Source::SourceZ::BARO),
83 :
84 : // @Param: 2_VELZ
85 : // @DisplayName: Velocity Vertical Source (Secondary)
86 : // @Description: Velocity Vertical Source (Secondary)
87 : // @Values: 0:None, 3:GPS, 4:Beacon, 6:ExternalNav
88 : // @User: Advanced
89 : AP_GROUPINFO("2_VELZ", 9, AP_NavEKF_Source, _source_set[1].velz, (int8_t)AP_NavEKF_Source::SourceZ::NONE),
90 :
91 : // @Param: 2_YAW
92 : // @DisplayName: Yaw Source (Secondary)
93 : // @Description: Yaw Source (Secondary)
94 : // @Values: 0:None, 1:Compass, 2:GPS, 3:GPS with Compass Fallback, 6:ExternalNav, 8:GSF
95 : // @User: Advanced
96 : AP_GROUPINFO("2_YAW", 10, AP_NavEKF_Source, _source_set[1].yaw, (int8_t)AP_NavEKF_Source::SourceYaw::NONE),
97 : #endif
98 :
99 : #if AP_NAKEKF_SOURCE_SET_MAX >= 3
100 : // @Param: 3_POSXY
101 : // @DisplayName: Position Horizontal Source (Tertiary)
102 : // @Description: Position Horizontal Source (Tertiary)
103 : // @Values: 0:None, 3:GPS, 4:Beacon, 6:ExternalNav
104 : // @User: Advanced
105 : AP_GROUPINFO("3_POSXY", 11, AP_NavEKF_Source, _source_set[2].posxy, (int8_t)AP_NavEKF_Source::SourceXY::NONE),
106 :
107 : // @Param: 3_VELXY
108 : // @DisplayName: Velocity Horizontal Source (Tertiary)
109 : // @Description: Velocity Horizontal Source (Tertiary)
110 : // @Values: 0:None, 3:GPS, 4:Beacon, 5:OpticalFlow, 6:ExternalNav, 7:WheelEncoder
111 : // @User: Advanced
112 : AP_GROUPINFO("3_VELXY", 12, AP_NavEKF_Source, _source_set[2].velxy, (int8_t)AP_NavEKF_Source::SourceXY::NONE),
113 :
114 : // @Param: 3_POSZ
115 : // @DisplayName: Position Vertical Source (Tertiary)
116 : // @Description: Position Vertical Source (Tertiary)
117 : // @Values: 0:None, 1:Baro, 2:RangeFinder, 3:GPS, 4:Beacon, 6:ExternalNav
118 : // @User: Advanced
119 : AP_GROUPINFO("3_POSZ", 13, AP_NavEKF_Source, _source_set[2].posz, (int8_t)AP_NavEKF_Source::SourceZ::BARO),
120 :
121 : // @Param: 3_VELZ
122 : // @DisplayName: Velocity Vertical Source (Tertiary)
123 : // @Description: Velocity Vertical Source (Tertiary)
124 : // @Values: 0:None, 3:GPS, 4:Beacon, 6:ExternalNav
125 : // @User: Advanced
126 : AP_GROUPINFO("3_VELZ", 14, AP_NavEKF_Source, _source_set[2].velz, (int8_t)AP_NavEKF_Source::SourceZ::NONE),
127 :
128 : // @Param: 3_YAW
129 : // @DisplayName: Yaw Source (Tertiary)
130 : // @Description: Yaw Source (Tertiary)
131 : // @Values: 0:None, 1:Compass, 2:GPS, 3:GPS with Compass Fallback, 6:ExternalNav, 8:GSF
132 : // @User: Advanced
133 : AP_GROUPINFO("3_YAW", 15, AP_NavEKF_Source, _source_set[2].yaw, (int8_t)AP_NavEKF_Source::SourceYaw::NONE),
134 : #endif
135 :
136 : // @Param: _OPTIONS
137 : // @DisplayName: EKF Source Options
138 : // @Description: EKF Source Options. Bit 0: Fuse all velocity sources present in EK3_SRCx_VEL_. Bit 1: Align external navigation position when using optical flow. Bit 3: Use SRC per core. By default, EKF source selection is controlled via the EK3_SRC parameters, allowing only one source to be active at a time across all cores (switchable via MAVLink, Lua, or RC). Enabling this bit maps EKF core 1 to SRC1, core 2 to SRC2, etc., allowing each core to run independently with a dedicated source.
139 : // @Bitmask: 0:FuseAllVelocities, 1:AlignExtNavPosWhenUsingOptFlow, 3: UsePerCoreEKFSources
140 : // @User: Advanced
141 : AP_GROUPINFO("_OPTIONS", 16, AP_NavEKF_Source, _options, 0),
142 :
143 : AP_GROUPEND
144 : };
145 :
146 8772 : AP_NavEKF_Source::AP_NavEKF_Source()
147 : {
148 2193 : AP_Param::setup_object_defaults(this, var_info);
149 2193 : }
150 :
151 : // set position, velocity and yaw sources to either 0=primary, 1=secondary, 2=tertiary
152 16 : void AP_NavEKF_Source::setPosVelYawSourceSet(AP_NavEKF_Source::SourceSetSelection source_set_idx)
153 : {
154 : // sanity check source idx
155 16 : if ((uint8_t)source_set_idx < AP_NAKEKF_SOURCE_SET_MAX) {
156 16 : active_source_set = (uint8_t)source_set_idx;
157 : #if HAL_LOGGING_ENABLED
158 : static const LogEvent evt[AP_NAKEKF_SOURCE_SET_MAX] {
159 : LogEvent::EK3_SOURCES_SET_TO_PRIMARY,
160 : LogEvent::EK3_SOURCES_SET_TO_SECONDARY,
161 : LogEvent::EK3_SOURCES_SET_TO_TERTIARY,
162 : };
163 16 : AP::logger().Write_Event(evt[active_source_set]);
164 : #endif
165 : }
166 16 : }
167 :
168 : // true/false of whether velocity source should be used
169 133536439 : bool AP_NavEKF_Source::useVelXYSource(SourceXY velxy_source, uint8_t core_index) const
170 : {
171 133536439 : if (velxy_source == _source_set[getActiveSourceSet(core_index)].velxy) {
172 45536533 : return true;
173 : }
174 :
175 : // check for fuse all velocities
176 87999906 : if (option_is_set(SourceOptions::FUSE_ALL_VELOCITIES)) {
177 0 : for (uint8_t i=0; i<AP_NAKEKF_SOURCE_SET_MAX; i++) {
178 0 : if (_source_set[i].velxy == velxy_source) {
179 0 : return true;
180 : }
181 : }
182 : }
183 :
184 : // if we got this far source should not be used
185 87999906 : return false;
186 : }
187 :
188 7888062 : bool AP_NavEKF_Source::useVelZSource(SourceZ velz_source, uint8_t core_index) const
189 : {
190 7888062 : if (velz_source == _source_set[getActiveSourceSet(core_index)].velz) {
191 6068930 : return true;
192 : }
193 :
194 : // check for fuse all velocities
195 1819132 : if (option_is_set(SourceOptions::FUSE_ALL_VELOCITIES)) {
196 0 : for (uint8_t i=0; i<AP_NAKEKF_SOURCE_SET_MAX; i++) {
197 0 : if (_source_set[i].velz == velz_source) {
198 0 : return true;
199 : }
200 : }
201 : }
202 :
203 : // if we got this far source should not be used
204 1819132 : return false;
205 : }
206 :
207 : // true if a velocity source is configured
208 0 : bool AP_NavEKF_Source::haveVelZSource(uint8_t core_index) const
209 : {
210 0 : if (_source_set[getActiveSourceSet(core_index)].velz != SourceZ::NONE) {
211 0 : return true;
212 : }
213 :
214 : // check for fuse all velocities
215 0 : if (option_is_set(SourceOptions::FUSE_ALL_VELOCITIES)) {
216 0 : for (uint8_t i=0; i<AP_NAKEKF_SOURCE_SET_MAX; i++) {
217 0 : if (_source_set[i].velz != SourceZ::NONE) {
218 0 : return true;
219 : }
220 : }
221 : }
222 :
223 : // if we got this far no velocity z source has been configured
224 0 : return false;
225 : }
226 :
227 : // get yaw source
228 78808863 : AP_NavEKF_Source::SourceYaw AP_NavEKF_Source::getYawSource(uint8_t core_index) const
229 : {
230 : // check for special case of disabled compasses
231 78808863 : if ((_source_set[getActiveSourceSet(core_index)].yaw == SourceYaw::COMPASS) && (AP::dal().compass().get_num_enabled() == 0)) {
232 26820 : return SourceYaw::NONE;
233 : }
234 :
235 78782043 : return _source_set[getActiveSourceSet(core_index)].yaw;
236 : }
237 :
238 : // get pos Z source
239 78691648 : AP_NavEKF_Source::SourceZ AP_NavEKF_Source::getPosZSource(uint8_t core_index) const
240 : {
241 : #ifdef HAL_BARO_ALLOW_INIT_NO_BARO
242 : // check for special case of missing baro
243 : if ((_source_set[getActiveSourceSet(core_index)].posz == SourceZ::BARO) && (AP::dal().baro().num_instances() == 0)) {
244 : return SourceZ::NONE;
245 : }
246 : #endif
247 78691648 : return _source_set[getActiveSourceSet(core_index)].posz;
248 : }
249 :
250 : // align position of inactive sources to ahrs
251 39298372 : void AP_NavEKF_Source::align_inactive_sources()
252 : {
253 : #if HAL_VISUALODOM_ENABLED
254 39298372 : auto *visual_odom = AP::dal().visualodom();
255 39298372 : if (!visual_odom || !visual_odom->enabled()) {
256 39019894 : return;
257 : }
258 :
259 278478 : bool allow_align_posxy = true;
260 278478 : bool allow_align_posz = true;
261 :
262 : // Check active XY sources across all EKF cores
263 634326 : for (uint8_t i = 0; i < MAX_EKF_CORES; i++) {
264 531821 : const SourceXY pos_xy_source = getPosXYSource(i);
265 531821 : const SourceXY vel_xy_source = getVelXYSource(i);
266 :
267 531821 : if (pos_xy_source == SourceXY::EXTNAV) {
268 : // ExtNav is actively being used, do not align XY
269 175973 : allow_align_posxy = false;
270 175973 : break;
271 : }
272 :
273 : const bool valid_xy =
274 0 : (pos_xy_source == SourceXY::GPS) ||
275 355848 : (pos_xy_source == SourceXY::BEACON) ||
276 0 : ((vel_xy_source == SourceXY::OPTFLOW) &&
277 0 : option_is_set(SourceOptions::ALIGN_EXTNAV_POS_WHEN_USING_OPTFLOW));
278 :
279 355848 : if (!valid_xy) {
280 0 : allow_align_posxy = false;
281 0 : break;
282 : }
283 : }
284 :
285 : // Check active Z sources across all EKF cores
286 738546 : for (uint8_t i = 0; i < MAX_EKF_CORES; i++) {
287 601301 : const SourceZ pos_z_source = getPosZSource(i);
288 :
289 601301 : if (pos_z_source == SourceZ::EXTNAV) {
290 : // ExtNav is actively being used, do not align Z
291 141233 : allow_align_posz = false;
292 141233 : break;
293 : }
294 :
295 460068 : const bool valid_z =
296 0 : (pos_z_source == SourceZ::BARO) ||
297 0 : (pos_z_source == SourceZ::RANGEFINDER) ||
298 460068 : (pos_z_source == SourceZ::GPS) ||
299 : (pos_z_source == SourceZ::BEACON);
300 :
301 460068 : if (!valid_z) {
302 0 : allow_align_posz = false;
303 0 : break;
304 : }
305 : }
306 :
307 : // Check if ExtNav is available in any source set
308 278478 : bool extnav_available_xy = false;
309 278478 : bool extnav_available_z = false;
310 :
311 1113912 : for (uint8_t i = 0; i < AP_NAKEKF_SOURCE_SET_MAX; i++) {
312 835434 : if (_source_set[i].posxy == SourceXY::EXTNAV) {
313 206091 : extnav_available_xy = true;
314 : }
315 835434 : if (_source_set[i].posz == SourceZ::EXTNAV) {
316 171351 : extnav_available_z = true;
317 : }
318 : }
319 :
320 : // Align only if allowed and ExtNav source available
321 278478 : visual_odom->align_position_to_ahrs(
322 278478 : allow_align_posxy && extnav_available_xy,
323 278478 : allow_align_posz && extnav_available_z
324 : );
325 :
326 : #endif
327 : }
328 :
329 : // sensor specific helper functions
330 40101292 : bool AP_NavEKF_Source::usingGPS(uint8_t core_index) const
331 : {
332 40530089 : return getPosXYSource(core_index) == SourceXY::GPS ||
333 857594 : getPosZSource(core_index) == SourceZ::GPS ||
334 853829 : getVelXYSource(core_index) == SourceXY::GPS ||
335 40955121 : getVelZSource(core_index) == SourceZ::GPS ||
336 40526236 : getYawSource(core_index) == SourceYaw::GSF;
337 : }
338 :
339 : // true if some parameters have been configured (used during parameter conversion)
340 0 : bool AP_NavEKF_Source::configured()
341 : {
342 0 : if (_configured) {
343 0 : return true;
344 : }
345 :
346 : // first source parameter is used to determine if configured or not
347 0 : _configured = _source_set[0].posxy.configured();
348 :
349 0 : return _configured;
350 : }
351 :
352 : // mark parameters as configured (used to ensure parameter conversion is only done once)
353 0 : void AP_NavEKF_Source::mark_configured()
354 : {
355 : // save first parameter's current value to mark as configured
356 0 : return _source_set[0].posxy.save(true);
357 : }
358 :
359 : // returns false if we fail arming checks, in which case the buffer will be populated with a failure message
360 : // requires_position should be true if vertical or horizontal position configuration should be checked
361 31446 : bool AP_NavEKF_Source::pre_arm_check(bool requires_position, char *failure_msg, uint8_t failure_msg_len) const
362 : {
363 31446 : auto &dal = AP::dal();
364 31446 : bool baro_required = false;
365 31446 : bool beacon_required = false;
366 31446 : bool compass_required = false;
367 31446 : bool gps_required = false;
368 31446 : bool rangefinder_required = false;
369 31446 : bool visualodom_required = false;
370 31446 : bool optflow_required = false;
371 31446 : bool wheelencoder_required = false;
372 :
373 : // check source params are valid
374 125782 : for (uint8_t i=0; i<AP_NAKEKF_SOURCE_SET_MAX; i++) {
375 :
376 94338 : if (requires_position) {
377 : // check posxy
378 56124 : switch ((SourceXY)_source_set[i].posxy.get()) {
379 37411 : case SourceXY::NONE:
380 37411 : break;
381 18665 : case SourceXY::GPS:
382 18665 : gps_required = true;
383 18665 : break;
384 0 : case SourceXY::BEACON:
385 0 : beacon_required = true;
386 0 : break;
387 48 : case SourceXY::EXTNAV:
388 48 : visualodom_required = true;
389 48 : break;
390 0 : case SourceXY::OPTFLOW:
391 : case SourceXY::WHEEL_ENCODER:
392 : default:
393 : // invalid posxy value
394 0 : hal.util->snprintf(failure_msg, failure_msg_len, "Check EK3_SRC%d_POSXY", (int)i+1);
395 0 : return false;
396 : }
397 :
398 : // check velxy
399 56124 : switch ((SourceXY)_source_set[i].velxy.get()) {
400 37368 : case SourceXY::NONE:
401 37368 : break;
402 18665 : case SourceXY::GPS:
403 18665 : gps_required = true;
404 18665 : break;
405 43 : case SourceXY::OPTFLOW:
406 43 : optflow_required = true;
407 43 : break;
408 48 : case SourceXY::EXTNAV:
409 48 : visualodom_required = true;
410 48 : break;
411 0 : case SourceXY::WHEEL_ENCODER:
412 0 : wheelencoder_required = true;
413 0 : break;
414 0 : case SourceXY::BEACON:
415 : default:
416 : // invalid velxy value
417 0 : hal.util->snprintf(failure_msg, failure_msg_len, "Check EK3_SRC%d_VELXY", (int)i+1);
418 0 : return false;
419 : }
420 :
421 : // check posz
422 56124 : switch ((SourceZ)_source_set[i].posz.get()) {
423 56036 : case SourceZ::BARO:
424 56036 : baro_required = true;
425 56036 : break;
426 0 : case SourceZ::RANGEFINDER:
427 0 : rangefinder_required = true;
428 0 : break;
429 58 : case SourceZ::GPS:
430 58 : gps_required = true;
431 58 : break;
432 0 : case SourceZ::BEACON:
433 0 : beacon_required = true;
434 0 : break;
435 30 : case SourceZ::EXTNAV:
436 30 : visualodom_required = true;
437 30 : break;
438 0 : case SourceZ::NONE:
439 0 : break;
440 0 : default:
441 : // invalid posz value
442 0 : hal.util->snprintf(failure_msg, failure_msg_len, "Check EK3_SRC%d_POSZ", (int)i+1);
443 0 : return false;
444 : }
445 :
446 : // check velz
447 56124 : switch ((SourceZ)_source_set[i].velz.get()) {
448 37429 : case SourceZ::NONE:
449 37429 : break;
450 18665 : case SourceZ::GPS:
451 18665 : gps_required = true;
452 18665 : break;
453 30 : case SourceZ::EXTNAV:
454 30 : visualodom_required = true;
455 30 : break;
456 0 : case SourceZ::BARO:
457 : case SourceZ::RANGEFINDER:
458 : case SourceZ::BEACON:
459 : default:
460 : // invalid velz value
461 0 : hal.util->snprintf(failure_msg, failure_msg_len, "Check EK3_SRC%d_VELZ", (int)i+1);
462 0 : return false;
463 : }
464 : }
465 :
466 : // check yaw
467 94338 : switch ((SourceYaw)_source_set[i].yaw.get()) {
468 62897 : case SourceYaw::NONE:
469 : case SourceYaw::GPS:
470 : // valid yaw value
471 62897 : break;
472 31275 : case SourceYaw::COMPASS:
473 : // skip compass check for easier user setup of compass-less operation
474 31275 : break;
475 74 : case SourceYaw::GPS_COMPASS_FALLBACK:
476 74 : compass_required = true;
477 74 : break;
478 90 : case SourceYaw::EXTNAV:
479 90 : visualodom_required = true;
480 90 : break;
481 0 : case SourceYaw::GSF:
482 0 : gps_required = true;
483 0 : break;
484 2 : default:
485 : // invalid yaw value
486 2 : hal.util->snprintf(failure_msg, failure_msg_len, "Check EK3_SRC%d_YAW", (int)i+1);
487 2 : return false;
488 : }
489 : }
490 :
491 : // check all required sensors are available
492 31444 : const char* ekf_requires_msg = "EK3 sources require %s";
493 31444 : if (baro_required && (dal.baro().num_instances() == 0)) {
494 0 : hal.util->snprintf(failure_msg, failure_msg_len, ekf_requires_msg, "Baro");
495 0 : return false;
496 : }
497 :
498 31444 : if (beacon_required) {
499 : #if AP_BEACON_ENABLED
500 0 : const bool beacon_available = (dal.beacon() != nullptr && dal.beacon()->enabled());
501 : #else
502 : const bool beacon_available = false;
503 : #endif
504 0 : if (!beacon_available) {
505 0 : hal.util->snprintf(failure_msg, failure_msg_len, ekf_requires_msg, "Beacon");
506 0 : return false;
507 : }
508 : }
509 :
510 31444 : if (compass_required && (dal.compass().get_num_enabled() == 0)) {
511 1 : hal.util->snprintf(failure_msg, failure_msg_len, ekf_requires_msg, "Compass");
512 1 : return false;
513 : }
514 :
515 31443 : if (gps_required && (dal.gps().num_sensors() == 0)) {
516 5 : hal.util->snprintf(failure_msg, failure_msg_len, ekf_requires_msg, "GPS");
517 5 : return false;
518 : }
519 :
520 31438 : if (optflow_required && !dal.opticalflow_enabled()) {
521 2 : hal.util->snprintf(failure_msg, failure_msg_len, ekf_requires_msg, "OpticalFlow");
522 2 : return false;
523 : }
524 :
525 31436 : if (rangefinder_required) {
526 : #if AP_RANGEFINDER_ENABLED
527 0 : const bool have_rangefinder = (dal.rangefinder() != nullptr && dal.rangefinder()->has_orientation(ROTATION_PITCH_270));
528 : #else
529 : const bool have_rangefinder = false;
530 : #endif
531 0 : if (!have_rangefinder) {
532 0 : hal.util->snprintf(failure_msg, failure_msg_len, ekf_requires_msg, "RangeFinder");
533 0 : return false;
534 : }
535 : }
536 :
537 31436 : if (visualodom_required) {
538 105 : bool visualodom_available = false;
539 : #if HAL_VISUALODOM_ENABLED
540 105 : auto *vo = AP::dal().visualodom();
541 105 : visualodom_available = vo && vo->enabled();
542 : #endif
543 105 : if (!visualodom_available) {
544 3 : hal.util->snprintf(failure_msg, failure_msg_len, ekf_requires_msg, "VisualOdom");
545 3 : return false;
546 : }
547 : }
548 :
549 31433 : if (wheelencoder_required && !dal.wheelencoder_enabled()) {
550 0 : hal.util->snprintf(failure_msg, failure_msg_len, ekf_requires_msg, "WheelEncoder");
551 0 : return false;
552 : }
553 :
554 31433 : return true;
555 : }
556 :
557 : // return true if ext nav is enabled on any source
558 15820 : bool AP_NavEKF_Source::ext_nav_enabled(void) const
559 : {
560 63072 : for (uint8_t i=0; i<AP_NAKEKF_SOURCE_SET_MAX; i++) {
561 47332 : const auto &src = _source_set[i];
562 47332 : if (src.posxy == SourceXY::EXTNAV) {
563 48 : return true;
564 : }
565 47284 : if (src.posz == SourceZ::EXTNAV) {
566 0 : return true;
567 : }
568 47284 : if (src.velxy == SourceXY::EXTNAV) {
569 16 : return true;
570 : }
571 47268 : if (src.velz == SourceZ::EXTNAV) {
572 0 : return true;
573 : }
574 47268 : if (src.yaw == SourceYaw::EXTNAV) {
575 16 : return true;
576 : }
577 : }
578 15740 : return false;
579 : }
580 :
581 : // return true if wheel encoder is enabled on any source
582 3955 : bool AP_NavEKF_Source::wheel_encoder_enabled(void) const
583 : {
584 15820 : for (uint8_t i=0; i<AP_NAKEKF_SOURCE_SET_MAX; i++) {
585 11865 : const auto &src = _source_set[i];
586 11865 : if (src.velxy == SourceXY::WHEEL_ENCODER) {
587 0 : return true;
588 : }
589 : }
590 3955 : return false;
591 : }
592 :
593 : // returns active source set
594 0 : uint8_t AP_NavEKF_Source::get_active_source_set() const
595 : {
596 0 : return active_source_set;
597 : }
598 :
599 : // return true if GPS yaw is enabled on any source
600 3955 : bool AP_NavEKF_Source::gps_yaw_enabled(void) const
601 : {
602 15760 : for (uint8_t i=0; i<AP_NAKEKF_SOURCE_SET_MAX; i++) {
603 11825 : const auto &src = _source_set[i];
604 11825 : const SourceYaw yaw = SourceYaw(src.yaw.get());
605 11825 : if (yaw == SourceYaw::GPS ||
606 : yaw == SourceYaw::GPS_COMPASS_FALLBACK) {
607 20 : return true;
608 : }
609 : }
610 3935 : return false;
611 : }
|
