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)
ac_char_update(controlled_throttle, v_ctl_pitch_of_vz, v_ctl_climb_setpoint, v_ctl_desired_acceleration);
v_ctl_throttle_setpoint = TRIM_UPPRZ(controlled_throttle * MAX_PPRZ);
}
/**
* \brief
*
*/
void h_ctl_course_loop(void)
{
static float last_err;
// Ground path error
float err = stateGetHorizontalSpeedDir_f() - h_ctl_course_setpoint;
NormRadAngle(err);
#ifdef STRONG_WIND
// Usefull path speed
const float reference_advance = (NOMINAL_AIRSPEED / 2.);
float advance = cos(err) * stateGetHorizontalSpeedNorm_f() / reference_advance;
if (
(advance < 1.) && // Path speed is small
(stateGetHorizontalSpeedNorm_f() < reference_advance) // Small path speed is due to wind (small groundspeed)
) {
/*
// rough crabangle approximation
float wind_mod = sqrt(wind_east*wind_east + wind_north*wind_north);
float wind_dir = atan2(wind_east,wind_north);
float wind_course = h_ctl_course_setpoint - wind_dir;
NormRadAngle(wind_course);
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;
opticflow_stab.desired_vx = sin(new_heading) * speed_pot * 100;
opticflow_stab.desired_vy = cos(new_heading) * speed_pot * 100;
/* 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)
}
/** \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);
}
