Line data Source code
1 : #include "AP_Vehicle_config.h"
2 :
3 : #if AP_VEHICLE_ENABLED
4 :
5 : #include "AP_Vehicle.h"
6 : #include <AP_InertialSensor/AP_InertialSensor_rate_config.h>
7 :
8 : #include <AP_BLHeli/AP_BLHeli.h>
9 : #include <AP_Common/AP_FWVersion.h>
10 : #include <AP_Arming/AP_Arming.h>
11 : #include <AP_Frsky_Telem/AP_Frsky_Parameters.h>
12 : #include <AP_Logger/AP_Logger.h>
13 : #include <AP_Mission/AP_Mission.h>
14 : #include <AP_OSD/AP_OSD.h>
15 : #include <SRV_Channel/SRV_Channel.h>
16 : #include <AP_Motors/AP_Motors.h>
17 : #include <AR_Motors/AP_MotorsUGV.h>
18 : #include <AP_CheckFirmware/AP_CheckFirmware.h>
19 : #include <GCS_MAVLink/GCS.h>
20 : #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
21 : #include <AP_HAL_ChibiOS/sdcard.h>
22 : #include <AP_HAL_ChibiOS/hwdef/common/stm32_util.h>
23 : #endif
24 : #include <AP_DDS/AP_DDS_Client.h>
25 : #if HAL_WITH_IO_MCU
26 : #include <AP_IOMCU/AP_IOMCU.h>
27 : extern AP_IOMCU iomcu;
28 : #endif
29 : #include <AP_Scripting/AP_Scripting.h>
30 : #include <SITL/SITL.h>
31 :
32 : #define SCHED_TASK(func, rate_hz, max_time_micros, prio) SCHED_TASK_CLASS(AP_Vehicle, &vehicle, func, rate_hz, max_time_micros, prio)
33 :
34 : /*
35 : 2nd group of parameters
36 : */
37 : const AP_Param::GroupInfo AP_Vehicle::var_info[] = {
38 :
39 : // 1: RunCam
40 :
41 : #if HAL_GYROFFT_ENABLED
42 : // @Group: FFT_
43 : // @Path: ../AP_GyroFFT/AP_GyroFFT.cpp
44 : AP_SUBGROUPINFO(gyro_fft, "FFT_", 2, AP_Vehicle, AP_GyroFFT),
45 : #endif
46 :
47 : #if HAL_VISUALODOM_ENABLED
48 : // @Group: VISO
49 : // @Path: ../AP_VisualOdom/AP_VisualOdom.cpp
50 : AP_SUBGROUPINFO(visual_odom, "VISO", 3, AP_Vehicle, AP_VisualOdom),
51 : #endif
52 :
53 : #if AP_VIDEOTX_ENABLED
54 : // @Group: VTX_
55 : // @Path: ../AP_VideoTX/AP_VideoTX.cpp
56 : AP_SUBGROUPINFO(vtx, "VTX_", 4, AP_Vehicle, AP_VideoTX),
57 : #endif
58 :
59 : #if HAL_MSP_ENABLED
60 : // @Group: MSP
61 : // @Path: ../AP_MSP/AP_MSP.cpp
62 : AP_SUBGROUPINFO(msp, "MSP", 5, AP_Vehicle, AP_MSP),
63 : #endif
64 :
65 : #if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
66 : // @Group: FRSKY_
67 : // @Path: ../AP_Frsky_Telem/AP_Frsky_Parameters.cpp
68 : AP_SUBGROUPINFO(frsky_parameters, "FRSKY_", 6, AP_Vehicle, AP_Frsky_Parameters),
69 : #endif
70 :
71 : #if HAL_GENERATOR_ENABLED
72 : // @Group: GEN_
73 : // @Path: ../AP_Generator/AP_Generator.cpp
74 : AP_SUBGROUPINFO(generator, "GEN_", 7, AP_Vehicle, AP_Generator),
75 : #endif
76 :
77 : #if AP_EXTERNAL_AHRS_ENABLED
78 : // @Group: EAHRS
79 : // @Path: ../AP_ExternalAHRS/AP_ExternalAHRS.cpp
80 : AP_SUBGROUPINFO(externalAHRS, "EAHRS", 8, AP_Vehicle, AP_ExternalAHRS),
81 : #endif
82 :
83 : #if HAL_EFI_ENABLED
84 : // @Group: EFI
85 : // @Path: ../AP_EFI/AP_EFI.cpp
86 : AP_SUBGROUPINFO(efi, "EFI", 9, AP_Vehicle, AP_EFI),
87 : #endif
88 :
89 : #if AP_AIRSPEED_ENABLED
90 : // @Group: ARSPD
91 : // @Path: ../AP_Airspeed/AP_Airspeed.cpp
92 : AP_SUBGROUPINFO(airspeed, "ARSPD", 10, AP_Vehicle, AP_Airspeed),
93 : #endif
94 :
95 : #if AP_CUSTOMROTATIONS_ENABLED
96 : // @Group: CUST_ROT
97 : // @Path: ../AP_CustomRotations/AP_CustomRotations.cpp
98 : AP_SUBGROUPINFO(custom_rotations, "CUST_ROT", 11, AP_Vehicle, AP_CustomRotations),
99 : #endif
100 :
101 : #if HAL_WITH_ESC_TELEM
102 : // @Group: ESC_TLM
103 : // @Path: ../AP_ESC_Telem/AP_ESC_Telem.cpp
104 : AP_SUBGROUPINFO(esc_telem, "ESC_TLM", 12, AP_Vehicle, AP_ESC_Telem),
105 : #endif
106 :
107 : #if AP_AIS_ENABLED
108 : // @Group: AIS_
109 : // @Path: ../AP_AIS/AP_AIS.cpp
110 : AP_SUBGROUPINFO(ais, "AIS_", 13, AP_Vehicle, AP_AIS),
111 : #endif
112 :
113 : #if AP_FENCE_ENABLED
114 : // @Group: FENCE_
115 : // @Path: ../AC_Fence/AC_Fence.cpp
116 : AP_SUBGROUPINFO(fence, "FENCE_", 14, AP_Vehicle, AC_Fence),
117 : #endif
118 :
119 : #if AP_OPENDRONEID_ENABLED
120 : // @Group: DID_
121 : // @Path: ../AP_OpenDroneID/AP_OpenDroneID.cpp
122 : AP_SUBGROUPINFO(opendroneid, "DID_", 15, AP_Vehicle, AP_OpenDroneID),
123 : #endif
124 :
125 : #if AP_TEMPERATURE_SENSOR_ENABLED
126 : // @Group: TEMP
127 : // @Path: ../AP_TemperatureSensor/AP_TemperatureSensor.cpp
128 : AP_SUBGROUPINFO(temperature_sensor, "TEMP", 16, AP_Vehicle, AP_TemperatureSensor),
129 : #endif
130 :
131 : #if HAL_NMEA_OUTPUT_ENABLED
132 : // @Group: NMEA_
133 : // @Path: ../AP_NMEA_Output/AP_NMEA_Output.cpp
134 : AP_SUBGROUPINFO(nmea, "NMEA_", 17, AP_Vehicle, AP_NMEA_Output),
135 : #endif
136 :
137 : #if AP_DDS_ENABLED
138 : // @Group: DDS
139 : // @Path: ../AP_DDS/AP_DDS_Client.cpp
140 : AP_SUBGROUPPTR(dds_client, "DDS", 18, AP_Vehicle, AP_DDS_Client),
141 : #endif
142 :
143 : #if AP_KDECAN_ENABLED
144 : // @Group: KDE_
145 : // @Path: ../AP_KDECAN/AP_KDECAN.cpp
146 : AP_SUBGROUPINFO(kdecan, "KDE_", 19, AP_Vehicle, AP_KDECAN),
147 : #endif
148 :
149 : #if APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_ArduPlane) || APM_BUILD_TYPE(APM_BUILD_Rover)
150 : // @Param: FLTMODE_GCSBLOCK
151 : // @DisplayName: Flight mode block from GCS
152 : // @Description: Bitmask of flight modes to disable for GCS selection. Mode can still be accessed via RC or failsafe.
153 : // @Bitmask{Copter}: 0:Stabilize
154 : // @Bitmask{Copter}: 1:Acro
155 : // @Bitmask{Copter}: 2:AltHold
156 : // @Bitmask{Copter}: 3:Auto
157 : // @Bitmask{Copter}: 4:Guided
158 : // @Bitmask{Copter}: 5:Loiter
159 : // @Bitmask{Copter}: 6:Circle
160 : // @Bitmask{Copter}: 7:Drift
161 : // @Bitmask{Copter}: 8:Sport
162 : // @Bitmask{Copter}: 9:Flip
163 : // @Bitmask{Copter}: 10:AutoTune
164 : // @Bitmask{Copter}: 11:PosHold
165 : // @Bitmask{Copter}: 12:Brake
166 : // @Bitmask{Copter}: 13:Throw
167 : // @Bitmask{Copter}: 14:Avoid_ADSB
168 : // @Bitmask{Copter}: 15:Guided_NoGPS
169 : // @Bitmask{Copter}: 16:Smart_RTL
170 : // @Bitmask{Copter}: 17:FlowHold
171 : // @Bitmask{Copter}: 18:Follow
172 : // @Bitmask{Copter}: 19:ZigZag
173 : // @Bitmask{Copter}: 20:SystemID
174 : // @Bitmask{Copter}: 21:Heli_Autorotate
175 : // @Bitmask{Copter}: 22:Auto RTL
176 : // @Bitmask{Copter}: 23:Turtle
177 : // @Bitmask{Plane}: 0:Manual
178 : // @Bitmask{Plane}: 1:Circle
179 : // @Bitmask{Plane}: 2:Stabilize
180 : // @Bitmask{Plane}: 3:Training
181 : // @Bitmask{Plane}: 4:ACRO
182 : // @Bitmask{Plane}: 5:FBWA
183 : // @Bitmask{Plane}: 6:FBWB
184 : // @Bitmask{Plane}: 7:CRUISE
185 : // @Bitmask{Plane}: 8:AUTOTUNE
186 : // @Bitmask{Plane}: 9:Auto
187 : // @Bitmask{Plane}: 10:Loiter
188 : // @Bitmask{Plane}: 11:Takeoff
189 : // @Bitmask{Plane}: 12:AVOID_ADSB
190 : // @Bitmask{Plane}: 13:Guided
191 : // @Bitmask{Plane}: 14:THERMAL
192 : // @Bitmask{Plane}: 15:QSTABILIZE
193 : // @Bitmask{Plane}: 16:QHOVER
194 : // @Bitmask{Plane}: 17:QLOITER
195 : // @Bitmask{Plane}: 18:QACRO
196 : // @Bitmask{Plane}: 19:QAUTOTUNE
197 : // @Bitmask{Plane}: 20:Loiter to QLand
198 : // @Bitmask{Plane}: 21:Autoland
199 : // @Bitmask{Rover}: 0:Manual
200 : // @Bitmask{Rover}: 1:Acro
201 : // @Bitmask{Rover}: 2:Steering
202 : // @Bitmask{Rover}: 3:Loiter
203 : // @Bitmask{Rover}: 4:Follow
204 : // @Bitmask{Rover}: 5:Simple
205 : // @Bitmask{Rover}: 6:Circle
206 : // @Bitmask{Rover}: 7:Auto
207 : // @Bitmask{Rover}: 8:RTL
208 : // @Bitmask{Rover}: 9:SmartRTL
209 : // @Bitmask{Rover}: 10:Guided
210 : // @Bitmask{Rover}: 11:Dock
211 : // @User: Standard
212 : AP_GROUPINFO("FLTMODE_GCSBLOCK", 20, AP_Vehicle, flight_mode_GCS_block, 0),
213 : #endif // APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_ArduPlane) || APM_BUILD_TYPE(APM_BUILD_Rover)
214 :
215 :
216 : #if AP_NETWORKING_ENABLED
217 : // @Group: NET_
218 : // @Path: ../AP_Networking/AP_Networking.cpp
219 : AP_SUBGROUPINFO(networking, "NET_", 21, AP_Vehicle, AP_Networking),
220 :
221 : /*
222 : the NET_Pn_ parameters need to be in AP_Vehicle as otherwise we
223 : are too deep in the parameter tree
224 : */
225 :
226 : #if AP_NETWORKING_REGISTER_PORT_ENABLED
227 : #if AP_NETWORKING_NUM_PORTS > 0
228 : // @Group: NET_P1_
229 : // @Path: ../AP_Networking/AP_Networking_port.cpp
230 : AP_SUBGROUPINFO(networking.ports[0], "NET_P1_", 22, AP_Vehicle, AP_Networking::Port),
231 : #endif
232 :
233 : #if AP_NETWORKING_NUM_PORTS > 1
234 : // @Group: NET_P2_
235 : // @Path: ../AP_Networking/AP_Networking_port.cpp
236 : AP_SUBGROUPINFO(networking.ports[1], "NET_P2_", 23, AP_Vehicle, AP_Networking::Port),
237 : #endif
238 :
239 : #if AP_NETWORKING_NUM_PORTS > 2
240 : // @Group: NET_P3_
241 : // @Path: ../AP_Networking/AP_Networking_port.cpp
242 : AP_SUBGROUPINFO(networking.ports[2], "NET_P3_", 24, AP_Vehicle, AP_Networking::Port),
243 : #endif
244 :
245 : #if AP_NETWORKING_NUM_PORTS > 3
246 : // @Group: NET_P4_
247 : // @Path: ../AP_Networking/AP_Networking_port.cpp
248 : AP_SUBGROUPINFO(networking.ports[3], "NET_P4_", 25, AP_Vehicle, AP_Networking::Port),
249 : #endif
250 : #endif // AP_NETWORKING_REGISTER_PORT_ENABLED
251 : #endif // AP_NETWORKING_ENABLED
252 :
253 : #if AP_FILTER_ENABLED
254 : // @Group: FILT
255 : // @Path: ../Filter/AP_Filter.cpp
256 : AP_SUBGROUPINFO(filters, "FILT", 26, AP_Vehicle, AP_Filters),
257 : #endif
258 :
259 : #if AP_STATS_ENABLED
260 : // @Group: STAT
261 : // @Path: ../AP_Stats/AP_Stats.cpp
262 : AP_SUBGROUPINFO(stats, "STAT", 27, AP_Vehicle, AP_Stats),
263 : #endif
264 :
265 : #if AP_SCRIPTING_ENABLED
266 : // @Group: SCR_
267 : // @Path: ../AP_Scripting/AP_Scripting.cpp
268 : AP_SUBGROUPINFO(scripting, "SCR_", 28, AP_Vehicle, AP_Scripting),
269 : #endif
270 :
271 : #if HAL_LOGGING_ENABLED
272 : // @Group: LOG
273 : // @Path: ../AP_Logger/AP_Logger.cpp
274 : AP_SUBGROUPINFO(logger, "LOG", 29, AP_Vehicle, AP_Logger),
275 : #endif
276 :
277 : #if AP_GRIPPER_ENABLED
278 : // @Group: GRIP_
279 : // @Path: ../AP_Gripper/AP_Gripper.cpp
280 : AP_SUBGROUPINFO(gripper, "GRIP_", 30, AP_Vehicle, AP_Gripper),
281 : #endif
282 :
283 : #if AP_SERIALMANAGER_ENABLED
284 : // @Group: SERIAL
285 : // @Path: ../AP_SerialManager/AP_SerialManager.cpp
286 : AP_SUBGROUPINFO(serial_manager, "SERIAL", 31, AP_Vehicle, AP_SerialManager),
287 : #endif
288 :
289 : #if AP_RPM_ENABLED
290 : // @Group: RPM
291 : // @Path: ../AP_RPM/AP_RPM.cpp
292 : AP_SUBGROUPINFO(rpm_sensor, "RPM", 32, AP_Vehicle, AP_RPM),
293 : #endif
294 :
295 : #if AP_BEACON_ENABLED
296 : // @Group: BCN
297 : // @Path: ../AP_Beacon/AP_Beacon.cpp
298 : AP_SUBGROUPINFO(beacon, "BCN", 33, AP_Vehicle, AP_Beacon),
299 : #endif // AP_BEACON_ENABLED
300 :
301 : AP_GROUPEND
302 : };
303 :
304 : // reference to the vehicle. using AP::vehicle() here does not work on clang
305 : #if APM_BUILD_TYPE(APM_BUILD_UNKNOWN) || APM_BUILD_TYPE(APM_BUILD_AP_Periph)
306 : AP_Vehicle& vehicle = *AP_Vehicle::get_singleton();
307 : #else
308 : extern AP_Vehicle& vehicle;
309 : #endif
310 :
311 : /*
312 : setup is called when the sketch starts
313 : */
314 2094 : void AP_Vehicle::setup()
315 : {
316 : // load the default values of variables listed in var_info[]
317 2094 : AP_Param::setup_sketch_defaults();
318 :
319 : #if AP_SERIALMANAGER_ENABLED
320 : // initialise serial port
321 2094 : serial_manager.init_console();
322 : #endif
323 :
324 2094 : DEV_PRINTF("\n\nInit %s"
325 : "\n\nFree RAM: %u\n",
326 : AP::fwversion().fw_string,
327 : (unsigned)hal.util->available_memory());
328 :
329 : #if AP_CHECK_FIRMWARE_ENABLED
330 2094 : check_firmware_print();
331 : #endif
332 :
333 : // validate the static parameter table, then load persistent
334 : // values from storage:
335 2094 : AP_Param::check_var_info();
336 2094 : load_parameters();
337 :
338 : #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
339 : if (AP_BoardConfig::get_sdcard_slowdown() != 0) {
340 : // user wants the SDcard slower, we need to remount
341 : sdcard_stop();
342 : sdcard_retry();
343 : }
344 : #endif
345 :
346 : #if AP_SCHEDULER_ENABLED
347 : // initialise the main loop scheduler
348 : const AP_Scheduler::Task *tasks;
349 : uint8_t task_count;
350 : uint32_t log_bit;
351 2094 : get_scheduler_tasks(tasks, task_count, log_bit);
352 2094 : AP::scheduler().init(tasks, task_count, log_bit);
353 :
354 : // time per loop - this gets updated in the main loop() based on
355 : // actual loop rate
356 2094 : G_Dt = scheduler.get_loop_period_s();
357 : #endif
358 :
359 : // this is here for Plane; its failsafe_check method requires the
360 : // RC channels to be set as early as possible for maximum
361 : // survivability.
362 2094 : set_control_channels();
363 :
364 : #if HAL_GCS_ENABLED
365 : // initialise serial manager as early as sensible to get
366 : // diagnostic output during boot process. We have to initialise
367 : // the GCS singleton first as it sets the global mavlink system ID
368 : // which may get used very early on.
369 2094 : gcs().init();
370 : #endif
371 :
372 : #if AP_SERIALMANAGER_ENABLED
373 : #if HAL_WITH_IO_MCU
374 : if (BoardConfig.io_enabled()) {
375 : serial_manager.set_protocol_and_baud(HAL_UART_IOMCU_IDX, AP_SerialManager::SerialProtocol_IOMCU, 0);
376 : }
377 : #endif
378 : // initialise serial ports
379 2094 : serial_manager.init();
380 : #endif
381 : #if HAL_GCS_ENABLED
382 2094 : gcs().setup_console();
383 : #endif
384 :
385 : #if AP_SCRIPTING_ENABLED
386 : #if AP_SCRIPTING_SERIALDEVICE_ENABLED
387 : // must be done now so ports are registered and drivers get set up properly
388 : // (in particular mavlink which checks during init_ardupilot())
389 2094 : scripting.init_serialdevice_ports();
390 : #endif
391 : #endif
392 :
393 : #if AP_NETWORKING_ENABLED
394 2094 : networking.init();
395 : #endif
396 :
397 : #if AP_SCHEDULER_ENABLED
398 : // Register scheduler_delay_cb, which will run anytime you have
399 : // more than 5ms remaining in your call to hal.scheduler->delay
400 2094 : hal.scheduler->register_delay_callback(scheduler_delay_callback, 5);
401 : #endif
402 :
403 : #if AP_EXTERNAL_AHRS_ENABLED
404 : // call externalAHRS init before init_ardupilot to allow for external sensors
405 2094 : externalAHRS.init();
406 : #endif
407 :
408 : #if HAL_GENERATOR_ENABLED
409 2094 : generator.init();
410 : #endif
411 :
412 : #if AP_STATS_ENABLED
413 : // initialise stats module
414 2094 : stats.init();
415 : #endif
416 :
417 2094 : BoardConfig.init();
418 :
419 : #if HAL_CANMANAGER_ENABLED
420 2094 : can_mgr.init();
421 : #endif
422 :
423 : #if HAL_MSP_ENABLED
424 : // call MSP init before init_ardupilot to allow for MSP sensors
425 2094 : msp.init();
426 : #endif
427 :
428 : #if HAL_LOGGING_ENABLED
429 2094 : logger.init(get_log_bitmask(), get_log_structures(), get_num_log_structures());
430 : #endif
431 :
432 : // init cargo gripper
433 : #if AP_GRIPPER_ENABLED
434 2094 : AP::gripper().init();
435 : #endif
436 :
437 : // init beacons used for non-gps position estimation
438 : #if AP_BEACON_ENABLED
439 2094 : beacon.init();
440 : #endif // AP_BEACON_ENABLED
441 :
442 : // init_ardupilot is where the vehicle does most of its initialisation.
443 2094 : init_ardupilot();
444 :
445 : #if AP_SCRIPTING_ENABLED
446 2086 : scripting.init();
447 : #endif // AP_SCRIPTING_ENABLED
448 :
449 : #if AP_AIRSPEED_ENABLED
450 2086 : airspeed.init();
451 2086 : if (airspeed.enabled()) {
452 699 : airspeed.calibrate(true);
453 : }
454 : #if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
455 : else {
456 1 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "No airspeed sensor");
457 : }
458 : #endif
459 : #endif // AP_AIRSPEED_ENABLED
460 :
461 :
462 : #if AP_SRV_CHANNELS_ENABLED
463 2086 : AP::srv().init();
464 : #endif
465 :
466 : // gyro FFT needs to be initialized really late
467 : #if HAL_GYROFFT_ENABLED
468 : #if AP_SCHEDULER_ENABLED
469 2086 : gyro_fft.init(AP::scheduler().get_loop_rate_hz());
470 : #else
471 : gyro_fft.init(1000);
472 : #endif
473 : #endif
474 : #if HAL_HOTT_TELEM_ENABLED
475 2086 : hott_telem.init();
476 : #endif
477 : #if HAL_VISUALODOM_ENABLED
478 : // init library used for visual position estimation
479 2086 : visual_odom.init();
480 : #endif
481 :
482 : #if AP_VIDEOTX_ENABLED
483 2086 : vtx.init();
484 : #endif
485 :
486 : #if AP_SMARTAUDIO_ENABLED
487 2086 : smartaudio.init();
488 : #endif
489 :
490 : #if AP_TRAMP_ENABLED
491 2086 : tramp.init();
492 : #endif
493 :
494 : #if AP_PARAM_KEY_DUMP
495 : AP_Param::show_all(hal.console, true);
496 : #endif
497 :
498 2086 : send_watchdog_reset_statustext();
499 :
500 : #if AP_OPENDRONEID_ENABLED
501 2086 : opendroneid.init();
502 : #endif
503 :
504 : // init EFI monitoring
505 : #if HAL_EFI_ENABLED
506 2086 : efi.init();
507 : #endif
508 :
509 : #if AP_TEMPERATURE_SENSOR_ENABLED
510 2086 : temperature_sensor.init();
511 : #endif
512 :
513 : #if AP_KDECAN_ENABLED
514 2086 : kdecan.init();
515 : #endif
516 :
517 : #if AP_AIS_ENABLED
518 2086 : ais.init();
519 : #endif
520 :
521 : #if HAL_NMEA_OUTPUT_ENABLED
522 2086 : nmea.init();
523 : #endif
524 :
525 : #if AP_FENCE_ENABLED
526 2086 : fence.init();
527 2086 : fence_init();
528 : #endif
529 :
530 : #if AP_CUSTOMROTATIONS_ENABLED
531 2086 : custom_rotations.init();
532 : #endif
533 :
534 : #if AP_FILTER_ENABLED
535 2086 : filters.init();
536 : #endif
537 :
538 : #if HAL_WITH_ESC_TELEM && HAL_GYROFFT_ENABLED
539 68838 : for (uint8_t i = 0; i<ESC_TELEM_MAX_ESCS; i++) {
540 66752 : esc_noise[i].set_cutoff_frequency(2);
541 : }
542 : #endif
543 :
544 : #if AP_RPM_ENABLED
545 2086 : rpm_sensor.init();
546 : #endif
547 :
548 : #if AP_ARMING_ENABLED
549 2086 : AP::arming().init();
550 : #endif
551 :
552 : // invalidate count in case an enable parameter changed during
553 : // initialisation
554 2086 : AP_Param::invalidate_count();
555 :
556 2086 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ArduPilot Ready");
557 :
558 : #if AP_DDS_ENABLED
559 : if (!init_dds_client()) {
560 : GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%s Failed to Initialize", AP_DDS_Client::msg_prefix);
561 : }
562 : #endif
563 :
564 : #if AP_IBUS_TELEM_ENABLED
565 2086 : ibus_telem.init();
566 : #endif
567 2086 : }
568 :
569 41686254 : void AP_Vehicle::loop()
570 : {
571 : #if AP_SCHEDULER_ENABLED
572 41686254 : scheduler.loop();
573 41684622 : G_Dt = scheduler.get_loop_period_s();
574 : #else
575 : hal.scheduler->delay(1);
576 : G_Dt = 0.001;
577 : #endif
578 :
579 41684622 : if (!done_safety_init) {
580 : /*
581 : disable safety if requested. This is delayed till after the
582 : first loop has run to ensure that all servos have received
583 : an update for their initial values. Otherwise we may end up
584 : briefly driving a servo to a position out of the configured
585 : range which could damage hardware
586 : */
587 2086 : done_safety_init = true;
588 2086 : BoardConfig.init_safety();
589 :
590 : // send RC output mode info if available
591 : char banner_msg[50];
592 2086 : if (hal.rcout->get_output_mode_banner(banner_msg, sizeof(banner_msg))) {
593 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s", banner_msg);
594 : }
595 : }
596 41684622 : const uint32_t new_internal_errors = AP::internalerror().errors();
597 41684622 : if(_last_internal_errors != new_internal_errors) {
598 1 : LOGGER_WRITE_ERROR(LogErrorSubsystem::INTERNAL_ERROR, LogErrorCode::INTERNAL_ERRORS_DETECTED);
599 1 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Internal Errors 0x%x", (unsigned)new_internal_errors);
600 1 : _last_internal_errors = new_internal_errors;
601 : }
602 41684622 : }
603 :
604 : #if AP_SCHEDULER_ENABLED
605 : /*
606 : scheduler table - all regular tasks apart from the fast_loop()
607 : should be listed here.
608 :
609 : All entries in this table must be ordered by priority.
610 :
611 : This table is interleaved with the table presnet in each of the
612 : vehicles to determine the order in which tasks are run. Convenience
613 : methods SCHED_TASK and SCHED_TASK_CLASS are provided to build
614 : entries in this structure:
615 :
616 : SCHED_TASK arguments:
617 : - name of static function to call
618 : - rate (in Hertz) at which the function should be called
619 : - expected time (in MicroSeconds) that the function should take to run
620 : - priority (0 through 255, lower number meaning higher priority)
621 :
622 : SCHED_TASK_CLASS arguments:
623 : - class name of method to be called
624 : - instance on which to call the method
625 : - method to call on that instance
626 : - rate (in Hertz) at which the method should be called
627 : - expected time (in MicroSeconds) that the method should take to run
628 : - priority (0 through 255, lower number meaning higher priority)
629 :
630 : */
631 : const AP_Scheduler::Task AP_Vehicle::scheduler_tasks[] = {
632 : #if HAL_GYROFFT_ENABLED
633 : FAST_TASK_CLASS(AP_GyroFFT, &vehicle.gyro_fft, sample_gyros),
634 : #endif
635 : #if AP_BEACON_ENABLED
636 : SCHED_TASK_CLASS(AP_Beacon, &vehicle.beacon, update, 400, 200, 24),
637 : #endif // AP_BEACON_ENABLED
638 : #if AP_AIRSPEED_ENABLED
639 : SCHED_TASK_CLASS(AP_Airspeed, &vehicle.airspeed, update, 10, 100, 41), // NOTE: the priority number here should be right before Plane's calc_airspeed_errors
640 : #endif
641 : #if COMPASS_CAL_ENABLED
642 : SCHED_TASK_CLASS(Compass, &vehicle.compass, cal_update, 100, 200, 75),
643 : #endif
644 : SCHED_TASK_CLASS(AP_Notify, &vehicle.notify, update, 50, 300, 78),
645 : #if HAL_NMEA_OUTPUT_ENABLED
646 : SCHED_TASK_CLASS(AP_NMEA_Output, &vehicle.nmea, update, 50, 50, 180),
647 : #endif
648 : #if HAL_GYROFFT_ENABLED
649 : SCHED_TASK_CLASS(AP_GyroFFT, &vehicle.gyro_fft, update, 400, 50, 205),
650 : SCHED_TASK_CLASS(AP_GyroFFT, &vehicle.gyro_fft, update_parameters, 1, 50, 210),
651 : #endif
652 : #if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED && !AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED
653 : SCHED_TASK(update_dynamic_notch_at_specified_rate, LOOP_RATE, 200, 215),
654 : #endif
655 : #if AP_VIDEOTX_ENABLED
656 : SCHED_TASK_CLASS(AP_VideoTX, &vehicle.vtx, update, 2, 100, 220),
657 : #endif
658 : #if AP_TRAMP_ENABLED
659 : SCHED_TASK_CLASS(AP_Tramp, &vehicle.tramp, update, 50, 50, 225),
660 : #endif
661 : SCHED_TASK(send_watchdog_reset_statustext, 0.1, 20, 225),
662 : #if HAL_WITH_ESC_TELEM
663 : // This update function is responsible for checking timeouts and invalidating the ESC telemetry data.
664 : // Be mindful of this if you are planning to reduce the frequency from 100Hz.
665 : SCHED_TASK_CLASS(AP_ESC_Telem, &vehicle.esc_telem, update, 100, 50, 230),
666 : #endif
667 : #if AP_SERVO_TELEM_ENABLED
668 : SCHED_TASK_CLASS(AP_Servo_Telem, &vehicle.servo_telem, update, 50, 50, 231),
669 : #endif
670 : #if HAL_GENERATOR_ENABLED
671 : SCHED_TASK_CLASS(AP_Generator, &vehicle.generator, update, 10, 50, 235),
672 : #endif
673 : #if AP_OPENDRONEID_ENABLED
674 : SCHED_TASK_CLASS(AP_OpenDroneID, &vehicle.opendroneid, update, 10, 50, 236),
675 : #endif
676 : #if AP_NETWORKING_ENABLED
677 : SCHED_TASK_CLASS(AP_Networking, &vehicle.networking, update, 10, 50, 238),
678 : #endif
679 : #if AP_RPM_ENABLED
680 : SCHED_TASK_CLASS(AP_RPM, &vehicle.rpm_sensor, update, 50, 100, 239),
681 : #endif
682 : #if OSD_ENABLED
683 : SCHED_TASK(publish_osd_info, 1, 10, 240),
684 : #endif
685 : #if AP_TEMPERATURE_SENSOR_ENABLED
686 : SCHED_TASK_CLASS(AP_TemperatureSensor, &vehicle.temperature_sensor, update, 5, 50, 242),
687 : #endif
688 : #if HAL_INS_ACCELCAL_ENABLED
689 : SCHED_TASK(accel_cal_update, 10, 100, 245),
690 : #endif
691 : #if AP_FENCE_ENABLED
692 : SCHED_TASK_CLASS(AC_Fence, &vehicle.fence, update, 10, 100, 248),
693 : #endif
694 : #if AP_AIS_ENABLED
695 : SCHED_TASK_CLASS(AP_AIS, &vehicle.ais, update, 5, 100, 249),
696 : #endif
697 : #if HAL_EFI_ENABLED
698 : SCHED_TASK_CLASS(AP_EFI, &vehicle.efi, update, 50, 200, 250),
699 : #endif
700 : #if AP_GRIPPER_ENABLED
701 : SCHED_TASK_CLASS(AP_Gripper, &vehicle.gripper, update, 10, 75, 251),
702 : #endif
703 : SCHED_TASK(one_Hz_update, 1, 100, 252),
704 : #if HAL_WITH_ESC_TELEM && HAL_GYROFFT_ENABLED
705 : SCHED_TASK(check_motor_noise, 5, 50, 252),
706 : #endif
707 : #if AP_FILTER_ENABLED
708 : SCHED_TASK_CLASS(AP_Filters, &vehicle.filters, update, 1, 100, 252),
709 : #endif
710 : #if AP_STATS_ENABLED
711 : SCHED_TASK_CLASS(AP_Stats, &vehicle.stats, update, 1, 100, 252),
712 : #endif
713 : #if AP_ARMING_ENABLED
714 : SCHED_TASK(update_arming, 1, 50, 253),
715 : #endif
716 : };
717 :
718 2094 : void AP_Vehicle::get_common_scheduler_tasks(const AP_Scheduler::Task*& tasks, uint8_t& num_tasks)
719 : {
720 2094 : tasks = scheduler_tasks;
721 2094 : num_tasks = ARRAY_SIZE(scheduler_tasks);
722 2094 : }
723 :
724 : /*
725 : * a delay() callback that processes MAVLink packets. We set this as the
726 : * callback in long running library initialisation routines to allow
727 : * MAVLink to process packets while waiting for the initialisation to
728 : * complete
729 : */
730 2415404 : void AP_Vehicle::scheduler_delay_callback()
731 : {
732 : #if APM_BUILD_TYPE(APM_BUILD_Replay)
733 : // compass.init() delays, so we end up here.
734 : return;
735 : #endif
736 :
737 : static uint32_t last_1hz, last_50hz, last_5s;
738 :
739 : #if HAL_LOGGING_ENABLED
740 2415404 : AP_Logger &logger = AP::logger();
741 :
742 : // don't allow potentially expensive logging calls:
743 2415404 : logger.EnableWrites(false);
744 : #endif
745 :
746 2415404 : const uint32_t tnow = AP_HAL::millis();
747 2415404 : if (tnow - last_1hz > 1000) {
748 3963 : last_1hz = tnow;
749 3963 : GCS_SEND_MESSAGE(MSG_HEARTBEAT);
750 3963 : GCS_SEND_MESSAGE(MSG_SYS_STATUS);
751 : }
752 2415404 : if (tnow - last_50hz > 20) {
753 220858 : last_50hz = tnow;
754 : #if HAL_GCS_ENABLED
755 220858 : gcs().update_receive();
756 220858 : gcs().update_send();
757 : #endif
758 220858 : _singleton->notify.update();
759 : }
760 2415404 : if (tnow - last_5s > 5000) {
761 108 : last_5s = tnow;
762 108 : if (AP_BoardConfig::in_config_error()) {
763 24 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Config Error: fix problem then reboot");
764 : } else {
765 84 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Initialising ArduPilot");
766 : }
767 : }
768 :
769 : #if HAL_LOGGING_ENABLED
770 2415404 : logger.EnableWrites(true);
771 : #endif
772 2415404 : }
773 : #endif // AP_SCHEDULER_ENABLED
774 :
775 : // if there's been a watchdog reset, notify the world via a statustext:
776 16242 : void AP_Vehicle::send_watchdog_reset_statustext()
777 : {
778 16242 : if (!hal.util->was_watchdog_reset()) {
779 16239 : return;
780 : }
781 3 : const AP_HAL::Util::PersistentData &pd = hal.util->last_persistent_data;
782 : (void)pd; // in case !HAL_GCS_ENABLED
783 3 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL,
784 : "WDG: T%d SL%u FL%u FT%u FA%x FTP%u FLR%x FICSR%u MM%u MC%u IE%u IEC%u TN:%.4s",
785 : pd.scheduler_task,
786 : pd.semaphore_line,
787 : pd.fault_line,
788 : pd.fault_type,
789 : (unsigned)pd.fault_addr,
790 : pd.fault_thd_prio,
791 : (unsigned)pd.fault_lr,
792 : (unsigned)pd.fault_icsr,
793 : pd.last_mavlink_msgid,
794 : pd.last_mavlink_cmd,
795 : (unsigned)pd.internal_errors,
796 : (unsigned)pd.internal_error_count,
797 : pd.thread_name4
798 : );
799 : }
800 :
801 8327 : bool AP_Vehicle::is_crashed() const
802 : {
803 : #if AP_ARMING_ENABLED
804 8327 : if (AP::arming().is_armed()) {
805 3620 : return false;
806 : }
807 4707 : return AP::arming().last_disarm_method() == AP_Arming::Method::CRASH;
808 : #else
809 : return false;
810 : #endif
811 : }
812 :
813 : #if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
814 : // update the harmonic notch filter for throttle based notch
815 67072 : void AP_Vehicle::update_throttle_notch(AP_InertialSensor::HarmonicNotch ¬ch)
816 : {
817 : #if APM_BUILD_TYPE(APM_BUILD_ArduPlane) || APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_Rover)
818 67072 : const float ref_freq = notch.params.center_freq_hz();
819 67072 : const float ref = notch.params.reference();
820 :
821 : #if APM_BUILD_TYPE(APM_BUILD_ArduPlane) || APM_BUILD_COPTER_OR_HELI
822 67072 : const AP_Motors* motors = AP::motors();
823 67072 : if (motors == nullptr) {
824 0 : notch.update_freq_hz(0);
825 0 : return;
826 : }
827 67072 : const float motors_throttle = MAX(0,motors->get_throttle_out());
828 : // set the harmonic notch filter frequency scaled on measured frequency
829 67072 : if (notch.params.hasOption(HarmonicNotchFilterParams::Options::DynamicHarmonic)) {
830 : float notches[INS_MAX_NOTCHES];
831 43293 : uint8_t motor_num = 0;
832 1428669 : for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
833 1385376 : float motor_throttle = 0;
834 1385376 : if (motors->get_thrust(i, motor_throttle)) {
835 175072 : notches[motor_num] = ref_freq * sqrtf(MAX(0, motor_throttle) / ref);
836 175072 : motor_num++;
837 : }
838 1385376 : if (motor_num >= INS_MAX_NOTCHES) {
839 0 : break;
840 : }
841 : }
842 43293 : notch.update_frequencies_hz(motor_num, notches);
843 : } else
844 : #else // APM_BUILD_Rover
845 0 : const AP_MotorsUGV *motors = AP::motors_ugv();
846 0 : const float motors_throttle = motors != nullptr ? abs(motors->get_throttle() * 0.01f) : 0;
847 : #endif
848 : {
849 23779 : float throttle_freq = ref_freq * sqrtf(MAX(0,motors_throttle) / ref);
850 :
851 23779 : notch.update_freq_hz(throttle_freq);
852 : }
853 :
854 : #endif
855 0 : }
856 :
857 : // update the harmonic notch filter center frequency dynamically
858 50037877 : void AP_Vehicle::update_dynamic_notch(AP_InertialSensor::HarmonicNotch ¬ch)
859 : {
860 : #if APM_BUILD_TYPE(APM_BUILD_ArduPlane)||APM_BUILD_COPTER_OR_HELI||APM_BUILD_TYPE(APM_BUILD_Rover)
861 47595007 : if (!notch.params.enabled()) {
862 47168376 : return;
863 : }
864 426631 : const float ref_freq = notch.params.center_freq_hz();
865 426631 : const float ref = notch.params.reference();
866 426631 : if (is_zero(ref)) {
867 14717 : notch.update_freq_hz(ref_freq);
868 14717 : return;
869 : }
870 :
871 411914 : switch (notch.params.tracking_mode()) {
872 62338 : case HarmonicNotchDynamicMode::UpdateThrottle: // throttle based tracking
873 : // set the harmonic notch filter frequency approximately scaled on motor rpm implied by throttle
874 62338 : update_throttle_notch(notch);
875 62338 : break;
876 :
877 : #if AP_RPM_ENABLED
878 0 : case HarmonicNotchDynamicMode::UpdateRPM: // rpm sensor based tracking
879 : case HarmonicNotchDynamicMode::UpdateRPM2: {
880 0 : uint8_t sensor = (notch.params.tracking_mode()==HarmonicNotchDynamicMode::UpdateRPM?0:1);
881 : float rpm;
882 0 : if (rpm_sensor.get_rpm(sensor, rpm)) {
883 : // set the harmonic notch filter frequency from the main rotor rpm
884 0 : notch.update_freq_hz(rpm * ref * (1.0/60));
885 : } else {
886 0 : notch.update_freq_hz(0);
887 : }
888 0 : break;
889 : }
890 : #endif // AP_RPM_ENABLED
891 : #if HAL_WITH_ESC_TELEM
892 228532 : case HarmonicNotchDynamicMode::UpdateBLHeli: // BLHeli based tracking
893 : // set the harmonic notch filter frequency scaled on measured frequency
894 228532 : if (notch.params.hasOption(HarmonicNotchFilterParams::Options::DynamicHarmonic)) {
895 : float notches[INS_MAX_NOTCHES];
896 : // ESC telemetry will return 0 for missing data, but only after 1s
897 151157 : const uint8_t num_notches = AP::esc_telem().get_motor_frequencies_hz(INS_MAX_NOTCHES, notches);
898 151157 : if (num_notches > 0) {
899 146423 : notch.update_frequencies_hz(num_notches, notches);
900 : } else { // throttle fallback
901 4734 : update_throttle_notch(notch);
902 : }
903 : } else {
904 77375 : notch.update_freq_hz(AP::esc_telem().get_average_motor_frequency_hz() * ref);
905 : }
906 228532 : break;
907 : #endif
908 : #if HAL_GYROFFT_ENABLED
909 97351 : case HarmonicNotchDynamicMode::UpdateGyroFFT: // FFT based tracking
910 : // set the harmonic notch filter frequency scaled on measured frequency
911 97351 : if (notch.params.hasOption(HarmonicNotchFilterParams::Options::DynamicHarmonic)) {
912 : float notches[INS_MAX_NOTCHES];
913 44424 : const uint8_t peaks = gyro_fft.get_weighted_noise_center_frequencies_hz(notch.num_dynamic_notches, notches);
914 :
915 44424 : if (peaks > 0) {
916 26255 : notch.set_inactive(false);
917 26255 : notch.update_frequencies_hz(peaks, notches);
918 : } else { // since FFT can be used post-filter it is better to disable the notch when there is no data
919 18169 : notch.set_inactive(true);
920 : }
921 : } else {
922 52927 : float center_freq = gyro_fft.get_weighted_noise_center_freq_hz();
923 52927 : notch.update_freq_hz(center_freq);
924 : }
925 97351 : break;
926 : #endif
927 23693 : case HarmonicNotchDynamicMode::Fixed: // static
928 : default:
929 23693 : notch.update_freq_hz(ref_freq);
930 23693 : break;
931 : }
932 : #endif // APM_BUILD_TYPE(APM_BUILD_ArduPlane)||APM_BUILD_COPTER_OR_HELI||APM_BUILD_TYPE(APM_BUILD_Rover)
933 2442870 : }
934 :
935 : // run notch update at either loop rate or 200Hz
936 41569286 : void AP_Vehicle::update_dynamic_notch_at_specified_rate()
937 : {
938 166277144 : for (auto ¬ch : ins.harmonic_notches) {
939 124707858 : if (notch.params.hasOption(HarmonicNotchFilterParams::Options::LoopRateUpdate)) {
940 43020 : update_dynamic_notch(notch);
941 : } else {
942 : // decimated update at 200Hz
943 124664838 : const uint32_t now = AP_HAL::millis();
944 124664838 : const uint8_t i = ¬ch - &ins.harmonic_notches[0];
945 124664838 : if (now - _last_notch_update_ms[i] > 5) {
946 49562530 : _last_notch_update_ms[i] = now;
947 49562530 : update_dynamic_notch(notch);
948 : }
949 : }
950 : }
951 41569286 : }
952 : #endif // AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
953 :
954 3 : void AP_Vehicle::notify_no_such_mode(uint8_t mode_number)
955 : {
956 3 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING,"No such mode %u", mode_number);
957 3 : LOGGER_WRITE_ERROR(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode_number));
958 3 : }
959 :
960 : // reboot the vehicle in an orderly manner, doing various cleanups and
961 : // flashing LEDs as appropriate
962 1639 : void AP_Vehicle::reboot(bool hold_in_bootloader)
963 : {
964 1639 : if (should_zero_rc_outputs_on_reboot()) {
965 69 : SRV_Channels::zero_rc_outputs();
966 : }
967 :
968 : // Notify might want to blink some LEDs:
969 1639 : AP_Notify::flags.firmware_update = 1;
970 1639 : notify.update();
971 :
972 : // force safety on
973 1639 : hal.rcout->force_safety_on();
974 :
975 : // flush pending parameter writes
976 1639 : AP_Param::flush();
977 :
978 : // do not process incoming mavlink messages while we delay:
979 1639 : hal.scheduler->register_delay_callback(nullptr, 5);
980 :
981 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
982 : // need to ensure the ack goes out:
983 1639 : hal.serial(0)->flush();
984 : #endif
985 :
986 : // delay to give the ACK a chance to get out, the LEDs to flash,
987 : // the IO board safety to be forced on, the parameters to flush, ...
988 1639 : hal.scheduler->delay(200);
989 :
990 : #if HAL_WITH_IO_MCU
991 : iomcu.soft_reboot();
992 : #endif
993 :
994 1639 : hal.scheduler->reboot(hold_in_bootloader);
995 0 : }
996 :
997 : #if OSD_ENABLED
998 149217 : void AP_Vehicle::publish_osd_info()
999 : {
1000 : #if AP_MISSION_ENABLED
1001 149217 : AP_OSD *osd = AP::osd();
1002 149217 : if (osd == nullptr) {
1003 16660 : return;
1004 : }
1005 : AP_OSD::NavInfo nav_info;
1006 147515 : if(!get_wp_distance_m(nav_info.wp_distance)) {
1007 14958 : return;
1008 : }
1009 : float wp_bearing_deg;
1010 132557 : if (!get_wp_bearing_deg(wp_bearing_deg)) {
1011 0 : return;
1012 : }
1013 132557 : nav_info.wp_bearing = (int32_t)wp_bearing_deg * 100; // OSD expects cd
1014 132557 : if (!get_wp_crosstrack_error_m(nav_info.wp_xtrack_error)) {
1015 0 : return;
1016 : }
1017 132557 : nav_info.wp_number = AP::mission().get_current_nav_index();
1018 132557 : osd->set_nav_info(nav_info);
1019 : #endif
1020 : }
1021 : #endif
1022 :
1023 1934 : void AP_Vehicle::get_osd_attitude_rad(float &roll, float &pitch, float &yaw)
1024 : {
1025 : #if AP_AHRS_ENABLED
1026 : // Take semaphore as this can be called from a thread
1027 1934 : WITH_SEMAPHORE(ahrs.get_semaphore());
1028 :
1029 1934 : roll = ahrs.get_roll_rad();
1030 1934 : pitch = ahrs.get_pitch_rad();
1031 1934 : yaw = ahrs.get_yaw_rad();
1032 : #else
1033 : roll = 0.0;
1034 : pitch = 0.0;
1035 : yaw = 0.0;
1036 : #endif
1037 1934 : }
1038 :
1039 : #if HAL_INS_ACCELCAL_ENABLED
1040 :
1041 : #ifndef HAL_CAL_ALWAYS_REBOOT
1042 : // allow for forced reboot after accelcal
1043 : #define HAL_CAL_ALWAYS_REBOOT 0
1044 : #endif
1045 :
1046 : /*
1047 : update accel cal
1048 : */
1049 1504089 : void AP_Vehicle::accel_cal_update()
1050 : {
1051 1504089 : if (hal.util->get_soft_armed()) {
1052 1004810 : return;
1053 : }
1054 499279 : ins.acal_update();
1055 :
1056 : #if AP_AHRS_ENABLED
1057 : // check if new trim values, and set them
1058 499279 : Vector3f trim_rad;
1059 499279 : if (ins.get_new_trim(trim_rad)) {
1060 1 : ahrs.set_trim(trim_rad);
1061 : }
1062 : #endif
1063 :
1064 : #if HAL_CAL_ALWAYS_REBOOT
1065 : if (ins.accel_cal_requires_reboot() &&
1066 : !hal.util->get_soft_armed()) {
1067 : hal.scheduler->delay(1000);
1068 : hal.scheduler->reboot();
1069 : }
1070 : #endif
1071 : }
1072 : #endif // HAL_INS_ACCELCAL_ENABLED
1073 :
1074 : #if AP_ARMING_ENABLED
1075 : // call the arming library's update function
1076 149217 : void AP_Vehicle::update_arming()
1077 : {
1078 149217 : AP::arming().update();
1079 149217 : }
1080 : #endif
1081 :
1082 : /*
1083 : one Hz checks common to all vehicles
1084 : */
1085 149217 : void AP_Vehicle::one_Hz_update(void)
1086 : {
1087 149217 : one_Hz_counter++;
1088 :
1089 : /*
1090 : every 10s check if using a 2M firmware on a 1M board
1091 : */
1092 149217 : if (one_Hz_counter % 10U == 0) {
1093 : #if defined(BOARD_CHECK_F427_USE_1M) && (HAL_PROGRAM_SIZE_LIMIT_KB>1024)
1094 : if (!hal.util->get_soft_armed() && check_limit_flash_1M()) {
1095 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, BOARD_CHECK_F427_USE_1M);
1096 : }
1097 : #endif
1098 : }
1099 :
1100 : /*
1101 : every 30s check if using a 1M firmware on a 2M board
1102 : */
1103 149217 : if (one_Hz_counter % 30U == 0) {
1104 : #if defined(BOARD_CHECK_F427_USE_1M) && (HAL_PROGRAM_SIZE_LIMIT_KB<=1024)
1105 : if (!hal.util->get_soft_armed() && !check_limit_flash_1M()) {
1106 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, BOARD_CHECK_F427_USE_2M);
1107 : }
1108 : #endif
1109 : }
1110 :
1111 : #if AP_SCRIPTING_ENABLED
1112 149217 : scripting.update();
1113 : #endif
1114 :
1115 : #if HAL_LOGGING_ENABLED && HAL_UART_STATS_ENABLED
1116 : // Log data rates of physical and virtual serial ports
1117 149217 : hal.util->uart_log();
1118 : #if AP_SERIALMANAGER_REGISTER_ENABLED
1119 149217 : serial_manager.registered_ports_log();
1120 : #endif
1121 : #endif
1122 :
1123 : #if HAL_GCS_ENABLED
1124 : // Check if available modes have changed
1125 149217 : const uint32_t available_mode_enabled_mask = get_available_mode_enabled_mask();
1126 149217 : if (available_mode_enabled_mask != last_available_mode_enabled_mask) {
1127 1930 : if (last_available_mode_enabled_mask != 0) {
1128 : // Last value is only zero at init, track changes after that
1129 26 : gcs().available_modes_changed();
1130 : }
1131 1930 : last_available_mode_enabled_mask = available_mode_enabled_mask;
1132 : }
1133 : #endif
1134 :
1135 149217 : }
1136 :
1137 751641 : void AP_Vehicle::check_motor_noise()
1138 : {
1139 : #if HAL_GYROFFT_ENABLED && HAL_WITH_ESC_TELEM
1140 751641 : if (!hal.util->get_soft_armed() || !gyro_fft.check_esc_noise() || !gyro_fft.using_post_filter_samples()) {
1141 751317 : return;
1142 : }
1143 : #if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
1144 324 : if (ins.has_fft_notch()) {
1145 0 : return;
1146 : }
1147 : #endif
1148 :
1149 : float esc_data[ESC_TELEM_MAX_ESCS];
1150 324 : const uint8_t numf = AP::esc_telem().get_motor_frequencies_hz(ESC_TELEM_MAX_ESCS, esc_data);
1151 324 : bool output_error = false;
1152 :
1153 1620 : for (uint8_t i = 0; i<numf; i++) {
1154 1296 : if (is_zero(esc_data[i])) {
1155 94 : continue;
1156 : }
1157 1202 : float energy = gyro_fft.has_noise_at_frequency_hz(esc_data[i]);
1158 1202 : energy = esc_noise[i].apply(energy, 0.2f);
1159 1202 : if (energy > 40.0f && AP_HAL::millis() - last_motor_noise_ms > 5000) {
1160 8 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Noise %.fdB on motor %u at %.fHz", energy, i+1, esc_data[i]);
1161 8 : output_error = true;
1162 : }
1163 : }
1164 :
1165 324 : if (output_error) {
1166 6 : last_motor_noise_ms = AP_HAL::millis();
1167 : }
1168 : #endif
1169 0 : }
1170 :
1171 : #if HAL_WITH_ESC_TELEM && (APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_ArduPlane))
1172 353204 : bool AP_Vehicle::motors_takeoff_check(float rpm_min, float rpm_max)
1173 : {
1174 353204 : auto motors = AP::motors();
1175 :
1176 : // Allow takeoff if check is disabled or if no motor class is present
1177 353204 : if (rpm_min <= 0 || motors == nullptr) {
1178 343513 : return true;
1179 : }
1180 :
1181 : // clear warning timer when disarmed
1182 9691 : uint32_t now_ms = AP_HAL::millis();
1183 9691 : if (!motors->armed()) {
1184 6158 : takeoff_check_state.warning_ms = now_ms;
1185 6158 : return false;
1186 : }
1187 :
1188 : // check ESCs are sending RPM at expected level
1189 3533 : uint32_t motor_mask = motors->get_motor_mask();
1190 3533 : const bool telem_active = AP::esc_telem().is_telemetry_active(motor_mask);
1191 3533 : const bool rpm_adequate = AP::esc_telem().are_motors_running(motor_mask, rpm_min, rpm_max);
1192 :
1193 : // if RPM is at the expected level clear block
1194 3533 : if (telem_active && rpm_adequate) {
1195 164 : return true;
1196 : }
1197 :
1198 : // warn the user every 2 seconds that telemetry is inactive or rpm is inadequate
1199 3369 : if (now_ms - takeoff_check_state.warning_ms > 2000) {
1200 14 : takeoff_check_state.warning_ms = now_ms;
1201 14 : const char* prefix_str = "Takeoff blocked:";
1202 14 : if (!telem_active) {
1203 0 : gcs().send_text(MAV_SEVERITY_CRITICAL, "%s waiting for ESC RPM", prefix_str);
1204 14 : } else if (!rpm_adequate) {
1205 14 : gcs().send_text(MAV_SEVERITY_CRITICAL, "%s ESC RPM out of range", prefix_str);
1206 : }
1207 : }
1208 3369 : return false;
1209 : }
1210 : #endif // HAL_WITH_ESC_TELEM && (APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_ArduPlane))
1211 :
1212 : #if AP_DDS_ENABLED
1213 : bool AP_Vehicle::init_dds_client()
1214 : {
1215 : dds_client = NEW_NOTHROW AP_DDS_Client();
1216 : if (dds_client == nullptr) {
1217 : return false;
1218 : }
1219 : return dds_client->start();
1220 : }
1221 : #endif // AP_DDS_ENABLED
1222 :
1223 : // Check if this mode can be entered from the GCS
1224 : #if APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_ArduPlane) || APM_BUILD_TYPE(APM_BUILD_Rover)
1225 1654613 : bool AP_Vehicle::block_GCS_mode_change(uint8_t mode_num, const uint8_t *mode_list, uint8_t mode_list_length) const
1226 : {
1227 1654613 : if (mode_list == nullptr) {
1228 0 : return false;
1229 : }
1230 :
1231 17935605 : for (uint8_t i = 0; i < mode_list_length; i++) {
1232 : // Find index of mode
1233 17772265 : if (mode_list[i] == mode_num) {
1234 1491273 : const uint32_t mask = 1U << i;
1235 1491273 : return (uint32_t(flight_mode_GCS_block) & mask) != 0;
1236 : }
1237 : }
1238 :
1239 163340 : return false;
1240 : }
1241 : #endif
1242 :
1243 : #if AP_FENCE_ENABLED
1244 2086 : void AP_Vehicle::fence_init()
1245 : {
1246 2086 : hal.scheduler->register_io_process(FUNCTOR_BIND_MEMBER(&AP_Vehicle::fence_checks_async, void));
1247 2086 : }
1248 : #endif // AP_FENCE_ENABLED
1249 :
1250 : AP_Vehicle *AP_Vehicle::_singleton = nullptr;
1251 :
1252 42587813 : AP_Vehicle *AP_Vehicle::get_singleton()
1253 : {
1254 42587813 : return _singleton;
1255 : }
1256 :
1257 : namespace AP {
1258 :
1259 42465416 : AP_Vehicle *vehicle()
1260 : {
1261 42465416 : return AP_Vehicle::get_singleton();
1262 : }
1263 :
1264 : };
1265 :
1266 : #endif // AP_VEHICLE_ENABLED
|
