diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index cd03a04c18..4ac540d62c 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -109,7 +109,6 @@ set(msg_files mission.msg mission_result.msg mount_orientation.msg - multirotor_motor_limits.msg navigator_mission_item.msg obstacle_distance.msg offboard_control_mode.msg diff --git a/msg/multirotor_motor_limits.msg b/msg/multirotor_motor_limits.msg deleted file mode 100644 index 2cd692fd36..0000000000 --- a/msg/multirotor_motor_limits.msg +++ /dev/null @@ -1,14 +0,0 @@ -uint64 timestamp # time since system start (microseconds) -uint16 saturation_status # Integer bit mask indicating which axes in the control mixer are saturated - -# 0 - True if the saturation status is valid -# 1 - True if any motor is saturated at the upper limit -# 2 - True if any motor is saturated at the lower limit -# 3 - True if a positive roll increment will increase motor saturation -# 4 - True if negative roll increment will increase motor saturation -# 5 - True if positive pitch increment will increase motor saturation -# 6 - True if negative pitch increment will increase motor saturation -# 7 - True if positive yaw increment will increase motor saturation -# 8 - True if negative yaw increment will increase motor saturation -# 9 - True if positive thrust increment will increase motor saturation -# 10 - True if negative thrust increment will increase motor saturation diff --git a/src/drivers/pwm_out/PWMOut.hpp b/src/drivers/pwm_out/PWMOut.hpp index b6167381cc..3c12019adc 100644 --- a/src/drivers/pwm_out/PWMOut.hpp +++ b/src/drivers/pwm_out/PWMOut.hpp @@ -59,7 +59,6 @@ #include #include #include -#include #include using namespace time_literals; diff --git a/src/lib/mixer/MixerBase/Mixer.hpp b/src/lib/mixer/MixerBase/Mixer.hpp index e90290e143..ecf4e20bdf 100644 --- a/src/lib/mixer/MixerBase/Mixer.hpp +++ b/src/lib/mixer/MixerBase/Mixer.hpp @@ -179,7 +179,7 @@ public: /** * Get the saturation status. * - * @return Integer bitmask containing saturation_status from multirotor_motor_limits.msg. + * @return Integer bitmask containing saturation_status from control_allocator_status.msg. */ virtual uint16_t get_saturation_status() { return 0; } diff --git a/src/lib/mixer/MultirotorMixer/MultirotorMixer.cpp b/src/lib/mixer/MultirotorMixer/MultirotorMixer.cpp index 92780e977c..5e1948940f 100644 --- a/src/lib/mixer/MultirotorMixer/MultirotorMixer.cpp +++ b/src/lib/mixer/MultirotorMixer/MultirotorMixer.cpp @@ -451,8 +451,8 @@ MultirotorMixer::update_saturation_status(unsigned index, bool clipping_high, bo _saturation_status.flags.yaw_neg = true; } - // A positive change in thrust will increase saturation - _saturation_status.flags.thrust_pos = true; + // A positive change in thrust will increase saturation (Z neg is up) + _saturation_status.flags.thrust_neg = true; } // The motor is saturated at the lower limit @@ -478,8 +478,8 @@ MultirotorMixer::update_saturation_status(unsigned index, bool clipping_high, bo _saturation_status.flags.pitch_pos = true; } - // A negative change in thrust will increase saturation - _saturation_status.flags.thrust_neg = true; + // A negative change in thrust will increase saturation (Z pos is down) + _saturation_status.flags.thrust_pos = true; } if (clipping_low_yaw) { diff --git a/src/lib/mixer_module/mixer_module.cpp b/src/lib/mixer_module/mixer_module.cpp index b5292e9406..45bd5c96c7 100644 --- a/src/lib/mixer_module/mixer_module.cpp +++ b/src/lib/mixer_module/mixer_module.cpp @@ -861,11 +861,51 @@ MixingOutput::publishMixerStatus(const actuator_outputs_s &actuator_outputs) saturation_status.value = _mixers->get_saturation_status(); if (saturation_status.flags.valid) { - multirotor_motor_limits_s motor_limits; - motor_limits.timestamp = actuator_outputs.timestamp; - motor_limits.saturation_status = saturation_status.value; + control_allocator_status_s sat{}; + sat.timestamp = hrt_absolute_time(); + sat.torque_setpoint_achieved = true; + sat.thrust_setpoint_achieved = true; - _to_mixer_status.publish(motor_limits); + // 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_status.flags.roll_pos) { + sat.unallocated_torque[0] = 1.f; + sat.torque_setpoint_achieved = false; + + } else if (saturation_status.flags.roll_neg) { + sat.unallocated_torque[0] = -1.f; + sat.torque_setpoint_achieved = false; + } + + if (saturation_status.flags.pitch_pos) { + sat.unallocated_torque[1] = 1.f; + sat.torque_setpoint_achieved = false; + + } else if (saturation_status.flags.pitch_neg) { + sat.unallocated_torque[1] = -1.f; + sat.torque_setpoint_achieved = false; + } + + if (saturation_status.flags.yaw_pos) { + sat.unallocated_torque[2] = 1.f; + sat.torque_setpoint_achieved = false; + + } else if (saturation_status.flags.yaw_neg) { + sat.unallocated_torque[2] = -1.f; + sat.torque_setpoint_achieved = false; + } + + if (saturation_status.flags.thrust_pos) { + sat.unallocated_thrust[2] = 1.f; + sat.thrust_setpoint_achieved = false; + + } else if (saturation_status.flags.thrust_neg) { + sat.unallocated_thrust[2] = -1.f; + sat.thrust_setpoint_achieved = false; + } + + _control_allocator_status_pub.publish(sat); } } diff --git a/src/lib/mixer_module/mixer_module.hpp b/src/lib/mixer_module/mixer_module.hpp index 4ef5f64480..15f6eec50c 100644 --- a/src/lib/mixer_module/mixer_module.hpp +++ b/src/lib/mixer_module/mixer_module.hpp @@ -50,7 +50,7 @@ #include #include #include -#include +#include #include #include @@ -294,7 +294,7 @@ private: uORB::SubscriptionCallbackWorkItem _control_subs[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; uORB::PublicationMulti _outputs_pub{ORB_ID(actuator_outputs)}; - uORB::PublicationMulti _to_mixer_status{ORB_ID(multirotor_motor_limits)}; ///< mixer status flags + uORB::PublicationMulti _control_allocator_status_pub{ORB_ID(control_allocator_status)}; actuator_controls_s _controls[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS] {}; actuator_armed_s _armed{}; diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp b/src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp index 034e455db2..b219e16ee7 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp @@ -68,9 +68,7 @@ ControlAllocation::setActuatorSetpoint( // Clip clipActuatorSetpoint(_actuator_sp); - // Compute achieved control - _control_allocated = _effectiveness * _actuator_sp; - + updateControlAllocated(); } void @@ -89,6 +87,13 @@ ControlAllocation::clipActuatorSetpoint(matrix::Vector ControlAllocation::normalizeActuatorSetpoint(const matrix::Vector &actuator) const diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp b/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp index 86a2cfc8ed..482d20cd04 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp @@ -75,7 +75,7 @@ class ControlAllocation { public: - ControlAllocation() = default; + ControlAllocation() { _control_allocation_scale.setAll(1.f); } virtual ~ControlAllocation() = default; static constexpr uint8_t NUM_ACTUATORS = 16; @@ -190,6 +190,11 @@ public: */ void clipActuatorSetpoint(matrix::Vector &actuator) const; + /** + * Compute the amount of allocated control thrust and torque + */ + void updateControlAllocated(); + /** * Normalize the actuator setpoint between minimum and maximum values. * @@ -208,6 +213,7 @@ public: protected: matrix::Matrix _effectiveness; //< Effectiveness matrix + matrix::Vector _control_allocation_scale; //< Scaling applied during allocation matrix::Vector _actuator_trim; //< Neutral actuator values matrix::Vector _actuator_min; //< Minimum actuator values matrix::Vector _actuator_max; //< Maximum actuator values diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp index 4ccf51167d..ef1245941e 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp @@ -66,26 +66,28 @@ ControlAllocationPseudoInverse::normalizeControlAllocationMatrix() // Same scale on roll and pitch const float roll_norm_sq = _mix.col(0).norm_squared(); const float pitch_norm_sq = _mix.col(1).norm_squared(); - const float roll_pitch_scale = sqrtf(fmaxf(roll_norm_sq, pitch_norm_sq) / (_num_actuators / 2.f)); + _control_allocation_scale(0) = sqrtf(fmaxf(roll_norm_sq, pitch_norm_sq) / (_num_actuators / 2.f)); + _control_allocation_scale(1) = _control_allocation_scale(0); - if (roll_pitch_scale > FLT_EPSILON) { - _mix.col(0) /= roll_pitch_scale; - _mix.col(1) /= roll_pitch_scale; + if (_control_allocation_scale(0) > FLT_EPSILON) { + _mix.col(0) /= _control_allocation_scale(0); + _mix.col(1) /= _control_allocation_scale(1); } // Scale yaw separately - const float yaw_scale = _mix.col(2).max(); + _control_allocation_scale(2) = _mix.col(2).max(); - if (yaw_scale > FLT_EPSILON) { - _mix.col(2) /= yaw_scale; + if (_control_allocation_scale(2) > FLT_EPSILON) { + _mix.col(2) /= _control_allocation_scale(2); } // Same scale on X and Y - const float xy_scale = fmaxf(_mix.col(3).max(), _mix.col(4).max()); + _control_allocation_scale(3) = fmaxf(_mix.col(3).max(), _mix.col(4).max()); + _control_allocation_scale(4) = _control_allocation_scale(3); - if (xy_scale > FLT_EPSILON) { - _mix.col(3) /= xy_scale; - _mix.col(4) /= xy_scale; + if (_control_allocation_scale(3) > FLT_EPSILON) { + _mix.col(3) /= _control_allocation_scale(3); + _mix.col(4) /= _control_allocation_scale(4); } // Scale Z thrust separately @@ -97,7 +99,8 @@ ControlAllocationPseudoInverse::normalizeControlAllocationMatrix() } if ((-z_sum > FLT_EPSILON) && (_num_actuators > 0)) { - _mix.col(5) /= -z_sum / _num_actuators; + _control_allocation_scale(5) = -z_sum / _num_actuators; + _mix.col(5) /= _control_allocation_scale(5); } // Set all the small elements to 0 to avoid issues @@ -123,6 +126,5 @@ ControlAllocationPseudoInverse::allocate() // Clip clipActuatorSetpoint(_actuator_sp); - // Compute achieved control - _control_allocated = _effectiveness * _actuator_sp; + ControlAllocation::updateControlAllocated(); } diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp b/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp index a497fd8c68..4fe2720ace 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp @@ -66,8 +66,7 @@ ControlAllocationSequentialDesaturation::allocate() // Clip clipActuatorSetpoint(_actuator_sp); - // Compute achieved control - _control_allocated = _effectiveness * _actuator_sp; + ControlAllocation::updateControlAllocated(); } void ControlAllocationSequentialDesaturation::desaturateActuators( diff --git a/src/modules/control_allocator/ControlAllocator.hpp b/src/modules/control_allocator/ControlAllocator.hpp index b72e5bb17a..0f07a7e49a 100644 --- a/src/modules/control_allocator/ControlAllocator.hpp +++ b/src/modules/control_allocator/ControlAllocator.hpp @@ -112,7 +112,6 @@ private: void publish_control_allocator_status(); void publish_legacy_actuator_controls(); - void publish_legacy_multirotor_motor_limits(); enum class AllocationMethod { NONE = -1, diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 424f7671c5..93bc748dc7 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -118,7 +118,7 @@ void LoggedTopics::add_default_topics() // multi topics add_topic_multi("actuator_outputs", 100, 3); add_topic_multi("airspeed_wind", 1000); - add_topic_multi("multirotor_motor_limits", 1000, 2); + add_topic_multi("control_allocator_status", 200, 2); add_topic_multi("rate_ctrl_status", 200, 2); add_topic_multi("telemetry_status", 1000, 4); diff --git a/src/modules/mc_rate_control/MulticopterRateControl.cpp b/src/modules/mc_rate_control/MulticopterRateControl.cpp index bcd246e3ef..c53a810998 100644 --- a/src/modules/mc_rate_control/MulticopterRateControl.cpp +++ b/src/modules/mc_rate_control/MulticopterRateControl.cpp @@ -215,16 +215,26 @@ MulticopterRateControl::Run() _rate_control.resetIntegral(); } - // update saturation status from mixer feedback - if (_motor_limits_sub.updated()) { - multirotor_motor_limits_s motor_limits; + // update saturation status from control allocation feedback + control_allocator_status_s control_allocator_status; - if (_motor_limits_sub.copy(&motor_limits)) { - MultirotorMixer::saturation_status saturation_status; - saturation_status.value = motor_limits.saturation_status; + if (_control_allocator_status_sub.update(&control_allocator_status)) { + Vector saturation_positive; + Vector saturation_negative; - _rate_control.setSaturationStatus(saturation_status); + if (!control_allocator_status.torque_setpoint_achieved) { + for (size_t i = 0; i < 3; i++) { + if (control_allocator_status.unallocated_torque[i] > FLT_EPSILON) { + saturation_positive(i) = true; + + } else if (control_allocator_status.unallocated_torque[i] < -FLT_EPSILON) { + saturation_negative(i) = true; + } + } } + + // TODO: send the unallocated value directly for better anti-windup + _rate_control.setSaturationStatus(saturation_positive, saturation_negative); } // run rate controller diff --git a/src/modules/mc_rate_control/MulticopterRateControl.hpp b/src/modules/mc_rate_control/MulticopterRateControl.hpp index 77a72e4fc3..4117dfd88d 100644 --- a/src/modules/mc_rate_control/MulticopterRateControl.hpp +++ b/src/modules/mc_rate_control/MulticopterRateControl.hpp @@ -50,9 +50,9 @@ #include #include #include +#include #include #include -#include #include #include #include @@ -101,7 +101,7 @@ private: uORB::Subscription _battery_status_sub{ORB_ID(battery_status)}; uORB::Subscription _landing_gear_sub{ORB_ID(landing_gear)}; uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; - uORB::Subscription _motor_limits_sub{ORB_ID(multirotor_motor_limits)}; + uORB::Subscription _control_allocator_status_sub{ORB_ID(control_allocator_status)}; uORB::Subscription _v_control_mode_sub{ORB_ID(vehicle_control_mode)}; uORB::Subscription _v_rates_sp_sub{ORB_ID(vehicle_rates_setpoint)}; uORB::Subscription _vehicle_angular_acceleration_sub{ORB_ID(vehicle_angular_acceleration)}; diff --git a/src/modules/mc_rate_control/RateControl/RateControl.cpp b/src/modules/mc_rate_control/RateControl/RateControl.cpp index 37a2525506..bf659315f8 100644 --- a/src/modules/mc_rate_control/RateControl/RateControl.cpp +++ b/src/modules/mc_rate_control/RateControl/RateControl.cpp @@ -47,14 +47,11 @@ void RateControl::setGains(const Vector3f &P, const Vector3f &I, const Vector3f _gain_d = D; } -void RateControl::setSaturationStatus(const MultirotorMixer::saturation_status &status) +void RateControl::setSaturationStatus(const Vector &saturation_positive, + const Vector &saturation_negative) { - _mixer_saturation_positive[0] = status.flags.roll_pos; - _mixer_saturation_positive[1] = status.flags.pitch_pos; - _mixer_saturation_positive[2] = status.flags.yaw_pos; - _mixer_saturation_negative[0] = status.flags.roll_neg; - _mixer_saturation_negative[1] = status.flags.pitch_neg; - _mixer_saturation_negative[2] = status.flags.yaw_neg; + _control_allocator_saturation_positive = saturation_positive; + _control_allocator_saturation_negative = saturation_negative; } Vector3f RateControl::update(const Vector3f &rate, const Vector3f &rate_sp, const Vector3f &angular_accel, @@ -78,12 +75,12 @@ void RateControl::updateIntegral(Vector3f &rate_error, const float dt) { for (int i = 0; i < 3; i++) { // prevent further positive control saturation - if (_mixer_saturation_positive[i]) { + if (_control_allocator_saturation_positive(i)) { rate_error(i) = math::min(rate_error(i), 0.f); } // prevent further negative control saturation - if (_mixer_saturation_negative[i]) { + if (_control_allocator_saturation_negative(i)) { rate_error(i) = math::max(rate_error(i), 0.f); } diff --git a/src/modules/mc_rate_control/RateControl/RateControl.hpp b/src/modules/mc_rate_control/RateControl/RateControl.hpp index a39dabb194..76cf1d6e64 100644 --- a/src/modules/mc_rate_control/RateControl/RateControl.hpp +++ b/src/modules/mc_rate_control/RateControl/RateControl.hpp @@ -73,9 +73,10 @@ public: /** * Set saturation status - * @param status message from mixer reporting about saturation + * @param control saturation vector from control allocator */ - void setSaturationStatus(const MultirotorMixer::saturation_status &status); + void setSaturationStatus(const matrix::Vector &saturation_positive, + const matrix::Vector &saturation_negative); /** * Run one control loop cycle calculation @@ -112,6 +113,7 @@ private: // States matrix::Vector3f _rate_int; ///< integral term of the rate controller - bool _mixer_saturation_positive[3] {}; - bool _mixer_saturation_negative[3] {}; + // Feedback from control allocation + matrix::Vector _control_allocator_saturation_negative; + matrix::Vector _control_allocator_saturation_positive; };