自学教程：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}

static inline void v_ctl_set_pitch ( void ) {  static float last_err = 0.;  if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0)    v_ctl_auto_pitch_sum_err = 0;  // Compute errors  float err = v_ctl_climb_setpoint - stateGetSpeedEnu_f()->z;  float d_err = err - last_err;  last_err = err;  if (v_ctl_auto_pitch_igain > 0.) {    v_ctl_auto_pitch_sum_err += err*(1./60.);    BoundAbs(v_ctl_auto_pitch_sum_err, V_CTL_AUTO_PITCH_MAX_SUM_ERR / v_ctl_auto_pitch_igain);  }  // PI loop + feedforward ctl  v_ctl_pitch_setpoint = nav_pitch    + v_ctl_auto_throttle_pitch_of_vz_pgain * v_ctl_climb_setpoint    + v_ctl_auto_pitch_pgain * err    + v_ctl_auto_pitch_dgain * d_err    + v_ctl_auto_pitch_igain * v_ctl_auto_pitch_sum_err;}

static inline void v_ctl_set_throttle( void ) {  static float last_err = 0.;  if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0)    v_ctl_auto_throttle_sum_err = 0;  // Compute errors  float err =  v_ctl_climb_setpoint - estimator_z_dot;  float d_err = err - last_err;  last_err = err;  if (v_ctl_auto_throttle_igain > 0.) {    v_ctl_auto_throttle_sum_err += err*(1./60.);    BoundAbs(v_ctl_auto_throttle_sum_err, V_CTL_AUTO_THROTTLE_MAX_SUM_ERR / v_ctl_auto_throttle_igain);  }  // PID loop + feedforward ctl  controlled_throttle = v_ctl_auto_throttle_cruise_throttle    + v_ctl_auto_throttle_climb_throttle_increment * v_ctl_climb_setpoint    + v_ctl_auto_throttle_pgain * err    + v_ctl_auto_throttle_dgain * d_err    + v_ctl_auto_throttle_igain * v_ctl_auto_throttle_sum_err;}

static inline void v_ctl_set_pitch ( void ) {  static float last_err = 0.;  if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0)    v_ctl_auto_pitch_sum_err = 0;  // Compute errors  float err = v_ctl_climb_setpoint - estimator_z_dot;  float d_err = err - last_err;  last_err = err;  if (v_ctl_auto_pitch_igain > 0.) {    v_ctl_auto_pitch_sum_err += err*(1./60.);    BoundAbs(v_ctl_auto_pitch_sum_err, V_CTL_AUTO_PITCH_MAX_SUM_ERR / v_ctl_auto_pitch_igain);  }  // PI loop + feedforward ctl  nav_pitch = 0. //nav_pitch FIXME it really sucks !    + v_ctl_auto_throttle_pitch_of_vz_pgain * v_ctl_climb_setpoint    + v_ctl_auto_pitch_pgain * err    + v_ctl_auto_pitch_dgain * d_err    + v_ctl_auto_pitch_igain * v_ctl_auto_pitch_sum_err;}

