diff --git a/radio/src/gui/128x64/model_setup.cpp b/radio/src/gui/128x64/model_setup.cpp index 14b8bc4ca..3fcacfa28 100644 --- a/radio/src/gui/128x64/model_setup.cpp +++ b/radio/src/gui/128x64/model_setup.cpp @@ -529,6 +529,12 @@ void menuModelSetup(event_t event) if (event == EVT_ENTRY) { reusableBuffer.moduleSetup.r9mPower = g_model.moduleData[EXTERNAL_MODULE].pxx.power; + + extern void gyroInit(); + gyroInit(); + + + } uint8_t sub = menuVerticalPosition - HEADER_LINE; diff --git a/radio/src/targets/common/arm/stm32/lsm6ds3_driver.cpp b/radio/src/targets/common/arm/stm32/lsm6ds3_driver.cpp index 98a5b15d5..8b8699b41 100644 --- a/radio/src/targets/common/arm/stm32/lsm6ds3_driver.cpp +++ b/radio/src/targets/common/arm/stm32/lsm6ds3_driver.cpp @@ -180,7 +180,8 @@ #define LSM6DS3_RESET_ADDR 0x12 #define LSM6DS3_RESET_MASK 0x01 -#define LSM6DS3_ADDRESS 0xD4 +#define LSM6DS3_ADDRESS 0xD6 +#define LSM6DSLTR_ID 0x6A static const char configure[][2] = { @@ -194,84 +195,143 @@ static const char configure[][2] = volatile int lsm6ds3_state = 1; -int lsm6ds3Init() +static void i2c2Init() { - int i; - char cmd[2]; + I2C_DeInit(I2CX); - I2C_transfer_block i2c_block[2]; + I2C_InitTypeDef I2C_InitStructure; + I2C_InitStructure.I2C_ClockSpeed = I2CX_SPEED; + I2C_InitStructure.I2C_DutyCycle = I2C_DutyCycle_2; + I2C_InitStructure.I2C_OwnAddress1 = 0x00; + I2C_InitStructure.I2C_Mode = I2C_Mode_I2C; + I2C_InitStructure.I2C_Ack = I2C_Ack_Enable; + I2C_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit; + I2C_Init(I2CX, &I2C_InitStructure); + I2C_Cmd(I2CX, ENABLE); - EXTILine_Config(); + GPIO_PinAFConfig(I2CX_SCL_GPIO, I2CX_SCL_GPIO_PinSource, I2CX_GPIO_AF); + GPIO_PinAFConfig(I2CX_SDA_GPIO, I2CX_SDA_GPIO_PinSource, I2CX_GPIO_AF); - cmd[0] = LSM6DS3_WHO_AM_I; + GPIO_InitTypeDef GPIO_InitStructure; + GPIO_InitStructure.GPIO_Pin = I2CX_SCL_GPIO_PIN; + GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz; + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; + GPIO_InitStructure.GPIO_OType = GPIO_OType_OD; + GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP; + GPIO_Init(I2CX_SCL_GPIO, &GPIO_InitStructure); - i2c_block[0].addr = LSM6DS3_ADDRESS; - i2c_block[0].rw = I2C_WRITE; - i2c_block[0].stop_mode = I2C_WRITE_BYPASS_STOP; - i2c_block[0].ack_mode = I2C_READ_RESPOND_ACK; - i2c_block[0].len = 1; - i2c_block[0].buf = cmd; + GPIO_InitStructure.GPIO_Pin = I2CX_SDA_GPIO_PIN; + GPIO_Init(I2CX_SDA_GPIO, &GPIO_InitStructure); +} - i2c_block[1].addr = LSM6DS3_ADDRESS; - i2c_block[1].rw = I2C_READ; - i2c_block[1].stop_mode = I2C_WRITE_APPEND_STOP; - i2c_block[1].ack_mode = I2C_READ_RESPOND_ACK; - i2c_block[1].len = 2; - i2c_block[1].buf = cmd; +#define I2C_TIMEOUT_MAX 10000 +bool I2C2_WaitEvent(uint32_t event) +{ + uint32_t timeout = I2C_TIMEOUT_MAX; + while (!I2C_CheckEvent(I2CX, event)) { + if ((timeout--) == 0) return false; + } + return true; +} - i2c_dma_transfer(i2c_block, 2); +bool I2C2_WaitEventCleared(uint32_t event) +{ + uint32_t timeout = I2C_TIMEOUT_MAX; + while (I2C_CheckEvent(I2CX, event)) { + if ((timeout--) == 0) return false; + } + return true; +} - if (cmd[0] != LSM6DS3_WHO_AM_I_DEF) { +int setGyroRegister(uint8_t address, uint8_t value) +{ + if (!I2C2_WaitEventCleared(I2C_FLAG_BUSY)) return -1; - } - cmd[0] = 0x12; - cmd[1] = 0x5; - i2c_block[0].addr = LSM6DS3_ADDRESS; - i2c_block[0].rw = I2C_WRITE; - i2c_block[0].stop_mode = I2C_WRITE_APPEND_STOP; + I2C_GenerateSTART(I2CX, ENABLE); + if (!I2C2_WaitEvent(I2C_EVENT_MASTER_MODE_SELECT)) + return -1; - i2c_block[0].ack_mode = I2C_READ_RESPOND_ACK; + I2C_Send7bitAddress(I2CX, LSM6DS3_ADDRESS, I2C_Direction_Transmitter); + if (!I2C2_WaitEvent(I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED)) + return -1; - i2c_block[0].len = 2; - i2c_block[0].buf = cmd; + I2C_SendData(I2CX, address); + if (!I2C2_WaitEvent(I2C_EVENT_MASTER_BYTE_TRANSMITTED)) // TODO TRANSMITTING + return -1; - i2c_dma_transfer(i2c_block, 1); + I2C_SendData(I2CX, value); + if (!I2C2_WaitEvent(I2C_EVENT_MASTER_BYTE_TRANSMITTED)) + return -1; - delay_ms(1); - - cmd[0] = 0x12; - cmd[1] = 0x4; - i2c_block[0].addr = LSM6DS3_ADDRESS; - i2c_block[0].rw = I2C_WRITE; - i2c_block[0].stop_mode = I2C_WRITE_APPEND_STOP; - - i2c_block[0].ack_mode = I2C_READ_RESPOND_ACK; - - i2c_block[0].len = 2; - i2c_block[0].buf = cmd; - - i2c_dma_transfer(i2c_block, 1); - - delay_ms(1); - - for (i = 0; i < sizeof(configure) / sizeof(configure[0]); i++) { - i2c_block[0].addr = LSM6DS3_ADDRESS; - i2c_block[0].rw = I2C_WRITE; - i2c_block[0].stop_mode = I2C_WRITE_APPEND_STOP; - - i2c_block[0].ack_mode = I2C_READ_RESPOND_ACK; - - i2c_block[0].len = 2; - i2c_block[0].buf = (char *) configure[i]; - - i2c_dma_transfer(i2c_block, 1); - } + I2C_GenerateSTOP(I2CX, ENABLE); return 0; } -int lsm6ds3Read(void *buffer, int len, OFFSET offset1) +int readGyroRegister(uint8_t address) +{ + if (!I2C2_WaitEventCleared(I2C_FLAG_BUSY)) + return -1; + + I2C_GenerateSTART(I2CX, ENABLE); + if (!I2C2_WaitEvent(I2C_EVENT_MASTER_MODE_SELECT)) + return -1; + + I2C_Send7bitAddress(I2CX, LSM6DS3_ADDRESS, I2C_Direction_Transmitter); + if (!I2C2_WaitEvent(I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED)) + return -1; + + I2C_SendData(I2CX, address); + if (!I2C2_WaitEvent(I2C_EVENT_MASTER_BYTE_TRANSMITTED)) + return -1; + + I2C_GenerateSTART(I2CX, ENABLE); + if (!I2C2_WaitEvent(I2C_EVENT_MASTER_MODE_SELECT)) + return -1; + + I2C_Send7bitAddress(I2CX, LSM6DS3_ADDRESS, I2C_Direction_Receiver); + + I2C_AcknowledgeConfig(I2CX, DISABLE); + if (!I2C2_WaitEvent(I2C_EVENT_MASTER_BYTE_RECEIVED)) + return -1; + + uint8_t value = I2C_ReceiveData(I2CX); + + I2C_GenerateSTOP(I2CX, ENABLE); + + return value; +} + +int gyroInit() +{ + i2c2Init(); + + uint8_t id = readGyroRegister(LSM6DS3_WHO_AM_I); + if (id != LSM6DSLTR_ID) { + return -1; + } + + TRACE("OK"); + + setGyroRegister(0x12, 0x05); + delay_ms(1); + setGyroRegister(0x12, 0x04); + delay_ms(1); + + TRACE("OK2"); + + for (uint8_t i = 0; i < DIM(configure); i++) { + setGyroRegister(configure[i][0], configure[i][1]); + } + + TRACE("OK4"); + + return 0; +} + +#if 0 +int gyroRead(void *buffer, int len, OFFSET offset1) { char t_char[10]; short t_short[7]; @@ -311,3 +371,5 @@ int lsm6ds3Read(void *buffer, int len, OFFSET offset1) return len; } + +#endif \ No newline at end of file diff --git a/radio/src/targets/taranis/board.cpp b/radio/src/targets/taranis/board.cpp index fd3b88ca1..6a8bfcf5f 100644 --- a/radio/src/targets/taranis/board.cpp +++ b/radio/src/targets/taranis/board.cpp @@ -151,14 +151,15 @@ void boardInit() INTMODULE_RCC_AHB1Periph | EXTMODULE_RCC_AHB1Periph | TELEMETRY_RCC_AHB1Periph | SPORT_UPDATE_RCC_AHB1Periph | SERIAL_RCC_AHB1Periph | TRAINER_RCC_AHB1Periph | - HEARTBEAT_RCC_AHB1Periph | BT_RCC_AHB1Periph, ENABLE); + HEARTBEAT_RCC_AHB1Periph | BT_RCC_AHB1Periph | GYRO_RCC_AHB1Periph, + ENABLE); RCC_APB1PeriphClockCmd(LCD_RCC_APB1Periph | AUDIO_RCC_APB1Periph | ADC_RCC_APB1Periph | BACKLIGHT_RCC_APB1Periph | HAPTIC_RCC_APB1Periph | INTERRUPT_xMS_RCC_APB1Periph | TIMER_2MHz_RCC_APB1Periph | I2C_RCC_APB1Periph | SD_RCC_APB1Periph | TRAINER_RCC_APB1Periph | TELEMETRY_RCC_APB1Periph | SERIAL_RCC_APB1Periph | - INTMODULE_RCC_APB1Periph | BT_RCC_APB1Periph, ENABLE); + INTMODULE_RCC_APB1Periph | BT_RCC_APB1Periph | GYRO_RCC_APB1Periph, ENABLE); RCC_APB2PeriphClockCmd(BACKLIGHT_RCC_APB2Periph | ADC_RCC_APB2Periph | HAPTIC_RCC_APB2Periph | INTMODULE_RCC_APB2Periph | diff --git a/radio/src/targets/taranis/extmodule_driver.cpp b/radio/src/targets/taranis/extmodule_driver.cpp index 4418a95df..fc590e2ba 100644 --- a/radio/src/targets/taranis/extmodule_driver.cpp +++ b/radio/src/targets/taranis/extmodule_driver.cpp @@ -179,8 +179,8 @@ void extmoduleSendBuffer(const uint8_t * data, uint8_t size) } // TODO remove this when we have adaptative speed -uint8_t counter = 0; -#include +//uint8_t counter = 0; +//#include #define USART_FLAG_ERRORS (USART_FLAG_ORE | USART_FLAG_NE | USART_FLAG_FE | USART_FLAG_PE) extern "C" void EXTMODULE_USART_IRQHandler(void) @@ -191,9 +191,9 @@ extern "C" void EXTMODULE_USART_IRQHandler(void) uint8_t data = EXTMODULE_USART->DR; if (status & USART_FLAG_ERRORS) { extmoduleFifo.errors++; - if (!counter++) { - TRACE_NOCRLF("%02X ", (uint8_t)status); - } +// if (!counter++) { +// TRACE_NOCRLF("%02X ", (uint8_t)status); +// } } else { extmoduleFifo.push(data); diff --git a/radio/src/targets/taranis/hal.h b/radio/src/targets/taranis/hal.h index 9b8d9ddf4..6f12e32d7 100644 --- a/radio/src/targets/taranis/hal.h +++ b/radio/src/targets/taranis/hal.h @@ -1191,8 +1191,8 @@ // Second I2C Bus: IMU #if defined(PCBXLITES) - #define I2CX_RCC_AHB1Periph (RCC_AHB1Periph_GPIOA | RCC_AHB1Periph_GPIOC) - #define I2CX_RCC_APB1Periph RCC_APB1Periph_I2C3 + #define GYRO_RCC_AHB1Periph (RCC_AHB1Periph_GPIOA | RCC_AHB1Periph_GPIOC) + #define GYRO_RCC_APB1Periph RCC_APB1Periph_I2C3 #define I2CX I2C3 #define I2CX_SCL_GPIO GPIOA #define I2CX_SCL_GPIO_PIN GPIO_Pin_8 // PA.08 @@ -1202,6 +1202,9 @@ #define I2CX_SCL_GPIO_PinSource GPIO_PinSource8 #define I2CX_SDA_GPIO_PinSource GPIO_PinSource9 #define I2CX_SPEED 400000 +#else + #define GYRO_RCC_AHB1Periph 0 + #define GYRO_RCC_APB1Periph 0 #endif // SD - SPI2 diff --git a/radio/src/targets/taranis/i2c2_driver.cpp b/radio/src/targets/taranis/i2c2_driver.cpp deleted file mode 100644 index fe7f3a2fb..000000000 --- a/radio/src/targets/taranis/i2c2_driver.cpp +++ /dev/null @@ -1,98 +0,0 @@ -/* - * Copyright (C) OpenTX - * - * Based on code named - * 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 "board.h" - -void i2c2Init() -{ - I2C_DeInit(I2CX); - - I2C_InitTypeDef I2C_InitStructure; - I2C_InitStructure.I2C_ClockSpeed = I2CX_SPEED; - I2C_InitStructure.I2C_DutyCycle = I2C_DutyCycle_2; - I2C_InitStructure.I2C_OwnAddress1 = 0x00; - I2C_InitStructure.I2C_Mode = I2C_Mode_I2C; - I2C_InitStructure.I2C_Ack = I2C_Ack_Enable; - I2C_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit; - I2C_Init(I2CX, &I2C_InitStructure); - I2C_Cmd(I2CX, ENABLE); - - GPIO_PinAFConfig(I2CX_SCL_GPIO, I2CX_SCL_GPIO_PinSource, I2CX_GPIO_AF); - GPIO_PinAFConfig(I2CX_SDA_GPIO, I2CX_SDA_GPIO_PinSource, I2CX_GPIO_AF); - - GPIO_InitTypeDef GPIO_InitStructure; - GPIO_InitStructure.GPIO_Pin = I2CX_SCL_GPIO_PIN; - GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz; - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; - GPIO_InitStructure.GPIO_OType = GPIO_OType_OD; - GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP; - GPIO_Init(I2CX_SCL_GPIO, &GPIO_InitStructure); - - GPIO_InitStructure.GPIO_Pin = I2CX_SDA_GPIO_PIN; - GPIO_Init(I2CX_SDA_GPIO, &GPIO_InitStructure); -} - -#define I2C_TIMEOUT_MAX 1000 -bool I2C_WaitEvent(uint32_t event) -{ - uint32_t timeout = I2C_TIMEOUT_MAX; - while (!I2C_CheckEvent(I2CX, event)) { - if ((timeout--) == 0) return false; - } - return true; -} - -bool I2C2_WaitEventCleared(uint32_t event) -{ - uint32_t timeout = I2C_TIMEOUT_MAX; - while (I2C_CheckEvent(I2CX, event)) { - if ((timeout--) == 0) return false; - } - return true; -} - -void setVolume(uint8_t volume) -{ - if (!I2C2_WaitEventCleared(I2C_FLAG_BUSY)) - return; - - I2C_GenerateSTART(I2CX, ENABLE); - if (!I2C2_WaitEvent(I2C_EVENT_MASTER_MODE_SELECT)) - return; - - I2C_Send7bitAddress(I2C, I2C_ADDRESS_VOLUME, I2C_Direction_Transmitter); - if (!I2C2_WaitEvent(I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED)) - return; - - I2C_SendData(I2CX, 0); - if (!I2C2_WaitEvent(I2C_EVENT_MASTER_BYTE_TRANSMITTING)) - return; - I2C_SendData(I2CX, volume); - if (!I2C2_WaitEvent(I2C_EVENT_MASTER_BYTE_TRANSMITTED)) - return; - - I2C_GenerateSTOP(I2CX, ENABLE); -} - -int32_t getVolume() -{ - return -1; // TODO -} -#endif