From 8da1418cae3afed6c2cd0b97ef3770b4a1a341ae Mon Sep 17 00:00:00 2001 From: 3djc Date: Sun, 7 Apr 2019 13:17:51 +0200 Subject: [PATCH 1/4] Add rssi choice --- radio/src/datastructs.h | 15 ++++++++------- radio/src/gui/128x64/model_telemetry.cpp | 16 +++++++++++++++- radio/src/gui/gui_common.h | 1 + radio/src/gui/gui_common_arm.cpp | 11 +++++++++++ radio/src/telemetry/frsky_sport.cpp | 11 +++++++++-- 5 files changed, 44 insertions(+), 10 deletions(-) diff --git a/radio/src/datastructs.h b/radio/src/datastructs.h index 1d04ef74b..8873e5259 100644 --- a/radio/src/datastructs.h +++ b/radio/src/datastructs.h @@ -335,6 +335,7 @@ PACK(struct FrSkyTelemetryData { int8_t varioCenterMin; int8_t varioMin; int8_t varioMax; + uint8_t rssiSource; }); #endif @@ -832,7 +833,7 @@ static inline void check_struct() CHKSIZE(FrSkyBarData, 6); CHKSIZE(FrSkyLineData, 4); CHKTYPE(union FrSkyScreenData, 24); - CHKSIZE(FrSkyTelemetryData, 104); + CHKSIZE(FrSkyTelemetryData, 105); CHKSIZE(ModelHeader, 12); CHKSIZE(CurveData, 4); #elif defined(PCBTARANIS) @@ -906,25 +907,25 @@ static inline void check_struct() #if defined(PCBXLITES) CHKSIZE(RadioData, 860); - CHKSIZE(ModelData, 6054); + CHKSIZE(ModelData, 6055); #elif defined(PCBXLITE) CHKSIZE(RadioData, 852); - CHKSIZE(ModelData, 6054); + CHKSIZE(ModelData, 6055); #elif defined(PCBX7) CHKSIZE(RadioData, 858); - CHKSIZE(ModelData, 6054); + CHKSIZE(ModelData, 6055); #elif defined(PCBX9E) CHKSIZE(RadioData, 960); - CHKSIZE(ModelData, 6549); + CHKSIZE(ModelData, 6550); #elif defined(PCBX9D) || defined(PCBX9DP) CHKSIZE(RadioData, 880); - CHKSIZE(ModelData, 6536); + CHKSIZE(ModelData, 6537); #elif defined(PCBSKY9X) CHKSIZE(RadioData, 735); CHKSIZE(ModelData, 5215); #elif defined(PCBHORUS) CHKSIZE(RadioData, 855); - CHKSIZE(ModelData, 9721); + CHKSIZE(ModelData, 9722); #endif #undef CHKSIZE diff --git a/radio/src/gui/128x64/model_telemetry.cpp b/radio/src/gui/128x64/model_telemetry.cpp index 5aa0672ae..45d3c7471 100644 --- a/radio/src/gui/128x64/model_telemetry.cpp +++ b/radio/src/gui/128x64/model_telemetry.cpp @@ -26,6 +26,7 @@ enum MenuModelTelemetryFrskyItems { ITEM_TELEMETRY_INVERTED_SERIAL, #endif ITEM_TELEMETRY_RSSI_LABEL, + ITEM_TELEMETRY_RSSI_SOURCE, ITEM_TELEMETRY_RSSI_ALARM1, ITEM_TELEMETRY_RSSI_ALARM2, ITEM_TELEMETRY_DISABLE_ALARMS, @@ -57,7 +58,7 @@ enum MenuModelTelemetryFrskyItems { #define USRDATA_ROWS -#define RSSI_ROWS LABEL(RSSI), 0, 0, 1, +#define RSSI_ROWS LABEL(RSSI), 0, 0, 0, 1, #define SCREEN_TYPE_ROWS 0 @@ -254,6 +255,19 @@ void menuModelTelemetryFrsky(event_t event) lcdDrawTextAlignedLeft(y, "RSSI"); break; + case ITEM_TELEMETRY_RSSI_SOURCE: { + lcdDrawTextAlignedLeft(y, STR_SOURCE); + drawSource(TELEM_COL2, y, g_model.frsky.rssiSource ? MIXSRC_FIRST_TELEM + 3 * (g_model.frsky.rssiSource - 1) : 0, attr); + if (g_model.frsky.rssiSource) { + TelemetrySensor * sensor = &g_model.telemetrySensors[g_model.frsky.rssiSource]; + drawReceiverName(TELEM_COL2 + 5 * FW, y, (sensor->instance >> 7) & 0x01, (sensor->instance >> 5) & 0x03, 0); + } + if (attr) { + g_model.frsky.rssiSource = checkIncDec(event, g_model.frsky.rssiSource, 0, MAX_TELEMETRY_SENSORS, EE_MODEL | NO_INCDEC_MARKS, isRssiSensorAvailable); + } + break; + } + case ITEM_TELEMETRY_RSSI_ALARM1: case ITEM_TELEMETRY_RSSI_ALARM2: { bool warning = (k==ITEM_TELEMETRY_RSSI_ALARM1); diff --git a/radio/src/gui/gui_common.h b/radio/src/gui/gui_common.h index 8a1b5dccd..c44969128 100644 --- a/radio/src/gui/gui_common.h +++ b/radio/src/gui/gui_common.h @@ -72,6 +72,7 @@ bool isCurrentSensor(int sensor); bool isTelemetryFieldAvailable(int index); bool isTelemetryFieldComparisonAvailable(int index); bool isSensorAvailable(int sensor); +bool isRssiSensorAvailable(int sensor); bool modelHasNotes(); diff --git a/radio/src/gui/gui_common_arm.cpp b/radio/src/gui/gui_common_arm.cpp index 278da5772..657755181 100644 --- a/radio/src/gui/gui_common_arm.cpp +++ b/radio/src/gui/gui_common_arm.cpp @@ -46,6 +46,17 @@ bool isInputAvailable(int input) return false; } +bool isRssiSensorAvailable(int sensor) +{ + if (sensor == 0) + return true; + else { + TelemetrySensor &telemSensor = g_model.telemetrySensors[abs(sensor) - 1]; + return (telemSensor.isAvailable() && telemSensor.id == RSSI_ID); + } +} + + bool isSensorAvailable(int sensor) { if (sensor == 0) diff --git a/radio/src/telemetry/frsky_sport.cpp b/radio/src/telemetry/frsky_sport.cpp index cce6cc737..5b5966b1d 100644 --- a/radio/src/telemetry/frsky_sport.cpp +++ b/radio/src/telemetry/frsky_sport.cpp @@ -164,8 +164,15 @@ void sportProcessTelemetryPacketWithoutCrc(uint8_t origin, const uint8_t * packe data = SPORT_DATA_U8(packet); if (data == 0) telemetryData.rssi.reset(); - else - telemetryData.rssi.set(data); + else { + if(g_model.frsky.rssiSource) { + TelemetrySensor * sensor = & g_model.telemetrySensors[g_model.frsky.rssiSource]; + if(sensor->instance == instance) + telemetryData.rssi.set(data); + } + else + telemetryData.rssi.set(data); + } } else if (dataId == R9_PWR_ID) { uint32_t r9pwr[] = {100, 200, 500, 1000}; From 71b26637d1a2f8563ce4c39d405ae57521167074 Mon Sep 17 00:00:00 2001 From: 3djc Date: Sun, 7 Apr 2019 14:28:12 +0200 Subject: [PATCH 2/4] Compilation fixes --- radio/src/datastructs.h | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/radio/src/datastructs.h b/radio/src/datastructs.h index 8873e5259..336ddd3ca 100644 --- a/radio/src/datastructs.h +++ b/radio/src/datastructs.h @@ -321,6 +321,7 @@ PACK(struct FrSkyTelemetryData { // TODO EEPROM change, rename to VarioData int8_t varioCenterMin; int8_t varioMin; int8_t varioMax; + uint8_t rssiSource; }); #else // TODO remove this also on Taranis @@ -848,7 +849,7 @@ static inline void check_struct() CHKSIZE(FrSkyBarData, 6); CHKSIZE(FrSkyLineData, 6); CHKTYPE(union FrSkyScreenData, 24); - CHKSIZE(FrSkyTelemetryData, 104); + CHKSIZE(FrSkyTelemetryData, 105); CHKSIZE(ModelHeader, 24); CHKSIZE(CurveData, 4); #elif defined(PCBHORUS) @@ -859,7 +860,7 @@ static inline void check_struct() CHKSIZE(FlightModeData, 44); CHKSIZE(TimerData, 16); CHKSIZE(SwashRingData, 8); - CHKSIZE(FrSkyTelemetryData, 5); + CHKSIZE(FrSkyTelemetryData, 6); CHKSIZE(ModelHeader, 27); CHKSIZE(CurveData, 4); CHKSIZE(CustomScreenData, 610); @@ -874,7 +875,7 @@ static inline void check_struct() CHKSIZE(SwashRingData, 8); CHKSIZE(FrSkyBarData, 5); CHKSIZE(FrSkyLineData, 2); - CHKSIZE(FrSkyTelemetryData, 88); + CHKSIZE(FrSkyTelemetryData, 89); CHKSIZE(ModelHeader, 12); CHKTYPE(CurveData, 4); #else @@ -922,7 +923,7 @@ static inline void check_struct() CHKSIZE(ModelData, 6537); #elif defined(PCBSKY9X) CHKSIZE(RadioData, 735); - CHKSIZE(ModelData, 5215); + CHKSIZE(ModelData, 5216); #elif defined(PCBHORUS) CHKSIZE(RadioData, 855); CHKSIZE(ModelData, 9722); From 06c523547efe6bb02bbe698844a684ccaac267dc Mon Sep 17 00:00:00 2001 From: 3djc Date: Sun, 7 Apr 2019 16:58:26 +0200 Subject: [PATCH 3/4] Accept both lsm6d33tr and lsm6dsltr --- .../common/arm/stm32/lsm6ds3_driver.cpp | 359 ----------------- .../common/arm/stm32/lsm6ds_driver.cpp | 360 ++++++++++++++++++ radio/src/targets/taranis/CMakeLists.txt | 2 +- 3 files changed, 361 insertions(+), 360 deletions(-) delete mode 100644 radio/src/targets/common/arm/stm32/lsm6ds3_driver.cpp create mode 100644 radio/src/targets/common/arm/stm32/lsm6ds_driver.cpp diff --git a/radio/src/targets/common/arm/stm32/lsm6ds3_driver.cpp b/radio/src/targets/common/arm/stm32/lsm6ds3_driver.cpp deleted file mode 100644 index 174499b1a..000000000 --- a/radio/src/targets/common/arm/stm32/lsm6ds3_driver.cpp +++ /dev/null @@ -1,359 +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 "opentx.h" - - -/* 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 0xD6 -#define LSM6DSLTR_ID 0x6A - -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() -{ - 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 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; -} - -bool I2C2_WaitEventCleared(uint32_t event) -{ - uint32_t timeout = I2C_TIMEOUT_MAX; - while (I2C_CheckEvent(I2CX, event)) { - if ((timeout--) == 0) { - TRACE("I2C Timeout!"); - return false; - } - } - return true; -} - -int setGyroRegister(uint8_t address, uint8_t value) -{ - 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_TRANSMITTING)) - return -1; - - I2C_SendData(I2CX, value); - if (!I2C2_WaitEvent(I2C_EVENT_MASTER_BYTE_TRANSMITTED)) - return -1; - - I2C_GenerateSTOP(I2CX, ENABLE); - - return 0; -} - -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; - } - - setGyroRegister(0x12, 0x05); - delay_ms(1); - setGyroRegister(0x12, 0x04); - delay_ms(1); - - for (uint8_t i = 0; i < DIM(configure); i++) { - setGyroRegister(configure[i][0], configure[i][1]); - } - - return 0; -} - -int gyroRead(uint8_t buffer[GYRO_BUFFER_LENGTH]) -{ - 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, LSM6DS3_GYRO_OUT_X_L_ADDR); - 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, ENABLE); - for (uint8_t i=0; i Date: Tue, 9 Apr 2019 07:08:27 +0200 Subject: [PATCH 4/4] Remove unnecessary code --- radio/src/telemetry/frsky_sport.cpp | 14 +++++--------- 1 file changed, 5 insertions(+), 9 deletions(-) diff --git a/radio/src/telemetry/frsky_sport.cpp b/radio/src/telemetry/frsky_sport.cpp index 5b5966b1d..8b73386c9 100644 --- a/radio/src/telemetry/frsky_sport.cpp +++ b/radio/src/telemetry/frsky_sport.cpp @@ -162,17 +162,13 @@ void sportProcessTelemetryPacketWithoutCrc(uint8_t origin, const uint8_t * packe if (dataId == RSSI_ID) { telemetryStreaming = TELEMETRY_TIMEOUT10ms; // reset counter only if valid packets are being detected data = SPORT_DATA_U8(packet); - if (data == 0) - telemetryData.rssi.reset(); - else { - if(g_model.frsky.rssiSource) { - TelemetrySensor * sensor = & g_model.telemetrySensors[g_model.frsky.rssiSource]; - if(sensor->instance == instance) - telemetryData.rssi.set(data); - } - else + if(g_model.frsky.rssiSource) { + TelemetrySensor * sensor = & g_model.telemetrySensors[g_model.frsky.rssiSource]; + if(sensor->instance == instance) telemetryData.rssi.set(data); } + else + telemetryData.rssi.set(data); } else if (dataId == R9_PWR_ID) { uint32_t r9pwr[] = {100, 200, 500, 1000};