-
Notifications
You must be signed in to change notification settings - Fork 0
/
Compass.cpp
81 lines (67 loc) · 2.3 KB
/
Compass.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
/* Reads data from an IMU sensor and computes a compass heading from it */
#include <Arduino.h>
#include "Compass.h"
#include "Utils.h"
#include <Arduino_LSM9DS1.h>
Compass::Compass() {
compassHeading = 0;
}
void Compass::begin() {
bool imuStartedSuccessfully = IMU.begin();
if (!imuStartedSuccessfully) {
Serial.println("Failed to initialize IMU!");
Utils::exitMainLoop();
}
}
void Compass::end() {
IMU.end();
}
void Compass::update() {
if (IMU.magneticFieldAvailable()) {
// These are used to get data out of the IMU
float x, y, z;
// update the magnetic field vector
IMU.readMagneticField(x, y, z);
updateBounds(x, y, z);
magnetic_field.set(x, y, z);
magnetic_field.map(min_magnetometer, max_magnetometer, -1, 1);
magnetic_field.normalize();
magnetic_field.set(magnetic_field.y, -magnetic_field.x, magnetic_field.z);
// update the 'up' vector
IMU.readAcceleration(x, y, z);
up.set(y, x, z);
up.normalize();
// update the north vector
float magneticUpComponent = Vector::dotProduct(magnetic_field, up);
north.set(
magnetic_field.x - magneticUpComponent * up.x,
magnetic_field.y - magneticUpComponent * up.y,
magnetic_field.z - magneticUpComponent * up.z
);
north.normalize();
// update east vector
east = Vector::vectProduct(north, up);
float boatUpComponent = Vector::dotProduct(boat, up);
headingVector.set(
boat.x - boatUpComponent * up.x,
boat.y - boatUpComponent * up.y,
boat.z - boatUpComponent * up.z
);
headingVector.normalize();
// update heading
float headingNorthComponent = Vector::dotProduct(headingVector, north);
float headingEastComponent = Vector::dotProduct(headingVector, east);
compassHeading = atan2(headingEastComponent, headingNorthComponent);
}
}
void Compass::updateBounds(float x, float y, float z) {
if (x < min_magnetometer.x) min_magnetometer.x = x;
if (y < min_magnetometer.y) min_magnetometer.y = y;
if (z < min_magnetometer.z) min_magnetometer.z = z;
if (x > max_magnetometer.x) max_magnetometer.x = x;
if (y > max_magnetometer.y) max_magnetometer.y = y;
if (z > max_magnetometer.z) max_magnetometer.z = z;
}
String Compass::toString() {
return String() + "<Compass:"+magnetic_field.toString()+","+compassHeading+">";
}