navigator.h

Go to the documentation of this file.

/***********************************************************************/
#ifndef NAVIGATORT_H_
#define NAVIGATORT_H_
 
#include <system_configuration.h>
#include <AHRS.h>
#include <soaring_flight_averager.h>
#include <variometer.h>
#include "GNSS.h"
#include "differentiator.h"
#include "atmosphere.h"
#include "data_structures.h"
#include "accumulating_averager.h"
#include "airborne_detector.h"
#include "wind_observer.h"
 
class navigator_t
{
public:
  navigator_t (void)
    :ahrs (0.01f),
#if DEVELOPMENT_ADDITIONS
     ahrs_magnetic (0.01f),
#endif
     atmosphere (101325.0f),
     flight_observer(),
     wind_observer(),
     airborne_detector(),
     air_pressure_resampler_100Hz_10Hz(0.04f), // f / fcutoff = 80% * 0.5 * 0.1
     pitot_pressure(0.0f),
     TAS( 0.0f),
     IAS( 0.0f),
     GNSS_velocity(),
     GNSS_speed(),
     GNSS_acceleration(),
     GNSS_heading(),
     GNSS_negative_altitude( ZERO),
     GNSS_fix_type( 0),
     vario_integrator( configuration( VARIO_INT_TC) < 0.25f
       ? configuration( VARIO_INT_TC) // normalized stop frequency given, old version
       : (FAST_SAMPLING_TIME / configuration( VARIO_INT_TC) ) ), // time-constant given, new version
     TAS_averager(1.0f / 1.0f / 100.0f),
     IAS_averager(1.0f / 1.0f / 100.0f)
  {};
 
  void update_magnetic_induction_data( float declination, float inclination)
  {
    ahrs.update_magnetic_induction_data( declination, inclination);
#if DEVELOPMENT_ADDITIONS
    ahrs_magnetic.update_magnetic_induction_data( declination, inclination);
#endif
  }
 
  void set_density_data( float temperature, float humidity)
  {
    if( ! isnan( temperature) && ! isnan( humidity) )
      atmosphere.set_ambient_air_data( CLIP( temperature, -40.0f, 50.0f), CLIP( humidity, 0.0f, 1.0f));
    else
      atmosphere.disregard_ambient_air_data();
  }
  void initialize_QFF_density_metering( float MSL_altitude)
  {
    atmosphere.initialize( MSL_altitude);
  }
 
  void feed_QFF_density_metering( float pressure, float MSL_altitude)
  {
    atmosphere.feed_QFF_density_metering( pressure, MSL_altitude);
  }
 
  void disregard_density_data( void)
  {
    atmosphere.disregard_ambient_air_data();
  }
 
  void report_data( output_data_t &d);
 
  void set_from_add_mag ( const float3vector &acc, const float3vector &mag)
  {
    ahrs.attitude_setup(acc, mag);
  }
  void set_from_euler ( float r, float n, float y)
  {
    ahrs.set_from_euler(r, n, y);
  }
  void update_pressure( float pressure)
  {
    atmosphere.set_pressure(pressure);
    air_pressure_resampler_100Hz_10Hz.respond(pressure);
  }
 
  void reset_altitude( void)
  {
    flight_observer.reset( atmosphere.get_negative_altitude(), GNSS_negative_altitude);
  }
  void update_pitot( float pressure)
  {
    pitot_pressure = pressure < 0.0f ? 0.0f : pressure;
    if( pitot_pressure > 4500.0f)
      return; // ignore implausible spikes
 
    TAS = atmosphere.get_TAS_from_dynamic_pressure ( pitot_pressure);
    if( TAS > 5.0f)
      TAS_averager.respond(TAS);
    else
      TAS_averager.settle(0.0f); // avoid underflow on decay
 
    IAS = atmosphere.get_IAS_from_dynamic_pressure ( pitot_pressure);
    if( IAS > 5.0f)
      IAS_averager.respond(IAS);
    else
      IAS_averager.settle(0.0f); // avoid underflow on decay
  }
 
  void update_at_100Hz( const float3vector &acc, const float3vector &mag, const float3vector &gyro);
 
  bool update_at_10Hz();
 
  void update_GNSS_data( const coordinates_t &coordinates);
 
//  const flight_observer_t &get_flight_observer( void) const
//    {
//    return flight_observer;
//    }
 
  void set_attitude( float roll, float nick, float yaw)
  {
    ahrs.set_from_euler(roll, nick, yaw);
#if DEVELOPMENT_ADDITIONS
    ahrs_magnetic.set_from_euler(roll, nick, yaw);
#endif
  }
 
  float get_IAS( void) const
  {
    return IAS;
  }
 
private:
  AHRS_type ahrs;
#if DEVELOPMENT_ADDITIONS
  AHRS_type ahrs_magnetic;
#endif
  atmosphere_t      atmosphere;
  variometer_t  flight_observer;
  wind_oberserver_t wind_observer;
  airborne_detector_t   airborne_detector;
 
  pt2<float,float> air_pressure_resampler_100Hz_10Hz;
  float     pitot_pressure;
  float     TAS;
  float     IAS;
 
  float3vector  GNSS_velocity; 
  float     GNSS_speed; 
  float3vector  GNSS_acceleration;
  float     GNSS_heading;
  float     GNSS_negative_altitude;
  unsigned  GNSS_fix_type;
 
  soaring_flight_averager< float, false, false> vario_integrator;
  pt2<float,float> TAS_averager;
  pt2<float,float> IAS_averager;
};
 
#endif /* NAVIGATORT_H_ */