Merged in BUGFIX_wind_decay

Author: realtimepeople

NAV_Algorithms/NAV_tuning_parameters.h

@@ -42,6 +42,7 @@
 #define DEFAULT_AVG_VARIO_TC 	30.0f
 #define DEFAULT_WIND_TC 	5.0f
 #define DEFAULT_WIND_AVG_TC 	30.0f
+#define NEGLECTABLE_WIND	0.1f
 
 // AHRS tuning parameters:
 // These parameters have been tuned for the flight-dynamics of gliders

NAV_Algorithms/soaring_flight_averager.h

@@ -147,6 +147,11 @@ template<class value_t, bool CLAMP_OUTPUT_FIRST_CIRCLE = false, bool SOFT_TAKEOF
       return true;
     }
 
+    void relax( void)
+    {
+      averager.settle({0});
+    }
+
   private:
 
     value_t get_boxcar_average( void)

NAV_Algorithms/wind_observer.h

@@ -73,17 +73,30 @@ class wind_oberserver_t
 
   void process_at_100_Hz( const float3vector &instant_wind)
   {
-    wind_resampler_100_10Hz.respond( instant_wind);
-    instant_wind_averager.respond( instant_wind);
+    if( instant_wind.abs() < NEGLECTABLE_WIND) // avoid float instability
+      {
+	 wind_resampler_100_10Hz.settle({0});
+	 instant_wind_averager.settle({0});
+      }
+    else
+      {
+	wind_resampler_100_10Hz.respond( instant_wind);
+	instant_wind_averager.respond( instant_wind);
+      }
   }
 
   void process_at_10_Hz( const AHRS_type & ahrs)
   {
     circling_state = ahrs.get_circling_state ();
-    wind_average_observer.update(
-	wind_resampler_100_10Hz.get_output(),
-    	ahrs.get_euler ().y,
-	circling_state);
+
+    float3vector instant_wind = wind_resampler_100_10Hz.get_output();
+    if( instant_wind.abs() < NEGLECTABLE_WIND) // avoid float instability
+	wind_average_observer.relax();
+    else
+      wind_average_observer.update(
+	  instant_wind,
+	  ahrs.get_euler ().y,
+	  circling_state);
 
     float3vector relative_wind_NAV  = wind_resampler_100_10Hz.get_output() - wind_average_observer.get_output();
     float3vector relative_wind_BODY =  ahrs.get_body2nav().reverse_map(relative_wind_NAV);
@@ -99,15 +112,16 @@ class wind_oberserver_t
   	  relative_wind_observer.reset({0});
   	}
         else
-          relative_wind_observer.update( relative_wind_BODY, ahrs.get_euler ().y, ahrs.get_circling_state ());
+          {
+	    relative_wind_observer.update( relative_wind_BODY, ahrs.get_euler ().y, ahrs.get_circling_state ());
+	    wind_correction_nav = ahrs.get_body2nav() * relative_wind_observer.get_output();
+	    wind_correction_nav[DOWN]=0.0f;
+
+	    corrected_wind_averager.respond( wind_resampler_100_10Hz.get_output() - wind_correction_nav);
+	    circling_wind_averager.update( wind_resampler_100_10Hz.get_output() - wind_correction_nav);
+          }
       }
 
-    wind_correction_nav = ahrs.get_body2nav() * relative_wind_observer.get_output();
-    wind_correction_nav[DOWN]=0.0f;
-
-    corrected_wind_averager.respond( wind_resampler_100_10Hz.get_output() - wind_correction_nav);
-    circling_wind_averager.update( wind_resampler_100_10Hz.get_output() - wind_correction_nav);
-
     old_circling_state = circling_state;
   }