C++ BoundAbs函数代码示例

void v_ctl_altitude_loop( void )
{
  // Airspeed Command Saturation
  if (v_ctl_auto_airspeed_setpoint <= STALL_AIRSPEED * 1.23) v_ctl_auto_airspeed_setpoint = STALL_AIRSPEED * 1.23;

  // Altitude Controller
  v_ctl_altitude_error = v_ctl_altitude_setpoint - stateGetPositionUtm_f()->alt;
  float sp = v_ctl_altitude_pgain * v_ctl_altitude_error + v_ctl_altitude_pre_climb ;

  // Vertical Speed Limiter
  BoundAbs(sp, v_ctl_max_climb);

  // Vertical Acceleration Limiter
  float incr = sp - v_ctl_climb_setpoint;
  BoundAbs(incr, 2 * dt_navigation);
  v_ctl_climb_setpoint += incr;
}

inline static void v_ctl_climb_auto_pitch_loop(void) {
  float err  = estimator_z_dot - v_ctl_climb_setpoint;
  v_ctl_throttle_setpoint = nav_throttle_setpoint;
  v_ctl_auto_pitch_sum_err += err;
  BoundAbs(v_ctl_auto_pitch_sum_err, V_CTL_AUTO_PITCH_MAX_SUM_ERR);
  nav_pitch = v_ctl_auto_pitch_pgain *
    (err + v_ctl_auto_pitch_igain * v_ctl_auto_pitch_sum_err);
  Bound(nav_pitch, V_CTL_AUTO_PITCH_MIN_PITCH, V_CTL_AUTO_PITCH_MAX_PITCH);
}

//static inline void fly_to_xy(float x, float y) {
void fly_to_xy(float x, float y) {
    desired_x = x;
    desired_y = y;
    if (nav_mode == NAV_MODE_COURSE) {
        h_ctl_course_setpoint = atan2(x - estimator_x, y - estimator_y);
        if (h_ctl_course_setpoint < 0.)
            h_ctl_course_setpoint += 2 * M_PI;
        lateral_mode = LATERAL_MODE_COURSE;
    } else {
        float diff = atan2(x - estimator_x, y - estimator_y) - estimator_hspeed_dir;
        NormRadAngle(diff);
        BoundAbs(diff,M_PI/2.);
        float s = sin(diff);
        h_ctl_roll_setpoint = atan(2 * estimator_hspeed_mod*estimator_hspeed_mod * s * (-h_ctl_course_pgain) / (CARROT * NOMINAL_AIRSPEED * 9.81) );
        BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint);
        lateral_mode = LATERAL_MODE_ROLL;
    }
}

