示例#1
0
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;
    }
  }
}
示例#2
0
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;
    }
  }
}
示例#3
0
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;
    }
  }
}