LAURUS / Mbed 2 deprecated Laurus_acc_gyro

read acceleration and angler ratio from mpu6050 and estimate pitch and roll angle

Dependencies:   mbed

Vector.cpp@3:40559ebef0f1, 2015-05-13 (annotated)

Committer:
ojan
Date:
Wed May 13 04:02:27 2015 +0000
Revision:
3:40559ebef0f1
Parent:
2:4a6b46653abf 
estimate gravity vector & geomagnetic vector

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ojan 2:4a6b46653abf 1 #include "mbed.h"
ojan 2:4a6b46653abf 2 #include "myConstants.h"
ojan 2:4a6b46653abf 3 #include "Vector.h"
ojan 2:4a6b46653abf 4
ojan 2:4a6b46653abf 5
ojan 2:4a6b46653abf 6 Vector::Vector(int dim) : dim(dim), components(0){
ojan 2:4a6b46653abf 7 components = new float[dim];
ojan 2:4a6b46653abf 8 if (!components) AbortWithMsg("Memory Allocation Error");
ojan 2:4a6b46653abf 9 for(int i=0; i<dim; i++) {
ojan 2:4a6b46653abf 10 components[i] = 0.0f;
ojan 2:4a6b46653abf 11 }
ojan 2:4a6b46653abf 12 }
ojan 2:4a6b46653abf 13
ojan 2:4a6b46653abf 14
ojan 2:4a6b46653abf 15 Vector::~Vector() {
ojan 2:4a6b46653abf 16 delete[] components;
ojan 2:4a6b46653abf 17 }
ojan 2:4a6b46653abf 18
ojan 2:4a6b46653abf 19 Vector::Vector(const Vector& v) : dim(v.dim), components(0) {
ojan 2:4a6b46653abf 20 components = new float[dim];
ojan 2:4a6b46653abf 21 if (!components) AbortWithMsg("Memory Allocation Error");
ojan 2:4a6b46653abf 22 memcpy(components, v.GetpComponents(), sizeof(float)*dim);
ojan 2:4a6b46653abf 23 }
ojan 2:4a6b46653abf 24
ojan 2:4a6b46653abf 25 Vector& Vector::operator=(const Vector& v) {
ojan 2:4a6b46653abf 26 if (this == &v) return *this;
ojan 2:4a6b46653abf 27 dim = v.dim;
ojan 2:4a6b46653abf 28 delete[] components;
ojan 2:4a6b46653abf 29 components = new float[dim];
ojan 2:4a6b46653abf 30 if (!components) AbortWithMsg("Memory Allocation Error");
ojan 2:4a6b46653abf 31 memcpy(components, v.GetpComponents(), sizeof(float)*dim);
ojan 2:4a6b46653abf 32
ojan 2:4a6b46653abf 33 return *this;
ojan 2:4a6b46653abf 34 }
ojan 2:4a6b46653abf 35
ojan 2:4a6b46653abf 36 Vector& Vector::operator*=(float c) {
ojan 2:4a6b46653abf 37 for (int i = 0; i < dim; i++) {
ojan 2:4a6b46653abf 38 components[i] *= c;
ojan 2:4a6b46653abf 39 }
ojan 2:4a6b46653abf 40
ojan 2:4a6b46653abf 41 return *this;
ojan 2:4a6b46653abf 42 }
ojan 2:4a6b46653abf 43
ojan 2:4a6b46653abf 44 Vector& Vector::operator/=(float c) {
ojan 2:4a6b46653abf 45 if (fabs(c) < NEARLY_ZERO) AbortWithMsg("Division by Zero");
ojan 2:4a6b46653abf 46 for (int i = 0; i < dim; i++) {
ojan 2:4a6b46653abf 47 components[i] /= c;
ojan 2:4a6b46653abf 48 }
ojan 2:4a6b46653abf 49
ojan 2:4a6b46653abf 50 return *this;
ojan 2:4a6b46653abf 51 }
ojan 2:4a6b46653abf 52
ojan 2:4a6b46653abf 53 Vector& Vector::operator+=(const Vector& v) {
ojan 2:4a6b46653abf 54 if (dim != v.dim) AbortWithMsg("failed to add: Irregular Dimention");
ojan 2:4a6b46653abf 55 for (int i = 0; i < dim; i++) {
ojan 2:4a6b46653abf 56 components[i] += v.components[i];
ojan 2:4a6b46653abf 57 }
ojan 2:4a6b46653abf 58
ojan 2:4a6b46653abf 59 this->CleanUp();
ojan 2:4a6b46653abf 60
ojan 2:4a6b46653abf 61 return *this;
ojan 2:4a6b46653abf 62 }
ojan 2:4a6b46653abf 63
ojan 2:4a6b46653abf 64 Vector& Vector::operator-=(const Vector& v) {
ojan 2:4a6b46653abf 65 if (dim != v.dim) AbortWithMsg("failed to subtract: Irregular Dimention");
ojan 2:4a6b46653abf 66 for (int i = 0; i < dim; i++) {
ojan 2:4a6b46653abf 67 components[i] -= v.components[i];
ojan 2:4a6b46653abf 68 }
ojan 2:4a6b46653abf 69
ojan 2:4a6b46653abf 70 this->CleanUp();
ojan 2:4a6b46653abf 71
ojan 2:4a6b46653abf 72 return *this;
ojan 2:4a6b46653abf 73 }
ojan 2:4a6b46653abf 74
ojan 2:4a6b46653abf 75 void Vector::SetComp(int dimNo, float val) {
ojan 2:4a6b46653abf 76 if (dimNo > dim) AbortWithMsg("Index Out of Bounds Error");
ojan 2:4a6b46653abf 77 components[dimNo-1] = val;
ojan 2:4a6b46653abf 78 }
ojan 2:4a6b46653abf 79
ojan 2:4a6b46653abf 80 float Vector::GetNorm() const {
ojan 2:4a6b46653abf 81 float norm = 0.0f;
ojan 2:4a6b46653abf 82 for (int i = 0; i < dim; i++) {
ojan 2:4a6b46653abf 83 norm += components[i] * components[i];
ojan 2:4a6b46653abf 84 }
ojan 2:4a6b46653abf 85 return sqrt(norm);
ojan 2:4a6b46653abf 86 }
ojan 2:4a6b46653abf 87
ojan 3:40559ebef0f1 88 Vector Vector::Normalize() const {
ojan 2:4a6b46653abf 89 float norm = GetNorm();
ojan 2:4a6b46653abf 90 Vector temp(*this);
ojan 2:4a6b46653abf 91 for (int i = 0; i < dim; i++) {
ojan 2:4a6b46653abf 92 temp.components[i] /= norm;
ojan 2:4a6b46653abf 93 }
ojan 2:4a6b46653abf 94 temp.CleanUp();
ojan 2:4a6b46653abf 95 return temp;
ojan 2:4a6b46653abf 96 }
ojan 2:4a6b46653abf 97
ojan 2:4a6b46653abf 98 void Vector::CleanUp() {
ojan 2:4a6b46653abf 99 float maxComp = 0.0f;
ojan 2:4a6b46653abf 100 for (int i = 0; i < dim; i++) {
ojan 2:4a6b46653abf 101 if (fabs(components[i]) > maxComp) maxComp = fabs(components[i]);
ojan 2:4a6b46653abf 102 }
ojan 2:4a6b46653abf 103 if (maxComp > NEARLY_ZERO) {
ojan 2:4a6b46653abf 104 for (int i = 0; i < dim; i++) {
ojan 2:4a6b46653abf 105 if (fabs(components[i]) / maxComp < ZERO_TOLERANCE) components[i] = 0.0f;
ojan 2:4a6b46653abf 106 }
ojan 2:4a6b46653abf 107 }
ojan 2:4a6b46653abf 108 }
ojan 2:4a6b46653abf 109
ojan 2:4a6b46653abf 110 Vector operator+(const Vector& lhv, const Vector& rhv) {
ojan 2:4a6b46653abf 111 Vector retVec(lhv);
ojan 2:4a6b46653abf 112 retVec += rhv;
ojan 2:4a6b46653abf 113 return retVec;
ojan 2:4a6b46653abf 114 }
ojan 2:4a6b46653abf 115
ojan 2:4a6b46653abf 116 Vector operator-(const Vector& lhv, const Vector& rhv) {
ojan 2:4a6b46653abf 117 Vector retVec(lhv);
ojan 2:4a6b46653abf 118 retVec -= rhv;
ojan 2:4a6b46653abf 119 return retVec;
ojan 2:4a6b46653abf 120 }
ojan 2:4a6b46653abf 121
ojan 2:4a6b46653abf 122 Vector Cross(const Vector& lhv, const Vector& rhv) {
ojan 2:4a6b46653abf 123 if (lhv.GetDim() != 3) AbortWithMsg("failed to cross: variable 'dim' must be 3");
ojan 2:4a6b46653abf 124 if (lhv.GetDim() != rhv.GetDim()) AbortWithMsg("failed to cross: Irregular Dimention");
ojan 2:4a6b46653abf 125
ojan 2:4a6b46653abf 126 Vector retVec(lhv.GetDim());
ojan 2:4a6b46653abf 127
ojan 2:4a6b46653abf 128 for (int i = 0; i < lhv.GetDim(); i++) {
ojan 2:4a6b46653abf 129 retVec.SetComp(i + 1, lhv.GetComp((i + 1) % 3 + 1) * rhv.GetComp((i + 2) % 3 + 1)
ojan 2:4a6b46653abf 130 - lhv.GetComp((i + 2) % 3 + 1) * rhv.GetComp((i + 1) % 3 + 1));
ojan 2:4a6b46653abf 131 }
ojan 2:4a6b46653abf 132
ojan 2:4a6b46653abf 133 return retVec;
ojan 2:4a6b46653abf 134 }
ojan 2:4a6b46653abf 135
ojan 2:4a6b46653abf 136 Vector operator*(const float c, const Vector& rhv) {
ojan 2:4a6b46653abf 137 Vector retVec(rhv);
ojan 2:4a6b46653abf 138 retVec *= c;
ojan 2:4a6b46653abf 139 return retVec;
ojan 2:4a6b46653abf 140 }
ojan 2:4a6b46653abf 141
ojan 2:4a6b46653abf 142 Vector operator*(const Vector& lhv, const float c) {
ojan 2:4a6b46653abf 143 Vector retVec(lhv);
ojan 2:4a6b46653abf 144 retVec *= c;
ojan 2:4a6b46653abf 145 return retVec;
ojan 2:4a6b46653abf 146 }
ojan 2:4a6b46653abf 147
ojan 2:4a6b46653abf 148 Vector operator*(const Matrix& lhm, const Vector& rhv) {
ojan 2:4a6b46653abf 149 if (lhm.GetCol() != rhv.GetDim()) AbortWithMsg("Irregular Dimention");
ojan 2:4a6b46653abf 150 Vector retVec(lhm.GetRow());
ojan 2:4a6b46653abf 151
ojan 2:4a6b46653abf 152 for (int i = 1; i <= lhm.GetRow(); i++) {
ojan 2:4a6b46653abf 153 float temp = 0.0f;
ojan 2:4a6b46653abf 154 for (int j = 1; j <= rhv.GetDim(); j++) {
ojan 2:4a6b46653abf 155 temp += lhm.GetComp(i, j)*rhv.GetComp(j);
ojan 2:4a6b46653abf 156 }
ojan 2:4a6b46653abf 157 retVec.SetComp(i, temp);
ojan 2:4a6b46653abf 158 }
ojan 2:4a6b46653abf 159
ojan 2:4a6b46653abf 160 retVec.CleanUp();
ojan 2:4a6b46653abf 161
ojan 2:4a6b46653abf 162 return retVec;
ojan 2:4a6b46653abf 163 }
ojan 2:4a6b46653abf 164
ojan 2:4a6b46653abf 165 Vector operator*(const Vector& lhv, const Matrix& rhm) {
ojan 2:4a6b46653abf 166 if (lhv.GetDim() != rhm.GetRow()) AbortWithMsg("Irregular Dimention");
ojan 2:4a6b46653abf 167 Vector retVec(rhm.GetCol());
ojan 2:4a6b46653abf 168
ojan 2:4a6b46653abf 169 for (int i = 1; i <= rhm.GetCol(); i++) {
ojan 2:4a6b46653abf 170 float temp = 0.0f;
ojan 2:4a6b46653abf 171 for (int j = 1; j <= lhv.GetDim(); j++) {
ojan 2:4a6b46653abf 172 temp += lhv.GetComp(j) * rhm.GetComp(j, i);
ojan 2:4a6b46653abf 173 }
ojan 2:4a6b46653abf 174 retVec.SetComp(i, temp);
ojan 2:4a6b46653abf 175 }
ojan 2:4a6b46653abf 176
ojan 2:4a6b46653abf 177 retVec.CleanUp();
ojan 2:4a6b46653abf 178
ojan 2:4a6b46653abf 179 return retVec;
ojan 2:4a6b46653abf 180 }
ojan 2:4a6b46653abf 181
ojan 2:4a6b46653abf 182 float operator*(const Vector& lhv, const Vector& rhv) {
ojan 2:4a6b46653abf 183 if (lhv.GetDim() != rhv.GetDim()) AbortWithMsg("Irregular Dimention");
ojan 2:4a6b46653abf 184 float retVal = 0.0f;
ojan 2:4a6b46653abf 185
ojan 2:4a6b46653abf 186 for (int i = 1; i <= lhv.GetDim(); i++) {
ojan 2:4a6b46653abf 187 retVal += lhv.GetComp(i) * rhv.GetComp(i);
ojan 2:4a6b46653abf 188 }
ojan 2:4a6b46653abf 189
ojan 2:4a6b46653abf 190 return retVal;
ojan 2:4a6b46653abf 191 }