Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Multicopter position control: Invalid setpoint can windup integrator #19041

Merged
merged 3 commits into from
Jan 19, 2022
Merged

Multicopter position control: Invalid setpoint can windup integrator #19041

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
68d4b1a
PositionControlTest: add integrator windup with invalid setpoint comb…
MaEtUgR Jan 19, 2022
f39488b
PosititionControl: fix integrator windup with invalid setpoint
MaEtUgR Jan 19, 2022
ad96992
MulticoperPositionController: time failsafe messages together
MaEtUgR Jan 19, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
8 changes: 4 additions & 4 deletions src/modules/mc_pos_control/MulticopterPositionControl.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -321,8 +321,6 @@ void MulticopterPositionControl::Run()

// set _dt in controllib Block for BlockDerivative
setDt(dt);

const bool was_in_failsafe = _in_failsafe;
_in_failsafe = false;

_vehicle_control_mode_sub.update(&_vehicle_control_mode);
Expand Down Expand Up @@ -478,14 +476,16 @@ void MulticopterPositionControl::Run()

} else {
// Failsafe
if ((time_stamp_now - _last_warn) > 2_s) {
const bool warn_failsafe = (time_stamp_now - _last_warn) > 2_s;

if (warn_failsafe) {
PX4_WARN("invalid setpoints");
_last_warn = time_stamp_now;
}

vehicle_local_position_setpoint_s failsafe_setpoint{};

failsafe(time_stamp_now, failsafe_setpoint, states, !was_in_failsafe);
failsafe(time_stamp_now, failsafe_setpoint, states, warn_failsafe);

// reset constraints
_vehicle_constraints = {0, NAN, NAN, false, {}};
Expand Down
37 changes: 23 additions & 14 deletions src/modules/mc_pos_control/PositionControl/PositionControl.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -104,18 +104,21 @@ void PositionControl::setInputSetpoint(const vehicle_local_position_setpoint_s &

bool PositionControl::update(const float dt)
{
// x and y input setpoints always have to come in pairs
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)));
bool valid = _inputValid();

_positionControl();
_velocityControl(dt);
if (valid) {
_positionControl();
_velocityControl(dt);

_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
_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
}

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()
Expand Down Expand Up @@ -204,10 +207,20 @@ void PositionControl::_accelerationControl()
_thr_sp = body_z * collective_thrust;
}

bool PositionControl::_updateSuccessful()
bool PositionControl::_inputValid()
{
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 (int i = 0; i <= 2; i++) {
if (PX4_ISFINITE(_pos_sp(i))) {
Expand All @@ -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;
}

Expand Down
2 changes: 1 addition & 1 deletion src/modules/mc_pos_control/PositionControl/PositionControl.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -179,7 +179,7 @@ class PositionControl
void getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_setpoint) const;

private:
bool _updateSuccessful();
bool _inputValid();

void _positionControl(); ///< Position proportional control
void _velocityControl(const float dt); ///< Velocity PID control
Expand Down
20 changes: 20 additions & 0 deletions src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -399,3 +399,23 @@ TEST_F(PositionControlBasicTest, UpdateHoverThrust)
// the output is still the same
EXPECT_EQ(_output_setpoint.thrust[2], -hover_thrust);
}

TEST_F(PositionControlBasicTest, IntegratorWindupWithInvalidSetpoint)
{
// GIVEN: the controller was ran with an invalid setpoint containing some valid values
_input_setpoint.x = .1f;
_input_setpoint.y = .2f;
// all z-axis setpoints stay NAN
EXPECT_FALSE(runController());

// WHEN: we run the controller with a valid setpoint
resetInputSetpoint();
_input_setpoint.vx = 0.f;
_input_setpoint.vy = 0.f;
_input_setpoint.vz = 0.f;
EXPECT_TRUE(runController());

// THEN: the integral did not wind up and produce unexpected deviation
EXPECT_FLOAT_EQ(_attitude.roll_body, 0.f);
EXPECT_FLOAT_EQ(_attitude.pitch_body, 0.f);
}