Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Plane: describe flight stages without using specific stage name

This commit is contained in:
Tom Pittenger 2017-01-09 23:42:57 -08:00
parent bf835c7aee
commit e709705ab8
6 changed files with 68 additions and 90 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
ArduPlane/Attitude.cpp
View File

@ -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) {

2
ArduPlane/GCS_Mavlink.cpp
View File

@ -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

7
ArduPlane/altitude.cpp
View File

@ -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()) {

2
ArduPlane/geofence.cpp
View File

@ -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;
}

101
ArduPlane/is_flying.cpp
View File

@ -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;
}

29
ArduPlane/servos.cpp
View File

@ -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;
}