dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

PositionControl: replace interfacemapping checks 

Removing the skip_controller and interfaceMapping
concept and replacing it with a single method checking
if the position control update was successful and
return the result in the PositionControl.update().
This commit is contained in:
Matthias Grob 2020-01-23 18:23:05 +01:00 committed by Kabir Mohammed
parent e06fff94bb
commit b75c1308f9
5 changed files with 126 additions and 166 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/modules/mc_pos_control/PositionControl/ControlMath.cpp
View File

@ -159,7 +159,6 @@ Vector2f constrainXY(const Vector2f &v0, const Vector2f &v1, const float &max)
// notes:
// - s (=scaling factor) needs to be positive
// - (max - ||v||) always larger than zero, otherwise it never entered this if-statement
Vector2f u1 = v1.normalized();
float m = u1.dot(v0);
float c = v0.dot(v0) - max * max;

154
src/modules/mc_pos_control/PositionControl/PositionControl.cpp
View File

@ -71,7 +71,7 @@ void PositionControl::setState(const PositionControlStates &states)
_vel_dot = states.acceleration;
}
bool PositionControl::setInputSetpoint(const vehicle_local_position_setpoint_s &setpoint)
void PositionControl::setInputSetpoint(const vehicle_local_position_setpoint_s &setpoint)
{
_pos_sp = Vector3f(setpoint.x, setpoint.y, setpoint.z);
_vel_sp = Vector3f(setpoint.vx, setpoint.vy, setpoint.vz);
@ -79,14 +79,6 @@ bool PositionControl::setInputSetpoint(const vehicle_local_position_setpoint_s &
_thr_sp = Vector3f(setpoint.thrust);
_yaw_sp = setpoint.yaw;
_yawspeed_sp = setpoint.yawspeed;
bool mapping_succeeded = _interfaceMapping();
// If full manual is required (thrust already generated), don't run position/velocity
// controller and just return thrust.
_skip_controller = PX4_ISFINITE(_thr_sp(0)) && PX4_ISFINITE(_thr_sp(1))
&& PX4_ISFINITE(_thr_sp(2));
return mapping_succeeded;
}
void PositionControl::setConstraints(const vehicle_constraints_s &constraints)
@ -110,132 +102,20 @@ void PositionControl::setConstraints(const vehicle_constraints_s &constraints)
// ignore _constraints.speed_xy TODO: remove it completely as soon as no task uses it anymore to avoid confusion
}
void PositionControl::update(const float dt)
bool PositionControl::update(const float dt)
{
if (_skip_controller) {
// Already received a valid thrust set-point.
// Limit the thrust vector.
float thr_mag = _thr_sp.length();
if (thr_mag > _lim_thr_max) {
_thr_sp = _thr_sp.normalized() * _lim_thr_max;
} else if (thr_mag < _lim_thr_min && thr_mag > FLT_EPSILON) {
_thr_sp = _thr_sp.normalized() * _lim_thr_min;
}
// Just set the set-points equal to the current vehicle state.
_pos_sp = _pos;
_vel_sp = _vel;
_acc_sp = _vel_dot;
return;
}
// 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(_thr_sp(0)) == PX4_ISFINITE(_thr_sp(1)));
_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
}
bool PositionControl::_interfaceMapping()
{
// if nothing is valid, then apply failsafe landing
bool failsafe = false;
// Respects FlightTask interface, where NAN-set-points are of no interest
// and do not require control. A valid position and velocity setpoint will
// be mapped to a desired position setpoint with a feed-forward term.
// States and setpoints which are integrals of the reference setpoint are set to 0.
// For instance: reference is velocity-setpoint -> position and position-setpoint = 0
// reference is thrust-setpoint -> position, velocity, position-/velocity-setpoint = 0
for (int i = 0; i <= 2; i++) {
if (PX4_ISFINITE(_pos_sp(i))) {
// Position control is required
if (!PX4_ISFINITE(_vel_sp(i))) {
// Velocity is not used as feedforward term.
_vel_sp(i) = 0.0f;
}
// thrust setpoint is not supported in position control
_thr_sp(i) = NAN;
// to run position control, we require valid position and velocity
if (!PX4_ISFINITE(_pos(i)) || !PX4_ISFINITE(_vel(i))) {
failsafe = true;
}
} else if (PX4_ISFINITE(_vel_sp(i))) {
// Velocity controller is active without position control.
// Set integral states and setpoints to 0
_pos_sp(i) = _pos(i) = 0.0f;
// thrust setpoint is not supported in velocity control
_thr_sp(i) = NAN;
// to run velocity control, we require valid velocity
if (!PX4_ISFINITE(_vel(i))) {
failsafe = true;
}
} else if (PX4_ISFINITE(_thr_sp(i))) {
// Thrust setpoint was generated from sticks directly.
// Set all integral states and setpoints to 0
_pos_sp(i) = _pos(i) = 0.0f;
_vel_sp(i) = _vel(i) = 0.0f;
// Reset the Integral term.
_vel_int(i) = 0.0f;
// Don't require velocity derivative.
_vel_dot(i) = 0.0f;
} else {
// nothing is valid. do failsafe
failsafe = true;
}
}
// ensure that vel_dot is finite, otherwise set to 0
if (!PX4_ISFINITE(_vel_dot(0)) || !PX4_ISFINITE(_vel_dot(1))) {
_vel_dot(0) = _vel_dot(1) = 0.0f;
}
if (!PX4_ISFINITE(_vel_dot(2))) {
_vel_dot(2) = 0.0f;
}
if (!PX4_ISFINITE(_yawspeed_sp)) {
// Set the yawspeed to 0 since not used.
_yawspeed_sp = 0.0f;
}
if (!PX4_ISFINITE(_yaw_sp)) {
// Set the yaw-sp equal the current yaw.
// That is the best we can do and it also
// agrees with FlightTask-interface definition.
if (PX4_ISFINITE(_yaw)) {
_yaw_sp = _yaw;
} else {
failsafe = true;
}
}
// check failsafe
if (failsafe) {
// point the thrust upwards
_thr_sp(0) = _thr_sp(1) = 0.0f;
// throttle down such that vehicle goes down with
// 70% of throttle range between min and hover
_thr_sp(2) = -(_lim_thr_min + (_hover_thrust - _lim_thr_min) * 0.7f);
// position and velocity control-loop is currently unused (flag only for logging purpose)
}
return !(failsafe);
return valid && _updateSuccessful();
}
void PositionControl::_positionControl()
@ -336,6 +216,26 @@ void PositionControl::_velocityControl(const float dt)
_vel_int(2) = math::min(fabsf(_vel_int(2)), _lim_thr_max) * math::sign(_vel_int(2));
}
bool PositionControl::_updateSuccessful()
{
bool valid = true;
// For each controlled state the estimate has to be valid
for (int i = 0; i <= 2; i++) {
if (PX4_ISFINITE(_pos_sp(i))) {
valid = valid && PX4_ISFINITE(_pos(i));
}
if (PX4_ISFINITE(_vel_sp(i))) {
valid = valid && PX4_ISFINITE(_vel(i)) && PX4_ISFINITE(_vel_dot(i));
}
}
// There has to be a valid output thrust setpoint otherwise there was no
// setpoint-state pair for each axis that can get controlled
valid = valid && PX4_ISFINITE(_thr_sp(0)) && PX4_ISFINITE(_thr_sp(1)) && PX4_ISFINITE(_thr_sp(2));
return valid;
}
void PositionControl::getLocalPositionSetpoint(vehicle_local_position_setpoint_s &local_position_setpoint) const
{

15
src/modules/mc_pos_control/PositionControl/PositionControl.hpp
View File

@ -129,9 +129,8 @@ public:
* Pass the desired setpoints
* Note: NAN value means no feed forward/leave state uncontrolled if there's no higher order setpoint.
* @param setpoint a vehicle_local_position_setpoint_s structure
* @return true if a valid setpoint was set
*/
bool setInputSetpoint(const vehicle_local_position_setpoint_s &setpoint);
void setInputSetpoint(const vehicle_local_position_setpoint_s &setpoint);
/**
* Pass constraints that are stricter than the global limits
@ -147,9 +146,9 @@ public:
* @see _yaw_sp
* @see _yawspeed_sp
* @param dt time in seconds since last iteration
* @return true if output setpoint is executable, false if not
* @return true if update succeeded and output setpoint is executable, false if not
*/
void update(const float dt);
bool update(const float dt);
/**
* Set the integral term in xy to 0.
@ -174,11 +173,7 @@ public:
void getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_setpoint) const;
private:
/**
* Maps setpoints to internal-setpoints.
* @return true if mapping succeeded.
*/
bool _interfaceMapping();
bool _updateSuccessful();
void _positionControl(); ///< Position proportional control
void _velocityControl(const float dt); ///< Velocity PID control
@ -215,6 +210,4 @@ private:
matrix::Vector3f _thr_sp; /**< desired thrust */
float _yaw_sp{}; /**< desired heading */
float _yawspeed_sp{}; /** desired yaw-speed */
bool _skip_controller{false}; /**< skips position/velocity controller. true for stabilized mode */
};

