notBrad/flipper-zero-tutorials
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

JS: vgm_sensor tutorial

Browse Source
This commit is contained in:
Derek Jamison 2024-04-12 15:00:38 -05:00
parent 6b1cc1013e
commit c70c4fb65e
12 changed files with 1290 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
js/vgm_sensor/README.md Normal file
View File

@ -0,0 +1,32 @@
# vgm_sensor tutorial
This is an example of extending JavaScript for the Flipper Zero. The files were used in the [YouTube video](https://youtu.be/EtOZN3Rh47c) which demonstrates the process in detail.
Our [a_demo.js](./a_demo.js) script will play a tone using the speaker module. It will then display sensor information from the video game module.
The [tone_rotate.js](./tone_rotate.js) script will play a tone using the speaker module. As you rotate the Flipper (change the yaw reading) the frequency of the tone will increase or decrese.
Copy the [a_demo.js](./a_demo.js) and [tone_rotate.js](./tone_rotate.js) files to your `SD Card/apps/Scripts` directory. You can then run the scripts from the Flipper Zero (`Apps`, `Scripts`, `a_demo.js`).
## Installing the JS speaker_api module
We wrote the speaker_api module in JavaScript (using furi_hal_xxx APIs to expose a new speaker module). This file [speaker_api.js](./speaker_api.js) should be copied to your `SD Card/apps/Scripts` directory. Once copied, your scripts can do a `let speaker = load(__dirpath + "/speaker_api.js");` and then `speaker.playTone(440, 1.0, 500);` to play a 440Hz tone, at full volume for 500ms. This file is a good example of how to use FFI to extend the Flipper Zero. If your APIs are part of the firmware, and don't use enumeration values, this is a good technqiue to use. (If they use enumeration values, it's possible that the values will change in the future, and your script will reference the improper id values.)
## Installing the C (js_vgm) module
To expose the sensor APIs to JavaScript we used a second technique of creating a module in C and compiling it into the firmware. This is a more robust technique, but requires more work for everyone that want to use the module. If you are using enumeration values, they get resolved at compile time so they will always be correct. You are able to include additional files in your C module, so you can access APIs that aren't part of the firmware (for example, accessing the sensors in the video game module). The downside is that you have to write C code, and everyone that wants to run your script needs to have their firmware compiled with your module.
To add the native C module to your firmware:
- Follow the steps in [Clone & Deploy firmware](https://github.com/jamisonderek/flipper-zero-tutorials/wiki/Install-Firmware-and-Apps#clone--deploy-firmware) instructions.
- Copy the [js_vgm](./js_vgm) folder to your firmware's `applications/system/js_app/modules/js_vgm` directory on your computer.
- Update your application.fam for js_app, adding an entry in your firmware's `applications/system/js_app/application.fam` file to include the js_vgm module at the bottom of the file. It should look like the following:
```c
App(
appid="js_vgm",
apptype=FlipperAppType.PLUGIN,
entry_point="js_vgm_ep",
requires=["js_app"],
sources=["modules/js_vgm/*.c", "modules/js_vgm/ICM42688P/*.c"],
)
```
- Recompile your firmware and deploy it to your Flipper Zero. (e.g. Run step 7 again in the *Clone & Deploy firmware* instructions.)
NOTE: If you create a useful module, you should try to make a pull-request to the firmware, so that other people can use your module without having to recompile the firmware.

22
js/vgm_sensor/a_demo.js Normal file
View File

@ -0,0 +1,22 @@
// Add new feature to JavaScript
// Speaker API! (created using FFI)
let __dirpath = "/ext/apps/Scripts";
let speaker = load(__dirpath + "/speaker_api.js");
// Play a sound at 440 Hz, volume 100% for 250 ms
speaker.play(440, 1.0, 250);
// Add new feature to JavaScript
// Video Game Module Sensor data!
// (created using C Module)
let vgm = require("vgm");
// Wait for 1 second so sensor data can be read
delay(1000);
print("Pitch is", vgm.getPitch());
print("Roll is", vgm.getRoll());
print("Yaw is", vgm.getYaw());
// Wait for up to 5 second for the yaw to change
// at least 30 degrees. Return the change in yaw
// in degrees.
print("deltaYaw is", vgm.deltaYaw(30.0, 5000));

56
js/vgm_sensor/application.fam Normal file
View File

@ -0,0 +1,56 @@
App(
appid="js_app",
name="JS Runner",
apptype=FlipperAppType.SYSTEM,
entry_point="js_app",
stack_size=2 * 1024,
resources="examples",
order=0,
)
App(
appid="js_app_start",
apptype=FlipperAppType.STARTUP,
entry_point="js_app_on_system_start",
order=160,
)
App(
appid="js_dialog",
apptype=FlipperAppType.PLUGIN,
entry_point="js_dialog_ep",
requires=["js_app"],
sources=["modules/js_dialog.c"],
)
App(
appid="js_notification",
apptype=FlipperAppType.PLUGIN,
entry_point="js_notification_ep",
requires=["js_app"],
sources=["modules/js_notification.c"],
)
App(
appid="js_badusb",
apptype=FlipperAppType.PLUGIN,
entry_point="js_badusb_ep",
requires=["js_app"],
sources=["modules/js_badusb.c"],
)
App(
appid="js_serial",
apptype=FlipperAppType.PLUGIN,
entry_point="js_serial_ep",
requires=["js_app"],
sources=["modules/js_serial.c"],
)
App(
appid="js_vgm",
apptype=FlipperAppType.PLUGIN,
entry_point="js_vgm_ep",
requires=["js_app"],
sources=["modules/js_vgm/*.c", "modules/js_vgm/ICM42688P/*.c"],
)

3
js/vgm_sensor/js_vgm/FILES.txt Normal file
View File

@ -0,0 +1,3 @@
The files imu.c, imu.h, and ICM42688P are from https://github.com/flipperdevices/flipperzero-game-engine.git
Please see that file for the license.

297
js/vgm_sensor/js_vgm/ICM42688P/ICM42688P.c Normal file
View File

@ -0,0 +1,297 @@
#include "ICM42688P_regs.h"
#include "ICM42688P.h"
#define TAG "ICM42688P"
#define ICM42688P_TIMEOUT 100
struct ICM42688P {
FuriHalSpiBusHandle* spi_bus;
const GpioPin* irq_pin;
float accel_scale;
float gyro_scale;
};
static const struct AccelFullScale {
float value;
uint8_t reg_mask;
} accel_fs_modes[] = {
[AccelFullScale16G] = {16.f, ICM42688_AFS_16G},
[AccelFullScale8G] = {8.f, ICM42688_AFS_8G},
[AccelFullScale4G] = {4.f, ICM42688_AFS_4G},
[AccelFullScale2G] = {2.f, ICM42688_AFS_2G},
};
static const struct GyroFullScale {
float value;
uint8_t reg_mask;
} gyro_fs_modes[] = {
[GyroFullScale2000DPS] = {2000.f, ICM42688_GFS_2000DPS},
[GyroFullScale1000DPS] = {1000.f, ICM42688_GFS_1000DPS},
[GyroFullScale500DPS] = {500.f, ICM42688_GFS_500DPS},
[GyroFullScale250DPS] = {250.f, ICM42688_GFS_250DPS},
[GyroFullScale125DPS] = {125.f, ICM42688_GFS_125DPS},
[GyroFullScale62_5DPS] = {62.5f, ICM42688_GFS_62_5DPS},
[GyroFullScale31_25DPS] = {31.25f, ICM42688_GFS_31_25DPS},
[GyroFullScale15_625DPS] = {15.625f, ICM42688_GFS_15_625DPS},
};
static bool icm42688p_write_reg(FuriHalSpiBusHandle* spi_bus, uint8_t addr, uint8_t value) {
bool res = false;
furi_hal_spi_acquire(spi_bus);
do {
uint8_t cmd_data[2] = {addr & 0x7F, value};
if(!furi_hal_spi_bus_tx(spi_bus, cmd_data, 2, ICM42688P_TIMEOUT)) break;
res = true;
} while(0);
furi_hal_spi_release(spi_bus);
return res;
}
static bool icm42688p_read_reg(FuriHalSpiBusHandle* spi_bus, uint8_t addr, uint8_t* value) {
bool res = false;
furi_hal_spi_acquire(spi_bus);
do {
uint8_t cmd_byte = addr | (1 << 7);
if(!furi_hal_spi_bus_tx(spi_bus, &cmd_byte, 1, ICM42688P_TIMEOUT)) break;
if(!furi_hal_spi_bus_rx(spi_bus, value, 1, ICM42688P_TIMEOUT)) break;
res = true;
} while(0);
furi_hal_spi_release(spi_bus);
return res;
}
static bool
icm42688p_read_mem(FuriHalSpiBusHandle* spi_bus, uint8_t addr, uint8_t* data, uint8_t len) {
bool res = false;
furi_hal_spi_acquire(spi_bus);
do {
uint8_t cmd_byte = addr | (1 << 7);
if(!furi_hal_spi_bus_tx(spi_bus, &cmd_byte, 1, ICM42688P_TIMEOUT)) break;
if(!furi_hal_spi_bus_rx(spi_bus, data, len, ICM42688P_TIMEOUT)) break;
res = true;
} while(0);
furi_hal_spi_release(spi_bus);
return res;
}
bool icm42688p_accel_config(
ICM42688P* icm42688p,
ICM42688PAccelFullScale full_scale,
ICM42688PDataRate rate) {
icm42688p->accel_scale = accel_fs_modes[full_scale].value;
uint8_t reg_value = accel_fs_modes[full_scale].reg_mask | rate;
return icm42688p_write_reg(icm42688p->spi_bus, ICM42688_ACCEL_CONFIG0, reg_value);
}
float icm42688p_accel_get_full_scale(ICM42688P* icm42688p) {
return icm42688p->accel_scale;
}
bool icm42688p_gyro_config(
ICM42688P* icm42688p,
ICM42688PGyroFullScale full_scale,
ICM42688PDataRate rate) {
icm42688p->gyro_scale = gyro_fs_modes[full_scale].value;
uint8_t reg_value = gyro_fs_modes[full_scale].reg_mask | rate;
return icm42688p_write_reg(icm42688p->spi_bus, ICM42688_GYRO_CONFIG0, reg_value);
}
float icm42688p_gyro_get_full_scale(ICM42688P* icm42688p) {
return icm42688p->gyro_scale;
}
bool icm42688p_read_accel_raw(ICM42688P* icm42688p, ICM42688PRawData* data) {
bool ret = icm42688p_read_mem(
icm42688p->spi_bus, ICM42688_ACCEL_DATA_X1, (uint8_t*)data, sizeof(ICM42688PRawData));
return ret;
}
bool icm42688p_read_gyro_raw(ICM42688P* icm42688p, ICM42688PRawData* data) {
bool ret = icm42688p_read_mem(
icm42688p->spi_bus, ICM42688_GYRO_DATA_X1, (uint8_t*)data, sizeof(ICM42688PRawData));
return ret;
}
bool icm42688p_write_gyro_offset(ICM42688P* icm42688p, ICM42688PScaledData* scaled_data) {
if((fabsf(scaled_data->x) > 64.f) || (fabsf(scaled_data->y) > 64.f) ||
(fabsf(scaled_data->z) > 64.f)) {
return false;
}
uint16_t offset_x = (uint16_t)(-(int16_t)(scaled_data->x * 32.f) * 16) >> 4;
uint16_t offset_y = (uint16_t)(-(int16_t)(scaled_data->y * 32.f) * 16) >> 4;
uint16_t offset_z = (uint16_t)(-(int16_t)(scaled_data->z * 32.f) * 16) >> 4;
uint8_t offset_regs[9];
icm42688p_write_reg(icm42688p->spi_bus, ICM42688_REG_BANK_SEL, 4);
icm42688p_read_mem(icm42688p->spi_bus, ICM42688_OFFSET_USER0, offset_regs, 5);
offset_regs[0] = offset_x & 0xFF;
offset_regs[1] = (offset_x & 0xF00) >> 8;
offset_regs[1] |= (offset_y & 0xF00) >> 4;
offset_regs[2] = offset_y & 0xFF;
offset_regs[3] = offset_z & 0xFF;
offset_regs[4] &= 0xF0;
offset_regs[4] |= (offset_z & 0x0F00) >> 8;
icm42688p_write_reg(icm42688p->spi_bus, ICM42688_OFFSET_USER0, offset_regs[0]);
icm42688p_write_reg(icm42688p->spi_bus, ICM42688_OFFSET_USER1, offset_regs[1]);
icm42688p_write_reg(icm42688p->spi_bus, ICM42688_OFFSET_USER2, offset_regs[2]);
icm42688p_write_reg(icm42688p->spi_bus, ICM42688_OFFSET_USER3, offset_regs[3]);
icm42688p_write_reg(icm42688p->spi_bus, ICM42688_OFFSET_USER4, offset_regs[4]);
icm42688p_write_reg(icm42688p->spi_bus, ICM42688_REG_BANK_SEL, 0);
return true;
}
void icm42688p_apply_scale(ICM42688PRawData* raw_data, float full_scale, ICM42688PScaledData* data) {
data->x = ((float)(raw_data->x)) / 32768.f * full_scale;
data->y = ((float)(raw_data->y)) / 32768.f * full_scale;
data->z = ((float)(raw_data->z)) / 32768.f * full_scale;
}
void icm42688p_apply_scale_fifo(
ICM42688P* icm42688p,
ICM42688PFifoPacket* fifo_data,
ICM42688PScaledData* accel_data,
ICM42688PScaledData* gyro_data) {
float full_scale = icm42688p->accel_scale;
accel_data->x = ((float)(fifo_data->a_x)) / 32768.f * full_scale;
accel_data->y = ((float)(fifo_data->a_y)) / 32768.f * full_scale;
accel_data->z = ((float)(fifo_data->a_z)) / 32768.f * full_scale;
full_scale = icm42688p->gyro_scale;
gyro_data->x = ((float)(fifo_data->g_x)) / 32768.f * full_scale;
gyro_data->y = ((float)(fifo_data->g_y)) / 32768.f * full_scale;
gyro_data->z = ((float)(fifo_data->g_z)) / 32768.f * full_scale;
}
float icm42688p_read_temp(ICM42688P* icm42688p) {
uint8_t reg_val[2];
icm42688p_read_mem(icm42688p->spi_bus, ICM42688_TEMP_DATA1, reg_val, 2);
int16_t temp_int = (reg_val[0] << 8) | reg_val[1];
return ((float)temp_int / 132.48f) + 25.f;
}
void icm42688_fifo_enable(
ICM42688P* icm42688p,
ICM42688PIrqCallback irq_callback,
void* irq_context) {
// FIFO mode: stream
icm42688p_write_reg(icm42688p->spi_bus, ICM42688_FIFO_CONFIG, (1 << 6));
// Little-endian data, FIFO count in records
icm42688p_write_reg(icm42688p->spi_bus, ICM42688_INTF_CONFIG0, (1 << 7) | (1 << 6));
// FIFO partial read, FIFO packet: gyro + accel TODO: 20bit
icm42688p_write_reg(
icm42688p->spi_bus, ICM42688_FIFO_CONFIG1, (1 << 6) | (1 << 5) | (1 << 1) | (1 << 0));
// FIFO irq watermark
uint16_t fifo_watermark = 1;
icm42688p_write_reg(icm42688p->spi_bus, ICM42688_FIFO_CONFIG2, fifo_watermark & 0xFF);
icm42688p_write_reg(icm42688p->spi_bus, ICM42688_FIFO_CONFIG3, fifo_watermark >> 8);
// IRQ1: push-pull, active high
icm42688p_write_reg(icm42688p->spi_bus, ICM42688_INT_CONFIG, (1 << 1) | (1 << 0));
// Clear IRQ on status read
icm42688p_write_reg(icm42688p->spi_bus, ICM42688_INT_CONFIG0, 0);
// IRQ pulse duration
icm42688p_write_reg(icm42688p->spi_bus, ICM42688_INT_CONFIG1, (1 << 6) | (1 << 5));
uint8_t reg_data = 0;
icm42688p_read_reg(icm42688p->spi_bus, ICM42688_INT_STATUS, &reg_data);
furi_hal_gpio_init(icm42688p->irq_pin, GpioModeInterruptRise, GpioPullDown, GpioSpeedVeryHigh);
furi_hal_gpio_remove_int_callback(icm42688p->irq_pin);
furi_hal_gpio_add_int_callback(icm42688p->irq_pin, irq_callback, irq_context);
// IRQ1 source: FIFO threshold
icm42688p_write_reg(icm42688p->spi_bus, ICM42688_INT_SOURCE0, (1 << 2));
}
void icm42688_fifo_disable(ICM42688P* icm42688p) {
furi_hal_gpio_remove_int_callback(icm42688p->irq_pin);
furi_hal_gpio_init(icm42688p->irq_pin, GpioModeAnalog, GpioPullNo, GpioSpeedLow);
icm42688p_write_reg(icm42688p->spi_bus, ICM42688_INT_SOURCE0, 0);
// FIFO mode: bypass
icm42688p_write_reg(icm42688p->spi_bus, ICM42688_FIFO_CONFIG, 0);
}
uint16_t icm42688_fifo_get_count(ICM42688P* icm42688p) {
uint16_t reg_val = 0;
icm42688p_read_mem(icm42688p->spi_bus, ICM42688_FIFO_COUNTH, (uint8_t*)&reg_val, 2);
return reg_val;
}
bool icm42688_fifo_read(ICM42688P* icm42688p, ICM42688PFifoPacket* data) {
icm42688p_read_mem(
icm42688p->spi_bus, ICM42688_FIFO_DATA, (uint8_t*)data, sizeof(ICM42688PFifoPacket));
return (data->header) & (1 << 7);
}
ICM42688P* icm42688p_alloc(FuriHalSpiBusHandle* spi_bus, const GpioPin* irq_pin) {
ICM42688P* icm42688p = malloc(sizeof(ICM42688P));
icm42688p->spi_bus = spi_bus;
icm42688p->irq_pin = irq_pin;
return icm42688p;
}
void icm42688p_free(ICM42688P* icm42688p) {
free(icm42688p);
}
bool icm42688p_init(ICM42688P* icm42688p) {
furi_hal_spi_bus_handle_init(icm42688p->spi_bus);
// Software reset
icm42688p_write_reg(icm42688p->spi_bus, ICM42688_REG_BANK_SEL, 0); // Set reg bank to 0
icm42688p_write_reg(icm42688p->spi_bus, ICM42688_DEVICE_CONFIG, 0x01); // SPI Mode 0, SW reset
furi_delay_ms(1);
uint8_t reg_value = 0;
bool read_ok = icm42688p_read_reg(icm42688p->spi_bus, ICM42688_WHO_AM_I, &reg_value);
if(!read_ok) {
FURI_LOG_E(TAG, "Chip ID read failed");
return false;
} else if(reg_value != ICM42688_WHOAMI) {
FURI_LOG_E(
TAG, "Sensor returned wrong ID 0x%02X, expected 0x%02X", reg_value, ICM42688_WHOAMI);
return false;
}
// Disable all interrupts
icm42688p_write_reg(icm42688p->spi_bus, ICM42688_INT_SOURCE0, 0);
icm42688p_write_reg(icm42688p->spi_bus, ICM42688_INT_SOURCE1, 0);
icm42688p_write_reg(icm42688p->spi_bus, ICM42688_INT_SOURCE3, 0);
icm42688p_write_reg(icm42688p->spi_bus, ICM42688_INT_SOURCE4, 0);
icm42688p_write_reg(icm42688p->spi_bus, ICM42688_REG_BANK_SEL, 4);
icm42688p_write_reg(icm42688p->spi_bus, ICM42688_INT_SOURCE6, 0);
icm42688p_write_reg(icm42688p->spi_bus, ICM42688_INT_SOURCE7, 0);
icm42688p_write_reg(icm42688p->spi_bus, ICM42688_REG_BANK_SEL, 0);
// Data format: little endian
icm42688p_write_reg(icm42688p->spi_bus, ICM42688_INTF_CONFIG0, 0);
// Enable all sensors
icm42688p_write_reg(
icm42688p->spi_bus,
ICM42688_PWR_MGMT0,
ICM42688_PWR_TEMP_ON | ICM42688_PWR_GYRO_MODE_LN | ICM42688_PWR_ACCEL_MODE_LN);
furi_delay_ms(45);
icm42688p_accel_config(icm42688p, AccelFullScale16G, DataRate1kHz);
icm42688p_gyro_config(icm42688p, GyroFullScale2000DPS, DataRate1kHz);
return true;
}
bool icm42688p_deinit(ICM42688P* icm42688p) {
// Software reset
icm42688p_write_reg(icm42688p->spi_bus, ICM42688_REG_BANK_SEL, 0); // Set reg bank to 0
icm42688p_write_reg(icm42688p->spi_bus, ICM42688_DEVICE_CONFIG, 0x01); // SPI Mode 0, SW reset
furi_hal_spi_bus_handle_deinit(icm42688p->spi_bus);
return true;
}

127
js/vgm_sensor/js_vgm/ICM42688P/ICM42688P.h Normal file
View File

@ -0,0 +1,127 @@
#pragma once
#include <furi.h>
#include <furi_hal.h>
#ifdef __cplusplus
extern "C" {
#endif
typedef enum {
DataRate32kHz = 0x01,
DataRate16kHz = 0x02,
DataRate8kHz = 0x03,
DataRate4kHz = 0x04,
DataRate2kHz = 0x05,
DataRate1kHz = 0x06,
DataRate200Hz = 0x07,
DataRate100Hz = 0x08,
DataRate50Hz = 0x09,
DataRate25Hz = 0x0A,
DataRate12_5Hz = 0x0B,
DataRate6_25Hz = 0x0C, // Accelerometer only
DataRate3_125Hz = 0x0D, // Accelerometer only
DataRate1_5625Hz = 0x0E, // Accelerometer only
DataRate500Hz = 0x0F,
} ICM42688PDataRate;
typedef enum {
AccelFullScale16G = 0,
AccelFullScale8G,
AccelFullScale4G,
AccelFullScale2G,
AccelFullScaleTotal,
} ICM42688PAccelFullScale;
typedef enum {
GyroFullScale2000DPS = 0,
GyroFullScale1000DPS,
GyroFullScale500DPS,
GyroFullScale250DPS,
GyroFullScale125DPS,
GyroFullScale62_5DPS,
GyroFullScale31_25DPS,
GyroFullScale15_625DPS,
GyroFullScaleTotal,
} ICM42688PGyroFullScale;
typedef struct {
int16_t x;
int16_t y;
int16_t z;
} __attribute__((packed)) ICM42688PRawData;
typedef struct {
uint8_t header;
int16_t a_x;
int16_t a_y;
int16_t a_z;
int16_t g_x;
int16_t g_y;
int16_t g_z;
uint8_t temp;
uint16_t ts;
} __attribute__((packed)) ICM42688PFifoPacket;
typedef struct {
float x;
float y;
float z;
} ICM42688PScaledData;
typedef struct ICM42688P ICM42688P;
typedef void (*ICM42688PIrqCallback)(void* ctx);
ICM42688P* icm42688p_alloc(FuriHalSpiBusHandle* spi_bus, const GpioPin* irq_pin);
bool icm42688p_init(ICM42688P* icm42688p);
bool icm42688p_deinit(ICM42688P* icm42688p);
void icm42688p_free(ICM42688P* icm42688p);
bool icm42688p_accel_config(
ICM42688P* icm42688p,
ICM42688PAccelFullScale full_scale,
ICM42688PDataRate rate);
float icm42688p_accel_get_full_scale(ICM42688P* icm42688p);
bool icm42688p_gyro_config(
ICM42688P* icm42688p,
ICM42688PGyroFullScale full_scale,
ICM42688PDataRate rate);
float icm42688p_gyro_get_full_scale(ICM42688P* icm42688p);
bool icm42688p_read_accel_raw(ICM42688P* icm42688p, ICM42688PRawData* data);
bool icm42688p_read_gyro_raw(ICM42688P* icm42688p, ICM42688PRawData* data);
bool icm42688p_write_gyro_offset(ICM42688P* icm42688p, ICM42688PScaledData* scaled_data);
void icm42688p_apply_scale(ICM42688PRawData* raw_data, float full_scale, ICM42688PScaledData* data);
void icm42688p_apply_scale_fifo(
ICM42688P* icm42688p,
ICM42688PFifoPacket* fifo_data,
ICM42688PScaledData* accel_data,
ICM42688PScaledData* gyro_data);
float icm42688p_read_temp(ICM42688P* icm42688p);
void icm42688_fifo_enable(
ICM42688P* icm42688p,
ICM42688PIrqCallback irq_callback,
void* irq_context);
void icm42688_fifo_disable(ICM42688P* icm42688p);
uint16_t icm42688_fifo_get_count(ICM42688P* icm42688p);
bool icm42688_fifo_read(ICM42688P* icm42688p, ICM42688PFifoPacket* data);
#ifdef __cplusplus
}
#endif

176
js/vgm_sensor/js_vgm/ICM42688P/ICM42688P_regs.h Normal file
View File

@ -0,0 +1,176 @@
#pragma once
#define ICM42688_WHOAMI 0x47
// Bank 0
#define ICM42688_DEVICE_CONFIG 0x11
#define ICM42688_DRIVE_CONFIG 0x13
#define ICM42688_INT_CONFIG 0x14
#define ICM42688_FIFO_CONFIG 0x16
#define ICM42688_TEMP_DATA1 0x1D
#define ICM42688_TEMP_DATA0 0x1E
#define ICM42688_ACCEL_DATA_X1 0x1F
#define ICM42688_ACCEL_DATA_X0 0x20
#define ICM42688_ACCEL_DATA_Y1 0x21
#define ICM42688_ACCEL_DATA_Y0 0x22
#define ICM42688_ACCEL_DATA_Z1 0x23
#define ICM42688_ACCEL_DATA_Z0 0x24
#define ICM42688_GYRO_DATA_X1 0x25
#define ICM42688_GYRO_DATA_X0 0x26
#define ICM42688_GYRO_DATA_Y1 0x27
#define ICM42688_GYRO_DATA_Y0 0x28
#define ICM42688_GYRO_DATA_Z1 0x29
#define ICM42688_GYRO_DATA_Z0 0x2A
#define ICM42688_TMST_FSYNCH 0x2B
#define ICM42688_TMST_FSYNCL 0x2C
#define ICM42688_INT_STATUS 0x2D
#define ICM42688_FIFO_COUNTH 0x2E
#define ICM42688_FIFO_COUNTL 0x2F
#define ICM42688_FIFO_DATA 0x30
#define ICM42688_APEX_DATA0 0x31
#define ICM42688_APEX_DATA1 0x32
#define ICM42688_APEX_DATA2 0x33
#define ICM42688_APEX_DATA3 0x34
#define ICM42688_APEX_DATA4 0x35
#define ICM42688_APEX_DATA5 0x36
#define ICM42688_INT_STATUS2 0x37
#define ICM42688_INT_STATUS3 0x38
#define ICM42688_SIGNAL_PATH_RESET 0x4B
#define ICM42688_INTF_CONFIG0 0x4C
#define ICM42688_INTF_CONFIG1 0x4D
#define ICM42688_PWR_MGMT0 0x4E
#define ICM42688_GYRO_CONFIG0 0x4F
#define ICM42688_ACCEL_CONFIG0 0x50
#define ICM42688_GYRO_CONFIG1 0x51
#define ICM42688_GYRO_ACCEL_CONFIG0 0x52
#define ICM42688_ACCEL_CONFIG1 0x53
#define ICM42688_TMST_CONFIG 0x54
#define ICM42688_APEX_CONFIG0 0x56
#define ICM42688_SMD_CONFIG 0x57
#define ICM42688_FIFO_CONFIG1 0x5F
#define ICM42688_FIFO_CONFIG2 0x60
#define ICM42688_FIFO_CONFIG3 0x61
#define ICM42688_FSYNC_CONFIG 0x62
#define ICM42688_INT_CONFIG0 0x63
#define ICM42688_INT_CONFIG1 0x64
#define ICM42688_INT_SOURCE0 0x65
#define ICM42688_INT_SOURCE1 0x66
#define ICM42688_INT_SOURCE3 0x68
#define ICM42688_INT_SOURCE4 0x69
#define ICM42688_FIFO_LOST_PKT0 0x6C
#define ICM42688_FIFO_LOST_PKT1 0x6D
#define ICM42688_SELF_TEST_CONFIG 0x70
#define ICM42688_WHO_AM_I 0x75
#define ICM42688_REG_BANK_SEL 0x76
// Bank 1
#define ICM42688_SENSOR_CONFIG0 0x03
#define ICM42688_GYRO_CONFIG_STATIC2 0x0B
#define ICM42688_GYRO_CONFIG_STATIC3 0x0C
#define ICM42688_GYRO_CONFIG_STATIC4 0x0D
#define ICM42688_GYRO_CONFIG_STATIC5 0x0E
#define ICM42688_GYRO_CONFIG_STATIC6 0x0F
#define ICM42688_GYRO_CONFIG_STATIC7 0x10
#define ICM42688_GYRO_CONFIG_STATIC8 0x11
#define ICM42688_GYRO_CONFIG_STATIC9 0x12
#define ICM42688_GYRO_CONFIG_STATIC10 0x13
#define ICM42688_XG_ST_DATA 0x5F
#define ICM42688_YG_ST_DATA 0x60
#define ICM42688_ZG_ST_DATA 0x61
#define ICM42688_TMSTVAL0 0x62
#define ICM42688_TMSTVAL1 0x63
#define ICM42688_TMSTVAL2 0x64
#define ICM42688_INTF_CONFIG4 0x7A
#define ICM42688_INTF_CONFIG5 0x7B
#define ICM42688_INTF_CONFIG6 0x7C
// Bank 2
#define ICM42688_ACCEL_CONFIG_STATIC2 0x03
#define ICM42688_ACCEL_CONFIG_STATIC3 0x04
#define ICM42688_ACCEL_CONFIG_STATIC4 0x05
#define ICM42688_XA_ST_DATA 0x3B
#define ICM42688_YA_ST_DATA 0x3C
#define ICM42688_ZA_ST_DATA 0x3D
// Bank 4
#define ICM42688_APEX_CONFIG1 0x40
#define ICM42688_APEX_CONFIG2 0x41
#define ICM42688_APEX_CONFIG3 0x42
#define ICM42688_APEX_CONFIG4 0x43
#define ICM42688_APEX_CONFIG5 0x44
#define ICM42688_APEX_CONFIG6 0x45
#define ICM42688_APEX_CONFIG7 0x46
#define ICM42688_APEX_CONFIG8 0x47
#define ICM42688_APEX_CONFIG9 0x48
#define ICM42688_ACCEL_WOM_X_THR 0x4A
#define ICM42688_ACCEL_WOM_Y_THR 0x4B
#define ICM42688_ACCEL_WOM_Z_THR 0x4C
#define ICM42688_INT_SOURCE6 0x4D
#define ICM42688_INT_SOURCE7 0x4E
#define ICM42688_INT_SOURCE8 0x4F
#define ICM42688_INT_SOURCE9 0x50
#define ICM42688_INT_SOURCE10 0x51
#define ICM42688_OFFSET_USER0 0x77
#define ICM42688_OFFSET_USER1 0x78
#define ICM42688_OFFSET_USER2 0x79
#define ICM42688_OFFSET_USER3 0x7A
#define ICM42688_OFFSET_USER4 0x7B
#define ICM42688_OFFSET_USER5 0x7C
#define ICM42688_OFFSET_USER6 0x7D
#define ICM42688_OFFSET_USER7 0x7E
#define ICM42688_OFFSET_USER8 0x7F
// PWR_MGMT0
#define ICM42688_PWR_TEMP_ON (0 << 5)
#define ICM42688_PWR_TEMP_OFF (1 << 5)
#define ICM42688_PWR_IDLE (1 << 4)
#define ICM42688_PWR_GYRO_MODE_OFF (0 << 2)
#define ICM42688_PWR_GYRO_MODE_LN (3 << 2)
#define ICM42688_PWR_ACCEL_MODE_OFF (0 << 0)
#define ICM42688_PWR_ACCEL_MODE_LP (2 << 0)
#define ICM42688_PWR_ACCEL_MODE_LN (3 << 0)
// GYRO_CONFIG0
#define ICM42688_GFS_2000DPS (0x00 << 5)
#define ICM42688_GFS_1000DPS (0x01 << 5)
#define ICM42688_GFS_500DPS (0x02 << 5)
#define ICM42688_GFS_250DPS (0x03 << 5)
#define ICM42688_GFS_125DPS (0x04 << 5)
#define ICM42688_GFS_62_5DPS (0x05 << 5)
#define ICM42688_GFS_31_25DPS (0x06 << 5)
#define ICM42688_GFS_15_625DPS (0x07 << 5)
#define ICM42688_GODR_32kHz 0x01
#define ICM42688_GODR_16kHz 0x02
#define ICM42688_GODR_8kHz 0x03
#define ICM42688_GODR_4kHz 0x04
#define ICM42688_GODR_2kHz 0x05
#define ICM42688_GODR_1kHz 0x06
#define ICM42688_GODR_200Hz 0x07
#define ICM42688_GODR_100Hz 0x08
#define ICM42688_GODR_50Hz 0x09
#define ICM42688_GODR_25Hz 0x0A
#define ICM42688_GODR_12_5Hz 0x0B
#define ICM42688_GODR_500Hz 0x0F
// ACCEL_CONFIG0
#define ICM42688_AFS_16G (0x00 << 5)
#define ICM42688_AFS_8G (0x01 << 5)
#define ICM42688_AFS_4G (0x02 << 5)
#define ICM42688_AFS_2G (0x03 << 5)
#define ICM42688_AODR_32kHz 0x01
#define ICM42688_AODR_16kHz 0x02
#define ICM42688_AODR_8kHz 0x03
#define ICM42688_AODR_4kHz 0x04
#define ICM42688_AODR_2kHz 0x05
#define ICM42688_AODR_1kHz 0x06
#define ICM42688_AODR_200Hz 0x07
#define ICM42688_AODR_100Hz 0x08
#define ICM42688_AODR_50Hz 0x09
#define ICM42688_AODR_25Hz 0x0A
#define ICM42688_AODR_12_5Hz 0x0B
#define ICM42688_AODR_6_25Hz 0x0C
#define ICM42688_AODR_3_125Hz 0x0D
#define ICM42688_AODR_1_5625Hz 0x0E
#define ICM42688_AODR_500Hz 0x0F

328
js/vgm_sensor/js_vgm/imu.c Normal file
View File

@ -0,0 +1,328 @@
#include <furi.h>
#include "imu.h"
#include "ICM42688P/ICM42688P.h"
#define TAG "IMU"
#define ACCEL_GYRO_RATE DataRate100Hz
#define FILTER_SAMPLE_FREQ 100.f
#define FILTER_BETA 0.08f
#define SAMPLE_RATE_DIV 5
#define SENSITIVITY_K 30.f
#define EXP_RATE 1.1f
#define IMU_CALI_AVG 64
typedef enum {
ImuStop = (1 << 0),
ImuNewData = (1 << 1),
} ImuThreadFlags;
#define FLAGS_ALL (ImuStop | ImuNewData)
typedef struct {
float q0;
float q1;
float q2;
float q3;
float roll;
float pitch;
float yaw;
} ImuProcessedData;
typedef struct {
FuriThread* thread;
ICM42688P* icm42688p;
ImuProcessedData processed_data;
bool lefty;
} ImuThread;
static void imu_madgwick_filter(
ImuProcessedData* out,
ICM42688PScaledData* accel,
ICM42688PScaledData* gyro);
static void imu_irq_callback(void* context) {
furi_assert(context);
ImuThread* imu = context;
furi_thread_flags_set(furi_thread_get_id(imu->thread), ImuNewData);
}
static void imu_process_data(ImuThread* imu, ICM42688PFifoPacket* in_data) {
ICM42688PScaledData accel_data;
ICM42688PScaledData gyro_data;
// Get accel and gyro data in g and degrees/s
icm42688p_apply_scale_fifo(imu->icm42688p, in_data, &accel_data, &gyro_data);
// Gyro: degrees/s to rads/s
gyro_data.x = gyro_data.x / 180.f * M_PI;
gyro_data.y = gyro_data.y / 180.f * M_PI;
gyro_data.z = gyro_data.z / 180.f * M_PI;
// Sensor Fusion algorithm
ImuProcessedData* out = &imu->processed_data;
imu_madgwick_filter(out, &accel_data, &gyro_data);
// Quaternion to euler angles
float roll = atan2f(
out->q0 * out->q1 + out->q2 * out->q3, 0.5f - out->q1 * out->q1 - out->q2 * out->q2);
float pitch = asinf(-2.0f * (out->q1 * out->q3 - out->q0 * out->q2));
float yaw = atan2f(
out->q1 * out->q2 + out->q0 * out->q3, 0.5f - out->q2 * out->q2 - out->q3 * out->q3);
// Euler angles: rads to degrees
out->roll = roll / M_PI * 180.f;
out->pitch = pitch / M_PI * 180.f;
out->yaw = yaw / M_PI * 180.f;
}
static void calibrate_gyro(ImuThread* imu) {
ICM42688PRawData data;
ICM42688PScaledData offset_scaled = {.x = 0.f, .y = 0.f, .z = 0.f};
icm42688p_write_gyro_offset(imu->icm42688p, &offset_scaled);
furi_delay_ms(10);
int32_t avg_x = 0;
int32_t avg_y = 0;
int32_t avg_z = 0;
for(uint8_t i = 0; i < IMU_CALI_AVG; i++) {
icm42688p_read_gyro_raw(imu->icm42688p, &data);
avg_x += data.x;
avg_y += data.y;
avg_z += data.z;
furi_delay_ms(2);
}
data.x = avg_x / IMU_CALI_AVG;
data.y = avg_y / IMU_CALI_AVG;
data.z = avg_z / IMU_CALI_AVG;
icm42688p_apply_scale(&data, icm42688p_gyro_get_full_scale(imu->icm42688p), &offset_scaled);
FURI_LOG_I(
TAG,
"Offsets: x %f, y %f, z %f",
(double)offset_scaled.x,
(double)offset_scaled.y,
(double)offset_scaled.z);
icm42688p_write_gyro_offset(imu->icm42688p, &offset_scaled);
}
// static float imu_angle_diff(float a, float b) {
// float diff = a - b;
// if(diff > 180.f)
// diff -= 360.f;
// else if(diff < -180.f)
// diff += 360.f;
// return diff;
// }
static int32_t imu_thread(void* context) {
furi_assert(context);
ImuThread* imu = context;
// float yaw_last = 0.f;
// float pitch_last = 0.f;
// float diff_x = 0.f;
// float diff_y = 0.f;
calibrate_gyro(imu);
icm42688p_accel_config(imu->icm42688p, AccelFullScale16G, ACCEL_GYRO_RATE);
icm42688p_gyro_config(imu->icm42688p, GyroFullScale2000DPS, ACCEL_GYRO_RATE);
imu->processed_data.q0 = 1.f;
imu->processed_data.q1 = 0.f;
imu->processed_data.q2 = 0.f;
imu->processed_data.q3 = 0.f;
icm42688_fifo_enable(imu->icm42688p, imu_irq_callback, imu);
while(1) {
uint32_t events = furi_thread_flags_wait(FLAGS_ALL, FuriFlagWaitAny, FuriWaitForever);
if(events & ImuStop) {
break;
}
if(events & ImuNewData) {
uint16_t data_pending = icm42688_fifo_get_count(imu->icm42688p);
ICM42688PFifoPacket data;
while(data_pending--) {
icm42688_fifo_read(imu->icm42688p, &data);
imu_process_data(imu, &data);
}
}
}
icm42688_fifo_disable(imu->icm42688p);
return 0;
}
ImuThread* imu_start(ICM42688P* icm42688p) {
ImuThread* imu = malloc(sizeof(ImuThread));
imu->icm42688p = icm42688p;
imu->thread = furi_thread_alloc_ex("ImuThread", 4096, imu_thread, imu);
imu->lefty = furi_hal_rtc_is_flag_set(FuriHalRtcFlagHandOrient);
furi_thread_start(imu->thread);
return imu;
}
void imu_stop(ImuThread* imu) {
furi_assert(imu);
furi_thread_flags_set(furi_thread_get_id(imu->thread), ImuStop);
furi_thread_join(imu->thread);
furi_thread_free(imu->thread);
free(imu);
}
static float imu_inv_sqrt(float number) {
union {
float f;
uint32_t i;
} conv = {.f = number};
conv.i = 0x5F3759Df - (conv.i >> 1);
conv.f *= 1.5f - (number * 0.5f * conv.f * conv.f);
return conv.f;
}
/* Simple madgwik filter, based on: https://github.com/arduino-libraries/MadgwickAHRS/ */
static void imu_madgwick_filter(
ImuProcessedData* out,
ICM42688PScaledData* accel,
ICM42688PScaledData* gyro) {
float recipNorm;
float s0, s1, s2, s3;
float qDot1, qDot2, qDot3, qDot4;
float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2, _8q1, _8q2, q0q0, q1q1, q2q2, q3q3;
// Rate of change of quaternion from gyroscope
qDot1 = 0.5f * (-out->q1 * gyro->x - out->q2 * gyro->y - out->q3 * gyro->z);
qDot2 = 0.5f * (out->q0 * gyro->x + out->q2 * gyro->z - out->q3 * gyro->y);
qDot3 = 0.5f * (out->q0 * gyro->y - out->q1 * gyro->z + out->q3 * gyro->x);
qDot4 = 0.5f * (out->q0 * gyro->z + out->q1 * gyro->y - out->q2 * gyro->x);
// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
if(!((accel->x == 0.0f) && (accel->y == 0.0f) && (accel->z == 0.0f))) {
// Normalise accelerometer measurement
recipNorm = imu_inv_sqrt(accel->x * accel->x + accel->y * accel->y + accel->z * accel->z);
accel->x *= recipNorm;
accel->y *= recipNorm;
accel->z *= recipNorm;
// Auxiliary variables to avoid repeated arithmetic
_2q0 = 2.0f * out->q0;
_2q1 = 2.0f * out->q1;
_2q2 = 2.0f * out->q2;
_2q3 = 2.0f * out->q3;
_4q0 = 4.0f * out->q0;
_4q1 = 4.0f * out->q1;
_4q2 = 4.0f * out->q2;
_8q1 = 8.0f * out->q1;
_8q2 = 8.0f * out->q2;
q0q0 = out->q0 * out->q0;
q1q1 = out->q1 * out->q1;
q2q2 = out->q2 * out->q2;
q3q3 = out->q3 * out->q3;
// Gradient decent algorithm corrective step
s0 = _4q0 * q2q2 + _2q2 * accel->x + _4q0 * q1q1 - _2q1 * accel->y;
s1 = _4q1 * q3q3 - _2q3 * accel->x + 4.0f * q0q0 * out->q1 - _2q0 * accel->y - _4q1 +
_8q1 * q1q1 + _8q1 * q2q2 + _4q1 * accel->z;
s2 = 4.0f * q0q0 * out->q2 + _2q0 * accel->x + _4q2 * q3q3 - _2q3 * accel->y - _4q2 +
_8q2 * q1q1 + _8q2 * q2q2 + _4q2 * accel->z;
s3 = 4.0f * q1q1 * out->q3 - _2q1 * accel->x + 4.0f * q2q2 * out->q3 - _2q2 * accel->y;
recipNorm =
imu_inv_sqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
s0 *= recipNorm;
s1 *= recipNorm;
s2 *= recipNorm;
s3 *= recipNorm;
// Apply feedback step
qDot1 -= FILTER_BETA * s0;
qDot2 -= FILTER_BETA * s1;
qDot3 -= FILTER_BETA * s2;
qDot4 -= FILTER_BETA * s3;
}
// Integrate rate of change of quaternion to yield quaternion
out->q0 += qDot1 * (1.0f / FILTER_SAMPLE_FREQ);
out->q1 += qDot2 * (1.0f / FILTER_SAMPLE_FREQ);
out->q2 += qDot3 * (1.0f / FILTER_SAMPLE_FREQ);
out->q3 += qDot4 * (1.0f / FILTER_SAMPLE_FREQ);
// Normalise quaternion
recipNorm = imu_inv_sqrt(
out->q0 * out->q0 + out->q1 * out->q1 + out->q2 * out->q2 + out->q3 * out->q3);
out->q0 *= recipNorm;
out->q1 *= recipNorm;
out->q2 *= recipNorm;
out->q3 *= recipNorm;
}
/* IMU API */
struct Imu {
FuriHalSpiBusHandle* icm42688p_device;
ICM42688P* icm42688p;
ImuThread* thread;
bool present;
};
Imu* imu_alloc(void) {
Imu* imu = malloc(sizeof(Imu));
imu->icm42688p_device = malloc(sizeof(FuriHalSpiBusHandle));
memcpy(imu->icm42688p_device, &furi_hal_spi_bus_handle_external, sizeof(FuriHalSpiBusHandle));
imu->icm42688p_device->cs = &gpio_ext_pc3;
imu->icm42688p = icm42688p_alloc(imu->icm42688p_device, &gpio_ext_pb2);
imu->present = icm42688p_init(imu->icm42688p);
if(imu->present) {
imu->thread = imu_start(imu->icm42688p);
}
return imu;
}
void imu_free(Imu* imu) {
if(imu->present) {
imu_stop(imu->thread);
}
icm42688p_deinit(imu->icm42688p);
icm42688p_free(imu->icm42688p);
free(imu->icm42688p_device);
free(imu);
}
bool imu_present(Imu* imu) {
return imu->present;
}
float imu_pitch_get(Imu* imu) {
// we pretend that reading a float is an atomic operation
return imu->thread->lefty ? -imu->thread->processed_data.pitch :
imu->thread->processed_data.pitch;
}
float imu_roll_get(Imu* imu) {
// we pretend that reading a float is an atomic operation
return imu->thread->processed_data.roll;
}
float imu_yaw_get(Imu* imu) {
// we pretend that reading a float is an atomic operation
return imu->thread->lefty ? -imu->thread->processed_data.yaw : imu->thread->processed_data.yaw;
}

15
js/vgm_sensor/js_vgm/imu.h Normal file
View File

@ -0,0 +1,15 @@
#pragma once
typedef struct Imu Imu;
Imu* imu_alloc(void);
void imu_free(Imu* imu);
bool imu_present(Imu* imu);
float imu_pitch_get(Imu* imu);
float imu_roll_get(Imu* imu);
float imu_yaw_get(Imu* imu);

159
js/vgm_sensor/js_vgm/js_vgm.c Normal file
View File

@ -0,0 +1,159 @@
#include "../../js_modules.h"
#include <furi.h>
#include <toolbox/path.h>
#include "imu.h"
#define TAG "JsVgm"
typedef struct {
Imu* imu;
bool present;
} JsVgmInst;
static void js_vgm_get_pitch(struct mjs* mjs) {
mjs_val_t obj_inst = mjs_get(mjs, mjs_get_this(mjs), INST_PROP_NAME, ~0);
JsVgmInst* vgm = mjs_get_ptr(mjs, obj_inst);
furi_assert(vgm);
if(vgm->present) {
mjs_return(mjs, mjs_mk_number(mjs, imu_pitch_get(vgm->imu)));
} else {
FURI_LOG_T(TAG, "VGM not present.");
mjs_return(mjs, mjs_mk_undefined());
}
}
static void js_vgm_get_roll(struct mjs* mjs) {
mjs_val_t obj_inst = mjs_get(mjs, mjs_get_this(mjs), INST_PROP_NAME, ~0);
JsVgmInst* vgm = mjs_get_ptr(mjs, obj_inst);
furi_assert(vgm);
if(vgm->present) {
mjs_return(mjs, mjs_mk_number(mjs, imu_roll_get(vgm->imu)));
} else {
FURI_LOG_T(TAG, "VGM not present.");
mjs_return(mjs, mjs_mk_undefined());
}
}
static void js_vgm_get_yaw(struct mjs* mjs) {
mjs_val_t obj_inst = mjs_get(mjs, mjs_get_this(mjs), INST_PROP_NAME, ~0);
JsVgmInst* vgm = mjs_get_ptr(mjs, obj_inst);
furi_assert(vgm);
if(vgm->present) {
mjs_return(mjs, mjs_mk_number(mjs, imu_yaw_get(vgm->imu)));
} else {
FURI_LOG_T(TAG, "VGM not present.");
mjs_return(mjs, mjs_mk_undefined());
}
}
static float distance(float current, float start, float angle) {
// make 0 to 359.999...
current = (current < 0) ? current + 360 : current;
start = (start < 0) ? start + 360 : start;
// get max and min
bool max_is_current = current > start;
float max = (max_is_current) ? current : start;
float min = (max_is_current) ? start : current;
// get diff, check if it's greater than 180, and adjust
float diff = max - min;
bool diff_gt_180 = diff > 180;
if(diff_gt_180) {
diff = 360 - diff;
}
// if diff is less than angle return 0
if(diff < angle) {
FURI_LOG_T(TAG, "diff: %f, angle: %f", (double)diff, (double)angle);
return 0;
}
// return diff with sign
return (max_is_current ^ diff_gt_180) ? -diff : diff;
}
static void js_vgm_delta_yaw(struct mjs* mjs) {
mjs_val_t obj_inst = mjs_get(mjs, mjs_get_this(mjs), INST_PROP_NAME, ~0);
JsVgmInst* vgm = mjs_get_ptr(mjs, obj_inst);
furi_assert(vgm);
size_t num_args = mjs_nargs(mjs);
if(num_args < 1 || num_args > 2) {
mjs_prepend_errorf(
mjs,
MJS_BAD_ARGS_ERROR,
"Invalid args. Pass (angle [, timeout]). Got %d args.",
num_args);
mjs_return(mjs, mjs_mk_undefined());
return;
}
if(!vgm->present) {
FURI_LOG_T(TAG, "VGM not present.");
mjs_return(mjs, MJS_UNDEFINED);
return;
}
double angle = mjs_get_double(mjs, mjs_arg(mjs, 0));
if(isnan(angle)) {
mjs_prepend_errorf(mjs, MJS_TYPE_ERROR, "Invalid arg (angle).");
mjs_return(mjs, mjs_mk_undefined());
return;
}
uint32_t ms = (num_args == 2) ? mjs_get_int(mjs, mjs_arg(mjs, 1)) : 3000;
uint32_t timeout = furi_get_tick() + ms;
float initial_yaw = imu_yaw_get(vgm->imu);
do {
float current_yaw = imu_yaw_get(vgm->imu);
double diff = distance(current_yaw, initial_yaw, angle);
if(diff != 0) {
mjs_return(mjs, mjs_mk_number(mjs, diff));
return;
}
furi_delay_ms(100);
} while(furi_get_tick() < timeout);
mjs_return(mjs, mjs_mk_number(mjs, 0));
}
static void* js_vgm_create(struct mjs* mjs, mjs_val_t* object) {
JsVgmInst* vgm = malloc(sizeof(JsVgmInst));
vgm->imu = imu_alloc();
vgm->present = imu_present(vgm->imu);
if(!vgm->present) FURI_LOG_W(TAG, "VGM not present.");
mjs_val_t vgm_obj = mjs_mk_object(mjs);
mjs_set(mjs, vgm_obj, INST_PROP_NAME, ~0, mjs_mk_foreign(mjs, vgm));
mjs_set(mjs, vgm_obj, "getPitch", ~0, MJS_MK_FN(js_vgm_get_pitch));
mjs_set(mjs, vgm_obj, "getRoll", ~0, MJS_MK_FN(js_vgm_get_roll));
mjs_set(mjs, vgm_obj, "getYaw", ~0, MJS_MK_FN(js_vgm_get_yaw));
mjs_set(mjs, vgm_obj, "deltaYaw", ~0, MJS_MK_FN(js_vgm_delta_yaw));
*object = vgm_obj;
return vgm;
}
static void js_vgm_destroy(void* inst) {
JsVgmInst* vgm = inst;
furi_assert(vgm);
imu_free(vgm->imu);
vgm->imu = NULL;
free(vgm);
}
static const JsModuleDescriptor js_vgm_desc = {
name: "vgm",
create: js_vgm_create,
destroy: js_vgm_destroy,
};
static const FlipperAppPluginDescriptor plugin_descriptor = {
.appid = PLUGIN_APP_ID,
.ep_api_version = PLUGIN_API_VERSION,
.entry_point = &js_vgm_desc,
};
const FlipperAppPluginDescriptor* js_vgm_ep(void) {
return &plugin_descriptor;
}

37
js/vgm_sensor/speaker_api.js Normal file
View File

@ -0,0 +1,37 @@
({
_acquired: false,
_acquire: ffi("int furi_hal_speaker_acquire(int)"),
_release: ffi("void furi_hal_speaker_release(void)"),
start: ffi("void furi_hal_speaker_start(float, float)"),
stop: ffi("void furi_hal_speaker_stop(void)"),
acquire: function (timeout) {
if (!this._acquired) {
this._acquired = this._acquire(timeout);
}
return this._acquired;
},
acquired: function () {
return this._acquired;
},
release: function () {
if (this._acquired) {
this._release();
this._acquired = false;
}
},
play: function (frequency, volume, duration) {
let already_acquired = this.acquired();
if (!already_acquired) {
this.acquire(1000);
};
if (this.acquired()) {
this.start(frequency, volume);
delay(duration);
this.stop();
}
if (!already_acquired) {
this.release();
}
},
}
)

38
js/vgm_sensor/tone_rotate.js Normal file
View File

@ -0,0 +1,38 @@
let vgm = require("vgm");
let __dirpath = "/ext/apps/Scripts";
let speaker = load(__dirpath + "/speaker_api.js");
let min_delta_angle = 29.98;
let max_wait_ms = 10000;
let freq_hz = 440;
//Uncomment below to test for various test cases...
//print(vgm.deltaYaw("30.312", 1000)); // Invalid arg (angle).
//print(vgm.deltaYaw()); // Invalid args. Pass (angle, [timeout]). Got 0 args.
//print(vgm.deltaYaw(1,2,3)); // Invalid args. Pass (angle, [timeout]). Got 3 args.
//print(vgm.deltaYaw(29.98, 1000)); // This should work fine.
// Show the pitch, roll, and yaw values for 5 seconds
for (let i = 0; i < 5; i++) {
print("P", vgm.getPitch(), "R", vgm.getRoll(), "Y", vgm.getYaw());
delay(1000);
}
// Play a sound that changes pitch based on yaw
for (let i = 0; i < 500; i++) {
print("Freq", freq_hz, "Hz");
speaker.play(freq_hz, 1.0, 250);
let delta = vgm.deltaYaw(min_delta_angle, max_wait_ms);
if (delta === undefined) {
print("No VGM detected. Exiting...");
break;
}
delta /= (min_delta_angle / 1.02);
if (delta > 0) {
freq_hz *= delta;
} else if (delta < 0) {
freq_hz /= -delta;
}
}