diff --git a/src/modules/systemlib/mixer/mixer.cpp b/src/modules/systemlib/mixer/mixer.cpp index 982b7db82b..a8a9b14eb2 100644 --- a/src/modules/systemlib/mixer/mixer.cpp +++ b/src/modules/systemlib/mixer/mixer.cpp @@ -169,6 +169,12 @@ NullMixer::mix(float *outputs, unsigned space, uint16_t *status_reg) return 0; } +uint16_t +NullMixer::get_saturation_status() +{ + return 0; +} + void NullMixer::groups_required(uint32_t &groups) { diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h index 380b9cb53b..249beef16b 100644 --- a/src/modules/systemlib/mixer/mixer.h +++ b/src/modules/systemlib/mixer/mixer.h @@ -176,6 +176,13 @@ public: */ virtual unsigned mix(float *outputs, unsigned space, uint16_t *status_reg) = 0; + /** + * Get the saturation status. + * + * @return Integer bitmask containing saturation_status from multirotor_motor_limits.msg . + */ + virtual uint16_t get_saturation_status(void) = 0; + /** * Analyses the mix configuration and updates a bitmask of groups * that are required. @@ -261,6 +268,7 @@ public: ~MixerGroup(); virtual unsigned mix(float *outputs, unsigned space, uint16_t *status_reg); + virtual uint16_t get_saturation_status(void); virtual void groups_required(uint32_t &groups); /** @@ -394,6 +402,7 @@ public: static NullMixer *from_text(const char *buf, unsigned &buflen); virtual unsigned mix(float *outputs, unsigned space, uint16_t *status_reg); + virtual uint16_t get_saturation_status(void); virtual void groups_required(uint32_t &groups); virtual void set_offset(float trim) {}; unsigned set_trim(float trim) @@ -465,6 +474,7 @@ public: uint16_t max); virtual unsigned mix(float *outputs, unsigned space, uint16_t *status_reg); + virtual uint16_t get_saturation_status(void); virtual void groups_required(uint32_t &groups); /** @@ -571,6 +581,7 @@ public: unsigned &buflen); virtual unsigned mix(float *outputs, unsigned space, uint16_t *status_reg); + virtual uint16_t get_saturation_status(void); virtual void groups_required(uint32_t &groups); /** @@ -602,19 +613,19 @@ private: 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; + 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); diff --git a/src/modules/systemlib/mixer/mixer_group.cpp b/src/modules/systemlib/mixer/mixer_group.cpp index 693ef31b40..f5f103b45c 100644 --- a/src/modules/systemlib/mixer/mixer_group.cpp +++ b/src/modules/systemlib/mixer/mixer_group.cpp @@ -141,6 +141,13 @@ MixerGroup::set_trims(int16_t *values, unsigned n) return index; } +uint16_t +MixerGroup::get_saturation_status() +{ + Mixer *mixer = _first; + return mixer->get_saturation_status(); +} + unsigned MixerGroup::count() { diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index 12056ec48a..0024917b76 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -312,6 +312,7 @@ MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg) if (min_out < 0.0f) { _saturation_status.flags.motor_neg = true; } + if (max_out > 1.0f) { _saturation_status.flags.motor_pos = true; } @@ -334,6 +335,7 @@ 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; } + } else if (out > 1.0f) { // allow to reduce thrust to get some yaw response float thrust_reduction = fminf(0.15f, out - 1.0f); @@ -506,3 +508,7 @@ MultirotorMixer::groups_required(uint32_t &groups) groups |= (1 << 0); } +uint16_t MultirotorMixer::get_saturation_status() +{ + return _saturation_status.value; +} diff --git a/src/modules/systemlib/mixer/mixer_simple.cpp b/src/modules/systemlib/mixer/mixer_simple.cpp index 13015d1f2c..f8d18fb2e1 100644 --- a/src/modules/systemlib/mixer/mixer_simple.cpp +++ b/src/modules/systemlib/mixer/mixer_simple.cpp @@ -308,6 +308,12 @@ SimpleMixer::mix(float *outputs, unsigned space, uint16_t *status_reg) return 1; } +uint16_t +SimpleMixer::get_saturation_status() +{ + return 0; +} + void SimpleMixer::groups_required(uint32_t &groups) {