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

PosititionControl: fix integrator windup with invalid setpoint

Browse Source
This commit is contained in:
Matthias Grob
2022-01-19 14:31:19 +01:00
parent 8811482f1d
commit d4e356a1ac
2 changed files with 24 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/PositionControl/PositionControl.cpp
+23 -14
View File
@@ -104,18 +104,21 @@ void PositionControl::setInputSetpoint(const vehicle_local_position_setpoint_s &
bool PositionControl::update(const float dt) bool PositionControl::update(const float dt)
{ {
// x and y input setpoints always have to come in pairs bool valid = _inputValid();
const bool valid = (PX4_ISFINITE(_pos_sp(0)) == PX4_ISFINITE(_pos_sp(1)))
&& (PX4_ISFINITE(_vel_sp(0)) == PX4_ISFINITE(_vel_sp(1)))
&& (PX4_ISFINITE(_acc_sp(0)) == PX4_ISFINITE(_acc_sp(1)));
_positionControl(); if (valid) {
_velocityControl(dt); _positionControl();
_velocityControl(dt);
_yawspeed_sp = PX4_ISFINITE(_yawspeed_sp) ? _yawspeed_sp : 0.f; _yawspeed_sp = PX4_ISFINITE(_yawspeed_sp) ? _yawspeed_sp : 0.f;
_yaw_sp = PX4_ISFINITE(_yaw_sp) ? _yaw_sp : _yaw; // TODO: better way to disable yaw control _yaw_sp = PX4_ISFINITE(_yaw_sp) ? _yaw_sp : _yaw; // TODO: better way to disable yaw control
}
return valid && _updateSuccessful(); // There has to be a valid output accleration and thrust setpoint otherwise something went wrong
valid = valid && PX4_ISFINITE(_acc_sp(0)) && PX4_ISFINITE(_acc_sp(1)) && PX4_ISFINITE(_acc_sp(2));
valid = valid && PX4_ISFINITE(_thr_sp(0)) && PX4_ISFINITE(_thr_sp(1)) && PX4_ISFINITE(_thr_sp(2));
return valid;
} }
void PositionControl::_positionControl() void PositionControl::_positionControl()
@@ -204,10 +207,20 @@ void PositionControl::_accelerationControl()
_thr_sp = body_z * collective_thrust; _thr_sp = body_z * collective_thrust;
} }
bool PositionControl::_updateSuccessful() bool PositionControl::_inputValid()
{ {
bool valid = true; bool valid = true;
// Every axis x, y, z needs to have some setpoint
for (int i = 0; i <= 2; i++) {
valid = valid && (PX4_ISFINITE(_pos_sp(i)) || PX4_ISFINITE(_vel_sp(i)) || PX4_ISFINITE(_acc_sp(i)));
}
// x and y input setpoints always have to come in pairs
valid = valid && (PX4_ISFINITE(_pos_sp(0)) == PX4_ISFINITE(_pos_sp(1)));
valid = valid && (PX4_ISFINITE(_vel_sp(0)) == PX4_ISFINITE(_vel_sp(1)));
valid = valid && (PX4_ISFINITE(_acc_sp(0)) == PX4_ISFINITE(_acc_sp(1)));
// For each controlled state the estimate has to be valid // For each controlled state the estimate has to be valid
for (int i = 0; i <= 2; i++) { for (int i = 0; i <= 2; i++) {
if (PX4_ISFINITE(_pos_sp(i))) { if (PX4_ISFINITE(_pos_sp(i))) {
@@ -219,10 +232,6 @@ bool PositionControl::_updateSuccessful()
} }
} }
// There has to be a valid output accleration and thrust setpoint otherwise there was no
// setpoint-state pair for each axis that can get controlled
valid = valid && PX4_ISFINITE(_acc_sp(0)) && PX4_ISFINITE(_acc_sp(1)) && PX4_ISFINITE(_acc_sp(2));
valid = valid && PX4_ISFINITE(_thr_sp(0)) && PX4_ISFINITE(_thr_sp(1)) && PX4_ISFINITE(_thr_sp(2));
return valid; return valid;
} }
src/modules/mc_pos_control/PositionControl/PositionControl.hpp
+1 -1
View File
@@ -179,7 +179,7 @@ public:
void getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_setpoint) const; void getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_setpoint) const;
private: private:
bool _updateSuccessful(); bool _inputValid();
void _positionControl(); ///< Position proportional control void _positionControl(); ///< Position proportional control
void _velocityControl(const float dt); ///< Velocity PID control void _velocityControl(const float dt); ///< Velocity PID control