Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Plane: added QLOITER mode 

quadplane loiter
This commit is contained in:
Andrew Tridgell 2015-12-26 19:51:05 +11:00
parent 00ca292160
commit 2983576067
9 changed files with 122 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduPlane/ArduPlane.cpp
View File

@ -690,7 +690,8 @@ void Plane::update_flight_mode(void)
case QSTABILIZE:
case QHOVER: {
case QHOVER:
case QLOITER: {
// set nav_roll and nav_pitch using sticks
nav_roll_cd = channel_roll->norm_input() * roll_limit_cd;
nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit_cd, roll_limit_cd);
@ -770,6 +771,7 @@ void Plane::update_navigation()
case CIRCLE:
case QSTABILIZE:
case QHOVER:
case QLOITER:
// nothing to do
break;
}

8
ArduPlane/Attitude.cpp
View File

@ -142,6 +142,7 @@ void Plane::stabilize_stick_mixing_direct()
control_mode == CRUISE ||
control_mode == QSTABILIZE ||
control_mode == QHOVER ||
control_mode == QLOITER ||
control_mode == TRAINING) {
return;
}
@ -163,6 +164,7 @@ void Plane::stabilize_stick_mixing_fbw()
control_mode == CRUISE ||
control_mode == QSTABILIZE ||
control_mode == QHOVER ||
control_mode == QLOITER ||
control_mode == TRAINING ||
(control_mode == AUTO && g.auto_fbw_steer)) {
return;
@ -362,6 +364,8 @@ void Plane::stabilize()
quadplane.control_stabilize();
} else if (control_mode == QHOVER) {
quadplane.control_hover();
} else if (control_mode == QLOITER) {
quadplane.control_loiter();
} else {
if (g.stick_mixing == STICK_MIXING_FBW && control_mode != STABILIZE) {
stabilize_stick_mixing_fbw();
@ -926,7 +930,9 @@ void Plane::set_servos(void)
guided_throttle_passthru) {
// manual pass through of throttle while in GUIDED
channel_throttle->radio_out = channel_throttle->radio_in;
} else if (control_mode == QSTABILIZE || control_mode == QHOVER) {
} else if (control_mode == QSTABILIZE ||
control_mode == QHOVER ||
control_mode == QLOITER) {
// no forward throttle for now
channel_throttle->servo_out = 0;
channel_throttle->calc_pwm();

2
ArduPlane/GCS_Mavlink.cpp
View File

@ -41,6 +41,7 @@ void Plane::send_heartbeat(mavlink_channel_t chan)
case FLY_BY_WIRE_B:
case QSTABILIZE:
case QHOVER:
case QLOITER:
case CRUISE:
base_mode = MAV_MODE_FLAG_STABILIZE_ENABLED;
break;
@ -173,6 +174,7 @@ void Plane::send_extended_status1(mavlink_channel_t chan)
case AUTOTUNE:
case QSTABILIZE:
case QHOVER:
case QLOITER:
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;

3
ArduPlane/defines.h
View File

@ -65,7 +65,8 @@ enum FlightMode {
GUIDED = 15,
INITIALISING = 16,
QSTABILIZE = 17,
QHOVER = 18
QHOVER = 18,
QLOITER = 19
};
// type of stick mixing enabled

2
ArduPlane/events.cpp
View File

@ -28,6 +28,7 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype)
break;
case QSTABILIZE:
case QLOITER:
failsafe.saved_mode = control_mode;
failsafe.saved_mode_set = 1;
set_mode(QHOVER);
@ -82,6 +83,7 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype)
break;
case QSTABILIZE:
case QLOITER:
set_mode(QHOVER);
break;

1
ArduPlane/make.inc
View File

@ -53,3 +53,4 @@ LIBRARIES += AP_Motors
LIBRARIES += AC_AttitudeControl
LIBRARIES += AC_PID
LIBRARIES += AP_InertialNav
LIBRARIES += AC_WPNav

108
ArduPlane/quadplane.cpp
View File

