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.
This commit is contained in:
rotorman
@@ -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()
|
||||
|
||||
@@ -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;
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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);
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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();
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
@@ -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();
|
||||
@@ -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 ||
|
||||
|
||||
Reference in New Issue
Block a user