More updates (thanks grnch!)

This commit is contained in:
UberGuidoZ 2022-12-28 22:30:12 -08:00
parent 74236940d7
commit 034d4cd172
1153 changed files with 103961 additions and 0 deletions

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

Binary file not shown.

After

Width:  |  Height:  |  Size: 1.6 KiB

View File

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

View File

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

File diff suppressed because it is too large Load Diff

View File

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

File diff suppressed because it is too large Load Diff

View File

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

View File

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

View File

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

View File

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

File diff suppressed because it is too large Load Diff

File diff suppressed because it is too large Load Diff

View File

@ -0,0 +1,189 @@
#include "main_loop.h"
#include <furi.h>
#include <furi_hal.h>
#include "imu/imu.h"
#include "orientation_tracker.h"
#include "calibration_data.h"
#define TAG "tracker"
static const float CURSOR_SPEED = 1024.0 / (M_PI / 4);
static const float STABILIZE_BIAS = 16.0;
float g_yaw = 0;
float g_pitch = 0;
float g_dYaw = 0;
float g_dPitch = 0;
bool firstRead = true;
bool stabilize = true;
CalibrationData calibration;
cardboard::OrientationTracker tracker(10000000l); // 10 ms / 100 Hz
uint64_t ippms, ippms2;
static inline float clamp(float val)
{
while (val <= -M_PI) {
val += 2 * M_PI;
}
while (val >= M_PI) {
val -= 2 * M_PI;
}
return val;
}
static inline float highpass(float oldVal, float newVal)
{
if (!stabilize) {
return newVal;
}
float delta = clamp(oldVal - newVal);
float alpha = (float) std::max(0.0, 1 - std::pow(std::fabs(delta) * CURSOR_SPEED / STABILIZE_BIAS, 3.0));
return newVal + alpha * delta;
}
void sendCurrentState(MouseMoveCallback mouse_move, void *context)
{
float dX = g_dYaw * CURSOR_SPEED;
float dY = g_dPitch * CURSOR_SPEED;
// Scale the shift down to fit the protocol.
if (dX > 127) {
dY *= 127.0 / dX;
dX = 127;
}
if (dX < -127) {
dY *= -127.0 / dX;
dX = -127;
}
if (dY > 127) {
dX *= 127.0 / dY;
dY = 127;
}
if (dY < -127) {
dX *= -127.0 / dY;
dY = -127;
}
const int8_t x = (int8_t)std::floor(dX + 0.5);
const int8_t y = (int8_t)std::floor(dY + 0.5);
mouse_move(x, y, context);
// Only subtract the part of the error that was already sent.
if (x != 0) {
g_dYaw -= x / CURSOR_SPEED;
}
if (y != 0) {
g_dPitch -= y / CURSOR_SPEED;
}
}
void onOrientation(cardboard::Vector4& quaternion)
{
float q1 = quaternion[0]; // X * sin(T/2)
float q2 = quaternion[1]; // Y * sin(T/2)
float q3 = quaternion[2]; // Z * sin(T/2)
float q0 = quaternion[3]; // cos(T/2)
float yaw = std::atan2(2 * (q0 * q3 - q1 * q2), (1 - 2 * (q1 * q1 + q3 * q3)));
float pitch = std::asin(2 * (q0 * q1 + q2 * q3));
// float roll = std::atan2(2 * (q0 * q2 - q1 * q3), (1 - 2 * (q1 * q1 + q2 * q2)));
if (yaw == NAN || pitch == NAN) {
// NaN case, skip it
return;
}
if (firstRead) {
g_yaw = yaw;
g_pitch = pitch;
firstRead = false;
} else {
const float newYaw = highpass(g_yaw, yaw);
const float newPitch = highpass(g_pitch, pitch);
float dYaw = clamp(g_yaw - newYaw);
float dPitch = g_pitch - newPitch;
g_yaw = newYaw;
g_pitch = newPitch;
// Accumulate the error locally.
g_dYaw += dYaw;
g_dPitch += dPitch;
}
}
extern "C" {
void calibration_begin() {
calibration.reset();
FURI_LOG_I(TAG, "Calibrating");
}
bool calibration_step() {
if (calibration.isComplete())
return true;
double vec[6];
if (imu_read(vec) & GYR_DATA_READY) {
cardboard::Vector3 data(vec[3], vec[4], vec[5]);
furi_delay_ms(9); // Artificially limit to ~100Hz
return calibration.add(data);
}
return false;
}
void calibration_end() {
CalibrationMedian store;
cardboard::Vector3 median = calibration.getMedian();
store.x = median[0];
store.y = median[1];
store.z = median[2];
CALIBRATION_DATA_SAVE(&store);
}
void tracking_begin() {
CalibrationMedian store;
cardboard::Vector3 median = calibration.getMedian();
if (CALIBRATION_DATA_LOAD(&store)) {
median[0] = store.x;
median[1] = store.y;
median[2] = store.z;
}
ippms = furi_hal_cortex_instructions_per_microsecond();
ippms2 = ippms / 2;
tracker.SetCalibration(median);
tracker.Resume();
}
void tracking_step(MouseMoveCallback mouse_move, void *context) {
double vec[6];
int ret = imu_read(vec);
if (ret != 0) {
uint64_t t = (DWT->CYCCNT * 1000llu + ippms2) / ippms;
if (ret & ACC_DATA_READY) {
cardboard::AccelerometerData adata
= { .system_timestamp = t, .sensor_timestamp_ns = t,
.data = cardboard::Vector3(vec[0], vec[1], vec[2]) };
tracker.OnAccelerometerData(adata);
}
if (ret & GYR_DATA_READY) {
cardboard::GyroscopeData gdata
= { .system_timestamp = t, .sensor_timestamp_ns = t,
.data = cardboard::Vector3(vec[3], vec[4], vec[5]) };
cardboard::Vector4 pose = tracker.OnGyroscopeData(gdata);
onOrientation(pose);
sendCurrentState(mouse_move, context);
}
}
}
void tracking_end() {
tracker.Pause();
}
}

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

Binary file not shown.

After

Width:  |  Height:  |  Size: 1.6 KiB

View File

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

View File

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

View File

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

View File

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

Binary file not shown.

After

Width:  |  Height:  |  Size: 1.7 KiB

View File

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

View File

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

Some files were not shown because too many files have changed in this diff Show More