Gyro support continued

This commit is contained in:
Bertrand Songis
2019-03-26 19:50:50 +01:00
parent 6e676f0960
commit a8a775e639
6 changed files with 142 additions and 168 deletions
+6
View File
@@ -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
+3 -2
View File
@@ -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);
+5 -2
View File
@@ -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
-98
View File
@@ -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