Skip to content

Commit

Permalink
update apps
Browse files Browse the repository at this point in the history
  • Loading branch information
@xMasterX
xMasterX committed Aug 13, 2023
1 parent f80911f commit 872ed96
Show file tree
Hide file tree
Showing 5 changed files with 232 additions and 184 deletions.
2 changes: 1 addition & 1 deletion application.fam
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,6 @@ App(
stack_size=10 * 1024,
fap_category="GPIO",
fap_icon="mouse_10px.png",
fap_version="0.6",
fap_version="0.8",
sources=["*.c", "*.cc"],
)
305 changes: 173 additions & 132 deletions tracking/main_loop.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,178 +12,219 @@
static const float CURSOR_SPEED = 1024.0 / (M_PI / 4);
static const float STABILIZE_BIAS = 16.0;

float g_yaw = 0;
float g_pitch = 0;
float g_dYaw = 0;
float g_dPitch = 0;
bool firstRead = true;
bool stabilize = true;
CalibrationData calibration;
cardboard::OrientationTracker tracker(10000000l); // 10 ms / 100 Hz
uint64_t ippms, ippms2;

static inline float clamp(float val)
{
while (val <= -M_PI) {
val += 2 * M_PI;
}
while (val >= M_PI) {
val -= 2 * M_PI;
class TrackingState {
private:
float yaw;
float pitch;
float dYaw;
float dPitch;
bool firstRead;
bool stabilize;
CalibrationData calibration;
cardboard::OrientationTracker tracker;
uint64_t ippus, ippus2;

private:
float clamp(float val) {
while (val <= -M_PI) {
val += 2 * M_PI;
}
while (val >= M_PI) {
val -= 2 * M_PI;
}
return val;
}
return val;
}

static inline float highpass(float oldVal, float newVal)
{
if (!stabilize) {
return newVal;
float highpass(float oldVal, float newVal) {
if (!stabilize) {
return newVal;
}
float delta = clamp(oldVal - newVal);
float alpha = (float) std::max(0.0, 1 - std::pow(std::fabs(delta) * CURSOR_SPEED / STABILIZE_BIAS, 3.0));
return newVal + alpha * delta;
}
float delta = clamp(oldVal - newVal);
float alpha = (float) std::max(0.0, 1 - std::pow(std::fabs(delta) * CURSOR_SPEED / STABILIZE_BIAS, 3.0));
return newVal + alpha * delta;
}

void sendCurrentState(MouseMoveCallback mouse_move, void *context)
{
float dX = g_dYaw * CURSOR_SPEED;
float dY = g_dPitch * CURSOR_SPEED;
void sendCurrentState(MouseMoveCallback mouse_move, void *context) {
float dX = dYaw * CURSOR_SPEED;
float dY = dPitch * CURSOR_SPEED;

// Scale the shift down to fit the protocol.
if (dX > 127) {
dY *= 127.0 / dX;
dX = 127;
}
if (dX < -127) {
dY *= -127.0 / dX;
dX = -127;
}
if (dY > 127) {
dX *= 127.0 / dY;
dY = 127;
}
if (dY < -127) {
dX *= -127.0 / dY;
dY = -127;
}

const int8_t x = (int8_t)std::floor(dX + 0.5);
const int8_t y = (int8_t)std::floor(dY + 0.5);

mouse_move(x, y, context);

// Scale the shift down to fit the protocol.
if (dX > 127) {
dY *= 127.0 / dX;
dX = 127;
// Only subtract the part of the error that was already sent.
if (x != 0) {
dYaw -= x / CURSOR_SPEED;
}
if (y != 0) {
dPitch -= y / CURSOR_SPEED;
}
}
if (dX < -127) {
dY *= -127.0 / dX;
dX = -127;

void onOrientation(cardboard::Vector4& quaternion) {
float q1 = quaternion[0]; // X * sin(T/2)
float q2 = quaternion[1]; // Y * sin(T/2)
float q3 = quaternion[2]; // Z * sin(T/2)
float q0 = quaternion[3]; // cos(T/2)

float yaw = std::atan2(2 * (q0 * q3 - q1 * q2), (1 - 2 * (q1 * q1 + q3 * q3)));
float pitch = std::asin(2 * (q0 * q1 + q2 * q3));
// float roll = std::atan2(2 * (q0 * q2 - q1 * q3), (1 - 2 * (q1 * q1 + q2 * q2)));

if (yaw == NAN || pitch == NAN) {
// NaN case, skip it
return;
}

if (firstRead) {
this->yaw = yaw;
this->pitch = pitch;
firstRead = false;
} else {
const float newYaw = highpass(this->yaw, yaw);
const float newPitch = highpass(this->pitch, pitch);

float dYaw = clamp(this->yaw - newYaw);
float dPitch = this->pitch - newPitch;
this->yaw = newYaw;
this->pitch = newPitch;

// Accumulate the error locally.
this->dYaw += dYaw;
this->dPitch += dPitch;
}
}
if (dY > 127) {
dX *= 127.0 / dY;
dY = 127;

public:
TrackingState()
: yaw(0)
, pitch(0)
, dYaw(0)
, dPitch(0)
, firstRead(true)
, stabilize(true)
, tracker(10000000l) { // 10 ms / 100 Hz
ippus = furi_hal_cortex_instructions_per_microsecond();
ippus2 = ippus / 2;
}
if (dY < -127) {
dX *= -127.0 / dY;
dY = -127;

void beginCalibration() {
calibration.reset();
}

const int8_t x = (int8_t)std::floor(dX + 0.5);
const int8_t y = (int8_t)std::floor(dY + 0.5);
bool stepCalibration() {
if (calibration.isComplete())
return true;

mouse_move(x, y, context);
double vec[6];
if (imu_read(vec) & GYR_DATA_READY) {
cardboard::Vector3 data(vec[3], vec[4], vec[5]);
furi_delay_ms(9); // Artificially limit to ~100Hz
return calibration.add(data);
}

// Only subtract the part of the error that was already sent.
if (x != 0) {
g_dYaw -= x / CURSOR_SPEED;
return false;
}
if (y != 0) {
g_dPitch -= y / CURSOR_SPEED;

void saveCalibration() {
CalibrationMedian store;
cardboard::Vector3 median = calibration.getMedian();
store.x = median[0];
store.y = median[1];
store.z = median[2];
CALIBRATION_DATA_SAVE(&store);
}
}

void onOrientation(cardboard::Vector4& quaternion)
{
float q1 = quaternion[0]; // X * sin(T/2)
float q2 = quaternion[1]; // Y * sin(T/2)
float q3 = quaternion[2]; // Z * sin(T/2)
float q0 = quaternion[3]; // cos(T/2)
void loadCalibration() {
CalibrationMedian store;
cardboard::Vector3 median = calibration.getMedian();
if (CALIBRATION_DATA_LOAD(&store)) {
median[0] = store.x;
median[1] = store.y;
median[2] = store.z;
}

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

if (yaw == NAN || pitch == NAN) {
// NaN case, skip it
return;
void beginTracking() {
loadCalibration();
tracker.Resume();
}

if (firstRead) {
g_yaw = yaw;
g_pitch = pitch;
firstRead = false;
} else {
const float newYaw = highpass(g_yaw, yaw);
const float newPitch = highpass(g_pitch, pitch);

float dYaw = clamp(g_yaw - newYaw);
float dPitch = g_pitch - newPitch;
g_yaw = newYaw;
g_pitch = newPitch;

// Accumulate the error locally.
g_dYaw += dYaw;
g_dPitch += dPitch;
void stepTracking(MouseMoveCallback mouse_move, void *context) {
double vec[6];
int ret = imu_read(vec);
if (ret != 0) {
uint64_t t = (DWT->CYCCNT * 1000llu + ippus2) / ippus;
if (ret & ACC_DATA_READY) {
cardboard::AccelerometerData adata
= { .system_timestamp = t, .sensor_timestamp_ns = t,
.data = cardboard::Vector3(vec[0], vec[1], vec[2]) };
tracker.OnAccelerometerData(adata);
}
if (ret & GYR_DATA_READY) {
cardboard::GyroscopeData gdata
= { .system_timestamp = t, .sensor_timestamp_ns = t,
.data = cardboard::Vector3(vec[3], vec[4], vec[5]) };
cardboard::Vector4 pose = tracker.OnGyroscopeData(gdata);
onOrientation(pose);
sendCurrentState(mouse_move, context);
}
}
}
}

void stopTracking() {
tracker.Pause();
}
};

static TrackingState g_state;

extern "C" {

void calibration_begin() {
calibration.reset();
g_state.beginCalibration();
FURI_LOG_I(TAG, "Calibrating");
}

bool calibration_step() {
if (calibration.isComplete())
return true;

double vec[6];
if (imu_read(vec) & GYR_DATA_READY) {
cardboard::Vector3 data(vec[3], vec[4], vec[5]);
furi_delay_ms(9); // Artificially limit to ~100Hz
return calibration.add(data);
}

return false;
return g_state.stepCalibration();
}

void calibration_end() {
CalibrationMedian store;
cardboard::Vector3 median = calibration.getMedian();
store.x = median[0];
store.y = median[1];
store.z = median[2];
CALIBRATION_DATA_SAVE(&store);
g_state.saveCalibration();
}

void tracking_begin() {
CalibrationMedian store;
cardboard::Vector3 median = calibration.getMedian();
if (CALIBRATION_DATA_LOAD(&store)) {
median[0] = store.x;
median[1] = store.y;
median[2] = store.z;
}

ippms = furi_hal_cortex_instructions_per_microsecond();
ippms2 = ippms / 2;
tracker.SetCalibration(median);
tracker.Resume();
g_state.beginTracking();
}

void tracking_step(MouseMoveCallback mouse_move, void *context) {
double vec[6];
int ret = imu_read(vec);
if (ret != 0) {
uint64_t t = (DWT->CYCCNT * 1000llu + ippms2) / ippms;
if (ret & ACC_DATA_READY) {
cardboard::AccelerometerData adata
= { .system_timestamp = t, .sensor_timestamp_ns = t,
.data = cardboard::Vector3(vec[0], vec[1], vec[2]) };
tracker.OnAccelerometerData(adata);
}
if (ret & GYR_DATA_READY) {
cardboard::GyroscopeData gdata
= { .system_timestamp = t, .sensor_timestamp_ns = t,
.data = cardboard::Vector3(vec[3], vec[4], vec[5]) };
cardboard::Vector4 pose = tracker.OnGyroscopeData(gdata);
onOrientation(pose);
sendCurrentState(mouse_move, context);
}
}
g_state.stepTracking(mouse_move, context);
}

void tracking_end() {
tracker.Pause();
g_state.stopTracking();
}

}
2 changes: 1 addition & 1 deletion tracking/orientation_tracker.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -89,7 +89,7 @@ Vector4 OrientationTracker::OnGyroscopeData(const GyroscopeData& event)

sensor_fusion_->ProcessGyroscopeSample(data);

return OrientationTracker::GetPose(data.sensor_timestamp_ns + sampling_period_ns_);
return GetPose(data.sensor_timestamp_ns + sampling_period_ns_);
}

} // namespace cardboard
Loading

0 comments on commit 872ed96

Please sign in to comment.