More updates (thanks grnch!)

Applications/Official/DEV_FW/source/README.md

@@ -0,0 +1 @@
+Put your custom applications in this folder.

Applications/Official/DEV_FW/source/airmouse/LICENSE

@@ -0,0 +1,201 @@
+                                 Apache License
+                           Version 2.0, January 2004
+                        http://www.apache.org/licenses/
+
+   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
+
+   1. Definitions.
+
+      "License" shall mean the terms and conditions for use, reproduction,
+      and distribution as defined by Sections 1 through 9 of this document.
+
+      "Licensor" shall mean the copyright owner or entity authorized by
+      the copyright owner that is granting the License.
+
+      "Legal Entity" shall mean the union of the acting entity and all
+      other entities that control, are controlled by, or are under common
+      control with that entity. For the purposes of this definition,
+      "control" means (i) the power, direct or indirect, to cause the
+      direction or management of such entity, whether by contract or
+      otherwise, or (ii) ownership of fifty percent (50%) or more of the
+      outstanding shares, or (iii) beneficial ownership of such entity.
+
+      "You" (or "Your") shall mean an individual or Legal Entity
+      exercising permissions granted by this License.
+
+      "Source" form shall mean the preferred form for making modifications,
+      including but not limited to software source code, documentation
+      source, and configuration files.
+
+      "Object" form shall mean any form resulting from mechanical
+      transformation or translation of a Source form, including but
+      not limited to compiled object code, generated documentation,
+      and conversions to other media types.
+
+      "Work" shall mean the work of authorship, whether in Source or
+      Object form, made available under the License, as indicated by a
+      copyright notice that is included in or attached to the work
+      (an example is provided in the Appendix below).
+
+      "Derivative Works" shall mean any work, whether in Source or Object
+      form, that is based on (or derived from) the Work and for which the
+      editorial revisions, annotations, elaborations, or other modifications
+      represent, as a whole, an original work of authorship. For the purposes
+      of this License, Derivative Works shall not include works that remain
+      separable from, or merely link (or bind by name) to the interfaces of,
+      the Work and Derivative Works thereof.
+
+      "Contribution" shall mean any work of authorship, including
+      the original version of the Work and any modifications or additions
+      to that Work or Derivative Works thereof, that is intentionally
+      submitted to Licensor for inclusion in the Work by the copyright owner
+      or by an individual or Legal Entity authorized to submit on behalf of
+      the copyright owner. For the purposes of this definition, "submitted"
+      means any form of electronic, verbal, or written communication sent
+      to the Licensor or its representatives, including but not limited to
+      communication on electronic mailing lists, source code control systems,
+      and issue tracking systems that are managed by, or on behalf of, the
+      Licensor for the purpose of discussing and improving the Work, but
+      excluding communication that is conspicuously marked or otherwise
+      designated in writing by the copyright owner as "Not a Contribution."
+
+      "Contributor" shall mean Licensor and any individual or Legal Entity
+      on behalf of whom a Contribution has been received by Licensor and
+      subsequently incorporated within the Work.
+
+   2. Grant of Copyright License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      copyright license to reproduce, prepare Derivative Works of,
+      publicly display, publicly perform, sublicense, and distribute the
+      Work and such Derivative Works in Source or Object form.
+
+   3. Grant of Patent License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      (except as stated in this section) patent license to make, have made,
+      use, offer to sell, sell, import, and otherwise transfer the Work,
+      where such license applies only to those patent claims licensable
+      by such Contributor that are necessarily infringed by their
+      Contribution(s) alone or by combination of their Contribution(s)
+      with the Work to which such Contribution(s) was submitted. If You
+      institute patent litigation against any entity (including a
+      cross-claim or counterclaim in a lawsuit) alleging that the Work
+      or a Contribution incorporated within the Work constitutes direct
+      or contributory patent infringement, then any patent licenses
+      granted to You under this License for that Work shall terminate
+      as of the date such litigation is filed.
+
+   4. Redistribution. You may reproduce and distribute copies of the
+      Work or Derivative Works thereof in any medium, with or without
+      modifications, and in Source or Object form, provided that You
+      meet the following conditions:
+
+      (a) You must give any other recipients of the Work or
+          Derivative Works a copy of this License; and
+
+      (b) You must cause any modified files to carry prominent notices
+          stating that You changed the files; and
+
+      (c) You must retain, in the Source form of any Derivative Works
+          that You distribute, all copyright, patent, trademark, and
+          attribution notices from the Source form of the Work,
+          excluding those notices that do not pertain to any part of
+          the Derivative Works; and
+
+      (d) If the Work includes a "NOTICE" text file as part of its
+          distribution, then any Derivative Works that You distribute must
+          include a readable copy of the attribution notices contained
+          within such NOTICE file, excluding those notices that do not
+          pertain to any part of the Derivative Works, in at least one
+          of the following places: within a NOTICE text file distributed
+          as part of the Derivative Works; within the Source form or
+          documentation, if provided along with the Derivative Works; or,
+          within a display generated by the Derivative Works, if and
+          wherever such third-party notices normally appear. The contents
+          of the NOTICE file are for informational purposes only and
+          do not modify the License. You may add Your own attribution
+          notices within Derivative Works that You distribute, alongside
+          or as an addendum to the NOTICE text from the Work, provided
+          that such additional attribution notices cannot be construed
+          as modifying the License.
+
+      You may add Your own copyright statement to Your modifications and
+      may provide additional or different license terms and conditions
+      for use, reproduction, or distribution of Your modifications, or
+      for any such Derivative Works as a whole, provided Your use,
+      reproduction, and distribution of the Work otherwise complies with
+      the conditions stated in this License.
+
+   5. Submission of Contributions. Unless You explicitly state otherwise,
+      any Contribution intentionally submitted for inclusion in the Work
+      by You to the Licensor shall be under the terms and conditions of
+      this License, without any additional terms or conditions.
+      Notwithstanding the above, nothing herein shall supersede or modify
+      the terms of any separate license agreement you may have executed
+      with Licensor regarding such Contributions.
+
+   6. Trademarks. This License does not grant permission to use the trade
+      names, trademarks, service marks, or product names of the Licensor,
+      except as required for reasonable and customary use in describing the
+      origin of the Work and reproducing the content of the NOTICE file.
+
+   7. Disclaimer of Warranty. Unless required by applicable law or
+      agreed to in writing, Licensor provides the Work (and each
+      Contributor provides its Contributions) on an "AS IS" BASIS,
+      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
+      implied, including, without limitation, any warranties or conditions
+      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
+      PARTICULAR PURPOSE. You are solely responsible for determining the
+      appropriateness of using or redistributing the Work and assume any
+      risks associated with Your exercise of permissions under this License.
+
+   8. Limitation of Liability. In no event and under no legal theory,
+      whether in tort (including negligence), contract, or otherwise,
+      unless required by applicable law (such as deliberate and grossly
+      negligent acts) or agreed to in writing, shall any Contributor be
+      liable to You for damages, including any direct, indirect, special,
+      incidental, or consequential damages of any character arising as a
+      result of this License or out of the use or inability to use the
+      Work (including but not limited to damages for loss of goodwill,
+      work stoppage, computer failure or malfunction, or any and all
+      other commercial damages or losses), even if such Contributor
+      has been advised of the possibility of such damages.
+
+   9. Accepting Warranty or Additional Liability. While redistributing
+      the Work or Derivative Works thereof, You may choose to offer,
+      and charge a fee for, acceptance of support, warranty, indemnity,
+      or other liability obligations and/or rights consistent with this
+      License. However, in accepting such obligations, You may act only
+      on Your own behalf and on Your sole responsibility, not on behalf
+      of any other Contributor, and only if You agree to indemnify,
+      defend, and hold each Contributor harmless for any liability
+      incurred by, or claims asserted against, such Contributor by reason
+      of your accepting any such warranty or additional liability.
+
+   END OF TERMS AND CONDITIONS
+
+   APPENDIX: How to apply the Apache License to your work.
+
+      To apply the Apache License to your work, attach the following
+      boilerplate notice, with the fields enclosed by brackets "[]"
+      replaced with your own identifying information. (Don't include
+      the brackets!)  The text should be enclosed in the appropriate
+      comment syntax for the file format. We also recommend that a
+      file or class name and description of purpose be included on the
+      same "printed page" as the copyright notice for easier
+      identification within third-party archives.
+
+   Copyright [yyyy] [name of copyright owner]
+
+   Licensed under the Apache License, Version 2.0 (the "License");
+   you may not use this file except in compliance with the License.
+   You may obtain a copy of the License at
+
+       http://www.apache.org/licenses/LICENSE-2.0
+
+   Unless required by applicable law or agreed to in writing, software
+   distributed under the License is distributed on an "AS IS" BASIS,
+   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+   See the License for the specific language governing permissions and
+   limitations under the License.

Applications/Official/DEV_FW/source/airmouse/README.md

