Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-05 23:43:58 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: try to smooth auto VTOL transitions

Browse Source
This commit is contained in:
Andrew Tridgell 2016-01-10 09:42:46 +11:00
parent 640332113c
commit 7e29761b99
2 changed files with 87 additions and 50 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

128
ArduPlane/quadplane.cpp
View File

@ -443,7 +443,7 @@ void QuadPlane::hold_stabilize(float throttle_in)
// call attitude controller // call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_smooth(plane.nav_roll_cd, attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_smooth(plane.nav_roll_cd,
plane.nav_pitch_cd, plane.nav_pitch_cd,
get_pilot_desired_yaw_rate_cds(), get_desired_yaw_rate_cds(),
smoothing_gain); smoothing_gain);
if (throttle_in <= 0) { if (throttle_in <= 0) {
@ -487,7 +487,7 @@ void QuadPlane::hold_hover(float target_climb_rate)
// call attitude controller // call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_smooth(plane.nav_roll_cd, attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_smooth(plane.nav_roll_cd,
plane.nav_pitch_cd, plane.nav_pitch_cd,
get_pilot_desired_yaw_rate_cds(), get_desired_yaw_rate_cds(),
smoothing_gain); smoothing_gain);
// call position controller // call position controller
@ -594,7 +594,7 @@ void QuadPlane::control_loiter()
// call attitude controller // call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(),
wp_nav->get_pitch(), wp_nav->get_pitch(),
get_pilot_desired_yaw_rate_cds()); get_desired_yaw_rate_cds());
// nav roll and pitch are controller by loiter controller // nav roll and pitch are controller by loiter controller
plane.nav_roll_cd = wp_nav->get_roll(); plane.nav_roll_cd = wp_nav->get_roll();
@ -606,22 +606,35 @@ void QuadPlane::control_loiter()
} }
/* /*
get desired yaw rate in cd/s get pilot input yaw rate in cd/s
*/ */
float QuadPlane::get_pilot_desired_yaw_rate_cds(void) float QuadPlane::get_pilot_input_yaw_rate_cds(void)
{ {
float yaw_cds = 0;
if (assisted_flight) {
// use bank angle to get desired yaw rate
yaw_cds += desired_yaw_rate_cds();
}
if (plane.channel_throttle->control_in <= 0 && !plane.auto_throttle_mode) { if (plane.channel_throttle->control_in <= 0 && !plane.auto_throttle_mode) {
// the user may be trying to disarm // the user may be trying to disarm
return 0; return 0;
} }
// add in rudder input // add in rudder input
yaw_cds += plane.channel_rudder->norm_input() * 100 * yaw_rate_max; return plane.channel_rudder->norm_input() * 100 * yaw_rate_max;
}
/*
get overall desired yaw rate in cd/s
*/
float QuadPlane::get_desired_yaw_rate_cds(void)
{
float yaw_cds = 0;
if (assisted_flight) {
// use bank angle to get desired yaw rate
yaw_cds += desired_auto_yaw_rate_cds();
}
if (plane.channel_throttle->control_in <= 0 && !plane.auto_throttle_mode) {
// the user may be trying to disarm
return 0;
}
// add in pilot input
yaw_cds += get_pilot_input_yaw_rate_cds();
return yaw_cds; return yaw_cds;
} }
@ -682,7 +695,7 @@ float QuadPlane::assist_climb_rate_cms(void)
/* /*
calculate desired yaw rate for assistance calculate desired yaw rate for assistance
*/ */
float QuadPlane::desired_yaw_rate_cds(void) float QuadPlane::desired_auto_yaw_rate_cds(void)
{ {
float aspeed; float aspeed;
if (!ahrs.airspeed_estimate(&aspeed) || aspeed < plane.aparm.airspeed_min) { if (!ahrs.airspeed_estimate(&aspeed) || aspeed < plane.aparm.airspeed_min) {
@ -962,8 +975,9 @@ void QuadPlane::control_auto(const Location &loc)
(plane.mission.get_current_nav_cmd().id == MAV_CMD_NAV_VTOL_LAND && (plane.mission.get_current_nav_cmd().id == MAV_CMD_NAV_VTOL_LAND &&
land_state >= QLAND_FINAL)) { land_state >= QLAND_FINAL)) {
/* /*
we need to use the loiter controller for final descent as for takeoff and land-final we need to use the loiter
the wpnav controller takes over the descent rate control controller for final descent as the wpnav controller takes
over the descent rate control
*/ */
float ekfGndSpdLimit, ekfNavVelGainScaler; float ekfGndSpdLimit, ekfNavVelGainScaler;
ahrs.getEkfControlLimits(ekfGndSpdLimit, ekfNavVelGainScaler); ahrs.getEkfControlLimits(ekfGndSpdLimit, ekfNavVelGainScaler);
@ -973,43 +987,63 @@ void QuadPlane::control_auto(const Location &loc)
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_smooth(plane.nav_roll_cd, attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_smooth(plane.nav_roll_cd,
plane.nav_pitch_cd, plane.nav_pitch_cd,
0, get_pilot_input_yaw_rate_cds(),
smoothing_gain); smoothing_gain);
} else { } else if (plane.mission.get_current_nav_cmd().id == MAV_CMD_NAV_VTOL_LAND &&
float aspeed; land_state == QLAND_POSITION) {
int pitch_limit_cd = plane.aparm.pitch_limit_max_cd; /*
if (assist_speed > 0 && ahrs.airspeed_estimate(&aspeed) && aspeed < assist_speed) { for land repositioning we run the wpnav controller, but
if (plane.mission.get_current_nav_cmd().id == MAV_CMD_NAV_VTOL_LAND && smoothly change its pitch limit to allow the plane to slow
land_state == QLAND_POSITION) { down gracefully
// when starting the reposition limit the pitch for less dramatic slow down */
const float threshold = 0.5f * assist_speed;
if (aspeed > threshold && plane.auto_state.wp_distance > 10 &&
!location_passed_point(plane.current_loc, plane.prev_WP_loc, plane.next_WP_loc)) {
float p = constrain_float((aspeed - threshold)/threshold, 0, 1);
pitch_limit_cd = p*plane.aparm.pitch_limit_max_cd + 500*(1-p);
plane.nav_pitch_cd = MIN(plane.nav_pitch_cd, pitch_limit_cd);
}
} else if (aspeed < assist_speed) {
// while transitioning limit pitch to let forward motor gain speed
pitch_limit_cd = 500;
}
}
// run wpnav controller // run wpnav controller
wp_nav->update_wpnav(); wp_nav->update_wpnav();
if (plane.mission.get_current_nav_cmd().id == MAV_CMD_NAV_VTOL_LAND && land_state >= QLAND_DESCEND) { // also run fixed wing navigation
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_smooth(plane.nav_roll_cd, plane.nav_controller->update_waypoint(plane.prev_WP_loc, plane.next_WP_loc);
plane.nav_pitch_cd, plane.nav_roll_cd = plane.nav_controller->nav_roll_cd();
0,
smoothing_gain); // yaw rate is set by fixed wing navigation controller, to keep the plane lined up nicely
} else { int32_t yaw_rate = get_desired_yaw_rate_cds();
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), // nav roll and pitch are controller by loiter controller
MIN(wp_nav->get_pitch(), pitch_limit_cd), plane.nav_roll_cd = wp_nav->get_roll();
wp_nav->get_yaw(), plane.nav_pitch_cd = wp_nav->get_pitch();
true);
} // during positioning we may be flying faster than the WP_Nav
// controller normally wants to fly. We let that happen by
// limiting the pitch controller of the WP_Nav controller
int32_t limit = plane.auto_state.wp_proportion * plane.aparm.pitch_limit_max_cd;
plane.nav_pitch_cd = constrain_int32(plane.nav_pitch_cd, plane.aparm.pitch_limit_min_cd, limit);
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_smooth(plane.nav_roll_cd,
plane.nav_pitch_cd,
yaw_rate,
smoothing_gain);
} else if (plane.mission.get_current_nav_cmd().id == MAV_CMD_NAV_VTOL_LAND && land_state >= QLAND_DESCEND) {
/*
for land descent we only use pilot input for yaw
*/
// run wpnav controller
wp_nav->update_wpnav();
// force yaw rate to zero
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_smooth(plane.nav_roll_cd,
plane.nav_pitch_cd,
get_pilot_input_yaw_rate_cds(),
smoothing_gain);
} else {
/*
this is full copter control of auto flight
*/
// run wpnav controller
wp_nav->update_wpnav();
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(),
wp_nav->get_pitch(),
wp_nav->get_yaw(),
true);
} }

9
ArduPlane/quadplane.h
View File

@ -94,8 +94,11 @@ private:
// hold stabilize (for transition) // hold stabilize (for transition)
void hold_stabilize(float throttle_in); void hold_stabilize(float throttle_in);
// get desired yaw rate in cd/s // get pilot desired yaw rate in cd/s
float get_pilot_desired_yaw_rate_cds(void); float get_pilot_input_yaw_rate_cds(void);
// get overall desired yaw rate in cd/s
float get_desired_yaw_rate_cds(void);
// get desired climb rate in cm/s // get desired climb rate in cm/s
float get_pilot_desired_climb_rate_cms(void); float get_pilot_desired_climb_rate_cms(void);
@ -116,7 +119,7 @@ private:
float assist_climb_rate_cms(void); float assist_climb_rate_cms(void);
// calculate desired yaw rate for assistance // calculate desired yaw rate for assistance
float desired_yaw_rate_cds(void); float desired_auto_yaw_rate_cds(void);
bool should_relax(void); bool should_relax(void);