Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
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,
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 }
baro_ev
static abi_event baro_ev
Definition: ins_float_invariant_wrapper.c:112
baro_cb
static void baro_cb(uint8_t sender_id, uint32_t stamp, float pressure)
Definition: ins_float_invariant_wrapper.c:123
MAGS_FLOAT_OF_BFP
#define MAGS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:807
LlaCoor_i::lon
int32_t lon
in degrees*1e7
Definition: pprz_geodetic_int.h:61
uint16_t
unsigned short uint16_t
Definition: types.h:16
val
uint16_t val[TCOUPLE_NB]
Definition: temp_tcouple_adc.c:49
ins_float_invariant_wrapper_init
void ins_float_invariant_wrapper_init(void)
Definition: ins_float_invariant_wrapper.c:215
LlaCoor_i::alt
int32_t alt
in millimeters above WGS84 reference ellipsoid
Definition: pprz_geodetic_int.h:62
gyro_ev
static abi_event gyro_ev
Definition: ins_float_invariant_wrapper.c:113
gps_cb
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
Definition: ins_float_invariant_wrapper.c:207
Int32Rates
angular rates
Definition: pprz_algebra_int.h:179
State::ned_origin_i
struct LtpDef_i ned_origin_i
Definition of the local (flat earth) coordinate system.
Definition: state.h:166
h
static void h(const real32_T x[7], const real32_T q[4], real32_T y[6])
Definition: UKF_Wind_Estimator.c:821
abi.h
ins_finv_last_stamp
static uint32_t ins_finv_last_stamp
last gyro msg timestamp
Definition: ins_float_invariant_wrapper.c:44
INS_FINV_MAG_ID
#define INS_FINV_MAG_ID
magnetometer
Definition: ins_float_invariant_wrapper.c:97
LtpDef_i::ecef
struct EcefCoor_i ecef
Reference point in ecef.
Definition: pprz_geodetic_int.h:99
GpsState
data structure for GPS information
Definition: gps.h:87
INS_FINV_GPS_ID
#define INS_FINV_GPS_ID
ABI binding for gps data.
Definition: ins_float_invariant_wrapper.c:108
RATES_FLOAT_OF_BFP
#define RATES_FLOAT_OF_BFP(_rf, _ri)
Definition: pprz_algebra.h:759
abi_struct
Event structure to store callbacks in a linked list.
Definition: abi_common.h:65
EcefCoor_i::x
int32_t x
in centimeters
Definition: pprz_geodetic_int.h:51
uint32_t
unsigned long uint32_t
Definition: types.h:18
ins_finv_accel
static struct FloatVect3 ins_finv_accel
last accel measurement
Definition: ins_float_invariant_wrapper.c:41
INS_FINV_IMU_ID
#define INS_FINV_IMU_ID
IMU (gyro, accel)
Definition: ins_float_invariant_wrapper.c:90
aligner_ev
static abi_event aligner_ev
Definition: ins_float_invariant_wrapper.c:115
body_to_imu_ev
static abi_event body_to_imu_ev
Definition: ins_float_invariant_wrapper.c:116
foo
uint16_t foo
Definition: main_demo5.c:59
State::ned_initialized_i
bool ned_initialized_i
true if local int coordinate frame is initialsed
Definition: state.h:171
ahrs_aligner.h
EcefCoor_i::y
int32_t y
in centimeters
Definition: pprz_geodetic_int.h:52
get_sys_time_usec
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:68
FloatVect3
Definition: pprz_algebra_float.h:54
LlaCoor_i::lat
int32_t lat
in degrees*1e7
Definition: pprz_geodetic_int.h:60
telemetry.h
FloatQuat
Roation quaternion.
Definition: pprz_algebra_float.h:63
EcefCoor_i::z
int32_t z
in centimeters
Definition: pprz_geodetic_int.h:53
body_to_imu_cb
static void body_to_imu_cb(uint8_t sender_id, struct FloatQuat *q_b2i_f)
Definition: ins_float_invariant_wrapper.c:183
ins_float_invariant_init
void ins_float_invariant_init(void)
Definition: ins_float_invariant.c:219
geo_mag_ev
static abi_event geo_mag_ev
Definition: ins_float_invariant_wrapper.c:119
dev
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
Int32Vect3
Definition: pprz_algebra_int.h:88
sys_time.h
Architecture independent timing functions.
uint8_t
unsigned char uint8_t
Definition: types.h:14
register_periodic_telemetry
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46
ins_float_invariant_update_mag
void ins_float_invariant_update_mag(struct FloatVect3 *mag)
Definition: ins_float_invariant.c:527
ins_float_invariant_update_gps
void ins_float_invariant_update_gps(struct GpsState *gps_s)
Definition: ins_float_invariant.c:439
InsFloatInv::mag_h
struct FloatVect3 mag_h
Definition: ins_float_invariant.h:118
send_ins_ref
static void send_ins_ref(struct transport_tx *trans, struct link_device *dev)
Definition: ins_float_invariant_wrapper.c:49
INS_FINV_BARO_ID
#define INS_FINV_BARO_ID
baro
Definition: ins_float_invariant_wrapper.c:83
gps_ev
static abi_event gps_ev
Definition: ins_float_invariant_wrapper.c:121
PRINT_CONFIG_MSG
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
INS_FINV_FILTER_ID
#define INS_FINV_FILTER_ID
Definition: ins_float_invariant_wrapper.c:37
send_filter_status
static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
Definition: ins_float_invariant_wrapper.c:61
mag_ev
static abi_event mag_ev
Definition: ins_float_invariant_wrapper.c:118
ins_float_invariant_align
void ins_float_invariant_align(struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel, struct FloatVect3 *lp_mag)
Definition: ins_float_invariant.c:315
LtpDef_i::hmsl
int32_t hmsl
Height above mean sea level in mm.
Definition: pprz_geodetic_int.h:102
InsFloatInv::is_aligned
bool is_aligned
Definition: ins_float_invariant.h:119
gyro_cb
static void gyro_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *gyro)
Call ins_float_invariant_propagate on new gyro measurements.
Definition: ins_float_invariant_wrapper.c:133
accel_cb
static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
Definition: ins_float_invariant_wrapper.c:159
aligner_cb
static void aligner_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel, struct Int32Vect3 *lp_mag)
Definition: ins_float_invariant_wrapper.c:166
ins_float_invariant_wrapper.h
ACCELS_FLOAT_OF_BFP
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:795
ins_float_invariant_update_baro
void ins_float_invariant_update_baro(float pressure)
Definition: ins_float_invariant.c:491
ins_float_inv
struct InsFloatInv ins_float_inv
Definition: ins_float_invariant.c:137
ins_float_invariant_propagate
void ins_float_invariant_propagate(struct FloatRates *gyro, struct FloatVect3 *accel, float dt)
Definition: ins_float_invariant.c:337
accel_ev
static abi_event accel_ev
Definition: ins_float_invariant_wrapper.c:114
state.h
ABI_BROADCAST
#define ABI_BROADCAST
Broadcast address.
Definition: abi_common.h:56
state
struct State state
Definition: state.c:36
geo_mag_cb
static void geo_mag_cb(uint8_t sender_id, struct FloatVect3 *h)
Definition: ins_float_invariant_wrapper.c:201
LtpDef_i::lla
struct LlaCoor_i lla
Reference point in lla.
Definition: pprz_geodetic_int.h:100
DefaultPeriodic
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
mag_cb
static void mag_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *mag)
Definition: ins_float_invariant_wrapper.c:190
ins_float_inv_set_body_to_imu_quat
void ins_float_inv_set_body_to_imu_quat(struct FloatQuat *q_b2i)
Definition: ins_float_invariant.c:713
ahrs_aligner_init
void ahrs_aligner_init(void)
Definition: ahrs_aligner.c:81
FloatRates
angular rates
Definition: pprz_algebra_float.h:93