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

Plane: Move landing stages inside AP_Landing and refactor

This commit is contained in:
Michael du Breuil 2017-01-10 12:47:31 -07:00 committed by Tom Pittenger
parent ac27241a42
commit 3e66dd10d7
6 changed files with 64 additions and 48 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

57
ArduPlane/ArduPlane.cpp
View File

@ -865,47 +865,17 @@ void Plane::set_flight_stage(AP_Vehicle::FixedWing::FlightStage fs)
return;
}
switch (fs) {
case AP_Vehicle::FixedWing::FLIGHT_LAND_APPROACH:
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Landing approach start at %.1fm", (double)relative_altitude());
if (fs == AP_Vehicle::FixedWing::FLIGHT_LAND) {
landing.in_progress = true;
#if GEOFENCE_ENABLED == ENABLED
if (g.fence_autoenable == 1) {
if (! geofence_set_enabled(false, AUTO_TOGGLED)) {
gcs_send_text(MAV_SEVERITY_NOTICE, "Disable fence failed (autodisable)");
} else {
gcs_send_text(MAV_SEVERITY_NOTICE, "Fence disabled (autodisable)");
}
} else if (g.fence_autoenable == 2) {
if (! geofence_set_floor_enabled(false)) {
gcs_send_text(MAV_SEVERITY_NOTICE, "Disable fence floor failed (autodisable)");
} else {
gcs_send_text(MAV_SEVERITY_NOTICE, "Fence floor disabled (auto disable)");
}
} else {
landing.in_progress = false;
if (fs == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) {
gcs_send_text_fmt(MAV_SEVERITY_NOTICE, "Landing aborted, climbing to %dm",
auto_state.takeoff_altitude_rel_cm/100);
}
#endif
break;
case AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND:
gcs_send_text_fmt(MAV_SEVERITY_NOTICE, "Landing aborted, climbing to %dm", auto_state.takeoff_altitude_rel_cm/100);
landing.in_progress = false;
break;
case AP_Vehicle::FixedWing::FLIGHT_LAND_PREFLARE:
case AP_Vehicle::FixedWing::FLIGHT_LAND_FINAL:
landing.in_progress = true;
break;
case AP_Vehicle::FixedWing::FLIGHT_NORMAL:
case AP_Vehicle::FixedWing::FLIGHT_VTOL:
case AP_Vehicle::FixedWing::FLIGHT_TAKEOFF:
landing.in_progress = false;
break;
}
flight_stage = fs;
landing.stage = fs;
if (should_log(MASK_LOG_MODE)) {
Log_Write_Status();
@ -975,10 +945,11 @@ void Plane::update_flight_stage(void)
} else if (landing.get_abort_throttle_enable() && channel_throttle->get_control_in() >= 90) {
plane.gcs_send_text(MAV_SEVERITY_INFO,"Landing aborted via throttle");
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND);
} else if (landing.is_complete()) {
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_LAND_FINAL);
} else if (landing.pre_flare == true) {
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_LAND_PREFLARE);
} else {
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_LAND);
}
/*
// FIXME: lift this into AP_Landing_Slope
} else if (flight_stage != AP_Vehicle::FixedWing::FLIGHT_LAND_APPROACH) {
bool heading_lined_up = abs(nav_controller->bearing_error_cd()) < 1000 && !nav_controller->data_is_stale();
const bool on_flight_line = fabsf(nav_controller->crosstrack_error()) < 5.0f && !nav_controller->data_is_stale();
@ -991,7 +962,7 @@ void Plane::update_flight_stage(void)
} else {
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_NORMAL);
}
}
}*/
} else if (quadplane.in_assisted_flight()) {
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_VTOL);
} else {
@ -1010,6 +981,10 @@ void Plane::update_flight_stage(void)
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_NORMAL);
}
if (flight_stage != AP_Vehicle::FixedWing::FLIGHT_LAND) {
landing.reset();
}
// tell AHRS the airspeed to true airspeed ratio
airspeed.set_EAS2TAS(barometer.get_EAS2TAS());
}

18
ArduPlane/commands_logic.cpp
View File

@ -401,6 +401,24 @@ void Plane::do_land(const AP_Mission::Mission_Command& cmd)
// zero rangefinder state, start to accumulate good samples now
memset(&rangefinder_state, 0, sizeof(rangefinder_state));
landing.do_land(cmd, relative_altitude());
#if GEOFENCE_ENABLED == ENABLED
if (g.fence_autoenable == 1) {
if (! geofence_set_enabled(false, AUTO_TOGGLED)) {
gcs_send_text(MAV_SEVERITY_NOTICE, "Disable fence failed (autodisable)");
} else {
gcs_send_text(MAV_SEVERITY_NOTICE, "Fence disabled (autodisable)");
}
} else if (g.fence_autoenable == 2) {
if (! geofence_set_floor_enabled(false)) {
gcs_send_text(MAV_SEVERITY_NOTICE, "Disable fence floor failed (autodisable)");
} else {
gcs_send_text(MAV_SEVERITY_NOTICE, "Fence floor disabled (auto disable)");
}
}
#endif
}
void Plane::loiter_set_direction_wp(const AP_Mission::Mission_Command& cmd)

