From 4f8e7da449f4f1b16186ac94371ad2137ef939a2 Mon Sep 17 00:00:00 2001 From: Bertrand Songis Date: Tue, 26 Mar 2019 16:49:55 +0100 Subject: [PATCH] GET_HW_INFO refactoring --- radio/src/gui/128x64/model_setup.cpp | 5 +- radio/src/gui/128x64/radio_version.cpp | 6 +- radio/src/opentx.h | 3 +- radio/src/pulses/pxx2.cpp | 6 +- radio/src/pulses/pxx2.h | 2 + .../common/arm/stm32/lsm6ds3_driver.cpp | 313 ++++++++++++++++++ radio/src/targets/taranis/CMakeLists.txt | 8 + 7 files changed, 335 insertions(+), 8 deletions(-) create mode 100644 radio/src/targets/common/arm/stm32/lsm6ds3_driver.cpp diff --git a/radio/src/gui/128x64/model_setup.cpp b/radio/src/gui/128x64/model_setup.cpp index e8fbd9643..026da16ed 100644 --- a/radio/src/gui/128x64/model_setup.cpp +++ b/radio/src/gui/128x64/model_setup.cpp @@ -2092,9 +2092,10 @@ void menuModelReceiverOptions(event_t event) } } - if (event == EVT_KEY_LONG(KEY_ENTER)) { + if (event == EVT_KEY_LONG(KEY_ENTER) && reusableBuffer.receiverSettings.dirty) { + killEvents(event); reusableBuffer.receiverSettings.state = PXX2_SETTINGS_WRITE; - reusableBuffer.receiverSettings.dirty = 2; + reusableBuffer.receiverSettings.dirty = 0; reusableBuffer.receiverSettings.timeout = 0; moduleSettings[g_moduleIdx].mode = MODULE_MODE_RECEIVER_SETTINGS; } diff --git a/radio/src/gui/128x64/radio_version.cpp b/radio/src/gui/128x64/radio_version.cpp index d56256787..559ab6cff 100644 --- a/radio/src/gui/128x64/radio_version.cpp +++ b/radio/src/gui/128x64/radio_version.cpp @@ -106,12 +106,14 @@ void menuRadioModulesVersion(event_t event) if (isModulePXX2(INTERNAL_MODULE) && IS_INTERNAL_MODULE_ON()) { moduleSettings[INTERNAL_MODULE].mode = MODULE_MODE_GET_HARDWARE_INFO; - reusableBuffer.hardware.modules[INTERNAL_MODULE].step = -1; + reusableBuffer.hardware.modules[INTERNAL_MODULE].current = PXX2_HW_INFO_TX_ID; + reusableBuffer.hardware.modules[INTERNAL_MODULE].maximum = MAX_RECEIVERS_PER_MODULE - 1; } if (isModulePXX2(EXTERNAL_MODULE) && IS_EXTERNAL_MODULE_ON()) { moduleSettings[EXTERNAL_MODULE].mode = MODULE_MODE_GET_HARDWARE_INFO; - reusableBuffer.hardware.modules[EXTERNAL_MODULE].step = -1; + reusableBuffer.hardware.modules[EXTERNAL_MODULE].current = PXX2_HW_INFO_TX_ID; + reusableBuffer.hardware.modules[EXTERNAL_MODULE].maximum = MAX_RECEIVERS_PER_MODULE - 1; } reusableBuffer.hardware.updateTime = get_tmr10ms() + 1000 /* 10s*/; diff --git a/radio/src/opentx.h b/radio/src/opentx.h index cf5beb6ff..afb419228 100644 --- a/radio/src/opentx.h +++ b/radio/src/opentx.h @@ -1188,7 +1188,8 @@ union ReusableBuffer struct { struct { - int8_t step; + int8_t current; + int8_t maximum; uint8_t timeout; PXX2HardwareInformation information; struct { diff --git a/radio/src/pulses/pxx2.cpp b/radio/src/pulses/pxx2.cpp index 1e22e9245..066283e34 100644 --- a/radio/src/pulses/pxx2.cpp +++ b/radio/src/pulses/pxx2.cpp @@ -130,12 +130,12 @@ void Pxx2Pulses::setupTelemetryFrame(uint8_t module) void Pxx2Pulses::setupHardwareInfoFrame(uint8_t module) { - if (reusableBuffer.hardware.modules[module].step >= -1 && reusableBuffer.hardware.modules[module].step < PXX2_MAX_RECEIVERS_PER_MODULE) { + if (reusableBuffer.hardware.modules[module].current <= reusableBuffer.hardware.modules[module].maximum) { if (reusableBuffer.hardware.modules[module].timeout == 0) { addFrameType(PXX2_TYPE_C_MODULE, PXX2_TYPE_ID_HW_INFO); - Pxx2Transport::addByte(reusableBuffer.hardware.modules[module].step); + Pxx2Transport::addByte(reusableBuffer.hardware.modules[module].current); reusableBuffer.hardware.modules[module].timeout = 20; - reusableBuffer.hardware.modules[module].step++; + reusableBuffer.hardware.modules[module].current++; } else { reusableBuffer.hardware.modules[module].timeout--; diff --git a/radio/src/pulses/pxx2.h b/radio/src/pulses/pxx2.h index de1580d8f..f41e821b1 100644 --- a/radio/src/pulses/pxx2.h +++ b/radio/src/pulses/pxx2.h @@ -53,6 +53,8 @@ #define PXX2_TX_SETTINGS_FLAG0_WRITE (1 << 6) #define PXX2_TX_SETTINGS_FLAG1_EXTERNAL_ANTENNA (1 << 3) +#define PXX2_HW_INFO_TX_ID 0xFF + enum PXX2RegisterSteps { REGISTER_START, REGISTER_RX_NAME_RECEIVED, diff --git a/radio/src/targets/common/arm/stm32/lsm6ds3_driver.cpp b/radio/src/targets/common/arm/stm32/lsm6ds3_driver.cpp new file mode 100644 index 000000000..98a5b15d5 --- /dev/null +++ b/radio/src/targets/common/arm/stm32/lsm6ds3_driver.cpp @@ -0,0 +1,313 @@ +/* + * 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 "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_INT1_FULL 0x20 +#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_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_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_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 + +/* 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_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_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_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_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_DRDY_IRQ_MASK 0x40 + +/* CUSTOM VALUES FOR STEP DETECTOR SENSOR */ +#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 + +/* 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_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_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 0xD4 + +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; + +int lsm6ds3Init() +{ + int i; + char cmd[2]; + + I2C_transfer_block i2c_block[2]; + + EXTILine_Config(); + + cmd[0] = LSM6DS3_WHO_AM_I; + + 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; + + 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; + + i2c_dma_transfer(i2c_block, 2); + + if (cmd[0] != LSM6DS3_WHO_AM_I_DEF) { + 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_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); + + 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); + } + + return 0; +} + +int lsm6ds3Read(void *buffer, int len, OFFSET offset1) +{ + char t_char[10]; + short t_short[7]; + + short *p_short; + I2C_transfer_block i2c_block[2]; + + xSemaphoreTake(lsm6ds3_sema, portMAX_DELAY); + + t_char[0] = LSM6DS3_GYRO_OUT_X_L_ADDR; + + 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_block[1].addr = LSM6DS3_ADDRESS; + i2c_block[1].rw = I2C_READ; + i2c_block[1].stop_mode = I2C_WRITE_APPEND_STOP; + + 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]; + } + + return len; +} diff --git a/radio/src/targets/taranis/CMakeLists.txt b/radio/src/targets/taranis/CMakeLists.txt index f6aefc9c6..0a6a952eb 100644 --- a/radio/src/targets/taranis/CMakeLists.txt +++ b/radio/src/targets/taranis/CMakeLists.txt @@ -143,6 +143,14 @@ 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 XLITE OR PCB STREQUAL XLITES OR PCB STREQUAL X3) option(PXX1 "PXX1 protocol support" OFF) option(PXX2 "PXX2 protocol support" ON)