ardupilot/ArduSub/mode.h

450 lines
13 KiB
C
Raw Normal View History

2023-04-03 09:11:21 -03:00
#pragma once
#include "Sub.h"
class Parameters;
class ParametersG2;
class GCS_Sub;
// Guided modes
enum GuidedSubMode {
Guided_WP,
Guided_Velocity,
Guided_PosVel,
Guided_Angle,
};
// Auto modes
enum AutoSubMode {
Auto_WP,
Auto_CircleMoveToEdge,
Auto_Circle,
Auto_NavGuided,
Auto_Loiter,
Auto_TerrainRecover
};
// RTL states
enum RTLState {
RTL_InitialClimb,
RTL_ReturnHome,
RTL_LoiterAtHome,
RTL_FinalDescent,
RTL_Land
};
class Mode
{
public:
// Auto Pilot Modes enumeration
enum class Number : uint8_t {
STABILIZE = 0, // manual angle with manual depth/throttle
ACRO = 1, // manual body-frame angular rate with manual depth/throttle
ALT_HOLD = 2, // manual angle with automatic depth/throttle
AUTO = 3, // fully automatic waypoint control using mission commands
GUIDED = 4, // fully automatic fly to coordinate or fly at velocity/direction using GCS immediate commands
CIRCLE = 7, // automatic circular flight with automatic throttle
SURFACE = 9, // automatically return to surface, pilot maintains horizontal control
POSHOLD = 16, // automatic position hold with manual override, with automatic throttle
MANUAL = 19, // Pass-through input with no stabilization
MOTOR_DETECT = 20 // Automatically detect motors orientation
};
// constructor
Mode(void);
// do not allow copying
CLASS_NO_COPY(Mode);
// child classes should override these methods
virtual bool init(bool ignore_checks) { return true; }
virtual void run() = 0;
virtual bool requires_GPS() const = 0;
virtual bool has_manual_throttle() const = 0;
virtual bool allows_arming(bool from_gcs) const = 0;
virtual bool is_autopilot() const { return false; }
virtual bool in_guided_mode() const { return false; }
// return a string for this flightmode
virtual const char *name() const = 0;
virtual const char *name4() const = 0;
// functions for reporting to GCS
virtual bool get_wp(Location &loc) { return false; }
virtual int32_t wp_bearing() const { return 0; }
virtual uint32_t wp_distance() const { return 0; }
virtual float crosstrack_error() const { return 0.0f; }
// pilot input processing
void get_pilot_desired_accelerations(float &right_out, float &front_out) const;
void get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int16_t yaw_in, float &roll_out, float &pitch_out, float &yaw_out);
protected:
// navigation support functions
virtual void run_autopilot() {}
// helper functions
bool is_disarmed_or_landed() const;
// functions to control landing
// in modes that support landing
void land_run_horizontal_control();
void land_run_vertical_control(bool pause_descent = false);
// convenience references to avoid code churn in conversion:
Parameters &g;
ParametersG2 &g2;
AP_InertialNav &inertial_nav;
AP_AHRS &ahrs;
AP_Motors6DOF &motors;
RC_Channel *&channel_roll;
RC_Channel *&channel_pitch;
RC_Channel *&channel_throttle;
RC_Channel *&channel_yaw;
RC_Channel *&channel_forward;
RC_Channel *&channel_lateral;
AC_PosControl *position_control;
AC_AttitudeControl_Sub *attitude_control;
// TODO: channels
float &G_Dt;
public:
// Navigation Yaw control
class AutoYaw
{
public:
// mode(): current method of determining desired yaw:
autopilot_yaw_mode mode() const
{
return (autopilot_yaw_mode)_mode;
}
void set_mode_to_default(bool rtl);
void set_mode(autopilot_yaw_mode new_mode);
autopilot_yaw_mode default_mode(bool rtl) const;
void set_rate(float new_rate_cds);
// set_roi(...): set a "look at" location:
void set_roi(const Location &roi_location);
void set_fixed_yaw(float angle_deg,
float turn_rate_dps,
int8_t direction,
bool relative_angle);
private:
// yaw_cd(): main product of AutoYaw; the heading:
float yaw_cd();
// rate_cds(): desired yaw rate in centidegrees/second:
float rate_cds();
float look_ahead_yaw();
float roi_yaw();
// auto flight mode's yaw mode
uint8_t _mode = AUTO_YAW_LOOK_AT_NEXT_WP;
// Yaw will point at this location if mode is set to AUTO_YAW_ROI
Vector3f roi;
// bearing from current location to the ROI
float _roi_yaw;
// yaw used for YAW_FIXED yaw_mode
int32_t _fixed_yaw;
// Deg/s we should turn
int16_t _fixed_yaw_slewrate;
// heading when in yaw_look_ahead_yaw
float _look_ahead_yaw;
// used to reduce update rate to 100hz:
uint8_t roi_yaw_counter;
GuidedSubMode guided_mode;
};
static AutoYaw auto_yaw;
// pass-through functions to reduce code churn on conversion;
// these are candidates for moving into the Mode base
// class.
bool set_mode(Mode::Number mode, ModeReason reason);
GCS_Sub &gcs();
// end pass-through functions
};
class ModeManual : public Mode
{
public:
// inherit constructor
using Mode::Mode;
virtual void run() override;
bool init(bool ignore_checks) override;
bool requires_GPS() const override { return false; }
bool has_manual_throttle() const override { return true; }
bool allows_arming(bool from_gcs) const override { return true; }
bool is_autopilot() const override { return false; }
protected:
const char *name() const override { return "MANUAL"; }
const char *name4() const override { return "MANU"; }
};
class ModeAcro : public Mode
{
public:
// inherit constructor
using Mode::Mode;
virtual void run() override;
bool init(bool ignore_checks) override;
bool requires_GPS() const override { return false; }
bool has_manual_throttle() const override { return true; }
bool allows_arming(bool from_gcs) const override { return true; }
bool is_autopilot() const override { return false; }
protected:
const char *name() const override { return "ACRO"; }
const char *name4() const override { return "ACRO"; }
};
class ModeStabilize : public Mode
{
public:
// inherit constructor
using Mode::Mode;
virtual void run() override;
bool init(bool ignore_checks) override;
bool requires_GPS() const override { return false; }
bool has_manual_throttle() const override { return true; }
bool allows_arming(bool from_gcs) const override { return true; }
bool is_autopilot() const override { return false; }
protected:
const char *name() const override { return "STABILIZE"; }
const char *name4() const override { return "STAB"; }
};
class ModeAlthold : public Mode
{
public:
// inherit constructor
using Mode::Mode;
virtual void run() override;
bool init(bool ignore_checks) override;
bool requires_GPS() const override { return false; }
bool has_manual_throttle() const override { return false; }
bool allows_arming(bool from_gcs) const override { return true; }
bool is_autopilot() const override { return false; }
void control_depth();
protected:
const char *name() const override { return "ALT_HOLD"; }
const char *name4() const override { return "ALTH"; }
};
class ModeGuided : public Mode
{
public:
// inherit constructor
using Mode::Mode;
virtual void run() override;
bool init(bool ignore_checks) override;
bool requires_GPS() const override { return true; }
bool has_manual_throttle() const override { return false; }
bool allows_arming(bool from_gcs) const override { return true; }
bool is_autopilot() const override { return true; }
bool guided_limit_check();
void guided_limit_init_time_and_pos();
void guided_set_angle(const Quaternion &q, float climb_rate_cms, bool use_yaw_rate, float yaw_rate_rads);
void guided_set_angle(const Quaternion&, float);
void guided_limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_max_cm, float horiz_max_cm);
bool guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity);
bool guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw);
2023-04-03 09:11:21 -03:00
bool guided_set_destination(const Vector3f& destination);
bool guided_set_destination(const Location&);
bool guided_set_destination(const Vector3f& destination, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw);
2023-04-03 09:11:21 -03:00
void guided_set_velocity(const Vector3f& velocity);
void guided_set_velocity(const Vector3f& velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw);
void guided_set_yaw_state(bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_angle);
2023-04-03 09:11:21 -03:00
float get_auto_heading();
void guided_limit_clear();
void set_auto_yaw_mode(autopilot_yaw_mode yaw_mode);
protected:
const char *name() const override { return "GUIDED"; }
const char *name4() const override { return "GUID"; }
autopilot_yaw_mode get_default_auto_yaw_mode(bool rtl) const;
private:
void guided_pos_control_run();
void guided_vel_control_run();
void guided_posvel_control_run();
void guided_angle_control_run();
void guided_takeoff_run();
void guided_pos_control_start();
void guided_vel_control_start();
void guided_posvel_control_start();
void guided_angle_control_start();
};
class ModeAuto : public ModeGuided
{
public:
// inherit constructor
using ModeGuided::ModeGuided;
virtual void run() override;
bool init(bool ignore_checks) override;
bool requires_GPS() const override { return true; }
bool has_manual_throttle() const override { return false; }
bool allows_arming(bool from_gcs) const override { return true; }
bool is_autopilot() const override { return true; }
bool auto_loiter_start();
void auto_wp_start(const Vector3f& destination);
void auto_wp_start(const Location& dest_loc);
void auto_circle_movetoedge_start(const Location &circle_center, float radius_m, bool ccw_turn);
void auto_circle_start();
void auto_nav_guided_start();
void set_auto_yaw_roi(const Location &roi_location);
void set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, int8_t direction, uint8_t relative_angle);
void set_yaw_rate(float turn_rate_dps);
2023-04-03 09:11:21 -03:00
bool auto_terrain_recover_start();
protected:
const char *name() const override { return "AUTO"; }
const char *name4() const override { return "AUTO"; }
private:
void auto_wp_run();
void auto_circle_run();
void auto_nav_guided_run();
void auto_loiter_run();
void auto_terrain_recover_run();
};
class ModePoshold : public ModeAlthold
{
public:
// inherit constructor
using ModeAlthold::ModeAlthold;
virtual void run() override;
bool init(bool ignore_checks) override;
bool requires_GPS() const override { return false; }
bool has_manual_throttle() const override { return false; }
bool allows_arming(bool from_gcs) const override { return true; }
bool is_autopilot() const override { return true; }
protected:
const char *name() const override { return "POSHOLD"; }
const char *name4() const override { return "POSH"; }
};
class ModeCircle : public Mode
{
public:
// inherit constructor
using Mode::Mode;
virtual void run() override;
bool init(bool ignore_checks) override;
bool requires_GPS() const override { return true; }
bool has_manual_throttle() const override { return false; }
bool allows_arming(bool from_gcs) const override { return true; }
bool is_autopilot() const override { return true; }
protected:
const char *name() const override { return "CIRCLE"; }
const char *name4() const override { return "CIRC"; }
};
class ModeSurface : public Mode
{
public:
// inherit constructor
using Mode::Mode;
virtual void run() override;
bool init(bool ignore_checks) override;
bool requires_GPS() const override { return true; }
bool has_manual_throttle() const override { return false; }
bool allows_arming(bool from_gcs) const override { return true; }
bool is_autopilot() const override { return true; }
protected:
const char *name() const override { return "SURFACE"; }
const char *name4() const override { return "SURF"; }
};
class ModeMotordetect : public Mode
{
public:
// inherit constructor
using Mode::Mode;
virtual void run() override;
bool init(bool ignore_checks) override;
bool requires_GPS() const override { return true; }
bool has_manual_throttle() const override { return false; }
bool allows_arming(bool from_gcs) const override { return true; }
bool is_autopilot() const override { return true; }
protected:
const char *name() const override { return "MOTORDETECT"; }
const char *name4() const override { return "DETE"; }
};