[X-LiteS] Gyro added

This commit is contained in:
Bertrand Songis
2019-04-02 20:12:50 +02:00
parent b967d5b08a
commit a13ee14bbb
18 changed files with 468 additions and 318 deletions
@@ -20,180 +20,175 @@
#include "opentx.h"
//#define ACC_LSB_VALUE 0.000488 // 0.488 mg/LSB
//#define GYRO_LSB_VALUE 0.07 // 70mDPS in 2000 deg/second
/* COMMON VALUES FOR ACCEL-GYRO SENSORS */
#define LSM6DS3_WHO_AM_I 0x0f
#define LSM6DS3_WHO_AM_I_DEF 0x69
#define LSM6DS3_AXIS_EN_MASK 0x38
#define LSM6DS3_INT1_CTRL_ADDR 0x0d
#define LSM6DS3_INT2_CTRL_ADDR 0x0e
#define LSM6DS3_WHO_AM_I 0x0f
#define LSM6DS3_WHO_AM_I_DEF 0x69
#define LSM6DS3_AXIS_EN_MASK 0x38
#define LSM6DS3_INT1_CTRL_ADDR 0x0d
#define LSM6DS3_INT2_CTRL_ADDR 0x0e
#define LSM6DS3_INT1_FULL 0x20
#define LSM6DS3_INT1_FTH 0x08
#define LSM6DS3_MD1_ADDR 0x5e
#define LSM6DS3_ODR_LIST_NUM 5
#define LSM6DS3_INT1_FTH 0x08
#define LSM6DS3_MD1_ADDR 0x5e
#define LSM6DS3_ODR_LIST_NUM 5
#define LSM6DS3_ODR_POWER_OFF_VAL 0x00
#define LSM6DS3_ODR_26HZ_VAL 0x02
#define LSM6DS3_ODR_52HZ_VAL 0x03
#define LSM6DS3_ODR_104HZ_VAL 0x04
#define LSM6DS3_ODR_208HZ_VAL 0x05
#define LSM6DS3_ODR_416HZ_VAL 0x06
#define LSM6DS3_FS_LIST_NUM 4
#define LSM6DS3_BDU_ADDR 0x12
#define LSM6DS3_BDU_MASK 0x40
#define LSM6DS3_EN_BIT 0x01
#define LSM6DS3_DIS_BIT 0x00
#define LSM6DS3_FUNC_EN_ADDR 0x19
#define LSM6DS3_FUNC_EN_MASK 0x04
#define LSM6DS3_FUNC_CFG_ACCESS_ADDR 0x01
#define LSM6DS3_FUNC_CFG_ACCESS_MASK 0x01
#define LSM6DS3_FUNC_CFG_ACCESS_MASK2 0x04
#define LSM6DS3_FUNC_CFG_REG2_MASK 0x80
#define LSM6DS3_FUNC_CFG_START1_ADDR 0x62
#define LSM6DS3_FUNC_CFG_START2_ADDR 0x63
#define LSM6DS3_SELFTEST_ADDR 0x14
#define LSM6DS3_SELFTEST_ACCEL_MASK 0x03
#define LSM6DS3_SELFTEST_GYRO_MASK 0x0c
#define LSM6DS3_SELF_TEST_DISABLED_VAL 0x00
#define LSM6DS3_SELF_TEST_POS_SIGN_VAL 0x01
#define LSM6DS3_SELF_TEST_NEG_ACCEL_SIGN_VAL 0x02
#define LSM6DS3_SELF_TEST_NEG_GYRO_SIGN_VAL 0x03
#define LSM6DS3_LIR_ADDR 0x58
#define LSM6DS3_LIR_MASK 0x01
#define LSM6DS3_TIMER_EN_ADDR 0x58
#define LSM6DS3_TIMER_EN_MASK 0x80
#define LSM6DS3_ODR_26HZ_VAL 0x02
#define LSM6DS3_ODR_52HZ_VAL 0x03
#define LSM6DS3_ODR_104HZ_VAL 0x04
#define LSM6DS3_ODR_208HZ_VAL 0x05
#define LSM6DS3_ODR_416HZ_VAL 0x06
#define LSM6DS3_FS_LIST_NUM 4
#define LSM6DS3_BDU_ADDR 0x12
#define LSM6DS3_BDU_MASK 0x40
#define LSM6DS3_EN_BIT 0x01
#define LSM6DS3_DIS_BIT 0x00
#define LSM6DS3_FUNC_EN_ADDR 0x19
#define LSM6DS3_FUNC_EN_MASK 0x04
#define LSM6DS3_FUNC_CFG_ACCESS_ADDR 0x01
#define LSM6DS3_FUNC_CFG_ACCESS_MASK 0x01
#define LSM6DS3_FUNC_CFG_ACCESS_MASK2 0x04
#define LSM6DS3_FUNC_CFG_REG2_MASK 0x80
#define LSM6DS3_FUNC_CFG_START1_ADDR 0x62
#define LSM6DS3_FUNC_CFG_START2_ADDR 0x63
#define LSM6DS3_SELFTEST_ADDR 0x14
#define LSM6DS3_SELFTEST_ACCEL_MASK 0x03
#define LSM6DS3_SELFTEST_GYRO_MASK 0x0c
#define LSM6DS3_SELF_TEST_DISABLED_VAL 0x00
#define LSM6DS3_SELF_TEST_POS_SIGN_VAL 0x01
#define LSM6DS3_SELF_TEST_NEG_ACCEL_SIGN_VAL 0x02
#define LSM6DS3_SELF_TEST_NEG_GYRO_SIGN_VAL 0x03
#define LSM6DS3_LIR_ADDR 0x58
#define LSM6DS3_LIR_MASK 0x01
#define LSM6DS3_TIMER_EN_ADDR 0x58
#define LSM6DS3_TIMER_EN_MASK 0x80
#define LSM6DS3_PEDOMETER_EN_ADDR 0x58
#define LSM6DS3_PEDOMETER_EN_MASK 0x40
#define LSM6DS3_INT2_ON_INT1_ADDR 0x13
#define LSM6DS3_INT2_ON_INT1_MASK 0x20
#define LSM6DS3_MIN_DURATION_MS 1638
#define LSM6DS3_ROUNDING_ADDR 0x16
#define LSM6DS3_ROUNDING_MASK 0x04
#define LSM6DS3_FIFO_MODE_ADDR 0x0a
#define LSM6DS3_FIFO_MODE_MASK 0x07
#define LSM6DS3_FIFO_MODE_BYPASS 0x00
#define LSM6DS3_FIFO_MODE_CONTINUOS 0x06
#define LSM6DS3_FIFO_THRESHOLD_IRQ_MASK 0x08
#define LSM6DS3_FIFO_ODR_ADDR 0x0a
#define LSM6DS3_FIFO_ODR_MASK 0x78
#define LSM6DS3_FIFO_ODR_MAX 0x07
#define LSM6DS3_FIFO_ODR_MAX_HZ 800
#define LSM6DS3_FIFO_ODR_OFF 0x00
#define LSM6DS3_FIFO_CTRL3_ADDR 0x08
#define LSM6DS3_MIN_DURATION_MS 1638
#define LSM6DS3_ROUNDING_ADDR 0x16
#define LSM6DS3_ROUNDING_MASK 0x04
#define LSM6DS3_FIFO_MODE_ADDR 0x0a
#define LSM6DS3_FIFO_MODE_MASK 0x07
#define LSM6DS3_FIFO_MODE_BYPASS 0x00
#define LSM6DS3_FIFO_MODE_CONTINUOS 0x06
#define LSM6DS3_FIFO_THRESHOLD_IRQ_MASK 0x08
#define LSM6DS3_FIFO_ODR_ADDR 0x0a
#define LSM6DS3_FIFO_ODR_MASK 0x78
#define LSM6DS3_FIFO_ODR_MAX 0x07
#define LSM6DS3_FIFO_ODR_MAX_HZ 800
#define LSM6DS3_FIFO_ODR_OFF 0x00
#define LSM6DS3_FIFO_CTRL3_ADDR 0x08
#define LSM6DS3_FIFO_ACCEL_DECIMATOR_MASK 0x07
#define LSM6DS3_FIFO_GYRO_DECIMATOR_MASK 0x38
#define LSM6DS3_FIFO_CTRL4_ADDR 0x09
#define LSM6DS3_FIFO_STEP_C_DECIMATOR_MASK 0x38
#define LSM6DS3_FIFO_THR_L_ADDR 0x06
#define LSM6DS3_FIFO_THR_H_ADDR 0x07
#define LSM6DS3_FIFO_THR_H_MASK 0x0f
#define LSM6DS3_FIFO_GYRO_DECIMATOR_MASK 0x38
#define LSM6DS3_FIFO_CTRL4_ADDR 0x09
#define LSM6DS3_FIFO_STEP_C_DECIMATOR_MASK 0x38
#define LSM6DS3_FIFO_THR_L_ADDR 0x06
#define LSM6DS3_FIFO_THR_H_ADDR 0x07
#define LSM6DS3_FIFO_THR_H_MASK 0x0f
#define LSM6DS3_FIFO_THR_IRQ_MASK 0x08
#define LSM6DS3_FIFO_PEDO_E_ADDR 0x07
#define LSM6DS3_FIFO_PEDO_E_MASK 0x80
#define LSM6DS3_FIFO_STEP_C_FREQ 25
#define LSM6DS3_FIFO_PEDO_E_ADDR 0x07
#define LSM6DS3_FIFO_PEDO_E_MASK 0x80
#define LSM6DS3_FIFO_STEP_C_FREQ 25
/* CUSTOM VALUES FOR ACCEL SENSOR */
#define LSM6DS3_ACCEL_ODR_ADDR 0x10
#define LSM6DS3_ACCEL_ODR_MASK 0xf0
#define LSM6DS3_ACCEL_FS_ADDR 0x10
#define LSM6DS3_ACCEL_FS_MASK 0x0c
#define LSM6DS3_ACCEL_FS_2G_VAL 0x00
#define LSM6DS3_ACCEL_FS_4G_VAL 0x02
#define LSM6DS3_ACCEL_FS_8G_VAL 0x03
#define LSM6DS3_ACCEL_FS_16G_VAL 0x01
#define LSM6DS3_ACCEL_FS_2G_GAIN 61
#define LSM6DS3_ACCEL_FS_4G_GAIN 122
#define LSM6DS3_ACCEL_FS_8G_GAIN 244
#define LSM6DS3_ACCEL_ODR_ADDR 0x10
#define LSM6DS3_ACCEL_ODR_MASK 0xf0
#define LSM6DS3_ACCEL_FS_ADDR 0x10
#define LSM6DS3_ACCEL_FS_MASK 0x0c
#define LSM6DS3_ACCEL_FS_2G_VAL 0x00
#define LSM6DS3_ACCEL_FS_4G_VAL 0x02
#define LSM6DS3_ACCEL_FS_8G_VAL 0x03
#define LSM6DS3_ACCEL_FS_16G_VAL 0x01
#define LSM6DS3_ACCEL_FS_2G_GAIN 61
#define LSM6DS3_ACCEL_FS_4G_GAIN 122
#define LSM6DS3_ACCEL_FS_8G_GAIN 244
#define LSM6DS3_ACCEL_FS_16G_GAIN 488
#define LSM6DS3_ACCEL_OUT_X_L_ADDR 0x28
#define LSM6DS3_ACCEL_OUT_Y_L_ADDR 0x2a
#define LSM6DS3_ACCEL_OUT_Z_L_ADDR 0x2c
#define LSM6DS3_ACCEL_AXIS_EN_ADDR 0x18
#define LSM6DS3_ACCEL_DRDY_IRQ_MASK 0x01
#define LSM6DS3_ACCEL_OUT_X_L_ADDR 0x28
#define LSM6DS3_ACCEL_OUT_Y_L_ADDR 0x2a
#define LSM6DS3_ACCEL_OUT_Z_L_ADDR 0x2c
#define LSM6DS3_ACCEL_AXIS_EN_ADDR 0x18
#define LSM6DS3_ACCEL_DRDY_IRQ_MASK 0x01
#define LSM6DS3_ACCEL_STD 1
#define LSM6DS3_ACCEL_STD_FROM_PD 2
/* CUSTOM VALUES FOR GYRO SENSOR */
#define LSM6DS3_GYRO_ODR_ADDR 0x11
#define LSM6DS3_GYRO_ODR_MASK 0xf0
#define LSM6DS3_GYRO_FS_ADDR 0x11
#define LSM6DS3_GYRO_FS_MASK 0x0c
#define LSM6DS3_GYRO_FS_245_VAL 0x00
#define LSM6DS3_GYRO_FS_500_VAL 0x01
#define LSM6DS3_GYRO_FS_1000_VAL 0x02
#define LSM6DS3_GYRO_FS_2000_VAL 0x03
#define LSM6DS3_GYRO_FS_245_GAIN 8750
#define LSM6DS3_GYRO_FS_500_GAIN 17500
#define LSM6DS3_GYRO_ODR_ADDR 0x11
#define LSM6DS3_GYRO_ODR_MASK 0xf0
#define LSM6DS3_GYRO_FS_ADDR 0x11
#define LSM6DS3_GYRO_FS_MASK 0x0c
#define LSM6DS3_GYRO_FS_245_VAL 0x00
#define LSM6DS3_GYRO_FS_500_VAL 0x01
#define LSM6DS3_GYRO_FS_1000_VAL 0x02
#define LSM6DS3_GYRO_FS_2000_VAL 0x03
#define LSM6DS3_GYRO_FS_245_GAIN 8750
#define LSM6DS3_GYRO_FS_500_GAIN 17500
#define LSM6DS3_GYRO_FS_1000_GAIN 35000
#define LSM6DS3_GYRO_FS_2000_GAIN 70000
#define LSM6DS3_GYRO_OUT_X_L_ADDR 0x22
#define LSM6DS3_GYRO_OUT_Y_L_ADDR 0x24
#define LSM6DS3_GYRO_OUT_Z_L_ADDR 0x26
#define LSM6DS3_GYRO_AXIS_EN_ADDR 0x19
#define LSM6DS3_GYRO_DRDY_IRQ_MASK 0x02
#define LSM6DS3_GYRO_STD 6
#define LSM6DS3_GYRO_STD_FROM_PD 2
#define LSM6DS3_GYRO_DRDY_IRQ_MASK 0x02
#define LSM6DS3_GYRO_STD 6
#define LSM6DS3_GYRO_STD_FROM_PD 2
#define LSM6DS3_OUT_XYZ_SIZE 8
#define LSM6DS3_OUT_XYZ_SIZE 8
/* CUSTOM VALUES FOR SIGNIFICANT MOTION SENSOR */
#define LSM6DS3_SIGN_MOTION_EN_ADDR 0x19
#define LSM6DS3_SIGN_MOTION_EN_MASK 0x01
#define LSM6DS3_SIGN_MOTION_EN_ADDR 0x19
#define LSM6DS3_SIGN_MOTION_EN_MASK 0x01
#define LSM6DS3_SIGN_MOTION_DRDY_IRQ_MASK 0x40
/* CUSTOM VALUES FOR STEP DETECTOR SENSOR */
#define LSM6DS3_STEP_DETECTOR_DRDY_IRQ_MASK 0x80
#define LSM6DS3_STEP_DETECTOR_DRDY_IRQ_MASK 0x80
/* CUSTOM VALUES FOR STEP COUNTER SENSOR */
#define LSM6DS3_STEP_COUNTER_DRDY_IRQ_MASK 0x80
#define LSM6DS3_STEP_COUNTER_OUT_L_ADDR 0x4b
#define LSM6DS3_STEP_COUNTER_OUT_SIZE 2
#define LSM6DS3_STEP_COUNTER_RES_ADDR 0x19
#define LSM6DS3_STEP_COUNTER_RES_MASK 0x06
#define LSM6DS3_STEP_COUNTER_RES_ALL_EN 0x03
#define LSM6DS3_STEP_COUNTER_RES_FUNC_EN 0x02
#define LSM6DS3_STEP_COUNTER_DURATION_ADDR 0x15
#define LSM6DS3_STEP_COUNTER_DRDY_IRQ_MASK 0x80
#define LSM6DS3_STEP_COUNTER_OUT_L_ADDR 0x4b
#define LSM6DS3_STEP_COUNTER_OUT_SIZE 2
#define LSM6DS3_STEP_COUNTER_RES_ADDR 0x19
#define LSM6DS3_STEP_COUNTER_RES_MASK 0x06
#define LSM6DS3_STEP_COUNTER_RES_ALL_EN 0x03
#define LSM6DS3_STEP_COUNTER_RES_FUNC_EN 0x02
#define LSM6DS3_STEP_COUNTER_DURATION_ADDR 0x15
/* CUSTOM VALUES FOR TILT SENSOR */
#define LSM6DS3_TILT_EN_ADDR 0x58
#define LSM6DS3_TILT_EN_MASK 0x20
#define LSM6DS3_TILT_DRDY_IRQ_MASK 0x02
#define LSM6DS3_TILT_EN_ADDR 0x58
#define LSM6DS3_TILT_EN_MASK 0x20
#define LSM6DS3_TILT_DRDY_IRQ_MASK 0x02
#define LSM6DS3_ENABLE_AXIS 0x07
#define LSM6DS3_FIFO_DIFF_L 0x3a
#define LSM6DS3_FIFO_DIFF_MASK 0x0fff
#define LSM6DS3_FIFO_DATA_OUT_L 0x3e
#define LSM6DS3_FIFO_ELEMENT_LEN_BYTE 6
#define LSM6DS3_FIFO_BYTE_FOR_CHANNEL 2
#define LSM6DS3_FIFO_DATA_OVR_2REGS 0x4000
#define LSM6DS3_FIFO_DATA_OVR 0x40
#define LSM6DS3_ENABLE_AXIS 0x07
#define LSM6DS3_FIFO_DIFF_L 0x3a
#define LSM6DS3_FIFO_DIFF_MASK 0x0fff
#define LSM6DS3_FIFO_DATA_OUT_L 0x3e
#define LSM6DS3_FIFO_ELEMENT_LEN_BYTE 6
#define LSM6DS3_FIFO_BYTE_FOR_CHANNEL 2
#define LSM6DS3_FIFO_DATA_OVR_2REGS 0x4000
#define LSM6DS3_FIFO_DATA_OVR 0x40
#define LSM6DS3_SRC_FUNC_ADDR 0x53
#define LSM6DS3_FIFO_DATA_AVL_ADDR 0x3b
#define LSM6DS3_SRC_FUNC_ADDR 0x53
#define LSM6DS3_FIFO_DATA_AVL_ADDR 0x3b
#define LSM6DS3_SRC_SIGN_MOTION_DATA_AVL 0x40
#define LSM6DS3_SRC_STEP_DETECTOR_DATA_AVL 0x10
#define LSM6DS3_SRC_TILT_DATA_AVL 0x20
#define LSM6DS3_SRC_STEP_COUNTER_DATA_AVL 0x80
#define LSM6DS3_FIFO_DATA_AVL 0x80
#define LSM6DS3_RESET_ADDR 0x12
#define LSM6DS3_RESET_MASK 0x01
#define LSM6DS3_SRC_SIGN_MOTION_DATA_AVL 0x40
#define LSM6DS3_SRC_STEP_DETECTOR_DATA_AVL 0x10
#define LSM6DS3_SRC_TILT_DATA_AVL 0x20
#define LSM6DS3_SRC_STEP_COUNTER_DATA_AVL 0x80
#define LSM6DS3_FIFO_DATA_AVL 0x80
#define LSM6DS3_RESET_ADDR 0x12
#define LSM6DS3_RESET_MASK 0x01
#define LSM6DS3_ADDRESS 0xD6
#define LSM6DSLTR_ID 0x6A
#define LSM6DS3_ADDRESS 0xD6
#define LSM6DSLTR_ID 0x6A
static const char configure[][2] =
{
{LSM6DS3_ACCEL_AXIS_EN_ADDR, 0x38}, ///< xyz
{LSM6DS3_ACCEL_ODR_ADDR, (0x3 << 4) | (0x1 << 2) | (0x3 << 0)}, //ODR_52Hz|16G|BW_50Hz
{LSM6DS3_GYRO_AXIS_EN_ADDR, 0x38}, ///< xyz
{LSM6DS3_GYRO_ODR_ADDR, (3 << 4) | (3 << 2) | (0 << 0)}, ///< ODR_52Hz|2000dps|
{LSM6DS3_INT1_CTRL_ADDR, 0x3}, ///< GYRO_READY | ACCL_READY
{LSM6DS3_INT2_CTRL_ADDR, 0x3}, ///< GYRO_READY | ACCL_READY
};
volatile int lsm6ds3_state = 1;
static const char configure[][2] = {
{LSM6DS3_ACCEL_AXIS_EN_ADDR, 0x38},
{LSM6DS3_ACCEL_ODR_ADDR, (0x3 << 4) | (0x1 << 2) | (0x3 << 0)},
{LSM6DS3_GYRO_AXIS_EN_ADDR, 0x38},
{LSM6DS3_GYRO_ODR_ADDR, (3 << 4) | (3 << 2) | (0 << 0)},
{LSM6DS3_INT1_CTRL_ADDR, 0x3},
{LSM6DS3_INT2_CTRL_ADDR, 0x3},
};
static void i2c2Init()
{
@@ -238,7 +233,10 @@ bool I2C2_WaitEventCleared(uint32_t event)
{
uint32_t timeout = I2C_TIMEOUT_MAX;
while (I2C_CheckEvent(I2CX, event)) {
if ((timeout--) == 0) return false;
if ((timeout--) == 0) {
TRACE("I2C Timeout!");
return false;
}
}
return true;
}
@@ -257,7 +255,7 @@ int setGyroRegister(uint8_t address, uint8_t value)
return -1;
I2C_SendData(I2CX, address);
if (!I2C2_WaitEvent(I2C_EVENT_MASTER_BYTE_TRANSMITTED)) // TODO TRANSMITTING
if (!I2C2_WaitEvent(I2C_EVENT_MASTER_BYTE_TRANSMITTING))
return -1;
I2C_SendData(I2CX, value);
@@ -312,64 +310,50 @@ int gyroInit()
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)
int gyroRead(uint8_t buffer[GYRO_BUFFER_LENGTH])
{
char t_char[10];
short t_short[7];
if (!I2C2_WaitEventCleared(I2C_FLAG_BUSY))
return -1;
short *p_short;
I2C_transfer_block i2c_block[2];
I2C_GenerateSTART(I2CX, ENABLE);
if (!I2C2_WaitEvent(I2C_EVENT_MASTER_MODE_SELECT))
return -1;
xSemaphoreTake(lsm6ds3_sema, portMAX_DELAY);
I2C_Send7bitAddress(I2CX, LSM6DS3_ADDRESS, I2C_Direction_Transmitter);
if (!I2C2_WaitEvent(I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED))
return -1;
t_char[0] = LSM6DS3_GYRO_OUT_X_L_ADDR;
I2C_SendData(I2CX, LSM6DS3_GYRO_OUT_X_L_ADDR);
if (!I2C2_WaitEvent(I2C_EVENT_MASTER_BYTE_TRANSMITTED))
return -1;
i2c_block[0].addr = LSM6DS3_ADDRESS;
i2c_block[0].rw = I2C_WRITE;
i2c_block[0].stop_mode = I2C_WRITE_BYPASS_STOP;
i2c_block[0].len = 1;
i2c_block[0].buf = t_char;
I2C_GenerateSTART(I2CX, ENABLE);
if (!I2C2_WaitEvent(I2C_EVENT_MASTER_MODE_SELECT))
return -1;
i2c_block[1].addr = LSM6DS3_ADDRESS;
i2c_block[1].rw = I2C_READ;
i2c_block[1].stop_mode = I2C_WRITE_APPEND_STOP;
I2C_Send7bitAddress(I2CX, LSM6DS3_ADDRESS, I2C_Direction_Receiver);
i2c_block[1].len = len;
i2c_block[1].buf = (char *) t_short;
i2c_dma_transfer(i2c_block, 2);
{
p_short = (short *) buffer;
*p_short++ = t_short[0];
*p_short++ = -t_short[1];
*p_short++ = t_short[2];
*p_short++ = -t_short[3];
*p_short++ = t_short[4];
*p_short++ = -t_short[5];
I2C_AcknowledgeConfig(I2CX, ENABLE);
for (uint8_t i=0; i<GYRO_BUFFER_LENGTH; i++) {
if (i == GYRO_BUFFER_LENGTH - 1)
I2C_AcknowledgeConfig(I2CX, DISABLE);
if (!I2C2_WaitEvent(I2C_EVENT_MASTER_BYTE_RECEIVED))
return -1;
buffer[i] = I2C_ReceiveData(I2CX);
}
return len;
I2C_GenerateSTOP(I2CX, ENABLE);
return 0;
}
#endif
+61 -33
View File
@@ -634,7 +634,67 @@ void pwrInit() { }
int usbPlugged() { return false; }
int getSelectedUsbMode() { return USB_JOYSTICK_MODE; }
void setSelectedUsbMode(int mode) {}
void delay_ms(uint32_t ms) { }
// GPIO fake functions
void GPIO_PinAFConfig(GPIO_TypeDef* GPIOx, uint16_t GPIO_PinSource, uint8_t GPIO_AF) { }
// PWR fake functions
void PWR_BackupAccessCmd(FunctionalState NewState) { }
void PWR_BackupRegulatorCmd(FunctionalState NewState) { }
// USART fake functions
void USART_DeInit(USART_TypeDef* ) { }
void USART_Init(USART_TypeDef* USARTx, USART_InitTypeDef* USART_InitStruct) { }
void USART_Cmd(USART_TypeDef* USARTx, FunctionalState NewState) { }
void USART_ClearITPendingBit(USART_TypeDef*, unsigned short) { }
void USART_SendData(USART_TypeDef* USARTx, uint16_t Data) { }
uint16_t USART_ReceiveData(USART_TypeDef*) { return 0; }
void USART_DMACmd(USART_TypeDef* USARTx, uint16_t USART_DMAReq, FunctionalState NewState) { }
void USART_ITConfig(USART_TypeDef* USARTx, uint16_t USART_IT, FunctionalState NewState) { }
FlagStatus USART_GetFlagStatus(USART_TypeDef* USARTx, uint16_t USART_FLAG) { return SET; }
// TIM fake functions
void TIM_DMAConfig(TIM_TypeDef* TIMx, uint16_t TIM_DMABase, uint16_t TIM_DMABurstLength) { }
void TIM_DMACmd(TIM_TypeDef* TIMx, uint16_t TIM_DMASource, FunctionalState NewState) { }
void TIM_CtrlPWMOutputs(TIM_TypeDef* TIMx, FunctionalState NewState) { }
// I2C fake functions
void I2C_DeInit(I2C_TypeDef*) { }
void I2C_Init(I2C_TypeDef*, I2C_InitTypeDef*) { }
void I2C_Cmd(I2C_TypeDef*, FunctionalState) { }
void I2C_Send7bitAddress(I2C_TypeDef*, unsigned char, unsigned char) { }
void I2C_SendData(I2C_TypeDef*, unsigned char) { }
void I2C_GenerateSTART(I2C_TypeDef*, FunctionalState) { }
void I2C_GenerateSTOP(I2C_TypeDef*, FunctionalState) { }
void I2C_AcknowledgeConfig(I2C_TypeDef*, FunctionalState) { }
uint8_t I2C_ReceiveData(I2C_TypeDef*) { return 0; }
ErrorStatus I2C_CheckEvent(I2C_TypeDef*, unsigned int) { return ERROR; }
// I2S fake functions
void I2S_Init(SPI_TypeDef* SPIx, I2S_InitTypeDef* I2S_InitStruct) { }
void I2S_Cmd(SPI_TypeDef* SPIx, FunctionalState NewState) { }
// SPI fake functions
void SPI_I2S_DeInit(SPI_TypeDef* SPIx) { }
void SPI_I2S_ITConfig(SPI_TypeDef* SPIx, uint8_t SPI_I2S_IT, FunctionalState NewState) { }
// RCC fake functions
void RCC_RTCCLKConfig(uint32_t RCC_RTCCLKSource) { }
void RCC_APB1PeriphClockCmd(uint32_t RCC_APB1Periph, FunctionalState NewState) { }
void RCC_RTCCLKCmd(FunctionalState NewState) { }
void RCC_PLLI2SConfig(uint32_t PLLI2SN, uint32_t PLLI2SR) { }
void RCC_PLLI2SCmd(FunctionalState NewState) { }
void RCC_I2SCLKConfig(uint32_t RCC_I2SCLKSource) { }
void RCC_LSEConfig(uint8_t RCC_LSE) { }
void RCC_GetClocksFreq(RCC_ClocksTypeDef* RCC_Clocks) { };
FlagStatus RCC_GetFlagStatus(uint8_t RCC_FLAG) { return SET; }
// RTC fake functions
ErrorStatus RTC_Init(RTC_InitTypeDef* RTC_InitStruct) { return SUCCESS; }
void RTC_TimeStructInit(RTC_TimeTypeDef* RTC_TimeStruct) { }
void RTC_DateStructInit(RTC_DateTypeDef* RTC_DateStruct) { }
ErrorStatus RTC_WaitForSynchro(void) { return SUCCESS; }
ErrorStatus RTC_SetTime(uint32_t RTC_Format, RTC_TimeTypeDef* RTC_TimeStruct) { return SUCCESS; }
ErrorStatus RTC_SetDate(uint32_t RTC_Format, RTC_DateTypeDef* RTC_DateStruct) { return SUCCESS; }
void RTC_GetTime(uint32_t RTC_Format, RTC_TimeTypeDef * RTC_TimeStruct)
@@ -657,39 +717,7 @@ void RTC_GetDate(uint32_t RTC_Format, RTC_DateTypeDef * RTC_DateStruct)
RTC_DateStruct->RTC_Date = timeinfo->tm_mday;
}
void RTC_TimeStructInit(RTC_TimeTypeDef* RTC_TimeStruct) { }
void RTC_DateStructInit(RTC_DateTypeDef* RTC_DateStruct) { }
void PWR_BackupAccessCmd(FunctionalState NewState) { }
void PWR_BackupRegulatorCmd(FunctionalState NewState) { }
void RCC_RTCCLKConfig(uint32_t RCC_RTCCLKSource) { }
void RCC_APB1PeriphClockCmd(uint32_t RCC_APB1Periph, FunctionalState NewState) { }
void RCC_RTCCLKCmd(FunctionalState NewState) { }
ErrorStatus RTC_Init(RTC_InitTypeDef* RTC_InitStruct) { return SUCCESS; }
void USART_SendData(USART_TypeDef* USARTx, uint16_t Data) { }
FlagStatus USART_GetFlagStatus(USART_TypeDef* USARTx, uint16_t USART_FLAG) { return SET; }
void GPIO_PinAFConfig(GPIO_TypeDef* GPIOx, uint16_t GPIO_PinSource, uint8_t GPIO_AF) { }
void USART_Init(USART_TypeDef* USARTx, USART_InitTypeDef* USART_InitStruct) { }
void USART_Cmd(USART_TypeDef* USARTx, FunctionalState NewState) { }
void USART_ClearITPendingBit(USART_TypeDef*, unsigned short) { }
uint16_t USART_ReceiveData(USART_TypeDef*) { return 0; }
void USART_DMACmd(USART_TypeDef* USARTx, uint16_t USART_DMAReq, FunctionalState NewState) { }
void USART_ITConfig(USART_TypeDef* USARTx, uint16_t USART_IT, FunctionalState NewState) { }
// void TIM_TimeBaseInit(TIM_TypeDef* TIMx, TIM_TimeBaseInitTypeDef* TIM_TimeBaseInitStruct) { }
// void TIM_OC1Init(TIM_TypeDef* TIMx, TIM_OCInitTypeDef* TIM_OCInitStruct) { }
void TIM_DMAConfig(TIM_TypeDef* TIMx, uint16_t TIM_DMABase, uint16_t TIM_DMABurstLength) { }
void TIM_DMACmd(TIM_TypeDef* TIMx, uint16_t TIM_DMASource, FunctionalState NewState) { }
void TIM_CtrlPWMOutputs(TIM_TypeDef* TIMx, FunctionalState NewState) { }
void RCC_PLLI2SConfig(uint32_t PLLI2SN, uint32_t PLLI2SR) { }
void RCC_PLLI2SCmd(FunctionalState NewState) { }
void RCC_I2SCLKConfig(uint32_t RCC_I2SCLKSource) { }
void SPI_I2S_DeInit(SPI_TypeDef* SPIx) { }
void I2S_Init(SPI_TypeDef* SPIx, I2S_InitTypeDef* I2S_InitStruct) { }
void I2S_Cmd(SPI_TypeDef* SPIx, FunctionalState NewState) { }
void SPI_I2S_ITConfig(SPI_TypeDef* SPIx, uint8_t SPI_I2S_IT, FunctionalState NewState) { }
void RCC_LSEConfig(uint8_t RCC_LSE) { }
void RCC_GetClocksFreq(RCC_ClocksTypeDef* RCC_Clocks) { };
FlagStatus RCC_GetFlagStatus(uint8_t RCC_FLAG) { return SET; }
ErrorStatus RTC_WaitForSynchro(void) { return SUCCESS; }
void unlockFlash() { }
void lockFlash() { }
void flashWrite(uint32_t *address, uint32_t *buffer) { simuSleep(100); }
+11 -7
View File
@@ -143,13 +143,17 @@ elseif(PCB STREQUAL XLITES)
set(STATUS_LEDS YES)
endif()
#if(PCB STREQUAL XLITES)
# add_definitions(-DGYRO)
# set(TARGET_SRC
# ${TARGET_SRC}
# ../common/arm/stm32/lsm6ds3_driver.cpp
# )
#endif()
if(PCB STREQUAL XLITES)
add_definitions(-DGYRO)
set(TARGET_SRC
${TARGET_SRC}
../common/arm/stm32/lsm6ds3_driver.cpp
)
set(SRC
${SRC}
gyro.cpp
)
endif()
if(PCB STREQUAL XLITE OR PCB STREQUAL XLITES OR PCB STREQUAL X3)
option(PXX1 "PXX1 protocol support" OFF)
+5
View File
@@ -290,6 +290,11 @@ void boardInit()
initHeadphoneTrainerSwitch();
vbattRTC = getRTCBattVoltage();
#if defined(GYRO)
gyroInit();
#endif
#endif // !defined(SIMU)
}
+6
View File
@@ -805,4 +805,10 @@ extern Fifo<uint8_t, TELEMETRY_FIFO_SIZE> telemetryFifo;
extern DMAFifo<32> serial2RxFifo;
#endif
// Gyro driver
#define GYRO_VALUES_COUNT 6
#define GYRO_BUFFER_LENGTH (GYRO_VALUES_COUNT * sizeof(int16_t))
int gyroInit();
int gyroRead(uint8_t buffer[GYRO_BUFFER_LENGTH]);
#endif // _BOARD_H_