Paparazzi UAS  v5.14.0_stable-0-g3f680d1
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 
39 #define GX3_CHKSM(_ubx_payload) (uint16_t)((uint16_t)(*((uint8_t*)_ubx_payload+66+1))|(uint16_t)(*((uint8_t*)_ubx_payload+66+0))<<8)
40 
48 
49 static inline bool gx3_verify_chk(volatile uint8_t *buff_add);
50 static inline float bef(volatile uint8_t *c);
51 
52 /* Big Endian to Float */
53 static inline float bef(volatile uint8_t *c)
54 {
55  float f;
56  int8_t *p;
57  p = ((int8_t *)&f) + 3;
58  *p-- = *c++;
59  *p-- = *c++;
60  *p-- = *c++;
61  *p = *c;
62  return f;
63 }
64 
65 static inline bool gx3_verify_chk(volatile uint8_t *buff_add)
66 {
67  uint16_t i, chk_calc;
68  chk_calc = 0;
69  for (i = 0; i < GX3_MSG_LEN - 2; i++) {
70  chk_calc += (uint8_t) * buff_add++;
71  }
72  return (chk_calc == ((((uint16_t) * buff_add) << 8) + (uint8_t) * (buff_add + 1)));
73 }
74 
75 void ahrs_gx3_align(void)
76 {
77  ahrs_gx3.is_aligned = false;
78 
79  //make the gyros zero, takes 10s (specified in Byte 4 and 5)
80  uart_put_byte(&GX3_PORT, 0, 0xcd);
81  uart_put_byte(&GX3_PORT, 0, 0xc1);
82  uart_put_byte(&GX3_PORT, 0, 0x29);
83  uart_put_byte(&GX3_PORT, 0, 0x27);
84  uart_put_byte(&GX3_PORT, 0, 0x10);
85 
86  ahrs_gx3.is_aligned = true;
87 }
88 
89 #if PERIODIC_TELEMETRY
91 
92 static void send_gx3(struct transport_tx *trans, struct link_device *dev)
93 {
94  pprz_msg_send_GX3_INFO(trans, dev, AC_ID,
95  &ahrs_gx3.freq,
98  &ahrs_gx3.chksm);
99 }
100 #endif
101 
102 /*
103  * GX3 can be set up during the startup, or it can be configured to
104  * start sending data automatically after power up.
105  */
106 void imu_gx3_init(void)
107 {
108  // Initialize variables
109  ahrs_gx3.is_aligned = false;
110 
111  // Initialize packet
113  ahrs_gx3.packet.msg_idx = 0;
114  ahrs_gx3.packet.msg_available = false;
117 
118  // It is necessary to wait for GX3 to power up for proper initialization
119  for (uint32_t startup_counter = 0; startup_counter < IMU_GX3_LONG_DELAY * 2; startup_counter++) {
120  __asm("nop");
121  }
122 
123 #ifdef GX3_INITIALIZE_DURING_STARTUP
124 #pragma message "GX3 initializing"
125  /*
126  // FOR NON-CONTINUOUS MODE UNCOMMENT THIS
127  //4 byte command for non-Continous Mode so we can set the other settings
128  uart_put_byte(&GX3_PORT, 0, 0xc4);
129  uart_put_byte(&GX3_PORT, 0, 0xc1);
130  uart_put_byte(&GX3_PORT, 0, 0x29);
131  uart_put_byte(&GX3_PORT, 0, 0x00); // stop
132  */
133 
134  //Sampling Settings (0xDB)
135  uart_put_byte(&GX3_PORT, 0, 0xdb); //set update speed
136  uart_put_byte(&GX3_PORT, 0, 0xa8);
137  uart_put_byte(&GX3_PORT, 0, 0xb9);
138  //set rate of IMU link, is 1000/IMU_DIV
139 #define IMU_DIV1 0
140 #define IMU_DIV2 2
141 #define ACC_FILT_DIV 2
142 #define MAG_FILT_DIV 30
143 #ifdef GX3_SAVE_SETTINGS
144  uart_put_byte(&GX3_PORT, 0, 0x02);//set params and save them in non-volatile memory
145 #else
146  uart_put_byte(&GX3_PORT, 0, 0x02); //set and don't save
147 #endif
148  uart_put_byte(&GX3_PORT, 0, IMU_DIV1);
149  uart_put_byte(&GX3_PORT, 0, IMU_DIV2);
150  uart_put_byte(&GX3_PORT, 0, 0b00000000); //set options byte 8 - GOOD
151  uart_put_byte(&GX3_PORT, 0, 0b00000011); //set options byte 7 - GOOD
152  //0 - calculate orientation, 1 - enable coning & sculling, 2-3 reserved, 4 - no little endian data,
153  // 5 - no NaN supressed, 6 - disable finite size correction, 7 - reserved,
154  // 8 - enable magnetometer, 9 - reserved, 10 - enable magnetic north compensation, 11 - enable gravity compensation
155  // 12 - no quaternion calculation, 13-15 reserved
156  uart_put_byte(&GX3_PORT, 0, ACC_FILT_DIV);
157  uart_put_byte(&GX3_PORT, 0, MAG_FILT_DIV); //mag window filter size == 33hz
158  uart_put_byte(&GX3_PORT, 0, 0x00);
159  uart_put_byte(&GX3_PORT, 0, 10); // Up Compensation in secs, def=10s
160  uart_put_byte(&GX3_PORT, 0, 0x00);
161  uart_put_byte(&GX3_PORT, 0, 10); // North Compensation in secs
162  uart_put_byte(&GX3_PORT, 0, 0x00); //power setting = 0, high power/bw
163  uart_put_byte(&GX3_PORT, 0, 0x00); //rest of the bytes are 0
164  uart_put_byte(&GX3_PORT, 0, 0x00);
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 
169  // OPTIONAL: realign up and north
170  /*
171  uart_put_byte(&GX3_PORT, 0, 0xdd);
172  uart_put_byte(&GX3_PORT, 0, 0x54);
173  uart_put_byte(&GX3_PORT, 0, 0x4c);
174  uart_put_byte(&GX3_PORT, 0, 3);
175  uart_put_byte(&GX3_PORT, 0, 10);
176  uart_put_byte(&GX3_PORT, 0, 10);
177  uart_put_byte(&GX3_PORT, 0, 0x00);
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  */
182 
183  //Another wait loop for proper GX3 init
184  for (uint32_t startup_counter = 0; startup_counter < IMU_GX3_LONG_DELAY; startup_counter++) {
185  __asm("nop");
186  }
187 
188 #ifdef GX3_SET_WAKEUP_MODE
189  //Mode Preset (0xD5)
190  uart_put_byte(&GX3_PORT, 0, 0xD5);
191  uart_put_byte(&GX3_PORT, 0, 0xBA);
192  uart_put_byte(&GX3_PORT, 0, 0x89);
193  uart_put_byte(&GX3_PORT, 0, 0x02); // wake up in continuous mode
194 
195  //Continuous preset (0xD6)
196  uart_put_byte(&GX3_PORT, 0, 0xD6);
197  uart_put_byte(&GX3_PORT, 0, 0xC6);
198  uart_put_byte(&GX3_PORT, 0, 0x6B);
199  uart_put_byte(&GX3_PORT, 0, 0xc8); // accel, gyro, R
200 #endif
201 
202  //4 byte command for Continous Mode
203  uart_put_byte(&GX3_PORT, 0, 0xc4);
204  uart_put_byte(&GX3_PORT, 0, 0xc1);
205  uart_put_byte(&GX3_PORT, 0, 0x29);
206  uart_put_byte(&GX3_PORT, 0, 0xc8); // accel,gyro, R
207 
208  // Reset gyros to zero
209  ahrs_gx3_align();
210 #endif
211 
212 #if PERIODIC_TELEMETRY
213  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GX3_INFO, send_gx3);
214 #endif
215 }
216 
217 
219 {
220  /* IF IN NON-CONTINUOUS MODE, REQUEST DATA NOW
221  uart_put_byte(&GX3_PORT, 0, 0xc8); // accel,gyro, R
222  */
223 }
224 
225 
227 {
234  ahrs_gx3.rmat.m[0] = bef(&ahrs_gx3.packet.msg_buf[25]);
235  ahrs_gx3.rmat.m[1] = bef(&ahrs_gx3.packet.msg_buf[29]);
236  ahrs_gx3.rmat.m[2] = bef(&ahrs_gx3.packet.msg_buf[33]);
237  ahrs_gx3.rmat.m[3] = bef(&ahrs_gx3.packet.msg_buf[37]);
238  ahrs_gx3.rmat.m[4] = bef(&ahrs_gx3.packet.msg_buf[41]);
239  ahrs_gx3.rmat.m[5] = bef(&ahrs_gx3.packet.msg_buf[45]);
240  ahrs_gx3.rmat.m[6] = bef(&ahrs_gx3.packet.msg_buf[49]);
241  ahrs_gx3.rmat.m[7] = bef(&ahrs_gx3.packet.msg_buf[53]);
242  ahrs_gx3.rmat.m[8] = bef(&ahrs_gx3.packet.msg_buf[57]);
243  ahrs_gx3.time = (uint32_t)(ahrs_gx3.packet.msg_buf[61] << 24 |
244  ahrs_gx3.packet.msg_buf[62] << 16 |
245  ahrs_gx3.packet.msg_buf[63] << 8 |
246  ahrs_gx3.packet.msg_buf[64]);
248 
249  ahrs_gx3.freq = 62500.0 / (float)(ahrs_gx3.time - ahrs_gx3.ltime);
251 
252  // Acceleration
253  VECT3_SMUL(ahrs_gx3.accel, ahrs_gx3.accel, 9.80665); // Convert g into m/s2
254  // for compatibility with fixed point interface
256 
257  // Rates
258  // for compatibility with fixed point interface
260  struct FloatRates body_rate;
261  /* compute body rates */
262  struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&imu.body_to_imu);
263  FLOAT_RMAT_TRANSP_RATEMULT(body_rate, *body_to_imu_rmat, ahrs_gx3.rate);
264  /* Set state */
265  stateSetBodyRates_f(&body_rate);
266 
267  // Attitude
268  struct FloatRMat ltp_to_body_rmat;
269  float_rmat_comp(ltp_to_body_rmat, ahrs_gx3.rmat, *body_to_imu_rmat);
270 
271 #if AHRS_USE_GPS_HEADING && USE_GPS
272  struct FloatEulers ltp_to_body_eulers;
273  float_eulers_of_rmat(&ltp_to_body_eulers, &ltp_to_body_rmat);
274  float course_f = (float)DegOfRad(gps.course / 1e7);
275  if (course_f > 180.0) {
276  course_f -= 360.0;
277  }
278  ltp_to_body_eulers.psi = (float)RadOfDeg(course_f);
279  stateSetNedToBodyEulers_f(&ltp_to_body_eulers);
280 #else // !AHRS_USE_GPS_HEADING
281 #ifdef IMU_MAG_OFFSET
282  struct FloatEulers ltp_to_body_eulers;
283  float_eulers_of_rmat(&ltp_to_body_eulers, &ltp_to_body_rmat);
284  ltp_to_body_eulers.psi -= ahrs_gx3.mag_offset;
285  stateSetNedToBodyEulers_f(&ltp_to_body_eulers);
286 #else
287  stateSetNedToBodyRMat_f(&ltp_to_body_rmat);
288 #endif // IMU_MAG_OFFSET
289 #endif // !AHRS_USE_GPS_HEADING
290 }
291 
292 
293 /* GX3 Packet Collection */
295 {
296  switch (ahrs_gx3.packet.status) {
297  case GX3PacketWaiting:
298  ahrs_gx3.packet.msg_idx = 0;
299  if (c == GX3_HEADER) {
303  } else {
305  }
306  break;
307  case GX3PacketReading:
313  } else {
314  ahrs_gx3.packet.msg_available = false;
316  }
317  ahrs_gx3.packet.status = 0;
318  }
319  break;
320  default:
321  ahrs_gx3.packet.status = 0;
322  ahrs_gx3.packet.msg_idx = 0;
323  break;
324  }
325 }
326 
327 void ahrs_gx3_init(void)
328 {
329  /* set ltp_to_imu so that body is zero */
330  struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&imu.body_to_imu);
331  QUAT_COPY(ahrs_gx3.ltp_to_imu_quat, *body_to_imu_quat);
332 #ifdef IMU_MAG_OFFSET
333  ahrs_gx3.mag_offset = IMU_MAG_OFFSET;
334 #else
335  ahrs_gx3.mag_offset = 0.0;
336 #endif
337  ahrs_gx3.is_aligned = false;
338 }
339 
341 {
342  ahrs_gx3_init();
344  ahrs_register_impl(NULL);
345 }
346 
347 
348 /* no scaling */
349 void imu_scale_gyro(struct Imu *_imu __attribute__((unused))) {}
350 void imu_scale_accel(struct Imu *_imu __attribute__((unused))) {}
351 void imu_scale_mag(struct Imu *_imu __attribute__((unused))) {}
352 
354 {
355  uint32_t now_ts = get_sys_time_usec();
356  AbiSendMsgIMU_GYRO_INT32(IMU_GX3_ID, now_ts, &imu.gyro);
357  AbiSendMsgIMU_ACCEL_INT32(IMU_GX3_ID, now_ts, &imu.accel);
358  AbiSendMsgIMU_MAG_INT32(IMU_GX3_ID, now_ts, &imu.mag);
359 }
360 
361 static inline void ReadGX3Buffer(void)
362 {
363  while (uart_char_available(&GX3_PORT) && !ahrs_gx3.packet.msg_available) {
364  gx3_packet_parse(uart_getch(&GX3_PORT));
365  }
366 }
367 
368 void imu_gx3_event(void)
369 {
370  if (uart_char_available(&GX3_PORT)) {
371  ReadGX3Buffer();
372  }
376  ahrs_gx3.packet.msg_available = false;
377  }
378 }
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:106
void imu_scale_gyro(struct Imu *_imu)
Definition: ahrs_gx3.c:349
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:840
void imu_scale_accel(struct Imu *_imu)
Definition: ahrs_gx3.c:350
void float_eulers_of_rmat(struct FloatEulers *e, struct FloatRMat *rm)
uint16_t uart_char_available(struct uart_periph *p)
Check UART for available chars in receive buffer.
Definition: uart_arch.c:324
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
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:47
void ahrs_gx3_init(void)
Definition: ahrs_gx3.c:327
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:92
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:353
uint8_t status
Definition: ahrs_gx3.h:59
void gx3_packet_parse(uint8_t c)
Definition: ahrs_gx3.c:294
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
#define FLOAT_RMAT_TRANSP_RATEMULT(_rb, _m_b2a, _ra)
uint32_t chksm_error
Definition: ahrs_gx3.h:56
void ahrs_gx3_register(void)
Definition: ahrs_gx3.c:340
static void ReadGX3Buffer(void)
Definition: ahrs_gx3.c:361
void ahrs_gx3_align(void)
Definition: ahrs_gx3.c:75
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:73
void uart_put_byte(struct uart_periph *p, long fd, uint8_t data)
Uart transmit implementation.
Definition: uart_arch.c:967
#define GX3_CHKSM(_ubx_payload)
Definition: ahrs_gx3.c:39
#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:351
int32_t course
GPS course over ground in rad*1e7, [0, 2*Pi]*1e7 (CW/north)
Definition: gps.h:93
#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:218
void imu_gx3_event(void)
Definition: ahrs_gx3.c:368
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:226
#define GX3_HEADER
Definition: ahrs_gx3.h:46
abstract IMU interface providing fixed point interface
Definition: imu.h:37
static float bef(volatile uint8_t *c)
Definition: ahrs_gx3.c:53
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:65
struct GpsState gps
global GPS state
Definition: gps.c:75
uint32_t ltime
aux time stamp
Definition: ahrs_gx3.h:78
angular rates
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