Paparazzi UAS  v5.10_stable-5-g83a0da5-dirty
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 
33 #ifndef INS_FINV_FILTER_ID
34 #define INS_FINV_FILTER_ID 2
35 #endif
36 
38 static struct FloatVect3 ins_finv_accel;
39 
42 
43 #if PERIODIC_TELEMETRY && !INS_FINV_USE_UTM
45 #include "state.h"
46 static void send_ins_ref(struct transport_tx *trans, struct link_device *dev)
47 {
48  float foo = 0.;
50  pprz_msg_send_INS_REF(trans, dev, AC_ID,
54  &state.ned_origin_i.hmsl, &foo);
55  }
56 }
57 
58 static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
59 {
61  uint8_t mde = 3;
62  uint16_t val = 0;
63  if (!ins_float_inv.is_aligned) { mde = 2; }
65  /* set lost if no new gyro measurements for 50ms */
66  if (t_diff > 50000) { mde = 5; }
67  pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &id, &mde, &val);
68 }
69 #endif
70 
71 
72 /*
73  * ABI bindings
74  */
76 #ifndef INS_FINV_BARO_ID
77 #if USE_BARO_BOARD
78 #define INS_FINV_BARO_ID BARO_BOARD_SENDER_ID
79 #else
80 #define INS_FINV_BARO_ID ABI_BROADCAST
81 #endif
82 #endif
83 PRINT_CONFIG_VAR(INS_FINV_BARO_ID)
84 
85 
86 #ifndef INS_FINV_IMU_ID
87 #define INS_FINV_IMU_ID ABI_BROADCAST
88 #endif
89 PRINT_CONFIG_VAR(INS_FINV_IMU_ID)
90 
91 
92 #ifndef INS_FINV_MAG_ID
93 #define INS_FINV_MAG_ID ABI_BROADCAST
94 #endif
95 PRINT_CONFIG_VAR(INS_FINV_MAG_ID)
96 
97 
100 #ifndef INS_FINV_GPS_ID
101 #define INS_FINV_GPS_ID GPS_MULTI_ID
102 #endif
103 PRINT_CONFIG_VAR(INS_FINV_GPS_ID)
104 
113 
114 static void baro_cb(uint8_t __attribute__((unused)) sender_id, float pressure)
115 {
117 }
118 
124 static void gyro_cb(uint8_t sender_id __attribute__((unused)),
125  uint32_t stamp, struct Int32Rates *gyro)
126 {
127  struct FloatRates gyro_f;
128  RATES_FLOAT_OF_BFP(gyro_f, *gyro);
129 
130 #if USE_AUTO_INS_FREQ || !defined(INS_PROPAGATE_FREQUENCY)
131  PRINT_CONFIG_MSG("Calculating dt for INS float_invariant propagation.")
132  /* timestamp in usec when last callback was received */
133  static uint32_t last_stamp = 0;
134 
135  if (last_stamp > 0) {
136  float dt = (float)(stamp - last_stamp) * 1e-6;
138  }
139  last_stamp = stamp;
140 #else
141  PRINT_CONFIG_MSG("Using fixed INS_PROPAGATE_FREQUENCY for INS float_invariant propagation.")
142  PRINT_CONFIG_VAR(INS_PROPAGATE_FREQUENCY)
143  const float dt = 1. / (INS_PROPAGATE_FREQUENCY);
145 #endif
146 
147  ins_finv_last_stamp = stamp;
148 }
149 
150 static void accel_cb(uint8_t sender_id __attribute__((unused)),
151  uint32_t stamp __attribute__((unused)),
152  struct Int32Vect3 *accel)
153 {
155 }
156 
157 static void mag_cb(uint8_t sender_id __attribute__((unused)),
158  uint32_t stamp __attribute__((unused)),
159  struct Int32Vect3 *mag)
160 {
162  struct FloatVect3 mag_f;
163  MAGS_FLOAT_OF_BFP(mag_f, *mag);
165  }
166 }
167 
168 static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
169  uint32_t stamp __attribute__((unused)),
170  struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
171  struct Int32Vect3 *lp_mag)
172 {
173  if (!ins_float_inv.is_aligned) {
174  /* convert to float */
175  struct FloatRates gyro_f;
176  RATES_FLOAT_OF_BFP(gyro_f, *lp_gyro);
177  struct FloatVect3 accel_f;
178  ACCELS_FLOAT_OF_BFP(accel_f, *lp_accel);
179  struct FloatVect3 mag_f;
180  MAGS_FLOAT_OF_BFP(mag_f, *lp_mag);
181  ins_float_invariant_align(&gyro_f, &accel_f, &mag_f);
182  }
183 }
184 
185 static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
186  struct FloatQuat *q_b2i_f)
187 {
189 }
190 
191 static void geo_mag_cb(uint8_t sender_id __attribute__((unused)), struct FloatVect3 *h)
192 {
193  ins_float_inv.mag_h = *h;
194 }
195 
196 static void gps_cb(uint8_t sender_id __attribute__((unused)),
197  uint32_t stamp __attribute__((unused)),
198  struct GpsState *gps_s)
199 {
201 }
202 
203 
205 {
207 
208  // Bind to ABI messages
209  AbiBindMsgBARO_ABS(INS_FINV_BARO_ID, &baro_ev, baro_cb);
210  AbiBindMsgIMU_MAG_INT32(INS_FINV_MAG_ID, &mag_ev, mag_cb);
211  AbiBindMsgIMU_GYRO_INT32(INS_FINV_IMU_ID, &gyro_ev, gyro_cb);
212  AbiBindMsgIMU_ACCEL_INT32(INS_FINV_IMU_ID, &accel_ev, accel_cb);
213  AbiBindMsgIMU_LOWPASSED(INS_FINV_IMU_ID, &aligner_ev, aligner_cb);
214  AbiBindMsgBODY_TO_IMU_QUAT(INS_FINV_IMU_ID, &body_to_imu_ev, body_to_imu_cb);
215  AbiBindMsgGEO_MAG(ABI_BROADCAST, &geo_mag_ev, geo_mag_cb);
216  AbiBindMsgGPS(INS_FINV_GPS_ID, &gps_ev, gps_cb);
217 
218 #if PERIODIC_TELEMETRY && !INS_FINV_USE_UTM
220  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STATE_FILTER_STATUS, send_filter_status);
221 #endif
222 }
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
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).
float dt
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:692
#define INS_FINV_MAG_ID
magnetometer
Architecture independent timing functions.
data structure for GPS information
Definition: gps.h:81
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:728
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)
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
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:740
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 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 baro_cb(uint8_t sender_id, float pressure)
static void send_ins_ref(struct transport_tx *trans, struct link_device *dev)
#define INS_FINV_BARO_ID
baro