Merge branch 'master' of https://github.com/iNavFlight/inav into Geoz…

…ones

docs/Settings.md

@@ -5248,7 +5248,7 @@ Value below which Crossfire SNR Alarm pops-up. (dB)
 
 | Default | Min | Max |
 | --- | --- | --- |
-| 4 | -20 | 10 |
+| 4 | -20 | 99 |
 
 ---
 

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,
 };

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;

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);

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;
 

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

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

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
 

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
@@ -1855,7 +1856,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);
@@ -1879,7 +1880,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:
@@ -1909,6 +1910,8 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
                 }
                 rxMspFrameReceive(frame, channelCount);
             }
+
+            return MSP_RESULT_NO_REPLY;
         }
         break;
 #endif
@@ -2740,21 +2743,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, &currentPower);
-                        if (newPower != currentPower) {
-                            vtxCommonSetPowerByIndex(vtxDevice, newPower);
+                        if (vtxSettingsConfig()->power != newPower) {
                             vtxSettingsConfigMutable()->power = newPower;
                         }
 
@@ -2770,28 +2767,37 @@ 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
                         }
 
                         // 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;
+                            }                        
+                        }
                     }
                 }
             }
@@ -2962,6 +2968,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);
@@ -3792,7 +3845,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 )

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
 
 }
@@ -418,7 +415,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

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;

src/main/fc/settings.yaml

@@ -3376,35 +3376,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