2018-09-25 01:11:26 -03:00
|
|
|
#pragma once
|
|
|
|
|
2015-11-24 04:24:04 -04:00
|
|
|
#include <AP_Motors/AP_Motors.h>
|
|
|
|
#include <AC_PID/AC_PID.h>
|
|
|
|
#include <AC_AttitudeControl/AC_AttitudeControl_Multi.h> // Attitude control library
|
|
|
|
#include <AP_InertialNav/AP_InertialNav.h>
|
|
|
|
#include <AC_AttitudeControl/AC_PosControl.h>
|
2015-12-26 04:51:05 -04:00
|
|
|
#include <AC_WPNav/AC_WPNav.h>
|
2018-03-27 23:24:05 -03:00
|
|
|
#include <AC_WPNav/AC_Loiter.h>
|
2016-06-20 02:33:47 -03:00
|
|
|
#include <AC_Fence/AC_Fence.h>
|
|
|
|
#include <AC_Avoidance/AC_Avoid.h>
|
2016-08-16 09:21:50 -03:00
|
|
|
#include <AP_Proximity/AP_Proximity.h>
|
2018-12-07 03:52:05 -04:00
|
|
|
#include "qautotune.h"
|
2020-07-07 21:22:56 -03:00
|
|
|
#include "defines.h"
|
2015-11-24 04:24:04 -04:00
|
|
|
|
|
|
|
/*
|
|
|
|
QuadPlane specific functionality
|
|
|
|
*/
|
|
|
|
class QuadPlane
|
|
|
|
{
|
|
|
|
public:
|
|
|
|
friend class Plane;
|
2016-05-06 01:43:39 -03:00
|
|
|
friend class AP_Tuning_Plane;
|
2016-08-11 00:19:00 -03:00
|
|
|
friend class GCS_MAVLINK_Plane;
|
2016-07-22 05:36:28 -03:00
|
|
|
friend class AP_AdvancedFailsafe_Plane;
|
2018-12-07 03:52:05 -04:00
|
|
|
friend class QAutoTune;
|
2019-03-04 23:51:44 -04:00
|
|
|
friend class AP_Arming_Plane;
|
2020-07-07 21:22:56 -03:00
|
|
|
friend class RC_Channel_Plane;
|
|
|
|
friend class RC_Channel;
|
2018-12-07 03:52:05 -04:00
|
|
|
|
2019-01-15 13:46:13 -04:00
|
|
|
friend class Mode;
|
|
|
|
friend class ModeAuto;
|
2020-07-29 12:01:34 -03:00
|
|
|
friend class ModeRTL;
|
2019-01-15 13:46:13 -04:00
|
|
|
friend class ModeAvoidADSB;
|
|
|
|
friend class ModeGuided;
|
|
|
|
friend class ModeQHover;
|
|
|
|
friend class ModeQLand;
|
|
|
|
friend class ModeQLoiter;
|
|
|
|
friend class ModeQRTL;
|
|
|
|
friend class ModeQStabilize;
|
|
|
|
friend class ModeQAutotune;
|
2019-05-03 20:37:10 -03:00
|
|
|
friend class ModeQAcro;
|
2019-01-15 13:46:13 -04:00
|
|
|
|
2015-11-24 04:24:04 -04:00
|
|
|
QuadPlane(AP_AHRS_NavEKF &_ahrs);
|
|
|
|
|
|
|
|
// var_info for holding Parameter information
|
|
|
|
static const struct AP_Param::GroupInfo var_info[];
|
2017-11-05 05:44:42 -04:00
|
|
|
static const struct AP_Param::GroupInfo var_info2[];
|
2015-11-24 04:24:04 -04:00
|
|
|
|
2015-12-26 06:40:40 -04:00
|
|
|
void control_run(void);
|
2019-10-01 11:11:48 -03:00
|
|
|
void control_auto(void);
|
2015-12-26 06:40:40 -04:00
|
|
|
bool init_mode(void);
|
2016-01-06 03:17:08 -04:00
|
|
|
bool setup(void);
|
2016-04-28 23:53:20 -03:00
|
|
|
|
2016-05-05 19:27:47 -03:00
|
|
|
void vtol_position_controller(void);
|
2016-04-28 23:53:20 -03:00
|
|
|
void setup_target_position(void);
|
|
|
|
void takeoff_controller(void);
|
|
|
|
void waypoint_controller(void);
|
2019-01-26 19:30:53 -04:00
|
|
|
|
2020-02-21 02:51:36 -04:00
|
|
|
void update_throttle_mix(void);
|
2015-11-24 04:24:04 -04:00
|
|
|
|
|
|
|
// update transition handling
|
|
|
|
void update(void);
|
|
|
|
|
|
|
|
// set motor arming
|
|
|
|
void set_armed(bool armed);
|
2015-12-26 06:40:40 -04:00
|
|
|
|
|
|
|
// is VTOL available?
|
|
|
|
bool available(void) const {
|
|
|
|
return initialised;
|
|
|
|
}
|
|
|
|
|
2016-01-01 02:36:41 -04:00
|
|
|
// is quadplane assisting?
|
|
|
|
bool in_assisted_flight(void) const {
|
|
|
|
return available() && assisted_flight;
|
|
|
|
}
|
2017-10-29 03:31:09 -03:00
|
|
|
|
|
|
|
/*
|
|
|
|
return true if we are in a transition to fwd flight from hover
|
|
|
|
*/
|
|
|
|
bool in_transition(void) const;
|
2017-10-30 01:19:38 -03:00
|
|
|
|
|
|
|
/*
|
|
|
|
return true if we are a tailsitter transitioning to VTOL flight
|
|
|
|
*/
|
2020-10-30 20:26:00 -03:00
|
|
|
bool in_tailsitter_vtol_transition(uint32_t now = 0) const;
|
|
|
|
|
2016-04-22 05:22:31 -03:00
|
|
|
bool handle_do_vtol_transition(enum MAV_VTOL_STATE state);
|
2015-12-26 06:40:40 -04:00
|
|
|
|
2016-01-01 05:42:10 -04:00
|
|
|
bool do_vtol_takeoff(const AP_Mission::Mission_Command& cmd);
|
|
|
|
bool do_vtol_land(const AP_Mission::Mission_Command& cmd);
|
|
|
|
bool verify_vtol_takeoff(const AP_Mission::Mission_Command &cmd);
|
2016-04-29 02:31:08 -03:00
|
|
|
bool verify_vtol_land(void);
|
2017-10-30 01:19:38 -03:00
|
|
|
bool in_vtol_auto(void) const;
|
|
|
|
bool in_vtol_mode(void) const;
|
2020-05-04 20:12:14 -03:00
|
|
|
bool in_vtol_posvel_mode(void) const;
|
2018-04-10 19:07:24 -03:00
|
|
|
void update_throttle_hover();
|
2020-09-25 18:16:38 -03:00
|
|
|
bool show_vtol_view() const;
|
2016-01-01 06:39:36 -04:00
|
|
|
|
|
|
|
// vtol help for is_flying()
|
|
|
|
bool is_flying(void);
|
2016-01-03 06:38:51 -04:00
|
|
|
|
|
|
|
// return current throttle as a percentate
|
|
|
|
uint8_t throttle_percentage(void) const {
|
2016-05-07 19:11:00 -03:00
|
|
|
return last_throttle * 100;
|
2016-01-03 06:38:51 -04:00
|
|
|
}
|
2016-03-24 22:33:19 -03:00
|
|
|
|
2016-04-20 02:13:20 -03:00
|
|
|
// return desired forward throttle percentage
|
2020-06-09 19:05:07 -03:00
|
|
|
int8_t forward_throttle_pct();
|
2016-04-20 03:23:17 -03:00
|
|
|
float get_weathervane_yaw_rate_cds(void);
|
2016-04-22 07:20:06 -03:00
|
|
|
|
|
|
|
// see if we are flying from vtol point of view
|
2018-08-24 02:42:37 -03:00
|
|
|
bool is_flying_vtol(void) const;
|
2017-02-11 01:50:03 -04:00
|
|
|
|
2017-02-11 04:12:56 -04:00
|
|
|
// return true when tailsitter frame configured
|
2017-10-30 01:19:38 -03:00
|
|
|
bool is_tailsitter(void) const;
|
2017-02-11 04:12:56 -04:00
|
|
|
|
2020-01-02 15:07:09 -04:00
|
|
|
// return true when flying a control surface only tailsitter tailsitter
|
|
|
|
bool is_contol_surface_tailsitter(void) const;
|
|
|
|
|
2017-02-11 04:12:56 -04:00
|
|
|
// return true when flying a tailsitter in VTOL
|
|
|
|
bool tailsitter_active(void);
|
2017-02-11 01:50:03 -04:00
|
|
|
|
|
|
|
// create outputs for tailsitters
|
|
|
|
void tailsitter_output(void);
|
2017-02-12 05:58:00 -04:00
|
|
|
|
2017-02-24 01:47:09 -04:00
|
|
|
// handle different tailsitter input types
|
|
|
|
void tailsitter_check_input(void);
|
|
|
|
|
2017-10-30 01:19:38 -03:00
|
|
|
// check if we have completed transition to fixed wing
|
|
|
|
bool tailsitter_transition_fw_complete(void);
|
2017-09-06 04:58:08 -03:00
|
|
|
|
2020-10-13 13:59:30 -03:00
|
|
|
// return true if we are a tailsitter in FW flight
|
|
|
|
bool is_tailsitter_in_fw_flight(void) const;
|
|
|
|
|
2017-10-30 01:19:38 -03:00
|
|
|
// check if we have completed transition to vtol
|
|
|
|
bool tailsitter_transition_vtol_complete(void) const;
|
2017-11-05 02:05:08 -04:00
|
|
|
|
2019-04-25 16:39:27 -03:00
|
|
|
// account for control surface speed scaling in VTOL modes
|
2017-11-05 02:05:08 -04:00
|
|
|
void tailsitter_speed_scaling(void);
|
2019-04-25 16:39:27 -03:00
|
|
|
|
2017-09-06 04:58:08 -03:00
|
|
|
// user initiated takeoff for guided mode
|
|
|
|
bool do_user_takeoff(float takeoff_altitude);
|
2018-07-25 18:32:42 -03:00
|
|
|
|
|
|
|
// return true if the wp_nav controller is being updated
|
|
|
|
bool using_wp_nav(void) const;
|
2018-12-24 05:06:22 -04:00
|
|
|
|
|
|
|
// return true if the user has set ENABLE
|
|
|
|
bool enabled(void) const { return enable != 0; }
|
2016-04-20 02:13:20 -03:00
|
|
|
|
2016-03-24 22:33:19 -03:00
|
|
|
struct PACKED log_QControl_Tuning {
|
|
|
|
LOG_PACKET_HEADER;
|
|
|
|
uint64_t time_us;
|
2018-12-21 03:42:59 -04:00
|
|
|
float throttle_in;
|
2016-03-25 09:09:59 -03:00
|
|
|
float angle_boost;
|
2016-03-24 22:33:19 -03:00
|
|
|
float throttle_out;
|
2018-12-21 03:42:59 -04:00
|
|
|
float throttle_hover;
|
2016-03-24 22:33:19 -03:00
|
|
|
float desired_alt;
|
|
|
|
float inav_alt;
|
2018-12-21 03:42:59 -04:00
|
|
|
int32_t baro_alt;
|
|
|
|
int16_t target_climb_rate;
|
2016-03-24 22:33:19 -03:00
|
|
|
int16_t climb_rate;
|
2017-04-18 09:25:14 -03:00
|
|
|
float throttle_mix;
|
2019-11-28 11:24:00 -04:00
|
|
|
float speed_scaler;
|
2020-09-25 18:17:13 -03:00
|
|
|
uint8_t transition_state;
|
2020-11-16 13:28:09 -04:00
|
|
|
uint8_t assist;
|
2016-03-24 22:33:19 -03:00
|
|
|
};
|
2019-03-01 02:27:53 -04:00
|
|
|
|
|
|
|
MAV_TYPE get_mav_type(void) const;
|
|
|
|
|
2020-01-02 15:06:39 -04:00
|
|
|
enum Q_ASSIST_STATE_ENUM {
|
|
|
|
Q_ASSIST_DISABLED,
|
|
|
|
Q_ASSIST_ENABLED,
|
|
|
|
Q_ASSIST_FORCE,
|
|
|
|
};
|
|
|
|
void set_q_assist_state(Q_ASSIST_STATE_ENUM state) {q_assist_state = state;};
|
|
|
|
|
2015-11-24 04:24:04 -04:00
|
|
|
private:
|
|
|
|
AP_AHRS_NavEKF &ahrs;
|
|
|
|
AP_Vehicle::MultiCopter aparm;
|
|
|
|
|
|
|
|
AP_InertialNav_NavEKF inertial_nav{ahrs};
|
|
|
|
|
2016-02-07 20:00:19 -04:00
|
|
|
AP_Int8 frame_class;
|
|
|
|
AP_Int8 frame_type;
|
2015-11-24 04:24:04 -04:00
|
|
|
|
2017-01-09 04:19:42 -04:00
|
|
|
AP_MotorsMulticopter *motors;
|
2017-02-27 02:39:46 -04:00
|
|
|
const struct AP_Param::GroupInfo *motors_var_info;
|
|
|
|
|
2015-12-26 06:40:40 -04:00
|
|
|
AC_AttitudeControl_Multi *attitude_control;
|
|
|
|
AC_PosControl *pos_control;
|
|
|
|
AC_WPNav *wp_nav;
|
2018-03-27 23:24:05 -03:00
|
|
|
AC_Loiter *loiter_nav;
|
2015-12-26 04:51:05 -04:00
|
|
|
|
2015-12-26 03:45:42 -04:00
|
|
|
// maximum vertical velocity the pilot may request
|
|
|
|
AP_Int16 pilot_velocity_z_max;
|
|
|
|
|
|
|
|
// vertical acceleration the pilot may request
|
|
|
|
AP_Int16 pilot_accel_z;
|
2016-08-29 03:44:54 -03:00
|
|
|
|
2020-07-07 21:22:56 -03:00
|
|
|
// air mode state: OFF, ON
|
|
|
|
AirMode air_mode;
|
|
|
|
|
2020-06-11 12:15:56 -03:00
|
|
|
// check for quadplane assistance needed
|
2020-01-02 15:06:39 -04:00
|
|
|
bool assistance_needed(float aspeed, bool have_airspeed);
|
2016-08-29 03:44:54 -03:00
|
|
|
|
2020-02-17 20:17:50 -04:00
|
|
|
// check if it is safe to provide assistance
|
|
|
|
bool assistance_safe();
|
|
|
|
|
2015-11-24 04:24:04 -04:00
|
|
|
// update transition handling
|
|
|
|
void update_transition(void);
|
2015-12-26 04:27:13 -04:00
|
|
|
|
2017-04-18 09:40:06 -03:00
|
|
|
// check for an EKF yaw reset
|
|
|
|
void check_yaw_reset(void);
|
|
|
|
|
2015-12-26 04:27:13 -04:00
|
|
|
// hold hover (for transition)
|
|
|
|
void hold_hover(float target_climb_rate);
|
|
|
|
|
|
|
|
// hold stabilize (for transition)
|
|
|
|
void hold_stabilize(float throttle_in);
|
2015-12-26 04:51:05 -04:00
|
|
|
|
2016-01-09 18:42:46 -04:00
|
|
|
// get pilot desired yaw rate in cd/s
|
2018-08-24 02:42:37 -03:00
|
|
|
float get_pilot_input_yaw_rate_cds(void) const;
|
2015-12-26 04:51:05 -04:00
|
|
|
|
2016-01-09 18:42:46 -04:00
|
|
|
// get overall desired yaw rate in cd/s
|
|
|
|
float get_desired_yaw_rate_cds(void);
|
|
|
|
|
2015-12-26 04:51:05 -04:00
|
|
|
// get desired climb rate in cm/s
|
2018-08-24 02:42:37 -03:00
|
|
|
float get_pilot_desired_climb_rate_cms(void) const;
|
2015-12-26 05:13:20 -04:00
|
|
|
|
|
|
|
// initialise throttle_wait when entering mode
|
|
|
|
void init_throttle_wait();
|
2015-12-26 06:40:40 -04:00
|
|
|
|
2017-03-13 04:45:46 -03:00
|
|
|
// use multicopter rate controller
|
2017-07-10 01:00:45 -03:00
|
|
|
void multicopter_attitude_rate_update(float yaw_rate_cds);
|
2017-03-13 04:45:46 -03:00
|
|
|
|
2015-12-26 06:40:40 -04:00
|
|
|
// main entry points for VTOL flight modes
|
|
|
|
void init_stabilize(void);
|
|
|
|
void control_stabilize(void);
|
|
|
|
|
2018-09-13 21:02:18 -03:00
|
|
|
void check_attitude_relax(void);
|
2019-02-17 14:42:05 -04:00
|
|
|
void init_qacro(void);
|
2019-04-04 14:57:11 -03:00
|
|
|
float get_pilot_throttle(void);
|
2019-02-17 14:42:05 -04:00
|
|
|
void control_qacro(void);
|
2015-12-26 06:40:40 -04:00
|
|
|
void init_hover(void);
|
|
|
|
void control_hover(void);
|
|
|
|
|
|
|
|
void init_loiter(void);
|
2019-02-22 02:04:35 -04:00
|
|
|
void init_qland(void);
|
2015-12-26 06:40:40 -04:00
|
|
|
void control_loiter(void);
|
2020-03-03 23:59:05 -04:00
|
|
|
bool check_land_complete(void);
|
2019-02-01 23:49:02 -04:00
|
|
|
bool land_detector(uint32_t timeout_ms);
|
|
|
|
bool check_land_final(void);
|
2015-12-26 06:40:40 -04:00
|
|
|
|
2016-04-29 02:31:08 -03:00
|
|
|
void init_qrtl(void);
|
|
|
|
void control_qrtl(void);
|
|
|
|
|
2018-08-24 02:42:37 -03:00
|
|
|
float assist_climb_rate_cms(void) const;
|
2015-12-26 06:40:40 -04:00
|
|
|
|
|
|
|
// calculate desired yaw rate for assistance
|
2018-08-24 02:42:37 -03:00
|
|
|
float desired_auto_yaw_rate_cds(void) const;
|
2015-12-26 06:40:40 -04:00
|
|
|
|
|
|
|
bool should_relax(void);
|
2018-09-17 21:35:24 -03:00
|
|
|
void motors_output(bool run_rate_controller = true);
|
2016-03-24 22:33:19 -03:00
|
|
|
void Log_Write_QControl_Tuning();
|
2018-08-24 02:42:37 -03:00
|
|
|
float landing_descent_rate_cms(float height_above_ground) const;
|
2016-03-24 22:11:56 -03:00
|
|
|
|
2016-02-07 20:00:19 -04:00
|
|
|
// setup correct aux channels for frame class
|
|
|
|
void setup_default_channels(uint8_t num_motors);
|
2016-05-05 22:28:26 -03:00
|
|
|
|
|
|
|
void guided_start(void);
|
|
|
|
void guided_update(void);
|
2016-08-01 19:21:44 -03:00
|
|
|
|
2019-02-20 20:01:32 -04:00
|
|
|
void update_throttle_suppression(void);
|
2017-03-13 04:04:09 -03:00
|
|
|
|
|
|
|
void run_z_controller(void);
|
2017-04-03 19:18:30 -03:00
|
|
|
|
|
|
|
void setup_defaults(void);
|
2017-11-05 05:44:42 -04:00
|
|
|
|
|
|
|
// calculate a stopping distance for fixed-wing to vtol transitions
|
|
|
|
float stopping_distance(void);
|
2015-11-24 04:24:04 -04:00
|
|
|
|
|
|
|
AP_Int16 transition_time_ms;
|
2015-12-26 06:40:40 -04:00
|
|
|
|
2017-11-05 05:44:42 -04:00
|
|
|
// transition deceleration, m/s/s
|
|
|
|
AP_Float transition_decel;
|
2018-09-25 12:13:34 -03:00
|
|
|
|
2018-12-14 22:40:43 -04:00
|
|
|
// transition failure milliseconds
|
|
|
|
AP_Int16 transition_failure;
|
|
|
|
|
2018-09-25 12:13:34 -03:00
|
|
|
// Quadplane trim, degrees
|
2018-11-12 01:00:00 -04:00
|
|
|
AP_Float ahrs_trim_pitch;
|
2019-02-04 13:14:01 -04:00
|
|
|
float _last_ahrs_trim_pitch;
|
2018-09-25 12:13:34 -03:00
|
|
|
|
2018-11-16 17:53:43 -04:00
|
|
|
// fw landing approach radius
|
|
|
|
AP_Float fw_land_approach_radius;
|
|
|
|
|
2015-12-26 06:40:40 -04:00
|
|
|
AP_Int16 rc_speed;
|
|
|
|
|
|
|
|
// min and max PWM for throttle
|
|
|
|
AP_Int16 thr_min_pwm;
|
|
|
|
AP_Int16 thr_max_pwm;
|
|
|
|
|
|
|
|
// speed below which quad assistance is given
|
|
|
|
AP_Float assist_speed;
|
2016-01-02 00:25:49 -04:00
|
|
|
|
2016-08-29 03:44:54 -03:00
|
|
|
// angular error at which quad assistance is given
|
|
|
|
AP_Int8 assist_angle;
|
|
|
|
uint32_t angle_error_start_ms;
|
2020-02-16 21:05:13 -04:00
|
|
|
AP_Float assist_delay;
|
2019-11-01 07:44:23 -03:00
|
|
|
|
|
|
|
// altitude to trigger assistance
|
|
|
|
AP_Int16 assist_alt;
|
|
|
|
uint32_t alt_error_start_ms;
|
|
|
|
bool in_alt_assist;
|
|
|
|
|
2016-01-02 00:25:49 -04:00
|
|
|
// maximum yaw rate in degrees/second
|
|
|
|
AP_Float yaw_rate_max;
|
2016-01-02 02:55:56 -04:00
|
|
|
|
|
|
|
// landing speed in cm/s
|
|
|
|
AP_Int16 land_speed_cms;
|
|
|
|
|
2016-04-29 02:31:08 -03:00
|
|
|
// QRTL start altitude, meters
|
|
|
|
AP_Int16 qrtl_alt;
|
|
|
|
|
2016-01-02 02:55:56 -04:00
|
|
|
// alt to switch to QLAND_FINAL
|
|
|
|
AP_Float land_final_alt;
|
2016-06-03 19:35:09 -03:00
|
|
|
AP_Float vel_forward_alt_cutoff;
|
2015-12-26 06:40:40 -04:00
|
|
|
|
|
|
|
AP_Int8 enable;
|
2016-01-15 01:49:49 -04:00
|
|
|
AP_Int8 transition_pitch_max;
|
2016-04-20 02:13:20 -03:00
|
|
|
|
2016-04-29 03:31:22 -03:00
|
|
|
// control if a VTOL RTL will be used
|
|
|
|
AP_Int8 rtl_mode;
|
2016-05-05 19:27:47 -03:00
|
|
|
|
|
|
|
// control if a VTOL GUIDED will be used
|
|
|
|
AP_Int8 guided_mode;
|
2016-06-02 00:10:39 -03:00
|
|
|
|
|
|
|
// control ESC throttle calibration
|
|
|
|
AP_Int8 esc_calibration;
|
|
|
|
void run_esc_calibration(void);
|
2016-07-25 02:46:17 -03:00
|
|
|
|
|
|
|
// ICEngine control on landing
|
|
|
|
AP_Int8 land_icengine_cut;
|
2017-04-18 09:40:06 -03:00
|
|
|
|
2017-10-20 01:39:44 -03:00
|
|
|
// HEARTBEAT mav_type override
|
|
|
|
AP_Int8 mav_type;
|
2019-04-04 14:57:11 -03:00
|
|
|
|
|
|
|
// manual throttle curve expo strength
|
|
|
|
AP_Float throttle_expo;
|
|
|
|
|
2020-06-09 19:05:07 -03:00
|
|
|
// manual forward throttle input
|
|
|
|
AP_Float fwd_thr_max;
|
|
|
|
RC_Channel *rc_fwd_thr_ch;
|
|
|
|
|
2019-03-24 00:35:13 -03:00
|
|
|
// QACRO mode max roll/pitch/yaw rates
|
|
|
|
AP_Float acro_roll_rate;
|
|
|
|
AP_Float acro_pitch_rate;
|
|
|
|
AP_Float acro_yaw_rate;
|
|
|
|
|
2017-04-18 09:40:06 -03:00
|
|
|
// time we last got an EKF yaw reset
|
|
|
|
uint32_t ekfYawReset_ms;
|
|
|
|
|
2016-04-20 02:13:20 -03:00
|
|
|
struct {
|
|
|
|
AP_Float gain;
|
|
|
|
float integrator;
|
2016-06-02 18:57:10 -03:00
|
|
|
uint32_t last_ms;
|
2016-04-20 02:13:20 -03:00
|
|
|
int8_t last_pct;
|
|
|
|
} vel_forward;
|
2016-04-20 03:23:17 -03:00
|
|
|
|
|
|
|
struct {
|
|
|
|
AP_Float gain;
|
2016-04-21 08:52:25 -03:00
|
|
|
AP_Float min_roll;
|
2016-04-20 03:23:17 -03:00
|
|
|
uint32_t last_pilot_input_ms;
|
|
|
|
float last_output;
|
|
|
|
} weathervane;
|
2016-01-15 01:49:49 -04:00
|
|
|
|
2015-12-26 06:40:40 -04:00
|
|
|
bool initialised;
|
2015-11-24 04:24:04 -04:00
|
|
|
|
2015-12-26 04:51:05 -04:00
|
|
|
// timer start for transition
|
|
|
|
uint32_t transition_start_ms;
|
2020-04-09 10:54:33 -03:00
|
|
|
float transition_initial_pitch;
|
2018-12-14 22:40:43 -04:00
|
|
|
uint32_t transition_low_airspeed_ms;
|
2015-11-24 04:24:04 -04:00
|
|
|
|
2015-12-26 06:40:40 -04:00
|
|
|
Location last_auto_target;
|
|
|
|
|
2015-11-24 04:24:04 -04:00
|
|
|
// last throttle value when active
|
2015-12-26 04:27:13 -04:00
|
|
|
float last_throttle;
|
2015-11-24 04:24:04 -04:00
|
|
|
|
2017-09-08 04:54:55 -03:00
|
|
|
// pitch when we enter loiter mode
|
|
|
|
int32_t loiter_initial_pitch_cd;
|
2015-12-26 04:27:13 -04:00
|
|
|
|
2018-09-13 21:02:18 -03:00
|
|
|
// when did we last run the attitude controller?
|
|
|
|
uint32_t last_att_control_ms;
|
|
|
|
|
2015-12-26 04:27:13 -04:00
|
|
|
// true if we have reached the airspeed threshold for transition
|
|
|
|
enum {
|
|
|
|
TRANSITION_AIRSPEED_WAIT,
|
|
|
|
TRANSITION_TIMER,
|
2017-10-30 01:19:38 -03:00
|
|
|
TRANSITION_ANGLE_WAIT_FW,
|
|
|
|
TRANSITION_ANGLE_WAIT_VTOL,
|
2015-12-26 04:27:13 -04:00
|
|
|
TRANSITION_DONE
|
|
|
|
} transition_state;
|
2015-12-26 05:13:20 -04:00
|
|
|
|
|
|
|
// true when waiting for pilot throttle
|
2016-08-01 19:21:44 -03:00
|
|
|
bool throttle_wait:1;
|
2015-12-26 06:40:40 -04:00
|
|
|
|
|
|
|
// true when quad is assisting a fixed wing mode
|
2016-08-01 19:21:44 -03:00
|
|
|
bool assisted_flight:1;
|
2015-12-26 06:40:40 -04:00
|
|
|
|
2016-08-29 03:44:54 -03:00
|
|
|
// true when in angle assist
|
|
|
|
bool in_angle_assist:1;
|
|
|
|
|
2017-09-06 04:58:08 -03:00
|
|
|
// are we in a guided takeoff?
|
|
|
|
bool guided_takeoff:1;
|
2017-10-29 03:31:09 -03:00
|
|
|
|
2016-06-16 05:17:46 -03:00
|
|
|
struct {
|
|
|
|
// time when motors reached lower limit
|
|
|
|
uint32_t lower_limit_start_ms;
|
|
|
|
uint32_t land_start_ms;
|
|
|
|
float vpos_start_m;
|
|
|
|
} landing_detect;
|
2016-01-01 03:18:53 -04:00
|
|
|
|
2020-03-04 02:05:13 -04:00
|
|
|
// throttle mix acceleration filter
|
|
|
|
LowPassFilterVector3f throttle_mix_accel_ef_filter = LowPassFilterVector3f(1.0f);
|
|
|
|
|
2016-01-01 03:18:53 -04:00
|
|
|
// time we last set the loiter target
|
|
|
|
uint32_t last_loiter_ms;
|
2016-01-01 05:42:10 -04:00
|
|
|
|
2016-05-05 19:27:47 -03:00
|
|
|
enum position_control_state {
|
|
|
|
QPOS_POSITION1,
|
|
|
|
QPOS_POSITION2,
|
|
|
|
QPOS_LAND_DESCEND,
|
|
|
|
QPOS_LAND_FINAL,
|
|
|
|
QPOS_LAND_COMPLETE
|
2016-04-28 23:53:20 -03:00
|
|
|
};
|
2016-04-10 07:36:20 -03:00
|
|
|
struct {
|
2016-05-05 19:27:47 -03:00
|
|
|
enum position_control_state state;
|
2016-04-10 07:36:20 -03:00
|
|
|
float speed_scale;
|
|
|
|
Vector2f target_velocity;
|
2016-04-20 05:07:04 -03:00
|
|
|
float max_speed;
|
2016-04-28 23:53:20 -03:00
|
|
|
Vector3f target;
|
2016-04-29 02:50:45 -03:00
|
|
|
bool slow_descent:1;
|
2016-05-05 19:27:47 -03:00
|
|
|
} poscontrol;
|
2016-02-07 20:00:19 -04:00
|
|
|
|
2016-03-12 19:05:10 -04:00
|
|
|
struct {
|
|
|
|
bool running;
|
|
|
|
uint32_t start_ms; // system time the motor test began
|
|
|
|
uint32_t timeout_ms = 0; // test will timeout this many milliseconds after the motor_test_start_ms
|
|
|
|
uint8_t seq = 0; // motor sequence number of motor being tested
|
|
|
|
uint8_t throttle_type = 0; // motor throttle type (0=throttle percentage, 1=PWM, 2=pilot throttle channel pass-through)
|
|
|
|
uint16_t throttle_value = 0; // throttle to be sent to motor, value depends upon it's type
|
2016-03-31 21:09:51 -03:00
|
|
|
uint8_t motor_count; // number of motors to cycle
|
2016-03-12 19:05:10 -04:00
|
|
|
} motor_test;
|
|
|
|
|
2016-05-27 01:16:44 -03:00
|
|
|
// time of last control log message
|
|
|
|
uint32_t last_ctrl_log_ms;
|
2016-07-03 09:02:38 -03:00
|
|
|
|
2019-10-19 12:19:29 -03:00
|
|
|
// time of last QTUN log message
|
|
|
|
uint32_t last_qtun_log_ms;
|
|
|
|
|
2016-07-03 09:02:38 -03:00
|
|
|
// types of tilt mechanisms
|
2019-03-18 21:00:25 -03:00
|
|
|
enum {TILT_TYPE_CONTINUOUS =0,
|
|
|
|
TILT_TYPE_BINARY =1,
|
|
|
|
TILT_TYPE_VECTORED_YAW =2,
|
|
|
|
TILT_TYPE_BICOPTER =3
|
|
|
|
};
|
|
|
|
|
2016-05-01 05:07:52 -03:00
|
|
|
// tiltrotor control variables
|
|
|
|
struct {
|
|
|
|
AP_Int16 tilt_mask;
|
2017-02-24 06:05:27 -04:00
|
|
|
AP_Int16 max_rate_up_dps;
|
|
|
|
AP_Int16 max_rate_down_dps;
|
2016-05-01 05:07:52 -03:00
|
|
|
AP_Int8 max_angle_deg;
|
2016-07-03 09:02:38 -03:00
|
|
|
AP_Int8 tilt_type;
|
2017-04-22 22:35:25 -03:00
|
|
|
AP_Float tilt_yaw_angle;
|
2016-05-01 05:07:52 -03:00
|
|
|
float current_tilt;
|
|
|
|
float current_throttle;
|
2016-05-01 20:33:54 -03:00
|
|
|
bool motors_active:1;
|
2016-05-01 05:07:52 -03:00
|
|
|
} tilt;
|
|
|
|
|
2019-12-01 21:11:06 -04:00
|
|
|
// bit 0 enables plane mode and bit 1 enables body-frame roll mode
|
2017-02-24 01:47:09 -04:00
|
|
|
enum tailsitter_input {
|
2019-12-01 21:11:06 -04:00
|
|
|
TAILSITTER_INPUT_PLANE = (1U<<0),
|
|
|
|
TAILSITTER_INPUT_BF_ROLL = (1U<<1)
|
2017-02-24 01:47:09 -04:00
|
|
|
};
|
2017-02-24 03:02:34 -04:00
|
|
|
|
|
|
|
enum tailsitter_mask {
|
2019-12-01 21:11:06 -04:00
|
|
|
TAILSITTER_MASK_AILERON = (1U<<0),
|
|
|
|
TAILSITTER_MASK_ELEVATOR = (1U<<1),
|
|
|
|
TAILSITTER_MASK_THROTTLE = (1U<<2),
|
|
|
|
TAILSITTER_MASK_RUDDER = (1U<<3),
|
2017-02-24 03:02:34 -04:00
|
|
|
};
|
2019-04-25 16:39:27 -03:00
|
|
|
|
|
|
|
enum tailsitter_gscl_mask {
|
2019-12-01 21:11:06 -04:00
|
|
|
TAILSITTER_GSCL_BOOST = (1U<<0),
|
|
|
|
TAILSITTER_GSCL_ATT_THR = (1U<<1),
|
2019-04-25 16:39:27 -03:00
|
|
|
};
|
|
|
|
|
2017-02-11 04:12:56 -04:00
|
|
|
// tailsitter control variables
|
|
|
|
struct {
|
|
|
|
AP_Int8 transition_angle;
|
2017-02-24 01:47:09 -04:00
|
|
|
AP_Int8 input_type;
|
2017-02-24 03:02:34 -04:00
|
|
|
AP_Int8 input_mask;
|
|
|
|
AP_Int8 input_mask_chan;
|
2017-04-09 21:01:58 -03:00
|
|
|
AP_Float vectored_forward_gain;
|
|
|
|
AP_Float vectored_hover_gain;
|
2017-05-04 18:34:27 -03:00
|
|
|
AP_Float vectored_hover_power;
|
2018-04-15 08:31:53 -03:00
|
|
|
AP_Float throttle_scale_max;
|
2019-04-25 16:39:27 -03:00
|
|
|
AP_Float gain_scaling_min;
|
2018-10-06 11:40:27 -03:00
|
|
|
AP_Float max_roll_angle;
|
2019-01-17 13:54:15 -04:00
|
|
|
AP_Int16 motor_mask;
|
2019-04-25 16:39:27 -03:00
|
|
|
AP_Float scaling_speed_min;
|
|
|
|
AP_Float scaling_speed_max;
|
2019-11-28 11:24:00 -04:00
|
|
|
AP_Int16 gain_scaling_mask;
|
2017-02-11 04:12:56 -04:00
|
|
|
} tailsitter;
|
|
|
|
|
2019-04-25 16:39:27 -03:00
|
|
|
// tailsitter speed scaler
|
2020-11-01 18:36:55 -04:00
|
|
|
float last_spd_scaler = 1.0f; // used to slew rate limiting with TAILSITTER_GSCL_ATT_THR option
|
|
|
|
float log_spd_scaler; // for QTUN log
|
2019-04-25 16:39:27 -03:00
|
|
|
|
2017-02-11 04:12:56 -04:00
|
|
|
// the attitude view of the VTOL attitude controller
|
|
|
|
AP_AHRS_View *ahrs_view;
|
|
|
|
|
2016-08-01 19:21:44 -03:00
|
|
|
// time when motors were last active
|
|
|
|
uint32_t last_motors_active_ms;
|
2017-03-13 04:04:09 -03:00
|
|
|
|
|
|
|
// time when we last ran the vertical accel controller
|
|
|
|
uint32_t last_pidz_active_ms;
|
2017-04-08 01:39:55 -03:00
|
|
|
uint32_t last_pidz_init_ms;
|
2017-10-30 01:19:38 -03:00
|
|
|
|
|
|
|
// time when we were last in a vtol control mode
|
|
|
|
uint32_t last_vtol_mode_ms;
|
2016-08-01 19:21:44 -03:00
|
|
|
|
2016-05-01 05:07:52 -03:00
|
|
|
void tiltrotor_slew(float tilt);
|
2016-07-03 09:02:38 -03:00
|
|
|
void tiltrotor_binary_slew(bool forward);
|
2016-05-01 05:07:52 -03:00
|
|
|
void tiltrotor_update(void);
|
2016-07-03 09:02:38 -03:00
|
|
|
void tiltrotor_continuous_update(void);
|
|
|
|
void tiltrotor_binary_update(void);
|
2017-04-22 22:35:25 -03:00
|
|
|
void tiltrotor_vectored_yaw(void);
|
2019-03-18 21:00:25 -03:00
|
|
|
void tiltrotor_bicopter(void);
|
2017-03-13 07:53:22 -03:00
|
|
|
void tilt_compensate_up(float *thrust, uint8_t num_motors);
|
|
|
|
void tilt_compensate_down(float *thrust, uint8_t num_motors);
|
2016-05-08 20:11:11 -03:00
|
|
|
void tilt_compensate(float *thrust, uint8_t num_motors);
|
2017-03-13 07:53:22 -03:00
|
|
|
bool is_motor_tilting(uint8_t motor) const {
|
|
|
|
return (((uint8_t)tilt.tilt_mask.get()) & (1U<<motor));
|
|
|
|
}
|
2017-01-22 22:06:31 -04:00
|
|
|
bool tiltrotor_fully_fwd(void);
|
2017-02-24 06:05:27 -04:00
|
|
|
float tilt_max_change(bool up);
|
2016-09-26 22:46:51 -03:00
|
|
|
|
|
|
|
void afs_terminate(void);
|
2016-09-30 19:35:58 -03:00
|
|
|
bool guided_mode_enabled(void);
|
2017-09-05 19:22:38 -03:00
|
|
|
|
|
|
|
// set altitude target to current altitude
|
|
|
|
void set_alt_target_current(void);
|
|
|
|
|
|
|
|
// adjust altitude target smoothly
|
|
|
|
void adjust_alt_target(float target_cm);
|
2017-10-29 03:31:09 -03:00
|
|
|
|
|
|
|
// additional options
|
|
|
|
AP_Int32 options;
|
|
|
|
enum {
|
|
|
|
OPTION_LEVEL_TRANSITION=(1<<0),
|
|
|
|
OPTION_ALLOW_FW_TAKEOFF=(1<<1),
|
|
|
|
OPTION_ALLOW_FW_LAND=(1<<2),
|
2018-08-13 17:16:45 -03:00
|
|
|
OPTION_RESPECT_TAKEOFF_FRAME=(1<<3),
|
2018-09-25 01:11:26 -03:00
|
|
|
OPTION_MISSION_LAND_FW_APPROACH=(1<<4),
|
2019-06-11 15:00:32 -03:00
|
|
|
OPTION_FS_QRTL=(1<<5),
|
2019-11-11 02:51:03 -04:00
|
|
|
OPTION_IDLE_GOV_MANUAL=(1<<6),
|
2020-05-26 13:41:08 -03:00
|
|
|
OPTION_Q_ASSIST_FORCE_ENABLE=(1<<7),
|
2020-05-26 13:43:05 -03:00
|
|
|
OPTION_TAILSIT_Q_ASSIST_MOTORS_ONLY=(1<<8),
|
2020-07-07 21:22:56 -03:00
|
|
|
OPTION_AIRMODE=(1<<9),
|
2020-06-11 12:15:56 -03:00
|
|
|
OPTION_DISARMED_TILT=(1<<10),
|
|
|
|
OPTION_DELAY_ARMING=(1<<11),
|
2020-10-04 19:22:50 -03:00
|
|
|
OPTION_DISABLE_SYNTHETIC_AIRSPEED_ASSIST=(1<<12),
|
2020-09-22 18:05:38 -03:00
|
|
|
OPTION_DISABLE_GROUND_EFFECT_COMP=(1<<13),
|
2017-10-29 03:31:09 -03:00
|
|
|
};
|
|
|
|
|
2018-12-15 01:23:56 -04:00
|
|
|
AP_Float takeoff_failure_scalar;
|
2018-12-15 16:47:49 -04:00
|
|
|
AP_Float maximum_takeoff_airspeed;
|
2018-12-15 01:23:56 -04:00
|
|
|
uint32_t takeoff_start_time_ms;
|
|
|
|
uint32_t takeoff_time_limit_ms;
|
|
|
|
|
2020-01-11 00:17:23 -04:00
|
|
|
float last_land_final_agl;
|
|
|
|
|
2020-06-16 15:52:26 -03:00
|
|
|
|
2020-06-11 12:15:56 -03:00
|
|
|
// oneshot with duration ARMING_DELAY_MS used by quadplane to delay spoolup after arming:
|
|
|
|
// ignored unless OPTION_DELAY_ARMING or OPTION_TILT_DISARMED is set
|
|
|
|
bool delay_arming;
|
|
|
|
|
2017-10-29 03:31:09 -03:00
|
|
|
/*
|
|
|
|
return true if current mission item is a vtol takeoff
|
|
|
|
*/
|
|
|
|
bool is_vtol_takeoff(uint16_t id) const;
|
|
|
|
|
|
|
|
/*
|
|
|
|
return true if current mission item is a vtol landing
|
|
|
|
*/
|
|
|
|
bool is_vtol_land(uint16_t id) const;
|
2018-12-07 03:52:05 -04:00
|
|
|
|
|
|
|
#if QAUTOTUNE_ENABLED
|
|
|
|
// qautotune mode
|
|
|
|
QAutoTune qautotune;
|
|
|
|
#endif
|
|
|
|
|
2019-02-23 22:53:52 -04:00
|
|
|
/*
|
|
|
|
are we in the approach phase of a VTOL landing?
|
|
|
|
*/
|
|
|
|
bool in_vtol_land_approach(void) const;
|
|
|
|
|
|
|
|
/*
|
|
|
|
are we in the descent phase of a VTOL landing?
|
|
|
|
*/
|
|
|
|
bool in_vtol_land_descent(void) const;
|
2019-09-11 04:20:04 -03:00
|
|
|
|
|
|
|
/*
|
|
|
|
are we in the final landing phase of a VTOL landing?
|
|
|
|
*/
|
|
|
|
bool in_vtol_land_final(void) const;
|
2019-12-27 17:13:27 -04:00
|
|
|
|
|
|
|
/*
|
|
|
|
are we in any of the phases of a VTOL landing?
|
|
|
|
*/
|
|
|
|
bool in_vtol_land_sequence(void) const;
|
2020-01-02 15:06:39 -04:00
|
|
|
|
|
|
|
// Q assist state, can be enabled, disabled or force. Default to enabled
|
|
|
|
Q_ASSIST_STATE_ENUM q_assist_state = Q_ASSIST_STATE_ENUM::Q_ASSIST_ENABLED;
|
|
|
|
|
2016-03-12 19:05:10 -04:00
|
|
|
public:
|
|
|
|
void motor_test_output();
|
2017-11-27 02:16:01 -04:00
|
|
|
MAV_RESULT mavlink_motor_test_start(mavlink_channel_t chan, uint8_t motor_seq, uint8_t throttle_type,
|
|
|
|
uint16_t throttle_value, float timeout_sec,
|
|
|
|
uint8_t motor_count);
|
2016-03-12 19:05:10 -04:00
|
|
|
private:
|
|
|
|
void motor_test_stop();
|
2015-11-24 04:24:04 -04:00
|
|
|
};
|