103
src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp
View File

@ -88,6 +88,11 @@ public:
_contraints.speed_up = NAN;
_contraints.speed_down = NAN;
resetInputSetpoint();
}
void resetInputSetpoint()
{
_input_setpoint.x = NAN;
_input_setpoint.y = NAN;
_input_setpoint.z = NAN;
@ -100,13 +105,14 @@ public:
Vector3f(NAN, NAN, NAN).copyTo(_input_setpoint.thrust);
}
void runController()
bool runController()
{
_position_control.setConstraints(_contraints);
_position_control.setInputSetpoint(_input_setpoint);
_position_control.update(.1f);
const bool ret = _position_control.update(.1f);
_position_control.getLocalPositionSetpoint(_output_setpoint);
_position_control.getAttitudeSetpoint(_attitude);
return ret;
}
PositionControl _position_control;
@ -138,7 +144,7 @@ TEST_F(PositionControlBasicDirectionTest, PositionDirection)
_input_setpoint.x = .1f;
_input_setpoint.y = .1f;
_input_setpoint.z = -.1f;
runController();
EXPECT_TRUE(runController());
checkDirection();
}
@ -147,7 +153,7 @@ TEST_F(PositionControlBasicDirectionTest, VelocityDirection)
_input_setpoint.vx = .1f;
_input_setpoint.vy = .1f;
_input_setpoint.vz = -.1f;
runController();
EXPECT_TRUE(runController());
checkDirection();
}
@ -157,14 +163,14 @@ TEST_F(PositionControlBasicTest, TiltLimit)
_input_setpoint.y = 10.f;
_input_setpoint.z = -0.f;
runController();
EXPECT_TRUE(runController());
Vector3f body_z = Quatf(_attitude.q_d).dcm_z();
float angle = acosf(body_z.dot(Vector3f(0.f, 0.f, 1.f)));
EXPECT_GT(angle, 0.f);
EXPECT_LE(angle, 1.f);
_contraints.tilt = .5f;
runController();
EXPECT_TRUE(runController());
body_z = Quatf(_attitude.q_d).dcm_z();
angle = acosf(body_z.dot(Vector3f(0.f, 0.f, 1.f)));
EXPECT_GT(angle, 0.f);
@ -177,7 +183,7 @@ TEST_F(PositionControlBasicTest, VelocityLimit)
_input_setpoint.y = 10.f;
_input_setpoint.z = -10.f;
runController();
EXPECT_TRUE(runController());
Vector2f velocity_xy(_output_setpoint.vx, _output_setpoint.vy);
EXPECT_LE(velocity_xy.norm(), 1.f);
EXPECT_LE(abs(_output_setpoint.vz), 1.f);
@ -189,7 +195,7 @@ TEST_F(PositionControlBasicTest, ThrustLimit)
_input_setpoint.y = 10.f;
_input_setpoint.z = -10.f;
runController();
EXPECT_TRUE(runController());
EXPECT_FLOAT_EQ(_attitude.thrust_body[0], 0.f);
EXPECT_FLOAT_EQ(_attitude.thrust_body[1], 0.f);
EXPECT_LT(_attitude.thrust_body[2], -.1f);
@ -202,7 +208,7 @@ TEST_F(PositionControlBasicTest, FailsafeInput)
_input_setpoint.thrust[0] = _input_setpoint.thrust[1] = 0.f;
_input_setpoint.acceleration[0] = _input_setpoint.acceleration[1] = 0.f;
runController();
EXPECT_TRUE(runController());
EXPECT_FLOAT_EQ(_attitude.thrust_body[0], 0.f);
EXPECT_FLOAT_EQ(_attitude.thrust_body[1], 0.f);
EXPECT_LT(_output_setpoint.thrust[2], -.1f);
@ -217,7 +223,7 @@ TEST_F(PositionControlBasicTest, InputCombinationsPosition)
_input_setpoint.y = .2f;
_input_setpoint.z = .3f;
runController();
EXPECT_TRUE(runController());
EXPECT_FLOAT_EQ(_output_setpoint.x, .1f);
EXPECT_FLOAT_EQ(_output_setpoint.y, .2f);
EXPECT_FLOAT_EQ(_output_setpoint.z, .3f);
@ -235,7 +241,7 @@ TEST_F(PositionControlBasicTest, InputCombinationsPositionVelocity)
_input_setpoint.vy = .2f;
_input_setpoint.z = .3f; // altitude
runController();
EXPECT_TRUE(runController());
// EXPECT_TRUE(isnan(_output_setpoint.x));
// EXPECT_TRUE(isnan(_output_setpoint.y));
EXPECT_FLOAT_EQ(_output_setpoint.z, .3f);
@ -246,3 +252,78 @@ TEST_F(PositionControlBasicTest, InputCombinationsPositionVelocity)
EXPECT_FALSE(isnan(_output_setpoint.thrust[1]));
EXPECT_FALSE(isnan(_output_setpoint.thrust[2]));
}
TEST_F(PositionControlBasicTest, SetpointValiditySimple)
{
EXPECT_FALSE(runController());
_input_setpoint.x = .1f;
EXPECT_FALSE(runController());
_input_setpoint.y = .2f;
EXPECT_FALSE(runController());
_input_setpoint.thrust[2] = .3f;
EXPECT_TRUE(runController());
}
TEST_F(PositionControlBasicTest, SetpointValidityAllCombinations)
{
// This test runs any position, velocity, thrust setpoint combination and checks if it gets accepted or rejected correctly
float *const setpoint_loop_access_map[] = {&_input_setpoint.x, &_input_setpoint.vx, &_input_setpoint.thrust[0],
&_input_setpoint.y, &_input_setpoint.vy, &_input_setpoint.thrust[1],
&_input_setpoint.z, &_input_setpoint.vz, &_input_setpoint.thrust[2]
};
for (int combination = 0; combination < 512; combination++) {
resetInputSetpoint();
for (int j = 0; j < 9; j++) {
if (combination & (1 << j)) {
// Set arbitrary finite value, some values clearly hit the limits to check these corner case combinations
*(setpoint_loop_access_map[j]) = static_cast<float>(combination) / static_cast<float>(j + 1);
}
}
// Expect at least one setpoint per axis
const bool has_x_setpoint = ((combination & 7) != 0);
const bool has_y_setpoint = (((combination >> 3) & 7) != 0);
const bool has_z_setpoint = (((combination >> 6) & 7) != 0);
// Expect xy setpoints to come in pairs
const bool has_xy_pairs = (combination & 7) == ((combination >> 3) & 7);
const bool expected_result = has_x_setpoint && has_y_setpoint && has_z_setpoint && has_xy_pairs;
EXPECT_EQ(runController(), expected_result) << "combination " << combination
<< std::endl << "input" << std::endl
<< "position " << _input_setpoint.x << ", " << _input_setpoint.y << ", " << _input_setpoint.z << std::endl
<< "velocity " << _input_setpoint.vx << ", " << _input_setpoint.vy << ", " << _input_setpoint.vz << std::endl
<< "thrust " << _input_setpoint.thrust[0] << ", " << _input_setpoint.thrust[1] << ", " << _input_setpoint.thrust[2]
<< std::endl << "output" << std::endl
<< "position " << _output_setpoint.x << ", " << _output_setpoint.y << ", " << _output_setpoint.z << std::endl
<< "velocity " << _output_setpoint.vx << ", " << _output_setpoint.vy << ", " << _output_setpoint.vz << std::endl
<< "thrust " << _output_setpoint.thrust[0] << ", " << _output_setpoint.thrust[1] << ", " << _output_setpoint.thrust[2]
<< std::endl;
}
}
TEST_F(PositionControlBasicTest, InvalidState)
{
_input_setpoint.x = .1f;
_input_setpoint.y = .2f;
_input_setpoint.z = .3f;
PositionControlStates states{};
states.position(0) = NAN;
_position_control.setState(states);
EXPECT_FALSE(runController());
states.velocity(0) = NAN;
_position_control.setState(states);
EXPECT_FALSE(runController());
states.position(0) = 0.f;
_position_control.setState(states);
EXPECT_FALSE(runController());
states.velocity(0) = 0.f;
states.acceleration(1) = NAN;
_position_control.setState(states);
EXPECT_FALSE(runController());
}

