[paparazzi-commits] [5009] new horizontal control loop

[Gautier Hattenberger]

Revision: 5009
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=5009
Author:   gautier
Date:     2010-06-29 10:57:17 +0000 (Tue, 29 Jun 2010)
Log Message:
-----------
new horizontal control loop

Modified Paths:
--------------
    paparazzi3/trunk/sw/airborne/fw_h_ctl_a.c
    paparazzi3/trunk/sw/airborne/fw_h_ctl_a.h

Modified: paparazzi3/trunk/sw/airborne/fw_h_ctl_a.c
===================================================================
--- paparazzi3/trunk/sw/airborne/fw_h_ctl_a.c   2010-06-28 11:23:00 UTC (rev 
5008)
+++ paparazzi3/trunk/sw/airborne/fw_h_ctl_a.c   2010-06-29 10:57:17 UTC (rev 
5009)
@@ -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_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 @@
 #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 @@
   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 @@
   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_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
 
-  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;
-#endif
 }
 
 /** 
@@ -156,44 +163,21 @@
 
   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);
+  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;
 
-
-
-#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;
-
   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 @@
     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;
 
+  // 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;
-  h_ctl_ref_roll_rate += h_ctl_ref_roll_accel * H_CTL_REF_DT;
-  h_ctl_ref_roll_angle += h_ctl_ref_roll_rate * H_CTL_REF_DT;
+  // 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,33 +298,46 @@
 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.;
+  if (h_ctl_pitch_of_roll <0.)
+    h_ctl_pitch_of_roll = 0.;
 
-  h_ctl_pitch_loop_setpoint =
-    h_ctl_pitch_setpoint 
-    - h_ctl_elevator_of_roll / h_ctl_pitch_pgain * fabs(estimator_phi);
+  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.;
+  }
 
-#ifdef LOITER_TRIM
-  h_ctl_pitch_loop_setpoint -= loiter();
-#endif
-
+  // 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;
 
-  if (pprz_mode == PPRZ_MODE_MANUAL)
-    h_ctl_pitch_sum_err = 0;
   h_ctl_pitch_sum_err += err * H_CTL_REF_DT;
   BoundAbs(h_ctl_pitch_sum_err, H_CTL_ROLL_SUM_ERR_MAX);
 
+  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;
 
-  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;
-
   //  cmd /= airspeed_ratio2;
 
   h_ctl_elevator_setpoint = TRIM_PPRZ(cmd);

Modified: paparazzi3/trunk/sw/airborne/fw_h_ctl_a.h
===================================================================
--- paparazzi3/trunk/sw/airborne/fw_h_ctl_a.h   2010-06-28 11:23:00 UTC (rev 
5008)
+++ paparazzi3/trunk/sw/airborne/fw_h_ctl_a.h   2010-06-29 10:57:17 UTC (rev 
5009)
@@ -40,8 +40,14 @@
 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; \