diff --git a/src/modules/angular_velocity_controller/AngularVelocityController.cpp b/src/modules/angular_velocity_controller/AngularVelocityController.cpp index 45bd171855..0c06de2c90 100644 --- a/src/modules/angular_velocity_controller/AngularVelocityController.cpp +++ b/src/modules/angular_velocity_controller/AngularVelocityController.cpp @@ -96,7 +96,8 @@ AngularVelocityController::parameters_updated() _control.setInertiaMatrix(matrix::Matrix3f(inertia)); // Hover thrust - if (!_param_mpc_use_hte.get()) { + if (!_param_mpc_use_hte.get() + || !_vehicle_control_mode.flag_armed) { _hover_thrust = _param_mpc_thr_hover.get(); } } @@ -149,11 +150,16 @@ AngularVelocityController::Run() } // Check for updates of hover thrust - if (_param_mpc_use_hte.get()) { + if (!_vehicle_control_mode.flag_armed) { + _hover_thrust = _param_mpc_thr_hover.get(); + + } else if (_param_mpc_use_hte.get()) { hover_thrust_estimate_s hte; if (_hover_thrust_estimate_sub.update(&hte)) { - _hover_thrust = hte.hover_thrust; + if (hte.valid) { + _hover_thrust = hte.hover_thrust; + } } } diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp index c06b868dae..26df37d49d 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.cpp @@ -394,6 +394,8 @@ void MulticopterPositionControl::Run() // make sure takeoff ramp is not amended by acceleration feed-forward if (!flying) { _setpoint.acceleration[2] = NAN; + // hover_thrust maybe reset on takeoff + _control.setHoverThrust(_param_mpc_thr_hover.get()); } const bool not_taken_off = (_takeoff.getTakeoffState() < TakeoffState::rampup);