diff --git a/src/modules/mc_pos_control/PositionControl/ControlMath.cpp b/src/modules/mc_pos_control/PositionControl/ControlMath.cpp index e5402d56b1..444237a30b 100644 --- a/src/modules/mc_pos_control/PositionControl/ControlMath.cpp +++ b/src/modules/mc_pos_control/PositionControl/ControlMath.cpp @@ -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; diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp index 1c9f634084..b68088f82b 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp @@ -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 { diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp b/src/modules/mc_pos_control/PositionControl/PositionControl.hpp index 07cf0b03f6..4384dfd74c 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.hpp @@ -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 */ }; diff --git a/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp b/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp index 5df1c420b0..dacca9b6ce 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp @@ -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(combination) / static_cast(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()); +} diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 10e5991e1f..11a6e83986 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -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).