From f6a34e1e80ff259838dd15b17b7eb97fd347ee01 Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Mon, 26 Apr 2021 07:53:53 +0300 Subject: [PATCH] vtol_type: reset accel to pitch integrator Signed-off-by: RomanBapst --- src/modules/vtol_att_control/vtol_type.cpp | 4 ++++ src/modules/vtol_att_control/vtol_type.h | 2 ++ 2 files changed, 6 insertions(+) diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index 031d1d2f13..3b204cd823 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -140,6 +140,8 @@ void VtolType::update_mc_state() _flag_idle_mc = set_idle_mc(); } + resetAccelToPitchPitchIntegrator(); + VtolType::set_all_motor_state(motor_state::ENABLED); // copy virtual attitude setpoint to real attitude setpoint @@ -157,6 +159,8 @@ void VtolType::update_fw_state() _flag_idle_mc = !set_idle_fw(); } + resetAccelToPitchPitchIntegrator(); + VtolType::set_alternate_motor_state(motor_state::DISABLED); // copy virtual attitude setpoint to real attitude setpoint diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index 0230efc5c1..ea9a3dd9f3 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -313,6 +313,8 @@ private: bool set_motor_state(const motor_state target_state, const int32_t channel_bitmap, const int value); + void resetAccelToPitchPitchIntegrator() { _accel_to_pitch_integ = 0.f; } + }; #endif