From 063f517424297ca30d2e8235f5cb32db3e0b6d99 Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Tue, 13 Dec 2016 19:18:48 -0800 Subject: [PATCH] Plane, TECS, AP_Landing: rename stage LAND_ABORT to ABORT_LAND this will reduce confusion when searching for FLIGHT_LAND_* and you get a bunch of takeoff related hits. It will also make more sense when the landing library fully manages the FLIGHT_LAND stage entirely because it will not mange FLIGHT_LAND_ABORT --- ArduPlane/ArduPlane.cpp | 10 +++++----- ArduPlane/Attitude.cpp | 6 +++--- ArduPlane/is_flying.cpp | 2 +- ArduPlane/servos.cpp | 4 ++-- libraries/AP_Landing/AP_Landing_Slope.cpp | 4 +--- libraries/AP_TECS/AP_TECS.cpp | 14 +++++++------- libraries/AP_Vehicle/AP_Vehicle.h | 2 +- 7 files changed, 20 insertions(+), 22 deletions(-) diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 92ccf6d099..e6c65e9a4d 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -543,7 +543,7 @@ void Plane::handle_auto_mode(void) if (quadplane.in_vtol_auto()) { quadplane.control_auto(next_WP_loc); } else if (nav_cmd_id == MAV_CMD_NAV_TAKEOFF || - (nav_cmd_id == MAV_CMD_NAV_LAND && flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND_ABORT)) { + (nav_cmd_id == MAV_CMD_NAV_LAND && flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND)) { takeoff_calc_roll(); takeoff_calc_pitch(); calc_throttle(); @@ -892,7 +892,7 @@ void Plane::set_flight_stage(AP_Vehicle::FixedWing::FlightStage fs) #endif break; - case AP_Vehicle::FixedWing::FLIGHT_LAND_ABORT: + 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; @@ -974,12 +974,12 @@ void Plane::update_flight_stage(void) set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_TAKEOFF); } else if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND) { - if (landing.is_commanded_go_around() || flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND_ABORT) { + if (landing.is_commanded_go_around() || flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) { // abort mode is sticky, it must complete while executing NAV_LAND - set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_LAND_ABORT); + set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND); } 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_LAND_ABORT); + 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) { diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index d21391889d..4d9a332303 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -494,7 +494,7 @@ void Plane::calc_nav_yaw_ground(void) if (gps.ground_speed() < 1 && channel_throttle->get_control_in() == 0 && flight_stage != AP_Vehicle::FixedWing::FLIGHT_TAKEOFF && - flight_stage != AP_Vehicle::FixedWing::FLIGHT_LAND_ABORT) { + flight_stage != AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) { // manual rudder control while still steer_state.locked_course = false; steer_state.locked_course_err = 0; @@ -504,7 +504,7 @@ void Plane::calc_nav_yaw_ground(void) float steer_rate = (rudder_input/4500.0f) * g.ground_steer_dps; if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF || - flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND_ABORT) { + flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) { steer_rate = 0; } if (!is_zero(steer_rate)) { @@ -514,7 +514,7 @@ void Plane::calc_nav_yaw_ground(void) // pilot has released the rudder stick or we are still - lock the course steer_state.locked_course = true; if (flight_stage != AP_Vehicle::FixedWing::FLIGHT_TAKEOFF && - flight_stage != AP_Vehicle::FixedWing::FLIGHT_LAND_ABORT) { + flight_stage != AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) { steer_state.locked_course_err = 0; } } diff --git a/ArduPlane/is_flying.cpp b/ArduPlane/is_flying.cpp index 4e3f9a7074..197b188026 100644 --- a/ArduPlane/is_flying.cpp +++ b/ArduPlane/is_flying.cpp @@ -99,7 +99,7 @@ void Plane::update_is_flying_5Hz(void) case AP_Vehicle::FixedWing::FLIGHT_LAND_FINAL: break; - case AP_Vehicle::FixedWing::FLIGHT_LAND_ABORT: + case AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND: if (auto_state.sink_rate < -0.5f) { // steep climb is_flying_bool = true; diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index 23662f8b48..2058adf7d8 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -469,7 +469,7 @@ void Plane::set_servos_controlled(void) min_throttle = 0; } - if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF || flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND_ABORT) { + if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF || flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) { if(aparm.takeoff_throttle_max != 0) { max_throttle = aparm.takeoff_throttle_max; } else { @@ -560,7 +560,7 @@ void Plane::set_servos_flaps(void) if (control_mode == AUTO) { switch (flight_stage) { case AP_Vehicle::FixedWing::FLIGHT_TAKEOFF: - case AP_Vehicle::FixedWing::FLIGHT_LAND_ABORT: + case AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND: if (g.takeoff_flap_percent != 0) { auto_flap_percent = g.takeoff_flap_percent; } diff --git a/libraries/AP_Landing/AP_Landing_Slope.cpp b/libraries/AP_Landing/AP_Landing_Slope.cpp index 903d8d26ba..5479e7ebe1 100644 --- a/libraries/AP_Landing/AP_Landing_Slope.cpp +++ b/libraries/AP_Landing/AP_Landing_Slope.cpp @@ -35,11 +35,9 @@ bool AP_Landing::type_slope_verify_land(const AP_Vehicle::FixedWing::FlightStage // when aborting a landing, mimic the verify_takeoff with steering hold. Once // the altitude has been reached, restart the landing sequence - if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND_ABORT) { + 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)); diff --git a/libraries/AP_TECS/AP_TECS.cpp b/libraries/AP_TECS/AP_TECS.cpp index 29b089e7f9..2a794b648d 100644 --- a/libraries/AP_TECS/AP_TECS.cpp +++ b/libraries/AP_TECS/AP_TECS.cpp @@ -658,7 +658,7 @@ void AP_TECS::_update_throttle_with_airspeed(void) // Calculate integrator state, constraining state // Set integrator to a max throttle value during climbout _integTHR_state = _integTHR_state + (_STE_error * _get_i_gain()) * _DT * K_STE2Thr; - if (_flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF || _flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND_ABORT) + if (_flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF || _flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) { if (!_flags.reached_speed_takeoff) { // ensure we run at full throttle until we reach the target airspeed @@ -768,7 +768,7 @@ void AP_TECS::_update_pitch(void) float SKE_weighting = constrain_float(_spdWeight, 0.0f, 2.0f); if (!_ahrs.airspeed_sensor_enabled()) { SKE_weighting = 0.0f; - } else if ( _flags.underspeed || _flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF || _flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND_ABORT) { + } else if ( _flags.underspeed || _flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF || _flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) { SKE_weighting = 2.0f; } else if (_flags.is_doing_auto_land) { if (_spdWeightLand < 0) { @@ -832,7 +832,7 @@ void AP_TECS::_update_pitch(void) } temp += SEBdot_error * pitch_damp; - if (_flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF || _flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND_ABORT) { + if (_flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF || _flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) { temp += _PITCHminf * gainInv; } float integSEB_min = (gainInv * (_PITCHminf - 0.0783f)) - temp; @@ -895,7 +895,7 @@ void AP_TECS::_initialise_states(int32_t ptchMinCO_cd, float hgt_afe) _DT = 0.1f; // when first starting TECS, use a // small time constant } - else if (_flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF || _flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND_ABORT) + else if (_flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF || _flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) { _PITCHminf = 0.000174533f * ptchMinCO_cd; _hgt_dem_adj_last = hgt_afe; @@ -907,7 +907,7 @@ void AP_TECS::_initialise_states(int32_t ptchMinCO_cd, float hgt_afe) _flags.badDescent = false; } - if (_flight_stage != AP_Vehicle::FixedWing::FLIGHT_TAKEOFF && _flight_stage != AP_Vehicle::FixedWing::FLIGHT_LAND_ABORT) { + if (_flight_stage != AP_Vehicle::FixedWing::FLIGHT_TAKEOFF && _flight_stage != AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) { // reset takeoff speed flag when not in takeoff _flags.reached_speed_takeoff = false; } @@ -948,7 +948,7 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm, _update_speed(load_factor); if (aparm.takeoff_throttle_max != 0 && - (_flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF || _flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND_ABORT)) { + (_flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF || _flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND)) { _THRmaxf = aparm.takeoff_throttle_max * 0.01f; } else { _THRmaxf = aparm.throttle_max * 0.01f; @@ -1010,7 +1010,7 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm, } } - if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF || flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND_ABORT) { + if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF || flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) { if (!_flags.reached_speed_takeoff && _TAS_state >= _TAS_dem_adj) { // we have reached our target speed in takeoff, allow for // normal throttle control diff --git a/libraries/AP_Vehicle/AP_Vehicle.h b/libraries/AP_Vehicle/AP_Vehicle.h index caa97ebe0b..91ba0a9724 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.h +++ b/libraries/AP_Vehicle/AP_Vehicle.h @@ -67,7 +67,7 @@ public: FLIGHT_LAND_APPROACH = 4, FLIGHT_LAND_PREFLARE = 5, FLIGHT_LAND_FINAL = 6, - FLIGHT_LAND_ABORT = 7 + FLIGHT_ABORT_LAND = 7 }; };