Revision 4bc1c0c1

View differences:

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

  
... This diff was truncated because it exceeds the maximum size that can be displayed.

Also available in: Unified diff