Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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
26
28#include "modules/ahrs/ahrs.h"
29#include "modules/core/abi.h"
30#include "state.h"
31
32PRINT_CONFIG_VAR(AHRS_DCM_TYPE)
33
37
38static void set_body_orientation_and_rates(void);
39
40#if PERIODIC_TELEMETRY
42#include "mcu_periph/sys_time.h"
43
44static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
45{
46 uint8_t mde = 3;
47 uint16_t val = 0;
48 if (!ahrs_dcm.is_aligned) { mde = 2; }
50 /* set lost if no new gyro measurements for 50ms */
51 if (t_diff > 50000) { mde = 5; }
52 pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &ahrs_dcm_id, &mde, &val);
53}
54#endif
55
59#ifndef AHRS_DCM_IMU_ID
60#define AHRS_DCM_IMU_ID ABI_BROADCAST
61#endif
66#ifndef AHRS_DCM_MAG_ID
67#define AHRS_DCM_MAG_ID AHRS_DCM_IMU_ID
68#endif
73#ifndef AHRS_DCM_GPS_ID
74#define AHRS_DCM_GPS_ID GPS_MULTI_ID
75#endif
82
83
84static void gyro_cb(uint8_t __attribute__((unused)) sender_id,
85 uint32_t stamp, struct Int32Rates *gyro)
86{
87 ahrs_dcm_last_stamp = stamp;
88 struct FloatRates gyro_f;
89 RATES_FLOAT_OF_BFP(gyro_f, *gyro);
90
91#if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY)
92 PRINT_CONFIG_MSG("Calculating dt for AHRS dcm propagation.")
93 /* timestamp in usec when last callback was received */
94 static uint32_t last_stamp = 0;
95 if (last_stamp > 0 && ahrs_dcm.is_aligned) {
96 float dt = (float)(stamp - last_stamp) * 1e-6;
97 ahrs_dcm_propagate(&gyro_f, dt);
99 }
100 last_stamp = stamp;
101#else
102 PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for AHRS dcm propagation.")
104 if (ahrs_dcm.is_aligned) {
105 const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
106 ahrs_dcm_propagate(&gyro_f, dt);
108 }
109#endif
110}
111
112static void accel_cb(uint8_t sender_id __attribute__((unused)),
113 uint32_t stamp __attribute__((unused)),
114 struct Int32Vect3 *accel)
115{
116 if (ahrs_dcm.is_aligned) {
117 struct FloatVect3 accel_f;
118 ACCELS_FLOAT_OF_BFP(accel_f, *accel);
119 ahrs_dcm_update_accel(&accel_f);
120 }
121}
122
123static void mag_cb(uint8_t sender_id __attribute__((unused)),
124 uint32_t stamp __attribute__((unused)),
125 struct Int32Vect3 *mag)
126{
127 if (ahrs_dcm.is_aligned) {
128 struct FloatVect3 mag_f;
129 MAGS_FLOAT_OF_BFP(mag_f, *mag);
130 ahrs_dcm_update_mag(&mag_f);
131 }
132}
133
134static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
135 uint32_t stamp __attribute__((unused)),
136 struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
137 struct Int32Vect3 *lp_mag)
138{
139 if (!ahrs_dcm.is_aligned) {
140 /* convert to float */
141 struct FloatRates gyro_f;
142 RATES_FLOAT_OF_BFP(gyro_f, *lp_gyro);
143 struct FloatVect3 accel_f;
144 ACCELS_FLOAT_OF_BFP(accel_f, *lp_accel);
145 struct FloatVect3 mag_f;
146 MAGS_FLOAT_OF_BFP(mag_f, *lp_mag);
147 /* set initial body orientation in state interface if alignment was successful */
148 if (ahrs_dcm_align(&gyro_f, &accel_f, &mag_f)) {
150 }
151 }
152}
153
154static void gps_cb(uint8_t sender_id __attribute__((unused)),
155 uint32_t stamp __attribute__((unused)),
156 struct GpsState *gps_s)
157{
158 ahrs_dcm_update_gps(gps_s);
159}
160
165{
166 /* Set the state */
167 stateSetBodyRates_f(MODULE_AHRS_FLOAT_DCM_ID, &ahrs_dcm.body_rate);
168
169 /* Convert eulers to RMaat and set state */
170 struct FloatRMat ltp_to_body_rmat;
171 float_rmat_of_eulers(&ltp_to_body_rmat, &ahrs_dcm.ltp_to_body_euler);
172 stateSetNedToBodyRMat_f(MODULE_AHRS_FLOAT_DCM_ID, &ltp_to_body_rmat);
173}
174
176{
178 if (AHRS_DCM_TYPE == AHRS_PRIMARY) {
180 } else {
182 }
183
184 /*
185 * Subscribe to scaled IMU measurements and attach callbacks
186 */
187 AbiBindMsgIMU_GYRO(AHRS_DCM_IMU_ID, &gyro_ev, gyro_cb);
188 AbiBindMsgIMU_ACCEL(AHRS_DCM_IMU_ID, &accel_ev, accel_cb);
189 AbiBindMsgIMU_MAG(AHRS_DCM_MAG_ID, &mag_ev, mag_cb);
190 AbiBindMsgIMU_LOWPASSED(ABI_BROADCAST, &aligner_ev, aligner_cb);
191 AbiBindMsgGPS(AHRS_DCM_GPS_ID, &gps_ev, gps_cb);
192
193#if PERIODIC_TELEMETRY
194 register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STATE_FILTER_STATUS, send_filter_status);
195#endif
196}
197
199{
200 if (enable) {
201 stateSetInputFilter(STATE_INPUT_ATTITUDE, MODULE_AHRS_FLOAT_DCM_ID);
202 stateSetInputFilter(STATE_INPUT_RATES, MODULE_AHRS_FLOAT_DCM_ID);
203 }
204 ahrs_dcm_enable = enable;
205}
206
Main include for ABI (AirBorneInterface).
struct abi_struct abi_event
Definition abi_common.h:101
#define ABI_BROADCAST
Broadcast address.
Definition abi_common.h:80
#define AHRS_COMP_ID_DCM
Definition ahrs.h:41
#define AHRS_PRIMARY
Definition ahrs.h:32
static void gyro_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *gyro)
static abi_event gyro_ev
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
static abi_event mag_ev
static void mag_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *mag)
static abi_event aligner_ev
static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
static void aligner_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel, struct Int32Vect3 *lp_mag)
static abi_event gps_ev
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)
void ahrs_dcm_wrapper_init(void)
void ahrs_float_dcm_wrapper_enable(uint8_t enable)
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
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 void send_filter_status(struct transport_tx *trans, struct link_device *dev)
static uint32_t ahrs_dcm_last_stamp
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)
uint8_t ahrs_dcm_enable
#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.
Paparazzi specific wrapper to run floating point DCM filter.
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
static struct uart_periph * dev
static abi_event accel_ev
static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
data structure for GPS information
Definition gps.h:90
#define STATE_INPUT_RATES
Definition state.h:145
#define STATE_INPUT_ATTITUDE
Definition state.h:144
#define float_rmat_of_eulers
rotation matrix
angular rates
#define MAGS_FLOAT_OF_BFP(_ef, _ei)
#define RATES_FLOAT_OF_BFP(_rf, _ri)
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
angular rates
static void stateSetNedToBodyRMat_f(uint16_t id, struct FloatRMat *ned_to_body_rmat)
Set vehicle body attitude from rotation matrix (float).
Definition state.h:1279
void stateSetInputFilter(uint8_t type, uint16_t flag)
set the input filter for a specified type of data.
Definition state.c:91
static void stateSetBodyRates_f(uint16_t id, struct FloatRates *body_rate)
Set vehicle body angular rate (float).
Definition state.h:1371
#define AHRS_PROPAGATE_FREQUENCY
Definition hf_float.c:55
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
PRINT_CONFIG_VAR(ONELOOP_ANDI_FILT_CUTOFF)
API to get/set the generic vehicle states.
Architecture independent timing functions.
int16_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint16_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.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.