Air density mechanism optimized.

NAV_Algorithms/air_density_observer.cpp

@@ -25,60 +25,69 @@
 #include "embedded_math.h"
 #include <air_density_observer.h>
 
-air_data_result air_density_observer::feed_metering( float pressure, float MSL_altitude)
+air_data_result air_density_observer_t::feed_metering( float pressure, float GNSS_altitude)
 {
   air_data_result air_data;
 
-#if DENSITY_MEASURMENT_COLLECTS_INTEGER
-  density_QFF_calculator.add_value( MSL_altitude * 100.0f, pressure);
-#else
-  // use values normalized around 1.0
-  density_QFF_calculator.add_value( MSL_altitude * 1e-3, pressure * 1e-5);
-#endif
+  pressure_decimation_filter.respond( pressure);
+  altitude_decimation_filter.respond( GNSS_altitude);
+  --decimation_counter;
+  if( decimation_counter > 0)
+    return air_data;
+  decimation_counter = AIR_DENSITY_DECIMATION;
+
+  density_QFF_calculator.add_value( GNSS_altitude * 100.0f, pressure);
 
-  if( MSL_altitude > max_altitude)
-    max_altitude = MSL_altitude;
+  // update elevation range
+  if( GNSS_altitude > max_altitude)
+    max_altitude = GNSS_altitude;
 
-  if( MSL_altitude < min_altitude)
-    min_altitude = MSL_altitude;
+  if( GNSS_altitude < min_altitude)
+    min_altitude = GNSS_altitude;
 
-  if( false == altitude_trigger.process(MSL_altitude))
+  // if range too low: continue
+  if( (max_altitude - min_altitude) < MINIMUM_ALTITUDE_RANGE)
     return air_data;
 
-  if( ((max_altitude - min_altitude) < MINIMUM_ALTITUDE_RANGE) // ... forget this measurement
-    || (density_QFF_calculator.get_count() < 3000))
-    {
-      max_altitude = min_altitude = MSL_altitude;
-      density_QFF_calculator.reset();
-      return air_data;
-    }
+  // elevation range triggering
+  if( (max_altitude - min_altitude < MAXIMUM_ALTITUDE_RANGE) &&
+      false == altitude_trigger.process(GNSS_altitude))
+    return air_data;
+
+  // if data points too rare: continue
+  if (density_QFF_calculator.get_count() < 100)
+    return air_data;
 
+  // process last acquisition phase data
   linear_least_square_result<evaluation_type> result;
   density_QFF_calculator.evaluate(result);
 
-//  Due to numeric effects, when using float the
-//  variance has been observed to be negative in some cases !
-//  Therefore using float this test can not be used.
- assert( result.variance_slope > 0);
- assert( result.variance_offset > 0);
+//  Due to numeric effects, the variance has been observed
+//  to be negative in some cases.
+//  If this is the case: Throw away this result.
+  if( ( result.variance_slope < 0) || ( result.variance_offset < 0))
+      {
+      density_QFF_calculator.reset();
+      air_data.valid=false;
+      return air_data;
+      }
 
- if( result.variance_slope < MAX_ALLOWED_VARIANCE)
+ if( (result.variance_slope < MAX_ALLOWED_SLOPE_VARIANCE) &&
+     (result.variance_offset < MAX_ALLOWED_OFFSET_VARIANCE) )
     {
       air_data.QFF = (float)(result.y_offset);
-      float density = 100.0f * (float)(result.slope) * -0.10194f; // div by -9.81f;
-
-#if DENSITY_MEASURMENT_COLLECTS_INTEGER
-      float pressure = density_QFF_calculator.get_mean_y();
-#else
-      float pressure = 1e5 * density_QFF_calculator.get_mean_y();
-#endif
+      float density = 100.0f * (float)(result.slope) * -0.101936f; // div by -9.81f;
 
-      float std_density = 1.0496346613e-5f * pressure + 0.1671546011f;
+      float reference_altitude = density_QFF_calculator.get_mean_x() * 0.01f;
+      float std_density =
+	  reference_altitude * reference_altitude *   0.000000003547494f
+	  -0.000115412739613f * reference_altitude +1.224096628212817f;
       air_data.density_correction = density / std_density;
+      air_data.density_variance = result.variance_slope;
       air_data.valid = true;
     }
 
-  max_altitude = min_altitude = MSL_altitude;
+  max_altitude = min_altitude = GNSS_altitude;
   density_QFF_calculator.reset();
 
   return air_data;

NAV_Algorithms/air_density_observer.h

@@ -25,39 +25,49 @@
 #ifndef AIR_DENSITY_OBSERVER_H_
 #define AIR_DENSITY_OBSERVER_H_
 
+#include "pt2.h"
 #include "Linear_Least_Square_Fit.h"
 #include "trigger.h"
 
-#define DENSITY_MEASURMENT_COLLECTS_INTEGER 1
 typedef double evaluation_type;
 typedef uint64_t measurement_type;
 
-#define MAX_ALLOWED_VARIANCE	1e-9
-#define MINIMUM_ALTITUDE_RANGE	300.0f
-#define ALTITUDE_TRIGGER_HYSTERESIS 50.0f
+#define ALTITUDE_TRIGGER_HYSTERESIS	50.0f
+#define MAX_ALLOWED_SLOPE_VARIANCE	3e-9
+#define MAX_ALLOWED_OFFSET_VARIANCE	200
+#define MINIMUM_ALTITUDE_RANGE		300.0f
+#define MAXIMUM_ALTITUDE_RANGE		800.0f
+#define USE_AIR_DENSITY_LETHARGY	1
+#define AIR_DENSITY_LETHARGY 		0.7
+#define AIR_DENSITY_DECIMATION		20
 
-//! this class maintains offset and slope of the air density measurement
+//! Maintains offset and slope of the air density measurement
 class air_data_result
 {
 public:
   air_data_result( void)
     : density_correction(1.0f),
+      density_variance(1.0f),
       QFF(101325.0f),
       valid( false)
   {}
   float density_correction;
+  float density_variance;
   float QFF;
   bool valid;
 };
 
-//! this class measures air density and QFF
-class air_density_observer
+//! Measures air density and reference pressure
+class air_density_observer_t
 {
 public:
-  air_density_observer (void)
+  air_density_observer_t (void)
   : min_altitude(10000.0f),
     max_altitude(0.0f),
-    altitude_trigger( ALTITUDE_TRIGGER_HYSTERESIS)
+    altitude_trigger( ALTITUDE_TRIGGER_HYSTERESIS),
+    decimation_counter( 20),
+    altitude_decimation_filter( 1.0f / AIR_DENSITY_DECIMATION * 0.25f),
+    pressure_decimation_filter( 1.0f / AIR_DENSITY_DECIMATION * 0.25f)
   {
   }
   air_data_result feed_metering( float pressure, float MSL_altitude);
@@ -75,6 +85,9 @@ class air_density_observer
     float min_altitude;
     float max_altitude;
     trigger altitude_trigger;
+    unsigned decimation_counter;
+    pt2 <float, float> altitude_decimation_filter;
+    pt2 <float, float> pressure_decimation_filter;
 };
 
 #endif /* AIR_DENSITY_OBSERVER_H_ */

NAV_Algorithms/airborne_detector.h

@@ -50,7 +50,13 @@ class airborne_detector_t
 	}
       }
   }