2
ArduPlane/navigation.cpp
View File

@ -95,7 +95,7 @@ void Plane::calc_airspeed_errors()
} else if (control_mode == AUTO && landing.in_progress) {
// Landing airspeed target
target_airspeed_cm = landing.get_target_airspeed_cm(flight_stage);
target_airspeed_cm = landing.get_target_airspeed_cm();
} else {
// Normal airspeed target

10
libraries/AP_Landing/AP_Landing.cpp
View File

@ -141,6 +141,16 @@ const AP_Param::GroupInfo AP_Landing::var_info[] = {
AP_GROUPEND
};
void AP_Landing::do_land(const AP_Mission::Mission_Command& cmd, const float relative_altitude) {
switch (type) {
case TYPE_STANDARD_GLIDE_SLOPE:
type_slope_do_land(cmd, relative_altitude);
break;
default:
// a incorrect type is handled in the verify_land
break;
}
}
/*
update navigation for landing. Called when on landing approach or

10
libraries/AP_Landing/AP_Landing.h
View File

@ -68,6 +68,7 @@ public:
// TODO: TYPE_HELICAL,
};
void do_land(const AP_Mission::Mission_Command& cmd, const float relative_altitude);
bool verify_abort_landing(const Location &prev_WP_loc, Location &next_WP_loc, const Location &current_loc,
const int32_t auto_state_takeoff_altitude_rel_cm, bool &throttle_suppressed);
bool verify_land(const Location &prev_WP_loc, Location &next_WP_loc, const Location &current_loc,
@ -157,10 +158,13 @@ private:
AP_Int8 type;
// Land Type STANDARD GLIDE SLOPE
enum slope_stage {SLOPE_APPROACH,
SLOPE_PREFLARE,
SLOPE_FINAL};
enum slope_stage {
SLOPE_APPROACH,
SLOPE_PREFLARE,
SLOPE_FINAL
};
slope_stage type_slope_stage;
void type_slope_do_land(const AP_Mission::Mission_Command& cmd, const float relative_altitude);
void type_slope_verify_abort_landing(const Location &prev_WP_loc, Location &next_WP_loc, bool &throttle_suppressed);
bool type_slope_verify_land(const Location &prev_WP_loc, Location &next_WP_loc, const Location &current_loc,
const float height, const float sink_rate, const float wp_proportion, const uint32_t last_flying_ms, const bool is_armed, const bool is_flying, const bool rangefinder_state_in_range);

15
libraries/AP_Landing/AP_Landing_Slope.cpp
View File

@ -21,6 +21,12 @@
#include <GCS_MAVLink/GCS.h>
#include <AP_HAL/AP_HAL.h>
void AP_Landing::type_slope_do_land(const AP_Mission::Mission_Command& cmd, const float relative_altitude)
{
type_slope_stage = SLOPE_APPROACH;
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Landing approach start at %.1fm", (double)relative_altitude);
}
void AP_Landing::type_slope_verify_abort_landing(const Location &prev_WP_loc, Location &next_WP_loc, bool &throttle_suppressed)
{
// when aborting a landing, mimic the verify_takeoff with steering hold. Once
@ -38,6 +44,7 @@ void AP_Landing::type_slope_verify_abort_landing(const Location &prev_WP_loc, Lo
bool AP_Landing::type_slope_verify_land(const Location &prev_WP_loc, Location &next_WP_loc, const Location &current_loc,
const float height, const float sink_rate, const float wp_proportion, const uint32_t last_flying_ms, const bool is_armed, const bool is_flying, const bool rangefinder_state_in_range)
{
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "Stage: %d Complete: %d", type_slope_stage, complete);
// we don't 'verify' landing in the sense that it never completes,
// so we don't verify command completion. Instead we use this to
// adjust final landing parameters
@ -325,18 +332,20 @@ int32_t AP_Landing::type_slope_get_target_airspeed_cm(void) {
bool AP_Landing::type_slope_is_flaring(void) const
{
return type_slope_stage == SLOPE_FINAL;
return in_progress && type_slope_stage == SLOPE_FINAL;
}
bool AP_Landing::type_slope_is_on_approach(void) const
{
return (type_slope_stage == SLOPE_APPROACH ||
return in_progress &&
(type_slope_stage == SLOPE_APPROACH ||
type_slope_stage == SLOPE_PREFLARE);
}
bool AP_Landing::type_slope_is_expecting_impact(void) const
{
return (type_slope_stage == SLOPE_PREFLARE ||
return in_progress &&
(type_slope_stage == SLOPE_PREFLARE ||
type_slope_stage == SLOPE_FINAL);
}