organizer.h

Go to the documentation of this file.

/***********************************************************************/
#ifndef ORGANIZER_H_
#define ORGANIZER_H_
 
#include <variometer.h>
#include "data_structures.h"
#include "navigator.h"
#include "earth_induction_model.h"
 
class organizer_t
{
public:
  organizer_t( void)
    : pitot_offset(0.0f),
      pitot_span(0.0f),
      QNH_offset(0.0f),
      magnetic_induction_update_counter(0)
  {
 
  }
 
  void initialize_before_measurement( void)
  {
    pitot_offset= configuration (PITOT_OFFSET);
    pitot_span  = configuration (PITOT_SPAN);
    QNH_offset  = configuration (QNH_OFFSET);
 
      {
        quaternion<float> q;
        q.from_euler (configuration (SENS_TILT_ROLL),
              configuration (SENS_TILT_PITCH),
              configuration (SENS_TILT_YAW));
        q.get_rotation_matrix (sensor_mapping);
      }
 
  }
 
  void update_magnetic_induction_data( double latitude, double longitude)
  {
    induction_values induction_data;
    induction_data = earth_induction_model.get_induction_data_at( latitude, longitude);
    if( induction_data.valid)
      navigator.update_magnetic_induction_data( induction_data.declination, induction_data.inclination);
  }
 
  void initialize_after_first_measurement( output_data_t & output_data)
  {
    navigator.update_pressure( output_data.m.static_pressure - QNH_offset);
    navigator.initialize_QFF_density_metering( -output_data.c.position[DOWN]);
    navigator.reset_altitude ();
 
    // setup initial attitude
    acc = sensor_mapping * output_data.m.acc;
    mag = sensor_mapping * output_data.m.mag;
 
    if (output_data.c.sat_fix_type & SAT_HEADING)
      {
    navigator.set_attitude ( 0.0f, 0.0f, output_data.c.relPosHeading); // todo use acc data some day ?
      }
    else
      navigator.set_from_add_mag( acc, mag); // initialize attitude from acceleration + compass
  }
 
  void on_new_pressure_data( output_data_t & output_data)
  {
    navigator.update_pressure(output_data.m.static_pressure - QNH_offset);
    navigator.update_pitot ( (output_data.m.pitot_pressure - pitot_offset) * pitot_span);
  }
 
  bool update_every_100ms( output_data_t & output_data)
  {
    bool landing_detected = navigator.update_at_10Hz ();
    navigator.feed_QFF_density_metering( output_data.m.static_pressure - QNH_offset, -output_data.c.position[DOWN]);
 
    if( ++magnetic_induction_update_counter > 36000) // every hour
      {
    update_magnetic_induction_data( output_data.c.latitude, output_data.c.longitude);
    magnetic_induction_update_counter=0;
      }
    return landing_detected;
  }
 
  void set_attitude ( float roll, float nick, float present_heading)
  {
    navigator.set_attitude ( roll, nick, present_heading);
  }
  void update_GNSS_data( const coordinates_t &coordinates)
  {
    navigator.update_GNSS_data(coordinates);
  }
 
  void update_every_10ms( output_data_t & output_data, float *tweaks)
  {
#if 0 // modify many parameters
    // rotate sensor coordinates into airframe coordinates
    float3vector acc_mod = output_data.m.acc;
 
    acc_mod[0] += tweaks[0];
    acc_mod[0] *= 1.0f + tweaks[1];
    acc_mod[1] += tweaks[2];
    acc_mod[1] *= 1.0f + tweaks[3];
    acc_mod[2] += tweaks[4];
    acc_mod[2] *= 1.0f + tweaks[5];
 
    float3vector mag_mod = output_data.m.mag;
        
    mag_mod[0] += tweaks[9] * output_data.m.mag[1] + tweaks[10] * output_data.m.mag[2];
    mag_mod[1] += tweaks[11] * output_data.m.mag[0] + tweaks[12] * output_data.m.mag[2];
    mag_mod[2] += tweaks[13] * output_data.m.mag[0] + tweaks[14] * output_data.m.mag[1];
 
    float3vector gyro_mod = output_data.m.gyro;
 
    gyro_mod[0] *= 1.0f + tweaks[6];
    gyro_mod[1] *= 1.0f + tweaks[7];
    gyro_mod[2] *= 1.0f + tweaks[8];
 
    acc  = sensor_mapping * acc_mod;
    mag  = sensor_mapping * mag_mod;
    gyro = sensor_mapping * gyro_mod;
#endif
#if 1
    float3vector acc_mod = output_data.m.acc;
    acc_mod[0] = (1.0f + tweaks[0]) * output_data.m.acc[0] + tweaks[1];
    acc_mod[1] = (1.0f + tweaks[2]) * output_data.m.acc[1] + tweaks[3];
    acc_mod[2] = (1.0f + tweaks[4]) * output_data.m.acc[2] + tweaks[5];
 
    acc  = sensor_mapping * acc_mod;
    mag  = sensor_mapping * output_data.m.mag;
    gyro = sensor_mapping * output_data.m.gyro;
#endif
#if 0 // magnetic sensor matrix optimization matrix
    float3vector mag_mod = output_data.m.mag;
    mag_mod[0] = 1.0f  * output_data.m.mag[0] + tweaks[1] * output_data.m.mag[1] + tweaks[2] * output_data.m.mag[2];
    mag_mod[1] = tweaks[3] * output_data.m.mag[0] + 1.0f * output_data.m.mag[1] + tweaks[5] * output_data.m.mag[2];
    mag_mod[2] = tweaks[6] * output_data.m.mag[0] + tweaks[7] * output_data.m.mag[1] + 1.0f  * output_data.m.mag[2];
 
    acc  = sensor_mapping * output_data.m.acc;
    mag  = sensor_mapping * mag_mod;
    gyro = sensor_mapping * output_data.m.gyro;
#endif
#if 0 // gyro matrix
    float3vector gyro_mod;
 
    gyro_mod[0] = (1.0f + tweaks[0]) * output_data.m.gyro[0] + tweaks[1] * output_data.m.gyro[1] + tweaks[2] * output_data.m.gyro[2];
    gyro_mod[1] = tweaks[3] * output_data.m.gyro[0] + (1.0f + tweaks[4]) * output_data.m.gyro[1] + tweaks[5] * output_data.m.gyro[2];
    gyro_mod[2] = tweaks[6] * output_data.m.gyro[0] + tweaks[7] * output_data.m.gyro[1] + (1.0f + tweaks[8]) * output_data.m.gyro[2];
 
    acc  = sensor_mapping * output_data.m.acc;
    mag  = sensor_mapping * output_data.m.mag;
    gyro = sensor_mapping * gyro_mod;
#endif
#if DEVELOPMENT_ADDITIONS
    output_data.body_acc  = acc;
    output_data.body_gyro = gyro;
#endif
 
    navigator.update_at_100Hz (acc, mag, gyro);
  }
 
  void report_data ( output_data_t &data)
  {
    navigator.report_data ( data);
  }
 
  void set_density_data( float temp, float humidity)
  {
    navigator.set_density_data( temp, humidity);
  }
 
  void disregard_density_data()
  {
    navigator.disregard_density_data();
  }
 
private:
  navigator_t navigator;
  float3vector acc; 
  float3vector mag; 
  float3vector gyro; 
  float3matrix sensor_mapping; 
  float pitot_offset; 
  float pitot_span;   
  float QNH_offset;   
  unsigned magnetic_induction_update_counter;
};
 
#endif /* ORGANIZER_H_ */