-    bool detect_just_landed( void)
+
+  bool is_airborne( void)
+    {
+      return airborne_counter > 0;
+    }
+
+  bool detect_just_landed( void)
     {
       if( just_landed)
 	{

NAV_Algorithms/atmosphere.h

@@ -28,6 +28,7 @@
 
 #include "embedded_math.h"
 #include <air_density_observer.h>
+#include "NAV_tuning_parameters.h"
 #include <pt2.h>
 
 #define RECIP_STD_DENSITY_TIMES_2 1.632f
@@ -41,7 +42,7 @@
 /*! The offest for the conversion from degree celsius to kelvin */
 #define CELSIUS_TO_KELVIN_OFFSET 273.15f
 
-//! this class maintains instant atmosphere data like pressure, density etc
+//! Maintenance of atmosphere data like pressure, density etc.
 class atmosphere_t
 {
 public:
@@ -52,18 +53,26 @@ class atmosphere_t
     temperature(20.0f),
     humidity( 0.0f),
     density_correction(1.0f),
-    density_correction_averager(0.001f),
-    QFF(101325)
+    extrapolated_sea_level_pressure(101325),
+    GNSS_altitude_based_density_available(false),
+    GNSS_altitude_based_density(1.2255f),
+    weight_sum(0.0f),
+    density_factor_weighed_sum(0.0f)
   {
-    density_correction_averager.settle(1.0f);
   }
-  void update_density_correction( void)
+  void update_density( float GNSS_altitude, bool valid)
   {
-    density_correction_averager.respond(density_correction);
+    if( valid)
+      {
+	GNSS_altitude_based_density = get_std_density( GNSS_altitude) * density_correction;
+	GNSS_altitude_based_density_available = true;
+      }
+    else
+      GNSS_altitude_based_density_available = false;
   }
   void initialize( float altitude)
   {
-    density_QFF_calculator.initialize(altitude);
+    air_density_observer.initialize(altitude);
   }
   void set_pressure( float p_abs)
   {
@@ -73,11 +82,25 @@ class atmosphere_t
   {
     return pressure;
   }
+  float get_std_density( float GNSS_altitude)
+  {
+    float std_density =
+	0.000000003547494f * GNSS_altitude * GNSS_altitude
+	-0.000115412739613f * GNSS_altitude + 1.224096628212817f;
+    return std_density;
+  }
+  float get_pressure_density( float static_pressure)
+  {
+    return 1.0496346613e-5f * static_pressure + 0.1671546011f;
+  }
   float get_density( void) const
   {
-    return  (1.0496346613e-5f * pressure + 0.1671546011f) * density_correction_averager.get_output();
+    if( GNSS_altitude_based_density_available)
+      return GNSS_altitude_based_density;
+    else
+      return  (1.0496346613e-5f * pressure + 0.1671546011f) * density_correction;
   }
-  float get_negative_altitude( void) const
+  float get_negative_pressure_altitude( void) const
   {
     float tmp = 8.104381531e-4f * pressure;
     return - tmp * tmp  + 0.20867299170f * pressure - 14421.43945f;
@@ -101,18 +124,28 @@ class atmosphere_t
     have_ambient_air_data = false;
   }
 
-  float get_QFF () const
+  float get_extrapolated_sea_level_pressure () const
   {
-    return QFF;
+    return extrapolated_sea_level_pressure;
   }
 
-  void feed_QFF_density_metering( float pressure, float MSL_altitude)
+  void air_density_metering( float pressure, float MSL_altitude)
     {
-    air_data_result result = density_QFF_calculator.feed_metering( pressure, MSL_altitude);
+    air_data_result result = air_density_observer.feed_metering( pressure, MSL_altitude);
       if( result.valid)
 	{
-	  QFF = result.QFF;
+#if USE_AIR_DENSITY_LETHARGY
+	  bool first_measurement = (weight_sum == 0.0f);
+
+	  weight_sum =  weight_sum * AIR_DENSITY_LETHARGY + (AIR_DENSITY_LETHARGY -1) / result.density_variance;
+	  density_factor_weighed_sum =  density_factor_weighed_sum * AIR_DENSITY_LETHARGY + (AIR_DENSITY_LETHARGY -1) * result.density_correction / result.density_variance;
+
+	  // postpone update unless we have two measurements
+	  if( ! first_measurement)
+	    density_correction = density_factor_weighed_sum / weight_sum;
+#else
 	  density_correction = result.density_correction;
+#endif
 	}
     }
 
@@ -127,9 +160,13 @@ class atmosphere_t
   float temperature;
   float humidity;
   float density_correction;
-  pt2<float,float> density_correction_averager;
-  float QFF;
-  air_density_observer density_QFF_calculator;
+  float extrapolated_sea_level_pressure;
+  air_density_observer_t air_density_observer;
+  bool GNSS_altitude_based_density_available;
+  float GNSS_altitude_based_density;
+  float old_density_correction;
+  float weight_sum;
+  float density_factor_weighed_sum;
 };
 
 #endif /* APPLICATION_ATMOSPHERE_H_ */

NAV_Algorithms/navigator.cpp

@@ -52,7 +52,7 @@ void navigator_t::update_at_100Hz (
       ahrs.get_nav_acceleration (),
       heading_vector,
       GNSS_negative_altitude,
-      atmosphere.get_negative_altitude(),
+      atmosphere.get_negative_pressure_altitude(),
       IAS,
       wind_observer.get_speed_compensator_wind(),
       (GNSS_fix_type != 0)
@@ -85,11 +85,8 @@ void navigator_t::update_GNSS_data( const coordinates_t &coordinates)
 bool navigator_t::update_at_10Hz ()
 {
   bool landing_detected=false;
-  atmosphere.feed_QFF_density_metering(
-	air_pressure_resampler_100Hz_10Hz.get_output(),
-	flight_observer.get_filtered_GNSS_altitude());
 
-  atmosphere.update_density_correction(); // here because of the 10 Hz call frequency
+  atmosphere.update_density( -GNSS_negative_altitude, GNSS_fix_type > 0);
 
   wind_observer.process_at_10_Hz( ahrs);
 
@@ -103,6 +100,13 @@ bool navigator_t::update_at_10Hz ()
       ahrs.write_calibration_into_EEPROM();
       landing_detected = true;
     }
+
+  if( airborne_detector.is_airborne())
+    atmosphere.air_density_metering(
+	air_pressure_resampler_100Hz_10Hz.get_output(),
+	flight_observer.get_filtered_GNSS_altitude());
+
+
   return landing_detected;
 }
 
@@ -133,14 +137,14 @@ void navigator_t::report_data( output_data_t &d)
     d.slip_angle		= ahrs.getSlipAngle();
     d.pitch_angle		= ahrs.getPitchAngle();
     d.G_load			= ahrs.get_G_load();
-    d.pressure_altitude		= - atmosphere.get_negative_altitude();
+    d.pressure_altitude		= - atmosphere.get_negative_pressure_altitude();
     d.magnetic_disturbance	= ahrs.getMagneticDisturbance();
     d.air_density		= atmosphere.get_density();
     d.nav_induction_gnss 	= ahrs.get_nav_induction();
 
 #if DEVELOPMENT_ADDITIONS
 
-    d.QFF			= atmosphere.get_QFF();
+    d.QFF			= atmosphere.get_extrapolated_sea_level_pressure();
     d.nav_correction		= ahrs.get_nav_correction();
     d.gyro_correction		= ahrs.get_gyro_correction();
     d.nav_acceleration_gnss 	= ahrs.get_nav_acceleration();