Paparazzi UAS  v6.2_unstable
Paparazzi is a free software Unmanned Aircraft System.
ahrs_int_cmpl_quat.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2008-2013 The Paparazzi Team
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  */
21 
31 #ifndef AHRS_INT_CMPL_QUAT_H
32 #define AHRS_INT_CMPL_QUAT_H
33 
34 #include "modules/ahrs/ahrs.h"
35 #include "modules/gps/gps.h"
36 #include "std.h"
37 #include "math/pprz_algebra_int.h"
39 
43 };
44 
55  struct Int32Vect3 mag_h;
56 
60  float weight;
61  float accel_inv_kp;
62  float accel_inv_ki;
63  float mag_kp;
64  float mag_ki;
65 
66  /* parameters/options that can be changed */
69 
74 
79  float accel_omega;
80 
85  float accel_zeta;
86 
90  float mag_omega;
91 
95  float mag_zeta;
96 
97  /* internal counters for the gains */
100 
103 };
104 
105 extern struct AhrsIntCmplQuat ahrs_icq;
106 
107 extern void ahrs_icq_init(void);
108 extern bool ahrs_icq_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
109  struct Int32Vect3 *lp_mag);
110 extern void ahrs_icq_propagate(struct Int32Rates *gyro, float dt);
111 extern void ahrs_icq_update_accel(struct Int32Vect3 *accel, float dt);
112 extern void ahrs_icq_update_mag(struct Int32Vect3 *mag, float dt);
113 extern void ahrs_icq_update_gps(struct GpsState *gps_s);
114 
120 
127 
128 
130 extern void ahrs_icq_set_accel_gains(void);
131 
132 static inline void ahrs_int_cmpl_quat_SetAccelOmega(float omega)
133 {
134  ahrs_icq.accel_omega = omega;
136 }
137 
138 static inline void ahrs_int_cmpl_quat_SetAccelZeta(float zeta)
139 {
140  ahrs_icq.accel_zeta = zeta;
142 }
143 
145 extern void ahrs_icq_set_mag_gains(void);
146 
147 static inline void ahrs_int_cmpl_quat_SetMagOmega(float omega)
148 {
149  ahrs_icq.mag_omega = omega;
151 }
152 
153 static inline void ahrs_int_cmpl_quat_SetMagZeta(float zeta)
154 {
155  ahrs_icq.mag_zeta = zeta;
157 }
158 
159 
160 #endif /* AHRS_INT_CMPL_QUAT_H */
uint8_t
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
ahrs_int_cmpl_quat_SetAccelZeta
static void ahrs_int_cmpl_quat_SetAccelZeta(float zeta)
Definition: ahrs_int_cmpl_quat.h:138
AhrsIntCmplQuat::accel_omega
float accel_omega
filter cut-off frequency for correcting the attitude from accels.
Definition: ahrs_int_cmpl_quat.h:79
Int32Rates
angular rates
Definition: pprz_algebra_int.h:179
AhrsIntCmplQuat::mag_omega
float mag_omega
filter cut-off frequency for correcting the attitude (heading) from magnetometer.
Definition: ahrs_int_cmpl_quat.h:90
AhrsIntCmplQuat::ltp_vel_norm_valid
bool ltp_vel_norm_valid
Definition: ahrs_int_cmpl_quat.h:58
AhrsIntCmplQuat::mag_cnt
uint16_t mag_cnt
number of propagations since last mag update
Definition: ahrs_int_cmpl_quat.h:99
AhrsIntCmplQuat::accel_inv_kp
float accel_inv_kp
Definition: ahrs_int_cmpl_quat.h:61
Int32Quat
Rotation quaternion.
Definition: pprz_algebra_int.h:99
GpsState
data structure for GPS information
Definition: gps.h:90
ahrs_icq_set_accel_gains
void ahrs_icq_set_accel_gains(void)
update pre-computed inv_kp and inv_ki gains from acc_omega and acc_zeta
Definition: ahrs_int_cmpl_quat.c:219
AHRS_ICQ_RUNNING
@ AHRS_ICQ_RUNNING
Definition: ahrs_int_cmpl_quat.h:42
AhrsIntCmplQuat::ltp_to_body_quat
struct Int32Quat ltp_to_body_quat
Definition: ahrs_int_cmpl_quat.h:54
ahrs_icq_propagate
void ahrs_icq_propagate(struct Int32Rates *gyro, float dt)
Definition: ahrs_int_cmpl_quat.c:186
AhrsIntCmplQuat::mag_zeta
float mag_zeta
filter damping for correcting the gyro bias from magnetometer.
Definition: ahrs_int_cmpl_quat.h:95
ahrs_int_cmpl_quat_SetAccelOmega
static void ahrs_int_cmpl_quat_SetAccelOmega(float omega)
Definition: ahrs_int_cmpl_quat.h:132
AhrsIntCmplQuat::status
enum AhrsICQStatus status
status of the AHRS, AHRS_ICQ_UNINIT or AHRS_ICQ_RUNNING
Definition: ahrs_int_cmpl_quat.h:101
ahrs_icq_align
bool ahrs_icq_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel, struct Int32Vect3 *lp_mag)
Definition: ahrs_int_cmpl_quat.c:157
AhrsIntCmplQuat::heading_aligned
bool heading_aligned
Definition: ahrs_int_cmpl_quat.h:59
Int64Rates
Definition: pprz_algebra_int.h:185
pprz_algebra_int.h
Paparazzi fixed point algebra.
AhrsIntCmplQuat
Ahrs implementation specifc values.
Definition: ahrs_int_cmpl_quat.h:48
std.h
ahrs_icq_set_mag_gains
void ahrs_icq_set_mag_gains(void)
update pre-computed kp and ki gains from mag_omega and mag_zeta
Definition: ahrs_int_cmpl_quat.c:393
AhrsIntCmplQuat::accel_inv_ki
float accel_inv_ki
Definition: ahrs_int_cmpl_quat.h:62
AhrsIntCmplQuat::weight
float weight
Definition: ahrs_int_cmpl_quat.h:60
AhrsIntCmplQuat::gyro_bias
struct Int32Rates gyro_bias
Definition: ahrs_int_cmpl_quat.h:49
AhrsIntCmplQuat::gravity_heuristic_factor
uint8_t gravity_heuristic_factor
sets how strongly the gravity heuristic reduces accel correction.
Definition: ahrs_int_cmpl_quat.h:73
gps.h
Device independent GPS code (interface)
ahrs_icq_update_gps
void ahrs_icq_update_gps(struct GpsState *gps_s)
Definition: ahrs_int_cmpl_quat.c:521
AhrsIntCmplQuat::correct_gravity
bool correct_gravity
enable gravity vector correction by removing centrifugal acceleration
Definition: ahrs_int_cmpl_quat.h:68
ahrs_icq_update_mag
void ahrs_icq_update_mag(struct Int32Vect3 *mag, float dt)
Definition: ahrs_int_cmpl_quat.c:376
ahrs_icq_update_heading
void ahrs_icq_update_heading(int32_t heading)
Update yaw based on a heading measurement.
Definition: ahrs_int_cmpl_quat.c:557
Int32Vect3
Definition: pprz_algebra_int.h:88
AhrsIntCmplQuat::mag_ki
float mag_ki
Definition: ahrs_int_cmpl_quat.h:64
AhrsIntCmplQuat::mag_h
struct Int32Vect3 mag_h
Definition: ahrs_int_cmpl_quat.h:55
AhrsICQStatus
AhrsICQStatus
Definition: ahrs_int_cmpl_quat.h:40
ahrs.h
ahrs_icq_update_accel
void ahrs_icq_update_accel(struct Int32Vect3 *accel, float dt)
Definition: ahrs_int_cmpl_quat.c:240
Int64Quat
Definition: pprz_algebra_int.h:107
ahrs_icq_init
void ahrs_icq_init(void)
Definition: ahrs_int_cmpl_quat.c:115
int32_t
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
AhrsIntCmplQuat::high_rez_quat
struct Int64Quat high_rez_quat
Definition: ahrs_int_cmpl_quat.h:52
ahrs_icq
struct AhrsIntCmplQuat ahrs_icq
Default Rate filter Low pass.
Definition: ahrs_int_cmpl_quat.c:110
pprz_orientation_conversion.h
ahrs_int_cmpl_quat_SetMagZeta
static void ahrs_int_cmpl_quat_SetMagZeta(float zeta)
Definition: ahrs_int_cmpl_quat.h:153
AhrsIntCmplQuat::high_rez_bias
struct Int64Rates high_rez_bias
Definition: ahrs_int_cmpl_quat.h:53
AhrsIntCmplQuat::mag_kp
float mag_kp
Definition: ahrs_int_cmpl_quat.h:63
AhrsIntCmplQuat::accel_cnt
uint16_t accel_cnt
number of propagations since last accel update
Definition: ahrs_int_cmpl_quat.h:98
AhrsIntCmplQuat::is_aligned
bool is_aligned
Definition: ahrs_int_cmpl_quat.h:102
ahrs_int_cmpl_quat_SetMagOmega
static void ahrs_int_cmpl_quat_SetMagOmega(float omega)
Definition: ahrs_int_cmpl_quat.h:147
AHRS_ICQ_UNINIT
@ AHRS_ICQ_UNINIT
Definition: ahrs_int_cmpl_quat.h:41
ahrs_icq_realign_heading
void ahrs_icq_realign_heading(int32_t heading)
Hard reset yaw to a heading.
Definition: ahrs_int_cmpl_quat.c:614
AhrsIntCmplQuat::ltp_vel_norm
int32_t ltp_vel_norm
Definition: ahrs_int_cmpl_quat.h:57
AhrsIntCmplQuat::accel_zeta
float accel_zeta
filter damping for correcting the gyro-bias from accels.
Definition: ahrs_int_cmpl_quat.h:85
AhrsIntCmplQuat::body_rate
struct Int32Rates body_rate
Definition: ahrs_int_cmpl_quat.h:50
AhrsIntCmplQuat::rate_correction
struct Int32Rates rate_correction
Definition: ahrs_int_cmpl_quat.h:51
uint16_t
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
heading
float heading
Definition: wedgebug.c:258