-
Notifications
You must be signed in to change notification settings - Fork 0
/
orientation_sensor.cpp
82 lines (68 loc) · 2.48 KB
/
orientation_sensor.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
82
#include "orientation_sensor.h"
namespace FCAero {
OrientationSensor::OrientationSensor(uint16_t filterUpdateRate) {
this->filterUpdateRate = filterUpdateRate;
fxos = Adafruit_FXOS8700(0x8700A, 0x8700B);
fxas = Adafruit_FXAS21002C(0x0021002C);
}
bool OrientationSensor::init() {
if (!fxos.begin() || !fxas.begin() || !cal.begin() || !cal.loadCalibration()) {
return false;
}
accelerometer = fxos.getAccelerometerSensor();
gyroscope = &fxas;
magnetometer = fxos.getMagnetometerSensor();
filter.begin(filterUpdateRate);
return true;
}
bool OrientationSensor::available() {
return (millis() - timestamp) >= (1000 / filterUpdateRate);
}
void OrientationSensor::readInto(Orientation* o) {
timestamp = millis();
sensors_event_t accel, gyro, mag;
accelerometer->getEvent(&accel);
gyroscope->getEvent(&gyro);
magnetometer->getEvent(&mag);
// cal.calibrate(mag);
// cal.calibrate(accel);
// cal.calibrate(gyro);
float gx = gyro.gyro.x * SENSORS_RADS_TO_DPS,
gy = gyro.gyro.y * SENSORS_RADS_TO_DPS,
gz = gyro.gyro.z * SENSORS_RADS_TO_DPS;
// this is needed, otherwise the return statement fails
// i don't know either
Serial.flush();
filter.update(gx, gy, gz,
accel.acceleration.x, accel.acceleration.y, accel.acceleration.z,
mag.magnetic.x, mag.magnetic.y, mag.magnetic.z);
// Orientation o;
o->heading = filter.getYaw();
o->pitch = filter.getPitch();
o->roll = filter.getRoll();
filter.getQuaternion(&o->qw, &o->qx, &o->qy, &o->qz);
o->rollRate = gx; // make sure that's the right axis
Serial.print(gx);
Serial.print(", ");
Serial.print(gy);
Serial.print(", ");
Serial.println(gz);
// Serial.println("about to return");
// return o;
}
void OrientationSensor::writeReport(Orientation data) {
Serial.print("{\"orientation\":{\"heading\":");
Serial.print(data.heading);
Serial.print(",\"pitch\":");
Serial.print(data.pitch);
Serial.print(",\"roll\":");
Serial.print(data.roll);
Serial.print(",\"rollRate\":");
Serial.print(data.rollRate);
Serial.println("}}");
}
Orientation OrientationSensor::read() {
Orientation o;
return o;
}
}