Permalink
Browse files

move vector orientation to accelerometer/magnetometer/gyro classes

  • Loading branch information...
1 parent 72b5e1d commit 4bab15ca4d4b5123e219fb8c989e7a619b506d3c @mholling committed Apr 9, 2010
Showing with 34 additions and 25 deletions.
  1. +6 −0 accelerometer.cpp
  2. +1 −1 accelerometer.h
  3. +5 −5 attitude.cpp
  4. +3 −5 gyros.cpp
  5. +2 −2 gyros.h
  6. +6 −0 magnetometer.cpp
  7. +1 −1 magnetometer.h
  8. +3 −3 matrix.h
  9. +6 −7 orientation.cpp
  10. +1 −1 orientation.h
View
@@ -3,3 +3,9 @@
unsigned char Accelerometer::InterruptConfigPacket::data[2] = { 0x08, 0x08 }; // send interrupts for DATA_READY to INT2
unsigned char Accelerometer::DataFormatPacket::data[1] = { 0x20 }; // active-low interrupts
+
+Vector Accelerometer::vector() const {
+ Vector result = measure.vector;
+ app.orientation.adjust(result);
+ return result;
+}
View
@@ -67,7 +67,7 @@ class Accelerometer {
MeasurementPacket measure;
- inline Vector vector() const { return measure.vector; }
+ Vector vector() const;
};
#endif
View
@@ -3,10 +3,10 @@
#include "vector.h"
void Attitude::Measure::run() {
- app.accelerometer.measure.wait();
-
- const Vector b1 = app.orientation.adjust(app.accelerometer.vector()).normalised(); // gravity
- const Vector b2 = b1.cross(app.orientation.adjust(app.magnetometer.vector())).normalised(); // magnetic west
+ app.accelerometer.measure.wait(); // TODO: move this inside Accelerometer::vector() ?
+
+ const Vector b1 = app.accelerometer.vector().normalise(); // gravity
+ const Vector b2 = b1.cross(app.magnetometer.vector()).normalise(); // magnetic west
const Vector b3 = b1.cross(b2);
const Vector b1r1a1_plus_b2r2a2(a2 * b2[2] - a1 * b1[1], a1 * b1[0], -a2 * b2[0]);
@@ -40,7 +40,7 @@ void Attitude::Estimate::init() {
}
void Attitude::Estimate::run() {
- Vector rates = app.orientation.adjust(app.gyros.rates());
+ const Vector rates = app.gyros.rates();
yaw.filter(app.attitude.measure.yaw(), rates[0], dt);
pitch.filter(app.attitude.measure.pitch(), rates[1], dt);
View
@@ -23,10 +23,8 @@ float Gyros::Gyro::rate() const {
return (value() / reference() - 1.0) * range;
}
-const Vector Gyros::rates() const {
- Vector result;
- result[0] = x.rate();
- result[1] = y.rate();
- result[2] = z.rate();
+Vector Gyros::rates() const {
+ Vector result(x.rate(), y.rate(), z.rate());
+ app.orientation.adjust(result);
return result;
}
View
@@ -42,9 +42,9 @@ class Gyros {
float rate() const;
};
- Gyro x, y, z;
+ Gyro x, y, z; // TODO: make these private
- const Vector rates() const;
+ Vector rates() const;
};
#endif
View
@@ -42,3 +42,9 @@ void Magnetometer::Calibrate::run() {
if (!state.stored.since(60 * autosave_minutes)) state.store(); // save calibration data every autosave interval
}
}
+
+Vector Magnetometer::vector() const {
+ Vector result = measure.vector - calibrate.bias();
+ app.orientation.adjust(result);
+ return result;
+}
View
@@ -76,7 +76,7 @@ class Magnetometer {
MeasurementPacket measure;
Calibrate calibrate;
- inline Vector vector() const { return measure.vector - calibrate.bias(); }
+ Vector vector() const;
};
#endif
View
@@ -59,7 +59,7 @@ class Matrix {
}
// Columns:
- const Matrix cross(const Matrix& rhs) const;
+ Matrix cross(const Matrix& rhs) const;
float dot(const Matrix& rhs) const { return (t() * rhs)[0]; }
float sqabs() const { return dot(*this); }
float abs() const { return sqrt(sqabs()); }
@@ -85,7 +85,7 @@ typedef Matrix<4, 1> Quaternion;
// Vector cross-product:
template <>
-inline const Vector Vector::cross(const Vector& rhs) const {
+inline Vector Vector::cross(const Vector& rhs) const {
return Vector(data[1] * rhs[2] - data[2] * rhs[1], data[2] * rhs[0] - data[0] * rhs[2], data[0] * rhs[1] - data[1] * rhs[0]);
}
@@ -114,7 +114,7 @@ inline void Vector::rotate_90(int times) {
// Quaternion cross-product:
template <>
-inline const Quaternion Quaternion::cross(const Quaternion& rhs) const {
+inline Quaternion Quaternion::cross(const Quaternion& rhs) const {
Quaternion result;
for (int n = 0; n < 3; n++) {
result[n] = data[3] * rhs[n] + rhs[3] * data[n];
View
@@ -5,16 +5,15 @@ void Orientation::init() {
angles.init();
}
-Vector Orientation::adjust(const Vector& vector) const {
- Vector result = vector;
- result.rotate_90<2>(angles.yaw);
- result.rotate_90<1>(angles.pitch);
- result.rotate_90<0>(angles.roll);
- return result;
+void Orientation::adjust(Vector& vector) const {
+ vector.rotate_90<2>(angles.yaw);
+ vector.rotate_90<1>(angles.pitch);
+ vector.rotate_90<0>(angles.roll);
}
Vector Orientation::variance_from_sd(float sdx, float sdy, float sdz) const {
- Vector result = adjust(Vector(sdx, sdy, sdz));
+ Vector result(sdx, sdy, sdz);
+ adjust(result);
for (int n = 0; n < 3; n++) result[n] *= result[n];
return result;
}
View
@@ -18,7 +18,7 @@ class Orientation {
Orientation() : angles(0xf0) { } // TODO: unified scheme for EEPROM addresses!
void init();
- Vector adjust(const Vector& vector) const;
+ void adjust(Vector& vector) const;
Vector variance_from_sd(float sdx, float sdy, float sdz) const;
};

0 comments on commit 4bab15c

Please sign in to comment.