void motor_mixing_run(bool_t motors_on, bool_t override_on, pprz_t in_cmd[] ) {
uint8_t i;
#if !HITL
if (motors_on) {
#else
if (FALSE) {
#endif
int32_t min_cmd = INT32_MAX;
int32_t max_cmd = INT32_MIN;
/* do the mixing in float to avoid overflows, implicitly casted back to int32_t */
for (i=0; i<MOTOR_MIXING_NB_MOTOR; i++) {
motor_mixing.commands[i] = MOTOR_MIXING_MIN_MOTOR +
(thrust_coef[i] * in_cmd[COMMAND_THRUST] +
roll_coef[i] * in_cmd[COMMAND_ROLL] +
pitch_coef[i] * in_cmd[COMMAND_PITCH] +
yaw_coef[i] * in_cmd[COMMAND_YAW] +
motor_mixing.trim[i]) / MOTOR_MIXING_SCALE *
(MOTOR_MIXING_MAX_MOTOR - MOTOR_MIXING_MIN_MOTOR) / MAX_PPRZ;
if (motor_mixing.commands[i] < min_cmd)
min_cmd = motor_mixing.commands[i];
if (motor_mixing.commands[i] > max_cmd)
max_cmd = motor_mixing.commands[i];
}
if (min_cmd < MOTOR_MIXING_MIN_MOTOR && max_cmd > MOTOR_MIXING_MAX_MOTOR)
motor_mixing.nb_failure++;
/* In case of both min and max saturation, only lower the throttle
* instead of applying both. This should prevent your quad shooting up,
* but it might loose altitude in case of such a saturation failure.
*/
if (max_cmd > MOTOR_MIXING_MAX_MOTOR) {
int32_t saturation_offset = MOTOR_MIXING_MAX_MOTOR - max_cmd;
BoundAbs(saturation_offset, MOTOR_MIXING_MAX_SATURATION_OFFSET);
offset_commands(saturation_offset);
motor_mixing.nb_saturation++;
}
else if (min_cmd < MOTOR_MIXING_MIN_MOTOR) {
int32_t saturation_offset = MOTOR_MIXING_MIN_MOTOR - min_cmd;
BoundAbs(saturation_offset, MOTOR_MIXING_MAX_SATURATION_OFFSET);
offset_commands(saturation_offset);
motor_mixing.nb_saturation++;
}
/* For testing motor failure */
if (motors_on && override_on) {
for (i = 0; i < MOTOR_MIXING_NB_MOTOR; i++) {
if (motor_mixing.override_enabled[i])
motor_mixing.commands[i] = motor_mixing.override_value[i];
}
}
bound_commands();
bound_commands_step();
}
else {
for (i=0; i<MOTOR_MIXING_NB_MOTOR; i++) {
motor_mixing.commands[i] = MOTOR_MIXING_STOP_MOTOR;
}
}
}
void motor_mixing_run(bool_t motors_on, bool_t override_on, pprz_t in_cmd[] ) {
uint8_t i;
if (motors_on) {
int32_t min_cmd = INT32_MAX;
int32_t max_cmd = INT32_MIN;
/* do the mixing in float to avoid overflows, implicitly casted back to int32_t */
for (i=0; i<MOTOR_MIXING_NB_MOTOR; i++) {
motor_mixing.commands[i] = MOTOR_MIXING_MIN_MOTOR +
(thrust_coef[i] * in_cmd[COMMAND_THRUST] +
roll_coef[i] * in_cmd[COMMAND_ROLL] +
pitch_coef[i] * in_cmd[COMMAND_PITCH] +
yaw_coef[i] * in_cmd[COMMAND_YAW] +
motor_mixing.trim[i]) / MOTOR_MIXING_SCALE *
(MOTOR_MIXING_MAX_MOTOR - MOTOR_MIXING_MIN_MOTOR) / MAX_PPRZ;
if (motor_mixing.commands[i] < min_cmd)
min_cmd = motor_mixing.commands[i];
if (motor_mixing.commands[i] > max_cmd)
max_cmd = motor_mixing.commands[i];
}
if (min_cmd < MOTOR_MIXING_MIN_MOTOR && max_cmd > MOTOR_MIXING_MAX_MOTOR)
motor_mixing.nb_failure++;
if (min_cmd < MOTOR_MIXING_MIN_MOTOR) {
int32_t saturation_offset = MOTOR_MIXING_MIN_MOTOR - min_cmd;
BoundAbs(saturation_offset, MOTOR_MIXING_MAX_SATURATION_OFFSET);
offset_commands(saturation_offset);
}
if (max_cmd > MOTOR_MIXING_MAX_MOTOR) {
int32_t saturation_offset = MOTOR_MIXING_MAX_MOTOR - max_cmd;
BoundAbs(saturation_offset, MOTOR_MIXING_MAX_SATURATION_OFFSET);
offset_commands(saturation_offset);
}
/* For testing motor failure */
if (motors_on && override_on) {
for (i = 0; i < MOTOR_MIXING_NB_MOTOR; i++) {
if (motor_mixing.override_enabled[i])
motor_mixing.commands[i] = motor_mixing.override_value[i];
}
}
bound_commands();
bound_commands_step();
}
else {
for (i=0; i<MOTOR_MIXING_NB_MOTOR; i++) {
motor_mixing.commands[i] = MOTOR_MIXING_STOP_MOTOR;
}
}
}
void motor_mixing_run(bool motors_on, bool override_on, pprz_t in_cmd[])
{
uint8_t i;
#if !HITL
if (motors_on) {
#else
if (FALSE) {
#endif
int32_t tmp_cmd;
int32_t max_overflow = 0;
/* first calculate the highest priority part of the command:
* - add trim + roll + pitch + thrust for each motor
* - calc max saturation/overflow when yaw command is also added
*/
for (i = 0; i < MOTOR_MIXING_NB_MOTOR; i++) {
motor_mixing.commands[i] = motor_mixing.trim[i] +
roll_coef[i] * in_cmd[COMMAND_ROLL] +
pitch_coef[i] * in_cmd[COMMAND_PITCH] +
thrust_coef[i] * in_cmd[COMMAND_THRUST];
/* compute the command with yaw for each motor to check how much it would saturate */
tmp_cmd = motor_mixing.commands[i] + yaw_coef[i] * in_cmd[COMMAND_YAW];
tmp_cmd /= MOTOR_MIXING_SCALE;
/* remember max overflow (how much in saturation) */
if (-tmp_cmd > max_overflow) {
max_overflow = -tmp_cmd;
}
else if (tmp_cmd - MAX_PPRZ > max_overflow) {
max_overflow = tmp_cmd - MAX_PPRZ;
}
}
/* calculate how much authority is left for yaw command */
int32_t yaw_authority = ABS(in_cmd[COMMAND_YAW]) - max_overflow;
Bound(yaw_authority, 0, MAX_PPRZ);
int32_t bounded_yaw_cmd = in_cmd[COMMAND_YAW];
BoundAbs(bounded_yaw_cmd, yaw_authority);
/* min/max of commands */
int32_t min_cmd = INT32_MAX;
int32_t max_cmd = INT32_MIN;
/* add the bounded yaw command and scale */
for (i = 0; i < MOTOR_MIXING_NB_MOTOR; i++) {
motor_mixing.commands[i] += yaw_coef[i] * bounded_yaw_cmd;
motor_mixing.commands[i] /= MOTOR_MIXING_SCALE;
/* remember min/max */
if (motor_mixing.commands[i] < min_cmd) {
min_cmd = motor_mixing.commands[i];
}
if (motor_mixing.commands[i] > max_cmd) {
max_cmd = motor_mixing.commands[i];
}
}
if (min_cmd < MOTOR_MIXING_MIN_MOTOR && max_cmd > MOTOR_MIXING_MAX_MOTOR) {
motor_mixing.nb_failure++;
}
/* In case of both min and max saturation, only lower the throttle
* instead of applying both. This should prevent your quad shooting up,
* but it might loose altitude in case of such a saturation failure.
*/
if (max_cmd > MOTOR_MIXING_MAX_MOTOR) {
int32_t saturation_offset = MOTOR_MIXING_MAX_MOTOR - max_cmd;
BoundAbs(saturation_offset, MOTOR_MIXING_MAX_SATURATION_OFFSET);
offset_commands(saturation_offset);
motor_mixing.nb_saturation++;
} else if (min_cmd < MOTOR_MIXING_MIN_MOTOR) {
int32_t saturation_offset = MOTOR_MIXING_MIN_MOTOR - min_cmd;
BoundAbs(saturation_offset, MOTOR_MIXING_MAX_SATURATION_OFFSET);
offset_commands(saturation_offset);
motor_mixing.nb_saturation++;
}
/* For testing motor failure */
if (motors_on && override_on) {
for (i = 0; i < MOTOR_MIXING_NB_MOTOR; i++) {
if (motor_mixing.override_enabled[i]) {
motor_mixing.commands[i] = motor_mixing.override_value[i];
}
}
}
bound_commands();
bound_commands_step();
} else {
for (i = 0; i < MOTOR_MIXING_NB_MOTOR; i++) {
motor_mixing.commands[i] = MOTOR_MIXING_STOP_MOTOR;
}
}
}
