Paparazzi UAS  v5.15_devel-88-gb3ad7fe
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
ahrs_float_mlkf_wrapper.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2014 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, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  */
21 
29 #include "subsystems/ahrs.h"
30 #include "subsystems/abi.h"
31 #include "state.h"
32 
33 #ifndef AHRS_MLKF_OUTPUT_ENABLED
34 #define AHRS_MLKF_OUTPUT_ENABLED TRUE
35 #endif
36 PRINT_CONFIG_VAR(AHRS_MLKF_OUTPUT_ENABLED)
37 
38 
42 
43 static void set_body_state_from_quat(void);
44 
45 #if PERIODIC_TELEMETRY
47 #include "mcu_periph/sys_time.h"
48 
49 static void send_euler(struct transport_tx *trans, struct link_device *dev)
50 {
51  struct FloatEulers ltp_to_imu_euler;
52  float_eulers_of_quat(&ltp_to_imu_euler, &ahrs_mlkf.ltp_to_imu_quat);
53  pprz_msg_send_AHRS_EULER(trans, dev, AC_ID,
54  &ltp_to_imu_euler.phi,
55  &ltp_to_imu_euler.theta,
56  &ltp_to_imu_euler.psi,
57  &ahrs_mlkf_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_mlkf_id);
66 }
67 
68 static void send_geo_mag(struct transport_tx *trans, struct link_device *dev)
69 {
70  pprz_msg_send_GEO_MAG(trans, dev, AC_ID,
72 }
73 
74 static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
75 {
76  uint8_t mde = 3;
77  uint16_t val = 0;
78  if (!ahrs_mlkf.is_aligned) { mde = 2; }
80  /* set lost if no new gyro measurements for 50ms */
81  if (t_diff > 50000) { mde = 5; }
82  pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &ahrs_mlkf_id, &mde, &val);
83 }
84 #endif
85 
86 
90 #ifndef AHRS_MLKF_IMU_ID
91 #define AHRS_MLKF_IMU_ID ABI_BROADCAST
92 #endif
93 PRINT_CONFIG_VAR(AHRS_MLKF_IMU_ID)
97 #ifndef AHRS_MLKF_MAG_ID
98 #define AHRS_MLKF_MAG_ID AHRS_MLKF_IMU_ID
99 #endif
100 PRINT_CONFIG_VAR(AHRS_MLKF_MAG_ID)
107 
108 
109 static void gyro_cb(uint8_t __attribute__((unused)) sender_id,
110  uint32_t stamp, struct Int32Rates *gyro)
111 {
112  ahrs_mlkf_last_stamp = stamp;
113  struct FloatRates gyro_f;
114  RATES_FLOAT_OF_BFP(gyro_f, *gyro);
115 
116 #if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY)
117  PRINT_CONFIG_MSG("Calculating dt for AHRS_MLKF propagation.")
118  /* timestamp in usec when last callback was received */
119  static uint32_t last_stamp = 0;
120 
121  if (last_stamp > 0 && ahrs_mlkf.is_aligned) {
122  float dt = (float)(stamp - last_stamp) * 1e-6;
123  ahrs_mlkf_propagate(&gyro_f, dt);
125  }
126  last_stamp = stamp;
127 #else
128  PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for AHRS_MLKF propagation.")
129  PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY)
131  const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
132  ahrs_mlkf_propagate(&gyro_f, dt);
134  }
135 #endif
136 }
137 
138 static void accel_cb(uint8_t sender_id __attribute__((unused)),
139  uint32_t stamp __attribute__((unused)),
140  struct Int32Vect3 *accel)
141 {
142  if (ahrs_mlkf.is_aligned) {
143  struct FloatVect3 accel_f;
144  ACCELS_FLOAT_OF_BFP(accel_f, *accel);
145  ahrs_mlkf_update_accel(&accel_f);
147  }
148 }
149 
150 static void mag_cb(uint8_t sender_id __attribute__((unused)),
151  uint32_t stamp __attribute__((unused)),
152  struct Int32Vect3 *mag)
153 {
154  if (ahrs_mlkf.is_aligned) {
155  struct FloatVect3 mag_f;
156  MAGS_FLOAT_OF_BFP(mag_f, *mag);
157  ahrs_mlkf_update_mag(&mag_f);
159  }
160 }
161 
162 static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
163  uint32_t stamp __attribute__((unused)),
164  struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
165  struct Int32Vect3 *lp_mag)
166 {
167  if (!ahrs_mlkf.is_aligned) {
168  /* convert to float */
169  struct FloatRates gyro_f;
170  RATES_FLOAT_OF_BFP(gyro_f, *lp_gyro);
171  struct FloatVect3 accel_f;
172  ACCELS_FLOAT_OF_BFP(accel_f, *lp_accel);
173  struct FloatVect3 mag_f;
174  MAGS_FLOAT_OF_BFP(mag_f, *lp_mag);
175  /* set initial body orientation in state interface if alignment was successful */
176  if (ahrs_mlkf_align(&gyro_f, &accel_f, &mag_f)) {
178  }
179  }
180 }
181 
182 static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
183  struct FloatQuat *q_b2i_f)
184 {
186 }
187 
188 static void geo_mag_cb(uint8_t sender_id __attribute__((unused)), struct FloatVect3 *h)
189 {
190  ahrs_mlkf.mag_h = *h;
191 }
192 
193 static bool ahrs_mlkf_enable_output(bool enable)
194 {
195  ahrs_mlkf_output_enabled = enable;
197 }
198 
202 static void set_body_state_from_quat(void)
203 {
204  if (ahrs_mlkf_output_enabled) {
205  struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_mlkf.body_to_imu);
206  struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ahrs_mlkf.body_to_imu);
207 
208  /* Compute LTP to BODY quaternion */
209  struct FloatQuat ltp_to_body_quat;
210  float_quat_comp_inv(&ltp_to_body_quat, &ahrs_mlkf.ltp_to_imu_quat, body_to_imu_quat);
211  /* Set in state interface */
212  stateSetNedToBodyQuat_f(&ltp_to_body_quat);
213 
214  /* compute body rates */
215  struct FloatRates body_rate;
216  float_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_mlkf.imu_rate);
217  /* Set state */
218  stateSetBodyRates_f(&body_rate);
219  }
220 }
221 
223 {
224  ahrs_mlkf_output_enabled = AHRS_MLKF_OUTPUT_ENABLED;
225  ahrs_mlkf_init();
227 
228  /*
229  * Subscribe to scaled IMU measurements and attach callbacks
230  */
231  AbiBindMsgIMU_GYRO_INT32(AHRS_MLKF_IMU_ID, &gyro_ev, gyro_cb);
232  AbiBindMsgIMU_ACCEL_INT32(AHRS_MLKF_IMU_ID, &accel_ev, accel_cb);
233  AbiBindMsgIMU_MAG_INT32(AHRS_MLKF_MAG_ID, &mag_ev, mag_cb);
234  AbiBindMsgIMU_LOWPASSED(ABI_BROADCAST, &aligner_ev, aligner_cb);
235  AbiBindMsgBODY_TO_IMU_QUAT(ABI_BROADCAST, &body_to_imu_ev, body_to_imu_cb);
236  AbiBindMsgGEO_MAG(ABI_BROADCAST, &geo_mag_ev, geo_mag_cb);
237 
238 #if PERIODIC_TELEMETRY
239  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AHRS_EULER, send_euler);
240  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AHRS_GYRO_BIAS_INT, send_bias);
242  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STATE_FILTER_STATUS, send_filter_status);
243 #endif
244 }
245 
static void send_geo_mag(struct transport_tx *trans, struct link_device *dev)
static void send_bias(struct transport_tx *trans, struct link_device *dev)
void ahrs_mlkf_propagate(struct FloatRates *gyro, float dt)
Event structure to store callbacks in a linked list.
Definition: abi_common.h:65
unsigned short uint16_t
Definition: types.h:16
void float_quat_comp_inv(struct FloatQuat *a2b, struct FloatQuat *a2c, struct FloatQuat *b2c)
Composition (multiplication) of two quaternions.
static void gyro_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *gyro)
angular rates
#define RATES_BFP_OF_REAL(_ri, _rf)
Definition: pprz_algebra.h:765
Dispatcher to register actual AHRS implementations.
static abi_event mag_ev
float phi
in radians
static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
#define AHRS_MLKF_IMU_ID
ABI binding for IMU data.
#define AHRS_PROPAGATE_FREQUENCY
Definition: hf_float.c:55
static abi_event body_to_imu_ev
#define AHRS_MLKF_OUTPUT_ENABLED
Periodic telemetry system header (includes downlink utility and generated code).
static struct FloatRMat * orientationGetRMat_f(struct OrientationReps *orientation)
Get vehicle body attitude rotation matrix (float).
struct FloatVect3 mag_h
Paparazzi specific wrapper to run MLKF filter.
struct FloatRates imu_rate
Rotational velocity in IMU frame.
static abi_event accel_ev
Main include for ABI (AirBorneInterface).
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 mag_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *mag)
static void stateSetNedToBodyQuat_f(struct FloatQuat *ned_to_body_quat)
Set vehicle body attitude from quaternion (float).
Definition: state.h:1093
void ahrs_mlkf_update_accel(struct FloatVect3 *accel)
float psi
in radians
static void send_euler(struct transport_tx *trans, struct link_device *dev)
struct FloatRates gyro_bias
static void body_to_imu_cb(uint8_t sender_id, struct FloatQuat *q_b2i_f)
enum AhrsMlkfStatus status
static void set_body_state_from_quat(void)
Compute body orientation and rates from imu orientation and rates.
static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
static abi_event geo_mag_ev
static uint32_t ahrs_mlkf_last_stamp
static abi_event aligner_ev
static bool ahrs_mlkf_enable_output(bool enable)
euler angles
Roation quaternion.
bool ahrs_mlkf_align(struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel, struct FloatVect3 *lp_mag)
float theta
in radians
int32_t r
in rad/s with INT32_RATE_FRAC
static abi_event gyro_ev
#define RATES_FLOAT_OF_BFP(_rf, _ri)
Definition: pprz_algebra.h:759
void ahrs_mlkf_update_mag(struct FloatVect3 *mag)
void ahrs_register_impl(AhrsEnableOutput enable)
Register an AHRS implementation.
Definition: ahrs.c:62
Architecture independent timing functions.
void ahrs_mlkf_set_body_to_imu_quat(struct FloatQuat *q_b2i)
uint16_t val[TCOUPLE_NB]
unsigned long uint32_t
Definition: types.h:18
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
bool is_aligned
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
void ahrs_mlkf_init(void)
#define AHRS_COMP_ID_MLKF
Definition: ahrs.h:40
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:795
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:73
struct AhrsMlkf ahrs_mlkf
static void h(const real32_T x[7], const real32_T q[4], real32_T y[6])
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
void float_rmat_transp_ratemult(struct FloatRates *rb, struct FloatRMat *m_b2a, struct FloatRates *ra)
rotate anglular rates by transposed rotation matrix.
static bool ahrs_mlkf_output_enabled
if TRUE with push the estimation results to the state interface
struct OrientationReps body_to_imu
body_to_imu rotation
static uint8_t ahrs_mlkf_id
struct FloatQuat ltp_to_imu_quat
Rotation from LocalTangentPlane to IMU frame as unit quaternion.
int32_t p
in rad/s with INT32_RATE_FRAC
rotation matrix
#define AHRS_MLKF_MAG_ID
ABI binding for magnetometer data.
#define ABI_BROADCAST
Broadcast address.
Definition: abi_common.h:56
#define MAGS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:807
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:68
static void stateSetBodyRates_f(struct FloatRates *body_rate)
Set vehicle body angular rate (float).
Definition: state.h:1181
void float_eulers_of_quat(struct FloatEulers *e, struct FloatQuat *q)
euler rotation 'ZYX'
int32_t q
in rad/s with INT32_RATE_FRAC
angular rates
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46
static struct FloatQuat * orientationGetQuat_f(struct OrientationReps *orientation)
Get vehicle body attitude quaternion (float).
static void geo_mag_cb(uint8_t sender_id, struct FloatVect3 *h)
void ahrs_mlkf_register(void)