Paparazzi UAS  v5.14.0_stable-0-g3f680d1
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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 "subsystems/ahrs.h"
29 #include "subsystems/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)
87 
88 
89 static void gyro_cb(uint8_t __attribute__((unused)) sender_id,
90  uint32_t stamp, struct Int32Rates *gyro)
91 {
92  ahrs_dcm_last_stamp = stamp;
93  struct FloatRates gyro_f;
94  RATES_FLOAT_OF_BFP(gyro_f, *gyro);
95 
96 #if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY)
97  PRINT_CONFIG_MSG("Calculating dt for AHRS dcm propagation.")
98  /* timestamp in usec when last callback was received */
99  static uint32_t last_stamp = 0;
100  if (last_stamp > 0 && ahrs_dcm.is_aligned) {
101  float dt = (float)(stamp - last_stamp) * 1e-6;
102  ahrs_dcm_propagate(&gyro_f, dt);
104  }
105  last_stamp = stamp;
106 #else
107  PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for AHRS dcm propagation.")
108  PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY)
109  if (ahrs_dcm.is_aligned) {
110  const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
111  ahrs_dcm_propagate(&gyro_f, dt);
113  }
114 #endif
115 }
116 
117 static void accel_cb(uint8_t sender_id __attribute__((unused)),
118  uint32_t stamp __attribute__((unused)),
119  struct Int32Vect3 *accel)
120 {
121  if (ahrs_dcm.is_aligned) {
122  struct FloatVect3 accel_f;
123  ACCELS_FLOAT_OF_BFP(accel_f, *accel);
124  ahrs_dcm_update_accel(&accel_f);
125  }
126 }
127 
128 static void mag_cb(uint8_t sender_id __attribute__((unused)),
129  uint32_t stamp __attribute__((unused)),
130  struct Int32Vect3 *mag)
131 {
132  if (ahrs_dcm.is_aligned) {
133  struct FloatVect3 mag_f;
134  MAGS_FLOAT_OF_BFP(mag_f, *mag);
135  ahrs_dcm_update_mag(&mag_f);
136  }
137 }
138 
139 static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
140  uint32_t stamp __attribute__((unused)),
141  struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
142  struct Int32Vect3 *lp_mag)
143 {
144  if (!ahrs_dcm.is_aligned) {
145  /* convert to float */
146  struct FloatRates gyro_f;
147  RATES_FLOAT_OF_BFP(gyro_f, *lp_gyro);
148  struct FloatVect3 accel_f;
149  ACCELS_FLOAT_OF_BFP(accel_f, *lp_accel);
150  struct FloatVect3 mag_f;
151  MAGS_FLOAT_OF_BFP(mag_f, *lp_mag);
152  /* set initial body orientation in state interface if alignment was successful */
153  if (ahrs_dcm_align(&gyro_f, &accel_f, &mag_f)) {
155  }
156  }
157 }
158 
159 static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
160  struct FloatQuat *q_b2i_f)
161 {
163 }
164 
165 static void gps_cb(uint8_t sender_id __attribute__((unused)),
166  uint32_t stamp __attribute__((unused)),
167  struct GpsState *gps_s)
168 {
169  ahrs_dcm_update_gps(gps_s);
170 }
171 
172 static bool ahrs_dcm_enable_output(bool enable)
173 {
174  ahrs_dcm_output_enabled = enable;
176 }
177 
182 {
183  if (ahrs_dcm_output_enabled) {
184  struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ahrs_dcm.body_to_imu);
185 
186  struct FloatRates body_rate;
187  float_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_dcm.imu_rate);
188  stateSetBodyRates_f(&body_rate);
189 
190  struct FloatRMat ltp_to_imu_rmat, ltp_to_body_rmat;
191  float_rmat_of_eulers(&ltp_to_imu_rmat, &ahrs_dcm.ltp_to_imu_euler);
192  float_rmat_comp_inv(&ltp_to_body_rmat, &ltp_to_imu_rmat, body_to_imu_rmat);
193 
194  stateSetNedToBodyRMat_f(&ltp_to_body_rmat);
195  }
196 }
197 
199 {
200  ahrs_dcm_output_enabled = AHRS_DCM_OUTPUT_ENABLED;
201  ahrs_dcm_init();
203 
204  /*
205  * Subscribe to scaled IMU measurements and attach callbacks
206  */
207  AbiBindMsgIMU_GYRO_INT32(AHRS_DCM_IMU_ID, &gyro_ev, gyro_cb);
208  AbiBindMsgIMU_ACCEL_INT32(AHRS_DCM_IMU_ID, &accel_ev, accel_cb);
209  AbiBindMsgIMU_MAG_INT32(AHRS_DCM_MAG_ID, &mag_ev, mag_cb);
210  AbiBindMsgIMU_LOWPASSED(ABI_BROADCAST, &aligner_ev, aligner_cb);
211  AbiBindMsgBODY_TO_IMU_QUAT(ABI_BROADCAST, &body_to_imu_ev, body_to_imu_cb);
212  AbiBindMsgGPS(AHRS_DCM_GPS_ID, &gps_ev, gps_cb);
213 
214 #if PERIODIC_TELEMETRY
215  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STATE_FILTER_STATUS, send_filter_status);
216 #endif
217 }
#define AHRS_DCM_OUTPUT_ENABLED
Event structure to store callbacks in a linked list.
Definition: abi_common.h:65
unsigned short uint16_t
Definition: types.h:16
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 abi_event aligner_ev
Dispatcher to register actual AHRS implementations.
#define float_rmat_of_eulers
bool ahrs_dcm_align(struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel, struct FloatVect3 *lp_mag)
#define AHRS_PROPAGATE_FREQUENCY
Definition: hf_float.c:55
Periodic telemetry system header (includes downlink utility and generated code).
static void set_body_orientation_and_rates(void)
Compute body orientation and rates from imu orientation and rates.
static struct FloatRMat * orientationGetRMat_f(struct OrientationReps *orientation)
Get vehicle body attitude rotation matrix (float).
void ahrs_dcm_update_mag(struct FloatVect3 *mag)
void ahrs_dcm_init(void)
Main include for ABI (AirBorneInterface).
static bool ahrs_dcm_enable_output(bool enable)
void float_rmat_comp_inv(struct FloatRMat *m_a2b, struct FloatRMat *m_a2c, struct FloatRMat *m_b2c)
Composition (multiplication) of two rotation matrices.
static uint8_t ahrs_dcm_id
#define AHRS_COMP_ID_DCM
Definition: ahrs.h:38
static void aligner_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel, struct Int32Vect3 *lp_mag)
Paparazzi specific wrapper to run floating point DCM filter.
Roation quaternion.
struct FloatEulers ltp_to_imu_euler
static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
#define RATES_FLOAT_OF_BFP(_rf, _ri)
Definition: pprz_algebra.h:759
void ahrs_register_impl(AhrsEnableOutput enable)
Register an AHRS implementation.
Definition: ahrs.c:62
Architecture independent timing functions.
data structure for GPS information
Definition: gps.h:81
struct AhrsFloatDCM ahrs_dcm
uint16_t val[TCOUPLE_NB]
unsigned long uint32_t
Definition: types.h:18
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
void ahrs_dcm_update_gps(struct GpsState *gps_s)
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
void ahrs_dcm_update_accel(struct FloatVect3 *accel)
static abi_event mag_ev
static abi_event body_to_imu_ev
static abi_event gps_ev
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:795
#define AHRS_DCM_MAG_ID
ABI binding for magnetometer data.
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:73
struct OrientationReps body_to_imu
static void gyro_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *gyro)
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
void float_rmat_transp_ratemult(struct FloatRates *rb, struct FloatRMat *m_b2a, struct FloatRates *ra)
rotate anglular rates by transposed rotation matrix.
static bool ahrs_dcm_output_enabled
if TRUE with push the estimation results to the state interface
#define AHRS_DCM_GPS_ID
ABI binding for gps data.
void ahrs_dcm_set_body_to_imu_quat(struct FloatQuat *q_b2i)
static uint32_t ahrs_dcm_last_stamp
static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
rotation matrix
static void body_to_imu_cb(uint8_t sender_id, struct FloatQuat *q_b2i_f)
#define ABI_BROADCAST
Broadcast address.
Definition: abi_common.h:56
#define AHRS_DCM_IMU_ID
ABI binding for IMU data.
#define MAGS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:807
void ahrs_dcm_propagate(struct FloatRates *gyro, float dt)
static abi_event gyro_ev
static void mag_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *mag)
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:68
static void stateSetBodyRates_f(struct FloatRates *body_rate)
Set vehicle body angular rate (float).
Definition: state.h:1181
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
static abi_event accel_ev
angular rates
struct FloatRates imu_rate
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46
void ahrs_dcm_register(void)