From ac0e126099865f2cba37d68acde0043b0f12540d Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 29 Sep 2018 17:34:36 +1000 Subject: [PATCH] Copter: correct AFS terminate-via-land for Copter In the case we're terminating via land it's a good idea to continue to constantly adjust our motor outputs for stabilization and the like.... --- ArduCopter/motors.cpp | 5 ++++- libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.h | 6 +++++- 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/ArduCopter/motors.cpp b/ArduCopter/motors.cpp index d0fa6b9a58..8ea6b5df9f 100644 --- a/ArduCopter/motors.cpp +++ b/ArduCopter/motors.cpp @@ -303,7 +303,10 @@ void Copter::motors_output() // OBC rules if (g2.afs.should_crash_vehicle()) { g2.afs.terminate_vehicle(); - return; + if (!g2.afs.terminating_vehicle_via_landing()) { + return; + } + // landing must continue to run the motors output } #endif diff --git a/libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.h b/libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.h index 38ec229902..b7ba803ad1 100644 --- a/libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.h +++ b/libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.h @@ -83,7 +83,11 @@ public: // for holding parameters static const struct AP_Param::GroupInfo var_info[]; - + + bool terminating_vehicle_via_landing() const { + return _terminate_action == TERMINATE_ACTION_LAND; + }; + protected: // setup failsafe values for if FMU firmware stops running virtual void setup_IO_failsafe(void) = 0;