Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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
27
29#include "modules/ahrs/ahrs.h"
30#include "modules/core/abi.h"
31#include "state.h"
32
33PRINT_CONFIG_VAR(AHRS_MLKF_TYPE)
34
38
39static void set_body_state_from_quat(void);
40
41#if PERIODIC_TELEMETRY
43#include "mcu_periph/sys_time.h"
44
45static void send_euler(struct transport_tx *trans, struct link_device *dev)
46{
47 struct FloatEulers ltp_to_body_euler;
48 float_eulers_of_quat(&ltp_to_body_euler, &ahrs_mlkf.ltp_to_body_quat);
49 pprz_msg_send_AHRS_EULER(trans, dev, AC_ID,
50 &ltp_to_body_euler.phi,
51 &ltp_to_body_euler.theta,
52 &ltp_to_body_euler.psi,
54}
55
56static void send_bias(struct transport_tx *trans, struct link_device *dev)
57{
58 struct Int32Rates gyro_bias;
59 RATES_BFP_OF_REAL(gyro_bias, ahrs_mlkf.gyro_bias);
60 pprz_msg_send_AHRS_GYRO_BIAS_INT(trans, dev, AC_ID,
61 &gyro_bias.p, &gyro_bias.q, &gyro_bias.r, &ahrs_mlkf_id);
62}
63
64static void send_geo_mag(struct transport_tx *trans, struct link_device *dev)
65{
66 pprz_msg_send_GEO_MAG(trans, dev, AC_ID,
67 &ahrs_mlkf.mag_h.x, &ahrs_mlkf.mag_h.y, &ahrs_mlkf.mag_h.z, &ahrs_mlkf_id);
68}
69
70static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
71{
72 uint8_t mde = 3;
73 uint16_t val = 0;
74 if (!ahrs_mlkf.is_aligned) { mde = 2; }
76 /* set lost if no new gyro measurements for 50ms */
77 if (t_diff > 50000) { mde = 5; }
78 pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &ahrs_mlkf_id, &mde, &val);
79}
80#endif
81
82
86#ifndef AHRS_MLKF_IMU_ID
87#define AHRS_MLKF_IMU_ID ABI_BROADCAST
88#endif
93#ifndef AHRS_MLKF_MAG_ID
94#define AHRS_MLKF_MAG_ID AHRS_MLKF_IMU_ID
95#endif
102
103
104static void gyro_cb(uint8_t __attribute__((unused)) sender_id,
105 uint32_t stamp, struct Int32Rates *gyro)
106{
107 ahrs_mlkf_last_stamp = stamp;
108 struct FloatRates gyro_f;
109 RATES_FLOAT_OF_BFP(gyro_f, *gyro);
110
111#if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY)
112 PRINT_CONFIG_MSG("Calculating dt for AHRS_MLKF propagation.")
113 /* timestamp in usec when last callback was received */
114 static uint32_t last_stamp = 0;
115
116 if (last_stamp > 0 && ahrs_mlkf.is_aligned) {
117 float dt = (float)(stamp - last_stamp) * 1e-6;
118 ahrs_mlkf_propagate(&gyro_f, dt);
120 }
121 last_stamp = stamp;
122#else
123 PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for AHRS_MLKF propagation.")
125 if (ahrs_mlkf.status == AHRS_MLKF_RUNNING) {
126 const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
127 ahrs_mlkf_propagate(&gyro_f, dt);
129 }
130#endif
131}
132
133static void accel_cb(uint8_t sender_id __attribute__((unused)),
134 uint32_t stamp __attribute__((unused)),
135 struct Int32Vect3 *accel)
136{
137 if (ahrs_mlkf.is_aligned) {
138 struct FloatVect3 accel_f;
139 ACCELS_FLOAT_OF_BFP(accel_f, *accel);
140 ahrs_mlkf_update_accel(&accel_f);
142 }
143}
144
145static void mag_cb(uint8_t sender_id __attribute__((unused)),
146 uint32_t stamp __attribute__((unused)),
147 struct Int32Vect3 *mag)
148{
149 if (ahrs_mlkf.is_aligned) {
150 struct FloatVect3 mag_f;
151 MAGS_FLOAT_OF_BFP(mag_f, *mag);
152 ahrs_mlkf_update_mag(&mag_f);
154 }
155}
156
157static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
158 uint32_t stamp __attribute__((unused)),
159 struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
160 struct Int32Vect3 *lp_mag)
161{
162 if (!ahrs_mlkf.is_aligned) {
163 /* convert to float */
164 struct FloatRates gyro_f;
165 RATES_FLOAT_OF_BFP(gyro_f, *lp_gyro);
166 struct FloatVect3 accel_f;
167 ACCELS_FLOAT_OF_BFP(accel_f, *lp_accel);
168 struct FloatVect3 mag_f;
169 MAGS_FLOAT_OF_BFP(mag_f, *lp_mag);
170 /* set initial body orientation in state interface if alignment was successful */
171 if (ahrs_mlkf_align(&gyro_f, &accel_f, &mag_f)) {
173 }
174 }
175}
176
177static void geo_mag_cb(uint8_t sender_id __attribute__((unused)), struct FloatVect3 *h)
178{
179 ahrs_mlkf.mag_h = *h;
180}
181
186{
187 /* Set in state interface */
188 stateSetNedToBodyQuat_f(MODULE_AHRS_FLOAT_MLKF_ID, &ahrs_mlkf.ltp_to_body_quat);
189 stateSetBodyRates_f(MODULE_AHRS_FLOAT_MLKF_ID, &ahrs_mlkf.body_rate);
190}
191
193{
195 if (AHRS_MLKF_TYPE == AHRS_PRIMARY) {
197 } else {
199 }
200
201 /*
202 * Subscribe to scaled IMU measurements and attach callbacks
203 */
204 AbiBindMsgIMU_GYRO(AHRS_MLKF_IMU_ID, &gyro_ev, gyro_cb);
205 AbiBindMsgIMU_ACCEL(AHRS_MLKF_IMU_ID, &accel_ev, accel_cb);
206 AbiBindMsgIMU_MAG(AHRS_MLKF_MAG_ID, &mag_ev, mag_cb);
207 AbiBindMsgIMU_LOWPASSED(ABI_BROADCAST, &aligner_ev, aligner_cb);
208 AbiBindMsgGEO_MAG(ABI_BROADCAST, &geo_mag_ev, geo_mag_cb);
209
210#if PERIODIC_TELEMETRY
212 register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AHRS_GYRO_BIAS_INT, send_bias);
214 register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STATE_FILTER_STATUS, send_filter_status);
215#endif
216}
217
219{
220 if (enable) {
221 stateSetInputFilter(STATE_INPUT_ATTITUDE, MODULE_AHRS_FLOAT_MLKF_ID);
222 stateSetInputFilter(STATE_INPUT_RATES, MODULE_AHRS_FLOAT_MLKF_ID);
223 }
224 ahrs_mlkf_enable = enable;
225}
226
static void h(const real32_T x[7], const real32_T q[4], real32_T y[6])
Main include for ABI (AirBorneInterface).
struct abi_struct abi_event
Definition abi_common.h:101
#define ABI_BROADCAST
Broadcast address.
Definition abi_common.h:80
#define AHRS_COMP_ID_MLKF
Definition ahrs.h:43
#define AHRS_PRIMARY
Definition ahrs.h:32
static void gyro_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *gyro)
static abi_event gyro_ev
static abi_event mag_ev
static void mag_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *mag)
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)
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 void send_bias(struct transport_tx *trans, struct link_device *dev)
void ahrs_mlkf_update_mag(struct FloatVect3 *mag)
void ahrs_mlkf_update_accel(struct FloatVect3 *accel)
void ahrs_mlkf_init(void)
bool ahrs_mlkf_align(struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel, struct FloatVect3 *lp_mag)
struct AhrsMlkf ahrs_mlkf
void ahrs_mlkf_propagate(struct FloatRates *gyro, float dt)
@ AHRS_MLKF_RUNNING
static uint8_t ahrs_mlkf_id
uint8_t ahrs_mlkf_enable
static void mag_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *mag)
void ahrs_mlkf_wrapper_init(void)
static void send_geo_mag(struct transport_tx *trans, struct link_device *dev)
static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
void ahrs_float_mlkf_wrapper_enable(uint8_t enable)
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 void set_body_state_from_quat(void)
Compute body orientation and rates from imu orientation and rates.
static void send_bias(struct transport_tx *trans, struct link_device *dev)
static uint32_t ahrs_mlkf_last_stamp
#define AHRS_MLKF_MAG_ID
ABI binding for magnetometer data.
#define AHRS_MLKF_IMU_ID
ABI binding for IMU data.
Paparazzi specific wrapper to run MLKF filter.
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
static struct uart_periph * dev
static abi_event accel_ev
static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
#define STATE_INPUT_RATES
Definition state.h:145
#define STATE_INPUT_ATTITUDE
Definition state.h:144
float phi
in radians
float theta
in radians
float psi
in radians
void float_eulers_of_quat(struct FloatEulers *e, const struct FloatQuat *q)
euler rotation 'ZYX'
euler angles
angular rates
#define MAGS_FLOAT_OF_BFP(_ef, _ei)
#define RATES_BFP_OF_REAL(_ri, _rf)
#define RATES_FLOAT_OF_BFP(_rf, _ri)
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
int32_t p
in rad/s with INT32_RATE_FRAC
int32_t r
in rad/s with INT32_RATE_FRAC
int32_t q
in rad/s with INT32_RATE_FRAC
angular rates
static void stateSetNedToBodyQuat_f(uint16_t id, struct FloatQuat *ned_to_body_quat)
Set vehicle body attitude from quaternion (float).
Definition state.h:1272
void stateSetInputFilter(uint8_t type, uint16_t flag)
set the input filter for a specified type of data.
Definition state.c:91
static void stateSetBodyRates_f(uint16_t id, struct FloatRates *body_rate)
Set vehicle body angular rate (float).
Definition state.h:1371
#define AHRS_PROPAGATE_FREQUENCY
Definition hf_float.c:55
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
PRINT_CONFIG_VAR(ONELOOP_ANDI_FILT_CUTOFF)
API to get/set the generic vehicle states.
Architecture independent timing functions.
int16_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint16_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.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.