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