diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index 17665fd95d..e88742bf19 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -218,17 +218,15 @@ void Plane::stabilize_stick_mixing_fbw() */ void Plane::stabilize_yaw(float speed_scaler) { - if (control_mode == AUTO && flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND_FINAL) { - // in land final setup for ground steering + if (landing.is_flaring()) { + // in flaring then enable ground steering steering_control.ground_steering = true; } else { // otherwise use ground steering when no input control and we // are below the GROUND_STEER_ALT steering_control.ground_steering = (channel_roll->get_control_in() == 0 && fabsf(relative_altitude()) < g.ground_steer_alt); - if (control_mode == AUTO && - (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND_APPROACH || - flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND_PREFLARE)) { + if (landing.is_on_approach()) { // don't use ground steering on landing approach steering_control.ground_steering = false; } @@ -237,12 +235,11 @@ void Plane::stabilize_yaw(float speed_scaler) /* first calculate steering_control.steering for a nose or tail - wheel. - We use "course hold" mode for the rudder when either in the - final stage of landing (when the wings are help level) or when - in course hold in FBWA mode (when we are below GROUND_STEER_ALT) + wheel. We use "course hold" mode for the rudder when either performing + a flare (when the wings are held level) or when in course hold in + FBWA mode (when we are below GROUND_STEER_ALT) */ - if ((control_mode == AUTO && flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND_FINAL) || + if (landing.is_flaring() || (steer_state.hold_course_cd != -1 && steering_control.ground_steering)) { calc_nav_yaw_course(); } else if (steering_control.ground_steering) { diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index bbe80d6936..3871be7c7c 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -1344,8 +1344,6 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg) case MAV_CMD_DO_GO_AROUND: result = MAV_RESULT_FAILED; - //Not allowing go around at FLIGHT_LAND_FINAL stage on purpose -- - //if plane is close to the ground a go around could be dangerous. if (plane.landing.in_progress) { // Initiate an aborted landing. This will trigger a pitch-up and // climb-out to a safe altitude holding heading then one of the diff --git a/ArduPlane/altitude.cpp b/ArduPlane/altitude.cpp index 3e050fb86f..dce8344780 100644 --- a/ArduPlane/altitude.cpp +++ b/ArduPlane/altitude.cpp @@ -29,12 +29,11 @@ void Plane::adjust_altitude_target() control_mode == CRUISE) { return; } - if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND_FINAL) { - // in land final TECS uses TECS_LAND_SINK as a target sink + if (landing.is_flaring()) { + // during a landing flare, use TECS_LAND_SINK as a target sink // rate, and ignores the target altitude set_target_altitude_location(next_WP_loc); - } else if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND_APPROACH || - flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND_PREFLARE) { + } else if (landing.is_on_approach()) { landing.setup_landing_glide_slope(prev_WP_loc, next_WP_loc, current_loc, target_altitude.offset_cm); landing.adjust_landing_slope_for_rangefinder_bump(rangefinder_state, prev_WP_loc, next_WP_loc, current_loc, auto_state.wp_distance, target_altitude.offset_cm); } else if (reached_loiter_target()) { diff --git a/ArduPlane/geofence.cpp b/ArduPlane/geofence.cpp index 7ade4d6c3b..f6dcb135e4 100644 --- a/ArduPlane/geofence.cpp +++ b/ArduPlane/geofence.cpp @@ -310,7 +310,7 @@ void Plane::geofence_check(bool altitude_check_only) struct Location loc; // Never trigger a fence breach in the final stage of landing - if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND_FINAL || flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND_PREFLARE) { + if (landing.is_expecting_impact()) { return; } diff --git a/ArduPlane/is_flying.cpp b/ArduPlane/is_flying.cpp index 197b188026..cb4d39e4ff 100644 --- a/ArduPlane/is_flying.cpp +++ b/ArduPlane/is_flying.cpp @@ -70,6 +70,10 @@ void Plane::update_is_flying_5Hz(void) crash_state.impact_detected = false; } + if (landing.is_on_approach() && fabsf(auto_state.sink_rate) > 0.2f) { + is_flying_bool = true; + } + switch (flight_stage) { case AP_Vehicle::FixedWing::FLIGHT_TAKEOFF: @@ -89,16 +93,6 @@ void Plane::update_is_flying_5Hz(void) // TODO: detect ground impacts break; - case AP_Vehicle::FixedWing::FLIGHT_LAND_APPROACH: - if (fabsf(auto_state.sink_rate) > 0.2f) { - is_flying_bool = true; - } - break; - - case AP_Vehicle::FixedWing::FLIGHT_LAND_PREFLARE: - case AP_Vehicle::FixedWing::FLIGHT_LAND_FINAL: - break; - case AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND: if (auto_state.sink_rate < -0.5f) { // steep climb @@ -114,9 +108,7 @@ void Plane::update_is_flying_5Hz(void) // when disarmed assume not flying and need overwhelming evidence that we ARE flying is_flying_bool = airspeed_movement && gps_confirmed_movement; - if ((control_mode == AUTO) && - ((flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF) || - (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND_FINAL)) ) { + if ((flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF) || landing.is_flaring()) { is_flying_bool = false; } } @@ -205,44 +197,7 @@ void Plane::crash_detection_update(void) if (!is_flying() && arming.is_armed()) { - switch (flight_stage) - { - case AP_Vehicle::FixedWing::FLIGHT_TAKEOFF: - if (g.takeoff_throttle_min_accel > 0 && - !throttle_suppressed) { - // if you have an acceleration holding back throttle, but you met the - // accel threshold but still not fying, then you either shook/hit the - // plane or it was a failed launch. - crashed = true; - crash_state.debounce_time_total_ms = CRASH_DETECTION_DELAY_MS; - } - // TODO: handle auto missions without NAV_TAKEOFF mission cmd - break; - - case AP_Vehicle::FixedWing::FLIGHT_NORMAL: - if (!in_preLaunch_flight_stage() && been_auto_flying) { - crashed = true; - crash_state.debounce_time_total_ms = CRASH_DETECTION_DELAY_MS; - } - break; - - case AP_Vehicle::FixedWing::FLIGHT_VTOL: - // we need a totally new method for this - crashed = false; - break; - - case AP_Vehicle::FixedWing::FLIGHT_LAND_APPROACH: - if (been_auto_flying) { - crashed = true; - crash_state.debounce_time_total_ms = CRASH_DETECTION_DELAY_MS; - } - // when altitude gets low, we automatically progress to FLIGHT_LAND_FINAL - // so ground crashes most likely can not be triggered from here. However, - // a crash into a tree would be caught here. - break; - - case AP_Vehicle::FixedWing::FLIGHT_LAND_PREFLARE: - case AP_Vehicle::FixedWing::FLIGHT_LAND_FINAL: + if (landing.is_expecting_impact()) { // We should be nice and level-ish in this flight stage. If not, we most // likely had a crazy landing. Throttle is inhibited already at the flare // but go ahead and notify GCS and perform any additional post-crash actions. @@ -264,11 +219,47 @@ void Plane::crash_detection_update(void) } crash_state.checkedHardLanding = true; - break; - default: - break; - } // switch + } else if (landing.is_on_approach()) { + // when altitude gets low, we automatically flare so ground crashes + // most likely can not be triggered from here. However, + // a crash into a tree would be caught here. + if (been_auto_flying) { + crashed = true; + crash_state.debounce_time_total_ms = CRASH_DETECTION_DELAY_MS; + } + + } else { + switch (flight_stage) + { + case AP_Vehicle::FixedWing::FLIGHT_TAKEOFF: + if (g.takeoff_throttle_min_accel > 0 && + !throttle_suppressed) { + // if you have an acceleration holding back throttle, but you met the + // accel threshold but still not fying, then you either shook/hit the + // plane or it was a failed launch. + crashed = true; + crash_state.debounce_time_total_ms = CRASH_DETECTION_DELAY_MS; + } + // TODO: handle auto missions without NAV_TAKEOFF mission cmd + break; + + case AP_Vehicle::FixedWing::FLIGHT_NORMAL: + if (!in_preLaunch_flight_stage() && been_auto_flying) { + crashed = true; + crash_state.debounce_time_total_ms = CRASH_DETECTION_DELAY_MS; + } + break; + + case AP_Vehicle::FixedWing::FLIGHT_VTOL: + // we need a totally new method for this + crashed = false; + break; + + default: + break; + } // switch + } } else { crash_state.checkedHardLanding = false; } diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index 3b0ba5dfad..c977ffc966 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -417,18 +417,14 @@ void Plane::set_servos_controlled(void) min_throttle = 0; } - if (control_mode == AUTO) { - if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND_FINAL) { - min_throttle = 0; - } - - 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 { - max_throttle = aparm.throttle_max; - } + 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 { + max_throttle = aparm.throttle_max; } + } else if (landing.is_flaring()) { + min_throttle = 0; } // apply watt limiter @@ -499,6 +495,10 @@ 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) { + auto_flap_percent = landing.get_flap_percent(); + } + /* special flap levels for takeoff and landing. This works better than speed based flaps as it leads to less @@ -518,13 +518,6 @@ void Plane::set_servos_flaps(void) auto_flap_percent = g.takeoff_flap_percent; } break; - case AP_Vehicle::FixedWing::FLIGHT_LAND_APPROACH: - case AP_Vehicle::FixedWing::FLIGHT_LAND_PREFLARE: - case AP_Vehicle::FixedWing::FLIGHT_LAND_FINAL: - if (landing.get_flap_percent() != 0) { - auto_flap_percent = landing.get_flap_percent(); - } - break; default: break; }