fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-17 22:07:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Mixer 6-DoF: Airmode and saturation enabled

Browse Source 
- More info at https://github.com/Auterion/Flight_Control_Prototyping_Scripts/tree/master/control_allocation
- The drone is still not flying stably, has vibration and flies away.
This commit is contained in:
Azarakhsh Keipour
2019-07-29 12:59:15 -04:00
parent 0d7d567c4b
commit d7f61e41d8
2 changed files with 102 additions and 83 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/mixer/MultirotorMixer/MultirotorMixer.hpp
+11 -12
View File
@@ -278,7 +278,6 @@ public:
float x_scale; /**< scales x thrust for this rotor */
float y_scale; /**< scales y thrust for this rotor */
float z_scale; /**< scales z thrust for this rotor */
float thrust_scale; /**< TODO: Remove later after fixing the Airmod */
};
/**
@@ -387,12 +386,12 @@ public:
uint16_t pitch_neg : 1; // 6 - true when a negative pitch demand change will increase saturation
uint16_t yaw_pos : 1; // 7 - true when a positive yaw demand change will increase saturation
uint16_t yaw_neg : 1; // 8 - true when a negative yaw demand change will increase saturation
uint16_t x_thrust_pos : 1; // 9 - true when a positive x thrust demand change will increase saturation
uint16_t x_thrust_neg : 1; //10 - true when a negative x thrust demand change will increase saturation
uint16_t y_thrust_pos : 1; //11 - true when a positive y thrust demand change will increase saturation
uint16_t y_thrust_neg : 1; //12 - true when a negative y thrust demand change will increase saturation
uint16_t z_thrust_pos : 1; //13 - true when a positive z thrust demand change will increase saturation
uint16_t z_thrust_neg : 1; //14 - true when a negative z thrust demand change will increase saturation
uint16_t x_pos : 1; // 9 - true when a positive x thrust demand change will increase saturation
uint16_t x_neg : 1; //10 - true when a negative x thrust demand change will increase saturation
uint16_t y_pos : 1; //11 - true when a positive y thrust demand change will increase saturation
uint16_t y_neg : 1; //12 - true when a negative y thrust demand change will increase saturation
uint16_t z_pos : 1; //13 - true when a positive z thrust demand change will increase saturation
uint16_t z_neg : 1; //14 - true when a negative z thrust demand change will increase saturation
} flags;
uint16_t value;
};
@@ -412,14 +411,14 @@ private:
* Minimize the saturation of the actuators by adding or substracting a fraction of desaturation_vector.
* desaturation_vector is the vector that added to the output outputs, modifies the thrust or angular
* acceleration on a specific axis.
* For example, if desaturation_vector is given to slide along the vertical thrust axis (thrust_scale), the
* For example, if desaturation_vector is given to slide along the vertical thrust axis (z_scale), the
* saturation will be minimized by shifting the vertical thrust setpoint, without changing the
* roll/pitch/yaw accelerations.
*
* Note that as we only slide along the given axis, in extreme cases outputs can still contain values
* outside of [min_output, max_output].
*
* @param desaturation_vector vector that is added to the outputs, e.g. thrust_scale
* @param desaturation_vector vector that is added to the outputs, e.g. z_scale
* @param outputs output vector that is modified
* @param sat_status saturation status output
* @param min_output minimum desired value in outputs
@@ -436,7 +435,7 @@ private:
* thrust is increased/decreased as much as required to meet the demanded roll/pitch.
* Yaw is not allowed to increase the thrust, @see mix_yaw() for the exact behavior.
*/
inline void mix_airmode_rp(float roll, float pitch, float yaw, float thrust, float *outputs);
inline void mix_airmode_rp(float roll, float pitch, float yaw, float x_thrust, float y_thrust, float z_thrust, float *outputs);
/**
* Mix roll, pitch, yaw, thrust and set the outputs vector.
@@ -445,7 +444,7 @@ private:
* thrust is increased/decreased as much as required to meet demanded the roll/pitch/yaw,
* while giving priority to roll and pitch over yaw.
*/
inline void mix_airmode_rpy(float roll, float pitch, float yaw, float thrust, float *outputs);
inline void mix_airmode_rpy(float roll, float pitch, float yaw, float x_thrust, float y_thrust, float z_thrust, float *outputs);
/**
* Mix roll, pitch, yaw, thrust and set the outputs vector.
@@ -455,7 +454,7 @@ private:
* Thrust can be reduced to unsaturate the upper side.
* @see mix_yaw() for the exact yaw behavior.
*/
inline void mix_airmode_disabled(float roll, float pitch, float yaw, float thrust, float *outputs);
inline void mix_airmode_disabled(float roll, float pitch, float yaw, float x_thrust, float y_thrust, float z_thrust, float *outputs);
/**
* Mix yaw by updating an existing output vector (that already contains roll/pitch/thrust).
src/lib/mixer/MultirotorMixer/MultirotorMixer6dof.cpp
+91 -71
View File
@@ -213,7 +213,7 @@ MultirotorMixer6dof::minimize_saturation(const float *desaturation_vector, float
}
void
MultirotorMixer6dof::mix_airmode_rp(float roll, float pitch, float yaw, float thrust, float *outputs)
MultirotorMixer6dof::mix_airmode_rp(float roll, float pitch, float yaw, float x_thrust, float y_thrust, float z_thrust, float *outputs)
{
// Airmode for roll and pitch, but not yaw
@@ -221,10 +221,12 @@ MultirotorMixer6dof::mix_airmode_rp(float roll, float pitch, float yaw, float th
for (unsigned i = 0; i < _rotor_count; i++) {
outputs[i] = roll * _rotors[i].roll_scale +
pitch * _rotors[i].pitch_scale +
thrust * _rotors[i].thrust_scale;
x_thrust * _rotors[i].x_scale +
y_thrust * _rotors[i].y_scale +
z_thrust * _rotors[i].z_scale;
// Thrust will be used to unsaturate if needed
_tmp_array[i] = _rotors[i].thrust_scale;
_tmp_array[i] = _rotors[i].z_scale;
}
minimize_saturation(_tmp_array, outputs, _saturation_status);
@@ -234,7 +236,7 @@ MultirotorMixer6dof::mix_airmode_rp(float roll, float pitch, float yaw, float th
}
void
MultirotorMixer6dof::mix_airmode_rpy(float roll, float pitch, float yaw, float thrust, float *outputs)
MultirotorMixer6dof::mix_airmode_rpy(float roll, float pitch, float yaw, float x_thrust, float y_thrust, float z_thrust, float *outputs)
{
// Airmode for roll, pitch and yaw
@@ -243,10 +245,12 @@ MultirotorMixer6dof::mix_airmode_rpy(float roll, float pitch, float yaw, float t
outputs[i] = roll * _rotors[i].roll_scale +
pitch * _rotors[i].pitch_scale +
yaw * _rotors[i].yaw_scale +
thrust * _rotors[i].thrust_scale;
x_thrust * _rotors[i].x_scale +
y_thrust * _rotors[i].y_scale +
z_thrust * _rotors[i].z_scale;
// Thrust will be used to unsaturate if needed
_tmp_array[i] = _rotors[i].thrust_scale;
// Z thrust will be used to unsaturate if needed
_tmp_array[i] = _rotors[i].z_scale;
}
minimize_saturation(_tmp_array, outputs, _saturation_status);
@@ -261,18 +265,20 @@ MultirotorMixer6dof::mix_airmode_rpy(float roll, float pitch, float yaw, float t
}
void
MultirotorMixer6dof::mix_airmode_disabled(float roll, float pitch, float yaw, float thrust, float *outputs)
MultirotorMixer6dof::mix_airmode_disabled(float roll, float pitch, float yaw, float x_thrust, float y_thrust, float z_thrust, float *outputs)
{
// Airmode disabled: never allow to increase the thrust to unsaturate a motor
// Mix without yaw
for (unsigned i = 0; i < _rotor_count; i++) {
outputs[i] = roll * _rotors[i].roll_scale +
pitch * _rotors[i].pitch_scale +
thrust * _rotors[i].thrust_scale;
pitch * _rotors[i].pitch_scale +
x_thrust * _rotors[i].x_scale +
y_thrust * _rotors[i].y_scale +
z_thrust * _rotors[i].z_scale;
// Thrust will be used to unsaturate if needed
_tmp_array[i] = _rotors[i].thrust_scale;
// Z thrust will be used to unsaturate if needed
_tmp_array[i] = _rotors[i].z_scale;
}
// only reduce thrust
@@ -310,7 +316,7 @@ void MultirotorMixer6dof::mix_yaw(float yaw, float *outputs)
minimize_saturation(_tmp_array, outputs, _saturation_status, 0.f, 1.15f);
for (unsigned i = 0; i < _rotor_count; i++) {
_tmp_array[i] = _rotors[i].thrust_scale;
_tmp_array[i] = _rotors[i].z_scale;
}
// reduce thrust only
@@ -329,60 +335,26 @@ MultirotorMixer6dof::mix(float *outputs, unsigned space)
float yaw = math::constrain(get_control(0, 2) * _yaw_scale, -1.0f, 1.0f);
float x_thrust = math::constrain(get_control(0, 4), 0.0f, 1.0f);
float y_thrust = math::constrain(get_control(0, 5), 0.0f, 1.0f);
float z_thrust = math::constrain(get_control(0, 3), 0.0f, 1.0f);
// TODO remove the - sign
float z_thrust = - math::constrain(get_control(0, 3), 0.0f, 1.0f);
// clean out class variable used to capture saturation
_saturation_status.value = 0;
// TODO: Enable Airmod for 6dof
// // Do the mixing using the strategy given by the current Airmode configuration
// switch (_airmode) {
// case Airmode::roll_pitch:
// mix_airmode_rp(roll, pitch, yaw, z_thrust, outputs);
// break;
// Do the mixing using the strategy given by the current Airmode configuration
switch (_airmode) {
case Airmode::roll_pitch:
mix_airmode_rp(roll, pitch, yaw, x_thrust, y_thrust, z_thrust, outputs);
break;
// case Airmode::roll_pitch_yaw:
// mix_airmode_rpy(roll, pitch, yaw, z_thrust, outputs);
// break;
case Airmode::roll_pitch_yaw:
mix_airmode_rpy(roll, pitch, yaw, x_thrust, y_thrust, z_thrust, outputs);
break;
// case Airmode::disabled:
// default: // just in case: default to disabled
// mix_airmode_disabled(roll, pitch, yaw, z_thrust, outputs);
// break;
// }
// TODO: Remove later after fixing Airmod till --- point
float min_out = 1.0f;
float max_out = 0.0f;
for (unsigned i = 0; i < _rotor_count; i++) {
float out = roll * _rotors[i].roll_scale +
pitch * _rotors[i].pitch_scale +
yaw * _rotors[i].yaw_scale +
x_thrust * _rotors[i].x_scale +
y_thrust * _rotors[i].y_scale +
// TODO remove the - sign
- z_thrust * _rotors[i].z_scale;
/* calculate min and max output values */
if (out < min_out) {
min_out = out;
}
if (out > max_out) {
max_out = out;
}
outputs[i] = out;
}
// capture saturation
if (min_out < 0.0f) {
_saturation_status.flags.motor_neg = true;
}
if (max_out > 1.0f) {
_saturation_status.flags.motor_pos = true;
case Airmode::disabled:
default: // just in case: default to disabled
mix_airmode_disabled(roll, pitch, yaw, x_thrust, y_thrust, z_thrust, outputs);
break;
}
// Apply thrust model and scale outputs to range [idle_speed, 1].
@@ -460,8 +432,6 @@ void
MultirotorMixer6dof::update_saturation_status(unsigned index, bool clipping_high, bool clipping_low_roll_pitch,
bool clipping_low_yaw)
{
// TODO: handle saturation for 6dof
// The motor is saturated at the upper limit
// check which control axes and which directions are contributing
if (clipping_high) {
@@ -494,10 +464,35 @@ MultirotorMixer6dof::update_saturation_status(unsigned index, bool clipping_high
_saturation_status.flags.yaw_neg = true;
}
// A positive change in thrust will increase saturation
_saturation_status.flags.x_thrust_pos = true;
_saturation_status.flags.y_thrust_pos = true;
_saturation_status.flags.z_thrust_pos = true;
// check if the x input is saturating
if (_rotors[index].x_scale > 0.0f) {
// A positive change in x will increase saturation
_saturation_status.flags.x_pos = true;
} else if (_rotors[index].x_scale < 0.0f) {
// A negative change in x will increase saturation
_saturation_status.flags.x_neg = true;
}
// check if the y input is saturating
if (_rotors[index].y_scale > 0.0f) {
// A positive change in y will increase saturation
_saturation_status.flags.y_pos = true;
} else if (_rotors[index].y_scale < 0.0f) {
// A negative change in y will increase saturation
_saturation_status.flags.y_neg = true;
}
// check if the z input is saturating
if (_rotors[index].z_scale > 0.0f) {
// A positive change in z will increase saturation
_saturation_status.flags.z_pos = true;
} else if (_rotors[index].z_scale < 0.0f) {
// A negative change in z will increase saturation
_saturation_status.flags.z_neg = true;
}
}
// The motor is saturated at the lower limit
@@ -523,10 +518,35 @@ MultirotorMixer6dof::update_saturation_status(unsigned index, bool clipping_high
_saturation_status.flags.pitch_pos = true;
}
// A negative change in thrust will increase saturation
_saturation_status.flags.x_thrust_neg = true;
_saturation_status.flags.y_thrust_neg = true;
_saturation_status.flags.z_thrust_neg = true;
// check if the x input is saturating
if (_rotors[index].x_scale > 0.0f) {
// A negative change in x will increase saturation
_saturation_status.flags.x_neg = true;
} else if (_rotors[index].x_scale < 0.0f) {
// A positive change in x will increase saturation
_saturation_status.flags.x_pos = true;
}
// check if the y input is saturating
if (_rotors[index].y_scale > 0.0f) {
// A negative change in y will increase saturation
_saturation_status.flags.y_neg = true;
} else if (_rotors[index].y_scale < 0.0f) {
// A positive change in y will increase saturation
_saturation_status.flags.y_pos = true;
}
// check if the z input is saturating
if (_rotors[index].z_scale > 0.0f) {
// A negative change in z will increase saturation
_saturation_status.flags.z_neg = true;
} else if (_rotors[index].z_scale < 0.0f) {
// A positive change in z will increase saturation
_saturation_status.flags.z_pos = true;
}
}
if (clipping_low_yaw) {