From 05ff279a18065a7527ea90f5240946ac9de8de8d Mon Sep 17 00:00:00 2001 From: Robert Lefebvre Date: Mon, 14 Dec 2015 16:40:36 -0500 Subject: [PATCH] Copter: Restrict mode changes for TradHeli. --- ArduCopter/control_althold.cpp | 5 +++-- ArduCopter/control_auto.cpp | 8 ++++++++ ArduCopter/control_autotune.cpp | 5 +++++ ArduCopter/control_brake.cpp | 8 ++++++++ ArduCopter/control_circle.cpp | 8 ++++++++ ArduCopter/control_drift.cpp | 8 ++++++++ ArduCopter/control_flip.cpp | 6 ++++++ ArduCopter/control_guided.cpp | 8 ++++++++ ArduCopter/control_loiter.cpp | 5 +++-- ArduCopter/control_poshold.cpp | 8 ++++++++ ArduCopter/control_rtl.cpp | 8 ++++++++ ArduCopter/control_sport.cpp | 8 ++++++++ 12 files changed, 81 insertions(+), 4 deletions(-) diff --git a/ArduCopter/control_althold.cpp b/ArduCopter/control_althold.cpp index 42dc882cef..55e9bcca3a 100644 --- a/ArduCopter/control_althold.cpp +++ b/ArduCopter/control_althold.cpp @@ -10,8 +10,9 @@ bool Copter::althold_init(bool ignore_checks) { #if FRAME_CONFIG == HELI_FRAME - // do not allow helis to enter Alt Hold if the Rotor Runup is not complete - if (!ignore_checks && !motors.rotor_runup_complete()){ + // do not allow helis to enter Alt Hold if the Rotor Runup is not complete and current control mode has manual throttle control, + // as this will force the helicopter to descend. + if (!ignore_checks && mode_has_manual_throttle(control_mode) && !motors.rotor_runup_complete()){ return false; } #endif diff --git a/ArduCopter/control_auto.cpp b/ArduCopter/control_auto.cpp index 0df9f1bdc8..9f919fdcba 100644 --- a/ArduCopter/control_auto.cpp +++ b/ArduCopter/control_auto.cpp @@ -22,6 +22,14 @@ // auto_init - initialise auto controller bool Copter::auto_init(bool ignore_checks) { +#if FRAME_CONFIG == HELI_FRAME + // do not allow helis to enter Alt Hold if the Rotor Runup is not complete and current control mode has manual throttle control, + // as this will force the helicopter to descend. + if (!ignore_checks && mode_has_manual_throttle(control_mode) && !motors.rotor_runup_complete()){ + return false; + } +#endif + if ((position_ok() && mission.num_commands() > 1) || ignore_checks) { auto_mode = Auto_Loiter; diff --git a/ArduCopter/control_autotune.cpp b/ArduCopter/control_autotune.cpp index 6aa7277f23..5b44e2344b 100644 --- a/ArduCopter/control_autotune.cpp +++ b/ArduCopter/control_autotune.cpp @@ -164,6 +164,11 @@ static float tune_yaw_rp, tune_yaw_rLPF, tune_yaw_sp, tune_yaw_accel; // autotune_init - should be called when autotune mode is selected bool Copter::autotune_init(bool ignore_checks) { +#if FRAME_CONFIG == HELI_FRAME + // Autotune mode not available for helicopters + return false; +#endif + bool success = true; switch (autotune_state.mode) { diff --git a/ArduCopter/control_brake.cpp b/ArduCopter/control_brake.cpp index 16e734c792..7cca0570ea 100644 --- a/ArduCopter/control_brake.cpp +++ b/ArduCopter/control_brake.cpp @@ -9,6 +9,14 @@ // brake_init - initialise brake controller bool Copter::brake_init(bool ignore_checks) { +#if FRAME_CONFIG == HELI_FRAME + // do not allow helis to enter Alt Hold if the Rotor Runup is not complete and current control mode has manual throttle control, + // as this will force the helicopter to descend. + if (!ignore_checks && mode_has_manual_throttle(control_mode) && !motors.rotor_runup_complete()){ + return false; + } +#endif + if (position_ok() || ignore_checks) { // set desired acceleration to zero diff --git a/ArduCopter/control_circle.cpp b/ArduCopter/control_circle.cpp index 03361349b7..2e9f4d8312 100644 --- a/ArduCopter/control_circle.cpp +++ b/ArduCopter/control_circle.cpp @@ -9,6 +9,14 @@ // circle_init - initialise circle controller flight mode bool Copter::circle_init(bool ignore_checks) { +#if FRAME_CONFIG == HELI_FRAME + // do not allow helis to enter Alt Hold if the Rotor Runup is not complete and current control mode has manual throttle control, + // as this will force the helicopter to descend. + if (!ignore_checks && mode_has_manual_throttle(control_mode) && !motors.rotor_runup_complete()){ + return false; + } +#endif + if (position_ok() || ignore_checks) { circle_pilot_yaw_override = false; diff --git a/ArduCopter/control_drift.cpp b/ArduCopter/control_drift.cpp index 369cbc1f86..ecb14d0335 100644 --- a/ArduCopter/control_drift.cpp +++ b/ArduCopter/control_drift.cpp @@ -31,6 +31,14 @@ // drift_init - initialise drift controller bool Copter::drift_init(bool ignore_checks) { +#if FRAME_CONFIG == HELI_FRAME + // do not allow helis to enter Alt Hold if the Rotor Runup is not complete and current control mode has manual throttle control, + // as this will force the helicopter to descend. + if (!ignore_checks && mode_has_manual_throttle(control_mode) && !motors.rotor_runup_complete()){ + return false; + } +#endif + if (position_ok() || ignore_checks) { return true; }else{ diff --git a/ArduCopter/control_flip.cpp b/ArduCopter/control_flip.cpp index 877392d8b5..6a889366cd 100644 --- a/ArduCopter/control_flip.cpp +++ b/ArduCopter/control_flip.cpp @@ -41,6 +41,12 @@ int8_t flip_pitch_dir; // pitch direction (-1 = pitch forward, 1 = // flip_init - initialise flip controller bool Copter::flip_init(bool ignore_checks) { + +#if FRAME_CONFIG == HELI_FRAME + // Flip mode not available for helis as it is untested. + return false; +#endif + // only allow flip from ACRO, Stabilize, AltHold or Drift flight modes if (control_mode != ACRO && control_mode != STABILIZE && control_mode != ALT_HOLD) { return false; diff --git a/ArduCopter/control_guided.cpp b/ArduCopter/control_guided.cpp index 1cba492b57..ef519e1bec 100644 --- a/ArduCopter/control_guided.cpp +++ b/ArduCopter/control_guided.cpp @@ -29,6 +29,14 @@ struct Guided_Limit { // guided_init - initialise guided controller bool Copter::guided_init(bool ignore_checks) { +#if FRAME_CONFIG == HELI_FRAME + // do not allow helis to enter Alt Hold if the Rotor Runup is not complete and current control mode has manual throttle control, + // as this will force the helicopter to descend. + if (!ignore_checks && mode_has_manual_throttle(control_mode) && !motors.rotor_runup_complete()){ + return false; + } +#endif + if (position_ok() || ignore_checks) { // initialise yaw set_auto_yaw_mode(get_default_auto_yaw_mode(false)); diff --git a/ArduCopter/control_loiter.cpp b/ArduCopter/control_loiter.cpp index 0f4572bad7..ef0c8c695d 100644 --- a/ArduCopter/control_loiter.cpp +++ b/ArduCopter/control_loiter.cpp @@ -10,8 +10,9 @@ bool Copter::loiter_init(bool ignore_checks) { #if FRAME_CONFIG == HELI_FRAME - // do not allow helis to enter Loiter if the Rotor Runup is not complete - if (!ignore_checks && !motors.rotor_runup_complete()){ + // do not allow helis to enter Alt Hold if the Rotor Runup is not complete and current control mode has manual throttle control, + // as this will force the helicopter to descend. + if (!ignore_checks && mode_has_manual_throttle(control_mode) && !motors.rotor_runup_complete()){ return false; } #endif diff --git a/ArduCopter/control_poshold.cpp b/ArduCopter/control_poshold.cpp index fde3ad8f66..7155d6f82e 100644 --- a/ArduCopter/control_poshold.cpp +++ b/ArduCopter/control_poshold.cpp @@ -77,6 +77,14 @@ static struct { // poshold_init - initialise PosHold controller bool Copter::poshold_init(bool ignore_checks) { +#if FRAME_CONFIG == HELI_FRAME + // do not allow helis to enter Alt Hold if the Rotor Runup is not complete and current control mode has manual throttle control, + // as this will force the helicopter to descend. + if (!ignore_checks && mode_has_manual_throttle(control_mode) && !motors.rotor_runup_complete()){ + return false; + } +#endif + // fail to initialise PosHold mode if no GPS lock if (!position_ok() && !ignore_checks) { return false; diff --git a/ArduCopter/control_rtl.cpp b/ArduCopter/control_rtl.cpp index 16b6d0a0d7..4b6aa98ef1 100644 --- a/ArduCopter/control_rtl.cpp +++ b/ArduCopter/control_rtl.cpp @@ -12,6 +12,14 @@ // rtl_init - initialise rtl controller bool Copter::rtl_init(bool ignore_checks) { +#if FRAME_CONFIG == HELI_FRAME + // do not allow helis to enter Alt Hold if the Rotor Runup is not complete and current control mode has manual throttle control, + // as this will force the helicopter to descend. + if (!ignore_checks && mode_has_manual_throttle(control_mode) && !motors.rotor_runup_complete()){ + return false; + } +#endif + if (position_ok() || ignore_checks) { rtl_climb_start(); return true; diff --git a/ArduCopter/control_sport.cpp b/ArduCopter/control_sport.cpp index 76c759638f..5c81bd7e0d 100644 --- a/ArduCopter/control_sport.cpp +++ b/ArduCopter/control_sport.cpp @@ -9,6 +9,14 @@ // sport_init - initialise sport controller bool Copter::sport_init(bool ignore_checks) { +#if FRAME_CONFIG == HELI_FRAME + // do not allow helis to enter Alt Hold if the Rotor Runup is not complete and current control mode has manual throttle control, + // as this will force the helicopter to descend. + if (!ignore_checks && mode_has_manual_throttle(control_mode) && !motors.rotor_runup_complete()){ + return false; + } +#endif + // initialize vertical speed and accelerationj pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max); pos_control.set_accel_z(g.pilot_accel_z);