19
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -563,19 +563,6 @@ MulticopterPositionControl::Run()
constraints = _flight_tasks.getConstraints();
_failsafe_land_hysteresis.set_state_and_update(false, time_stamp_now);
// Check if position, velocity or thrust pairs are valid -> trigger failsaife if no pair is valid
if (!(PX4_ISFINITE(setpoint.x) && PX4_ISFINITE(setpoint.y)) &&
!(PX4_ISFINITE(setpoint.vx) && PX4_ISFINITE(setpoint.vy)) &&
!(PX4_ISFINITE(setpoint.thrust[0]) && PX4_ISFINITE(setpoint.thrust[1]))) {
failsafe(setpoint, _states, true, !was_in_failsafe);
}
// Check if altitude, climbrate or thrust in D-direction are valid -> trigger failsafe if none
// of these setpoints are valid
if (!PX4_ISFINITE(setpoint.z) && !PX4_ISFINITE(setpoint.vz) && !PX4_ISFINITE(setpoint.thrust[2])) {
failsafe(setpoint, _states, true, !was_in_failsafe);
}
}
// publish trajectory setpoint
@ -617,16 +604,16 @@ MulticopterPositionControl::Run()
// Run position control
_control.setState(_states);
_control.setConstraints(constraints);
_control.setInputSetpoint(setpoint);
if (!_control.setInputSetpoint(setpoint)) {
if (!_control.update(_dt)) {
warn_rate_limited("PositionControl: invalid setpoints");
failsafe(setpoint, _states, true, !was_in_failsafe);
_control.setInputSetpoint(setpoint);
constraints = FlightTask::empty_constraints;
_control.update(_dt);
}
_control.update(_dt);
// Fill local position, velocity and thrust setpoint.
// This message contains setpoints where each type of setpoint is either the input to the PositionController
// or was generated by the PositionController and therefore corresponds to the PositioControl internal states (states that were generated by P-PID).