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

AP_Landing: allow separate handling for verify_land() vs verify_abort()

This commit is contained in:
Tom Pittenger 2017-01-09 23:44:25 -08:00
parent e709705ab8
commit b570c11e26
4 changed files with 61 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
ArduPlane/commands_logic.cpp
View File

@ -247,12 +247,14 @@ bool Plane::verify_command(const AP_Mission::Mission_Command& cmd) // Ret
return verify_nav_wp(cmd);
case MAV_CMD_NAV_LAND:
{
// use rangefinder to correct if possible
float height = height_above_target() - rangefinder_correction();
if (flight_stage == AP_Vehicle::FixedWing::FlightStage::FLIGHT_ABORT_LAND) {
return landing.verify_abort_landing(prev_WP_loc, next_WP_loc, current_loc, auto_state.takeoff_altitude_rel_cm, throttle_suppressed);
return landing.verify_land(flight_stage, prev_WP_loc, next_WP_loc, current_loc,
auto_state.takeoff_altitude_rel_cm, height, auto_state.sink_rate, auto_state.wp_proportion, auto_state.last_flying_ms, arming.is_armed(), is_flying(), rangefinder_state.in_range, throttle_suppressed);
} else {
// use rangefinder to correct if possible
const float height = height_above_target() - rangefinder_correction();
return landing.verify_land(prev_WP_loc, next_WP_loc, current_loc,
height, auto_state.sink_rate, auto_state.wp_proportion, auto_state.last_flying_ms, arming.is_armed(), is_flying(), rangefinder_state.in_range);
}
case MAV_CMD_NAV_LOITER_UNLIM:

35
libraries/AP_Landing/AP_Landing.cpp
View File

@ -146,13 +146,13 @@ const AP_Param::GroupInfo AP_Landing::var_info[] = {
update navigation for landing. Called when on landing approach or
final flare
*/
bool AP_Landing::verify_land(const AP_Vehicle::FixedWing::FlightStage flight_stage, const Location &prev_WP_loc, Location &next_WP_loc, const Location &current_loc,
const int32_t auto_state_takeoff_altitude_rel_cm, 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, bool &throttle_suppressed)
bool AP_Landing::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)
{
switch (type) {
case TYPE_STANDARD_GLIDE_SLOPE:
return type_slope_verify_land(flight_stage,prev_WP_loc, next_WP_loc, current_loc,
auto_state_takeoff_altitude_rel_cm, height,sink_rate, wp_proportion, last_flying_ms, is_armed, is_flying, rangefinder_state_in_range, throttle_suppressed);
return type_slope_verify_land(prev_WP_loc, next_WP_loc, current_loc,
height, sink_rate, wp_proportion, last_flying_ms, is_armed, is_flying, rangefinder_state_in_range);
default:
// returning TRUE while executing verify_land() will increment the
// mission index which in many cases will trigger an RTL for end-of-mission
@ -161,6 +161,33 @@ bool AP_Landing::verify_land(const AP_Vehicle::FixedWing::FlightStage flight_sta
}
}
bool AP_Landing::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)
{
switch (type) {
case TYPE_STANDARD_GLIDE_SLOPE:
type_slope_verify_abort_landing(prev_WP_loc, next_WP_loc, throttle_suppressed);
break;
default:
break;
}
// see if we have reached abort altitude
if (adjusted_relative_altitude_cm_fn() > auto_state_takeoff_altitude_rel_cm) {
next_WP_loc = current_loc;
mission.stop();
if (restart_landing_sequence()) {
mission.resume();
}
// else we're in AUTO with a stopped mission and handle_auto_mode() will set RTL
}
// make sure to always return false so it leaves the mission index alone
return false;
}
void AP_Landing::adjust_landing_slope_for_rangefinder_bump(AP_Vehicle::FixedWing::Rangefinder_State &rangefinder_state, Location &prev_WP_loc, Location &next_WP_loc, const Location &current_loc, const float wp_distance, int32_t &target_altitude_offset_cm)
{
switch (type) {

11
libraries/AP_Landing/AP_Landing.h
View File

@ -68,8 +68,10 @@ public:
// TODO: TYPE_HELICAL,
};
bool verify_land(const AP_Vehicle::FixedWing::FlightStage flight_stage, const Location &prev_WP_loc, Location &next_WP_loc, const Location &current_loc,
const int32_t auto_state_takeoff_altitude_rel_cm, 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, bool &throttle_suppressed);
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,
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);
void adjust_landing_slope_for_rangefinder_bump(AP_Vehicle::FixedWing::Rangefinder_State &rangefinder_state, Location &prev_WP_loc, Location &next_WP_loc, const Location &current_loc, const float wp_distance, int32_t &target_altitude_offset_cm);
void setup_landing_glide_slope(const Location &prev_WP_loc, const Location &next_WP_loc, const Location &current_loc, int32_t &target_altitude_offset_cm);
void check_if_need_to_abort(const AP_Vehicle::FixedWing::Rangefinder_State &rangefinder_state);
@ -161,8 +163,9 @@ private:
AP_Int8 type;
// Land Type STANDARD GLIDE SLOPE
bool type_slope_verify_land(const AP_Vehicle::FixedWing::FlightStage flight_stage, const Location &prev_WP_loc, Location &next_WP_loc, const Location &current_loc,
const int32_t auto_state_takeoff_altitude_rel_cm, 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, bool &throttle_suppressed);
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);
void type_slope_adjust_landing_slope_for_rangefinder_bump(AP_Vehicle::FixedWing::Rangefinder_State &rangefinder_state, Location &prev_WP_loc, Location &next_WP_loc, const Location &current_loc, const float wp_distance, int32_t &target_altitude_offset_cm);

