Gyro support continued
This commit is contained in:
@@ -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;
|
||||
|
||||
@@ -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
|
||||
@@ -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 |
|
||||
|
||||
@@ -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 <stdio.h>
|
||||
//uint8_t counter = 0;
|
||||
//#include <stdio.h>
|
||||
|
||||
#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);
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
Reference in New Issue
Block a user