mirror of
https://github.com/DarkFlippers/unleashed-firmware.git
synced 2024-11-23 10:01:58 +03:00
js vgm module and widget fix
VGM js module by jamisonderek Js fixes by jamisonderek
This commit is contained in:
MX
parent
f88a916ccc
commit
3269f751cd
@ -117,3 +117,11 @@ App(
|
||||
requires=["js_app"],
|
||||
sources=["modules/js_widget.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"],
|
||||
)
|
||||
|
||||
297
applications/system/js_app/modules/js_vgm/ICM42688P/ICM42688P.c
Normal file
297
applications/system/js_app/modules/js_vgm/ICM42688P/ICM42688P.c
Normal 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, ®_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*)®_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, ®_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
applications/system/js_app/modules/js_vgm/ICM42688P/ICM42688P.h
Normal file
127
applications/system/js_app/modules/js_vgm/ICM42688P/ICM42688P.h
Normal 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
|
||||
@ -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
|
||||
3
applications/system/js_app/modules/js_vgm/README.md
Normal file
3
applications/system/js_app/modules/js_vgm/README.md
Normal 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.
|
||||
328
applications/system/js_app/modules/js_vgm/imu.c
Normal file
328
applications/system/js_app/modules/js_vgm/imu.c
Normal 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
applications/system/js_app/modules/js_vgm/imu.h
Normal file
15
applications/system/js_app/modules/js_vgm/imu.h
Normal 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
applications/system/js_app/modules/js_vgm/js_vgm.c
Normal file
159
applications/system/js_app/modules/js_vgm/js_vgm.c
Normal 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;
|
||||
}
|
||||
@ -851,9 +851,8 @@ static void widget_remove_view(void* context) {
|
||||
ComponentArray_it(it, model->component);
|
||||
while(!ComponentArray_end_p(it)) {
|
||||
WidgetComponent* component = *ComponentArray_ref(it);
|
||||
if(component->free) {
|
||||
if(component && component->free) {
|
||||
component->free(component);
|
||||
component->free = NULL;
|
||||
}
|
||||
ComponentArray_next(it);
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user