diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp index 3a5793e582..8a1f0df1f4 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp @@ -44,6 +44,7 @@ #include #include +#include enum class AllocationMethod { NONE = -1, @@ -189,13 +190,20 @@ public: * @param actuator_sp input & output setpoint */ virtual void updateSetpoint(const matrix::Vector &control_sp, - int matrix_index, ActuatorVector &actuator_sp) {} + int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, + const matrix::Vector &actuator_max) {} /** * Get a bitmask of motors to be stopped */ virtual uint32_t getStoppedMotors() const { return 0; } + /** + * Fill in the allocated and unallocated torque and thrust. + * Should return false if not filled in and the effectivenes matrix should be used instead + */ + virtual bool getAllocatedAndUnallocatedControl(control_allocator_status_s &status) const { return false; } + protected: FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT}; ///< Current flight phase }; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp index 97b7f89c0b..4a8dc763f1 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp @@ -62,7 +62,8 @@ ActuatorEffectivenessFixedWing::getEffectivenessMatrix(Configuration &configurat } void ActuatorEffectivenessFixedWing::updateSetpoint(const matrix::Vector &control_sp, - int matrix_index, ActuatorVector &actuator_sp) + int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, + const matrix::Vector &actuator_max) { // apply flaps actuator_controls_s actuator_controls_0; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp index 35583eaf8b..1f8183992d 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp @@ -50,7 +50,8 @@ public: const char *name() const override { return "Fixed Wing"; } void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, - ActuatorVector &actuator_sp) override; + ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, + const matrix::Vector &actuator_max) override; private: ActuatorEffectivenessRotors _rotors; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp index 78573c8a3e..8cf7a9cb5d 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp @@ -128,8 +128,11 @@ ActuatorEffectivenessHelicopter::getEffectivenessMatrix(Configuration &configura } void ActuatorEffectivenessHelicopter::updateSetpoint(const matrix::Vector &control_sp, - int matrix_index, ActuatorVector &actuator_sp) + int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, + const matrix::Vector &actuator_max) { + _saturation_flags = {}; + // throttle/collective pitch curve const float throttle = math::interpolateN(-control_sp(ControlAxis::THRUST_Z), _geometry.throttle_curve) * throttleSpoolupProgress(); @@ -142,13 +145,31 @@ void ActuatorEffectivenessHelicopter::updateSetpoint(const matrix::Vector actuator_max(1)) { + setSaturationFlag(_geometry.yaw_sign, _saturation_flags.yaw_pos, _saturation_flags.yaw_neg); + } + for (int i = 0; i < _geometry.num_swash_plate_servos; i++) { + float roll_coeff = sinf(_geometry.swash_plate_servos[i].angle) * _geometry.swash_plate_servos[i].arm_length; + float pitch_coeff = cosf(_geometry.swash_plate_servos[i].angle) * _geometry.swash_plate_servos[i].arm_length; actuator_sp(_first_swash_plate_servo_index + i) = collective_pitch - + control_sp(ControlAxis::PITCH) * cosf(_geometry.swash_plate_servos[i].angle) * - _geometry.swash_plate_servos[i].arm_length - - control_sp(ControlAxis::ROLL) * sinf(_geometry.swash_plate_servos[i].angle) * - _geometry.swash_plate_servos[i].arm_length + + control_sp(ControlAxis::PITCH) * pitch_coeff + - control_sp(ControlAxis::ROLL) * roll_coeff + _geometry.swash_plate_servos[i].trim; + + // Saturation check for roll & pitch + if (actuator_sp(_first_swash_plate_servo_index + i) < actuator_min(_first_swash_plate_servo_index + i)) { + setSaturationFlag(roll_coeff, _saturation_flags.roll_pos, _saturation_flags.roll_neg); + setSaturationFlag(pitch_coeff, _saturation_flags.pitch_neg, _saturation_flags.pitch_pos); + + } else if (actuator_sp(_first_swash_plate_servo_index + i) > actuator_max(_first_swash_plate_servo_index + i)) { + setSaturationFlag(roll_coeff, _saturation_flags.roll_neg, _saturation_flags.roll_pos); + setSaturationFlag(pitch_coeff, _saturation_flags.pitch_pos, _saturation_flags.pitch_neg); + } } } @@ -182,3 +203,62 @@ float ActuatorEffectivenessHelicopter::throttleSpoolupProgress() return 1.f; } + + +void ActuatorEffectivenessHelicopter::setSaturationFlag(float coeff, bool &positive_flag, bool &negative_flag) +{ + if (coeff > 0.f) { + // A positive change in given axis will increase saturation + positive_flag = true; + + } else if (coeff < 0.f) { + // A negative change in given axis will increase saturation + negative_flag = true; + } +} + +bool ActuatorEffectivenessHelicopter::getAllocatedAndUnallocatedControl(control_allocator_status_s &status) const +{ + status.torque_setpoint_achieved = true; + status.thrust_setpoint_achieved = true; + + // Note: the values '-1', '1' and '0' are just to indicate a negative, + // positive or no saturation to the rate controller. The actual magnitude is not used. + if (_saturation_flags.roll_pos) { + status.unallocated_torque[0] = 1.f; + status.torque_setpoint_achieved = false; + + } else if (_saturation_flags.roll_neg) { + status.unallocated_torque[0] = -1.f; + status.torque_setpoint_achieved = false; + } + + if (_saturation_flags.pitch_pos) { + status.unallocated_torque[1] = 1.f; + status.torque_setpoint_achieved = false; + + } else if (_saturation_flags.pitch_neg) { + status.unallocated_torque[1] = -1.f; + status.torque_setpoint_achieved = false; + } + + if (_saturation_flags.yaw_pos) { + status.unallocated_torque[2] = 1.f; + status.torque_setpoint_achieved = false; + + } else if (_saturation_flags.yaw_neg) { + status.unallocated_torque[2] = -1.f; + status.torque_setpoint_achieved = false; + } + + if (_saturation_flags.thrust_pos) { + status.unallocated_thrust[2] = 1.f; + status.thrust_setpoint_achieved = false; + + } else if (_saturation_flags.thrust_neg) { + status.unallocated_thrust[2] = -1.f; + status.thrust_setpoint_achieved = false; + } + + return true; +} diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp index a57be4a8aa..64bf52138b 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp @@ -76,13 +76,28 @@ public: const Geometry &geometry() const { return _geometry; } void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, - ActuatorVector &actuator_sp) override; + ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, + const matrix::Vector &actuator_max) override; + + bool getAllocatedAndUnallocatedControl(control_allocator_status_s &status) const override; private: float throttleSpoolupProgress(); bool mainMotorEnaged(); void updateParams() override; + struct SaturationFlags { + bool roll_pos; + bool roll_neg; + bool pitch_pos; + bool pitch_neg; + bool yaw_pos; + bool yaw_neg; + bool thrust_pos; + bool thrust_neg; + }; + static void setSaturationFlag(float coeff, bool &positive_flag, bool &negative_flag); + struct ParamHandlesSwashPlate { param_t angle; param_t arm_length; @@ -103,6 +118,7 @@ private: Geometry _geometry{}; int _first_swash_plate_servo_index{}; + SaturationFlags _saturation_flags; // Throttle spoolup state uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterTest.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterTest.cpp index 1531604b30..c0f29f5a20 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterTest.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterTest.cpp @@ -60,48 +60,52 @@ TEST(ActuatorEffectivenessHelicopterTest, ThrottleCurve) Vector control_sp{}; ActuatorEffectiveness::ActuatorVector actuator_sp{}; + ActuatorEffectiveness::ActuatorVector actuator_min{}; + actuator_min.setAll(0.f); + ActuatorEffectiveness::ActuatorVector actuator_max{}; + actuator_max.setAll(1.f); control_sp(ActuatorEffectiveness::ControlAxis::THRUST_Z) = 0.1f; - helicopter.updateSetpoint(control_sp, 0, actuator_sp); + helicopter.updateSetpoint(control_sp, 0, actuator_sp, actuator_min, actuator_max); EXPECT_FLOAT_EQ(actuator_sp(0), 0.f); control_sp(ActuatorEffectiveness::ControlAxis::THRUST_Z) = 0.f; - helicopter.updateSetpoint(control_sp, 0, actuator_sp); + helicopter.updateSetpoint(control_sp, 0, actuator_sp, actuator_min, actuator_max); EXPECT_FLOAT_EQ(actuator_sp(0), 0.f); control_sp(ActuatorEffectiveness::ControlAxis::THRUST_Z) = -.125f; - helicopter.updateSetpoint(control_sp, 0, actuator_sp); + helicopter.updateSetpoint(control_sp, 0, actuator_sp, actuator_min, actuator_max); EXPECT_FLOAT_EQ(actuator_sp(0), .15f); control_sp(ActuatorEffectiveness::ControlAxis::THRUST_Z) = -.25f; - helicopter.updateSetpoint(control_sp, 0, actuator_sp); + helicopter.updateSetpoint(control_sp, 0, actuator_sp, actuator_min, actuator_max); EXPECT_FLOAT_EQ(actuator_sp(0), .3f); control_sp(ActuatorEffectiveness::ControlAxis::THRUST_Z) = -.375f; - helicopter.updateSetpoint(control_sp, 0, actuator_sp); + helicopter.updateSetpoint(control_sp, 0, actuator_sp, actuator_min, actuator_max); EXPECT_FLOAT_EQ(actuator_sp(0), .45f); control_sp(ActuatorEffectiveness::ControlAxis::THRUST_Z) = -.5f; - helicopter.updateSetpoint(control_sp, 0, actuator_sp); + helicopter.updateSetpoint(control_sp, 0, actuator_sp, actuator_min, actuator_max); EXPECT_FLOAT_EQ(actuator_sp(0), .6f); control_sp(ActuatorEffectiveness::ControlAxis::THRUST_Z) = -.625f; - helicopter.updateSetpoint(control_sp, 0, actuator_sp); + helicopter.updateSetpoint(control_sp, 0, actuator_sp, actuator_min, actuator_max); EXPECT_FLOAT_EQ(actuator_sp(0), .7f); control_sp(ActuatorEffectiveness::ControlAxis::THRUST_Z) = -.75f; - helicopter.updateSetpoint(control_sp, 0, actuator_sp); + helicopter.updateSetpoint(control_sp, 0, actuator_sp, actuator_min, actuator_max); EXPECT_FLOAT_EQ(actuator_sp(0), .8f); control_sp(ActuatorEffectiveness::ControlAxis::THRUST_Z) = -.875f; - helicopter.updateSetpoint(control_sp, 0, actuator_sp); + helicopter.updateSetpoint(control_sp, 0, actuator_sp, actuator_min, actuator_max); EXPECT_FLOAT_EQ(actuator_sp(0), .9f); control_sp(ActuatorEffectiveness::ControlAxis::THRUST_Z) = -1.f; - helicopter.updateSetpoint(control_sp, 0, actuator_sp); + helicopter.updateSetpoint(control_sp, 0, actuator_sp, actuator_min, actuator_max); EXPECT_FLOAT_EQ(actuator_sp(0), 1.f); control_sp(ActuatorEffectiveness::ControlAxis::THRUST_Z) = -1.1f; - helicopter.updateSetpoint(control_sp, 0, actuator_sp); + helicopter.updateSetpoint(control_sp, 0, actuator_sp, actuator_min, actuator_max); EXPECT_FLOAT_EQ(actuator_sp(0), 1.f); } diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp index 4ac953b53b..41005d38a7 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp @@ -77,7 +77,8 @@ ActuatorEffectivenessMCTilt::getEffectivenessMatrix(Configuration &configuration } void ActuatorEffectivenessMCTilt::updateSetpoint(const matrix::Vector &control_sp, - int matrix_index, ActuatorVector &actuator_sp) + int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, + const matrix::Vector &actuator_max) { actuator_sp += _tilt_offsets; // TODO: dynamic matrix update diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp index db9f790c51..15c12957ed 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp @@ -56,7 +56,8 @@ public: } void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, - ActuatorVector &actuator_sp) override; + ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, + const matrix::Vector &actuator_max) override; const char *name() const override { return "MC Tilt"; } diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp index 63fa1481c9..587edb4edb 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp @@ -64,7 +64,8 @@ ActuatorEffectivenessStandardVTOL::getEffectivenessMatrix(Configuration &configu } void ActuatorEffectivenessStandardVTOL::updateSetpoint(const matrix::Vector &control_sp, - int matrix_index, ActuatorVector &actuator_sp) + int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, + const matrix::Vector &actuator_max) { // apply flaps if (matrix_index == 1) { diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp index 7eddbba718..174d499941 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp @@ -73,7 +73,8 @@ public: } void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, - ActuatorVector &actuator_sp) override; + ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, + const matrix::Vector &actuator_max) override; void setFlightPhase(const FlightPhase &flight_phase) override; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp index b2c14e6c54..9379b83786 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp @@ -92,7 +92,8 @@ ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(Configuration &config } void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector &control_sp, - int matrix_index, ActuatorVector &actuator_sp) + int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, + const matrix::Vector &actuator_max) { // apply flaps if (matrix_index == 1) { diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp index fb7219973f..7f242915fe 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp @@ -75,7 +75,8 @@ public: void setFlightPhase(const FlightPhase &flight_phase) override; void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, - ActuatorVector &actuator_sp) override; + ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, + const matrix::Vector &actuator_max) override; const char *name() const override { return "VTOL Tiltrotor"; } diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index dbff6c1179..c72d26ebd9 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -416,7 +416,8 @@ ControlAllocator::Run() // Do allocation _control_allocation[i]->allocate(); - _actuator_effectiveness->updateSetpoint(c[i], i, _control_allocation[i]->_actuator_sp); + _actuator_effectiveness->updateSetpoint(c[i], i, _control_allocation[i]->_actuator_sp, + _control_allocation[i]->getActuatorMin(), _control_allocation[i]->getActuatorMax()); if (_has_slew_rate) { _control_allocation[i]->applySlewRateLimit(dt); @@ -585,30 +586,33 @@ ControlAllocator::publish_control_allocator_status(int matrix_index) // TODO: disabled motors (?) - // Allocated control - const matrix::Vector &allocated_control = _control_allocation[matrix_index]->getAllocatedControl(); - control_allocator_status.allocated_torque[0] = allocated_control(0); - control_allocator_status.allocated_torque[1] = allocated_control(1); - control_allocator_status.allocated_torque[2] = allocated_control(2); - control_allocator_status.allocated_thrust[0] = allocated_control(3); - control_allocator_status.allocated_thrust[1] = allocated_control(4); - control_allocator_status.allocated_thrust[2] = allocated_control(5); + if (!_actuator_effectiveness->getAllocatedAndUnallocatedControl(control_allocator_status)) { + // Allocated control + const matrix::Vector &allocated_control = _control_allocation[matrix_index]->getAllocatedControl(); + control_allocator_status.allocated_torque[0] = allocated_control(0); + control_allocator_status.allocated_torque[1] = allocated_control(1); + control_allocator_status.allocated_torque[2] = allocated_control(2); + control_allocator_status.allocated_thrust[0] = allocated_control(3); + control_allocator_status.allocated_thrust[1] = allocated_control(4); + control_allocator_status.allocated_thrust[2] = allocated_control(5); - // Unallocated control - const matrix::Vector unallocated_control = _control_allocation[matrix_index]->getControlSetpoint() - - allocated_control; - control_allocator_status.unallocated_torque[0] = unallocated_control(0); - control_allocator_status.unallocated_torque[1] = unallocated_control(1); - control_allocator_status.unallocated_torque[2] = unallocated_control(2); - control_allocator_status.unallocated_thrust[0] = unallocated_control(3); - control_allocator_status.unallocated_thrust[1] = unallocated_control(4); - control_allocator_status.unallocated_thrust[2] = unallocated_control(5); + // Unallocated control + const matrix::Vector unallocated_control = _control_allocation[matrix_index]->getControlSetpoint() - + allocated_control; + control_allocator_status.unallocated_torque[0] = unallocated_control(0); + control_allocator_status.unallocated_torque[1] = unallocated_control(1); + control_allocator_status.unallocated_torque[2] = unallocated_control(2); + control_allocator_status.unallocated_thrust[0] = unallocated_control(3); + control_allocator_status.unallocated_thrust[1] = unallocated_control(4); + control_allocator_status.unallocated_thrust[2] = unallocated_control(5); + + // Allocation success flags + control_allocator_status.torque_setpoint_achieved = (Vector3f(unallocated_control(0), unallocated_control(1), + unallocated_control(2)).norm_squared() < 1e-6f); + control_allocator_status.thrust_setpoint_achieved = (Vector3f(unallocated_control(3), unallocated_control(4), + unallocated_control(5)).norm_squared() < 1e-6f); + } - // Allocation success flags - control_allocator_status.torque_setpoint_achieved = (Vector3f(unallocated_control(0), unallocated_control(1), - unallocated_control(2)).norm_squared() < 1e-6f); - control_allocator_status.thrust_setpoint_achieved = (Vector3f(unallocated_control(3), unallocated_control(4), - unallocated_control(5)).norm_squared() < 1e-6f); // Actuator saturation const matrix::Vector &actuator_sp = _control_allocation[matrix_index]->getActuatorSetpoint();