2017-07-18 23:17:45 -03:00
# pragma once
2022-08-19 07:55:01 -03:00
# include "Rover.h"
2017-07-18 23:17:45 -03:00
2017-11-22 08:38:57 -04:00
// pre-define ModeRTL so Auto can appear higher in this file
class ModeRTL ;
2017-07-18 23:17:45 -03:00
class Mode
{
public :
2018-06-06 03:41:17 -03:00
// Auto Pilot modes
// ----------------
2023-09-21 12:47:41 -03:00
enum class Number : uint8_t {
2018-06-06 03:41:17 -03:00
MANUAL = 0 ,
ACRO = 1 ,
STEERING = 3 ,
HOLD = 4 ,
LOITER = 5 ,
2018-05-24 01:47:07 -03:00
FOLLOW = 6 ,
2018-07-02 04:21:37 -03:00
SIMPLE = 7 ,
2024-08-04 21:54:28 -03:00
# if MODE_DOCK_ENABLED
2022-07-10 01:02:55 -03:00
DOCK = 8 ,
# endif
2023-04-20 08:53:05 -03:00
CIRCLE = 9 ,
2018-06-06 03:41:17 -03:00
AUTO = 10 ,
RTL = 11 ,
SMART_RTL = 12 ,
GUIDED = 15 ,
2022-07-10 01:02:55 -03:00
INITIALISING = 16 ,
2018-06-06 03:41:17 -03:00
} ;
2017-07-18 23:17:45 -03:00
// Constructor
Mode ( ) ;
2019-01-02 23:07:55 -04:00
// do not allow copying
2022-09-30 06:50:43 -03:00
CLASS_NO_COPY ( Mode ) ;
2019-01-02 23:07:55 -04:00
2017-07-18 23:17:45 -03:00
// enter this mode, returns false if we failed to enter
bool enter ( ) ;
// perform any cleanups required:
void exit ( ) ;
// returns a unique number specific to this mode
2023-09-21 12:47:41 -03:00
virtual Number mode_number ( ) const = 0 ;
2017-07-18 23:17:45 -03:00
2017-11-28 22:35:33 -04:00
// returns short text name (up to 4 bytes)
virtual const char * name4 ( ) const = 0 ;
2017-07-18 23:17:45 -03: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 ;
//
// attributes of the mode
//
2018-01-10 21:54:40 -04:00
// return if in non-manual mode : Auto, Guided, RTL, SmartRTL
2017-07-18 23:17:45 -03:00
virtual bool is_autopilot_mode ( ) const { return false ; }
2019-03-08 01:41:50 -04:00
// return if external control is allowed in this mode (Guided or Guided-within-Auto)
virtual bool in_guided_mode ( ) const { return false ; }
2018-01-10 21:54:40 -04:00
// returns true if vehicle can be armed or disarmed from the transmitter in this mode
virtual bool allows_arming_from_transmitter ( ) { return ! is_autopilot_mode ( ) ; }
2017-07-18 23:17:45 -03:00
2020-02-13 02:59:42 -04:00
// returns false if vehicle cannot be armed in this mode
virtual bool allows_arming ( ) const { return true ; }
2019-04-20 20:10:51 -03:00
bool allows_stick_mixing ( ) const { return is_autopilot_mode ( ) ; }
2017-07-18 23:17:45 -03:00
//
// attributes for mavlink system status reporting
//
// returns true if any RC input is used
virtual bool has_manual_input ( ) const { return false ; }
// true if heading is controlled
virtual bool attitude_stabilized ( ) const { return true ; }
2018-01-22 07:00:47 -04:00
// true if mode requires position and/or velocity estimate
virtual bool requires_position ( ) const { return true ; }
virtual bool requires_velocity ( ) const { return true ; }
2017-11-29 02:30:37 -04:00
2018-12-11 21:07:47 -04:00
// return heading (in degrees) and cross track error (in meters) for reporting to ground station (NAV_CONTROLLER_OUTPUT message)
virtual float wp_bearing ( ) const ;
virtual float nav_bearing ( ) const ;
virtual float crosstrack_error ( ) const ;
2019-05-06 02:57:03 -03:00
virtual float get_desired_lat_accel ( ) const ;
2018-12-11 10:04:50 -04:00
2019-05-04 03:23:16 -03:00
// get speed error in m/s, not currently supported
float speed_error ( ) const { return 0.0f ; }
2017-08-03 05:08:09 -03:00
//
// navigation methods
//
// return distance (in meters) to destination
virtual float get_distance_to_destination ( ) const { return 0.0f ; }
2019-05-17 03:55:31 -03:00
// return desired location (used in Guided, Auto, RTL, etc)
// return true on success, false if there is no valid destination
virtual bool get_desired_location ( Location & destination ) const WARN_IF_UNUSED { return false ; }
2019-05-04 03:49:28 -03:00
// set desired location (used in Guided, Auto)
2021-11-18 23:38:12 -04:00
// set next_destination (if known). If not provided vehicle stops at destination
virtual bool set_desired_location ( const Location & destination , Location next_destination = Location ( ) ) WARN_IF_UNUSED ;
2017-08-09 22:30:35 -03:00
2017-08-03 05:08:09 -03:00
// true if vehicle has reached desired location. defaults to true because this is normally used by missions and we do not want the mission to become stuck
2019-03-15 01:26:01 -03:00
virtual bool reached_destination ( ) const { return true ; }
2017-08-04 20:59:15 -03:00
2017-12-05 21:41:28 -04:00
// get default speed for this mode (held in CRUISE_SPEED, WP_SPEED or RTL_SPEED)
// rtl argument should be true if called from RTL or SmartRTL modes (handled here to avoid duplication)
float get_speed_default ( bool rtl = false ) const ;
2018-06-08 22:19:41 -03:00
// set desired speed in m/s
2019-11-04 04:31:13 -04:00
virtual bool set_desired_speed ( float speed ) { return false ; }
2017-12-05 21:41:28 -04:00
2018-08-08 01:48:38 -03:00
// execute the mission in reverse (i.e. backing up)
void set_reversed ( bool value ) ;
2024-06-17 03:03:10 -03:00
// init reversed flag for autopilot mode
virtual void init_reversed_flag ( ) { if ( is_autopilot_mode ( ) ) { set_reversed ( false ) ; } }
2018-09-14 04:09:07 -03:00
// handle tacking request (from auxiliary switch) in sailboats
virtual void handle_tack_request ( ) ;
2017-07-18 23:17:45 -03: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 ; }
2018-05-05 22:47:59 -03:00
// decode pilot steering and throttle inputs and return in steer_out and throttle_out arguments
2018-05-03 05:57:40 -03:00
// steering_out is in the range -4500 ~ +4500 with positive numbers meaning rotate clockwise
// throttle_out is in the range -100 ~ +100
2024-02-18 06:33:45 -04:00
void get_pilot_desired_steering_and_throttle ( float & steering_out , float & throttle_out ) const ;
2017-11-27 09:11:45 -04:00
2018-05-05 22:47:59 -03:00
// decode pilot input steering and return steering_out and speed_out (in m/s)
2024-02-18 06:33:45 -04:00
void get_pilot_desired_steering_and_speed ( float & steering_out , float & speed_out ) const ;
2018-05-05 22:47:59 -03:00
2018-05-10 04:10:34 -03:00
// decode pilot lateral movement input and return in lateral_out argument
2024-02-18 06:33:45 -04:00
void get_pilot_desired_lateral ( float & lateral_out ) const ;
2018-05-10 04:10:34 -03:00
2018-07-02 04:21:37 -03:00
// decode pilot's input and return heading_out (in cd) and speed_out (in m/s)
2024-02-18 06:33:45 -04:00
void get_pilot_desired_heading_and_speed ( float & heading_out , float & speed_out ) const ;
2018-07-02 04:21:37 -03:00
2020-07-22 10:42:56 -03:00
// decode pilot roll and pitch inputs and return in roll_out and pitch_out arguments
// outputs are in the range -1 to +1
2024-02-18 06:33:45 -04:00
void get_pilot_desired_roll_and_pitch ( float & roll_out , float & pitch_out ) const ;
2020-07-22 10:42:56 -03:00
2020-08-28 15:51:26 -03:00
// decode pilot height inputs and return in height_out arguments
// outputs are in the range -1 to +1
2024-02-18 06:33:45 -04:00
void get_pilot_desired_walking_height ( float & walking_height_out ) const ;
2020-08-28 15:51:26 -03:00
2019-04-29 03:31:45 -03:00
// high level call to navigate to waypoint
void navigate_to_waypoint ( ) ;
2020-09-27 14:43:26 -03:00
// calculate steering output given a turn rate
// desired turn rate in radians/sec. Positive to the right.
void calc_steering_from_turn_rate ( float turn_rate ) ;
2017-12-23 01:54:11 -04:00
2017-07-18 23:17:45 -03:00
// calculate steering angle given a desired lateral acceleration
2017-12-23 02:17:49 -04:00
void calc_steering_from_lateral_acceleration ( float lat_accel , bool reversed = false ) ;
2017-07-18 23:17:45 -03:00
2017-12-23 01:54:35 -04:00
// calculate steering output to drive towards desired heading
2018-09-10 04:25:27 -03:00
// rate_max is a maximum turn rate in deg/s. set to zero to use default turn rate limits
void calc_steering_to_heading ( float desired_heading_cd , float rate_max_degs = 0.0f ) ;
2017-08-03 05:08:09 -03:00
// calculates the amount of throttle that should be output based
// on things like proximity to corners and current speed
2019-05-04 00:09:24 -03:00
virtual void calc_throttle ( float target_speed , bool avoidance_enabled ) ;
2017-07-18 23:17:45 -03:00
2017-08-10 00:06:43 -03:00
// performs a controlled stop. returns true once vehicle has stopped
bool stop_vehicle ( ) ;
2017-08-08 21:24:30 -03:00
// estimate maximum vehicle speed (in m/s)
2017-12-31 23:46:46 -04:00
// cruise_speed is in m/s, cruise_throttle should be in the range -1 to +1
float calc_speed_max ( float cruise_speed , float cruise_throttle ) const ;
2017-08-08 21:24:30 -03:00
// calculate pilot input to nudge speed up or down
// target_speed should be in meters/sec
2019-05-04 00:09:24 -03:00
// reversed should be true if the vehicle is intentionally backing up which allows the pilot to increase the backing up speed by pulling the throttle stick down
float calc_speed_nudge ( float target_speed , bool reversed ) ;
2017-07-18 04:27:05 -03:00
2018-05-05 22:47:59 -03:00
protected :
// decode pilot steering and throttle inputs and return in steer_out and throttle_out arguments
// steering_out is in the range -4500 ~ +4500 with positive numbers meaning rotate clockwise
// throttle_out is in the range -100 ~ +100
2024-02-18 06:33:45 -04:00
void get_pilot_input ( float & steering_out , float & throttle_out ) const ;
2019-04-20 20:02:51 -03:00
void set_steering ( float steering_value ) ;
2018-05-05 22:47:59 -03:00
2017-07-18 23:17:45 -03:00
// references to avoid code churn:
2017-08-03 05:08:09 -03:00
class AP_AHRS & ahrs ;
2017-07-18 23:17:45 -03:00
class Parameters & g ;
class ParametersG2 & g2 ;
2017-08-03 06:41:47 -03:00
class RC_Channel * & channel_steer ;
2017-07-18 23:17:45 -03:00
class RC_Channel * & channel_throttle ;
2018-05-10 04:10:34 -03:00
class RC_Channel * & channel_lateral ;
2020-07-22 10:42:56 -03:00
class RC_Channel * & channel_roll ;
class RC_Channel * & channel_pitch ;
2020-08-28 15:51:26 -03:00
class RC_Channel * & channel_walking_height ;
2017-08-08 02:37:21 -03:00
class AR_AttitudeControl & attitude_control ;
2017-08-03 05:08:09 -03:00
// private members for waypoint navigation
float _distance_to_destination ; // distance from vehicle to final destination in meters
bool _reached_destination ; // true once the vehicle has reached the destination
2019-05-04 03:40:58 -03:00
float _desired_yaw_cd ; // desired yaw in centi-degrees. used in Auto, Guided and Loiter
2017-07-18 23:17:45 -03:00
} ;
2017-11-28 02:59:13 -04:00
class ModeAcro : public Mode
{
public :
2023-09-21 12:47:41 -03:00
Number mode_number ( ) const override { return Number : : ACRO ; }
2017-11-28 22:35:33 -04:00
const char * name4 ( ) const override { return " ACRO " ; }
2017-11-28 02:59:13 -04:00
// methods that affect movement of the vehicle in this mode
void update ( ) override ;
// attributes for mavlink system status reporting
bool has_manual_input ( ) const override { return true ; }
2018-01-22 07:00:47 -04:00
2018-04-11 16:48:25 -03:00
// acro mode requires a velocity estimate for non skid-steer rovers
2018-01-22 07:00:47 -04:00
bool requires_position ( ) const override { return false ; }
2018-04-11 16:48:25 -03:00
bool requires_velocity ( ) const override ;
2018-09-14 04:09:07 -03:00
// sailboats in acro mode support user manually initiating tacking from transmitter
void handle_tack_request ( ) override ;
2017-11-28 02:59:13 -04:00
} ;
2017-07-18 23:17:45 -03:00
class ModeAuto : public Mode
{
public :
2023-09-21 12:47:41 -03:00
Number mode_number ( ) const override { return Number : : AUTO ; }
2017-11-28 22:35:33 -04:00
const char * name4 ( ) const override { return " AUTO " ; }
2017-07-18 23:17:45 -03:00
// methods that affect movement of the vehicle in this mode
void update ( ) override ;
2019-05-04 00:09:24 -03:00
void calc_throttle ( float target_speed , bool avoidance_enabled ) override ;
2017-07-18 23:17:45 -03:00
// attributes of the mode
bool is_autopilot_mode ( ) const override { return true ; }
2019-03-08 01:41:50 -04:00
// return if external control is allowed in this mode (Guided or Guided-within-Auto)
2023-09-21 10:56:25 -03:00
bool in_guided_mode ( ) const override { return _submode = = SubMode : : Guided | | _submode = = SubMode : : NavScriptTime ; }
2019-03-08 01:41:50 -04:00
2023-04-20 08:53:05 -03:00
// return heading (in degrees) and cross track error (in meters) for reporting to ground station (NAV_CONTROLLER_OUTPUT message)
float wp_bearing ( ) const override ;
float nav_bearing ( ) const override ;
float crosstrack_error ( ) const override ;
float get_desired_lat_accel ( ) const override ;
2017-08-03 05:11:45 -03:00
// return distance (in meters) to destination
2017-12-08 23:32:53 -04:00
float get_distance_to_destination ( ) const override ;
2017-08-03 05:11:45 -03:00
2019-05-17 03:55:31 -03:00
// get or set desired location
bool get_desired_location ( Location & destination ) const override WARN_IF_UNUSED ;
2021-11-18 23:38:12 -04:00
bool set_desired_location ( const Location & destination , Location next_destination = Location ( ) ) override WARN_IF_UNUSED ;
2019-03-15 01:26:01 -03:00
bool reached_destination ( ) const override ;
2017-08-03 05:11:45 -03:00
2019-11-04 04:31:13 -04:00
// set desired speed in m/s
bool set_desired_speed ( float speed ) override ;
2017-08-03 05:11:45 -03:00
2017-11-22 08:38:57 -04:00
// start RTL (within auto)
void start_RTL ( ) ;
2022-02-19 01:36:35 -04:00
// lua accessors for nav script time support
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 ) ;
2022-02-19 01:36:35 -04:00
void nav_script_time_done ( uint16_t id ) ;
2024-06-17 03:03:10 -03:00
//
void init_reversed_flag ( ) override {
if ( ! mission . is_resume ( ) ) {
set_reversed ( false ) ;
}
}
2018-12-11 20:10:20 -04:00
AP_Mission mission {
FUNCTOR_BIND_MEMBER ( & ModeAuto : : start_command , bool , const AP_Mission : : Mission_Command & ) ,
FUNCTOR_BIND_MEMBER ( & ModeAuto : : verify_command_callback , bool , const AP_Mission : : Mission_Command & ) ,
FUNCTOR_BIND_MEMBER ( & ModeAuto : : exit_mission , void ) } ;
2024-06-17 02:11:56 -03:00
enum class DoneBehaviour : uint8_t {
HOLD = 0 ,
LOITER = 1 ,
ACRO = 2 ,
MANUAL = 3 ,
2021-11-23 14:48:40 -04:00
} ;
2017-07-18 23:17:45 -03:00
protected :
bool _enter ( ) override ;
void _exit ( ) override ;
2017-08-03 05:11:45 -03:00
2023-09-21 10:56:25 -03:00
enum SubMode : uint8_t {
WP , // drive to a given location
HeadingAndSpeed , // turn to a given heading
RTL , // perform RTL within auto mode
Loiter , // perform Loiter within auto mode
Guided , // handover control to external navigation system from within auto mode
Stop , // stop the vehicle as quickly as possible
NavScriptTime , // accept targets from lua scripts while NAV_SCRIPT_TIME commands are executing
Circle , // circle a given location
2017-08-03 05:11:45 -03:00
} _submode ;
2017-07-18 23:17:45 -03:00
private :
bool check_trigger ( void ) ;
2019-03-15 21:43:49 -03:00
bool start_loiter ( ) ;
void start_guided ( const Location & target_loc ) ;
2019-08-13 23:26:11 -03:00
void start_stop ( ) ;
2019-03-15 21:43:49 -03:00
void send_guided_position_target ( ) ;
2018-12-11 20:10:20 -04:00
bool start_command ( const AP_Mission : : Mission_Command & cmd ) ;
void exit_mission ( ) ;
bool verify_command_callback ( const AP_Mission : : Mission_Command & cmd ) ;
bool verify_command ( const AP_Mission : : Mission_Command & cmd ) ;
void do_RTL ( void ) ;
2019-05-08 21:54:40 -03:00
bool do_nav_wp ( const AP_Mission : : Mission_Command & cmd , bool always_stop_at_destination ) ;
2019-03-08 01:41:50 -04:00
void do_nav_guided_enable ( const AP_Mission : : Mission_Command & cmd ) ;
2018-12-11 20:10:20 -04:00
void do_nav_set_yaw_speed ( const AP_Mission : : Mission_Command & cmd ) ;
2019-08-09 15:42:19 -03:00
void do_nav_delay ( const AP_Mission : : Mission_Command & cmd ) ;
bool verify_nav_delay ( const AP_Mission : : Mission_Command & cmd ) ;
2018-12-11 20:10:20 -04:00
bool verify_nav_wp ( const AP_Mission : : Mission_Command & cmd ) ;
2021-02-01 12:26:24 -04:00
bool verify_RTL ( ) const ;
2018-12-11 20:10:20 -04:00
bool verify_loiter_unlimited ( const AP_Mission : : Mission_Command & cmd ) ;
bool verify_loiter_time ( const AP_Mission : : Mission_Command & cmd ) ;
2019-03-08 01:41:50 -04:00
bool verify_nav_guided_enable ( const AP_Mission : : Mission_Command & cmd ) ;
2018-12-11 20:10:20 -04:00
bool verify_nav_set_yaw_speed ( ) ;
2023-04-20 08:53:05 -03:00
bool do_circle ( const AP_Mission : : Mission_Command & cmd ) ;
bool verify_circle ( const AP_Mission : : Mission_Command & cmd ) ;
2018-12-11 20:10:20 -04:00
void do_wait_delay ( const AP_Mission : : Mission_Command & cmd ) ;
void do_within_distance ( const AP_Mission : : Mission_Command & cmd ) ;
bool verify_wait_delay ( ) ;
bool verify_within_distance ( ) ;
void do_change_speed ( const AP_Mission : : Mission_Command & cmd ) ;
void do_set_home ( const AP_Mission : : Mission_Command & cmd ) ;
void do_set_reverse ( const AP_Mission : : Mission_Command & cmd ) ;
2019-03-08 04:53:22 -04:00
void do_guided_limits ( const AP_Mission : : Mission_Command & cmd ) ;
2022-02-19 01:36:35 -04:00
# if AP_SCRIPTING_ENABLED
void do_nav_script_time ( const AP_Mission : : Mission_Command & cmd ) ;
bool verify_nav_script_time ( ) ;
# endif
2018-12-11 20:10:20 -04:00
2021-11-15 02:06:45 -04:00
bool waiting_to_start ; // true if waiting for EKF origin before starting mission
2019-03-15 21:43:49 -03:00
bool auto_triggered ; // true when auto has been triggered to start
2019-11-04 04:31:13 -04:00
// HeadingAndSpeed sub mode variables
float _desired_speed ; // desired speed in HeadingAndSpeed submode
bool _reached_heading ; // true when vehicle has reached desired heading in TurnToHeading sub mode
2019-03-15 21:43:49 -03:00
2018-12-11 20:10:20 -04:00
// Loiter control
uint16_t loiter_duration ; // How long we should loiter at the nav_waypoint (time in seconds)
uint32_t loiter_start_time ; // How long have we been loitering - The start time in millis
bool previously_reached_wp ; // set to true if we have EVER reached the waypoint
2019-03-15 01:22:36 -03:00
// Guided-within-Auto variables
struct {
Location loc ; // location target sent to external navigation
bool valid ; // true if loc is valid
uint32_t last_sent_ms ; // system time that target was last sent to offboard navigation
} guided_target ;
2019-03-08 01:41:50 -04:00
2018-12-11 20:10:20 -04:00
// 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
int32_t condition_start ;
2019-08-09 15:42:19 -03:00
// Delay the next navigation command
uint32_t nav_delay_time_max_ms ; // used for delaying the navigation commands
uint32_t nav_delay_time_start_ms ;
2022-02-19 01:36:35 -04:00
# if AP_SCRIPTING_ENABLED
// nav_script_time command variables
struct {
bool done ; // true once lua script indicates it has completed
uint16_t id ; // unique id to avoid race conditions between commands and lua scripts
uint32_t start_ms ; // system time nav_script_time command was received (used for timeout)
uint8_t command ; // command number provided by mission command
uint8_t timeout_s ; // timeout (in seconds) provided by mission command
float arg1 ; // 1st argument provided by mission command
float arg2 ; // 2nd argument provided by mission command
2022-10-13 21:37:52 -03:00
int16_t arg3 ; // 3rd argument provided by mission command
int16_t arg4 ; // 4th argument provided by mission command
2022-02-19 01:36:35 -04:00
} nav_scripting ;
# endif
2021-11-15 06:27:35 -04:00
// Mission change detector
AP_Mission_ChangeDetector mis_change_detector ;
2017-07-18 23:17:45 -03:00
} ;
2023-04-20 08:53:05 -03:00
class ModeCircle : public Mode
{
public :
// need a constructor for parameters
ModeCircle ( ) ;
// Does not allow copies
CLASS_NO_COPY ( ModeCircle ) ;
2023-09-21 12:47:41 -03:00
Number mode_number ( ) const override { return Number : : CIRCLE ; }
2023-04-20 08:53:05 -03:00
const char * name4 ( ) const override { return " CIRC " ; }
// initialise with specific center location, radius (in meters) and direction
// replaces use of _enter when initialised from within Auto mode
bool set_center ( const Location & center_loc , float radius_m , bool dir_ccw ) ;
// methods that affect movement of the vehicle in this mode
void update ( ) override ;
bool is_autopilot_mode ( ) const override { return true ; }
// return desired heading (in degrees) and cross track error (in meters) for reporting to ground station (NAV_CONTROLLER_OUTPUT message)
float wp_bearing ( ) const override ;
float nav_bearing ( ) const override ;
float crosstrack_error ( ) const override { return dist_to_edge_m ; }
float get_desired_lat_accel ( ) const override ;
// set desired speed in m/s
bool set_desired_speed ( float speed_ms ) override ;
// return distance (in meters) to destination
float get_distance_to_destination ( ) const override { return _distance_to_destination ; }
// get or set desired location
bool get_desired_location ( Location & destination ) const override WARN_IF_UNUSED ;
// return total angle in radians that vehicle has circled
// fabsf is used so that full rotations in either direction are counted
float get_angle_total_rad ( ) const { return fabsf ( angle_total_rad ) ; }
static const struct AP_Param : : GroupInfo var_info [ ] ;
protected :
AP_Float radius ; // circle radius in meters
AP_Float speed ; // vehicle speed in m/s. If zero uses WP_SPEED
AP_Int8 direction ; // direction 0:clockwise, 1:counter-clockwise
// initialise mode
bool _enter ( ) override ;
2024-07-27 18:34:31 -03:00
// Update position controller targets driving to the circle edge
void update_drive_to_radius ( ) ;
// Update position controller targets while circling
void update_circling ( ) ;
2023-04-20 08:53:05 -03:00
// initialise target_yaw_rad using the vehicle's position and yaw
// if there is no current position estimate target_yaw_rad is set to vehicle yaw
void init_target_yaw_rad ( ) ;
2023-06-27 22:15:16 -03:00
// limit config speed so that lateral acceleration is within limits
// outputs warning to user if speed is reduced
void check_config_speed ( ) ;
// ensure config radius is no smaller then vehicle's TURN_RADIUS
// radius is increased if necessary and warning is output to the user
void check_config_radius ( ) ;
2023-04-20 08:53:05 -03:00
// enum for Direction parameter
enum class Direction {
CW = 0 ,
CCW = 1
} ;
// local members
struct {
Location center_loc ; // circle center as a Location
Vector2f center_pos ; // circle center as an offset (in meters) from the EKF origin
float radius ; // circle radius
float speed ; // desired speed around circle in m/s
Direction dir ; // direction, 0:clockwise, 1:counter-clockwise
} config ;
struct {
float speed ; // vehicle's target speed around circle in m/s
float yaw_rad ; // earth-frame angle of tarrget point on the circle
Vector2p pos ; // latest position target sent to position controller
Vector2f vel ; // latest velocity target sent to position controller
Vector2f accel ; // latest accel target sent to position controller
} target ;
float angle_total_rad ; // total angle in radians that vehicle has circled
bool reached_edge ; // true once vehicle has reached edge of circle
float dist_to_edge_m ; // distance to edge of circle in meters (equivalent to crosstrack error)
} ;
2017-07-18 23:17:45 -03:00
class ModeGuided : public Mode
{
public :
2023-10-07 07:04:56 -03:00
# if AP_EXTERNAL_CONTROL_ENABLED
friend class AP_ExternalControl_Rover ;
# endif
2017-07-18 23:17:45 -03:00
2023-09-21 12:47:41 -03:00
Number mode_number ( ) const override { return Number : : GUIDED ; }
2017-11-28 22:35:33 -04:00
const char * name4 ( ) const override { return " GUID " ; }
2017-07-18 23:17:45 -03:00
// methods that affect movement of the vehicle in this mode
void update ( ) override ;
// attributes of the mode
bool is_autopilot_mode ( ) const override { return true ; }
2019-03-08 01:41:50 -04:00
// return if external control is allowed in this mode (Guided or Guided-within-Auto)
bool in_guided_mode ( ) const override { return true ; }
2021-11-19 23:05:23 -04:00
// return heading (in degrees) and cross track error (in meters) for reporting to ground station (NAV_CONTROLLER_OUTPUT message)
float wp_bearing ( ) const override ;
float nav_bearing ( ) const override ;
float crosstrack_error ( ) const override ;
float get_desired_lat_accel ( ) const override ;
2017-08-03 05:14:16 -03:00
// return distance (in meters) to destination
float get_distance_to_destination ( ) const override ;
2019-03-08 01:41:50 -04:00
// return true if vehicle has reached destination
2019-03-15 01:26:01 -03:00
bool reached_destination ( ) const override ;
2019-03-08 01:41:50 -04:00
2019-11-04 04:31:13 -04:00
// set desired speed in m/s
bool set_desired_speed ( float speed ) override ;
2019-05-17 03:55:31 -03:00
// get or set desired location
bool get_desired_location ( Location & destination ) const override WARN_IF_UNUSED ;
2021-11-18 23:38:12 -04:00
bool set_desired_location ( const Location & destination , Location next_destination = Location ( ) ) override WARN_IF_UNUSED ;
2019-05-17 03:55:31 -03:00
// set desired heading and speed
2019-11-04 04:31:13 -04:00
void set_desired_heading_and_speed ( float yaw_angle_cd , float target_speed ) ;
2017-08-03 05:14:16 -03:00
// set desired heading-delta, turn-rate and speed
void set_desired_heading_delta_and_speed ( float yaw_delta_cd , float target_speed ) ;
void set_desired_turn_rate_and_speed ( float turn_rate_cds , float target_speed ) ;
2020-06-15 04:29:00 -03:00
// set steering and throttle (-1 to +1). Only called from scripts
void set_steering_and_throttle ( float steering , float throttle ) ;
2018-12-03 15:25:52 -04:00
// vehicle start loiter
bool start_loiter ( ) ;
2021-12-01 22:34:51 -04:00
// start stopping
void start_stop ( ) ;
2019-03-08 04:53:22 -04:00
// guided limits
void limit_set ( uint32_t timeout_ms , float horiz_max ) ;
void limit_clear ( ) ;
void limit_init_time_and_location ( ) ;
2019-03-15 01:22:36 -03:00
bool limit_breached ( ) const ;
2019-03-08 04:53:22 -04:00
2017-08-03 05:14:16 -03:00
protected :
2023-09-21 10:44:53 -03:00
enum class SubMode : uint8_t {
WP ,
HeadingAndSpeed ,
TurnRateAndSpeed ,
Loiter ,
SteeringAndThrottle ,
Stop
2017-07-18 23:17:45 -03:00
} ;
2021-11-19 23:05:23 -04:00
// enum for GUID_OPTIONS parameter
enum class Options : int32_t {
SCurvesUsedForNavigation = ( 1U < < 6 )
} ;
2017-08-03 05:14:16 -03:00
bool _enter ( ) override ;
2017-07-18 23:17:45 -03:00
2021-11-19 23:05:23 -04:00
// returns true if GUID_OPTIONS bit set to use scurve navigation instead of position controller input shaping
// scurves provide path planning and object avoidance but cannot handle fast updates to the destination (for fast updates use position controller input shaping)
bool use_scurves_for_navigation ( ) const ;
2023-09-21 10:44:53 -03:00
SubMode _guided_mode ; // stores which GUIDED mode the vehicle is in
2017-07-18 23:17:45 -03:00
2017-08-03 05:14:16 -03:00
// attitude control
bool have_attitude_target ; // true if we have a valid attitude target
uint32_t _des_att_time_ms ; // system time last call to set_desired_attitude was made (used for timeout)
float _desired_yaw_rate_cds ; // target turn rate centi-degrees per second
2021-03-03 00:51:46 -04:00
bool send_notification ; // used to send one time notification to ground station
2019-11-04 04:31:13 -04:00
float _desired_speed ; // desired speed used only in HeadingAndSpeed submode
2019-03-08 04:53:22 -04:00
2020-06-15 04:29:00 -03:00
// direct steering and throttle control
bool _have_strthr ; // true if we have a valid direct steering and throttle inputs
uint32_t _strthr_time_ms ; // system time last call to set_steering_and_throttle was made (used for timeout)
float _strthr_steering ; // direct steering input in the range -1 to +1
float _strthr_throttle ; // direct throttle input in the range -1 to +1
2019-03-08 04:53:22 -04:00
// limits
2019-03-15 01:22:36 -03:00
struct {
uint32_t timeout_ms ; // timeout from the time that guided is invoked
2019-03-08 04:53:22 -04:00
float horiz_max ; // horizontal position limit in meters from where guided mode was initiated (0 = no limit)
2019-03-15 03:00:31 -03:00
uint32_t start_time_ms ; // system time in milliseconds that control was handed to the external computer
2019-03-08 04:53:22 -04:00
Location start_loc ; // starting location for checking horiz_max limit
} limit ;
2017-07-18 23:17:45 -03:00
} ;
class ModeHold : public Mode
{
public :
2023-09-21 12:47:41 -03:00
Number mode_number ( ) const override { return Number : : HOLD ; }
2017-11-28 22:35:33 -04:00
const char * name4 ( ) const override { return " HOLD " ; }
2017-07-18 23:17:45 -03:00
// methods that affect movement of the vehicle in this mode
void update ( ) override ;
// attributes for mavlink system status reporting
bool attitude_stabilized ( ) const override { return false ; }
2017-12-05 01:30:38 -04:00
2018-01-22 07:00:47 -04:00
// hold mode does not require position or velocity estimate
bool requires_position ( ) const override { return false ; }
bool requires_velocity ( ) const override { return false ; }
2017-07-18 23:17:45 -03:00
} ;
2018-04-28 03:31:31 -03:00
class ModeLoiter : public Mode
{
public :
2023-09-21 12:47:41 -03:00
Number mode_number ( ) const override { return Number : : LOITER ; }
2018-04-28 03:31:31 -03:00
const char * name4 ( ) const override { return " LOIT " ; }
// methods that affect movement of the vehicle in this mode
void update ( ) override ;
2018-12-11 21:24:02 -04:00
// attributes of the mode
bool is_autopilot_mode ( ) const override { return true ; }
// return desired heading (in degrees) and cross track error (in meters) for reporting to ground station (NAV_CONTROLLER_OUTPUT message)
2019-04-29 03:31:45 -03:00
float wp_bearing ( ) const override { return _desired_yaw_cd * 0.01f ; }
float nav_bearing ( ) const override { return _desired_yaw_cd * 0.01f ; }
2018-12-11 21:24:02 -04:00
float crosstrack_error ( ) const override { return 0.0f ; }
2019-05-17 03:55:31 -03:00
// return desired location
bool get_desired_location ( Location & destination ) const override WARN_IF_UNUSED ;
2018-04-28 03:31:31 -03:00
// return distance (in meters) to destination
float get_distance_to_destination ( ) const override { return _distance_to_destination ; }
protected :
bool _enter ( ) override ;
2019-04-29 03:31:45 -03:00
Location _destination ; // target location to hold position around
2019-11-04 04:31:13 -04:00
float _desired_speed ; // desired speed (ramped down from initial speed to zero)
2018-04-28 03:31:31 -03:00
} ;
2017-07-18 23:17:45 -03:00
class ModeManual : public Mode
{
public :
2023-09-21 12:47:41 -03:00
Number mode_number ( ) const override { return Number : : MANUAL ; }
2017-11-28 22:35:33 -04:00
const char * name4 ( ) const override { return " MANU " ; }
2017-07-18 23:17:45 -03:00
// methods that affect movement of the vehicle in this mode
void update ( ) override ;
// attributes for mavlink system status reporting
bool has_manual_input ( ) const override { return true ; }
bool attitude_stabilized ( ) const override { return false ; }
2017-11-29 02:30:37 -04:00
2018-01-22 07:00:47 -04:00
// manual mode does not require position or velocity estimate
bool requires_position ( ) const override { return false ; }
bool requires_velocity ( ) const override { return false ; }
2018-05-10 04:10:34 -03:00
protected :
void _exit ( ) override ;
2017-07-18 23:17:45 -03:00
} ;
class ModeRTL : public Mode
{
public :
2023-09-21 12:47:41 -03:00
Number mode_number ( ) const override { return Number : : RTL ; }
2017-11-28 22:35:33 -04:00
const char * name4 ( ) const override { return " RTL " ; }
2017-07-18 23:17:45 -03:00
// methods that affect movement of the vehicle in this mode
void update ( ) override ;
// attributes of the mode
bool is_autopilot_mode ( ) const override { return true ; }
2020-02-13 02:59:42 -04:00
// do not allow arming from this mode
bool allows_arming ( ) const override { return false ; }
2019-05-17 03:55:31 -03:00
// return desired location
bool get_desired_location ( Location & destination ) const override WARN_IF_UNUSED ;
// return distance (in meters) to destination
2017-08-03 05:14:52 -03:00
float get_distance_to_destination ( ) const override { return _distance_to_destination ; }
2019-04-29 03:31:45 -03:00
bool reached_destination ( ) const override ;
2017-08-03 05:14:52 -03:00
2019-11-04 04:31:13 -04:00
// set desired speed in m/s
bool set_desired_speed ( float speed ) override ;
2017-07-18 23:17:45 -03:00
protected :
bool _enter ( ) override ;
2019-04-29 03:31:45 -03:00
2021-03-03 00:51:46 -04:00
bool send_notification ; // used to send one time notification to ground station
2019-10-04 21:20:51 -03:00
bool _loitering ; // true if loitering at end of RTL
2019-10-04 15:26:56 -03:00
2017-07-18 23:17:45 -03:00
} ;
2017-11-29 21:58:11 -04:00
class ModeSmartRTL : public Mode
{
public :
2023-09-21 12:47:41 -03:00
Number mode_number ( ) const override { return Number : : SMART_RTL ; }
2017-11-29 21:58:11 -04:00
const char * name4 ( ) const override { return " SRTL " ; }
// methods that affect movement of the vehicle in this mode
void update ( ) override ;
// attributes of the mode
bool is_autopilot_mode ( ) const override { return true ; }
2020-02-13 02:59:42 -04:00
// do not allow arming from this mode
bool allows_arming ( ) const override { return false ; }
2019-05-17 03:55:31 -03:00
// return desired location
bool get_desired_location ( Location & destination ) const override WARN_IF_UNUSED ;
// return distance (in meters) to destination
2017-11-29 21:58:11 -04:00
float get_distance_to_destination ( ) const override { return _distance_to_destination ; }
2023-09-21 10:58:41 -03:00
bool reached_destination ( ) const override { return smart_rtl_state = = SmartRTLState : : StopAtHome ; }
2017-11-29 21:58:11 -04:00
2019-11-04 04:31:13 -04:00
// set desired speed in m/s
bool set_desired_speed ( float speed ) override ;
2017-11-29 21:58:11 -04:00
// save current position for use by the smart_rtl flight mode
2018-02-12 01:16:23 -04:00
void save_position ( ) ;
2017-11-29 21:58:11 -04:00
protected :
// Safe RTL states
2023-09-21 10:58:41 -03:00
enum class SmartRTLState : uint8_t {
WaitForPathCleanup ,
PathFollow ,
StopAtHome ,
Failure
2017-11-29 21:58:11 -04:00
} smart_rtl_state ;
bool _enter ( ) override ;
bool _load_point ;
2019-10-04 21:20:51 -03:00
bool _loitering ; // true if loitering at end of SRTL
2017-11-29 21:58:11 -04:00
} ;
2024-02-18 06:33:45 -04:00
2017-07-18 23:17:45 -03:00
class ModeSteering : public Mode
{
public :
2023-09-21 12:47:41 -03:00
Number mode_number ( ) const override { return Number : : STEERING ; }
2017-11-28 22:35:33 -04:00
const char * name4 ( ) const override { return " STER " ; }
2017-07-18 23:17:45 -03:00
// methods that affect movement of the vehicle in this mode
void update ( ) override ;
// attributes for mavlink system status reporting
bool has_manual_input ( ) const override { return true ; }
2018-01-22 07:00:47 -04:00
// steering requires velocity but not position
bool requires_position ( ) const override { return false ; }
bool requires_velocity ( ) const override { return true ; }
2019-05-06 02:57:03 -03:00
// return desired lateral acceleration
float get_desired_lat_accel ( ) const override { return _desired_lat_accel ; }
private :
float _desired_lat_accel ; // desired lateral acceleration calculated from pilot steering input
2017-07-18 23:17:45 -03:00
} ;
class ModeInitializing : public Mode
{
public :
2023-09-21 12:47:41 -03:00
Number mode_number ( ) const override { return Number : : INITIALISING ; }
2017-11-28 22:35:33 -04:00
const char * name4 ( ) const override { return " INIT " ; }
2017-07-18 23:17:45 -03:00
// methods that affect movement of the vehicle in this mode
void update ( ) override { }
2021-01-25 09:31:31 -04:00
// do not allow arming from this mode
bool allows_arming ( ) const override { return false ; }
2017-07-18 23:17:45 -03:00
// attributes for mavlink system status reporting
bool has_manual_input ( ) const override { return true ; }
bool attitude_stabilized ( ) const override { return false ; }
2021-01-25 09:31:31 -04:00
protected :
bool _enter ( ) override { return false ; } ;
2017-07-18 23:17:45 -03:00
} ;
2018-05-24 01:47:07 -03:00
2024-08-04 21:54:28 -03:00
# if MODE_FOLLOW_ENABLED
2018-07-07 02:12:45 -03:00
class ModeFollow : public Mode
2018-05-24 01:47:07 -03:00
{
public :
2023-09-21 12:47:41 -03:00
Number mode_number ( ) const override { return Number : : FOLLOW ; }
2018-05-24 01:47:07 -03:00
const char * name4 ( ) const override { return " FOLL " ; }
// methods that affect movement of the vehicle in this mode
void update ( ) override ;
2018-12-11 21:07:47 -04:00
// attributes of the mode
bool is_autopilot_mode ( ) const override { return true ; }
// return desired heading (in degrees) and cross track error (in meters) for reporting to ground station (NAV_CONTROLLER_OUTPUT message)
float wp_bearing ( ) const override ;
float nav_bearing ( ) const override { return wp_bearing ( ) ; }
float crosstrack_error ( ) const override { return 0.0f ; }
2019-05-17 03:55:31 -03:00
// return desired location
bool get_desired_location ( Location & destination ) const override WARN_IF_UNUSED { return false ; }
2018-12-11 21:07:47 -04:00
// return distance (in meters) to destination
float get_distance_to_destination ( ) const override ;
2019-11-04 04:31:13 -04:00
// set desired speed in m/s
bool set_desired_speed ( float speed ) override ;
2018-05-24 01:47:07 -03:00
protected :
bool _enter ( ) override ;
2019-10-03 22:24:36 -03:00
void _exit ( ) override ;
2019-11-04 04:31:13 -04:00
float _desired_speed ; // desired speed in m/s
2018-05-24 01:47:07 -03:00
} ;
2023-08-11 09:28:32 -03:00
# endif
2018-07-02 04:21:37 -03:00
class ModeSimple : public Mode
{
public :
2023-09-21 12:47:41 -03:00
Number mode_number ( ) const override { return Number : : SIMPLE ; }
2018-07-02 04:21:37 -03:00
const char * name4 ( ) const override { return " SMPL " ; }
// methods that affect movement of the vehicle in this mode
void update ( ) override ;
2018-09-10 01:45:06 -03:00
void init_heading ( ) ;
2018-07-02 04:21:37 -03:00
// simple type enum used for SIMPLE_TYPE parameter
enum simple_type {
Simple_InitialHeading = 0 ,
Simple_CardinalDirections = 1 ,
} ;
2018-09-10 01:45:06 -03:00
2019-03-01 19:25:26 -04:00
private :
2018-09-10 01:45:06 -03:00
float _initial_heading_cd ; // vehicle heading (in centi-degrees) at moment vehicle was armed
float _desired_heading_cd ; // latest desired heading (in centi-degrees) from pilot
2018-07-02 04:21:37 -03:00
} ;
2024-08-04 21:54:28 -03:00
# if MODE_DOCK_ENABLED
2022-07-10 01:02:55 -03:00
class ModeDock : public Mode
{
public :
// need a constructor for parameters
ModeDock ( void ) ;
// Does not allow copies
CLASS_NO_COPY ( ModeDock ) ;
2023-09-21 12:47:41 -03:00
Number mode_number ( ) const override { return Number : : DOCK ; }
2022-07-10 01:02:55 -03:00
const char * name4 ( ) const override { return " DOCK " ; }
// methods that affect movement of the vehicle in this mode
void update ( ) override ;
bool is_autopilot_mode ( ) const override { return true ; }
// return distance (in meters) to destination
float get_distance_to_destination ( ) const override { return _distance_to_destination ; }
static const struct AP_Param : : GroupInfo var_info [ ] ;
protected :
AP_Float speed ; // dock mode speed
AP_Float desired_dir ; // desired direction of approach
AP_Int8 hdg_corr_enable ; // enable heading correction
AP_Float hdg_corr_weight ; // heading correction weight
AP_Float stopping_dist ; // how far away from the docking target should we start stopping
bool _enter ( ) override ;
// return reduced speed of vehicle based on error in position and current distance from the dock
float apply_slowdown ( float desired_speed ) ;
// calculate position of dock relative to the vehicle
bool calc_dock_pos_rel_vehicle_NE ( Vector2f & dock_pos_rel_vehicle ) const ;
// we force the vehicle to use real dock target vector when this much close to the docking station
const float _force_real_target_limit_cm = 300.0f ;
// acceptable lateral error in vehicle's position with respect to dock. This is used while slowing down the vehicle
const float _acceptable_pos_error_cm = 20.0f ;
Vector2f _dock_pos_rel_origin_cm ; // position vector towards docking target relative to ekf origin
Vector2f _desired_heading_NE ; // unit vector in desired direction of docking
bool _docking_complete = false ; // flag to mark docking complete when we are close enough to the dock
bool _loitering = false ; // true if we are loitering after mission completion
} ;
# endif