inline static void v_ctl_climb_auto_throttle_loop(void) {
  float f_throttle = 0;
  float controlled_throttle;
  float v_ctl_pitch_of_vz;

  // Limit rate of change of climb setpoint (to ensure that airspeed loop can catch-up)
  static float v_ctl_climb_setpoint_last = 0;
  float diff_climb = v_ctl_climb_setpoint - v_ctl_climb_setpoint_last;
  Bound(diff_climb, -V_CTL_AUTO_CLIMB_LIMIT, V_CTL_AUTO_CLIMB_LIMIT);
  v_ctl_climb_setpoint = v_ctl_climb_setpoint_last + diff_climb;
  v_ctl_climb_setpoint_last = v_ctl_climb_setpoint;

  // Pitch control (input: rate of climb error, output: pitch setpoint)
  float err  = estimator_z_dot - v_ctl_climb_setpoint;
  v_ctl_auto_pitch_sum_err += err;
  BoundAbs(v_ctl_auto_pitch_sum_err, V_CTL_AUTO_PITCH_MAX_SUM_ERR);
  v_ctl_pitch_of_vz = v_ctl_auto_pitch_pgain *
    (err + v_ctl_auto_pitch_igain * v_ctl_auto_pitch_sum_err);

  // Ground speed control loop (input: groundspeed error, output: airspeed controlled)
  float err_groundspeed = (v_ctl_auto_groundspeed_setpoint - estimator_hspeed_mod);
  v_ctl_auto_groundspeed_sum_err += err_groundspeed;
  BoundAbs(v_ctl_auto_groundspeed_sum_err, V_CTL_AUTO_GROUNDSPEED_MAX_SUM_ERR);
  v_ctl_auto_airspeed_controlled = (err_groundspeed + v_ctl_auto_groundspeed_sum_err * v_ctl_auto_groundspeed_igain) * v_ctl_auto_groundspeed_pgain;

  // Do not allow controlled airspeed below the setpoint
  if (v_ctl_auto_airspeed_controlled < v_ctl_auto_airspeed_setpoint) {
    v_ctl_auto_airspeed_controlled = v_ctl_auto_airspeed_setpoint;
    v_ctl_auto_groundspeed_sum_err = v_ctl_auto_airspeed_controlled/(v_ctl_auto_groundspeed_pgain*v_ctl_auto_groundspeed_igain); // reset integrator of ground speed loop
  }

  // Airspeed control loop (input: airspeed controlled, output: throttle controlled)
  float err_airspeed = (v_ctl_auto_airspeed_controlled - estimator_airspeed);
  v_ctl_auto_airspeed_sum_err += err_airspeed;
  BoundAbs(v_ctl_auto_airspeed_sum_err, V_CTL_AUTO_AIRSPEED_MAX_SUM_ERR);
  controlled_throttle = (err_airspeed + v_ctl_auto_airspeed_sum_err * v_ctl_auto_airspeed_igain) * v_ctl_auto_airspeed_pgain;

  // Done, set outputs
  Bound(controlled_throttle, 0, V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE);
  f_throttle = controlled_throttle;
  nav_pitch = v_ctl_pitch_of_vz;
  v_ctl_throttle_setpoint = TRIM_UPPRZ(f_throttle * MAX_PPRZ);
  Bound(nav_pitch, V_CTL_AUTO_PITCH_MIN_PITCH, V_CTL_AUTO_PITCH_MAX_PITCH);
}

void v_ctl_landing_loop(void)
{
#if CTRL_VERTICAL_LANDING
  static float land_speed_i_err;
  static float land_alt_i_err;
  static float kill_alt;
  float land_speed_err = v_ctl_landing_desired_speed - stateGetHorizontalSpeedNorm_f();
  float land_alt_err = v_ctl_altitude_setpoint - stateGetPositionUtm_f()->alt;

  if (kill_throttle
      && (kill_alt - v_ctl_altitude_setpoint)
          > (v_ctl_landing_alt_throttle_kill - v_ctl_landing_alt_flare)) {
    v_ctl_throttle_setpoint = 0.0;  // Throttle is already in KILL (command redundancy)
    nav_pitch = v_ctl_landing_pitch_flare;  // desired final flare pitch
    lateral_mode = LATERAL_MODE_ROLL;  //override course correction during flare - eliminate possibility of catching wing tip due to course correction
    h_ctl_roll_setpoint = 0.0;  // command zero roll during flare
  } else {
    // set throttle based on altitude error
    v_ctl_throttle_setpoint = land_alt_err * v_ctl_landing_throttle_pgain
        + land_alt_i_err * v_ctl_landing_throttle_igain;
    Bound(v_ctl_throttle_setpoint, 0, v_ctl_landing_throttle_max * MAX_PPRZ);  // cut off throttle cmd at specified MAX

    land_alt_i_err += land_alt_err / CONTROL_FREQUENCY;  // integrator land_alt_err, divide by control frequency
    BoundAbs(land_alt_i_err, 50);  // integrator sat limits

    // set pitch based on ground speed error
    nav_pitch -= land_speed_err * v_ctl_landing_pitch_pgain / 1000
        + land_speed_i_err * v_ctl_landing_pitch_igain / 1000;  // 1000 is a multiplier to get more reasonable gains for ctl_basic
    Bound(nav_pitch, -v_ctl_landing_pitch_limits, v_ctl_landing_pitch_limits);  //max pitch authority for landing

    land_speed_i_err += land_speed_err / CONTROL_FREQUENCY;  // integrator land_speed_err, divide by control frequency
    BoundAbs(land_speed_i_err, .2);  // integrator sat limits

    // update kill_alt until final kill throttle is initiated - allows for mode switch to first part of if statement above
    // eliminates the need for knowing the altitude of TD
    if (!kill_throttle) {
      kill_alt = v_ctl_altitude_setpoint;  //
      if (land_alt_err > 0.0) {
        nav_pitch = 0.01;  //  if below desired alt close to ground command level pitch
      }
    }
  }
#endif /* CTRL_VERTICAL_LANDING */
}

