Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-22 12:28:03 +08:00
Code Issues Actions 26 Packages Projects Releases Wiki Activity

new horizontal control loop

Browse Source
This commit is contained in:
Gautier Hattenberger
2010-06-29 10:57:17 +00:00
parent 8e0e120ef1
commit efb18dc099
2 changed files with 143 additions and 92 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/fw_h_ctl_a.c
+136 -91
View File
@@ -1,7 +1,7 @@
/*
* Paparazzi $Id: fw_h_ctl.c 3603 2009-07-01 20:06:53Z hecto $
*
* Copyright (C) 2006 Pascal Brisset, Antoine Drouin, Michel Gorraz
* Copyright (C) 2009-2010 ENAC
*
* This file is part of paparazzi.
*
@@ -60,33 +60,41 @@ float h_ctl_ref_roll_accel;
float h_ctl_ref_pitch_angle;
float h_ctl_ref_pitch_rate;
float h_ctl_ref_pitch_accel;
#define H_CTL_REF_W 10.
#define H_CTL_REF_W 2.5
#define H_CTL_REF_XI 0.85
#define H_CTL_REF_MAX_P RadOfDeg(100.)
#define H_CTL_REF_MAX_P_DOT RadOfDeg(500.)
#define H_CTL_REF_MAX_Q RadOfDeg(100.)
#define H_CTL_REF_MAX_Q_DOT RadOfDeg(500.)
/* inner roll loop parameters */
float h_ctl_roll_setpoint;
float h_ctl_roll_setpoint;
float h_ctl_roll_attitude_gain;
float h_ctl_roll_rate_gain;
float h_ctl_roll_igain;
float h_ctl_pitch_igain;
float h_ctl_roll_sum_err;
float h_ctl_pitch_sum_err;
float h_ctl_roll_Kff;
float h_ctl_roll_igain;
float h_ctl_roll_sum_err;
float h_ctl_roll_Kffa;
float h_ctl_roll_Kffd;
pprz_t h_ctl_aileron_setpoint;
float h_ctl_roll_slew;
float h_ctl_roll_slew;
float h_ctl_roll_pgain;
float h_ctl_roll_pgain;
/* inner pitch loop parameters */
float h_ctl_pitch_setpoint;
float h_ctl_pitch_loop_setpoint;
float h_ctl_pitch_pgain;
float h_ctl_pitch_dgain;
float h_ctl_pitch_setpoint;
float h_ctl_pitch_loop_setpoint;
float h_ctl_pitch_pgain;
float h_ctl_pitch_dgain;
float h_ctl_pitch_igain;
float h_ctl_pitch_sum_err;
float h_ctl_pitch_Kffa;
float h_ctl_pitch_Kffd;
pprz_t h_ctl_elevator_setpoint;
/* inner loop pre-command */
float h_ctl_aileron_of_throttle;
float h_ctl_elevator_of_roll;
float h_ctl_pitch_of_roll; // Should be used instead of elevator_of_roll
inline static void h_ctl_roll_loop( void );
@@ -104,10 +112,6 @@ inline static void h_ctl_pitch_loop( void );
#define H_CTL_ROLL_RATE_GAIN 0.
#endif
#ifdef AGR_CLIMB
static float nav_ratio;
#endif
void h_ctl_init( void ) {
h_ctl_course_setpoint = 0.;
h_ctl_course_pre_bank = 0.;
@@ -119,6 +123,12 @@ void h_ctl_init( void ) {
h_ctl_disabled = FALSE;
h_ctl_roll_setpoint = 0.;
h_ctl_roll_attitude_gain = H_CTL_ROLL_ATTITUDE_GAIN;
h_ctl_roll_rate_gain = H_CTL_ROLL_RATE_GAIN;
h_ctl_roll_igain = H_CTL_ROLL_IGAIN;
h_ctl_roll_sum_err = 0;
h_ctl_roll_Kffa = H_CTL_ROLL_KFFA;
h_ctl_roll_Kffd = H_CTL_ROLL_KFFD;
h_ctl_aileron_setpoint = 0;
#ifdef H_CTL_AILERON_OF_THROTTLE
h_ctl_aileron_of_throttle = H_CTL_AILERON_OF_THROTTLE;
@@ -128,19 +138,16 @@ void h_ctl_init( void ) {
h_ctl_pitch_loop_setpoint = 0.;
h_ctl_pitch_pgain = H_CTL_PITCH_PGAIN;
h_ctl_pitch_dgain = H_CTL_PITCH_DGAIN;
h_ctl_pitch_igain = H_CTL_PITCH_IGAIN;
h_ctl_pitch_sum_err = 0.;
h_ctl_pitch_Kffa = H_CTL_PITCH_KFFA;
h_ctl_pitch_Kffd = H_CTL_PITCH_KFFD;
h_ctl_elevator_setpoint = 0;
h_ctl_elevator_of_roll = H_CTL_ELEVATOR_OF_ROLL;
h_ctl_roll_attitude_gain = H_CTL_ROLL_ATTITUDE_GAIN;
h_ctl_roll_rate_gain = H_CTL_ROLL_RATE_GAIN;
h_ctl_roll_igain = H_CTL_ROLL_IGAIN;
h_ctl_roll_sum_err = 0;
h_ctl_roll_Kff = H_CTL_ROLL_KFF;
#ifdef AGR_CLIMB
nav_ratio=0;
h_ctl_elevator_of_roll = 0; //H_CTL_ELEVATOR_OF_ROLL;
#ifdef H_CTL_PITCH_OF_ROLL
h_ctl_pitch_of_roll = H_CTL_PITCH_OF_ROLL;
#endif
}
/**
@@ -156,44 +163,21 @@ void h_ctl_course_loop ( void ) {
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);
float cmd = h_ctl_course_pgain * speed_depend_nav * (err + d_err * h_ctl_course_dgain);
#if defined AGR_CLIMB
/** 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_BLENDED) {
BoundAbs(cmd, h_ctl_roll_max_setpoint); /* bound cmd before NAV_RATIO and again after */
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;
h_ctl_roll_setpoint = roll_setpoint;
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);
}
static float airspeed_ratio2;
void h_ctl_attitude_loop ( void ) {
if (!h_ctl_disabled) {
static inline void compute_airspeed_ratio( void ) {
float throttle_diff = v_ctl_throttle_setpoint / (float)MAX_PPRZ - v_ctl_auto_throttle_nominal_cruise_throttle;
float airspeed = NOMINAL_AIRSPEED; /* Estimated from the throttle */
if (throttle_diff > 0)
@@ -204,61 +188,109 @@ void h_ctl_attitude_loop ( void ) {
float airspeed_ratio = airspeed / NOMINAL_AIRSPEED;
Bound(airspeed_ratio, 0.5, 2.);
airspeed_ratio2 = airspeed_ratio*airspeed_ratio;
}
void h_ctl_attitude_loop ( void ) {
if (!h_ctl_disabled) {
// compute_airspeed_ratio();
h_ctl_roll_loop();
h_ctl_pitch_loop();
}
}
#define H_CTL_REF_DT (1./60.)
#define KFFA_UPDATE 0.1
#define KFFD_UPDATE 0.05
inline static void h_ctl_roll_loop( void ) {
static float cmd_fb = 0.;
if (pprz_mode == PPRZ_MODE_MANUAL)
h_ctl_roll_sum_err = 0;
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;
h_ctl_ref_roll_rate += h_ctl_ref_roll_accel * H_CTL_REF_DT;
// 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.;
}
#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 derr = estimator_p - h_ctl_ref_roll_rate;
float d_err = (estimator_p - h_ctl_ref_roll_rate) / H_CTL_REF_DT;
h_ctl_roll_sum_err += err * H_CTL_REF_DT;
BoundAbs(h_ctl_roll_sum_err, H_CTL_ROLL_SUM_ERR_MAX);
float cmd = h_ctl_roll_Kff * h_ctl_ref_roll_accel
- h_ctl_roll_attitude_gain * err
- h_ctl_roll_rate_gain * derr
cmd_fb = h_ctl_roll_attitude_gain * err;// + h_ctl_roll_rate_gain * derr;
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_Kffp * 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 * derr
// - h_ctl_roll_igain * h_ctl_roll_sum_err
// + v_ctl_throttle_setpoint * h_ctl_aileron_of_throttle;
//x cmd /= airspeed_ratio2;
// Set aileron commands
h_ctl_aileron_setpoint = TRIM_PPRZ(cmd);
}
// NOT USED
#ifdef LOITER_TRIM
float v_ctl_auto_throttle_loiter_trim = V_CTL_AUTO_THROTTLE_LOITER_TRIM;
float v_ctl_auto_throttle_dash_trim = V_CTL_AUTO_THROTTLE_DASH_TRIM;
#endif
#ifdef PITCH_TRIM
float v_ctl_pitch_loiter_trim = V_CTL_PITCH_LOITER_TRIM;
float v_ctl_pitch_dash_trim = V_CTL_PITCH_DASH_TRIM;
inline static float loiter(void) {
float elevator_trim;
inline static void loiter(void) {
float pitch_trim;
float throttle_dif = v_ctl_auto_throttle_cruise_throttle - v_ctl_auto_throttle_nominal_cruise_throttle;
if (throttle_dif > 0) {
float max_dif = Max(V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE - v_ctl_auto_throttle_nominal_cruise_throttle, 0.1);
elevator_trim = throttle_dif / max_dif * v_ctl_auto_throttle_dash_trim;
float throttle_diff = v_ctl_throttle_setpoint / (float)MAX_PPRZ - v_ctl_auto_throttle_nominal_cruise_throttle;
if (throttle_diff > 0) {
float max_diff = Max(V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE - v_ctl_auto_throttle_nominal_cruise_throttle, 0.1);
pitch_trim = throttle_diff / max_diff * v_ctl_pitch_dash_trim;
} else {
float max_dif = Max(v_ctl_auto_throttle_nominal_cruise_throttle - V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE, 0.1);
elevator_trim = - throttle_dif / max_dif * v_ctl_auto_throttle_loiter_trim;
float max_diff = Max(v_ctl_auto_throttle_nominal_cruise_throttle - V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE, 0.1);
pitch_trim = -throttle_diff / max_diff * v_ctl_pitch_loiter_trim;
}
return elevator_trim / h_ctl_pitch_pgain; /* Normalisation to keep historical values */
h_ctl_pitch_loop_setpoint += pitch_trim;
}
#endif
@@ -266,32 +298,45 @@ inline static float loiter(void) {
inline static void h_ctl_pitch_loop( void ) {
static float last_err;
/* sanity check */
if (h_ctl_elevator_of_roll <0.)
h_ctl_elevator_of_roll = 0.;
h_ctl_pitch_loop_setpoint =
h_ctl_pitch_setpoint
- h_ctl_elevator_of_roll / h_ctl_pitch_pgain * fabs(estimator_phi);
h_ctl_ref_pitch_accel = H_CTL_REF_W*H_CTL_REF_W * (h_ctl_pitch_loop_setpoint - h_ctl_ref_pitch_angle) - 2*H_CTL_REF_XI*H_CTL_REF_W * h_ctl_ref_pitch_rate;
h_ctl_ref_pitch_rate += h_ctl_ref_pitch_accel * H_CTL_REF_DT;
h_ctl_ref_pitch_angle += h_ctl_ref_pitch_rate * H_CTL_REF_DT;
#ifdef LOITER_TRIM
h_ctl_pitch_loop_setpoint -= loiter();
#endif
float err = estimator_theta - h_ctl_ref_pitch_angle;
if (h_ctl_pitch_of_roll <0.)
h_ctl_pitch_of_roll = 0.;
if (pprz_mode == PPRZ_MODE_MANUAL)
h_ctl_pitch_sum_err = 0;
h_ctl_pitch_loop_setpoint = h_ctl_pitch_setpoint + h_ctl_pitch_of_roll * fabs(estimator_phi);
#ifdef PITCH_TRIM
loiter();
#endif
// Update reference setpoints for pitch
h_ctl_ref_pitch_accel = H_CTL_REF_W*H_CTL_REF_W * (h_ctl_pitch_loop_setpoint - h_ctl_ref_pitch_angle) - 2*H_CTL_REF_XI*H_CTL_REF_W * h_ctl_ref_pitch_rate;
h_ctl_ref_pitch_rate += h_ctl_ref_pitch_accel * H_CTL_REF_DT;
h_ctl_ref_pitch_angle += h_ctl_ref_pitch_rate * H_CTL_REF_DT;
// 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.;
}
// Compute errors
float err = estimator_theta - h_ctl_ref_pitch_angle;
float d_err = (err - last_err)/H_CTL_REF_DT - h_ctl_ref_pitch_rate;
last_err = err;
h_ctl_pitch_sum_err += err * H_CTL_REF_DT;
BoundAbs(h_ctl_pitch_sum_err, H_CTL_ROLL_SUM_ERR_MAX);
float d_err = (err - last_err)/H_CTL_REF_DT - h_ctl_ref_pitch_rate;
last_err = err;
float cmd = h_ctl_pitch_pgain * err + h_ctl_pitch_dgain * d_err + h_ctl_pitch_igain * h_ctl_pitch_sum_err;
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;
sw/airborne/fw_h_ctl_a.h
+7 -1
View File
@@ -40,8 +40,14 @@ extern float h_ctl_roll_sum_err;
extern float h_ctl_pitch_sum_err;
extern float h_ctl_roll_igain;
extern float h_ctl_pitch_igain;
extern float h_ctl_roll_Kff;
extern float h_ctl_roll_Kffa;
extern float h_ctl_roll_Kffd;
extern float h_ctl_pitch_Kffa;
extern float h_ctl_pitch_Kffd;
extern float h_ctl_pitch_of_roll;
#define H_CTL_ROLL_SUM_ERR_MAX 100.
#define H_CTL_PITCH_SUM_ERR_MAX 100.
#define fw_h_ctl_a_SetRollIGain(_gain) { \
h_ctl_roll_sum_err = 0; \