diff --git a/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturationTest.cpp b/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturationTest.cpp index 2a67131074..dcc40194b3 100644 --- a/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturationTest.cpp +++ b/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturationTest.cpp @@ -65,6 +65,7 @@ public: class ControlAllocationSequentialDesaturationTestQuadX : public ::testing::Test { public: + static constexpr uint8_t NUM_ACTUATORS = 4; TestControlAllocationSequentialDesaturation _control_allocation; void SetUp() override @@ -94,6 +95,7 @@ public: int num_actuators = ActuatorEffectivenessRotors::computeEffectivenessMatrix(quadx_geometry, actuator_configuration.effectiveness_matrices[0], actuator_configuration.num_actuators_matrix[0]); + EXPECT_EQ(num_actuators, NUM_ACTUATORS); actuator_configuration.actuatorsAdded(ActuatorType::MOTORS, num_actuators); // Load effectiveness into allocation @@ -134,6 +136,9 @@ public: } }; +// Make constant available, see https://stackoverflow.com/questions/42756443/undefined-reference-with-gtest +constexpr uint8_t ControlAllocationSequentialDesaturationTestQuadX::NUM_ACTUATORS; + TEST_F(ControlAllocationSequentialDesaturationTestQuadX, Zero) { EXPECT_EQ(allocate(0.f, 0.f, 0.f, 0.f), Vector4f()); @@ -207,342 +212,71 @@ TEST_F(ControlAllocationSequentialDesaturationTestQuadX, RollPitchYawZeroThrustA EXPECT_EQ(allocate(0.f, 0.f, -1.f, 0.f), Vector4f(0.f, .5f, 0.f, .5f)); } -namespace -{ - -// Makes and returns a Geometry object for a "standard" quad-x quadcopter. -ActuatorEffectivenessRotors::Geometry make_quad_x_geometry() -{ - ActuatorEffectivenessRotors::Geometry geometry = {}; - geometry.rotors[0].position(0) = 1.0f; - geometry.rotors[0].position(1) = 1.0f; - geometry.rotors[0].position(2) = 0.0f; - geometry.rotors[0].axis(0) = 0.0f; - geometry.rotors[0].axis(1) = 0.0f; - geometry.rotors[0].axis(2) = -1.0f; - geometry.rotors[0].thrust_coef = 1.0f; - geometry.rotors[0].moment_ratio = 0.05f; - - geometry.rotors[1].position(0) = -1.0f; - geometry.rotors[1].position(1) = -1.0f; - geometry.rotors[1].position(2) = 0.0f; - geometry.rotors[1].axis(0) = 0.0f; - geometry.rotors[1].axis(1) = 0.0f; - geometry.rotors[1].axis(2) = -1.0f; - geometry.rotors[1].thrust_coef = 1.0f; - geometry.rotors[1].moment_ratio = 0.05f; - - geometry.rotors[2].position(0) = 1.0f; - geometry.rotors[2].position(1) = -1.0f; - geometry.rotors[2].position(2) = 0.0f; - geometry.rotors[2].axis(0) = 0.0f; - geometry.rotors[2].axis(1) = 0.0f; - geometry.rotors[2].axis(2) = -1.0f; - geometry.rotors[2].thrust_coef = 1.0f; - geometry.rotors[2].moment_ratio = -0.05f; - - geometry.rotors[3].position(0) = -1.0f; - geometry.rotors[3].position(1) = 1.0f; - geometry.rotors[3].position(2) = 0.0f; - geometry.rotors[3].axis(0) = 0.0f; - geometry.rotors[3].axis(1) = 0.0f; - geometry.rotors[3].axis(2) = -1.0f; - geometry.rotors[3].thrust_coef = 1.0f; - geometry.rotors[3].moment_ratio = -0.05f; - - geometry.num_rotors = 4; - - return geometry; -} - -// Returns an effective matrix for a sample quad-copter configuration. -ActuatorEffectiveness::EffectivenessMatrix make_quad_x_effectiveness() -{ - ActuatorEffectiveness::EffectivenessMatrix effectiveness; - effectiveness.setZero(); - const auto geometry = make_quad_x_geometry(); - ActuatorEffectivenessRotors::computeEffectivenessMatrix(geometry, effectiveness); - return effectiveness; -} - -// Configures a ControlAllocationSequentialDesaturation object for a sample quad-copter. -void setup_quad_allocator(ControlAllocationSequentialDesaturation &allocator) -{ - const auto effectiveness = make_quad_x_effectiveness(); - matrix::Vector actuator_trim; - matrix::Vector linearization_point; - constexpr bool UPDATE_NORMALIZATION_SCALE{false}; - allocator.setEffectivenessMatrix( - effectiveness, - actuator_trim, - linearization_point, - ActuatorEffectiveness::NUM_ACTUATORS, - UPDATE_NORMALIZATION_SCALE - ); -} - -static constexpr float EXPECT_NEAR_TOL{1e-4f}; - -} // namespace - // This tests that yaw-only control setpoint at zero actuator setpoint results in zero actuator // allocation. -TEST(ControlAllocationSequentialDesaturationTest, AirmodeDisabledOnlyYaw) +TEST_F(ControlAllocationSequentialDesaturationTestQuadX, AirmodeDisabledOnlyYaw) { - ControlAllocationSequentialDesaturation allocator; - setup_quad_allocator(allocator); - matrix::Vector control_sp; - control_sp(ControlAllocation::ControlAxis::ROLL) = 0.f; - control_sp(ControlAllocation::ControlAxis::PITCH) = 0.f; - control_sp(ControlAllocation::ControlAxis::YAW) = 1.f; - control_sp(ControlAllocation::ControlAxis::THRUST_X) = 0.f; - control_sp(ControlAllocation::ControlAxis::THRUST_Y) = 0.f; - control_sp(ControlAllocation::ControlAxis::THRUST_Z) = 0.f; - allocator.setControlSetpoint(control_sp); - - // Since MC_AIRMODE was not set explicitly, assume airmode is disabled. - allocator.allocate(); - - const auto &actuator_sp = allocator.getActuatorSetpoint(); - matrix::Vector zero; - EXPECT_EQ(actuator_sp, zero); + setAirmode(0); // Airmode disabled + EXPECT_EQ(allocate(0.f, 0.f, 1.f, 0.f), Vector4f(0.f, 0.f, 0.f, 0.f)); } // This tests that a control setpoint for z-thrust returns the desired actuator setpoint. // Each motor should have an actuator setpoint that when summed together should be equal to // control setpoint. -TEST(ControlAllocationSequentialDesaturationTest, AirmodeDisabledThrustZ) +TEST_F(ControlAllocationSequentialDesaturationTestQuadX, AirmodeDisabledThrustZ) { - ControlAllocationSequentialDesaturation allocator; - setup_quad_allocator(allocator); - matrix::Vector control_sp; - // Negative, because +z is "downward". - constexpr float THRUST_Z_TOTAL{-0.75f}; - control_sp(ControlAllocation::ControlAxis::ROLL) = 0.f; - control_sp(ControlAllocation::ControlAxis::PITCH) = 0.f; - control_sp(ControlAllocation::ControlAxis::YAW) = 0.f; - control_sp(ControlAllocation::ControlAxis::THRUST_X) = 0.f; - control_sp(ControlAllocation::ControlAxis::THRUST_Y) = 0.f; - control_sp(ControlAllocation::ControlAxis::THRUST_Z) = THRUST_Z_TOTAL; - allocator.setControlSetpoint(control_sp); - - // Since MC_AIRMODE was not set explicitly, assume airmode is disabled. - allocator.allocate(); - - const auto &actuator_sp = allocator.getActuatorSetpoint(); - constexpr int MOTOR_COUNT{4}; - constexpr float THRUST_Z_PER_MOTOR{-THRUST_Z_TOTAL / MOTOR_COUNT}; - - for (int i{0}; i < MOTOR_COUNT; ++i) { - EXPECT_NEAR(actuator_sp(i), THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); - } - - for (int i{MOTOR_COUNT}; i < ActuatorEffectiveness::NUM_ACTUATORS; ++i) { - EXPECT_NEAR(actuator_sp(i), 0.f, EXPECT_NEAR_TOL); - } + setAirmode(0); // Airmode disabled + constexpr float THRUST = 0.75f; + EXPECT_EQ(allocate(0.f, 0.f, 0.f, -THRUST), Vector4f(THRUST, THRUST, THRUST, THRUST)); } // This tests that a control setpoint for z-thrust + yaw returns the desired actuator setpoint. // This test does not saturate the yaw response. -TEST(ControlAllocationSequentialDesaturationTest, AirmodeDisabledThrustAndYaw) +TEST_F(ControlAllocationSequentialDesaturationTestQuadX, AirmodeDisabledThrustAndYaw) { - ControlAllocationSequentialDesaturation allocator; - setup_quad_allocator(allocator); - matrix::Vector control_sp; - // Negative, because +z is "downward". - constexpr float THRUST_Z_TOTAL{-0.75f}; - // This is low enough to not saturate the motors. - constexpr float YAW_CONTROL_SP{0.02f}; - control_sp(ControlAllocation::ControlAxis::ROLL) = 0.f; - control_sp(ControlAllocation::ControlAxis::PITCH) = 0.f; - control_sp(ControlAllocation::ControlAxis::YAW) = YAW_CONTROL_SP; - control_sp(ControlAllocation::ControlAxis::THRUST_X) = 0.f; - control_sp(ControlAllocation::ControlAxis::THRUST_Y) = 0.f; - control_sp(ControlAllocation::ControlAxis::THRUST_Z) = THRUST_Z_TOTAL; - allocator.setControlSetpoint(control_sp); - - // Since MC_AIRMODE was not set explicitly, assume airmode is disabled. - allocator.allocate(); - - const auto &actuator_sp = allocator.getActuatorSetpoint(); - // This value is based off of the effectiveness matrix. If the effectiveness matrix is changed, - // this will need to be changed. - constexpr float YAW_EFFECTIVENESS_FACTOR{5.f}; - constexpr float YAW_DIFF_PER_MOTOR{YAW_CONTROL_SP * YAW_EFFECTIVENESS_FACTOR}; - // At yaw condition, there will be 2 different actuator values. - constexpr int MOTOR_COUNT{4}; - constexpr float HIGH_THRUST_Z_PER_MOTOR{-THRUST_Z_TOTAL / MOTOR_COUNT + YAW_DIFF_PER_MOTOR}; - constexpr float LOW_THRUST_Z_PER_MOTOR{-THRUST_Z_TOTAL / MOTOR_COUNT - YAW_DIFF_PER_MOTOR}; - - for (int i{0}; i < MOTOR_COUNT / 2; ++i) { - EXPECT_NEAR(actuator_sp(i), HIGH_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); - } - - for (int i{MOTOR_COUNT / 2}; i < MOTOR_COUNT; ++i) { - EXPECT_NEAR(actuator_sp(i), LOW_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); - } - - for (int i{MOTOR_COUNT}; i < ActuatorEffectiveness::NUM_ACTUATORS; ++i) { - EXPECT_NEAR(actuator_sp(i), 0.f, EXPECT_NEAR_TOL); - } + setAirmode(0); // Airmode disabled + constexpr float THRUST = 0.75f; + constexpr float YAW_TORQUE = 0.02f; + constexpr float YAW = YAW_TORQUE / NUM_ACTUATORS; + EXPECT_EQ(allocate(0.f, 0.f, YAW_TORQUE, -THRUST), Vector4f(THRUST + YAW, THRUST - YAW, THRUST + YAW, THRUST - YAW)); } // This tests that a control setpoint for z-thrust + yaw returns the desired actuator setpoint. // This test saturates the yaw response, but does not reduce total thrust. -TEST(ControlAllocationSequentialDesaturationTest, AirmodeDisabledThrustAndSaturatedYaw) +TEST_F(ControlAllocationSequentialDesaturationTestQuadX, AirmodeDisabledThrustAndSaturatedYaw) { - ControlAllocationSequentialDesaturation allocator; - setup_quad_allocator(allocator); - matrix::Vector control_sp; - // Negative, because +z is "downward". - constexpr float THRUST_Z_TOTAL{-0.75f}; - // This is arbitrarily high to trigger strongest possible (saturated) yaw response. - constexpr float YAW_CONTROL_SP{0.25f}; - control_sp(ControlAllocation::ControlAxis::ROLL) = 0.f; - control_sp(ControlAllocation::ControlAxis::PITCH) = 0.f; - control_sp(ControlAllocation::ControlAxis::YAW) = YAW_CONTROL_SP; - control_sp(ControlAllocation::ControlAxis::THRUST_X) = 0.f; - control_sp(ControlAllocation::ControlAxis::THRUST_Y) = 0.f; - control_sp(ControlAllocation::ControlAxis::THRUST_Z) = THRUST_Z_TOTAL; - allocator.setControlSetpoint(control_sp); - - // Since MC_AIRMODE was not set explicitly, assume airmode is disabled. - allocator.allocate(); - - const auto &actuator_sp = allocator.getActuatorSetpoint(); - // At max yaw, only 2 motors will carry all of the thrust. - constexpr int YAW_MOTORS{2}; - constexpr float THRUST_Z_PER_MOTOR{-THRUST_Z_TOTAL / YAW_MOTORS}; - - for (int i{0}; i < YAW_MOTORS; ++i) { - EXPECT_NEAR(actuator_sp(i), THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); - } - - for (int i{YAW_MOTORS}; i < ActuatorEffectiveness::NUM_ACTUATORS; ++i) { - EXPECT_NEAR(actuator_sp(i), 0.f, EXPECT_NEAR_TOL); - } + setAirmode(0); // Airmode disabled + constexpr float THRUST = 0.75f; + constexpr float YAW_TORQUE = 1.f; + constexpr float YAW = YAW_TORQUE / NUM_ACTUATORS; + EXPECT_EQ(allocate(0.f, 0.f, YAW_TORQUE, -THRUST), Vector4f(THRUST + YAW, THRUST - YAW, THRUST + YAW, THRUST - YAW)); } // This tests that a control setpoint for z-thrust + pitch returns the desired actuator setpoint. // This test does not saturate the pitch response. -TEST(ControlAllocationSequentialDesaturationTest, AirmodeDisabledThrustAndPitch) +TEST_F(ControlAllocationSequentialDesaturationTestQuadX, AirmodeDisabledThrustAndPitch) { - ControlAllocationSequentialDesaturation allocator; - setup_quad_allocator(allocator); - matrix::Vector control_sp; - // Negative, because +z is "downward". - constexpr float THRUST_Z_TOTAL{-0.75f}; - // This is low enough to not saturate the motors. - constexpr float PITCH_CONTROL_SP{0.1f}; - control_sp(ControlAllocation::ControlAxis::ROLL) = 0.f; - control_sp(ControlAllocation::ControlAxis::PITCH) = PITCH_CONTROL_SP; - control_sp(ControlAllocation::ControlAxis::YAW) = 0.f; - control_sp(ControlAllocation::ControlAxis::THRUST_X) = 0.f; - control_sp(ControlAllocation::ControlAxis::THRUST_Y) = 0.f; - control_sp(ControlAllocation::ControlAxis::THRUST_Z) = THRUST_Z_TOTAL; - allocator.setControlSetpoint(control_sp); - - // Since MC_AIRMODE was not set explicitly, assume airmode is disabled. - allocator.allocate(); - - const auto &actuator_sp = allocator.getActuatorSetpoint(); - // This value is based off of the effectiveness matrix. If the effectiveness matrix is changed, - // this will need to be changed. - constexpr int MOTOR_COUNT{4}; - constexpr float PITCH_DIFF_PER_MOTOR{PITCH_CONTROL_SP / MOTOR_COUNT}; - // At control set point, there will be 2 different actuator values. - constexpr float HIGH_THRUST_Z_PER_MOTOR{-THRUST_Z_TOTAL / MOTOR_COUNT + PITCH_DIFF_PER_MOTOR}; - constexpr float LOW_THRUST_Z_PER_MOTOR{-THRUST_Z_TOTAL / MOTOR_COUNT - PITCH_DIFF_PER_MOTOR}; - EXPECT_NEAR(actuator_sp(0), HIGH_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); - EXPECT_NEAR(actuator_sp(1), LOW_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); - EXPECT_NEAR(actuator_sp(2), HIGH_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); - EXPECT_NEAR(actuator_sp(3), LOW_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); - - for (int i{MOTOR_COUNT}; i < ActuatorEffectiveness::NUM_ACTUATORS; ++i) { - EXPECT_NEAR(actuator_sp(i), 0.f, EXPECT_NEAR_TOL); - } + setAirmode(0); // Airmode disabled + constexpr float THRUST = 0.75f; + constexpr float PITCH_TORQUE = 0.1f; + constexpr float PITCH = PITCH_TORQUE / NUM_ACTUATORS; + EXPECT_EQ(allocate(0.f, PITCH_TORQUE, 0.f, -THRUST), + Vector4f(THRUST + PITCH, THRUST - PITCH, THRUST - PITCH, THRUST + PITCH)); } // This tests that a control setpoint for z-thrust + yaw returns the desired actuator setpoint. // This test saturates yaw and demonstrates reduction of thrust for yaw. -TEST(ControlAllocationSequentialDesaturationTest, AirmodeDisabledReducedThrustAndYaw) +TEST_F(ControlAllocationSequentialDesaturationTestQuadX, AirmodeDisabledReducedThrustAndYaw) { - ControlAllocationSequentialDesaturation allocator; - setup_quad_allocator(allocator); - matrix::Vector control_sp; - // Negative, because +z is "downward". - constexpr float DESIRED_THRUST_Z_PER_MOTOR{0.8f}; - constexpr int MOTOR_COUNT{4}; - constexpr float THRUST_Z_TOTAL{-DESIRED_THRUST_Z_PER_MOTOR * MOTOR_COUNT}; - // This is arbitrarily high to trigger strongest possible (saturated) yaw response. - constexpr float YAW_CONTROL_SP{1.f}; - control_sp(ControlAllocation::ControlAxis::ROLL) = 0.f; - control_sp(ControlAllocation::ControlAxis::PITCH) = 0.f; - control_sp(ControlAllocation::ControlAxis::YAW) = YAW_CONTROL_SP; - control_sp(ControlAllocation::ControlAxis::THRUST_X) = 0.f; - control_sp(ControlAllocation::ControlAxis::THRUST_Y) = 0.f; - control_sp(ControlAllocation::ControlAxis::THRUST_Z) = THRUST_Z_TOTAL; - allocator.setControlSetpoint(control_sp); - - // Since MC_AIRMODE was not set explicitly, assume airmode is disabled. - allocator.allocate(); - - const auto &actuator_sp = allocator.getActuatorSetpoint(); - // In the case of yaw saturation, thrust per motor will be reduced by the hard-coded - // magic-number yaw margin of 0.15f. - constexpr float YAW_MARGIN{0.15f}; // get this from a centralized source when available. - constexpr float YAW_DIFF_PER_MOTOR{1.0f + YAW_MARGIN - DESIRED_THRUST_Z_PER_MOTOR}; - // At control set point, there will be 2 different actuator values. - constexpr float HIGH_THRUST_Z_PER_MOTOR{DESIRED_THRUST_Z_PER_MOTOR + YAW_DIFF_PER_MOTOR - YAW_MARGIN}; - constexpr float LOW_THRUST_Z_PER_MOTOR{DESIRED_THRUST_Z_PER_MOTOR - YAW_DIFF_PER_MOTOR - YAW_MARGIN}; - EXPECT_NEAR(actuator_sp(0), HIGH_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); - EXPECT_NEAR(actuator_sp(1), HIGH_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); - EXPECT_NEAR(actuator_sp(2), LOW_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); - EXPECT_NEAR(actuator_sp(3), LOW_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); - - for (int i{MOTOR_COUNT}; i < ActuatorEffectiveness::NUM_ACTUATORS; ++i) { - EXPECT_NEAR(actuator_sp(i), 0.f, EXPECT_NEAR_TOL); - } + setAirmode(0); // Airmode disabled + constexpr float YAW_MARGIN = ControlAllocationSequentialDesaturation::MINIMUM_YAW_MARGIN; + EXPECT_EQ(allocate(0.f, 0.f, 1.f, -3.2f), Vector4f(1.f, 1.f - (2.f * YAW_MARGIN), 1.f, 1.f - (2.f * YAW_MARGIN))); } // This tests that a control setpoint for z-thrust + pitch returns the desired actuator setpoint. // This test saturates the pitch response such that thrust is reduced to (partially) compensate. -TEST(ControlAllocationSequentialDesaturationTest, AirmodeDisabledReducedThrustAndPitch) +TEST_F(ControlAllocationSequentialDesaturationTestQuadX, AirmodeDisabledReducedThrustAndPitch) { - ControlAllocationSequentialDesaturation allocator; - setup_quad_allocator(allocator); - matrix::Vector control_sp; - // Negative, because +z is "downward". - constexpr float THRUST_Z_TOTAL{-0.75f * 4.f}; - // This is high enough to saturate the pitch control. - constexpr float PITCH_CONTROL_SP{2.f}; - control_sp(ControlAllocation::ControlAxis::ROLL) = 0.f; - control_sp(ControlAllocation::ControlAxis::PITCH) = PITCH_CONTROL_SP; - control_sp(ControlAllocation::ControlAxis::YAW) = 0.f; - control_sp(ControlAllocation::ControlAxis::THRUST_X) = 0.f; - control_sp(ControlAllocation::ControlAxis::THRUST_Y) = 0.f; - control_sp(ControlAllocation::ControlAxis::THRUST_Z) = THRUST_Z_TOTAL; - allocator.setControlSetpoint(control_sp); - - // Since MC_AIRMODE was not set explicitly, assume airmode is disabled. - allocator.allocate(); - - const auto &actuator_sp = allocator.getActuatorSetpoint(); - constexpr int MOTOR_COUNT{4}; - // The maximum actuator value is - // THRUST_Z_TOTAL / MOTOR_COUNT + PITCH_CONTROL_SP / MOTOR_COUNT. - // The amount over 1 is the amount that each motor is reduced by. - // At control set point, there will be 2 different actuator values. - constexpr float OVERAGE_PER_MOTOR{-THRUST_Z_TOTAL / MOTOR_COUNT + PITCH_CONTROL_SP / MOTOR_COUNT - 1}; - EXPECT_TRUE(OVERAGE_PER_MOTOR > 0.f); - constexpr float HIGH_THRUST_Z_PER_MOTOR{-THRUST_Z_TOTAL / MOTOR_COUNT + PITCH_CONTROL_SP / MOTOR_COUNT - OVERAGE_PER_MOTOR}; - constexpr float LOW_THRUST_Z_PER_MOTOR{-THRUST_Z_TOTAL / MOTOR_COUNT - PITCH_CONTROL_SP / MOTOR_COUNT - OVERAGE_PER_MOTOR}; - EXPECT_NEAR(actuator_sp(0), HIGH_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); - EXPECT_NEAR(actuator_sp(1), LOW_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); - EXPECT_NEAR(actuator_sp(2), HIGH_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); - EXPECT_NEAR(actuator_sp(3), LOW_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); - - for (int i{MOTOR_COUNT}; i < ActuatorEffectiveness::NUM_ACTUATORS; ++i) { - EXPECT_NEAR(actuator_sp(i), 0.f, EXPECT_NEAR_TOL); - } + setAirmode(0); // Airmode disabled + EXPECT_EQ(allocate(0.f, 2.f, 0.f, -3.f), Vector4f(1.f, 0.f, 0.f, 1.f)); }