AHRS.h

Go to the documentation of this file.

/***********************************************************************/
#ifndef AHRS_H_
#define AHRS_H_
 
#include "embedded_math.h"
#include "quaternion.h"
#include "float3vector.h"
#include "float3matrix.h"
#include "integrator.h"
#include "compass_calibration.h"
#include "compass_calibrator_3D.h"
#include "HP_LP_fusion.h"
#include "pt2.h"
 
extern float3vector nav_induction;
 
enum { ROLL, PITCH, YAW};
enum { FRONT, RIGHT, BOTTOM};
enum { NORTH, EAST, DOWN};
 
typedef enum  { STRAIGHT_FLIGHT, TRANSITION, CIRCLING} circle_state_t;
 
typedef integrator<float, float3vector> vector3integrator;
 
class AHRS_type
{
public:
    AHRS_type(float sampling_time);
    void attitude_setup( const float3vector & acceleration, const float3vector & induction);
 
    void update_magnetic_induction_data( float declination, float inclination)
    {
      declination *= (M_PI_F / 180.0f); // degrees to radiant
      inclination *= (M_PI_F / 180.0f);
 
      expected_nav_induction[NORTH] = COS( inclination);
      expected_nav_induction[EAST]  = COS( inclination) * SIN( declination);
      expected_nav_induction[DOWN]  = SIN( inclination);
      update_magnetic_loop_gain(); // adapt to magnetic inclination
    }
 
    void update( const float3vector &gyro, const float3vector &acc, const float3vector &mag,
        const float3vector &GNSS_acceleration,
        float GNSS_heading,
        bool GNSS_heading_valid
        );
 
    inline void set_from_euler( float r, float n, float y)
    {
        attitude.from_euler( r, n, y);
        attitude.get_rotation_matrix( body2nav);
        euler = attitude;
    }
    inline eulerangle<ftype> get_euler(void) const
    {
        return euler;
    }
    inline quaternion<ftype> get_attitude(void) const
    {
        return attitude;
    }
    inline const float3vector &get_nav_acceleration(void) const
    {
        return acceleration_nav_frame;
    }
    inline const float3vector &get_nav_induction(void) const
    {
        return induction_nav_frame;
    }
    inline float get_north(void) const
    {
        return attitude.get_north();
    }
    inline float get_east(void) const
    {
        return attitude.get_east();
    }
    inline float get_down(void) const
    {
        return attitude.get_down();
    }
    inline float get_cross_acc_correction(void) const
    {
        return cross_acc_correction;
    }
    inline const float3vector get_orientation(void) const
    {
      float3vector retv;
      retv[NORTH] = get_north();
      retv[EAST]  = get_east();
      retv[DOWN]  = get_down();
      return retv;
    }
    inline const float3vector &get_gyro_correction(void) const
    {
        return gyro_correction;
    }
    inline const float3matrix &get_body2nav( void) const
    {
      return body2nav;
    }
    inline const float3vector &get_nav_correction(void)
    {
      return nav_correction;
    }
    inline const float3vector &get_gyro_correction(void)
    {
      return gyro_correction;
    }
    circle_state_t get_circling_state(void) const
    {
      return circling_state;
    }
    void write_calibration_into_EEPROM( void);
  float
  getSlipAngle () const
  {
    return slip_angle_averager.get_output();
  }
 
  float
  getPitchAngle () const
  {
    return pitch_angle_averager.get_output();
  }
 
  float get_turn_rate( void ) const
  {
    return turn_rate_averager.get_output();
  }
  float get_G_load( void ) const
  {
    return G_load_averager.get_output();
  }
 
  void update_compass(
          const float3vector &gyro, const float3vector &acc, const float3vector &mag,
          const float3vector &GNSS_acceleration); 
#if 1 // SOFT_IRON_TEST
  void update_ACC_only(
          const float3vector &gyro, const float3vector &acc, const float3vector &mag,
          const float3vector &GNSS_acceleration); 
#endif
  float
  getHeadingDifferenceAhrsDgnss () const
  {
    return heading_difference_AHRS_DGNSS;
  }
 
  float getMagneticDisturbance () const
  {
    return magnetic_disturbance;
  }
 
  float3vector getBodyInductionError () const
  {
    return body_induction_error;
  }
 
  float3vector getBodyInduction () const
  {
    return body_induction;
  }
 
  float getGyro_correction_Power () const
  {
    return gyro_correction_power;
  }
 
private:
  void handle_magnetic_calibration( char type);
 
  void update_magnetic_loop_gain( void)
  {
    float expected_horizontal_induction = SQRT( SQR(expected_nav_induction[EAST])+SQR(expected_nav_induction[NORTH]));
    if( expected_horizontal_induction < 0.001f) // fail-safe default
      magnetic_control_gain = M_H_GAIN;
    else
      magnetic_control_gain = M_H_GAIN / expected_horizontal_induction;
  }
 
  void feed_magnetic_induction_observer(const float3vector &mag_sensor);
  circle_state_t update_circling_state( void);
 
  void update_diff_GNSS( const float3vector &gyro, const float3vector &acc, const float3vector &mag,
      const float3vector &GNSS_acceleration,
      float GNSS_heading);
 
  void update_attitude( const float3vector &acc, const float3vector &gyro, const float3vector &mag);
 
  ftype Ts;
  ftype Ts_div_2;
  quaternion<ftype>attitude;
  float3vector gyro_integrator;
  unsigned circling_counter;
  circle_state_t circling_state;
  float3vector nav_correction;
  float3vector gyro_correction;
  float3vector acceleration_nav_frame;
  float3vector induction_nav_frame;     
  float3vector expected_nav_induction;  
  float3vector expected_body_induction; 
  float3matrix body2nav;
  eulerangle<ftype> euler;
  pt2<float,float> slip_angle_averager;
  pt2<float,float> pitch_angle_averager;
  pt2<float,float> turn_rate_averager;
  pt2<float,float> G_load_averager;
  linear_least_square_fit<int64_t, float> mag_calibration_data_collector_right_turn[3];
  linear_least_square_fit<int64_t, float> mag_calibration_data_collector_left_turn[3];
  compass_calibration_t <int64_t, float> compass_calibration;
  compass_calibrator_3D calib_3D;
  float antenna_DOWN_correction;  
  float antenna_RIGHT_correction; 
  float heading_difference_AHRS_DGNSS;
  float cross_acc_correction;
  float magnetic_disturbance; 
  float magnetic_control_gain; 
  bool automatic_magnetic_calibration;
  bool automatic_earth_field_parameters; // todo unused, remove me some day
  bool magnetic_calibration_updated;
  float3vector body_induction;
  float3vector body_induction_error;
  float gyro_correction_power;
};
 
#endif /* AHRS_H_ */