diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h
index bd00862010520991d4cc83f80d118672f3edc1c5..380b9cb53b8386c5fa91924387d2cf8292cd7aef 100644
--- a/src/modules/systemlib/mixer/mixer.h
+++ b/src/modules/systemlib/mixer/mixer.h
@@ -601,6 +601,24 @@ private:
orb_advert_t _limits_pub;
multirotor_motor_limits_s _limits;
+ union {
+ struct {
+ uint16_t motor_pos : 1; // 0 - true when any motor has saturated in the positive direction
+ uint16_t motor_neg : 1; // 1 - true when any motor has saturated in the negative direction
+ uint16_t roll_pos : 1; // 2 - true when a positive roll demand change will increase saturation
+ uint16_t roll_neg : 1; // 3 - true when a negative roll demand change will increase saturation
+ uint16_t pitch_pos : 1; // 4 - true when a positive pitch demand change will increase saturation
+ uint16_t pitch_neg : 1; // 5 - true when a negative pitch demand change will increase saturation
+ uint16_t yaw_pos : 1; // 6 - true when a positive yaw demand change will increase saturation
+ uint16_t yaw_neg : 1; // 7 - true when a negative yaw demand change will increase saturation
+ uint16_t thrust_pos : 1; // 8 - true when a positive thrust demand change will increase saturation
+ uint16_t thrust_neg : 1; // 9 - true when a negative thrust demand change will increase saturation
+ } flags;
+ uint16_t value;
+ } _saturation_status;
+
+ void update_saturation_status(unsigned index, bool clipping_high, bool clipping_low);
+
unsigned _rotor_count;
const Rotor *_rotors;
diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp
index 950bf3d6870a23bd18f6a7c5f6bdd55cce6abe01..12056ec48ae5b268d86ece5d8628f476fd16d88e 100644
--- a/src/modules/systemlib/mixer/mixer_multirotor.cpp
+++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp
@@ -240,10 +240,8 @@ MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
float min_out = 1.0f;
float max_out = 0.0f;
- // clean register for saturation status flags
- if (status_reg != NULL) {
- (*status_reg) = 0;
- }
+ // clean out class variable used to capture saturation
+ _saturation_status.value = 0;
// thrust boost parameters
float thrust_increase_factor = 1.5f;
@@ -269,7 +267,7 @@ MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
outputs[i] = out;
}
- float boost = 0.0f; // value added to demanded thrust (can also be negative)
+ float boost = 0.0f; // value added to demanded thrust (can also be negative)
float roll_pitch_scale = 1.0f; // scale for demanded roll and pitch
if (min_out < 0.0f && max_out < 1.0f && -min_out <= 1.0f - max_out) {
@@ -310,17 +308,12 @@ MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
roll_pitch_scale = (thrust + boost) / (thrust - min_out);
}
- // notify if saturation has occurred
+ // capture saturation
if (min_out < 0.0f) {
- if (status_reg != NULL) {
- (*status_reg) |= PX4IO_P_STATUS_MIXER_LOWER_LIMIT;
- }
+ _saturation_status.flags.motor_neg = true;
}
-
if (max_out > 1.0f) {
- if (status_reg != NULL) {
- (*status_reg) |= PX4IO_P_STATUS_MIXER_UPPER_LIMIT;
- }
+ _saturation_status.flags.motor_pos = true;
}
// mix again but now with thrust boost, scale roll/pitch and also add yaw
@@ -341,11 +334,6 @@ MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
yaw = -((roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) *
roll_pitch_scale + thrust + boost) / _rotors[i].yaw_scale;
}
-
- if (status_reg != NULL) {
- (*status_reg) |= PX4IO_P_STATUS_MIXER_YAW_LIMIT;
- }
-
} else if (out > 1.0f) {
// allow to reduce thrust to get some yaw response
float thrust_reduction = fminf(0.15f, out - 1.0f);
@@ -358,10 +346,6 @@ MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
yaw = (1.0f - ((roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) *
roll_pitch_scale + thrust + boost)) / _rotors[i].yaw_scale;
}
-
- if (status_reg != NULL) {
- (*status_reg) |= PX4IO_P_STATUS_MIXER_YAW_LIMIT;
- }
}
}
@@ -374,28 +358,147 @@ MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
outputs[i] = constrain(_idle_speed + (outputs[i] * (1.0f - _idle_speed)), _idle_speed, 1.0f);
- // slew rate limiting
+ }
+
+ /* slew rate limiting and saturation checking */
+ for (unsigned i = 0; i < _rotor_count; i++) {
+ bool clipping_high = false;
+ bool clipping_low = false;
+
+ // check for saturation against static limits
+ if (outputs[i] > 0.99f) {
+ clipping_high = true;
+
+ } else if (outputs[i] < _idle_speed + 0.01f) {
+ clipping_low = true;
+
+ }
+
+ // check for saturation against slew rate limits
if (_delta_out_max > 0.0f) {
float delta_out = outputs[i] - _outputs_prev[i];
if (delta_out > _delta_out_max) {
outputs[i] = _outputs_prev[i] + _delta_out_max;
+ clipping_high = true;
} else if (delta_out < -_delta_out_max) {
outputs[i] = _outputs_prev[i] - _delta_out_max;
+ clipping_low = true;
+
}
}
_outputs_prev[i] = outputs[i];
+ // update the saturation status report
+ update_saturation_status(i, clipping_high, clipping_low);
+
}
// this will force the caller of the mixer to always supply new slew rate values, otherwise no slew rate limiting will happen
_delta_out_max = 0.0f;
+ // Notify saturation status
+ if (status_reg != NULL) {
+ (*status_reg) = _saturation_status.value;
+ }
+
return _rotor_count;
}
+/*
+ * This function update the control saturation status report using hte following inputs:
+ *
+ * index: 0 based index identifying the motor that is saturating
+ * clipping_high: true if the motor demand is being limited in the positive direction
+ * clipping_low: true if the motor demand is being limited in the negative direction
+*/
+void
+MultirotorMixer::update_saturation_status(unsigned index, bool clipping_high, bool clipping_low)
+{
+ // The motor is saturated at the upper limit
+ // check which control axes and which directions are contributing
+ if (clipping_high) {
+ if (_rotors[index].roll_scale > 0.0f) {
+ // A positive change in roll will increase saturation
+ _saturation_status.flags.roll_pos = true;
+
+ } else if (_rotors[index].roll_scale < 0.0f) {
+ // A negative change in roll will increase saturation
+ _saturation_status.flags.roll_neg = true;
+
+ }
+
+ // check if the pitch input is saturating
+ if (_rotors[index].pitch_scale > 0.0f) {
+ // A positive change in pitch will increase saturation
+ _saturation_status.flags.pitch_pos = true;
+
+ } else if (_rotors[index].pitch_scale < 0.0f) {
+ // A negative change in pitch will increase saturation
+ _saturation_status.flags.pitch_neg = true;
+
+ }
+
+ // check if the yaw input is saturating
+ if (_rotors[index].yaw_scale > 0.0f) {
+ // A positive change in yaw will increase saturation
+ _saturation_status.flags.yaw_pos = true;
+
+ } else if (_rotors[index].yaw_scale < 0.0f) {
+ // A negative change in yaw will increase saturation
+ _saturation_status.flags.yaw_neg = true;
+
+ }
+
+ // A positive change in thrust will increase saturation
+ _saturation_status.flags.thrust_pos = true;
+
+ }
+
+ // The motor is saturated at the lower limit
+ // check which control axes and which directions are contributing
+ if (clipping_low) {
+ // check if the roll input is saturating
+ if (_rotors[index].roll_scale > 0.0f) {
+ // A negative change in roll will increase saturation
+ _saturation_status.flags.roll_neg = true;
+
+ } else if (_rotors[index].roll_scale < 0.0f) {
+ // A positive change in roll will increase saturation
+ _saturation_status.flags.roll_pos = true;
+
+ }
+
+ // check if the pitch input is saturating
+ if (_rotors[index].pitch_scale > 0.0f) {
+ // A negative change in pitch will increase saturation
+ _saturation_status.flags.pitch_neg = true;
+
+ } else if (_rotors[index].pitch_scale < 0.0f) {
+ // A positive change in pitch will increase saturation
+ _saturation_status.flags.pitch_pos = true;
+
+ }
+
+ // check if the yaw input is saturating
+ if (_rotors[index].yaw_scale > 0.0f) {
+ // A negative change in yaw will increase saturation
+ _saturation_status.flags.yaw_neg = true;
+
+ } else if (_rotors[index].yaw_scale < 0.0f) {
+ // A positive change in yaw will increase saturation
+ _saturation_status.flags.yaw_pos = true;
+
+ }
+
+ // A negative change in thrust will increase saturation
+ _saturation_status.flags.thrust_neg = true;
+
+ }
+}
+
void
MultirotorMixer::groups_required(uint32_t &groups)
{