Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
ins_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 #include "ins_module.h"
28 #include "ins_xsens.h"
29 #include "subsystems/ins.h"
30 
31 #include <inttypes.h>
32 
33 #include "generated/airframe.h"
34 
35 #include "mcu_periph/sys_time.h"
36 #include "messages.h"
37 
38 #if USE_GPS_XSENS
39 #if !USE_GPS
40 #error "USE_GPS needs to be 1 to use the Xsens GPS!"
41 #endif
42 #include "subsystems/gps.h"
45 #include "subsystems/navigation/common_nav.h" /* needed for nav_utm_zone0 */
46 bool_t gps_xsens_msg_available;
47 #endif
48 
49 // positions
53 
54 // velocities
58 
59 // body angles
63 
64 // angle rates
68 
69 // accelerations
73 
74 // magnetic
78 
79 #if USE_INS_MODULE
80 float ins_pitch_neutral;
81 float ins_roll_neutral;
82 #endif
83 
84 
86 //
87 // XSens Specific
88 //
89 
91 
92 #define XsensInitCheksum() { send_ck = 0; }
93 #define XsensUpdateChecksum(c) { send_ck += c; }
94 
95 #define XsensSend1(c) { uint8_t i8=c; InsUartSend1(i8); XsensUpdateChecksum(i8); }
96 #define XsensSend1ByAddr(x) { XsensSend1(*x); }
97 #define XsensSend2ByAddr(x) { XsensSend1(*(x+1)); XsensSend1(*x); }
98 #define XsensSend4ByAddr(x) { XsensSend1(*(x+3)); XsensSend1(*(x+2)); XsensSend1(*(x+1)); XsensSend1(*x); }
99 
100 #define XsensHeader(msg_id, len) { \
101  InsUartSend1(XSENS_START); \
102  XsensInitCheksum(); \
103  XsensSend1(XSENS_BID); \
104  XsensSend1(msg_id); \
105  XsensSend1(len); \
106 }
107 #define XsensTrailer() { uint8_t i8=0x100-send_ck; InsUartSend1(i8); }
108 
110 #include "xsens_protocol.h"
111 
112 
113 #define XSENS_MAX_PAYLOAD 254
115 
116 /* output mode : calibrated, orientation, position, velocity, status
117  * -----------
118  *
119  * bit 0 temp
120  * bit 1 calibrated
121  * bit 2 orentation
122  * bit 3 aux
123  *
124  * bit 4 position
125  * bit 5 velocity
126  * bit 6-7 Reserved
127  *
128  * bit 8-10 Reserved
129  * bit 11 status
130  *
131  * bit 12 GPS PVT+baro
132  * bit 13 Reserved
133  * bit 14 Raw
134  * bit 15 Reserved
135  */
136 
137 #ifndef XSENS_OUTPUT_MODE
138 #define XSENS_OUTPUT_MODE 0x1836
139 #endif
140 /* output settings : timestamp, euler, acc, rate, mag, float, no aux, lla, m/s, NED
141  * -----------
142  *
143  * bit 01 0=none, 1=sample counter, 2=utc, 3=sample+utc
144  * bit 23 0=quaternion, 1=euler, 2=DCM
145  *
146  * bit 4 1=disable acc output
147  * bit 5 1=disable gyro output
148  * bit 6 1=disable magneto output
149  * bit 7 Reserved
150  *
151  * bit 89 0=float, 1=fixedpoint12.20, 2=fixedpoint16.32
152  * bit 10 1=disable aux analog 1
153  * bit 11 1=disable aux analog 2
154  *
155  * bit 12-15 0-only: 14-16 WGS84
156  *
157  * bit 16-19 0-only: 16-18 m/s XYZ
158  *
159  * bit 20-23 Reserved
160  *
161  * bit 24-27 Reseverd
162  *
163  * bit 28-30 Reseverd
164  * bit 31 0=X-North-Z-Up, 1=North-East-Down
165  */
166 #ifndef XSENS_OUTPUT_SETTINGS
167 #define XSENS_OUTPUT_SETTINGS 0x80000C05
168 #endif
169 
170 #define UNINIT 0
171 #define GOT_START 1
172 #define GOT_BID 2
173 #define GOT_MID 3
174 #define GOT_LEN 4
175 #define GOT_DATA 5
176 #define GOT_CHECKSUM 6
177 
178 // FIXME Debugging Only
179 #include "mcu_periph/uart.h"
180 #include "messages.h"
182 
183 
189 
190 
192 float xsens_gps_arm_x = 0;
193 float xsens_gps_arm_y = 0;
194 float xsens_gps_arm_z = 0;
195 
196 #if USE_GPS_XSENS
197 struct LlaCoor_f lla_f;
198 struct UtmCoor_f utm_f;
199 #endif
200 
202 
207 static uint8_t ck;
209 
210 volatile int xsens_configured = 0;
211 
212 void xsens_init(void);
213 void xsens_periodic(void);
214 
215 void xsens_init(void) {
216 
218  xsens_configured = 20;
219 
220 #if USE_INS_MODULE
223 #endif
224 
225  xsens_msg_status = 0;
226  xsens_time_stamp = 0;
229 }
230 
231 #if USE_IMU
233 
234 void imu_impl_init(void) {
235  xsens_init();
239 }
240 
241 void imu_periodic(void) {
242  xsens_periodic();
243 }
244 #endif /* USE_IMU */
245 
246 #if USE_INS_MODULE
247 void ins_init(void) {
248  struct UtmCoor_f utm0 = { nav_utm_north0, nav_utm_east0, 0., nav_utm_zone0 };
250  stateSetPositionUtm_f(&utm0);
251 
252  xsens_init();
253 }
254 
255 void ins_periodic(void) {
256  xsens_periodic();
257 }
258 
259 void ins_update_gps(void) {
260  struct UtmCoor_f utm;
261  utm.east = gps.utm_pos.east / 100.;
262  utm.north = gps.utm_pos.north / 100.;
263  utm.zone = nav_utm_zone0;
264  utm.alt = gps.hmsl / 1000.;
265 
266  // set position
267  stateSetPositionUtm_f(&utm);
268 
269  struct NedCoor_f ned_vel = {
270  gps.ned_vel.x / 100.,
271  gps.ned_vel.y / 100.,
272  gps.ned_vel.z / 100.
273  };
274  // set velocity
275  stateSetSpeedNed_f(&ned_vel);
276 }
277 #endif
278 
279 #if USE_GPS_XSENS
280 void gps_impl_init(void) {
281  gps.nb_channels = 0;
282  gps_xsens_msg_available = FALSE;
283 }
284 #endif
285 
286 void xsens_periodic(void) {
287  if (xsens_configured > 0)
288  {
289  switch (xsens_configured)
290  {
291  case 20:
292  /* send mode and settings to MT */
293  XSENS_GoToConfig();
294  XSENS_SetOutputMode(xsens_output_mode);
295  XSENS_SetOutputSettings(xsens_output_settings);
296  break;
297  case 18:
298  // Give pulses on SyncOut
299  XSENS_SetSyncOutSettings(0,0x0002);
300  break;
301  case 17:
302  // 1 pulse every 100 samples
303  XSENS_SetSyncOutSettings(1,100);
304  break;
305  case 2:
306  XSENS_ReqLeverArmGps();
307  break;
308 
309  case 6:
310  XSENS_ReqMagneticDeclination();
311  break;
312 
313  case 13:
314 #ifdef AHRS_H_X
315 #pragma message "Sending XSens Magnetic Declination."
316  xsens_declination = atan2(AHRS_H_Y, AHRS_H_X);
317  XSENS_SetMagneticDeclination(xsens_declination);
318 #endif
319  break;
320  case 12:
321 #ifdef GPS_IMU_LEVER_ARM_X
322 #pragma message "Sending XSens GPS Arm."
323  XSENS_SetLeverArmGps(GPS_IMU_LEVER_ARM_X,GPS_IMU_LEVER_ARM_Y,GPS_IMU_LEVER_ARM_Z);
324 #endif
325  break;
326  case 10:
327  {
328  uint8_t baud = 1;
329  XSENS_SetBaudrate(baud);
330  }
331  break;
332 
333  case 1:
334  XSENS_GoToMeasurment();
335  break;
336 
337  default:
338  break;
339  }
341  return;
342  }
343 
344  RunOnceEvery(100,XSENS_ReqGPSStatus());
345 }
346 
347 #if USE_INS_MODULE
348 #include "state.h"
349 
350 static inline void update_fw_estimator(void) {
351  // Send to Estimator (Control)
352 #ifdef XSENS_BACKWARDS
353  struct FloatEulers att = {
356  ins_psi + RadOfDeg(180)
357  };
358  struct FloatRates rates = {
359  -ins_p,
360  -ins_q,
361  ins_r
362  };
363 #else
364  struct FloatEulers att = {
367  ins_psi
368  };
369  struct FloatRates rates = {
370  ins_p,
371  ins_q,
372  ins_r
373  };
374 #endif
376  stateSetBodyRates_f(&rates);
377 }
378 #endif /* USE_INS_MODULE */
379 
380 void handle_ins_msg(void) {
381 
382 #if USE_INS_MODULE
383  update_fw_estimator();
384 #endif
385 
386 #if USE_IMU
387 #ifdef XSENS_BACKWARDS
390  }
393  }
394  if (imu_xsens.mag_available) {
396  }
397 #else
400  }
403  }
404  if (imu_xsens.mag_available) {
406  }
407 #endif /* XSENS_BACKWARDS */
408 #endif /* USE_IMU */
409 
410 #if USE_GPS_XSENS
411 
412  // Horizontal speed
413  float fspeed = sqrt(ins_vx*ins_vx + ins_vy*ins_vy);
414  if (gps.fix != GPS_FIX_3D) {
415  fspeed = 0;
416  }
417  gps.gspeed = fspeed * 100.;
418  gps.speed_3d = (uint16_t)(sqrt(ins_vx*ins_vx + ins_vy*ins_vy + ins_vz*ins_vz) * 100);
419 
420  float fcourse = atan2f((float)ins_vy, (float)ins_vx);
421  gps.course = fcourse * 1e7;
422 #endif // USE_GPS_XSENS
423 }
424 
425 void parse_ins_msg(void) {
426  uint8_t offset = 0;
427  if (xsens_id == XSENS_ReqOutputModeAck_ID) {
428  xsens_output_mode = XSENS_ReqOutputModeAck_mode(xsens_msg_buf);
429  }
430  else if (xsens_id == XSENS_ReqOutputSettings_ID) {
431  xsens_output_settings = XSENS_ReqOutputSettingsAck_settings(xsens_msg_buf);
432  }
433  else if (xsens_id == XSENS_ReqMagneticDeclinationAck_ID) {
434  xsens_declination = DegOfRad (XSENS_ReqMagneticDeclinationAck_declination(xsens_msg_buf) );
435 
437  }
438  else if (xsens_id == XSENS_ReqLeverArmGpsAck_ID) {
439  xsens_gps_arm_x = XSENS_ReqLeverArmGpsAck_x(xsens_msg_buf);
440  xsens_gps_arm_y = XSENS_ReqLeverArmGpsAck_y(xsens_msg_buf);
441  xsens_gps_arm_z = XSENS_ReqLeverArmGpsAck_z(xsens_msg_buf);
442 
444  }
445  else if (xsens_id == XSENS_Error_ID) {
446  xsens_errorcode = XSENS_Error_errorcode(xsens_msg_buf);
447  }
448 #if USE_GPS_XSENS
449  else if (xsens_id == XSENS_GPSStatus_ID) {
450  gps.nb_channels = XSENS_GPSStatus_nch(xsens_msg_buf);
451  gps.num_sv = 0;
452 
453  uint8_t i;
454  // Do not write outside buffer
455  for(i = 0; i < Min(gps.nb_channels, GPS_NB_CHANNELS); i++) {
456  uint8_t ch = XSENS_GPSStatus_chn(xsens_msg_buf,i);
457  if (ch > gps.nb_channels) continue;
458  gps.svinfos[ch].svid = XSENS_GPSStatus_svid(xsens_msg_buf, i);
459  gps.svinfos[ch].flags = XSENS_GPSStatus_bitmask(xsens_msg_buf, i);
460  gps.svinfos[ch].qi = XSENS_GPSStatus_qi(xsens_msg_buf, i);
461  gps.svinfos[ch].cno = XSENS_GPSStatus_cnr(xsens_msg_buf, i);
462  if (gps.svinfos[ch].flags > 0)
463  {
464  gps.num_sv++;
465  }
466  }
467  }
468 #endif
469  else if (xsens_id == XSENS_MTData_ID) {
470  /* test RAW modes else calibrated modes */
471  //if ((XSENS_MASK_RAWInertial(xsens_output_mode)) || (XSENS_MASK_RAWGPS(xsens_output_mode))) {
472  if (XSENS_MASK_RAWInertial(xsens_output_mode)) {
473  ins_p = XSENS_DATA_RAWInertial_gyrX(xsens_msg_buf,offset);
474  ins_q = XSENS_DATA_RAWInertial_gyrY(xsens_msg_buf,offset);
475  ins_r = XSENS_DATA_RAWInertial_gyrZ(xsens_msg_buf,offset);
476 #if USE_IMU
478 #endif
479  offset += XSENS_DATA_RAWInertial_LENGTH;
480  }
481  if (XSENS_MASK_RAWGPS(xsens_output_mode)) {
482 #if USE_GPS_XSENS_RAW_DATA && USE_GPS_XSENS
483 
484  gps.week = 0; // FIXME
485  gps.tow = XSENS_DATA_RAWGPS_itow(xsens_msg_buf,offset) * 10;
486  gps.lla_pos.lat = XSENS_DATA_RAWGPS_lat(xsens_msg_buf,offset);
487  gps.lla_pos.lon = XSENS_DATA_RAWGPS_lon(xsens_msg_buf,offset);
488  gps.lla_pos.alt = XSENS_DATA_RAWGPS_alt(xsens_msg_buf,offset);
489 
490  /* Set the real UTM zone */
491  gps.utm_pos.zone = (gps.lla_pos.lon / 1e7 + 180) / 6 + 1;
494  /* convert to utm */
496  /* copy results of utm conversion */
497  gps.utm_pos.east = utm_f.east*100;
498  gps.utm_pos.north = utm_f.north*100;
500 
501  ins_x = utm_f.east;
502  ins_y = utm_f.north;
503  // Altitude: Xsens LLH gives ellipsoid height
504  ins_z = -(INS_FORMAT)XSENS_DATA_RAWGPS_alt(xsens_msg_buf,offset) / 1000.;
505 
506  // Compute geoid (MSL) height
507  float hmsl;
509  gps.hmsl = XSENS_DATA_RAWGPS_alt(xsens_msg_buf,offset) - (hmsl * 1000.0f);
510 
511  ins_vx = ((INS_FORMAT)XSENS_DATA_RAWGPS_vel_n(xsens_msg_buf,offset)) / 100.;
512  ins_vy = ((INS_FORMAT)XSENS_DATA_RAWGPS_vel_e(xsens_msg_buf,offset)) / 100.;
513  ins_vz = ((INS_FORMAT)XSENS_DATA_RAWGPS_vel_d(xsens_msg_buf,offset)) / 100.;
514  gps.ned_vel.x = XSENS_DATA_RAWGPS_vel_n(xsens_msg_buf,offset);
515  gps.ned_vel.y = XSENS_DATA_RAWGPS_vel_e(xsens_msg_buf,offset);
516  gps.ned_vel.z = XSENS_DATA_RAWGPS_vel_d(xsens_msg_buf,offset);
517  gps.pacc = XSENS_DATA_RAWGPS_hacc(xsens_msg_buf,offset) / 100;
518  gps.sacc = XSENS_DATA_RAWGPS_sacc(xsens_msg_buf,offset) / 100;
519  gps.pdop = 5; // FIXME Not output by XSens
520 
521  gps_xsens_msg_available = TRUE;
522 #endif
523  offset += XSENS_DATA_RAWGPS_LENGTH;
524  }
525  //} else {
526  if (XSENS_MASK_Temp(xsens_output_mode)) {
527  offset += XSENS_DATA_Temp_LENGTH;
528  }
529  if (XSENS_MASK_Calibrated(xsens_output_mode)) {
530  uint8_t l = 0;
531  if (!XSENS_MASK_AccOut(xsens_output_settings)) {
532  ins_ax = XSENS_DATA_Calibrated_accX(xsens_msg_buf,offset);
533  ins_ay = XSENS_DATA_Calibrated_accY(xsens_msg_buf,offset);
534  ins_az = XSENS_DATA_Calibrated_accZ(xsens_msg_buf,offset);
535 #if USE_IMU
537 #endif
538  l++;
539  }
540  if (!XSENS_MASK_GyrOut(xsens_output_settings)) {
541  ins_p = XSENS_DATA_Calibrated_gyrX(xsens_msg_buf,offset);
542  ins_q = XSENS_DATA_Calibrated_gyrY(xsens_msg_buf,offset);
543  ins_r = XSENS_DATA_Calibrated_gyrZ(xsens_msg_buf,offset);
544 #if USE_IMU
546 #endif
547  l++;
548  }
549  if (!XSENS_MASK_MagOut(xsens_output_settings)) {
550  ins_mx = XSENS_DATA_Calibrated_magX(xsens_msg_buf,offset);
551  ins_my = XSENS_DATA_Calibrated_magY(xsens_msg_buf,offset);
552  ins_mz = XSENS_DATA_Calibrated_magZ(xsens_msg_buf,offset);
553 #if USE_IMU
555 #endif
556  l++;
557  }
558  offset += l * XSENS_DATA_Calibrated_LENGTH / 3;
559  }
560  if (XSENS_MASK_Orientation(xsens_output_mode)) {
561  if (XSENS_MASK_OrientationMode(xsens_output_settings) == 0x00) {
562  float q0 = XSENS_DATA_Quaternion_q0(xsens_msg_buf,offset);
563  float q1 = XSENS_DATA_Quaternion_q1(xsens_msg_buf,offset);
564  float q2 = XSENS_DATA_Quaternion_q2(xsens_msg_buf,offset);
565  float q3 = XSENS_DATA_Quaternion_q3(xsens_msg_buf,offset);
566  float dcm00 = 1.0 - 2 * (q2*q2 + q3*q3);
567  float dcm01 = 2 * (q1*q2 + q0*q3);
568  float dcm02 = 2 * (q1*q3 - q0*q2);
569  float dcm12 = 2 * (q2*q3 + q0*q1);
570  float dcm22 = 1.0 - 2 * (q1*q1 + q2*q2);
571  ins_phi = atan2(dcm12, dcm22);
572  ins_theta = -asin(dcm02);
573  ins_psi = atan2(dcm01, dcm00);
574  offset += XSENS_DATA_Quaternion_LENGTH;
575  }
576  if (XSENS_MASK_OrientationMode(xsens_output_settings) == 0x01) {
577  ins_phi = XSENS_DATA_Euler_roll(xsens_msg_buf,offset) * M_PI / 180;
578  ins_theta = XSENS_DATA_Euler_pitch(xsens_msg_buf,offset) * M_PI / 180;
579  ins_psi = XSENS_DATA_Euler_yaw(xsens_msg_buf,offset) * M_PI / 180;
580  offset += XSENS_DATA_Euler_LENGTH;
581  }
582  if (XSENS_MASK_OrientationMode(xsens_output_settings) == 0x10) {
583  offset += XSENS_DATA_Matrix_LENGTH;
584  }
585  new_ins_attitude = 1;
586  }
587  if (XSENS_MASK_Auxiliary(xsens_output_mode)) {
588  uint8_t l = 0;
589  if (!XSENS_MASK_Aux1Out(xsens_output_settings)) {
590  l++;
591  }
592  if (!XSENS_MASK_Aux2Out(xsens_output_settings)) {
593  l++;
594  }
595  offset += l * XSENS_DATA_Auxiliary_LENGTH / 2;
596  }
597  if (XSENS_MASK_Position(xsens_output_mode)) {
598 #if (! USE_GPS_XSENS_RAW_DATA) && USE_GPS_XSENS
599  lla_f.lat = RadOfDeg(XSENS_DATA_Position_lat(xsens_msg_buf,offset));
600  lla_f.lon = RadOfDeg(XSENS_DATA_Position_lon(xsens_msg_buf,offset));
601  gps.lla_pos.lat = (int32_t)(DegOfRad(lla_f.lat) * 1e7);
602  gps.lla_pos.lon = (int32_t)(DegOfRad(lla_f.lon) * 1e7);
603  gps.utm_pos.zone = (DegOfRad(lla_f.lon)+180) / 6 + 1;
604  /* convert to utm */
606  /* copy results of utm conversion */
607  gps.utm_pos.east = utm_f.east*100;
608  gps.utm_pos.north = utm_f.north*100;
609  ins_x = utm_f.east;
610  ins_y = utm_f.north;
611  ins_z = XSENS_DATA_Position_alt(xsens_msg_buf,offset);//TODO is this hms or above ellipsoid?
612  gps.hmsl = ins_z * 1000;
613  // what about gps.lla_pos.alt and gps.utm_pos.alt ?
614 
615  gps_xsens_msg_available = TRUE;
616 #endif
617  offset += XSENS_DATA_Position_LENGTH;
618  }
619  if (XSENS_MASK_Velocity(xsens_output_mode)) {
620 #if (! USE_GPS_XSENS_RAW_DATA) && USE_GPS_XSENS
621  ins_vx = XSENS_DATA_Velocity_vx(xsens_msg_buf,offset);
622  ins_vy = XSENS_DATA_Velocity_vy(xsens_msg_buf,offset);
623  ins_vz = XSENS_DATA_Velocity_vz(xsens_msg_buf,offset);
624 #endif
625  offset += XSENS_DATA_Velocity_LENGTH;
626  }
627  if (XSENS_MASK_Status(xsens_output_mode)) {
628  xsens_msg_status = XSENS_DATA_Status_status(xsens_msg_buf,offset);
629 #if USE_GPS_XSENS
630  if (bit_is_set(xsens_msg_status,2)) gps.fix = GPS_FIX_3D; // gps fix
631  else if (bit_is_set(xsens_msg_status,1)) gps.fix = 0x01; // efk valid
632  else gps.fix = GPS_FIX_NONE;
633 #ifdef GPS_LED
634  if (gps.fix == GPS_FIX_3D) {
635  LED_ON(GPS_LED);
636  }
637  else {
638  LED_TOGGLE(GPS_LED);
639  }
640 #endif // GPS_LED
641 #endif // USE_GPS_XSENS
642  offset += XSENS_DATA_Status_LENGTH;
643  }
644  if (XSENS_MASK_TimeStamp(xsens_output_settings)) {
645  xsens_time_stamp = XSENS_DATA_TimeStamp_ts(xsens_msg_buf,offset);
646 #if USE_GPS_XSENS
648 #endif
649  offset += XSENS_DATA_TimeStamp_LENGTH;
650  }
651  if (XSENS_MASK_UTC(xsens_output_settings)) {
652  xsens_time.hour = XSENS_DATA_UTC_hour(xsens_msg_buf,offset);
653  xsens_time.min = XSENS_DATA_UTC_min(xsens_msg_buf,offset);
654  xsens_time.sec = XSENS_DATA_UTC_sec(xsens_msg_buf,offset);
655  xsens_time.nanosec = XSENS_DATA_UTC_nanosec(xsens_msg_buf,offset);
656  xsens_time.year = XSENS_DATA_UTC_year(xsens_msg_buf,offset);
657  xsens_time.month = XSENS_DATA_UTC_month(xsens_msg_buf,offset);
658  xsens_time.day = XSENS_DATA_UTC_day(xsens_msg_buf,offset);
659 
660  offset += XSENS_DATA_UTC_LENGTH;
661  }
662  //}
663  }
664 
665 }
666 
667 
669  ck += c;
670  switch (xsens_status) {
671  case UNINIT:
672  if (c != XSENS_START)
673  goto error;
674  xsens_status++;
675  ck = 0;
676  break;
677  case GOT_START:
678  if (c != XSENS_BID)
679  goto error;
680  xsens_status++;
681  break;
682  case GOT_BID:
683  xsens_id = c;
684  xsens_status++;
685  break;
686  case GOT_MID:
687  xsens_len = c;
689  goto error;
690  xsens_msg_idx = 0;
691  xsens_status++;
692  break;
693  case GOT_LEN:
695  xsens_msg_idx++;
696  if (xsens_msg_idx >= xsens_len)
697  xsens_status++;
698  break;
699  case GOT_DATA:
700  if (ck != 0)
701  goto error;
703  goto restart;
704  break;
705  default:
706  break;
707  }
708  return;
709  error:
710  restart:
712  return;
713 }
INS_FORMAT ins_my
Definition: ins_xsens.c:76
unsigned short uint16_t
Definition: types.h:16
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
Definition: gps.h:65
static void stateSetNedToBodyEulers_f(struct FloatEulers *ned_to_body_eulers)
Set vehicle body attitude from euler angles (float).
Definition: state.h:995
struct NedCoor_i ned_vel
speed NED in cm/s
Definition: gps.h:69
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
int32_t lat
in degrees*1e7
#define XSENS_OUTPUT_SETTINGS
Definition: ins_xsens.c:167
void xsens_init(void)
Definition: ins_xsens.c:215
INS_FORMAT ins_y
Definition: ins_xsens.c:51
int32_t course
GPS course over ground in rad*1e7, [0, 2*Pi]*1e7 (CW/north)
Definition: gps.h:72
#define Min(x, y)
float xsens_gps_arm_x
Definition: ins_xsens.c:192
#define UNINIT
Definition: ins_xsens.c:170
void ins_update_gps(void)
Update INS state with GPS measurements.
INS_FORMAT ins_psi
Definition: ins_xsens.c:62
uint32_t sacc
speed accuracy in cm/s
Definition: gps.h:74
uint8_t zone
UTM zone number.
angular rates
INS_FORMAT ins_p
Definition: ins_xsens.c:65
Library for the XSENS AHRS.
Definition: ins_xsens.h:34
#define ACCEL_BFP_OF_REAL(_af)
static uint8_t xsens_id
Definition: ins_xsens.c:203
uint8_t xsens_msg_buf[XSENS_MAX_PAYLOAD]
Definition: ins_xsens.c:114
static uint8_t xsens_status
Definition: ins_xsens.c:204
int8_t min
Definition: ins_xsens.h:36
#define INS_ROLL_NEUTRAL_DEFAULT
uint16_t pdop
position dilution of precision scaled by 100
Definition: gps.h:76
uint8_t xsens_msg_status
Definition: ins_xsens.c:185
int32_t x
North.
uint8_t nav_utm_zone0
Definition: common_nav.c:44
struct Int32Rates gyro_unscaled
unscaled gyroscope measurements
Definition: imu.h:49
#define LED_ON(i)
Definition: led_hw.h:28
Integrated Navigation System interface.
int32_t hmsl
height above mean sea level in mm
Definition: gps.h:67
float lat
in radians
void ins_init(void)
INS initialization.
Definition: ins_alt_float.c:76
INS_FORMAT ins_x
Definition: ins_xsens.c:50
INS_FORMAT ins_vz
Definition: ins_xsens.c:57
struct ImuXsens imu_xsens
Definition: ins_xsens.c:232
void gps_impl_init(void)
Definition: gps_ardrone2.c:38
uint8_t fix
status of fix
Definition: gps.h:78
INS_FORMAT ins_vy
Definition: ins_xsens.c:56
#define GPS_FIX_3D
Definition: gps.h:43
euler angles
volatile int xsens_configured
Definition: ins_xsens.c:210
#define GPS_NB_CHANNELS
Definition: gps_ardrone2.h:33
uint32_t xsens_output_settings
Definition: ins_xsens.c:188
#define GOT_BID
Definition: ins_xsens.c:172
int16_t week
GPS week.
Definition: gps.h:79
uint8_t qi
quality bitfield (GPS receiver specific)
Definition: gps.h:56
volatile uint8_t ins_msg_received
Definition: ins_xsens.c:90
uint8_t xsens_errorcode
Definition: ins_xsens.c:184
vector in Latitude, Longitude and Altitude
#define FALSE
Definition: imu_chimu.h:141
#define GOT_MID
Definition: ins_xsens.c:173
int32_t alt
in millimeters above WGS84 reference ellipsoid
void ins_periodic(void)
INS periodic call.
Definition: ins_ardrone2.c:71
static void stateSetPositionUtm_f(struct UtmCoor_f *utm_pos)
Set position from UTM coordinates (float).
Definition: state.h:555
Paparazzi floating point math for geodetic calculations.
int32_t nanosec
Definition: ins_xsens.h:38
uint32_t tow
GPS time of week in ms.
Definition: gps.h:80
int32_t z
Down.
static void stateSetSpeedNed_f(struct NedCoor_f *ned_speed)
Set ground speed in local NED coordinates (float).
Definition: state.h:754
uint8_t zone
UTM zone number.
struct Int32Vect3 accel_unscaled
unscaled accelerometer measurements
Definition: imu.h:50
void parse_ins_buffer(uint8_t c)
Definition: ins_xsens.c:668
struct LlaCoor_f lla_f
Definition: ins_xsens700.c:159
#define MAG_BFP_OF_REAL(_af)
int32_t nav_utm_north0
Definition: common_nav.c:43
uint8_t send_ck
Definition: ins_xsens.c:208
int16_t gspeed
norm of 2d ground speed in cm/s
Definition: gps.h:70
vector in North East Down coordinates Units: meters
Architecture independent timing functions.
bool_t mag_available
Definition: ins_xsens.h:59
struct XsensTime xsens_time
Definition: ins_xsens.c:201
bool_t gyro_available
Definition: ins_xsens.h:57
float north
in meters
#define GPS_FIX_NONE
Definition: gps.h:41
Device independent GPS code (interface)
INS_FORMAT ins_theta
Definition: ins_xsens.c:61
struct Imu imu
global IMU state
Definition: imu_aspirin2.c:47
int8_t day
Definition: ins_xsens.h:41
position in UTM coordinates Units: meters
unsigned long uint32_t
Definition: types.h:18
#define INS_PITCH_NEUTRAL_DEFAULT
int32_t alt
in millimeters above WGS84 reference ellipsoid
void parse_ins_msg(void)
Definition: ins_xsens.c:425
INS_FORMAT ins_mx
Definition: ins_xsens.c:75
bool_t accel_available
Definition: ins_xsens.h:58
int8_t hour
Definition: ins_xsens.h:35
int32_t y
East.
uint16_t xsens_time_stamp
Definition: ins_xsens.c:186
int32_t north
in centimeters
#define LED_TOGGLE(i)
Definition: led_hw.h:30
struct Int32Vect3 mag_unscaled
unscaled magnetometer measurements
Definition: imu.h:51
struct UtmCoor_f utm_f
Definition: ins_xsens700.c:160
float ins_pitch_neutral
Definition: ins_arduimu.c:15
void handle_ins_msg(void)
Definition: ins_xsens.c:380
struct SVinfo svinfos[GPS_NB_CHANNELS]
holds information from the Space Vehicles (Satellites)
Definition: gps.h:83
#define GOT_START
Definition: ins_xsens.c:171
signed long int32_t
Definition: types.h:19
#define INS_FORMAT
Definition: ins_module.h:35
#define TRUE
Definition: imu_chimu.h:144
int8_t sec
Definition: ins_xsens.h:37
#define VECT3_ASSIGN(_a, _x, _y, _z)
Definition: pprz_algebra.h:107
int16_t speed_3d
norm of 3d speed in cm/s
Definition: gps.h:71
INS_FORMAT ins_vx
Definition: ins_xsens.c:55
int32_t east
in centimeters
int8_t month
Definition: ins_xsens.h:40
INS_FORMAT ins_q
Definition: ins_xsens.c:66
uint32_t pacc
position accuracy in cm
Definition: gps.h:73
uint8_t flags
bitfield with GPS receiver specific flags
Definition: gps.h:55
unsigned char uint8_t
Definition: types.h:14
INS_FORMAT ins_phi
Definition: ins_xsens.c:60
API to get/set the generic vehicle states.
volatile uint8_t new_ins_attitude
INS_FORMAT ins_mz
Definition: ins_xsens.c:77
#define WGS84_ELLIPSOID_TO_GEOID(_Lat, _Lon, _diff)
int32_t lon
in degrees*1e7
uint8_t svid
Satellite ID.
Definition: gps.h:54
#define RATES_ASSIGN(_ra, _p, _q, _r)
Definition: pprz_algebra.h:319
#define GOT_LEN
Definition: ins_xsens.c:174
int32_t nav_utm_east0
Definition: common_nav.c:42
#define LLA_FLOAT_OF_BFP(_o, _i)
float xsens_gps_arm_y
Definition: ins_xsens.c:193
static uint8_t ck
Definition: ins_xsens.c:207
uint16_t xsens_output_mode
Definition: ins_xsens.c:187
float xsens_gps_arm_z
Definition: ins_xsens.c:194
void imu_periodic(void)
Definition: ins_xsens.c:241
struct UtmCoor_i utm_pos
position in UTM (north,east: cm; alt: mm over ellipsoid)
Definition: gps.h:66
#define RATE_BFP_OF_REAL(_af)
INS_FORMAT ins_z
Definition: ins_xsens.c:52
float ins_roll_neutral
Definition: ins_arduimu.c:14
INS_FORMAT ins_ay
Definition: ins_xsens.c:71
float xsens_declination
Definition: ins_xsens.c:191
void imu_impl_init(void)
Definition: ins_xsens.c:234
float lon
in radians
INS_FORMAT ins_ax
Definition: ins_xsens.c:70
INS_FORMAT ins_r
Definition: ins_xsens.c:67
void xsens_periodic(void)
Definition: ins_xsens.c:286
static uint8_t xsens_len
Definition: ins_xsens.c:205
static void stateSetBodyRates_f(struct FloatRates *body_rate)
Set vehicle body angular rate (float).
Definition: state.h:1062
static uint8_t xsens_msg_idx
Definition: ins_xsens.c:206
float east
in meters
#define XSENS_OUTPUT_MODE
Definition: ins_xsens.c:138
#define GOT_DATA
Definition: ins_xsens.c:175
int16_t year
Definition: ins_xsens.h:39
static void stateSetLocalUtmOrigin_f(struct UtmCoor_f *utm_def)
Set the local (flat earth) coordinate frame origin from UTM (float).
Definition: state.h:459
struct GpsState gps
global GPS state
Definition: gps.c:41
Device independent INS code.
INS_FORMAT ins_az
Definition: ins_xsens.c:72
uint8_t cno
Carrier to Noise Ratio (Signal Strength) in dbHz.
Definition: gps.h:57
#define XSENS_MAX_PAYLOAD
Includes macros generated from xsens_MTi-G.xml.
Definition: ins_xsens.c:113
void utm_of_lla_f(struct UtmCoor_f *utm, struct LlaCoor_f *lla)
uint8_t nb_channels
Number of scanned satellites.
Definition: gps.h:82
uint8_t num_sv
number of sat in fix
Definition: gps.h:77