Revision 4bc1c0c1
Adafruit_BNO055.cpp | ||
---|---|---|
1 |
/*************************************************************************** |
|
2 |
This is a library for the BNO055 orientation sensor |
|
3 |
|
|
4 |
Designed specifically to work with the Adafruit BNO055 Breakout. |
|
5 |
|
|
6 |
Pick one up today in the adafruit shop! |
|
7 |
------> http://www.adafruit.com/products |
|
8 |
|
|
9 |
These sensors use I2C to communicate, 2 pins are required to interface. |
|
10 |
|
|
11 |
Adafruit invests time and resources providing this open source code, |
|
12 |
please support Adafruit andopen-source hardware by purchasing products |
|
13 |
from Adafruit! |
|
14 |
|
|
15 |
Written by KTOWN for Adafruit Industries. |
|
16 |
|
|
17 |
MIT license, all text above must be included in any redistribution |
|
18 |
***************************************************************************/ |
|
19 |
|
|
20 |
#if ARDUINO >= 100 |
|
21 |
#include "Arduino.h" |
|
22 |
#else |
|
23 |
#include "WProgram.h" |
|
24 |
#endif |
|
25 |
|
|
26 |
#include <math.h> |
|
27 |
#include <limits.h> |
|
28 |
|
|
29 |
#include "Adafruit_BNO055.h" |
|
30 |
|
|
31 |
/*************************************************************************** |
|
32 |
CONSTRUCTOR |
|
33 |
***************************************************************************/ |
|
34 |
|
|
35 |
/**************************************************************************/ |
|
36 |
/*! |
|
37 |
@brief Instantiates a new Adafruit_BNO055 class |
|
38 |
*/ |
|
39 |
/**************************************************************************/ |
|
40 |
Adafruit_BNO055::Adafruit_BNO055(int32_t sensorID, uint8_t address) |
|
41 |
{ |
|
42 |
_sensorID = sensorID; |
|
43 |
_address = address; |
|
44 |
} |
|
45 |
|
|
46 |
/*************************************************************************** |
|
47 |
PUBLIC FUNCTIONS |
|
48 |
***************************************************************************/ |
|
49 |
|
|
50 |
/**************************************************************************/ |
|
51 |
/*! |
|
52 |
@brief Sets up the HW |
|
53 |
*/ |
|
54 |
/**************************************************************************/ |
|
55 |
bool Adafruit_BNO055::begin(adafruit_bno055_opmode_t mode) |
|
56 |
{ |
|
57 |
/* Enable I2C */ |
|
58 |
Wire.begin(); |
|
59 |
|
|
60 |
/* Make sure we have the right device */ |
|
61 |
uint8_t id = read8(BNO055_CHIP_ID_ADDR); |
|
62 |
if(id != BNO055_ID) |
|
63 |
{ |
|
64 |
return false; |
|
65 |
} |
|
66 |
|
|
67 |
/* Switch to config mode (just in case since this is the default) */ |
|
68 |
setMode(OPERATION_MODE_CONFIG); |
|
69 |
|
|
70 |
/* Set to normal power mode */ |
|
71 |
write8(BNO055_PWR_MODE_ADDR, POWER_MODE_NORMAL); |
|
72 |
delay(10); |
|
73 |
|
|
74 |
/* Set the requested operating mode (see section 3.3) */ |
|
75 |
write8(BNO055_OPR_MODE_ADDR, mode); |
|
76 |
delay(20); |
|
77 |
|
|
78 |
return true; |
|
79 |
} |
|
80 |
|
|
81 |
/**************************************************************************/ |
|
82 |
/*! |
|
83 |
@brief Puts the chip in the specified operating mode |
|
84 |
*/ |
|
85 |
/**************************************************************************/ |
|
86 |
void Adafruit_BNO055::setMode(adafruit_bno055_opmode_t mode) |
|
87 |
{ |
|
88 |
_mode = mode; |
|
89 |
|
|
90 |
write8(BNO055_OPR_MODE_ADDR, _mode); |
|
91 |
delay(30); |
|
92 |
} |
|
93 |
|
|
94 |
/**************************************************************************/ |
|
95 |
/*! |
|
96 |
@brief Gets the latest system status info |
|
97 |
*/ |
|
98 |
/**************************************************************************/ |
|
99 |
void Adafruit_BNO055::getSystemStatus(adafruit_bno055_system_status_t * status) |
|
100 |
{ |
|
101 |
memset(status, 0, sizeof(adafruit_bno055_system_status_t)); |
|
102 |
|
|
103 |
/* Read the system status register */ |
|
104 |
status->system_status = read8(BNO055_SYS_STAT_ADDR); |
|
105 |
status->self_test_result = read8(BNO055_SELFTEST_RESULT_ADDR); |
|
106 |
status->system_error = read8(BNO055_SYS_ERR_ADDR); |
|
107 |
} |
|
108 |
|
|
109 |
/**************************************************************************/ |
|
110 |
/*! |
|
111 |
@brief Displays system status info via Serial.print |
|
112 |
*/ |
|
113 |
/**************************************************************************/ |
|
114 |
void Adafruit_BNO055::displaySystemStatus(void) |
|
115 |
{ |
|
116 |
adafruit_bno055_system_status_t status; |
|
117 |
getSystemStatus(&status); |
|
118 |
|
|
119 |
/* System Status (see section 4.3.58) |
|
120 |
--------------------------------- |
|
121 |
0 = Idle |
|
122 |
1 = System Error |
|
123 |
2 = Initializing Peripherals |
|
124 |
3 = System Iniitalization |
|
125 |
4 = Executing Self-Test |
|
126 |
5 = Sensor fusio algorithm running |
|
127 |
6 = System running without fusion algorithms */ |
|
128 |
|
|
129 |
Serial.print("System Status: 0x"); |
|
130 |
Serial.println(status.system_status, HEX); |
|
131 |
|
|
132 |
/* Self Test Results (see section ) |
|
133 |
-------------------------------- |
|
134 |
1 = test passed, 0 = test failed |
|
135 |
|
|
136 |
Bit 0 = Accelerometer self test |
|
137 |
Bit 1 = Magnetometer self test |
|
138 |
Bit 2 = Gyroscope self test |
|
139 |
Bit 3 = MCU self test |
|
140 |
|
|
141 |
0x0F = all good! */ |
|
142 |
|
|
143 |
Serial.print("Self Test Results: 0x"); |
|
144 |
Serial.println(status.self_test_result, HEX); |
|
145 |
|
|
146 |
/* System Error (see section 4.3.59) |
|
147 |
--------------------------------- |
|
148 |
0 = No error |
|
149 |
1 = Peripheral initialization error |
|
150 |
2 = System initialization error |
|
151 |
3 = Self test result failed |
|
152 |
4 = Register map value out of range |
|
153 |
5 = Register map address out of range |
|
154 |
6 = Register map write error |
|
155 |
7 = BNO low power mode not available for selected operat ion mode |
|
156 |
8 = Accelerometer power mode not available |
|
157 |
9 = Fusion algorithm configuration error |
|
158 |
A = Sensor configuration error */ |
|
159 |
|
|
160 |
Serial.print("System Error: 0x"); |
|
161 |
Serial.println(status.system_error, HEX); |
|
162 |
} |
|
163 |
|
|
164 |
/**************************************************************************/ |
|
165 |
/*! |
|
166 |
@brief Gets the chip revision numbers |
|
167 |
*/ |
|
168 |
/**************************************************************************/ |
|
169 |
void Adafruit_BNO055::getRevInfo(adafruit_bno055_rev_info_t* info) |
|
170 |
{ |
|
171 |
uint8_t a, b; |
|
172 |
|
|
173 |
memset(info, 0, sizeof(adafruit_bno055_rev_info_t)); |
|
174 |
|
|
175 |
info->accel_rev = read8(BNO055_ACCEL_REV_ID_ADDR); |
|
176 |
info->mag_rev = read8(BNO055_MAG_REV_ID_ADDR); |
|
177 |
info->gyro_rev = read8(BNO055_GYRO_REV_ID_ADDR); |
|
178 |
info->bl_rev = read8(BNO055_BL_REV_ID_ADDR); |
|
179 |
|
|
180 |
a = read8(BNO055_SW_REV_ID_LSB_ADDR); |
|
181 |
b = read8(BNO055_SW_REV_ID_MSB_ADDR); |
|
182 |
info->sw_rev = (((uint16_t)b) << 8) | ((uint16_t)a); |
|
183 |
} |
|
184 |
|
|
185 |
/**************************************************************************/ |
|
186 |
/*! |
|
187 |
@brief Displays the chip revision numbers via Serial.print |
|
188 |
*/ |
|
189 |
/**************************************************************************/ |
|
190 |
void Adafruit_BNO055::displayRevInfo(void) |
|
191 |
{ |
|
192 |
adafruit_bno055_rev_info_t info; |
|
193 |
getRevInfo(&info); |
|
194 |
|
|
195 |
/* Check the accelerometer revision */ |
|
196 |
Serial.print("Accelerometer Revision: 0x"); |
|
197 |
Serial.println(info.accel_rev, HEX); |
|
198 |
|
|
199 |
/* Check the magnetometer revision */ |
|
200 |
Serial.print("Magnetometer Revision: 0x"); |
|
201 |
Serial.println(info.mag_rev, HEX); |
|
202 |
|
|
203 |
/* Check the gyroscope revision */ |
|
204 |
Serial.print("Gyroscope Revision: 0x"); |
|
205 |
Serial.println(info.gyro_rev, HEX); |
|
206 |
|
|
207 |
/* Check the SW revision */ |
|
208 |
Serial.print("SW Revision: 0x"); |
|
209 |
Serial.println(info.sw_rev, HEX); |
|
210 |
|
|
211 |
/* Check the bootloader revision */ |
|
212 |
Serial.print("Bootloader Revision: 0x"); |
|
213 |
Serial.println(info.bl_rev, HEX); |
|
214 |
} |
|
215 |
|
|
216 |
/**************************************************************************/ |
|
217 |
/*! |
|
218 |
@brief Gets a new heading/roll/pitch sample in Euler angles |
|
219 |
*/ |
|
220 |
/**************************************************************************/ |
|
221 |
imu::Vector<3> Adafruit_BNO055::getEuler(void) |
|
222 |
{ |
|
223 |
imu::Vector<3> hrp; |
|
224 |
uint8_t buffer[6]; |
|
225 |
memset (buffer, 0, 6); |
|
226 |
|
|
227 |
int16_t h, r, p; |
|
228 |
h = r = p = 0; |
|
229 |
|
|
230 |
/* Read HRP data (6 bytes) */ |
|
231 |
readLen(BNO055_EULER_H_LSB_ADDR, buffer, 6); |
|
232 |
h = (((uint16_t)buffer[1]) << 8) | ((uint16_t)buffer[0]); |
|
233 |
r = (((uint16_t)buffer[3]) << 8) | ((uint16_t)buffer[2]); |
|
234 |
p = (((uint16_t)buffer[5]) << 8) | ((uint16_t)buffer[4]); |
|
235 |
|
|
236 |
/* Assign to Vector */ |
|
237 |
hrp[0] = (double)h; |
|
238 |
hrp[1] = (double)r; |
|
239 |
hrp[2] = (double)p; |
|
240 |
|
|
241 |
return hrp; |
|
242 |
} |
|
243 |
|
|
244 |
/**************************************************************************/ |
|
245 |
/*! |
|
246 |
@brief Gets a new accelerometer sample |
|
247 |
*/ |
|
248 |
/**************************************************************************/ |
|
249 |
imu::Vector<3> Adafruit_BNO055::getAccel(void) |
|
250 |
{ |
|
251 |
imu::Vector<3> xyz; |
|
252 |
uint8_t buffer[6]; |
|
253 |
memset (buffer, 0, 6); |
|
254 |
|
|
255 |
int16_t x, y, z; |
|
256 |
x = y = z = 0; |
|
257 |
|
|
258 |
/* Read accel data (6 bytes) */ |
|
259 |
readLen(BNO055_ACCEL_DATA_X_LSB_ADDR, buffer, 6); |
|
260 |
x = (((uint16_t)buffer[1]) << 8) | ((uint16_t)buffer[0]); |
|
261 |
y = (((uint16_t)buffer[3]) << 8) | ((uint16_t)buffer[2]); |
|
262 |
z = (((uint16_t)buffer[5]) << 8) | ((uint16_t)buffer[4]); |
|
263 |
|
|
264 |
/* Assign to Vector */ |
|
265 |
xyz[0] = (double)x; |
|
266 |
xyz[1] = (double)y; |
|
267 |
xyz[2] = (double)z; |
|
268 |
|
|
269 |
return xyz; |
|
270 |
} |
|
271 |
|
|
272 |
/**************************************************************************/ |
|
273 |
/*! |
|
274 |
@brief Provides the sensor_t data for this sensor |
|
275 |
*/ |
|
276 |
/**************************************************************************/ |
|
277 |
void Adafruit_BNO055::getSensor(sensor_t *sensor) |
|
278 |
{ |
|
279 |
/* Clear the sensor_t object */ |
|
280 |
memset(sensor, 0, sizeof(sensor_t)); |
|
281 |
|
|
282 |
/* Insert the sensor name in the fixed length char array */ |
|
283 |
strncpy (sensor->name, "BNO055", sizeof(sensor->name) - 1); |
|
284 |
sensor->name[sizeof(sensor->name)- 1] = 0; |
|
285 |
sensor->version = 1; |
|
286 |
sensor->sensor_id = _sensorID; |
|
287 |
sensor->type = SENSOR_TYPE_ORIENTATION; |
|
288 |
sensor->min_delay = 0; |
|
289 |
sensor->max_value = 0.0F; |
|
290 |
sensor->min_value = 0.0F; |
|
291 |
sensor->resolution = 0.01F; |
|
292 |
} |
|
293 |
|
|
294 |
/**************************************************************************/ |
|
295 |
/*! |
|
296 |
@brief Reads the sensor and returns the data as a sensors_event_t |
|
297 |
*/ |
|
298 |
/**************************************************************************/ |
|
299 |
bool Adafruit_BNO055::getEvent(sensors_event_t *event) |
|
300 |
{ |
|
301 |
float orientation; |
|
302 |
|
|
303 |
/* Clear the event */ |
|
304 |
memset(event, 0, sizeof(sensors_event_t)); |
|
305 |
|
|
306 |
event->version = sizeof(sensors_event_t); |
|
307 |
event->sensor_id = _sensorID; |
|
308 |
event->type = SENSOR_TYPE_ORIENTATION; |
|
309 |
event->timestamp = 0; |
|
310 |
/* |
|
311 |
getPressure(&pressure_kPa); |
|
312 |
event->pressure = pressure_kPa / 100.0F; |
|
313 |
*/ |
|
314 |
|
|
315 |
return true; |
|
316 |
} |
|
317 |
|
|
318 |
/*************************************************************************** |
|
319 |
PRIVATE FUNCTIONS |
|
320 |
***************************************************************************/ |
|
321 |
|
|
322 |
/**************************************************************************/ |
|
323 |
/*! |
|
324 |
@brief Writes an 8 bit value over I2C |
|
325 |
*/ |
|
326 |
/**************************************************************************/ |
|
327 |
bool Adafruit_BNO055::write8(adafruit_bno055_reg_t reg, byte value) |
|
328 |
{ |
|
329 |
Wire.beginTransmission(_address); |
|
330 |
#if ARDUINO >= 100 |
|
331 |
Wire.write((uint8_t)reg); |
|
332 |
Wire.write((uint8_t)value); |
|
333 |
#else |
|
334 |
Wire.send(reg); |
|
335 |
Wire.send(value); |
|
336 |
#endif |
|
337 |
Wire.endTransmission(); |
|
338 |
|
|
339 |
/* ToDo: Check for error! */ |
|
340 |
return true; |
|
341 |
} |
|
342 |
|
|
343 |
/**************************************************************************/ |
|
344 |
/*! |
|
345 |
@brief Reads an 8 bit value over I2C |
|
346 |
*/ |
|
347 |
/**************************************************************************/ |
|
348 |
byte Adafruit_BNO055::read8(adafruit_bno055_reg_t reg ) |
|
349 |
{ |
|
350 |
byte value = 0; |
|
351 |
|
|
352 |
Wire.beginTransmission(_address); |
|
353 |
#if ARDUINO >= 100 |
|
354 |
Wire.write((uint8_t)reg); |
|
355 |
#else |
|
356 |
Wire.send(reg); |
|
357 |
#endif |
|
358 |
Wire.endTransmission(); |
|
359 |
Wire.requestFrom(_address, (byte)1); |
|
360 |
#if ARDUINO >= 100 |
|
361 |
value = Wire.read(); |
|
362 |
#else |
|
363 |
value = Wire.receive(); |
|
364 |
#endif |
|
365 |
|
|
366 |
return value; |
|
367 |
} |
|
368 |
|
|
369 |
/**************************************************************************/ |
|
370 |
/*! |
|
371 |
@brief Reads the specified number of bytes over I2C |
|
372 |
*/ |
|
373 |
/**************************************************************************/ |
|
374 |
bool Adafruit_BNO055::readLen(adafruit_bno055_reg_t reg, byte * buffer, uint8_t len) |
|
375 |
{ |
|
376 |
Wire.beginTransmission(_address); |
|
377 |
#if ARDUINO >= 100 |
|
378 |
Wire.write((uint8_t)reg); |
|
379 |
#else |
|
380 |
Wire.send(reg); |
|
381 |
#endif |
|
382 |
Wire.endTransmission(); |
|
383 |
Wire.requestFrom(_address, (byte)len); |
|
384 |
|
|
385 |
/* Wait until data is available */ |
|
386 |
while (Wire.available() < len); |
|
387 |
|
|
388 |
for (uint8_t i = 0; i < len; i++) |
|
389 |
{ |
|
390 |
#if ARDUINO >= 100 |
|
391 |
buffer[i] = Wire.read(); |
|
392 |
#else |
|
393 |
buffer[i] = Wire.receive(); |
|
394 |
#endif |
|
395 |
} |
|
396 |
|
|
397 |
/* ToDo: Check for errors! */ |
|
398 |
return true; |
|
399 |
} |
Adafruit_BNO055.h | ||
---|---|---|
1 |
/*************************************************************************** |
|
2 |
This is a library for the BNO055 orientation sensor |
|
3 |
|
|
4 |
Designed specifically to work with the Adafruit BNO055 Breakout. |
|
5 |
|
|
6 |
Pick one up today in the adafruit shop! |
|
7 |
------> http://www.adafruit.com/products |
|
8 |
|
|
9 |
These sensors use I2C to communicate, 2 pins are required to interface. |
|
10 |
|
|
11 |
Adafruit invests time and resources providing this open source code, |
|
12 |
please support Adafruit andopen-source hardware by purchasing products |
|
13 |
from Adafruit! |
|
14 |
|
|
15 |
Written by KTOWN for Adafruit Industries. |
|
16 |
|
|
17 |
MIT license, all text above must be included in any redistribution |
|
18 |
***************************************************************************/ |
|
19 |
|
|
20 |
#ifndef __ADAFRUIT_BNO055_H__ |
|
21 |
#define __ADAFRUIT_BNO055_H__ |
|
22 |
|
|
23 |
#if (ARDUINO >= 100) |
|
24 |
#include "Arduino.h" |
|
25 |
#else |
|
26 |
#include "WProgram.h" |
|
27 |
#endif |
|
28 |
#include "Wire.h" |
|
29 |
|
|
30 |
#include <Adafruit_Sensor.h> |
|
31 |
#include <utility/imumaths.h> |
|
32 |
|
|
33 |
#define BNO055_ADDRESS_A (0x28) |
|
34 |
#define BNO055_ADDRESS_B (0x29) |
|
35 |
#define BNO055_ID (0xA0) |
|
36 |
|
|
37 |
class Adafruit_BNO055 /* : public Adafruit_Sensor */ |
|
38 |
{ |
|
39 |
public: |
|
40 |
typedef enum |
|
41 |
{ |
|
42 |
/* Page id register definition */ |
|
43 |
BNO055_PAGE_ID_ADDR = 0X07, |
|
44 |
|
|
45 |
/* PAGE0 REGISTER DEFINITION START*/ |
|
46 |
BNO055_CHIP_ID_ADDR = 0x00, |
|
47 |
BNO055_ACCEL_REV_ID_ADDR = 0x01, |
|
48 |
BNO055_MAG_REV_ID_ADDR = 0x02, |
|
49 |
BNO055_GYRO_REV_ID_ADDR = 0x03, |
|
50 |
BNO055_SW_REV_ID_LSB_ADDR = 0x04, |
|
51 |
BNO055_SW_REV_ID_MSB_ADDR = 0x05, |
|
52 |
BNO055_BL_REV_ID_ADDR = 0X06, |
|
53 |
|
|
54 |
/* Accel data register */ |
|
55 |
BNO055_ACCEL_DATA_X_LSB_ADDR = 0X08, |
|
56 |
BNO055_ACCEL_DATA_X_MSB_ADDR = 0X09, |
|
57 |
BNO055_ACCEL_DATA_Y_LSB_ADDR = 0X0A, |
|
58 |
BNO055_ACCEL_DATA_Y_MSB_ADDR = 0X0B, |
|
59 |
BNO055_ACCEL_DATA_Z_LSB_ADDR = 0X0C, |
|
60 |
BNO055_ACCEL_DATA_Z_MSB_ADDR = 0X0D, |
|
61 |
|
|
62 |
/* Mag data register */ |
|
63 |
BNO055_MAG_DATA_X_LSB_ADDR = 0X0E, |
|
64 |
BNO055_MAG_DATA_X_MSB_ADDR = 0X0F, |
|
65 |
BNO055_MAG_DATA_Y_LSB_ADDR = 0X10, |
|
66 |
BNO055_MAG_DATA_Y_MSB_ADDR = 0X11, |
|
67 |
BNO055_MAG_DATA_Z_LSB_ADDR = 0X12, |
|
68 |
BNO055_MAG_DATA_Z_MSB_ADDR = 0X13, |
|
69 |
|
|
70 |
/* Gyro data registers */ |
|
71 |
BNO055_GYRO_DATA_X_LSB_ADDR = 0X14, |
|
72 |
BNO055_GYRO_DATA_X_MSB_ADDR = 0X15, |
|
73 |
BNO055_GYRO_DATA_Y_LSB_ADDR = 0X16, |
|
74 |
BNO055_GYRO_DATA_Y_MSB_ADDR = 0X17, |
|
75 |
BNO055_GYRO_DATA_Z_LSB_ADDR = 0X18, |
|
76 |
BNO055_GYRO_DATA_Z_MSB_ADDR = 0X19, |
|
77 |
|
|
78 |
/* Euler data registers */ |
|
79 |
BNO055_EULER_H_LSB_ADDR = 0X1A, |
|
80 |
BNO055_EULER_H_MSB_ADDR = 0X1B, |
|
81 |
BNO055_EULER_R_LSB_ADDR = 0X1C, |
|
82 |
BNO055_EULER_R_MSB_ADDR = 0X1D, |
|
83 |
BNO055_EULER_P_LSB_ADDR = 0X1E, |
|
84 |
BNO055_EULER_P_MSB_ADDR = 0X1F, |
|
85 |
|
|
86 |
/* Quaternion data registers */ |
|
87 |
BNO055_QUATERNION_DATA_W_LSB_ADDR = 0X20, |
|
88 |
BNO055_QUATERNION_DATA_W_MSB_ADDR = 0X21, |
|
89 |
BNO055_QUATERNION_DATA_X_LSB_ADDR = 0X22, |
|
90 |
BNO055_QUATERNION_DATA_X_MSB_ADDR = 0X23, |
|
91 |
BNO055_QUATERNION_DATA_Y_LSB_ADDR = 0X24, |
|
92 |
BNO055_QUATERNION_DATA_Y_MSB_ADDR = 0X25, |
|
93 |
BNO055_QUATERNION_DATA_Z_LSB_ADDR = 0X26, |
|
94 |
BNO055_QUATERNION_DATA_Z_MSB_ADDR = 0X27, |
|
95 |
|
|
96 |
/* Linear acceleration data registers */ |
|
97 |
BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR = 0X28, |
|
98 |
BNO055_LINEAR_ACCEL_DATA_X_MSB_ADDR = 0X29, |
|
99 |
BNO055_LINEAR_ACCEL_DATA_Y_LSB_ADDR = 0X2A, |
|
100 |
BNO055_LINEAR_ACCEL_DATA_Y_MSB_ADDR = 0X2B, |
|
101 |
BNO055_LINEAR_ACCEL_DATA_Z_LSB_ADDR = 0X2C, |
|
102 |
BNO055_LINEAR_ACCEL_DATA_Z_MSB_ADDR = 0X2D, |
|
103 |
|
|
104 |
/* Gravity data registers */ |
|
105 |
BNO055_GRAVITY_DATA_X_LSB_ADDR = 0X2E, |
|
106 |
BNO055_GRAVITY_DATA_X_MSB_ADDR = 0X2F, |
|
107 |
BNO055_GRAVITY_DATA_Y_LSB_ADDR = 0X30, |
|
108 |
BNO055_GRAVITY_DATA_Y_MSB_ADDR = 0X31, |
|
109 |
BNO055_GRAVITY_DATA_Z_LSB_ADDR = 0X32, |
|
110 |
BNO055_GRAVITY_DATA_Z_MSB_ADDR = 0X33, |
|
111 |
|
|
112 |
/* Temperature data register */ |
|
113 |
BNO055_TEMP_ADDR = 0X34, |
|
114 |
|
|
115 |
/* Status registers */ |
|
116 |
BNO055_CALIB_STAT_ADDR = 0X35, |
|
117 |
BNO055_SELFTEST_RESULT_ADDR = 0X36, |
|
118 |
BNO055_INTR_STAT_ADDR = 0X37, |
|
119 |
|
|
120 |
BNO055_SYS_CLK_STAT_ADDR = 0X38, |
|
121 |
BNO055_SYS_STAT_ADDR = 0X39, |
|
122 |
BNO055_SYS_ERR_ADDR = 0X3A, |
|
123 |
|
|
124 |
/* Unit selection register */ |
|
125 |
BNO055_UNIT_SEL_ADDR = 0X3B, |
|
126 |
BNO055_DATA_SELECT_ADDR = 0X3C, |
|
127 |
|
|
128 |
/* Mode registers */ |
|
129 |
BNO055_OPR_MODE_ADDR = 0X3D, |
|
130 |
BNO055_PWR_MODE_ADDR = 0X3E, |
|
131 |
|
|
132 |
BNO055_SYS_TRIGGER_ADDR = 0X3F, |
|
133 |
BNO055_TEMP_SOURCE_ADDR = 0X40, |
|
134 |
|
|
135 |
/* Axis remap registers */ |
|
136 |
BNO055_AXIS_MAP_CONFIG_ADDR = 0X41, |
|
137 |
BNO055_AXIS_MAP_SIGN_ADDR = 0X42, |
|
138 |
|
|
139 |
/* SIC registers */ |
|
140 |
BNO055_SIC_MATRIX_0_LSB_ADDR = 0X43, |
|
141 |
BNO055_SIC_MATRIX_0_MSB_ADDR = 0X44, |
|
142 |
BNO055_SIC_MATRIX_1_LSB_ADDR = 0X45, |
|
143 |
BNO055_SIC_MATRIX_1_MSB_ADDR = 0X46, |
|
144 |
BNO055_SIC_MATRIX_2_LSB_ADDR = 0X47, |
|
145 |
BNO055_SIC_MATRIX_2_MSB_ADDR = 0X48, |
|
146 |
BNO055_SIC_MATRIX_3_LSB_ADDR = 0X49, |
|
147 |
BNO055_SIC_MATRIX_3_MSB_ADDR = 0X4A, |
|
148 |
BNO055_SIC_MATRIX_4_LSB_ADDR = 0X4B, |
|
149 |
BNO055_SIC_MATRIX_4_MSB_ADDR = 0X4C, |
|
150 |
BNO055_SIC_MATRIX_5_LSB_ADDR = 0X4D, |
|
151 |
BNO055_SIC_MATRIX_5_MSB_ADDR = 0X4E, |
|
152 |
BNO055_SIC_MATRIX_6_LSB_ADDR = 0X4F, |
|
153 |
BNO055_SIC_MATRIX_6_MSB_ADDR = 0X50, |
|
154 |
BNO055_SIC_MATRIX_7_LSB_ADDR = 0X51, |
|
155 |
BNO055_SIC_MATRIX_7_MSB_ADDR = 0X52, |
|
156 |
BNO055_SIC_MATRIX_8_LSB_ADDR = 0X53, |
|
157 |
BNO055_SIC_MATRIX_8_MSB_ADDR = 0X54, |
|
158 |
|
|
159 |
/* Accelerometer Offset registers */ |
|
160 |
ACCEL_OFFSET_X_LSB_ADDR = 0X55, |
|
161 |
ACCEL_OFFSET_X_MSB_ADDR = 0X56, |
|
162 |
ACCEL_OFFSET_Y_LSB_ADDR = 0X57, |
|
163 |
ACCEL_OFFSET_Y_MSB_ADDR = 0X58, |
|
164 |
ACCEL_OFFSET_Z_LSB_ADDR = 0X59, |
|
165 |
ACCEL_OFFSET_Z_MSB_ADDR = 0X5A, |
|
166 |
|
|
167 |
/* Magnetometer Offset registers */ |
|
168 |
MAG_OFFSET_X_LSB_ADDR = 0X5B, |
|
169 |
MAG_OFFSET_X_MSB_ADDR = 0X5C, |
|
170 |
MAG_OFFSET_Y_LSB_ADDR = 0X5D, |
|
171 |
MAG_OFFSET_Y_MSB_ADDR = 0X5E, |
|
172 |
MAG_OFFSET_Z_LSB_ADDR = 0X5F, |
|
173 |
MAG_OFFSET_Z_MSB_ADDR = 0X60, |
|
174 |
|
|
175 |
/* Gyroscope Offset register s*/ |
|
176 |
GYRO_OFFSET_X_LSB_ADDR = 0X61, |
|
177 |
GYRO_OFFSET_X_MSB_ADDR = 0X62, |
|
178 |
GYRO_OFFSET_Y_LSB_ADDR = 0X63, |
|
179 |
GYRO_OFFSET_Y_MSB_ADDR = 0X64, |
|
180 |
GYRO_OFFSET_Z_LSB_ADDR = 0X65, |
|
181 |
GYRO_OFFSET_Z_MSB_ADDR = 0X66, |
|
182 |
|
|
183 |
/* Radius registers */ |
|
184 |
ACCEL_RADIUS_LSB_ADDR = 0X67, |
|
185 |
ACCEL_RADIUS_MSB_ADDR = 0X68, |
|
186 |
MAG_RADIUS_LSB_ADDR = 0X69, |
|
187 |
MAG_RADIUS_MSB_ADDR = 0X6A |
|
188 |
} adafruit_bno055_reg_t; |
|
189 |
|
|
190 |
typedef enum |
|
191 |
{ |
|
192 |
POWER_MODE_NORMAL = 0X00, |
|
193 |
POWER_MODE_LOWPOWER = 0X01, |
|
194 |
POWER_MODE_SUSPEND = 0X02 |
|
195 |
} adafruit_bno055_powermode_t; |
|
196 |
|
|
197 |
typedef enum |
|
198 |
{ |
|
199 |
/* Operation mode settings*/ |
|
200 |
OPERATION_MODE_CONFIG = 0X00, |
|
201 |
OPERATION_MODE_ACCONLY = 0X01, |
|
202 |
OPERATION_MODE_MAGONLY = 0X02, |
|
203 |
OPERATION_MODE_GYRONLY = 0X03, |
|
204 |
OPERATION_MODE_ACCMAG = 0X04, |
|
205 |
OPERATION_MODE_ACCGYRO = 0X05, |
|
206 |
OPERATION_MODE_MAGGYRO = 0X06, |
|
207 |
OPERATION_MODE_AMG = 0X07, |
|
208 |
OPERATION_MODE_IMUPLUS = 0X08, |
|
209 |
OPERATION_MODE_COMPASS = 0X09, |
|
210 |
OPERATION_MODE_M4G = 0X0A, |
|
211 |
OPERATION_MODE_NDOF_FMC_OFF = 0X0B, |
|
212 |
OPERATION_MODE_NDOF = 0X0C |
|
213 |
} adafruit_bno055_opmode_t; |
|
214 |
|
|
215 |
typedef struct |
|
216 |
{ |
|
217 |
uint8_t accel_rev; |
|
218 |
uint8_t mag_rev; |
|
219 |
uint8_t gyro_rev; |
|
220 |
uint16_t sw_rev; |
|
221 |
uint8_t bl_rev; |
|
222 |
} adafruit_bno055_rev_info_t; |
|
223 |
|
|
224 |
typedef struct |
|
225 |
{ |
|
226 |
uint8_t system_status; |
|
227 |
uint8_t self_test_result; |
|
228 |
uint8_t system_error; |
|
229 |
} adafruit_bno055_system_status_t; |
|
230 |
|
|
231 |
Adafruit_BNO055 ( int32_t sensorID = -1, uint8_t address = BNO055_ADDRESS_A ); |
|
232 |
|
|
233 |
bool begin ( adafruit_bno055_opmode_t mode = OPERATION_MODE_NDOF ); |
|
234 |
void setMode ( adafruit_bno055_opmode_t mode ); |
|
235 |
void getRevInfo ( adafruit_bno055_rev_info_t* ); |
|
236 |
void displayRevInfo ( void ); |
|
237 |
void getSystemStatus ( adafruit_bno055_system_status_t* ); |
|
238 |
void displaySystemStatus ( void ); |
|
239 |
|
|
240 |
imu::Vector<3> getEuler ( void ); |
|
241 |
imu::Vector<3> getAccel ( void ); |
|
242 |
|
|
243 |
/* Adafruit_Sensor implementation */ |
|
244 |
bool getEvent ( sensors_event_t* ); |
|
245 |
void getSensor ( sensor_t* ); |
|
246 |
|
|
247 |
private: |
|
248 |
byte read8 ( adafruit_bno055_reg_t ); |
|
249 |
bool readLen ( adafruit_bno055_reg_t, byte* buffer, uint8_t len ); |
|
250 |
bool write8 ( adafruit_bno055_reg_t, byte value ); |
|
251 |
|
|
252 |
uint8_t _address; |
|
253 |
int32_t _sensorID; |
|
254 |
adafruit_bno055_opmode_t _mode; |
|
255 |
}; |
|
256 |
|
|
257 |
#endif |
README.md | ||
---|---|---|
1 |
#Adafruit Unified BNO055 Driver (AHRS/Orientation) # |
|
2 |
|
|
3 |
This driver is for the Adafruit BNO055 Breakout (http://www.adafruit.com/products/xxxx), |
|
4 |
and is based on Adafruit's Unified Sensor Library (Adafruit_Sensor). |
|
5 |
|
|
6 |
## What is the Adafruit Unified Sensor Library? ## |
|
7 |
|
|
8 |
The Adafruit Unified Sensor Library ([Adafruit_Sensor](https://github.com/adafruit/Adafruit_Sensor)) provides a common interface and data type for any supported sensor. It defines some basic information about the sensor (sensor limits, etc.), and returns standard SI units of a specific type and scale for each supported sensor type. |
|
9 |
|
|
10 |
It provides a simple abstraction layer between your application and the actual sensor HW, allowing you to drop in any comparable sensor with only one or two lines of code to change in your project (essentially the constructor since the functions to read sensor data and get information about the sensor are defined in the base Adafruit_Sensor class). |
|
11 |
|
|
12 |
This is imporant useful for two reasons: |
|
13 |
|
|
14 |
1.) You can use the data right away because it's already converted to SI units that you understand and can compare, rather than meaningless values like 0..1023. |
|
15 |
|
|
16 |
2.) Because SI units are standardised in the sensor library, you can also do quick sanity checks when working with new sensors, or drop in any comparable sensor if you need better sensitivity or if a lower cost unit becomes available, etc. |
|
17 |
|
|
18 |
Light sensors will always report units in lux, gyroscopes will always report units in rad/s, etc. ... freeing you up to focus on the data, rather than digging through the datasheet to understand what the sensor's raw numbers really mean. |
|
19 |
|
|
20 |
## About this Driver ## |
|
21 |
|
|
22 |
Adafruit invests time and resources providing this open source code. Please support Adafruit and open-source hardware by purchasing products from Adafruit! |
|
23 |
|
|
24 |
Written by Kevin (KTOWN) Townsend for Adafruit Industries. |
examples/sensorapi/sensorapi.ino | ||
---|---|---|
1 |
#include <Wire.h> |
|
2 |
#include <Adafruit_Sensor.h> |
|
3 |
#include <Adafruit_BNO055.h> |
|
4 |
#include <utility/imumaths.h> |
|
5 |
|
|
6 |
/* This driver uses the Adafruit unified sensor library (Adafruit_Sensor), |
|
7 |
which provides a common 'type' for sensor data and some helper functions. |
|
8 |
|
|
9 |
To use this driver you will also need to download the Adafruit_Sensor |
|
10 |
library and include it in your libraries folder. |
|
11 |
|
|
12 |
You should also assign a unique ID to this sensor for use with |
|
13 |
the Adafruit Sensor API so that you can identify this particular |
|
14 |
sensor in any data logs, etc. To assign a unique ID, simply |
|
15 |
provide an appropriate value in the constructor below (12345 |
|
16 |
is used by default in this example). |
|
17 |
|
|
18 |
Connections |
|
19 |
=========== |
|
20 |
Connect SCL to analog 5 |
|
21 |
Connect SDA to analog 4 |
|
22 |
Connect VDD to 3.3V DC |
|
23 |
Connect GROUND to common ground |
|
24 |
|
|
25 |
History |
|
26 |
======= |
|
27 |
2015/MAR/03 - First release (KTOWN) |
|
28 |
*/ |
|
29 |
|
|
30 |
/* Set the delay between fresh samples */ |
|
31 |
#define BNO055_SAMPLERATE_DELAY_MS (500) |
|
32 |
|
|
33 |
Adafruit_BNO055 bno = Adafruit_BNO055(55); |
|
34 |
|
|
35 |
/**************************************************************************/ |
|
36 |
/* |
|
37 |
Displays some basic information on this sensor from the unified |
|
38 |
sensor API sensor_t type (see Adafruit_Sensor for more information) |
|
39 |
*/ |
|
40 |
/**************************************************************************/ |
|
41 |
void displaySensorDetails(void) |
|
42 |
{ |
|
43 |
sensor_t sensor; |
|
44 |
bno.getSensor(&sensor); |
|
45 |
Serial.println("------------------------------------"); |
|
46 |
Serial.print ("Sensor: "); Serial.println(sensor.name); |
|
47 |
Serial.print ("Driver Ver: "); Serial.println(sensor.version); |
|
48 |
Serial.print ("Unique ID: "); Serial.println(sensor.sensor_id); |
|
49 |
Serial.print ("Max Value: "); Serial.print(sensor.max_value); Serial.println(" xxx"); |
|
50 |
Serial.print ("Min Value: "); Serial.print(sensor.min_value); Serial.println(" xxx"); |
|
51 |
Serial.print ("Resolution: "); Serial.print(sensor.resolution); Serial.println(" xxx"); |
|
52 |
Serial.println("------------------------------------"); |
|
53 |
Serial.println(""); |
|
54 |
delay(500); |
|
55 |
} |
|
56 |
|
|
57 |
/**************************************************************************/ |
|
58 |
/* |
|
59 |
Arduino setup function (automatically called at startup) |
|
60 |
*/ |
|
61 |
/**************************************************************************/ |
|
62 |
void setup(void) |
|
63 |
{ |
|
64 |
Serial.begin(9600); |
|
65 |
Serial.println("Orientation Sensor Test"); Serial.println(""); |
|
66 |
|
|
67 |
/* Initialise the sensor */ |
|
68 |
if(!bno.begin()) |
|
69 |
{ |
|
70 |
/* There was a problem detecting the BNO055 ... check your connections */ |
|
71 |
Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!"); |
|
72 |
while(1); |
|
73 |
} |
|
74 |
|
|
75 |
delay(1000); |
|
76 |
|
|
77 |
/* Display some basic information on this sensor */ |
|
78 |
displaySensorDetails(); |
|
79 |
|
|
80 |
/* Display system info (optional) */ |
|
81 |
bno.displaySystemStatus(); |
|
82 |
Serial.println(""); |
|
83 |
|
|
84 |
/* Display chip revision details (optional) */ |
|
85 |
bno.displayRevInfo(); |
|
86 |
Serial.println(""); |
|
87 |
} |
|
88 |
|
|
89 |
/**************************************************************************/ |
|
90 |
/* |
|
91 |
Arduino loop function, called once 'setup' is complete (your own code |
|
92 |
should go here) |
|
93 |
*/ |
|
94 |
/**************************************************************************/ |
|
95 |
void loop(void) |
|
96 |
{ |
|
97 |
imu::Vector<3> euler = bno.getEuler(); |
|
98 |
|
|
99 |
Serial.print("X: "); |
|
100 |
Serial.println((int)euler.x(), DEC); |
|
101 |
Serial.print("Y: "); |
|
102 |
Serial.println((int)euler.y(), DEC); |
|
103 |
Serial.print("Z: "); |
|
104 |
Serial.println((int)euler.z(), DEC); |
|
105 |
Serial.println(""); |
|
106 |
|
|
107 |
delay(BNO055_SAMPLERATE_DELAY_MS); |
|
108 |
} |
utility/imumaths.h | ||
---|---|---|
1 |
/* |
|
2 |
Inertial Measurement Unit Maths Library |
|
3 |
Copyright (C) 2013-2014 Samuel Cowen |
|
4 |
www.camelsoftware.com |
|
5 |
|
|
6 |
This program is free software: you can redistribute it and/or modify |
|
7 |
it under the terms of the GNU General Public License as published by |
|
8 |
the Free Software Foundation, either version 3 of the License, or |
|
9 |
(at your option) any later version. |
|
10 |
|
|
11 |
This program is distributed in the hope that it will be useful, |
|
12 |
but WITHOUT ANY WARRANTY; without even the implied warranty of |
|
13 |
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
|
14 |
GNU General Public License for more details. |
|
15 |
|
|
16 |
You should have received a copy of the GNU General Public License |
|
17 |
along with this program. If not, see <http://www.gnu.org/licenses/>. |
|
18 |
*/ |
|
19 |
|
|
20 |
#ifndef IMUMATH_H |
|
21 |
#define IMUMATH_H |
|
22 |
|
|
23 |
|
|
24 |
#include "vector.h" |
|
25 |
#include "matrix.h" |
|
26 |
#include "quaternion.h" |
|
27 |
|
|
28 |
|
|
29 |
#endif |
|
30 |
|
utility/matrix.h | ||
---|---|---|
1 |
/* |
|
2 |
Inertial Measurement Unit Maths Library |
|
3 |
Copyright (C) 2013-2014 Samuel Cowen |
|
4 |
www.camelsoftware.com |
|
5 |
|
|
6 |
This program is free software: you can redistribute it and/or modify |
|
7 |
it under the terms of the GNU General Public License as published by |
|
8 |
the Free Software Foundation, either version 3 of the License, or |
|
9 |
(at your option) any later version. |
|
10 |
|
|
11 |
This program is distributed in the hope that it will be useful, |
|
12 |
but WITHOUT ANY WARRANTY; without even the implied warranty of |
|
13 |
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
|
14 |
GNU General Public License for more details. |
|
15 |
|
|
16 |
You should have received a copy of the GNU General Public License |
|
17 |
along with this program. If not, see <http://www.gnu.org/licenses/>. |
|
18 |
*/ |
|
19 |
|
|
20 |
#ifndef IMUMATH_MATRIX_HPP |
|
21 |
#define IMUMATH_MATRIX_HPP |
|
22 |
|
|
23 |
#include <stdlib.h> |
|
24 |
#include <string.h> |
|
25 |
#include <stdint.h> |
|
26 |
#include <math.h> |
|
27 |
|
|
28 |
namespace imu |
|
29 |
{ |
|
30 |
|
|
31 |
|
|
32 |
template <uint8_t N> class Matrix |
|
33 |
{ |
|
34 |
public: |
|
35 |
Matrix() |
|
36 |
{ |
|
37 |
int r = sizeof(double)*N; |
|
38 |
_cell = (double*)malloc(r*r); |
|
39 |
memset(_cell, 0, r*r); |
|
40 |
} |
|
41 |
|
|
42 |
Matrix(const Matrix &v) |
|
43 |
{ |
|
44 |
int r = sizeof(double)*N; |
|
45 |
_cell = (double*)malloc(r*r); |
|
46 |
memset(_cell, 0, r*r); |
|
47 |
for (int x = 0; x < N; x++ ) |
|
48 |
{ |
|
49 |
for(int y = 0; y < N; y++) |
|
50 |
{ |
|
51 |
_cell[x*N+y] = v._cell[x*N+y]; |
|
52 |
} |
|
53 |
} |
|
54 |
} |
|
55 |
|
|
56 |
~Matrix() |
|
57 |
{ |
|
58 |
free(_cell); |
|
59 |
} |
|
60 |
|
|
61 |
void operator = (Matrix m) |
|
62 |
{ |
|
63 |
for(int x = 0; x < N; x++) |
|
64 |
{ |
|
65 |
for(int y = 0; y < N; y++) |
|
66 |
{ |
|
67 |
cell(x, y) = m.cell(x, y); |
|
68 |
} |
|
69 |
} |
|
70 |
} |
|
71 |
|
|
72 |
Vector<N> row_to_vector(int y) |
|
73 |
{ |
|
74 |
Vector<N> ret; |
|
75 |
for(int i = 0; i < N; i++) |
|
76 |
{ |
|
77 |
ret[i] = _cell[y*N+i]; |
|
78 |
} |
|
79 |
return ret; |
|
80 |
} |
|
81 |
|
|
82 |
Vector<N> col_to_vector(int x) |
|
83 |
{ |
|
84 |
Vector<N> ret; |
|
85 |
for(int i = 0; i < N; i++) |
|
86 |
{ |
|
87 |
ret[i] = _cell[i*N+x]; |
|
88 |
} |
|
89 |
return ret; |
|
90 |
} |
|
91 |
|
|
92 |
void vector_to_row(Vector<N> v, int row) |
|
93 |
{ |
|
94 |
for(int i = 0; i < N; i++) |
|
95 |
{ |
|
96 |
cell(row, i) = v(i); |
|
97 |
} |
|
98 |
} |
|
99 |
|
|
100 |
void vector_to_col(Vector<N> v, int col) |
|
101 |
{ |
|
102 |
for(int i = 0; i < N; i++) |
|
103 |
{ |
|
104 |
cell(i, col) = v(i); |
|
105 |
} |
|
106 |
} |
|
107 |
|
|
108 |
double& operator ()(int x, int y) |
|
109 |
{ |
|
110 |
return _cell[x*N+y]; |
|
111 |
} |
|
112 |
|
|
113 |
double& cell(int x, int y) |
|
114 |
{ |
|
115 |
return _cell[x*N+y]; |
|
116 |
} |
|
117 |
|
|
118 |
|
|
119 |
Matrix operator + (Matrix m) |
|
120 |
{ |
|
121 |
Matrix ret; |
|
122 |
for(int x = 0; x < N; x++) |
|
123 |
{ |
|
124 |
for(int y = 0; y < N; y++) |
|
125 |
{ |
|
126 |
ret._cell[x*N+y] = _cell[x*N+y] + m._cell[x*N+y]; |
|
127 |
} |
|
128 |
} |
|
129 |
return ret; |
|
130 |
} |
|
131 |
|
|
132 |
Matrix operator - (Matrix m) |
|
133 |
{ |
|
134 |
Matrix ret; |
|
135 |
for(int x = 0; x < N; x++) |
|
136 |
{ |
|
137 |
for(int y = 0; y < N; y++) |
|
138 |
{ |
|
139 |
ret._cell[x*N+y] = _cell[x*N+y] - m._cell[x*N+y]; |
|
140 |
} |
|
141 |
} |
|
142 |
return ret; |
|
143 |
} |
|
144 |
|
|
145 |
Matrix operator * (double scalar) |
|
146 |
{ |
|
147 |
Matrix ret; |
|
148 |
for(int x = 0; x < N; x++) |
|
149 |
{ |
|
150 |
for(int y = 0; y < N; y++) |
|
151 |
{ |
|
152 |
ret._cell[x*N+y] = _cell[x*N+y] * scalar; |
|
153 |
} |
|
154 |
} |
|
155 |
return ret; |
|
156 |
} |
|
157 |
|
|
158 |
Matrix operator * (Matrix m) |
|
159 |
{ |
|
160 |
Matrix ret; |
|
161 |
for(int x = 0; x < N; x++) |
|
162 |
{ |
|
163 |
for(int y = 0; y < N; y++) |
|
164 |
{ |
|
165 |
Vector<N> row = row_to_vector(x); |
|
166 |
Vector<N> col = m.col_to_vector(y); |
|
167 |
ret.cell(x, y) = row.dot(col); |
|
168 |
} |
|
169 |
} |
|
170 |
return ret; |
|
171 |
} |
|
172 |
|
|
173 |
Matrix transpose() |
|
174 |
{ |
|
175 |
Matrix ret; |
|
176 |
for(int x = 0; x < N; x++) |
|
177 |
{ |
|
178 |
for(int y = 0; y < N; y++) |
|
179 |
{ |
|
180 |
ret.cell(y, x) = cell(x, y); |
|
181 |
} |
|
182 |
} |
|
183 |
return ret; |
|
184 |
} |
|
185 |
|
|
186 |
Matrix<N-1> minor_matrix(int row, int col) |
|
187 |
{ |
|
188 |
int colCount = 0, rowCount = 0; |
|
189 |
Matrix<N-1> ret; |
|
190 |
for(int i = 0; i < N; i++ ) |
|
191 |
{ |
|
192 |
if( i != row ) |
|
193 |
{ |
|
194 |
for(int j = 0; j < N; j++ ) |
|
195 |
{ |
|
196 |
if( j != col ) |
|
197 |
{ |
|
198 |
ret(rowCount, colCount) = cell(i, j); |
|
199 |
colCount++; |
|
200 |
} |
|
201 |
} |
|
202 |
rowCount++; |
|
203 |
} |
|
204 |
} |
|
205 |
return ret; |
|
206 |
} |
|
207 |
|
|
208 |
double determinant() |
|
209 |
{ |
|
210 |
if(N == 1) |
|
211 |
return cell(0, 0); |
|
212 |
|
|
213 |
float det = 0.0; |
|
214 |
for(int i = 0; i < N; i++ ) |
|
215 |
{ |
|
216 |
Matrix<N-1> minor = minor_matrix(0, i); |
|
217 |
det += (i%2==1?-1.0:1.0) * cell(0, i) * minor.determinant(); |
|
218 |
} |
|
219 |
return det; |
|
220 |
} |
|
221 |
|
|
222 |
Matrix invert() |
|
223 |
{ |
|
224 |
Matrix ret; |
|
225 |
float det = determinant(); |
|
226 |
|
|
227 |
for(int x = 0; x < N; x++) |
|
228 |
{ |
|
229 |
for(int y = 0; y < N; y++) |
|
230 |
{ |
|
231 |
Matrix<N-1> minor = minor_matrix(y, x); |
|
232 |
ret(x, y) = det*minor.determinant(); |
|
233 |
if( (x+y)%2 == 1) |
|
234 |
ret(x, y) = -ret(x, y); |
|
235 |
} |
|
236 |
} |
|
237 |
return ret; |
|
238 |
} |
|
239 |
|
|
240 |
private: |
|
241 |
double* _cell; |
|
242 |
}; |
|
243 |
|
|
244 |
|
|
245 |
}; |
|
246 |
|
|
247 |
#endif |
|
248 |
|
utility/quaternion.h | ||
---|---|---|
1 |
/* |
|
2 |
Inertial Measurement Unit Maths Library |
|
3 |
Copyright (C) 2013-2014 Samuel Cowen |
|
4 |
www.camelsoftware.com |
|
5 |
|
|
6 |
This program is free software: you can redistribute it and/or modify |
|
7 |
it under the terms of the GNU General Public License as published by |
|
8 |
the Free Software Foundation, either version 3 of the License, or |
|
9 |
(at your option) any later version. |
|
10 |
|
|
11 |
This program is distributed in the hope that it will be useful, |
|
12 |
but WITHOUT ANY WARRANTY; without even the implied warranty of |
|
13 |
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
|
14 |
GNU General Public License for more details. |
|
15 |
|
|
16 |
You should have received a copy of the GNU General Public License |
|
17 |
along with this program. If not, see <http://www.gnu.org/licenses/>. |
|
18 |
*/ |
|
19 |
|
|
20 |
|
|
21 |
#ifndef IMUMATH_QUATERNION_HPP |
|
22 |
#define IMUMATH_QUATERNION_HPP |
|
23 |
|
|
24 |
#include <stdlib.h> |
|
25 |
#include <string.h> |
|
26 |
#include <stdint.h> |
|
27 |
#include <math.h> |
|
28 |
|
|
29 |
#include "vector.h" |
|
30 |
|
|
31 |
|
|
32 |
namespace imu |
|
33 |
{ |
|
34 |
|
|
35 |
|
|
36 |
|
|
37 |
class Quaternion |
|
38 |
{ |
|
39 |
public: |
|
40 |
Quaternion() |
|
41 |
{ |
|
42 |
_w = 1.0; |
|
43 |
_x = _y = _z = 0.0; |
|
44 |
} |
|
45 |
|
|
46 |
Quaternion(double iw, double ix, double iy, double iz) |
|
47 |
{ |
|
48 |
_w = iw; |
|
49 |
_x = ix; |
|
50 |
_y = iy; |
|
51 |
_z = iz; |
|
52 |
} |
|
53 |
|
|
54 |
Quaternion(double w, Vector<3> vec) |
|
55 |
{ |
|
56 |
_w = w; |
|
57 |
_x = vec.x(); |
|
58 |
_y = vec.y(); |
|
59 |
_z = vec.z(); |
|
60 |
} |
|
61 |
|
|
62 |
double& w() |
|
63 |
{ |
|
64 |
return _w; |
|
65 |
} |
|
66 |
double& x() |
|
67 |
{ |
|
68 |
return _x; |
|
69 |
} |
|
70 |
double& y() |
|
71 |
{ |
|
72 |
return _y; |
|
73 |
} |
|
74 |
double& z() |
|
75 |
{ |
|
76 |
return _z; |
|
77 |
} |
|
78 |
|
|
79 |
double magnitude() |
|
80 |
{ |
|
81 |
double res = (_w*_w) + (_x*_x) + (_y*_y) + (_z*_z); |
|
82 |
return sqrt(res); |
|
83 |
} |
|
84 |
|
|
85 |
void normalize() |
|
86 |
{ |
|
87 |
double mag = magnitude(); |
|
88 |
*this = this->scale(1/mag); |
|
89 |
} |
|
90 |
|
|
91 |
|
|
92 |
Quaternion conjugate() |
|
93 |
{ |
|
94 |
Quaternion q; |
|
95 |
q.w() = _w; |
|
96 |
q.x() = -_x; |
|
97 |
q.y() = -_y; |
|
98 |
q.z() = -_z; |
|
99 |
return q; |
|
100 |
} |
|
101 |
|
|
102 |
void fromAxisAngle(Vector<3> axis, double theta) |
|
103 |
{ |
|
104 |
_w = cos(theta/2); |
|
105 |
//only need to calculate sine of half theta once |
|
106 |
double sht = sin(theta/2); |
|
107 |
_x = axis.x() * sht; |
|
108 |
_y = axis.y() * sht; |
|
109 |
_z = axis.z() * sht; |
|
110 |
} |
|
111 |
|
|
112 |
void fromMatrix(Matrix<3> m) |
|
113 |
{ |
|
114 |
float tr = m(0, 0) + m(1, 1) + m(2, 2); |
|
115 |
|
|
116 |
float S = 0.0; |
|
117 |
if (tr > 0) |
|
118 |
{ |
|
119 |
S = sqrt(tr+1.0) * 2; |
|
120 |
_w = 0.25 * S; |
|
121 |
_x = (m(2, 1) - m(1, 2)) / S; |
|
122 |
_y = (m(0, 2) - m(2, 0)) / S; |
|
123 |
_z = (m(1, 0) - m(0, 1)) / S; |
|
124 |
} |
|
125 |
else if ((m(0, 0) < m(1, 1))&(m(0, 0) < m(2, 2))) |
|
126 |
{ |
|
127 |
S = sqrt(1.0 + m(0, 0) - m(1, 1) - m(2, 2)) * 2; |
|
128 |
_w = (m(2, 1) - m(1, 2)) / S; |
|
129 |
_x = 0.25 * S; |
|
130 |
_y = (m(0, 1) + m(1, 0)) / S; |
|
131 |
_z = (m(0, 2) + m(2, 0)) / S; |
|
132 |
} |
|
133 |
else if (m(1, 1) < m(2, 2)) |
|
134 |
{ |
|
135 |
S = sqrt(1.0 + m(1, 1) - m(0, 0) - m(2, 2)) * 2; |
|
136 |
_w = (m(0, 2) - m(2, 0)) / S; |
|
137 |
_x = (m(0, 1) + m(1, 0)) / S; |
|
138 |
_y = 0.25 * S; |
|
139 |
_z = (m(1, 2) + m(2, 1)) / S; |
|
140 |
} |
|
141 |
else |
|
142 |
{ |
|
143 |
S = sqrt(1.0 + m(2, 2) - m(0, 0) - m(1, 1)) * 2; |
|
144 |
_w = (m(1, 0) - m(0, 1)) / S; |
|
145 |
_x = (m(0, 2) + m(2, 0)) / S; |
|
146 |
_y = (m(1, 2) + m(2, 1)) / S; |
|
147 |
_z = 0.25 * S; |
|
148 |
} |
|
149 |
} |
|
150 |
|
|
151 |
void toAxisAngle(Vector<3>& axis, float& angle) |
|
152 |
{ |
|
153 |
float sqw = sqrt(1-_w*_w); |
|
154 |
if(sqw == 0) //it's a singularity and divide by zero, avoid |
|
155 |
return; |
|
156 |
|
|
157 |
angle = 2 * acos(_w); |
|
158 |
axis.x() = _x / sqw; |
|
159 |
axis.y() = _y / sqw; |
|
160 |
axis.z() = _z / sqw; |
|
161 |
} |
|
162 |
|
|
163 |
Matrix<3> toMatrix() |
|
164 |
{ |
|
165 |
Matrix<3> ret; |
|
166 |
ret.cell(0, 0) = 1-(2*(_y*_y))-(2*(_z*_z)); |
|
167 |
ret.cell(0, 1) = (2*_x*_y)-(2*_w*_z); |
|
168 |
ret.cell(0, 2) = (2*_x*_z)+(2*_w*_y); |
|
169 |
|
|
170 |
ret.cell(1, 0) = (2*_x*_y)+(2*_w*_z); |
|
171 |
ret.cell(1, 1) = 1-(2*(_x*_x))-(2*(_z*_z)); |
|
172 |
ret.cell(1, 2) = (2*(_y*_z))-(2*(_w*_x)); |
|
173 |
|
|
174 |
ret.cell(2, 0) = (2*(_x*_z))-(2*_w*_y); |
|
175 |
ret.cell(2, 1) = (2*_y*_z)+(2*_w*_x); |
|
176 |
ret.cell(2, 2) = 1-(2*(_x*_x))-(2*(_y*_y)); |
|
177 |
return ret; |
|
178 |
} |
|
179 |
|
|
180 |
|
|
181 |
Vector<3> toEuler() |
|
182 |
{ |
|
183 |
Vector<3> ret; |
|
184 |
double sqw = _w*_w; |
|
185 |
double sqx = _x*_x; |
|
186 |
double sqy = _y*_y; |
|
187 |
double sqz = _z*_z; |
|
188 |
|
|
189 |
ret.x() = atan2(2.0*(_x*_y+_z*_w),(sqx-sqy-sqz+sqw)); |
|
190 |
ret.y() = asin(-2.0*(_x*_z-_y*_w)/(sqx+sqy+sqz+sqw)); |
|
191 |
ret.z() = atan2(2.0*(_y*_z+_x*_w),(-sqx-sqy+sqz+sqw)); |
|
192 |
|
|
193 |
return ret; |
|
194 |
} |
|
195 |
|
|
196 |
Vector<3> toAngularVelocity(float dt) |
|
197 |
{ |
|
198 |
Vector<3> ret; |
|
199 |
Quaternion one(1.0, 0.0, 0.0, 0.0); |
|
200 |
Quaternion delta = one - *this; |
|
201 |
Quaternion r = (delta/dt); |
|
202 |
r = r * 2; |
|
203 |
r = r * one; |
|
204 |
|
|
205 |
ret.x() = r.x(); |
|
206 |
ret.y() = r.y(); |
|
207 |
ret.z() = r.z(); |
|
208 |
return ret; |
|
209 |
} |
|
210 |
|
|
211 |
Vector<3> rotateVector(Vector<2> v) |
|
212 |
{ |
|
213 |
Vector<3> ret(v.x(), v.y(), 0.0); |
|
214 |
return rotateVector(ret); |
|
215 |
} |
|
216 |
|
|
217 |
Vector<3> rotateVector(Vector<3> v) |
|
218 |
{ |
|
219 |
Vector<3> qv(this->x(), this->y(), this->z()); |
|
220 |
Vector<3> t; |
|
221 |
t = qv.cross(v) * 2.0; |
|
222 |
return v + (t * _w) + qv.cross(t); |
|
223 |
} |
|
224 |
|
|
225 |
|
|
226 |
Quaternion operator * (Quaternion q) |
|
227 |
{ |
|
228 |
Quaternion ret; |
|
229 |
ret._w = ((_w*q._w) - (_x*q._x) - (_y*q._y) - (_z*q._z)); |
|
230 |
ret._x = ((_w*q._x) + (_x*q._w) + (_y*q._z) - (_z*q._y)); |
|
231 |
ret._y = ((_w*q._y) - (_x*q._z) + (_y*q._w) + (_z*q._x)); |
|
232 |
ret._z = ((_w*q._z) + (_x*q._y) - (_y*q._x) + (_z*q._w)); |
|
233 |
return ret; |
|
234 |
} |
|
235 |
|
|
236 |
Quaternion operator + (Quaternion q) |
|
237 |
{ |
|
238 |
Quaternion ret; |
|
239 |
ret._w = _w + q._w; |
|
240 |
ret._x = _x + q._x; |
|
241 |
ret._y = _y + q._y; |
|
242 |
ret._z = _z + q._z; |
|
243 |
return ret; |
|
244 |
} |
|
245 |
|
|
246 |
Quaternion operator - (Quaternion q) |
|
247 |
{ |
|
248 |
Quaternion ret; |
|
249 |
ret._w = _w - q._w; |
|
250 |
ret._x = _x - q._x; |
|
251 |
ret._y = _y - q._y; |
|
252 |
ret._z = _z - q._z; |
|
253 |
return ret; |
|
254 |
} |
|
255 |
|
|
256 |
Quaternion operator / (float scalar) |
|
257 |
{ |
|
258 |
Quaternion ret; |
|
259 |
ret._w = this->_w/scalar; |
|
260 |
ret._x = this->_x/scalar; |
|
261 |
ret._y = this->_y/scalar; |
|
262 |
ret._z = this->_z/scalar; |
|
263 |
return ret; |
|
264 |
} |
|
265 |
|
|
266 |
Quaternion operator * (float scalar) |
|
267 |
{ |
|
268 |
Quaternion ret; |
|
269 |
ret._w = this->_w*scalar; |
|
270 |
ret._x = this->_x*scalar; |
|
271 |
ret._y = this->_y*scalar; |
|
272 |
ret._z = this->_z*scalar; |
|
273 |
return ret; |
|
274 |
} |
|
275 |
|
|
276 |
Quaternion scale(double scalar) |
|
277 |
{ |
|
278 |
Quaternion ret; |
|
279 |
ret._w = this->_w*scalar; |
|
280 |
ret._x = this->_x*scalar; |
|
281 |
ret._y = this->_y*scalar; |
|
282 |
ret._z = this->_z*scalar; |
|
283 |
return ret; |
|
284 |
} |
|
285 |
|
|
286 |
private: |
|
287 |
double _w, _x, _y, _z; |
|
288 |
}; |
|
289 |
|
|
290 |
|
|
291 |
}; |
|
292 |
|
|
293 |
#endif |
utility/vector.h | ||
---|---|---|
1 |
/* |
|
2 |
Inertial Measurement Unit Maths Library |
|
3 |
Copyright (C) 2013-2014 Samuel Cowen |
|
4 |
www.camelsoftware.com |
|
5 |
|
|
6 |
This program is free software: you can redistribute it and/or modify |
|
7 |
it under the terms of the GNU General Public License as published by |
|
8 |
the Free Software Foundation, either version 3 of the License, or |
|
9 |
(at your option) any later version. |
|
10 |
|
|
11 |
This program is distributed in the hope that it will be useful, |
|
12 |
but WITHOUT ANY WARRANTY; without even the implied warranty of |
|
13 |
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
|
14 |
GNU General Public License for more details. |
|
15 |
|
|
16 |
You should have received a copy of the GNU General Public License |
|
17 |
along with this program. If not, see <http://www.gnu.org/licenses/>. |
|
18 |
*/ |
|
19 |
|
|
20 |
#ifndef IMUMATH_VECTOR_HPP |
|
21 |
#define IMUMATH_VECTOR_HPP |
|
22 |
|
|
23 |
#include <stdlib.h> |
|
24 |
#include <string.h> |
|
25 |
#include <stdint.h> |
|
26 |
#include <math.h> |
|
27 |
|
|
28 |
|
|
29 |
namespace imu |
|
30 |
{ |
|
31 |
|
|
32 |
template <uint8_t N> class Vector |
|
33 |
{ |
|
34 |
public: |
|
35 |
Vector() |
|
36 |
{ |
|
37 |
p_vec = (double*)malloc(sizeof(double)*N+1); |
|
38 |
memset(p_vec, 0, sizeof(double)*N); |
|
39 |
} |
|
40 |
|
|
41 |
Vector(double a) |
|
42 |
{ |
|
43 |
p_vec = (double*)malloc(sizeof(double)*N+1); |
|
44 |
memset(p_vec, 0, sizeof(double)*N); |
|
45 |
p_vec[0] = a; |
|
46 |
} |
|
47 |
|
|
48 |
Vector(double a, double b) |
|
49 |
{ |
|
50 |
p_vec = (double*)malloc(sizeof(double)*N+1); |
|
51 |
memset(p_vec, 0, sizeof(double)*N); |
|
52 |
p_vec[0] = a; |
|
53 |
p_vec[1] = b; |
|
54 |
} |
|
55 |
|
|
56 |
Vector(double a, double b, double c) |
|
57 |
{ |
|
58 |
p_vec = (double*)malloc(sizeof(double)*N+1); |
|
59 |
memset(p_vec, 0, sizeof(double)*N); |
|
60 |
p_vec[0] = a; |
|
61 |
p_vec[1] = b; |
|
62 |
p_vec[2] = c; |
|
63 |
} |
|
64 |
|
|
65 |
Vector(double a, double b, double c, double d) |
|
66 |
{ |
|
67 |
p_vec = (double*)malloc(sizeof(double)*N+1); |
|
68 |
memset(p_vec, 0, sizeof(double)*N); |
|
69 |
p_vec[0] = a; |
|
70 |
p_vec[1] = b; |
|
71 |
p_vec[2] = c; |
|
72 |
p_vec[3] = d; |
|
73 |
} |
|
74 |
|
|
75 |
Vector(const Vector<N> &v) |
|
76 |
{ |
|
77 |
p_vec = (double*)malloc(sizeof(double)*N); |
|
78 |
memset(p_vec, 0, sizeof(double)*N); |
|
79 |
for (int x = 0; x < N; x++ ) |
|
80 |
p_vec[x] = v.p_vec[x]; |
|
81 |
} |
|
82 |
|
|
83 |
~Vector() |
|
84 |
{ |
|
85 |
free(p_vec); |
|
86 |
} |
|
87 |
|
Also available in: Unified diff