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
ins_float_invariant_wrapper.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012-2013 Jean-Philippe Condomines, Gautier Hattenberger
3  * 2015 Felix Ruess <felix.ruess@gmail.com>
4  *
5  * This file is part of paparazzi.
6  *
7  * paparazzi is free software; you can redistribute it and/or modify
8  * it under the terms of the GNU General Public License as published by
9  * the Free Software Foundation; either version 2, or (at your option)
10  * any later version.
11  *
12  * paparazzi is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15  * GNU General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with Paparazzi; see the file COPYING. If not, see
19  * <http://www.gnu.org/licenses/>.
20  */
21 
29 #include "subsystems/abi.h"
30 #include "mcu_periph/sys_time.h"
31 #include "message_pragmas.h"
32 #if USE_AHRS_ALIGNER
34 #endif
35 
36 #ifndef INS_FINV_FILTER_ID
37 #define INS_FINV_FILTER_ID 2
38 #endif
39 
41 static struct FloatVect3 ins_finv_accel;
42 
45 
46 #if PERIODIC_TELEMETRY && !INS_FINV_USE_UTM
48 #include "state.h"
49 static void send_ins_ref(struct transport_tx *trans, struct link_device *dev)
50 {
51  float foo = 0.;
53  pprz_msg_send_INS_REF(trans, dev, AC_ID,
57  &state.ned_origin_i.hmsl, &foo);
58  }
59 }
60 
61 static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
62 {
64  uint8_t mde = 3;
65  uint16_t val = 0;
66  if (!ins_float_inv.is_aligned) { mde = 2; }
68  /* set lost if no new gyro measurements for 50ms */
69  if (t_diff > 50000) { mde = 5; }
70  pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &id, &mde, &val);
71 }
72 #endif
73 
74 
75 /*
76  * ABI bindings
77  */
79 #ifndef INS_FINV_BARO_ID
80 #if USE_BARO_BOARD
81 #define INS_FINV_BARO_ID BARO_BOARD_SENDER_ID
82 #else
83 #define INS_FINV_BARO_ID ABI_BROADCAST
84 #endif
85 #endif
86 PRINT_CONFIG_VAR(INS_FINV_BARO_ID)
87 
88 
89 #ifndef INS_FINV_IMU_ID
90 #define INS_FINV_IMU_ID ABI_BROADCAST
91 #endif
92 PRINT_CONFIG_VAR(INS_FINV_IMU_ID)
93 
94 
95 #if USE_MAGNETOMETER
96 #ifndef INS_FINV_MAG_ID
97 #define INS_FINV_MAG_ID ABI_BROADCAST
98 #endif
99 PRINT_CONFIG_VAR(INS_FINV_MAG_ID)
100 #else
101 PRINT_CONFIG_MSG("INS invariant use GPS heading as magnetometer")
102 #endif
103 
107 #ifndef INS_FINV_GPS_ID
108 #define INS_FINV_GPS_ID GPS_MULTI_ID
109 #endif
110 PRINT_CONFIG_VAR(INS_FINV_GPS_ID)
111 
117 #if USE_MAGNETOMETER
120 #endif
122 
123 static void baro_cb(uint8_t __attribute__((unused)) sender_id, __attribute__((unused)) uint32_t stamp, float pressure)
124 {
126 }
127 
133 static void gyro_cb(uint8_t sender_id __attribute__((unused)),
134  uint32_t stamp, struct Int32Rates *gyro)
135 {
136  struct FloatRates gyro_f;
137  RATES_FLOAT_OF_BFP(gyro_f, *gyro);
138 
139 #if USE_AUTO_INS_FREQ || !defined(INS_PROPAGATE_FREQUENCY)
140  PRINT_CONFIG_MSG("Calculating dt for INS float_invariant propagation.")
141  /* timestamp in usec when last callback was received */
142  static uint32_t last_stamp = 0;
143 
144  if (last_stamp > 0) {
145  float dt = (float)(stamp - last_stamp) * 1e-6;
147  }
148  last_stamp = stamp;
149 #else
150  PRINT_CONFIG_MSG("Using fixed INS_PROPAGATE_FREQUENCY for INS float_invariant propagation.")
151  PRINT_CONFIG_VAR(INS_PROPAGATE_FREQUENCY)
152  const float dt = 1. / (INS_PROPAGATE_FREQUENCY);
154 #endif
155 
156  ins_finv_last_stamp = stamp;
157 }
158 
159 static void accel_cb(uint8_t sender_id __attribute__((unused)),
160  uint32_t stamp __attribute__((unused)),
161  struct Int32Vect3 *accel)
162 {
164 }
165 
166 static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
167  uint32_t stamp __attribute__((unused)),
168  struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
169  struct Int32Vect3 *lp_mag)
170 {
171  if (!ins_float_inv.is_aligned) {
172  /* convert to float */
173  struct FloatRates gyro_f;
174  RATES_FLOAT_OF_BFP(gyro_f, *lp_gyro);
175  struct FloatVect3 accel_f;
176  ACCELS_FLOAT_OF_BFP(accel_f, *lp_accel);
177  struct FloatVect3 mag_f;
178  MAGS_FLOAT_OF_BFP(mag_f, *lp_mag);
179  ins_float_invariant_align(&gyro_f, &accel_f, &mag_f);
180  }
181 }
182 
183 static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
184  struct FloatQuat *q_b2i_f)
185 {
187 }
188 
189 #if USE_MAGNETOMETER
190 static void mag_cb(uint8_t sender_id __attribute__((unused)),
191  uint32_t stamp __attribute__((unused)),
192  struct Int32Vect3 *mag)
193 {
195  struct FloatVect3 mag_f;
196  MAGS_FLOAT_OF_BFP(mag_f, *mag);
198  }
199 }
200 
201 static void geo_mag_cb(uint8_t sender_id __attribute__((unused)), struct FloatVect3 *h)
202 {
203  ins_float_inv.mag_h = *h;
204 }
205 #endif
206 
207 static void gps_cb(uint8_t sender_id __attribute__((unused)),
208  uint32_t stamp __attribute__((unused)),
209  struct GpsState *gps_s)
210 {
212 }
213 
214 
216 {
217  // aligner
218 #if USE_AHRS_ALIGNER
220 #endif
221 
223 
224  // Bind to ABI messages
225  AbiBindMsgBARO_ABS(INS_FINV_BARO_ID, &baro_ev, baro_cb);
226  AbiBindMsgIMU_GYRO_INT32(INS_FINV_IMU_ID, &gyro_ev, gyro_cb);
227  AbiBindMsgIMU_ACCEL_INT32(INS_FINV_IMU_ID, &accel_ev, accel_cb);
228  AbiBindMsgIMU_LOWPASSED(INS_FINV_IMU_ID, &aligner_ev, aligner_cb);
229  AbiBindMsgBODY_TO_IMU_QUAT(INS_FINV_IMU_ID, &body_to_imu_ev, body_to_imu_cb);
230 #if USE_MAGNETOMETER
231  AbiBindMsgIMU_MAG_INT32(INS_FINV_MAG_ID, &mag_ev, mag_cb);
232  AbiBindMsgGEO_MAG(ABI_BROADCAST, &geo_mag_ev, geo_mag_cb);
233 #endif
234  AbiBindMsgGPS(INS_FINV_GPS_ID, &gps_ev, gps_cb);
235 
236 #if PERIODIC_TELEMETRY && !INS_FINV_USE_UTM
238  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STATE_FILTER_STATUS, send_filter_status);
239 #endif
240 }
static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
static abi_event gps_ev
struct FloatVect3 mag_h
Event structure to store callbacks in a linked list.
Definition: abi_common.h:65
static abi_event aligner_ev
Interface to align the AHRS via low-passed measurements at startup.
unsigned short uint16_t
Definition: types.h:16
int32_t z
in centimeters
#define INS_FINV_IMU_ID
IMU (gyro, accel)
angular rates
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 body_to_imu_cb(uint8_t sender_id, struct FloatQuat *q_b2i_f)
static void gyro_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *gyro)
Call ins_float_invariant_propagate on new gyro measurements.
static abi_event geo_mag_ev
Periodic telemetry system header (includes downlink utility and generated code).
void ins_float_invariant_update_mag(struct FloatVect3 *mag)
int32_t y
in centimeters
Main include for ABI (AirBorneInterface).
void ins_float_invariant_update_gps(struct GpsState *gps_s)
struct EcefCoor_i ecef
Reference point in ecef.
int32_t hmsl
Height above mean sea level in mm.
static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
static abi_event mag_ev
int32_t alt
in millimeters above WGS84 reference ellipsoid
Roation quaternion.
Paparazzi specific wrapper to run INVARIANT filter.
#define RATES_FLOAT_OF_BFP(_rf, _ri)
Definition: pprz_algebra.h:759
#define INS_FINV_MAG_ID
magnetometer
Architecture independent timing functions.
data structure for GPS information
Definition: gps.h:87
uint16_t val[TCOUPLE_NB]
unsigned long uint32_t
Definition: types.h:18
static struct FloatVect3 ins_finv_accel
last accel measurement
static abi_event accel_ev
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
static void geo_mag_cb(uint8_t sender_id, struct FloatVect3 *h)
struct LlaCoor_i lla
Reference point in lla.
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
int32_t lon
in degrees*1e7
uint16_t foo
Definition: main_demo5.c:59
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:795
bool ned_initialized_i
true if local int coordinate frame is initialsed
Definition: state.h:171
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:73
struct LtpDef_i ned_origin_i
Definition of the local (flat earth) coordinate system.
Definition: state.h:166
void ins_float_invariant_update_baro(float pressure)
static abi_event body_to_imu_ev
static void mag_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *mag)
void ins_float_invariant_wrapper_init(void)
static void h(const real32_T x[7], const real32_T q[4], real32_T y[6])
void ins_float_inv_set_body_to_imu_quat(struct FloatQuat *q_b2i)
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
static abi_event gyro_ev
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
static abi_event baro_ev
static void baro_cb(uint8_t sender_id, uint32_t stamp, float pressure)
int32_t x
in centimeters
#define ABI_BROADCAST
Broadcast address.
Definition: abi_common.h:56
#define MAGS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:807
void ins_float_invariant_align(struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel, struct FloatVect3 *lp_mag)
static uint32_t ins_finv_last_stamp
last gyro msg timestamp
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:68
int32_t lat
in degrees*1e7
struct InsFloatInv ins_float_inv
#define INS_FINV_FILTER_ID
void ahrs_aligner_init(void)
Definition: ahrs_aligner.c:81
void ins_float_invariant_propagate(struct FloatRates *gyro, struct FloatVect3 *accel, float dt)
void ins_float_invariant_init(void)
angular rates
#define INS_FINV_GPS_ID
ABI binding for gps data.
struct State state
Definition: state.c:36
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 void send_ins_ref(struct transport_tx *trans, struct link_device *dev)
#define INS_FINV_BARO_ID
baro