Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-19 14:23:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: check stage==LAND instead of landing.in_progress

Browse Source
This commit is contained in:
Tom Pittenger 2017-01-10 21:29:03 -08:00
parent 3e66dd10d7
commit 61bc0a6206
11 changed files with 22 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
ArduPlane/ArduPlane.cpp
View File

@ -865,15 +865,12 @@ void Plane::set_flight_stage(AP_Vehicle::FixedWing::FlightStage fs)
return;
}
if (fs == AP_Vehicle::FixedWing::FLIGHT_LAND) {
landing.in_progress = true;
} else {
landing.in_progress = false;
landing.set_in_progress(fs == AP_Vehicle::FixedWing::FLIGHT_LAND);
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;
@ -910,7 +907,7 @@ void Plane::update_alt()
if (auto_throttle_mode && !throttle_suppressed) {
float distance_beyond_land_wp = 0;
if (landing.in_progress && location_passed_point(current_loc, prev_WP_loc, next_WP_loc)) {
if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND && location_passed_point(current_loc, prev_WP_loc, next_WP_loc)) {
distance_beyond_land_wp = get_distance(current_loc, next_WP_loc);
}
@ -1053,7 +1050,7 @@ float Plane::tecs_hgt_afe(void)
coming.
*/
float hgt_afe;
if (landing.in_progress) {
if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND) {
hgt_afe = height_above_target();
hgt_afe -= rangefinder_correction();
} else {

2
ArduPlane/GCS_Mavlink.cpp
View File

@ -1344,7 +1344,7 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg)
case MAV_CMD_DO_GO_AROUND:
result = MAV_RESULT_FAILED;
if (plane.landing.in_progress) {
if (plane.flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND) {
// Initiate an aborted landing. This will trigger a pitch-up and
// climb-out to a safe altitude holding heading then one of the
// following actions will occur, check for in this order:

10
ArduPlane/altitude.cpp
View File

@ -379,7 +379,7 @@ void Plane::set_offset_altitude_location(const Location &loc)
}
#endif
if (!landing.in_progress) {
if (flight_stage != AP_Vehicle::FixedWing::FLIGHT_LAND) {
// if we are within GLIDE_SLOPE_MIN meters of the target altitude
// then reset the offset to not use a glide slope. This allows for
// more accurate flight of missions where the aircraft may lose or
@ -470,7 +470,7 @@ float Plane::mission_alt_offset(void)
{
float ret = g.alt_offset;
if (control_mode == AUTO &&
(landing.in_progress || auto_state.wp_is_land_approach)) {
(flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND || auto_state.wp_is_land_approach)) {
// when landing after an aborted landing due to too high glide
// slope we use an offset from the last landing attempt
ret += landing.alt_offset;
@ -572,9 +572,7 @@ float Plane::rangefinder_correction(void)
}
// for now we only support the rangefinder for landing
bool using_rangefinder = (g.rangefinder_landing &&
control_mode == AUTO &&
landing.in_progress);
bool using_rangefinder = (g.rangefinder_landing && flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND);
if (!using_rangefinder) {
return 0;
}
@ -615,7 +613,7 @@ void Plane::rangefinder_height_update(void)
} else {
rangefinder_state.in_range = true;
if (!rangefinder_state.in_use &&
(landing.in_progress ||
(flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND ||
control_mode == QLAND ||
control_mode == QRTL ||
(control_mode == AUTO && plane.mission.get_current_nav_cmd().id == MAV_CMD_NAV_VTOL_LAND)) &&

2
ArduPlane/avoidance_adsb.cpp
View File

@ -25,7 +25,7 @@ MAV_COLLISION_ACTION AP_Avoidance_Plane::handle_avoidance(const AP_Avoidance::Ob
// take no action in some flight modes
if (plane.control_mode == MANUAL ||
(plane.control_mode == AUTO && !plane.auto_state.takeoff_complete) ||
(plane.control_mode == AUTO && plane.landing.in_progress) || // TODO: consider allowing action during approach
(plane.flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND) || // TODO: consider allowing action during approach
plane.control_mode == AUTOTUNE ||
plane.control_mode == QLAND) {
actual_action = MAV_COLLISION_ACTION_NONE;

2
ArduPlane/events.cpp
View File

@ -141,7 +141,7 @@ void Plane::low_battery_event(void)
}
gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Low battery %.2fV used %.0f mAh",
(double)battery.voltage(), (double)battery.current_total_mah());
if (!landing.in_progress) {
if (flight_stage != AP_Vehicle::FixedWing::FLIGHT_LAND) {
set_mode(RTL, MODE_REASON_BATTERY_FAILSAFE);
aparm.throttle_cruise.load();
}

2
ArduPlane/navigation.cpp
View File

@ -93,7 +93,7 @@ void Plane::calc_airspeed_errors()
channel_throttle->get_control_in()) +
((int32_t)aparm.airspeed_min * 100);
} else if (control_mode == AUTO && landing.in_progress) {
} else if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND) {
// Landing airspeed target
target_airspeed_cm = landing.get_target_airspeed_cm();

2
ArduPlane/sensors.cpp
View File

@ -33,7 +33,7 @@ void Plane::read_rangefinder(void)
#endif
{
// use the best available alt estimate via baro above home
if (landing.in_progress) {
if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND) {
// ensure the rangefinder is powered-on when land alt is higher than home altitude.
// This is done using the target alt which we know is below us and we are sinking to it
height = height_above_target();

4
ArduPlane/servos.cpp
View File

@ -28,7 +28,7 @@ void Plane::throttle_slew_limit(void)
if (control_mode==AUTO) {
if (auto_state.takeoff_complete == false && g.takeoff_throttle_slewrate != 0) {
slewrate = g.takeoff_throttle_slewrate;
} else if (landing.get_throttle_slewrate() != 0 && landing.in_progress) {
} else if (landing.get_throttle_slewrate() != 0 && flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND) {
slewrate = landing.get_throttle_slewrate();
}
}
@ -495,7 +495,7 @@ void Plane::set_servos_flaps(void)
auto_flap_percent = g.flap_1_percent;
} //else flaps stay at default zero deflection
if (landing.in_progress && landing.get_flap_percent() != 0) {
if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND && landing.get_flap_percent() != 0) {
auto_flap_percent = landing.get_flap_percent();
}

4
ArduPlane/system.cpp
View File

@ -561,7 +561,7 @@ void Plane::check_long_failsafe()
uint32_t tnow = millis();
// only act on changes
// -------------------
if(failsafe.state != FAILSAFE_LONG && failsafe.state != FAILSAFE_GCS && !landing.in_progress) {
if (failsafe.state != FAILSAFE_LONG && failsafe.state != FAILSAFE_GCS && flight_stage != AP_Vehicle::FixedWing::FLIGHT_LAND) {
if (failsafe.state == FAILSAFE_SHORT &&
(tnow - failsafe.ch3_timer_ms) > g.long_fs_timeout*1000) {
failsafe_long_on_event(FAILSAFE_LONG, MODE_REASON_RADIO_FAILSAFE);
@ -594,7 +594,7 @@ void Plane::check_short_failsafe()
{
// only act on changes
// -------------------
if(failsafe.state == FAILSAFE_NONE && !landing.in_progress) {
if(failsafe.state == FAILSAFE_NONE && flight_stage != AP_Vehicle::FixedWing::FLIGHT_LAND) {
// The condition is checked and the flag ch3_failsafe is set in radio.cpp
if(failsafe.ch3_failsafe) {
failsafe_short_on_event(FAILSAFE_SHORT, MODE_REASON_RADIO_FAILSAFE);

9
libraries/AP_Landing/AP_Landing.h
View File

@ -19,7 +19,6 @@
#include <AP_Mission/AP_Mission.h>
#include <AP_Common/AP_Common.h>
#include <AP_SpdHgtControl/AP_SpdHgtControl.h>
#include <AP_Vehicle/AP_Vehicle.h>
#include <AP_Navigation/AP_Navigation.h>
/// @class AP_Landing
@ -79,7 +78,7 @@ public:
bool request_go_around(void);
bool is_flaring(void) const;
bool is_on_approach(void) const;
void set_in_progress(const bool _in_progress) { in_progress = _in_progress; }
// helper functions
void reset(void);
@ -106,9 +105,6 @@ public:
// Set land_complete if we are within 2 seconds distance or within 3 meters altitude of touchdown
bool complete;
// are we in auto and flight mode is approach || pre-flare || final (flare)
bool in_progress;
// landing altitude offset (meters)
float alt_offset;
@ -128,6 +124,9 @@ private:
// calculated approach slope during auto-landing: ((prev_WP_loc.alt - next_WP_loc.alt)*0.01f - flare_sec * sink_rate) / get_distance(prev_WP_loc, next_WP_loc)
float slope;
// are we in auto and flight_stage is LAND
bool in_progress;
AP_Mission &mission;
AP_AHRS &ahrs;
AP_SpdHgtControl *SpdHgt_Controller;

2
libraries/AP_TECS/AP_TECS.cpp
View File

@ -936,7 +936,7 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm,
_DT = (now - _update_pitch_throttle_last_usec) * 1.0e-6f;
_update_pitch_throttle_last_usec = now;
_flags.is_doing_auto_land = _landing.in_progress;
_flags.is_doing_auto_land = (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND);
_distance_beyond_land_wp = distance_beyond_land_wp;
_flight_stage = flight_stage;