upd

main/drivers/bmi270.h

@@ -2,12 +2,18 @@
 // --- Core IDs ---
 #include <stdint.h>
 #define BMI270_CHIP_ID 0x00
-#define BMI270_REV_ID 0x01
+#define BMI270_ERR_REG 0x02
+#define BMI270_STATUS 0x03
 
 // --- Power / system ---
 #define BMI270_PWR_CONF 0x7C
 #define BMI270_PWR_CTRL 0x7D
 #define BMI270_CMD 0x7E
+#define BMI270_INIT_CTRL 0x59
+#define BMI270_INIT_ADDR_0 0x5B
+#define BMI270_INIT_ADDR_1 0x5C
+#define BMI270_INIT_DATA 0x5E
+#define BMI270_INTERNAL_STAT 0x21
 
 // --- Accel config ---
 #define BMI270_ACC_CONF 0x40
@@ -18,37 +24,67 @@
 #define BMI270_GYR_RANGE 0x43
 
 // --- Data registers ---
-#define BMI270_ACC_X_L 0x0C
+#define BMI270_ACC_DATA_X_LSB 0x0C
-#define BMI270_ACC_X_H 0x0D
+#define BMI270_ACC_DATA_X_HSB 0x0D
-#define BMI270_ACC_Y_L 0x0E
+#define BMI270_ACC_DATA_Y_LSB 0x0E
-#define BMI270_ACC_Y_H 0x0F
+#define BMI270_ACC_DATA_Y_HSB 0x0F
-#define BMI270_ACC_Z_L 0x10
+#define BMI270_ACC_DATA_Z_LSB 0x10
-#define BMI270_ACC_Z_H 0x11
+#define BMI270_ACC_DATA_Z_HSB 0x11
 
-#define BMI270_GYR_X_L 0x12
+#define BMI270_GYR_DATA_X_LSB 0x12
-#define BMI270_GYR_X_H 0x13
+#define BMI270_GYR_DATA_X_HSB 0x13
-#define BMI270_GYR_Y_L 0x14
+#define BMI270_GYR_DATA_Y_LSB 0x14
-#define BMI270_GYR_Y_H 0x15
+#define BMI270_GYR_DATA_Y_HSB 0x15
-#define BMI270_GYR_Z_L 0x16
+#define BMI270_GYR_DATA_Z_LSB 0x16
-#define BMI270_GYR_Z_H 0x17
+#define BMI270_GYR_DATA_Z_HSB 0x17
+
+#define BMI270_FIFO_LENGTH_0 0x24
+#define BMI270_FIFO_DATA 0x26
+#define BMI270_FIFO_CONFIG_0 0x48
+#define BMI270_FIFO_CONFIG_1 0x49
 
 // --- Commands ---
 #define BMI270_CMD_SOFTRESET 0xB6
 #define BMI270_CMD_FIFO_FLUSH 0xB0
 
 // --- Power bits ---
-#define BMI270_PWR_ACCEL_EN (1 << 3)
+#define BMI270_PWR_TEMP_EN  (1 << 3)  // 0x08
-#define BMI270_PWR_GYRO_EN (1 << 2)
+#define BMI270_PWR_ACCEL_EN (1 << 2)  // 0x04
+#define BMI270_PWR_GYRO_EN  (1 << 1)  // 0x02
+#define BMI270_PWR_AUX_EN   (1 << 0)  // 0x01
 
 // --- Config presets (minimal usable defaults) ---
 #define BMI270_ACC_CONF_100HZ 0xA8
 #define BMI270_GYR_CONF_100HZ 0xA9
 
+#define ACC_SCALE (4.0f / 32768.0f)
+#define G_TO_MS2 9.80665f
+
+#define GYR_SCALE (2000.0f / 32768.0f)
+#define DEG_TO_RAD 0.01745329251f
+
 typedef struct {
-  int16_t ax, ay, az;
+  int16_t ax_raw;
-  int16_t gx, gy, gz;
+  int16_t ay_raw;
+  int16_t az_raw;
+
+  int16_t gx_raw;
+  int16_t gy_raw;
+  int16_t gz_raw;
+
+  float ax;
+  float ay;
+  float az;
+
+  float gx;
+  float gy;
+  float gz;
+
+  uint32_t timestamp_ms;
 } bmi270_data_t;
 