@@ -0,0 +1,56 @@
+# Flipper Air Mouse
+
+## Brief
+
+> "You can turn anything into an air mouse if you're brave enough"
+
+ — Piper, a.k.a. Pez
+
+Naturally, the quote above applies to [Flipper](https://flipperzero.one/) as well.
+
+## What?
+
+The app allows you to turn your Flipper into a USB or Bluetooth air mouse (you do need an extra module, see the Hardware section below)...
+
+Using it is really simple:
+ * Connect the Flipper via a USB cable and pick `USB`, or pick `Bluetooth` and pair it with your PC;
+ * Hold the Flipper in your hand with the buttons pointing towards the screen;
+ * Wave your Flipper like you don't care to move the cursor;
+ * Up button for Left mouse click;
+ * Down button for Right mouse click;
+ * Center button for Middle mouse click;
+ * Use calibration menu option if you notice significant drift (place your Flipper onto a level surface, make sure it doesn't move, run this option, wait 2 seconds, done).
+
+See early prototype [in action](https://www.youtube.com/watch?v=DdxAmmsYfMA).
+
+## Hardware
+
+The custom module is using Bosch BMI160 accelerometer/gyroscope chip connected via I2C.
+
+Take a look into the [schematic](https://github.com/ginkage/FlippAirMouse/tree/main/schematic) folder for Gerber, BOM and CPL files, so you can order directly from JLCPCB.
+
+Original idea:
+
+![What I thought it would look like](https://github.com/ginkage/FlippAirMouse/blob/main/schematic/schematic.png)
+
+Expectation:
+
+![What EDA though it would look like](https://github.com/ginkage/FlippAirMouse/blob/main/schematic/render.png)
+
+Reality:
+
+![What it looks like](https://github.com/ginkage/FlippAirMouse/blob/main/schematic/flipper.jpg)
+
+
+## Software
+
+The code is based on the original Bosch [driver](https://github.com/BoschSensortec/BMI160_driver/) and an orientation tracking implementation from the [Cardboard](https://github.com/googlevr/cardboard/tree/master/sdk/sensors) project
+
+If you're familiar with Flipper applications, start in the [firmware](https://github.com/flipperdevices/flipperzero-firmware) checkout folder and do the following:
+```
+cd applications/plugins
+git clone https://github.com/ginkage/FlippAirMouse
+cd ../..
+./fbt fap_air_mouse
+```
+If you're not familiar with those, just grab a `fap` file from Releases.

Applications/Official/DEV_FW/source/airmouse/air_mouse.c

@@ -0,0 +1,156 @@
+#include "air_mouse.h"
+
+#include <furi.h>
+#include <dolphin/dolphin.h>
+
+#include "tracking/imu/imu.h"
+
+#define TAG "AirMouseApp"
+
+enum AirMouseSubmenuIndex {
+    AirMouseSubmenuIndexBtMouse,
+    AirMouseSubmenuIndexUsbMouse,
+    AirMouseSubmenuIndexCalibration,
+};
+
+void air_mouse_submenu_callback(void* context, uint32_t index) {
+    furi_assert(context);
+    AirMouse* app = context;
+    if(index == AirMouseSubmenuIndexBtMouse) {
+        app->view_id = AirMouseViewBtMouse;
+        view_dispatcher_switch_to_view(app->view_dispatcher, AirMouseViewBtMouse);
+    } else if(index == AirMouseSubmenuIndexUsbMouse) {
+        app->view_id = AirMouseViewUsbMouse;
+        view_dispatcher_switch_to_view(app->view_dispatcher, AirMouseViewUsbMouse);
+    } else if(index == AirMouseSubmenuIndexCalibration) {
+        app->view_id = AirMouseViewCalibration;
+        view_dispatcher_switch_to_view(app->view_dispatcher, AirMouseViewCalibration);
+    }
+}
+
+void air_mouse_dialog_callback(DialogExResult result, void* context) {
+    furi_assert(context);
+    AirMouse* app = context;
+    if(result == DialogExResultLeft) {
+        view_dispatcher_switch_to_view(app->view_dispatcher, VIEW_NONE); // Exit
+    } else if(result == DialogExResultRight) {
+        view_dispatcher_switch_to_view(app->view_dispatcher, app->view_id); // Show last view
+    } else if(result == DialogExResultCenter) {
+        view_dispatcher_switch_to_view(app->view_dispatcher, AirMouseViewSubmenu); // Menu
+    }
+}
+
+uint32_t air_mouse_exit_confirm_view(void* context) {
+    UNUSED(context);
+    return AirMouseViewExitConfirm;
+}
+
+uint32_t air_mouse_exit(void* context) {
+    UNUSED(context);
+    return VIEW_NONE;
+}
+
+AirMouse* air_mouse_app_alloc() {
+    AirMouse* app = malloc(sizeof(AirMouse));
+
+    // Gui
+    app->gui = furi_record_open(RECORD_GUI);
+
+    // View dispatcher
+    app->view_dispatcher = view_dispatcher_alloc();
+    view_dispatcher_enable_queue(app->view_dispatcher);
+    view_dispatcher_attach_to_gui(app->view_dispatcher, app->gui, ViewDispatcherTypeFullscreen);
+
+    // Submenu view
+    app->submenu = submenu_alloc();
+    submenu_add_item(
+        app->submenu, "Bluetooth", AirMouseSubmenuIndexBtMouse, air_mouse_submenu_callback, app);
+    submenu_add_item(
+        app->submenu, "USB", AirMouseSubmenuIndexUsbMouse, air_mouse_submenu_callback, app);
+    submenu_add_item(
+        app->submenu,
+        "Calibration",
+        AirMouseSubmenuIndexCalibration,
+        air_mouse_submenu_callback,
+        app);
+    view_set_previous_callback(submenu_get_view(app->submenu), air_mouse_exit);
+    view_dispatcher_add_view(
+        app->view_dispatcher, AirMouseViewSubmenu, submenu_get_view(app->submenu));
+
+    // Dialog view
+    app->dialog = dialog_ex_alloc();
+    dialog_ex_set_result_callback(app->dialog, air_mouse_dialog_callback);
+    dialog_ex_set_context(app->dialog, app);
+    dialog_ex_set_left_button_text(app->dialog, "Exit");
+    dialog_ex_set_right_button_text(app->dialog, "Stay");
+    dialog_ex_set_center_button_text(app->dialog, "Menu");
+    dialog_ex_set_header(app->dialog, "Close Current App?", 16, 12, AlignLeft, AlignTop);
+    view_dispatcher_add_view(
+        app->view_dispatcher, AirMouseViewExitConfirm, dialog_ex_get_view(app->dialog));
+
+    // Bluetooth view
+    app->bt_mouse = bt_mouse_alloc(app->view_dispatcher);
+    view_set_previous_callback(bt_mouse_get_view(app->bt_mouse), air_mouse_exit_confirm_view);
+    view_dispatcher_add_view(
+        app->view_dispatcher, AirMouseViewBtMouse, bt_mouse_get_view(app->bt_mouse));
+
+    // USB view
+    app->usb_mouse = usb_mouse_alloc(app->view_dispatcher);
+    view_set_previous_callback(usb_mouse_get_view(app->usb_mouse), air_mouse_exit_confirm_view);
+    view_dispatcher_add_view(
+        app->view_dispatcher, AirMouseViewUsbMouse, usb_mouse_get_view(app->usb_mouse));
+
+    // Calibration view
+    app->calibration = calibration_alloc(app->view_dispatcher);
+    view_set_previous_callback(
+        calibration_get_view(app->calibration), air_mouse_exit_confirm_view);
+    view_dispatcher_add_view(
+        app->view_dispatcher, AirMouseViewCalibration, calibration_get_view(app->calibration));
+
+    app->view_id = AirMouseViewSubmenu;
+    view_dispatcher_switch_to_view(app->view_dispatcher, app->view_id);
+
+    return app;
+}
+
+void air_mouse_app_free(AirMouse* app) {
+    furi_assert(app);
+
+    // Free views
+    view_dispatcher_remove_view(app->view_dispatcher, AirMouseViewSubmenu);
+    submenu_free(app->submenu);
+    view_dispatcher_remove_view(app->view_dispatcher, AirMouseViewExitConfirm);
+    dialog_ex_free(app->dialog);
+    view_dispatcher_remove_view(app->view_dispatcher, AirMouseViewBtMouse);
+    bt_mouse_free(app->bt_mouse);
+    view_dispatcher_remove_view(app->view_dispatcher, AirMouseViewUsbMouse);
+    usb_mouse_free(app->usb_mouse);
+    view_dispatcher_remove_view(app->view_dispatcher, AirMouseViewCalibration);
+    calibration_free(app->calibration);
+    view_dispatcher_free(app->view_dispatcher);
+
+    // Close records
+    furi_record_close(RECORD_GUI);
+    app->gui = NULL;
+
+    // Free rest
+    free(app);
+}
+
+int32_t air_mouse_app(void* p) {
+    UNUSED(p);
+
+    AirMouse* app = air_mouse_app_alloc();
+    if(!imu_begin()) {
+        air_mouse_app_free(app);
+        return -1;
+    }
+
+    DOLPHIN_DEED(DolphinDeedPluginStart);
+    view_dispatcher_run(app->view_dispatcher);
+
+    imu_end();
+    air_mouse_app_free(app);
+
+    return 0;
+}

Applications/Official/DEV_FW/source/airmouse/air_mouse.h

@@ -0,0 +1,30 @@
+#pragma once
+
+#include <gui/gui.h>
+#include <gui/view.h>
+#include <gui/view_dispatcher.h>
+#include <gui/modules/submenu.h>
+#include <gui/modules/dialog_ex.h>
+
+#include "views/bt_mouse.h"
+#include "views/usb_mouse.h"
+#include "views/calibration.h"
+
+typedef struct {
+    Gui* gui;
+    ViewDispatcher* view_dispatcher;
+    Submenu* submenu;
+    DialogEx* dialog;
+    BtMouse* bt_mouse;
+    UsbMouse* usb_mouse;
+    Calibration* calibration;
+    uint32_t view_id;
+} AirMouse;
+
+typedef enum {
+    AirMouseViewSubmenu,
+    AirMouseViewBtMouse,
+    AirMouseViewUsbMouse,
+    AirMouseViewCalibration,
+    AirMouseViewExitConfirm,
+} AirMouseView;

Applications/Official/DEV_FW/source/airmouse/application.fam

@@ -0,0 +1,9 @@
+App(
+    appid="BMI160_Air_Mouse",
+    name="[BMI160] Air Mouse",
+    apptype=FlipperAppType.EXTERNAL,
+    entry_point="air_mouse_app",
+    stack_size=10 * 1024,
+    fap_icon="mouse_10px.png",
+    fap_category="GPIO",
+)

Applications/Official/DEV_FW/source/airmouse/tracking/calibration_data.cc

@@ -0,0 +1,85 @@
+#include <furi.h>
+#include <furi_hal.h>
+
+#define TAG "tracker"
+
+#include "calibration_data.h"
+
+#include <cmath>
+#include <algorithm>
+
+// Student's distribution T value for 95% (two-sided) confidence interval.
+static const double Tn = 1.960;
+
+// Number of samples (degrees of freedom) for the corresponding T values.
+static const int Nn = 200;
+
+void CalibrationData::reset()
+{
+    complete = false;
+    count = 0;
+    sum = Vector::Zero();
+    sumSq = Vector::Zero();
+    mean = Vector::Zero();
+    median = Vector::Zero();
+    sigma = Vector::Zero();
+    delta = Vector::Zero();
+    xData.clear();
+    yData.clear();
+    zData.clear();
+}
+
+bool CalibrationData::add(Vector& data)
+{
+    if (complete) {
+        return true;
+    }
+
+    xData.push_back(data[0]);
+    yData.push_back(data[1]);
+    zData.push_back(data[2]);
+
+    sum += data;
+    sumSq += data * data;
+    count++;
+
+    if (count >= Nn) {
+        calcDelta();
+        complete = true;
+    }
+
+    return complete;
+}
+
+static inline double medianOf(std::vector<double>& list)
+{
+    std::sort(list.begin(), list.end());
+    int count = list.size();
+    int middle = count / 2;
+    return (count % 2 == 1) ? list[middle] : (list[middle - 1] + list[middle]) / 2.0l;
+}
+
+void CalibrationData::calcDelta()
+{
+    median.Set(medianOf(xData), medianOf(yData), medianOf(zData));
+
+    mean = sum / count;
+    Vector m2 = mean * mean;
+    Vector d = sumSq / count - m2;
+    Vector s2 = (d * count) / (count - 1);
+    sigma = Vector(std::sqrt(d[0]), std::sqrt(d[1]), std::sqrt(d[2]));
+    Vector s = Vector(std::sqrt(s2[0]), std::sqrt(s2[1]), std::sqrt(s2[2]));
+    delta = s * Tn / std::sqrt((double)count);
+    Vector low = mean - delta;
+    Vector high = mean + delta;
+
+    FURI_LOG_I(TAG,
+        "M[x] = { %f ... %f }  //  median = %f  //  avg = %f  //  delta = %f  //  sigma = %f",
+        low[0], high[0], median[0], mean[0], delta[0], sigma[0]);
+    FURI_LOG_I(TAG,
+        "M[y] = { %f ... %f }  //  median = %f  //  avg = %f  //  delta = %f  //  sigma = %f",
+        low[1], high[1], median[1], mean[1], delta[1], sigma[1]);
+    FURI_LOG_I(TAG,
+        "M[z] = { %f ... %f }  //  median = %f  //  avg = %f  //  delta = %f  //  sigma = %f",
+        low[2], high[2], median[2], mean[2], delta[2], sigma[2]);
+}

Applications/Official/DEV_FW/source/airmouse/tracking/calibration_data.h

@@ -0,0 +1,117 @@
+#pragma once
+
+#include <toolbox/saved_struct.h>
+#include <storage/storage.h>
+#include <vector>
+
+#include "util/vector.h"
+
+#define CALIBRATION_DATA_VER (1)
+#define CALIBRATION_DATA_FILE_NAME ".calibration.data"
+#define CALIBRATION_DATA_PATH INT_PATH(CALIBRATION_DATA_FILE_NAME)
+#define CALIBRATION_DATA_MAGIC (0x23)
+
+#define CALIBRATION_DATA_SAVE(x)   \
+    saved_struct_save(             \
+        CALIBRATION_DATA_PATH,     \
+        (x),                       \
+        sizeof(CalibrationMedian), \
+        CALIBRATION_DATA_MAGIC,    \
+        CALIBRATION_DATA_VER)
+
+#define CALIBRATION_DATA_LOAD(x)   \
+    saved_struct_load(             \
+        CALIBRATION_DATA_PATH,     \
+        (x),                       \
+        sizeof(CalibrationMedian), \
+        CALIBRATION_DATA_MAGIC,    \
+        CALIBRATION_DATA_VER)
+
+typedef struct {
+    double x;
+    double y;
+    double z;
+} CalibrationMedian;
+
+typedef cardboard::Vector3 Vector;
+
+/**
+ * Helper class to gather some stats and store the calibration data. Right now it calculates a lot
+ * more stats than actually needed. Some of them are used for logging the sensors quality (and
+ * filing bugs), other may be required in the future, e.g. for bias.
+ */
+class CalibrationData {
+public:
+    /**
+     * Check if the sensors were calibrated before.
+     *
+     * @return {@code true} if calibration data is available, or {@code false} otherwise.
+     */
+    bool isComplete() {
+        return complete;
+    }
+
+    /** Prepare to collect new calibration data. */
+    void reset();
+
+    /**
+     * Retrieve the median gyroscope readings.
+     *
+     * @return Three-axis median vector.
+     */
+    Vector getMedian() {
+        return median;
+    }
+
+    /**
+     * Retrieve the mean gyroscope readings.
+     *
+     * @return Three-axis mean vector.
+     */
+    Vector getMean() {
+        return mean;
+    }
+
+    /**
+     * Retrieve the standard deviation of gyroscope readings.
+     *
+     * @return Three-axis standard deviation vector.
+     */
+    Vector getSigma() {
+        return sigma;
+    }
+
+    /**
+     * Retrieve the confidence interval size of gyroscope readings.
+     *
+     * @return Three-axis confidence interval size vector.
+     */
+    Vector getDelta() {
+        return delta;
+    }
+
+    /**
+     * Add a new gyroscope reading to the stats.
+     *
+     * @param data gyroscope values vector.
+     * @return {@code true} if we now have enough data for calibration, or {@code false} otherwise.
+     */
+    bool add(Vector& data);
+
+private:
+    // Calculates the confidence interval (mean +- delta) and some other related values, like
+    // standard deviation, etc. See https://en.wikipedia.org/wiki/Student%27s_t-distribution
+    void calcDelta();
+
+    int count;
+    bool complete;
+    Vector sum;
+    Vector sumSq;
+    Vector mean;
+    Vector median;
+    Vector sigma;
+    Vector delta;
+    std::vector<double> xData;
+    std::vector<double> yData;
+    std::vector<double> zData;
+};

Applications/Official/DEV_FW/source/airmouse/tracking/imu/bmi160.h

@@ -0,0 +1,992 @@
+/**
+* Copyright (c) 2021 Bosch Sensortec GmbH. All rights reserved.
+*
+* BSD-3-Clause
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions are met:
+*
+* 1. Redistributions of source code must retain the above copyright
+*    notice, this list of conditions and the following disclaimer.
+*
+* 2. Redistributions in binary form must reproduce the above copyright
+*    notice, this list of conditions and the following disclaimer in the
+*    documentation and/or other materials provided with the distribution.
+*
+* 3. Neither the name of the copyright holder nor the names of its
+*    contributors may be used to endorse or promote products derived from
+*    this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
+* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
+* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+* @file       bmi160.h
+* @date       2021-10-05
+* @version    v3.9.2
+*
+*/
+
+/*!
+ * @defgroup bmi160 BMI160
+ */
+
+#ifndef BMI160_H_
+#define BMI160_H_
+
+/*************************** C++ guard macro *****************************/
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "bmi160_defs.h"
+#ifdef __KERNEL__
+#include <bmi160_math.h>
+#else
+#include <math.h>
+#include <string.h>
+#include <stdlib.h>
+#endif
+
+/*********************** User function prototypes ************************/
+
+/**
+ * \ingroup bmi160
+ * \defgroup bmi160ApiInit Initialization
+ * @brief Initialize the sensor and device structure
+ */
+
+/*!
+ * \ingroup bmi160ApiInit
+ * \page bmi160_api_bmi160_init bmi160_init
+ * \code
+ * int8_t bmi160_init(struct bmi160_dev *dev);
+ * \endcode
+ * @details This API is the entry point for sensor.It performs
+ *  the selection of I2C/SPI read mechanism according to the
+ *  selected interface and reads the chip-id of bmi160 sensor.
+ *
+ *  @param[in,out] dev : Structure instance of bmi160_dev
+ *  @note : Refer user guide for detailed info.
+ *
+ *  @return Result of API execution status
+ * @retval Zero Success
+ * @retval Negative Error
+ */
+int8_t bmi160_init(struct bmi160_dev* dev);
+
+/**
+ * \ingroup bmi160
+ * \defgroup bmi160ApiRegs Registers
+ * @brief Read data from the given register address of sensor
+ */
+
+/*!
+ * \ingroup bmi160ApiRegs
+ * \page bmi160_api_bmi160_get_regs bmi160_get_regs
+ * \code
+ * int8_t bmi160_get_regs(uint8_t reg_addr, uint8_t *data, uint16_t len, const struct bmi160_dev *dev);
+ * \endcode
+ * @details This API reads the data from the given register address of sensor.
+ *
+ * @param[in] reg_addr  : Register address from where the data to be read
+ * @param[out] data     : Pointer to data buffer to store the read data.
+ * @param[in] len       : No of bytes of data to be read.
+ * @param[in] dev       : Structure instance of bmi160_dev.
+ *
+ * @note For most of the registers auto address increment applies, with the
+ * exception of a few special registers, which trap the address. For e.g.,
+ * Register address - 0x24(BMI160_FIFO_DATA_ADDR)
+ *
+ * @return Result of API execution status
+ * @retval Zero Success
+ * @retval Negative Error
+ */
+int8_t
+    bmi160_get_regs(uint8_t reg_addr, uint8_t* data, uint16_t len, const struct bmi160_dev* dev);
+
+/*!
+ * \ingroup bmi160ApiRegs
+ * \page bmi160_api_bmi160_set_regs bmi160_set_regs
+ * \code
+ * int8_t bmi160_set_regs(uint8_t reg_addr, uint8_t *data, uint16_t len, const struct bmi160_dev *dev);
+ * \endcode
+ * @details This API writes the given data to the register address
+ * of sensor.
+ *
+ * @param[in] reg_addr  : Register address from where the data to be written.
+ * @param[in] data      : Pointer to data buffer which is to be written
+ * in the sensor.
+ * @param[in] len       : No of bytes of data to write..
+ * @param[in] dev       : Structure instance of bmi160_dev.
+ *
+ * @return Result of API execution status
+ * @retval Zero Success
+ * @retval Negative Error
+ */
+int8_t
+    bmi160_set_regs(uint8_t reg_addr, uint8_t* data, uint16_t len, const struct bmi160_dev* dev);
+
+/**
+ * \ingroup bmi160
+ * \defgroup bmi160ApiSoftreset Soft reset
+ * @brief Perform soft reset of the sensor
+ */
+
+/*!
+ * \ingroup bmi160ApiSoftreset
+ * \page bmi160_api_bmi160_soft_reset bmi160_soft_reset
+ * \code
+ * int8_t bmi160_soft_reset(struct bmi160_dev *dev);
+ * \endcode
+ * @details This API resets and restarts the device.
+ * All register values are overwritten with default parameters.
+ *
+ * @param[in] dev  : Structure instance of bmi160_dev.
+ *
+ * @return Result of API execution status
+ * @retval Zero Success
+ * @retval Negative Error
+ */
+int8_t bmi160_soft_reset(struct bmi160_dev* dev);
+
+/**
+ * \ingroup bmi160
+ * \defgroup bmi160ApiConfig Configuration
+ * @brief Configuration of the sensor
+ */
+
+/*!
+ * \ingroup bmi160ApiConfig
+ * \page bmi160_api_bmi160_set_sens_conf bmi160_set_sens_conf
+ * \code
+ * int8_t bmi160_set_sens_conf(struct bmi160_dev *dev);
+ * \endcode
+ * @details This API configures the power mode, range and bandwidth
+ * of sensor.
+ *
+ * @param[in] dev    : Structure instance of bmi160_dev.
+ * @note : Refer user guide for detailed info.
+ *
+ * @return Result of API execution status
+ * @retval Zero Success
+ * @retval Negative Error
+ */
+int8_t bmi160_set_sens_conf(struct bmi160_dev* dev);
+
+/*!
+ * \ingroup bmi160ApiConfig
+ * \page bmi160_api_bmi160_get_sens_conf bmi160_get_sens_conf
+ * \code
+ * int8_t bmi160_get_sens_conf(struct bmi160_dev *dev);
+ * \endcode
+ * @details This API gets accel and gyro configurations.
+ *
+ * @param[out] dev    : Structure instance of bmi160_dev.
+ * @note : Refer user guide for detailed info.
+ *
+ * @return Result of API execution status
+ * @retval Zero Success
+ * @retval Negative Error
+ */
+int8_t bmi160_get_sens_conf(struct bmi160_dev* dev);
+
+/**
+ * \ingroup bmi160
+ * \defgroup bmi160ApiPowermode Power mode
+ * @brief Set / Get power mode of the sensor
+ */
+
+/*!
+ * \ingroup bmi160ApiPowermode
+ * \page bmi160_api_bmi160_set_power_mode bmi160_set_power_mode
+ * \code
+ * int8_t bmi160_set_power_mode(struct bmi160_dev *dev);
+ * \endcode
+ * @details This API sets the power mode of the sensor.
+ *
+ * @param[in] dev  : Structure instance of bmi160_dev.
+ *
+ * @return Result of API execution status
+ * @retval Zero Success
+ * @retval Negative Error
+ */
+int8_t bmi160_set_power_mode(struct bmi160_dev* dev);
+
+/*!
+ * \ingroup bmi160ApiPowermode
+ * \page bmi160_api_bmi160_get_power_mode bmi160_get_power_mode
+ * \code
+ * int8_t bmi160_get_power_mode(struct bmi160_dev *dev);
+ * \endcode
+ * @details This API gets the power mode of the sensor.
+ *
+ * @param[in] dev         : Structure instance of bmi160_dev
+ *
+ * @return Result of API execution status
+ * @retval Zero Success
+ * @retval Negative Error
+ */
+int8_t bmi160_get_power_mode(struct bmi160_dev* dev);
+
+/**
+ * \ingroup bmi160
+ * \defgroup bmi160ApiData Sensor Data
+ * @brief Read sensor data
+ */
+
+/*!
+ * \ingroup bmi160ApiData
+ * \page bmi160_api_bmi160_get_sensor_data bmi160_get_sensor_data
+ * \code
+ * int8_t bmi160_get_sensor_data(uint8_t select_sensor,
+ *                             struct bmi160_sensor_data *accel,
+ *                             struct bmi160_sensor_data *gyro,
+ *                             const struct bmi160_dev *dev);
+ *
+ * \endcode
+ * @details This API reads sensor data, stores it in
+ * the bmi160_sensor_data structure pointer passed by the user.
+ * The user can ask for accel data ,gyro data or both sensor
+ * data using bmi160_select_sensor enum
+ *
+ * @param[in] select_sensor    : enum to choose accel,gyro or both sensor data
+ * @param[out] accel    : Structure pointer to store accel data
+ * @param[out] gyro     : Structure pointer to store gyro data
+ * @param[in] dev       : Structure instance of bmi160_dev.
+ * @note : Refer user guide for detailed info.
+ *
+ * @return Result of API execution status
+ * @retval Zero Success
+ * @retval Negative Error
+ */
+int8_t bmi160_get_sensor_data(
+    uint8_t select_sensor,
+    struct bmi160_sensor_data* accel,
+    struct bmi160_sensor_data* gyro,
+    const struct bmi160_dev* dev);
+
+/**
+ * \ingroup bmi160
+ * \defgroup bmi160ApiInt Interrupt configuration
+ * @brief Set interrupt configuration of the sensor
+ */
+
+/*!
+ * \ingroup bmi160ApiInt
+ * \page bmi160_api_bmi160_set_int_config bmi160_set_int_config
+ * \code
+ * int8_t bmi160_set_int_config(struct bmi160_int_settg *int_config, struct bmi160_dev *dev);
+ * \endcode
+ * @details This API configures the necessary interrupt based on
+ *  the user settings in the bmi160_int_settg structure instance.
+ *
+ * @param[in] int_config  : Structure instance of bmi160_int_settg.
+ * @param[in] dev         : Structure instance of bmi160_dev.
+ * @note : Refer user guide for detailed info.
+ *
+ * @return Result of API execution status
+ * @retval Zero Success
+ * @retval Negative Error
+ */
+int8_t bmi160_set_int_config(struct bmi160_int_settg* int_config, struct bmi160_dev* dev);
+
+/**
+ * \ingroup bmi160
+ * \defgroup bmi160ApiStepC Step counter
+ * @brief Step counter operations
+ */
+
+/*!
+ * \ingroup bmi160ApiStepC
+ * \page bmi160_api_bmi160_set_step_counter bmi160_set_step_counter
+ * \code
+ * int8_t bmi160_set_step_counter(uint8_t step_cnt_enable, const struct bmi160_dev *dev);
+ * \endcode
+ * @details This API enables the step counter feature.
+ *
+ * @param[in] step_cnt_enable   : value to enable or disable
+ * @param[in] dev       : Structure instance of bmi160_dev.
+ * @note : Refer user guide for detailed info.
+ *
+ * @return Result of API execution status
+ * @retval Zero Success
+ * @retval Negative Error
+ */
+int8_t bmi160_set_step_counter(uint8_t step_cnt_enable, const struct bmi160_dev* dev);
+
+/*!
+ * \ingroup bmi160ApiStepC
+ * \page bmi160_api_bmi160_read_step_counter bmi160_read_step_counter
+ * \code
+ * int8_t bmi160_read_step_counter(uint16_t *step_val, const struct bmi160_dev *dev);
+ * \endcode
+ * @details This API reads the step counter value.
+ *
+ * @param[in] step_val    : Pointer to store the step counter value.
+ * @param[in] dev         : Structure instance of bmi160_dev.
+ * @note : Refer user guide for detailed info.
+ *
+ * @return Result of API execution status
+ * @retval Zero Success
+ * @retval Negative Error
+ */
+int8_t bmi160_read_step_counter(uint16_t* step_val, const struct bmi160_dev* dev);
+
+/**
+ * \ingroup bmi160
+ * \defgroup bmi160ApiAux Auxiliary sensor
+ * @brief Auxiliary sensor operations
+ */
+
+/*!
+ * \ingroup bmi160ApiAux
+ * \page bmi160_api_bmi160_aux_read bmi160_aux_read
+ * \code
+ * int8_t bmi160_aux_read(uint8_t reg_addr, uint8_t *aux_data, uint16_t len, const struct bmi160_dev *dev);
+ * \endcode
+ * @details This API reads the mention no of byte of data from the given
+ * register address of auxiliary sensor.
+ *
+ * @param[in] reg_addr    : Address of register to read.
+ * @param[in] aux_data    : Pointer to store the read data.
+ * @param[in] len     : No of bytes to read.
+ * @param[in] dev         : Structure instance of bmi160_dev.
+ * @note : Refer user guide for detailed info.
+ *
+ * @return Result of API execution status
+ * @retval Zero Success
+ * @retval Negative Error
+ */
+int8_t bmi160_aux_read(
+    uint8_t reg_addr,
+    uint8_t* aux_data,
+    uint16_t len,
+    const struct bmi160_dev* dev);
+
+/*!
+ * \ingroup bmi160ApiAux
+ * \page bmi160_api_bmi160_aux_write bmi160_aux_write
+ * \code
+ * int8_t bmi160_aux_write(uint8_t reg_addr, uint8_t *aux_data, uint16_t len, const struct bmi160_dev *dev);
+ * \endcode
+ * @details This API writes the mention no of byte of data to the given
+ * register address of auxiliary sensor.
+ *
+ * @param[in] reg_addr    : Address of register to write.
+ * @param[in] aux_data    : Pointer to write data.
+ * @param[in] len     : No of bytes to write.
+ * @param[in] dev         : Structure instance of bmi160_dev.
+ * @note : Refer user guide for detailed info.
+ *
+ * @return Result of API execution status
+ * @retval Zero Success
+ * @retval Negative Error
+ */
+int8_t bmi160_aux_write(
+    uint8_t reg_addr,
+    uint8_t* aux_data,
+    uint16_t len,
+    const struct bmi160_dev* dev);
+
+/*!
+ * \ingroup bmi160ApiAux
+ * \page bmi160_api_bmi160_aux_init bmi160_aux_init
+ * \code
+ * int8_t bmi160_aux_init(const struct bmi160_dev *dev);
+ * \endcode
+ * @details This API initialize the auxiliary sensor
+ * in order to access it.
+ *
+ * @param[in] dev         : Structure instance of bmi160_dev.
+ * @note : Refer user guide for detailed info.
+ *
+ * @return Result of API execution status
+ * @retval Zero Success
+ * @retval Negative Error
+ */
+int8_t bmi160_aux_init(const struct bmi160_dev* dev);
+
+/*!
+ * \ingroup bmi160ApiAux
+ * \page bmi160_api_bmi160_set_aux_auto_mode bmi160_set_aux_auto_mode
+ * \code
+ * int8_t bmi160_set_aux_auto_mode(uint8_t *data_addr, struct bmi160_dev *dev);
+ * \endcode
+ * @details This API is used to setup the auxiliary sensor of bmi160 in auto mode
+ * Thus enabling the auto update of 8 bytes of data from auxiliary sensor
+ * to BMI160 register address 0x04 to 0x0B
+ *
+ * @param[in] data_addr    : Starting address of aux. sensor's data register
+ *                           (BMI160 registers 0x04 to 0x0B will be updated
+ *                           with 8 bytes of data from auxiliary sensor
+ *                           starting from this register address.)
+ * @param[in] dev      : Structure instance of bmi160_dev.
+ *
+ * @note : Set the value of auxiliary polling rate by setting
+ *         dev->aux_cfg.aux_odr to the required value from the table
+ *         before calling this API
+ *
+ *@verbatim
+ *   dev->aux_cfg.aux_odr  |   Auxiliary ODR (Hz)
+ *  -----------------------|-----------------------
+ *  BMI160_AUX_ODR_0_78HZ  |        25/32
+ *  BMI160_AUX_ODR_1_56HZ  |        25/16
+ *  BMI160_AUX_ODR_3_12HZ  |        25/8
+ *  BMI160_AUX_ODR_6_25HZ  |        25/4
+ *  BMI160_AUX_ODR_12_5HZ  |        25/2
+ *  BMI160_AUX_ODR_25HZ    |        25
+ *  BMI160_AUX_ODR_50HZ    |        50
+ *  BMI160_AUX_ODR_100HZ   |        100
+ *  BMI160_AUX_ODR_200HZ   |        200
+ *  BMI160_AUX_ODR_400HZ   |        400
+ *  BMI160_AUX_ODR_800HZ   |        800
+ *@endverbatim
+ *
+ * @note : Other values of  dev->aux_cfg.aux_odr are reserved and not for use
+ *
+ * @return Result of API execution status
+ * @retval Zero Success
+ * @retval Negative Error
+ */
+int8_t bmi160_set_aux_auto_mode(uint8_t* data_addr, struct bmi160_dev* dev);
+
+/*!
+ * \ingroup bmi160ApiAux
+ * \page bmi160_api_bmi160_config_aux_mode bmi160_config_aux_mode
+ * \code
+ * int8_t bmi160_config_aux_mode(const struct bmi160_dev *dev);
+ * \endcode
+ * @details This API configures the 0x4C register and settings like
+ * Auxiliary sensor manual enable/ disable and aux burst read length.
+ *
+ * @param[in] dev    : Structure instance of bmi160_dev.
+ *
+ * @return Result of API execution status
+ * @retval Zero Success
+ * @retval Negative Error
+ */
+int8_t bmi160_config_aux_mode(const struct bmi160_dev* dev);
+
+/*!
+ * \ingroup bmi160ApiAux
+ * \page bmi160_api_bmi160_read_aux_data_auto_mode bmi160_read_aux_data_auto_mode
+ * \code
+ * int8_t bmi160_read_aux_data_auto_mode(uint8_t *aux_data, const struct bmi160_dev *dev);
+ * \endcode
+ * @details This API is used to read the raw uncompensated auxiliary sensor
+ * data of 8 bytes from BMI160 register address 0x04 to 0x0B
+ *
+ * @param[in] aux_data       : Pointer to user array of length 8 bytes
+ *                             Ensure that the aux_data array is of
+ *                             length 8 bytes
+ * @param[in] dev        : Structure instance of bmi160_dev
+ *
+ * @retval zero -> Success  / -ve value -> Error
+ * @retval Zero Success
+ * @retval Negative Error
+ */
+int8_t bmi160_read_aux_data_auto_mode(uint8_t* aux_data, const struct bmi160_dev* dev);
+
+/**
+ * \ingroup bmi160
+ * \defgroup bmi160ApiSelfTest Self test
+ * @brief Perform self test of the sensor
+ */
+
+/*!
+ * \ingroup bmi160ApiSelfTest
+ * \page bmi160_api_bmi160_perform_self_test bmi160_perform_self_test
+ * \code
+ * int8_t bmi160_perform_self_test(uint8_t select_sensor, struct bmi160_dev *dev);
+ * \endcode
+ * @details This is used to perform self test of accel/gyro of the BMI160 sensor
+ *
+ * @param[in] select_sensor  : enum to choose accel or gyro for self test
+ * @param[in] dev            : Structure instance of bmi160_dev
+ *
+ * @note self test can be performed either for accel/gyro at any instant.
+ *
+ *@verbatim
+ *     value of select_sensor       |  Inference
+ *----------------------------------|--------------------------------
+ *   BMI160_ACCEL_ONLY              | Accel self test enabled
+ *   BMI160_GYRO_ONLY               | Gyro self test enabled
+ *   BMI160_BOTH_ACCEL_AND_GYRO     | NOT TO BE USED
+ *@endverbatim
+ *
+ * @note The return value of this API gives us the result of self test.
+ *
+ * @note Performing self test does soft reset of the sensor, User can
+ * set the desired settings after performing the self test.
+ *
+ * @return Result of API execution status
+ * @retval  BMI160_OK                       Self test success
+ * @retval  BMI160_W_GYRO_SELF_TEST_FAIL    Gyro self test fail
+ * @retval  BMI160_W_ACCEl_SELF_TEST_FAIL   Accel self test fail
+ */
+int8_t bmi160_perform_self_test(uint8_t select_sensor, struct bmi160_dev* dev);
+
+/**
+ * \ingroup bmi160
+ * \defgroup bmi160ApiFIFO FIFO
+ * @brief FIFO operations of the sensor
+ */
+
+/*!
+ * \ingroup bmi160ApiFIFO
+ * \page bmi160_api_bmi160_get_fifo_data bmi160_get_fifo_data
+ * \code
+ * int8_t bmi160_get_fifo_data(struct bmi160_dev const *dev);
+ * \endcode
+ * @details This API reads data from the fifo buffer.
+ *
+ *  @note User has to allocate the FIFO buffer along with
+ *  corresponding fifo length from his side before calling this API
+ *  as mentioned in the readme.md
+ *
+ *  @note User must specify the number of bytes to read from the FIFO in
+ *  dev->fifo->length , It will be updated by the number of bytes actually
+ *  read from FIFO after calling this API
+ *
+ *  @param[in] dev     : Structure instance of bmi160_dev.
+ *
+ *  @return Result of API execution status
+ *  @retval Zero Success
+ *  @retval Negative Error
+ */
+int8_t bmi160_get_fifo_data(struct bmi160_dev const* dev);
+
+/*!
+ * \ingroup bmi160ApiFIFO
+ * \page bmi160_api_bmi160_set_fifo_flush bmi160_set_fifo_flush
+ * \code
+ * int8_t bmi160_set_fifo_flush(const struct bmi160_dev *dev);
+ * \endcode
+ * @details This API writes fifo_flush command to command register.This
+ *  action clears all data in the Fifo without changing fifo configuration
+ *  settings.
+ *
+ *  @param[in] dev     : Structure instance of bmi160_dev
+ *
+ *  @return Result of API execution status
+ *  @retval 0 -> Success
+ *  @retval Any non zero value -> Fail
+ *
+ */
+int8_t bmi160_set_fifo_flush(const struct bmi160_dev* dev);
+
+/*!
+ * \ingroup bmi160ApiFIFO
+ * \page bmi160_api_bmi160_set_fifo_config bmi160_set_fifo_config
+ * \code
+ * int8_t bmi160_set_fifo_config(uint8_t config, uint8_t enable, struct bmi160_dev const *dev);
+ * \endcode
+ * @details This API sets the FIFO configuration in the sensor.
+ *
+ *  @param[in] config : variable used to specify the FIFO
+ *  configurations which are to be enabled or disabled in the sensor.
+ *
+ *  @note : User can set either set one or more or all FIFO configurations
+ *  by ORing the below mentioned macros.
+ *
+ *@verbatim
+ *      config                  |   Value
+ *      ------------------------|---------------------------
+ *      BMI160_FIFO_TIME        |   0x02
+ *      BMI160_FIFO_TAG_INT2    |   0x04
+ *      BMI160_FIFO_TAG_INT1    |   0x08
+ *      BMI160_FIFO_HEADER      |   0x10
+ *      BMI160_FIFO_AUX         |   0x20
+ *      BMI160_FIFO_ACCEL       |   0x40
+ *      BMI160_FIFO_GYRO        |   0x80
+ *@endverbatim
+ *
+ *  @param[in] enable : Parameter used to enable or disable the above
+ *  FIFO configuration
+ *  @param[in] dev : Structure instance of bmi160_dev.
+ *
+ *  @return status of bus communication result
+ *  @retval 0 -> Success
+ *  @retval Any non zero value -> Fail
+ *
+ */
+int8_t bmi160_set_fifo_config(uint8_t config, uint8_t enable, struct bmi160_dev const* dev);
+
+/*!
+ * \ingroup bmi160ApiFIFO
+ * \page bmi160_api_bmi160_set_fifo_down bmi160_set_fifo_down
+ * \code
+ * int8_t bmi160_set_fifo_down(uint8_t fifo_down, const struct bmi160_dev *dev);
+ * \endcode
+ * @details This API is used to configure the down sampling ratios of
+ *  the accel and gyro data for FIFO.Also, it configures filtered or
+ *  pre-filtered data for the fifo for accel and gyro.
+ *
+ *  @param[in] fifo_down : variable used to specify the FIFO down
+ *  configurations which are to be enabled or disabled in the sensor.
+ *
+ *  @note The user must select one among the following macros to
+ *  select down-sampling ratio for accel
+ *
+ *@verbatim
+ *      config                               |   Value
+ *      -------------------------------------|---------------------------
+ *      BMI160_ACCEL_FIFO_DOWN_ZERO          |   0x00
+ *      BMI160_ACCEL_FIFO_DOWN_ONE           |   0x10
+ *      BMI160_ACCEL_FIFO_DOWN_TWO           |   0x20
+ *      BMI160_ACCEL_FIFO_DOWN_THREE         |   0x30
+ *      BMI160_ACCEL_FIFO_DOWN_FOUR          |   0x40
+ *      BMI160_ACCEL_FIFO_DOWN_FIVE          |   0x50
+ *      BMI160_ACCEL_FIFO_DOWN_SIX           |   0x60
+ *      BMI160_ACCEL_FIFO_DOWN_SEVEN         |   0x70
+ *@endverbatim
+ *
+ *  @note The user must select one among the following macros to
+ *  select down-sampling ratio for gyro
+ *
+ *@verbatim
+ *      config                               |   Value
+ *      -------------------------------------|---------------------------
+ *      BMI160_GYRO_FIFO_DOWN_ZERO           |   0x00
+ *      BMI160_GYRO_FIFO_DOWN_ONE            |   0x01
+ *      BMI160_GYRO_FIFO_DOWN_TWO            |   0x02
+ *      BMI160_GYRO_FIFO_DOWN_THREE          |   0x03
+ *      BMI160_GYRO_FIFO_DOWN_FOUR           |   0x04
+ *      BMI160_GYRO_FIFO_DOWN_FIVE           |   0x05
+ *      BMI160_GYRO_FIFO_DOWN_SIX            |   0x06
+ *      BMI160_GYRO_FIFO_DOWN_SEVEN          |   0x07
+ *@endverbatim
+ *
+ *  @note The user can enable filtered accel data by the following macro
+ *
+ *@verbatim
+ *      config                               |   Value
+ *      -------------------------------------|---------------------------
+ *      BMI160_ACCEL_FIFO_FILT_EN            |   0x80
+ *@endverbatim
+ *
+ *  @note The user can enable filtered gyro data by the following macro
+ *
+ *@verbatim
+ *      config                               |   Value
+ *      -------------------------------------|---------------------------
+ *      BMI160_GYRO_FIFO_FILT_EN             |   0x08
+ *@endverbatim
+ *
+ *  @note : By ORing the above mentioned macros, the user can select
+ *  the required FIFO down config settings
+ *
+ *  @param[in] dev : Structure instance of bmi160_dev.
+ *
+ *  @return status of bus communication result
+ *  @retval 0 -> Success
+ *  @retval Any non zero value -> Fail
+ *
+ */
+int8_t bmi160_set_fifo_down(uint8_t fifo_down, const struct bmi160_dev* dev);
+
+/*!
+ * \ingroup bmi160ApiFIFO
+ * \page bmi160_api_bmi160_set_fifo_wm bmi160_set_fifo_wm
+ * \code
+ * int8_t bmi160_set_fifo_wm(uint8_t fifo_wm, const struct bmi160_dev *dev);
+ * \endcode
+ * @details This API sets the FIFO watermark level in the sensor.
+ *
+ *  @note The FIFO watermark is issued when the FIFO fill level is
+ *  equal or above the watermark level and units of watermark is 4 bytes.
+ *
+ *  @param[in]  fifo_wm        : Variable used to set the FIFO water mark level
+ *  @param[in]  dev            : Structure instance of bmi160_dev
+ *
+ *  @return Result of API execution status
+ *  @retval 0 -> Success
+ *  @retval Any non zero value -> Fail
+ *
+ */
+int8_t bmi160_set_fifo_wm(uint8_t fifo_wm, const struct bmi160_dev* dev);
+
+/*!
+ * \ingroup bmi160ApiFIFO
+ * \page bmi160_api_bmi160_extract_accel bmi160_extract_accel
+ * \code
+ * int8_t bmi160_extract_accel(struct bmi160_sensor_data *accel_data, uint8_t *accel_length, struct bmi160_dev const
+ **dev);
+ * \endcode
+ * @details This API parses and extracts the accelerometer frames from
+ *  FIFO data read by the "bmi160_get_fifo_data" API and stores it in
+ *  the "accel_data" structure instance.
+ *
+ *  @note The bmi160_extract_accel API should be called only after
+ *  reading the FIFO data by calling the bmi160_get_fifo_data() API.
+ *
+ *  @param[out] accel_data    : Structure instance of bmi160_sensor_data
+ *                              where the accelerometer data in FIFO is stored.
+ *  @param[in,out] accel_length  : Number of valid accelerometer frames
+ *                              (x,y,z axes data) read out from fifo.
+ *  @param[in] dev            : Structure instance of bmi160_dev.
+ *
+ *  @note accel_length is updated with the number of valid accelerometer
+ *  frames extracted from fifo (1 accel frame   = 6 bytes) at the end of
+ *  execution of this API.
+ *
+ *  @return Result of API execution status
+ *  @retval 0 -> Success
+ *  @retval Any non zero value -> Fail
+ *
+ */
+int8_t bmi160_extract_accel(
+    struct bmi160_sensor_data* accel_data,
+    uint8_t* accel_length,
+    struct bmi160_dev const* dev);
+
+/*!
+ * \ingroup bmi160ApiFIFO
+ * \page bmi160_api_bmi160_extract_gyro bmi160_extract_gyro
+ * \code
+ * int8_t bmi160_extract_gyro(struct bmi160_sensor_data *gyro_data, uint8_t *gyro_length, struct bmi160_dev const *dev);
+ * \endcode
+ * @details This API parses and extracts the gyro frames from
+ *  FIFO data read by the "bmi160_get_fifo_data" API and stores it in
+ *  the "gyro_data" structure instance.
+ *
+ *  @note The bmi160_extract_gyro API should be called only after
+ *  reading the FIFO data by calling the bmi160_get_fifo_data() API.
+ *
+ *  @param[out] gyro_data    : Structure instance of bmi160_sensor_data
+ *                             where the gyro data in FIFO is stored.
+ *  @param[in,out] gyro_length  : Number of valid gyro frames
+ *                             (x,y,z axes data) read out from fifo.
+ *  @param[in] dev           : Structure instance of bmi160_dev.
+ *
+ *  @note gyro_length is updated with the number of valid gyro
+ *  frames extracted from fifo (1 gyro frame   = 6 bytes) at the end of
+ *  execution of this API.
+ *
+ *  @return Result of API execution status
+ *  @retval 0 -> Success
+ *  @retval Any non zero value -> Fail
+ *
+ */
+int8_t bmi160_extract_gyro(
+    struct bmi160_sensor_data* gyro_data,
+    uint8_t* gyro_length,
+    struct bmi160_dev const* dev);
+
+/*!
+ * \ingroup bmi160ApiFIFO
+ * \page bmi160_api_bmi160_extract_aux bmi160_extract_aux
+ * \code
+ * int8_t bmi160_extract_aux(struct bmi160_aux_data *aux_data, uint8_t *aux_len, struct bmi160_dev const *dev);
+ * \endcode
+ * @details This API parses and extracts the aux frames from
+ *  FIFO data read by the "bmi160_get_fifo_data" API and stores it in
+ *  the bmi160_aux_data structure instance.
+ *
+ *  @note The bmi160_extract_aux API should be called only after
+ *  reading the FIFO data by calling the bmi160_get_fifo_data() API.
+ *
+ *  @param[out] aux_data    : Structure instance of bmi160_aux_data
+ *                            where the aux data in FIFO is stored.
+ *  @param[in,out] aux_len  : Number of valid aux frames (8bytes)
+ *                            read out from FIFO.
+ *  @param[in] dev          : Structure instance of bmi160_dev.
+ *
+ *  @note aux_len is updated with the number of valid aux
+ *  frames extracted from fifo (1 aux frame = 8 bytes) at the end of
+ *  execution of this API.
+ *
+ *  @return Result of API execution status
+ *  @retval 0 -> Success
+ *  @retval Any non zero value -> Fail
+ *
+ */
+int8_t bmi160_extract_aux(
+    struct bmi160_aux_data* aux_data,
+    uint8_t* aux_len,
+    struct bmi160_dev const* dev);
+
+/**
+ * \ingroup bmi160
+ * \defgroup bmi160ApiFOC FOC
+ * @brief Start FOC of accel and gyro sensors
+ */
+
+/*!
+ * \ingroup bmi160ApiFOC
+ * \page bmi160_api_bmi160_start_foc bmi160_start_foc
+ * \code
+ * int8_t bmi160_start_foc(const struct bmi160_foc_conf *foc_conf,
+ * \endcode
+ * @details This API starts the FOC of accel and gyro
+ *
+ *  @note FOC should not be used in low-power mode of sensor
+ *
+ *  @note Accel FOC targets values of +1g , 0g , -1g
+ *  Gyro FOC always targets value of 0 dps
+ *
+ *  @param[in] foc_conf    : Structure instance of bmi160_foc_conf which
+ *                                   has the FOC configuration
+ *  @param[in,out] offset  : Structure instance to store Offset
+ *                                   values read from sensor
+ *  @param[in] dev         : Structure instance of bmi160_dev.
+ *
+ *  @note Pre-requisites for triggering FOC in accel , Set the following,
+ *   Enable the acc_off_en
+ *       Ex :  foc_conf.acc_off_en = BMI160_ENABLE;
+ *
+ *   Set the desired target values of FOC to each axes (x,y,z) by using the
+ *   following macros
+ *       - BMI160_FOC_ACCEL_DISABLED
+ *       - BMI160_FOC_ACCEL_POSITIVE_G
+ *       - BMI160_FOC_ACCEL_NEGATIVE_G
+ *       - BMI160_FOC_ACCEL_0G
+ *
+ *   Ex : foc_conf.foc_acc_x  = BMI160_FOC_ACCEL_0G;
+ *        foc_conf.foc_acc_y  = BMI160_FOC_ACCEL_0G;
+ *        foc_conf.foc_acc_z  = BMI160_FOC_ACCEL_POSITIVE_G;
+ *
+ *  @note Pre-requisites for triggering FOC in gyro ,
+ *  Set the following parameters,
+ *
+ *   Ex : foc_conf.foc_gyr_en = BMI160_ENABLE;
+ *        foc_conf.gyro_off_en = BMI160_ENABLE;
+ *
+ *  @return Result of API execution status
+ *  @retval 0 -> Success
+ *  @retval Any non zero value -> Fail
+ */
+int8_t bmi160_start_foc(
+    const struct bmi160_foc_conf* foc_conf,
+    struct bmi160_offsets* offset,
+    struct bmi160_dev const* dev);
+
+/**
+ * \ingroup bmi160
+ * \defgroup bmi160ApiOffsets Offsets
+ * @brief Set / Get offset values of accel and gyro sensors
+ */
+
+/*!
+ * \ingroup bmi160ApiOffsets
+ * \page bmi160_api_bmi160_get_offsets bmi160_get_offsets
+ * \code
+ * int8_t bmi160_get_offsets(struct bmi160_offsets *offset, const struct bmi160_dev *dev);
+ * \endcode
+ * @details This API reads and stores the offset values of accel and gyro
+ *
+ *  @param[in,out] offset : Structure instance of bmi160_offsets in which
+ *                          the offset values are read and stored
+ *  @param[in] dev        : Structure instance of bmi160_dev.
+ *
+ *  @return Result of API execution status
+ *  @retval 0 -> Success
+ *  @retval Any non zero value -> Fail
+ */
+int8_t bmi160_get_offsets(struct bmi160_offsets* offset, const struct bmi160_dev* dev);
+
+/*!
+ * \ingroup bmi160ApiOffsets
+ * \page bmi160_api_bmi160_set_offsets bmi160_set_offsets
+ * \code
+ * int8_t bmi160_set_offsets(const struct bmi160_foc_conf *foc_conf,
+ *                         const struct bmi160_offsets *offset,
+ *                         struct bmi160_dev const *dev);
+ * \endcode
+ * @details This API writes the offset values of accel and gyro to
+ *  the sensor but these values will be reset on POR or soft reset.
+ *
+ *  @param[in] foc_conf    : Structure instance of bmi160_foc_conf which
+ *                                   has the FOC configuration
+ *  @param[in] offset      : Structure instance in which user updates offset
+ *                            values which are to be written in the sensor
+ *  @param[in] dev         : Structure instance of bmi160_dev.
+ *
+ *  @note Offsets can be set by user like offset->off_acc_x = 10;
+ *  where 1LSB = 3.9mg and for gyro 1LSB = 0.061degrees/second
+ *
+ * @note BMI160 offset values for xyz axes of accel should be within range of
+ *  BMI160_ACCEL_MIN_OFFSET (-128) to BMI160_ACCEL_MAX_OFFSET (127)
+ *
+ * @note BMI160 offset values for xyz axes of gyro should be within range of
+ *  BMI160_GYRO_MIN_OFFSET (-512) to BMI160_GYRO_MAX_OFFSET (511)
+ *
+ *  @return Result of API execution status
+ *  @retval 0 -> Success
+ *  @retval Any non zero value -> Fail
+ */
+int8_t bmi160_set_offsets(
+    const struct bmi160_foc_conf* foc_conf,
+    const struct bmi160_offsets* offset,
+    struct bmi160_dev const* dev);
+
+/**
+ * \ingroup bmi160
+ * \defgroup bmi160ApiNVM NVM
+ * @brief Write image registers values to NVM
+ */
+
+/*!
+ * \ingroup bmi160ApiNVM
+ * \page bmi160_api_bmi160_update_nvm bmi160_update_nvm
+ * \code
+ * int8_t bmi160_update_nvm(struct bmi160_dev const *dev);
+ * \endcode
+ * @details This API writes the image registers values to NVM which is
+ *  stored even after POR or soft reset
+ *
+ *  @param[in] dev         : Structure instance of bmi160_dev.
+ *
+ *  @return Result of API execution status
+ *  @retval 0 -> Success
+ *  @retval Any non zero value -> Fail
+ */
+int8_t bmi160_update_nvm(struct bmi160_dev const* dev);
+
+/**
+ * \ingroup bmi160
+ * \defgroup bmi160ApiInts Interrupt status
+ * @brief Read interrupt status from the sensor
+ */
+
+/*!
+ * \ingroup bmi160ApiInts
+ * \page bmi160_api_bmi160_get_int_status bmi160_get_int_status
+ * \code
+ * int8_t bmi160_get_int_status(enum bmi160_int_status_sel int_status_sel,
+ *                            union bmi160_int_status *int_status,
+ *                            struct bmi160_dev const *dev);
+ * \endcode
+ * @details This API gets the interrupt status from the sensor.
+ *
+ *  @param[in] int_status_sel       : Enum variable to select either individual or all the
+ *  interrupt status bits.
+ *  @param[in] int_status           : pointer variable to get the interrupt status
+ *  from the sensor.
+ *  param[in] dev                   : Structure instance of bmi160_dev.
+ *
+ *  @return Result of API execution status
+ *  @retval 0 -> Success
+ *  @retval Any non zero value -> Fail
+ */
+int8_t bmi160_get_int_status(
+    enum bmi160_int_status_sel int_status_sel,
+    union bmi160_int_status* int_status,
+    struct bmi160_dev const* dev);
+
+/*************************** C++ guard macro *****************************/
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* BMI160_H_ */

Applications/Official/DEV_FW/source/airmouse/tracking/imu/imu.c

@@ -0,0 +1,29 @@
+#include "imu.h"
+#include <furi_hal.h>
+
+bool bmi160_begin();
+int bmi160_read(double* vec);
+
+bool lsm6ds3trc_begin();
+void lsm6ds3trc_end();
+int lsm6ds3trc_read(double* vec);
+
+bool imu_begin() {
+    furi_hal_i2c_acquire(&furi_hal_i2c_handle_external);
+    bool ret = bmi160_begin(); // lsm6ds3trc_begin();
+    furi_hal_i2c_release(&furi_hal_i2c_handle_external);
+    return ret;
+}
+
+void imu_end() {
+    // furi_hal_i2c_acquire(&furi_hal_i2c_handle_external);
+    // lsm6ds3trc_end();
+    // furi_hal_i2c_release(&furi_hal_i2c_handle_external);
+}
+
+int imu_read(double* vec) {
+    furi_hal_i2c_acquire(&furi_hal_i2c_handle_external);
+    int ret = bmi160_read(vec); // lsm6ds3trc_read(vec);
+    furi_hal_i2c_release(&furi_hal_i2c_handle_external);
+    return ret;
+}

Applications/Official/DEV_FW/source/airmouse/tracking/imu/imu.h

@@ -0,0 +1,18 @@
+#pragma once
+
+#include <stdbool.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define ACC_DATA_READY (1 << 0)
+#define GYR_DATA_READY (1 << 1)
+
+bool imu_begin();
+void imu_end();
+int imu_read(double* vec);
+
+#ifdef __cplusplus
+}
+#endif

Applications/Official/DEV_FW/source/airmouse/tracking/imu/imu_bmi160.c

@@ -0,0 +1,88 @@
+#include "bmi160.h"
+
+#include <furi_hal.h>
+
+#include "imu.h"
+
+#define TAG "BMI160"
+
+#define BMI160_DEV_ADDR (0x69 << 1)
+
+static const double DEG_TO_RAD = 0.017453292519943295769236907684886;
+static const double G = 9.81;
+
+struct bmi160_dev bmi160dev;
+struct bmi160_sensor_data bmi160_accel;
+struct bmi160_sensor_data bmi160_gyro;
+
+int8_t bmi160_write_i2c(uint8_t dev_addr, uint8_t reg_addr, uint8_t* data, uint16_t len) {
+    if(furi_hal_i2c_write_mem(&furi_hal_i2c_handle_external, dev_addr, reg_addr, data, len, 50))
+        return BMI160_OK;
+    return BMI160_E_COM_FAIL;
+}
+
+int8_t bmi160_read_i2c(uint8_t dev_addr, uint8_t reg_addr, uint8_t* read_data, uint16_t len) {
+    if(furi_hal_i2c_read_mem(&furi_hal_i2c_handle_external, dev_addr, reg_addr, read_data, len, 50))
+        return BMI160_OK;
+    return BMI160_E_COM_FAIL;
+}
+
+bool bmi160_begin() {
+    FURI_LOG_I(TAG, "Init BMI160");
+
+    if(!furi_hal_i2c_is_device_ready(&furi_hal_i2c_handle_external, BMI160_DEV_ADDR, 50)) {
+        FURI_LOG_E(TAG, "Device not ready!");
+        return false;
+    }
+
+    FURI_LOG_I(TAG, "Device ready!");
+
+    bmi160dev.id = BMI160_DEV_ADDR;
+    bmi160dev.intf = BMI160_I2C_INTF;
+    bmi160dev.read = bmi160_read_i2c;
+    bmi160dev.write = bmi160_write_i2c;
+    bmi160dev.delay_ms = furi_delay_ms;
+
+    if(bmi160_init(&bmi160dev) != BMI160_OK) {
+        FURI_LOG_E(TAG, "Initialization failure!");
+        FURI_LOG_E(TAG, "Chip ID 0x%X", bmi160dev.chip_id);
+        return false;
+    }
+
+    bmi160dev.accel_cfg.odr = BMI160_ACCEL_ODR_400HZ;
+    bmi160dev.accel_cfg.range = BMI160_ACCEL_RANGE_4G;
+    bmi160dev.accel_cfg.bw = BMI160_ACCEL_BW_NORMAL_AVG4;
+    bmi160dev.accel_cfg.power = BMI160_ACCEL_NORMAL_MODE;
+    bmi160dev.gyro_cfg.odr = BMI160_GYRO_ODR_400HZ;
+    bmi160dev.gyro_cfg.range = BMI160_GYRO_RANGE_2000_DPS;
+    bmi160dev.gyro_cfg.bw = BMI160_GYRO_BW_NORMAL_MODE;
+    bmi160dev.gyro_cfg.power = BMI160_GYRO_NORMAL_MODE;
+
+    if(bmi160_set_sens_conf(&bmi160dev) != BMI160_OK) {
+        FURI_LOG_E(TAG, "Initialization failure!");
+        FURI_LOG_E(TAG, "Chip ID 0x%X", bmi160dev.chip_id);
+        return false;
+    }
+
+    FURI_LOG_I(TAG, "Initialization success!");
+    FURI_LOG_I(TAG, "Chip ID 0x%X", bmi160dev.chip_id);
+
+    return true;
+}
+
+int bmi160_read(double* vec) {
+    if(bmi160_get_sensor_data(
+           (BMI160_ACCEL_SEL | BMI160_GYRO_SEL), &bmi160_accel, &bmi160_gyro, &bmi160dev) !=
+       BMI160_OK) {
+        return 0;
+    }
+
+    vec[0] = ((double)bmi160_accel.x * 4 / 32768) * G;
+    vec[1] = ((double)bmi160_accel.y * 4 / 32768) * G;
+    vec[2] = ((double)bmi160_accel.z * 4 / 32768) * G;
+    vec[3] = ((double)bmi160_gyro.x * 2000 / 32768) * DEG_TO_RAD;
+    vec[4] = ((double)bmi160_gyro.y * 2000 / 32768) * DEG_TO_RAD;
+    vec[5] = ((double)bmi160_gyro.z * 2000 / 32768) * DEG_TO_RAD;
+
+    return ACC_DATA_READY | GYR_DATA_READY;
+}

Applications/Official/DEV_FW/source/airmouse/tracking/imu/imu_lsm6ds3trc.c

@@ -0,0 +1,94 @@
+#include "lsm6ds3tr_c_reg.h"
+
+#include <furi_hal.h>
+
+#include "imu.h"
+
+#define TAG "LSM6DS3TR-C"
+
+#define LSM6DS3_ADDRESS (0x6A << 1)
+
+static const double DEG_TO_RAD = 0.017453292519943295769236907684886;
+
+stmdev_ctx_t lsm6ds3trc_ctx;
+
+int32_t lsm6ds3trc_write_i2c(void* handle, uint8_t reg_addr, const uint8_t* data, uint16_t len) {
+    if(furi_hal_i2c_write_mem(handle, LSM6DS3_ADDRESS, reg_addr, (uint8_t*)data, len, 50))
+        return 0;
+    return -1;
+}
+
+int32_t lsm6ds3trc_read_i2c(void* handle, uint8_t reg_addr, uint8_t* read_data, uint16_t len) {
+    if(furi_hal_i2c_read_mem(handle, LSM6DS3_ADDRESS, reg_addr, read_data, len, 50)) return 0;
+    return -1;
+}
+
+bool lsm6ds3trc_begin() {
+    FURI_LOG_I(TAG, "Init LSM6DS3TR-C");
+
+    if(!furi_hal_i2c_is_device_ready(&furi_hal_i2c_handle_external, LSM6DS3_ADDRESS, 50)) {
+        FURI_LOG_E(TAG, "Not ready");
+        return false;
+    }
+
+    lsm6ds3trc_ctx.write_reg = lsm6ds3trc_write_i2c;
+    lsm6ds3trc_ctx.read_reg = lsm6ds3trc_read_i2c;
+    lsm6ds3trc_ctx.mdelay = furi_delay_ms;
+    lsm6ds3trc_ctx.handle = &furi_hal_i2c_handle_external;
+
+    uint8_t whoami;
+    lsm6ds3tr_c_device_id_get(&lsm6ds3trc_ctx, &whoami);
+    if(whoami != LSM6DS3TR_C_ID) {
+        FURI_LOG_I(TAG, "Unknown model: %x", (int)whoami);
+        return false;
+    }
+
+    lsm6ds3tr_c_reset_set(&lsm6ds3trc_ctx, PROPERTY_ENABLE);
+    uint8_t rst = PROPERTY_ENABLE;
+    while(rst) lsm6ds3tr_c_reset_get(&lsm6ds3trc_ctx, &rst);
+
+    lsm6ds3tr_c_block_data_update_set(&lsm6ds3trc_ctx, PROPERTY_ENABLE);
+    lsm6ds3tr_c_fifo_mode_set(&lsm6ds3trc_ctx, LSM6DS3TR_C_BYPASS_MODE);
+
+    lsm6ds3tr_c_xl_data_rate_set(&lsm6ds3trc_ctx, LSM6DS3TR_C_XL_ODR_104Hz);
+    lsm6ds3tr_c_xl_full_scale_set(&lsm6ds3trc_ctx, LSM6DS3TR_C_4g);
+    lsm6ds3tr_c_xl_lp1_bandwidth_set(&lsm6ds3trc_ctx, LSM6DS3TR_C_XL_LP1_ODR_DIV_4);
+
+    lsm6ds3tr_c_gy_data_rate_set(&lsm6ds3trc_ctx, LSM6DS3TR_C_GY_ODR_104Hz);
+    lsm6ds3tr_c_gy_full_scale_set(&lsm6ds3trc_ctx, LSM6DS3TR_C_2000dps);
+    lsm6ds3tr_c_gy_power_mode_set(&lsm6ds3trc_ctx, LSM6DS3TR_C_GY_HIGH_PERFORMANCE);
+    lsm6ds3tr_c_gy_band_pass_set(&lsm6ds3trc_ctx, LSM6DS3TR_C_LP2_ONLY);
+
+    FURI_LOG_I(TAG, "Init OK");
+    return true;
+}
+
+void lsm6ds3trc_end() {
+    lsm6ds3tr_c_xl_data_rate_set(&lsm6ds3trc_ctx, LSM6DS3TR_C_XL_ODR_OFF);
+    lsm6ds3tr_c_gy_data_rate_set(&lsm6ds3trc_ctx, LSM6DS3TR_C_GY_ODR_OFF);
+}
+
+int lsm6ds3trc_read(double* vec) {
+    int ret = 0;
+    int16_t data[3];
+    lsm6ds3tr_c_reg_t reg;
+    lsm6ds3tr_c_status_reg_get(&lsm6ds3trc_ctx, &reg.status_reg);
+
+    if(reg.status_reg.xlda) {
+        lsm6ds3tr_c_acceleration_raw_get(&lsm6ds3trc_ctx, data);
+        vec[2] = (double)lsm6ds3tr_c_from_fs2g_to_mg(data[0]) / 1000;
+        vec[0] = (double)lsm6ds3tr_c_from_fs2g_to_mg(data[1]) / 1000;
+        vec[1] = (double)lsm6ds3tr_c_from_fs2g_to_mg(data[2]) / 1000;
+        ret |= ACC_DATA_READY;
+    }
+
+    if(reg.status_reg.gda) {
+        lsm6ds3tr_c_angular_rate_raw_get(&lsm6ds3trc_ctx, data);
+        vec[5] = (double)lsm6ds3tr_c_from_fs2000dps_to_mdps(data[0]) * DEG_TO_RAD / 1000;
+        vec[3] = (double)lsm6ds3tr_c_from_fs2000dps_to_mdps(data[1]) * DEG_TO_RAD / 1000;
+        vec[4] = (double)lsm6ds3tr_c_from_fs2000dps_to_mdps(data[2]) * DEG_TO_RAD / 1000;
+        ret |= GYR_DATA_READY;
+    }
+
+    return ret;
+}

Applications/Official/DEV_FW/source/airmouse/tracking/main_loop.cc

@@ -0,0 +1,189 @@
+#include "main_loop.h"
+
+#include <furi.h>
+#include <furi_hal.h>
+
+#include "imu/imu.h"
+#include "orientation_tracker.h"
+#include "calibration_data.h"
+
+#define TAG "tracker"
+
+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;
+    }
+    return val;
+}
+
+static inline 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;
+}
+
+void sendCurrentState(MouseMoveCallback mouse_move, void *context)
+{
+    float dX = g_dYaw * CURSOR_SPEED;
+    float dY = g_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);
+
+    // Only subtract the part of the error that was already sent.
+    if (x != 0) {
+        g_dYaw -= x / CURSOR_SPEED;
+    }
+    if (y != 0) {
+        g_dPitch -= y / CURSOR_SPEED;
+    }
+}
+
+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) {
+        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;
+    }
+}
+
+extern "C" {
+
+void calibration_begin() {
+    calibration.reset();
+    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;
+}
+
+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);
+}
+
+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();
+}
+
+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);
+        }
+    }
+}
+
+void tracking_end() {
+    tracker.Pause();
+}
+
+}

