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
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  if (ins.vf_realign == TRUE) {
191  UM6_imu_align();
192  }
193 
194  /* We would request for data here - optional
195  //GET_DATA command 0xAE
196  buf_out[0] = 's';
197  buf_out[1] = 'n';
198  buf_out[2] = 'p';
199  buf_out[3] = 0; // zeros
200  buf_out[4] = 0xAE; // GET_DATA command
201  data_chk = UM6_calculate_checksum(buf_out, 5);
202  buf_out[5] = (uint8_t)(data_chk >> 8);
203  buf_out[6] = (uint8_t)data_chk;
204  UM6_send_packet(buf_out, 7);
205  */
206 }
207 
209  if ((UM6_status == UM6Running) && PacketLength > 11) {
210  switch (PacketAddr) {
211  case IMU_UM6_GYRO_PROC:
212  UM6_rate.p =
213  ((float)((int16_t)
215  UM6_rate.q =
216  ((float)((int16_t)
218  UM6_rate.r =
219  ((float)((int16_t)
221  RATES_SMUL(UM6_rate, UM6_rate, 0.0174532925); //Convert deg/sec to rad/sec
223  break;
224  case IMU_UM6_ACCEL_PROC:
225  UM6_accel.x =
226  ((float)((int16_t)
228  UM6_accel.y =
229  ((float)((int16_t)
231  UM6_accel.z =
232  ((float)((int16_t)
234  VECT3_SMUL(UM6_accel, UM6_accel, 9.80665); //Convert g into m/s2
235  ACCELS_BFP_OF_REAL(imu.accel, UM6_accel); // float to int
236  break;
237  case IMU_UM6_MAG_PROC:
238  UM6_mag.x =
239  ((float)((int16_t)
241  UM6_mag.y =
242  ((float)((int16_t)
244  UM6_mag.z =
245  ((float)((int16_t)
247  // Assume the same units for magnetic field
248  // Magnitude at the Earth's surface ranges from 25 to 65 microteslas (0.25 to 0.65 gauss).
250  break;
251  case IMU_UM6_EULER:
252  UM6_eulers.phi =
253  ((float)((int16_t)
255  UM6_eulers.theta =
256  ((float)((int16_t)
258  UM6_eulers.psi =
259  ((float)((int16_t)
261  EULERS_SMUL(UM6_eulers,UM6_eulers, 0.0174532925); //Convert deg to rad
262  EULERS_BFP_OF_REAL(ahrs_impl.ltp_to_imu_euler, UM6_eulers);
263  break;
264  case IMU_UM6_QUAT:
265  UM6_quat.qi =
266  ((float)((int16_t)
268  UM6_quat.qx =
269  ((float)((int16_t)
271  UM6_quat.qy =
272  ((float)((int16_t)
274  UM6_quat.qz =
275  ((float)((int16_t)
278  break;
279  default:
280  break;
281  }
282  }
283 }
284 
285 /* UM6 Packet Collection */
287  switch (UM6_packet.status) {
288  case UM6PacketWaiting:
289  UM6_packet.msg_idx = 0;
290  if (c == 's') {
294  } else {
297  }
298  break;
299  case UM6PacketReadingS:
300  if (c == 'n') {
304  } else {
307  }
308  break;
309  case UM6PacketReadingN:
310  if (c == 'p') {
314  } else {
317  }
318  break;
319  case UM6PacketReadingPT:
320  PacketType = c;
324  if ((PacketType & 0xC0) == 0xC0) {
325  PacketLength = 4*((PacketType >>2) & 0xF) + 7; // Batch, has 4*BatchLength bytes of data
326  }
327  else if ((PacketType & 0xC0) == 0x80) {
328  PacketLength = 11; // Not batch, has 4 bytes of data
329  }
330  else if ((PacketType & 0xC0) == 0x00) {
331  PacketLength = 7; // Not batch, no data
332  }
333  break;
335  PacketAddr = c;
339  break;
346  } else {
349  }
351  }
352  break;
353  default:
355  UM6_packet.msg_idx = 0;
356  break;
357  }
358 }
struct Ins ins
global INS state
Definition: ins.c:30
unsigned short uint16_t
Definition: types.h:16
void UM6_packet_read_message(void)
Definition: imu_um6.c:208
#define MAGS_BFP_OF_REAL(_ef, _ei)
Definition: pprz_algebra.h:659
uint8_t PacketLength
Definition: imu_um6.c:41
#define EULERS_SMUL(_eo, _ei, _s)
Definition: pprz_algebra.h:269
#define IMU_UM6_MAG_PROC
Definition: imu_um6.h:78
uint8_t msg_buf[IMU_UM6_BUFFER_LENGTH]
Definition: imu_um6.h:104
#define QUAT_BFP_OF_REAL(_qi, _qf)
Definition: pprz_algebra.h:610
#define IMU_UM6_QUAT
Definition: imu_um6.h:80
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:160
uint8_t buf_out[IMU_UM6_BUFFER_LENGTH]
Definition: imu_um6.c:38
struct Int32Vect3 accel
accelerometer measurements
Definition: imu.h:41
void UM6_imu_align(void)
Definition: imu_um6.c:80
uint8_t msg_idx
Definition: imu_um6.h:106
float theta
in radians
euler angles
struct Int32Quat ltp_to_imu_quat
Definition: ahrs_ardrone2.h:46
UM6Status
Definition: imu_um6.h:118
#define RATES_SMUL(_ro, _ri, _s)
Definition: pprz_algebra.h:343
#define IMU_UM6_DATA_OFFSET
Definition: imu_um6.h:64
#define FALSE
Definition: imu_chimu.h:141
#define IMU_UM6_ZERO_GYROS_CMD
Definition: imu_um6.h:70
float p
in rad/s^2
#define IMU_UM6_BUFFER_LENGTH
Definition: imu_um6.h:63
Roation quaternion.
struct FloatVect3 UM6_mag
Definition: imu_um6.c:51
#define IMU_UM6_RESET_EKF_CMD
Definition: imu_um6.h:71
#define UM6Link(_x)
Definition: imu_um6.h:44
uint16_t chk_calc
Definition: imu_um6.c:46
#define IMU_UM6_GYRO_PROC
Definition: imu_um6.h:76
float phi
in radians
#define RATES_BFP_OF_REAL(_ri, _rf)
Definition: pprz_algebra.h:623
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:50
signed short int16_t
Definition: types.h:17
void imu_periodic(void)
Definition: imu_um6.c:189
uint8_t PacketAddr
Definition: imu_um6.c:43
bool_t msg_available
Definition: imu_um6.h:101
uint8_t PacketType
Definition: imu_um6.c:42
#define IMU_UM6_SET_MAG_REF
Definition: imu_um6.h:74
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^2
#define TRUE
Definition: imu_chimu.h:144
uint8_t status
Definition: imu_um6.h:105
struct Int32Vect3 mag
magnetometer measurements
Definition: imu.h:42
#define IMU_UM6_COMMUNICATION_REG
Definition: imu_um6.h:67
uint16_t data_chk
Definition: imu_um6.c:39
#define IMU_UM6_SET_ACCEL_REF
Definition: imu_um6.h:73
unsigned char uint8_t
Definition: types.h:14
uint32_t hdr_error
Definition: imu_um6.h:103
void UM6_packet_parse(uint8_t c)
Definition: imu_um6.c:286
#define IMU_UM6_EULER
Definition: imu_um6.h:79
void imu_impl_init(void)
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:102
struct FloatQuat UM6_quat
Definition: imu_um6.c:53
struct Int32Rates gyro
gyroscope measurements
Definition: imu.h:40
float q
in rad/s^2
bool_t vf_realign
realign vertically if true
Definition: ins.h:47
#define IMU_UM6_MISC_CONFIG_REG
Definition: imu_um6.h:68
Driver for CH Robotics UM6 IMU/AHRS subsystem.
static struct point c
Definition: discsurvey.c:39
#define EULERS_BFP_OF_REAL(_ei, _ef)
Definition: pprz_algebra.h:573
struct AhrsARDrone ahrs_impl
Definition: ahrs_ardrone2.c:40
#define ACCELS_BFP_OF_REAL(_ef, _ei)
Definition: pprz_algebra.h:647
#define IMU_UM6_ACCEL_PROC
Definition: imu_um6.h:77
bool_t UM6_verify_chk(uint8_t packet_buffer[], uint8_t packet_length)
Definition: imu_um6.c:60