Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: adding new mode QAUTOTUNE

Browse Source 
copter like autotune support for quadplanes and tailsitter in VTOL mode.

cleanup
This commit is contained in:
Nikhil Upadhye 2018-12-07 13:22:05 +05:30 committed by Andrew Tridgell
parent 47265c6fb7
commit d1e93bae83
15 changed files with 177 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduPlane/ArduPlane.cpp
View File

@ -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;
}

7
ArduPlane/Attitude.cpp
View File

@ -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 {

2
ArduPlane/GCS_Mavlink.cpp
View File

@ -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;
}

14
ArduPlane/Parameters.cpp
View File

@ -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),

1
ArduPlane/Plane.h
View File

@ -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;

3
ArduPlane/defines.h
View File

@ -65,7 +65,8 @@ enum FlightMode {
QHOVER = 18,
QLOITER = 19,
QLAND = 20,
QRTL = 21
QRTL = 21,
QAUTOTUNE = 22
};
enum mode_reason_t {

2
ArduPlane/events.cpp
View File

@ -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;

65
ArduPlane/qautotune.cpp Normal file
View File

@ -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

29
ArduPlane/qautotune.h Normal file
View File

@ -0,0 +1,29 @@
/*
support for autotune of quadplanes
*/
#pragma once
#include <AP_HAL/AP_HAL.h>
#define QAUTOTUNE_ENABLED !HAL_MINIMIZE_FEATURES
#if QAUTOTUNE_ENABLED
#include <AC_AutoTune/AC_AutoTune.h>
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

24
ArduPlane/quadplane.cpp
View File

@ -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;
}

7
ArduPlane/quadplane.h
View File

@ -10,6 +10,7 @@
#include <AC_Fence/AC_Fence.h>
#include <AC_Avoidance/AC_Avoid.h>
#include <AP_Proximity/AP_Proximity.h>
#include "qautotune.h"
/*
QuadPlane specific functionality
@ -21,6 +22,7 @@ public:
friend class AP_Tuning_Plane;
friend class GCS_MAVLINK_Plane;
friend class AP_AdvancedFailsafe_Plane;
friend class QAutoTune;
QuadPlane(AP_AHRS_NavEKF &_ahrs);
@ -472,6 +474,11 @@ private:
*/
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,

1
ArduPlane/sensors.cpp
View File

@ -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;

27
ArduPlane/system.cpp
View File

@ -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;
}

3
ArduPlane/tiltrotor.cpp
View File

@ -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;
}

1
ArduPlane/wscript
View File

@ -32,6 +32,7 @@ def build(bld):
'AP_Soaring',
'AP_Devo_Telem',
'AP_OSD',
'AC_AutoTune',
],
)