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
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 "subsystems/abi.h"
36 #include "mcu_periph/sys_time.h"
37 
41 
46 
49 
55 
56 inline void UM6_imu_align(void);
57 inline void UM6_send_packet(uint8_t *packet_buffer, uint8_t packet_length);
58 inline uint16_t UM6_calculate_checksum(uint8_t packet_buffer[], uint8_t packet_length);
59 inline bool UM6_verify_chk(uint8_t packet_buffer[], uint8_t packet_length);
60 
61 inline bool UM6_verify_chk(uint8_t packet_buffer[], uint8_t packet_length)
62 {
63  chk_rec = (packet_buffer[packet_length - 2] << 8) | packet_buffer[packet_length - 1];
64  chk_calc = UM6_calculate_checksum(packet_buffer, packet_length - 2);
65  return (chk_calc == chk_rec);
66 }
67 
68 inline uint16_t UM6_calculate_checksum(uint8_t packet_buffer[], uint8_t packet_length)
69 {
70  uint16_t chk = 0;
71  for (int i = 0; i < packet_length; i++) {
72  chk += packet_buffer[i];
73  }
74  return chk;
75 }
76 
77 inline void UM6_send_packet(uint8_t *packet_buffer, uint8_t packet_length)
78 {
79  for (int i = 0; i < packet_length; i++) {
80  uart_put_byte(&(UM6_LINK), 0, packet_buffer[i]);
81  }
82 }
83 
84 void UM6_imu_align(void)
85 {
87 
88  // Acceleration vector realign
89  buf_out[0] = 's';
90  buf_out[1] = 'n';
91  buf_out[2] = 'p';
92  buf_out[3] = 0;
95  buf_out[5] = (uint8_t)(data_chk >> 8);
96  buf_out[6] = (uint8_t)data_chk;
98 
99  // Magnetic realign
100  buf_out[0] = 's';
101  buf_out[1] = 'n';
102  buf_out[2] = 'p';
103  buf_out[3] = 0;
106  buf_out[5] = (uint8_t)(data_chk >> 8);
107  buf_out[6] = (uint8_t)data_chk;
109 
110  // Zero gyros 0xAC, takes around 3s
111  buf_out[0] = 's';
112  buf_out[1] = 'n';
113  buf_out[2] = 'p';
114  buf_out[3] = 0;
117  buf_out[5] = (uint8_t)(data_chk >> 8);
118  buf_out[6] = (uint8_t)data_chk;
120 
121  // Reset EKF 0xAD
122  buf_out[0] = 's';
123  buf_out[1] = 'n';
124  buf_out[2] = 'p';
125  buf_out[3] = 0;
128  buf_out[5] = (uint8_t)(data_chk >> 8);
129  buf_out[6] = (uint8_t)data_chk;
131 
133 }
134 
135 void imu_um6_init(void)
136 {
137  // Initialize variables
139 
140  // Initialize packet
142  UM6_packet.msg_idx = 0;
143  UM6_packet.msg_available = false;
145  UM6_packet.hdr_error = 0;
146 
147  // Communication register 0x00
148  buf_out[0] = 's';
149  buf_out[1] = 'n';
150  buf_out[2] = 'p';
151  buf_out[3] = 0x80; // has data, zeros
152  buf_out[4] = IMU_UM6_COMMUNICATION_REG; // communication register address
153  buf_out[5] = 0b01000110; // B3 - broadcast enabled, processed data enabled (acc, gyro, mag)
154  buf_out[6] = 0b10000000; // B2 - no euler angles, quaternions enabled
155  buf_out[7] = 0b00101101; // B1 - baud 115200
156  buf_out[8] = 100; // B0 - broadcast rate, 1=20Hz, 100=130Hz, 255=300Hz
158  buf_out[9] = (uint8_t)(data_chk >> 8);
159  buf_out[10] = (uint8_t)data_chk;
161 
162  // Config register 0x01
163  buf_out[0] = 's';
164  buf_out[1] = 'n';
165  buf_out[2] = 'p';
166  buf_out[3] = 0x80; // has data, zeros
167  buf_out[4] = IMU_UM6_MISC_CONFIG_REG; // config register address
168  buf_out[5] = 0b11110000; // B3 - mag, accel updates enabled, use quaternions
169  buf_out[6] = 0; // B2 - RES
170  buf_out[7] = 0; // B1 - RES
171  buf_out[8] = 0; // B0 - RES
173  buf_out[9] = (uint8_t)(data_chk >> 8);
174  buf_out[10] = (uint8_t)data_chk;
176 
177  /*
178  // Get firmware
179  buf_out[0] = 's';
180  buf_out[1] = 'n';
181  buf_out[2] = 'p';
182  buf_out[3] = 0;
183  buf_out[4] = IMU_UM6_GET_FIRMWARE_CMD;
184  data_chk = UM6_calculate_checksum(buf_out, 5);
185  buf_out[5] = (uint8_t)(data_chk >> 8);
186  buf_out[6] = (uint8_t)data_chk;
187  UM6_send_packet(buf_out, 7);
188  */
189 
190  UM6_imu_align();
191 }
192 
193 
194 
196 {
197  /* We would request for data here - optional
198  //GET_DATA command 0xAE
199  buf_out[0] = 's';
200  buf_out[1] = 'n';
201  buf_out[2] = 'p';
202  buf_out[3] = 0; // zeros
203  buf_out[4] = 0xAE; // GET_DATA command
204  data_chk = UM6_calculate_checksum(buf_out, 5);
205  buf_out[5] = (uint8_t)(data_chk >> 8);
206  buf_out[6] = (uint8_t)data_chk;
207  UM6_send_packet(buf_out, 7);
208  */
209 }
210 
212 {
213  if ((UM6_status == UM6Running) && PacketLength > 11) {
214  switch (PacketAddr) {
215  case IMU_UM6_GYRO_PROC:
216  UM6_rate.p =
217  ((float)((int16_t)
218  ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 0] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 1]))) * 0.0610352;
219  UM6_rate.q =
220  ((float)((int16_t)
221  ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 2] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 3]))) * 0.0610352;
222  UM6_rate.r =
223  ((float)((int16_t)
224  ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 4] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 5]))) * 0.0610352;
225  RATES_SMUL(UM6_rate, UM6_rate, 0.0174532925); //Convert deg/sec to rad/sec
227  break;
228  case IMU_UM6_ACCEL_PROC:
229  UM6_accel.x =
230  ((float)((int16_t)
231  ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 0] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 1]))) * 0.000183105;
232  UM6_accel.y =
233  ((float)((int16_t)
234  ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 2] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 3]))) * 0.000183105;
235  UM6_accel.z =
236  ((float)((int16_t)
237  ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 4] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 5]))) * 0.000183105;
238  VECT3_SMUL(UM6_accel, UM6_accel, 9.80665); //Convert g into m/s2
239  ACCELS_BFP_OF_REAL(imu.accel, UM6_accel); // float to int
240  break;
241  case IMU_UM6_MAG_PROC:
242  UM6_mag.x =
243  ((float)((int16_t)
244  ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 0] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 1]))) * 0.000305176;
245  UM6_mag.y =
246  ((float)((int16_t)
247  ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 2] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 3]))) * 0.000305176;
248  UM6_mag.z =
249  ((float)((int16_t)
250  ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 4] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 5]))) * 0.000305176;
251  // Assume the same units for magnetic field
252  // Magnitude at the Earth's surface ranges from 25 to 65 microteslas (0.25 to 0.65 gauss).
254  break;
255  case IMU_UM6_EULER:
256  UM6_eulers.phi =
257  ((float)((int16_t)
258  ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 0] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 1]))) * 0.0109863;
259  UM6_eulers.theta =
260  ((float)((int16_t)
261  ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 2] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 3]))) * 0.0109863;
262  UM6_eulers.psi =
263  ((float)((int16_t)
264  ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 4] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 5]))) * 0.0109863;
265  EULERS_SMUL(UM6_eulers, UM6_eulers, 0.0174532925); //Convert deg to rad
266  EULERS_BFP_OF_REAL(ahrs_impl.ltp_to_imu_euler, UM6_eulers);
267  break;
268  case IMU_UM6_QUAT:
269  UM6_quat.qi =
270  ((float)((int16_t)
271  ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 0] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 1]))) * 0.0000335693;
272  UM6_quat.qx =
273  ((float)((int16_t)
274  ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 2] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 3]))) * 0.0000335693;
275  UM6_quat.qy =
276  ((float)((int16_t)
277  ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 4] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 5]))) * 0.0000335693;
278  UM6_quat.qz =
279  ((float)((int16_t)
280  ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 6] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 7]))) * 0.0000335693;
281  QUAT_BFP_OF_REAL(ahrs_impl.ltp_to_imu_quat, UM6_quat);
282  break;
283  default:
284  break;
285  }
286  }
287 }
288 
289 /* UM6 Packet Collection */
291 {
292  switch (UM6_packet.status) {
293  case UM6PacketWaiting:
294  UM6_packet.msg_idx = 0;
295  if (c == 's') {
299  } else {
302  }
303  break;
304  case UM6PacketReadingS:
305  if (c == 'n') {
309  } else {
312  }
313  break;
314  case UM6PacketReadingN:
315  if (c == 'p') {
319  } else {
322  }
323  break;
324  case UM6PacketReadingPT:
325  PacketType = c;
329  if ((PacketType & 0xC0) == 0xC0) {
330  PacketLength = 4 * ((PacketType >> 2) & 0xF) + 7; // Batch, has 4*BatchLength bytes of data
331  } else if ((PacketType & 0xC0) == 0x80) {
332  PacketLength = 11; // Not batch, has 4 bytes of data
333  } else if ((PacketType & 0xC0) == 0x00) {
334  PacketLength = 7; // Not batch, no data
335  }
336  break;
338  PacketAddr = c;
342  break;
348  UM6_packet.msg_available = true;
349  } else {
350  UM6_packet.msg_available = false;
352  }
354  }
355  break;
356  default:
358  UM6_packet.msg_idx = 0;
359  break;
360  }
361 }
362 
363 /* no scaling */
364 void imu_scale_gyro(struct Imu *_imu __attribute__((unused))) {}
365 void imu_scale_accel(struct Imu *_imu __attribute__((unused))) {}
366 void imu_scale_mag(struct Imu *_imu __attribute__((unused))) {}
367 
368 
369 void imu_um6_publish(void)
370 {
371  uint32_t now_ts = get_sys_time_usec();
372  AbiSendMsgIMU_GYRO_INT32(IMU_UM6_ID, now_ts, &imu.gyro);
373  AbiSendMsgIMU_ACCEL_INT32(IMU_UM6_ID, now_ts, &imu.accel);
374  AbiSendMsgIMU_MAG_INT32(IMU_UM6_ID, now_ts, &imu.mag);
375 }
unsigned short uint16_t
Definition: types.h:16
void UM6_packet_read_message(void)
Definition: imu_um6.c:211
#define RATES_BFP_OF_REAL(_ri, _rf)
Definition: pprz_algebra.h:765
uint8_t PacketLength
Definition: imu_um6.c:42
uint8_t msg_idx
Definition: imu_um6.h:85
#define IMU_UM6_MAG_PROC
Definition: imu_um6.h:56
float phi
in radians
#define IMU_UM6_QUAT
Definition: imu_um6.h:58
#define EULERS_BFP_OF_REAL(_ei, _ef)
Definition: pprz_algebra.h:715
void UM6_send_packet(uint8_t *packet_buffer, uint8_t packet_length)
Definition: imu_um6.c:77
struct FloatRates UM6_rate
Definition: imu_um6.c:51
float r
in rad/s
uint8_t buf_out[IMU_UM6_BUFFER_LENGTH]
Definition: imu_um6.c:39
Main include for ABI (AirBorneInterface).
float psi
in radians
void UM6_imu_align(void)
Definition: imu_um6.c:84
void imu_um6_publish(void)
Definition: imu_um6.c:369
bool UM6_verify_chk(uint8_t packet_buffer[], uint8_t packet_length)
Definition: imu_um6.c:61
void imu_um6_periodic(void)
Definition: imu_um6.c:195
struct Imu imu
global IMU state
Definition: imu.c:108
float q
in rad/s
float p
in rad/s
uint32_t hdr_error
Definition: imu_um6.h:82
UM6Status
Definition: imu_um6.h:97
#define IMU_UM6_DATA_OFFSET
Definition: imu_um6.h:42
euler angles
#define ACCELS_BFP_OF_REAL(_ef, _ei)
Definition: pprz_algebra.h:801
Roation quaternion.
#define IMU_UM6_ZERO_GYROS_CMD
Definition: imu_um6.h:48
#define IMU_UM6_BUFFER_LENGTH
Definition: imu_um6.h:41
float theta
in radians
struct FloatVect3 UM6_mag
Definition: imu_um6.c:52
#define MAGS_BFP_OF_REAL(_ef, _ei)
Definition: pprz_algebra.h:813
struct Int32Vect3 accel
accelerometer measurements in m/s^2 in BFP with INT32_ACCEL_FRAC
Definition: imu.h:39
#define IMU_UM6_RESET_EKF_CMD
Definition: imu_um6.h:49
uint16_t chk_calc
Definition: imu_um6.c:47
#define IMU_UM6_GYRO_PROC
Definition: imu_um6.h:54
enum UM6Status UM6_status
Definition: imu_um6.c:45
Architecture independent timing functions.
bool msg_available
Definition: imu_um6.h:80
struct FloatVect3 UM6_accel
Definition: imu_um6.c:50
#define EULERS_SMUL(_eo, _ei, _s)
Definition: pprz_algebra.h:303
uint8_t msg_buf[IMU_UM6_BUFFER_LENGTH]
Definition: imu_um6.h:83
unsigned long uint32_t
Definition: types.h:18
#define QUAT_BFP_OF_REAL(_qi, _qf)
Definition: pprz_algebra.h:752
signed short int16_t
Definition: types.h:17
uint8_t PacketAddr
Definition: imu_um6.c:44
uint8_t PacketType
Definition: imu_um6.c:43
#define IMU_UM6_SET_MAG_REF
Definition: imu_um6.h:52
struct Int32Vect3 mag
magnetometer measurements scaled to 1 in BFP with INT32_MAG_FRAC
Definition: imu.h:40
Inertial Measurement Unit interface.
struct UM6Packet UM6_packet
Definition: imu_um6.c:38
#define RATES_SMUL(_ro, _ri, _s)
Definition: pprz_algebra.h:379
uint16_t chk_rec
Definition: imu_um6.c:48
void uart_put_byte(struct uart_periph *p, long fd, uint8_t data)
Uart transmit implementation.
Definition: uart_arch.c:967
#define IMU_UM6_COMMUNICATION_REG
Definition: imu_um6.h:45
uint16_t data_chk
Definition: imu_um6.c:40
#define VECT3_SMUL(_vo, _vi, _s)
Definition: pprz_algebra.h:189
#define IMU_UM6_SET_ACCEL_REF
Definition: imu_um6.h:51
unsigned char uint8_t
Definition: types.h:14
void UM6_packet_parse(uint8_t c)
Definition: imu_um6.c:290
#define IMU_UM6_EULER
Definition: imu_um6.h:57
struct FloatEulers UM6_eulers
Definition: imu_um6.c:53
uint16_t UM6_calculate_checksum(uint8_t packet_buffer[], uint8_t packet_length)
Definition: imu_um6.c:68
void imu_um6_init(void)
Definition: imu_um6.c:135
void imu_scale_accel(struct Imu *_imu)
Definition: imu_um6.c:365
#define IMU_UM6_ID
struct FloatQuat UM6_quat
Definition: imu_um6.c:54
#define IMU_UM6_MISC_CONFIG_REG
Definition: imu_um6.h:46
Driver for CH Robotics UM6 IMU/AHRS subsystem.
abstract IMU interface providing fixed point interface
Definition: imu.h:37
uint8_t status
Definition: imu_um6.h:84
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:68
void imu_scale_gyro(struct Imu *_imu)
Definition: imu_um6.c:364
void imu_scale_mag(struct Imu *_imu)
Definition: imu_um6.c:366
uint32_t chksm_error
Definition: imu_um6.h:81
angular rates
#define IMU_UM6_ACCEL_PROC
Definition: imu_um6.h:55
struct Int32Rates gyro
gyroscope measurements in rad/s in BFP with INT32_RATE_FRAC
Definition: imu.h:38