diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 527b884417..914051174d 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -647,7 +647,8 @@ void Plane::update_flight_mode(void) case QHOVER: case QLOITER: case QLAND: - case QRTL: { + case QRTL: + case QAUTOTUNE: { // set nav_roll and nav_pitch using sticks int16_t roll_limit = MIN(roll_limit_cd, quadplane.aparm.angle_max); float pitch_input = channel_pitch->norm_input(); @@ -772,6 +773,7 @@ void Plane::update_navigation() case QLOITER: case QLAND: case QRTL: + case QAUTOTUNE: // nothing to do break; } diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index 8a64542ac6..025d1fd41b 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -155,7 +155,8 @@ void Plane::stabilize_stick_mixing_direct() control_mode == QLOITER || control_mode == QLAND || control_mode == QRTL || - control_mode == TRAINING) { + control_mode == TRAINING || + control_mode == QAUTOTUNE) { return; } int16_t aileron = SRV_Channels::get_output_scaled(SRV_Channel::k_aileron); @@ -185,6 +186,7 @@ void Plane::stabilize_stick_mixing_fbw() control_mode == QLAND || control_mode == QRTL || control_mode == TRAINING || + control_mode == QAUTOTUNE || (control_mode == AUTO && g.auto_fbw_steer == 42)) { return; } @@ -393,7 +395,8 @@ void Plane::stabilize() control_mode == QHOVER || control_mode == QLOITER || control_mode == QLAND || - control_mode == QRTL) && + control_mode == QRTL || + control_mode == QAUTOTUNE) && !quadplane.in_tailsitter_vtol_transition()) { quadplane.control_run(); } else { diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index a93f73a2aa..d8dbb773b9 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -34,6 +34,7 @@ MAV_MODE GCS_MAVLINK_Plane::base_mode() const case QLOITER: case QLAND: case CRUISE: + case QAUTOTUNE: _base_mode = MAV_MODE_FLAG_STABILIZE_ENABLED; break; case AUTO: @@ -1545,6 +1546,7 @@ bool GCS_MAVLINK_Plane::set_mode(const uint8_t mode) case QLOITER: case QLAND: case QRTL: + case QAUTOTUNE: plane.set_mode((enum FlightMode)mode, MODE_REASON_GCS_COMMAND); return true; } diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 321f5281f6..b24b4583d7 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -538,49 +538,49 @@ const AP_Param::Info Plane::var_info[] = { // @Param: FLTMODE1 // @DisplayName: FlightMode1 // @Description: Flight mode for switch position 1 (910 to 1230 and above 2049) - // @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,8:AUTOTUNE,10:Auto,11:RTL,12:Loiter,14:AVOID_ADSB,15:Guided,17:QSTABILIZE,18:QHOVER,19:QLOITER,20:QLAND,21:QRTL + // @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,8:AUTOTUNE,10:Auto,11:RTL,12:Loiter,14:AVOID_ADSB,15:Guided,17:QSTABILIZE,18:QHOVER,19:QLOITER,20:QLAND,21:QRTL,22:QAUTOTUNE // @User: Standard GSCALAR(flight_mode1, "FLTMODE1", FLIGHT_MODE_1), // @Param: FLTMODE2 // @DisplayName: FlightMode2 // @Description: Flight mode for switch position 2 (1231 to 1360) - // @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,8:AUTOTUNE,10:Auto,11:RTL,12:Loiter,14:AVOID_ADSB,15:Guided,17:QSTABILIZE,18:QHOVER,19:QLOITER,20:QLAND,21:QRTL + // @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,8:AUTOTUNE,10:Auto,11:RTL,12:Loiter,14:AVOID_ADSB,15:Guided,17:QSTABILIZE,18:QHOVER,19:QLOITER,20:QLAND,21:QRTL,22:QAUTOTUNE // @User: Standard GSCALAR(flight_mode2, "FLTMODE2", FLIGHT_MODE_2), // @Param: FLTMODE3 // @DisplayName: FlightMode3 // @Description: Flight mode for switch position 3 (1361 to 1490) - // @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,8:AUTOTUNE,10:Auto,11:RTL,12:Loiter,14:AVOID_ADSB,15:Guided,17:QSTABILIZE,18:QHOVER,19:QLOITER,20:QLAND,21:QRTL + // @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,8:AUTOTUNE,10:Auto,11:RTL,12:Loiter,14:AVOID_ADSB,15:Guided,17:QSTABILIZE,18:QHOVER,19:QLOITER,20:QLAND,21:QRTL,22:QAUTOTUNE // @User: Standard GSCALAR(flight_mode3, "FLTMODE3", FLIGHT_MODE_3), // @Param: FLTMODE4 // @DisplayName: FlightMode4 // @Description: Flight mode for switch position 4 (1491 to 1620) - // @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,8:AUTOTUNE,10:Auto,11:RTL,12:Loiter,14:AVOID_ADSB,15:Guided,17:QSTABILIZE,18:QHOVER,19:QLOITER,20:QLAND,21:QRTL + // @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,8:AUTOTUNE,10:Auto,11:RTL,12:Loiter,14:AVOID_ADSB,15:Guided,17:QSTABILIZE,18:QHOVER,19:QLOITER,20:QLAND,21:QRTL,22:QAUTOTUNE // @User: Standard GSCALAR(flight_mode4, "FLTMODE4", FLIGHT_MODE_4), // @Param: FLTMODE5 // @DisplayName: FlightMode5 // @Description: Flight mode for switch position 5 (1621 to 1749) - // @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,8:AUTOTUNE,10:Auto,11:RTL,12:Loiter,14:AVOID_ADSB,15:Guided,17:QSTABILIZE,18:QHOVER,19:QLOITER,20:QLAND,21:QRTL + // @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,8:AUTOTUNE,10:Auto,11:RTL,12:Loiter,14:AVOID_ADSB,15:Guided,17:QSTABILIZE,18:QHOVER,19:QLOITER,20:QLAND,21:QRTL,22:QAUTOTUNE // @User: Standard GSCALAR(flight_mode5, "FLTMODE5", FLIGHT_MODE_5), // @Param: FLTMODE6 // @DisplayName: FlightMode6 // @Description: Flight mode for switch position 6 (1750 to 2049) - // @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,8:AUTOTUNE,10:Auto,11:RTL,12:Loiter,14:AVOID_ADSB,15:Guided,17:QSTABILIZE,18:QHOVER,19:QLOITER,20:QLAND,21:QRTL + // @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,8:AUTOTUNE,10:Auto,11:RTL,12:Loiter,14:AVOID_ADSB,15:Guided,17:QSTABILIZE,18:QHOVER,19:QLOITER,20:QLAND,21:QRTL,22:QAUTOTUNE // @User: Standard GSCALAR(flight_mode6, "FLTMODE6", FLIGHT_MODE_6), // @Param: INITIAL_MODE // @DisplayName: Initial flight mode // @Description: This selects the mode to start in on boot. This is useful for when you want to start in AUTO mode on boot without a receiver. - // @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,8:AUTOTUNE,10:Auto,11:RTL,12:Loiter,14:AVOID_ADSB,15:Guided,17:QSTABILIZE,18:QHOVER,19:QLOITER,20:QLAND,21:QRTL + // @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,8:AUTOTUNE,10:Auto,11:RTL,12:Loiter,14:AVOID_ADSB,15:Guided,17:QSTABILIZE,18:QHOVER,19:QLOITER,20:QLAND,21:QRTL,22:QAUTOTUNE // @User: Advanced GSCALAR(initial_mode, "INITIAL_MODE", MANUAL), diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 150fced5b8..53619c017e 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -151,6 +151,7 @@ public: friend class ParametersG2; friend class AP_Arming_Plane; friend class QuadPlane; + friend class QAutoTune; friend class AP_Tuning_Plane; friend class AP_AdvancedFailsafe_Plane; friend class AP_Avoidance_Plane; diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index 1f1057114e..8c9b9a8680 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -65,7 +65,8 @@ enum FlightMode { QHOVER = 18, QLOITER = 19, QLAND = 20, - QRTL = 21 + QRTL = 21, + QAUTOTUNE = 22 }; enum mode_reason_t { diff --git a/ArduPlane/events.cpp b/ArduPlane/events.cpp index 17640a8eb5..9abb2a126b 100644 --- a/ArduPlane/events.cpp +++ b/ArduPlane/events.cpp @@ -28,6 +28,7 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype, mode_reason_t re case QSTABILIZE: case QLOITER: case QHOVER: + case QAUTOTUNE: failsafe.saved_mode = control_mode; failsafe.saved_mode_set = true; set_mode(QLAND, reason); @@ -90,6 +91,7 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype, mode_reason_t rea case QSTABILIZE: case QHOVER: case QLOITER: + case QAUTOTUNE: set_mode(QLAND, reason); break; diff --git a/ArduPlane/qautotune.cpp b/ArduPlane/qautotune.cpp new file mode 100644 index 0000000000..0409862fe5 --- /dev/null +++ b/ArduPlane/qautotune.cpp @@ -0,0 +1,65 @@ +#include "Plane.h" +#include "qautotune.h" + +#if QAUTOTUNE_ENABLED + +/* + initialise QAUTOTUNE mode + */ +bool QAutoTune::init() +{ + if (!plane.quadplane.available()) { + return false; + } + + // use position hold while tuning if we were in QLOITER + bool position_hold = (plane.previous_mode == QLOITER); + + return init_internals(position_hold, + plane.quadplane.attitude_control, + plane.quadplane.pos_control, + plane.quadplane.ahrs_view, + &plane.quadplane.inertial_nav); +} + +float QAutoTune::get_pilot_desired_climb_rate_cms(void) const +{ + return plane.quadplane.get_pilot_desired_climb_rate_cms(); +} + +void QAutoTune::get_pilot_desired_rp_yrate_cd(int32_t &des_roll_cd, int32_t &des_pitch_cd, int32_t &yaw_rate_cds) +{ + if (plane.channel_roll->get_control_in() == 0 && plane.channel_pitch->get_control_in() == 0) { + des_roll_cd = 0; + des_pitch_cd = 0; + } else { + des_roll_cd = plane.nav_roll_cd; + des_pitch_cd = plane.nav_pitch_cd; + } + yaw_rate_cds = plane.quadplane.get_desired_yaw_rate_cds(); +} + +void QAutoTune::init_z_limits() +{ + plane.quadplane.pos_control->set_max_speed_z(-plane.quadplane.pilot_velocity_z_max, plane.quadplane.pilot_velocity_z_max); + plane.quadplane.pos_control->set_max_accel_z(plane.quadplane.pilot_accel_z); +} + + +// Wrote an event packet +void QAutoTune::Log_Write_Event(enum at_event id) +{ + // offset of 30 aligned with ArduCopter autotune events + uint8_t ev_id = 30 + (uint8_t)id; + DataFlash_Class::instance()->Log_Write( + "EVT", + "TimeUS,Id", + "s-", + "F-", + "QB", + AP_HAL::micros64(), + ev_id); +} + +#endif // QAUTOTUNE_ENABLED + diff --git a/ArduPlane/qautotune.h b/ArduPlane/qautotune.h new file mode 100644 index 0000000000..3e09364188 --- /dev/null +++ b/ArduPlane/qautotune.h @@ -0,0 +1,29 @@ +/* + support for autotune of quadplanes + */ + +#pragma once + +#include + +#define QAUTOTUNE_ENABLED !HAL_MINIMIZE_FEATURES + +#if QAUTOTUNE_ENABLED + +#include + +class QAutoTune : public AC_AutoTune +{ +public: + friend class QuadPlane; + + bool init() override; + +protected: + float get_pilot_desired_climb_rate_cms(void) const override; + void get_pilot_desired_rp_yrate_cd(int32_t &roll_cd, int32_t &pitch_cd, int32_t &yaw_rate_cds) override; + void init_z_limits() override; + void Log_Write_Event(enum at_event id) override; +}; + +#endif // QAUTOTUNE_ENABLED diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 4610efa3af..6d125364f3 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -332,7 +332,7 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = { AP_GROUPINFO("OPTIONS", 58, QuadPlane, options, 0), AP_SUBGROUPEXTENSION("",59, QuadPlane, var_info2), - + AP_GROUPEND }; @@ -377,6 +377,12 @@ const AP_Param::GroupInfo QuadPlane::var_info2[] = { // @User: Standard AP_GROUPINFO("TAILSIT_RLL_MX", 5, QuadPlane, tailsitter.max_roll_angle, 0), +#if QAUTOTUNE_ENABLED + // @Group: AUTOTUNE_ + // @Path: qautotune.cpp + AP_SUBGROUPINFO(qautotune, "AUTOTUNE_", 6, QuadPlane, QAutoTune), +#endif + AP_GROUPEND }; @@ -930,7 +936,7 @@ bool QuadPlane::is_flying_vtol(void) const if (plane.control_mode == GUIDED && guided_takeoff) { return true; } - if (plane.control_mode == QSTABILIZE || plane.control_mode == QHOVER || plane.control_mode == QLOITER) { + if (plane.control_mode == QSTABILIZE || plane.control_mode == QHOVER || plane.control_mode == QLOITER || plane.control_mode == QAUTOTUNE) { // in manual flight modes only consider aircraft landed when pilot demanded throttle is zero return plane.get_throttle_input() > 0; } @@ -1630,6 +1636,11 @@ void QuadPlane::control_run(void) case QRTL: control_qrtl(); break; +#if QAUTOTUNE_ENABLED + case QAUTOTUNE: + qautotune.run(); + break; +#endif default: break; } @@ -1673,6 +1684,10 @@ bool QuadPlane::init_mode(void) case GUIDED: guided_takeoff = false; break; +#if QAUTOTUNE_ENABLED + case QAUTOTUNE: + return qautotune.init(); +#endif default: break; } @@ -1762,6 +1777,7 @@ bool QuadPlane::in_vtol_mode(void) const plane.control_mode == QLOITER || plane.control_mode == QLAND || plane.control_mode == QRTL || + plane.control_mode == QAUTOTUNE || ((plane.control_mode == GUIDED || plane.control_mode == AVOID_ADSB) && plane.auto_state.vtol_loiter) || in_vtol_auto()); } @@ -2338,7 +2354,8 @@ int8_t QuadPlane::forward_throttle_pct(void) !motors->armed() || vel_forward.gain <= 0 || plane.control_mode == QSTABILIZE || - plane.control_mode == QHOVER) { + plane.control_mode == QHOVER || + plane.control_mode == QAUTOTUNE) { return 0; } @@ -2415,7 +2432,8 @@ float QuadPlane::get_weathervane_yaw_rate_cds(void) !motors->armed() || weathervane.gain <= 0 || plane.control_mode == QSTABILIZE || - plane.control_mode == QHOVER) { + plane.control_mode == QHOVER || + plane.control_mode == QAUTOTUNE) { weathervane.last_output = 0; return 0; } diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 6fdb7fd271..0cd0ecc0e9 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -10,6 +10,7 @@ #include #include #include +#include "qautotune.h" /* QuadPlane specific functionality @@ -21,7 +22,8 @@ public: friend class AP_Tuning_Plane; friend class GCS_MAVLINK_Plane; friend class AP_AdvancedFailsafe_Plane; - + friend class QAutoTune; + QuadPlane(AP_AHRS_NavEKF &_ahrs); // var_info for holding Parameter information @@ -471,7 +473,12 @@ private: return true if current mission item is a vtol landing */ bool is_vtol_land(uint16_t id) const; - + +#if QAUTOTUNE_ENABLED + // qautotune mode + QAutoTune qautotune; +#endif + public: void motor_test_output(); MAV_RESULT mavlink_motor_test_start(mavlink_channel_t chan, uint8_t motor_seq, uint8_t throttle_type, diff --git a/ArduPlane/sensors.cpp b/ArduPlane/sensors.cpp index 22013a41e1..155848241d 100644 --- a/ArduPlane/sensors.cpp +++ b/ArduPlane/sensors.cpp @@ -162,6 +162,7 @@ void Plane::update_sensor_status_flags(void) case QHOVER: case QLAND: case QLOITER: + case QAUTOTUNE: control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL; // 3D angular rate control control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION; // attitude stabilisation break; diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index 4ec8be994f..9d417e58e0 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -278,6 +278,13 @@ enum FlightMode Plane::get_previous_mode() { void Plane::set_mode(enum FlightMode mode, mode_reason_t reason) { +#if !QAUTOTUNE_ENABLED + if (mode == QAUTOTUNE) { + gcs().send_text(MAV_SEVERITY_INFO,"QAUTOTUNE disabled"); + mode = QHOVER; + } +#endif + if(control_mode == mode) { // don't switch modes if we are already in the correct mode. return; @@ -481,6 +488,7 @@ void Plane::set_mode(enum FlightMode mode, mode_reason_t reason) case QLOITER: case QLAND: case QRTL: + case QAUTOTUNE: throttle_allows_nudging = true; auto_navigation_mode = false; if (!quadplane.init_mode()) { @@ -510,7 +518,8 @@ void Plane::set_mode(enum FlightMode mode, mode_reason_t reason) void Plane::exit_mode(enum FlightMode mode) { // stop mission when we leave auto - if (mode == AUTO) { + switch (mode) { + case AUTO: if (mission.state() == AP_Mission::MISSION_RUNNING) { mission.stop(); @@ -521,6 +530,14 @@ void Plane::exit_mode(enum FlightMode mode) } } auto_state.started_flying_in_auto_ms = 0; + break; +#if QAUTOTUNE_ENABLED + case QAUTOTUNE: + quadplane.qautotune.stop(); + break; +#endif + default: + break; } } @@ -703,6 +720,9 @@ void Plane::notify_flight_mode(enum FlightMode mode) case QRTL: notify.set_flight_mode_str("QRTL"); break; + case QAUTOTUNE: + notify.set_flight_mode_str("QAUTOTUNE"); + break; default: notify.set_flight_mode_str("----"); break; @@ -781,5 +801,10 @@ bool Plane::disarm_motors(void) // reload target airspeed which could have been modified by a mission plane.aparm.airspeed_cruise_cm.load(); +#if QAUTOTUNE_ENABLED + //save qautotune gains if enabled and success + quadplane.qautotune.save_tuning_gains(); +#endif + return true; } diff --git a/ArduPlane/tiltrotor.cpp b/ArduPlane/tiltrotor.cpp index 89b3a83809..da3ce73d04 100644 --- a/ArduPlane/tiltrotor.cpp +++ b/ArduPlane/tiltrotor.cpp @@ -110,7 +110,8 @@ void QuadPlane::tiltrotor_continuous_update(void) to forward flight and should put the rotors all the way forward */ if (plane.control_mode == QSTABILIZE || - plane.control_mode == QHOVER) { + plane.control_mode == QHOVER || + plane.control_mode == QAUTOTUNE) { tiltrotor_slew(0); return; } diff --git a/ArduPlane/wscript b/ArduPlane/wscript index 81cc188ed7..4da085b38e 100644 --- a/ArduPlane/wscript +++ b/ArduPlane/wscript @@ -32,6 +32,7 @@ def build(bld): 'AP_Soaring', 'AP_Devo_Telem', 'AP_OSD', + 'AC_AutoTune', ], )