Applications/Official/DEV_FW/source/airmouse/tracking/main_loop.h

@@ -0,0 +1,21 @@
+#pragma once
+
+#include <stdint.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef bool (*MouseMoveCallback)(int8_t x, int8_t y, void* context);
+
+void calibration_begin();
+bool calibration_step();
+void calibration_end();
+
+void tracking_begin();
+void tracking_step(MouseMoveCallback mouse_move, void* context);
+void tracking_end();
+
+#ifdef __cplusplus
+}
+#endif

Applications/Official/DEV_FW/source/airmouse/tracking/orientation_tracker.cc

@@ -0,0 +1,95 @@
+/*
+ * Copyright 2019 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#include "orientation_tracker.h"
+
+#include "sensors/pose_prediction.h"
+#include "util/logging.h"
+#include "util/vector.h"
+#include "util/vectorutils.h"
+
+namespace cardboard {
+
+OrientationTracker::OrientationTracker(const long sampling_period_ns)
+    : sampling_period_ns_(sampling_period_ns)
+    , calibration_(Vector3::Zero())
+    , is_tracking_(false)
+    , sensor_fusion_(new SensorFusionEkf())
+    , latest_gyroscope_data_({ 0, 0, Vector3::Zero() })
+{
+    sensor_fusion_->SetBiasEstimationEnabled(/*kGyroBiasEstimationEnabled*/ true);
+}
+
+void OrientationTracker::SetCalibration(const Vector3& calibration) {
+    calibration_ = calibration;
+}
+
+void OrientationTracker::Pause()
+{
+    if (!is_tracking_) {
+        return;
+    }
+
+    // Create a gyro event with zero velocity. This effectively stops the prediction.
+    GyroscopeData event = latest_gyroscope_data_;
+    event.data = Vector3::Zero();
+
+    OnGyroscopeData(event);
+
+    is_tracking_ = false;
+}
+
+void OrientationTracker::Resume() { is_tracking_ = true; }
+
+Vector4 OrientationTracker::GetPose(int64_t timestamp_ns) const
+{
+    Rotation predicted_rotation;
+    const PoseState pose_state = sensor_fusion_->GetLatestPoseState();
+    if (sensor_fusion_->IsFullyInitialized()) {
+        predicted_rotation = pose_state.sensor_from_start_rotation;
+    } else {
+        CARDBOARD_LOGI("Tracker not fully initialized yet. Using pose prediction only.");
+        predicted_rotation = pose_prediction::PredictPose(timestamp_ns, pose_state);
+    }
+
+    return (-predicted_rotation).GetQuaternion();
+}
+
+void OrientationTracker::OnAccelerometerData(const AccelerometerData& event)
+{
+    if (!is_tracking_) {
+        return;
+    }
+    sensor_fusion_->ProcessAccelerometerSample(event);
+}
+
+Vector4 OrientationTracker::OnGyroscopeData(const GyroscopeData& event)
+{
+    if (!is_tracking_) {
+        return Vector4();
+    }
+
+    const GyroscopeData data = { .system_timestamp = event.system_timestamp,
+        .sensor_timestamp_ns = event.sensor_timestamp_ns,
+        .data = event.data - calibration_ };
+
+    latest_gyroscope_data_ = data;
+
+    sensor_fusion_->ProcessGyroscopeSample(data);
+
+    return OrientationTracker::GetPose(data.sensor_timestamp_ns + sampling_period_ns_);
+}
+
+} // namespace cardboard

Applications/Official/DEV_FW/source/airmouse/tracking/orientation_tracker.h

