Paparazzi UAS  v5.0.5_stable-7-g4b8bbb7
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 #ifndef DOWNLINK_DEVICE
180 #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
181 #endif
182 #include "mcu_periph/uart.h"
183 #include "messages.h"
185 
186 
192 
193 
195 float xsens_gps_arm_x = 0;
196 float xsens_gps_arm_y = 0;
197 float xsens_gps_arm_z = 0;
198 
199 #if USE_GPS_XSENS
200 struct LlaCoor_f lla_f;
201 struct UtmCoor_f utm_f;
202 #endif
203 
205 
210 static uint8_t ck;
212 
213 volatile int xsens_configured = 0;
214 
215 void xsens_init(void);
216 void xsens_periodic(void);
217 
218 void xsens_init(void) {
219 
221  xsens_configured = 20;
222 
223 #if USE_INS_MODULE
226 #endif
227 
228  xsens_msg_status = 0;
229  xsens_time_stamp = 0;
232 }
233 
234 #if USE_IMU
236 
237 void imu_impl_init(void) {
238  xsens_init();
242 }
243 
244 void imu_periodic(void) {
245  xsens_periodic();
246 }
247 #endif /* USE_IMU */
248 
249 #if USE_INS_MODULE
250 void ins_init(void) {
251  struct UtmCoor_f utm0 = { nav_utm_north0, nav_utm_east0, 0., nav_utm_zone0 };
253  stateSetPositionUtm_f(&utm0);
254 
255  xsens_init();
256 }
257 
258 void ins_periodic(void) {
259  xsens_periodic();
260 }
261 
262 void ins_update_gps(void) {
263  struct UtmCoor_f utm;
264  utm.east = gps.utm_pos.east / 100.;
265  utm.north = gps.utm_pos.north / 100.;
266  utm.zone = nav_utm_zone0;
267  utm.alt = gps.hmsl / 1000.;
268 
269  // set position
270  stateSetPositionUtm_f(&utm);
271 
272  struct NedCoor_f ned_vel = {
273  gps.ned_vel.x / 100.,
274  gps.ned_vel.y / 100.,
275  gps.ned_vel.z / 100.
276  };
277  // set velocity
278  stateSetSpeedNed_f(&ned_vel);
279 }
280 #endif
281 
282 #if USE_GPS_XSENS
283 void gps_impl_init(void) {
284  gps.nb_channels = 0;
285  gps_xsens_msg_available = FALSE;
286 }
287 #endif
288 
289 void xsens_periodic(void) {
290  if (xsens_configured > 0)
291  {
292  switch (xsens_configured)
293  {
294  case 20:
295  /* send mode and settings to MT */
296  XSENS_GoToConfig();
297  XSENS_SetOutputMode(xsens_output_mode);
298  XSENS_SetOutputSettings(xsens_output_settings);
299  break;
300  case 18:
301  // Give pulses on SyncOut
302  XSENS_SetSyncOutSettings(0,0x0002);
303  break;
304  case 17:
305  // 1 pulse every 100 samples
306  XSENS_SetSyncOutSettings(1,100);
307  break;
308  case 2:
309  XSENS_ReqLeverArmGps();
310  break;
311 
312  case 6:
313  XSENS_ReqMagneticDeclination();
314  break;
315 
316  case 13:
317 #ifdef AHRS_H_X
318 #pragma message "Sending XSens Magnetic Declination."
320  XSENS_SetMagneticDeclination(xsens_declination);
321 #endif
322  break;
323  case 12:
324 #ifdef GPS_IMU_LEVER_ARM_X
325 #pragma message "Sending XSens GPS Arm."
326  XSENS_SetLeverArmGps(GPS_IMU_LEVER_ARM_X,GPS_IMU_LEVER_ARM_Y,GPS_IMU_LEVER_ARM_Z);
327 #endif
328  break;
329  case 10:
330  {
331  uint8_t baud = 1;
332  XSENS_SetBaudrate(baud);
333  }
334  break;
335 
336  case 1:
337  XSENS_GoToMeasurment();
338  break;
339 
340  default:
341  break;
342  }
344  return;
345  }
346 
347  RunOnceEvery(100,XSENS_ReqGPSStatus());
348 }
349 
350 #if USE_INS_MODULE
351 #include "state.h"
352 
353 static inline void update_fw_estimator(void) {
354  // Send to Estimator (Control)
355 #ifdef XSENS_BACKWARDS
356  struct FloatEulers att = {
359  ins_psi + RadOfDeg(180)
360  };
361  struct FloatRates rates = {
362  -ins_p,
363  -ins_q,
364  ins_r
365  };
366 #else
367  struct FloatEulers att = {
370  ins_psi
371  };
372  struct FloatRates rates = {
373  ins_p,
374  ins_q,
375  ins_r
376  };
377 #endif
379  stateSetBodyRates_f(&rates);
380 }
381 #endif /* USE_INS_MODULE */
382 
383 void handle_ins_msg(void) {
384 
385 #if USE_INS_MODULE
386  update_fw_estimator();
387 #endif
388 
389 #if USE_IMU
390 #ifdef XSENS_BACKWARDS
393  }
396  }
397  if (imu_xsens.mag_available) {
399  }
400 #else
403  }
406  }
407  if (imu_xsens.mag_available) {
409  }
410 #endif /* XSENS_BACKWARDS */
411 #endif /* USE_IMU */
412 
413 #if USE_GPS_XSENS
414 
415  // Horizontal speed
416  float fspeed = sqrt(ins_vx*ins_vx + ins_vy*ins_vy);
417  if (gps.fix != GPS_FIX_3D) {
418  fspeed = 0;
419  }
420  gps.gspeed = fspeed * 100.;
421  gps.speed_3d = (uint16_t)(sqrt(ins_vx*ins_vx + ins_vy*ins_vy + ins_vz*ins_vz) * 100);
422 
423  float fcourse = atan2f((float)ins_vy, (float)ins_vx);
424  gps.course = fcourse * 1e7;
425 #endif // USE_GPS_XSENS
426 }
427 
428 void parse_ins_msg(void) {
429  uint8_t offset = 0;
430  if (xsens_id == XSENS_ReqOutputModeAck_ID) {
431  xsens_output_mode = XSENS_ReqOutputModeAck_mode(xsens_msg_buf);
432  }
433  else if (xsens_id == XSENS_ReqOutputSettings_ID) {
434  xsens_output_settings = XSENS_ReqOutputSettingsAck_settings(xsens_msg_buf);
435  }
436  else if (xsens_id == XSENS_ReqMagneticDeclinationAck_ID) {
437  xsens_declination = DegOfRad (XSENS_ReqMagneticDeclinationAck_declination(xsens_msg_buf) );
438 
440  }
441  else if (xsens_id == XSENS_ReqLeverArmGpsAck_ID) {
442  xsens_gps_arm_x = XSENS_ReqLeverArmGpsAck_x(xsens_msg_buf);
443  xsens_gps_arm_y = XSENS_ReqLeverArmGpsAck_y(xsens_msg_buf);
444  xsens_gps_arm_z = XSENS_ReqLeverArmGpsAck_z(xsens_msg_buf);
445 
447  }
448  else if (xsens_id == XSENS_Error_ID) {
449  xsens_errorcode = XSENS_Error_errorcode(xsens_msg_buf);
450  }
451 #if USE_GPS_XSENS
452  else if (xsens_id == XSENS_GPSStatus_ID) {
453  gps.nb_channels = XSENS_GPSStatus_nch(xsens_msg_buf);
454  gps.num_sv = 0;
455 
456  uint8_t i;
457  // Do not write outside buffer
458  for(i = 0; i < Min(gps.nb_channels, GPS_NB_CHANNELS); i++) {
459  uint8_t ch = XSENS_GPSStatus_chn(xsens_msg_buf,i);
460  if (ch > gps.nb_channels) continue;
461  gps.svinfos[ch].svid = XSENS_GPSStatus_svid(xsens_msg_buf, i);
462  gps.svinfos[ch].flags = XSENS_GPSStatus_bitmask(xsens_msg_buf, i);
463  gps.svinfos[ch].qi = XSENS_GPSStatus_qi(xsens_msg_buf, i);
464  gps.svinfos[ch].cno = XSENS_GPSStatus_cnr(xsens_msg_buf, i);
465  if (gps.svinfos[ch].flags > 0)
466  {
467  gps.num_sv++;
468  }
469  }
470  }
471 #endif
472  else if (xsens_id == XSENS_MTData_ID) {
473  /* test RAW modes else calibrated modes */
474  //if ((XSENS_MASK_RAWInertial(xsens_output_mode)) || (XSENS_MASK_RAWGPS(xsens_output_mode))) {
475  if (XSENS_MASK_RAWInertial(xsens_output_mode)) {
476  ins_p = XSENS_DATA_RAWInertial_gyrX(xsens_msg_buf,offset);
477  ins_q = XSENS_DATA_RAWInertial_gyrY(xsens_msg_buf,offset);
478  ins_r = XSENS_DATA_RAWInertial_gyrZ(xsens_msg_buf,offset);
479 #if USE_IMU
481 #endif
482  offset += XSENS_DATA_RAWInertial_LENGTH;
483  }
484  if (XSENS_MASK_RAWGPS(xsens_output_mode)) {
485 #if USE_GPS_XSENS_RAW_DATA && USE_GPS_XSENS
486 
487  gps.week = 0; // FIXME
488  gps.tow = XSENS_DATA_RAWGPS_itow(xsens_msg_buf,offset) * 10;
489  gps.lla_pos.lat = RadOfDeg(XSENS_DATA_RAWGPS_lat(xsens_msg_buf,offset));
490  gps.lla_pos.lon = RadOfDeg(XSENS_DATA_RAWGPS_lon(xsens_msg_buf,offset));
491  gps.lla_pos.alt = XSENS_DATA_RAWGPS_alt(xsens_msg_buf,offset);
492 
493  /* Set the real UTM zone */
494  gps.utm_pos.zone = (DegOfRad(gps.lla_pos.lon/1e7)+180) / 6 + 1;
495 
496  lla_f.lat = ((float) gps.lla_pos.lat) / 1e7;
497  lla_f.lon = ((float) gps.lla_pos.lon) / 1e7;
499  /* convert to utm */
501  /* copy results of utm conversion */
502  gps.utm_pos.east = utm_f.east*100;
503  gps.utm_pos.north = utm_f.north*100;
505 
506  ins_x = utm_f.east;
507  ins_y = utm_f.north;
508  // Altitude: Xsens LLH gives ellipsoid height
509  ins_z = -(INS_FORMAT)XSENS_DATA_RAWGPS_alt(xsens_msg_buf,offset) / 1000.;
510 
511  // Compute geoid (MSL) height
512  float hmsl;
514  gps.hmsl = XSENS_DATA_RAWGPS_alt(xsens_msg_buf,offset) - (hmsl * 1000.0f);
515 
516  ins_vx = ((INS_FORMAT)XSENS_DATA_RAWGPS_vel_n(xsens_msg_buf,offset)) / 100.;
517  ins_vy = ((INS_FORMAT)XSENS_DATA_RAWGPS_vel_e(xsens_msg_buf,offset)) / 100.;
518  ins_vz = ((INS_FORMAT)XSENS_DATA_RAWGPS_vel_d(xsens_msg_buf,offset)) / 100.;
519  gps.ned_vel.x = XSENS_DATA_RAWGPS_vel_n(xsens_msg_buf,offset);
520  gps.ned_vel.y = XSENS_DATA_RAWGPS_vel_e(xsens_msg_buf,offset);
521  gps.ned_vel.z = XSENS_DATA_RAWGPS_vel_d(xsens_msg_buf,offset);
522  gps.pacc = XSENS_DATA_RAWGPS_hacc(xsens_msg_buf,offset) / 100;
523  gps.sacc = XSENS_DATA_RAWGPS_sacc(xsens_msg_buf,offset) / 100;
524  gps.pdop = 5; // FIXME Not output by XSens
525 
526  gps_xsens_msg_available = TRUE;
527 #endif
528  offset += XSENS_DATA_RAWGPS_LENGTH;
529  }
530  //} else {
531  if (XSENS_MASK_Temp(xsens_output_mode)) {
532  offset += XSENS_DATA_Temp_LENGTH;
533  }
534  if (XSENS_MASK_Calibrated(xsens_output_mode)) {
535  uint8_t l = 0;
536  if (!XSENS_MASK_AccOut(xsens_output_settings)) {
537  ins_ax = XSENS_DATA_Calibrated_accX(xsens_msg_buf,offset);
538  ins_ay = XSENS_DATA_Calibrated_accY(xsens_msg_buf,offset);
539  ins_az = XSENS_DATA_Calibrated_accZ(xsens_msg_buf,offset);
540 #if USE_IMU
542 #endif
543  l++;
544  }
545  if (!XSENS_MASK_GyrOut(xsens_output_settings)) {
546  ins_p = XSENS_DATA_Calibrated_gyrX(xsens_msg_buf,offset);
547  ins_q = XSENS_DATA_Calibrated_gyrY(xsens_msg_buf,offset);
548  ins_r = XSENS_DATA_Calibrated_gyrZ(xsens_msg_buf,offset);
549 #if USE_IMU
551 #endif
552  l++;
553  }
554  if (!XSENS_MASK_MagOut(xsens_output_settings)) {
555  ins_mx = XSENS_DATA_Calibrated_magX(xsens_msg_buf,offset);
556  ins_my = XSENS_DATA_Calibrated_magY(xsens_msg_buf,offset);
557  ins_mz = XSENS_DATA_Calibrated_magZ(xsens_msg_buf,offset);
558 #if USE_IMU
560 #endif
561  l++;
562  }
563  offset += l * XSENS_DATA_Calibrated_LENGTH / 3;
564  }
565  if (XSENS_MASK_Orientation(xsens_output_mode)) {
566  if (XSENS_MASK_OrientationMode(xsens_output_settings) == 0x00) {
567  float q0 = XSENS_DATA_Quaternion_q0(xsens_msg_buf,offset);
568  float q1 = XSENS_DATA_Quaternion_q1(xsens_msg_buf,offset);
569  float q2 = XSENS_DATA_Quaternion_q2(xsens_msg_buf,offset);
570  float q3 = XSENS_DATA_Quaternion_q3(xsens_msg_buf,offset);
571  float dcm00 = 1.0 - 2 * (q2*q2 + q3*q3);
572  float dcm01 = 2 * (q1*q2 + q0*q3);
573  float dcm02 = 2 * (q1*q3 - q0*q2);
574  float dcm12 = 2 * (q2*q3 + q0*q1);
575  float dcm22 = 1.0 - 2 * (q1*q1 + q2*q2);
576  ins_phi = atan2(dcm12, dcm22);
577  ins_theta = -asin(dcm02);
578  ins_psi = atan2(dcm01, dcm00);
579  offset += XSENS_DATA_Quaternion_LENGTH;
580  }
581  if (XSENS_MASK_OrientationMode(xsens_output_settings) == 0x01) {
582  ins_phi = XSENS_DATA_Euler_roll(xsens_msg_buf,offset) * M_PI / 180;
583  ins_theta = XSENS_DATA_Euler_pitch(xsens_msg_buf,offset) * M_PI / 180;
584  ins_psi = XSENS_DATA_Euler_yaw(xsens_msg_buf,offset) * M_PI / 180;
585  offset += XSENS_DATA_Euler_LENGTH;
586  }
587  if (XSENS_MASK_OrientationMode(xsens_output_settings) == 0x10) {
588  offset += XSENS_DATA_Matrix_LENGTH;
589  }
590  new_ins_attitude = 1;
591  }
592  if (XSENS_MASK_Auxiliary(xsens_output_mode)) {
593  uint8_t l = 0;
594  if (!XSENS_MASK_Aux1Out(xsens_output_settings)) {
595  l++;
596  }
597  if (!XSENS_MASK_Aux2Out(xsens_output_settings)) {
598  l++;
599  }
600  offset += l * XSENS_DATA_Auxiliary_LENGTH / 2;
601  }
602  if (XSENS_MASK_Position(xsens_output_mode)) {
603 #if (! USE_GPS_XSENS_RAW_DATA) && USE_GPS_XSENS
604  lla_f.lat = RadOfDeg(XSENS_DATA_Position_lat(xsens_msg_buf,offset));
605  lla_f.lon = RadOfDeg(XSENS_DATA_Position_lon(xsens_msg_buf,offset));
606  gps.lla_pos.lat = (int32_t)(lla_f.lat * 1e7);
607  gps.lla_pos.lon = (int32_t)(lla_f.lon * 1e7);
608  gps.utm_pos.zone = (DegOfRad(lla_f.lon)+180) / 6 + 1;
609  /* convert to utm */
611  /* copy results of utm conversion */
612  gps.utm_pos.east = utm_f.east*100;
613  gps.utm_pos.north = utm_f.north*100;
614  ins_x = utm_f.east;
615  ins_y = utm_f.north;
616  ins_z = XSENS_DATA_Position_alt(xsens_msg_buf,offset);//TODO is this hms or above ellipsoid?
617  gps.hmsl = ins_z * 1000;
618  // what about gps.lla_pos.alt and gps.utm_pos.alt ?
619 
620  gps_xsens_msg_available = TRUE;
621 #endif
622  offset += XSENS_DATA_Position_LENGTH;
623  }
624  if (XSENS_MASK_Velocity(xsens_output_mode)) {
625 #if (! USE_GPS_XSENS_RAW_DATA) && USE_GPS_XSENS
626  ins_vx = XSENS_DATA_Velocity_vx(xsens_msg_buf,offset);
627  ins_vy = XSENS_DATA_Velocity_vy(xsens_msg_buf,offset);
628  ins_vz = XSENS_DATA_Velocity_vz(xsens_msg_buf,offset);
629 #endif
630  offset += XSENS_DATA_Velocity_LENGTH;
631  }
632  if (XSENS_MASK_Status(xsens_output_mode)) {
633  xsens_msg_status = XSENS_DATA_Status_status(xsens_msg_buf,offset);
634 #if USE_GPS_XSENS
635  if (bit_is_set(xsens_msg_status,2)) gps.fix = GPS_FIX_3D; // gps fix
636  else if (bit_is_set(xsens_msg_status,1)) gps.fix = 0x01; // efk valid
637  else gps.fix = GPS_FIX_NONE;
638 #ifdef GPS_LED
639  if (gps.fix == GPS_FIX_3D) {
640  LED_ON(GPS_LED);
641  }
642  else {
643  LED_TOGGLE(GPS_LED);
644  }
645 #endif // GPS_LED
646 #endif // USE_GPS_XSENS
647  offset += XSENS_DATA_Status_LENGTH;
648  }
649  if (XSENS_MASK_TimeStamp(xsens_output_settings)) {
650  xsens_time_stamp = XSENS_DATA_TimeStamp_ts(xsens_msg_buf,offset);
651 #if USE_GPS_XSENS
653 #endif
654  offset += XSENS_DATA_TimeStamp_LENGTH;
655  }
656  if (XSENS_MASK_UTC(xsens_output_settings)) {
657  xsens_time.hour = XSENS_DATA_UTC_hour(xsens_msg_buf,offset);
658  xsens_time.min = XSENS_DATA_UTC_min(xsens_msg_buf,offset);
659  xsens_time.sec = XSENS_DATA_UTC_sec(xsens_msg_buf,offset);
660  xsens_time.nanosec = XSENS_DATA_UTC_nanosec(xsens_msg_buf,offset);
661  xsens_time.year = XSENS_DATA_UTC_year(xsens_msg_buf,offset);
662  xsens_time.month = XSENS_DATA_UTC_month(xsens_msg_buf,offset);
663  xsens_time.day = XSENS_DATA_UTC_day(xsens_msg_buf,offset);
664 
665  offset += XSENS_DATA_UTC_LENGTH;
666  }
667  //}
668  }
669 
670 }
671 
672 
674  ck += c;
675  switch (xsens_status) {
676  case UNINIT:
677  if (c != XSENS_START)
678  goto error;
679  xsens_status++;
680  ck = 0;
681  break;
682  case GOT_START:
683  if (c != XSENS_BID)
684  goto error;
685  xsens_status++;
686  break;
687  case GOT_BID:
688  xsens_id = c;
689  xsens_status++;
690  break;
691  case GOT_MID:
692  xsens_len = c;
694  goto error;
695  xsens_msg_idx = 0;
696  xsens_status++;
697  break;
698  case GOT_LEN:
700  xsens_msg_idx++;
701  if (xsens_msg_idx >= xsens_len)
702  xsens_status++;
703  break;
704  case GOT_DATA:
705  if (ck != 0)
706  goto error;
708  goto restart;
709  break;
710  default:
711  break;
712  }
713  return;
714  error:
715  restart:
717  return;
718 }
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: rad*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 radians*1e7
#define XSENS_OUTPUT_SETTINGS
Definition: ins_xsens.c:167
void xsens_init(void)
Definition: ins_xsens.c:218
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:195
#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:206
uint8_t xsens_msg_buf[XSENS_MAX_PAYLOAD]
Definition: ins_xsens.c:114
static uint8_t xsens_status
Definition: ins_xsens.c:207
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:188
int32_t x
North.
uint8_t nav_utm_zone0
Definition: common_nav.c:44
struct Int32Rates gyro_unscaled
unscaled gyroscope measurements
Definition: imu.h:48
#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
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:235
void gps_impl_init(void)
Definition: gps_ardrone2.c:34
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:213
#define GPS_NB_CHANNELS
Definition: gps_ardrone2.h:33
uint32_t xsens_output_settings
Definition: ins_xsens.c:191
#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:187
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
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:49
void parse_ins_buffer(uint8_t c)
Definition: ins_xsens.c:673
struct LlaCoor_f lla_f
Definition: ins_xsens700.c:173
#define MAG_BFP_OF_REAL(_af)
int32_t nav_utm_north0
Definition: common_nav.c:43
uint8_t send_ck
Definition: ins_xsens.c:211
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:204
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:50
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:428
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:189
int32_t north
in centimeters
#define LED_TOGGLE(i)
Definition: led_hw.h:30
struct Int32Vect3 mag_unscaled
unscaled magnetometer measurements
Definition: imu.h:50
struct UtmCoor_f utm_f
Definition: ins_xsens700.c:174
float ins_pitch_neutral
Definition: ins_arduimu.c:15
void handle_ins_msg(void)
Definition: ins_xsens.c:383
struct SVinfo svinfos[GPS_NB_CHANNELS]
holds information from the Space Vehicles (Satellites)
Definition: gps.h:83
#define AHRS_H_Y
#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:97
int16_t speed_3d
norm of 3d speed in cm/s
Definition: gps.h:71
INS_FORMAT ins_vx
Definition: ins_xsens.c:55
#define AHRS_H_X
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 radians*1e7
uint8_t svid
Satellite ID.
Definition: gps.h:54
#define RATES_ASSIGN(_ra, _p, _q, _r)
Definition: pprz_algebra.h:294
#define GOT_LEN
Definition: ins_xsens.c:174
int32_t nav_utm_east0
Definition: common_nav.c:42
float xsens_gps_arm_y
Definition: ins_xsens.c:196
static uint8_t ck
Definition: ins_xsens.c:210
uint16_t xsens_output_mode
Definition: ins_xsens.c:190
float xsens_gps_arm_z
Definition: ins_xsens.c:197
void ins_init(void)
INS initialization.
Definition: ins_xsens700.c:178
void imu_periodic(void)
Definition: ins_xsens.c:244
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:194
void imu_impl_init(void)
Definition: ins_xsens.c:237
float lon
in radians
void ins_periodic(void)
INS periodic call.
Definition: ins_alt_float.c:75
static struct point c
Definition: discsurvey.c:39
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:289
static uint8_t xsens_len
Definition: ins_xsens.c:208
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:209
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:33
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