void balance_control()

in src/balancer.c [173:341]

• 55 lines of code
• 2 McCabe index (conditional complexity)

void balance_control(float args_cmd_forward, float args_cmd_turn, float
                     args_gyro, float args_gyro_offset, float
                     args_theta_m_l, float args_theta_m_r, float
                     args_battery, signed char *ret_pwm_l, signed char *ret_pwm_r)
{
  {
    float tmp_theta;
    float tmp_theta_lpf;
    float tmp_pwm_r_limiter;
    float tmp_psidot;
    float tmp_pwm_turn;
    float tmp_pwm_l_limiter;
    float tmp_thetadot_cmd_lpf;
    float tmp[4];
    float tmp_theta_0[4];
    long tmp_0;

    /* Sum: '<S8>/Sum' incorporates:
     *  Constant: '<S3>/Constant6'
     *  Constant: '<S8>/Constant'
     *  Constant: '<S8>/Constant1'
     *  Gain: '<S3>/Gain1'
     *  Gain: '<S8>/Gain2'
     *  Inport: '<Root>/cmd_forward'
     *  Product: '<S3>/Divide'
     *  Product: '<S8>/Product'
     *  Sum: '<S8>/Sum1'
     *  UnitDelay: '<S8>/Unit Delay'
     */
    tmp_thetadot_cmd_lpf = (((args_cmd_forward / CMD_MAX) * K_THETADOT) * (1.0F
      - A_R)) + (A_R * ud_thetadot_cmd_lpf);

    /* Gain: '<S4>/Gain' incorporates:
     *  Gain: '<S4>/deg2rad'
     *  Gain: '<S4>/deg2rad1'
     *  Inport: '<Root>/theta_m_l'
     *  Inport: '<Root>/theta_m_r'
     *  Sum: '<S4>/Sum1'
     *  Sum: '<S4>/Sum4'
     *  Sum: '<S4>/Sum6'
     *  UnitDelay: '<S10>/Unit Delay'
     */
    tmp_theta = (((DEG2RAD * args_theta_m_l) + ud_psi) + ((DEG2RAD *
      args_theta_m_r) + ud_psi)) * 0.5F;

    /* Sum: '<S11>/Sum' incorporates:
     *  Constant: '<S11>/Constant'
     *  Constant: '<S11>/Constant1'
     *  Gain: '<S11>/Gain2'
     *  Product: '<S11>/Product'
     *  Sum: '<S11>/Sum1'
     *  UnitDelay: '<S11>/Unit Delay'
     */
    tmp_theta_lpf = ((1.0F - A_D) * tmp_theta) + (A_D * ud_theta_lpf);

    /* Gain: '<S4>/deg2rad2' incorporates:
     *  Inport: '<Root>/gyro'
     *  Inport: '<Root>/gyro_offset'
     *  Sum: '<S4>/Sum2'
     */
    tmp_psidot = (args_gyro - args_gyro_offset) * DEG2RAD;

    /* Gain: '<S2>/Gain' incorporates:
     *  Constant: '<S3>/Constant2'
     *  Constant: '<S3>/Constant3'
     *  Constant: '<S6>/Constant'
     *  Constant: '<S9>/Constant'
     *  Gain: '<S1>/FeedbackGain'
     *  Gain: '<S1>/IntegralGain'
     *  Gain: '<S6>/Gain3'
     *  Inport: '<Root>/battery'
     *  Product: '<S2>/Product'
     *  Product: '<S9>/Product'
     *  Sum: '<S1>/Sum2'
     *  Sum: '<S1>/sum_err'
     *  Sum: '<S6>/Sum2'
     *  Sum: '<S9>/Sum'
     *  UnitDelay: '<S10>/Unit Delay'
     *  UnitDelay: '<S11>/Unit Delay'
     *  UnitDelay: '<S5>/Unit Delay'
     *  UnitDelay: '<S7>/Unit Delay'
     */
    tmp[0] = ud_theta_ref;
    tmp[1] = 0.0F;
    tmp[2] = tmp_thetadot_cmd_lpf;
    tmp[3] = 0.0F;
    tmp_theta_0[0] = tmp_theta;
    tmp_theta_0[1] = ud_psi;
    tmp_theta_0[2] = (tmp_theta_lpf - ud_theta_lpf) / EXEC_PERIOD;
    tmp_theta_0[3] = tmp_psidot;
    tmp_pwm_r_limiter = 0.0F;
    for (tmp_0 = 0; tmp_0 < 4; tmp_0++) {
      tmp_pwm_r_limiter += (tmp[tmp_0] - tmp_theta_0[tmp_0]) * K_F[(tmp_0)];
    }

    tmp_pwm_r_limiter = (((K_I * ud_err_theta) + tmp_pwm_r_limiter) /
                         ((BATTERY_GAIN * args_battery) - BATTERY_OFFSET)) *
      100.0F;

    /* Gain: '<S3>/Gain2' incorporates:
     *  Constant: '<S3>/Constant1'
     *  Inport: '<Root>/cmd_turn'
     *  Product: '<S3>/Divide1'
     */
    tmp_pwm_turn = (args_cmd_turn / CMD_MAX) * K_PHIDOT;

    /* Sum: '<S2>/Sum' */
    tmp_pwm_l_limiter = tmp_pwm_r_limiter + tmp_pwm_turn;

    /* Saturate: '<S2>/pwm_l_limiter' */
    tmp_pwm_l_limiter = rt_SATURATE(tmp_pwm_l_limiter, -100.0F, 100.0F);

    /* Outport: '<Root>/pwm_l' incorporates:
     *  DataTypeConversion: '<S1>/Data Type Conversion'
     */
    (*ret_pwm_l) = (signed char)tmp_pwm_l_limiter;

    /* Sum: '<S2>/Sum1' */
    tmp_pwm_r_limiter -= tmp_pwm_turn;

    /* Saturate: '<S2>/pwm_r_limiter' */
    tmp_pwm_r_limiter = rt_SATURATE(tmp_pwm_r_limiter, -100.0F, 100.0F);

    /* Outport: '<Root>/pwm_r' incorporates:
     *  DataTypeConversion: '<S1>/Data Type Conversion6'
     */
    (*ret_pwm_r) = (signed char)tmp_pwm_r_limiter;

    /* Sum: '<S7>/Sum' incorporates:
     *  Gain: '<S7>/Gain'
     *  UnitDelay: '<S7>/Unit Delay'
     */
    tmp_pwm_l_limiter = (EXEC_PERIOD * tmp_thetadot_cmd_lpf) + ud_theta_ref;

    /* Sum: '<S10>/Sum' incorporates:
     *  Gain: '<S10>/Gain'
     *  UnitDelay: '<S10>/Unit Delay'
     */
    tmp_pwm_turn = (EXEC_PERIOD * tmp_psidot) + ud_psi;

    /* Sum: '<S5>/Sum' incorporates:
     *  Gain: '<S5>/Gain'
     *  Sum: '<S1>/Sum1'
     *  UnitDelay: '<S5>/Unit Delay'
     *  UnitDelay: '<S7>/Unit Delay'
     */
    tmp_pwm_r_limiter = ((ud_theta_ref - tmp_theta) * EXEC_PERIOD) +
      ud_err_theta;

    /* user code (Update function Body) */
    /* System '<Root>' */
    /* 次回演算用状態量保存処理 */

    /* Update for UnitDelay: '<S5>/Unit Delay' */
    ud_err_theta = tmp_pwm_r_limiter;

    /* Update for UnitDelay: '<S7>/Unit Delay' */
    ud_theta_ref = tmp_pwm_l_limiter;

    /* Update for UnitDelay: '<S8>/Unit Delay' */
    ud_thetadot_cmd_lpf = tmp_thetadot_cmd_lpf;

    /* Update for UnitDelay: '<S10>/Unit Delay' */
    ud_psi = tmp_pwm_turn;

    /* Update for UnitDelay: '<S11>/Unit Delay' */
    ud_theta_lpf = tmp_theta_lpf;
  }
}