fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-28 15:50:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
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
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/MulticopterPositionControl.cpp
+5 -5
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");