Paparazzi UAS  v4.0.4_stable-3-gf39211a
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
ins_xsens.c
Go to the documentation of this file.
1 /*
2  * Paparazzi mcu0 $Id$
3  *
4  * Copyright (C) 2003 Pascal Brisset, Antoine Drouin
5  *
6  * This file is part of paparazzi.
7  *
8  * paparazzi is free software; you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation; either version 2, or (at your option)
11  * any later version.
12  *
13  * paparazzi is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  * GNU General Public License for more details.
17  *
18  * You should have received a copy of the GNU General Public License
19  * along with paparazzi; see the file COPYING. If not, write to
20  * the Free Software Foundation, 59 Temple Place - Suite 330,
21  * Boston, MA 02111-1307, USA.
22  *
23  */
24 
29 #include "ins_module.h"
30 #include "ins_xsens.h"
31 
32 #include <inttypes.h>
33 
34 #include "generated/airframe.h"
35 
36 #include "mcu_periph/sys_time.h"
38 #include "messages.h"
39 
40 #if USE_GPS_XSENS
41 #include "subsystems/gps.h"
44 #include "subsystems/navigation/common_nav.h" /* needed for nav_utm_zone0 */
45 #endif
46 
50 
54 
58 
62 
66 
70 
73 
74 
75 void ahrs_init(void)
76 {
77  ins_init();
78 }
79 
80 
81 #ifdef USE_IMU
82 
83 #include "subsystems/imu.h"
84 
85 void imu_init(void)
86 {
87  ins_init();
88 }
89 
90 void imu_periodic(void)
91 {
93 }
94 
95 #endif
96 
98 //
99 // XSens Specific
100 //
101 
103 
104 #define XsensInitCheksum() { send_ck = 0; }
105 #define XsensUpdateChecksum(c) { send_ck += c; }
106 
107 #define XsensSend1(c) { uint8_t i8=c; InsUartSend1(i8); XsensUpdateChecksum(i8); }
108 #define XsensSend1ByAddr(x) { XsensSend1(*x); }
109 #define XsensSend2ByAddr(x) { XsensSend1(*(x+1)); XsensSend1(*x); }
110 #define XsensSend4ByAddr(x) { XsensSend1(*(x+3)); XsensSend1(*(x+2)); XsensSend1(*(x+1)); XsensSend1(*x); }
111 
112 #define XsensHeader(msg_id, len) { \
113  InsUartSend1(XSENS_START); \
114  XsensInitCheksum(); \
115  XsensSend1(XSENS_BID); \
116  XsensSend1(msg_id); \
117  XsensSend1(len); \
118 }
119 #define XsensTrailer() { uint8_t i8=0x100-send_ck; InsUartSend1(i8); }
120 
122 #include "xsens_protocol.h"
123 
124 
125 #define XSENS_MAX_PAYLOAD 254
127 
128 /* output mode : calibrated, orientation, position, velocity, status
129  * -----------
130  *
131  * bit 0 temp
132  * bit 1 calibrated
133  * bit 2 orentation
134  * bit 3 aux
135  *
136  * bit 4 position
137  * bit 5 velocity
138  * bit 6-7 Reserved
139  *
140  * bit 8-10 Reserved
141  * bit 11 status
142  *
143  * bit 12 GPS PVT+baro
144  * bit 13 Reserved
145  * bit 14 Raw
146  * bit 15 Reserved
147  */
148 
149 #ifndef XSENS_OUTPUT_MODE
150 #define XSENS_OUTPUT_MODE 0x1836
151 #endif
152 /* output settings : timestamp, euler, acc, rate, mag, float, no aux, lla, m/s, NED
153  * -----------
154  *
155  * bit 01 0=none, 1=sample counter, 2=utc, 3=sample+utc
156  * bit 23 0=quaternion, 1=euler, 2=DCM
157  *
158  * bit 4 1=disable acc output
159  * bit 5 1=disable gyro output
160  * bit 6 1=disable magneto output
161  * bit 7 Reserved
162  *
163  * bit 89 0=float, 1=fixedpoint12.20, 2=fixedpoint16.32
164  * bit 10 1=disable aux analog 1
165  * bit 11 1=disable aux analog 2
166  *
167  * bit 12-15 0-only: 14-16 WGS84
168  *
169  * bit 16-19 0-only: 16-18 m/s XYZ
170  *
171  * bit 20-23 Reserved
172  *
173  * bit 24-27 Reseverd
174  *
175  * bit 28-30 Reseverd
176  * bit 31 0=X-North-Z-Up, 1=North-East-Down
177  */
178 #ifndef XSENS_OUTPUT_SETTINGS
179 #define XSENS_OUTPUT_SETTINGS 0x80000C05
180 #endif
181 
182 #define UNINIT 0
183 #define GOT_START 1
184 #define GOT_BID 2
185 #define GOT_MID 3
186 #define GOT_LEN 4
187 #define GOT_DATA 5
188 #define GOT_CHECKSUM 6
189 
190 // FIXME Debugging Only
191 #ifndef DOWNLINK_DEVICE
192 #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
193 #endif
194 #include "mcu_periph/uart.h"
195 #include "messages.h"
197 
198 
205 float xsens_gps_arm_x = 0;
206 float xsens_gps_arm_y = 0;
207 float xsens_gps_arm_z = 0;
208 
209 
217 
222 static uint8_t ck;
224 
227 
228 volatile int xsens_configured = 0;
229 
230 void ins_init( void ) {
231 
233  xsens_configured = 20;
234 
235  ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
236  ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;
237 
238  xsens_msg_status = 0;
239  xsens_time_stamp = 0;
242 
243  gps.nb_channels = 0;
244 }
245 
246 void ins_periodic_task( void ) {
247  if (xsens_configured > 0)
248  {
249  switch (xsens_configured)
250  {
251  case 20:
252  /* send mode and settings to MT */
253  XSENS_GoToConfig();
254  XSENS_SetOutputMode(xsens_output_mode);
255  XSENS_SetOutputSettings(xsens_output_settings);
256  break;
257  case 18:
258  // Give pulses on SyncOut
259  XSENS_SetSyncOutSettings(0,0x0002);
260  break;
261  case 17:
262  // 1 pulse every 100 samples
263  XSENS_SetSyncOutSettings(1,100);
264  break;
265  case 2:
266  XSENS_ReqLeverArmGps();
267  break;
268 
269  case 6:
270  XSENS_ReqMagneticDeclination();
271  break;
272 
273  case 13:
274  #ifdef AHRS_H_X
275  #pragma message "Sending XSens Magnetic Declination."
277  XSENS_SetMagneticDeclination(xsens_declination);
278  #endif
279  break;
280  case 12:
281  #ifdef GPS_IMU_LEVER_ARM_X
282  #pragma message "Sending XSens GPS Arm."
283  XSENS_SetLeverArmGps(GPS_IMU_LEVER_ARM_X,GPS_IMU_LEVER_ARM_Y,GPS_IMU_LEVER_ARM_Z);
284  #endif
285  break;
286  case 10:
287  {
288  uint8_t baud = 1;
289  XSENS_SetBaudrate(baud);
290  }
291  break;
292 
293  case 1:
294  XSENS_GoToMeasurment();
295  break;
296  }
298  return;
299  }
300 
301  RunOnceEvery(100,XSENS_ReqGPSStatus());
302 }
303 
304 #include "estimator.h"
305 
306 void handle_ins_msg( void) {
307 
308 
309  // Send to Estimator (Control)
310 #ifdef XSENS_BACKWARDS
313 #else
316 #endif
317 
318  // Position
319  float gps_east = gps.utm_pos.east / 100.;
320  float gps_north = gps.utm_pos.north / 100.;
321  gps_east -= nav_utm_east0;
322  gps_north -= nav_utm_north0;
323  EstimatorSetPosXY(gps_east, gps_north);
324 
325  // Altitude and vertical speed
326  float hmsl = gps.hmsl;
327  hmsl /= 1000.0f;
328  EstimatorSetAlt(hmsl);
329 
330  #ifndef ALT_KALMAN
331  #warning NO_VZ
332  #endif
333 
334  // Horizontal speed
335  float fspeed = sqrt(ins_vx*ins_vx + ins_vy*ins_vy);
336  if (gps.fix != GPS_FIX_3D)
337  {
338  fspeed = 0;
339  }
340  float fclimb = -ins_vz;
341  float fcourse = atan2f((float)ins_vy, (float)ins_vx);
342  EstimatorSetSpeedPol(fspeed, fcourse, fclimb);
343 
344  // Now also finish filling the gps struct for telemetry purposes
345  gps.gspeed = fspeed * 100.;
346  gps.speed_3d = (uint16_t)(sqrt(ins_vx*ins_vx + ins_vy*ins_vy + ins_vz*ins_vz) * 100);
347  gps.course = fcourse * 1e7;
348 
349 }
350 
351 void parse_ins_msg( void ) {
352  uint8_t offset = 0;
353  if (xsens_id == XSENS_ReqOutputModeAck_ID) {
354  xsens_output_mode = XSENS_ReqOutputModeAck_mode(xsens_msg_buf);
355  }
356  else if (xsens_id == XSENS_ReqOutputSettings_ID) {
357  xsens_output_settings = XSENS_ReqOutputSettingsAck_settings(xsens_msg_buf);
358  }
359  else if (xsens_id == XSENS_ReqMagneticDeclinationAck_ID) {
360  xsens_declination = DegOfRad (XSENS_ReqMagneticDeclinationAck_declination(xsens_msg_buf) );
361 
363  }
364  else if (xsens_id == XSENS_ReqLeverArmGpsAck_ID) {
365  xsens_gps_arm_x = XSENS_ReqLeverArmGpsAck_x(xsens_msg_buf);
366  xsens_gps_arm_y = XSENS_ReqLeverArmGpsAck_y(xsens_msg_buf);
367  xsens_gps_arm_z = XSENS_ReqLeverArmGpsAck_z(xsens_msg_buf);
368 
370  }
371  else if (xsens_id == XSENS_Error_ID) {
372  xsens_errorcode = XSENS_Error_errorcode(xsens_msg_buf);
373  }
374 #if USE_GPS_XSENS
375  else if (xsens_id == XSENS_GPSStatus_ID) {
376  gps.nb_channels = XSENS_GPSStatus_nch(xsens_msg_buf);
377  gps.num_sv = 0;
378 
380 
381  uint8_t i;
382  // Do not write outside buffer
383  for(i = 0; i < Min(gps.nb_channels, GPS_NB_CHANNELS); i++) {
384  uint8_t ch = XSENS_GPSStatus_chn(xsens_msg_buf,i);
385  if (ch > gps.nb_channels) continue;
386  gps.svinfos[ch].svid = XSENS_GPSStatus_svid(xsens_msg_buf, i);
387  gps.svinfos[ch].flags = XSENS_GPSStatus_bitmask(xsens_msg_buf, i);
388  gps.svinfos[ch].qi = XSENS_GPSStatus_qi(xsens_msg_buf, i);
389  gps.svinfos[ch].cno = XSENS_GPSStatus_cnr(xsens_msg_buf, i);
390  if (gps.svinfos[ch].flags > 0)
391  {
392  gps.num_sv++;
393  }
394  }
395  }
396 #endif
397  else if (xsens_id == XSENS_MTData_ID) {
398  /* test RAW modes else calibrated modes */
399  //if ((XSENS_MASK_RAWInertial(xsens_output_mode)) || (XSENS_MASK_RAWGPS(xsens_output_mode))) {
400  if (XSENS_MASK_RAWInertial(xsens_output_mode)) {
401  ins_p = XSENS_DATA_RAWInertial_gyrX(xsens_msg_buf,offset);
402  ins_q = XSENS_DATA_RAWInertial_gyrY(xsens_msg_buf,offset);
403  ins_r = XSENS_DATA_RAWInertial_gyrZ(xsens_msg_buf,offset);
404  offset += XSENS_DATA_RAWInertial_LENGTH;
405  }
406  if (XSENS_MASK_RAWGPS(xsens_output_mode)) {
407 #if USE_GPS_XSENS_RAW_DATA && USE_GPS_XSENS
408 #ifdef GPS_LED
409  LED_TOGGLE(GPS_LED);
410 #endif
412  gps.week = 0; // FIXME
413  gps.tow = XSENS_DATA_RAWGPS_itow(xsens_msg_buf,offset) * 10;
414  gps.lla_pos.lat = RadOfDeg(XSENS_DATA_RAWGPS_lat(xsens_msg_buf,offset));
415  gps.lla_pos.lon = RadOfDeg(XSENS_DATA_RAWGPS_lon(xsens_msg_buf,offset));
416  gps.lla_pos.alt = XSENS_DATA_RAWGPS_alt(xsens_msg_buf,offset);
417 
418 
419  /* Set the real UTM zone */
420  gps.utm_pos.zone = (DegOfRad(gps.lla_pos.lon/1e7)+180) / 6 + 1;
421 
422  lla_f.lat = ((float) gps.lla_pos.lat) / 1e7;
423  lla_f.lon = ((float) gps.lla_pos.lon) / 1e7;
425  /* convert to utm */
427  /* copy results of utm conversion */
428  gps.utm_pos.east = utm_f.east*100;
429  gps.utm_pos.north = utm_f.north*100;
431 
432  ins_x = utm_f.east;
433  ins_y = utm_f.north;
434  // Altitude: Xsens LLH gives ellipsoid height
435  ins_z = -(INS_FORMAT)XSENS_DATA_RAWGPS_alt(xsens_msg_buf,offset) / 1000.;
436 
437  // Compute geoid (MSL) height
438  float hmsl;
440  gps.hmsl = XSENS_DATA_RAWGPS_alt(xsens_msg_buf,offset) - (hmsl * 1000.0f);
441 
442  ins_vx = ((INS_FORMAT)XSENS_DATA_RAWGPS_vel_n(xsens_msg_buf,offset)) / 100.;
443  ins_vy = ((INS_FORMAT)XSENS_DATA_RAWGPS_vel_e(xsens_msg_buf,offset)) / 100.;
444  ins_vz = ((INS_FORMAT)XSENS_DATA_RAWGPS_vel_d(xsens_msg_buf,offset)) / 100.;
445  gps.ned_vel.x = XSENS_DATA_RAWGPS_vel_n(xsens_msg_buf,offset);
446  gps.ned_vel.y = XSENS_DATA_RAWGPS_vel_e(xsens_msg_buf,offset);
447  gps.ned_vel.z = XSENS_DATA_RAWGPS_vel_d(xsens_msg_buf,offset);
448  gps.pacc = XSENS_DATA_RAWGPS_hacc(xsens_msg_buf,offset) / 100;
449  gps.sacc = XSENS_DATA_RAWGPS_sacc(xsens_msg_buf,offset) / 100;
450  gps.pdop = 5; // FIXME Not output by XSens
451 #endif
452  offset += XSENS_DATA_RAWGPS_LENGTH;
453  }
454  //} else {
455  if (XSENS_MASK_Temp(xsens_output_mode)) {
456  offset += XSENS_DATA_Temp_LENGTH;
457  }
458  if (XSENS_MASK_Calibrated(xsens_output_mode)) {
459  uint8_t l = 0;
460  if (!XSENS_MASK_AccOut(xsens_output_settings)) {
461  ins_ax = XSENS_DATA_Calibrated_accX(xsens_msg_buf,offset);
462  ins_ay = XSENS_DATA_Calibrated_accY(xsens_msg_buf,offset);
463  ins_az = XSENS_DATA_Calibrated_accZ(xsens_msg_buf,offset);
464  l++;
465  }
466  if (!XSENS_MASK_GyrOut(xsens_output_settings)) {
467  ins_p = XSENS_DATA_Calibrated_gyrX(xsens_msg_buf,offset);
468  ins_q = XSENS_DATA_Calibrated_gyrY(xsens_msg_buf,offset);
469  ins_r = XSENS_DATA_Calibrated_gyrZ(xsens_msg_buf,offset);
470  l++;
471  }
472  if (!XSENS_MASK_MagOut(xsens_output_settings)) {
473  ins_mx = XSENS_DATA_Calibrated_magX(xsens_msg_buf,offset);
474  ins_my = XSENS_DATA_Calibrated_magY(xsens_msg_buf,offset);
475  ins_mz = XSENS_DATA_Calibrated_magZ(xsens_msg_buf,offset);
476  l++;
477  }
478  offset += l * XSENS_DATA_Calibrated_LENGTH / 3;
479  }
480  if (XSENS_MASK_Orientation(xsens_output_mode)) {
481  if (XSENS_MASK_OrientationMode(xsens_output_settings) == 0x00) {
482  float q0 = XSENS_DATA_Quaternion_q0(xsens_msg_buf,offset);
483  float q1 = XSENS_DATA_Quaternion_q1(xsens_msg_buf,offset);
484  float q2 = XSENS_DATA_Quaternion_q2(xsens_msg_buf,offset);
485  float q3 = XSENS_DATA_Quaternion_q3(xsens_msg_buf,offset);
486  float dcm00 = 1.0 - 2 * (q2*q2 + q3*q3);
487  float dcm01 = 2 * (q1*q2 + q0*q3);
488  float dcm02 = 2 * (q1*q3 - q0*q2);
489  float dcm12 = 2 * (q2*q3 + q0*q1);
490  float dcm22 = 1.0 - 2 * (q1*q1 + q2*q2);
491  ins_phi = atan2(dcm12, dcm22);
492  ins_theta = -asin(dcm02);
493  ins_psi = atan2(dcm01, dcm00);
494  offset += XSENS_DATA_Quaternion_LENGTH;
495  }
496  if (XSENS_MASK_OrientationMode(xsens_output_settings) == 0x01) {
497  ins_phi = XSENS_DATA_Euler_roll(xsens_msg_buf,offset) * M_PI / 180;
498  ins_theta = XSENS_DATA_Euler_pitch(xsens_msg_buf,offset) * M_PI / 180;
499  ins_psi = XSENS_DATA_Euler_yaw(xsens_msg_buf,offset) * M_PI / 180;
500  offset += XSENS_DATA_Euler_LENGTH;
501  }
502  if (XSENS_MASK_OrientationMode(xsens_output_settings) == 0x10) {
503  offset += XSENS_DATA_Matrix_LENGTH;
504  }
505  new_ins_attitude = 1;
506  }
507  if (XSENS_MASK_Auxiliary(xsens_output_mode)) {
508  uint8_t l = 0;
509  if (!XSENS_MASK_Aux1Out(xsens_output_settings)) {
510  l++;
511  }
512  if (!XSENS_MASK_Aux2Out(xsens_output_settings)) {
513  l++;
514  }
515  offset += l * XSENS_DATA_Auxiliary_LENGTH / 2;
516  }
517  if (XSENS_MASK_Position(xsens_output_mode)) {
518 #if (! USE_GPS_XSENS_RAW_DATA) && USE_GPS_XSENS
520 
521  lla_f.lat = RadOfDeg(XSENS_DATA_Position_lat(xsens_msg_buf,offset));
522  lla_f.lon = RadOfDeg(XSENS_DATA_Position_lon(xsens_msg_buf,offset));
523  gps.lla_pos.lat = (int32_t)(lla_f.lat * 1e7);
524  gps.lla_pos.lon = (int32_t)(lla_f.lon * 1e7);
525  gps.utm_pos.zone = (DegOfRad(lla_f.lon)+180) / 6 + 1;
526  /* convert to utm */
528  /* copy results of utm conversion */
529  gps.utm_pos.east = utm_f.east*100;
530  gps.utm_pos.north = utm_f.north*100;
531  ins_x = utm_f.east;
532  ins_y = utm_f.north;
533  ins_z = XSENS_DATA_Position_alt(xsens_msg_buf,offset);//TODO is this hms or above ellipsoid?
534  gps.hmsl = ins_z * 1000;
535  // what about gps.lla_pos.alt and gps.utm_pos.alt ?
536 #endif
537  offset += XSENS_DATA_Position_LENGTH;
538  }
539  if (XSENS_MASK_Velocity(xsens_output_mode)) {
540 #if (! USE_GPS_XSENS_RAW_DATA) && USE_GPS_XSENS
541  ins_vx = XSENS_DATA_Velocity_vx(xsens_msg_buf,offset);
542  ins_vy = XSENS_DATA_Velocity_vy(xsens_msg_buf,offset);
543  ins_vz = XSENS_DATA_Velocity_vz(xsens_msg_buf,offset);
544 #endif
545  offset += XSENS_DATA_Velocity_LENGTH;
546  }
547  if (XSENS_MASK_Status(xsens_output_mode)) {
548  xsens_msg_status = XSENS_DATA_Status_status(xsens_msg_buf,offset);
549 #if USE_GPS_XSENS
550  if (bit_is_set(xsens_msg_status,2)) gps.fix = GPS_FIX_3D; // gps fix
551  else if (bit_is_set(xsens_msg_status,1)) gps.fix = 0x01; // efk valid
552  else gps.fix = GPS_FIX_NONE;
553 #endif
554  offset += XSENS_DATA_Status_LENGTH;
555  }
556  if (XSENS_MASK_TimeStamp(xsens_output_settings)) {
557  xsens_time_stamp = XSENS_DATA_TimeStamp_ts(xsens_msg_buf,offset);
558 #if USE_GPS_XSENS
560 #endif
561  offset += XSENS_DATA_TimeStamp_LENGTH;
562  }
563  if (XSENS_MASK_UTC(xsens_output_settings)) {
564  xsens_hour = XSENS_DATA_UTC_hour(xsens_msg_buf,offset);
565  xsens_min = XSENS_DATA_UTC_min(xsens_msg_buf,offset);
566  xsens_sec = XSENS_DATA_UTC_sec(xsens_msg_buf,offset);
567  xsens_nanosec = XSENS_DATA_UTC_nanosec(xsens_msg_buf,offset);
568  xsens_year = XSENS_DATA_UTC_year(xsens_msg_buf,offset);
569  xsens_month = XSENS_DATA_UTC_month(xsens_msg_buf,offset);
570  xsens_day = XSENS_DATA_UTC_day(xsens_msg_buf,offset);
571 
572  offset += XSENS_DATA_UTC_LENGTH;
573  }
574  //}
575  }
576 
577 }
578 
579 
581  ck += c;
582  switch (xsens_status) {
583  case UNINIT:
584  if (c != XSENS_START)
585  goto error;
586  xsens_status++;
587  ck = 0;
588  break;
589  case GOT_START:
590  if (c != XSENS_BID)
591  goto error;
592  xsens_status++;
593  break;
594  case GOT_BID:
595  xsens_id = c;
596  xsens_status++;
597  break;
598  case GOT_MID:
599  xsens_len = c;
601  goto error;
602  xsens_msg_idx = 0;
603  xsens_status++;
604  break;
605  case GOT_LEN:
607  xsens_msg_idx++;
608  if (xsens_msg_idx >= xsens_len)
609  xsens_status++;
610  break;
611  case GOT_DATA:
612  if (ck != 0)
613  goto error;
615  goto restart;
616  break;
617  }
618  return;
619  error:
620  restart:
622  return;
623 }
INS_FORMAT ins_my
Definition: ins_xsens.c:68
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:64
void ins_periodic_task(void)
Definition: ins_xsens.c:246
struct NedCoor_i ned_vel
speed NED in cm/s
Definition: gps.h:68
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
int32_t lat
in radians*1e7
float ins_roll_neutral
driver for the VectorNav VN100
Definition: ins_xsens.c:72
#define XSENS_OUTPUT_SETTINGS
Definition: ins_xsens.c:179
INS_FORMAT ins_y
Definition: ins_xsens.c:48
void imu_periodic(void)
Definition: imu_b2_arch.c:83
int32_t course
GPS heading in rad*1e7 (CW/north)
Definition: gps.h:71
#define Min(x, y)
uint32_t last_fix_time
cpu time in sec at last valid fix
Definition: gps.h:85
int32_t xsens_nanosec
Definition: ins_xsens.c:213
float xsens_gps_arm_x
Definition: ins_xsens.c:205
#define UNINIT
Definition: ins_xsens.c:182
int8_t xsens_day
Definition: ins_xsens.c:216
INS_FORMAT ins_psi
Definition: ins_xsens.c:57
uint32_t sacc
speed accuracy in cm/s
Definition: gps.h:73
uint8_t zone
UTM zone number.
INS_FORMAT ins_p
Definition: ins_xsens.c:59
float ins_pitch_neutral
Definition: ins_xsens.c:71
static uint8_t xsens_id
Definition: ins_xsens.c:218
#define EstimatorSetAtt(phi, psi, theta)
Definition: estimator.h:131
uint8_t xsens_msg_buf[XSENS_MAX_PAYLOAD]
Definition: ins_xsens.c:126
static uint8_t xsens_status
Definition: ins_xsens.c:219
void ins_init(void)
Definition: ins_xsens.c:230
uint16_t pdop
position dilution of precision scaled by 100
Definition: gps.h:75
uint8_t xsens_msg_status
Definition: ins_xsens.c:200
uint8_t nav_utm_zone0
Definition: common_nav.c:43
int32_t hmsl
height above mean sea level in mm
Definition: gps.h:66
float lat
in radians
INS_FORMAT ins_x
Definition: ins_xsens.c:47
INS_FORMAT ins_vz
Definition: ins_xsens.c:53
#define EstimatorSetAlt(z)
Definition: estimator.h:119
uint8_t fix
status of fix
Definition: gps.h:77
INS_FORMAT ins_vy
Definition: ins_xsens.c:52
#define GPS_FIX_3D
Definition: gps.h:42
struct LlaCoor_f lla_f
Definition: ins_xsens.c:225
volatile int xsens_configured
Definition: ins_xsens.c:228
uint32_t xsens_output_settings
Definition: ins_xsens.c:203
#define GOT_BID
Definition: ins_xsens.c:184
int16_t week
GPS week.
Definition: gps.h:78
uint8_t qi
quality bitfield (GPS receiver specific)
Definition: gps.h:55
volatile uint8_t ins_msg_received
Definition: ins_xsens.c:102
uint8_t xsens_errorcode
Definition: ins_xsens.c:199
vector in Latitude, Longitude and Altitude
#define GOT_MID
Definition: ins_xsens.c:185
int32_t alt
in millimeters above WGS84 reference ellipsoid
Paparazzi floating point math for geodetic calculations.
uint32_t tow
GPS time of week in ms.
Definition: gps.h:79
#define GPS_NB_CHANNELS
Definition: gps_nmea.h:34
int8_t xsens_sec
Definition: ins_xsens.c:212
uint8_t zone
UTM zone number.
void ahrs_init(void)
AHRS initialization.
Definition: ins_xsens.c:75
int8_t xsens_hour
Library for the XSENS AHRS.
Definition: ins_xsens.c:210
void parse_ins_buffer(uint8_t c)
Definition: ins_xsens.c:580
volatile uint8_t new_ins_attitude
Definition: ins_chimu_spi.c:36
int32_t nav_utm_north0
Definition: common_nav.c:42
uint8_t send_ck
Definition: ins_xsens.c:223
int16_t gspeed
norm of 2d ground speed in cm/s
Definition: gps.h:69
Architecture independent timing functions.
float north
in meters
#define GPS_FIX_NONE
Definition: gps.h:40
int8_t xsens_min
Definition: ins_xsens.c:211
Device independent GPS code (interface)
INS_FORMAT ins_theta
Definition: ins_xsens.c:56
position in UTM coordinates Units: meters
unsigned long uint32_t
Definition: types.h:18
signed short int16_t
Definition: types.h:17
int32_t alt
in millimeters above WGS84 reference ellipsoid
void parse_ins_msg(void)
Definition: ins_xsens.c:351
int8_t xsens_month
Definition: ins_xsens.c:215
INS_FORMAT ins_mx
Definition: ins_xsens.c:67
#define EstimatorSetSpeedPol(vhmod, vhdir, vz)
Definition: estimator.h:120
uint16_t xsens_time_stamp
Definition: ins_xsens.c:201
int32_t north
in centimeters
Inertial Measurement Unit interface.
#define LED_TOGGLE(i)
Definition: led_hw.h:30
void handle_ins_msg(void)
Definition: ins_xsens.c:306
struct SVinfo svinfos[GPS_NB_CHANNELS]
holds information from the Space Vehicles (Satellites)
Definition: gps.h:82
#define AHRS_H_Y
#define GOT_START
Definition: ins_xsens.c:183
signed long int32_t
Definition: types.h:19
#define INS_FORMAT
Definition: ins_module.h:38
#define TRUE
Definition: imu_chimu.h:144
int16_t speed_3d
norm of 3d speed in cm/s
Definition: gps.h:70
INS_FORMAT ins_vx
Definition: ins_xsens.c:51
#define AHRS_H_X
int32_t east
in centimeters
INS_FORMAT ins_q
Definition: ins_xsens.c:60
#define EstimatorSetPosXY(x, y)
Definition: estimator.h:118
uint32_t pacc
position accuracy in cm
Definition: gps.h:72
uint8_t flags
bitfield with GPS receiver specific flags
Definition: gps.h:54
unsigned char uint8_t
Definition: types.h:14
INS_FORMAT ins_phi
Definition: ins_xsens.c:55
INS_FORMAT ins_mz
Definition: ins_xsens.c:69
#define WGS84_ELLIPSOID_TO_GEOID(_Lat, _Lon, _diff)
int32_t lon
in radians*1e7
uint8_t svid
Satellite ID.
Definition: gps.h:53
#define GOT_LEN
Definition: ins_xsens.c:186
int32_t nav_utm_east0
Definition: common_nav.c:41
float xsens_gps_arm_y
Definition: ins_xsens.c:206
volatile uint32_t nb_sec
full seconds since startup
Definition: sys_time.h:55
static uint8_t ck
Definition: ins_xsens.c:222
uint16_t xsens_output_mode
Definition: ins_xsens.c:202
float xsens_gps_arm_z
Definition: ins_xsens.c:207
State estimation, fusioning sensors.
struct UtmCoor_i utm_pos
position in UTM (north,east: cm; alt: mm over ellipsoid)
Definition: gps.h:65
int16_t xsens_year
Definition: ins_xsens.c:214
INS_FORMAT ins_z
Definition: ins_xsens.c:49
INS_FORMAT ins_ay
Definition: ins_xsens.c:64
struct UtmCoor_f utm_f
Definition: ins_xsens.c:226
float xsens_declination
Definition: ins_xsens.c:204
float lon
in radians
signed char int8_t
Definition: types.h:15
static struct point c
Definition: discsurvey.c:13
void imu_init(void)
Definition: imu.c:32
INS_FORMAT ins_ax
Definition: ins_xsens.c:63
INS_FORMAT ins_r
Definition: ins_xsens.c:61
static uint8_t xsens_len
Definition: ins_xsens.c:220
static uint8_t xsens_msg_idx
Definition: ins_xsens.c:221
float east
in meters
#define XSENS_OUTPUT_MODE
Definition: ins_xsens.c:150
#define GOT_DATA
Definition: ins_xsens.c:187
struct GpsState gps
global GPS state
Definition: gps.c:31
INS_FORMAT ins_az
Definition: ins_xsens.c:65
uint8_t cno
Carrier to Noise Ratio (Signal Strength) in dbHz.
Definition: gps.h:56
#define XSENS_MAX_PAYLOAD
Includes macros generated from xsens_MTi-G.xml.
Definition: ins_xsens.c:125
void utm_of_lla_f(struct UtmCoor_f *utm, struct LlaCoor_f *lla)
#define EstimatorSetRate(p, q, r)
Definition: estimator.h:134
uint8_t nb_channels
Number of scanned satellites.
Definition: gps.h:81
uint8_t num_sv
number of sat in fix
Definition: gps.h:76