diff --git a/docs/Settings.md b/docs/Settings.md
index ce4ff9c1ce4..f52da9c2c4b 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -5178,7 +5178,7 @@ Value below which Crossfire SNR Alarm pops-up. (dB)
| Default | Min | Max |
| --- | --- | --- |
-| 4 | -20 | 10 |
+| 4 | -20 | 99 |
---
diff --git a/src/main/cms/cms_menu_osd.c b/src/main/cms/cms_menu_osd.c
index e99e0cc52f2..0e85d0e656d 100644
--- a/src/main/cms/cms_menu_osd.c
+++ b/src/main/cms/cms_menu_osd.c
@@ -147,10 +147,10 @@ static const OSD_Entry menuCrsfRxEntries[]=
OSD_SETTING_ENTRY("LQ ALARM LEVEL", SETTING_OSD_LINK_QUALITY_ALARM),
OSD_SETTING_ENTRY("SNR ALARM LEVEL", SETTING_OSD_SNR_ALARM),
OSD_SETTING_ENTRY("RX SENSITIVITY", SETTING_OSD_RSSI_DBM_MIN),
- OSD_ELEMENT_ENTRY("RX RSSI DBM", OSD_CRSF_RSSI_DBM),
- OSD_ELEMENT_ENTRY("RX LQ", OSD_CRSF_LQ),
- OSD_ELEMENT_ENTRY("RX SNR ALARM", OSD_CRSF_SNR_DB),
- OSD_ELEMENT_ENTRY("TX POWER", OSD_CRSF_TX_POWER),
+ OSD_ELEMENT_ENTRY("RX RSSI DBM", OSD_RSSI_DBM),
+ OSD_ELEMENT_ENTRY("RX LQ", OSD_LQ_UPLINK),
+ OSD_ELEMENT_ENTRY("RX SNR ALARM", OSD_SNR_DB),
+ OSD_ELEMENT_ENTRY("TX POWER", OSD_TX_POWER_UPLINK),
OSD_BACK_AND_END_ENTRY,
};
diff --git a/src/main/common/streambuf.c b/src/main/common/streambuf.c
index 7a4e8c8cc99..5a766423421 100644
--- a/src/main/common/streambuf.c
+++ b/src/main/common/streambuf.c
@@ -98,6 +98,11 @@ uint8_t sbufReadU8(sbuf_t *src)
return *src->ptr++;
}
+int8_t sbufReadI8(sbuf_t *src)
+{
+ return *src->ptr++;
+}
+
uint16_t sbufReadU16(sbuf_t *src)
{
uint16_t ret;
diff --git a/src/main/common/streambuf.h b/src/main/common/streambuf.h
index 74331147da8..a2ac1f681a6 100644
--- a/src/main/common/streambuf.h
+++ b/src/main/common/streambuf.h
@@ -42,6 +42,7 @@ void sbufWriteU16BigEndian(sbuf_t *dst, uint16_t val);
void sbufWriteU32BigEndian(sbuf_t *dst, uint32_t val);
uint8_t sbufReadU8(sbuf_t *src);
+int8_t sbufReadI8(sbuf_t *src);
uint16_t sbufReadU16(sbuf_t *src);
uint32_t sbufReadU32(sbuf_t *src);
void sbufReadData(const sbuf_t *dst, void *data, int len);
diff --git a/src/main/drivers/accgyro/accgyro_icm42605.c b/src/main/drivers/accgyro/accgyro_icm42605.c
index 13bc22fcee7..d0c4e3490aa 100644
--- a/src/main/drivers/accgyro/accgyro_icm42605.c
+++ b/src/main/drivers/accgyro/accgyro_icm42605.c
@@ -62,6 +62,7 @@
#define ICM42605_RA_GYRO_DATA_X1 0x25
#define ICM42605_RA_ACCEL_DATA_X1 0x1F
+#define ICM42605_RA_TEMP_DATA1 0x1D
#define ICM42605_RA_INT_CONFIG 0x14
#define ICM42605_INT1_MODE_PULSED (0 << 2)
@@ -321,6 +322,20 @@ static bool icm42605GyroRead(gyroDev_t *gyro)
return true;
}
+static bool icm42605ReadTemperature(gyroDev_t *gyro, int16_t * temp)
+{
+ uint8_t data[2];
+
+ const bool ack = busReadBuf(gyro->busDev, ICM42605_RA_TEMP_DATA1, data, 2);
+ if (!ack) {
+ return false;
+ }
+ // From datasheet: Temperature in Degrees Centigrade = (TEMP_DATA / 132.48) + 25
+ *temp = ( int16_val_big_endian(data, 0) / 13.248 ) + 250; // Temperature stored as degC*10
+
+ return true;
+}
+
bool icm42605GyroDetect(gyroDev_t *gyro)
{
gyro->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_ICM42605, gyro->imuSensorToUse, OWNER_MPU);
@@ -340,7 +355,7 @@ bool icm42605GyroDetect(gyroDev_t *gyro)
gyro->initFn = icm42605AccAndGyroInit;
gyro->readFn = icm42605GyroRead;
gyro->intStatusFn = gyroCheckDataReady;
- gyro->temperatureFn = NULL;
+ gyro->temperatureFn = icm42605ReadTemperature;
gyro->scale = 1.0f / 16.4f; // 16.4 dps/lsb scalefactor
gyro->gyroAlign = gyro->busDev->param;
diff --git a/src/main/drivers/barometer/barometer_dps310.c b/src/main/drivers/barometer/barometer_dps310.c
index 51e18eb12b0..663dcb27117 100644
--- a/src/main/drivers/barometer/barometer_dps310.c
+++ b/src/main/drivers/barometer/barometer_dps310.c
@@ -61,6 +61,7 @@
#define DPS310_ID_REV_AND_PROD_ID (0x10)
+#define SPL07_003_CHIP_ID (0x11)
#define DPS310_RESET_BIT_SOFT_RST (0x09) // 0b1001
@@ -96,6 +97,8 @@ typedef struct {
int16_t c20; // 16bit
int16_t c21; // 16bit
int16_t c30; // 16bit
+ int16_t c31; // 12bit
+ int16_t c40; // 12bit
} calibrationCoefficients_t;
typedef struct {
@@ -105,6 +108,7 @@ typedef struct {
} baroState_t;
static baroState_t baroState;
+static uint8_t chipId[1];
// Helper functions
@@ -167,7 +171,10 @@ static bool deviceConfigure(busDevice_t * busDev)
// 1. Read the pressure calibration coefficients (c00, c10, c20, c30, c01, c11, and c21) from the Calibration Coefficient register.
// Note: The coefficients read from the coefficient register are 2's complement numbers.
- uint8_t coef[18];
+
+ unsigned coefficientLength = chipId[0] == SPL07_003_CHIP_ID ? 21 : 18;
+ uint8_t coef[coefficientLength];
+
if (!busReadBuf(busDev, DPS310_REG_COEF, coef, sizeof(coef))) {
return false;
}
@@ -199,6 +206,17 @@ static bool deviceConfigure(busDevice_t * busDev)
// 0x20 c30 [15:8] + 0x21 c30 [7:0]
baroState.calib.c30 = getTwosComplement(((uint32_t)coef[16] << 8) | (uint32_t)coef[17], 16);
+ if (chipId[0] == SPL07_003_CHIP_ID) {
+ // 0x23 c31 [3:0] + 0x22 c31 [11:4]
+ baroState.calib.c31 = getTwosComplement(((uint32_t)coef[18] << 4) | (((uint32_t)coef[19] >> 4) & 0x0F), 12);
+
+ // 0x23 c40 [11:8] + 0x24 c40 [7:0]
+ baroState.calib.c40 = getTwosComplement((((uint32_t)coef[19] & 0x0F) << 8) | (uint32_t)coef[20], 12);
+ } else {
+ baroState.calib.c31 = 0;
+ baroState.calib.c40 = 0;
+ }
+
// MEAS_CFG: Make sure the device is in IDLE mode
registerWriteBits(busDev, DPS310_REG_MEAS_CFG, DPS310_MEAS_CFG_MEAS_CTRL_MASK, DPS310_MEAS_CFG_MEAS_IDLE);
@@ -218,8 +236,12 @@ static bool deviceConfigure(busDevice_t * busDev)
registerSetBits(busDev, DPS310_REG_PRS_CFG, DPS310_PRS_CFG_BIT_PM_RATE_32HZ | DPS310_PRS_CFG_BIT_PM_PRC_16);
// TMP_CFG: temperature measurement rate (32 Hz) and oversampling (16 times)
- const uint8_t TMP_COEF_SRCE = registerRead(busDev, DPS310_REG_COEF_SRCE) & DPS310_COEF_SRCE_BIT_TMP_COEF_SRCE;
- registerSetBits(busDev, DPS310_REG_TMP_CFG, DPS310_TMP_CFG_BIT_TMP_RATE_32HZ | DPS310_TMP_CFG_BIT_TMP_PRC_16 | TMP_COEF_SRCE);
+ if (chipId[0] == SPL07_003_CHIP_ID) {
+ registerSetBits(busDev, DPS310_REG_TMP_CFG, DPS310_TMP_CFG_BIT_TMP_RATE_32HZ | DPS310_TMP_CFG_BIT_TMP_PRC_16);
+ } else {
+ const uint8_t TMP_COEF_SRCE = registerRead(busDev, DPS310_REG_COEF_SRCE) & DPS310_COEF_SRCE_BIT_TMP_COEF_SRCE;
+ registerSetBits(busDev, DPS310_REG_TMP_CFG, DPS310_TMP_CFG_BIT_TMP_RATE_32HZ | DPS310_TMP_CFG_BIT_TMP_PRC_16 | TMP_COEF_SRCE);
+ }
// CFG_REG: set pressure and temperature result bit-shift (required when the oversampling rate is >8 times)
registerSetBits(busDev, DPS310_REG_CFG_REG, DPS310_CFG_REG_BIT_T_SHIFT | DPS310_CFG_REG_BIT_P_SHIFT);
@@ -265,9 +287,17 @@ static bool deviceReadMeasurement(baroDev_t *baro)
const float c20 = baroState.calib.c20;
const float c21 = baroState.calib.c21;
const float c30 = baroState.calib.c30;
+ const float c31 = baroState.calib.c31;
+ const float c40 = baroState.calib.c40;
// See section 4.9.1, How to Calculate Compensated Pressure Values, of datasheet
- baroState.pressure = c00 + Praw_sc * (c10 + Praw_sc * (c20 + Praw_sc * c30)) + Traw_sc * c01 + Traw_sc * Praw_sc * (c11 + Praw_sc * c21);
+ // baroState.pressure = c00 + Praw_sc * (c10 + Praw_sc * (c20 + Praw_sc * c30)) + Traw_sc * c01 + Traw_sc * Praw_sc * (c11 + Praw_sc * c21);
+ if (chipId[0] == SPL07_003_CHIP_ID) {
+ baroState.pressure = c00 + Praw_sc * (c10 + Praw_sc * (c20 + Praw_sc * (c30 + Praw_sc * c40))) + Traw_sc * c01 + Traw_sc * Praw_sc * (c11 + Praw_sc * (c21 + Praw_sc * c31));
+ } else {
+ baroState.pressure = c00 + Praw_sc * (c10 + Praw_sc * (c20 + Praw_sc * c30)) + Traw_sc * c01 + Traw_sc * Praw_sc * (c11 + Praw_sc * c21);
+ }
+
const float c0 = baroState.calib.c0;
const float c1 = baroState.calib.c1;
@@ -299,13 +329,11 @@ static bool deviceCalculate(baroDev_t *baro, int32_t *pressure, int32_t *tempera
static bool deviceDetect(busDevice_t * busDev)
{
for (int retry = 0; retry < DETECTION_MAX_RETRY_COUNT; retry++) {
- uint8_t chipId[1];
-
delay(100);
bool ack = busReadBuf(busDev, DPS310_REG_ID, chipId, 1);
- if (ack && chipId[0] == DPS310_ID_REV_AND_PROD_ID) {
+ if (ack && (chipId[0] == DPS310_ID_REV_AND_PROD_ID || chipId[0] == SPL07_003_CHIP_ID)) {
return true;
}
};
diff --git a/src/main/drivers/osd_symbols.h b/src/main/drivers/osd_symbols.h
index 90c0bc97131..fe673c93a57 100644
--- a/src/main/drivers/osd_symbols.h
+++ b/src/main/drivers/osd_symbols.h
@@ -234,6 +234,8 @@
#define SYM_AH_CH_CENTER 0x166 // 358 Crossair center
#define SYM_FLIGHT_DIST_REMAINING 0x167 // 359 Flight distance reminaing
#define SYM_ODOMETER 0x168 // 360 Odometer
+#define SYM_RX_BAND 0x169 // 361 RX Band
+#define SYM_RX_MODE 0x16A // 362 RX Mode
#define SYM_AH_CH_TYPE3 0x190 // 400 to 402, crosshair 3
#define SYM_AH_CH_TYPE4 0x193 // 403 to 405, crosshair 4
diff --git a/src/main/drivers/uart_inverter.c b/src/main/drivers/uart_inverter.c
index b815883df79..5168831b471 100644
--- a/src/main/drivers/uart_inverter.c
+++ b/src/main/drivers/uart_inverter.c
@@ -145,7 +145,12 @@ void uartInverterSet(USART_TypeDef *USARTx, uartInverterLine_e line, bool enable
// UART4
#if defined(INVERTER_PIN_UART4_RX) || defined(INVERTER_PIN_UART4_TX)
- if (USARTx == USART4) {
+#if defined(STM32F4)
+ if (USARTx == UART4)
+#else
+ if (USARTx == USART4)
+#endif
+ {
#if defined(INVERTER_PIN_UART4_RX)
rx_pin = IOGetByTag(IO_TAG(INVERTER_PIN_UART4_RX));
#endif
diff --git a/src/main/drivers/vtx_common.h b/src/main/drivers/vtx_common.h
index 80b957c5d13..83e608dc49b 100644
--- a/src/main/drivers/vtx_common.h
+++ b/src/main/drivers/vtx_common.h
@@ -36,11 +36,11 @@
#define VTX_SETTINGS_POWER_COUNT 8
#define VTX_SETTINGS_DEFAULT_POWER 1
-#define VTX_SETTINGS_MIN_POWER 1
+#define VTX_SETTINGS_MIN_POWER 0
#define VTX_SETTINGS_MIN_USER_FREQ 5000
#define VTX_SETTINGS_MAX_USER_FREQ 5999
#define VTX_SETTINGS_FREQCMD
-#define VTX_SETTINGS_MAX_POWER (VTX_SETTINGS_POWER_COUNT - VTX_SETTINGS_MIN_POWER + 1)
+#define VTX_SETTINGS_MAX_POWER (VTX_SETTINGS_POWER_COUNT - VTX_SETTINGS_MIN_POWER)
#else
diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index f87b7f079c7..f490d28128a 100644
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -34,6 +34,7 @@
#include "common/color.h"
#include "common/maths.h"
#include "common/streambuf.h"
+#include "common/string_light.h"
#include "common/bitarray.h"
#include "common/time.h"
#include "common/utils.h"
@@ -215,7 +216,7 @@ static void mspSerialPassthroughFn(serialPort_t *serialPort)
static void mspFcSetPassthroughCommand(sbuf_t *dst, sbuf_t *src, mspPostProcessFnPtr *mspPostProcessFn)
{
- const unsigned int dataSize = sbufBytesRemaining(src);
+ const unsigned int dataSize = sbufBytesRemaining(src); /* Payload size in Bytes */
if (dataSize == 0) {
// Legacy format
@@ -1807,7 +1808,7 @@ static void mspFcWaypointOutCommand(sbuf_t *dst, sbuf_t *src)
#ifdef USE_FLASHFS
static void mspFcDataFlashReadCommand(sbuf_t *dst, sbuf_t *src)
{
- const unsigned int dataSize = sbufBytesRemaining(src);
+ const unsigned int dataSize = sbufBytesRemaining(src); /* Payload size in Bytes */
uint16_t readLength;
const uint32_t readAddress = sbufReadU32(src);
@@ -1831,7 +1832,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
uint8_t tmp_u8;
uint16_t tmp_u16;
- const unsigned int dataSize = sbufBytesRemaining(src);
+ const unsigned int dataSize = sbufBytesRemaining(src); /* Payload size in Bytes */
switch (cmdMSP) {
case MSP_SELECT_SETTING:
@@ -2692,21 +2693,15 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
if (newFrequency <= VTXCOMMON_MSP_BANDCHAN_CHKVAL) { //value is band and channel
const uint8_t newBand = (newFrequency / 8) + 1;
const uint8_t newChannel = (newFrequency % 8) + 1;
-
- if(vtxSettingsConfig()->band != newBand || vtxSettingsConfig()->channel != newChannel) {
- vtxCommonSetBandAndChannel(vtxDevice, newBand, newChannel);
+ if (vtxSettingsConfig()->band != newBand || vtxSettingsConfig()->channel != newChannel) {
+ vtxSettingsConfigMutable()->band = newBand;
+ vtxSettingsConfigMutable()->channel = newChannel;
}
-
- vtxSettingsConfigMutable()->band = newBand;
- vtxSettingsConfigMutable()->channel = newChannel;
}
if (sbufBytesRemaining(src) > 1) {
uint8_t newPower = sbufReadU8(src);
- uint8_t currentPower = 0;
- vtxCommonGetPowerIndex(vtxDevice, ¤tPower);
- if (newPower != currentPower) {
- vtxCommonSetPowerByIndex(vtxDevice, newPower);
+ if (vtxSettingsConfig()->power != newPower) {
vtxSettingsConfigMutable()->power = newPower;
}
@@ -2722,9 +2717,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
vtxSettingsConfigMutable()->lowPowerDisarm = sbufReadU8(src);
}
- // //////////////////////////////////////////////////////////
- // this code is taken from BF, it's hack for HDZERO VTX MSP frame
- // API version 1.42 - this parameter kept separate since clients may already be supplying
+ // API version 1.42 - extension for pitmode frequency
if (sbufBytesRemaining(src) >= 2) {
sbufReadU16(src); //skip pitModeFreq
}
@@ -2732,18 +2725,29 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
// API version 1.42 - extensions for non-encoded versions of the band, channel or frequency
if (sbufBytesRemaining(src) >= 4) {
uint8_t newBand = sbufReadU8(src);
+ if (vtxSettingsConfig()->band != newBand) {
+ vtxSettingsConfigMutable()->band = newBand;
+ }
+
const uint8_t newChannel = sbufReadU8(src);
- vtxSettingsConfigMutable()->band = newBand;
- vtxSettingsConfigMutable()->channel = newChannel;
+ if (vtxSettingsConfig()->channel != newChannel) {
+ vtxSettingsConfigMutable()->channel = newChannel;
+ }
}
- /* if (sbufBytesRemaining(src) >= 4) {
- sbufRead8(src); // freq_l
- sbufRead8(src); // freq_h
- sbufRead8(src); // band count
- sbufRead8(src); // channel count
- }*/
- // //////////////////////////////////////////////////////////
+ if (sbufBytesRemaining(src) >= 2) {
+ sbufReadU16(src); // freq
+ }
+
+ if (sbufBytesRemaining(src) >= 3) {
+ sbufReadU8(src); // band count
+ sbufReadU8(src); // channel count
+
+ uint8_t newPowerCount = sbufReadU8(src);
+ if (newPowerCount > 0 && newPowerCount < (vtxDevice->capability.powerCount)) {
+ vtxDevice->capability.powerCount = newPowerCount;
+ }
+ }
}
}
}
@@ -2914,6 +2918,53 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
return MSP_RESULT_ERROR;
break;
+#ifdef USE_RX_MSP
+ case MSP2_COMMON_SET_MSP_RC_LINK_STATS: {
+ if (dataSize >= 7) {
+ uint8_t sublinkID = sbufReadU8(src); // Sublink ID
+ sbufReadU8(src); // Valid link (Failsafe backup)
+ if (sublinkID == 0) {
+ setRSSIFromMSP_RC(sbufReadU8(src)); // RSSI %
+ rxLinkStatistics.uplinkRSSI = -sbufReadU8(src);
+ rxLinkStatistics.downlinkLQ = sbufReadU8(src);
+ rxLinkStatistics.uplinkLQ = sbufReadU8(src);
+ rxLinkStatistics.uplinkSNR = sbufReadI8(src);
+ }
+
+ return MSP_RESULT_NO_REPLY;
+ } else
+ return MSP_RESULT_ERROR;
+ }
+ break;
+
+ case MSP2_COMMON_SET_MSP_RC_INFO: {
+ if (dataSize >= 15) {
+ uint8_t sublinkID = sbufReadU8(src);
+
+ if (sublinkID == 0) {
+ rxLinkStatistics.uplinkTXPower = sbufReadU16(src);
+ rxLinkStatistics.downlinkTXPower = sbufReadU16(src);
+
+ for (int i = 0; i < 4; i++) {
+ rxLinkStatistics.band[i] = sbufReadU8(src);
+ }
+
+ sl_toupperptr(rxLinkStatistics.band);
+
+ for (int i = 0; i < 6; i++) {
+ rxLinkStatistics.mode[i] = sbufReadU8(src);
+ }
+
+ sl_toupperptr(rxLinkStatistics.mode);
+ }
+
+ return MSP_RESULT_NO_REPLY;
+ } else
+ return MSP_RESULT_ERROR;
+ }
+ break;
+#endif
+
case MSP_SET_FAILSAFE_CONFIG:
if (dataSize == 20) {
failsafeConfigMutable()->failsafe_delay = sbufReadU8(src);
@@ -3700,7 +3751,7 @@ void mspWriteSimulatorOSD(sbuf_t *dst)
while (bytesCount < 80) //whole response should be less 155 bytes at worst.
{
bool blink1;
- uint16_t lastChar;
+ uint16_t lastChar = 0;
count = 0;
while ( true )
@@ -4198,7 +4249,7 @@ mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostPro
if (cmd->flags & MSP_FLAG_DONT_REPLY) {
ret = MSP_RESULT_NO_REPLY;
}
-
+ reply->flags = cmd->flags;
reply->result = ret;
return ret;
}
diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c
index afb880db526..97cf005a5b6 100755
--- a/src/main/fc/fc_tasks.c
+++ b/src/main/fc/fc_tasks.c
@@ -120,9 +120,6 @@ void taskHandleSerial(timeUs_t currentTimeUs)
#ifdef USE_MSP_OSD
// Capture MSP Displayport messages to determine if VTX is connected
mspOsdSerialProcess(mspFcProcessCommand);
-#ifdef USE_VTX_MSP
- mspVtxSerialProcess(mspFcProcessCommand);
-#endif
#endif
}
@@ -409,7 +406,7 @@ void fcTasksInit(void)
setTaskEnabled(TASK_OPFLOW, sensors(SENSOR_OPFLOW));
#endif
#ifdef USE_VTX_CONTROL
-#if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP)
+#if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP) || defined(USE_VTX_MSP)
setTaskEnabled(TASK_VTXCTRL, true);
#endif
#endif
diff --git a/src/main/fc/rc_adjustments.c b/src/main/fc/rc_adjustments.c
index 37c338fedd8..27b88800841 100644
--- a/src/main/fc/rc_adjustments.c
+++ b/src/main/fc/rc_adjustments.c
@@ -576,7 +576,7 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t
case ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE:
applyAdjustmentU16(ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE, &navConfigMutable()->fw.minThrottleDownPitchAngle, delta, SETTING_FW_MIN_THROTTLE_DOWN_PITCH_MIN, SETTING_FW_MIN_THROTTLE_DOWN_PITCH_MAX);
break;
-#if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP)
+#if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP) || defined(USE_VTX_MSP)
case ADJUSTMENT_VTX_POWER_LEVEL:
{
vtxDeviceCapability_t vtxDeviceCapability;
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 663926f1984..acfc5680beb 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -3371,35 +3371,35 @@ groups:
min: -550
max: 1250
- name: osd_snr_alarm
- condition: USE_SERIALRX_CRSF
+ condition: USE_SERIALRX_CRSF || USE_RX_MSP
description: "Value below which Crossfire SNR Alarm pops-up. (dB)"
default_value: 4
field: snr_alarm
min: -20
- max: 10
+ max: 99
- name: osd_link_quality_alarm
- condition: USE_SERIALRX_CRSF
+ condition: USE_SERIALRX_CRSF || USE_RX_MSP
description: "LQ % indicator blinks below this value. For Crossfire use 70%, for Tracer use 50%"
default_value: 70
field: link_quality_alarm
min: 0
max: 100
- name: osd_rssi_dbm_alarm
- condition: USE_SERIALRX_CRSF
+ condition: USE_SERIALRX_CRSF || USE_RX_MSP
description: "RSSI dBm indicator blinks below this value [dBm]. 0 disables this alarm"
default_value: 0
field: rssi_dbm_alarm
min: -130
max: 0
- name: osd_rssi_dbm_max
- condition: USE_SERIALRX_CRSF
+ condition: USE_SERIALRX_CRSF || USE_RX_MSP
description: "RSSI dBm upper end of curve. Perfect rssi (max) = 100%"
default_value: -30
field: rssi_dbm_max
min: -50
max: 0
- name: osd_rssi_dbm_min
- condition: USE_SERIALRX_CRSF
+ condition: USE_SERIALRX_CRSF || USE_RX_MSP
description: "RSSI dBm lower end of curve or RX sensitivity level. Worst rssi (min) = 0%"
default_value: -120
field: rssi_dbm_min
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index 41c20dda928..f4ab6f7df7f 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -181,7 +181,7 @@ typedef struct statistic_s {
uint16_t min_voltage; // /100
int16_t max_current;
int32_t max_power;
- int16_t min_rssi;
+ uint8_t min_rssi;
int16_t min_lq; // for CRSF
int16_t min_rssi_dbm; // for CRSF
int32_t max_altitude;
@@ -224,8 +224,8 @@ static bool osdDisplayHasCanvas;
#define AH_MAX_PITCH_DEFAULT 20 // Specify default maximum AHI pitch value displayed (degrees)
-PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 13);
-PG_REGISTER_WITH_RESET_FN(osdLayoutsConfig_t, osdLayoutsConfig, PG_OSD_LAYOUTS_CONFIG, 2);
+PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 14);
+PG_REGISTER_WITH_RESET_FN(osdLayoutsConfig_t, osdLayoutsConfig, PG_OSD_LAYOUTS_CONFIG, 3);
void osdStartedSaveProcess(void) {
savingSettings = true;
@@ -616,11 +616,11 @@ char *osdFormatTrimWhiteSpace(char *buff)
/**
* Converts RSSI into a % value used by the OSD.
+ * Range is [0, 100]
*/
-static uint16_t osdConvertRSSI(void)
+static uint8_t osdConvertRSSI(void)
{
- // change range to [0, 99]
- return constrain(getRSSI() * 100 / RSSI_MAX_VALUE, 0, 99);
+ return constrain(getRSSI() * 100 / RSSI_MAX_VALUE, 0, 100);
}
static uint16_t osdGetCrsfLQ(void)
@@ -1712,9 +1712,13 @@ static bool osdDrawSingleElement(uint8_t item)
}
case OSD_RSSI_VALUE:
{
- uint16_t osdRssi = osdConvertRSSI();
+ uint8_t osdRssi = osdConvertRSSI();
buff[0] = SYM_RSSI;
- tfp_sprintf(buff + 1, "%2d", osdRssi);
+ if (osdRssi < 100)
+ tfp_sprintf(buff + 1, "%2d", osdRssi);
+ else
+ tfp_sprintf(buff + 1, "%c ", SYM_MAX);
+
if (osdRssi < osdConfig()->rssi_alarm) {
TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
}
@@ -2463,15 +2467,15 @@ static bool osdDrawSingleElement(uint8_t item)
return true;
}
-#if defined(USE_SERIALRX_CRSF)
- case OSD_CRSF_RSSI_DBM:
+#if defined(USE_SERIALRX_CRSF) || defined(USE_RX_MSP)
+ case OSD_RSSI_DBM:
{
int16_t rssi = rxLinkStatistics.uplinkRSSI;
buff[0] = (rxLinkStatistics.activeAntenna == 0) ? SYM_RSSI : SYM_2RSS; // Separate symbols for each antenna
if (rssi <= -100) {
tfp_sprintf(buff + 1, "%4d%c", rssi, SYM_DBM);
} else {
- tfp_sprintf(buff + 1, "%3d%c%c", rssi, SYM_DBM, ' ');
+ tfp_sprintf(buff + 1, " %3d%c", rssi, SYM_DBM);
}
if (!failsafeIsReceivingRxData()){
TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
@@ -2480,19 +2484,15 @@ static bool osdDrawSingleElement(uint8_t item)
}
break;
}
- case OSD_CRSF_LQ:
+ case OSD_LQ_UPLINK:
{
buff[0] = SYM_LQ;
- int16_t statsLQ = rxLinkStatistics.uplinkLQ;
- int16_t scaledLQ = scaleRange(constrain(statsLQ, 0, 100), 0, 100, 170, 300);
- switch (osdConfig()->crsf_lq_format) {
- case OSD_CRSF_LQ_TYPE1:
- if (!failsafeIsReceivingRxData()) {
- tfp_sprintf(buff+1, "%3d", 0);
- } else {
- tfp_sprintf(buff+1, "%3d", rxLinkStatistics.uplinkLQ);
- }
- break;
+ uint8_t lqFormat = osdConfig()->crsf_lq_format;
+
+ if (rxConfig()->receiverType == RX_TYPE_MSP)
+ lqFormat = OSD_CRSF_LQ_TYPE1;
+
+ switch (lqFormat) {
case OSD_CRSF_LQ_TYPE2:
if (!failsafeIsReceivingRxData()) {
tfp_sprintf(buff+1, "%s:%3d", " ", 0);
@@ -2504,9 +2504,18 @@ static bool osdDrawSingleElement(uint8_t item)
if (!failsafeIsReceivingRxData()) {
tfp_sprintf(buff+1, "%3d", 0);
} else {
+ int16_t scaledLQ = scaleRange(constrain(rxLinkStatistics.uplinkLQ, 0, 100), 0, 100, 170, 300);
tfp_sprintf(buff+1, "%3d", rxLinkStatistics.rfMode >= 2 ? scaledLQ : rxLinkStatistics.uplinkLQ);
}
break;
+ case OSD_CRSF_LQ_TYPE1:
+ default:
+ if (!failsafeIsReceivingRxData()) {
+ tfp_sprintf(buff+1, "%3d", 0);
+ } else {
+ tfp_sprintf(buff+1, "%3d", rxLinkStatistics.uplinkLQ);
+ }
+ break;
}
if (!failsafeIsReceivingRxData()) {
TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
@@ -2516,7 +2525,24 @@ static bool osdDrawSingleElement(uint8_t item)
break;
}
- case OSD_CRSF_SNR_DB:
+ case OSD_LQ_DOWNLINK:
+ {
+ buff[0] = SYM_LQ;
+ if (!failsafeIsReceivingRxData()) {
+ tfp_sprintf(buff+1, "%3d%c", 0, SYM_AH_DECORATION_DOWN);
+ } else {
+ tfp_sprintf(buff+1, "%3d%c", rxLinkStatistics.downlinkLQ, SYM_AH_DECORATION_DOWN);
+ }
+
+ if (!failsafeIsReceivingRxData()) {
+ TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
+ } else if (rxLinkStatistics.downlinkLQ < osdConfig()->link_quality_alarm) {
+ TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
+ }
+ break;
+ }
+
+ case OSD_SNR_DB:
{
static pt1Filter_t snrFilterState;
static timeMs_t snrUpdated = 0;
@@ -2535,23 +2561,49 @@ static bool osdDrawSingleElement(uint8_t item)
}
} else if (snrFiltered <= osdConfig()->snr_alarm) {
buff[0] = SYM_SNR;
- if (snrFiltered <= -10) {
+ if (snrFiltered <= -10 || snrFiltered >= 10) {
tfp_sprintf(buff + 1, "%3d%c", snrFiltered, SYM_DB);
} else {
- tfp_sprintf(buff + 1, "%2d%c%c", snrFiltered, SYM_DB, ' ');
+ tfp_sprintf(buff + 1, " %2d%c", snrFiltered, SYM_DB);
}
}
break;
}
- case OSD_CRSF_TX_POWER:
+ case OSD_TX_POWER_UPLINK:
{
if (!failsafeIsReceivingRxData())
- tfp_sprintf(buff, "%s%c", " ", SYM_BLANK);
+ tfp_sprintf(buff, "%s%c", " ", SYM_MW);
else
tfp_sprintf(buff, "%4d%c", rxLinkStatistics.uplinkTXPower, SYM_MW);
break;
}
+
+ case OSD_RX_POWER_DOWNLINK:
+ {
+ if (!failsafeIsReceivingRxData())
+ tfp_sprintf(buff, "%s%c%c", " ", SYM_MW, SYM_AH_DECORATION_DOWN);
+ else
+ tfp_sprintf(buff, "%4d%c%c", rxLinkStatistics.downlinkTXPower, SYM_MW, SYM_AH_DECORATION_DOWN);
+ break;
+ }
+ case OSD_RX_BAND:
+ displayWriteChar(osdDisplayPort, elemPosX++, elemPosY, SYM_RX_BAND);
+ strcat(buff, rxLinkStatistics.band);
+ if (strlen(rxLinkStatistics.band) < 4)
+ for (uint8_t i = strlen(rxLinkStatistics.band); i < 4; i++)
+ buff[i] = ' ';
+ buff[4] = '\0';
+ break;
+
+ case OSD_RX_MODE:
+ displayWriteChar(osdDisplayPort, elemPosX++, elemPosY, SYM_RX_MODE);
+ strcat(buff, rxLinkStatistics.mode);
+ if (strlen(rxLinkStatistics.mode) < 6)
+ for (uint8_t i = strlen(rxLinkStatistics.mode); i < 6; i++)
+ buff[i] = ' ';
+ buff[6] = '\0';
+ break;
#endif
case OSD_FORMATION_FLIGHT:
@@ -3992,7 +4044,7 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig,
.adsb_distance_alert = SETTING_OSD_ADSB_DISTANCE_ALERT_DEFAULT,
.adsb_ignore_plane_above_me_limit = SETTING_OSD_ADSB_IGNORE_PLANE_ABOVE_ME_LIMIT_DEFAULT,
#endif
-#ifdef USE_SERIALRX_CRSF
+#if defined(USE_SERIALRX_CRSF) || defined(USE_RX_MSP)
.snr_alarm = SETTING_OSD_SNR_ALARM_DEFAULT,
.crsf_lq_format = SETTING_OSD_CRSF_LQ_FORMAT_DEFAULT,
.link_quality_alarm = SETTING_OSD_LINK_QUALITY_ALARM_DEFAULT,
@@ -4143,11 +4195,15 @@ void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig)
osdLayoutsConfig->item_pos[0][OSD_PILOT_LOGO] = OSD_POS(20, 3);
osdLayoutsConfig->item_pos[0][OSD_VTX_CHANNEL] = OSD_POS(8, 6);
-#ifdef USE_SERIALRX_CRSF
- osdLayoutsConfig->item_pos[0][OSD_CRSF_RSSI_DBM] = OSD_POS(23, 12);
- osdLayoutsConfig->item_pos[0][OSD_CRSF_LQ] = OSD_POS(23, 11);
- osdLayoutsConfig->item_pos[0][OSD_CRSF_SNR_DB] = OSD_POS(24, 9);
- osdLayoutsConfig->item_pos[0][OSD_CRSF_TX_POWER] = OSD_POS(24, 10);
+#if defined(USE_SERIALRX_CRSF) || defined(USE_RX_MSP)
+ osdLayoutsConfig->item_pos[0][OSD_RSSI_DBM] = OSD_POS(23, 12);
+ osdLayoutsConfig->item_pos[0][OSD_LQ_UPLINK] = OSD_POS(23, 10);
+ osdLayoutsConfig->item_pos[0][OSD_LQ_DOWNLINK] = OSD_POS(23, 11);
+ osdLayoutsConfig->item_pos[0][OSD_SNR_DB] = OSD_POS(24, 9);
+ osdLayoutsConfig->item_pos[0][OSD_TX_POWER_UPLINK] = OSD_POS(24, 10);
+ osdLayoutsConfig->item_pos[0][OSD_RX_POWER_DOWNLINK] = OSD_POS(24, 11);
+ osdLayoutsConfig->item_pos[0][OSD_RX_BAND] = OSD_POS(24, 12);
+ osdLayoutsConfig->item_pos[0][OSD_RX_MODE] = OSD_POS(24, 13);
#endif
osdLayoutsConfig->item_pos[0][OSD_ONTIME] = OSD_POS(23, 8);
@@ -4582,7 +4638,7 @@ static void osdResetStats(void)
stats.max_3D_speed = 0;
stats.max_air_speed = 0;
stats.min_voltage = 12000;
- stats.min_rssi = 99;
+ stats.min_rssi = 100;
stats.min_lq = 300;
stats.min_rssi_dbm = 0;
stats.max_altitude = 0;
diff --git a/src/main/io/osd.h b/src/main/io/osd.h
index b0423d40eff..c57bd62f640 100644
--- a/src/main/io/osd.h
+++ b/src/main/io/osd.h
@@ -242,10 +242,10 @@ typedef enum {
OSD_ESC_RPM,
OSD_ESC_TEMPERATURE,
OSD_AZIMUTH,
- OSD_CRSF_RSSI_DBM,
- OSD_CRSF_LQ,
- OSD_CRSF_SNR_DB,
- OSD_CRSF_TX_POWER,
+ OSD_RSSI_DBM,
+ OSD_LQ_UPLINK,
+ OSD_SNR_DB,
+ OSD_TX_POWER_UPLINK,
OSD_GVAR_0,
OSD_GVAR_1,
OSD_GVAR_2,
@@ -291,7 +291,11 @@ typedef enum {
OSD_CUSTOM_ELEMENT_5,
OSD_CUSTOM_ELEMENT_6,
OSD_CUSTOM_ELEMENT_7,
- OSD_CUSTOM_ELEMENT_8, // 158
+ OSD_CUSTOM_ELEMENT_8,
+ OSD_LQ_DOWNLINK,
+ OSD_RX_POWER_DOWNLINK, // 160
+ OSD_RX_BAND,
+ OSD_RX_MODE,
OSD_ITEM_COUNT // MUST BE LAST
} osd_items_e;
@@ -369,7 +373,7 @@ typedef struct osdConfig_s {
float gforce_alarm;
float gforce_axis_alarm_min;
float gforce_axis_alarm_max;
-#ifdef USE_SERIALRX_CRSF
+#if defined(USE_SERIALRX_CRSF) || defined(USE_RX_MSP)
int8_t snr_alarm; //CRSF SNR alarm in dB
int8_t link_quality_alarm;
int16_t rssi_dbm_alarm; // in dBm
diff --git a/src/main/io/vtx_msp.c b/src/main/io/vtx_msp.c
index 696918e5705..84746e0c9f3 100644
--- a/src/main/io/vtx_msp.c
+++ b/src/main/io/vtx_msp.c
@@ -17,157 +17,102 @@
*
* If not, see .
*/
+/* Created by geoffsim */
-/* Created by phobos- */
-
-#include
#include
-#include
-#include
-#include
#include
+#include
#include "platform.h"
#if defined(USE_VTX_MSP) && defined(USE_VTX_CONTROL) && defined(USE_VTX_COMMON)
-#include "build/debug.h"
-
-//#include "cms/cms_menu_vtx_msp.h"
-#include "common/crc.h"
#include "common/log.h"
-#include "config/feature.h"
-
+#include "common/crc.h"
#include "drivers/vtx_common.h"
-//#include "drivers/vtx_table.h"
-
-#include "fc/runtime_config.h"
-#include "flight/failsafe.h"
-
-#include "io/serial.h"
-#include "io/vtx_msp.h"
-#include "io/vtx_control.h"
-#include "io/vtx_string.h"
-#include "io/vtx_smartaudio.h"
-#include "io/vtx.h"
-#include "io/displayport_msp_osd.h"
-
#include "msp/msp_protocol.h"
-#include "msp/msp_serial.h"
-#include "msp/msp.h"
-
-//#include "pg/vtx_table.h"
-#include "fc/settings.h"
-
-#include "rx/crsf.h"
-//#include "rx/crsf_protocol.h"
#include "rx/rx.h"
-
-#include "telemetry/msp_shared.h"
-
-//static uint16_t mspConfFreq = 0;
-static uint8_t mspConfBand = SETTING_VTX_BAND_DEFAULT;
-static uint8_t mspConfChannel = SETTING_VTX_CHANNEL_DEFAULT;
-//static uint16_t mspConfPower = 0;
-static uint16_t mspConfPowerIndex = SETTING_VTX_POWER_DEFAULT;
-static uint8_t mspConfPitMode = 0;
-static bool mspVtxConfigChanged = false;
-static timeUs_t mspVtxLastTimeUs = 0;
-static bool prevLowPowerDisarmedState = false;
-
-static const vtxVTable_t mspVTable; // forward
-static vtxDevice_t vtxMsp = {
- .vTable = &mspVTable,
- .capability.bandCount = VTX_MSP_TABLE_MAX_BANDS,
- .capability.channelCount = VTX_MSP_TABLE_MAX_CHANNELS,
- .capability.powerCount = VTX_MSP_TABLE_MAX_POWER_LEVELS,
- .capability.bandNames = (char **)vtx58BandNames,
- .capability.channelNames = (char **)vtx58ChannelNames,
- .capability.powerNames = (char**)saPowerNames
-
+#include "rx/crsf.h"
+#include "telemetry/crsf.h"
+#include "vtx.h"
+#include "displayport_msp_osd.h"
+#include "vtx_string.h"
+#include "vtx_msp.h"
+
+#define VTX_MSP_MIN_BAND (1)
+#define VTX_MSP_MAX_BAND (VTX_MSP_MIN_BAND + VTX_MSP_BAND_COUNT - 1)
+#define VTX_MSP_MIN_CHANNEL (1)
+#define VTX_MSP_MAX_CHANNEL (VTX_MSP_MIN_CHANNEL + VTX_MSP_CHANNEL_COUNT -1)
+
+#define VTX_UPDATE_REQ_NONE 0x00
+#define VTX_UPDATE_REQ_FREQUENCY 0x01
+#define VTX_UPDATE_REQ_POWER 0x02
+#define VTX_UPDATE_REQ_PIT_MODE 0x04
+
+typedef struct {
+ bool ready;
+ uint8_t timeouts;
+ uint8_t updateReqMask;
+ bool crsfTelemetryEnabled;
+
+ struct {
+ uint8_t band;
+ uint8_t channel;
+ uint16_t freq;
+ uint8_t power;
+ uint8_t powerIndex;
+ uint8_t pitMode;
+ } request;
+;
+} vtxProtoState_t;
+
+const char * const vtxMspBandNames[VTX_MSP_BAND_COUNT + 1] = {
+ "-----", "A 2.4", "B 2.4", "E 2.4", "F 2.4", "R 2.4"
};
-STATIC_UNIT_TESTED mspVtxStatus_e mspVtxStatus = MSP_VTX_STATUS_OFFLINE;
-static uint8_t mspVtxPortIdentifier = 255;
-
-#define MSP_VTX_REQUEST_PERIOD_US (200 * 1000) // 200ms
-
-static bool isCrsfPortConfig(const serialPortConfig_t *portConfig)
-{
- return portConfig->functionMask & FUNCTION_RX_SERIAL && portConfig->functionMask & FUNCTION_VTX_MSP && rxConfig()->serialrx_provider == SERIALRX_CRSF;
-}
-
-static bool isLowPowerDisarmed(void)
-{
- return (!ARMING_FLAG(ARMED) && !failsafeIsActive() &&
- (vtxSettingsConfig()->lowPowerDisarm == VTX_LOW_POWER_DISARM_ALWAYS ||
- (vtxSettingsConfig()->lowPowerDisarm == VTX_LOW_POWER_DISARM_UNTIL_FIRST_ARM && !ARMING_FLAG(WAS_EVER_ARMED))));
-}
+const char * vtxMspBandLetters = "-ABEFR";
-bool isVtxConfigValid(const vtxConfig_t *cfg)
-{
- LOG_DEBUG(VTX, "msp isVtxConfigValid\r\n");
- for (int i = 0; i < MAX_CHANNEL_ACTIVATION_CONDITION_COUNT; ++i) {
-
- if (cfg->vtxChannelActivationConditions[i].band ||
- (cfg->vtxChannelActivationConditions[i].range.startStep && cfg->vtxChannelActivationConditions[i].range.endStep) ||
- cfg->vtxChannelActivationConditions[i].auxChannelIndex ||
- cfg->vtxChannelActivationConditions[i].channel) {
- return true;
- }
- }
-
- LOG_DEBUG(VTX, "msp Invalid Config!\r\n");
- return false;
-}
+const char * const vtxMspChannelNames[VTX_MSP_CHANNEL_COUNT + 1] = {
+ "-", "1", "2", "3", "4", "5", "6", "7", "8"
+};
+const char * const vtxMspPowerNames[VTX_MSP_POWER_COUNT + 1] = {
+ "0", "25", "200", "500", "MAX"
+};
-void setMspVtxDeviceStatusReady(const int descriptor)
-{
- LOG_DEBUG(VTX, "msp setMspVtxDeviceStatusReady\r\n");
- UNUSED(descriptor);
+const unsigned vtxMspPowerTable[VTX_MSP_POWER_COUNT + 1] = {
+ 0, 25, 200, 500, 1000
+};
- mspVtxStatus = MSP_VTX_STATUS_READY;
- mspVtxConfigChanged = true;
-}
+static serialPortIdentifier_e mspVtxPortIdentifier;
+static vtxProtoState_t vtxState;
+static vtxDevType_e vtxMspGetDeviceType(const vtxDevice_t *);
+static bool vtxMspIsReady(const vtxDevice_t *);
+static vtxDevice_t vtxMsp;
-void prepareMspFrame(uint8_t *mspFrame)
+static void prepareMspFrame(vtxDevice_t *vtxDevice, uint8_t *mspFrame)
{
- LOG_DEBUG(VTX, "msp PrepareMspFrame\r\n");
-/*
-HDZERO parsing
- fc_band_rx = msp_rx_buf[1];
- fc_channel_rx = msp_rx_buf[2];
- fc_pwr_rx = msp_rx_buf[3];
- fc_pit_rx = msp_rx_buf[4];
- fc_lp_rx = msp_rx_buf[8];
-*/
-
- uint8_t pitmode = 0;
- vtxCommonGetPitMode(&vtxMsp, &pitmode);
-
- mspFrame[0] = VTXDEV_MSP,
- mspFrame[1] = vtxSettingsConfig()->band;
- mspFrame[2] = vtxSettingsConfig()->channel;
- mspFrame[3] = isLowPowerDisarmed() ? 1 : vtxSettingsConfig()->power; // index based
- mspFrame[4] = pitmode;
- mspFrame[5] = 0; // Freq_L
- mspFrame[6] = 0; // Freq_H
- mspFrame[7] = (mspVtxStatus == MSP_VTX_STATUS_READY) ? 1 : 0;
- mspFrame[8] = isLowPowerDisarmed();
- mspFrame[9] = 0; // Pitmode freq Low
- mspFrame[10] = 0; // pitmod freq High
+ // Send an MSP_VTX_V2 frame to the VTX
+ mspFrame[0] = vtxMspGetDeviceType(vtxDevice);
+ mspFrame[1] = vtxState.request.band;
+ mspFrame[2] = vtxState.request.channel;
+ mspFrame[3] = vtxState.request.powerIndex;
+ mspFrame[4] = vtxState.request.pitMode;
+ mspFrame[5] = 0; // Freq_L
+ mspFrame[6] = 0; // Freq_H
+ mspFrame[7] = vtxMspIsReady(vtxDevice);
+ mspFrame[8] = vtxSettingsConfig()->lowPowerDisarm;
+ mspFrame[9] = 0; // pitmode freq Low
+ mspFrame[10] = 0; // pitmode freq High
mspFrame[11] = 0; // 1 if using vtx table
- mspFrame[12] = 0; // vtx table bands or 0
- mspFrame[13] = 0; // vtx table channels or 0
- mspFrame[14] = 0; // vtx table power levels or 0
-}
+ mspFrame[12] = vtxMsp.capability.bandCount; // bands or 0
+ mspFrame[13] = vtxMsp.capability.channelCount; // channels or 0
+ mspFrame[14] = vtxMsp.capability.powerCount; // power levels or 0
+}
static void mspCrsfPush(const uint8_t mspCommand, const uint8_t *mspFrame, const uint8_t mspFrameSize)
{
-
- LOG_DEBUG(VTX, "msp CrsfPush\r\n");
#ifndef USE_TELEMETRY_CRSF
UNUSED(mspCommand);
UNUSED(mspFrame);
@@ -201,285 +146,144 @@ static void mspCrsfPush(const uint8_t mspCommand, const uint8_t *mspFrame, const
crc8_dvb_s2_sbuf_append(dst, &crsfFrame[2]); // start at byte 2, since CRC does not include device address and frame length
sbufSwitchToReader(dst, crsfFrame);
- crsfRxSendTelemetryData(); //give the FC a chance to send outstanding telemetry
+ crsfRxSendTelemetryData(); // give the FC a chance to send outstanding telemetry
crsfRxWriteTelemetryData(sbufPtr(dst), sbufBytesRemaining(dst));
crsfRxSendTelemetryData();
#endif
}
-static uint16_t packetCounter = 0;
-
-static bool isVtxConfigChanged(void)
-{
- if(mspVtxStatus == MSP_VTX_STATUS_READY) {
- if (mspVtxConfigChanged == true)
- return true;
-
- if (isLowPowerDisarmed() != prevLowPowerDisarmedState) {
- LOG_DEBUG(VTX, "msp vtx config changed (lower power disarm 2)\r\n");
- mspVtxConfigChanged = true;
- prevLowPowerDisarmedState = isLowPowerDisarmed();
- }
-
- if (mspConfPowerIndex != vtxSettingsConfig()->power) {
- LOG_DEBUG(VTX, "msp vtx config changed (power 2)\r\n");
- mspVtxConfigChanged = true;
- }
-
- if (mspConfBand != vtxSettingsConfig()->band || mspConfChannel != vtxSettingsConfig()->channel) {
- LOG_DEBUG(VTX, "msp vtx config changed (band and channel 2)\r\n");
- mspVtxConfigChanged = true;
- }
-
- return mspVtxConfigChanged;
- }
-
- return false;
-}
-
static void vtxMspProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs)
{
UNUSED(vtxDevice);
- const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_MSP_OSD);
- uint8_t frame[15];
+ uint8_t mspFrame[15];
- switch (mspVtxStatus) {
- case MSP_VTX_STATUS_OFFLINE:
- LOG_DEBUG(VTX, "msp MspProcess: OFFLINE\r\n");
- // wait for MSP communication from the VTX
-#ifdef USE_CMS
- //mspCmsUpdateStatusString();
-#endif
- break;
- case MSP_VTX_STATUS_READY:
- LOG_DEBUG(VTX, "msp MspProcess: READY\r\n");
- // send an update if stuff has changed with 200ms period
- if ((isVtxConfigChanged()) && cmp32(currentTimeUs, mspVtxLastTimeUs) >= MSP_VTX_REQUEST_PERIOD_US) {
-
- LOG_DEBUG(VTX, "msp-vtx: vtxInfo Changed\r\n");
- prepareMspFrame(frame);
-
- if (isCrsfPortConfig(portConfig)) {
- mspCrsfPush(MSP_VTX_CONFIG, frame, sizeof(frame));
- } else {
- mspPort_t *port = getMspOsdPort();
- if(port != NULL && port->port) {
- LOG_DEBUG(VTX, "msp-vtx: mspSerialPushPort\r\n");
- int sent = mspSerialPushPort(MSP_VTX_CONFIG, frame, sizeof(frame), port, MSP_V2_NATIVE);
- if (sent <= 0) {
- break;
- }
- }
+ mspPort_t *mspPort = getMspOsdPort();
+ unsigned lastActivity = (currentTimeUs/1000) - mspPort->lastActivityMs;
+ if (lastActivity > VTX_MSP_TIMEOUT) {
+ if (vtxState.timeouts++ > 3) {
+ if (vtxState.ready) {
+ vtxState.ready = false;
}
- packetCounter++;
- mspVtxLastTimeUs = currentTimeUs;
- mspVtxConfigChanged = false;
-
-#ifdef USE_CMS
- //mspCmsUpdateStatusString();
-#endif
}
- break;
- default:
- mspVtxStatus = MSP_VTX_STATUS_OFFLINE;
- break;
+ } else { // active
+ if (!vtxState.ready) {
+ vtxState.ready = true;
+ }
}
-#if 0
- DEBUG_SET(DEBUG_VTX_MSP, 0, packetCounter);
- DEBUG_SET(DEBUG_VTX_MSP, 1, isCrsfPortConfig(portConfig));
- DEBUG_SET(DEBUG_VTX_MSP, 2, isLowPowerDisarmed());
-#if defined(USE_MSP_OVER_TELEMETRY)
- DEBUG_SET(DEBUG_VTX_MSP, 3, isCrsfPortConfig(portConfig) ? getMspTelemetryDescriptor() : getMspSerialPortDescriptor(mspVtxPortIdentifier));
-#else
- DEBUG_SET(DEBUG_VTX_MSP, 3, getMspSerialPortDescriptor(mspVtxPortIdentifier));
-#endif
-#endif
+ if (vtxState.ready) {
+ if (vtxState.updateReqMask != VTX_UPDATE_REQ_NONE) {
+ prepareMspFrame(vtxDevice, mspFrame);
+ if (vtxState.crsfTelemetryEnabled) { // keep ELRS LUA up to date ?
+ mspCrsfPush(MSP_VTX_CONFIG, mspFrame, sizeof(mspFrame));
+ }
+
+ int sent = mspSerialPushPort(MSP_VTX_CONFIG, mspFrame, sizeof(mspFrame), mspPort, MSP_V2_NATIVE);
+ if (sent > 0) {
+ vtxState.updateReqMask = VTX_UPDATE_REQ_NONE;
+ }
+ }
+ }
}
static vtxDevType_e vtxMspGetDeviceType(const vtxDevice_t *vtxDevice)
{
- //LOG_DEBUG(VTX, "msp GetDeviceType\r\n");
UNUSED(vtxDevice);
return VTXDEV_MSP;
}
static bool vtxMspIsReady(const vtxDevice_t *vtxDevice)
{
- //LOG_DEBUG(VTX, "msp vtxIsReady: %s\r\n", (vtxDevice != NULL && mspVtxStatus == MSP_VTX_STATUS_READY) ? "Y": "N");
- return vtxDevice != NULL && mspVtxStatus == MSP_VTX_STATUS_READY;
+ return vtxDevice != NULL && mspVtxPortIdentifier >=0 && vtxState.ready;
}
static void vtxMspSetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint8_t channel)
{
- LOG_DEBUG(VTX, "msp SetBandAndChannel\r\n");
UNUSED(vtxDevice);
- if (band != mspConfBand || channel != mspConfChannel) {
- LOG_DEBUG(VTX, "msp vtx config changed (band and channel)\r\n");
- mspVtxConfigChanged = true;
+
+ if (band < VTX_MSP_MIN_BAND || band > VTX_MSP_MAX_BAND || channel < VTX_MSP_MIN_CHANNEL || channel > VTX_MSP_MAX_CHANNEL) {
+ return;
}
- mspConfBand = band;
- mspConfChannel = channel;
+
+ vtxState.request.band = band;
+ vtxState.request.channel = channel;
+ vtxState.request.freq = vtx58_Bandchan2Freq(band, channel);
+ vtxState.updateReqMask |= VTX_UPDATE_REQ_FREQUENCY;
}
static void vtxMspSetPowerByIndex(vtxDevice_t *vtxDevice, uint8_t index)
{
- LOG_DEBUG(VTX, "msp SetPowerByIndex\r\n");
UNUSED(vtxDevice);
- if (index > 0 && (index < VTX_MSP_TABLE_MAX_POWER_LEVELS))
- {
- if (index != mspConfPowerIndex)
- {
- LOG_DEBUG(VTX, "msp vtx config changed (power by index)\r\n");
- mspVtxConfigChanged = true;
- }
- mspConfPowerIndex = index;
- }
+ vtxState.request.power = vtxMspPowerTable[index];
+ vtxState.request.powerIndex = index;
+ vtxState.updateReqMask |= VTX_UPDATE_REQ_POWER;
}
-static void vtxMspSetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff)
+static void vtxMspSetPitMode(vtxDevice_t *vtxDevice, uint8_t onOff)
{
- LOG_DEBUG(VTX, "msp SetPitMode\r\n");
UNUSED(vtxDevice);
- if (onoff != mspConfPitMode) {
- LOG_DEBUG(VTX, "msp vtx config changed (pitmode)\r\n");
- mspVtxConfigChanged = true;
- }
- mspConfPitMode = onoff;
-}
-#if 0
-static void vtxMspSetFreq(vtxDevice_t *vtxDevice, uint16_t freq)
-{
- UNUSED(vtxDevice);
- if (freq != mspConfFreq) {
- mspVtxConfigChanged = true;
- }
- mspConfFreq = freq;
+ vtxState.request.pitMode = onOff;
+ vtxState.updateReqMask |= VTX_UPDATE_REQ_PIT_MODE;
}
-#endif
-
-
-
static bool vtxMspGetBandAndChannel(const vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel)
{
- if (!vtxMspIsReady(vtxDevice)) {
- return false;
- }
-
- *pBand = vtxSettingsConfig()->band;
- *pChannel = vtxSettingsConfig()->channel;
-
- //LOG_DEBUG(VTX, "msp GetBandAndChannel: %02x:%02x\r\n", vtxSettingsConfig()->band, vtxSettingsConfig()->channel);
+ UNUSED(vtxDevice);
+ *pBand = vtxState.request.band;
+ *pChannel = vtxState.request.channel;
return true;
}
static bool vtxMspGetPowerIndex(const vtxDevice_t *vtxDevice, uint8_t *pIndex)
{
- if (!vtxMspIsReady(vtxDevice)) {
- return false;
- }
-
- uint8_t power = isLowPowerDisarmed() ? 1 : vtxSettingsConfig()->power;
- // Special case, power not set
- if (power > VTX_MSP_TABLE_MAX_POWER_LEVELS) {
- *pIndex = 0;
- //LOG_DEBUG(VTX, "msp GetPowerIndex: %u\r\n", *pIndex);
- return true;
- }
-
- *pIndex = power;
+ UNUSED(vtxDevice);
- //LOG_DEBUG(VTX, "msp GetPowerIndex: %u\r\n", *pIndex);
+ *pIndex = vtxState.request.powerIndex;
return true;
}
-static bool vtxMspGetFreq(const vtxDevice_t *vtxDevice, uint16_t *pFreq)
+static bool vtxMspGetPitMode(const vtxDevice_t *vtxDevice, uint8_t *pOnOff)
{
- LOG_DEBUG(VTX, "msp GetFreq\r\n");
- if (!vtxMspIsReady(vtxDevice)) {
- return false;
- }
+ UNUSED(vtxDevice);
- *pFreq = 5800;
+ *pOnOff = vtxState.request.pitMode;
return true;
}
-static bool vtxMspGetPower(const vtxDevice_t *vtxDevice, uint8_t *pIndex, uint16_t *pPowerMw)
+static bool vtxMspGetFreq(const vtxDevice_t *vtxDevice, uint16_t *pFreq)
{
- LOG_DEBUG(VTX, "msp GetPower\r\n");
- uint8_t powerIndex;
-
- if (!vtxMspGetPowerIndex(vtxDevice, &powerIndex)) {
- return false;
- }
-
+ UNUSED(vtxDevice);
- *pIndex = powerIndex;
- *pPowerMw = *pIndex;
+ *pFreq = vtxState.request.freq;
return true;
}
-static bool vtxMspGetOsdInfo(const vtxDevice_t *vtxDevice, vtxDeviceOsdInfo_t * pOsdInfo)
+static bool vtxMspGetPower(const vtxDevice_t *vtxDevice, uint8_t *pIndex, uint16_t *pPowerMw)
{
- LOG_DEBUG(VTX, "msp GetOsdInfo\r\n");
- uint8_t powerIndex;
- uint16_t powerMw;
- uint16_t freq;
- uint8_t band, channel;
-
- if (!vtxMspGetBandAndChannel(vtxDevice, &band, &channel)) {
- return false;
- }
-
- if (!vtxMspGetFreq(vtxDevice, &freq)) {
- return false;
- }
-
- if (!vtxMspGetPower(vtxDevice, &powerIndex, &powerMw)) {
- return false;
- }
+ UNUSED(vtxDevice);
- pOsdInfo->band = band;
- pOsdInfo->channel = channel;
- pOsdInfo->frequency = freq;
- pOsdInfo->powerIndex = powerIndex;
- pOsdInfo->powerMilliwatt = powerMw;
- pOsdInfo->bandLetter = vtx58BandNames[band][0];
- pOsdInfo->bandName = vtx58BandNames[band];
- pOsdInfo->channelName = vtx58ChannelNames[channel];
- pOsdInfo->powerIndexLetter = '0' + powerIndex;
+ *pIndex = vtxState.request.powerIndex;
+ *pPowerMw = vtxState.request.power;
return true;
}
-
-bool vtxMspInit(void)
+static bool vtxMspGetOsdInfo(const vtxDevice_t *vtxDevice, vtxDeviceOsdInfo_t *pOsdInfo)
{
- LOG_DEBUG(VTX, "msp %s\r\n", __FUNCTION__);
- // don't bother setting up this device if we don't have MSP vtx enabled
- // Port is shared with MSP_OSD
- const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_MSP_OSD);
- if (!portConfig) {
- return false;
- }
-
- mspVtxPortIdentifier = portConfig->identifier;
-
- // XXX Effect of USE_VTX_COMMON should be reviewed, as following call to vtxInit will do nothing if vtxCommonSetDevice is not called.
-#if defined(USE_VTX_COMMON)
- vtxCommonSetDevice(&vtxMsp);
-#endif
-
- mspConfBand = vtxSettingsConfig()->band;
- mspConfChannel = vtxSettingsConfig()->channel;
- mspConfPowerIndex = isLowPowerDisarmed() ? 1 : vtxSettingsConfig()->power; // index based
- vtxCommonGetPitMode(&vtxMsp, &mspConfPitMode);
+ UNUSED(vtxDevice);
- vtxInit();
+ pOsdInfo->band = vtxState.request.band;
+ pOsdInfo->channel = vtxState.request.channel;
+ pOsdInfo->frequency = vtxState.request.freq;
+ pOsdInfo->powerIndex = vtxState.request.powerIndex;
+ pOsdInfo->powerMilliwatt = vtxState.request.power;
+ pOsdInfo->bandName = vtxMspBandNames[vtxState.request.band];
+ pOsdInfo->bandLetter = vtxMspBandLetters[vtxState.request.band];
+ pOsdInfo->channelName = vtxMspChannelNames[vtxState.request.channel];
+ pOsdInfo->powerIndexLetter = '0' + vtxState.request.powerIndex;
return true;
}
@@ -491,211 +295,48 @@ static const vtxVTable_t mspVTable = {
.setBandAndChannel = vtxMspSetBandAndChannel,
.setPowerByIndex = vtxMspSetPowerByIndex,
.setPitMode = vtxMspSetPitMode,
- //.setFrequency = vtxMspSetFreq,
.getBandAndChannel = vtxMspGetBandAndChannel,
.getPowerIndex = vtxMspGetPowerIndex,
+ .getPitMode = vtxMspGetPitMode,
.getFrequency = vtxMspGetFreq,
- //.getStatus = vtxMspGetStatus,
.getPower = vtxMspGetPower,
- //.serializeCustomDeviceStatus = NULL,
- .getOsdInfo = vtxMspGetOsdInfo,
+ .getOsdInfo = vtxMspGetOsdInfo
};
-static mspResult_e mspVtxProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn)
-{
- //LOG_DEBUG(VTX, "msp VTX_MSP_PROCESS\r\n");
- UNUSED(mspPostProcessFn);
-
- sbuf_t *dst = &reply->buf;
- sbuf_t *src = &cmd->buf;
-
- const unsigned int dataSize = sbufBytesRemaining(src);
- UNUSED(dst);
- UNUSED(src);
-
- // Start initializing the reply message
- reply->cmd = cmd->cmd;
- reply->result = MSP_RESULT_ACK;
-
- vtxDevice_t *vtxDevice = vtxCommonDevice();
- if (!vtxDevice || vtxCommonGetDeviceType(vtxDevice) != VTXDEV_MSP) {
- LOG_DEBUG(VTX, "msp wrong vtx\r\n");
- return MSP_RESULT_ERROR;
- }
-
- switch (cmd->cmd)
- {
- case MSP_VTXTABLE_BAND:
- {
- LOG_DEBUG(VTX, "msp MSP_VTXTABLE_BAND\r\n");
- uint8_t deviceType = vtxCommonGetDeviceType(vtxDevice);
- if (deviceType == VTXDEV_MSP)
- {
- /*
- char bandName[MSP_VTX_TABLE_BAND_NAME_LENGTH + 1];
- memset(bandName, 0, MSP_VTX_TABLE_BAND_NAME_LENGTH + 1);
- uint16_t frequencies[MSP_VTX_TABLE_MAX_CHANNELS];
- const uint8_t band = sbufReadU8(src);
- const uint8_t bandNameLength = sbufReadU8(src);
- for (int i = 0; i < bandNameLength; i++) {
- const char nameChar = sbufReadU8(src);
- if (i < MSP_VTX_TABLE_BAND_NAME_LENGTH) {
- bandName[i] = toupper(nameChar);
- }
- }
- const char bandLetter = toupper(sbufReadU8(src));
- const bool isFactoryBand = (bool)sbufReadU8(src);
- const uint8_t channelCount = sbufReadU8(src);
- for (int i = 0; i < channelCount; i++)
- {
- const uint16_t frequency = sbufReadU16(src);
- if (i < vtxTableConfig()->channels)
- {
- frequencies[i] = frequency;
- }
- }
- */
-
- setMspVtxDeviceStatusReady(1);
- }
- break;
- }
- case MSP_VTXTABLE_POWERLEVEL:
- {
- LOG_DEBUG(VTX, "msp MSP_VTXTABLE_POWERLEVEL\r\n");
-
- /*
- char powerLevelLabel[VTX_TABLE_POWER_LABEL_LENGTH + 1];
- memset(powerLevelLabel, 0, VTX_TABLE_POWER_LABEL_LENGTH + 1);
- const uint8_t powerLevel = sbufReadU8(src);
- const uint16_t powerValue = sbufReadU16(src);
- const uint8_t powerLevelLabelLength = sbufReadU8(src);
- for (int i = 0; i < powerLevelLabelLength; i++)
- {
- const char labelChar = sbufReadU8(src);
- if (i < VTX_TABLE_POWER_LABEL_LENGTH)
- {
- powerLevelLabel[i] = toupper(labelChar);
- }
- }
- */
- setMspVtxDeviceStatusReady(1);
- }
- break;
- case MSP_VTX_CONFIG:
- {
- LOG_DEBUG(VTX, "msp MSP_VTX_CONFIG received\r\n");
- LOG_DEBUG(VTX, "msp MSP_VTX_CONFIG VTXDEV_MSP\r\n");
- uint8_t pitmode = 0;
- vtxCommonGetPitMode(vtxDevice, &pitmode);
-
- // VTXDEV_MSP,
- sbufWriteU8(dst, VTXDEV_MSP);
- // band;
- sbufWriteU8(dst, vtxSettingsConfig()->band);
- // channel;
- sbufWriteU8(dst, vtxSettingsConfig()->channel);
- // power; // index based
- sbufWriteU8(dst, vtxSettingsConfig()->power);
- // pit mode;
- // Freq_L
- sbufWriteU8(dst, 0);
- // Freq_H
- sbufWriteU8(dst, 0);
- // vtx status
- sbufWriteU8(dst, 1);
- // lowPowerDisarm
-
- sbufWriteU8(dst, vtxSettingsConfig()->lowPowerDisarm);
- // Pitmode freq Low
- sbufWriteU8(dst, 0);
- // pitmod freq High
- sbufWriteU8(dst, 0);
- // 1 if using vtx table
- sbufWriteU8(dst, 0);
- // vtx table bands or 0
- sbufWriteU8(dst, 0);
- // vtx table channels or 0
- sbufWriteU8(dst, 0);
-
- setMspVtxDeviceStatusReady(1);
- break;
- }
- case MSP_SET_VTX_CONFIG:
- LOG_DEBUG(VTX, "msp MSP_SET_VTX_CONFIG\r\n");
- if (dataSize == 15)
- {
- if (vtxCommonGetDeviceType(vtxDevice) != VTXDEV_UNKNOWN)
- {
- for (int i = 0; i < 15; ++i)
- {
- uint8_t data = sbufReadU8(src);
- switch (i)
- {
- case 1:
- vtxSettingsConfigMutable()->band = data;
- break;
- case 2:
- vtxSettingsConfigMutable()->channel = data;
- break;
- case 3:
- vtxSettingsConfigMutable()->power = data;
- break;
- case 4:
- vtxCommonSetPitMode(vtxDevice, data);
- break;
- case 7:
- // vtx ready
- break;
- case 8:
- vtxSettingsConfigMutable()->lowPowerDisarm = data;
- break;
- }
- }
- }
-
- setMspVtxDeviceStatusReady(1);
- break;
- }
- LOG_DEBUG(VTX, "msp MSP_RESULT_ERROR\r\n");
- return MSP_RESULT_ERROR;
- default:
- // debug[1]++;
- // debug[2] = cmd->cmd;
- if(cmd->cmd != MSP_STATUS && cmd->cmd != MSP_STATUS_EX && cmd->cmd != MSP_RC) {
- LOG_DEBUG(VTX, "msp default: %02x\r\n", cmd->cmd);
- }
- reply->result = MSP_RESULT_ERROR;
- break;
- }
-
- // Process DONT_REPLY flag
- if (cmd->flags & MSP_FLAG_DONT_REPLY)
- {
- reply->result = MSP_RESULT_NO_REPLY;
- }
-
- return reply->result;
-}
+static vtxDevice_t vtxMsp = {
+ .vTable = &mspVTable,
+ .capability.bandCount = VTX_MSP_MAX_BAND,
+ .capability.channelCount = VTX_MSP_MAX_CHANNEL,
+ .capability.powerCount = VTX_MSP_POWER_COUNT,
+ .capability.bandNames = (char **)vtxMspBandNames,
+ .capability.channelNames = (char **)vtxMspChannelNames,
+ .capability.powerNames = (char **)vtxMspPowerNames
+};
-void mspVtxSerialProcess(mspProcessCommandFnPtr mspProcessCommandFn)
+bool vtxMspInit(void)
{
- UNUSED(mspProcessCommandFn);
- // Check if VTX is ready
- /*
- if (mspVtxStatus != MSP_VTX_STATUS_READY) {
- LOG_DEBUG(VTX, "msp VTX NOT READY, skipping\r\n");
- return;
+ const serialPortConfig_t *portConfig;
+
+ // Shares MSP_OSD port
+ portConfig = findSerialPortConfig(FUNCTION_VTX_MSP);
+ if (!portConfig) {
+ return false;
}
- */
-
- mspPort_t *port = getMspOsdPort();
- if(port) {
- mspSerialProcessOnePort(port, MSP_SKIP_NON_MSP_DATA, mspVtxProcessMspCommand);
- }
+ portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
+
+ vtxState.ready = false;
+ vtxState.timeouts = 0;
+ vtxState.updateReqMask = VTX_UPDATE_REQ_NONE;
+ vtxState.crsfTelemetryEnabled = crsfRxIsActive();
+ vtxState.request.band = vtxSettingsConfig()->band;
+ vtxState.request.channel = vtxSettingsConfig()->channel;
+ vtxState.request.freq = vtx58_Bandchan2Freq(vtxState.request.band, vtxState.request.channel);
+ vtxState.request.power = vtxSettingsConfig()->power;
+ vtxState.request.pitMode = 0;
+ vtxCommonSetDevice(&vtxMsp);
+ return true;
}
-
#endif
\ No newline at end of file
diff --git a/src/main/io/vtx_msp.h b/src/main/io/vtx_msp.h
index 30ca245fed3..ceb2d5bea17 100644
--- a/src/main/io/vtx_msp.h
+++ b/src/main/io/vtx_msp.h
@@ -17,37 +17,17 @@
*
* If not, see .
*/
+/* Created by geoffsim */
-#pragma once
+#ifndef _VTX_MSP_H
+#define _VTX_MSP_H
-#include
-
-#include "build/build_config.h"
-
-#include "msp/msp_protocol.h"
-#include "msp/msp_serial.h"
-
-typedef enum {
- // Offline - device hasn't responded yet
- MSP_VTX_STATUS_OFFLINE = 0,
- MSP_VTX_STATUS_READY,
-} mspVtxStatus_e;
-
-typedef struct mspPowerTable_s {
- int mW; // rfpower
- int16_t dbi; // valueV1
-} mspPowerTable_t;
-
-#define VTX_MSP_TABLE_MAX_BANDS 5 // default freq table has 5 bands
-#define VTX_MSP_TABLE_MAX_CHANNELS 8 // and eight channels
-#define VTX_MSP_TABLE_MAX_POWER_LEVELS 5 //max of VTX_TRAMP_POWER_COUNT, VTX_SMARTAUDIO_POWER_COUNT and VTX_RTC6705_POWER_COUNT
-#define VTX_MSP_TABLE_CHANNEL_NAME_LENGTH 1
-#define VTX_MSP_TABLE_BAND_NAME_LENGTH 8
-#define VTX_MSP_TABLE_POWER_LABEL_LENGTH 3
+#define VTX_MSP_TIMEOUT 250 // ms
+#define VTX_MSP_BAND_COUNT 5
+#define VTX_MSP_CHANNEL_COUNT 8
+#define VTX_MSP_POWER_COUNT 4
bool vtxMspInit(void);
-void setMspVtxDeviceStatusReady(const int descriptor);
-void prepareMspFrame(uint8_t *mspFrame);
-void mspVtxSerialProcess(mspProcessCommandFnPtr mspProcessCommandFn);
+#endif // _VTX_MSP_H
diff --git a/src/main/msp/msp.h b/src/main/msp/msp.h
index 0c00b81ece7..e988ea2eddf 100644
--- a/src/main/msp/msp.h
+++ b/src/main/msp/msp.h
@@ -46,6 +46,7 @@ typedef struct mspPacket_s {
typedef enum {
MSP_FLAG_DONT_REPLY = (1 << 0),
+ MSP_FLAG_ILMI = (1 << 1), // "In-Line Message identifier"
} mspFlags_e;
struct serialPort_s;
diff --git a/src/main/msp/msp_protocol_v2_common.h b/src/main/msp/msp_protocol_v2_common.h
index e778a1808c9..8784b253114 100644
--- a/src/main/msp/msp_protocol_v2_common.h
+++ b/src/main/msp/msp_protocol_v2_common.h
@@ -15,22 +15,25 @@
* along with INAV. If not, see .
*/
-#define MSP2_COMMON_TZ 0x1001 //out message Gets the TZ offset for the local time (returns: minutes(i16))
-#define MSP2_COMMON_SET_TZ 0x1002 //in message Sets the TZ offset for the local time (args: minutes(i16))
-#define MSP2_COMMON_SETTING 0x1003 //in/out message Returns the value for a setting
-#define MSP2_COMMON_SET_SETTING 0x1004 //in message Sets the value for a setting
+#define MSP2_COMMON_TZ 0x1001 //out message Gets the TZ offset for the local time (returns: minutes(i16))
+#define MSP2_COMMON_SET_TZ 0x1002 //in message Sets the TZ offset for the local time (args: minutes(i16))
+#define MSP2_COMMON_SETTING 0x1003 //in/out message Returns the value for a setting
+#define MSP2_COMMON_SET_SETTING 0x1004 //in message Sets the value for a setting
+
+#define MSP2_COMMON_MOTOR_MIXER 0x1005
+#define MSP2_COMMON_SET_MOTOR_MIXER 0x1006
+
+#define MSP2_COMMON_SETTING_INFO 0x1007 //in/out message Returns info about a setting (PG, type, flags, min/max, etc..).
+#define MSP2_COMMON_PG_LIST 0x1008 //in/out message Returns a list of the PG ids used by the settings
+
+#define MSP2_COMMON_SERIAL_CONFIG 0x1009
+#define MSP2_COMMON_SET_SERIAL_CONFIG 0x100A
+
+// radar commands
+#define MSP2_COMMON_SET_RADAR_POS 0x100B //SET radar position information
+#define MSP2_COMMON_SET_RADAR_ITD 0x100C //SET radar information to display
-#define MSP2_COMMON_MOTOR_MIXER 0x1005
-#define MSP2_COMMON_SET_MOTOR_MIXER 0x1006
+#define MSP2_COMMON_SET_MSP_RC_LINK_STATS 0x100D //in message Sets the MSP RC stats
+#define MSP2_COMMON_SET_MSP_RC_INFO 0x100E //in message Sets the MSP RC info
-#define MSP2_COMMON_SETTING_INFO 0x1007 //in/out message Returns info about a setting (PG, type, flags, min/max, etc..).
-#define MSP2_COMMON_PG_LIST 0x1008 //in/out message Returns a list of the PG ids used by the settings
-
-#define MSP2_COMMON_SERIAL_CONFIG 0x1009
-#define MSP2_COMMON_SET_SERIAL_CONFIG 0x100A
-
-// radar commands
-#define MSP2_COMMON_SET_RADAR_POS 0x100B //SET radar position information
-#define MSP2_COMMON_SET_RADAR_ITD 0x100C //SET radar information to display
-
-#define MSP2_BETAFLIGHT_BIND 0x3000
+#define MSP2_BETAFLIGHT_BIND 0x3000
diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c
index 594db21417c..4143664616f 100644
--- a/src/main/programming/logic_condition.c
+++ b/src/main/programming/logic_condition.c
@@ -294,48 +294,53 @@ static int logicConditionCompute(
return true;
break;
#endif
- case LOGIC_CONDITION_SET_VTX_POWER_LEVEL:
+
#if defined(USE_VTX_CONTROL)
-#if(defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP))
- if (
- logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL] != operandA &&
- vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)
- ) {
- logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL] = constrain(operandA, VTX_SETTINGS_MIN_POWER, vtxDeviceCapability.powerCount);
- vtxSettingsConfigMutable()->power = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL];
- return logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL];
- } else {
- return false;
+ case LOGIC_CONDITION_SET_VTX_POWER_LEVEL:
+ {
+ uint8_t newPower = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL];
+ if ((newPower != operandA || newPower != vtxSettingsConfig()->power) && vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) {
+ newPower = constrain(operandA, VTX_SETTINGS_MIN_POWER, vtxDeviceCapability.powerCount);
+ logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_POWER_LEVEL] = newPower;
+ if (newPower != vtxSettingsConfig()->power) {
+ vtxCommonSetPowerByIndex(vtxCommonDevice(), newPower); // Force setting if modified elsewhere
+ }
+ vtxSettingsConfigMutable()->power = newPower;
+ return newPower;
}
- break;
-#else
return false;
-#endif
-
+ break;
+ }
case LOGIC_CONDITION_SET_VTX_BAND:
- if (
- logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_BAND] != operandA &&
- vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)
- ) {
- logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_BAND] = constrain(operandA, VTX_SETTINGS_MIN_BAND, VTX_SETTINGS_MAX_BAND);
- vtxSettingsConfigMutable()->band = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_BAND];
- return logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_BAND];
- } else {
- return false;
+ {
+ uint8_t newBand = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_BAND];
+ if ((newBand != operandA || newBand != vtxSettingsConfig()->band) && vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) {
+ newBand = constrain(operandA, VTX_SETTINGS_MIN_BAND, vtxDeviceCapability.bandCount);
+ logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_BAND] = newBand;
+ if (newBand != vtxSettingsConfig()->band) {
+ vtxCommonSetBandAndChannel(vtxCommonDevice(), newBand, vtxSettingsConfig()->channel);
+ }
+ vtxSettingsConfigMutable()->band = newBand;
+ return newBand;
}
+ return false;
break;
+ }
case LOGIC_CONDITION_SET_VTX_CHANNEL:
- if (
- logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_CHANNEL] != operandA &&
- vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)
- ) {
- logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_CHANNEL] = constrain(operandA, VTX_SETTINGS_MIN_CHANNEL, VTX_SETTINGS_MAX_CHANNEL);
- vtxSettingsConfigMutable()->channel = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_CHANNEL];
- return logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_CHANNEL];
- } else {
- return false;
+ {
+ uint8_t newChannel = logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_CHANNEL];
+ if ((newChannel != operandA || newChannel != vtxSettingsConfig()->channel) && vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) {
+ newChannel = constrain(operandA, VTX_SETTINGS_MIN_CHANNEL, vtxDeviceCapability.channelCount);
+ logicConditionValuesByType[LOGIC_CONDITION_SET_VTX_CHANNEL] = newChannel;
+ if (newChannel != vtxSettingsConfig()->channel) {
+ vtxCommonSetBandAndChannel(vtxCommonDevice(), vtxSettingsConfig()->band, newChannel);
+ }
+ vtxSettingsConfigMutable()->channel = newChannel;
+ return newChannel;
}
+ return false;
break;
+ }
#endif
case LOGIC_CONDITION_INVERT_ROLL:
LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_INVERT_ROLL);
@@ -796,20 +801,36 @@ static int logicConditionGetFlightOperandValue(int operand) {
return constrain(calc_length_pythagorean_2D(GPS_distanceToHome, getEstimatedActualPosition(Z) / 100.0f), 0, INT32_MAX);
break;
- case LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_LQ:
- #ifdef USE_SERIALRX_CRSF
+ case LOGIC_CONDITION_OPERAND_FLIGHT_LQ_UPLINK:
+#if defined(USE_SERIALRX_CRSF) || defined(USE_RX_MSP)
return rxLinkStatistics.uplinkLQ;
- #else
+#else
return 0;
- #endif
+#endif
+ break;
+
+ case LOGIC_CONDITION_OPERAND_FLIGHT_UPLINK_RSSI_DBM:
+#if defined(USE_SERIALRX_CRSF) || defined(USE_RX_MSP)
+ return rxLinkStatistics.uplinkRSSI;
+#else
+ return 0;
+#endif
break;
- case LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_SNR:
- #ifdef USE_SERIALRX_CRSF
+case LOGIC_CONDITION_OPERAND_FLIGHT_LQ_DOWNLINK:
+#if defined(USE_SERIALRX_CRSF) || defined(USE_RX_MSP)
+ return rxLinkStatistics.downlinkLQ;
+#else
+ return 0;
+#endif
+ break;
+
+ case LOGIC_CONDITION_OPERAND_FLIGHT_SNR:
+#if defined(USE_SERIALRX_CRSF) || defined(USE_RX_MSP)
return rxLinkStatistics.uplinkSNR;
- #else
+#else
return 0;
- #endif
+#endif
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_ACTIVE_PROFILE: // int
diff --git a/src/main/programming/logic_condition.h b/src/main/programming/logic_condition.h
index 74a7765be40..f5653bb68cc 100644
--- a/src/main/programming/logic_condition.h
+++ b/src/main/programming/logic_condition.h
@@ -129,8 +129,8 @@ typedef enum {
LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_PITCH, // 26
LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_YAW, // 27
LOGIC_CONDITION_OPERAND_FLIGHT_3D_HOME_DISTANCE, // 28
- LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_LQ, // 29
- LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_SNR, // 39
+ LOGIC_CONDITION_OPERAND_FLIGHT_LQ_UPLINK, // 29
+ LOGIC_CONDITION_OPERAND_FLIGHT_SNR, // 39
LOGIC_CONDITION_OPERAND_FLIGHT_GPS_VALID, // 0/1 // 31
LOGIC_CONDITION_OPERAND_FLIGHT_LOITER_RADIUS, // 32
LOGIC_CONDITION_OPERAND_FLIGHT_ACTIVE_PROFILE, //int // 33
@@ -144,6 +144,8 @@ typedef enum {
LOGIC_CONDITION_OPERAND_FLIGHT_FW_LAND_STATE, // 41
LOGIC_CONDITION_OPERAND_FLIGHT_BATT_PROFILE, // int // 42
LOGIC_CONDITION_OPERAND_FLIGHT_FLOWN_LOITER_RADIUS, // 43
+ LOGIC_CONDITION_OPERAND_FLIGHT_LQ_DOWNLINK, // 44
+ LOGIC_CONDITION_OPERAND_FLIGHT_UPLINK_RSSI_DBM, // 45
} logicFlightOperands_e;
typedef enum {
diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c
index f941eaebe20..00ed23ab25c 100755
--- a/src/main/rx/rx.c
+++ b/src/main/rx/rx.c
@@ -572,6 +572,18 @@ static void setRSSIValue(uint16_t rssiValue, rssiSource_e source, bool filtered)
rssi = constrain(scaleRange(rssi, rssiMin, rssiMax, 0, RSSI_MAX_VALUE), 0, RSSI_MAX_VALUE);
}
+void setRSSIFromMSP_RC(uint8_t newMspRssi)
+{
+ if (activeRssiSource == RSSI_SOURCE_NONE && (rxConfig()->rssi_source == RSSI_SOURCE_MSP || rxConfig()->rssi_source == RSSI_SOURCE_AUTO)) {
+ activeRssiSource = RSSI_SOURCE_MSP;
+ }
+
+ if (activeRssiSource == RSSI_SOURCE_MSP) {
+ rssi = constrain(scaleRange(constrain(newMspRssi, 0, 100), 0, 100, 0, RSSI_MAX_VALUE), 0, RSSI_MAX_VALUE);
+ lastMspRssiUpdateUs = micros();
+ }
+}
+
void setRSSIFromMSP(uint8_t newMspRssi)
{
if (activeRssiSource == RSSI_SOURCE_NONE && (rxConfig()->rssi_source == RSSI_SOURCE_MSP || rxConfig()->rssi_source == RSSI_SOURCE_AUTO)) {
diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h
index c841838a5ea..64b97b172e2 100644
--- a/src/main/rx/rx.h
+++ b/src/main/rx/rx.h
@@ -181,12 +181,16 @@ typedef enum {
} rssiSource_e;
typedef struct rxLinkStatistics_s {
- int16_t uplinkRSSI; // RSSI value in dBm
- uint8_t uplinkLQ; // A protocol specific measure of the link quality in [0..100]
- int8_t uplinkSNR; // The SNR of the uplink in dB
- uint8_t rfMode; // A protocol specific measure of the transmission bandwidth [2 = 150Hz, 1 = 50Hz, 0 = 4Hz]
- uint16_t uplinkTXPower; // power in mW
- uint8_t activeAntenna;
+ int16_t uplinkRSSI; // RSSI value in dBm
+ uint8_t uplinkLQ; // A protocol specific measure of the link quality in [0..100]
+ uint8_t downlinkLQ; // A protocol specific measure of the link quality in [0..100]
+ int8_t uplinkSNR; // The SNR of the uplink in dB
+ uint8_t rfMode; // A protocol specific measure of the transmission bandwidth [2 = 150Hz, 1 = 50Hz, 0 = 4Hz]
+ uint16_t uplinkTXPower; // power in mW
+ uint16_t downlinkTXPower; // power in mW
+ uint8_t activeAntenna;
+ char band[4];
+ char mode[6];
} rxLinkStatistics_t;
typedef uint16_t (*rcReadRawDataFnPtr)(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); // used by receiver driver to return channel data
@@ -212,6 +216,7 @@ bool isRxPulseValid(uint16_t pulseDuration);
uint8_t calculateChannelRemapping(const uint8_t *channelMap, uint8_t channelMapEntryCount, uint8_t channelToRemap);
void parseRcChannels(const char *input);
+void setRSSIFromMSP_RC(uint8_t newMspRssi);
void setRSSIFromMSP(uint8_t newMspRssi);
void updateRSSI(timeUs_t currentTimeUs);
// Returns RSSI in [0, RSSI_MAX_VALUE] range.
diff --git a/src/main/target/KAKUTEF4/target.h b/src/main/target/KAKUTEF4/target.h
index c1ce47b9e80..92cbcf13414 100644
--- a/src/main/target/KAKUTEF4/target.h
+++ b/src/main/target/KAKUTEF4/target.h
@@ -78,6 +78,8 @@
# define BARO_I2C_BUS BUS_I2C1
# define USE_BARO_MS5611
# define USE_BARO_BMP280
+# define USE_BARO_DPS310
+# define USE_BARO_SPL06
#else // V1 does not have I2C exposed, common_post.h will pull in USE_*_MSP
# define USE_BARO
# define USE_MAG
diff --git a/src/main/target/KAKUTEF7/target.h b/src/main/target/KAKUTEF7/target.h
index f3f4edfed40..79f9d079353 100644
--- a/src/main/target/KAKUTEF7/target.h
+++ b/src/main/target/KAKUTEF7/target.h
@@ -131,6 +131,8 @@
#define USE_BARO
#define USE_BARO_BMP280
#define USE_BARO_MS5611
+#define USE_BARO_DPS310
+#define USE_BARO_SPL06
#define BARO_I2C_BUS BUS_I2C1
#define USE_MAG
diff --git a/src/main/target/KAKUTEF7MINIV3/target.h b/src/main/target/KAKUTEF7MINIV3/target.h
index bf485ebc82b..89ba7c632b5 100644
--- a/src/main/target/KAKUTEF7MINIV3/target.h
+++ b/src/main/target/KAKUTEF7MINIV3/target.h
@@ -117,6 +117,8 @@
#define BARO_I2C_BUS BUS_I2C1
#define USE_BARO_BMP280
#define USE_BARO_MS5611
+#define USE_BARO_DPS310
+#define USE_BARO_SPL06
/*
* Mag
diff --git a/src/main/target/TUNERCF405/target.h b/src/main/target/TUNERCF405/target.h
index f193377c6ea..b359545e697 100644
--- a/src/main/target/TUNERCF405/target.h
+++ b/src/main/target/TUNERCF405/target.h
@@ -103,6 +103,8 @@
#define USE_UART4
#define UART4_RX_PIN PA1
#define UART4_TX_PIN PA0
+#define USE_UART_INVERTER
+#define INVERTER_PIN_UART4_RX PC14
#define USE_UART5
#define UART5_RX_PIN PD2
diff --git a/src/main/telemetry/crsf.h b/src/main/telemetry/crsf.h
old mode 100755
new mode 100644