diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index f200e41e95..1e0dfbbb5c 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -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 { diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 3871be7c7c..6adafa6d7c 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -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: diff --git a/ArduPlane/altitude.cpp b/ArduPlane/altitude.cpp index dce8344780..5649511a06 100644 --- a/ArduPlane/altitude.cpp +++ b/ArduPlane/altitude.cpp @@ -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)) && diff --git a/ArduPlane/avoidance_adsb.cpp b/ArduPlane/avoidance_adsb.cpp index e0c8a56dd2..33dcb6199e 100644 --- a/ArduPlane/avoidance_adsb.cpp +++ b/ArduPlane/avoidance_adsb.cpp @@ -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; diff --git a/ArduPlane/events.cpp b/ArduPlane/events.cpp index 04bb91df2e..b01b68eee4 100644 --- a/ArduPlane/events.cpp +++ b/ArduPlane/events.cpp @@ -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(); } diff --git a/ArduPlane/navigation.cpp b/ArduPlane/navigation.cpp index f1c788e89e..fa26ea86cb 100644 --- a/ArduPlane/navigation.cpp +++ b/ArduPlane/navigation.cpp @@ -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(); diff --git a/ArduPlane/sensors.cpp b/ArduPlane/sensors.cpp index ba84a50819..c88d802058 100644 --- a/ArduPlane/sensors.cpp +++ b/ArduPlane/sensors.cpp @@ -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(); diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index c977ffc966..0eb2509f46 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -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(); } diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index c7804ec5e5..0fba581f6c 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -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); diff --git a/libraries/AP_Landing/AP_Landing.h b/libraries/AP_Landing/AP_Landing.h index 9275c0fc50..60bf75c272 100644 --- a/libraries/AP_Landing/AP_Landing.h +++ b/libraries/AP_Landing/AP_Landing.h @@ -19,7 +19,6 @@ #include #include #include -#include #include /// @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; diff --git a/libraries/AP_TECS/AP_TECS.cpp b/libraries/AP_TECS/AP_TECS.cpp index 9d22f6bc07..ec155c8c0a 100644 --- a/libraries/AP_TECS/AP_TECS.cpp +++ b/libraries/AP_TECS/AP_TECS.cpp @@ -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;