aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Landing: shadow copy of plane flight_stage into landing

Browse Source 
.. and describe stages without using specific stage name
This commit is contained in:
Tom Pittenger 2017-01-09 21:29:50 -08:00
parent d1434602a5
commit 6c0296ba13
4 changed files with 85 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduPlane/ArduPlane.cpp
View File

@ -905,6 +905,7 @@ void Plane::set_flight_stage(AP_Vehicle::FixedWing::FlightStage fs)
flight_stage = fs;
landing.stage = fs;
if (should_log(MASK_LOG_MODE)) {
Log_Write_Status();

55
libraries/AP_Landing/AP_Landing.cpp
View File

@ -150,23 +150,66 @@ bool AP_Landing::verify_land(const AP_Vehicle::FixedWing::FlightStage flight_sta
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)
{
switch (type) {
default:
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);
default:
// returning TRUE while executing verify_land() will increment the
// mission index which in many cases will trigger an RTL for end-of-mission
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "Landing configuration error, invalid LAND_TYPE");
return true;
}
}
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) {
default:
case TYPE_STANDARD_GLIDE_SLOPE:
type_slope_adjust_landing_slope_for_rangefinder_bump(rangefinder_state, prev_WP_loc, next_WP_loc, current_loc, wp_distance, target_altitude_offset_cm);
break;
default:
break;
}
}
// return true while the aircraft should be in a flaring state
bool AP_Landing::is_flaring(void) const
{
switch (type) {
case TYPE_STANDARD_GLIDE_SLOPE:
return type_slope_is_flaring();
default:
return false;
}
}
// return true while the aircraft is performing a landing approach
bool AP_Landing::is_on_approach(void) const
{
switch (type) {
case TYPE_STANDARD_GLIDE_SLOPE:
return type_slope_is_on_approach();
default:
return false;
}
}
// return true when at the last stages of a land when an impact with the ground is expected soon
bool AP_Landing::is_expecting_impact(void) const
{
switch (type) {
case TYPE_STANDARD_GLIDE_SLOPE:
return type_slope_is_expecting_impact();
default:
return false;
}
}
/*
a special glide slope calculation for the landing approach
@ -179,10 +222,12 @@ void AP_Landing::adjust_landing_slope_for_rangefinder_bump(AP_Vehicle::FixedWing
void AP_Landing::setup_landing_glide_slope(const Location &prev_WP_loc, const Location &next_WP_loc, const Location &current_loc, int32_t &target_altitude_offset_cm)
{
switch (type) {
default:
case TYPE_STANDARD_GLIDE_SLOPE:
type_slope_setup_landing_glide_slope(prev_WP_loc, next_WP_loc, current_loc, target_altitude_offset_cm);
break;
default:
break;
}
}
@ -332,9 +377,11 @@ int32_t AP_Landing::get_target_airspeed_cm(const AP_Vehicle::FixedWing::FlightSt
bool AP_Landing::request_go_around(void)
{
switch (type) {
default:
case TYPE_STANDARD_GLIDE_SLOPE:
return type_slope_request_go_around();
default:
return false;
}
}

14
libraries/AP_Landing/AP_Landing.h
View File

@ -58,6 +58,9 @@ public:
AP_Param::setup_object_defaults(this, var_info);
}
// NOTE: make sure to update is_type_valid()
enum Landing_Type {
TYPE_STANDARD_GLIDE_SLOPE = 0,
// TODO: TYPE_DEEPSTALL,
@ -71,6 +74,8 @@ public:
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);
bool request_go_around(void);
bool is_flaring(void) const;
bool is_on_approach(void) const;
// helper functions
@ -92,9 +97,10 @@ public:
int8_t get_throttle_slewrate(void) const { return throttle_slewrate; }
bool is_commanded_go_around(void) const { return commanded_go_around; }
bool is_complete(void) const { return complete; }
void set_initial_slope() { initial_slope = slope; }
void set_initial_slope(void) { initial_slope = slope; }
bool is_expecting_impact(void) const;
AP_Vehicle::FixedWing::FlightStage stage;
// Flag to indicate if we have landed.
// Set land_complete if we are within 2 seconds distance or within 3 meters altitude of touchdown
@ -163,5 +169,9 @@ private:
void type_slope_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 type_slope_check_if_need_to_abort(const AP_Vehicle::FixedWing::Rangefinder_State &rangefinder_state);
bool type_slope_request_go_around(void);
bool type_slope_is_flaring(void) const;
bool type_slope_is_on_approach(void) const;
bool type_slope_is_expecting_impact(void) const;
};

26
libraries/AP_Landing/AP_Landing_Slope.cpp
View File

@ -37,10 +37,10 @@ bool AP_Landing::type_slope_verify_land(const AP_Vehicle::FixedWing::FlightStage
// 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));
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) {
@ -53,7 +53,7 @@ bool AP_Landing::type_slope_verify_land(const AP_Vehicle::FixedWing::FlightStage
}
// make sure to return false so it leaves the mission index alone
return false;
}
}
/* Set land_complete (which starts the flare) under 3 conditions:
@ -298,4 +298,20 @@ void AP_Landing::type_slope_setup_landing_glide_slope(const Location &prev_WP_lo
constrain_target_altitude_location_fn(loc, prev_WP_loc);
}
bool AP_Landing::type_slope_is_flaring(void) const
{
return (stage == AP_Vehicle::FixedWing::FlightStage::FLIGHT_LAND_FINAL);
}
bool AP_Landing::type_slope_is_on_approach(void) const
{
return (stage == AP_Vehicle::FixedWing::FLIGHT_LAND_APPROACH ||
stage == AP_Vehicle::FixedWing::FLIGHT_LAND_PREFLARE);
}
bool AP_Landing::type_slope_is_expecting_impact(void) const
{
return (stage == AP_Vehicle::FixedWing::FLIGHT_LAND_PREFLARE ||
stage == AP_Vehicle::FixedWing::FLIGHT_LAND_FINAL);
}