diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp
deleted file mode 100644
index 1f6d0c6159502..0000000000000
--- a/ArduPlane/ArduPlane.cpp
+++ /dev/null
@@ -1,1052 +0,0 @@
-/*
- Lead developer: Andrew Tridgell
-
- 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
- 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
-
- Please contribute your ideas! See https://ardupilot.org/dev for details
-
- This program is free software: you can redistribute it and/or modify
- it under the terms of the GNU General Public License as published by
- the Free Software Foundation, either version 3 of the License, or
- (at your option) any later version.
-
- This program is distributed in the hope that it will be useful,
- but WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- GNU General Public License for more details.
-
- You should have received a copy of the GNU General Public License
- along with this program. If not, see .
- */
-
-#include "Plane.h"
-
-#define SCHED_TASK(func, rate_hz, max_time_micros, priority) SCHED_TASK_CLASS(Plane, &plane, func, rate_hz, max_time_micros, priority)
-#define FAST_TASK(func) FAST_TASK_CLASS(Plane, &plane, func)
-
-
-/*
- scheduler table - all regular tasks should be listed here.
-
- All entries in this table must be ordered by priority.
-
- This table is interleaved with the table present in each of the
- vehicles to determine the order in which tasks are run. Convenience
- methods SCHED_TASK and SCHED_TASK_CLASS are provided to build
- entries in this structure:
-
-SCHED_TASK arguments:
- - name of static function to call
- - rate (in Hertz) at which the function should be called
- - expected time (in MicroSeconds) that the function should take to run
- - priority (0 through 255, lower number meaning higher priority)
-
-SCHED_TASK_CLASS arguments:
- - class name of method to be called
- - instance on which to call the method
- - method to call on that instance
- - rate (in Hertz) at which the method should be called
- - expected time (in MicroSeconds) that the method should take to run
- - priority (0 through 255, lower number meaning higher priority)
-
-FAST_TASK entries are run on every loop even if that means the loop
-overruns its allotted time
- */
-const AP_Scheduler::Task Plane::scheduler_tasks[] = {
- // Units: Hz us
- FAST_TASK(ahrs_update),
- FAST_TASK(update_control_mode),
- FAST_TASK(stabilize),
- FAST_TASK(set_servos),
- SCHED_TASK(read_radio, 50, 100, 6),
- SCHED_TASK(check_short_failsafe, 50, 100, 9),
- SCHED_TASK(update_speed_height, 50, 200, 12),
- SCHED_TASK(update_throttle_hover, 100, 90, 24),
- SCHED_TASK_CLASS(RC_Channels, (RC_Channels*)&plane.g2.rc_channels, read_mode_switch, 7, 100, 27),
- SCHED_TASK(update_GPS_50Hz, 50, 300, 30),
- SCHED_TASK(update_GPS_10Hz, 10, 400, 33),
- SCHED_TASK(navigate, 10, 150, 36),
- SCHED_TASK(update_compass, 10, 200, 39),
- SCHED_TASK(calc_airspeed_errors, 10, 100, 42),
- SCHED_TASK(update_alt, 10, 200, 45),
- SCHED_TASK(adjust_altitude_target, 10, 200, 48),
-#if AP_ADVANCEDFAILSAFE_ENABLED
- SCHED_TASK(afs_fs_check, 10, 100, 51),
-#endif
- SCHED_TASK(ekf_check, 10, 75, 54),
- SCHED_TASK_CLASS(GCS, (GCS*)&plane._gcs, update_receive, 300, 500, 57),
- SCHED_TASK_CLASS(GCS, (GCS*)&plane._gcs, update_send, 300, 750, 60),
-#if AP_SERVORELAYEVENTS_ENABLED
- SCHED_TASK_CLASS(AP_ServoRelayEvents, &plane.ServoRelayEvents, update_events, 50, 150, 63),
-#endif
- SCHED_TASK_CLASS(AP_BattMonitor, &plane.battery, read, 10, 300, 66),
-#if AP_RANGEFINDER_ENABLED
- SCHED_TASK(read_rangefinder, 50, 100, 78),
-#endif
-#if AP_ICENGINE_ENABLED
- SCHED_TASK_CLASS(AP_ICEngine, &plane.g2.ice_control, update, 10, 100, 81),
-#endif
-#if AP_OPTICALFLOW_ENABLED
- SCHED_TASK_CLASS(AP_OpticalFlow, &plane.optflow, update, 50, 50, 87),
-#endif
- SCHED_TASK(one_second_loop, 1, 400, 90),
- SCHED_TASK(three_hz_loop, 3, 75, 93),
- SCHED_TASK(check_long_failsafe, 3, 400, 96),
-#if AP_RPM_ENABLED
- SCHED_TASK_CLASS(AP_RPM, &plane.rpm_sensor, update, 10, 100, 99),
-#endif
-#if AP_AIRSPEED_AUTOCAL_ENABLE
- SCHED_TASK(airspeed_ratio_update, 1, 100, 102),
-#endif // AP_AIRSPEED_AUTOCAL_ENABLE
-#if HAL_MOUNT_ENABLED
- SCHED_TASK_CLASS(AP_Mount, &plane.camera_mount, update, 50, 100, 105),
-#endif // HAL_MOUNT_ENABLED
-#if AP_CAMERA_ENABLED
- SCHED_TASK_CLASS(AP_Camera, &plane.camera, update, 50, 100, 108),
-#endif // CAMERA == ENABLED
-#if HAL_LOGGING_ENABLED
- SCHED_TASK_CLASS(AP_Scheduler, &plane.scheduler, update_logging, 0.2, 100, 111),
-#endif
- SCHED_TASK(compass_save, 0.1, 200, 114),
-#if HAL_LOGGING_ENABLED
- SCHED_TASK(Log_Write_FullRate, 400, 300, 117),
- SCHED_TASK(update_logging10, 10, 300, 120),
- SCHED_TASK(update_logging25, 25, 300, 123),
-#endif
-#if HAL_SOARING_ENABLED
- SCHED_TASK(update_soaring, 50, 400, 126),
-#endif
- SCHED_TASK(parachute_check, 10, 200, 129),
-#if AP_TERRAIN_AVAILABLE
- SCHED_TASK_CLASS(AP_Terrain, &plane.terrain, update, 10, 200, 132),
-#endif // AP_TERRAIN_AVAILABLE
- SCHED_TASK(update_is_flying_5Hz, 5, 100, 135),
-#if HAL_LOGGING_ENABLED
- SCHED_TASK_CLASS(AP_Logger, &plane.logger, periodic_tasks, 50, 400, 138),
-#endif
- SCHED_TASK_CLASS(AP_InertialSensor, &plane.ins, periodic, 50, 50, 141),
-#if HAL_ADSB_ENABLED
- SCHED_TASK(avoidance_adsb_update, 10, 100, 144),
-#endif
- SCHED_TASK_CLASS(RC_Channels, (RC_Channels*)&plane.g2.rc_channels, read_aux_all, 10, 200, 147),
-#if HAL_BUTTON_ENABLED
- SCHED_TASK_CLASS(AP_Button, &plane.button, update, 5, 100, 150),
-#endif
-#if AP_LANDINGGEAR_ENABLED
- SCHED_TASK(landing_gear_update, 5, 50, 159),
-#endif
-#if AC_PRECLAND_ENABLED
- SCHED_TASK(precland_update, 400, 50, 160),
-#endif
-#if AP_QUICKTUNE_ENABLED
- SCHED_TASK(update_quicktune, 40, 100, 163),
-#endif
-};
-
-void Plane::get_scheduler_tasks(const AP_Scheduler::Task *&tasks,
- uint8_t &task_count,
- uint32_t &log_bit)
-{
- tasks = &scheduler_tasks[0];
- task_count = ARRAY_SIZE(scheduler_tasks);
- log_bit = MASK_LOG_PM;
-}
-
-#if HAL_QUADPLANE_ENABLED
-constexpr int8_t Plane::_failsafe_priorities[7];
-#else
-constexpr int8_t Plane::_failsafe_priorities[6];
-#endif
-
-// update AHRS system
-void Plane::ahrs_update()
-{
- arming.update_soft_armed();
-
- ahrs.update();
-
-#if HAL_LOGGING_ENABLED
- if (should_log(MASK_LOG_IMU)) {
- AP::ins().Write_IMU();
- }
-#endif
-
- // calculate a scaled roll limit based on current pitch
- roll_limit_cd = aparm.roll_limit*100;
- pitch_limit_min = aparm.pitch_limit_min;
-
- bool rotate_limits = true;
-#if HAL_QUADPLANE_ENABLED
- if (quadplane.tailsitter.active()) {
- rotate_limits = false;
- }
-#endif
- if (rotate_limits) {
- roll_limit_cd *= ahrs.cos_pitch();
- pitch_limit_min *= fabsf(ahrs.cos_roll());
- }
-
- // updated the summed gyro used for ground steering and
- // auto-takeoff. Dot product of DCM.c with gyro vector gives earth
- // frame yaw rate
- steer_state.locked_course_err += ahrs.get_yaw_rate_earth() * G_Dt;
- steer_state.locked_course_err = wrap_PI(steer_state.locked_course_err);
-
-#if HAL_QUADPLANE_ENABLED
- // check if we have had a yaw reset from the EKF
- quadplane.check_yaw_reset();
-
- // update inertial_nav for quadplane
- quadplane.inertial_nav.update();
-#endif
-
-#if HAL_LOGGING_ENABLED
- if (should_log(MASK_LOG_VIDEO_STABILISATION)) {
- ahrs.write_video_stabilisation();
- }
-#endif
-}
-
-/*
- update 50Hz speed/height controller
- */
-void Plane::update_speed_height(void)
-{
- bool should_run_tecs = control_mode->does_auto_throttle();
-#if HAL_QUADPLANE_ENABLED
- if (quadplane.should_disable_TECS()) {
- should_run_tecs = false;
- }
-#endif
-
- if (auto_state.idle_mode) {
- should_run_tecs = false;
- }
-
-#if AP_PLANE_GLIDER_PULLUP_ENABLED
- if (mode_auto.in_pullup()) {
- should_run_tecs = false;
- }
-#endif
-
- if (should_run_tecs) {
- // Call TECS 50Hz update. Note that we call this regardless of
- // throttle suppressed, as this needs to be running for
- // takeoff detection
- TECS_controller.update_50hz();
- }
-
-#if HAL_QUADPLANE_ENABLED
- if (quadplane.in_vtol_mode() ||
- quadplane.in_assisted_flight()) {
- quadplane.update_throttle_mix();
- }
-#endif
-}
-
-
-/*
- read and update compass
- */
-void Plane::update_compass(void)
-{
- compass.read();
-}
-
-#if HAL_LOGGING_ENABLED
-/*
- do 10Hz logging
- */
-void Plane::update_logging10(void)
-{
- bool log_faster = (should_log(MASK_LOG_ATTITUDE_FULLRATE) || should_log(MASK_LOG_ATTITUDE_FAST));
- if (should_log(MASK_LOG_ATTITUDE_MED) && !log_faster) {
- Log_Write_Attitude();
- ahrs.Write_AOA_SSA();
- } else if (log_faster) {
- ahrs.Write_AOA_SSA();
- }
-#if HAL_MOUNT_ENABLED
- if (should_log(MASK_LOG_CAMERA)) {
- camera_mount.write_log();
- }
-#endif
-}
-
-/*
- do 25Hz logging
- */
-void Plane::update_logging25(void)
-{
- // MASK_LOG_ATTITUDE_FULLRATE logs at 400Hz, MASK_LOG_ATTITUDE_FAST at 25Hz, MASK_LOG_ATTIUDE_MED logs at 10Hz
- // highest rate selected wins
- bool log_faster = should_log(MASK_LOG_ATTITUDE_FULLRATE);
- if (should_log(MASK_LOG_ATTITUDE_FAST) && !log_faster) {
- Log_Write_Attitude();
- }
-
- if (should_log(MASK_LOG_CTUN)) {
- Log_Write_Control_Tuning();
-#if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
- if (!should_log(MASK_LOG_NOTCH_FULLRATE)) {
- AP::ins().write_notch_log_messages();
- }
-#endif
-#if HAL_GYROFFT_ENABLED
- gyro_fft.write_log_messages();
-#endif
- }
-
- if (should_log(MASK_LOG_NTUN)) {
- Log_Write_Nav_Tuning();
- Log_Write_Guided();
- }
-
- if (should_log(MASK_LOG_RC))
- Log_Write_RC();
-
- if (should_log(MASK_LOG_IMU))
- AP::ins().Write_Vibration();
-}
-#endif // HAL_LOGGING_ENABLED
-
-/*
- check for AFS failsafe check
- */
-#if AP_ADVANCEDFAILSAFE_ENABLED
-void Plane::afs_fs_check(void)
-{
- afs.check(failsafe.AFS_last_valid_rc_ms);
-}
-#endif
-
-#if HAL_WITH_IO_MCU
-#include
-extern AP_IOMCU iomcu;
-#endif
-
-void Plane::one_second_loop()
-{
- // make it possible to change control channel ordering at runtime
- set_control_channels();
-
-#if HAL_WITH_IO_MCU
- iomcu.setup_mixing(g.override_channel.get(), g.mixing_gain, g2.manual_rc_mask);
-#endif
-
-#if HAL_ADSB_ENABLED
- adsb.set_stall_speed_cm(aparm.airspeed_min * 100); // convert m/s to cm/s
- adsb.set_max_speed(aparm.airspeed_max);
-#endif
-
- if (flight_option_enabled(FlightOptions::ENABLE_DEFAULT_AIRSPEED)) {
- // use average of min and max airspeed as default airspeed fusion with high variance
- ahrs.writeDefaultAirSpeed((float)((aparm.airspeed_min + aparm.airspeed_max)/2),
- (float)((aparm.airspeed_max - aparm.airspeed_min)/2));
- }
-
- // sync MAVLink system ID
- mavlink_system.sysid = g.sysid_this_mav;
-
- AP::srv().enable_aux_servos();
-
- // update notify flags
- AP_Notify::flags.pre_arm_check = arming.pre_arm_checks(false);
- AP_Notify::flags.pre_arm_gps_check = true;
- AP_Notify::flags.armed = arming.is_armed() || arming.arming_required() == AP_Arming::Required::NO;
-
-#if AP_TERRAIN_AVAILABLE && HAL_LOGGING_ENABLED
- if (should_log(MASK_LOG_GPS)) {
- terrain.log_terrain_data();
- }
-#endif
-
- // update home position if NOT armed and gps position has
- // changed. Update every 5s at most
- if (!arming.is_armed() &&
- gps.last_message_time_ms() - last_home_update_ms > 5000 &&
- gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
- last_home_update_ms = gps.last_message_time_ms();
- update_home();
-
- // reset the landing altitude correction
- landing.alt_offset = 0;
- }
-
- // this ensures G_Dt is correct, catching startup issues with constructors
- // calling the scheduler methods
- if (!is_equal(1.0f/scheduler.get_loop_rate_hz(), scheduler.get_loop_period_s()) ||
- !is_equal(G_Dt, scheduler.get_loop_period_s())) {
- INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
- }
-
- const float loop_rate = AP::scheduler().get_filtered_loop_rate_hz();
-#if HAL_QUADPLANE_ENABLED
- if (quadplane.available()) {
- quadplane.attitude_control->set_notch_sample_rate(loop_rate);
- }
-#endif
- rollController.set_notch_sample_rate(loop_rate);
- pitchController.set_notch_sample_rate(loop_rate);
- yawController.set_notch_sample_rate(loop_rate);
-}
-
-void Plane::three_hz_loop()
-{
-#if AP_FENCE_ENABLED
- fence_check();
-#endif
-}
-
-void Plane::compass_save()
-{
- if (AP::compass().available() &&
- compass.get_learn_type() >= Compass::LEARN_INTERNAL &&
- !hal.util->get_soft_armed()) {
- /*
- only save offsets when disarmed
- */
- compass.save_offsets();
- }
-}
-
-#if AP_AIRSPEED_AUTOCAL_ENABLE
-/*
- once a second update the airspeed calibration ratio
- */
-void Plane::airspeed_ratio_update(void)
-{
- if (!airspeed.enabled() ||
- gps.status() < AP_GPS::GPS_OK_FIX_3D ||
- gps.ground_speed() < 4) {
- // don't calibrate when not moving
- return;
- }
- if (airspeed.get_airspeed() < aparm.airspeed_min &&
- gps.ground_speed() < (uint32_t)aparm.airspeed_min) {
- // don't calibrate when flying below the minimum airspeed. We
- // check both airspeed and ground speed to catch cases where
- // the airspeed ratio is way too low, which could lead to it
- // never coming up again
- return;
- }
- if (labs(ahrs.roll_sensor) > roll_limit_cd ||
- ahrs.pitch_sensor > aparm.pitch_limit_max*100 ||
- ahrs.pitch_sensor < pitch_limit_min*100) {
- // don't calibrate when going beyond normal flight envelope
- return;
- }
- const Vector3f &vg = gps.velocity();
- airspeed.update_calibration(vg, aparm.airspeed_max);
-}
-#endif // AP_AIRSPEED_AUTOCAL_ENABLE
-
-/*
- read the GPS and update position
- */
-void Plane::update_GPS_50Hz(void)
-{
- gps.update();
-
- update_current_loc();
-}
-
-/*
- read update GPS position - 10Hz update
- */
-void Plane::update_GPS_10Hz(void)
-{
- static uint32_t last_gps_msg_ms;
- if (gps.last_message_time_ms() != last_gps_msg_ms && gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
- last_gps_msg_ms = gps.last_message_time_ms();
-
- if (ground_start_count > 1) {
- ground_start_count--;
- } else if (ground_start_count == 1) {
- // We countdown N number of good GPS fixes
- // so that the altitude is more accurate
- // -------------------------------------
- if (current_loc.lat == 0 && current_loc.lng == 0) {
- ground_start_count = 5;
-
- } else if (!hal.util->was_watchdog_reset()) {
- if (!set_home_persistently(gps.location())) {
- // silently ignore failure...
- }
-
- next_WP_loc = prev_WP_loc = home;
-
- ground_start_count = 0;
- }
- }
-
- // update wind estimate
- ahrs.estimate_wind();
- } else if (gps.status() < AP_GPS::GPS_OK_FIX_3D && ground_start_count != 0) {
- // lost 3D fix, start again
- ground_start_count = 5;
- }
-
- calc_gndspeed_undershoot();
-}
-
-/*
- main control mode dependent update code
- */
-void Plane::update_control_mode(void)
-{
- if ((control_mode != &mode_auto) && (control_mode != &mode_takeoff)) {
- // hold_course is only used in takeoff and landing
- steer_state.hold_course_cd = -1;
- }
- // refresh the throttle limits, to avoid using stale values
- // they will be updated once takeoff_calc_throttle is called
- takeoff_state.throttle_lim_max = 100.0f;
- takeoff_state.throttle_lim_min = -100.0f;
-
- update_fly_forward();
-
- control_mode->update();
-}
-
-
-void Plane::update_fly_forward(void)
-{
- // ensure we are fly-forward when we are flying as a pure fixed
- // wing aircraft. This helps the EKF produce better state
- // estimates as it can make stronger assumptions
-#if HAL_QUADPLANE_ENABLED
- if (quadplane.available()) {
- if (quadplane.tailsitter.is_in_fw_flight()) {
- ahrs.set_fly_forward(true);
- return;
- }
-
- if (quadplane.in_vtol_mode()) {
- ahrs.set_fly_forward(false);
- return;
- }
-
- if (quadplane.in_assisted_flight()) {
- ahrs.set_fly_forward(false);
- return;
- }
- }
-#endif
-
- if (auto_state.idle_mode) {
- // don't fuse airspeed when in balloon lift
- ahrs.set_fly_forward(false);
- return;
- }
-
- if (flight_stage == AP_FixedWing::FlightStage::LAND) {
- ahrs.set_fly_forward(landing.is_flying_forward());
- return;
- }
-
- ahrs.set_fly_forward(true);
-}
-
-/*
- set the flight stage
- */
-void Plane::set_flight_stage(AP_FixedWing::FlightStage fs)
-{
- if (fs == flight_stage) {
- return;
- }
-
- landing.handle_flight_stage_change(fs == AP_FixedWing::FlightStage::LAND);
-
- if (fs == AP_FixedWing::FlightStage::ABORT_LANDING) {
- gcs().send_text(MAV_SEVERITY_NOTICE, "Landing aborted, climbing to %dm",
- int(auto_state.takeoff_altitude_rel_cm/100));
- }
-
- flight_stage = fs;
-#if HAL_LOGGING_ENABLED
- Log_Write_Status();
-#endif
-}
-
-void Plane::update_alt()
-{
- barometer.update();
-
- // calculate the sink rate.
- float sink_rate;
- Vector3f vel;
- if (ahrs.get_velocity_NED(vel)) {
- sink_rate = vel.z;
- } else if (gps.status() >= AP_GPS::GPS_OK_FIX_3D && gps.have_vertical_velocity()) {
- sink_rate = gps.velocity().z;
- } else {
- sink_rate = -barometer.get_climb_rate();
- }
-
- // low pass the sink rate to take some of the noise out
- auto_state.sink_rate = 0.8f * auto_state.sink_rate + 0.2f*sink_rate;
-#if HAL_PARACHUTE_ENABLED
- parachute.set_sink_rate(auto_state.sink_rate);
-#endif
-
- update_flight_stage();
-
-#if AP_SCRIPTING_ENABLED
- if (nav_scripting_active()) {
- // don't call TECS while we are in a trick
- return;
- }
-#endif
-
- bool should_run_tecs = control_mode->does_auto_throttle();
-#if HAL_QUADPLANE_ENABLED
- if (quadplane.should_disable_TECS()) {
- should_run_tecs = false;
- }
-#endif
-
- if (auto_state.idle_mode) {
- should_run_tecs = false;
- }
-
-#if AP_PLANE_GLIDER_PULLUP_ENABLED
- if (mode_auto.in_pullup()) {
- should_run_tecs = false;
- }
-#endif
-
- if (should_run_tecs && !throttle_suppressed) {
-
- float distance_beyond_land_wp = 0;
- if (flight_stage == AP_FixedWing::FlightStage::LAND &&
- current_loc.past_interval_finish_line(prev_WP_loc, next_WP_loc)) {
- distance_beyond_land_wp = current_loc.get_distance(next_WP_loc);
- }
-
- tecs_target_alt_cm = relative_target_altitude_cm();
-
- if (control_mode == &mode_rtl && !rtl.done_climb && (g2.rtl_climb_min > 0 || (plane.flight_option_enabled(FlightOptions::CLIMB_BEFORE_TURN)))) {
- // ensure we do the initial climb in RTL. We add an extra
- // 10m in the demanded height to push TECS to climb
- // quickly
- tecs_target_alt_cm = MAX(tecs_target_alt_cm, prev_WP_loc.alt - home.alt) + (g2.rtl_climb_min+10)*100;
- }
-
- TECS_controller.update_pitch_throttle(tecs_target_alt_cm,
- target_airspeed_cm,
- flight_stage,
- distance_beyond_land_wp,
- get_takeoff_pitch_min_cd(),
- throttle_nudge,
- tecs_hgt_afe(),
- aerodynamic_load_factor,
- g.pitch_trim.get());
- }
-}
-
-/*
- recalculate the flight_stage
- */
-void Plane::update_flight_stage(void)
-{
- // Update the speed & height controller states
- if (control_mode->does_auto_throttle() && !throttle_suppressed) {
- if (control_mode == &mode_auto) {
-#if HAL_QUADPLANE_ENABLED
- if (quadplane.in_vtol_auto()) {
- set_flight_stage(AP_FixedWing::FlightStage::VTOL);
- return;
- }
-#endif
- if (auto_state.takeoff_complete == false) {
- set_flight_stage(AP_FixedWing::FlightStage::TAKEOFF);
- return;
- } else if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND) {
- if (landing.is_commanded_go_around() || flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING) {
- // abort mode is sticky, it must complete while executing NAV_LAND
- set_flight_stage(AP_FixedWing::FlightStage::ABORT_LANDING);
- } else if (landing.get_abort_throttle_enable() && get_throttle_input() >= 90 &&
- landing.request_go_around()) {
- gcs().send_text(MAV_SEVERITY_INFO,"Landing aborted via throttle");
- set_flight_stage(AP_FixedWing::FlightStage::ABORT_LANDING);
- } else {
- set_flight_stage(AP_FixedWing::FlightStage::LAND);
- }
- return;
- }
-#if HAL_QUADPLANE_ENABLED
- if (quadplane.in_assisted_flight()) {
- set_flight_stage(AP_FixedWing::FlightStage::VTOL);
- return;
- }
-#endif
- set_flight_stage(AP_FixedWing::FlightStage::NORMAL);
- } else if (control_mode != &mode_takeoff) {
- // If not in AUTO then assume normal operation for normal TECS operation.
- // This prevents TECS from being stuck in the wrong stage if you switch from
- // AUTO to, say, FBWB during a landing, an aborted landing or takeoff.
- set_flight_stage(AP_FixedWing::FlightStage::NORMAL);
- }
- return;
- }
-#if HAL_QUADPLANE_ENABLED
- if (quadplane.in_vtol_mode() ||
- quadplane.in_assisted_flight()) {
- set_flight_stage(AP_FixedWing::FlightStage::VTOL);
- return;
- }
-#endif
- set_flight_stage(AP_FixedWing::FlightStage::NORMAL);
-}
-
-
-
-
-/*
- If land_DisarmDelay is enabled (non-zero), check for a landing then auto-disarm after time expires
-
- only called from AP_Landing, when the landing library is ready to disarm
- */
-void Plane::disarm_if_autoland_complete()
-{
- if (landing.get_disarm_delay() > 0 &&
- !is_flying() &&
- arming.arming_required() != AP_Arming::Required::NO &&
- arming.is_armed()) {
- /* we have auto disarm enabled. See if enough time has passed */
- if (millis() - auto_state.last_flying_ms >= landing.get_disarm_delay()*1000UL) {
- if (arming.disarm(AP_Arming::Method::AUTOLANDED)) {
- gcs().send_text(MAV_SEVERITY_INFO,"Auto disarmed");
- }
- }
- }
-}
-
-bool Plane::trigger_land_abort(const float climb_to_alt_m)
-{
- if (plane.control_mode != &plane.mode_auto) {
- return false;
- }
-#if HAL_QUADPLANE_ENABLED
- if (plane.quadplane.in_vtol_auto()) {
- return quadplane.abort_landing();
- }
-#endif
-
- uint16_t mission_id = plane.mission.get_current_nav_cmd().id;
- bool is_in_landing = (plane.flight_stage == AP_FixedWing::FlightStage::LAND) ||
- plane.is_land_command(mission_id);
- if (is_in_landing) {
- // fly a user planned abort pattern if available
- if (plane.have_position && plane.mission.jump_to_abort_landing_sequence(plane.current_loc)) {
- return true;
- }
-
- // only fly a fixed wing abort if we aren't doing quadplane stuff, or potentially
- // shooting a quadplane approach
-#if HAL_QUADPLANE_ENABLED
- const bool attempt_go_around =
- (!plane.quadplane.available()) ||
- ((!plane.quadplane.in_vtol_auto()) &&
- (!plane.quadplane.landing_with_fixed_wing_spiral_approach()));
-#else
- const bool attempt_go_around = true;
-#endif
- if (attempt_go_around) {
- // Initiate an aborted landing. This will trigger a pitch-up and
- // climb-out to a safe altitude holding heading then one of the
- // following actions will occur, check for in this order:
- // - If MAV_CMD_CONTINUE_AND_CHANGE_ALT is next command in mission,
- // increment mission index to execute it
- // - else if DO_LAND_START is available, jump to it
- // - else decrement the mission index to repeat the landing approach
-
- if (!is_zero(climb_to_alt_m)) {
- plane.auto_state.takeoff_altitude_rel_cm = climb_to_alt_m * 100;
- }
- if (plane.landing.request_go_around()) {
- plane.auto_state.next_wp_crosstrack = false;
- return true;
- }
- }
- }
- return false;
-}
-
-
-/*
- the height above field elevation that we pass to TECS
- */
-float Plane::tecs_hgt_afe(void)
-{
- /*
- pass the height above field elevation as the height above
- the ground when in landing, which means that TECS gets the
- rangefinder information and thus can know when the flare is
- coming.
- */
- float hgt_afe;
-
- if (flight_stage == AP_FixedWing::FlightStage::LAND) {
-
- #if AP_MAVLINK_MAV_CMD_SET_HAGL_ENABLED
- // if external HAGL is active use that
- if (get_external_HAGL(hgt_afe)) {
- return hgt_afe;
- }
- #endif
-
- hgt_afe = height_above_target();
-#if AP_RANGEFINDER_ENABLED
- hgt_afe -= rangefinder_correction();
-#endif
- } else {
- // when in normal flight we pass the hgt_afe as relative
- // altitude to home
- hgt_afe = relative_altitude;
- }
- return hgt_afe;
-}
-
-// vehicle specific waypoint info helpers
-bool Plane::get_wp_distance_m(float &distance) const
-{
- // see GCS_MAVLINK_Plane::send_nav_controller_output()
- if (control_mode == &mode_manual) {
- return false;
- }
-#if HAL_QUADPLANE_ENABLED
- if (quadplane.in_vtol_mode()) {
- distance = quadplane.using_wp_nav() ? quadplane.wp_nav->get_wp_distance_to_destination() * 0.01 : 0;
- return true;
- }
-#endif
- distance = auto_state.wp_distance;
- return true;
-}
-
-bool Plane::get_wp_bearing_deg(float &bearing) const
-{
- // see GCS_MAVLINK_Plane::send_nav_controller_output()
- if (control_mode == &mode_manual) {
- return false;
- }
-#if HAL_QUADPLANE_ENABLED
- if (quadplane.in_vtol_mode()) {
- bearing = quadplane.using_wp_nav() ? quadplane.wp_nav->get_wp_bearing_to_destination() : 0;
- return true;
- }
-#endif
- bearing = nav_controller->target_bearing_cd() * 0.01;
- return true;
-}
-
-bool Plane::get_wp_crosstrack_error_m(float &xtrack_error) const
-{
- // see GCS_MAVLINK_Plane::send_nav_controller_output()
- if (control_mode == &mode_manual) {
- return false;
- }
-#if HAL_QUADPLANE_ENABLED
- if (quadplane.in_vtol_mode()) {
- xtrack_error = quadplane.using_wp_nav() ? quadplane.wp_nav->crosstrack_error() : 0;
- return true;
- }
-#endif
- xtrack_error = nav_controller->crosstrack_error();
- return true;
-}
-
-#if AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED
-// set target location (for use by external control and scripting)
-bool Plane::set_target_location(const Location &target_loc)
-{
- Location loc{target_loc};
-
- if (plane.control_mode != &plane.mode_guided) {
- // only accept position updates when in GUIDED mode
- return false;
- }
- // add home alt if needed
- if (loc.relative_alt) {
- loc.alt += plane.home.alt;
- loc.relative_alt = 0;
- }
- plane.set_guided_WP(loc);
- return true;
-}
-#endif //AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED
-
-#if AP_SCRIPTING_ENABLED
-// get target location (for use by scripting)
-bool Plane::get_target_location(Location& target_loc)
-{
- switch (control_mode->mode_number()) {
- case Mode::Number::RTL:
- case Mode::Number::AVOID_ADSB:
- case Mode::Number::GUIDED:
- case Mode::Number::AUTO:
- case Mode::Number::LOITER:
- case Mode::Number::TAKEOFF:
-#if HAL_QUADPLANE_ENABLED
- case Mode::Number::QLOITER:
- case Mode::Number::QLAND:
- case Mode::Number::QRTL:
-#endif
- target_loc = next_WP_loc;
- return true;
- break;
- default:
- break;
- }
- return false;
-}
-
-/*
- update_target_location() works in all auto navigation modes
- */
-bool Plane::update_target_location(const Location &old_loc, const Location &new_loc)
-{
- /*
- by checking the caller has provided the correct old target
- location we prevent a race condition where the user changes mode
- or commands a different target in the controlling lua script
- */
- if (!old_loc.same_loc_as(next_WP_loc) ||
- old_loc.get_alt_frame() != new_loc.get_alt_frame()) {
- return false;
- }
- next_WP_loc = new_loc;
-
-#if HAL_QUADPLANE_ENABLED
- if (control_mode == &mode_qland || control_mode == &mode_qloiter) {
- mode_qloiter.last_target_loc_set_ms = AP_HAL::millis();
- }
-#endif
-
- return true;
-}
-
-// allow for velocity matching in VTOL
-bool Plane::set_velocity_match(const Vector2f &velocity)
-{
-#if HAL_QUADPLANE_ENABLED
- if (quadplane.in_vtol_mode() || quadplane.in_vtol_land_sequence()) {
- quadplane.poscontrol.velocity_match = velocity;
- quadplane.poscontrol.last_velocity_match_ms = AP_HAL::millis();
- return true;
- }
-#endif
- return false;
-}
-
-// allow for override of land descent rate
-bool Plane::set_land_descent_rate(float descent_rate)
-{
-#if HAL_QUADPLANE_ENABLED
- if (quadplane.in_vtol_land_descent() ||
- control_mode == &mode_qland) {
- quadplane.poscontrol.override_descent_rate = descent_rate;
- quadplane.poscontrol.last_override_descent_ms = AP_HAL::millis();
- return true;
- }
-#endif
- return false;
-}
-
-// Allow for scripting to have control over the crosstracking when exiting and resuming missions or guided flight
-// It's up to the Lua script to ensure the provided location makes sense
-bool Plane::set_crosstrack_start(const Location &new_start_location)
-{
- prev_WP_loc = new_start_location;
- auto_state.crosstrack = true;
- return true;
-}
-
-#endif // AP_SCRIPTING_ENABLED
-
-// returns true if vehicle is landing.
-bool Plane::is_landing() const
-{
-#if HAL_QUADPLANE_ENABLED
- if (plane.quadplane.in_vtol_land_descent()) {
- return true;
- }
-#endif
- return control_mode->is_landing();
-}
-
-// returns true if vehicle is taking off.
-bool Plane::is_taking_off() const
-{
-#if HAL_QUADPLANE_ENABLED
- if (plane.quadplane.in_vtol_takeoff()) {
- return true;
- }
-#endif
- return control_mode->is_taking_off();
-}
-
-// correct AHRS pitch for PTCH_TRIM_DEG in non-VTOL modes, and return VTOL view in VTOL
-void Plane::get_osd_roll_pitch_rad(float &roll, float &pitch) const
-{
-#if HAL_QUADPLANE_ENABLED
- if (quadplane.show_vtol_view()) {
- pitch = quadplane.ahrs_view->pitch;
- roll = quadplane.ahrs_view->roll;
- return;
- }
-#endif
- pitch = ahrs.get_pitch();
- roll = ahrs.get_roll();
- if (!(flight_option_enabled(FlightOptions::OSD_REMOVE_TRIM_PITCH))) { // correct for PTCH_TRIM_DEG
- pitch -= g.pitch_trim * DEG_TO_RAD;
- }
-}
-
-/*
- update current_loc Location
- */
-void Plane::update_current_loc(void)
-{
- have_position = plane.ahrs.get_location(plane.current_loc);
-
- // re-calculate relative altitude
- ahrs.get_relative_position_D_home(plane.relative_altitude);
- relative_altitude *= -1.0f;
-}
-
-// check if FLIGHT_OPTION is enabled
-bool Plane::flight_option_enabled(FlightOptions flight_option) const
-{
- return g2.flight_options & flight_option;
-}
-
-#if AC_PRECLAND_ENABLED
-void Plane::precland_update(void)
-{
- // alt will be unused if we pass false through as the second parameter:
-#if AP_RANGEFINDER_ENABLED
- return g2.precland.update(rangefinder_state.height_estimate*100, rangefinder_state.in_range);
-#else
- return g2.precland.update(0, false);
-#endif
-}
-#endif
-
-#if AP_QUICKTUNE_ENABLED
-/*
- update AP_Quicktune object. We pass the supports_quicktune() method
- in so that quicktune can detect if the user changes to a
- non-quicktune capable mode while tuning and the gains can be reverted
- */
-void Plane::update_quicktune(void)
-{
- quicktune.update(control_mode->supports_quicktune());
-}
-#endif
-
-AP_HAL_MAIN_CALLBACKS(&plane);
diff --git a/ArduPlane/Plane.cpp b/ArduPlane/Plane.cpp
index 4be3c0df7c60e..1990ca0ed2e78 100644
--- a/ArduPlane/Plane.cpp
+++ b/ArduPlane/Plane.cpp
@@ -1,4 +1,11 @@
/*
+ Lead developer: Andrew Tridgell
+
+ 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
+ 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
+
+ Please contribute your ideas! See https://ardupilot.org/dev for details
+
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
@@ -12,6 +19,7 @@
You should have received a copy of the GNU General Public License
along with this program. If not, see .
*/
+
#include "Plane.h"
#define FORCE_VERSION_H_INCLUDE
@@ -20,6 +28,1033 @@
const AP_HAL::HAL& hal = AP_HAL::get_HAL();
+#define SCHED_TASK(func, rate_hz, max_time_micros, priority) SCHED_TASK_CLASS(Plane, &plane, func, rate_hz, max_time_micros, priority)
+#define FAST_TASK(func) FAST_TASK_CLASS(Plane, &plane, func)
+
+
+/*
+ scheduler table - all regular tasks should be listed here.
+
+ All entries in this table must be ordered by priority.
+
+ This table is interleaved with the table present in each of the
+ vehicles to determine the order in which tasks are run. Convenience
+ methods SCHED_TASK and SCHED_TASK_CLASS are provided to build
+ entries in this structure:
+
+SCHED_TASK arguments:
+ - name of static function to call
+ - rate (in Hertz) at which the function should be called
+ - expected time (in MicroSeconds) that the function should take to run
+ - priority (0 through 255, lower number meaning higher priority)
+
+SCHED_TASK_CLASS arguments:
+ - class name of method to be called
+ - instance on which to call the method
+ - method to call on that instance
+ - rate (in Hertz) at which the method should be called
+ - expected time (in MicroSeconds) that the method should take to run
+ - priority (0 through 255, lower number meaning higher priority)
+
+FAST_TASK entries are run on every loop even if that means the loop
+overruns its allotted time
+ */
+const AP_Scheduler::Task Plane::scheduler_tasks[] = {
+ // Units: Hz us
+ FAST_TASK(ahrs_update),
+ FAST_TASK(update_control_mode),
+ FAST_TASK(stabilize),
+ FAST_TASK(set_servos),
+ SCHED_TASK(read_radio, 50, 100, 6),
+ SCHED_TASK(check_short_failsafe, 50, 100, 9),
+ SCHED_TASK(update_speed_height, 50, 200, 12),
+ SCHED_TASK(update_throttle_hover, 100, 90, 24),
+ SCHED_TASK_CLASS(RC_Channels, (RC_Channels*)&plane.g2.rc_channels, read_mode_switch, 7, 100, 27),
+ SCHED_TASK(update_GPS_50Hz, 50, 300, 30),
+ SCHED_TASK(update_GPS_10Hz, 10, 400, 33),
+ SCHED_TASK(navigate, 10, 150, 36),
+ SCHED_TASK(update_compass, 10, 200, 39),
+ SCHED_TASK(calc_airspeed_errors, 10, 100, 42),
+ SCHED_TASK(update_alt, 10, 200, 45),
+ SCHED_TASK(adjust_altitude_target, 10, 200, 48),
+#if AP_ADVANCEDFAILSAFE_ENABLED
+ SCHED_TASK(afs_fs_check, 10, 100, 51),
+#endif
+ SCHED_TASK(ekf_check, 10, 75, 54),
+ SCHED_TASK_CLASS(GCS, (GCS*)&plane._gcs, update_receive, 300, 500, 57),
+ SCHED_TASK_CLASS(GCS, (GCS*)&plane._gcs, update_send, 300, 750, 60),
+#if AP_SERVORELAYEVENTS_ENABLED
+ SCHED_TASK_CLASS(AP_ServoRelayEvents, &plane.ServoRelayEvents, update_events, 50, 150, 63),
+#endif
+ SCHED_TASK_CLASS(AP_BattMonitor, &plane.battery, read, 10, 300, 66),
+#if AP_RANGEFINDER_ENABLED
+ SCHED_TASK(read_rangefinder, 50, 100, 78),
+#endif
+#if AP_ICENGINE_ENABLED
+ SCHED_TASK_CLASS(AP_ICEngine, &plane.g2.ice_control, update, 10, 100, 81),
+#endif
+#if AP_OPTICALFLOW_ENABLED
+ SCHED_TASK_CLASS(AP_OpticalFlow, &plane.optflow, update, 50, 50, 87),
+#endif
+ SCHED_TASK(one_second_loop, 1, 400, 90),
+ SCHED_TASK(three_hz_loop, 3, 75, 93),
+ SCHED_TASK(check_long_failsafe, 3, 400, 96),
+#if AP_RPM_ENABLED
+ SCHED_TASK_CLASS(AP_RPM, &plane.rpm_sensor, update, 10, 100, 99),
+#endif
+#if AP_AIRSPEED_AUTOCAL_ENABLE
+ SCHED_TASK(airspeed_ratio_update, 1, 100, 102),
+#endif // AP_AIRSPEED_AUTOCAL_ENABLE
+#if HAL_MOUNT_ENABLED
+ SCHED_TASK_CLASS(AP_Mount, &plane.camera_mount, update, 50, 100, 105),
+#endif // HAL_MOUNT_ENABLED
+#if AP_CAMERA_ENABLED
+ SCHED_TASK_CLASS(AP_Camera, &plane.camera, update, 50, 100, 108),
+#endif // CAMERA == ENABLED
+#if HAL_LOGGING_ENABLED
+ SCHED_TASK_CLASS(AP_Scheduler, &plane.scheduler, update_logging, 0.2, 100, 111),
+#endif
+ SCHED_TASK(compass_save, 0.1, 200, 114),
+#if HAL_LOGGING_ENABLED
+ SCHED_TASK(Log_Write_FullRate, 400, 300, 117),
+ SCHED_TASK(update_logging10, 10, 300, 120),
+ SCHED_TASK(update_logging25, 25, 300, 123),
+#endif
+#if HAL_SOARING_ENABLED
+ SCHED_TASK(update_soaring, 50, 400, 126),
+#endif
+ SCHED_TASK(parachute_check, 10, 200, 129),
+#if AP_TERRAIN_AVAILABLE
+ SCHED_TASK_CLASS(AP_Terrain, &plane.terrain, update, 10, 200, 132),
+#endif // AP_TERRAIN_AVAILABLE
+ SCHED_TASK(update_is_flying_5Hz, 5, 100, 135),
+#if HAL_LOGGING_ENABLED
+ SCHED_TASK_CLASS(AP_Logger, &plane.logger, periodic_tasks, 50, 400, 138),
+#endif
+ SCHED_TASK_CLASS(AP_InertialSensor, &plane.ins, periodic, 50, 50, 141),
+#if HAL_ADSB_ENABLED
+ SCHED_TASK(avoidance_adsb_update, 10, 100, 144),
+#endif
+ SCHED_TASK_CLASS(RC_Channels, (RC_Channels*)&plane.g2.rc_channels, read_aux_all, 10, 200, 147),
+#if HAL_BUTTON_ENABLED
+ SCHED_TASK_CLASS(AP_Button, &plane.button, update, 5, 100, 150),
+#endif
+#if AP_LANDINGGEAR_ENABLED
+ SCHED_TASK(landing_gear_update, 5, 50, 159),
+#endif
+#if AC_PRECLAND_ENABLED
+ SCHED_TASK(precland_update, 400, 50, 160),
+#endif
+#if AP_QUICKTUNE_ENABLED
+ SCHED_TASK(update_quicktune, 40, 100, 163),
+#endif
+};
+
+void Plane::get_scheduler_tasks(const AP_Scheduler::Task *&tasks,
+ uint8_t &task_count,
+ uint32_t &log_bit)
+{
+ tasks = &scheduler_tasks[0];
+ task_count = ARRAY_SIZE(scheduler_tasks);
+ log_bit = MASK_LOG_PM;
+}
+
+#if HAL_QUADPLANE_ENABLED
+constexpr int8_t Plane::_failsafe_priorities[7];
+#else
+constexpr int8_t Plane::_failsafe_priorities[6];
+#endif
+
+// update AHRS system
+void Plane::ahrs_update()
+{
+ arming.update_soft_armed();
+
+ ahrs.update();
+
+#if HAL_LOGGING_ENABLED
+ if (should_log(MASK_LOG_IMU)) {
+ AP::ins().Write_IMU();
+ }
+#endif
+
+ // calculate a scaled roll limit based on current pitch
+ roll_limit_cd = aparm.roll_limit*100;
+ pitch_limit_min = aparm.pitch_limit_min;
+
+ bool rotate_limits = true;
+#if HAL_QUADPLANE_ENABLED
+ if (quadplane.tailsitter.active()) {
+ rotate_limits = false;
+ }
+#endif
+ if (rotate_limits) {
+ roll_limit_cd *= ahrs.cos_pitch();
+ pitch_limit_min *= fabsf(ahrs.cos_roll());
+ }
+
+ // updated the summed gyro used for ground steering and
+ // auto-takeoff. Dot product of DCM.c with gyro vector gives earth
+ // frame yaw rate
+ steer_state.locked_course_err += ahrs.get_yaw_rate_earth() * G_Dt;
+ steer_state.locked_course_err = wrap_PI(steer_state.locked_course_err);
+
+#if HAL_QUADPLANE_ENABLED
+ // check if we have had a yaw reset from the EKF
+ quadplane.check_yaw_reset();
+
+ // update inertial_nav for quadplane
+ quadplane.inertial_nav.update();
+#endif
+
+#if HAL_LOGGING_ENABLED
+ if (should_log(MASK_LOG_VIDEO_STABILISATION)) {
+ ahrs.write_video_stabilisation();
+ }
+#endif
+}
+
+/*
+ update 50Hz speed/height controller
+ */
+void Plane::update_speed_height(void)
+{
+ bool should_run_tecs = control_mode->does_auto_throttle();
+#if HAL_QUADPLANE_ENABLED
+ if (quadplane.should_disable_TECS()) {
+ should_run_tecs = false;
+ }
+#endif
+
+ if (auto_state.idle_mode) {
+ should_run_tecs = false;
+ }
+
+#if AP_PLANE_GLIDER_PULLUP_ENABLED
+ if (mode_auto.in_pullup()) {
+ should_run_tecs = false;
+ }
+#endif
+
+ if (should_run_tecs) {
+ // Call TECS 50Hz update. Note that we call this regardless of
+ // throttle suppressed, as this needs to be running for
+ // takeoff detection
+ TECS_controller.update_50hz();
+ }
+
+#if HAL_QUADPLANE_ENABLED
+ if (quadplane.in_vtol_mode() ||
+ quadplane.in_assisted_flight()) {
+ quadplane.update_throttle_mix();
+ }
+#endif
+}
+
+
+/*
+ read and update compass
+ */
+void Plane::update_compass(void)
+{
+ compass.read();
+}
+
+#if HAL_LOGGING_ENABLED
+/*
+ do 10Hz logging
+ */
+void Plane::update_logging10(void)
+{
+ bool log_faster = (should_log(MASK_LOG_ATTITUDE_FULLRATE) || should_log(MASK_LOG_ATTITUDE_FAST));
+ if (should_log(MASK_LOG_ATTITUDE_MED) && !log_faster) {
+ Log_Write_Attitude();
+ ahrs.Write_AOA_SSA();
+ } else if (log_faster) {
+ ahrs.Write_AOA_SSA();
+ }
+#if HAL_MOUNT_ENABLED
+ if (should_log(MASK_LOG_CAMERA)) {
+ camera_mount.write_log();
+ }
+#endif
+}
+
+/*
+ do 25Hz logging
+ */
+void Plane::update_logging25(void)
+{
+ // MASK_LOG_ATTITUDE_FULLRATE logs at 400Hz, MASK_LOG_ATTITUDE_FAST at 25Hz, MASK_LOG_ATTIUDE_MED logs at 10Hz
+ // highest rate selected wins
+ bool log_faster = should_log(MASK_LOG_ATTITUDE_FULLRATE);
+ if (should_log(MASK_LOG_ATTITUDE_FAST) && !log_faster) {
+ Log_Write_Attitude();
+ }
+
+ if (should_log(MASK_LOG_CTUN)) {
+ Log_Write_Control_Tuning();
+#if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
+ if (!should_log(MASK_LOG_NOTCH_FULLRATE)) {
+ AP::ins().write_notch_log_messages();
+ }
+#endif
+#if HAL_GYROFFT_ENABLED
+ gyro_fft.write_log_messages();
+#endif
+ }
+
+ if (should_log(MASK_LOG_NTUN)) {
+ Log_Write_Nav_Tuning();
+ Log_Write_Guided();
+ }
+
+ if (should_log(MASK_LOG_RC))
+ Log_Write_RC();
+
+ if (should_log(MASK_LOG_IMU))
+ AP::ins().Write_Vibration();
+}
+#endif // HAL_LOGGING_ENABLED
+
+/*
+ check for AFS failsafe check
+ */
+#if AP_ADVANCEDFAILSAFE_ENABLED
+void Plane::afs_fs_check(void)
+{
+ afs.check(failsafe.AFS_last_valid_rc_ms);
+}
+#endif
+
+#if HAL_WITH_IO_MCU
+#include
+extern AP_IOMCU iomcu;
+#endif
+
+void Plane::one_second_loop()
+{
+ // make it possible to change control channel ordering at runtime
+ set_control_channels();
+
+#if HAL_WITH_IO_MCU
+ iomcu.setup_mixing(g.override_channel.get(), g.mixing_gain, g2.manual_rc_mask);
+#endif
+
+#if HAL_ADSB_ENABLED
+ adsb.set_stall_speed_cm(aparm.airspeed_min * 100); // convert m/s to cm/s
+ adsb.set_max_speed(aparm.airspeed_max);
+#endif
+
+ if (flight_option_enabled(FlightOptions::ENABLE_DEFAULT_AIRSPEED)) {
+ // use average of min and max airspeed as default airspeed fusion with high variance
+ ahrs.writeDefaultAirSpeed((float)((aparm.airspeed_min + aparm.airspeed_max)/2),
+ (float)((aparm.airspeed_max - aparm.airspeed_min)/2));
+ }
+
+ // sync MAVLink system ID
+ mavlink_system.sysid = g.sysid_this_mav;
+
+ AP::srv().enable_aux_servos();
+
+ // update notify flags
+ AP_Notify::flags.pre_arm_check = arming.pre_arm_checks(false);
+ AP_Notify::flags.pre_arm_gps_check = true;
+ AP_Notify::flags.armed = arming.is_armed() || arming.arming_required() == AP_Arming::Required::NO;
+
+#if AP_TERRAIN_AVAILABLE && HAL_LOGGING_ENABLED
+ if (should_log(MASK_LOG_GPS)) {
+ terrain.log_terrain_data();
+ }
+#endif
+
+ // update home position if NOT armed and gps position has
+ // changed. Update every 5s at most
+ if (!arming.is_armed() &&
+ gps.last_message_time_ms() - last_home_update_ms > 5000 &&
+ gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
+ last_home_update_ms = gps.last_message_time_ms();
+ update_home();
+
+ // reset the landing altitude correction
+ landing.alt_offset = 0;
+ }
+
+ // this ensures G_Dt is correct, catching startup issues with constructors
+ // calling the scheduler methods
+ if (!is_equal(1.0f/scheduler.get_loop_rate_hz(), scheduler.get_loop_period_s()) ||
+ !is_equal(G_Dt, scheduler.get_loop_period_s())) {
+ INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
+ }
+
+ const float loop_rate = AP::scheduler().get_filtered_loop_rate_hz();
+#if HAL_QUADPLANE_ENABLED
+ if (quadplane.available()) {
+ quadplane.attitude_control->set_notch_sample_rate(loop_rate);
+ }
+#endif
+ rollController.set_notch_sample_rate(loop_rate);
+ pitchController.set_notch_sample_rate(loop_rate);
+ yawController.set_notch_sample_rate(loop_rate);
+}
+
+void Plane::three_hz_loop()
+{
+#if AP_FENCE_ENABLED
+ fence_check();
+#endif
+}
+
+void Plane::compass_save()
+{
+ if (AP::compass().available() &&
+ compass.get_learn_type() >= Compass::LEARN_INTERNAL &&
+ !hal.util->get_soft_armed()) {
+ /*
+ only save offsets when disarmed
+ */
+ compass.save_offsets();
+ }
+}
+
+#if AP_AIRSPEED_AUTOCAL_ENABLE
+/*
+ once a second update the airspeed calibration ratio
+ */
+void Plane::airspeed_ratio_update(void)
+{
+ if (!airspeed.enabled() ||
+ gps.status() < AP_GPS::GPS_OK_FIX_3D ||
+ gps.ground_speed() < 4) {
+ // don't calibrate when not moving
+ return;
+ }
+ if (airspeed.get_airspeed() < aparm.airspeed_min &&
+ gps.ground_speed() < (uint32_t)aparm.airspeed_min) {
+ // don't calibrate when flying below the minimum airspeed. We
+ // check both airspeed and ground speed to catch cases where
+ // the airspeed ratio is way too low, which could lead to it
+ // never coming up again
+ return;
+ }
+ if (labs(ahrs.roll_sensor) > roll_limit_cd ||
+ ahrs.pitch_sensor > aparm.pitch_limit_max*100 ||
+ ahrs.pitch_sensor < pitch_limit_min*100) {
+ // don't calibrate when going beyond normal flight envelope
+ return;
+ }
+ const Vector3f &vg = gps.velocity();
+ airspeed.update_calibration(vg, aparm.airspeed_max);
+}
+#endif // AP_AIRSPEED_AUTOCAL_ENABLE
+
+/*
+ read the GPS and update position
+ */
+void Plane::update_GPS_50Hz(void)
+{
+ gps.update();
+
+ update_current_loc();
+}
+
+/*
+ read update GPS position - 10Hz update
+ */
+void Plane::update_GPS_10Hz(void)
+{
+ static uint32_t last_gps_msg_ms;
+ if (gps.last_message_time_ms() != last_gps_msg_ms && gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
+ last_gps_msg_ms = gps.last_message_time_ms();
+
+ if (ground_start_count > 1) {
+ ground_start_count--;
+ } else if (ground_start_count == 1) {
+ // We countdown N number of good GPS fixes
+ // so that the altitude is more accurate
+ // -------------------------------------
+ if (current_loc.lat == 0 && current_loc.lng == 0) {
+ ground_start_count = 5;
+
+ } else if (!hal.util->was_watchdog_reset()) {
+ if (!set_home_persistently(gps.location())) {
+ // silently ignore failure...
+ }
+
+ next_WP_loc = prev_WP_loc = home;
+
+ ground_start_count = 0;
+ }
+ }
+
+ // update wind estimate
+ ahrs.estimate_wind();
+ } else if (gps.status() < AP_GPS::GPS_OK_FIX_3D && ground_start_count != 0) {
+ // lost 3D fix, start again
+ ground_start_count = 5;
+ }
+
+ calc_gndspeed_undershoot();
+}
+
+/*
+ main control mode dependent update code
+ */
+void Plane::update_control_mode(void)
+{
+ if ((control_mode != &mode_auto) && (control_mode != &mode_takeoff)) {
+ // hold_course is only used in takeoff and landing
+ steer_state.hold_course_cd = -1;
+ }
+ // refresh the throttle limits, to avoid using stale values
+ // they will be updated once takeoff_calc_throttle is called
+ takeoff_state.throttle_lim_max = 100.0f;
+ takeoff_state.throttle_lim_min = -100.0f;
+
+ update_fly_forward();
+
+ control_mode->update();
+}
+
+
+void Plane::update_fly_forward(void)
+{
+ // ensure we are fly-forward when we are flying as a pure fixed
+ // wing aircraft. This helps the EKF produce better state
+ // estimates as it can make stronger assumptions
+#if HAL_QUADPLANE_ENABLED
+ if (quadplane.available()) {
+ if (quadplane.tailsitter.is_in_fw_flight()) {
+ ahrs.set_fly_forward(true);
+ return;
+ }
+
+ if (quadplane.in_vtol_mode()) {
+ ahrs.set_fly_forward(false);
+ return;
+ }
+
+ if (quadplane.in_assisted_flight()) {
+ ahrs.set_fly_forward(false);
+ return;
+ }
+ }
+#endif
+
+ if (auto_state.idle_mode) {
+ // don't fuse airspeed when in balloon lift
+ ahrs.set_fly_forward(false);
+ return;
+ }
+
+ if (flight_stage == AP_FixedWing::FlightStage::LAND) {
+ ahrs.set_fly_forward(landing.is_flying_forward());
+ return;
+ }
+
+ ahrs.set_fly_forward(true);
+}
+
+/*
+ set the flight stage
+ */
+void Plane::set_flight_stage(AP_FixedWing::FlightStage fs)
+{
+ if (fs == flight_stage) {
+ return;
+ }
+
+ landing.handle_flight_stage_change(fs == AP_FixedWing::FlightStage::LAND);
+
+ if (fs == AP_FixedWing::FlightStage::ABORT_LANDING) {
+ gcs().send_text(MAV_SEVERITY_NOTICE, "Landing aborted, climbing to %dm",
+ int(auto_state.takeoff_altitude_rel_cm/100));
+ }
+
+ flight_stage = fs;
+#if HAL_LOGGING_ENABLED
+ Log_Write_Status();
+#endif
+}
+
+void Plane::update_alt()
+{
+ barometer.update();
+
+ // calculate the sink rate.
+ float sink_rate;
+ Vector3f vel;
+ if (ahrs.get_velocity_NED(vel)) {
+ sink_rate = vel.z;
+ } else if (gps.status() >= AP_GPS::GPS_OK_FIX_3D && gps.have_vertical_velocity()) {
+ sink_rate = gps.velocity().z;
+ } else {
+ sink_rate = -barometer.get_climb_rate();
+ }
+
+ // low pass the sink rate to take some of the noise out
+ auto_state.sink_rate = 0.8f * auto_state.sink_rate + 0.2f*sink_rate;
+#if HAL_PARACHUTE_ENABLED
+ parachute.set_sink_rate(auto_state.sink_rate);
+#endif
+
+ update_flight_stage();
+
+#if AP_SCRIPTING_ENABLED
+ if (nav_scripting_active()) {
+ // don't call TECS while we are in a trick
+ return;
+ }
+#endif
+
+ bool should_run_tecs = control_mode->does_auto_throttle();
+#if HAL_QUADPLANE_ENABLED
+ if (quadplane.should_disable_TECS()) {
+ should_run_tecs = false;
+ }
+#endif
+
+ if (auto_state.idle_mode) {
+ should_run_tecs = false;
+ }
+
+#if AP_PLANE_GLIDER_PULLUP_ENABLED
+ if (mode_auto.in_pullup()) {
+ should_run_tecs = false;
+ }
+#endif
+
+ if (should_run_tecs && !throttle_suppressed) {
+
+ float distance_beyond_land_wp = 0;
+ if (flight_stage == AP_FixedWing::FlightStage::LAND &&
+ current_loc.past_interval_finish_line(prev_WP_loc, next_WP_loc)) {
+ distance_beyond_land_wp = current_loc.get_distance(next_WP_loc);
+ }
+
+ tecs_target_alt_cm = relative_target_altitude_cm();
+
+ if (control_mode == &mode_rtl && !rtl.done_climb && (g2.rtl_climb_min > 0 || (plane.flight_option_enabled(FlightOptions::CLIMB_BEFORE_TURN)))) {
+ // ensure we do the initial climb in RTL. We add an extra
+ // 10m in the demanded height to push TECS to climb
+ // quickly
+ tecs_target_alt_cm = MAX(tecs_target_alt_cm, prev_WP_loc.alt - home.alt) + (g2.rtl_climb_min+10)*100;
+ }
+
+ TECS_controller.update_pitch_throttle(tecs_target_alt_cm,
+ target_airspeed_cm,
+ flight_stage,
+ distance_beyond_land_wp,
+ get_takeoff_pitch_min_cd(),
+ throttle_nudge,
+ tecs_hgt_afe(),
+ aerodynamic_load_factor,
+ g.pitch_trim.get());
+ }
+}
+
+/*
+ recalculate the flight_stage
+ */
+void Plane::update_flight_stage(void)
+{
+ // Update the speed & height controller states
+ if (control_mode->does_auto_throttle() && !throttle_suppressed) {
+ if (control_mode == &mode_auto) {
+#if HAL_QUADPLANE_ENABLED
+ if (quadplane.in_vtol_auto()) {
+ set_flight_stage(AP_FixedWing::FlightStage::VTOL);
+ return;
+ }
+#endif
+ if (auto_state.takeoff_complete == false) {
+ set_flight_stage(AP_FixedWing::FlightStage::TAKEOFF);
+ return;
+ } else if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND) {
+ if (landing.is_commanded_go_around() || flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING) {
+ // abort mode is sticky, it must complete while executing NAV_LAND
+ set_flight_stage(AP_FixedWing::FlightStage::ABORT_LANDING);
+ } else if (landing.get_abort_throttle_enable() && get_throttle_input() >= 90 &&
+ landing.request_go_around()) {
+ gcs().send_text(MAV_SEVERITY_INFO,"Landing aborted via throttle");
+ set_flight_stage(AP_FixedWing::FlightStage::ABORT_LANDING);
+ } else {
+ set_flight_stage(AP_FixedWing::FlightStage::LAND);
+ }
+ return;
+ }
+#if HAL_QUADPLANE_ENABLED
+ if (quadplane.in_assisted_flight()) {
+ set_flight_stage(AP_FixedWing::FlightStage::VTOL);
+ return;
+ }
+#endif
+ set_flight_stage(AP_FixedWing::FlightStage::NORMAL);
+ } else if (control_mode != &mode_takeoff) {
+ // If not in AUTO then assume normal operation for normal TECS operation.
+ // This prevents TECS from being stuck in the wrong stage if you switch from
+ // AUTO to, say, FBWB during a landing, an aborted landing or takeoff.
+ set_flight_stage(AP_FixedWing::FlightStage::NORMAL);
+ }
+ return;
+ }
+#if HAL_QUADPLANE_ENABLED
+ if (quadplane.in_vtol_mode() ||
+ quadplane.in_assisted_flight()) {
+ set_flight_stage(AP_FixedWing::FlightStage::VTOL);
+ return;
+ }
+#endif
+ set_flight_stage(AP_FixedWing::FlightStage::NORMAL);
+}
+
+
+
+
+/*
+ If land_DisarmDelay is enabled (non-zero), check for a landing then auto-disarm after time expires
+
+ only called from AP_Landing, when the landing library is ready to disarm
+ */
+void Plane::disarm_if_autoland_complete()
+{
+ if (landing.get_disarm_delay() > 0 &&
+ !is_flying() &&
+ arming.arming_required() != AP_Arming::Required::NO &&
+ arming.is_armed()) {
+ /* we have auto disarm enabled. See if enough time has passed */
+ if (millis() - auto_state.last_flying_ms >= landing.get_disarm_delay()*1000UL) {
+ if (arming.disarm(AP_Arming::Method::AUTOLANDED)) {
+ gcs().send_text(MAV_SEVERITY_INFO,"Auto disarmed");
+ }
+ }
+ }
+}
+
+bool Plane::trigger_land_abort(const float climb_to_alt_m)
+{
+ if (plane.control_mode != &plane.mode_auto) {
+ return false;
+ }
+#if HAL_QUADPLANE_ENABLED
+ if (plane.quadplane.in_vtol_auto()) {
+ return quadplane.abort_landing();
+ }
+#endif
+
+ uint16_t mission_id = plane.mission.get_current_nav_cmd().id;
+ bool is_in_landing = (plane.flight_stage == AP_FixedWing::FlightStage::LAND) ||
+ plane.is_land_command(mission_id);
+ if (is_in_landing) {
+ // fly a user planned abort pattern if available
+ if (plane.have_position && plane.mission.jump_to_abort_landing_sequence(plane.current_loc)) {
+ return true;
+ }
+
+ // only fly a fixed wing abort if we aren't doing quadplane stuff, or potentially
+ // shooting a quadplane approach
+#if HAL_QUADPLANE_ENABLED
+ const bool attempt_go_around =
+ (!plane.quadplane.available()) ||
+ ((!plane.quadplane.in_vtol_auto()) &&
+ (!plane.quadplane.landing_with_fixed_wing_spiral_approach()));
+#else
+ const bool attempt_go_around = true;
+#endif
+ if (attempt_go_around) {
+ // Initiate an aborted landing. This will trigger a pitch-up and
+ // climb-out to a safe altitude holding heading then one of the
+ // following actions will occur, check for in this order:
+ // - If MAV_CMD_CONTINUE_AND_CHANGE_ALT is next command in mission,
+ // increment mission index to execute it
+ // - else if DO_LAND_START is available, jump to it
+ // - else decrement the mission index to repeat the landing approach
+
+ if (!is_zero(climb_to_alt_m)) {
+ plane.auto_state.takeoff_altitude_rel_cm = climb_to_alt_m * 100;
+ }
+ if (plane.landing.request_go_around()) {
+ plane.auto_state.next_wp_crosstrack = false;
+ return true;
+ }
+ }
+ }
+ return false;
+}
+
+
+/*
+ the height above field elevation that we pass to TECS
+ */
+float Plane::tecs_hgt_afe(void)
+{
+ /*
+ pass the height above field elevation as the height above
+ the ground when in landing, which means that TECS gets the
+ rangefinder information and thus can know when the flare is
+ coming.
+ */
+ float hgt_afe;
+
+ if (flight_stage == AP_FixedWing::FlightStage::LAND) {
+
+ #if AP_MAVLINK_MAV_CMD_SET_HAGL_ENABLED
+ // if external HAGL is active use that
+ if (get_external_HAGL(hgt_afe)) {
+ return hgt_afe;
+ }
+ #endif
+
+ hgt_afe = height_above_target();
+#if AP_RANGEFINDER_ENABLED
+ hgt_afe -= rangefinder_correction();
+#endif
+ } else {
+ // when in normal flight we pass the hgt_afe as relative
+ // altitude to home
+ hgt_afe = relative_altitude;
+ }
+ return hgt_afe;
+}
+
+// vehicle specific waypoint info helpers
+bool Plane::get_wp_distance_m(float &distance) const
+{
+ // see GCS_MAVLINK_Plane::send_nav_controller_output()
+ if (control_mode == &mode_manual) {
+ return false;
+ }
+#if HAL_QUADPLANE_ENABLED
+ if (quadplane.in_vtol_mode()) {
+ distance = quadplane.using_wp_nav() ? quadplane.wp_nav->get_wp_distance_to_destination() * 0.01 : 0;
+ return true;
+ }
+#endif
+ distance = auto_state.wp_distance;
+ return true;
+}
+
+bool Plane::get_wp_bearing_deg(float &bearing) const
+{
+ // see GCS_MAVLINK_Plane::send_nav_controller_output()
+ if (control_mode == &mode_manual) {
+ return false;
+ }
+#if HAL_QUADPLANE_ENABLED
+ if (quadplane.in_vtol_mode()) {
+ bearing = quadplane.using_wp_nav() ? quadplane.wp_nav->get_wp_bearing_to_destination() : 0;
+ return true;
+ }
+#endif
+ bearing = nav_controller->target_bearing_cd() * 0.01;
+ return true;
+}
+
+bool Plane::get_wp_crosstrack_error_m(float &xtrack_error) const
+{
+ // see GCS_MAVLINK_Plane::send_nav_controller_output()
+ if (control_mode == &mode_manual) {
+ return false;
+ }
+#if HAL_QUADPLANE_ENABLED
+ if (quadplane.in_vtol_mode()) {
+ xtrack_error = quadplane.using_wp_nav() ? quadplane.wp_nav->crosstrack_error() : 0;
+ return true;
+ }
+#endif
+ xtrack_error = nav_controller->crosstrack_error();
+ return true;
+}
+
+#if AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED
+// set target location (for use by external control and scripting)
+bool Plane::set_target_location(const Location &target_loc)
+{
+ Location loc{target_loc};
+
+ if (plane.control_mode != &plane.mode_guided) {
+ // only accept position updates when in GUIDED mode
+ return false;
+ }
+ // add home alt if needed
+ if (loc.relative_alt) {
+ loc.alt += plane.home.alt;
+ loc.relative_alt = 0;
+ }
+ plane.set_guided_WP(loc);
+ return true;
+}
+#endif //AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED
+
+#if AP_SCRIPTING_ENABLED
+// get target location (for use by scripting)
+bool Plane::get_target_location(Location& target_loc)
+{
+ switch (control_mode->mode_number()) {
+ case Mode::Number::RTL:
+ case Mode::Number::AVOID_ADSB:
+ case Mode::Number::GUIDED:
+ case Mode::Number::AUTO:
+ case Mode::Number::LOITER:
+ case Mode::Number::TAKEOFF:
+#if HAL_QUADPLANE_ENABLED
+ case Mode::Number::QLOITER:
+ case Mode::Number::QLAND:
+ case Mode::Number::QRTL:
+#endif
+ target_loc = next_WP_loc;
+ return true;
+ break;
+ default:
+ break;
+ }
+ return false;
+}
+
+/*
+ update_target_location() works in all auto navigation modes
+ */
+bool Plane::update_target_location(const Location &old_loc, const Location &new_loc)
+{
+ /*
+ by checking the caller has provided the correct old target
+ location we prevent a race condition where the user changes mode
+ or commands a different target in the controlling lua script
+ */
+ if (!old_loc.same_loc_as(next_WP_loc) ||
+ old_loc.get_alt_frame() != new_loc.get_alt_frame()) {
+ return false;
+ }
+ next_WP_loc = new_loc;
+
+#if HAL_QUADPLANE_ENABLED
+ if (control_mode == &mode_qland || control_mode == &mode_qloiter) {
+ mode_qloiter.last_target_loc_set_ms = AP_HAL::millis();
+ }
+#endif
+
+ return true;
+}
+
+// allow for velocity matching in VTOL
+bool Plane::set_velocity_match(const Vector2f &velocity)
+{
+#if HAL_QUADPLANE_ENABLED
+ if (quadplane.in_vtol_mode() || quadplane.in_vtol_land_sequence()) {
+ quadplane.poscontrol.velocity_match = velocity;
+ quadplane.poscontrol.last_velocity_match_ms = AP_HAL::millis();
+ return true;
+ }
+#endif
+ return false;
+}
+
+// allow for override of land descent rate
+bool Plane::set_land_descent_rate(float descent_rate)
+{
+#if HAL_QUADPLANE_ENABLED
+ if (quadplane.in_vtol_land_descent() ||
+ control_mode == &mode_qland) {
+ quadplane.poscontrol.override_descent_rate = descent_rate;
+ quadplane.poscontrol.last_override_descent_ms = AP_HAL::millis();
+ return true;
+ }
+#endif
+ return false;
+}
+
+// Allow for scripting to have control over the crosstracking when exiting and resuming missions or guided flight
+// It's up to the Lua script to ensure the provided location makes sense
+bool Plane::set_crosstrack_start(const Location &new_start_location)
+{
+ prev_WP_loc = new_start_location;
+ auto_state.crosstrack = true;
+ return true;
+}
+
+#endif // AP_SCRIPTING_ENABLED
+
+// returns true if vehicle is landing.
+bool Plane::is_landing() const
+{
+#if HAL_QUADPLANE_ENABLED
+ if (plane.quadplane.in_vtol_land_descent()) {
+ return true;
+ }
+#endif
+ return control_mode->is_landing();
+}
+
+// returns true if vehicle is taking off.
+bool Plane::is_taking_off() const
+{
+#if HAL_QUADPLANE_ENABLED
+ if (plane.quadplane.in_vtol_takeoff()) {
+ return true;
+ }
+#endif
+ return control_mode->is_taking_off();
+}
+
+// correct AHRS pitch for PTCH_TRIM_DEG in non-VTOL modes, and return VTOL view in VTOL
+void Plane::get_osd_roll_pitch_rad(float &roll, float &pitch) const
+{
+#if HAL_QUADPLANE_ENABLED
+ if (quadplane.show_vtol_view()) {
+ pitch = quadplane.ahrs_view->pitch;
+ roll = quadplane.ahrs_view->roll;
+ return;
+ }
+#endif
+ pitch = ahrs.get_pitch();
+ roll = ahrs.get_roll();
+ if (!(flight_option_enabled(FlightOptions::OSD_REMOVE_TRIM_PITCH))) { // correct for PTCH_TRIM_DEG
+ pitch -= g.pitch_trim * DEG_TO_RAD;
+ }
+}
+
+/*
+ update current_loc Location
+ */
+void Plane::update_current_loc(void)
+{
+ have_position = plane.ahrs.get_location(plane.current_loc);
+
+ // re-calculate relative altitude
+ ahrs.get_relative_position_D_home(plane.relative_altitude);
+ relative_altitude *= -1.0f;
+}
+
+// check if FLIGHT_OPTION is enabled
+bool Plane::flight_option_enabled(FlightOptions flight_option) const
+{
+ return g2.flight_options & flight_option;
+}
+
+#if AC_PRECLAND_ENABLED
+void Plane::precland_update(void)
+{
+ // alt will be unused if we pass false through as the second parameter:
+#if AP_RANGEFINDER_ENABLED
+ return g2.precland.update(rangefinder_state.height_estimate*100, rangefinder_state.in_range);
+#else
+ return g2.precland.update(0, false);
+#endif
+}
+#endif
+
+#if AP_QUICKTUNE_ENABLED
+/*
+ update AP_Quicktune object. We pass the supports_quicktune() method
+ in so that quicktune can detect if the user changes to a
+ non-quicktune capable mode while tuning and the gains can be reverted
+ */
+void Plane::update_quicktune(void)
+{
+ quicktune.update(control_mode->supports_quicktune());
+}
+#endif
+
/*
constructor for main Plane class
*/
@@ -31,3 +1066,5 @@ Plane::Plane(void)
Plane plane;
AP_Vehicle& vehicle = plane;
+
+AP_HAL_MAIN_CALLBACKS(&plane);
diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h
index df180b6f745a0..ec66ada1b2650 100644
--- a/ArduPlane/Plane.h
+++ b/ArduPlane/Plane.h
@@ -1060,7 +1060,7 @@ class Plane : public AP_Vehicle {
bool in_fence_recovery() const;
#endif
- // ArduPlane.cpp
+ // Plane.cpp
void disarm_if_autoland_complete();
bool trigger_land_abort(const float climb_to_alt_m);
void get_osd_roll_pitch_rad(float &roll, float &pitch) const override;
diff --git a/libraries/AP_HAL/board/esp32.h b/libraries/AP_HAL/board/esp32.h
index e927165b8cd25..34846d8b4cd9b 100644
--- a/libraries/AP_HAL/board/esp32.h
+++ b/libraries/AP_HAL/board/esp32.h
@@ -95,7 +95,7 @@
// disble temp cal of gyros by default
#define HAL_INS_TEMPERATURE_CAL_ENABLE 0
-//turn off a bunch of advanced plane scheduler table things. see ArduPlane.cpp
+//turn off a bunch of advanced plane scheduler table things. see Plane.cpp
#define AP_ADVANCEDFAILSAFE_ENABLED 0
#define AP_ICENGINE_ENABLED 0
#define AP_OPTICALFLOW_ENABLED 0