Paparazzi UAS  v5.17_devel-145-ga57bdbf-dirty
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
ahrs_gx3.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2013 Michal Podhradsky
3  * Utah State University, http://aggieair.usu.edu/
4  *
5  * This file is part of paparazzi.
6  *
7  * paparazzi is free software; you can redistribute it and/or modify
8  * it under the terms of the GNU General Public License as published by
9  * the Free Software Foundation; either version 2, or (at your option)
10  * any later version.
11  *
12  * paparazzi is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15  * GNU General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with paparazzi; see the file COPYING. If not, write to
19  * the Free Software Foundation, 59 Temple Place - Suite 330,
20  * Boston, MA 02111-1307, USA.
21  */
22 
35 
36 // for ahrs_register_impl
37 #include "subsystems/ahrs.h"
38 #include "subsystems/abi.h"
39 
40 #define GX3_CHKSM(_ubx_payload) (uint16_t)((uint16_t)(*((uint8_t*)_ubx_payload+66+1))|(uint16_t)(*((uint8_t*)_ubx_payload+66+0))<<8)
41 
49 
50 static inline bool gx3_verify_chk(volatile uint8_t *buff_add);
51 static inline float bef(volatile uint8_t *c);
52 
53 /* Big Endian to Float */
54 static inline float bef(volatile uint8_t *c)
55 {
56  float f;
57  int8_t *p;
58  p = ((int8_t *)&f) + 3;
59  *p-- = *c++;
60  *p-- = *c++;
61  *p-- = *c++;
62  *p = *c;
63  return f;
64 }
65 
66 static inline bool gx3_verify_chk(volatile uint8_t *buff_add)
67 {
68  uint16_t i, chk_calc;
69  chk_calc = 0;
70  for (i = 0; i < GX3_MSG_LEN - 2; i++) {
71  chk_calc += (uint8_t) * buff_add++;
72  }
73  return (chk_calc == ((((uint16_t) * buff_add) << 8) + (uint8_t) * (buff_add + 1)));
74 }
75 
76 void ahrs_gx3_align(void)
77 {
78  ahrs_gx3.is_aligned = false;
79 
80  //make the gyros zero, takes 10s (specified in Byte 4 and 5)
81  uart_put_byte(&GX3_PORT, 0, 0xcd);
82  uart_put_byte(&GX3_PORT, 0, 0xc1);
83  uart_put_byte(&GX3_PORT, 0, 0x29);
84  uart_put_byte(&GX3_PORT, 0, 0x27);
85  uart_put_byte(&GX3_PORT, 0, 0x10);
86 
87  ahrs_gx3.is_aligned = true;
88 }
89 
90 #if PERIODIC_TELEMETRY
92 
93 static void send_gx3(struct transport_tx *trans, struct link_device *dev)
94 {
95  pprz_msg_send_GX3_INFO(trans, dev, AC_ID,
96  &ahrs_gx3.freq,
99  &ahrs_gx3.chksm);
100 }
101 #endif
102 
103 /*
104  * GX3 can be set up during the startup, or it can be configured to
105  * start sending data automatically after power up.
106  */
107 void imu_gx3_init(void)
108 {
109  // Initialize variables
110  ahrs_gx3.is_aligned = false;
111 
112  // Initialize packet
114  ahrs_gx3.packet.msg_idx = 0;
115  ahrs_gx3.packet.msg_available = false;
118 
119  // It is necessary to wait for GX3 to power up for proper initialization
120  for (uint32_t startup_counter = 0; startup_counter < IMU_GX3_LONG_DELAY * 2; startup_counter++) {
121  __asm("nop");
122  }
123 
124 #ifdef GX3_INITIALIZE_DURING_STARTUP
125 #pragma message "GX3 initializing"
126  /*
127  // FOR NON-CONTINUOUS MODE UNCOMMENT THIS
128  //4 byte command for non-Continous Mode so we can set the other settings
129  uart_put_byte(&GX3_PORT, 0, 0xc4);
130  uart_put_byte(&GX3_PORT, 0, 0xc1);
131  uart_put_byte(&GX3_PORT, 0, 0x29);
132  uart_put_byte(&GX3_PORT, 0, 0x00); // stop
133  */
134 
135  //Sampling Settings (0xDB)
136  uart_put_byte(&GX3_PORT, 0, 0xdb); //set update speed
137  uart_put_byte(&GX3_PORT, 0, 0xa8);
138  uart_put_byte(&GX3_PORT, 0, 0xb9);
139  //set rate of IMU link, is 1000/IMU_DIV
140 #define IMU_DIV1 0
141 #define IMU_DIV2 2
142 #define ACC_FILT_DIV 2
143 #define MAG_FILT_DIV 30
144 #ifdef GX3_SAVE_SETTINGS
145  uart_put_byte(&GX3_PORT, 0, 0x02);//set params and save them in non-volatile memory
146 #else
147  uart_put_byte(&GX3_PORT, 0, 0x02); //set and don't save
148 #endif
149  uart_put_byte(&GX3_PORT, 0, IMU_DIV1);
150  uart_put_byte(&GX3_PORT, 0, IMU_DIV2);
151  uart_put_byte(&GX3_PORT, 0, 0b00000000); //set options byte 8 - GOOD
152  uart_put_byte(&GX3_PORT, 0, 0b00000011); //set options byte 7 - GOOD
153  //0 - calculate orientation, 1 - enable coning & sculling, 2-3 reserved, 4 - no little endian data,
154  // 5 - no NaN supressed, 6 - disable finite size correction, 7 - reserved,
155  // 8 - enable magnetometer, 9 - reserved, 10 - enable magnetic north compensation, 11 - enable gravity compensation
156  // 12 - no quaternion calculation, 13-15 reserved
157  uart_put_byte(&GX3_PORT, 0, ACC_FILT_DIV);
158  uart_put_byte(&GX3_PORT, 0, MAG_FILT_DIV); //mag window filter size == 33hz
159  uart_put_byte(&GX3_PORT, 0, 0x00);
160  uart_put_byte(&GX3_PORT, 0, 10); // Up Compensation in secs, def=10s
161  uart_put_byte(&GX3_PORT, 0, 0x00);
162  uart_put_byte(&GX3_PORT, 0, 10); // North Compensation in secs
163  uart_put_byte(&GX3_PORT, 0, 0x00); //power setting = 0, high power/bw
164  uart_put_byte(&GX3_PORT, 0, 0x00); //rest of the bytes are 0
165  uart_put_byte(&GX3_PORT, 0, 0x00);
166  uart_put_byte(&GX3_PORT, 0, 0x00);
167  uart_put_byte(&GX3_PORT, 0, 0x00);
168  uart_put_byte(&GX3_PORT, 0, 0x00);
169 
170  // OPTIONAL: realign up and north
171  /*
172  uart_put_byte(&GX3_PORT, 0, 0xdd);
173  uart_put_byte(&GX3_PORT, 0, 0x54);
174  uart_put_byte(&GX3_PORT, 0, 0x4c);
175  uart_put_byte(&GX3_PORT, 0, 3);
176  uart_put_byte(&GX3_PORT, 0, 10);
177  uart_put_byte(&GX3_PORT, 0, 10);
178  uart_put_byte(&GX3_PORT, 0, 0x00);
179  uart_put_byte(&GX3_PORT, 0, 0x00);
180  uart_put_byte(&GX3_PORT, 0, 0x00);
181  uart_put_byte(&GX3_PORT, 0, 0x00);
182  */
183 
184  //Another wait loop for proper GX3 init
185  for (uint32_t startup_counter = 0; startup_counter < IMU_GX3_LONG_DELAY; startup_counter++) {
186  __asm("nop");
187  }
188 
189 #ifdef GX3_SET_WAKEUP_MODE
190  //Mode Preset (0xD5)
191  uart_put_byte(&GX3_PORT, 0, 0xD5);
192  uart_put_byte(&GX3_PORT, 0, 0xBA);
193  uart_put_byte(&GX3_PORT, 0, 0x89);
194  uart_put_byte(&GX3_PORT, 0, 0x02); // wake up in continuous mode
195 
196  //Continuous preset (0xD6)
197  uart_put_byte(&GX3_PORT, 0, 0xD6);
198  uart_put_byte(&GX3_PORT, 0, 0xC6);
199  uart_put_byte(&GX3_PORT, 0, 0x6B);
200  uart_put_byte(&GX3_PORT, 0, 0xc8); // accel, gyro, R
201 #endif
202 
203  //4 byte command for Continous Mode
204  uart_put_byte(&GX3_PORT, 0, 0xc4);
205  uart_put_byte(&GX3_PORT, 0, 0xc1);
206  uart_put_byte(&GX3_PORT, 0, 0x29);
207  uart_put_byte(&GX3_PORT, 0, 0xc8); // accel,gyro, R
208 
209  // Reset gyros to zero
210  ahrs_gx3_align();
211 #endif
212 
213 #if PERIODIC_TELEMETRY
214  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GX3_INFO, send_gx3);
215 #endif
216 }
217 
218 
220 {
221  /* IF IN NON-CONTINUOUS MODE, REQUEST DATA NOW
222  uart_put_byte(&GX3_PORT, 0, 0xc8); // accel,gyro, R
223  */
224 }
225 
226 
228 {
235  ahrs_gx3.rmat.m[0] = bef(&ahrs_gx3.packet.msg_buf[25]);
236  ahrs_gx3.rmat.m[1] = bef(&ahrs_gx3.packet.msg_buf[29]);
237  ahrs_gx3.rmat.m[2] = bef(&ahrs_gx3.packet.msg_buf[33]);
238  ahrs_gx3.rmat.m[3] = bef(&ahrs_gx3.packet.msg_buf[37]);
239  ahrs_gx3.rmat.m[4] = bef(&ahrs_gx3.packet.msg_buf[41]);
240  ahrs_gx3.rmat.m[5] = bef(&ahrs_gx3.packet.msg_buf[45]);
241  ahrs_gx3.rmat.m[6] = bef(&ahrs_gx3.packet.msg_buf[49]);
242  ahrs_gx3.rmat.m[7] = bef(&ahrs_gx3.packet.msg_buf[53]);
243  ahrs_gx3.rmat.m[8] = bef(&ahrs_gx3.packet.msg_buf[57]);
244  ahrs_gx3.time = (uint32_t)(ahrs_gx3.packet.msg_buf[61] << 24 |
245  ahrs_gx3.packet.msg_buf[62] << 16 |
246  ahrs_gx3.packet.msg_buf[63] << 8 |
247  ahrs_gx3.packet.msg_buf[64]);
249 
250  ahrs_gx3.freq = 62500.0 / (float)(ahrs_gx3.time - ahrs_gx3.ltime);
252 
253  // Acceleration
254  VECT3_SMUL(ahrs_gx3.accel, ahrs_gx3.accel, 9.80665); // Convert g into m/s2
255  // for compatibility with fixed point interface
257 
258  // Rates
259  // for compatibility with fixed point interface
261  struct FloatRates body_rate;
262  /* compute body rates */
263  struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&imu.body_to_imu);
264  float_rmat_ratemult(&body_rate, body_to_imu_rmat, &ahrs_gx3.rate);
265  /* Set state */
266  stateSetBodyRates_f(&body_rate);
267 
268  // Attitude
269  struct FloatRMat ltp_to_body_rmat;
270  float_rmat_comp(&ltp_to_body_rmat, &ahrs_gx3.rmat, body_to_imu_rmat);
271 
272 #if AHRS_USE_GPS_HEADING && USE_GPS
273  struct FloatEulers ltp_to_body_eulers;
274  float_eulers_of_rmat(&ltp_to_body_eulers, &ltp_to_body_rmat);
275  float course_f = (float)DegOfRad(gps.course / 1e7);
276  if (course_f > 180.0) {
277  course_f -= 360.0;
278  }
279  ltp_to_body_eulers.psi = (float)RadOfDeg(course_f);
280  stateSetNedToBodyEulers_f(&ltp_to_body_eulers);
281 #else // !AHRS_USE_GPS_HEADING
282 #ifdef IMU_MAG_OFFSET
283  struct FloatEulers ltp_to_body_eulers;
284  float_eulers_of_rmat(&ltp_to_body_eulers, &ltp_to_body_rmat);
285  ltp_to_body_eulers.psi -= ahrs_gx3.mag_offset;
286  stateSetNedToBodyEulers_f(&ltp_to_body_eulers);
287 #else
288  stateSetNedToBodyRMat_f(&ltp_to_body_rmat);
289 #endif // IMU_MAG_OFFSET
290 #endif // !AHRS_USE_GPS_HEADING
291 }
292 
293 
294 /* GX3 Packet Collection */
296 {
297  switch (ahrs_gx3.packet.status) {
298  case GX3PacketWaiting:
299  ahrs_gx3.packet.msg_idx = 0;
300  if (c == GX3_HEADER) {
304  } else {
306  }
307  break;
308  case GX3PacketReading:
314  } else {
315  ahrs_gx3.packet.msg_available = false;
317  }
318  ahrs_gx3.packet.status = 0;
319  }
320  break;
321  default:
322  ahrs_gx3.packet.status = 0;
323  ahrs_gx3.packet.msg_idx = 0;
324  break;
325  }
326 }
327 
328 void ahrs_gx3_init(void)
329 {
330  /* set ltp_to_imu so that body is zero */
331  struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&imu.body_to_imu);
332  QUAT_COPY(ahrs_gx3.ltp_to_imu_quat, *body_to_imu_quat);
333 #ifdef IMU_MAG_OFFSET
334  ahrs_gx3.mag_offset = IMU_MAG_OFFSET;
335 #else
336  ahrs_gx3.mag_offset = 0.0;
337 #endif
338  ahrs_gx3.is_aligned = false;
339 }
340 
342 {
343  ahrs_gx3_init();
345  ahrs_register_impl(NULL);
346 }
347 
348 
349 /* no scaling */
350 void imu_scale_gyro(struct Imu *_imu __attribute__((unused))) {}
351 void imu_scale_accel(struct Imu *_imu __attribute__((unused))) {}
352 void imu_scale_mag(struct Imu *_imu __attribute__((unused))) {}
353 
355 {
356  uint32_t now_ts = get_sys_time_usec();
357  AbiSendMsgIMU_GYRO_INT32(IMU_GX3_ID, now_ts, &imu.gyro);
358  AbiSendMsgIMU_ACCEL_INT32(IMU_GX3_ID, now_ts, &imu.accel);
359  AbiSendMsgIMU_MAG_INT32(IMU_GX3_ID, now_ts, &imu.mag);
360 }
361 
362 static inline void ReadGX3Buffer(void)
363 {
364  while (uart_char_available(&GX3_PORT) && !ahrs_gx3.packet.msg_available) {
365  gx3_packet_parse(uart_getch(&GX3_PORT));
366  }
367 }
368 
369 void imu_gx3_event(void)
370 {
371  if (uart_char_available(&GX3_PORT)) {
372  ReadGX3Buffer();
373  }
377  ahrs_gx3.packet.msg_available = false;
378  }
379 }
unsigned short uint16_t
Definition: types.h:16
#define RATES_BFP_OF_REAL(_ri, _rf)
Definition: pprz_algebra.h:765
static void stateSetNedToBodyRMat_f(struct FloatRMat *ned_to_body_rmat)
Set vehicle body attitude from rotation matrix (float).
Definition: state.h:1099
static void stateSetNedToBodyEulers_f(struct FloatEulers *ned_to_body_eulers)
Set vehicle body attitude from euler angles (float).
Definition: state.h:1105
float freq
data frequency
Definition: ahrs_gx3.h:75
Dispatcher to register actual AHRS implementations.
void imu_gx3_init(void)
Definition: ahrs_gx3.c:107
void imu_scale_gyro(struct Imu *_imu)
Definition: ahrs_gx3.c:350
bool msg_available
Definition: ahrs_gx3.h:55
uint8_t msg_buf[GX3_MAX_PAYLOAD]
Definition: ahrs_gx3.h:58
Driver for Microstrain GX3 IMU/AHRS subsystem.
Periodic telemetry system header (includes downlink utility and generated code).
uint8_t uart_getch(struct uart_periph *p)
Definition: uart_arch.c:968
void imu_scale_accel(struct Imu *_imu)
Definition: ahrs_gx3.c:351
void float_eulers_of_rmat(struct FloatEulers *e, struct FloatRMat *rm)
static struct FloatRMat * orientationGetRMat_f(struct OrientationReps *orientation)
Get vehicle body attitude rotation matrix (float).
float r
in rad/s
#define IMU_GX3_LONG_DELAY
Definition: ahrs_gx3.h:49
Main include for ABI (AirBorneInterface).
uint32_t hdr_error
Definition: ahrs_gx3.h:57
float psi
in radians
struct AhrsGX3 ahrs_gx3
Axis definition: X axis pointing forward, Y axis pointing to the right and Z axis pointing down...
Definition: ahrs_gx3.c:48
void ahrs_gx3_init(void)
Definition: ahrs_gx3.c:328
struct Imu imu
global IMU state
Definition: imu.c:108
float q
in rad/s
float p
in rad/s
static void send_gx3(struct transport_tx *trans, struct link_device *dev)
Definition: ahrs_gx3.c:93
float mag_offset
Difference between true and magnetic north.
Definition: ahrs_gx3.h:72
struct FloatQuat ltp_to_imu_quat
Rotation from LocalTangentPlane to IMU frame as quaternions.
Definition: ahrs_gx3.h:71
euler angles
#define ACCELS_BFP_OF_REAL(_ef, _ei)
Definition: pprz_algebra.h:801
Roation quaternion.
#define QUAT_COPY(_qo, _qi)
Definition: pprz_algebra.h:596
bool is_aligned
Definition: ahrs_gx3.h:82
struct FloatVect3 accel
measured acceleration in IMU frame
Definition: ahrs_gx3.h:79
void float_rmat_comp(struct FloatRMat *m_a2c, struct FloatRMat *m_a2b, struct FloatRMat *m_b2c)
Composition (multiplication) of two rotation matrices.
struct Int32Vect3 accel
accelerometer measurements in m/s^2 in BFP with INT32_ACCEL_FRAC
Definition: imu.h:39
uint16_t chk_calc
Definition: imu_um6.c:47
void ahrs_register_impl(AhrsEnableOutput enable)
Register an AHRS implementation.
Definition: ahrs.c:62
void ahrs_gx3_publish_imu(void)
Definition: ahrs_gx3.c:354
uint8_t status
Definition: ahrs_gx3.h:59
void gx3_packet_parse(uint8_t c)
Definition: ahrs_gx3.c:295
unsigned long uint32_t
Definition: types.h:18
uint32_t time
GX3 time stamp.
Definition: ahrs_gx3.h:77
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
uint32_t chksm_error
Definition: ahrs_gx3.h:56
void ahrs_gx3_register(void)
Definition: ahrs_gx3.c:341
static void ReadGX3Buffer(void)
Definition: ahrs_gx3.c:362
uint16_t f
Camera baseline, in meters (i.e. horizontal distance between the two cameras of the stereo setup) ...
Definition: wedgebug.c:204
void ahrs_gx3_align(void)
Definition: ahrs_gx3.c:76
struct Int32Vect3 mag
magnetometer measurements scaled to 1 in BFP with INT32_MAG_FRAC
Definition: imu.h:40
struct OrientationReps body_to_imu
rotation from body to imu frame
Definition: imu.h:49
float m[3 *3]
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
void uart_put_byte(struct uart_periph *p, long fd, uint8_t data)
Uart transmit implementation.
Definition: uart_arch.c:1095
#define GX3_CHKSM(_ubx_payload)
Definition: ahrs_gx3.c:40
#define VECT3_SMUL(_vo, _vi, _s)
Definition: pprz_algebra.h:189
unsigned char uint8_t
Definition: types.h:14
void imu_scale_mag(struct Imu *_imu)
Definition: ahrs_gx3.c:352
int32_t course
GPS course over ground in rad*1e7, [0, 2*Pi]*1e7 (CW/north)
Definition: gps.h:99
#define IMU_GX3_ID
struct FloatRMat rmat
measured attitude in IMU frame (rotational matrix)
Definition: ahrs_gx3.h:81
#define GX3_MSG_LEN
Definition: ahrs_gx3.h:45
rotation matrix
void imu_gx3_periodic(void)
Definition: ahrs_gx3.c:219
void imu_gx3_event(void)
Definition: ahrs_gx3.c:369
struct GX3Packet packet
Packet struct.
Definition: ahrs_gx3.h:74
uint16_t chksm
aux variable for checksum
Definition: ahrs_gx3.h:76
static float p[2][2]
void gx3_packet_read_message(void)
Definition: ahrs_gx3.c:227
#define GX3_HEADER
Definition: ahrs_gx3.h:46
void float_rmat_ratemult(struct FloatRates *rb, struct FloatRMat *m_a2b, struct FloatRates *ra)
rotate anglular rates by rotation matrix.
abstract IMU interface providing fixed point interface
Definition: imu.h:37
static float bef(volatile uint8_t *c)
Definition: ahrs_gx3.c:54
signed char int8_t
Definition: types.h:15
struct FloatRates rate
measured angular rates in IMU frame
Definition: ahrs_gx3.h:80
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:68
static void stateSetBodyRates_f(struct FloatRates *body_rate)
Set vehicle body angular rate (float).
Definition: state.h:1181
uint8_t msg_idx
Definition: ahrs_gx3.h:60
static bool gx3_verify_chk(volatile uint8_t *buff_add)
Definition: ahrs_gx3.c:66
struct GpsState gps
global GPS state
Definition: gps.c:69
uint32_t ltime
aux time stamp
Definition: ahrs_gx3.h:78
angular rates
int uart_char_available(struct uart_periph *p)
Check UART for available chars in receive buffer.
Definition: uart_arch.c:323
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46
static struct FloatQuat * orientationGetQuat_f(struct OrientationReps *orientation)
Get vehicle body attitude quaternion (float).
struct Int32Rates gyro
gyroscope measurements in rad/s in BFP with INT32_RATE_FRAC
Definition: imu.h:38