inline static void v_ctl_climb_auto_pitch_loop(void)
{
  float err  = stateGetSpeedEnu_f()->z - v_ctl_climb_setpoint;
  v_ctl_throttle_setpoint = nav_throttle_setpoint;
  v_ctl_auto_pitch_sum_err += err;
  BoundAbs(v_ctl_auto_pitch_sum_err, V_CTL_AUTO_PITCH_MAX_SUM_ERR);
  v_ctl_pitch_setpoint = v_ctl_pitch_trim - v_ctl_auto_pitch_pgain *
                         (err + v_ctl_auto_pitch_igain * v_ctl_auto_pitch_sum_err);
  Bound(v_ctl_pitch_setpoint, V_CTL_AUTO_PITCH_MIN_PITCH, V_CTL_AUTO_PITCH_MAX_PITCH);
}

//static inline void fly_to_xy(float x, float y) {
void fly_to_xy(float x, float y) {
  struct EnuCoor_f* pos = stateGetPositionEnu_f();
  desired_x = x;
  desired_y = y;
  if (nav_mode == NAV_MODE_COURSE) {
    h_ctl_course_setpoint = atan2f(x - pos->x, y - pos->y);
    if (h_ctl_course_setpoint < 0.)
      h_ctl_course_setpoint += 2 * M_PI;
    lateral_mode = LATERAL_MODE_COURSE;
  } else {
    float diff = atan2f(x - pos->x, y - pos->y) - (*stateGetHorizontalSpeedDir_f());
    NormRadAngle(diff);
    BoundAbs(diff,M_PI/2.);
    float s = sinf(diff);
    float speed = *stateGetHorizontalSpeedNorm_f();
    h_ctl_roll_setpoint = atanf(2 * speed * speed * s * h_ctl_course_pgain / (CARROT * NOMINAL_AIRSPEED * 9.81) );
    BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint);
    lateral_mode = LATERAL_MODE_ROLL;
  }
}

void vi_update_wp(uint8_t wp_id)
{
  struct Int16Vect3 wp_speed;
  wp_speed.x = ViMaxHSpeed * vi.input.h_sp.speed.x / 128;
  wp_speed.y = ViMaxHSpeed * vi.input.h_sp.speed.y / 128;
  wp_speed.z = ViMaxVSpeed * vi.input.v_sp.climb / 128;
  VECT3_BOUND_BOX(wp_speed, wp_speed, wp_speed_max);
  int16_t heading_rate = vi.input.h_sp.speed.z;
  BoundAbs(heading_rate, ViMaxHeadingRate);
  navigation_update_wp_from_speed(wp_id , wp_speed, heading_rate);

}

void v_ctl_altitude_loop( void ) {
  static float v_ctl_climb_setpoint_last = 0.;
  //static float last_lead_input = 0.;

  // Altitude error
  v_ctl_altitude_error = v_ctl_altitude_setpoint - estimator_z;
  v_ctl_climb_setpoint = v_ctl_altitude_pgain * v_ctl_altitude_error + v_ctl_altitude_pre_climb;

  // Lead controller
  //v_ctl_climb_setpoint = ((LEAD_CTRL_TAU*LEAD_CTRL_A + LEAD_CTRL_Te)*climb_sp + LEAD_CTRL_TAU*(v_ctl_climb_setpoint - LEAD_CTRL_A*last_lead_input)) / (LEAD_CTRL_TAU + LEAD_CTRL_Te);
  //last_lead_input = pitch_sp;

  // Limit rate of change of climb setpoint
  float diff_climb = v_ctl_climb_setpoint - v_ctl_climb_setpoint_last;
  BoundAbs(diff_climb, V_CTL_AUTO_CLIMB_LIMIT);
  v_ctl_climb_setpoint = v_ctl_climb_setpoint_last + diff_climb;

  // Limit climb setpoint
  BoundAbs(v_ctl_climb_setpoint, V_CTL_ALTITUDE_MAX_CLIMB);
  v_ctl_climb_setpoint_last = v_ctl_climb_setpoint;
}

