├── icon.png ├── screenshot1.png ├── screenshot2.png ├── screenshot3.png ├── schematic ├── render.png ├── flipper.jpg ├── schematic.png ├── airmouse_gerber.zip ├── airmouse_bom.csv └── airmouse_cpl.csv ├── views ├── usb_mouse.h ├── calibration.h ├── bt_mouse.h ├── calibration.c ├── usb_mouse.c └── bt_mouse.c ├── .gitmodules ├── tracking ├── main_loop.h ├── imu │ ├── imu.h │ ├── imu.c │ ├── bmi160.c │ ├── lsm6dso.c │ └── lsm6ds3trc.c ├── util │ ├── matrix_4x4.h │ ├── logging.h │ ├── vectorutils.cc │ ├── vectorutils.h │ ├── matrix_4x4.cc │ ├── matrixutils.h │ ├── matrix_3x3.cc │ ├── rotation.cc │ ├── matrix_3x3.h │ ├── matrixutils.cc │ ├── rotation.h │ └── vector.h ├── sensors │ ├── gyroscope_data.h │ ├── accelerometer_data.h │ ├── mean_filter.cc │ ├── mean_filter.h │ ├── pose_state.h │ ├── median_filter.h │ ├── median_filter.cc │ ├── pose_prediction.h │ ├── lowpass_filter.cc │ ├── lowpass_filter.h │ ├── pose_prediction.cc │ ├── gyroscope_bias_estimator.h │ ├── sensor_fusion_ekf.h │ ├── gyroscope_bias_estimator.cc │ └── sensor_fusion_ekf.cc ├── orientation_tracker.h ├── calibration_data.cc ├── orientation_tracker.cc ├── calibration_data.h └── main_loop.cc ├── air_mouse.h ├── application.fam ├── CHANGELOG.md ├── DESCRIPTION.md ├── README.md ├── air_mouse.c └── LICENSE /icon.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ginkage/FlippAirMouse/HEAD/icon.png -------------------------------------------------------------------------------- /screenshot1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ginkage/FlippAirMouse/HEAD/screenshot1.png -------------------------------------------------------------------------------- /screenshot2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ginkage/FlippAirMouse/HEAD/screenshot2.png -------------------------------------------------------------------------------- /screenshot3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ginkage/FlippAirMouse/HEAD/screenshot3.png -------------------------------------------------------------------------------- /schematic/render.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ginkage/FlippAirMouse/HEAD/schematic/render.png -------------------------------------------------------------------------------- /schematic/flipper.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ginkage/FlippAirMouse/HEAD/schematic/flipper.jpg -------------------------------------------------------------------------------- /schematic/schematic.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ginkage/FlippAirMouse/HEAD/schematic/schematic.png -------------------------------------------------------------------------------- /schematic/airmouse_gerber.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ginkage/FlippAirMouse/HEAD/schematic/airmouse_gerber.zip -------------------------------------------------------------------------------- /views/usb_mouse.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | typedef struct UsbMouse UsbMouse; 7 | 8 | UsbMouse* usb_mouse_alloc(ViewDispatcher* view_dispatcher); 9 | 10 | void usb_mouse_free(UsbMouse* usb_mouse); 11 | 12 | View* usb_mouse_get_view(UsbMouse* usb_mouse); 13 | -------------------------------------------------------------------------------- /views/calibration.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | typedef struct Calibration Calibration; 7 | 8 | Calibration* calibration_alloc(ViewDispatcher* view_dispatcher); 9 | 10 | void calibration_free(Calibration* calibration); 11 | 12 | View* calibration_get_view(Calibration* calibration); 13 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "lib/bmi160-api"] 2 | path = lib/bmi160-api 3 | url = https://github.com/boschsensortec/BMI160_SensorAPI.git 4 | [submodule "lib/lsm6dso-api"] 5 | path = lib/lsm6dso-api 6 | url = https://github.com/STMicroelectronics/stm32-lsm6dso.git 7 | [submodule "lib/lsm6ds3tr-api"] 8 | path = lib/lsm6ds3tr-api 9 | url = https://github.com/STMicroelectronics/lsm6ds3tr-c-pid.git 10 | -------------------------------------------------------------------------------- /views/bt_mouse.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | typedef struct BtMouse BtMouse; 7 | 8 | BtMouse* bt_mouse_alloc(ViewDispatcher* view_dispatcher); 9 | 10 | void bt_mouse_free(BtMouse* bt_mouse); 11 | 12 | View* bt_mouse_get_view(BtMouse* bt_mouse); 13 | 14 | void bt_mouse_set_connected_status(BtMouse* bt_mouse, bool connected); 15 | 16 | void bt_mouse_remove_pairing(void); 17 | -------------------------------------------------------------------------------- /tracking/main_loop.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #ifdef __cplusplus 6 | extern "C" { 7 | #endif 8 | 9 | typedef bool (*MouseMoveCallback)(int8_t x, int8_t y, void* context); 10 | 11 | void calibration_begin(); 12 | bool calibration_step(); 13 | void calibration_end(); 14 | 15 | void tracking_begin(); 16 | void tracking_step(MouseMoveCallback mouse_move, void* context); 17 | void tracking_end(); 18 | 19 | #ifdef __cplusplus 20 | } 21 | #endif 22 | -------------------------------------------------------------------------------- /schematic/airmouse_bom.csv: -------------------------------------------------------------------------------- 1 | ID Name Designator Footprint Quantity Manufacturer Part Manufacturer Supplier Supplier Part Price 2 | "1" "100nF" "C1" "C0603" "1" "CC0603KRX7R9BB104" "YAGEO" "LCSC" "C14663" "" 3 | "2" "Header-Male-2.54_1x10" "H1" "HDR-TH_10P-P2.54-V-M-1" "1" "Headers  Pins2.54mm1*10P" "" "LCSC" "C57369" "" 4 | "3" "3.9kΩ" "R1,R2,R3,R4" "R0603" "4" "0603WAF3901T5E" "UNI-ROYAL(Uniroyal Elec)" "LCSC" "C23018" "" 5 | "4" "BMI160" "U1" "LGA-14_L3.0-W2.5-P0.50-BL" "1" "BMI160" "Bosch" "LCSC" "C94021" "" 6 | -------------------------------------------------------------------------------- /tracking/imu/imu.h: -------------------------------------------------------------------------------- 1 | #ifndef IMU_H 2 | #define IMU_H 3 | 4 | #include 5 | #include 6 | 7 | #ifdef __cplusplus 8 | extern "C" { 9 | #endif 10 | 11 | struct imu_t 12 | { 13 | unsigned int address; 14 | bool (*begin)(void); 15 | void (*end)(void); 16 | int (*read)(double* vec); 17 | char* name; 18 | }; 19 | 20 | #define ACC_DATA_READY (1 << 0) 21 | #define GYR_DATA_READY (1 << 1) 22 | 23 | static const double DEG_TO_RAD = 0.017453292519943295769236907684886; 24 | static const double GRAVITY = 9.81; 25 | 26 | bool imu_begin(); 27 | void imu_end(); 28 | int imu_read(double* vec); 29 | 30 | #ifdef __cplusplus 31 | } 32 | #endif 33 | #endif // IMU_H 34 | -------------------------------------------------------------------------------- /schematic/airmouse_cpl.csv: -------------------------------------------------------------------------------- 1 | Designator Footprint Mid X Mid Y Ref X Ref Y Pad X Pad Y Layer Rotation Comment 2 | "C1" "C0603" "22.86mm" "-6.86mm" "22.86mm" "-6.86mm" "22.16mm" "-6.86mm" "T" "0" "100nF" 3 | "H1" "HDR-TH_10P-P2.54-V-M-1" "13.97mm" "-2.54mm" "13.97mm" "-2.54mm" "25.4mm" "-2.54mm" "B" "180" "Header-Male-2.54_1x10" 4 | "R1" "R0603" "7.62mm" "-6.86mm" "7.62mm" "-6.86mm" "7.62mm" "-7.61mm" "T" "90" "3.9kΩ" 5 | "R2" "R0603" "5.08mm" "-6.86mm" "5.08mm" "-6.86mm" "5.08mm" "-7.61mm" "T" "90" "3.9kΩ" 6 | "R3" "R0603" "10.16mm" "-6.86mm" "10.16mm" "-6.86mm" "10.16mm" "-7.61mm" "T" "90" "3.9kΩ" 7 | "R4" "R0603" "17.78mm" "-6.86mm" "17.78mm" "-6.86mm" "17.78mm" "-6.1mm" "T" "270" "3.9kΩ" 8 | "U1" "LGA-14_L3.0-W2.5-P0.50-BL" "13.97mm" "-6.86mm" "13.97mm" "-6.86mm" "12.58mm" "-6.11mm" "T" "270" "BMI160" 9 | -------------------------------------------------------------------------------- /air_mouse.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include "views/bt_mouse.h" 10 | #include "views/usb_mouse.h" 11 | #include "views/calibration.h" 12 | 13 | typedef struct { 14 | Gui* gui; 15 | ViewDispatcher* view_dispatcher; 16 | Submenu* submenu; 17 | DialogEx* dialog; 18 | DialogEx* error_dialog; 19 | BtMouse* bt_mouse; 20 | UsbMouse* usb_mouse; 21 | Calibration* calibration; 22 | uint32_t view_id; 23 | } AirMouse; 24 | 25 | typedef enum { 26 | AirMouseViewSubmenu, 27 | AirMouseViewBtMouse, 28 | AirMouseViewUsbMouse, 29 | AirMouseViewCalibration, 30 | AirMouseViewExitConfirm, 31 | AirMouseViewError, 32 | } AirMouseView; 33 | -------------------------------------------------------------------------------- /application.fam: -------------------------------------------------------------------------------- 1 | App( 2 | appid="air_mouse", 3 | name="BMI Air Mouse", 4 | apptype=FlipperAppType.EXTERNAL, 5 | entry_point="air_mouse_app", 6 | stack_size=10 * 1024, 7 | fap_category="GPIO", 8 | fap_icon="icon.png", 9 | fap_version="1.4", 10 | fap_libs=["ble_profile"], 11 | sources=["*.c", "*.cc"], 12 | fap_private_libs=[ 13 | Lib( 14 | name="bmi160-api", 15 | cflags=["-Wno-error"], 16 | sources=[ 17 | "bmi160.c", 18 | "bmi160.h", 19 | "bmi160_defs.h", 20 | ], 21 | ), 22 | Lib( 23 | name="lsm6ds3tr-api", 24 | cflags=["-Wno-error"], 25 | ), 26 | Lib( 27 | name="lsm6dso-api", 28 | cflags=["-Wno-error"], 29 | ), 30 | ], 31 | ) 32 | -------------------------------------------------------------------------------- /CHANGELOG.md: -------------------------------------------------------------------------------- 1 | # Changelog 2 | 3 | ## v1.4 (2025-04-11) 4 | 5 | **Fix build with SDK 85** 6 | 7 | ## v1.3 (2024-08-15) 8 | 9 | **Fix new firmwares compatibility** 10 | 11 | ## v1.2 (2024-06-05) 12 | 13 | **Support multiple IMU models** 14 | 15 | - Apart from BMI160, you can now use LSM6DS3 or LSM6DSO 16 | 17 | ## v1.1 (2024-03-14) 18 | 19 | **BLE updates/fixes** 20 | 21 | ## v1.0 (2024-03-09) 22 | 23 | **Update for SDK 0.99** 24 | 25 | ## v0.0.6 (2023-08-09) 26 | 27 | **Fix firmware 0.88 issues** 28 | 29 | ## v0.0.5 (2023-06-18) 30 | 31 | **Closed issues:** 32 | 33 | - Error on firmware 0.77.1 34 | - Flipper Z with airmouse like LG pilot on Android TV 35 | - PINOUT 36 | - Noob Question 37 | - BMI160 alternative? 38 | 39 | ## v0.0.4 (2023-01-22) 40 | 41 | **Closed issues:** 42 | 43 | - Request: change directional buttons 44 | 45 | **Merged pull requests:** 46 | 47 | - Manifest build sources 48 | 49 | ## v0.0.3 (2022-12-30) 50 | 51 | **Closed issues:** 52 | 53 | - Firmware requirement? 54 | 55 | **Merged pull requests:** 56 | 57 | - adjust button function \(add scrolling\) 58 | -------------------------------------------------------------------------------- /tracking/util/matrix_4x4.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2019 Google Inc. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | #ifndef CARDBOARD_SDK_UTIL_MATRIX_4X4_H_ 17 | #define CARDBOARD_SDK_UTIL_MATRIX_4X4_H_ 18 | 19 | #include 20 | 21 | namespace cardboard { 22 | 23 | class Matrix4x4 { 24 | public: 25 | static Matrix4x4 Identity(); 26 | static Matrix4x4 Zeros(); 27 | static Matrix4x4 Translation(float x, float y, float z); 28 | static Matrix4x4 Perspective(const std::array& fov, float zNear, float zFar); 29 | void ToArray(float* array) const; 30 | 31 | private: 32 | std::array, 4> m; 33 | }; 34 | 35 | } // namespace cardboard 36 | 37 | #endif // CARDBOARD_SDK_UTIL_MATRIX4X4_H_ 38 | -------------------------------------------------------------------------------- /DESCRIPTION.md: -------------------------------------------------------------------------------- 1 | # Flipper Air Mouse 2 | 3 | ## What? 4 | 5 | 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)... 6 | 7 | Using it is really simple: 8 | * Connect the Flipper via a USB cable and pick USB, or pick Bluetooth and pair it with your PC; 9 | * Hold the Flipper in your hand with the buttons pointing towards the screen; 10 | * Wave your Flipper like you don't care to move the cursor; 11 | * Up button for Left mouse click; 12 | * Down button for Right mouse click; 13 | * Center button for Middle mouse click; 14 | * Left and Right buttons for scrolling; 15 | * 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). 16 | 17 | ## Hardware 18 | 19 | The custom module is using Bosch BMI160 accelerometer/gyroscope chip connected via I2C. 20 | 21 | Take a look into the schematic folder for Gerber, BOM and CPL files, so you can order directly from JLCPCB. 22 | 23 | ## Software 24 | 25 | The code is based on the original Bosch driver and an orientation tracking implementation from the Google Cardboard project 26 | 27 | ## License 28 | 29 | TL;DR: Use the code however you want, give credit where it's due, no warranty of any kind is provided. 30 | -------------------------------------------------------------------------------- /tracking/sensors/gyroscope_data.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2019 Google Inc. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | #ifndef CARDBOARD_SDK_SENSORS_GYROSCOPE_DATA_H_ 17 | #define CARDBOARD_SDK_SENSORS_GYROSCOPE_DATA_H_ 18 | 19 | #include "../util/vector.h" 20 | 21 | namespace cardboard { 22 | 23 | struct GyroscopeData { 24 | // System wall time. 25 | uint64_t system_timestamp; 26 | 27 | // Sensor clock time in nanoseconds. 28 | uint64_t sensor_timestamp_ns; 29 | 30 | // Rate of rotation around the x,y,z axes in rad/s. This follows android 31 | // specification 32 | // (https://developer.android.com/guide/topics/sensors/sensors_overview.html#sensors-coords). 33 | Vector3 data; 34 | }; 35 | 36 | } // namespace cardboard 37 | 38 | #endif // CARDBOARD_SDK_SENSORS_GYROSCOPE_DATA_H_ 39 | -------------------------------------------------------------------------------- /tracking/util/logging.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2019 Google Inc. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | #ifndef CARDBOARD_SDK_UTIL_LOGGING_H_ 17 | #define CARDBOARD_SDK_UTIL_LOGGING_H_ 18 | 19 | #include 20 | #include 21 | 22 | #if defined(__ANDROID__) 23 | 24 | #include 25 | 26 | // Uncomment these to enable debug logging from native code 27 | 28 | #define CARDBOARD_LOGI(...) // __android_log_print(ANDROID_LOG_INFO, "CardboardSDK", __VA_ARGS__) 29 | #define CARDBOARD_LOGE(...) // __android_log_print(ANDROID_LOG_ERROR, "CardboardSDK", __VA_ARGS__) 30 | 31 | #else 32 | 33 | #define CARDBOARD_LOGI(...) // FURI_LOG_I("CardboardSDK", __VA_ARGS__) 34 | #define CARDBOARD_LOGE(...) // FURI_LOG_E("CardboardSDK", __VA_ARGS__) 35 | 36 | #endif 37 | 38 | #endif // CARDBOARD_SDK_UTIL_LOGGING_H_ 39 | -------------------------------------------------------------------------------- /tracking/sensors/accelerometer_data.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2019 Google Inc. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | #ifndef CARDBOARD_SDK_SENSORS_ACCELEROMETER_DATA_H_ 17 | #define CARDBOARD_SDK_SENSORS_ACCELEROMETER_DATA_H_ 18 | 19 | #include "../util/vector.h" 20 | 21 | namespace cardboard { 22 | 23 | struct AccelerometerData { 24 | // System wall time. 25 | uint64_t system_timestamp; 26 | 27 | // Sensor clock time in nanoseconds. 28 | uint64_t sensor_timestamp_ns; 29 | 30 | // Acceleration force along the x,y,z axes in m/s^2. This follows android 31 | // specification 32 | // (https://developer.android.com/guide/topics/sensors/sensors_overview.html#sensors-coords). 33 | Vector3 data; 34 | }; 35 | 36 | } // namespace cardboard 37 | 38 | #endif // CARDBOARD_SDK_SENSORS_ACCELEROMETER_DATA_H_ 39 | -------------------------------------------------------------------------------- /tracking/sensors/mean_filter.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2019 Google Inc. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | #include "mean_filter.h" 17 | 18 | namespace cardboard { 19 | 20 | MeanFilter::MeanFilter(size_t filter_size) 21 | : filter_size_(filter_size) 22 | { 23 | } 24 | 25 | void MeanFilter::AddSample(const Vector3& sample) 26 | { 27 | buffer_.push_back(sample); 28 | if (buffer_.size() > filter_size_) { 29 | buffer_.pop_front(); 30 | } 31 | } 32 | 33 | bool MeanFilter::IsValid() const { return buffer_.size() == filter_size_; } 34 | 35 | Vector3 MeanFilter::GetFilteredData() const 36 | { 37 | // Compute mean of the samples stored in buffer_. 38 | Vector3 mean = Vector3::Zero(); 39 | for (auto sample : buffer_) { 40 | mean += sample; 41 | } 42 | 43 | return mean / static_cast(filter_size_); 44 | } 45 | 46 | } // namespace cardboard 47 | -------------------------------------------------------------------------------- /tracking/util/vectorutils.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2019 Google Inc. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | #include "vectorutils.h" 17 | 18 | namespace cardboard { 19 | 20 | // Returns the dot (inner) product of two Vectors. 21 | double Dot(const Vector<3>& v0, const Vector<3>& v1) 22 | { 23 | return v0[0] * v1[0] + v0[1] * v1[1] + v0[2] * v1[2]; 24 | } 25 | 26 | // Returns the dot (inner) product of two Vectors. 27 | double Dot(const Vector<4>& v0, const Vector<4>& v1) 28 | { 29 | return v0[0] * v1[0] + v0[1] * v1[1] + v0[2] * v1[2] + v0[3] * v1[3]; 30 | } 31 | 32 | // Returns the 3-dimensional cross product of 2 Vectors. Note that this is 33 | // defined only for 3-dimensional Vectors. 34 | Vector<3> Cross(const Vector<3>& v0, const Vector<3>& v1) 35 | { 36 | return Vector<3>(v0[1] * v1[2] - v0[2] * v1[1], v0[2] * v1[0] - v0[0] * v1[2], 37 | v0[0] * v1[1] - v0[1] * v1[0]); 38 | } 39 | 40 | } // namespace cardboard 41 | -------------------------------------------------------------------------------- /tracking/sensors/mean_filter.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2019 Google Inc. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | #ifndef CARDBOARD_SDK_SENSORS_MEAN_FILTER_H_ 17 | #define CARDBOARD_SDK_SENSORS_MEAN_FILTER_H_ 18 | 19 | #include 20 | #include 21 | 22 | #include "../util/vector.h" 23 | 24 | namespace cardboard { 25 | 26 | // Fixed window FIFO mean filter for vectors of the given dimension. 27 | class MeanFilter { 28 | public: 29 | // Create a mean filter of size filter_size. 30 | // @param filter_size size of the internal filter. 31 | explicit MeanFilter(size_t filter_size); 32 | 33 | // Add sample to buffer_ if buffer_ is full it drop the oldest sample. 34 | void AddSample(const Vector3& sample); 35 | 36 | // Returns true if buffer has filter_size_ sample, false otherwise. 37 | bool IsValid() const; 38 | 39 | // Returns the mean of values stored in the internal buffer. 40 | Vector3 GetFilteredData() const; 41 | 42 | private: 43 | const size_t filter_size_; 44 | std::deque buffer_; 45 | }; 46 | 47 | } // namespace cardboard 48 | 49 | #endif // CARDBOARD_SDK_SENSORS_MEAN_FILTER_H_ 50 | -------------------------------------------------------------------------------- /tracking/imu/imu.c: -------------------------------------------------------------------------------- 1 | #include "imu.h" 2 | 3 | #define IMU_TAG "IMU_H" 4 | 5 | extern struct imu_t imu_bmi160; 6 | extern struct imu_t imu_lsm6ds3trc; 7 | extern struct imu_t imu_lsm6dso; 8 | 9 | struct imu_t* imu_types[] = { 10 | &imu_bmi160, 11 | &imu_lsm6ds3trc, 12 | &imu_lsm6dso 13 | }; 14 | 15 | static const int imu_count = sizeof(imu_types) / sizeof(struct imu_t*); 16 | 17 | static struct imu_t* imu_found; 18 | 19 | struct imu_t* find_imu() { 20 | unsigned int i; 21 | for(i = 0; i < imu_count; i++) { 22 | if(furi_hal_i2c_is_device_ready(&furi_hal_i2c_handle_external, imu_types[i]->address, 50)) { 23 | FURI_LOG_E(IMU_TAG, "found i2c device address 0x%X", imu_types[i]->address); 24 | return imu_types[i]; 25 | } 26 | } 27 | return NULL; 28 | } 29 | 30 | bool imu_begin() { 31 | bool ret = false; 32 | furi_hal_i2c_acquire(&furi_hal_i2c_handle_external); 33 | 34 | if (imu_found == NULL) { 35 | imu_found = find_imu(); 36 | if (imu_found != NULL) 37 | FURI_LOG_E(IMU_TAG, "Found Device %s", imu_found->name); 38 | } 39 | 40 | if (imu_found != NULL) 41 | ret = imu_found->begin(); 42 | 43 | furi_hal_i2c_release(&furi_hal_i2c_handle_external); 44 | return ret; 45 | } 46 | 47 | void imu_end() { 48 | if (imu_found == NULL) 49 | return; 50 | furi_hal_i2c_acquire(&furi_hal_i2c_handle_external); 51 | imu_found->end(); 52 | furi_hal_i2c_release(&furi_hal_i2c_handle_external); 53 | } 54 | 55 | int imu_read(double* vec) { 56 | if (imu_found == NULL) 57 | return 0; 58 | furi_hal_i2c_acquire(&furi_hal_i2c_handle_external); 59 | int ret = imu_found->read(vec); 60 | furi_hal_i2c_release(&furi_hal_i2c_handle_external); 61 | return ret; 62 | } 63 | -------------------------------------------------------------------------------- /tracking/sensors/pose_state.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2019 Google Inc. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | #ifndef CARDBOARD_SDK_SENSORS_POSE_STATE_H_ 17 | #define CARDBOARD_SDK_SENSORS_POSE_STATE_H_ 18 | 19 | #include "../util/rotation.h" 20 | #include "../util/vector.h" 21 | 22 | namespace cardboard { 23 | 24 | enum { 25 | kPoseStateFlagInvalid = 1U << 0, 26 | kPoseStateFlagInitializing = 1U << 1, 27 | kPoseStateFlagHas6DoF = 1U << 2, 28 | }; 29 | 30 | // Stores a head pose pose plus derivatives. This can be used for prediction. 31 | struct PoseState { 32 | // System wall time. 33 | int64_t timestamp; 34 | 35 | // Rotation from Sensor Space to Start Space. 36 | Rotation sensor_from_start_rotation; 37 | 38 | // First derivative of the rotation. 39 | Vector3 sensor_from_start_rotation_velocity; 40 | 41 | // Current gyroscope bias in rad/s. 42 | Vector3 bias; 43 | 44 | // The position of the headset. 45 | Vector3 position = Vector3(0, 0, 0); 46 | 47 | // In the same coordinate frame as the position. 48 | Vector3 velocity = Vector3(0, 0, 0); 49 | 50 | // Flags indicating the status of the pose. 51 | uint64_t flags = 0U; 52 | }; 53 | 54 | } // namespace cardboard 55 | 56 | #endif // CARDBOARD_SDK_SENSORS_POSE_STATE_H_ 57 | -------------------------------------------------------------------------------- /tracking/sensors/median_filter.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2019 Google Inc. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | #ifndef CARDBOARD_SDK_SENSORS_MEDIAN_FILTER_H_ 17 | #define CARDBOARD_SDK_SENSORS_MEDIAN_FILTER_H_ 18 | 19 | #include 20 | #include 21 | 22 | #include "../util/vector.h" 23 | 24 | namespace cardboard { 25 | 26 | // Fixed window FIFO median filter for vectors of the given dimension = 3. 27 | class MedianFilter { 28 | public: 29 | // Creates a median filter of size filter_size. 30 | // @param filter_size size of the internal filter. 31 | explicit MedianFilter(size_t filter_size); 32 | 33 | // Adds sample to buffer_ if buffer_ is full it drops the oldest sample. 34 | void AddSample(const Vector3& sample); 35 | 36 | // Returns true if buffer has filter_size_ sample, false otherwise. 37 | bool IsValid() const; 38 | 39 | // Returns the median of values store in the internal buffer. 40 | Vector3 GetFilteredData() const; 41 | 42 | // Resets the filter, removing all samples that have been added. 43 | void Reset(); 44 | 45 | private: 46 | const size_t filter_size_; 47 | std::deque buffer_; 48 | // Contains norms of the elements stored in buffer_. 49 | std::deque norms_; 50 | }; 51 | 52 | } // namespace cardboard 53 | 54 | #endif // CARDBOARD_SDK_SENSORS_MEDIAN_FILTER_H_ 55 | -------------------------------------------------------------------------------- /tracking/sensors/median_filter.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2019 Google Inc. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | #include "median_filter.h" 17 | 18 | #include 19 | #include 20 | 21 | #include "../util/vector.h" 22 | #include "../util/vectorutils.h" 23 | 24 | namespace cardboard { 25 | 26 | MedianFilter::MedianFilter(size_t filter_size) 27 | : filter_size_(filter_size) 28 | { 29 | } 30 | 31 | void MedianFilter::AddSample(const Vector3& sample) 32 | { 33 | buffer_.push_back(sample); 34 | norms_.push_back(Length(sample)); 35 | if (buffer_.size() > filter_size_) { 36 | buffer_.pop_front(); 37 | norms_.pop_front(); 38 | } 39 | } 40 | 41 | bool MedianFilter::IsValid() const { return buffer_.size() == filter_size_; } 42 | 43 | Vector3 MedianFilter::GetFilteredData() const 44 | { 45 | std::vector norms(norms_.begin(), norms_.end()); 46 | 47 | // Get median of value of the norms. 48 | std::nth_element(norms.begin(), norms.begin() + filter_size_ / 2, norms.end()); 49 | const float median_norm = norms[filter_size_ / 2]; 50 | 51 | // Get median value based on their norm. 52 | auto median_it = buffer_.begin(); 53 | for (const auto norm : norms_) { 54 | if (norm == median_norm) { 55 | break; 56 | } 57 | ++median_it; 58 | } 59 | 60 | return *median_it; 61 | } 62 | 63 | void MedianFilter::Reset() 64 | { 65 | buffer_.clear(); 66 | norms_.clear(); 67 | } 68 | 69 | } // namespace cardboard 70 | -------------------------------------------------------------------------------- /tracking/orientation_tracker.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2019 Google Inc. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | #pragma once 17 | 18 | #include 19 | #include 20 | #include // NOLINT 21 | 22 | #include "sensors/accelerometer_data.h" 23 | #include "sensors/gyroscope_data.h" 24 | #include "sensors/sensor_fusion_ekf.h" 25 | #include "util/rotation.h" 26 | 27 | namespace cardboard { 28 | 29 | // OrientationTracker encapsulates pose tracking by connecting sensors 30 | // to SensorFusion. 31 | // This pose tracker reports poses in display space. 32 | class OrientationTracker { 33 | public: 34 | OrientationTracker(const long sampling_period_ns); 35 | 36 | void SetCalibration(const Vector3& calibration); 37 | 38 | // Pauses tracking and sensors. 39 | void Pause(); 40 | 41 | // Resumes tracking ans sensors. 42 | void Resume(); 43 | 44 | // Gets the predicted pose for a given timestamp. 45 | Vector4 GetPose(int64_t timestamp_ns) const; 46 | 47 | // Function called when receiving AccelerometerData. 48 | // 49 | // @param event sensor event. 50 | void OnAccelerometerData(const AccelerometerData& event); 51 | 52 | // Function called when receiving GyroscopeData. 53 | // 54 | // @param event sensor event. 55 | Vector4 OnGyroscopeData(const GyroscopeData& event); 56 | 57 | private: 58 | long sampling_period_ns_; 59 | Vector3 calibration_; 60 | 61 | std::atomic is_tracking_; 62 | // Sensor Fusion object that stores the internal state of the filter. 63 | std::unique_ptr sensor_fusion_; 64 | // Latest gyroscope data. 65 | GyroscopeData latest_gyroscope_data_; 66 | }; 67 | 68 | } // namespace cardboard 69 | -------------------------------------------------------------------------------- /views/calibration.c: -------------------------------------------------------------------------------- 1 | #include "calibration.h" 2 | #include "../tracking/main_loop.h" 3 | #include "../air_mouse.h" 4 | 5 | #include 6 | #include 7 | 8 | struct Calibration { 9 | View* view; 10 | ViewDispatcher* view_dispatcher; 11 | }; 12 | 13 | static void calibration_draw_callback(Canvas* canvas, void* context) { 14 | UNUSED(context); 15 | canvas_clear(canvas); 16 | canvas_set_font(canvas, FontPrimary); 17 | canvas_draw_str(canvas, 0, 10, "Calibrating..."); 18 | canvas_set_font(canvas, FontSecondary); 19 | canvas_draw_str(canvas, 0, 63, "Please wait"); 20 | } 21 | 22 | void calibration_enter_callback(void* context) { 23 | furi_assert(context); 24 | Calibration* calibration = context; 25 | calibration_begin(); 26 | view_dispatcher_send_custom_event(calibration->view_dispatcher, 0); 27 | } 28 | 29 | bool calibration_custom_callback(uint32_t event, void* context) { 30 | UNUSED(event); 31 | furi_assert(context); 32 | Calibration* calibration = context; 33 | 34 | if(calibration_step()) { 35 | view_dispatcher_switch_to_view(calibration->view_dispatcher, AirMouseViewSubmenu); 36 | } else { 37 | view_dispatcher_send_custom_event(calibration->view_dispatcher, 0); 38 | } 39 | 40 | return true; 41 | } 42 | 43 | void calibration_exit_callback(void* context) { 44 | furi_assert(context); 45 | calibration_end(); 46 | } 47 | 48 | Calibration* calibration_alloc(ViewDispatcher* view_dispatcher) { 49 | Calibration* calibration = malloc(sizeof(Calibration)); 50 | calibration->view = view_alloc(); 51 | calibration->view_dispatcher = view_dispatcher; 52 | view_set_context(calibration->view, calibration); 53 | view_set_draw_callback(calibration->view, calibration_draw_callback); 54 | view_set_enter_callback(calibration->view, calibration_enter_callback); 55 | view_set_custom_callback(calibration->view, calibration_custom_callback); 56 | view_set_exit_callback(calibration->view, calibration_exit_callback); 57 | return calibration; 58 | } 59 | 60 | void calibration_free(Calibration* calibration) { 61 | furi_assert(calibration); 62 | view_free(calibration->view); 63 | free(calibration); 64 | } 65 | 66 | View* calibration_get_view(Calibration* calibration) { 67 | furi_assert(calibration); 68 | return calibration->view; 69 | } 70 | -------------------------------------------------------------------------------- /tracking/sensors/pose_prediction.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2019 Google Inc. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | #ifndef CARDBOARD_SDK_SENSORS_POSE_PREDICTION_H_ 17 | #define CARDBOARD_SDK_SENSORS_POSE_PREDICTION_H_ 18 | 19 | #include 20 | 21 | #include "pose_state.h" 22 | #include "../util/rotation.h" 23 | 24 | namespace cardboard { 25 | namespace pose_prediction { 26 | 27 | // Returns a rotation matrix based on the integration of the gyroscope_value 28 | // over the timestep_s in seconds. 29 | // TODO(pfg): Document the space better here. 30 | // 31 | // @param gyroscope_value gyroscope sensor values. 32 | // @param timestep_s integration period in seconds. 33 | // @return Integration of the gyroscope value the rotation is from Start to 34 | // Sensor Space. 35 | Rotation GetRotationFromGyroscope(const Vector3& gyroscope_value, double timestep_s); 36 | 37 | // Gets a predicted pose for a given time in the future (e.g. rendering time) 38 | // based on a linear prediction model. This uses the system current state 39 | // (position, velocity, etc) from the past to extrapolate a position in the 40 | // future. 41 | // 42 | // @param requested_pose_timestamp time at which you want the pose. 43 | // @param current_state current state that stores the pose and linear model at a 44 | // given time prior to requested_pose_timestamp_ns. 45 | // @return pose from Start to Sensor Space. 46 | Rotation PredictPose(int64_t requested_pose_timestamp, const PoseState& current_state); 47 | 48 | // Equivalent to PredictPose, but for use with poses relative to Start Space 49 | // rather than sensor space. 50 | Rotation PredictPoseInv(int64_t requested_pose_timestamp, const PoseState& current_state); 51 | 52 | } // namespace pose_prediction 53 | } // namespace cardboard 54 | 55 | #endif // CARDBOARD_SDK_SENSORS_POSE_PREDICTION_H_ 56 | -------------------------------------------------------------------------------- /tracking/util/vectorutils.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2019 Google Inc. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | #ifndef CARDBOARD_SDK_UTIL_VECTORUTILS_H_ 17 | #define CARDBOARD_SDK_UTIL_VECTORUTILS_H_ 18 | 19 | // 20 | // This file contains free functions that operate on Vector instances. 21 | // 22 | 23 | #include 24 | 25 | #include "vector.h" 26 | 27 | namespace cardboard { 28 | 29 | // Returns the dot (inner) product of two Vectors. 30 | double Dot(const Vector<3>& v0, const Vector<3>& v1); 31 | 32 | // Returns the dot (inner) product of two Vectors. 33 | double Dot(const Vector<4>& v0, const Vector<4>& v1); 34 | 35 | // Returns the 3-dimensional cross product of 2 Vectors. Note that this is 36 | // defined only for 3-dimensional Vectors. 37 | Vector<3> Cross(const Vector<3>& v0, const Vector<3>& v1); 38 | 39 | // Returns the square of the length of a Vector. 40 | template 41 | double LengthSquared(const Vector& v) { 42 | return Dot(v, v); 43 | } 44 | 45 | // Returns the geometric length of a Vector. 46 | template 47 | double Length(const Vector& v) { 48 | return sqrt(LengthSquared(v)); 49 | } 50 | 51 | // the Vector untouched and returns false. 52 | template 53 | bool Normalize(Vector* v) { 54 | const double len = Length(*v); 55 | if(len == 0) { 56 | return false; 57 | } else { 58 | (*v) /= len; 59 | return true; 60 | } 61 | } 62 | 63 | // Returns a unit-length version of a Vector. If the given Vector has no 64 | // length, this returns a Zero() Vector. 65 | template 66 | Vector Normalized(const Vector& v) { 67 | Vector result = v; 68 | if(Normalize(&result)) 69 | return result; 70 | else 71 | return Vector::Zero(); 72 | } 73 | 74 | } // namespace cardboard 75 | 76 | #endif // CARDBOARD_SDK_UTIL_VECTORUTILS_H_ 77 | -------------------------------------------------------------------------------- /tracking/calibration_data.cc: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #define TAG "tracker" 5 | 6 | #include "calibration_data.h" 7 | 8 | #include 9 | #include 10 | 11 | // Student's distribution T value for 95% (two-sided) confidence interval. 12 | static const double Tn = 1.960; 13 | 14 | // Number of samples (degrees of freedom) for the corresponding T values. 15 | static const int Nn = 200; 16 | 17 | void CalibrationData::reset() 18 | { 19 | complete = false; 20 | count = 0; 21 | sum = Vector::Zero(); 22 | sumSq = Vector::Zero(); 23 | mean = Vector::Zero(); 24 | median = Vector::Zero(); 25 | sigma = Vector::Zero(); 26 | delta = Vector::Zero(); 27 | xData.clear(); 28 | yData.clear(); 29 | zData.clear(); 30 | } 31 | 32 | bool CalibrationData::add(Vector& data) 33 | { 34 | if (complete) { 35 | return true; 36 | } 37 | 38 | xData.push_back(data[0]); 39 | yData.push_back(data[1]); 40 | zData.push_back(data[2]); 41 | 42 | sum += data; 43 | sumSq += data * data; 44 | count++; 45 | 46 | if (count >= Nn) { 47 | calcDelta(); 48 | complete = true; 49 | } 50 | 51 | return complete; 52 | } 53 | 54 | static inline double medianOf(std::vector& list) 55 | { 56 | std::sort(list.begin(), list.end()); 57 | int count = list.size(); 58 | int middle = count / 2; 59 | return (count % 2 == 1) ? list[middle] : (list[middle - 1] + list[middle]) / 2.0l; 60 | } 61 | 62 | void CalibrationData::calcDelta() 63 | { 64 | median.Set(medianOf(xData), medianOf(yData), medianOf(zData)); 65 | 66 | mean = sum / count; 67 | Vector m2 = mean * mean; 68 | Vector d = sumSq / count - m2; 69 | Vector s2 = (d * count) / (count - 1); 70 | sigma = Vector(std::sqrt(d[0]), std::sqrt(d[1]), std::sqrt(d[2])); 71 | Vector s = Vector(std::sqrt(s2[0]), std::sqrt(s2[1]), std::sqrt(s2[2])); 72 | delta = s * Tn / std::sqrt((double)count); 73 | Vector low = mean - delta; 74 | Vector high = mean + delta; 75 | 76 | FURI_LOG_I(TAG, 77 | "M[x] = { %f ... %f } // median = %f // avg = %f // delta = %f // sigma = %f", 78 | low[0], high[0], median[0], mean[0], delta[0], sigma[0]); 79 | FURI_LOG_I(TAG, 80 | "M[y] = { %f ... %f } // median = %f // avg = %f // delta = %f // sigma = %f", 81 | low[1], high[1], median[1], mean[1], delta[1], sigma[1]); 82 | FURI_LOG_I(TAG, 83 | "M[z] = { %f ... %f } // median = %f // avg = %f // delta = %f // sigma = %f", 84 | low[2], high[2], median[2], mean[2], delta[2], sigma[2]); 85 | } -------------------------------------------------------------------------------- /tracking/util/matrix_4x4.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2019 Google Inc. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | #include "matrix_4x4.h" 17 | 18 | #include 19 | #include 20 | #include 21 | 22 | namespace cardboard { 23 | 24 | Matrix4x4 Matrix4x4::Identity() 25 | { 26 | Matrix4x4 ret; 27 | for (int j = 0; j < 4; ++j) { 28 | for (int i = 0; i < 4; ++i) { 29 | ret.m[j][i] = (i == j) ? 1 : 0; 30 | } 31 | } 32 | 33 | return ret; 34 | } 35 | 36 | Matrix4x4 Matrix4x4::Zeros() 37 | { 38 | Matrix4x4 ret; 39 | for (int j = 0; j < 4; ++j) { 40 | for (int i = 0; i < 4; ++i) { 41 | ret.m[j][i] = 0; 42 | } 43 | } 44 | 45 | return ret; 46 | } 47 | 48 | Matrix4x4 Matrix4x4::Translation(float x, float y, float z) 49 | { 50 | Matrix4x4 ret = Matrix4x4::Identity(); 51 | ret.m[3][0] = x; 52 | ret.m[3][1] = y; 53 | ret.m[3][2] = z; 54 | 55 | return ret; 56 | } 57 | 58 | Matrix4x4 Matrix4x4::Perspective(const std::array& fov, float zNear, float zFar) 59 | { 60 | Matrix4x4 ret = Matrix4x4::Zeros(); 61 | 62 | const float xLeft = -std::tan(fov[0] * M_PI / 180.0f) * zNear; 63 | const float xRight = std::tan(fov[1] * M_PI / 180.0f) * zNear; 64 | const float yBottom = -std::tan(fov[2] * M_PI / 180.0f) * zNear; 65 | const float yTop = std::tan(fov[3] * M_PI / 180.0f) * zNear; 66 | 67 | const float X = (2 * zNear) / (xRight - xLeft); 68 | const float Y = (2 * zNear) / (yTop - yBottom); 69 | const float A = (xRight + xLeft) / (xRight - xLeft); 70 | const float B = (yTop + yBottom) / (yTop - yBottom); 71 | const float C = (zNear + zFar) / (zNear - zFar); 72 | const float D = (2 * zNear * zFar) / (zNear - zFar); 73 | 74 | ret.m[0][0] = X; 75 | ret.m[2][0] = A; 76 | ret.m[1][1] = Y; 77 | ret.m[2][1] = B; 78 | ret.m[2][2] = C; 79 | ret.m[3][2] = D; 80 | ret.m[2][3] = -1; 81 | 82 | return ret; 83 | } 84 | 85 | void Matrix4x4::ToArray(float* array) const { std::memcpy(array, &m[0][0], 16 * sizeof(float)); } 86 | 87 | } // namespace cardboard 88 | -------------------------------------------------------------------------------- /tracking/util/matrixutils.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2019 Google Inc. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | #ifndef CARDBOARD_SDK_UTIL_MATRIXUTILS_H_ 17 | #define CARDBOARD_SDK_UTIL_MATRIXUTILS_H_ 18 | 19 | // 20 | // This file contains operators and free functions that define generic Matrix 21 | // operations. 22 | // 23 | 24 | #include "matrix_3x3.h" 25 | #include "rotation.h" 26 | #include "vector.h" 27 | 28 | namespace cardboard { 29 | 30 | // Returns the transpose of a matrix. 31 | Matrix3x3 Transpose(const Matrix3x3& m); 32 | 33 | // Multiplies a Matrix and a column Vector of the same Dimension to produce 34 | // another column Vector. 35 | Vector3 operator*(const Matrix3x3& m, const Vector3& v); 36 | 37 | // Returns the determinant of the matrix. This function is defined for all the 38 | // typedef'ed Matrix types. 39 | double Determinant(const Matrix3x3& m); 40 | 41 | // Returns the adjugate of the matrix, which is defined as the transpose of the 42 | // cofactor matrix. This function is defined for all the typedef'ed Matrix 43 | // types. The determinant of the matrix is computed as a side effect, so it is 44 | // returned in the determinant parameter if it is not null. 45 | Matrix3x3 AdjugateWithDeterminant(const Matrix3x3& m, double* determinant); 46 | 47 | // Returns the inverse of the matrix. This function is defined for all the 48 | // typedef'ed Matrix types. The determinant of the matrix is computed as a 49 | // side effect, so it is returned in the determinant parameter if it is not 50 | // null. If the determinant is 0, the returned matrix has all zeroes. 51 | Matrix3x3 InverseWithDeterminant(const Matrix3x3& m, double* determinant); 52 | 53 | // Returns the inverse of the matrix. This function is defined for all the 54 | // typedef'ed Matrix types. If the determinant of the matrix is 0, the returned 55 | // matrix has all zeroes. 56 | Matrix3x3 Inverse(const Matrix3x3& m); 57 | 58 | // Returns a 3x3 Matrix representing a 3D rotation. This creates a Matrix that 59 | // does not work with homogeneous coordinates, so the function name ends in 60 | // "NH". 61 | Matrix3x3 RotationMatrixNH(const Rotation& r); 62 | 63 | } // namespace cardboard 64 | 65 | #endif // CARDBOARD_SDK_UTIL_MATRIXUTILS_H_ 66 | -------------------------------------------------------------------------------- /tracking/sensors/lowpass_filter.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2019 Google Inc. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | #include "lowpass_filter.h" 17 | 18 | #include 19 | 20 | namespace { 21 | 22 | const double kSecondsFromNanoseconds = 1.0e-9; 23 | 24 | // Minimum time step between sensor updates. This corresponds to 1000 Hz. 25 | const double kMinTimestepS = 0.001f; 26 | 27 | // Maximum time step between sensor updates. This corresponds to 1 Hz. 28 | const double kMaxTimestepS = 1.00f; 29 | 30 | } // namespace 31 | 32 | namespace cardboard { 33 | 34 | LowpassFilter::LowpassFilter(double cutoff_freq_hz) 35 | : cutoff_time_constant_(1 / (2 * (double)M_PI * cutoff_freq_hz)) 36 | , initialized_(false) 37 | { 38 | Reset(); 39 | } 40 | 41 | void LowpassFilter::AddSample(const Vector3& sample, uint64_t timestamp_ns) 42 | { 43 | AddWeightedSample(sample, timestamp_ns, 1.0); 44 | } 45 | 46 | void LowpassFilter::AddWeightedSample(const Vector3& sample, uint64_t timestamp_ns, double weight) 47 | { 48 | if (!initialized_) { 49 | // Initialize filter state 50 | filtered_data_ = { sample[0], sample[1], sample[2] }; 51 | timestamp_most_recent_update_ns_ = timestamp_ns; 52 | initialized_ = true; 53 | return; 54 | } 55 | 56 | if (timestamp_ns < timestamp_most_recent_update_ns_) { 57 | timestamp_most_recent_update_ns_ = timestamp_ns; 58 | return; 59 | } 60 | 61 | const double delta_s = static_cast(timestamp_ns - timestamp_most_recent_update_ns_) 62 | * kSecondsFromNanoseconds; 63 | if (delta_s <= kMinTimestepS || delta_s > kMaxTimestepS) { 64 | timestamp_most_recent_update_ns_ = timestamp_ns; 65 | return; 66 | } 67 | 68 | const double weighted_delta_secs = weight * delta_s; 69 | 70 | const double alpha = weighted_delta_secs / (cutoff_time_constant_ + weighted_delta_secs); 71 | 72 | for (int i = 0; i < 3; ++i) { 73 | filtered_data_[i] = (1 - alpha) * filtered_data_[i] + alpha * sample[i]; 74 | } 75 | timestamp_most_recent_update_ns_ = timestamp_ns; 76 | } 77 | 78 | void LowpassFilter::Reset() 79 | { 80 | initialized_ = false; 81 | filtered_data_ = { 0, 0, 0 }; 82 | } 83 | 84 | } // namespace cardboard 85 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | [!["Buy Me A Coffee"](https://www.buymeacoffee.com/assets/img/custom_images/orange_img.png)](https://www.buymeacoffee.com/ginkage) 2 | [![paypal](https://www.paypalobjects.com/en_GB/i/btn/btn_donate_LG.gif)](https://www.paypal.com/cgi-bin/webscr?cmd=_s-xclick&hosted_button_id=LF9S5WAF6E4VA) 3 | 4 | # BMI Air Mouse 5 | 6 | ## Brief 7 | 8 | > "You can turn anything into an air mouse if you're brave enough" 9 | 10 | — Piper, a.k.a. Pez 11 | 12 | Naturally, the quote above applies to [Flipper](https://flipperzero.one/) as well. 13 | 14 | ## What? 15 | 16 | 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)... 17 | 18 | Using it is really simple: 19 | * Connect the Flipper via a USB cable and pick `USB`, or pick `Bluetooth` and pair it with your PC; 20 | * Hold the Flipper in your hand with the buttons pointing towards the screen; 21 | * Wave your Flipper like you don't care to move the cursor; 22 | * Up button for Left mouse click; 23 | * Down button for Right mouse click; 24 | * Center button for Middle mouse click; 25 | * Left and Right buttons for scrolling; 26 | * 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). 27 | 28 | See early prototype [in action](https://www.youtube.com/watch?v=DdxAmmsYfMA). 29 | 30 | ## How? 31 | 32 | * Clone this repository with `git clone --recurse-submodules` to include the driver implementations. 33 | * Build the project using `ufbt` 34 | 35 | ## Hardware 36 | 37 | The custom module is using Bosch BMI160 accelerometer/gyroscope chip connected via I2C. 38 | 39 | Note: in fact, some other IMU chips are also supported. 40 | It's detected via the first found I2C Address. 41 | 42 | | Chip | Expected I2C Address | 43 | |:--------:|:--------------------:| 44 | | BMI160 | 0x69 | 45 | | LSM6DS3 | 0x6A | 46 | | LSM6DSO | 0x6B | 47 | 48 | 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. 49 | 50 | Original idea: 51 | 52 | ![What I thought it would look like](https://github.com/ginkage/FlippAirMouse/blob/main/schematic/schematic.png) 53 | 54 | Expectation: 55 | 56 | ![What EDA though it would look like](https://github.com/ginkage/FlippAirMouse/blob/main/schematic/render.png) 57 | 58 | Reality: 59 | 60 | ![What it looks like](https://github.com/ginkage/FlippAirMouse/blob/main/schematic/flipper.jpg) 61 | 62 | ## Software 63 | 64 | The code is based on the original Bosch [driver](https://github.com/BoschSensortec/BMI160_driver/) and an orientation tracking implementation from the Google [Cardboard](https://github.com/googlevr/cardboard/tree/master/sdk/sensors) project 65 | 66 | ## License 67 | 68 | TL;DR: Use the code however you want, give credit where it's due, no warranty of any kind is provided. 69 | -------------------------------------------------------------------------------- /tracking/sensors/lowpass_filter.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2019 Google Inc. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | #ifndef CARDBOARD_SDK_SENSORS_LOWPASS_FILTER_H_ 17 | #define CARDBOARD_SDK_SENSORS_LOWPASS_FILTER_H_ 18 | 19 | #include 20 | #include 21 | 22 | #include "../util/vector.h" 23 | 24 | namespace cardboard { 25 | 26 | // Implements an IIR, first order, low pass filter over vectors of the given 27 | // dimension = 3. 28 | // See http://en.wikipedia.org/wiki/Low-pass_filter 29 | class LowpassFilter { 30 | public: 31 | // Initializes a filter with the given cutoff frequency in Hz. 32 | explicit LowpassFilter(double cutoff_freq_hz); 33 | 34 | // Updates the filter with the given sample. Note that samples with 35 | // non-monotonic timestamps and successive samples with a time steps below 1 36 | // ms or above 1 s are ignored. 37 | // 38 | // @param sample current sample data. 39 | // @param timestamp_ns timestamp associated to this sample in nanoseconds. 40 | void AddSample(const Vector3& sample, uint64_t timestamp_ns); 41 | 42 | // Updates the filter with the given weighted sample. 43 | // 44 | // @param sample current sample data. 45 | // @param timestamp_ns timestamp associated to this sample in nanoseconds. 46 | // @param weight typically a [0, 1] weight factor used when applying a new 47 | // sample. A weight of 1 corresponds to calling AddSample. A weight of 0 48 | // makes the update no-op. The first initial sample is not affected by 49 | // this. 50 | void AddWeightedSample(const Vector3& sample, uint64_t timestamp_ns, double weight); 51 | 52 | // Returns the filtered value. A vector with zeros is returned if no samples 53 | // have been added. 54 | Vector3 GetFilteredData() const { 55 | return filtered_data_; 56 | } 57 | 58 | // Returns the most recent update timestamp in ns. 59 | uint64_t GetMostRecentTimestampNs() const { 60 | return timestamp_most_recent_update_ns_; 61 | } 62 | 63 | // Returns true when the filter is initialized. 64 | bool IsInitialized() const { 65 | return initialized_; 66 | } 67 | 68 | // Resets filter state. 69 | void Reset(); 70 | 71 | private: 72 | const double cutoff_time_constant_; 73 | uint64_t timestamp_most_recent_update_ns_; 74 | bool initialized_; 75 | 76 | Vector3 filtered_data_; 77 | }; 78 | 79 | } // namespace cardboard 80 | 81 | #endif // CARDBOARD_SDK_SENSORS_LOWPASS_FILTER_H_ 82 | -------------------------------------------------------------------------------- /tracking/sensors/pose_prediction.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2019 Google Inc. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | #include "pose_prediction.h" 17 | 18 | #include // NOLINT 19 | 20 | #include "../util/logging.h" 21 | #include "../util/vectorutils.h" 22 | 23 | namespace cardboard { 24 | 25 | namespace { 26 | const double kEpsilon = 1.0e-15; 27 | } // namespace 28 | 29 | namespace pose_prediction { 30 | 31 | Rotation GetRotationFromGyroscope(const Vector3& gyroscope_value, double timestep_s) 32 | { 33 | const double velocity = Length(gyroscope_value); 34 | 35 | // When there is no rotation data return an identity rotation. 36 | if (velocity < kEpsilon) { 37 | CARDBOARD_LOGI("PosePrediction::GetRotationFromGyroscope: Velocity really small, " 38 | "returning identity rotation."); 39 | return Rotation::Identity(); 40 | } 41 | // Since the gyroscope_value is a start from sensor transformation we need to 42 | // invert it to have a sensor from start transformation, hence the minus sign. 43 | // For more info: 44 | // http://developer.android.com/guide/topics/sensors/sensors_motion.html#sensors-motion-gyro 45 | return Rotation::FromAxisAndAngle(gyroscope_value / velocity, -timestep_s * velocity); 46 | } 47 | 48 | Rotation PredictPose(int64_t requested_pose_timestamp, const PoseState& current_state) 49 | { 50 | // Subtracting unsigned numbers is bad when the result is negative. 51 | const int64_t diff = requested_pose_timestamp - current_state.timestamp; 52 | const double timestep_s = diff * 1.0e-9; 53 | 54 | const Rotation update = GetRotationFromGyroscope( 55 | current_state.sensor_from_start_rotation_velocity, timestep_s); 56 | return update * current_state.sensor_from_start_rotation; 57 | } 58 | 59 | Rotation PredictPoseInv(int64_t requested_pose_timestamp, const PoseState& current_state) 60 | { 61 | // Subtracting unsigned numbers is bad when the result is negative. 62 | const int64_t diff = requested_pose_timestamp - current_state.timestamp; 63 | const double timestep_s = diff * 1.0e-9; 64 | 65 | const Rotation update = GetRotationFromGyroscope( 66 | current_state.sensor_from_start_rotation_velocity, timestep_s); 67 | return current_state.sensor_from_start_rotation * (-update); 68 | } 69 | 70 | } // namespace pose_prediction 71 | } // namespace cardboard 72 | -------------------------------------------------------------------------------- /tracking/orientation_tracker.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2019 Google Inc. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | #include "orientation_tracker.h" 17 | 18 | #include "sensors/pose_prediction.h" 19 | #include "util/logging.h" 20 | #include "util/vector.h" 21 | #include "util/vectorutils.h" 22 | 23 | namespace cardboard { 24 | 25 | OrientationTracker::OrientationTracker(const long sampling_period_ns) 26 | : sampling_period_ns_(sampling_period_ns) 27 | , calibration_(Vector3::Zero()) 28 | , is_tracking_(false) 29 | , sensor_fusion_(new SensorFusionEkf()) 30 | , latest_gyroscope_data_({ 0, 0, Vector3::Zero() }) 31 | { 32 | sensor_fusion_->SetBiasEstimationEnabled(/*kGyroBiasEstimationEnabled*/ true); 33 | } 34 | 35 | void OrientationTracker::SetCalibration(const Vector3& calibration) { 36 | calibration_ = calibration; 37 | } 38 | 39 | void OrientationTracker::Pause() 40 | { 41 | if (!is_tracking_) { 42 | return; 43 | } 44 | 45 | // Create a gyro event with zero velocity. This effectively stops the prediction. 46 | GyroscopeData event = latest_gyroscope_data_; 47 | event.data = Vector3::Zero(); 48 | 49 | OnGyroscopeData(event); 50 | 51 | is_tracking_ = false; 52 | } 53 | 54 | void OrientationTracker::Resume() { is_tracking_ = true; } 55 | 56 | Vector4 OrientationTracker::GetPose(int64_t timestamp_ns) const 57 | { 58 | Rotation predicted_rotation; 59 | const PoseState pose_state = sensor_fusion_->GetLatestPoseState(); 60 | if (sensor_fusion_->IsFullyInitialized()) { 61 | predicted_rotation = pose_state.sensor_from_start_rotation; 62 | } else { 63 | CARDBOARD_LOGI("Tracker not fully initialized yet. Using pose prediction only."); 64 | predicted_rotation = pose_prediction::PredictPose(timestamp_ns, pose_state); 65 | } 66 | 67 | return (-predicted_rotation).GetQuaternion(); 68 | } 69 | 70 | void OrientationTracker::OnAccelerometerData(const AccelerometerData& event) 71 | { 72 | if (!is_tracking_) { 73 | return; 74 | } 75 | sensor_fusion_->ProcessAccelerometerSample(event); 76 | } 77 | 78 | Vector4 OrientationTracker::OnGyroscopeData(const GyroscopeData& event) 79 | { 80 | if (!is_tracking_) { 81 | return Vector4(); 82 | } 83 | 84 | const GyroscopeData data = { .system_timestamp = event.system_timestamp, 85 | .sensor_timestamp_ns = event.sensor_timestamp_ns, 86 | .data = event.data - calibration_ }; 87 | 88 | latest_gyroscope_data_ = data; 89 | 90 | sensor_fusion_->ProcessGyroscopeSample(data); 91 | 92 | return GetPose(data.sensor_timestamp_ns + sampling_period_ns_); 93 | } 94 | 95 | } // namespace cardboard 96 | -------------------------------------------------------------------------------- /tracking/imu/bmi160.c: -------------------------------------------------------------------------------- 1 | #include "imu.h" 2 | #include "../../lib/bmi160-api/bmi160.h" 3 | 4 | #define BMI160_TAG "BMI160" 5 | #define BMI160_DEV_ADDR (0x69 << 1) 6 | 7 | struct bmi160_dev bmi160dev; 8 | struct bmi160_sensor_data bmi160_accel; 9 | struct bmi160_sensor_data bmi160_gyro; 10 | 11 | int8_t bmi160_write_i2c(uint8_t dev_addr, uint8_t reg_addr, uint8_t* data, uint16_t len) { 12 | if(furi_hal_i2c_write_mem(&furi_hal_i2c_handle_external, dev_addr, reg_addr, data, len, 50)) 13 | return BMI160_OK; 14 | return BMI160_E_COM_FAIL; 15 | } 16 | 17 | int8_t bmi160_read_i2c(uint8_t dev_addr, uint8_t reg_addr, uint8_t* read_data, uint16_t len) { 18 | if(furi_hal_i2c_read_mem(&furi_hal_i2c_handle_external, dev_addr, reg_addr, read_data, len, 50)) 19 | return BMI160_OK; 20 | return BMI160_E_COM_FAIL; 21 | } 22 | 23 | bool bmi160_begin() { 24 | FURI_LOG_I(BMI160_TAG, "Init BMI160"); 25 | 26 | if(!furi_hal_i2c_is_device_ready(&furi_hal_i2c_handle_external, BMI160_DEV_ADDR, 50)) { 27 | FURI_LOG_E(BMI160_TAG, "Device not ready!"); 28 | return false; 29 | } 30 | 31 | FURI_LOG_I(BMI160_TAG, "Device ready!"); 32 | 33 | bmi160dev.id = BMI160_DEV_ADDR; 34 | bmi160dev.intf = BMI160_I2C_INTF; 35 | bmi160dev.read = bmi160_read_i2c; 36 | bmi160dev.write = bmi160_write_i2c; 37 | bmi160dev.delay_ms = furi_delay_ms; 38 | 39 | if(bmi160_init(&bmi160dev) != BMI160_OK) { 40 | FURI_LOG_E(BMI160_TAG, "Initialization failure!"); 41 | FURI_LOG_E(BMI160_TAG, "Chip ID 0x%X", bmi160dev.chip_id); 42 | return false; 43 | } 44 | 45 | bmi160dev.accel_cfg.odr = BMI160_ACCEL_ODR_400HZ; 46 | bmi160dev.accel_cfg.range = BMI160_ACCEL_RANGE_4G; 47 | bmi160dev.accel_cfg.bw = BMI160_ACCEL_BW_NORMAL_AVG4; 48 | bmi160dev.accel_cfg.power = BMI160_ACCEL_NORMAL_MODE; 49 | bmi160dev.gyro_cfg.odr = BMI160_GYRO_ODR_400HZ; 50 | bmi160dev.gyro_cfg.range = BMI160_GYRO_RANGE_2000_DPS; 51 | bmi160dev.gyro_cfg.bw = BMI160_GYRO_BW_NORMAL_MODE; 52 | bmi160dev.gyro_cfg.power = BMI160_GYRO_NORMAL_MODE; 53 | 54 | if(bmi160_set_sens_conf(&bmi160dev) != BMI160_OK) { 55 | FURI_LOG_E(BMI160_TAG, "Initialization failure!"); 56 | FURI_LOG_E(BMI160_TAG, "Chip ID 0x%X", bmi160dev.chip_id); 57 | return false; 58 | } 59 | 60 | FURI_LOG_I(BMI160_TAG, "Initialization success!"); 61 | FURI_LOG_I(BMI160_TAG, "Chip ID 0x%X", bmi160dev.chip_id); 62 | 63 | return true; 64 | } 65 | 66 | int bmi160_read(double* vec) { 67 | if(bmi160_get_sensor_data( 68 | (BMI160_ACCEL_SEL | BMI160_GYRO_SEL), &bmi160_accel, &bmi160_gyro, &bmi160dev) != 69 | BMI160_OK) { 70 | return 0; 71 | } 72 | 73 | vec[0] = ((double)bmi160_accel.x * 4 / 32768) * GRAVITY; 74 | vec[1] = ((double)bmi160_accel.y * 4 / 32768) * GRAVITY; 75 | vec[2] = ((double)bmi160_accel.z * 4 / 32768) * GRAVITY; 76 | vec[3] = ((double)bmi160_gyro.x * 2000 / 32768) * DEG_TO_RAD; 77 | vec[4] = ((double)bmi160_gyro.y * 2000 / 32768) * DEG_TO_RAD; 78 | vec[5] = ((double)bmi160_gyro.z * 2000 / 32768) * DEG_TO_RAD; 79 | 80 | return ACC_DATA_READY | GYR_DATA_READY; 81 | } 82 | 83 | void bmi160_end() { 84 | } 85 | 86 | struct imu_t imu_bmi160 = { 87 | BMI160_DEV_ADDR, 88 | bmi160_begin, 89 | bmi160_end, 90 | bmi160_read, 91 | BMI160_TAG 92 | }; 93 | -------------------------------------------------------------------------------- /tracking/calibration_data.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include "util/vector.h" 8 | 9 | #define CALIBRATION_DATA_VER (1) 10 | #define CALIBRATION_DATA_FILE_NAME ".calibration.data" 11 | #define CALIBRATION_DATA_PATH INT_PATH(CALIBRATION_DATA_FILE_NAME) 12 | #define CALIBRATION_DATA_MAGIC (0x23) 13 | 14 | #define CALIBRATION_DATA_SAVE(x) \ 15 | saved_struct_save( \ 16 | CALIBRATION_DATA_PATH, \ 17 | (x), \ 18 | sizeof(CalibrationMedian), \ 19 | CALIBRATION_DATA_MAGIC, \ 20 | CALIBRATION_DATA_VER) 21 | 22 | #define CALIBRATION_DATA_LOAD(x) \ 23 | saved_struct_load( \ 24 | CALIBRATION_DATA_PATH, \ 25 | (x), \ 26 | sizeof(CalibrationMedian), \ 27 | CALIBRATION_DATA_MAGIC, \ 28 | CALIBRATION_DATA_VER) 29 | 30 | typedef struct { 31 | double x; 32 | double y; 33 | double z; 34 | } CalibrationMedian; 35 | 36 | typedef cardboard::Vector3 Vector; 37 | 38 | /** 39 | * Helper class to gather some stats and store the calibration data. Right now it calculates a lot 40 | * more stats than actually needed. Some of them are used for logging the sensors quality (and 41 | * filing bugs), other may be required in the future, e.g. for bias. 42 | */ 43 | class CalibrationData { 44 | public: 45 | /** 46 | * Check if the sensors were calibrated before. 47 | * 48 | * @return {@code true} if calibration data is available, or {@code false} otherwise. 49 | */ 50 | bool isComplete() { 51 | return complete; 52 | } 53 | 54 | /** Prepare to collect new calibration data. */ 55 | void reset(); 56 | 57 | /** 58 | * Retrieve the median gyroscope readings. 59 | * 60 | * @return Three-axis median vector. 61 | */ 62 | Vector getMedian() { 63 | return median; 64 | } 65 | 66 | /** 67 | * Retrieve the mean gyroscope readings. 68 | * 69 | * @return Three-axis mean vector. 70 | */ 71 | Vector getMean() { 72 | return mean; 73 | } 74 | 75 | /** 76 | * Retrieve the standard deviation of gyroscope readings. 77 | * 78 | * @return Three-axis standard deviation vector. 79 | */ 80 | Vector getSigma() { 81 | return sigma; 82 | } 83 | 84 | /** 85 | * Retrieve the confidence interval size of gyroscope readings. 86 | * 87 | * @return Three-axis confidence interval size vector. 88 | */ 89 | Vector getDelta() { 90 | return delta; 91 | } 92 | 93 | /** 94 | * Add a new gyroscope reading to the stats. 95 | * 96 | * @param data gyroscope values vector. 97 | * @return {@code true} if we now have enough data for calibration, or {@code false} otherwise. 98 | */ 99 | bool add(Vector& data); 100 | 101 | private: 102 | // Calculates the confidence interval (mean +- delta) and some other related values, like 103 | // standard deviation, etc. See https://en.wikipedia.org/wiki/Student%27s_t-distribution 104 | void calcDelta(); 105 | 106 | int count; 107 | bool complete; 108 | Vector sum; 109 | Vector sumSq; 110 | Vector mean; 111 | Vector median; 112 | Vector sigma; 113 | Vector delta; 114 | std::vector xData; 115 | std::vector yData; 116 | std::vector zData; 117 | }; 118 | -------------------------------------------------------------------------------- /tracking/imu/lsm6dso.c: -------------------------------------------------------------------------------- 1 | #include "imu.h" 2 | #include "../../lib/lsm6dso-api/lsm6dso_reg.h" 3 | 4 | #define LSM6DSO_TAG "LSM6DO" 5 | #define LSM6DSO_DEV_ADDRESS (0x6B << 1) 6 | 7 | stmdev_ctx_t lsm6dso_ctx; 8 | 9 | int32_t lsm6dso_write_i2c(void* handle, uint8_t reg_addr, uint8_t* data, uint16_t len) { 10 | if(furi_hal_i2c_write_mem(handle, LSM6DSO_DEV_ADDRESS, reg_addr, data, len, 50)) 11 | return 0; 12 | return -2; 13 | } 14 | 15 | int32_t lsm6dso_read_i2c(void* handle, uint8_t reg_addr, uint8_t* read_data, uint16_t len) { 16 | if(furi_hal_i2c_read_mem(handle, LSM6DSO_DEV_ADDRESS, reg_addr, read_data, len, 50)) 17 | return 0; 18 | return -2; 19 | } 20 | 21 | bool lsm6dso_begin() { 22 | FURI_LOG_I(LSM6DSO_TAG, "Init LSM6DSOTR-C"); 23 | 24 | if(!furi_hal_i2c_is_device_ready(&furi_hal_i2c_handle_external, LSM6DSO_DEV_ADDRESS, 50)) { 25 | FURI_LOG_E(LSM6DSO_TAG, "Not ready"); 26 | return false; 27 | } 28 | 29 | lsm6dso_ctx.write_reg = lsm6dso_write_i2c; 30 | lsm6dso_ctx.read_reg = lsm6dso_read_i2c; 31 | lsm6dso_ctx.mdelay = furi_delay_ms; 32 | lsm6dso_ctx.handle = (void*)&furi_hal_i2c_handle_external; 33 | 34 | uint8_t whoami; 35 | lsm6dso_device_id_get(&lsm6dso_ctx, &whoami); 36 | if(whoami != LSM6DSO_ID) { 37 | FURI_LOG_I(LSM6DSO_TAG, "Unknown model: %x", (int)whoami); 38 | return false; 39 | } 40 | 41 | lsm6dso_reset_set(&lsm6dso_ctx, PROPERTY_ENABLE); 42 | uint8_t rst = PROPERTY_ENABLE; 43 | while(rst) lsm6dso_reset_get(&lsm6dso_ctx, &rst); 44 | 45 | lsm6dso_block_data_update_set(&lsm6dso_ctx, PROPERTY_ENABLE); 46 | lsm6dso_fifo_mode_set(&lsm6dso_ctx, LSM6DSO_BYPASS_MODE); 47 | 48 | lsm6dso_xl_data_rate_set(&lsm6dso_ctx, LSM6DSO_XL_ODR_104Hz); 49 | lsm6dso_xl_full_scale_set(&lsm6dso_ctx, LSM6DSO_4g); 50 | //lsm6dso_xl_lp1_bandwidth_set(&lsm6dso_ctx, LSM6DSO_XL_LP1_ODR_DIV_4); 51 | 52 | lsm6dso_gy_data_rate_set(&lsm6dso_ctx, LSM6DSO_GY_ODR_104Hz); 53 | lsm6dso_gy_full_scale_set(&lsm6dso_ctx, LSM6DSO_2000dps); 54 | lsm6dso_gy_power_mode_set(&lsm6dso_ctx, LSM6DSO_GY_HIGH_PERFORMANCE); 55 | //lsm6dso_gy_band_pass_set(&lsm6dso_ctx, LSM6DSO_LP2_ONLY); 56 | 57 | FURI_LOG_I(LSM6DSO_TAG, "Init OK"); 58 | return true; 59 | } 60 | 61 | void lsm6dso_end() { 62 | lsm6dso_xl_data_rate_set(&lsm6dso_ctx, LSM6DSO_XL_ODR_OFF); 63 | lsm6dso_gy_data_rate_set(&lsm6dso_ctx, LSM6DSO_GY_ODR_OFF); 64 | } 65 | 66 | int lsm6dso_read(double* vec) { 67 | int ret = 0; 68 | int16_t data[3]; 69 | lsm6dso_reg_t reg; 70 | lsm6dso_status_reg_get(&lsm6dso_ctx, ®.status_reg); 71 | 72 | if(reg.status_reg.xlda) { 73 | lsm6dso_acceleration_raw_get(&lsm6dso_ctx, data); 74 | vec[2] = (double)lsm6dso_from_fs2_to_mg(data[0]) / 1000; 75 | vec[0] = (double)lsm6dso_from_fs2_to_mg(data[1]) / 1000; 76 | vec[1] = (double)lsm6dso_from_fs2_to_mg(data[2]) / 1000; 77 | ret |= ACC_DATA_READY; 78 | } 79 | 80 | if(reg.status_reg.gda) { 81 | lsm6dso_angular_rate_raw_get(&lsm6dso_ctx, data); 82 | vec[5] = (double)lsm6dso_from_fs2000_to_mdps(data[0]) * DEG_TO_RAD / 1000; 83 | vec[3] = (double)lsm6dso_from_fs2000_to_mdps(data[1]) * DEG_TO_RAD / 1000; 84 | vec[4] = (double)lsm6dso_from_fs2000_to_mdps(data[2]) * DEG_TO_RAD / 1000; 85 | ret |= GYR_DATA_READY; 86 | } 87 | 88 | return ret; 89 | } 90 | 91 | struct imu_t imu_lsm6dso = { 92 | LSM6DSO_DEV_ADDRESS, 93 | lsm6dso_begin, 94 | lsm6dso_end, 95 | lsm6dso_read, 96 | LSM6DSO_TAG 97 | }; 98 | -------------------------------------------------------------------------------- /tracking/util/matrix_3x3.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2019 Google Inc. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | #include "matrix_3x3.h" 17 | 18 | namespace cardboard { 19 | 20 | Matrix3x3::Matrix3x3(double m00, double m01, double m02, double m10, double m11, double m12, 21 | double m20, double m21, double m22) 22 | : elem_ { { { m00, m01, m02 }, { m10, m11, m12 }, { m20, m21, m22 } } } 23 | { 24 | } 25 | 26 | Matrix3x3::Matrix3x3() 27 | { 28 | for (int row = 0; row < 3; ++row) { 29 | for (int col = 0; col < 3; ++col) 30 | elem_[row][col] = 0; 31 | } 32 | } 33 | 34 | Matrix3x3 Matrix3x3::Zero() 35 | { 36 | Matrix3x3 result; 37 | return result; 38 | } 39 | 40 | Matrix3x3 Matrix3x3::Identity() 41 | { 42 | Matrix3x3 result; 43 | for (int row = 0; row < 3; ++row) { 44 | result.elem_[row][row] = 1; 45 | } 46 | return result; 47 | } 48 | 49 | void Matrix3x3::MultiplyScalar(double s) 50 | { 51 | for (int row = 0; row < 3; ++row) { 52 | for (int col = 0; col < 3; ++col) 53 | elem_[row][col] *= s; 54 | } 55 | } 56 | 57 | Matrix3x3 Matrix3x3::Negation() const 58 | { 59 | Matrix3x3 result; 60 | for (int row = 0; row < 3; ++row) { 61 | for (int col = 0; col < 3; ++col) 62 | result.elem_[row][col] = -elem_[row][col]; 63 | } 64 | return result; 65 | } 66 | 67 | Matrix3x3 Matrix3x3::Scale(const Matrix3x3& m, double s) 68 | { 69 | Matrix3x3 result; 70 | for (int row = 0; row < 3; ++row) { 71 | for (int col = 0; col < 3; ++col) 72 | result.elem_[row][col] = m.elem_[row][col] * s; 73 | } 74 | return result; 75 | } 76 | 77 | Matrix3x3 Matrix3x3::Addition(const Matrix3x3& lhs, const Matrix3x3& rhs) 78 | { 79 | Matrix3x3 result; 80 | for (int row = 0; row < 3; ++row) { 81 | for (int col = 0; col < 3; ++col) 82 | result.elem_[row][col] = lhs.elem_[row][col] + rhs.elem_[row][col]; 83 | } 84 | return result; 85 | } 86 | 87 | Matrix3x3 Matrix3x3::Subtraction(const Matrix3x3& lhs, const Matrix3x3& rhs) 88 | { 89 | Matrix3x3 result; 90 | for (int row = 0; row < 3; ++row) { 91 | for (int col = 0; col < 3; ++col) 92 | result.elem_[row][col] = lhs.elem_[row][col] - rhs.elem_[row][col]; 93 | } 94 | return result; 95 | } 96 | 97 | Matrix3x3 Matrix3x3::Product(const Matrix3x3& m0, const Matrix3x3& m1) 98 | { 99 | Matrix3x3 result; 100 | for (int row = 0; row < 3; ++row) { 101 | for (int col = 0; col < 3; ++col) { 102 | result.elem_[row][col] = 0; 103 | for (int i = 0; i < 3; ++i) 104 | result.elem_[row][col] += m0.elem_[row][i] * m1.elem_[i][col]; 105 | } 106 | } 107 | return result; 108 | } 109 | 110 | bool Matrix3x3::AreEqual(const Matrix3x3& m0, const Matrix3x3& m1) 111 | { 112 | for (int row = 0; row < 3; ++row) { 113 | for (int col = 0; col < 3; ++col) { 114 | if (m0.elem_[row][col] != m1.elem_[row][col]) 115 | return false; 116 | } 117 | } 118 | return true; 119 | } 120 | 121 | } // namespace cardboard 122 | -------------------------------------------------------------------------------- /tracking/imu/lsm6ds3trc.c: -------------------------------------------------------------------------------- 1 | #include "imu.h" 2 | #include "../../lib/lsm6ds3tr-api/lsm6ds3tr-c_reg.h" 3 | 4 | #define LSM6DS3_TAG "LSM6DS3" 5 | #define LSM6DS3_DEV_ADDRESS (0x6A << 1) 6 | 7 | stmdev_ctx_t lsm6ds3trc_ctx; 8 | 9 | int32_t lsm6ds3trc_write_i2c(void* handle, uint8_t reg_addr, const uint8_t* data, uint16_t len) { 10 | if(furi_hal_i2c_write_mem(handle, LSM6DS3_DEV_ADDRESS, reg_addr, (uint8_t*)data, len, 50)) 11 | return 0; 12 | return -1; 13 | } 14 | 15 | int32_t lsm6ds3trc_read_i2c(void* handle, uint8_t reg_addr, uint8_t* read_data, uint16_t len) { 16 | if(furi_hal_i2c_read_mem(handle, LSM6DS3_DEV_ADDRESS, reg_addr, read_data, len, 50)) 17 | return 0; 18 | return -1; 19 | } 20 | 21 | bool lsm6ds3trc_begin() { 22 | FURI_LOG_I(LSM6DS3_TAG, "Init LSM6DS3TR-C"); 23 | 24 | if(!furi_hal_i2c_is_device_ready(&furi_hal_i2c_handle_external, LSM6DS3_DEV_ADDRESS, 50)) { 25 | FURI_LOG_E(LSM6DS3_TAG, "Not ready"); 26 | return false; 27 | } 28 | 29 | lsm6ds3trc_ctx.write_reg = lsm6ds3trc_write_i2c; 30 | lsm6ds3trc_ctx.read_reg = lsm6ds3trc_read_i2c; 31 | lsm6ds3trc_ctx.mdelay = furi_delay_ms; 32 | lsm6ds3trc_ctx.handle = (void*)&furi_hal_i2c_handle_external; 33 | 34 | uint8_t whoami; 35 | lsm6ds3tr_c_device_id_get(&lsm6ds3trc_ctx, &whoami); 36 | if(whoami != LSM6DS3TR_C_ID) { 37 | FURI_LOG_I(LSM6DS3_TAG, "Unknown model: %x", (int)whoami); 38 | return false; 39 | } 40 | 41 | lsm6ds3tr_c_reset_set(&lsm6ds3trc_ctx, PROPERTY_ENABLE); 42 | uint8_t rst = PROPERTY_ENABLE; 43 | while(rst) lsm6ds3tr_c_reset_get(&lsm6ds3trc_ctx, &rst); 44 | 45 | lsm6ds3tr_c_block_data_update_set(&lsm6ds3trc_ctx, PROPERTY_ENABLE); 46 | lsm6ds3tr_c_fifo_mode_set(&lsm6ds3trc_ctx, LSM6DS3TR_C_BYPASS_MODE); 47 | 48 | lsm6ds3tr_c_xl_data_rate_set(&lsm6ds3trc_ctx, LSM6DS3TR_C_XL_ODR_104Hz); 49 | lsm6ds3tr_c_xl_full_scale_set(&lsm6ds3trc_ctx, LSM6DS3TR_C_4g); 50 | lsm6ds3tr_c_xl_lp1_bandwidth_set(&lsm6ds3trc_ctx, LSM6DS3TR_C_XL_LP1_ODR_DIV_4); 51 | 52 | lsm6ds3tr_c_gy_data_rate_set(&lsm6ds3trc_ctx, LSM6DS3TR_C_GY_ODR_104Hz); 53 | lsm6ds3tr_c_gy_full_scale_set(&lsm6ds3trc_ctx, LSM6DS3TR_C_2000dps); 54 | lsm6ds3tr_c_gy_power_mode_set(&lsm6ds3trc_ctx, LSM6DS3TR_C_GY_HIGH_PERFORMANCE); 55 | lsm6ds3tr_c_gy_band_pass_set(&lsm6ds3trc_ctx, LSM6DS3TR_C_LP2_ONLY); 56 | 57 | FURI_LOG_I(LSM6DS3_TAG, "Init OK"); 58 | return true; 59 | } 60 | 61 | void lsm6ds3trc_end() { 62 | lsm6ds3tr_c_xl_data_rate_set(&lsm6ds3trc_ctx, LSM6DS3TR_C_XL_ODR_OFF); 63 | lsm6ds3tr_c_gy_data_rate_set(&lsm6ds3trc_ctx, LSM6DS3TR_C_GY_ODR_OFF); 64 | } 65 | 66 | int lsm6ds3trc_read(double* vec) { 67 | int ret = 0; 68 | int16_t data[3]; 69 | lsm6ds3tr_c_reg_t reg; 70 | lsm6ds3tr_c_status_reg_get(&lsm6ds3trc_ctx, ®.status_reg); 71 | 72 | if(reg.status_reg.xlda) { 73 | lsm6ds3tr_c_acceleration_raw_get(&lsm6ds3trc_ctx, data); 74 | vec[1] = (double)lsm6ds3tr_c_from_fs2g_to_mg(data[0]) / 1000; 75 | vec[0] = (double)lsm6ds3tr_c_from_fs2g_to_mg(data[1]) / 1000; 76 | vec[2] = -(double)lsm6ds3tr_c_from_fs2g_to_mg(data[2]) / 1000; 77 | ret |= ACC_DATA_READY; 78 | } 79 | 80 | if(reg.status_reg.gda) { 81 | lsm6ds3tr_c_angular_rate_raw_get(&lsm6ds3trc_ctx, data); 82 | vec[4] = (double)lsm6ds3tr_c_from_fs2000dps_to_mdps(data[0]) * DEG_TO_RAD / 1000; 83 | vec[3] = (double)lsm6ds3tr_c_from_fs2000dps_to_mdps(data[1]) * DEG_TO_RAD / 1000; 84 | vec[5] = -(double)lsm6ds3tr_c_from_fs2000dps_to_mdps(data[2]) * DEG_TO_RAD / 1000; 85 | ret |= GYR_DATA_READY; 86 | } 87 | 88 | return ret; 89 | } 90 | 91 | struct imu_t imu_lsm6ds3trc = { 92 | LSM6DS3_DEV_ADDRESS, 93 | lsm6ds3trc_begin, 94 | lsm6ds3trc_end, 95 | lsm6ds3trc_read, 96 | LSM6DS3_TAG 97 | }; 98 | -------------------------------------------------------------------------------- /tracking/util/rotation.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2019 Google Inc. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | #include "rotation.h" 17 | 18 | #include 19 | #include 20 | 21 | #include "vectorutils.h" 22 | 23 | namespace cardboard { 24 | 25 | void Rotation::SetAxisAndAngle(const VectorType& axis, double angle) 26 | { 27 | VectorType unit_axis = axis; 28 | if (!Normalize(&unit_axis)) { 29 | *this = Identity(); 30 | } else { 31 | double a = angle / 2; 32 | const double s = sin(a); 33 | const VectorType v(unit_axis * s); 34 | SetQuaternion(QuaternionType(v[0], v[1], v[2], cos(a))); 35 | } 36 | } 37 | 38 | Rotation Rotation::FromRotationMatrix(const Matrix3x3& mat) 39 | { 40 | static const double kOne = 1.0; 41 | static const double kFour = 4.0; 42 | 43 | const double d0 = mat(0, 0), d1 = mat(1, 1), d2 = mat(2, 2); 44 | const double ww = kOne + d0 + d1 + d2; 45 | const double xx = kOne + d0 - d1 - d2; 46 | const double yy = kOne - d0 + d1 - d2; 47 | const double zz = kOne - d0 - d1 + d2; 48 | 49 | const double max = std::max(ww, std::max(xx, std::max(yy, zz))); 50 | if (ww == max) { 51 | const double w4 = sqrt(ww * kFour); 52 | return Rotation::FromQuaternion(QuaternionType((mat(2, 1) - mat(1, 2)) / w4, 53 | (mat(0, 2) - mat(2, 0)) / w4, (mat(1, 0) - mat(0, 1)) / w4, w4 / kFour)); 54 | } 55 | 56 | if (xx == max) { 57 | const double x4 = sqrt(xx * kFour); 58 | return Rotation::FromQuaternion(QuaternionType(x4 / kFour, (mat(0, 1) + mat(1, 0)) / x4, 59 | (mat(0, 2) + mat(2, 0)) / x4, (mat(2, 1) - mat(1, 2)) / x4)); 60 | } 61 | 62 | if (yy == max) { 63 | const double y4 = sqrt(yy * kFour); 64 | return Rotation::FromQuaternion(QuaternionType((mat(0, 1) + mat(1, 0)) / y4, y4 / kFour, 65 | (mat(1, 2) + mat(2, 1)) / y4, (mat(0, 2) - mat(2, 0)) / y4)); 66 | } 67 | 68 | // zz is the largest component. 69 | const double z4 = sqrt(zz * kFour); 70 | return Rotation::FromQuaternion(QuaternionType((mat(0, 2) + mat(2, 0)) / z4, 71 | (mat(1, 2) + mat(2, 1)) / z4, z4 / kFour, (mat(1, 0) - mat(0, 1)) / z4)); 72 | } 73 | 74 | void Rotation::GetAxisAndAngle(VectorType* axis, double* angle) const 75 | { 76 | VectorType vec(quat_[0], quat_[1], quat_[2]); 77 | if (Normalize(&vec)) { 78 | *angle = 2 * acos(quat_[3]); 79 | *axis = vec; 80 | } else { 81 | *axis = VectorType(1, 0, 0); 82 | *angle = 0.0; 83 | } 84 | } 85 | 86 | Rotation Rotation::RotateInto(const VectorType& from, const VectorType& to) 87 | { 88 | static const double kTolerance = std::numeric_limits::epsilon() * 100; 89 | 90 | // Directly build the quaternion using the following technique: 91 | // http://lolengine.net/blog/2014/02/24/quaternion-from-two-vectors-final 92 | const double norm_u_norm_v = sqrt(LengthSquared(from) * LengthSquared(to)); 93 | double real_part = norm_u_norm_v + Dot(from, to); 94 | VectorType w; 95 | if (real_part < kTolerance * norm_u_norm_v) { 96 | // If |from| and |to| are exactly opposite, rotate 180 degrees around an 97 | // arbitrary orthogonal axis. Axis normalization can happen later, when we 98 | // normalize the quaternion. 99 | real_part = 0.0; 100 | w = (abs(from[0]) > abs(from[2])) ? VectorType(-from[1], from[0], 0) 101 | : VectorType(0, -from[2], from[1]); 102 | } else { 103 | // Otherwise, build the quaternion the standard way. 104 | w = Cross(from, to); 105 | } 106 | 107 | // Build and return a normalized quaternion. 108 | // Note that Rotation::FromQuaternion automatically performs normalization. 109 | return Rotation::FromQuaternion(QuaternionType(w[0], w[1], w[2], real_part)); 110 | } 111 | 112 | Rotation::VectorType Rotation::operator*(const Rotation::VectorType& v) const 113 | { 114 | return ApplyToVector(v); 115 | } 116 | 117 | } // namespace cardboard 118 | -------------------------------------------------------------------------------- /tracking/util/matrix_3x3.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2019 Google Inc. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | #ifndef CARDBOARD_SDK_UTIL_MATRIX_3X3_H_ 17 | #define CARDBOARD_SDK_UTIL_MATRIX_3X3_H_ 18 | 19 | #include 20 | #include // For memcpy(). 21 | #include // NOLINT 22 | #include // NOLINT 23 | 24 | namespace cardboard { 25 | 26 | // The Matrix3x3 class defines a square 3-dimensional matrix. Elements are 27 | // stored in row-major order. 28 | // TODO(b/135461889): Make this class consistent with Matrix4x4. 29 | class Matrix3x3 { 30 | public: 31 | // The default constructor zero-initializes all elements. 32 | Matrix3x3(); 33 | 34 | // Dimension-specific constructors that are passed individual element values. 35 | Matrix3x3( 36 | double m00, 37 | double m01, 38 | double m02, 39 | double m10, 40 | double m11, 41 | double m12, 42 | double m20, 43 | double m21, 44 | double m22); 45 | 46 | // Constructor that reads elements from a linear array of the correct size. 47 | explicit Matrix3x3(const double array[3 * 3]); 48 | 49 | // Returns a Matrix3x3 containing all zeroes. 50 | static Matrix3x3 Zero(); 51 | 52 | // Returns an identity Matrix3x3. 53 | static Matrix3x3 Identity(); 54 | 55 | // Mutable element accessors. 56 | double& operator()(int row, int col) { 57 | return elem_[row][col]; 58 | } 59 | std::array& operator[](int row) { 60 | return elem_[row]; 61 | } 62 | 63 | // Read-only element accessors. 64 | const double& operator()(int row, int col) const { 65 | return elem_[row][col]; 66 | } 67 | const std::array& operator[](int row) const { 68 | return elem_[row]; 69 | } 70 | 71 | // Return a pointer to the data for interfacing with libraries. 72 | double* Data() { 73 | return &elem_[0][0]; 74 | } 75 | const double* Data() const { 76 | return &elem_[0][0]; 77 | } 78 | 79 | // Self-modifying multiplication operators. 80 | void operator*=(double s) { 81 | MultiplyScalar(s); 82 | } 83 | void operator*=(const Matrix3x3& m) { 84 | *this = Product(*this, m); 85 | } 86 | 87 | // Unary operators. 88 | Matrix3x3 operator-() const { 89 | return Negation(); 90 | } 91 | 92 | // Binary scale operators. 93 | friend Matrix3x3 operator*(const Matrix3x3& m, double s) { 94 | return Scale(m, s); 95 | } 96 | friend Matrix3x3 operator*(double s, const Matrix3x3& m) { 97 | return Scale(m, s); 98 | } 99 | 100 | // Binary matrix addition. 101 | friend Matrix3x3 operator+(const Matrix3x3& lhs, const Matrix3x3& rhs) { 102 | return Addition(lhs, rhs); 103 | } 104 | 105 | // Binary matrix subtraction. 106 | friend Matrix3x3 operator-(const Matrix3x3& lhs, const Matrix3x3& rhs) { 107 | return Subtraction(lhs, rhs); 108 | } 109 | 110 | // Binary multiplication operator. 111 | friend Matrix3x3 operator*(const Matrix3x3& m0, const Matrix3x3& m1) { 112 | return Product(m0, m1); 113 | } 114 | 115 | // Exact equality and inequality comparisons. 116 | friend bool operator==(const Matrix3x3& m0, const Matrix3x3& m1) { 117 | return AreEqual(m0, m1); 118 | } 119 | friend bool operator!=(const Matrix3x3& m0, const Matrix3x3& m1) { 120 | return !AreEqual(m0, m1); 121 | } 122 | 123 | private: 124 | // These private functions implement most of the operators. 125 | void MultiplyScalar(double s); 126 | Matrix3x3 Negation() const; 127 | static Matrix3x3 Addition(const Matrix3x3& lhs, const Matrix3x3& rhs); 128 | static Matrix3x3 Subtraction(const Matrix3x3& lhs, const Matrix3x3& rhs); 129 | static Matrix3x3 Scale(const Matrix3x3& m, double s); 130 | static Matrix3x3 Product(const Matrix3x3& m0, const Matrix3x3& m1); 131 | static bool AreEqual(const Matrix3x3& m0, const Matrix3x3& m1); 132 | 133 | std::array, 3> elem_; 134 | }; 135 | 136 | } // namespace cardboard 137 | 138 | #endif // CARDBOARD_SDK_UTIL_MATRIX_3X3_H_ 139 | -------------------------------------------------------------------------------- /views/usb_mouse.c: -------------------------------------------------------------------------------- 1 | #include "usb_mouse.h" 2 | #include "../tracking/main_loop.h" 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | struct UsbMouse { 10 | View* view; 11 | ViewDispatcher* view_dispatcher; 12 | FuriHalUsbInterface* usb_mode_prev; 13 | }; 14 | 15 | static void usb_mouse_draw_callback(Canvas* canvas, void* context) { 16 | UNUSED(context); 17 | canvas_clear(canvas); 18 | canvas_set_font(canvas, FontPrimary); 19 | canvas_draw_str(canvas, 0, 10, "USB Mouse mode"); 20 | canvas_set_font(canvas, FontSecondary); 21 | canvas_draw_str(canvas, 0, 63, "Hold [back] to exit"); 22 | } 23 | 24 | #define MOUSE_SCROLL 2 25 | 26 | static void usb_mouse_process(UsbMouse* usb_mouse, InputEvent* event) { 27 | with_view_model( 28 | usb_mouse->view, 29 | void* model, 30 | { 31 | UNUSED(model); 32 | if(event->key == InputKeyUp) { 33 | if(event->type == InputTypePress) { 34 | furi_hal_hid_mouse_press(HID_MOUSE_BTN_LEFT); 35 | } else if(event->type == InputTypeRelease) { 36 | furi_hal_hid_mouse_release(HID_MOUSE_BTN_LEFT); 37 | } 38 | } else if(event->key == InputKeyDown) { 39 | if(event->type == InputTypePress) { 40 | furi_hal_hid_mouse_press(HID_MOUSE_BTN_RIGHT); 41 | } else if(event->type == InputTypeRelease) { 42 | furi_hal_hid_mouse_release(HID_MOUSE_BTN_RIGHT); 43 | } 44 | } else if(event->key == InputKeyOk) { 45 | if(event->type == InputTypePress) { 46 | furi_hal_hid_mouse_press(HID_MOUSE_BTN_WHEEL); 47 | } else if(event->type == InputTypeRelease) { 48 | furi_hal_hid_mouse_release(HID_MOUSE_BTN_WHEEL); 49 | } 50 | } else if(event->key == InputKeyRight) { 51 | if(event->type == InputTypePress || event->type == InputTypeRepeat) { 52 | furi_hal_hid_mouse_scroll(MOUSE_SCROLL); 53 | } 54 | } else if(event->key == InputKeyLeft) { 55 | if(event->type == InputTypePress || event->type == InputTypeRepeat) { 56 | furi_hal_hid_mouse_scroll(-MOUSE_SCROLL); 57 | } 58 | } 59 | }, 60 | true); 61 | } 62 | 63 | static bool usb_mouse_input_callback(InputEvent* event, void* context) { 64 | furi_assert(context); 65 | UsbMouse* usb_mouse = context; 66 | bool consumed = false; 67 | 68 | if(event->type == InputTypeLong && event->key == InputKeyBack) { 69 | // furi_hal_hid_mouse_release_all(); 70 | } else { 71 | usb_mouse_process(usb_mouse, event); 72 | consumed = true; 73 | } 74 | 75 | return consumed; 76 | } 77 | 78 | bool usb_mouse_move(int8_t dx, int8_t dy, void* context) { 79 | UNUSED(context); 80 | return furi_hal_hid_mouse_move(dx, dy); 81 | } 82 | 83 | void usb_mouse_tick_event_callback(void* context) { 84 | furi_assert(context); 85 | tracking_step(usb_mouse_move, context); 86 | } 87 | 88 | void usb_mouse_enter_callback(void* context) { 89 | furi_assert(context); 90 | UsbMouse* usb_mouse = context; 91 | 92 | usb_mouse->usb_mode_prev = furi_hal_usb_get_config(); 93 | furi_hal_usb_unlock(); 94 | furi_check(furi_hal_usb_set_config(&usb_hid, NULL) == true); 95 | 96 | tracking_begin(); 97 | 98 | view_dispatcher_set_event_callback_context(usb_mouse->view_dispatcher, usb_mouse); 99 | view_dispatcher_set_tick_event_callback(usb_mouse->view_dispatcher, usb_mouse_tick_event_callback, furi_ms_to_ticks(10)); 100 | } 101 | 102 | void usb_mouse_exit_callback(void* context) { 103 | furi_assert(context); 104 | UsbMouse* usb_mouse = context; 105 | 106 | view_dispatcher_set_tick_event_callback(usb_mouse->view_dispatcher, NULL, FuriWaitForever); 107 | 108 | tracking_end(); 109 | 110 | furi_hal_usb_set_config(usb_mouse->usb_mode_prev, NULL); 111 | } 112 | 113 | UsbMouse* usb_mouse_alloc(ViewDispatcher* view_dispatcher) { 114 | UsbMouse* usb_mouse = malloc(sizeof(UsbMouse)); 115 | usb_mouse->view = view_alloc(); 116 | usb_mouse->view_dispatcher = view_dispatcher; 117 | view_set_context(usb_mouse->view, usb_mouse); 118 | view_set_draw_callback(usb_mouse->view, usb_mouse_draw_callback); 119 | view_set_input_callback(usb_mouse->view, usb_mouse_input_callback); 120 | view_set_enter_callback(usb_mouse->view, usb_mouse_enter_callback); 121 | view_set_exit_callback(usb_mouse->view, usb_mouse_exit_callback); 122 | return usb_mouse; 123 | } 124 | 125 | void usb_mouse_free(UsbMouse* usb_mouse) { 126 | furi_assert(usb_mouse); 127 | view_free(usb_mouse->view); 128 | free(usb_mouse); 129 | } 130 | 131 | View* usb_mouse_get_view(UsbMouse* usb_mouse) { 132 | furi_assert(usb_mouse); 133 | return usb_mouse->view; 134 | } 135 | -------------------------------------------------------------------------------- /tracking/util/matrixutils.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2019 Google Inc. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | #include "matrixutils.h" 17 | 18 | #include "vectorutils.h" 19 | 20 | namespace cardboard { 21 | 22 | namespace { 23 | 24 | // Returns true if the cofactor for a given row and column should be negated. 25 | static bool IsCofactorNegated(int row, int col) 26 | { 27 | // Negated iff (row + col) is odd. 28 | return ((row + col) & 1) != 0; 29 | } 30 | 31 | static double CofactorElement3(const Matrix3x3& m, int row, int col) 32 | { 33 | static const int index[3][2] = { { 1, 2 }, { 0, 2 }, { 0, 1 } }; 34 | const int i0 = index[row][0]; 35 | const int i1 = index[row][1]; 36 | const int j0 = index[col][0]; 37 | const int j1 = index[col][1]; 38 | const double cofactor = m(i0, j0) * m(i1, j1) - m(i0, j1) * m(i1, j0); 39 | return IsCofactorNegated(row, col) ? -cofactor : cofactor; 40 | } 41 | 42 | // Multiplies a matrix and some type of column vector to 43 | // produce another column vector of the same type. 44 | Vector3 MultiplyMatrixAndVector(const Matrix3x3& m, const Vector3& v) 45 | { 46 | Vector3 result = Vector3::Zero(); 47 | for (int row = 0; row < 3; ++row) { 48 | for (int col = 0; col < 3; ++col) 49 | result[row] += m(row, col) * v[col]; 50 | } 51 | return result; 52 | } 53 | 54 | // Sets the upper 3x3 of a Matrix to represent a 3D rotation. 55 | void RotationMatrix3x3(const Rotation& r, Matrix3x3* matrix) 56 | { 57 | // 58 | // Given a quaternion (a,b,c,d) where d is the scalar part, the 3x3 rotation 59 | // matrix is: 60 | // 61 | // a^2 - b^2 - c^2 + d^2 2ab - 2cd 2ac + 2bd 62 | // 2ab + 2cd -a^2 + b^2 - c^2 + d^2 2bc - 2ad 63 | // 2ac - 2bd 2bc + 2ad -a^2 - b^2 + c^2 + d^2 64 | // 65 | const Vector<4>& quat = r.GetQuaternion(); 66 | const double aa = quat[0] * quat[0]; 67 | const double bb = quat[1] * quat[1]; 68 | const double cc = quat[2] * quat[2]; 69 | const double dd = quat[3] * quat[3]; 70 | 71 | const double ab = quat[0] * quat[1]; 72 | const double ac = quat[0] * quat[2]; 73 | const double bc = quat[1] * quat[2]; 74 | 75 | const double ad = quat[0] * quat[3]; 76 | const double bd = quat[1] * quat[3]; 77 | const double cd = quat[2] * quat[3]; 78 | 79 | Matrix3x3& m = *matrix; 80 | m[0][0] = aa - bb - cc + dd; 81 | m[0][1] = 2 * ab - 2 * cd; 82 | m[0][2] = 2 * ac + 2 * bd; 83 | m[1][0] = 2 * ab + 2 * cd; 84 | m[1][1] = -aa + bb - cc + dd; 85 | m[1][2] = 2 * bc - 2 * ad; 86 | m[2][0] = 2 * ac - 2 * bd; 87 | m[2][1] = 2 * bc + 2 * ad; 88 | m[2][2] = -aa - bb + cc + dd; 89 | } 90 | 91 | } // anonymous namespace 92 | 93 | Vector3 operator*(const Matrix3x3& m, const Vector3& v) { return MultiplyMatrixAndVector(m, v); } 94 | 95 | Matrix3x3 CofactorMatrix(const Matrix3x3& m) 96 | { 97 | Matrix3x3 result; 98 | for (int row = 0; row < 3; ++row) { 99 | for (int col = 0; col < 3; ++col) 100 | result(row, col) = CofactorElement3(m, row, col); 101 | } 102 | return result; 103 | } 104 | 105 | Matrix3x3 AdjugateWithDeterminant(const Matrix3x3& m, double* determinant) 106 | { 107 | const Matrix3x3 cofactor_matrix = CofactorMatrix(m); 108 | if (determinant) { 109 | *determinant = m(0, 0) * cofactor_matrix(0, 0) + m(0, 1) * cofactor_matrix(0, 1) 110 | + m(0, 2) * cofactor_matrix(0, 2); 111 | } 112 | return Transpose(cofactor_matrix); 113 | } 114 | 115 | // Returns the transpose of a matrix. 116 | Matrix3x3 Transpose(const Matrix3x3& m) 117 | { 118 | Matrix3x3 result; 119 | for (int row = 0; row < 3; ++row) { 120 | for (int col = 0; col < 3; ++col) 121 | result(row, col) = m(col, row); 122 | } 123 | return result; 124 | } 125 | 126 | Matrix3x3 InverseWithDeterminant(const Matrix3x3& m, double* determinant) 127 | { 128 | // The inverse is the adjugate divided by the determinant. 129 | double det; 130 | Matrix3x3 adjugate = AdjugateWithDeterminant(m, &det); 131 | if (determinant) 132 | *determinant = det; 133 | if (det == 0) 134 | return Matrix3x3::Zero(); 135 | else 136 | return adjugate * (1 / det); 137 | } 138 | 139 | Matrix3x3 Inverse(const Matrix3x3& m) { return InverseWithDeterminant(m, nullptr); } 140 | 141 | Matrix3x3 RotationMatrixNH(const Rotation& r) 142 | { 143 | Matrix3x3 m; 144 | RotationMatrix3x3(r, &m); 145 | return m; 146 | } 147 | 148 | } // namespace cardboard 149 | -------------------------------------------------------------------------------- /tracking/sensors/gyroscope_bias_estimator.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2019 Google Inc. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | #ifndef CARDBOARD_SDK_SENSORS_GYROSCOPE_BIAS_ESTIMATOR_H_ 17 | #define CARDBOARD_SDK_SENSORS_GYROSCOPE_BIAS_ESTIMATOR_H_ 18 | 19 | #include // NOLINT 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | #include "lowpass_filter.h" 26 | #include "mean_filter.h" 27 | #include "median_filter.h" 28 | #include "../util/vector.h" 29 | 30 | namespace cardboard { 31 | 32 | // Class that attempts to estimate the gyroscope's bias. 33 | // Its main idea is that it averages the gyroscope values when the phone is 34 | // considered stationary. 35 | // Usage: A client should call the ProcessGyroscope and ProcessAccelerometer 36 | // methods for every accelerometer and gyroscope sensor sample. This class 37 | // expects these calls to be frequent, i.e., at least at 10 Hz. The client can 38 | // then call GetGyroBias to retrieve the current estimate of the gyroscope bias. 39 | // For best results, the fastest available delay option should be used when 40 | // registering to sensors. Note that this class is not thread-safe. 41 | // 42 | // The filtering applied to the accelerometer to estimate a rotation 43 | // from it follows : 44 | // Baptiste Delporte, Laurent Perroton, Thierry Grandpierre, Jacques Trichet. 45 | // Accelerometer and Magnetometer Based Gyroscope Emulation on Smart Sensor 46 | // for a Virtual Reality Application. Sensor and Transducers Journal, 2012. 47 | // 48 | // which is a combination of a IIR filter, a median and a mean filter. 49 | class GyroscopeBiasEstimator { 50 | public: 51 | GyroscopeBiasEstimator(); 52 | virtual ~GyroscopeBiasEstimator(); 53 | 54 | // Updates the estimator with a gyroscope event. 55 | // 56 | // @param gyroscope_sample the angular speed around the x, y, z axis in 57 | // radians/sec. 58 | // @param timestamp_ns the nanosecond at which the event occurred. Only 59 | // guaranteed to be comparable with timestamps from other PocessGyroscope 60 | // invocations. 61 | virtual void ProcessGyroscope(const Vector3& gyroscope_sample, uint64_t timestamp_ns); 62 | 63 | // Processes accelerometer samples to estimate if device is 64 | // stable or not. 65 | // 66 | // First we filter the accelerometer. This is done with 3 filters. 67 | // - A IIR low-pass filter 68 | // - A median filter 69 | // - A mean filter. 70 | // Then a rotation is computed between consecutive filtered accelerometer 71 | // samples. 72 | // Finally this is converted to a velocity to emulate a gyroscope. 73 | // 74 | // @param accelerometer_sample the acceleration (including gravity) on the x, 75 | // y, z axis in meters/s^2. 76 | // @param timestamp_ns the nanosecond at which the event occurred. Only 77 | // guaranteed to be comparable with timestamps from other 78 | // ProcessAccelerometer invocations. 79 | virtual void ProcessAccelerometer(const Vector3& accelerometer_sample, uint64_t timestamp_ns); 80 | 81 | // Returns the estimated gyroscope bias. 82 | // 83 | // @return Estimated gyroscope bias. A vector with zeros is returned if no 84 | // estimate has been computed. 85 | virtual Vector3 GetGyroscopeBias() const; 86 | 87 | // Resets the estimator state. 88 | void Reset(); 89 | 90 | // Returns true if the current estimate returned by GetGyroscopeBias is 91 | // correct. The device (measured using the sensors) has to be static for this 92 | // function to return true. 93 | virtual bool IsCurrentEstimateValid() const; 94 | 95 | private: 96 | // A helper class to keep track of whether some signal can be considered 97 | // static over specified number of frames. 98 | class IsStaticCounter; 99 | 100 | // Updates gyroscope bias estimation. 101 | // 102 | // @return false if the current sample is too large. 103 | bool UpdateGyroscopeBias(const Vector3& gyroscope_sample, uint64_t timestamp_ns); 104 | 105 | // Returns device angular velocity (rad/s) from the latest accelerometer data. 106 | // 107 | // @param timestep in seconds between the last two samples. 108 | // @return rotation velocity from latest accelerometer. This can be 109 | // interpreted as an gyroscope. 110 | Vector3 ComputeAngularVelocityFromLatestAccelerometer(double timestep) const; 111 | 112 | LowpassFilter accelerometer_lowpass_filter_; 113 | LowpassFilter simulated_gyroscope_from_accelerometer_lowpass_filter_; 114 | LowpassFilter gyroscope_lowpass_filter_; 115 | LowpassFilter gyroscope_bias_lowpass_filter_; 116 | 117 | std::unique_ptr accelerometer_static_counter_; 118 | std::unique_ptr gyroscope_static_counter_; 119 | 120 | // Sum of the weight of sample used for gyroscope filtering. 121 | float current_accumulated_weights_gyroscope_bias_; 122 | 123 | // Set of filters for accelerometer data to estimate a rotation 124 | // based only on accelerometer. 125 | MeanFilter mean_filter_; 126 | MedianFilter median_filter_; 127 | 128 | // Last computed filter accelerometer value used for finite differences. 129 | Vector3 last_mean_filtered_accelerometer_value_; 130 | }; 131 | 132 | } // namespace cardboard 133 | 134 | #endif // CARDBOARD_SDK_SENSORS_GYROSCOPE_BIAS_ESTIMATOR_H_ 135 | -------------------------------------------------------------------------------- /tracking/util/rotation.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2019 Google Inc. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | #ifndef CARDBOARD_SDK_UTIL_ROTATION_H_ 17 | #define CARDBOARD_SDK_UTIL_ROTATION_H_ 18 | 19 | #include "matrix_3x3.h" 20 | #include "vector.h" 21 | #include "vectorutils.h" 22 | 23 | namespace cardboard { 24 | 25 | // The Rotation class represents a rotation around a 3-dimensional axis. It 26 | // uses normalized quaternions internally to make the math robust. 27 | class Rotation { 28 | public: 29 | // Convenience typedefs for vector of the correct type. 30 | typedef Vector<3> VectorType; 31 | typedef Vector<4> QuaternionType; 32 | 33 | // The default constructor creates an identity Rotation, which has no effect. 34 | Rotation() { 35 | quat_.Set(0, 0, 0, 1); 36 | } 37 | 38 | // Returns an identity Rotation, which has no effect. 39 | static Rotation Identity() { 40 | return Rotation(); 41 | } 42 | 43 | // Sets the Rotation from a quaternion (4D vector), which is first normalized. 44 | void SetQuaternion(const QuaternionType& quaternion) { 45 | quat_ = Normalized(quaternion); 46 | } 47 | 48 | // Returns the Rotation as a normalized quaternion (4D vector). 49 | const QuaternionType& GetQuaternion() const { 50 | return quat_; 51 | } 52 | 53 | // Sets the Rotation to rotate by the given angle around the given axis, 54 | // following the right-hand rule. The axis does not need to be unit 55 | // length. If it is zero length, this results in an identity Rotation. 56 | void SetAxisAndAngle(const VectorType& axis, double angle); 57 | 58 | // Returns the right-hand rule axis and angle corresponding to the 59 | // Rotation. If the Rotation is the identity rotation, this returns the +X 60 | // axis and an angle of 0. 61 | void GetAxisAndAngle(VectorType* axis, double* angle) const; 62 | 63 | // Convenience function that constructs and returns a Rotation given an axis 64 | // and angle. 65 | static Rotation FromAxisAndAngle(const VectorType& axis, double angle) { 66 | Rotation r; 67 | r.SetAxisAndAngle(axis, angle); 68 | return r; 69 | } 70 | 71 | // Convenience function that constructs and returns a Rotation given a 72 | // quaternion. 73 | static Rotation FromQuaternion(const QuaternionType& quat) { 74 | Rotation r; 75 | r.SetQuaternion(quat); 76 | return r; 77 | } 78 | 79 | // Convenience function that constructs and returns a Rotation given a 80 | // rotation matrix R with $R^\top R = I && det(R) = 1$. 81 | static Rotation FromRotationMatrix(const Matrix3x3& mat); 82 | 83 | // Convenience function that constructs and returns a Rotation given Euler 84 | // angles that are applied in the order of rotate-Z by roll, rotate-X by 85 | // pitch, rotate-Y by yaw (same as GetRollPitchYaw). 86 | static Rotation FromRollPitchYaw(double roll, double pitch, double yaw) { 87 | VectorType x(1, 0, 0), y(0, 1, 0), z(0, 0, 1); 88 | return FromAxisAndAngle(z, roll) * (FromAxisAndAngle(x, pitch) * FromAxisAndAngle(y, yaw)); 89 | } 90 | 91 | // Convenience function that constructs and returns a Rotation given Euler 92 | // angles that are applied in the order of rotate-Y by yaw, rotate-X by 93 | // pitch, rotate-Z by roll (same as GetYawPitchRoll). 94 | static Rotation FromYawPitchRoll(double yaw, double pitch, double roll) { 95 | VectorType x(1, 0, 0), y(0, 1, 0), z(0, 0, 1); 96 | return FromAxisAndAngle(y, yaw) * (FromAxisAndAngle(x, pitch) * FromAxisAndAngle(z, roll)); 97 | } 98 | 99 | // Constructs and returns a Rotation that rotates one vector to another along 100 | // the shortest arc. This returns an identity rotation if either vector has 101 | // zero length. 102 | static Rotation RotateInto(const VectorType& from, const VectorType& to); 103 | 104 | // The negation operator returns the inverse rotation. 105 | friend Rotation operator-(const Rotation& r) { 106 | // Because we store normalized quaternions, the inverse is found by 107 | // negating the vector part. 108 | return Rotation(-r.quat_[0], -r.quat_[1], -r.quat_[2], r.quat_[3]); 109 | } 110 | 111 | // Appends a rotation to this one. 112 | Rotation& operator*=(const Rotation& r) { 113 | const QuaternionType& qr = r.quat_; 114 | QuaternionType& qt = quat_; 115 | SetQuaternion(QuaternionType( 116 | qr[3] * qt[0] + qr[0] * qt[3] + qr[2] * qt[1] - qr[1] * qt[2], 117 | qr[3] * qt[1] + qr[1] * qt[3] + qr[0] * qt[2] - qr[2] * qt[0], 118 | qr[3] * qt[2] + qr[2] * qt[3] + qr[1] * qt[0] - qr[0] * qt[1], 119 | qr[3] * qt[3] - qr[0] * qt[0] - qr[1] * qt[1] - qr[2] * qt[2])); 120 | return *this; 121 | } 122 | 123 | // Binary multiplication operator - returns a composite Rotation. 124 | friend const Rotation operator*(const Rotation& r0, const Rotation& r1) { 125 | Rotation r = r0; 126 | r *= r1; 127 | return r; 128 | } 129 | 130 | // Multiply a Rotation and a Vector to get a Vector. 131 | VectorType operator*(const VectorType& v) const; 132 | 133 | private: 134 | // Private constructor that builds a Rotation from quaternion components. 135 | Rotation(double q0, double q1, double q2, double q3) 136 | : quat_(q0, q1, q2, q3) { 137 | } 138 | 139 | // Applies a Rotation to a Vector to rotate the Vector. Method borrowed from: 140 | // http://blog.molecular-matters.com/2013/05/24/a-faster-quaternion-vector-multiplication/ 141 | VectorType ApplyToVector(const VectorType& v) const { 142 | VectorType im(quat_[0], quat_[1], quat_[2]); 143 | VectorType temp = 2.0 * Cross(im, v); 144 | return v + quat_[3] * temp + Cross(im, temp); 145 | } 146 | 147 | // The rotation represented as a normalized quaternion. (Unit quaternions are 148 | // required for constructing rotation matrices, so it makes sense to always 149 | // store them that way.) The vector part is in the first 3 elements, and the 150 | // scalar part is in the last element. 151 | QuaternionType quat_; 152 | }; 153 | 154 | } // namespace cardboard 155 | 156 | #endif // CARDBOARD_SDK_UTIL_ROTATION_H_ 157 | -------------------------------------------------------------------------------- /air_mouse.c: -------------------------------------------------------------------------------- 1 | #include "air_mouse.h" 2 | 3 | #include 4 | #include 5 | 6 | #include "tracking/imu/imu.h" 7 | 8 | #define TAG "AirMouseApp" 9 | 10 | enum AirMouseSubmenuIndex { 11 | AirMouseSubmenuIndexBtMouse, 12 | AirMouseSubmenuIndexUsbMouse, 13 | AirMouseSubmenuIndexCalibration, 14 | AirMouseSubmenuIndexRemovePairing, 15 | }; 16 | 17 | void air_mouse_submenu_callback(void* context, uint32_t index) { 18 | furi_assert(context); 19 | AirMouse* app = context; 20 | if(index == AirMouseSubmenuIndexBtMouse) { 21 | app->view_id = AirMouseViewBtMouse; 22 | view_dispatcher_switch_to_view(app->view_dispatcher, AirMouseViewBtMouse); 23 | } else if(index == AirMouseSubmenuIndexUsbMouse) { 24 | app->view_id = AirMouseViewUsbMouse; 25 | view_dispatcher_switch_to_view(app->view_dispatcher, AirMouseViewUsbMouse); 26 | } else if(index == AirMouseSubmenuIndexCalibration) { 27 | app->view_id = AirMouseViewCalibration; 28 | view_dispatcher_switch_to_view(app->view_dispatcher, AirMouseViewCalibration); 29 | } else if(index == AirMouseSubmenuIndexRemovePairing) { 30 | bt_mouse_remove_pairing(); 31 | } 32 | } 33 | 34 | void air_mouse_dialog_callback(DialogExResult result, void* context) { 35 | furi_assert(context); 36 | AirMouse* app = context; 37 | if(result == DialogExResultLeft) { 38 | view_dispatcher_switch_to_view(app->view_dispatcher, VIEW_NONE); // Exit 39 | } else if(result == DialogExResultRight) { 40 | view_dispatcher_switch_to_view(app->view_dispatcher, app->view_id); // Show last view 41 | } else if(result == DialogExResultCenter) { 42 | view_dispatcher_switch_to_view(app->view_dispatcher, AirMouseViewSubmenu); // Menu 43 | } 44 | } 45 | 46 | uint32_t air_mouse_exit_confirm_view(void* context) { 47 | UNUSED(context); 48 | return AirMouseViewExitConfirm; 49 | } 50 | 51 | uint32_t air_mouse_exit(void* context) { 52 | UNUSED(context); 53 | return VIEW_NONE; 54 | } 55 | 56 | AirMouse* air_mouse_app_alloc() { 57 | AirMouse* app = malloc(sizeof(AirMouse)); 58 | 59 | // Gui 60 | app->gui = furi_record_open(RECORD_GUI); 61 | 62 | // View dispatcher 63 | app->view_dispatcher = view_dispatcher_alloc(); 64 | view_dispatcher_attach_to_gui(app->view_dispatcher, app->gui, ViewDispatcherTypeFullscreen); 65 | 66 | // Submenu view 67 | app->submenu = submenu_alloc(); 68 | submenu_add_item( 69 | app->submenu, "Bluetooth", AirMouseSubmenuIndexBtMouse, air_mouse_submenu_callback, app); 70 | submenu_add_item( 71 | app->submenu, "USB", AirMouseSubmenuIndexUsbMouse, air_mouse_submenu_callback, app); 72 | submenu_add_item( 73 | app->submenu, 74 | "Calibration", 75 | AirMouseSubmenuIndexCalibration, 76 | air_mouse_submenu_callback, 77 | app); 78 | submenu_add_item( 79 | app->submenu, 80 | "Clear Bluetooth Pairings", 81 | AirMouseSubmenuIndexRemovePairing, 82 | air_mouse_submenu_callback, 83 | app); 84 | view_set_previous_callback(submenu_get_view(app->submenu), air_mouse_exit); 85 | view_dispatcher_add_view( 86 | app->view_dispatcher, AirMouseViewSubmenu, submenu_get_view(app->submenu)); 87 | 88 | // Dialog views 89 | app->dialog = dialog_ex_alloc(); 90 | dialog_ex_set_result_callback(app->dialog, air_mouse_dialog_callback); 91 | dialog_ex_set_context(app->dialog, app); 92 | dialog_ex_set_left_button_text(app->dialog, "Exit"); 93 | dialog_ex_set_right_button_text(app->dialog, "Stay"); 94 | dialog_ex_set_center_button_text(app->dialog, "Menu"); 95 | dialog_ex_set_header(app->dialog, "Close Current App?", 16, 12, AlignLeft, AlignTop); 96 | view_dispatcher_add_view( 97 | app->view_dispatcher, AirMouseViewExitConfirm, dialog_ex_get_view(app->dialog)); 98 | 99 | app->error_dialog = dialog_ex_alloc(); 100 | dialog_ex_set_header(app->error_dialog, "Failed to init IMU", 63, 0, AlignCenter, AlignTop); 101 | dialog_ex_set_text(app->error_dialog, "Please connect sensor module", 63, 30, AlignCenter, AlignTop); 102 | view_set_previous_callback(dialog_ex_get_view(app->error_dialog), air_mouse_exit); 103 | view_dispatcher_add_view( 104 | app->view_dispatcher, AirMouseViewError, dialog_ex_get_view(app->error_dialog)); 105 | 106 | // Bluetooth view 107 | app->bt_mouse = bt_mouse_alloc(app->view_dispatcher); 108 | view_set_previous_callback(bt_mouse_get_view(app->bt_mouse), air_mouse_exit_confirm_view); 109 | view_dispatcher_add_view( 110 | app->view_dispatcher, AirMouseViewBtMouse, bt_mouse_get_view(app->bt_mouse)); 111 | 112 | // USB view 113 | app->usb_mouse = usb_mouse_alloc(app->view_dispatcher); 114 | view_set_previous_callback(usb_mouse_get_view(app->usb_mouse), air_mouse_exit_confirm_view); 115 | view_dispatcher_add_view( 116 | app->view_dispatcher, AirMouseViewUsbMouse, usb_mouse_get_view(app->usb_mouse)); 117 | 118 | // Calibration view 119 | app->calibration = calibration_alloc(app->view_dispatcher); 120 | view_set_previous_callback( 121 | calibration_get_view(app->calibration), air_mouse_exit_confirm_view); 122 | view_dispatcher_add_view( 123 | app->view_dispatcher, AirMouseViewCalibration, calibration_get_view(app->calibration)); 124 | 125 | app->view_id = AirMouseViewSubmenu; 126 | view_dispatcher_switch_to_view(app->view_dispatcher, app->view_id); 127 | 128 | view_dispatcher_set_tick_event_callback(app->view_dispatcher, NULL, furi_ms_to_ticks(10)); 129 | 130 | return app; 131 | } 132 | 133 | void air_mouse_app_free(AirMouse* app) { 134 | furi_assert(app); 135 | 136 | // Free views 137 | view_dispatcher_remove_view(app->view_dispatcher, AirMouseViewSubmenu); 138 | submenu_free(app->submenu); 139 | view_dispatcher_remove_view(app->view_dispatcher, AirMouseViewExitConfirm); 140 | dialog_ex_free(app->dialog); 141 | view_dispatcher_remove_view(app->view_dispatcher, AirMouseViewError); 142 | dialog_ex_free(app->error_dialog); 143 | view_dispatcher_remove_view(app->view_dispatcher, AirMouseViewBtMouse); 144 | bt_mouse_free(app->bt_mouse); 145 | view_dispatcher_remove_view(app->view_dispatcher, AirMouseViewUsbMouse); 146 | usb_mouse_free(app->usb_mouse); 147 | view_dispatcher_remove_view(app->view_dispatcher, AirMouseViewCalibration); 148 | calibration_free(app->calibration); 149 | view_dispatcher_free(app->view_dispatcher); 150 | 151 | // Close records 152 | furi_record_close(RECORD_GUI); 153 | app->gui = NULL; 154 | 155 | // Free rest 156 | free(app); 157 | } 158 | 159 | int32_t air_mouse_app(void* p) { 160 | UNUSED(p); 161 | 162 | AirMouse* app = air_mouse_app_alloc(); 163 | if(!imu_begin()) { 164 | view_dispatcher_switch_to_view(app->view_dispatcher, AirMouseViewError); 165 | } 166 | 167 | view_dispatcher_run(app->view_dispatcher); 168 | 169 | imu_end(); 170 | air_mouse_app_free(app); 171 | 172 | return 0; 173 | } 174 | -------------------------------------------------------------------------------- /tracking/main_loop.cc: -------------------------------------------------------------------------------- 1 | #include "main_loop.h" 2 | 3 | #include 4 | #include 5 | 6 | #include "imu/imu.h" 7 | #include "orientation_tracker.h" 8 | #include "calibration_data.h" 9 | 10 | #define TAG "tracker" 11 | 12 | static const float CURSOR_SPEED = 1024.0 / (M_PI / 4); 13 | static const float STABILIZE_BIAS = 16.0; 14 | 15 | class TrackingState { 16 | private: 17 | float yaw; 18 | float pitch; 19 | float dYaw; 20 | float dPitch; 21 | bool firstRead; 22 | bool stabilize; 23 | CalibrationData calibration; 24 | cardboard::OrientationTracker tracker; 25 | uint64_t ippus, ippus2; 26 | 27 | private: 28 | float clamp(float val) { 29 | while (val <= -M_PI) { 30 | val += 2 * M_PI; 31 | } 32 | while (val >= M_PI) { 33 | val -= 2 * M_PI; 34 | } 35 | return val; 36 | } 37 | 38 | float highpass(float oldVal, float newVal) { 39 | if (!stabilize) { 40 | return newVal; 41 | } 42 | float delta = clamp(oldVal - newVal); 43 | float alpha = (float) std::max(0.0, 1 - std::pow(std::fabs(delta) * CURSOR_SPEED / STABILIZE_BIAS, 3.0)); 44 | return newVal + alpha * delta; 45 | } 46 | 47 | void sendCurrentState(MouseMoveCallback mouse_move, void *context) { 48 | float dX = dYaw * CURSOR_SPEED; 49 | float dY = dPitch * CURSOR_SPEED; 50 | 51 | // Scale the shift down to fit the protocol. 52 | if (dX > 127) { 53 | dY *= 127.0 / dX; 54 | dX = 127; 55 | } 56 | if (dX < -127) { 57 | dY *= -127.0 / dX; 58 | dX = -127; 59 | } 60 | if (dY > 127) { 61 | dX *= 127.0 / dY; 62 | dY = 127; 63 | } 64 | if (dY < -127) { 65 | dX *= -127.0 / dY; 66 | dY = -127; 67 | } 68 | 69 | const int8_t x = (int8_t)std::floor(dX + 0.5); 70 | const int8_t y = (int8_t)std::floor(dY + 0.5); 71 | 72 | mouse_move(x, y, context); 73 | 74 | // Only subtract the part of the error that was already sent. 75 | if (x != 0) { 76 | dYaw -= x / CURSOR_SPEED; 77 | } 78 | if (y != 0) { 79 | dPitch -= y / CURSOR_SPEED; 80 | } 81 | } 82 | 83 | void onOrientation(cardboard::Vector4& quaternion) { 84 | float q1 = quaternion[0]; // X * sin(T/2) 85 | float q2 = quaternion[1]; // Y * sin(T/2) 86 | float q3 = quaternion[2]; // Z * sin(T/2) 87 | float q0 = quaternion[3]; // cos(T/2) 88 | 89 | float yaw = std::atan2(2 * (q0 * q3 - q1 * q2), (1 - 2 * (q1 * q1 + q3 * q3))); 90 | float pitch = std::asin(2 * (q0 * q1 + q2 * q3)); 91 | // float roll = std::atan2(2 * (q0 * q2 - q1 * q3), (1 - 2 * (q1 * q1 + q2 * q2))); 92 | 93 | if (yaw == NAN || pitch == NAN) { 94 | // NaN case, skip it 95 | return; 96 | } 97 | 98 | if (firstRead) { 99 | this->yaw = yaw; 100 | this->pitch = pitch; 101 | firstRead = false; 102 | } else { 103 | const float newYaw = highpass(this->yaw, yaw); 104 | const float newPitch = highpass(this->pitch, pitch); 105 | 106 | float dYaw = clamp(this->yaw - newYaw); 107 | float dPitch = this->pitch - newPitch; 108 | this->yaw = newYaw; 109 | this->pitch = newPitch; 110 | 111 | // Accumulate the error locally. 112 | this->dYaw += dYaw; 113 | this->dPitch += dPitch; 114 | } 115 | } 116 | 117 | public: 118 | TrackingState() 119 | : yaw(0) 120 | , pitch(0) 121 | , dYaw(0) 122 | , dPitch(0) 123 | , firstRead(true) 124 | , stabilize(true) 125 | , tracker(10000000l) { // 10 ms / 100 Hz 126 | ippus = furi_hal_cortex_instructions_per_microsecond(); 127 | ippus2 = ippus / 2; 128 | } 129 | 130 | void beginCalibration() { 131 | calibration.reset(); 132 | } 133 | 134 | bool stepCalibration() { 135 | if (calibration.isComplete()) 136 | return true; 137 | 138 | double vec[6]; 139 | if (imu_read(vec) & GYR_DATA_READY) { 140 | cardboard::Vector3 data(vec[3], vec[4], vec[5]); 141 | furi_delay_ms(9); // Artificially limit to ~100Hz 142 | return calibration.add(data); 143 | } 144 | 145 | return false; 146 | } 147 | 148 | void saveCalibration() { 149 | CalibrationMedian store; 150 | cardboard::Vector3 median = calibration.getMedian(); 151 | store.x = median[0]; 152 | store.y = median[1]; 153 | store.z = median[2]; 154 | CALIBRATION_DATA_SAVE(&store); 155 | } 156 | 157 | void loadCalibration() { 158 | CalibrationMedian store; 159 | cardboard::Vector3 median = calibration.getMedian(); 160 | if (CALIBRATION_DATA_LOAD(&store)) { 161 | median[0] = store.x; 162 | median[1] = store.y; 163 | median[2] = store.z; 164 | } 165 | 166 | tracker.SetCalibration(median); 167 | } 168 | 169 | void beginTracking() { 170 | loadCalibration(); 171 | tracker.Resume(); 172 | } 173 | 174 | void stepTracking(MouseMoveCallback mouse_move, void *context) { 175 | double vec[6]; 176 | int ret = imu_read(vec); 177 | if (ret != 0) { 178 | uint64_t t = (DWT->CYCCNT * 1000llu + ippus2) / ippus; 179 | if (ret & ACC_DATA_READY) { 180 | cardboard::AccelerometerData adata 181 | = { .system_timestamp = t, .sensor_timestamp_ns = t, 182 | .data = cardboard::Vector3(vec[0], vec[1], vec[2]) }; 183 | tracker.OnAccelerometerData(adata); 184 | } 185 | if (ret & GYR_DATA_READY) { 186 | cardboard::GyroscopeData gdata 187 | = { .system_timestamp = t, .sensor_timestamp_ns = t, 188 | .data = cardboard::Vector3(vec[3], vec[4], vec[5]) }; 189 | cardboard::Vector4 pose = tracker.OnGyroscopeData(gdata); 190 | onOrientation(pose); 191 | sendCurrentState(mouse_move, context); 192 | } 193 | } 194 | } 195 | 196 | void stopTracking() { 197 | tracker.Pause(); 198 | } 199 | }; 200 | 201 | static TrackingState g_state; 202 | 203 | extern "C" { 204 | 205 | void calibration_begin() { 206 | g_state.beginCalibration(); 207 | FURI_LOG_I(TAG, "Calibrating"); 208 | } 209 | 210 | bool calibration_step() { 211 | return g_state.stepCalibration(); 212 | } 213 | 214 | void calibration_end() { 215 | g_state.saveCalibration(); 216 | } 217 | 218 | void tracking_begin() { 219 | g_state.beginTracking(); 220 | } 221 | 222 | void tracking_step(MouseMoveCallback mouse_move, void *context) { 223 | g_state.stepTracking(mouse_move, context); 224 | } 225 | 226 | void tracking_end() { 227 | g_state.stopTracking(); 228 | } 229 | 230 | } 231 | -------------------------------------------------------------------------------- /tracking/util/vector.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2019 Google Inc. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | #ifndef CARDBOARD_SDK_UTIL_VECTOR_H_ 17 | #define CARDBOARD_SDK_UTIL_VECTOR_H_ 18 | 19 | #include 20 | 21 | namespace cardboard { 22 | 23 | // Geometric N-dimensional Vector class. 24 | template 25 | class Vector { 26 | public: 27 | // The default constructor zero-initializes all elements. 28 | Vector(); 29 | 30 | // Dimension-specific constructors that are passed individual element values. 31 | constexpr Vector(double e0, double e1, double e2); 32 | constexpr Vector(double e0, double e1, double e2, double e3); 33 | 34 | // Constructor for a Vector of dimension N from a Vector of dimension N-1 and 35 | // a scalar of the correct type, assuming N is at least 2. 36 | // constexpr Vector(const Vector& v, double s); 37 | 38 | void Set(double e0, double e1, double e2); // Only when Dimension == 3. 39 | void Set(double e0, double e1, double e2, 40 | double e3); // Only when Dimension == 4. 41 | 42 | // Mutable element accessor. 43 | double& operator[](int index) { 44 | return elem_[index]; 45 | } 46 | 47 | // Element accessor. 48 | double operator[](int index) const { 49 | return elem_[index]; 50 | } 51 | 52 | // Returns a Vector containing all zeroes. 53 | static Vector Zero(); 54 | 55 | // Self-modifying operators. 56 | void operator+=(const Vector& v) { 57 | Add(v); 58 | } 59 | void operator-=(const Vector& v) { 60 | Subtract(v); 61 | } 62 | void operator*=(double s) { 63 | Multiply(s); 64 | } 65 | void operator/=(double s) { 66 | Divide(s); 67 | } 68 | 69 | // Unary negation operator. 70 | Vector operator-() const { 71 | return Negation(); 72 | } 73 | 74 | // Binary operators. 75 | friend Vector operator+(const Vector& v0, const Vector& v1) { 76 | return Sum(v0, v1); 77 | } 78 | friend Vector operator-(const Vector& v0, const Vector& v1) { 79 | return Difference(v0, v1); 80 | } 81 | friend Vector operator*(const Vector& v, double s) { 82 | return Scale(v, s); 83 | } 84 | friend Vector operator*(double s, const Vector& v) { 85 | return Scale(v, s); 86 | } 87 | friend Vector operator*(const Vector& v, const Vector& s) { 88 | return Product(v, s); 89 | } 90 | friend Vector operator/(const Vector& v, double s) { 91 | return Divide(v, s); 92 | } 93 | 94 | // Self-modifying addition. 95 | void Add(const Vector& v); 96 | // Self-modifying subtraction. 97 | void Subtract(const Vector& v); 98 | // Self-modifying multiplication by a scalar. 99 | void Multiply(double s); 100 | // Self-modifying division by a scalar. 101 | void Divide(double s); 102 | 103 | // Unary negation. 104 | Vector Negation() const; 105 | 106 | // Binary component-wise multiplication. 107 | static Vector Product(const Vector& v0, const Vector& v1); 108 | // Binary component-wise addition. 109 | static Vector Sum(const Vector& v0, const Vector& v1); 110 | // Binary component-wise subtraction. 111 | static Vector Difference(const Vector& v0, const Vector& v1); 112 | // Binary multiplication by a scalar. 113 | static Vector Scale(const Vector& v, double s); 114 | // Binary division by a scalar. 115 | static Vector Divide(const Vector& v, double s); 116 | 117 | private: 118 | std::array elem_; 119 | }; 120 | //------------------------------------------------------------------------------ 121 | 122 | template 123 | Vector::Vector() { 124 | for(int i = 0; i < Dimension; i++) { 125 | elem_[i] = 0; 126 | } 127 | } 128 | 129 | template 130 | constexpr Vector::Vector(double e0, double e1, double e2) 131 | : elem_{e0, e1, e2} { 132 | } 133 | 134 | template 135 | constexpr Vector::Vector(double e0, double e1, double e2, double e3) 136 | : elem_{e0, e1, e2, e3} { 137 | } 138 | /* 139 | template <> 140 | constexpr Vector<4>::Vector(const Vector<3>& v, double s) 141 | : elem_{v[0], v[1], v[2], s} {} 142 | */ 143 | template 144 | void Vector::Set(double e0, double e1, double e2) { 145 | elem_[0] = e0; 146 | elem_[1] = e1; 147 | elem_[2] = e2; 148 | } 149 | 150 | template 151 | void Vector::Set(double e0, double e1, double e2, double e3) { 152 | elem_[0] = e0; 153 | elem_[1] = e1; 154 | elem_[2] = e2; 155 | elem_[3] = e3; 156 | } 157 | 158 | template 159 | Vector Vector::Zero() { 160 | Vector v; 161 | return v; 162 | } 163 | 164 | template 165 | void Vector::Add(const Vector& v) { 166 | for(int i = 0; i < Dimension; i++) { 167 | elem_[i] += v[i]; 168 | } 169 | } 170 | 171 | template 172 | void Vector::Subtract(const Vector& v) { 173 | for(int i = 0; i < Dimension; i++) { 174 | elem_[i] -= v[i]; 175 | } 176 | } 177 | 178 | template 179 | void Vector::Multiply(double s) { 180 | for(int i = 0; i < Dimension; i++) { 181 | elem_[i] *= s; 182 | } 183 | } 184 | 185 | template 186 | void Vector::Divide(double s) { 187 | for(int i = 0; i < Dimension; i++) { 188 | elem_[i] /= s; 189 | } 190 | } 191 | 192 | template 193 | Vector Vector::Negation() const { 194 | Vector ret; 195 | for(int i = 0; i < Dimension; i++) { 196 | ret.elem_[i] = -elem_[i]; 197 | } 198 | return ret; 199 | } 200 | 201 | template 202 | Vector Vector::Product(const Vector& v0, const Vector& v1) { 203 | Vector ret; 204 | for(int i = 0; i < Dimension; i++) { 205 | ret.elem_[i] = v0[i] * v1[i]; 206 | } 207 | return ret; 208 | } 209 | 210 | template 211 | Vector Vector::Sum(const Vector& v0, const Vector& v1) { 212 | Vector ret; 213 | for(int i = 0; i < Dimension; i++) { 214 | ret.elem_[i] = v0[i] + v1[i]; 215 | } 216 | return ret; 217 | } 218 | 219 | template 220 | Vector Vector::Difference(const Vector& v0, const Vector& v1) { 221 | Vector ret; 222 | for(int i = 0; i < Dimension; i++) { 223 | ret.elem_[i] = v0[i] - v1[i]; 224 | } 225 | return ret; 226 | } 227 | 228 | template 229 | Vector Vector::Scale(const Vector& v, double s) { 230 | Vector ret; 231 | for(int i = 0; i < Dimension; i++) { 232 | ret.elem_[i] = v[i] * s; 233 | } 234 | return ret; 235 | } 236 | 237 | template 238 | Vector Vector::Divide(const Vector& v, double s) { 239 | Vector ret; 240 | for(int i = 0; i < Dimension; i++) { 241 | ret.elem_[i] = v[i] / s; 242 | } 243 | return ret; 244 | } 245 | 246 | typedef Vector<3> Vector3; 247 | typedef Vector<4> Vector4; 248 | 249 | } // namespace cardboard 250 | 251 | #endif // CARDBOARD_SDK_UTIL_VECTOR_H_ 252 | -------------------------------------------------------------------------------- /tracking/sensors/sensor_fusion_ekf.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2019 Google Inc. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | #ifndef CARDBOARD_SDK_SENSORS_SENSOR_FUSION_EKF_H_ 17 | #define CARDBOARD_SDK_SENSORS_SENSOR_FUSION_EKF_H_ 18 | 19 | #include 20 | #include 21 | #include 22 | 23 | #include "accelerometer_data.h" 24 | #include "gyroscope_bias_estimator.h" 25 | #include "gyroscope_data.h" 26 | #include "pose_state.h" 27 | #include "../util/matrix_3x3.h" 28 | #include "../util/rotation.h" 29 | #include "../util/vector.h" 30 | 31 | namespace cardboard { 32 | 33 | // Sensor fusion class that implements an Extended Kalman Filter (EKF) to 34 | // estimate a 3D rotation from a gyroscope and an accelerometer. 35 | // This system only has one state, the pose. It does not estimate any velocity 36 | // or acceleration. 37 | // 38 | // To learn more about Kalman filtering one can read this article which is a 39 | // good introduction: https://en.wikipedia.org/wiki/Kalman_filter 40 | class SensorFusionEkf { 41 | public: 42 | SensorFusionEkf(); 43 | 44 | // Resets the state of the sensor fusion. It sets the velocity for 45 | // prediction to zero. The reset will happen with the next 46 | // accelerometer sample. Gyroscope sample will be discarded until a new 47 | // accelerometer sample arrives. 48 | void Reset(); 49 | 50 | // Gets the PoseState representing the latest pose and derivatives at a 51 | // particular timestamp as estimated by SensorFusion. 52 | PoseState GetLatestPoseState() const; 53 | 54 | // Processes one gyroscope sample event. This updates the pose of the system 55 | // and the prediction model. The gyroscope data is assumed to be in axis angle 56 | // form. Angle = ||v|| and Axis = v / ||v||, with v = [v_x, v_y, v_z]^T. 57 | // 58 | // @param sample gyroscope sample data. 59 | void ProcessGyroscopeSample(const GyroscopeData& sample); 60 | 61 | // Processes one accelerometer sample event. This updates the pose of the 62 | // system. If the Accelerometer norm changes too much between sample it is not 63 | // trusted as much. 64 | // 65 | // @param sample accelerometer sample data. 66 | void ProcessAccelerometerSample(const AccelerometerData& sample); 67 | 68 | // Enables or disables the drift correction by estimating the gyroscope bias. 69 | // 70 | // @param enable Enable drift correction. 71 | void SetBiasEstimationEnabled(bool enable); 72 | 73 | // Returns a boolean that indicates if bias estimation is enabled or disabled. 74 | // 75 | // @return true if bias estimation is enabled, false otherwise. 76 | bool IsBiasEstimationEnabled() const; 77 | 78 | // Returns the current gyroscope bias estimate from GyroscopeBiasEstimator. 79 | Vector3 GetGyroscopeBias() const { 80 | return { 81 | gyroscope_bias_estimate_[0], gyroscope_bias_estimate_[1], gyroscope_bias_estimate_[2]}; 82 | } 83 | 84 | // Returns true after receiving the first accelerometer measurement. 85 | bool IsFullyInitialized() const { 86 | return is_aligned_with_gravity_; 87 | } 88 | 89 | private: 90 | // Estimates the average timestep between gyroscope event. 91 | void FilterGyroscopeTimestep(double gyroscope_timestep); 92 | 93 | // Updates the state covariance with an incremental motion. It changes the 94 | // space of the quadric. 95 | void UpdateStateCovariance(const Matrix3x3& motion_update); 96 | 97 | // Computes the innovation vector of the Kalman based on the input pose. 98 | // It uses the latest measurement vector (i.e. accelerometer data), which must 99 | // be set prior to calling this function. 100 | Vector3 ComputeInnovation(const Rotation& pose); 101 | 102 | // This computes the measurement_jacobian_ via numerical differentiation based 103 | // on the current value of sensor_from_start_rotation_. 104 | void ComputeMeasurementJacobian(); 105 | 106 | // Updates the accelerometer covariance matrix. 107 | // 108 | // This looks at the norm of recent accelerometer readings. If it has changed 109 | // significantly, it means the phone receives additional acceleration than 110 | // just gravity, and so the down vector information gravity signal is noisier. 111 | void UpdateMeasurementCovariance(); 112 | 113 | // Reset all internal states. This is not thread safe. Lock should be acquired 114 | // outside of it. This function is called in ProcessAccelerometerSample. 115 | void ResetState(); 116 | 117 | // Current transformation from Sensor Space to Start Space. 118 | // x_sensor = sensor_from_start_rotation_ * x_start; 119 | PoseState current_state_; 120 | 121 | // Filtering of the gyroscope timestep started? 122 | bool is_timestep_filter_initialized_; 123 | // Filtered gyroscope timestep valid? 124 | bool is_gyroscope_filter_valid_; 125 | // Sensor fusion currently aligned with gravity? After initialization 126 | // it will requires a couple of accelerometer data for the system to get 127 | // aligned. 128 | std::atomic is_aligned_with_gravity_; 129 | 130 | // Covariance of Kalman filter state (P in common formulation). 131 | Matrix3x3 state_covariance_; 132 | // Covariance of the process noise (Q in common formulation). 133 | Matrix3x3 process_covariance_; 134 | // Covariance of the accelerometer measurement (R in common formulation). 135 | Matrix3x3 accelerometer_measurement_covariance_; 136 | // Covariance of innovation (S in common formulation). 137 | Matrix3x3 innovation_covariance_; 138 | // Jacobian of the measurements (H in common formulation). 139 | Matrix3x3 accelerometer_measurement_jacobian_; 140 | // Gain of the Kalman filter (K in common formulation). 141 | Matrix3x3 kalman_gain_; 142 | // Parameter update a.k.a. innovation vector. (\nu in common formulation). 143 | Vector3 innovation_; 144 | // Measurement vector (z in common formulation). 145 | Vector3 accelerometer_measurement_; 146 | // Current prediction vector (g in common formulation). 147 | Vector3 prediction_; 148 | // Control input, currently this is only the gyroscope data (\mu in common 149 | // formulation). 150 | Vector3 control_input_; 151 | // Update of the state vector. (x in common formulation). 152 | Vector3 state_update_; 153 | 154 | // Sensor time of the last gyroscope processed event. 155 | uint64_t current_gyroscope_sensor_timestamp_ns_; 156 | // Sensor time of the last accelerometer processed event. 157 | uint64_t current_accelerometer_sensor_timestamp_ns_; 158 | 159 | // Estimates of the timestep between gyroscope event in seconds. 160 | double filtered_gyroscope_timestep_s_; 161 | // Number of timestep samples processed so far by the filter. 162 | uint32_t num_gyroscope_timestep_samples_; 163 | // Norm of the accelerometer for the previous measurement. 164 | double previous_accelerometer_norm_; 165 | // Moving average of the accelerometer norm changes. It is computed for every 166 | // sensor datum. 167 | double moving_average_accelerometer_norm_change_; 168 | 169 | // Flag indicating if a state reset should be executed with the next 170 | // accelerometer sample. 171 | std::atomic execute_reset_with_next_accelerometer_sample_; 172 | 173 | // Flag indicating if bias estimation is enabled (enabled by default). 174 | std::atomic bias_estimation_enabled_; 175 | 176 | // Bias estimator and static device detector. 177 | GyroscopeBiasEstimator gyroscope_bias_estimator_; 178 | 179 | // Current bias estimate_; 180 | Vector3 gyroscope_bias_estimate_; 181 | 182 | SensorFusionEkf(const SensorFusionEkf&) = delete; 183 | SensorFusionEkf& operator=(const SensorFusionEkf&) = delete; 184 | }; 185 | 186 | } // namespace cardboard 187 | 188 | #endif // CARDBOARD_SDK_SENSORS_SENSOR_FUSION_EKF_H_ 189 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | APPENDIX: How to apply the Apache License to your work. 179 | 180 | To apply the Apache License to your work, attach the following 181 | boilerplate notice, with the fields enclosed by brackets "[]" 182 | replaced with your own identifying information. (Don't include 183 | the brackets!) The text should be enclosed in the appropriate 184 | comment syntax for the file format. We also recommend that a 185 | file or class name and description of purpose be included on the 186 | same "printed page" as the copyright notice for easier 187 | identification within third-party archives. 188 | 189 | Copyright [yyyy] [name of copyright owner] 190 | 191 | Licensed under the Apache License, Version 2.0 (the "License"); 192 | you may not use this file except in compliance with the License. 193 | You may obtain a copy of the License at 194 | 195 | http://www.apache.org/licenses/LICENSE-2.0 196 | 197 | Unless required by applicable law or agreed to in writing, software 198 | distributed under the License is distributed on an "AS IS" BASIS, 199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 200 | See the License for the specific language governing permissions and 201 | limitations under the License. 202 | -------------------------------------------------------------------------------- /views/bt_mouse.c: -------------------------------------------------------------------------------- 1 | #include "bt_mouse.h" 2 | #include "../tracking/main_loop.h" 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | typedef struct ButtonEvent { 16 | int8_t button; 17 | bool state; 18 | } ButtonEvent; 19 | 20 | #define BTN_EVT_QUEUE_SIZE 32 21 | 22 | struct BtMouse { 23 | FuriHalBleProfileBase* hid; 24 | View* view; 25 | ViewDispatcher* view_dispatcher; 26 | Bt* bt; 27 | NotificationApp* notifications; 28 | FuriMutex* mutex; 29 | FuriThread* thread; 30 | bool connected; 31 | 32 | // Current mouse state 33 | uint8_t btn; 34 | int dx; 35 | int dy; 36 | int wheel; 37 | 38 | // Circular buffer; 39 | // (qhead == qtail) means either empty or overflow. 40 | // We'll ignore overflow and treat it as empty. 41 | int qhead; 42 | int qtail; 43 | ButtonEvent queue[BTN_EVT_QUEUE_SIZE]; 44 | }; 45 | 46 | static const BleProfileHidParams ble_hid_params = { 47 | .device_name_prefix = "AirMouse", 48 | .mac_xor = 0x0001, 49 | }; 50 | 51 | #define BT_MOUSE_FLAG_INPUT_EVENT (1UL << 0) 52 | #define BT_MOUSE_FLAG_KILL_THREAD (1UL << 1) 53 | #define BT_MOUSE_FLAG_ALL (BT_MOUSE_FLAG_INPUT_EVENT | BT_MOUSE_FLAG_KILL_THREAD) 54 | 55 | #define MOUSE_SCROLL 2 56 | 57 | #define HID_BT_KEYS_STORAGE_NAME ".bt_hid.keys" 58 | 59 | static void bt_mouse_notify_event(BtMouse* bt_mouse) { 60 | FuriThreadId thread_id = furi_thread_get_id(bt_mouse->thread); 61 | furi_assert(thread_id); 62 | furi_thread_flags_set(thread_id, BT_MOUSE_FLAG_INPUT_EVENT); 63 | } 64 | 65 | static void bt_mouse_draw_callback(Canvas* canvas, void* context) { 66 | UNUSED(context); 67 | canvas_clear(canvas); 68 | canvas_set_font(canvas, FontPrimary); 69 | canvas_draw_str(canvas, 0, 10, "Bluetooth Mouse mode"); 70 | canvas_set_font(canvas, FontSecondary); 71 | canvas_draw_str(canvas, 0, 63, "Hold [back] to exit"); 72 | } 73 | 74 | static void bt_mouse_button_state(BtMouse* bt_mouse, int8_t button, bool state) { 75 | ButtonEvent event; 76 | event.button = button; 77 | event.state = state; 78 | 79 | if(bt_mouse->connected) { 80 | furi_mutex_acquire(bt_mouse->mutex, FuriWaitForever); 81 | bt_mouse->queue[bt_mouse->qtail++] = event; 82 | bt_mouse->qtail %= BTN_EVT_QUEUE_SIZE; 83 | furi_mutex_release(bt_mouse->mutex); 84 | bt_mouse_notify_event(bt_mouse); 85 | } 86 | } 87 | 88 | static void bt_mouse_process(BtMouse* bt_mouse, InputEvent* event) { 89 | with_view_model( 90 | bt_mouse->view, 91 | void* model, 92 | { 93 | UNUSED(model); 94 | if(event->key == InputKeyUp) { 95 | if(event->type == InputTypePress) { 96 | bt_mouse_button_state(bt_mouse, HID_MOUSE_BTN_LEFT, true); 97 | } else if(event->type == InputTypeRelease) { 98 | bt_mouse_button_state(bt_mouse, HID_MOUSE_BTN_LEFT, false); 99 | } 100 | } else if(event->key == InputKeyDown) { 101 | if(event->type == InputTypePress) { 102 | bt_mouse_button_state(bt_mouse, HID_MOUSE_BTN_RIGHT, true); 103 | } else if(event->type == InputTypeRelease) { 104 | bt_mouse_button_state(bt_mouse, HID_MOUSE_BTN_RIGHT, false); 105 | } 106 | } else if(event->key == InputKeyOk) { 107 | if(event->type == InputTypePress) { 108 | bt_mouse_button_state(bt_mouse, HID_MOUSE_BTN_WHEEL, true); 109 | } else if(event->type == InputTypeRelease) { 110 | bt_mouse_button_state(bt_mouse, HID_MOUSE_BTN_WHEEL, false); 111 | } 112 | } else if(event->key == InputKeyRight) { 113 | if(event->type == InputTypePress || event->type == InputTypeRepeat) { 114 | bt_mouse->wheel = MOUSE_SCROLL; 115 | } 116 | } else if(event->key == InputKeyLeft) { 117 | if(event->type == InputTypePress || event->type == InputTypeRepeat) { 118 | bt_mouse->wheel = -MOUSE_SCROLL; 119 | } 120 | } 121 | }, 122 | true); 123 | } 124 | 125 | static bool bt_mouse_input_callback(InputEvent* event, void* context) { 126 | furi_assert(context); 127 | BtMouse* bt_mouse = context; 128 | bool consumed = false; 129 | 130 | if(event->type == InputTypeLong && event->key == InputKeyBack) { 131 | ble_profile_hid_mouse_release_all(bt_mouse->hid); 132 | } else { 133 | bt_mouse_process(bt_mouse, event); 134 | consumed = true; 135 | } 136 | 137 | return consumed; 138 | } 139 | 140 | void bt_mouse_connection_status_changed_callback(BtStatus status, void* context) { 141 | furi_assert(context); 142 | BtMouse* bt_mouse = context; 143 | 144 | bt_mouse->connected = (status == BtStatusConnected); 145 | if(!bt_mouse->notifications) { 146 | tracking_end(); 147 | return; 148 | } 149 | 150 | if(bt_mouse->connected) { 151 | notification_internal_message(bt_mouse->notifications, &sequence_set_blue_255); 152 | tracking_begin(); 153 | } else { 154 | tracking_end(); 155 | notification_internal_message(bt_mouse->notifications, &sequence_reset_blue); 156 | } 157 | } 158 | 159 | bool bt_mouse_move(int8_t dx, int8_t dy, void* context) { 160 | furi_assert(context); 161 | BtMouse* bt_mouse = context; 162 | 163 | if(bt_mouse->connected) { 164 | furi_mutex_acquire(bt_mouse->mutex, FuriWaitForever); 165 | bt_mouse->dx += dx; 166 | bt_mouse->dy += dy; 167 | furi_mutex_release(bt_mouse->mutex); 168 | bt_mouse_notify_event(bt_mouse); 169 | } 170 | 171 | return true; 172 | } 173 | 174 | static int8_t clamp(int t) { 175 | if(t < -128) { 176 | return -128; 177 | } else if(t > 127) { 178 | return 127; 179 | } 180 | return t; 181 | } 182 | 183 | static int32_t bt_mouse_thread_callback(void* context) { 184 | furi_assert(context); 185 | BtMouse* bt_mouse = (BtMouse*)context; 186 | 187 | while(1) { 188 | uint32_t flags = 189 | furi_thread_flags_wait(BT_MOUSE_FLAG_ALL, FuriFlagWaitAny, FuriWaitForever); 190 | if(flags & BT_MOUSE_FLAG_KILL_THREAD) { 191 | break; 192 | } 193 | if(flags & BT_MOUSE_FLAG_INPUT_EVENT) { 194 | furi_mutex_acquire(bt_mouse->mutex, FuriWaitForever); 195 | 196 | ButtonEvent event; 197 | bool send_buttons = false; 198 | if(bt_mouse->qhead != bt_mouse->qtail) { 199 | event = bt_mouse->queue[bt_mouse->qhead++]; 200 | bt_mouse->qhead %= BTN_EVT_QUEUE_SIZE; 201 | send_buttons = true; 202 | } 203 | 204 | int8_t dx = clamp(bt_mouse->dx); 205 | bt_mouse->dx -= dx; 206 | int8_t dy = clamp(bt_mouse->dy); 207 | bt_mouse->dy -= dy; 208 | int8_t wheel = clamp(bt_mouse->wheel); 209 | bt_mouse->wheel -= wheel; 210 | 211 | furi_mutex_release(bt_mouse->mutex); 212 | 213 | if(bt_mouse->connected && send_buttons) { 214 | if(event.state) { 215 | ble_profile_hid_mouse_press(bt_mouse->hid, event.button); 216 | } else { 217 | ble_profile_hid_mouse_release(bt_mouse->hid, event.button); 218 | } 219 | } 220 | 221 | if(bt_mouse->connected && (dx != 0 || dy != 0)) { 222 | ble_profile_hid_mouse_move(bt_mouse->hid, dx, dy); 223 | } 224 | 225 | if(bt_mouse->connected && wheel != 0) { 226 | ble_profile_hid_mouse_scroll(bt_mouse->hid, wheel); 227 | } 228 | } 229 | } 230 | 231 | return 0; 232 | } 233 | 234 | void bt_mouse_thread_start(BtMouse* bt_mouse) { 235 | furi_assert(bt_mouse); 236 | bt_mouse->mutex = furi_mutex_alloc(FuriMutexTypeNormal); 237 | bt_mouse->thread = furi_thread_alloc(); 238 | furi_thread_set_name(bt_mouse->thread, "BtSender"); 239 | furi_thread_set_stack_size(bt_mouse->thread, 1024); 240 | furi_thread_set_context(bt_mouse->thread, bt_mouse); 241 | furi_thread_set_callback(bt_mouse->thread, bt_mouse_thread_callback); 242 | furi_thread_start(bt_mouse->thread); 243 | } 244 | 245 | void bt_mouse_thread_stop(BtMouse* bt_mouse) { 246 | furi_assert(bt_mouse); 247 | FuriThreadId thread_id = furi_thread_get_id(bt_mouse->thread); 248 | furi_assert(thread_id); 249 | furi_thread_flags_set(thread_id, BT_MOUSE_FLAG_KILL_THREAD); 250 | furi_thread_join(bt_mouse->thread); 251 | furi_thread_free(bt_mouse->thread); 252 | furi_mutex_free(bt_mouse->mutex); 253 | bt_mouse->mutex = NULL; 254 | bt_mouse->thread = NULL; 255 | } 256 | 257 | void bt_mouse_tick_event_callback(void* context) { 258 | furi_assert(context); 259 | tracking_step(bt_mouse_move, context); 260 | } 261 | 262 | void bt_mouse_enter_callback(void* context) { 263 | furi_assert(context); 264 | BtMouse* bt_mouse = context; 265 | 266 | bt_mouse->bt = furi_record_open(RECORD_BT); 267 | bt_disconnect(bt_mouse->bt); 268 | 269 | furi_delay_ms(200); 270 | bt_keys_storage_set_storage_path(bt_mouse->bt, APP_DATA_PATH(HID_BT_KEYS_STORAGE_NAME)); 271 | 272 | bt_mouse->notifications = furi_record_open(RECORD_NOTIFICATION); 273 | bt_set_status_changed_callback( 274 | bt_mouse->bt, bt_mouse_connection_status_changed_callback, bt_mouse); 275 | bt_mouse->hid = bt_profile_start(bt_mouse->bt, ble_profile_hid, (void*)&ble_hid_params); 276 | furi_assert(bt_mouse->hid); 277 | furi_hal_bt_start_advertising(); 278 | bt_mouse_thread_start(bt_mouse); 279 | 280 | view_dispatcher_set_event_callback_context(bt_mouse->view_dispatcher, bt_mouse); 281 | view_dispatcher_set_tick_event_callback(bt_mouse->view_dispatcher, bt_mouse_tick_event_callback, furi_ms_to_ticks(10)); 282 | } 283 | 284 | void bt_mouse_remove_pairing(void) { 285 | Bt* bt = furi_record_open(RECORD_BT); 286 | bt_disconnect(bt); 287 | 288 | furi_delay_ms(200); 289 | furi_hal_bt_stop_advertising(); 290 | 291 | bt_keys_storage_set_storage_path(bt, APP_DATA_PATH(HID_BT_KEYS_STORAGE_NAME)); 292 | bt_forget_bonded_devices(bt); 293 | 294 | furi_delay_ms(200); 295 | bt_keys_storage_set_default_path(bt); 296 | 297 | furi_check(bt_profile_restore_default(bt)); 298 | furi_record_close(RECORD_BT); 299 | } 300 | 301 | void bt_mouse_exit_callback(void* context) { 302 | furi_assert(context); 303 | BtMouse* bt_mouse = context; 304 | 305 | view_dispatcher_set_tick_event_callback(bt_mouse->view_dispatcher, NULL, FuriWaitForever); 306 | 307 | bt_mouse_thread_stop(bt_mouse); 308 | tracking_end(); 309 | notification_internal_message(bt_mouse->notifications, &sequence_reset_blue); 310 | 311 | bt_set_status_changed_callback(bt_mouse->bt, NULL, NULL); 312 | bt_disconnect(bt_mouse->bt); 313 | 314 | furi_delay_ms(200); 315 | bt_keys_storage_set_default_path(bt_mouse->bt); 316 | 317 | furi_hal_bt_stop_advertising(); 318 | bt_profile_restore_default(bt_mouse->bt); 319 | 320 | furi_record_close(RECORD_NOTIFICATION); 321 | bt_mouse->notifications = NULL; 322 | furi_record_close(RECORD_BT); 323 | bt_mouse->bt = NULL; 324 | } 325 | 326 | BtMouse* bt_mouse_alloc(ViewDispatcher* view_dispatcher) { 327 | BtMouse* bt_mouse = malloc(sizeof(BtMouse)); 328 | memset(bt_mouse, 0, sizeof(BtMouse)); 329 | 330 | bt_mouse->view = view_alloc(); 331 | bt_mouse->view_dispatcher = view_dispatcher; 332 | view_set_context(bt_mouse->view, bt_mouse); 333 | view_set_draw_callback(bt_mouse->view, bt_mouse_draw_callback); 334 | view_set_input_callback(bt_mouse->view, bt_mouse_input_callback); 335 | view_set_enter_callback(bt_mouse->view, bt_mouse_enter_callback); 336 | view_set_exit_callback(bt_mouse->view, bt_mouse_exit_callback); 337 | return bt_mouse; 338 | } 339 | 340 | void bt_mouse_free(BtMouse* bt_mouse) { 341 | furi_assert(bt_mouse); 342 | view_free(bt_mouse->view); 343 | free(bt_mouse); 344 | } 345 | 346 | View* bt_mouse_get_view(BtMouse* bt_mouse) { 347 | furi_assert(bt_mouse); 348 | return bt_mouse->view; 349 | } 350 | -------------------------------------------------------------------------------- /tracking/sensors/gyroscope_bias_estimator.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2019 Google Inc. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | #include "gyroscope_bias_estimator.h" 17 | 18 | #include 19 | #include // NOLINT 20 | 21 | #include "../util/rotation.h" 22 | #include "../util/vector.h" 23 | 24 | namespace { 25 | 26 | // Cutoff frequencies in Hertz applied to our various signals, and their 27 | // corresponding filters. 28 | const float kAccelerometerLowPassCutOffFrequencyHz = 1.0f; 29 | const float kRotationVelocityBasedAccelerometerLowPassCutOffFrequencyHz = 0.15f; 30 | const float kGyroscopeLowPassCutOffFrequencyHz = 1.0f; 31 | const float kGyroscopeBiasLowPassCutOffFrequencyHz = 0.15f; 32 | 33 | // Note that MEMS IMU are not that precise. 34 | const double kEpsilon = 1.0e-8; 35 | 36 | // Size of the filtering window for the mean and median filter. The larger the 37 | // windows the larger the filter delay. 38 | const int kFilterWindowSize = 5; 39 | 40 | // Threshold used to compare rotation computed from the accelerometer and the 41 | // gyroscope bias. 42 | const double kRatioBetweenGyroBiasAndAccel = 1.5; 43 | 44 | // The minimum sum of weights we need to acquire before returning a bias 45 | // estimation. 46 | const float kMinSumOfWeightsGyroBiasThreshold = 25.0f; 47 | 48 | // Amount of change in m/s^3 we allow on the smoothed accelerometer values to 49 | // consider the phone static. 50 | const double kAccelerometerDeltaStaticThreshold = 0.5; 51 | 52 | // Amount of change in radians/s^2 we allow on the smoothed gyroscope values to 53 | // consider the phone static. 54 | const double kGyroscopeDeltaStaticThreshold = 0.03; 55 | 56 | // If the gyroscope value is above this threshold, don't update the gyroscope 57 | // bias estimation. This threshold is applied to the magnitude of gyroscope 58 | // vectors in radians/s. 59 | const float kGyroscopeForBiasThreshold = 0.30f; 60 | 61 | // Used to monitor if accelerometer and gyroscope have been static for a few 62 | // frames. 63 | const int kStaticFrameDetectionThreshold = 50; 64 | 65 | // Minimum time step between sensor updates. 66 | const double kMinTimestep = 1; // std::chrono::nanoseconds(1); 67 | } // namespace 68 | 69 | namespace cardboard { 70 | 71 | // A helper class to keep track of whether some signal can be considered static 72 | // over specified number of frames. 73 | class GyroscopeBiasEstimator::IsStaticCounter { 74 | public: 75 | // Initializes a counter with the number of consecutive frames we require 76 | // the signal to be static before IsRecentlyStatic returns true. 77 | // 78 | // @param min_static_frames_threshold number of consecutive frames we 79 | // require the signal to be static before IsRecentlyStatic returns true. 80 | explicit IsStaticCounter(int min_static_frames_threshold) 81 | : min_static_frames_threshold_(min_static_frames_threshold) 82 | , consecutive_static_frames_(0) 83 | { 84 | } 85 | 86 | // Specifies whether the current frame is considered static. 87 | // 88 | // @param is_static static flag for current frame. 89 | void AppendFrame(bool is_static) 90 | { 91 | if (is_static) { 92 | ++consecutive_static_frames_; 93 | } else { 94 | consecutive_static_frames_ = 0; 95 | } 96 | } 97 | 98 | // Returns if static movement is assumed. 99 | bool IsRecentlyStatic() const 100 | { 101 | return consecutive_static_frames_ >= min_static_frames_threshold_; 102 | } 103 | // Resets counter. 104 | void Reset() { consecutive_static_frames_ = 0; } 105 | 106 | private: 107 | const int min_static_frames_threshold_; 108 | int consecutive_static_frames_; 109 | }; 110 | 111 | GyroscopeBiasEstimator::GyroscopeBiasEstimator() 112 | : accelerometer_lowpass_filter_(kAccelerometerLowPassCutOffFrequencyHz) 113 | , simulated_gyroscope_from_accelerometer_lowpass_filter_( 114 | kRotationVelocityBasedAccelerometerLowPassCutOffFrequencyHz) 115 | , gyroscope_lowpass_filter_(kGyroscopeLowPassCutOffFrequencyHz) 116 | , gyroscope_bias_lowpass_filter_(kGyroscopeBiasLowPassCutOffFrequencyHz) 117 | , accelerometer_static_counter_(new IsStaticCounter(kStaticFrameDetectionThreshold)) 118 | , gyroscope_static_counter_(new IsStaticCounter(kStaticFrameDetectionThreshold)) 119 | , current_accumulated_weights_gyroscope_bias_(0.f) 120 | , mean_filter_(kFilterWindowSize) 121 | , median_filter_(kFilterWindowSize) 122 | , last_mean_filtered_accelerometer_value_({ 0, 0, 0 }) 123 | { 124 | Reset(); 125 | } 126 | 127 | GyroscopeBiasEstimator::~GyroscopeBiasEstimator() { } 128 | 129 | void GyroscopeBiasEstimator::Reset() 130 | { 131 | accelerometer_lowpass_filter_.Reset(); 132 | gyroscope_lowpass_filter_.Reset(); 133 | gyroscope_bias_lowpass_filter_.Reset(); 134 | accelerometer_static_counter_->Reset(); 135 | gyroscope_static_counter_->Reset(); 136 | } 137 | 138 | void GyroscopeBiasEstimator::ProcessGyroscope( 139 | const Vector3& gyroscope_sample, uint64_t timestamp_ns) 140 | { 141 | // Update gyroscope and gyroscope delta low-pass filters. 142 | gyroscope_lowpass_filter_.AddSample(gyroscope_sample, timestamp_ns); 143 | 144 | const auto smoothed_gyroscope_delta 145 | = gyroscope_sample - gyroscope_lowpass_filter_.GetFilteredData(); 146 | 147 | gyroscope_static_counter_->AppendFrame( 148 | Length(smoothed_gyroscope_delta) < kGyroscopeDeltaStaticThreshold); 149 | 150 | // Only update the bias if the gyroscope and accelerometer signals have been 151 | // relatively static recently. 152 | if (gyroscope_static_counter_->IsRecentlyStatic() 153 | && accelerometer_static_counter_->IsRecentlyStatic()) { 154 | // Reset static counter when updating the bias fails. 155 | if (!UpdateGyroscopeBias(gyroscope_sample, timestamp_ns)) { 156 | // Bias update fails because of large motion, thus reset the static 157 | // counter. 158 | gyroscope_static_counter_->AppendFrame(false); 159 | } 160 | } else { 161 | // Reset weights, if not static. 162 | current_accumulated_weights_gyroscope_bias_ = 0; 163 | } 164 | } 165 | 166 | void GyroscopeBiasEstimator::ProcessAccelerometer( 167 | const Vector3& accelerometer_sample, uint64_t timestamp_ns) 168 | { 169 | // Get current state of the filter. 170 | const uint64_t previous_accel_timestamp_ns 171 | = accelerometer_lowpass_filter_.GetMostRecentTimestampNs(); 172 | const bool is_low_pass_filter_init = accelerometer_lowpass_filter_.IsInitialized(); 173 | 174 | // Update accel and accel delta low-pass filters. 175 | accelerometer_lowpass_filter_.AddSample(accelerometer_sample, timestamp_ns); 176 | 177 | const auto smoothed_accelerometer_delta 178 | = accelerometer_sample - accelerometer_lowpass_filter_.GetFilteredData(); 179 | 180 | accelerometer_static_counter_->AppendFrame( 181 | Length(smoothed_accelerometer_delta) < kAccelerometerDeltaStaticThreshold); 182 | 183 | // Rotation from accel cannot be differentiated with only one sample. 184 | if (!is_low_pass_filter_init) { 185 | simulated_gyroscope_from_accelerometer_lowpass_filter_.AddSample({ 0, 0, 0 }, timestamp_ns); 186 | return; 187 | } 188 | 189 | // No need to update the simulated gyroscope at this point because the motion 190 | // is too large. 191 | if (!accelerometer_static_counter_->IsRecentlyStatic()) { 192 | return; 193 | } 194 | 195 | median_filter_.AddSample(accelerometer_lowpass_filter_.GetFilteredData()); 196 | 197 | // This processing can only be started if the buffer is fully initialized. 198 | if (!median_filter_.IsValid()) { 199 | mean_filter_.AddSample(accelerometer_lowpass_filter_.GetFilteredData()); 200 | 201 | // Update the last filtered accelerometer value. 202 | last_mean_filtered_accelerometer_value_ = accelerometer_lowpass_filter_.GetFilteredData(); 203 | return; 204 | } 205 | 206 | mean_filter_.AddSample(median_filter_.GetFilteredData()); 207 | 208 | // Compute a mock gyroscope value from accelerometer. 209 | const int64_t diff = timestamp_ns - previous_accel_timestamp_ns; 210 | const double timestep = static_cast(diff); 211 | 212 | simulated_gyroscope_from_accelerometer_lowpass_filter_.AddSample( 213 | ComputeAngularVelocityFromLatestAccelerometer(timestep), timestamp_ns); 214 | last_mean_filtered_accelerometer_value_ = mean_filter_.GetFilteredData(); 215 | } 216 | 217 | Vector3 GyroscopeBiasEstimator::ComputeAngularVelocityFromLatestAccelerometer(double timestep) const 218 | { 219 | if (timestep < kMinTimestep) { 220 | return { 0, 0, 0 }; 221 | } 222 | 223 | const auto mean_of_median = mean_filter_.GetFilteredData(); 224 | 225 | // Compute an incremental rotation between the last state and the current 226 | // state. 227 | // 228 | // Note that we switch to double precision here because of precision problem 229 | // with small rotation. 230 | const auto incremental_rotation = Rotation::RotateInto( 231 | Vector3(last_mean_filtered_accelerometer_value_[0], 232 | last_mean_filtered_accelerometer_value_[1], last_mean_filtered_accelerometer_value_[2]), 233 | Vector3(mean_of_median[0], mean_of_median[1], mean_of_median[2])); 234 | 235 | // We use axis angle here because this is how gyroscope values are stored. 236 | Vector3 incremental_rotation_axis; 237 | double incremental_rotation_angle; 238 | incremental_rotation.GetAxisAndAngle(&incremental_rotation_axis, &incremental_rotation_angle); 239 | 240 | incremental_rotation_axis *= incremental_rotation_angle / timestep; 241 | 242 | return { static_cast(incremental_rotation_axis[0]), 243 | static_cast(incremental_rotation_axis[1]), 244 | static_cast(incremental_rotation_axis[2]) }; 245 | } 246 | 247 | bool GyroscopeBiasEstimator::UpdateGyroscopeBias( 248 | const Vector3& gyroscope_sample, uint64_t timestamp_ns) 249 | { 250 | // Gyroscope values that are too big are potentially dangerous as they could 251 | // originate from slow and steady head rotations. 252 | // 253 | // Therefore we compute an update weight which: 254 | // * favors gyroscope values that are closer to 0 255 | // * is set to zero if gyroscope values are greater than a threshold. 256 | // 257 | // This way, the gyroscope bias estimation converges faster if the phone is 258 | // flat on a table, as opposed to held up somewhat stationary in the user's 259 | // hands. 260 | 261 | // If magnitude is too big, don't update the filter at all so that we don't 262 | // artificially increase the number of samples accumulated by the filter. 263 | const float gyroscope_sample_norm2 = Length(gyroscope_sample); 264 | if (gyroscope_sample_norm2 >= kGyroscopeForBiasThreshold) { 265 | return false; 266 | } 267 | 268 | float update_weight 269 | = std::max(0.0f, 1 - gyroscope_sample_norm2 / kGyroscopeForBiasThreshold); 270 | update_weight *= update_weight; 271 | gyroscope_bias_lowpass_filter_.AddWeightedSample( 272 | gyroscope_lowpass_filter_.GetFilteredData(), timestamp_ns, update_weight); 273 | 274 | // This counter is only partially valid as the low pass filter drops large 275 | // samples. 276 | current_accumulated_weights_gyroscope_bias_ += update_weight; 277 | 278 | return true; 279 | } 280 | 281 | Vector3 GyroscopeBiasEstimator::GetGyroscopeBias() const 282 | { 283 | return gyroscope_bias_lowpass_filter_.GetFilteredData(); 284 | } 285 | 286 | bool GyroscopeBiasEstimator::IsCurrentEstimateValid() const 287 | { 288 | // Remove any bias component along the gravity because they cannot be 289 | // evaluated from accelerometer. 290 | const auto current_gravity_dir = Normalized(last_mean_filtered_accelerometer_value_); 291 | const auto gyro_bias_lowpass = gyroscope_bias_lowpass_filter_.GetFilteredData(); 292 | 293 | const auto off_gravity_gyro_bias 294 | = gyro_bias_lowpass - current_gravity_dir * Dot(gyro_bias_lowpass, current_gravity_dir); 295 | 296 | // Checks that the current bias estimate is not correlated with the 297 | // rotation computed from accelerometer. 298 | const auto gyro_from_accel 299 | = simulated_gyroscope_from_accelerometer_lowpass_filter_.GetFilteredData(); 300 | const bool isGyroscopeBiasCorrelatedWithSimulatedGyro 301 | = (Length(gyro_from_accel) * kRatioBetweenGyroBiasAndAccel 302 | > (Length(off_gravity_gyro_bias) + kEpsilon)); 303 | const bool hasEnoughSamples 304 | = current_accumulated_weights_gyroscope_bias_ > kMinSumOfWeightsGyroBiasThreshold; 305 | const bool areCountersStatic = gyroscope_static_counter_->IsRecentlyStatic() 306 | && accelerometer_static_counter_->IsRecentlyStatic(); 307 | 308 | const bool isStatic 309 | = hasEnoughSamples && areCountersStatic && !isGyroscopeBiasCorrelatedWithSimulatedGyro; 310 | return isStatic; 311 | } 312 | 313 | } // namespace cardboard 314 | -------------------------------------------------------------------------------- /tracking/sensors/sensor_fusion_ekf.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2019 Google Inc. All Rights Reserved. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | #include "sensor_fusion_ekf.h" 17 | 18 | #include 19 | #include 20 | 21 | #include "accelerometer_data.h" 22 | #include "gyroscope_data.h" 23 | #include "pose_prediction.h" 24 | #include "../util/matrixutils.h" 25 | 26 | namespace cardboard { 27 | 28 | namespace { 29 | 30 | const double kFiniteDifferencingEpsilon = 1.0e-7; 31 | const double kEpsilon = 1.0e-15; 32 | // Default gyroscope frequency. This corresponds to 100 Hz. 33 | const double kDefaultGyroscopeTimestep_s = 0.01f; 34 | // Maximum time between gyroscope before we start limiting the integration. 35 | const double kMaximumGyroscopeSampleDelay_s = 0.04f; 36 | // Compute a first-order exponential moving average of changes in accel norm per 37 | // frame. 38 | const double kSmoothingFactor = 0.5; 39 | // Minimum and maximum values used for accelerometer noise covariance matrix. 40 | // The smaller the sigma value, the more weight is given to the accelerometer 41 | // signal. 42 | const double kMinAccelNoiseSigma = 0.75; 43 | const double kMaxAccelNoiseSigma = 7.0; 44 | // Initial value for the diagonal elements of the different covariance matrices. 45 | const double kInitialStateCovarianceValue = 25.0; 46 | const double kInitialProcessCovarianceValue = 1.0; 47 | // Maximum accelerometer norm change allowed before capping it covariance to a 48 | // large value. 49 | const double kMaxAccelNormChange = 0.15; 50 | // Timestep IIR filtering coefficient. 51 | const double kTimestepFilterCoeff = 0.95; 52 | // Minimum number of sample for timestep filtering. 53 | const int kTimestepFilterMinSamples = 10; 54 | 55 | // Z direction in start space. 56 | const Vector3 kCanonicalZDirection(0.0, 0.0, 1.0); 57 | 58 | // Computes an axis-angle rotation from the input vector. 59 | // angle = norm(a) 60 | // axis = a.normalized() 61 | // If norm(a) == 0, it returns an identity rotation. 62 | static inline void RotationFromVector(const Vector3& a, Rotation& r) 63 | { 64 | const double norm_a = Length(a); 65 | if (norm_a < kEpsilon) { 66 | r = Rotation::Identity(); 67 | return; 68 | } 69 | r = Rotation::FromAxisAndAngle(a / norm_a, norm_a); 70 | } 71 | 72 | } // namespace 73 | 74 | SensorFusionEkf::SensorFusionEkf() 75 | : execute_reset_with_next_accelerometer_sample_(false) 76 | , bias_estimation_enabled_(true) 77 | , gyroscope_bias_estimate_({ 0, 0, 0 }) 78 | { 79 | ResetState(); 80 | } 81 | 82 | void SensorFusionEkf::Reset() { execute_reset_with_next_accelerometer_sample_ = true; } 83 | 84 | void SensorFusionEkf::ResetState() 85 | { 86 | current_state_.sensor_from_start_rotation = Rotation::Identity(); 87 | current_state_.sensor_from_start_rotation_velocity = Vector3::Zero(); 88 | 89 | current_gyroscope_sensor_timestamp_ns_ = 0; 90 | current_accelerometer_sensor_timestamp_ns_ = 0; 91 | 92 | state_covariance_ = Matrix3x3::Identity() * kInitialStateCovarianceValue; 93 | process_covariance_ = Matrix3x3::Identity() * kInitialProcessCovarianceValue; 94 | accelerometer_measurement_covariance_ 95 | = Matrix3x3::Identity() * kMinAccelNoiseSigma * kMinAccelNoiseSigma; 96 | innovation_covariance_ = Matrix3x3::Identity(); 97 | 98 | accelerometer_measurement_jacobian_ = Matrix3x3::Zero(); 99 | kalman_gain_ = Matrix3x3::Zero(); 100 | innovation_ = Vector3::Zero(); 101 | accelerometer_measurement_ = Vector3::Zero(); 102 | prediction_ = Vector3::Zero(); 103 | control_input_ = Vector3::Zero(); 104 | state_update_ = Vector3::Zero(); 105 | 106 | moving_average_accelerometer_norm_change_ = 0.0; 107 | 108 | is_timestep_filter_initialized_ = false; 109 | is_gyroscope_filter_valid_ = false; 110 | is_aligned_with_gravity_ = false; 111 | 112 | // Reset biases. 113 | gyroscope_bias_estimator_.Reset(); 114 | gyroscope_bias_estimate_ = { 0, 0, 0 }; 115 | } 116 | 117 | // Here I am doing something wrong relative to time stamps. The state timestamps 118 | // always correspond to the gyrostamps because it would require additional 119 | // extrapolation if I wanted to do otherwise. 120 | PoseState SensorFusionEkf::GetLatestPoseState() const { return current_state_; } 121 | 122 | void SensorFusionEkf::ProcessGyroscopeSample(const GyroscopeData& sample) 123 | { 124 | // Don't accept gyroscope sample when waiting for a reset. 125 | if (execute_reset_with_next_accelerometer_sample_) { 126 | return; 127 | } 128 | 129 | // Discard outdated samples. 130 | if (current_gyroscope_sensor_timestamp_ns_ >= sample.sensor_timestamp_ns) { 131 | current_gyroscope_sensor_timestamp_ns_ = sample.sensor_timestamp_ns; 132 | return; 133 | } 134 | 135 | // Checks that we received at least one gyroscope sample in the past. 136 | if (current_gyroscope_sensor_timestamp_ns_ != 0) { 137 | double current_timestep_s = std::chrono::duration_cast>( 138 | std::chrono::nanoseconds( 139 | sample.sensor_timestamp_ns - current_gyroscope_sensor_timestamp_ns_)) 140 | .count(); 141 | if (current_timestep_s > kMaximumGyroscopeSampleDelay_s) { 142 | if (is_gyroscope_filter_valid_) { 143 | // Replaces the delta timestamp by the filtered estimates of the delta time. 144 | current_timestep_s = filtered_gyroscope_timestep_s_; 145 | } else { 146 | current_timestep_s = kDefaultGyroscopeTimestep_s; 147 | } 148 | } else { 149 | FilterGyroscopeTimestep(current_timestep_s); 150 | } 151 | 152 | if (bias_estimation_enabled_) { 153 | gyroscope_bias_estimator_.ProcessGyroscope(sample.data, sample.sensor_timestamp_ns); 154 | 155 | if (gyroscope_bias_estimator_.IsCurrentEstimateValid()) { 156 | // As soon as the device is considered to be static, the bias estimator 157 | // should have a precise estimate of the gyroscope bias. 158 | gyroscope_bias_estimate_ = gyroscope_bias_estimator_.GetGyroscopeBias(); 159 | } 160 | } 161 | 162 | // Only integrate after receiving an accelerometer sample. 163 | if (is_aligned_with_gravity_) { 164 | const Rotation rotation_from_gyroscope = pose_prediction::GetRotationFromGyroscope( 165 | { sample.data[0] - gyroscope_bias_estimate_[0], 166 | sample.data[1] - gyroscope_bias_estimate_[1], 167 | sample.data[2] - gyroscope_bias_estimate_[2] }, 168 | current_timestep_s); 169 | current_state_.sensor_from_start_rotation 170 | = rotation_from_gyroscope * current_state_.sensor_from_start_rotation; 171 | UpdateStateCovariance(RotationMatrixNH(rotation_from_gyroscope)); 172 | state_covariance_ = state_covariance_ 173 | + ((current_timestep_s * current_timestep_s) * process_covariance_); 174 | } 175 | } 176 | 177 | // Saves gyroscope event for future prediction. 178 | current_state_.timestamp = sample.system_timestamp; 179 | current_gyroscope_sensor_timestamp_ns_ = sample.sensor_timestamp_ns; 180 | current_state_.sensor_from_start_rotation_velocity.Set( 181 | sample.data[0] - gyroscope_bias_estimate_[0], sample.data[1] - gyroscope_bias_estimate_[1], 182 | sample.data[2] - gyroscope_bias_estimate_[2]); 183 | } 184 | 185 | Vector3 SensorFusionEkf::ComputeInnovation(const Rotation& pose) 186 | { 187 | const Vector3 predicted_down_direction = pose * kCanonicalZDirection; 188 | 189 | const Rotation rotation 190 | = Rotation::RotateInto(predicted_down_direction, accelerometer_measurement_); 191 | Vector3 axis; 192 | double angle; 193 | rotation.GetAxisAndAngle(&axis, &angle); 194 | return axis * angle; 195 | } 196 | 197 | void SensorFusionEkf::ComputeMeasurementJacobian() 198 | { 199 | for (int dof = 0; dof < 3; dof++) { 200 | Vector3 delta = Vector3::Zero(); 201 | delta[dof] = kFiniteDifferencingEpsilon; 202 | 203 | Rotation epsilon_rotation; 204 | RotationFromVector(delta, epsilon_rotation); 205 | const Vector3 delta_rotation 206 | = ComputeInnovation(epsilon_rotation * current_state_.sensor_from_start_rotation); 207 | 208 | const Vector3 col = (innovation_ - delta_rotation) / kFiniteDifferencingEpsilon; 209 | accelerometer_measurement_jacobian_(0, dof) = col[0]; 210 | accelerometer_measurement_jacobian_(1, dof) = col[1]; 211 | accelerometer_measurement_jacobian_(2, dof) = col[2]; 212 | } 213 | } 214 | 215 | void SensorFusionEkf::ProcessAccelerometerSample(const AccelerometerData& sample) 216 | { 217 | // Discard outdated samples. 218 | if (current_accelerometer_sensor_timestamp_ns_ >= sample.sensor_timestamp_ns) { 219 | current_accelerometer_sensor_timestamp_ns_ = sample.sensor_timestamp_ns; 220 | return; 221 | } 222 | 223 | // Call reset state if required. 224 | if (execute_reset_with_next_accelerometer_sample_.exchange(false)) { 225 | ResetState(); 226 | } 227 | 228 | accelerometer_measurement_.Set(sample.data[0], sample.data[1], sample.data[2]); 229 | current_accelerometer_sensor_timestamp_ns_ = sample.sensor_timestamp_ns; 230 | 231 | if (bias_estimation_enabled_) { 232 | gyroscope_bias_estimator_.ProcessAccelerometer(sample.data, sample.sensor_timestamp_ns); 233 | } 234 | 235 | if (!is_aligned_with_gravity_) { 236 | // This is the first accelerometer measurement so it initializes the 237 | // orientation estimate. 238 | current_state_.sensor_from_start_rotation 239 | = Rotation::RotateInto(kCanonicalZDirection, accelerometer_measurement_); 240 | is_aligned_with_gravity_ = true; 241 | 242 | previous_accelerometer_norm_ = Length(accelerometer_measurement_); 243 | return; 244 | } 245 | 246 | UpdateMeasurementCovariance(); 247 | 248 | innovation_ = ComputeInnovation(current_state_.sensor_from_start_rotation); 249 | ComputeMeasurementJacobian(); 250 | 251 | // S = H * P * H' + R 252 | innovation_covariance_ = accelerometer_measurement_jacobian_ * state_covariance_ 253 | * Transpose(accelerometer_measurement_jacobian_) 254 | + accelerometer_measurement_covariance_; 255 | 256 | // K = P * H' * S^-1 257 | kalman_gain_ = state_covariance_ * Transpose(accelerometer_measurement_jacobian_) 258 | * Inverse(innovation_covariance_); 259 | 260 | // x_update = K*nu 261 | state_update_ = kalman_gain_ * innovation_; 262 | 263 | // P = (I - K * H) * P; 264 | state_covariance_ = (Matrix3x3::Identity() - kalman_gain_ * accelerometer_measurement_jacobian_) 265 | * state_covariance_; 266 | 267 | // Updates pose and associate covariance matrix. 268 | Rotation rotation_from_state_update; 269 | RotationFromVector(state_update_, rotation_from_state_update); 270 | 271 | current_state_.sensor_from_start_rotation 272 | = rotation_from_state_update * current_state_.sensor_from_start_rotation; 273 | UpdateStateCovariance(RotationMatrixNH(rotation_from_state_update)); 274 | } 275 | 276 | void SensorFusionEkf::UpdateStateCovariance(const Matrix3x3& motion_update) 277 | { 278 | state_covariance_ = motion_update * state_covariance_ * Transpose(motion_update); 279 | } 280 | 281 | void SensorFusionEkf::FilterGyroscopeTimestep(double gyroscope_timestep_s) 282 | { 283 | if (!is_timestep_filter_initialized_) { 284 | // Initializes the filter. 285 | filtered_gyroscope_timestep_s_ = gyroscope_timestep_s; 286 | num_gyroscope_timestep_samples_ = 1; 287 | is_timestep_filter_initialized_ = true; 288 | return; 289 | } 290 | 291 | // Computes the IIR filter response. 292 | filtered_gyroscope_timestep_s_ = kTimestepFilterCoeff * filtered_gyroscope_timestep_s_ 293 | + (1 - kTimestepFilterCoeff) * gyroscope_timestep_s; 294 | ++num_gyroscope_timestep_samples_; 295 | 296 | if (num_gyroscope_timestep_samples_ > kTimestepFilterMinSamples) { 297 | is_gyroscope_filter_valid_ = true; 298 | } 299 | } 300 | 301 | void SensorFusionEkf::UpdateMeasurementCovariance() 302 | { 303 | const double current_accelerometer_norm = Length(accelerometer_measurement_); 304 | // Norm change between current and previous accel readings. 305 | const double current_accelerometer_norm_change 306 | = std::abs(current_accelerometer_norm - previous_accelerometer_norm_); 307 | previous_accelerometer_norm_ = current_accelerometer_norm; 308 | 309 | moving_average_accelerometer_norm_change_ = kSmoothingFactor * current_accelerometer_norm_change 310 | + (1 - kSmoothingFactor) * moving_average_accelerometer_norm_change_; 311 | 312 | // If we hit the accel norm change threshold, we use the maximum noise sigma 313 | // for the accel covariance. For anything below that, we use a linear 314 | // combination between min and max sigma values. 315 | const double norm_change_ratio 316 | = moving_average_accelerometer_norm_change_ / kMaxAccelNormChange; 317 | const double accelerometer_noise_sigma = std::min(kMaxAccelNoiseSigma, 318 | kMinAccelNoiseSigma + norm_change_ratio * (kMaxAccelNoiseSigma - kMinAccelNoiseSigma)); 319 | 320 | // Updates the accel covariance matrix with the new sigma value. 321 | accelerometer_measurement_covariance_ 322 | = Matrix3x3::Identity() * accelerometer_noise_sigma * accelerometer_noise_sigma; 323 | } 324 | 325 | bool SensorFusionEkf::IsBiasEstimationEnabled() const { return bias_estimation_enabled_; } 326 | 327 | void SensorFusionEkf::SetBiasEstimationEnabled(bool enable) 328 | { 329 | if (bias_estimation_enabled_ != enable) { 330 | bias_estimation_enabled_ = enable; 331 | gyroscope_bias_estimate_ = { 0, 0, 0 }; 332 | gyroscope_bias_estimator_.Reset(); 333 | } 334 | } 335 | 336 | } // namespace cardboard 337 | --------------------------------------------------------------------------------