Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
ahrs_float_dcm_wrapper.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2015 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, see
18  * <http://www.gnu.org/licenses/>.
19  */
20 
28 #include "modules/ahrs/ahrs.h"
29 #include "modules/core/abi.h"
30 #include "state.h"
31 
32 #ifndef AHRS_DCM_OUTPUT_ENABLED
33 #define AHRS_DCM_OUTPUT_ENABLED TRUE
34 #endif
35 PRINT_CONFIG_VAR(AHRS_DCM_OUTPUT_ENABLED)
36 
37 
41 
42 static void set_body_orientation_and_rates(void);
43 
44 #if PERIODIC_TELEMETRY
46 #include "mcu_periph/sys_time.h"
47 
48 static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
49 {
50  uint8_t mde = 3;
51  uint16_t val = 0;
52  if (!ahrs_dcm.is_aligned) { mde = 2; }
54  /* set lost if no new gyro measurements for 50ms */
55  if (t_diff > 50000) { mde = 5; }
56  pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &ahrs_dcm_id, &mde, &val);
57 }
58 #endif
59 
63 #ifndef AHRS_DCM_IMU_ID
64 #define AHRS_DCM_IMU_ID ABI_BROADCAST
65 #endif
66 PRINT_CONFIG_VAR(AHRS_DCM_IMU_ID)
70 #ifndef AHRS_DCM_MAG_ID
71 #define AHRS_DCM_MAG_ID AHRS_DCM_IMU_ID
72 #endif
73 PRINT_CONFIG_VAR(AHRS_DCM_MAG_ID)
77 #ifndef AHRS_DCM_GPS_ID
78 #define AHRS_DCM_GPS_ID GPS_MULTI_ID
79 #endif
80 PRINT_CONFIG_VAR(AHRS_DCM_GPS_ID)
86 
87 
88 static void gyro_cb(uint8_t __attribute__((unused)) sender_id,
89  uint32_t stamp, struct Int32Rates *gyro)
90 {
91  ahrs_dcm_last_stamp = stamp;
92  struct FloatRates gyro_f;
93  RATES_FLOAT_OF_BFP(gyro_f, *gyro);
94 
95 #if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY)
96  PRINT_CONFIG_MSG("Calculating dt for AHRS dcm propagation.")
97  /* timestamp in usec when last callback was received */
98  static uint32_t last_stamp = 0;
99  if (last_stamp > 0 && ahrs_dcm.is_aligned) {
100  float dt = (float)(stamp - last_stamp) * 1e-6;
101  ahrs_dcm_propagate(&gyro_f, dt);
103  }
104  last_stamp = stamp;
105 #else
106  PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for AHRS dcm propagation.")
107  PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY)
108  if (ahrs_dcm.is_aligned) {
109  const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
110  ahrs_dcm_propagate(&gyro_f, dt);
112  }
113 #endif
114 }
115 
116 static void accel_cb(uint8_t sender_id __attribute__((unused)),
117  uint32_t stamp __attribute__((unused)),
118  struct Int32Vect3 *accel)
119 {
120  if (ahrs_dcm.is_aligned) {
121  struct FloatVect3 accel_f;
122  ACCELS_FLOAT_OF_BFP(accel_f, *accel);
123  ahrs_dcm_update_accel(&accel_f);
124  }
125 }
126 
127 static void mag_cb(uint8_t sender_id __attribute__((unused)),
128  uint32_t stamp __attribute__((unused)),
129  struct Int32Vect3 *mag)
130 {
131  if (ahrs_dcm.is_aligned) {
132  struct FloatVect3 mag_f;
133  MAGS_FLOAT_OF_BFP(mag_f, *mag);
134  ahrs_dcm_update_mag(&mag_f);
135  }
136 }
137 
138 static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
139  uint32_t stamp __attribute__((unused)),
140  struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
141  struct Int32Vect3 *lp_mag)
142 {
143  if (!ahrs_dcm.is_aligned) {
144  /* convert to float */
145  struct FloatRates gyro_f;
146  RATES_FLOAT_OF_BFP(gyro_f, *lp_gyro);
147  struct FloatVect3 accel_f;
148  ACCELS_FLOAT_OF_BFP(accel_f, *lp_accel);
149  struct FloatVect3 mag_f;
150  MAGS_FLOAT_OF_BFP(mag_f, *lp_mag);
151  /* set initial body orientation in state interface if alignment was successful */
152  if (ahrs_dcm_align(&gyro_f, &accel_f, &mag_f)) {
154  }
155  }
156 }
157 
158 static void gps_cb(uint8_t sender_id __attribute__((unused)),
159  uint32_t stamp __attribute__((unused)),
160  struct GpsState *gps_s)
161 {
162  ahrs_dcm_update_gps(gps_s);
163 }
164 
165 static bool ahrs_dcm_enable_output(bool enable)
166 {
167  ahrs_dcm_output_enabled = enable;
169 }
170 
175 {
177  /* Set the state */
179 
180  /* Convert eulers to RMaat and set state */
181  struct FloatRMat ltp_to_body_rmat;
182  float_rmat_of_eulers(&ltp_to_body_rmat, &ahrs_dcm.ltp_to_body_euler);
183  stateSetNedToBodyRMat_f(&ltp_to_body_rmat);
184  }
185 }
186 
188 {
190  ahrs_dcm_init();
192 
193  /*
194  * Subscribe to scaled IMU measurements and attach callbacks
195  */
196  AbiBindMsgIMU_GYRO(AHRS_DCM_IMU_ID, &gyro_ev, gyro_cb);
197  AbiBindMsgIMU_ACCEL(AHRS_DCM_IMU_ID, &accel_ev, accel_cb);
198  AbiBindMsgIMU_MAG(AHRS_DCM_MAG_ID, &mag_ev, mag_cb);
199  AbiBindMsgIMU_LOWPASSED(ABI_BROADCAST, &aligner_ev, aligner_cb);
200  AbiBindMsgGPS(AHRS_DCM_GPS_ID, &gps_ev, gps_cb);
201 
202 #if PERIODIC_TELEMETRY
203  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STATE_FILTER_STATUS, send_filter_status);
204 #endif
205 }
Main include for ABI (AirBorneInterface).
#define ABI_BROADCAST
Broadcast address.
Definition: abi_common.h:58
Event structure to store callbacks in a linked list.
Definition: abi_common.h:67
void ahrs_register_impl(AhrsEnableOutput enable)
Register an AHRS implementation.
Definition: ahrs.c:62
Dispatcher to register actual AHRS implementations.
#define AHRS_COMP_ID_DCM
Definition: ahrs.h:38
void ahrs_dcm_propagate(struct FloatRates *gyro, float dt)
void ahrs_dcm_update_gps(struct GpsState *gps_s UNUSED)
void ahrs_dcm_update_mag(struct FloatVect3 *mag)
void ahrs_dcm_init(void)
void ahrs_dcm_update_accel(struct FloatVect3 *accel)
struct AhrsFloatDCM ahrs_dcm
bool ahrs_dcm_align(struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel, struct FloatVect3 *lp_mag)
struct FloatRates body_rate
struct FloatEulers ltp_to_body_euler
#define AHRS_DCM_OUTPUT_ENABLED
static bool ahrs_dcm_output_enabled
if TRUE with push the estimation results to the state interface
void ahrs_dcm_register(void)
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
static abi_event mag_ev
static abi_event accel_ev
static void gyro_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *gyro)
static void mag_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *mag)
#define AHRS_DCM_GPS_ID
ABI binding for gps data.
#define AHRS_DCM_IMU_ID
ABI binding for IMU data.
static abi_event gyro_ev
static abi_event aligner_ev
static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
static uint32_t ahrs_dcm_last_stamp
static bool ahrs_dcm_enable_output(bool enable)
static uint8_t ahrs_dcm_id
static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
static void aligner_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel, struct Int32Vect3 *lp_mag)
#define AHRS_DCM_MAG_ID
ABI binding for magnetometer data.
static void set_body_orientation_and_rates(void)
Compute body orientation and rates from imu orientation and rates.
static abi_event gps_ev
Paparazzi specific wrapper to run floating point DCM filter.
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:71
data structure for GPS information
Definition: gps.h:85
#define float_rmat_of_eulers
rotation matrix
angular rates
#define MAGS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:807
#define RATES_FLOAT_OF_BFP(_rf, _ri)
Definition: pprz_algebra.h:759
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:795
angular rates
static void stateSetNedToBodyRMat_f(struct FloatRMat *ned_to_body_rmat)
Set vehicle body attitude from rotation matrix (float).
Definition: state.h:1099
static void stateSetBodyRates_f(struct FloatRates *body_rate)
Set vehicle body angular rate (float).
Definition: state.h:1181
#define AHRS_PROPAGATE_FREQUENCY
Definition: hf_float.c:55
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
API to get/set the generic vehicle states.
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
Architecture independent timing functions.
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_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.
Definition: vl53l1_types.h:88
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98