inline static void h_ctl_yaw_loop(void)
{

#if H_CTL_YAW_TRIM_NY
  // Actual Acceleration from IMU:
#if (!defined SITL || defined USE_NPS)
  struct Int32Vect3 accel_meas_body, accel_ned;
  struct Int32RMat *ned_to_body_rmat = stateGetNedToBodyRMat_i();
  struct NedCoor_i *accel_tmp = stateGetAccelNed_i();
  VECT3_COPY(accel_ned, (*accel_tmp));
  accel_ned.z -= ACCEL_BFP_OF_REAL(9.81f);
  int32_rmat_vmult(&accel_meas_body, ned_to_body_rmat, &accel_ned);
  float ny = -ACCEL_FLOAT_OF_BFP(accel_meas_body.y) / 9.81f; // Lateral load factor (in g)
#else
  float ny = 0.f;
#endif

  if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0) {
    h_ctl_yaw_ny_sum_err = 0.;
  } else {
    if (h_ctl_yaw_ny_igain > 0.) {
      // only update when: phi<60degrees and ny<2g
      if (fabsf(stateGetNedToBodyEulers_f()->phi) < 1.05 && fabsf(ny) < 2.) {
        h_ctl_yaw_ny_sum_err += ny * H_CTL_REF_DT;
        // max half rudder deflection for trim
        BoundAbs(h_ctl_yaw_ny_sum_err, MAX_PPRZ / (2. * h_ctl_yaw_ny_igain));
      }
    } else {
      h_ctl_yaw_ny_sum_err = 0.;
    }
  }
#endif

#ifdef USE_AIRSPEED
  float Vo = stateGetAirspeed_f();
  Bound(Vo, STALL_AIRSPEED, RACE_AIRSPEED);
#else
  float Vo = NOMINAL_AIRSPEED;
#endif

  h_ctl_ref.yaw_rate = h_ctl_yaw_rate_setpoint // set by RC
                       + 9.81f / Vo * sinf(h_ctl_roll_setpoint); // for turns
  float d_err = h_ctl_ref.yaw_rate - stateGetBodyRates_f()->r;

  float cmd = + h_ctl_yaw_dgain * d_err
#if H_CTL_YAW_TRIM_NY
              + h_ctl_yaw_ny_igain * h_ctl_yaw_ny_sum_err
#endif
              ;
  cmd /= airspeed_ratio2;
  h_ctl_rudder_setpoint = TRIM_PPRZ(cmd);
}

void stabilization_indi_run(bool enable_integrator __attribute__((unused)), bool rate_control)
{
  /* attitude error                          */
  struct Int32Quat att_err;
  struct Int32Quat *att_quat = stateGetNedToBodyQuat_i();
  int32_quat_inv_comp(&att_err, att_quat, &stab_att_sp_quat);
  /* wrap it in the shortest direction       */
  int32_quat_wrap_shortest(&att_err);
  int32_quat_normalize(&att_err);

  /* compute the INDI command */
  stabilization_indi_calc_cmd(stabilization_att_indi_cmd, &att_err, rate_control);

  /* copy the INDI command */
  stabilization_cmd[COMMAND_ROLL] = stabilization_att_indi_cmd[COMMAND_ROLL];
  stabilization_cmd[COMMAND_PITCH] = stabilization_att_indi_cmd[COMMAND_PITCH];
  stabilization_cmd[COMMAND_YAW] = stabilization_att_indi_cmd[COMMAND_YAW];

  /* bound the result */
  BoundAbs(stabilization_cmd[COMMAND_ROLL], MAX_PPRZ);
  BoundAbs(stabilization_cmd[COMMAND_PITCH], MAX_PPRZ);
  BoundAbs(stabilization_cmd[COMMAND_YAW], MAX_PPRZ);
}