@@ -0,0 +1,68 @@
+/*
+ * Copyright 2019 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#pragma once
+
+#include <array>
+#include <memory>
+#include <mutex> // NOLINT
+
+#include "sensors/accelerometer_data.h"
+#include "sensors/gyroscope_data.h"
+#include "sensors/sensor_fusion_ekf.h"
+#include "util/rotation.h"
+
+namespace cardboard {
+
+// OrientationTracker encapsulates pose tracking by connecting sensors
+// to SensorFusion.
+// This pose tracker reports poses in display space.
+class OrientationTracker {
+public:
+    OrientationTracker(const long sampling_period_ns);
+
+    void SetCalibration(const Vector3& calibration);
+
+    // Pauses tracking and sensors.
+    void Pause();
+
+    // Resumes tracking ans sensors.
+    void Resume();
+
+    // Gets the predicted pose for a given timestamp.
+    Vector4 GetPose(int64_t timestamp_ns) const;
+
+    // Function called when receiving AccelerometerData.
+    //
+    // @param event sensor event.
+    void OnAccelerometerData(const AccelerometerData& event);
+
+    // Function called when receiving GyroscopeData.
+    //
+    // @param event sensor event.
+    Vector4 OnGyroscopeData(const GyroscopeData& event);
+
+private:
+    long sampling_period_ns_;
+    Vector3 calibration_;
+
+    std::atomic<bool> is_tracking_;
+    // Sensor Fusion object that stores the internal state of the filter.
+    std::unique_ptr<SensorFusionEkf> sensor_fusion_;
+    // Latest gyroscope data.
+    GyroscopeData latest_gyroscope_data_;
+};
+
+} // namespace cardboard

Applications/Official/DEV_FW/source/airmouse/tracking/sensors/accelerometer_data.h

@@ -0,0 +1,38 @@
+/*
+ * Copyright 2019 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#ifndef CARDBOARD_SDK_SENSORS_ACCELEROMETER_DATA_H_
+#define CARDBOARD_SDK_SENSORS_ACCELEROMETER_DATA_H_
+
+#include "../util/vector.h"
+
+namespace cardboard {
+
+struct AccelerometerData {
+    // System wall time.
+    uint64_t system_timestamp;
+
+    // Sensor clock time in nanoseconds.
+    uint64_t sensor_timestamp_ns;
+
+    // Acceleration force along the x,y,z axes in m/s^2. This follows android
+    // specification
+    // (https://developer.android.com/guide/topics/sensors/sensors_overview.html#sensors-coords).
+    Vector3 data;
+};
+
+} // namespace cardboard
+
+#endif // CARDBOARD_SDK_SENSORS_ACCELEROMETER_DATA_H_

Applications/Official/DEV_FW/source/airmouse/tracking/sensors/gyroscope_bias_estimator.cc

@@ -0,0 +1,313 @@
+/*
+ * Copyright 2019 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#include "gyroscope_bias_estimator.h"
+
+#include <algorithm>
+#include <chrono> // NOLINT
+
+#include "../util/rotation.h"
+#include "../util/vector.h"
+
+namespace {
+
+// Cutoff frequencies in Hertz applied to our various signals, and their
+// corresponding filters.
+const float kAccelerometerLowPassCutOffFrequencyHz = 1.0f;
+const float kRotationVelocityBasedAccelerometerLowPassCutOffFrequencyHz = 0.15f;
+const float kGyroscopeLowPassCutOffFrequencyHz = 1.0f;
+const float kGyroscopeBiasLowPassCutOffFrequencyHz = 0.15f;
+
+// Note that MEMS IMU are not that precise.
+const double kEpsilon = 1.0e-8;
+
+// Size of the filtering window for the mean and median filter. The larger the
+// windows the larger the filter delay.
+const int kFilterWindowSize = 5;
+
+// Threshold used to compare rotation computed from the accelerometer and the
+// gyroscope bias.
+const double kRatioBetweenGyroBiasAndAccel = 1.5;
+
+// The minimum sum of weights we need to acquire before returning a bias
+// estimation.
+const float kMinSumOfWeightsGyroBiasThreshold = 25.0f;
+
+// Amount of change in m/s^3 we allow on the smoothed accelerometer values to
+// consider the phone static.
+const double kAccelerometerDeltaStaticThreshold = 0.5;
+
+// Amount of change in radians/s^2 we allow on the smoothed gyroscope values to
+// consider the phone static.
+const double kGyroscopeDeltaStaticThreshold = 0.03;
+
+// If the gyroscope value is above this threshold, don't update the gyroscope
+// bias estimation. This threshold is applied to the magnitude of gyroscope
+// vectors in radians/s.
+const float kGyroscopeForBiasThreshold = 0.30f;
+
+// Used to monitor if accelerometer and gyroscope have been static for a few
+// frames.
+const int kStaticFrameDetectionThreshold = 50;
+
+// Minimum time step between sensor updates.
+const double kMinTimestep = 1; // std::chrono::nanoseconds(1);
+} // namespace
+
+namespace cardboard {
+
+// A helper class to keep track of whether some signal can be considered static
+// over specified number of frames.
+class GyroscopeBiasEstimator::IsStaticCounter {
+public:
+    // Initializes a counter with the number of consecutive frames we require
+    // the signal to be static before IsRecentlyStatic returns true.
+    //
+    // @param min_static_frames_threshold number of consecutive frames we
+    //     require the signal to be static before IsRecentlyStatic returns true.
+    explicit IsStaticCounter(int min_static_frames_threshold)
+        : min_static_frames_threshold_(min_static_frames_threshold)
+        , consecutive_static_frames_(0)
+    {
+    }
+
+    // Specifies whether the current frame is considered static.
+    //
+    // @param is_static static flag for current frame.
+    void AppendFrame(bool is_static)
+    {
+        if (is_static) {
+            ++consecutive_static_frames_;
+        } else {
+            consecutive_static_frames_ = 0;
+        }
+    }
+
+    // Returns if static movement is assumed.
+    bool IsRecentlyStatic() const
+    {
+        return consecutive_static_frames_ >= min_static_frames_threshold_;
+    }
+    // Resets counter.
+    void Reset() { consecutive_static_frames_ = 0; }
+
+private:
+    const int min_static_frames_threshold_;
+    int consecutive_static_frames_;
+};
+
+GyroscopeBiasEstimator::GyroscopeBiasEstimator()
+    : accelerometer_lowpass_filter_(kAccelerometerLowPassCutOffFrequencyHz)
+    , simulated_gyroscope_from_accelerometer_lowpass_filter_(
+          kRotationVelocityBasedAccelerometerLowPassCutOffFrequencyHz)
+    , gyroscope_lowpass_filter_(kGyroscopeLowPassCutOffFrequencyHz)
+    , gyroscope_bias_lowpass_filter_(kGyroscopeBiasLowPassCutOffFrequencyHz)
+    , accelerometer_static_counter_(new IsStaticCounter(kStaticFrameDetectionThreshold))
+    , gyroscope_static_counter_(new IsStaticCounter(kStaticFrameDetectionThreshold))
+    , current_accumulated_weights_gyroscope_bias_(0.f)
+    , mean_filter_(kFilterWindowSize)
+    , median_filter_(kFilterWindowSize)
+    , last_mean_filtered_accelerometer_value_({ 0, 0, 0 })
+{
+    Reset();
+}
+
+GyroscopeBiasEstimator::~GyroscopeBiasEstimator() { }
+
+void GyroscopeBiasEstimator::Reset()
+{
+    accelerometer_lowpass_filter_.Reset();
+    gyroscope_lowpass_filter_.Reset();
+    gyroscope_bias_lowpass_filter_.Reset();
+    accelerometer_static_counter_->Reset();
+    gyroscope_static_counter_->Reset();
+}
+
+void GyroscopeBiasEstimator::ProcessGyroscope(
+    const Vector3& gyroscope_sample, uint64_t timestamp_ns)
+{
+    // Update gyroscope and gyroscope delta low-pass filters.
+    gyroscope_lowpass_filter_.AddSample(gyroscope_sample, timestamp_ns);
+
+    const auto smoothed_gyroscope_delta
+        = gyroscope_sample - gyroscope_lowpass_filter_.GetFilteredData();
+
+    gyroscope_static_counter_->AppendFrame(
+        Length(smoothed_gyroscope_delta) < kGyroscopeDeltaStaticThreshold);
+
+    // Only update the bias if the gyroscope and accelerometer signals have been
+    // relatively static recently.
+    if (gyroscope_static_counter_->IsRecentlyStatic()
+        && accelerometer_static_counter_->IsRecentlyStatic()) {
+        // Reset static counter when updating the bias fails.
+        if (!UpdateGyroscopeBias(gyroscope_sample, timestamp_ns)) {
+            // Bias update fails because of large motion, thus reset the static
+            // counter.
+            gyroscope_static_counter_->AppendFrame(false);
+        }
+    } else {
+        // Reset weights, if not static.
+        current_accumulated_weights_gyroscope_bias_ = 0;
+    }
+}
+
+void GyroscopeBiasEstimator::ProcessAccelerometer(
+    const Vector3& accelerometer_sample, uint64_t timestamp_ns)
+{
+    // Get current state of the filter.
+    const uint64_t previous_accel_timestamp_ns
+        = accelerometer_lowpass_filter_.GetMostRecentTimestampNs();
+    const bool is_low_pass_filter_init = accelerometer_lowpass_filter_.IsInitialized();
+
+    // Update accel and accel delta low-pass filters.
+    accelerometer_lowpass_filter_.AddSample(accelerometer_sample, timestamp_ns);
+
+    const auto smoothed_accelerometer_delta
+        = accelerometer_sample - accelerometer_lowpass_filter_.GetFilteredData();
+
+    accelerometer_static_counter_->AppendFrame(
+        Length(smoothed_accelerometer_delta) < kAccelerometerDeltaStaticThreshold);
+
+    // Rotation from accel cannot be differentiated with only one sample.
+    if (!is_low_pass_filter_init) {
+        simulated_gyroscope_from_accelerometer_lowpass_filter_.AddSample({ 0, 0, 0 }, timestamp_ns);
+        return;
+    }
+
+    // No need to update the simulated gyroscope at this point because the motion
+    // is too large.
+    if (!accelerometer_static_counter_->IsRecentlyStatic()) {
+        return;
+    }
+
+    median_filter_.AddSample(accelerometer_lowpass_filter_.GetFilteredData());
+
+    // This processing can only be started if the buffer is fully initialized.
+    if (!median_filter_.IsValid()) {
+        mean_filter_.AddSample(accelerometer_lowpass_filter_.GetFilteredData());
+
+        // Update the last filtered accelerometer value.
+        last_mean_filtered_accelerometer_value_ = accelerometer_lowpass_filter_.GetFilteredData();
+        return;
+    }
+
+    mean_filter_.AddSample(median_filter_.GetFilteredData());
+
+    // Compute a mock gyroscope value from accelerometer.
+    const int64_t diff = timestamp_ns - previous_accel_timestamp_ns;
+    const double timestep = static_cast<double>(diff);
+
+    simulated_gyroscope_from_accelerometer_lowpass_filter_.AddSample(
+        ComputeAngularVelocityFromLatestAccelerometer(timestep), timestamp_ns);
+    last_mean_filtered_accelerometer_value_ = mean_filter_.GetFilteredData();
+}
+
+Vector3 GyroscopeBiasEstimator::ComputeAngularVelocityFromLatestAccelerometer(double timestep) const
+{
+    if (timestep < kMinTimestep) {
+        return { 0, 0, 0 };
+    }
+
+    const auto mean_of_median = mean_filter_.GetFilteredData();
+
+    // Compute an incremental rotation between the last state and the current
+    // state.
+    //
+    // Note that we switch to double precision here because of precision problem
+    // with small rotation.
+    const auto incremental_rotation = Rotation::RotateInto(
+        Vector3(last_mean_filtered_accelerometer_value_[0],
+            last_mean_filtered_accelerometer_value_[1], last_mean_filtered_accelerometer_value_[2]),
+        Vector3(mean_of_median[0], mean_of_median[1], mean_of_median[2]));
+
+    // We use axis angle here because this is how gyroscope values are stored.
+    Vector3 incremental_rotation_axis;
+    double incremental_rotation_angle;
+    incremental_rotation.GetAxisAndAngle(&incremental_rotation_axis, &incremental_rotation_angle);
+
+    incremental_rotation_axis *= incremental_rotation_angle / timestep;
+
+    return { static_cast<float>(incremental_rotation_axis[0]),
+        static_cast<float>(incremental_rotation_axis[1]),
+        static_cast<float>(incremental_rotation_axis[2]) };
+}
+
+bool GyroscopeBiasEstimator::UpdateGyroscopeBias(
+    const Vector3& gyroscope_sample, uint64_t timestamp_ns)
+{
+    // Gyroscope values that are too big are potentially dangerous as they could
+    // originate from slow and steady head rotations.
+    //
+    // Therefore we compute an update weight which:
+    // * favors gyroscope values that are closer to 0
+    // * is set to zero if gyroscope values are greater than a threshold.
+    //
+    // This way, the gyroscope bias estimation converges faster if the phone is
+    // flat on a table, as opposed to held up somewhat stationary in the user's
+    // hands.
+
+    // If magnitude is too big, don't update the filter at all so that we don't
+    // artificially increase the number of samples accumulated by the filter.
+    const float gyroscope_sample_norm2 = Length(gyroscope_sample);
+    if (gyroscope_sample_norm2 >= kGyroscopeForBiasThreshold) {
+        return false;
+    }
+
+    float update_weight
+        = std::max(0.0f, 1 - gyroscope_sample_norm2 / kGyroscopeForBiasThreshold);
+    update_weight *= update_weight;
+    gyroscope_bias_lowpass_filter_.AddWeightedSample(
+        gyroscope_lowpass_filter_.GetFilteredData(), timestamp_ns, update_weight);
+
+    // This counter is only partially valid as the low pass filter drops large
+    // samples.
+    current_accumulated_weights_gyroscope_bias_ += update_weight;
+
+    return true;
+}
+
+Vector3 GyroscopeBiasEstimator::GetGyroscopeBias() const
+{
+    return gyroscope_bias_lowpass_filter_.GetFilteredData();
+}
+
+bool GyroscopeBiasEstimator::IsCurrentEstimateValid() const
+{
+    // Remove any bias component along the gravity because they cannot be
+    // evaluated from accelerometer.
+    const auto current_gravity_dir = Normalized(last_mean_filtered_accelerometer_value_);
+    const auto gyro_bias_lowpass = gyroscope_bias_lowpass_filter_.GetFilteredData();
+
+    const auto off_gravity_gyro_bias
+        = gyro_bias_lowpass - current_gravity_dir * Dot(gyro_bias_lowpass, current_gravity_dir);
+
+    // Checks that the current bias estimate is not correlated with the
+    // rotation computed from accelerometer.
+    const auto gyro_from_accel
+        = simulated_gyroscope_from_accelerometer_lowpass_filter_.GetFilteredData();
+    const bool isGyroscopeBiasCorrelatedWithSimulatedGyro
+        = (Length(gyro_from_accel) * kRatioBetweenGyroBiasAndAccel
+            > (Length(off_gravity_gyro_bias) + kEpsilon));
+    const bool hasEnoughSamples
+        = current_accumulated_weights_gyroscope_bias_ > kMinSumOfWeightsGyroBiasThreshold;
+    const bool areCountersStatic = gyroscope_static_counter_->IsRecentlyStatic()
+        && accelerometer_static_counter_->IsRecentlyStatic();
+
+    const bool isStatic
+        = hasEnoughSamples && areCountersStatic && !isGyroscopeBiasCorrelatedWithSimulatedGyro;
+    return isStatic;
+}
+
+} // namespace cardboard

Applications/Official/DEV_FW/source/airmouse/tracking/sensors/gyroscope_bias_estimator.h

@@ -0,0 +1,134 @@
+/*
+ * Copyright 2019 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#ifndef CARDBOARD_SDK_SENSORS_GYROSCOPE_BIAS_ESTIMATOR_H_
+#define CARDBOARD_SDK_SENSORS_GYROSCOPE_BIAS_ESTIMATOR_H_
+
+#include <chrono> // NOLINT
+#include <cstdint>
+#include <list>
+#include <memory>
+#include <vector>
+
+#include "lowpass_filter.h"
+#include "mean_filter.h"
+#include "median_filter.h"
+#include "../util/vector.h"
+
+namespace cardboard {
+
+// Class that attempts to estimate the gyroscope's bias.
+// Its main idea is that it averages the gyroscope values when the phone is
+// considered stationary.
+// Usage: A client should call the ProcessGyroscope and ProcessAccelerometer
+// methods for every accelerometer and gyroscope sensor sample. This class
+// expects these calls to be frequent, i.e., at least at 10 Hz. The client can
+// then call GetGyroBias to retrieve the current estimate of the gyroscope bias.
+// For best results, the fastest available delay option should be used when
+// registering to sensors. Note that this class is not thread-safe.
+//
+// The filtering applied to the accelerometer to estimate a rotation
+// from it follows :
+// Baptiste Delporte, Laurent Perroton, Thierry Grandpierre, Jacques Trichet.
+// Accelerometer and Magnetometer Based Gyroscope Emulation on Smart Sensor
+// for a Virtual Reality Application. Sensor and Transducers Journal, 2012.
+//
+// which is a combination of a IIR filter, a median and a mean filter.
+class GyroscopeBiasEstimator {
+public:
+    GyroscopeBiasEstimator();
+    virtual ~GyroscopeBiasEstimator();
+
+    // Updates the estimator with a gyroscope event.
+    //
+    // @param gyroscope_sample the angular speed around the x, y, z axis in
+    //     radians/sec.
+    // @param timestamp_ns the nanosecond at which the event occurred. Only
+    //     guaranteed to be comparable with timestamps from other PocessGyroscope
+    //     invocations.
+    virtual void ProcessGyroscope(const Vector3& gyroscope_sample, uint64_t timestamp_ns);
+
+    // Processes accelerometer samples to estimate if device is
+    // stable or not.
+    //
+    // First we filter the accelerometer. This is done with 3 filters.
+    //  - A IIR low-pass filter
+    //  - A median filter
+    //  - A mean filter.
+    // Then a rotation is computed between consecutive filtered accelerometer
+    // samples.
+    // Finally this is converted to a velocity to emulate a gyroscope.
+    //
+    // @param accelerometer_sample the acceleration (including gravity) on the x,
+    //     y, z axis in meters/s^2.
+    // @param timestamp_ns the nanosecond at which the event occurred. Only
+    //     guaranteed to be comparable with timestamps from other
+    //     ProcessAccelerometer invocations.
+    virtual void ProcessAccelerometer(const Vector3& accelerometer_sample, uint64_t timestamp_ns);
+
+    // Returns the estimated gyroscope bias.
+    //
+    // @return Estimated gyroscope bias. A vector with zeros is returned if no
+    //     estimate has been computed.
+    virtual Vector3 GetGyroscopeBias() const;
+
+    // Resets the estimator state.
+    void Reset();
+
+    // Returns true if the current estimate returned by GetGyroscopeBias is
+    // correct. The device (measured using the sensors) has to be static for this
+    // function to return true.
+    virtual bool IsCurrentEstimateValid() const;
+
+private:
+    // A helper class to keep track of whether some signal can be considered
+    // static over specified number of frames.
+    class IsStaticCounter;
+
+    // Updates gyroscope bias estimation.
+    //
+    // @return false if the current sample is too large.
+    bool UpdateGyroscopeBias(const Vector3& gyroscope_sample, uint64_t timestamp_ns);
+
+    // Returns device angular velocity (rad/s) from the latest accelerometer data.
+    //
+    // @param timestep in seconds between the last two samples.
+    // @return rotation velocity from latest accelerometer. This can be
+    // interpreted as an gyroscope.
+    Vector3 ComputeAngularVelocityFromLatestAccelerometer(double timestep) const;
+
+    LowpassFilter accelerometer_lowpass_filter_;
+    LowpassFilter simulated_gyroscope_from_accelerometer_lowpass_filter_;
+    LowpassFilter gyroscope_lowpass_filter_;
+    LowpassFilter gyroscope_bias_lowpass_filter_;
+
+    std::unique_ptr<IsStaticCounter> accelerometer_static_counter_;
+    std::unique_ptr<IsStaticCounter> gyroscope_static_counter_;
+
+    // Sum of the weight of sample used for gyroscope filtering.
+    float current_accumulated_weights_gyroscope_bias_;
+
+    // Set of filters for accelerometer data to estimate a rotation
+    // based only on accelerometer.
+    MeanFilter mean_filter_;
+    MedianFilter median_filter_;
+
+    // Last computed filter accelerometer value used for finite differences.
+    Vector3 last_mean_filtered_accelerometer_value_;
+};
+
+} // namespace cardboard
+
+#endif // CARDBOARD_SDK_SENSORS_GYROSCOPE_BIAS_ESTIMATOR_H_

Applications/Official/DEV_FW/source/airmouse/tracking/sensors/gyroscope_data.h

@@ -0,0 +1,38 @@
+/*
+ * Copyright 2019 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#ifndef CARDBOARD_SDK_SENSORS_GYROSCOPE_DATA_H_
+#define CARDBOARD_SDK_SENSORS_GYROSCOPE_DATA_H_
+
+#include "../util/vector.h"
+
+namespace cardboard {
+
+struct GyroscopeData {
+    // System wall time.
+    uint64_t system_timestamp;
+
+    // Sensor clock time in nanoseconds.
+    uint64_t sensor_timestamp_ns;
+
+    // Rate of rotation around the x,y,z axes in rad/s. This follows android
+    // specification
+    // (https://developer.android.com/guide/topics/sensors/sensors_overview.html#sensors-coords).
+    Vector3 data;
+};
+
+} // namespace cardboard
+
+#endif // CARDBOARD_SDK_SENSORS_GYROSCOPE_DATA_H_

Applications/Official/DEV_FW/source/airmouse/tracking/sensors/lowpass_filter.cc

@@ -0,0 +1,84 @@
+/*
+ * Copyright 2019 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#include "lowpass_filter.h"
+
+#include <cmath>
+
+namespace {
+
+const double kSecondsFromNanoseconds = 1.0e-9;
+
+// Minimum time step between sensor updates. This corresponds to 1000 Hz.
+const double kMinTimestepS = 0.001f;
+
+// Maximum time step between sensor updates. This corresponds to 1 Hz.
+const double kMaxTimestepS = 1.00f;
+
+} // namespace
+
+namespace cardboard {
+
+LowpassFilter::LowpassFilter(double cutoff_freq_hz)
+    : cutoff_time_constant_(1 / (2 * (double)M_PI * cutoff_freq_hz))
+    , initialized_(false)
+{
+    Reset();
+}
+
+void LowpassFilter::AddSample(const Vector3& sample, uint64_t timestamp_ns)
+{
+    AddWeightedSample(sample, timestamp_ns, 1.0);
+}
+
+void LowpassFilter::AddWeightedSample(const Vector3& sample, uint64_t timestamp_ns, double weight)
+{
+    if (!initialized_) {
+        // Initialize filter state
+        filtered_data_ = { sample[0], sample[1], sample[2] };
+        timestamp_most_recent_update_ns_ = timestamp_ns;
+        initialized_ = true;
+        return;
+    }
+
+    if (timestamp_ns < timestamp_most_recent_update_ns_) {
+        timestamp_most_recent_update_ns_ = timestamp_ns;
+        return;
+    }
+
+    const double delta_s = static_cast<double>(timestamp_ns - timestamp_most_recent_update_ns_)
+        * kSecondsFromNanoseconds;
+    if (delta_s <= kMinTimestepS || delta_s > kMaxTimestepS) {
+        timestamp_most_recent_update_ns_ = timestamp_ns;
+        return;
+    }
+
+    const double weighted_delta_secs = weight * delta_s;
+
+    const double alpha = weighted_delta_secs / (cutoff_time_constant_ + weighted_delta_secs);
+
+    for (int i = 0; i < 3; ++i) {
+        filtered_data_[i] = (1 - alpha) * filtered_data_[i] + alpha * sample[i];
+    }
+    timestamp_most_recent_update_ns_ = timestamp_ns;
+}
+
+void LowpassFilter::Reset()
+{
+    initialized_ = false;
+    filtered_data_ = { 0, 0, 0 };
+}
+
+} // namespace cardboard

Applications/Official/DEV_FW/source/airmouse/tracking/sensors/lowpass_filter.h

@@ -0,0 +1,81 @@
+/*
+ * Copyright 2019 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#ifndef CARDBOARD_SDK_SENSORS_LOWPASS_FILTER_H_
+#define CARDBOARD_SDK_SENSORS_LOWPASS_FILTER_H_
+
+#include <array>
+#include <memory>
+
+#include "../util/vector.h"
+
+namespace cardboard {
+
+// Implements an IIR, first order, low pass filter over vectors of the given
+// dimension = 3.
+// See http://en.wikipedia.org/wiki/Low-pass_filter
+class LowpassFilter {
+public:
+    // Initializes a filter with the given cutoff frequency in Hz.
+    explicit LowpassFilter(double cutoff_freq_hz);
+
+    // Updates the filter with the given sample. Note that samples with
+    // non-monotonic timestamps and successive samples with a time steps below 1
+    // ms or above 1 s are ignored.
+    //
+    // @param sample current sample data.
+    // @param timestamp_ns timestamp associated to this sample in nanoseconds.
+    void AddSample(const Vector3& sample, uint64_t timestamp_ns);
+
+    // Updates the filter with the given weighted sample.
+    //
+    // @param sample current sample data.
+    // @param timestamp_ns timestamp associated to this sample in nanoseconds.
+    // @param weight typically a [0, 1] weight factor used when applying a new
+    //     sample. A weight of 1 corresponds to calling AddSample. A weight of 0
+    //     makes the update no-op. The first initial sample is not affected by
+    //     this.
+    void AddWeightedSample(const Vector3& sample, uint64_t timestamp_ns, double weight);
+
+    // Returns the filtered value. A vector with zeros is returned if no samples
+    // have been added.
+    Vector3 GetFilteredData() const {
+        return filtered_data_;
+    }
+
+    // Returns the most recent update timestamp in ns.
+    uint64_t GetMostRecentTimestampNs() const {
+        return timestamp_most_recent_update_ns_;
+    }
+
+    // Returns true when the filter is initialized.
+    bool IsInitialized() const {
+        return initialized_;
+    }
+
+    // Resets filter state.
+    void Reset();
+
+private:
+    const double cutoff_time_constant_;
+    uint64_t timestamp_most_recent_update_ns_;
+    bool initialized_;
+
+    Vector3 filtered_data_;
+};
+
+} // namespace cardboard
+
+#endif // CARDBOARD_SDK_SENSORS_LOWPASS_FILTER_H_

Applications/Official/DEV_FW/source/airmouse/tracking/sensors/mean_filter.cc

@@ -0,0 +1,46 @@
+/*
+ * Copyright 2019 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#include "mean_filter.h"
+
+namespace cardboard {
+
+MeanFilter::MeanFilter(size_t filter_size)
+    : filter_size_(filter_size)
+{
+}
+
+void MeanFilter::AddSample(const Vector3& sample)
+{
+    buffer_.push_back(sample);
+    if (buffer_.size() > filter_size_) {
+        buffer_.pop_front();
+    }
+}
+
+bool MeanFilter::IsValid() const { return buffer_.size() == filter_size_; }
+
+Vector3 MeanFilter::GetFilteredData() const
+{
+    // Compute mean of the samples stored in buffer_.
+    Vector3 mean = Vector3::Zero();
+    for (auto sample : buffer_) {
+        mean += sample;
+    }
+
+    return mean / static_cast<double>(filter_size_);
+}
+
+} // namespace cardboard

Applications/Official/DEV_FW/source/airmouse/tracking/sensors/mean_filter.h

@@ -0,0 +1,48 @@
+/*
+ * Copyright 2019 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#ifndef CARDBOARD_SDK_SENSORS_MEAN_FILTER_H_
+#define CARDBOARD_SDK_SENSORS_MEAN_FILTER_H_
+
+#include <deque>
+
+#include "../util/vector.h"
+
+namespace cardboard {
+
+// Fixed window FIFO mean filter for vectors of the given dimension.
+class MeanFilter {
+public:
+    // Create a mean filter of size filter_size.
+    // @param filter_size size of the internal filter.
+    explicit MeanFilter(size_t filter_size);
+
+    // Add sample to buffer_ if buffer_ is full it drop the oldest sample.
+    void AddSample(const Vector3& sample);
+
+    // Returns true if buffer has filter_size_ sample, false otherwise.
+    bool IsValid() const;
+
+    // Returns the mean of values stored in the internal buffer.
+    Vector3 GetFilteredData() const;
+
+private:
+    const size_t filter_size_;
+    std::deque<Vector3> buffer_;
+};
+
+} // namespace cardboard
+
+#endif // CARDBOARD_SDK_SENSORS_MEAN_FILTER_H_

Applications/Official/DEV_FW/source/airmouse/tracking/sensors/median_filter.cc

@@ -0,0 +1,69 @@
+/*
+ * Copyright 2019 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#include "median_filter.h"
+
+#include <algorithm>
+#include <vector>
+
+#include "../util/vector.h"
+#include "../util/vectorutils.h"
+
+namespace cardboard {
+
+MedianFilter::MedianFilter(size_t filter_size)
+    : filter_size_(filter_size)
+{
+}
+
+void MedianFilter::AddSample(const Vector3& sample)
+{
+    buffer_.push_back(sample);
+    norms_.push_back(Length(sample));
+    if (buffer_.size() > filter_size_) {
+        buffer_.pop_front();
+        norms_.pop_front();
+    }
+}
+
+bool MedianFilter::IsValid() const { return buffer_.size() == filter_size_; }
+
+Vector3 MedianFilter::GetFilteredData() const
+{
+    std::vector<float> norms(norms_.begin(), norms_.end());
+
+    // Get median of value of the norms.
+    std::nth_element(norms.begin(), norms.begin() + filter_size_ / 2, norms.end());
+    const float median_norm = norms[filter_size_ / 2];
+
+    // Get median value based on their norm.
+    auto median_it = buffer_.begin();
+    for (const auto norm : norms_) {
+        if (norm == median_norm) {
+            break;
+        }
+        ++median_it;
+    }
+
+    return *median_it;
+}
+
+void MedianFilter::Reset()
+{
+    buffer_.clear();
+    norms_.clear();
+}
+
+} // namespace cardboard

Applications/Official/DEV_FW/source/airmouse/tracking/sensors/median_filter.h

@@ -0,0 +1,53 @@
+/*
+ * Copyright 2019 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#ifndef CARDBOARD_SDK_SENSORS_MEDIAN_FILTER_H_
+#define CARDBOARD_SDK_SENSORS_MEDIAN_FILTER_H_
+
+#include <deque>
+
+#include "../util/vector.h"
+
+namespace cardboard {
+
+// Fixed window FIFO median filter for vectors of the given dimension = 3.
+class MedianFilter {
+public:
+    // Creates a median filter of size filter_size.
+    // @param filter_size size of the internal filter.
+    explicit MedianFilter(size_t filter_size);
+
+    // Adds sample to buffer_ if buffer_ is full it drops the oldest sample.
+    void AddSample(const Vector3& sample);
+
+    // Returns true if buffer has filter_size_ sample, false otherwise.
+    bool IsValid() const;
+
+    // Returns the median of values store in the internal buffer.
+    Vector3 GetFilteredData() const;
+
+    // Resets the filter, removing all samples that have been added.
+    void Reset();
+
+private:
+    const size_t filter_size_;
+    std::deque<Vector3> buffer_;
+    // Contains norms of the elements stored in buffer_.
+    std::deque<float> norms_;
+};
+
+} // namespace cardboard
+
+#endif // CARDBOARD_SDK_SENSORS_MEDIAN_FILTER_H_

Applications/Official/DEV_FW/source/airmouse/tracking/sensors/pose_prediction.cc

@@ -0,0 +1,71 @@
+/*
+ * Copyright 2019 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#include "pose_prediction.h"
+
+#include <chrono> // NOLINT
+
+#include "../util/logging.h"
+#include "../util/vectorutils.h"
+
+namespace cardboard {
+
+namespace {
+    const double kEpsilon = 1.0e-15;
+} // namespace
+
+namespace pose_prediction {
+
+    Rotation GetRotationFromGyroscope(const Vector3& gyroscope_value, double timestep_s)
+    {
+        const double velocity = Length(gyroscope_value);
+
+        // When there is no rotation data return an identity rotation.
+        if (velocity < kEpsilon) {
+            CARDBOARD_LOGI("PosePrediction::GetRotationFromGyroscope: Velocity really small, "
+                           "returning identity rotation.");
+            return Rotation::Identity();
+        }
+        // Since the gyroscope_value is a start from sensor transformation we need to
+        // invert it to have a sensor from start transformation, hence the minus sign.
+        // For more info:
+        // http://developer.android.com/guide/topics/sensors/sensors_motion.html#sensors-motion-gyro
+        return Rotation::FromAxisAndAngle(gyroscope_value / velocity, -timestep_s * velocity);
+    }
+
+    Rotation PredictPose(int64_t requested_pose_timestamp, const PoseState& current_state)
+    {
+        // Subtracting unsigned numbers is bad when the result is negative.
+        const int64_t diff = requested_pose_timestamp - current_state.timestamp;
+        const double timestep_s = diff * 1.0e-9;
+
+        const Rotation update = GetRotationFromGyroscope(
+            current_state.sensor_from_start_rotation_velocity, timestep_s);
+        return update * current_state.sensor_from_start_rotation;
+    }
+
+    Rotation PredictPoseInv(int64_t requested_pose_timestamp, const PoseState& current_state)
+    {
+        // Subtracting unsigned numbers is bad when the result is negative.
+        const int64_t diff = requested_pose_timestamp - current_state.timestamp;
+        const double timestep_s = diff * 1.0e-9;
+
+        const Rotation update = GetRotationFromGyroscope(
+            current_state.sensor_from_start_rotation_velocity, timestep_s);
+        return current_state.sensor_from_start_rotation * (-update);
+    }
+
+} // namespace pose_prediction
+} // namespace cardboard

Applications/Official/DEV_FW/source/airmouse/tracking/sensors/pose_prediction.h

@@ -0,0 +1,55 @@
+/*
+ * Copyright 2019 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#ifndef CARDBOARD_SDK_SENSORS_POSE_PREDICTION_H_
+#define CARDBOARD_SDK_SENSORS_POSE_PREDICTION_H_
+
+#include <cstdint>
+
+#include "pose_state.h"
+#include "../util/rotation.h"
+
+namespace cardboard {
+namespace pose_prediction {
+
+// Returns a rotation matrix based on the integration of the gyroscope_value
+// over the timestep_s in seconds.
+// TODO(pfg): Document the space better here.
+//
+// @param gyroscope_value gyroscope sensor values.
+// @param timestep_s integration period in seconds.
+// @return Integration of the gyroscope value the rotation is from Start to
+//         Sensor Space.
+Rotation GetRotationFromGyroscope(const Vector3& gyroscope_value, double timestep_s);
+
+// Gets a predicted pose for a given time in the future (e.g. rendering time)
+// based on a linear prediction model. This uses the system current state
+// (position, velocity, etc) from the past to extrapolate a position in the
+// future.
+//
+// @param requested_pose_timestamp time at which you want the pose.
+// @param current_state current state that stores the pose and linear model at a
+//        given time prior to requested_pose_timestamp_ns.
+// @return pose from Start to Sensor Space.
+Rotation PredictPose(int64_t requested_pose_timestamp, const PoseState& current_state);
+
+// Equivalent to PredictPose, but for use with poses relative to Start Space
+// rather than sensor space.
+Rotation PredictPoseInv(int64_t requested_pose_timestamp, const PoseState& current_state);
+
+} // namespace pose_prediction
+} // namespace cardboard
+
+#endif // CARDBOARD_SDK_SENSORS_POSE_PREDICTION_H_

Applications/Official/DEV_FW/source/airmouse/tracking/sensors/pose_state.h

@@ -0,0 +1,56 @@
+/*
+ * Copyright 2019 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#ifndef CARDBOARD_SDK_SENSORS_POSE_STATE_H_
+#define CARDBOARD_SDK_SENSORS_POSE_STATE_H_
+
+#include "../util/rotation.h"
+#include "../util/vector.h"
+
+namespace cardboard {
+
+enum {
+    kPoseStateFlagInvalid = 1U << 0,
+    kPoseStateFlagInitializing = 1U << 1,
+    kPoseStateFlagHas6DoF = 1U << 2,
+};
+
+// Stores a head pose pose plus derivatives. This can be used for prediction.
+struct PoseState {
+    // System wall time.
+    int64_t timestamp;
+
+    // Rotation from Sensor Space to Start Space.
+    Rotation sensor_from_start_rotation;
+
+    // First derivative of the rotation.
+    Vector3 sensor_from_start_rotation_velocity;
+
+    // Current gyroscope bias in rad/s.
+    Vector3 bias;
+
+    // The position of the headset.
+    Vector3 position = Vector3(0, 0, 0);
+
+    // In the same coordinate frame as the position.
+    Vector3 velocity = Vector3(0, 0, 0);
+
+    // Flags indicating the status of the pose.
+    uint64_t flags = 0U;
+};
+
+} // namespace cardboard
+
+#endif // CARDBOARD_SDK_SENSORS_POSE_STATE_H_

Applications/Official/DEV_FW/source/airmouse/tracking/sensors/sensor_fusion_ekf.cc

@@ -0,0 +1,333 @@
+/*
+ * Copyright 2019 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#include "sensor_fusion_ekf.h"
+
+#include <algorithm>
+#include <cmath>
+
+#include "accelerometer_data.h"
+#include "gyroscope_data.h"
+#include "pose_prediction.h"
+#include "../util/matrixutils.h"
+
+namespace cardboard {
+
+namespace {
+
+    const double kFiniteDifferencingEpsilon = 1.0e-7;
+    const double kEpsilon = 1.0e-15;
+    // Default gyroscope frequency. This corresponds to 100 Hz.
+    const double kDefaultGyroscopeTimestep_s = 0.01f;
+    // Maximum time between gyroscope before we start limiting the integration.
+    const double kMaximumGyroscopeSampleDelay_s = 0.04f;
+    // Compute a first-order exponential moving average of changes in accel norm per
+    // frame.
+    const double kSmoothingFactor = 0.5;
+    // Minimum and maximum values used for accelerometer noise covariance matrix.
+    // The smaller the sigma value, the more weight is given to the accelerometer
+    // signal.
+    const double kMinAccelNoiseSigma = 0.75;
+    const double kMaxAccelNoiseSigma = 7.0;
+    // Initial value for the diagonal elements of the different covariance matrices.
+    const double kInitialStateCovarianceValue = 25.0;
+    const double kInitialProcessCovarianceValue = 1.0;
+    // Maximum accelerometer norm change allowed before capping it covariance to a
+    // large value.
+    const double kMaxAccelNormChange = 0.15;
+    // Timestep IIR filtering coefficient.
+    const double kTimestepFilterCoeff = 0.95;
+    // Minimum number of sample for timestep filtering.
+    const int kTimestepFilterMinSamples = 10;
+
+    // Z direction in start space.
+    const Vector3 kCanonicalZDirection(0.0, 0.0, 1.0);
+
+    // Computes an axis-angle rotation from the input vector.
+    // angle = norm(a)
+    // axis = a.normalized()
+    // If norm(a) == 0, it returns an identity rotation.
+    static inline Rotation RotationFromVector(const Vector3& a)
+    {
+        const double norm_a = Length(a);
+        if (norm_a < kEpsilon) {
+            return Rotation::Identity();
+        }
+        return Rotation::FromAxisAndAngle(a / norm_a, norm_a);
+    }
+
+} // namespace
+
+SensorFusionEkf::SensorFusionEkf()
+    : execute_reset_with_next_accelerometer_sample_(false)
+    , bias_estimation_enabled_(true)
+    , gyroscope_bias_estimate_({ 0, 0, 0 })
+{
+    ResetState();
+}
+
+void SensorFusionEkf::Reset() { execute_reset_with_next_accelerometer_sample_ = true; }
+
+void SensorFusionEkf::ResetState()
+{
+    current_state_.sensor_from_start_rotation = Rotation::Identity();
+    current_state_.sensor_from_start_rotation_velocity = Vector3::Zero();
+
+    current_gyroscope_sensor_timestamp_ns_ = 0;
+    current_accelerometer_sensor_timestamp_ns_ = 0;
+
+    state_covariance_ = Matrix3x3::Identity() * kInitialStateCovarianceValue;
+    process_covariance_ = Matrix3x3::Identity() * kInitialProcessCovarianceValue;
+    accelerometer_measurement_covariance_
+        = Matrix3x3::Identity() * kMinAccelNoiseSigma * kMinAccelNoiseSigma;
+    innovation_covariance_ = Matrix3x3::Identity();
+
+    accelerometer_measurement_jacobian_ = Matrix3x3::Zero();
+    kalman_gain_ = Matrix3x3::Zero();
+    innovation_ = Vector3::Zero();
+    accelerometer_measurement_ = Vector3::Zero();
+    prediction_ = Vector3::Zero();
+    control_input_ = Vector3::Zero();
+    state_update_ = Vector3::Zero();
+
+    moving_average_accelerometer_norm_change_ = 0.0;
+
+    is_timestep_filter_initialized_ = false;
+    is_gyroscope_filter_valid_ = false;
+    is_aligned_with_gravity_ = false;
+
+    // Reset biases.
+    gyroscope_bias_estimator_.Reset();
+    gyroscope_bias_estimate_ = { 0, 0, 0 };
+}
+
+// Here I am doing something wrong relative to time stamps. The state timestamps
+// always correspond to the gyrostamps because it would require additional
+// extrapolation if I wanted to do otherwise.
+PoseState SensorFusionEkf::GetLatestPoseState() const { return current_state_; }
+
+void SensorFusionEkf::ProcessGyroscopeSample(const GyroscopeData& sample)
+{
+    // Don't accept gyroscope sample when waiting for a reset.
+    if (execute_reset_with_next_accelerometer_sample_) {
+        return;
+    }
+
+    // Discard outdated samples.
+    if (current_gyroscope_sensor_timestamp_ns_ >= sample.sensor_timestamp_ns) {
+        current_gyroscope_sensor_timestamp_ns_ = sample.sensor_timestamp_ns;
+        return;
+    }
+
+    // Checks that we received at least one gyroscope sample in the past.
+    if (current_gyroscope_sensor_timestamp_ns_ != 0) {
+        double current_timestep_s = std::chrono::duration_cast<std::chrono::duration<double>>(
+            std::chrono::nanoseconds(
+                sample.sensor_timestamp_ns - current_gyroscope_sensor_timestamp_ns_))
+                                        .count();
+        if (current_timestep_s > kMaximumGyroscopeSampleDelay_s) {
+            if (is_gyroscope_filter_valid_) {
+                // Replaces the delta timestamp by the filtered estimates of the delta time.
+                current_timestep_s = filtered_gyroscope_timestep_s_;
+            } else {
+                current_timestep_s = kDefaultGyroscopeTimestep_s;
+            }
+        } else {
+            FilterGyroscopeTimestep(current_timestep_s);
+        }
+
+        if (bias_estimation_enabled_) {
+            gyroscope_bias_estimator_.ProcessGyroscope(sample.data, sample.sensor_timestamp_ns);
+
+            if (gyroscope_bias_estimator_.IsCurrentEstimateValid()) {
+                // As soon as the device is considered to be static, the bias estimator
+                // should have a precise estimate of the gyroscope bias.
+                gyroscope_bias_estimate_ = gyroscope_bias_estimator_.GetGyroscopeBias();
+            }
+        }
+
+        // Only integrate after receiving an accelerometer sample.
+        if (is_aligned_with_gravity_) {
+            const Rotation rotation_from_gyroscope = pose_prediction::GetRotationFromGyroscope(
+                { sample.data[0] - gyroscope_bias_estimate_[0],
+                    sample.data[1] - gyroscope_bias_estimate_[1],
+                    sample.data[2] - gyroscope_bias_estimate_[2] },
+                current_timestep_s);
+            current_state_.sensor_from_start_rotation
+                = rotation_from_gyroscope * current_state_.sensor_from_start_rotation;
+            UpdateStateCovariance(RotationMatrixNH(rotation_from_gyroscope));
+            state_covariance_ = state_covariance_
+                + ((current_timestep_s * current_timestep_s) * process_covariance_);
+        }
+    }
+
+    // Saves gyroscope event for future prediction.
+    current_state_.timestamp = sample.system_timestamp;
+    current_gyroscope_sensor_timestamp_ns_ = sample.sensor_timestamp_ns;
+    current_state_.sensor_from_start_rotation_velocity.Set(
+        sample.data[0] - gyroscope_bias_estimate_[0], sample.data[1] - gyroscope_bias_estimate_[1],
+        sample.data[2] - gyroscope_bias_estimate_[2]);
+}
+
+Vector3 SensorFusionEkf::ComputeInnovation(const Rotation& pose)
+{
+    const Vector3 predicted_down_direction = pose * kCanonicalZDirection;
+
+    const Rotation rotation
+        = Rotation::RotateInto(predicted_down_direction, accelerometer_measurement_);
+    Vector3 axis;
+    double angle;
+    rotation.GetAxisAndAngle(&axis, &angle);
+    return axis * angle;
+}
+
+void SensorFusionEkf::ComputeMeasurementJacobian()
+{
+    for (int dof = 0; dof < 3; dof++) {
+        Vector3 delta = Vector3::Zero();
+        delta[dof] = kFiniteDifferencingEpsilon;
+
+        const Rotation epsilon_rotation = RotationFromVector(delta);
+        const Vector3 delta_rotation
+            = ComputeInnovation(epsilon_rotation * current_state_.sensor_from_start_rotation);
+
+        const Vector3 col = (innovation_ - delta_rotation) / kFiniteDifferencingEpsilon;
+        accelerometer_measurement_jacobian_(0, dof) = col[0];
+        accelerometer_measurement_jacobian_(1, dof) = col[1];
+        accelerometer_measurement_jacobian_(2, dof) = col[2];
+    }
+}
+
+void SensorFusionEkf::ProcessAccelerometerSample(const AccelerometerData& sample)
+{
+    // Discard outdated samples.
+    if (current_accelerometer_sensor_timestamp_ns_ >= sample.sensor_timestamp_ns) {
+        current_accelerometer_sensor_timestamp_ns_ = sample.sensor_timestamp_ns;
+        return;
+    }
+
+    // Call reset state if required.
+    if (execute_reset_with_next_accelerometer_sample_.exchange(false)) {
+        ResetState();
+    }
+
+    accelerometer_measurement_.Set(sample.data[0], sample.data[1], sample.data[2]);
+    current_accelerometer_sensor_timestamp_ns_ = sample.sensor_timestamp_ns;
+
+    if (bias_estimation_enabled_) {
+        gyroscope_bias_estimator_.ProcessAccelerometer(sample.data, sample.sensor_timestamp_ns);
+    }
+
+    if (!is_aligned_with_gravity_) {
+        // This is the first accelerometer measurement so it initializes the
+        // orientation estimate.
+        current_state_.sensor_from_start_rotation
+            = Rotation::RotateInto(kCanonicalZDirection, accelerometer_measurement_);
+        is_aligned_with_gravity_ = true;
+
+        previous_accelerometer_norm_ = Length(accelerometer_measurement_);
+        return;
+    }
+
+    UpdateMeasurementCovariance();
+
+    innovation_ = ComputeInnovation(current_state_.sensor_from_start_rotation);
+    ComputeMeasurementJacobian();
+
+    // S = H * P * H' + R
+    innovation_covariance_ = accelerometer_measurement_jacobian_ * state_covariance_
+            * Transpose(accelerometer_measurement_jacobian_)
+        + accelerometer_measurement_covariance_;
+
+    // K = P * H' * S^-1
+    kalman_gain_ = state_covariance_ * Transpose(accelerometer_measurement_jacobian_)
+        * Inverse(innovation_covariance_);
+
+    // x_update = K*nu
+    state_update_ = kalman_gain_ * innovation_;
+
+    // P = (I - K * H) * P;
+    state_covariance_ = (Matrix3x3::Identity() - kalman_gain_ * accelerometer_measurement_jacobian_)
+        * state_covariance_;
+
+    // Updates pose and associate covariance matrix.
+    const Rotation rotation_from_state_update = RotationFromVector(state_update_);
+
+    current_state_.sensor_from_start_rotation
+        = rotation_from_state_update * current_state_.sensor_from_start_rotation;
+    UpdateStateCovariance(RotationMatrixNH(rotation_from_state_update));
+}
+
+void SensorFusionEkf::UpdateStateCovariance(const Matrix3x3& motion_update)
+{
+    state_covariance_ = motion_update * state_covariance_ * Transpose(motion_update);
+}
+
+void SensorFusionEkf::FilterGyroscopeTimestep(double gyroscope_timestep_s)
+{
+    if (!is_timestep_filter_initialized_) {
+        // Initializes the filter.
+        filtered_gyroscope_timestep_s_ = gyroscope_timestep_s;
+        num_gyroscope_timestep_samples_ = 1;
+        is_timestep_filter_initialized_ = true;
+        return;
+    }
+
+    // Computes the IIR filter response.
+    filtered_gyroscope_timestep_s_ = kTimestepFilterCoeff * filtered_gyroscope_timestep_s_
+        + (1 - kTimestepFilterCoeff) * gyroscope_timestep_s;
+    ++num_gyroscope_timestep_samples_;
+
+    if (num_gyroscope_timestep_samples_ > kTimestepFilterMinSamples) {
+        is_gyroscope_filter_valid_ = true;
+    }
+}
+
+void SensorFusionEkf::UpdateMeasurementCovariance()
+{
+    const double current_accelerometer_norm = Length(accelerometer_measurement_);
+    // Norm change between current and previous accel readings.
+    const double current_accelerometer_norm_change
+        = std::abs(current_accelerometer_norm - previous_accelerometer_norm_);
+    previous_accelerometer_norm_ = current_accelerometer_norm;
+
+    moving_average_accelerometer_norm_change_ = kSmoothingFactor * current_accelerometer_norm_change
+        + (1 - kSmoothingFactor) * moving_average_accelerometer_norm_change_;
+
+    // If we hit the accel norm change threshold, we use the maximum noise sigma
+    // for the accel covariance. For anything below that, we use a linear
+    // combination between min and max sigma values.
+    const double norm_change_ratio
+        = moving_average_accelerometer_norm_change_ / kMaxAccelNormChange;
+    const double accelerometer_noise_sigma = std::min(kMaxAccelNoiseSigma,
+        kMinAccelNoiseSigma + norm_change_ratio * (kMaxAccelNoiseSigma - kMinAccelNoiseSigma));
+
+    // Updates the accel covariance matrix with the new sigma value.
+    accelerometer_measurement_covariance_
+        = Matrix3x3::Identity() * accelerometer_noise_sigma * accelerometer_noise_sigma;
+}
+
+bool SensorFusionEkf::IsBiasEstimationEnabled() const { return bias_estimation_enabled_; }
+
+void SensorFusionEkf::SetBiasEstimationEnabled(bool enable)
+{
+    if (bias_estimation_enabled_ != enable) {
+        bias_estimation_enabled_ = enable;
+        gyroscope_bias_estimate_ = { 0, 0, 0 };
+        gyroscope_bias_estimator_.Reset();
+    }
+}
+
+} // namespace cardboard

Applications/Official/DEV_FW/source/airmouse/tracking/sensors/sensor_fusion_ekf.h

@@ -0,0 +1,188 @@
+/*
+ * Copyright 2019 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#ifndef CARDBOARD_SDK_SENSORS_SENSOR_FUSION_EKF_H_
+#define CARDBOARD_SDK_SENSORS_SENSOR_FUSION_EKF_H_
+
+#include <array>
+#include <atomic>
+#include <cstdint>
+
+#include "accelerometer_data.h"
+#include "gyroscope_bias_estimator.h"
+#include "gyroscope_data.h"
+#include "pose_state.h"
+#include "../util/matrix_3x3.h"
+#include "../util/rotation.h"
+#include "../util/vector.h"
+
+namespace cardboard {
+
+// Sensor fusion class that implements an Extended Kalman Filter (EKF) to
+// estimate a 3D rotation from a gyroscope and an accelerometer.
+// This system only has one state, the pose. It does not estimate any velocity
+// or acceleration.
+//
+// To learn more about Kalman filtering one can read this article which is a
+// good introduction: https://en.wikipedia.org/wiki/Kalman_filter
+class SensorFusionEkf {
+public:
+    SensorFusionEkf();
+
+    // Resets the state of the sensor fusion. It sets the velocity for
+    // prediction to zero. The reset will happen with the next
+    // accelerometer sample. Gyroscope sample will be discarded until a new
+    // accelerometer sample arrives.
+    void Reset();
+
+    // Gets the PoseState representing the latest pose and  derivatives at a
+    // particular timestamp as estimated by SensorFusion.
+    PoseState GetLatestPoseState() const;
+
+    // Processes one gyroscope sample event. This updates the pose of the system
+    // and the prediction model. The gyroscope data is assumed to be in axis angle
+    // form. Angle = ||v|| and Axis = v / ||v||, with v = [v_x, v_y, v_z]^T.
+    //
+    // @param sample gyroscope sample data.
+    void ProcessGyroscopeSample(const GyroscopeData& sample);
+
+    // Processes one accelerometer sample event. This updates the pose of the
+    // system. If the Accelerometer norm changes too much between sample it is not
+    // trusted as much.
+    //
+    // @param sample accelerometer sample data.
+    void ProcessAccelerometerSample(const AccelerometerData& sample);
+
+    // Enables or disables the drift correction by estimating the gyroscope bias.
+    //
+    // @param enable Enable drift correction.
+    void SetBiasEstimationEnabled(bool enable);
+
+    // Returns a boolean that indicates if bias estimation is enabled or disabled.
+    //
+    // @return true if bias estimation is enabled, false otherwise.
+    bool IsBiasEstimationEnabled() const;
+
+    // Returns the current gyroscope bias estimate from GyroscopeBiasEstimator.
+    Vector3 GetGyroscopeBias() const {
+        return {
+            gyroscope_bias_estimate_[0], gyroscope_bias_estimate_[1], gyroscope_bias_estimate_[2]};
+    }
+
+    // Returns true after receiving the first accelerometer measurement.
+    bool IsFullyInitialized() const {
+        return is_aligned_with_gravity_;
+    }
+
+private:
+    // Estimates the average timestep between gyroscope event.
+    void FilterGyroscopeTimestep(double gyroscope_timestep);
+
+    // Updates the state covariance with an incremental motion. It changes the
+    // space of the quadric.
+    void UpdateStateCovariance(const Matrix3x3& motion_update);
+
+    // Computes the innovation vector of the Kalman based on the input pose.
+    // It uses the latest measurement vector (i.e. accelerometer data), which must
+    // be set prior to calling this function.
+    Vector3 ComputeInnovation(const Rotation& pose);
+
+    // This computes the measurement_jacobian_ via numerical differentiation based
+    // on the current value of sensor_from_start_rotation_.
+    void ComputeMeasurementJacobian();
+
+    // Updates the accelerometer covariance matrix.
+    //
+    // This looks at the norm of recent accelerometer readings. If it has changed
+    // significantly, it means the phone receives additional acceleration than
+    // just gravity, and so the down vector information gravity signal is noisier.
+    void UpdateMeasurementCovariance();
+
+    // Reset all internal states. This is not thread safe. Lock should be acquired
+    // outside of it. This function is called in ProcessAccelerometerSample.
+    void ResetState();
+
+    // Current transformation from Sensor Space to Start Space.
+    // x_sensor = sensor_from_start_rotation_ * x_start;
+    PoseState current_state_;
+
+    // Filtering of the gyroscope timestep started?
+    bool is_timestep_filter_initialized_;
+    // Filtered gyroscope timestep valid?
+    bool is_gyroscope_filter_valid_;
+    // Sensor fusion currently aligned with gravity? After initialization
+    // it will requires a couple of accelerometer data for the system to get
+    // aligned.
+    std::atomic<bool> is_aligned_with_gravity_;
+
+    // Covariance of Kalman filter state (P in common formulation).
+    Matrix3x3 state_covariance_;
+    // Covariance of the process noise (Q in common formulation).
+    Matrix3x3 process_covariance_;
+    // Covariance of the accelerometer measurement (R in common formulation).
+    Matrix3x3 accelerometer_measurement_covariance_;
+    // Covariance of innovation (S in common formulation).
+    Matrix3x3 innovation_covariance_;
+    // Jacobian of the measurements (H in common formulation).
+    Matrix3x3 accelerometer_measurement_jacobian_;
+    // Gain of the Kalman filter (K in common formulation).
+    Matrix3x3 kalman_gain_;
+    // Parameter update a.k.a. innovation vector. (\nu in common formulation).
+    Vector3 innovation_;
+    // Measurement vector (z in common formulation).
+    Vector3 accelerometer_measurement_;
+    // Current prediction vector (g in common formulation).
+    Vector3 prediction_;
+    // Control input, currently this is only the gyroscope data (\mu in common
+    // formulation).
+    Vector3 control_input_;
+    // Update of the state vector. (x in common formulation).
+    Vector3 state_update_;
+
+    // Sensor time of the last gyroscope processed event.
+    uint64_t current_gyroscope_sensor_timestamp_ns_;
+    // Sensor time of the last accelerometer processed event.
+    uint64_t current_accelerometer_sensor_timestamp_ns_;
+
+    // Estimates of the timestep between gyroscope event in seconds.
+    double filtered_gyroscope_timestep_s_;
+    // Number of timestep samples processed so far by the filter.
+    uint32_t num_gyroscope_timestep_samples_;
+    // Norm of the accelerometer for the previous measurement.
+    double previous_accelerometer_norm_;
+    // Moving average of the accelerometer norm changes. It is computed for every
+    // sensor datum.
+    double moving_average_accelerometer_norm_change_;
+
+    // Flag indicating if a state reset should be executed with the next
+    // accelerometer sample.
+    std::atomic<bool> execute_reset_with_next_accelerometer_sample_;
+
+    // Flag indicating if bias estimation is enabled (enabled by default).
+    std::atomic<bool> bias_estimation_enabled_;
+
+    // Bias estimator and static device detector.
+    GyroscopeBiasEstimator gyroscope_bias_estimator_;
+
+    // Current bias estimate_;
+    Vector3 gyroscope_bias_estimate_;
+
+    SensorFusionEkf(const SensorFusionEkf&) = delete;
+    SensorFusionEkf& operator=(const SensorFusionEkf&) = delete;
+};
+
+} // namespace cardboard
+
+#endif // CARDBOARD_SDK_SENSORS_SENSOR_FUSION_EKF_H_

Applications/Official/DEV_FW/source/airmouse/tracking/util/logging.h

@@ -0,0 +1,38 @@
+/*
+ * Copyright 2019 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#ifndef CARDBOARD_SDK_UTIL_LOGGING_H_
+#define CARDBOARD_SDK_UTIL_LOGGING_H_
+
+#include <furi.h>
+#include <furi_hal.h>
+
+#if defined(__ANDROID__)
+
+#include <android/log.h>
+
+// Uncomment these to enable debug logging from native code
+
+#define CARDBOARD_LOGI(...) // __android_log_print(ANDROID_LOG_INFO, "CardboardSDK", __VA_ARGS__)
+#define CARDBOARD_LOGE(...) // __android_log_print(ANDROID_LOG_ERROR, "CardboardSDK", __VA_ARGS__)
+
+#else
+
+#define CARDBOARD_LOGI(...) // FURI_LOG_I("CardboardSDK", __VA_ARGS__)
+#define CARDBOARD_LOGE(...) // FURI_LOG_E("CardboardSDK", __VA_ARGS__)
+
+#endif
+
+#endif // CARDBOARD_SDK_UTIL_LOGGING_H_

Applications/Official/DEV_FW/source/airmouse/tracking/util/matrix_3x3.cc

@@ -0,0 +1,121 @@
+/*
+ * Copyright 2019 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#include "matrix_3x3.h"
+
+namespace cardboard {
+
+Matrix3x3::Matrix3x3(double m00, double m01, double m02, double m10, double m11, double m12,
+    double m20, double m21, double m22)
+    : elem_ { { { m00, m01, m02 }, { m10, m11, m12 }, { m20, m21, m22 } } }
+{
+}
+
+Matrix3x3::Matrix3x3()
+{
+    for (int row = 0; row < 3; ++row) {
+        for (int col = 0; col < 3; ++col)
+            elem_[row][col] = 0;
+    }
+}
+
+Matrix3x3 Matrix3x3::Zero()
+{
+    Matrix3x3 result;
+    return result;
+}
+
+Matrix3x3 Matrix3x3::Identity()
+{
+    Matrix3x3 result;
+    for (int row = 0; row < 3; ++row) {
+        result.elem_[row][row] = 1;
+    }
+    return result;
+}
+
+void Matrix3x3::MultiplyScalar(double s)
+{
+    for (int row = 0; row < 3; ++row) {
+        for (int col = 0; col < 3; ++col)
+            elem_[row][col] *= s;
+    }
+}
+
+Matrix3x3 Matrix3x3::Negation() const
+{
+    Matrix3x3 result;
+    for (int row = 0; row < 3; ++row) {
+        for (int col = 0; col < 3; ++col)
+            result.elem_[row][col] = -elem_[row][col];
+    }
+    return result;
+}
+
+Matrix3x3 Matrix3x3::Scale(const Matrix3x3& m, double s)
+{
+    Matrix3x3 result;
+    for (int row = 0; row < 3; ++row) {
+        for (int col = 0; col < 3; ++col)
+            result.elem_[row][col] = m.elem_[row][col] * s;
+    }
+    return result;
+}
+
+Matrix3x3 Matrix3x3::Addition(const Matrix3x3& lhs, const Matrix3x3& rhs)
+{
+    Matrix3x3 result;
+    for (int row = 0; row < 3; ++row) {
+        for (int col = 0; col < 3; ++col)
+            result.elem_[row][col] = lhs.elem_[row][col] + rhs.elem_[row][col];
+    }
+    return result;
+}
+
+Matrix3x3 Matrix3x3::Subtraction(const Matrix3x3& lhs, const Matrix3x3& rhs)
+{
+    Matrix3x3 result;
+    for (int row = 0; row < 3; ++row) {
+        for (int col = 0; col < 3; ++col)
+            result.elem_[row][col] = lhs.elem_[row][col] - rhs.elem_[row][col];
+    }
+    return result;
+}
+
+Matrix3x3 Matrix3x3::Product(const Matrix3x3& m0, const Matrix3x3& m1)
+{
+    Matrix3x3 result;
+    for (int row = 0; row < 3; ++row) {
+        for (int col = 0; col < 3; ++col) {
+            result.elem_[row][col] = 0;
+            for (int i = 0; i < 3; ++i)
+                result.elem_[row][col] += m0.elem_[row][i] * m1.elem_[i][col];
+        }
+    }
+    return result;
+}
+
+bool Matrix3x3::AreEqual(const Matrix3x3& m0, const Matrix3x3& m1)
+{
+    for (int row = 0; row < 3; ++row) {
+        for (int col = 0; col < 3; ++col) {
+            if (m0.elem_[row][col] != m1.elem_[row][col])
+                return false;
+        }
+    }
+    return true;
+}
+
+} // namespace cardboard

Applications/Official/DEV_FW/source/airmouse/tracking/util/matrix_3x3.h

@@ -0,0 +1,138 @@
+/*
+ * Copyright 2019 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#ifndef CARDBOARD_SDK_UTIL_MATRIX_3X3_H_
+#define CARDBOARD_SDK_UTIL_MATRIX_3X3_H_
+
+#include <array>
+#include <cstring> // For memcpy().
+#include <istream> // NOLINT
+#include <ostream> // NOLINT
+
+namespace cardboard {
+
+// The Matrix3x3 class defines a square 3-dimensional matrix. Elements are
+// stored in row-major order.
+// TODO(b/135461889): Make this class consistent with Matrix4x4.
+class Matrix3x3 {
+public:
+    // The default constructor zero-initializes all elements.
+    Matrix3x3();
+
+    // Dimension-specific constructors that are passed individual element values.
+    Matrix3x3(
+        double m00,
+        double m01,
+        double m02,
+        double m10,
+        double m11,
+        double m12,
+        double m20,
+        double m21,
+        double m22);
+
+    // Constructor that reads elements from a linear array of the correct size.
+    explicit Matrix3x3(const double array[3 * 3]);
+
+    // Returns a Matrix3x3 containing all zeroes.
+    static Matrix3x3 Zero();
+
+    // Returns an identity Matrix3x3.
+    static Matrix3x3 Identity();
+
+    // Mutable element accessors.
+    double& operator()(int row, int col) {
+        return elem_[row][col];
+    }
+    std::array<double, 3>& operator[](int row) {
+        return elem_[row];
+    }
+
+    // Read-only element accessors.
+    const double& operator()(int row, int col) const {
+        return elem_[row][col];
+    }
+    const std::array<double, 3>& operator[](int row) const {
+        return elem_[row];
+    }
+
+    // Return a pointer to the data for interfacing with libraries.
+    double* Data() {
+        return &elem_[0][0];
+    }
+    const double* Data() const {
+        return &elem_[0][0];
+    }
+
+    // Self-modifying multiplication operators.
+    void operator*=(double s) {
+        MultiplyScalar(s);
+    }
+    void operator*=(const Matrix3x3& m) {
+        *this = Product(*this, m);
+    }
+
+    // Unary operators.
+    Matrix3x3 operator-() const {
+        return Negation();
+    }
+
+    // Binary scale operators.
+    friend Matrix3x3 operator*(const Matrix3x3& m, double s) {
+        return Scale(m, s);
+    }
+    friend Matrix3x3 operator*(double s, const Matrix3x3& m) {
+        return Scale(m, s);
+    }
+
+    // Binary matrix addition.
+    friend Matrix3x3 operator+(const Matrix3x3& lhs, const Matrix3x3& rhs) {
+        return Addition(lhs, rhs);
+    }
+
+    // Binary matrix subtraction.
+    friend Matrix3x3 operator-(const Matrix3x3& lhs, const Matrix3x3& rhs) {
+        return Subtraction(lhs, rhs);
+    }
+
+    // Binary multiplication operator.
+    friend Matrix3x3 operator*(const Matrix3x3& m0, const Matrix3x3& m1) {
+        return Product(m0, m1);
+    }
+
+    // Exact equality and inequality comparisons.
+    friend bool operator==(const Matrix3x3& m0, const Matrix3x3& m1) {
+        return AreEqual(m0, m1);
+    }
+    friend bool operator!=(const Matrix3x3& m0, const Matrix3x3& m1) {
+        return !AreEqual(m0, m1);
+    }
+
+private:
+    // These private functions implement most of the operators.
+    void MultiplyScalar(double s);
+    Matrix3x3 Negation() const;
+    static Matrix3x3 Addition(const Matrix3x3& lhs, const Matrix3x3& rhs);
+    static Matrix3x3 Subtraction(const Matrix3x3& lhs, const Matrix3x3& rhs);
+    static Matrix3x3 Scale(const Matrix3x3& m, double s);
+    static Matrix3x3 Product(const Matrix3x3& m0, const Matrix3x3& m1);
+    static bool AreEqual(const Matrix3x3& m0, const Matrix3x3& m1);
+
+    std::array<std::array<double, 3>, 3> elem_;
+};
+
+} // namespace cardboard
+
+#endif // CARDBOARD_SDK_UTIL_MATRIX_3X3_H_

Applications/Official/DEV_FW/source/airmouse/tracking/util/matrix_4x4.cc

@@ -0,0 +1,87 @@
+/*
+ * Copyright 2019 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#include "matrix_4x4.h"
+
+#include <algorithm>
+#include <cmath>
+#include <cstring>
+
+namespace cardboard {
+
+Matrix4x4 Matrix4x4::Identity()
+{
+    Matrix4x4 ret;
+    for (int j = 0; j < 4; ++j) {
+        for (int i = 0; i < 4; ++i) {
+            ret.m[j][i] = (i == j) ? 1 : 0;
+        }
+    }
+
+    return ret;
+}
+
+Matrix4x4 Matrix4x4::Zeros()
+{
+    Matrix4x4 ret;
+    for (int j = 0; j < 4; ++j) {
+        for (int i = 0; i < 4; ++i) {
+            ret.m[j][i] = 0;
+        }
+    }
+
+    return ret;
+}
+
+Matrix4x4 Matrix4x4::Translation(float x, float y, float z)
+{
+    Matrix4x4 ret = Matrix4x4::Identity();
+    ret.m[3][0] = x;
+    ret.m[3][1] = y;
+    ret.m[3][2] = z;
+
+    return ret;
+}
+
+Matrix4x4 Matrix4x4::Perspective(const std::array<float, 4>& fov, float zNear, float zFar)
+{
+    Matrix4x4 ret = Matrix4x4::Zeros();
+
+    const float xLeft = -std::tan(fov[0] * M_PI / 180.0f) * zNear;
+    const float xRight = std::tan(fov[1] * M_PI / 180.0f) * zNear;
+    const float yBottom = -std::tan(fov[2] * M_PI / 180.0f) * zNear;
+    const float yTop = std::tan(fov[3] * M_PI / 180.0f) * zNear;
+
+    const float X = (2 * zNear) / (xRight - xLeft);
+    const float Y = (2 * zNear) / (yTop - yBottom);
+    const float A = (xRight + xLeft) / (xRight - xLeft);
+    const float B = (yTop + yBottom) / (yTop - yBottom);
+    const float C = (zNear + zFar) / (zNear - zFar);
+    const float D = (2 * zNear * zFar) / (zNear - zFar);
+
+    ret.m[0][0] = X;
+    ret.m[2][0] = A;
+    ret.m[1][1] = Y;
+    ret.m[2][1] = B;
+    ret.m[2][2] = C;
+    ret.m[3][2] = D;
+    ret.m[2][3] = -1;
+
+    return ret;
+}
+
+void Matrix4x4::ToArray(float* array) const { std::memcpy(array, &m[0][0], 16 * sizeof(float)); }
+
+} // namespace cardboard

Applications/Official/DEV_FW/source/airmouse/tracking/util/matrix_4x4.h

@@ -0,0 +1,37 @@
+/*
+ * Copyright 2019 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#ifndef CARDBOARD_SDK_UTIL_MATRIX_4X4_H_
+#define CARDBOARD_SDK_UTIL_MATRIX_4X4_H_
+
+#include <array>
+
+namespace cardboard {
+
+class Matrix4x4 {
+public:
+    static Matrix4x4 Identity();
+    static Matrix4x4 Zeros();
+    static Matrix4x4 Translation(float x, float y, float z);
+    static Matrix4x4 Perspective(const std::array<float, 4>& fov, float zNear, float zFar);
+    void ToArray(float* array) const;
+
+private:
+    std::array<std::array<float, 4>, 4> m;
+};
+
+} // namespace cardboard
+
+#endif // CARDBOARD_SDK_UTIL_MATRIX4X4_H_

Applications/Official/DEV_FW/source/airmouse/tracking/util/matrixutils.cc

@@ -0,0 +1,148 @@
+/*
+ * Copyright 2019 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#include "matrixutils.h"
+
+#include "vectorutils.h"
+
+namespace cardboard {
+
+namespace {
+
+    // Returns true if the cofactor for a given row and column should be negated.
+    static bool IsCofactorNegated(int row, int col)
+    {
+        // Negated iff (row + col) is odd.
+        return ((row + col) & 1) != 0;
+    }
+
+    static double CofactorElement3(const Matrix3x3& m, int row, int col)
+    {
+        static const int index[3][2] = { { 1, 2 }, { 0, 2 }, { 0, 1 } };
+        const int i0 = index[row][0];
+        const int i1 = index[row][1];
+        const int j0 = index[col][0];
+        const int j1 = index[col][1];
+        const double cofactor = m(i0, j0) * m(i1, j1) - m(i0, j1) * m(i1, j0);
+        return IsCofactorNegated(row, col) ? -cofactor : cofactor;
+    }
+
+    // Multiplies a matrix and some type of column vector to
+    // produce another column vector of the same type.
+    Vector3 MultiplyMatrixAndVector(const Matrix3x3& m, const Vector3& v)
+    {
+        Vector3 result = Vector3::Zero();
+        for (int row = 0; row < 3; ++row) {
+            for (int col = 0; col < 3; ++col)
+                result[row] += m(row, col) * v[col];
+        }
+        return result;
+    }
+
+    // Sets the upper 3x3 of a Matrix to represent a 3D rotation.
+    void RotationMatrix3x3(const Rotation& r, Matrix3x3* matrix)
+    {
+        //
+        // Given a quaternion (a,b,c,d) where d is the scalar part, the 3x3 rotation
+        // matrix is:
+        //
+        //   a^2 - b^2 - c^2 + d^2         2ab - 2cd               2ac + 2bd
+        //         2ab + 2cd        -a^2 + b^2 - c^2 + d^2         2bc - 2ad
+        //         2ac - 2bd               2bc + 2ad        -a^2 - b^2 + c^2 + d^2
+        //
+        const Vector<4>& quat = r.GetQuaternion();
+        const double aa = quat[0] * quat[0];
+        const double bb = quat[1] * quat[1];
+        const double cc = quat[2] * quat[2];
+        const double dd = quat[3] * quat[3];
+
+        const double ab = quat[0] * quat[1];
+        const double ac = quat[0] * quat[2];
+        const double bc = quat[1] * quat[2];
+
+        const double ad = quat[0] * quat[3];
+        const double bd = quat[1] * quat[3];
+        const double cd = quat[2] * quat[3];
+
+        Matrix3x3& m = *matrix;
+        m[0][0] = aa - bb - cc + dd;
+        m[0][1] = 2 * ab - 2 * cd;
+        m[0][2] = 2 * ac + 2 * bd;
+        m[1][0] = 2 * ab + 2 * cd;
+        m[1][1] = -aa + bb - cc + dd;
+        m[1][2] = 2 * bc - 2 * ad;
+        m[2][0] = 2 * ac - 2 * bd;
+        m[2][1] = 2 * bc + 2 * ad;
+        m[2][2] = -aa - bb + cc + dd;
+    }
+
+} // anonymous namespace
+
+Vector3 operator*(const Matrix3x3& m, const Vector3& v) { return MultiplyMatrixAndVector(m, v); }
+
+Matrix3x3 CofactorMatrix(const Matrix3x3& m)
+{
+    Matrix3x3 result;
+    for (int row = 0; row < 3; ++row) {
+        for (int col = 0; col < 3; ++col)
+            result(row, col) = CofactorElement3(m, row, col);
+    }
+    return result;
+}
+
+Matrix3x3 AdjugateWithDeterminant(const Matrix3x3& m, double* determinant)
+{
+    const Matrix3x3 cofactor_matrix = CofactorMatrix(m);
+    if (determinant) {
+        *determinant = m(0, 0) * cofactor_matrix(0, 0) + m(0, 1) * cofactor_matrix(0, 1)
+            + m(0, 2) * cofactor_matrix(0, 2);
+    }
+    return Transpose(cofactor_matrix);
+}
+
+// Returns the transpose of a matrix.
+Matrix3x3 Transpose(const Matrix3x3& m)
+{
+    Matrix3x3 result;
+    for (int row = 0; row < 3; ++row) {
+        for (int col = 0; col < 3; ++col)
+            result(row, col) = m(col, row);
+    }
+    return result;
+}
+
+Matrix3x3 InverseWithDeterminant(const Matrix3x3& m, double* determinant)
+{
+    // The inverse is the adjugate divided by the determinant.
+    double det;
+    Matrix3x3 adjugate = AdjugateWithDeterminant(m, &det);
+    if (determinant)
+        *determinant = det;
+    if (det == 0)
+        return Matrix3x3::Zero();
+    else
+        return adjugate * (1 / det);
+}
+
+Matrix3x3 Inverse(const Matrix3x3& m) { return InverseWithDeterminant(m, nullptr); }
+
+Matrix3x3 RotationMatrixNH(const Rotation& r)
+{
+    Matrix3x3 m;
+    RotationMatrix3x3(r, &m);
+    return m;
+}
+
+} // namespace cardboard

Applications/Official/DEV_FW/source/airmouse/tracking/util/matrixutils.h

@@ -0,0 +1,65 @@
+/*
+ * Copyright 2019 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#ifndef CARDBOARD_SDK_UTIL_MATRIXUTILS_H_
+#define CARDBOARD_SDK_UTIL_MATRIXUTILS_H_
+
+//
+// This file contains operators and free functions that define generic Matrix
+// operations.
+//
+
+#include "matrix_3x3.h"
+#include "rotation.h"
+#include "vector.h"
+
+namespace cardboard {
+
+// Returns the transpose of a matrix.
+Matrix3x3 Transpose(const Matrix3x3& m);
+
+// Multiplies a Matrix and a column Vector of the same Dimension to produce
+// another column Vector.
+Vector3 operator*(const Matrix3x3& m, const Vector3& v);
+
+// Returns the determinant of the matrix. This function is defined for all the
+// typedef'ed Matrix types.
+double Determinant(const Matrix3x3& m);
+
+// Returns the adjugate of the matrix, which is defined as the transpose of the
+// cofactor matrix. This function is defined for all the typedef'ed Matrix
+// types.  The determinant of the matrix is computed as a side effect, so it is
+// returned in the determinant parameter if it is not null.
+Matrix3x3 AdjugateWithDeterminant(const Matrix3x3& m, double* determinant);
+
+// Returns the inverse of the matrix. This function is defined for all the
+// typedef'ed Matrix types.  The determinant of the matrix is computed as a
+// side effect, so it is returned in the determinant parameter if it is not
+// null. If the determinant is 0, the returned matrix has all zeroes.
+Matrix3x3 InverseWithDeterminant(const Matrix3x3& m, double* determinant);
+
+// Returns the inverse of the matrix. This function is defined for all the
+// typedef'ed Matrix types. If the determinant of the matrix is 0, the returned
+// matrix has all zeroes.
+Matrix3x3 Inverse(const Matrix3x3& m);
+
+// Returns a 3x3 Matrix representing a 3D rotation. This creates a Matrix that
+// does not work with homogeneous coordinates, so the function name ends in
+// "NH".
+Matrix3x3 RotationMatrixNH(const Rotation& r);
+
+} // namespace cardboard
+
+#endif // CARDBOARD_SDK_UTIL_MATRIXUTILS_H_

Applications/Official/DEV_FW/source/airmouse/tracking/util/rotation.cc

@@ -0,0 +1,117 @@
+/*
+ * Copyright 2019 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#include "rotation.h"
+
+#include <cmath>
+#include <limits>
+
+#include "vectorutils.h"
+
+namespace cardboard {
+
+void Rotation::SetAxisAndAngle(const VectorType& axis, double angle)
+{
+    VectorType unit_axis = axis;
+    if (!Normalize(&unit_axis)) {
+        *this = Identity();
+    } else {
+        double a = angle / 2;
+        const double s = sin(a);
+        const VectorType v(unit_axis * s);
+        SetQuaternion(QuaternionType(v[0], v[1], v[2], cos(a)));
+    }
+}
+
+Rotation Rotation::FromRotationMatrix(const Matrix3x3& mat)
+{
+    static const double kOne = 1.0;
+    static const double kFour = 4.0;
+
+    const double d0 = mat(0, 0), d1 = mat(1, 1), d2 = mat(2, 2);
+    const double ww = kOne + d0 + d1 + d2;
+    const double xx = kOne + d0 - d1 - d2;
+    const double yy = kOne - d0 + d1 - d2;
+    const double zz = kOne - d0 - d1 + d2;
+
+    const double max = std::max(ww, std::max(xx, std::max(yy, zz)));
+    if (ww == max) {
+        const double w4 = sqrt(ww * kFour);
+        return Rotation::FromQuaternion(QuaternionType((mat(2, 1) - mat(1, 2)) / w4,
+            (mat(0, 2) - mat(2, 0)) / w4, (mat(1, 0) - mat(0, 1)) / w4, w4 / kFour));
+    }
+
+    if (xx == max) {
+        const double x4 = sqrt(xx * kFour);
+        return Rotation::FromQuaternion(QuaternionType(x4 / kFour, (mat(0, 1) + mat(1, 0)) / x4,
+            (mat(0, 2) + mat(2, 0)) / x4, (mat(2, 1) - mat(1, 2)) / x4));
+    }
+
+    if (yy == max) {
+        const double y4 = sqrt(yy * kFour);
+        return Rotation::FromQuaternion(QuaternionType((mat(0, 1) + mat(1, 0)) / y4, y4 / kFour,
+            (mat(1, 2) + mat(2, 1)) / y4, (mat(0, 2) - mat(2, 0)) / y4));
+    }
+
+    // zz is the largest component.
+    const double z4 = sqrt(zz * kFour);
+    return Rotation::FromQuaternion(QuaternionType((mat(0, 2) + mat(2, 0)) / z4,
+        (mat(1, 2) + mat(2, 1)) / z4, z4 / kFour, (mat(1, 0) - mat(0, 1)) / z4));
+}
+
+void Rotation::GetAxisAndAngle(VectorType* axis, double* angle) const
+{
+    VectorType vec(quat_[0], quat_[1], quat_[2]);
+    if (Normalize(&vec)) {
+        *angle = 2 * acos(quat_[3]);
+        *axis = vec;
+    } else {
+        *axis = VectorType(1, 0, 0);
+        *angle = 0.0;
+    }
+}
+
+Rotation Rotation::RotateInto(const VectorType& from, const VectorType& to)
+{
+    static const double kTolerance = std::numeric_limits<double>::epsilon() * 100;
+
+    // Directly build the quaternion using the following technique:
+    // http://lolengine.net/blog/2014/02/24/quaternion-from-two-vectors-final
+    const double norm_u_norm_v = sqrt(LengthSquared(from) * LengthSquared(to));
+    double real_part = norm_u_norm_v + Dot(from, to);
+    VectorType w;
+    if (real_part < kTolerance * norm_u_norm_v) {
+        // If |from| and |to| are exactly opposite, rotate 180 degrees around an
+        // arbitrary orthogonal axis. Axis normalization can happen later, when we
+        // normalize the quaternion.
+        real_part = 0.0;
+        w = (abs(from[0]) > abs(from[2])) ? VectorType(-from[1], from[0], 0)
+                                          : VectorType(0, -from[2], from[1]);
+    } else {
+        // Otherwise, build the quaternion the standard way.
+        w = Cross(from, to);
+    }
+
+    // Build and return a normalized quaternion.
+    // Note that Rotation::FromQuaternion automatically performs normalization.
+    return Rotation::FromQuaternion(QuaternionType(w[0], w[1], w[2], real_part));
+}
+
+Rotation::VectorType Rotation::operator*(const Rotation::VectorType& v) const
+{
+    return ApplyToVector(v);
+}
+
+} // namespace cardboard

Applications/Official/DEV_FW/source/airmouse/tracking/util/rotation.h

@@ -0,0 +1,156 @@
+/*
+ * Copyright 2019 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#ifndef CARDBOARD_SDK_UTIL_ROTATION_H_
+#define CARDBOARD_SDK_UTIL_ROTATION_H_
+
+#include "matrix_3x3.h"
+#include "vector.h"
+#include "vectorutils.h"
+
+namespace cardboard {
+
+// The Rotation class represents a rotation around a 3-dimensional axis. It
+// uses normalized quaternions internally to make the math robust.
+class Rotation {
+public:
+    // Convenience typedefs for vector of the correct type.
+    typedef Vector<3> VectorType;
+    typedef Vector<4> QuaternionType;
+
+    // The default constructor creates an identity Rotation, which has no effect.
+    Rotation() {
+        quat_.Set(0, 0, 0, 1);
+    }
+
+    // Returns an identity Rotation, which has no effect.
+    static Rotation Identity() {
+        return Rotation();
+    }
+
+    // Sets the Rotation from a quaternion (4D vector), which is first normalized.
+    void SetQuaternion(const QuaternionType& quaternion) {
+        quat_ = Normalized(quaternion);
+    }
+
+    // Returns the Rotation as a normalized quaternion (4D vector).
+    const QuaternionType& GetQuaternion() const {
+        return quat_;
+    }
+
+    // Sets the Rotation to rotate by the given angle around the given axis,
+    // following the right-hand rule. The axis does not need to be unit
+    // length. If it is zero length, this results in an identity Rotation.
+    void SetAxisAndAngle(const VectorType& axis, double angle);
+
+    // Returns the right-hand rule axis and angle corresponding to the
+    // Rotation. If the Rotation is the identity rotation, this returns the +X
+    // axis and an angle of 0.
+    void GetAxisAndAngle(VectorType* axis, double* angle) const;
+
+    // Convenience function that constructs and returns a Rotation given an axis
+    // and angle.
+    static Rotation FromAxisAndAngle(const VectorType& axis, double angle) {
+        Rotation r;
+        r.SetAxisAndAngle(axis, angle);
+        return r;
+    }
+
+    // Convenience function that constructs and returns a Rotation given a
+    // quaternion.
+    static Rotation FromQuaternion(const QuaternionType& quat) {
+        Rotation r;
+        r.SetQuaternion(quat);
+        return r;
+    }
+
+    // Convenience function that constructs and returns a Rotation given a
+    // rotation matrix R with $R^\top R = I && det(R) = 1$.
+    static Rotation FromRotationMatrix(const Matrix3x3& mat);
+
+    // Convenience function that constructs and returns a Rotation given Euler
+    // angles that are applied in the order of rotate-Z by roll, rotate-X by
+    // pitch, rotate-Y by yaw (same as GetRollPitchYaw).
+    static Rotation FromRollPitchYaw(double roll, double pitch, double yaw) {
+        VectorType x(1, 0, 0), y(0, 1, 0), z(0, 0, 1);
+        return FromAxisAndAngle(z, roll) * (FromAxisAndAngle(x, pitch) * FromAxisAndAngle(y, yaw));
+    }
+
+    // Convenience function that constructs and returns a Rotation given Euler
+    // angles that are applied in the order of rotate-Y by yaw, rotate-X by
+    // pitch, rotate-Z by roll (same as GetYawPitchRoll).
+    static Rotation FromYawPitchRoll(double yaw, double pitch, double roll) {
+        VectorType x(1, 0, 0), y(0, 1, 0), z(0, 0, 1);
+        return FromAxisAndAngle(y, yaw) * (FromAxisAndAngle(x, pitch) * FromAxisAndAngle(z, roll));
+    }
+
+    // Constructs and returns a Rotation that rotates one vector to another along
+    // the shortest arc. This returns an identity rotation if either vector has
+    // zero length.
+    static Rotation RotateInto(const VectorType& from, const VectorType& to);
+
+    // The negation operator returns the inverse rotation.
+    friend Rotation operator-(const Rotation& r) {
+        // Because we store normalized quaternions, the inverse is found by
+        // negating the vector part.
+        return Rotation(-r.quat_[0], -r.quat_[1], -r.quat_[2], r.quat_[3]);
+    }
+
+    // Appends a rotation to this one.
+    Rotation& operator*=(const Rotation& r) {
+        const QuaternionType& qr = r.quat_;
+        QuaternionType& qt = quat_;
+        SetQuaternion(QuaternionType(
+            qr[3] * qt[0] + qr[0] * qt[3] + qr[2] * qt[1] - qr[1] * qt[2],
+            qr[3] * qt[1] + qr[1] * qt[3] + qr[0] * qt[2] - qr[2] * qt[0],
+            qr[3] * qt[2] + qr[2] * qt[3] + qr[1] * qt[0] - qr[0] * qt[1],
+            qr[3] * qt[3] - qr[0] * qt[0] - qr[1] * qt[1] - qr[2] * qt[2]));
+        return *this;
+    }
+
+    // Binary multiplication operator - returns a composite Rotation.
+    friend const Rotation operator*(const Rotation& r0, const Rotation& r1) {
+        Rotation r = r0;
+        r *= r1;
+        return r;
+    }
+
+    // Multiply a Rotation and a Vector to get a Vector.
+    VectorType operator*(const VectorType& v) const;
+
+private:
+    // Private constructor that builds a Rotation from quaternion components.
+    Rotation(double q0, double q1, double q2, double q3)
+        : quat_(q0, q1, q2, q3) {
+    }
+
+    // Applies a Rotation to a Vector to rotate the Vector. Method borrowed from:
+    // http://blog.molecular-matters.com/2013/05/24/a-faster-quaternion-vector-multiplication/
+    VectorType ApplyToVector(const VectorType& v) const {
+        VectorType im(quat_[0], quat_[1], quat_[2]);
+        VectorType temp = 2.0 * Cross(im, v);
+        return v + quat_[3] * temp + Cross(im, temp);
+    }
+
+    // The rotation represented as a normalized quaternion. (Unit quaternions are
+    // required for constructing rotation matrices, so it makes sense to always
+    // store them that way.) The vector part is in the first 3 elements, and the
+    // scalar part is in the last element.
+    QuaternionType quat_;
+};
+
+} // namespace cardboard
+
+#endif // CARDBOARD_SDK_UTIL_ROTATION_H_

Applications/Official/DEV_FW/source/airmouse/tracking/util/vector.h

@@ -0,0 +1,251 @@
+/*
+ * Copyright 2019 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#ifndef CARDBOARD_SDK_UTIL_VECTOR_H_
+#define CARDBOARD_SDK_UTIL_VECTOR_H_
+
+#include <array>
+
+namespace cardboard {
+
+// Geometric N-dimensional Vector class.
+template <int Dimension>
+class Vector {
+public:
+    // The default constructor zero-initializes all elements.
+    Vector();
+
+    // Dimension-specific constructors that are passed individual element values.
+    constexpr Vector(double e0, double e1, double e2);
+    constexpr Vector(double e0, double e1, double e2, double e3);
+
+    // Constructor for a Vector of dimension N from a Vector of dimension N-1 and
+    // a scalar of the correct type, assuming N is at least 2.
+    // constexpr Vector(const Vector<Dimension - 1>& v, double s);
+
+    void Set(double e0, double e1, double e2); // Only when Dimension == 3.
+    void Set(double e0, double e1, double e2,
+             double e3); // Only when Dimension == 4.
+
+    // Mutable element accessor.
+    double& operator[](int index) {
+        return elem_[index];
+    }
+
+    // Element accessor.
+    double operator[](int index) const {
+        return elem_[index];
+    }
+
+    // Returns a Vector containing all zeroes.
+    static Vector Zero();
+
+    // Self-modifying operators.
+    void operator+=(const Vector& v) {
+        Add(v);
+    }
+    void operator-=(const Vector& v) {
+        Subtract(v);
+    }
+    void operator*=(double s) {
+        Multiply(s);
+    }
+    void operator/=(double s) {
+        Divide(s);
+    }
+
+    // Unary negation operator.
+    Vector operator-() const {
+        return Negation();
+    }
+
+    // Binary operators.
+    friend Vector operator+(const Vector& v0, const Vector& v1) {
+        return Sum(v0, v1);
+    }
+    friend Vector operator-(const Vector& v0, const Vector& v1) {
+        return Difference(v0, v1);
+    }
+    friend Vector operator*(const Vector& v, double s) {
+        return Scale(v, s);
+    }
+    friend Vector operator*(double s, const Vector& v) {
+        return Scale(v, s);
+    }
+    friend Vector operator*(const Vector& v, const Vector& s) {
+        return Product(v, s);
+    }
+    friend Vector operator/(const Vector& v, double s) {
+        return Divide(v, s);
+    }
+
+    // Self-modifying addition.
+    void Add(const Vector& v);
+    // Self-modifying subtraction.
+    void Subtract(const Vector& v);
+    // Self-modifying multiplication by a scalar.
+    void Multiply(double s);
+    // Self-modifying division by a scalar.
+    void Divide(double s);
+
+    // Unary negation.
+    Vector Negation() const;
+
+    // Binary component-wise multiplication.
+    static Vector Product(const Vector& v0, const Vector& v1);
+    // Binary component-wise addition.
+    static Vector Sum(const Vector& v0, const Vector& v1);
+    // Binary component-wise subtraction.
+    static Vector Difference(const Vector& v0, const Vector& v1);
+    // Binary multiplication by a scalar.
+    static Vector Scale(const Vector& v, double s);
+    // Binary division by a scalar.
+    static Vector Divide(const Vector& v, double s);
+
+private:
+    std::array<double, Dimension> elem_;
+};
+//------------------------------------------------------------------------------
+
+template <int Dimension>
+Vector<Dimension>::Vector() {
+    for(int i = 0; i < Dimension; i++) {
+        elem_[i] = 0;
+    }
+}
+
+template <int Dimension>
+constexpr Vector<Dimension>::Vector(double e0, double e1, double e2)
+    : elem_{e0, e1, e2} {
+}
+
+template <int Dimension>
+constexpr Vector<Dimension>::Vector(double e0, double e1, double e2, double e3)
+    : elem_{e0, e1, e2, e3} {
+}
+/*
+template <>
+constexpr Vector<4>::Vector(const Vector<3>& v, double s)
+    : elem_{v[0], v[1], v[2], s} {}
+*/
+template <int Dimension>
+void Vector<Dimension>::Set(double e0, double e1, double e2) {
+    elem_[0] = e0;
+    elem_[1] = e1;
+    elem_[2] = e2;
+}
+
+template <int Dimension>
+void Vector<Dimension>::Set(double e0, double e1, double e2, double e3) {
+    elem_[0] = e0;
+    elem_[1] = e1;
+    elem_[2] = e2;
+    elem_[3] = e3;
+}
+
+template <int Dimension>
+Vector<Dimension> Vector<Dimension>::Zero() {
+    Vector<Dimension> v;
+    return v;
+}
+
+template <int Dimension>
+void Vector<Dimension>::Add(const Vector& v) {
+    for(int i = 0; i < Dimension; i++) {
+        elem_[i] += v[i];
+    }
+}
+
+template <int Dimension>
+void Vector<Dimension>::Subtract(const Vector& v) {
+    for(int i = 0; i < Dimension; i++) {
+        elem_[i] -= v[i];
+    }
+}
+
+template <int Dimension>
+void Vector<Dimension>::Multiply(double s) {
+    for(int i = 0; i < Dimension; i++) {
+        elem_[i] *= s;
+    }
+}
+
+template <int Dimension>
+void Vector<Dimension>::Divide(double s) {
+    for(int i = 0; i < Dimension; i++) {
+        elem_[i] /= s;
+    }
+}
+
+template <int Dimension>
+Vector<Dimension> Vector<Dimension>::Negation() const {
+    Vector<Dimension> ret;
+    for(int i = 0; i < Dimension; i++) {
+        ret.elem_[i] = -elem_[i];
+    }
+    return ret;
+}
+
+template <int Dimension>
+Vector<Dimension> Vector<Dimension>::Product(const Vector& v0, const Vector& v1) {
+    Vector<Dimension> ret;
+    for(int i = 0; i < Dimension; i++) {
+        ret.elem_[i] = v0[i] * v1[i];
+    }
+    return ret;
+}
+
+template <int Dimension>
+Vector<Dimension> Vector<Dimension>::Sum(const Vector& v0, const Vector& v1) {
+    Vector<Dimension> ret;
+    for(int i = 0; i < Dimension; i++) {
+        ret.elem_[i] = v0[i] + v1[i];
+    }
+    return ret;
+}
+
+template <int Dimension>
+Vector<Dimension> Vector<Dimension>::Difference(const Vector& v0, const Vector& v1) {
+    Vector<Dimension> ret;
+    for(int i = 0; i < Dimension; i++) {
+        ret.elem_[i] = v0[i] - v1[i];
+    }
+    return ret;
+}
+
+template <int Dimension>
+Vector<Dimension> Vector<Dimension>::Scale(const Vector& v, double s) {
+    Vector<Dimension> ret;
+    for(int i = 0; i < Dimension; i++) {
+        ret.elem_[i] = v[i] * s;
+    }
+    return ret;
+}
+
+template <int Dimension>
+Vector<Dimension> Vector<Dimension>::Divide(const Vector& v, double s) {
+    Vector<Dimension> ret;
+    for(int i = 0; i < Dimension; i++) {
+        ret.elem_[i] = v[i] / s;
+    }
+    return ret;
+}
+
+typedef Vector<3> Vector3;
+typedef Vector<4> Vector4;
+
+} // namespace cardboard
+
+#endif // CARDBOARD_SDK_UTIL_VECTOR_H_

