Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
imu_xsens.c
Go to the documentation of this file.
1/*
2 * Copyright (C) 2003 Pascal Brisset, Antoine Drouin
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, write to
18 * the Free Software Foundation, 59 Temple Place - Suite 330,
19 * Boston, MA 02111-1307, USA.
20 *
21 */
22
27
28#include "imu_xsens.h"
29#include "xsens.h"
30
31#include "generated/airframe.h"
32
33#include "mcu_periph/sys_time.h"
34#include "modules/core/abi.h"
35
36static void handle_ins_msg(void);
37
39{
40 xsens_init();
41}
42
44{
45 xsens_parser_event(&(xsens.parser));
46 if (xsens.parser.msg_received) {
49 xsens.parser.msg_received = FALSE;
50 }
51}
52
53static void handle_ins_msg(void)
54{
55 uint32_t now_ts = get_sys_time_usec();
56#ifdef XSENS_BACKWARDS
57 if (xsens.gyro_available) {
58 struct Int32Rates gyro = {
59 -RATE_BFP_OF_REAL(xsens.gyro.p),
60 -RATE_BFP_OF_REAL(xsens.gyro.q),
62 };
63 xsens.gyro_available = FALSE
64 AbiSendMsgIMU_GYRO_RAW(IMU_XSENS_ID, now_ts, &gyro, 1, NAN, NAN); //FIXME: samplerate?
65 }
66 if (xsens.accel_available) {
67 struct Int32Vect3 accel = {
68 -ACCEL_BFP_OF_REAL(xsens.accel.x),
69 -ACCEL_BFP_OF_REAL(xsens.accel.y),
71 };
72 xsens.accel_available = FALSE;
73 AbiSendMsgIMU_ACCEL_RAW(IMU_XSENS_ID, now_ts, &accel, 1, NAN, NAN); //FIXME: samplerate?
74 }
75 if (xsens.mag_available) {
76 struct Int32Vect3 mag = {
77 -MAG_BFP_OF_REAL(xsens.mag.x),
78 -MAG_BFP_OF_REAL(xsens.mag.y),
80 };
81 xsens.mag_available = FALSE;
82 AbiSendMsgIMU_MAG_RAW(IMU_XSENS_ID, now_ts, &mag);
83 }
84#else
85 if (xsens.gyro_available) {
86 struct Int32Rates gyro = {
87 RATE_BFP_OF_REAL(xsens.gyro.p),
88 RATE_BFP_OF_REAL(xsens.gyro.q),
90 };
91 AbiSendMsgIMU_GYRO_RAW(IMU_XSENS_ID, now_ts, &gyro, 1, NAN, NAN); //FIXME: samplerate?
92 xsens.gyro_available = FALSE;
93 }
94 if (xsens.accel_available) {
95 struct Int32Vect3 accel = {
96 ACCEL_BFP_OF_REAL(xsens.accel.x),
97 ACCEL_BFP_OF_REAL(xsens.accel.y),
99 };
100 AbiSendMsgIMU_ACCEL_RAW(IMU_XSENS_ID, now_ts, &accel, 1, NAN, NAN); //FIXME: samplerate?
101 xsens.accel_available = FALSE;
102 }
103 if (xsens.mag_available) {
104 struct Int32Vect3 mag = {
105 MAG_BFP_OF_REAL(xsens.mag.x),
106 MAG_BFP_OF_REAL(xsens.mag.y),
108 };
109 AbiSendMsgIMU_MAG_RAW(IMU_XSENS_ID, now_ts, &mag);
110 xsens.mag_available = FALSE;
111 }
112#endif /* XSENS_BACKWARDS */
113}
Main include for ABI (AirBorneInterface).
#define IMU_XSENS_ID
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
#define ACCEL_BFP_OF_REAL(_af)
#define MAG_BFP_OF_REAL(_af)
#define RATE_BFP_OF_REAL(_af)
angular rates
static void handle_ins_msg(void)
Definition imu_xsens.c:53
void imu_xsens_event(void)
Definition imu_xsens.c:43
void imu_xsens_init(void)
Definition imu_xsens.c:38
XSENS to just provide IMU measurements.
#define FALSE
Definition std.h:5
Architecture independent timing functions.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
void xsens_init(void)
Definition xsens.c:113
void parse_xsens_msg(void)
Definition xsens.c:184
struct Xsens xsens
Definition xsens.c:109
Parser for the Xsens protocol.
void xsens_parser_event(struct XsensParser *xsensparser)