2019-01-15 13:46:13 -04:00
|
|
|
#pragma once
|
|
|
|
|
2019-05-04 06:56:07 -03:00
|
|
|
#include <AP_Param/AP_Param.h>
|
2019-10-09 23:46:46 -03:00
|
|
|
#include <AP_Common/Location.h>
|
2019-01-15 13:46:13 -04:00
|
|
|
#include <stdint.h>
|
2020-09-16 04:46:56 -03:00
|
|
|
#include <AP_Soaring/AP_Soaring.h>
|
2020-09-22 14:00:39 -03:00
|
|
|
#include <AP_ADSB/AP_ADSB.h>
|
2020-09-16 04:47:59 -03:00
|
|
|
#include <AP_Vehicle/ModeReason.h>
|
2021-07-24 11:06:30 -03:00
|
|
|
#include "quadplane.h"
|
2023-02-12 09:33:27 -04:00
|
|
|
#include <AP_AHRS/AP_AHRS.h>
|
2022-12-29 01:55:13 -04:00
|
|
|
#include <AP_Mission/AP_Mission.h>
|
2019-01-15 13:46:13 -04:00
|
|
|
|
2021-07-24 11:06:30 -03:00
|
|
|
class AC_PosControl;
|
|
|
|
class AC_AttitudeControl_Multi;
|
|
|
|
class AC_Loiter;
|
2019-01-15 13:46:13 -04:00
|
|
|
class Mode
|
|
|
|
{
|
|
|
|
public:
|
|
|
|
|
|
|
|
/* Do not allow copies */
|
2022-09-30 06:50:43 -03:00
|
|
|
CLASS_NO_COPY(Mode);
|
2019-01-15 13:46:13 -04:00
|
|
|
|
|
|
|
// Auto Pilot modes
|
|
|
|
// ----------------
|
2019-10-17 00:49:32 -03:00
|
|
|
enum Number : uint8_t {
|
2019-01-15 13:46:13 -04:00
|
|
|
MANUAL = 0,
|
|
|
|
CIRCLE = 1,
|
|
|
|
STABILIZE = 2,
|
|
|
|
TRAINING = 3,
|
|
|
|
ACRO = 4,
|
|
|
|
FLY_BY_WIRE_A = 5,
|
|
|
|
FLY_BY_WIRE_B = 6,
|
|
|
|
CRUISE = 7,
|
|
|
|
AUTOTUNE = 8,
|
|
|
|
AUTO = 10,
|
|
|
|
RTL = 11,
|
|
|
|
LOITER = 12,
|
2019-05-04 06:56:07 -03:00
|
|
|
TAKEOFF = 13,
|
2019-01-15 13:46:13 -04:00
|
|
|
AVOID_ADSB = 14,
|
|
|
|
GUIDED = 15,
|
|
|
|
INITIALISING = 16,
|
2021-09-10 03:28:21 -03:00
|
|
|
#if HAL_QUADPLANE_ENABLED
|
2019-01-15 13:46:13 -04:00
|
|
|
QSTABILIZE = 17,
|
|
|
|
QHOVER = 18,
|
|
|
|
QLOITER = 19,
|
|
|
|
QLAND = 20,
|
|
|
|
QRTL = 21,
|
2021-09-10 03:28:21 -03:00
|
|
|
#if QAUTOTUNE_ENABLED
|
2019-01-15 13:46:13 -04:00
|
|
|
QAUTOTUNE = 22,
|
2021-09-10 03:28:21 -03:00
|
|
|
#endif
|
2019-01-15 13:46:13 -04:00
|
|
|
QACRO = 23,
|
2021-09-10 03:28:21 -03:00
|
|
|
#endif
|
2020-09-16 04:46:56 -03:00
|
|
|
THERMAL = 24,
|
2021-09-18 20:15:41 -03:00
|
|
|
#if HAL_QUADPLANE_ENABLED
|
|
|
|
LOITER_ALT_QLAND = 25,
|
|
|
|
#endif
|
2019-01-15 13:46:13 -04:00
|
|
|
};
|
|
|
|
|
|
|
|
// Constructor
|
|
|
|
Mode();
|
|
|
|
|
|
|
|
// enter this mode, always returns true/success
|
|
|
|
bool enter();
|
|
|
|
|
|
|
|
// perform any cleanups required:
|
|
|
|
void exit();
|
|
|
|
|
2021-07-24 11:05:37 -03:00
|
|
|
// run controllers specific to this mode
|
2023-04-24 21:39:34 -03:00
|
|
|
virtual void run();
|
2021-07-24 11:05:37 -03:00
|
|
|
|
2019-01-15 13:46:13 -04:00
|
|
|
// returns a unique number specific to this mode
|
|
|
|
virtual Number mode_number() const = 0;
|
|
|
|
|
|
|
|
// returns full text name
|
|
|
|
virtual const char *name() const = 0;
|
|
|
|
|
|
|
|
// returns a string for this flightmode, exactly 4 bytes
|
|
|
|
virtual const char *name4() const = 0;
|
|
|
|
|
2021-01-21 19:28:26 -04:00
|
|
|
// returns true if the vehicle can be armed in this mode
|
2023-02-14 08:18:09 -04:00
|
|
|
bool pre_arm_checks(size_t buflen, char *buffer) const;
|
2021-01-21 19:28:26 -04:00
|
|
|
|
2023-04-24 09:28:55 -03:00
|
|
|
// Reset rate and steering controllers
|
|
|
|
void reset_controllers();
|
|
|
|
|
2019-01-15 13:46:13 -04:00
|
|
|
//
|
|
|
|
// methods that sub classes should override to affect movement of the vehicle in this mode
|
|
|
|
//
|
|
|
|
|
|
|
|
// convert user input to targets, implement high level control for this mode
|
|
|
|
virtual void update() = 0;
|
|
|
|
|
2019-04-01 14:54:34 -03:00
|
|
|
// true for all q modes
|
|
|
|
virtual bool is_vtol_mode() const { return false; }
|
2020-08-24 13:44:50 -03:00
|
|
|
virtual bool is_vtol_man_throttle() const;
|
2020-07-26 11:35:51 -03:00
|
|
|
virtual bool is_vtol_man_mode() const { return false; }
|
2021-09-10 03:28:21 -03:00
|
|
|
|
2020-09-22 14:00:39 -03:00
|
|
|
// guided or adsb mode
|
|
|
|
virtual bool is_guided_mode() const { return false; }
|
2019-04-01 14:54:34 -03:00
|
|
|
|
2020-07-07 09:25:21 -03:00
|
|
|
// true if mode can have terrain following disabled by switch
|
|
|
|
virtual bool allows_terrain_disable() const { return false; }
|
|
|
|
|
2020-12-30 16:04:30 -04:00
|
|
|
// true if automatic switch to thermal mode is supported.
|
|
|
|
virtual bool does_automatic_thermal_switch() const {return false; }
|
|
|
|
|
2020-08-19 04:51:00 -03:00
|
|
|
// subclasses override this if they require navigation.
|
|
|
|
virtual void navigate() { return; }
|
2020-07-29 12:01:34 -03:00
|
|
|
|
2020-12-22 20:47:58 -04:00
|
|
|
// this allows certain flight modes to mix RC input with throttle
|
|
|
|
// depending on airspeed_nudge_cm
|
|
|
|
virtual bool allows_throttle_nudging() const { return false; }
|
|
|
|
|
2020-12-23 00:56:54 -04:00
|
|
|
// true if the mode sets the vehicle destination, which controls
|
|
|
|
// whether control input is ignored with STICK_MIXING=0
|
|
|
|
virtual bool does_auto_navigation() const { return false; }
|
|
|
|
|
2020-12-23 01:25:35 -04:00
|
|
|
// true if the mode sets the vehicle destination, which controls
|
|
|
|
// whether control input is ignored with STICK_MIXING=0
|
|
|
|
virtual bool does_auto_throttle() const { return false; }
|
2023-01-26 16:59:58 -04:00
|
|
|
|
|
|
|
// true if the mode supports autotuning (via switch for modes other
|
|
|
|
// that AUTOTUNE itself
|
|
|
|
virtual bool mode_allows_autotuning() const { return false; }
|
2020-12-23 01:25:35 -04:00
|
|
|
|
2021-06-01 03:03:52 -03:00
|
|
|
// method for mode specific target altitude profiles
|
2022-12-14 08:39:29 -04:00
|
|
|
virtual void update_target_altitude();
|
2021-09-27 16:28:39 -03:00
|
|
|
|
|
|
|
// handle a guided target request from GCS
|
|
|
|
virtual bool handle_guided_request(Location target_loc) { return false; }
|
|
|
|
|
2023-06-17 14:34:08 -03:00
|
|
|
// true if is landing
|
|
|
|
virtual bool is_landing() const { return false; }
|
|
|
|
|
|
|
|
// true if is taking
|
2023-06-21 16:30:05 -03:00
|
|
|
virtual bool is_taking_off() const;
|
2023-06-17 14:34:08 -03:00
|
|
|
|
2023-12-22 12:18:29 -04:00
|
|
|
// true if throttle min/max limits should be applied
|
|
|
|
bool use_throttle_limits() const;
|
2024-01-17 16:18:18 -04:00
|
|
|
|
|
|
|
// true if voltage correction should be applied to throttle
|
|
|
|
bool use_battery_compensation() const;
|
2023-06-17 14:34:08 -03:00
|
|
|
|
2019-01-15 13:46:13 -04:00
|
|
|
protected:
|
|
|
|
|
|
|
|
// subclasses override this to perform checks before entering the mode
|
|
|
|
virtual bool _enter() { return true; }
|
|
|
|
|
|
|
|
// subclasses override this to perform any required cleanup when exiting the mode
|
|
|
|
virtual void _exit() { return; }
|
2021-07-24 11:06:30 -03:00
|
|
|
|
2023-02-14 08:18:09 -04:00
|
|
|
// mode specific pre-arm checks
|
|
|
|
virtual bool _pre_arm_checks(size_t buflen, char *buffer) const;
|
|
|
|
|
2023-07-05 20:01:16 -03:00
|
|
|
// Helper to output to both k_rudder and k_steering servo functions
|
|
|
|
void output_rudder_and_steering(float val);
|
|
|
|
|
2024-01-31 11:16:30 -04:00
|
|
|
// Output pilot throttle, this is used in stabilized modes without auto throttle control
|
|
|
|
void output_pilot_throttle();
|
|
|
|
|
2021-09-10 03:28:21 -03:00
|
|
|
#if HAL_QUADPLANE_ENABLED
|
2021-07-24 11:06:30 -03:00
|
|
|
// References for convenience, used by QModes
|
|
|
|
AC_PosControl*& pos_control;
|
|
|
|
AC_AttitudeControl_Multi*& attitude_control;
|
|
|
|
AC_Loiter*& loiter_nav;
|
2021-09-10 03:28:21 -03:00
|
|
|
QuadPlane& quadplane;
|
2021-07-24 11:06:30 -03:00
|
|
|
QuadPlane::PosControlState &poscontrol;
|
2021-09-10 03:28:21 -03:00
|
|
|
#endif
|
2023-02-12 09:33:27 -04:00
|
|
|
AP_AHRS& ahrs;
|
2019-01-15 13:46:13 -04:00
|
|
|
};
|
|
|
|
|
|
|
|
|
|
|
|
class ModeAcro : public Mode
|
|
|
|
{
|
2023-02-12 09:45:25 -04:00
|
|
|
friend class ModeQAcro;
|
2019-01-15 13:46:13 -04:00
|
|
|
public:
|
|
|
|
|
|
|
|
Mode::Number mode_number() const override { return Mode::Number::ACRO; }
|
|
|
|
const char *name() const override { return "ACRO"; }
|
|
|
|
const char *name4() const override { return "ACRO"; }
|
|
|
|
|
|
|
|
// methods that affect movement of the vehicle in this mode
|
|
|
|
void update() override;
|
|
|
|
|
2023-02-12 09:45:25 -04:00
|
|
|
void run() override;
|
|
|
|
|
|
|
|
void stabilize();
|
|
|
|
|
|
|
|
void stabilize_quaternion();
|
|
|
|
|
2019-01-15 13:46:13 -04:00
|
|
|
protected:
|
|
|
|
|
2023-02-12 09:45:25 -04:00
|
|
|
// ACRO controller state
|
|
|
|
struct {
|
|
|
|
bool locked_roll;
|
|
|
|
bool locked_pitch;
|
|
|
|
float locked_roll_err;
|
|
|
|
int32_t locked_pitch_cd;
|
|
|
|
Quaternion q;
|
|
|
|
bool roll_active_last;
|
|
|
|
bool pitch_active_last;
|
|
|
|
bool yaw_active_last;
|
|
|
|
} acro_state;
|
|
|
|
|
2019-01-15 13:46:13 -04:00
|
|
|
bool _enter() override;
|
|
|
|
};
|
|
|
|
|
|
|
|
class ModeAuto : public Mode
|
|
|
|
{
|
|
|
|
public:
|
|
|
|
|
|
|
|
Number mode_number() const override { return Number::AUTO; }
|
|
|
|
const char *name() const override { return "AUTO"; }
|
|
|
|
const char *name4() const override { return "AUTO"; }
|
|
|
|
|
2020-12-30 16:04:30 -04:00
|
|
|
bool does_automatic_thermal_switch() const override { return true; }
|
|
|
|
|
2019-01-15 13:46:13 -04:00
|
|
|
// methods that affect movement of the vehicle in this mode
|
|
|
|
void update() override;
|
|
|
|
|
2020-08-19 04:51:00 -03:00
|
|
|
void navigate() override;
|
|
|
|
|
2020-12-22 20:47:58 -04:00
|
|
|
bool allows_throttle_nudging() const override { return true; }
|
|
|
|
|
2022-01-30 21:26:51 -04:00
|
|
|
bool does_auto_navigation() const override;
|
2020-12-23 00:56:54 -04:00
|
|
|
|
2022-01-30 21:26:51 -04:00
|
|
|
bool does_auto_throttle() const override;
|
2023-01-26 16:59:58 -04:00
|
|
|
|
|
|
|
bool mode_allows_autotuning() const override { return true; }
|
2020-12-23 01:25:35 -04:00
|
|
|
|
2023-06-17 14:34:08 -03:00
|
|
|
bool is_landing() const override;
|
2022-12-29 01:55:13 -04:00
|
|
|
|
|
|
|
void do_nav_delay(const AP_Mission::Mission_Command& cmd);
|
|
|
|
bool verify_nav_delay(const AP_Mission::Mission_Command& cmd);
|
|
|
|
|
2023-10-28 17:05:57 -03:00
|
|
|
void run() override;
|
|
|
|
|
2019-01-15 13:46:13 -04:00
|
|
|
protected:
|
|
|
|
|
|
|
|
bool _enter() override;
|
|
|
|
void _exit() override;
|
2023-02-14 08:18:09 -04:00
|
|
|
bool _pre_arm_checks(size_t buflen, char *buffer) const override;
|
2022-12-29 01:55:13 -04:00
|
|
|
|
|
|
|
private:
|
|
|
|
|
|
|
|
// Delay the next navigation command
|
|
|
|
struct {
|
|
|
|
uint32_t time_max_ms;
|
|
|
|
uint32_t time_start_ms;
|
|
|
|
} nav_delay;
|
|
|
|
|
2019-01-15 13:46:13 -04:00
|
|
|
};
|
|
|
|
|
|
|
|
|
|
|
|
class ModeAutoTune : public Mode
|
|
|
|
{
|
|
|
|
public:
|
|
|
|
|
|
|
|
Number mode_number() const override { return Number::AUTOTUNE; }
|
|
|
|
const char *name() const override { return "AUTOTUNE"; }
|
|
|
|
const char *name4() const override { return "ATUN"; }
|
|
|
|
|
|
|
|
// methods that affect movement of the vehicle in this mode
|
|
|
|
void update() override;
|
2023-01-26 16:59:58 -04:00
|
|
|
|
|
|
|
bool mode_allows_autotuning() const override { return true; }
|
2019-01-15 13:46:13 -04:00
|
|
|
|
2024-01-31 11:16:30 -04:00
|
|
|
void run() override;
|
|
|
|
|
2019-01-15 13:46:13 -04:00
|
|
|
protected:
|
|
|
|
|
|
|
|
bool _enter() override;
|
|
|
|
};
|
|
|
|
|
|
|
|
class ModeGuided : public Mode
|
|
|
|
{
|
|
|
|
public:
|
|
|
|
|
|
|
|
Number mode_number() const override { return Number::GUIDED; }
|
|
|
|
const char *name() const override { return "GUIDED"; }
|
|
|
|
const char *name4() const override { return "GUID"; }
|
|
|
|
|
|
|
|
// methods that affect movement of the vehicle in this mode
|
|
|
|
void update() override;
|
|
|
|
|
2020-08-19 04:51:00 -03:00
|
|
|
void navigate() override;
|
|
|
|
|
2020-09-22 14:00:39 -03:00
|
|
|
virtual bool is_guided_mode() const override { return true; }
|
|
|
|
|
2020-12-22 20:47:58 -04:00
|
|
|
bool allows_throttle_nudging() const override { return true; }
|
|
|
|
|
2020-12-23 00:56:54 -04:00
|
|
|
bool does_auto_navigation() const override { return true; }
|
|
|
|
|
2020-12-23 01:25:35 -04:00
|
|
|
bool does_auto_throttle() const override { return true; }
|
|
|
|
|
2021-09-27 16:28:39 -03:00
|
|
|
// handle a guided target request from GCS
|
|
|
|
bool handle_guided_request(Location target_loc) override;
|
|
|
|
|
2022-10-10 18:58:30 -03:00
|
|
|
void set_radius_and_direction(const float radius, const bool direction_is_ccw);
|
|
|
|
|
2022-12-14 08:39:29 -04:00
|
|
|
void update_target_altitude() override;
|
|
|
|
|
2019-01-15 13:46:13 -04:00
|
|
|
protected:
|
|
|
|
|
|
|
|
bool _enter() override;
|
2023-02-14 08:18:09 -04:00
|
|
|
bool _pre_arm_checks(size_t buflen, char *buffer) const override { return true; }
|
2022-10-10 18:58:30 -03:00
|
|
|
|
|
|
|
private:
|
|
|
|
float active_radius_m;
|
2019-01-15 13:46:13 -04:00
|
|
|
};
|
|
|
|
|
|
|
|
class ModeCircle: public Mode
|
|
|
|
{
|
|
|
|
public:
|
|
|
|
|
|
|
|
Number mode_number() const override { return Number::CIRCLE; }
|
|
|
|
const char *name() const override { return "CIRCLE"; }
|
|
|
|
const char *name4() const override { return "CIRC"; }
|
|
|
|
|
|
|
|
// methods that affect movement of the vehicle in this mode
|
|
|
|
void update() override;
|
|
|
|
|
2020-12-23 00:56:54 -04:00
|
|
|
bool does_auto_navigation() const override { return true; }
|
|
|
|
|
2020-12-23 01:25:35 -04:00
|
|
|
bool does_auto_throttle() const override { return true; }
|
|
|
|
|
2019-01-15 13:46:13 -04:00
|
|
|
protected:
|
|
|
|
|
|
|
|
bool _enter() override;
|
|
|
|
};
|
|
|
|
|
|
|
|
class ModeLoiter : public Mode
|
|
|
|
{
|
|
|
|
public:
|
|
|
|
|
|
|
|
Number mode_number() const override { return Number::LOITER; }
|
|
|
|
const char *name() const override { return "LOITER"; }
|
|
|
|
const char *name4() const override { return "LOIT"; }
|
|
|
|
|
|
|
|
// methods that affect movement of the vehicle in this mode
|
|
|
|
void update() override;
|
|
|
|
|
2020-08-19 04:51:00 -03:00
|
|
|
void navigate() override;
|
|
|
|
|
2019-06-07 12:08:28 -03:00
|
|
|
bool isHeadingLinedUp(const Location loiterCenterLoc, const Location targetLoc);
|
2024-01-27 20:09:15 -04:00
|
|
|
bool isHeadingLinedUp_cd(const int32_t bearing_cd, const int32_t heading_cd);
|
2020-01-28 00:14:43 -04:00
|
|
|
bool isHeadingLinedUp_cd(const int32_t bearing_cd);
|
2019-06-05 15:25:40 -03:00
|
|
|
|
2020-12-23 01:25:35 -04:00
|
|
|
bool allows_throttle_nudging() const override { return true; }
|
|
|
|
|
2020-12-23 00:56:54 -04:00
|
|
|
bool does_auto_navigation() const override { return true; }
|
|
|
|
|
2020-12-23 01:25:35 -04:00
|
|
|
bool does_auto_throttle() const override { return true; }
|
2022-12-14 08:39:29 -04:00
|
|
|
|
2022-05-10 09:53:10 -03:00
|
|
|
bool allows_terrain_disable() const override { return true; }
|
2020-12-23 01:25:35 -04:00
|
|
|
|
2022-12-14 08:39:29 -04:00
|
|
|
void update_target_altitude() override;
|
2023-01-26 16:59:58 -04:00
|
|
|
|
|
|
|
bool mode_allows_autotuning() const override { return true; }
|
2022-12-14 08:39:29 -04:00
|
|
|
|
2019-01-15 13:46:13 -04:00
|
|
|
protected:
|
|
|
|
|
|
|
|
bool _enter() override;
|
|
|
|
};
|
|
|
|
|
2021-09-18 20:15:41 -03:00
|
|
|
#if HAL_QUADPLANE_ENABLED
|
|
|
|
class ModeLoiterAltQLand : public ModeLoiter
|
|
|
|
{
|
|
|
|
public:
|
|
|
|
|
|
|
|
Number mode_number() const override { return Number::LOITER_ALT_QLAND; }
|
|
|
|
const char *name() const override { return "Loiter to QLAND"; }
|
|
|
|
const char *name4() const override { return "L2QL"; }
|
|
|
|
|
2021-09-27 16:29:03 -03:00
|
|
|
// handle a guided target request from GCS
|
|
|
|
bool handle_guided_request(Location target_loc) override;
|
|
|
|
|
2021-09-18 20:15:41 -03:00
|
|
|
protected:
|
|
|
|
bool _enter() override;
|
|
|
|
|
|
|
|
void navigate() override;
|
|
|
|
|
|
|
|
private:
|
|
|
|
void switch_qland();
|
|
|
|
|
|
|
|
};
|
|
|
|
#endif // HAL_QUADPLANE_ENABLED
|
|
|
|
|
2019-01-15 13:46:13 -04:00
|
|
|
class ModeManual : public Mode
|
|
|
|
{
|
|
|
|
public:
|
|
|
|
|
|
|
|
Number mode_number() const override { return Number::MANUAL; }
|
|
|
|
const char *name() const override { return "MANUAL"; }
|
|
|
|
const char *name4() const override { return "MANU"; }
|
|
|
|
|
|
|
|
// methods that affect movement of the vehicle in this mode
|
|
|
|
void update() override;
|
2023-04-24 09:28:55 -03:00
|
|
|
|
|
|
|
void run() override;
|
2024-02-14 07:51:45 -04:00
|
|
|
|
|
|
|
// true if throttle min/max limits should be applied
|
|
|
|
bool use_throttle_limits() const { return false; }
|
|
|
|
|
|
|
|
// true if voltage correction should be applied to throttle
|
|
|
|
bool use_battery_compensation() const { return false; }
|
|
|
|
|
2019-01-15 13:46:13 -04:00
|
|
|
};
|
|
|
|
|
|
|
|
|
|
|
|
class ModeRTL : public Mode
|
|
|
|
{
|
|
|
|
public:
|
|
|
|
|
|
|
|
Number mode_number() const override { return Number::RTL; }
|
|
|
|
const char *name() const override { return "RTL"; }
|
|
|
|
const char *name4() const override { return "RTL "; }
|
|
|
|
|
|
|
|
// methods that affect movement of the vehicle in this mode
|
|
|
|
void update() override;
|
|
|
|
|
2020-08-19 04:51:00 -03:00
|
|
|
void navigate() override;
|
|
|
|
|
2020-12-22 20:47:58 -04:00
|
|
|
bool allows_throttle_nudging() const override { return true; }
|
|
|
|
|
2020-12-23 00:56:54 -04:00
|
|
|
bool does_auto_navigation() const override { return true; }
|
|
|
|
|
2020-12-23 01:25:35 -04:00
|
|
|
bool does_auto_throttle() const override { return true; }
|
|
|
|
|
2019-01-15 13:46:13 -04:00
|
|
|
protected:
|
|
|
|
|
|
|
|
bool _enter() override;
|
2023-02-14 08:18:09 -04:00
|
|
|
bool _pre_arm_checks(size_t buflen, char *buffer) const override { return false; }
|
2021-01-30 16:14:19 -04:00
|
|
|
|
|
|
|
private:
|
|
|
|
|
|
|
|
// Switch to QRTL if enabled and within radius
|
2022-11-24 18:50:36 -04:00
|
|
|
bool switch_QRTL();
|
2019-01-15 13:46:13 -04:00
|
|
|
};
|
|
|
|
|
|
|
|
class ModeStabilize : public Mode
|
|
|
|
{
|
|
|
|
public:
|
|
|
|
|
|
|
|
Number mode_number() const override { return Number::STABILIZE; }
|
|
|
|
const char *name() const override { return "STABILIZE"; }
|
|
|
|
const char *name4() const override { return "STAB"; }
|
|
|
|
|
|
|
|
// methods that affect movement of the vehicle in this mode
|
|
|
|
void update() override;
|
2023-04-24 21:31:04 -03:00
|
|
|
|
|
|
|
void run() override;
|
|
|
|
|
|
|
|
private:
|
|
|
|
void stabilize_stick_mixing_direct();
|
|
|
|
|
2019-01-15 13:46:13 -04:00
|
|
|
};
|
|
|
|
|
|
|
|
class ModeTraining : public Mode
|
|
|
|
{
|
|
|
|
public:
|
|
|
|
|
|
|
|
Number mode_number() const override { return Number::TRAINING; }
|
|
|
|
const char *name() const override { return "TRAINING"; }
|
|
|
|
const char *name4() const override { return "TRAN"; }
|
|
|
|
|
|
|
|
// methods that affect movement of the vehicle in this mode
|
|
|
|
void update() override;
|
2023-02-12 09:38:37 -04:00
|
|
|
|
|
|
|
void run() override;
|
|
|
|
|
2019-01-15 13:46:13 -04:00
|
|
|
};
|
|
|
|
|
|
|
|
class ModeInitializing : public Mode
|
|
|
|
{
|
|
|
|
public:
|
|
|
|
|
|
|
|
Number mode_number() const override { return Number::INITIALISING; }
|
|
|
|
const char *name() const override { return "INITIALISING"; }
|
|
|
|
const char *name4() const override { return "INIT"; }
|
|
|
|
|
2021-01-20 20:48:01 -04:00
|
|
|
bool _enter() override { return false; }
|
|
|
|
|
2019-01-15 13:46:13 -04:00
|
|
|
// methods that affect movement of the vehicle in this mode
|
|
|
|
void update() override { }
|
|
|
|
|
2020-12-22 20:47:58 -04:00
|
|
|
bool allows_throttle_nudging() const override { return true; }
|
|
|
|
|
2020-12-23 01:25:35 -04:00
|
|
|
bool does_auto_throttle() const override { return true; }
|
2023-02-14 08:18:09 -04:00
|
|
|
|
|
|
|
protected:
|
|
|
|
bool _pre_arm_checks(size_t buflen, char *buffer) const override { return false; }
|
|
|
|
|
2019-01-15 13:46:13 -04:00
|
|
|
};
|
|
|
|
|
|
|
|
class ModeFBWA : public Mode
|
|
|
|
{
|
|
|
|
public:
|
|
|
|
|
|
|
|
Number mode_number() const override { return Number::FLY_BY_WIRE_A; }
|
|
|
|
const char *name() const override { return "FLY_BY_WIRE_A"; }
|
|
|
|
const char *name4() const override { return "FBWA"; }
|
|
|
|
|
|
|
|
// methods that affect movement of the vehicle in this mode
|
|
|
|
void update() override;
|
2023-01-26 16:59:58 -04:00
|
|
|
|
|
|
|
bool mode_allows_autotuning() const override { return true; }
|
2019-01-15 13:46:13 -04:00
|
|
|
|
2024-01-31 11:16:30 -04:00
|
|
|
void run() override;
|
|
|
|
|
2019-01-15 13:46:13 -04:00
|
|
|
};
|
|
|
|
|
|
|
|
class ModeFBWB : public Mode
|
|
|
|
{
|
|
|
|
public:
|
|
|
|
|
|
|
|
Number mode_number() const override { return Number::FLY_BY_WIRE_B; }
|
|
|
|
const char *name() const override { return "FLY_BY_WIRE_B"; }
|
|
|
|
const char *name4() const override { return "FBWB"; }
|
|
|
|
|
2020-07-07 09:25:21 -03:00
|
|
|
bool allows_terrain_disable() const override { return true; }
|
|
|
|
|
2020-12-30 16:04:30 -04:00
|
|
|
bool does_automatic_thermal_switch() const override { return true; }
|
|
|
|
|
2019-01-15 13:46:13 -04:00
|
|
|
// methods that affect movement of the vehicle in this mode
|
|
|
|
void update() override;
|
|
|
|
|
2020-12-23 01:25:35 -04:00
|
|
|
bool does_auto_throttle() const override { return true; }
|
2023-01-26 16:59:58 -04:00
|
|
|
|
|
|
|
bool mode_allows_autotuning() const override { return true; }
|
2020-12-23 01:25:35 -04:00
|
|
|
|
2022-12-14 08:39:29 -04:00
|
|
|
void update_target_altitude() override {};
|
|
|
|
|
2019-01-15 13:46:13 -04:00
|
|
|
protected:
|
|
|
|
|
|
|
|
bool _enter() override;
|
|
|
|
};
|
|
|
|
|
|
|
|
class ModeCruise : public Mode
|
|
|
|
{
|
|
|
|
public:
|
|
|
|
|
|
|
|
Number mode_number() const override { return Number::CRUISE; }
|
|
|
|
const char *name() const override { return "CRUISE"; }
|
|
|
|
const char *name4() const override { return "CRUS"; }
|
|
|
|
|
2020-07-07 09:25:21 -03:00
|
|
|
bool allows_terrain_disable() const override { return true; }
|
|
|
|
|
2020-12-30 16:04:30 -04:00
|
|
|
bool does_automatic_thermal_switch() const override { return true; }
|
|
|
|
|
2019-01-15 13:46:13 -04:00
|
|
|
// methods that affect movement of the vehicle in this mode
|
|
|
|
void update() override;
|
|
|
|
|
2020-08-19 04:51:00 -03:00
|
|
|
void navigate() override;
|
|
|
|
|
2021-02-01 12:26:22 -04:00
|
|
|
bool get_target_heading_cd(int32_t &target_heading) const;
|
2020-08-03 15:22:00 -03:00
|
|
|
|
2020-12-23 01:25:35 -04:00
|
|
|
bool does_auto_throttle() const override { return true; }
|
|
|
|
|
2022-12-14 08:39:29 -04:00
|
|
|
void update_target_altitude() override {};
|
|
|
|
|
2019-01-15 13:46:13 -04:00
|
|
|
protected:
|
|
|
|
|
|
|
|
bool _enter() override;
|
2020-08-03 15:22:00 -03:00
|
|
|
|
|
|
|
bool locked_heading;
|
|
|
|
int32_t locked_heading_cd;
|
|
|
|
uint32_t lock_timer_ms;
|
2019-01-15 13:46:13 -04:00
|
|
|
};
|
|
|
|
|
2020-09-22 14:00:39 -03:00
|
|
|
#if HAL_ADSB_ENABLED
|
2019-01-15 13:46:13 -04:00
|
|
|
class ModeAvoidADSB : public Mode
|
|
|
|
{
|
|
|
|
public:
|
|
|
|
|
|
|
|
Number mode_number() const override { return Number::AVOID_ADSB; }
|
|
|
|
const char *name() const override { return "AVOID_ADSB"; }
|
|
|
|
const char *name4() const override { return "AVOI"; }
|
|
|
|
|
|
|
|
// methods that affect movement of the vehicle in this mode
|
|
|
|
void update() override;
|
|
|
|
|
2020-08-19 04:51:00 -03:00
|
|
|
void navigate() override;
|
2020-09-22 14:00:39 -03:00
|
|
|
|
|
|
|
virtual bool is_guided_mode() const override { return true; }
|
|
|
|
|
2020-12-23 01:25:35 -04:00
|
|
|
bool does_auto_throttle() const override { return true; }
|
|
|
|
|
2019-01-15 13:46:13 -04:00
|
|
|
protected:
|
|
|
|
|
|
|
|
bool _enter() override;
|
|
|
|
};
|
2020-09-22 14:00:39 -03:00
|
|
|
#endif
|
2019-01-15 13:46:13 -04:00
|
|
|
|
2021-09-10 03:28:21 -03:00
|
|
|
#if HAL_QUADPLANE_ENABLED
|
2019-01-15 13:46:13 -04:00
|
|
|
class ModeQStabilize : public Mode
|
|
|
|
{
|
|
|
|
public:
|
|
|
|
|
|
|
|
Number mode_number() const override { return Number::QSTABILIZE; }
|
|
|
|
const char *name() const override { return "QSTABILIZE"; }
|
|
|
|
const char *name4() const override { return "QSTB"; }
|
|
|
|
|
2019-04-01 14:54:34 -03:00
|
|
|
bool is_vtol_mode() const override { return true; }
|
2020-07-07 21:22:56 -03:00
|
|
|
bool is_vtol_man_throttle() const override { return true; }
|
2020-07-26 11:35:51 -03:00
|
|
|
virtual bool is_vtol_man_mode() const override { return true; }
|
2020-12-22 20:47:58 -04:00
|
|
|
bool allows_throttle_nudging() const override { return true; }
|
2019-04-01 14:54:34 -03:00
|
|
|
|
2019-01-15 13:46:13 -04:00
|
|
|
// methods that affect movement of the vehicle in this mode
|
|
|
|
void update() override;
|
|
|
|
|
|
|
|
// used as a base class for all Q modes
|
|
|
|
bool _enter() override;
|
|
|
|
|
2021-07-24 11:05:37 -03:00
|
|
|
void run() override;
|
|
|
|
|
2019-01-15 13:46:13 -04:00
|
|
|
protected:
|
2021-02-19 14:30:26 -04:00
|
|
|
private:
|
|
|
|
|
|
|
|
void set_tailsitter_roll_pitch(const float roll_input, const float pitch_input);
|
|
|
|
void set_limited_roll_pitch(const float roll_input, const float pitch_input);
|
2019-01-15 13:46:13 -04:00
|
|
|
|
|
|
|
};
|
|
|
|
|
|
|
|
class ModeQHover : public Mode
|
|
|
|
{
|
|
|
|
public:
|
|
|
|
|
|
|
|
Number mode_number() const override { return Number::QHOVER; }
|
|
|
|
const char *name() const override { return "QHOVER"; }
|
|
|
|
const char *name4() const override { return "QHOV"; }
|
|
|
|
|
2019-04-01 14:54:34 -03:00
|
|
|
bool is_vtol_mode() const override { return true; }
|
2020-07-26 11:35:51 -03:00
|
|
|
virtual bool is_vtol_man_mode() const override { return true; }
|
2019-04-01 14:54:34 -03:00
|
|
|
|
2019-01-15 13:46:13 -04:00
|
|
|
// methods that affect movement of the vehicle in this mode
|
|
|
|
void update() override;
|
|
|
|
|
2021-07-24 11:05:37 -03:00
|
|
|
void run() override;
|
|
|
|
|
2019-01-15 13:46:13 -04:00
|
|
|
protected:
|
|
|
|
|
|
|
|
bool _enter() override;
|
|
|
|
};
|
|
|
|
|
|
|
|
class ModeQLoiter : public Mode
|
|
|
|
{
|
2021-07-24 11:05:37 -03:00
|
|
|
friend class QuadPlane;
|
|
|
|
friend class ModeQLand;
|
2019-01-15 13:46:13 -04:00
|
|
|
public:
|
|
|
|
|
|
|
|
Number mode_number() const override { return Number::QLOITER; }
|
|
|
|
const char *name() const override { return "QLOITER"; }
|
|
|
|
const char *name4() const override { return "QLOT"; }
|
|
|
|
|
2019-04-01 14:54:34 -03:00
|
|
|
bool is_vtol_mode() const override { return true; }
|
2020-07-26 11:35:51 -03:00
|
|
|
virtual bool is_vtol_man_mode() const override { return true; }
|
2019-04-01 14:54:34 -03:00
|
|
|
|
2019-01-15 13:46:13 -04:00
|
|
|
// methods that affect movement of the vehicle in this mode
|
|
|
|
void update() override;
|
|
|
|
|
2021-07-24 11:05:37 -03:00
|
|
|
void run() override;
|
|
|
|
|
2019-01-15 13:46:13 -04:00
|
|
|
protected:
|
|
|
|
|
|
|
|
bool _enter() override;
|
|
|
|
};
|
|
|
|
|
|
|
|
class ModeQLand : public Mode
|
|
|
|
{
|
|
|
|
public:
|
|
|
|
|
|
|
|
Number mode_number() const override { return Number::QLAND; }
|
|
|
|
const char *name() const override { return "QLAND"; }
|
|
|
|
const char *name4() const override { return "QLND"; }
|
|
|
|
|
2019-04-01 14:54:34 -03:00
|
|
|
bool is_vtol_mode() const override { return true; }
|
|
|
|
|
2019-01-15 13:46:13 -04:00
|
|
|
// methods that affect movement of the vehicle in this mode
|
|
|
|
void update() override;
|
|
|
|
|
2021-07-24 11:05:37 -03:00
|
|
|
void run() override;
|
|
|
|
|
2019-01-15 13:46:13 -04:00
|
|
|
protected:
|
|
|
|
|
|
|
|
bool _enter() override;
|
2023-02-14 08:18:09 -04:00
|
|
|
bool _pre_arm_checks(size_t buflen, char *buffer) const override { return false; }
|
|
|
|
|
2019-01-15 13:46:13 -04:00
|
|
|
};
|
|
|
|
|
|
|
|
class ModeQRTL : public Mode
|
|
|
|
{
|
|
|
|
public:
|
|
|
|
|
|
|
|
Number mode_number() const override { return Number::QRTL; }
|
|
|
|
const char *name() const override { return "QRTL"; }
|
|
|
|
const char *name4() const override { return "QRTL"; }
|
|
|
|
|
2019-04-01 14:54:34 -03:00
|
|
|
bool is_vtol_mode() const override { return true; }
|
|
|
|
|
2019-01-15 13:46:13 -04:00
|
|
|
// methods that affect movement of the vehicle in this mode
|
|
|
|
void update() override;
|
|
|
|
|
2021-07-24 11:05:37 -03:00
|
|
|
void run() override;
|
|
|
|
|
2021-05-18 20:20:48 -03:00
|
|
|
bool does_auto_throttle() const override { return true; }
|
|
|
|
|
2022-12-14 08:39:29 -04:00
|
|
|
void update_target_altitude() override;
|
2021-06-01 03:03:52 -03:00
|
|
|
|
2021-12-24 20:27:59 -04:00
|
|
|
bool allows_throttle_nudging() const override;
|
|
|
|
|
2023-01-25 06:35:25 -04:00
|
|
|
float get_VTOL_return_radius() const;
|
|
|
|
|
2019-01-15 13:46:13 -04:00
|
|
|
protected:
|
|
|
|
|
|
|
|
bool _enter() override;
|
2023-02-14 08:18:09 -04:00
|
|
|
bool _pre_arm_checks(size_t buflen, char *buffer) const override { return false; }
|
2022-05-30 22:38:55 -03:00
|
|
|
|
|
|
|
private:
|
|
|
|
|
|
|
|
enum class SubMode {
|
|
|
|
climb,
|
|
|
|
RTL,
|
|
|
|
} submode;
|
2019-01-15 13:46:13 -04:00
|
|
|
};
|
|
|
|
|
|
|
|
class ModeQAcro : public Mode
|
|
|
|
{
|
|
|
|
public:
|
|
|
|
|
|
|
|
Number mode_number() const override { return Number::QACRO; }
|
2023-01-01 09:39:06 -04:00
|
|
|
const char *name() const override { return "QACRO"; }
|
|
|
|
const char *name4() const override { return "QACO"; }
|
2019-01-15 13:46:13 -04:00
|
|
|
|
2019-04-01 14:54:34 -03:00
|
|
|
bool is_vtol_mode() const override { return true; }
|
2020-07-07 21:22:56 -03:00
|
|
|
bool is_vtol_man_throttle() const override { return true; }
|
2020-12-30 13:29:03 -04:00
|
|
|
virtual bool is_vtol_man_mode() const override { return true; }
|
2019-04-01 14:54:34 -03:00
|
|
|
|
2019-01-15 13:46:13 -04:00
|
|
|
// methods that affect movement of the vehicle in this mode
|
|
|
|
void update() override;
|
|
|
|
|
2021-07-24 11:05:37 -03:00
|
|
|
void run() override;
|
|
|
|
|
2019-01-15 13:46:13 -04:00
|
|
|
protected:
|
|
|
|
|
|
|
|
bool _enter() override;
|
|
|
|
};
|
|
|
|
|
2021-09-10 03:28:21 -03:00
|
|
|
#if QAUTOTUNE_ENABLED
|
2019-01-15 13:46:13 -04:00
|
|
|
class ModeQAutotune : public Mode
|
|
|
|
{
|
|
|
|
public:
|
|
|
|
|
|
|
|
Number mode_number() const override { return Number::QAUTOTUNE; }
|
|
|
|
const char *name() const override { return "QAUTOTUNE"; }
|
|
|
|
const char *name4() const override { return "QATN"; }
|
|
|
|
|
2019-04-01 14:54:34 -03:00
|
|
|
bool is_vtol_mode() const override { return true; }
|
2020-07-26 11:35:51 -03:00
|
|
|
virtual bool is_vtol_man_mode() const override { return true; }
|
2019-04-01 14:54:34 -03:00
|
|
|
|
2021-07-24 11:05:37 -03:00
|
|
|
void run() override;
|
|
|
|
|
2019-01-15 13:46:13 -04:00
|
|
|
// methods that affect movement of the vehicle in this mode
|
|
|
|
void update() override;
|
|
|
|
|
|
|
|
protected:
|
|
|
|
|
|
|
|
bool _enter() override;
|
2019-04-30 01:43:10 -03:00
|
|
|
void _exit() override;
|
2019-01-15 13:46:13 -04:00
|
|
|
};
|
2021-09-10 03:28:21 -03:00
|
|
|
#endif // QAUTOTUNE_ENABLED
|
2019-05-04 06:56:07 -03:00
|
|
|
|
2021-09-10 03:28:21 -03:00
|
|
|
#endif // HAL_QUADPLANE_ENABLED
|
2019-05-04 06:56:07 -03:00
|
|
|
|
|
|
|
class ModeTakeoff: public Mode
|
|
|
|
{
|
|
|
|
public:
|
|
|
|
ModeTakeoff();
|
|
|
|
|
|
|
|
Number mode_number() const override { return Number::TAKEOFF; }
|
|
|
|
const char *name() const override { return "TAKEOFF"; }
|
|
|
|
const char *name4() const override { return "TKOF"; }
|
|
|
|
|
|
|
|
// methods that affect movement of the vehicle in this mode
|
|
|
|
void update() override;
|
|
|
|
|
2020-08-19 04:51:00 -03:00
|
|
|
void navigate() override;
|
|
|
|
|
2020-12-22 20:47:58 -04:00
|
|
|
bool allows_throttle_nudging() const override { return true; }
|
|
|
|
|
2020-12-23 00:56:54 -04:00
|
|
|
bool does_auto_navigation() const override { return true; }
|
|
|
|
|
2020-12-23 01:25:35 -04:00
|
|
|
bool does_auto_throttle() const override { return true; }
|
|
|
|
|
2019-05-04 06:56:07 -03:00
|
|
|
// var_info for holding parameter information
|
|
|
|
static const struct AP_Param::GroupInfo var_info[];
|
2020-12-30 13:29:03 -04:00
|
|
|
|
2021-02-19 21:26:36 -04:00
|
|
|
AP_Int16 target_alt;
|
|
|
|
AP_Int16 level_alt;
|
2021-02-19 21:00:38 -04:00
|
|
|
AP_Float ground_pitch;
|
|
|
|
|
2019-05-04 06:56:07 -03:00
|
|
|
protected:
|
|
|
|
AP_Int16 target_dist;
|
|
|
|
AP_Int8 level_pitch;
|
|
|
|
|
2024-01-17 13:39:20 -04:00
|
|
|
bool takeoff_mode_setup;
|
2019-10-09 23:46:46 -03:00
|
|
|
Location start_loc;
|
2019-05-04 06:56:07 -03:00
|
|
|
|
|
|
|
bool _enter() override;
|
|
|
|
};
|
2020-09-16 04:46:56 -03:00
|
|
|
|
2020-09-23 05:16:45 -03:00
|
|
|
#if HAL_SOARING_ENABLED
|
2020-09-16 04:46:56 -03:00
|
|
|
|
|
|
|
class ModeThermal: public Mode
|
|
|
|
{
|
|
|
|
public:
|
|
|
|
|
|
|
|
Number mode_number() const override { return Number::THERMAL; }
|
|
|
|
const char *name() const override { return "THERMAL"; }
|
|
|
|
const char *name4() const override { return "THML"; }
|
|
|
|
|
|
|
|
// methods that affect movement of the vehicle in this mode
|
|
|
|
void update() override;
|
|
|
|
|
2020-10-07 06:54:33 -03:00
|
|
|
// Update thermal tracking and exiting logic.
|
|
|
|
void update_soaring();
|
|
|
|
|
2020-09-16 04:46:56 -03:00
|
|
|
void navigate() override;
|
|
|
|
|
2020-12-22 20:47:58 -04:00
|
|
|
bool allows_throttle_nudging() const override { return true; }
|
|
|
|
|
2020-12-23 00:56:54 -04:00
|
|
|
bool does_auto_navigation() const override { return true; }
|
|
|
|
|
2020-12-23 01:25:35 -04:00
|
|
|
// true if we are in an auto-throttle mode, which means
|
|
|
|
// we need to run the speed/height controller
|
|
|
|
bool does_auto_throttle() const override { return true; }
|
|
|
|
|
2020-09-16 04:46:56 -03:00
|
|
|
protected:
|
|
|
|
|
2020-09-16 04:47:59 -03:00
|
|
|
bool exit_heading_aligned() const;
|
|
|
|
void restore_mode(const char *reason, ModeReason modereason);
|
|
|
|
|
2020-09-16 04:46:56 -03:00
|
|
|
bool _enter() override;
|
|
|
|
};
|
|
|
|
|
|
|
|
#endif
|