Skip to content

Commit 9d4f47b

Browse files
ginkageginkage
committed
Improve tracking loop encapsulation
Because global variables are evil...
1 parent 9a580e5 commit 9d4f47b

File tree

1 file changed

+173
-132
lines changed

1 file changed

+173
-132
lines changed

‎tracking/main_loop.cc

+173-132
Original file line numberDiff line numberDiff line change
@@ -12,178 +12,219 @@
1212
static const float CURSOR_SPEED = 1024.0 / (M_PI / 4);
1313
static const float STABILIZE_BIAS = 16.0;
1414

15-
float g_yaw = 0;
16-
float g_pitch = 0;
17-
float g_dYaw = 0;
18-
float g_dPitch = 0;
19-
bool firstRead = true;
20-
bool stabilize = true;
21-
CalibrationData calibration;
22-
cardboard::OrientationTracker tracker(10000000l); // 10 ms / 100 Hz
23-
uint64_t ippms, ippms2;
24-
25-
static inline float clamp(float val)
26-
{
27-
while (val <= -M_PI) {
28-
val += 2 * M_PI;
29-
}
30-
while (val >= M_PI) {
31-
val -= 2 * M_PI;
15+
class TrackingState {
16+
private:
17+
float yaw;
18+
float pitch;
19+
float dYaw;
20+
float dPitch;
21+
bool firstRead;
22+
bool stabilize;
23+
CalibrationData calibration;
24+
cardboard::OrientationTracker tracker;
25+
uint64_t ippus, ippus2;
26+
27+
private:
28+
float clamp(float val) {
29+
while (val <= -M_PI) {
30+
val += 2 * M_PI;
31+
}
32+
while (val >= M_PI) {
33+
val -= 2 * M_PI;
34+
}
35+
return val;
3236
}
33-
return val;
34-
}
3537

36-
static inline float highpass(float oldVal, float newVal)
37-
{
38-
if (!stabilize) {
39-
return newVal;
38+
float highpass(float oldVal, float newVal) {
39+
if (!stabilize) {
40+
return newVal;
41+
}
42+
float delta = clamp(oldVal - newVal);
43+
float alpha = (float) std::max(0.0, 1 - std::pow(std::fabs(delta) * CURSOR_SPEED / STABILIZE_BIAS, 3.0));
44+
return newVal + alpha * delta;
4045
}
41-
float delta = clamp(oldVal - newVal);
42-
float alpha = (float) std::max(0.0, 1 - std::pow(std::fabs(delta) * CURSOR_SPEED / STABILIZE_BIAS, 3.0));
43-
return newVal + alpha * delta;
44-
}
4546

46-
void sendCurrentState(MouseMoveCallback mouse_move, void *context)
47-
{
48-
float dX = g_dYaw * CURSOR_SPEED;
49-
float dY = g_dPitch * CURSOR_SPEED;
47+
void sendCurrentState(MouseMoveCallback mouse_move, void *context) {
48+
float dX = dYaw * CURSOR_SPEED;
49+
float dY = dPitch * CURSOR_SPEED;
50+
51+
// Scale the shift down to fit the protocol.
52+
if (dX > 127) {
53+
dY *= 127.0 / dX;
54+
dX = 127;
55+
}
56+
if (dX < -127) {
57+
dY *= -127.0 / dX;
58+
dX = -127;
59+
}
60+
if (dY > 127) {
61+
dX *= 127.0 / dY;
62+
dY = 127;
63+
}
64+
if (dY < -127) {
65+
dX *= -127.0 / dY;
66+
dY = -127;
67+
}
68+
69+
const int8_t x = (int8_t)std::floor(dX + 0.5);
70+
const int8_t y = (int8_t)std::floor(dY + 0.5);
71+
72+
mouse_move(x, y, context);
5073

51-
// Scale the shift down to fit the protocol.
52-
if (dX > 127) {
53-
dY *= 127.0 / dX;
54-
dX = 127;
74+
// Only subtract the part of the error that was already sent.
75+
if (x != 0) {
76+
dYaw -= x / CURSOR_SPEED;
77+
}
78+
if (y != 0) {
79+
dPitch -= y / CURSOR_SPEED;
80+
}
5581
}
56-
if (dX < -127) {
57-
dY *= -127.0 / dX;
58-
dX = -127;
82+
83+
void onOrientation(cardboard::Vector4& quaternion) {
84+
float q1 = quaternion[0]; // X * sin(T/2)
85+
float q2 = quaternion[1]; // Y * sin(T/2)
86+
float q3 = quaternion[2]; // Z * sin(T/2)
87+
float q0 = quaternion[3]; // cos(T/2)
88+
89+
float yaw = std::atan2(2 * (q0 * q3 - q1 * q2), (1 - 2 * (q1 * q1 + q3 * q3)));
90+
float pitch = std::asin(2 * (q0 * q1 + q2 * q3));
91+
// float roll = std::atan2(2 * (q0 * q2 - q1 * q3), (1 - 2 * (q1 * q1 + q2 * q2)));
92+
93+
if (yaw == NAN || pitch == NAN) {
94+
// NaN case, skip it
95+
return;
96+
}
97+
98+
if (firstRead) {
99+
this->yaw = yaw;
100+
this->pitch = pitch;
101+
firstRead = false;
102+
} else {
103+
const float newYaw = highpass(this->yaw, yaw);
104+
const float newPitch = highpass(this->pitch, pitch);
105+
106+
float dYaw = clamp(this->yaw - newYaw);
107+
float dPitch = this->pitch - newPitch;
108+
this->yaw = newYaw;
109+
this->pitch = newPitch;
110+
111+
// Accumulate the error locally.
112+
this->dYaw += dYaw;
113+
this->dPitch += dPitch;
114+
}
59115
}
60-
if (dY > 127) {
61-
dX *= 127.0 / dY;
62-
dY = 127;
116+
117+
public:
118+
TrackingState()
119+
: yaw(0)
120+
, pitch(0)
121+
, dYaw(0)
122+
, dPitch(0)
123+
, firstRead(true)
124+
, stabilize(true)
125+
, tracker(10000000l) { // 10 ms / 100 Hz
126+
ippus = furi_hal_cortex_instructions_per_microsecond();
127+
ippus2 = ippus / 2;
63128
}
64-
if (dY < -127) {
65-
dX *= -127.0 / dY;
66-
dY = -127;
129+
130+
void beginCalibration() {
131+
calibration.reset();
67132
}
68133

69-
const int8_t x = (int8_t)std::floor(dX + 0.5);
70-
const int8_t y = (int8_t)std::floor(dY + 0.5);
134+
bool stepCalibration() {
135+
if (calibration.isComplete())
136+
return true;
71137

72-
mouse_move(x, y, context);
138+
double vec[6];
139+
if (imu_read(vec) & GYR_DATA_READY) {
140+
cardboard::Vector3 data(vec[3], vec[4], vec[5]);
141+
furi_delay_ms(9); // Artificially limit to ~100Hz
142+
return calibration.add(data);
143+
}
73144

74-
// Only subtract the part of the error that was already sent.
75-
if (x != 0) {
76-
g_dYaw -= x / CURSOR_SPEED;
145+
return false;
77146
}
78-
if (y != 0) {
79-
g_dPitch -= y / CURSOR_SPEED;
147+
148+
void saveCalibration() {
149+
CalibrationMedian store;
150+
cardboard::Vector3 median = calibration.getMedian();
151+
store.x = median[0];
152+
store.y = median[1];
153+
store.z = median[2];
154+
CALIBRATION_DATA_SAVE(&store);
80155
}
81-
}
82156

83-
void onOrientation(cardboard::Vector4& quaternion)
84-
{
85-
float q1 = quaternion[0]; // X * sin(T/2)
86-
float q2 = quaternion[1]; // Y * sin(T/2)
87-
float q3 = quaternion[2]; // Z * sin(T/2)
88-
float q0 = quaternion[3]; // cos(T/2)
157+
void loadCalibration() {
158+
CalibrationMedian store;
159+
cardboard::Vector3 median = calibration.getMedian();
160+
if (CALIBRATION_DATA_LOAD(&store)) {
161+
median[0] = store.x;
162+
median[1] = store.y;
163+
median[2] = store.z;
164+
}
89165

90-
float yaw = std::atan2(2 * (q0 * q3 - q1 * q2), (1 - 2 * (q1 * q1 + q3 * q3)));
91-
float pitch = std::asin(2 * (q0 * q1 + q2 * q3));
92-
// float roll = std::atan2(2 * (q0 * q2 - q1 * q3), (1 - 2 * (q1 * q1 + q2 * q2)));
166+
tracker.SetCalibration(median);
167+
}
93168

94-
if (yaw == NAN || pitch == NAN) {
95-
// NaN case, skip it
96-
return;
169+
void beginTracking() {
170+
loadCalibration();
171+
tracker.Resume();
97172
}
98173

99-
if (firstRead) {
100-
g_yaw = yaw;
101-
g_pitch = pitch;
102-
firstRead = false;
103-
} else {
104-
const float newYaw = highpass(g_yaw, yaw);
105-
const float newPitch = highpass(g_pitch, pitch);
106-
107-
float dYaw = clamp(g_yaw - newYaw);
108-
float dPitch = g_pitch - newPitch;
109-
g_yaw = newYaw;
110-
g_pitch = newPitch;
111-
112-
// Accumulate the error locally.
113-
g_dYaw += dYaw;
114-
g_dPitch += dPitch;
174+
void stepTracking(MouseMoveCallback mouse_move, void *context) {
175+
double vec[6];
176+
int ret = imu_read(vec);
177+
if (ret != 0) {
178+
uint64_t t = (DWT->CYCCNT * 1000llu + ippus2) / ippus;
179+
if (ret & ACC_DATA_READY) {
180+
cardboard::AccelerometerData adata
181+
= { .system_timestamp = t, .sensor_timestamp_ns = t,
182+
.data = cardboard::Vector3(vec[0], vec[1], vec[2]) };
183+
tracker.OnAccelerometerData(adata);
184+
}
185+
if (ret & GYR_DATA_READY) {
186+
cardboard::GyroscopeData gdata
187+
= { .system_timestamp = t, .sensor_timestamp_ns = t,
188+
.data = cardboard::Vector3(vec[3], vec[4], vec[5]) };
189+
cardboard::Vector4 pose = tracker.OnGyroscopeData(gdata);
190+
onOrientation(pose);
191+
sendCurrentState(mouse_move, context);
192+
}
193+
}
115194
}
116-
}
195+
196+
void stopTracking() {
197+
tracker.Pause();
198+
}
199+
};
200+
201+
static TrackingState g_state;
117202

118203
extern "C" {
119204

120205
void calibration_begin() {
121-
calibration.reset();
206+
g_state.beginCalibration();
122207
FURI_LOG_I(TAG, "Calibrating");
123208
}
124209

125210
bool calibration_step() {
126-
if (calibration.isComplete())
127-
return true;
128-
129-
double vec[6];
130-
if (imu_read(vec) & GYR_DATA_READY) {
131-
cardboard::Vector3 data(vec[3], vec[4], vec[5]);
132-
furi_delay_ms(9); // Artificially limit to ~100Hz
133-
return calibration.add(data);
134-
}
135-
136-
return false;
211+
return g_state.stepCalibration();
137212
}
138213

139214
void calibration_end() {
140-
CalibrationMedian store;
141-
cardboard::Vector3 median = calibration.getMedian();
142-
store.x = median[0];
143-
store.y = median[1];
144-
store.z = median[2];
145-
CALIBRATION_DATA_SAVE(&store);
215+
g_state.saveCalibration();
146216
}
147217

148218
void tracking_begin() {
149-
CalibrationMedian store;
150-
cardboard::Vector3 median = calibration.getMedian();
151-
if (CALIBRATION_DATA_LOAD(&store)) {
152-
median[0] = store.x;
153-
median[1] = store.y;
154-
median[2] = store.z;
155-
}
156-
157-
ippms = furi_hal_cortex_instructions_per_microsecond();
158-
ippms2 = ippms / 2;
159-
tracker.SetCalibration(median);
160-
tracker.Resume();
219+
g_state.beginTracking();
161220
}
162221

163222
void tracking_step(MouseMoveCallback mouse_move, void *context) {
164-
double vec[6];
165-
int ret = imu_read(vec);
166-
if (ret != 0) {
167-
uint64_t t = (DWT->CYCCNT * 1000llu + ippms2) / ippms;
168-
if (ret & ACC_DATA_READY) {
169-
cardboard::AccelerometerData adata
170-
= { .system_timestamp = t, .sensor_timestamp_ns = t,
171-
.data = cardboard::Vector3(vec[0], vec[1], vec[2]) };
172-
tracker.OnAccelerometerData(adata);
173-
}
174-
if (ret & GYR_DATA_READY) {
175-
cardboard::GyroscopeData gdata
176-
= { .system_timestamp = t, .sensor_timestamp_ns = t,
177-
.data = cardboard::Vector3(vec[3], vec[4], vec[5]) };
178-
cardboard::Vector4 pose = tracker.OnGyroscopeData(gdata);
179-
onOrientation(pose);
180-
sendCurrentState(mouse_move, context);
181-
}
182-
}
223+
g_state.stepTracking(mouse_move, context);
183224
}
184225

185226
void tracking_end() {
186-
tracker.Pause();
227+
g_state.stopTracking();
187228
}
188229

189230
}

0 commit comments

Comments
 (0)