Line data Source code
1 : #include "Plane.h"
2 :
3 : #include "qautotune.h"
4 :
5 67467802 : static void failsafe_check_static()
6 : {
7 67467802 : plane.failsafe_check();
8 67467802 : }
9 :
10 692 : void Plane::init_ardupilot()
11 : {
12 :
13 692 : ins.set_log_raw_bit(MASK_LOG_IMU_RAW);
14 :
15 : // initialise rc channels including setting mode
16 692 : rc().init();
17 :
18 : #if AP_RELAY_ENABLED
19 692 : relay.init();
20 : #endif
21 :
22 : // initialise notify system
23 692 : notify.init();
24 692 : notify_mode(*control_mode);
25 :
26 692 : init_rc_out_main();
27 :
28 : // init baro
29 692 : barometer.init();
30 :
31 : #if AP_RANGEFINDER_ENABLED
32 : // initialise rangefinder
33 692 : rangefinder.set_log_rfnd_bit(MASK_LOG_SONAR);
34 692 : rangefinder.init(ROTATION_PITCH_270);
35 : #endif
36 :
37 : // initialise battery monitoring
38 692 : battery.init();
39 :
40 : #if AP_RSSI_ENABLED
41 692 : rssi.init();
42 : #endif
43 :
44 : // setup telem slots with serial ports
45 692 : gcs().setup_uarts();
46 :
47 :
48 : #if OSD_ENABLED
49 692 : osd.init();
50 : #endif
51 :
52 692 : AP::compass().set_log_bit(MASK_LOG_COMPASS);
53 692 : AP::compass().init();
54 :
55 : #if AP_AIRSPEED_ENABLED
56 692 : airspeed.set_fixedwing_parameters(&aparm);
57 692 : airspeed.set_log_bit(MASK_LOG_IMU);
58 : #endif
59 :
60 : // GPS Initialization
61 692 : gps.set_log_gps_bit(MASK_LOG_GPS);
62 692 : gps.init();
63 :
64 692 : init_rc_in(); // sets up rc channels from radio
65 :
66 : #if HAL_MOUNT_ENABLED
67 : // initialise camera mount
68 692 : camera_mount.init();
69 : #endif
70 :
71 : #if AP_CAMERA_ENABLED
72 : // initialise camera
73 692 : camera.init();
74 : #endif
75 :
76 : #if AP_LANDINGGEAR_ENABLED
77 : // initialise landing gear position
78 692 : g2.landing_gear.init();
79 : #endif
80 :
81 : #if FENCE_TRIGGERED_PIN > 0
82 : hal.gpio->pinMode(FENCE_TRIGGERED_PIN, HAL_GPIO_OUTPUT);
83 : hal.gpio->write(FENCE_TRIGGERED_PIN, 0);
84 : #endif
85 :
86 : /*
87 : * setup the 'main loop is dead' check. Note that this relies on
88 : * the RC library being initialised.
89 : */
90 692 : hal.scheduler->register_timer_failsafe(failsafe_check_static, 1000);
91 :
92 : #if HAL_QUADPLANE_ENABLED
93 692 : quadplane.setup();
94 : #endif
95 :
96 692 : AP_Param::reload_defaults_file(true);
97 :
98 : // ALT_OFFSET always starts at zero, independently of FLIGHT_OPTIONS.
99 692 : reset_alt_offset(true);
100 :
101 692 : set_mode(mode_initializing, ModeReason::INITIALISED);
102 :
103 : #if (GROUND_START_DELAY > 0)
104 : gcs().send_text(MAV_SEVERITY_NOTICE,"Ground start with delay");
105 : delay(GROUND_START_DELAY * 1000);
106 : #else
107 692 : gcs().send_text(MAV_SEVERITY_INFO,"Ground start");
108 : #endif
109 :
110 : //INS ground start
111 : //------------------------
112 : //
113 692 : startup_INS();
114 :
115 : // Save the settings for in-air restart
116 : // ------------------------------------
117 : //save_EEPROM_groundstart();
118 :
119 : // initialise mission library
120 691 : mission.init();
121 : #if HAL_LOGGING_ENABLED
122 691 : mission.set_log_start_mission_item_bit(MASK_LOG_CMD);
123 : #endif
124 :
125 : // initialise AP_Logger library
126 : #if HAL_LOGGING_ENABLED
127 691 : logger.setVehicle_Startup_Writer(
128 : FUNCTOR_BIND(&plane, &Plane::Log_Write_Vehicle_Startup_Messages, void)
129 : );
130 : #endif
131 :
132 : // reset last heartbeat time, so we don't trigger failsafe on slow
133 : // startup
134 691 : gcs().sysid_mygcs_seen(AP_HAL::millis());
135 :
136 : // don't initialise aux rc output until after quadplane is setup as
137 : // that can change initial values of channels
138 691 : init_rc_out_aux();
139 :
140 691 : if (g2.oneshot_mask != 0) {
141 0 : hal.rcout->set_output_mode(g2.oneshot_mask, AP_HAL::RCOutput::MODE_PWM_ONESHOT);
142 : }
143 691 : hal.rcout->set_dshot_esc_type(SRV_Channels::get_dshot_esc_type());
144 :
145 691 : set_mode_by_number((enum Mode::Number)g.initial_mode.get(), ModeReason::INITIALISED);
146 :
147 : // set the correct flight mode
148 : // ---------------------------
149 691 : rc().reset_mode_switch();
150 :
151 : // initialise sensor
152 : #if AP_OPTICALFLOW_ENABLED
153 691 : if (optflow.enabled()) {
154 0 : optflow.init(-1);
155 : }
156 : #endif
157 :
158 : #if AC_PRECLAND_ENABLED
159 : // scheduler table specifies 400Hz, but we can call it no faster
160 : // than the scheduler loop rate:
161 691 : g2.precland.init(MIN(400, scheduler.get_loop_rate_hz()));
162 : #endif
163 :
164 : #if AP_ICENGINE_ENABLED
165 691 : g2.ice_control.init();
166 : #endif
167 :
168 691 : }
169 :
170 : #if AP_FENCE_ENABLED
171 : /*
172 : return true if a mode reason is an automatic mode change due to
173 : landing sequencing.
174 : */
175 27 : static bool mode_reason_is_landing_sequence(const ModeReason reason)
176 : {
177 27 : switch (reason) {
178 2 : case ModeReason::RTL_COMPLETE_SWITCHING_TO_FIXEDWING_AUTOLAND:
179 : case ModeReason::RTL_COMPLETE_SWITCHING_TO_VTOL_LAND_RTL:
180 : case ModeReason::QRTL_INSTEAD_OF_RTL:
181 : case ModeReason::QLAND_INSTEAD_OF_RTL:
182 2 : return true;
183 25 : default:
184 25 : break;
185 : }
186 25 : return false;
187 : }
188 : #endif // AP_FENCE_ENABLED
189 :
190 : // Check if this mode can be entered from the GCS
191 825 : bool Plane::gcs_mode_enabled(const Mode::Number mode_num) const
192 : {
193 : // List of modes that can be blocked, index is bit number in parameter bitmask
194 : static const uint8_t mode_list [] {
195 : (uint8_t)Mode::Number::MANUAL,
196 : (uint8_t)Mode::Number::CIRCLE,
197 : (uint8_t)Mode::Number::STABILIZE,
198 : (uint8_t)Mode::Number::TRAINING,
199 : (uint8_t)Mode::Number::ACRO,
200 : (uint8_t)Mode::Number::FLY_BY_WIRE_A,
201 : (uint8_t)Mode::Number::FLY_BY_WIRE_B,
202 : (uint8_t)Mode::Number::CRUISE,
203 : (uint8_t)Mode::Number::AUTOTUNE,
204 : (uint8_t)Mode::Number::AUTO,
205 : (uint8_t)Mode::Number::LOITER,
206 : (uint8_t)Mode::Number::TAKEOFF,
207 : (uint8_t)Mode::Number::AVOID_ADSB,
208 : (uint8_t)Mode::Number::GUIDED,
209 : (uint8_t)Mode::Number::THERMAL,
210 : #if HAL_QUADPLANE_ENABLED
211 : (uint8_t)Mode::Number::QSTABILIZE,
212 : (uint8_t)Mode::Number::QHOVER,
213 : (uint8_t)Mode::Number::QLOITER,
214 : (uint8_t)Mode::Number::QACRO,
215 : #if QAUTOTUNE_ENABLED
216 : (uint8_t)Mode::Number::QAUTOTUNE,
217 : #else
218 : 0xFF, // Need to use place holders for modes that can be compiled out so the bits do not change
219 : #endif // QAUTOTUNE_ENABLED
220 : (uint8_t)Mode::Number::LOITER_ALT_QLAND,
221 : #else
222 : 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,
223 : #endif // HAL_QUADPLANE_ENABLED
224 : #if MODE_AUTOLAND_ENABLED
225 : (uint8_t)Mode::Number::AUTOLAND,
226 : #else
227 : 0xFF,
228 : #endif // MODE_AUTOLAND_ENABLED
229 : };
230 :
231 : // Place holders should mean that array is always the same size
232 : static_assert(ARRAY_SIZE(mode_list) == 22, "mode_list placeholders incorrect");
233 :
234 825 : return !block_GCS_mode_change((uint8_t)mode_num, mode_list, ARRAY_SIZE(mode_list));
235 : }
236 :
237 : // Return mask of enabled modes, order does not matter, its just for tracking changes
238 54357 : uint32_t Plane::get_available_mode_enabled_mask() const
239 : {
240 : // plane does not enable or disable modes at run-time.
241 : // This means that the FLTMODE_GCSBLOCK param is the only way modes will be disabled at runtime.
242 : // Rather than tracking modes we can just track the param itself for changes.
243 54357 : return ~uint32_t(flight_mode_GCS_block);
244 : }
245 :
246 3404 : bool Plane::set_mode(Mode &new_mode, const ModeReason reason)
247 : {
248 :
249 3404 : if (control_mode == &new_mode) {
250 : // don't switch modes if we are already in the correct mode.
251 : // only make happy noise if using a different method to switch, this stops beeping for repeated change mode requests from GCS
252 1452 : if ((reason != control_mode_reason) && (reason != ModeReason::INITIALISED)) {
253 648 : AP_Notify::events.user_mode_change = 1;
254 : }
255 1452 : return true;
256 : }
257 :
258 : #if HAL_QUADPLANE_ENABLED
259 1952 : if (new_mode.is_vtol_mode() && !plane.quadplane.available()) {
260 : // dont try and switch to a Q mode if quadplane is not enabled and initialized
261 0 : gcs().send_text(MAV_SEVERITY_INFO,"Q_ENABLE 0");
262 : // make sad noise
263 0 : if (reason != ModeReason::INITIALISED) {
264 0 : AP_Notify::events.user_mode_change_failed = 1;
265 : }
266 0 : return false;
267 : }
268 :
269 : #else
270 : if (new_mode.is_vtol_mode()) {
271 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
272 : gcs().send_text(MAV_SEVERITY_INFO,"HAL_QUADPLANE_ENABLED=0");
273 : // make sad noise
274 : if (reason != ModeReason::INITIALISED) {
275 : AP_Notify::events.user_mode_change_failed = 1;
276 : }
277 : return false;
278 : }
279 : #endif // HAL_QUADPLANE_ENABLED
280 :
281 : #if AP_FENCE_ENABLED
282 : // may not be allowed to change mode if recovering from fence breach
283 2515 : if (hal.util->get_soft_armed() &&
284 656 : fence.enabled() &&
285 185 : fence.option_enabled(AC_Fence::OPTIONS::DISABLE_MODE_CHANGE) &&
286 156 : fence.get_breaches() &&
287 2579 : in_fence_recovery() &&
288 27 : !mode_reason_is_landing_sequence(reason)) {
289 25 : gcs().send_text(MAV_SEVERITY_NOTICE,"Mode change to %s denied, in fence recovery", new_mode.name());
290 25 : AP_Notify::events.user_mode_change_failed = 1;
291 25 : return false;
292 : }
293 : #endif
294 :
295 : // Check if GCS mode change is disabled via parameter
296 1927 : if ((reason == ModeReason::GCS_COMMAND) && !gcs_mode_enabled(new_mode.mode_number())) {
297 0 : gcs().send_text(MAV_SEVERITY_NOTICE,"Mode change to %s denied, GCS entry disabled (FLTMODE_GCSBLOCK)", new_mode.name());
298 0 : return false;
299 : }
300 :
301 : // backup current control_mode and previous_mode
302 1927 : Mode &old_previous_mode = *previous_mode;
303 1927 : Mode &old_mode = *control_mode;
304 :
305 : // update control_mode assuming success
306 : // TODO: move these to be after enter() once start_command_callback() no longer checks control_mode
307 1927 : previous_mode = control_mode;
308 1927 : control_mode = &new_mode;
309 1927 : const ModeReason old_previous_mode_reason = previous_mode_reason;
310 1927 : previous_mode_reason = control_mode_reason;
311 1927 : control_mode_reason = reason;
312 :
313 : // Apply the reset before mode entry so altitude targets use the new value,
314 : // including when mode entry subsequently fails.
315 1927 : reset_alt_offset();
316 :
317 : // attempt to enter new mode
318 1927 : if (!new_mode.enter()) {
319 : // Log error that we failed to enter desired flight mode
320 1 : gcs().send_text(MAV_SEVERITY_WARNING, "Flight mode change failed");
321 :
322 : // we failed entering new mode, roll back to old
323 1 : previous_mode = &old_previous_mode;
324 1 : control_mode = &old_mode;
325 1 : control_mode_reason = previous_mode_reason;
326 1 : previous_mode_reason = old_previous_mode_reason;
327 :
328 : // make sad noise
329 1 : if (reason != ModeReason::INITIALISED) {
330 1 : AP_Notify::events.user_mode_change_failed = 1;
331 : }
332 1 : return false;
333 : }
334 :
335 : // exit previous mode
336 1926 : old_mode.exit();
337 :
338 : // log and notify mode change
339 : #if HAL_LOGGING_ENABLED
340 1926 : logger.Write_Mode(control_mode->mode_number(), control_mode_reason);
341 : #endif
342 1926 : notify_mode(*control_mode);
343 1926 : gcs().send_message(MSG_HEARTBEAT);
344 :
345 : // make happy noise
346 1926 : if (reason != ModeReason::INITIALISED) {
347 1235 : AP_Notify::events.user_mode_change = 1;
348 : }
349 1926 : return true;
350 : }
351 :
352 1153 : bool Plane::set_mode(const uint8_t new_mode, const ModeReason reason)
353 : {
354 : static_assert(sizeof(Mode::Number) == sizeof(new_mode), "The new mode can't be mapped to the vehicles mode number");
355 :
356 1153 : return set_mode_by_number(static_cast<Mode::Number>(new_mode), reason);
357 : }
358 :
359 2550 : bool Plane::set_mode_by_number(const Mode::Number new_mode_number, const ModeReason reason)
360 : {
361 2550 : Mode *new_mode = plane.mode_from_mode_num(new_mode_number);
362 2550 : if (new_mode == nullptr) {
363 0 : notify_no_such_mode(new_mode_number);
364 0 : return false;
365 : }
366 2550 : return set_mode(*new_mode, reason);
367 : }
368 :
369 167812 : void Plane::check_long_failsafe()
370 : {
371 167812 : const uint32_t gcs_last_seen_ms = gcs().sysid_mygcs_last_seen_time_ms();
372 167812 : const uint32_t rc_last_seen_ms = failsafe.last_valid_rc_ms;
373 167812 : const uint32_t tnow = millis();
374 : // only act on changes
375 : // -------------------
376 167812 : if (failsafe.state != FAILSAFE_LONG &&
377 167802 : failsafe.state != FAILSAFE_GCS &&
378 167776 : flight_stage != AP_FixedWing::FlightStage::LAND) {
379 156582 : if (failsafe.rc_failsafe &&
380 77 : (tnow - rc_last_seen_ms) > g.fs_timeout_long*1000) {
381 5 : failsafe_long_on_event(FAILSAFE_LONG, ModeReason::RADIO_FAILSAFE);
382 156500 : } else if (g.gcs_heartbeat_fs_enabled == GCS_FAILSAFE_HB_AUTO && control_mode == &mode_auto &&
383 156500 : gcs_last_seen_ms != 0 &&
384 0 : (tnow - gcs_last_seen_ms) > g.fs_timeout_long*1000) {
385 0 : failsafe_long_on_event(FAILSAFE_GCS, ModeReason::GCS_FAILSAFE);
386 156500 : } else if ((g.gcs_heartbeat_fs_enabled == GCS_FAILSAFE_HEARTBEAT ||
387 156500 : g.gcs_heartbeat_fs_enabled == GCS_FAILSAFE_HB_RSSI) &&
388 313000 : gcs_last_seen_ms != 0 &&
389 312 : (tnow - gcs_last_seen_ms) > g.fs_timeout_long*1000) {
390 5 : failsafe_long_on_event(FAILSAFE_GCS, ModeReason::GCS_FAILSAFE);
391 156495 : } else if (g.gcs_heartbeat_fs_enabled == GCS_FAILSAFE_HB_RSSI &&
392 0 : gcs().chan(0) != nullptr &&
393 156495 : gcs().chan(0)->last_radio_status_remrssi_ms() != 0 &&
394 0 : (tnow - gcs().chan(0)->last_radio_status_remrssi_ms()) > g.fs_timeout_long*1000) {
395 0 : failsafe_long_on_event(FAILSAFE_GCS, ModeReason::GCS_FAILSAFE);
396 : }
397 : } else {
398 11307 : float timeout_seconds = g.fs_timeout_long;
399 11307 : if (failsafe.state == FAILSAFE_GCS &&
400 26 : (tnow - gcs_last_seen_ms) < timeout_seconds*1000) {
401 5 : failsafe_long_off_event(ModeReason::GCS_FAILSAFE);
402 11302 : } else if (failsafe.state == FAILSAFE_LONG &&
403 10 : !failsafe.rc_failsafe) {
404 5 : failsafe_long_off_event(ModeReason::RADIO_FAILSAFE);
405 : }
406 : }
407 167812 : }
408 :
409 2736769 : void Plane::check_short_rc_failsafe()
410 : {
411 2736769 : if (g.fs_action_short != FS_ACTION_SHORT_DISABLED &&
412 5471701 : failsafe.state == FAILSAFE_NONE &&
413 2734932 : flight_stage != AP_FixedWing::FlightStage::LAND) {
414 : // The condition is checked and the flag rc_failsafe is set in radio.cpp
415 2554406 : if (failsafe.rc_failsafe) {
416 5 : rc_failsafe_short_on_event();
417 : }
418 : }
419 :
420 2736769 : if(failsafe.state == FAILSAFE_SHORT) {
421 740 : if(!failsafe.rc_failsafe || g.fs_action_short == FS_ACTION_SHORT_DISABLED) {
422 2 : rc_failsafe_short_off_event();
423 : }
424 : }
425 2736769 : }
426 :
427 :
428 692 : void Plane::startup_INS(void)
429 : {
430 692 : if (ins.gyro_calibration_timing() != AP_InertialSensor::GYRO_CAL_NEVER) {
431 61 : gcs().send_text(MAV_SEVERITY_ALERT, "Beginning INS calibration. Do not move plane");
432 : } else {
433 631 : gcs().send_text(MAV_SEVERITY_ALERT, "Skipping INS calibration");
434 : }
435 :
436 692 : ahrs.init();
437 692 : ahrs.set_fly_forward(true);
438 692 : ahrs.set_vehicle_class(AP_AHRS::VehicleClass::FIXED_WING);
439 692 : ahrs.set_wind_estimation_enabled(true);
440 :
441 692 : ins.init(scheduler.get_loop_rate_hz());
442 692 : ahrs.reset();
443 :
444 : // read Baro pressure at ground
445 : //-----------------------------
446 692 : barometer.set_log_baro_bit(MASK_LOG_IMU);
447 692 : barometer.calibrate();
448 691 : }
449 :
450 : // sets notify object flight mode information
451 2618 : void Plane::notify_mode(const Mode& mode)
452 : {
453 2618 : notify.flags.flight_mode = mode.mode_number();
454 2618 : notify.set_flight_mode_str(mode.name4());
455 2618 : }
456 :
457 : #if HAL_LOGGING_ENABLED
458 : /*
459 : should we log a message type now?
460 : */
461 45817473 : bool Plane::should_log(uint32_t mask)
462 : {
463 45817473 : return logger.should_log(mask);
464 : }
465 : #endif
466 :
467 : /*
468 : return throttle percentage from 0 to 100 for normal use and -100 to 100 when using reverse thrust
469 : */
470 1667524 : int8_t Plane::throttle_percentage(void)
471 : {
472 : #if HAL_QUADPLANE_ENABLED
473 1667524 : if (quadplane.in_vtol_mode() && !quadplane.tailsitter.in_vtol_transition()) {
474 91412 : return quadplane.motors->get_throttle_out() * 100.0;
475 : }
476 : #endif
477 1576112 : float throttle = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle);
478 1576112 : if (!have_reverse_thrust()) {
479 1576112 : return constrain_int16(throttle, 0, 100);
480 : }
481 0 : return constrain_int16(throttle, -100, 100);
482 : }
|
