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

AP_Landing: port more from plane

This commit is contained in:
Tom Pittenger 2016-11-18 14:05:45 -08:00 committed by Tom Pittenger
parent 10027b21d6
commit 9a79b79f1e
8 changed files with 42 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduPlane/ArduPlane.cpp
View File

@ -972,7 +972,7 @@ void Plane::update_flight_stage(void)
} else if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND) {
if ((g.land_abort_throttle_enable && channel_throttle->get_control_in() >= 90) ||
auto_state.commanded_go_around ||
landing.commanded_go_around ||
flight_stage == AP_SpdHgtControl::FLIGHT_LAND_ABORT){
// abort mode is sticky, it must complete while executing NAV_LAND
set_flight_stage(AP_SpdHgtControl::FLIGHT_LAND_ABORT);

2
ArduPlane/GCS_Mavlink.cpp
View File

@ -1360,7 +1360,7 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg)
if (!is_zero(packet.param1)) {
plane.auto_state.takeoff_altitude_rel_cm = packet.param1 * 100;
}
plane.auto_state.commanded_go_around = true;
plane.landing.commanded_go_around = true;
result = MAV_RESULT_ACCEPTED;
plane.gcs_send_text(MAV_SEVERITY_INFO,"Go around command accepted");

3
ArduPlane/Plane.h
View File

@ -472,9 +472,6 @@ private:
// in FBWA taildragger takeoff mode
bool fbwa_tdrag_takeoff_mode:1;
// denotes if a go-around has been commanded for landing
bool commanded_go_around:1;
// Altitude threshold to complete a takeoff command in autonomous modes. Centimeters
// are we in idle mode? used for balloon launch to stop servo
// movement until altitude is reached

6
ArduPlane/commands_logic.cpp
View File

@ -25,7 +25,7 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd)
auto_state.takeoff_complete = true;
// if a go around had been commanded, clear it now.
auto_state.commanded_go_around = false;
landing.commanded_go_around = false;
// start non-idle
auto_state.idle_mode = false;
@ -145,7 +145,7 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd)
case MAV_CMD_DO_LAND_START:
//ensure go around hasn't been set
auto_state.commanded_go_around = false;
landing.commanded_go_around = false;
break;
case MAV_CMD_DO_FENCE_ENABLE:
@ -383,7 +383,7 @@ void Plane::do_nav_wp(const AP_Mission::Mission_Command& cmd)
void Plane::do_land(const AP_Mission::Mission_Command& cmd)
{
auto_state.commanded_go_around = false;
landing.commanded_go_around = false;
set_next_WP(cmd.content.location);
// configure abort altitude and pitch

26
ArduPlane/landing.cpp
View File

@ -157,31 +157,7 @@ void Plane::adjust_landing_slope_for_rangefinder_bump(void)
// re-calculate auto_state.land_slope with updated prev_WP_loc
setup_landing_glide_slope();
if (rangefinder_state.correction >= 0) { // we're too low or object is below us
// correction positive means we're too low so we should continue on with
// the newly computed shallower slope instead of pitching/throttling up
} else if (aparm.land_slope_recalc_steep_threshold_to_abort > 0) {
// correction negative means we're too high and need to point down (and speed up) to re-align
// to land on target. A large negative correction means we would have to dive down a lot and will
// generating way too much speed that we can not bleed off in time. It is better to remember
// the large baro altitude offset and abort the landing to come around again with the correct altitude
// offset and "perfect" slope.
// calculate projected slope with projected alt
float new_slope_deg = degrees(atan(landing.slope));
float initial_slope_deg = degrees(atan(landing.initial_slope));
// is projected slope too steep?
if (new_slope_deg - initial_slope_deg > aparm.land_slope_recalc_steep_threshold_to_abort) {
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Steep landing slope (%.0fm %.1fdeg)",
(double)rangefinder_state.correction, (double)(new_slope_deg - initial_slope_deg));
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "aborting landing!");
landing.alt_offset = rangefinder_state.correction;
auto_state.commanded_go_around = 1;
aparm.land_slope_recalc_steep_threshold_to_abort = 0; // disable this feature so we only perform it once
}
}
landing.check_if_need_to_abort(rangefinder_state);
}
/*

2
ArduPlane/system.cpp
View File

@ -338,7 +338,7 @@ void Plane::set_mode(enum FlightMode mode, mode_reason_t reason)
landing.checked_for_autoland = false;
// reset go around command
auto_state.commanded_go_around = false;
landing.commanded_go_around = false;
// not in pre-flare
landing.pre_flare = false;

27
libraries/AP_Landing/AP_Landing.cpp
View File

@ -187,4 +187,31 @@ Location AP_Landing::setup_landing_glide_slope(const Location &prev_WP_loc, cons
return loc;
}
void AP_Landing::check_if_need_to_abort(const AP_Vehicle::FixedWing::Rangefinder_State &rangefinder_state)
{
if (rangefinder_state.correction >= 0) { // we're too low or object is below us
// correction positive means we're too low so we should continue on with
// the newly computed shallower slope instead of pitching/throttling up
} else if (aparm.land_slope_recalc_steep_threshold_to_abort > 0 && !has_aborted_due_to_slope_recalc) {
// correction negative means we're too high and need to point down (and speed up) to re-align
// to land on target. A large negative correction means we would have to dive down a lot and will
// generating way too much speed that we can not bleed off in time. It is better to remember
// the large baro altitude offset and abort the landing to come around again with the correct altitude
// offset and "perfect" slope.
// calculate projected slope with projected alt
float new_slope_deg = degrees(atan(slope));
float initial_slope_deg = degrees(atan(initial_slope));
// is projected slope too steep?
if (new_slope_deg - initial_slope_deg > aparm.land_slope_recalc_steep_threshold_to_abort) {
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Steep landing slope (%.0fm %.1fdeg)",
(double)rangefinder_state.correction, (double)(new_slope_deg - initial_slope_deg));
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "aborting landing!");
alt_offset = rangefinder_state.correction;
commanded_go_around = true;
has_aborted_due_to_slope_recalc = true; // only allow this once.
}
}
}

8
libraries/AP_Landing/AP_Landing.h
View File

@ -19,6 +19,7 @@
#include <AP_Mission/AP_Mission.h>
#include <AP_Common/AP_Common.h>
#include <AP_SpdHgtControl/AP_SpdHgtControl.h>
#include <AP_Vehicle/AP_Vehicle.h>
/// @class AP_Landing
/// @brief Class managing ArduPlane landing methods
@ -39,6 +40,7 @@ public:
bool jump_to_landing_sequence(void);
Location setup_landing_glide_slope(const Location &prev_WP_loc, const Location &next_WP_loc);
void check_if_need_to_abort(const AP_Vehicle::FixedWing::Rangefinder_State &rangefinder_state);
static const struct AP_Param::GroupInfo var_info[];
@ -67,8 +69,14 @@ public:
// have we checked for an auto-land?
bool checked_for_autoland;
// denotes if a go-around has been commanded for landing
bool commanded_go_around;
private:
bool has_aborted_due_to_slope_recalc;
AP_Mission &mission;
AP_AHRS &ahrs;
AP_SpdHgtControl *SpdHgt_Controller;