zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Plane: Move landing stages inside AP_Landing and refactor

mission-4.1.18
Michael du Breuil 8 years ago committed by Tom Pittenger
parent
ac27241a42
commit
3e66dd10d7
6 changed files with 64 additions and 48 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 57
      ArduPlane/ArduPlane.cpp
  2. 18
      ArduPlane/commands_logic.cpp
  3. 2
      ArduPlane/navigation.cpp
  4. 10
      libraries/AP_Landing/AP_Landing.cpp
  5. 10
      libraries/AP_Landing/AP_Landing.h
  6. 15
      libraries/AP_Landing/AP_Landing_Slope.cpp

57
ArduPlane/ArduPlane.cpp
Unescape View File

@ -865,47 +865,17 @@ void Plane::set_flight_stage(AP_Vehicle::FixedWing::FlightStage fs) @@ -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)");
}
}
#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:
} else {
landing.in_progress = false;
break;
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);
}
}
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) @@ -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) @@ -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) @@ -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
Unescape View File

@ -401,6 +401,24 @@ void Plane::do_land(const AP_Mission::Mission_Command& cmd) @@ -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
Unescape View File

@ -95,7 +95,7 @@ void Plane::calc_airspeed_errors() @@ -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
Unescape View File

@ -141,6 +141,16 @@ const AP_Param::GroupInfo AP_Landing::var_info[] = { @@ -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
Unescape View File

@ -68,6 +68,7 @@ public: @@ -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: @@ -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
Unescape View File

@ -21,6 +21,12 @@ @@ -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 @@ -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) { @@ -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);
}

Write Preview
Loading…
Cancel
Save