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
imu_um6.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 "subsystems/imu/imu_um6.h"
34 #include "subsystems/imu.h"
35 #include "mcu_periph/sys_time.h"
36 
40 
45 
48 
54 
55 inline void UM6_imu_align(void);
56 inline void UM6_send_packet(uint8_t *packet_buffer, uint8_t packet_length);
57 inline uint16_t UM6_calculate_checksum(uint8_t packet_buffer[], uint8_t packet_length);
58 inline bool_t UM6_verify_chk(uint8_t packet_buffer[], uint8_t packet_length);
59 
60 inline bool_t UM6_verify_chk(uint8_t packet_buffer[], uint8_t packet_length) {
61  chk_rec = (packet_buffer[packet_length-2] << 8) | packet_buffer[packet_length-1];
62  chk_calc = UM6_calculate_checksum(packet_buffer, packet_length-2);
63  return (chk_calc == chk_rec);
64 }
65 
66 inline uint16_t UM6_calculate_checksum(uint8_t packet_buffer[], uint8_t packet_length) {
67  uint16_t chk = 0;
68  for (int i=0; i<packet_length; i++) {
69  chk += packet_buffer[i];
70  }
71  return chk;
72 }
73 
74 inline void UM6_send_packet(uint8_t *packet_buffer, uint8_t packet_length) {
75  for (int i=0; i<packet_length; i++) {
76  UM6Link(Transmit(packet_buffer[i]));
77  }
78 }
79 
80 void UM6_imu_align(void) {
82 
83  // Acceleration vector realign
84  buf_out[0] = 's';
85  buf_out[1] = 'n';
86  buf_out[2] = 'p';
87  buf_out[3] = 0;
90  buf_out[5] = (uint8_t)(data_chk >> 8);
91  buf_out[6] = (uint8_t)data_chk;
93 
94  // Magnetic realign
95  buf_out[0] = 's';
96  buf_out[1] = 'n';
97  buf_out[2] = 'p';
98  buf_out[3] = 0;
101  buf_out[5] = (uint8_t)(data_chk >> 8);
102  buf_out[6] = (uint8_t)data_chk;
104 
105  // Zero gyros 0xAC, takes around 3s
106  buf_out[0] = 's';
107  buf_out[1] = 'n';
108  buf_out[2] = 'p';
109  buf_out[3] = 0;
112  buf_out[5] = (uint8_t)(data_chk >> 8);
113  buf_out[6] = (uint8_t)data_chk;
115 
116  // Reset EKF 0xAD
117  buf_out[0] = 's';
118  buf_out[1] = 'n';
119  buf_out[2] = 'p';
120  buf_out[3] = 0;
123  buf_out[5] = (uint8_t)(data_chk >> 8);
124  buf_out[6] = (uint8_t)data_chk;
126 
128 }
129 
130 void imu_impl_init(void) {
131  // Initialize variables
133 
134  // Initialize packet
136  UM6_packet.msg_idx = 0;
139  UM6_packet.hdr_error = 0;
140 
141  // Communication register 0x00
142  buf_out[0] = 's';
143  buf_out[1] = 'n';
144  buf_out[2] = 'p';
145  buf_out[3] = 0x80; // has data, zeros
146  buf_out[4] = IMU_UM6_COMMUNICATION_REG; // communication register address
147  buf_out[5] = 0b01000110; // B3 - broadcast enabled, processed data enabled (acc, gyro, mag)
148  buf_out[6] = 0b10000000; // B2 - no euler angles, quaternions enabled
149  buf_out[7] = 0b00101101; // B1 - baud 115200
150  buf_out[8] = 100; // B0 - broadcast rate, 1=20Hz, 100=130Hz, 255=300Hz
152  buf_out[9] = (uint8_t)(data_chk >> 8);
153  buf_out[10] = (uint8_t)data_chk;
155 
156  // Config register 0x01
157  buf_out[0] = 's';
158  buf_out[1] = 'n';
159  buf_out[2] = 'p';
160  buf_out[3] = 0x80; // has data, zeros
161  buf_out[4] = IMU_UM6_MISC_CONFIG_REG; // config register address
162  buf_out[5] = 0b11110000; // B3 - mag, accel updates enabled, use quaternions
163  buf_out[6] = 0; // B2 - RES
164  buf_out[7] = 0; // B1 - RES
165  buf_out[8] = 0; // B0 - RES
167  buf_out[9] = (uint8_t)(data_chk >> 8);
168  buf_out[10] = (uint8_t)data_chk;
170 
171  /*
172  // Get firmware
173  buf_out[0] = 's';
174  buf_out[1] = 'n';
175  buf_out[2] = 'p';
176  buf_out[3] = 0;
177  buf_out[4] = IMU_UM6_GET_FIRMWARE_CMD;
178  data_chk = UM6_calculate_checksum(buf_out, 5);
179  buf_out[5] = (uint8_t)(data_chk >> 8);
180  buf_out[6] = (uint8_t)data_chk;
181  UM6_send_packet(buf_out, 7);
182  */
183 
184  UM6_imu_align();
185 }
186 
187 
188 
189 void imu_periodic(void) {
190  /* We would request for data here - optional
191  //GET_DATA command 0xAE
192  buf_out[0] = 's';
193  buf_out[1] = 'n';
194  buf_out[2] = 'p';
195  buf_out[3] = 0; // zeros
196  buf_out[4] = 0xAE; // GET_DATA command
197  data_chk = UM6_calculate_checksum(buf_out, 5);
198  buf_out[5] = (uint8_t)(data_chk >> 8);
199  buf_out[6] = (uint8_t)data_chk;
200  UM6_send_packet(buf_out, 7);
201  */
202 }
203 
205  if ((UM6_status == UM6Running) && PacketLength > 11) {
206  switch (PacketAddr) {
207  case IMU_UM6_GYRO_PROC:
208  UM6_rate.p =
209  ((float)((int16_t)
211  UM6_rate.q =
212  ((float)((int16_t)
214  UM6_rate.r =
215  ((float)((int16_t)
217  RATES_SMUL(UM6_rate, UM6_rate, 0.0174532925); //Convert deg/sec to rad/sec
219  break;
220  case IMU_UM6_ACCEL_PROC:
221  UM6_accel.x =
222  ((float)((int16_t)
224  UM6_accel.y =
225  ((float)((int16_t)
227  UM6_accel.z =
228  ((float)((int16_t)
230  VECT3_SMUL(UM6_accel, UM6_accel, 9.80665); //Convert g into m/s2
231  ACCELS_BFP_OF_REAL(imu.accel, UM6_accel); // float to int
232  break;
233  case IMU_UM6_MAG_PROC:
234  UM6_mag.x =
235  ((float)((int16_t)
237  UM6_mag.y =
238  ((float)((int16_t)
240  UM6_mag.z =
241  ((float)((int16_t)
243  // Assume the same units for magnetic field
244  // Magnitude at the Earth's surface ranges from 25 to 65 microteslas (0.25 to 0.65 gauss).
246  break;
247  case IMU_UM6_EULER:
248  UM6_eulers.phi =
249  ((float)((int16_t)
251  UM6_eulers.theta =
252  ((float)((int16_t)
254  UM6_eulers.psi =
255  ((float)((int16_t)
257  EULERS_SMUL(UM6_eulers,UM6_eulers, 0.0174532925); //Convert deg to rad
258  EULERS_BFP_OF_REAL(ahrs_impl.ltp_to_imu_euler, UM6_eulers);
259  break;
260  case IMU_UM6_QUAT:
261  UM6_quat.qi =
262  ((float)((int16_t)
264  UM6_quat.qx =
265  ((float)((int16_t)
267  UM6_quat.qy =
268  ((float)((int16_t)
270  UM6_quat.qz =
271  ((float)((int16_t)
274  break;
275  default:
276  break;
277  }
278  }
279 }
280 
281 /* UM6 Packet Collection */
283  switch (UM6_packet.status) {
284  case UM6PacketWaiting:
285  UM6_packet.msg_idx = 0;
286  if (c == 's') {
290  } else {
293  }
294  break;
295  case UM6PacketReadingS:
296  if (c == 'n') {
300  } else {
303  }
304  break;
305  case UM6PacketReadingN:
306  if (c == 'p') {
310  } else {
313  }
314  break;
315  case UM6PacketReadingPT:
316  PacketType = c;
320  if ((PacketType & 0xC0) == 0xC0) {
321  PacketLength = 4*((PacketType >>2) & 0xF) + 7; // Batch, has 4*BatchLength bytes of data
322  }
323  else if ((PacketType & 0xC0) == 0x80) {
324  PacketLength = 11; // Not batch, has 4 bytes of data
325  }
326  else if ((PacketType & 0xC0) == 0x00) {
327  PacketLength = 7; // Not batch, no data
328  }
329  break;
331  PacketAddr = c;
335  break;
342  } else {
345  }
347  }
348  break;
349  default:
351  UM6_packet.msg_idx = 0;
352  break;
353  }
354 }
unsigned short uint16_t
Definition: types.h:16
void UM6_packet_read_message(void)
Definition: imu_um6.c:204
#define MAGS_BFP_OF_REAL(_ef, _ei)
Definition: pprz_algebra.h:726
uint8_t PacketLength
Definition: imu_um6.c:41
#define EULERS_SMUL(_eo, _ei, _s)
Definition: pprz_algebra.h:292
#define IMU_UM6_MAG_PROC
Definition: imu_um6.h:77
uint8_t msg_buf[IMU_UM6_BUFFER_LENGTH]
Definition: imu_um6.h:103
#define QUAT_BFP_OF_REAL(_qi, _qf)
Definition: pprz_algebra.h:665
#define IMU_UM6_QUAT
Definition: imu_um6.h:79
void UM6_send_packet(uint8_t *packet_buffer, uint8_t packet_length)
Definition: imu_um6.c:74
angular rates
struct FloatRates UM6_rate
Definition: imu_um6.c:50
float psi
in radians
#define VECT3_SMUL(_vo, _vi, _s)
Definition: pprz_algebra.h:178
uint8_t buf_out[IMU_UM6_BUFFER_LENGTH]
Definition: imu_um6.c:38
struct Int32Vect3 accel
accelerometer measurements in m/s^2 in BFP with INT32_ACCEL_FRAC
Definition: imu.h:42
void UM6_imu_align(void)
Definition: imu_um6.c:80
uint8_t msg_idx
Definition: imu_um6.h:105
float theta
in radians
euler angles
struct Int32Quat ltp_to_imu_quat
Definition: ahrs_ardrone2.h:46
UM6Status
Definition: imu_um6.h:117
#define RATES_SMUL(_ro, _ri, _s)
Definition: pprz_algebra.h:368
#define IMU_UM6_DATA_OFFSET
Definition: imu_um6.h:63
#define FALSE
Definition: imu_chimu.h:141
#define IMU_UM6_ZERO_GYROS_CMD
Definition: imu_um6.h:69
float p
in rad/s
#define IMU_UM6_BUFFER_LENGTH
Definition: imu_um6.h:62
Roation quaternion.
struct FloatVect3 UM6_mag
Definition: imu_um6.c:51
#define IMU_UM6_RESET_EKF_CMD
Definition: imu_um6.h:70
#define UM6Link(_x)
Definition: imu_um6.h:43
uint16_t chk_calc
Definition: imu_um6.c:46
#define IMU_UM6_GYRO_PROC
Definition: imu_um6.h:75
float phi
in radians
#define RATES_BFP_OF_REAL(_ri, _rf)
Definition: pprz_algebra.h:678
enum UM6Status UM6_status
Definition: imu_um6.c:44
Architecture independent timing functions.
struct FloatVect3 UM6_accel
Definition: imu_um6.c:49
struct Imu imu
global IMU state
Definition: imu_aspirin2.c:47
signed short int16_t
Definition: types.h:17
void imu_periodic(void)
Handle all the periodic tasks of the Navstik IMU components.
Definition: imu_um6.c:189
uint8_t PacketAddr
Definition: imu_um6.c:43
bool_t msg_available
Definition: imu_um6.h:100
uint8_t PacketType
Definition: imu_um6.c:42
#define IMU_UM6_SET_MAG_REF
Definition: imu_um6.h:73
Inertial Measurement Unit interface.
struct UM6Packet UM6_packet
Definition: imu_um6.c:37
uint16_t chk_rec
Definition: imu_um6.c:47
float r
in rad/s
#define TRUE
Definition: imu_chimu.h:144
uint8_t status
Definition: imu_um6.h:104
struct Int32Vect3 mag
magnetometer measurements scaled to 1 in BFP with INT32_MAG_FRAC
Definition: imu.h:43
#define IMU_UM6_COMMUNICATION_REG
Definition: imu_um6.h:66
uint16_t data_chk
Definition: imu_um6.c:39
#define IMU_UM6_SET_ACCEL_REF
Definition: imu_um6.h:72
unsigned char uint8_t
Definition: types.h:14
uint32_t hdr_error
Definition: imu_um6.h:102
void UM6_packet_parse(uint8_t c)
Definition: imu_um6.c:282
#define IMU_UM6_EULER
Definition: imu_um6.h:78
void imu_impl_init(void)
Navstik IMU initializtion of the MPU-60x0 and HMC58xx.
Definition: imu_um6.c:130
struct FloatEulers UM6_eulers
Definition: imu_um6.c:52
uint16_t UM6_calculate_checksum(uint8_t packet_buffer[], uint8_t packet_length)
Definition: imu_um6.c:66
uint32_t chksm_error
Definition: imu_um6.h:101
struct FloatQuat UM6_quat
Definition: imu_um6.c:53
struct Int32Rates gyro
gyroscope measurements in rad/s in BFP with INT32_RATE_FRAC
Definition: imu.h:41
float q
in rad/s
#define IMU_UM6_MISC_CONFIG_REG
Definition: imu_um6.h:67
Driver for CH Robotics UM6 IMU/AHRS subsystem.
#define EULERS_BFP_OF_REAL(_ei, _ef)
Definition: pprz_algebra.h:628
struct AhrsARDrone ahrs_impl
Definition: ahrs_ardrone2.c:45
#define ACCELS_BFP_OF_REAL(_ef, _ei)
Definition: pprz_algebra.h:714
#define IMU_UM6_ACCEL_PROC
Definition: imu_um6.h:76
bool_t UM6_verify_chk(uint8_t packet_buffer[], uint8_t packet_length)
Definition: imu_um6.c:60