-void bmi270_init(void);
+bool bmi270_init(void);
 
-void bmi270_read(bmi270_data_t *d);
+bool bmi270_read(bmi270_data_t *d);
+
+extern const uint8_t bmi270_config_file[8192];

main/drivers/es8311.c

@@ -8,10 +8,19 @@
 
 #include "i2c.h"
 #include "pins.h"
+#include <math.h>
 
-void es8311_write(uint8_t reg, uint8_t val) {
+i2c_master_dev_handle_t es8311_i2c_dev;
-  i2c_write_reg(ES8311_I2C_ADDR, reg, val);
+
+int es8311_write(uint8_t reg, uint8_t val) {
+  esp_err_t ret = i2c_write_reg(es8311_i2c_dev, reg, val);
+  if (ret != ESP_OK) {
+    printf("ES8311 write failed: reg=0x%02X, ret=%d\n", reg, ret);
+    return -1;
   }
+  return 0;
+}
+
 
 void es8311_set_dac_volume(uint8_t vol) {
   es8311_write(ES8311_DAC_VOL_L, vol);
@@ -20,35 +29,92 @@ void es8311_set_dac_volume(uint8_t vol) {
 
 void es8311_set_adc_volume(uint8_t vol) { es8311_write(ES8311_ADC_VOL, vol); }
 
+
+uint8_t es8311_read(uint8_t reg) {
+  uint8_t val;
+  esp_err_t ret = i2c_read_reg(es8311_i2c_dev, reg, &val);
+  if (ret != ESP_OK) {
+    printf("ES8311 read failed: reg=0x%02X, ret=%d\n", reg, ret);
+    return 0xFF;
+  }
+  return val;
+}
+
+
+
+
+void es8311_verify_init(void) {
+  printf("ES8311 Register Dump:\n");
+  printf("PWR_UP_DOWN (0x0C): 0x%02X\n", es8311_read(0x0C));
+  printf("PWR_ANALOG (0x0D): 0x%02X\n", es8311_read(0x0D));
+  printf("SER_FMT (0x10): 0x%02X\n", es8311_read(0x10));
+  printf("DAC_VOL_L (0x33): 0x%02X\n", es8311_read(0x33));
+  printf("DAC_VOL_R (0x34): 0x%02X\n", es8311_read(0x34));
+}
+
 void es8311_init(void) {
+  printf("ES8311: Starting initialization...\n");
+
+  i2c_device_config_t dev_cfg = {
+      .device_address = ES8311_I2C_ADDR,
+      .scl_speed_hz = 400000,
+  };
+
+  esp_err_t ret = i2c_master_bus_add_device(bus, &dev_cfg, &es8311_i2c_dev);
+  if (ret != ESP_OK) {
+    printf("ES8311: Failed to add I2C device: %d\n", ret);
+    return;
+  }
+
+  // Reset chip
+  printf("ES8311: Resetting...\n");
   es8311_write(ES8311_RESET, ES8311_RESET_CMD);
+  vTaskDelay(pdMS_TO_TICKS(ES8311_RESET_DELAY_MS));
 
-  vTaskDelay(pdMS_TO_TICKS(20));
+  // Configure clock
+  printf("ES8311: Configuring clocks...\n");
+  es8311_write(ES8311_CLK_MANAGE, ES8311_CLK_ALL_EN);
+  es8311_write(ES8311_CLK_DIV, ES8311_CLK_DIV_DEFAULT);
 
-  // enable ADC + DAC
+  // Power up sequence
-  es8311_write(ES8311_PWR_UP_DOWN, ES8311_PWR_DAC_EN | ES8311_PWR_ADC_EN);
+  printf("ES8311: Powering up...\n");
+  es8311_write(ES8311_PWR_ANALOG, ES8311_PWR_ANALOG_OFF);
+  vTaskDelay(pdMS_TO_TICKS(ES8311_POWER_DELAY_MS));
 
-  es8311_write(ES8311_PWR_ANALOG, ES8311_ALL_ON);
+  es8311_write(ES8311_PWR_ANALOG, ES8311_PWR_ANALOG_ON);
+  vTaskDelay(pdMS_TO_TICKS(ES8311_POWER_DELAY_MS));
 
-  // I2S 16-bit
+  // I2S format
+  printf("ES8311: Configuring I2S format...\n");
   es8311_write(ES8311_SER_FMT, ES8311_FMT_I2S | ES8311_WORD_LEN_16);
 
-  // ADC config
+  // ADC configuration
-  es8311_write(ES8311_ADC_CTRL1, 0x44);
+  printf("ES8311: Configuring ADC...\n");
+  es8311_write(ES8311_ADC_CTRL1, ES8311_ADC_CTRL1_DEFAULT);
+  es8311_write(ES8311_ADC_CTRL2, ES8311_ADC_CTRL2_DEFAULT);
+  es8311_set_adc_volume(ES8311_ADC_VOL_DEFAULT);
 
-  es8311_write(ES8311_ADC_CTRL2, 0x0C);
+  // DAC configuration
+  printf("ES8311: Configuring DAC...\n");
+  es8311_write(ES8311_DAC_CTRL1, ES8311_DAC_CTRL1_DEFAULT);
+  es8311_write(ES8311_DAC_CTRL2, ES8311_DAC_CTRL2_DEFAULT);
+  es8311_set_dac_volume(ES8311_DAC_VOL_DEFAULT);
 
-  // DAC config
+  // Output configuration
-  es8311_write(ES8311_DAC_CTRL1, 0x04);
+  printf("ES8311: Configuring outputs...\n");
+  es8311_write(ES8311_HP_CTRL, ES8311_HP_CTRL_DEFAULT);
+  es8311_write(ES8311_SPK_CTRL, ES8311_SPK_CTRL_MAX);
 
-  es8311_write(ES8311_DAC_CTRL2, 0x0C);
+  // Enable ADC and DAC
-
+  printf("ES8311: Enabling ADC/DAC...\n");
-  es8311_set_dac_volume(0xBF);
+  es8311_write(ES8311_PWR_UP_DOWN, ES8311_PWR_DAC_EN | ES8311_PWR_ADC_EN);
-  es8311_set_adc_volume(0x88);
+  vTaskDelay(pdMS_TO_TICKS(ES8311_STARTUP_DELAY_MS));
-
-  es8311_write(ES8311_SPK_CTRL, 0xFF);
 
+  printf("ES8311: Initializing I2S...\n");
   audio_i2s_init();
+
+  printf("ES8311: Initialization complete\n");
+  es8311_verify_init();
 }
 
 int audio_write(const int16_t *samples, size_t count) {
@@ -76,14 +142,41 @@ int audio_read(int16_t *samples, size_t count) {
 }
 
 void audio_beep(void) {
-  static int16_t buf[4410];
+  // ES8311 expects stereo I2S (left + right channels)
+  static int16_t buf[ES8311_BEEP_DURATION_SAMPLES * 2]; // *2 for stereo
 
-  for (int i = 0; i < 4410; i++) {
+  for (int i = 0; i < ES8311_BEEP_DURATION_SAMPLES; i++) {
+    int phase = (i / ES8311_BEEP_HALF_PERIOD) & 1;
+    int16_t sample = phase ? ES8311_BEEP_AMPLITUDE : -ES8311_BEEP_AMPLITUDE;
 
-    int phase = (i / 20) & 1;
+    buf[i * 2] = sample;     // Left channel
-
+    buf[i * 2 + 1] = sample; // Right channel (same as left for mono)
-    buf[i] = phase ? 12000 : -12000;
   }
 
-  audio_write(buf, 4410);
+  int written = audio_write(buf, ES8311_BEEP_DURATION_SAMPLES * 2);
+  printf("Beep: wrote %d samples\n", written);
+}
+
+void audio_test_tone(void) {
+  const int sample_rate = 44100;
+  const int duration_ms = 1000;
+  const int freq_hz = 440; // A4 note
+  const int num_samples = (sample_rate * duration_ms) / 1000;
+
+  int16_t *buf = malloc(num_samples * 2 * sizeof(int16_t));
+  if (!buf) {
+    printf("Failed to allocate tone buffer\n");
+    return;
+  }
+
+  for (int i = 0; i < num_samples; i++) {
+    float t = (float)i / sample_rate;
+    int16_t sample = (int16_t)(sin(2.0f * M_PI * freq_hz * t) * 8000.0f);
+    buf[i * 2] = sample;     // Left
+    buf[i * 2 + 1] = sample; // Right
+  }
+
+  printf("Playing 440Hz tone for 1 second...\n");
+  audio_write(buf, num_samples * 2);
+  free(buf);
 }

main/drivers/es8311.h

@@ -6,7 +6,6 @@
 // ----------------------
 // I2C
 // ----------------------
-#define ES8311_I2C_ADDR 0x18
 
 // ----------------------
 // System
@@ -64,6 +63,58 @@
 #define ES8311_RESET_CMD 0x1F
 #define ES8311_ALL_ON 0xFF
 
+
+// ----------------------
+// Clock Management Values
+// ----------------------
+#define ES8311_CLK_ALL_EN 0x3F        // Enable all clocks
+#define ES8311_CLK_DIV_DEFAULT 0x00   // Default divider
+
+// ----------------------
+// Power Analog Values
+// ----------------------
+#define ES8311_PWR_ANALOG_OFF 0x00    // All analog blocks off
+#define ES8311_PWR_ANALOG_ON 0xBF     // Power up analog circuits
+
+// ----------------------
+// ADC Control Values
+// ----------------------
+#define ES8311_ADC_CTRL1_DEFAULT 0x44 // TODO: Document bit meanings
+#define ES8311_ADC_CTRL2_DEFAULT 0x0C // TODO: Document bit meanings
+
+// ----------------------
+// DAC Control Values
+// ----------------------
+#define ES8311_DAC_CTRL1_DEFAULT 0x04 // TODO: Document bit meanings
+#define ES8311_DAC_CTRL2_DEFAULT 0x0C // TODO: Document bit meanings
+
+// ----------------------
+// Output Control Values
+// ----------------------
+#define ES8311_HP_CTRL_DEFAULT 0xF0   // TODO: Document bit meanings
+#define ES8311_SPK_CTRL_MAX 0xFF      // Maximum speaker output
+
+// ----------------------
+// Volume Defaults
+// ----------------------
+#define ES8311_DAC_VOL_DEFAULT 0xBF   // ~75% volume
+#define ES8311_ADC_VOL_DEFAULT 0x88   // ~50% gain
+
+// ----------------------
+// Timing Constants (milliseconds)
+// ----------------------
+#define ES8311_RESET_DELAY_MS 20
+#define ES8311_POWER_DELAY_MS 10
+#define ES8311_STARTUP_DELAY_MS 50
+
+// ----------------------
+// Beep Configuration
+// ----------------------
+#define ES8311_BEEP_DURATION_SAMPLES 4410  // 100ms at 44.1kHz
+#define ES8311_BEEP_AMPLITUDE 12000        // ~36% of 16-bit max
+#define ES8311_BEEP_HALF_PERIOD 20         // ~2.2kHz tone
+
+
 void es8311_init(void);
 
 void es8311_set_dac_volume(uint8_t vol);
@@ -74,3 +125,11 @@ int audio_write(const int16_t *samples, size_t count);
 int audio_read(int16_t *samples, size_t count);
 
 void audio_beep(void);
+
+void audio_test_tone(void);
+
+int es8311_write(uint8_t reg, uint8_t val);
+
+uint8_t es8311_read(uint8_t reg);
+
+void es8311_verify_init(void);

main/drivers/i2c.c

@@ -1,4 +1,5 @@
 #include "i2c.h"
+#include "driver/i2c_types.h"
 #include "esp_err.h"
 #include <string.h>
 
@@ -17,71 +18,46 @@ void i2c_init(int sda, int scl) {
   i2c_new_master_bus(&cfg, &bus);
 }
 
-esp_err_t i2c_write_reg(uint8_t addr, uint8_t reg, uint8_t val) {
+esp_err_t i2c_write_reg(i2c_master_dev_handle_t dev, uint8_t reg, uint8_t val) {
   uint8_t data[2] = {reg, val};
 
-  i2c_master_dev_handle_t dev;
-  i2c_device_config_t dev_cfg = {
-      .device_address = addr,
-      .scl_speed_hz = 400000,
-  };
-
-  i2c_master_bus_add_device(bus, &dev_cfg, &dev);
-
   esp_err_t r = i2c_master_transmit(dev, data, 2, -1);
-
-  i2c_master_bus_rm_device(dev);
   return r;
 }
 
-esp_err_t i2c_read_reg(uint8_t addr, uint8_t reg, uint8_t *val) {
+esp_err_t i2c_write_reg_multi(i2c_master_dev_handle_t dev, uint8_t reg, const uint8_t *data,
-  i2c_master_dev_handle_t dev;
+                          size_t len) {
-  i2c_device_config_t dev_cfg = {
-      .device_address = addr,
-      .scl_speed_hz = 400000,
-  };
 
-  i2c_master_bus_add_device(bus, &dev_cfg, &dev);
+  esp_err_t err;
+  // reg + payload
+  uint8_t buf[len + 1];
+  buf[0] = reg;
+  memcpy(&buf[1], data, len);
 
+  err = i2c_master_transmit(dev, buf, len + 1, -1);
+
+  return err;
+}
+
+esp_err_t i2c_read_reg(i2c_master_dev_handle_t dev, uint8_t reg, uint8_t *val) {
   i2c_master_transmit_receive(dev, &reg, 1, val, 1, -1);
 
-  i2c_master_bus_rm_device(dev);
   return ESP_OK;
 }
 
-esp_err_t i2c_read_multi(uint8_t addr, uint8_t reg, uint8_t *buf, int len) {
+esp_err_t i2c_read_multi(i2c_master_dev_handle_t dev, uint8_t reg, uint8_t *buf, int len) {
-  i2c_master_dev_handle_t dev;
-  i2c_device_config_t dev_cfg = {
-      .device_address = addr,
-      .scl_speed_hz = 400000,
-  };
-
-  i2c_master_bus_add_device(bus, &dev_cfg, &dev);
 
   i2c_master_transmit_receive(dev, &reg, 1, buf, len, -1);
 
-  i2c_master_bus_rm_device(dev);
   return ESP_OK;
 }
 
-esp_err_t i2c_read_reg_multi(uint8_t addr, uint8_t reg, uint8_t *buf,
+esp_err_t i2c_read_reg_multi(i2c_master_dev_handle_t dev, uint8_t reg, uint8_t *buf,
                              size_t len) {
-  i2c_master_dev_handle_t dev;
-
-  i2c_device_config_t cfg = {
-      .device_address = addr,
-      .scl_speed_hz = 400000,
-  };
 
   esp_err_t err;
 
-  err = i2c_master_bus_add_device(bus, &cfg, &dev);
-  if (err != ESP_OK)
-    return err;
-
   err = i2c_master_transmit_receive(dev, &reg, 1, buf, len, -1);
 
-  i2c_master_bus_rm_device(dev);
-
   return err;
 }

main/drivers/i2c.h

@@ -8,9 +8,11 @@ extern i2c_master_bus_handle_t bus;
 
 void i2c_init(int sda, int scl);
 
-esp_err_t i2c_write_reg(uint8_t addr, uint8_t reg, uint8_t val);
+esp_err_t i2c_write_reg(i2c_master_dev_handle_t dev, uint8_t reg, uint8_t val);
-esp_err_t i2c_read_reg(uint8_t addr, uint8_t reg, uint8_t *val);
+esp_err_t i2c_write_reg_multi(i2c_master_dev_handle_t dev, uint8_t reg, const uint8_t *data,
-esp_err_t i2c_read_multi(uint8_t addr, uint8_t reg, uint8_t *buf, int len);
+                          size_t len);
+esp_err_t i2c_read_reg(i2c_master_dev_handle_t dev, uint8_t reg, uint8_t *val);
+esp_err_t i2c_read_multi(i2c_master_dev_handle_t dev, uint8_t reg, uint8_t *buf, int len);
 
-esp_err_t i2c_read_reg_multi(uint8_t addr, uint8_t reg, uint8_t *buf,
+esp_err_t i2c_read_reg_multi(i2c_master_dev_handle_t dev, uint8_t reg, uint8_t *buf,
                              size_t len);

main/drivers/st7789.c

@@ -281,7 +281,7 @@ void st7789_draw_string(uint16_t xOld, uint16_t yOld, const char *s,
   uint16_t y = yOld;
   while (*s) {
     if (*s == '\n') {
-      y += 5;
+      y += 9;
       x = xOld;
       s++;
       continue;

main/drivers/tca8418.c

@@ -4,21 +4,28 @@
 #include <stdint.h>
 #include <string.h>
 
+i2c_master_dev_handle_t tca8418_i2c_dev;
+
 KeyboardState_t keyboardState;
 
 void tca8418_init(void) {
-  i2c_write_reg(KEYBOARD_I2C_ADDRESS, TCA8418_REG_KP_GPIO1, 0x7F);
+  i2c_device_config_t dev_cfg = {
-  i2c_write_reg(KEYBOARD_I2C_ADDRESS, TCA8418_REG_KP_GPIO2, 0xFF);
+      .device_address = KEYBOARD_I2C_ADDRESS,
-  i2c_write_reg(KEYBOARD_I2C_ADDRESS, TCA8418_REG_KP_GPIO3, 0x00);
+      .scl_speed_hz = 400000,
+  };
+  i2c_master_bus_add_device(bus, &dev_cfg, &tca8418_i2c_dev);
+  i2c_write_reg(tca8418_i2c_dev, TCA8418_REG_KP_GPIO1, 0x7F);
+  i2c_write_reg(tca8418_i2c_dev, TCA8418_REG_KP_GPIO2, 0xFF);
+  i2c_write_reg(tca8418_i2c_dev, TCA8418_REG_KP_GPIO3, 0x00);
 
-  i2c_write_reg(KEYBOARD_I2C_ADDRESS, TCA8418_REG_CFG,
+  i2c_write_reg(tca8418_i2c_dev, TCA8418_REG_CFG,
                 TCA8418_CFG_INT_ENABLE | TCA8418_CFG_OVERFLOW_MODE);
 
-  i2c_write_reg(KEYBOARD_I2C_ADDRESS, TCA8418_REG_UNLOCK1, 0x00);
+  i2c_write_reg(tca8418_i2c_dev, TCA8418_REG_UNLOCK1, 0x00);
-  i2c_write_reg(KEYBOARD_I2C_ADDRESS, TCA8418_REG_UNLOCK2, 0x00);
+  i2c_write_reg(tca8418_i2c_dev, TCA8418_REG_UNLOCK2, 0x00);
-  i2c_write_reg(KEYBOARD_I2C_ADDRESS, TCA8418_REG_KP_LCK_TIMER, 0x00);
+  i2c_write_reg(tca8418_i2c_dev, TCA8418_REG_KP_LCK_TIMER, 0x00);
 
-  i2c_write_reg(KEYBOARD_I2C_ADDRESS, TCA8418_REG_INT_STAT, 0xFF);
+  i2c_write_reg(tca8418_i2c_dev, TCA8418_REG_INT_STAT, 0xFF);
   memset(&keyboardState, 0, sizeof(keyboardState));
 }
 
@@ -26,7 +33,7 @@ int tca8418_read(uint8_t *key, uint8_t *pressed) {
   uint8_t count;
 
   // read event counter
-  i2c_read_reg(KEYBOARD_I2C_ADDRESS, TCA8418_REG_KEY_LCK_EC, &count);
+  i2c_read_reg(tca8418_i2c_dev, TCA8418_REG_KEY_LCK_EC, &count);
 
   count &= 0x0F;
 
@@ -37,7 +44,7 @@ int tca8418_read(uint8_t *key, uint8_t *pressed) {
   uint8_t ev;
 
   // reading this register POPS one FIFO event
-  i2c_read_reg(KEYBOARD_I2C_ADDRESS, TCA8418_REG_KEY_EVENT_A, &ev);
+  i2c_read_reg(tca8418_i2c_dev, TCA8418_REG_KEY_EVENT_A, &ev);
 
   // bit7:
   // 0 = press
@@ -52,8 +59,7 @@ int tca8418_read(uint8_t *key, uint8_t *pressed) {
     return 0;
 
   // clear ONLY key event interrupt
-  i2c_write_reg(KEYBOARD_I2C_ADDRESS, TCA8418_REG_INT_STAT,
+  i2c_write_reg(tca8418_i2c_dev, TCA8418_REG_INT_STAT, TCA8418_INT_KEY_EVENT);
-                TCA8418_INT_KEY_EVENT);
 
   // update key state table
   keyboardState.pressedKeys[*key] = *pressed;

main/main.c

@@ -12,9 +12,9 @@
 
 #include "drivers/bmi270.h"
 #include "drivers/es8311.h"
-#include "drivers/sdcard.h"
-#include "drivers/irled.h"
 #include "drivers/i2s.h"
+#include "drivers/irled.h"
+#include "drivers/sdcard.h"
 #include "drivers/st7789.h"
 #include "drivers/tca8418.h"
 #include "drivers/ws2812.h"
@@ -28,11 +28,14 @@ printf("boot\n");
   i2c_init(BUS_I2C_SDA, BUS_I2C_SCL);
 
   ws2812_init();
-  bmi270_init();
   es8311_init();
   tca8418_init();
+  if (!bmi270_init()) {
+    printf("BMI Init failed\n");
+  }
   ir_pwm_init();
   ir_carrier_off();
+
   initSD();
   st7789_init();
   printf("init done");
@@ -48,7 +51,6 @@ printf("boot\n");
 
 
   */
-  audio_beep();
 
   while (1) {
 
@@ -56,9 +58,10 @@ printf("boot\n");
     uint8_t key;
     uint8_t pressed;
 
-    char buf[64];
+    char buf[256];
     st7789_fill_rect(1, 1, LCD_WIDTH - 2, LCD_HEIGHT - 2, 0x0000);
-    st7789_draw_string(10, 10, "CARDPUTER TEST", 0xFFFF, 0x0000, false, font5x7);
+    st7789_draw_string(10, 10, "CARDPUTER TEST", 0xFFFF, 0x0000, false,
+                       font5x7);
     while (tca8418_read(&key, &pressed)) {
 
       snprintf(buf, sizeof(buf), "key=%c %s %d %s", getKeyboardChar(key),
@@ -66,20 +69,23 @@ printf("boot\n");
       printf("%s\n", buf);
 
       st7789_draw_string(30, 30, buf, 0x07E0, 0x0000, true, fontHitachi);
+      audio_beep();
     }
+
     bmi270_data_t bmiData;
 
-    bmi270_read(&bmiData);
+    if (!bmi270_read(&bmiData)) {
-
+      printf("BMI read failed\n");
-    snprintf(buf, sizeof(buf), "acc %d %d %d gyro %d %d %d\n",
+    } else {
-        bmiData.ax, bmiData.ay, bmiData.az,
-        bmiData.gx, bmiData.gy, bmiData.gz);
 
+      snprintf(buf, sizeof(buf), "acc %f %f %f\ngyro %f %f %f\n", bmiData.ax,
+               bmiData.ay, bmiData.az, bmiData.gx, bmiData.gy, bmiData.gz);
       st7789_draw_string(30, 60, buf, 0x07E0, 0x0000, true, fontHitachi);
+    }
 
-
+    snprintf(buf, sizeof(buf), "BAT: %d%%(%.3fV)", getBatteryPercentage(),
-    snprintf(buf, sizeof(buf), "Battery: %d%%(%.3fV)", getBatteryPercentage(), getBatteryVoltage());
+             getBatteryVoltage());
-    st7789_draw_string(100, 10, buf, 0x07E0, 0x0000, true, fontHitachi);
+    st7789_draw_string(150, 10, buf, 0x07E0, 0x0000, true, fontHitachi);
     st7789_flush();
 
     vTaskDelay(pdMS_TO_TICKS(20)); // 50 fps max