Revision d964148c
| Adafruit_BNO055.h | ||
|---|---|---|
| 25 | 25 |
#else |
| 26 | 26 |
#include "WProgram.h" |
| 27 | 27 |
#endif |
| 28 |
#include "Wire.h" |
|
| 28 |
|
|
| 29 |
#ifdef __AVR_ATtiny85__ |
|
| 30 |
#include <TinyWireM.h> |
|
| 31 |
#define Wire TinyWireM |
|
| 32 |
#else |
|
| 33 |
#include <Wire.h> |
|
| 34 |
#endif |
|
| 29 | 35 |
|
| 30 | 36 |
#include <Adafruit_Sensor.h> |
| 31 | 37 |
#include <utility/imumaths.h> |
| utility/matrix.h | ||
|---|---|---|
| 1 | 1 |
/* |
| 2 | 2 |
Inertial Measurement Unit Maths Library |
| 3 | 3 |
Copyright (C) 2013-2014 Samuel Cowen |
| 4 |
www.camelsoftware.com
|
|
| 4 |
www.camelsoftware.com
|
|
| 5 | 5 |
|
| 6 | 6 |
This program is free software: you can redistribute it and/or modify |
| 7 | 7 |
it under the terms of the GNU General Public License as published by |
| ... | ... | |
| 32 | 32 |
template <uint8_t N> class Matrix |
| 33 | 33 |
{
|
| 34 | 34 |
public: |
| 35 |
Matrix()
|
|
| 36 |
{
|
|
| 37 |
int r = sizeof(double)*N;
|
|
| 38 |
_cell = (double*)malloc(r*r);
|
|
| 35 |
Matrix()
|
|
| 36 |
{
|
|
| 37 |
int r = sizeof(double)*N;
|
|
| 38 |
_cell = &_cell_data[0];
|
|
| 39 | 39 |
memset(_cell, 0, r*r); |
| 40 |
}
|
|
| 40 |
}
|
|
| 41 | 41 |
|
| 42 | 42 |
Matrix(const Matrix &v) |
| 43 | 43 |
{
|
| 44 | 44 |
int r = sizeof(double)*N; |
| 45 |
_cell = (double*)malloc(r*r);
|
|
| 45 |
_cell = &_cell_data[0];
|
|
| 46 | 46 |
memset(_cell, 0, r*r); |
| 47 | 47 |
for (int x = 0; x < N; x++ ) |
| 48 | 48 |
{
|
| ... | ... | |
| 55 | 55 |
|
| 56 | 56 |
~Matrix() |
| 57 | 57 |
{
|
| 58 |
free(_cell); |
|
| 59 | 58 |
} |
| 60 | 59 |
|
| 61 | 60 |
void operator = (Matrix m) |
| ... | ... | |
| 239 | 238 |
|
| 240 | 239 |
private: |
| 241 | 240 |
double* _cell; |
| 241 |
double _cell_data[N*N]; |
|
| 242 | 242 |
}; |
| 243 | 243 |
|
| 244 | 244 |
|
| utility/vector.h | ||
|---|---|---|
| 1 | 1 |
/* |
| 2 | 2 |
Inertial Measurement Unit Maths Library |
| 3 | 3 |
Copyright (C) 2013-2014 Samuel Cowen |
| 4 |
www.camelsoftware.com
|
|
| 4 |
www.camelsoftware.com
|
|
| 5 | 5 |
|
| 6 | 6 |
This program is free software: you can redistribute it and/or modify |
| 7 | 7 |
it under the terms of the GNU General Public License as published by |
| ... | ... | |
| 32 | 32 |
template <uint8_t N> class Vector |
| 33 | 33 |
{
|
| 34 | 34 |
public: |
| 35 |
Vector()
|
|
| 36 |
{
|
|
| 37 |
p_vec = (double*)malloc(sizeof(double)*N+1);
|
|
| 35 |
Vector()
|
|
| 36 |
{
|
|
| 37 |
p_vec = &p_vec_data[0];
|
|
| 38 | 38 |
memset(p_vec, 0, sizeof(double)*N); |
| 39 |
}
|
|
| 39 |
}
|
|
| 40 | 40 |
|
| 41 |
Vector(double a)
|
|
| 42 |
{
|
|
| 43 |
p_vec = (double*)malloc(sizeof(double)*N+1);
|
|
| 41 |
Vector(double a)
|
|
| 42 |
{
|
|
| 43 |
p_vec = &p_vec_data[0];
|
|
| 44 | 44 |
memset(p_vec, 0, sizeof(double)*N); |
| 45 |
p_vec[0] = a;
|
|
| 46 |
}
|
|
| 45 |
p_vec[0] = a;
|
|
| 46 |
}
|
|
| 47 | 47 |
|
| 48 |
Vector(double a, double b)
|
|
| 49 |
{
|
|
| 50 |
p_vec = (double*)malloc(sizeof(double)*N+1);
|
|
| 48 |
Vector(double a, double b)
|
|
| 49 |
{
|
|
| 50 |
p_vec = &p_vec_data[0];
|
|
| 51 | 51 |
memset(p_vec, 0, sizeof(double)*N); |
| 52 |
p_vec[0] = a;
|
|
| 53 |
p_vec[1] = b;
|
|
| 54 |
}
|
|
| 52 |
p_vec[0] = a;
|
|
| 53 |
p_vec[1] = b;
|
|
| 54 |
}
|
|
| 55 | 55 |
|
| 56 |
Vector(double a, double b, double c)
|
|
| 57 |
{
|
|
| 58 |
p_vec = (double*)malloc(sizeof(double)*N+1);
|
|
| 56 |
Vector(double a, double b, double c)
|
|
| 57 |
{
|
|
| 58 |
p_vec = &p_vec_data[0];
|
|
| 59 | 59 |
memset(p_vec, 0, sizeof(double)*N); |
| 60 |
p_vec[0] = a;
|
|
| 61 |
p_vec[1] = b;
|
|
| 62 |
p_vec[2] = c;
|
|
| 63 |
}
|
|
| 60 |
p_vec[0] = a;
|
|
| 61 |
p_vec[1] = b;
|
|
| 62 |
p_vec[2] = c;
|
|
| 63 |
}
|
|
| 64 | 64 |
|
| 65 | 65 |
Vector(double a, double b, double c, double d) |
| 66 | 66 |
{
|
| 67 |
p_vec = (double*)malloc(sizeof(double)*N+1);
|
|
| 67 |
p_vec = &p_vec_data[0];
|
|
| 68 | 68 |
memset(p_vec, 0, sizeof(double)*N); |
| 69 | 69 |
p_vec[0] = a; |
| 70 |
p_vec[1] = b;
|
|
| 71 |
p_vec[2] = c;
|
|
| 72 |
p_vec[3] = d;
|
|
| 70 |
p_vec[1] = b;
|
|
| 71 |
p_vec[2] = c;
|
|
| 72 |
p_vec[3] = d;
|
|
| 73 | 73 |
} |
| 74 | 74 |
|
| 75 | 75 |
Vector(const Vector<N> &v) |
| 76 | 76 |
{
|
| 77 |
p_vec = (double*)malloc(sizeof(double)*N);
|
|
| 77 |
p_vec = &p_vec_data[0];
|
|
| 78 | 78 |
memset(p_vec, 0, sizeof(double)*N); |
| 79 | 79 |
for (int x = 0; x < N; x++ ) |
| 80 | 80 |
p_vec[x] = v.p_vec[x]; |
| ... | ... | |
| 82 | 82 |
|
| 83 | 83 |
~Vector() |
| 84 | 84 |
{
|
| 85 |
free(p_vec); |
|
| 86 | 85 |
} |
| 87 | 86 |
|
| 88 | 87 |
uint8_t n() { return N; }
|
| ... | ... | |
| 157 | 156 |
{
|
| 158 | 157 |
for (int x = 0; x < N; x++ ) |
| 159 | 158 |
p_vec[x] = v.p_vec[x]; |
| 160 |
return *this;
|
|
| 159 |
return *this;
|
|
| 161 | 160 |
} |
| 162 | 161 |
|
| 163 | 162 |
double& operator [](int n) |
| ... | ... | |
| 218 | 217 |
|
| 219 | 218 |
private: |
| 220 | 219 |
double* p_vec; |
| 220 |
double p_vec_data[N]; |
|
| 221 | 221 |
}; |
| 222 | 222 |
|
| 223 | 223 |
|
Also available in: Unified diff