Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
ahrs_float_dcm.h
Go to the documentation of this file.
1 /*
2  * Released under Creative Commons License
3  *
4  * 2010 The Paparazzi Team
5  *
6  *
7  * Based on Code by Jordi Munoz and William Premerlani, Supported by Chris Anderson (Wired) and Nathan Sindle (SparkFun).
8  * Version 1.0 for flat board updated by Doug Weibel and Jose Julio
9  *
10  * Modified at Hochschule Bremen, Germany
11  * 2010 Heinrich Warmers, Christoph Niemann, Oliver Riesener
12  *
13  */
14 
24 #ifndef AHRS_FLOAT_DCM_H
25 #define AHRS_FLOAT_DCM_H
26 
27 #include <inttypes.h>
30 #include "subsystems/gps.h"
31 
35 };
36 
37 struct AhrsFloatDCM {
40 
43 
44  float gps_speed;
46  float gps_course;
49 
51 
53  bool is_aligned;
54 };
55 extern struct AhrsFloatDCM ahrs_dcm;
56 
57 // DCM Parameters
58 
59 //#define Kp_ROLLPITCH 0.2
60 #define Kp_ROLLPITCH 0.015
61 #define Ki_ROLLPITCH 0.000010
62 #define Kp_YAW 0.9 //High yaw drift correction gain - use with caution!
63 #define Ki_YAW 0.00005
64 
65 #define GRAVITY 9.81
66 
67 
68 #ifndef OUTPUTMODE
69 #define OUTPUTMODE 1
70 #endif
71 // Mode 0 = DCM integration without Ki gyro bias
72 // Mode 1 = DCM integration with Kp and Ki
73 // Mode 2 = direct accelerometer -> euler
74 
75 
76 #define PERFORMANCE_REPORTING 1
77 #if PERFORMANCE_REPORTING == 1
78 extern int renorm_sqrt_count;
79 extern int renorm_blowup_count;
80 extern float imu_health;
81 #endif
82 
83 extern void ahrs_dcm_init(void);
85 extern void ahrs_dcm_set_body_to_imu_quat(struct FloatQuat *q_b2i);
86 extern bool ahrs_dcm_align(struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel,
87  struct FloatVect3 *lp_mag);
88 extern void ahrs_dcm_propagate(struct FloatRates *gyro, float dt);
89 extern void ahrs_dcm_update_accel(struct FloatVect3 *accel);
90 extern void ahrs_dcm_update_mag(struct FloatVect3 *mag);
91 extern void ahrs_dcm_update_gps(struct GpsState *gps_s);
92 
93 #endif // AHRS_FLOAT_DCM_H
AhrsFloatDCM::gps_age
uint8_t gps_age
Definition: ahrs_float_dcm.h:48
OrientationReps
Definition: pprz_orientation_conversion.h:79
ahrs_dcm_set_body_to_imu_quat
void ahrs_dcm_set_body_to_imu_quat(struct FloatQuat *q_b2i)
Definition: ahrs_float_dcm.c:541
AhrsFloatDCM
Definition: ahrs_float_dcm.h:37
GpsState
data structure for GPS information
Definition: gps.h:87
AhrsFloatDCM::imu_rate
struct FloatRates imu_rate
Definition: ahrs_float_dcm.h:42
AHRS_DCM_UNINIT
@ AHRS_DCM_UNINIT
Definition: ahrs_float_dcm.h:33
imu_health
float imu_health
pprz_algebra_float.h
Paparazzi floating point algebra.
AhrsFloatDCM::rate_correction
struct FloatRates rate_correction
Definition: ahrs_float_dcm.h:39
AhrsFloatDCM::is_aligned
bool is_aligned
Definition: ahrs_float_dcm.h:53
ahrs_dcm
struct AhrsFloatDCM ahrs_dcm
Definition: ahrs_float_dcm.c:52
AhrsDCMStatus
AhrsDCMStatus
Definition: ahrs_float_dcm.h:32
FloatVect3
Definition: pprz_algebra_float.h:54
FloatQuat
Roation quaternion.
Definition: pprz_algebra_float.h:63
AhrsFloatDCM::gyro_bias
struct FloatRates gyro_bias
Definition: ahrs_float_dcm.h:38
gps.h
Device independent GPS code (interface)
AHRS_DCM_RUNNING
@ AHRS_DCM_RUNNING
Definition: ahrs_float_dcm.h:34
AhrsFloatDCM::status
enum AhrsDCMStatus status
Definition: ahrs_float_dcm.h:52
uint8_t
unsigned char uint8_t
Definition: types.h:14
renorm_sqrt_count
int renorm_sqrt_count
AhrsFloatDCM::ltp_to_imu_euler
struct FloatEulers ltp_to_imu_euler
Definition: ahrs_float_dcm.h:41
AhrsFloatDCM::gps_course_valid
bool gps_course_valid
Definition: ahrs_float_dcm.h:47
ahrs_dcm_propagate
void ahrs_dcm_propagate(struct FloatRates *gyro, float dt)
Definition: ahrs_float_dcm.c:148
AhrsFloatDCM::gps_course
float gps_course
Definition: ahrs_float_dcm.h:46
pprz_orientation_conversion.h
ahrs_dcm_set_body_to_imu
void ahrs_dcm_set_body_to_imu(struct OrientationReps *body_to_imu)
Definition: ahrs_float_dcm.c:536
renorm_blowup_count
int renorm_blowup_count
body_to_imu
static struct OrientationReps body_to_imu
Definition: ins_alt_float.c:93
ahrs_dcm_align
bool ahrs_dcm_align(struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel, struct FloatVect3 *lp_mag)
Definition: ahrs_float_dcm.c:119
FloatEulers
euler angles
Definition: pprz_algebra_float.h:84
ahrs_dcm_update_accel
void ahrs_dcm_update_accel(struct FloatVect3 *accel)
Definition: ahrs_float_dcm.c:200
AhrsFloatDCM::gps_speed
float gps_speed
Definition: ahrs_float_dcm.h:44
AhrsFloatDCM::gps_acceleration
float gps_acceleration
Definition: ahrs_float_dcm.h:45
inttypes.h
ahrs_dcm_init
void ahrs_dcm_init(void)
Definition: ahrs_float_dcm.c:99
AhrsFloatDCM::body_to_imu
struct OrientationReps body_to_imu
Definition: ahrs_float_dcm.h:50
ahrs_dcm_update_mag
void ahrs_dcm_update_mag(struct FloatVect3 *mag)
Definition: ahrs_float_dcm.c:227
FloatRates
angular rates
Definition: pprz_algebra_float.h:93
ahrs_dcm_update_gps
void ahrs_dcm_update_gps(struct GpsState *gps_s)