From b123caba0685ed5540518616a591cd4d36f1cb0a Mon Sep 17 00:00:00 2001 From: rotorman Date: Sat, 21 Aug 2021 15:08:50 +0200 Subject: [PATCH] Adds support of LSM6DS33 I2C IMU to TX16S via AUX1 port. Using CMake option -DIMU_LSM6DS33=YES the std. serial USART3/AUX1 functionality is changed to I2C2 bus. The temperature, gyro and linear acceleration values of LSM6DS33 are sampled and displayed at radio analogs dialog screen in SI-units. --- radio/src/CMakeLists.txt | 4 + radio/src/gui/colorlcd/radio_diaganas.cpp | 18 ++ radio/src/options.h | 3 + .../common/arm/stm32/bootloader/boot.cpp | 4 +- radio/src/targets/horus/CMakeLists.txt | 8 + radio/src/targets/horus/board.cpp | 16 +- radio/src/targets/horus/board.h | 4 + radio/src/targets/horus/hal.h | 58 ++-- radio/src/targets/horus/i2c_driver.cpp | 75 +++-- radio/src/targets/horus/imu_lsm6ds33.cpp | 263 ++++++++++++++++++ radio/src/targets/horus/imu_lsm6ds33.h | 139 +++++++++ radio/src/targets/horus/tp_gt911.cpp | 28 +- 12 files changed, 552 insertions(+), 68 deletions(-) create mode 100644 radio/src/targets/horus/imu_lsm6ds33.cpp create mode 100644 radio/src/targets/horus/imu_lsm6ds33.h diff --git a/radio/src/CMakeLists.txt b/radio/src/CMakeLists.txt index 4b7a7457d..4c602f75e 100644 --- a/radio/src/CMakeLists.txt +++ b/radio/src/CMakeLists.txt @@ -349,6 +349,10 @@ if(FLYSKY_HALL_STICKS_REVERSE) add_definitions(-DFLYSKY_HALL_STICKS_REVERSE) endif() +if(IMU_LSM6DS33) + add_definitions(-DIMU_LSM6DS33) +endif() + if(EEPROM_VARIANT_NEEDED) add_definitions(-DEEPROM_VARIANT=${EEPROM_VARIANT}) endif() diff --git a/radio/src/gui/colorlcd/radio_diaganas.cpp b/radio/src/gui/colorlcd/radio_diaganas.cpp index 9e3547fe6..3f6c38a98 100644 --- a/radio/src/gui/colorlcd/radio_diaganas.cpp +++ b/radio/src/gui/colorlcd/radio_diaganas.cpp @@ -22,6 +22,10 @@ #include "radio_diaganas.h" #include "libopenui.h" +#if defined(IMU_LSM6DS33) +#include "imu_lsm6ds33.h" +#endif + constexpr coord_t ANA_OFFSET = 150; class RadioAnalogsDiagsWindow: public Window { @@ -53,6 +57,20 @@ class RadioAnalogsDiagsWindow: public Window { dc->drawNumber(x + ANA_OFFSET, y, (int16_t) calibratedAnalogs[CONVERT_MODE(i)] * 25 / 256, RIGHT); } +#if !defined(SIMU) && defined(IMU_LSM6DS33) + coord_t yimu = MENU_CONTENT_TOP + 3 * FH; + coord_t ximu = MENUS_MARGIN_LEFT; + char imudata[80]; + sprintf(imudata, "IMU temp.: %.2f deg.C, Gyro XYZ [rad/s]: %.2f, %.2f, %.2f", + IMUoutput.fTemperatureDegC, + IMUoutput.fGyroXradps, IMUoutput.fGyroYradps, IMUoutput.fGyroZradps); + dc->drawText(ximu, yimu, imudata); + yimu = MENU_CONTENT_TOP + 4 * FH; + sprintf(imudata, "Linear acceleration XYZ [m/s^2]: %.2f %.2f %.2f", + IMUoutput.fAccX, IMUoutput.fAccY, IMUoutput.fAccZ); + dc->drawText(ximu, yimu, imudata); +#endif + #if defined(HARDWARE_TOUCH) constexpr coord_t y = MENU_CONTENT_TOP + 5 * FH; diff --git a/radio/src/options.h b/radio/src/options.h index 9918baa60..1e56afb69 100644 --- a/radio/src/options.h +++ b/radio/src/options.h @@ -101,6 +101,9 @@ static const char * const options[] = { #if defined(FLYSKY_HALL_STICKS) "flyskyhallsticks", #endif +#if defined(IMU_LSM6DS33) + "lsm6ds33", +#endif #if defined(BIND_KEY) "bindkey", #endif diff --git a/radio/src/targets/common/arm/stm32/bootloader/boot.cpp b/radio/src/targets/common/arm/stm32/bootloader/boot.cpp index fd7c36a6d..1a5599473 100644 --- a/radio/src/targets/common/arm/stm32/bootloader/boot.cpp +++ b/radio/src/targets/common/arm/stm32/bootloader/boot.cpp @@ -206,11 +206,11 @@ int main() RCC_AHB1PeriphClockCmd(PWR_RCC_AHB1Periph | KEYS_RCC_AHB1Periph | LCD_RCC_AHB1Periph | BACKLIGHT_RCC_AHB1Periph | AUX_SERIAL_RCC_AHB1Periph | AUX2_SERIAL_RCC_AHB1Periph | - I2C_RCC_AHB1Periph | KEYS_BACKLIGHT_RCC_AHB1Periph | + I2C_TP_RCC_AHB1Periph | KEYS_BACKLIGHT_RCC_AHB1Periph | SD_RCC_AHB1Periph, ENABLE); RCC_APB1PeriphClockCmd(ROTARY_ENCODER_RCC_APB1Periph | LCD_RCC_APB1Periph | BACKLIGHT_RCC_APB1Periph | - INTERRUPT_xMS_RCC_APB1Periph | I2C_RCC_APB1Periph | + INTERRUPT_xMS_RCC_APB1Periph | I2C_TP_RCC_APB1Periph | AUX_SERIAL_RCC_APB1Periph | AUX2_SERIAL_RCC_APB1Periph | SD_RCC_APB1Periph, ENABLE); diff --git a/radio/src/targets/horus/CMakeLists.txt b/radio/src/targets/horus/CMakeLists.txt index 591b37270..bce7c1276 100644 --- a/radio/src/targets/horus/CMakeLists.txt +++ b/radio/src/targets/horus/CMakeLists.txt @@ -1,6 +1,7 @@ option(DISK_CACHE "Enable SD card disk cache" ON) option(UNEXPECTED_SHUTDOWN "Enable the Unexpected Shutdown screen" ON) option(FLYSKY_HALL_STICKS "Enable FlySky Hall Sticks" OFF) +option(IMU_LSM6DS33 "Enable I2C2 and LSM6DS33 IMU" OFF) option(PXX1 "PXX1 protocol support" ON) option(PXX2 "PXX2 protocol support" OFF) option(AFHDS3 "AFHDS3 TX Module" ON) @@ -189,6 +190,12 @@ if(FLYSKY_HALL_STICKS) set(FLYSKY_HALL_STICKS_DRIVER flyskyHallStick_driver.cpp) endif() +if(IMU_LSM6DS33) + add_definitions(-DIMU_LSM6DS33) + set(AUX_SERIAL OFF) + set(IMU_LSM6DS33_DRIVER imu_lsm6ds33.cpp) +endif() + if(HARDWARE_EXTERNAL_MODULE) add_definitions(-DHARDWARE_EXTERNAL_MODULE) endif() @@ -278,6 +285,7 @@ set(FIRMWARE_TARGET_SRC ${LCD_DRIVER} ${AUX_SERIAL_DRIVER} ${FLYSKY_HALL_STICKS_DRIVER} + ${IMU_LSM6DS33_DRIVER} ${TOUCH_DRIVER} board.cpp backlight_driver.cpp diff --git a/radio/src/targets/horus/board.cpp b/radio/src/targets/horus/board.cpp index 4faa08b93..505193418 100644 --- a/radio/src/targets/horus/board.cpp +++ b/radio/src/targets/horus/board.cpp @@ -123,7 +123,11 @@ void boardInit() #if defined(FLYSKY_HALL_STICKS) FLYSKY_HALL_RCC_AHB1Periph | #endif +#if defined(IMU_LSM6DS33) + I2C_IMU_RCC_AHB1Periph | +#else AUX_SERIAL_RCC_AHB1Periph | +#endif AUX2_SERIAL_RCC_AHB1Periph | TELEMETRY_RCC_AHB1Periph | TRAINER_RCC_AHB1Periph | @@ -132,7 +136,7 @@ void boardInit() HAPTIC_RCC_AHB1Periph | INTMODULE_RCC_AHB1Periph | EXTMODULE_RCC_AHB1Periph | - I2C_RCC_AHB1Periph | + I2C_TP_RCC_AHB1Periph | GPS_RCC_AHB1Periph | SPORT_UPDATE_RCC_AHB1Periph | TOUCH_INT_RCC_AHB1Periph | @@ -147,14 +151,18 @@ void boardInit() #if defined(FLYSKY_HALL_STICKS) FLYSKY_HALL_RCC_APB1Periph | #endif +#if defined(IMU_LSM6DS33) + I2C_IMU_RCC_APB1Periph | +#else AUX_SERIAL_RCC_APB1Periph | +#endif AUX2_SERIAL_RCC_APB1Periph | TELEMETRY_RCC_APB1Periph | TRAINER_RCC_APB1Periph | AUDIO_RCC_APB1Periph | INTMODULE_RCC_APB1Periph | EXTMODULE_RCC_APB1Periph | - I2C_RCC_APB1Periph | + I2C_TP_RCC_APB1Periph | MIXER_SCHEDULER_TIMER_RCC_APB1Periph | GPS_RCC_APB1Periph | BACKLIGHT_RCC_APB1Periph, @@ -217,6 +225,10 @@ void boardInit() flysky_hall_stick_init(); #endif +#if defined(IMU_LSM6DS33) + imu_lsm6ds33_init(); +#endif + init2MhzTimer(); init1msTimer(); usbInit(); diff --git a/radio/src/targets/horus/board.h b/radio/src/targets/horus/board.h index 504a4d216..641d60acf 100644 --- a/radio/src/targets/horus/board.h +++ b/radio/src/targets/horus/board.h @@ -30,6 +30,10 @@ #include "tp_gt911.h" #endif +#if defined(IMU_LSM6DS33) +#include "imu_lsm6ds33.h" +#endif + PACK(typedef struct { uint8_t pcbrev:2; uint8_t sticksPwmDisabled:1; diff --git a/radio/src/targets/horus/hal.h b/radio/src/targets/horus/hal.h index 4532ef8d7..b5362fc79 100644 --- a/radio/src/targets/horus/hal.h +++ b/radio/src/targets/horus/hal.h @@ -408,7 +408,7 @@ #endif // Serial Port (DEBUG) -#if (defined(PCBX12S) || (defined(RADIO_TX16S)) && !defined(HARDWARE_EXTERNAL_ACCESS_MOD)) +#if (defined(PCBX12S) || (defined(RADIO_TX16S)) && !defined(HARDWARE_EXTERNAL_ACCESS_MOD)) && !defined(IMU_LSM6DS33) #define AUX_SERIAL_RCC_AHB1Periph (RCC_AHB1Periph_GPIOA | RCC_AHB1Periph_GPIOB | RCC_AHB1Periph_DMA1) #define AUX_SERIAL_RCC_APB1Periph RCC_APB1Periph_USART3 #define AUX_SERIAL_RCC_APB2Periph 0 @@ -705,29 +705,45 @@ #define TOUCH_RST_RCC_AHB1Periph 0 #endif -// I2C Bus +// I2C Bus for touch #if defined(RADIO_T18) - #define I2C_RCC_AHB1Periph RCC_AHB1Periph_GPIOH - #define I2C_RCC_APB1Periph RCC_APB1Periph_I2C3 - #define I2C I2C3 - #define I2C_GPIO GPIOH - #define I2C_SCL_GPIO_PIN GPIO_Pin_7 // PH.07 - #define I2C_SDA_GPIO_PIN GPIO_Pin_8 // PH.08 - #define I2C_GPIO_AF GPIO_AF_I2C3 - #define I2C_SCL_GPIO_PinSource GPIO_PinSource7 - #define I2C_SDA_GPIO_PinSource GPIO_PinSource8 + #define I2C_TP_RCC_AHB1Periph RCC_AHB1Periph_GPIOH + #define I2C_TP_RCC_APB1Periph RCC_APB1Periph_I2C3 + #define I2C_TP I2C3 + #define I2C_TP_GPIO GPIOH + #define I2C_TP_SCL_GPIO_PIN GPIO_Pin_7 // PH.07 + #define I2C_TP_SDA_GPIO_PIN GPIO_Pin_8 // PH.08 + #define I2C_TP_GPIO_AF GPIO_AF_I2C3 + #define I2C_TP_SCL_GPIO_PinSource GPIO_PinSource7 + #define I2C_TP_SDA_GPIO_PinSource GPIO_PinSource8 #else - #define I2C_RCC_AHB1Periph RCC_AHB1Periph_GPIOB - #define I2C_RCC_APB1Periph RCC_APB1Periph_I2C1 - #define I2C I2C1 - #define I2C_GPIO GPIOB - #define I2C_SCL_GPIO_PIN GPIO_Pin_8 // PB.08 - #define I2C_SDA_GPIO_PIN GPIO_Pin_9 // PB.09 - #define I2C_GPIO_AF GPIO_AF_I2C1 - #define I2C_SCL_GPIO_PinSource GPIO_PinSource8 - #define I2C_SDA_GPIO_PinSource GPIO_PinSource9 + #define I2C_TP_RCC_AHB1Periph RCC_AHB1Periph_GPIOB + #define I2C_TP_RCC_APB1Periph RCC_APB1Periph_I2C1 + #define I2C_TP I2C1 + #define I2C_TP_GPIO GPIOB + #define I2C_TP_SCL_GPIO_PIN GPIO_Pin_8 // PB.08 + #define I2C_TP_SDA_GPIO_PIN GPIO_Pin_9 // PB.09 + #define I2C_TP_GPIO_AF GPIO_AF_I2C1 + #define I2C_TP_SCL_GPIO_PinSource GPIO_PinSource8 + #define I2C_TP_SDA_GPIO_PinSource GPIO_PinSource9 +#endif +#define I2C_TP_CLK_RATE 400000 + +// I2C Bus for IMU +#if defined(RADIO_TX16S) && defined(IMU_LSM6DS33) + #define I2C_IMU_RCC_AHB1Periph RCC_AHB1Periph_GPIOB + #define I2C_IMU_RCC_APB1Periph RCC_APB1Periph_I2C2 + #define I2C_IMU I2C2 + #define I2C_IMU_GPIO GPIOB + #define I2C_IMU_SCL_GPIO_PIN GPIO_Pin_10 // PB.10 + #define I2C_IMU_SDA_GPIO_PIN GPIO_Pin_11 // PB.11 + #define I2C_IMU_GPIO_AF GPIO_AF_I2C2 + #define I2C_IMU_SCL_GPIO_PinSource GPIO_PinSource10 + #define I2C_IMU_SDA_GPIO_PinSource GPIO_PinSource11 + #define I2C_IMU_CLK_RATE 100000 + #define AUX_IMU_PWR_GPIO GPIOA + #define AUX_IMU_PWR_GPIO_PIN GPIO_Pin_15 // PA.15 #endif -#define I2C_CLK_RATE 400000 // Haptic #define HAPTIC_PWM diff --git a/radio/src/targets/horus/i2c_driver.cpp b/radio/src/targets/horus/i2c_driver.cpp index c5c109df3..56e979073 100644 --- a/radio/src/targets/horus/i2c_driver.cpp +++ b/radio/src/targets/horus/i2c_driver.cpp @@ -29,39 +29,56 @@ * @param hi2c: I2C handle pointer * @retval None */ -void HAL_I2C_MspInit(I2C_HandleTypeDef* hi2c) +void I2C_MspInit(I2C_HandleTypeDef* hi2c) { GPIO_InitTypeDef GPIO_InitStruct = {0}; - if (I2C_GPIO == GPIOA) - __HAL_RCC_GPIOA_CLK_ENABLE(); - else if (I2C_GPIO == GPIOB) - __HAL_RCC_GPIOB_CLK_ENABLE(); - else if (I2C_GPIO == GPIOC) - __HAL_RCC_GPIOC_CLK_ENABLE(); - else if (I2C_GPIO == GPIOH) - __HAL_RCC_GPIOH_CLK_ENABLE(); - else - TRACE("I2C ERROR: HAL_I2C_MspInit() I2C_GPIO misconfiguration"); - GPIO_PinAFConfig(I2C_GPIO, I2C_SCL_GPIO_PinSource, I2C_GPIO_AF); - GPIO_PinAFConfig(I2C_GPIO, I2C_SDA_GPIO_PinSource, I2C_GPIO_AF); + if (I2C_TP_GPIO == GPIOA) + __HAL_RCC_GPIOA_CLK_ENABLE(); + else if (I2C_TP_GPIO == GPIOB) + __HAL_RCC_GPIOB_CLK_ENABLE(); + else if (I2C_TP_GPIO == GPIOC) + __HAL_RCC_GPIOC_CLK_ENABLE(); + else if (I2C_TP_GPIO == GPIOH) + __HAL_RCC_GPIOH_CLK_ENABLE(); + else + TRACE("I2C ERROR: HAL_I2C_MspInit() I2C_GPIO misconfiguration"); - GPIO_InitStruct.GPIO_Pin = I2C_SCL_GPIO_PIN | I2C_SDA_GPIO_PIN; + if ((hi2c->Instance==I2C1) || (hi2c->Instance==I2C3)) // TP + { + GPIO_PinAFConfig(I2C_TP_GPIO, I2C_TP_SCL_GPIO_PinSource, I2C_TP_GPIO_AF); + GPIO_PinAFConfig(I2C_TP_GPIO, I2C_TP_SDA_GPIO_PinSource, I2C_TP_GPIO_AF); + + GPIO_InitStruct.GPIO_Pin = I2C_TP_SCL_GPIO_PIN | I2C_TP_SDA_GPIO_PIN; GPIO_InitStruct.GPIO_Speed = GPIO_Speed_2MHz; GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF; GPIO_InitStruct.GPIO_OType = GPIO_OType_OD; GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_NOPULL; - GPIO_Init(I2C_GPIO, &GPIO_InitStruct); + GPIO_Init(I2C_TP_GPIO, &GPIO_InitStruct); + } - /* Peripheral clock enable */ - if (I2C == I2C1) - __HAL_RCC_I2C1_CLK_ENABLE(); - else if (I2C == I2C2) - __HAL_RCC_I2C2_CLK_ENABLE(); - else if (I2C == I2C3) - __HAL_RCC_I2C3_CLK_ENABLE(); - else - TRACE("I2C ERROR: HAL_I2C_MspInit() I2C misconfiguration"); + if (hi2c->Instance==I2C2) // IMU + { + GPIO_PinAFConfig(I2C_IMU_GPIO, I2C_IMU_SCL_GPIO_PinSource, I2C_IMU_GPIO_AF); + GPIO_PinAFConfig(I2C_IMU_GPIO, I2C_IMU_SDA_GPIO_PinSource, I2C_IMU_GPIO_AF); + + GPIO_InitStruct.GPIO_Pin = I2C_IMU_SCL_GPIO_PIN | I2C_IMU_SDA_GPIO_PIN; + GPIO_InitStruct.GPIO_Speed = GPIO_Speed_2MHz; + GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF; + GPIO_InitStruct.GPIO_OType = GPIO_OType_OD; + GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_NOPULL; + GPIO_Init(I2C_IMU_GPIO, &GPIO_InitStruct); + } + + /* Peripheral clock enable */ + if (I2C_TP == I2C1) + __HAL_RCC_I2C1_CLK_ENABLE(); + else if (I2C_TP == I2C2) + __HAL_RCC_I2C2_CLK_ENABLE(); + else if (I2C_TP == I2C3) + __HAL_RCC_I2C3_CLK_ENABLE(); + else + TRACE("I2C ERROR: HAL_I2C_MspInit() I2C misconfiguration"); } /* De-initializes the GPIOx peripheral registers to their default reset values. @@ -154,15 +171,15 @@ void HAL_I2C_MspDeInit(I2C_HandleTypeDef* hi2c) GPIO_InitTypeDef GPIO_InitStructure; /* Configure the default Alternate Function in current IO */ - GPIO_PinAFConfig(I2C_GPIO, I2C_SCL_GPIO_PinSource, 0); - GPIO_PinAFConfig(I2C_GPIO, I2C_SDA_GPIO_PinSource, 0); + GPIO_PinAFConfig(I2C_TP_GPIO, I2C_TP_SCL_GPIO_PinSource, 0); + GPIO_PinAFConfig(I2C_TP_GPIO, I2C_TP_SDA_GPIO_PinSource, 0); - GPIO_InitStructure.GPIO_Pin = I2C_SCL_GPIO_PIN | I2C_SDA_GPIO_PIN; + GPIO_InitStructure.GPIO_Pin = I2C_TP_SCL_GPIO_PIN | I2C_TP_SDA_GPIO_PIN; GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz; /* Configure a low value for IO Speed */ GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN; /* Configure IO Direction in Input Floating Mode */ GPIO_InitStructure.GPIO_OType = GPIO_OType_OD; /* Leave the configuration to Open Drain */ GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; /* Deactivate the Pull-up and Pull-down resistor for the current IO */ - GPIO_Init(I2C_GPIO, &GPIO_InitStructure); + GPIO_Init(I2C_TP_GPIO, &GPIO_InitStructure); } /* Initializes the I2C according to the specified parameters @@ -199,7 +216,7 @@ HAL_StatusTypeDef HAL_I2C_Init(I2C_HandleTypeDef *hi2c) hi2c->Lock = HAL_UNLOCKED; /* Init the low level hardware : GPIO, CLOCK, NVIC */ - HAL_I2C_MspInit(hi2c); + I2C_MspInit(hi2c); } hi2c->State = HAL_I2C_STATE_BUSY; diff --git a/radio/src/targets/horus/imu_lsm6ds33.cpp b/radio/src/targets/horus/imu_lsm6ds33.cpp new file mode 100644 index 000000000..41b7d80e5 --- /dev/null +++ b/radio/src/targets/horus/imu_lsm6ds33.cpp @@ -0,0 +1,263 @@ +/* + * Copyright (C) EdgeTX + * + * Based on code named + * opentx - https://github.com/opentx/opentx + * th9x - http://code.google.com/p/th9x + * er9x - http://code.google.com/p/er9x + * gruvin9x - http://code.google.com/p/gruvin9x + * + * License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + */ + +#include "opentx.h" +#include "i2c_driver.h" +#include "imu_lsm6ds33.h" + +I2C_HandleTypeDef hi2c2; + +sIMUsettings IMUsettings = {-1, -1.0, -1, -1, -1.0}; +sIMUoutput IMUoutput = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + +float fIMUtemp = 0.0; + +bool I2C_LSM6DS33_ReadRegister(uint8_t reg, uint8_t * buf, uint8_t len) +{ + if (HAL_I2C_Master_Transmit(&hi2c2, LSM6DS33_I2C_ADDR << 1, ®, 1, 10000) != HAL_OK) + { + TRACE("I2C IMU ERROR: ReadRegister write reg. address failed"); + return false; + } + + if (HAL_I2C_Master_Receive(&hi2c2, LSM6DS33_I2C_ADDR << 1, buf, len, 10000) != HAL_OK) + { + TRACE("I2C IMU ERROR: ReadRegister read register failed"); + return false; + } + return true; +} + +bool I2C_LSM6DS33_WriteRegister(uint8_t reg, uint8_t * buf, uint8_t len) +{ + uint8_t uAddrAndBuf[15]; + uAddrAndBuf[0] = reg; + + if (len > 0) + { + for (int i = 0;i < len;i++) + { + uAddrAndBuf[i + 1] = buf[i]; + } + } + + if (HAL_I2C_Master_Transmit(&hi2c2, LSM6DS33_I2C_ADDR << 1, uAddrAndBuf, len + 1, 10000) != HAL_OK) + { + TRACE("I2C IMU ERROR: WriteRegister failed"); + return false; + } + return true; +} + +bool IMUgetConf(void) +{ + uint8_t ui8_regs[2] = {0}; + + if (!I2C_LSM6DS33_ReadRegister(LSM6DS33_CTRL1_XL_ADDR, ui8_regs, 2)) + { + TRACE("ERROR: LSM6DS33 did not respond to I2C address"); + return false; + } + + switch ((ui8_regs[0] >> 4) & 0x0F) // CTRL1_XL + { + case LSM6DS33_ODR_0Hz: IMUsettings.linacc_odr = 0; TRACE("LSM6DS33 lin. acc. output turned off"); break; + case LSM6DS33_ODR_12_5Hz: IMUsettings.linacc_odr = 13; break; + case LSM6DS33_ODR_26Hz: IMUsettings.linacc_odr = 26; break; + case LSM6DS33_ODR_52Hz: IMUsettings.linacc_odr = 52; break; + case LSM6DS33_ODR_104Hz: IMUsettings.linacc_odr = 104; break; + case LSM6DS33_ODR_208Hz: IMUsettings.linacc_odr = 208; break; + case LSM6DS33_ODR_416Hz: IMUsettings.linacc_odr = 417; break; + case LSM6DS33_ODR_833Hz: IMUsettings.linacc_odr = 833; break; + case LSM6DS33_ODR_1_66kHz: IMUsettings.linacc_odr = 1667; break; + case LSM6DS33_ODR_3_33kHz: IMUsettings.linacc_odr = 3333; break; + case LSM6DS33_ODR_6_66kHz: IMUsettings.linacc_odr = 6667; break; + default: TRACE("ERROR: LSM6DS33 lin.acc. output data rate at illegal setting"); return false; break; + } + if (IMUsettings.linacc_odr > 0) TRACE("LSM6DS33 lin. acc. output at %i Hz", IMUsettings.linacc_odr); + + switch ((ui8_regs[0] >> 2) & 0x03) + { + case LSM6DS33_LA_FS_2G: IMUsettings.linacc_mG = 0.061; break; + case LSM6DS33_LA_FS_4G: IMUsettings.linacc_mG = 0.122; break; + case LSM6DS33_LA_FS_8G: IMUsettings.linacc_mG = 0.244; break; + case LSM6DS33_LA_FS_16G: IMUsettings.linacc_mG = 0.488; break; + default: TRACE("ERROR: LSM6DS33 lin. acc full scale value at illegal setting"); return false; break; + } + TRACE("LSM6DS33 lin. acc. resolution %.2f G", IMUsettings.linacc_mG); + + switch ((ui8_regs[0] >> 2) & 0x03) + { + case LSM6DS33_LA_AABW_400Hz: IMUsettings.linacc_aabw = 400; break; + case LSM6DS33_LA_AABW_200Hz: IMUsettings.linacc_aabw = 200; break; + case LSM6DS33_LA_AABW_100Hz: IMUsettings.linacc_aabw = 100; break; + case LSM6DS33_LA_AABW_50Hz: IMUsettings.linacc_aabw = 50; break; + default: TRACE("ERROR: LSM6DS33 lin. acc anti aliasing bandwidth at illegal setting"); return false; break; + } + TRACE("LSM6DS33 lin. acc. anti aliasing bandwidth at %i Hz", IMUsettings.linacc_aabw); + + switch ((ui8_regs[1] >> 4) & 0x0F) // CTRL2_G + { + case LSM6DS33_ODR_0Hz: IMUsettings.gyro_odr = 0; TRACE("LSM6DS33 gyro output turned off"); break; + case LSM6DS33_ODR_12_5Hz: IMUsettings.gyro_odr = 13; break; + case LSM6DS33_ODR_26Hz: IMUsettings.gyro_odr = 26; break; + case LSM6DS33_ODR_52Hz: IMUsettings.gyro_odr = 52; break; + case LSM6DS33_ODR_104Hz: IMUsettings.gyro_odr = 104; break; + case LSM6DS33_ODR_208Hz: IMUsettings.gyro_odr = 208; break; + case LSM6DS33_ODR_416Hz: IMUsettings.gyro_odr = 417; break; + case LSM6DS33_ODR_833Hz: IMUsettings.gyro_odr = 833; break; + case LSM6DS33_ODR_1_66kHz: IMUsettings.gyro_odr = 1667; break; + default: TRACE("ERROR: LSM6DS33 gyro output data rate at illegal setting"); return false; break; + } + if (IMUsettings.gyro_odr > 0) TRACE("LSM6DS33 gyro output at %i Hz", IMUsettings.gyro_odr); + + switch ((ui8_regs[1] >> 1) & 0x07) + { + case LSM6DS33_GY_FS_125dps: IMUsettings.gyro_mDPS = 4.375; break; + case LSM6DS33_GY_FS_250dps: IMUsettings.gyro_mDPS = 8.75; break; + case LSM6DS33_GY_FS_500dps: IMUsettings.gyro_mDPS = 17.5; break; + case LSM6DS33_GY_FS_1000dps: IMUsettings.gyro_mDPS = 35.0; break; + case LSM6DS33_GY_FS_2000dps: IMUsettings.gyro_mDPS = 70.0; break; + default: TRACE("ERROR: LSM6DS33 gyro full scale value at illegal setting"); return false; break; + } + TRACE("LSM6DS33 raw gyro resolution: %.2f milli-degrees/sec.", IMUsettings.gyro_mDPS); + return true; +} + +void init_imu_i2c(void) +{ + TRACE("I2C IMU Init"); + + hi2c2.Instance = I2C_IMU; + hi2c2.Init.ClockSpeed = I2C_IMU_CLK_RATE; + hi2c2.Init.DutyCycle = I2C_DUTYCYCLE_2; + hi2c2.Init.OwnAddress1 = 0; + hi2c2.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT; + hi2c2.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE; + hi2c2.Init.OwnAddress2 = 0; + hi2c2.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE; + hi2c2.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE; + if (HAL_I2C_Init(&hi2c2) != HAL_OK) + { + TRACE("I2C IMU ERROR: HAL_I2C_Init() failed"); + } + // Configure Analogue filter + if (HAL_I2CEx_ConfigAnalogFilter(&hi2c2, I2C_ANALOGFILTER_ENABLE) != HAL_OK) + { + TRACE("I2C IMU ERROR: HAL_I2CEx_ConfigAnalogFilter() failed"); + } + // Configure Digital filter + if (HAL_I2CEx_ConfigDigitalFilter(&hi2c2, 0) != HAL_OK) + { + TRACE("I2C IMU ERROR: HAL_I2CEx_ConfigDigitalFilter() failed"); + } +} + +bool imu_lsm6ds33_init(void) +{ + uint8_t ui8_reg[2] = {0}; + + GPIO_SetBits(AUX_IMU_PWR_GPIO, AUX_IMU_PWR_GPIO_PIN); // Turn on AUX1 power + + init_imu_i2c(); + + if (!I2C_LSM6DS33_ReadRegister(LSM6DS33_WHO_AM_I_ADDR, ui8_reg, 1)) + { + TRACE("ERROR: LSM6DS33 did not respond to I2C address"); + return false; + } + if (ui8_reg[0] != LSM6DS33_WHOAMI) + { + TRACE("LSM6DS33 ERROR: Product ID read failes"); + return false; + } + TRACE("LSM6DS33 detected"); + + // Set IMU configuration + ui8_reg[0] = (LSM6DS33_ODR_104Hz << 4) | (LSM6DS33_LA_FS_4G << 2) | LSM6DS33_LA_AABW_100Hz; // Linear acceleration configuration + ui8_reg[1] = (LSM6DS33_ODR_104Hz << 4) | (LSM6DS33_GY_FS_125dps < 1); // Gyro configuration + if (!I2C_LSM6DS33_WriteRegister(LSM6DS33_CTRL1_XL_ADDR, ui8_reg, 2)) + { + TRACE("ERROR: Failed to write LSM6DS33 configuration"); + return false; + } + + if (!IMUgetConf()) + { + TRACE("ERROR: Failed ro read LSM6DS33 configuration"); + return false; + } + return true; +} + +bool imu_lsm6ds33_read() +{ + uint8_t ui8_regs[14] = {0}; + + int16_t i16_rawTemperature = 0; + int16_t i16_rawGyroX = 0; + int16_t i16_rawGyroY = 0; + int16_t i16_rawGyroZ = 0; + int16_t i16_rawAccX = 0; + int16_t i16_rawAccY = 0; + int16_t i16_rawAccZ = 0; + + // Check first if new data is available + if (!I2C_LSM6DS33_ReadRegister(LSM6DS33_STATUS_REG_ADDR, ui8_regs, 1)) + { + TRACE("ERROR: LSM6DS33 did not respond to I2C address"); + return false; + } + if ((ui8_regs[0] & 0x07) != 0x07) + { + TRACE("ERROR: LSM6DS33 has no new data available yet"); + return false; + } + + if (!I2C_LSM6DS33_ReadRegister(LSM6DS33_OUT_TEMP_L_ADDR, ui8_regs, 14)) + { + TRACE("ERROR: LSM6DS33 did not respond to I2C address"); + return false; + } + + i16_rawTemperature = (ui8_regs[1] << 8) | ui8_regs[0]; + i16_rawGyroX = (ui8_regs[3] << 8) | ui8_regs[2]; + i16_rawGyroY = (ui8_regs[5] << 8) | ui8_regs[4]; + i16_rawGyroZ = (ui8_regs[7] << 8) | ui8_regs[6]; + i16_rawAccX = (ui8_regs[9] << 8) | ui8_regs[8]; + i16_rawAccY = (ui8_regs[11] << 8) | ui8_regs[10]; + i16_rawAccZ = (ui8_regs[13] << 8) | ui8_regs[12]; + + IMUoutput.fGyroXradps = i16_rawGyroX * IMUsettings.gyro_mDPS * M_PI/180.0 / 1000.0; + IMUoutput.fGyroYradps = i16_rawGyroY * IMUsettings.gyro_mDPS * M_PI/180.0 / 1000.0; + IMUoutput.fGyroZradps = i16_rawGyroZ * IMUsettings.gyro_mDPS * M_PI/180.0 / 1000.0; + + IMUoutput.fAccX = i16_rawAccX * IMUsettings.linacc_mG * GRAVITY_EARTH / 1000.0; + IMUoutput.fAccY = i16_rawAccY * IMUsettings.linacc_mG * GRAVITY_EARTH / 1000.0; + IMUoutput.fAccZ = i16_rawAccZ * IMUsettings.linacc_mG * GRAVITY_EARTH / 1000.0; + + IMUoutput.fTemperatureDegC = (float)i16_rawTemperature / 16.0 + 25.0; + /*TRACE("LSM6DS33: %.2f deg.C, GyroXYZ [rad/s]: %.2f, %.2f, %.2f, AccXYZ [m/s^2]: %.2f %.2f %.2f", + IMUoutput.fTemperatureDegC, + IMUoutput.fGyroXradps, IMUoutput.fGyroYradps, IMUoutput.fGyroZradps, + IMUoutput.fAccX, IMUoutput.fAccY, IMUoutput.fAccZ); */ + return true; +} diff --git a/radio/src/targets/horus/imu_lsm6ds33.h b/radio/src/targets/horus/imu_lsm6ds33.h new file mode 100644 index 000000000..16ec03f8b --- /dev/null +++ b/radio/src/targets/horus/imu_lsm6ds33.h @@ -0,0 +1,139 @@ +/* + * Copyright (C) EdgeTX + * + * Based on code named + * opentx - https://github.com/opentx/opentx + * th9x - http://code.google.com/p/th9x + * er9x - http://code.google.com/p/er9x + * gruvin9x - http://code.google.com/p/gruvin9x + * + * License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + */ + +#pragma once + +#include "i2c_driver.h" + +#define GRAVITY_EARTH 9.80665F // m/s^2 + +#define LSM6DS33_I2C_ADDR 0x6A +#define LSM6DS33_WHOAMI 0x69 +#define LSM6DS33_TIMEOUT 3 // 3ms + +// LSM6DS33 register map +#define LSM6DS33_FUNC_CFG_ACCESS_ADDR 0x01 +#define LSM6DS33_FIFO_CTRL1_ADDR 0x06 +#define LSM6DS33_FIFO_CTRL2_ADDR 0x07 +#define LSM6DS33_FIFO_CTRL3_ADDR 0x08 +#define LSM6DS33_FIFO_CTRL4_ADDR 0x09 +#define LSM6DS33_FIFO_CTRL5_ADDR 0x0A +#define LSM6DS33_ORIENT_CFG_G_ADDR 0x0B +#define LSM6DS33_INT1_CTRL_ADDR 0x0D +#define LSM6DS33_INT2_CTRL_ADDR 0x0E +#define LSM6DS33_WHO_AM_I_ADDR 0x0F +#define LSM6DS33_CTRL1_XL_ADDR 0x10 +#define LSM6DS33_CTRL2_G_ADDR 0x11 +#define LSM6DS33_CTRL3_C_ADDR 0x12 +#define LSM6DS33_CTRL4_C_ADDR 0x13 +#define LSM6DS33_CTRL5_C_ADDR 0x14 +#define LSM6DS33_CTRL6_C_ADDR 0x15 +#define LSM6DS33_CTRL7_G_ADDR 0x16 +#define LSM6DS33_CTRL8_XL_ADDR 0x17 +#define LSM6DS33_CTRL9_XL_ADDR 0x18 +#define LSM6DS33_CTRL10_C_ADDR 0x19 +#define LSM6DS33_WAKE_UP_SRC_ADDR 0x1B +#define LSM6DS33_TAP_SRC_ADDR 0x1C +#define LSM6DS33_D6D_SRC_ADDR 0x1D +#define LSM6DS33_STATUS_REG_ADDR 0x1E +#define LSM6DS33_OUT_TEMP_L_ADDR 0x20 +#define LSM6DS33_OUT_TEMP_H_ADDR 0x21 +#define LSM6DS33_OUTX_L_G_ADDR 0x22 +#define LSM6DS33_OUTX_H_G_ADDR 0x23 +#define LSM6DS33_OUTY_L_G_ADDR 0x24 +#define LSM6DS33_OUTY_H_G_ADDR 0x25 +#define LSM6DS33_OUTZ_L_G_ADDR 0x26 +#define LSM6DS33_OUTZ_H_G_ADDR 0x27 +#define LSM6DS33_OUTX_L_XL_ADDR 0x28 +#define LSM6DS33_OUTX_H_XL_ADDR 0x29 +#define LSM6DS33_OUTY_L_XL_ADDR 0x2A +#define LSM6DS33_OUTY_H_XL_ADDR 0x2B +#define LSM6DS33_OUTZ_L_XL_ADDR 0x2C +#define LSM6DS33_OUTZ_H_XL_ADDR 0x2D +#define LSM6DS33_FIFO_STATUS1_ADDR 0x3A +#define LSM6DS33_FIFO_STATUS2_ADDR 0x3B +#define LSM6DS33_FIFO_STATUS3_ADDR 0x3C +#define LSM6DS33_FIFO_STATUS4_ADDR 0x3D +#define LSM6DS33_FIFO_DATA_OUT_L_ADDR 0x3E +#define LSM6DS33_FIFO_DATA_OUT_H_ADDR 0x3F +#define LSM6DS33_TIMESTAMP0_REG_ADDR 0x40 +#define LSM6DS33_TIMESTAMP1_REG_ADDR 0x41 +#define LSM6DS33_TIMESTAMP2_REG_ADDR 0x42 +#define LSM6DS33_STEP_TIMESTAMP_L_ADDR 0x49 +#define LSM6DS33_STEP_TIMESTAMP_H_ADDR 0x4A +#define LSM6DS33_STEP_COUNTER_L_ADDR 0x4B +#define LSM6DS33_STEP_COUNTER_H_ADDR 0x4C +#define LSM6DS33_FUNC_SRC_ADDR 0x53 +#define LSM6DS33_TAP_CFG_ADDR 0x58 +#define LSM6DS33_TAP_THS_6D_ADDR 0x59 +#define LSM6DS33_INT_DUR2_ADDR 0x5A +#define LSM6DS33_WAKE_UP_THS_ADDR 0x5B +#define LSM6DS33_WAKE_UP_DUR_ADDR 0x5C +#define LSM6DS33_FREE_FALL_ADDR 0x5D +#define LSM6DS33_MD1_CFG_ADDR 0x5E +#define LSM6DS33_MD2_CFG_ADDR 0x5F + +#define LSM6DS33_ODR_0Hz 0x00 +#define LSM6DS33_ODR_12_5Hz 0x01 +#define LSM6DS33_ODR_26Hz 0x02 +#define LSM6DS33_ODR_52Hz 0x03 +#define LSM6DS33_ODR_104Hz 0x04 +#define LSM6DS33_ODR_208Hz 0x05 +#define LSM6DS33_ODR_416Hz 0x06 +#define LSM6DS33_ODR_833Hz 0x07 +#define LSM6DS33_ODR_1_66kHz 0x08 +#define LSM6DS33_ODR_3_33kHz 0x09 +#define LSM6DS33_ODR_6_66kHz 0x0A + +#define LSM6DS33_LA_FS_2G 0x00 +#define LSM6DS33_LA_FS_16G 0x01 +#define LSM6DS33_LA_FS_4G 0x02 +#define LSM6DS33_LA_FS_8G 0x03 + +#define LSM6DS33_LA_AABW_400Hz 0x00 +#define LSM6DS33_LA_AABW_200Hz 0x01 +#define LSM6DS33_LA_AABW_100Hz 0x02 +#define LSM6DS33_LA_AABW_50Hz 0x03 + +#define LSM6DS33_GY_FS_125dps 0x01 +#define LSM6DS33_GY_FS_250dps 0x00 +#define LSM6DS33_GY_FS_500dps 0x02 +#define LSM6DS33_GY_FS_1000dps 0x04 +#define LSM6DS33_GY_FS_2000dps 0x05 + +typedef struct { + int16_t linacc_odr; + float linacc_mG; // milli G per Bit + int16_t linacc_aabw; + int16_t gyro_odr; + float gyro_mDPS; // milli degrees per second per Bit +} sIMUsettings; + +typedef struct { + float fTemperatureDegC; //°C + float fAccX, fAccY, fAccZ; // m/s^2 + float fGyroXradps, fGyroYradps, fGyroZradps; // rad/s +} sIMUoutput; + +extern sIMUoutput IMUoutput; + +bool imu_lsm6ds33_init(void); +bool imu_lsm6ds33_read(); diff --git a/radio/src/targets/horus/tp_gt911.cpp b/radio/src/targets/horus/tp_gt911.cpp index 667df4b50..f6a3c7c57 100644 --- a/radio/src/targets/horus/tp_gt911.cpp +++ b/radio/src/targets/horus/tp_gt911.cpp @@ -489,10 +489,10 @@ void TOUCH_AF_INT_Change(void) void I2C_Init_Radio(void) { - TRACE("I2C Init"); + TRACE("I2C TP Init"); - hi2c1.Instance = I2C; - hi2c1.Init.ClockSpeed = I2C_CLK_RATE; + hi2c1.Instance = I2C_TP; + hi2c1.Init.ClockSpeed = I2C_TP_CLK_RATE; hi2c1.Init.DutyCycle = I2C_DUTYCYCLE_16_9; hi2c1.Init.OwnAddress1 = 0; hi2c1.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT; @@ -502,17 +502,17 @@ void I2C_Init_Radio(void) hi2c1.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE; if (HAL_I2C_Init(&hi2c1) != HAL_OK) { - TRACE("I2C ERROR: HAL_I2C_Init() failed"); + TRACE("I2C TP ERROR: HAL_I2C_Init() failed"); } // Configure Analogue filter if (HAL_I2CEx_ConfigAnalogFilter(&hi2c1, I2C_ANALOGFILTER_ENABLE) != HAL_OK) { - TRACE("I2C ERROR: HAL_I2CEx_ConfigAnalogFilter() failed"); + TRACE("I2C TP ERROR: HAL_I2CEx_ConfigAnalogFilter() failed"); } // Configure Digital filter if (HAL_I2CEx_ConfigDigitalFilter(&hi2c1, 0) != HAL_OK) { - TRACE("I2C ERROR: HAL_I2CEx_ConfigDigitalFilter() failed"); + TRACE("I2C TP ERROR: HAL_I2CEx_ConfigDigitalFilter() failed"); } } @@ -532,7 +532,7 @@ bool I2C_GT911_WriteRegister(uint16_t reg, uint8_t * buf, uint8_t len) if (HAL_I2C_Master_Transmit(&hi2c1, GT911_I2C_ADDR << 1, uAddrAndBuf, len + 2, 100) != HAL_OK) { - TRACE("I2C ERROR: WriteRegister failed"); + TRACE("I2C TP ERROR: WriteRegister failed"); return false; } return true; @@ -546,13 +546,13 @@ bool I2C_GT911_ReadRegister(uint16_t reg, uint8_t * buf, uint8_t len) if (HAL_I2C_Master_Transmit(&hi2c1, GT911_I2C_ADDR << 1, uRegAddr, 2, 10) != HAL_OK) { - TRACE("I2C ERROR: ReadRegister write reg address failed"); + TRACE("I2C TP ERROR: ReadRegister write reg address failed"); return false; } if (HAL_I2C_Master_Receive(&hi2c1, GT911_I2C_ADDR << 1, buf, len, 100) != HAL_OK) { - TRACE("I2C ERROR: ReadRegister read reg address failed"); + TRACE("I2C TP ERROR: ReadRegister read reg address failed"); return false; } return true; @@ -677,15 +677,15 @@ bool touchPanelInit(void) bool I2C_ReInit(void) { - TRACE("I2C ReInit"); + TRACE("I2C TP ReInit"); touchPanelDeInit(); if (HAL_I2C_DeInit(&hi2c1) != HAL_OK) - TRACE("I2C ReInit - I2C DeInit failed"); + TRACE("I2C TP ReInit - I2C DeInit failed"); // If DeInit fails, try to re-init anyway if (!touchPanelInit()) { - TRACE("I2C ReInit - touchPanelInit failed"); + TRACE("I2C TP ReInit - touchPanelInit failed"); return false; } return true; @@ -728,7 +728,7 @@ void touchPanelRead() touchGT911hiccups++; TRACE("GT911 I2C read XY error"); if (!I2C_ReInit()) - TRACE("I2C ReInit failed"); + TRACE("I2C TP ReInit failed"); return; } @@ -750,7 +750,7 @@ void touchPanelRead() touchGT911hiccups++; TRACE("GT911 I2C data read error"); if (!I2C_ReInit()) - TRACE("I2C ReInit failed"); + TRACE("I2C TP ReInit failed"); return; } if (touchState.event == TE_NONE || touchState.event == TE_UP ||