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

mc_pos_control: only update constraints if topic updated

Browse Source 
- update failsafe constraints
This commit is contained in:
Daniel Agar 2021-01-18 12:02:37 -05:00 committed by Matthias Grob
parent 39b0c7b2bf
commit ac8f4e3c48
1 changed files with 11 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -249,12 +249,14 @@ void MulticopterPositionControl::Run()
_control.setState(_states);
vehicle_constraints_s constraints;
_vehicle_constraints_sub.update(&constraints);
_control.setConstraints(constraints);
_control.setThrustLimits(constraints.minimum_thrust, _param_mpc_thr_max.get());
if (constraints.reset_integral) {
_control.resetIntegral();
if (_vehicle_constraints_sub.update(&constraints)) {
_control.setConstraints(constraints);
_control.setThrustLimits(constraints.minimum_thrust, _param_mpc_thr_max.get());
if (constraints.reset_integral) {
_control.resetIntegral();
}
}
// Run position control
@ -269,8 +271,12 @@ void MulticopterPositionControl::Run()
}
failsafe(time_stamp_now, setpoint, _states, !was_in_failsafe);
_control.setInputSetpoint(setpoint);
constraints = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, false, {}};
_control.setConstraints(constraints);
_control.update(dt);
}