mirror of https://github.com/ArduPilot/ardupilot
485 lines
14 KiB
C++
485 lines
14 KiB
C++
#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
|
|
SURFTRAK = 21 // Track distance above seafloor (hold range)
|
|
};
|
|
|
|
// 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:
|
|
|
|
void run_pre();
|
|
void run_post();
|
|
|
|
const char *name() const override { return "ALT_HOLD"; }
|
|
const char *name4() const override { return "ALTH"; }
|
|
};
|
|
|
|
|
|
class ModeSurftrak : public ModeAlthold
|
|
{
|
|
|
|
public:
|
|
// constructor
|
|
ModeSurftrak();
|
|
|
|
void run() override;
|
|
|
|
bool init(bool ignore_checks) override;
|
|
|
|
float get_rangefinder_target_cm() const WARN_IF_UNUSED { return rangefinder_target_cm; }
|
|
bool set_rangefinder_target_cm(float target_cm);
|
|
|
|
protected:
|
|
|
|
const char *name() const override { return "SURFTRAK"; }
|
|
const char *name4() const override { return "STRK"; }
|
|
|
|
private:
|
|
|
|
void reset();
|
|
void control_range();
|
|
void update_surface_offset();
|
|
|
|
float rangefinder_target_cm;
|
|
|
|
bool pilot_in_control; // pilot is moving up/down
|
|
float pilot_control_start_z_cm; // alt when pilot took control
|
|
};
|
|
|
|
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);
|
|
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);
|
|
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);
|
|
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);
|
|
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"; }
|
|
};
|