Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
ahrs_int_cmpl_euler_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_ICE_OUTPUT_ENABLED
33 #define AHRS_ICE_OUTPUT_ENABLED TRUE
34 #endif
35 PRINT_CONFIG_VAR(AHRS_ICE_OUTPUT_ENABLED)
36 
37 
41 
42 static void set_body_state_from_euler(void);
43 
44 #if PERIODIC_TELEMETRY
46 #include "mcu_periph/sys_time.h"
47 #include "state.h"
48 
49 static void send_filter(struct transport_tx *trans, struct link_device *dev)
50 {
51  pprz_msg_send_FILTER(trans, dev, AC_ID,
67  &ahrs_ice_id);
68 }
69 
70 static void send_euler(struct transport_tx *trans, struct link_device *dev)
71 {
72  struct Int32Eulers *eulers = stateGetNedToBodyEulers_i();
73  pprz_msg_send_AHRS_EULER_INT(trans, dev, AC_ID,
77  &(eulers->phi),
78  &(eulers->theta),
79  &(eulers->psi),
80  &ahrs_ice_id);
81 }
82 
83 static void send_bias(struct transport_tx *trans, struct link_device *dev)
84 {
85  pprz_msg_send_AHRS_GYRO_BIAS_INT(trans, dev, AC_ID,
88 }
89 
90 static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
91 {
92  uint8_t mde = 3;
93  uint16_t val = 0;
94  if (!ahrs_ice.is_aligned) { mde = 2; }
96  /* set lost if no new gyro measurements for 50ms */
97  if (t_diff > 50000) { mde = 5; }
98  pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &ahrs_ice_id, &mde, &val);
99 }
100 #endif
101 
105 #ifndef AHRS_ICE_IMU_ID
106 #define AHRS_ICE_IMU_ID ABI_BROADCAST
107 #endif
108 PRINT_CONFIG_VAR(AHRS_ICE_IMU_ID)
112 #ifndef AHRS_ICE_MAG_ID
113 #define AHRS_ICE_MAG_ID AHRS_ICE_IMU_ID
114 #endif
115 PRINT_CONFIG_VAR(AHRS_ICE_MAG_ID)
121 
122 
123 static void gyro_cb(uint8_t sender_id __attribute__((unused)),
124  uint32_t stamp, struct Int32Rates *gyro)
125 {
126  ahrs_ice_last_stamp = stamp;
127  if (ahrs_ice.is_aligned) {
128  ahrs_ice_propagate(gyro);
130  }
131 }
132 
133 static void accel_cb(uint8_t sender_id __attribute__((unused)),
134  uint32_t stamp __attribute__((unused)),
135  struct Int32Vect3 *accel)
136 {
137  if (ahrs_ice.is_aligned) {
138  ahrs_ice_update_accel(accel);
139  }
140 }
141 
142 static void mag_cb(uint8_t sender_id __attribute__((unused)),
143  uint32_t stamp __attribute__((unused)),
144  struct Int32Vect3 *mag)
145 {
146  if (ahrs_ice.is_aligned) {
147  ahrs_ice_update_mag(mag);
148  }
149 }
150 
151 static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
152  uint32_t stamp __attribute__((unused)),
153  struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
154  struct Int32Vect3 *lp_mag)
155 {
156  if (!ahrs_ice.is_aligned) {
157  if (ahrs_ice_align(lp_gyro, lp_accel, lp_mag)) {
159  }
160  }
161 }
162 
163 static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
164  struct FloatQuat *q_b2i_f)
165 {
167 }
168 
169 static bool ahrs_ice_enable_output(bool enable)
170 {
171  ahrs_ice_output_enabled = enable;
173 }
174 
175 /* Rotate angles and rates from imu to body frame and set state */
176 static void set_body_state_from_euler(void)
177 {
179  struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&ahrs_ice.body_to_imu);
180  struct Int32RMat ltp_to_imu_rmat, ltp_to_body_rmat;
181  /* Compute LTP to IMU rotation matrix */
182  int32_rmat_of_eulers(&ltp_to_imu_rmat, &ahrs_ice.ltp_to_imu_euler);
183  /* Compute LTP to BODY rotation matrix */
184  int32_rmat_comp_inv(&ltp_to_body_rmat, &ltp_to_imu_rmat, body_to_imu_rmat);
185  /* Set state */
186  stateSetNedToBodyRMat_i(&ltp_to_body_rmat);
187 
188  struct Int32Rates body_rate;
189  /* compute body rates */
190  int32_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_ice.imu_rate);
191  /* Set state */
192  stateSetBodyRates_i(&body_rate);
193  }
194 }
195 
197 {
199  ahrs_ice_init();
201 
202  /*
203  * Subscribe to scaled IMU measurements and attach callbacks
204  */
205  AbiBindMsgIMU_GYRO_INT32(AHRS_ICE_IMU_ID, &gyro_ev, gyro_cb);
206  AbiBindMsgIMU_ACCEL_INT32(AHRS_ICE_IMU_ID, &accel_ev, accel_cb);
207  AbiBindMsgIMU_MAG_INT32(AHRS_ICE_MAG_ID, &mag_ev, mag_cb);
208  AbiBindMsgIMU_LOWPASSED(ABI_BROADCAST, &aligner_ev, aligner_cb);
209  AbiBindMsgBODY_TO_IMU_QUAT(ABI_BROADCAST, &body_to_imu_ev, body_to_imu_cb);
210 
211 #if PERIODIC_TELEMETRY
213  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AHRS_EULER_INT, send_euler);
214  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AHRS_GYRO_BIAS_INT, send_bias);
215  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STATE_FILTER_STATUS, send_filter_status);
216 #endif
217 }
ahrs_ice
struct AhrsIntCmplEuler ahrs_ice
Definition: ahrs_int_cmpl_euler.c:46
set_body_state_from_euler
static void set_body_state_from_euler(void)
Definition: ahrs_int_cmpl_euler_wrapper.c:176
send_filter
static void send_filter(struct transport_tx *trans, struct link_device *dev)
Definition: ahrs_int_cmpl_euler_wrapper.c:49
Int32Eulers::theta
int32_t theta
in rad with INT32_ANGLE_FRAC
Definition: pprz_algebra_int.h:148
uint16_t
unsigned short uint16_t
Definition: types.h:16
val
uint16_t val[TCOUPLE_NB]
Definition: temp_tcouple_adc.c:49
Int32RMat
rotation matrix
Definition: pprz_algebra_int.h:159
Int32Rates
angular rates
Definition: pprz_algebra_int.h:179
abi.h
Int32Rates::q
int32_t q
in rad/s with INT32_RATE_FRAC
Definition: pprz_algebra_int.h:181
aligner_ev
static abi_event aligner_ev
Definition: ahrs_int_cmpl_euler_wrapper.c:119
send_euler
static void send_euler(struct transport_tx *trans, struct link_device *dev)
Definition: ahrs_int_cmpl_euler_wrapper.c:70
abi_struct
Event structure to store callbacks in a linked list.
Definition: abi_common.h:65
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: ahrs_int_cmpl_euler_wrapper.c:151
uint32_t
unsigned long uint32_t
Definition: types.h:18
ahrs_ice_id
static uint8_t ahrs_ice_id
Definition: ahrs_int_cmpl_euler_wrapper.c:40
ahrs_ice_init
void ahrs_ice_init(void)
Definition: ahrs_int_cmpl_euler.c:63
AhrsIntCmplEuler::hi_res_euler
struct Int32Eulers hi_res_euler
Definition: ahrs_int_cmpl_euler.h:46
AhrsIntCmplEuler::imu_rate
struct Int32Rates imu_rate
Definition: ahrs_int_cmpl_euler.h:45
AHRS_ICE_MAG_ID
#define AHRS_ICE_MAG_ID
ABI binding for magnetometer data.
Definition: ahrs_int_cmpl_euler_wrapper.c:113
get_sys_time_usec
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:68
Int32Rates::p
int32_t p
in rad/s with INT32_RATE_FRAC
Definition: pprz_algebra_int.h:180
telemetry.h
FloatQuat
Roation quaternion.
Definition: pprz_algebra_float.h:63
stateSetBodyRates_i
static void stateSetBodyRates_i(struct Int32Rates *body_rate)
Set vehicle body angular rate (int).
Definition: state.h:1173
send_filter_status
static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
Definition: ahrs_int_cmpl_euler_wrapper.c:90
gyro_ev
static abi_event gyro_ev
Definition: ahrs_int_cmpl_euler_wrapper.c:116
Int32Eulers::psi
int32_t psi
in rad with INT32_ANGLE_FRAC
Definition: pprz_algebra_int.h:149
Int32Eulers::phi
int32_t phi
in rad with INT32_ANGLE_FRAC
Definition: pprz_algebra_int.h:147
dev
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
AhrsIntCmplEuler::is_aligned
bool is_aligned
Definition: ahrs_int_cmpl_euler.h:57
Int32Vect3
Definition: pprz_algebra_int.h:88
sys_time.h
Architecture independent timing functions.
uint8_t
unsigned char uint8_t
Definition: types.h:14
mag_ev
static abi_event mag_ev
Definition: ahrs_int_cmpl_euler_wrapper.c:118
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
ahrs_int_cmpl_euler_wrapper.h
mag_cb
static void mag_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *mag)
Definition: ahrs_int_cmpl_euler_wrapper.c:142
Int32Eulers
euler angles
Definition: pprz_algebra_int.h:146
ahrs_ice_update_mag
void ahrs_ice_update_mag(struct Int32Vect3 *mag)
Definition: ahrs_int_cmpl_euler.c:224
int32_rmat_of_eulers
#define int32_rmat_of_eulers
Rotation matrix from Euler angles.
Definition: pprz_algebra_int.h:408
ahrs.h
int32_rmat_comp_inv
void int32_rmat_comp_inv(struct Int32RMat *m_a2b, struct Int32RMat *m_a2c, struct Int32RMat *m_b2c)
Composition (multiplication) of two rotation matrices.
Definition: pprz_algebra_int.c:91
AhrsIntCmplEuler::gyro_bias
struct Int32Rates gyro_bias
Definition: ahrs_int_cmpl_euler.h:44
body_to_imu_cb
static void body_to_imu_cb(uint8_t sender_id, struct FloatQuat *q_b2i_f)
Definition: ahrs_int_cmpl_euler_wrapper.c:163
send_bias
static void send_bias(struct transport_tx *trans, struct link_device *dev)
Definition: ahrs_int_cmpl_euler_wrapper.c:83
ahrs_ice_update_accel
void ahrs_ice_update_accel(struct Int32Vect3 *accel)
Definition: ahrs_int_cmpl_euler.c:202
int32_rmat_transp_ratemult
void int32_rmat_transp_ratemult(struct Int32Rates *rb, struct Int32RMat *m_b2a, struct Int32Rates *ra)
rotate anglular rates by transposed rotation matrix.
Definition: pprz_algebra_int.c:137
body_to_imu_ev
static abi_event body_to_imu_ev
Definition: ahrs_int_cmpl_euler_wrapper.c:120
ahrs_ice_output_enabled
static bool ahrs_ice_output_enabled
if TRUE with push the estimation results to the state interface
Definition: ahrs_int_cmpl_euler_wrapper.c:38
AHRS_ICE_OUTPUT_ENABLED
#define AHRS_ICE_OUTPUT_ENABLED
Definition: ahrs_int_cmpl_euler_wrapper.c:33
accel_cb
static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
Definition: ahrs_int_cmpl_euler_wrapper.c:133
AHRS_COMP_ID_ICE
#define AHRS_COMP_ID_ICE
Definition: ahrs.h:36
orientationGetRMat_i
static struct Int32RMat * orientationGetRMat_i(struct OrientationReps *orientation)
Get vehicle body attitude rotation matrix (int).
Definition: pprz_orientation_conversion.h:207
ahrs_ice_enable_output
static bool ahrs_ice_enable_output(bool enable)
Definition: ahrs_int_cmpl_euler_wrapper.c:169
AhrsIntCmplEuler::measure
struct Int32Eulers measure
Definition: ahrs_int_cmpl_euler.h:47
stateGetNedToBodyEulers_i
static struct Int32Eulers * stateGetNedToBodyEulers_i(void)
Get vehicle body attitude euler angles (int).
Definition: state.h:1125
ahrs_ice_last_stamp
static uint32_t ahrs_ice_last_stamp
Definition: ahrs_int_cmpl_euler_wrapper.c:39
stateSetNedToBodyRMat_i
static void stateSetNedToBodyRMat_i(struct Int32RMat *ned_to_body_rmat)
Set vehicle body attitude from rotation matrix (int).
Definition: state.h:1081
ahrs_register_impl
void ahrs_register_impl(AhrsEnableOutput enable)
Register an AHRS implementation.
Definition: ahrs.c:62
state.h
AhrsIntCmplEuler::ltp_to_imu_euler
struct Int32Eulers ltp_to_imu_euler
Definition: ahrs_int_cmpl_euler.h:50
AHRS_ICE_IMU_ID
#define AHRS_ICE_IMU_ID
ABI binding for IMU data.
Definition: ahrs_int_cmpl_euler_wrapper.c:106
ahrs_ice_register
void ahrs_ice_register(void)
Definition: ahrs_int_cmpl_euler_wrapper.c:196
AhrsIntCmplEuler::residual
struct Int32Eulers residual
Definition: ahrs_int_cmpl_euler.h:48
gyro_cb
static void gyro_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *gyro)
Definition: ahrs_int_cmpl_euler_wrapper.c:123
ABI_BROADCAST
#define ABI_BROADCAST
Broadcast address.
Definition: abi_common.h:56
accel_ev
static abi_event accel_ev
Definition: ahrs_int_cmpl_euler_wrapper.c:117
Int32Rates::r
int32_t r
in rad/s with INT32_RATE_FRAC
Definition: pprz_algebra_int.h:182
DefaultPeriodic
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
ahrs_ice_align
bool ahrs_ice_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel, struct Int32Vect3 *lp_mag)
Definition: ahrs_int_cmpl_euler.c:78
AhrsIntCmplEuler::body_to_imu
struct OrientationReps body_to_imu
Definition: ahrs_int_cmpl_euler.h:54
ahrs_ice_set_body_to_imu_quat
void ahrs_ice_set_body_to_imu_quat(struct FloatQuat *q_b2i)
Definition: ahrs_int_cmpl_euler.c:282
ahrs_ice_propagate
void ahrs_ice_propagate(struct Int32Rates *gyro)
Definition: ahrs_int_cmpl_euler.c:154