From 070f53e66781811da30f2825fe3924ae905df310 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sat, 26 Aug 2023 23:52:08 -0400 Subject: [PATCH 01/18] Testing I2C work with full motion controls. --- tools/sandbox_test/Makefile | 2 +- tools/sandbox_test/buildhelp.c | 61 ++--- tools/sandbox_test/sandbox.c | 15 +- tools/sandbox_test/test_i2c/Makefile | 46 ++++ tools/sandbox_test/test_i2c/sandbox.S | 60 +++++ tools/sandbox_test/test_i2c/sandbox.c | 367 ++++++++++++++++++++++++++ 6 files changed, 517 insertions(+), 34 deletions(-) create mode 100644 tools/sandbox_test/test_i2c/Makefile create mode 100644 tools/sandbox_test/test_i2c/sandbox.S create mode 100644 tools/sandbox_test/test_i2c/sandbox.c diff --git a/tools/sandbox_test/Makefile b/tools/sandbox_test/Makefile index 8bd95731a..ef72bbe79 100644 --- a/tools/sandbox_test/Makefile +++ b/tools/sandbox_test/Makefile @@ -18,7 +18,7 @@ build : mkdir -p build sandbox.o : buildhelp sandbox.c $(SYSELF) build - ./buildhelp $(SYSELF) + ./buildhelp $(SYSELF) ../.. xtensa-esp32s2-elf-objdump -s build/sandbox.o > build/debug_sandbox_s.txt xtensa-esp32s2-elf-objdump -t build/sandbox.o > build/debug_sandbox_t.txt xtensa-esp32s2-elf-objdump -S build/sandbox.o > build/debug_sandbox_S.txt diff --git a/tools/sandbox_test/buildhelp.c b/tools/sandbox_test/buildhelp.c index a3324d342..ef0b9a483 100644 --- a/tools/sandbox_test/buildhelp.c +++ b/tools/sandbox_test/buildhelp.c @@ -52,9 +52,9 @@ int main( int argc, char ** argv ) return -5; } - if( argc != 2 ) + if( argc != 3 ) { - fprintf( stderr, "Error: Usage: [tool] [ESP32 .elf file]\n" ); + fprintf( stderr, "Error: Usage: [tool] [ESP32 .elf file] [relative exec path]\n" ); return -1; } @@ -175,34 +175,35 @@ int main( int argc, char ** argv ) snprintf( temp, sizeof( temp ) - 1, "-I%s/components/nvs_flash/include", idf_path ); appendcflag( temp ); snprintf( temp, sizeof( temp ) - 1, "-I%s/components/soc/esp32s2/include", idf_path ); appendcflag( temp ); - appendcflag( "-I../../components/hdw-battmon/include" ); - appendcflag( "-I../../components" ); - appendcflag( "-I../../components/hdw-qma6981" ); - appendcflag( "-I../../main/display" ); - appendcflag( "-I../../components/hdw-btn/include" ); - appendcflag( "-I../../components/hdw-bzr/include" ); - appendcflag( "-I../../components/crashwrap/include" ); - appendcflag( "-I../../components/hdw-touch/include" ); - appendcflag( "-I../../components/hdw-esp-now/include" ); - appendcflag( "-I../../components/hdw-temperature/include" ); - appendcflag( "-I../../components/hdw-accel/include" ); - appendcflag( "-I../../components/hdw-mic/include" ); - appendcflag( "-I../../components/hdw-spiffs/include" ); - appendcflag( "-I../../components/hdw-led/include" ); - appendcflag( "-I../../components/hdw-usb/include" ); - appendcflag( "-I../../components/hdw-tft/include" ); - appendcflag( "-I../../components/hdw-led/include" ); - appendcflag( "-I../../components/hdw-nvs/include" ); - appendcflag( "-I../../managed_components/espressif__tinyusb/src" ); - appendcflag( "-I../../managed_components/espressif__esp_tinyusb/include" ); - appendcflag( "-I../../main" ); - appendcflag( "-I../../main/utils" ); - appendcflag( "-I../../main/menu" ); - appendcflag( "-I../../main/modes" ); - appendcflag( "-I../../main/asset_loaders" ); - appendcflag( "-I../../main/modes/mainMenu" ); - appendcflag( "-I../../main/modes/colorchord" ); - appendcflag( "-I../../build/config" ); + snprintf( temp, sizeof( temp ) - 1, "-I%s/components/components/hdw-battmon/include", argv[2] ); appendcflag( temp ); + snprintf( temp, sizeof( temp ) - 1, "-I%s/components", argv[2] ); appendcflag( temp ); + snprintf( temp, sizeof( temp ) - 1, "-I%s/components/hdw-qma6981", argv[2] ); appendcflag( temp ); + snprintf( temp, sizeof( temp ) - 1, "-I%s/main/display", argv[2] ); appendcflag( temp ); + snprintf( temp, sizeof( temp ) - 1, "-I%s/components/hdw-btn/include", argv[2] ); appendcflag( temp ); + snprintf( temp, sizeof( temp ) - 1, "-I%s/components/hdw-bzr/include", argv[2] ); appendcflag( temp ); + snprintf( temp, sizeof( temp ) - 1, "-I%s/components/crashwrap/include", argv[2] ); appendcflag( temp ); + snprintf( temp, sizeof( temp ) - 1, "-I%s/components/hdw-touch/include", argv[2] ); appendcflag( temp ); + snprintf( temp, sizeof( temp ) - 1, "-I%s/components/hdw-esp-now/include", argv[2] ); appendcflag( temp ); + snprintf( temp, sizeof( temp ) - 1, "-I%s/components/hdw-temperature/include", argv[2] ); appendcflag( temp ); + snprintf( temp, sizeof( temp ) - 1, "-I%s/components/hdw-accel/include", argv[2] ); appendcflag( temp ); + snprintf( temp, sizeof( temp ) - 1, "-I%s/components/hdw-mic/include", argv[2] ); appendcflag( temp ); + snprintf( temp, sizeof( temp ) - 1, "-I%s/components/hdw-spiffs/include", argv[2] ); appendcflag( temp ); + snprintf( temp, sizeof( temp ) - 1, "-I%s/components/hdw-led/include", argv[2] ); appendcflag( temp ); + snprintf( temp, sizeof( temp ) - 1, "-I%s/components/hdw-usb/include", argv[2] ); appendcflag( temp ); + snprintf( temp, sizeof( temp ) - 1, "-I%s/components/hdw-battmon/include", argv[2] ); appendcflag( temp ); + snprintf( temp, sizeof( temp ) - 1, "-I%s/components/hdw-tft/include", argv[2] ); appendcflag( temp ); + snprintf( temp, sizeof( temp ) - 1, "-I%s/components/hdw-led/include", argv[2] ); appendcflag( temp ); + snprintf( temp, sizeof( temp ) - 1, "-I%s/components/hdw-nvs/include", argv[2] ); appendcflag( temp ); + snprintf( temp, sizeof( temp ) - 1, "-I%s/managed_components/espressif__tinyusb/src", argv[2] ); appendcflag( temp ); + snprintf( temp, sizeof( temp ) - 1, "-I%s/managed_components/espressif__esp_tinyusb/include", argv[2] ); appendcflag( temp ); + snprintf( temp, sizeof( temp ) - 1, "-I%s/main", argv[2] ); appendcflag( temp ); + snprintf( temp, sizeof( temp ) - 1, "-I%s/main/utils", argv[2] ); appendcflag( temp ); + snprintf( temp, sizeof( temp ) - 1, "-I%s/main/menu", argv[2] ); appendcflag( temp ); + snprintf( temp, sizeof( temp ) - 1, "-I%s/main/modes", argv[2] ); appendcflag( temp ); + snprintf( temp, sizeof( temp ) - 1, "-I%s/main/asset_loaders", argv[2] ); appendcflag( temp ); + snprintf( temp, sizeof( temp ) - 1, "-I%s/main/modes/mainMenu", argv[2] ); appendcflag( temp ); + snprintf( temp, sizeof( temp ) - 1, "-I%s/main/modes/colorchord", argv[2] ); appendcflag( temp ); + snprintf( temp, sizeof( temp ) - 1, "-I%s/build/config", argv[2] ); appendcflag( temp ); appendcflag( "-DTUP_DCD_ENDPOINT_MAX=8" ); } diff --git a/tools/sandbox_test/sandbox.c b/tools/sandbox_test/sandbox.c index 6f3c1dff8..9f1741ab1 100644 --- a/tools/sandbox_test/sandbox.c +++ b/tools/sandbox_test/sandbox.c @@ -20,6 +20,7 @@ const char * menu_Bootload = "Bootloader"; //#define REBOOT_TEST //#define PROFILE_TEST +//#define TEST_JOYSTICK #define MODE7_TEST #ifdef MODE7_TEST @@ -94,11 +95,11 @@ void sandbox_exit() void sandbox_tick() { #if 1 - uint32_t start, end; #ifdef PROFILE_TEST volatile uint32_t profiles[7]; // Use of volatile here to force compiler to order instructions and not cheat. // Profile function call into assembly land // Mostly used to understand function call overhead. + uint32_t start, end; start = getCycleCount(); minimal_function(); end = getCycleCount(); @@ -169,6 +170,11 @@ void sandbox_tick() menu = menuButton(menu, evt); } #endif + +#ifdef TEST_JOYSTICK + uint32_t start, end; + + // Show king donut where have our joystick. int32_t phi = 0; int32_t r = 0; int32_t intensity = 0; @@ -185,10 +191,13 @@ void sandbox_tick() if( phi >= 360 ) phi -= 360; int32_t sX = getSin1024( phi ) * r; - drawWsg( &example_sprite, 120-10 + (sX>>15), 140-20 + (sY>>15), 0, 0, 0 ); + drawWsg( &example_sprite, 120-10 + (sX>>15), 140-20 + (sY>>15), 0, 0, 360-phi ); } - ESP_LOGI( "sandbox", "TBV [%lu] %ld %ld %ld %d", end-start, phi, r, intensity, tbv ); + ESP_LOGI( "sandbox", "[%lu] %ld %ld %ld %d", end-start, phi, r, intensity, tbv ); + +#endif + } void sandboxBackgroundDrawCallback(int16_t x, int16_t y, int16_t w, int16_t h, int16_t up, int16_t upNum ) diff --git a/tools/sandbox_test/test_i2c/Makefile b/tools/sandbox_test/test_i2c/Makefile new file mode 100644 index 000000000..cc5d90aab --- /dev/null +++ b/tools/sandbox_test/test_i2c/Makefile @@ -0,0 +1,46 @@ +all : ../sandbox_upload run ../sandbox_interactive + +UNAME := $(shell uname) + +ifeq ($(UNAME), Linux) +CFLAGS:=-g -O0 +LDFLAGS:=-ludev +CC:=gcc +else +CFLAGS:=-Os -s +CC:=gcc +LDFLAGS:=C:/windows/system32/setupapi.dll +endif + +SYSELF:=../../../build/swadge2024.elf + +build : + mkdir -p build + +sandbox.o : ../buildhelp sandbox.c $(SYSELF) build + ../buildhelp $(SYSELF) ../../.. + xtensa-esp32s2-elf-objdump -s build/sandbox.o > build/debug_sandbox_s.txt + xtensa-esp32s2-elf-objdump -t build/sandbox.o > build/debug_sandbox_t.txt + xtensa-esp32s2-elf-objdump -S build/sandbox.o > build/debug_sandbox_S.txt + +run : ../sandbox_upload sandbox.o + ../sandbox_upload + +../buildhelp : ../buildhelp.c + $(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS) + +../sandbox_upload : ../sandbox_upload.c + $(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS) + +../sandbox_interactive : ../sandbox_interactive.c + $(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS) + +monitor : sandbox_interactive + ../sandbox_interactive + +interactive : ../sandbox_interactive build + ../sandbox_interactive sandbox.c sandbox.S $(SYSELF) + +clean : + rm -rf *.o *~ buildhelp build/debug_sandbox_s.txt build/debug_sandbox_t.txt build/debug_sandbox_S.txt build/sandbox_inst.bin build/sandbox_data.bin build/buildhelp build/sandbox.o sandbox_upload build/sandbox.lds build/provided.lds build/sandbox_symbols.txt build/system_symbols.txt sandbox_interactive buildhelp.exe sandbox_upload.exe + diff --git a/tools/sandbox_test/test_i2c/sandbox.S b/tools/sandbox_test/test_i2c/sandbox.S new file mode 100644 index 000000000..7aeeb3406 --- /dev/null +++ b/tools/sandbox_test/test_i2c/sandbox.S @@ -0,0 +1,60 @@ +# This is a .S file. While you can operate with volatile __asm__, it's easier many +# times to use a .S file. Which lets you freely write assembly. + +# Because this is a .S file you actually can #include things, etc. + +#include "soc/gpio_reg.h" + +# You can use this reference: https://0x04.net/~mwk/doc/xtensa.pdf +# A guide on how to use gas (this .S format) is here: +# https://ftp.gnu.org/old-gnu/Manuals/gas-2.9.1/html_chapter/as_7.html +# https://dl.espressif.com/github_assets/espressif/xtensa-isa-doc/releases/download/latest/Xtensa.pdf + +.align 4 + +# A little weird, you store referneces and big constants up here, before your code. +# Then you reference it below. + +_advanced_usb_printf_head: .long advanced_usb_printf_head +_GPIO_IN_REG: .long GPIO_IN_REG + +# uint32_t test_function( uint32_t parameter ) +# a2 is the first parameter, then a3, etc. +# a2 is also the return value. +# See this for more ABI info: http://wiki.linux-xtensa.org/index.php/ABI_Interface +# +# This function will take on one parameter, shift it up by 16, then or it with the +# value read from advanced_usb_printf_head +# +.align 4 +.global test_function +test_function: + entry sp, 32 # This saves off some of the registers we're using + l32r a3, _advanced_usb_printf_head + slli a2, a2, 16 # Putting this code here hides the latency from the l32r + l32i a3, a3, 0 + or a2, a2, a3 + retw # This restores the registers. + +.align 4 +.global minimal_function +minimal_function: + entry sp, 16 + retw + + +.align 4 +.global asm_read_gpio +asm_read_gpio: + entry sp, 16 + + l32r a2, _GPIO_IN_REG + + # you can actually fit 6 instructions in here, for free if they don't rely on the result. + # Most of the time it doesn't work out that nicely, but here it happened to. + # l32r's are really, _realy_ slow. + + l32i a2, a2, 0 + retw + + diff --git a/tools/sandbox_test/test_i2c/sandbox.c b/tools/sandbox_test/test_i2c/sandbox.c new file mode 100644 index 000000000..955eaa3aa --- /dev/null +++ b/tools/sandbox_test/test_i2c/sandbox.c @@ -0,0 +1,367 @@ +#include +#include +#include "esp_system.h" +#include "swadge2024.h" +#include "hal/gpio_types.h" +#include "esp_log.h" +#include "soc/efuse_reg.h" +#include "soc/soc.h" +#include "soc/system_reg.h" +#include "hdw-tft.h" +#include "mainMenu.h" +#include "soc/rtc_cntl_reg.h" +#include "soc/gpio_reg.h" +#include "coreutil.h" +#include "hdw-btn.h" + +#define LSM6DSL_ADDRESS 0x6a +#define QMC6308_ADDRESS 0x2c + +#define LSM6DSL_FUNC_CFG_ACCESS 0x01 +#define LSM6DSL_SENSOR_SYNC_TIME_FRAME 0x04 +#define LSM6DSL_FIFO_CTRL1 0x06 +#define LSM6DSL_FIFO_CTRL2 0x07 +#define LSM6DSL_FIFO_CTRL3 0x08 +#define LSM6DSL_FIFO_CTRL4 0x09 +#define LSM6DSL_FIFO_CTRL5 0x0a +#define LSM6DSL_ORIENT_CFG_G 0x0b +#define LSM6DSL_INT1_CTRL 0x0d +#define LSM6DSL_INT2_CTRL 0x0e +#define LMS6DS3_WHO_AM_I 0x0f +#define LSM6DSL_CTRL1_XL 0x10 +#define LSM6DSL_CTRL2_G 0x11 +#define LSM6DSL_CTRL3_C 0x12 +#define LSM6DSL_CTRL4_C 0x13 +#define LSM6DSL_CTRL5_C 0x14 +#define LSM6DSL_CTRL6_C 0x15 +#define LSM6DSL_CTRL7_G 0x16 +#define LSM6DSL_CTRL8_XL 0x17 +#define LSM6DSL_CTRL9_XL 0x18 +#define LSM6DSL_CTRL10_C 0x19 +#define LSM6DSL_MASTER_CONFIG 0x1a +#define LSM6DSL_WAKE_UP_SRC 0x1b +#define LSM6DSL_TAP_SRC 0x1c +#define LSM6DSL_D6D_SRC 0x1d +#define LSM6DSL_STATUS_REG 0x1e +#define LSM6DSL_OUT_TEMP_L 0x20 +#define LSM6DSL_OUT_TEMP_H 0x21 +#define LMS6DS3_OUTX_L_G 0x22 +#define LMS6DS3_OUTX_H_G 0x23 +#define LMS6DS3_OUTY_L_G 0x24 +#define LMS6DS3_OUTY_H_G 0x25 +#define LMS6DS3_OUTZ_L_G 0x26 +#define LMS6DS3_OUTZ_H_G 0x27 +#define LMS6DS3_OUTX_L_XL 0x28 +#define LMS6DS3_OUTX_H_XL 0x29 +#define LMS6DS3_OUTY_L_XL 0x2a +#define LMS6DS3_OUTY_H_XL 0x2b +#define LMS6DS3_OUTZ_L_XL 0x2c +#define LMS6DS3_OUTZ_H_XL 0x2d + + +struct LSM6DSLData +{ + uint16_t temp; + int32_t gyroaccum[3]; + int16_t gyrolast[3]; +} LSM6DSL; + +static esp_err_t GeneralSet( int dev, int reg, int val ) +{ + i2c_cmd_handle_t cmdHandle = i2c_cmd_link_create(); + i2c_master_start(cmdHandle); + i2c_master_write_byte(cmdHandle, dev << 1, false); + i2c_master_write_byte(cmdHandle, reg, false); + i2c_master_write_byte(cmdHandle, val, true); + i2c_master_stop(cmdHandle); + esp_err_t err = i2c_master_cmd_begin(I2C_NUM_0, cmdHandle, 100); + i2c_cmd_link_delete(cmdHandle); + return err; +} + +static esp_err_t LSM6DSLSet( int reg, int val ) +{ + return GeneralSet( LSM6DSL_ADDRESS, reg, val ); +} + +static int GeneralI2CGet( int device, int reg, uint8_t * data, int data_len ) +{ + i2c_cmd_handle_t cmdHandle = i2c_cmd_link_create(); + i2c_master_start(cmdHandle); + i2c_master_write_byte(cmdHandle, device << 1, false); + i2c_master_write_byte(cmdHandle, reg, false); + i2c_master_start(cmdHandle); + i2c_master_write_byte(cmdHandle, device << 1 | I2C_MASTER_READ, false); + i2c_master_read(cmdHandle, data, data_len, I2C_MASTER_LAST_NACK); + i2c_master_stop(cmdHandle); + esp_err_t err = i2c_master_cmd_begin(I2C_NUM_0, cmdHandle, 100); + i2c_cmd_link_delete(cmdHandle); + if( err ) return err; + else return data_len; +} + + +static int ReadLSM6DSL( uint8_t * data, int data_len ) +{ + i2c_cmd_handle_t cmdHandle = i2c_cmd_link_create(); + i2c_master_start(cmdHandle); + i2c_master_write_byte(cmdHandle, LSM6DSL_ADDRESS << 1, false); + i2c_master_write_byte(cmdHandle, 0x3A, false); + i2c_master_start(cmdHandle); + i2c_master_write_byte(cmdHandle, LSM6DSL_ADDRESS << 1 | I2C_MASTER_READ, false); + uint32_t fifolen = 0; + i2c_master_read(cmdHandle, (uint8_t*)&fifolen, 3, I2C_MASTER_LAST_NACK); + i2c_master_stop(cmdHandle); + esp_err_t err = i2c_master_cmd_begin(I2C_NUM_0, cmdHandle, 100); + i2c_cmd_link_delete(cmdHandle); + if( err < 0 ) return err; + + fifolen &= 0x3ff; + if( fifolen > data_len / 2 ) fifolen = data_len / 2; + + cmdHandle = i2c_cmd_link_create(); + i2c_master_start(cmdHandle); + i2c_master_write_byte(cmdHandle, LSM6DSL_ADDRESS << 1 | I2C_MASTER_READ, false); + i2c_master_read(cmdHandle, data, fifolen * 2, I2C_MASTER_LAST_NACK); + i2c_master_stop(cmdHandle); + err = i2c_master_cmd_begin(I2C_NUM_0, cmdHandle, 100); + + i2c_cmd_link_delete(cmdHandle); + if( err < 0 ) return err; + + return fifolen; +} + +static void LSM6DSLIntegrate() +{ + struct LSM6DSLData * ld = &LSM6DSL; + + int16_t data[72]; + int r = GeneralI2CGet( LSM6DSL_ADDRESS, 0x20, (uint8_t*)data, 2 ); + if( r < 0 ) return; + + ld->temp = data[0]; + int readr = ReadLSM6DSL( (uint8_t*)data, sizeof( data ) ); + + int samp; + int16_t * cdata = data; + + for( samp = 0; samp < readr; samp+=12 ) + { + ld->gyroaccum[0] += cdata[0]; + ld->gyroaccum[1] += cdata[1]; + ld->gyroaccum[2] += cdata[2]; + + ld->gyrolast[0] = cdata[0]; + ld->gyrolast[1] = cdata[1]; + ld->gyrolast[2] = cdata[2]; + + cdata += 6; + } +} + +static void LMS6DS3Setup() +{ + memset( &LSM6DSL, 0, sizeof(LSM6DSL) ); + // Enable access + LSM6DSLSet( LSM6DSL_FUNC_CFG_ACCESS, 0x20 ); + LSM6DSLSet( LSM6DSL_CTRL3_C, 0x81 ); // Force reset + vTaskDelay( 1 ); + LSM6DSLSet( LSM6DSL_CTRL3_C, 0x44 ); // unforce reset + LSM6DSLSet( LSM6DSL_FIFO_CTRL5, (0b0101 << 3) | 0b110 ); // 208 Hz ODR + LSM6DSLSet( LSM6DSL_FIFO_CTRL3, 0b00001001 ); // Put both devices in FIFO. + LSM6DSLSet( LSM6DSL_CTRL1_XL, 0b01011001 ); // Setup accel (16 g's FS) + LSM6DSLSet( LSM6DSL_CTRL2_G, 0b01010100 ); // Setup gyro, 500dps + LSM6DSLSet( LSM6DSL_CTRL7_G, 0b10000000 ); // Setup gyro, not high performance mode. + LSM6DSLSet( LSM6DSL_FIFO_CTRL2, 0b00000000 ); //Temp not in fifo (Why no work?) + + uint8_t who = 0xaa; + int r = GeneralI2CGet( LSM6DSL_ADDRESS, LMS6DS3_WHO_AM_I, &who, 1 ); + if( r != 1 || who != 0x6a ) + { + ESP_LOGE( "LSM6DSL", "WHOAMI Failed (%02x), %d. Cannot start part.\n", who, r ); + } +} + + +int global_i = 100; +menu_t * menu; +const char * menu_Bootload = "Bootloader"; + +// External functions defined in .S file for you assembly people. +void minimal_function(); +uint32_t test_function( uint32_t x ); +uint32_t asm_read_gpio(); + +wsg_t example_sprite; + +static void mainMenuCb(const char* label, bool selected, uint32_t settingVal) +{ + if( label == mainMenuMode.modeName ) + { + switchToSwadgeMode( &mainMenuMode ); + } + else if( label == menu_Bootload ) + { + // Uncomment this to reboot the chip into the bootloader. + // This is to test to make sure we can call ROM functions. + REG_WRITE(RTC_CNTL_OPTION1_REG, RTC_CNTL_FORCE_DOWNLOAD_BOOT); + void software_reset( uint32_t x ); + software_reset( 0 ); + } +} + +void sandbox_main(void) +{ + + ESP_LOGI( "sandbox", "Running from IRAM. %d", global_i ); + + REG_WRITE( GPIO_FUNC7_OUT_SEL_CFG_REG,4 ); // select ledc_ls_sig_out0 + + menu = initMenu("USB Sandbox", mainMenuCb); + addSingleItemToMenu(menu, mainMenuMode.modeName); + addSingleItemToMenu(menu, menu_Bootload); + + loadWsg("kid0.wsg", &example_sprite, true); + + // Try to reinstall, just in case. + i2c_config_t conf = { + .mode = I2C_MODE_MASTER, + .sda_io_num = GPIO_NUM_3, + .sda_pullup_en = GPIO_PULLUP_DISABLE, + .scl_io_num = GPIO_NUM_41, + .scl_pullup_en = GPIO_PULLUP_DISABLE, + .master.clk_speed = 1000000, + .clk_flags = I2C_SCLK_SRC_FLAG_FOR_NOMAL, + }; + + //i2c_driver_delete( I2C_NUM_0 ); + ESP_LOGI( "sandbox", "i2c_param_config=%d", i2c_param_config(I2C_NUM_0, &conf) ); + ESP_LOGI( "sandbox", "i2c_driver_install=%d", i2c_driver_install(I2C_NUM_0, conf.mode, 0, 0, 0) ); + + LMS6DS3Setup(); + GeneralSet( QMC6308_ADDRESS, 0x0b, 0x80 ); + GeneralSet( QMC6308_ADDRESS, 0x0b, 0x03 ); + GeneralSet( QMC6308_ADDRESS, 0x0a, 0x83 ); + + ESP_LOGI( "sandbox", "Loaded" ); +} + +void sandbox_exit() +{ +} + +void sandbox_tick() +{ + for( int mode = 0; mode < 8; mode++ ) + { + drawWsg( &example_sprite, 50+mode*20, (global_i%20)-10, !!(mode&1), !!(mode & 2), (mode & 4)*10); + drawWsg( &example_sprite, 50+mode*20, (global_i%20)+230, !!(mode&1), !!(mode & 2), (mode & 4)*10); + drawWsg( &example_sprite, (global_i%20)-10, 50+mode*20, !!(mode&1), !!(mode & 2), (mode & 4)*10); + drawWsg( &example_sprite, (global_i%20)+270, 50+mode*20, !!(mode&1), !!(mode & 2), (mode & 4)*10); + } + + buttonEvt_t evt = {0}; + while (checkButtonQueueWrapper(&evt)) + { + menu = menuButton(menu, evt); + } + + int i; + char ctsbuffer[1024]; + char *cts = ctsbuffer; + +#if 0 + // 0x12 = QMA7981 + // 0x2c = QMC6308 + // 0x6a = LSM6DSL + for( i = 0; i < 128; i++ ) + { + if( !(i & 0xf) ) + { + cts+=sprintf( cts, "\n%02x: ", i ); + } + + i2c_cmd_handle_t cmdHandle = i2c_cmd_link_create(); + i2c_master_start(cmdHandle); + i2c_master_write_byte(cmdHandle, i << 1, false); + i2c_master_write_byte(cmdHandle, 0, true); + i2c_master_stop(cmdHandle); + esp_err_t err = i2c_master_cmd_begin(I2C_NUM_0, cmdHandle, 100); + i2c_cmd_link_delete(cmdHandle); + + cts+=sprintf( cts, "%2x ", (err>=0)?i:0 ); + } + ESP_LOGI( "sandbox", "%s", ctsbuffer ); +#endif + +#if 0 + uint8_t rxbuf[128]; + cts = ctsbuffer; + int r = GeneralI2CGet( 0x6a, 0, rxbuf, 96 ); + for( i = 0; i < 96; i++ ) + { + if( ( i & 0x7 ) == 0 ) cts += sprintf( cts, "\n%02x: ", i ); + cts += sprintf( cts, " %02x", rxbuf[i] ); + } + ESP_LOGI( "I2C", "%s\n", ctsbuffer ); +#endif + + +#if 0 + + uint8_t rxbuf[128]; + cts = ctsbuffer; + + int r = GeneralI2CGet( LSM6DSL_ADDRESS, 0x20, rxbuf, 2 ); + int temp = rxbuf[0] | rxbuf[1]<<8; + + uint16_t data[64]; + int readr = ReadLSM6DSL( data, sizeof( data ) ); + + cts += sprintf( cts, "TEMP: %d %d ", temp, readr ); + + for( i = 0; i < readr; i++ ) + { + cts += sprintf( cts, " %04x", data[i] ); + } +#endif + + LSM6DSLIntegrate(); + + cts += sprintf( cts, "%d / %5d %5d %5d / %ld %ld %ld", LSM6DSL.temp, + LSM6DSL.gyrolast[0], LSM6DSL.gyrolast[1], LSM6DSL.gyrolast[2], + LSM6DSL.gyroaccum[0], LSM6DSL.gyroaccum[1], LSM6DSL.gyroaccum[2] ); + + ESP_LOGI( "I2C", "%s", ctsbuffer ); +} + +void sandboxBackgroundDrawCallback(int16_t x, int16_t y, int16_t w, int16_t h, int16_t up, int16_t upNum ) +{ + int i; + + //uint32_t start = getCycleCount(); + fillDisplayArea(x, y, x+w, y+h, 0 ); + for( i = 0; i < 16; i++ ) + fillDisplayArea(i*16+8, y, i*16+16+8, y+16, up*16+i ); + //mode7timing = getCycleCount() - start; +} + + +swadgeMode_t sandbox_mode = { + .modeName = "sandbox", + .wifiMode = NO_WIFI, + .overrideUsb = false, + .usesAccelerometer = false, + .usesThermometer = false, + .fnEnterMode = sandbox_main, + .fnExitMode = sandbox_exit, + .fnMainLoop = sandbox_tick, + .fnAudioCallback = NULL, + .fnBackgroundDrawCallback = sandboxBackgroundDrawCallback, + .fnEspNowRecvCb = NULL, + .fnEspNowSendCb = NULL, + .fnAdvancedUSB = NULL +}; + + From e21674d092664792247358533a54da7a8bd913e0 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sun, 27 Aug 2023 05:57:42 -0400 Subject: [PATCH 02/18] Getting closer to some math that works. --- tools/sandbox_test/test_i2c/sandbox.c | 169 ++++++++++++++++++++++---- 1 file changed, 148 insertions(+), 21 deletions(-) diff --git a/tools/sandbox_test/test_i2c/sandbox.c b/tools/sandbox_test/test_i2c/sandbox.c index 955eaa3aa..484144ce9 100644 --- a/tools/sandbox_test/test_i2c/sandbox.c +++ b/tools/sandbox_test/test_i2c/sandbox.c @@ -61,11 +61,101 @@ struct LSM6DSLData { - uint16_t temp; + int32_t temp; + uint32_t caltime; int32_t gyroaccum[3]; + + float fqQuat[4]; // Quats are wxyz + + + // For debug int16_t gyrolast[3]; + int16_t accellast[3]; + float fqQuatLast[4]; // Quats are wxyz + } LSM6DSL; +#include + +float rsqrtf ( float x ) +{ + typedef union { int32_t i; float f; } fiunion; + const float xhalf = 0.5f * x; + fiunion i = { .f = x }; + + i.i = 0x5f375a86 - ( i.i >> 1 ); + x = i.f; + x = x * ( 1.5f - xhalf * x * x ); + x = x * ( 1.5f - xhalf * x * x ); + + return x; +} + +void mathEulerToQuat( float * q, const float * euler ) +{ + float pitch = euler[0]; + float yaw = euler[1]; + float roll = euler[2]; + float cr = cosf(pitch * 0.5); + float sr = sinf(pitch * 0.5); + float cp = cosf(yaw * 0.5); + float sp = sinf(yaw * 0.5); + float cy = cosf(roll * 0.5); + float sy = sinf(roll * 0.5); + q[0] = cr * cp * cy + sr * sp * sy; + q[1] = sr * cp * cy - cr * sp * sy; + q[2] = cr * sp * cy + sr * cp * sy; + q[3] = cr * cp * sy - sr * sp * cy; +} + +void mathQuatApply(float * qout, const float * q1, const float * q2) { + // NOTE: Does not normalize + float tmpw, tmpx, tmpy; + tmpw = (q1[0] * q2[0]) - (q1[1] * q2[1]) - (q1[2] * q2[2]) - (q1[3] * q2[3]); + tmpx = (q1[0] * q2[1]) + (q1[1] * q2[0]) + (q1[2] * q2[3]) - (q1[3] * q2[2]); + tmpy = (q1[0] * q2[2]) - (q1[1] * q2[3]) + (q1[2] * q2[0]) + (q1[3] * q2[1]); + qout[3] = (q1[0] * q2[3]) + (q1[1] * q2[2]) - (q1[2] * q2[1]) + (q1[3] * q2[0]); + qout[2] = tmpy; + qout[1] = tmpx; + qout[0] = tmpw; +} + +void mathQuatNormalize(float * qout, const float * qin ) +{ + float qmag = qin[0] * qin[0] + qin[1] * qin[1] + qin[2] * qin[2] + qin[3] * qin[3]; + qmag = rsqrtf( qmag ); + qout[0] = qin[0] * qmag; + qout[1] = qin[1] * qmag; + qout[2] = qin[2] * qmag; + qout[3] = qin[3] * qmag; +} + +void mathCrossProduct(float * p, const float * a, const float * b) +{ + float tx = a[1] * b[2] - a[2] * b[1]; + float ty = a[2] * b[0] - a[0] * b[2]; + p[2] = a[0] * b[1] - a[1] * b[0]; + p[1] = ty; + p[0] = tx; +} + +void mathRotateVectorByQuaternion(float * pout, const float * q, const float * p ) +{ + // return v + 2.0 * cross(q.xyz, cross(q.xyz, v) + q.w * v); + float iqo[3]; + mathCrossProduct( iqo, q + 1, p ); + iqo[0] += q[0] * p[0]; + iqo[1] += q[0] * p[1]; + iqo[2] += q[0] * p[2]; + float ret[3]; + mathCrossProduct( ret, q + 1, iqo ); + pout[0] = ret[0] * 2.0 + p[0]; + pout[1] = ret[1] * 2.0 + p[1]; + pout[2] = ret[2] * 2.0 + p[2]; +} + + + static esp_err_t GeneralSet( int dev, int reg, int val ) { i2c_cmd_handle_t cmdHandle = i2c_cmd_link_create(); @@ -139,30 +229,58 @@ static void LSM6DSLIntegrate() int16_t data[72]; int r = GeneralI2CGet( LSM6DSL_ADDRESS, 0x20, (uint8_t*)data, 2 ); if( r < 0 ) return; - - ld->temp = data[0]; + if( r == 2 ) ld->temp = data[0]; int readr = ReadLSM6DSL( (uint8_t*)data, sizeof( data ) ); int samp; int16_t * cdata = data; + uint32_t start = getCycleCount(); + for( samp = 0; samp < readr; samp+=12 ) { - ld->gyroaccum[0] += cdata[0]; - ld->gyroaccum[1] += cdata[1]; - ld->gyroaccum[2] += cdata[2]; - - ld->gyrolast[0] = cdata[0]; - ld->gyrolast[1] = cdata[1]; - ld->gyrolast[2] = cdata[2]; + int16_t * euler_deltas = cdata; + ld->gyroaccum[0] += euler_deltas[0]; + ld->gyroaccum[1] += euler_deltas[1]; + ld->gyroaccum[2] += euler_deltas[2]; + + // 2000 dps full-scale + // 32768 is full-scale + // 208 SPS + // convert to radians. + float fScale = ( 4000.0f / 32768.0f / 208.0f * 3.14159f / 180.0f ); + + float fEulers[3] = { + 0,//-euler_deltas[0] * fScale, + euler_deltas[2], + euler_deltas[1] * 0 }; + // [0] = +X axis coming out right of controller. + // [1] = +Y axis, pointing straight up out of controller. + // [2] = +Z axis, pointing straight up out of controller. + + mathEulerToQuat( ld->fqQuatLast, fEulers ); + mathQuatApply( ld->fqQuat, ld->fqQuat, ld->fqQuatLast ); cdata += 6; } + + //mathQuatNormalize( ld->fqQuat, ld->fqQuat ); + + ld->gyrolast[0] = cdata[-6]; + ld->gyrolast[1] = cdata[-5]; + ld->gyrolast[2] = cdata[-4]; + ld->accellast[0] = cdata[-3]; + ld->accellast[1] = cdata[-2]; + ld->accellast[2] = cdata[-1]; + + ld->caltime = getCycleCount() - start; } static void LMS6DS3Setup() { memset( &LSM6DSL, 0, sizeof(LSM6DSL) ); + LSM6DSL.fqQuat[0] = 1; + // Enable access LSM6DSLSet( LSM6DSL_FUNC_CFG_ACCESS, 0x20 ); LSM6DSLSet( LSM6DSL_CTRL3_C, 0x81 ); // Force reset @@ -171,8 +289,9 @@ static void LMS6DS3Setup() LSM6DSLSet( LSM6DSL_FIFO_CTRL5, (0b0101 << 3) | 0b110 ); // 208 Hz ODR LSM6DSLSet( LSM6DSL_FIFO_CTRL3, 0b00001001 ); // Put both devices in FIFO. LSM6DSLSet( LSM6DSL_CTRL1_XL, 0b01011001 ); // Setup accel (16 g's FS) - LSM6DSLSet( LSM6DSL_CTRL2_G, 0b01010100 ); // Setup gyro, 500dps - LSM6DSLSet( LSM6DSL_CTRL7_G, 0b10000000 ); // Setup gyro, not high performance mode. + LSM6DSLSet( LSM6DSL_CTRL2_G, 0b01011100 ); // Setup gyro, 2000dps + LSM6DSLSet( LSM6DSL_CTRL4_C, 0x00 ); // Disable all filtering. + LSM6DSLSet( LSM6DSL_CTRL7_G, 0b00000000 ); // Setup gyro, not high performance mode = 0x80. High perf = 0x00 LSM6DSLSet( LSM6DSL_FIFO_CTRL2, 0b00000000 ); //Temp not in fifo (Why no work?) uint8_t who = 0xaa; @@ -236,8 +355,8 @@ void sandbox_main(void) }; //i2c_driver_delete( I2C_NUM_0 ); - ESP_LOGI( "sandbox", "i2c_param_config=%d", i2c_param_config(I2C_NUM_0, &conf) ); - ESP_LOGI( "sandbox", "i2c_driver_install=%d", i2c_driver_install(I2C_NUM_0, conf.mode, 0, 0, 0) ); +// ESP_LOGI( "sandbox", "i2c_param_config=%d", i2c_param_config(I2C_NUM_0, &conf) ); +// ESP_LOGI( "sandbox", "i2c_driver_install=%d", i2c_driver_install(I2C_NUM_0, conf.mode, 0, 0, 0) ); LMS6DS3Setup(); GeneralSet( QMC6308_ADDRESS, 0x0b, 0x80 ); @@ -261,17 +380,18 @@ void sandbox_tick() drawWsg( &example_sprite, (global_i%20)+270, 50+mode*20, !!(mode&1), !!(mode & 2), (mode & 4)*10); } - buttonEvt_t evt = {0}; + buttonEvt_t evt = {0}; while (checkButtonQueueWrapper(&evt)) { menu = menuButton(menu, evt); } - int i; + char ctsbuffer[1024]; char *cts = ctsbuffer; #if 0 + int i; // 0x12 = QMA7981 // 0x2c = QMC6308 // 0x6a = LSM6DSL @@ -328,10 +448,19 @@ void sandbox_tick() #endif LSM6DSLIntegrate(); - - cts += sprintf( cts, "%d / %5d %5d %5d / %ld %ld %ld", LSM6DSL.temp, +/* + cts += sprintf( cts, "%ld %ld / %5d %5d %5d / %5d %5d %5d / %ld %ld %ld / %f %f %f %f", + LSM6DSL.caltime, LSM6DSL.temp, + LSM6DSL.accellast[0], LSM6DSL.accellast[1], LSM6DSL.accellast[2], LSM6DSL.gyrolast[0], LSM6DSL.gyrolast[1], LSM6DSL.gyrolast[2], - LSM6DSL.gyroaccum[0], LSM6DSL.gyroaccum[1], LSM6DSL.gyroaccum[2] ); + LSM6DSL.gyroaccum[0], LSM6DSL.gyroaccum[1], LSM6DSL.gyroaccum[2], + LSM6DSL.fqQuat[0], LSM6DSL.fqQuat[1], LSM6DSL.fqQuat[2], LSM6DSL.fqQuat[3] ); +*/ + + float plusy[3] = { 0, 1, 0 }; + mathRotateVectorByQuaternion( plusy, LSM6DSL.fqQuat, plusy ); + cts += sprintf( cts, "%f %f %f %f / %f %f %f", LSM6DSL.fqQuat[0], LSM6DSL.fqQuat[1], LSM6DSL.fqQuat[2], LSM6DSL.fqQuat[3], + plusy[0], plusy[1], plusy[2] ); ESP_LOGI( "I2C", "%s", ctsbuffer ); } @@ -340,11 +469,9 @@ void sandboxBackgroundDrawCallback(int16_t x, int16_t y, int16_t w, int16_t h, i { int i; - //uint32_t start = getCycleCount(); fillDisplayArea(x, y, x+w, y+h, 0 ); for( i = 0; i < 16; i++ ) fillDisplayArea(i*16+8, y, i*16+16+8, y+16, up*16+i ); - //mode7timing = getCycleCount() - start; } From d29b3821233e5ef48b54b14557165429028a2641 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sun, 27 Aug 2023 21:37:36 -0400 Subject: [PATCH 03/18] Add run counter. --- tools/sandbox_test/buildhelp.c | 16 ++++ tools/sandbox_test/test_i2c/sandbox.c | 115 +++++++++++++++++++------- 2 files changed, 100 insertions(+), 31 deletions(-) diff --git a/tools/sandbox_test/buildhelp.c b/tools/sandbox_test/buildhelp.c index ef0b9a483..93f061a5b 100644 --- a/tools/sandbox_test/buildhelp.c +++ b/tools/sandbox_test/buildhelp.c @@ -218,6 +218,22 @@ int main( int argc, char ** argv ) if( r ) { fprintf( stderr, "Error shelling symbols. Error: %d\n", r ); return -6; } } + + // Count + { + int i = 0; + FILE * f = fopen( "build/count.txt", "r" ); + if( f ) { fscanf( f, "%d", &i ); fclose( f ); } + i++; + f = fopen( "build/count.txt", "w" ); + if( f ) + { + fprintf( f, "%d\n", i ); + fclose( f ); + } + } + + { FILE * provided = fopen( "build/provided.lds", "w" ); FILE * f = fopen( "build/system_symbols.txt", "r" ); diff --git a/tools/sandbox_test/test_i2c/sandbox.c b/tools/sandbox_test/test_i2c/sandbox.c index 484144ce9..57fe50751 100644 --- a/tools/sandbox_test/test_i2c/sandbox.c +++ b/tools/sandbox_test/test_i2c/sandbox.c @@ -63,7 +63,6 @@ struct LSM6DSLData { int32_t temp; uint32_t caltime; - int32_t gyroaccum[3]; float fqQuat[4]; // Quats are wxyz @@ -77,6 +76,14 @@ struct LSM6DSLData #include +/* Coordinate frame: + OpenGL / OpenVR / Godot / Etc... + + +X goes right. + +Y comes out top of controller. + +Z comes toward user (Into User's Eyes) +*/ + float rsqrtf ( float x ) { typedef union { int32_t i; float f; } fiunion; @@ -97,11 +104,11 @@ void mathEulerToQuat( float * q, const float * euler ) float yaw = euler[1]; float roll = euler[2]; float cr = cosf(pitch * 0.5); - float sr = sinf(pitch * 0.5); - float cp = cosf(yaw * 0.5); - float sp = sinf(yaw * 0.5); + float sr = sinf(pitch * 0.5); // Pitch: About X + float cp = cosf(yaw * 0.5); + float sp = sinf(yaw * 0.5); // Yaw: About Y float cy = cosf(roll * 0.5); - float sy = sinf(roll * 0.5); + float sy = sinf(roll * 0.5); // Roll: About Z q[0] = cr * cp * cy + sr * sp * sy; q[1] = sr * cp * cy - cr * sp * sy; q[2] = cr * sp * cy + sr * cp * sy; @@ -154,8 +161,6 @@ void mathRotateVectorByQuaternion(float * pout, const float * q, const float * p pout[2] = ret[2] * 2.0 + p[2]; } - - static esp_err_t GeneralSet( int dev, int reg, int val ) { i2c_cmd_handle_t cmdHandle = i2c_cmd_link_create(); @@ -237,12 +242,20 @@ static void LSM6DSLIntegrate() uint32_t start = getCycleCount(); + // STEP 0: Decide your coordinate frame. + + // [0] = +X axis coming out right of controller. + // [1] = +Y axis, pointing straight up out of controller, out where the USB port is. + // [2] = +Z axis, pointing up from the face of the controller. + for( samp = 0; samp < readr; samp+=12 ) { - int16_t * euler_deltas = cdata; - ld->gyroaccum[0] += euler_deltas[0]; - ld->gyroaccum[1] += euler_deltas[1]; - ld->gyroaccum[2] += euler_deltas[2]; + // Extract data from IMU + int16_t * euler_deltas = cdata; // Euler angles, from gyro. + int16_t * accel_data = cdata + 3; + + // STEP 1: Visually inspect the gyro values. + // STEP 2: Integrate the gyro values, verify they are correct. // 2000 dps full-scale // 32768 is full-scale @@ -250,28 +263,69 @@ static void LSM6DSLIntegrate() // convert to radians. float fScale = ( 4000.0f / 32768.0f / 208.0f * 3.14159f / 180.0f ); + // STEP 3: Integrate gyro values into a quaternion. + // This step is validated by working with just one axis at a time + // then apply a coordinate frame to ld->fqQuat and validate that it is + // correct. float fEulers[3] = { - 0,//-euler_deltas[0] * fScale, - euler_deltas[2], - euler_deltas[1] * 0 }; - // [0] = +X axis coming out right of controller. - // [1] = +Y axis, pointing straight up out of controller. - // [2] = +Z axis, pointing straight up out of controller. + -euler_deltas[0] * fScale, + euler_deltas[1] * fScale, + -euler_deltas[2] * fScale }; mathEulerToQuat( ld->fqQuatLast, fEulers ); mathQuatApply( ld->fqQuat, ld->fqQuat, ld->fqQuatLast ); + // STEP 4: Validate yor values by doing 4 90 degree turns + // across multiple axes. + // i.e. rotate controller down, clockwise from top, up, counter-clockwise. + // while investigating quaternion. It should return to identity. + + // STEP 6: Determine our "error" based on accelerometer. + // NOTE: This step could be done on the inner loop if you want, and done over + // every accelerometer cycle, or it can be done on the outside, every few cycles. + // all that realy matters is that it is done periodically. + + // STEP 6A: Examine vectors. Generally speaking, we want an "up" vector, not a gravity vector. + // this is "up" in the controller's point of view. + int32_t raw_up[3] = { -accel_data[0], accel_data[1], -accel_data[2] }; + //ESP_LOGI( "SB", "%ld %ld %ld", raw_up[0], raw_up[1], raw_up[2] ); + + + // TODO which directon of frame of reference? Up relative to controller? OR controller relative to world? + + cdata += 6; } - //mathQuatNormalize( ld->fqQuat, ld->fqQuat ); + // Now we move onto the outer loop. + // STEP 5: We now want to normalize the quat periodically. Don't do this too + // soon, otherwise you won't notice math errors. Realistically, this should + // only need to be done every hundreds of thousands of samples. + // + // Also, don't do this too often, otherwise you will reduce accuracy, + // unnecessarily. + float * qRot = ld->fqQuat; + float qmagsq = qRot[0] * qRot[0] + qRot[1] * qRot[1] + qRot[2] * qRot[2] + qRot[3] * qRot[3]; + if( qmagsq > 1.05 || qmagsq < 0.95 ) + { + // This normalizes everything. + qmagsq = rsqrtf( qmagsq ); + qRot[0] = qRot[0] * qmagsq; + qRot[1] = qRot[1] * qmagsq; + qRot[2] = qRot[2] * qmagsq; + qRot[3] = qRot[3] * qmagsq; + } - ld->gyrolast[0] = cdata[-6]; - ld->gyrolast[1] = cdata[-5]; - ld->gyrolast[2] = cdata[-4]; - ld->accellast[0] = cdata[-3]; - ld->accellast[1] = cdata[-2]; - ld->accellast[2] = cdata[-1]; + + if( samp ) + { + ld->gyrolast[0] = cdata[-6]; + ld->gyrolast[1] = cdata[-5]; + ld->gyrolast[2] = cdata[-4]; + ld->accellast[0] = cdata[-3]; + ld->accellast[1] = cdata[-2]; + ld->accellast[2] = cdata[-1]; + } ld->caltime = getCycleCount() - start; } @@ -342,7 +396,7 @@ void sandbox_main(void) addSingleItemToMenu(menu, menu_Bootload); loadWsg("kid0.wsg", &example_sprite, true); - +/* // Try to reinstall, just in case. i2c_config_t conf = { .mode = I2C_MODE_MASTER, @@ -354,10 +408,10 @@ void sandbox_main(void) .clk_flags = I2C_SCLK_SRC_FLAG_FOR_NOMAL, }; - //i2c_driver_delete( I2C_NUM_0 ); -// ESP_LOGI( "sandbox", "i2c_param_config=%d", i2c_param_config(I2C_NUM_0, &conf) ); -// ESP_LOGI( "sandbox", "i2c_driver_install=%d", i2c_driver_install(I2C_NUM_0, conf.mode, 0, 0, 0) ); - +// i2c_driver_delete( I2C_NUM_0 ); + ESP_LOGI( "sandbox", "i2c_param_config=%d", i2c_param_config(I2C_NUM_0, &conf) ); + ESP_LOGI( "sandbox", "i2c_driver_install=%d", i2c_driver_install(I2C_NUM_0, conf.mode, 0, 0, 0) ); +*/ LMS6DS3Setup(); GeneralSet( QMC6308_ADDRESS, 0x0b, 0x80 ); GeneralSet( QMC6308_ADDRESS, 0x0b, 0x03 ); @@ -453,11 +507,10 @@ void sandbox_tick() LSM6DSL.caltime, LSM6DSL.temp, LSM6DSL.accellast[0], LSM6DSL.accellast[1], LSM6DSL.accellast[2], LSM6DSL.gyrolast[0], LSM6DSL.gyrolast[1], LSM6DSL.gyrolast[2], - LSM6DSL.gyroaccum[0], LSM6DSL.gyroaccum[1], LSM6DSL.gyroaccum[2], LSM6DSL.fqQuat[0], LSM6DSL.fqQuat[1], LSM6DSL.fqQuat[2], LSM6DSL.fqQuat[3] ); */ - float plusy[3] = { 0, 1, 0 }; + float plusy[3] = { 1, 0, 0 }; mathRotateVectorByQuaternion( plusy, LSM6DSL.fqQuat, plusy ); cts += sprintf( cts, "%f %f %f %f / %f %f %f", LSM6DSL.fqQuat[0], LSM6DSL.fqQuat[1], LSM6DSL.fqQuat[2], LSM6DSL.fqQuat[3], plusy[0], plusy[1], plusy[2] ); From 1e97fa77661a2be9cbb21f2c3e06634786c2d947 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Mon, 28 Aug 2023 04:40:05 -0400 Subject: [PATCH 04/18] Motion controls work! --- tools/sandbox_test/test_i2c/bunny.h | 948 +++++++++++++++++++ tools/sandbox_test/test_i2c/bunny_out.obj | 947 ++++++++++++++++++ tools/sandbox_test/test_i2c/bunny_to_array.c | 36 + tools/sandbox_test/test_i2c/sandbox.c | 178 +++- 4 files changed, 2096 insertions(+), 13 deletions(-) create mode 100644 tools/sandbox_test/test_i2c/bunny.h create mode 100644 tools/sandbox_test/test_i2c/bunny_out.obj create mode 100644 tools/sandbox_test/test_i2c/bunny_to_array.c diff --git a/tools/sandbox_test/test_i2c/bunny.h b/tools/sandbox_test/test_i2c/bunny.h new file mode 100644 index 000000000..4c5a29ca9 --- /dev/null +++ b/tools/sandbox_test/test_i2c/bunny.h @@ -0,0 +1,948 @@ +#include +int16_t bunny_verts[] = { + -18622, 11337, 3876, + -17108, 3601, 701, + -1011, 8368, 2344, + -1459, 6234, 7752, + -10384, -14050, 10578, + 9108, 9861, 4806, + -9196, 15608, 8497, + -16283, -6559, 2406, + -17417, -4365, 2474, + -18389, -1521, 4815, + 22219, -6826, 5279, + 23808, -7886, 4430, + -6203, 20422, -1965, + -12974, 18821, -4263, + -10629, 18285, -6951, + -10895, 20703, -13634, + -6764, -17568, -4534, + 902, 382, -7702, + -4497, -1374, -7109, + 5036, 9289, 6298, + 4426, 9208, -394, + -12239, -17581, -2460, + -18917, 5765, 7129, + -16378, -4102, 7845, + -2280, -12287, -3501, + -12167, 5531, -3406, + -17908, 15766, 4905, + -15097, 15808, 8499, + 9825, 797, 12888, + -18283, 10280, 791, + 11007, 9190, -244, + 11474, -579, -7581, + -12864, 16571, -195, + -14640, 16042, -1221, + -18332, 937, 2637, + -15354, 21703, -13553, + -7377, 15262, 2845, + -8799, 16541, 2581, + -19797, 6164, 4316, + -16158, 9222, 14006, + -14416, 12151, 13174, + -14141, 23469, -15211, + -11722, 23695, -17151, + -1964, -15383, 13753, + -7229, -8215, -4409, + -8680, -4539, -6585, + -6087, -5457, -5940, + -13259, 12437, -2907, + -10978, 15370, -1647, + -7396, -13948, 4297, + -7434, -11583, 4207, + -13163, 4985, 13937, + 1898, -3045, -11656, + 5657, -4100, -11268, + -3407, 15167, -771, + -17300, -2898, -778, + -10938, 12846, -2752, + -11874, 6626, 14013, + -10253, 21665, -18053, + 406, 21723, -6583, + -4073, 17099, -3927, + 19181, 786, 295, + 20580, -11662, -2457, + 22102, -13351, 369, + -12808, 13196, -5196, + 6063, 4006, 11507, + -7276, 12853, 114, + -12565, -2250, 11523, + -14217, 2765, 9542, + 5881, -11781, 14728, + 17536, -16747, 2519, + 18781, -2145, -2653, + -10697, -17317, 5038, + -11142, 4154, 9941, + 9481, -2448, 14961, + 8235, -10250, 14859, + -18197, 11232, 7365, + -19184, 6914, 11000, + -10944, 15814, -10459, + 3849, -13857, 12844, + -7194, 12657, 2472, + 11483, -11451, 13236, + 9840, -14518, 12029, + 23883, -9568, 1171, + -10820, 18420, -17126, + -9924, -1255, 12014, + 12198, 1609, 11989, + -10748, 15532, 9298, + -9741, -5600, 11373, + -13269, -6464, 9384, + 19845, -7054, 398, + 11278, -5139, 14319, + -6202, 16623, 1654, + -9097, -11092, 997, + 19002, -13005, 8517, + 4327, 20154, -7214, + 16851, 4385, 6126, + 17234, -11990, 8371, + -9161, 14173, -1062, + 12680, -17098, -1248, + -9040, -10189, -1993, + -13250, 21697, -15633, + -12137, 16307, -15587, + -7565, 5595, 8715, + -14057, 14807, -8716, + 5154, 1528, 11805, + -11515, -15866, -2486, + -6006, -6786, 10928, + -6082, -14740, -3845, + -8903, -15390, 12402, + -6839, -1601, 12095, + 21051, -9013, 7491, + 19199, -7770, 6398, + -4794, -14773, -7000, + -6012, 2917, 10643, + 5839, -6447, 15623, + 12814, 6195, 8839, + -13806, -17119, 795, + 13926, -16890, 6446, + -6942, -12919, 9554, + -3844, -13534, 10374, + -4457, 1559, 10787, + -18851, 14741, 3666, + -5487, 7874, 6174, + 3966, 23210, -9228, + 15092, -1954, -6602, + 16589, -4402, -5654, + -4916, -3395, 11258, + -3318, -1827, 11642, + -3552, 20709, -364, + -2107, -3341, 11219, + 3977, 6907, 9497, + -602, 2200, 11199, + -931, -11679, 9396, + -655, 23590, -2817, + 4846, -113, 14813, + -218, -281, 12015, + 22675, -7139, 849, + 10125, 7755, 8482, + 42, -14047, 14169, + 6394, 4546, -6382, + 1421, -12865, 12448, + 1947, 25502, -6345, + -7830, -12984, 6944, + -3380, -3774, -6830, + 21840, -10979, 7400, + -2985, -17516, -9333, + 308, -13721, -8558, + 579, -4080, 15241, + 1959, -8651, 14162, + -15752, 11553, -1543, + 16322, -1004, 10720, + 1282, 17758, -1900, + -4127, -17024, 11409, + 14526, -13115, -5876, + 12320, -14519, -6873, + 17448, -8568, -4231, + 17020, -11895, -2736, + 12869, -8659, 12529, + -467, -16936, 14591, + 16302, 2969, -3536, + -9403, -13394, 1276, + 22627, -13027, 5091, + -10928, 11196, 9917, + -10058, 7526, 11324, + 16770, -9731, 9964, + 18984, -721, 6457, + -1915, 3997, -5987, + 15735, -5467, 11114, + 848, 2358, -7139, + 3381, 21754, -6535, + 15097, -13331, 9377, + 11684, -16912, 11440, + 3494, 18714, -4534, + -8499, 12161, 7570, + -10517, -17147, 11966, + 17230, -15895, -966, + -1186, -13034, -7256, + -14490, -1096, -4663, + 10883, -17063, -6685, + -2943, -17367, 9110, + -17066, 3515, 5290, + -17896, 13182, 9407, + -18402, 4534, 10173, + 8336, 933, -6933, + -17574, 6887, -1239, + 5278, -17480, -7386, + -12685, -14815, 928, + -11989, -16758, 7220, + -94, -1429, -10547, + -15531, 4607, 12668, + 12598, -10979, -8259, + -5175, -17496, -8370, + 9509, -17363, -4797, + 7835, -733, -9933, + 20083, -6435, 3491, + -12583, 16811, 5585, + -10347, 16387, 415, + -5156, 9190, 3024, + -1162, -5218, -11018, + 15898, 5878, 927, + -17104, 635, 7228, + -15184, 18465, -8552, + 13851, 7498, 5806, + 12339, 1989, -6235, + -8783, -13504, -2022, + -7103, 9054, -1646, + -8929, -480, -6679, + -17555, 5593, 12647, + -8668, 3516, -5825, + -362, -10012, -9571, + 11355, 6507, -4159, + -13316, 15141, -12590, + 12642, -17080, 1813, + -14678, 14770, -2184, + -15414, -5951, -2011, + -12268, -6067, -4971, + -16604, -530, -2303, + -2216, 7063, -2848, + 4859, 7950, -3136, + 10975, -17013, 371, + -5388, 7670, -3722, + -4905, -17613, 6862, + 8484, -12836, -9001, + 9315, -6801, -9938, + 7941, -14924, -7560, + 8259, -9562, -9772, + 2547, -9033, -11113, + 4684, -13353, -9431, + -7524, -9388, 8770, + 10549, -17413, 5320, + 9878, -16766, 9250, + 2213, -17293, 11551, + 13056, -16797, 3233, + 11736, -17220, 5482, + -544, -17336, 11491, + -13043, -9710, 3903, + -8320, -17724, 3639, + -6497, -17425, 1846, + -3296, -17519, -6480, +}; +uint8_t bunny_lines[] = { + 1, 217, + 19, 3, + 19, 2, + 46, 18, + 39, 51, + 194, 224, + 62, 63, + 71, 195, + 0, 214, + 180, 235, + 234, 118, + 234, 70, + 71, 156, + 71, 90, + 135, 115, + 88, 110, + 4, 119, + 86, 116, + 139, 43, + 2, 198, + 0, 150, + 131, 132, + 148, 115, + 171, 165, + 171, 82, + 163, 6, + 168, 81, + 168, 158, + 80, 54, + 69, 79, + 116, 203, + 116, 138, + 27, 182, + 69, 82, + 77, 39, + 209, 25, + 209, 207, + 212, 101, + 140, 169, + 204, 184, + 194, 189, + 194, 53, + 230, 234, + 8, 23, + 98, 206, + 20, 5, + 199, 52, + 155, 223, + 51, 57, + 226, 228, + 230, 172, + 230, 231, + 65, 138, + 230, 118, + 69, 81, + 79, 82, + 21, 72, + 3, 131, + 237, 238, + 123, 174, + 105, 116, + 107, 119, + 54, 60, + 121, 127, + 84, 15, + 139, 141, + 28, 74, + 142, 134, + 54, 152, + 111, 11, + 71, 160, + 57, 164, + 111, 10, + 109, 175, + 109, 153, + 144, 24, + 96, 151, + 64, 104, + 170, 95, + 84, 58, + 21, 237, + 159, 43, + 159, 153, + 179, 155, + 7, 23, + 38, 181, + 184, 194, + 184, 17, + 196, 32, + 175, 188, + 31, 204, + 203, 30, + 139, 133, + 44, 205, + 77, 208, + 39, 182, + 31, 184, + 211, 204, + 64, 212, + 122, 214, + 216, 45, + 204, 160, + 180, 153, + 61, 195, + 213, 233, + 213, 234, + 213, 70, + 135, 74, + 156, 90, + 7, 236, + 56, 98, + 105, 65, + 5, 19, + 10, 11, + 197, 98, + 25, 221, + 15, 14, + 62, 157, + 53, 227, + 65, 132, + 123, 103, + 107, 127, + 107, 128, + 65, 131, + 121, 132, + 29, 38, + 29, 185, + 76, 182, + 80, 174, + 170, 124, + 152, 173, + 80, 36, + 151, 91, + 79, 172, + 151, 168, + 153, 43, + 99, 213, + 113, 16, + 144, 18, + 37, 197, + 148, 149, + 109, 119, + 172, 231, + 8, 9, + 67, 85, + 183, 208, + 68, 183, + 115, 91, + 146, 225, + 41, 15, + 214, 150, + 13, 14, + 191, 223, + 191, 226, + 8, 55, + 40, 57, + 224, 53, + 226, 227, + 186, 225, + 112, 10, + 146, 147, + 136, 135, + 57, 163, + 21, 117, + 79, 139, + 216, 100, + 189, 144, + 130, 148, + 77, 183, + 38, 185, + 180, 175, + 146, 192, + 73, 114, + 142, 170, + 142, 124, + 183, 190, + 6, 174, + 54, 36, + 68, 201, + 35, 101, + 199, 144, + 47, 214, + 2, 218, + 195, 137, + 112, 195, + 46, 24, + 160, 30, + 150, 29, + 120, 139, + 43, 120, + 99, 70, + 127, 128, + 92, 134, + 81, 158, + 129, 92, + 19, 20, + 224, 227, + 228, 227, + 59, 124, + 162, 118, + 19, 138, + 159, 235, + 157, 90, + 41, 13, + 195, 90, + 68, 190, + 99, 62, + 59, 95, + 76, 0, + 238, 239, + 80, 206, + 87, 27, + 87, 196, + 135, 105, + 200, 96, + 203, 138, + 178, 209, + 178, 25, + 205, 106, + 210, 144, + 30, 211, + 16, 21, + 31, 125, + 153, 175, + 123, 198, + 63, 162, + 68, 22, + 124, 95, + 221, 2, + 102, 64, + 5, 203, + 122, 0, + 128, 132, + 128, 136, + 54, 173, + 91, 75, + 192, 147, + 148, 135, + 148, 136, + 154, 155, + 91, 158, + 218, 20, + 13, 35, + 27, 40, + 155, 191, + 225, 223, + 59, 60, + 102, 78, + 40, 87, + 236, 89, + 117, 106, + 179, 225, + 32, 197, + 181, 201, + 99, 176, + 197, 12, + 31, 194, + 120, 153, + 192, 113, + 204, 140, + 122, 76, + 122, 182, + 5, 138, + 115, 74, + 132, 105, + 111, 145, + 149, 115, + 161, 93, + 87, 163, + 24, 113, + 103, 114, + 69, 115, + 110, 121, + 139, 159, + 11, 137, + 67, 68, + 72, 49, + 215, 236, + 23, 236, + 72, 237, + 101, 102, + 21, 106, + 100, 215, + 213, 220, + 0, 38, + 185, 25, + 34, 9, + 39, 40, + 49, 50, + 155, 225, + 34, 217, + 223, 228, + 117, 72, + 101, 58, + 211, 140, + 90, 137, + 24, 177, + 211, 219, + 16, 192, + 233, 234, + 12, 142, + 27, 196, + 206, 198, + 32, 48, + 101, 42, + 187, 205, + 59, 12, + 1, 181, + 206, 2, + 49, 161, + 187, 161, + 72, 188, + 166, 151, + 176, 62, + 176, 63, + 72, 143, + 2, 123, + 136, 105, + 193, 239, + 173, 60, + 70, 162, + 114, 110, + 28, 105, + 83, 137, + 231, 232, + 25, 47, + 215, 55, + 161, 205, + 106, 187, + 215, 7, + 167, 209, + 44, 45, + 44, 46, + 45, 18, + 13, 48, + 52, 53, + 199, 210, + 140, 167, + 147, 113, + 104, 214, + 35, 33, + 61, 200, + 98, 60, + 91, 74, + 40, 182, + 155, 157, + 172, 118, + 193, 186, + 166, 165, + 91, 168, + 3, 121, + 100, 236, + 166, 112, + 193, 146, + 155, 99, + 57, 73, + 66, 60, + 156, 126, + 1, 185, + 71, 125, + 49, 187, + 137, 62, + 137, 10, + 6, 196, + 203, 200, + 28, 135, + 93, 49, + 102, 58, + 104, 212, + 217, 25, + 6, 37, + 6, 87, + 39, 57, + 64, 78, + 64, 56, + 68, 73, + 94, 111, + 111, 112, + 69, 149, + 107, 120, + 107, 130, + 136, 132, + 223, 226, + 94, 145, + 141, 149, + 145, 162, + 68, 51, + 164, 73, + 92, 170, + 164, 103, + 92, 142, + 83, 162, + 106, 16, + 99, 179, + 70, 176, + 54, 92, + 22, 181, + 22, 183, + 171, 118, + 52, 194, + 197, 196, + 197, 48, + 207, 178, + 23, 201, + 185, 150, + 101, 214, + 160, 211, + 52, 189, + 216, 178, + 207, 45, + 14, 48, + 13, 32, + 36, 92, + 36, 37, + 45, 46, + 191, 224, + 23, 67, + 67, 73, + 61, 166, + 88, 67, + 88, 89, + 23, 89, + 37, 129, + 16, 238, + 175, 72, + 73, 103, + 106, 108, + 86, 28, + 3, 123, + 172, 232, + 110, 127, + 114, 123, + 60, 95, + 114, 3, + 61, 96, + 75, 158, + 195, 166, + 79, 159, + 125, 160, + 219, 30, + 163, 164, + 193, 179, + 172, 171, + 118, 94, + 125, 204, + 102, 212, + 101, 202, + 181, 68, + 212, 214, + 62, 90, + 93, 100, + 10, 195, + 9, 201, + 228, 147, + 228, 210, + 227, 199, + 227, 210, + 50, 93, + 236, 50, + 236, 229, + 83, 11, + 85, 110, + 206, 221, + 180, 222, + 167, 207, + 29, 0, + 78, 56, + 69, 141, + 3, 132, + 128, 130, + 12, 129, + 129, 134, + 12, 134, + 133, 149, + 9, 23, + 78, 14, + 17, 189, + 156, 191, + 173, 95, + 239, 146, + 17, 140, + 202, 214, + 215, 178, + 215, 217, + 186, 146, + 214, 26, + 189, 199, + 205, 100, + 193, 220, + 62, 83, + 22, 76, + 76, 77, + 232, 159, + 232, 235, + 220, 179, + 93, 205, + 217, 55, + 17, 18, + 17, 144, + 9, 217, + 34, 201, + 34, 181, + 19, 131, + 70, 63, + 37, 12, + 86, 151, + 156, 157, + 145, 11, + 97, 118, + 119, 120, + 37, 92, + 120, 133, + 22, 77, + 130, 133, + 13, 33, + 113, 177, + 200, 160, + 126, 191, + 108, 205, + 13, 15, + 50, 229, + 99, 157, + 33, 202, + 142, 59, + 33, 214, + 194, 125, + 51, 73, + 81, 165, + 81, 171, + 187, 72, + 164, 123, + 97, 171, + 143, 188, + 11, 162, + 151, 165, + 36, 174, + 56, 14, + 114, 121, + 131, 138, + 6, 36, + 75, 81, + 116, 151, + 2, 3, + 66, 206, + 33, 26, + 33, 196, + 221, 209, + 196, 26, + 172, 159, + 185, 47, + 44, 216, + 44, 100, + 224, 226, + 178, 45, + 71, 61, + 70, 118, + 222, 237, + 222, 72, + 150, 47, + 91, 86, + 91, 28, + 37, 196, + 35, 202, + 96, 116, + 25, 206, + 190, 39, + 190, 208, + 187, 117, + 42, 58, + 210, 147, + 214, 64, + 1, 34, + 97, 165, + 18, 167, + 236, 93, + 102, 84, + 163, 174, + 92, 173, + 108, 16, + 18, 169, + 77, 182, + 4, 175, + 113, 108, + 119, 143, + 35, 41, + 143, 49, + 17, 194, + 192, 239, + 97, 111, + 97, 112, + 107, 110, + 86, 105, + 140, 184, + 121, 128, + 120, 130, + 143, 50, + 54, 66, + 64, 47, + 92, 152, + 40, 163, + 8, 215, + 20, 219, + 12, 98, + 66, 80, + 81, 82, + 89, 67, + 73, 85, + 66, 98, + 125, 224, + 85, 88, + 25, 56, + 73, 110, + 41, 101, + 24, 210, + 80, 198, + 107, 229, + 96, 203, + 200, 30, + 60, 12, + 149, 130, + 229, 119, + 177, 147, + 108, 24, + 78, 84, + 78, 15, + 154, 191, + 169, 17, + 71, 126, + 179, 186, + 51, 190, + 82, 172, + 170, 173, + 4, 188, + 167, 169, + 48, 56, + 4, 143, + 218, 221, + 75, 115, + 225, 228, + 30, 20, + 153, 235, + 69, 75, + 116, 65, + 80, 123, + 96, 166, + 89, 229, + 180, 72, + 7, 8, + 167, 219, + 1, 38, + 41, 42, + 125, 191, + 20, 2, + 177, 210, + 215, 216, + 167, 20, + 15, 42, + 143, 229, + 24, 44, + 24, 205, + 38, 22, + 39, 208, + 67, 201, + 18, 207, + 76, 38, + 157, 191, + 140, 219, + 157, 154, + 178, 217, + 26, 182, + 63, 83, + 162, 94, + 165, 168, + 164, 174, + 165, 112, + 15, 58, + 133, 141, + 141, 79, + 26, 122, + 160, 61, + 109, 120, + 125, 126, + 130, 136, + 225, 147, + 53, 199, + 16, 237, + 94, 97, + 16, 239, + 33, 32, + 88, 107, + 4, 109, + 88, 229, + 1, 25, + 99, 220, + 48, 98, + 167, 218, + 26, 27, + 5, 30, + 167, 221, + 144, 46, + 47, 56, + 9, 55, + 206, 56, +}; diff --git a/tools/sandbox_test/test_i2c/bunny_out.obj b/tools/sandbox_test/test_i2c/bunny_out.obj new file mode 100644 index 000000000..e0f6907d5 --- /dev/null +++ b/tools/sandbox_test/test_i2c/bunny_out.obj @@ -0,0 +1,947 @@ +# Blender v3.0.1 OBJ File: '' +# www.blender.org +mtllib bunny_out.mtl +o bunny_in +v -0.066510 0.040490 0.013845 +v -0.061102 0.012863 0.002506 +v -0.003613 0.029887 0.008373 +v -0.005212 0.022265 0.027687 +v -0.037086 -0.050182 0.037782 +v 0.032529 0.035221 0.017166 +v -0.032844 0.055743 0.030349 +v -0.058157 -0.023425 0.008593 +v -0.062206 -0.015591 0.008836 +v -0.065676 -0.005435 0.017198 +v 0.079356 -0.024382 0.018854 +v 0.085032 -0.028166 0.015824 +v -0.022154 0.072937 -0.007019 +v -0.046338 0.067219 -0.015228 +v -0.037962 0.065307 -0.024828 +v -0.038911 0.073940 -0.048696 +v -0.024158 -0.062746 -0.016193 +v 0.003223 0.001366 -0.027508 +v -0.016062 -0.004910 -0.025392 +v 0.017987 0.033175 0.022493 +v 0.015809 0.032889 -0.001409 +v -0.043712 -0.062792 -0.008789 +v -0.067562 0.020590 0.025463 +v -0.058494 -0.014652 0.028021 +v -0.008146 -0.043884 -0.012506 +v -0.043457 0.019756 -0.012165 +v -0.063958 0.056310 0.017519 +v -0.053921 0.056458 0.030356 +v 0.035090 0.002848 0.046031 +v -0.065298 0.036716 0.002826 +v 0.039313 0.032824 -0.000872 +v 0.040981 -0.002070 -0.027076 +v -0.045943 0.059184 -0.000697 +v -0.052287 0.057296 -0.004362 +v -0.065473 0.003348 0.009420 +v -0.054838 0.077514 -0.048406 +v -0.026349 0.054508 0.010164 +v -0.031425 0.059076 0.009221 +v -0.070704 0.022015 0.015416 +v -0.057710 0.032936 0.050024 +v -0.051488 0.043397 0.047050 +v -0.050505 0.083821 -0.054326 +v -0.041866 0.084627 -0.061256 +v -0.007015 -0.054942 0.049119 +v -0.025819 -0.029341 -0.015747 +v -0.031001 -0.016212 -0.023518 +v -0.021740 -0.019491 -0.021215 +v -0.047355 0.044418 -0.010384 +v -0.039208 0.054894 -0.005883 +v -0.026417 -0.049815 0.015349 +v -0.026552 -0.041368 0.015025 +v -0.047011 0.017807 0.049776 +v 0.006779 -0.010878 -0.041632 +v 0.020206 -0.014643 -0.040244 +v -0.012170 0.054170 -0.002755 +v -0.061787 -0.010350 -0.002779 +v -0.039065 0.045879 -0.009832 +v -0.042408 0.023666 0.050048 +v -0.036618 0.077377 -0.064477 +v 0.001453 0.077585 -0.023513 +v -0.014547 0.061070 -0.014028 +v 0.068507 0.002810 0.001056 +v 0.073500 -0.041653 -0.008778 +v 0.078938 -0.047684 0.001320 +v -0.045746 0.047131 -0.018560 +v 0.021654 0.014308 0.041099 +v -0.025987 0.045907 0.000408 +v -0.044876 -0.008038 0.041157 +v -0.050778 0.009878 0.034079 +v 0.021004 -0.042077 0.052603 +v 0.062632 -0.059812 0.008999 +v 0.067077 -0.007662 -0.009475 +v -0.038206 -0.061847 0.017996 +v -0.039793 0.014838 0.035507 +v 0.033863 -0.008743 0.053435 +v 0.029413 -0.036610 0.053071 +v -0.064990 0.040115 0.026306 +v -0.068516 0.024693 0.039289 +v -0.039087 0.056480 -0.037355 +v 0.013747 -0.049490 0.045873 +v -0.025696 0.045206 0.008830 +v 0.041011 -0.040898 0.047273 +v 0.035145 -0.051850 0.042963 +v 0.085298 -0.034172 0.004185 +v -0.038644 0.065787 -0.061165 +v -0.035444 -0.004484 0.042910 +v 0.043567 0.005747 0.042820 +v -0.038389 0.055472 0.033210 +v -0.034792 -0.020002 0.040621 +v -0.047391 -0.023088 0.033517 +v 0.070878 -0.025195 0.001424 +v 0.040279 -0.018354 0.051141 +v -0.022153 0.059370 0.005909 +v -0.032491 -0.039615 0.003563 +v 0.067866 -0.046449 0.030420 +v 0.015457 0.071982 -0.025766 +v 0.060185 0.015663 0.021879 +v 0.061550 -0.042822 0.029897 +v -0.032720 0.050621 -0.003795 +v 0.045288 -0.061066 -0.004460 +v -0.032289 -0.036391 -0.007118 +v -0.047324 0.077491 -0.055833 +v -0.043348 0.058240 -0.055670 +v -0.027021 0.019984 0.031126 +v -0.050204 0.052885 -0.031130 +v 0.018410 0.005460 0.042164 +v -0.041126 -0.056665 -0.008881 +v -0.021453 -0.024238 0.039030 +v -0.021724 -0.052646 -0.013735 +v -0.031799 -0.054965 0.044296 +v -0.024427 -0.005719 0.043197 +v 0.075185 -0.032191 0.026755 +v 0.068571 -0.027751 0.022850 +v -0.017122 -0.052764 -0.025001 +v -0.021473 0.010420 0.038014 +v 0.020857 -0.023025 0.055799 +v 0.045767 0.022127 0.031570 +v -0.049310 -0.061142 0.002841 +v 0.049737 -0.060324 0.023022 +v -0.024794 -0.046141 0.034123 +v -0.013731 -0.048336 0.037051 +v -0.015918 0.005568 0.038527 +v -0.067326 0.052647 0.013093 +v -0.019597 0.028124 0.022051 +v 0.014165 0.082895 -0.032958 +v 0.053903 -0.006981 -0.023582 +v 0.059248 -0.015722 -0.020193 +v -0.017560 -0.012127 0.040210 +v -0.011850 -0.006527 0.041581 +v -0.012686 0.073963 -0.001303 +v -0.007525 -0.011933 0.040069 +v 0.014204 0.024670 0.033919 +v -0.002153 0.007858 0.039997 +v -0.003328 -0.041714 0.033559 +v -0.002340 0.084250 -0.010061 +v 0.017308 -0.000406 0.052905 +v -0.000782 -0.001005 0.042913 +v 0.080983 -0.025497 0.003033 +v 0.036161 0.027697 0.030294 +v 0.000153 -0.050170 0.050606 +v 0.022838 0.016237 -0.022794 +v 0.005078 -0.045948 0.044458 +v 0.006956 0.091082 -0.022663 +v -0.027967 -0.046374 0.024800 +v -0.012074 -0.013479 -0.024394 +v 0.078003 -0.039212 0.026429 +v -0.010662 -0.062558 -0.033333 +v 0.001101 -0.049004 -0.030567 +v 0.002068 -0.014572 0.054434 +v 0.006998 -0.030898 0.050580 +v -0.056258 0.041262 -0.005511 +v 0.058294 -0.003587 0.038287 +v 0.004580 0.063422 -0.006786 +v -0.014742 -0.060803 0.040749 +v 0.051882 -0.046841 -0.020987 +v 0.044003 -0.051854 -0.024547 +v 0.062317 -0.030602 -0.015112 +v 0.060788 -0.042483 -0.009773 +v 0.045961 -0.030927 0.044748 +v -0.001669 -0.060487 0.052113 +v 0.058224 0.010604 -0.012629 +v -0.033584 -0.047838 0.004559 +v 0.080813 -0.046527 0.018183 +v -0.039032 0.039987 0.035421 +v -0.035923 0.026879 0.040445 +v 0.059895 -0.034755 0.035589 +v 0.067800 -0.002575 0.023064 +v -0.006840 0.014275 -0.021384 +v 0.056199 -0.019528 0.039696 +v 0.003032 0.008424 -0.025497 +v 0.012075 0.077693 -0.023342 +v 0.053921 -0.047613 0.033490 +v 0.041732 -0.060400 0.040858 +v 0.012479 0.066836 -0.016195 +v -0.030357 0.043434 0.027039 +v -0.037564 -0.061242 0.042738 +v 0.061537 -0.056769 -0.003453 +v -0.004239 -0.046552 -0.025916 +v -0.051750 -0.003917 -0.016655 +v 0.038868 -0.060940 -0.023876 +v -0.010514 -0.062025 0.032539 +v -0.060952 0.012554 0.018895 +v -0.063916 0.047079 0.033599 +v -0.065723 0.016193 0.036333 +v 0.029772 0.003333 -0.024764 +v -0.062765 0.024598 -0.004426 +v 0.018851 -0.062430 -0.026380 +v -0.045304 -0.052913 0.003317 +v -0.042820 -0.059850 0.025786 +v -0.000337 -0.005106 -0.037668 +v -0.055468 0.016456 0.045246 +v 0.044995 -0.039212 -0.029498 +v -0.018483 -0.062487 -0.029893 +v 0.033964 -0.062013 -0.017134 +v 0.027984 -0.002618 -0.035477 +v 0.071728 -0.022984 0.012469 +v -0.044940 0.060042 0.019949 +v -0.036954 0.058528 0.001484 +v -0.018416 0.032822 0.010801 +v -0.004150 -0.018637 -0.039351 +v 0.056782 0.020996 0.003312 +v -0.061088 0.002270 0.025815 +v -0.054229 0.065949 -0.030546 +v 0.049470 0.026782 0.020737 +v 0.044071 0.007106 -0.022271 +v -0.031371 -0.048229 -0.007222 +v -0.025371 0.032336 -0.005879 +v -0.031892 -0.001716 -0.023855 +v -0.062699 0.019978 0.045171 +v -0.030960 0.012558 -0.020807 +v -0.001296 -0.035758 -0.034185 +v 0.040557 0.023240 -0.014854 +v -0.047560 0.054076 -0.044965 +v 0.045150 -0.061002 0.006478 +v -0.052424 0.052753 -0.007803 +v -0.055050 -0.021255 -0.007185 +v -0.043816 -0.021671 -0.017756 +v -0.059303 -0.001895 -0.008227 +v -0.007915 0.025225 -0.010174 +v 0.017357 0.028393 -0.011203 +v 0.039197 -0.060764 0.001328 +v -0.019245 0.027393 -0.013295 +v -0.017518 -0.062907 0.024509 +v 0.030301 -0.045846 -0.032149 +v 0.033268 -0.024292 -0.035494 +v 0.028361 -0.053303 -0.027000 +v 0.029498 -0.034153 -0.034902 +v 0.009097 -0.032264 -0.039690 +v 0.016732 -0.047692 -0.033685 +v -0.026872 -0.033532 0.031323 +v 0.037675 -0.062190 0.019002 +v 0.035282 -0.059879 0.033037 +v 0.007906 -0.061763 0.041254 +v 0.046631 -0.059992 0.011549 +v 0.041917 -0.061500 0.019580 +v -0.001945 -0.061917 0.041042 +v -0.046585 -0.034682 0.013941 +v -0.029716 -0.063302 0.012999 +v -0.023206 -0.062235 0.006594 +v -0.011773 -0.062570 -0.023146 +l 2 218 +l 20 4 +l 20 3 +l 47 19 +l 40 52 +l 195 225 +l 63 64 +l 72 196 +l 1 215 +l 181 236 +l 235 119 +l 235 71 +l 72 157 +l 72 91 +l 136 116 +l 89 111 +l 5 120 +l 87 117 +l 140 44 +l 3 199 +l 1 151 +l 132 133 +l 149 116 +l 172 166 +l 172 83 +l 164 7 +l 169 82 +l 169 159 +l 81 55 +l 70 80 +l 117 204 +l 117 139 +l 28 183 +l 70 83 +l 78 40 +l 210 26 +l 210 208 +l 213 102 +l 141 170 +l 205 185 +l 195 190 +l 195 54 +l 231 235 +l 9 24 +l 99 207 +l 21 6 +l 200 53 +l 156 224 +l 52 58 +l 227 229 +l 231 173 +l 231 232 +l 66 139 +l 231 119 +l 70 82 +l 80 83 +l 22 73 +l 4 132 +l 238 239 +l 124 175 +l 106 117 +l 108 120 +l 55 61 +l 122 128 +l 85 16 +l 140 142 +l 29 75 +l 143 135 +l 55 153 +l 112 12 +l 72 161 +l 58 165 +l 112 11 +l 110 176 +l 110 154 +l 145 25 +l 97 152 +l 65 105 +l 171 96 +l 85 59 +l 22 238 +l 160 44 +l 160 154 +l 180 156 +l 8 24 +l 39 182 +l 185 195 +l 185 18 +l 197 33 +l 176 189 +l 32 205 +l 204 31 +l 140 134 +l 45 206 +l 78 209 +l 40 183 +l 32 185 +l 212 205 +l 65 213 +l 123 215 +l 217 46 +l 205 161 +l 181 154 +l 62 196 +l 214 234 +l 214 235 +l 214 71 +l 136 75 +l 157 91 +l 8 237 +l 57 99 +l 106 66 +l 6 20 +l 11 12 +l 198 99 +l 26 222 +l 16 15 +l 63 158 +l 54 228 +l 66 133 +l 124 104 +l 108 128 +l 108 129 +l 66 132 +l 122 133 +l 30 39 +l 30 186 +l 77 183 +l 81 175 +l 171 125 +l 153 174 +l 81 37 +l 152 92 +l 80 173 +l 152 169 +l 154 44 +l 100 214 +l 114 17 +l 145 19 +l 38 198 +l 149 150 +l 110 120 +l 173 232 +l 9 10 +l 68 86 +l 184 209 +l 69 184 +l 116 92 +l 147 226 +l 42 16 +l 215 151 +l 14 15 +l 192 224 +l 192 227 +l 9 56 +l 41 58 +l 225 54 +l 227 228 +l 187 226 +l 113 11 +l 147 148 +l 137 136 +l 58 164 +l 22 118 +l 80 140 +l 217 101 +l 190 145 +l 131 149 +l 78 184 +l 39 186 +l 181 176 +l 147 193 +l 74 115 +l 143 171 +l 143 125 +l 184 191 +l 7 175 +l 55 37 +l 69 202 +l 36 102 +l 200 145 +l 48 215 +l 3 219 +l 196 138 +l 113 196 +l 47 25 +l 161 31 +l 151 30 +l 121 140 +l 44 121 +l 100 71 +l 128 129 +l 93 135 +l 82 159 +l 130 93 +l 20 21 +l 225 228 +l 229 228 +l 60 125 +l 163 119 +l 20 139 +l 160 236 +l 158 91 +l 42 14 +l 196 91 +l 69 191 +l 100 63 +l 60 96 +l 77 1 +l 239 240 +l 81 207 +l 88 28 +l 88 197 +l 136 106 +l 201 97 +l 204 139 +l 179 210 +l 179 26 +l 206 107 +l 211 145 +l 31 212 +l 17 22 +l 32 126 +l 154 176 +l 124 199 +l 64 163 +l 69 23 +l 125 96 +l 222 3 +l 103 65 +l 6 204 +l 123 1 +l 129 133 +l 129 137 +l 55 174 +l 92 76 +l 193 148 +l 149 136 +l 149 137 +l 155 156 +l 92 159 +l 219 21 +l 14 36 +l 28 41 +l 156 192 +l 226 224 +l 60 61 +l 103 79 +l 41 88 +l 237 90 +l 118 107 +l 180 226 +l 33 198 +l 182 202 +l 100 177 +l 198 13 +l 32 195 +l 121 154 +l 193 114 +l 205 141 +l 123 77 +l 123 183 +l 6 139 +l 116 75 +l 133 106 +l 112 146 +l 150 116 +l 162 94 +l 88 164 +l 25 114 +l 104 115 +l 70 116 +l 111 122 +l 140 160 +l 12 138 +l 68 69 +l 73 50 +l 216 237 +l 24 237 +l 73 238 +l 102 103 +l 22 107 +l 101 216 +l 214 221 +l 1 39 +l 186 26 +l 35 10 +l 40 41 +l 50 51 +l 156 226 +l 35 218 +l 224 229 +l 118 73 +l 102 59 +l 212 141 +l 91 138 +l 25 178 +l 212 220 +l 17 193 +l 234 235 +l 13 143 +l 28 197 +l 207 199 +l 33 49 +l 102 43 +l 188 206 +l 60 13 +l 2 182 +l 207 3 +l 50 162 +l 188 162 +l 73 189 +l 167 152 +l 177 63 +l 177 64 +l 73 144 +l 3 124 +l 137 106 +l 194 240 +l 174 61 +l 71 163 +l 115 111 +l 29 106 +l 84 138 +l 232 233 +l 26 48 +l 216 56 +l 162 206 +l 107 188 +l 216 8 +l 168 210 +l 45 46 +l 45 47 +l 46 19 +l 14 49 +l 53 54 +l 200 211 +l 141 168 +l 148 114 +l 105 215 +l 36 34 +l 62 201 +l 99 61 +l 92 75 +l 41 183 +l 156 158 +l 173 119 +l 194 187 +l 167 166 +l 92 169 +l 4 122 +l 101 237 +l 167 113 +l 194 147 +l 156 100 +l 58 74 +l 67 61 +l 157 127 +l 2 186 +l 72 126 +l 50 188 +l 138 63 +l 138 11 +l 7 197 +l 204 201 +l 29 136 +l 94 50 +l 103 59 +l 105 213 +l 218 26 +l 7 38 +l 7 88 +l 40 58 +l 65 79 +l 65 57 +l 69 74 +l 95 112 +l 112 113 +l 70 150 +l 108 121 +l 108 131 +l 137 133 +l 224 227 +l 95 146 +l 142 150 +l 146 163 +l 69 52 +l 165 74 +l 93 171 +l 165 104 +l 93 143 +l 84 163 +l 107 17 +l 100 180 +l 71 177 +l 55 93 +l 23 182 +l 23 184 +l 172 119 +l 53 195 +l 198 197 +l 198 49 +l 208 179 +l 24 202 +l 186 151 +l 102 215 +l 161 212 +l 53 190 +l 217 179 +l 208 46 +l 15 49 +l 14 33 +l 37 93 +l 37 38 +l 46 47 +l 192 225 +l 24 68 +l 68 74 +l 62 167 +l 89 68 +l 89 90 +l 24 90 +l 38 130 +l 17 239 +l 176 73 +l 74 104 +l 107 109 +l 87 29 +l 4 124 +l 173 233 +l 111 128 +l 115 124 +l 61 96 +l 115 4 +l 62 97 +l 76 159 +l 196 167 +l 80 160 +l 126 161 +l 220 31 +l 164 165 +l 194 180 +l 173 172 +l 119 95 +l 126 205 +l 103 213 +l 102 203 +l 182 69 +l 213 215 +l 63 91 +l 94 101 +l 11 196 +l 10 202 +l 229 148 +l 229 211 +l 228 200 +l 228 211 +l 51 94 +l 237 51 +l 237 230 +l 84 12 +l 86 111 +l 207 222 +l 181 223 +l 168 208 +l 30 1 +l 79 57 +l 70 142 +l 4 133 +l 129 131 +l 13 130 +l 130 135 +l 13 135 +l 134 150 +l 10 24 +l 79 15 +l 18 190 +l 157 192 +l 174 96 +l 240 147 +l 18 141 +l 203 215 +l 216 179 +l 216 218 +l 187 147 +l 215 27 +l 190 200 +l 206 101 +l 194 221 +l 63 84 +l 23 77 +l 77 78 +l 233 160 +l 233 236 +l 221 180 +l 94 206 +l 218 56 +l 18 19 +l 18 145 +l 10 218 +l 35 202 +l 35 182 +l 20 132 +l 71 64 +l 38 13 +l 87 152 +l 157 158 +l 146 12 +l 98 119 +l 120 121 +l 38 93 +l 121 134 +l 23 78 +l 131 134 +l 14 34 +l 114 178 +l 201 161 +l 127 192 +l 109 206 +l 14 16 +l 51 230 +l 100 158 +l 34 203 +l 143 60 +l 34 215 +l 195 126 +l 52 74 +l 82 166 +l 82 172 +l 188 73 +l 165 124 +l 98 172 +l 144 189 +l 12 163 +l 152 166 +l 37 175 +l 57 15 +l 115 122 +l 132 139 +l 7 37 +l 76 82 +l 117 152 +l 3 4 +l 67 207 +l 34 27 +l 34 197 +l 222 210 +l 197 27 +l 173 160 +l 186 48 +l 45 217 +l 45 101 +l 225 227 +l 179 46 +l 72 62 +l 71 119 +l 223 238 +l 223 73 +l 151 48 +l 92 87 +l 92 29 +l 38 197 +l 36 203 +l 97 117 +l 26 207 +l 191 40 +l 191 209 +l 188 118 +l 43 59 +l 211 148 +l 215 65 +l 2 35 +l 98 166 +l 19 168 +l 237 94 +l 103 85 +l 164 175 +l 93 174 +l 109 17 +l 19 170 +l 78 183 +l 5 176 +l 114 109 +l 120 144 +l 36 42 +l 144 50 +l 18 195 +l 193 240 +l 98 112 +l 98 113 +l 108 111 +l 87 106 +l 141 185 +l 122 129 +l 121 131 +l 144 51 +l 55 67 +l 65 48 +l 93 153 +l 41 164 +l 9 216 +l 21 220 +l 13 99 +l 67 81 +l 82 83 +l 90 68 +l 74 86 +l 67 99 +l 126 225 +l 86 89 +l 26 57 +l 74 111 +l 42 102 +l 25 211 +l 81 199 +l 108 230 +l 97 204 +l 201 31 +l 61 13 +l 150 131 +l 230 120 +l 178 148 +l 109 25 +l 79 85 +l 79 16 +l 155 192 +l 170 18 +l 72 127 +l 180 187 +l 52 191 +l 83 173 +l 171 174 +l 5 189 +l 168 170 +l 49 57 +l 5 144 +l 219 222 +l 76 116 +l 226 229 +l 31 21 +l 154 236 +l 70 76 +l 117 66 +l 81 124 +l 97 167 +l 90 230 +l 181 73 +l 8 9 +l 168 220 +l 2 39 +l 42 43 +l 126 192 +l 21 3 +l 178 211 +l 216 217 +l 168 21 +l 16 43 +l 144 230 +l 25 45 +l 25 206 +l 39 23 +l 40 209 +l 68 202 +l 19 208 +l 77 39 +l 158 192 +l 141 220 +l 158 155 +l 179 218 +l 27 183 +l 64 84 +l 163 95 +l 166 169 +l 165 175 +l 166 113 +l 16 59 +l 134 142 +l 142 80 +l 27 123 +l 161 62 +l 110 121 +l 126 127 +l 131 137 +l 226 148 +l 54 200 +l 17 238 +l 95 98 +l 17 240 +l 34 33 +l 89 108 +l 5 110 +l 89 230 +l 2 26 +l 100 221 +l 49 99 +l 168 219 +l 27 28 +l 6 31 +l 168 222 +l 145 47 +l 48 57 +l 10 56 +l 207 57 diff --git a/tools/sandbox_test/test_i2c/bunny_to_array.c b/tools/sandbox_test/test_i2c/bunny_to_array.c new file mode 100644 index 000000000..f8d992c7b --- /dev/null +++ b/tools/sandbox_test/test_i2c/bunny_to_array.c @@ -0,0 +1,36 @@ +#include + +int main() +{ + FILE * f = fopen( "bunny_out.obj", "r" ); + char buffer[1024]; + float scale = 280000; + char * line; + int mode = 1; + FILE * bh = fopen( "bunny.h", "w" ); + fprintf( bh, "#include \nint16_t bunny_verts[] = {\n" ); + while( line = fgets( buffer, sizeof(buffer)-1, f ) ) + { + if( line[0] == 'v' ) + { + float x, y, z; + sscanf( line + 2, "%f %f %f", &x, &y, &z ); + fprintf( bh, "\t%d, %d, %d,\n", (int)(x*scale), (int)(y*scale), (int)(z*scale) ); + } + if( line[0] == 'l' ) + { + if( mode == 1 ) + { + mode = 2; + fprintf( bh, "};\nuint8_t bunny_lines[] = {\n" ); + } + + int x, y; + sscanf( line + 2, "%d %d", &x, &y ); + fprintf( bh, "\t%d, %d,\n", x-1, y-1 ); + } + } + fprintf( bh, "};\n" ); + +} + diff --git a/tools/sandbox_test/test_i2c/sandbox.c b/tools/sandbox_test/test_i2c/sandbox.c index 57fe50751..be39c3a9a 100644 --- a/tools/sandbox_test/test_i2c/sandbox.c +++ b/tools/sandbox_test/test_i2c/sandbox.c @@ -14,6 +14,13 @@ #include "coreutil.h" #include "hdw-btn.h" +#include "bunny.h" + +int16_t bunny_verts_out[ sizeof(bunny_verts)/3/2*3 ]; + + +int frameno; + #define LSM6DSL_ADDRESS 0x6a #define QMC6308_ADDRESS 0x2c @@ -68,9 +75,15 @@ struct LSM6DSLData // For debug + int lastreadr; + int32_t gyroaccum[3]; + uint32_t gyrocount; int16_t gyrolast[3]; int16_t accellast[3]; - float fqQuatLast[4]; // Quats are wxyz + + // Quats are wxyz. + // You can take a vector, in controller space, rotate by this vector, and you get it in world space. + float fqQuatLast[4]; } LSM6DSL; @@ -98,6 +111,17 @@ float rsqrtf ( float x ) return x; } +float mathsqrtf( float x ) +{ + // Trick to do approximate, fast square roots. + float o = x; + o = (o+x/o)/2; + o = (o+x/o)/2; + o = (o+x/o)/2; + o = (o+x/o)/2; + return o; +} + void mathEulerToQuat( float * q, const float * euler ) { float pitch = euler[0]; @@ -150,12 +174,27 @@ void mathRotateVectorByQuaternion(float * pout, const float * q, const float * p { // return v + 2.0 * cross(q.xyz, cross(q.xyz, v) + q.w * v); float iqo[3]; - mathCrossProduct( iqo, q + 1, p ); + mathCrossProduct( iqo, q + 1 /*.xyz*/, p ); + iqo[0] += q[0] * p[0]; + iqo[1] += q[0] * p[1]; + iqo[2] += q[0] * p[2]; + float ret[3]; + mathCrossProduct( ret, q + 1 /*.xyz*/, iqo ); + pout[0] = ret[0] * 2.0 + p[0]; + pout[1] = ret[1] * 2.0 + p[1]; + pout[2] = ret[2] * 2.0 + p[2]; +} + +void mathRotateVectorByInverseOfQuaternion(float * pout, const float * q, const float * p ) +{ + // return v + 2.0 * cross(q.xyz, cross(q.xyz, v) + q.w * v); + float iqo[3]; + mathCrossProduct( iqo, p, q + 1 /*.xyz*/ ); iqo[0] += q[0] * p[0]; iqo[1] += q[0] * p[1]; iqo[2] += q[0] * p[2]; float ret[3]; - mathCrossProduct( ret, q + 1, iqo ); + mathCrossProduct( ret, iqo, q + 1 /*.xyz*/ ); pout[0] = ret[0] * 2.0 + p[0]; pout[1] = ret[1] * 2.0 + p[1]; pout[2] = ret[2] * 2.0 + p[2]; @@ -231,7 +270,7 @@ static void LSM6DSLIntegrate() { struct LSM6DSLData * ld = &LSM6DSL; - int16_t data[72]; + int16_t data[6*16]; int r = GeneralI2CGet( LSM6DSL_ADDRESS, 0x20, (uint8_t*)data, 2 ); if( r < 0 ) return; if( r == 2 ) ld->temp = data[0]; @@ -248,20 +287,35 @@ static void LSM6DSLIntegrate() // [1] = +Y axis, pointing straight up out of controller, out where the USB port is. // [2] = +Z axis, pointing up from the face of the controller. + ld->lastreadr = readr; + for( samp = 0; samp < readr; samp+=12 ) { // Extract data from IMU int16_t * euler_deltas = cdata; // Euler angles, from gyro. int16_t * accel_data = cdata + 3; + // Manual cal, used only for Steps 2..8 + euler_deltas[0] -= 12; + euler_deltas[1] += 22; + euler_deltas[2] += 4; + + // We can sum rotations to understand the amount of counts in a full circle. + ld->gyroaccum[0] += euler_deltas[0]; + ld->gyroaccum[1] += euler_deltas[1]; + ld->gyroaccum[2] += euler_deltas[2]; + ld->gyrocount++; + // STEP 1: Visually inspect the gyro values. // STEP 2: Integrate the gyro values, verify they are correct. // 2000 dps full-scale // 32768 is full-scale // 208 SPS - // convert to radians. - float fScale = ( 4000.0f / 32768.0f / 208.0f * 3.14159f / 180.0f ); + // convert to radians. ( 2000.0f / 32768.0f / 208.0f * 2.0 * 3.14159f / 180.0f ); + // Measured = 560,000 counts per scale (Measured by looking at sum) + // Testing -> 3.14159 * 2.0 / 566000; + float fScale = ( 2000.0f / 32768.0f / 208.0f * 2.0 * 3.14159f / 180.0f ); // STEP 3: Integrate gyro values into a quaternion. // This step is validated by working with just one axis at a time @@ -287,13 +341,57 @@ static void LSM6DSLIntegrate() // STEP 6A: Examine vectors. Generally speaking, we want an "up" vector, not a gravity vector. // this is "up" in the controller's point of view. - int32_t raw_up[3] = { -accel_data[0], accel_data[1], -accel_data[2] }; + float accel_up[3] = { + -accel_data[0], + accel_data[1], + -accel_data[2] + }; + + float accel_inverse_mag = rsqrtf( accel_up[0] * accel_up[0] + accel_up[1] * accel_up[1] + accel_up[2] * accel_up[2] ); + accel_up[0] *= accel_inverse_mag; + accel_up[1] *= accel_inverse_mag; + accel_up[2] *= accel_inverse_mag; + //ESP_LOGI( "SB", "%ld %ld %ld", raw_up[0], raw_up[1], raw_up[2] ); + // Step 6B: Next, compute what we think "up" should be from our point of view. We will use +Y Up. + float what_we_think_is_up[3] = { 0, 1, 0 }; + mathRotateVectorByInverseOfQuaternion( what_we_think_is_up, LSM6DSL.fqQuat, what_we_think_is_up ); - // TODO which directon of frame of reference? Up relative to controller? OR controller relative to world? + // Step 6C: Next, we determine how far off we are. This will tell us our error. + float corrective_quaternion[4]; + + // TRICKY: The ouput of this is actually the axis of rotation, which is ironically + // in vector-form the same as a quaternion. So we can write directly into the quat. + mathCrossProduct( corrective_quaternion + 1, accel_up, what_we_think_is_up ); + + // Now, we apply this in step 7. + + // First, we can compute what the drift values of our axes are, to anti-drift them. + // If you do only this, you will always end up in an unstable oscillation. + // Second, we can apply a very small corrective tug. This helps prevent oscillation + // about the correct answer. This acts sort of like a P term to a PID loop. + const float corrective_force = 0.001f; + corrective_quaternion[1] *= corrective_force; + corrective_quaternion[2] *= corrective_force; + corrective_quaternion[3] *= corrective_force; + + // x^2+y^2+z^2+q^2 -> ALGEBRA! -> sqrt( 1-x^2-y^2-z^2 ) = w + corrective_quaternion[0] = mathsqrtf( 1 + - corrective_quaternion[1]*corrective_quaternion[1] + - corrective_quaternion[2]*corrective_quaternion[2] + - corrective_quaternion[3]*corrective_quaternion[3] ); +// ESP_LOGI( "x", "%f %f %f %f\n", corrective_quaternion[0], corrective_quaternion[1], corrective_quaternion[2], corrective_quaternion[3] ); + mathQuatApply( ld->fqQuat, ld->fqQuat, corrective_quaternion ); + + + // Magnitude of correction angle = inverse_sin( magntiude( axis_of_correction ) ); + // We want to significantly reduce that. To mute any effect. + + + // TODO which directon of frame of reference? Up relative to controller? OR controller relative to world? cdata += 6; } @@ -386,6 +484,7 @@ static void mainMenuCb(const char* label, bool selected, uint32_t settingVal) void sandbox_main(void) { + frameno = 0; ESP_LOGI( "sandbox", "Running from IRAM. %d", global_i ); @@ -426,6 +525,7 @@ void sandbox_exit() void sandbox_tick() { +/* for( int mode = 0; mode < 8; mode++ ) { drawWsg( &example_sprite, 50+mode*20, (global_i%20)-10, !!(mode&1), !!(mode & 2), (mode & 4)*10); @@ -433,6 +533,7 @@ void sandbox_tick() drawWsg( &example_sprite, (global_i%20)-10, 50+mode*20, !!(mode&1), !!(mode & 2), (mode & 4)*10); drawWsg( &example_sprite, (global_i%20)+270, 50+mode*20, !!(mode&1), !!(mode & 2), (mode & 4)*10); } +*/ buttonEvt_t evt = {0}; while (checkButtonQueueWrapper(&evt)) @@ -510,10 +611,63 @@ void sandbox_tick() LSM6DSL.fqQuat[0], LSM6DSL.fqQuat[1], LSM6DSL.fqQuat[2], LSM6DSL.fqQuat[3] ); */ - float plusy[3] = { 1, 0, 0 }; + float plusy[3] = { 0, 1, 0 }; + float plusy_out[3] = { 0, 1, 0 }; mathRotateVectorByQuaternion( plusy, LSM6DSL.fqQuat, plusy ); - cts += sprintf( cts, "%f %f %f %f / %f %f %f", LSM6DSL.fqQuat[0], LSM6DSL.fqQuat[1], LSM6DSL.fqQuat[2], LSM6DSL.fqQuat[3], - plusy[0], plusy[1], plusy[2] ); +// mathRotateVectorByInverseOfQuaternion( plusy_out, LSM6DSL.fqQuat, plusy_out ); + mathRotateVectorByQuaternion( plusy_out, LSM6DSL.fqQuat, plusy_out ); + + float plusx_out[3] = { 1, 0, 0 }; + mathRotateVectorByQuaternion( plusx_out, LSM6DSL.fqQuat, plusx_out ); + + float plusz_out[3] = { 0, 0, 1 }; + mathRotateVectorByQuaternion( plusz_out, LSM6DSL.fqQuat, plusz_out ); + + int i, vertices = 0; + for( i = 0; i < sizeof(bunny_verts)/2; i+= 3 ) + { + float bz = -bunny_verts[i+0]; + float by = bunny_verts[i+1]; + float bx = bunny_verts[i+2]; + + float box = bx * plusx_out[0] + by * plusx_out[1] + bz * plusx_out[2]; + float boy = bx * plusy_out[0] + by * plusy_out[1] + bz * plusy_out[2]; + float boz = bx * plusz_out[0] + by * plusz_out[1] + bz * plusz_out[2]; + + bunny_verts_out[vertices*3+0] = box/250 + 280/2; + bunny_verts_out[vertices*3+1] = -boy/250 + 240/2; // Convert from right-handed to left-handed coordinate frame. + bunny_verts_out[vertices*3+2] = boz; + + vertices++; + } +/* + int centerX = 280/2; + int centerY = 240/2; + float xcomp = -plusy_out[0]; + float ycomp = plusy_out[1]; + int v0x = xcomp * 90; + int v0y = ycomp * 90; + drawLineFast(centerX-v0x, centerY-v0y, centerX+v0x, centerY+v0y, 215 ); +*/ + +#if 1 + int lines = 0; + for( i = 0; i < sizeof(bunny_lines); i+= 2 ) + { + int v1 = bunny_lines[i]*3; + int v2 = bunny_lines[i+1]*3; + float col = bunny_verts_out[v1+2]/2000 + 8; + if( col > 5 ) col = 5; + else if( col < 0 ) continue; + drawLineFast(bunny_verts_out[v1], bunny_verts_out[v1+1],bunny_verts_out[v2], bunny_verts_out[v2+1], col); + lines++; + } +#endif + + cts += sprintf( cts, "%ld %d %f %f %f %f / %f %f %f / %f %f %f / %d %d", LSM6DSL.caltime, LSM6DSL.lastreadr, + LSM6DSL.fqQuat[0], LSM6DSL.fqQuat[1], LSM6DSL.fqQuat[2], LSM6DSL.fqQuat[3], + plusy[0], plusy[1], plusy[2], + plusy_out[0], plusy_out[1], plusy_out[2], vertices, lines ); ESP_LOGI( "I2C", "%s", ctsbuffer ); } @@ -523,8 +677,6 @@ void sandboxBackgroundDrawCallback(int16_t x, int16_t y, int16_t w, int16_t h, i int i; fillDisplayArea(x, y, x+w, y+h, 0 ); - for( i = 0; i < 16; i++ ) - fillDisplayArea(i*16+8, y, i*16+16+8, y+16, up*16+i ); } From ac109f80a789ee2cc2c954be2e223be6fd32edf1 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Mon, 28 Aug 2023 04:56:14 -0400 Subject: [PATCH 05/18] Tweak output --- tools/sandbox_test/test_i2c/sandbox.c | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/tools/sandbox_test/test_i2c/sandbox.c b/tools/sandbox_test/test_i2c/sandbox.c index be39c3a9a..7132b9899 100644 --- a/tools/sandbox_test/test_i2c/sandbox.c +++ b/tools/sandbox_test/test_i2c/sandbox.c @@ -664,10 +664,9 @@ void sandbox_tick() } #endif - cts += sprintf( cts, "%ld %d %f %f %f %f / %f %f %f / %f %f %f / %d %d", LSM6DSL.caltime, LSM6DSL.lastreadr, + cts += sprintf( cts, "%ld %d %f %f %f %f / %f %f %f / %d %d", LSM6DSL.caltime, LSM6DSL.lastreadr, LSM6DSL.fqQuat[0], LSM6DSL.fqQuat[1], LSM6DSL.fqQuat[2], LSM6DSL.fqQuat[3], - plusy[0], plusy[1], plusy[2], - plusy_out[0], plusy_out[1], plusy_out[2], vertices, lines ); + plusx_out[0], plusx_out[1], plusx_out[2], vertices, lines ); ESP_LOGI( "I2C", "%s", ctsbuffer ); } From 487247867f8dbdfac5744568ebc5ce957e5047c3 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Wed, 30 Aug 2023 05:36:39 -0400 Subject: [PATCH 06/18] Still working on the controls. --- tools/sandbox_test/test_i2c/sandbox.c | 57 ++++++++++++++++++--------- 1 file changed, 39 insertions(+), 18 deletions(-) diff --git a/tools/sandbox_test/test_i2c/sandbox.c b/tools/sandbox_test/test_i2c/sandbox.c index 7132b9899..9167e8762 100644 --- a/tools/sandbox_test/test_i2c/sandbox.c +++ b/tools/sandbox_test/test_i2c/sandbox.c @@ -71,8 +71,13 @@ struct LSM6DSLData int32_t temp; uint32_t caltime; + // Quats are wxyz. + // You can take a vector, in controller space, rotate by this quat, and you get it in world space. + float fqQuatLast[4]; float fqQuat[4]; // Quats are wxyz + // Bias for all of the euler angles. + float fvBias[3]; // For debug int lastreadr; @@ -80,10 +85,7 @@ struct LSM6DSLData uint32_t gyrocount; int16_t gyrolast[3]; int16_t accellast[3]; - - // Quats are wxyz. - // You can take a vector, in controller space, rotate by this vector, and you get it in world space. - float fqQuatLast[4]; + float fCorrectLast[3]; } LSM6DSL; @@ -114,12 +116,17 @@ float rsqrtf ( float x ) float mathsqrtf( float x ) { // Trick to do approximate, fast square roots. + int sign = x < 0; + if( sign ) x = -x; float o = x; o = (o+x/o)/2; o = (o+x/o)/2; o = (o+x/o)/2; o = (o+x/o)/2; - return o; + if( sign ) + return -o; + else + return o; } void mathEulerToQuat( float * q, const float * euler ) @@ -296,9 +303,9 @@ static void LSM6DSLIntegrate() int16_t * accel_data = cdata + 3; // Manual cal, used only for Steps 2..8 - euler_deltas[0] -= 12; - euler_deltas[1] += 22; - euler_deltas[2] += 4; + // euler_deltas[0] -= 12; + // euler_deltas[1] += 22; + // euler_deltas[2] += 4; // We can sum rotations to understand the amount of counts in a full circle. ld->gyroaccum[0] += euler_deltas[0]; @@ -315,16 +322,22 @@ static void LSM6DSLIntegrate() // convert to radians. ( 2000.0f / 32768.0f / 208.0f * 2.0 * 3.14159f / 180.0f ); // Measured = 560,000 counts per scale (Measured by looking at sum) // Testing -> 3.14159 * 2.0 / 566000; - float fScale = ( 2000.0f / 32768.0f / 208.0f * 2.0 * 3.14159f / 180.0f ); + float fFudge = 1.1; + float fScale = ( 2000.0f / 32768.0f / 208.0f * 2.0 * 3.14159f / 180.0f ) * fFudge; // STEP 3: Integrate gyro values into a quaternion. // This step is validated by working with just one axis at a time // then apply a coordinate frame to ld->fqQuat and validate that it is // correct. + float fEulerScales[3] = { + -fScale, + fScale, + -fScale }; + float fEulers[3] = { - -euler_deltas[0] * fScale, - euler_deltas[1] * fScale, - -euler_deltas[2] * fScale }; + euler_deltas[0] * fEulerScales[0] + ld->fvBias[0], + euler_deltas[1] * fEulerScales[1] + ld->fvBias[1], + euler_deltas[2] * fEulerScales[2] + ld->fvBias[2] }; mathEulerToQuat( ld->fqQuatLast, fEulers ); mathQuatApply( ld->fqQuat, ld->fqQuat, ld->fqQuatLast ); @@ -369,11 +382,17 @@ static void LSM6DSLIntegrate() // First, we can compute what the drift values of our axes are, to anti-drift them. // If you do only this, you will always end up in an unstable oscillation. - + memcpy( ld->fCorrectLast, corrective_quaternion+1, 12 ); + // XXX TODO: We need to multiply by amount the accelerometer gives us assurance. + ld->fvBias[0] += mathsqrtf(corrective_quaternion[1]) * 0.0000002; + ld->fvBias[1] += mathsqrtf(corrective_quaternion[2]) * 0.0000002; + ld->fvBias[2] += mathsqrtf(corrective_quaternion[3]) * 0.0000002; + // Second, we can apply a very small corrective tug. This helps prevent oscillation // about the correct answer. This acts sort of like a P term to a PID loop. - const float corrective_force = 0.001f; + // This is actually the **primary**, or fastest responding thing. + const float corrective_force = 0.005f; corrective_quaternion[1] *= corrective_force; corrective_quaternion[2] *= corrective_force; corrective_quaternion[3] *= corrective_force; @@ -386,7 +405,6 @@ static void LSM6DSLIntegrate() // ESP_LOGI( "x", "%f %f %f %f\n", corrective_quaternion[0], corrective_quaternion[1], corrective_quaternion[2], corrective_quaternion[3] ); mathQuatApply( ld->fqQuat, ld->fqQuat, corrective_quaternion ); - // Magnitude of correction angle = inverse_sin( magntiude( axis_of_correction ) ); // We want to significantly reduce that. To mute any effect. @@ -664,9 +682,12 @@ void sandbox_tick() } #endif - cts += sprintf( cts, "%ld %d %f %f %f %f / %f %f %f / %d %d", LSM6DSL.caltime, LSM6DSL.lastreadr, - LSM6DSL.fqQuat[0], LSM6DSL.fqQuat[1], LSM6DSL.fqQuat[2], LSM6DSL.fqQuat[3], - plusx_out[0], plusx_out[1], plusx_out[2], vertices, lines ); + cts += sprintf( cts, "%ld %d %f %f %f / %f %f %f / %3d %3d %3d / %4d %4d %4d / %d %d", LSM6DSL.caltime, LSM6DSL.lastreadr, + LSM6DSL.fCorrectLast[0], LSM6DSL.fCorrectLast[1], LSM6DSL.fCorrectLast[2], + LSM6DSL.fvBias[0], LSM6DSL.fvBias[1], LSM6DSL.fvBias[2], + LSM6DSL.gyrolast[0], LSM6DSL.gyrolast[1], LSM6DSL.gyrolast[2], + LSM6DSL.accellast[0], LSM6DSL.accellast[1], LSM6DSL.accellast[2], + vertices, lines ); ESP_LOGI( "I2C", "%s", ctsbuffer ); } From e988066d866ac53148a2c58fc788da6cac2a06de Mon Sep 17 00:00:00 2001 From: cnlohr Date: Wed, 6 Sep 2023 02:48:50 -0400 Subject: [PATCH 07/18] Manual bus reset. --- tools/sandbox_test/test_i2c/sandbox.c | 36 +++++++++++++++++++++++++-- 1 file changed, 34 insertions(+), 2 deletions(-) diff --git a/tools/sandbox_test/test_i2c/sandbox.c b/tools/sandbox_test/test_i2c/sandbox.c index 9167e8762..7dc3d1741 100644 --- a/tools/sandbox_test/test_i2c/sandbox.c +++ b/tools/sandbox_test/test_i2c/sandbox.c @@ -11,6 +11,10 @@ #include "mainMenu.h" #include "soc/rtc_cntl_reg.h" #include "soc/gpio_reg.h" +#include "soc/io_mux_reg.h" +#include "rom/gpio.h" +#include "soc/i2c_reg.h" +#include "soc/gpio_struct.h" #include "coreutil.h" #include "hdw-btn.h" @@ -19,6 +23,7 @@ int16_t bunny_verts_out[ sizeof(bunny_verts)/3/2*3 ]; +extern uint32_t frameRateUs; int frameno; #define LSM6DSL_ADDRESS 0x6a @@ -118,6 +123,7 @@ float mathsqrtf( float x ) // Trick to do approximate, fast square roots. int sign = x < 0; if( sign ) x = -x; + if( x < 0.0000001 ) return 0.0001; float o = x; o = (o+x/o)/2; o = (o+x/o)/2; @@ -257,6 +263,8 @@ static int ReadLSM6DSL( uint8_t * data, int data_len ) i2c_cmd_link_delete(cmdHandle); if( err < 0 ) return err; + if( fifolen == 0 ) return 0; + fifolen &= 0x3ff; if( fifolen > data_len / 2 ) fifolen = data_len / 2; @@ -392,7 +400,7 @@ static void LSM6DSLIntegrate() // Second, we can apply a very small corrective tug. This helps prevent oscillation // about the correct answer. This acts sort of like a P term to a PID loop. // This is actually the **primary**, or fastest responding thing. - const float corrective_force = 0.005f; + const float corrective_force = 0.001f; corrective_quaternion[1] *= corrective_force; corrective_quaternion[2] *= corrective_force; corrective_quaternion[3] *= corrective_force; @@ -448,8 +456,28 @@ static void LSM6DSLIntegrate() static void LMS6DS3Setup() { + + // Shake any device off the bus. + int i; + int gpio_scl = 41; + for( i = 0; i < 16; i++ ) + { + gpio_matrix_out( gpio_scl, 256, 1, 0 ); + GPIO.out1_w1tc.val = (1<<(gpio_scl-32)); + esp_rom_delay_us(10); + gpio_matrix_out( gpio_scl, 256, 1, 0 ); + GPIO.out1_w1ts.val = (1<<(gpio_scl-32)); + esp_rom_delay_us(10); + } + gpio_matrix_out( gpio_scl, 29, 0, 0 ); + WRITE_PERI_REG( I2C_SCL_LOW_PERIOD_REG( 0 ), 3 ); + WRITE_PERI_REG( I2C_SDA_HOLD_REG( 0 ), 3 ); + WRITE_PERI_REG( I2C_SDA_SAMPLE_REG( 0 ), 3 ); + WRITE_PERI_REG( I2C_SCL_HIGH_PERIOD_REG( 0 ), 3 ); + memset( &LSM6DSL, 0, sizeof(LSM6DSL) ); LSM6DSL.fqQuat[0] = 1; + LSM6DSL.fqQuatLast[0] = 1; // Enable access LSM6DSLSet( LSM6DSL_FUNC_CFG_ACCESS, 0x20 ); @@ -513,6 +541,8 @@ void sandbox_main(void) addSingleItemToMenu(menu, menu_Bootload); loadWsg("kid0.wsg", &example_sprite, true); + + frameRateUs = 0; /* // Try to reinstall, just in case. i2c_config_t conf = { @@ -633,6 +663,7 @@ void sandbox_tick() float plusy_out[3] = { 0, 1, 0 }; mathRotateVectorByQuaternion( plusy, LSM6DSL.fqQuat, plusy ); // mathRotateVectorByInverseOfQuaternion( plusy_out, LSM6DSL.fqQuat, plusy_out ); + mathRotateVectorByQuaternion( plusy_out, LSM6DSL.fqQuat, plusy_out ); float plusx_out[3] = { 1, 0, 0 }; @@ -682,7 +713,8 @@ void sandbox_tick() } #endif - cts += sprintf( cts, "%ld %d %f %f %f / %f %f %f / %3d %3d %3d / %4d %4d %4d / %d %d", LSM6DSL.caltime, LSM6DSL.lastreadr, + cts += sprintf( cts, "%ld %d %f %f %f / %f %f %f / %3d %3d %3d / %4d %4d %4d / %d %d", + LSM6DSL.caltime, LSM6DSL.lastreadr, LSM6DSL.fCorrectLast[0], LSM6DSL.fCorrectLast[1], LSM6DSL.fCorrectLast[2], LSM6DSL.fvBias[0], LSM6DSL.fvBias[1], LSM6DSL.fvBias[2], LSM6DSL.gyrolast[0], LSM6DSL.gyrolast[1], LSM6DSL.gyrolast[2], From a3458cf55b2d826f147303893984d2fe04140c70 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sun, 24 Sep 2023 19:20:16 -0400 Subject: [PATCH 08/18] bump update --- tools/sandbox_test/test_i2c/sandbox.c | 28 ++++++++++++++++++++------- 1 file changed, 21 insertions(+), 7 deletions(-) diff --git a/tools/sandbox_test/test_i2c/sandbox.c b/tools/sandbox_test/test_i2c/sandbox.c index 7dc3d1741..ab821fb24 100644 --- a/tools/sandbox_test/test_i2c/sandbox.c +++ b/tools/sandbox_test/test_i2c/sandbox.c @@ -25,6 +25,7 @@ int16_t bunny_verts_out[ sizeof(bunny_verts)/3/2*3 ]; extern uint32_t frameRateUs; int frameno; +int bQuit; #define LSM6DSL_ADDRESS 0x6a #define QMC6308_ADDRESS 0x2c @@ -470,10 +471,20 @@ static void LMS6DS3Setup() esp_rom_delay_us(10); } gpio_matrix_out( gpio_scl, 29, 0, 0 ); - WRITE_PERI_REG( I2C_SCL_LOW_PERIOD_REG( 0 ), 3 ); - WRITE_PERI_REG( I2C_SDA_HOLD_REG( 0 ), 3 ); - WRITE_PERI_REG( I2C_SDA_SAMPLE_REG( 0 ), 3 ); - WRITE_PERI_REG( I2C_SCL_HIGH_PERIOD_REG( 0 ), 3 ); + + esp_err_t ret_val = ESP_OK; + + /* Install i2c driver */ + i2c_config_t conf = { + .mode = I2C_MODE_MASTER, + .sda_io_num = GPIO_NUM_3, + .sda_pullup_en = GPIO_PULLUP_ENABLE, + .scl_io_num = GPIO_NUM_41, + .scl_pullup_en = GPIO_PULLUP_ENABLE, + .master.clk_speed = 800000, + .clk_flags = I2C_SCLK_SRC_FLAG_FOR_NOMAL, + }; + ret_val |= i2c_param_config(I2C_NUM_0, &conf); memset( &LSM6DSL, 0, sizeof(LSM6DSL) ); LSM6DSL.fqQuat[0] = 1; @@ -531,6 +542,7 @@ static void mainMenuCb(const char* label, bool selected, uint32_t settingVal) void sandbox_main(void) { frameno = 0; + bQuit = 0; ESP_LOGI( "sandbox", "Running from IRAM. %d", global_i ); @@ -560,19 +572,21 @@ void sandbox_main(void) ESP_LOGI( "sandbox", "i2c_driver_install=%d", i2c_driver_install(I2C_NUM_0, conf.mode, 0, 0, 0) ); */ LMS6DS3Setup(); - GeneralSet( QMC6308_ADDRESS, 0x0b, 0x80 ); - GeneralSet( QMC6308_ADDRESS, 0x0b, 0x03 ); - GeneralSet( QMC6308_ADDRESS, 0x0a, 0x83 ); + //GeneralSet( QMC6308_ADDRESS, 0x0b, 0x80 ); + //GeneralSet( QMC6308_ADDRESS, 0x0b, 0x03 ); + //GeneralSet( QMC6308_ADDRESS, 0x0a, 0x83 ); ESP_LOGI( "sandbox", "Loaded" ); } void sandbox_exit() { + bQuit = 1; } void sandbox_tick() { + if( bQuit ) return; /* for( int mode = 0; mode < 8; mode++ ) { From 8bcf3de1abe53e9dc722c1429922889284d700a3 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sat, 14 Oct 2023 02:35:05 -0400 Subject: [PATCH 09/18] Make sandbox work with 5.1.1 --- tools/sandbox_test/buildhelp.c | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/tools/sandbox_test/buildhelp.c b/tools/sandbox_test/buildhelp.c index 93f061a5b..001d8a0b8 100644 --- a/tools/sandbox_test/buildhelp.c +++ b/tools/sandbox_test/buildhelp.c @@ -174,6 +174,7 @@ int main( int argc, char ** argv ) snprintf( temp, sizeof( temp ) - 1, "-I%s/components/driver/uart/include", idf_path ); appendcflag( temp ); snprintf( temp, sizeof( temp ) - 1, "-I%s/components/nvs_flash/include", idf_path ); appendcflag( temp ); snprintf( temp, sizeof( temp ) - 1, "-I%s/components/soc/esp32s2/include", idf_path ); appendcflag( temp ); + snprintf( temp, sizeof( temp ) - 1, "-I%s/components/freertos/FreeRTOS-Kernel/include/freertos", idf_path ); appendcflag( temp ); snprintf( temp, sizeof( temp ) - 1, "-I%s/components/components/hdw-battmon/include", argv[2] ); appendcflag( temp ); snprintf( temp, sizeof( temp ) - 1, "-I%s/components", argv[2] ); appendcflag( temp ); @@ -204,7 +205,8 @@ int main( int argc, char ** argv ) snprintf( temp, sizeof( temp ) - 1, "-I%s/main/modes/mainMenu", argv[2] ); appendcflag( temp ); snprintf( temp, sizeof( temp ) - 1, "-I%s/main/modes/colorchord", argv[2] ); appendcflag( temp ); snprintf( temp, sizeof( temp ) - 1, "-I%s/build/config", argv[2] ); appendcflag( temp ); - appendcflag( "-DTUP_DCD_ENDPOINT_MAX=8" ); + + appendcflag( "-DTUP_DCD_ENDPOINT_MAX=8 -DESP_PLATFORM=1" ); } int r; From 10e4d37e48419f930b9528737efadd23e69f26da Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sat, 14 Oct 2023 04:05:45 -0400 Subject: [PATCH 10/18] Get close to a prototype function/algo. --- tools/sandbox_test/test_i2c/sandbox.c | 82 +++++++++++++-------------- 1 file changed, 40 insertions(+), 42 deletions(-) diff --git a/tools/sandbox_test/test_i2c/sandbox.c b/tools/sandbox_test/test_i2c/sandbox.c index ab821fb24..d0cc2299c 100644 --- a/tools/sandbox_test/test_i2c/sandbox.c +++ b/tools/sandbox_test/test_i2c/sandbox.c @@ -22,8 +22,6 @@ int16_t bunny_verts_out[ sizeof(bunny_verts)/3/2*3 ]; - -extern uint32_t frameRateUs; int frameno; int bQuit; @@ -75,7 +73,7 @@ int bQuit; struct LSM6DSLData { int32_t temp; - uint32_t caltime; + uint32_t computetime; // Quats are wxyz. // You can take a vector, in controller space, rotate by this quat, and you get it in world space. @@ -121,7 +119,7 @@ float rsqrtf ( float x ) float mathsqrtf( float x ) { - // Trick to do approximate, fast square roots. + // Trick to do approximate, fast square roots. (Though it is surprisingly fast) int sign = x < 0; if( sign ) x = -x; if( x < 0.0000001 ) return 0.0001; @@ -201,6 +199,7 @@ void mathRotateVectorByQuaternion(float * pout, const float * q, const float * p void mathRotateVectorByInverseOfQuaternion(float * pout, const float * q, const float * p ) { + // General note: Performing a transform this way can be about 20-30% slower than a well formed 3x3 matrix. // return v + 2.0 * cross(q.xyz, cross(q.xyz, v) + q.w * v); float iqo[3]; mathCrossProduct( iqo, p, q + 1 /*.xyz*/ ); @@ -287,6 +286,8 @@ static void LSM6DSLIntegrate() struct LSM6DSLData * ld = &LSM6DSL; int16_t data[6*16]; + + // Get temperature sensor... Why? Yolo? int r = GeneralI2CGet( LSM6DSL_ADDRESS, 0x20, (uint8_t*)data, 2 ); if( r < 0 ) return; if( r == 2 ) ld->temp = data[0]; @@ -452,7 +453,7 @@ static void LSM6DSLIntegrate() ld->accellast[2] = cdata[-1]; } - ld->caltime = getCycleCount() - start; + ld->computetime = getCycleCount() - start; } static void LMS6DS3Setup() @@ -474,6 +475,9 @@ static void LMS6DS3Setup() esp_err_t ret_val = ESP_OK; + i2c_driver_delete( I2C_NUM_0 ); + + /* Install i2c driver */ i2c_config_t conf = { .mode = I2C_MODE_MASTER, @@ -481,11 +485,13 @@ static void LMS6DS3Setup() .sda_pullup_en = GPIO_PULLUP_ENABLE, .scl_io_num = GPIO_NUM_41, .scl_pullup_en = GPIO_PULLUP_ENABLE, - .master.clk_speed = 800000, + .master.clk_speed = 800000, //tested upto 1.4Mbit/s .clk_flags = I2C_SCLK_SRC_FLAG_FOR_NOMAL, }; + ESP_LOGI( "sandbox", "i2c_driver_install=%d", i2c_driver_install(I2C_NUM_0, conf.mode, 0, 0, 0) ); ret_val |= i2c_param_config(I2C_NUM_0, &conf); + memset( &LSM6DSL, 0, sizeof(LSM6DSL) ); LSM6DSL.fqQuat[0] = 1; LSM6DSL.fqQuatLast[0] = 1; @@ -496,7 +502,7 @@ static void LMS6DS3Setup() vTaskDelay( 1 ); LSM6DSLSet( LSM6DSL_CTRL3_C, 0x44 ); // unforce reset LSM6DSLSet( LSM6DSL_FIFO_CTRL5, (0b0101 << 3) | 0b110 ); // 208 Hz ODR - LSM6DSLSet( LSM6DSL_FIFO_CTRL3, 0b00001001 ); // Put both devices in FIFO. + LSM6DSLSet( LSM6DSL_FIFO_CTRL3, 0b00001001 ); // Put both devices (Accel + Gyro) in FIFO. LSM6DSLSet( LSM6DSL_CTRL1_XL, 0b01011001 ); // Setup accel (16 g's FS) LSM6DSLSet( LSM6DSL_CTRL2_G, 0b01011100 ); // Setup gyro, 2000dps LSM6DSLSet( LSM6DSL_CTRL4_C, 0x00 ); // Disable all filtering. @@ -554,7 +560,7 @@ void sandbox_main(void) loadWsg("kid0.wsg", &example_sprite, true); - frameRateUs = 0; + setFrameRateUs(0); /* // Try to reinstall, just in case. i2c_config_t conf = { @@ -567,7 +573,7 @@ void sandbox_main(void) .clk_flags = I2C_SCLK_SRC_FLAG_FOR_NOMAL, }; -// i2c_driver_delete( I2C_NUM_0 ); + i2c_driver_delete( I2C_NUM_0 ); ESP_LOGI( "sandbox", "i2c_param_config=%d", i2c_param_config(I2C_NUM_0, &conf) ); ESP_LOGI( "sandbox", "i2c_driver_install=%d", i2c_driver_install(I2C_NUM_0, conf.mode, 0, 0, 0) ); */ @@ -667,53 +673,43 @@ void sandbox_tick() LSM6DSLIntegrate(); /* cts += sprintf( cts, "%ld %ld / %5d %5d %5d / %5d %5d %5d / %ld %ld %ld / %f %f %f %f", - LSM6DSL.caltime, LSM6DSL.temp, + LSM6DSL.computetime, LSM6DSL.temp, LSM6DSL.accellast[0], LSM6DSL.accellast[1], LSM6DSL.accellast[2], LSM6DSL.gyrolast[0], LSM6DSL.gyrolast[1], LSM6DSL.gyrolast[2], LSM6DSL.fqQuat[0], LSM6DSL.fqQuat[1], LSM6DSL.fqQuat[2], LSM6DSL.fqQuat[3] ); */ float plusy[3] = { 0, 1, 0 }; + + // Produce a model matrix from a quaternion. + float plusx_out[3] = { 1, 0, 0 }; float plusy_out[3] = { 0, 1, 0 }; + float plusz_out[3] = { 0, 0, 1 }; mathRotateVectorByQuaternion( plusy, LSM6DSL.fqQuat, plusy ); -// mathRotateVectorByInverseOfQuaternion( plusy_out, LSM6DSL.fqQuat, plusy_out ); - mathRotateVectorByQuaternion( plusy_out, LSM6DSL.fqQuat, plusy_out ); - - float plusx_out[3] = { 1, 0, 0 }; mathRotateVectorByQuaternion( plusx_out, LSM6DSL.fqQuat, plusx_out ); - - float plusz_out[3] = { 0, 0, 1 }; mathRotateVectorByQuaternion( plusz_out, LSM6DSL.fqQuat, plusz_out ); + + uint32_t cycStart = getCycleCount(); + int i, vertices = 0; for( i = 0; i < sizeof(bunny_verts)/2; i+= 3 ) { - float bz = -bunny_verts[i+0]; - float by = bunny_verts[i+1]; + // Performingthe transform this way is about 700us. float bx = bunny_verts[i+2]; - - float box = bx * plusx_out[0] + by * plusx_out[1] + bz * plusx_out[2]; - float boy = bx * plusy_out[0] + by * plusy_out[1] + bz * plusy_out[2]; - float boz = bx * plusz_out[0] + by * plusz_out[1] + bz * plusz_out[2]; - - bunny_verts_out[vertices*3+0] = box/250 + 280/2; - bunny_verts_out[vertices*3+1] = -boy/250 + 240/2; // Convert from right-handed to left-handed coordinate frame. - bunny_verts_out[vertices*3+2] = boz; - + float by = bunny_verts[i+1]; + float bz =-bunny_verts[i+0]; + float bunnyvert[3] = { + bx * plusx_out[0] + by * plusx_out[1] + bz * plusx_out[2], + bx * plusy_out[0] + by * plusy_out[1] + bz * plusy_out[2], + bx * plusz_out[0] + by * plusz_out[1] + bz * plusz_out[2] }; + bunny_verts_out[vertices*3+0] = bunnyvert[0]/250 + 280/2; + bunny_verts_out[vertices*3+1] = -bunnyvert[1]/250 + 240/2; // Convert from right-handed to left-handed coordinate frame. + bunny_verts_out[vertices*3+2] = bunnyvert[2]; vertices++; } -/* - int centerX = 280/2; - int centerY = 240/2; - float xcomp = -plusy_out[0]; - float ycomp = plusy_out[1]; - int v0x = xcomp * 90; - int v0y = ycomp * 90; - drawLineFast(centerX-v0x, centerY-v0y, centerX+v0x, centerY+v0y, 215 ); -*/ -#if 1 int lines = 0; for( i = 0; i < sizeof(bunny_lines); i+= 2 ) { @@ -725,15 +721,17 @@ void sandbox_tick() drawLineFast(bunny_verts_out[v1], bunny_verts_out[v1+1],bunny_verts_out[v2], bunny_verts_out[v2+1], col); lines++; } -#endif - cts += sprintf( cts, "%ld %d %f %f %f / %f %f %f / %3d %3d %3d / %4d %4d %4d / %d %d", - LSM6DSL.caltime, LSM6DSL.lastreadr, + + uint32_t renderTime = getCycleCount() - cycStart; + + + cts += sprintf( cts, "%ld %ld %d %f %f %f / %f %f %f / %3d %3d %3d / %4d %4d %4d", + LSM6DSL.computetime, renderTime, LSM6DSL.lastreadr, LSM6DSL.fCorrectLast[0], LSM6DSL.fCorrectLast[1], LSM6DSL.fCorrectLast[2], LSM6DSL.fvBias[0], LSM6DSL.fvBias[1], LSM6DSL.fvBias[2], LSM6DSL.gyrolast[0], LSM6DSL.gyrolast[1], LSM6DSL.gyrolast[2], - LSM6DSL.accellast[0], LSM6DSL.accellast[1], LSM6DSL.accellast[2], - vertices, lines ); + LSM6DSL.accellast[0], LSM6DSL.accellast[1], LSM6DSL.accellast[2] ); ESP_LOGI( "I2C", "%s", ctsbuffer ); } From 825f28168189caef97b3136e0eefd57a34952776 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sun, 15 Oct 2023 03:28:03 -0400 Subject: [PATCH 11/18] Getting close to something working. --- components/hdw-accel-legacy/CMakeLists.txt | 3 + components/hdw-accel-legacy/hdw-accel.c | 289 +++++++ .../hdw-accel-legacy/include/hdw-accel.h | 82 ++ components/hdw-accel/hdw-accel.c | 781 ++++++++++++++---- components/hdw-accel/include/hdw-accel.h | 106 +-- main/swadge2024.c | 7 +- tools/sandbox_test/test_i2c/sandbox.c | 101 ++- 7 files changed, 1131 insertions(+), 238 deletions(-) create mode 100644 components/hdw-accel-legacy/CMakeLists.txt create mode 100644 components/hdw-accel-legacy/hdw-accel.c create mode 100644 components/hdw-accel-legacy/include/hdw-accel.h diff --git a/components/hdw-accel-legacy/CMakeLists.txt b/components/hdw-accel-legacy/CMakeLists.txt new file mode 100644 index 000000000..421570635 --- /dev/null +++ b/components/hdw-accel-legacy/CMakeLists.txt @@ -0,0 +1,3 @@ +idf_component_register(SRCS "" + INCLUDE_DIRS "" + REQUIRES driver) diff --git a/components/hdw-accel-legacy/hdw-accel.c b/components/hdw-accel-legacy/hdw-accel.c new file mode 100644 index 000000000..7ff0a607d --- /dev/null +++ b/components/hdw-accel-legacy/hdw-accel.c @@ -0,0 +1,289 @@ +//============================================================================== +// Includes +//============================================================================== + +#include + +#include "hdw-accel.h" + +//============================================================================== +// Enums +//============================================================================== + +typedef enum __attribute__((packed)) +{ + QMA7981_REG_CHIP_ID = 0x00, + QMA7981_REG_DX_L = 0x01, + QMA7981_REG_DX_H = 0x02, + QMA7981_REG_DY_L = 0x03, + QMA7981_REG_DY_H = 0x04, + QMA7981_REG_DZ_L = 0x05, + QMA7981_REG_DZ_H = 0x06, + QMA7981_REG_STEP_L = 0x07, + QMA7981_REG_STEP_H = 0x08, + QMA7981_REG_INT_STAT_0 = 0x0A, + QMA7981_REG_INT_STAT_1 = 0x0B, + QMA7981_REG_INT_STAT_4 = 0x0D, + QMA7981_REG_RANGE = 0x0F, + QMA7981_REG_BAND_WIDTH = 0x10, + QMA7981_REG_PWR_MANAGE = 0x11, + QMA7981_REG_STEP_CONF_0 = 0x12, + QMA7981_REG_STEP_CONF_1 = 0x13, + QMA7981_REG_STEP_CONF_2 = 0x14, + QMA7981_REG_STEP_CONF_3 = 0x15, + QMA7981_REG_INT_EN_0 = 0x16, + QMA7981_REG_INT_EN_1 = 0x17, + QMA7981_REG_INT_MAP_0 = 0x19, + QMA7981_REG_INT_MAP_1 = 0x1A, + QMA7981_REG_INT_MAP_2 = 0x1B, + QMA7981_REG_INT_MAP_3 = 0x1C, + QMA7981_REG_SIG_STEP_TH = 0x1D, + QMA7981_REG_STEP = 0x1F +} qmaReg_t; + +//============================================================================== +// Defines +//============================================================================== + +#define QMA7981_ADDR 0x12 + +//============================================================================== +// Variables +//============================================================================== + +static qma_range_t qma_range = QMA_RANGE_2G; +static i2c_port_t i2c_port; + +//============================================================================== +// Function Prototypes +//============================================================================== + +static esp_err_t qma7981_read_byte(qmaReg_t reg_addr, uint8_t* data); +static esp_err_t qma7981_write_byte(qmaReg_t reg_addr, uint8_t data); +static esp_err_t qma7981_read_bytes(qmaReg_t reg_addr, size_t data_len, uint8_t* data); +static int16_t signExtend10bit(uint16_t in); + +//============================================================================== +// Functions +//============================================================================== + +/** + * @brief Initialize the accelerometer + * + * @param _i2c_port The i2c port to use for the accelerometer + * @param sda The GPIO for the Serial DAta line + * @param scl The GPIO for the Serial CLock line + * @param pullup Either \c GPIO_PULLUP_DISABLE if there are external pullup resistors on SDA and SCL or \c + * GPIO_PULLUP_ENABLE if internal pull-ups should be used + * @param clkHz The frequency of the I2C clock + * @param range The range to measure, between ::QMA_RANGE_2G and ::QMA_RANGE_32G + * @param bandwidth The bandwidth to measure at, between ::QMA_BANDWIDTH_128_HZ and ::QMA_BANDWIDTH_1024_HZ + * @return ESP_OK if the accelerometer initialized, or a non-zero value if it did not + */ +esp_err_t initAccelerometer(i2c_port_t _i2c_port, gpio_num_t sda, gpio_num_t scl, gpio_pullup_t pullup, uint32_t clkHz, + qma_range_t range, qma_bandwidth_t bandwidth) +{ + i2c_port = _i2c_port; + esp_err_t ret_val = ESP_OK; + + /* Install i2c driver */ + i2c_config_t conf = { + .mode = I2C_MODE_MASTER, + .sda_io_num = sda, + .sda_pullup_en = pullup, + .scl_io_num = scl, + .scl_pullup_en = pullup, + .master.clk_speed = clkHz, + .clk_flags = I2C_SCLK_SRC_FLAG_FOR_NOMAL, + }; + ret_val |= i2c_param_config(i2c_port, &conf); + ret_val |= i2c_driver_install(i2c_port, conf.mode, 0, 0, 0); + + /* Exit sleep mode*/ + ret_val |= qma7981_write_byte(QMA7981_REG_PWR_MANAGE, 0xC0); + vTaskDelay(pdMS_TO_TICKS(20)); + + /* Set range */ + ret_val |= qma7981_write_byte(QMA7981_REG_RANGE, range); + /* Set bandwidth */ + ret_val |= qma7981_write_byte(QMA7981_REG_BAND_WIDTH, bandwidth); + + return ret_val; +} + +/** + * @brief Deinit the accelerometer (nothing to do) + * + * @return ESP_OK + */ +esp_err_t deInitAccelerometer(void) +{ + return ESP_OK; +} + +/** + * @brief Read a single byte from the accelerometer + * + * @param reg_addr The register to read from + * @param data The byte which was read is written here + * @return ESP_OK if the byte was read, or a non-zero value if it was not + */ +static esp_err_t qma7981_read_byte(qmaReg_t reg_addr, uint8_t* data) +{ + return qma7981_read_bytes(reg_addr, 1, data); +} + +/** + * @brief Read multiple bytes from the accelerometer + * + * @param reg_addr The register to read from + * @param data_len The number of bytes to read + * @param data The bytes which were read are written here + * @return ESP_OK if the bytes were read, or a non-zero value if they were not + */ +static esp_err_t qma7981_read_bytes(qmaReg_t reg_addr, size_t data_len, uint8_t* data) +{ + // Write the register to read from + i2c_cmd_handle_t cmdHandle = i2c_cmd_link_create(); + i2c_master_start(cmdHandle); + i2c_master_write_byte(cmdHandle, QMA7981_ADDR << 1 | I2C_MASTER_WRITE, false); + i2c_master_write_byte(cmdHandle, reg_addr, false); + i2c_master_stop(cmdHandle); + esp_err_t err = i2c_master_cmd_begin(i2c_port, cmdHandle, 100); + i2c_cmd_link_delete(cmdHandle); + + if (ESP_OK != err) + { + return err; + } + + // Read from the register + cmdHandle = i2c_cmd_link_create(); + i2c_master_start(cmdHandle); + i2c_master_write_byte(cmdHandle, QMA7981_ADDR << 1 | I2C_MASTER_READ, false); + i2c_master_read(cmdHandle, data, data_len, I2C_MASTER_LAST_NACK); + i2c_master_stop(cmdHandle); + err = i2c_master_cmd_begin(i2c_port, cmdHandle, 100); + i2c_cmd_link_delete(cmdHandle); + + return err; +} + +/** + * @brief Write a single byte to the accelerometer + * + * @param reg_addr The register address to write to + * @param data The byte to write + * @return ESP_OK if the byte was written, or a non-zero value if it was not + */ +static esp_err_t qma7981_write_byte(qmaReg_t reg_addr, uint8_t data) +{ + i2c_cmd_handle_t cmdHandle = i2c_cmd_link_create(); + i2c_master_start(cmdHandle); + + i2c_master_write_byte(cmdHandle, QMA7981_ADDR << 1, false); + i2c_master_write_byte(cmdHandle, reg_addr, false); + i2c_master_write_byte(cmdHandle, data, false); + + i2c_master_stop(cmdHandle); + esp_err_t err = i2c_master_cmd_begin(i2c_port, cmdHandle, 100); + i2c_cmd_link_delete(cmdHandle); + return err; +} + +/** + * @brief Read and return the 16-bit step counter + * + * Note that this can be configured with ::QMA7981_REG_STEP_CONF_0 through ::QMA7981_REG_STEP_CONF_3 + * + * @param data The step counter value is written here + * @return ESP_OK if the step count was read, or a non-zero value if it was not + */ +esp_err_t accelGetStep(uint16_t* data) +{ + esp_err_t ret_val = ESP_OK; + uint8_t step_h = 0, step_l = 0; + + if (NULL == data) + { + return ESP_ERR_INVALID_ARG; + } + + ret_val |= qma7981_read_byte(QMA7981_REG_STEP_L, &step_l); + ret_val |= qma7981_read_byte(QMA7981_REG_STEP_H, &step_h); + + *data = (step_h << 8) + step_l; + + return ret_val; +} + +/** + * @brief Set the accelerometer's measurement range + * + * @param range The range to measure, from ::QMA_RANGE_2G to ::QMA_RANGE_32G + * @return ESP_OK if the range was set, or a non-zero value if it was not + */ +esp_err_t accelSetRange(qma_range_t range) +{ + esp_err_t ret_val = qma7981_write_byte(QMA7981_REG_RANGE, range); + qma_range = range; + + return ret_val; +} + +/** + * @brief Read the current acceleration vector from the accelerometer and return + * the vector through arguments. If the read fails, the last known values are + * returned instead. + * + * @param x The X component of the acceleration vector is written here + * @param y The Y component of the acceleration vector is written here + * @param z The Z component of the acceleration vector is written here + * @return ESP_OK if the acceleration was read, or a non-zero value if it was not + */ +esp_err_t accelGetAccelVec(int16_t* x, int16_t* y, int16_t* z) +{ + static int16_t lastX = 0; + static int16_t lastY = 0; + static int16_t lastZ = 0; + + // Read 6 bytes of data(0x00) + uint8_t raw_data[6]; + // Do the read + esp_err_t ret_val = qma7981_read_bytes(QMA7981_REG_DX_L, 6, raw_data); + + // If the read was successful + if (ESP_OK == ret_val) + { + // Sign extend the 10 bit value to 16 bits and save it as the last known value + // TODO The datasheet mentions this is a 14 bit reading, not a 10 bit one? + lastX = signExtend10bit(((raw_data[0] >> 6) | (raw_data[1]) << 2) & 0x03FF); + lastY = -signExtend10bit(((raw_data[2] >> 6) | (raw_data[3]) << 2) & 0x03FF); + lastZ = -signExtend10bit(((raw_data[4] >> 6) | (raw_data[5]) << 2) & 0x03FF); + } + + // Copy out the acceleration value + *x = lastX; + *y = lastY; + *z = lastZ; + + return ret_val; +} + +/** + * @brief Helper function to sign-extend a 10 bit two's complement number to 16 bit + * + * @param in The two's compliment number to sign-extend + * @return The sign-extended two's compliment number + */ +static int16_t signExtend10bit(uint16_t in) +{ + if (in & 0x200) + { + return (in | 0xFC00); // extend the sign bit all the way out + } + else + { + return (in & 0x01FF); // make sure the sign bits are cleared + } +} diff --git a/components/hdw-accel-legacy/include/hdw-accel.h b/components/hdw-accel-legacy/include/hdw-accel.h new file mode 100644 index 000000000..2aa8f7312 --- /dev/null +++ b/components/hdw-accel-legacy/include/hdw-accel.h @@ -0,0 +1,82 @@ +/*! \file hdw-accel.h + * + * \section accel_design Design Philosophy + * + * The accelerometer used is a QMA7981. + * The datasheet can be found here: QMA7981 + * Datasheet. + * + * The accelerometer component does not automatically poll the accelerometer. + * All it does is set up and configure the accelerometer, then it is up to the Swadge mode to query for acceleration as + * appropriate. + * + * This component requires the I2C bus to be initialized, so it does that as well. + * If other I2C peripherals are added in the future, common I2C bus initialization should be moved to a more common + * location. + * + * \section accel_usage Usage + * + * You don't need to call initAccelerometer() or deInitAccelerometer(). The system does this the appropriate time. + * + * You do need to call accelGetAccelVec() to get the current acceleration vector. + * If you want to poll this from your Swadge mode's main function, you may. + * + * You may call accelSetRange() if you want to adjust the measurement range. + * + * accelGetStep() exists, but it has not been tested, so use it with caution. + * You may need to configure parameters related to step counting. + * + * \section accel_example Example + * + * \code{.c} + * // Declare variables to receive acceleration + * int16_t a_x, a_y, a_z; + * + * // Get the current acceleration + * if(ESP_OK == accelGetAccelVec(&a_x, &a_y, &a_z)) + * { + * // Print data to debug logs + * printf("x: %"PRId16", y: %"PRId16", z:%"PRId16, a_x, a_y, a_z); + * } + * \endcode + */ + +#ifndef _HDW_ACCEL_H_ +#define _HDW_ACCEL_H_ + +#include + +#include +#include +#include + +/** + * @brief The ranges for acceleration measurement from 2G to 32G (Earth gravity) + */ +typedef enum +{ + QMA_RANGE_2G = 0b0001, ///< Two G's of measurement range + QMA_RANGE_4G = 0b0010, ///< Four G's of measurement range + QMA_RANGE_8G = 0b0100, ///< Eight G's of measurement range + QMA_RANGE_16G = 0b1000, ///< Sixteen G's of measurement range + QMA_RANGE_32G = 0b1111, ///< Thirty-two G's of measurement range +} qma_range_t; + +/** + * @brief The bandwidth for acceleration measurement + */ +typedef enum +{ + QMA_BANDWIDTH_128_HZ = 0b111, ///< 128Hz bandwidth + QMA_BANDWIDTH_256_HZ = 0b110, ///< 256Hz bandwidth + QMA_BANDWIDTH_1024_HZ = 0b101, ///< 1024Hz bandwidth +} qma_bandwidth_t; + +esp_err_t initAccelerometer(i2c_port_t _i2c_port, gpio_num_t sda, gpio_num_t scl, gpio_pullup_t pullup, uint32_t clkHz, + qma_range_t range, qma_bandwidth_t bandwidth); +esp_err_t deInitAccelerometer(void); +esp_err_t accelSetRange(qma_range_t range); +esp_err_t accelGetAccelVec(int16_t* x, int16_t* y, int16_t* z); +esp_err_t accelGetStep(uint16_t* data); + +#endif diff --git a/components/hdw-accel/hdw-accel.c b/components/hdw-accel/hdw-accel.c index 7ff0a607d..4f383c1ac 100644 --- a/components/hdw-accel/hdw-accel.c +++ b/components/hdw-accel/hdw-accel.c @@ -3,8 +3,15 @@ //============================================================================== #include - +#include +#include #include "hdw-accel.h" +#include "soc/rtc_cntl_reg.h" +#include "soc/gpio_reg.h" +#include "soc/io_mux_reg.h" +#include "rom/gpio.h" +#include "soc/i2c_reg.h" +#include "soc/gpio_struct.h" //============================================================================== // Enums @@ -12,278 +19,690 @@ typedef enum __attribute__((packed)) { - QMA7981_REG_CHIP_ID = 0x00, - QMA7981_REG_DX_L = 0x01, - QMA7981_REG_DX_H = 0x02, - QMA7981_REG_DY_L = 0x03, - QMA7981_REG_DY_H = 0x04, - QMA7981_REG_DZ_L = 0x05, - QMA7981_REG_DZ_H = 0x06, - QMA7981_REG_STEP_L = 0x07, - QMA7981_REG_STEP_H = 0x08, - QMA7981_REG_INT_STAT_0 = 0x0A, - QMA7981_REG_INT_STAT_1 = 0x0B, - QMA7981_REG_INT_STAT_4 = 0x0D, - QMA7981_REG_RANGE = 0x0F, - QMA7981_REG_BAND_WIDTH = 0x10, - QMA7981_REG_PWR_MANAGE = 0x11, - QMA7981_REG_STEP_CONF_0 = 0x12, - QMA7981_REG_STEP_CONF_1 = 0x13, - QMA7981_REG_STEP_CONF_2 = 0x14, - QMA7981_REG_STEP_CONF_3 = 0x15, - QMA7981_REG_INT_EN_0 = 0x16, - QMA7981_REG_INT_EN_1 = 0x17, - QMA7981_REG_INT_MAP_0 = 0x19, - QMA7981_REG_INT_MAP_1 = 0x1A, - QMA7981_REG_INT_MAP_2 = 0x1B, - QMA7981_REG_INT_MAP_3 = 0x1C, - QMA7981_REG_SIG_STEP_TH = 0x1D, - QMA7981_REG_STEP = 0x1F -} qmaReg_t; + LSM6DSL_FUNC_CFG_ACCESS = 0x01, + LSM6DSL_SENSOR_SYNC_TIME_FRAME = 0x04, + LSM6DSL_FIFO_CTRL1 = 0x06, + LSM6DSL_FIFO_CTRL2 = 0x07, + LSM6DSL_FIFO_CTRL3 = 0x08, + LSM6DSL_FIFO_CTRL4 = 0x09, + LSM6DSL_FIFO_CTRL5 = 0x0a, + LSM6DSL_ORIENT_CFG_G = 0x0b, + LSM6DSL_INT1_CTRL = 0x0d, + LSM6DSL_INT2_CTRL = 0x0e, + LMS6DS3_WHO_AM_I = 0x0f, + LSM6DSL_CTRL1_XL = 0x10, + LSM6DSL_CTRL2_G = 0x11, + LSM6DSL_CTRL3_C = 0x12, + LSM6DSL_CTRL4_C = 0x13, + LSM6DSL_CTRL5_C = 0x14, + LSM6DSL_CTRL6_C = 0x15, + LSM6DSL_CTRL7_G = 0x16, + LSM6DSL_CTRL8_XL = 0x17, + LSM6DSL_CTRL9_XL = 0x18, + LSM6DSL_CTRL10_C = 0x19, + LSM6DSL_MASTER_CONFIG = 0x1a, + LSM6DSL_WAKE_UP_SRC = 0x1b, + LSM6DSL_TAP_SRC = 0x1c, + LSM6DSL_D6D_SRC = 0x1d, + LSM6DSL_STATUS_REG = 0x1e, + LSM6DSL_OUT_TEMP_L = 0x20, + LSM6DSL_OUT_TEMP_H = 0x21, + LMS6DS3_OUTX_L_G = 0x22, + LMS6DS3_OUTX_H_G = 0x23, + LMS6DS3_OUTY_L_G = 0x24, + LMS6DS3_OUTY_H_G = 0x25, + LMS6DS3_OUTZ_L_G = 0x26, + LMS6DS3_OUTZ_H_G = 0x27, + LMS6DS3_OUTX_L_XL = 0x28, + LMS6DS3_OUTX_H_XL = 0x29, + LMS6DS3_OUTY_L_XL = 0x2a, + LMS6DS3_OUTY_H_XL = 0x2b, + LMS6DS3_OUTZ_L_XL = 0x2c, + LMS6DS3_OUTZ_H_XL = 0x2d, +} lsm6dslReg_t; //============================================================================== // Defines //============================================================================== -#define QMA7981_ADDR 0x12 +#define LSM6DSL_ADDRESS 0x6a +#define QMC6308_ADDRESS 0x2c //============================================================================== // Variables //============================================================================== -static qma_range_t qma_range = QMA_RANGE_2G; static i2c_port_t i2c_port; +LSM6DSLData LSM6DSL; + +//============================================================================== +// Utility Prototypes +//============================================================================== + +float rsqrtf(float x); +float mathsqrtf(float x); +void mathEulerToQuat(float * q, const float * euler); +void mathQuatApply(float * qout, const float * q1, const float * q2); +void mathQuatNormalize(float * qout, const float * qin ); +void mathCrossProduct(float * p, const float * a, const float * b); +void mathRotateVectorByInverseOfQuaternion(float * pout, const float * q, const float * p ); +void mathRotateVectorByQuaternion(float * pout, const float * q, const float * p); + +static inline uint32_t getCycleCount(); + +esp_err_t GeneralSet( int dev, int reg, int val ); +esp_err_t LSM6DSLSet( int reg, int val ); +int GeneralI2CGet( int device, int reg, uint8_t * data, int data_len ); +int ReadLSM6DSL( uint8_t * data, int data_len ); //============================================================================== // Function Prototypes //============================================================================== -static esp_err_t qma7981_read_byte(qmaReg_t reg_addr, uint8_t* data); -static esp_err_t qma7981_write_byte(qmaReg_t reg_addr, uint8_t data); -static esp_err_t qma7981_read_bytes(qmaReg_t reg_addr, size_t data_len, uint8_t* data); -static int16_t signExtend10bit(uint16_t in); +esp_err_t initAccelerometer(i2c_port_t _i2c_port, gpio_num_t sda, gpio_num_t scl, gpio_pullup_t pullup, uint32_t clkHz ); +esp_err_t deInitAccelerometer(void); +esp_err_t accelGetAccelVec(int16_t* x, int16_t* y, int16_t* z); +esp_err_t accelGetQuaternion(float * q); +esp_err_t accelIntegrate(); //============================================================================== -// Functions +// Utility Functions //============================================================================== + /** - * @brief Initialize the accelerometer + * @brief Perform a fast, approximate reciprocal square root * - * @param _i2c_port The i2c port to use for the accelerometer - * @param sda The GPIO for the Serial DAta line - * @param scl The GPIO for the Serial CLock line - * @param pullup Either \c GPIO_PULLUP_DISABLE if there are external pullup resistors on SDA and SCL or \c - * GPIO_PULLUP_ENABLE if internal pull-ups should be used - * @param clkHz The frequency of the I2C clock - * @param range The range to measure, between ::QMA_RANGE_2G and ::QMA_RANGE_32G - * @param bandwidth The bandwidth to measure at, between ::QMA_BANDWIDTH_128_HZ and ::QMA_BANDWIDTH_1024_HZ - * @return ESP_OK if the accelerometer initialized, or a non-zero value if it did not + * @param x The number to take a recriprocal square root of. + * @return approximately 1/sqrt(x) */ -esp_err_t initAccelerometer(i2c_port_t _i2c_port, gpio_num_t sda, gpio_num_t scl, gpio_pullup_t pullup, uint32_t clkHz, - qma_range_t range, qma_bandwidth_t bandwidth) +float rsqrtf(float x) { - i2c_port = _i2c_port; - esp_err_t ret_val = ESP_OK; + typedef union { int32_t i; float f; } fiunion; + const float xhalf = 0.5f * x; + fiunion i = { .f = x }; + i.i = 0x5f375a86 - ( i.i >> 1 ); + x = i.f; + x = x * ( 1.5f - xhalf * x * x ); + x = x * ( 1.5f - xhalf * x * x ); + return x; +} - /* Install i2c driver */ - i2c_config_t conf = { - .mode = I2C_MODE_MASTER, - .sda_io_num = sda, - .sda_pullup_en = pullup, - .scl_io_num = scl, - .scl_pullup_en = pullup, - .master.clk_speed = clkHz, - .clk_flags = I2C_SCLK_SRC_FLAG_FOR_NOMAL, - }; - ret_val |= i2c_param_config(i2c_port, &conf); - ret_val |= i2c_driver_install(i2c_port, conf.mode, 0, 0, 0); +/** + * @brief Perform a fast, approximate square root + * + * @param x The number to take a square root of. + * @return approximately sqrt(x) (but is much faster) + */ +float mathsqrtf(float x) +{ + // Trick to do approximate, fast square roots. (Though it is surprisingly fast) + int sign = x < 0; + if( sign ) x = -x; + if( x < 0.0000001 ) return 0.0001; + float o = x; + o = (o+x/o)/2; + o = (o+x/o)/2; + o = (o+x/o)/2; + o = (o+x/o)/2; + if( sign ) + return -o; + else + return o; +} + +/** + * @brief convert euler angles (in radians) to a quaternion. + * + * @param q Pointer to the wxyz quat (float[4]) to be written. + * @param euler Pointer to a float[3] of euler angles. + */ +void mathEulerToQuat(float * q, const float * euler) +{ + float pitch = euler[0]; + float yaw = euler[1]; + float roll = euler[2]; + float cr = cosf(pitch * 0.5); + float sr = sinf(pitch * 0.5); // Pitch: About X + float cp = cosf(yaw * 0.5); + float sp = sinf(yaw * 0.5); // Yaw: About Y + float cy = cosf(roll * 0.5); + float sy = sinf(roll * 0.5); // Roll: About Z + q[0] = cr * cp * cy + sr * sp * sy; + q[1] = sr * cp * cy - cr * sp * sy; + q[2] = cr * sp * cy + sr * cp * sy; + q[3] = cr * cp * sy - sr * sp * cy; +} - /* Exit sleep mode*/ - ret_val |= qma7981_write_byte(QMA7981_REG_PWR_MANAGE, 0xC0); - vTaskDelay(pdMS_TO_TICKS(20)); +/** + * @brief Rotate one quaternion by another (and do not normalize) + * + * @param qout Pointer to the wxyz quat (float[4]) to be written. + * @param q1 First quaternion to be rotated. + * @param q2 Quaternion to rotate q1 by. + */ +void mathQuatApply(float * qout, const float * q1, const float * q2) +{ + // NOTE: Does not normalize - you will need to normalize eventually. + float tmpw, tmpx, tmpy; + tmpw = (q1[0] * q2[0]) - (q1[1] * q2[1]) - (q1[2] * q2[2]) - (q1[3] * q2[3]); + tmpx = (q1[0] * q2[1]) + (q1[1] * q2[0]) + (q1[2] * q2[3]) - (q1[3] * q2[2]); + tmpy = (q1[0] * q2[2]) - (q1[1] * q2[3]) + (q1[2] * q2[0]) + (q1[3] * q2[1]); + qout[3] = (q1[0] * q2[3]) + (q1[1] * q2[2]) - (q1[2] * q2[1]) + (q1[3] * q2[0]); + qout[2] = tmpy; + qout[1] = tmpx; + qout[0] = tmpw; +} + +/** + * @brief Normalize a quaternion + * + * @param qout Pointer to the wxyz quat (float[4]) to be written. + * @param qin Pointer to the quaterion to normalize. + */ +void mathQuatNormalize(float * qout, const float * qin ) +{ + float qmag = qin[0] * qin[0] + qin[1] * qin[1] + qin[2] * qin[2] + qin[3] * qin[3]; + qmag = rsqrtf( qmag ); + qout[0] = qin[0] * qmag; + qout[1] = qin[1] * qmag; + qout[2] = qin[2] * qmag; + qout[3] = qin[3] * qmag; +} - /* Set range */ - ret_val |= qma7981_write_byte(QMA7981_REG_RANGE, range); - /* Set bandwidth */ - ret_val |= qma7981_write_byte(QMA7981_REG_BAND_WIDTH, bandwidth); - return ret_val; +/** + * @brief Perform a 3D cross product + * + * @param p Pointer to the float[3] output of the cross product (p = a x b) + * @param a Pointer to the float[3] of the cross product a vector. + * @param a Pointer to the float[3] of the cross product b vector. + */ +void mathCrossProduct(float * p, const float * a, const float * b) +{ + float tx = a[1] * b[2] - a[2] * b[1]; + float ty = a[2] * b[0] - a[0] * b[2]; + p[2] = a[0] * b[1] - a[1] * b[0]; + p[1] = ty; + p[0] = tx; } /** - * @brief Deinit the accelerometer (nothing to do) + * @brief Rotate a 3D vector by a quaternion * - * @return ESP_OK + * @param pout Pointer to the float[3] output of the rotation + * @param q Pointer to the wzyz quaternion (float[4]) of the rotation. + * @param p Pointer to the float[3] of the vector to rotates. */ -esp_err_t deInitAccelerometer(void) +void mathRotateVectorByQuaternion(float * pout, const float * q, const float * p) { - return ESP_OK; + // return v + 2.0 * cross(q.xyz, cross(q.xyz, v) + q.w * v); + float iqo[3]; + mathCrossProduct( iqo, q + 1 /*.xyz*/, p ); + iqo[0] += q[0] * p[0]; + iqo[1] += q[0] * p[1]; + iqo[2] += q[0] * p[2]; + float ret[3]; + mathCrossProduct( ret, q + 1 /*.xyz*/, iqo ); + pout[0] = ret[0] * 2.0 + p[0]; + pout[1] = ret[1] * 2.0 + p[1]; + pout[2] = ret[2] * 2.0 + p[2]; } /** - * @brief Read a single byte from the accelerometer + * @brief Rotate a 3D vector by the inverse of a quaternion * - * @param reg_addr The register to read from - * @param data The byte which was read is written here - * @return ESP_OK if the byte was read, or a non-zero value if it was not + * @param pout Pointer to the float[3] output of the antirotation. + * @param q Pointer to the wzyz quaternion (float[4]) opposite of the rotation. + * @param p Pointer to the float[3] of the vector to antirotates. */ -static esp_err_t qma7981_read_byte(qmaReg_t reg_addr, uint8_t* data) +void mathRotateVectorByInverseOfQuaternion(float * pout, const float * q, const float * p) +{ + // General note: Performing a transform this way can be about 20-30% slower than a well formed 3x3 matrix. + // return v + 2.0 * cross(q.xyz, cross(q.xyz, v) + q.w * v); + float iqo[3]; + mathCrossProduct( iqo, p, q + 1 /*.xyz*/ ); + iqo[0] += q[0] * p[0]; + iqo[1] += q[0] * p[1]; + iqo[2] += q[0] * p[2]; + float ret[3]; + mathCrossProduct( ret, iqo, q + 1 /*.xyz*/ ); + pout[0] = ret[0] * 2.0 + p[0]; + pout[1] = ret[1] * 2.0 + p[1]; + pout[2] = ret[2] * 2.0 + p[2]; +} + +static inline uint32_t getCycleCount() { - return qma7981_read_bytes(reg_addr, 1, data); + uint32_t ccount; + asm volatile("rsr %0,ccount" : "=a"(ccount)); + return ccount; } +//============================================================================== +// Internal Functions +//============================================================================== + + /** - * @brief Read multiple bytes from the accelerometer + * @brief Set a specific register on the IMU to a value. * - * @param reg_addr The register to read from - * @param data_len The number of bytes to read - * @param data The bytes which were read are written here - * @return ESP_OK if the bytes were read, or a non-zero value if they were not + * @param dev The 7-bit address of the device to set the register to. + * @param reg The 8-bit register # + * @param val The 8-bit value to set the register to. + * @return ESP_OK if the operation was successful. */ -static esp_err_t qma7981_read_bytes(qmaReg_t reg_addr, size_t data_len, uint8_t* data) +esp_err_t GeneralSet( int dev, int reg, int val ) { - // Write the register to read from i2c_cmd_handle_t cmdHandle = i2c_cmd_link_create(); i2c_master_start(cmdHandle); - i2c_master_write_byte(cmdHandle, QMA7981_ADDR << 1 | I2C_MASTER_WRITE, false); - i2c_master_write_byte(cmdHandle, reg_addr, false); + i2c_master_write_byte(cmdHandle, dev << 1, false); + i2c_master_write_byte(cmdHandle, reg, false); + i2c_master_write_byte(cmdHandle, val, true); i2c_master_stop(cmdHandle); - esp_err_t err = i2c_master_cmd_begin(i2c_port, cmdHandle, 100); + esp_err_t err = i2c_master_cmd_begin(I2C_NUM_0, cmdHandle, 100); i2c_cmd_link_delete(cmdHandle); + return err; +} - if (ESP_OK != err) - { - return err; - } +/** + * @brief Set a specific register on the LSM6DSL to a value. + * + * @param reg The 8-bit register # + * @param val The 8-bit value to set the register to. + * @return ESP_OK if the operation was successful. + */ +esp_err_t LSM6DSLSet( int reg, int val ) +{ + return GeneralSet( LSM6DSL_ADDRESS, reg, val ); +} - // Read from the register - cmdHandle = i2c_cmd_link_create(); +/** + * @brief Read a buffer back from a specific I2C device. + * + * @param device The 7-bit device address + * @param reg The 8-bit register # to start at. + * @param data The buffer to load the data into. + * @param data_len Number of bytes to read. + * @return positive number if operation was successful, or esp_err_t if failure. + */ +int GeneralI2CGet( int device, int reg, uint8_t * data, int data_len ) +{ + i2c_cmd_handle_t cmdHandle = i2c_cmd_link_create(); i2c_master_start(cmdHandle); - i2c_master_write_byte(cmdHandle, QMA7981_ADDR << 1 | I2C_MASTER_READ, false); + i2c_master_write_byte(cmdHandle, device << 1, false); + i2c_master_write_byte(cmdHandle, reg, false); + i2c_master_start(cmdHandle); + i2c_master_write_byte(cmdHandle, device << 1 | I2C_MASTER_READ, false); i2c_master_read(cmdHandle, data, data_len, I2C_MASTER_LAST_NACK); i2c_master_stop(cmdHandle); - err = i2c_master_cmd_begin(i2c_port, cmdHandle, 100); + esp_err_t err = i2c_master_cmd_begin(I2C_NUM_0, cmdHandle, 100); i2c_cmd_link_delete(cmdHandle); - - return err; + if( err ) return -1; + else return data_len; } + /** - * @brief Write a single byte to the accelerometer + * @brief Read the FIFO out of the LSM6DSL * - * @param reg_addr The register address to write to - * @param data The byte to write - * @return ESP_OK if the byte was written, or a non-zero value if it was not + * @param data The buffer to write the FIFO data into. + * @param data_len The maximum size (in words) to read. + * @return positive number if operation was successful, or esp_err_t if failure. */ -static esp_err_t qma7981_write_byte(qmaReg_t reg_addr, uint8_t data) +int ReadLSM6DSL( uint8_t * data, int data_len ) { i2c_cmd_handle_t cmdHandle = i2c_cmd_link_create(); i2c_master_start(cmdHandle); + i2c_master_write_byte(cmdHandle, LSM6DSL_ADDRESS << 1, false); + i2c_master_write_byte(cmdHandle, 0x3A, false); + i2c_master_start(cmdHandle); + i2c_master_write_byte(cmdHandle, LSM6DSL_ADDRESS << 1 | I2C_MASTER_READ, false); + uint32_t fifolen = 0; + i2c_master_read(cmdHandle, (uint8_t*)&fifolen, 3, I2C_MASTER_LAST_NACK); + i2c_master_stop(cmdHandle); + esp_err_t err = i2c_master_cmd_begin(I2C_NUM_0, cmdHandle, 100); + i2c_cmd_link_delete(cmdHandle); + if( err < 0 ) return -1; - i2c_master_write_byte(cmdHandle, QMA7981_ADDR << 1, false); - i2c_master_write_byte(cmdHandle, reg_addr, false); - i2c_master_write_byte(cmdHandle, data, false); + if( fifolen == 0 ) return 0; + fifolen &= 0x3ff; + if( fifolen > data_len / 2 ) fifolen = data_len / 2; + + cmdHandle = i2c_cmd_link_create(); + i2c_master_start(cmdHandle); + i2c_master_write_byte(cmdHandle, LSM6DSL_ADDRESS << 1 | I2C_MASTER_READ, false); + i2c_master_read(cmdHandle, data, fifolen * 2, I2C_MASTER_LAST_NACK); i2c_master_stop(cmdHandle); - esp_err_t err = i2c_master_cmd_begin(i2c_port, cmdHandle, 100); + err = i2c_master_cmd_begin(I2C_NUM_0, cmdHandle, 100); + i2c_cmd_link_delete(cmdHandle); - return err; + if( err < 0 ) return -2; + + return fifolen; } + + + +//============================================================================== +// Functions +//============================================================================== + + + + /** - * @brief Read and return the 16-bit step counter + * @brief Initialize the IMU * - * Note that this can be configured with ::QMA7981_REG_STEP_CONF_0 through ::QMA7981_REG_STEP_CONF_3 - * - * @param data The step counter value is written here - * @return ESP_OK if the step count was read, or a non-zero value if it was not + * @param _i2c_port The i2c port to use for the IMU + * @param sda The GPIO for the Serial DAta line + * @param scl The GPIO for the Serial CLock line + * @param pullup Either \c GPIO_PULLUP_DISABLE if there are external pullup resistors on SDA and SCL or \c + * GPIO_PULLUP_ENABLE if internal pull-ups should be used + * @param clkHz The frequency of the I2C clock + * @return ESP_OK if the accelerometer initialized, or a non-zero value if it did not */ -esp_err_t accelGetStep(uint16_t* data) +esp_err_t initAccelerometer(i2c_port_t _i2c_port, gpio_num_t sda, gpio_num_t scl, gpio_pullup_t pullup, uint32_t clkHz ) { - esp_err_t ret_val = ESP_OK; - uint8_t step_h = 0, step_l = 0; + int retry = 0; + i2c_port = _i2c_port; + esp_err_t ret_val; + + memset( &LSM6DSL, 0, sizeof(LSM6DSL) ); + LSM6DSL.fqQuat[0] = 1; + LSM6DSL.fqQuatLast[0] = 1; - if (NULL == data) - { - return ESP_ERR_INVALID_ARG; - } - ret_val |= qma7981_read_byte(QMA7981_REG_STEP_L, &step_l); - ret_val |= qma7981_read_byte(QMA7981_REG_STEP_H, &step_h); +do_retry: - *data = (step_h << 8) + step_l; - return ret_val; + // Shake any device off the bus. + int i; + int gpio_scl = 41; + for( i = 0; i < 16; i++ ) + { + gpio_matrix_out( gpio_scl, 256, 1, 0 ); + GPIO.out1_w1tc.val = (1<<(gpio_scl-32)); + esp_rom_delay_us(10); + gpio_matrix_out( gpio_scl, 256, 1, 0 ); + GPIO.out1_w1ts.val = (1<<(gpio_scl-32)); + esp_rom_delay_us(10); + } + gpio_matrix_out( gpio_scl, 29, 0, 0 ); + + ret_val = ESP_OK; + + i2c_driver_delete( _i2c_port ); + + /* Install i2c driver */ + i2c_config_t conf = { + .mode = I2C_MODE_MASTER, + .sda_io_num = sda, + .sda_pullup_en = pullup, + .scl_io_num = scl, + .scl_pullup_en = pullup, + .master.clk_speed = clkHz, //tested upto 1.4Mbit/s + .clk_flags = I2C_SCLK_SRC_FLAG_FOR_NOMAL, + }; + ESP_LOGI( "accel", "i2c_driver_install=%d", i2c_driver_install(_i2c_port, conf.mode, 0, 0, 0) ); + ret_val |= i2c_param_config(i2c_port, &conf); + + // Enable access + LSM6DSLSet( LSM6DSL_FUNC_CFG_ACCESS, 0x20 ); + LSM6DSLSet( LSM6DSL_CTRL3_C, 0x81 ); // Force reset + vTaskDelay( 1 ); + LSM6DSLSet( LSM6DSL_CTRL3_C, 0x44 ); // unforce reset + + uint8_t who = 0xaa; + int r = GeneralI2CGet( LSM6DSL_ADDRESS, LMS6DS3_WHO_AM_I, &who, 1 ); + if( r != 1 || who != 0x6a ) + { + ESP_LOGW( "LSM6DSL", "WHOAMI Failed (%02x), %d\n", who, r ); + if( retry++ < 10 ) goto do_retry; + ESP_LOGE( "LSM6DSL", "Init failed on 1" ); + return ESP_FAIL; + } + ESP_LOGI( "LSM6DSL", "Init Start" ); + + for( i = 0; i < 2; i++ ) + { + LSM6DSLSet( LSM6DSL_FIFO_CTRL5, (0b0101 << 3) | 0b110 ); // 208 Hz ODR + LSM6DSLSet( LSM6DSL_FIFO_CTRL3, 0b00001001 ); // Put both devices (Accel + Gyro) in FIFO. + LSM6DSLSet( LSM6DSL_CTRL1_XL, 0b01011001 ); // Setup accel (16 g's FS) + LSM6DSLSet( LSM6DSL_CTRL2_G, 0b01011100 ); // Setup gyro, 2000dps + LSM6DSLSet( LSM6DSL_CTRL4_C, 0x00 ); // Disable all filtering. + LSM6DSLSet( LSM6DSL_CTRL7_G, 0b00000000 ); // Setup gyro, not high performance mode = 0x80. High perf = 0x00 + LSM6DSLSet( LSM6DSL_FIFO_CTRL2, 0b00000000 ); //Temp not in fifo (Why no work?) + + vTaskDelay( 5 ); + + if( accelIntegrate() != ESP_OK ) + { + ESP_LOGI( "LSM6DSL", "Init Fault Retry" ); + if( retry++ < 10 ) goto do_retry; + ESP_LOGI( "LSM6DSL", "Init failed on 2" ); + return ESP_FAIL; + } + } + + ESP_LOGI( "LSM6DSL", "Init Ok" ); + return ESP_OK; } /** - * @brief Set the accelerometer's measurement range + * @brief Deinit the accelerometer (nothing to do) * - * @param range The range to measure, from ::QMA_RANGE_2G to ::QMA_RANGE_32G - * @return ESP_OK if the range was set, or a non-zero value if it was not + * @return ESP_OK */ -esp_err_t accelSetRange(qma_range_t range) +esp_err_t deInitAccelerometer(void) { - esp_err_t ret_val = qma7981_write_byte(QMA7981_REG_RANGE, range); - qma_range = range; - - return ret_val; + return ESP_OK; } + /** - * @brief Read the current acceleration vector from the accelerometer and return - * the vector through arguments. If the read fails, the last known values are - * returned instead. + * @brief Deinit the accelerometer (nothing to do) * - * @param x The X component of the acceleration vector is written here - * @param y The Y component of the acceleration vector is written here - * @param z The Z component of the acceleration vector is written here - * @return ESP_OK if the acceleration was read, or a non-zero value if it was not + * @return ESP_OK if successful, or nonzero if error. */ +esp_err_t accelIntegrate() +{ + LSM6DSLData * ld = &LSM6DSL; + + int16_t data[6*16]; + + // Get temperature sensor (in case we ever want to use it) + int r = GeneralI2CGet( LSM6DSL_ADDRESS, 0x20, (uint8_t*)data, 2 ); + if( r < 0 ) return r; + if( r == 2 ) ld->temp = data[0]; + int readr = ReadLSM6DSL( (uint8_t*)data, sizeof( data ) ); + if( readr < 0 ) return readr; + + int samp; + int16_t * cdata = data; + + uint32_t start = getCycleCount(); + + // STEP 0: Decide your coordinate frame. + + // [0] = +X axis coming out right of controller. + // [1] = +Y axis, pointing straight up out of controller, out where the USB port is. + // [2] = +Z axis, pointing up from the face of the controller. + + ld->lastreadr = readr; + + for( samp = 0; samp < readr; samp+=12 ) + { + // Extract data from IMU + int16_t * euler_deltas = cdata; // Euler angles, from gyro. + int16_t * accel_data = cdata + 3; + + // Manual cal, used only for Steps 2..8 + // euler_deltas[0] -= 12; + // euler_deltas[1] += 22; + // euler_deltas[2] += 4; + + // We can sum rotations to understand the amount of counts in a full circle. + ld->gyroaccum[0] += euler_deltas[0]; + ld->gyroaccum[1] += euler_deltas[1]; + ld->gyroaccum[2] += euler_deltas[2]; + ld->gyrocount++; + + // STEP 1: Visually inspect the gyro values. + // STEP 2: Integrate the gyro values, verify they are correct. + + // 2000 dps full-scale + // 32768 is full-scale + // 208 SPS + // convert to radians. ( 2000.0f / 32768.0f / 208.0f * 2.0 * 3.14159f / 180.0f ); + // Measured = 560,000 counts per scale (Measured by looking at sum) + // Testing -> 3.14159 * 2.0 / 566000; + float fFudge = 1.1; //XXX TODO: Investigate. + float fScale = ( 2000.0f / 32768.0f / 208.0f * 2.0 * 3.14159f / 180.0f ) * fFudge; + + // STEP 3: Integrate gyro values into a quaternion. + // This step is validated by working with just one axis at a time + // then apply a coordinate frame to ld->fqQuat and validate that it is + // correct. + float fEulerScales[3] = { + -fScale, + fScale, + -fScale }; + + float fEulers[3] = { + euler_deltas[0] * fEulerScales[0] + ld->fvBias[0], + euler_deltas[1] * fEulerScales[1] + ld->fvBias[1], + euler_deltas[2] * fEulerScales[2] + ld->fvBias[2] }; + + mathEulerToQuat( ld->fqQuatLast, fEulers ); + mathQuatApply( ld->fqQuat, ld->fqQuat, ld->fqQuatLast ); + + // STEP 4: Validate yor values by doing 4 90 degree turns + // across multiple axes. + // i.e. rotate controller down, clockwise from top, up, counter-clockwise. + // while investigating quaternion. It should return to identity. + + // STEP 6: Determine our "error" based on accelerometer. + // NOTE: This step could be done on the inner loop if you want, and done over + // every accelerometer cycle, or it can be done on the outside, every few cycles. + // all that realy matters is that it is done periodically. + + // STEP 6A: Examine vectors. Generally speaking, we want an "up" vector, not a gravity vector. + // this is "up" in the controller's point of view. + float accel_up[3] = { + -accel_data[0], + accel_data[1], + -accel_data[2] + }; + + float accel_inverse_mag = rsqrtf( accel_up[0] * accel_up[0] + accel_up[1] * accel_up[1] + accel_up[2] * accel_up[2] ); + accel_up[0] *= accel_inverse_mag; + accel_up[1] *= accel_inverse_mag; + accel_up[2] *= accel_inverse_mag; + + //ESP_LOGI( "SB", "%ld %ld %ld", raw_up[0], raw_up[1], raw_up[2] ); + + // Step 6B: Next, compute what we think "up" should be from our point of view. We will use +Y Up. + float what_we_think_is_up[3] = { 0, 1, 0 }; + mathRotateVectorByInverseOfQuaternion( what_we_think_is_up, LSM6DSL.fqQuat, what_we_think_is_up ); + + // Step 6C: Next, we determine how far off we are. This will tell us our error. + float corrective_quaternion[4]; + + // TRICKY: The ouput of this is actually the axis of rotation, which is ironically + // in vector-form the same as a quaternion. So we can write directly into the quat. + mathCrossProduct( corrective_quaternion + 1, accel_up, what_we_think_is_up ); + + // Now, we apply this in step 7. + + // First, we can compute what the drift values of our axes are, to anti-drift them. + // If you do only this, you will always end up in an unstable oscillation. + memcpy( ld->fCorrectLast, corrective_quaternion+1, 12 ); + // XXX TODO: We need to multiply by amount the accelerometer gives us assurance. + ld->fvBias[0] += mathsqrtf(corrective_quaternion[1]) * 0.0000002; + ld->fvBias[1] += mathsqrtf(corrective_quaternion[2]) * 0.0000002; + ld->fvBias[2] += mathsqrtf(corrective_quaternion[3]) * 0.0000002; + + + // Second, we can apply a very small corrective tug. This helps prevent oscillation + // about the correct answer. This acts sort of like a P term to a PID loop. + // This is actually the **primary**, or fastest responding thing. + const float corrective_force = 0.001f; + corrective_quaternion[1] *= corrective_force; + corrective_quaternion[2] *= corrective_force; + corrective_quaternion[3] *= corrective_force; + + // x^2+y^2+z^2+q^2 -> ALGEBRA! -> sqrt( 1-x^2-y^2-z^2 ) = w + corrective_quaternion[0] = mathsqrtf( 1 + - corrective_quaternion[1]*corrective_quaternion[1] + - corrective_quaternion[2]*corrective_quaternion[2] + - corrective_quaternion[3]*corrective_quaternion[3] ); +// ESP_LOGI( "x", "%f %f %f %f\n", corrective_quaternion[0], corrective_quaternion[1], corrective_quaternion[2], corrective_quaternion[3] ); + mathQuatApply( ld->fqQuat, ld->fqQuat, corrective_quaternion ); + + // Magnitude of correction angle = inverse_sin( magntiude( axis_of_correction ) ); + // We want to significantly reduce that. To mute any effect. + + cdata += 6; + } + + // Now we move onto the outer loop. + // STEP 5: We now want to normalize the quat periodically. Don't do this too + // soon, otherwise you won't notice math errors. Realistically, this should + // only need to be done every hundreds of thousands of samples. + // + // Also, don't do this too often, otherwise you will reduce accuracy, + // unnecessarily. + float * qRot = ld->fqQuat; + float qmagsq = qRot[0] * qRot[0] + qRot[1] * qRot[1] + qRot[2] * qRot[2] + qRot[3] * qRot[3]; + if( qmagsq > 1.05 || qmagsq < 0.95 ) + { + // This normalizes everything. + qmagsq = rsqrtf( qmagsq ); + qRot[0] = qRot[0] * qmagsq; + qRot[1] = qRot[1] * qmagsq; + qRot[2] = qRot[2] * qmagsq; + qRot[3] = qRot[3] * qmagsq; + } + + + if( samp ) + { + ld->gyrolast[0] = cdata[-6]; + ld->gyrolast[1] = cdata[-5]; + ld->gyrolast[2] = cdata[-4]; + ld->accellast[0] = cdata[-3]; + ld->accellast[1] = cdata[-2]; + ld->accellast[2] = cdata[-1]; + } + + ld->computetime = getCycleCount() - start; + + return ESP_OK; +} + esp_err_t accelGetAccelVec(int16_t* x, int16_t* y, int16_t* z) { - static int16_t lastX = 0; - static int16_t lastY = 0; - static int16_t lastZ = 0; - - // Read 6 bytes of data(0x00) - uint8_t raw_data[6]; - // Do the read - esp_err_t ret_val = qma7981_read_bytes(QMA7981_REG_DX_L, 6, raw_data); - - // If the read was successful - if (ESP_OK == ret_val) - { - // Sign extend the 10 bit value to 16 bits and save it as the last known value - // TODO The datasheet mentions this is a 14 bit reading, not a 10 bit one? - lastX = signExtend10bit(((raw_data[0] >> 6) | (raw_data[1]) << 2) & 0x03FF); - lastY = -signExtend10bit(((raw_data[2] >> 6) | (raw_data[3]) << 2) & 0x03FF); - lastZ = -signExtend10bit(((raw_data[4] >> 6) | (raw_data[5]) << 2) & 0x03FF); - } - - // Copy out the acceleration value - *x = lastX; - *y = lastY; - *z = lastZ; - - return ret_val; + if( accelIntegrate() < 0 ) + return ESP_FAIL; + + float plusy[3] = { 0, 1, 0 }; + mathRotateVectorByQuaternion( plusy, LSM6DSL.fqQuat, plusy ); + *x = plusy[0] * 1023; + *y = plusy[1] * 1023; + *z = plusy[2] * 1023; + return ESP_OK; } -/** - * @brief Helper function to sign-extend a 10 bit two's complement number to 16 bit - * - * @param in The two's compliment number to sign-extend - * @return The sign-extended two's compliment number - */ -static int16_t signExtend10bit(uint16_t in) +esp_err_t accelGetQuaternion(float * q) { - if (in & 0x200) - { - return (in | 0xFC00); // extend the sign bit all the way out - } - else - { - return (in & 0x01FF); // make sure the sign bits are cleared - } + float * fq = LSM6DSL.fqQuat; + q[0] = fq[0]; + q[1] = fq[1]; + q[2] = fq[2]; + q[3] = fq[3]; + return ESP_OK; } + diff --git a/components/hdw-accel/include/hdw-accel.h b/components/hdw-accel/include/hdw-accel.h index 2aa8f7312..9591675b8 100644 --- a/components/hdw-accel/include/hdw-accel.h +++ b/components/hdw-accel/include/hdw-accel.h @@ -1,42 +1,38 @@ /*! \file hdw-accel.h * - * \section accel_design Design Philosophy + * \section imu_design Design Philosophy * - * The accelerometer used is a QMA7981. - * The datasheet can be found here: QMA7981 - * Datasheet. + * Originally swadges were planned to use a LSM6DSL and a QMC6308, however, because the batteries are so close to the + * magnetometer, the quality of the data was low enough we dcided to proceed with with a LSM6DSL-only IMU. * - * The accelerometer component does not automatically poll the accelerometer. - * All it does is set up and configure the accelerometer, then it is up to the Swadge mode to query for acceleration as - * appropriate. - * - * This component requires the I2C bus to be initialized, so it does that as well. - * If other I2C peripherals are added in the future, common I2C bus initialization should be moved to a more common - * location. + * Unlike the accelerometer process, the IMU fuses the gyroscope and accelerometer data from the LMS6DSL. By fusing + * both sensors, we are able to produce a quaternion to represent the rotation of the swadge. The idea is that + * we run the IMU at 208 Hz, and we use the hardware FIFO built into the LSM6DSL to queue up events. Then, every + * frame, we empty out the FIFO. * * \section accel_usage Usage * - * You don't need to call initAccelerometer() or deInitAccelerometer(). The system does this the appropriate time. - * - * You do need to call accelGetAccelVec() to get the current acceleration vector. - * If you want to poll this from your Swadge mode's main function, you may. + * The core system will call initAccelerometer() and deInitAccelerometer() appropriately. And you can at any point + * call any of the proper IMU / accel functions. * - * You may call accelSetRange() if you want to adjust the measurement range. + * The functions you can use are: + * esp_err_t accelGetAccelVec(int16_t* x, int16_t* y, int16_t* z); + * esp_err_t accelGetQuaternion( float * quaternion ); * - * accelGetStep() exists, but it has not been tested, so use it with caution. - * You may need to configure parameters related to step counting. + * You can, of course at any time call: + * esp_err_t accelIntegrate(); * * \section accel_example Example * * \code{.c} - * // Declare variables to receive acceleration - * int16_t a_x, a_y, a_z; + * // Declare variables to receive rotation + * float q[4]; * - * // Get the current acceleration - * if(ESP_OK == accelGetAccelVec(&a_x, &a_y, &a_z)) + * // Get the current rotation + * if(ESP_OK == accelGetQuaternion( q )) * { * // Print data to debug logs - * printf("x: %"PRId16", y: %"PRId16", z:%"PRId16, a_x, a_y, a_z); + * printf( "%f %f %f %f\n", q[0], q[1], q[2], q[3] ); * } * \endcode */ @@ -50,33 +46,51 @@ #include #include -/** - * @brief The ranges for acceleration measurement from 2G to 32G (Earth gravity) - */ -typedef enum +typedef struct { - QMA_RANGE_2G = 0b0001, ///< Two G's of measurement range - QMA_RANGE_4G = 0b0010, ///< Four G's of measurement range - QMA_RANGE_8G = 0b0100, ///< Eight G's of measurement range - QMA_RANGE_16G = 0b1000, ///< Sixteen G's of measurement range - QMA_RANGE_32G = 0b1111, ///< Thirty-two G's of measurement range -} qma_range_t; + int32_t temp; + uint32_t computetime; -/** - * @brief The bandwidth for acceleration measurement - */ -typedef enum -{ - QMA_BANDWIDTH_128_HZ = 0b111, ///< 128Hz bandwidth - QMA_BANDWIDTH_256_HZ = 0b110, ///< 256Hz bandwidth - QMA_BANDWIDTH_1024_HZ = 0b101, ///< 1024Hz bandwidth -} qma_bandwidth_t; + // Quats are wxyz. + // You can take a vector, in controller space, rotate by this quat, and you get it in world space. + float fqQuatLast[4]; + float fqQuat[4]; // Quats are wxyz + + // Bias for all of the euler angles. + float fvBias[3]; -esp_err_t initAccelerometer(i2c_port_t _i2c_port, gpio_num_t sda, gpio_num_t scl, gpio_pullup_t pullup, uint32_t clkHz, - qma_range_t range, qma_bandwidth_t bandwidth); + // For debug + int lastreadr; + int32_t gyroaccum[3]; + uint32_t gyrocount; + int16_t gyrolast[3]; + int16_t accellast[3]; + float fCorrectLast[3]; +} __attribute((packed)) LSM6DSLData __attribute__((aligned(4))); + +extern LSM6DSLData LSM6DSL; + +esp_err_t initAccelerometer(i2c_port_t _i2c_port, gpio_num_t sda, gpio_num_t scl, gpio_pullup_t pullup, uint32_t clkHz ); esp_err_t deInitAccelerometer(void); -esp_err_t accelSetRange(qma_range_t range); esp_err_t accelGetAccelVec(int16_t* x, int16_t* y, int16_t* z); -esp_err_t accelGetStep(uint16_t* data); +esp_err_t accelGetQuaternion(float * q); +esp_err_t accelIntegrate(); + + +// Utility functions (to replace at a later time) + +float rsqrtf(float x); +float mathsqrtf(float x); +void mathEulerToQuat(float * q, const float * euler); +void mathQuatApply(float * qout, const float * q1, const float * q2); +void mathQuatNormalize(float * qout, const float * qin ); +void mathCrossProduct(float * p, const float * a, const float * b); +void mathRotateVectorByInverseOfQuaternion(float * pout, const float * q, const float * p ); +void mathRotateVectorByQuaternion(float * pout, const float * q, const float * p); +esp_err_t GeneralSet( int dev, int reg, int val ); +esp_err_t LSM6DSLSet( int reg, int val ); +int GeneralI2CGet( int device, int reg, uint8_t * data, int data_len ); +int ReadLSM6DSL( uint8_t * data, int data_len ); + #endif diff --git a/main/swadge2024.c b/main/swadge2024.c index f17d7c3fc..ac5be6c76 100644 --- a/main/swadge2024.c +++ b/main/swadge2024.c @@ -208,6 +208,8 @@ void app_main(void) // Read settings from NVS readAllSettings(); + ESP_LOGI( "test", "%p %p %p %p %p\n", &i2c_driver_delete, &LSM6DSLSet, &rsqrtf, &mathCrossProduct, &accelIntegrate ); + // If test mode was passed if (getTestModePassedSetting()) { @@ -308,7 +310,8 @@ void app_main(void) initAccelerometer(I2C_NUM_0, GPIO_NUM_3, // SDA GPIO_NUM_41, // SCL - GPIO_PULLUP_DISABLE, 1000000, QMA_RANGE_2G, QMA_BANDWIDTH_1024_HZ); + GPIO_PULLUP_ENABLE, 1000000 ); + accelIntegrate(); } // Init the temperature sensor @@ -665,4 +668,4 @@ bool checkButtonQueueWrapper(buttonEvt_t* evt) void setFrameRateUs(uint32_t newFrameRateUs) { frameRateUs = newFrameRateUs; -} \ No newline at end of file +} diff --git a/tools/sandbox_test/test_i2c/sandbox.c b/tools/sandbox_test/test_i2c/sandbox.c index d0cc2299c..e1bb23a62 100644 --- a/tools/sandbox_test/test_i2c/sandbox.c +++ b/tools/sandbox_test/test_i2c/sandbox.c @@ -20,11 +20,15 @@ #include "bunny.h" +#include "hdw-accel.h" + int16_t bunny_verts_out[ sizeof(bunny_verts)/3/2*3 ]; int frameno; int bQuit; +#if 0 + #define LSM6DSL_ADDRESS 0x6a #define QMC6308_ADDRESS 0x2c @@ -281,18 +285,83 @@ static int ReadLSM6DSL( uint8_t * data, int data_len ) return fifolen; } + + + + +#endif + + +#define LSM6DSL_ADDRESS 0x6a + +typedef enum __attribute__((packed)) +{ + LSM6DSL_FUNC_CFG_ACCESS = 0x01, + LSM6DSL_SENSOR_SYNC_TIME_FRAME = 0x04, + LSM6DSL_FIFO_CTRL1 = 0x06, + LSM6DSL_FIFO_CTRL2 = 0x07, + LSM6DSL_FIFO_CTRL3 = 0x08, + LSM6DSL_FIFO_CTRL4 = 0x09, + LSM6DSL_FIFO_CTRL5 = 0x0a, + LSM6DSL_ORIENT_CFG_G = 0x0b, + LSM6DSL_INT1_CTRL = 0x0d, + LSM6DSL_INT2_CTRL = 0x0e, + LMS6DS3_WHO_AM_I = 0x0f, + LSM6DSL_CTRL1_XL = 0x10, + LSM6DSL_CTRL2_G = 0x11, + LSM6DSL_CTRL3_C = 0x12, + LSM6DSL_CTRL4_C = 0x13, + LSM6DSL_CTRL5_C = 0x14, + LSM6DSL_CTRL6_C = 0x15, + LSM6DSL_CTRL7_G = 0x16, + LSM6DSL_CTRL8_XL = 0x17, + LSM6DSL_CTRL9_XL = 0x18, + LSM6DSL_CTRL10_C = 0x19, + LSM6DSL_MASTER_CONFIG = 0x1a, + LSM6DSL_WAKE_UP_SRC = 0x1b, + LSM6DSL_TAP_SRC = 0x1c, + LSM6DSL_D6D_SRC = 0x1d, + LSM6DSL_STATUS_REG = 0x1e, + LSM6DSL_OUT_TEMP_L = 0x20, + LSM6DSL_OUT_TEMP_H = 0x21, + LMS6DS3_OUTX_L_G = 0x22, + LMS6DS3_OUTX_H_G = 0x23, + LMS6DS3_OUTY_L_G = 0x24, + LMS6DS3_OUTY_H_G = 0x25, + LMS6DS3_OUTZ_L_G = 0x26, + LMS6DS3_OUTZ_H_G = 0x27, + LMS6DS3_OUTX_L_XL = 0x28, + LMS6DS3_OUTX_H_XL = 0x29, + LMS6DS3_OUTY_L_XL = 0x2a, + LMS6DS3_OUTY_H_XL = 0x2b, + LMS6DS3_OUTZ_L_XL = 0x2c, + LMS6DS3_OUTZ_H_XL = 0x2d, +} lsm6dslReg_t; +float rsqrtf(float x); +float mathsqrtf(float x); +void mathEulerToQuat(float * q, const float * euler); +void mathQuatApply(float * qout, const float * q1, const float * q2); +void mathQuatNormalize(float * qout, const float * qin ); +void mathCrossProduct(float * p, const float * a, const float * b); +void mathRotateVectorByInverseOfQuaternion(float * pout, const float * q, const float * p ); +void mathRotateVectorByQuaternion(float * pout, const float * q, const float * p); +esp_err_t LSM6DSLSet( int reg, int val ); +int GeneralI2CGet( int device, int reg, uint8_t * data, int data_len ); +int ReadLSM6DSL( uint8_t * data, int data_len ); + +#if 1 static void LSM6DSLIntegrate() { - struct LSM6DSLData * ld = &LSM6DSL; + LSM6DSLData * ld = &LSM6DSL; int16_t data[6*16]; // Get temperature sensor... Why? Yolo? int r = GeneralI2CGet( LSM6DSL_ADDRESS, 0x20, (uint8_t*)data, 2 ); + ESP_LOGI( "x", "rrl %d", r ); if( r < 0 ) return; if( r == 2 ) ld->temp = data[0]; int readr = ReadLSM6DSL( (uint8_t*)data, sizeof( data ) ); - int samp; int16_t * cdata = data; @@ -418,8 +487,6 @@ static void LSM6DSLIntegrate() // Magnitude of correction angle = inverse_sin( magntiude( axis_of_correction ) ); // We want to significantly reduce that. To mute any effect. - - // TODO which directon of frame of reference? Up relative to controller? OR controller relative to world? cdata += 6; } @@ -455,7 +522,10 @@ static void LSM6DSLIntegrate() ld->computetime = getCycleCount() - start; } +#endif + +#if 0 static void LMS6DS3Setup() { @@ -516,7 +586,7 @@ static void LMS6DS3Setup() ESP_LOGE( "LSM6DSL", "WHOAMI Failed (%02x), %d. Cannot start part.\n", who, r ); } } - +#endif int global_i = 100; menu_t * menu; @@ -561,6 +631,18 @@ void sandbox_main(void) loadWsg("kid0.wsg", &example_sprite, true); setFrameRateUs(0); + + + + + LSM6DSLSet( LSM6DSL_FIFO_CTRL5, (0b0101 << 3) | 0b110 ); // 208 Hz ODR + LSM6DSLSet( LSM6DSL_FIFO_CTRL3, 0b00001001 ); // Put both devices (Accel + Gyro) in FIFO. + LSM6DSLSet( LSM6DSL_CTRL1_XL, 0b01011001 ); // Setup accel (16 g's FS) + LSM6DSLSet( LSM6DSL_CTRL2_G, 0b01011100 ); // Setup gyro, 2000dps + LSM6DSLSet( LSM6DSL_CTRL4_C, 0x00 ); // Disable all filtering. + LSM6DSLSet( LSM6DSL_CTRL7_G, 0b00000000 ); // Setup gyro, not high performance mode = 0x80. High perf = 0x00 + LSM6DSLSet( LSM6DSL_FIFO_CTRL2, 0b00000000 ); //Temp not in fifo (Why no work?) + /* // Try to reinstall, just in case. i2c_config_t conf = { @@ -577,7 +659,7 @@ void sandbox_main(void) ESP_LOGI( "sandbox", "i2c_param_config=%d", i2c_param_config(I2C_NUM_0, &conf) ); ESP_LOGI( "sandbox", "i2c_driver_install=%d", i2c_driver_install(I2C_NUM_0, conf.mode, 0, 0, 0) ); */ - LMS6DS3Setup(); + //LMS6DS3Setup(); //GeneralSet( QMC6308_ADDRESS, 0x0b, 0x80 ); //GeneralSet( QMC6308_ADDRESS, 0x0b, 0x03 ); //GeneralSet( QMC6308_ADDRESS, 0x0a, 0x83 ); @@ -670,7 +752,10 @@ void sandbox_tick() } #endif +// accelIntegrate(); LSM6DSLIntegrate(); + + /* cts += sprintf( cts, "%ld %ld / %5d %5d %5d / %5d %5d %5d / %ld %ld %ld / %f %f %f %f", LSM6DSL.computetime, LSM6DSL.temp, @@ -738,8 +823,6 @@ void sandbox_tick() void sandboxBackgroundDrawCallback(int16_t x, int16_t y, int16_t w, int16_t h, int16_t up, int16_t upNum ) { - int i; - fillDisplayArea(x, y, x+w, y+h, 0 ); } @@ -748,7 +831,7 @@ swadgeMode_t sandbox_mode = { .modeName = "sandbox", .wifiMode = NO_WIFI, .overrideUsb = false, - .usesAccelerometer = false, + .usesAccelerometer = true, .usesThermometer = false, .fnEnterMode = sandbox_main, .fnExitMode = sandbox_exit, From a81cd4ecfbcda76bfde492185ea9fd2c3c7393c1 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sun, 15 Oct 2023 05:21:12 -0400 Subject: [PATCH 12/18] Functional testing on accel --- components/hdw-accel/hdw-accel.c | 78 +++-- components/hdw-accel/include/hdw-accel.h | 12 +- components/hdw-usb/advanced_usb_control.c | 7 +- tools/sandbox_test/test_i2c/sandbox.c | 379 +--------------------- 4 files changed, 67 insertions(+), 409 deletions(-) diff --git a/components/hdw-accel/hdw-accel.c b/components/hdw-accel/hdw-accel.c index 4f383c1ac..b2371d60b 100644 --- a/components/hdw-accel/hdw-accel.c +++ b/components/hdw-accel/hdw-accel.c @@ -118,7 +118,7 @@ esp_err_t accelIntegrate(); */ float rsqrtf(float x) { - typedef union { int32_t i; float f; } fiunion; + typedef union { int32_t i; float f; } fiunion; const float xhalf = 0.5f * x; fiunion i = { .f = x }; i.i = 0x5f375a86 - ( i.i >> 1 ); @@ -164,7 +164,7 @@ void mathEulerToQuat(float * q, const float * euler) float roll = euler[2]; float cr = cosf(pitch * 0.5); float sr = sinf(pitch * 0.5); // Pitch: About X - float cp = cosf(yaw * 0.5); + float cp = cosf(yaw * 0.5); float sp = sinf(yaw * 0.5); // Yaw: About Y float cy = cosf(roll * 0.5); float sy = sinf(roll * 0.5); // Roll: About Z @@ -338,11 +338,14 @@ int GeneralI2CGet( int device, int reg, uint8_t * data, int data_len ) i2c_master_stop(cmdHandle); esp_err_t err = i2c_master_cmd_begin(I2C_NUM_0, cmdHandle, 100); i2c_cmd_link_delete(cmdHandle); - if( err ) return -1; + if( err ) + { + ESP_LOGE( "accel", "Error on link: %d", err ); + return -1; + } else return data_len; } - /** * @brief Read the FIFO out of the LSM6DSL * @@ -365,9 +368,18 @@ int ReadLSM6DSL( uint8_t * data, int data_len ) i2c_cmd_link_delete(cmdHandle); if( err < 0 ) return -1; - if( fifolen == 0 ) return 0; + if( fifolen & 0x4000 ) + { + // reset fifo. + // If we overflow, and we don't do this, bad things happen. + LSM6DSLSet( LSM6DSL_FIFO_CTRL5, (0b0101 << 3) | 0b000 ); // Disable fifo + LSM6DSLSet( LSM6DSL_FIFO_CTRL5, (0b0101 << 3) | 0b110 ); // 208 Hz ODR + LSM6DSL.sampCount = 0; + return 0; + } - fifolen &= 0x3ff; + fifolen &= 0x7ff; + if( fifolen == 0 ) return 0; if( fifolen > data_len / 2 ) fifolen = data_len / 2; cmdHandle = i2c_cmd_link_create(); @@ -413,7 +425,7 @@ esp_err_t initAccelerometer(i2c_port_t _i2c_port, gpio_num_t sda, gpio_num_t scl memset( &LSM6DSL, 0, sizeof(LSM6DSL) ); LSM6DSL.fqQuat[0] = 1; LSM6DSL.fqQuatLast[0] = 1; - + LSM6DSL.sampCount = 0; do_retry: @@ -459,35 +471,36 @@ esp_err_t initAccelerometer(i2c_port_t _i2c_port, gpio_num_t sda, gpio_num_t scl int r = GeneralI2CGet( LSM6DSL_ADDRESS, LMS6DS3_WHO_AM_I, &who, 1 ); if( r != 1 || who != 0x6a ) { - ESP_LOGW( "LSM6DSL", "WHOAMI Failed (%02x), %d\n", who, r ); + ESP_LOGW( "accel", "WHOAMI Failed (%02x), %d", who, r ); if( retry++ < 10 ) goto do_retry; - ESP_LOGE( "LSM6DSL", "Init failed on 1" ); + ESP_LOGE( "accel", "Init failed on 1" ); return ESP_FAIL; } - ESP_LOGI( "LSM6DSL", "Init Start" ); + ESP_LOGI( "accel", "Init Start" ); + + LSM6DSLSet( LSM6DSL_FIFO_CTRL5, (0b0101 << 3) | 0b110 ); // 208 Hz ODR, Continuous mode. Bypass mode until trigger is deasserted, then Continuous mode. + LSM6DSLSet( LSM6DSL_FIFO_CTRL3, 0b00001001 ); // Put both devices (Accel + Gyro) in FIFO. + LSM6DSLSet( LSM6DSL_CTRL1_XL, 0b01011001 ); // Setup accel (16 g's FS) + LSM6DSLSet( LSM6DSL_CTRL2_G, 0b01011100 ); // Setup gyro, 2000dps + LSM6DSLSet( LSM6DSL_CTRL4_C, 0x00 ); // Disable all filtering. + LSM6DSLSet( LSM6DSL_CTRL7_G, 0b00000000 ); // Setup gyro, not high performance mode = 0x80. High perf = 0x00 + LSM6DSLSet( LSM6DSL_FIFO_CTRL2, 0b00000000 ); //Temp not in fifo (Why no work?) for( i = 0; i < 2; i++ ) { - LSM6DSLSet( LSM6DSL_FIFO_CTRL5, (0b0101 << 3) | 0b110 ); // 208 Hz ODR - LSM6DSLSet( LSM6DSL_FIFO_CTRL3, 0b00001001 ); // Put both devices (Accel + Gyro) in FIFO. - LSM6DSLSet( LSM6DSL_CTRL1_XL, 0b01011001 ); // Setup accel (16 g's FS) - LSM6DSLSet( LSM6DSL_CTRL2_G, 0b01011100 ); // Setup gyro, 2000dps - LSM6DSLSet( LSM6DSL_CTRL4_C, 0x00 ); // Disable all filtering. - LSM6DSLSet( LSM6DSL_CTRL7_G, 0b00000000 ); // Setup gyro, not high performance mode = 0x80. High perf = 0x00 - LSM6DSLSet( LSM6DSL_FIFO_CTRL2, 0b00000000 ); //Temp not in fifo (Why no work?) - - vTaskDelay( 5 ); - - if( accelIntegrate() != ESP_OK ) + vTaskDelay( 1 ); + int check = accelIntegrate(); + if( check != ESP_OK ) { - ESP_LOGI( "LSM6DSL", "Init Fault Retry" ); + ESP_LOGI( "accel", "Init Fault Retry" ); if( retry++ < 10 ) goto do_retry; - ESP_LOGI( "LSM6DSL", "Init failed on 2" ); + ESP_LOGI( "accel", "Init failed on 2" ); return ESP_FAIL; } + ESP_LOGI( "accel", "Check %d", check ); } - ESP_LOGI( "LSM6DSL", "Init Ok" ); + ESP_LOGI( "accel", "Init Ok" ); return ESP_OK; } @@ -519,7 +532,6 @@ esp_err_t accelIntegrate() if( r == 2 ) ld->temp = data[0]; int readr = ReadLSM6DSL( (uint8_t*)data, sizeof( data ) ); if( readr < 0 ) return readr; - int samp; int16_t * cdata = data; @@ -545,10 +557,10 @@ esp_err_t accelIntegrate() // euler_deltas[2] += 4; // We can sum rotations to understand the amount of counts in a full circle. + // Note: this is actually more of a debug mechanism. ld->gyroaccum[0] += euler_deltas[0]; ld->gyroaccum[1] += euler_deltas[1]; ld->gyroaccum[2] += euler_deltas[2]; - ld->gyrocount++; // STEP 1: Visually inspect the gyro values. // STEP 2: Integrate the gyro values, verify they are correct. @@ -559,7 +571,7 @@ esp_err_t accelIntegrate() // convert to radians. ( 2000.0f / 32768.0f / 208.0f * 2.0 * 3.14159f / 180.0f ); // Measured = 560,000 counts per scale (Measured by looking at sum) // Testing -> 3.14159 * 2.0 / 566000; - float fFudge = 1.1; //XXX TODO: Investigate. + float fFudge = 1.125; //XXX TODO: Investigate. float fScale = ( 2000.0f / 32768.0f / 208.0f * 2.0 * 3.14159f / 180.0f ) * fFudge; // STEP 3: Integrate gyro values into a quaternion. @@ -602,7 +614,9 @@ esp_err_t accelIntegrate() accel_up[1] *= accel_inverse_mag; accel_up[2] *= accel_inverse_mag; - //ESP_LOGI( "SB", "%ld %ld %ld", raw_up[0], raw_up[1], raw_up[2] ); + ld->fvLastAccelRaw[0] = accel_up[0]; + ld->fvLastAccelRaw[1] = accel_up[1]; + ld->fvLastAccelRaw[2] = accel_up[2]; // Step 6B: Next, compute what we think "up" should be from our point of view. We will use +Y Up. float what_we_think_is_up[3] = { 0, 1, 0 }; @@ -620,16 +634,17 @@ esp_err_t accelIntegrate() // First, we can compute what the drift values of our axes are, to anti-drift them. // If you do only this, you will always end up in an unstable oscillation. memcpy( ld->fCorrectLast, corrective_quaternion+1, 12 ); + // XXX TODO: We need to multiply by amount the accelerometer gives us assurance. ld->fvBias[0] += mathsqrtf(corrective_quaternion[1]) * 0.0000002; ld->fvBias[1] += mathsqrtf(corrective_quaternion[2]) * 0.0000002; ld->fvBias[2] += mathsqrtf(corrective_quaternion[3]) * 0.0000002; + float corrective_force = (ld->sampCount++ == 0) ? 0.5f : 0.0005f; // Second, we can apply a very small corrective tug. This helps prevent oscillation // about the correct answer. This acts sort of like a P term to a PID loop. // This is actually the **primary**, or fastest responding thing. - const float corrective_force = 0.001f; corrective_quaternion[1] *= corrective_force; corrective_quaternion[2] *= corrective_force; corrective_quaternion[3] *= corrective_force; @@ -639,11 +654,8 @@ esp_err_t accelIntegrate() - corrective_quaternion[1]*corrective_quaternion[1] - corrective_quaternion[2]*corrective_quaternion[2] - corrective_quaternion[3]*corrective_quaternion[3] ); -// ESP_LOGI( "x", "%f %f %f %f\n", corrective_quaternion[0], corrective_quaternion[1], corrective_quaternion[2], corrective_quaternion[3] ); - mathQuatApply( ld->fqQuat, ld->fqQuat, corrective_quaternion ); - // Magnitude of correction angle = inverse_sin( magntiude( axis_of_correction ) ); - // We want to significantly reduce that. To mute any effect. + mathQuatApply( ld->fqQuat, ld->fqQuat, corrective_quaternion ); cdata += 6; } diff --git a/components/hdw-accel/include/hdw-accel.h b/components/hdw-accel/include/hdw-accel.h index 9591675b8..87b14c037 100644 --- a/components/hdw-accel/include/hdw-accel.h +++ b/components/hdw-accel/include/hdw-accel.h @@ -53,20 +53,24 @@ typedef struct // Quats are wxyz. // You can take a vector, in controller space, rotate by this quat, and you get it in world space. - float fqQuatLast[4]; - float fqQuat[4]; // Quats are wxyz + float fqQuatLast[4]; // Delta + float fqQuat[4]; // Absolute + + // The last raw accelerometer (NOT FUSED) + float fvLastAccelRaw[3]; // Bias for all of the euler angles. float fvBias[3]; + uint32_t sampCount; + // For debug int lastreadr; int32_t gyroaccum[3]; - uint32_t gyrocount; int16_t gyrolast[3]; int16_t accellast[3]; float fCorrectLast[3]; -} __attribute((packed)) LSM6DSLData __attribute__((aligned(4))); +} LSM6DSLData; extern LSM6DSLData LSM6DSL; diff --git a/components/hdw-usb/advanced_usb_control.c b/components/hdw-usb/advanced_usb_control.c index 54f63af33..36795a711 100644 --- a/components/hdw-usb/advanced_usb_control.c +++ b/components/hdw-usb/advanced_usb_control.c @@ -22,7 +22,8 @@ // Logging can cause issues in operation, so by default it should remain off. #define ULOG(x...) // ESP_LOGI( "advanced_usb_control", x ) -#define AUPB_SIZE 2048 +// Size of printf buffer. +#define AUPB_SIZE 4096 //============================================================================== // Variables @@ -138,9 +139,9 @@ int handle_advanced_usb_terminal_get(uint8_t* data, int reqLen) int mark = 1; if (togo) { - if (togo > reqLen - 2) + if (togo > reqLen - 3) { - togo = reqLen - 2; + togo = reqLen - 3; } while (mark <= togo) { diff --git a/tools/sandbox_test/test_i2c/sandbox.c b/tools/sandbox_test/test_i2c/sandbox.c index e1bb23a62..66f0665ae 100644 --- a/tools/sandbox_test/test_i2c/sandbox.c +++ b/tools/sandbox_test/test_i2c/sandbox.c @@ -27,271 +27,6 @@ int16_t bunny_verts_out[ sizeof(bunny_verts)/3/2*3 ]; int frameno; int bQuit; -#if 0 - -#define LSM6DSL_ADDRESS 0x6a -#define QMC6308_ADDRESS 0x2c - -#define LSM6DSL_FUNC_CFG_ACCESS 0x01 -#define LSM6DSL_SENSOR_SYNC_TIME_FRAME 0x04 -#define LSM6DSL_FIFO_CTRL1 0x06 -#define LSM6DSL_FIFO_CTRL2 0x07 -#define LSM6DSL_FIFO_CTRL3 0x08 -#define LSM6DSL_FIFO_CTRL4 0x09 -#define LSM6DSL_FIFO_CTRL5 0x0a -#define LSM6DSL_ORIENT_CFG_G 0x0b -#define LSM6DSL_INT1_CTRL 0x0d -#define LSM6DSL_INT2_CTRL 0x0e -#define LMS6DS3_WHO_AM_I 0x0f -#define LSM6DSL_CTRL1_XL 0x10 -#define LSM6DSL_CTRL2_G 0x11 -#define LSM6DSL_CTRL3_C 0x12 -#define LSM6DSL_CTRL4_C 0x13 -#define LSM6DSL_CTRL5_C 0x14 -#define LSM6DSL_CTRL6_C 0x15 -#define LSM6DSL_CTRL7_G 0x16 -#define LSM6DSL_CTRL8_XL 0x17 -#define LSM6DSL_CTRL9_XL 0x18 -#define LSM6DSL_CTRL10_C 0x19 -#define LSM6DSL_MASTER_CONFIG 0x1a -#define LSM6DSL_WAKE_UP_SRC 0x1b -#define LSM6DSL_TAP_SRC 0x1c -#define LSM6DSL_D6D_SRC 0x1d -#define LSM6DSL_STATUS_REG 0x1e -#define LSM6DSL_OUT_TEMP_L 0x20 -#define LSM6DSL_OUT_TEMP_H 0x21 -#define LMS6DS3_OUTX_L_G 0x22 -#define LMS6DS3_OUTX_H_G 0x23 -#define LMS6DS3_OUTY_L_G 0x24 -#define LMS6DS3_OUTY_H_G 0x25 -#define LMS6DS3_OUTZ_L_G 0x26 -#define LMS6DS3_OUTZ_H_G 0x27 -#define LMS6DS3_OUTX_L_XL 0x28 -#define LMS6DS3_OUTX_H_XL 0x29 -#define LMS6DS3_OUTY_L_XL 0x2a -#define LMS6DS3_OUTY_H_XL 0x2b -#define LMS6DS3_OUTZ_L_XL 0x2c -#define LMS6DS3_OUTZ_H_XL 0x2d - - -struct LSM6DSLData -{ - int32_t temp; - uint32_t computetime; - - // Quats are wxyz. - // You can take a vector, in controller space, rotate by this quat, and you get it in world space. - float fqQuatLast[4]; - float fqQuat[4]; // Quats are wxyz - - // Bias for all of the euler angles. - float fvBias[3]; - - // For debug - int lastreadr; - int32_t gyroaccum[3]; - uint32_t gyrocount; - int16_t gyrolast[3]; - int16_t accellast[3]; - float fCorrectLast[3]; - -} LSM6DSL; - -#include - -/* Coordinate frame: - OpenGL / OpenVR / Godot / Etc... - - +X goes right. - +Y comes out top of controller. - +Z comes toward user (Into User's Eyes) -*/ - -float rsqrtf ( float x ) -{ - typedef union { int32_t i; float f; } fiunion; - const float xhalf = 0.5f * x; - fiunion i = { .f = x }; - - i.i = 0x5f375a86 - ( i.i >> 1 ); - x = i.f; - x = x * ( 1.5f - xhalf * x * x ); - x = x * ( 1.5f - xhalf * x * x ); - - return x; -} - -float mathsqrtf( float x ) -{ - // Trick to do approximate, fast square roots. (Though it is surprisingly fast) - int sign = x < 0; - if( sign ) x = -x; - if( x < 0.0000001 ) return 0.0001; - float o = x; - o = (o+x/o)/2; - o = (o+x/o)/2; - o = (o+x/o)/2; - o = (o+x/o)/2; - if( sign ) - return -o; - else - return o; -} - -void mathEulerToQuat( float * q, const float * euler ) -{ - float pitch = euler[0]; - float yaw = euler[1]; - float roll = euler[2]; - float cr = cosf(pitch * 0.5); - float sr = sinf(pitch * 0.5); // Pitch: About X - float cp = cosf(yaw * 0.5); - float sp = sinf(yaw * 0.5); // Yaw: About Y - float cy = cosf(roll * 0.5); - float sy = sinf(roll * 0.5); // Roll: About Z - q[0] = cr * cp * cy + sr * sp * sy; - q[1] = sr * cp * cy - cr * sp * sy; - q[2] = cr * sp * cy + sr * cp * sy; - q[3] = cr * cp * sy - sr * sp * cy; -} - -void mathQuatApply(float * qout, const float * q1, const float * q2) { - // NOTE: Does not normalize - float tmpw, tmpx, tmpy; - tmpw = (q1[0] * q2[0]) - (q1[1] * q2[1]) - (q1[2] * q2[2]) - (q1[3] * q2[3]); - tmpx = (q1[0] * q2[1]) + (q1[1] * q2[0]) + (q1[2] * q2[3]) - (q1[3] * q2[2]); - tmpy = (q1[0] * q2[2]) - (q1[1] * q2[3]) + (q1[2] * q2[0]) + (q1[3] * q2[1]); - qout[3] = (q1[0] * q2[3]) + (q1[1] * q2[2]) - (q1[2] * q2[1]) + (q1[3] * q2[0]); - qout[2] = tmpy; - qout[1] = tmpx; - qout[0] = tmpw; -} - -void mathQuatNormalize(float * qout, const float * qin ) -{ - float qmag = qin[0] * qin[0] + qin[1] * qin[1] + qin[2] * qin[2] + qin[3] * qin[3]; - qmag = rsqrtf( qmag ); - qout[0] = qin[0] * qmag; - qout[1] = qin[1] * qmag; - qout[2] = qin[2] * qmag; - qout[3] = qin[3] * qmag; -} - -void mathCrossProduct(float * p, const float * a, const float * b) -{ - float tx = a[1] * b[2] - a[2] * b[1]; - float ty = a[2] * b[0] - a[0] * b[2]; - p[2] = a[0] * b[1] - a[1] * b[0]; - p[1] = ty; - p[0] = tx; -} - -void mathRotateVectorByQuaternion(float * pout, const float * q, const float * p ) -{ - // return v + 2.0 * cross(q.xyz, cross(q.xyz, v) + q.w * v); - float iqo[3]; - mathCrossProduct( iqo, q + 1 /*.xyz*/, p ); - iqo[0] += q[0] * p[0]; - iqo[1] += q[0] * p[1]; - iqo[2] += q[0] * p[2]; - float ret[3]; - mathCrossProduct( ret, q + 1 /*.xyz*/, iqo ); - pout[0] = ret[0] * 2.0 + p[0]; - pout[1] = ret[1] * 2.0 + p[1]; - pout[2] = ret[2] * 2.0 + p[2]; -} - -void mathRotateVectorByInverseOfQuaternion(float * pout, const float * q, const float * p ) -{ - // General note: Performing a transform this way can be about 20-30% slower than a well formed 3x3 matrix. - // return v + 2.0 * cross(q.xyz, cross(q.xyz, v) + q.w * v); - float iqo[3]; - mathCrossProduct( iqo, p, q + 1 /*.xyz*/ ); - iqo[0] += q[0] * p[0]; - iqo[1] += q[0] * p[1]; - iqo[2] += q[0] * p[2]; - float ret[3]; - mathCrossProduct( ret, iqo, q + 1 /*.xyz*/ ); - pout[0] = ret[0] * 2.0 + p[0]; - pout[1] = ret[1] * 2.0 + p[1]; - pout[2] = ret[2] * 2.0 + p[2]; -} - -static esp_err_t GeneralSet( int dev, int reg, int val ) -{ - i2c_cmd_handle_t cmdHandle = i2c_cmd_link_create(); - i2c_master_start(cmdHandle); - i2c_master_write_byte(cmdHandle, dev << 1, false); - i2c_master_write_byte(cmdHandle, reg, false); - i2c_master_write_byte(cmdHandle, val, true); - i2c_master_stop(cmdHandle); - esp_err_t err = i2c_master_cmd_begin(I2C_NUM_0, cmdHandle, 100); - i2c_cmd_link_delete(cmdHandle); - return err; -} - -static esp_err_t LSM6DSLSet( int reg, int val ) -{ - return GeneralSet( LSM6DSL_ADDRESS, reg, val ); -} - -static int GeneralI2CGet( int device, int reg, uint8_t * data, int data_len ) -{ - i2c_cmd_handle_t cmdHandle = i2c_cmd_link_create(); - i2c_master_start(cmdHandle); - i2c_master_write_byte(cmdHandle, device << 1, false); - i2c_master_write_byte(cmdHandle, reg, false); - i2c_master_start(cmdHandle); - i2c_master_write_byte(cmdHandle, device << 1 | I2C_MASTER_READ, false); - i2c_master_read(cmdHandle, data, data_len, I2C_MASTER_LAST_NACK); - i2c_master_stop(cmdHandle); - esp_err_t err = i2c_master_cmd_begin(I2C_NUM_0, cmdHandle, 100); - i2c_cmd_link_delete(cmdHandle); - if( err ) return err; - else return data_len; -} - - -static int ReadLSM6DSL( uint8_t * data, int data_len ) -{ - i2c_cmd_handle_t cmdHandle = i2c_cmd_link_create(); - i2c_master_start(cmdHandle); - i2c_master_write_byte(cmdHandle, LSM6DSL_ADDRESS << 1, false); - i2c_master_write_byte(cmdHandle, 0x3A, false); - i2c_master_start(cmdHandle); - i2c_master_write_byte(cmdHandle, LSM6DSL_ADDRESS << 1 | I2C_MASTER_READ, false); - uint32_t fifolen = 0; - i2c_master_read(cmdHandle, (uint8_t*)&fifolen, 3, I2C_MASTER_LAST_NACK); - i2c_master_stop(cmdHandle); - esp_err_t err = i2c_master_cmd_begin(I2C_NUM_0, cmdHandle, 100); - i2c_cmd_link_delete(cmdHandle); - if( err < 0 ) return err; - - if( fifolen == 0 ) return 0; - - fifolen &= 0x3ff; - if( fifolen > data_len / 2 ) fifolen = data_len / 2; - - cmdHandle = i2c_cmd_link_create(); - i2c_master_start(cmdHandle); - i2c_master_write_byte(cmdHandle, LSM6DSL_ADDRESS << 1 | I2C_MASTER_READ, false); - i2c_master_read(cmdHandle, data, fifolen * 2, I2C_MASTER_LAST_NACK); - i2c_master_stop(cmdHandle); - err = i2c_master_cmd_begin(I2C_NUM_0, cmdHandle, 100); - - i2c_cmd_link_delete(cmdHandle); - if( err < 0 ) return err; - - return fifolen; -} - - - - - -#endif - - #define LSM6DSL_ADDRESS 0x6a typedef enum __attribute__((packed)) @@ -356,12 +91,12 @@ static void LSM6DSLIntegrate() int16_t data[6*16]; - // Get temperature sensor... Why? Yolo? + // Get temperature sensor (in case we ever want to use it) int r = GeneralI2CGet( LSM6DSL_ADDRESS, 0x20, (uint8_t*)data, 2 ); - ESP_LOGI( "x", "rrl %d", r ); if( r < 0 ) return; if( r == 2 ) ld->temp = data[0]; int readr = ReadLSM6DSL( (uint8_t*)data, sizeof( data ) ); + if( readr < 0 ) return; int samp; int16_t * cdata = data; @@ -387,10 +122,10 @@ static void LSM6DSLIntegrate() // euler_deltas[2] += 4; // We can sum rotations to understand the amount of counts in a full circle. + // Note: this is actually more of a debug mechanism. ld->gyroaccum[0] += euler_deltas[0]; ld->gyroaccum[1] += euler_deltas[1]; ld->gyroaccum[2] += euler_deltas[2]; - ld->gyrocount++; // STEP 1: Visually inspect the gyro values. // STEP 2: Integrate the gyro values, verify they are correct. @@ -401,7 +136,7 @@ static void LSM6DSLIntegrate() // convert to radians. ( 2000.0f / 32768.0f / 208.0f * 2.0 * 3.14159f / 180.0f ); // Measured = 560,000 counts per scale (Measured by looking at sum) // Testing -> 3.14159 * 2.0 / 566000; - float fFudge = 1.1; + float fFudge = 1.125; //XXX TODO: Investigate. float fScale = ( 2000.0f / 32768.0f / 208.0f * 2.0 * 3.14159f / 180.0f ) * fFudge; // STEP 3: Integrate gyro values into a quaternion. @@ -444,7 +179,6 @@ static void LSM6DSLIntegrate() accel_up[1] *= accel_inverse_mag; accel_up[2] *= accel_inverse_mag; - //ESP_LOGI( "SB", "%ld %ld %ld", raw_up[0], raw_up[1], raw_up[2] ); // Step 6B: Next, compute what we think "up" should be from our point of view. We will use +Y Up. float what_we_think_is_up[3] = { 0, 1, 0 }; @@ -467,11 +201,11 @@ static void LSM6DSLIntegrate() ld->fvBias[1] += mathsqrtf(corrective_quaternion[2]) * 0.0000002; ld->fvBias[2] += mathsqrtf(corrective_quaternion[3]) * 0.0000002; + float corrective_force = (ld->sampCount++ == 0) ? 0.5f : 0.0005f; // Second, we can apply a very small corrective tug. This helps prevent oscillation // about the correct answer. This acts sort of like a P term to a PID loop. // This is actually the **primary**, or fastest responding thing. - const float corrective_force = 0.001f; corrective_quaternion[1] *= corrective_force; corrective_quaternion[2] *= corrective_force; corrective_quaternion[3] *= corrective_force; @@ -481,7 +215,7 @@ static void LSM6DSLIntegrate() - corrective_quaternion[1]*corrective_quaternion[1] - corrective_quaternion[2]*corrective_quaternion[2] - corrective_quaternion[3]*corrective_quaternion[3] ); -// ESP_LOGI( "x", "%f %f %f %f\n", corrective_quaternion[0], corrective_quaternion[1], corrective_quaternion[2], corrective_quaternion[3] ); + mathQuatApply( ld->fqQuat, ld->fqQuat, corrective_quaternion ); // Magnitude of correction angle = inverse_sin( magntiude( axis_of_correction ) ); @@ -525,68 +259,6 @@ static void LSM6DSLIntegrate() #endif -#if 0 -static void LMS6DS3Setup() -{ - - // Shake any device off the bus. - int i; - int gpio_scl = 41; - for( i = 0; i < 16; i++ ) - { - gpio_matrix_out( gpio_scl, 256, 1, 0 ); - GPIO.out1_w1tc.val = (1<<(gpio_scl-32)); - esp_rom_delay_us(10); - gpio_matrix_out( gpio_scl, 256, 1, 0 ); - GPIO.out1_w1ts.val = (1<<(gpio_scl-32)); - esp_rom_delay_us(10); - } - gpio_matrix_out( gpio_scl, 29, 0, 0 ); - - esp_err_t ret_val = ESP_OK; - - i2c_driver_delete( I2C_NUM_0 ); - - - /* Install i2c driver */ - i2c_config_t conf = { - .mode = I2C_MODE_MASTER, - .sda_io_num = GPIO_NUM_3, - .sda_pullup_en = GPIO_PULLUP_ENABLE, - .scl_io_num = GPIO_NUM_41, - .scl_pullup_en = GPIO_PULLUP_ENABLE, - .master.clk_speed = 800000, //tested upto 1.4Mbit/s - .clk_flags = I2C_SCLK_SRC_FLAG_FOR_NOMAL, - }; - ESP_LOGI( "sandbox", "i2c_driver_install=%d", i2c_driver_install(I2C_NUM_0, conf.mode, 0, 0, 0) ); - ret_val |= i2c_param_config(I2C_NUM_0, &conf); - - - memset( &LSM6DSL, 0, sizeof(LSM6DSL) ); - LSM6DSL.fqQuat[0] = 1; - LSM6DSL.fqQuatLast[0] = 1; - - // Enable access - LSM6DSLSet( LSM6DSL_FUNC_CFG_ACCESS, 0x20 ); - LSM6DSLSet( LSM6DSL_CTRL3_C, 0x81 ); // Force reset - vTaskDelay( 1 ); - LSM6DSLSet( LSM6DSL_CTRL3_C, 0x44 ); // unforce reset - LSM6DSLSet( LSM6DSL_FIFO_CTRL5, (0b0101 << 3) | 0b110 ); // 208 Hz ODR - LSM6DSLSet( LSM6DSL_FIFO_CTRL3, 0b00001001 ); // Put both devices (Accel + Gyro) in FIFO. - LSM6DSLSet( LSM6DSL_CTRL1_XL, 0b01011001 ); // Setup accel (16 g's FS) - LSM6DSLSet( LSM6DSL_CTRL2_G, 0b01011100 ); // Setup gyro, 2000dps - LSM6DSLSet( LSM6DSL_CTRL4_C, 0x00 ); // Disable all filtering. - LSM6DSLSet( LSM6DSL_CTRL7_G, 0b00000000 ); // Setup gyro, not high performance mode = 0x80. High perf = 0x00 - LSM6DSLSet( LSM6DSL_FIFO_CTRL2, 0b00000000 ); //Temp not in fifo (Why no work?) - - uint8_t who = 0xaa; - int r = GeneralI2CGet( LSM6DSL_ADDRESS, LMS6DS3_WHO_AM_I, &who, 1 ); - if( r != 1 || who != 0x6a ) - { - ESP_LOGE( "LSM6DSL", "WHOAMI Failed (%02x), %d. Cannot start part.\n", who, r ); - } -} -#endif int global_i = 100; menu_t * menu; @@ -630,39 +302,9 @@ void sandbox_main(void) loadWsg("kid0.wsg", &example_sprite, true); - setFrameRateUs(0); - - - - - LSM6DSLSet( LSM6DSL_FIFO_CTRL5, (0b0101 << 3) | 0b110 ); // 208 Hz ODR - LSM6DSLSet( LSM6DSL_FIFO_CTRL3, 0b00001001 ); // Put both devices (Accel + Gyro) in FIFO. - LSM6DSLSet( LSM6DSL_CTRL1_XL, 0b01011001 ); // Setup accel (16 g's FS) - LSM6DSLSet( LSM6DSL_CTRL2_G, 0b01011100 ); // Setup gyro, 2000dps - LSM6DSLSet( LSM6DSL_CTRL4_C, 0x00 ); // Disable all filtering. - LSM6DSLSet( LSM6DSL_CTRL7_G, 0b00000000 ); // Setup gyro, not high performance mode = 0x80. High perf = 0x00 - LSM6DSLSet( LSM6DSL_FIFO_CTRL2, 0b00000000 ); //Temp not in fifo (Why no work?) + LSM6DSL.sampCount = 0; -/* - // Try to reinstall, just in case. - i2c_config_t conf = { - .mode = I2C_MODE_MASTER, - .sda_io_num = GPIO_NUM_3, - .sda_pullup_en = GPIO_PULLUP_DISABLE, - .scl_io_num = GPIO_NUM_41, - .scl_pullup_en = GPIO_PULLUP_DISABLE, - .master.clk_speed = 1000000, - .clk_flags = I2C_SCLK_SRC_FLAG_FOR_NOMAL, - }; - - i2c_driver_delete( I2C_NUM_0 ); - ESP_LOGI( "sandbox", "i2c_param_config=%d", i2c_param_config(I2C_NUM_0, &conf) ); - ESP_LOGI( "sandbox", "i2c_driver_install=%d", i2c_driver_install(I2C_NUM_0, conf.mode, 0, 0, 0) ); -*/ - //LMS6DS3Setup(); - //GeneralSet( QMC6308_ADDRESS, 0x0b, 0x80 ); - //GeneralSet( QMC6308_ADDRESS, 0x0b, 0x03 ); - //GeneralSet( QMC6308_ADDRESS, 0x0a, 0x83 ); + setFrameRateUs(0); ESP_LOGI( "sandbox", "Loaded" ); } @@ -752,9 +394,8 @@ void sandbox_tick() } #endif -// accelIntegrate(); - LSM6DSLIntegrate(); - + int r = accelIntegrate(); +// LSM6DSLIntegrate(); /* cts += sprintf( cts, "%ld %ld / %5d %5d %5d / %5d %5d %5d / %ld %ld %ld / %f %f %f %f", From 702bf96026aecdee440036e7f63bb480a20a94a5 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sun, 15 Oct 2023 20:13:12 -0400 Subject: [PATCH 13/18] Add accel cal to factory test mode + update accel test screen --- components/hdw-accel/CMakeLists.txt | 3 +- components/hdw-accel/hdw-accel.c | 884 ++++++++++++--------- components/hdw-accel/include/hdw-accel.h | 44 +- main/modes/accelTest/accelTest.c | 70 +- main/modes/accelTest/bunny.h | 948 +++++++++++++++++++++++ main/modes/factoryTest/factoryTest.c | 116 ++- main/swadge2024.c | 2 - main/utils/touchUtils.c | 4 +- tools/sandbox_test/test_i2c/sandbox.c | 96 ++- 9 files changed, 1725 insertions(+), 442 deletions(-) create mode 100644 main/modes/accelTest/bunny.h diff --git a/components/hdw-accel/CMakeLists.txt b/components/hdw-accel/CMakeLists.txt index 791564a37..642727a1d 100644 --- a/components/hdw-accel/CMakeLists.txt +++ b/components/hdw-accel/CMakeLists.txt @@ -1,3 +1,4 @@ idf_component_register(SRCS "hdw-accel.c" INCLUDE_DIRS "include" - REQUIRES driver) + REQUIRES driver + PRIV_REQUIRES hdw-nvs) diff --git a/components/hdw-accel/hdw-accel.c b/components/hdw-accel/hdw-accel.c index b2371d60b..1104255d5 100644 --- a/components/hdw-accel/hdw-accel.c +++ b/components/hdw-accel/hdw-accel.c @@ -12,6 +12,7 @@ #include "rom/gpio.h" #include "soc/i2c_reg.h" #include "soc/gpio_struct.h" +#include "hdw-nvs.h" //============================================================================== // Enums @@ -19,54 +20,54 @@ typedef enum __attribute__((packed)) { - LSM6DSL_FUNC_CFG_ACCESS = 0x01, - LSM6DSL_SENSOR_SYNC_TIME_FRAME = 0x04, - LSM6DSL_FIFO_CTRL1 = 0x06, - LSM6DSL_FIFO_CTRL2 = 0x07, - LSM6DSL_FIFO_CTRL3 = 0x08, - LSM6DSL_FIFO_CTRL4 = 0x09, - LSM6DSL_FIFO_CTRL5 = 0x0a, - LSM6DSL_ORIENT_CFG_G = 0x0b, - LSM6DSL_INT1_CTRL = 0x0d, - LSM6DSL_INT2_CTRL = 0x0e, - LMS6DS3_WHO_AM_I = 0x0f, - LSM6DSL_CTRL1_XL = 0x10, - LSM6DSL_CTRL2_G = 0x11, - LSM6DSL_CTRL3_C = 0x12, - LSM6DSL_CTRL4_C = 0x13, - LSM6DSL_CTRL5_C = 0x14, - LSM6DSL_CTRL6_C = 0x15, - LSM6DSL_CTRL7_G = 0x16, - LSM6DSL_CTRL8_XL = 0x17, - LSM6DSL_CTRL9_XL = 0x18, - LSM6DSL_CTRL10_C = 0x19, - LSM6DSL_MASTER_CONFIG = 0x1a, - LSM6DSL_WAKE_UP_SRC = 0x1b, - LSM6DSL_TAP_SRC = 0x1c, - LSM6DSL_D6D_SRC = 0x1d, - LSM6DSL_STATUS_REG = 0x1e, - LSM6DSL_OUT_TEMP_L = 0x20, - LSM6DSL_OUT_TEMP_H = 0x21, - LMS6DS3_OUTX_L_G = 0x22, - LMS6DS3_OUTX_H_G = 0x23, - LMS6DS3_OUTY_L_G = 0x24, - LMS6DS3_OUTY_H_G = 0x25, - LMS6DS3_OUTZ_L_G = 0x26, - LMS6DS3_OUTZ_H_G = 0x27, - LMS6DS3_OUTX_L_XL = 0x28, - LMS6DS3_OUTX_H_XL = 0x29, - LMS6DS3_OUTY_L_XL = 0x2a, - LMS6DS3_OUTY_H_XL = 0x2b, - LMS6DS3_OUTZ_L_XL = 0x2c, - LMS6DS3_OUTZ_H_XL = 0x2d, + LSM6DSL_FUNC_CFG_ACCESS = 0x01, + LSM6DSL_SENSOR_SYNC_TIME_FRAME = 0x04, + LSM6DSL_FIFO_CTRL1 = 0x06, + LSM6DSL_FIFO_CTRL2 = 0x07, + LSM6DSL_FIFO_CTRL3 = 0x08, + LSM6DSL_FIFO_CTRL4 = 0x09, + LSM6DSL_FIFO_CTRL5 = 0x0a, + LSM6DSL_ORIENT_CFG_G = 0x0b, + LSM6DSL_INT1_CTRL = 0x0d, + LSM6DSL_INT2_CTRL = 0x0e, + LMS6DS3_WHO_AM_I = 0x0f, + LSM6DSL_CTRL1_XL = 0x10, + LSM6DSL_CTRL2_G = 0x11, + LSM6DSL_CTRL3_C = 0x12, + LSM6DSL_CTRL4_C = 0x13, + LSM6DSL_CTRL5_C = 0x14, + LSM6DSL_CTRL6_C = 0x15, + LSM6DSL_CTRL7_G = 0x16, + LSM6DSL_CTRL8_XL = 0x17, + LSM6DSL_CTRL9_XL = 0x18, + LSM6DSL_CTRL10_C = 0x19, + LSM6DSL_MASTER_CONFIG = 0x1a, + LSM6DSL_WAKE_UP_SRC = 0x1b, + LSM6DSL_TAP_SRC = 0x1c, + LSM6DSL_D6D_SRC = 0x1d, + LSM6DSL_STATUS_REG = 0x1e, + LSM6DSL_OUT_TEMP_L = 0x20, + LSM6DSL_OUT_TEMP_H = 0x21, + LMS6DS3_OUTX_L_G = 0x22, + LMS6DS3_OUTX_H_G = 0x23, + LMS6DS3_OUTY_L_G = 0x24, + LMS6DS3_OUTY_H_G = 0x25, + LMS6DS3_OUTZ_L_G = 0x26, + LMS6DS3_OUTZ_H_G = 0x27, + LMS6DS3_OUTX_L_XL = 0x28, + LMS6DS3_OUTX_H_XL = 0x29, + LMS6DS3_OUTY_L_XL = 0x2a, + LMS6DS3_OUTY_H_XL = 0x2b, + LMS6DS3_OUTZ_L_XL = 0x2c, + LMS6DS3_OUTZ_H_XL = 0x2d, } lsm6dslReg_t; //============================================================================== // Defines //============================================================================== -#define LSM6DSL_ADDRESS 0x6a -#define QMC6308_ADDRESS 0x2c +#define LSM6DSL_ADDRESS 0x6a +#define QMC6308_ADDRESS 0x2c //============================================================================== // Variables @@ -102,8 +103,11 @@ int ReadLSM6DSL( uint8_t * data, int data_len ); esp_err_t initAccelerometer(i2c_port_t _i2c_port, gpio_num_t sda, gpio_num_t scl, gpio_pullup_t pullup, uint32_t clkHz ); esp_err_t deInitAccelerometer(void); esp_err_t accelGetAccelVec(int16_t* x, int16_t* y, int16_t* z); +esp_err_t accelGetOrientVec(int16_t* x, int16_t* y, int16_t* z); esp_err_t accelGetQuaternion(float * q); esp_err_t accelIntegrate(); +float accelGetStdDevInCal(); +void accelSetRegistersAndReset(); //============================================================================== // Utility Functions @@ -118,7 +122,7 @@ esp_err_t accelIntegrate(); */ float rsqrtf(float x) { - typedef union { int32_t i; float f; } fiunion; + typedef union { int32_t i; float f; } fiunion; const float xhalf = 0.5f * x; fiunion i = { .f = x }; i.i = 0x5f375a86 - ( i.i >> 1 ); @@ -136,19 +140,19 @@ float rsqrtf(float x) */ float mathsqrtf(float x) { - // Trick to do approximate, fast square roots. (Though it is surprisingly fast) - int sign = x < 0; - if( sign ) x = -x; - if( x < 0.0000001 ) return 0.0001; - float o = x; - o = (o+x/o)/2; - o = (o+x/o)/2; - o = (o+x/o)/2; - o = (o+x/o)/2; - if( sign ) - return -o; - else - return o; + // Trick to do approximate, fast square roots. (Though it is surprisingly fast) + int sign = x < 0; + if( sign ) x = -x; + if( x < 0.0000001 ) return 0.0001; + float o = x; + o = (o+x/o)/2; + o = (o+x/o)/2; + o = (o+x/o)/2; + o = (o+x/o)/2; + if( sign ) + return -o; + else + return o; } /** @@ -159,9 +163,9 @@ float mathsqrtf(float x) */ void mathEulerToQuat(float * q, const float * euler) { - float pitch = euler[0]; - float yaw = euler[1]; - float roll = euler[2]; + float pitch = euler[0]; + float yaw = euler[1]; + float roll = euler[2]; float cr = cosf(pitch * 0.5); float sr = sinf(pitch * 0.5); // Pitch: About X float cp = cosf(yaw * 0.5); @@ -183,15 +187,15 @@ void mathEulerToQuat(float * q, const float * euler) */ void mathQuatApply(float * qout, const float * q1, const float * q2) { - // NOTE: Does not normalize - you will need to normalize eventually. - float tmpw, tmpx, tmpy; - tmpw = (q1[0] * q2[0]) - (q1[1] * q2[1]) - (q1[2] * q2[2]) - (q1[3] * q2[3]); - tmpx = (q1[0] * q2[1]) + (q1[1] * q2[0]) + (q1[2] * q2[3]) - (q1[3] * q2[2]); - tmpy = (q1[0] * q2[2]) - (q1[1] * q2[3]) + (q1[2] * q2[0]) + (q1[3] * q2[1]); - qout[3] = (q1[0] * q2[3]) + (q1[1] * q2[2]) - (q1[2] * q2[1]) + (q1[3] * q2[0]); - qout[2] = tmpy; - qout[1] = tmpx; - qout[0] = tmpw; + // NOTE: Does not normalize - you will need to normalize eventually. + float tmpw, tmpx, tmpy; + tmpw = (q1[0] * q2[0]) - (q1[1] * q2[1]) - (q1[2] * q2[2]) - (q1[3] * q2[3]); + tmpx = (q1[0] * q2[1]) + (q1[1] * q2[0]) + (q1[2] * q2[3]) - (q1[3] * q2[2]); + tmpy = (q1[0] * q2[2]) - (q1[1] * q2[3]) + (q1[2] * q2[0]) + (q1[3] * q2[1]); + qout[3] = (q1[0] * q2[3]) + (q1[1] * q2[2]) - (q1[2] * q2[1]) + (q1[3] * q2[0]); + qout[2] = tmpy; + qout[1] = tmpx; + qout[0] = tmpw; } /** @@ -202,12 +206,12 @@ void mathQuatApply(float * qout, const float * q1, const float * q2) */ void mathQuatNormalize(float * qout, const float * qin ) { - float qmag = qin[0] * qin[0] + qin[1] * qin[1] + qin[2] * qin[2] + qin[3] * qin[3]; - qmag = rsqrtf( qmag ); - qout[0] = qin[0] * qmag; - qout[1] = qin[1] * qmag; - qout[2] = qin[2] * qmag; - qout[3] = qin[3] * qmag; + float qmag = qin[0] * qin[0] + qin[1] * qin[1] + qin[2] * qin[2] + qin[3] * qin[3]; + qmag = rsqrtf( qmag ); + qout[0] = qin[0] * qmag; + qout[1] = qin[1] * qmag; + qout[2] = qin[2] * qmag; + qout[3] = qin[3] * qmag; } @@ -220,11 +224,11 @@ void mathQuatNormalize(float * qout, const float * qin ) */ void mathCrossProduct(float * p, const float * a, const float * b) { - float tx = a[1] * b[2] - a[2] * b[1]; + float tx = a[1] * b[2] - a[2] * b[1]; float ty = a[2] * b[0] - a[0] * b[2]; p[2] = a[0] * b[1] - a[1] * b[0]; - p[1] = ty; - p[0] = tx; + p[1] = ty; + p[0] = tx; } /** @@ -236,17 +240,17 @@ void mathCrossProduct(float * p, const float * a, const float * b) */ void mathRotateVectorByQuaternion(float * pout, const float * q, const float * p) { - // return v + 2.0 * cross(q.xyz, cross(q.xyz, v) + q.w * v); - float iqo[3]; - mathCrossProduct( iqo, q + 1 /*.xyz*/, p ); - iqo[0] += q[0] * p[0]; - iqo[1] += q[0] * p[1]; - iqo[2] += q[0] * p[2]; - float ret[3]; - mathCrossProduct( ret, q + 1 /*.xyz*/, iqo ); - pout[0] = ret[0] * 2.0 + p[0]; - pout[1] = ret[1] * 2.0 + p[1]; - pout[2] = ret[2] * 2.0 + p[2]; + // return v + 2.0 * cross(q.xyz, cross(q.xyz, v) + q.w * v); + float iqo[3]; + mathCrossProduct( iqo, q + 1 /*.xyz*/, p ); + iqo[0] += q[0] * p[0]; + iqo[1] += q[0] * p[1]; + iqo[2] += q[0] * p[2]; + float ret[3]; + mathCrossProduct( ret, q + 1 /*.xyz*/, iqo ); + pout[0] = ret[0] * 2.0 + p[0]; + pout[1] = ret[1] * 2.0 + p[1]; + pout[2] = ret[2] * 2.0 + p[2]; } /** @@ -258,18 +262,18 @@ void mathRotateVectorByQuaternion(float * pout, const float * q, const float * p */ void mathRotateVectorByInverseOfQuaternion(float * pout, const float * q, const float * p) { - // General note: Performing a transform this way can be about 20-30% slower than a well formed 3x3 matrix. - // return v + 2.0 * cross(q.xyz, cross(q.xyz, v) + q.w * v); - float iqo[3]; - mathCrossProduct( iqo, p, q + 1 /*.xyz*/ ); - iqo[0] += q[0] * p[0]; - iqo[1] += q[0] * p[1]; - iqo[2] += q[0] * p[2]; - float ret[3]; - mathCrossProduct( ret, iqo, q + 1 /*.xyz*/ ); - pout[0] = ret[0] * 2.0 + p[0]; - pout[1] = ret[1] * 2.0 + p[1]; - pout[2] = ret[2] * 2.0 + p[2]; + // General note: Performing a transform this way can be about 20-30% slower than a well formed 3x3 matrix. + // return v + 2.0 * cross(q.xyz, cross(q.xyz, v) + q.w * v); + float iqo[3]; + mathCrossProduct( iqo, p, q + 1 /*.xyz*/ ); + iqo[0] += q[0] * p[0]; + iqo[1] += q[0] * p[1]; + iqo[2] += q[0] * p[2]; + float ret[3]; + mathCrossProduct( ret, iqo, q + 1 /*.xyz*/ ); + pout[0] = ret[0] * 2.0 + p[0]; + pout[1] = ret[1] * 2.0 + p[1]; + pout[2] = ret[2] * 2.0 + p[2]; } static inline uint32_t getCycleCount() @@ -302,7 +306,7 @@ esp_err_t GeneralSet( int dev, int reg, int val ) i2c_master_stop(cmdHandle); esp_err_t err = i2c_master_cmd_begin(I2C_NUM_0, cmdHandle, 100); i2c_cmd_link_delete(cmdHandle); - return err; + return err; } /** @@ -314,7 +318,7 @@ esp_err_t GeneralSet( int dev, int reg, int val ) */ esp_err_t LSM6DSLSet( int reg, int val ) { - return GeneralSet( LSM6DSL_ADDRESS, reg, val ); + return GeneralSet( LSM6DSL_ADDRESS, reg, val ); } /** @@ -338,12 +342,12 @@ int GeneralI2CGet( int device, int reg, uint8_t * data, int data_len ) i2c_master_stop(cmdHandle); esp_err_t err = i2c_master_cmd_begin(I2C_NUM_0, cmdHandle, 100); i2c_cmd_link_delete(cmdHandle); - if( err ) - { - ESP_LOGE( "accel", "Error on link: %d", err ); - return -1; - } - else return data_len; + if( err ) + { + ESP_LOGE( "accel", "Error on link: %d", err ); + return -1; + } + else return data_len; } /** @@ -361,28 +365,29 @@ int ReadLSM6DSL( uint8_t * data, int data_len ) i2c_master_write_byte(cmdHandle, 0x3A, false); i2c_master_start(cmdHandle); i2c_master_write_byte(cmdHandle, LSM6DSL_ADDRESS << 1 | I2C_MASTER_READ, false); - uint32_t fifolen = 0; + uint32_t fifolen = 0; i2c_master_read(cmdHandle, (uint8_t*)&fifolen, 3, I2C_MASTER_LAST_NACK); i2c_master_stop(cmdHandle); esp_err_t err = i2c_master_cmd_begin(I2C_NUM_0, cmdHandle, 100); i2c_cmd_link_delete(cmdHandle); - if( err < 0 ) return -1; - - if( fifolen & 0x4000 ) - { - // reset fifo. - // If we overflow, and we don't do this, bad things happen. - LSM6DSLSet( LSM6DSL_FIFO_CTRL5, (0b0101 << 3) | 0b000 ); // Disable fifo - LSM6DSLSet( LSM6DSL_FIFO_CTRL5, (0b0101 << 3) | 0b110 ); // 208 Hz ODR - LSM6DSL.sampCount = 0; - return 0; - } - - fifolen &= 0x7ff; - if( fifolen == 0 ) return 0; - if( fifolen > data_len / 2 ) fifolen = data_len / 2; - - cmdHandle = i2c_cmd_link_create(); + if( err < 0 ) return -1; + + // Is fifo overflow. + if( fifolen & 0x4000 ) + { + // reset fifo. + // If we overflow, and we don't do this, bad things happen. + LSM6DSLSet( LSM6DSL_FIFO_CTRL5, (0b0101 << 3) | 0b000 ); // Disable fifo + LSM6DSLSet( LSM6DSL_FIFO_CTRL5, (0b0101 << 3) | 0b110 ); // 208 Hz ODR + LSM6DSL.sampCount = 0; + return 0; + } + + fifolen &= 0x7ff; + if( fifolen == 0 ) return 0; + if( fifolen > data_len / 2 ) fifolen = data_len / 2; + + cmdHandle = i2c_cmd_link_create(); i2c_master_start(cmdHandle); i2c_master_write_byte(cmdHandle, LSM6DSL_ADDRESS << 1 | I2C_MASTER_READ, false); i2c_master_read(cmdHandle, data, fifolen * 2, I2C_MASTER_LAST_NACK); @@ -390,9 +395,9 @@ int ReadLSM6DSL( uint8_t * data, int data_len ) err = i2c_master_cmd_begin(I2C_NUM_0, cmdHandle, 100); i2c_cmd_link_delete(cmdHandle); - if( err < 0 ) return -2; + if( err < 0 ) return -2; - return fifolen; + return fifolen; } @@ -418,35 +423,30 @@ int ReadLSM6DSL( uint8_t * data, int data_len ) */ esp_err_t initAccelerometer(i2c_port_t _i2c_port, gpio_num_t sda, gpio_num_t scl, gpio_pullup_t pullup, uint32_t clkHz ) { - int retry = 0; + int retry = 0; i2c_port = _i2c_port; esp_err_t ret_val; - memset( &LSM6DSL, 0, sizeof(LSM6DSL) ); - LSM6DSL.fqQuat[0] = 1; - LSM6DSL.fqQuatLast[0] = 1; - LSM6DSL.sampCount = 0; - do_retry: - // Shake any device off the bus. - int i; - int gpio_scl = 41; - for( i = 0; i < 16; i++ ) - { - gpio_matrix_out( gpio_scl, 256, 1, 0 ); - GPIO.out1_w1tc.val = (1<<(gpio_scl-32)); - esp_rom_delay_us(10); - gpio_matrix_out( gpio_scl, 256, 1, 0 ); - GPIO.out1_w1ts.val = (1<<(gpio_scl-32)); - esp_rom_delay_us(10); - } - gpio_matrix_out( gpio_scl, 29, 0, 0 ); + // Shake any device off the bus. + int i; + int gpio_scl = 41; + for( i = 0; i < 16; i++ ) + { + gpio_matrix_out( gpio_scl, 256, 1, 0 ); + GPIO.out1_w1tc.val = (1<<(gpio_scl-32)); + esp_rom_delay_us(10); + gpio_matrix_out( gpio_scl, 256, 1, 0 ); + GPIO.out1_w1ts.val = (1<<(gpio_scl-32)); + esp_rom_delay_us(10); + } + gpio_matrix_out( gpio_scl, 29, 0, 0 ); - ret_val = ESP_OK; + ret_val = ESP_OK; - i2c_driver_delete( _i2c_port ); + i2c_driver_delete( _i2c_port ); /* Install i2c driver */ i2c_config_t conf = { @@ -458,50 +458,44 @@ esp_err_t initAccelerometer(i2c_port_t _i2c_port, gpio_num_t sda, gpio_num_t scl .master.clk_speed = clkHz, //tested upto 1.4Mbit/s .clk_flags = I2C_SCLK_SRC_FLAG_FOR_NOMAL, }; - ESP_LOGI( "accel", "i2c_driver_install=%d", i2c_driver_install(_i2c_port, conf.mode, 0, 0, 0) ); + ESP_LOGI( "accel", "i2c_driver_install=%d", i2c_driver_install(_i2c_port, conf.mode, 0, 0, 0) ); ret_val |= i2c_param_config(i2c_port, &conf); - // Enable access - LSM6DSLSet( LSM6DSL_FUNC_CFG_ACCESS, 0x20 ); - LSM6DSLSet( LSM6DSL_CTRL3_C, 0x81 ); // Force reset - vTaskDelay( 1 ); - LSM6DSLSet( LSM6DSL_CTRL3_C, 0x44 ); // unforce reset - - uint8_t who = 0xaa; - int r = GeneralI2CGet( LSM6DSL_ADDRESS, LMS6DS3_WHO_AM_I, &who, 1 ); - if( r != 1 || who != 0x6a ) - { - ESP_LOGW( "accel", "WHOAMI Failed (%02x), %d", who, r ); - if( retry++ < 10 ) goto do_retry; - ESP_LOGE( "accel", "Init failed on 1" ); - return ESP_FAIL; - } - ESP_LOGI( "accel", "Init Start" ); - - LSM6DSLSet( LSM6DSL_FIFO_CTRL5, (0b0101 << 3) | 0b110 ); // 208 Hz ODR, Continuous mode. Bypass mode until trigger is deasserted, then Continuous mode. - LSM6DSLSet( LSM6DSL_FIFO_CTRL3, 0b00001001 ); // Put both devices (Accel + Gyro) in FIFO. - LSM6DSLSet( LSM6DSL_CTRL1_XL, 0b01011001 ); // Setup accel (16 g's FS) - LSM6DSLSet( LSM6DSL_CTRL2_G, 0b01011100 ); // Setup gyro, 2000dps - LSM6DSLSet( LSM6DSL_CTRL4_C, 0x00 ); // Disable all filtering. - LSM6DSLSet( LSM6DSL_CTRL7_G, 0b00000000 ); // Setup gyro, not high performance mode = 0x80. High perf = 0x00 - LSM6DSLSet( LSM6DSL_FIFO_CTRL2, 0b00000000 ); //Temp not in fifo (Why no work?) - - for( i = 0; i < 2; i++ ) - { - vTaskDelay( 1 ); - int check = accelIntegrate(); - if( check != ESP_OK ) - { - ESP_LOGI( "accel", "Init Fault Retry" ); - if( retry++ < 10 ) goto do_retry; - ESP_LOGI( "accel", "Init failed on 2" ); - return ESP_FAIL; - } - ESP_LOGI( "accel", "Check %d", check ); - } - - ESP_LOGI( "accel", "Init Ok" ); - return ESP_OK; + // Enable access + LSM6DSLSet( LSM6DSL_FUNC_CFG_ACCESS, 0x20 ); + LSM6DSLSet( LSM6DSL_CTRL3_C, 0x81 ); // Force reset + vTaskDelay( 1 ); + LSM6DSLSet( LSM6DSL_CTRL3_C, 0x44 ); // unforce reset + + uint8_t who = 0xaa; + int r = GeneralI2CGet( LSM6DSL_ADDRESS, LMS6DS3_WHO_AM_I, &who, 1 ); + if( r != 1 || who != 0x6a ) + { + ESP_LOGW( "accel", "WHOAMI Failed (%02x), %d", who, r ); + if( retry++ < 10 ) goto do_retry; + ESP_LOGE( "accel", "Init failed on 1" ); + return ESP_FAIL; + } + ESP_LOGI( "accel", "Init Start" ); + + accelSetRegistersAndReset(); + + for( i = 0; i < 2; i++ ) + { + vTaskDelay( 1 ); + int check = accelIntegrate(); + if( check != ESP_OK ) + { + ESP_LOGI( "accel", "Init Fault Retry" ); + if( retry++ < 10 ) goto do_retry; + ESP_LOGI( "accel", "Init failed on 2" ); + return ESP_FAIL; + } + ESP_LOGI( "accel", "Check %d", check ); + } + + ESP_LOGI( "accel", "Init Ok" ); + return ESP_OK; } /** @@ -516,205 +510,361 @@ esp_err_t deInitAccelerometer(void) /** - * @brief Deinit the accelerometer (nothing to do) + * @brief Read all pending samples in IMU and perform a sensor fusion pass * * @return ESP_OK if successful, or nonzero if error. */ esp_err_t accelIntegrate() { - LSM6DSLData * ld = &LSM6DSL; + LSM6DSLData * ld = &LSM6DSL; - int16_t data[6*16]; + int16_t data[6*16]; - // Get temperature sensor (in case we ever want to use it) - int r = GeneralI2CGet( LSM6DSL_ADDRESS, 0x20, (uint8_t*)data, 2 ); - if( r < 0 ) return r; - if( r == 2 ) ld->temp = data[0]; - int readr = ReadLSM6DSL( (uint8_t*)data, sizeof( data ) ); - if( readr < 0 ) return readr; - int samp; - int16_t * cdata = data; + // Get temperature sensor (in case we ever want to use it) + int r = GeneralI2CGet( LSM6DSL_ADDRESS, 0x20, (uint8_t*)data, 2 ); + if( r < 0 ) return r; + if( r == 2 ) ld->temp = data[0]; + int readr = ReadLSM6DSL( (uint8_t*)data, sizeof( data ) ); + if( readr < 0 ) return readr; + int samp; + int16_t * cdata = data; uint32_t start = getCycleCount(); - // STEP 0: Decide your coordinate frame. + // STEP 0: Decide your coordinate frame. + + // [0] = +X axis coming out right of controller. + // [1] = +Y axis, pointing straight up out of controller, out where the USB port is. + // [2] = +Z axis, pointing up from the face of the controller. + + ld->lastreadr = readr; + + for( samp = 0; samp < readr; samp+=12 ) + { + // Extract data from IMU + int16_t * euler_deltas = cdata; // Euler angles, from gyro. + int16_t * accel_data = cdata + 3; + + // Manual cal, used only for Steps 2..8 + // euler_deltas[0] -= 12; + // euler_deltas[1] += 22; + // euler_deltas[2] += 4; + + // We can sum rotations to understand the amount of counts in a full circle. + // Note: this is actually more of a debug mechanism. + ld->gyroaccum[0] += euler_deltas[0]; + ld->gyroaccum[1] += euler_deltas[1]; + ld->gyroaccum[2] += euler_deltas[2]; + + // STEP 1: Visually inspect the gyro values. + // STEP 2: Integrate the gyro values, verify they are correct. + + // 2000 dps full-scale + // 32768 is full-scale + // 208 SPS + // convert to radians. ( 2000.0f / 32768.0f / 208.0f * 2.0 * 3.14159f / 180.0f ); + // Measured = 560,000 counts per scale (Measured by looking at sum) + // Testing -> 3.14159 * 2.0 / 566000; + float fFudge = 1.125/2.0; //XXX TODO: Investigate. + float fScale = ( 2000.0f / 32768.0f / 208.0f * 2.0 * 3.14159f / 180.0f ) * fFudge; + + // STEP 3: Integrate gyro values into a quaternion. + // This step is validated by working with just one axis at a time + // then apply a coordinate frame to ld->fqQuat and validate that it is + // correct. + float fEulerScales[3] = { + -fScale, + fScale, + -fScale }; + + float fEulers[3] = { + euler_deltas[0] * fEulerScales[0], + euler_deltas[1] * fEulerScales[1], + euler_deltas[2] * fEulerScales[2] }; + + // Used for calibration + if( ld->performCal ) + { + float diff[3] = { + fEulers[0] - ld->fvAverage[0], + fEulers[1] - ld->fvAverage[1], + fEulers[2] - ld->fvAverage[2] }; + + float diffsq[3] = { + (diff[0]<0)?-diff[0]:diff[0], + (diff[1]<0)?-diff[1]:diff[1], + (diff[2]<0)?-diff[2]:diff[2] }; + + diffsq[0] *= 1000.0; + diffsq[1] *= 1000.0; + diffsq[2] *= 1000.0; + + ld->fvDeviation[0] -= 0.004; + ld->fvDeviation[1] -= 0.004; + ld->fvDeviation[2] -= 0.004; + + if( ld->fvDeviation[0] < diffsq[0] ) ld->fvDeviation[0] = diffsq[0]; + if( ld->fvDeviation[1] < diffsq[1] ) ld->fvDeviation[1] = diffsq[1]; + if( ld->fvDeviation[2] < diffsq[2] ) ld->fvDeviation[2] = diffsq[2]; + + if( ld->fvDeviation[0] > 0.8 ) ld->fvDeviation[0] = 0.8; + if( ld->fvDeviation[1] > 0.8 ) ld->fvDeviation[1] = 0.8; + if( ld->fvDeviation[2] > 0.8 ) ld->fvDeviation[2] = 0.8; + + diff[0] *= mathsqrtf( ld->fvDeviation[0] ) * 0.5; + diff[1] *= mathsqrtf( ld->fvDeviation[1] ) * 0.5; + diff[2] *= mathsqrtf( ld->fvDeviation[2] ) * 0.5; + + ld->fvAverage[0] += diff[0]; + ld->fvAverage[1] += diff[1]; + ld->fvAverage[2] += diff[2]; + + // Compute the running RMS error. + float fvEuler = accelGetStdDevInCal(); + + if( fvEuler < 0.00015f ) + { + ld->fvBias[0] = -ld->fvAverage[0]; + ld->fvBias[1] = -ld->fvAverage[1]; + ld->fvBias[2] = -ld->fvAverage[2]; + writeNvs32( "gyrocalx", *(int32_t*)(&ld->fvBias[0]) ); + writeNvs32( "gyrocaly", *(int32_t*)(&ld->fvBias[1]) ); + writeNvs32( "gyrocalz", *(int32_t*)(&ld->fvBias[2]) ); + ld->performCal = 0; + ld->fvDeviation[0] = 0; + ld->fvDeviation[0] = 1; + ld->fvDeviation[0] = 2; + } + } + + fEulers[0] += ld->fvBias[0]; + fEulers[1] += ld->fvBias[1]; + fEulers[2] += ld->fvBias[2]; + + + mathEulerToQuat( ld->fqQuatLast, fEulers ); + mathQuatApply( ld->fqQuat, ld->fqQuat, ld->fqQuatLast ); + + // STEP 4: Validate yor values by doing 4 90 degree turns + // across multiple axes. + // i.e. rotate controller down, clockwise from top, up, counter-clockwise. + // while investigating quaternion. It should return to identity. + + // STEP 6: Determine our "error" based on accelerometer. + // NOTE: This step could be done on the inner loop if you want, and done over + // every accelerometer cycle, or it can be done on the outside, every few cycles. + // all that realy matters is that it is done periodically. + + // STEP 6A: Examine vectors. Generally speaking, we want an "up" vector, not a gravity vector. + // this is "up" in the controller's point of view. + float accel_up[3] = { + -accel_data[0], + accel_data[1], + -accel_data[2] + }; + + float accel_inverse_mag = rsqrtf( accel_up[0] * accel_up[0] + accel_up[1] * accel_up[1] + accel_up[2] * accel_up[2] ); + accel_up[0] *= accel_inverse_mag; + accel_up[1] *= accel_inverse_mag; + accel_up[2] *= accel_inverse_mag; + + ld->fvLastAccelRaw[0] = accel_up[0]; + ld->fvLastAccelRaw[1] = accel_up[1]; + ld->fvLastAccelRaw[2] = accel_up[2]; + + // Step 6B: Next, compute what we think "up" should be from our point of view. We will use +Y Up. + float what_we_think_is_up[3] = { 0, 1, 0 }; + mathRotateVectorByInverseOfQuaternion( what_we_think_is_up, LSM6DSL.fqQuat, what_we_think_is_up ); + + // Step 6C: Next, we determine how far off we are. This will tell us our error. + float corrective_quaternion[4]; + + // TRICKY: The ouput of this is actually the axis of rotation, which is ironically + // in vector-form the same as a quaternion. So we can write directly into the quat. + mathCrossProduct( corrective_quaternion + 1, accel_up, what_we_think_is_up ); + + // Now, we apply this in step 7. + + // First, we can compute what the drift values of our axes are, to anti-drift them. + // If you do only this, you will always end up in an unstable oscillation. + memcpy( ld->fCorrectLast, corrective_quaternion+1, 12 ); + + // XXX TODO: We need to multiply by amount the accelerometer gives us assurance. + ld->fvBias[0] += mathsqrtf(corrective_quaternion[1]) * 0.0000002; + ld->fvBias[1] += mathsqrtf(corrective_quaternion[2]) * 0.0000002; + ld->fvBias[2] += mathsqrtf(corrective_quaternion[3]) * 0.0000002; + + float corrective_force = (ld->sampCount++ == 0) ? 0.5f : 0.0005f; + + // Second, we can apply a very small corrective tug. This helps prevent oscillation + // about the correct answer. This acts sort of like a P term to a PID loop. + // This is actually the **primary**, or fastest responding thing. + corrective_quaternion[1] *= corrective_force; + corrective_quaternion[2] *= corrective_force; + corrective_quaternion[3] *= corrective_force; + + // x^2+y^2+z^2+q^2 -> ALGEBRA! -> sqrt( 1-x^2-y^2-z^2 ) = w + corrective_quaternion[0] = mathsqrtf( 1 + - corrective_quaternion[1]*corrective_quaternion[1] + - corrective_quaternion[2]*corrective_quaternion[2] + - corrective_quaternion[3]*corrective_quaternion[3] ); + + mathQuatApply( ld->fqQuat, ld->fqQuat, corrective_quaternion ); + + cdata += 6; + } + + // Now we move onto the outer loop. + // STEP 5: We now want to normalize the quat periodically. Don't do this too + // soon, otherwise you won't notice math errors. Realistically, this should + // only need to be done every hundreds of thousands of samples. + // + // Also, don't do this too often, otherwise you will reduce accuracy, + // unnecessarily. + float * qRot = ld->fqQuat; + float qmagsq = qRot[0] * qRot[0] + qRot[1] * qRot[1] + qRot[2] * qRot[2] + qRot[3] * qRot[3]; + if( qmagsq > 1.05 || qmagsq < 0.95 ) + { + // This normalizes everything. + qmagsq = rsqrtf( qmagsq ); + qRot[0] = qRot[0] * qmagsq; + qRot[1] = qRot[1] * qmagsq; + qRot[2] = qRot[2] * qmagsq; + qRot[3] = qRot[3] * qmagsq; + } + + + if( samp ) + { + ld->gyrolast[0] = cdata[-6]; + ld->gyrolast[1] = cdata[-5]; + ld->gyrolast[2] = cdata[-4]; + ld->accellast[0] = cdata[-3]; + ld->accellast[1] = cdata[-2]; + ld->accellast[2] = cdata[-1]; + } - // [0] = +X axis coming out right of controller. - // [1] = +Y axis, pointing straight up out of controller, out where the USB port is. - // [2] = +Z axis, pointing up from the face of the controller. + ld->computetime = getCycleCount() - start; - ld->lastreadr = readr; + return ESP_OK; +} - for( samp = 0; samp < readr; samp+=12 ) - { - // Extract data from IMU - int16_t * euler_deltas = cdata; // Euler angles, from gyro. - int16_t * accel_data = cdata + 3; - - // Manual cal, used only for Steps 2..8 - // euler_deltas[0] -= 12; - // euler_deltas[1] += 22; - // euler_deltas[2] += 4; - - // We can sum rotations to understand the amount of counts in a full circle. - // Note: this is actually more of a debug mechanism. - ld->gyroaccum[0] += euler_deltas[0]; - ld->gyroaccum[1] += euler_deltas[1]; - ld->gyroaccum[2] += euler_deltas[2]; - - // STEP 1: Visually inspect the gyro values. - // STEP 2: Integrate the gyro values, verify they are correct. - - // 2000 dps full-scale - // 32768 is full-scale - // 208 SPS - // convert to radians. ( 2000.0f / 32768.0f / 208.0f * 2.0 * 3.14159f / 180.0f ); - // Measured = 560,000 counts per scale (Measured by looking at sum) - // Testing -> 3.14159 * 2.0 / 566000; - float fFudge = 1.125; //XXX TODO: Investigate. - float fScale = ( 2000.0f / 32768.0f / 208.0f * 2.0 * 3.14159f / 180.0f ) * fFudge; - - // STEP 3: Integrate gyro values into a quaternion. - // This step is validated by working with just one axis at a time - // then apply a coordinate frame to ld->fqQuat and validate that it is - // correct. - float fEulerScales[3] = { - -fScale, - fScale, - -fScale }; - - float fEulers[3] = { - euler_deltas[0] * fEulerScales[0] + ld->fvBias[0], - euler_deltas[1] * fEulerScales[1] + ld->fvBias[1], - euler_deltas[2] * fEulerScales[2] + ld->fvBias[2] }; - - mathEulerToQuat( ld->fqQuatLast, fEulers ); - mathQuatApply( ld->fqQuat, ld->fqQuat, ld->fqQuatLast ); - - // STEP 4: Validate yor values by doing 4 90 degree turns - // across multiple axes. - // i.e. rotate controller down, clockwise from top, up, counter-clockwise. - // while investigating quaternion. It should return to identity. - - // STEP 6: Determine our "error" based on accelerometer. - // NOTE: This step could be done on the inner loop if you want, and done over - // every accelerometer cycle, or it can be done on the outside, every few cycles. - // all that realy matters is that it is done periodically. - - // STEP 6A: Examine vectors. Generally speaking, we want an "up" vector, not a gravity vector. - // this is "up" in the controller's point of view. - float accel_up[3] = { - -accel_data[0], - accel_data[1], - -accel_data[2] - }; - - float accel_inverse_mag = rsqrtf( accel_up[0] * accel_up[0] + accel_up[1] * accel_up[1] + accel_up[2] * accel_up[2] ); - accel_up[0] *= accel_inverse_mag; - accel_up[1] *= accel_inverse_mag; - accel_up[2] *= accel_inverse_mag; - - ld->fvLastAccelRaw[0] = accel_up[0]; - ld->fvLastAccelRaw[1] = accel_up[1]; - ld->fvLastAccelRaw[2] = accel_up[2]; - - // Step 6B: Next, compute what we think "up" should be from our point of view. We will use +Y Up. - float what_we_think_is_up[3] = { 0, 1, 0 }; - mathRotateVectorByInverseOfQuaternion( what_we_think_is_up, LSM6DSL.fqQuat, what_we_think_is_up ); - - // Step 6C: Next, we determine how far off we are. This will tell us our error. - float corrective_quaternion[4]; - - // TRICKY: The ouput of this is actually the axis of rotation, which is ironically - // in vector-form the same as a quaternion. So we can write directly into the quat. - mathCrossProduct( corrective_quaternion + 1, accel_up, what_we_think_is_up ); - - // Now, we apply this in step 7. - - // First, we can compute what the drift values of our axes are, to anti-drift them. - // If you do only this, you will always end up in an unstable oscillation. - memcpy( ld->fCorrectLast, corrective_quaternion+1, 12 ); - - // XXX TODO: We need to multiply by amount the accelerometer gives us assurance. - ld->fvBias[0] += mathsqrtf(corrective_quaternion[1]) * 0.0000002; - ld->fvBias[1] += mathsqrtf(corrective_quaternion[2]) * 0.0000002; - ld->fvBias[2] += mathsqrtf(corrective_quaternion[3]) * 0.0000002; - - float corrective_force = (ld->sampCount++ == 0) ? 0.5f : 0.0005f; - - // Second, we can apply a very small corrective tug. This helps prevent oscillation - // about the correct answer. This acts sort of like a P term to a PID loop. - // This is actually the **primary**, or fastest responding thing. - corrective_quaternion[1] *= corrective_force; - corrective_quaternion[2] *= corrective_force; - corrective_quaternion[3] *= corrective_force; - - // x^2+y^2+z^2+q^2 -> ALGEBRA! -> sqrt( 1-x^2-y^2-z^2 ) = w - corrective_quaternion[0] = mathsqrtf( 1 - - corrective_quaternion[1]*corrective_quaternion[1] - - corrective_quaternion[2]*corrective_quaternion[2] - - corrective_quaternion[3]*corrective_quaternion[3] ); - - mathQuatApply( ld->fqQuat, ld->fqQuat, corrective_quaternion ); - - cdata += 6; - } +/** + * @brief Get the RAW accelerometer "up" vector from device point of view. + * + * @return ESP_OK + */ +esp_err_t accelGetAccelVec(int16_t* x, int16_t* y, int16_t* z) +{ + *x = LSM6DSL.fvLastAccelRaw[0] * 256; + *y = LSM6DSL.fvLastAccelRaw[1] * 256; + *z = LSM6DSL.fvLastAccelRaw[2] * 256; + return ESP_OK; +} - // Now we move onto the outer loop. - // STEP 5: We now want to normalize the quat periodically. Don't do this too - // soon, otherwise you won't notice math errors. Realistically, this should - // only need to be done every hundreds of thousands of samples. - // - // Also, don't do this too often, otherwise you will reduce accuracy, - // unnecessarily. - float * qRot = ld->fqQuat; - float qmagsq = qRot[0] * qRot[0] + qRot[1] * qRot[1] + qRot[2] * qRot[2] + qRot[3] * qRot[3]; - if( qmagsq > 1.05 || qmagsq < 0.95 ) - { - // This normalizes everything. - qmagsq = rsqrtf( qmagsq ); - qRot[0] = qRot[0] * qmagsq; - qRot[1] = qRot[1] * qmagsq; - qRot[2] = qRot[2] * qmagsq; - qRot[3] = qRot[3] * qmagsq; - } +/** + * @brief Get the current orientation "up" vector from device point of view. + * + * @return ESP_OK + */ +esp_err_t accelGetOrientVec(int16_t* x, int16_t* y, int16_t* z) +{ + float plusy[3] = { 0, 1, 0 }; + mathRotateVectorByQuaternion( plusy, LSM6DSL.fqQuat, plusy ); + *x = plusy[0] * 256; + *y = plusy[1] * 256; + *z = plusy[2] * 256; + return ESP_OK; +} - if( samp ) - { - ld->gyrolast[0] = cdata[-6]; - ld->gyrolast[1] = cdata[-5]; - ld->gyrolast[2] = cdata[-4]; - ld->accellast[0] = cdata[-3]; - ld->accellast[1] = cdata[-2]; - ld->accellast[2] = cdata[-1]; - } +/** + * @brief Get the current estimated quaternion of the IMU. + * + * @return ESP_OK + */ +esp_err_t accelGetQuaternion(float * q) +{ + float * fq = LSM6DSL.fqQuat; + q[0] = fq[0]; + q[1] = fq[1]; + q[2] = fq[2]; + q[3] = fq[3]; + return ESP_OK; +} - ld->computetime = getCycleCount() - start; +/** + * @brief Initialize calibration of IMU. + * + * @return ESP_OK If cal start was OK. + */ +esp_err_t accelPerformCal() +{ + LSM6DSL.performCal = 1; + LSM6DSL.fvDeviation[0] = 1; + LSM6DSL.fvDeviation[1] = 1; + LSM6DSL.fvDeviation[2] = 1; + LSM6DSL.fvAverage[0] = 0; + LSM6DSL.fvAverage[1] = 0; + LSM6DSL.fvAverage[2] = 0; + + // Ignore failures here. + eraseNvsKey( "gyrocalx" ); + eraseNvsKey( "gyrocaly" ); + eraseNvsKey( "gyrocalz" ); - return ESP_OK; + return ESP_OK; } -esp_err_t accelGetAccelVec(int16_t* x, int16_t* y, int16_t* z) +/** + * @brief Get current divergence as the cal algorithm attempts to converge on a good cal. + * + * @return If in cal mode, will return current motion deviation. If out of cal mode, will return 0.0f + */ +float accelGetStdDevInCal() { - if( accelIntegrate() < 0 ) - return ESP_FAIL; - - float plusy[3] = { 0, 1, 0 }; - mathRotateVectorByQuaternion( plusy, LSM6DSL.fqQuat, plusy ); - *x = plusy[0] * 1023; - *y = plusy[1] * 1023; - *z = plusy[2] * 1023; - return ESP_OK; + if (LSM6DSL.performCal == 0) return 0.0f; + float * d = LSM6DSL.fvDeviation; + return (d[0] * d[0] + + d[1] * d[1] + + d[2] * d[2]); } -esp_err_t accelGetQuaternion(float * q) + + +/** + * @brief Reset the IMU's core registers. Should be done any time you are entering a mode with the IMU. + * + */ +void accelSetRegistersAndReset() { - float * fq = LSM6DSL.fqQuat; - q[0] = fq[0]; - q[1] = fq[1]; - q[2] = fq[2]; - q[3] = fq[3]; - return ESP_OK; + LSM6DSLSet( LSM6DSL_FIFO_CTRL5, (0b0101 << 3) | 0b000 ); // Reset FIFO + LSM6DSLSet( LSM6DSL_FIFO_CTRL5, (0b0101 << 3) | 0b110 ); // 208 Hz ODR, Continuous mode. If the FIFO is full, the new sample overwrites the older one. + LSM6DSLSet( LSM6DSL_FIFO_CTRL3, 0b00001001 ); // Put both devices (Accel + Gyro) in FIFO. + LSM6DSLSet( LSM6DSL_CTRL1_XL, 0b01011001 ); // Setup accel (16 g's FS) + LSM6DSLSet( LSM6DSL_CTRL2_G, 0b01011100 ); // Setup gyro, 2000dps + LSM6DSLSet( LSM6DSL_CTRL4_C, 0x00 ); // Disable all filtering. + LSM6DSLSet( LSM6DSL_CTRL7_G, 0b00000000 ); // Setup gyro, not high performance mode = 0x80. High perf = 0x00 + LSM6DSLSet( LSM6DSL_FIFO_CTRL2, 0b00000000 ); //Temp not in fifo (Why no work?) + + memset( &LSM6DSL, 0, sizeof(LSM6DSL) ); + LSM6DSL.fqQuat[0] = 1; + LSM6DSL.fqQuatLast[0] = 1; + LSM6DSL.sampCount = 0; + if( !readNvs32( "gyrocalx", (int32_t*)&LSM6DSL.fvBias[0] ) ) + { + LSM6DSL.performCal = 1; + LSM6DSL.fvBias[0] = 0; + } + if( !readNvs32( "gyrocaly", (int32_t*)&LSM6DSL.fvBias[1] ) ) + { + LSM6DSL.performCal = 1; + LSM6DSL.fvBias[1] = 0; + } + if( !readNvs32( "gyrocalz", (int32_t*)&LSM6DSL.fvBias[2] ) ) + { + LSM6DSL.performCal = 1; + LSM6DSL.fvBias[2] = 0; + } } diff --git a/components/hdw-accel/include/hdw-accel.h b/components/hdw-accel/include/hdw-accel.h index 87b14c037..288480d19 100644 --- a/components/hdw-accel/include/hdw-accel.h +++ b/components/hdw-accel/include/hdw-accel.h @@ -48,28 +48,33 @@ typedef struct { - int32_t temp; - uint32_t computetime; + int32_t temp; + uint32_t computetime; + uint32_t performCal; // 1 if expecting a zero cal. - // Quats are wxyz. - // You can take a vector, in controller space, rotate by this quat, and you get it in world space. - float fqQuatLast[4]; // Delta - float fqQuat[4]; // Absolute + // Quats are wxyz. + // You can take a vector, in controller space, rotate by this quat, and you get it in world space. + float fqQuatLast[4]; // Delta + float fqQuat[4]; // Absolute - // The last raw accelerometer (NOT FUSED) - float fvLastAccelRaw[3]; + // The last raw accelerometer (NOT FUSED) + float fvLastAccelRaw[3]; - // Bias for all of the euler angles. - float fvBias[3]; + // Bias for all of the euler angles. + float fvBias[3]; - uint32_t sampCount; + // Used for calibration + float fvDeviation[3]; + float fvAverage[3]; - // For debug - int lastreadr; - int32_t gyroaccum[3]; - int16_t gyrolast[3]; - int16_t accellast[3]; - float fCorrectLast[3]; + uint32_t sampCount; + + // For debug + int lastreadr; + int32_t gyroaccum[3]; + int16_t gyrolast[3]; + int16_t accellast[3]; + float fCorrectLast[3]; } LSM6DSLData; extern LSM6DSLData LSM6DSL; @@ -77,9 +82,12 @@ extern LSM6DSLData LSM6DSL; esp_err_t initAccelerometer(i2c_port_t _i2c_port, gpio_num_t sda, gpio_num_t scl, gpio_pullup_t pullup, uint32_t clkHz ); esp_err_t deInitAccelerometer(void); esp_err_t accelGetAccelVec(int16_t* x, int16_t* y, int16_t* z); +esp_err_t accelGetOrientVec(int16_t* x, int16_t* y, int16_t* z); esp_err_t accelGetQuaternion(float * q); esp_err_t accelIntegrate(); - +esp_err_t accelPerformCal(); +float accelGetStdDevInCal(); +void accelSetRegistersAndReset(); // Utility functions (to replace at a later time) diff --git a/main/modes/accelTest/accelTest.c b/main/modes/accelTest/accelTest.c index fb2b785ab..b56bb3507 100644 --- a/main/modes/accelTest/accelTest.c +++ b/main/modes/accelTest/accelTest.c @@ -19,6 +19,7 @@ #include "fill.h" #include "linked_list.h" #include "font.h" +#include "bunny.h" //============================================================================== // Defines @@ -99,6 +100,7 @@ static void accelTestExitMode(void); static void accelTestReset(void); static void accelTestSample(int16_t x, int16_t y, int16_t z); static void accelTestHandleInput(void); +static void accelDrawBunny(void); static void accelTestBackgroundDrawCallback(int16_t x, int16_t y, int16_t w, int16_t h, int16_t up, int16_t upNum); static void accelTestDrawGraph(void); @@ -160,6 +162,9 @@ static void accelTestEnterMode(void) // writeTextlabels doesn't get reset by accelTestReset(), so initialize that here accelTest->writeTextLabels = true; + // We shold go as fast as we can. + setFrameRateUs(0); + // Then, accelTestReset() takes care of everything else accelTestReset(); } @@ -203,6 +208,9 @@ static void accelTestMainLoop(int64_t elapsedUs) // Do update each loop accelTestHandleInput(); + // Draw the bunny + accelDrawBunny(); + // Draw the field accelTestDrawGraph(); } @@ -245,11 +253,60 @@ static void accelTestHandleInput(void) } } +/** + * @brief Draw the bunny + */ +static void accelDrawBunny(void) +{ + + // Produce a model matrix from a quaternion. + float plusx_out[3] = { 1, 0, 0 }; + float plusy_out[3] = { 0, 1, 0 }; + float plusz_out[3] = { 0, 0, 1 }; + + mathRotateVectorByQuaternion( plusy_out, LSM6DSL.fqQuat, plusy_out ); + mathRotateVectorByQuaternion( plusx_out, LSM6DSL.fqQuat, plusx_out ); + mathRotateVectorByQuaternion( plusz_out, LSM6DSL.fqQuat, plusz_out ); + + int16_t bunny_verts_out[ sizeof(bunny_verts)/3/2*3 ]; + int i, vertices = 0; + for( i = 0; i < sizeof(bunny_verts)/2; i+= 3 ) + { + // Performingthe transform this way is about 700us. + float bx = bunny_verts[i+2]; + float by = bunny_verts[i+1]; + float bz =-bunny_verts[i+0]; + float bunnyvert[3] = { + bx * plusx_out[0] + by * plusx_out[1] + bz * plusx_out[2], + bx * plusy_out[0] + by * plusy_out[1] + bz * plusy_out[2], + bx * plusz_out[0] + by * plusz_out[1] + bz * plusz_out[2] }; + bunny_verts_out[vertices*3+0] = bunnyvert[0]/250 + 280/2; + bunny_verts_out[vertices*3+1] = -bunnyvert[1]/250 + 240/2; // Convert from right-handed to left-handed coordinate frame. + bunny_verts_out[vertices*3+2] = bunnyvert[2]; + vertices++; + } + + int lines = 0; + for( i = 0; i < sizeof(bunny_lines); i+= 2 ) + { + int v1 = bunny_lines[i]*3; + int v2 = bunny_lines[i+1]*3; + float col = bunny_verts_out[v1+2]/2000 + 8; + if( col > 5 ) col = 5; + else if( col < 0 ) continue; + drawLineFast(bunny_verts_out[v1], bunny_verts_out[v1+1],bunny_verts_out[v2], bunny_verts_out[v2+1], col); + lines++; + } +} + + /** * @brief Reset the accelerometer test mode variables */ static void accelTestReset(void) { + accelSetRegistersAndReset(); + accelTest->first = 0; accelTest->last = 0; @@ -279,17 +336,8 @@ static void accelTestReset(void) */ static void accelTestBackgroundDrawCallback(int16_t x, int16_t y, int16_t w, int16_t h, int16_t up, int16_t upNum) { - // Use TURBO drawing mode to draw individual pixels fast - SETUP_FOR_TURBO(); - - // Blank the display - for (int16_t yp = y; yp < y + h; yp++) - { - for (int16_t xp = x; xp < x + w; xp++) - { - TURBO_SET_PIXEL(xp, yp, GRAPH_BG_COLOR); - } - } + accelIntegrate(); + fillDisplayArea(x, y, x+w, y+h, GRAPH_BG_COLOR ); } /** diff --git a/main/modes/accelTest/bunny.h b/main/modes/accelTest/bunny.h new file mode 100644 index 000000000..2197db363 --- /dev/null +++ b/main/modes/accelTest/bunny.h @@ -0,0 +1,948 @@ +#include +const int16_t bunny_verts[] = { + -18622, 11337, 3876, + -17108, 3601, 701, + -1011, 8368, 2344, + -1459, 6234, 7752, + -10384, -14050, 10578, + 9108, 9861, 4806, + -9196, 15608, 8497, + -16283, -6559, 2406, + -17417, -4365, 2474, + -18389, -1521, 4815, + 22219, -6826, 5279, + 23808, -7886, 4430, + -6203, 20422, -1965, + -12974, 18821, -4263, + -10629, 18285, -6951, + -10895, 20703, -13634, + -6764, -17568, -4534, + 902, 382, -7702, + -4497, -1374, -7109, + 5036, 9289, 6298, + 4426, 9208, -394, + -12239, -17581, -2460, + -18917, 5765, 7129, + -16378, -4102, 7845, + -2280, -12287, -3501, + -12167, 5531, -3406, + -17908, 15766, 4905, + -15097, 15808, 8499, + 9825, 797, 12888, + -18283, 10280, 791, + 11007, 9190, -244, + 11474, -579, -7581, + -12864, 16571, -195, + -14640, 16042, -1221, + -18332, 937, 2637, + -15354, 21703, -13553, + -7377, 15262, 2845, + -8799, 16541, 2581, + -19797, 6164, 4316, + -16158, 9222, 14006, + -14416, 12151, 13174, + -14141, 23469, -15211, + -11722, 23695, -17151, + -1964, -15383, 13753, + -7229, -8215, -4409, + -8680, -4539, -6585, + -6087, -5457, -5940, + -13259, 12437, -2907, + -10978, 15370, -1647, + -7396, -13948, 4297, + -7434, -11583, 4207, + -13163, 4985, 13937, + 1898, -3045, -11656, + 5657, -4100, -11268, + -3407, 15167, -771, + -17300, -2898, -778, + -10938, 12846, -2752, + -11874, 6626, 14013, + -10253, 21665, -18053, + 406, 21723, -6583, + -4073, 17099, -3927, + 19181, 786, 295, + 20580, -11662, -2457, + 22102, -13351, 369, + -12808, 13196, -5196, + 6063, 4006, 11507, + -7276, 12853, 114, + -12565, -2250, 11523, + -14217, 2765, 9542, + 5881, -11781, 14728, + 17536, -16747, 2519, + 18781, -2145, -2653, + -10697, -17317, 5038, + -11142, 4154, 9941, + 9481, -2448, 14961, + 8235, -10250, 14859, + -18197, 11232, 7365, + -19184, 6914, 11000, + -10944, 15814, -10459, + 3849, -13857, 12844, + -7194, 12657, 2472, + 11483, -11451, 13236, + 9840, -14518, 12029, + 23883, -9568, 1171, + -10820, 18420, -17126, + -9924, -1255, 12014, + 12198, 1609, 11989, + -10748, 15532, 9298, + -9741, -5600, 11373, + -13269, -6464, 9384, + 19845, -7054, 398, + 11278, -5139, 14319, + -6202, 16623, 1654, + -9097, -11092, 997, + 19002, -13005, 8517, + 4327, 20154, -7214, + 16851, 4385, 6126, + 17234, -11990, 8371, + -9161, 14173, -1062, + 12680, -17098, -1248, + -9040, -10189, -1993, + -13250, 21697, -15633, + -12137, 16307, -15587, + -7565, 5595, 8715, + -14057, 14807, -8716, + 5154, 1528, 11805, + -11515, -15866, -2486, + -6006, -6786, 10928, + -6082, -14740, -3845, + -8903, -15390, 12402, + -6839, -1601, 12095, + 21051, -9013, 7491, + 19199, -7770, 6398, + -4794, -14773, -7000, + -6012, 2917, 10643, + 5839, -6447, 15623, + 12814, 6195, 8839, + -13806, -17119, 795, + 13926, -16890, 6446, + -6942, -12919, 9554, + -3844, -13534, 10374, + -4457, 1559, 10787, + -18851, 14741, 3666, + -5487, 7874, 6174, + 3966, 23210, -9228, + 15092, -1954, -6602, + 16589, -4402, -5654, + -4916, -3395, 11258, + -3318, -1827, 11642, + -3552, 20709, -364, + -2107, -3341, 11219, + 3977, 6907, 9497, + -602, 2200, 11199, + -931, -11679, 9396, + -655, 23590, -2817, + 4846, -113, 14813, + -218, -281, 12015, + 22675, -7139, 849, + 10125, 7755, 8482, + 42, -14047, 14169, + 6394, 4546, -6382, + 1421, -12865, 12448, + 1947, 25502, -6345, + -7830, -12984, 6944, + -3380, -3774, -6830, + 21840, -10979, 7400, + -2985, -17516, -9333, + 308, -13721, -8558, + 579, -4080, 15241, + 1959, -8651, 14162, + -15752, 11553, -1543, + 16322, -1004, 10720, + 1282, 17758, -1900, + -4127, -17024, 11409, + 14526, -13115, -5876, + 12320, -14519, -6873, + 17448, -8568, -4231, + 17020, -11895, -2736, + 12869, -8659, 12529, + -467, -16936, 14591, + 16302, 2969, -3536, + -9403, -13394, 1276, + 22627, -13027, 5091, + -10928, 11196, 9917, + -10058, 7526, 11324, + 16770, -9731, 9964, + 18984, -721, 6457, + -1915, 3997, -5987, + 15735, -5467, 11114, + 848, 2358, -7139, + 3381, 21754, -6535, + 15097, -13331, 9377, + 11684, -16912, 11440, + 3494, 18714, -4534, + -8499, 12161, 7570, + -10517, -17147, 11966, + 17230, -15895, -966, + -1186, -13034, -7256, + -14490, -1096, -4663, + 10883, -17063, -6685, + -2943, -17367, 9110, + -17066, 3515, 5290, + -17896, 13182, 9407, + -18402, 4534, 10173, + 8336, 933, -6933, + -17574, 6887, -1239, + 5278, -17480, -7386, + -12685, -14815, 928, + -11989, -16758, 7220, + -94, -1429, -10547, + -15531, 4607, 12668, + 12598, -10979, -8259, + -5175, -17496, -8370, + 9509, -17363, -4797, + 7835, -733, -9933, + 20083, -6435, 3491, + -12583, 16811, 5585, + -10347, 16387, 415, + -5156, 9190, 3024, + -1162, -5218, -11018, + 15898, 5878, 927, + -17104, 635, 7228, + -15184, 18465, -8552, + 13851, 7498, 5806, + 12339, 1989, -6235, + -8783, -13504, -2022, + -7103, 9054, -1646, + -8929, -480, -6679, + -17555, 5593, 12647, + -8668, 3516, -5825, + -362, -10012, -9571, + 11355, 6507, -4159, + -13316, 15141, -12590, + 12642, -17080, 1813, + -14678, 14770, -2184, + -15414, -5951, -2011, + -12268, -6067, -4971, + -16604, -530, -2303, + -2216, 7063, -2848, + 4859, 7950, -3136, + 10975, -17013, 371, + -5388, 7670, -3722, + -4905, -17613, 6862, + 8484, -12836, -9001, + 9315, -6801, -9938, + 7941, -14924, -7560, + 8259, -9562, -9772, + 2547, -9033, -11113, + 4684, -13353, -9431, + -7524, -9388, 8770, + 10549, -17413, 5320, + 9878, -16766, 9250, + 2213, -17293, 11551, + 13056, -16797, 3233, + 11736, -17220, 5482, + -544, -17336, 11491, + -13043, -9710, 3903, + -8320, -17724, 3639, + -6497, -17425, 1846, + -3296, -17519, -6480, +}; +const uint8_t bunny_lines[] = { + 1, 217, + 19, 3, + 19, 2, + 46, 18, + 39, 51, + 194, 224, + 62, 63, + 71, 195, + 0, 214, + 180, 235, + 234, 118, + 234, 70, + 71, 156, + 71, 90, + 135, 115, + 88, 110, + 4, 119, + 86, 116, + 139, 43, + 2, 198, + 0, 150, + 131, 132, + 148, 115, + 171, 165, + 171, 82, + 163, 6, + 168, 81, + 168, 158, + 80, 54, + 69, 79, + 116, 203, + 116, 138, + 27, 182, + 69, 82, + 77, 39, + 209, 25, + 209, 207, + 212, 101, + 140, 169, + 204, 184, + 194, 189, + 194, 53, + 230, 234, + 8, 23, + 98, 206, + 20, 5, + 199, 52, + 155, 223, + 51, 57, + 226, 228, + 230, 172, + 230, 231, + 65, 138, + 230, 118, + 69, 81, + 79, 82, + 21, 72, + 3, 131, + 237, 238, + 123, 174, + 105, 116, + 107, 119, + 54, 60, + 121, 127, + 84, 15, + 139, 141, + 28, 74, + 142, 134, + 54, 152, + 111, 11, + 71, 160, + 57, 164, + 111, 10, + 109, 175, + 109, 153, + 144, 24, + 96, 151, + 64, 104, + 170, 95, + 84, 58, + 21, 237, + 159, 43, + 159, 153, + 179, 155, + 7, 23, + 38, 181, + 184, 194, + 184, 17, + 196, 32, + 175, 188, + 31, 204, + 203, 30, + 139, 133, + 44, 205, + 77, 208, + 39, 182, + 31, 184, + 211, 204, + 64, 212, + 122, 214, + 216, 45, + 204, 160, + 180, 153, + 61, 195, + 213, 233, + 213, 234, + 213, 70, + 135, 74, + 156, 90, + 7, 236, + 56, 98, + 105, 65, + 5, 19, + 10, 11, + 197, 98, + 25, 221, + 15, 14, + 62, 157, + 53, 227, + 65, 132, + 123, 103, + 107, 127, + 107, 128, + 65, 131, + 121, 132, + 29, 38, + 29, 185, + 76, 182, + 80, 174, + 170, 124, + 152, 173, + 80, 36, + 151, 91, + 79, 172, + 151, 168, + 153, 43, + 99, 213, + 113, 16, + 144, 18, + 37, 197, + 148, 149, + 109, 119, + 172, 231, + 8, 9, + 67, 85, + 183, 208, + 68, 183, + 115, 91, + 146, 225, + 41, 15, + 214, 150, + 13, 14, + 191, 223, + 191, 226, + 8, 55, + 40, 57, + 224, 53, + 226, 227, + 186, 225, + 112, 10, + 146, 147, + 136, 135, + 57, 163, + 21, 117, + 79, 139, + 216, 100, + 189, 144, + 130, 148, + 77, 183, + 38, 185, + 180, 175, + 146, 192, + 73, 114, + 142, 170, + 142, 124, + 183, 190, + 6, 174, + 54, 36, + 68, 201, + 35, 101, + 199, 144, + 47, 214, + 2, 218, + 195, 137, + 112, 195, + 46, 24, + 160, 30, + 150, 29, + 120, 139, + 43, 120, + 99, 70, + 127, 128, + 92, 134, + 81, 158, + 129, 92, + 19, 20, + 224, 227, + 228, 227, + 59, 124, + 162, 118, + 19, 138, + 159, 235, + 157, 90, + 41, 13, + 195, 90, + 68, 190, + 99, 62, + 59, 95, + 76, 0, + 238, 239, + 80, 206, + 87, 27, + 87, 196, + 135, 105, + 200, 96, + 203, 138, + 178, 209, + 178, 25, + 205, 106, + 210, 144, + 30, 211, + 16, 21, + 31, 125, + 153, 175, + 123, 198, + 63, 162, + 68, 22, + 124, 95, + 221, 2, + 102, 64, + 5, 203, + 122, 0, + 128, 132, + 128, 136, + 54, 173, + 91, 75, + 192, 147, + 148, 135, + 148, 136, + 154, 155, + 91, 158, + 218, 20, + 13, 35, + 27, 40, + 155, 191, + 225, 223, + 59, 60, + 102, 78, + 40, 87, + 236, 89, + 117, 106, + 179, 225, + 32, 197, + 181, 201, + 99, 176, + 197, 12, + 31, 194, + 120, 153, + 192, 113, + 204, 140, + 122, 76, + 122, 182, + 5, 138, + 115, 74, + 132, 105, + 111, 145, + 149, 115, + 161, 93, + 87, 163, + 24, 113, + 103, 114, + 69, 115, + 110, 121, + 139, 159, + 11, 137, + 67, 68, + 72, 49, + 215, 236, + 23, 236, + 72, 237, + 101, 102, + 21, 106, + 100, 215, + 213, 220, + 0, 38, + 185, 25, + 34, 9, + 39, 40, + 49, 50, + 155, 225, + 34, 217, + 223, 228, + 117, 72, + 101, 58, + 211, 140, + 90, 137, + 24, 177, + 211, 219, + 16, 192, + 233, 234, + 12, 142, + 27, 196, + 206, 198, + 32, 48, + 101, 42, + 187, 205, + 59, 12, + 1, 181, + 206, 2, + 49, 161, + 187, 161, + 72, 188, + 166, 151, + 176, 62, + 176, 63, + 72, 143, + 2, 123, + 136, 105, + 193, 239, + 173, 60, + 70, 162, + 114, 110, + 28, 105, + 83, 137, + 231, 232, + 25, 47, + 215, 55, + 161, 205, + 106, 187, + 215, 7, + 167, 209, + 44, 45, + 44, 46, + 45, 18, + 13, 48, + 52, 53, + 199, 210, + 140, 167, + 147, 113, + 104, 214, + 35, 33, + 61, 200, + 98, 60, + 91, 74, + 40, 182, + 155, 157, + 172, 118, + 193, 186, + 166, 165, + 91, 168, + 3, 121, + 100, 236, + 166, 112, + 193, 146, + 155, 99, + 57, 73, + 66, 60, + 156, 126, + 1, 185, + 71, 125, + 49, 187, + 137, 62, + 137, 10, + 6, 196, + 203, 200, + 28, 135, + 93, 49, + 102, 58, + 104, 212, + 217, 25, + 6, 37, + 6, 87, + 39, 57, + 64, 78, + 64, 56, + 68, 73, + 94, 111, + 111, 112, + 69, 149, + 107, 120, + 107, 130, + 136, 132, + 223, 226, + 94, 145, + 141, 149, + 145, 162, + 68, 51, + 164, 73, + 92, 170, + 164, 103, + 92, 142, + 83, 162, + 106, 16, + 99, 179, + 70, 176, + 54, 92, + 22, 181, + 22, 183, + 171, 118, + 52, 194, + 197, 196, + 197, 48, + 207, 178, + 23, 201, + 185, 150, + 101, 214, + 160, 211, + 52, 189, + 216, 178, + 207, 45, + 14, 48, + 13, 32, + 36, 92, + 36, 37, + 45, 46, + 191, 224, + 23, 67, + 67, 73, + 61, 166, + 88, 67, + 88, 89, + 23, 89, + 37, 129, + 16, 238, + 175, 72, + 73, 103, + 106, 108, + 86, 28, + 3, 123, + 172, 232, + 110, 127, + 114, 123, + 60, 95, + 114, 3, + 61, 96, + 75, 158, + 195, 166, + 79, 159, + 125, 160, + 219, 30, + 163, 164, + 193, 179, + 172, 171, + 118, 94, + 125, 204, + 102, 212, + 101, 202, + 181, 68, + 212, 214, + 62, 90, + 93, 100, + 10, 195, + 9, 201, + 228, 147, + 228, 210, + 227, 199, + 227, 210, + 50, 93, + 236, 50, + 236, 229, + 83, 11, + 85, 110, + 206, 221, + 180, 222, + 167, 207, + 29, 0, + 78, 56, + 69, 141, + 3, 132, + 128, 130, + 12, 129, + 129, 134, + 12, 134, + 133, 149, + 9, 23, + 78, 14, + 17, 189, + 156, 191, + 173, 95, + 239, 146, + 17, 140, + 202, 214, + 215, 178, + 215, 217, + 186, 146, + 214, 26, + 189, 199, + 205, 100, + 193, 220, + 62, 83, + 22, 76, + 76, 77, + 232, 159, + 232, 235, + 220, 179, + 93, 205, + 217, 55, + 17, 18, + 17, 144, + 9, 217, + 34, 201, + 34, 181, + 19, 131, + 70, 63, + 37, 12, + 86, 151, + 156, 157, + 145, 11, + 97, 118, + 119, 120, + 37, 92, + 120, 133, + 22, 77, + 130, 133, + 13, 33, + 113, 177, + 200, 160, + 126, 191, + 108, 205, + 13, 15, + 50, 229, + 99, 157, + 33, 202, + 142, 59, + 33, 214, + 194, 125, + 51, 73, + 81, 165, + 81, 171, + 187, 72, + 164, 123, + 97, 171, + 143, 188, + 11, 162, + 151, 165, + 36, 174, + 56, 14, + 114, 121, + 131, 138, + 6, 36, + 75, 81, + 116, 151, + 2, 3, + 66, 206, + 33, 26, + 33, 196, + 221, 209, + 196, 26, + 172, 159, + 185, 47, + 44, 216, + 44, 100, + 224, 226, + 178, 45, + 71, 61, + 70, 118, + 222, 237, + 222, 72, + 150, 47, + 91, 86, + 91, 28, + 37, 196, + 35, 202, + 96, 116, + 25, 206, + 190, 39, + 190, 208, + 187, 117, + 42, 58, + 210, 147, + 214, 64, + 1, 34, + 97, 165, + 18, 167, + 236, 93, + 102, 84, + 163, 174, + 92, 173, + 108, 16, + 18, 169, + 77, 182, + 4, 175, + 113, 108, + 119, 143, + 35, 41, + 143, 49, + 17, 194, + 192, 239, + 97, 111, + 97, 112, + 107, 110, + 86, 105, + 140, 184, + 121, 128, + 120, 130, + 143, 50, + 54, 66, + 64, 47, + 92, 152, + 40, 163, + 8, 215, + 20, 219, + 12, 98, + 66, 80, + 81, 82, + 89, 67, + 73, 85, + 66, 98, + 125, 224, + 85, 88, + 25, 56, + 73, 110, + 41, 101, + 24, 210, + 80, 198, + 107, 229, + 96, 203, + 200, 30, + 60, 12, + 149, 130, + 229, 119, + 177, 147, + 108, 24, + 78, 84, + 78, 15, + 154, 191, + 169, 17, + 71, 126, + 179, 186, + 51, 190, + 82, 172, + 170, 173, + 4, 188, + 167, 169, + 48, 56, + 4, 143, + 218, 221, + 75, 115, + 225, 228, + 30, 20, + 153, 235, + 69, 75, + 116, 65, + 80, 123, + 96, 166, + 89, 229, + 180, 72, + 7, 8, + 167, 219, + 1, 38, + 41, 42, + 125, 191, + 20, 2, + 177, 210, + 215, 216, + 167, 20, + 15, 42, + 143, 229, + 24, 44, + 24, 205, + 38, 22, + 39, 208, + 67, 201, + 18, 207, + 76, 38, + 157, 191, + 140, 219, + 157, 154, + 178, 217, + 26, 182, + 63, 83, + 162, 94, + 165, 168, + 164, 174, + 165, 112, + 15, 58, + 133, 141, + 141, 79, + 26, 122, + 160, 61, + 109, 120, + 125, 126, + 130, 136, + 225, 147, + 53, 199, + 16, 237, + 94, 97, + 16, 239, + 33, 32, + 88, 107, + 4, 109, + 88, 229, + 1, 25, + 99, 220, + 48, 98, + 167, 218, + 26, 27, + 5, 30, + 167, 221, + 144, 46, + 47, 56, + 9, 55, + 206, 56, +}; diff --git a/main/modes/factoryTest/factoryTest.c b/main/modes/factoryTest/factoryTest.c index 4ebf3de06..16cefdca0 100644 --- a/main/modes/factoryTest/factoryTest.c +++ b/main/modes/factoryTest/factoryTest.c @@ -25,9 +25,9 @@ #define AB_SEP 6 #define DPAD_SEP 15 -#define ACCEL_BAR_HEIGHT 8 -#define ACCEL_BAR_SEP 1 -#define MAX_ACCEL_BAR_W 60 +#define ACCEL_BAR_WIDTH 8 +#define ACCEL_BAR_CENT 150 +#define MAX_ACCEL_BAR_SIZE 40 #define TOUCHBAR_WIDTH 60 #define TOUCHBAR_HEIGHT 60 @@ -103,6 +103,11 @@ typedef struct int16_t x; int16_t y; int16_t z; + int16_t ox; + int16_t oy; + int16_t oz; + int32_t stability; + int32_t accelFlags; // LED led_t cLed; testLedState_t ledState; @@ -170,6 +175,9 @@ void testEnterMode(void) loadSong("stereo_test.sng", &test->song, false); bzrPlayBgm(&test->song, BZR_STEREO); + // Clear out accel setting. + accelPerformCal(); + // Set NVM to indicate test not passed yet // setTestModePassedSetting(false); } @@ -289,28 +297,59 @@ void testMainLoop(int64_t elapsedUs __attribute__((unused))) } // Plot X accel - int16_t barWidth = ((test->x + 256) * MAX_ACCEL_BAR_W) / 512; - if (barWidth >= 0) - { - fillDisplayArea(TFT_WIDTH - barWidth, barY, TFT_WIDTH, barY + ACCEL_BAR_HEIGHT, accelColor); - } - barY += (ACCEL_BAR_HEIGHT + ACCEL_BAR_SEP); + int16_t barDelta = ((test->x) * MAX_ACCEL_BAR_SIZE) / 256; + int16_t barDeltaInv = -barDelta; + if( barDelta < 0 ) barDelta = 0; + if( barDeltaInv < 0 ) barDeltaInv = 0; + accelColor = ((test->accelFlags & 0b1001) == 0b1001)?c050:c500; + fillDisplayArea(ACCEL_BAR_WIDTH, ACCEL_BAR_CENT - barDeltaInv, ACCEL_BAR_WIDTH*2, ACCEL_BAR_CENT + barDelta, accelColor); // Plot Y accel - barWidth = ((test->y + 256) * MAX_ACCEL_BAR_W) / 512; - if (barWidth >= 0) - { - fillDisplayArea(TFT_WIDTH - barWidth, barY, TFT_WIDTH, barY + ACCEL_BAR_HEIGHT, accelColor); - } - barY += (ACCEL_BAR_HEIGHT + ACCEL_BAR_SEP); + barDelta = ((test->y) * MAX_ACCEL_BAR_SIZE) / 256; + barDeltaInv = -barDelta; + if( barDelta < 0 ) barDelta = 0; + if( barDeltaInv < 0 ) barDeltaInv = 0; + accelColor = ((test->accelFlags & 0b10010) == 0b10010)?c050:c500; + fillDisplayArea(ACCEL_BAR_WIDTH*3, ACCEL_BAR_CENT - barDeltaInv, ACCEL_BAR_WIDTH*4, ACCEL_BAR_CENT + barDelta, accelColor); // Plot Z accel - barWidth = ((test->z + 256) * MAX_ACCEL_BAR_W) / 512; - if (barWidth >= 0) - { - fillDisplayArea(TFT_WIDTH - barWidth, barY, TFT_WIDTH, barY + ACCEL_BAR_HEIGHT, accelColor); - } - barY += (ACCEL_BAR_HEIGHT + ACCEL_BAR_SEP); + barDelta = ((test->z) * MAX_ACCEL_BAR_SIZE) / 256; + barDeltaInv = -barDelta; + if( barDelta < 0 ) barDelta = 0; + if( barDeltaInv < 0 ) barDeltaInv = 0; + accelColor = ((test->accelFlags & 0b100100) == 0b100100)?c050:c500; + fillDisplayArea(ACCEL_BAR_WIDTH*5, ACCEL_BAR_CENT - barDeltaInv, ACCEL_BAR_WIDTH*6, ACCEL_BAR_CENT + barDelta, accelColor); + + // Plot orient X + barDelta = ((test->ox) * MAX_ACCEL_BAR_SIZE) / 256; + barDeltaInv = -barDelta; + if( barDelta < 0 ) barDelta = 0; + if( barDeltaInv < 0 ) barDeltaInv = 0; + accelColor = ((test->accelFlags & 0b1001000000) == 0b1001000000)?c050:c500; + fillDisplayArea(ACCEL_BAR_WIDTH*7, ACCEL_BAR_CENT - barDeltaInv, ACCEL_BAR_WIDTH*8, ACCEL_BAR_CENT + barDelta, accelColor); + + // Plot orient Y accel + barDelta = ((test->oy) * MAX_ACCEL_BAR_SIZE) / 256; + barDeltaInv = -barDelta; + if( barDelta < 0 ) barDelta = 0; + if( barDeltaInv < 0 ) barDeltaInv = 0; + accelColor = ((test->accelFlags & 0b10010000000) == 0b10010000000)?c050:c500; + fillDisplayArea(ACCEL_BAR_WIDTH*9, ACCEL_BAR_CENT - barDeltaInv, ACCEL_BAR_WIDTH*10, ACCEL_BAR_CENT + barDelta, accelColor); + + // Plot orientZ accel + barDelta = ((test->oz) * MAX_ACCEL_BAR_SIZE) / 256; + barDeltaInv = -barDelta; + if( barDelta < 0 ) barDelta = 0; + if( barDeltaInv < 0 ) barDeltaInv = 0; + accelColor = ((test->accelFlags & 0b100100000000) == 0b100100000000)?c050:c500; + fillDisplayArea(ACCEL_BAR_WIDTH*11, ACCEL_BAR_CENT - barDeltaInv, ACCEL_BAR_WIDTH*12, ACCEL_BAR_CENT + barDelta, accelColor); + + + // Plot stability accel + barDelta = MAX_ACCEL_BAR_SIZE - (test->stability); + if( barDelta < -MAX_ACCEL_BAR_SIZE ) barDelta = -MAX_ACCEL_BAR_SIZE; + accelColor = ((test->accelFlags & 0b1000000000000) == 0b1000000000000)?c050:c500; + fillDisplayArea(ACCEL_BAR_WIDTH*13, ACCEL_BAR_CENT + barDelta, ACCEL_BAR_WIDTH*14, ACCEL_BAR_CENT + MAX_ACCEL_BAR_SIZE, accelColor); // Draw the 8-direction touchpad with center circle int16_t tBarX = TFT_WIDTH - TOUCHBAR_WIDTH; @@ -716,22 +755,43 @@ void testReadAndValidateAccelerometer(void) { // Read accel int16_t x, y, z; + int16_t ox, oy, oz; + + if( accelIntegrate() < 0 ) + return; + accelGetAccelVec(&x, &y, &z); + accelGetOrientVec(&ox, &oy, &oz); // Save accel values test->x = x; test->y = y; test->z = z; + test->ox = ox; + test->oy = oy; + test->oz = oz; + float fStab = accelGetStdDevInCal(); + test->stability = fStab * 200.0f; + + if (x > 180) test->accelFlags |= 1; + if (y > 180) test->accelFlags |= 2; + if (z > 180) test->accelFlags |= 4; + if (x <-180) test->accelFlags |= 8; + if (y <-180) test->accelFlags |= 16; + if (z <-180) test->accelFlags |= 32; + + if (ox > 180) test->accelFlags |= 64; + if (oy > 180) test->accelFlags |= 128; + if (oz > 180) test->accelFlags |= 256; + if (ox <-180) test->accelFlags |= 512; + if (oy <-180) test->accelFlags |= 1024; + if (oz <-180) test->accelFlags |= 2048; + if (fStab < 0.00001f ) test->accelFlags |= 4096; // Make sure all values are nonzero - if ((x != 0) && (y != 0) && (z != 0)) + if ( test->accelFlags == 0b1111111111111 ) { - // Make sure some value is shook - if ((x > 500) || (y > 500) || (z > 500)) - { - // Pass! - test->accelPassed = true; - } + test->accelPassed = true; } } diff --git a/main/swadge2024.c b/main/swadge2024.c index ac5be6c76..695970b90 100644 --- a/main/swadge2024.c +++ b/main/swadge2024.c @@ -208,8 +208,6 @@ void app_main(void) // Read settings from NVS readAllSettings(); - ESP_LOGI( "test", "%p %p %p %p %p\n", &i2c_driver_delete, &LSM6DSLSet, &rsqrtf, &mathCrossProduct, &accelIntegrate ); - // If test mode was passed if (getTestModePassedSetting()) { diff --git a/main/utils/touchUtils.c b/main/utils/touchUtils.c index b893341c8..1204fc614 100644 --- a/main/utils/touchUtils.c +++ b/main/utils/touchUtils.c @@ -48,7 +48,7 @@ touchJoystick_t getTouchJoystickZones(int32_t angle, int32_t radius, bool useCen int16_t offset = 360 - (360 / sectors) / 2; // Check if we're too close to the center, and return just TB_CENTER - if (useCenter && radius < 64) + if (useCenter && radius < 128) { // TODO is this an OK value for the "center pad" position? return TB_CENTER; @@ -169,4 +169,4 @@ void getTouchSpins(touchSpinState_t* state, int32_t angle, int32_t radius) state->lastRadius = radius; } } -} \ No newline at end of file +} diff --git a/tools/sandbox_test/test_i2c/sandbox.c b/tools/sandbox_test/test_i2c/sandbox.c index 66f0665ae..53e207726 100644 --- a/tools/sandbox_test/test_i2c/sandbox.c +++ b/tools/sandbox_test/test_i2c/sandbox.c @@ -84,7 +84,10 @@ esp_err_t LSM6DSLSet( int reg, int val ); int GeneralI2CGet( int device, int reg, uint8_t * data, int data_len ); int ReadLSM6DSL( uint8_t * data, int data_len ); -#if 1 +#if 0 + +// For main add ESP_LOGI( "test", "%p %p %p %p %p\n", &i2c_driver_delete, &LSM6DSLSet, &rsqrtf, &mathCrossProduct, &accelIntegrate ); + static void LSM6DSLIntegrate() { LSM6DSLData * ld = &LSM6DSL; @@ -117,9 +120,9 @@ static void LSM6DSLIntegrate() int16_t * accel_data = cdata + 3; // Manual cal, used only for Steps 2..8 - // euler_deltas[0] -= 12; - // euler_deltas[1] += 22; - // euler_deltas[2] += 4; + // euler_deltas[0] -= 12; + // euler_deltas[1] += 22; + // euler_deltas[2] += 4; // We can sum rotations to understand the amount of counts in a full circle. // Note: this is actually more of a debug mechanism. @@ -149,10 +152,68 @@ static void LSM6DSLIntegrate() -fScale }; float fEulers[3] = { - euler_deltas[0] * fEulerScales[0] + ld->fvBias[0], - euler_deltas[1] * fEulerScales[1] + ld->fvBias[1], - euler_deltas[2] * fEulerScales[2] + ld->fvBias[2] }; - + euler_deltas[0] * fEulerScales[0], + euler_deltas[1] * fEulerScales[1], + euler_deltas[2] * fEulerScales[2] }; + + // Used for calibration + if( ld->performCal ) + { + float diff[3] = { + fEulers[0] - ld->fvAverage[0], + fEulers[1] - ld->fvAverage[1], + fEulers[2] - ld->fvAverage[2] }; + + float diffsq[3] = { + (diff[0]<0)?-diff[0]:diff[0], + (diff[1]<0)?-diff[1]:diff[1], + (diff[2]<0)?-diff[2]:diff[2] }; + + diffsq[0] *= 1000.0; + diffsq[1] *= 1000.0; + diffsq[2] *= 1000.0; + + ld->fvDeviation[0] -= 0.004; + ld->fvDeviation[1] -= 0.004; + ld->fvDeviation[2] -= 0.004; + + if( ld->fvDeviation[0] < diffsq[0] ) ld->fvDeviation[0] = diffsq[0]; + if( ld->fvDeviation[1] < diffsq[1] ) ld->fvDeviation[1] = diffsq[1]; + if( ld->fvDeviation[2] < diffsq[2] ) ld->fvDeviation[2] = diffsq[2]; + + if( ld->fvDeviation[0] > 0.8 ) ld->fvDeviation[0] = 0.8; + if( ld->fvDeviation[1] > 0.8 ) ld->fvDeviation[1] = 0.8; + if( ld->fvDeviation[2] > 0.8 ) ld->fvDeviation[2] = 0.8; + + diff[0] *= mathsqrtf( ld->fvDeviation[0] ) * 0.5; + diff[1] *= mathsqrtf( ld->fvDeviation[1] ) * 0.5; + diff[2] *= mathsqrtf( ld->fvDeviation[2] ) * 0.5; + + ld->fvAverage[0] += diff[0]; + ld->fvAverage[1] += diff[1]; + ld->fvAverage[2] += diff[2]; + + // Compute the running RMS error. + float fvEuler = ( + ld->fvDeviation[0] * ld->fvDeviation[0] + + ld->fvDeviation[1] * ld->fvDeviation[1] + + ld->fvDeviation[2] * ld->fvDeviation[2] ); + + if( fvEuler < 0.00015f ) + { + ld->fvBias[0] = -ld->fvAverage[0]; + ld->fvBias[1] = -ld->fvAverage[1]; + ld->fvBias[2] = -ld->fvAverage[2]; + writeNvs32( "gyrocalx", *(int32_t*)(&ld->fvBias[0]) ); + writeNvs32( "gyrocaly", *(int32_t*)(&ld->fvBias[1]) ); + writeNvs32( "gyrocalz", *(int32_t*)(&ld->fvBias[2]) ); + ld->performCal = 0; + } + } + + fEulers[0] += ld->fvBias[0]; + fEulers[1] += ld->fvBias[1]; + fEulers[2] += ld->fvBias[2]; mathEulerToQuat( ld->fqQuatLast, fEulers ); mathQuatApply( ld->fqQuat, ld->fqQuat, ld->fqQuatLast ); @@ -197,9 +258,9 @@ static void LSM6DSLIntegrate() // If you do only this, you will always end up in an unstable oscillation. memcpy( ld->fCorrectLast, corrective_quaternion+1, 12 ); // XXX TODO: We need to multiply by amount the accelerometer gives us assurance. - ld->fvBias[0] += mathsqrtf(corrective_quaternion[1]) * 0.0000002; - ld->fvBias[1] += mathsqrtf(corrective_quaternion[2]) * 0.0000002; - ld->fvBias[2] += mathsqrtf(corrective_quaternion[3]) * 0.0000002; + //ld->fvBias[0] += mathsqrtf(corrective_quaternion[1]) * 0.0000002; + //ld->fvBias[1] += mathsqrtf(corrective_quaternion[2]) * 0.0000002; + //ld->fvBias[2] += mathsqrtf(corrective_quaternion[3]) * 0.0000002; float corrective_force = (ld->sampCount++ == 0) ? 0.5f : 0.0005f; @@ -302,7 +363,7 @@ void sandbox_main(void) loadWsg("kid0.wsg", &example_sprite, true); - LSM6DSL.sampCount = 0; + accelSetRegistersAndReset(); setFrameRateUs(0); @@ -394,7 +455,7 @@ void sandbox_tick() } #endif - int r = accelIntegrate(); + accelIntegrate(); // LSM6DSLIntegrate(); /* @@ -452,6 +513,13 @@ void sandbox_tick() uint32_t renderTime = getCycleCount() - cycStart; + ESP_LOGI( "I2C", "%d %d %f %f %f %f %f %f %f %f %f", + (int)LSM6DSL.sampCount, (int)LSM6DSL.performCal, + LSM6DSL.fvBias[0], LSM6DSL.fvBias[1], LSM6DSL.fvBias[2], + LSM6DSL.fvDeviation[0], LSM6DSL.fvDeviation[1], LSM6DSL.fvDeviation[2], + LSM6DSL.fvAverage[0], LSM6DSL.fvAverage[1], LSM6DSL.fvAverage[2] ); + +/* cts += sprintf( cts, "%ld %ld %d %f %f %f / %f %f %f / %3d %3d %3d / %4d %4d %4d", LSM6DSL.computetime, renderTime, LSM6DSL.lastreadr, LSM6DSL.fCorrectLast[0], LSM6DSL.fCorrectLast[1], LSM6DSL.fCorrectLast[2], @@ -460,10 +528,12 @@ void sandbox_tick() LSM6DSL.accellast[0], LSM6DSL.accellast[1], LSM6DSL.accellast[2] ); ESP_LOGI( "I2C", "%s", ctsbuffer ); +*/ } void sandboxBackgroundDrawCallback(int16_t x, int16_t y, int16_t w, int16_t h, int16_t up, int16_t upNum ) { + accelIntegrate(); fillDisplayArea(x, y, x+w, y+h, 0 ); } From 018fff2e6afdc6d38ecace54df6ee409dcdc7a01 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sun, 15 Oct 2023 20:45:28 -0400 Subject: [PATCH 14/18] Working with pong + improved definition for orientation. --- components/hdw-accel/hdw-accel.c | 26 +++-- components/hdw-accel/include/hdw-accel.h | 10 +- emulator/src/components/hdw-accel/hdw-accel.c | 95 ++++++++++++++----- emulator/src/extensions/replay/ext_replay.c | 2 +- main/modes/accelTest/accelTest.c | 2 +- main/modes/factoryTest/factoryTest.c | 28 +++--- main/modes/gamepad/gamepad.c | 4 +- main/modes/pong/pong.c | 6 +- 8 files changed, 117 insertions(+), 56 deletions(-) diff --git a/components/hdw-accel/hdw-accel.c b/components/hdw-accel/hdw-accel.c index 1104255d5..404073ce6 100644 --- a/components/hdw-accel/hdw-accel.c +++ b/components/hdw-accel/hdw-accel.c @@ -102,12 +102,12 @@ int ReadLSM6DSL( uint8_t * data, int data_len ); esp_err_t initAccelerometer(i2c_port_t _i2c_port, gpio_num_t sda, gpio_num_t scl, gpio_pullup_t pullup, uint32_t clkHz ); esp_err_t deInitAccelerometer(void); -esp_err_t accelGetAccelVec(int16_t* x, int16_t* y, int16_t* z); +esp_err_t accelGetAccelVecRaw(int16_t* x, int16_t* y, int16_t* z); esp_err_t accelGetOrientVec(int16_t* x, int16_t* y, int16_t* z); esp_err_t accelGetQuaternion(float * q); -esp_err_t accelIntegrate(); -float accelGetStdDevInCal(); -void accelSetRegistersAndReset(); +esp_err_t accelIntegrate(void); +float accelGetStdDevInCal(void); +void accelSetRegistersAndReset(void); //============================================================================== // Utility Functions @@ -627,9 +627,15 @@ esp_err_t accelIntegrate() ld->fvBias[0] = -ld->fvAverage[0]; ld->fvBias[1] = -ld->fvAverage[1]; ld->fvBias[2] = -ld->fvAverage[2]; - writeNvs32( "gyrocalx", *(int32_t*)(&ld->fvBias[0]) ); - writeNvs32( "gyrocaly", *(int32_t*)(&ld->fvBias[1]) ); - writeNvs32( "gyrocalz", *(int32_t*)(&ld->fvBias[2]) ); + struct fiunion { union { int32_t i; float f; } u; }; + struct fiunion x, y, z; + x.u.f = ld->fvBias[0]; + y.u.f = ld->fvBias[1]; + z.u.f = ld->fvBias[2]; + writeNvs32( "gyrocalx", x.u.i ); + writeNvs32( "gyrocaly", y.u.i ); + writeNvs32( "gyrocalz", z.u.i ); + ld->performCal = 0; ld->fvDeviation[0] = 0; ld->fvDeviation[0] = 1; @@ -754,7 +760,7 @@ esp_err_t accelIntegrate() * * @return ESP_OK */ -esp_err_t accelGetAccelVec(int16_t* x, int16_t* y, int16_t* z) +esp_err_t accelGetAccelVecRaw(int16_t* x, int16_t* y, int16_t* z) { *x = LSM6DSL.fvLastAccelRaw[0] * 256; *y = LSM6DSL.fvLastAccelRaw[1] * 256; @@ -770,7 +776,7 @@ esp_err_t accelGetAccelVec(int16_t* x, int16_t* y, int16_t* z) esp_err_t accelGetOrientVec(int16_t* x, int16_t* y, int16_t* z) { float plusy[3] = { 0, 1, 0 }; - mathRotateVectorByQuaternion( plusy, LSM6DSL.fqQuat, plusy ); + mathRotateVectorByInverseOfQuaternion( plusy, LSM6DSL.fqQuat, plusy ); *x = plusy[0] * 256; *y = plusy[1] * 256; *z = plusy[2] * 256; @@ -836,7 +842,7 @@ float accelGetStdDevInCal() * @brief Reset the IMU's core registers. Should be done any time you are entering a mode with the IMU. * */ -void accelSetRegistersAndReset() +void accelSetRegistersAndReset( void ) { LSM6DSLSet( LSM6DSL_FIFO_CTRL5, (0b0101 << 3) | 0b000 ); // Reset FIFO LSM6DSLSet( LSM6DSL_FIFO_CTRL5, (0b0101 << 3) | 0b110 ); // 208 Hz ODR, Continuous mode. If the FIFO is full, the new sample overwrites the older one. diff --git a/components/hdw-accel/include/hdw-accel.h b/components/hdw-accel/include/hdw-accel.h index 288480d19..1ef687f77 100644 --- a/components/hdw-accel/include/hdw-accel.h +++ b/components/hdw-accel/include/hdw-accel.h @@ -81,13 +81,13 @@ extern LSM6DSLData LSM6DSL; esp_err_t initAccelerometer(i2c_port_t _i2c_port, gpio_num_t sda, gpio_num_t scl, gpio_pullup_t pullup, uint32_t clkHz ); esp_err_t deInitAccelerometer(void); -esp_err_t accelGetAccelVec(int16_t* x, int16_t* y, int16_t* z); +esp_err_t accelGetAccelVecRaw(int16_t* x, int16_t* y, int16_t* z); esp_err_t accelGetOrientVec(int16_t* x, int16_t* y, int16_t* z); esp_err_t accelGetQuaternion(float * q); -esp_err_t accelIntegrate(); -esp_err_t accelPerformCal(); -float accelGetStdDevInCal(); -void accelSetRegistersAndReset(); +esp_err_t accelIntegrate(void); +esp_err_t accelPerformCal(void); +float accelGetStdDevInCal(void); +void accelSetRegistersAndReset(void); // Utility functions (to replace at a later time) diff --git a/emulator/src/components/hdw-accel/hdw-accel.c b/emulator/src/components/hdw-accel/hdw-accel.c index 2e42ceb4b..218aab012 100644 --- a/emulator/src/components/hdw-accel/hdw-accel.c +++ b/emulator/src/components/hdw-accel/hdw-accel.c @@ -9,7 +9,7 @@ #include "emu_args.h" #include "macros.h" -#define ONE_G 242 +#define ONE_G 256 #define ACCEL_MIN -512 #define ACCEL_MAX 512 @@ -19,6 +19,8 @@ static int16_t _accelX = 0; static int16_t _accelY = 0; static int16_t _accelZ = 0; +LSM6DSLData LSM6DSL; + /** * @brief Initialize the accelerometer * @@ -27,8 +29,7 @@ static int16_t _accelZ = 0; * @param bandwidth The bandwidth to measure at, between ::QMA_BANDWIDTH_128_HZ and ::QMA_BANDWIDTH_1024_HZ * @return ESP_OK if the accelerometer initialized, or a non-zero value if it did not */ -esp_err_t initAccelerometer(i2c_port_t _i2c_port, gpio_num_t sda, gpio_num_t scl, gpio_pullup_t pullup, uint32_t clkHz, - qma_range_t range, qma_bandwidth_t bandwidth) +esp_err_t initAccelerometer(i2c_port_t _i2c_port, gpio_num_t sda, gpio_num_t scl, gpio_pullup_t pullup, uint32_t clkHz ) { // Default to the swadge sitting still, face-up on a table somewhere on earth _accelX = 0; @@ -72,24 +73,6 @@ esp_err_t accelGetStep(uint16_t* data) } } -/** - * @brief Set the accelerometer's measurement range - * - * @param range The range to measure, from ::QMA_RANGE_2G to ::QMA_RANGE_32G - * @return ESP_OK if the range was set, or a non-zero value if it was not - */ -esp_err_t accelSetRange(qma_range_t range) -{ - if (accelInit) - { - return ESP_OK; - } - else - { - return ESP_ERR_INVALID_STATE; - } -} - /** * @brief Read the current acceleration vector from the accelerometer and return * the vector through arguments. If the read fails, the last known values are @@ -100,7 +83,7 @@ esp_err_t accelSetRange(qma_range_t range) * @param z The Z component of the acceleration vector is written here * @return ESP_OK if the acceleration was read, or a non-zero value if it was not */ -esp_err_t accelGetAccelVec(int16_t* x, int16_t* y, int16_t* z) +esp_err_t accelGetAccelVecRaw(int16_t* x, int16_t* y, int16_t* z) { if (accelInit) { @@ -116,6 +99,11 @@ esp_err_t accelGetAccelVec(int16_t* x, int16_t* y, int16_t* z) } } +esp_err_t accelGetOrientVec(int16_t* x, int16_t* y, int16_t* z) +{ + return accelGetAccelVecRaw( x, y, z ); +} + /** * @brief Sets the raw accelerometer reading to be returned by the emulator * @@ -170,3 +158,66 @@ void emulatorSetAccelerometerRotation(int16_t value, uint16_t yaw, uint16_t pitc _accelY = CLAMP(value * getSin1024(yaw % 360) / 1024 * getSin1024(pitch % 360) / 1024, ACCEL_MIN, ACCEL_MAX); _accelZ = CLAMP(value * getCos1024(yaw % 360) / 1024, ACCEL_MIN, ACCEL_MAX); } + +esp_err_t accelIntegrate() +{ + // Do nothing (is stub function) + return ESP_OK; +} + +/** + * @brief Rotate a 3D vector by a quaternion + * + * @param pout Pointer to the float[3] output of the rotation + * @param q Pointer to the wzyz quaternion (float[4]) of the rotation. + * @param p Pointer to the float[3] of the vector to rotates. + */ +void mathRotateVectorByQuaternion(float * pout, const float * q, const float * p) +{ + // return v + 2.0 * cross(q.xyz, cross(q.xyz, v) + q.w * v); + float iqo[3]; + mathCrossProduct( iqo, q + 1 /*.xyz*/, p ); + iqo[0] += q[0] * p[0]; + iqo[1] += q[0] * p[1]; + iqo[2] += q[0] * p[2]; + float ret[3]; + mathCrossProduct( ret, q + 1 /*.xyz*/, iqo ); + pout[0] = ret[0] * 2.0 + p[0]; + pout[1] = ret[1] * 2.0 + p[1]; + pout[2] = ret[2] * 2.0 + p[2]; +} + + +/** + * @brief Perform a 3D cross product + * + * @param p Pointer to the float[3] output of the cross product (p = a x b) + * @param a Pointer to the float[3] of the cross product a vector. + * @param a Pointer to the float[3] of the cross product b vector. + */ +void mathCrossProduct(float * p, const float * a, const float * b) +{ + float tx = a[1] * b[2] - a[2] * b[1]; + float ty = a[2] * b[0] - a[0] * b[2]; + p[2] = a[0] * b[1] - a[1] * b[0]; + p[1] = ty; + p[0] = tx; +} + +// stub +void accelSetRegistersAndReset() +{ +} + +// stub +esp_err_t accelPerformCal() +{ + return ESP_OK; +} + +// stub +float accelGetStdDevInCal() +{ + return 0.0f; +} + diff --git a/emulator/src/extensions/replay/ext_replay.c b/emulator/src/extensions/replay/ext_replay.c index 1ccf20111..340487282 100644 --- a/emulator/src/extensions/replay/ext_replay.c +++ b/emulator/src/extensions/replay/ext_replay.c @@ -199,7 +199,7 @@ static void replayRecordFrame(uint64_t frame) } int16_t accelX, accelY, accelZ; - accelGetAccelVec(&accelX, &accelY, &accelZ); + accelGetOrientVec(&accelX, &accelY, &accelZ); buttonBit_t curButtons = emulatorGetButtonState(); diff --git a/main/modes/accelTest/accelTest.c b/main/modes/accelTest/accelTest.c index b56bb3507..0ad3d75a2 100644 --- a/main/modes/accelTest/accelTest.c +++ b/main/modes/accelTest/accelTest.c @@ -247,7 +247,7 @@ static void accelTestHandleInput(void) { // Declare variables to receive acceleration // Get the current acceleration - if (ESP_OK == accelGetAccelVec(&(accelTest->x), &(accelTest->y), &(accelTest->z))) + if (ESP_OK == accelGetAccelVecRaw(&(accelTest->x), &(accelTest->y), &(accelTest->z))) { accelTestSample(accelTest->x, accelTest->y, accelTest->z); } diff --git a/main/modes/factoryTest/factoryTest.c b/main/modes/factoryTest/factoryTest.c index 16cefdca0..eb6e842c5 100644 --- a/main/modes/factoryTest/factoryTest.c +++ b/main/modes/factoryTest/factoryTest.c @@ -760,7 +760,7 @@ void testReadAndValidateAccelerometer(void) if( accelIntegrate() < 0 ) return; - accelGetAccelVec(&x, &y, &z); + accelGetAccelVecRaw(&x, &y, &z); accelGetOrientVec(&ox, &oy, &oz); // Save accel values @@ -773,19 +773,19 @@ void testReadAndValidateAccelerometer(void) float fStab = accelGetStdDevInCal(); test->stability = fStab * 200.0f; - if (x > 180) test->accelFlags |= 1; - if (y > 180) test->accelFlags |= 2; - if (z > 180) test->accelFlags |= 4; - if (x <-180) test->accelFlags |= 8; - if (y <-180) test->accelFlags |= 16; - if (z <-180) test->accelFlags |= 32; - - if (ox > 180) test->accelFlags |= 64; - if (oy > 180) test->accelFlags |= 128; - if (oz > 180) test->accelFlags |= 256; - if (ox <-180) test->accelFlags |= 512; - if (oy <-180) test->accelFlags |= 1024; - if (oz <-180) test->accelFlags |= 2048; + if (x > 150) test->accelFlags |= 1; + if (y > 150) test->accelFlags |= 2; + if (z > 150) test->accelFlags |= 4; + if (x <-150) test->accelFlags |= 8; + if (y <-150) test->accelFlags |= 16; + if (z <-150) test->accelFlags |= 32; + + if (ox > 150) test->accelFlags |= 64; + if (oy > 150) test->accelFlags |= 128; + if (oz > 150) test->accelFlags |= 256; + if (ox <-150) test->accelFlags |= 512; + if (oy <-150) test->accelFlags |= 1024; + if (oz <-150) test->accelFlags |= 2048; if (fStab < 0.00001f ) test->accelFlags |= 4096; // Make sure all values are nonzero diff --git a/main/modes/gamepad/gamepad.c b/main/modes/gamepad/gamepad.c index ebe190a33..7a5169478 100644 --- a/main/modes/gamepad/gamepad.c +++ b/main/modes/gamepad/gamepad.c @@ -424,6 +424,8 @@ void gamepadMenuLoop(int64_t elapsedUs) } // No wifi mode stuff } + + accelIntegrate(); } /** @@ -676,7 +678,7 @@ void gamepadMainLoop(int64_t elapsedUs __attribute__((unused))) // Declare variables to receive acceleration int16_t a_x, a_y, a_z; // Get the current acceleration - if (ESP_OK == accelGetAccelVec(&a_x, &a_y, &a_z)) + if (ESP_OK == accelGetOrientVec(&a_x, &a_y, &a_z)) { // Values are roughly -256 to 256, so divide, clamp, and save gamepad->gpState.rx = CLAMP((a_x) / 2, -128, 127); diff --git a/main/modes/pong/pong.c b/main/modes/pong/pong.c index b2cf693ad..aac6f21de 100644 --- a/main/modes/pong/pong.c +++ b/main/modes/pong/pong.c @@ -491,10 +491,10 @@ static void pongControlPlayerPaddle(void) // Declare variables to receive acceleration int16_t a_x, a_y, a_z; // Get the current acceleration - if (ESP_OK == accelGetAccelVec(&a_x, &a_y, &a_z)) + if (ESP_OK == accelGetOrientVec(&a_x, &a_y, &a_z)) { // Move the paddle to the current tilt location - pong->paddleL.y = CLAMP(((a_x + 100) * (FIELD_HEIGHT - pong->paddleL.height)) / 350, 0, + pong->paddleL.y = CLAMP(((a_x + 120) * (FIELD_HEIGHT - pong->paddleL.height)) / 350, 0, (FIELD_HEIGHT - pong->paddleL.height)); } break; @@ -721,6 +721,8 @@ static void pongIncreaseBallVelocity(int16_t magnitude) */ static void pongBackgroundDrawCallback(int16_t x, int16_t y, int16_t w, int16_t h, int16_t up, int16_t upNum) { + accelIntegrate(); + // Use TURBO drawing mode to draw individual pixels fast SETUP_FOR_TURBO(); From 4084da9ebf3c84a6cce80ff9c6dbd1b5f0e88c6a Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sun, 15 Oct 2023 21:14:04 -0400 Subject: [PATCH 15/18] Fix sample enumeration. --- components/hdw-accel/hdw-accel.c | 4 ++-- tools/sandbox_test/test_i2c/sandbox.c | 18 +++++++++--------- 2 files changed, 11 insertions(+), 11 deletions(-) diff --git a/components/hdw-accel/hdw-accel.c b/components/hdw-accel/hdw-accel.c index 404073ce6..6475aa951 100644 --- a/components/hdw-accel/hdw-accel.c +++ b/components/hdw-accel/hdw-accel.c @@ -539,7 +539,7 @@ esp_err_t accelIntegrate() ld->lastreadr = readr; - for( samp = 0; samp < readr; samp+=12 ) + for( samp = 0; samp < readr; samp+=6 ) { // Extract data from IMU int16_t * euler_deltas = cdata; // Euler angles, from gyro. @@ -565,7 +565,7 @@ esp_err_t accelIntegrate() // convert to radians. ( 2000.0f / 32768.0f / 208.0f * 2.0 * 3.14159f / 180.0f ); // Measured = 560,000 counts per scale (Measured by looking at sum) // Testing -> 3.14159 * 2.0 / 566000; - float fFudge = 1.125/2.0; //XXX TODO: Investigate. + float fFudge = 0.5625; //XXX TODO: Investigate. float fScale = ( 2000.0f / 32768.0f / 208.0f * 2.0 * 3.14159f / 180.0f ) * fFudge; // STEP 3: Integrate gyro values into a quaternion. diff --git a/tools/sandbox_test/test_i2c/sandbox.c b/tools/sandbox_test/test_i2c/sandbox.c index 53e207726..6c4f9663c 100644 --- a/tools/sandbox_test/test_i2c/sandbox.c +++ b/tools/sandbox_test/test_i2c/sandbox.c @@ -84,7 +84,7 @@ esp_err_t LSM6DSLSet( int reg, int val ); int GeneralI2CGet( int device, int reg, uint8_t * data, int data_len ); int ReadLSM6DSL( uint8_t * data, int data_len ); -#if 0 +#if 1 // For main add ESP_LOGI( "test", "%p %p %p %p %p\n", &i2c_driver_delete, &LSM6DSLSet, &rsqrtf, &mathCrossProduct, &accelIntegrate ); @@ -99,6 +99,7 @@ static void LSM6DSLIntegrate() if( r < 0 ) return; if( r == 2 ) ld->temp = data[0]; int readr = ReadLSM6DSL( (uint8_t*)data, sizeof( data ) ); + if( readr < 0 ) return; int samp; int16_t * cdata = data; @@ -113,7 +114,7 @@ static void LSM6DSLIntegrate() ld->lastreadr = readr; - for( samp = 0; samp < readr; samp+=12 ) + for( samp = 0; samp < readr; samp+=6 ) { // Extract data from IMU int16_t * euler_deltas = cdata; // Euler angles, from gyro. @@ -139,7 +140,7 @@ static void LSM6DSLIntegrate() // convert to radians. ( 2000.0f / 32768.0f / 208.0f * 2.0 * 3.14159f / 180.0f ); // Measured = 560,000 counts per scale (Measured by looking at sum) // Testing -> 3.14159 * 2.0 / 566000; - float fFudge = 1.125; //XXX TODO: Investigate. + float fFudge = 0.5 * 1.15; //XXX TODO: Investigate. float fScale = ( 2000.0f / 32768.0f / 208.0f * 2.0 * 3.14159f / 180.0f ) * fFudge; // STEP 3: Integrate gyro values into a quaternion. @@ -365,7 +366,7 @@ void sandbox_main(void) accelSetRegistersAndReset(); - setFrameRateUs(0); + setFrameRateUs(5000); ESP_LOGI( "sandbox", "Loaded" ); } @@ -454,10 +455,6 @@ void sandbox_tick() cts += sprintf( cts, " %04x", data[i] ); } #endif - - accelIntegrate(); -// LSM6DSLIntegrate(); - /* cts += sprintf( cts, "%ld %ld / %5d %5d %5d / %5d %5d %5d / %ld %ld %ld / %f %f %f %f", LSM6DSL.computetime, LSM6DSL.temp, @@ -533,7 +530,10 @@ void sandbox_tick() void sandboxBackgroundDrawCallback(int16_t x, int16_t y, int16_t w, int16_t h, int16_t up, int16_t upNum ) { - accelIntegrate(); +// accelIntegrate(); + if( up + 1 == upNum ) + LSM6DSLIntegrate(); + fillDisplayArea(x, y, x+w, y+h, 0 ); } From 9cf6935cb8282a5354c29a9b1fcacc51191d0b57 Mon Sep 17 00:00:00 2001 From: gelakinetic Date: Sun, 15 Oct 2023 21:19:45 -0400 Subject: [PATCH 16/18] Autoformat --- components/hdw-accel/hdw-accel.c | 583 ++++----- components/hdw-accel/include/hdw-accel.h | 29 +- emulator/src/components/hdw-accel/hdw-accel.c | 26 +- main/modes/accelTest/accelTest.c | 58 +- main/modes/accelTest/bunny.h | 1057 ++--------------- main/modes/factoryTest/factoryTest.c | 154 ++- main/modes/gamepad/gamepad.c | 2 +- main/modes/pong/pong.c | 2 +- main/swadge2024.c | 4 +- 9 files changed, 558 insertions(+), 1357 deletions(-) diff --git a/components/hdw-accel/hdw-accel.c b/components/hdw-accel/hdw-accel.c index 6475aa951..6a26c05dc 100644 --- a/components/hdw-accel/hdw-accel.c +++ b/components/hdw-accel/hdw-accel.c @@ -20,54 +20,54 @@ typedef enum __attribute__((packed)) { - LSM6DSL_FUNC_CFG_ACCESS = 0x01, - LSM6DSL_SENSOR_SYNC_TIME_FRAME = 0x04, - LSM6DSL_FIFO_CTRL1 = 0x06, - LSM6DSL_FIFO_CTRL2 = 0x07, - LSM6DSL_FIFO_CTRL3 = 0x08, - LSM6DSL_FIFO_CTRL4 = 0x09, - LSM6DSL_FIFO_CTRL5 = 0x0a, - LSM6DSL_ORIENT_CFG_G = 0x0b, - LSM6DSL_INT1_CTRL = 0x0d, - LSM6DSL_INT2_CTRL = 0x0e, - LMS6DS3_WHO_AM_I = 0x0f, - LSM6DSL_CTRL1_XL = 0x10, - LSM6DSL_CTRL2_G = 0x11, - LSM6DSL_CTRL3_C = 0x12, - LSM6DSL_CTRL4_C = 0x13, - LSM6DSL_CTRL5_C = 0x14, - LSM6DSL_CTRL6_C = 0x15, - LSM6DSL_CTRL7_G = 0x16, - LSM6DSL_CTRL8_XL = 0x17, - LSM6DSL_CTRL9_XL = 0x18, - LSM6DSL_CTRL10_C = 0x19, - LSM6DSL_MASTER_CONFIG = 0x1a, - LSM6DSL_WAKE_UP_SRC = 0x1b, - LSM6DSL_TAP_SRC = 0x1c, - LSM6DSL_D6D_SRC = 0x1d, - LSM6DSL_STATUS_REG = 0x1e, - LSM6DSL_OUT_TEMP_L = 0x20, - LSM6DSL_OUT_TEMP_H = 0x21, - LMS6DS3_OUTX_L_G = 0x22, - LMS6DS3_OUTX_H_G = 0x23, - LMS6DS3_OUTY_L_G = 0x24, - LMS6DS3_OUTY_H_G = 0x25, - LMS6DS3_OUTZ_L_G = 0x26, - LMS6DS3_OUTZ_H_G = 0x27, - LMS6DS3_OUTX_L_XL = 0x28, - LMS6DS3_OUTX_H_XL = 0x29, - LMS6DS3_OUTY_L_XL = 0x2a, - LMS6DS3_OUTY_H_XL = 0x2b, - LMS6DS3_OUTZ_L_XL = 0x2c, - LMS6DS3_OUTZ_H_XL = 0x2d, + LSM6DSL_FUNC_CFG_ACCESS = 0x01, + LSM6DSL_SENSOR_SYNC_TIME_FRAME = 0x04, + LSM6DSL_FIFO_CTRL1 = 0x06, + LSM6DSL_FIFO_CTRL2 = 0x07, + LSM6DSL_FIFO_CTRL3 = 0x08, + LSM6DSL_FIFO_CTRL4 = 0x09, + LSM6DSL_FIFO_CTRL5 = 0x0a, + LSM6DSL_ORIENT_CFG_G = 0x0b, + LSM6DSL_INT1_CTRL = 0x0d, + LSM6DSL_INT2_CTRL = 0x0e, + LMS6DS3_WHO_AM_I = 0x0f, + LSM6DSL_CTRL1_XL = 0x10, + LSM6DSL_CTRL2_G = 0x11, + LSM6DSL_CTRL3_C = 0x12, + LSM6DSL_CTRL4_C = 0x13, + LSM6DSL_CTRL5_C = 0x14, + LSM6DSL_CTRL6_C = 0x15, + LSM6DSL_CTRL7_G = 0x16, + LSM6DSL_CTRL8_XL = 0x17, + LSM6DSL_CTRL9_XL = 0x18, + LSM6DSL_CTRL10_C = 0x19, + LSM6DSL_MASTER_CONFIG = 0x1a, + LSM6DSL_WAKE_UP_SRC = 0x1b, + LSM6DSL_TAP_SRC = 0x1c, + LSM6DSL_D6D_SRC = 0x1d, + LSM6DSL_STATUS_REG = 0x1e, + LSM6DSL_OUT_TEMP_L = 0x20, + LSM6DSL_OUT_TEMP_H = 0x21, + LMS6DS3_OUTX_L_G = 0x22, + LMS6DS3_OUTX_H_G = 0x23, + LMS6DS3_OUTY_L_G = 0x24, + LMS6DS3_OUTY_H_G = 0x25, + LMS6DS3_OUTZ_L_G = 0x26, + LMS6DS3_OUTZ_H_G = 0x27, + LMS6DS3_OUTX_L_XL = 0x28, + LMS6DS3_OUTX_H_XL = 0x29, + LMS6DS3_OUTY_L_XL = 0x2a, + LMS6DS3_OUTY_H_XL = 0x2b, + LMS6DS3_OUTZ_L_XL = 0x2c, + LMS6DS3_OUTZ_H_XL = 0x2d, } lsm6dslReg_t; //============================================================================== // Defines //============================================================================== -#define LSM6DSL_ADDRESS 0x6a -#define QMC6308_ADDRESS 0x2c +#define LSM6DSL_ADDRESS 0x6a +#define QMC6308_ADDRESS 0x2c //============================================================================== // Variables @@ -82,29 +82,29 @@ LSM6DSLData LSM6DSL; float rsqrtf(float x); float mathsqrtf(float x); -void mathEulerToQuat(float * q, const float * euler); -void mathQuatApply(float * qout, const float * q1, const float * q2); -void mathQuatNormalize(float * qout, const float * qin ); -void mathCrossProduct(float * p, const float * a, const float * b); -void mathRotateVectorByInverseOfQuaternion(float * pout, const float * q, const float * p ); -void mathRotateVectorByQuaternion(float * pout, const float * q, const float * p); +void mathEulerToQuat(float* q, const float* euler); +void mathQuatApply(float* qout, const float* q1, const float* q2); +void mathQuatNormalize(float* qout, const float* qin); +void mathCrossProduct(float* p, const float* a, const float* b); +void mathRotateVectorByInverseOfQuaternion(float* pout, const float* q, const float* p); +void mathRotateVectorByQuaternion(float* pout, const float* q, const float* p); static inline uint32_t getCycleCount(); -esp_err_t GeneralSet( int dev, int reg, int val ); -esp_err_t LSM6DSLSet( int reg, int val ); -int GeneralI2CGet( int device, int reg, uint8_t * data, int data_len ); -int ReadLSM6DSL( uint8_t * data, int data_len ); +esp_err_t GeneralSet(int dev, int reg, int val); +esp_err_t LSM6DSLSet(int reg, int val); +int GeneralI2CGet(int device, int reg, uint8_t* data, int data_len); +int ReadLSM6DSL(uint8_t* data, int data_len); //============================================================================== // Function Prototypes //============================================================================== -esp_err_t initAccelerometer(i2c_port_t _i2c_port, gpio_num_t sda, gpio_num_t scl, gpio_pullup_t pullup, uint32_t clkHz ); +esp_err_t initAccelerometer(i2c_port_t _i2c_port, gpio_num_t sda, gpio_num_t scl, gpio_pullup_t pullup, uint32_t clkHz); esp_err_t deInitAccelerometer(void); esp_err_t accelGetAccelVecRaw(int16_t* x, int16_t* y, int16_t* z); esp_err_t accelGetOrientVec(int16_t* x, int16_t* y, int16_t* z); -esp_err_t accelGetQuaternion(float * q); +esp_err_t accelGetQuaternion(float* q); esp_err_t accelIntegrate(void); float accelGetStdDevInCal(void); void accelSetRegistersAndReset(void); @@ -113,7 +113,6 @@ void accelSetRegistersAndReset(void); // Utility Functions //============================================================================== - /** * @brief Perform a fast, approximate reciprocal square root * @@ -122,13 +121,17 @@ void accelSetRegistersAndReset(void); */ float rsqrtf(float x) { - typedef union { int32_t i; float f; } fiunion; + typedef union + { + int32_t i; + float f; + } fiunion; const float xhalf = 0.5f * x; - fiunion i = { .f = x }; - i.i = 0x5f375a86 - ( i.i >> 1 ); - x = i.f; - x = x * ( 1.5f - xhalf * x * x ); - x = x * ( 1.5f - xhalf * x * x ); + fiunion i = {.f = x}; + i.i = 0x5f375a86 - (i.i >> 1); + x = i.f; + x = x * (1.5f - xhalf * x * x); + x = x * (1.5f - xhalf * x * x); return x; } @@ -142,14 +145,16 @@ float mathsqrtf(float x) { // Trick to do approximate, fast square roots. (Though it is surprisingly fast) int sign = x < 0; - if( sign ) x = -x; - if( x < 0.0000001 ) return 0.0001; + if (sign) + x = -x; + if (x < 0.0000001) + return 0.0001; float o = x; - o = (o+x/o)/2; - o = (o+x/o)/2; - o = (o+x/o)/2; - o = (o+x/o)/2; - if( sign ) + o = (o + x / o) / 2; + o = (o + x / o) / 2; + o = (o + x / o) / 2; + o = (o + x / o) / 2; + if (sign) return -o; else return o; @@ -161,21 +166,21 @@ float mathsqrtf(float x) * @param q Pointer to the wxyz quat (float[4]) to be written. * @param euler Pointer to a float[3] of euler angles. */ -void mathEulerToQuat(float * q, const float * euler) +void mathEulerToQuat(float* q, const float* euler) { float pitch = euler[0]; - float yaw = euler[1]; - float roll = euler[2]; - float cr = cosf(pitch * 0.5); - float sr = sinf(pitch * 0.5); // Pitch: About X - float cp = cosf(yaw * 0.5); - float sp = sinf(yaw * 0.5); // Yaw: About Y - float cy = cosf(roll * 0.5); - float sy = sinf(roll * 0.5); // Roll: About Z - q[0] = cr * cp * cy + sr * sp * sy; - q[1] = sr * cp * cy - cr * sp * sy; - q[2] = cr * sp * cy + sr * cp * sy; - q[3] = cr * cp * sy - sr * sp * cy; + float yaw = euler[1]; + float roll = euler[2]; + float cr = cosf(pitch * 0.5); + float sr = sinf(pitch * 0.5); // Pitch: About X + float cp = cosf(yaw * 0.5); + float sp = sinf(yaw * 0.5); // Yaw: About Y + float cy = cosf(roll * 0.5); + float sy = sinf(roll * 0.5); // Roll: About Z + q[0] = cr * cp * cy + sr * sp * sy; + q[1] = sr * cp * cy - cr * sp * sy; + q[2] = cr * sp * cy + sr * cp * sy; + q[3] = cr * cp * sy - sr * sp * cy; } /** @@ -185,13 +190,13 @@ void mathEulerToQuat(float * q, const float * euler) * @param q1 First quaternion to be rotated. * @param q2 Quaternion to rotate q1 by. */ -void mathQuatApply(float * qout, const float * q1, const float * q2) +void mathQuatApply(float* qout, const float* q1, const float* q2) { // NOTE: Does not normalize - you will need to normalize eventually. float tmpw, tmpx, tmpy; - tmpw = (q1[0] * q2[0]) - (q1[1] * q2[1]) - (q1[2] * q2[2]) - (q1[3] * q2[3]); - tmpx = (q1[0] * q2[1]) + (q1[1] * q2[0]) + (q1[2] * q2[3]) - (q1[3] * q2[2]); - tmpy = (q1[0] * q2[2]) - (q1[1] * q2[3]) + (q1[2] * q2[0]) + (q1[3] * q2[1]); + tmpw = (q1[0] * q2[0]) - (q1[1] * q2[1]) - (q1[2] * q2[2]) - (q1[3] * q2[3]); + tmpx = (q1[0] * q2[1]) + (q1[1] * q2[0]) + (q1[2] * q2[3]) - (q1[3] * q2[2]); + tmpy = (q1[0] * q2[2]) - (q1[1] * q2[3]) + (q1[2] * q2[0]) + (q1[3] * q2[1]); qout[3] = (q1[0] * q2[3]) + (q1[1] * q2[2]) - (q1[2] * q2[1]) + (q1[3] * q2[0]); qout[2] = tmpy; qout[1] = tmpx; @@ -204,17 +209,16 @@ void mathQuatApply(float * qout, const float * q1, const float * q2) * @param qout Pointer to the wxyz quat (float[4]) to be written. * @param qin Pointer to the quaterion to normalize. */ -void mathQuatNormalize(float * qout, const float * qin ) +void mathQuatNormalize(float* qout, const float* qin) { float qmag = qin[0] * qin[0] + qin[1] * qin[1] + qin[2] * qin[2] + qin[3] * qin[3]; - qmag = rsqrtf( qmag ); - qout[0] = qin[0] * qmag; - qout[1] = qin[1] * qmag; - qout[2] = qin[2] * qmag; - qout[3] = qin[3] * qmag; + qmag = rsqrtf(qmag); + qout[0] = qin[0] * qmag; + qout[1] = qin[1] * qmag; + qout[2] = qin[2] * qmag; + qout[3] = qin[3] * qmag; } - /** * @brief Perform a 3D cross product * @@ -222,13 +226,13 @@ void mathQuatNormalize(float * qout, const float * qin ) * @param a Pointer to the float[3] of the cross product a vector. * @param a Pointer to the float[3] of the cross product b vector. */ -void mathCrossProduct(float * p, const float * a, const float * b) +void mathCrossProduct(float* p, const float* a, const float* b) { float tx = a[1] * b[2] - a[2] * b[1]; float ty = a[2] * b[0] - a[0] * b[2]; - p[2] = a[0] * b[1] - a[1] * b[0]; - p[1] = ty; - p[0] = tx; + p[2] = a[0] * b[1] - a[1] * b[0]; + p[1] = ty; + p[0] = tx; } /** @@ -238,16 +242,16 @@ void mathCrossProduct(float * p, const float * a, const float * b) * @param q Pointer to the wzyz quaternion (float[4]) of the rotation. * @param p Pointer to the float[3] of the vector to rotates. */ -void mathRotateVectorByQuaternion(float * pout, const float * q, const float * p) +void mathRotateVectorByQuaternion(float* pout, const float* q, const float* p) { // return v + 2.0 * cross(q.xyz, cross(q.xyz, v) + q.w * v); float iqo[3]; - mathCrossProduct( iqo, q + 1 /*.xyz*/, p ); + mathCrossProduct(iqo, q + 1 /*.xyz*/, p); iqo[0] += q[0] * p[0]; iqo[1] += q[0] * p[1]; iqo[2] += q[0] * p[2]; float ret[3]; - mathCrossProduct( ret, q + 1 /*.xyz*/, iqo ); + mathCrossProduct(ret, q + 1 /*.xyz*/, iqo); pout[0] = ret[0] * 2.0 + p[0]; pout[1] = ret[1] * 2.0 + p[1]; pout[2] = ret[2] * 2.0 + p[2]; @@ -260,17 +264,17 @@ void mathRotateVectorByQuaternion(float * pout, const float * q, const float * p * @param q Pointer to the wzyz quaternion (float[4]) opposite of the rotation. * @param p Pointer to the float[3] of the vector to antirotates. */ -void mathRotateVectorByInverseOfQuaternion(float * pout, const float * q, const float * p) +void mathRotateVectorByInverseOfQuaternion(float* pout, const float* q, const float* p) { // General note: Performing a transform this way can be about 20-30% slower than a well formed 3x3 matrix. // return v + 2.0 * cross(q.xyz, cross(q.xyz, v) + q.w * v); float iqo[3]; - mathCrossProduct( iqo, p, q + 1 /*.xyz*/ ); + mathCrossProduct(iqo, p, q + 1 /*.xyz*/); iqo[0] += q[0] * p[0]; iqo[1] += q[0] * p[1]; iqo[2] += q[0] * p[2]; float ret[3]; - mathCrossProduct( ret, iqo, q + 1 /*.xyz*/ ); + mathCrossProduct(ret, iqo, q + 1 /*.xyz*/); pout[0] = ret[0] * 2.0 + p[0]; pout[1] = ret[1] * 2.0 + p[1]; pout[2] = ret[2] * 2.0 + p[2]; @@ -287,7 +291,6 @@ static inline uint32_t getCycleCount() // Internal Functions //============================================================================== - /** * @brief Set a specific register on the IMU to a value. * @@ -296,7 +299,7 @@ static inline uint32_t getCycleCount() * @param val The 8-bit value to set the register to. * @return ESP_OK if the operation was successful. */ -esp_err_t GeneralSet( int dev, int reg, int val ) +esp_err_t GeneralSet(int dev, int reg, int val) { i2c_cmd_handle_t cmdHandle = i2c_cmd_link_create(); i2c_master_start(cmdHandle); @@ -316,9 +319,9 @@ esp_err_t GeneralSet( int dev, int reg, int val ) * @param val The 8-bit value to set the register to. * @return ESP_OK if the operation was successful. */ -esp_err_t LSM6DSLSet( int reg, int val ) +esp_err_t LSM6DSLSet(int reg, int val) { - return GeneralSet( LSM6DSL_ADDRESS, reg, val ); + return GeneralSet(LSM6DSL_ADDRESS, reg, val); } /** @@ -330,7 +333,7 @@ esp_err_t LSM6DSLSet( int reg, int val ) * @param data_len Number of bytes to read. * @return positive number if operation was successful, or esp_err_t if failure. */ -int GeneralI2CGet( int device, int reg, uint8_t * data, int data_len ) +int GeneralI2CGet(int device, int reg, uint8_t* data, int data_len) { i2c_cmd_handle_t cmdHandle = i2c_cmd_link_create(); i2c_master_start(cmdHandle); @@ -342,12 +345,13 @@ int GeneralI2CGet( int device, int reg, uint8_t * data, int data_len ) i2c_master_stop(cmdHandle); esp_err_t err = i2c_master_cmd_begin(I2C_NUM_0, cmdHandle, 100); i2c_cmd_link_delete(cmdHandle); - if( err ) + if (err) { - ESP_LOGE( "accel", "Error on link: %d", err ); + ESP_LOGE("accel", "Error on link: %d", err); return -1; } - else return data_len; + else + return data_len; } /** @@ -357,7 +361,7 @@ int GeneralI2CGet( int device, int reg, uint8_t * data, int data_len ) * @param data_len The maximum size (in words) to read. * @return positive number if operation was successful, or esp_err_t if failure. */ -int ReadLSM6DSL( uint8_t * data, int data_len ) +int ReadLSM6DSL(uint8_t* data, int data_len) { i2c_cmd_handle_t cmdHandle = i2c_cmd_link_create(); i2c_master_start(cmdHandle); @@ -370,22 +374,25 @@ int ReadLSM6DSL( uint8_t * data, int data_len ) i2c_master_stop(cmdHandle); esp_err_t err = i2c_master_cmd_begin(I2C_NUM_0, cmdHandle, 100); i2c_cmd_link_delete(cmdHandle); - if( err < 0 ) return -1; + if (err < 0) + return -1; // Is fifo overflow. - if( fifolen & 0x4000 ) + if (fifolen & 0x4000) { // reset fifo. // If we overflow, and we don't do this, bad things happen. - LSM6DSLSet( LSM6DSL_FIFO_CTRL5, (0b0101 << 3) | 0b000 ); // Disable fifo - LSM6DSLSet( LSM6DSL_FIFO_CTRL5, (0b0101 << 3) | 0b110 ); // 208 Hz ODR + LSM6DSLSet(LSM6DSL_FIFO_CTRL5, (0b0101 << 3) | 0b000); // Disable fifo + LSM6DSLSet(LSM6DSL_FIFO_CTRL5, (0b0101 << 3) | 0b110); // 208 Hz ODR LSM6DSL.sampCount = 0; return 0; } fifolen &= 0x7ff; - if( fifolen == 0 ) return 0; - if( fifolen > data_len / 2 ) fifolen = data_len / 2; + if (fifolen == 0) + return 0; + if (fifolen > data_len / 2) + fifolen = data_len / 2; cmdHandle = i2c_cmd_link_create(); i2c_master_start(cmdHandle); @@ -395,21 +402,16 @@ int ReadLSM6DSL( uint8_t * data, int data_len ) err = i2c_master_cmd_begin(I2C_NUM_0, cmdHandle, 100); i2c_cmd_link_delete(cmdHandle); - if( err < 0 ) return -2; + if (err < 0) + return -2; return fifolen; } - - - //============================================================================== // Functions //============================================================================== - - - /** * @brief Initialize the IMU * @@ -421,32 +423,31 @@ int ReadLSM6DSL( uint8_t * data, int data_len ) * @param clkHz The frequency of the I2C clock * @return ESP_OK if the accelerometer initialized, or a non-zero value if it did not */ -esp_err_t initAccelerometer(i2c_port_t _i2c_port, gpio_num_t sda, gpio_num_t scl, gpio_pullup_t pullup, uint32_t clkHz ) +esp_err_t initAccelerometer(i2c_port_t _i2c_port, gpio_num_t sda, gpio_num_t scl, gpio_pullup_t pullup, uint32_t clkHz) { int retry = 0; - i2c_port = _i2c_port; + i2c_port = _i2c_port; esp_err_t ret_val; do_retry: - // Shake any device off the bus. int i; int gpio_scl = 41; - for( i = 0; i < 16; i++ ) + for (i = 0; i < 16; i++) { - gpio_matrix_out( gpio_scl, 256, 1, 0 ); - GPIO.out1_w1tc.val = (1<<(gpio_scl-32)); + gpio_matrix_out(gpio_scl, 256, 1, 0); + GPIO.out1_w1tc.val = (1 << (gpio_scl - 32)); esp_rom_delay_us(10); - gpio_matrix_out( gpio_scl, 256, 1, 0 ); - GPIO.out1_w1ts.val = (1<<(gpio_scl-32)); + gpio_matrix_out(gpio_scl, 256, 1, 0); + GPIO.out1_w1ts.val = (1 << (gpio_scl - 32)); esp_rom_delay_us(10); } - gpio_matrix_out( gpio_scl, 29, 0, 0 ); + gpio_matrix_out(gpio_scl, 29, 0, 0); ret_val = ESP_OK; - i2c_driver_delete( _i2c_port ); + i2c_driver_delete(_i2c_port); /* Install i2c driver */ i2c_config_t conf = { @@ -455,46 +456,48 @@ esp_err_t initAccelerometer(i2c_port_t _i2c_port, gpio_num_t sda, gpio_num_t scl .sda_pullup_en = pullup, .scl_io_num = scl, .scl_pullup_en = pullup, - .master.clk_speed = clkHz, //tested upto 1.4Mbit/s + .master.clk_speed = clkHz, // tested upto 1.4Mbit/s .clk_flags = I2C_SCLK_SRC_FLAG_FOR_NOMAL, }; - ESP_LOGI( "accel", "i2c_driver_install=%d", i2c_driver_install(_i2c_port, conf.mode, 0, 0, 0) ); + ESP_LOGI("accel", "i2c_driver_install=%d", i2c_driver_install(_i2c_port, conf.mode, 0, 0, 0)); ret_val |= i2c_param_config(i2c_port, &conf); // Enable access - LSM6DSLSet( LSM6DSL_FUNC_CFG_ACCESS, 0x20 ); - LSM6DSLSet( LSM6DSL_CTRL3_C, 0x81 ); // Force reset - vTaskDelay( 1 ); - LSM6DSLSet( LSM6DSL_CTRL3_C, 0x44 ); // unforce reset + LSM6DSLSet(LSM6DSL_FUNC_CFG_ACCESS, 0x20); + LSM6DSLSet(LSM6DSL_CTRL3_C, 0x81); // Force reset + vTaskDelay(1); + LSM6DSLSet(LSM6DSL_CTRL3_C, 0x44); // unforce reset uint8_t who = 0xaa; - int r = GeneralI2CGet( LSM6DSL_ADDRESS, LMS6DS3_WHO_AM_I, &who, 1 ); - if( r != 1 || who != 0x6a ) + int r = GeneralI2CGet(LSM6DSL_ADDRESS, LMS6DS3_WHO_AM_I, &who, 1); + if (r != 1 || who != 0x6a) { - ESP_LOGW( "accel", "WHOAMI Failed (%02x), %d", who, r ); - if( retry++ < 10 ) goto do_retry; - ESP_LOGE( "accel", "Init failed on 1" ); + ESP_LOGW("accel", "WHOAMI Failed (%02x), %d", who, r); + if (retry++ < 10) + goto do_retry; + ESP_LOGE("accel", "Init failed on 1"); return ESP_FAIL; } - ESP_LOGI( "accel", "Init Start" ); + ESP_LOGI("accel", "Init Start"); - accelSetRegistersAndReset(); + accelSetRegistersAndReset(); - for( i = 0; i < 2; i++ ) + for (i = 0; i < 2; i++) { - vTaskDelay( 1 ); + vTaskDelay(1); int check = accelIntegrate(); - if( check != ESP_OK ) + if (check != ESP_OK) { - ESP_LOGI( "accel", "Init Fault Retry" ); - if( retry++ < 10 ) goto do_retry; - ESP_LOGI( "accel", "Init failed on 2" ); + ESP_LOGI("accel", "Init Fault Retry"); + if (retry++ < 10) + goto do_retry; + ESP_LOGI("accel", "Init failed on 2"); return ESP_FAIL; } - ESP_LOGI( "accel", "Check %d", check ); + ESP_LOGI("accel", "Check %d", check); } - ESP_LOGI( "accel", "Init Ok" ); + ESP_LOGI("accel", "Init Ok"); return ESP_OK; } @@ -508,7 +511,6 @@ esp_err_t deInitAccelerometer(void) return ESP_OK; } - /** * @brief Read all pending samples in IMU and perform a sensor fusion pass * @@ -516,18 +518,21 @@ esp_err_t deInitAccelerometer(void) */ esp_err_t accelIntegrate() { - LSM6DSLData * ld = &LSM6DSL; + LSM6DSLData* ld = &LSM6DSL; - int16_t data[6*16]; + int16_t data[6 * 16]; // Get temperature sensor (in case we ever want to use it) - int r = GeneralI2CGet( LSM6DSL_ADDRESS, 0x20, (uint8_t*)data, 2 ); - if( r < 0 ) return r; - if( r == 2 ) ld->temp = data[0]; - int readr = ReadLSM6DSL( (uint8_t*)data, sizeof( data ) ); - if( readr < 0 ) return readr; + int r = GeneralI2CGet(LSM6DSL_ADDRESS, 0x20, (uint8_t*)data, 2); + if (r < 0) + return r; + if (r == 2) + ld->temp = data[0]; + int readr = ReadLSM6DSL((uint8_t*)data, sizeof(data)); + if (readr < 0) + return readr; int samp; - int16_t * cdata = data; + int16_t* cdata = data; uint32_t start = getCycleCount(); @@ -539,16 +544,16 @@ esp_err_t accelIntegrate() ld->lastreadr = readr; - for( samp = 0; samp < readr; samp+=6 ) + for (samp = 0; samp < readr; samp += 6) { // Extract data from IMU - int16_t * euler_deltas = cdata; // Euler angles, from gyro. - int16_t * accel_data = cdata + 3; + int16_t* euler_deltas = cdata; // Euler angles, from gyro. + int16_t* accel_data = cdata + 3; // Manual cal, used only for Steps 2..8 - // euler_deltas[0] -= 12; - // euler_deltas[1] += 22; - // euler_deltas[2] += 4; + // euler_deltas[0] -= 12; + // euler_deltas[1] += 22; + // euler_deltas[2] += 4; // We can sum rotations to understand the amount of counts in a full circle. // Note: this is actually more of a debug mechanism. @@ -562,38 +567,29 @@ esp_err_t accelIntegrate() // 2000 dps full-scale // 32768 is full-scale // 208 SPS - // convert to radians. ( 2000.0f / 32768.0f / 208.0f * 2.0 * 3.14159f / 180.0f ); + // convert to radians. ( 2000.0f / 32768.0f / 208.0f * 2.0 * 3.14159f / 180.0f ); // Measured = 560,000 counts per scale (Measured by looking at sum) // Testing -> 3.14159 * 2.0 / 566000; - float fFudge = 0.5625; //XXX TODO: Investigate. - float fScale = ( 2000.0f / 32768.0f / 208.0f * 2.0 * 3.14159f / 180.0f ) * fFudge; + float fFudge = 0.5625; // XXX TODO: Investigate. + float fScale = (2000.0f / 32768.0f / 208.0f * 2.0 * 3.14159f / 180.0f) * fFudge; // STEP 3: Integrate gyro values into a quaternion. // This step is validated by working with just one axis at a time // then apply a coordinate frame to ld->fqQuat and validate that it is // correct. - float fEulerScales[3] = { - -fScale, - fScale, - -fScale }; + float fEulerScales[3] = {-fScale, fScale, -fScale}; - float fEulers[3] = { - euler_deltas[0] * fEulerScales[0], - euler_deltas[1] * fEulerScales[1], - euler_deltas[2] * fEulerScales[2] }; + float fEulers[3] + = {euler_deltas[0] * fEulerScales[0], euler_deltas[1] * fEulerScales[1], euler_deltas[2] * fEulerScales[2]}; // Used for calibration - if( ld->performCal ) + if (ld->performCal) { - float diff[3] = { - fEulers[0] - ld->fvAverage[0], - fEulers[1] - ld->fvAverage[1], - fEulers[2] - ld->fvAverage[2] }; + float diff[3] + = {fEulers[0] - ld->fvAverage[0], fEulers[1] - ld->fvAverage[1], fEulers[2] - ld->fvAverage[2]}; - float diffsq[3] = { - (diff[0]<0)?-diff[0]:diff[0], - (diff[1]<0)?-diff[1]:diff[1], - (diff[2]<0)?-diff[2]:diff[2] }; + float diffsq[3] = {(diff[0] < 0) ? -diff[0] : diff[0], (diff[1] < 0) ? -diff[1] : diff[1], + (diff[2] < 0) ? -diff[2] : diff[2]}; diffsq[0] *= 1000.0; diffsq[1] *= 1000.0; @@ -603,17 +599,23 @@ esp_err_t accelIntegrate() ld->fvDeviation[1] -= 0.004; ld->fvDeviation[2] -= 0.004; - if( ld->fvDeviation[0] < diffsq[0] ) ld->fvDeviation[0] = diffsq[0]; - if( ld->fvDeviation[1] < diffsq[1] ) ld->fvDeviation[1] = diffsq[1]; - if( ld->fvDeviation[2] < diffsq[2] ) ld->fvDeviation[2] = diffsq[2]; + if (ld->fvDeviation[0] < diffsq[0]) + ld->fvDeviation[0] = diffsq[0]; + if (ld->fvDeviation[1] < diffsq[1]) + ld->fvDeviation[1] = diffsq[1]; + if (ld->fvDeviation[2] < diffsq[2]) + ld->fvDeviation[2] = diffsq[2]; - if( ld->fvDeviation[0] > 0.8 ) ld->fvDeviation[0] = 0.8; - if( ld->fvDeviation[1] > 0.8 ) ld->fvDeviation[1] = 0.8; - if( ld->fvDeviation[2] > 0.8 ) ld->fvDeviation[2] = 0.8; + if (ld->fvDeviation[0] > 0.8) + ld->fvDeviation[0] = 0.8; + if (ld->fvDeviation[1] > 0.8) + ld->fvDeviation[1] = 0.8; + if (ld->fvDeviation[2] > 0.8) + ld->fvDeviation[2] = 0.8; - diff[0] *= mathsqrtf( ld->fvDeviation[0] ) * 0.5; - diff[1] *= mathsqrtf( ld->fvDeviation[1] ) * 0.5; - diff[2] *= mathsqrtf( ld->fvDeviation[2] ) * 0.5; + diff[0] *= mathsqrtf(ld->fvDeviation[0]) * 0.5; + diff[1] *= mathsqrtf(ld->fvDeviation[1]) * 0.5; + diff[2] *= mathsqrtf(ld->fvDeviation[2]) * 0.5; ld->fvAverage[0] += diff[0]; ld->fvAverage[1] += diff[1]; @@ -622,21 +624,28 @@ esp_err_t accelIntegrate() // Compute the running RMS error. float fvEuler = accelGetStdDevInCal(); - if( fvEuler < 0.00015f ) + if (fvEuler < 0.00015f) { ld->fvBias[0] = -ld->fvAverage[0]; ld->fvBias[1] = -ld->fvAverage[1]; ld->fvBias[2] = -ld->fvAverage[2]; - struct fiunion { union { int32_t i; float f; } u; }; - struct fiunion x, y, z; - x.u.f = ld->fvBias[0]; - y.u.f = ld->fvBias[1]; - z.u.f = ld->fvBias[2]; - writeNvs32( "gyrocalx", x.u.i ); - writeNvs32( "gyrocaly", y.u.i ); - writeNvs32( "gyrocalz", z.u.i ); - - ld->performCal = 0; + struct fiunion + { + union + { + int32_t i; + float f; + } u; + }; + struct fiunion x, y, z; + x.u.f = ld->fvBias[0]; + y.u.f = ld->fvBias[1]; + z.u.f = ld->fvBias[2]; + writeNvs32("gyrocalx", x.u.i); + writeNvs32("gyrocaly", y.u.i); + writeNvs32("gyrocalz", z.u.i); + + ld->performCal = 0; ld->fvDeviation[0] = 0; ld->fvDeviation[0] = 1; ld->fvDeviation[0] = 2; @@ -647,9 +656,8 @@ esp_err_t accelIntegrate() fEulers[1] += ld->fvBias[1]; fEulers[2] += ld->fvBias[2]; - - mathEulerToQuat( ld->fqQuatLast, fEulers ); - mathQuatApply( ld->fqQuat, ld->fqQuat, ld->fqQuatLast ); + mathEulerToQuat(ld->fqQuatLast, fEulers); + mathQuatApply(ld->fqQuat, ld->fqQuat, ld->fqQuatLast); // STEP 4: Validate yor values by doing 4 90 degree turns // across multiple axes. @@ -663,13 +671,10 @@ esp_err_t accelIntegrate() // STEP 6A: Examine vectors. Generally speaking, we want an "up" vector, not a gravity vector. // this is "up" in the controller's point of view. - float accel_up[3] = { - -accel_data[0], - accel_data[1], - -accel_data[2] - }; + float accel_up[3] = {-accel_data[0], accel_data[1], -accel_data[2]}; - float accel_inverse_mag = rsqrtf( accel_up[0] * accel_up[0] + accel_up[1] * accel_up[1] + accel_up[2] * accel_up[2] ); + float accel_inverse_mag + = rsqrtf(accel_up[0] * accel_up[0] + accel_up[1] * accel_up[1] + accel_up[2] * accel_up[2]); accel_up[0] *= accel_inverse_mag; accel_up[1] *= accel_inverse_mag; accel_up[2] *= accel_inverse_mag; @@ -679,21 +684,21 @@ esp_err_t accelIntegrate() ld->fvLastAccelRaw[2] = accel_up[2]; // Step 6B: Next, compute what we think "up" should be from our point of view. We will use +Y Up. - float what_we_think_is_up[3] = { 0, 1, 0 }; - mathRotateVectorByInverseOfQuaternion( what_we_think_is_up, LSM6DSL.fqQuat, what_we_think_is_up ); + float what_we_think_is_up[3] = {0, 1, 0}; + mathRotateVectorByInverseOfQuaternion(what_we_think_is_up, LSM6DSL.fqQuat, what_we_think_is_up); // Step 6C: Next, we determine how far off we are. This will tell us our error. float corrective_quaternion[4]; // TRICKY: The ouput of this is actually the axis of rotation, which is ironically // in vector-form the same as a quaternion. So we can write directly into the quat. - mathCrossProduct( corrective_quaternion + 1, accel_up, what_we_think_is_up ); + mathCrossProduct(corrective_quaternion + 1, accel_up, what_we_think_is_up); // Now, we apply this in step 7. // First, we can compute what the drift values of our axes are, to anti-drift them. - // If you do only this, you will always end up in an unstable oscillation. - memcpy( ld->fCorrectLast, corrective_quaternion+1, 12 ); + // If you do only this, you will always end up in an unstable oscillation. + memcpy(ld->fCorrectLast, corrective_quaternion + 1, 12); // XXX TODO: We need to multiply by amount the accelerometer gives us assurance. ld->fvBias[0] += mathsqrtf(corrective_quaternion[1]) * 0.0000002; @@ -710,12 +715,11 @@ esp_err_t accelIntegrate() corrective_quaternion[3] *= corrective_force; // x^2+y^2+z^2+q^2 -> ALGEBRA! -> sqrt( 1-x^2-y^2-z^2 ) = w - corrective_quaternion[0] = mathsqrtf( 1 - - corrective_quaternion[1]*corrective_quaternion[1] - - corrective_quaternion[2]*corrective_quaternion[2] - - corrective_quaternion[3]*corrective_quaternion[3] ); + corrective_quaternion[0] = mathsqrtf(1 - corrective_quaternion[1] * corrective_quaternion[1] + - corrective_quaternion[2] * corrective_quaternion[2] + - corrective_quaternion[3] * corrective_quaternion[3]); - mathQuatApply( ld->fqQuat, ld->fqQuat, corrective_quaternion ); + mathQuatApply(ld->fqQuat, ld->fqQuat, corrective_quaternion); cdata += 6; } @@ -725,26 +729,25 @@ esp_err_t accelIntegrate() // soon, otherwise you won't notice math errors. Realistically, this should // only need to be done every hundreds of thousands of samples. // - // Also, don't do this too often, otherwise you will reduce accuracy, + // Also, don't do this too often, otherwise you will reduce accuracy, // unnecessarily. - float * qRot = ld->fqQuat; + float* qRot = ld->fqQuat; float qmagsq = qRot[0] * qRot[0] + qRot[1] * qRot[1] + qRot[2] * qRot[2] + qRot[3] * qRot[3]; - if( qmagsq > 1.05 || qmagsq < 0.95 ) + if (qmagsq > 1.05 || qmagsq < 0.95) { // This normalizes everything. - qmagsq = rsqrtf( qmagsq ); + qmagsq = rsqrtf(qmagsq); qRot[0] = qRot[0] * qmagsq; qRot[1] = qRot[1] * qmagsq; qRot[2] = qRot[2] * qmagsq; qRot[3] = qRot[3] * qmagsq; } - - if( samp ) + if (samp) { - ld->gyrolast[0] = cdata[-6]; - ld->gyrolast[1] = cdata[-5]; - ld->gyrolast[2] = cdata[-4]; + ld->gyrolast[0] = cdata[-6]; + ld->gyrolast[1] = cdata[-5]; + ld->gyrolast[2] = cdata[-4]; ld->accellast[0] = cdata[-3]; ld->accellast[1] = cdata[-2]; ld->accellast[2] = cdata[-1]; @@ -775,27 +778,26 @@ esp_err_t accelGetAccelVecRaw(int16_t* x, int16_t* y, int16_t* z) */ esp_err_t accelGetOrientVec(int16_t* x, int16_t* y, int16_t* z) { - float plusy[3] = { 0, 1, 0 }; - mathRotateVectorByInverseOfQuaternion( plusy, LSM6DSL.fqQuat, plusy ); + float plusy[3] = {0, 1, 0}; + mathRotateVectorByInverseOfQuaternion(plusy, LSM6DSL.fqQuat, plusy); *x = plusy[0] * 256; *y = plusy[1] * 256; *z = plusy[2] * 256; return ESP_OK; } - /** * @brief Get the current estimated quaternion of the IMU. * * @return ESP_OK */ -esp_err_t accelGetQuaternion(float * q) +esp_err_t accelGetQuaternion(float* q) { - float * fq = LSM6DSL.fqQuat; - q[0] = fq[0]; - q[1] = fq[1]; - q[2] = fq[2]; - q[3] = fq[3]; + float* fq = LSM6DSL.fqQuat; + q[0] = fq[0]; + q[1] = fq[1]; + q[2] = fq[2]; + q[3] = fq[3]; return ESP_OK; } @@ -806,18 +808,18 @@ esp_err_t accelGetQuaternion(float * q) */ esp_err_t accelPerformCal() { - LSM6DSL.performCal = 1; + LSM6DSL.performCal = 1; LSM6DSL.fvDeviation[0] = 1; LSM6DSL.fvDeviation[1] = 1; LSM6DSL.fvDeviation[2] = 1; - LSM6DSL.fvAverage[0] = 0; - LSM6DSL.fvAverage[1] = 0; - LSM6DSL.fvAverage[2] = 0; + LSM6DSL.fvAverage[0] = 0; + LSM6DSL.fvAverage[1] = 0; + LSM6DSL.fvAverage[2] = 0; - // Ignore failures here. - eraseNvsKey( "gyrocalx" ); - eraseNvsKey( "gyrocaly" ); - eraseNvsKey( "gyrocalz" ); + // Ignore failures here. + eraseNvsKey("gyrocalx"); + eraseNvsKey("gyrocaly"); + eraseNvsKey("gyrocalz"); return ESP_OK; } @@ -829,48 +831,47 @@ esp_err_t accelPerformCal() */ float accelGetStdDevInCal() { - if (LSM6DSL.performCal == 0) return 0.0f; - float * d = LSM6DSL.fvDeviation; - return (d[0] * d[0] + - d[1] * d[1] + - d[2] * d[2]); + if (LSM6DSL.performCal == 0) + return 0.0f; + float* d = LSM6DSL.fvDeviation; + return (d[0] * d[0] + d[1] * d[1] + d[2] * d[2]); } - - /** * @brief Reset the IMU's core registers. Should be done any time you are entering a mode with the IMU. * */ -void accelSetRegistersAndReset( void ) +void accelSetRegistersAndReset(void) { - LSM6DSLSet( LSM6DSL_FIFO_CTRL5, (0b0101 << 3) | 0b000 ); // Reset FIFO - LSM6DSLSet( LSM6DSL_FIFO_CTRL5, (0b0101 << 3) | 0b110 ); // 208 Hz ODR, Continuous mode. If the FIFO is full, the new sample overwrites the older one. - LSM6DSLSet( LSM6DSL_FIFO_CTRL3, 0b00001001 ); // Put both devices (Accel + Gyro) in FIFO. - LSM6DSLSet( LSM6DSL_CTRL1_XL, 0b01011001 ); // Setup accel (16 g's FS) - LSM6DSLSet( LSM6DSL_CTRL2_G, 0b01011100 ); // Setup gyro, 2000dps - LSM6DSLSet( LSM6DSL_CTRL4_C, 0x00 ); // Disable all filtering. - LSM6DSLSet( LSM6DSL_CTRL7_G, 0b00000000 ); // Setup gyro, not high performance mode = 0x80. High perf = 0x00 - LSM6DSLSet( LSM6DSL_FIFO_CTRL2, 0b00000000 ); //Temp not in fifo (Why no work?) - - memset( &LSM6DSL, 0, sizeof(LSM6DSL) ); - LSM6DSL.fqQuat[0] = 1; + LSM6DSLSet(LSM6DSL_FIFO_CTRL5, (0b0101 << 3) | 0b000); // Reset FIFO + LSM6DSLSet( + LSM6DSL_FIFO_CTRL5, + (0b0101 << 3) + | 0b110); // 208 Hz ODR, Continuous mode. If the FIFO is full, the new sample overwrites the older one. + LSM6DSLSet(LSM6DSL_FIFO_CTRL3, 0b00001001); // Put both devices (Accel + Gyro) in FIFO. + LSM6DSLSet(LSM6DSL_CTRL1_XL, 0b01011001); // Setup accel (16 g's FS) + LSM6DSLSet(LSM6DSL_CTRL2_G, 0b01011100); // Setup gyro, 2000dps + LSM6DSLSet(LSM6DSL_CTRL4_C, 0x00); // Disable all filtering. + LSM6DSLSet(LSM6DSL_CTRL7_G, 0b00000000); // Setup gyro, not high performance mode = 0x80. High perf = 0x00 + LSM6DSLSet(LSM6DSL_FIFO_CTRL2, 0b00000000); // Temp not in fifo (Why no work?) + + memset(&LSM6DSL, 0, sizeof(LSM6DSL)); + LSM6DSL.fqQuat[0] = 1; LSM6DSL.fqQuatLast[0] = 1; - LSM6DSL.sampCount = 0; - if( !readNvs32( "gyrocalx", (int32_t*)&LSM6DSL.fvBias[0] ) ) - { - LSM6DSL.performCal = 1; - LSM6DSL.fvBias[0] = 0; - } - if( !readNvs32( "gyrocaly", (int32_t*)&LSM6DSL.fvBias[1] ) ) - { - LSM6DSL.performCal = 1; - LSM6DSL.fvBias[1] = 0; - } - if( !readNvs32( "gyrocalz", (int32_t*)&LSM6DSL.fvBias[2] ) ) - { - LSM6DSL.performCal = 1; - LSM6DSL.fvBias[2] = 0; - } + LSM6DSL.sampCount = 0; + if (!readNvs32("gyrocalx", (int32_t*)&LSM6DSL.fvBias[0])) + { + LSM6DSL.performCal = 1; + LSM6DSL.fvBias[0] = 0; + } + if (!readNvs32("gyrocaly", (int32_t*)&LSM6DSL.fvBias[1])) + { + LSM6DSL.performCal = 1; + LSM6DSL.fvBias[1] = 0; + } + if (!readNvs32("gyrocalz", (int32_t*)&LSM6DSL.fvBias[2])) + { + LSM6DSL.performCal = 1; + LSM6DSL.fvBias[2] = 0; + } } - diff --git a/components/hdw-accel/include/hdw-accel.h b/components/hdw-accel/include/hdw-accel.h index 1ef687f77..731062d69 100644 --- a/components/hdw-accel/include/hdw-accel.h +++ b/components/hdw-accel/include/hdw-accel.h @@ -46,7 +46,7 @@ #include #include -typedef struct +typedef struct { int32_t temp; uint32_t computetime; @@ -55,7 +55,7 @@ typedef struct // Quats are wxyz. // You can take a vector, in controller space, rotate by this quat, and you get it in world space. float fqQuatLast[4]; // Delta - float fqQuat[4]; // Absolute + float fqQuat[4]; // Absolute // The last raw accelerometer (NOT FUSED) float fvLastAccelRaw[3]; @@ -79,11 +79,11 @@ typedef struct extern LSM6DSLData LSM6DSL; -esp_err_t initAccelerometer(i2c_port_t _i2c_port, gpio_num_t sda, gpio_num_t scl, gpio_pullup_t pullup, uint32_t clkHz ); +esp_err_t initAccelerometer(i2c_port_t _i2c_port, gpio_num_t sda, gpio_num_t scl, gpio_pullup_t pullup, uint32_t clkHz); esp_err_t deInitAccelerometer(void); esp_err_t accelGetAccelVecRaw(int16_t* x, int16_t* y, int16_t* z); esp_err_t accelGetOrientVec(int16_t* x, int16_t* y, int16_t* z); -esp_err_t accelGetQuaternion(float * q); +esp_err_t accelGetQuaternion(float* q); esp_err_t accelIntegrate(void); esp_err_t accelPerformCal(void); float accelGetStdDevInCal(void); @@ -93,16 +93,15 @@ void accelSetRegistersAndReset(void); float rsqrtf(float x); float mathsqrtf(float x); -void mathEulerToQuat(float * q, const float * euler); -void mathQuatApply(float * qout, const float * q1, const float * q2); -void mathQuatNormalize(float * qout, const float * qin ); -void mathCrossProduct(float * p, const float * a, const float * b); -void mathRotateVectorByInverseOfQuaternion(float * pout, const float * q, const float * p ); -void mathRotateVectorByQuaternion(float * pout, const float * q, const float * p); -esp_err_t GeneralSet( int dev, int reg, int val ); -esp_err_t LSM6DSLSet( int reg, int val ); -int GeneralI2CGet( int device, int reg, uint8_t * data, int data_len ); -int ReadLSM6DSL( uint8_t * data, int data_len ); - +void mathEulerToQuat(float* q, const float* euler); +void mathQuatApply(float* qout, const float* q1, const float* q2); +void mathQuatNormalize(float* qout, const float* qin); +void mathCrossProduct(float* p, const float* a, const float* b); +void mathRotateVectorByInverseOfQuaternion(float* pout, const float* q, const float* p); +void mathRotateVectorByQuaternion(float* pout, const float* q, const float* p); +esp_err_t GeneralSet(int dev, int reg, int val); +esp_err_t LSM6DSLSet(int reg, int val); +int GeneralI2CGet(int device, int reg, uint8_t* data, int data_len); +int ReadLSM6DSL(uint8_t* data, int data_len); #endif diff --git a/emulator/src/components/hdw-accel/hdw-accel.c b/emulator/src/components/hdw-accel/hdw-accel.c index 218aab012..244ca537c 100644 --- a/emulator/src/components/hdw-accel/hdw-accel.c +++ b/emulator/src/components/hdw-accel/hdw-accel.c @@ -29,7 +29,7 @@ LSM6DSLData LSM6DSL; * @param bandwidth The bandwidth to measure at, between ::QMA_BANDWIDTH_128_HZ and ::QMA_BANDWIDTH_1024_HZ * @return ESP_OK if the accelerometer initialized, or a non-zero value if it did not */ -esp_err_t initAccelerometer(i2c_port_t _i2c_port, gpio_num_t sda, gpio_num_t scl, gpio_pullup_t pullup, uint32_t clkHz ) +esp_err_t initAccelerometer(i2c_port_t _i2c_port, gpio_num_t sda, gpio_num_t scl, gpio_pullup_t pullup, uint32_t clkHz) { // Default to the swadge sitting still, face-up on a table somewhere on earth _accelX = 0; @@ -101,7 +101,7 @@ esp_err_t accelGetAccelVecRaw(int16_t* x, int16_t* y, int16_t* z) esp_err_t accelGetOrientVec(int16_t* x, int16_t* y, int16_t* z) { - return accelGetAccelVecRaw( x, y, z ); + return accelGetAccelVecRaw(x, y, z); } /** @@ -161,8 +161,8 @@ void emulatorSetAccelerometerRotation(int16_t value, uint16_t yaw, uint16_t pitc esp_err_t accelIntegrate() { - // Do nothing (is stub function) - return ESP_OK; + // Do nothing (is stub function) + return ESP_OK; } /** @@ -172,22 +172,21 @@ esp_err_t accelIntegrate() * @param q Pointer to the wzyz quaternion (float[4]) of the rotation. * @param p Pointer to the float[3] of the vector to rotates. */ -void mathRotateVectorByQuaternion(float * pout, const float * q, const float * p) +void mathRotateVectorByQuaternion(float* pout, const float* q, const float* p) { // return v + 2.0 * cross(q.xyz, cross(q.xyz, v) + q.w * v); float iqo[3]; - mathCrossProduct( iqo, q + 1 /*.xyz*/, p ); + mathCrossProduct(iqo, q + 1 /*.xyz*/, p); iqo[0] += q[0] * p[0]; iqo[1] += q[0] * p[1]; iqo[2] += q[0] * p[2]; float ret[3]; - mathCrossProduct( ret, q + 1 /*.xyz*/, iqo ); + mathCrossProduct(ret, q + 1 /*.xyz*/, iqo); pout[0] = ret[0] * 2.0 + p[0]; pout[1] = ret[1] * 2.0 + p[1]; pout[2] = ret[2] * 2.0 + p[2]; } - /** * @brief Perform a 3D cross product * @@ -195,13 +194,13 @@ void mathRotateVectorByQuaternion(float * pout, const float * q, const float * p * @param a Pointer to the float[3] of the cross product a vector. * @param a Pointer to the float[3] of the cross product b vector. */ -void mathCrossProduct(float * p, const float * a, const float * b) +void mathCrossProduct(float* p, const float* a, const float* b) { float tx = a[1] * b[2] - a[2] * b[1]; float ty = a[2] * b[0] - a[0] * b[2]; - p[2] = a[0] * b[1] - a[1] * b[0]; - p[1] = ty; - p[0] = tx; + p[2] = a[0] * b[1] - a[1] * b[0]; + p[1] = ty; + p[0] = tx; } // stub @@ -212,7 +211,7 @@ void accelSetRegistersAndReset() // stub esp_err_t accelPerformCal() { - return ESP_OK; + return ESP_OK; } // stub @@ -220,4 +219,3 @@ float accelGetStdDevInCal() { return 0.0f; } - diff --git a/main/modes/accelTest/accelTest.c b/main/modes/accelTest/accelTest.c index 0ad3d75a2..8fa043193 100644 --- a/main/modes/accelTest/accelTest.c +++ b/main/modes/accelTest/accelTest.c @@ -258,54 +258,54 @@ static void accelTestHandleInput(void) */ static void accelDrawBunny(void) { - // Produce a model matrix from a quaternion. - float plusx_out[3] = { 1, 0, 0 }; - float plusy_out[3] = { 0, 1, 0 }; - float plusz_out[3] = { 0, 0, 1 }; + float plusx_out[3] = {1, 0, 0}; + float plusy_out[3] = {0, 1, 0}; + float plusz_out[3] = {0, 0, 1}; - mathRotateVectorByQuaternion( plusy_out, LSM6DSL.fqQuat, plusy_out ); - mathRotateVectorByQuaternion( plusx_out, LSM6DSL.fqQuat, plusx_out ); - mathRotateVectorByQuaternion( plusz_out, LSM6DSL.fqQuat, plusz_out ); + mathRotateVectorByQuaternion(plusy_out, LSM6DSL.fqQuat, plusy_out); + mathRotateVectorByQuaternion(plusx_out, LSM6DSL.fqQuat, plusx_out); + mathRotateVectorByQuaternion(plusz_out, LSM6DSL.fqQuat, plusz_out); - int16_t bunny_verts_out[ sizeof(bunny_verts)/3/2*3 ]; + int16_t bunny_verts_out[sizeof(bunny_verts) / 3 / 2 * 3]; int i, vertices = 0; - for( i = 0; i < sizeof(bunny_verts)/2; i+= 3 ) + for (i = 0; i < sizeof(bunny_verts) / 2; i += 3) { // Performingthe transform this way is about 700us. - float bx = bunny_verts[i+2]; - float by = bunny_verts[i+1]; - float bz =-bunny_verts[i+0]; - float bunnyvert[3] = { - bx * plusx_out[0] + by * plusx_out[1] + bz * plusx_out[2], - bx * plusy_out[0] + by * plusy_out[1] + bz * plusy_out[2], - bx * plusz_out[0] + by * plusz_out[1] + bz * plusz_out[2] }; - bunny_verts_out[vertices*3+0] = bunnyvert[0]/250 + 280/2; - bunny_verts_out[vertices*3+1] = -bunnyvert[1]/250 + 240/2; // Convert from right-handed to left-handed coordinate frame. - bunny_verts_out[vertices*3+2] = bunnyvert[2]; + float bx = bunny_verts[i + 2]; + float by = bunny_verts[i + 1]; + float bz = -bunny_verts[i + 0]; + float bunnyvert[3] = {bx * plusx_out[0] + by * plusx_out[1] + bz * plusx_out[2], + bx * plusy_out[0] + by * plusy_out[1] + bz * plusy_out[2], + bx * plusz_out[0] + by * plusz_out[1] + bz * plusz_out[2]}; + bunny_verts_out[vertices * 3 + 0] = bunnyvert[0] / 250 + 280 / 2; + bunny_verts_out[vertices * 3 + 1] + = -bunnyvert[1] / 250 + 240 / 2; // Convert from right-handed to left-handed coordinate frame. + bunny_verts_out[vertices * 3 + 2] = bunnyvert[2]; vertices++; } int lines = 0; - for( i = 0; i < sizeof(bunny_lines); i+= 2 ) + for (i = 0; i < sizeof(bunny_lines); i += 2) { - int v1 = bunny_lines[i]*3; - int v2 = bunny_lines[i+1]*3; - float col = bunny_verts_out[v1+2]/2000 + 8; - if( col > 5 ) col = 5; - else if( col < 0 ) continue; - drawLineFast(bunny_verts_out[v1], bunny_verts_out[v1+1],bunny_verts_out[v2], bunny_verts_out[v2+1], col); + int v1 = bunny_lines[i] * 3; + int v2 = bunny_lines[i + 1] * 3; + float col = bunny_verts_out[v1 + 2] / 2000 + 8; + if (col > 5) + col = 5; + else if (col < 0) + continue; + drawLineFast(bunny_verts_out[v1], bunny_verts_out[v1 + 1], bunny_verts_out[v2], bunny_verts_out[v2 + 1], col); lines++; } } - /** * @brief Reset the accelerometer test mode variables */ static void accelTestReset(void) { - accelSetRegistersAndReset(); + accelSetRegistersAndReset(); accelTest->first = 0; accelTest->last = 0; @@ -337,7 +337,7 @@ static void accelTestReset(void) static void accelTestBackgroundDrawCallback(int16_t x, int16_t y, int16_t w, int16_t h, int16_t up, int16_t upNum) { accelIntegrate(); - fillDisplayArea(x, y, x+w, y+h, GRAPH_BG_COLOR ); + fillDisplayArea(x, y, x + w, y + h, GRAPH_BG_COLOR); } /** diff --git a/main/modes/accelTest/bunny.h b/main/modes/accelTest/bunny.h index 2197db363..7e23c6944 100644 --- a/main/modes/accelTest/bunny.h +++ b/main/modes/accelTest/bunny.h @@ -1,948 +1,119 @@ #include const int16_t bunny_verts[] = { - -18622, 11337, 3876, - -17108, 3601, 701, - -1011, 8368, 2344, - -1459, 6234, 7752, - -10384, -14050, 10578, - 9108, 9861, 4806, - -9196, 15608, 8497, - -16283, -6559, 2406, - -17417, -4365, 2474, - -18389, -1521, 4815, - 22219, -6826, 5279, - 23808, -7886, 4430, - -6203, 20422, -1965, - -12974, 18821, -4263, - -10629, 18285, -6951, - -10895, 20703, -13634, - -6764, -17568, -4534, - 902, 382, -7702, - -4497, -1374, -7109, - 5036, 9289, 6298, - 4426, 9208, -394, - -12239, -17581, -2460, - -18917, 5765, 7129, - -16378, -4102, 7845, - -2280, -12287, -3501, - -12167, 5531, -3406, - -17908, 15766, 4905, - -15097, 15808, 8499, - 9825, 797, 12888, - -18283, 10280, 791, - 11007, 9190, -244, - 11474, -579, -7581, - -12864, 16571, -195, - -14640, 16042, -1221, - -18332, 937, 2637, - -15354, 21703, -13553, - -7377, 15262, 2845, - -8799, 16541, 2581, - -19797, 6164, 4316, - -16158, 9222, 14006, - -14416, 12151, 13174, - -14141, 23469, -15211, - -11722, 23695, -17151, - -1964, -15383, 13753, - -7229, -8215, -4409, - -8680, -4539, -6585, - -6087, -5457, -5940, - -13259, 12437, -2907, - -10978, 15370, -1647, - -7396, -13948, 4297, - -7434, -11583, 4207, - -13163, 4985, 13937, - 1898, -3045, -11656, - 5657, -4100, -11268, - -3407, 15167, -771, - -17300, -2898, -778, - -10938, 12846, -2752, - -11874, 6626, 14013, - -10253, 21665, -18053, - 406, 21723, -6583, - -4073, 17099, -3927, - 19181, 786, 295, - 20580, -11662, -2457, - 22102, -13351, 369, - -12808, 13196, -5196, - 6063, 4006, 11507, - -7276, 12853, 114, - -12565, -2250, 11523, - -14217, 2765, 9542, - 5881, -11781, 14728, - 17536, -16747, 2519, - 18781, -2145, -2653, - -10697, -17317, 5038, - -11142, 4154, 9941, - 9481, -2448, 14961, - 8235, -10250, 14859, - -18197, 11232, 7365, - -19184, 6914, 11000, - -10944, 15814, -10459, - 3849, -13857, 12844, - -7194, 12657, 2472, - 11483, -11451, 13236, - 9840, -14518, 12029, - 23883, -9568, 1171, - -10820, 18420, -17126, - -9924, -1255, 12014, - 12198, 1609, 11989, - -10748, 15532, 9298, - -9741, -5600, 11373, - -13269, -6464, 9384, - 19845, -7054, 398, - 11278, -5139, 14319, - -6202, 16623, 1654, - -9097, -11092, 997, - 19002, -13005, 8517, - 4327, 20154, -7214, - 16851, 4385, 6126, - 17234, -11990, 8371, - -9161, 14173, -1062, - 12680, -17098, -1248, - -9040, -10189, -1993, - -13250, 21697, -15633, - -12137, 16307, -15587, - -7565, 5595, 8715, - -14057, 14807, -8716, - 5154, 1528, 11805, - -11515, -15866, -2486, - -6006, -6786, 10928, - -6082, -14740, -3845, - -8903, -15390, 12402, - -6839, -1601, 12095, - 21051, -9013, 7491, - 19199, -7770, 6398, - -4794, -14773, -7000, - -6012, 2917, 10643, - 5839, -6447, 15623, - 12814, 6195, 8839, - -13806, -17119, 795, - 13926, -16890, 6446, - -6942, -12919, 9554, - -3844, -13534, 10374, - -4457, 1559, 10787, - -18851, 14741, 3666, - -5487, 7874, 6174, - 3966, 23210, -9228, - 15092, -1954, -6602, - 16589, -4402, -5654, - -4916, -3395, 11258, - -3318, -1827, 11642, - -3552, 20709, -364, - -2107, -3341, 11219, - 3977, 6907, 9497, - -602, 2200, 11199, - -931, -11679, 9396, - -655, 23590, -2817, - 4846, -113, 14813, - -218, -281, 12015, - 22675, -7139, 849, - 10125, 7755, 8482, - 42, -14047, 14169, - 6394, 4546, -6382, - 1421, -12865, 12448, - 1947, 25502, -6345, - -7830, -12984, 6944, - -3380, -3774, -6830, - 21840, -10979, 7400, - -2985, -17516, -9333, - 308, -13721, -8558, - 579, -4080, 15241, - 1959, -8651, 14162, - -15752, 11553, -1543, - 16322, -1004, 10720, - 1282, 17758, -1900, - -4127, -17024, 11409, - 14526, -13115, -5876, - 12320, -14519, -6873, - 17448, -8568, -4231, - 17020, -11895, -2736, - 12869, -8659, 12529, - -467, -16936, 14591, - 16302, 2969, -3536, - -9403, -13394, 1276, - 22627, -13027, 5091, - -10928, 11196, 9917, - -10058, 7526, 11324, - 16770, -9731, 9964, - 18984, -721, 6457, - -1915, 3997, -5987, - 15735, -5467, 11114, - 848, 2358, -7139, - 3381, 21754, -6535, - 15097, -13331, 9377, - 11684, -16912, 11440, - 3494, 18714, -4534, - -8499, 12161, 7570, - -10517, -17147, 11966, - 17230, -15895, -966, - -1186, -13034, -7256, - -14490, -1096, -4663, - 10883, -17063, -6685, - -2943, -17367, 9110, - -17066, 3515, 5290, - -17896, 13182, 9407, - -18402, 4534, 10173, - 8336, 933, -6933, - -17574, 6887, -1239, - 5278, -17480, -7386, - -12685, -14815, 928, - -11989, -16758, 7220, - -94, -1429, -10547, - -15531, 4607, 12668, - 12598, -10979, -8259, - -5175, -17496, -8370, - 9509, -17363, -4797, - 7835, -733, -9933, - 20083, -6435, 3491, - -12583, 16811, 5585, - -10347, 16387, 415, - -5156, 9190, 3024, - -1162, -5218, -11018, - 15898, 5878, 927, - -17104, 635, 7228, - -15184, 18465, -8552, - 13851, 7498, 5806, - 12339, 1989, -6235, - -8783, -13504, -2022, - -7103, 9054, -1646, - -8929, -480, -6679, - -17555, 5593, 12647, - -8668, 3516, -5825, - -362, -10012, -9571, - 11355, 6507, -4159, - -13316, 15141, -12590, - 12642, -17080, 1813, - -14678, 14770, -2184, - -15414, -5951, -2011, - -12268, -6067, -4971, - -16604, -530, -2303, - -2216, 7063, -2848, - 4859, 7950, -3136, - 10975, -17013, 371, - -5388, 7670, -3722, - -4905, -17613, 6862, - 8484, -12836, -9001, - 9315, -6801, -9938, - 7941, -14924, -7560, - 8259, -9562, -9772, - 2547, -9033, -11113, - 4684, -13353, -9431, - -7524, -9388, 8770, - 10549, -17413, 5320, - 9878, -16766, 9250, - 2213, -17293, 11551, - 13056, -16797, 3233, - 11736, -17220, 5482, - -544, -17336, 11491, - -13043, -9710, 3903, - -8320, -17724, 3639, - -6497, -17425, 1846, - -3296, -17519, -6480, + -18622, 11337, 3876, -17108, 3601, 701, -1011, 8368, 2344, -1459, 6234, 7752, -10384, -14050, + 10578, 9108, 9861, 4806, -9196, 15608, 8497, -16283, -6559, 2406, -17417, -4365, 2474, -18389, + -1521, 4815, 22219, -6826, 5279, 23808, -7886, 4430, -6203, 20422, -1965, -12974, 18821, -4263, + -10629, 18285, -6951, -10895, 20703, -13634, -6764, -17568, -4534, 902, 382, -7702, -4497, -1374, + -7109, 5036, 9289, 6298, 4426, 9208, -394, -12239, -17581, -2460, -18917, 5765, 7129, -16378, + -4102, 7845, -2280, -12287, -3501, -12167, 5531, -3406, -17908, 15766, 4905, -15097, 15808, 8499, + 9825, 797, 12888, -18283, 10280, 791, 11007, 9190, -244, 11474, -579, -7581, -12864, 16571, + -195, -14640, 16042, -1221, -18332, 937, 2637, -15354, 21703, -13553, -7377, 15262, 2845, -8799, + 16541, 2581, -19797, 6164, 4316, -16158, 9222, 14006, -14416, 12151, 13174, -14141, 23469, -15211, + -11722, 23695, -17151, -1964, -15383, 13753, -7229, -8215, -4409, -8680, -4539, -6585, -6087, -5457, + -5940, -13259, 12437, -2907, -10978, 15370, -1647, -7396, -13948, 4297, -7434, -11583, 4207, -13163, + 4985, 13937, 1898, -3045, -11656, 5657, -4100, -11268, -3407, 15167, -771, -17300, -2898, -778, + -10938, 12846, -2752, -11874, 6626, 14013, -10253, 21665, -18053, 406, 21723, -6583, -4073, 17099, + -3927, 19181, 786, 295, 20580, -11662, -2457, 22102, -13351, 369, -12808, 13196, -5196, 6063, + 4006, 11507, -7276, 12853, 114, -12565, -2250, 11523, -14217, 2765, 9542, 5881, -11781, 14728, + 17536, -16747, 2519, 18781, -2145, -2653, -10697, -17317, 5038, -11142, 4154, 9941, 9481, -2448, + 14961, 8235, -10250, 14859, -18197, 11232, 7365, -19184, 6914, 11000, -10944, 15814, -10459, 3849, + -13857, 12844, -7194, 12657, 2472, 11483, -11451, 13236, 9840, -14518, 12029, 23883, -9568, 1171, + -10820, 18420, -17126, -9924, -1255, 12014, 12198, 1609, 11989, -10748, 15532, 9298, -9741, -5600, + 11373, -13269, -6464, 9384, 19845, -7054, 398, 11278, -5139, 14319, -6202, 16623, 1654, -9097, + -11092, 997, 19002, -13005, 8517, 4327, 20154, -7214, 16851, 4385, 6126, 17234, -11990, 8371, + -9161, 14173, -1062, 12680, -17098, -1248, -9040, -10189, -1993, -13250, 21697, -15633, -12137, 16307, + -15587, -7565, 5595, 8715, -14057, 14807, -8716, 5154, 1528, 11805, -11515, -15866, -2486, -6006, + -6786, 10928, -6082, -14740, -3845, -8903, -15390, 12402, -6839, -1601, 12095, 21051, -9013, 7491, + 19199, -7770, 6398, -4794, -14773, -7000, -6012, 2917, 10643, 5839, -6447, 15623, 12814, 6195, + 8839, -13806, -17119, 795, 13926, -16890, 6446, -6942, -12919, 9554, -3844, -13534, 10374, -4457, + 1559, 10787, -18851, 14741, 3666, -5487, 7874, 6174, 3966, 23210, -9228, 15092, -1954, -6602, + 16589, -4402, -5654, -4916, -3395, 11258, -3318, -1827, 11642, -3552, 20709, -364, -2107, -3341, + 11219, 3977, 6907, 9497, -602, 2200, 11199, -931, -11679, 9396, -655, 23590, -2817, 4846, + -113, 14813, -218, -281, 12015, 22675, -7139, 849, 10125, 7755, 8482, 42, -14047, 14169, + 6394, 4546, -6382, 1421, -12865, 12448, 1947, 25502, -6345, -7830, -12984, 6944, -3380, -3774, + -6830, 21840, -10979, 7400, -2985, -17516, -9333, 308, -13721, -8558, 579, -4080, 15241, 1959, + -8651, 14162, -15752, 11553, -1543, 16322, -1004, 10720, 1282, 17758, -1900, -4127, -17024, 11409, + 14526, -13115, -5876, 12320, -14519, -6873, 17448, -8568, -4231, 17020, -11895, -2736, 12869, -8659, + 12529, -467, -16936, 14591, 16302, 2969, -3536, -9403, -13394, 1276, 22627, -13027, 5091, -10928, + 11196, 9917, -10058, 7526, 11324, 16770, -9731, 9964, 18984, -721, 6457, -1915, 3997, -5987, + 15735, -5467, 11114, 848, 2358, -7139, 3381, 21754, -6535, 15097, -13331, 9377, 11684, -16912, + 11440, 3494, 18714, -4534, -8499, 12161, 7570, -10517, -17147, 11966, 17230, -15895, -966, -1186, + -13034, -7256, -14490, -1096, -4663, 10883, -17063, -6685, -2943, -17367, 9110, -17066, 3515, 5290, + -17896, 13182, 9407, -18402, 4534, 10173, 8336, 933, -6933, -17574, 6887, -1239, 5278, -17480, + -7386, -12685, -14815, 928, -11989, -16758, 7220, -94, -1429, -10547, -15531, 4607, 12668, 12598, + -10979, -8259, -5175, -17496, -8370, 9509, -17363, -4797, 7835, -733, -9933, 20083, -6435, 3491, + -12583, 16811, 5585, -10347, 16387, 415, -5156, 9190, 3024, -1162, -5218, -11018, 15898, 5878, + 927, -17104, 635, 7228, -15184, 18465, -8552, 13851, 7498, 5806, 12339, 1989, -6235, -8783, + -13504, -2022, -7103, 9054, -1646, -8929, -480, -6679, -17555, 5593, 12647, -8668, 3516, -5825, + -362, -10012, -9571, 11355, 6507, -4159, -13316, 15141, -12590, 12642, -17080, 1813, -14678, 14770, + -2184, -15414, -5951, -2011, -12268, -6067, -4971, -16604, -530, -2303, -2216, 7063, -2848, 4859, + 7950, -3136, 10975, -17013, 371, -5388, 7670, -3722, -4905, -17613, 6862, 8484, -12836, -9001, + 9315, -6801, -9938, 7941, -14924, -7560, 8259, -9562, -9772, 2547, -9033, -11113, 4684, -13353, + -9431, -7524, -9388, 8770, 10549, -17413, 5320, 9878, -16766, 9250, 2213, -17293, 11551, 13056, + -16797, 3233, 11736, -17220, 5482, -544, -17336, 11491, -13043, -9710, 3903, -8320, -17724, 3639, + -6497, -17425, 1846, -3296, -17519, -6480, }; const uint8_t bunny_lines[] = { - 1, 217, - 19, 3, - 19, 2, - 46, 18, - 39, 51, - 194, 224, - 62, 63, - 71, 195, - 0, 214, - 180, 235, - 234, 118, - 234, 70, - 71, 156, - 71, 90, - 135, 115, - 88, 110, - 4, 119, - 86, 116, - 139, 43, - 2, 198, - 0, 150, - 131, 132, - 148, 115, - 171, 165, - 171, 82, - 163, 6, - 168, 81, - 168, 158, - 80, 54, - 69, 79, - 116, 203, - 116, 138, - 27, 182, - 69, 82, - 77, 39, - 209, 25, - 209, 207, - 212, 101, - 140, 169, - 204, 184, - 194, 189, - 194, 53, - 230, 234, - 8, 23, - 98, 206, - 20, 5, - 199, 52, - 155, 223, - 51, 57, - 226, 228, - 230, 172, - 230, 231, - 65, 138, - 230, 118, - 69, 81, - 79, 82, - 21, 72, - 3, 131, - 237, 238, - 123, 174, - 105, 116, - 107, 119, - 54, 60, - 121, 127, - 84, 15, - 139, 141, - 28, 74, - 142, 134, - 54, 152, - 111, 11, - 71, 160, - 57, 164, - 111, 10, - 109, 175, - 109, 153, - 144, 24, - 96, 151, - 64, 104, - 170, 95, - 84, 58, - 21, 237, - 159, 43, - 159, 153, - 179, 155, - 7, 23, - 38, 181, - 184, 194, - 184, 17, - 196, 32, - 175, 188, - 31, 204, - 203, 30, - 139, 133, - 44, 205, - 77, 208, - 39, 182, - 31, 184, - 211, 204, - 64, 212, - 122, 214, - 216, 45, - 204, 160, - 180, 153, - 61, 195, - 213, 233, - 213, 234, - 213, 70, - 135, 74, - 156, 90, - 7, 236, - 56, 98, - 105, 65, - 5, 19, - 10, 11, - 197, 98, - 25, 221, - 15, 14, - 62, 157, - 53, 227, - 65, 132, - 123, 103, - 107, 127, - 107, 128, - 65, 131, - 121, 132, - 29, 38, - 29, 185, - 76, 182, - 80, 174, - 170, 124, - 152, 173, - 80, 36, - 151, 91, - 79, 172, - 151, 168, - 153, 43, - 99, 213, - 113, 16, - 144, 18, - 37, 197, - 148, 149, - 109, 119, - 172, 231, - 8, 9, - 67, 85, - 183, 208, - 68, 183, - 115, 91, - 146, 225, - 41, 15, - 214, 150, - 13, 14, - 191, 223, - 191, 226, - 8, 55, - 40, 57, - 224, 53, - 226, 227, - 186, 225, - 112, 10, - 146, 147, - 136, 135, - 57, 163, - 21, 117, - 79, 139, - 216, 100, - 189, 144, - 130, 148, - 77, 183, - 38, 185, - 180, 175, - 146, 192, - 73, 114, - 142, 170, - 142, 124, - 183, 190, - 6, 174, - 54, 36, - 68, 201, - 35, 101, - 199, 144, - 47, 214, - 2, 218, - 195, 137, - 112, 195, - 46, 24, - 160, 30, - 150, 29, - 120, 139, - 43, 120, - 99, 70, - 127, 128, - 92, 134, - 81, 158, - 129, 92, - 19, 20, - 224, 227, - 228, 227, - 59, 124, - 162, 118, - 19, 138, - 159, 235, - 157, 90, - 41, 13, - 195, 90, - 68, 190, - 99, 62, - 59, 95, - 76, 0, - 238, 239, - 80, 206, - 87, 27, - 87, 196, - 135, 105, - 200, 96, - 203, 138, - 178, 209, - 178, 25, - 205, 106, - 210, 144, - 30, 211, - 16, 21, - 31, 125, - 153, 175, - 123, 198, - 63, 162, - 68, 22, - 124, 95, - 221, 2, - 102, 64, - 5, 203, - 122, 0, - 128, 132, - 128, 136, - 54, 173, - 91, 75, - 192, 147, - 148, 135, - 148, 136, - 154, 155, - 91, 158, - 218, 20, - 13, 35, - 27, 40, - 155, 191, - 225, 223, - 59, 60, - 102, 78, - 40, 87, - 236, 89, - 117, 106, - 179, 225, - 32, 197, - 181, 201, - 99, 176, - 197, 12, - 31, 194, - 120, 153, - 192, 113, - 204, 140, - 122, 76, - 122, 182, - 5, 138, - 115, 74, - 132, 105, - 111, 145, - 149, 115, - 161, 93, - 87, 163, - 24, 113, - 103, 114, - 69, 115, - 110, 121, - 139, 159, - 11, 137, - 67, 68, - 72, 49, - 215, 236, - 23, 236, - 72, 237, - 101, 102, - 21, 106, - 100, 215, - 213, 220, - 0, 38, - 185, 25, - 34, 9, - 39, 40, - 49, 50, - 155, 225, - 34, 217, - 223, 228, - 117, 72, - 101, 58, - 211, 140, - 90, 137, - 24, 177, - 211, 219, - 16, 192, - 233, 234, - 12, 142, - 27, 196, - 206, 198, - 32, 48, - 101, 42, - 187, 205, - 59, 12, - 1, 181, - 206, 2, - 49, 161, - 187, 161, - 72, 188, - 166, 151, - 176, 62, - 176, 63, - 72, 143, - 2, 123, - 136, 105, - 193, 239, - 173, 60, - 70, 162, - 114, 110, - 28, 105, - 83, 137, - 231, 232, - 25, 47, - 215, 55, - 161, 205, - 106, 187, - 215, 7, - 167, 209, - 44, 45, - 44, 46, - 45, 18, - 13, 48, - 52, 53, - 199, 210, - 140, 167, - 147, 113, - 104, 214, - 35, 33, - 61, 200, - 98, 60, - 91, 74, - 40, 182, - 155, 157, - 172, 118, - 193, 186, - 166, 165, - 91, 168, - 3, 121, - 100, 236, - 166, 112, - 193, 146, - 155, 99, - 57, 73, - 66, 60, - 156, 126, - 1, 185, - 71, 125, - 49, 187, - 137, 62, - 137, 10, - 6, 196, - 203, 200, - 28, 135, - 93, 49, - 102, 58, - 104, 212, - 217, 25, - 6, 37, - 6, 87, - 39, 57, - 64, 78, - 64, 56, - 68, 73, - 94, 111, - 111, 112, - 69, 149, - 107, 120, - 107, 130, - 136, 132, - 223, 226, - 94, 145, - 141, 149, - 145, 162, - 68, 51, - 164, 73, - 92, 170, - 164, 103, - 92, 142, - 83, 162, - 106, 16, - 99, 179, - 70, 176, - 54, 92, - 22, 181, - 22, 183, - 171, 118, - 52, 194, - 197, 196, - 197, 48, - 207, 178, - 23, 201, - 185, 150, - 101, 214, - 160, 211, - 52, 189, - 216, 178, - 207, 45, - 14, 48, - 13, 32, - 36, 92, - 36, 37, - 45, 46, - 191, 224, - 23, 67, - 67, 73, - 61, 166, - 88, 67, - 88, 89, - 23, 89, - 37, 129, - 16, 238, - 175, 72, - 73, 103, - 106, 108, - 86, 28, - 3, 123, - 172, 232, - 110, 127, - 114, 123, - 60, 95, - 114, 3, - 61, 96, - 75, 158, - 195, 166, - 79, 159, - 125, 160, - 219, 30, - 163, 164, - 193, 179, - 172, 171, - 118, 94, - 125, 204, - 102, 212, - 101, 202, - 181, 68, - 212, 214, - 62, 90, - 93, 100, - 10, 195, - 9, 201, - 228, 147, - 228, 210, - 227, 199, - 227, 210, - 50, 93, - 236, 50, - 236, 229, - 83, 11, - 85, 110, - 206, 221, - 180, 222, - 167, 207, - 29, 0, - 78, 56, - 69, 141, - 3, 132, - 128, 130, - 12, 129, - 129, 134, - 12, 134, - 133, 149, - 9, 23, - 78, 14, - 17, 189, - 156, 191, - 173, 95, - 239, 146, - 17, 140, - 202, 214, - 215, 178, - 215, 217, - 186, 146, - 214, 26, - 189, 199, - 205, 100, - 193, 220, - 62, 83, - 22, 76, - 76, 77, - 232, 159, - 232, 235, - 220, 179, - 93, 205, - 217, 55, - 17, 18, - 17, 144, - 9, 217, - 34, 201, - 34, 181, - 19, 131, - 70, 63, - 37, 12, - 86, 151, - 156, 157, - 145, 11, - 97, 118, - 119, 120, - 37, 92, - 120, 133, - 22, 77, - 130, 133, - 13, 33, - 113, 177, - 200, 160, - 126, 191, - 108, 205, - 13, 15, - 50, 229, - 99, 157, - 33, 202, - 142, 59, - 33, 214, - 194, 125, - 51, 73, - 81, 165, - 81, 171, - 187, 72, - 164, 123, - 97, 171, - 143, 188, - 11, 162, - 151, 165, - 36, 174, - 56, 14, - 114, 121, - 131, 138, - 6, 36, - 75, 81, - 116, 151, - 2, 3, - 66, 206, - 33, 26, - 33, 196, - 221, 209, - 196, 26, - 172, 159, - 185, 47, - 44, 216, - 44, 100, - 224, 226, - 178, 45, - 71, 61, - 70, 118, - 222, 237, - 222, 72, - 150, 47, - 91, 86, - 91, 28, - 37, 196, - 35, 202, - 96, 116, - 25, 206, - 190, 39, - 190, 208, - 187, 117, - 42, 58, - 210, 147, - 214, 64, - 1, 34, - 97, 165, - 18, 167, - 236, 93, - 102, 84, - 163, 174, - 92, 173, - 108, 16, - 18, 169, - 77, 182, - 4, 175, - 113, 108, - 119, 143, - 35, 41, - 143, 49, - 17, 194, - 192, 239, - 97, 111, - 97, 112, - 107, 110, - 86, 105, - 140, 184, - 121, 128, - 120, 130, - 143, 50, - 54, 66, - 64, 47, - 92, 152, - 40, 163, - 8, 215, - 20, 219, - 12, 98, - 66, 80, - 81, 82, - 89, 67, - 73, 85, - 66, 98, - 125, 224, - 85, 88, - 25, 56, - 73, 110, - 41, 101, - 24, 210, - 80, 198, - 107, 229, - 96, 203, - 200, 30, - 60, 12, - 149, 130, - 229, 119, - 177, 147, - 108, 24, - 78, 84, - 78, 15, - 154, 191, - 169, 17, - 71, 126, - 179, 186, - 51, 190, - 82, 172, - 170, 173, - 4, 188, - 167, 169, - 48, 56, - 4, 143, - 218, 221, - 75, 115, - 225, 228, - 30, 20, - 153, 235, - 69, 75, - 116, 65, - 80, 123, - 96, 166, - 89, 229, - 180, 72, - 7, 8, - 167, 219, - 1, 38, - 41, 42, - 125, 191, - 20, 2, - 177, 210, - 215, 216, - 167, 20, - 15, 42, - 143, 229, - 24, 44, - 24, 205, - 38, 22, - 39, 208, - 67, 201, - 18, 207, - 76, 38, - 157, 191, - 140, 219, - 157, 154, - 178, 217, - 26, 182, - 63, 83, - 162, 94, - 165, 168, - 164, 174, - 165, 112, - 15, 58, - 133, 141, - 141, 79, - 26, 122, - 160, 61, - 109, 120, - 125, 126, - 130, 136, - 225, 147, - 53, 199, - 16, 237, - 94, 97, - 16, 239, - 33, 32, - 88, 107, - 4, 109, - 88, 229, - 1, 25, - 99, 220, - 48, 98, - 167, 218, - 26, 27, - 5, 30, - 167, 221, - 144, 46, - 47, 56, - 9, 55, - 206, 56, + 1, 217, 19, 3, 19, 2, 46, 18, 39, 51, 194, 224, 62, 63, 71, 195, 0, 214, 180, 235, 234, 118, 234, + 70, 71, 156, 71, 90, 135, 115, 88, 110, 4, 119, 86, 116, 139, 43, 2, 198, 0, 150, 131, 132, 148, 115, + 171, 165, 171, 82, 163, 6, 168, 81, 168, 158, 80, 54, 69, 79, 116, 203, 116, 138, 27, 182, 69, 82, 77, + 39, 209, 25, 209, 207, 212, 101, 140, 169, 204, 184, 194, 189, 194, 53, 230, 234, 8, 23, 98, 206, 20, 5, + 199, 52, 155, 223, 51, 57, 226, 228, 230, 172, 230, 231, 65, 138, 230, 118, 69, 81, 79, 82, 21, 72, 3, + 131, 237, 238, 123, 174, 105, 116, 107, 119, 54, 60, 121, 127, 84, 15, 139, 141, 28, 74, 142, 134, 54, 152, + 111, 11, 71, 160, 57, 164, 111, 10, 109, 175, 109, 153, 144, 24, 96, 151, 64, 104, 170, 95, 84, 58, 21, + 237, 159, 43, 159, 153, 179, 155, 7, 23, 38, 181, 184, 194, 184, 17, 196, 32, 175, 188, 31, 204, 203, 30, + 139, 133, 44, 205, 77, 208, 39, 182, 31, 184, 211, 204, 64, 212, 122, 214, 216, 45, 204, 160, 180, 153, 61, + 195, 213, 233, 213, 234, 213, 70, 135, 74, 156, 90, 7, 236, 56, 98, 105, 65, 5, 19, 10, 11, 197, 98, + 25, 221, 15, 14, 62, 157, 53, 227, 65, 132, 123, 103, 107, 127, 107, 128, 65, 131, 121, 132, 29, 38, 29, + 185, 76, 182, 80, 174, 170, 124, 152, 173, 80, 36, 151, 91, 79, 172, 151, 168, 153, 43, 99, 213, 113, 16, + 144, 18, 37, 197, 148, 149, 109, 119, 172, 231, 8, 9, 67, 85, 183, 208, 68, 183, 115, 91, 146, 225, 41, + 15, 214, 150, 13, 14, 191, 223, 191, 226, 8, 55, 40, 57, 224, 53, 226, 227, 186, 225, 112, 10, 146, 147, + 136, 135, 57, 163, 21, 117, 79, 139, 216, 100, 189, 144, 130, 148, 77, 183, 38, 185, 180, 175, 146, 192, 73, + 114, 142, 170, 142, 124, 183, 190, 6, 174, 54, 36, 68, 201, 35, 101, 199, 144, 47, 214, 2, 218, 195, 137, + 112, 195, 46, 24, 160, 30, 150, 29, 120, 139, 43, 120, 99, 70, 127, 128, 92, 134, 81, 158, 129, 92, 19, + 20, 224, 227, 228, 227, 59, 124, 162, 118, 19, 138, 159, 235, 157, 90, 41, 13, 195, 90, 68, 190, 99, 62, + 59, 95, 76, 0, 238, 239, 80, 206, 87, 27, 87, 196, 135, 105, 200, 96, 203, 138, 178, 209, 178, 25, 205, + 106, 210, 144, 30, 211, 16, 21, 31, 125, 153, 175, 123, 198, 63, 162, 68, 22, 124, 95, 221, 2, 102, 64, + 5, 203, 122, 0, 128, 132, 128, 136, 54, 173, 91, 75, 192, 147, 148, 135, 148, 136, 154, 155, 91, 158, 218, + 20, 13, 35, 27, 40, 155, 191, 225, 223, 59, 60, 102, 78, 40, 87, 236, 89, 117, 106, 179, 225, 32, 197, + 181, 201, 99, 176, 197, 12, 31, 194, 120, 153, 192, 113, 204, 140, 122, 76, 122, 182, 5, 138, 115, 74, 132, + 105, 111, 145, 149, 115, 161, 93, 87, 163, 24, 113, 103, 114, 69, 115, 110, 121, 139, 159, 11, 137, 67, 68, + 72, 49, 215, 236, 23, 236, 72, 237, 101, 102, 21, 106, 100, 215, 213, 220, 0, 38, 185, 25, 34, 9, 39, + 40, 49, 50, 155, 225, 34, 217, 223, 228, 117, 72, 101, 58, 211, 140, 90, 137, 24, 177, 211, 219, 16, 192, + 233, 234, 12, 142, 27, 196, 206, 198, 32, 48, 101, 42, 187, 205, 59, 12, 1, 181, 206, 2, 49, 161, 187, + 161, 72, 188, 166, 151, 176, 62, 176, 63, 72, 143, 2, 123, 136, 105, 193, 239, 173, 60, 70, 162, 114, 110, + 28, 105, 83, 137, 231, 232, 25, 47, 215, 55, 161, 205, 106, 187, 215, 7, 167, 209, 44, 45, 44, 46, 45, + 18, 13, 48, 52, 53, 199, 210, 140, 167, 147, 113, 104, 214, 35, 33, 61, 200, 98, 60, 91, 74, 40, 182, + 155, 157, 172, 118, 193, 186, 166, 165, 91, 168, 3, 121, 100, 236, 166, 112, 193, 146, 155, 99, 57, 73, 66, + 60, 156, 126, 1, 185, 71, 125, 49, 187, 137, 62, 137, 10, 6, 196, 203, 200, 28, 135, 93, 49, 102, 58, + 104, 212, 217, 25, 6, 37, 6, 87, 39, 57, 64, 78, 64, 56, 68, 73, 94, 111, 111, 112, 69, 149, 107, + 120, 107, 130, 136, 132, 223, 226, 94, 145, 141, 149, 145, 162, 68, 51, 164, 73, 92, 170, 164, 103, 92, 142, + 83, 162, 106, 16, 99, 179, 70, 176, 54, 92, 22, 181, 22, 183, 171, 118, 52, 194, 197, 196, 197, 48, 207, + 178, 23, 201, 185, 150, 101, 214, 160, 211, 52, 189, 216, 178, 207, 45, 14, 48, 13, 32, 36, 92, 36, 37, + 45, 46, 191, 224, 23, 67, 67, 73, 61, 166, 88, 67, 88, 89, 23, 89, 37, 129, 16, 238, 175, 72, 73, + 103, 106, 108, 86, 28, 3, 123, 172, 232, 110, 127, 114, 123, 60, 95, 114, 3, 61, 96, 75, 158, 195, 166, + 79, 159, 125, 160, 219, 30, 163, 164, 193, 179, 172, 171, 118, 94, 125, 204, 102, 212, 101, 202, 181, 68, 212, + 214, 62, 90, 93, 100, 10, 195, 9, 201, 228, 147, 228, 210, 227, 199, 227, 210, 50, 93, 236, 50, 236, 229, + 83, 11, 85, 110, 206, 221, 180, 222, 167, 207, 29, 0, 78, 56, 69, 141, 3, 132, 128, 130, 12, 129, 129, + 134, 12, 134, 133, 149, 9, 23, 78, 14, 17, 189, 156, 191, 173, 95, 239, 146, 17, 140, 202, 214, 215, 178, + 215, 217, 186, 146, 214, 26, 189, 199, 205, 100, 193, 220, 62, 83, 22, 76, 76, 77, 232, 159, 232, 235, 220, + 179, 93, 205, 217, 55, 17, 18, 17, 144, 9, 217, 34, 201, 34, 181, 19, 131, 70, 63, 37, 12, 86, 151, + 156, 157, 145, 11, 97, 118, 119, 120, 37, 92, 120, 133, 22, 77, 130, 133, 13, 33, 113, 177, 200, 160, 126, + 191, 108, 205, 13, 15, 50, 229, 99, 157, 33, 202, 142, 59, 33, 214, 194, 125, 51, 73, 81, 165, 81, 171, + 187, 72, 164, 123, 97, 171, 143, 188, 11, 162, 151, 165, 36, 174, 56, 14, 114, 121, 131, 138, 6, 36, 75, + 81, 116, 151, 2, 3, 66, 206, 33, 26, 33, 196, 221, 209, 196, 26, 172, 159, 185, 47, 44, 216, 44, 100, + 224, 226, 178, 45, 71, 61, 70, 118, 222, 237, 222, 72, 150, 47, 91, 86, 91, 28, 37, 196, 35, 202, 96, + 116, 25, 206, 190, 39, 190, 208, 187, 117, 42, 58, 210, 147, 214, 64, 1, 34, 97, 165, 18, 167, 236, 93, + 102, 84, 163, 174, 92, 173, 108, 16, 18, 169, 77, 182, 4, 175, 113, 108, 119, 143, 35, 41, 143, 49, 17, + 194, 192, 239, 97, 111, 97, 112, 107, 110, 86, 105, 140, 184, 121, 128, 120, 130, 143, 50, 54, 66, 64, 47, + 92, 152, 40, 163, 8, 215, 20, 219, 12, 98, 66, 80, 81, 82, 89, 67, 73, 85, 66, 98, 125, 224, 85, + 88, 25, 56, 73, 110, 41, 101, 24, 210, 80, 198, 107, 229, 96, 203, 200, 30, 60, 12, 149, 130, 229, 119, + 177, 147, 108, 24, 78, 84, 78, 15, 154, 191, 169, 17, 71, 126, 179, 186, 51, 190, 82, 172, 170, 173, 4, + 188, 167, 169, 48, 56, 4, 143, 218, 221, 75, 115, 225, 228, 30, 20, 153, 235, 69, 75, 116, 65, 80, 123, + 96, 166, 89, 229, 180, 72, 7, 8, 167, 219, 1, 38, 41, 42, 125, 191, 20, 2, 177, 210, 215, 216, 167, + 20, 15, 42, 143, 229, 24, 44, 24, 205, 38, 22, 39, 208, 67, 201, 18, 207, 76, 38, 157, 191, 140, 219, + 157, 154, 178, 217, 26, 182, 63, 83, 162, 94, 165, 168, 164, 174, 165, 112, 15, 58, 133, 141, 141, 79, 26, + 122, 160, 61, 109, 120, 125, 126, 130, 136, 225, 147, 53, 199, 16, 237, 94, 97, 16, 239, 33, 32, 88, 107, + 4, 109, 88, 229, 1, 25, 99, 220, 48, 98, 167, 218, 26, 27, 5, 30, 167, 221, 144, 46, 47, 56, 9, + 55, 206, 56, }; diff --git a/main/modes/factoryTest/factoryTest.c b/main/modes/factoryTest/factoryTest.c index eb6e842c5..be6dfa65b 100644 --- a/main/modes/factoryTest/factoryTest.c +++ b/main/modes/factoryTest/factoryTest.c @@ -25,9 +25,9 @@ #define AB_SEP 6 #define DPAD_SEP 15 -#define ACCEL_BAR_WIDTH 8 -#define ACCEL_BAR_CENT 150 -#define MAX_ACCEL_BAR_SIZE 40 +#define ACCEL_BAR_WIDTH 8 +#define ACCEL_BAR_CENT 150 +#define MAX_ACCEL_BAR_SIZE 40 #define TOUCHBAR_WIDTH 60 #define TOUCHBAR_HEIGHT 60 @@ -175,7 +175,7 @@ void testEnterMode(void) loadSong("stereo_test.sng", &test->song, false); bzrPlayBgm(&test->song, BZR_STEREO); - // Clear out accel setting. + // Clear out accel setting. accelPerformCal(); // Set NVM to indicate test not passed yet @@ -297,59 +297,78 @@ void testMainLoop(int64_t elapsedUs __attribute__((unused))) } // Plot X accel - int16_t barDelta = ((test->x) * MAX_ACCEL_BAR_SIZE) / 256; + int16_t barDelta = ((test->x) * MAX_ACCEL_BAR_SIZE) / 256; int16_t barDeltaInv = -barDelta; - if( barDelta < 0 ) barDelta = 0; - if( barDeltaInv < 0 ) barDeltaInv = 0; - accelColor = ((test->accelFlags & 0b1001) == 0b1001)?c050:c500; - fillDisplayArea(ACCEL_BAR_WIDTH, ACCEL_BAR_CENT - barDeltaInv, ACCEL_BAR_WIDTH*2, ACCEL_BAR_CENT + barDelta, accelColor); + if (barDelta < 0) + barDelta = 0; + if (barDeltaInv < 0) + barDeltaInv = 0; + accelColor = ((test->accelFlags & 0b1001) == 0b1001) ? c050 : c500; + fillDisplayArea(ACCEL_BAR_WIDTH, ACCEL_BAR_CENT - barDeltaInv, ACCEL_BAR_WIDTH * 2, ACCEL_BAR_CENT + barDelta, + accelColor); // Plot Y accel - barDelta = ((test->y) * MAX_ACCEL_BAR_SIZE) / 256; + barDelta = ((test->y) * MAX_ACCEL_BAR_SIZE) / 256; barDeltaInv = -barDelta; - if( barDelta < 0 ) barDelta = 0; - if( barDeltaInv < 0 ) barDeltaInv = 0; - accelColor = ((test->accelFlags & 0b10010) == 0b10010)?c050:c500; - fillDisplayArea(ACCEL_BAR_WIDTH*3, ACCEL_BAR_CENT - barDeltaInv, ACCEL_BAR_WIDTH*4, ACCEL_BAR_CENT + barDelta, accelColor); + if (barDelta < 0) + barDelta = 0; + if (barDeltaInv < 0) + barDeltaInv = 0; + accelColor = ((test->accelFlags & 0b10010) == 0b10010) ? c050 : c500; + fillDisplayArea(ACCEL_BAR_WIDTH * 3, ACCEL_BAR_CENT - barDeltaInv, ACCEL_BAR_WIDTH * 4, ACCEL_BAR_CENT + barDelta, + accelColor); // Plot Z accel - barDelta = ((test->z) * MAX_ACCEL_BAR_SIZE) / 256; + barDelta = ((test->z) * MAX_ACCEL_BAR_SIZE) / 256; barDeltaInv = -barDelta; - if( barDelta < 0 ) barDelta = 0; - if( barDeltaInv < 0 ) barDeltaInv = 0; - accelColor = ((test->accelFlags & 0b100100) == 0b100100)?c050:c500; - fillDisplayArea(ACCEL_BAR_WIDTH*5, ACCEL_BAR_CENT - barDeltaInv, ACCEL_BAR_WIDTH*6, ACCEL_BAR_CENT + barDelta, accelColor); + if (barDelta < 0) + barDelta = 0; + if (barDeltaInv < 0) + barDeltaInv = 0; + accelColor = ((test->accelFlags & 0b100100) == 0b100100) ? c050 : c500; + fillDisplayArea(ACCEL_BAR_WIDTH * 5, ACCEL_BAR_CENT - barDeltaInv, ACCEL_BAR_WIDTH * 6, ACCEL_BAR_CENT + barDelta, + accelColor); // Plot orient X - barDelta = ((test->ox) * MAX_ACCEL_BAR_SIZE) / 256; + barDelta = ((test->ox) * MAX_ACCEL_BAR_SIZE) / 256; barDeltaInv = -barDelta; - if( barDelta < 0 ) barDelta = 0; - if( barDeltaInv < 0 ) barDeltaInv = 0; - accelColor = ((test->accelFlags & 0b1001000000) == 0b1001000000)?c050:c500; - fillDisplayArea(ACCEL_BAR_WIDTH*7, ACCEL_BAR_CENT - barDeltaInv, ACCEL_BAR_WIDTH*8, ACCEL_BAR_CENT + barDelta, accelColor); + if (barDelta < 0) + barDelta = 0; + if (barDeltaInv < 0) + barDeltaInv = 0; + accelColor = ((test->accelFlags & 0b1001000000) == 0b1001000000) ? c050 : c500; + fillDisplayArea(ACCEL_BAR_WIDTH * 7, ACCEL_BAR_CENT - barDeltaInv, ACCEL_BAR_WIDTH * 8, ACCEL_BAR_CENT + barDelta, + accelColor); // Plot orient Y accel - barDelta = ((test->oy) * MAX_ACCEL_BAR_SIZE) / 256; + barDelta = ((test->oy) * MAX_ACCEL_BAR_SIZE) / 256; barDeltaInv = -barDelta; - if( barDelta < 0 ) barDelta = 0; - if( barDeltaInv < 0 ) barDeltaInv = 0; - accelColor = ((test->accelFlags & 0b10010000000) == 0b10010000000)?c050:c500; - fillDisplayArea(ACCEL_BAR_WIDTH*9, ACCEL_BAR_CENT - barDeltaInv, ACCEL_BAR_WIDTH*10, ACCEL_BAR_CENT + barDelta, accelColor); + if (barDelta < 0) + barDelta = 0; + if (barDeltaInv < 0) + barDeltaInv = 0; + accelColor = ((test->accelFlags & 0b10010000000) == 0b10010000000) ? c050 : c500; + fillDisplayArea(ACCEL_BAR_WIDTH * 9, ACCEL_BAR_CENT - barDeltaInv, ACCEL_BAR_WIDTH * 10, ACCEL_BAR_CENT + barDelta, + accelColor); // Plot orientZ accel - barDelta = ((test->oz) * MAX_ACCEL_BAR_SIZE) / 256; + barDelta = ((test->oz) * MAX_ACCEL_BAR_SIZE) / 256; barDeltaInv = -barDelta; - if( barDelta < 0 ) barDelta = 0; - if( barDeltaInv < 0 ) barDeltaInv = 0; - accelColor = ((test->accelFlags & 0b100100000000) == 0b100100000000)?c050:c500; - fillDisplayArea(ACCEL_BAR_WIDTH*11, ACCEL_BAR_CENT - barDeltaInv, ACCEL_BAR_WIDTH*12, ACCEL_BAR_CENT + barDelta, accelColor); - + if (barDelta < 0) + barDelta = 0; + if (barDeltaInv < 0) + barDeltaInv = 0; + accelColor = ((test->accelFlags & 0b100100000000) == 0b100100000000) ? c050 : c500; + fillDisplayArea(ACCEL_BAR_WIDTH * 11, ACCEL_BAR_CENT - barDeltaInv, ACCEL_BAR_WIDTH * 12, ACCEL_BAR_CENT + barDelta, + accelColor); // Plot stability accel barDelta = MAX_ACCEL_BAR_SIZE - (test->stability); - if( barDelta < -MAX_ACCEL_BAR_SIZE ) barDelta = -MAX_ACCEL_BAR_SIZE; - accelColor = ((test->accelFlags & 0b1000000000000) == 0b1000000000000)?c050:c500; - fillDisplayArea(ACCEL_BAR_WIDTH*13, ACCEL_BAR_CENT + barDelta, ACCEL_BAR_WIDTH*14, ACCEL_BAR_CENT + MAX_ACCEL_BAR_SIZE, accelColor); + if (barDelta < -MAX_ACCEL_BAR_SIZE) + barDelta = -MAX_ACCEL_BAR_SIZE; + accelColor = ((test->accelFlags & 0b1000000000000) == 0b1000000000000) ? c050 : c500; + fillDisplayArea(ACCEL_BAR_WIDTH * 13, ACCEL_BAR_CENT + barDelta, ACCEL_BAR_WIDTH * 14, + ACCEL_BAR_CENT + MAX_ACCEL_BAR_SIZE, accelColor); // Draw the 8-direction touchpad with center circle int16_t tBarX = TFT_WIDTH - TOUCHBAR_WIDTH; @@ -757,39 +776,52 @@ void testReadAndValidateAccelerometer(void) int16_t x, y, z; int16_t ox, oy, oz; - if( accelIntegrate() < 0 ) + if (accelIntegrate() < 0) return; accelGetAccelVecRaw(&x, &y, &z); accelGetOrientVec(&ox, &oy, &oz); // Save accel values - test->x = x; - test->y = y; - test->z = z; - test->ox = ox; - test->oy = oy; - test->oz = oz; - float fStab = accelGetStdDevInCal(); + test->x = x; + test->y = y; + test->z = z; + test->ox = ox; + test->oy = oy; + test->oz = oz; + float fStab = accelGetStdDevInCal(); test->stability = fStab * 200.0f; - if (x > 150) test->accelFlags |= 1; - if (y > 150) test->accelFlags |= 2; - if (z > 150) test->accelFlags |= 4; - if (x <-150) test->accelFlags |= 8; - if (y <-150) test->accelFlags |= 16; - if (z <-150) test->accelFlags |= 32; - - if (ox > 150) test->accelFlags |= 64; - if (oy > 150) test->accelFlags |= 128; - if (oz > 150) test->accelFlags |= 256; - if (ox <-150) test->accelFlags |= 512; - if (oy <-150) test->accelFlags |= 1024; - if (oz <-150) test->accelFlags |= 2048; - if (fStab < 0.00001f ) test->accelFlags |= 4096; + if (x > 150) + test->accelFlags |= 1; + if (y > 150) + test->accelFlags |= 2; + if (z > 150) + test->accelFlags |= 4; + if (x < -150) + test->accelFlags |= 8; + if (y < -150) + test->accelFlags |= 16; + if (z < -150) + test->accelFlags |= 32; + + if (ox > 150) + test->accelFlags |= 64; + if (oy > 150) + test->accelFlags |= 128; + if (oz > 150) + test->accelFlags |= 256; + if (ox < -150) + test->accelFlags |= 512; + if (oy < -150) + test->accelFlags |= 1024; + if (oz < -150) + test->accelFlags |= 2048; + if (fStab < 0.00001f) + test->accelFlags |= 4096; // Make sure all values are nonzero - if ( test->accelFlags == 0b1111111111111 ) + if (test->accelFlags == 0b1111111111111) { test->accelPassed = true; } diff --git a/main/modes/gamepad/gamepad.c b/main/modes/gamepad/gamepad.c index 7a5169478..ce2512dbc 100644 --- a/main/modes/gamepad/gamepad.c +++ b/main/modes/gamepad/gamepad.c @@ -425,7 +425,7 @@ void gamepadMenuLoop(int64_t elapsedUs) // No wifi mode stuff } - accelIntegrate(); + accelIntegrate(); } /** diff --git a/main/modes/pong/pong.c b/main/modes/pong/pong.c index aac6f21de..b08625a93 100644 --- a/main/modes/pong/pong.c +++ b/main/modes/pong/pong.c @@ -721,7 +721,7 @@ static void pongIncreaseBallVelocity(int16_t magnitude) */ static void pongBackgroundDrawCallback(int16_t x, int16_t y, int16_t w, int16_t h, int16_t up, int16_t upNum) { - accelIntegrate(); + accelIntegrate(); // Use TURBO drawing mode to draw individual pixels fast SETUP_FOR_TURBO(); diff --git a/main/swadge2024.c b/main/swadge2024.c index 695970b90..576ddbcc8 100644 --- a/main/swadge2024.c +++ b/main/swadge2024.c @@ -308,8 +308,8 @@ void app_main(void) initAccelerometer(I2C_NUM_0, GPIO_NUM_3, // SDA GPIO_NUM_41, // SCL - GPIO_PULLUP_ENABLE, 1000000 ); - accelIntegrate(); + GPIO_PULLUP_ENABLE, 1000000); + accelIntegrate(); } // Init the temperature sensor From ea52d6366911ef0f535392e822cc9ca8dc1049f2 Mon Sep 17 00:00:00 2001 From: gelakinetic Date: Sun, 15 Oct 2023 21:40:44 -0400 Subject: [PATCH 17/18] Rename hdw-accel to hdw-imu Rename hdw-accel-legacy back to hdw-accel --- components/hdw-accel-legacy/CMakeLists.txt | 3 - components/hdw-accel-legacy/hdw-accel.c | 289 ------ .../hdw-accel-legacy/include/hdw-accel.h | 82 -- components/hdw-accel/CMakeLists.txt | 7 +- components/hdw-accel/hdw-accel.c | 922 ++++-------------- components/hdw-accel/include/hdw-accel.h | 119 +-- components/hdw-imu/CMakeLists.txt | 4 + components/hdw-imu/hdw-imu.c | 877 +++++++++++++++++ components/hdw-imu/include/hdw-imu.h | 107 ++ .../hdw-accel.c => hdw-imu/hdw-imu.c} | 4 +- .../hdw-accel_emu.h => hdw-imu/hdw-imu_emu.h} | 0 emulator/src/emu_main.c | 2 +- emulator/src/extensions/fuzzer/ext_fuzzer.c | 2 +- emulator/src/extensions/replay/ext_replay.c | 4 +- main/CMakeLists.txt | 2 +- main/swadge2024.c | 2 +- main/swadge2024.h | 2 +- tools/sandbox_test/buildhelp.c | 2 +- tools/sandbox_test/test_i2c/sandbox.c | 2 +- 19 files changed, 1216 insertions(+), 1216 deletions(-) delete mode 100644 components/hdw-accel-legacy/CMakeLists.txt delete mode 100644 components/hdw-accel-legacy/hdw-accel.c delete mode 100644 components/hdw-accel-legacy/include/hdw-accel.h create mode 100644 components/hdw-imu/CMakeLists.txt create mode 100644 components/hdw-imu/hdw-imu.c create mode 100644 components/hdw-imu/include/hdw-imu.h rename emulator/src/components/{hdw-accel/hdw-accel.c => hdw-imu/hdw-imu.c} (99%) rename emulator/src/components/{hdw-accel/hdw-accel_emu.h => hdw-imu/hdw-imu_emu.h} (100%) diff --git a/components/hdw-accel-legacy/CMakeLists.txt b/components/hdw-accel-legacy/CMakeLists.txt deleted file mode 100644 index 421570635..000000000 --- a/components/hdw-accel-legacy/CMakeLists.txt +++ /dev/null @@ -1,3 +0,0 @@ -idf_component_register(SRCS "" - INCLUDE_DIRS "" - REQUIRES driver) diff --git a/components/hdw-accel-legacy/hdw-accel.c b/components/hdw-accel-legacy/hdw-accel.c deleted file mode 100644 index 7ff0a607d..000000000 --- a/components/hdw-accel-legacy/hdw-accel.c +++ /dev/null @@ -1,289 +0,0 @@ -//============================================================================== -// Includes -//============================================================================== - -#include - -#include "hdw-accel.h" - -//============================================================================== -// Enums -//============================================================================== - -typedef enum __attribute__((packed)) -{ - QMA7981_REG_CHIP_ID = 0x00, - QMA7981_REG_DX_L = 0x01, - QMA7981_REG_DX_H = 0x02, - QMA7981_REG_DY_L = 0x03, - QMA7981_REG_DY_H = 0x04, - QMA7981_REG_DZ_L = 0x05, - QMA7981_REG_DZ_H = 0x06, - QMA7981_REG_STEP_L = 0x07, - QMA7981_REG_STEP_H = 0x08, - QMA7981_REG_INT_STAT_0 = 0x0A, - QMA7981_REG_INT_STAT_1 = 0x0B, - QMA7981_REG_INT_STAT_4 = 0x0D, - QMA7981_REG_RANGE = 0x0F, - QMA7981_REG_BAND_WIDTH = 0x10, - QMA7981_REG_PWR_MANAGE = 0x11, - QMA7981_REG_STEP_CONF_0 = 0x12, - QMA7981_REG_STEP_CONF_1 = 0x13, - QMA7981_REG_STEP_CONF_2 = 0x14, - QMA7981_REG_STEP_CONF_3 = 0x15, - QMA7981_REG_INT_EN_0 = 0x16, - QMA7981_REG_INT_EN_1 = 0x17, - QMA7981_REG_INT_MAP_0 = 0x19, - QMA7981_REG_INT_MAP_1 = 0x1A, - QMA7981_REG_INT_MAP_2 = 0x1B, - QMA7981_REG_INT_MAP_3 = 0x1C, - QMA7981_REG_SIG_STEP_TH = 0x1D, - QMA7981_REG_STEP = 0x1F -} qmaReg_t; - -//============================================================================== -// Defines -//============================================================================== - -#define QMA7981_ADDR 0x12 - -//============================================================================== -// Variables -//============================================================================== - -static qma_range_t qma_range = QMA_RANGE_2G; -static i2c_port_t i2c_port; - -//============================================================================== -// Function Prototypes -//============================================================================== - -static esp_err_t qma7981_read_byte(qmaReg_t reg_addr, uint8_t* data); -static esp_err_t qma7981_write_byte(qmaReg_t reg_addr, uint8_t data); -static esp_err_t qma7981_read_bytes(qmaReg_t reg_addr, size_t data_len, uint8_t* data); -static int16_t signExtend10bit(uint16_t in); - -//============================================================================== -// Functions -//============================================================================== - -/** - * @brief Initialize the accelerometer - * - * @param _i2c_port The i2c port to use for the accelerometer - * @param sda The GPIO for the Serial DAta line - * @param scl The GPIO for the Serial CLock line - * @param pullup Either \c GPIO_PULLUP_DISABLE if there are external pullup resistors on SDA and SCL or \c - * GPIO_PULLUP_ENABLE if internal pull-ups should be used - * @param clkHz The frequency of the I2C clock - * @param range The range to measure, between ::QMA_RANGE_2G and ::QMA_RANGE_32G - * @param bandwidth The bandwidth to measure at, between ::QMA_BANDWIDTH_128_HZ and ::QMA_BANDWIDTH_1024_HZ - * @return ESP_OK if the accelerometer initialized, or a non-zero value if it did not - */ -esp_err_t initAccelerometer(i2c_port_t _i2c_port, gpio_num_t sda, gpio_num_t scl, gpio_pullup_t pullup, uint32_t clkHz, - qma_range_t range, qma_bandwidth_t bandwidth) -{ - i2c_port = _i2c_port; - esp_err_t ret_val = ESP_OK; - - /* Install i2c driver */ - i2c_config_t conf = { - .mode = I2C_MODE_MASTER, - .sda_io_num = sda, - .sda_pullup_en = pullup, - .scl_io_num = scl, - .scl_pullup_en = pullup, - .master.clk_speed = clkHz, - .clk_flags = I2C_SCLK_SRC_FLAG_FOR_NOMAL, - }; - ret_val |= i2c_param_config(i2c_port, &conf); - ret_val |= i2c_driver_install(i2c_port, conf.mode, 0, 0, 0); - - /* Exit sleep mode*/ - ret_val |= qma7981_write_byte(QMA7981_REG_PWR_MANAGE, 0xC0); - vTaskDelay(pdMS_TO_TICKS(20)); - - /* Set range */ - ret_val |= qma7981_write_byte(QMA7981_REG_RANGE, range); - /* Set bandwidth */ - ret_val |= qma7981_write_byte(QMA7981_REG_BAND_WIDTH, bandwidth); - - return ret_val; -} - -/** - * @brief Deinit the accelerometer (nothing to do) - * - * @return ESP_OK - */ -esp_err_t deInitAccelerometer(void) -{ - return ESP_OK; -} - -/** - * @brief Read a single byte from the accelerometer - * - * @param reg_addr The register to read from - * @param data The byte which was read is written here - * @return ESP_OK if the byte was read, or a non-zero value if it was not - */ -static esp_err_t qma7981_read_byte(qmaReg_t reg_addr, uint8_t* data) -{ - return qma7981_read_bytes(reg_addr, 1, data); -} - -/** - * @brief Read multiple bytes from the accelerometer - * - * @param reg_addr The register to read from - * @param data_len The number of bytes to read - * @param data The bytes which were read are written here - * @return ESP_OK if the bytes were read, or a non-zero value if they were not - */ -static esp_err_t qma7981_read_bytes(qmaReg_t reg_addr, size_t data_len, uint8_t* data) -{ - // Write the register to read from - i2c_cmd_handle_t cmdHandle = i2c_cmd_link_create(); - i2c_master_start(cmdHandle); - i2c_master_write_byte(cmdHandle, QMA7981_ADDR << 1 | I2C_MASTER_WRITE, false); - i2c_master_write_byte(cmdHandle, reg_addr, false); - i2c_master_stop(cmdHandle); - esp_err_t err = i2c_master_cmd_begin(i2c_port, cmdHandle, 100); - i2c_cmd_link_delete(cmdHandle); - - if (ESP_OK != err) - { - return err; - } - - // Read from the register - cmdHandle = i2c_cmd_link_create(); - i2c_master_start(cmdHandle); - i2c_master_write_byte(cmdHandle, QMA7981_ADDR << 1 | I2C_MASTER_READ, false); - i2c_master_read(cmdHandle, data, data_len, I2C_MASTER_LAST_NACK); - i2c_master_stop(cmdHandle); - err = i2c_master_cmd_begin(i2c_port, cmdHandle, 100); - i2c_cmd_link_delete(cmdHandle); - - return err; -} - -/** - * @brief Write a single byte to the accelerometer - * - * @param reg_addr The register address to write to - * @param data The byte to write - * @return ESP_OK if the byte was written, or a non-zero value if it was not - */ -static esp_err_t qma7981_write_byte(qmaReg_t reg_addr, uint8_t data) -{ - i2c_cmd_handle_t cmdHandle = i2c_cmd_link_create(); - i2c_master_start(cmdHandle); - - i2c_master_write_byte(cmdHandle, QMA7981_ADDR << 1, false); - i2c_master_write_byte(cmdHandle, reg_addr, false); - i2c_master_write_byte(cmdHandle, data, false); - - i2c_master_stop(cmdHandle); - esp_err_t err = i2c_master_cmd_begin(i2c_port, cmdHandle, 100); - i2c_cmd_link_delete(cmdHandle); - return err; -} - -/** - * @brief Read and return the 16-bit step counter - * - * Note that this can be configured with ::QMA7981_REG_STEP_CONF_0 through ::QMA7981_REG_STEP_CONF_3 - * - * @param data The step counter value is written here - * @return ESP_OK if the step count was read, or a non-zero value if it was not - */ -esp_err_t accelGetStep(uint16_t* data) -{ - esp_err_t ret_val = ESP_OK; - uint8_t step_h = 0, step_l = 0; - - if (NULL == data) - { - return ESP_ERR_INVALID_ARG; - } - - ret_val |= qma7981_read_byte(QMA7981_REG_STEP_L, &step_l); - ret_val |= qma7981_read_byte(QMA7981_REG_STEP_H, &step_h); - - *data = (step_h << 8) + step_l; - - return ret_val; -} - -/** - * @brief Set the accelerometer's measurement range - * - * @param range The range to measure, from ::QMA_RANGE_2G to ::QMA_RANGE_32G - * @return ESP_OK if the range was set, or a non-zero value if it was not - */ -esp_err_t accelSetRange(qma_range_t range) -{ - esp_err_t ret_val = qma7981_write_byte(QMA7981_REG_RANGE, range); - qma_range = range; - - return ret_val; -} - -/** - * @brief Read the current acceleration vector from the accelerometer and return - * the vector through arguments. If the read fails, the last known values are - * returned instead. - * - * @param x The X component of the acceleration vector is written here - * @param y The Y component of the acceleration vector is written here - * @param z The Z component of the acceleration vector is written here - * @return ESP_OK if the acceleration was read, or a non-zero value if it was not - */ -esp_err_t accelGetAccelVec(int16_t* x, int16_t* y, int16_t* z) -{ - static int16_t lastX = 0; - static int16_t lastY = 0; - static int16_t lastZ = 0; - - // Read 6 bytes of data(0x00) - uint8_t raw_data[6]; - // Do the read - esp_err_t ret_val = qma7981_read_bytes(QMA7981_REG_DX_L, 6, raw_data); - - // If the read was successful - if (ESP_OK == ret_val) - { - // Sign extend the 10 bit value to 16 bits and save it as the last known value - // TODO The datasheet mentions this is a 14 bit reading, not a 10 bit one? - lastX = signExtend10bit(((raw_data[0] >> 6) | (raw_data[1]) << 2) & 0x03FF); - lastY = -signExtend10bit(((raw_data[2] >> 6) | (raw_data[3]) << 2) & 0x03FF); - lastZ = -signExtend10bit(((raw_data[4] >> 6) | (raw_data[5]) << 2) & 0x03FF); - } - - // Copy out the acceleration value - *x = lastX; - *y = lastY; - *z = lastZ; - - return ret_val; -} - -/** - * @brief Helper function to sign-extend a 10 bit two's complement number to 16 bit - * - * @param in The two's compliment number to sign-extend - * @return The sign-extended two's compliment number - */ -static int16_t signExtend10bit(uint16_t in) -{ - if (in & 0x200) - { - return (in | 0xFC00); // extend the sign bit all the way out - } - else - { - return (in & 0x01FF); // make sure the sign bits are cleared - } -} diff --git a/components/hdw-accel-legacy/include/hdw-accel.h b/components/hdw-accel-legacy/include/hdw-accel.h deleted file mode 100644 index 2aa8f7312..000000000 --- a/components/hdw-accel-legacy/include/hdw-accel.h +++ /dev/null @@ -1,82 +0,0 @@ -/*! \file hdw-accel.h - * - * \section accel_design Design Philosophy - * - * The accelerometer used is a QMA7981. - * The datasheet can be found here: QMA7981 - * Datasheet. - * - * The accelerometer component does not automatically poll the accelerometer. - * All it does is set up and configure the accelerometer, then it is up to the Swadge mode to query for acceleration as - * appropriate. - * - * This component requires the I2C bus to be initialized, so it does that as well. - * If other I2C peripherals are added in the future, common I2C bus initialization should be moved to a more common - * location. - * - * \section accel_usage Usage - * - * You don't need to call initAccelerometer() or deInitAccelerometer(). The system does this the appropriate time. - * - * You do need to call accelGetAccelVec() to get the current acceleration vector. - * If you want to poll this from your Swadge mode's main function, you may. - * - * You may call accelSetRange() if you want to adjust the measurement range. - * - * accelGetStep() exists, but it has not been tested, so use it with caution. - * You may need to configure parameters related to step counting. - * - * \section accel_example Example - * - * \code{.c} - * // Declare variables to receive acceleration - * int16_t a_x, a_y, a_z; - * - * // Get the current acceleration - * if(ESP_OK == accelGetAccelVec(&a_x, &a_y, &a_z)) - * { - * // Print data to debug logs - * printf("x: %"PRId16", y: %"PRId16", z:%"PRId16, a_x, a_y, a_z); - * } - * \endcode - */ - -#ifndef _HDW_ACCEL_H_ -#define _HDW_ACCEL_H_ - -#include - -#include -#include -#include - -/** - * @brief The ranges for acceleration measurement from 2G to 32G (Earth gravity) - */ -typedef enum -{ - QMA_RANGE_2G = 0b0001, ///< Two G's of measurement range - QMA_RANGE_4G = 0b0010, ///< Four G's of measurement range - QMA_RANGE_8G = 0b0100, ///< Eight G's of measurement range - QMA_RANGE_16G = 0b1000, ///< Sixteen G's of measurement range - QMA_RANGE_32G = 0b1111, ///< Thirty-two G's of measurement range -} qma_range_t; - -/** - * @brief The bandwidth for acceleration measurement - */ -typedef enum -{ - QMA_BANDWIDTH_128_HZ = 0b111, ///< 128Hz bandwidth - QMA_BANDWIDTH_256_HZ = 0b110, ///< 256Hz bandwidth - QMA_BANDWIDTH_1024_HZ = 0b101, ///< 1024Hz bandwidth -} qma_bandwidth_t; - -esp_err_t initAccelerometer(i2c_port_t _i2c_port, gpio_num_t sda, gpio_num_t scl, gpio_pullup_t pullup, uint32_t clkHz, - qma_range_t range, qma_bandwidth_t bandwidth); -esp_err_t deInitAccelerometer(void); -esp_err_t accelSetRange(qma_range_t range); -esp_err_t accelGetAccelVec(int16_t* x, int16_t* y, int16_t* z); -esp_err_t accelGetStep(uint16_t* data); - -#endif diff --git a/components/hdw-accel/CMakeLists.txt b/components/hdw-accel/CMakeLists.txt index 642727a1d..421570635 100644 --- a/components/hdw-accel/CMakeLists.txt +++ b/components/hdw-accel/CMakeLists.txt @@ -1,4 +1,3 @@ -idf_component_register(SRCS "hdw-accel.c" - INCLUDE_DIRS "include" - REQUIRES driver - PRIV_REQUIRES hdw-nvs) +idf_component_register(SRCS "" + INCLUDE_DIRS "" + REQUIRES driver) diff --git a/components/hdw-accel/hdw-accel.c b/components/hdw-accel/hdw-accel.c index 6a26c05dc..7ff0a607d 100644 --- a/components/hdw-accel/hdw-accel.c +++ b/components/hdw-accel/hdw-accel.c @@ -3,16 +3,8 @@ //============================================================================== #include -#include -#include + #include "hdw-accel.h" -#include "soc/rtc_cntl_reg.h" -#include "soc/gpio_reg.h" -#include "soc/io_mux_reg.h" -#include "rom/gpio.h" -#include "soc/i2c_reg.h" -#include "soc/gpio_struct.h" -#include "hdw-nvs.h" //============================================================================== // Enums @@ -20,858 +12,278 @@ typedef enum __attribute__((packed)) { - LSM6DSL_FUNC_CFG_ACCESS = 0x01, - LSM6DSL_SENSOR_SYNC_TIME_FRAME = 0x04, - LSM6DSL_FIFO_CTRL1 = 0x06, - LSM6DSL_FIFO_CTRL2 = 0x07, - LSM6DSL_FIFO_CTRL3 = 0x08, - LSM6DSL_FIFO_CTRL4 = 0x09, - LSM6DSL_FIFO_CTRL5 = 0x0a, - LSM6DSL_ORIENT_CFG_G = 0x0b, - LSM6DSL_INT1_CTRL = 0x0d, - LSM6DSL_INT2_CTRL = 0x0e, - LMS6DS3_WHO_AM_I = 0x0f, - LSM6DSL_CTRL1_XL = 0x10, - LSM6DSL_CTRL2_G = 0x11, - LSM6DSL_CTRL3_C = 0x12, - LSM6DSL_CTRL4_C = 0x13, - LSM6DSL_CTRL5_C = 0x14, - LSM6DSL_CTRL6_C = 0x15, - LSM6DSL_CTRL7_G = 0x16, - LSM6DSL_CTRL8_XL = 0x17, - LSM6DSL_CTRL9_XL = 0x18, - LSM6DSL_CTRL10_C = 0x19, - LSM6DSL_MASTER_CONFIG = 0x1a, - LSM6DSL_WAKE_UP_SRC = 0x1b, - LSM6DSL_TAP_SRC = 0x1c, - LSM6DSL_D6D_SRC = 0x1d, - LSM6DSL_STATUS_REG = 0x1e, - LSM6DSL_OUT_TEMP_L = 0x20, - LSM6DSL_OUT_TEMP_H = 0x21, - LMS6DS3_OUTX_L_G = 0x22, - LMS6DS3_OUTX_H_G = 0x23, - LMS6DS3_OUTY_L_G = 0x24, - LMS6DS3_OUTY_H_G = 0x25, - LMS6DS3_OUTZ_L_G = 0x26, - LMS6DS3_OUTZ_H_G = 0x27, - LMS6DS3_OUTX_L_XL = 0x28, - LMS6DS3_OUTX_H_XL = 0x29, - LMS6DS3_OUTY_L_XL = 0x2a, - LMS6DS3_OUTY_H_XL = 0x2b, - LMS6DS3_OUTZ_L_XL = 0x2c, - LMS6DS3_OUTZ_H_XL = 0x2d, -} lsm6dslReg_t; + QMA7981_REG_CHIP_ID = 0x00, + QMA7981_REG_DX_L = 0x01, + QMA7981_REG_DX_H = 0x02, + QMA7981_REG_DY_L = 0x03, + QMA7981_REG_DY_H = 0x04, + QMA7981_REG_DZ_L = 0x05, + QMA7981_REG_DZ_H = 0x06, + QMA7981_REG_STEP_L = 0x07, + QMA7981_REG_STEP_H = 0x08, + QMA7981_REG_INT_STAT_0 = 0x0A, + QMA7981_REG_INT_STAT_1 = 0x0B, + QMA7981_REG_INT_STAT_4 = 0x0D, + QMA7981_REG_RANGE = 0x0F, + QMA7981_REG_BAND_WIDTH = 0x10, + QMA7981_REG_PWR_MANAGE = 0x11, + QMA7981_REG_STEP_CONF_0 = 0x12, + QMA7981_REG_STEP_CONF_1 = 0x13, + QMA7981_REG_STEP_CONF_2 = 0x14, + QMA7981_REG_STEP_CONF_3 = 0x15, + QMA7981_REG_INT_EN_0 = 0x16, + QMA7981_REG_INT_EN_1 = 0x17, + QMA7981_REG_INT_MAP_0 = 0x19, + QMA7981_REG_INT_MAP_1 = 0x1A, + QMA7981_REG_INT_MAP_2 = 0x1B, + QMA7981_REG_INT_MAP_3 = 0x1C, + QMA7981_REG_SIG_STEP_TH = 0x1D, + QMA7981_REG_STEP = 0x1F +} qmaReg_t; //============================================================================== // Defines //============================================================================== -#define LSM6DSL_ADDRESS 0x6a -#define QMC6308_ADDRESS 0x2c +#define QMA7981_ADDR 0x12 //============================================================================== // Variables //============================================================================== +static qma_range_t qma_range = QMA_RANGE_2G; static i2c_port_t i2c_port; -LSM6DSLData LSM6DSL; - -//============================================================================== -// Utility Prototypes -//============================================================================== - -float rsqrtf(float x); -float mathsqrtf(float x); -void mathEulerToQuat(float* q, const float* euler); -void mathQuatApply(float* qout, const float* q1, const float* q2); -void mathQuatNormalize(float* qout, const float* qin); -void mathCrossProduct(float* p, const float* a, const float* b); -void mathRotateVectorByInverseOfQuaternion(float* pout, const float* q, const float* p); -void mathRotateVectorByQuaternion(float* pout, const float* q, const float* p); - -static inline uint32_t getCycleCount(); - -esp_err_t GeneralSet(int dev, int reg, int val); -esp_err_t LSM6DSLSet(int reg, int val); -int GeneralI2CGet(int device, int reg, uint8_t* data, int data_len); -int ReadLSM6DSL(uint8_t* data, int data_len); //============================================================================== // Function Prototypes //============================================================================== -esp_err_t initAccelerometer(i2c_port_t _i2c_port, gpio_num_t sda, gpio_num_t scl, gpio_pullup_t pullup, uint32_t clkHz); -esp_err_t deInitAccelerometer(void); -esp_err_t accelGetAccelVecRaw(int16_t* x, int16_t* y, int16_t* z); -esp_err_t accelGetOrientVec(int16_t* x, int16_t* y, int16_t* z); -esp_err_t accelGetQuaternion(float* q); -esp_err_t accelIntegrate(void); -float accelGetStdDevInCal(void); -void accelSetRegistersAndReset(void); +static esp_err_t qma7981_read_byte(qmaReg_t reg_addr, uint8_t* data); +static esp_err_t qma7981_write_byte(qmaReg_t reg_addr, uint8_t data); +static esp_err_t qma7981_read_bytes(qmaReg_t reg_addr, size_t data_len, uint8_t* data); +static int16_t signExtend10bit(uint16_t in); //============================================================================== -// Utility Functions +// Functions //============================================================================== /** - * @brief Perform a fast, approximate reciprocal square root + * @brief Initialize the accelerometer * - * @param x The number to take a recriprocal square root of. - * @return approximately 1/sqrt(x) - */ -float rsqrtf(float x) -{ - typedef union - { - int32_t i; - float f; - } fiunion; - const float xhalf = 0.5f * x; - fiunion i = {.f = x}; - i.i = 0x5f375a86 - (i.i >> 1); - x = i.f; - x = x * (1.5f - xhalf * x * x); - x = x * (1.5f - xhalf * x * x); - return x; -} - -/** - * @brief Perform a fast, approximate square root - * - * @param x The number to take a square root of. - * @return approximately sqrt(x) (but is much faster) + * @param _i2c_port The i2c port to use for the accelerometer + * @param sda The GPIO for the Serial DAta line + * @param scl The GPIO for the Serial CLock line + * @param pullup Either \c GPIO_PULLUP_DISABLE if there are external pullup resistors on SDA and SCL or \c + * GPIO_PULLUP_ENABLE if internal pull-ups should be used + * @param clkHz The frequency of the I2C clock + * @param range The range to measure, between ::QMA_RANGE_2G and ::QMA_RANGE_32G + * @param bandwidth The bandwidth to measure at, between ::QMA_BANDWIDTH_128_HZ and ::QMA_BANDWIDTH_1024_HZ + * @return ESP_OK if the accelerometer initialized, or a non-zero value if it did not */ -float mathsqrtf(float x) +esp_err_t initAccelerometer(i2c_port_t _i2c_port, gpio_num_t sda, gpio_num_t scl, gpio_pullup_t pullup, uint32_t clkHz, + qma_range_t range, qma_bandwidth_t bandwidth) { - // Trick to do approximate, fast square roots. (Though it is surprisingly fast) - int sign = x < 0; - if (sign) - x = -x; - if (x < 0.0000001) - return 0.0001; - float o = x; - o = (o + x / o) / 2; - o = (o + x / o) / 2; - o = (o + x / o) / 2; - o = (o + x / o) / 2; - if (sign) - return -o; - else - return o; -} + i2c_port = _i2c_port; + esp_err_t ret_val = ESP_OK; -/** - * @brief convert euler angles (in radians) to a quaternion. - * - * @param q Pointer to the wxyz quat (float[4]) to be written. - * @param euler Pointer to a float[3] of euler angles. - */ -void mathEulerToQuat(float* q, const float* euler) -{ - float pitch = euler[0]; - float yaw = euler[1]; - float roll = euler[2]; - float cr = cosf(pitch * 0.5); - float sr = sinf(pitch * 0.5); // Pitch: About X - float cp = cosf(yaw * 0.5); - float sp = sinf(yaw * 0.5); // Yaw: About Y - float cy = cosf(roll * 0.5); - float sy = sinf(roll * 0.5); // Roll: About Z - q[0] = cr * cp * cy + sr * sp * sy; - q[1] = sr * cp * cy - cr * sp * sy; - q[2] = cr * sp * cy + sr * cp * sy; - q[3] = cr * cp * sy - sr * sp * cy; -} + /* Install i2c driver */ + i2c_config_t conf = { + .mode = I2C_MODE_MASTER, + .sda_io_num = sda, + .sda_pullup_en = pullup, + .scl_io_num = scl, + .scl_pullup_en = pullup, + .master.clk_speed = clkHz, + .clk_flags = I2C_SCLK_SRC_FLAG_FOR_NOMAL, + }; + ret_val |= i2c_param_config(i2c_port, &conf); + ret_val |= i2c_driver_install(i2c_port, conf.mode, 0, 0, 0); -/** - * @brief Rotate one quaternion by another (and do not normalize) - * - * @param qout Pointer to the wxyz quat (float[4]) to be written. - * @param q1 First quaternion to be rotated. - * @param q2 Quaternion to rotate q1 by. - */ -void mathQuatApply(float* qout, const float* q1, const float* q2) -{ - // NOTE: Does not normalize - you will need to normalize eventually. - float tmpw, tmpx, tmpy; - tmpw = (q1[0] * q2[0]) - (q1[1] * q2[1]) - (q1[2] * q2[2]) - (q1[3] * q2[3]); - tmpx = (q1[0] * q2[1]) + (q1[1] * q2[0]) + (q1[2] * q2[3]) - (q1[3] * q2[2]); - tmpy = (q1[0] * q2[2]) - (q1[1] * q2[3]) + (q1[2] * q2[0]) + (q1[3] * q2[1]); - qout[3] = (q1[0] * q2[3]) + (q1[1] * q2[2]) - (q1[2] * q2[1]) + (q1[3] * q2[0]); - qout[2] = tmpy; - qout[1] = tmpx; - qout[0] = tmpw; -} + /* Exit sleep mode*/ + ret_val |= qma7981_write_byte(QMA7981_REG_PWR_MANAGE, 0xC0); + vTaskDelay(pdMS_TO_TICKS(20)); -/** - * @brief Normalize a quaternion - * - * @param qout Pointer to the wxyz quat (float[4]) to be written. - * @param qin Pointer to the quaterion to normalize. - */ -void mathQuatNormalize(float* qout, const float* qin) -{ - float qmag = qin[0] * qin[0] + qin[1] * qin[1] + qin[2] * qin[2] + qin[3] * qin[3]; - qmag = rsqrtf(qmag); - qout[0] = qin[0] * qmag; - qout[1] = qin[1] * qmag; - qout[2] = qin[2] * qmag; - qout[3] = qin[3] * qmag; -} + /* Set range */ + ret_val |= qma7981_write_byte(QMA7981_REG_RANGE, range); + /* Set bandwidth */ + ret_val |= qma7981_write_byte(QMA7981_REG_BAND_WIDTH, bandwidth); -/** - * @brief Perform a 3D cross product - * - * @param p Pointer to the float[3] output of the cross product (p = a x b) - * @param a Pointer to the float[3] of the cross product a vector. - * @param a Pointer to the float[3] of the cross product b vector. - */ -void mathCrossProduct(float* p, const float* a, const float* b) -{ - float tx = a[1] * b[2] - a[2] * b[1]; - float ty = a[2] * b[0] - a[0] * b[2]; - p[2] = a[0] * b[1] - a[1] * b[0]; - p[1] = ty; - p[0] = tx; + return ret_val; } /** - * @brief Rotate a 3D vector by a quaternion + * @brief Deinit the accelerometer (nothing to do) * - * @param pout Pointer to the float[3] output of the rotation - * @param q Pointer to the wzyz quaternion (float[4]) of the rotation. - * @param p Pointer to the float[3] of the vector to rotates. + * @return ESP_OK */ -void mathRotateVectorByQuaternion(float* pout, const float* q, const float* p) +esp_err_t deInitAccelerometer(void) { - // return v + 2.0 * cross(q.xyz, cross(q.xyz, v) + q.w * v); - float iqo[3]; - mathCrossProduct(iqo, q + 1 /*.xyz*/, p); - iqo[0] += q[0] * p[0]; - iqo[1] += q[0] * p[1]; - iqo[2] += q[0] * p[2]; - float ret[3]; - mathCrossProduct(ret, q + 1 /*.xyz*/, iqo); - pout[0] = ret[0] * 2.0 + p[0]; - pout[1] = ret[1] * 2.0 + p[1]; - pout[2] = ret[2] * 2.0 + p[2]; + return ESP_OK; } /** - * @brief Rotate a 3D vector by the inverse of a quaternion + * @brief Read a single byte from the accelerometer * - * @param pout Pointer to the float[3] output of the antirotation. - * @param q Pointer to the wzyz quaternion (float[4]) opposite of the rotation. - * @param p Pointer to the float[3] of the vector to antirotates. + * @param reg_addr The register to read from + * @param data The byte which was read is written here + * @return ESP_OK if the byte was read, or a non-zero value if it was not */ -void mathRotateVectorByInverseOfQuaternion(float* pout, const float* q, const float* p) +static esp_err_t qma7981_read_byte(qmaReg_t reg_addr, uint8_t* data) { - // General note: Performing a transform this way can be about 20-30% slower than a well formed 3x3 matrix. - // return v + 2.0 * cross(q.xyz, cross(q.xyz, v) + q.w * v); - float iqo[3]; - mathCrossProduct(iqo, p, q + 1 /*.xyz*/); - iqo[0] += q[0] * p[0]; - iqo[1] += q[0] * p[1]; - iqo[2] += q[0] * p[2]; - float ret[3]; - mathCrossProduct(ret, iqo, q + 1 /*.xyz*/); - pout[0] = ret[0] * 2.0 + p[0]; - pout[1] = ret[1] * 2.0 + p[1]; - pout[2] = ret[2] * 2.0 + p[2]; + return qma7981_read_bytes(reg_addr, 1, data); } -static inline uint32_t getCycleCount() -{ - uint32_t ccount; - asm volatile("rsr %0,ccount" : "=a"(ccount)); - return ccount; -} - -//============================================================================== -// Internal Functions -//============================================================================== - /** - * @brief Set a specific register on the IMU to a value. + * @brief Read multiple bytes from the accelerometer * - * @param dev The 7-bit address of the device to set the register to. - * @param reg The 8-bit register # - * @param val The 8-bit value to set the register to. - * @return ESP_OK if the operation was successful. + * @param reg_addr The register to read from + * @param data_len The number of bytes to read + * @param data The bytes which were read are written here + * @return ESP_OK if the bytes were read, or a non-zero value if they were not */ -esp_err_t GeneralSet(int dev, int reg, int val) +static esp_err_t qma7981_read_bytes(qmaReg_t reg_addr, size_t data_len, uint8_t* data) { + // Write the register to read from i2c_cmd_handle_t cmdHandle = i2c_cmd_link_create(); i2c_master_start(cmdHandle); - i2c_master_write_byte(cmdHandle, dev << 1, false); - i2c_master_write_byte(cmdHandle, reg, false); - i2c_master_write_byte(cmdHandle, val, true); + i2c_master_write_byte(cmdHandle, QMA7981_ADDR << 1 | I2C_MASTER_WRITE, false); + i2c_master_write_byte(cmdHandle, reg_addr, false); i2c_master_stop(cmdHandle); - esp_err_t err = i2c_master_cmd_begin(I2C_NUM_0, cmdHandle, 100); + esp_err_t err = i2c_master_cmd_begin(i2c_port, cmdHandle, 100); i2c_cmd_link_delete(cmdHandle); - return err; -} -/** - * @brief Set a specific register on the LSM6DSL to a value. - * - * @param reg The 8-bit register # - * @param val The 8-bit value to set the register to. - * @return ESP_OK if the operation was successful. - */ -esp_err_t LSM6DSLSet(int reg, int val) -{ - return GeneralSet(LSM6DSL_ADDRESS, reg, val); -} + if (ESP_OK != err) + { + return err; + } -/** - * @brief Read a buffer back from a specific I2C device. - * - * @param device The 7-bit device address - * @param reg The 8-bit register # to start at. - * @param data The buffer to load the data into. - * @param data_len Number of bytes to read. - * @return positive number if operation was successful, or esp_err_t if failure. - */ -int GeneralI2CGet(int device, int reg, uint8_t* data, int data_len) -{ - i2c_cmd_handle_t cmdHandle = i2c_cmd_link_create(); - i2c_master_start(cmdHandle); - i2c_master_write_byte(cmdHandle, device << 1, false); - i2c_master_write_byte(cmdHandle, reg, false); + // Read from the register + cmdHandle = i2c_cmd_link_create(); i2c_master_start(cmdHandle); - i2c_master_write_byte(cmdHandle, device << 1 | I2C_MASTER_READ, false); + i2c_master_write_byte(cmdHandle, QMA7981_ADDR << 1 | I2C_MASTER_READ, false); i2c_master_read(cmdHandle, data, data_len, I2C_MASTER_LAST_NACK); i2c_master_stop(cmdHandle); - esp_err_t err = i2c_master_cmd_begin(I2C_NUM_0, cmdHandle, 100); + err = i2c_master_cmd_begin(i2c_port, cmdHandle, 100); i2c_cmd_link_delete(cmdHandle); - if (err) - { - ESP_LOGE("accel", "Error on link: %d", err); - return -1; - } - else - return data_len; + + return err; } /** - * @brief Read the FIFO out of the LSM6DSL + * @brief Write a single byte to the accelerometer * - * @param data The buffer to write the FIFO data into. - * @param data_len The maximum size (in words) to read. - * @return positive number if operation was successful, or esp_err_t if failure. + * @param reg_addr The register address to write to + * @param data The byte to write + * @return ESP_OK if the byte was written, or a non-zero value if it was not */ -int ReadLSM6DSL(uint8_t* data, int data_len) +static esp_err_t qma7981_write_byte(qmaReg_t reg_addr, uint8_t data) { i2c_cmd_handle_t cmdHandle = i2c_cmd_link_create(); i2c_master_start(cmdHandle); - i2c_master_write_byte(cmdHandle, LSM6DSL_ADDRESS << 1, false); - i2c_master_write_byte(cmdHandle, 0x3A, false); - i2c_master_start(cmdHandle); - i2c_master_write_byte(cmdHandle, LSM6DSL_ADDRESS << 1 | I2C_MASTER_READ, false); - uint32_t fifolen = 0; - i2c_master_read(cmdHandle, (uint8_t*)&fifolen, 3, I2C_MASTER_LAST_NACK); - i2c_master_stop(cmdHandle); - esp_err_t err = i2c_master_cmd_begin(I2C_NUM_0, cmdHandle, 100); - i2c_cmd_link_delete(cmdHandle); - if (err < 0) - return -1; - // Is fifo overflow. - if (fifolen & 0x4000) - { - // reset fifo. - // If we overflow, and we don't do this, bad things happen. - LSM6DSLSet(LSM6DSL_FIFO_CTRL5, (0b0101 << 3) | 0b000); // Disable fifo - LSM6DSLSet(LSM6DSL_FIFO_CTRL5, (0b0101 << 3) | 0b110); // 208 Hz ODR - LSM6DSL.sampCount = 0; - return 0; - } - - fifolen &= 0x7ff; - if (fifolen == 0) - return 0; - if (fifolen > data_len / 2) - fifolen = data_len / 2; + i2c_master_write_byte(cmdHandle, QMA7981_ADDR << 1, false); + i2c_master_write_byte(cmdHandle, reg_addr, false); + i2c_master_write_byte(cmdHandle, data, false); - cmdHandle = i2c_cmd_link_create(); - i2c_master_start(cmdHandle); - i2c_master_write_byte(cmdHandle, LSM6DSL_ADDRESS << 1 | I2C_MASTER_READ, false); - i2c_master_read(cmdHandle, data, fifolen * 2, I2C_MASTER_LAST_NACK); i2c_master_stop(cmdHandle); - err = i2c_master_cmd_begin(I2C_NUM_0, cmdHandle, 100); - + esp_err_t err = i2c_master_cmd_begin(i2c_port, cmdHandle, 100); i2c_cmd_link_delete(cmdHandle); - if (err < 0) - return -2; - - return fifolen; + return err; } -//============================================================================== -// Functions -//============================================================================== - /** - * @brief Initialize the IMU + * @brief Read and return the 16-bit step counter * - * @param _i2c_port The i2c port to use for the IMU - * @param sda The GPIO for the Serial DAta line - * @param scl The GPIO for the Serial CLock line - * @param pullup Either \c GPIO_PULLUP_DISABLE if there are external pullup resistors on SDA and SCL or \c - * GPIO_PULLUP_ENABLE if internal pull-ups should be used - * @param clkHz The frequency of the I2C clock - * @return ESP_OK if the accelerometer initialized, or a non-zero value if it did not + * Note that this can be configured with ::QMA7981_REG_STEP_CONF_0 through ::QMA7981_REG_STEP_CONF_3 + * + * @param data The step counter value is written here + * @return ESP_OK if the step count was read, or a non-zero value if it was not */ -esp_err_t initAccelerometer(i2c_port_t _i2c_port, gpio_num_t sda, gpio_num_t scl, gpio_pullup_t pullup, uint32_t clkHz) +esp_err_t accelGetStep(uint16_t* data) { - int retry = 0; - i2c_port = _i2c_port; - esp_err_t ret_val; - -do_retry: - - // Shake any device off the bus. - int i; - int gpio_scl = 41; - for (i = 0; i < 16; i++) - { - gpio_matrix_out(gpio_scl, 256, 1, 0); - GPIO.out1_w1tc.val = (1 << (gpio_scl - 32)); - esp_rom_delay_us(10); - gpio_matrix_out(gpio_scl, 256, 1, 0); - GPIO.out1_w1ts.val = (1 << (gpio_scl - 32)); - esp_rom_delay_us(10); - } - gpio_matrix_out(gpio_scl, 29, 0, 0); - - ret_val = ESP_OK; - - i2c_driver_delete(_i2c_port); - - /* Install i2c driver */ - i2c_config_t conf = { - .mode = I2C_MODE_MASTER, - .sda_io_num = sda, - .sda_pullup_en = pullup, - .scl_io_num = scl, - .scl_pullup_en = pullup, - .master.clk_speed = clkHz, // tested upto 1.4Mbit/s - .clk_flags = I2C_SCLK_SRC_FLAG_FOR_NOMAL, - }; - ESP_LOGI("accel", "i2c_driver_install=%d", i2c_driver_install(_i2c_port, conf.mode, 0, 0, 0)); - ret_val |= i2c_param_config(i2c_port, &conf); - - // Enable access - LSM6DSLSet(LSM6DSL_FUNC_CFG_ACCESS, 0x20); - LSM6DSLSet(LSM6DSL_CTRL3_C, 0x81); // Force reset - vTaskDelay(1); - LSM6DSLSet(LSM6DSL_CTRL3_C, 0x44); // unforce reset + esp_err_t ret_val = ESP_OK; + uint8_t step_h = 0, step_l = 0; - uint8_t who = 0xaa; - int r = GeneralI2CGet(LSM6DSL_ADDRESS, LMS6DS3_WHO_AM_I, &who, 1); - if (r != 1 || who != 0x6a) + if (NULL == data) { - ESP_LOGW("accel", "WHOAMI Failed (%02x), %d", who, r); - if (retry++ < 10) - goto do_retry; - ESP_LOGE("accel", "Init failed on 1"); - return ESP_FAIL; + return ESP_ERR_INVALID_ARG; } - ESP_LOGI("accel", "Init Start"); - accelSetRegistersAndReset(); + ret_val |= qma7981_read_byte(QMA7981_REG_STEP_L, &step_l); + ret_val |= qma7981_read_byte(QMA7981_REG_STEP_H, &step_h); - for (i = 0; i < 2; i++) - { - vTaskDelay(1); - int check = accelIntegrate(); - if (check != ESP_OK) - { - ESP_LOGI("accel", "Init Fault Retry"); - if (retry++ < 10) - goto do_retry; - ESP_LOGI("accel", "Init failed on 2"); - return ESP_FAIL; - } - ESP_LOGI("accel", "Check %d", check); - } + *data = (step_h << 8) + step_l; - ESP_LOGI("accel", "Init Ok"); - return ESP_OK; + return ret_val; } /** - * @brief Deinit the accelerometer (nothing to do) + * @brief Set the accelerometer's measurement range * - * @return ESP_OK + * @param range The range to measure, from ::QMA_RANGE_2G to ::QMA_RANGE_32G + * @return ESP_OK if the range was set, or a non-zero value if it was not */ -esp_err_t deInitAccelerometer(void) +esp_err_t accelSetRange(qma_range_t range) { - return ESP_OK; + esp_err_t ret_val = qma7981_write_byte(QMA7981_REG_RANGE, range); + qma_range = range; + + return ret_val; } /** - * @brief Read all pending samples in IMU and perform a sensor fusion pass + * @brief Read the current acceleration vector from the accelerometer and return + * the vector through arguments. If the read fails, the last known values are + * returned instead. * - * @return ESP_OK if successful, or nonzero if error. + * @param x The X component of the acceleration vector is written here + * @param y The Y component of the acceleration vector is written here + * @param z The Z component of the acceleration vector is written here + * @return ESP_OK if the acceleration was read, or a non-zero value if it was not */ -esp_err_t accelIntegrate() +esp_err_t accelGetAccelVec(int16_t* x, int16_t* y, int16_t* z) { - LSM6DSLData* ld = &LSM6DSL; - - int16_t data[6 * 16]; - - // Get temperature sensor (in case we ever want to use it) - int r = GeneralI2CGet(LSM6DSL_ADDRESS, 0x20, (uint8_t*)data, 2); - if (r < 0) - return r; - if (r == 2) - ld->temp = data[0]; - int readr = ReadLSM6DSL((uint8_t*)data, sizeof(data)); - if (readr < 0) - return readr; - int samp; - int16_t* cdata = data; - - uint32_t start = getCycleCount(); - - // STEP 0: Decide your coordinate frame. - - // [0] = +X axis coming out right of controller. - // [1] = +Y axis, pointing straight up out of controller, out where the USB port is. - // [2] = +Z axis, pointing up from the face of the controller. - - ld->lastreadr = readr; - - for (samp = 0; samp < readr; samp += 6) - { - // Extract data from IMU - int16_t* euler_deltas = cdata; // Euler angles, from gyro. - int16_t* accel_data = cdata + 3; - - // Manual cal, used only for Steps 2..8 - // euler_deltas[0] -= 12; - // euler_deltas[1] += 22; - // euler_deltas[2] += 4; - - // We can sum rotations to understand the amount of counts in a full circle. - // Note: this is actually more of a debug mechanism. - ld->gyroaccum[0] += euler_deltas[0]; - ld->gyroaccum[1] += euler_deltas[1]; - ld->gyroaccum[2] += euler_deltas[2]; - - // STEP 1: Visually inspect the gyro values. - // STEP 2: Integrate the gyro values, verify they are correct. - - // 2000 dps full-scale - // 32768 is full-scale - // 208 SPS - // convert to radians. ( 2000.0f / 32768.0f / 208.0f * 2.0 * 3.14159f / 180.0f ); - // Measured = 560,000 counts per scale (Measured by looking at sum) - // Testing -> 3.14159 * 2.0 / 566000; - float fFudge = 0.5625; // XXX TODO: Investigate. - float fScale = (2000.0f / 32768.0f / 208.0f * 2.0 * 3.14159f / 180.0f) * fFudge; - - // STEP 3: Integrate gyro values into a quaternion. - // This step is validated by working with just one axis at a time - // then apply a coordinate frame to ld->fqQuat and validate that it is - // correct. - float fEulerScales[3] = {-fScale, fScale, -fScale}; - - float fEulers[3] - = {euler_deltas[0] * fEulerScales[0], euler_deltas[1] * fEulerScales[1], euler_deltas[2] * fEulerScales[2]}; - - // Used for calibration - if (ld->performCal) - { - float diff[3] - = {fEulers[0] - ld->fvAverage[0], fEulers[1] - ld->fvAverage[1], fEulers[2] - ld->fvAverage[2]}; - - float diffsq[3] = {(diff[0] < 0) ? -diff[0] : diff[0], (diff[1] < 0) ? -diff[1] : diff[1], - (diff[2] < 0) ? -diff[2] : diff[2]}; - - diffsq[0] *= 1000.0; - diffsq[1] *= 1000.0; - diffsq[2] *= 1000.0; - - ld->fvDeviation[0] -= 0.004; - ld->fvDeviation[1] -= 0.004; - ld->fvDeviation[2] -= 0.004; - - if (ld->fvDeviation[0] < diffsq[0]) - ld->fvDeviation[0] = diffsq[0]; - if (ld->fvDeviation[1] < diffsq[1]) - ld->fvDeviation[1] = diffsq[1]; - if (ld->fvDeviation[2] < diffsq[2]) - ld->fvDeviation[2] = diffsq[2]; - - if (ld->fvDeviation[0] > 0.8) - ld->fvDeviation[0] = 0.8; - if (ld->fvDeviation[1] > 0.8) - ld->fvDeviation[1] = 0.8; - if (ld->fvDeviation[2] > 0.8) - ld->fvDeviation[2] = 0.8; - - diff[0] *= mathsqrtf(ld->fvDeviation[0]) * 0.5; - diff[1] *= mathsqrtf(ld->fvDeviation[1]) * 0.5; - diff[2] *= mathsqrtf(ld->fvDeviation[2]) * 0.5; - - ld->fvAverage[0] += diff[0]; - ld->fvAverage[1] += diff[1]; - ld->fvAverage[2] += diff[2]; - - // Compute the running RMS error. - float fvEuler = accelGetStdDevInCal(); - - if (fvEuler < 0.00015f) - { - ld->fvBias[0] = -ld->fvAverage[0]; - ld->fvBias[1] = -ld->fvAverage[1]; - ld->fvBias[2] = -ld->fvAverage[2]; - struct fiunion - { - union - { - int32_t i; - float f; - } u; - }; - struct fiunion x, y, z; - x.u.f = ld->fvBias[0]; - y.u.f = ld->fvBias[1]; - z.u.f = ld->fvBias[2]; - writeNvs32("gyrocalx", x.u.i); - writeNvs32("gyrocaly", y.u.i); - writeNvs32("gyrocalz", z.u.i); - - ld->performCal = 0; - ld->fvDeviation[0] = 0; - ld->fvDeviation[0] = 1; - ld->fvDeviation[0] = 2; - } - } - - fEulers[0] += ld->fvBias[0]; - fEulers[1] += ld->fvBias[1]; - fEulers[2] += ld->fvBias[2]; - - mathEulerToQuat(ld->fqQuatLast, fEulers); - mathQuatApply(ld->fqQuat, ld->fqQuat, ld->fqQuatLast); - - // STEP 4: Validate yor values by doing 4 90 degree turns - // across multiple axes. - // i.e. rotate controller down, clockwise from top, up, counter-clockwise. - // while investigating quaternion. It should return to identity. - - // STEP 6: Determine our "error" based on accelerometer. - // NOTE: This step could be done on the inner loop if you want, and done over - // every accelerometer cycle, or it can be done on the outside, every few cycles. - // all that realy matters is that it is done periodically. - - // STEP 6A: Examine vectors. Generally speaking, we want an "up" vector, not a gravity vector. - // this is "up" in the controller's point of view. - float accel_up[3] = {-accel_data[0], accel_data[1], -accel_data[2]}; - - float accel_inverse_mag - = rsqrtf(accel_up[0] * accel_up[0] + accel_up[1] * accel_up[1] + accel_up[2] * accel_up[2]); - accel_up[0] *= accel_inverse_mag; - accel_up[1] *= accel_inverse_mag; - accel_up[2] *= accel_inverse_mag; - - ld->fvLastAccelRaw[0] = accel_up[0]; - ld->fvLastAccelRaw[1] = accel_up[1]; - ld->fvLastAccelRaw[2] = accel_up[2]; - - // Step 6B: Next, compute what we think "up" should be from our point of view. We will use +Y Up. - float what_we_think_is_up[3] = {0, 1, 0}; - mathRotateVectorByInverseOfQuaternion(what_we_think_is_up, LSM6DSL.fqQuat, what_we_think_is_up); - - // Step 6C: Next, we determine how far off we are. This will tell us our error. - float corrective_quaternion[4]; - - // TRICKY: The ouput of this is actually the axis of rotation, which is ironically - // in vector-form the same as a quaternion. So we can write directly into the quat. - mathCrossProduct(corrective_quaternion + 1, accel_up, what_we_think_is_up); - - // Now, we apply this in step 7. - - // First, we can compute what the drift values of our axes are, to anti-drift them. - // If you do only this, you will always end up in an unstable oscillation. - memcpy(ld->fCorrectLast, corrective_quaternion + 1, 12); - - // XXX TODO: We need to multiply by amount the accelerometer gives us assurance. - ld->fvBias[0] += mathsqrtf(corrective_quaternion[1]) * 0.0000002; - ld->fvBias[1] += mathsqrtf(corrective_quaternion[2]) * 0.0000002; - ld->fvBias[2] += mathsqrtf(corrective_quaternion[3]) * 0.0000002; - - float corrective_force = (ld->sampCount++ == 0) ? 0.5f : 0.0005f; - - // Second, we can apply a very small corrective tug. This helps prevent oscillation - // about the correct answer. This acts sort of like a P term to a PID loop. - // This is actually the **primary**, or fastest responding thing. - corrective_quaternion[1] *= corrective_force; - corrective_quaternion[2] *= corrective_force; - corrective_quaternion[3] *= corrective_force; - - // x^2+y^2+z^2+q^2 -> ALGEBRA! -> sqrt( 1-x^2-y^2-z^2 ) = w - corrective_quaternion[0] = mathsqrtf(1 - corrective_quaternion[1] * corrective_quaternion[1] - - corrective_quaternion[2] * corrective_quaternion[2] - - corrective_quaternion[3] * corrective_quaternion[3]); - - mathQuatApply(ld->fqQuat, ld->fqQuat, corrective_quaternion); - - cdata += 6; - } + static int16_t lastX = 0; + static int16_t lastY = 0; + static int16_t lastZ = 0; - // Now we move onto the outer loop. - // STEP 5: We now want to normalize the quat periodically. Don't do this too - // soon, otherwise you won't notice math errors. Realistically, this should - // only need to be done every hundreds of thousands of samples. - // - // Also, don't do this too often, otherwise you will reduce accuracy, - // unnecessarily. - float* qRot = ld->fqQuat; - float qmagsq = qRot[0] * qRot[0] + qRot[1] * qRot[1] + qRot[2] * qRot[2] + qRot[3] * qRot[3]; - if (qmagsq > 1.05 || qmagsq < 0.95) - { - // This normalizes everything. - qmagsq = rsqrtf(qmagsq); - qRot[0] = qRot[0] * qmagsq; - qRot[1] = qRot[1] * qmagsq; - qRot[2] = qRot[2] * qmagsq; - qRot[3] = qRot[3] * qmagsq; - } + // Read 6 bytes of data(0x00) + uint8_t raw_data[6]; + // Do the read + esp_err_t ret_val = qma7981_read_bytes(QMA7981_REG_DX_L, 6, raw_data); - if (samp) + // If the read was successful + if (ESP_OK == ret_val) { - ld->gyrolast[0] = cdata[-6]; - ld->gyrolast[1] = cdata[-5]; - ld->gyrolast[2] = cdata[-4]; - ld->accellast[0] = cdata[-3]; - ld->accellast[1] = cdata[-2]; - ld->accellast[2] = cdata[-1]; + // Sign extend the 10 bit value to 16 bits and save it as the last known value + // TODO The datasheet mentions this is a 14 bit reading, not a 10 bit one? + lastX = signExtend10bit(((raw_data[0] >> 6) | (raw_data[1]) << 2) & 0x03FF); + lastY = -signExtend10bit(((raw_data[2] >> 6) | (raw_data[3]) << 2) & 0x03FF); + lastZ = -signExtend10bit(((raw_data[4] >> 6) | (raw_data[5]) << 2) & 0x03FF); } - ld->computetime = getCycleCount() - start; - - return ESP_OK; -} - -/** - * @brief Get the RAW accelerometer "up" vector from device point of view. - * - * @return ESP_OK - */ -esp_err_t accelGetAccelVecRaw(int16_t* x, int16_t* y, int16_t* z) -{ - *x = LSM6DSL.fvLastAccelRaw[0] * 256; - *y = LSM6DSL.fvLastAccelRaw[1] * 256; - *z = LSM6DSL.fvLastAccelRaw[2] * 256; - return ESP_OK; -} - -/** - * @brief Get the current orientation "up" vector from device point of view. - * - * @return ESP_OK - */ -esp_err_t accelGetOrientVec(int16_t* x, int16_t* y, int16_t* z) -{ - float plusy[3] = {0, 1, 0}; - mathRotateVectorByInverseOfQuaternion(plusy, LSM6DSL.fqQuat, plusy); - *x = plusy[0] * 256; - *y = plusy[1] * 256; - *z = plusy[2] * 256; - return ESP_OK; -} - -/** - * @brief Get the current estimated quaternion of the IMU. - * - * @return ESP_OK - */ -esp_err_t accelGetQuaternion(float* q) -{ - float* fq = LSM6DSL.fqQuat; - q[0] = fq[0]; - q[1] = fq[1]; - q[2] = fq[2]; - q[3] = fq[3]; - return ESP_OK; -} + // Copy out the acceleration value + *x = lastX; + *y = lastY; + *z = lastZ; -/** - * @brief Initialize calibration of IMU. - * - * @return ESP_OK If cal start was OK. - */ -esp_err_t accelPerformCal() -{ - LSM6DSL.performCal = 1; - LSM6DSL.fvDeviation[0] = 1; - LSM6DSL.fvDeviation[1] = 1; - LSM6DSL.fvDeviation[2] = 1; - LSM6DSL.fvAverage[0] = 0; - LSM6DSL.fvAverage[1] = 0; - LSM6DSL.fvAverage[2] = 0; - - // Ignore failures here. - eraseNvsKey("gyrocalx"); - eraseNvsKey("gyrocaly"); - eraseNvsKey("gyrocalz"); - - return ESP_OK; + return ret_val; } /** - * @brief Get current divergence as the cal algorithm attempts to converge on a good cal. + * @brief Helper function to sign-extend a 10 bit two's complement number to 16 bit * - * @return If in cal mode, will return current motion deviation. If out of cal mode, will return 0.0f + * @param in The two's compliment number to sign-extend + * @return The sign-extended two's compliment number */ -float accelGetStdDevInCal() +static int16_t signExtend10bit(uint16_t in) { - if (LSM6DSL.performCal == 0) - return 0.0f; - float* d = LSM6DSL.fvDeviation; - return (d[0] * d[0] + d[1] * d[1] + d[2] * d[2]); -} - -/** - * @brief Reset the IMU's core registers. Should be done any time you are entering a mode with the IMU. - * - */ -void accelSetRegistersAndReset(void) -{ - LSM6DSLSet(LSM6DSL_FIFO_CTRL5, (0b0101 << 3) | 0b000); // Reset FIFO - LSM6DSLSet( - LSM6DSL_FIFO_CTRL5, - (0b0101 << 3) - | 0b110); // 208 Hz ODR, Continuous mode. If the FIFO is full, the new sample overwrites the older one. - LSM6DSLSet(LSM6DSL_FIFO_CTRL3, 0b00001001); // Put both devices (Accel + Gyro) in FIFO. - LSM6DSLSet(LSM6DSL_CTRL1_XL, 0b01011001); // Setup accel (16 g's FS) - LSM6DSLSet(LSM6DSL_CTRL2_G, 0b01011100); // Setup gyro, 2000dps - LSM6DSLSet(LSM6DSL_CTRL4_C, 0x00); // Disable all filtering. - LSM6DSLSet(LSM6DSL_CTRL7_G, 0b00000000); // Setup gyro, not high performance mode = 0x80. High perf = 0x00 - LSM6DSLSet(LSM6DSL_FIFO_CTRL2, 0b00000000); // Temp not in fifo (Why no work?) - - memset(&LSM6DSL, 0, sizeof(LSM6DSL)); - LSM6DSL.fqQuat[0] = 1; - LSM6DSL.fqQuatLast[0] = 1; - LSM6DSL.sampCount = 0; - if (!readNvs32("gyrocalx", (int32_t*)&LSM6DSL.fvBias[0])) + if (in & 0x200) { - LSM6DSL.performCal = 1; - LSM6DSL.fvBias[0] = 0; + return (in | 0xFC00); // extend the sign bit all the way out } - if (!readNvs32("gyrocaly", (int32_t*)&LSM6DSL.fvBias[1])) - { - LSM6DSL.performCal = 1; - LSM6DSL.fvBias[1] = 0; - } - if (!readNvs32("gyrocalz", (int32_t*)&LSM6DSL.fvBias[2])) + else { - LSM6DSL.performCal = 1; - LSM6DSL.fvBias[2] = 0; + return (in & 0x01FF); // make sure the sign bits are cleared } } diff --git a/components/hdw-accel/include/hdw-accel.h b/components/hdw-accel/include/hdw-accel.h index 731062d69..2aa8f7312 100644 --- a/components/hdw-accel/include/hdw-accel.h +++ b/components/hdw-accel/include/hdw-accel.h @@ -1,38 +1,42 @@ /*! \file hdw-accel.h * - * \section imu_design Design Philosophy + * \section accel_design Design Philosophy * - * Originally swadges were planned to use a LSM6DSL and a QMC6308, however, because the batteries are so close to the - * magnetometer, the quality of the data was low enough we dcided to proceed with with a LSM6DSL-only IMU. + * The accelerometer used is a QMA7981. + * The datasheet can be found here: QMA7981 + * Datasheet. * - * Unlike the accelerometer process, the IMU fuses the gyroscope and accelerometer data from the LMS6DSL. By fusing - * both sensors, we are able to produce a quaternion to represent the rotation of the swadge. The idea is that - * we run the IMU at 208 Hz, and we use the hardware FIFO built into the LSM6DSL to queue up events. Then, every - * frame, we empty out the FIFO. + * The accelerometer component does not automatically poll the accelerometer. + * All it does is set up and configure the accelerometer, then it is up to the Swadge mode to query for acceleration as + * appropriate. + * + * This component requires the I2C bus to be initialized, so it does that as well. + * If other I2C peripherals are added in the future, common I2C bus initialization should be moved to a more common + * location. * * \section accel_usage Usage * - * The core system will call initAccelerometer() and deInitAccelerometer() appropriately. And you can at any point - * call any of the proper IMU / accel functions. + * You don't need to call initAccelerometer() or deInitAccelerometer(). The system does this the appropriate time. + * + * You do need to call accelGetAccelVec() to get the current acceleration vector. + * If you want to poll this from your Swadge mode's main function, you may. * - * The functions you can use are: - * esp_err_t accelGetAccelVec(int16_t* x, int16_t* y, int16_t* z); - * esp_err_t accelGetQuaternion( float * quaternion ); + * You may call accelSetRange() if you want to adjust the measurement range. * - * You can, of course at any time call: - * esp_err_t accelIntegrate(); + * accelGetStep() exists, but it has not been tested, so use it with caution. + * You may need to configure parameters related to step counting. * * \section accel_example Example * * \code{.c} - * // Declare variables to receive rotation - * float q[4]; + * // Declare variables to receive acceleration + * int16_t a_x, a_y, a_z; * - * // Get the current rotation - * if(ESP_OK == accelGetQuaternion( q )) + * // Get the current acceleration + * if(ESP_OK == accelGetAccelVec(&a_x, &a_y, &a_z)) * { * // Print data to debug logs - * printf( "%f %f %f %f\n", q[0], q[1], q[2], q[3] ); + * printf("x: %"PRId16", y: %"PRId16", z:%"PRId16, a_x, a_y, a_z); * } * \endcode */ @@ -46,62 +50,33 @@ #include #include -typedef struct +/** + * @brief The ranges for acceleration measurement from 2G to 32G (Earth gravity) + */ +typedef enum { - int32_t temp; - uint32_t computetime; - uint32_t performCal; // 1 if expecting a zero cal. - - // Quats are wxyz. - // You can take a vector, in controller space, rotate by this quat, and you get it in world space. - float fqQuatLast[4]; // Delta - float fqQuat[4]; // Absolute - - // The last raw accelerometer (NOT FUSED) - float fvLastAccelRaw[3]; - - // Bias for all of the euler angles. - float fvBias[3]; - - // Used for calibration - float fvDeviation[3]; - float fvAverage[3]; + QMA_RANGE_2G = 0b0001, ///< Two G's of measurement range + QMA_RANGE_4G = 0b0010, ///< Four G's of measurement range + QMA_RANGE_8G = 0b0100, ///< Eight G's of measurement range + QMA_RANGE_16G = 0b1000, ///< Sixteen G's of measurement range + QMA_RANGE_32G = 0b1111, ///< Thirty-two G's of measurement range +} qma_range_t; - uint32_t sampCount; - - // For debug - int lastreadr; - int32_t gyroaccum[3]; - int16_t gyrolast[3]; - int16_t accellast[3]; - float fCorrectLast[3]; -} LSM6DSLData; - -extern LSM6DSLData LSM6DSL; +/** + * @brief The bandwidth for acceleration measurement + */ +typedef enum +{ + QMA_BANDWIDTH_128_HZ = 0b111, ///< 128Hz bandwidth + QMA_BANDWIDTH_256_HZ = 0b110, ///< 256Hz bandwidth + QMA_BANDWIDTH_1024_HZ = 0b101, ///< 1024Hz bandwidth +} qma_bandwidth_t; -esp_err_t initAccelerometer(i2c_port_t _i2c_port, gpio_num_t sda, gpio_num_t scl, gpio_pullup_t pullup, uint32_t clkHz); +esp_err_t initAccelerometer(i2c_port_t _i2c_port, gpio_num_t sda, gpio_num_t scl, gpio_pullup_t pullup, uint32_t clkHz, + qma_range_t range, qma_bandwidth_t bandwidth); esp_err_t deInitAccelerometer(void); -esp_err_t accelGetAccelVecRaw(int16_t* x, int16_t* y, int16_t* z); -esp_err_t accelGetOrientVec(int16_t* x, int16_t* y, int16_t* z); -esp_err_t accelGetQuaternion(float* q); -esp_err_t accelIntegrate(void); -esp_err_t accelPerformCal(void); -float accelGetStdDevInCal(void); -void accelSetRegistersAndReset(void); - -// Utility functions (to replace at a later time) - -float rsqrtf(float x); -float mathsqrtf(float x); -void mathEulerToQuat(float* q, const float* euler); -void mathQuatApply(float* qout, const float* q1, const float* q2); -void mathQuatNormalize(float* qout, const float* qin); -void mathCrossProduct(float* p, const float* a, const float* b); -void mathRotateVectorByInverseOfQuaternion(float* pout, const float* q, const float* p); -void mathRotateVectorByQuaternion(float* pout, const float* q, const float* p); -esp_err_t GeneralSet(int dev, int reg, int val); -esp_err_t LSM6DSLSet(int reg, int val); -int GeneralI2CGet(int device, int reg, uint8_t* data, int data_len); -int ReadLSM6DSL(uint8_t* data, int data_len); +esp_err_t accelSetRange(qma_range_t range); +esp_err_t accelGetAccelVec(int16_t* x, int16_t* y, int16_t* z); +esp_err_t accelGetStep(uint16_t* data); #endif diff --git a/components/hdw-imu/CMakeLists.txt b/components/hdw-imu/CMakeLists.txt new file mode 100644 index 000000000..47dacdc1a --- /dev/null +++ b/components/hdw-imu/CMakeLists.txt @@ -0,0 +1,4 @@ +idf_component_register(SRCS "hdw-imu.c" + INCLUDE_DIRS "include" + REQUIRES driver + PRIV_REQUIRES hdw-nvs) diff --git a/components/hdw-imu/hdw-imu.c b/components/hdw-imu/hdw-imu.c new file mode 100644 index 000000000..1b292d279 --- /dev/null +++ b/components/hdw-imu/hdw-imu.c @@ -0,0 +1,877 @@ +//============================================================================== +// Includes +//============================================================================== + +#include +#include +#include +#include "hdw-imu.h" +#include "soc/rtc_cntl_reg.h" +#include "soc/gpio_reg.h" +#include "soc/io_mux_reg.h" +#include "rom/gpio.h" +#include "soc/i2c_reg.h" +#include "soc/gpio_struct.h" +#include "hdw-nvs.h" + +//============================================================================== +// Enums +//============================================================================== + +typedef enum __attribute__((packed)) +{ + LSM6DSL_FUNC_CFG_ACCESS = 0x01, + LSM6DSL_SENSOR_SYNC_TIME_FRAME = 0x04, + LSM6DSL_FIFO_CTRL1 = 0x06, + LSM6DSL_FIFO_CTRL2 = 0x07, + LSM6DSL_FIFO_CTRL3 = 0x08, + LSM6DSL_FIFO_CTRL4 = 0x09, + LSM6DSL_FIFO_CTRL5 = 0x0a, + LSM6DSL_ORIENT_CFG_G = 0x0b, + LSM6DSL_INT1_CTRL = 0x0d, + LSM6DSL_INT2_CTRL = 0x0e, + LMS6DS3_WHO_AM_I = 0x0f, + LSM6DSL_CTRL1_XL = 0x10, + LSM6DSL_CTRL2_G = 0x11, + LSM6DSL_CTRL3_C = 0x12, + LSM6DSL_CTRL4_C = 0x13, + LSM6DSL_CTRL5_C = 0x14, + LSM6DSL_CTRL6_C = 0x15, + LSM6DSL_CTRL7_G = 0x16, + LSM6DSL_CTRL8_XL = 0x17, + LSM6DSL_CTRL9_XL = 0x18, + LSM6DSL_CTRL10_C = 0x19, + LSM6DSL_MASTER_CONFIG = 0x1a, + LSM6DSL_WAKE_UP_SRC = 0x1b, + LSM6DSL_TAP_SRC = 0x1c, + LSM6DSL_D6D_SRC = 0x1d, + LSM6DSL_STATUS_REG = 0x1e, + LSM6DSL_OUT_TEMP_L = 0x20, + LSM6DSL_OUT_TEMP_H = 0x21, + LMS6DS3_OUTX_L_G = 0x22, + LMS6DS3_OUTX_H_G = 0x23, + LMS6DS3_OUTY_L_G = 0x24, + LMS6DS3_OUTY_H_G = 0x25, + LMS6DS3_OUTZ_L_G = 0x26, + LMS6DS3_OUTZ_H_G = 0x27, + LMS6DS3_OUTX_L_XL = 0x28, + LMS6DS3_OUTX_H_XL = 0x29, + LMS6DS3_OUTY_L_XL = 0x2a, + LMS6DS3_OUTY_H_XL = 0x2b, + LMS6DS3_OUTZ_L_XL = 0x2c, + LMS6DS3_OUTZ_H_XL = 0x2d, +} lsm6dslReg_t; + +//============================================================================== +// Defines +//============================================================================== + +#define LSM6DSL_ADDRESS 0x6a +#define QMC6308_ADDRESS 0x2c + +//============================================================================== +// Variables +//============================================================================== + +static i2c_port_t i2c_port; +LSM6DSLData LSM6DSL; + +//============================================================================== +// Utility Prototypes +//============================================================================== + +float rsqrtf(float x); +float mathsqrtf(float x); +void mathEulerToQuat(float* q, const float* euler); +void mathQuatApply(float* qout, const float* q1, const float* q2); +void mathQuatNormalize(float* qout, const float* qin); +void mathCrossProduct(float* p, const float* a, const float* b); +void mathRotateVectorByInverseOfQuaternion(float* pout, const float* q, const float* p); +void mathRotateVectorByQuaternion(float* pout, const float* q, const float* p); + +static inline uint32_t getCycleCount(); + +esp_err_t GeneralSet(int dev, int reg, int val); +esp_err_t LSM6DSLSet(int reg, int val); +int GeneralI2CGet(int device, int reg, uint8_t* data, int data_len); +int ReadLSM6DSL(uint8_t* data, int data_len); + +//============================================================================== +// Function Prototypes +//============================================================================== + +esp_err_t initAccelerometer(i2c_port_t _i2c_port, gpio_num_t sda, gpio_num_t scl, gpio_pullup_t pullup, uint32_t clkHz); +esp_err_t deInitAccelerometer(void); +esp_err_t accelGetAccelVecRaw(int16_t* x, int16_t* y, int16_t* z); +esp_err_t accelGetOrientVec(int16_t* x, int16_t* y, int16_t* z); +esp_err_t accelGetQuaternion(float* q); +esp_err_t accelIntegrate(void); +float accelGetStdDevInCal(void); +void accelSetRegistersAndReset(void); + +//============================================================================== +// Utility Functions +//============================================================================== + +/** + * @brief Perform a fast, approximate reciprocal square root + * + * @param x The number to take a recriprocal square root of. + * @return approximately 1/sqrt(x) + */ +float rsqrtf(float x) +{ + typedef union + { + int32_t i; + float f; + } fiunion; + const float xhalf = 0.5f * x; + fiunion i = {.f = x}; + i.i = 0x5f375a86 - (i.i >> 1); + x = i.f; + x = x * (1.5f - xhalf * x * x); + x = x * (1.5f - xhalf * x * x); + return x; +} + +/** + * @brief Perform a fast, approximate square root + * + * @param x The number to take a square root of. + * @return approximately sqrt(x) (but is much faster) + */ +float mathsqrtf(float x) +{ + // Trick to do approximate, fast square roots. (Though it is surprisingly fast) + int sign = x < 0; + if (sign) + x = -x; + if (x < 0.0000001) + return 0.0001; + float o = x; + o = (o + x / o) / 2; + o = (o + x / o) / 2; + o = (o + x / o) / 2; + o = (o + x / o) / 2; + if (sign) + return -o; + else + return o; +} + +/** + * @brief convert euler angles (in radians) to a quaternion. + * + * @param q Pointer to the wxyz quat (float[4]) to be written. + * @param euler Pointer to a float[3] of euler angles. + */ +void mathEulerToQuat(float* q, const float* euler) +{ + float pitch = euler[0]; + float yaw = euler[1]; + float roll = euler[2]; + float cr = cosf(pitch * 0.5); + float sr = sinf(pitch * 0.5); // Pitch: About X + float cp = cosf(yaw * 0.5); + float sp = sinf(yaw * 0.5); // Yaw: About Y + float cy = cosf(roll * 0.5); + float sy = sinf(roll * 0.5); // Roll: About Z + q[0] = cr * cp * cy + sr * sp * sy; + q[1] = sr * cp * cy - cr * sp * sy; + q[2] = cr * sp * cy + sr * cp * sy; + q[3] = cr * cp * sy - sr * sp * cy; +} + +/** + * @brief Rotate one quaternion by another (and do not normalize) + * + * @param qout Pointer to the wxyz quat (float[4]) to be written. + * @param q1 First quaternion to be rotated. + * @param q2 Quaternion to rotate q1 by. + */ +void mathQuatApply(float* qout, const float* q1, const float* q2) +{ + // NOTE: Does not normalize - you will need to normalize eventually. + float tmpw, tmpx, tmpy; + tmpw = (q1[0] * q2[0]) - (q1[1] * q2[1]) - (q1[2] * q2[2]) - (q1[3] * q2[3]); + tmpx = (q1[0] * q2[1]) + (q1[1] * q2[0]) + (q1[2] * q2[3]) - (q1[3] * q2[2]); + tmpy = (q1[0] * q2[2]) - (q1[1] * q2[3]) + (q1[2] * q2[0]) + (q1[3] * q2[1]); + qout[3] = (q1[0] * q2[3]) + (q1[1] * q2[2]) - (q1[2] * q2[1]) + (q1[3] * q2[0]); + qout[2] = tmpy; + qout[1] = tmpx; + qout[0] = tmpw; +} + +/** + * @brief Normalize a quaternion + * + * @param qout Pointer to the wxyz quat (float[4]) to be written. + * @param qin Pointer to the quaterion to normalize. + */ +void mathQuatNormalize(float* qout, const float* qin) +{ + float qmag = qin[0] * qin[0] + qin[1] * qin[1] + qin[2] * qin[2] + qin[3] * qin[3]; + qmag = rsqrtf(qmag); + qout[0] = qin[0] * qmag; + qout[1] = qin[1] * qmag; + qout[2] = qin[2] * qmag; + qout[3] = qin[3] * qmag; +} + +/** + * @brief Perform a 3D cross product + * + * @param p Pointer to the float[3] output of the cross product (p = a x b) + * @param a Pointer to the float[3] of the cross product a vector. + * @param a Pointer to the float[3] of the cross product b vector. + */ +void mathCrossProduct(float* p, const float* a, const float* b) +{ + float tx = a[1] * b[2] - a[2] * b[1]; + float ty = a[2] * b[0] - a[0] * b[2]; + p[2] = a[0] * b[1] - a[1] * b[0]; + p[1] = ty; + p[0] = tx; +} + +/** + * @brief Rotate a 3D vector by a quaternion + * + * @param pout Pointer to the float[3] output of the rotation + * @param q Pointer to the wzyz quaternion (float[4]) of the rotation. + * @param p Pointer to the float[3] of the vector to rotates. + */ +void mathRotateVectorByQuaternion(float* pout, const float* q, const float* p) +{ + // return v + 2.0 * cross(q.xyz, cross(q.xyz, v) + q.w * v); + float iqo[3]; + mathCrossProduct(iqo, q + 1 /*.xyz*/, p); + iqo[0] += q[0] * p[0]; + iqo[1] += q[0] * p[1]; + iqo[2] += q[0] * p[2]; + float ret[3]; + mathCrossProduct(ret, q + 1 /*.xyz*/, iqo); + pout[0] = ret[0] * 2.0 + p[0]; + pout[1] = ret[1] * 2.0 + p[1]; + pout[2] = ret[2] * 2.0 + p[2]; +} + +/** + * @brief Rotate a 3D vector by the inverse of a quaternion + * + * @param pout Pointer to the float[3] output of the antirotation. + * @param q Pointer to the wzyz quaternion (float[4]) opposite of the rotation. + * @param p Pointer to the float[3] of the vector to antirotates. + */ +void mathRotateVectorByInverseOfQuaternion(float* pout, const float* q, const float* p) +{ + // General note: Performing a transform this way can be about 20-30% slower than a well formed 3x3 matrix. + // return v + 2.0 * cross(q.xyz, cross(q.xyz, v) + q.w * v); + float iqo[3]; + mathCrossProduct(iqo, p, q + 1 /*.xyz*/); + iqo[0] += q[0] * p[0]; + iqo[1] += q[0] * p[1]; + iqo[2] += q[0] * p[2]; + float ret[3]; + mathCrossProduct(ret, iqo, q + 1 /*.xyz*/); + pout[0] = ret[0] * 2.0 + p[0]; + pout[1] = ret[1] * 2.0 + p[1]; + pout[2] = ret[2] * 2.0 + p[2]; +} + +static inline uint32_t getCycleCount() +{ + uint32_t ccount; + asm volatile("rsr %0,ccount" : "=a"(ccount)); + return ccount; +} + +//============================================================================== +// Internal Functions +//============================================================================== + +/** + * @brief Set a specific register on the IMU to a value. + * + * @param dev The 7-bit address of the device to set the register to. + * @param reg The 8-bit register # + * @param val The 8-bit value to set the register to. + * @return ESP_OK if the operation was successful. + */ +esp_err_t GeneralSet(int dev, int reg, int val) +{ + i2c_cmd_handle_t cmdHandle = i2c_cmd_link_create(); + i2c_master_start(cmdHandle); + i2c_master_write_byte(cmdHandle, dev << 1, false); + i2c_master_write_byte(cmdHandle, reg, false); + i2c_master_write_byte(cmdHandle, val, true); + i2c_master_stop(cmdHandle); + esp_err_t err = i2c_master_cmd_begin(I2C_NUM_0, cmdHandle, 100); + i2c_cmd_link_delete(cmdHandle); + return err; +} + +/** + * @brief Set a specific register on the LSM6DSL to a value. + * + * @param reg The 8-bit register # + * @param val The 8-bit value to set the register to. + * @return ESP_OK if the operation was successful. + */ +esp_err_t LSM6DSLSet(int reg, int val) +{ + return GeneralSet(LSM6DSL_ADDRESS, reg, val); +} + +/** + * @brief Read a buffer back from a specific I2C device. + * + * @param device The 7-bit device address + * @param reg The 8-bit register # to start at. + * @param data The buffer to load the data into. + * @param data_len Number of bytes to read. + * @return positive number if operation was successful, or esp_err_t if failure. + */ +int GeneralI2CGet(int device, int reg, uint8_t* data, int data_len) +{ + i2c_cmd_handle_t cmdHandle = i2c_cmd_link_create(); + i2c_master_start(cmdHandle); + i2c_master_write_byte(cmdHandle, device << 1, false); + i2c_master_write_byte(cmdHandle, reg, false); + i2c_master_start(cmdHandle); + i2c_master_write_byte(cmdHandle, device << 1 | I2C_MASTER_READ, false); + i2c_master_read(cmdHandle, data, data_len, I2C_MASTER_LAST_NACK); + i2c_master_stop(cmdHandle); + esp_err_t err = i2c_master_cmd_begin(I2C_NUM_0, cmdHandle, 100); + i2c_cmd_link_delete(cmdHandle); + if (err) + { + ESP_LOGE("accel", "Error on link: %d", err); + return -1; + } + else + return data_len; +} + +/** + * @brief Read the FIFO out of the LSM6DSL + * + * @param data The buffer to write the FIFO data into. + * @param data_len The maximum size (in words) to read. + * @return positive number if operation was successful, or esp_err_t if failure. + */ +int ReadLSM6DSL(uint8_t* data, int data_len) +{ + i2c_cmd_handle_t cmdHandle = i2c_cmd_link_create(); + i2c_master_start(cmdHandle); + i2c_master_write_byte(cmdHandle, LSM6DSL_ADDRESS << 1, false); + i2c_master_write_byte(cmdHandle, 0x3A, false); + i2c_master_start(cmdHandle); + i2c_master_write_byte(cmdHandle, LSM6DSL_ADDRESS << 1 | I2C_MASTER_READ, false); + uint32_t fifolen = 0; + i2c_master_read(cmdHandle, (uint8_t*)&fifolen, 3, I2C_MASTER_LAST_NACK); + i2c_master_stop(cmdHandle); + esp_err_t err = i2c_master_cmd_begin(I2C_NUM_0, cmdHandle, 100); + i2c_cmd_link_delete(cmdHandle); + if (err < 0) + return -1; + + // Is fifo overflow. + if (fifolen & 0x4000) + { + // reset fifo. + // If we overflow, and we don't do this, bad things happen. + LSM6DSLSet(LSM6DSL_FIFO_CTRL5, (0b0101 << 3) | 0b000); // Disable fifo + LSM6DSLSet(LSM6DSL_FIFO_CTRL5, (0b0101 << 3) | 0b110); // 208 Hz ODR + LSM6DSL.sampCount = 0; + return 0; + } + + fifolen &= 0x7ff; + if (fifolen == 0) + return 0; + if (fifolen > data_len / 2) + fifolen = data_len / 2; + + cmdHandle = i2c_cmd_link_create(); + i2c_master_start(cmdHandle); + i2c_master_write_byte(cmdHandle, LSM6DSL_ADDRESS << 1 | I2C_MASTER_READ, false); + i2c_master_read(cmdHandle, data, fifolen * 2, I2C_MASTER_LAST_NACK); + i2c_master_stop(cmdHandle); + err = i2c_master_cmd_begin(I2C_NUM_0, cmdHandle, 100); + + i2c_cmd_link_delete(cmdHandle); + if (err < 0) + return -2; + + return fifolen; +} + +//============================================================================== +// Functions +//============================================================================== + +/** + * @brief Initialize the IMU + * + * @param _i2c_port The i2c port to use for the IMU + * @param sda The GPIO for the Serial DAta line + * @param scl The GPIO for the Serial CLock line + * @param pullup Either \c GPIO_PULLUP_DISABLE if there are external pullup resistors on SDA and SCL or \c + * GPIO_PULLUP_ENABLE if internal pull-ups should be used + * @param clkHz The frequency of the I2C clock + * @return ESP_OK if the accelerometer initialized, or a non-zero value if it did not + */ +esp_err_t initAccelerometer(i2c_port_t _i2c_port, gpio_num_t sda, gpio_num_t scl, gpio_pullup_t pullup, uint32_t clkHz) +{ + int retry = 0; + i2c_port = _i2c_port; + esp_err_t ret_val; + +do_retry: + + // Shake any device off the bus. + int i; + int gpio_scl = 41; + for (i = 0; i < 16; i++) + { + gpio_matrix_out(gpio_scl, 256, 1, 0); + GPIO.out1_w1tc.val = (1 << (gpio_scl - 32)); + esp_rom_delay_us(10); + gpio_matrix_out(gpio_scl, 256, 1, 0); + GPIO.out1_w1ts.val = (1 << (gpio_scl - 32)); + esp_rom_delay_us(10); + } + gpio_matrix_out(gpio_scl, 29, 0, 0); + + ret_val = ESP_OK; + + i2c_driver_delete(_i2c_port); + + /* Install i2c driver */ + i2c_config_t conf = { + .mode = I2C_MODE_MASTER, + .sda_io_num = sda, + .sda_pullup_en = pullup, + .scl_io_num = scl, + .scl_pullup_en = pullup, + .master.clk_speed = clkHz, // tested upto 1.4Mbit/s + .clk_flags = I2C_SCLK_SRC_FLAG_FOR_NOMAL, + }; + ESP_LOGI("accel", "i2c_driver_install=%d", i2c_driver_install(_i2c_port, conf.mode, 0, 0, 0)); + ret_val |= i2c_param_config(i2c_port, &conf); + + // Enable access + LSM6DSLSet(LSM6DSL_FUNC_CFG_ACCESS, 0x20); + LSM6DSLSet(LSM6DSL_CTRL3_C, 0x81); // Force reset + vTaskDelay(1); + LSM6DSLSet(LSM6DSL_CTRL3_C, 0x44); // unforce reset + + uint8_t who = 0xaa; + int r = GeneralI2CGet(LSM6DSL_ADDRESS, LMS6DS3_WHO_AM_I, &who, 1); + if (r != 1 || who != 0x6a) + { + ESP_LOGW("accel", "WHOAMI Failed (%02x), %d", who, r); + if (retry++ < 10) + goto do_retry; + ESP_LOGE("accel", "Init failed on 1"); + return ESP_FAIL; + } + ESP_LOGI("accel", "Init Start"); + + accelSetRegistersAndReset(); + + for (i = 0; i < 2; i++) + { + vTaskDelay(1); + int check = accelIntegrate(); + if (check != ESP_OK) + { + ESP_LOGI("accel", "Init Fault Retry"); + if (retry++ < 10) + goto do_retry; + ESP_LOGI("accel", "Init failed on 2"); + return ESP_FAIL; + } + ESP_LOGI("accel", "Check %d", check); + } + + ESP_LOGI("accel", "Init Ok"); + return ESP_OK; +} + +/** + * @brief Deinit the accelerometer (nothing to do) + * + * @return ESP_OK + */ +esp_err_t deInitAccelerometer(void) +{ + return ESP_OK; +} + +/** + * @brief Read all pending samples in IMU and perform a sensor fusion pass + * + * @return ESP_OK if successful, or nonzero if error. + */ +esp_err_t accelIntegrate() +{ + LSM6DSLData* ld = &LSM6DSL; + + int16_t data[6 * 16]; + + // Get temperature sensor (in case we ever want to use it) + int r = GeneralI2CGet(LSM6DSL_ADDRESS, 0x20, (uint8_t*)data, 2); + if (r < 0) + return r; + if (r == 2) + ld->temp = data[0]; + int readr = ReadLSM6DSL((uint8_t*)data, sizeof(data)); + if (readr < 0) + return readr; + int samp; + int16_t* cdata = data; + + uint32_t start = getCycleCount(); + + // STEP 0: Decide your coordinate frame. + + // [0] = +X axis coming out right of controller. + // [1] = +Y axis, pointing straight up out of controller, out where the USB port is. + // [2] = +Z axis, pointing up from the face of the controller. + + ld->lastreadr = readr; + + for (samp = 0; samp < readr; samp += 6) + { + // Extract data from IMU + int16_t* euler_deltas = cdata; // Euler angles, from gyro. + int16_t* accel_data = cdata + 3; + + // Manual cal, used only for Steps 2..8 + // euler_deltas[0] -= 12; + // euler_deltas[1] += 22; + // euler_deltas[2] += 4; + + // We can sum rotations to understand the amount of counts in a full circle. + // Note: this is actually more of a debug mechanism. + ld->gyroaccum[0] += euler_deltas[0]; + ld->gyroaccum[1] += euler_deltas[1]; + ld->gyroaccum[2] += euler_deltas[2]; + + // STEP 1: Visually inspect the gyro values. + // STEP 2: Integrate the gyro values, verify they are correct. + + // 2000 dps full-scale + // 32768 is full-scale + // 208 SPS + // convert to radians. ( 2000.0f / 32768.0f / 208.0f * 2.0 * 3.14159f / 180.0f ); + // Measured = 560,000 counts per scale (Measured by looking at sum) + // Testing -> 3.14159 * 2.0 / 566000; + float fFudge = 0.5625; // XXX TODO: Investigate. + float fScale = (2000.0f / 32768.0f / 208.0f * 2.0 * 3.14159f / 180.0f) * fFudge; + + // STEP 3: Integrate gyro values into a quaternion. + // This step is validated by working with just one axis at a time + // then apply a coordinate frame to ld->fqQuat and validate that it is + // correct. + float fEulerScales[3] = {-fScale, fScale, -fScale}; + + float fEulers[3] + = {euler_deltas[0] * fEulerScales[0], euler_deltas[1] * fEulerScales[1], euler_deltas[2] * fEulerScales[2]}; + + // Used for calibration + if (ld->performCal) + { + float diff[3] + = {fEulers[0] - ld->fvAverage[0], fEulers[1] - ld->fvAverage[1], fEulers[2] - ld->fvAverage[2]}; + + float diffsq[3] = {(diff[0] < 0) ? -diff[0] : diff[0], (diff[1] < 0) ? -diff[1] : diff[1], + (diff[2] < 0) ? -diff[2] : diff[2]}; + + diffsq[0] *= 1000.0; + diffsq[1] *= 1000.0; + diffsq[2] *= 1000.0; + + ld->fvDeviation[0] -= 0.004; + ld->fvDeviation[1] -= 0.004; + ld->fvDeviation[2] -= 0.004; + + if (ld->fvDeviation[0] < diffsq[0]) + ld->fvDeviation[0] = diffsq[0]; + if (ld->fvDeviation[1] < diffsq[1]) + ld->fvDeviation[1] = diffsq[1]; + if (ld->fvDeviation[2] < diffsq[2]) + ld->fvDeviation[2] = diffsq[2]; + + if (ld->fvDeviation[0] > 0.8) + ld->fvDeviation[0] = 0.8; + if (ld->fvDeviation[1] > 0.8) + ld->fvDeviation[1] = 0.8; + if (ld->fvDeviation[2] > 0.8) + ld->fvDeviation[2] = 0.8; + + diff[0] *= mathsqrtf(ld->fvDeviation[0]) * 0.5; + diff[1] *= mathsqrtf(ld->fvDeviation[1]) * 0.5; + diff[2] *= mathsqrtf(ld->fvDeviation[2]) * 0.5; + + ld->fvAverage[0] += diff[0]; + ld->fvAverage[1] += diff[1]; + ld->fvAverage[2] += diff[2]; + + // Compute the running RMS error. + float fvEuler = accelGetStdDevInCal(); + + if (fvEuler < 0.00015f) + { + ld->fvBias[0] = -ld->fvAverage[0]; + ld->fvBias[1] = -ld->fvAverage[1]; + ld->fvBias[2] = -ld->fvAverage[2]; + struct fiunion + { + union + { + int32_t i; + float f; + } u; + }; + struct fiunion x, y, z; + x.u.f = ld->fvBias[0]; + y.u.f = ld->fvBias[1]; + z.u.f = ld->fvBias[2]; + writeNvs32("gyrocalx", x.u.i); + writeNvs32("gyrocaly", y.u.i); + writeNvs32("gyrocalz", z.u.i); + + ld->performCal = 0; + ld->fvDeviation[0] = 0; + ld->fvDeviation[0] = 1; + ld->fvDeviation[0] = 2; + } + } + + fEulers[0] += ld->fvBias[0]; + fEulers[1] += ld->fvBias[1]; + fEulers[2] += ld->fvBias[2]; + + mathEulerToQuat(ld->fqQuatLast, fEulers); + mathQuatApply(ld->fqQuat, ld->fqQuat, ld->fqQuatLast); + + // STEP 4: Validate yor values by doing 4 90 degree turns + // across multiple axes. + // i.e. rotate controller down, clockwise from top, up, counter-clockwise. + // while investigating quaternion. It should return to identity. + + // STEP 6: Determine our "error" based on accelerometer. + // NOTE: This step could be done on the inner loop if you want, and done over + // every accelerometer cycle, or it can be done on the outside, every few cycles. + // all that realy matters is that it is done periodically. + + // STEP 6A: Examine vectors. Generally speaking, we want an "up" vector, not a gravity vector. + // this is "up" in the controller's point of view. + float accel_up[3] = {-accel_data[0], accel_data[1], -accel_data[2]}; + + float accel_inverse_mag + = rsqrtf(accel_up[0] * accel_up[0] + accel_up[1] * accel_up[1] + accel_up[2] * accel_up[2]); + accel_up[0] *= accel_inverse_mag; + accel_up[1] *= accel_inverse_mag; + accel_up[2] *= accel_inverse_mag; + + ld->fvLastAccelRaw[0] = accel_up[0]; + ld->fvLastAccelRaw[1] = accel_up[1]; + ld->fvLastAccelRaw[2] = accel_up[2]; + + // Step 6B: Next, compute what we think "up" should be from our point of view. We will use +Y Up. + float what_we_think_is_up[3] = {0, 1, 0}; + mathRotateVectorByInverseOfQuaternion(what_we_think_is_up, LSM6DSL.fqQuat, what_we_think_is_up); + + // Step 6C: Next, we determine how far off we are. This will tell us our error. + float corrective_quaternion[4]; + + // TRICKY: The ouput of this is actually the axis of rotation, which is ironically + // in vector-form the same as a quaternion. So we can write directly into the quat. + mathCrossProduct(corrective_quaternion + 1, accel_up, what_we_think_is_up); + + // Now, we apply this in step 7. + + // First, we can compute what the drift values of our axes are, to anti-drift them. + // If you do only this, you will always end up in an unstable oscillation. + memcpy(ld->fCorrectLast, corrective_quaternion + 1, 12); + + // XXX TODO: We need to multiply by amount the accelerometer gives us assurance. + ld->fvBias[0] += mathsqrtf(corrective_quaternion[1]) * 0.0000002; + ld->fvBias[1] += mathsqrtf(corrective_quaternion[2]) * 0.0000002; + ld->fvBias[2] += mathsqrtf(corrective_quaternion[3]) * 0.0000002; + + float corrective_force = (ld->sampCount++ == 0) ? 0.5f : 0.0005f; + + // Second, we can apply a very small corrective tug. This helps prevent oscillation + // about the correct answer. This acts sort of like a P term to a PID loop. + // This is actually the **primary**, or fastest responding thing. + corrective_quaternion[1] *= corrective_force; + corrective_quaternion[2] *= corrective_force; + corrective_quaternion[3] *= corrective_force; + + // x^2+y^2+z^2+q^2 -> ALGEBRA! -> sqrt( 1-x^2-y^2-z^2 ) = w + corrective_quaternion[0] = mathsqrtf(1 - corrective_quaternion[1] * corrective_quaternion[1] + - corrective_quaternion[2] * corrective_quaternion[2] + - corrective_quaternion[3] * corrective_quaternion[3]); + + mathQuatApply(ld->fqQuat, ld->fqQuat, corrective_quaternion); + + cdata += 6; + } + + // Now we move onto the outer loop. + // STEP 5: We now want to normalize the quat periodically. Don't do this too + // soon, otherwise you won't notice math errors. Realistically, this should + // only need to be done every hundreds of thousands of samples. + // + // Also, don't do this too often, otherwise you will reduce accuracy, + // unnecessarily. + float* qRot = ld->fqQuat; + float qmagsq = qRot[0] * qRot[0] + qRot[1] * qRot[1] + qRot[2] * qRot[2] + qRot[3] * qRot[3]; + if (qmagsq > 1.05 || qmagsq < 0.95) + { + // This normalizes everything. + qmagsq = rsqrtf(qmagsq); + qRot[0] = qRot[0] * qmagsq; + qRot[1] = qRot[1] * qmagsq; + qRot[2] = qRot[2] * qmagsq; + qRot[3] = qRot[3] * qmagsq; + } + + if (samp) + { + ld->gyrolast[0] = cdata[-6]; + ld->gyrolast[1] = cdata[-5]; + ld->gyrolast[2] = cdata[-4]; + ld->accellast[0] = cdata[-3]; + ld->accellast[1] = cdata[-2]; + ld->accellast[2] = cdata[-1]; + } + + ld->computetime = getCycleCount() - start; + + return ESP_OK; +} + +/** + * @brief Get the RAW accelerometer "up" vector from device point of view. + * + * @return ESP_OK + */ +esp_err_t accelGetAccelVecRaw(int16_t* x, int16_t* y, int16_t* z) +{ + *x = LSM6DSL.fvLastAccelRaw[0] * 256; + *y = LSM6DSL.fvLastAccelRaw[1] * 256; + *z = LSM6DSL.fvLastAccelRaw[2] * 256; + return ESP_OK; +} + +/** + * @brief Get the current orientation "up" vector from device point of view. + * + * @return ESP_OK + */ +esp_err_t accelGetOrientVec(int16_t* x, int16_t* y, int16_t* z) +{ + float plusy[3] = {0, 1, 0}; + mathRotateVectorByInverseOfQuaternion(plusy, LSM6DSL.fqQuat, plusy); + *x = plusy[0] * 256; + *y = plusy[1] * 256; + *z = plusy[2] * 256; + return ESP_OK; +} + +/** + * @brief Get the current estimated quaternion of the IMU. + * + * @return ESP_OK + */ +esp_err_t accelGetQuaternion(float* q) +{ + float* fq = LSM6DSL.fqQuat; + q[0] = fq[0]; + q[1] = fq[1]; + q[2] = fq[2]; + q[3] = fq[3]; + return ESP_OK; +} + +/** + * @brief Initialize calibration of IMU. + * + * @return ESP_OK If cal start was OK. + */ +esp_err_t accelPerformCal() +{ + LSM6DSL.performCal = 1; + LSM6DSL.fvDeviation[0] = 1; + LSM6DSL.fvDeviation[1] = 1; + LSM6DSL.fvDeviation[2] = 1; + LSM6DSL.fvAverage[0] = 0; + LSM6DSL.fvAverage[1] = 0; + LSM6DSL.fvAverage[2] = 0; + + // Ignore failures here. + eraseNvsKey("gyrocalx"); + eraseNvsKey("gyrocaly"); + eraseNvsKey("gyrocalz"); + + return ESP_OK; +} + +/** + * @brief Get current divergence as the cal algorithm attempts to converge on a good cal. + * + * @return If in cal mode, will return current motion deviation. If out of cal mode, will return 0.0f + */ +float accelGetStdDevInCal() +{ + if (LSM6DSL.performCal == 0) + return 0.0f; + float* d = LSM6DSL.fvDeviation; + return (d[0] * d[0] + d[1] * d[1] + d[2] * d[2]); +} + +/** + * @brief Reset the IMU's core registers. Should be done any time you are entering a mode with the IMU. + * + */ +void accelSetRegistersAndReset(void) +{ + LSM6DSLSet(LSM6DSL_FIFO_CTRL5, (0b0101 << 3) | 0b000); // Reset FIFO + LSM6DSLSet( + LSM6DSL_FIFO_CTRL5, + (0b0101 << 3) + | 0b110); // 208 Hz ODR, Continuous mode. If the FIFO is full, the new sample overwrites the older one. + LSM6DSLSet(LSM6DSL_FIFO_CTRL3, 0b00001001); // Put both devices (Accel + Gyro) in FIFO. + LSM6DSLSet(LSM6DSL_CTRL1_XL, 0b01011001); // Setup accel (16 g's FS) + LSM6DSLSet(LSM6DSL_CTRL2_G, 0b01011100); // Setup gyro, 2000dps + LSM6DSLSet(LSM6DSL_CTRL4_C, 0x00); // Disable all filtering. + LSM6DSLSet(LSM6DSL_CTRL7_G, 0b00000000); // Setup gyro, not high performance mode = 0x80. High perf = 0x00 + LSM6DSLSet(LSM6DSL_FIFO_CTRL2, 0b00000000); // Temp not in fifo (Why no work?) + + memset(&LSM6DSL, 0, sizeof(LSM6DSL)); + LSM6DSL.fqQuat[0] = 1; + LSM6DSL.fqQuatLast[0] = 1; + LSM6DSL.sampCount = 0; + if (!readNvs32("gyrocalx", (int32_t*)&LSM6DSL.fvBias[0])) + { + LSM6DSL.performCal = 1; + LSM6DSL.fvBias[0] = 0; + } + if (!readNvs32("gyrocaly", (int32_t*)&LSM6DSL.fvBias[1])) + { + LSM6DSL.performCal = 1; + LSM6DSL.fvBias[1] = 0; + } + if (!readNvs32("gyrocalz", (int32_t*)&LSM6DSL.fvBias[2])) + { + LSM6DSL.performCal = 1; + LSM6DSL.fvBias[2] = 0; + } +} diff --git a/components/hdw-imu/include/hdw-imu.h b/components/hdw-imu/include/hdw-imu.h new file mode 100644 index 000000000..fec1381a1 --- /dev/null +++ b/components/hdw-imu/include/hdw-imu.h @@ -0,0 +1,107 @@ +/*! \file hdw-imu.h + * + * \section imu_design Design Philosophy + * + * Originally Swadges were planned to use a LSM6DSL and a QMC6308, however, because the batteries are so close to the + * magnetometer, the quality of the data was low enough we decided to proceed with with a LSM6DSL-only IMU. + * + * Unlike the accelerometer process, the IMU fuses the gyroscope and accelerometer data from the LMS6DSL. By fusing + * both sensors, we are able to produce a quaternion to represent the rotation of the swadge. The idea is that + * we run the IMU at 208 Hz, and we use the hardware FIFO built into the LSM6DSL to queue up events. Then, every + * frame, we empty out the FIFO. + * + * \section accel_usage Usage + * + * The core system will call initAccelerometer() and deInitAccelerometer() appropriately. And you can at any point + * call any of the proper IMU / accel functions. + * + * The functions you can use are: + * esp_err_t accelGetAccelVec(int16_t* x, int16_t* y, int16_t* z); + * esp_err_t accelGetQuaternion( float * quaternion ); + * + * You can, of course at any time call: + * esp_err_t accelIntegrate(); + * + * \section accel_example Example + * + * \code{.c} + * // Declare variables to receive rotation + * float q[4]; + * + * // Get the current rotation + * if(ESP_OK == accelGetQuaternion( q )) + * { + * // Print data to debug logs + * printf( "%f %f %f %f\n", q[0], q[1], q[2], q[3] ); + * } + * \endcode + */ + +#ifndef _HDW_ACCEL_H_ +#define _HDW_ACCEL_H_ + +#include + +#include +#include +#include + +typedef struct +{ + int32_t temp; + uint32_t computetime; + uint32_t performCal; // 1 if expecting a zero cal. + + // Quats are wxyz. + // You can take a vector, in controller space, rotate by this quat, and you get it in world space. + float fqQuatLast[4]; // Delta + float fqQuat[4]; // Absolute + + // The last raw accelerometer (NOT FUSED) + float fvLastAccelRaw[3]; + + // Bias for all of the euler angles. + float fvBias[3]; + + // Used for calibration + float fvDeviation[3]; + float fvAverage[3]; + + uint32_t sampCount; + + // For debug + int lastreadr; + int32_t gyroaccum[3]; + int16_t gyrolast[3]; + int16_t accellast[3]; + float fCorrectLast[3]; +} LSM6DSLData; + +extern LSM6DSLData LSM6DSL; + +esp_err_t initAccelerometer(i2c_port_t _i2c_port, gpio_num_t sda, gpio_num_t scl, gpio_pullup_t pullup, uint32_t clkHz); +esp_err_t deInitAccelerometer(void); +esp_err_t accelGetAccelVecRaw(int16_t* x, int16_t* y, int16_t* z); +esp_err_t accelGetOrientVec(int16_t* x, int16_t* y, int16_t* z); +esp_err_t accelGetQuaternion(float* q); +esp_err_t accelIntegrate(void); +esp_err_t accelPerformCal(void); +float accelGetStdDevInCal(void); +void accelSetRegistersAndReset(void); + +// Utility functions (to replace at a later time) + +float rsqrtf(float x); +float mathsqrtf(float x); +void mathEulerToQuat(float* q, const float* euler); +void mathQuatApply(float* qout, const float* q1, const float* q2); +void mathQuatNormalize(float* qout, const float* qin); +void mathCrossProduct(float* p, const float* a, const float* b); +void mathRotateVectorByInverseOfQuaternion(float* pout, const float* q, const float* p); +void mathRotateVectorByQuaternion(float* pout, const float* q, const float* p); +esp_err_t GeneralSet(int dev, int reg, int val); +esp_err_t LSM6DSLSet(int reg, int val); +int GeneralI2CGet(int device, int reg, uint8_t* data, int data_len); +int ReadLSM6DSL(uint8_t* data, int data_len); + +#endif diff --git a/emulator/src/components/hdw-accel/hdw-accel.c b/emulator/src/components/hdw-imu/hdw-imu.c similarity index 99% rename from emulator/src/components/hdw-accel/hdw-accel.c rename to emulator/src/components/hdw-imu/hdw-imu.c index 244ca537c..6a673fb87 100644 --- a/emulator/src/components/hdw-accel/hdw-accel.c +++ b/emulator/src/components/hdw-imu/hdw-imu.c @@ -2,8 +2,8 @@ // Includes //============================================================================== -#include "hdw-accel.h" -#include "hdw-accel_emu.h" +#include "hdw-imu.h" +#include "hdw-imu_emu.h" #include "trigonometry.h" #include "esp_random.h" #include "emu_args.h" diff --git a/emulator/src/components/hdw-accel/hdw-accel_emu.h b/emulator/src/components/hdw-imu/hdw-imu_emu.h similarity index 100% rename from emulator/src/components/hdw-accel/hdw-accel_emu.h rename to emulator/src/components/hdw-imu/hdw-imu_emu.h diff --git a/emulator/src/emu_main.c b/emulator/src/emu_main.c index 90fc5482c..fcbbd3d9f 100644 --- a/emulator/src/emu_main.c +++ b/emulator/src/emu_main.c @@ -26,7 +26,7 @@ #include "hdw-bzr.h" #include "hdw-btn.h" #include "hdw-btn_emu.h" -#include "hdw-accel_emu.h" +#include "hdw-imu_emu.h" #include "swadge2024.h" #include "macros.h" #include "trigonometry.h" diff --git a/emulator/src/extensions/fuzzer/ext_fuzzer.c b/emulator/src/extensions/fuzzer/ext_fuzzer.c index 5c81c5033..114d9acb6 100644 --- a/emulator/src/extensions/fuzzer/ext_fuzzer.c +++ b/emulator/src/extensions/fuzzer/ext_fuzzer.c @@ -8,7 +8,7 @@ #include "ext_fuzzer.h" #include "emu_args.h" #include "hdw-btn_emu.h" -#include "hdw-accel_emu.h" +#include "hdw-imu_emu.h" //============================================================================== // Function Prototypes diff --git a/emulator/src/extensions/replay/ext_replay.c b/emulator/src/extensions/replay/ext_replay.c index 340487282..4e3576044 100644 --- a/emulator/src/extensions/replay/ext_replay.c +++ b/emulator/src/extensions/replay/ext_replay.c @@ -3,8 +3,8 @@ #include "esp_timer.h" #include "hdw-btn.h" #include "hdw-btn_emu.h" -#include "hdw-accel.h" -#include "hdw-accel_emu.h" +#include "hdw-imu.h" +#include "hdw-imu_emu.h" #include "macros.h" #include "emu_main.h" #include "ext_modes.h" diff --git a/main/CMakeLists.txt b/main/CMakeLists.txt index 0b7a0e96b..46c667a82 100644 --- a/main/CMakeLists.txt +++ b/main/CMakeLists.txt @@ -100,7 +100,7 @@ idf_component_register(SRCS "swadge2024.c" "utils/trigonometry.c" "utils/vector2d.c" "utils/wheel_menu.c" - PRIV_REQUIRES hdw-accel + PRIV_REQUIRES hdw-imu hdw-battmon hdw-btn hdw-bzr diff --git a/main/swadge2024.c b/main/swadge2024.c index 576ddbcc8..246fbee59 100644 --- a/main/swadge2024.c +++ b/main/swadge2024.c @@ -71,7 +71,7 @@ * - hdw-battmon.h: Learn how to check the battery voltage! * - hdw-btn.h: Learn how to use both push and touch button input! * - hdw-bzr.h: Learn how to use the buzzer! - * - hdw-accel.h: Learn how to use the accelerometer! + * - hdw-imu.h: Learn how to use the inertial measurement unit! * - hdw-led.h: Learn how to use the LEDs! * - hdw-mic.h: Learn how to use the microphone! * - hdw-temperature.h: Learn how to use the temperature sensor! diff --git a/main/swadge2024.h b/main/swadge2024.h index 2c6c2b7e9..e4a5ddbcf 100644 --- a/main/swadge2024.h +++ b/main/swadge2024.h @@ -165,7 +165,7 @@ // Hardware interfaces #include "crashwrap.h" -#include "hdw-accel.h" +#include "hdw-imu.h" #include "hdw-battmon.h" #include "hdw-btn.h" #include "hdw-bzr.h" diff --git a/tools/sandbox_test/buildhelp.c b/tools/sandbox_test/buildhelp.c index 001d8a0b8..65b418a79 100644 --- a/tools/sandbox_test/buildhelp.c +++ b/tools/sandbox_test/buildhelp.c @@ -186,7 +186,7 @@ int main( int argc, char ** argv ) snprintf( temp, sizeof( temp ) - 1, "-I%s/components/hdw-touch/include", argv[2] ); appendcflag( temp ); snprintf( temp, sizeof( temp ) - 1, "-I%s/components/hdw-esp-now/include", argv[2] ); appendcflag( temp ); snprintf( temp, sizeof( temp ) - 1, "-I%s/components/hdw-temperature/include", argv[2] ); appendcflag( temp ); - snprintf( temp, sizeof( temp ) - 1, "-I%s/components/hdw-accel/include", argv[2] ); appendcflag( temp ); + snprintf( temp, sizeof( temp ) - 1, "-I%s/components/hdw-imu/include", argv[2] ); appendcflag( temp ); snprintf( temp, sizeof( temp ) - 1, "-I%s/components/hdw-mic/include", argv[2] ); appendcflag( temp ); snprintf( temp, sizeof( temp ) - 1, "-I%s/components/hdw-spiffs/include", argv[2] ); appendcflag( temp ); snprintf( temp, sizeof( temp ) - 1, "-I%s/components/hdw-led/include", argv[2] ); appendcflag( temp ); diff --git a/tools/sandbox_test/test_i2c/sandbox.c b/tools/sandbox_test/test_i2c/sandbox.c index 6c4f9663c..dde90d369 100644 --- a/tools/sandbox_test/test_i2c/sandbox.c +++ b/tools/sandbox_test/test_i2c/sandbox.c @@ -20,7 +20,7 @@ #include "bunny.h" -#include "hdw-accel.h" +#include "hdw-imu.h" int16_t bunny_verts_out[ sizeof(bunny_verts)/3/2*3 ]; From 90b6ffe048c82f08c6575cc105d6fb2cb72785a2 Mon Sep 17 00:00:00 2001 From: gelakinetic Date: Sun, 15 Oct 2023 21:46:00 -0400 Subject: [PATCH 18/18] Fix doxygen warnings --- components/hdw-imu/hdw-imu.c | 2 +- components/hdw-imu/include/hdw-imu.h | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/components/hdw-imu/hdw-imu.c b/components/hdw-imu/hdw-imu.c index 1b292d279..b2b49a544 100644 --- a/components/hdw-imu/hdw-imu.c +++ b/components/hdw-imu/hdw-imu.c @@ -224,7 +224,7 @@ void mathQuatNormalize(float* qout, const float* qin) * * @param p Pointer to the float[3] output of the cross product (p = a x b) * @param a Pointer to the float[3] of the cross product a vector. - * @param a Pointer to the float[3] of the cross product b vector. + * @param b Pointer to the float[3] of the cross product b vector. */ void mathCrossProduct(float* p, const float* a, const float* b) { diff --git a/components/hdw-imu/include/hdw-imu.h b/components/hdw-imu/include/hdw-imu.h index fec1381a1..425952efc 100644 --- a/components/hdw-imu/include/hdw-imu.h +++ b/components/hdw-imu/include/hdw-imu.h @@ -10,7 +10,7 @@ * we run the IMU at 208 Hz, and we use the hardware FIFO built into the LSM6DSL to queue up events. Then, every * frame, we empty out the FIFO. * - * \section accel_usage Usage + * \section imu_usage Usage * * The core system will call initAccelerometer() and deInitAccelerometer() appropriately. And you can at any point * call any of the proper IMU / accel functions. @@ -22,7 +22,7 @@ * You can, of course at any time call: * esp_err_t accelIntegrate(); * - * \section accel_example Example + * \section imu_example Example * * \code{.c} * // Declare variables to receive rotation