Added a D gain to the fwd heading control

paparazzi · Sep 23, 2016 · 7b5fca4 · 7b5fca4
1 parent 16c07a2
commit 7b5fca4
Show file tree

Hide file tree

Showing 3 changed files with 15 additions and 2 deletions.
diff --git a/conf/settings/control/hybrid_guidance.xml b/conf/settings/control/hybrid_guidance.xml
@@ -15,6 +15,7 @@
       <dl_setting var="filter_omega" MIN="1.0" STEP="0.1" MAX="50.0" module="guidance/guidance_indi" shortname="indi_omega"/>
       <dl_setting var="wind_heading_deg" MIN="0.0" STEP="0.1" MAX="360.0" module="guidance/guidance_h" shortname="wind_heading"/>
       <dl_setting var="reset_wind_heading" min="0" step="1" max="1" module="guidance/guidance_h" shortname="reset_wind" values="FALSE|TRUE"/>
+      <dl_setting var="perpen_dgain" MIN="0.0" STEP="0.01" MAX="360.0" module="guidance/guidance_h" shortname="perpen_dgain"/>
     </dl_settings>
   </dl_settings>
 </settings>
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_hybrid.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_hybrid.c
@@ -80,16 +80,20 @@ static bool force_forward_flight;
 int32_t v_control_pitch = 0;
 float low_airspeed_pitch_gain = OUTBACK_LOW_AIRSPEED_PITCH_GAIN;
 struct NedCoor_i ned_gps_vel;
+float dperpendicular = 0.0;
+float perpendicular_prev = 0.0;
+float perpen_dgain = 0.0;
 
 #if PERIODIC_TELEMETRY
 #include "subsystems/datalink/telemetry.h"
 
 static void send_hybrid_guidance(struct transport_tx *trans, struct link_device *dev)
 {
+  int32_t dperpen_show = dperpendicular;
   struct NedCoor_i *pos = stateGetPositionNed_i();
   struct NedCoor_i *speed = stateGetSpeedNed_i();
   pprz_msg_send_HYBRID_GUIDANCE(trans, dev, AC_ID,
-                                &(pos->x), &(pos->y),
+                                &dperpen_show, &(pos->y),
                                 &(speed->x), &(speed->y),
                                 &wind_estimate.x, &wind_estimate.y,
                                 &guidance_h_pos_err.x,
@@ -124,6 +128,9 @@ void guidance_hybrid_init(void)
   INT_VECT2_ZERO(guidance_hybrid_ref_airspeed);
   INT_VECT2_ZERO(wind_estimate_high_res);
 
+  dperpendicular = 0.0;
+  perpendicular_prev = 0.0;
+
 #if PERIODIC_TELEMETRY
   register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_HYBRID_GUIDANCE, send_hybrid_guidance);
 #endif
@@ -296,6 +303,10 @@ void guidance_hybrid_attitude_outback(struct Int32Eulers *ypr_sp)
   float to_wp         =   north * cosh + east * sinh;
   float perpendicular = - north * sinh + east * cosh;
 
+  dperpendicular = (perpendicular - perpendicular_prev)*PERIODIC_FREQUENCY;
+
+  perpendicular_prev = perpendicular;
+
   // towp = 4*4/5 + 3 * 3/5 = 16+9 / 5 = 5
   // perpendic = -4 * 3/5 + 3 * 4/5 = 0/5 = 0
 
@@ -305,7 +316,7 @@ void guidance_hybrid_attitude_outback(struct Int32Eulers *ypr_sp)
   // perpendicular = -5*3/5 + 0*4/5 = -3 : we vliegen teveel naar links
 
   // linearize atan (angle less than 45 degree
-  float heading_diff = - perpendicular / 20.0f;  // m/s
+  float heading_diff = - (perpendicular / 20.0f + dperpendicular * perpen_dgain);  // m/s
 
 /*
   Even proberen vervangen: als het niet werkt moet dit weer aan.

diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_hybrid.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_hybrid.h
@@ -48,6 +48,7 @@ extern float low_airspeed_pitch_gain;
 enum hybrid_mode {HB_HOVER, HB_FORWARD};
 extern enum hybrid_mode outback_hybrid_mode;
 extern struct Int32Eulers guidance_hybrid_ypr_sp;
+extern float perpen_dgain;
 
 /** Runs the Hybrid Guidance main functions.
  */