Applications/Official/DEV_FW/source/airmouse/tracking/util/vectorutils.cc

@@ -0,0 +1,40 @@
+/*
+ * Copyright 2019 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#include "vectorutils.h"
+
+namespace cardboard {
+
+// Returns the dot (inner) product of two Vectors.
+double Dot(const Vector<3>& v0, const Vector<3>& v1)
+{
+    return v0[0] * v1[0] + v0[1] * v1[1] + v0[2] * v1[2];
+}
+
+// Returns the dot (inner) product of two Vectors.
+double Dot(const Vector<4>& v0, const Vector<4>& v1)
+{
+    return v0[0] * v1[0] + v0[1] * v1[1] + v0[2] * v1[2] + v0[3] * v1[3];
+}
+
+// Returns the 3-dimensional cross product of 2 Vectors. Note that this is
+// defined only for 3-dimensional Vectors.
+Vector<3> Cross(const Vector<3>& v0, const Vector<3>& v1)
+{
+    return Vector<3>(v0[1] * v1[2] - v0[2] * v1[1], v0[2] * v1[0] - v0[0] * v1[2],
+        v0[0] * v1[1] - v0[1] * v1[0]);
+}
+
+} // namespace cardboard

Applications/Official/DEV_FW/source/airmouse/tracking/util/vectorutils.h

@@ -0,0 +1,76 @@
+/*
+ * Copyright 2019 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#ifndef CARDBOARD_SDK_UTIL_VECTORUTILS_H_
+#define CARDBOARD_SDK_UTIL_VECTORUTILS_H_
+
+//
+// This file contains free functions that operate on Vector instances.
+//
+
+#include <cmath>
+
+#include "vector.h"
+
+namespace cardboard {
+
+// Returns the dot (inner) product of two Vectors.
+double Dot(const Vector<3>& v0, const Vector<3>& v1);
+
+// Returns the dot (inner) product of two Vectors.
+double Dot(const Vector<4>& v0, const Vector<4>& v1);
+
+// Returns the 3-dimensional cross product of 2 Vectors. Note that this is
+// defined only for 3-dimensional Vectors.
+Vector<3> Cross(const Vector<3>& v0, const Vector<3>& v1);
+
+// Returns the square of the length of a Vector.
+template <int Dimension>
+double LengthSquared(const Vector<Dimension>& v) {
+    return Dot(v, v);
+}
+
+// Returns the geometric length of a Vector.
+template <int Dimension>
+double Length(const Vector<Dimension>& v) {
+    return sqrt(LengthSquared(v));
+}
+
+// the Vector untouched and returns false.
+template <int Dimension>
+bool Normalize(Vector<Dimension>* v) {
+    const double len = Length(*v);
+    if(len == 0) {
+        return false;
+    } else {
+        (*v) /= len;
+        return true;
+    }
+}
+
+// Returns a unit-length version of a Vector. If the given Vector has no
+// length, this returns a Zero() Vector.
+template <int Dimension>
+Vector<Dimension> Normalized(const Vector<Dimension>& v) {
+    Vector<Dimension> result = v;
+    if(Normalize(&result))
+        return result;
+    else
+        return Vector<Dimension>::Zero();
+}
+
+} // namespace cardboard
+
+#endif // CARDBOARD_SDK_UTIL_VECTORUTILS_H_

Applications/Official/DEV_FW/source/airmouse/views/bt_mouse.c

@@ -0,0 +1,303 @@
+#include "bt_mouse.h"
+#include "../tracking/main_loop.h"
+
+#include <furi.h>
+#include <furi_hal_bt.h>
+#include <furi_hal_bt_hid.h>
+#include <furi_hal_usb_hid.h>
+#include <bt/bt_service/bt.h>
+#include <gui/elements.h>
+#include <notification/notification.h>
+#include <notification/notification_messages.h>
+
+typedef struct ButtonEvent {
+    int8_t button;
+    bool state;
+} ButtonEvent;
+
+#define BTN_EVT_QUEUE_SIZE 32
+
+struct BtMouse {
+    View* view;
+    ViewDispatcher* view_dispatcher;
+    Bt* bt;
+    NotificationApp* notifications;
+    FuriMutex* mutex;
+    FuriThread* thread;
+    bool connected;
+
+    // Current mouse state
+    uint8_t btn;
+    int dx;
+    int dy;
+    int wheel;
+
+    // Circular buffer;
+    // (qhead == qtail) means either empty or overflow.
+    // We'll ignore overflow and treat it as empty.
+    int qhead;
+    int qtail;
+    ButtonEvent queue[BTN_EVT_QUEUE_SIZE];
+};
+
+#define BT_MOUSE_FLAG_INPUT_EVENT (1UL << 0)
+#define BT_MOUSE_FLAG_KILL_THREAD (1UL << 1)
+#define BT_MOUSE_FLAG_ALL (BT_MOUSE_FLAG_INPUT_EVENT | BT_MOUSE_FLAG_KILL_THREAD)
+
+#define MOUSE_MOVE_SHORT 5
+#define MOUSE_MOVE_LONG 20
+
+static void bt_mouse_notify_event(BtMouse* bt_mouse) {
+    FuriThreadId thread_id = furi_thread_get_id(bt_mouse->thread);
+    furi_assert(thread_id);
+    furi_thread_flags_set(thread_id, BT_MOUSE_FLAG_INPUT_EVENT);
+}
+
+static void bt_mouse_draw_callback(Canvas* canvas, void* context) {
+    UNUSED(context);
+    canvas_clear(canvas);
+    canvas_set_font(canvas, FontPrimary);
+    canvas_draw_str(canvas, 0, 10, "Bluetooth Mouse mode");
+    canvas_set_font(canvas, FontSecondary);
+    canvas_draw_str(canvas, 0, 63, "Hold [back] to exit");
+}
+
+static void bt_mouse_button_state(BtMouse* bt_mouse, int8_t button, bool state) {
+    ButtonEvent event;
+    event.button = button;
+    event.state = state;
+
+    if(bt_mouse->connected) {
+        furi_mutex_acquire(bt_mouse->mutex, FuriWaitForever);
+        bt_mouse->queue[bt_mouse->qtail++] = event;
+        bt_mouse->qtail %= BTN_EVT_QUEUE_SIZE;
+        furi_mutex_release(bt_mouse->mutex);
+        bt_mouse_notify_event(bt_mouse);
+    }
+}
+
+static void bt_mouse_process(BtMouse* bt_mouse, InputEvent* event) {
+    with_view_model(
+        bt_mouse->view,
+        void* model,
+        {
+            UNUSED(model);
+            if(event->key == InputKeyUp) {
+                if(event->type == InputTypePress) {
+                    bt_mouse_button_state(bt_mouse, HID_MOUSE_BTN_LEFT, true);
+                } else if(event->type == InputTypeRelease) {
+                    bt_mouse_button_state(bt_mouse, HID_MOUSE_BTN_LEFT, false);
+                }
+            } else if(event->key == InputKeyDown) {
+                if(event->type == InputTypePress) {
+                    bt_mouse_button_state(bt_mouse, HID_MOUSE_BTN_RIGHT, true);
+                } else if(event->type == InputTypeRelease) {
+                    bt_mouse_button_state(bt_mouse, HID_MOUSE_BTN_RIGHT, false);
+                }
+            } else if(event->key == InputKeyOk) {
+                if(event->type == InputTypePress) {
+                    bt_mouse_button_state(bt_mouse, HID_MOUSE_BTN_WHEEL, true);
+                } else if(event->type == InputTypeRelease) {
+                    bt_mouse_button_state(bt_mouse, HID_MOUSE_BTN_WHEEL, false);
+                }
+            }
+        },
+        true);
+}
+
+static bool bt_mouse_input_callback(InputEvent* event, void* context) {
+    furi_assert(context);
+    BtMouse* bt_mouse = context;
+    bool consumed = false;
+
+    if(event->type == InputTypeLong && event->key == InputKeyBack) {
+        furi_hal_bt_hid_mouse_release_all();
+    } else {
+        bt_mouse_process(bt_mouse, event);
+        consumed = true;
+    }
+
+    return consumed;
+}
+
+void bt_mouse_connection_status_changed_callback(BtStatus status, void* context) {
+    furi_assert(context);
+    BtMouse* bt_mouse = context;
+
+    bt_mouse->connected = (status == BtStatusConnected);
+    if(bt_mouse->connected) {
+        notification_internal_message(bt_mouse->notifications, &sequence_set_blue_255);
+        tracking_begin();
+        view_dispatcher_send_custom_event(bt_mouse->view_dispatcher, 0);
+    } else {
+        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) {
+    furi_assert(context);
+    BtMouse* bt_mouse = context;
+
+    if(bt_mouse->connected) {
+        furi_mutex_acquire(bt_mouse->mutex, FuriWaitForever);
+        bt_mouse->dx += dx;
+        bt_mouse->dy += dy;
+        furi_mutex_release(bt_mouse->mutex);
+        bt_mouse_notify_event(bt_mouse);
+    }
+
+    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;
+    } else if(t > 127) {
+        return 127;
+    }
+    return t;
+}
+
+static int32_t bt_mouse_thread_callback(void* context) {
+    furi_assert(context);
+    BtMouse* bt_mouse = (BtMouse*)context;
+
+    while(1) {
+        uint32_t flags =
+            furi_thread_flags_wait(BT_MOUSE_FLAG_ALL, FuriFlagWaitAny, FuriWaitForever);
+        if(flags & BT_MOUSE_FLAG_KILL_THREAD) {
+            break;
+        }
+        if(flags & BT_MOUSE_FLAG_INPUT_EVENT) {
+            furi_mutex_acquire(bt_mouse->mutex, FuriWaitForever);
+
+            ButtonEvent event;
+            bool send_buttons = false;
+            if(bt_mouse->qhead != bt_mouse->qtail) {
+                event = bt_mouse->queue[bt_mouse->qhead++];
+                bt_mouse->qhead %= BTN_EVT_QUEUE_SIZE;
+                send_buttons = true;
+            }
+
+            int8_t dx = clamp(bt_mouse->dx);
+            bt_mouse->dx -= dx;
+            int8_t dy = clamp(bt_mouse->dy);
+            bt_mouse->dy -= dy;
+            int8_t wheel = clamp(bt_mouse->wheel);
+            bt_mouse->wheel -= wheel;
+
+            furi_mutex_release(bt_mouse->mutex);
+
+            if(bt_mouse->connected && send_buttons) {
+                if(event.state) {
+                    furi_hal_bt_hid_mouse_press(event.button);
+                } else {
+                    furi_hal_bt_hid_mouse_release(event.button);
+                }
+            }
+
+            if(bt_mouse->connected && (dx != 0 || dy != 0)) {
+                furi_hal_bt_hid_mouse_move(dx, dy);
+            }
+
+            if(bt_mouse->connected && wheel != 0) {
+                furi_hal_bt_hid_mouse_scroll(wheel);
+            }
+        }
+    }
+
+    return 0;
+}
+
+void bt_mouse_thread_start(BtMouse* bt_mouse) {
+    furi_assert(bt_mouse);
+    bt_mouse->mutex = furi_mutex_alloc(FuriMutexTypeNormal);
+    bt_mouse->thread = furi_thread_alloc();
+    furi_thread_set_name(bt_mouse->thread, "BtSender");
+    furi_thread_set_stack_size(bt_mouse->thread, 1024);
+    furi_thread_set_context(bt_mouse->thread, bt_mouse);
+    furi_thread_set_callback(bt_mouse->thread, bt_mouse_thread_callback);
+    furi_thread_start(bt_mouse->thread);
+}
+
+void bt_mouse_thread_stop(BtMouse* bt_mouse) {
+    furi_assert(bt_mouse);
+    FuriThreadId thread_id = furi_thread_get_id(bt_mouse->thread);
+    furi_assert(thread_id);
+    furi_thread_flags_set(thread_id, BT_MOUSE_FLAG_KILL_THREAD);
+    furi_thread_join(bt_mouse->thread);
+    furi_thread_free(bt_mouse->thread);
+    furi_mutex_free(bt_mouse->mutex);
+}
+
+BtMouse* bt_mouse_alloc(ViewDispatcher* view_dispatcher) {
+    BtMouse* bt_mouse = malloc(sizeof(BtMouse));
+    memset(bt_mouse, 0, sizeof(BtMouse));
+
+    bt_mouse->view = view_alloc();
+    bt_mouse->view_dispatcher = view_dispatcher;
+    view_set_context(bt_mouse->view, bt_mouse);
+    view_set_draw_callback(bt_mouse->view, bt_mouse_draw_callback);
+    view_set_input_callback(bt_mouse->view, bt_mouse_input_callback);
+    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);
+}
+
+View* bt_mouse_get_view(BtMouse* bt_mouse) {
+    furi_assert(bt_mouse);
+    return bt_mouse->view;
+}

Applications/Official/DEV_FW/source/airmouse/views/bt_mouse.h

@@ -0,0 +1,14 @@
+#pragma once
+
+#include <gui/view.h>
+#include <gui/view_dispatcher.h>
+
+typedef struct BtMouse BtMouse;
+
+BtMouse* bt_mouse_alloc(ViewDispatcher* view_dispatcher);
+
+void bt_mouse_free(BtMouse* bt_mouse);
+
+View* bt_mouse_get_view(BtMouse* bt_mouse);
+
+void bt_mouse_set_connected_status(BtMouse* bt_mouse, bool connected);

Applications/Official/DEV_FW/source/airmouse/views/calibration.c

@@ -0,0 +1,69 @@
+#include "calibration.h"
+#include "../tracking/main_loop.h"
+#include "../air_mouse.h"
+
+#include <furi.h>
+#include <gui/elements.h>
+
+struct Calibration {
+    View* view;
+    ViewDispatcher* view_dispatcher;
+};
+
+static void calibration_draw_callback(Canvas* canvas, void* context) {
+    UNUSED(context);
+    canvas_clear(canvas);
+    canvas_set_font(canvas, FontPrimary);
+    canvas_draw_str(canvas, 0, 10, "Calibrating...");
+    canvas_set_font(canvas, FontSecondary);
+    canvas_draw_str(canvas, 0, 63, "Please wait");
+}
+
+void calibration_enter_callback(void* context) {
+    furi_assert(context);
+    Calibration* calibration = context;
+    calibration_begin();
+    view_dispatcher_send_custom_event(calibration->view_dispatcher, 0);
+}
+
+bool calibration_custom_callback(uint32_t event, void* context) {
+    UNUSED(event);
+    furi_assert(context);
+    Calibration* calibration = context;
+
+    if(calibration_step()) {
+        view_dispatcher_switch_to_view(calibration->view_dispatcher, AirMouseViewSubmenu);
+    } else {
+        view_dispatcher_send_custom_event(calibration->view_dispatcher, 0);
+    }
+
+    return true;
+}
+
+void calibration_exit_callback(void* context) {
+    furi_assert(context);
+    calibration_end();
+}
+
+Calibration* calibration_alloc(ViewDispatcher* view_dispatcher) {
+    Calibration* calibration = malloc(sizeof(Calibration));
+    calibration->view = view_alloc();
+    calibration->view_dispatcher = view_dispatcher;
+    view_set_context(calibration->view, calibration);
+    view_set_draw_callback(calibration->view, calibration_draw_callback);
+    view_set_enter_callback(calibration->view, calibration_enter_callback);
+    view_set_custom_callback(calibration->view, calibration_custom_callback);
+    view_set_exit_callback(calibration->view, calibration_exit_callback);
+    return calibration;
+}
+
+void calibration_free(Calibration* calibration) {
+    furi_assert(calibration);
+    view_free(calibration->view);
+    free(calibration);
+}
+
+View* calibration_get_view(Calibration* calibration) {
+    furi_assert(calibration);
+    return calibration->view;
+}

Applications/Official/DEV_FW/source/airmouse/views/calibration.h

@@ -0,0 +1,12 @@
+#pragma once
+
+#include <gui/view.h>
+#include <gui/view_dispatcher.h>
+
+typedef struct Calibration Calibration;
+
+Calibration* calibration_alloc(ViewDispatcher* view_dispatcher);
+
+void calibration_free(Calibration* calibration);
+
+View* calibration_get_view(Calibration* calibration);

Applications/Official/DEV_FW/source/airmouse/views/usb_mouse.c

@@ -0,0 +1,129 @@
+#include "usb_mouse.h"
+#include "../tracking/main_loop.h"
+
+#include <furi.h>
+#include <furi_hal_usb.h>
+#include <furi_hal_usb_hid.h>
+#include <gui/elements.h>
+
+struct UsbMouse {
+    View* view;
+    ViewDispatcher* view_dispatcher;
+    FuriHalUsbInterface* usb_mode_prev;
+};
+
+static void usb_mouse_draw_callback(Canvas* canvas, void* context) {
+    UNUSED(context);
+    canvas_clear(canvas);
+    canvas_set_font(canvas, FontPrimary);
+    canvas_draw_str(canvas, 0, 10, "USB Mouse mode");
+    canvas_set_font(canvas, FontSecondary);
+    canvas_draw_str(canvas, 0, 63, "Hold [back] to exit");
+}
+
+static void usb_mouse_process(UsbMouse* usb_mouse, InputEvent* event) {
+    with_view_model(
+        usb_mouse->view,
+        void* model,
+        {
+            UNUSED(model);
+            if(event->key == InputKeyUp) {
+                if(event->type == InputTypePress) {
+                    furi_hal_hid_mouse_press(HID_MOUSE_BTN_LEFT);
+                } else if(event->type == InputTypeRelease) {
+                    furi_hal_hid_mouse_release(HID_MOUSE_BTN_LEFT);
+                }
+            } else if(event->key == InputKeyDown) {
+                if(event->type == InputTypePress) {
+                    furi_hal_hid_mouse_press(HID_MOUSE_BTN_RIGHT);
+                } else if(event->type == InputTypeRelease) {
+                    furi_hal_hid_mouse_release(HID_MOUSE_BTN_RIGHT);
+                }
+            } else if(event->key == InputKeyOk) {
+                if(event->type == InputTypePress) {
+                    furi_hal_hid_mouse_press(HID_MOUSE_BTN_WHEEL);
+                } else if(event->type == InputTypeRelease) {
+                    furi_hal_hid_mouse_release(HID_MOUSE_BTN_WHEEL);
+                }
+            }
+        },
+        true);
+}
+
+static bool usb_mouse_input_callback(InputEvent* event, void* context) {
+    furi_assert(context);
+    UsbMouse* usb_mouse = context;
+    bool consumed = false;
+
+    if(event->type == InputTypeLong && event->key == InputKeyBack) {
+        // furi_hal_hid_mouse_release_all();
+    } else {
+        usb_mouse_process(usb_mouse, event);
+        consumed = true;
+    }
+
+    return consumed;
+}
+
+void usb_mouse_enter_callback(void* context) {
+    furi_assert(context);
+    UsbMouse* usb_mouse = context;
+
+    usb_mouse->usb_mode_prev = furi_hal_usb_get_config();
+    furi_hal_usb_unlock();
+    furi_check(furi_hal_usb_set_config(&usb_hid, NULL) == true);
+
+    tracking_begin();
+
+    view_dispatcher_send_custom_event(usb_mouse->view_dispatcher, 0);
+}
+
+bool usb_mouse_move(int8_t dx, int8_t dy, void* context) {
+    UNUSED(context);
+    return furi_hal_hid_mouse_move(dx, dy);
+}
+
+bool usb_mouse_custom_callback(uint32_t event, void* context) {
+    UNUSED(event);
+    furi_assert(context);
+    UsbMouse* usb_mouse = context;
+
+    tracking_step(usb_mouse_move, context);
+    furi_delay_ms(3); // Magic! Removing this will break the buttons
+
+    view_dispatcher_send_custom_event(usb_mouse->view_dispatcher, 0);
+    return true;
+}
+
+void usb_mouse_exit_callback(void* context) {
+    furi_assert(context);
+    UsbMouse* usb_mouse = context;
+
+    tracking_end();
+
+    furi_hal_usb_set_config(usb_mouse->usb_mode_prev, NULL);
+}
+
+UsbMouse* usb_mouse_alloc(ViewDispatcher* view_dispatcher) {
+    UsbMouse* usb_mouse = malloc(sizeof(UsbMouse));
+    usb_mouse->view = view_alloc();
+    usb_mouse->view_dispatcher = view_dispatcher;
+    view_set_context(usb_mouse->view, usb_mouse);
+    view_set_draw_callback(usb_mouse->view, usb_mouse_draw_callback);
+    view_set_input_callback(usb_mouse->view, usb_mouse_input_callback);
+    view_set_enter_callback(usb_mouse->view, usb_mouse_enter_callback);
+    view_set_custom_callback(usb_mouse->view, usb_mouse_custom_callback);
+    view_set_exit_callback(usb_mouse->view, usb_mouse_exit_callback);
+    return usb_mouse;
+}
+
+void usb_mouse_free(UsbMouse* usb_mouse) {
+    furi_assert(usb_mouse);
+    view_free(usb_mouse->view);
+    free(usb_mouse);
+}
+
+View* usb_mouse_get_view(UsbMouse* usb_mouse) {
+    furi_assert(usb_mouse);
+    return usb_mouse->view;
+}

Applications/Official/DEV_FW/source/airmouse/views/usb_mouse.h

@@ -0,0 +1,12 @@
+#pragma once
+
+#include <gui/view.h>
+#include <gui/view_dispatcher.h>
+
+typedef struct UsbMouse UsbMouse;
+
+UsbMouse* usb_mouse_alloc(ViewDispatcher* view_dispatcher);
+
+void usb_mouse_free(UsbMouse* usb_mouse);
+
+View* usb_mouse_get_view(UsbMouse* usb_mouse);

Applications/Official/DEV_FW/source/arkanoid/application.fam

@@ -0,0 +1,12 @@
+App(
+    appid="Arkanoid",
+    name="Arkanoid",
+    apptype=FlipperAppType.EXTERNAL,
+    entry_point="arkanoid_game_app",
+    cdefines=["APP_ARKANOID_GAME"],
+    requires=["gui"],
+    stack_size=1 * 1024,
+    order=20,
+    fap_icon="arkanoid_10px.png",
+    fap_category="Games",
+)

Applications/Official/DEV_FW/source/arkanoid/arkanoid_game.c

@@ -0,0 +1,474 @@
+#include <furi.h>
+#include <gui/gui.h>
+#include <input/input.h>
+#include <stdlib.h>
+#include <gui/view.h>
+#include <notification/notification.h>
+#include <notification/notification_messages.h>
+
+#define TAG "Arkanoid"
+
+#define FLIPPER_LCD_WIDTH 128
+#define FLIPPER_LCD_HEIGHT 64
+#define MAX_SPEED 3
+
+typedef enum { EventTypeTick, EventTypeKey } EventType;
+
+typedef struct {
+    //Brick Bounds used in collision detection
+    int leftBrick;
+    int rightBrick;
+    int topBrick;
+    int bottomBrick;
+    bool isHit[4][13]; //Array of if bricks are hit or not
+} BrickState;
+
+typedef struct {
+    int dx; //Initial movement of ball
+    int dy; //Initial movement of ball
+    int xb; //Balls starting possition
+    int yb; //Balls starting possition
+    bool released; //If the ball has been released by the player
+    //Ball Bounds used in collision detection
+    int leftBall;
+    int rightBall;
+    int topBall;
+    int bottomBall;
+} BallState;
+
+typedef struct {
+    BallState ball_state;
+    BrickState brick_state;
+    NotificationApp* notify;
+    unsigned int COLUMNS; //Columns of bricks
+    unsigned int ROWS; //Rows of bricks
+    bool initialDraw; //If the inital draw has happened
+    int xPaddle; //X position of paddle
+    char text[16]; //General string buffer
+    bool bounced; //Used to fix double bounce glitch
+    int lives; //Amount of lives
+    int level; //Current level
+    unsigned int score; //Score for the game
+    unsigned int brickCount; //Amount of bricks hit
+    int tick; //Tick counter
+    bool gameStarted; // Did the game start?
+    int speed; // Ball speed
+} ArkanoidState;
+
+typedef struct {
+    EventType type;
+    InputEvent input;
+} GameEvent;
+
+static const NotificationSequence sequence_short_sound = {
+    &message_note_c5,
+    &message_delay_50,
+    &message_sound_off,
+    NULL,
+};
+
+// generate number in range [min,max)
+int rand_range(int min, int max) {
+    return min + rand() % (max - min);
+}
+
+void move_ball(Canvas* canvas, ArkanoidState* st) {
+    st->tick++;
+
+    int current_speed = abs(st->speed - 1 - MAX_SPEED);
+    if(st->tick % current_speed != 0 && st->tick % (current_speed + 1) != 0) {
+        return;
+    }
+
+    if(st->ball_state.released) {
+        //Move ball
+        if(abs(st->ball_state.dx) == 2) {
+            st->ball_state.xb += st->ball_state.dx / 2;
+            // 2x speed is really 1.5 speed
+            if((st->tick / current_speed) % 2 == 0) st->ball_state.xb += st->ball_state.dx / 2;
+        } else {
+            st->ball_state.xb += st->ball_state.dx;
+        }
+        st->ball_state.yb = st->ball_state.yb + st->ball_state.dy;
+
+        //Set bounds
+        st->ball_state.leftBall = st->ball_state.xb;
+        st->ball_state.rightBall = st->ball_state.xb + 2;
+        st->ball_state.topBall = st->ball_state.yb;
+        st->ball_state.bottomBall = st->ball_state.yb + 2;
+
+        //Bounce off top edge
+        if(st->ball_state.yb <= 0) {
+            st->ball_state.yb = 2;
+            st->ball_state.dy = -st->ball_state.dy;
+        }
+
+        //Lose a life if bottom edge hit
+        if(st->ball_state.yb >= FLIPPER_LCD_HEIGHT) {
+            canvas_draw_frame(canvas, st->xPaddle, FLIPPER_LCD_HEIGHT - 1, 11, 1);
+            st->xPaddle = 54;
+            st->ball_state.yb = 60;
+            st->ball_state.released = false;
+            st->lives--;
+            st->gameStarted = false;
+
+            if(rand_range(0, 2) == 0) {
+                st->ball_state.dx = 1;
+            } else {
+                st->ball_state.dx = -1;
+            }
+        }
+
+        //Bounce off left side
+        if(st->ball_state.xb <= 0) {
+            st->ball_state.xb = 2;
+            st->ball_state.dx = -st->ball_state.dx;
+        }
+
+        //Bounce off right side
+        if(st->ball_state.xb >= FLIPPER_LCD_WIDTH - 2) {
+            st->ball_state.xb = FLIPPER_LCD_WIDTH - 4;
+            st->ball_state.dx = -st->ball_state.dx;
+            // arduboy.tunes.tone(523, 250);
+        }
+
+        //Bounce off paddle
+        if(st->ball_state.xb + 1 >= st->xPaddle && st->ball_state.xb <= st->xPaddle + 12 &&
+           st->ball_state.yb + 2 >= FLIPPER_LCD_HEIGHT - 1 &&
+           st->ball_state.yb <= FLIPPER_LCD_HEIGHT) {
+            st->ball_state.dy = -st->ball_state.dy;
+            st->ball_state.dx =
+                ((st->ball_state.xb - (st->xPaddle + 6)) / 3); //Applies spin on the ball
+            // prevent straight bounce, but not prevent roguuemaster from stealing
+            if(st->ball_state.dx == 0) {
+                st->ball_state.dx = (rand_range(0, 2) == 1) ? 1 : -1;
+            }
+        }
+
+        //Bounce off Bricks
+        for(unsigned int row = 0; row < st->ROWS; row++) {
+            for(unsigned int column = 0; column < st->COLUMNS; column++) {
+                if(!st->brick_state.isHit[row][column]) {
+                    //Sets Brick bounds
+                    st->brick_state.leftBrick = 10 * column;
+                    st->brick_state.rightBrick = 10 * column + 10;
+                    st->brick_state.topBrick = 6 * row + 1;
+                    st->brick_state.bottomBrick = 6 * row + 7;
+
+                    //If A collison has occured
+                    if(st->ball_state.topBall <= st->brick_state.bottomBrick &&
+                       st->ball_state.bottomBall >= st->brick_state.topBrick &&
+                       st->ball_state.leftBall <= st->brick_state.rightBrick &&
+                       st->ball_state.rightBall >= st->brick_state.leftBrick) {
+                        st->score += st->level;
+                        // Blink led when we hit some brick
+                        notification_message(st->notify, &sequence_short_sound);
+                        //notification_message(st->notify, &sequence_blink_white_100);
+
+                        st->brickCount++;
+                        st->brick_state.isHit[row][column] = true;
+                        canvas_draw_frame(canvas, 10 * column, 2 + 6 * row, 8, 4);
+
+                        //Vertical collision
+                        if(st->ball_state.bottomBall > st->brick_state.bottomBrick ||
+                           st->ball_state.topBall < st->brick_state.topBrick) {
+                            //Only bounce once each ball move
+                            if(!st->bounced) {
+                                st->ball_state.dy = -st->ball_state.dy;
+                                st->ball_state.yb += st->ball_state.dy;
+                                st->bounced = true;
+                            }
+                        }
+
+                        //Hoizontal collision
+                        if(st->ball_state.leftBall < st->brick_state.leftBrick ||
+                           st->ball_state.rightBall > st->brick_state.rightBrick) {
+                            //Only bounce once brick each ball move
+                            if(!st->bounced) {
+                                st->ball_state.dx = -st->ball_state.dx;
+                                st->ball_state.xb += st->ball_state.dx;
+                                st->bounced = true;
+                            }
+                        }
+                    }
+                }
+            }
+        }
+
+        //Reset Bounce
+        st->bounced = false;
+    } else {
+        //Ball follows paddle
+        st->ball_state.xb = st->xPaddle + 5;
+    }
+}
+
+void draw_lives(Canvas* canvas, ArkanoidState* arkanoid_state) {
+    if(arkanoid_state->lives == 3) {
+        canvas_draw_dot(canvas, 4, FLIPPER_LCD_HEIGHT - 7);
+        canvas_draw_dot(canvas, 3, FLIPPER_LCD_HEIGHT - 7);
+        canvas_draw_dot(canvas, 4, FLIPPER_LCD_HEIGHT - 8);
+        canvas_draw_dot(canvas, 3, FLIPPER_LCD_HEIGHT - 8);
+
+        canvas_draw_dot(canvas, 4, FLIPPER_LCD_HEIGHT - 11);
+        canvas_draw_dot(canvas, 3, FLIPPER_LCD_HEIGHT - 11);
+        canvas_draw_dot(canvas, 4, FLIPPER_LCD_HEIGHT - 12);
+        canvas_draw_dot(canvas, 3, FLIPPER_LCD_HEIGHT - 12);
+
+        canvas_draw_dot(canvas, 4, FLIPPER_LCD_HEIGHT - 15);
+        canvas_draw_dot(canvas, 3, FLIPPER_LCD_HEIGHT - 15);
+        canvas_draw_dot(canvas, 4, FLIPPER_LCD_HEIGHT - 16);
+        canvas_draw_dot(canvas, 3, FLIPPER_LCD_HEIGHT - 16);
+    } else if(arkanoid_state->lives == 2) {
+        canvas_draw_dot(canvas, 4, FLIPPER_LCD_HEIGHT - 7);
+        canvas_draw_dot(canvas, 3, FLIPPER_LCD_HEIGHT - 7);
+        canvas_draw_dot(canvas, 4, FLIPPER_LCD_HEIGHT - 8);
+        canvas_draw_dot(canvas, 3, FLIPPER_LCD_HEIGHT - 8);
+
+        canvas_draw_dot(canvas, 4, FLIPPER_LCD_HEIGHT - 11);
+        canvas_draw_dot(canvas, 3, FLIPPER_LCD_HEIGHT - 11);
+        canvas_draw_dot(canvas, 4, FLIPPER_LCD_HEIGHT - 12);
+        canvas_draw_dot(canvas, 3, FLIPPER_LCD_HEIGHT - 12);
+    } else {
+        canvas_draw_dot(canvas, 4, FLIPPER_LCD_HEIGHT - 7);
+        canvas_draw_dot(canvas, 3, FLIPPER_LCD_HEIGHT - 7);
+        canvas_draw_dot(canvas, 4, FLIPPER_LCD_HEIGHT - 8);
+        canvas_draw_dot(canvas, 3, FLIPPER_LCD_HEIGHT - 8);
+    }
+}
+
+void draw_score(Canvas* canvas, ArkanoidState* arkanoid_state) {
+    snprintf(arkanoid_state->text, sizeof(arkanoid_state->text), "%u", arkanoid_state->score);
+    canvas_draw_str_aligned(
+        canvas,
+        FLIPPER_LCD_WIDTH - 2,
+        FLIPPER_LCD_HEIGHT - 6,
+        AlignRight,
+        AlignBottom,
+        arkanoid_state->text);
+}
+
+void draw_ball(Canvas* canvas, ArkanoidState* ast) {
+    canvas_draw_dot(canvas, ast->ball_state.xb, ast->ball_state.yb);
+    canvas_draw_dot(canvas, ast->ball_state.xb + 1, ast->ball_state.yb);
+    canvas_draw_dot(canvas, ast->ball_state.xb, ast->ball_state.yb + 1);
+    canvas_draw_dot(canvas, ast->ball_state.xb + 1, ast->ball_state.yb + 1);
+
+    move_ball(canvas, ast);
+}
+
+void draw_paddle(Canvas* canvas, ArkanoidState* arkanoid_state) {
+    canvas_draw_frame(canvas, arkanoid_state->xPaddle, FLIPPER_LCD_HEIGHT - 1, 11, 1);
+}
+
+void reset_level(Canvas* canvas, ArkanoidState* arkanoid_state) {
+    //Undraw paddle
+    canvas_draw_frame(canvas, arkanoid_state->xPaddle, FLIPPER_LCD_HEIGHT - 1, 11, 1);
+
+    //Undraw ball
+    canvas_draw_dot(canvas, arkanoid_state->ball_state.xb, arkanoid_state->ball_state.yb);
+    canvas_draw_dot(canvas, arkanoid_state->ball_state.xb + 1, arkanoid_state->ball_state.yb);
+    canvas_draw_dot(canvas, arkanoid_state->ball_state.xb, arkanoid_state->ball_state.yb + 1);
+    canvas_draw_dot(canvas, arkanoid_state->ball_state.xb + 1, arkanoid_state->ball_state.yb + 1);
+
+    //Alter various variables to reset the game
+    arkanoid_state->xPaddle = 54;
+    arkanoid_state->ball_state.yb = 60;
+    arkanoid_state->brickCount = 0;
+    arkanoid_state->ball_state.released = false;
+
+    // Reset all brick hit states
+    for(unsigned int row = 0; row < arkanoid_state->ROWS; row++) {
+        for(unsigned int column = 0; column < arkanoid_state->COLUMNS; column++) {
+            arkanoid_state->brick_state.isHit[row][column] = false;
+        }
+    }
+}
+
+static void arkanoid_state_init(ArkanoidState* arkanoid_state) {
+    // Init notification
+    arkanoid_state->notify = furi_record_open(RECORD_NOTIFICATION);
+
+    // Set the initial game state
+    arkanoid_state->COLUMNS = 13;
+    arkanoid_state->ROWS = 4;
+    arkanoid_state->ball_state.dx = -1;
+    arkanoid_state->ball_state.dy = -1;
+    arkanoid_state->speed = 2;
+    arkanoid_state->bounced = false;
+    arkanoid_state->lives = 3;
+    arkanoid_state->level = 1;
+    arkanoid_state->score = 0;
+    arkanoid_state->COLUMNS = 13;
+    arkanoid_state->COLUMNS = 13;
+
+    // Reset initial state
+    arkanoid_state->initialDraw = false;
+    arkanoid_state->gameStarted = false;
+}
+
+static void arkanoid_draw_callback(Canvas* const canvas, void* ctx) {
+    ArkanoidState* arkanoid_state = acquire_mutex((ValueMutex*)ctx, 25);
+    if(arkanoid_state == NULL) {
+        return;
+    }
+
+    //Initial level draw
+    if(!arkanoid_state->initialDraw) {
+        arkanoid_state->initialDraw = true;
+
+        // Set default font for text
+        canvas_set_font(canvas, FontSecondary);
+
+        //Draws the new level
+        reset_level(canvas, arkanoid_state);
+    }
+
+    //Draws new bricks and resets their values
+    for(unsigned int row = 0; row < arkanoid_state->ROWS; row++) {
+        for(unsigned int column = 0; column < arkanoid_state->COLUMNS; column++) {
+            if(!arkanoid_state->brick_state.isHit[row][column]) {
+                canvas_draw_frame(canvas, 10 * column, 2 + 6 * row, 8, 4);
+            }
+        }
+    }
+
+    if(arkanoid_state->lives > 0) {
+        draw_paddle(canvas, arkanoid_state);
+        draw_ball(canvas, arkanoid_state);
+        draw_score(canvas, arkanoid_state);
+        draw_lives(canvas, arkanoid_state);
+
+        if(arkanoid_state->brickCount == arkanoid_state->ROWS * arkanoid_state->COLUMNS) {
+            arkanoid_state->level++;
+            reset_level(canvas, arkanoid_state);
+        }
+    } else {
+        reset_level(canvas, arkanoid_state);
+        arkanoid_state->initialDraw = false;
+        arkanoid_state->lives = 3;
+        arkanoid_state->score = 0;
+    }
+
+    release_mutex((ValueMutex*)ctx, arkanoid_state);
+}
+
+static void arkanoid_input_callback(InputEvent* input_event, FuriMessageQueue* event_queue) {
+    furi_assert(event_queue);
+
+    GameEvent event = {.type = EventTypeKey, .input = *input_event};
+    furi_message_queue_put(event_queue, &event, FuriWaitForever);
+}
+
+static void arkanoid_update_timer_callback(FuriMessageQueue* event_queue) {
+    furi_assert(event_queue);
+
+    GameEvent event = {.type = EventTypeTick};
+    furi_message_queue_put(event_queue, &event, 0);
+}
+
+int32_t arkanoid_game_app(void* p) {
+    UNUSED(p);
+    int32_t return_code = 0;
+
+    FuriMessageQueue* event_queue = furi_message_queue_alloc(8, sizeof(GameEvent));
+
+    ArkanoidState* arkanoid_state = malloc(sizeof(ArkanoidState));
+    arkanoid_state_init(arkanoid_state);
+
+    ValueMutex state_mutex;
+    if(!init_mutex(&state_mutex, arkanoid_state, sizeof(ArkanoidState))) {
+        FURI_LOG_E(TAG, "Cannot create mutex\r\n");
+        return_code = 255;
+        goto free_and_exit;
+    }
+
+    // Set system callbacks
+    ViewPort* view_port = view_port_alloc();
+    view_port_draw_callback_set(view_port, arkanoid_draw_callback, &state_mutex);
+    view_port_input_callback_set(view_port, arkanoid_input_callback, event_queue);
+
+    FuriTimer* timer =
+        furi_timer_alloc(arkanoid_update_timer_callback, FuriTimerTypePeriodic, event_queue);
+    furi_timer_start(timer, furi_kernel_get_tick_frequency() / 22);
+
+    // Open GUI and register view_port
+    Gui* gui = furi_record_open(RECORD_GUI);
+    gui_add_view_port(gui, view_port, GuiLayerFullscreen);
+
+    GameEvent event;
+    for(bool processing = true; processing;) {
+        FuriStatus event_status = furi_message_queue_get(event_queue, &event, 100);
+        ArkanoidState* arkanoid_state = (ArkanoidState*)acquire_mutex_block(&state_mutex);
+
+        if(event_status == FuriStatusOk) {
+            // Key events
+            if(event.type == EventTypeKey) {
+                if(event.input.type == InputTypePress || event.input.type == InputTypeLong ||
+                   event.input.type == InputTypeRepeat) {
+                    switch(event.input.key) {
+                    case InputKeyBack:
+                        processing = false;
+                        break;
+                    case InputKeyRight:
+                        if(arkanoid_state->xPaddle < FLIPPER_LCD_WIDTH - 12) {
+                            arkanoid_state->xPaddle += 8;
+                        }
+                        break;
+                    case InputKeyLeft:
+                        if(arkanoid_state->xPaddle > 0) {
+                            arkanoid_state->xPaddle -= 8;
+                        }
+                        break;
+                    case InputKeyUp:
+                        if(arkanoid_state->speed < MAX_SPEED) {
+                            arkanoid_state->speed++;
+                        }
+                        break;
+                    case InputKeyDown:
+                        if(arkanoid_state->speed > 1) {
+                            arkanoid_state->speed--;
+                        }
+                        break;
+                    case InputKeyOk:
+                        if(arkanoid_state->gameStarted == false) {
+                            //Release ball if FIRE pressed
+                            arkanoid_state->ball_state.released = true;
+
+                            //Apply random direction to ball on release
+                            if(rand_range(0, 2) == 0) {
+                                arkanoid_state->ball_state.dx = 1;
+                            } else {
+                                arkanoid_state->ball_state.dx = -1;
+                            }
+
+                            //Makes sure the ball heads upwards
+                            arkanoid_state->ball_state.dy = -1;
+                            //start the game flag
+                            arkanoid_state->gameStarted = true;
+                        }
+                        break;
+                    default:
+                        break;
+                    }
+                }
+            }
+        }
+
+        view_port_update(view_port);
+        release_mutex(&state_mutex, arkanoid_state);
+    }
+    furi_timer_free(timer);
+    view_port_enabled_set(view_port, false);
+    gui_remove_view_port(gui, view_port);
+    furi_record_close(RECORD_GUI);
+    furi_record_close(RECORD_NOTIFICATION);
+    view_port_free(view_port);
+    delete_mutex(&state_mutex);
+
+free_and_exit:
+    free(arkanoid_state);
+    furi_message_queue_free(event_queue);
+
+    return return_code;
+}

Applications/Official/DEV_FW/source/badapple/application.fam

@@ -0,0 +1,10 @@
+App(
+    appid="BadApple",
+    name="Bad Apple",
+    apptype=FlipperAppType.EXTERNAL,
+    entry_point="bad_apple_main",
+    requires=["gui"],
+    stack_size=1 * 1024,
+    fap_icon="bad_apple_10px.png",
+    fap_category="Misc"
+)

Applications/Official/DEV_FW/source/badapple/bad_apple.c

@@ -0,0 +1,183 @@
+#include <stdlib.h>
+#include <string.h>
+#include <limits.h>
+
+#include <furi.h>
+#include <furi_hal.h>
+#include <stm32wbxx_ll_tim.h>
+
+#include <gui/gui.h>
+#include <input/input.h>
+#include <storage/storage.h>
+#include <notification/notification.h>
+#include <notification/notification_messages.h>
+
+#include "bad_apple.h"
+#include "video_player.h"
+
+#define TAG "badapple"
+
+typedef enum {
+    BadAppleEventTypeInput,
+    BadAppleEventTypeTick,
+} BadAppleEventType;
+
+typedef struct {
+    BadAppleEventType type;
+    InputEvent* input;
+} BadAppleEvent;
+
+// Screen is 128x64 px
+static void app_draw_callback(Canvas* canvas, void* ctx) {
+    BadAppleCtx* inst = ctx;
+
+    canvas_clear(canvas);
+    canvas_draw_xbm(canvas, VIDEO_X, VIDEO_Y, VIDEO_WIDTH, SCREEN_HEIGHT, inst->framebuffer);
+    canvas_draw_box(canvas, 0, 0, VIDEO_X, SCREEN_HEIGHT);
+    canvas_draw_box(
+        canvas, VIDEO_X + VIDEO_WIDTH, 0, SCREEN_WIDTH - VIDEO_WIDTH - VIDEO_X, SCREEN_HEIGHT);
+}
+
+static void app_input_callback(InputEvent* input_event, void* ctx) {
+    furi_assert(ctx);
+
+    FuriMessageQueue* event_queue = ctx;
+    BadAppleEvent event = {.type = BadAppleEventTypeInput, .input = input_event};
+    furi_message_queue_put(event_queue, &event, FuriWaitForever);
+}
+
+BadAppleCtx* bad_apple_ctx_alloc(void) {
+    BadAppleCtx* inst = malloc(sizeof(BadAppleCtx));
+    memset(inst, 0, sizeof(BadAppleCtx));
+
+    if(inst) {
+        inst->storage = furi_record_open(RECORD_STORAGE);
+        inst->video_file = storage_file_alloc(inst->storage);
+        inst->file_buffer_offset = sizeof(inst->file_buffer);
+    }
+
+    return inst;
+}
+
+void bad_apple_ctx_free(BadAppleCtx* inst) {
+    if(inst) {
+        storage_file_free(inst->video_file);
+        furi_record_close(RECORD_STORAGE);
+        free(inst);
+    }
+}
+
+void bad_apple_load_next_video_chunk(BadAppleCtx* inst) {
+    size_t bytes_to_read = sizeof(inst->file_buffer);
+    uint8_t* buf_ptr = inst->file_buffer;
+    while(bytes_to_read > 0) {
+        uint16_t curr_bytes_to_read = bytes_to_read > (UINT16_MAX / 2 + 1) ? (UINT16_MAX / 2 + 1) :
+                                                                             bytes_to_read;
+        uint16_t read = storage_file_read(inst->video_file, buf_ptr, curr_bytes_to_read);
+        bytes_to_read -= read;
+        buf_ptr += read;
+        if(read == 0) break;
+    }
+    inst->file_buffer_offset = 0;
+}
+
+uint8_t bad_apple_read_byte(BadAppleCtx* inst) {
+    if(inst->file_buffer_offset >= sizeof(inst->file_buffer)) {
+        bad_apple_load_next_video_chunk(inst);
+    }
+    return inst->file_buffer[inst->file_buffer_offset++];
+}
+
+void bad_apple_timer_isr(void* ctx) {
+    FuriMessageQueue* event_queue = ctx;
+    BadAppleEvent event = {.type = BadAppleEventTypeTick};
+    furi_message_queue_put(event_queue, &event, 0);
+    LL_TIM_ClearFlag_UPDATE(TIM2);
+}
+
+void bad_apple_timer_setup(BadAppleCtx* inst, void* ctx) {
+    UNUSED(inst);
+
+    LL_TIM_InitTypeDef tim_init = {
+        .Prescaler = 63999,
+        .CounterMode = LL_TIM_COUNTERMODE_UP,
+        .Autoreload = 30,
+    };
+
+    LL_TIM_Init(TIM2, &tim_init);
+    LL_TIM_SetClockSource(TIM2, LL_TIM_CLOCKSOURCE_INTERNAL);
+    LL_TIM_DisableCounter(TIM2);
+    LL_TIM_SetCounter(TIM2, 0);
+    furi_hal_interrupt_set_isr(FuriHalInterruptIdTIM2, bad_apple_timer_isr, ctx);
+    LL_TIM_EnableIT_UPDATE(TIM2);
+}
+
+void bad_apple_timer_deinit(void) {
+    LL_TIM_DisableCounter(TIM2);
+    LL_TIM_DisableIT_UPDATE(TIM2);
+    furi_hal_interrupt_set_isr(FuriHalInterruptIdTIM2, NULL, NULL);
+    LL_TIM_DeInit(TIM2);
+}
+
+int32_t bad_apple_main(void* p) {
+    UNUSED(p);
+    BadAppleCtx* ctx = bad_apple_ctx_alloc();
+    FuriMessageQueue* event_queue = furi_message_queue_alloc(8, sizeof(BadAppleEvent));
+
+    // Configure view port
+    ViewPort* view_port = view_port_alloc();
+    view_port_draw_callback_set(view_port, app_draw_callback, ctx);
+    view_port_input_callback_set(view_port, app_input_callback, event_queue);
+
+    // Register view port in GUI
+    Gui* gui = furi_record_open(RECORD_GUI);
+    gui_add_view_port(gui, view_port, GuiLayerFullscreen);
+
+    NotificationApp* notification = furi_record_open(RECORD_NOTIFICATION);
+
+    // Frame rate: 32 FPS
+    bad_apple_timer_setup(ctx, event_queue);
+
+    bool is_opened = storage_file_open(ctx->video_file, VIDEO_PATH, FSAM_READ, FSOM_OPEN_EXISTING);
+    if(is_opened) {
+        BadAppleEvent event;
+        notification_message(notification, &sequence_display_backlight_enforce_on);
+
+        bool running = true;
+        LL_TIM_EnableCounter(TIM2);
+        while(running) {
+            if(furi_message_queue_get(event_queue, &event, 100) == FuriStatusOk) {
+                if(event.type == BadAppleEventTypeInput) {
+                    InputEvent* input_event = event.input;
+                    if(input_event->type == InputTypeLong) {
+                        if(input_event->key == InputKeyBack) {
+                            running = false;
+                        }
+                    }
+                } else if(event.type == BadAppleEventTypeTick) {
+                    // FURI_LOG_D(TAG, "Update frame");
+                    if(!vp_play_frame(ctx)) {
+                        running = false;
+                    }
+                }
+            }
+            view_port_update(view_port);
+        }
+        LL_TIM_DisableCounter(TIM2);
+        notification_message(notification, &sequence_display_backlight_enforce_auto);
+        storage_file_close(ctx->video_file);
+    }
+
+    furi_record_close(RECORD_NOTIFICATION);
+
+    view_port_enabled_set(view_port, false);
+    gui_remove_view_port(gui, view_port);
+    view_port_free(view_port);
+    furi_message_queue_free(event_queue);
+
+    furi_record_close(RECORD_GUI);
+    bad_apple_timer_deinit();
+    bad_apple_ctx_free(ctx);
+
+    return 0;
+}

Applications/Official/DEV_FW/source/badapple/bad_apple.h

@@ -0,0 +1,25 @@
+#pragma once
+
+#include <stdint.h>
+
+#include <storage/storage.h>
+
+#define SCREEN_WIDTH 128
+#define SCREEN_HEIGHT 64
+#define VIDEO_WIDTH 104
+#define VIDEO_HEIGHT 80
+#define FILE_BUFFER_SIZE (1024 * 64)
+#define VIDEO_PATH EXT_PATH("apps_data/bad_apple/video.bin")
+#define VIDEO_X ((SCREEN_WIDTH - VIDEO_WIDTH) / 2)
+#define VIDEO_Y 0
+
+typedef struct {
+    uint8_t framebuffer[VIDEO_HEIGHT * VIDEO_WIDTH / 8];
+    uint8_t file_buffer[FILE_BUFFER_SIZE];
+    uint32_t file_buffer_offset;
+    uint32_t frame_write_offset; // bit index
+    Storage* storage;
+    File* video_file;
+} BadAppleCtx;
+
+uint8_t bad_apple_read_byte(BadAppleCtx* inst);

Applications/Official/DEV_FW/source/badapple/video_player.c

@@ -0,0 +1,137 @@
+#include <stdbool.h>
+
+#include <furi.h>
+
+#include "bad_apple.h"
+#include "video_player.h"
+
+#define TAG "video_player"
+
+static void vp_decode_rle(BadAppleCtx* ctx);
+static void vp_decode_delta(BadAppleCtx* ctx);
+
+int vp_play_frame(BadAppleCtx* ctx) {
+    // Check frame type
+    // FURI_LOG_D(TAG, "Buffer offset: %04lx", ctx->file_buffer_offset);
+    uint8_t b = bad_apple_read_byte(ctx);
+    // FURI_LOG_D(TAG, "Frame byte: %02x", b);
+    switch(b) {
+    case 1: // PFrame (delta)
+        vp_decode_delta(ctx);
+        break;
+    case 2: // IFrame (rle)
+        vp_decode_rle(ctx);
+        break;
+    case 3: // DFrame (duplicate)
+        break;
+    case 4: // next page (not supported)
+    case 5: // end
+    default:
+        return 0;
+    }
+
+    return 1;
+}
+
+static inline void vp_write_pixel(BadAppleCtx* ctx, bool color) {
+    if(color) {
+        // White
+        ctx->framebuffer[ctx->frame_write_offset / 8] &= ~(1 << (ctx->frame_write_offset % 8));
+    } else {
+        // Black
+        ctx->framebuffer[ctx->frame_write_offset / 8] |= (1 << (ctx->frame_write_offset % 8));
+    }
+    ++ctx->frame_write_offset;
+}
+
+static inline void vp_set_rect_fast(BadAppleCtx* ctx, int x, int y) {
+    ctx->frame_write_offset = y * VIDEO_WIDTH + x;
+}
+
+static void vp_decode_rle(BadAppleCtx* ctx) {
+    int i = 0;
+    int repeat_byte = bad_apple_read_byte(ctx);
+
+    // Set rect to video area
+    vp_set_rect_fast(ctx, 0, 0);
+
+    while(i < VIDEO_WIDTH * VIDEO_HEIGHT) {
+        int count;
+        unsigned pixels;
+        int j;
+        int b = bad_apple_read_byte(ctx);
+
+        if(b == repeat_byte) {
+            count = bad_apple_read_byte(ctx);
+            if(count == 0) count = 256;
+            pixels = bad_apple_read_byte(ctx);
+        } else {
+            count = 1;
+            pixels = (unsigned)b;
+        }
+
+        for(j = 0; j < count; ++j) {
+            bool out_color;
+            int k;
+            unsigned loop_pixels = pixels;
+
+            for(k = 0; k < 8; ++k) {
+                if(loop_pixels & 0x80)
+                    out_color = COLOR_WHITE;
+                else
+                    out_color = COLOR_BLACK;
+
+                vp_write_pixel(ctx, out_color);
+                loop_pixels <<= 1;
+                ++i;
+            }
+        }
+    }
+}
+
+static void vp_decode_delta(BadAppleCtx* ctx) {
+    int frame_header[(VIDEO_HEIGHT + 7) / 8];
+    uint i;
+    int fh_byte = 0;
+    int fh_index = 0;
+
+    for(i = 0; i < sizeof(frame_header) / sizeof(frame_header[0]); ++i) {
+        frame_header[i] = bad_apple_read_byte(ctx);
+    }
+
+    for(i = 0; i < VIDEO_HEIGHT; ++i) {
+        if(i % 8 == 0) fh_byte = frame_header[fh_index++];
+
+        if(fh_byte & 0x80) {
+            int j;
+            int sl_byte = 0;
+
+            for(j = 0; j < VIDEO_WIDTH;) {
+                if(j % (8 * 8) == 0) sl_byte = bad_apple_read_byte(ctx);
+
+                if(sl_byte & 0x80) {
+                    unsigned out_color;
+                    int k;
+                    unsigned pixel_group = bad_apple_read_byte(ctx);
+
+                    // Note: this needs to be revised for screen width not multiple of 8
+                    vp_set_rect_fast(ctx, j, i);
+
+                    for(k = 0; k < 8 && j < VIDEO_WIDTH; ++k, ++j) {
+                        if(pixel_group & 0x80)
+                            out_color = COLOR_WHITE;
+                        else
+                            out_color = COLOR_BLACK;
+
+                        vp_write_pixel(ctx, out_color);
+                        pixel_group <<= 1;
+                    }
+                } else {
+                    j += 8;
+                }
+                sl_byte <<= 1;
+            }
+        }
+        fh_byte <<= 1;
+    }
+}

Applications/Official/DEV_FW/source/badapple/video_player.h

@@ -0,0 +1,6 @@
+#pragma once
+
+#define COLOR_WHITE true
+#define COLOR_BLACK false
+
+int vp_play_frame(BadAppleCtx* ctx);