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
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  */
33 #include "mcu_periph/sys_time.h"
34 
35 #define GX3_TIME(_ubx_payload) (uint32_t)((uint32_t)(*((uint8_t*)_ubx_payload+62+3))|(uint32_t)(*((uint8_t*)_ubx_payload+62+2))<<8|(uint32_t)(*((uint8_t*)_ubx_payload+62+1))<<16|(uint32_t)(*((uint8_t*)_ubx_payload+62+0))<<24)
36 #define GX3_CHKSM(_ubx_payload) (uint16_t)((uint16_t)(*((uint8_t*)_ubx_payload+66+1))|(uint16_t)(*((uint8_t*)_ubx_payload+66+0))<<8)
37 
38 /*
39  * Axis definition: X axis pointing forward, Y axis pointing to the right and Z axis pointing down.
40  * Positive pitch : nose up
41  * Positive roll : right wing down
42  * Positive yaw : clockwise
43  */
50 float GX3_freq;
51 
57 
60 
61 static inline bool_t GX3_verify_chk(volatile uint8_t *buff_add);
62 static inline float bef(volatile uint8_t *c);
63 
64 /* Big Endian to Float */
65 static inline float bef(volatile uint8_t *c) {
66  float f;
67  int8_t * p;
68  p = ((int8_t *)&f)+3;
69  *p-- = *c++;
70  *p-- = *c++;
71  *p-- = *c++;
72  *p = *c;
73  return f;
74 }
75 
76 static inline bool_t GX3_verify_chk(volatile uint8_t *buff_add) {
78  chk_calc = 0;
79  for (i=0;i<GX3_MSG_LEN-2;i++) {
80  chk_calc += (uint8_t)*buff_add++;
81  }
82  return (chk_calc == ( (((uint16_t)*buff_add)<<8) + (uint8_t)*(buff_add+1) ));
83 }
84 
85 void ahrs_align(void) {
87 
88  //make the gyros zero, takes 10s (specified in Byte 4 and 5)
89  GX3Link(Transmit(0xcd));
90  GX3Link(Transmit(0xc1));
91  GX3Link(Transmit(0x29));
92  GX3Link(Transmit(0x27));
93  GX3Link(Transmit(0x10));
94 
96 }
97 
98 
99 void imu_impl_init(void) {
100  // Initialize variables
102 
103  // Initialize packet
105  GX3_packet.msg_idx = 0;
108  GX3_packet.hdr_error = 0;
109 
110  // It is necessary to wait for GX3 to power up for proper initialization
111  for (uint32_t startup_counter=0; startup_counter<IMU_GX3_LONG_DELAY*2; startup_counter++){
112  __asm("nop");
113  }
114 
115  /*
116  // FOR NON-CONTINUOUS MODE UNCOMMENT THIS
117  //4 byte command for non-Continous Mode so we can set the other settings
118  GX3Link(Transmit(0xc4));
119  GX3Link(Transmit(0xc1));
120  GX3Link(Transmit(0x29));
121  GX3Link(Transmit(0x00)); // stop
122  */
123 
124  //Sampling Settings (0xDB)
125  GX3Link(Transmit(0xdb)); //set update speed
126  GX3Link(Transmit(0xa8));
127  GX3Link(Transmit(0xb9));
128  //set rate of IMU link, is 1000/IMU_DIV
129 #define IMU_DIV1 0
130 #define IMU_DIV2 2
131 #define ACC_FILT_DIV 2
132 #define MAG_FILT_DIV 30
133  GX3Link(Transmit(0x01));//set params, don't store
134  GX3Link(Transmit(IMU_DIV1));
135  GX3Link(Transmit(IMU_DIV2));
136  GX3Link(Transmit(0b00000000)); //set options byte 8 - GOOD
137  GX3Link(Transmit(0b00000011)); //set options byte 7 - GOOD
138  //0 - calculate orientation, 1 - enable coning & sculling, 2-3 reserved, 4 - no little endian data,
139  // 5 - no NaN supressed, 6 - disable finite size correction, 7 - reserved,
140  // 8 - enable magnetometer, 9 - reserved, 10 - enable magnetic north compensation, 11 - enable gravity compensation
141  // 12 - no quaternion calculation, 13-15 reserved
142  GX3Link(Transmit(ACC_FILT_DIV));
143  GX3Link(Transmit(MAG_FILT_DIV)); //mag window filter size == 33hz
144  GX3Link(Transmit(0x00));
145  GX3Link(Transmit(10)); // Up Compensation in secs, def=10s
146  GX3Link(Transmit(0x00));
147  GX3Link(Transmit(10)); // North Compensation in secs
148  GX3Link(Transmit(0x00)); //power setting = 0, high power/bw
149  GX3Link(Transmit(0x00)); //rest of the bytes are 0
150  GX3Link(Transmit(0x00));
151  GX3Link(Transmit(0x00));
152  GX3Link(Transmit(0x00));
153  GX3Link(Transmit(0x00));
154 
155  // OPTIONAL: realign up and north
156  /*
157  GX3Link(Transmit(0xdd));
158  GX3Link(Transmit(0x54));
159  GX3Link(Transmit(0x4c));
160  GX3Link(Transmit(3));
161  GX3Link(Transmit(10));
162  GX3Link(Transmit(10));
163  GX3Link(Transmit(0x00));
164  GX3Link(Transmit(0x00));
165  GX3Link(Transmit(0x00));
166  GX3Link(Transmit(0x00));
167  */
168 
169  // Another wait loop for proper GX3 init
170  for (uint32_t startup_counter=0; startup_counter<IMU_GX3_LONG_DELAY; startup_counter++){
171  __asm("nop");
172  }
173 
174  //4 byte command for Continous Mode
175  GX3Link(Transmit(0xc4));
176  GX3Link(Transmit(0xc1));
177  GX3Link(Transmit(0x29));
178  GX3Link(Transmit(0xc8)); // accel,gyro,R
179 
180  // Reset gyros to zerp
181  ahrs_align();
182 }
183 
184 
185 void imu_periodic(void) {
186  /* IF IN NON-CONTINUOUS MODE, REQUEST DATA NOW
187  GX3Link(Transmit(0xc8)); // accel,gyro,R
188  */
189 }
190 
191 
196  GX3_rate.p = bef(&GX3_packet.msg_buf[13]);
197  GX3_rate.q = bef(&GX3_packet.msg_buf[17]);
198  GX3_rate.r = bef(&GX3_packet.msg_buf[21]);
199  GX3_rmat.m[0] = bef(&GX3_packet.msg_buf[25]);
200  GX3_rmat.m[1] = bef(&GX3_packet.msg_buf[29]);
201  GX3_rmat.m[2] = bef(&GX3_packet.msg_buf[33]);
202  GX3_rmat.m[3] = bef(&GX3_packet.msg_buf[37]);
203  GX3_rmat.m[4] = bef(&GX3_packet.msg_buf[41]);
204  GX3_rmat.m[5] = bef(&GX3_packet.msg_buf[45]);
205  GX3_rmat.m[6] = bef(&GX3_packet.msg_buf[49]);
206  GX3_rmat.m[7] = bef(&GX3_packet.msg_buf[53]);
207  GX3_rmat.m[8] = bef(&GX3_packet.msg_buf[57]);
210  GX3_calcsm = 0;
211 
212  GX3_freq = ((GX3_time - GX3_ltime))/16000000.0;
213  GX3_freq = 1.0/GX3_freq;
215 
216  // Acceleration
217  VECT3_SMUL(GX3_accel, GX3_accel, 9.80665); // Convert g into m/s2
219  imuf.accel = GX3_accel;
220 
221  // Rates
222  struct FloatRates body_rate;
224  /* compute body rates */
225  FLOAT_RMAT_TRANSP_RATEMULT(body_rate, imuf.body_to_imu_rmat, ahrs_impl.imu_rate);
226  /* Set state */
227  stateSetBodyRates_f(&body_rate);
228 
229  // Quaternions from rotation matrix
232  /* Compute LTP to BODY quaternion */
233  struct FloatQuat ltp_to_body_quat;
234  FLOAT_QUAT_COMP_INV(ltp_to_body_quat, ahrs_impl.ltp_to_imu_quat, imuf.body_to_imu_quat);
235  stateSetNedToBodyQuat_f(&ltp_to_body_quat);
236 
237  // TODO: compensate for magnetic offset
238 }
239 
240 
241 /* GX3 Packet Collection */
243  switch (GX3_packet.status) {
244  case GX3PacketWaiting:
245  GX3_packet.msg_idx = 0;
246  if (c == GX3_HEADER) {
247  GX3_packet.status++;
250  } else {
252  }
253  break;
254  case GX3PacketReading:
257  if (GX3_packet.msg_idx == GX3_MSG_LEN) {
260  } else {
263  }
264  GX3_packet.status = 0;
265  }
266  break;
267  default:
268  GX3_packet.status = 0;
269  GX3_packet.msg_idx = 0;
270  break;
271  }
272 }
273 
274 void ahrs_init(void) {
276 
277  /* set ltp_to_imu so that body is zero */
278  QUAT_COPY(ahrs_impl.ltp_to_imu_quat, imuf.body_to_imu_quat);
280 
281 #ifdef IMU_MAG_OFFSET
282  ahrs_impl.mag_offset = IMU_MAG_OFFSET;
283 #else
284  ahrs_impl.mag_offset = 0.;
285 #endif
286 
288 }
289 
290 void ahrs_aligner_run(void) {
291 #ifdef AHRS_ALIGNER_LED
292  LED_TOGGLE(AHRS_ALIGNER_LED);
293 #endif
294 
295  if (GX3_freq > GX3_MIN_FREQ) {
297 #ifdef AHRS_ALIGNER_LED
298  LED_ON(AHRS_ALIGNER_LED);
299 #endif
300  }
301 }
302 
303 void ahrs_aligner_init(void) {
304 }
305 
306 void ahrs_propagate(void) {
307 }
308 
309 void ahrs_update_accel(void) {
310 }
311 
312 void ahrs_update_mag(void) {
313 }
314 
315 void ahrs_update_gps(void) {
316 }
#define FLOAT_RATES_ZERO(_r)
#define IMU_DIV1
unsigned short uint16_t
Definition: types.h:16
#define ACC_FILT_DIV
#define GX3Link(_x)
Definition: ahrs_gx3.h:47
rotation matrix
uint8_t msg_idx
Definition: ahrs_gx3.h:90
uint8_t msg_buf[GX3_MAX_PAYLOAD]
Definition: ahrs_gx3.h:88
void ahrs_init(void)
AHRS initialization.
Definition: ahrs_gx3.c:274
Driver for Microstrain GX3 IMU/AHRS subsystem.
float m[3 *3]
angular rates
void ahrs_aligner_init(void)
Definition: ahrs_gx3.c:303
struct FloatRates imu_rate
Rotational velocity in IMU frame.
Definition: ahrs_gx3.h:107
struct FloatRates GX3_rate
Definition: ahrs_gx3.c:53
#define VECT3_SMUL(_vo, _vi, _s)
Definition: pprz_algebra.h:160
void ahrs_update_accel(void)
Update AHRS state with accerleration measurements.
Definition: ahrs_gx3.c:309
void ahrs_aligner_run(void)
Definition: ahrs_gx3.c:290
float GX3_freq
Definition: ahrs_gx3.c:50
#define IMU_GX3_LONG_DELAY
Definition: ahrs_gx3.h:71
struct Ahrs ahrs
global AHRS state
Definition: ahrs.c:30
uint16_t GX3_chksm
Definition: ahrs_gx3.c:48
void ahrs_align(void)
Aligns the AHRS.
Definition: ahrs_gx3.c:85
static void stateSetNedToBodyQuat_f(struct FloatQuat *ned_to_body_quat)
Set vehicle body attitude from quaternion (float).
Definition: state.h:985
struct Int32Vect3 accel
accelerometer measurements
Definition: imu.h:41
#define LED_ON(i)
Definition: led_hw.h:28
GX3Status
Definition: ahrs_gx3.h:98
uint32_t hdr_error
Definition: ahrs_gx3.h:87
#define GX3_TIME(_ubx_payload)
Definition: ahrs_gx3.c:35
uint8_t status
Definition: ahrs_aligner.h:45
struct AhrsFloatQuat ahrs_impl
Definition: ahrs_gx3.c:58
euler angles
void ahrs_update_mag(void)
Update AHRS state with magnetometer measurements.
Definition: ahrs_gx3.c:312
#define IMU_DIV2
#define FALSE
Definition: imu_chimu.h:141
uint8_t status
Definition: ahrs_gx3.h:89
float p
in rad/s^2
float mag_offset
Definition: ahrs_gx3.h:108
#define FLOAT_RMAT_TRANSP_RATEMULT(_vb, _m_b2a, _va)
Roation quaternion.
void GX3_packet_read_message(void)
Definition: ahrs_gx3.c:192
void imu_periodic(void)
Definition: ahrs_gx3.c:185
#define MAG_FILT_DIV
uint16_t chk_calc
Definition: imu_um6.c:46
enum GX3Status GX3_status
Definition: ahrs_gx3.c:45
Architecture independent timing functions.
bool_t msg_available
Definition: ahrs_gx3.h:85
void ahrs_update_gps(void)
Update AHRS state with GPS measurements.
Definition: ahrs_gx3.c:315
struct Imu imu
global IMU state
Definition: imu_aspirin2.c:50
uint8_t status
status of the AHRS, AHRS_UNINIT or AHRS_RUNNING
Definition: ahrs.h:45
unsigned long uint32_t
Definition: types.h:18
#define AHRS_ALIGNER_LOCKED
Definition: ahrs_aligner.h:37
void ahrs_propagate(void)
Propagation.
Definition: ahrs_gx3.c:306
struct FloatRMat GX3_rmat
Definition: ahrs_gx3.c:54
uint32_t GX3_time
Definition: ahrs_gx3.c:46
void imu_impl_init(void)
Definition: ahrs_gx3.c:99
#define LED_TOGGLE(i)
Definition: led_hw.h:30
float r
in rad/s^2
#define AHRS_UNINIT
Definition: ahrs.h:35
#define TRUE
Definition: imu_chimu.h:144
uint32_t GX3_ltime
Definition: ahrs_gx3.c:47
#define GX3_CHKSM(_ubx_payload)
Definition: ahrs_gx3.c:36
struct AhrsAligner ahrs_aligner
Definition: ahrs_gx3.c:59
unsigned char uint8_t
Definition: types.h:14
#define QUAT_COPY(_qo, _qi)
Definition: pprz_algebra.h:476
#define FLOAT_QUAT_OF_RMAT(_q, _r)
uint16_t GX3_calcsm
Definition: ahrs_gx3.c:49
uint32_t chksm_error
Definition: ahrs_gx3.h:86
#define GX3_MSG_LEN
Definition: ahrs_gx3.h:67
struct FloatEulers GX3_euler
Definition: ahrs_gx3.c:56
static float p[2][2]
#define GX3_HEADER
Definition: ahrs_gx3.h:68
struct FloatVect3 GX3_accel
Definition: ahrs_gx3.c:52
float q
in rad/s^2
struct FloatQuat ltp_to_imu_quat
Rotation from LocalTangentPlane to IMU frame as quaternions.
Definition: ahrs_gx3.h:106
#define FLOAT_QUAT_COMP_INV(_a2b, _a2c, _b2c)
static float bef(volatile uint8_t *c)
Definition: ahrs_gx3.c:65
signed char int8_t
Definition: types.h:15
static struct point c
Definition: discsurvey.c:39
static void stateSetBodyRates_f(struct FloatRates *body_rate)
Set vehicle body angular rate (float).
Definition: state.h:1062
void GX3_packet_parse(uint8_t c)
Definition: ahrs_gx3.c:242
#define ACCELS_BFP_OF_REAL(_ef, _ei)
Definition: pprz_algebra.h:647
#define GX3_MIN_FREQ
Definition: ahrs_gx3.h:69
#define AHRS_RUNNING
Definition: ahrs.h:36
struct FloatQuat GX3_quat
Definition: ahrs_gx3.c:55
static bool_t GX3_verify_chk(volatile uint8_t *buff_add)
Definition: ahrs_gx3.c:76