Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
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 
34 #ifdef AHRS_UPDATE_FW_ESTIMATOR
35 // remotely settable
36 #ifndef INS_ROLL_NEUTRAL_DEFAULT
37 #define INS_ROLL_NEUTRAL_DEFAULT 0
38 #endif
39 #ifndef INS_PITCH_NEUTRAL_DEFAULT
40 #define INS_PITCH_NEUTRAL_DEFAULT 0
41 #endif
44 #endif
45 
46 #define GX3_CHKSM(_ubx_payload) (uint16_t)((uint16_t)(*((uint8_t*)_ubx_payload+66+1))|(uint16_t)(*((uint8_t*)_ubx_payload+66+0))<<8)
47 
48 /*
49  * Axis definition: X axis pointing forward, Y axis pointing to the right and Z axis pointing down.
50  * Positive pitch : nose up
51  * Positive roll : right wing down
52  * Positive yaw : clockwise
53  */
56 
57 static inline bool_t gx3_verify_chk(volatile uint8_t *buff_add);
58 static inline float bef(volatile uint8_t *c);
59 
60 /* Big Endian to Float */
61 static inline float bef(volatile uint8_t *c) {
62  float f;
63  int8_t * p;
64  p = ((int8_t *)&f)+3;
65  *p-- = *c++;
66  *p-- = *c++;
67  *p-- = *c++;
68  *p = *c;
69  return f;
70 }
71 
72 static inline bool_t gx3_verify_chk(volatile uint8_t *buff_add) {
74  chk_calc = 0;
75  for (i=0;i<GX3_MSG_LEN-2;i++) {
76  chk_calc += (uint8_t)*buff_add++;
77  }
78  return (chk_calc == ( (((uint16_t)*buff_add)<<8) + (uint8_t)*(buff_add+1) ));
79 }
80 
81 void ahrs_align(void) {
83 
84  //make the gyros zero, takes 10s (specified in Byte 4 and 5)
85  uart_transmit(&GX3_PORT, 0xcd);
86  uart_transmit(&GX3_PORT, 0xc1);
87  uart_transmit(&GX3_PORT, 0x29);
88  uart_transmit(&GX3_PORT, 0x27);
89  uart_transmit(&GX3_PORT, 0x10);
90 
92 }
93 
94 #if PERIODIC_TELEMETRY
96 
97 static send_gx3(void) {
98  DOWNLINK_SEND_GX3_INFO(DefaultChannel, DefaultDevice,
103 }
104 #endif
105 
106 /*
107  * GX3 can be set up during the startup, or it can be configured to
108  * start sending data automatically after power up.
109  */
110 void imu_impl_init(void) {
111  // Initialize variables
113 
114  // Initialize packet
120 
121  // It is necessary to wait for GX3 to power up for proper initialization
122  for (uint32_t startup_counter=0; startup_counter<IMU_GX3_LONG_DELAY*2; startup_counter++){
123  __asm("nop");
124  }
125 
126 #ifdef GX3_INITIALIZE_DURING_STARTUP
127 #pragma message "GX3 initializing"
128  /*
129  // FOR NON-CONTINUOUS MODE UNCOMMENT THIS
130  //4 byte command for non-Continous Mode so we can set the other settings
131  uart_transmit(&GX3_PORT, 0xc4);
132  uart_transmit(&GX3_PORT, 0xc1);
133  uart_transmit(&GX3_PORT, 0x29);
134  uart_transmit(&GX3_PORT, 0x00); // stop
135  */
136 
137  //Sampling Settings (0xDB)
138  uart_transmit(&GX3_PORT, 0xdb); //set update speed
139  uart_transmit(&GX3_PORT, 0xa8);
140  uart_transmit(&GX3_PORT, 0xb9);
141  //set rate of IMU link, is 1000/IMU_DIV
142 #define IMU_DIV1 0
143 #define IMU_DIV2 2
144 #define ACC_FILT_DIV 2
145 #define MAG_FILT_DIV 30
146 #ifdef GX3_SAVE_SETTINGS
147  uart_transmit(&GX3_PORT, 0x02);//set params and save them in non-volatile memory
148 #else
149  uart_transmit(&GX3_PORT, 0x02); //set and don't save
150 #endif
151  uart_transmit(&GX3_PORT, IMU_DIV1);
152  uart_transmit(&GX3_PORT, IMU_DIV2);
153  uart_transmit(&GX3_PORT, 0b00000000); //set options byte 8 - GOOD
154  uart_transmit(&GX3_PORT, 0b00000011); //set options byte 7 - GOOD
155  //0 - calculate orientation, 1 - enable coning & sculling, 2-3 reserved, 4 - no little endian data,
156  // 5 - no NaN supressed, 6 - disable finite size correction, 7 - reserved,
157  // 8 - enable magnetometer, 9 - reserved, 10 - enable magnetic north compensation, 11 - enable gravity compensation
158  // 12 - no quaternion calculation, 13-15 reserved
159  uart_transmit(&GX3_PORT, ACC_FILT_DIV);
160  uart_transmit(&GX3_PORT, MAG_FILT_DIV); //mag window filter size == 33hz
161  uart_transmit(&GX3_PORT, 0x00);
162  uart_transmit(&GX3_PORT, 10); // Up Compensation in secs, def=10s
163  uart_transmit(&GX3_PORT, 0x00);
164  uart_transmit(&GX3_PORT, 10); // North Compensation in secs
165  uart_transmit(&GX3_PORT, 0x00); //power setting = 0, high power/bw
166  uart_transmit(&GX3_PORT, 0x00); //rest of the bytes are 0
167  uart_transmit(&GX3_PORT, 0x00);
168  uart_transmit(&GX3_PORT, 0x00);
169  uart_transmit(&GX3_PORT, 0x00);
170  uart_transmit(&GX3_PORT, 0x00);
171 
172  // OPTIONAL: realign up and north
173  /*
174  uart_transmit(&GX3_PORT, 0xdd);
175  uart_transmit(&GX3_PORT, 0x54);
176  uart_transmit(&GX3_PORT, 0x4c);
177  uart_transmit(&GX3_PORT, 3);
178  uart_transmit(&GX3_PORT, 10);
179  uart_transmit(&GX3_PORT, 10);
180  uart_transmit(&GX3_PORT, 0x00);
181  uart_transmit(&GX3_PORT, 0x00);
182  uart_transmit(&GX3_PORT, 0x00);
183  uart_transmit(&GX3_PORT, 0x00);
184  */
185 
186  //Another wait loop for proper GX3 init
187  for (uint32_t startup_counter=0; startup_counter<IMU_GX3_LONG_DELAY; startup_counter++){
188  __asm("nop");
189  }
190 
191 #ifdef GX3_SET_WAKEUP_MODE
192  //Mode Preset (0xD5)
193  uart_transmit(&GX3_PORT, 0xD5);
194  uart_transmit(&GX3_PORT, 0xBA);
195  uart_transmit(&GX3_PORT, 0x89);
196  uart_transmit(&GX3_PORT, 0x02); // wake up in continuous mode
197 
198  //Continuous preset (0xD6)
199  uart_transmit(&GX3_PORT, 0xD6);
200  uart_transmit(&GX3_PORT, 0xC6);
201  uart_transmit(&GX3_PORT, 0x6B);
202  uart_transmit(&GX3_PORT, 0xc8); // accel, gyro, R
203 #endif
204 
205  //4 byte command for Continous Mode
206  uart_transmit(&GX3_PORT, 0xc4);
207  uart_transmit(&GX3_PORT, 0xc1);
208  uart_transmit(&GX3_PORT, 0x29);
209  uart_transmit(&GX3_PORT, 0xc8); // accel,gyro,R
210 
211  // Reset gyros to zero
212  ahrs_align();
213 #endif
215 
216 #if PERIODIC_TELEMETRY
217  register_periodic_telemetry(DefaultPeriodic, "GX3_INFO", send_gx3);
218 #endif
219 }
220 
221 
222 void imu_periodic(void) {
223  /* IF IN NON-CONTINUOUS MODE, REQUEST DATA NOW
224  uart_transmit(&GX3_PORT, 0xc8); // accel,gyro,R
225  */
226 }
227 
228 
248 
249  ahrs_impl.gx3_freq = 62500.0 / (float)(ahrs_impl.gx3_time - ahrs_impl.gx3_ltime);
251 
252  // Acceleration
253  VECT3_SMUL(ahrs_impl.gx3_accel, ahrs_impl.gx3_accel, 9.80665); // Convert g into m/s2
254  ACCELS_BFP_OF_REAL(imu.accel, ahrs_impl.gx3_accel); // for backwards compatibility with fixed point interface
256 
257  // Rates
258  struct FloatRates body_rate;
260  /* compute body rates */
261  struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&imuf.body_to_imu);
262  FLOAT_RMAT_TRANSP_RATEMULT(body_rate, *body_to_imu_rmat, imuf.gyro);
263  /* Set state */
264  stateSetBodyRates_f(&body_rate);
265 
266  // Attitude
267  struct FloatRMat ltp_to_body_rmat;
268  FLOAT_RMAT_COMP(ltp_to_body_rmat, ahrs_impl.gx3_rmat, *body_to_imu_rmat);
269 #ifdef AHRS_UPDATE_FW_ESTIMATOR // fixedwing
270  struct FloatEulers ltp_to_body_eulers;
271  FLOAT_EULERS_OF_RMAT(ltp_to_body_eulers, ltp_to_body_rmat);
272  ltp_to_body_eulers.phi -= ins_roll_neutral;
273  ltp_to_body_eulers.theta -= ins_pitch_neutral;
274 #if AHRS_USE_GPS_HEADING && USE_GPS
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 #endif
281  stateSetNedToBodyEulers_f(&ltp_to_body_eulers);
282 #else
283 #ifdef IMU_MAG_OFFSET //rotorcraft
284  struct FloatEulers ltp_to_body_eulers;
285  FLOAT_EULERS_OF_RMAT(ltp_to_body_eulers, ltp_to_body_rmat);
286  ltp_to_body_eulers.psi -= ahrs_impl.mag_offset;
287  stateSetNedToBodyEulers_f(&ltp_to_body_eulers);
288 #else
289  stateSetNedToBodyRMat_f(&ltp_to_body_rmat);
290 #endif
291 #endif
292 }
293 
294 
295 /* GX3 Packet Collection */
297  switch (ahrs_impl.gx3_packet.status) {
298  case GX3PacketWaiting:
300  if (c == GX3_HEADER) {
304  } else {
306  }
307  break;
308  case GX3PacketReading:
314  } else {
317  }
319  }
320  break;
321  default:
324  break;
325  }
326 }
327 
328 void ahrs_init(void) {
330  /* set ltp_to_imu so that body is zero */
331  struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&imuf.body_to_imu);
332  QUAT_COPY(ahrs_impl.ltp_to_imu_quat, *body_to_imu_quat);
333 #ifdef IMU_MAG_OFFSET
334  ahrs_impl.mag_offset = IMU_MAG_OFFSET;
335 #else
336  ahrs_impl.mag_offset = 0.0;
337 #endif
339 }
340 
341 void ahrs_aligner_run(void) {
342 #ifdef AHRS_ALIGNER_LED
343  LED_ON(AHRS_ALIGNER_LED);
344 #endif
346 }
347 
348 
349 void ahrs_aligner_init(void) {
350 }
351 
unsigned short uint16_t
Definition: types.h:16
static void stateSetNedToBodyRMat_f(struct FloatRMat *ned_to_body_rmat)
Set vehicle body attitude from rotation matrix (float).
Definition: state.h:990
static void stateSetNedToBodyEulers_f(struct FloatEulers *ned_to_body_eulers)
Set vehicle body attitude from euler angles (float).
Definition: state.h:995
rotation matrix
int32_t course
GPS course over ground in rad*1e7, [0, 2*Pi]*1e7 (CW/north)
Definition: gps.h:72
struct OrientationReps body_to_imu
rotation from body to imu frame
Definition: imu.h:66
uint8_t status
Definition: ahrs_gx3.h:76
void ahrs_init(void)
AHRS initialization.
Definition: ahrs_gx3.c:328
struct FloatRates gx3_rate
measured angular rates in IMU frame
Definition: ahrs_gx3.h:102
Driver for Microstrain GX3 IMU/AHRS subsystem.
Periodic telemetry system header (includes downlink utility and generated code).
float m[3 *3]
struct GX3Packet gx3_packet
Packet struct.
Definition: ahrs_gx3.h:95
angular rates
void ahrs_aligner_init(void)
Definition: ahrs_gx3.c:349
float psi
in radians
#define VECT3_SMUL(_vo, _vi, _s)
Definition: pprz_algebra.h:178
static struct FloatRMat * orientationGetRMat_f(struct OrientationReps *orientation)
Get vehicle body attitude rotation matrix (float).
struct FloatRates gyro
Definition: imu.h:62
void ahrs_aligner_run(void)
Definition: ahrs_gx3.c:341
#define IMU_GX3_LONG_DELAY
Definition: ahrs_gx3.h:66
struct Ahrs ahrs
global AHRS state
Definition: ahrs.c:30
void ahrs_align(void)
Aligns the AHRS.
Definition: ahrs_gx3.c:81
#define INS_ROLL_NEUTRAL_DEFAULT
float gx3_freq
data frequency
Definition: ahrs_gx3.h:97
struct Int32Vect3 accel
accelerometer measurements in m/s^2 in BFP with INT32_ACCEL_FRAC
Definition: imu.h:42
#define LED_ON(i)
Definition: led_hw.h:28
#define FLOAT_RMAT_COMP(_m_a2c, _m_a2b, _m_b2c)
uint8_t status
Definition: ahrs_aligner.h:45
struct ImuFloat imuf
Definition: imu.c:111
float theta
in radians
struct AhrsFloatQuat ahrs_impl
Definition: ahrs_gx3.c:54
void uart_transmit(struct uart_periph *p, uint8_t data)
Definition: uart_arch.c:75
#define FLOAT_EULERS_OF_RMAT(_e, _rm)
bool_t register_periodic_telemetry(struct pprz_telemetry *_pt, const char *_msg, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:38
euler angles
#define FALSE
Definition: imu_chimu.h:141
float p
in rad/s
float mag_offset
Difference between true and magnetic north.
Definition: ahrs_gx3.h:93
#define FLOAT_RMAT_TRANSP_RATEMULT(_vb, _m_b2a, _va)
static bool_t gx3_verify_chk(volatile uint8_t *buff_add)
Definition: ahrs_gx3.c:72
Roation quaternion.
void imu_periodic(void)
Definition: ahrs_gx3.c:222
struct FloatVect3 accel
Definition: imu.h:63
uint16_t chk_calc
Definition: imu_um6.c:46
float phi
in radians
void gx3_packet_parse(uint8_t c)
Definition: ahrs_gx3.c:296
struct Imu imu
global IMU state
Definition: imu_aspirin2.c:47
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 INS_PITCH_NEUTRAL_DEFAULT
#define AHRS_ALIGNER_LOCKED
Definition: ahrs_aligner.h:37
uint32_t hdr_error
Definition: ahrs_gx3.h:74
void imu_impl_init(void)
Definition: ahrs_gx3.c:110
uint8_t msg_buf[GX3_MAX_PAYLOAD]
Definition: ahrs_gx3.h:75
bool_t msg_available
Definition: ahrs_gx3.h:72
float ins_pitch_neutral
Definition: ins_arduimu.c:15
float r
in rad/s
#define AHRS_UNINIT
Definition: ahrs.h:35
#define TRUE
Definition: imu_chimu.h:144
uint32_t gx3_time
GX3 time stamp.
Definition: ahrs_gx3.h:99
enum GX3Status gx3_status
GX3 status.
Definition: ahrs_gx3.h:96
struct FloatRMat gx3_rmat
measured attitude in IMU frame (rotational matrix)
Definition: ahrs_gx3.h:103
#define GX3_CHKSM(_ubx_payload)
Definition: ahrs_gx3.c:46
struct AhrsAligner ahrs_aligner
Definition: ahrs_gx3.c:55
unsigned char uint8_t
Definition: types.h:14
#define QUAT_COPY(_qo, _qi)
Definition: pprz_algebra.h:512
uint8_t msg_idx
Definition: ahrs_gx3.h:77
uint16_t gx3_chksm
aux variable for checksum
Definition: ahrs_gx3.h:98
#define GX3_MSG_LEN
Definition: ahrs_gx3.h:62
static float p[2][2]
void gx3_packet_read_message(void)
Definition: ahrs_gx3.c:229
#define GX3_HEADER
Definition: ahrs_gx3.h:63
float q
in rad/s
struct FloatQuat ltp_to_imu_quat
Rotation from LocalTangentPlane to IMU frame as quaternions.
Definition: ahrs_gx3.h:92
float ins_roll_neutral
Definition: ins_arduimu.c:14
static float bef(volatile uint8_t *c)
Definition: ahrs_gx3.c:61
signed char int8_t
Definition: types.h:15
static void stateSetBodyRates_f(struct FloatRates *body_rate)
Set vehicle body angular rate (float).
Definition: state.h:1062
uint32_t gx3_ltime
aux time stamp
Definition: ahrs_gx3.h:100
#define ACCELS_BFP_OF_REAL(_ef, _ei)
Definition: pprz_algebra.h:714
struct GpsState gps
global GPS state
Definition: gps.c:41
struct FloatVect3 gx3_accel
measured acceleration in IMU frame
Definition: ahrs_gx3.h:101
#define AHRS_RUNNING
Definition: ahrs.h:36
uint32_t chksm_error
Definition: ahrs_gx3.h:73
static struct FloatQuat * orientationGetQuat_f(struct OrientationReps *orientation)
Get vehicle body attitude quaternion (float).