Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
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 }
FloatQuat::qz
float qz
Definition: pprz_algebra_float.h:67
UM6Packet::msg_idx
uint8_t msg_idx
Definition: imu_um6.h:85
c
VIC slots used for the LPC2148 define name e g gps UART1_VIC_SLOT e g modem SPI1_VIC_SLOT SPI1 in mcu_periph spi_arch c or spi_slave_hs_arch c(and some others not using the SPI peripheral yet..) I2C0_VIC_SLOT 8 mcu_periph/i2c_arch.c I2C1_VIC_SLOT 9 mcu_periph/i2c_arch.c USB_VIC_SLOT 10 usb
IMU_UM6_COMMUNICATION_REG
#define IMU_UM6_COMMUNICATION_REG
Definition: imu_um6.h:45
uint16_t
unsigned short uint16_t
Definition: types.h:16
IMU_UM6_GYRO_PROC
#define IMU_UM6_GYRO_PROC
Definition: imu_um6.h:54
UM6_calculate_checksum
uint16_t UM6_calculate_checksum(uint8_t packet_buffer[], uint8_t packet_length)
Definition: imu_um6.c:68
IMU_UM6_RESET_EKF_CMD
#define IMU_UM6_RESET_EKF_CMD
Definition: imu_um6.h:49
IMU_UM6_MAG_PROC
#define IMU_UM6_MAG_PROC
Definition: imu_um6.h:56
VECT3_SMUL
#define VECT3_SMUL(_vo, _vi, _s)
Definition: pprz_algebra.h:189
IMU_UM6_SET_ACCEL_REF
#define IMU_UM6_SET_ACCEL_REF
Definition: imu_um6.h:51
abi.h
chk_calc
uint16_t chk_calc
Definition: imu_um6.c:47
Imu::accel
struct Int32Vect3 accel
accelerometer measurements in m/s^2 in BFP with INT32_ACCEL_FRAC
Definition: imu.h:39
UM6PacketReadingN
@ UM6PacketReadingN
Definition: imu_um6.h:91
RATES_SMUL
#define RATES_SMUL(_ro, _ri, _s)
Definition: pprz_algebra.h:379
uint32_t
unsigned long uint32_t
Definition: types.h:18
imu_um6_publish
void imu_um6_publish(void)
Definition: imu_um6.c:369
UM6Status
UM6Status
Definition: imu_um6.h:97
UM6Packet::chksm_error
uint32_t chksm_error
Definition: imu_um6.h:81
UM6_packet_read_message
void UM6_packet_read_message(void)
Definition: imu_um6.c:211
UM6_mag
struct FloatVect3 UM6_mag
Definition: imu_um6.c:52
EULERS_SMUL
#define EULERS_SMUL(_eo, _ei, _s)
Definition: pprz_algebra.h:303
UM6Uninit
@ UM6Uninit
Definition: imu_um6.h:98
UM6Packet::hdr_error
uint32_t hdr_error
Definition: imu_um6.h:82
FloatEulers::theta
float theta
in radians
Definition: pprz_algebra_float.h:86
get_sys_time_usec
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:68
FloatVect3
Definition: pprz_algebra_float.h:54
imu.h
IMU_UM6_EULER
#define IMU_UM6_EULER
Definition: imu_um6.h:57
FloatQuat
Roation quaternion.
Definition: pprz_algebra_float.h:63
Imu
abstract IMU interface providing fixed point interface
Definition: imu.h:37
UM6_eulers
struct FloatEulers UM6_eulers
Definition: imu_um6.c:53
ACCELS_BFP_OF_REAL
#define ACCELS_BFP_OF_REAL(_ef, _ei)
Definition: pprz_algebra.h:801
data_chk
uint16_t data_chk
Definition: imu_um6.c:40
PacketLength
uint8_t PacketLength
Definition: imu_um6.c:42
IMU_UM6_ZERO_GYROS_CMD
#define IMU_UM6_ZERO_GYROS_CMD
Definition: imu_um6.h:48
FloatEulers::phi
float phi
in radians
Definition: pprz_algebra_float.h:85
RATES_BFP_OF_REAL
#define RATES_BFP_OF_REAL(_ri, _rf)
Definition: pprz_algebra.h:765
UM6PacketReadingAddr
@ UM6PacketReadingAddr
Definition: imu_um6.h:93
IMU_UM6_QUAT
#define IMU_UM6_QUAT
Definition: imu_um6.h:58
UM6Packet::msg_available
bool msg_available
Definition: imu_um6.h:80
UM6_rate
struct FloatRates UM6_rate
Definition: imu_um6.c:51
buf_out
uint8_t buf_out[IMU_UM6_BUFFER_LENGTH]
Definition: imu_um6.c:39
IMU_UM6_ID
#define IMU_UM6_ID
Definition: abi_sender_ids.h:323
int16_t
signed short int16_t
Definition: types.h:17
imu_scale_accel
void imu_scale_accel(struct Imu *_imu)
Definition: imu_um6.c:365
sys_time.h
Architecture independent timing functions.
IMU_UM6_SET_MAG_REF
#define IMU_UM6_SET_MAG_REF
Definition: imu_um6.h:52
uint8_t
unsigned char uint8_t
Definition: types.h:14
FloatRates::r
float r
in rad/s
Definition: pprz_algebra_float.h:96
UM6Packet::msg_buf
uint8_t msg_buf[IMU_UM6_BUFFER_LENGTH]
Definition: imu_um6.h:83
PacketAddr
uint8_t PacketAddr
Definition: imu_um6.c:44
PacketType
uint8_t PacketType
Definition: imu_um6.c:43
UM6PacketReadingPT
@ UM6PacketReadingPT
Definition: imu_um6.h:92
uart_put_byte
void uart_put_byte(struct uart_periph *p, long fd, uint8_t data)
Uart transmit implementation.
Definition: uart_arch.c:1095
QUAT_BFP_OF_REAL
#define QUAT_BFP_OF_REAL(_qi, _qf)
Definition: pprz_algebra.h:752
UM6_quat
struct FloatQuat UM6_quat
Definition: imu_um6.c:54
imu_um6.h
UM6_packet
struct UM6Packet UM6_packet
Definition: imu_um6.c:38
chk_rec
uint16_t chk_rec
Definition: imu_um6.c:48
UM6_verify_chk
bool UM6_verify_chk(uint8_t packet_buffer[], uint8_t packet_length)
Definition: imu_um6.c:61
imu
struct Imu imu
global IMU state
Definition: imu.c:108
imu_um6_periodic
void imu_um6_periodic(void)
Definition: imu_um6.c:195
FloatVect3::y
float y
Definition: pprz_algebra_float.h:56
FloatQuat::qx
float qx
Definition: pprz_algebra_float.h:65
FloatRates::q
float q
in rad/s
Definition: pprz_algebra_float.h:95
imu_scale_gyro
void imu_scale_gyro(struct Imu *_imu)
Definition: imu_um6.c:364
UM6Packet
Definition: imu_um6.h:79
UM6Running
@ UM6Running
Definition: imu_um6.h:99
UM6_packet_parse
void UM6_packet_parse(uint8_t c)
Definition: imu_um6.c:290
FloatVect3::x
float x
Definition: pprz_algebra_float.h:55
UM6_status
enum UM6Status UM6_status
Definition: imu_um6.c:45
FloatQuat::qi
float qi
Definition: pprz_algebra_float.h:64
UM6_accel
struct FloatVect3 UM6_accel
Definition: imu_um6.c:50
IMU_UM6_MISC_CONFIG_REG
#define IMU_UM6_MISC_CONFIG_REG
Definition: imu_um6.h:46
Imu::mag
struct Int32Vect3 mag
magnetometer measurements scaled to 1 in BFP with INT32_MAG_FRAC
Definition: imu.h:40
imu_um6_init
void imu_um6_init(void)
Definition: imu_um6.c:135
FloatVect3::z
float z
Definition: pprz_algebra_float.h:57
UM6PacketReadingS
@ UM6PacketReadingS
Definition: imu_um6.h:90
FloatEulers
euler angles
Definition: pprz_algebra_float.h:84
FloatQuat::qy
float qy
Definition: pprz_algebra_float.h:66
UM6PacketWaiting
@ UM6PacketWaiting
Definition: imu_um6.h:89
FloatEulers::psi
float psi
in radians
Definition: pprz_algebra_float.h:87
UM6_imu_align
void UM6_imu_align(void)
Definition: imu_um6.c:84
UM6Packet::status
uint8_t status
Definition: imu_um6.h:84
EULERS_BFP_OF_REAL
#define EULERS_BFP_OF_REAL(_ei, _ef)
Definition: pprz_algebra.h:715
UM6_send_packet
void UM6_send_packet(uint8_t *packet_buffer, uint8_t packet_length)
Definition: imu_um6.c:77
imu_scale_mag
void imu_scale_mag(struct Imu *_imu)
Definition: imu_um6.c:366
IMU_UM6_DATA_OFFSET
#define IMU_UM6_DATA_OFFSET
Definition: imu_um6.h:42
FloatRates::p
float p
in rad/s
Definition: pprz_algebra_float.h:94
IMU_UM6_ACCEL_PROC
#define IMU_UM6_ACCEL_PROC
Definition: imu_um6.h:55
MAGS_BFP_OF_REAL
#define MAGS_BFP_OF_REAL(_ef, _ei)
Definition: pprz_algebra.h:813
FloatRates
angular rates
Definition: pprz_algebra_float.h:93
Imu::gyro
struct Int32Rates gyro
gyroscope measurements in rad/s in BFP with INT32_RATE_FRAC
Definition: imu.h:38
IMU_UM6_BUFFER_LENGTH
#define IMU_UM6_BUFFER_LENGTH
Definition: imu_um6.h:41
UM6PacketReadingData
@ UM6PacketReadingData
Definition: imu_um6.h:94