inline static void h_ctl_roll_loop( void ) {  static float cmd_fb = 0.;#if USE_ANGLE_REF  // Update reference setpoints for roll  h_ctl_ref.roll_angle += h_ctl_ref.roll_rate * H_CTL_REF_DT;  h_ctl_ref.roll_rate += h_ctl_ref.roll_accel * H_CTL_REF_DT;  h_ctl_ref.roll_accel = H_CTL_REF_W_P*H_CTL_REF_W_P * (h_ctl_roll_setpoint - h_ctl_ref.roll_angle) - 2*H_CTL_REF_XI_P*H_CTL_REF_W_P * h_ctl_ref.roll_rate;  // Saturation on references  BoundAbs(h_ctl_ref.roll_accel, h_ctl_ref.max_p_dot);  if (h_ctl_ref.roll_rate > h_ctl_ref.max_p) {    h_ctl_ref.roll_rate = h_ctl_ref.max_p;    if (h_ctl_ref.roll_accel > 0.) h_ctl_ref.roll_accel = 0.;  }  else if (h_ctl_ref.roll_rate < - h_ctl_ref.max_p) {    h_ctl_ref.roll_rate = - h_ctl_ref.max_p;    if (h_ctl_ref.roll_accel < 0.) h_ctl_ref.roll_accel = 0.;  }#else  h_ctl_ref.roll_angle = h_ctl_roll_setpoint;  h_ctl_ref.roll_rate = 0.;  h_ctl_ref.roll_accel = 0.;#endif#ifdef USE_KFF_UPDATE  // update Kff gains  h_ctl_roll_Kffa += KFFA_UPDATE * h_ctl_ref.roll_accel * cmd_fb / (h_ctl_ref.max_p_dot*h_ctl_ref.max_p_dot);  h_ctl_roll_Kffd += KFFD_UPDATE * h_ctl_ref.roll_rate  * cmd_fb / (h_ctl_ref.max_p*h_ctl_ref.max_p);#ifdef SITL  printf("%f %f %f/n", h_ctl_roll_Kffa, h_ctl_roll_Kffd, cmd_fb);#endif  h_ctl_roll_Kffa = Max(h_ctl_roll_Kffa, 0);  h_ctl_roll_Kffd = Max(h_ctl_roll_Kffd, 0);#endif  // Compute errors  float err = h_ctl_ref.roll_angle - stateGetNedToBodyEulers_f()->phi;  struct FloatRates* body_rate = stateGetBodyRates_f();  float d_err = h_ctl_ref.roll_rate - body_rate->p;  if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0) {    h_ctl_roll_sum_err = 0.;  }  else {    if (h_ctl_roll_igain > 0.) {      h_ctl_roll_sum_err += err * H_CTL_REF_DT;      BoundAbs(h_ctl_roll_sum_err, H_CTL_ROLL_SUM_ERR_MAX / h_ctl_roll_igain);    } else {      h_ctl_roll_sum_err = 0.;    }  }  cmd_fb = h_ctl_roll_attitude_gain * err;  float cmd = - h_ctl_roll_Kffa * h_ctl_ref.roll_accel    - h_ctl_roll_Kffd * h_ctl_ref.roll_rate    - cmd_fb    - h_ctl_roll_rate_gain * d_err    - h_ctl_roll_igain * h_ctl_roll_sum_err    + v_ctl_throttle_setpoint * h_ctl_aileron_of_throttle;  cmd /= airspeed_ratio2;  // Set aileron commands  h_ctl_aileron_setpoint = TRIM_PPRZ(cmd);}

// Airspeed control loop (input: [airspeed controlled, climb_setpoint], output: [throttle controlled, pitch setpoint])static inline void v_ctl_set_airspeed( void ) {  static float last_err_vz = 0.;  static float last_err_as = 0.;  // Bound airspeed setpoint  Bound(v_ctl_auto_airspeed_setpoint, V_CTL_AIRSPEED_MIN, V_CTL_AIRSPEED_MAX);  // Compute errors  float err_vz = v_ctl_climb_setpoint - estimator_z_dot;  float d_err_vz = (err_vz - last_err_vz)*AIRSPEED_LOOP_PERIOD;  last_err_vz = err_vz;  if (v_ctl_auto_throttle_igain > 0.) {    v_ctl_auto_throttle_sum_err += err_vz*AIRSPEED_LOOP_PERIOD;    BoundAbs(v_ctl_auto_throttle_sum_err, V_CTL_AUTO_THROTTLE_MAX_SUM_ERR / v_ctl_auto_throttle_igain);  }  if (v_ctl_auto_pitch_igain > 0.) {    v_ctl_auto_pitch_sum_err += err_vz*AIRSPEED_LOOP_PERIOD;    BoundAbs(v_ctl_auto_pitch_sum_err, V_CTL_AUTO_PITCH_MAX_SUM_ERR / v_ctl_auto_pitch_igain);  }  float err_airspeed = v_ctl_auto_airspeed_setpoint - estimator_airspeed;  float d_err_airspeed = (err_airspeed - last_err_as)*AIRSPEED_LOOP_PERIOD;  last_err_as = err_airspeed;  if (v_ctl_auto_airspeed_throttle_igain > 0.) {    v_ctl_auto_airspeed_throttle_sum_err += err_airspeed*AIRSPEED_LOOP_PERIOD;    BoundAbs(v_ctl_auto_airspeed_throttle_sum_err, V_CTL_AUTO_AIRSPEED_THROTTLE_MAX_SUM_ERR / v_ctl_auto_airspeed_throttle_igain);  }  if (v_ctl_auto_airspeed_pitch_igain > 0.) {    v_ctl_auto_airspeed_pitch_sum_err += err_airspeed*AIRSPEED_LOOP_PERIOD;    BoundAbs(v_ctl_auto_airspeed_pitch_sum_err, V_CTL_AUTO_AIRSPEED_PITCH_MAX_SUM_ERR / v_ctl_auto_airspeed_pitch_igain);  }  // Reset integrators in manual or before flight  if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0) {    v_ctl_auto_throttle_sum_err = 0.;    v_ctl_auto_pitch_sum_err = 0.;    v_ctl_auto_airspeed_throttle_sum_err = 0.;    v_ctl_auto_airspeed_pitch_sum_err = 0.;  }  // Pitch loop  nav_pitch = 0. //nav_pitch FIXME it really sucks !    + v_ctl_auto_throttle_pitch_of_vz_pgain * v_ctl_climb_setpoint    + v_ctl_auto_pitch_pgain * err_vz    + v_ctl_auto_pitch_dgain * d_err_vz    + v_ctl_auto_pitch_igain * v_ctl_auto_pitch_sum_err    - v_ctl_auto_airspeed_pitch_pgain * err_airspeed    - v_ctl_auto_airspeed_pitch_dgain * d_err_airspeed    - v_ctl_auto_airspeed_pitch_igain * v_ctl_auto_airspeed_pitch_sum_err;  // Throttle loop  controlled_throttle = v_ctl_auto_throttle_cruise_throttle    + v_ctl_auto_throttle_climb_throttle_increment * v_ctl_climb_setpoint    + v_ctl_auto_throttle_pgain * err_vz    + v_ctl_auto_throttle_dgain * d_err_vz    + v_ctl_auto_throttle_igain * v_ctl_auto_throttle_sum_err    + v_ctl_auto_airspeed_throttle_pgain * err_airspeed    + v_ctl_auto_airspeed_throttle_dgain * d_err_airspeed    + v_ctl_auto_airspeed_throttle_igain * v_ctl_auto_airspeed_throttle_sum_err;}

inline static void v_ctl_climb_auto_throttle_loop(void){  static float last_err;  float f_throttle = 0;  float err  = stateGetSpeedEnu_f()->z - v_ctl_climb_setpoint;  float d_err = err - last_err;  last_err = err;  float controlled_throttle = v_ctl_auto_throttle_cruise_throttle                              + v_ctl_auto_throttle_climb_throttle_increment * v_ctl_climb_setpoint                              - v_ctl_auto_throttle_pgain *                              (err + v_ctl_auto_throttle_igain * v_ctl_auto_throttle_sum_err                               + v_ctl_auto_throttle_dgain * d_err);  /* pitch pre-command */  float v_ctl_pitch_of_vz = (v_ctl_climb_setpoint + d_err * v_ctl_auto_throttle_pitch_of_vz_dgain) *                            v_ctl_auto_throttle_pitch_of_vz_pgain;#if defined AGR_CLIMB  switch (v_ctl_auto_throttle_submode) {    case V_CTL_AUTO_THROTTLE_AGRESSIVE:      if (v_ctl_climb_setpoint > 0) { /* Climbing */        f_throttle =  AGR_CLIMB_THROTTLE;        v_ctl_pitch_setpoint = AGR_CLIMB_PITCH;      } else { /* Going down */        f_throttle =  AGR_DESCENT_THROTTLE;        v_ctl_pitch_setpoint = AGR_DESCENT_PITCH;      }      break;    case V_CTL_AUTO_THROTTLE_BLENDED: {      float ratio = (fabs(v_ctl_altitude_error) - AGR_BLEND_END)                    / (AGR_BLEND_START - AGR_BLEND_END);      f_throttle = (1 - ratio) * controlled_throttle;      v_ctl_pitch_setpoint = (1 - ratio) * (v_ctl_pitch_of_vz + v_ctl_pitch_trim);      v_ctl_auto_throttle_sum_err += (1 - ratio) * err;      BoundAbs(v_ctl_auto_throttle_sum_err, V_CTL_AUTO_THROTTLE_MAX_SUM_ERR);      /* positive error -> too low */      if (v_ctl_altitude_error > 0) {        f_throttle +=  ratio * AGR_CLIMB_THROTTLE;        v_ctl_pitch_setpoint += ratio * AGR_CLIMB_PITCH;      } else {        f_throttle += ratio * AGR_DESCENT_THROTTLE;        v_ctl_pitch_setpoint += ratio * AGR_DESCENT_PITCH;      }      break;    }    case V_CTL_AUTO_THROTTLE_STANDARD:    default:#endif      f_throttle = controlled_throttle;      v_ctl_auto_throttle_sum_err += err;      BoundAbs(v_ctl_auto_throttle_sum_err, V_CTL_AUTO_THROTTLE_MAX_SUM_ERR);      v_ctl_pitch_setpoint = v_ctl_pitch_of_vz + v_ctl_pitch_trim + nav_pitch;#if defined AGR_CLIMB      break;  } /* switch submode */#endif  v_ctl_throttle_setpoint = TRIM_UPPRZ(f_throttle * MAX_PPRZ);}

static inline void stabilization_indi_calc_cmd(int32_t indi_commands[], struct Int32Quat *att_err, bool rate_control){  /* Propagate the second order filter on the gyroscopes */  struct FloatRates *body_rates = stateGetBodyRates_f();  stabilization_indi_second_order_filter(&indi.rate, body_rates);  //The rates used for feedback are by default the measured rates. If needed they can be filtered (see below)  struct FloatRates rates_for_feedback;  RATES_COPY(rates_for_feedback, (*body_rates));  //If there is a lot of noise on the gyroscope, it might be good to use the filtered value for feedback.  //Note that due to the delay, the PD controller can not be as aggressive.#if STABILIZATION_INDI_FILTER_ROLL_RATE  rates_for_feedback.p = indi.rate.x.p;#endif#if STABILIZATION_INDI_FILTER_PITCH_RATE  rates_for_feedback.q = indi.rate.x.q;#endif#if STABILIZATION_INDI_FILTER_YAW_RATE  rates_for_feedback.r = indi.rate.x.r;#endif  indi.angular_accel_ref.p = indi.reference_acceleration.err_p * QUAT1_FLOAT_OF_BFP(att_err->qx)                             - indi.reference_acceleration.rate_p * rates_for_feedback.p;  indi.angular_accel_ref.q = indi.reference_acceleration.err_q * QUAT1_FLOAT_OF_BFP(att_err->qy)                             - indi.reference_acceleration.rate_q * rates_for_feedback.q;  //This separates the P and D controller and lets you impose a maximum yaw rate.  float rate_ref_r = indi.reference_acceleration.err_r * QUAT1_FLOAT_OF_BFP(att_err->qz)/indi.reference_acceleration.rate_r;  BoundAbs(rate_ref_r, indi.attitude_max_yaw_rate);  indi.angular_accel_ref.r = indi.reference_acceleration.rate_r * (rate_ref_r - rates_for_feedback.r);  /* Check if we are running the rate controller and overwrite */  if(rate_control) {    indi.angular_accel_ref.p =  indi.reference_acceleration.rate_p * ((float)radio_control.values[RADIO_ROLL]  / MAX_PPRZ * indi.max_rate - body_rates->p);    indi.angular_accel_ref.q =  indi.reference_acceleration.rate_q * ((float)radio_control.values[RADIO_PITCH] / MAX_PPRZ * indi.max_rate - body_rates->q);    indi.angular_accel_ref.r =  indi.reference_acceleration.rate_r * ((float)radio_control.values[RADIO_YAW]   / MAX_PPRZ * indi.max_rate - body_rates->r);  }  //Increment in angular acceleration requires increment in control input  //G1 is the control effectiveness. In the yaw axis, we need something additional: G2.  //It takes care of the angular acceleration caused by the change in rotation rate of the propellers  //(they have significant inertia, see the paper mentioned in the header for more explanation)  indi.du.p = 1.0 / indi.g1.p * (indi.angular_accel_ref.p - indi.rate.dx.p);  indi.du.q = 1.0 / indi.g1.q * (indi.angular_accel_ref.q - indi.rate.dx.q);  indi.du.r = 1.0 / (indi.g1.r + indi.g2) * (indi.angular_accel_ref.r - indi.rate.dx.r + indi.g2 * indi.du.r);  //add the increment to the total control input  indi.u_in.p = indi.u.x.p + indi.du.p;  indi.u_in.q = indi.u.x.q + indi.du.q;  indi.u_in.r = indi.u.x.r + indi.du.r;  //bound the total control input  Bound(indi.u_in.p, -4500, 4500);  Bound(indi.u_in.q, -4500, 4500);  Bound(indi.u_in.r, -4500, 4500);  //Propagate input filters  //first order actuator dynamics  indi.u_act_dyn.p = indi.u_act_dyn.p + STABILIZATION_INDI_ACT_DYN_P * (indi.u_in.p - indi.u_act_dyn.p);  indi.u_act_dyn.q = indi.u_act_dyn.q + STABILIZATION_INDI_ACT_DYN_Q * (indi.u_in.q - indi.u_act_dyn.q);  indi.u_act_dyn.r = indi.u_act_dyn.r + STABILIZATION_INDI_ACT_DYN_R * (indi.u_in.r - indi.u_act_dyn.r);  //sensor filter  stabilization_indi_second_order_filter(&indi.u, &indi.u_act_dyn);  //Don't increment if thrust is off  //TODO: this should be something more elegant, but without this the inputs will increment to the maximum before  //even getting in the air.  if (stabilization_cmd[COMMAND_THRUST] < 300) {    FLOAT_RATES_ZERO(indi.du);    FLOAT_RATES_ZERO(indi.u_act_dyn);    FLOAT_RATES_ZERO(indi.u_in);    FLOAT_RATES_ZERO(indi.u.x);    FLOAT_RATES_ZERO(indi.u.dx);    FLOAT_RATES_ZERO(indi.u.ddx);  } else {    // only run the estimation if the commands are not zero.    lms_estimation();  }  /*  INDI feedback */  indi_commands[COMMAND_ROLL] = indi.u_in.p;  indi_commands[COMMAND_PITCH] = indi.u_in.q;  indi_commands[COMMAND_YAW] = indi.u_in.r;}

inline static void h_ctl_roll_loop( void ) {  static float cmd_fb = 0.;#ifdef USE_ANGLE_REF  // Update reference setpoints for roll  h_ctl_ref_roll_angle += h_ctl_ref_roll_rate * H_CTL_REF_DT;  h_ctl_ref_roll_rate += h_ctl_ref_roll_accel * H_CTL_REF_DT;  h_ctl_ref_roll_accel = H_CTL_REF_W*H_CTL_REF_W * (h_ctl_roll_setpoint - h_ctl_ref_roll_angle) - 2*H_CTL_REF_XI*H_CTL_REF_W * h_ctl_ref_roll_rate;  // Saturation on references  BoundAbs(h_ctl_ref_roll_accel, H_CTL_REF_MAX_P_DOT);  if (h_ctl_ref_roll_rate > H_CTL_REF_MAX_P) {    h_ctl_ref_roll_rate = H_CTL_REF_MAX_P;    if (h_ctl_ref_roll_accel > 0.) h_ctl_ref_roll_accel = 0.;  }  else if (h_ctl_ref_roll_rate < - H_CTL_REF_MAX_P) {    h_ctl_ref_roll_rate = - H_CTL_REF_MAX_P;    if (h_ctl_ref_roll_accel < 0.) h_ctl_ref_roll_accel = 0.;  }#else  h_ctl_ref_roll_angle = h_ctl_roll_setpoint;  h_ctl_ref_roll_rate = 0.;  h_ctl_ref_roll_accel = 0.;#endif#ifdef USE_KFF_UPDATE  // update Kff gains  h_ctl_roll_Kffa -= KFFA_UPDATE * h_ctl_ref_roll_accel * cmd_fb / (H_CTL_REF_MAX_P_DOT*H_CTL_REF_MAX_P_DOT);  h_ctl_roll_Kffd -= KFFD_UPDATE * h_ctl_ref_roll_rate  * cmd_fb / (H_CTL_REF_MAX_P*H_CTL_REF_MAX_P);#ifdef SITL  printf("%f %f %f/n", h_ctl_roll_Kffa, h_ctl_roll_Kffd, cmd_fb);#endif  h_ctl_roll_Kffa = Min(h_ctl_roll_Kffa, 0);  h_ctl_roll_Kffd = Min(h_ctl_roll_Kffd, 0);#endif  // Compute errors  float err = estimator_phi - h_ctl_ref_roll_angle;#ifdef SITL  static float last_err = 0;  estimator_p = (err - last_err)/(1/60.);  last_err = err;#endif  float d_err = estimator_p - h_ctl_ref_roll_rate;  if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0) {    h_ctl_roll_sum_err = 0.;  }  else {    if (h_ctl_roll_igain < 0.) {      h_ctl_roll_sum_err += err * H_CTL_REF_DT;      BoundAbs(h_ctl_roll_sum_err, (- H_CTL_ROLL_SUM_ERR_MAX / h_ctl_roll_igain));    } else {      h_ctl_roll_sum_err = 0.;    }  }  cmd_fb = h_ctl_roll_attitude_gain * err;// + h_ctl_roll_rate_gain * d_err;  float cmd = h_ctl_roll_Kffa * h_ctl_ref_roll_accel    + h_ctl_roll_Kffd * h_ctl_ref_roll_rate    - cmd_fb    - h_ctl_roll_rate_gain * d_err    - h_ctl_roll_igain * h_ctl_roll_sum_err    + v_ctl_throttle_setpoint * h_ctl_aileron_of_throttle;//  float cmd = h_ctl_roll_Kffa * h_ctl_ref_roll_accel//    + h_ctl_roll_Kffd * h_ctl_ref_roll_rate//    - h_ctl_roll_attitude_gain * err//    - h_ctl_roll_rate_gain * d_err//    - h_ctl_roll_igain * h_ctl_roll_sum_err//    + v_ctl_throttle_setpoint * h_ctl_aileron_of_throttle;//  cmd /= airspeed_ratio2;  // Set aileron commands  h_ctl_aileron_setpoint = TRIM_PPRZ(cmd);}

/** * Auto-throttle inner loop * /brief */void v_ctl_climb_loop(void){  // Airspeed setpoint rate limiter:  // AIRSPEED_SETPOINT_SLEW in m/s/s - a change from 15m/s to 18m/s takes 3s with the default value of 1  float airspeed_incr = v_ctl_auto_airspeed_setpoint - v_ctl_auto_airspeed_setpoint_slew;  BoundAbs(airspeed_incr, AIRSPEED_SETPOINT_SLEW * dt_attidude);  v_ctl_auto_airspeed_setpoint_slew += airspeed_incr;#ifdef V_CTL_AUTO_GROUNDSPEED_SETPOINT// Ground speed control loop (input: groundspeed error, output: airspeed controlled)  float err_groundspeed = (v_ctl_auto_groundspeed_setpoint - stateGetHorizontalSpeedNorm_f());  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_slew) {    v_ctl_auto_airspeed_controlled = v_ctl_auto_airspeed_setpoint_slew;    // reset integrator of ground speed loop    v_ctl_auto_groundspeed_sum_err = v_ctl_auto_airspeed_controlled / (v_ctl_auto_groundspeed_pgain *                                     v_ctl_auto_groundspeed_igain);  }#else  v_ctl_auto_airspeed_controlled = v_ctl_auto_airspeed_setpoint_slew;#endif  // Airspeed outerloop: positive means we need to accelerate  float speed_error = v_ctl_auto_airspeed_controlled - stateGetAirspeed_f();  // Speed Controller to PseudoControl: gain 1 -> 5m/s error = 0.5g acceleration  v_ctl_desired_acceleration = speed_error * v_ctl_airspeed_pgain / 9.81f;  BoundAbs(v_ctl_desired_acceleration, v_ctl_max_acceleration);  // Actual Acceleration from IMU: attempt to reconstruct the actual kinematic acceleration#ifndef SITL  struct Int32Vect3 accel_meas_body;  struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu);  int32_rmat_transp_vmult(&accel_meas_body, body_to_imu_rmat, &imu.accel);  float vdot = ACCEL_FLOAT_OF_BFP(accel_meas_body.x) / 9.81f - sinf(stateGetNedToBodyEulers_f()->theta);#else  float vdot = 0;#endif  // Acceleration Error: positive means UAV needs to accelerate: needs extra energy  float vdot_err = low_pass_vdot(v_ctl_desired_acceleration - vdot);  // Flight Path Outerloop: positive means needs to climb more: needs extra energy  float gamma_err  = (v_ctl_climb_setpoint - stateGetSpeedEnu_f()->z) / v_ctl_auto_airspeed_controlled;  // Total Energy Error: positive means energy should be added  float en_tot_err = gamma_err + vdot_err;  // Energy Distribution Error: positive means energy should go from overspeed to altitude = pitch up  float en_dis_err = gamma_err - vdot_err;  // Auto Cruise Throttle  if (launch && (v_ctl_mode >= V_CTL_MODE_AUTO_CLIMB)) {    v_ctl_auto_throttle_nominal_cruise_throttle +=      v_ctl_auto_throttle_of_airspeed_igain * speed_error * dt_attidude      + en_tot_err * v_ctl_energy_total_igain * dt_attidude;    Bound(v_ctl_auto_throttle_nominal_cruise_throttle, 0.0f, 1.0f);  }  // Total Controller  float controlled_throttle = v_ctl_auto_throttle_nominal_cruise_throttle                              + v_ctl_auto_throttle_climb_throttle_increment * v_ctl_climb_setpoint                              + v_ctl_auto_throttle_of_airspeed_pgain * speed_error                              + v_ctl_energy_total_pgain * en_tot_err;  if ((controlled_throttle >= 1.0f) || (controlled_throttle <= 0.0f) || (kill_throttle == 1)) {    // If your energy supply is not sufficient, then neglect the climb requirement    en_dis_err = -vdot_err;    // adjust climb_setpoint to maintain airspeed in case of an engine failure or an unrestriced climb    if (v_ctl_climb_setpoint > 0) { v_ctl_climb_setpoint += - 30. * dt_attidude; }    if (v_ctl_climb_setpoint < 0) { v_ctl_climb_setpoint +=   30. * dt_attidude; }  }  /* pitch pre-command */  if (launch && (v_ctl_mode >= V_CTL_MODE_AUTO_CLIMB)) {    v_ctl_auto_throttle_nominal_cruise_pitch +=  v_ctl_auto_pitch_of_airspeed_igain * (-speed_error) * dt_attidude        + v_ctl_energy_diff_igain * en_dis_err * dt_attidude;    Bound(v_ctl_auto_throttle_nominal_cruise_pitch, H_CTL_PITCH_MIN_SETPOINT, H_CTL_PITCH_MAX_SETPOINT);  }  float v_ctl_pitch_of_vz =    + (v_ctl_climb_setpoint /*+ d_err * v_ctl_auto_throttle_pitch_of_vz_dgain*/) * v_ctl_auto_throttle_pitch_of_vz_pgain    - v_ctl_auto_pitch_of_airspeed_pgain * speed_error    + v_ctl_auto_pitch_of_airspeed_dgain * vdot    + v_ctl_energy_diff_pgain * en_dis_err    + v_ctl_auto_throttle_nominal_cruise_pitch;  if (kill_throttle) { v_ctl_pitch_of_vz = v_ctl_pitch_of_vz - 1 / V_CTL_GLIDE_RATIO; }  v_ctl_pitch_setpoint = v_ctl_pitch_of_vz + nav_pitch;  Bound(v_ctl_pitch_setpoint, H_CTL_PITCH_MIN_SETPOINT, H_CTL_PITCH_MAX_SETPOINT)//.........这里部分代码省略.........

//.........这里部分代码省略.........    estimator_hspeed_dir = estimator_psi;    float crab = sin(wind_dir-estimator_psi) * atan2(wind_mod,NOMINAL_AIRSPEED);    //crab = estimator_hspeed_mod - estimator_psi;    NormRadAngle(crab);    */    // Heading error    float herr = stateGetNedToBodyEulers_f()->psi - h_ctl_course_setpoint; //+crab);    NormRadAngle(herr);    if (advance < -0.5) {            //<! moving in the wrong direction / big > 90 degree turn      err = herr;    } else if (advance < 0.) {       //<!      err = (-advance) * 2. * herr;    } else {      err = advance * err;    }    // Reset differentiator when switching mode    //if (h_ctl_course_heading_mode == 0)    //  last_err = err;    //h_ctl_course_heading_mode = 1;  }  /*  else      {      // Reset differentiator when switching mode      if (h_ctl_course_heading_mode == 1)      last_err = err;      h_ctl_course_heading_mode = 0;      }  */#endif //STRONG_WIND  float d_err = err - last_err;  last_err = err;  NormRadAngle(d_err);#ifdef H_CTL_COURSE_SLEW_INCREMENT  /* slew severe course changes (i.e. waypoint moves, block changes or perpendicular routes) */  static float h_ctl_course_slew_rate = 0.;  float nav_angle_saturation = h_ctl_roll_max_setpoint / h_ctl_course_pgain; /* heading error corresponding to max_roll */  float half_nav_angle_saturation = nav_angle_saturation / 2.;  if (autopilot.launch) {  /* prevent accumulator run-up on the ground */    if (err > half_nav_angle_saturation) {      h_ctl_course_slew_rate = Max(h_ctl_course_slew_rate, 0.);      err = Min(err, (half_nav_angle_saturation + h_ctl_course_slew_rate));      h_ctl_course_slew_rate += h_ctl_course_slew_increment;    } else if (err < -half_nav_angle_saturation) {      h_ctl_course_slew_rate = Min(h_ctl_course_slew_rate, 0.);      err = Max(err, (-half_nav_angle_saturation + h_ctl_course_slew_rate));      h_ctl_course_slew_rate -= h_ctl_course_slew_increment;    } else {      h_ctl_course_slew_rate = 0.;    }  }#endif  float speed_depend_nav = stateGetHorizontalSpeedNorm_f() / NOMINAL_AIRSPEED;  Bound(speed_depend_nav, 0.66, 1.5);  float cmd = -h_ctl_course_pgain * speed_depend_nav * (err + d_err * h_ctl_course_dgain);#if defined(AGR_CLIMB) && !USE_AIRSPEED  /** limit navigation during extreme altitude changes */  if (AGR_BLEND_START > AGR_BLEND_END && AGR_BLEND_END > 0) { /* prevent divide by zero, reversed or negative values */    if (v_ctl_auto_throttle_submode == V_CTL_AUTO_THROTTLE_AGRESSIVE ||        v_ctl_auto_throttle_submode == V_CTL_AUTO_THROTTLE_BLENDED) {      BoundAbs(cmd, h_ctl_roll_max_setpoint); /* bound cmd before NAV_RATIO and again after */      /* altitude: z-up is positive -> positive error -> too low */      if (v_ctl_altitude_error > 0) {        nav_ratio = AGR_CLIMB_NAV_RATIO + (1 - AGR_CLIMB_NAV_RATIO) * (1 - (fabs(v_ctl_altitude_error) - AGR_BLEND_END) /                    (AGR_BLEND_START - AGR_BLEND_END));        Bound(nav_ratio, AGR_CLIMB_NAV_RATIO, 1);      } else {        nav_ratio = AGR_DESCENT_NAV_RATIO + (1 - AGR_DESCENT_NAV_RATIO) * (1 - (fabs(v_ctl_altitude_error) - AGR_BLEND_END) /                    (AGR_BLEND_START - AGR_BLEND_END));        Bound(nav_ratio, AGR_DESCENT_NAV_RATIO, 1);      }      cmd *= nav_ratio;    }  }#endif  float roll_setpoint = cmd + h_ctl_course_pre_bank_correction * h_ctl_course_pre_bank;#ifdef H_CTL_ROLL_SLEW  float diff_roll = roll_setpoint - h_ctl_roll_setpoint;  BoundAbs(diff_roll, h_ctl_roll_slew);  h_ctl_roll_setpoint += diff_roll;#else  h_ctl_roll_setpoint = roll_setpoint;#endif  BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint);}

void stabilization_attitude_run(bool_t  in_flight) {  stabilization_attitude_ref_update();  /* Compute feedforward */  stabilization_att_ff_cmd[COMMAND_ROLL] =    stabilization_gains.dd.x * stab_att_ref_accel.p;  stabilization_att_ff_cmd[COMMAND_PITCH] =    stabilization_gains.dd.y * stab_att_ref_accel.q;  stabilization_att_ff_cmd[COMMAND_YAW] =    stabilization_gains.dd.z * stab_att_ref_accel.r;  /* Compute feedback                  */  /* attitude error            */  struct FloatEulers *att_float = stateGetNedToBodyEulers_f();  struct FloatEulers att_err;  EULERS_DIFF(att_err, stab_att_ref_euler, *att_float);  FLOAT_ANGLE_NORMALIZE(att_err.psi);  if (in_flight) {    /* update integrator */    EULERS_ADD(stabilization_att_sum_err, att_err);    EULERS_BOUND_CUBE(stabilization_att_sum_err, -MAX_SUM_ERR, MAX_SUM_ERR);  }  else {    FLOAT_EULERS_ZERO(stabilization_att_sum_err);  }  /*  rate error                */  struct FloatRates* rate_float = stateGetBodyRates_f();  struct FloatRates rate_err;  RATES_DIFF(rate_err, stab_att_ref_rate, *rate_float);  /*  PID                  */  stabilization_att_fb_cmd[COMMAND_ROLL] =    stabilization_gains.p.x  * att_err.phi +    stabilization_gains.d.x  * rate_err.p +    stabilization_gains.i.x  * stabilization_att_sum_err.phi;  stabilization_att_fb_cmd[COMMAND_PITCH] =    stabilization_gains.p.y  * att_err.theta +    stabilization_gains.d.y  * rate_err.q +    stabilization_gains.i.y  * stabilization_att_sum_err.theta;  stabilization_att_fb_cmd[COMMAND_YAW] =    stabilization_gains.p.z  * att_err.psi +    stabilization_gains.d.z  * rate_err.r +    stabilization_gains.i.z  * stabilization_att_sum_err.psi;  stabilization_cmd[COMMAND_ROLL] =    (stabilization_att_fb_cmd[COMMAND_ROLL]+stabilization_att_ff_cmd[COMMAND_ROLL]);  stabilization_cmd[COMMAND_PITCH] =    (stabilization_att_fb_cmd[COMMAND_PITCH]+stabilization_att_ff_cmd[COMMAND_PITCH]);  stabilization_cmd[COMMAND_YAW] =    (stabilization_att_fb_cmd[COMMAND_YAW]+stabilization_att_ff_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_attitude_run(bool enable_integrator){  /*   * Update reference   * Warning: dt is currently not used in the quat_int ref impl   * PERIODIC_FREQUENCY is assumed to be 512Hz   */  static const float dt = (1./PERIODIC_FREQUENCY);  attitude_ref_quat_int_update(&att_ref_quat_i, &stab_att_sp_quat, dt);  /*   * Compute errors for feedback   */  /* attitude error                          */  struct Int32Quat att_err;  struct Int32Quat *att_quat = stateGetNedToBodyQuat_i();  INT32_QUAT_INV_COMP(att_err, *att_quat, att_ref_quat_i.quat);  /* wrap it in the shortest direction       */  int32_quat_wrap_shortest(&att_err);  int32_quat_normalize(&att_err);  /*  rate error                */  const struct Int32Rates rate_ref_scaled = {    OFFSET_AND_ROUND(att_ref_quat_i.rate.p, (REF_RATE_FRAC - INT32_RATE_FRAC)),    OFFSET_AND_ROUND(att_ref_quat_i.rate.q, (REF_RATE_FRAC - INT32_RATE_FRAC)),    OFFSET_AND_ROUND(att_ref_quat_i.rate.r, (REF_RATE_FRAC - INT32_RATE_FRAC))  };  struct Int32Rates rate_err;  struct Int32Rates *body_rate = stateGetBodyRates_i();  RATES_DIFF(rate_err, rate_ref_scaled, (*body_rate));#define INTEGRATOR_BOUND 100000  /* integrated error */  if (enable_integrator) {    stabilization_att_sum_err_quat.qx += att_err.qx / IERROR_SCALE;    stabilization_att_sum_err_quat.qy += att_err.qy / IERROR_SCALE;    stabilization_att_sum_err_quat.qz += att_err.qz / IERROR_SCALE;    Bound(stabilization_att_sum_err_quat.qx, -INTEGRATOR_BOUND, INTEGRATOR_BOUND);    Bound(stabilization_att_sum_err_quat.qy, -INTEGRATOR_BOUND, INTEGRATOR_BOUND);    Bound(stabilization_att_sum_err_quat.qz, -INTEGRATOR_BOUND, INTEGRATOR_BOUND);  } else {    /* reset accumulator */    int32_quat_identity(&stabilization_att_sum_err_quat);  }  /* compute the feed forward command */  attitude_run_ff(stabilization_att_ff_cmd, &stabilization_gains, &att_ref_quat_i.accel);  /* compute the feed back command */  attitude_run_fb(stabilization_att_fb_cmd, &stabilization_gains, &att_err, &rate_err, &stabilization_att_sum_err_quat);  /* sum feedforward and feedback */  stabilization_cmd[COMMAND_ROLL] = stabilization_att_fb_cmd[COMMAND_ROLL] + stabilization_att_ff_cmd[COMMAND_ROLL];  stabilization_cmd[COMMAND_PITCH] = stabilization_att_fb_cmd[COMMAND_PITCH] + stabilization_att_ff_cmd[COMMAND_PITCH];  stabilization_cmd[COMMAND_YAW] = stabilization_att_fb_cmd[COMMAND_YAW] + stabilization_att_ff_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);}

/** * Update the controls based on a vision result * @param[in] *result The opticflow calculation result used for control */void OA_update(){  float v_x = 0;  float v_y = 0;  if (opti_speed_flag == 1) {    //rotation_vector.psi = stateGetNedToBodyEulers_f()->psi;    //opti_speed = stateGetSpeedNed_f();    //Transform to body frame.    //float_rmat_of_eulers_312(&T, &rotation_vector)Attractforce_goal_send;    //MAT33_VECT3_MUL(*opti_speed, T, *opti_speed);    // Calculate the speed in body frame    struct FloatVect2 speed_cur;    float psi = stateGetNedToBodyEulers_f()->psi;    float s_psi = sin(psi);    float c_psi = cos(psi);    speed_cur.x = c_psi * stateGetSpeedNed_f()->x + s_psi * stateGetSpeedNed_f()->y;    speed_cur.y = -s_psi * stateGetSpeedNed_f()->x + c_psi * stateGetSpeedNed_f()->y;    opti_speed_read.x = speed_cur.x * 100;    opti_speed_read.y = speed_cur.y * 100;    //set result_vel    v_x = speed_cur.y * 100;    v_y = speed_cur.x * 100;  } else {  }  if (OA_method_flag == NO_OBSTACLE_AVOIDANCE) {    /* Calculate the error if we have enough flow */    opticflow_stab.desired_vx = 0;    opticflow_stab.desired_vy = 0;    err_vx = opticflow_stab.desired_vx - v_x;    err_vy = opticflow_stab.desired_vy - v_y;    /* Calculate the integrated errors (TODO: bound??) */    opticflow_stab.err_vx_int += err_vx / 100;    opticflow_stab.err_vy_int += err_vy / 100;    /* Calculate the commands */    opticflow_stab.cmd.phi   = opticflow_stab.phi_pgain * err_vx / 100                               + opticflow_stab.phi_igain * opticflow_stab.err_vx_int;    opticflow_stab.cmd.theta = -(opticflow_stab.theta_pgain * err_vy / 100                                 + opticflow_stab.theta_igain * opticflow_stab.err_vy_int);    /* Bound the roll and pitch commands */    BoundAbs(opticflow_stab.cmd.phi, CMD_OF_SAT);    BoundAbs(opticflow_stab.cmd.theta, CMD_OF_SAT);  }  if (OA_method_flag == PINGPONG) {    opticflow_stab.cmd.phi = ANGLE_BFP_OF_REAL(ref_roll);    opticflow_stab.cmd.theta = ANGLE_BFP_OF_REAL(ref_pitch);  }  if (OA_method_flag == 2) {    Total_Kan_x = r_dot_new;    Total_Kan_y = speed_pot;    alpha_fil = 0.1;    yaw_rate = (int32_t)(alpha_fil * ANGLE_BFP_OF_REAL(r_dot_new));    opticflow_stab.cmd.psi  = stateGetNedToBodyEulers_i()->psi + yaw_rate;    INT32_ANGLE_NORMALIZE(opticflow_stab.cmd.psi);    opticflow_stab.desired_vx = 0;    opticflow_stab.desired_vy = speed_pot;    /* Calculate the error if we have enough flow */    err_vx = opticflow_stab.desired_vx - v_x;    err_vy = opticflow_stab.desired_vy - v_y;    /* Calculate the integrated errors (TODO: bound??) */    opticflow_stab.err_vx_int += err_vx / 100;    opticflow_stab.err_vy_int += err_vy / 100;    /* Calculate the commands */    opticflow_stab.cmd.phi   = opticflow_stab.phi_pgain * err_vx / 100                               + opticflow_stab.phi_igain * opticflow_stab.err_vx_int;    opticflow_stab.cmd.theta = -(opticflow_stab.theta_pgain * err_vy / 100                                 + opticflow_stab.theta_igain * opticflow_stab.err_vy_int);    /* Bound the roll and pitch commands */    BoundAbs(opticflow_stab.cmd.phi, CMD_OF_SAT);    BoundAbs(opticflow_stab.cmd.theta, CMD_OF_SAT);  }  if (OA_method_flag == POT_HEADING) {    new_heading = ref_pitch;//.........这里部分代码省略.........

/** /brief Computes slewed throttle from throttle setpoint    called at 20Hz */void v_ctl_throttle_slew( void ) {  pprz_t diff_throttle = v_ctl_throttle_setpoint - v_ctl_throttle_slewed;  BoundAbs(diff_throttle, TRIM_PPRZ(V_CTL_THROTTLE_SLEW*MAX_PPRZ));  v_ctl_throttle_slewed += diff_throttle;}

inline static void h_ctl_pitch_loop( void ) {#if !USE_GYRO_PITCH_RATE  static float last_err;#endif  /* sanity check */  if (h_ctl_pitch_of_roll <0.)    h_ctl_pitch_of_roll = 0.;  h_ctl_pitch_loop_setpoint = h_ctl_pitch_setpoint + h_ctl_pitch_of_roll * fabs(stateGetNedToBodyEulers_f()->phi);#if USE_PITCH_TRIM  loiter();#endif#if USE_ANGLE_REF  // Update reference setpoints for pitch  h_ctl_ref.pitch_angle += h_ctl_ref.pitch_rate * H_CTL_REF_DT;  h_ctl_ref.pitch_rate += h_ctl_ref.pitch_accel * H_CTL_REF_DT;  h_ctl_ref.pitch_accel = H_CTL_REF_W_Q*H_CTL_REF_W_Q * (h_ctl_pitch_loop_setpoint - h_ctl_ref.pitch_angle) - 2*H_CTL_REF_XI_Q*H_CTL_REF_W_Q * h_ctl_ref.pitch_rate;  // Saturation on references  BoundAbs(h_ctl_ref.pitch_accel, h_ctl_ref.max_q_dot);  if (h_ctl_ref.pitch_rate > h_ctl_ref.max_q) {    h_ctl_ref.pitch_rate = h_ctl_ref.max_q;    if (h_ctl_ref.pitch_accel > 0.) h_ctl_ref.pitch_accel = 0.;  }  else if (h_ctl_ref.pitch_rate < - h_ctl_ref.max_q) {    h_ctl_ref.pitch_rate = - h_ctl_ref.max_q;    if (h_ctl_ref.pitch_accel < 0.) h_ctl_ref.pitch_accel = 0.;  }#else  h_ctl_ref.pitch_angle = h_ctl_pitch_loop_setpoint;  h_ctl_ref.pitch_rate = 0.;  h_ctl_ref.pitch_accel = 0.;#endif  // Compute errors  float err =  h_ctl_ref.pitch_angle - stateGetNedToBodyEulers_f()->theta;#if USE_GYRO_PITCH_RATE  float d_err = h_ctl_ref.pitch_rate - stateGetBodyRates_f()->q;#else // soft derivation  float d_err = (err - last_err)/H_CTL_REF_DT - h_ctl_ref.pitch_rate;  last_err = err;#endif  if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0) {    h_ctl_pitch_sum_err = 0.;  }  else {    if (h_ctl_pitch_igain > 0.) {      h_ctl_pitch_sum_err += err * H_CTL_REF_DT;      BoundAbs(h_ctl_pitch_sum_err, H_CTL_PITCH_SUM_ERR_MAX / h_ctl_pitch_igain);    } else {      h_ctl_pitch_sum_err = 0.;    }  }  float cmd = - h_ctl_pitch_Kffa * h_ctl_ref.pitch_accel    - h_ctl_pitch_Kffd * h_ctl_ref.pitch_rate    + h_ctl_pitch_pgain * err    + h_ctl_pitch_dgain * d_err    + h_ctl_pitch_igain * h_ctl_pitch_sum_err;  cmd /= airspeed_ratio2;  h_ctl_elevator_setpoint = TRIM_PPRZ(cmd);}