Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
xsens700.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2013 Christophe De Wagter
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 
28 #include "xsens700.h"
29 
30 #include "led.h"
31 #include "generated/airframe.h"
32 #include "mcu_periph/uart.h"
33 
34 #if USE_GPS_XSENS
36 #endif
37 
38 
44 
45 float xsens_gps_arm_x = 0;
46 float xsens_gps_arm_y = 0;
47 float xsens_gps_arm_z = 0;
48 
49 struct Xsens xsens700;
50 
51 volatile int xsens_configured = 0;
52 
53 void xsens700_init(void)
54 {
56  xsens_configured = 30;
57 
59  xsens_time_stamp = 0;
60 }
61 
63 {
64  uint8_t foo = 0;
68  XsensSend1ByAddr(&freq);
69 }
70 
72 {
73  if (xsens_configured > 0) {
74  switch (xsens_configured) {
75  case 25:
76  /* send mode and settings to MT */
77  XSENS_GoToConfig();
78  //XSENS_SetOutputMode(xsens_output_mode);
79  //XSENS_SetOutputSettings(xsens_output_settings);
80  break;
81  case 18:
82  // Give pulses on SyncOut
83  //XSENS_SetSyncOutSettings(0,0x0002);
84  break;
85  case 17:
86 
87  XsensHeader(XSENS_SetOutputConfiguration_ID, 44);
88  xsens_ask_message_rate(0x10, 0x10, 4); // UTC
89  xsens_ask_message_rate(0x20, 0x30, 100); // Attitude Euler
90  xsens_ask_message_rate(0x40, 0x10, 100); // Delta-V
91  xsens_ask_message_rate(0x80, 0x20, 100); // Rate of turn
92  xsens_ask_message_rate(0xE0, 0x20, 50); // Status
93  xsens_ask_message_rate(0x30, 0x10, 50); // Baro Pressure
94  xsens_ask_message_rate(0x88, 0x40, 1); // NavSol
95  xsens_ask_message_rate(0x88, 0xA0, 1); // SV Info
96  xsens_ask_message_rate(0x50, 0x20, 50); // GPS Altitude Ellipsiod
97  xsens_ask_message_rate(0x50, 0x40, 50); // GPS Position
98  xsens_ask_message_rate(0xD0, 0x10, 50); // GPS Speed
99  XsensTrailer();
100  break;
101  case 2:
102  //XSENS_ReqLeverArmGps();
103  break;
104 
105  case 6:
106  //XSENS_ReqMagneticDeclination();
107  break;
108 
109  case 13:
110  //#ifdef AHRS_H_X
111  //#pragma message "Sending XSens Magnetic Declination."
112  //xsens_declination = atan2(AHRS_H_Y, AHRS_H_X);
113  //XSENS_SetMagneticDeclination(xsens_declination);
114  //#endif
115  break;
116  case 12:
117 #ifdef GPS_IMU_LEVER_ARM_X
118 #pragma message "Sending XSens GPS Arm."
119  XSENS_SetLeverArmGps(GPS_IMU_LEVER_ARM_X, GPS_IMU_LEVER_ARM_Y, GPS_IMU_LEVER_ARM_Z);
120 #endif
121  break;
122  case 10: {
123  uint8_t baud = 1;
124  XSENS_SetBaudrate(baud);
125  }
126  break;
127 
128  case 1:
129  XSENS_GoToMeasurment();
130  break;
131  default:
132  break;
133  }
135  return;
136  }
137 
138 }
139 
140 
142 {
143  uint8_t offset = 0;
144  if (xsens700.parser.id == XSENS_ReqLeverArmGpsAck_ID) {
145  xsens_gps_arm_x = XSENS_ReqLeverArmGpsAck_x(xsens700.parser.msg_buf);
146  xsens_gps_arm_y = XSENS_ReqLeverArmGpsAck_y(xsens700.parser.msg_buf);
147  xsens_gps_arm_z = XSENS_ReqLeverArmGpsAck_z(xsens700.parser.msg_buf);
148  } else if (xsens700.parser.id == XSENS_Error_ID) {
149  xsens_errorcode = XSENS_Error_errorcode(xsens700.parser.msg_buf);
150  } else if (xsens700.parser.id == XSENS_MTData2_ID) {
151  for (offset = 0; offset < xsens700.parser.len;) {
153  uint8_t code2 = xsens700.parser.msg_buf[offset + 1];
154  int subpacklen = (int)xsens700.parser.msg_buf[offset + 2];
155  offset += 3;
156 
157  if (code1 == 0x10) {
158  if (code2 == 0x10) {
159  // UTC
160  } else if (code2 == 0x20) {
161  // Packet Counter
162  }
163  if (code2 == 0x30) {
164  // ITOW
165  }
166  } else if (code1 == 0x20) {
167  if (code2 == 0x30) {
168  // Attitude Euler
169  xsens700.euler.phi = XSENS_DATA_Euler_roll(xsens700.parser.msg_buf, offset) * M_PI / 180;
170  xsens700.euler.theta = XSENS_DATA_Euler_pitch(xsens700.parser.msg_buf, offset) * M_PI / 180;
171  xsens700.euler.psi = XSENS_DATA_Euler_yaw(xsens700.parser.msg_buf, offset) * M_PI / 180;
172 
174  }
175  } else if (code1 == 0x40) {
176  if (code2 == 0x10) {
177  // Delta-V
178  xsens700.accel.x = XSENS_DATA_Acceleration_x(xsens700.parser.msg_buf, offset) * 100.0f;
179  xsens700.accel.y = XSENS_DATA_Acceleration_y(xsens700.parser.msg_buf, offset) * 100.0f;
180  xsens700.accel.z = XSENS_DATA_Acceleration_z(xsens700.parser.msg_buf, offset) * 100.0f;
181  }
182  } else if (code1 == 0x80) {
183  if (code2 == 0x20) {
184  // Rate Of Turn
185  xsens700.gyro.p = XSENS_DATA_RateOfTurn_x(xsens700.parser.msg_buf, offset) * M_PI / 180;
186  xsens700.gyro.q = XSENS_DATA_RateOfTurn_y(xsens700.parser.msg_buf, offset) * M_PI / 180;
187  xsens700.gyro.r = XSENS_DATA_RateOfTurn_z(xsens700.parser.msg_buf, offset) * M_PI / 180;
188  }
189  } else if (code1 == 0x30) {
190  if (code2 == 0x10) {
191  // Baro Pressure
192  }
193  } else if (code1 == 0xE0) {
194  if (code2 == 0x20) {
195  // Status Word
196  xsens_msg_statusword = XSENS_DATA_StatusWord_status(xsens700.parser.msg_buf, offset);
197  //xsens700.gps.tow = xsens_msg_statusword;
198 #if USE_GPS_XSENS
199  if (bit_is_set(xsens_msg_statusword, 2) && bit_is_set(xsens_msg_statusword, 1)) {
200  if (bit_is_set(xsens_msg_statusword, 23) && bit_is_set(xsens_msg_statusword, 24)) {
201  xsens700.gps.fix = GPS_FIX_3D;
202  } else {
203  xsens700.gps.fix = GPS_FIX_2D;
204  }
205  } else { xsens700.gps.fix = GPS_FIX_NONE; }
206 #endif
207  }
208  } else if (code1 == 0x88) {
209  if (code2 == 0x40) {
210  xsens700.gps.week = XSENS_DATA_GpsSol_week(xsens700.parser.msg_buf, offset);
211  xsens700.gps.num_sv = XSENS_DATA_GpsSol_numSv(xsens700.parser.msg_buf, offset);
212  xsens700.gps.pacc = XSENS_DATA_GpsSol_pAcc(xsens700.parser.msg_buf, offset);
213  xsens700.gps.sacc = XSENS_DATA_GpsSol_sAcc(xsens700.parser.msg_buf, offset);
214  xsens700.gps.pdop = XSENS_DATA_GpsSol_pDop(xsens700.parser.msg_buf, offset);
215  } else if (code2 == 0xA0) {
216  // SVINFO
217  xsens700.gps.tow = XSENS_XDI_GpsSvInfo_iTOW(xsens700.parser.msg_buf + offset);
218 
219 #if USE_GPS_XSENS
220  xsens700.gps.nb_channels = XSENS_XDI_GpsSvInfo_nch(xsens700.parser.msg_buf + offset);
221 
222  xsens700.gps.last_3dfix_ticks = sys_time.nb_sec_rem;
223  xsens700.gps.last_3dfix_time = sys_time.nb_sec;
224 
225  uint8_t i;
226  // Do not write outside buffer
227  for (i = 0; i < Min(xsens700.gps.nb_channels, GPS_NB_CHANNELS); i++) {
228  uint8_t ch = XSENS_XDI_GpsSvInfo_chn(xsens700.parser.msg_buf + offset, i);
229  if (ch > xsens700.gps.nb_channels) { continue; }
230  xsens700.gps.svinfos[ch].svid = XSENS_XDI_GpsSvInfo_svid(xsens700.parser.msg_buf + offset, i);
231  xsens700.gps.svinfos[ch].flags = XSENS_XDI_GpsSvInfo_bitmask(xsens700.parser.msg_buf + offset, i);
232  xsens700.gps.svinfos[ch].qi = XSENS_XDI_GpsSvInfo_qi(xsens700.parser.msg_buf + offset, i);
233  xsens700.gps.svinfos[ch].cno = XSENS_XDI_GpsSvInfo_cnr(xsens700.parser.msg_buf + offset, i);
234  }
235 #endif
236  }
237  } else if (code1 == 0x50) {
238  if (code2 == 0x10) {
239  //xsens700.gps.hmsl = XSENS_DATA_Altitude_h(xsens700.parser.msg_buf,offset)* 1000.0f;
240  } else if (code2 == 0x20) {
241  // Altitude Elipsoid
242  xsens700.gps.lla_pos.alt = XSENS_DATA_Altitude_h(xsens700.parser.msg_buf, offset) * 1000.0f;
243 
244  // Compute geoid (MSL) height
246  xsens700.gps.hmsl = xsens700.gps.lla_pos.alt - (geoid_h * 1000.0f);
247  SetBit(xsens700.gps.valid_fields, GPS_VALID_HMSL_BIT);
248 
249  //xsens700.gps.tow = geoid_h * 1000.0f; //xsens700.gps.utm_pos.alt;
250  } else if (code2 == 0x40) {
251  // LatLong
252 #ifdef GPS_LED
253  LED_TOGGLE(GPS_LED);
254 #endif
255  xsens700.gps.last_3dfix_ticks = sys_time.nb_sec_rem;
256  xsens700.gps.last_3dfix_time = sys_time.nb_sec;
257  xsens700.gps.week = 0; // FIXME
258 
259  xsens700.lla_f.lat = RadOfDeg(XSENS_DATA_LatLon_lat(xsens700.parser.msg_buf, offset));
260  xsens700.lla_f.lon = RadOfDeg(XSENS_DATA_LatLon_lon(xsens700.parser.msg_buf, offset));
261  }
262  } else if (code1 == 0xD0) {
263  if (code2 == 0x10) {
264  // Velocity
265  xsens700.vel.x = XSENS_DATA_VelocityXYZ_x(xsens700.parser.msg_buf, offset);
266  xsens700.vel.y = XSENS_DATA_VelocityXYZ_y(xsens700.parser.msg_buf, offset);
267  xsens700.vel.z = XSENS_DATA_VelocityXYZ_z(xsens700.parser.msg_buf, offset);
268  xsens700.gps.ned_vel.x = xsens700.vel.x;
269  xsens700.gps.ned_vel.y = xsens700.vel.y;
270  xsens700.gps.ned_vel.z = xsens700.vel.x;
271  SetBit(xsens700.gps.valid_fields, GPS_VALID_VEL_NED_BIT);
272  }
273  }
274 
275  if (subpacklen < 0) {
276  subpacklen = 0;
277  }
278  offset += subpacklen;
279  }
280  }
281 }
static uint16_t c1
Definition: baro_MS5534A.c:203
static uint16_t c2
Definition: baro_MS5534A.c:203
#define LED_TOGGLE(i)
Definition: led_hw.h:53
static const float offset[]
if(GpsFixValid() &&e_identification_started)
#define Min(x, y)
Definition: esc_dshot.c:109
#define GPS_VALID_VEL_NED_BIT
Definition: gps.h:51
#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
#define GPS_FIX_2D
2D GPS fix
Definition: gps.h:42
#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
static float wgs84_ellipsoid_to_geoid_f(float lat, float lon)
Get WGS84 ellipsoid/geoid separation.
arch independent LED (Light Emitting Diodes) API
uint16_t foo
Definition: main_demo5.c:58
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
volatile uint32_t nb_sec
full seconds since startup
Definition: sys_time.h:72
volatile uint32_t nb_sec_rem
remainder of seconds since startup in CPU_TICKS
Definition: sys_time.h:73
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
void xsens700_init(void)
Definition: xsens700.c:53
float xsens_gps_arm_y
Definition: xsens700.c:46
volatile int xsens_configured
Definition: xsens700.c:51
uint16_t xsens_time_stamp
Definition: xsens700.c:41
uint8_t xsens_errorcode
Definition: xsens700.c:39
uint32_t xsens_msg_statusword
Definition: xsens700.c:40
float xsens_gps_arm_z
Definition: xsens700.c:47
uint32_t xsens_output_settings
Definition: xsens700.c:43
uint16_t xsens_output_mode
Definition: xsens700.c:42
void parse_xsens700_msg(void)
Definition: xsens700.c:141
static void xsens_ask_message_rate(uint8_t c1, uint8_t c2, uint8_t freq)
Definition: xsens700.c:62
void xsens700_periodic(void)
Definition: xsens700.c:71
struct Xsens xsens700
Definition: xsens700.c:49
float xsens_gps_arm_x
Definition: xsens700.c:45
Parser for the Xsens protocol.
struct FloatVect3 accel
Definition: xsens.h:57
struct LlaCoor_f lla_f
Definition: xsens.h:60
struct XsensParser parser
Definition: xsens.h:66
struct FloatEulers euler
Definition: xsens.h:64
struct FloatVect3 vel
NED velocity in m/s.
Definition: xsens.h:61
struct FloatRates gyro
Definition: xsens.h:56
volatile bool new_attitude
Definition: xsens.h:67
Definition: xsens.h:52
uint8_t status
Definition: xsens_parser.h:49
#define XsensHeader(msg_id, len)
Definition: xsens_parser.h:71
#define XsensTrailer()
Definition: xsens_parser.h:78
uint8_t id
Definition: xsens_parser.h:48
uint8_t msg_buf[XSENS_MAX_PAYLOAD]
Definition: xsens_parser.h:53
uint8_t len
Definition: xsens_parser.h:50
#define XsensSend1ByAddr(x)
Definition: xsens_parser.h:67