From 9549d2df93b63d5d1b0edd1b3ef7eb78a96e34b4 Mon Sep 17 00:00:00 2001 From: Bill Geyer Date: Tue, 5 Apr 2022 23:58:38 -0400 Subject: [PATCH] Copter: properly set feedforward enabled before exit --- ArduCopter/mode.h | 1 + ArduCopter/mode_systemid.cpp | 9 ++++++++- 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 782c7934fa..6ccbb6e28e 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -1471,6 +1471,7 @@ public: bool init(bool ignore_checks) override; void run() override; + void exit() override; bool requires_GPS() const override { return false; } bool has_manual_throttle() const override { return true; } diff --git a/ArduCopter/mode_systemid.cpp b/ArduCopter/mode_systemid.cpp index 0ce41da15a..60cb56718b 100644 --- a/ArduCopter/mode_systemid.cpp +++ b/ArduCopter/mode_systemid.cpp @@ -104,6 +104,13 @@ bool ModeSystemId::init(bool ignore_checks) return true; } +// systemId_exit - clean up systemId controller before exiting +void ModeSystemId::exit() +{ + // reset the feedfoward enabled parameter to the initialized state + attitude_control->bf_feedforward(att_bf_feedforward); +} + // systemId_run - runs the systemId controller // should be called at 100hz or more void ModeSystemId::run() @@ -179,9 +186,9 @@ void ModeSystemId::run() switch (systemid_state) { case SystemIDModeState::SYSTEMID_STATE_STOPPED: + attitude_control->bf_feedforward(att_bf_feedforward); break; case SystemIDModeState::SYSTEMID_STATE_TESTING: - attitude_control->bf_feedforward(att_bf_feedforward); if (copter.ap.land_complete) { systemid_state = SystemIDModeState::SYSTEMID_STATE_STOPPED;