Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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
25
26#include "xsens.h"
27
28#include "generated/airframe.h"
29#include "led.h"
30
31#if USE_GPS_XSENS
33#endif
34
35// FIXME Debugging Only
36#include "mcu_periph/uart.h"
37#include "pprzlink/messages.h"
39
40
41/* output mode : calibrated, orientation, position, velocity, status
42 * -----------
43 *
44 * bit 0 temp
45 * bit 1 calibrated
46 * bit 2 orentation
47 * bit 3 aux
48 *
49 * bit 4 position
50 * bit 5 velocity
51 * bit 6-7 Reserved
52 *
53 * bit 8-10 Reserved
54 * bit 11 status
55 *
56 * bit 12 GPS PVT+baro
57 * bit 13 Reserved
58 * bit 14 Raw
59 * bit 15 Reserved
60 */
61
62#ifndef XSENS_OUTPUT_MODE
63#define XSENS_OUTPUT_MODE 0x1836
64#endif
65/* output settings : timestamp, euler, acc, rate, mag, float, no aux, lla, m/s, NED
66 * -----------
67 *
68 * bit 01 0=none, 1=sample counter, 2=utc, 3=sample+utc
69 * bit 23 0=quaternion, 1=euler, 2=DCM
70 *
71 * bit 4 1=disable acc output
72 * bit 5 1=disable gyro output
73 * bit 6 1=disable magneto output
74 * bit 7 Reserved
75 *
76 * bit 89 0=float, 1=fixedpoint12.20, 2=fixedpoint16.32
77 * bit 10 1=disable aux analog 1
78 * bit 11 1=disable aux analog 2
79 *
80 * bit 12-15 0-only: 14-16 WGS84
81 *
82 * bit 16-19 0-only: 16-18 m/s XYZ
83 *
84 * bit 20-23 Reserved
85 *
86 * bit 24-27 Reseverd
87 *
88 * bit 28-30 Reseverd
89 * bit 31 0=X-North-Z-Up, 1=North-East-Down
90 */
91#ifndef XSENS_OUTPUT_SETTINGS
92#define XSENS_OUTPUT_SETTINGS 0x80000C05
93#endif
94
100
101
106
107volatile int xsens_configured = 0;
108
109struct Xsens xsens = {0};
110
112
123
125{
126 if (xsens_configured > 0) {
127 switch (xsens_configured) {
128 case 20:
129 /* send mode and settings to MT */
130 XSENS_GoToConfig();
131 XSENS_SetOutputMode(xsens_output_mode);
132 XSENS_SetOutputSettings(xsens_output_settings);
133 break;
134 case 18:
135 // Give pulses on SyncOut
136 XSENS_SetSyncOutSettings(0, 0x0002);
137 break;
138 case 17:
139 // 1 pulse every 100 samples
140 XSENS_SetSyncOutSettings(1, 100);
141 break;
142 case 2:
143 XSENS_ReqLeverArmGps();
144 break;
145
146 case 6:
147 XSENS_ReqMagneticDeclination();
148 break;
149
150 case 13:
151#ifdef AHRS_H_X
152#pragma message "Sending XSens Magnetic Declination."
153 xsens_declination = atan2(AHRS_H_Y, AHRS_H_X);
154 XSENS_SetMagneticDeclination(xsens_declination);
155#endif
156 break;
157 case 12:
158#ifdef GPS_IMU_LEVER_ARM_X
159#pragma message "Sending XSens GPS Arm."
160 XSENS_SetLeverArmGps(GPS_IMU_LEVER_ARM_X, GPS_IMU_LEVER_ARM_Y, GPS_IMU_LEVER_ARM_Z);
161#endif
162 break;
163 case 10: {
164 uint8_t baud = 1;
165 XSENS_SetBaudrate(baud);
166 }
167 break;
168
169 case 1:
170 XSENS_GoToMeasurment();
171 break;
172
173 default:
174 break;
175 }
177 return;
178 }
179
180 RunOnceEvery(100, XSENS_ReqGPSStatus());
181}
182
183
185{
186 uint8_t offset = 0;
187 if (xsens.parser.id == XSENS_ReqOutputModeAck_ID) {
188 xsens_output_mode = XSENS_ReqOutputModeAck_mode(xsens.parser.msg_buf);
189 } else if (xsens.parser.id == XSENS_ReqOutputSettings_ID) {
190 xsens_output_settings = XSENS_ReqOutputSettingsAck_settings(xsens.parser.msg_buf);
191 } else if (xsens.parser.id == XSENS_ReqMagneticDeclinationAck_ID) {
192 xsens_declination = DegOfRad(XSENS_ReqMagneticDeclinationAck_declination(xsens.parser.msg_buf));
193
196 } else if (xsens.parser.id == XSENS_ReqLeverArmGpsAck_ID) {
197 xsens_gps_arm_x = XSENS_ReqLeverArmGpsAck_x(xsens.parser.msg_buf);
198 xsens_gps_arm_y = XSENS_ReqLeverArmGpsAck_y(xsens.parser.msg_buf);
199 xsens_gps_arm_z = XSENS_ReqLeverArmGpsAck_z(xsens.parser.msg_buf);
200
203 } else if (xsens.parser.id == XSENS_Error_ID) {
204 xsens_errorcode = XSENS_Error_errorcode(xsens.parser.msg_buf);
205 }
206#if USE_GPS_XSENS
207 else if (xsens.parser.id == XSENS_GPSStatus_ID) {
208 xsens.gps.nb_channels = XSENS_GPSStatus_nch(xsens.parser.msg_buf);
209 xsens.gps.num_sv = 0;
210
211 uint8_t i;
212 // Do not write outside buffer
213 for (i = 0; i < Min(xsens.gps.nb_channels, GPS_NB_CHANNELS); i++) {
214 uint8_t ch = XSENS_GPSStatus_chn(xsens.parser.msg_buf, i);
215 if (ch > xsens.gps.nb_channels) { continue; }
216 xsens.gps.svinfos[ch].svid = XSENS_GPSStatus_svid(xsens.parser.msg_buf, i);
217 xsens.gps.svinfos[ch].flags = XSENS_GPSStatus_bitmask(xsens.parser.msg_buf, i);
218 xsens.gps.svinfos[ch].qi = XSENS_GPSStatus_qi(xsens.parser.msg_buf, i);
219 xsens.gps.svinfos[ch].cno = XSENS_GPSStatus_cnr(xsens.parser.msg_buf, i);
220 if (xsens.gps.svinfos[ch].flags > 0) {
221 xsens.gps.num_sv++;
222 }
223 }
224 }
225#endif
226 else if (xsens.parser.id == XSENS_MTData_ID) {
227 /* test RAW modes else calibrated modes */
228 //if ((XSENS_MASK_RAWInertial(xsens_output_mode)) || (XSENS_MASK_RAWGPS(xsens_output_mode))) {
229 if (XSENS_MASK_RAWInertial(xsens_output_mode)) {
230 /* should we write raw data to separate struct? */
231 xsens.gyro.p = XSENS_DATA_RAWInertial_gyrX(xsens.parser.msg_buf, offset);
232 xsens.gyro.q = XSENS_DATA_RAWInertial_gyrY(xsens.parser.msg_buf, offset);
233 xsens.gyro.r = XSENS_DATA_RAWInertial_gyrZ(xsens.parser.msg_buf, offset);
234 xsens.gyro_available = TRUE;
235 offset += XSENS_DATA_RAWInertial_LENGTH;
236 }
237 if (XSENS_MASK_RAWGPS(xsens_output_mode)) {
238#if USE_GPS_XSENS_RAW_DATA && USE_GPS_XSENS
239 xsens.gps.week = 0; // FIXME
240 xsens.gps.tow = XSENS_DATA_RAWGPS_itow(xsens.parser.msg_buf, offset) * 10;
241 xsens.gps.lla_pos.lat = XSENS_DATA_RAWGPS_lat(xsens.parser.msg_buf, offset);
242 xsens.gps.lla_pos.lon = XSENS_DATA_RAWGPS_lon(xsens.parser.msg_buf, offset);
243 xsens.gps.lla_pos.alt = XSENS_DATA_RAWGPS_alt(xsens.parser.msg_buf, offset);
244 SetBit(xsens.gps.valid_fields, GPS_VALID_POS_LLA_BIT);
245
246 // Compute geoid (MSL) height
247 uint32_t hmsl = wgs84_ellipsoid_to_geoid_i(xsens.gps.lla_pos.lat, xsens.gps.lla_pos.lon);
248 xsens.gps.hmsl = XSENS_DATA_RAWGPS_alt(xsens.parser.msg_buf, offset) - hmsl;
249 SetBit(gps.valid_fields, GPS_VALID_HMSL_BIT);
250
251 xsens.gps.ned_vel.x = XSENS_DATA_RAWGPS_vel_n(xsens.parser.msg_buf, offset);
252 xsens.gps.ned_vel.y = XSENS_DATA_RAWGPS_vel_e(xsens.parser.msg_buf, offset);
253 xsens.gps.ned_vel.z = XSENS_DATA_RAWGPS_vel_d(xsens.parser.msg_buf, offset);
254 SetBit(gps.valid_fields, GPS_VALID_VEL_NED_BIT);
255 xsens.gps.pacc = XSENS_DATA_RAWGPS_hacc(xsens.parser.msg_buf, offset) / 100;
256 xsens.gps.sacc = XSENS_DATA_RAWGPS_sacc(xsens.parser.msg_buf, offset) / 100;
257 xsens.gps.pdop = 5; // FIXME Not output by XSens
258
259 xsens.gps_available = TRUE;
260#endif
261 offset += XSENS_DATA_RAWGPS_LENGTH;
262 }
263 //} else {
264 if (XSENS_MASK_Temp(xsens_output_mode)) {
265 offset += XSENS_DATA_Temp_LENGTH;
266 }
267 if (XSENS_MASK_Calibrated(xsens_output_mode)) {
268 uint8_t l = 0;
269 if (!XSENS_MASK_AccOut(xsens_output_settings)) {
270 xsens.accel.x = XSENS_DATA_Calibrated_accX(xsens.parser.msg_buf, offset);
271 xsens.accel.y = XSENS_DATA_Calibrated_accY(xsens.parser.msg_buf, offset);
272 xsens.accel.z = XSENS_DATA_Calibrated_accZ(xsens.parser.msg_buf, offset);
273 xsens.accel_available = TRUE;
274 l++;
275 }
276 if (!XSENS_MASK_GyrOut(xsens_output_settings)) {
277 xsens.gyro.p = XSENS_DATA_Calibrated_gyrX(xsens.parser.msg_buf, offset);
278 xsens.gyro.q = XSENS_DATA_Calibrated_gyrY(xsens.parser.msg_buf, offset);
279 xsens.gyro.r = XSENS_DATA_Calibrated_gyrZ(xsens.parser.msg_buf, offset);
280 xsens.gyro_available = TRUE;
281 l++;
282 }
283 if (!XSENS_MASK_MagOut(xsens_output_settings)) {
284 xsens.mag.x = XSENS_DATA_Calibrated_magX(xsens.parser.msg_buf, offset);
285 xsens.mag.y = XSENS_DATA_Calibrated_magY(xsens.parser.msg_buf, offset);
286 xsens.mag.z = XSENS_DATA_Calibrated_magZ(xsens.parser.msg_buf, offset);
287 xsens.mag_available = TRUE;
288 l++;
289 }
290 offset += l * XSENS_DATA_Calibrated_LENGTH / 3;
291 }
292 if (XSENS_MASK_Orientation(xsens_output_mode)) {
293 if (XSENS_MASK_OrientationMode(xsens_output_settings) == 0x00) {
294 xsens.quat.qi = XSENS_DATA_Quaternion_q0(xsens.parser.msg_buf, offset);
295 xsens.quat.qx = XSENS_DATA_Quaternion_q1(xsens.parser.msg_buf, offset);
296 xsens.quat.qy = XSENS_DATA_Quaternion_q2(xsens.parser.msg_buf, offset);
297 xsens.quat.qz = XSENS_DATA_Quaternion_q3(xsens.parser.msg_buf, offset);
298 //float_eulers_of_quat(&xsens.euler, &xsens.quat);
299 offset += XSENS_DATA_Quaternion_LENGTH;
300 }
301 if (XSENS_MASK_OrientationMode(xsens_output_settings) == 0x01) {
302 xsens.euler.phi = XSENS_DATA_Euler_roll(xsens.parser.msg_buf, offset) * M_PI / 180;
303 xsens.euler.theta = XSENS_DATA_Euler_pitch(xsens.parser.msg_buf, offset) * M_PI / 180;
304 xsens.euler.psi = XSENS_DATA_Euler_yaw(xsens.parser.msg_buf, offset) * M_PI / 180;
305 offset += XSENS_DATA_Euler_LENGTH;
306 }
307 if (XSENS_MASK_OrientationMode(xsens_output_settings) == 0x10) {
308 offset += XSENS_DATA_Matrix_LENGTH;
309 }
310 xsens.new_attitude = TRUE;
311 }
312 if (XSENS_MASK_Auxiliary(xsens_output_mode)) {
313 uint8_t l = 0;
314 if (!XSENS_MASK_Aux1Out(xsens_output_settings)) {
315 l++;
316 }
317 if (!XSENS_MASK_Aux2Out(xsens_output_settings)) {
318 l++;
319 }
320 offset += l * XSENS_DATA_Auxiliary_LENGTH / 2;
321 }
322 if (XSENS_MASK_Position(xsens_output_mode)) {
323 xsens.lla_f.lat = RadOfDeg(XSENS_DATA_Position_lat(xsens.parser.msg_buf, offset));
324 xsens.lla_f.lon = RadOfDeg(XSENS_DATA_Position_lon(xsens.parser.msg_buf, offset));
325 xsens.lla_f.alt = XSENS_DATA_Position_alt(xsens.parser.msg_buf, offset);
326 offset += XSENS_DATA_Position_LENGTH;
327
328#if (! USE_GPS_XSENS_RAW_DATA) && USE_GPS_XSENS
329 LLA_BFP_OF_REAL(xsens.gps.lla_pos, xsens.lla_f);
330 SetBit(gps.valid_fields, GPS_VALID_POS_LLA_BIT);
331 xsens.gps_available = TRUE;
332#endif
333 }
334 if (XSENS_MASK_Velocity(xsens_output_mode)) {
335 xsens.vel.x = XSENS_DATA_Velocity_vx(xsens.parser.msg_buf, offset);
336 xsens.vel.y = XSENS_DATA_Velocity_vy(xsens.parser.msg_buf, offset);
337 xsens.vel.z = XSENS_DATA_Velocity_vz(xsens.parser.msg_buf, offset);
338 offset += XSENS_DATA_Velocity_LENGTH;
339 }
340 if (XSENS_MASK_Status(xsens_output_mode)) {
341 xsens_msg_status = XSENS_DATA_Status_status(xsens.parser.msg_buf, offset);
342#if USE_GPS_XSENS
343 if (bit_is_set(xsens_msg_status, 2)) { xsens.gps.fix = GPS_FIX_3D; } // gps fix
344 else if (bit_is_set(xsens_msg_status, 1)) { xsens.gps.fix = 0x01; } // efk valid
345 else { xsens.gps.fix = GPS_FIX_NONE; }
346#ifdef GPS_LED
347 if (xsens.gps.fix == GPS_FIX_3D) {
348 LED_ON(GPS_LED);
349 } else {
350 LED_TOGGLE(GPS_LED);
351 }
352#endif // GPS_LED
353#endif // USE_GPS_XSENS
354 offset += XSENS_DATA_Status_LENGTH;
355 }
356 if (XSENS_MASK_TimeStamp(xsens_output_settings)) {
357 xsens.time_stamp = XSENS_DATA_TimeStamp_ts(xsens.parser.msg_buf, offset);
358 offset += XSENS_DATA_TimeStamp_LENGTH;
359 }
360 if (XSENS_MASK_UTC(xsens_output_settings)) {
361 xsens.time.hour = XSENS_DATA_UTC_hour(xsens.parser.msg_buf, offset);
362 xsens.time.min = XSENS_DATA_UTC_min(xsens.parser.msg_buf, offset);
363 xsens.time.sec = XSENS_DATA_UTC_sec(xsens.parser.msg_buf, offset);
364 xsens.time.nanosec = XSENS_DATA_UTC_nanosec(xsens.parser.msg_buf, offset);
365 xsens.time.year = XSENS_DATA_UTC_year(xsens.parser.msg_buf, offset);
366 xsens.time.month = XSENS_DATA_UTC_month(xsens.parser.msg_buf, offset);
367 xsens.time.day = XSENS_DATA_UTC_day(xsens.parser.msg_buf, offset);
368
369 offset += XSENS_DATA_UTC_LENGTH;
370 }
371 }
372
373}
#define LED_ON(i)
Definition led_hw.h:51
#define LED_TOGGLE(i)
Definition led_hw.h:53
#define Min(x, y)
Definition esc_dshot.c:110
struct GpsState gps
global GPS state
Definition gps.c:74
#define GPS_VALID_VEL_NED_BIT
Definition gps.h:52
#define GPS_VALID_POS_LLA_BIT
Definition gps.h:49
#define GPS_NB_CHANNELS
Definition gps.h:57
#define GPS_FIX_NONE
No GPS fix.
Definition gps.h:42
#define GPS_VALID_HMSL_BIT
Definition gps.h:53
#define GPS_FIX_3D
3D GPS fix
Definition gps.h:44
#define LLA_BFP_OF_REAL(_o, _i)
static int32_t wgs84_ellipsoid_to_geoid_i(int32_t lat, int32_t lon)
Get WGS84 ellipsoid/geoid separation.
arch independent LED (Light Emitting Diodes) API
WGS-84 Geoid Heights.
#define UNINIT
Receiving pprz messages.
Definition protocol.c:11
#define TRUE
Definition std.h:4
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
static const float offset[]
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.
Definition xsens.h:52
uint8_t xsens_msg_status
Definition xsens.c:96
void xsens_init(void)
Definition xsens.c:113
float xsens_gps_arm_y
Definition xsens.c:104
void xsens_periodic(void)
Definition xsens.c:124
volatile int xsens_configured
Definition xsens.c:107
uint16_t xsens_time_stamp
Definition xsens.c:97
uint8_t xsens_errorcode
Definition xsens.c:95
#define XSENS_OUTPUT_MODE
Definition xsens.c:63
#define XSENS_OUTPUT_SETTINGS
Definition xsens.c:92
void parse_xsens_msg(void)
Definition xsens.c:184
float xsens_gps_arm_z
Definition xsens.c:105
uint32_t xsens_output_settings
Definition xsens.c:99
uint16_t xsens_output_mode
Definition xsens.c:98
float xsens_declination
Definition xsens.c:102
struct Xsens xsens
Definition xsens.c:109
float xsens_gps_arm_x
Definition xsens.c:103
void parse_xsens_buffer(uint8_t c)
Parser for the Xsens protocol.