dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

MulticopterPositionControl: avoid invalid setpoint message when switching to altitude controlled mode

This commit is contained in:
Matthias Grob 2022-11-14 18:43:35 +01:00
parent 60c448ce3a
commit 55b454a8a5
2 changed files with 6 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
src/modules/mc_pos_control/MulticopterPositionControl.cpp
View File

@ -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");

2
src/modules/mc_pos_control/MulticopterPositionControl.hpp
View File

@ -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);
};