46
libraries/AP_Landing/AP_Landing_Slope.cpp
View File

@ -21,41 +21,28 @@
#include <GCS_MAVLink/GCS.h>
#include <AP_HAL/AP_HAL.h>
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
// the altitude has been reached, restart the landing sequence
throttle_suppressed = false;
complete = false;
pre_flare = false;
nav_controller->update_heading_hold(get_bearing_cd(prev_WP_loc, next_WP_loc));
}
/*
update navigation for landing. Called when on landing approach or
final flare
*/
bool AP_Landing::type_slope_verify_land(const AP_Vehicle::FixedWing::FlightStage flight_stage, const Location &prev_WP_loc, Location &next_WP_loc, const Location &current_loc,
const int32_t auto_state_takeoff_altitude_rel_cm, 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, bool &throttle_suppressed)
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)
{
// 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
// when aborting a landing, mimic the verify_takeoff with steering hold. Once
// the altitude has been reached, restart the landing sequence
if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) {
throttle_suppressed = false;
complete = false;
pre_flare = false;
nav_controller->update_heading_hold(get_bearing_cd(prev_WP_loc, next_WP_loc));
// see if we have reached abort altitude
if (adjusted_relative_altitude_cm_fn() > auto_state_takeoff_altitude_rel_cm) {
next_WP_loc = current_loc;
mission.stop();
if (restart_landing_sequence()) {
mission.resume();
}
// else we're in AUTO with a stopped mission and handle_auto_mode() will set RTL
}
// make sure to return false so it leaves the mission index alone
return false;
}
/* Set land_complete (which starts the flare) under 3 conditions:
1) we are within LAND_FLARE_ALT meters of the landing altitude
2) we are within LAND_FLARE_SEC of the landing point vertically
@ -72,11 +59,10 @@ bool AP_Landing::type_slope_verify_land(const AP_Vehicle::FixedWing::FlightStage
// 2) passed land point and don't have an accurate AGL
// 3) probably crashed (ensures motor gets turned off)
bool on_approach_stage = (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND_APPROACH ||
flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND_PREFLARE);
bool below_flare_alt = (height <= flare_alt);
bool below_flare_sec = (flare_sec > 0 && height <= sink_rate * flare_sec);
bool probably_crashed = (aparm.crash_detection_enable && fabsf(sink_rate) < 0.2f && !is_flying);
const bool on_approach_stage = type_slope_is_on_approach();
const bool below_flare_alt = (height <= flare_alt);
const bool below_flare_sec = (flare_sec > 0 && height <= sink_rate * flare_sec);
const bool probably_crashed = (aparm.crash_detection_enable && fabsf(sink_rate) < 0.2f && !is_flying);
if ((on_approach_stage && below_flare_alt) ||
(on_approach_stage && below_flare_sec && (wp_proportion > 0.5)) ||