fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-30 06:53:33 +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-19 14:44:42 +01:00
committed by Matthias Grob
parent 39b0c7b2bf
commit ac8f4e3c48
1 changed files with 11 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/MulticopterPositionControl.cpp
+11 -5
View File
@@ -249,12 +249,14 @@ void MulticopterPositionControl::Run()
_control.setState(_states); _control.setState(_states);
vehicle_constraints_s constraints; 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) { if (_vehicle_constraints_sub.update(&constraints)) {
_control.resetIntegral(); _control.setConstraints(constraints);
_control.setThrustLimits(constraints.minimum_thrust, _param_mpc_thr_max.get());
if (constraints.reset_integral) {
_control.resetIntegral();
}
} }
// Run position control // Run position control
@@ -269,8 +271,12 @@ void MulticopterPositionControl::Run()
} }
failsafe(time_stamp_now, setpoint, _states, !was_in_failsafe); failsafe(time_stamp_now, setpoint, _states, !was_in_failsafe);
_control.setInputSetpoint(setpoint); _control.setInputSetpoint(setpoint);
constraints = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, false, {}}; constraints = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, false, {}};
_control.setConstraints(constraints);
_control.update(dt); _control.update(dt);
} }