Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
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 
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 
103 float xsens_gps_arm_x = 0;
104 float xsens_gps_arm_y = 0;
105 float xsens_gps_arm_z = 0;
106 
107 volatile int xsens_configured = 0;
108 
109 struct Xsens xsens;
110 
112 
113 void xsens_init(void)
114 {
116  xsens_configured = 20;
117 
118  xsens_msg_status = 0;
119  xsens_time_stamp = 0;
122 }
123 
124 void xsens_periodic(void)
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 
184 void parse_xsens_msg(void)
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);
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;
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);
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);
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);
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);
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  }
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);
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
static const float offset[]
#define Min(x, y)
Definition: esc_dshot.c:109
struct GpsState gps
global GPS state
Definition: gps.c:69
#define GPS_VALID_VEL_NED_BIT
Definition: gps.h:51
#define GPS_VALID_POS_LLA_BIT
Definition: gps.h:48
#define GPS_NB_CHANNELS
Definition: gps.h:56
#define GPS_FIX_NONE
No GPS fix.
Definition: gps.h:41
#define GPS_VALID_HMSL_BIT
Definition: gps.h:52
uint8_t valid_fields
bitfield indicating valid fields (GPS_VALID_x_BIT)
Definition: gps.h:87
#define GPS_FIX_3D
3D GPS fix
Definition: gps.h:43
float q
in rad/s
float phi
in radians
float p
in rad/s
float r
in rad/s
float theta
in radians
float psi
in radians
#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
float alt
in meters (normally above WGS84 reference ellipsoid)
float lon
in radians
float lat
in radians
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
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
struct FloatVect3 accel
Definition: xsens.h:57
int8_t sec
Definition: xsens.h:45
struct FloatQuat quat
Definition: xsens.h:63
bool mag_available
Definition: xsens.h:71
struct LlaCoor_f lla_f
Definition: xsens.h:60
struct XsensParser parser
Definition: xsens.h:66
int8_t day
Definition: xsens.h:49
int16_t year
Definition: xsens.h:47
int8_t hour
Definition: xsens.h:43
int8_t month
Definition: xsens.h:48
bool accel_available
Definition: xsens.h:70
struct FloatEulers euler
Definition: xsens.h:64
int32_t nanosec
Definition: xsens.h:46
struct FloatVect3 vel
NED velocity in m/s.
Definition: xsens.h:61
struct FloatVect3 mag
Definition: xsens.h:58
struct FloatRates gyro
Definition: xsens.h:56
bool gyro_available
Definition: xsens.h:69
int8_t min
Definition: xsens.h:44
uint16_t time_stamp
Definition: xsens.h:54
struct XsensTime time
Definition: xsens.h:53
volatile bool new_attitude
Definition: xsens.h:67
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.
uint8_t status
Definition: xsens_parser.h:49
uint8_t id
Definition: xsens_parser.h:48
uint8_t msg_buf[XSENS_MAX_PAYLOAD]
Definition: xsens_parser.h:53