diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp index 293410702dc2..4d3787b47381 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp @@ -71,18 +71,8 @@ void PositionControl::setState(const PositionControlStates &states) _vel_dot = states.acceleration; } -void PositionControl::_setCtrlFlag(bool value) -{ - for (int i = 0; i <= 2; i++) { - _ctrl_pos[i] = _ctrl_vel[i] = value; - } -} - bool PositionControl::setInputSetpoint(const vehicle_local_position_setpoint_s &setpoint) { - // by default we use the entire position-velocity control-loop pipeline (flag only for logging purpose) - _setCtrlFlag(true); - _pos_sp = Vector3f(setpoint.x, setpoint.y, setpoint.z); _vel_sp = Vector3f(setpoint.vx, setpoint.vy, setpoint.vz); _acc_sp = Vector3f(setpoint.acceleration); @@ -180,7 +170,6 @@ bool PositionControl::_interfaceMapping() // Velocity controller is active without position control. // Set integral states and setpoints to 0 _pos_sp(i) = _pos(i) = 0.0f; - _ctrl_pos[i] = false; // position control-loop is not used // thrust setpoint is not supported in velocity control _thr_sp(i) = NAN; @@ -196,7 +185,6 @@ bool PositionControl::_interfaceMapping() // Set all integral states and setpoints to 0 _pos_sp(i) = _pos(i) = 0.0f; _vel_sp(i) = _vel(i) = 0.0f; - _ctrl_pos[i] = _ctrl_vel[i] = false; // position/velocity control loop is not used // Reset the Integral term. _vel_int(i) = 0.0f; @@ -243,7 +231,6 @@ bool PositionControl::_interfaceMapping() // 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) - _setCtrlFlag(false); } return !(failsafe); diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp b/src/modules/mc_pos_control/PositionControl/PositionControl.hpp index 1aebe52aa3ce..d6c6f76dc1a1 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.hpp @@ -158,27 +158,6 @@ class PositionControl */ void resetIntegral() { _vel_int.setZero(); } - /** - * Get the - * @see _vel_sp - * @return The velocity set-point that was executed in the control-loop. Nan if velocity control-loop was skipped. - */ - const matrix::Vector3f getVelSp() const - { - matrix::Vector3f vel_sp{}; - - for (int i = 0; i <= 2; i++) { - if (_ctrl_vel[i]) { - vel_sp(i) = _vel_sp(i); - - } else { - vel_sp(i) = NAN; - } - } - - return vel_sp; - } - /** * Get the controllers output local position setpoint * These setpoints are the ones which were executed on including PID output and feed-forward. @@ -204,7 +183,6 @@ class PositionControl void _positionControl(); ///< Position proportional control void _velocityControl(const float dt); ///< Velocity PID control - void _setCtrlFlag(bool value); /**< set control-loop flags (only required for logging) */ // Gains matrix::Vector3f _gain_pos_p; ///< Position control proportional gain @@ -240,6 +218,4 @@ class PositionControl float _yawspeed_sp{}; /** desired yaw-speed */ bool _skip_controller{false}; /**< skips position/velocity controller. true for stabilized mode */ - bool _ctrl_pos[3] = {true, true, true}; /**< True if the control-loop for position was used */ - bool _ctrl_vel[3] = {true, true, true}; /**< True if the control-loop for velocity was used */ }; diff --git a/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp b/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp index 43ff73f4c994..10c3d228c1bf 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp @@ -133,7 +133,7 @@ class PositionControlBasicDirectionTest : public PositionControlBasicTest } }; -TEST_F(PositionControlBasicDirectionTest, PositionControlDirectionP) +TEST_F(PositionControlBasicDirectionTest, PositionDirection) { _input_setpoint.x = .1f; _input_setpoint.y = .1f; @@ -142,7 +142,7 @@ TEST_F(PositionControlBasicDirectionTest, PositionControlDirectionP) checkDirection(); } -TEST_F(PositionControlBasicDirectionTest, VelocityControlDirection) +TEST_F(PositionControlBasicDirectionTest, VelocityDirection) { _input_setpoint.vx = .1f; _input_setpoint.vy = .1f; @@ -151,7 +151,7 @@ TEST_F(PositionControlBasicDirectionTest, VelocityControlDirection) checkDirection(); } -TEST_F(PositionControlBasicTest, PositionControlMaxTilt) +TEST_F(PositionControlBasicTest, TiltLimit) { _input_setpoint.x = 10.f; _input_setpoint.y = 10.f; @@ -171,7 +171,7 @@ TEST_F(PositionControlBasicTest, PositionControlMaxTilt) EXPECT_LE(angle, .50001f); } -TEST_F(PositionControlBasicTest, PositionControlMaxVelocity) +TEST_F(PositionControlBasicTest, VelocityLimit) { _input_setpoint.x = 10.f; _input_setpoint.y = 10.f; @@ -183,7 +183,7 @@ TEST_F(PositionControlBasicTest, PositionControlMaxVelocity) EXPECT_LE(abs(_output_setpoint.vz), 1.f); } -TEST_F(PositionControlBasicTest, PositionControlThrustLimit) +TEST_F(PositionControlBasicTest, ThrustLimit) { _input_setpoint.x = 10.f; _input_setpoint.y = 10.f; @@ -196,7 +196,7 @@ TEST_F(PositionControlBasicTest, PositionControlThrustLimit) EXPECT_GE(_attitude.thrust_body[2], -0.9f); } -TEST_F(PositionControlBasicTest, PositionControlFailsafeInput) +TEST_F(PositionControlBasicTest, FailsafeInput) { _input_setpoint.vz = .7f; _input_setpoint.acceleration[0] = _input_setpoint.acceleration[1] = 0.f; @@ -209,3 +209,39 @@ TEST_F(PositionControlBasicTest, PositionControlFailsafeInput) EXPECT_GT(_attitude.thrust_body[2], -.5f); EXPECT_LE(_attitude.thrust_body[2], -.1f); } + +TEST_F(PositionControlBasicTest, InputCombinationsPosition) +{ + _input_setpoint.x = .1f; + _input_setpoint.y = .2f; + _input_setpoint.z = .3f; + + runController(); + EXPECT_EQ(_output_setpoint.x, .1f); + EXPECT_EQ(_output_setpoint.y, .2f); + EXPECT_EQ(_output_setpoint.z, .3f); + EXPECT_FALSE(isnan(_output_setpoint.vx)); + EXPECT_FALSE(isnan(_output_setpoint.vy)); + EXPECT_FALSE(isnan(_output_setpoint.vz)); + EXPECT_FALSE(isnan(_output_setpoint.thrust[0])); + EXPECT_FALSE(isnan(_output_setpoint.thrust[1])); + EXPECT_FALSE(isnan(_output_setpoint.thrust[2])); +} + +TEST_F(PositionControlBasicTest, InputCombinationsPositionVelocity) +{ + _input_setpoint.vx = .1f; + _input_setpoint.vy = .2f; + _input_setpoint.z = .3f; // altitude + + runController(); + // EXPECT_TRUE(isnan(_output_setpoint.x)); + // EXPECT_TRUE(isnan(_output_setpoint.y)); + EXPECT_EQ(_output_setpoint.z, .3f); + EXPECT_EQ(_output_setpoint.vx, .1f); + EXPECT_EQ(_output_setpoint.vy, .2f); + EXPECT_FALSE(isnan(_output_setpoint.vz)); + EXPECT_FALSE(isnan(_output_setpoint.thrust[0])); + EXPECT_FALSE(isnan(_output_setpoint.thrust[1])); + EXPECT_FALSE(isnan(_output_setpoint.thrust[2])); +} 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 c9cd66320030..a601e5828cad 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -652,9 +652,6 @@ MulticopterPositionControl::Run() local_pos_sp.x = setpoint.x; local_pos_sp.y = setpoint.y; local_pos_sp.z = setpoint.z; - local_pos_sp.vx = PX4_ISFINITE(_control.getVelSp()(0)) ? _control.getVelSp()(0) : setpoint.vx; - local_pos_sp.vy = PX4_ISFINITE(_control.getVelSp()(1)) ? _control.getVelSp()(1) : setpoint.vy; - local_pos_sp.vz = PX4_ISFINITE(_control.getVelSp()(2)) ? _control.getVelSp()(2) : setpoint.vz; // Publish local position setpoint // This message will be used by other modules (such as Landdetector) to determine vehicle intention. @@ -662,7 +659,8 @@ MulticopterPositionControl::Run() // Inform FlightTask about the input and output of the velocity controller // This is used to properly initialize the velocity setpoint when onpening the position loop (position unlock) - _flight_tasks.updateVelocityControllerIO(_control.getVelSp(), Vector3f(local_pos_sp.thrust)); + _flight_tasks.updateVelocityControllerIO(Vector3f(local_pos_sp.vx, local_pos_sp.vy, local_pos_sp.vz), + Vector3f(local_pos_sp.thrust)); vehicle_attitude_setpoint_s attitude_setpoint{}; attitude_setpoint.timestamp = time_stamp_now;