Line data Source code
1 : /*
2 : Lead developer: Andrew Tridgell
3 :
4 : Authors: Doug Weibel, Jose Julio, Jordi Munoz, Jason Short, Randy Mackay, Pat Hickey, John Arne Birkeland, Olivier Adler, Amilcar Lucas, Gregory Fletcher, Paul Riseborough, Brandon Jones, Jon Challinger, Tom Pittenger
5 : Thanks to: Chris Anderson, Michael Oborne, Paul Mather, Bill Premerlani, James Cohen, JB from rotorFX, Automatik, Fefenin, Peter Meister, Remzibi, Yury Smirnov, Sandro Benigno, Max Levine, Roberto Navoni, Lorenz Meier, Yury MonZon
6 :
7 : Please contribute your ideas! See https://ardupilot.club/dev for details
8 :
9 : This program is free software: you can redistribute it and/or modify
10 : it under the terms of the GNU General Public License as published by
11 : the Free Software Foundation, either version 3 of the License, or
12 : (at your option) any later version.
13 :
14 : This program is distributed in the hope that it will be useful,
15 : but WITHOUT ANY WARRANTY; without even the implied warranty of
16 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17 : GNU General Public License for more details.
18 :
19 : You should have received a copy of the GNU General Public License
20 : along with this program. If not, see <http://www.gnu.org/licenses/>.
21 : */
22 :
23 : #include "Plane.h"
24 :
25 : #define FORCE_VERSION_H_INCLUDE
26 : #include "version.h"
27 : #undef FORCE_VERSION_H_INCLUDE
28 :
29 : const AP_HAL::HAL& hal = AP_HAL::get_HAL();
30 :
31 : #define SCHED_TASK(func, rate_hz, max_time_micros, priority) SCHED_TASK_CLASS(Plane, &plane, func, rate_hz, max_time_micros, priority)
32 : #define FAST_TASK(func) FAST_TASK_CLASS(Plane, &plane, func)
33 :
34 :
35 : /*
36 : scheduler table - all regular tasks should be listed here.
37 :
38 : All entries in this table must be ordered by priority.
39 :
40 : This table is interleaved with the table present in each of the
41 : vehicles to determine the order in which tasks are run. Convenience
42 : methods SCHED_TASK and SCHED_TASK_CLASS are provided to build
43 : entries in this structure:
44 :
45 : SCHED_TASK arguments:
46 : - name of static function to call
47 : - rate (in Hertz) at which the function should be called
48 : - expected time (in MicroSeconds) that the function should take to run
49 : - priority (0 through 255, lower number meaning higher priority)
50 :
51 : SCHED_TASK_CLASS arguments:
52 : - class name of method to be called
53 : - instance on which to call the method
54 : - method to call on that instance
55 : - rate (in Hertz) at which the method should be called
56 : - expected time (in MicroSeconds) that the method should take to run
57 : - priority (0 through 255, lower number meaning higher priority)
58 :
59 : FAST_TASK entries are run on every loop even if that means the loop
60 : overruns its allotted time
61 : */
62 : const AP_Scheduler::Task Plane::scheduler_tasks[] = {
63 : // Units: Hz us
64 : FAST_TASK(ahrs_update),
65 : FAST_TASK(update_control_mode),
66 : FAST_TASK(stabilize),
67 : FAST_TASK(set_servos),
68 : SCHED_TASK(read_radio, 50, 100, 6),
69 : SCHED_TASK(check_short_rc_failsafe, 50, 100, 9),
70 : SCHED_TASK(update_speed_height, 50, 200, 12),
71 : SCHED_TASK(update_throttle_hover, 100, 90, 24),
72 : SCHED_TASK_CLASS(RC_Channels, (RC_Channels*)&plane.g2.rc_channels, read_mode_switch, 7, 100, 27),
73 : SCHED_TASK(update_GPS_50Hz, 50, 300, 30),
74 : SCHED_TASK(update_GPS_10Hz, 10, 400, 33),
75 : SCHED_TASK(navigate, 10, 150, 36),
76 : SCHED_TASK(update_compass, 10, 200, 39),
77 : SCHED_TASK(calc_airspeed_errors, 10, 100, 42),
78 : SCHED_TASK(update_alt, 10, 200, 45),
79 : SCHED_TASK(adjust_altitude_target, 10, 200, 48),
80 : #if AP_ADVANCEDFAILSAFE_ENABLED
81 : SCHED_TASK(afs_fs_check, 10, 100, 51),
82 : #endif
83 : SCHED_TASK(ekf_check, 10, 75, 54),
84 : SCHED_TASK_CLASS(GCS, (GCS*)&plane._gcs, update_receive, 300, 500, 57),
85 : SCHED_TASK_CLASS(GCS, (GCS*)&plane._gcs, update_send, 300, 750, 60),
86 : #if AP_SERVORELAYEVENTS_ENABLED
87 : SCHED_TASK_CLASS(AP_ServoRelayEvents, &plane.ServoRelayEvents, update_events, 50, 150, 63),
88 : #endif
89 : SCHED_TASK_CLASS(AP_BattMonitor, &plane.battery, read, 10, 300, 66),
90 : #if AP_RANGEFINDER_ENABLED
91 : SCHED_TASK(read_rangefinder, 50, 100, 78),
92 : #endif
93 : #if AP_ICENGINE_ENABLED
94 : SCHED_TASK_CLASS(AP_ICEngine, &plane.g2.ice_control, update, 10, 100, 81),
95 : #endif
96 : #if AP_OPTICALFLOW_ENABLED
97 : SCHED_TASK_CLASS(AP_OpticalFlow, &plane.optflow, update, 50, 50, 87),
98 : #endif
99 : SCHED_TASK(one_second_loop, 1, 400, 90),
100 : SCHED_TASK(three_hz_loop, 3, 75, 93),
101 : SCHED_TASK(check_long_failsafe, 3, 400, 96),
102 : #if AP_AIRSPEED_AUTOCAL_ENABLE
103 : SCHED_TASK(airspeed_ratio_update, 1, 100, 102),
104 : #endif // AP_AIRSPEED_AUTOCAL_ENABLE
105 : #if HAL_MOUNT_ENABLED
106 : SCHED_TASK_CLASS(AP_Mount, &plane.camera_mount, update, 50, 100, 105),
107 : #endif // HAL_MOUNT_ENABLED
108 : #if AP_CAMERA_ENABLED
109 : SCHED_TASK_CLASS(AP_Camera, &plane.camera, update, 50, 100, 108),
110 : #endif // AP_CAMERA_ENABLED
111 : #if HAL_LOGGING_ENABLED
112 : SCHED_TASK_CLASS(AP_Scheduler, &plane.scheduler, update_logging, 0.2, 100, 111),
113 : #endif
114 : #if HAL_LOGGING_ENABLED
115 : SCHED_TASK(Log_Write_FullRate, 400, 300, 117),
116 : SCHED_TASK(update_logging10, 10, 300, 120),
117 : SCHED_TASK(update_logging25, 25, 300, 123),
118 : #endif
119 : #if HAL_SOARING_ENABLED
120 : SCHED_TASK(update_soaring, 50, 400, 126),
121 : #endif
122 : SCHED_TASK(parachute_check, 10, 200, 129),
123 : #if AP_TERRAIN_AVAILABLE
124 : SCHED_TASK_CLASS(AP_Terrain, &plane.terrain, update, 10, 200, 132),
125 : #endif // AP_TERRAIN_AVAILABLE
126 : SCHED_TASK(update_is_flying_5Hz, 5, 100, 135),
127 : #if HAL_LOGGING_ENABLED
128 : SCHED_TASK_CLASS(AP_Logger, &plane.logger, periodic_tasks, 400, 300, 138),
129 : #endif
130 : SCHED_TASK_CLASS(AP_InertialSensor, &plane.ins, periodic, 50, 50, 141),
131 : #if HAL_ADSB_ENABLED || AP_ADSB_AVOIDANCE_ENABLED
132 : SCHED_TASK(avoidance_adsb_update, 10, 100, 144),
133 : #endif // HAL_ADSB_ENABLED || AP_ADSB_AVOIDANCE_ENABLED
134 : SCHED_TASK_CLASS(RC_Channels, (RC_Channels*)&plane.g2.rc_channels, read_aux_all, 10, 200, 147),
135 : #if HAL_BUTTON_ENABLED
136 : SCHED_TASK_CLASS(AP_Button, &plane.button, update, 5, 100, 150),
137 : #endif
138 : #if AP_LANDINGGEAR_ENABLED
139 : SCHED_TASK(landing_gear_update, 5, 50, 159),
140 : #endif
141 : #if AC_PRECLAND_ENABLED
142 : SCHED_TASK(precland_update, 400, 50, 160),
143 : #endif
144 : #if AP_QUICKTUNE_ENABLED
145 : SCHED_TASK(update_quicktune, 40, 100, 163),
146 : #endif
147 : };
148 :
149 692 : void Plane::get_scheduler_tasks(const AP_Scheduler::Task *&tasks,
150 : uint8_t &task_count,
151 : uint32_t &log_bit)
152 : {
153 692 : tasks = &scheduler_tasks[0];
154 692 : task_count = ARRAY_SIZE(scheduler_tasks);
155 692 : log_bit = MASK_LOG_PM;
156 692 : }
157 :
158 : #if HAL_QUADPLANE_ENABLED
159 : constexpr int8_t Plane::_failsafe_priorities[7];
160 : #else
161 : constexpr int8_t Plane::_failsafe_priorities[6];
162 : #endif
163 :
164 : // update AHRS system
165 8377547 : void Plane::ahrs_update()
166 : {
167 8377547 : arming.update_soft_armed();
168 :
169 8377547 : ahrs.update();
170 :
171 : #if HAL_LOGGING_ENABLED
172 8377547 : if (should_log(MASK_LOG_IMU)) {
173 7831972 : AP::ins().Write_IMU();
174 : }
175 : #endif
176 :
177 : // calculate a scaled roll limit based on current pitch
178 8377547 : roll_limit_cd = aparm.roll_limit*100;
179 8377547 : pitch_limit_min = aparm.pitch_limit_min;
180 :
181 8377547 : bool rotate_limits = true;
182 : #if HAL_QUADPLANE_ENABLED
183 8377547 : if (quadplane.tailsitter.active()) {
184 96659 : rotate_limits = false;
185 : }
186 : #endif
187 8377547 : if (rotate_limits) {
188 8280888 : roll_limit_cd *= ahrs.cos_pitch();
189 8280888 : pitch_limit_min *= fabsf(ahrs.cos_roll());
190 : }
191 :
192 : // updated the summed gyro used for ground steering and
193 : // auto-takeoff. Dot product of DCM.c with gyro vector gives earth
194 : // frame yaw rate
195 8377547 : steer_state.locked_course_err += ahrs.get_yaw_rate_earth() * G_Dt;
196 8377547 : steer_state.locked_course_err = wrap_PI(steer_state.locked_course_err);
197 :
198 : // Check if there has been a change in attitude estimate which the attitude controllers should be told about
199 8377547 : check_ahrs_reset();
200 :
201 : #if HAL_QUADPLANE_ENABLED
202 : // update inertial_nav for quadplane
203 8377547 : quadplane.inertial_nav.update();
204 8377547 : if (quadplane.available()) {
205 6504054 : quadplane.pos_control->update_estimates();
206 : }
207 : #endif
208 : #if AP_SCRIPTING_ENABLED && AP_FOLLOW_ENABLED
209 8377547 : g2.follow.update_estimates();
210 : #endif
211 :
212 : #if HAL_LOGGING_ENABLED
213 8377547 : if (should_log(MASK_LOG_VIDEO_STABILISATION)) {
214 0 : ahrs.write_video_stabilisation();
215 : }
216 : #endif
217 8377547 : }
218 :
219 : /*
220 : update 50Hz speed/height controller
221 : */
222 2736769 : void Plane::update_speed_height(void)
223 : {
224 2736769 : bool should_run_tecs = control_mode->does_auto_throttle();
225 : #if HAL_QUADPLANE_ENABLED
226 2736769 : if (quadplane.should_disable_TECS()) {
227 149840 : should_run_tecs = false;
228 : }
229 : #endif
230 :
231 2736769 : if (auto_state.idle_mode) {
232 52328 : should_run_tecs = false;
233 : }
234 :
235 : #if AP_PLANE_GLIDER_PULLUP_ENABLED
236 2736769 : if (mode_auto.in_pullup()) {
237 405 : should_run_tecs = false;
238 : }
239 : #endif
240 :
241 2736769 : if (should_run_tecs) {
242 : // Call TECS 50Hz update. Note that we call this regardless of
243 : // throttle suppressed, as this needs to be running for
244 : // takeoff detection
245 1657292 : TECS_controller.update_50hz();
246 : }
247 :
248 : #if HAL_QUADPLANE_ENABLED
249 5016592 : if (quadplane.in_vtol_mode() ||
250 2279823 : quadplane.in_assisted_flight()) {
251 527359 : quadplane.update_throttle_mix();
252 : }
253 : #endif
254 2736769 : }
255 :
256 :
257 : /*
258 : read and update compass
259 : */
260 547097 : void Plane::update_compass(void)
261 : {
262 547097 : compass.read();
263 547097 : }
264 :
265 : #if HAL_LOGGING_ENABLED
266 : /*
267 : do 10Hz logging
268 : */
269 547089 : void Plane::update_logging10(void)
270 : {
271 547089 : const bool attitude_med = should_log(MASK_LOG_ATTITUDE_MED);
272 547089 : const bool attitude_faster = should_log(MASK_LOG_ATTITUDE_FULLRATE) || should_log(MASK_LOG_ATTITUDE_FAST);
273 :
274 : // Log attitude only if no faster logging is selected
275 547089 : if (attitude_med && !attitude_faster) {
276 196072 : Log_Write_Attitude();
277 196072 : AP::ahrs().Log_Write();
278 : }
279 :
280 : // If any attitude logging is enabled log AOA and SSA
281 547089 : if (attitude_med || attitude_faster) {
282 524212 : ahrs.Write_AOA_SSA();
283 : }
284 :
285 : #if HAL_MOUNT_ENABLED
286 547089 : if (should_log(MASK_LOG_CAMERA)) {
287 521283 : camera_mount.write_log();
288 : }
289 : #endif
290 : #if AP_RANGEFINDER_ENABLED
291 547089 : if (should_log(MASK_LOG_NTUN)) {
292 542410 : if (rangefinder.has_orientation(rangefinder_orientation()) &&
293 18198 : (g.rangefinder_landing.get() > 0)) {
294 5098 : Log_Write_RFNS();
295 : }
296 : }
297 : #endif
298 547089 : }
299 :
300 : /*
301 : do 25Hz logging
302 : */
303 1367994 : void Plane::update_logging25(void)
304 : {
305 1367994 : const bool attitude_fast = should_log(MASK_LOG_ATTITUDE_FAST);
306 1367994 : const bool attitude_full_rate = should_log(MASK_LOG_ATTITUDE_FULLRATE);
307 :
308 : // Log at fast rate if fast logging is the fastest enabled
309 1367994 : if (attitude_fast && !attitude_full_rate) {
310 820469 : Log_Write_Attitude();
311 : }
312 :
313 : // Log AHRS at fast rate if either fast or full rate is selected
314 1367994 : if (attitude_fast || attitude_full_rate) {
315 820469 : AP::ahrs().Log_Write();
316 : }
317 :
318 1367994 : if (should_log(MASK_LOG_CTUN)) {
319 1310707 : Log_Write_Control_Tuning();
320 : #if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
321 1310707 : if (!should_log(MASK_LOG_NOTCH_FULLRATE)) {
322 1310707 : AP::ins().write_notch_log_messages();
323 : }
324 : #endif
325 : #if HAL_GYROFFT_ENABLED
326 1310707 : gyro_fft.write_log_messages();
327 : #endif
328 : }
329 :
330 1367994 : if (should_log(MASK_LOG_NTUN)) {
331 1310707 : Log_Write_Nav_Tuning();
332 1310707 : Log_Write_Guided();
333 : }
334 :
335 1367994 : if (should_log(MASK_LOG_RC))
336 1310707 : Log_Write_RC();
337 :
338 1367994 : if (should_log(MASK_LOG_IMU))
339 1310707 : AP::ins().Write_Vibration();
340 :
341 : #if AP_PLANE_BLACKBOX_LOGGING
342 : Log_Write_Blackbox();
343 : #endif
344 1367994 : }
345 : #endif // HAL_LOGGING_ENABLED
346 :
347 : /*
348 : check for AFS failsafe check
349 : */
350 : #if AP_ADVANCEDFAILSAFE_ENABLED
351 547097 : void Plane::afs_fs_check(void)
352 : {
353 547097 : afs.check(failsafe.AFS_last_valid_rc_ms);
354 547097 : }
355 : #endif
356 :
357 : #if HAL_WITH_IO_MCU
358 : #include <AP_IOMCU/AP_IOMCU.h>
359 : extern AP_IOMCU iomcu;
360 : #endif
361 :
362 54357 : void Plane::one_second_loop()
363 : {
364 : // make it possible to change control channel ordering at runtime
365 54357 : set_control_channels();
366 :
367 : #if HAL_WITH_IO_MCU
368 : iomcu.setup_mixing(g.override_channel.get(), g.mixing_gain, g2.manual_rc_mask);
369 : #endif
370 :
371 : #if HAL_ADSB_ENABLED
372 54357 : adsb.set_stall_speed_cm(aparm.airspeed_min * 100); // convert m/s to cm/s
373 54357 : adsb.set_max_speed(aparm.airspeed_max);
374 : #endif
375 :
376 54357 : if (flight_option_enabled(FlightOptions::ENABLE_DEFAULT_AIRSPEED)) {
377 : // use average of min and max airspeed as default airspeed fusion with high variance
378 0 : ahrs.writeDefaultAirSpeed((float)((aparm.airspeed_min + aparm.airspeed_max)/2),
379 0 : (float)((aparm.airspeed_max - aparm.airspeed_min)/2));
380 : }
381 :
382 54357 : AP::srv().enable_aux_servos();
383 :
384 : // update notify flags
385 54357 : AP_Notify::flags.pre_arm_check = arming.pre_arm_checks(false);
386 54357 : AP_Notify::flags.pre_arm_gps_check = true;
387 54357 : AP_Notify::flags.armed = arming.is_armed() || arming.arming_required() == AP_Arming::Required::NO;
388 :
389 : #if AP_TERRAIN_AVAILABLE && HAL_LOGGING_ENABLED
390 54357 : if (should_log(MASK_LOG_GPS)) {
391 52128 : terrain.log_terrain_data();
392 : }
393 : #endif
394 :
395 : // update home position if NOT armed and gps position has
396 : // changed. Update every 5s at most
397 54357 : if (!arming.is_armed() &&
398 56356 : gps.last_message_time_ms() - last_home_update_ms > 5000 &&
399 1999 : gps.status() >= AP_GPS_FixType::FIX_3D) {
400 1953 : last_home_update_ms = gps.last_message_time_ms();
401 1953 : update_home();
402 :
403 : // reset the landing altitude correction
404 1953 : landing.alt_offset = 0;
405 : }
406 :
407 54357 : const float loop_rate = AP::scheduler().get_filtered_loop_rate_hz();
408 : #if HAL_QUADPLANE_ENABLED
409 54357 : if (quadplane.available()) {
410 21571 : quadplane.attitude_control->set_notch_sample_rate(loop_rate);
411 : }
412 : #endif
413 54357 : rollController.set_notch_sample_rate(loop_rate);
414 54357 : pitchController.set_notch_sample_rate(loop_rate);
415 54357 : yawController.set_notch_sample_rate(loop_rate);
416 54357 : }
417 :
418 167812 : void Plane::three_hz_loop()
419 : {
420 : #if AP_FENCE_ENABLED
421 167812 : fence_check();
422 : #endif
423 167812 : }
424 :
425 : #if AP_AIRSPEED_AUTOCAL_ENABLE
426 : /*
427 : once a second update the airspeed calibration ratio
428 : */
429 54357 : void Plane::airspeed_ratio_update(void)
430 : {
431 54357 : if (!hal.util->get_soft_armed() ||
432 41445 : !ahrs.get_fly_forward() ||
433 31561 : !is_flying() ||
434 60858 : !airspeed.enabled() ||
435 126231 : gps.status() < AP_GPS_FixType::FIX_3D ||
436 30225 : gps.ground_speed() < 4) {
437 : // don't calibrate when not moving
438 24850 : return;
439 : }
440 30522 : if (airspeed.get_airspeed() < aparm.airspeed_min &&
441 1015 : gps.ground_speed() < (uint32_t)aparm.airspeed_min) {
442 : // don't calibrate when flying below the minimum airspeed. We
443 : // check both airspeed and ground speed to catch cases where
444 : // the airspeed ratio is way too low, which could lead to it
445 : // never coming up again
446 799 : return;
447 : }
448 85855 : if (labs(ahrs.roll_sensor) > roll_limit_cd ||
449 57120 : ahrs.get_pitch_deg() > aparm.pitch_limit_max ||
450 28412 : ahrs.get_pitch_deg() < pitch_limit_min) {
451 : // don't calibrate when going beyond normal flight envelope
452 453 : return;
453 : }
454 28255 : const Vector3f &vg = gps.velocity();
455 28255 : airspeed.update_calibration(vg, aparm.airspeed_max);
456 : }
457 : #endif // AP_AIRSPEED_AUTOCAL_ENABLE
458 :
459 : /*
460 : read the GPS and update position
461 : */
462 2736769 : void Plane::update_GPS_50Hz(void)
463 : {
464 2736769 : gps.update();
465 :
466 2736769 : update_current_loc();
467 2736769 : }
468 :
469 : /*
470 : read update GPS position - 10Hz update
471 : */
472 547097 : void Plane::update_GPS_10Hz(void)
473 : {
474 : static uint32_t last_gps_msg_ms;
475 547097 : if (gps.last_message_time_ms() != last_gps_msg_ms && gps.status() >= AP_GPS_FixType::FIX_3D) {
476 258716 : last_gps_msg_ms = gps.last_message_time_ms();
477 :
478 258716 : if (ground_start_count > 1) {
479 1412 : ground_start_count--;
480 257304 : } else if (ground_start_count == 1) {
481 : // We countdown N number of good GPS fixes
482 : // so that the altitude is more accurate
483 : // -------------------------------------
484 405 : if (current_loc.lat == 0 && current_loc.lng == 0) {
485 0 : ground_start_count = 5;
486 :
487 405 : } else if (!hal.util->was_watchdog_reset()) {
488 351 : if (!set_home_persistently(gps.location())) {
489 : // silently ignore failure...
490 : }
491 :
492 351 : next_WP_loc = prev_WP_loc = home;
493 :
494 351 : ground_start_count = 0;
495 : }
496 : }
497 288381 : } else if (gps.status() < AP_GPS_FixType::FIX_3D && ground_start_count != 0) {
498 : // lost 3D fix, start again
499 33000 : ground_start_count = 5;
500 : }
501 :
502 547097 : calc_gndspeed_undershoot();
503 547097 : }
504 :
505 : /*
506 : main control mode dependent update code
507 : */
508 8377547 : void Plane::update_control_mode(void)
509 : {
510 8377547 : if ((control_mode != &mode_auto) && (control_mode != &mode_takeoff)) {
511 : // hold_course is only used in takeoff and landing
512 4635334 : steer_state.hold_course_cd = -1;
513 : }
514 : // refresh the throttle limits, to avoid using stale values
515 : // they will be updated once takeoff_calc_throttle is called
516 8377547 : takeoff_state.throttle_lim_max = 100.0f;
517 8377547 : takeoff_state.throttle_lim_min = -100.0f;
518 :
519 8377547 : update_fly_forward();
520 :
521 8377547 : control_mode->update();
522 :
523 : #if MODE_AUTOLAND_ENABLED
524 8377547 : mode_autoland.check_takeoff_direction();
525 : #endif
526 8377547 : }
527 :
528 :
529 8377547 : void Plane::update_fly_forward(void)
530 : {
531 : // ensure we are fly-forward when we are flying as a pure fixed
532 : // wing aircraft. This helps the EKF produce better state
533 : // estimates as it can make stronger assumptions
534 : #if HAL_QUADPLANE_ENABLED
535 8377547 : if (quadplane.available()) {
536 6504054 : if (quadplane.tailsitter.is_in_fw_flight()) {
537 60753 : ahrs.set_fly_forward(true);
538 60753 : return;
539 : }
540 :
541 6443301 : if (quadplane.in_vtol_mode()) {
542 2741706 : ahrs.set_fly_forward(false);
543 2741706 : return;
544 : }
545 :
546 3701595 : if (quadplane.in_assisted_flight()) {
547 419556 : ahrs.set_fly_forward(false);
548 419556 : return;
549 : }
550 : }
551 : #endif
552 :
553 5155532 : if (auto_state.idle_mode) {
554 : // don't fuse airspeed when in balloon lift
555 199220 : ahrs.set_fly_forward(false);
556 199220 : return;
557 : }
558 :
559 4956312 : if (flight_stage == AP_FixedWing::FlightStage::LAND) {
560 197641 : ahrs.set_fly_forward(landing.is_flying_forward());
561 197641 : return;
562 : }
563 :
564 4758671 : ahrs.set_fly_forward(true);
565 : }
566 :
567 : /*
568 : set the flight stage
569 : */
570 627562 : void Plane::set_flight_stage(AP_FixedWing::FlightStage fs)
571 : {
572 627562 : if (fs == flight_stage) {
573 619477 : return;
574 : }
575 :
576 8085 : const bool is_landing = (fs == AP_FixedWing::FlightStage::LAND);
577 :
578 8085 : landing.handle_flight_stage_change(is_landing);
579 :
580 : #if AP_LANDINGGEAR_ENABLED
581 8085 : if (is_landing) {
582 232 : plane.g2.landing_gear.deploy_for_landing();
583 : }
584 :
585 8085 : const bool is_takeoff_complete = (flight_stage == AP_FixedWing::FlightStage::TAKEOFF &&
586 : fs == AP_FixedWing::FlightStage::NORMAL);
587 3521 : if (is_takeoff_complete &&
588 11606 : arming.is_armed_and_safety_off() &&
589 369 : is_flying()) {
590 122 : g2.landing_gear.retract_after_takeoff();
591 : }
592 : #endif
593 :
594 8085 : if (fs == AP_FixedWing::FlightStage::ABORT_LANDING) {
595 2 : gcs().send_text(MAV_SEVERITY_NOTICE, "Landing aborted, climbing to %dm",
596 2 : int(auto_state.takeoff_altitude_rel_cm/100));
597 : }
598 :
599 8085 : flight_stage = fs;
600 : #if HAL_LOGGING_ENABLED
601 8085 : Log_Write_Status();
602 : #endif
603 : }
604 :
605 547097 : void Plane::update_alt()
606 : {
607 547097 : barometer.update();
608 :
609 : // calculate the sink rate.
610 : float sink_rate;
611 547097 : Vector3f vel;
612 547097 : if (ahrs.get_velocity_NED(vel)) {
613 514955 : sink_rate = vel.z;
614 32142 : } else if (gps.status() >= AP_GPS_FixType::FIX_3D && gps.have_vertical_velocity()) {
615 2 : sink_rate = gps.velocity().z;
616 : } else {
617 32140 : sink_rate = -barometer.get_climb_rate();
618 : }
619 :
620 : // low pass the sink rate to take some of the noise out
621 547097 : auto_state.sink_rate = 0.8f * auto_state.sink_rate + 0.2f*sink_rate;
622 : #if HAL_PARACHUTE_ENABLED
623 547097 : parachute.set_sink_rate(auto_state.sink_rate);
624 : #endif
625 :
626 547097 : update_flight_stage();
627 :
628 : #if AP_SCRIPTING_ENABLED
629 547097 : if (nav_scripting_active()) {
630 : // don't call TECS while we are in a trick
631 1331 : return;
632 : }
633 : #endif
634 :
635 545766 : bool should_run_tecs = control_mode->does_auto_throttle();
636 : #if HAL_QUADPLANE_ENABLED
637 545766 : if (quadplane.should_disable_TECS()) {
638 29987 : should_run_tecs = false;
639 : }
640 : #endif
641 :
642 545766 : if (auto_state.idle_mode) {
643 10465 : should_run_tecs = false;
644 : }
645 :
646 : #if AP_PLANE_GLIDER_PULLUP_ENABLED
647 545766 : if (mode_auto.in_pullup()) {
648 81 : should_run_tecs = false;
649 : }
650 : #endif
651 :
652 545766 : if (should_run_tecs && !throttle_suppressed) {
653 :
654 313893 : float distance_beyond_land_wp = 0;
655 349999 : if (flight_stage == AP_FixedWing::FlightStage::LAND &&
656 36106 : current_loc.past_interval_finish_line(prev_WP_loc, next_WP_loc)) {
657 15770 : distance_beyond_land_wp = current_loc.get_distance(next_WP_loc);
658 : }
659 :
660 313893 : tecs_target_alt_cm = relative_target_altitude_cm();
661 :
662 313893 : if (control_mode == &mode_rtl && !rtl.done_climb && (g2.rtl_climb_min > 0 || (plane.flight_option_enabled(FlightOptions::CLIMB_BEFORE_TURN)))) {
663 : // ensure we do the initial climb in RTL. We add an extra
664 : // 10m in the demanded height to push TECS to climb
665 : // quickly
666 391 : tecs_target_alt_cm = MAX(tecs_target_alt_cm, prev_WP_loc.alt - home.alt) + (g2.rtl_climb_min+10)*100;
667 : }
668 :
669 627786 : TECS_controller.update_pitch_throttle(tecs_target_alt_cm,
670 : target_airspeed_cm,
671 : flight_stage,
672 : distance_beyond_land_wp,
673 313893 : get_takeoff_pitch_min_cd(),
674 313893 : throttle_nudge,
675 : tecs_hgt_afe(),
676 : aerodynamic_load_factor,
677 313893 : g.pitch_trim.get());
678 : }
679 : }
680 :
681 : /*
682 : recalculate the flight_stage
683 : */
684 553198 : void Plane::update_flight_stage(void)
685 : {
686 : // Update the speed & height controller states
687 553198 : if (control_mode->does_auto_throttle() && !throttle_suppressed) {
688 355708 : if (control_mode == &mode_auto) {
689 : #if HAL_QUADPLANE_ENABLED
690 238091 : if (quadplane.in_vtol_auto()) {
691 28627 : set_flight_stage(AP_FixedWing::FlightStage::VTOL);
692 28627 : return;
693 : }
694 : #endif
695 209464 : if (auto_state.takeoff_complete == false) {
696 3689 : set_flight_stage(AP_FixedWing::FlightStage::TAKEOFF);
697 3689 : return;
698 205775 : } else if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND) {
699 35416 : if (landing.is_commanded_go_around() || flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING) {
700 : // abort mode is sticky, it must complete while executing NAV_LAND
701 247 : set_flight_stage(AP_FixedWing::FlightStage::ABORT_LANDING);
702 35169 : } else if (landing.get_abort_throttle_enable() && get_throttle_input() >= 90 &&
703 0 : landing.request_go_around()) {
704 0 : gcs().send_text(MAV_SEVERITY_INFO,"Landing aborted via throttle");
705 0 : set_flight_stage(AP_FixedWing::FlightStage::ABORT_LANDING);
706 : } else {
707 35169 : set_flight_stage(AP_FixedWing::FlightStage::LAND);
708 : }
709 35416 : return;
710 : }
711 : #if HAL_QUADPLANE_ENABLED
712 170359 : if (quadplane.in_assisted_flight()) {
713 4429 : set_flight_stage(AP_FixedWing::FlightStage::VTOL);
714 4429 : return;
715 : }
716 : #endif
717 165930 : set_flight_stage(AP_FixedWing::FlightStage::NORMAL);
718 117617 : } else if ((control_mode != &mode_takeoff)
719 : #if MODE_AUTOLAND_ENABLED
720 104768 : && (control_mode != &mode_autoland)
721 : #endif
722 : ) {
723 : // If not in AUTO then assume normal operation for normal TECS operation.
724 : // This prevents TECS from being stuck in the wrong stage if you switch from
725 : // AUTO to, say, FBWB during a landing, an aborted landing or takeoff.
726 99307 : set_flight_stage(AP_FixedWing::FlightStage::NORMAL);
727 : }
728 283547 : return;
729 : }
730 : #if HAL_QUADPLANE_ENABLED
731 345884 : if (quadplane.in_vtol_mode() ||
732 148394 : quadplane.in_assisted_flight()) {
733 51576 : set_flight_stage(AP_FixedWing::FlightStage::VTOL);
734 51576 : return;
735 : }
736 : #endif
737 145914 : set_flight_stage(AP_FixedWing::FlightStage::NORMAL);
738 : }
739 :
740 :
741 :
742 :
743 : /*
744 : If land_DisarmDelay is enabled (non-zero), check for a landing then auto-disarm after time expires
745 :
746 : only called from AP_Landing, when the landing library is ready to disarm
747 : */
748 26301 : void Plane::disarm_if_autoland_complete()
749 : {
750 26301 : if (landing.get_disarm_delay() > 0 &&
751 32333 : !is_flying() &&
752 58634 : arming.arming_required() != AP_Arming::Required::NO &&
753 6047 : arming.is_armed()) {
754 : /* we have auto disarm enabled. See if enough time has passed */
755 3285 : if (millis() - auto_state.last_flying_ms >= landing.get_disarm_delay()*1000UL) {
756 112 : if (arming.disarm(AP_Arming::Method::AUTOLANDED)) {
757 112 : gcs().send_text(MAV_SEVERITY_INFO,"Auto disarmed");
758 : }
759 : }
760 : }
761 26301 : }
762 :
763 2 : bool Plane::trigger_land_abort(const float climb_to_alt_m)
764 : {
765 2 : if (plane.control_mode != &plane.mode_auto) {
766 0 : return false;
767 : }
768 : #if HAL_QUADPLANE_ENABLED
769 2 : if (plane.quadplane.in_vtol_auto()) {
770 0 : return quadplane.abort_landing();
771 : }
772 : #endif
773 :
774 2 : uint16_t mission_id = plane.mission.get_current_nav_cmd().id;
775 2 : bool is_in_landing = (plane.flight_stage == AP_FixedWing::FlightStage::LAND) ||
776 0 : plane.is_land_command(mission_id);
777 2 : if (is_in_landing) {
778 : // fly a user planned abort pattern if available
779 2 : if (plane.have_position && plane.mission.jump_to_abort_landing_sequence(plane.current_loc)) {
780 0 : return true;
781 : }
782 :
783 : // only fly a fixed wing abort if we aren't doing quadplane stuff, or potentially
784 : // shooting a quadplane approach
785 : #if HAL_QUADPLANE_ENABLED
786 : const bool attempt_go_around =
787 2 : (!plane.quadplane.available()) ||
788 0 : ((!plane.quadplane.in_vtol_auto()) &&
789 0 : (!plane.quadplane.landing_with_fixed_wing_spiral_approach()));
790 : #else
791 : const bool attempt_go_around = true;
792 : #endif
793 2 : if (attempt_go_around) {
794 : // Initiate an aborted landing. This will trigger a pitch-up and
795 : // climb-out to a safe altitude holding heading then one of the
796 : // following actions will occur, check for in this order:
797 : // - If MAV_CMD_CONTINUE_AND_CHANGE_ALT is next command in mission,
798 : // increment mission index to execute it
799 : // - else if DO_LAND_START is available, jump to it
800 : // - else decrement the mission index to repeat the landing approach
801 :
802 2 : if (!is_zero(climb_to_alt_m)) {
803 2 : plane.auto_state.takeoff_altitude_rel_cm = climb_to_alt_m * 100;
804 : }
805 2 : if (plane.landing.request_go_around()) {
806 2 : plane.auto_state.next_wp_crosstrack = false;
807 2 : return true;
808 : }
809 : }
810 : }
811 0 : return false;
812 : }
813 :
814 :
815 : /*
816 : the height above field elevation that we pass to TECS
817 : */
818 313893 : float Plane::tecs_hgt_afe(void)
819 : {
820 : /*
821 : pass the height above field elevation as the height above
822 : the ground when in landing, which means that TECS gets the
823 : rangefinder information and thus can know when the flare is
824 : coming.
825 : */
826 : float hgt_afe;
827 :
828 313893 : if (flight_stage == AP_FixedWing::FlightStage::LAND) {
829 :
830 : #if AP_MAVLINK_MAV_CMD_SET_HAGL_ENABLED
831 : // if external HAGL is active use that
832 36106 : if (get_external_HAGL(hgt_afe)) {
833 0 : return hgt_afe;
834 : }
835 : #endif
836 :
837 36106 : hgt_afe = height_above_target();
838 : #if AP_RANGEFINDER_ENABLED
839 36106 : hgt_afe -= rangefinder_correction();
840 : #endif
841 : } else {
842 : // when in normal flight we pass the hgt_afe as relative
843 : // altitude to home
844 277787 : hgt_afe = relative_altitude;
845 : }
846 313893 : return hgt_afe;
847 : }
848 :
849 : // vehicle specific waypoint info helpers
850 54357 : bool Plane::get_wp_distance_m(float &distance) const
851 : {
852 : // see GCS_MAVLINK_Plane::send_nav_controller_output()
853 54357 : if (control_mode == &mode_manual) {
854 7563 : return false;
855 : }
856 : #if HAL_QUADPLANE_ENABLED
857 46794 : if (quadplane.in_vtol_mode()) {
858 9146 : distance = quadplane.using_wp_nav() ? quadplane.wp_nav->get_wp_distance_to_destination_m() : 0;
859 9146 : return true;
860 : }
861 : #endif
862 37648 : distance = auto_state.wp_distance;
863 37648 : return true;
864 : }
865 :
866 58161 : bool Plane::get_wp_bearing_deg(float &bearing) const
867 : {
868 : // see GCS_MAVLINK_Plane::send_nav_controller_output()
869 58161 : if (control_mode == &mode_manual) {
870 0 : return false;
871 : }
872 : #if HAL_QUADPLANE_ENABLED
873 58161 : if (quadplane.in_vtol_mode()) {
874 11960 : bearing = quadplane.using_wp_nav() ? quadplane.wp_nav->get_wp_bearing_to_destination_cd() : 0;
875 11960 : return true;
876 : }
877 : #endif
878 46201 : bearing = nav_controller->target_bearing_cd() * 0.01;
879 46201 : return true;
880 : }
881 :
882 46794 : bool Plane::get_wp_crosstrack_error_m(float &xtrack_error) const
883 : {
884 : // see GCS_MAVLINK_Plane::send_nav_controller_output()
885 46794 : if (control_mode == &mode_manual) {
886 0 : return false;
887 : }
888 : #if HAL_QUADPLANE_ENABLED
889 46794 : if (quadplane.in_vtol_mode()) {
890 9146 : xtrack_error = quadplane.using_wp_nav() ? quadplane.wp_nav->crosstrack_error_m() : 0;
891 9146 : return true;
892 : }
893 : #endif
894 37648 : xtrack_error = nav_controller->crosstrack_error_m();
895 37648 : return true;
896 : }
897 :
898 : #if AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED
899 : // set target location (for use by external control and scripting)
900 0 : bool Plane::set_target_location(const Location &target_loc)
901 : {
902 0 : Location loc{target_loc};
903 0 : fix_terrain_WP(loc, __AP_LINE__);
904 :
905 0 : if (plane.control_mode != &plane.mode_guided) {
906 : // only accept position updates when in GUIDED mode
907 0 : return false;
908 : }
909 : // add home alt if needed
910 0 : if (!loc.terrain_alt) {
911 0 : loc.change_alt_frame(Location::AltFrame::ABSOLUTE);
912 : }
913 0 : plane.set_guided_WP(loc);
914 0 : return true;
915 : }
916 : #endif //AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED
917 :
918 : #if AP_SCRIPTING_ENABLED
919 : // get target location (for use by scripting)
920 25262 : bool Plane::get_target_location(Location& target_loc)
921 : {
922 25262 : switch (control_mode->mode_number()) {
923 25032 : case Mode::Number::RTL:
924 : case Mode::Number::AVOID_ADSB:
925 : case Mode::Number::GUIDED:
926 : case Mode::Number::AUTO:
927 : case Mode::Number::LOITER:
928 : case Mode::Number::TAKEOFF:
929 : #if HAL_QUADPLANE_ENABLED
930 : case Mode::Number::QLOITER:
931 : case Mode::Number::QLAND:
932 : case Mode::Number::QRTL:
933 : #endif
934 25032 : target_loc = next_WP_loc;
935 25032 : return true;
936 : break;
937 230 : default:
938 230 : break;
939 : }
940 230 : return false;
941 : }
942 :
943 : /*
944 : update_target_location() works in all auto navigation modes
945 : */
946 3551 : bool Plane::update_target_location(const Location &old_loc, const Location &new_loc)
947 : {
948 : /*
949 : by checking the caller has provided the correct old target
950 : location we prevent a race condition where the user changes mode
951 : or commands a different target in the controlling lua script
952 : */
953 7102 : if (!old_loc.same_loc_as(next_WP_loc) ||
954 3551 : old_loc.get_alt_frame() != new_loc.get_alt_frame()) {
955 0 : return false;
956 : }
957 3551 : next_WP_loc = new_loc;
958 :
959 3551 : fix_terrain_WP(next_WP_loc, __AP_LINE__);
960 :
961 : #if HAL_QUADPLANE_ENABLED
962 3551 : if (control_mode == &mode_qland || control_mode == &mode_qloiter) {
963 0 : mode_qloiter.last_target_loc_set_ms = AP_HAL::millis();
964 : }
965 : #endif
966 :
967 3551 : return true;
968 : }
969 :
970 : // allow for velocity matching in VTOL
971 1745 : bool Plane::set_velocity_match(const Vector2f &velocity_ne_ms)
972 : {
973 : #if HAL_QUADPLANE_ENABLED
974 1745 : if (quadplane.in_vtol_mode() || quadplane.in_vtol_land_sequence()) {
975 1745 : quadplane.poscontrol.velocity_match_ms = velocity_ne_ms;
976 1745 : quadplane.poscontrol.last_velocity_match_ms = AP_HAL::millis();
977 1745 : return true;
978 : }
979 : #endif
980 0 : return false;
981 : }
982 :
983 : // allow for override of land descent rate
984 67 : bool Plane::set_land_descent_rate(float descent_rate_ms)
985 : {
986 : #if HAL_QUADPLANE_ENABLED
987 67 : if (quadplane.in_vtol_land_descent() ||
988 0 : control_mode == &mode_qland) {
989 67 : quadplane.poscontrol.override_descent_rate_ms = descent_rate_ms;
990 67 : quadplane.poscontrol.last_override_descent_ms = AP_HAL::millis();
991 67 : return true;
992 : }
993 : #endif
994 0 : return false;
995 : }
996 :
997 : // Allow for scripting to have control over the crosstracking when exiting and resuming missions or guided flight
998 : // It's up to the Lua script to ensure the provided location makes sense
999 18 : bool Plane::set_crosstrack_start(const Location &new_start_location)
1000 : {
1001 18 : prev_WP_loc = new_start_location;
1002 18 : auto_state.crosstrack = true;
1003 18 : return true;
1004 : }
1005 :
1006 : #endif // AP_SCRIPTING_ENABLED
1007 :
1008 : // returns true if vehicle is landing.
1009 7270 : bool Plane::is_landing() const
1010 : {
1011 : #if HAL_QUADPLANE_ENABLED
1012 7270 : if (plane.quadplane.in_vtol_land_descent()) {
1013 1392 : return true;
1014 : }
1015 : #endif
1016 5878 : return control_mode->is_landing();
1017 : }
1018 :
1019 : // returns true if vehicle is taking off.
1020 7683 : bool Plane::is_taking_off() const
1021 : {
1022 : #if HAL_QUADPLANE_ENABLED
1023 7683 : if (plane.quadplane.in_vtol_takeoff()) {
1024 949 : return true;
1025 : }
1026 : #endif
1027 6734 : return control_mode->is_taking_off();
1028 : }
1029 :
1030 : #if HAL_QUADPLANE_ENABLED
1031 0 : bool Plane::start_takeoff(const float alt_m) {
1032 0 : return plane.quadplane.available() && quadplane.do_user_takeoff(alt_m);
1033 : }
1034 : #endif
1035 :
1036 : // correct AHRS pitch for PTCH_TRIM_DEG in non-VTOL modes, and return VTOL view in VTOL
1037 150 : void Plane::get_osd_attitude_rad(float &roll, float &pitch, float &yaw)
1038 : {
1039 : // Take semaphore as this can be called from a thread
1040 150 : WITH_SEMAPHORE(ahrs.get_semaphore());
1041 :
1042 : #if HAL_QUADPLANE_ENABLED
1043 150 : if (quadplane.show_vtol_view()) {
1044 0 : pitch = quadplane.ahrs_view->get_pitch_rad();
1045 0 : roll = quadplane.ahrs_view->get_roll_rad();
1046 0 : yaw = quadplane.ahrs_view->get_yaw_rad();
1047 0 : return;
1048 : }
1049 : #endif
1050 :
1051 150 : pitch = ahrs.get_pitch_rad();
1052 150 : roll = ahrs.get_roll_rad();
1053 150 : if (!(flight_option_enabled(FlightOptions::OSD_REMOVE_TRIM_PITCH))) { // correct for PTCH_TRIM_DEG
1054 150 : pitch -= g.pitch_trim * DEG_TO_RAD;
1055 : }
1056 150 : yaw = ahrs.get_yaw_rad();
1057 150 : }
1058 :
1059 : /*
1060 : update current_loc Location
1061 : */
1062 2739278 : void Plane::update_current_loc(void)
1063 : {
1064 2739278 : have_position = plane.ahrs.get_location(plane.current_loc);
1065 :
1066 : // re-calculate relative altitude
1067 2739278 : ahrs.get_relative_position_D_home(plane.relative_altitude);
1068 2739278 : relative_altitude *= -1.0f;
1069 2739278 : }
1070 :
1071 : // check if FLIGHT_OPTION is enabled
1072 38120554 : bool Plane::flight_option_enabled(FlightOptions flight_option) const
1073 : {
1074 38120554 : return g2.flight_options & flight_option;
1075 : }
1076 :
1077 : #if AC_PRECLAND_ENABLED
1078 8376953 : void Plane::precland_update(void)
1079 : {
1080 : // alt will be unused if we pass false through as the second parameter:
1081 : #if AP_RANGEFINDER_ENABLED
1082 8376953 : return g2.precland.update(rangefinder_state.height_estimate*100, rangefinder_state.in_range);
1083 : #else
1084 : return g2.precland.update(0, false);
1085 : #endif
1086 : }
1087 : #endif
1088 :
1089 : #if AP_QUICKTUNE_ENABLED
1090 : /*
1091 : update AP_Quicktune object. We pass the supports_quicktune() method
1092 : in so that quicktune can detect if the user changes to a
1093 : non-quicktune capable mode while tuning and the gains can be reverted
1094 : */
1095 2566790 : void Plane::update_quicktune(void)
1096 : {
1097 2566790 : quicktune.update(control_mode->supports_quicktune());
1098 2566790 : }
1099 : #endif
1100 :
1101 : /*
1102 : constructor for main Plane class
1103 : */
1104 692 : Plane::Plane(void)
1105 : {
1106 : // C++11 doesn't allow in-class initialisation of bitfields
1107 692 : auto_state.takeoff_complete = true;
1108 692 : }
1109 :
1110 : Plane plane;
1111 : AP_Vehicle& vehicle = plane;
1112 :
1113 692 : AP_HAL_MAIN_CALLBACKS(&plane);
|
