Browse Source

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
master
Tom Pittenger 8 years ago
parent
commit
063f517424
  1. 10
      ArduPlane/ArduPlane.cpp
  2. 6
      ArduPlane/Attitude.cpp
  3. 2
      ArduPlane/is_flying.cpp
  4. 4
      ArduPlane/servos.cpp
  5. 4
      libraries/AP_Landing/AP_Landing_Slope.cpp
  6. 14
      libraries/AP_TECS/AP_TECS.cpp
  7. 2
      libraries/AP_Vehicle/AP_Vehicle.h

10
ArduPlane/ArduPlane.cpp

@ -543,7 +543,7 @@ void Plane::handle_auto_mode(void) @@ -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) @@ -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) @@ -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) {

6
ArduPlane/Attitude.cpp

@ -494,7 +494,7 @@ void Plane::calc_nav_yaw_ground(void) @@ -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) @@ -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) @@ -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;
}
}

2
ArduPlane/is_flying.cpp

@ -99,7 +99,7 @@ void Plane::update_is_flying_5Hz(void) @@ -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;

4
ArduPlane/servos.cpp

@ -469,7 +469,7 @@ void Plane::set_servos_controlled(void) @@ -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) @@ -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;
}

4
libraries/AP_Landing/AP_Landing_Slope.cpp

@ -35,11 +35,9 @@ bool AP_Landing::type_slope_verify_land(const AP_Vehicle::FixedWing::FlightStage @@ -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));

14
libraries/AP_TECS/AP_TECS.cpp

@ -658,7 +658,7 @@ void AP_TECS::_update_throttle_with_airspeed(void) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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, @@ -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, @@ -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

2
libraries/AP_Vehicle/AP_Vehicle.h

@ -67,7 +67,7 @@ public: @@ -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
};
};

Loading…
Cancel
Save