update apps

application.fam

@@ -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"],
 )

tracking/main_loop.cc

@@ -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();
 }
 
 }

tracking/orientation_tracker.cc

@@ -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

tracking/sensors/sensor_fusion_ekf.cc

@@ -59,13 +59,14 @@ namespace {
     // angle = norm(a)
     // axis = a.normalized()
     // If norm(a) == 0, it returns an identity rotation.
-    static inline Rotation RotationFromVector(const Vector3& a)
+    static inline void RotationFromVector(const Vector3& a, Rotation& r)
     {
         const double norm_a = Length(a);
         if (norm_a < kEpsilon) {
-            return Rotation::Identity();
+            r = Rotation::Identity();
+            return;
         }
-        return Rotation::FromAxisAndAngle(a / norm_a, norm_a);
+        r = Rotation::FromAxisAndAngle(a / norm_a, norm_a);
     }
 
 } // namespace
@@ -199,7 +200,8 @@ void SensorFusionEkf::ComputeMeasurementJacobian()
         Vector3 delta = Vector3::Zero();
         delta[dof] = kFiniteDifferencingEpsilon;
 
-        const Rotation epsilon_rotation = RotationFromVector(delta);
+        Rotation epsilon_rotation;
+        RotationFromVector(delta, epsilon_rotation);
         const Vector3 delta_rotation
             = ComputeInnovation(epsilon_rotation * current_state_.sensor_from_start_rotation);
 
@@ -263,7 +265,8 @@ void SensorFusionEkf::ProcessAccelerometerSample(const AccelerometerData& sample
         * state_covariance_;
 
     // Updates pose and associate covariance matrix.
-    const Rotation rotation_from_state_update = RotationFromVector(state_update_);
+    Rotation rotation_from_state_update;
+    RotationFromVector(state_update_, rotation_from_state_update);
 
     current_state_.sensor_from_start_rotation
         = rotation_from_state_update * current_state_.sensor_from_start_rotation;

views/bt_mouse.c

@@ -132,6 +132,11 @@ void bt_mouse_connection_status_changed_callback(BtStatus status, void* context)
     BtMouse* bt_mouse = context;
 
     bt_mouse->connected = (status == BtStatusConnected);
+    if(!bt_mouse->notifications) {
+        tracking_end();
+        return;
+    }
+
     if(bt_mouse->connected) {
         notification_internal_message(bt_mouse->notifications, &sequence_set_blue_255);
         tracking_begin();
@@ -140,9 +145,6 @@ void bt_mouse_connection_status_changed_callback(BtStatus status, void* context)
         tracking_end();
         notification_internal_message(bt_mouse->notifications, &sequence_reset_blue);
     }
-
-    //with_view_model(
-    //    bt_mouse->view, void * model, { model->connected = connected; }, true);
 }
 
 bool bt_mouse_move(int8_t dx, int8_t dy, void* context) {
@@ -160,46 +162,6 @@ bool bt_mouse_move(int8_t dx, int8_t dy, void* context) {
     return true;
 }
 
-void bt_mouse_enter_callback(void* context) {
-    furi_assert(context);
-    BtMouse* bt_mouse = context;
-
-    bt_mouse->bt = furi_record_open(RECORD_BT);
-    bt_mouse->notifications = furi_record_open(RECORD_NOTIFICATION);
-    bt_set_status_changed_callback(
-        bt_mouse->bt, bt_mouse_connection_status_changed_callback, bt_mouse);
-    furi_assert(bt_set_profile(bt_mouse->bt, BtProfileHidKeyboard));
-    furi_hal_bt_start_advertising();
-}
-
-bool bt_mouse_custom_callback(uint32_t event, void* context) {
-    UNUSED(event);
-    furi_assert(context);
-    BtMouse* bt_mouse = context;
-
-    tracking_step(bt_mouse_move, context);
-    furi_delay_ms(3); // Magic! Removing this will break the buttons
-
-    view_dispatcher_send_custom_event(bt_mouse->view_dispatcher, 0);
-    return true;
-}
-
-void bt_mouse_exit_callback(void* context) {
-    furi_assert(context);
-    BtMouse* bt_mouse = context;
-
-    tracking_end();
-    notification_internal_message(bt_mouse->notifications, &sequence_reset_blue);
-
-    furi_hal_bt_stop_advertising();
-    bt_set_profile(bt_mouse->bt, BtProfileSerial);
-
-    furi_record_close(RECORD_NOTIFICATION);
-    bt_mouse->notifications = NULL;
-    furi_record_close(RECORD_BT);
-    bt_mouse->bt = NULL;
-}
-
 static int8_t clamp(int t) {
     if(t < -128) {
         return -128;
@@ -279,6 +241,50 @@ void bt_mouse_thread_stop(BtMouse* bt_mouse) {
     furi_thread_join(bt_mouse->thread);
     furi_thread_free(bt_mouse->thread);
     furi_mutex_free(bt_mouse->mutex);
+    bt_mouse->mutex = NULL;
+    bt_mouse->thread = NULL;
+}
+
+void bt_mouse_enter_callback(void* context) {
+    furi_assert(context);
+    BtMouse* bt_mouse = context;
+
+    bt_mouse->bt = furi_record_open(RECORD_BT);
+    bt_mouse->notifications = furi_record_open(RECORD_NOTIFICATION);
+    bt_set_status_changed_callback(
+        bt_mouse->bt, bt_mouse_connection_status_changed_callback, bt_mouse);
+    furi_assert(bt_set_profile(bt_mouse->bt, BtProfileHidKeyboard));
+    furi_hal_bt_start_advertising();
+    bt_mouse_thread_start(bt_mouse);
+}
+
+bool bt_mouse_custom_callback(uint32_t event, void* context) {
+    UNUSED(event);
+    furi_assert(context);
+    BtMouse* bt_mouse = context;
+
+    tracking_step(bt_mouse_move, context);
+    furi_delay_ms(3); // Magic! Removing this will break the buttons
+
+    view_dispatcher_send_custom_event(bt_mouse->view_dispatcher, 0);
+    return true;
+}
+
+void bt_mouse_exit_callback(void* context) {
+    furi_assert(context);
+    BtMouse* bt_mouse = context;
+
+    bt_mouse_thread_stop(bt_mouse);
+    tracking_end();
+    notification_internal_message(bt_mouse->notifications, &sequence_reset_blue);
+
+    furi_hal_bt_stop_advertising();
+    bt_set_profile(bt_mouse->bt, BtProfileSerial);
+
+    furi_record_close(RECORD_NOTIFICATION);
+    bt_mouse->notifications = NULL;
+    furi_record_close(RECORD_BT);
+    bt_mouse->bt = NULL;
 }
 
 BtMouse* bt_mouse_alloc(ViewDispatcher* view_dispatcher) {
@@ -293,13 +299,11 @@ BtMouse* bt_mouse_alloc(ViewDispatcher* view_dispatcher) {
     view_set_enter_callback(bt_mouse->view, bt_mouse_enter_callback);
     view_set_custom_callback(bt_mouse->view, bt_mouse_custom_callback);
     view_set_exit_callback(bt_mouse->view, bt_mouse_exit_callback);
-    bt_mouse_thread_start(bt_mouse);
     return bt_mouse;
 }
 
 void bt_mouse_free(BtMouse* bt_mouse) {
     furi_assert(bt_mouse);
-    bt_mouse_thread_stop(bt_mouse);
     view_free(bt_mouse->view);
     free(bt_mouse);
 }