Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
ahrs_float_cmpl_wrapper.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2015 Felix Ruess <felix.ruess@gmail.com>
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, see
18  * <http://www.gnu.org/licenses/>.
19  */
20 
28 #include "modules/ahrs/ahrs.h"
29 #include "modules/core/abi.h"
30 #include "state.h"
31 
32 #ifndef AHRS_FC_OUTPUT_ENABLED
33 #define AHRS_FC_OUTPUT_ENABLED TRUE
34 #endif
35 PRINT_CONFIG_VAR(AHRS_FC_OUTPUT_ENABLED)
36 
37 
41 
42 static void compute_body_orientation_and_rates(void);
43 
44 #if PERIODIC_TELEMETRY
46 #include "mcu_periph/sys_time.h"
47 #include "state.h"
48 
49 static void send_euler(struct transport_tx *trans, struct link_device *dev)
50 {
51  struct FloatEulers ltp_to_body_euler;
52  float_eulers_of_quat(&ltp_to_body_euler, &ahrs_fc.ltp_to_body_quat);
53  pprz_msg_send_AHRS_EULER(trans, dev, AC_ID,
54  &ltp_to_body_euler.phi,
55  &ltp_to_body_euler.theta,
56  &ltp_to_body_euler.psi,
57  &ahrs_fc_id);
58 }
59 
60 static void send_bias(struct transport_tx *trans, struct link_device *dev)
61 {
62  struct Int32Rates gyro_bias;
64  pprz_msg_send_AHRS_GYRO_BIAS_INT(trans, dev, AC_ID,
65  &gyro_bias.p, &gyro_bias.q, &gyro_bias.r, &ahrs_fc_id);
66 }
67 
68 static void send_euler_int(struct transport_tx *trans, struct link_device *dev)
69 {
70  /* compute eulers in int (IMU frame) */
71  struct FloatEulers ltp_to_body_euler;
72  float_eulers_of_quat(&ltp_to_body_euler, &ahrs_fc.ltp_to_body_quat);
73  struct Int32Eulers eulers_body;
74  EULERS_BFP_OF_REAL(eulers_body, ltp_to_body_euler);
75 
76  pprz_msg_send_AHRS_EULER_INT(trans, dev, AC_ID,
77  &eulers_body.phi,
78  &eulers_body.theta,
79  &eulers_body.psi,
80  &eulers_body.phi,
81  &eulers_body.theta,
82  &eulers_body.psi,
83  &ahrs_fc_id);
84 }
85 
86 static void send_geo_mag(struct transport_tx *trans, struct link_device *dev)
87 {
88  pprz_msg_send_GEO_MAG(trans, dev, AC_ID,
90 }
91 
92 static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
93 {
94  uint8_t mde = 3;
95  uint16_t val = 0;
96  if (!ahrs_fc.is_aligned) { mde = 2; }
98  /* set lost if no new gyro measurements for 50ms */
99  if (t_diff > 50000) { mde = 5; }
100  pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &ahrs_fc_id, &mde, &val);
101 }
102 #endif
103 
104 
108 #ifndef AHRS_FC_IMU_ID
109 #define AHRS_FC_IMU_ID ABI_BROADCAST
110 #endif
111 PRINT_CONFIG_VAR(AHRS_FC_IMU_ID)
115 #ifndef AHRS_FC_MAG_ID
116 #define AHRS_FC_MAG_ID AHRS_FC_IMU_ID
117 #endif
118 PRINT_CONFIG_VAR(AHRS_FC_MAG_ID)
122 #ifndef AHRS_FC_GPS_ID
123 #define AHRS_FC_GPS_ID GPS_MULTI_ID
124 #endif
125 PRINT_CONFIG_VAR(AHRS_FC_GPS_ID)
132 
133 
134 static void gyro_cb(uint8_t __attribute__((unused)) sender_id,
135  uint32_t stamp, struct Int32Rates *gyro)
136 {
137  ahrs_fc_last_stamp = stamp;
138  struct FloatRates gyro_f;
139  RATES_FLOAT_OF_BFP(gyro_f, *gyro);
140 
141 #if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY)
142  PRINT_CONFIG_MSG("Calculating dt for AHRS_FC propagation.")
143  /* timestamp in usec when last callback was received */
144  static uint32_t last_stamp = 0;
145 
146  if (last_stamp > 0 && ahrs_fc.is_aligned) {
147  float dt = (float)(stamp - last_stamp) * 1e-6;
148  ahrs_fc_propagate(&gyro_f, dt);
150  }
151  last_stamp = stamp;
152 #else
153  PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for AHRS_FC propagation.")
154  PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY)
155  if (ahrs_fc.status == AHRS_FC_RUNNING) {
156  const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
157  ahrs_fc_propagate(&gyro_f, dt);
159  }
160 #endif
161 }
162 
163 static void accel_cb(uint8_t __attribute__((unused)) sender_id,
164  uint32_t __attribute__((unused)) stamp,
165  struct Int32Vect3 *accel)
166 {
167  struct FloatVect3 accel_f;
168  ACCELS_FLOAT_OF_BFP(accel_f, *accel);
169 
170 #if USE_AUTO_AHRS_FREQ || !defined(AHRS_CORRECT_FREQUENCY)
171  PRINT_CONFIG_MSG("Calculating dt for AHRS float_cmpl accel update.")
172  static uint32_t last_stamp = 0;
173  if (last_stamp > 0 && ahrs_fc.is_aligned) {
174  float dt = (float)(stamp - last_stamp) * 1e-6;
175  ahrs_fc_update_accel(&accel_f, dt);
176  }
177  last_stamp = stamp;
178 #else
179  PRINT_CONFIG_MSG("Using fixed AHRS_CORRECT_FREQUENCY for AHRS float_cmpl accel update.")
180  PRINT_CONFIG_VAR(AHRS_CORRECT_FREQUENCY)
181  if (ahrs_fc.is_aligned) {
182  const float dt = 1. / (AHRS_CORRECT_FREQUENCY);
183  ahrs_fc_update_accel(&accel_f, dt);
184  }
185 #endif
186 }
187 
188 static void mag_cb(uint8_t __attribute__((unused)) sender_id,
189  uint32_t __attribute__((unused)) stamp,
190  struct Int32Vect3 *mag)
191 {
192  struct FloatVect3 mag_f;
193  MAGS_FLOAT_OF_BFP(mag_f, *mag);
194 
195 #if USE_AUTO_AHRS_FREQ || !defined(AHRS_MAG_CORRECT_FREQUENCY)
196  PRINT_CONFIG_MSG("Calculating dt for AHRS float_cmpl mag update.")
197  static uint32_t last_stamp = 0;
198  if (last_stamp > 0 && ahrs_fc.is_aligned) {
199  float dt = (float)(stamp - last_stamp) * 1e-6;
200  ahrs_fc_update_mag(&mag_f, dt);
201  }
202  last_stamp = stamp;
203 #else
204  PRINT_CONFIG_MSG("Using fixed AHRS_MAG_CORRECT_FREQUENCY for AHRS float_cmpl mag update.")
205  PRINT_CONFIG_VAR(AHRS_MAG_CORRECT_FREQUENCY)
206  if (ahrs_fc.is_aligned) {
207  const float dt = 1. / (AHRS_MAG_CORRECT_FREQUENCY);
208  ahrs_fc_update_mag(&mag_f, dt);
209  }
210 #endif
211 }
212 
213 static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
214  uint32_t stamp __attribute__((unused)),
215  struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
216  struct Int32Vect3 *lp_mag)
217 {
218  if (!ahrs_fc.is_aligned) {
219  /* convert to float */
220  struct FloatRates gyro_f;
221  RATES_FLOAT_OF_BFP(gyro_f, *lp_gyro);
222  struct FloatVect3 accel_f;
223  ACCELS_FLOAT_OF_BFP(accel_f, *lp_accel);
224  struct FloatVect3 mag_f;
225  MAGS_FLOAT_OF_BFP(mag_f, *lp_mag);
226  /* use low passed values to align */
227  if (ahrs_fc_align(&gyro_f, &accel_f, &mag_f)) {
229  }
230  }
231 }
232 
233 static void geo_mag_cb(uint8_t sender_id __attribute__((unused)), struct FloatVect3 *h)
234 {
235  ahrs_fc.mag_h = *h;
236 }
237 
238 static void gps_cb(uint8_t sender_id __attribute__((unused)),
239  uint32_t stamp __attribute__((unused)),
240  struct GpsState *gps_s)
241 {
242  ahrs_fc_update_gps(gps_s);
244 }
245 
246 static bool ahrs_fc_enable_output(bool enable)
247 {
248  ahrs_fc_output_enabled = enable;
249  return ahrs_fc_output_enabled;
250 }
251 
256 {
258  /* Set state */
261  }
262 }
263 
265 {
267  ahrs_fc_init();
269 
270  /*
271  * Subscribe to scaled IMU measurements and attach callbacks
272  */
273  AbiBindMsgIMU_GYRO(AHRS_FC_IMU_ID, &gyro_ev, gyro_cb);
274  AbiBindMsgIMU_ACCEL(AHRS_FC_IMU_ID, &accel_ev, accel_cb);
275  AbiBindMsgIMU_MAG(AHRS_FC_MAG_ID, &mag_ev, mag_cb);
276  AbiBindMsgIMU_LOWPASSED(ABI_BROADCAST, &aligner_ev, aligner_cb);
277  AbiBindMsgGEO_MAG(ABI_BROADCAST, &geo_mag_ev, geo_mag_cb);
278  AbiBindMsgGPS(AHRS_FC_GPS_ID, &gps_ev, gps_cb);
279 
280 #if PERIODIC_TELEMETRY
281  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AHRS_EULER, send_euler);
282  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AHRS_GYRO_BIAS_INT, send_bias);
283  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AHRS_EULER_INT, send_euler_int);
285  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STATE_FILTER_STATUS, send_filter_status);
286 #endif
287 }
static void h(const real32_T x[7], const real32_T q[4], real32_T y[6])
Main include for ABI (AirBorneInterface).
#define ABI_BROADCAST
Broadcast address.
Definition: abi_common.h:58
Event structure to store callbacks in a linked list.
Definition: abi_common.h:67
void ahrs_register_impl(AhrsEnableOutput enable)
Register an AHRS implementation.
Definition: ahrs.c:62
Dispatcher to register actual AHRS implementations.
#define AHRS_COMP_ID_FC
Definition: ahrs.h:37
struct AhrsFloatCmpl ahrs_fc
void ahrs_fc_init(void)
void ahrs_fc_update_mag(struct FloatVect3 *mag, float dt)
bool ahrs_fc_align(struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel, struct FloatVect3 *lp_mag)
void ahrs_fc_propagate(struct FloatRates *gyro, float dt)
void ahrs_fc_update_accel(struct FloatVect3 *accel, float dt)
void ahrs_fc_update_gps(struct GpsState *gps_s)
struct FloatRates gyro_bias
struct FloatRates body_rate
@ AHRS_FC_RUNNING
enum AhrsFCStatus status
struct FloatVect3 mag_h
struct FloatQuat ltp_to_body_quat
static uint8_t ahrs_fc_id
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
static void compute_body_orientation_and_rates(void)
Compute body orientation and rates from imu orientation and rates.
static abi_event mag_ev
static abi_event accel_ev
static void gyro_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *gyro)
static void mag_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *mag)
#define AHRS_FC_MAG_ID
ABI binding for magnetometer data.
static abi_event gyro_ev
#define AHRS_FC_IMU_ID
ABI binding for IMU data.
static void send_geo_mag(struct transport_tx *trans, struct link_device *dev)
static abi_event aligner_ev
static abi_event geo_mag_ev
static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
#define AHRS_FC_OUTPUT_ENABLED
void ahrs_fc_register(void)
static void send_euler_int(struct transport_tx *trans, struct link_device *dev)
static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
static bool ahrs_fc_output_enabled
if TRUE with push the estimation results to the state interface
static void geo_mag_cb(uint8_t sender_id, struct FloatVect3 *h)
static void aligner_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel, struct Int32Vect3 *lp_mag)
static void send_euler(struct transport_tx *trans, struct link_device *dev)
static uint32_t ahrs_fc_last_stamp
static void send_bias(struct transport_tx *trans, struct link_device *dev)
static bool ahrs_fc_enable_output(bool enable)
#define AHRS_FC_GPS_ID
ABI binding for gps data.
static abi_event gps_ev
Paparazzi specific wrapper to run floating point complementary filter.
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:71
data structure for GPS information
Definition: gps.h:86
float phi
in radians
float theta
in radians
float psi
in radians
void float_eulers_of_quat(struct FloatEulers *e, struct FloatQuat *q)
euler rotation 'ZYX'
euler angles
angular rates
#define MAGS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:807
#define EULERS_BFP_OF_REAL(_ei, _ef)
Definition: pprz_algebra.h:715
#define RATES_BFP_OF_REAL(_ri, _rf)
Definition: pprz_algebra.h:765
#define RATES_FLOAT_OF_BFP(_rf, _ri)
Definition: pprz_algebra.h:759
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:795
int32_t p
in rad/s with INT32_RATE_FRAC
int32_t r
in rad/s with INT32_RATE_FRAC
int32_t phi
in rad with INT32_ANGLE_FRAC
int32_t q
in rad/s with INT32_RATE_FRAC
int32_t psi
in rad with INT32_ANGLE_FRAC
int32_t theta
in rad with INT32_ANGLE_FRAC
euler angles
angular rates
static void stateSetNedToBodyQuat_f(struct FloatQuat *ned_to_body_quat)
Set vehicle body attitude from quaternion (float).
Definition: state.h:1093
static void stateSetBodyRates_f(struct FloatRates *body_rate)
Set vehicle body angular rate (float).
Definition: state.h:1181
#define AHRS_PROPAGATE_FREQUENCY
Definition: hf_float.c:55
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
API to get/set the generic vehicle states.
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
Architecture independent timing functions.
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:51
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
uint16_t val[TCOUPLE_NB]
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98