2015-05-13 03:09:36 -03:00
|
|
|
/*
|
2016-05-17 22:37:11 -03:00
|
|
|
Lead developer: Andrew Tridgell & Tom Pittenger
|
2016-05-05 19:10:08 -03:00
|
|
|
|
2016-05-17 23:16:02 -03:00
|
|
|
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
|
2015-05-13 03:09:36 -03:00
|
|
|
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 http://dev.ardupilot.com 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 <http://www.gnu.org/licenses/>.
|
|
|
|
*/
|
2016-05-05 19:10:08 -03:00
|
|
|
#pragma once
|
2015-05-13 03:09:36 -03:00
|
|
|
|
|
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
|
|
// Header includes
|
|
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
|
|
|
2016-03-31 18:43:36 -03:00
|
|
|
#include <cmath>
|
2015-05-13 03:09:36 -03:00
|
|
|
#include <stdarg.h>
|
|
|
|
#include <stdio.h>
|
|
|
|
|
2015-08-11 03:28:40 -03:00
|
|
|
#include <AP_HAL/AP_HAL.h>
|
|
|
|
#include <AP_Common/AP_Common.h>
|
2022-03-12 07:48:55 -04:00
|
|
|
#include <AP_Airspeed/AP_Airspeed.h>
|
2015-08-11 03:28:40 -03:00
|
|
|
#include <AP_Param/AP_Param.h>
|
|
|
|
#include <StorageManager/StorageManager.h>
|
|
|
|
#include <AP_Math/AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
|
|
|
|
#include <AP_InertialSensor/AP_InertialSensor.h> // Inertial Sensor Library
|
2015-10-21 18:11:52 -03:00
|
|
|
#include <AP_AccelCal/AP_AccelCal.h> // interface and maths for accelerometer calibration
|
2015-08-11 03:28:40 -03:00
|
|
|
#include <AP_AHRS/AP_AHRS.h> // ArduPilot Mega DCM Library
|
2016-10-22 07:27:57 -03:00
|
|
|
#include <SRV_Channel/SRV_Channel.h>
|
2015-08-11 03:28:40 -03:00
|
|
|
#include <AP_RangeFinder/AP_RangeFinder.h> // Range finder library
|
|
|
|
#include <Filter/Filter.h> // Filter library
|
|
|
|
#include <AP_Camera/AP_Camera.h> // Photo or video camera
|
|
|
|
#include <AP_Terrain/AP_Terrain.h>
|
2015-09-24 07:58:18 -03:00
|
|
|
#include <AP_RPM/AP_RPM.h>
|
2016-10-25 22:24:41 -03:00
|
|
|
#include <AP_Stats/AP_Stats.h> // statistics library
|
2016-11-23 03:14:03 -04:00
|
|
|
#include <AP_Beacon/AP_Beacon.h>
|
2015-08-11 03:28:40 -03:00
|
|
|
|
2016-07-22 04:45:05 -03:00
|
|
|
#include <AP_AdvancedFailsafe/AP_AdvancedFailsafe.h>
|
2015-08-11 03:28:40 -03:00
|
|
|
#include <APM_Control/APM_Control.h>
|
|
|
|
#include <APM_Control/AP_AutoTune.h>
|
|
|
|
#include <GCS_MAVLink/GCS_MAVLink.h> // MAVLink GCS definitions
|
|
|
|
#include <AP_Mount/AP_Mount.h> // Camera/Antenna mount
|
|
|
|
#include <AP_Declination/AP_Declination.h> // ArduPilot Mega Declination Helper Library
|
2019-01-18 00:23:42 -04:00
|
|
|
#include <AP_Logger/AP_Logger.h>
|
2015-08-11 03:28:40 -03:00
|
|
|
#include <AP_Scheduler/AP_Scheduler.h> // main loop scheduler
|
2017-11-12 23:44:16 -04:00
|
|
|
#include <AP_Scheduler/PerfInfo.h> // loop perf monitoring
|
2015-08-11 03:28:40 -03:00
|
|
|
|
|
|
|
#include <AP_Navigation/AP_Navigation.h>
|
|
|
|
#include <AP_L1_Control/AP_L1_Control.h>
|
|
|
|
#include <AP_RCMapper/AP_RCMapper.h> // RC input mapping library
|
|
|
|
|
|
|
|
#include <AP_Vehicle/AP_Vehicle.h>
|
|
|
|
#include <AP_TECS/AP_TECS.h>
|
2015-09-21 02:28:25 -03:00
|
|
|
#include <AP_NavEKF2/AP_NavEKF2.h>
|
2016-08-14 06:46:24 -03:00
|
|
|
#include <AP_NavEKF3/AP_NavEKF3.h>
|
2015-08-11 03:28:40 -03:00
|
|
|
#include <AP_Mission/AP_Mission.h> // Mission command library
|
|
|
|
|
2017-02-26 19:18:38 -04:00
|
|
|
#include <AP_Soaring/AP_Soaring.h>
|
2015-08-11 03:28:40 -03:00
|
|
|
#include <AP_BattMonitor/AP_BattMonitor.h> // Battery monitor library
|
|
|
|
|
|
|
|
#include <AP_Arming/AP_Arming.h>
|
|
|
|
#include <AP_Frsky_Telem/AP_Frsky_Telem.h>
|
2018-06-23 04:11:05 -03:00
|
|
|
#include <AP_OSD/AP_OSD.h>
|
2015-08-11 03:28:40 -03:00
|
|
|
|
|
|
|
#include <AP_Rally/AP_Rally.h>
|
|
|
|
|
|
|
|
#include <AP_OpticalFlow/AP_OpticalFlow.h> // Optical Flow library
|
2015-06-18 07:45:33 -03:00
|
|
|
#include <AP_Parachute/AP_Parachute.h>
|
2015-11-19 14:24:55 -04:00
|
|
|
#include <AP_ADSB/AP_ADSB.h>
|
2016-07-23 04:37:42 -03:00
|
|
|
#include <AP_ICEngine/AP_ICEngine.h>
|
2016-11-16 21:05:37 -04:00
|
|
|
#include <AP_Landing/AP_Landing.h>
|
2018-06-10 03:33:06 -03:00
|
|
|
#include <AP_LandingGear/AP_LandingGear.h> // Landing Gear library
|
2022-01-04 00:53:18 -04:00
|
|
|
#include <AP_Follow/AP_Follow.h>
|
2023-08-08 21:45:57 -03:00
|
|
|
#include <AP_ExternalControl/AP_ExternalControl_config.h>
|
|
|
|
#if AP_EXTERNAL_CONTROL_ENABLED
|
|
|
|
#include <AP_ExternalControl/AP_ExternalControl.h>
|
|
|
|
#endif
|
2015-05-13 03:09:36 -03:00
|
|
|
|
2016-05-27 09:53:19 -03:00
|
|
|
#include "GCS_Mavlink.h"
|
2016-08-25 04:48:27 -03:00
|
|
|
#include "GCS_Plane.h"
|
2015-11-24 04:24:04 -04:00
|
|
|
#include "quadplane.h"
|
2023-09-19 06:51:08 -03:00
|
|
|
#include <AP_Tuning/AP_Tuning_config.h>
|
|
|
|
#if AP_TUNING_ENABLED
|
2016-04-16 07:26:43 -03:00
|
|
|
#include "tuning.h"
|
2023-09-19 06:51:08 -03:00
|
|
|
#endif
|
2015-11-24 04:24:04 -04:00
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
// Configuration
|
|
|
|
#include "config.h"
|
|
|
|
|
2023-02-06 01:39:02 -04:00
|
|
|
#if AP_ADVANCEDFAILSAFE_ENABLED
|
2019-01-07 20:10:15 -04:00
|
|
|
#include "afs_plane.h"
|
2019-01-08 21:05:40 -04:00
|
|
|
#endif
|
2019-01-07 20:10:15 -04:00
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
// Local modules
|
|
|
|
#include "defines.h"
|
2019-01-15 13:46:13 -04:00
|
|
|
#include "mode.h"
|
2015-05-13 03:09:36 -03:00
|
|
|
|
2021-11-15 01:08:34 -04:00
|
|
|
#if AP_SCRIPTING_ENABLED
|
2018-09-27 20:03:56 -03:00
|
|
|
#include <AP_Scripting/AP_Scripting.h>
|
|
|
|
#endif
|
|
|
|
|
2018-04-26 22:01:37 -03:00
|
|
|
#include "RC_Channel.h" // RC Channel Library
|
2015-05-13 03:09:36 -03:00
|
|
|
#include "Parameters.h"
|
2020-09-19 05:38:20 -03:00
|
|
|
#if HAL_ADSB_ENABLED
|
2016-08-12 18:28:21 -03:00
|
|
|
#include "avoidance_adsb.h"
|
2020-09-19 05:38:20 -03:00
|
|
|
#endif
|
2017-01-09 03:56:56 -04:00
|
|
|
#include "AP_Arming.h"
|
2015-05-13 03:09:36 -03:00
|
|
|
|
2015-06-04 00:13:34 -03:00
|
|
|
/*
|
|
|
|
main APM:Plane class
|
|
|
|
*/
|
2018-12-27 02:34:20 -04:00
|
|
|
class Plane : public AP_Vehicle {
|
2015-05-13 03:09:36 -03:00
|
|
|
public:
|
2016-05-27 09:53:19 -03:00
|
|
|
friend class GCS_MAVLINK_Plane;
|
2015-05-13 03:09:36 -03:00
|
|
|
friend class Parameters;
|
2016-07-23 04:37:42 -03:00
|
|
|
friend class ParametersG2;
|
2015-06-04 00:13:34 -03:00
|
|
|
friend class AP_Arming_Plane;
|
2015-11-24 04:24:04 -04:00
|
|
|
friend class QuadPlane;
|
2018-12-07 03:52:05 -04:00
|
|
|
friend class QAutoTune;
|
2016-05-06 01:43:39 -03:00
|
|
|
friend class AP_Tuning_Plane;
|
2016-07-22 05:14:53 -03:00
|
|
|
friend class AP_AdvancedFailsafe_Plane;
|
2016-08-12 18:28:21 -03:00
|
|
|
friend class AP_Avoidance_Plane;
|
2016-08-25 04:48:27 -03:00
|
|
|
friend class GCS_Plane;
|
2018-09-05 01:22:14 -03:00
|
|
|
friend class RC_Channel_Plane;
|
2018-04-26 22:01:37 -03:00
|
|
|
friend class RC_Channels_Plane;
|
2021-07-14 17:15:49 -03:00
|
|
|
friend class Tailsitter;
|
2021-09-04 19:55:25 -03:00
|
|
|
friend class Tiltrotor;
|
2021-09-17 20:26:58 -03:00
|
|
|
friend class SLT_Transition;
|
|
|
|
friend class Tailsitter_Transition;
|
2015-05-13 03:09:36 -03:00
|
|
|
|
2019-01-15 13:46:13 -04:00
|
|
|
friend class Mode;
|
|
|
|
friend class ModeCircle;
|
|
|
|
friend class ModeStabilize;
|
|
|
|
friend class ModeTraining;
|
|
|
|
friend class ModeAcro;
|
|
|
|
friend class ModeFBWA;
|
|
|
|
friend class ModeFBWB;
|
|
|
|
friend class ModeCruise;
|
|
|
|
friend class ModeAutoTune;
|
|
|
|
friend class ModeAuto;
|
|
|
|
friend class ModeRTL;
|
|
|
|
friend class ModeLoiter;
|
|
|
|
friend class ModeAvoidADSB;
|
|
|
|
friend class ModeGuided;
|
|
|
|
friend class ModeInitializing;
|
|
|
|
friend class ModeManual;
|
|
|
|
friend class ModeQStabilize;
|
|
|
|
friend class ModeQHover;
|
|
|
|
friend class ModeQLoiter;
|
|
|
|
friend class ModeQLand;
|
|
|
|
friend class ModeQRTL;
|
|
|
|
friend class ModeQAcro;
|
|
|
|
friend class ModeQAutotune;
|
2019-05-04 06:56:07 -03:00
|
|
|
friend class ModeTakeoff;
|
2020-09-16 04:46:56 -03:00
|
|
|
friend class ModeThermal;
|
2021-09-18 20:15:41 -03:00
|
|
|
friend class ModeLoiterAltQLand;
|
2019-01-15 13:46:13 -04:00
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
Plane(void);
|
2015-10-19 15:28:58 -03:00
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
private:
|
2017-08-19 06:50:44 -03:00
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
// key aircraft parameters passed to multiple libraries
|
2022-09-29 20:10:41 -03:00
|
|
|
AP_FixedWing aparm;
|
2015-05-13 03:09:36 -03:00
|
|
|
|
2016-07-21 21:28:17 -03:00
|
|
|
// Global parameters are all contained within the 'g' and 'g2' classes.
|
2015-05-13 03:09:36 -03:00
|
|
|
Parameters g;
|
2016-07-21 21:28:17 -03:00
|
|
|
ParametersG2 g2;
|
2015-05-13 03:09:36 -03:00
|
|
|
|
|
|
|
// mapping between input channels
|
2017-12-12 21:06:15 -04:00
|
|
|
RCMapper rcmap;
|
2015-05-13 03:09:36 -03:00
|
|
|
|
2016-10-22 07:27:57 -03:00
|
|
|
// primary input channels
|
2015-05-13 03:09:36 -03:00
|
|
|
RC_Channel *channel_roll;
|
|
|
|
RC_Channel *channel_pitch;
|
|
|
|
RC_Channel *channel_throttle;
|
|
|
|
RC_Channel *channel_rudder;
|
2020-09-02 05:31:38 -03:00
|
|
|
RC_Channel *channel_flap;
|
2020-09-02 04:33:02 -03:00
|
|
|
RC_Channel *channel_airbrake;
|
2015-05-13 03:09:36 -03:00
|
|
|
|
2019-01-18 00:23:42 -04:00
|
|
|
AP_Logger logger;
|
2015-05-13 03:09:36 -03:00
|
|
|
|
|
|
|
// scaled roll limit based on pitch
|
|
|
|
int32_t roll_limit_cd;
|
|
|
|
int32_t pitch_limit_min_cd;
|
|
|
|
|
|
|
|
// flight modes convenience array
|
2015-05-31 19:48:54 -03:00
|
|
|
AP_Int8 *flight_modes = &g.flight_mode1;
|
2022-07-13 22:10:43 -03:00
|
|
|
const uint8_t num_flight_modes = 6;
|
2015-05-13 03:09:36 -03:00
|
|
|
|
2022-09-29 20:10:41 -03:00
|
|
|
AP_FixedWing::Rangefinder_State rangefinder_state;
|
2015-06-01 03:16:49 -03:00
|
|
|
|
2022-07-21 01:34:31 -03:00
|
|
|
#if AP_RPM_ENABLED
|
2017-12-12 21:06:15 -04:00
|
|
|
AP_RPM rpm_sensor;
|
2022-07-21 01:34:31 -03:00
|
|
|
#endif
|
2017-08-25 15:18:56 -03:00
|
|
|
|
2022-07-28 20:41:57 -03:00
|
|
|
AP_TECS TECS_controller{ahrs, aparm, landing, MASK_LOG_TECS};
|
2017-12-12 21:06:15 -04:00
|
|
|
AP_L1_Control L1_controller{ahrs, &TECS_controller};
|
2015-05-13 03:09:36 -03:00
|
|
|
|
|
|
|
// Attitude to servo controllers
|
2021-07-20 08:15:06 -03:00
|
|
|
AP_RollController rollController{aparm};
|
|
|
|
AP_PitchController pitchController{aparm};
|
|
|
|
AP_YawController yawController{aparm};
|
|
|
|
AP_SteerController steerController{};
|
2015-05-13 03:09:36 -03:00
|
|
|
|
|
|
|
// Training mode
|
|
|
|
bool training_manual_roll; // user has manual roll control
|
|
|
|
bool training_manual_pitch; // user has manual pitch control
|
|
|
|
|
|
|
|
// should throttle be pass-thru in guided?
|
|
|
|
bool guided_throttle_passthru;
|
|
|
|
|
|
|
|
// are we doing calibration? This is used to allow heartbeat to
|
|
|
|
// external failsafe boards during baro and airspeed calibration
|
|
|
|
bool in_calibration;
|
|
|
|
|
2023-08-08 10:24:11 -03:00
|
|
|
// are we currently in long failsafe but have postponed it in MODE TAKEOFF until min level alt is reached
|
|
|
|
bool long_failsafe_pending;
|
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
// GCS selection
|
2016-08-25 04:48:27 -03:00
|
|
|
GCS_Plane _gcs; // avoid using this; use gcs()
|
|
|
|
GCS_Plane &gcs() { return _gcs; }
|
2015-05-13 03:09:36 -03:00
|
|
|
|
|
|
|
// selected navigation controller
|
2015-05-31 19:48:54 -03:00
|
|
|
AP_Navigation *nav_controller = &L1_controller;
|
2015-05-13 03:09:36 -03:00
|
|
|
|
|
|
|
// Camera
|
2022-06-02 05:28:26 -03:00
|
|
|
#if AP_CAMERA_ENABLED
|
2019-03-26 08:08:15 -03:00
|
|
|
AP_Camera camera{MASK_LOG_CAMERA};
|
2015-05-13 03:09:36 -03:00
|
|
|
#endif
|
|
|
|
|
2021-12-24 04:17:51 -04:00
|
|
|
#if AP_OPTICALFLOW_ENABLED
|
2015-05-13 03:09:36 -03:00
|
|
|
// Optical flow sensor
|
2022-08-14 22:31:14 -03:00
|
|
|
AP_OpticalFlow optflow;
|
2015-07-10 01:50:35 -03:00
|
|
|
#endif
|
2015-05-13 03:09:36 -03:00
|
|
|
|
2023-04-07 05:25:34 -03:00
|
|
|
#if HAL_RALLY_ENABLED
|
2023-10-11 04:41:54 -03:00
|
|
|
// Rally Points
|
2019-02-18 05:38:43 -04:00
|
|
|
AP_Rally rally;
|
2023-04-07 05:25:34 -03:00
|
|
|
#endif
|
|
|
|
|
|
|
|
// returns a Location for a rally point or home; if
|
|
|
|
// HAL_RALLY_ENABLED is false, just home.
|
|
|
|
Location calc_best_rally_or_home_location(const Location ¤t_loc, float rtl_home_alt_amsl_cm) const;
|
2017-08-29 20:28:35 -03:00
|
|
|
|
2020-10-28 16:39:13 -03:00
|
|
|
#if OSD_ENABLED || OSD_PARAM_ENABLED
|
2018-06-23 04:35:28 -03:00
|
|
|
AP_OSD osd;
|
|
|
|
#endif
|
2020-09-10 03:31:18 -03:00
|
|
|
|
2019-01-15 13:46:13 -04:00
|
|
|
ModeCircle mode_circle;
|
|
|
|
ModeStabilize mode_stabilize;
|
|
|
|
ModeTraining mode_training;
|
|
|
|
ModeAcro mode_acro;
|
|
|
|
ModeFBWA mode_fbwa;
|
|
|
|
ModeFBWB mode_fbwb;
|
|
|
|
ModeCruise mode_cruise;
|
|
|
|
ModeAutoTune mode_autotune;
|
|
|
|
ModeAuto mode_auto;
|
|
|
|
ModeRTL mode_rtl;
|
|
|
|
ModeLoiter mode_loiter;
|
2020-09-22 14:00:39 -03:00
|
|
|
#if HAL_ADSB_ENABLED
|
2019-01-15 13:46:13 -04:00
|
|
|
ModeAvoidADSB mode_avoidADSB;
|
2020-09-22 14:00:39 -03:00
|
|
|
#endif
|
2019-01-15 13:46:13 -04:00
|
|
|
ModeGuided mode_guided;
|
|
|
|
ModeInitializing mode_initializing;
|
|
|
|
ModeManual mode_manual;
|
2021-09-10 03:28:21 -03:00
|
|
|
#if HAL_QUADPLANE_ENABLED
|
2019-01-15 13:46:13 -04:00
|
|
|
ModeQStabilize mode_qstabilize;
|
|
|
|
ModeQHover mode_qhover;
|
|
|
|
ModeQLoiter mode_qloiter;
|
|
|
|
ModeQLand mode_qland;
|
|
|
|
ModeQRTL mode_qrtl;
|
|
|
|
ModeQAcro mode_qacro;
|
2022-02-03 23:24:05 -04:00
|
|
|
ModeLoiterAltQLand mode_loiter_qland;
|
2021-09-10 03:28:21 -03:00
|
|
|
#if QAUTOTUNE_ENABLED
|
2019-01-15 13:46:13 -04:00
|
|
|
ModeQAutotune mode_qautotune;
|
2021-09-10 03:28:21 -03:00
|
|
|
#endif // QAUTOTUNE_ENABLED
|
|
|
|
#endif // HAL_QUADPLANE_ENABLED
|
2019-05-04 06:56:07 -03:00
|
|
|
ModeTakeoff mode_takeoff;
|
2020-09-23 05:16:45 -03:00
|
|
|
#if HAL_SOARING_ENABLED
|
2020-09-16 04:46:56 -03:00
|
|
|
ModeThermal mode_thermal;
|
|
|
|
#endif
|
2019-01-15 13:46:13 -04:00
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
// This is the state of the flight control system
|
|
|
|
// There are multiple states defined such as MANUAL, FBW-A, AUTO
|
2019-01-15 13:46:13 -04:00
|
|
|
Mode *control_mode = &mode_initializing;
|
|
|
|
Mode *previous_mode = &mode_initializing;
|
2015-05-13 03:09:36 -03:00
|
|
|
|
2017-04-22 08:47:19 -03:00
|
|
|
// time of last mode change
|
|
|
|
uint32_t last_mode_change_ms;
|
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
// Used to maintain the state of the previous control switch position
|
|
|
|
// This is set to 254 when we need to re-read the switch
|
2015-05-31 19:48:54 -03:00
|
|
|
uint8_t oldSwitchPosition = 254;
|
2015-05-13 03:09:36 -03:00
|
|
|
|
|
|
|
// This is used to enable the inverted flight feature
|
|
|
|
bool inverted_flight;
|
|
|
|
|
2019-07-06 21:16:47 -03:00
|
|
|
// last time we ran roll/pitch stabilization
|
|
|
|
uint32_t last_stabilize_ms;
|
2021-02-23 12:32:06 -04:00
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
// Failsafe
|
|
|
|
struct {
|
2023-10-11 04:41:54 -03:00
|
|
|
// Used to track if the value on channel 3 (throttle) has fallen below the failsafe threshold
|
2015-05-13 03:09:36 -03:00
|
|
|
// RC receiver should be set up to output a low throttle value when signal is lost
|
2019-09-09 16:49:52 -03:00
|
|
|
bool rc_failsafe;
|
2015-05-13 03:09:36 -03:00
|
|
|
|
2016-08-12 16:20:13 -03:00
|
|
|
// true if an adsb related failsafe has occurred
|
2019-09-09 16:49:52 -03:00
|
|
|
bool adsb;
|
2016-08-12 16:20:13 -03:00
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
// saved flight mode
|
2019-01-15 13:46:13 -04:00
|
|
|
enum Mode::Number saved_mode_number;
|
2015-05-13 03:09:36 -03:00
|
|
|
|
|
|
|
// A tracking variable for type of failsafe active
|
|
|
|
// Used for failsafe based on loss of RC signal or GCS signal
|
|
|
|
int16_t state;
|
|
|
|
|
2017-10-27 17:21:28 -03:00
|
|
|
// number of low throttle values
|
|
|
|
uint8_t throttle_counter;
|
2015-05-13 03:09:36 -03:00
|
|
|
|
|
|
|
// A timer used to track how long we have been in a "short failsafe" condition due to loss of RC signal
|
2017-10-27 17:21:28 -03:00
|
|
|
uint32_t short_timer_ms;
|
2021-02-23 12:32:06 -04:00
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
uint32_t last_valid_rc_ms;
|
2016-03-24 16:17:41 -03:00
|
|
|
|
|
|
|
//keeps track of the last valid rc as it relates to the AFS system
|
|
|
|
//Does not count rc inputs as valid if the standard failsafe is on
|
|
|
|
uint32_t AFS_last_valid_rc_ms;
|
2015-05-13 03:09:36 -03:00
|
|
|
} failsafe;
|
|
|
|
|
2018-09-25 01:11:26 -03:00
|
|
|
enum Landing_ApproachStage {
|
2021-03-23 18:27:37 -03:00
|
|
|
RTL,
|
2018-09-25 01:11:26 -03:00
|
|
|
LOITER_TO_ALT,
|
2018-12-05 17:18:27 -04:00
|
|
|
ENSURE_RADIUS,
|
2018-09-25 01:11:26 -03:00
|
|
|
WAIT_FOR_BREAKOUT,
|
|
|
|
APPROACH_LINE,
|
|
|
|
VTOL_LANDING,
|
|
|
|
};
|
|
|
|
|
2021-09-10 03:28:21 -03:00
|
|
|
#if HAL_QUADPLANE_ENABLED
|
2018-09-25 01:11:26 -03:00
|
|
|
// Landing
|
|
|
|
struct {
|
|
|
|
enum Landing_ApproachStage approach_stage;
|
|
|
|
float approach_direction_deg;
|
|
|
|
} vtol_approach_s;
|
2021-09-10 03:28:21 -03:00
|
|
|
#endif
|
2018-09-25 01:11:26 -03:00
|
|
|
|
2018-03-22 06:16:35 -03:00
|
|
|
bool any_failsafe_triggered() {
|
|
|
|
return failsafe.state != FAILSAFE_NONE || battery.has_failsafed() || failsafe.adsb;
|
|
|
|
}
|
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
// A counter used to count down valid gps fixes to allow the gps estimate to settle
|
|
|
|
// before recording our home position (and executing a ground start if we booted with an air start)
|
2015-05-31 19:48:54 -03:00
|
|
|
uint8_t ground_start_count = 5;
|
2015-05-13 03:09:36 -03:00
|
|
|
|
|
|
|
// true if we have a position estimate from AHRS
|
|
|
|
bool have_position;
|
|
|
|
|
|
|
|
// Airspeed
|
|
|
|
// The calculated airspeed to use in FBW-B. Also used in higher modes for insuring min ground speed is met.
|
|
|
|
// Also used for flap deployment criteria. Centimeters per second.
|
|
|
|
int32_t target_airspeed_cm;
|
2021-02-23 12:32:06 -04:00
|
|
|
int32_t new_airspeed_cm = -1; //temp variable for AUTO and GUIDED mode speed changes
|
2015-05-13 03:09:36 -03:00
|
|
|
|
2016-05-10 16:44:16 -03:00
|
|
|
// The difference between current and desired airspeed. Used in the pitch controller. Meters per second.
|
|
|
|
float airspeed_error;
|
2015-05-13 03:09:36 -03:00
|
|
|
|
|
|
|
// An amount that the airspeed should be increased in auto modes based on the user positioning the
|
|
|
|
// throttle stick in the top half of the range. Centimeters per second.
|
|
|
|
int16_t airspeed_nudge_cm;
|
|
|
|
|
|
|
|
// Similar to airspeed_nudge, but used when no airspeed sensor.
|
|
|
|
// 0-(throttle_max - throttle_cruise) : throttle nudge in Auto mode using top 1/2 of throttle stick travel
|
|
|
|
int16_t throttle_nudge;
|
|
|
|
|
|
|
|
// Ground speed
|
|
|
|
// The amount current ground speed is below min ground speed. Centimeters per second
|
|
|
|
int32_t groundspeed_undershoot;
|
2023-06-02 03:08:20 -03:00
|
|
|
bool groundspeed_undershoot_is_valid;
|
2015-05-13 03:09:36 -03:00
|
|
|
|
|
|
|
// Difference between current altitude and desired altitude. Centimeters
|
|
|
|
int32_t altitude_error_cm;
|
|
|
|
|
2021-12-28 21:23:09 -04:00
|
|
|
// speed scaler for control surfaces, updated at 10Hz
|
|
|
|
float surface_speed_scaler = 1.0;
|
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
// Battery Sensors
|
2017-11-09 18:34:12 -04:00
|
|
|
AP_BattMonitor battery{MASK_LOG_CURRENT,
|
|
|
|
FUNCTOR_BIND_MEMBER(&Plane::handle_battery_failsafe, void, const char*, const int8_t),
|
|
|
|
_failsafe_priorities};
|
2015-05-13 03:09:36 -03:00
|
|
|
|
2016-05-13 08:34:45 -03:00
|
|
|
struct {
|
|
|
|
uint32_t last_tkoff_arm_time;
|
|
|
|
uint32_t last_check_ms;
|
2023-04-11 14:11:33 -03:00
|
|
|
uint32_t rudder_takeoff_warn_ms;
|
2016-05-13 08:34:45 -03:00
|
|
|
uint32_t last_report_ms;
|
|
|
|
bool launchTimerStarted;
|
2018-09-13 19:43:29 -03:00
|
|
|
uint8_t accel_event_counter;
|
|
|
|
uint32_t accel_event_ms;
|
2019-01-31 19:17:00 -04:00
|
|
|
uint32_t start_time_ms;
|
2023-04-11 14:11:33 -03:00
|
|
|
bool waiting_for_rudder_neutral;
|
2016-05-13 08:34:45 -03:00
|
|
|
} takeoff_state;
|
2019-09-09 16:49:52 -03:00
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
// ground steering controller state
|
|
|
|
struct {
|
|
|
|
// Direction held during phases of takeoff and landing centidegrees
|
|
|
|
// A value of -1 indicates the course has not been set/is not in use
|
|
|
|
// this is a 0..36000 value, or -1 for disabled
|
2019-09-09 16:49:52 -03:00
|
|
|
int32_t hold_course_cd = -1;
|
2015-05-13 03:09:36 -03:00
|
|
|
|
2021-02-23 12:32:06 -04:00
|
|
|
// locked_course and locked_course_cd are used in stabilize mode
|
2015-05-13 03:09:36 -03:00
|
|
|
// when ground steering is active, and for steering in auto-takeoff
|
|
|
|
bool locked_course;
|
|
|
|
float locked_course_err;
|
2021-12-03 03:27:16 -04:00
|
|
|
uint32_t last_steer_ms;
|
2019-09-09 16:49:52 -03:00
|
|
|
} steer_state;
|
2015-05-13 03:09:36 -03:00
|
|
|
|
|
|
|
// flight mode specific
|
|
|
|
struct {
|
|
|
|
// Altitude threshold to complete a takeoff command in autonomous
|
|
|
|
// modes. Centimeters above home
|
|
|
|
int32_t takeoff_altitude_rel_cm;
|
|
|
|
|
2016-03-09 18:22:53 -04:00
|
|
|
// Begin leveling out the enforced takeoff pitch angle min at this height to reduce/eliminate overshoot
|
|
|
|
int32_t height_below_takeoff_to_level_off_cm;
|
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
// the highest airspeed we have reached since entering AUTO. Used
|
|
|
|
// to control ground takeoff
|
|
|
|
float highest_airspeed;
|
2021-02-23 12:32:06 -04:00
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
// turn angle for next leg of mission
|
2015-05-31 19:48:54 -03:00
|
|
|
float next_turn_angle {90};
|
2015-05-13 03:09:36 -03:00
|
|
|
|
|
|
|
// filtered sink rate for landing
|
2015-06-13 06:11:49 -03:00
|
|
|
float sink_rate;
|
2015-05-13 03:09:36 -03:00
|
|
|
|
|
|
|
// time when we first pass min GPS speed on takeoff
|
|
|
|
uint32_t takeoff_speed_time_ms;
|
2021-02-23 12:32:06 -04:00
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
// distance to next waypoint
|
|
|
|
float wp_distance;
|
2021-02-23 12:32:06 -04:00
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
// proportion to next waypoint
|
|
|
|
float wp_proportion;
|
2021-02-23 12:32:06 -04:00
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
// last time is_flying() returned true in milliseconds
|
|
|
|
uint32_t last_flying_ms;
|
2015-08-17 18:14:10 -03:00
|
|
|
|
2015-06-03 16:22:24 -03:00
|
|
|
// time stamp of when we start flying while in auto mode in milliseconds
|
|
|
|
uint32_t started_flying_in_auto_ms;
|
2015-09-17 02:35:37 -03:00
|
|
|
|
|
|
|
// barometric altitude at start of takeoff
|
|
|
|
float baro_takeoff_alt;
|
2015-12-26 06:40:40 -04:00
|
|
|
|
2019-09-09 16:49:52 -03:00
|
|
|
// initial pitch. Used to detect if nose is rising in a tail dragger
|
|
|
|
int16_t initial_pitch_cd;
|
|
|
|
|
|
|
|
// Minimum pitch to hold during takeoff command execution. Hundredths of a degree
|
|
|
|
int16_t takeoff_pitch_cd;
|
|
|
|
|
|
|
|
// used to 'wiggle' servos in idle mode to prevent them freezing
|
|
|
|
// at high altitudes
|
|
|
|
uint8_t idle_wiggle_stage;
|
|
|
|
|
|
|
|
// Flag for using gps ground course instead of INS yaw. Set false when takeoff command in process.
|
|
|
|
bool takeoff_complete;
|
|
|
|
|
|
|
|
// are we headed to the land approach waypoint? Works for any nav type
|
|
|
|
bool wp_is_land_approach;
|
|
|
|
|
|
|
|
// should we fly inverted?
|
|
|
|
bool inverted_flight;
|
|
|
|
|
|
|
|
// should we enable cross-tracking for the next waypoint?
|
|
|
|
bool next_wp_crosstrack;
|
|
|
|
|
|
|
|
// should we use cross-tracking for this waypoint?
|
|
|
|
bool crosstrack;
|
|
|
|
|
|
|
|
// in FBWA taildragger takeoff mode
|
|
|
|
bool fbwa_tdrag_takeoff_mode;
|
|
|
|
|
|
|
|
// have we checked for an auto-land?
|
|
|
|
bool checked_for_autoland;
|
|
|
|
|
2016-05-11 02:14:43 -03:00
|
|
|
// are we in VTOL mode in AUTO?
|
2019-09-09 16:49:52 -03:00
|
|
|
bool vtol_mode;
|
2016-05-11 02:14:43 -03:00
|
|
|
|
2016-05-11 02:57:41 -03:00
|
|
|
// are we doing loiter mode as a VTOL?
|
2019-09-09 16:49:52 -03:00
|
|
|
bool vtol_loiter;
|
2019-09-14 19:32:12 -03:00
|
|
|
|
|
|
|
// how much correction have we added for terrain data
|
|
|
|
float terrain_correction;
|
2023-08-02 20:45:27 -03:00
|
|
|
|
|
|
|
// last home altitude for detecting changes
|
|
|
|
int32_t last_home_alt_cm;
|
2015-05-13 03:09:36 -03:00
|
|
|
} auto_state;
|
|
|
|
|
2021-11-15 01:08:34 -04:00
|
|
|
#if AP_SCRIPTING_ENABLED
|
2021-10-25 00:59:23 -03:00
|
|
|
// support for scripting nav commands, with verify
|
|
|
|
struct {
|
2022-02-06 22:23:17 -04:00
|
|
|
bool enabled;
|
2021-10-25 00:59:23 -03:00
|
|
|
uint16_t id;
|
|
|
|
float roll_rate_dps;
|
|
|
|
float pitch_rate_dps;
|
2021-11-25 19:18:13 -04:00
|
|
|
float yaw_rate_dps;
|
2021-10-25 00:59:23 -03:00
|
|
|
float throttle_pct;
|
|
|
|
uint32_t start_ms;
|
2022-02-06 22:23:17 -04:00
|
|
|
uint32_t current_ms;
|
2022-12-22 17:21:59 -04:00
|
|
|
float rudder_offset_pct;
|
|
|
|
bool run_yaw_rate_controller;
|
2021-10-25 00:59:23 -03:00
|
|
|
} nav_scripting;
|
|
|
|
#endif
|
|
|
|
|
2016-05-09 11:34:11 -03:00
|
|
|
struct {
|
2016-06-30 12:50:17 -03:00
|
|
|
// roll pitch yaw commanded from external controller in centidegrees
|
|
|
|
Vector3l forced_rpy_cd;
|
2016-05-09 11:34:11 -03:00
|
|
|
// last time we heard from the external controller
|
2016-06-30 12:50:17 -03:00
|
|
|
Vector3l last_forced_rpy_ms;
|
|
|
|
|
|
|
|
// throttle commanded from external controller in percent
|
|
|
|
float forced_throttle;
|
|
|
|
uint32_t last_forced_throttle_ms;
|
2020-05-16 01:02:01 -03:00
|
|
|
|
|
|
|
#if OFFBOARD_GUIDED == ENABLED
|
|
|
|
// airspeed adjustments
|
|
|
|
float target_airspeed_cm = -1; // don't default to zero here, as zero is a valid speed.
|
|
|
|
float target_airspeed_accel;
|
|
|
|
uint32_t target_airspeed_time_ms;
|
|
|
|
|
|
|
|
// altitude adjustments
|
|
|
|
float target_alt = -1; // don't default to zero here, as zero is a valid alt.
|
|
|
|
uint32_t last_target_alt = 0;
|
|
|
|
float target_alt_accel;
|
|
|
|
uint32_t target_alt_time_ms = 0;
|
|
|
|
uint8_t target_alt_frame = 0;
|
|
|
|
|
|
|
|
// heading track
|
|
|
|
float target_heading = -4; // don't default to zero or -1 here, as both are valid headings in radians
|
|
|
|
float target_heading_accel_limit;
|
|
|
|
uint32_t target_heading_time_ms;
|
|
|
|
guided_heading_type_t target_heading_type;
|
2023-09-23 14:56:42 -03:00
|
|
|
bool target_heading_limit;
|
2020-05-16 01:02:01 -03:00
|
|
|
#endif // OFFBOARD_GUIDED == ENABLED
|
2018-06-10 03:33:06 -03:00
|
|
|
} guided_state;
|
2016-05-09 11:34:11 -03:00
|
|
|
|
2022-10-01 07:21:38 -03:00
|
|
|
#if AP_LANDINGGEAR_ENABLED
|
2018-06-10 03:33:06 -03:00
|
|
|
// landing gear state
|
|
|
|
struct {
|
2022-09-29 20:10:41 -03:00
|
|
|
AP_FixedWing::FlightStage last_flight_stage;
|
2018-06-10 03:33:06 -03:00
|
|
|
} gear;
|
|
|
|
#endif
|
2021-02-23 12:32:06 -04:00
|
|
|
|
2015-08-20 17:44:32 -03:00
|
|
|
struct {
|
|
|
|
// on hard landings, only check once after directly a landing so you
|
|
|
|
// don't trigger a crash when picking up the aircraft
|
2019-09-09 16:49:52 -03:00
|
|
|
bool checkedHardLanding;
|
2015-08-20 17:44:32 -03:00
|
|
|
|
|
|
|
// crash detection. True when we are crashed
|
2019-09-09 16:49:52 -03:00
|
|
|
bool is_crashed;
|
2015-08-20 17:44:32 -03:00
|
|
|
|
2015-11-17 20:11:21 -04:00
|
|
|
// impact detection flag. Expires after a few seconds via impact_timer_ms
|
2019-09-09 16:49:52 -03:00
|
|
|
bool impact_detected;
|
2015-11-17 20:11:21 -04:00
|
|
|
|
2015-08-20 17:44:32 -03:00
|
|
|
// debounce timer
|
|
|
|
uint32_t debounce_timer_ms;
|
2015-11-17 20:11:21 -04:00
|
|
|
|
2016-03-28 18:52:30 -03:00
|
|
|
// delay time for debounce to count to
|
|
|
|
uint32_t debounce_time_total_ms;
|
|
|
|
|
2015-12-30 20:15:57 -04:00
|
|
|
// length of time impact_detected has been true. Times out after a few seconds. Used to clip isFlyingProbability
|
2015-11-17 20:11:21 -04:00
|
|
|
uint32_t impact_timer_ms;
|
2015-08-20 17:44:32 -03:00
|
|
|
} crash_state;
|
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
// this controls throttle suppression in auto modes
|
2016-11-21 20:09:32 -04:00
|
|
|
bool throttle_suppressed;
|
2021-02-23 12:32:06 -04:00
|
|
|
|
2016-01-05 19:34:21 -04:00
|
|
|
// reduce throttle to eliminate battery over-current
|
|
|
|
int8_t throttle_watt_limit_max;
|
|
|
|
int8_t throttle_watt_limit_min; // for reverse thrust
|
|
|
|
uint32_t throttle_watt_limit_timer_ms;
|
|
|
|
|
2022-09-29 20:10:41 -03:00
|
|
|
AP_FixedWing::FlightStage flight_stage = AP_FixedWing::FlightStage::NORMAL;
|
2015-05-13 03:09:36 -03:00
|
|
|
|
|
|
|
// probability of aircraft is currently in flight. range from 0 to
|
|
|
|
// 1 where 1 is 100% sure we're in flight
|
|
|
|
float isFlyingProbability;
|
|
|
|
|
2015-08-20 17:44:32 -03:00
|
|
|
// previous value of is_flying()
|
|
|
|
bool previous_is_flying;
|
|
|
|
|
2015-08-30 04:35:44 -03:00
|
|
|
// time since started flying in any mode in milliseconds
|
|
|
|
uint32_t started_flying_ms;
|
|
|
|
|
2021-10-29 07:37:52 -03:00
|
|
|
// ground mode is true when disarmed and not flying
|
|
|
|
bool ground_mode;
|
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
// Navigation control variables
|
|
|
|
// The instantaneous desired bank angle. Hundredths of a degree
|
|
|
|
int32_t nav_roll_cd;
|
|
|
|
|
|
|
|
// The instantaneous desired pitch angle. Hundredths of a degree
|
|
|
|
int32_t nav_pitch_cd;
|
|
|
|
|
2023-10-11 04:41:54 -03:00
|
|
|
// the aerodynamic load factor. This is calculated from the demanded
|
2015-05-13 03:09:36 -03:00
|
|
|
// roll before the roll is clipped, using 1/sqrt(cos(nav_roll))
|
2015-05-31 19:48:54 -03:00
|
|
|
float aerodynamic_load_factor = 1.0f;
|
2015-05-13 03:09:36 -03:00
|
|
|
|
|
|
|
// a smoothed airspeed estimate, used for limiting roll angle
|
|
|
|
float smoothed_airspeed;
|
|
|
|
|
|
|
|
// Mission library
|
2018-04-24 20:09:33 -03:00
|
|
|
AP_Mission mission{
|
2015-05-31 19:48:54 -03:00
|
|
|
FUNCTOR_BIND_MEMBER(&Plane::start_command_callback, bool, const AP_Mission::Mission_Command &),
|
|
|
|
FUNCTOR_BIND_MEMBER(&Plane::verify_command_callback, bool, const AP_Mission::Mission_Command &),
|
2017-12-12 21:06:15 -04:00
|
|
|
FUNCTOR_BIND_MEMBER(&Plane::exit_mission_callback, void)};
|
2015-05-31 19:48:54 -03:00
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
|
2015-06-18 07:45:33 -03:00
|
|
|
#if PARACHUTE == ENABLED
|
2022-12-22 19:55:03 -04:00
|
|
|
AP_Parachute parachute;
|
2015-06-18 07:45:33 -03:00
|
|
|
#endif
|
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
// terrain handling
|
|
|
|
#if AP_TERRAIN_AVAILABLE
|
2022-02-01 23:28:34 -04:00
|
|
|
AP_Terrain terrain;
|
2015-05-13 03:09:36 -03:00
|
|
|
#endif
|
|
|
|
|
2021-11-12 13:53:28 -04:00
|
|
|
AP_Landing landing{mission,ahrs,&TECS_controller,nav_controller,aparm,
|
2017-12-12 21:06:15 -04:00
|
|
|
FUNCTOR_BIND_MEMBER(&Plane::set_target_altitude_proportion, void, const Location&, float),
|
|
|
|
FUNCTOR_BIND_MEMBER(&Plane::constrain_target_altitude_location, void, const Location&, const Location&),
|
|
|
|
FUNCTOR_BIND_MEMBER(&Plane::adjusted_altitude_cm, int32_t),
|
|
|
|
FUNCTOR_BIND_MEMBER(&Plane::adjusted_relative_altitude_cm, int32_t),
|
|
|
|
FUNCTOR_BIND_MEMBER(&Plane::disarm_if_autoland_complete, void),
|
|
|
|
FUNCTOR_BIND_MEMBER(&Plane::update_flight_stage, void)};
|
2020-09-19 05:38:20 -03:00
|
|
|
#if HAL_ADSB_ENABLED
|
2018-01-01 16:41:53 -04:00
|
|
|
AP_ADSB adsb;
|
2015-11-19 14:24:55 -04:00
|
|
|
|
2020-09-22 14:00:39 -03:00
|
|
|
// avoidance of adsb enabled vehicles (normally manned vehicles)
|
2019-07-09 03:05:51 -03:00
|
|
|
AP_Avoidance_Plane avoidance_adsb{adsb};
|
2020-09-19 05:38:20 -03:00
|
|
|
#endif
|
2016-08-12 18:28:21 -03:00
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
// Outback Challenge Failsafe Support
|
2023-02-06 01:39:02 -04:00
|
|
|
#if AP_ADVANCEDFAILSAFE_ENABLED
|
2021-07-30 09:25:41 -03:00
|
|
|
AP_AdvancedFailsafe_Plane afs;
|
2019-01-08 21:05:40 -04:00
|
|
|
#endif
|
2015-05-13 03:09:36 -03:00
|
|
|
|
|
|
|
/*
|
|
|
|
meta data to support counting the number of circles in a loiter
|
|
|
|
*/
|
|
|
|
struct {
|
|
|
|
// previous target bearing, used to update sum_cd
|
|
|
|
int32_t old_target_bearing_cd;
|
|
|
|
|
2021-02-23 12:32:06 -04:00
|
|
|
// Total desired rotation in a loiter. Used for Loiter Turns commands.
|
2015-05-13 03:09:36 -03:00
|
|
|
int32_t total_cd;
|
|
|
|
|
|
|
|
// total angle completed in the loiter so far
|
|
|
|
int32_t sum_cd;
|
|
|
|
|
|
|
|
// Direction for loiter. 1 for clockwise, -1 for counter-clockwise
|
|
|
|
int8_t direction;
|
|
|
|
|
2017-02-21 07:56:32 -04:00
|
|
|
// when loitering and an altitude is involved, this flag is true when it has been reached at least once
|
|
|
|
bool reached_target_alt;
|
|
|
|
|
|
|
|
// check for scenarios where updrafts can keep you from loitering down indefinitely.
|
|
|
|
bool unable_to_acheive_target_alt;
|
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
// start time of the loiter. Milliseconds.
|
|
|
|
uint32_t start_time_ms;
|
|
|
|
|
2017-02-21 07:56:32 -04:00
|
|
|
// altitude at start of loiter loop lap. Used to detect delta alt of each lap.
|
|
|
|
// only valid when sum_cd > 36000
|
|
|
|
int32_t start_lap_alt_cm;
|
|
|
|
int32_t next_sum_lap_cd;
|
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
// The amount of time we should stay in a loiter for the Loiter Time command. Milliseconds.
|
|
|
|
uint32_t time_max_ms;
|
2023-05-15 21:18:12 -03:00
|
|
|
|
|
|
|
// current value of loiter radius in metres used by the controller
|
|
|
|
float radius;
|
2015-05-13 03:09:36 -03:00
|
|
|
} loiter;
|
|
|
|
|
|
|
|
// Conditional command
|
|
|
|
// A value used in condition commands (eg delay, change alt, etc.)
|
|
|
|
// For example in a change altitude command, it is the altitude to change to.
|
|
|
|
int32_t condition_value;
|
|
|
|
|
|
|
|
// A starting value used to check the status of a conditional command.
|
|
|
|
// For example in a delay command the condition_start records that start time for the delay
|
|
|
|
uint32_t condition_start;
|
|
|
|
// A value used in condition commands. For example the rate at which to change altitude.
|
|
|
|
int16_t condition_rate;
|
|
|
|
|
|
|
|
// 3D Location vectors
|
|
|
|
// Location structure defined in AP_Common
|
2023-02-02 18:58:40 -04:00
|
|
|
const Location &home = ahrs.get_home();
|
2015-05-13 03:09:36 -03:00
|
|
|
|
|
|
|
// The location of the previous waypoint. Used for track following and altitude ramp calculations
|
2015-06-20 00:26:34 -03:00
|
|
|
Location prev_WP_loc {};
|
2015-05-13 03:09:36 -03:00
|
|
|
|
|
|
|
// The plane's current location
|
2023-02-02 18:58:40 -04:00
|
|
|
Location current_loc {};
|
2015-05-13 03:09:36 -03:00
|
|
|
|
|
|
|
// The location of the current/active waypoint. Used for altitude ramp, track following and loiter calculations.
|
2015-06-20 00:26:34 -03:00
|
|
|
Location next_WP_loc {};
|
2015-05-13 03:09:36 -03:00
|
|
|
|
|
|
|
// Altitude control
|
|
|
|
struct {
|
|
|
|
// target altitude above sea level in cm. Used for barometric
|
|
|
|
// altitude navigation
|
|
|
|
int32_t amsl_cm;
|
|
|
|
|
|
|
|
// Altitude difference between previous and current waypoint in
|
|
|
|
// centimeters. Used for glide slope handling
|
|
|
|
int32_t offset_cm;
|
|
|
|
|
|
|
|
#if AP_TERRAIN_AVAILABLE
|
|
|
|
// are we trying to follow terrain?
|
|
|
|
bool terrain_following;
|
|
|
|
|
2023-10-03 03:33:54 -03:00
|
|
|
// are we waiting to load terrain data to init terrain following
|
|
|
|
bool terrain_following_pending;
|
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
// target altitude above terrain in cm, valid if terrain_following
|
|
|
|
// is set
|
|
|
|
int32_t terrain_alt_cm;
|
|
|
|
|
|
|
|
// lookahead value for height error reporting
|
|
|
|
float lookahead;
|
|
|
|
#endif
|
2017-05-09 05:05:36 -03:00
|
|
|
|
|
|
|
// last input for FBWB/CRUISE height control
|
|
|
|
float last_elevator_input;
|
|
|
|
|
|
|
|
// last time we checked for pilot control of height
|
|
|
|
uint32_t last_elev_check_us;
|
2015-06-20 00:26:34 -03:00
|
|
|
} target_altitude {};
|
2015-05-13 03:09:36 -03:00
|
|
|
|
2019-09-09 16:49:52 -03:00
|
|
|
float relative_altitude;
|
2017-01-30 15:48:22 -04:00
|
|
|
|
2018-02-09 19:09:24 -04:00
|
|
|
// loop performance monitoring:
|
|
|
|
AP::PerfInfo perf_info;
|
2016-10-15 07:41:55 -03:00
|
|
|
struct {
|
|
|
|
uint32_t last_trim_check;
|
|
|
|
uint32_t last_trim_save;
|
|
|
|
} auto_trim;
|
2016-10-13 20:58:51 -03:00
|
|
|
|
2020-05-14 21:12:02 -03:00
|
|
|
struct {
|
|
|
|
bool done_climb;
|
|
|
|
} rtl;
|
|
|
|
|
2016-10-13 20:58:51 -03:00
|
|
|
// last time home was updated while disarmed
|
|
|
|
uint32_t last_home_update_ms;
|
2017-08-29 16:48:10 -03:00
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
// Camera/Antenna mount tracking and stabilisation stuff
|
2020-07-24 14:23:23 -03:00
|
|
|
#if HAL_MOUNT_ENABLED
|
2019-03-26 08:37:00 -03:00
|
|
|
AP_Mount camera_mount;
|
2015-05-13 03:09:36 -03:00
|
|
|
#endif
|
|
|
|
|
2023-10-11 04:41:54 -03:00
|
|
|
// Arming/Disarming management class
|
2018-06-25 02:44:04 -03:00
|
|
|
AP_Arming_Plane arming;
|
2015-05-13 03:09:36 -03:00
|
|
|
|
2015-05-31 19:48:54 -03:00
|
|
|
AP_Param param_loader {var_info};
|
2015-05-13 03:09:36 -03:00
|
|
|
|
2023-08-08 21:45:57 -03:00
|
|
|
// dummy implementation of external control
|
|
|
|
#if AP_EXTERNAL_CONTROL_ENABLED
|
|
|
|
AP_ExternalControl external_control;
|
|
|
|
#endif
|
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
static const AP_Scheduler::Task scheduler_tasks[];
|
|
|
|
static const AP_Param::Info var_info[];
|
|
|
|
|
2015-07-23 08:47:37 -03:00
|
|
|
// time that rudder arming has been running
|
2016-07-12 02:02:30 -03:00
|
|
|
uint32_t rudder_arm_timer;
|
2015-05-13 03:09:36 -03:00
|
|
|
|
2023-01-16 01:46:22 -04:00
|
|
|
// have we seen neutral rudder since arming with rudder?
|
|
|
|
bool seen_neutral_rudder;
|
|
|
|
|
2021-09-10 03:28:21 -03:00
|
|
|
#if HAL_QUADPLANE_ENABLED
|
2015-11-24 04:24:04 -04:00
|
|
|
// support for quadcopter-plane
|
|
|
|
QuadPlane quadplane{ahrs};
|
2021-09-10 03:28:21 -03:00
|
|
|
#endif
|
2016-01-21 22:48:28 -04:00
|
|
|
|
2023-09-19 06:51:08 -03:00
|
|
|
#if AP_TUNING_ENABLED
|
2016-04-16 07:26:43 -03:00
|
|
|
// support for transmitter tuning
|
2016-05-06 01:43:39 -03:00
|
|
|
AP_Tuning_Plane tuning;
|
2023-09-19 06:51:08 -03:00
|
|
|
#endif
|
2016-04-16 07:26:43 -03:00
|
|
|
|
|
|
|
static const struct LogStructure log_structure[];
|
2018-12-29 11:42:22 -04:00
|
|
|
|
|
|
|
// rudder mixing gain for differential thrust (0 - 1)
|
|
|
|
float rudder_dt;
|
|
|
|
|
2019-06-10 14:07:24 -03:00
|
|
|
// soaring mode-change timer
|
2020-06-26 14:04:19 -03:00
|
|
|
uint32_t soaring_mode_timer_ms;
|
2019-06-10 14:07:24 -03:00
|
|
|
|
2020-07-07 09:25:21 -03:00
|
|
|
// terrain disable for non AUTO modes, set with an RC Option switch
|
|
|
|
bool non_auto_terrain_disable;
|
|
|
|
bool terrain_disabled();
|
2021-03-19 20:16:25 -03:00
|
|
|
#if AP_TERRAIN_AVAILABLE
|
|
|
|
bool terrain_enabled_in_current_mode() const;
|
2021-09-18 20:12:33 -03:00
|
|
|
bool terrain_enabled_in_mode(Mode::Number num) const;
|
2021-03-19 20:16:25 -03:00
|
|
|
enum class terrain_bitmask {
|
|
|
|
ALL = 1U << 0,
|
|
|
|
FLY_BY_WIRE_B = 1U << 1,
|
|
|
|
CRUISE = 1U << 2,
|
|
|
|
AUTO = 1U << 3,
|
|
|
|
RTL = 1U << 4,
|
|
|
|
AVOID_ADSB = 1U << 5,
|
|
|
|
GUIDED = 1U << 6,
|
|
|
|
LOITER = 1U << 7,
|
|
|
|
CIRCLE = 1U << 8,
|
|
|
|
QRTL = 1U << 9,
|
|
|
|
QLAND = 1U << 10,
|
|
|
|
QLOITER = 1U << 11,
|
|
|
|
};
|
|
|
|
struct TerrainLookupTable{
|
|
|
|
Mode::Number mode_num;
|
|
|
|
terrain_bitmask bitmask;
|
|
|
|
};
|
|
|
|
static const TerrainLookupTable Terrain_lookup[];
|
|
|
|
#endif
|
2020-07-07 09:25:21 -03:00
|
|
|
|
2020-06-01 16:26:38 -03:00
|
|
|
// Attitude.cpp
|
2015-05-13 03:09:36 -03:00
|
|
|
void adjust_nav_pitch_throttle(void);
|
|
|
|
void update_load_factor(void);
|
|
|
|
void adjust_altitude_target();
|
|
|
|
void setup_glide_slope(void);
|
2021-06-01 03:03:52 -03:00
|
|
|
int32_t get_RTL_altitude_cm() const;
|
2016-06-03 18:10:26 -03:00
|
|
|
float relative_ground_altitude(bool use_rangefinder_if_available);
|
2023-01-21 20:37:58 -04:00
|
|
|
float relative_ground_altitude(bool use_rangefinder_if_available, bool use_terrain_if_available);
|
2015-05-13 03:09:36 -03:00
|
|
|
void set_target_altitude_current(void);
|
|
|
|
void set_target_altitude_current_adjusted(void);
|
|
|
|
void set_target_altitude_location(const Location &loc);
|
|
|
|
int32_t relative_target_altitude_cm(void);
|
|
|
|
void change_target_altitude(int32_t change_cm);
|
|
|
|
void set_target_altitude_proportion(const Location &loc, float proportion);
|
|
|
|
void constrain_target_altitude_location(const Location &loc1, const Location &loc2);
|
|
|
|
int32_t calc_altitude_error_cm(void);
|
2022-01-28 11:31:33 -04:00
|
|
|
void check_fbwb_altitude(void);
|
2015-05-13 03:09:36 -03:00
|
|
|
void reset_offset_altitude(void);
|
2021-11-10 14:42:59 -04:00
|
|
|
void set_offset_altitude_location(const Location &start_loc, const Location &destination_loc);
|
2015-05-13 03:09:36 -03:00
|
|
|
bool above_location_current(const Location &loc);
|
2023-10-03 03:33:54 -03:00
|
|
|
void setup_terrain_target_alt(Location &loc);
|
2015-05-13 03:09:36 -03:00
|
|
|
int32_t adjusted_altitude_cm(void);
|
|
|
|
int32_t adjusted_relative_altitude_cm(void);
|
2016-07-19 20:09:21 -03:00
|
|
|
float mission_alt_offset(void);
|
2015-05-13 03:09:36 -03:00
|
|
|
float height_above_target(void);
|
|
|
|
float lookahead_adjustment(void);
|
|
|
|
float rangefinder_correction(void);
|
|
|
|
void rangefinder_height_update(void);
|
2019-04-06 01:53:16 -03:00
|
|
|
void rangefinder_terrain_correction(float &height);
|
2020-06-01 16:26:38 -03:00
|
|
|
void stabilize();
|
|
|
|
void calc_throttle();
|
|
|
|
void calc_nav_roll();
|
|
|
|
void calc_nav_pitch();
|
2021-12-28 21:23:09 -04:00
|
|
|
float calc_speed_scaler(void);
|
|
|
|
float get_speed_scaler(void) const { return surface_speed_scaler; }
|
2020-06-01 16:26:38 -03:00
|
|
|
bool stick_mixing_enabled(void);
|
2023-02-01 07:17:27 -04:00
|
|
|
void stabilize_roll();
|
|
|
|
float stabilize_roll_get_roll_out();
|
|
|
|
void stabilize_pitch();
|
|
|
|
float stabilize_pitch_get_pitch_out();
|
2020-06-01 16:26:38 -03:00
|
|
|
void stabilize_stick_mixing_fbw();
|
2023-02-01 07:17:27 -04:00
|
|
|
void stabilize_yaw();
|
2023-07-05 20:01:16 -03:00
|
|
|
int16_t calc_nav_yaw_coordinated();
|
|
|
|
int16_t calc_nav_yaw_course(void);
|
|
|
|
int16_t calc_nav_yaw_ground(void);
|
2020-06-01 16:26:38 -03:00
|
|
|
|
|
|
|
// Log.cpp
|
2021-08-09 03:21:18 -03:00
|
|
|
uint32_t last_log_fast_ms;
|
|
|
|
|
2022-07-28 23:42:21 -03:00
|
|
|
void Log_Write_FullRate(void);
|
2020-06-01 16:26:38 -03:00
|
|
|
void Log_Write_Attitude(void);
|
|
|
|
void Log_Write_Control_Tuning();
|
|
|
|
void Log_Write_OFG_Guided();
|
|
|
|
void Log_Write_Guided(void);
|
|
|
|
void Log_Write_Nav_Tuning();
|
|
|
|
void Log_Write_Status();
|
|
|
|
void Log_Write_RC(void);
|
|
|
|
void Log_Write_Vehicle_Startup_Messages();
|
|
|
|
void Log_Write_AETR();
|
|
|
|
void log_init();
|
|
|
|
|
|
|
|
// Parameters.cpp
|
|
|
|
void load_parameters(void) override;
|
|
|
|
|
|
|
|
// commands_logic.cpp
|
2023-02-02 18:58:40 -04:00
|
|
|
void set_next_WP(const Location &loc);
|
2016-04-29 02:31:08 -03:00
|
|
|
void do_RTL(int32_t alt);
|
2015-05-13 03:09:36 -03:00
|
|
|
bool verify_takeoff();
|
2018-11-16 17:52:48 -04:00
|
|
|
bool verify_loiter_unlim(const AP_Mission::Mission_Command &cmd);
|
2015-05-13 03:09:36 -03:00
|
|
|
bool verify_loiter_time();
|
2018-11-16 17:52:48 -04:00
|
|
|
bool verify_loiter_turns(const AP_Mission::Mission_Command &cmd);
|
|
|
|
bool verify_loiter_to_alt(const AP_Mission::Mission_Command &cmd);
|
2015-05-13 03:09:36 -03:00
|
|
|
bool verify_RTL();
|
|
|
|
bool verify_continue_and_change_alt();
|
|
|
|
bool verify_wait_delay();
|
|
|
|
bool verify_within_distance();
|
2015-06-13 06:12:54 -03:00
|
|
|
bool verify_altitude_wait(const AP_Mission::Mission_Command &cmd);
|
2015-05-13 03:09:36 -03:00
|
|
|
void do_loiter_at_location();
|
2016-03-02 12:42:27 -04:00
|
|
|
bool verify_loiter_heading(bool init);
|
2015-05-13 03:09:36 -03:00
|
|
|
void exit_mission_callback();
|
2020-06-01 16:26:38 -03:00
|
|
|
bool start_command(const AP_Mission::Mission_Command& cmd);
|
|
|
|
bool verify_command(const AP_Mission::Mission_Command& cmd);
|
|
|
|
void do_takeoff(const AP_Mission::Mission_Command& cmd);
|
|
|
|
void do_nav_wp(const AP_Mission::Mission_Command& cmd);
|
|
|
|
void do_land(const AP_Mission::Mission_Command& cmd);
|
2021-09-10 03:28:21 -03:00
|
|
|
#if HAL_QUADPLANE_ENABLED
|
2020-06-01 16:26:38 -03:00
|
|
|
void do_landing_vtol_approach(const AP_Mission::Mission_Command& cmd);
|
2021-09-10 03:28:21 -03:00
|
|
|
#endif
|
2020-06-01 16:26:38 -03:00
|
|
|
void loiter_set_direction_wp(const AP_Mission::Mission_Command& cmd);
|
|
|
|
void do_loiter_unlimited(const AP_Mission::Mission_Command& cmd);
|
|
|
|
void do_loiter_turns(const AP_Mission::Mission_Command& cmd);
|
|
|
|
void do_loiter_time(const AP_Mission::Mission_Command& cmd);
|
|
|
|
void do_continue_and_change_alt(const AP_Mission::Mission_Command& cmd);
|
|
|
|
void do_loiter_to_alt(const AP_Mission::Mission_Command& cmd);
|
|
|
|
void do_vtol_takeoff(const AP_Mission::Mission_Command& cmd);
|
|
|
|
void do_vtol_land(const AP_Mission::Mission_Command& cmd);
|
|
|
|
bool verify_nav_wp(const AP_Mission::Mission_Command& cmd);
|
2021-09-10 03:28:21 -03:00
|
|
|
#if HAL_QUADPLANE_ENABLED
|
2020-06-01 16:26:38 -03:00
|
|
|
bool verify_landing_vtol_approach(const AP_Mission::Mission_Command& cmd);
|
2021-09-10 03:28:21 -03:00
|
|
|
#endif
|
2020-06-01 16:26:38 -03:00
|
|
|
void do_wait_delay(const AP_Mission::Mission_Command& cmd);
|
|
|
|
void do_within_distance(const AP_Mission::Mission_Command& cmd);
|
2020-10-04 15:20:47 -03:00
|
|
|
bool do_change_speed(const AP_Mission::Mission_Command& cmd);
|
2020-06-01 16:26:38 -03:00
|
|
|
void do_set_home(const AP_Mission::Mission_Command& cmd);
|
|
|
|
bool start_command_callback(const AP_Mission::Mission_Command &cmd);
|
|
|
|
bool verify_command_callback(const AP_Mission::Mission_Command& cmd);
|
2021-08-30 15:25:45 -03:00
|
|
|
float get_wp_radius() const;
|
2020-06-01 16:26:38 -03:00
|
|
|
|
2023-01-17 02:47:03 -04:00
|
|
|
bool is_land_command(uint16_t cmd) const;
|
|
|
|
|
2023-08-29 18:41:10 -03:00
|
|
|
bool do_change_speed(uint8_t speedtype, float speed_target_ms, float rhtottle_pct);
|
2023-01-17 02:47:03 -04:00
|
|
|
/*
|
|
|
|
return true if in a specific AUTO mission command
|
|
|
|
*/
|
|
|
|
bool in_auto_mission_id(uint16_t command) const;
|
2022-12-29 01:55:13 -04:00
|
|
|
|
2021-11-15 01:08:34 -04:00
|
|
|
#if AP_SCRIPTING_ENABLED
|
2021-10-25 00:59:23 -03:00
|
|
|
// nav scripting support
|
|
|
|
void do_nav_script_time(const AP_Mission::Mission_Command& cmd);
|
|
|
|
bool verify_nav_script_time(const AP_Mission::Mission_Command& cmd);
|
|
|
|
#endif
|
|
|
|
|
2020-06-01 16:26:38 -03:00
|
|
|
// commands.cpp
|
2022-02-09 19:44:58 -04:00
|
|
|
void set_guided_WP(const Location &loc);
|
2023-02-16 18:03:22 -04:00
|
|
|
|
|
|
|
// update home position. Return true if update done
|
|
|
|
bool update_home();
|
|
|
|
|
|
|
|
// update current_loc
|
|
|
|
void update_current_loc(void);
|
|
|
|
|
2020-06-01 16:26:38 -03:00
|
|
|
// set home location and store it persistently:
|
|
|
|
bool set_home_persistently(const Location &loc) WARN_IF_UNUSED;
|
|
|
|
|
|
|
|
// control_modes.cpp
|
2015-05-13 03:09:36 -03:00
|
|
|
void read_control_switch();
|
2021-02-01 12:26:22 -04:00
|
|
|
uint8_t readSwitch(void) const;
|
2015-05-13 03:09:36 -03:00
|
|
|
void autotune_start(void);
|
|
|
|
void autotune_restore(void);
|
2015-06-13 06:16:02 -03:00
|
|
|
void autotune_enable(bool enable);
|
2015-05-13 03:09:36 -03:00
|
|
|
bool fly_inverted(void);
|
2023-01-26 16:59:58 -04:00
|
|
|
bool mode_allows_autotuning(void);
|
2020-06-01 16:26:38 -03:00
|
|
|
uint8_t get_mode() const override { return (uint8_t)control_mode->mode_number(); }
|
|
|
|
Mode *mode_from_mode_num(const enum Mode::Number num);
|
2023-04-12 02:23:26 -03:00
|
|
|
bool current_mode_requires_mission() const override {
|
|
|
|
return control_mode == &mode_auto;
|
|
|
|
}
|
|
|
|
|
2023-01-26 16:59:58 -04:00
|
|
|
bool autotuning;
|
2020-06-01 16:26:38 -03:00
|
|
|
|
|
|
|
// events.cpp
|
2019-10-17 00:49:32 -03:00
|
|
|
void failsafe_short_on_event(enum failsafe_state fstype, ModeReason reason);
|
|
|
|
void failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason);
|
|
|
|
void failsafe_short_off_event(ModeReason reason);
|
|
|
|
void failsafe_long_off_event(ModeReason reason);
|
2017-11-09 18:34:12 -04:00
|
|
|
void handle_battery_failsafe(const char* type_str, const int8_t action);
|
2021-09-10 03:28:21 -03:00
|
|
|
bool failsafe_in_landing_sequence() const; // returns true if the vehicle is in landing sequence. Intended only for use in failsafe code.
|
2020-06-01 16:26:38 -03:00
|
|
|
|
2022-07-19 08:33:13 -03:00
|
|
|
#if AP_FENCE_ENABLED
|
2020-09-10 03:31:18 -03:00
|
|
|
// fence.cpp
|
|
|
|
void fence_check();
|
|
|
|
bool fence_stickmixing() const;
|
2022-08-12 16:34:31 -03:00
|
|
|
bool in_fence_recovery() const;
|
2020-09-10 03:31:18 -03:00
|
|
|
#endif
|
|
|
|
|
2020-06-01 16:26:38 -03:00
|
|
|
// ArduPlane.cpp
|
2015-05-13 03:09:36 -03:00
|
|
|
void disarm_if_autoland_complete();
|
2023-03-13 22:30:39 -03:00
|
|
|
bool trigger_land_abort(const float climb_to_alt_m);
|
2021-08-01 15:34:37 -03:00
|
|
|
void get_osd_roll_pitch_rad(float &roll, float &pitch) const override;
|
2015-05-13 03:09:36 -03:00
|
|
|
float tecs_hgt_afe(void);
|
2020-06-01 16:26:38 -03:00
|
|
|
void efi_update(void);
|
|
|
|
void get_scheduler_tasks(const AP_Scheduler::Task *&tasks,
|
|
|
|
uint8_t &task_count,
|
|
|
|
uint32_t &log_bit) override;
|
|
|
|
void ahrs_update();
|
|
|
|
void update_speed_height(void);
|
|
|
|
void update_GPS_50Hz(void);
|
|
|
|
void update_GPS_10Hz(void);
|
|
|
|
void update_compass(void);
|
|
|
|
void update_alt(void);
|
2023-02-06 01:39:02 -04:00
|
|
|
#if AP_ADVANCEDFAILSAFE_ENABLED
|
2020-06-01 16:26:38 -03:00
|
|
|
void afs_fs_check(void);
|
|
|
|
#endif
|
|
|
|
void one_second_loop(void);
|
2020-09-10 03:31:18 -03:00
|
|
|
void three_hz_loop(void);
|
2020-11-24 10:13:10 -04:00
|
|
|
#if AP_AIRSPEED_AUTOCAL_ENABLE
|
2020-06-01 16:26:38 -03:00
|
|
|
void airspeed_ratio_update(void);
|
2021-02-23 12:32:06 -04:00
|
|
|
#endif
|
2020-06-01 16:26:38 -03:00
|
|
|
void compass_save(void);
|
2022-07-28 23:42:21 -03:00
|
|
|
void update_logging10(void);
|
|
|
|
void update_logging25(void);
|
2020-06-01 16:26:38 -03:00
|
|
|
void update_control_mode(void);
|
2021-09-10 03:28:21 -03:00
|
|
|
void update_fly_forward(void);
|
2020-06-01 16:26:38 -03:00
|
|
|
void update_flight_stage();
|
2022-09-29 20:10:41 -03:00
|
|
|
void set_flight_stage(AP_FixedWing::FlightStage fs);
|
2023-04-26 00:27:13 -03:00
|
|
|
bool flight_option_enabled(FlightOptions flight_option) const;
|
2020-06-01 16:26:38 -03:00
|
|
|
|
|
|
|
// navigation.cpp
|
2015-05-13 03:09:36 -03:00
|
|
|
void loiter_angle_reset(void);
|
|
|
|
void loiter_angle_update(void);
|
|
|
|
void navigate();
|
2023-08-02 20:45:27 -03:00
|
|
|
void check_home_alt_change(void);
|
2015-05-13 03:09:36 -03:00
|
|
|
void calc_airspeed_errors();
|
2021-09-10 03:28:21 -03:00
|
|
|
float mode_auto_target_airspeed_cm();
|
2015-05-13 03:09:36 -03:00
|
|
|
void calc_gndspeed_undershoot();
|
2016-01-19 00:04:30 -04:00
|
|
|
void update_loiter(uint16_t radius);
|
2021-09-10 03:28:21 -03:00
|
|
|
void update_loiter_update_nav(uint16_t radius);
|
2015-05-13 03:09:36 -03:00
|
|
|
void update_cruise();
|
|
|
|
void update_fbwb_speed_height(void);
|
|
|
|
void setup_turn_angle(void);
|
2016-05-07 21:40:42 -03:00
|
|
|
bool reached_loiter_target(void);
|
2020-06-01 16:26:38 -03:00
|
|
|
|
|
|
|
// radio.cpp
|
2020-02-11 21:01:17 -04:00
|
|
|
void set_control_channels(void) override;
|
2015-05-13 03:09:36 -03:00
|
|
|
void init_rc_in();
|
2016-06-30 03:48:30 -03:00
|
|
|
void init_rc_out_main();
|
|
|
|
void init_rc_out_aux();
|
2015-07-16 14:47:08 -03:00
|
|
|
void rudder_arm_disarm_check();
|
2015-05-13 03:09:36 -03:00
|
|
|
void read_radio();
|
2018-08-20 21:38:41 -03:00
|
|
|
int16_t rudder_input(void);
|
2017-10-27 17:23:28 -03:00
|
|
|
void control_failsafe();
|
2021-09-21 18:41:35 -03:00
|
|
|
void trim_radio();
|
2019-08-25 23:57:39 -03:00
|
|
|
bool rc_throttle_value_ok(void) const;
|
2018-08-20 21:38:41 -03:00
|
|
|
bool rc_failsafe_active(void) const;
|
2020-06-01 16:26:38 -03:00
|
|
|
|
|
|
|
// sensors.cpp
|
2015-05-13 03:09:36 -03:00
|
|
|
void read_rangefinder(void);
|
2020-06-01 16:26:38 -03:00
|
|
|
|
|
|
|
// system.cpp
|
2020-01-16 06:31:56 -04:00
|
|
|
void init_ardupilot() override;
|
2015-05-13 03:09:36 -03:00
|
|
|
void startup_ground(void);
|
2019-10-17 00:49:32 -03:00
|
|
|
bool set_mode(Mode& new_mode, const ModeReason reason);
|
|
|
|
bool set_mode(const uint8_t mode, const ModeReason reason) override;
|
|
|
|
bool set_mode_by_number(const Mode::Number new_mode_number, const ModeReason reason);
|
2015-05-13 03:09:36 -03:00
|
|
|
void check_long_failsafe();
|
|
|
|
void check_short_failsafe();
|
|
|
|
void startup_INS_ground(void);
|
|
|
|
bool should_log(uint32_t mask);
|
2016-01-30 02:38:31 -04:00
|
|
|
int8_t throttle_percentage(void);
|
2020-06-01 16:26:38 -03:00
|
|
|
void notify_mode(const Mode& mode);
|
2023-06-18 12:31:10 -03:00
|
|
|
bool gcs_mode_enabled(const Mode::Number mode_num) const;
|
2020-06-01 16:26:38 -03:00
|
|
|
|
|
|
|
// takeoff.cpp
|
2015-05-13 03:09:36 -03:00
|
|
|
bool auto_takeoff_check(void);
|
|
|
|
void takeoff_calc_roll(void);
|
|
|
|
void takeoff_calc_pitch(void);
|
|
|
|
int8_t takeoff_tail_hold(void);
|
2016-03-09 18:22:53 -04:00
|
|
|
int16_t get_takeoff_pitch_min_cd(void);
|
2018-11-09 00:40:20 -04:00
|
|
|
void landing_gear_update(void);
|
2020-06-01 16:26:38 -03:00
|
|
|
|
|
|
|
// avoidance_adsb.cpp
|
2016-08-12 18:28:21 -03:00
|
|
|
void avoidance_adsb_update(void);
|
2020-06-01 16:26:38 -03:00
|
|
|
|
|
|
|
// servos.cpp
|
2015-06-13 06:12:54 -03:00
|
|
|
void set_servos_idle(void);
|
2015-05-13 03:09:36 -03:00
|
|
|
void set_servos();
|
2016-10-09 05:14:17 -03:00
|
|
|
void set_servos_controlled(void);
|
2023-10-25 20:25:23 -03:00
|
|
|
void set_takeoff_expected(void);
|
2016-10-09 05:14:17 -03:00
|
|
|
void set_servos_old_elevons(void);
|
2016-10-09 05:21:12 -03:00
|
|
|
void set_servos_flaps(void);
|
2018-06-10 03:33:06 -03:00
|
|
|
void set_landing_gear(void);
|
2017-07-01 03:02:09 -03:00
|
|
|
void dspoiler_update(void);
|
2018-11-12 11:41:22 -04:00
|
|
|
void airbrake_update(void);
|
2021-09-16 14:23:06 -03:00
|
|
|
void landing_neutral_control_surface_servos(void);
|
2016-10-11 01:23:14 -03:00
|
|
|
void servos_output(void);
|
2016-10-15 07:41:55 -03:00
|
|
|
void servos_auto_trim(void);
|
2017-02-12 21:03:22 -04:00
|
|
|
void servos_twin_engine_mix();
|
2020-10-02 21:52:56 -03:00
|
|
|
void force_flare();
|
2021-02-01 12:26:22 -04:00
|
|
|
void throttle_voltage_comp(int8_t &min_throttle, int8_t &max_throttle) const;
|
2016-10-09 05:14:17 -03:00
|
|
|
void throttle_watt_limiter(int8_t &min_throttle, int8_t &max_throttle);
|
2019-02-01 21:24:00 -04:00
|
|
|
void throttle_slew_limit(SRV_Channel::Aux_servo_function_t func);
|
2015-05-13 03:09:36 -03:00
|
|
|
bool suppress_throttle(void);
|
2018-04-10 19:07:24 -03:00
|
|
|
void update_throttle_hover();
|
2017-04-19 08:33:35 -03:00
|
|
|
void channel_function_mixer(SRV_Channel::Aux_servo_function_t func1_in, SRV_Channel::Aux_servo_function_t func2_in,
|
2021-02-01 12:26:22 -04:00
|
|
|
SRV_Channel::Aux_servo_function_t func1_out, SRV_Channel::Aux_servo_function_t func2_out) const;
|
2022-01-06 21:34:34 -04:00
|
|
|
void flaperon_update();
|
2023-04-11 14:11:33 -03:00
|
|
|
void indicate_waiting_for_rud_neutral_to_takeoff(void);
|
2020-06-01 16:26:38 -03:00
|
|
|
|
|
|
|
// is_flying.cpp
|
|
|
|
void update_is_flying_5Hz(void);
|
|
|
|
void crash_detection_update(void);
|
|
|
|
bool in_preLaunch_flight_stage(void);
|
|
|
|
bool is_flying(void);
|
|
|
|
|
|
|
|
// parachute.cpp
|
2015-06-18 07:45:33 -03:00
|
|
|
void parachute_check();
|
2017-01-02 23:58:07 -04:00
|
|
|
#if PARACHUTE == ENABLED
|
|
|
|
void do_parachute(const AP_Mission::Mission_Command& cmd);
|
2015-06-18 07:45:33 -03:00
|
|
|
void parachute_release();
|
2015-10-27 01:17:13 -03:00
|
|
|
bool parachute_manual_release();
|
2018-07-25 20:47:06 -03:00
|
|
|
#endif
|
2020-06-01 16:26:38 -03:00
|
|
|
|
|
|
|
// soaring.cpp
|
2020-09-23 05:16:45 -03:00
|
|
|
#if HAL_SOARING_ENABLED
|
2017-02-26 19:18:38 -04:00
|
|
|
void update_soaring();
|
2018-07-29 19:57:55 -03:00
|
|
|
#endif
|
2015-06-18 07:45:33 -03:00
|
|
|
|
2021-10-24 02:20:43 -03:00
|
|
|
// RC_Channel.cpp
|
|
|
|
bool emergency_landing;
|
|
|
|
|
2020-12-05 14:44:48 -04:00
|
|
|
// vehicle specific waypoint info helpers
|
|
|
|
bool get_wp_distance_m(float &distance) const override;
|
|
|
|
bool get_wp_bearing_deg(float &bearing) const override;
|
|
|
|
bool get_wp_crosstrack_error_m(float &xtrack_error) const override;
|
|
|
|
|
2020-06-01 16:26:38 -03:00
|
|
|
// reverse_thrust.cpp
|
2018-11-09 19:00:20 -04:00
|
|
|
bool reversed_throttle;
|
|
|
|
bool have_reverse_throttle_rc_option;
|
2018-11-09 18:38:43 -04:00
|
|
|
bool allow_reverse_thrust(void) const;
|
|
|
|
bool have_reverse_thrust(void) const;
|
2021-09-18 15:03:54 -03:00
|
|
|
float get_throttle_input(bool no_deadzone=false) const;
|
|
|
|
float get_adjusted_throttle_input(bool no_deadzone=false) const;
|
2018-11-09 18:38:43 -04:00
|
|
|
|
2021-11-15 01:08:34 -04:00
|
|
|
#if AP_SCRIPTING_ENABLED
|
2021-10-25 00:59:23 -03:00
|
|
|
// support for NAV_SCRIPT_TIME mission command
|
2022-10-26 19:50:10 -03:00
|
|
|
bool nav_scripting_active(void);
|
2022-10-13 21:37:52 -03:00
|
|
|
bool nav_script_time(uint16_t &id, uint8_t &cmd, float &arg1, float &arg2, int16_t &arg3, int16_t &arg4) override;
|
2021-10-25 00:59:23 -03:00
|
|
|
void nav_script_time_done(uint16_t id) override;
|
|
|
|
|
|
|
|
// command throttle percentage and roll, pitch, yaw target
|
|
|
|
// rates. For use with scripting controllers
|
2022-02-07 16:29:39 -04:00
|
|
|
void set_target_throttle_rate_rpy(float throttle_pct, float roll_rate_dps, float pitch_rate_dps, float yaw_rate_dps) override;
|
2022-12-22 17:21:59 -04:00
|
|
|
void set_rudder_offset(float rudder_pct, bool run_yaw_rate_controller) override;
|
2022-02-06 22:23:17 -04:00
|
|
|
bool nav_scripting_enable(uint8_t mode) override;
|
2021-10-25 00:59:23 -03:00
|
|
|
#endif
|
2022-02-06 22:23:17 -04:00
|
|
|
|
2017-11-09 18:34:12 -04:00
|
|
|
enum Failsafe_Action {
|
|
|
|
Failsafe_Action_None = 0,
|
|
|
|
Failsafe_Action_RTL = 1,
|
|
|
|
Failsafe_Action_Land = 2,
|
2018-11-23 19:00:31 -04:00
|
|
|
Failsafe_Action_Terminate = 3,
|
2021-09-10 03:28:21 -03:00
|
|
|
#if HAL_QUADPLANE_ENABLED
|
2018-11-23 19:00:31 -04:00
|
|
|
Failsafe_Action_QLand = 4,
|
2021-09-10 03:28:21 -03:00
|
|
|
#endif
|
2021-09-27 16:39:18 -03:00
|
|
|
Failsafe_Action_Parachute = 5,
|
|
|
|
#if HAL_QUADPLANE_ENABLED
|
|
|
|
Failsafe_Action_Loiter_alt_QLand = 6,
|
|
|
|
#endif
|
2017-11-09 18:34:12 -04:00
|
|
|
};
|
|
|
|
|
|
|
|
// list of priorities, highest priority first
|
|
|
|
static constexpr int8_t _failsafe_priorities[] = {
|
|
|
|
Failsafe_Action_Terminate,
|
2019-02-11 10:20:38 -04:00
|
|
|
Failsafe_Action_Parachute,
|
2021-09-10 03:28:21 -03:00
|
|
|
#if HAL_QUADPLANE_ENABLED
|
2018-11-23 19:00:31 -04:00
|
|
|
Failsafe_Action_QLand,
|
2021-09-10 03:28:21 -03:00
|
|
|
#endif
|
2017-11-09 18:34:12 -04:00
|
|
|
Failsafe_Action_Land,
|
|
|
|
Failsafe_Action_RTL,
|
|
|
|
Failsafe_Action_None,
|
|
|
|
-1 // the priority list must end with a sentinel of -1
|
|
|
|
};
|
|
|
|
static_assert(_failsafe_priorities[ARRAY_SIZE(_failsafe_priorities) - 1] == -1,
|
|
|
|
"_failsafe_priorities is missing the sentinel");
|
|
|
|
|
2020-05-04 20:12:14 -03:00
|
|
|
// EKF checks for loss of navigation performed in ekf_check.cpp
|
|
|
|
// These are specific to VTOL operation
|
|
|
|
void ekf_check();
|
|
|
|
bool ekf_over_threshold();
|
|
|
|
void failsafe_ekf_event();
|
|
|
|
void failsafe_ekf_off_event(void);
|
2021-02-23 12:32:06 -04:00
|
|
|
|
2020-07-15 16:22:50 -03:00
|
|
|
enum class CrowMode {
|
|
|
|
NORMAL,
|
|
|
|
PROGRESSIVE,
|
|
|
|
CROW_DISABLED,
|
|
|
|
};
|
|
|
|
|
2020-07-31 05:47:39 -03:00
|
|
|
enum class ThrFailsafe {
|
|
|
|
Disabled = 0,
|
|
|
|
Enabled = 1,
|
|
|
|
EnabledNoFS = 2
|
|
|
|
};
|
|
|
|
|
2020-07-15 16:22:50 -03:00
|
|
|
CrowMode crow_mode = CrowMode::NORMAL;
|
2020-05-04 20:12:14 -03:00
|
|
|
|
2020-10-14 13:34:12 -03:00
|
|
|
enum class FlareMode {
|
|
|
|
FLARE_DISABLED = 0,
|
|
|
|
ENABLED_NO_PITCH_TARGET,
|
|
|
|
ENABLED_PITCH_TARGET
|
|
|
|
};
|
2022-11-01 10:46:52 -03:00
|
|
|
|
|
|
|
enum class AutoTuneAxis {
|
|
|
|
ROLL = 1U <<0,
|
|
|
|
PITCH = 1U <<1,
|
|
|
|
YAW = 1U <<2,
|
|
|
|
};
|
2020-10-14 13:34:12 -03:00
|
|
|
|
|
|
|
FlareMode flare_mode;
|
2021-11-20 20:03:18 -04:00
|
|
|
bool throttle_at_zero(void) const;
|
2020-10-02 21:52:56 -03:00
|
|
|
|
2021-07-17 03:40:38 -03:00
|
|
|
// expo handling
|
|
|
|
float roll_in_expo(bool use_dz) const;
|
|
|
|
float pitch_in_expo(bool use_dz) const;
|
|
|
|
float rudder_in_expo(bool use_dz) const;
|
|
|
|
|
2022-07-08 19:10:01 -03:00
|
|
|
// mode reason for entering previous mode
|
|
|
|
ModeReason previous_mode_reason = ModeReason::UNKNOWN;
|
|
|
|
|
2022-10-16 19:33:38 -03:00
|
|
|
// last target alt we passed to tecs
|
|
|
|
int32_t tecs_target_alt_cm;
|
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
public:
|
|
|
|
void failsafe_check(void);
|
2023-06-26 15:07:33 -03:00
|
|
|
bool is_landing() const override;
|
|
|
|
bool is_taking_off() const override;
|
2021-11-15 01:08:34 -04:00
|
|
|
#if AP_SCRIPTING_ENABLED
|
2020-03-06 06:11:09 -04:00
|
|
|
bool set_target_location(const Location& target_loc) override;
|
|
|
|
bool get_target_location(Location& target_loc) override;
|
2022-01-04 00:53:45 -04:00
|
|
|
bool update_target_location(const Location &old_loc, const Location &new_loc) override;
|
|
|
|
bool set_velocity_match(const Vector2f &velocity) override;
|
2023-01-17 02:47:03 -04:00
|
|
|
|
|
|
|
// allow for landing descent rate to be overridden by a script, may be -ve to climb
|
|
|
|
bool set_land_descent_rate(float descent_rate) override;
|
2021-11-15 01:08:34 -04:00
|
|
|
#endif // AP_SCRIPTING_ENABLED
|
2021-07-18 14:36:36 -03:00
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
};
|
|
|
|
|
|
|
|
extern Plane plane;
|
2015-08-24 03:21:21 -03:00
|
|
|
|
2015-11-19 23:04:52 -04:00
|
|
|
using AP_HAL::millis;
|
|
|
|
using AP_HAL::micros;
|