ahrs_float_dcm.h

Go to the documentation of this file.

/*
 * Released under Creative Commons License
 *
 * 2010 The Paparazzi Team
 *
 *
 * Based on Code by Jordi Munoz and William Premerlani, Supported by Chris Anderson (Wired) and Nathan Sindle (SparkFun).
 * Version 1.0 for flat board updated by Doug Weibel and Jose Julio
 *
 * Modified at Hochschule Bremen, Germany
 * 2010 Heinrich Warmers, Christoph Niemann, Oliver Riesener
 *
 */
 
#ifndef AHRS_FLOAT_DCM_H
#define AHRS_FLOAT_DCM_H
 
#include <inttypes.h>
#include "math/pprz_algebra_float.h"
#include "math/pprz_orientation_conversion.h"
#include "subsystems/gps.h"
 
enum AhrsDCMStatus {
  AHRS_DCM_UNINIT,
  AHRS_DCM_RUNNING
};
 
struct AhrsFloatDCM {
  struct FloatRates gyro_bias;
  struct FloatRates rate_correction;
 
  struct FloatEulers ltp_to_imu_euler;
  struct FloatRates imu_rate;
 
  float gps_speed;
  float gps_acceleration;
  float gps_course;
  bool gps_course_valid;
  uint8_t gps_age;
 
  struct OrientationReps body_to_imu;
 
  enum AhrsDCMStatus status;
  bool is_aligned;
};
extern struct AhrsFloatDCM ahrs_dcm;
 
// DCM Parameters
 
//#define Kp_ROLLPITCH 0.2
#define Kp_ROLLPITCH 0.015
#define Ki_ROLLPITCH 0.000010
#define Kp_YAW 0.9          //High yaw drift correction gain - use with caution!
#define Ki_YAW 0.00005
 
#define GRAVITY 9.81
 
 
#ifndef OUTPUTMODE
#define OUTPUTMODE 1
#endif
// Mode 0 = DCM integration without Ki gyro bias
// Mode 1 = DCM integration with Kp and Ki
// Mode 2 = direct accelerometer -> euler
 
 
#define PERFORMANCE_REPORTING 1
#if PERFORMANCE_REPORTING == 1
extern int renorm_sqrt_count;
extern int renorm_blowup_count;
extern float imu_health;
#endif
 
extern void ahrs_dcm_init(void);
extern void ahrs_dcm_set_body_to_imu(struct OrientationReps *body_to_imu);
extern void ahrs_dcm_set_body_to_imu_quat(struct FloatQuat *q_b2i);
extern bool ahrs_dcm_align(struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel,
                             struct FloatVect3 *lp_mag);
extern void ahrs_dcm_propagate(struct FloatRates *gyro, float dt);
extern void ahrs_dcm_update_accel(struct FloatVect3 *accel);
extern void ahrs_dcm_update_mag(struct FloatVect3 *mag);
extern void ahrs_dcm_update_gps(struct GpsState *gps_s);
 
#endif // AHRS_FLOAT_DCM_H