@ -135,11 +135,11 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = {
// @Param: TRANSITION_MS
// @DisplayName: Transition time
// @Description: Transition time in milliseconds
// @Description: Transition time in milliseconds after minimum airspeed is reached
// @Units: milli-seconds
// @Range: 0 30000
// @User: Advanced
AP_GROUPINFO("TRANSITION_MS", 10, QuadPlane, transition_time_ms, 3000),
AP_GROUPINFO("TRANSITION_MS", 10, QuadPlane, transition_time_ms, 5000),
// @Param: PZ_P
// @DisplayName: Position (vertical) controller P gain
@ -240,6 +240,10 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = {
// @User: Standard
AP_GROUPINFO("ACCEL_Z", 18, QuadPlane, pilot_accel_z, 250),
// @Group: WP_
// @Path: ../libraries/AC_WPNav/AC_WPNav.cpp
AP_SUBGROUPINFO(wp_nav, "WP_", 19, QuadPlane, AC_WPNav),
AP_GROUPEND
};
@ -273,23 +277,17 @@ void QuadPlane::init_stabilize(void)
// hold in stabilize with given throttle
void QuadPlane::hold_stabilize(float throttle_in)
{
// max 100 degrees/sec for now
const float yaw_rate_max_dps = 100;
float yaw_rate_ef_cds = plane.channel_rudder->norm_input() * 100 * yaw_rate_max_dps;
{
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(plane.nav_roll_cd,
plane.nav_pitch_cd,
yaw_rate_ef_cds,
get_pilot_desired_yaw_rate_cds(),
smoothing_gain);
attitude_control.set_throttle_out(throttle_in, true, 0);
// run low level rate controllers that only require IMU data
attitude_control.rate_controller_run();
last_throttle = motors.get_throttle();
}
// quadplane stabilize mode
@ -298,8 +296,8 @@ void QuadPlane::control_stabilize(void)
int16_t pilot_throttle_scaled = plane.channel_throttle->control_in * 10;
hold_stabilize(pilot_throttle_scaled);
last_run_ms = millis();
transition_state = TRANSITION_AIRSPEED_WAIT;
last_throttle = motors.get_throttle();
}
// init quadplane hover mode
@ -323,13 +321,10 @@ void QuadPlane::hold_hover(float target_climb_rate)
pos_control.set_speed_z(-pilot_velocity_z_max, pilot_velocity_z_max);
pos_control.set_accel_z(pilot_accel_z);
const float yaw_rate_max_dps = 100;
float yaw_rate_ef_cds = plane.channel_rudder->norm_input() * 100 * yaw_rate_max_dps;
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(plane.nav_roll_cd,
plane.nav_pitch_cd,
yaw_rate_ef_cds,
get_pilot_desired_yaw_rate_cds(),
smoothing_gain);
// call position controller
@ -347,15 +342,79 @@ void QuadPlane::hold_hover(float target_climb_rate)
*/
void QuadPlane::control_hover(void)
{
// get pilot desired climb rate
float target_climb_rate = pilot_velocity_z_max * (plane.channel_throttle->control_in - 50) / 50.0;
hold_hover(get_pilot_desired_climb_rate_cms());
hold_hover(target_climb_rate);
last_run_ms = millis();
transition_state = TRANSITION_AIRSPEED_WAIT;
}
void QuadPlane::init_loiter(void)
{
// set target to current position
wp_nav.init_loiter_target();
// initialize vertical speed and acceleration
pos_control.set_speed_z(-pilot_velocity_z_max, pilot_velocity_z_max);
pos_control.set_accel_z(pilot_accel_z);
// initialise position and desired velocity
pos_control.set_alt_target(inertial_nav.get_altitude());
pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
}
// run quadplane loiter controller
void QuadPlane::control_loiter()
{
// initialize vertical speed and acceleration
pos_control.set_speed_z(-pilot_velocity_z_max, pilot_velocity_z_max);
pos_control.set_accel_z(pilot_accel_z);
// process pilot's roll and pitch input
wp_nav.set_pilot_desired_acceleration(plane.channel_roll->control_in,
plane.channel_pitch->control_in);
// Update EKF speed limit - used to limit speed when we are using optical flow
float ekfGndSpdLimit, ekfNavVelGainScaler;
ahrs.getEkfControlLimits(ekfGndSpdLimit, ekfNavVelGainScaler);
// run loiter controller
wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(),
wp_nav.get_pitch(),
get_pilot_desired_yaw_rate_cds());
// nav roll and pitch are controller by loiter controller
plane.nav_roll_cd = wp_nav.get_roll();
plane.nav_pitch_cd = wp_nav.get_pitch();
// update altitude target and call position controller
pos_control.set_alt_target_from_climb_rate_ff(get_pilot_desired_climb_rate_cms(), plane.G_Dt, false);
pos_control.update_z_controller();
// run low level rate controllers that only require IMU data
attitude_control.rate_controller_run();
last_throttle = motors.get_throttle();
}
/*
get desired yaw rate in cd/s
*/
float QuadPlane::get_pilot_desired_yaw_rate_cds(void)
{
const float yaw_rate_max_dps = 100;
return plane.channel_rudder->norm_input() * 100 * yaw_rate_max_dps;
}
// get pilot desired climb rate in cm/s
float QuadPlane::get_pilot_desired_climb_rate_cms(void)
{
return pilot_velocity_z_max * (plane.channel_throttle->control_in - 50) / 50.0;
}
// set motor arming
void QuadPlane::set_armed(bool armed)
{
@ -383,7 +442,7 @@ void QuadPlane::update_transition(void)
// we hold in hover until the required airspeed is reached
float aspeed;
if (ahrs.airspeed_estimate(&aspeed) && aspeed > plane.aparm.airspeed_min) {
last_run_ms = millis();
transition_start_ms = millis();
transition_state = TRANSITION_TIMER;
}
hold_hover(0);
@ -393,10 +452,10 @@ void QuadPlane::update_transition(void)
case TRANSITION_TIMER: {
// after airspeed is reached we degrade throttle over the
// transition time, but continue to stabilize
if (millis() - last_run_ms > (unsigned)transition_time_ms) {
if (millis() - transition_start_ms > (unsigned)transition_time_ms) {
transition_state = TRANSITION_DONE;
}
float throttle_scaled = last_throttle * (transition_time_ms - (millis() - last_run_ms)) / (float)transition_time_ms;
float throttle_scaled = last_throttle * (transition_time_ms - (millis() - transition_start_ms)) / (float)transition_time_ms;
if (throttle_scaled < 0) {
throttle_scaled = 0;
}
@ -417,7 +476,8 @@ void QuadPlane::update_transition(void)
void QuadPlane::update(void)
{
if (plane.control_mode != QSTABILIZE &&
plane.control_mode != QHOVER) {
plane.control_mode != QHOVER &&
plane.control_mode != QLOITER) {
update_transition();
} else {
motors.output();

17
ArduPlane/quadplane.h
View File

@ -5,6 +5,7 @@
#include <AC_AttitudeControl/AC_AttitudeControl_Multi.h> // Attitude control library
#include <AP_InertialNav/AP_InertialNav.h>
#include <AC_AttitudeControl/AC_PosControl.h>
#include <AC_WPNav/AC_WPNav.h>
/*
QuadPlane specific functionality
@ -24,9 +25,13 @@ public:
// main entry points for VTOL flight modes
void init_stabilize(void);
void control_stabilize(void);
void init_hover(void);
void control_hover(void);
void init_loiter(void);
void control_loiter(void);
// update transition handling
void update(void);
@ -60,6 +65,8 @@ private:
p_alt_hold, p_vel_z, pid_accel_z,
p_pos_xy, pi_vel_xy};
AC_WPNav wp_nav{inertial_nav, ahrs, pos_control, attitude_control};
// maximum vertical velocity the pilot may request
AP_Int16 pilot_velocity_z_max;
@ -74,11 +81,17 @@ private:
// hold stabilize (for transition)
void hold_stabilize(float throttle_in);
// get desired yaw rate in cd/s
float get_pilot_desired_yaw_rate_cds(void);
// get desired climb rate in cm/s
float get_pilot_desired_climb_rate_cms(void);
AP_Int16 transition_time_ms;
// last time quadplane was active, used for transition
uint32_t last_run_ms;
// timer start for transition
uint32_t transition_start_ms;
// last throttle value when active
float last_throttle;

6
ArduPlane/system.cpp
View File

@ -455,6 +455,11 @@ void Plane::set_mode(enum FlightMode mode)
auto_throttle_mode = false;
quadplane.init_hover();
break;
case QLOITER:
auto_throttle_mode = false;
quadplane.init_loiter();
break;
}
// start with throttle suppressed in auto_throttle modes
@ -491,6 +496,7 @@ bool Plane::mavlink_set_mode(uint8_t mode)
case LOITER:
case QSTABILIZE:
case QHOVER:
case QLOITER:
set_mode((enum FlightMode)mode);
return true;
}