void stabilization_rate_run(bool_t in_flight)
{
  /* compute feed-back command */
  struct Int32Rates _error;
  struct Int32Rates *body_rate = stateGetBodyRates_i();
  RATES_DIFF(_error, stabilization_rate_sp, (*body_rate));
  if (in_flight) {
    /* update integrator */
    //divide the sum_err_increment to make sure it doesn't accumulate to the max too fast
    struct Int32Rates sum_err_increment;
    RATES_SDIV(sum_err_increment, _error, 5);
    RATES_ADD(stabilization_rate_sum_err, sum_err_increment);
    RATES_BOUND_CUBE(stabilization_rate_sum_err, -MAX_SUM_ERR, MAX_SUM_ERR);
  } else {
    INT_RATES_ZERO(stabilization_rate_sum_err);
  }

  /* PI */
  stabilization_rate_fb_cmd.p = stabilization_rate_gain.p * _error.p +
                                OFFSET_AND_ROUND2((stabilization_rate_igain.p  * stabilization_rate_sum_err.p), 6);

  stabilization_rate_fb_cmd.q = stabilization_rate_gain.q * _error.q +
                                OFFSET_AND_ROUND2((stabilization_rate_igain.q  * stabilization_rate_sum_err.q), 6);

  stabilization_rate_fb_cmd.r = stabilization_rate_gain.r * _error.r +
                                OFFSET_AND_ROUND2((stabilization_rate_igain.r  * stabilization_rate_sum_err.r), 6);

  stabilization_cmd[COMMAND_ROLL]  = stabilization_rate_fb_cmd.p >> 11;
  stabilization_cmd[COMMAND_PITCH] = stabilization_rate_fb_cmd.q >> 11;
  stabilization_cmd[COMMAND_YAW]   = stabilization_rate_fb_cmd.r >> 11;

  /* bound the result */
  BoundAbs(stabilization_cmd[COMMAND_ROLL], MAX_PPRZ);
  BoundAbs(stabilization_cmd[COMMAND_PITCH], MAX_PPRZ);
  BoundAbs(stabilization_cmd[COMMAND_YAW], MAX_PPRZ);

}

/**
 * \brief
 *
 */
void h_ctl_course_loop ( void ) {
  static float last_err;

  // Ground path error
  float err = h_ctl_course_setpoint - (*stateGetHorizontalSpeedDir_f());
  NormRadAngle(err);

  float d_err = err - last_err;
  last_err = err;
  NormRadAngle(d_err);

  float speed_depend_nav = (*stateGetHorizontalSpeedNorm_f())/NOMINAL_AIRSPEED;
  Bound(speed_depend_nav, 0.66, 1.5);

  h_ctl_roll_setpoint = h_ctl_course_pre_bank_correction * h_ctl_course_pre_bank
    + h_ctl_course_pgain * speed_depend_nav * err
    + h_ctl_course_dgain * d_err;

  BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint);
}

/**
 * \brief
 *
 */
void h_ctl_course_loop ( void ) {
  static float last_err;

  // Ground path error
  float err = estimator_hspeed_dir - h_ctl_course_setpoint;
  NormRadAngle(err);

  float d_err = err - last_err;
  last_err = err;
  NormRadAngle(d_err);

  float speed_depend_nav = estimator_hspeed_mod/NOMINAL_AIRSPEED;
  Bound(speed_depend_nav, 0.66, 1.5);

  h_ctl_roll_setpoint = h_ctl_course_pre_bank_correction * h_ctl_course_pre_bank
    + h_ctl_course_pgain * speed_depend_nav * err
    + h_ctl_course_dgain * d_err;

  BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint);
}

/** Computes h_ctl_aileron_setpoint from h_ctl_roll_setpoint */
inline static void h_ctl_roll_loop( void ) {
  float err = estimator_phi - h_ctl_roll_setpoint;
  float cmd = h_ctl_roll_pgain * err
    + v_ctl_throttle_setpoint * h_ctl_aileron_of_throttle;
  h_ctl_aileron_setpoint = TRIM_PPRZ(cmd);

#ifdef H_CTL_RATE_LOOP
  if (h_ctl_auto1_rate) {
    /** Runs only the roll rate loop */
    h_ctl_roll_rate_setpoint = h_ctl_roll_setpoint * 10.;
    h_ctl_roll_rate_loop();
  } else {
    h_ctl_roll_rate_setpoint = h_ctl_roll_rate_setpoint_pgain * err;
    BoundAbs(h_ctl_roll_rate_setpoint, H_CTL_ROLL_RATE_MAX_SETPOINT);

    float saved_aileron_setpoint = h_ctl_aileron_setpoint;
    h_ctl_roll_rate_loop();
    h_ctl_aileron_setpoint = Blend(h_ctl_aileron_setpoint, saved_aileron_setpoint, h_ctl_roll_rate_mode) ;
  }
#endif
}