mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-03-13 10:03:57 -03:00
Plane: refactored quadplane landing controller
allow use in other than AUTO mode
This commit is contained in:
Andrew Tridgell
parent
fff21a1db9
commit
460885c478
@ -527,7 +527,7 @@ void QuadPlane::init_land(void)
|
||||
{
|
||||
init_loiter();
|
||||
throttle_wait = false;
|
||||
land_state = QLAND_DESCEND;
|
||||
land.state = QLAND_DESCEND;
|
||||
motors_lower_limit_start_ms = 0;
|
||||
}
|
||||
|
||||
@ -638,10 +638,10 @@ void QuadPlane::control_loiter()
|
||||
} else {
|
||||
height_above_ground = plane.adjusted_relative_altitude_cm() * 0.01;
|
||||
}
|
||||
if (height_above_ground < land_final_alt && land_state < QLAND_FINAL) {
|
||||
land_state = QLAND_FINAL;
|
||||
if (height_above_ground < land_final_alt && land.state < QLAND_FINAL) {
|
||||
land.state = QLAND_FINAL;
|
||||
}
|
||||
float descent_rate = (land_state == QLAND_FINAL)? land_speed_cms:landing_descent_rate_cms(height_above_ground);
|
||||
float descent_rate = (land.state == QLAND_FINAL)? land_speed_cms:landing_descent_rate_cms(height_above_ground);
|
||||
pos_control->set_alt_target_from_climb_rate(-descent_rate, plane.G_Dt, true);
|
||||
check_land_complete();
|
||||
} else {
|
||||
@ -1034,63 +1034,27 @@ bool QuadPlane::in_vtol_mode(void)
|
||||
in_vtol_auto());
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
handle auto-mode when auto_state.vtol_mode is true
|
||||
main landing controller. Used for landing and RTL.
|
||||
*/
|
||||
void QuadPlane::control_auto(const Location &loc)
|
||||
void QuadPlane::land_controller(void)
|
||||
{
|
||||
if (!setup()) {
|
||||
return;
|
||||
}
|
||||
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
setup_target_position();
|
||||
|
||||
Location origin = inertial_nav.get_origin();
|
||||
Vector2f diff2d;
|
||||
Vector3f target;
|
||||
diff2d = location_diff(origin, loc);
|
||||
target.x = diff2d.x * 100;
|
||||
target.y = diff2d.y * 100;
|
||||
target.z = loc.alt - origin.alt;
|
||||
|
||||
if (!locations_are_same(loc, last_auto_target) ||
|
||||
loc.alt != last_auto_target.alt ||
|
||||
millis() - last_loiter_ms > 500) {
|
||||
wp_nav->set_wp_destination(target);
|
||||
last_auto_target = loc;
|
||||
}
|
||||
last_loiter_ms = millis();
|
||||
const Location &loc = plane.next_WP_loc;
|
||||
float ekfGndSpdLimit, ekfNavVelGainScaler;
|
||||
ahrs.getEkfControlLimits(ekfGndSpdLimit, ekfNavVelGainScaler);
|
||||
|
||||
// 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);
|
||||
|
||||
if (plane.mission.get_current_nav_cmd().id == MAV_CMD_NAV_VTOL_TAKEOFF) {
|
||||
/*
|
||||
for takeoff we need to use the loiter controller wpnav controller takes over the descent rate
|
||||
control
|
||||
*/
|
||||
float ekfGndSpdLimit, ekfNavVelGainScaler;
|
||||
ahrs.getEkfControlLimits(ekfGndSpdLimit, ekfNavVelGainScaler);
|
||||
|
||||
// run loiter controller
|
||||
wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
|
||||
|
||||
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);
|
||||
|
||||
// nav roll and pitch are controller by position controller
|
||||
plane.nav_roll_cd = pos_control->get_roll();
|
||||
plane.nav_pitch_cd = pos_control->get_pitch();
|
||||
} else if (plane.mission.get_current_nav_cmd().id == MAV_CMD_NAV_VTOL_LAND &&
|
||||
land_state >= QLAND_FINAL) {
|
||||
switch (land.state) {
|
||||
case QLAND_FINAL:
|
||||
/*
|
||||
for land-final we use the loiter controller
|
||||
*/
|
||||
float ekfGndSpdLimit, ekfNavVelGainScaler;
|
||||
ahrs.getEkfControlLimits(ekfGndSpdLimit, ekfNavVelGainScaler);
|
||||
|
||||
// run loiter controller
|
||||
wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
|
||||
@ -1102,8 +1066,9 @@ void QuadPlane::control_auto(const Location &loc)
|
||||
// nav roll and pitch are controller by position controller
|
||||
plane.nav_roll_cd = pos_control->get_roll();
|
||||
plane.nav_pitch_cd = pos_control->get_pitch();
|
||||
} else if (plane.mission.get_current_nav_cmd().id == MAV_CMD_NAV_VTOL_LAND &&
|
||||
land_state == QLAND_POSITION1) {
|
||||
break;
|
||||
|
||||
case QLAND_POSITION1: {
|
||||
Vector2f diff_wp = location_diff(plane.current_loc, loc);
|
||||
|
||||
if (land.speed_scale <= 0) {
|
||||
@ -1125,7 +1090,7 @@ void QuadPlane::control_auto(const Location &loc)
|
||||
}
|
||||
|
||||
// run fixed wing navigation
|
||||
plane.nav_controller->update_waypoint(plane.prev_WP_loc, plane.next_WP_loc);
|
||||
plane.nav_controller->update_waypoint(plane.prev_WP_loc, loc);
|
||||
|
||||
/*
|
||||
calculate target velocity, not dropping it below 2m/s
|
||||
@ -1146,9 +1111,6 @@ void QuadPlane::control_auto(const Location &loc)
|
||||
pos_control->set_desired_velocity_xy(target_speed_xy.x*100,
|
||||
target_speed_xy.y*100);
|
||||
|
||||
float ekfGndSpdLimit, ekfNavVelGainScaler;
|
||||
ahrs.getEkfControlLimits(ekfGndSpdLimit, ekfNavVelGainScaler);
|
||||
|
||||
pos_control->update_vel_controller_xyz(ekfNavVelGainScaler);
|
||||
|
||||
const Vector3f& curr_pos = inertial_nav.get_position();
|
||||
@ -1185,23 +1147,24 @@ void QuadPlane::control_auto(const Location &loc)
|
||||
smoothing_gain);
|
||||
if (plane.auto_state.wp_proportion >= 1 ||
|
||||
plane.auto_state.wp_distance < 5) {
|
||||
land_state = QLAND_POSITION2;
|
||||
land.state = QLAND_POSITION2;
|
||||
wp_nav->init_loiter_target();
|
||||
plane.gcs_send_text_fmt(MAV_SEVERITY_INFO,"Land position2 started v=%.1f d=%.1f",
|
||||
(double)ahrs.groundspeed(), (double)plane.auto_state.wp_distance);
|
||||
}
|
||||
} else if (plane.mission.get_current_nav_cmd().id == MAV_CMD_NAV_VTOL_LAND) {
|
||||
break;
|
||||
}
|
||||
|
||||
case QLAND_POSITION2:
|
||||
case QLAND_DESCEND:
|
||||
/*
|
||||
for final land repositioning and descent we run the loiter controller
|
||||
*/
|
||||
|
||||
// also run fixed wing navigation
|
||||
plane.nav_controller->update_waypoint(plane.prev_WP_loc, plane.next_WP_loc);
|
||||
plane.nav_controller->update_waypoint(plane.prev_WP_loc, loc);
|
||||
|
||||
pos_control->set_xy_target(target.x, target.y);
|
||||
|
||||
float ekfGndSpdLimit, ekfNavVelGainScaler;
|
||||
ahrs.getEkfControlLimits(ekfGndSpdLimit, ekfNavVelGainScaler);
|
||||
pos_control->set_xy_target(land.target.x, land.target.y);
|
||||
|
||||
// run loiter controller
|
||||
wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
|
||||
@ -1215,47 +1178,147 @@ void QuadPlane::control_auto(const Location &loc)
|
||||
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();
|
||||
break;
|
||||
|
||||
// call attitude controller
|
||||
attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(),
|
||||
wp_nav->get_pitch(),
|
||||
wp_nav->get_yaw(),
|
||||
true);
|
||||
// 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();
|
||||
case QLAND_COMPLETE:
|
||||
// nothing to do
|
||||
break;
|
||||
}
|
||||
|
||||
// now height control
|
||||
switch (land.state) {
|
||||
case QLAND_POSITION1:
|
||||
case QLAND_POSITION2:
|
||||
pos_control->set_alt_target_from_climb_rate(0, plane.G_Dt, false);
|
||||
break;
|
||||
|
||||
switch (plane.mission.get_current_nav_cmd().id) {
|
||||
case MAV_CMD_NAV_VTOL_LAND:
|
||||
if (land_state <= QLAND_POSITION2) {
|
||||
pos_control->set_alt_target_from_climb_rate(0, plane.G_Dt, false);
|
||||
} else if (land_state > QLAND_POSITION2 && land_state < QLAND_FINAL) {
|
||||
float height_above_ground = (plane.current_loc.alt - plane.next_WP_loc.alt)*0.01;
|
||||
pos_control->set_alt_target_from_climb_rate(-landing_descent_rate_cms(height_above_ground),
|
||||
plane.G_Dt, true);
|
||||
} else {
|
||||
pos_control->set_alt_target_from_climb_rate(-land_speed_cms, plane.G_Dt, true);
|
||||
}
|
||||
case QLAND_DESCEND: {
|
||||
float height_above_ground = (plane.current_loc.alt - loc.alt)*0.01;
|
||||
pos_control->set_alt_target_from_climb_rate(-landing_descent_rate_cms(height_above_ground),
|
||||
plane.G_Dt, true);
|
||||
break;
|
||||
case MAV_CMD_NAV_VTOL_TAKEOFF:
|
||||
pos_control->set_alt_target_from_climb_rate(100, plane.G_Dt, true);
|
||||
}
|
||||
|
||||
case QLAND_FINAL:
|
||||
pos_control->set_alt_target_from_climb_rate(-land_speed_cms, plane.G_Dt, true);
|
||||
break;
|
||||
default:
|
||||
pos_control->set_alt_target_from_climb_rate_ff(assist_climb_rate_cms(), plane.G_Dt, false);
|
||||
|
||||
case QLAND_COMPLETE:
|
||||
break;
|
||||
}
|
||||
|
||||
pos_control->update_z_controller();
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
setup the target position based on plane.next_WP_loc
|
||||
*/
|
||||
void QuadPlane::setup_target_position(void)
|
||||
{
|
||||
const Location &loc = plane.next_WP_loc;
|
||||
Location origin = inertial_nav.get_origin();
|
||||
Vector2f diff2d;
|
||||
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
|
||||
diff2d = location_diff(origin, loc);
|
||||
land.target.x = diff2d.x * 100;
|
||||
land.target.y = diff2d.y * 100;
|
||||
land.target.z = plane.next_WP_loc.alt - origin.alt;
|
||||
|
||||
if (!locations_are_same(loc, last_auto_target) ||
|
||||
plane.next_WP_loc.alt != last_auto_target.alt ||
|
||||
millis() - last_loiter_ms > 500) {
|
||||
wp_nav->set_wp_destination(land.target);
|
||||
last_auto_target = loc;
|
||||
}
|
||||
last_loiter_ms = millis();
|
||||
|
||||
// setup 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);
|
||||
}
|
||||
|
||||
/*
|
||||
run takeoff controller to climb vertically
|
||||
*/
|
||||
void QuadPlane::takeoff_controller(void)
|
||||
{
|
||||
/*
|
||||
for takeoff we need to use the loiter controller wpnav controller takes over the descent rate
|
||||
control
|
||||
*/
|
||||
float ekfGndSpdLimit, ekfNavVelGainScaler;
|
||||
ahrs.getEkfControlLimits(ekfGndSpdLimit, ekfNavVelGainScaler);
|
||||
|
||||
setup_target_position();
|
||||
|
||||
// run loiter controller
|
||||
wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
|
||||
|
||||
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);
|
||||
|
||||
// nav roll and pitch are controller by position controller
|
||||
plane.nav_roll_cd = pos_control->get_roll();
|
||||
plane.nav_pitch_cd = pos_control->get_pitch();
|
||||
|
||||
pos_control->set_alt_target_from_climb_rate(wp_nav->get_speed_up(), plane.G_Dt, true);
|
||||
pos_control->update_z_controller();
|
||||
}
|
||||
|
||||
/*
|
||||
run waypoint controller between prev_WP_loc and next_WP_loc
|
||||
*/
|
||||
void QuadPlane::waypoint_controller(void)
|
||||
{
|
||||
setup_target_position();
|
||||
|
||||
/*
|
||||
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);
|
||||
// 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();
|
||||
|
||||
// climb based on altitude error
|
||||
pos_control->set_alt_target_from_climb_rate_ff(assist_climb_rate_cms(), plane.G_Dt, false);
|
||||
pos_control->update_z_controller();
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
handle auto-mode when auto_state.vtol_mode is true
|
||||
*/
|
||||
void QuadPlane::control_auto(const Location &loc)
|
||||
{
|
||||
if (!setup()) {
|
||||
return;
|
||||
}
|
||||
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
|
||||
if (plane.mission.get_current_nav_cmd().id == MAV_CMD_NAV_VTOL_TAKEOFF) {
|
||||
takeoff_controller();
|
||||
} else if (plane.mission.get_current_nav_cmd().id == MAV_CMD_NAV_VTOL_LAND) {
|
||||
land_controller();
|
||||
} else {
|
||||
waypoint_controller();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/*
|
||||
start a VTOL takeoff
|
||||
*/
|
||||
@ -1303,12 +1366,11 @@ bool QuadPlane::do_vtol_land(const AP_Mission::Mission_Command& cmd)
|
||||
plane.set_next_WP(cmd.content.location);
|
||||
// initially aim for current altitude
|
||||
plane.next_WP_loc.alt = plane.current_loc.alt;
|
||||
land_state = QLAND_POSITION1;
|
||||
land.state = QLAND_POSITION1;
|
||||
land.speed_scale = 0;
|
||||
wp_nav->init_loiter_target();
|
||||
|
||||
throttle_wait = false;
|
||||
land.yaw_cd = get_bearing_cd(plane.prev_WP_loc, plane.next_WP_loc);
|
||||
motors_lower_limit_start_ms = 0;
|
||||
Location origin = inertial_nav.get_origin();
|
||||
Vector2f diff2d;
|
||||
@ -1342,11 +1404,11 @@ bool QuadPlane::verify_vtol_takeoff(const AP_Mission::Mission_Command &cmd)
|
||||
|
||||
void QuadPlane::check_land_complete(void)
|
||||
{
|
||||
if (land_state == QLAND_FINAL &&
|
||||
if (land.state == QLAND_FINAL &&
|
||||
(motors_lower_limit_start_ms != 0 &&
|
||||
millis() - motors_lower_limit_start_ms > 5000)) {
|
||||
plane.disarm_motors();
|
||||
land_state = QLAND_COMPLETE;
|
||||
land.state = QLAND_COMPLETE;
|
||||
plane.gcs_send_text(MAV_SEVERITY_INFO,"Land complete");
|
||||
// reload target airspeed which could have been modified by the mission
|
||||
plane.g.airspeed_cruise_cm.load();
|
||||
@ -1361,9 +1423,9 @@ bool QuadPlane::verify_vtol_land(const AP_Mission::Mission_Command &cmd)
|
||||
if (!available()) {
|
||||
return true;
|
||||
}
|
||||
if (land_state == QLAND_POSITION2 &&
|
||||
if (land.state == QLAND_POSITION2 &&
|
||||
plane.auto_state.wp_distance < 2) {
|
||||
land_state = QLAND_DESCEND;
|
||||
land.state = QLAND_DESCEND;
|
||||
plane.gcs_send_text(MAV_SEVERITY_INFO,"Land descend started");
|
||||
plane.set_next_WP(cmd.content.location);
|
||||
}
|
||||
@ -1373,9 +1435,9 @@ bool QuadPlane::verify_vtol_land(const AP_Mission::Mission_Command &cmd)
|
||||
}
|
||||
|
||||
// at land_final_alt begin final landing
|
||||
if (land_state == QLAND_DESCEND &&
|
||||
if (land.state == QLAND_DESCEND &&
|
||||
plane.current_loc.alt < plane.next_WP_loc.alt+land_final_alt*100) {
|
||||
land_state = QLAND_FINAL;
|
||||
land.state = QLAND_FINAL;
|
||||
pos_control->set_alt_target(inertial_nav.get_altitude());
|
||||
plane.gcs_send_text(MAV_SEVERITY_INFO,"Land final started");
|
||||
}
|
||||
|
||||
@ -35,6 +35,11 @@ public:
|
||||
bool init_mode(void);
|
||||
bool setup(void);
|
||||
void setup_defaults(void);
|
||||
|
||||
void land_controller(void);
|
||||
void setup_target_position(void);
|
||||
void takeoff_controller(void);
|
||||
void waypoint_controller(void);
|
||||
|
||||
// update transition handling
|
||||
void update(void);
|
||||
@ -233,18 +238,19 @@ private:
|
||||
// time we last set the loiter target
|
||||
uint32_t last_loiter_ms;
|
||||
|
||||
enum {
|
||||
enum land_state {
|
||||
QLAND_POSITION1,
|
||||
QLAND_POSITION2,
|
||||
QLAND_DESCEND,
|
||||
QLAND_FINAL,
|
||||
QLAND_COMPLETE
|
||||
} land_state;
|
||||
};
|
||||
struct {
|
||||
int32_t yaw_cd;
|
||||
enum land_state state;
|
||||
float speed_scale;
|
||||
Vector2f target_velocity;
|
||||
float max_speed;
|
||||
Vector3f target;
|
||||
} land;
|
||||
|
||||
enum frame_class {
|
||||
|
||||
Loading…
Reference in New Issue
Block a user