forked from Archive/PX4-Autopilot
MC mixer: replace multirotor_motor_limits by control_allocator_status
CA: fix saturation computation Since the CA matrix is normalized, the same scale applied to be used when using the effectiveness matrix MCRateControl: use control_allocator_status to get saturation info
This commit is contained in:
parent
dd83ef1813
commit
d47f9f155a
|
@ -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
|
||||
|
|
|
@ -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
|
|
@ -59,7 +59,6 @@
|
|||
#include <uORB/topics/actuator_armed.h>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/actuator_outputs.h>
|
||||
#include <uORB/topics/multirotor_motor_limits.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
|
||||
using namespace time_literals;
|
||||
|
|
|
@ -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; }
|
||||
|
||||
|
|
|
@ -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) {
|
||||
|
|
|
@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -50,7 +50,7 @@
|
|||
#include <uORB/topics/actuator_armed.h>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/actuator_outputs.h>
|
||||
#include <uORB/topics/multirotor_motor_limits.h>
|
||||
#include <uORB/topics/control_allocator_status.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/test_motor.h>
|
||||
|
||||
|
@ -294,7 +294,7 @@ private:
|
|||
uORB::SubscriptionCallbackWorkItem _control_subs[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
|
||||
|
||||
uORB::PublicationMulti<actuator_outputs_s> _outputs_pub{ORB_ID(actuator_outputs)};
|
||||
uORB::PublicationMulti<multirotor_motor_limits_s> _to_mixer_status{ORB_ID(multirotor_motor_limits)}; ///< mixer status flags
|
||||
uORB::PublicationMulti<control_allocator_status_s> _control_allocator_status_pub{ORB_ID(control_allocator_status)};
|
||||
|
||||
actuator_controls_s _controls[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS] {};
|
||||
actuator_armed_s _armed{};
|
||||
|
|
|
@ -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<float, ControlAllocation:
|
|||
}
|
||||
}
|
||||
|
||||
void
|
||||
ControlAllocation::updateControlAllocated()
|
||||
{
|
||||
// Compute achieved control
|
||||
_control_allocated = (_effectiveness * _actuator_sp).emult(_control_allocation_scale);
|
||||
}
|
||||
|
||||
matrix::Vector<float, ControlAllocation::NUM_ACTUATORS>
|
||||
ControlAllocation::normalizeActuatorSetpoint(const matrix::Vector<float, ControlAllocation::NUM_ACTUATORS> &actuator)
|
||||
const
|
||||
|
|
|
@ -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<float, NUM_ACTUATORS> &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<float, NUM_AXES, NUM_ACTUATORS> _effectiveness; //< Effectiveness matrix
|
||||
matrix::Vector<float, NUM_AXES> _control_allocation_scale; //< Scaling applied during allocation
|
||||
matrix::Vector<float, NUM_ACTUATORS> _actuator_trim; //< Neutral actuator values
|
||||
matrix::Vector<float, NUM_ACTUATORS> _actuator_min; //< Minimum actuator values
|
||||
matrix::Vector<float, NUM_ACTUATORS> _actuator_max; //< Maximum actuator values
|
||||
|
|
|
@ -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();
|
||||
}
|
||||
|
|
|
@ -66,8 +66,7 @@ ControlAllocationSequentialDesaturation::allocate()
|
|||
// Clip
|
||||
clipActuatorSetpoint(_actuator_sp);
|
||||
|
||||
// Compute achieved control
|
||||
_control_allocated = _effectiveness * _actuator_sp;
|
||||
ControlAllocation::updateControlAllocated();
|
||||
}
|
||||
|
||||
void ControlAllocationSequentialDesaturation::desaturateActuators(
|
||||
|
|
|
@ -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,
|
||||
|
|
|
@ -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);
|
||||
|
||||
|
|
|
@ -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<bool, 3> saturation_positive;
|
||||
Vector<bool, 3> 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
|
||||
|
|
|
@ -50,9 +50,9 @@
|
|||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/actuator_controls_status.h>
|
||||
#include <uORB/topics/battery_status.h>
|
||||
#include <uORB/topics/control_allocator_status.h>
|
||||
#include <uORB/topics/landing_gear.h>
|
||||
#include <uORB/topics/manual_control_setpoint.h>
|
||||
#include <uORB/topics/multirotor_motor_limits.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/rate_ctrl_status.h>
|
||||
#include <uORB/topics/vehicle_angular_acceleration.h>
|
||||
|
@ -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)};
|
||||
|
|
|
@ -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<bool, 3> &saturation_positive,
|
||||
const Vector<bool, 3> &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);
|
||||
}
|
||||
|
||||
|
|
|
@ -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<bool, 3> &saturation_positive,
|
||||
const matrix::Vector<bool, 3> &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<bool, 3> _control_allocator_saturation_negative;
|
||||
matrix::Vector<bool, 3> _control_allocator_saturation_positive;
|
||||
};
|
||||
|
|
Loading…
Reference in New Issue