Merge branch 'master' into delayed_safehome

README.md

@@ -39,7 +39,7 @@ Tool for Blackbox logs analysis is available [here](https://github.com/iNavFligh
 
 ### Telemetry screen for OpenTX
 
-Users of FrSky Taranis X9 and Q X7 can use INAV Lua Telemetry screen created by @teckel12 . Software and installation instruction are available here: [https://github.com/iNavFlight/LuaTelemetry](https://github.com/iNavFlight/LuaTelemetry)
+Users of OpenTX radios (Taranis, Horus, Jumper, Radiomaster, Nirvana) can use INAV OpenTX Telemetry Widget screen. Software and installation instruction are available here: [https://github.com/iNavFlight/OpenTX-Telemetry-Widget](https://github.com/iNavFlight/OpenTX-Telemetry-Widget)
 
 ## Installation
 

docs/Telemetry.md

@@ -123,7 +123,7 @@ The following sensors are transmitted
 * **0450** : 'Flight Path Vector' or 'Course over ground' in degrees*10
 ### Compatible SmartPort/INAV telemetry flight status
 
-To quickly and easily monitor these SmartPort sensors and flight modes, install [iNav LuaTelemetry](https://github.com/iNavFlight/LuaTelemetry) to your Taranis Q X7, X9D, X9D+ or X9E transmitter.
+To quickly and easily monitor these SmartPort sensors and flight modes, install [OpenTX Telemetry Widget](https://github.com/iNavFlight/OpenTX-Telemetry-Widget) to your Taranis Q X7, X9D, X9D+ or X9E transmitter.
 
 ## FrSky telemetry
 

src/main/fc/fc_msp.c

@@ -567,6 +567,11 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
             sbufWriteU16(dst, programmingPids(i)->gains.FF);
         }
         break;
+    case MSP2_INAV_PROGRAMMING_PID_STATUS:
+        for (int i = 0; i < MAX_PROGRAMMING_PID_COUNT; i++) {
+            sbufWriteU32(dst, programmingPidGetOutput(i));
+        }
+        break;
 #endif
     case MSP2_COMMON_MOTOR_MIXER:
         for (uint8_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) {

src/main/fc/settings.yaml

@@ -2807,8 +2807,8 @@ groups:
         description: "Value below which Crossfire SNR Alarm pops-up. (dB)"
         default_value: "4"
         field: snr_alarm
-        min: -12
-        max: 8
+        min: -20
+        max: 10
       - name: osd_link_quality_alarm
         condition: USE_SERIALRX_CRSF
         description: "LQ % indicator blinks below this value. For Crossfire use 70%, for Tracer use 50%"

src/main/io/osd.c

@@ -1699,7 +1699,7 @@ static bool osdDrawSingleElement(uint8_t item)
         }
 
     case OSD_CRSF_SNR_DB: {
-        const char* showsnr = "-12";
+        const char* showsnr = "-20";
         const char* hidesnr = "     ";
         int16_t osdSNR_Alarm = rxLinkStatistics.uplinkSNR;
         if (osdSNR_Alarm <= osdConfig()->snr_alarm) {
@@ -1776,9 +1776,6 @@ static bool osdDrawSingleElement(uint8_t item)
                 gpsLocation_t wp2;
                 int j;
 
-                tfp_sprintf(buff, "W%u/%u", posControl.activeWaypointIndex, posControl.waypointCount);
-                displayWrite(osdGetDisplayPort(), 13, osdConfig()->hud_margin_v - 1, buff);
-
                 for (int i = osdConfig()->hud_wp_disp - 1; i >= 0 ; i--) { // Display in reverse order so the next WP is always written on top
                     j = posControl.activeWaypointIndex + i;
                     if (posControl.waypointList[j].lat != 0 && posControl.waypointList[j].lon != 0 && j <= posControl.waypointCount) {

src/main/io/osd.h

@@ -297,7 +297,7 @@ typedef struct osdConfig_s {
     float gforce_axis_alarm_min;
     float gforce_axis_alarm_max;
 #ifdef USE_SERIALRX_CRSF
-    int16_t snr_alarm; //CRSF SNR alarm in dB
+    int8_t snr_alarm; //CRSF SNR alarm in dB
     int8_t link_quality_alarm;
 #endif
 #ifdef USE_BARO

src/main/msp/msp_protocol_v2_inav.h

@@ -63,6 +63,7 @@
 #define MSP2_INAV_GVAR_STATUS                   0x2027
 #define MSP2_INAV_PROGRAMMING_PID               0x2028
 #define MSP2_INAV_SET_PROGRAMMING_PID           0x2029
+#define MSP2_INAV_PROGRAMMING_PID_STATUS        0x202A
 
 #define MSP2_PID                                0x2030
 #define MSP2_SET_PID                            0x2031
@@ -79,4 +80,3 @@
 #define MSP2_INAV_SET_SAFEHOME                  0x2039
 
 #define MSP2_INAV_MISC2                         0x203A
-

src/main/navigation/navigation_pos_estimator.c

@@ -569,7 +569,7 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
         if (ctx->newFlags & EST_GPS_Z_VALID) {
             // Trust GPS velocity only if residual/error is less than 2.5 m/s, scale weight according to gaussian distribution
             const float gpsRocResidual = posEstimator.gps.vel.z - posEstimator.est.vel.z;
-            const float gpsRocScaler = bellCurve(gpsRocResidual, 2.5f);
+            const float gpsRocScaler = bellCurve(gpsRocResidual, 250.0f);
             ctx->estVelCorr.z += gpsRocResidual * positionEstimationConfig()->w_z_gps_v * gpsRocScaler * ctx->dt;
         }
 

src/main/programming/logic_condition.c

@@ -418,7 +418,7 @@ static int logicConditionGetFlightOperandValue(int operand) {
             break;
 
         case LOGIC_CONDITION_OPERAND_FLIGHT_VERTICAL_SPEED: // cm/s
-            return constrain(getEstimatedActualVelocity(Z), 0, INT16_MAX);
+            return constrain(getEstimatedActualVelocity(Z), INT16_MIN, INT16_MAX);
             break;
 
         case LOGIC_CONDITION_OPERAND_FLIGHT_TROTTLE_POS: // %

src/main/programming/pid.c

@@ -110,7 +110,7 @@ void programmingPidInit(void)
     }
 }
 
-int programmingPidGetOutput(uint8_t i) {
+int32_t programmingPidGetOutput(uint8_t i) {
     return programmingPidState[constrain(i, 0, MAX_PROGRAMMING_PID_COUNT)].output;
 }
 

src/main/programming/pid.h

@@ -51,4 +51,4 @@ typedef struct programmingPidState_s {
 void programmingPidUpdateTask(timeUs_t currentTimeUs);
 void programmingPidInit(void);
 void programmingPidReset(void);
-int programmingPidGetOutput(uint8_t i);
+int32_t programmingPidGetOutput(uint8_t i);

src/main/sensors/barometer.c

@@ -56,7 +56,7 @@
 
 baro_t baro;                        // barometer access functions
 
-PG_REGISTER_WITH_RESET_TEMPLATE(barometerConfig_t, barometerConfig, PG_BAROMETER_CONFIG, 2);
+PG_REGISTER_WITH_RESET_TEMPLATE(barometerConfig_t, barometerConfig, PG_BAROMETER_CONFIG, 3);
 
 #ifdef USE_BARO
 #define BARO_HARDWARE_DEFAULT    BARO_AUTODETECT
@@ -65,7 +65,7 @@ PG_REGISTER_WITH_RESET_TEMPLATE(barometerConfig_t, barometerConfig, PG_BAROMETER
 #endif
 PG_RESET_TEMPLATE(barometerConfig_t, barometerConfig,
     .baro_hardware = BARO_HARDWARE_DEFAULT,
-    .use_median_filtering = 0,
+    .use_median_filtering = 1,
     .baro_calibration_tolerance = 150
 );
 

src/main/sensors/compass.c

@@ -350,7 +350,7 @@ void compassUpdate(timeUs_t currentTimeUs)
     static sensorCalibrationState_t calState;
     static timeUs_t calStartedAt = 0;
     static int16_t magPrev[XYZ_AXIS_COUNT];
-    static int magGain[XYZ_AXIS_COUNT] = {-4096, -4096, -4096};
+    static int magAxisDeviation[XYZ_AXIS_COUNT];
 
     // Check magZero
     if (
@@ -381,6 +381,7 @@ void compassUpdate(timeUs_t currentTimeUs)
             compassConfigMutable()->magZero.raw[axis] = 0;
             compassConfigMutable()->magGain[axis] = 1024;
             magPrev[axis] = 0;
+            magAxisDeviation[axis] = 0;  // Gain is based on the biggest absolute deviation from the mag zero point. Gain computation starts at 0
         }
 
         beeper(BEEPER_ACTION_SUCCESS);
@@ -400,9 +401,9 @@ void compassUpdate(timeUs_t currentTimeUs)
                 diffMag += (mag.magADC[axis] - magPrev[axis]) * (mag.magADC[axis] - magPrev[axis]);
                 avgMag += (mag.magADC[axis] + magPrev[axis]) * (mag.magADC[axis] + magPrev[axis]) / 4.0f;
 
-                const int32_t sample = ABS(mag.magADC[axis]);
-                if (sample > magGain[axis]) {
-                    magGain[axis] = sample;
+                // Find the biggest sample deviation together with sample' sign
+                if (ABS(mag.magADC[axis]) > ABS(magAxisDeviation[axis])) {
+                    magAxisDeviation[axis] = mag.magADC[axis];
                 }
 
             }
@@ -429,7 +430,7 @@ void compassUpdate(timeUs_t currentTimeUs)
              * It is dirty, but worth checking if this will solve the problem of changing mag vector when UAV is tilted
              */
             for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-                compassConfigMutable()->magGain[axis] = magGain[axis] - compassConfig()->magZero.raw[axis];
+                compassConfigMutable()->magGain[axis] = ABS(magAxisDeviation[axis] - compassConfig()->magZero.raw[axis]);
             }
 
             calStartedAt = 0;

src/main/target/RUSH_BLADE_F7/CMakeLists.txt

@@ -0,0 +1 @@
+target_stm32f722xe(RUSH_BLADE_F7 SKIP_RELEASES)

src/main/target/RUSH_BLADE_F7/target.c

@@ -0,0 +1,37 @@
+/*
+ * This file is part of INAV.
+ *
+ * INAV is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * INAV 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with INAV.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdint.h>
+#include "platform.h"
+#include "drivers/bus.h"
+#include "drivers/io.h"
+#include "drivers/pwm_mapping.h"
+#include "drivers/timer.h"
+
+const timerHardware_t timerHardware[] = {
+
+    DEF_TIM(TIM8, CH3, PC8,     TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),   // S1   UP2-1   D(2, 4, 7)
+    DEF_TIM(TIM8, CH4, PC9,     TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),   // S2   UP2-1   D(2, 7, 7)
+    DEF_TIM(TIM3, CH4, PB1,     TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0),   // S6   UP1-2   D(1, 2, 5)
+    DEF_TIM(TIM3, CH3, PB0,     TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0),   // S5   UP1-2   D(1, 7, 5)
+    DEF_TIM(TIM8, CH2, PC7,     TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),   // S5 D(2, 3, 7)
+    DEF_TIM(TIM8, CH1, PC6,     TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),   // S6 D(1, 4, 5)
+
+    DEF_TIM(TIM2, CH1, PA15,    TIM_USE_LED, 0, 0),                            // LED 
+};
+
+const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);

src/main/target/RUSH_BLADE_F7/target.h

@@ -0,0 +1,147 @@
+/*
+ * This file is part of INAV.
+ *
+ * INAV is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * INAV 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with INAV.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+
+#pragma once
+
+#define TARGET_BOARD_IDENTIFIER "RBF7"
+#define USBD_PRODUCT_STRING  "RUSH_BLADE_F7"
+
+#define LED0                    PB10  //Blue   SWCLK
+// #define LED1                    PA13  //Green  SWDIO
+
+#define BEEPER                  PB2
+#define BEEPER_INVERTED
+
+// *************** SPI1 Gyro & ACC *******************
+#define USE_SPI
+#define USE_SPI_DEVICE_1
+#define SPI1_SCK_PIN            PA5
+#define SPI1_MISO_PIN           PA6
+#define SPI1_MOSI_PIN           PA7
+
+#define USE_EXTI
+#define USE_MPU_DATA_READY_SIGNAL
+#define GYRO_INT_EXTI           PA4
+
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW270_DEG
+#define MPU6000_CS_PIN          PC4
+#define MPU6000_SPI_BUS         BUS_SPI1
+
+// *************** I2C /Baro/Mag *********************
+#define USE_I2C
+#define USE_I2C_DEVICE_1
+#define I2C1_SCL                PB8
+#define I2C1_SDA                PB9
+
+#define USE_BARO
+#define BARO_I2C_BUS            BUS_I2C1
+#define USE_BARO_BMP280
+#define USE_BARO_MS5611
+#define USE_BARO_DPS310
+
+#define USE_MAG
+#define MAG_I2C_BUS             BUS_I2C1
+#define USE_MAG_AK8975
+#define USE_MAG_HMC5883
+#define USE_MAG_QMC5883
+#define USE_MAG_IST8310
+#define USE_MAG_IST8308
+#define USE_MAG_MAG3110
+#define USE_MAG_LIS3MDL
+
+#define TEMPERATURE_I2C_BUS     BUS_I2C1
+
+#define PITOT_I2C_BUS           BUS_I2C1
+
+#define USE_RANGEFINDER
+#define RANGEFINDER_I2C_BUS     BUS_I2C1
+
+// *************** SPI2 Flash ***********************
+#define USE_SPI_DEVICE_2
+#define SPI2_SCK_PIN            PB13
+#define SPI2_MISO_PIN           PB14
+#define SPI2_MOSI_PIN           PB15
+
+#define USE_FLASHFS
+#define USE_FLASH_M25P16
+#define M25P16_SPI_BUS          BUS_SPI2
+#define M25P16_CS_PIN           PB12
+#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
+
+// *************** UART *****************************
+#define USE_VCP
+#define USB_DETECT_PIN          PC14
+#define USE_USB_DETECT
+
+#define USE_UART1
+#define UART1_TX_PIN            PA9
+#define UART1_RX_PIN            PA10
+
+#define USE_UART2
+#define UART2_TX_PIN            PA2
+#define UART2_RX_PIN            PA3
+
+#define USE_UART3
+#define UART3_TX_PIN            PC10
+#define UART3_RX_PIN            PC11
+
+#define USE_UART4
+#define UART4_TX_PIN            PA0
+#define UART4_RX_PIN            PA1
+
+#define USE_UART5
+#define UART5_TX_PIN            PC12
+#define UART5_RX_PIN            PD2
+
+#define SERIAL_PORT_COUNT       6
+
+#define DEFAULT_RX_TYPE         RX_TYPE_SERIAL
+#define SERIALRX_PROVIDER       SERIALRX_SBUS
+#define SERIALRX_UART           SERIAL_PORT_USART2
+
+// *************** ADC *****************************
+#define USE_ADC
+#define ADC_INSTANCE                ADC1
+#define ADC1_DMA_STREAM             DMA2_Stream0
+
+#define ADC_CHANNEL_1_PIN           PC0
+#define ADC_CHANNEL_2_PIN           PC1
+
+#define VBAT_ADC_CHANNEL            ADC_CHN_1
+#define CURRENT_METER_ADC_CHANNEL   ADC_CHN_2
+
+// *************** LEDSTRIP ************************
+#define USE_LED_STRIP
+#define WS2811_PIN                  PA8
+
+#define DEFAULT_FEATURES   (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL)
+#define CURRENT_METER_SCALE     179
+
+#define USE_SERIAL_4WAY_BLHELI_INTERFACE
+
+#define TARGET_IO_PORTA 0xffff
+#define TARGET_IO_PORTB 0xffff
+#define TARGET_IO_PORTC 0xffff
+#define TARGET_IO_PORTD 0xffff
+
+#define MAX_PWM_OUTPUT_PORTS        10
+#define USE_OSD
+#define USE_DSHOT
+#define USE_SERIALSHOT
+#define USE_ESC_SENSOR