From 55b454a8a561f0a2b401d16053666c022a73f55d Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 14 Nov 2022 18:43:35 +0100 Subject: [PATCH] MulticopterPositionControl: avoid invalid setpoint message when switching to altitude controlled mode --- .../mc_pos_control/MulticopterPositionControl.cpp | 10 +++++----- .../mc_pos_control/MulticopterPositionControl.hpp | 2 +- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp index 9f7722541f..b2f88d4c65 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.cpp @@ -421,7 +421,7 @@ void MulticopterPositionControl::Run() if ((_setpoint.timestamp < _time_position_control_enabled) && (vehicle_local_position.timestamp_sample > _time_position_control_enabled)) { - _setpoint = generateFailsafeSetpoint(vehicle_local_position.timestamp_sample, states); + _setpoint = generateFailsafeSetpoint(vehicle_local_position.timestamp_sample, states, false); } } @@ -542,7 +542,7 @@ void MulticopterPositionControl::Run() // Failsafe _vehicle_constraints = {0, NAN, NAN, false, {}}; // reset constraints - _control.setInputSetpoint(generateFailsafeSetpoint(vehicle_local_position.timestamp_sample, states)); + _control.setInputSetpoint(generateFailsafeSetpoint(vehicle_local_position.timestamp_sample, states, true)); _control.setVelocityLimits(_param_mpc_xy_vel_max.get(), _param_mpc_z_vel_max_up.get(), _param_mpc_z_vel_max_dn.get()); _control.update(dt); } @@ -583,10 +583,10 @@ void MulticopterPositionControl::Run() } trajectory_setpoint_s MulticopterPositionControl::generateFailsafeSetpoint(const hrt_abstime &now, - const PositionControlStates &states) + const PositionControlStates &states, bool warn) { - // do not warn while we are disarmed, as we might not have valid setpoints yet - bool warn = _vehicle_control_mode.flag_armed && ((now - _last_warn) > 2_s); + // rate limit the warnings + warn = warn && (now - _last_warn) > 2_s; if (warn) { PX4_WARN("invalid setpoints"); diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.hpp b/src/modules/mc_pos_control/MulticopterPositionControl.hpp index 7a6be18ccd..97c4fd2169 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.hpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.hpp @@ -223,5 +223,5 @@ private: * Used to handle transitions where no proper setpoint was generated yet and when the received setpoint is invalid. * This should only happen briefly when transitioning and never during mode operation or by design. */ - trajectory_setpoint_s generateFailsafeSetpoint(const hrt_abstime &now, const PositionControlStates &states); + trajectory_setpoint_s generateFailsafeSetpoint(const hrt_abstime &now, const PositionControlStates &states, bool warn); };