dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

mixer: improve multirotor motor limits reporting

This commit is contained in:
Paul Riseborough 2016-11-22 08:00:58 +11:00 committed by Lorenz Meier
parent 3835e8cd79
commit c4ccfeae0d
2 changed files with 144 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
src/modules/systemlib/mixer/mixer.h
View File

@ -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;

149
src/modules/systemlib/mixer/mixer_multirotor.cpp
View File

@ -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)
{