diff --git a/NAV_Algorithms/air_density_observer.cpp b/NAV_Algorithms/air_density_observer.cpp index 7e4c1c5..45d3d21 100644 --- a/NAV_Algorithms/air_density_observer.cpp +++ b/NAV_Algorithms/air_density_observer.cpp @@ -25,60 +25,69 @@ #include "embedded_math.h" #include -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 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; diff --git a/NAV_Algorithms/air_density_observer.h b/NAV_Algorithms/air_density_observer.h index f5cbc0e..5f126f5 100644 --- a/NAV_Algorithms/air_density_observer.h +++ b/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 altitude_decimation_filter; + pt2 pressure_decimation_filter; }; #endif /* AIR_DENSITY_OBSERVER_H_ */ diff --git a/NAV_Algorithms/airborne_detector.h b/NAV_Algorithms/airborne_detector.h index d3fb97e..39e716c 100644 --- a/NAV_Algorithms/airborne_detector.h +++ b/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) { diff --git a/NAV_Algorithms/atmosphere.h b/NAV_Algorithms/atmosphere.h index 851ab12..f5c37c6 100644 --- a/NAV_Algorithms/atmosphere.h +++ b/NAV_Algorithms/atmosphere.h @@ -28,6 +28,7 @@ #include "embedded_math.h" #include +#include "NAV_tuning_parameters.h" #include #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 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_ */ diff --git a/NAV_Algorithms/navigator.cpp b/NAV_Algorithms/navigator.cpp index 5949775..cb27554 100644 --- a/NAV_Algorithms/navigator.cpp +++ b/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(); diff --git a/NAV_Algorithms/navigator.h b/NAV_Algorithms/navigator.h index 1fe285f..815993f 100644 --- a/NAV_Algorithms/navigator.h +++ b/NAV_Algorithms/navigator.h @@ -50,7 +50,7 @@ class navigator_t flight_observer(), wind_observer(), airborne_detector(), - air_pressure_resampler_100Hz_10Hz(0.04f), // f / fcutoff = 80% * 0.5 * 0.1 + air_pressure_resampler_100Hz_10Hz(0.025f), // = 2.5 Hz @ 100Hz pitot_pressure(0.0f), TAS( 0.0f), IAS( 0.0f), @@ -74,7 +74,6 @@ class navigator_t ahrs_magnetic.update_magnetic_induction_data( declination, inclination); #endif } - void set_density_data( float temperature, float humidity) { if( ! isnan( temperature) && ! isnan( humidity) ) @@ -89,7 +88,7 @@ class navigator_t void feed_QFF_density_metering( float pressure, float MSL_altitude) { - atmosphere.feed_QFF_density_metering( pressure, MSL_altitude); + atmosphere.air_density_metering( pressure, MSL_altitude); } void disregard_density_data( void) @@ -119,7 +118,7 @@ class navigator_t void reset_altitude( void) { - flight_observer.reset( atmosphere.get_negative_altitude(), GNSS_negative_altitude); + flight_observer.reset( atmosphere.get_negative_pressure_altitude(), GNSS_negative_altitude); } /** * @brief update pitot pressure diff --git a/NAV_Algorithms/organizer.h b/NAV_Algorithms/organizer.h index 73efd95..e6a0643 100644 --- a/NAV_Algorithms/organizer.h +++ b/NAV_Algorithms/organizer.h @@ -115,16 +115,9 @@ class organizer_t void update_every_10ms( output_data_t & output_data) { - // rotate sensor coordinates into airframe coordinates -#if USE_LOWCOST_IMU == 1 - acc = sensor_mapping * output_data.m.lowcost_acc; - mag = sensor_mapping * output_data.m.lowcost_mag; - gyro = sensor_mapping * output_data.m.lowcost_gyro; -#else acc = sensor_mapping * output_data.m.acc; mag = sensor_mapping * output_data.m.mag; gyro = sensor_mapping * output_data.m.gyro; -#endif #if DEVELOPMENT_ADDITIONS output_data.body_acc = acc;