From cb706308417fbc7a679b24b70c018bc4a8ba9d17 Mon Sep 17 00:00:00 2001
From: Olivier C <olivierc600@gmail.com>
Date: Sun, 1 Dec 2019 11:43:35 +0100
Subject: [PATCH 001/179] Show next waypoints as POI in the hud

+ Fix for the AH mode displayed instead of WP
+ Less hud Y margins (because NTSC only 13 lines)
+ Improved side-stacking for the POIs
+ Smaller min range for radar POIs (3m)
---
 src/main/cms/cms_menu_osd.c    |  1 +
 src/main/drivers/osd_symbols.h |  2 +-
 src/main/fc/settings.yaml      |  8 +++++--
 src/main/io/osd.c              | 43 ++++++++++++++++++++++++++++------
 src/main/io/osd.h              |  1 +
 src/main/io/osd_hud.c          | 31 ++++++++++++++----------
 src/main/io/osd_hud.h          |  2 +-
 7 files changed, 64 insertions(+), 24 deletions(-)

diff --git a/src/main/cms/cms_menu_osd.c b/src/main/cms/cms_menu_osd.c
index 5ba507addd..71040bafe8 100755
--- a/src/main/cms/cms_menu_osd.c
+++ b/src/main/cms/cms_menu_osd.c
@@ -374,6 +374,7 @@ static const OSD_Entry menuOsdHud2Entries[] = {
     OSD_SETTING_ENTRY("RADAR MIN RANGE", SETTING_OSD_HUD_RADAR_RANGE_MIN),
     OSD_SETTING_ENTRY("RADAR MAX RANGE", SETTING_OSD_HUD_RADAR_RANGE_MAX),
     OSD_SETTING_ENTRY("RADAR DET. NEAREST", SETTING_OSD_HUD_RADAR_NEAREST),
+    OSD_SETTING_ENTRY("NEXT WAYPOINTS", SETTING_OSD_HUD_WP_DISP),
     OSD_BACK_ENTRY,
     OSD_END_ENTRY,
 };
diff --git a/src/main/drivers/osd_symbols.h b/src/main/drivers/osd_symbols.h
index cd9bb83258..c289bfbf44 100644
--- a/src/main/drivers/osd_symbols.h
+++ b/src/main/drivers/osd_symbols.h
@@ -117,7 +117,7 @@
 #define SYM_3D_MPH                0x8A // 138 MPH 3D
 
 #define SYM_RPM                   0x8B // 139 RPM
-//                                0x8C // 140 -
+#define SYM_WAYPOINT              0x8C // 140 Waypoint
 //                                0x8D // 141 -
 //                                0x8E // 142 -
 //                                0x8F // 143 -
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 8c583229c8..7078b9dc96 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -1876,8 +1876,8 @@ groups:
         max: 4
       - name: osd_hud_margin_v
         field: hud_margin_v
-        min: 0
-        max: 4
+        min: 1
+        max: 3
       - name: osd_hud_homing
         field: hud_homing
         type: bool
@@ -1900,6 +1900,10 @@ groups:
         field: hud_radar_nearest
         min: 0
         max: 4000
+      - name: osd_hud_wp_disp
+        field: hud_wp_disp
+        min: 0
+        max: 3
       - name: osd_left_sidebar_scroll
         field: left_sidebar_scroll
         table: osd_sidebar_scroll
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index a61947d4e3..e7550c6eec 100755
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -797,7 +797,7 @@ static const char * navigationStateMessage(void)
             break;
         case MW_NAV_STATE_WP_ENROUTE:
             // TODO: Show WP number
-            return OSD_MESSAGE_STR("EN ROUTE TO WAYPOINT");
+            return OSD_MESSAGE_STR("TO WP");
         case MW_NAV_STATE_PROCESS_NEXT:
             return OSD_MESSAGE_STR("PREPARING FOR NEXT WAYPOINT");
         case MW_NAV_STATE_DO_JUMP:
@@ -1644,13 +1644,14 @@ static bool osdDrawSingleElement(uint8_t item)
                 p = "3CRS";
             else if (FLIGHT_MODE(NAV_CRUISE_MODE))
                 p = "CRS ";
+            else if (FLIGHT_MODE(NAV_WP_MODE))
+                p = " WP ";
             else if (FLIGHT_MODE(NAV_ALTHOLD_MODE) && navigationRequiresAngleMode()) {
                 // If navigationRequiresAngleMode() returns false when ALTHOLD is active,
                 // it means it can be combined with ANGLE, HORIZON, ACRO, etc...
                 // and its display is handled by OSD_MESSAGES rather than OSD_FLYMODE.
                 p = " AH ";
-            } else if (FLIGHT_MODE(NAV_WP_MODE))
-                p = " WP ";
+            }
             else if (FLIGHT_MODE(ANGLE_MODE))
                 p = "ANGL";
             else if (FLIGHT_MODE(HORIZON_MODE))
@@ -1725,14 +1726,18 @@ static bool osdDrawSingleElement(uint8_t item)
 
         if (STATE(GPS_FIX) && isImuHeadingValid()) {
 
-            if (osdConfig()->hud_homepoint || osdConfig()->hud_radar_disp > 0) {
+            if (osdConfig()->hud_homepoint || osdConfig()->hud_radar_disp > 0 || osdConfig()->hud_wp_disp > 0) {
                     osdHudClear();
             }
 
+            // -------- POI : Home point
+
             if (osdConfig()->hud_homepoint) { // Display the home point (H)
-                osdHudDrawPoi(GPS_distanceToHome, GPS_directionToHome, -osdGetAltitude() / 100, 0, 5, SYM_HOME);
+                osdHudDrawPoi(GPS_distanceToHome, GPS_directionToHome, -osdGetAltitude() / 100, 0, SYM_HOME, 0 , 0);
             }
 
+            // -------- POI : Nearby aircrafts from ESP32 radar
+
             if (osdConfig()->hud_radar_disp > 0) { // Display the POI from the radar
                 for (uint8_t i = 0; i < osdConfig()->hud_radar_disp; i++) {
                     if (radar_pois[i].gps.lat != 0 && radar_pois[i].gps.lon != 0 && radar_pois[i].state < 2) { // state 2 means POI has been lost and must be skipped
@@ -1743,7 +1748,7 @@ static bool osdDrawSingleElement(uint8_t item)
                         if (radar_pois[i].distance >= osdConfig()->hud_radar_range_min && radar_pois[i].distance <= osdConfig()->hud_radar_range_max) {
                             radar_pois[i].direction = calculateBearingToDestination(&poi) / 100; // In °
                             radar_pois[i].altitude = (radar_pois[i].gps.alt - osdGetAltitudeMsl()) / 100;
-                            osdHudDrawPoi(radar_pois[i].distance, osdGetHeadingAngle(radar_pois[i].direction), radar_pois[i].altitude, radar_pois[i].heading, radar_pois[i].lq, 65 + i);
+                            osdHudDrawPoi(radar_pois[i].distance, osdGetHeadingAngle(radar_pois[i].direction), radar_pois[i].altitude, 1, 65 + i, radar_pois[i].heading, radar_pois[i].lq);
                         }
                     }
                 }
@@ -1755,6 +1760,29 @@ static bool osdDrawSingleElement(uint8_t item)
                     }
                 }
             }
+
+            // -------- POI : Next waypoints from navigation
+
+            if (osdConfig()->hud_wp_disp > 0 && posControl.waypointListValid && posControl.waypointCount > 0) { // Display the next waypoints
+                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 (uint8_t 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) {
+                        wp2.lat = posControl.waypointList[j].lat;
+                        wp2.lon = posControl.waypointList[j].lon;
+                        wp2.alt = posControl.waypointList[j].alt;
+                        fpVector3_t poi;
+                        geoConvertGeodeticToLocal(&poi, &posControl.gpsOrigin, &wp2, GEO_ALT_RELATIVE);
+                        while (j > 9) j -= 10; // Only the last digit displayed if WP>=10, no room for more
+                        osdHudDrawPoi(calculateDistanceToDestination(&poi) / 100, osdGetHeadingAngle(calculateBearingToDestination(&poi) / 100), (posControl.waypointList[j].alt - osdGetAltitude())/ 100, 2, SYM_WAYPOINT, 49 + j, i);
+                    }
+                }
+            }
         }
 
         return true;
@@ -2727,9 +2755,10 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig)
     osdConfig->hud_homing = 0;
     osdConfig->hud_homepoint = 0;
     osdConfig->hud_radar_disp = 0;
-    osdConfig->hud_radar_range_min = 10;
+    osdConfig->hud_radar_range_min = 3;
     osdConfig->hud_radar_range_max = 4000;
     osdConfig->hud_radar_nearest = 0;
+    osdConfig->hud_wp_disp = 0;
     osdConfig->left_sidebar_scroll = OSD_SIDEBAR_SCROLL_NONE;
     osdConfig->right_sidebar_scroll = OSD_SIDEBAR_SCROLL_NONE;
     osdConfig->sidebar_scroll_arrows = 0;
diff --git a/src/main/io/osd.h b/src/main/io/osd.h
index 6205e772c5..43fa41274c 100755
--- a/src/main/io/osd.h
+++ b/src/main/io/osd.h
@@ -231,6 +231,7 @@ typedef struct osdConfig_s {
     uint16_t hud_radar_range_min;
     uint16_t hud_radar_range_max;
     uint16_t hud_radar_nearest;
+    uint8_t hud_wp_disp;
     
     uint8_t left_sidebar_scroll; // from osd_sidebar_scroll_e
     uint8_t right_sidebar_scroll; // from osd_sidebar_scroll_e
diff --git a/src/main/io/osd_hud.c b/src/main/io/osd_hud.c
index 35c1f58078..535bb32c9e 100644
--- a/src/main/io/osd_hud.c
+++ b/src/main/io/osd_hud.c
@@ -36,7 +36,7 @@
 
 #ifdef USE_OSD
 
-#define HUD_DRAWN_MAXCHARS 40 // 6 POI (1 home, 5 radar) x 7 chars max for each minus 2 for H (no icons for heading and signal)
+#define HUD_DRAWN_MAXCHARS 54 // 8 POI (1 home, 4 radar, 3 WP) x 7 chars max for each, minus 2 for H
 
 static int8_t hud_drawn[HUD_DRAWN_MAXCHARS][2];
 static int8_t hud_drawn_pt;
@@ -113,16 +113,20 @@ int8_t radarGetNearestPOI(void)
 }
 
 /*
- * Display one POI on the hud, centered on crosshair position.
- * Distance (m), Direction (°), Altitude (relative, m, negative means below), Heading (°), Signal 0 to 5, Symbol 0 to 480
+ * Display a POI as a 3D-marker on the hud
+ * Distance (m), Direction (°), Altitude (relative, m, negative means below), Heading (°),
+ * Type = 0 : Home point
+ * Type = 1 : Radar POI, P1: Heading, P2: Signal
+ * Type = 2 : Waypoint, P1: WP number, P2: 1=WP+1, 2=WP+2, 3=WP+3
  */
-void osdHudDrawPoi(uint32_t poiDistance, int16_t poiDirection, int32_t poiAltitude, int16_t poiHeading, uint8_t poiSignal, uint16_t poiSymbol)
+void osdHudDrawPoi(uint32_t poiDistance, int16_t poiDirection, int32_t poiAltitude, uint8_t poiType, uint16_t poiSymbol, int16_t poiP1, int16_t poiP2)
 {
     int poi_x;
     int poi_y;
     uint8_t center_x;
     uint8_t center_y;
     bool poi_is_oos = 0;
+    char buff[3];
 
     uint8_t minX = osdConfig()->hud_margin_h + 2;
     uint8_t maxX = osdGetDisplayPort()->cols - osdConfig()->hud_margin_h - 3;
@@ -161,10 +165,10 @@ void osdHudDrawPoi(uint32_t poiDistance, int16_t poiDirection, int32_t poiAltitu
         uint16_t c;
 
         poi_x = (error_x > 0 ) ? maxX : minX;
-        poi_y = center_y;
+        poi_y = center_y - 1;
 
         if (displayReadCharWithAttr(osdGetDisplayPort(), poi_x, poi_y, &c, NULL) && c != SYM_BLANK) {
-            poi_y = center_y - 2;
+            poi_y = center_y - 3;
             while (displayReadCharWithAttr(osdGetDisplayPort(), poi_x, poi_y, &c, NULL) && c != SYM_BLANK && poi_y < maxY - 3) { // Stacks the out-of-sight POI from top to bottom
                 poi_y += 2;
             }
@@ -180,21 +184,22 @@ void osdHudDrawPoi(uint32_t poiDistance, int16_t poiDirection, int32_t poiAltitu
         }
     }
 
-    // POI marker (A B C ...)
+    // Markers
 
     osdHudWrite(poi_x, poi_y, poiSymbol, 1);
 
-    // Signal on the right, heading on the left
-
-    if (poiSignal < 5) { // 0 to 4 = signal bars, 5 = No LQ and no heading displayed
-        error_x = hudWrap360(poiHeading - DECIDEGREES_TO_DEGREES(osdGetHeading()));
+    if (poiType == 1) { // POI from the ESP radar
+        error_x = hudWrap360(poiP1 - DECIDEGREES_TO_DEGREES(osdGetHeading()));
         osdHudWrite(poi_x - 1, poi_y, SYM_DIRECTION + ((error_x + 22) / 45) % 8, 1);
-        osdHudWrite(poi_x + 1, poi_y, SYM_HUD_SIGNAL_0 + poiSignal, 1);
+        osdHudWrite(poi_x + 1, poi_y, SYM_HUD_SIGNAL_0 + poiP2, 1);
     }
+    else if (poiType == 2) { // Waypoint,
+        osdHudWrite(poi_x - 1, poi_y, SYM_HUD_ARROWS_U1 + poiP2, 1);
+        osdHudWrite(poi_x + 1, poi_y, poiP1, 1);
+        }
 
     // Distance
 
-    char buff[3];
     if ((osd_unit_e)osdConfig()->units == OSD_UNIT_IMPERIAL) {
         osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(poiDistance * 100), FEET_PER_MILE, 0, 3, 3);
     }
diff --git a/src/main/io/osd_hud.h b/src/main/io/osd_hud.h
index 9744ad0ec4..3845c2347b 100644
--- a/src/main/io/osd_hud.h
+++ b/src/main/io/osd_hud.h
@@ -23,7 +23,7 @@
 void osdHudClear(void);
 void osdHudDrawCrosshair(uint8_t px, uint8_t py);
 void osdHudDrawHoming(uint8_t px, uint8_t py);
-void osdHudDrawPoi(uint32_t poiDistance, int16_t poiDirection, int32_t poiAltitude, int16_t poiHeading, uint8_t poiSignal, uint16_t poiSymbol);
+void osdHudDrawPoi(uint32_t poiDistance, int16_t poiDirection, int32_t poiAltitude, uint8_t poiType, uint16_t poiSymbol, int16_t poiP1, int16_t poiP2);
 void osdHudDrawExtras(uint8_t poi_id);
 int8_t radarGetNearestPOI(void);
 

From 90fc08dbf49998c938fd62ba607ad73c2790f226 Mon Sep 17 00:00:00 2001
From: Konstantin Sharlaimov <konstantin.sharlaimov@gmail.com>
Date: Sat, 30 Nov 2019 18:47:04 +0100
Subject: [PATCH 002/179] Revert "Version bump to 2.4"

This reverts commit a9f6f55a991fe0aab1bfe25cdda7514c0511e42e.
---
 src/main/build/version.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/build/version.h b/src/main/build/version.h
index ec17abe33a..f509db3b54 100644
--- a/src/main/build/version.h
+++ b/src/main/build/version.h
@@ -16,7 +16,7 @@
  */
 
 #define FC_VERSION_MAJOR            2  // increment when a major release is made (big new feature, etc)
-#define FC_VERSION_MINOR            4  // increment when a minor release is made (small new feature, change etc)
+#define FC_VERSION_MINOR            3  // increment when a minor release is made (small new feature, change etc)
 #define FC_VERSION_PATCH_LEVEL      0  // increment when a bug is fixed
 
 #define STR_HELPER(x) #x

From 4547af289ac71c7b1c3a64e521179a89d2f9b29c Mon Sep 17 00:00:00 2001
From: Olivier C <olivierc600@gmail.com>
Date: Sun, 1 Dec 2019 20:47:13 +0100
Subject: [PATCH 003/179] Int var fix

---
 src/main/io/osd.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index e7550c6eec..15418e4e1d 100755
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -1770,7 +1770,7 @@ static bool osdDrawSingleElement(uint8_t item)
                 tfp_sprintf(buff, "W%u/%u", posControl.activeWaypointIndex, posControl.waypointCount);
                 displayWrite(osdGetDisplayPort(), 13, osdConfig()->hud_margin_v - 1, buff);
 
-                for (uint8_t i = osdConfig()->hud_wp_disp - 1; i >= 0 ; i--) { // Display in reverse order so the next WP is always written on top
+                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) {
                         wp2.lat = posControl.waypointList[j].lat;

From 6c7fd8ad3640e3b9bf24a3043fb39e5ec7da797b Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Thu, 6 Feb 2020 14:43:19 +0100
Subject: [PATCH 004/179] First run on Global Variables

---
 make/source.mk                     |  1 +
 src/main/common/global_variables.c | 49 ++++++++++++++++++++++++++++++
 src/main/common/global_variables.h | 30 ++++++++++++++++++
 src/main/common/logic_condition.c  | 45 +++++++++++++++++++++++++++
 src/main/common/logic_condition.h  |  8 +++++
 src/main/flight/servos.c           |  7 +++++
 src/main/flight/servos.h           |  4 +++
 7 files changed, 144 insertions(+)
 create mode 100644 src/main/common/global_variables.c
 create mode 100644 src/main/common/global_variables.h

diff --git a/make/source.mk b/make/source.mk
index 4fb7f2618a..e59c2afd30 100644
--- a/make/source.mk
+++ b/make/source.mk
@@ -16,6 +16,7 @@ COMMON_SRC = \
             common/log.c \
             common/logic_condition.c \
             common/global_functions.c \
+            common/global_variables.c \
             common/maths.c \
             common/memory.c \
             common/olc.c \
diff --git a/src/main/common/global_variables.c b/src/main/common/global_variables.c
new file mode 100644
index 0000000000..a07d7f09df
--- /dev/null
+++ b/src/main/common/global_variables.c
@@ -0,0 +1,49 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute 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.
+ *
+ * This file 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 this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#include "platform.h"
+
+#ifdef USE_LOGIC_CONDITIONS
+
+#include <stdint.h>
+#include "common/global_variables.h"
+#include "build/build_config.h"
+ 
+static EXTENDED_FASTRAM int globalVariableState[MAX_GLOBAL_VARIABLES];
+
+int gvGet(uint8_t index) {
+    if (index < MAX_GLOBAL_VARIABLES) {
+        return globalVariableState[index];
+    } else {
+        return 0;
+    }
+}
+
+void gvSet(uint8_t index, int value) {
+    if (index < MAX_GLOBAL_VARIABLES) {
+        globalVariableState[index] = value;
+    }
+}
+
+#endif
\ No newline at end of file
diff --git a/src/main/common/global_variables.h b/src/main/common/global_variables.h
new file mode 100644
index 0000000000..bebfa6904b
--- /dev/null
+++ b/src/main/common/global_variables.h
@@ -0,0 +1,30 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute 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.
+ *
+ * This file 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 this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#pragma once
+
+#define MAX_GLOBAL_VARIABLES 4
+
+int gvGet(uint8_t index);
+void gvSet(uint8_t index, int value);
\ No newline at end of file
diff --git a/src/main/common/logic_condition.c b/src/main/common/logic_condition.c
index 14d6cadcaa..062524740b 100644
--- a/src/main/common/logic_condition.c
+++ b/src/main/common/logic_condition.c
@@ -29,6 +29,7 @@
 #include "config/parameter_group_ids.h"
 
 #include "common/logic_condition.h"
+#include "common/global_variables.h"
 #include "common/utils.h"
 #include "rx/rx.h"
 #include "maths.h"
@@ -50,6 +51,7 @@ static int logicConditionCompute(
     int operandA,
     int operandB
 ) {
+    int temporaryValue;
     switch (operation) {
 
         case LOGIC_CONDITION_TRUE:
@@ -118,6 +120,43 @@ static int logicConditionCompute(
             return currentVaue;
             break;
 
+        case LOGIC_CONDITION_GVAR_SET:
+            gvSet(operandA, operandB);
+            return operandB;
+            break;
+        
+        case LOGIC_CONDITION_GVAR_INC:
+            temporaryValue = gvGet(operandA) + operandB;
+            gvSet(operandA, temporaryValue);
+            return temporaryValue;
+            break;
+
+        case LOGIC_CONDITION_GVAR_DEC:
+            temporaryValue = gvGet(operandA) - operandB;
+            gvSet(operandA, temporaryValue);
+            return temporaryValue;
+            break;
+
+        case LOGIC_CONDITION_ADD:
+            return operandA + operandB;
+            break;
+
+        case LOGIC_CONDITION_SUB:
+            return operandA - operandB;
+            break;
+
+        case LOGIC_CONDITION_MUL:
+            return operandA * operandB;
+            break;
+
+        case LOGIC_CONDITION_DIV:
+            if (operandB != 0) {
+                return operandA / operandB;
+            } else {
+                return operandA;
+            }
+            break;
+
         default:
             return false;
             break; 
@@ -314,6 +353,12 @@ int logicConditionGetOperandValue(logicOperandType_e type, int operand) {
             }
             break;
 
+        case LOGIC_CONDITION_OPERAND_TYPE_GVAR:
+            if (operand >= 0 && operand < MAX_GLOBAL_VARIABLES) {
+                retVal = gvGet(operand);
+            }
+            break;
+
         default:
             break;
     }
diff --git a/src/main/common/logic_condition.h b/src/main/common/logic_condition.h
index df6518a4c5..737d52a92a 100644
--- a/src/main/common/logic_condition.h
+++ b/src/main/common/logic_condition.h
@@ -43,6 +43,13 @@ typedef enum {
     LOGIC_CONDITION_NOR,            // 11
     LOGIC_CONDITION_NOT,            // 12
     LOGIC_CONDITION_STICKY,         // 13
+    LOGIC_CONDITION_ADD,            // 14
+    LOGIC_CONDITION_SUB,            // 15
+    LOGIC_CONDITION_MUL,            // 16
+    LOGIC_CONDITION_DIV,            // 17
+    LOGIC_CONDITION_GVAR_SET,       // 18
+    LOGIC_CONDITION_GVAR_INC,       // 19
+    LOGIC_CONDITION_GVAR_DEC,       // 21
     LOGIC_CONDITION_LAST
 } logicOperation_e;
 
@@ -52,6 +59,7 @@ typedef enum logicOperandType_s {
     LOGIC_CONDITION_OPERAND_TYPE_FLIGHT,
     LOGIC_CONDITION_OPERAND_TYPE_FLIGHT_MODE,
     LOGIC_CONDITION_OPERAND_TYPE_LC,    // Result of different LC and LC operand
+    LOGIC_CONDITION_OPERAND_TYPE_GVAR,  // Value from a global variable
     LOGIC_CONDITION_OPERAND_TYPE_LAST
 } logicOperandType_e;
 
diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c
index 3ddf770afe..aa78812c43 100755
--- a/src/main/flight/servos.c
+++ b/src/main/flight/servos.c
@@ -28,6 +28,7 @@
 #include "common/axis.h"
 #include "common/filter.h"
 #include "common/maths.h"
+#include "common/global_variables.h"
 
 #include "config/config_reset.h"
 #include "config/feature.h"
@@ -263,6 +264,12 @@ void servoMixer(float dT)
     input[INPUT_FEATURE_FLAPS] = FLIGHT_MODE(FLAPERON) ? servoConfig()->flaperon_throw_offset : 0;
 
     input[INPUT_LOGIC_ONE] = 500;
+#ifdef USE_LOGIC_CONDITIONS
+    input[INPUT_GVAR_0] = constrain(gvGet(0), -1000, 1000);
+    input[INPUT_GVAR_1] = constrain(gvGet(1), -1000, 1000);
+    input[INPUT_GVAR_2] = constrain(gvGet(2), -1000, 1000);
+    input[INPUT_GVAR_3] = constrain(gvGet(3), -1000, 1000);
+#endif
 
     if (IS_RC_MODE_ACTIVE(BOXCAMSTAB)) {
         input[INPUT_GIMBAL_PITCH] = scaleRange(attitude.values.pitch, -900, 900, -500, +500);
diff --git a/src/main/flight/servos.h b/src/main/flight/servos.h
index 3025bc4edc..8745cf239d 100644
--- a/src/main/flight/servos.h
+++ b/src/main/flight/servos.h
@@ -54,6 +54,10 @@ typedef enum {
     INPUT_STABILIZED_YAW_PLUS       = 27,
     INPUT_STABILIZED_YAW_MINUS      = 28,
     INPUT_LOGIC_ONE                 = 29,
+    INPUT_GVAR_0                    = 30,
+    INPUT_GVAR_1                    = 31,
+    INPUT_GVAR_2                    = 32,
+    INPUT_GVAR_3                    = 33,
 
     INPUT_SOURCE_COUNT
 } inputSource_e;

From 004082b67dc46cd21c8726656bc4e3b384d18166 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Mon, 17 Feb 2020 11:30:00 +0100
Subject: [PATCH 005/179] add msp frame to get GVAR values

---
 src/main/fc/fc_msp.c                | 6 ++++++
 src/main/msp/msp_protocol_v2_inav.h | 1 +
 2 files changed, 7 insertions(+)

diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index 8adea1b05b..596db0dc5c 100644
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -36,6 +36,7 @@
 #include "common/time.h"
 #include "common/utils.h"
 #include "common/global_functions.h"
+#include "common/global_variables.h"
 
 #include "config/parameter_group_ids.h"
 
@@ -542,6 +543,11 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
             sbufWriteU32(dst, logicConditionGetValue(i));
         }
         break;
+    case MSP2_INAV_GVAR_STATUS:
+        for (int i = 0; i < MAX_GLOBAL_VARIABLES; i++) {
+            sbufWriteU32(dst, gvGet(i));
+        }
+        break;
 #endif
 #ifdef USE_GLOBAL_FUNCTIONS
     case MSP2_INAV_GLOBAL_FUNCTIONS:
diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h
index fd6a7c5e27..0367bc5a3b 100755
--- a/src/main/msp/msp_protocol_v2_inav.h
+++ b/src/main/msp/msp_protocol_v2_inav.h
@@ -60,6 +60,7 @@
 #define MSP2_INAV_GLOBAL_FUNCTIONS              0x2024
 #define MSP2_INAV_SET_GLOBAL_FUNCTIONS          0x2025
 #define MSP2_INAV_LOGIC_CONDITIONS_STATUS       0x2026
+#define MSP2_INAV_GVAR_STATUS                   0x2027
 
 #define MSP2_PID                                0x2030
 #define MSP2_SET_PID                            0x2031

From 87d4713dabf1c286467977b8a2e6a1eca3cc5148 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Mon, 17 Feb 2020 14:42:33 +0100
Subject: [PATCH 006/179] Make GVAR min and max configurable

---
 src/main/common/global_variables.c    | 17 ++++++++++++++++-
 src/main/common/global_variables.h    |  8 ++++++++
 src/main/config/parameter_group_ids.h |  3 ++-
 3 files changed, 26 insertions(+), 2 deletions(-)

diff --git a/src/main/common/global_variables.c b/src/main/common/global_variables.c
index a07d7f09df..ea0327eaeb 100644
--- a/src/main/common/global_variables.c
+++ b/src/main/common/global_variables.c
@@ -27,11 +27,26 @@
 #ifdef USE_LOGIC_CONDITIONS
 
 #include <stdint.h>
+#include "config/config_reset.h"
+#include "config/parameter_group.h"
+#include "config/parameter_group_ids.h"
 #include "common/global_variables.h"
+#include "common/maths.h"
 #include "build/build_config.h"
  
 static EXTENDED_FASTRAM int globalVariableState[MAX_GLOBAL_VARIABLES];
 
+PG_REGISTER_ARRAY_WITH_RESET_FN(globalVariableConfig_t, MAX_GLOBAL_VARIABLES, globalVariableConfigs, PG_GLOBAL_VARIABLE_CONFIG, 0);
+
+void pgResetFn_globalVariableConfigs(globalVariableConfig_t *globalVariableConfigs)
+{
+    // set default calibration to full range and 1:1 mapping
+    for (int i = 0; i < MAX_GLOBAL_VARIABLES; i++) {
+        globalVariableConfigs[i].min = INT32_MIN;
+        globalVariableConfigs[i].max = INT32_MAX;
+    }
+}
+
 int gvGet(uint8_t index) {
     if (index < MAX_GLOBAL_VARIABLES) {
         return globalVariableState[index];
@@ -42,7 +57,7 @@ int gvGet(uint8_t index) {
 
 void gvSet(uint8_t index, int value) {
     if (index < MAX_GLOBAL_VARIABLES) {
-        globalVariableState[index] = value;
+        globalVariableState[index] = constrain(value, globalVariableConfigs(index)->min, globalVariableConfigs(index)->max);
     }
 }
 
diff --git a/src/main/common/global_variables.h b/src/main/common/global_variables.h
index bebfa6904b..0a63713b1d 100644
--- a/src/main/common/global_variables.h
+++ b/src/main/common/global_variables.h
@@ -24,7 +24,15 @@
 
 #pragma once
 
+#include "config/parameter_group.h"
+
 #define MAX_GLOBAL_VARIABLES 4
 
+typedef struct globalVariableConfig_s {
+    int min;
+    int max;
+} globalVariableConfig_t;
+PG_DECLARE_ARRAY(globalVariableConfig_t, MAX_GLOBAL_VARIABLES, globalVariableConfigs);
+
 int gvGet(uint8_t index);
 void gvSet(uint8_t index, int value);
\ No newline at end of file
diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h
index 8cb31ec3da..65fc415848 100644
--- a/src/main/config/parameter_group_ids.h
+++ b/src/main/config/parameter_group_ids.h
@@ -110,7 +110,8 @@
 #define PG_GLOBAL_FUNCTIONS 1020
 #define PG_ESC_SENSOR_CONFIG 1021
 #define PG_RPM_FILTER_CONFIG 1022
-#define PG_INAV_END 1022
+#define PG_GLOBAL_VARIABLE_CONFIG 1023
+#define PG_INAV_END 1023
 
 // OSD configuration (subject to change)
 //#define PG_OSD_FONT_CONFIG 2047

From 51420235095b7a480bfbccc20fac64d7ccd160f9 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Mon, 17 Feb 2020 14:57:53 +0100
Subject: [PATCH 007/179] CLI interface for setting min and max of global
 variables

---
 src/main/fc/cli.c | 79 +++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 79 insertions(+)

diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c
index f043800cb0..283390db20 100755
--- a/src/main/fc/cli.c
+++ b/src/main/fc/cli.c
@@ -44,6 +44,7 @@ extern uint8_t __config_end;
 #include "common/time.h"
 #include "common/typeconversion.h"
 #include "common/global_functions.h"
+#include "common/global_variables.h"
 
 #include "config/config_eeprom.h"
 #include "config/feature.h"
@@ -1829,6 +1830,77 @@ static void cliLogic(char *cmdline) {
         }
     }
 }
+
+static void printGvar(uint8_t dumpMask, const globalVariableConfig_t *gvars, const globalVariableConfig_t *defaultGvars)
+{
+    const char *format = "gvar %d %d %d";
+    for (uint32_t i = 0; i < MAX_GLOBAL_VARIABLES; i++) {
+        const globalVariableConfig_t gvar = gvars[i];
+
+        bool equalsDefault = false;
+        if (defaultGvars) {
+            globalVariableConfig_t defaultValue = defaultGvars[i];
+            equalsDefault =
+                gvar.min == defaultValue.min &&
+                gvar.max == defaultValue.max;
+
+            cliDefaultPrintLinef(dumpMask, equalsDefault, format,
+                i,
+                gvar.min,
+                gvar.max
+            );
+        }
+        cliDumpPrintLinef(dumpMask, equalsDefault, format,
+            i,
+            gvar.min,
+            gvar.max
+        );
+    }
+}
+
+static void cliGvar(char *cmdline) {
+    char * saveptr;
+    int args[3], check = 0;
+    uint8_t len = strlen(cmdline);
+
+    if (len == 0) {
+        printGvar(DUMP_MASTER, globalVariableConfigs(0), NULL);
+    } else if (sl_strncasecmp(cmdline, "reset", 5) == 0) {
+        pgResetCopy(globalVariableConfigsMutable(0), PG_GLOBAL_VARIABLE_CONFIG);
+    } else {
+        enum {
+            INDEX = 0,
+            MIN,
+            MAX,
+            ARGS_COUNT
+            };
+        char *ptr = strtok_r(cmdline, " ", &saveptr);
+        while (ptr != NULL && check < ARGS_COUNT) {
+            args[check++] = fastA2I(ptr);
+            ptr = strtok_r(NULL, " ", &saveptr);
+        }
+
+        if (ptr != NULL || check != ARGS_COUNT) {
+            cliShowParseError();
+            return;
+        }
+
+        int32_t i = args[INDEX];
+        if (
+            i >= 0 && i < MAX_GLOBAL_VARIABLES &&
+            args[MIN] >= INT32_MIN && args[MIN] <= INT32_MAX &&  
+            args[MAX] >= INT32_MIN && args[MAX] <= INT32_MAX  
+        ) {
+            globalVariableConfigsMutable(i)->min = args[MIN];
+            globalVariableConfigsMutable(i)->max = args[MAX];
+
+            cliGvar("");
+        } else {
+            cliShowParseError();
+        }
+    }
+}
+
 #endif
 
 #ifdef USE_GLOBAL_FUNCTIONS
@@ -3162,6 +3234,9 @@ static void printConfig(const char *cmdline, bool doDiff)
 #ifdef USE_LOGIC_CONDITIONS
         cliPrintHashLine("logic");
         printLogic(dumpMask, logicConditions_CopyArray, logicConditions(0));
+
+        cliPrintHashLine("gvar");
+        printGvar(dumpMask, globalVariableConfigs_CopyArray, globalVariableConfigs(0));
 #endif
 
 #ifdef USE_GLOBAL_FUNCTIONS
@@ -3419,6 +3494,10 @@ const clicmd_t cmdTable[] = {
     CLI_COMMAND_DEF("logic", "configure logic conditions",
         "<rule> <enabled> <operation> <operand A type> <operand A value> <operand B type> <operand B value> <flags>\r\n"
         "\treset\r\n", cliLogic),
+
+    CLI_COMMAND_DEF("gvar", "configure global variables",
+        "<gvar> <min> <max>\r\n"
+        "\treset\r\n", cliGvar),
 #endif
 #ifdef USE_GLOBAL_FUNCTIONS
     CLI_COMMAND_DEF("gf", "configure global functions",

From cecdb9344734da034dc51ca231043a94cde58027 Mon Sep 17 00:00:00 2001
From: Fabian Schwartau <fabian@opencode.eu>
Date: Fri, 21 Feb 2020 16:45:22 +0100
Subject: [PATCH 008/179] Navigation RTH now respects nav_rth_tail_first

---
 src/main/navigation/navigation.c | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index 274d5bc220..27fe957090 100755
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -1412,7 +1412,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na
                     return NAV_FSM_EVENT_SUCCESS;   // will switch to NAV_STATE_WAYPOINT_REACHED
                 }
                 else {
-                    setDesiredPosition(&posControl.activeWaypoint.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_BEARING);
+                    if(navConfig()->general.flags.rth_tail_first && !STATE(FIXED_WING_LEGACY))
+                        setDesiredPosition(&posControl.activeWaypoint.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_BEARING_TAIL_FIRST);
+                    else
+                        setDesiredPosition(&posControl.activeWaypoint.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_BEARING);
                     setDesiredPosition(rthGetHomeTargetPosition(RTH_HOME_ENROUTE_PROPORTIONAL), 0, NAV_POS_UPDATE_Z);
                     return NAV_FSM_EVENT_NONE;      // will re-process state in >10ms
                 }

From 9b146470a9ee1659e7cb4fbc90dc26dc82576bac Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Sat, 22 Feb 2020 21:54:24 +0100
Subject: [PATCH 009/179] Simplify throttle range logic

---
 src/main/flight/mixer.c | 32 +++++++++++++++-----------------
 1 file changed, 15 insertions(+), 17 deletions(-)

diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c
index bb9b515db1..91b96e9ffb 100755
--- a/src/main/flight/mixer.c
+++ b/src/main/flight/mixer.c
@@ -363,13 +363,14 @@ void FAST_CODE NOINLINE mixTable(const float dT)
     int16_t rpyMixRange = rpyMixMax - rpyMixMin;
     int16_t throttleRange;
     int16_t throttleMin, throttleMax;
+    int16_t throttleRangeMin, throttleRangeMax;
     static int16_t throttlePrevious = 0;   // Store the last throttle direction for deadband transitions
 
     // Find min and max throttle based on condition.
 #ifdef USE_GLOBAL_FUNCTIONS
     if (GLOBAL_FUNCTION_FLAG(GLOBAL_FUNCTION_FLAG_OVERRIDE_THROTTLE)) {
-        throttleMin = throttleIdleValue;
-        throttleMax = motorConfig()->maxthrottle;
+        throttleRangeMin = throttleIdleValue;
+        throttleRangeMax = motorConfig()->maxthrottle;
         mixerThrottleCommand = constrain(globalFunctionValues[GLOBAL_FUNCTION_ACTION_OVERRIDE_THROTTLE], throttleMin, throttleMax); 
     } else
 #endif
@@ -377,24 +378,24 @@ void FAST_CODE NOINLINE mixTable(const float dT)
         if (!ARMING_FLAG(ARMED)) throttlePrevious = PWM_RANGE_MIDDLE; // When disarmed set to mid_rc. It always results in positive direction after arming.
 
         if ((rcCommand[THROTTLE] <= (PWM_RANGE_MIDDLE - rcControlsConfig()->mid_throttle_deadband))) { // Out of band handling
-            throttleMax = reversibleMotorsConfig()->deadband_low;
-            throttleMin = throttleIdleValue;
+            throttleRangeMax = reversibleMotorsConfig()->deadband_low;
+            throttleRangeMin = throttleIdleValue;
             throttlePrevious = mixerThrottleCommand = rcCommand[THROTTLE];
         } else if (rcCommand[THROTTLE] >= (PWM_RANGE_MIDDLE + rcControlsConfig()->mid_throttle_deadband)) { // Positive handling
-            throttleMax = motorConfig()->maxthrottle;
-            throttleMin = reversibleMotorsConfig()->deadband_high;
+            throttleRangeMax = motorConfig()->maxthrottle;
+            throttleRangeMin = reversibleMotorsConfig()->deadband_high;
             throttlePrevious = mixerThrottleCommand = rcCommand[THROTTLE];
         } else if ((throttlePrevious <= (PWM_RANGE_MIDDLE - rcControlsConfig()->mid_throttle_deadband)))  { // Deadband handling from negative to positive
             mixerThrottleCommand = throttleMax = reversibleMotorsConfig()->deadband_low;
-            throttleMin = throttleIdleValue;
+            throttleRangeMin = throttleIdleValue;
         } else {  // Deadband handling from positive to negative
-            throttleMax = motorConfig()->maxthrottle;
+            throttleRangeMax = motorConfig()->maxthrottle;
             mixerThrottleCommand = throttleMin = reversibleMotorsConfig()->deadband_high;
         }
     } else {
         mixerThrottleCommand = rcCommand[THROTTLE];
-        throttleMin = throttleIdleValue;
-        throttleMax = motorConfig()->maxthrottle;
+        throttleRangeMin = throttleIdleValue;
+        throttleRangeMax = motorConfig()->maxthrottle;
 
         // Throttle scaling to limit max throttle when battery is full
     #ifdef USE_GLOBAL_FUNCTIONS
@@ -408,6 +409,9 @@ void FAST_CODE NOINLINE mixTable(const float dT)
         }
     }
 
+    throttleMin = throttleRangeMin;
+    throttleMax = throttleRangeMax;
+
     throttleRange = throttleMax - throttleMin;
 
     #define THROTTLE_CLIPPING_FACTOR    0.33f
@@ -434,14 +438,8 @@ void FAST_CODE NOINLINE mixTable(const float dT)
 
             if (failsafeIsActive()) {
                 motor[i] = constrain(motor[i], motorConfig()->mincommand, motorConfig()->maxthrottle);
-            } else if (feature(FEATURE_REVERSIBLE_MOTORS)) {
-                if (throttlePrevious <= (PWM_RANGE_MIDDLE - rcControlsConfig()->mid_throttle_deadband)) {
-                    motor[i] = constrain(motor[i], throttleIdleValue, reversibleMotorsConfig()->deadband_low);
-                } else {
-                    motor[i] = constrain(motor[i], reversibleMotorsConfig()->deadband_high, motorConfig()->maxthrottle);
-                }
             } else {
-                motor[i] = constrain(motor[i], throttleIdleValue, motorConfig()->maxthrottle);
+                motor[i] = constrain(motor[i], throttleRangeMin,throttleRangeMax);
             }
 
             // Motor stop handling

From 0ffcac29749d90991e803b1f3078a8d2622b992f Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Sun, 23 Feb 2020 19:31:16 +0100
Subject: [PATCH 010/179] Correctly handle reversible motors in mixer

---
 src/main/fc/fc_core.c                        |  4 +-
 src/main/fc/rc_controls.c                    | 13 ++-
 src/main/fc/rc_controls.h                    |  7 +-
 src/main/flight/failsafe.c                   |  2 +-
 src/main/flight/mixer.c                      | 94 +++++++++++++++-----
 src/main/flight/mixer.h                      |  6 ++
 src/main/navigation/navigation.c             |  4 +-
 src/main/navigation/navigation_fw_launch.c   |  2 +-
 src/main/navigation/navigation_multicopter.c |  2 +-
 src/main/sensors/battery.c                   |  2 +-
 src/main/telemetry/frsky_d.c                 |  2 +-
 11 files changed, 105 insertions(+), 33 deletions(-)

diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c
index 11ec133532..951857716e 100755
--- a/src/main/fc/fc_core.c
+++ b/src/main/fc/fc_core.c
@@ -200,7 +200,7 @@ static void updateArmingStatus(void)
         /* CHECK: Throttle */
         if (!armingConfig()->fixed_wing_auto_arm) {
             // Don't want this check if fixed_wing_auto_arm is in use - machine arms on throttle > LOW
-            if (calculateThrottleStatus() != THROTTLE_LOW) {
+            if (calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC) != THROTTLE_LOW) {
                 ENABLE_ARMING_FLAG(ARMING_DISABLED_THROTTLE);
             } else {
                 DISABLE_ARMING_FLAG(ARMING_DISABLED_THROTTLE);
@@ -529,7 +529,7 @@ void processRx(timeUs_t currentTimeUs)
 
     failsafeUpdateState();
 
-    const throttleStatus_e throttleStatus = calculateThrottleStatus();
+    const throttleStatus_e throttleStatus = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC);
 
     // When armed and motors aren't spinning, do beeps periodically
     if (ARMING_FLAG(ARMED) && feature(FEATURE_MOTOR_STOP) && !STATE(FIXED_WING_LEGACY)) {
diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c
index b741e75b87..82711d4859 100644
--- a/src/main/fc/rc_controls.c
+++ b/src/main/fc/rc_controls.c
@@ -99,12 +99,19 @@ bool areSticksDeflectedMoreThanPosHoldDeadband(void)
     return (ABS(rcCommand[ROLL]) > rcControlsConfig()->pos_hold_deadband) || (ABS(rcCommand[PITCH]) > rcControlsConfig()->pos_hold_deadband);
 }
 
-throttleStatus_e calculateThrottleStatus(void)
+throttleStatus_e calculateThrottleStatus(throttleStatusType_e type)
 {
+    int value;
+    if (type == THROTTLE_STATUS_TYPE_RC) {
+        value = rxGetChannelValue(THROTTLE);
+    } else {
+        value = rcCommand[THROTTLE];
+    }
+
     const uint16_t mid_throttle_deadband = rcControlsConfig()->mid_throttle_deadband;
-    if (feature(FEATURE_REVERSIBLE_MOTORS) && (rxGetChannelValue(THROTTLE) > (PWM_RANGE_MIDDLE - mid_throttle_deadband) && rxGetChannelValue(THROTTLE) < (PWM_RANGE_MIDDLE + mid_throttle_deadband)))
+    if (feature(FEATURE_REVERSIBLE_MOTORS) && (value > (PWM_RANGE_MIDDLE - mid_throttle_deadband) && value < (PWM_RANGE_MIDDLE + mid_throttle_deadband)))
         return THROTTLE_LOW;
-    else if (!feature(FEATURE_REVERSIBLE_MOTORS) && (rxGetChannelValue(THROTTLE) < rxConfig()->mincheck))
+    else if (!feature(FEATURE_REVERSIBLE_MOTORS) && (value < rxConfig()->mincheck))
         return THROTTLE_LOW;
 
     return THROTTLE_HIGH;
diff --git a/src/main/fc/rc_controls.h b/src/main/fc/rc_controls.h
index 23c8887cfa..b985c60394 100644
--- a/src/main/fc/rc_controls.h
+++ b/src/main/fc/rc_controls.h
@@ -45,6 +45,11 @@ typedef enum {
     THROTTLE_HIGH
 } throttleStatus_e;
 
+typedef enum {
+    THROTTLE_STATUS_TYPE_RC = 0,
+    THROTTLE_STATUS_TYPE_COMMAND
+} throttleStatusType_e;
+
 typedef enum {
     NOT_CENTERED = 0,
     CENTERED
@@ -100,7 +105,7 @@ bool checkStickPosition(stickPositions_e stickPos);
 
 bool areSticksInApModePosition(uint16_t ap_mode);
 bool areSticksDeflectedMoreThanPosHoldDeadband(void);
-throttleStatus_e calculateThrottleStatus(void);
+throttleStatus_e calculateThrottleStatus(throttleStatusType_e type);
 rollPitchStatus_e calculateRollPitchCenterStatus(void);
 void processRcStickPositions(throttleStatus_e throttleStatus);
 
diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c
index c0f951dcc3..6ac2783c18 100644
--- a/src/main/flight/failsafe.c
+++ b/src/main/flight/failsafe.c
@@ -397,7 +397,7 @@ void failsafeUpdateState(void)
             case FAILSAFE_IDLE:
                 if (armed) {
                     // Track throttle command below minimum time
-                    if (THROTTLE_HIGH == calculateThrottleStatus()) {
+                    if (THROTTLE_HIGH == calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC)) {
                         failsafeState.throttleLowPeriod = millis() + failsafeConfig()->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND;
                     }
                     if (!receivingRxDataAndNotFailsafeMode) {
diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c
index 91b96e9ffb..45706fa972 100755
--- a/src/main/flight/mixer.c
+++ b/src/main/flight/mixer.c
@@ -63,6 +63,11 @@ static EXTENDED_FASTRAM uint8_t motorCount = 0;
 EXTENDED_FASTRAM int mixerThrottleCommand;
 static EXTENDED_FASTRAM int throttleIdleValue = 0;
 static EXTENDED_FASTRAM int motorValueWhenStopped = 0;
+static reversibleMotorsThrottleState_e reversibleMotorsThrottleState = MOTOR_DIRECTION_FORWARD;
+static EXTENDED_FASTRAM int throttleDeadbandLow = 0;
+static EXTENDED_FASTRAM int throttleDeadbandHigh = 0;
+static EXTENDED_FASTRAM int throttleRangeMin = 0;
+static EXTENDED_FASTRAM int throttleRangeMax = 0;
 
 PG_REGISTER_WITH_RESET_TEMPLATE(reversibleMotorsConfig_t, reversibleMotorsConfig, PG_REVERSIBLE_MOTORS_CONFIG, 0);
 
@@ -238,6 +243,9 @@ void mixerInit(void)
         mixerScale = 0.5f;
     }
 
+    throttleDeadbandLow = PWM_RANGE_MIDDLE - rcControlsConfig()->mid_throttle_deadband;
+    throttleDeadbandHigh = PWM_RANGE_MIDDLE + rcControlsConfig()->mid_throttle_deadband;
+
     mixerResetDisarmedMotors();
 
     if (motorConfig()->motorAccelTimeMs || motorConfig()->motorDecelTimeMs) {
@@ -249,7 +257,19 @@ void mixerInit(void)
 
 void mixerResetDisarmedMotors(void)
 {
-    const int motorZeroCommand = feature(FEATURE_REVERSIBLE_MOTORS) ? reversibleMotorsConfig()->neutral : motorConfig()->mincommand;
+    int motorZeroCommand;
+    
+    if (feature(FEATURE_REVERSIBLE_MOTORS)) {
+        motorZeroCommand = reversibleMotorsConfig()->neutral;
+        throttleRangeMin = throttleDeadbandHigh;
+        throttleRangeMax = motorConfig()->maxthrottle;
+    } else {
+        motorZeroCommand = motorConfig()->mincommand;
+        throttleRangeMin = getThrottleIdleValue();
+        throttleRangeMax = motorConfig()->maxthrottle;
+    }
+
+    reversibleMotorsThrottleState = MOTOR_DIRECTION_FORWARD;
 
     if (feature(FEATURE_MOTOR_STOP)) {
         motorValueWhenStopped = motorZeroCommand;
@@ -296,7 +316,26 @@ void FAST_CODE NOINLINE writeMotors(void)
             }
         }
         else {
-            motorValue = motor[i];
+            if (feature(FEATURE_REVERSIBLE_MOTORS)) {
+                if (reversibleMotorsThrottleState == MOTOR_DIRECTION_FORWARD) {
+                    if (motor[i] < throttleRangeMin) {
+                        motorValue = motor[i];
+                    } else {
+                        motorValue = scaleRangef(motor[i], throttleRangeMin, throttleRangeMax, reversibleMotorsConfig()->deadband_high,  motorConfig()->maxthrottle);
+                        motorValue = constrain(motorValue, reversibleMotorsConfig()->deadband_high,  motorConfig()->maxthrottle);
+                    }
+                } else {
+                     if (motor[i] > throttleRangeMax) {
+                        motorValue = motor[i];
+                    } else {
+                        motorValue = scaleRangef(motor[i], throttleRangeMin, throttleRangeMax,  motorConfig()->mincommand, reversibleMotorsConfig()->deadband_low);
+                        motorValue = constrain(motorValue, motorConfig()->mincommand,  reversibleMotorsConfig()->deadband_low);
+                    }
+                }
+            } else {
+                motorValue = motor[i];
+            }
+
         }
 #else
         // We don't define USE_DSHOT
@@ -328,6 +367,19 @@ void stopPwmAllMotors(void)
     pwmShutdownPulsesForAllMotors(motorCount);
 }
 
+static int getReversibleMotorsThrottleDeadband(void)
+{
+    int directionValue;
+
+    if (reversibleMotorsThrottleState == MOTOR_DIRECTION_BACKWARD) {
+        directionValue = reversibleMotorsConfig()->deadband_low;
+    } else {
+        directionValue = reversibleMotorsConfig()->deadband_high;
+    }
+
+    return feature(FEATURE_MOTOR_STOP) ? reversibleMotorsConfig()->neutral : directionValue;
+}
+
 void FAST_CODE NOINLINE mixTable(const float dT)
 {
     int16_t input[3];   // RPY, range [-500:+500]
@@ -363,8 +415,7 @@ void FAST_CODE NOINLINE mixTable(const float dT)
     int16_t rpyMixRange = rpyMixMax - rpyMixMin;
     int16_t throttleRange;
     int16_t throttleMin, throttleMax;
-    int16_t throttleRangeMin, throttleRangeMax;
-    static int16_t throttlePrevious = 0;   // Store the last throttle direction for deadband transitions
+    // static int16_t throttlePrevious = 0;   // Store the last throttle direction for deadband transitions
 
     // Find min and max throttle based on condition.
 #ifdef USE_GLOBAL_FUNCTIONS
@@ -375,23 +426,26 @@ void FAST_CODE NOINLINE mixTable(const float dT)
     } else
 #endif
     if (feature(FEATURE_REVERSIBLE_MOTORS)) {
-        if (!ARMING_FLAG(ARMED)) throttlePrevious = PWM_RANGE_MIDDLE; // When disarmed set to mid_rc. It always results in positive direction after arming.
 
-        if ((rcCommand[THROTTLE] <= (PWM_RANGE_MIDDLE - rcControlsConfig()->mid_throttle_deadband))) { // Out of band handling
-            throttleRangeMax = reversibleMotorsConfig()->deadband_low;
-            throttleRangeMin = throttleIdleValue;
-            throttlePrevious = mixerThrottleCommand = rcCommand[THROTTLE];
-        } else if (rcCommand[THROTTLE] >= (PWM_RANGE_MIDDLE + rcControlsConfig()->mid_throttle_deadband)) { // Positive handling
+        if (rcCommand[THROTTLE] >= (throttleDeadbandHigh)) {
+            /*
+             * Throttle is above deadband, FORWARD direction
+             */
+            reversibleMotorsThrottleState = MOTOR_DIRECTION_FORWARD;
             throttleRangeMax = motorConfig()->maxthrottle;
-            throttleRangeMin = reversibleMotorsConfig()->deadband_high;
-            throttlePrevious = mixerThrottleCommand = rcCommand[THROTTLE];
-        } else if ((throttlePrevious <= (PWM_RANGE_MIDDLE - rcControlsConfig()->mid_throttle_deadband)))  { // Deadband handling from negative to positive
-            mixerThrottleCommand = throttleMax = reversibleMotorsConfig()->deadband_low;
-            throttleRangeMin = throttleIdleValue;
-        } else {  // Deadband handling from positive to negative
-            throttleRangeMax = motorConfig()->maxthrottle;
-            mixerThrottleCommand = throttleMin = reversibleMotorsConfig()->deadband_high;
+            throttleRangeMin = throttleDeadbandHigh;
+        } else if (rcCommand[THROTTLE] <= throttleDeadbandLow) {
+            /*
+             * Throttle is below deadband, BACKWARD direction
+             */
+            reversibleMotorsThrottleState = MOTOR_DIRECTION_BACKWARD;
+            throttleRangeMax = throttleDeadbandLow;
+            throttleRangeMin = motorConfig()->mincommand;
         }
+
+
+        motorValueWhenStopped = getReversibleMotorsThrottleDeadband();
+        mixerThrottleCommand = constrain(rcCommand[THROTTLE], throttleRangeMin, throttleRangeMax);
     } else {
         mixerThrottleCommand = rcCommand[THROTTLE];
         throttleRangeMin = throttleIdleValue;
@@ -439,7 +493,7 @@ void FAST_CODE NOINLINE mixTable(const float dT)
             if (failsafeIsActive()) {
                 motor[i] = constrain(motor[i], motorConfig()->mincommand, motorConfig()->maxthrottle);
             } else {
-                motor[i] = constrain(motor[i], throttleRangeMin,throttleRangeMax);
+                motor[i] = constrain(motor[i], throttleRangeMin, throttleRangeMax);
             }
 
             // Motor stop handling
@@ -463,7 +517,7 @@ motorStatus_e getMotorStatus(void)
         return MOTOR_STOPPED_AUTO;
     }
 
-    if (calculateThrottleStatus() == THROTTLE_LOW) {
+    if (calculateThrottleStatus(feature(FEATURE_REVERSIBLE_MOTORS) ? THROTTLE_STATUS_TYPE_COMMAND : THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW) {
         if ((STATE(FIXED_WING_LEGACY) || !STATE(AIRMODE_ACTIVE)) && (!(navigationIsFlyingAutonomousMode() && navConfig()->general.flags.auto_overrides_motor_stop)) && (!failsafeIsActive())) {
             return MOTOR_STOPPED_USER;
         }
diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h
index fcde6008ff..cfb01b4086 100644
--- a/src/main/flight/mixer.h
+++ b/src/main/flight/mixer.h
@@ -102,6 +102,12 @@ typedef enum {
     MOTOR_RUNNING
 } motorStatus_e;
 
+typedef enum {
+    MOTOR_DIRECTION_FORWARD,
+    MOTOR_DIRECTION_BACKWARD,
+    MOTOR_DIRECTION_DEADBAND
+} reversibleMotorsThrottleState_e;
+
 extern int16_t motor[MAX_SUPPORTED_MOTORS];
 extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
 extern int mixerThrottleCommand;
diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index 84f89b238c..2a4f2a8ae2 100755
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -1552,7 +1552,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_WAIT(navigationF
 
     //allow to leave NAV_LAUNCH_MODE if it has being enabled as feature by moving sticks with low throttle.
     if (feature(FEATURE_FW_LAUNCH)) {
-        throttleStatus_e throttleStatus = calculateThrottleStatus();
+        throttleStatus_e throttleStatus = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC);
         if ((throttleStatus == THROTTLE_LOW) && (areSticksDeflectedMoreThanPosHoldDeadband())) {
             abortFixedWingLaunch();
             return NAV_FSM_EVENT_SWITCH_TO_IDLE;
@@ -2909,7 +2909,7 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
             else {
                 // If we were in LAUNCH mode - force switch to IDLE only if the throttle is low
                 if (FLIGHT_MODE(NAV_LAUNCH_MODE)) {
-                    throttleStatus_e throttleStatus = calculateThrottleStatus();
+                    throttleStatus_e throttleStatus = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC);
                     if (throttleStatus != THROTTLE_LOW)
                         return NAV_FSM_EVENT_NONE;
                     else
diff --git a/src/main/navigation/navigation_fw_launch.c b/src/main/navigation/navigation_fw_launch.c
index ebdd30b5ce..6426b41afb 100755
--- a/src/main/navigation/navigation_fw_launch.c
+++ b/src/main/navigation/navigation_fw_launch.c
@@ -146,7 +146,7 @@ static void applyFixedWingLaunchIdleLogic(void)
     else
     {
         static float timeThrottleRaisedMs;
-        if (calculateThrottleStatus() == THROTTLE_LOW)
+        if (calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW)
         {
             timeThrottleRaisedMs = millis();
         }
diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c
index 596b47f646..dff676931d 100755
--- a/src/main/navigation/navigation_multicopter.c
+++ b/src/main/navigation/navigation_multicopter.c
@@ -164,7 +164,7 @@ bool adjustMulticopterAltitudeFromRCInput(void)
 
 void setupMulticopterAltitudeController(void)
 {
-    const throttleStatus_e throttleStatus = calculateThrottleStatus();
+    const throttleStatus_e throttleStatus = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC);
 
     if (navConfig()->general.flags.use_thr_mid_for_althold) {
         altHoldThrottleRCZero = rcLookupThrottleMid();
diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c
index 872c7aab49..01d2582ded 100644
--- a/src/main/sensors/battery.c
+++ b/src/main/sensors/battery.c
@@ -466,7 +466,7 @@ void currentMeterUpdate(timeUs_t timeDelta)
         case CURRENT_SENSOR_VIRTUAL:
             amperage = batteryMetersConfig()->current.offset;
             if (ARMING_FLAG(ARMED)) {
-                throttleStatus_e throttleStatus = calculateThrottleStatus();
+                throttleStatus_e throttleStatus = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC);
                 int32_t throttleOffset = ((throttleStatus == THROTTLE_LOW) && feature(FEATURE_MOTOR_STOP)) ? 0 : (int32_t)rcCommand[THROTTLE] - 1000;
                 int32_t throttleFactor = throttleOffset + (throttleOffset * throttleOffset / 50);
                 amperage += throttleFactor * batteryMetersConfig()->current.scale / 1000;
diff --git a/src/main/telemetry/frsky_d.c b/src/main/telemetry/frsky_d.c
index 457b760ba2..b551f6b9d1 100644
--- a/src/main/telemetry/frsky_d.c
+++ b/src/main/telemetry/frsky_d.c
@@ -196,7 +196,7 @@ static void sendThrottleOrBatterySizeAsRpm(void)
     uint16_t throttleForRPM = rcCommand[THROTTLE] / BLADE_NUMBER_DIVIDER;
     sendDataHead(ID_RPM);
     if (ARMING_FLAG(ARMED)) {
-        const throttleStatus_e throttleStatus = calculateThrottleStatus();
+        const throttleStatus_e throttleStatus = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC);
         if (throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP))
                     throttleForRPM = 0;
         serialize16(throttleForRPM);

From af5b36b0fe14a04ef7c7b5266304d17d165bae4c Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Sun, 23 Feb 2020 20:32:22 +0100
Subject: [PATCH 011/179] Unify protocol scaling handling

---
 src/main/fc/rc_controls.c |   2 +-
 src/main/flight/mixer.c   | 109 ++++++++++++++++++++++++++++----------
 2 files changed, 81 insertions(+), 30 deletions(-)

diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c
index 82711d4859..a160909565 100644
--- a/src/main/fc/rc_controls.c
+++ b/src/main/fc/rc_controls.c
@@ -99,7 +99,7 @@ bool areSticksDeflectedMoreThanPosHoldDeadband(void)
     return (ABS(rcCommand[ROLL]) > rcControlsConfig()->pos_hold_deadband) || (ABS(rcCommand[PITCH]) > rcControlsConfig()->pos_hold_deadband);
 }
 
-throttleStatus_e calculateThrottleStatus(throttleStatusType_e type)
+throttleStatus_e FAST_CODE NOINLINE calculateThrottleStatus(throttleStatusType_e type)
 {
     int value;
     if (type == THROTTLE_STATUS_TYPE_RC) {
diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c
index 45706fa972..a8b3579e67 100755
--- a/src/main/flight/mixer.c
+++ b/src/main/flight/mixer.c
@@ -283,6 +283,34 @@ void mixerResetDisarmedMotors(void)
     }
 }
 
+static FAST_CODE NOINLINE uint16_t handleOutputScaling(
+    int16_t input,          // Input value from the mixer
+    int16_t stopThreshold,  // Threshold value to check if motor should be rotating or not
+    int16_t onStopValue,    // Value sent to the ESC when min rotation is required - on motor_stop it is STOP command, without motor_stop it's a value that keeps rotation
+    int16_t inputScaleMin,  // Input range - min value
+    int16_t inputScaleMax,  // Input range - max value
+    int16_t outputScaleMin, // Output range - min value
+    int16_t outputScaleMax, // Output range - max value
+    bool moveForward        // If motor should be rotating FORWARD or BACKWARD
+)
+{
+    int value;
+    if (moveForward && input < stopThreshold) {
+        //Send motor stop command
+        value = onStopValue;
+    }
+    else if (!moveForward && input > stopThreshold) {
+        //Send motor stop command
+        value = onStopValue;
+    }
+    else {
+        //Scale input to protocol output values
+        value = scaleRangef(input, inputScaleMin, inputScaleMax, outputScaleMin, outputScaleMax);
+        value = constrain(value, outputScaleMin, outputScaleMax);
+    }
+    return value;
+}
+
 void FAST_CODE NOINLINE writeMotors(void)
 {
     for (int i = 0; i < motorCount; i++) {
@@ -293,44 +321,67 @@ void FAST_CODE NOINLINE writeMotors(void)
         if (isMotorProtocolDigital()) {
 
             if (feature(FEATURE_REVERSIBLE_MOTORS)) {
-                if (motor[i] >= throttleIdleValue && motor[i] <= reversibleMotorsConfig()->deadband_low) {
-                    motorValue = scaleRangef(motor[i], motorConfig()->mincommand, reversibleMotorsConfig()->deadband_low, DSHOT_3D_DEADBAND_LOW, DSHOT_MIN_THROTTLE);
-                    motorValue = constrain(motorValue, DSHOT_MIN_THROTTLE, DSHOT_3D_DEADBAND_LOW);
-                }
-                else if (motor[i] >= reversibleMotorsConfig()->deadband_high && motor[i] <= motorConfig()->maxthrottle) {
-                    motorValue = scaleRangef(motor[i], reversibleMotorsConfig()->deadband_high, motorConfig()->maxthrottle, DSHOT_3D_DEADBAND_HIGH, DSHOT_MAX_THROTTLE);
-                    motorValue = constrain(motorValue, DSHOT_3D_DEADBAND_HIGH, DSHOT_MAX_THROTTLE);
-                }
-                else {
-                    motorValue = DSHOT_DISARM_COMMAND;
+                if (reversibleMotorsThrottleState == MOTOR_DIRECTION_FORWARD) {
+                    motorValue = handleOutputScaling(
+                        motor[i],
+                        throttleRangeMin,
+                        DSHOT_DISARM_COMMAND,
+                        throttleRangeMin,
+                        throttleRangeMax,
+                        DSHOT_3D_DEADBAND_HIGH,
+                        DSHOT_MAX_THROTTLE,
+                        true
+                    );
+                } else {
+                    motorValue = handleOutputScaling(
+                        motor[i],
+                        throttleRangeMax,
+                        DSHOT_DISARM_COMMAND,
+                        throttleRangeMin,
+                        throttleRangeMax,
+                        DSHOT_MIN_THROTTLE,
+                        DSHOT_3D_DEADBAND_LOW,
+                        false
+                    );
                 }
             }
             else {
-                if (motor[i] < throttleIdleValue) {    // motor disarmed
-                    motorValue = DSHOT_DISARM_COMMAND;
-                }
-                else {
-                    motorValue = scaleRangef(motor[i], motorConfig()->mincommand, motorConfig()->maxthrottle, DSHOT_MIN_THROTTLE, DSHOT_MAX_THROTTLE);
-                    motorValue = constrain(motorValue, DSHOT_MIN_THROTTLE, DSHOT_MAX_THROTTLE);
-                }
+                motorValue = handleOutputScaling(
+                    motor[i],
+                    throttleIdleValue,
+                    DSHOT_DISARM_COMMAND,
+                    motorConfig()->mincommand,
+                    motorConfig()->maxthrottle,
+                    DSHOT_MIN_THROTTLE,
+                    DSHOT_MAX_THROTTLE,
+                    true
+                );
             }
         }
         else {
             if (feature(FEATURE_REVERSIBLE_MOTORS)) {
                 if (reversibleMotorsThrottleState == MOTOR_DIRECTION_FORWARD) {
-                    if (motor[i] < throttleRangeMin) {
-                        motorValue = motor[i];
-                    } else {
-                        motorValue = scaleRangef(motor[i], throttleRangeMin, throttleRangeMax, reversibleMotorsConfig()->deadband_high,  motorConfig()->maxthrottle);
-                        motorValue = constrain(motorValue, reversibleMotorsConfig()->deadband_high,  motorConfig()->maxthrottle);
-                    }
+                    motorValue = handleOutputScaling(
+                        motor[i],
+                        throttleRangeMin,
+                        motor[i],
+                        throttleRangeMin,
+                        throttleRangeMax,
+                        reversibleMotorsConfig()->deadband_high,
+                        motorConfig()->maxthrottle,
+                        true
+                    );
                 } else {
-                     if (motor[i] > throttleRangeMax) {
-                        motorValue = motor[i];
-                    } else {
-                        motorValue = scaleRangef(motor[i], throttleRangeMin, throttleRangeMax,  motorConfig()->mincommand, reversibleMotorsConfig()->deadband_low);
-                        motorValue = constrain(motorValue, motorConfig()->mincommand,  reversibleMotorsConfig()->deadband_low);
-                    }
+                    motorValue = handleOutputScaling(
+                        motor[i],
+                        throttleRangeMax,
+                        motor[i],
+                        throttleRangeMin,
+                        throttleRangeMax,
+                        motorConfig()->mincommand,
+                        reversibleMotorsConfig()->deadband_low,
+                        false
+                    );
                 }
             } else {
                 motorValue = motor[i];

From 906b2deacffc3eea96b92a1da2ff6df297a5b48b Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Sun, 23 Feb 2020 21:00:33 +0100
Subject: [PATCH 012/179] Inform the mixer about arming event

---
 src/main/fc/fc_core.c        | 2 ++
 src/main/fc/runtime_config.h | 1 +
 src/main/flight/mixer.c      | 3 ++-
 3 files changed, 5 insertions(+), 1 deletion(-)

diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c
index 951857716e..64329dbe17 100755
--- a/src/main/fc/fc_core.c
+++ b/src/main/fc/fc_core.c
@@ -475,6 +475,8 @@ void tryArm(void)
 
         ENABLE_ARMING_FLAG(ARMED);
         ENABLE_ARMING_FLAG(WAS_EVER_ARMED);
+        //It is required to inform the mixer that arming was executed and it has to switch to the FORWARD direction
+        ENABLE_STATE(SET_REVERSIBLE_MOTORS_FORWARD);
         logicConditionReset();
         headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
 
diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h
index 67a2a40cca..472ccdb6f2 100644
--- a/src/main/fc/runtime_config.h
+++ b/src/main/fc/runtime_config.h
@@ -129,6 +129,7 @@ typedef enum {
     BOAT                                = (1 << 20),
     ALTITUDE_CONTROL                    = (1 << 21),    //It means it can fly
     MOVE_FORWARD_ONLY                   = (1 << 22),
+    SET_REVERSIBLE_MOTORS_FORWARD       = (1 << 23),
 } stateFlags_t;
 
 #define DISABLE_STATE(mask) (stateFlags &= ~(mask))
diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c
index a8b3579e67..9f8b8344a1 100755
--- a/src/main/flight/mixer.c
+++ b/src/main/flight/mixer.c
@@ -478,13 +478,14 @@ void FAST_CODE NOINLINE mixTable(const float dT)
 #endif
     if (feature(FEATURE_REVERSIBLE_MOTORS)) {
 
-        if (rcCommand[THROTTLE] >= (throttleDeadbandHigh)) {
+        if (rcCommand[THROTTLE] >= (throttleDeadbandHigh) || STATE(SET_REVERSIBLE_MOTORS_FORWARD)) {
             /*
              * Throttle is above deadband, FORWARD direction
              */
             reversibleMotorsThrottleState = MOTOR_DIRECTION_FORWARD;
             throttleRangeMax = motorConfig()->maxthrottle;
             throttleRangeMin = throttleDeadbandHigh;
+            DISABLE_STATE(SET_REVERSIBLE_MOTORS_FORWARD);
         } else if (rcCommand[THROTTLE] <= throttleDeadbandLow) {
             /*
              * Throttle is below deadband, BACKWARD direction

From 00bc08db1a8d3e384c7b5f9935b81a7dc3157b82 Mon Sep 17 00:00:00 2001
From: Michel Pastor <shellixyz@gmail.com>
Date: Tue, 25 Feb 2020 16:48:31 +0100
Subject: [PATCH 013/179] Add a F.Port section to the RX documentation

---
 docs/Rx.md | 16 ++++++++++++++++
 1 file changed, 16 insertions(+)

diff --git a/docs/Rx.md b/docs/Rx.md
index 51cd57b2a2..9b96f89b76 100644
--- a/docs/Rx.md
+++ b/docs/Rx.md
@@ -83,6 +83,22 @@ The bug prevents use of all 16 channels.  Upgrade to the latest OpenTX version t
 without the fix you are limited to 8 channels regardless of the CH1-16/D16 settings.
 
 
+### F.Port
+
+F.Port is a protocol running on async serial allowing 16 controls channels and telemetry on a single UART.
+
+Supported receivers include FrSky R-XSR, X4R, X4R-SB, XSR, XSR-M, R9M Slim, R9M Slim+, R9 Mini. For ACCST receivers you need to flash the corresponding firmware for it to output F.Port. For ACCESS receivers the protocol output from the receiver can be switched between S.Bus and F.Port from the model's setup page in the RX options.
+
+#### Connection
+
+Just connect the S.Port wire from the receiver to the TX pad of a free UART on your flight controller
+
+#### Configuration
+
+```
+set serialrx_inverted = true
+set serialrx_halfduplex = true
+```
 
 ### XBUS
 

From ccb97de6aaa4c44f57755b8cf66a59684b865668 Mon Sep 17 00:00:00 2001
From: nyway <niway@vip.qq.com>
Date: Thu, 27 Feb 2020 17:11:53 +0800
Subject: [PATCH 014/179] Add DALRCF722DUAL to release targets and modified
 support for DSHOT

Add DALRCF722DUAL to release targets and modified support for DSHOT
---
 make/release.mk                        | 2 +-
 src/main/target/DALRCF722DUAL/target.h | 3 +++
 2 files changed, 4 insertions(+), 1 deletion(-)

diff --git a/make/release.mk b/make/release.mk
index bc7c18e83e..0ebf3199bf 100644
--- a/make/release.mk
+++ b/make/release.mk
@@ -1,6 +1,6 @@
 RELEASE_TARGETS = AIRHEROF3 AIRHEROF3_QUAD
 
-RELEASE_TARGETS += LUX_RACE FURYF3 FURYF3_SPIFLASH RCEXPLORERF3 RMDO SPARKY KFC32F3_INAV FALCORE MOTOLAB ANYFC BLUEJAYF4 COLIBRI F4BY DALRCF405
+RELEASE_TARGETS += LUX_RACE FURYF3 FURYF3_SPIFLASH RCEXPLORERF3 RMDO SPARKY KFC32F3_INAV FALCORE MOTOLAB ANYFC BLUEJAYF4 COLIBRI F4BY DALRCF405 DALRCF722DUAL
 RELEASE_TARGETS += QUANTON REVO SPARKY2 PIKOBLX CLRACINGF4AIR CLRACINGF4AIRV2 PIXRACER BEEROTORF4 ANYFCF7 ANYFCF7_EXTERNAL_BARO
 RELEASE_TARGETS += ALIENFLIGHTNGF7
 
diff --git a/src/main/target/DALRCF722DUAL/target.h b/src/main/target/DALRCF722DUAL/target.h
index 124069874f..ccf6ac6138 100644
--- a/src/main/target/DALRCF722DUAL/target.h
+++ b/src/main/target/DALRCF722DUAL/target.h
@@ -143,6 +143,9 @@
 #define USE_SERIAL_4WAY_BLHELI_INTERFACE
 
 #define MAX_PWM_OUTPUT_PORTS    6
+#define USE_DSHOT
+#define USE_SERIALSHOT
+#define USE_ESC_SENSOR
 
 #define TARGET_IO_PORTA         0xffff
 #define TARGET_IO_PORTB         0xffff

From 68a909ce6b63fcd50de40f6a5292df3d038c6cea Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Mon, 9 Mar 2020 13:32:50 +0100
Subject: [PATCH 015/179] Allow dynamic and RPM filter to go down to 30Hz

---
 src/main/fc/settings.yaml | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index ec6f6cf9b8..b0e4b22d85 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -202,7 +202,7 @@ groups:
       - name: dynamic_gyro_notch_min_hz
         field: dynamicGyroNotchMinHz
         condition: USE_DYNAMIC_FILTERS
-        min: 60
+        min: 30
         max: 1000
       - name: gyro_to_use
         condition: USE_DUAL_GYRO
@@ -885,7 +885,7 @@ groups:
       - name: rpm_gyro_min_hz
         field: gyro_min_hz
         type: uint8_t
-        min: 50
+        min: 30
         max: 200
       - name: rpm_gyro_q
         field: gyro_q

From 6eda2af3596ffe3edd21d7a6c37fff9031ff58cd Mon Sep 17 00:00:00 2001
From: Jacky2k <fabian@opencode.eu>
Date: Mon, 9 Mar 2020 15:21:26 +0100
Subject: [PATCH 016/179] Waypoint hold time feature (#5398)

* Added support for waypoint hold time
---
 docs/Navigation.md                         |  4 +-
 src/main/fc/cli.c                          |  4 +-
 src/main/io/osd.c                          |  3 +-
 src/main/navigation/navigation.c           | 59 ++++++++++++++++++++--
 src/main/navigation/navigation.h           |  7 +--
 src/main/navigation/navigation_fixedwing.c |  2 +-
 src/main/navigation/navigation_private.h   |  7 +++
 7 files changed, 75 insertions(+), 11 deletions(-)

diff --git a/docs/Navigation.md b/docs/Navigation.md
index f56ae80489..a227b90629 100755
--- a/docs/Navigation.md
+++ b/docs/Navigation.md
@@ -66,7 +66,9 @@ Parameters:
   
   * `<alt>` - Altitude in cm.
   
-  * `<p1>` - For a "RTH waypoint" p1 > 0 alow landing. For a normal waypoint it means speed to this waypoint, it is taken into account only for multicopters and when > 50 and < nav_auto_speed.
+  * `<p1>` - For a "RTH waypoint" p1 > 0 alow landing. For a normal waypoint it means speed to this waypoint, it is taken into account only for multicopters and when > 50 and < nav_auto_speed. For POSHOLD TIME waypoint it is time time to wait in seconds.
+  
+  * `<p2>` - For a POSHOLD TIME it means speed to this waypoint, it is taken into account only for multicopters and when > 50 and < nav_auto_speed.
   
   * `<flag>` - Last waypoint must have set `flag` to 165 (0xA5), otherwise 0.
   
diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c
index 35bd228907..a4cba8f4e7 100755
--- a/src/main/fc/cli.c
+++ b/src/main/fc/cli.c
@@ -1322,7 +1322,7 @@ static void cliWaypoints(char *cmdline)
     } else if (sl_strcasecmp(cmdline, "save") == 0) {
         posControl.waypointListValid = false;
         for (int i = 0; i < NAV_MAX_WAYPOINTS; i++) {
-            if (!(posControl.waypointList[i].action == NAV_WP_ACTION_WAYPOINT || posControl.waypointList[i].action == NAV_WP_ACTION_JUMP || posControl.waypointList[i].action == NAV_WP_ACTION_RTH)) break;
+            if (!(posControl.waypointList[i].action == NAV_WP_ACTION_WAYPOINT || posControl.waypointList[i].action == NAV_WP_ACTION_JUMP || posControl.waypointList[i].action == NAV_WP_ACTION_RTH || posControl.waypointList[i].action == NAV_WP_ACTION_HOLD_TIME)) break;
             if (posControl.waypointList[i].flag == NAV_WP_FLAG_LAST) {
                 posControl.waypointCount = i + 1;
                 posControl.waypointListValid = true;
@@ -1374,7 +1374,7 @@ static void cliWaypoints(char *cmdline)
             }
             if (validArgumentCount < 4) {
                 cliShowParseError();
-            } else if (!(action == 0 || action == NAV_WP_ACTION_WAYPOINT || action == NAV_WP_ACTION_RTH) || (p1 < 0) || !(flag == 0 || flag == NAV_WP_FLAG_LAST)) {
+            } else if (!(action == 0 || action == NAV_WP_ACTION_WAYPOINT || action == NAV_WP_ACTION_RTH || action == NAV_WP_ACTION_HOLD_TIME) || (p1 < 0) || !(flag == 0 || flag == NAV_WP_FLAG_LAST)) {
                 cliShowParseError();
             } else {
                 posControl.waypointList[i].action = action;
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index 220bf8e50f..8e45afc499 100755
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -787,7 +787,8 @@ static const char * navigationStateMessage(void)
             // Used by HOLD flight modes. No information to add.
             break;
         case MW_NAV_STATE_HOLD_TIMED:
-            // Not used anymore
+            // TODO: Maybe we can display a count down
+            return OSD_MESSAGE_STR("HOLDING WAYPOINT");
             break;
         case MW_NAV_STATE_WP_ENROUTE:
             // TODO: Show WP number
diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index 07608ad754..cc9a3e7593 100755
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -231,6 +231,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_INITIALIZE(nav
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(navigationFSMState_t previousState);
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(navigationFSMState_t previousState);
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_REACHED(navigationFSMState_t previousState);
+static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME(navigationFSMState_t previousState);
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_NEXT(navigationFSMState_t previousState);
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_FINISHED(navigationFSMState_t previousState);
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_RTH_LAND(navigationFSMState_t previousState);
@@ -665,6 +666,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
         .onEvent = {
             [NAV_FSM_EVENT_TIMEOUT]                     = NAV_STATE_WAYPOINT_REACHED,   // re-process state
             [NAV_FSM_EVENT_SUCCESS]                     = NAV_STATE_WAYPOINT_NEXT,
+            [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME] = NAV_STATE_WAYPOINT_HOLD_TIME,
             [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED] = NAV_STATE_WAYPOINT_FINISHED,
             [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND] = NAV_STATE_WAYPOINT_RTH_LAND,
             [NAV_FSM_EVENT_SWITCH_TO_IDLE]              = NAV_STATE_IDLE,
@@ -677,6 +679,27 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
         }
     },
 
+    [NAV_STATE_WAYPOINT_HOLD_TIME] = {
+        .persistentId = NAV_PERSISTENT_ID_WAYPOINT_HOLD_TIME,                             // There is no state for timed hold?
+        .onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME,
+        .timeoutMs = 10,
+        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
+        .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
+        .mwState = MW_NAV_STATE_HOLD_TIMED,
+        .mwError = MW_NAV_ERROR_NONE,
+        .onEvent = {
+            [NAV_FSM_EVENT_TIMEOUT]                     = NAV_STATE_WAYPOINT_HOLD_TIME,     // re-process the state
+            [NAV_FSM_EVENT_SUCCESS]                     = NAV_STATE_WAYPOINT_NEXT,          // successfully reached waypoint
+            [NAV_FSM_EVENT_SWITCH_TO_IDLE]              = NAV_STATE_IDLE,
+            [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD]           = NAV_STATE_ALTHOLD_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D]        = NAV_STATE_POSHOLD_3D_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_RTH]               = NAV_STATE_RTH_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D]         = NAV_STATE_CRUISE_2D_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D]         = NAV_STATE_CRUISE_3D_INITIALIZE,
+        }
+    },
+
     [NAV_STATE_WAYPOINT_RTH_LAND] = {
         .persistentId = NAV_PERSISTENT_ID_WAYPOINT_RTH_LAND,
         .onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_RTH_LAND,
@@ -1371,6 +1394,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav
     UNUSED(previousState);
 
     switch ((navWaypointActions_e)posControl.waypointList[posControl.activeWaypointIndex].action) {
+        case NAV_WP_ACTION_HOLD_TIME:
         case NAV_WP_ACTION_WAYPOINT:
             calculateAndSetActiveWaypoint(&posControl.waypointList[posControl.activeWaypointIndex]);
             posControl.wpInitialDistance = calculateDistanceToDestination(&posControl.activeWaypoint.pos);
@@ -1419,6 +1443,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na
     // If no position sensor available - land immediately
     if ((posControl.flags.estPosStatus >= EST_USABLE) && (posControl.flags.estHeadingStatus >= EST_USABLE)) {
         switch ((navWaypointActions_e)posControl.waypointList[posControl.activeWaypointIndex].action) {
+            case NAV_WP_ACTION_HOLD_TIME:
             case NAV_WP_ACTION_WAYPOINT:
                 if (isWaypointReached(&posControl.activeWaypoint, false) || isWaypointMissed(&posControl.activeWaypoint)) {
                     return NAV_FSM_EVENT_SUCCESS;   // will switch to NAV_STATE_WAYPOINT_REACHED
@@ -1479,11 +1504,30 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_REACHED(naviga
             else {
                 return NAV_FSM_EVENT_SUCCESS;   // NAV_STATE_WAYPOINT_NEXT
             }
+        case NAV_WP_ACTION_HOLD_TIME:
+            // Save the current time for the time the waypoint was reached
+            posControl.wpReachedTime = millis();
+            return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME;
     }
 
     UNREACHABLE();
 }
 
+static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME(navigationFSMState_t previousState)
+{
+    UNUSED(previousState);
+
+    timeMs_t currentTime = millis();
+
+    if(posControl.waypointList[posControl.activeWaypointIndex].p1 <= 0)
+        return NAV_FSM_EVENT_SUCCESS;
+
+    if(posControl.wpReachedTime != 0 && currentTime - posControl.wpReachedTime >= (timeMs_t)posControl.waypointList[posControl.activeWaypointIndex].p1*1000L)
+        return NAV_FSM_EVENT_SUCCESS;
+
+    return NAV_FSM_EVENT_NONE;      // will re-process state in >10ms
+}
+
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_RTH_LAND(navigationFSMState_t previousState)
 {
     UNUSED(previousState);
@@ -2641,7 +2685,7 @@ void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData)
     }
     // WP #1 - #NAV_MAX_WAYPOINTS - common waypoints - pre-programmed mission
     else if ((wpNumber >= 1) && (wpNumber <= NAV_MAX_WAYPOINTS) && !ARMING_FLAG(ARMED)) {
-        if (wpData->action == NAV_WP_ACTION_WAYPOINT || wpData->action == NAV_WP_ACTION_JUMP || wpData->action == NAV_WP_ACTION_RTH) {
+        if (wpData->action == NAV_WP_ACTION_WAYPOINT || wpData->action == NAV_WP_ACTION_JUMP || wpData->action == NAV_WP_ACTION_RTH || wpData->action == NAV_WP_ACTION_HOLD_TIME) {
             // Only allow upload next waypoint (continue upload mission) or first waypoint (new mission)
             if (wpNumber == (posControl.waypointCount + 1) || wpNumber == 1) {
                 posControl.waypointList[wpNumber - 1] = *wpData;
@@ -2768,6 +2812,11 @@ bool isApproachingLastWaypoint(void)
     }
 }
 
+bool isWaypointWait(void)
+{
+    return NAV_Status.state == MW_NAV_STATE_HOLD_TIMED;
+}
+
 float getActiveWaypointSpeed(void)
 {
     if (posControl.flags.isAdjustingPosition) {
@@ -2778,8 +2827,12 @@ float getActiveWaypointSpeed(void)
         uint16_t waypointSpeed = navConfig()->general.max_auto_speed;
 
         if (navGetStateFlags(posControl.navState) & NAV_AUTO_WP) {
-            if (posControl.waypointCount > 0 && posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_WAYPOINT) {
-                const float wpSpecificSpeed = posControl.waypointList[posControl.activeWaypointIndex].p1;
+            if (posControl.waypointCount > 0 && (posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_WAYPOINT || posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_HOLD_TIME)) {
+                float wpSpecificSpeed = 0.0f;
+                if(posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_WAYPOINT)
+                    wpSpecificSpeed = posControl.waypointList[posControl.activeWaypointIndex].p1;
+                else
+                    wpSpecificSpeed = posControl.waypointList[posControl.activeWaypointIndex].p2;
                 if (wpSpecificSpeed >= 50.0f && wpSpecificSpeed <= navConfig()->general.max_auto_speed) {
                     waypointSpeed = wpSpecificSpeed;
                 }
diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h
index d20f3b60e9..fcb5a4649d 100755
--- a/src/main/navigation/navigation.h
+++ b/src/main/navigation/navigation.h
@@ -233,9 +233,10 @@ typedef struct gpsOrigin_s {
 } gpsOrigin_t;
 
 typedef enum {
-    NAV_WP_ACTION_WAYPOINT = 0x01,
-    NAV_WP_ACTION_JUMP     = 0x06, 
-    NAV_WP_ACTION_RTH      = 0x04
+    NAV_WP_ACTION_WAYPOINT  = 0x01,
+    NAV_WP_ACTION_HOLD_TIME = 0x03,
+    NAV_WP_ACTION_JUMP      = 0x06, 
+    NAV_WP_ACTION_RTH       = 0x04
 } navWaypointActions_e;
 
 typedef enum {
diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c
index dcdf18d991..fe2e34ba18 100755
--- a/src/main/navigation/navigation_fixedwing.c
+++ b/src/main/navigation/navigation_fixedwing.c
@@ -243,7 +243,7 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod)
 
     // If angular visibility of a waypoint is less than 30deg, don't calculate circular loiter, go straight to the target
     #define TAN_15DEG    0.26795f
-    bool needToCalculateCircularLoiter = isApproachingLastWaypoint()
+    bool needToCalculateCircularLoiter = (isApproachingLastWaypoint() || isWaypointWait())
                                             && (distanceToActualTarget <= (navConfig()->fw.loiter_radius / TAN_15DEG))
                                             && (distanceToActualTarget > 50.0f)
                                             && !FLIGHT_MODE(NAV_CRUISE_MODE);
diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h
index a11e9a0f62..2a17449318 100755
--- a/src/main/navigation/navigation_private.h
+++ b/src/main/navigation/navigation_private.h
@@ -139,9 +139,11 @@ typedef enum {
 
     NAV_FSM_EVENT_STATE_SPECIFIC_1,             // State-specific event
     NAV_FSM_EVENT_STATE_SPECIFIC_2,             // State-specific event
+    NAV_FSM_EVENT_STATE_SPECIFIC_3,             // State-specific event
     NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING = NAV_FSM_EVENT_STATE_SPECIFIC_1,
     NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND = NAV_FSM_EVENT_STATE_SPECIFIC_1,
     NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED = NAV_FSM_EVENT_STATE_SPECIFIC_2,
+    NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME = NAV_FSM_EVENT_STATE_SPECIFIC_3,
 
     NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D,
     NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D,
@@ -198,6 +200,8 @@ typedef enum {
     NAV_PERSISTENT_ID_CRUISE_3D_INITIALIZE                      = 32,
     NAV_PERSISTENT_ID_CRUISE_3D_IN_PROGRESS                     = 33,
     NAV_PERSISTENT_ID_CRUISE_3D_ADJUSTING                       = 34,
+
+    NAV_PERSISTENT_ID_WAYPOINT_HOLD_TIME                        = 35,
 } navigationPersistentId_e;
 
 typedef enum {
@@ -224,6 +228,7 @@ typedef enum {
     NAV_STATE_WAYPOINT_PRE_ACTION,
     NAV_STATE_WAYPOINT_IN_PROGRESS,
     NAV_STATE_WAYPOINT_REACHED,
+    NAV_STATE_WAYPOINT_HOLD_TIME,
     NAV_STATE_WAYPOINT_NEXT,
     NAV_STATE_WAYPOINT_FINISHED,
     NAV_STATE_WAYPOINT_RTH_LAND,
@@ -357,6 +362,7 @@ typedef struct {
     float                       wpInitialAltitude; // Altitude at start of WP
     float                       wpInitialDistance; // Distance when starting flight to WP
     float                       wpDistance;        // Distance to active WP
+    timeMs_t                    wpReachedTime;     // Time the waypoint was reached
 
     /* Internals & statistics */
     int16_t                     rcAdjustment[4];
@@ -393,6 +399,7 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAlti
 
 bool isWaypointReached(const navWaypointPosition_t * waypoint, const bool isWaypointHome);
 bool isWaypointMissed(const navWaypointPosition_t * waypoint);
+bool isWaypointWait(void);
 bool isApproachingLastWaypoint(void);
 float getActiveWaypointSpeed(void);
 

From 61b66f7a19a60cb75c28eba9195a10212d61c359 Mon Sep 17 00:00:00 2001
From: Konstantin Sharlaimov <konstantin.sharlaimov@gmail.com>
Date: Sat, 30 Nov 2019 18:47:04 +0100
Subject: [PATCH 017/179] Revert "Revert "Version bump to 2.4""

This reverts commit 90fc08dbf49998c938fd62ba607ad73c2790f226.
---
 src/main/build/version.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/build/version.h b/src/main/build/version.h
index f509db3b54..ec17abe33a 100644
--- a/src/main/build/version.h
+++ b/src/main/build/version.h
@@ -16,7 +16,7 @@
  */
 
 #define FC_VERSION_MAJOR            2  // increment when a major release is made (big new feature, etc)
-#define FC_VERSION_MINOR            3  // increment when a minor release is made (small new feature, change etc)
+#define FC_VERSION_MINOR            4  // increment when a minor release is made (small new feature, change etc)
 #define FC_VERSION_PATCH_LEVEL      0  // increment when a bug is fixed
 
 #define STR_HELPER(x) #x

From 89c00fc354a78cf1d6a970ae5364ea37e2c14e44 Mon Sep 17 00:00:00 2001
From: "Konstantin (DigitalEntity) Sharlaimov"
 <konstantin.sharlaimov@gmail.com>
Date: Mon, 9 Mar 2020 21:01:38 +0100
Subject: [PATCH 018/179] [VTX] Remove unused check; Fix Pit mode CMS

---
 src/main/cms/cms_menu_vtx.c |  3 +--
 src/main/io/vtx.c           | 20 +-------------------
 2 files changed, 2 insertions(+), 21 deletions(-)

diff --git a/src/main/cms/cms_menu_vtx.c b/src/main/cms/cms_menu_vtx.c
index bf35aa8997..249683fe87 100644
--- a/src/main/cms/cms_menu_vtx.c
+++ b/src/main/cms/cms_menu_vtx.c
@@ -142,7 +142,7 @@ static void cms_Vtx_initSettings(void)
         vtxPitMode = onoff ? 2 : 1;
     }
     else {
-        vtxPitMode = vtxSettingsConfig()->lowPowerDisarm ? 2 : 1;
+        vtxPitMode = 0;
     }
 }
 
@@ -165,7 +165,6 @@ static long cms_Vtx_Commence(displayPort_t *pDisp, const void *self)
     vtxSettingsConfigMutable()->band = vtxBand;
     vtxSettingsConfigMutable()->channel = vtxChan;
     vtxSettingsConfigMutable()->power = vtxPower;
-    vtxSettingsConfigMutable()->lowPowerDisarm = (vtxPitMode == 2 ? 1 : 0);
 
     saveConfigAndNotify();
 
diff --git a/src/main/io/vtx.c b/src/main/io/vtx.c
index 485a880353..1971b09b04 100644
--- a/src/main/io/vtx.c
+++ b/src/main/io/vtx.c
@@ -56,7 +56,6 @@ typedef enum {
     VTX_PARAM_POWER = 0,
     VTX_PARAM_BANDCHAN,
     VTX_PARAM_PITMODE,
-    VTX_PARAM_CONFIRM,
     VTX_PARAM_COUNT
 } vtxScheduleParams_e;
 
@@ -91,7 +90,7 @@ static bool vtxProcessBandAndChannel(vtxDevice_t *vtxDevice, const vtxSettingsCo
         return false;
     }
 
-    if(!ARMING_FLAG(ARMED)) {
+    if(!ARMING_FLAG(ARMED) && runtimeSettings->band) {
         uint8_t vtxBand;
         uint8_t vtxChan;
         if (!vtxCommonGetBandAndChannel(vtxDevice, &vtxBand, &vtxChan)) {
@@ -149,20 +148,6 @@ static bool vtxProcessPitMode(vtxDevice_t *vtxDevice, const vtxSettingsConfig_t
     return false;
 }
 
-static bool vtxProcessCheckParameters(vtxDevice_t *vtxDevice, const vtxSettingsConfig_t * runtimeSettings)
-{
-    uint8_t vtxBand;
-    uint8_t vtxChan;
-    uint8_t vtxPower;
-
-    vtxCommonGetPowerIndex(vtxDevice, &vtxPower);
-    vtxCommonGetBandAndChannel(vtxDevice, &vtxBand, &vtxChan);
-
-    return (runtimeSettings->band && runtimeSettings->band != vtxBand) ||
-           (runtimeSettings->channel != vtxChan) ||
-           (runtimeSettings->power != vtxPower);
-}
-
 void vtxUpdate(timeUs_t currentTimeUs)
 {
     static uint8_t currentSchedule = 0;
@@ -191,9 +176,6 @@ void vtxUpdate(timeUs_t currentTimeUs)
             case VTX_PARAM_PITMODE:
                 vtxUpdatePending = vtxProcessPitMode(vtxDevice, runtimeSettings);
                 break;
-            case VTX_PARAM_CONFIRM:
-                vtxUpdatePending = vtxProcessCheckParameters(vtxDevice, runtimeSettings);
-                break;
             default:
                 break;
         }

From 8f963b240e611bb3ebbb6e4733ce79868877758e Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Mon, 9 Mar 2020 21:09:00 +0100
Subject: [PATCH 019/179] Allow some files to be compiled with -O2 instead of
 -Os

---
 Makefile       | 16 +++++++++++-----
 make/source.mk |  6 ++++++
 2 files changed, 17 insertions(+), 5 deletions(-)

diff --git a/Makefile b/Makefile
index f19b1b0938..700e26125b 100644
--- a/Makefile
+++ b/Makefile
@@ -178,8 +178,7 @@ ifeq ($(DEBUG),GDB)
 OPTIMIZE    = -O0
 LTO_FLAGS   = $(OPTIMIZE)
 else
-OPTIMIZE    = -Os
-LTO_FLAGS   = -flto -fuse-linker-plugin $(OPTIMIZE)
+LTO_FLAGS   = -flto -fuse-linker-plugin
 endif
 
 ifneq ($(SEMIHOSTING),)
@@ -301,16 +300,23 @@ $(TARGET_ELF): $(TARGET_OBJS)
 	$(V1) $(CROSS_CC) -o $@ $(filter %.o, $^) $(LDFLAGS)
 	$(V0) $(SIZE) $(TARGET_ELF)
 
+define compile_file
+	echo "%% ($(1)) $<" "$(STDOUT)" && \
+	$(CROSS_CC) -c -o $@ $(CFLAGS) $(2) $<
+endef
+
 # Compile
 $(TARGET_OBJ_DIR)/%.o: %.c
 	$(V1) mkdir -p $(dir $@)
-	$(V1) echo %% $(notdir $<) "$(STDOUT)"
-	$(V1) $(CROSS_CC) -c -o $@ $(CFLAGS) $<
+	$(V1) $(if $(findstring $<,$(NORMAL_OPTIMISED_SRC)), \
+		$(call compile_file,normal optimised,-O2) \
+	, \
+		$(call compile_file,size optimised,-Os) \
+	)
 ifeq ($(GENERATE_ASM), 1)
 	$(V1) $(CROSS_CC) -S -fverbose-asm -Wa,-aslh -o $(patsubst %.o,%.txt.S,$@) -g $(ASM_CFLAGS) $<
 endif
 
-
 # Assemble
 $(TARGET_OBJ_DIR)/%.o: %.s
 	$(V1) mkdir -p $(dir $@)
diff --git a/make/source.mk b/make/source.mk
index 288219253e..0166efeb58 100644
--- a/make/source.mk
+++ b/make/source.mk
@@ -277,3 +277,9 @@ endif
 
 # Search path and source files for the ST stdperiph library
 VPATH        := $(VPATH):$(STDPERIPH_DIR)/src
+
+NORMAL_OPTIMISED_SRC := ""
+NORMAL_OPTIMISED_SRC := $(NORMAL_OPTIMISED_SRC) \
+            ./src/main/common/filter.c \
+            ./src/main/sensors/gyro.c \
+            ./src/main/common/maths.c \
\ No newline at end of file

From 780728d633b698a822be368a622275746994ff35 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Mon, 9 Mar 2020 22:33:54 +0100
Subject: [PATCH 020/179] Revisit FAST_RAM usages

---
 src/main/common/maths.c                        |  2 +-
 src/main/config/feature.c                      |  2 +-
 src/main/fc/runtime_config.c                   |  2 +-
 src/main/flight/mixer.c                        |  2 +-
 src/main/flight/pid.c                          | 12 ++++++------
 src/main/flight/servos.c                       |  6 +++---
 src/main/navigation/navigation_fw_launch.c     |  4 ++--
 src/main/navigation/navigation_pos_estimator.c |  2 +-
 8 files changed, 16 insertions(+), 16 deletions(-)

diff --git a/src/main/common/maths.c b/src/main/common/maths.c
index 13d615e361..e96ac7c1e6 100644
--- a/src/main/common/maths.c
+++ b/src/main/common/maths.c
@@ -159,7 +159,7 @@ int constrain(int amt, int low, int high)
         return amt;
 }
 
-float FAST_CODE NOINLINE constrainf(float amt, float low, float high)
+float constrainf(float amt, float low, float high)
 {
     if (amt < low)
         return low;
diff --git a/src/main/config/feature.c b/src/main/config/feature.c
index 67715427e0..ae65df4b77 100755
--- a/src/main/config/feature.c
+++ b/src/main/config/feature.c
@@ -34,7 +34,7 @@ bool featureConfigured(uint32_t mask)
     return featureConfig()->enabledFeatures & mask;
 }
 
-bool FAST_CODE NOINLINE feature(uint32_t mask)
+bool feature(uint32_t mask)
 {
     return activeFeaturesLatch & mask;
 }
diff --git a/src/main/fc/runtime_config.c b/src/main/fc/runtime_config.c
index 0d1a60a25c..60f911bd6b 100644
--- a/src/main/fc/runtime_config.c
+++ b/src/main/fc/runtime_config.c
@@ -80,7 +80,7 @@ uint32_t disableFlightMode(flightModeFlags_e mask)
     return flightModeFlags;
 }
 
-bool FAST_CODE NOINLINE sensors(uint32_t mask)
+bool sensors(uint32_t mask)
 {
     return enabledSensors & mask;
 }
diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c
index 80587d0c1e..09f7b32e8f 100755
--- a/src/main/flight/mixer.c
+++ b/src/main/flight/mixer.c
@@ -138,7 +138,7 @@ static void computeMotorCount(void)
     }
 }
 
-uint8_t FAST_CODE NOINLINE getMotorCount(void) {
+uint8_t getMotorCount(void) {
     return motorCount;
 }
 
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 1fb455f336..5965700597 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -574,7 +574,7 @@ static void pidLevel(pidState_t *pidState, flight_dynamics_index_t axis, float h
 }
 
 /* Apply angular acceleration limit to rate target to limit extreme stick inputs to respect physical capabilities of the machine */
-static void FAST_CODE pidApplySetpointRateLimiting(pidState_t *pidState, flight_dynamics_index_t axis, float dT)
+static void pidApplySetpointRateLimiting(pidState_t *pidState, flight_dynamics_index_t axis, float dT)
 {
     const uint32_t axisAccelLimit = (axis == FD_YAW) ? pidProfile()->axisAccelerationLimitYaw : pidProfile()->axisAccelerationLimitRollPitch;
 
@@ -596,7 +596,7 @@ bool isFixedWingItermLimitActive(float stickPosition)
     return fabsf(stickPosition) > pidProfile()->fixedWingItermLimitOnStickPosition;
 }
 
-static FAST_CODE NOINLINE float pTermProcess(pidState_t *pidState, float rateError, float dT) {
+static float pTermProcess(pidState_t *pidState, float rateError, float dT) {
     float newPTerm = rateError * pidState->kP;
 
     return pidState->ptermFilterApplyFn(&pidState->ptermLpfState, newPTerm, yawLpfHz, dT);
@@ -940,7 +940,7 @@ static void pidApplyFpvCameraAngleMix(pidState_t *pidState, uint8_t fpvCameraAng
     pidState[YAW].rateTarget = constrainf(yawRate * cosCameraAngle + rollRate * sinCameraAngle, -GYRO_SATURATION_LIMIT, GYRO_SATURATION_LIMIT);
 }
 
-void FAST_CODE checkItermLimitingActive(pidState_t *pidState)
+void checkItermLimitingActive(pidState_t *pidState)
 {
     bool shouldActivate;
     if (usedPidControllerType == PID_TYPE_PIFF) {
@@ -953,7 +953,7 @@ void FAST_CODE checkItermLimitingActive(pidState_t *pidState)
     pidState->itermLimitActive = STATE(ANTI_WINDUP) || shouldActivate; 
 }
 
-void FAST_CODE NOINLINE pidController(float dT)
+void pidController(float dT)
 {
     if (!pidFiltersConfigured) {
         return;
@@ -1119,9 +1119,9 @@ void pidInit(void)
     }
 }
 
-const pidBank_t FAST_CODE NOINLINE * pidBank(void) { 
+const pidBank_t * pidBank(void) { 
     return usedPidControllerType == PID_TYPE_PIFF ? &pidProfile()->bank_fw : &pidProfile()->bank_mc; 
 }
-pidBank_t FAST_CODE NOINLINE * pidBankMutable(void) { 
+pidBank_t * pidBankMutable(void) { 
     return usedPidControllerType == PID_TYPE_PIFF ? &pidProfileMutable()->bank_fw : &pidProfileMutable()->bank_mc;
 }
diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c
index dac370616b..f705318c38 100755
--- a/src/main/flight/servos.c
+++ b/src/main/flight/servos.c
@@ -444,17 +444,17 @@ void processServoAutotrim(void)
     }
 }
 
-bool FAST_CODE NOINLINE isServoOutputEnabled(void)
+bool isServoOutputEnabled(void)
 {
     return servoOutputEnabled;
 }
 
-void NOINLINE setServoOutputEnabled(bool flag)
+void setServoOutputEnabled(bool flag)
 {
     servoOutputEnabled = flag;
 }
 
-bool FAST_CODE NOINLINE isMixerUsingServos(void)
+bool isMixerUsingServos(void)
 {
     return mixerUsesServos;
 }
diff --git a/src/main/navigation/navigation_fw_launch.c b/src/main/navigation/navigation_fw_launch.c
index 6426b41afb..843d27f650 100755
--- a/src/main/navigation/navigation_fw_launch.c
+++ b/src/main/navigation/navigation_fw_launch.c
@@ -106,7 +106,7 @@ void resetFixedWingLaunchController(timeUs_t currentTimeUs)
     launchState.motorControlAllowed = false;
 }
 
-bool FAST_CODE isFixedWingLaunchDetected(void)
+bool isFixedWingLaunchDetected(void)
 {
     return launchState.launchDetected;
 }
@@ -117,7 +117,7 @@ void enableFixedWingLaunchController(timeUs_t currentTimeUs)
     launchState.motorControlAllowed = true;
 }
 
-bool FAST_CODE isFixedWingLaunchFinishedOrAborted(void)
+bool isFixedWingLaunchFinishedOrAborted(void)
 {
     return launchState.launchFinished;
 }
diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c
index 3e2b9047a4..d4dbb9a2c6 100755
--- a/src/main/navigation/navigation_pos_estimator.c
+++ b/src/main/navigation/navigation_pos_estimator.c
@@ -823,7 +823,7 @@ void initializePositionEstimator(void)
  * Update estimator
  *  Update rate: loop rate (>100Hz)
  */
-void FAST_CODE NOINLINE updatePositionEstimator(void)
+void updatePositionEstimator(void)
 {
     static bool isInitialized = false;
 

From 10b74bb9f804c7348eb25ff646352f7f14fa53c4 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Tue, 10 Mar 2020 08:32:00 +0100
Subject: [PATCH 021/179] Leave pidController in ITCM_RAM

---
 src/main/flight/pid.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 5965700597..eced2d6a4c 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -953,7 +953,7 @@ void checkItermLimitingActive(pidState_t *pidState)
     pidState->itermLimitActive = STATE(ANTI_WINDUP) || shouldActivate; 
 }
 
-void pidController(float dT)
+void FAST_CODE pidController(float dT)
 {
     if (!pidFiltersConfigured) {
         return;

From 4a5d924a7ead5e03f74a936d05f885d5bc49d5fd Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Tue, 10 Mar 2020 13:27:47 +0100
Subject: [PATCH 022/179] Allow file and function optimization compiler
 directives

---
 make/source.mk                | 11 +++++++----
 src/main/target/common_post.h | 23 +++++++++++++++++++++++
 2 files changed, 30 insertions(+), 4 deletions(-)

diff --git a/make/source.mk b/make/source.mk
index 0166efeb58..83030af968 100644
--- a/make/source.mk
+++ b/make/source.mk
@@ -279,7 +279,10 @@ endif
 VPATH        := $(VPATH):$(STDPERIPH_DIR)/src
 
 NORMAL_OPTIMISED_SRC := ""
-NORMAL_OPTIMISED_SRC := $(NORMAL_OPTIMISED_SRC) \
-            ./src/main/common/filter.c \
-            ./src/main/sensors/gyro.c \
-            ./src/main/common/maths.c \
\ No newline at end of file
+ifneq ($(TARGET),$(filter $(TARGET),$(F3_TARGETS)))
+# NORMAL_OPTIMISED_SRC := $(NORMAL_OPTIMISED_SRC) \
+#             ./src/main/common/filter.c \
+#             ./src/main/sensors/gyro.c \
+#             ./src/main/common/maths.c \
+
+endif #!F3
\ No newline at end of file
diff --git a/src/main/target/common_post.h b/src/main/target/common_post.h
index 92a0faf835..cba8d1ffb1 100644
--- a/src/main/target/common_post.h
+++ b/src/main/target/common_post.h
@@ -60,3 +60,26 @@
 // This feature uses 'arm_math.h', which does not exist for x86.
 #undef USE_DYNAMIC_FILTERS
 #endif
+
+//Defines for compiler optimizations
+#ifndef STM32F3
+#define OPTIMIZE_SPEED "-Ofast"
+#define OPTIMIZE_NORMAL "-O2"
+#define OPTIMIZE_SIZE "-Os"
+#define FUNCTION_COMPILE_SIZE __attribute__((optimize(OPTIMIZE_SIZE)))
+#define FUNCTION_COMPILE_NORMAL __attribute__((optimize(OPTIMIZE_NORMAL)))
+#define FUNCTION_COMPILE_SPEED __attribute__((optimize(OPTIMIZE_SPEED)))
+#define FILE_COMPILE_SIZE _Pragma("GCC optimize(\"Os\")")
+#define FILE_COMPILE_NORMAL _Pragma("GCC optimize(\"O2\")")
+#define FILE_COMPILE_SPEED _Pragma("GCC optimize(\"Ofast\")")
+#else
+#define OPTIMIZE_SIZE
+#define OPTIMIZE_NORMAL
+#define OPTIMIZE_SPEED
+#define FUNCTION_COMPILE_SIZE
+#define FUNCTION_COMPILE_NORMAL
+#define FUNCTION_COMPILE_SPEED
+#define FILE_COMPILE_SIZE
+#define FILE_COMPILE_NORMAL
+#define FILE_COMPILE_SPEED
+#endif

From a9b127340bfc8567510d4705af08efa1b7503f16 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Tue, 10 Mar 2020 13:37:42 +0100
Subject: [PATCH 023/179] Change naming of optimizer defines

---
 src/main/target/common_post.h | 16 ++++++++--------
 1 file changed, 8 insertions(+), 8 deletions(-)

diff --git a/src/main/target/common_post.h b/src/main/target/common_post.h
index cba8d1ffb1..276c5f23e8 100644
--- a/src/main/target/common_post.h
+++ b/src/main/target/common_post.h
@@ -66,20 +66,20 @@
 #define OPTIMIZE_SPEED "-Ofast"
 #define OPTIMIZE_NORMAL "-O2"
 #define OPTIMIZE_SIZE "-Os"
-#define FUNCTION_COMPILE_SIZE __attribute__((optimize(OPTIMIZE_SIZE)))
+#define FUNCTION_COMPILE_FOR_SIZE __attribute__((optimize(OPTIMIZE_SIZE)))
 #define FUNCTION_COMPILE_NORMAL __attribute__((optimize(OPTIMIZE_NORMAL)))
-#define FUNCTION_COMPILE_SPEED __attribute__((optimize(OPTIMIZE_SPEED)))
-#define FILE_COMPILE_SIZE _Pragma("GCC optimize(\"Os\")")
+#define FUNCTION_COMPILE_FOR_SIZE __attribute__((optimize(OPTIMIZE_SPEED)))
+#define FILE_COMPILE_FOR_SIZE _Pragma("GCC optimize(\"Os\")")
 #define FILE_COMPILE_NORMAL _Pragma("GCC optimize(\"O2\")")
-#define FILE_COMPILE_SPEED _Pragma("GCC optimize(\"Ofast\")")
+#define FILE_COMPILE_FOR_SPEED _Pragma("GCC optimize(\"Ofast\")")
 #else
 #define OPTIMIZE_SIZE
 #define OPTIMIZE_NORMAL
 #define OPTIMIZE_SPEED
-#define FUNCTION_COMPILE_SIZE
+#define FUNCTION_COMPILE_FOR_SIZE
 #define FUNCTION_COMPILE_NORMAL
-#define FUNCTION_COMPILE_SPEED
-#define FILE_COMPILE_SIZE
+#define FUNCTION_COMPILE_FOR_SIZE
+#define FILE_COMPILE_FOR_SIZE
 #define FILE_COMPILE_NORMAL
-#define FILE_COMPILE_SPEED
+#define FILE_COMPILE_FOR_SPEED
 #endif

From 2360782c7c6429fedc505ae299d2f99786346fa9 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Tue, 10 Mar 2020 13:39:17 +0100
Subject: [PATCH 024/179] Simplify defines

---
 src/main/target/common_post.h | 12 +++---------
 1 file changed, 3 insertions(+), 9 deletions(-)

diff --git a/src/main/target/common_post.h b/src/main/target/common_post.h
index 276c5f23e8..5b4e8c7563 100644
--- a/src/main/target/common_post.h
+++ b/src/main/target/common_post.h
@@ -63,19 +63,13 @@
 
 //Defines for compiler optimizations
 #ifndef STM32F3
-#define OPTIMIZE_SPEED "-Ofast"
-#define OPTIMIZE_NORMAL "-O2"
-#define OPTIMIZE_SIZE "-Os"
-#define FUNCTION_COMPILE_FOR_SIZE __attribute__((optimize(OPTIMIZE_SIZE)))
-#define FUNCTION_COMPILE_NORMAL __attribute__((optimize(OPTIMIZE_NORMAL)))
-#define FUNCTION_COMPILE_FOR_SIZE __attribute__((optimize(OPTIMIZE_SPEED)))
+#define FUNCTION_COMPILE_FOR_SIZE __attribute__((optimize("-Os")))
+#define FUNCTION_COMPILE_NORMAL __attribute__((optimize("-O2")))
+#define FUNCTION_COMPILE_FOR_SIZE __attribute__((optimize("-Ofast")))
 #define FILE_COMPILE_FOR_SIZE _Pragma("GCC optimize(\"Os\")")
 #define FILE_COMPILE_NORMAL _Pragma("GCC optimize(\"O2\")")
 #define FILE_COMPILE_FOR_SPEED _Pragma("GCC optimize(\"Ofast\")")
 #else
-#define OPTIMIZE_SIZE
-#define OPTIMIZE_NORMAL
-#define OPTIMIZE_SPEED
 #define FUNCTION_COMPILE_FOR_SIZE
 #define FUNCTION_COMPILE_NORMAL
 #define FUNCTION_COMPILE_FOR_SIZE

From 6753c410c6151aef75ce5e993ecd8c13a6bdfb10 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Tue, 10 Mar 2020 19:38:52 +0100
Subject: [PATCH 025/179] support 3 optimization profiles in Makefile

---
 Makefile       | 17 ++++++++++++++---
 make/source.mk | 11 ++++++-----
 2 files changed, 20 insertions(+), 8 deletions(-)

diff --git a/Makefile b/Makefile
index 700e26125b..6446ef3b1e 100644
--- a/Makefile
+++ b/Makefile
@@ -305,13 +305,24 @@ define compile_file
 	$(CROSS_CC) -c -o $@ $(CFLAGS) $(2) $<
 endef
 
+ifneq ($(TARGET),$(filter $(TARGET),$(F3_TARGETS)))
+
+else
+
+endif
+
 # Compile
 $(TARGET_OBJ_DIR)/%.o: %.c
 	$(V1) mkdir -p $(dir $@)
-	$(V1) $(if $(findstring $<,$(NORMAL_OPTIMISED_SRC)), \
-		$(call compile_file,normal optimised,-O2) \
+
+	$(V1) $(if $(findstring $<,$(SIZE_OPTIMISED_SRC)), \
+		$(call compile_file,(size),$(CC_NO_OPTIMISATION)) \
 	, \
-		$(call compile_file,size optimised,-Os) \
+		$(if $(findstring $(subst ./src/main/,,$<),$(SPEED_OPTIMISED_SRC)), \
+			$(call compile_file,(speed)),$(CC_SPEED_OPTIMISATION)) \
+		, \
+			$(call compile_file,,$(CC_SIZE_OPTIMISATION)) \
+		) \
 	)
 ifeq ($(GENERATE_ASM), 1)
 	$(V1) $(CROSS_CC) -S -fverbose-asm -Wa,-aslh -o $(patsubst %.o,%.txt.S,$@) -g $(ASM_CFLAGS) $<
diff --git a/make/source.mk b/make/source.mk
index 83030af968..cf8f36df61 100644
--- a/make/source.mk
+++ b/make/source.mk
@@ -268,7 +268,6 @@ TARGET_SRC += $(DSP_LIB)/Source/TransformFunctions/arm_cfft_f32.c
 TARGET_SRC += $(DSP_LIB)/Source/TransformFunctions/arm_rfft_fast_init_f32.c
 TARGET_SRC += $(DSP_LIB)/Source/TransformFunctions/arm_cfft_radix8_f32.c
 TARGET_SRC += $(DSP_LIB)/Source/CommonTables/arm_common_tables.c
-
 TARGET_SRC += $(DSP_LIB)/Source/ComplexMathFunctions/arm_cmplx_mag_f32.c
 TARGET_SRC += $(DSP_LIB)/Source/StatisticsFunctions/arm_max_f32.c
 
@@ -278,11 +277,13 @@ endif
 # Search path and source files for the ST stdperiph library
 VPATH        := $(VPATH):$(STDPERIPH_DIR)/src
 
-NORMAL_OPTIMISED_SRC := ""
+SIZE_OPTIMISED_SRC := ""
+SPEED_OPTIMISED_SRC := ""
 ifneq ($(TARGET),$(filter $(TARGET),$(F3_TARGETS)))
-# NORMAL_OPTIMISED_SRC := $(NORMAL_OPTIMISED_SRC) \
+# SIZE_OPTIMISED_SRC := $(SIZE_OPTIMISED_SRC) \
+#             ./src/main/common/filter.c \
+
+# SPEED_OPTIMISED_SRC := $(SPEED_OPTIMISED_SRC) \
 #             ./src/main/common/filter.c \
-#             ./src/main/sensors/gyro.c \
-#             ./src/main/common/maths.c \
 
 endif #!F3
\ No newline at end of file

From 0da14f5ad58fc685a25df1d88e7b4b2d1ebe18dd Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Tue, 10 Mar 2020 20:06:26 +0100
Subject: [PATCH 026/179] Allow for different optimization profiles depending
 on file and target in Makefile

---
 Makefile                      | 18 +++++++++++-------
 make/source.mk                |  4 ++--
 src/main/target/common_post.h |  4 ++--
 3 files changed, 15 insertions(+), 11 deletions(-)

diff --git a/Makefile b/Makefile
index 6446ef3b1e..a81de2402c 100644
--- a/Makefile
+++ b/Makefile
@@ -301,14 +301,18 @@ $(TARGET_ELF): $(TARGET_OBJS)
 	$(V0) $(SIZE) $(TARGET_ELF)
 
 define compile_file
-	echo "%% ($(1)) $<" "$(STDOUT)" && \
+	echo "%% $(1) $<" "$(STDOUT)" && \
 	$(CROSS_CC) -c -o $@ $(CFLAGS) $(2) $<
 endef
 
 ifneq ($(TARGET),$(filter $(TARGET),$(F3_TARGETS)))
-
+	OPTIMIZE_FLAG_SPEED = -Ofast
+	OPTIMIZE_FLAG_SIZE = -Os
+	OPTIMIZE_FLAG_NORMAL = -O2
 else
-
+	OPTIMIZE_FLAG_SPEED = -Os
+	OPTIMIZE_FLAG_SIZE = -Os
+	OPTIMIZE_FLAG_NORMAL = -Os
 endif
 
 # Compile
@@ -316,12 +320,12 @@ $(TARGET_OBJ_DIR)/%.o: %.c
 	$(V1) mkdir -p $(dir $@)
 
 	$(V1) $(if $(findstring $<,$(SIZE_OPTIMISED_SRC)), \
-		$(call compile_file,(size),$(CC_NO_OPTIMISATION)) \
+		$(call compile_file,(size),$(OPTIMIZE_FLAG_SIZE)) \
 	, \
-		$(if $(findstring $(subst ./src/main/,,$<),$(SPEED_OPTIMISED_SRC)), \
-			$(call compile_file,(speed)),$(CC_SPEED_OPTIMISATION)) \
+		$(if $(findstring $<,$(SPEED_OPTIMISED_SRC)), \
+			$(call compile_file,(speed),$(OPTIMIZE_FLAG_SPEED)) \
 		, \
-			$(call compile_file,,$(CC_SIZE_OPTIMISATION)) \
+			$(call compile_file,,$(OPTIMIZE_FLAG_NORMAL)) \
 		) \
 	)
 ifeq ($(GENERATE_ASM), 1)
diff --git a/make/source.mk b/make/source.mk
index cf8f36df61..50b10f8419 100644
--- a/make/source.mk
+++ b/make/source.mk
@@ -281,9 +281,9 @@ SIZE_OPTIMISED_SRC := ""
 SPEED_OPTIMISED_SRC := ""
 ifneq ($(TARGET),$(filter $(TARGET),$(F3_TARGETS)))
 # SIZE_OPTIMISED_SRC := $(SIZE_OPTIMISED_SRC) \
-#             ./src/main/common/filter.c \
+            # ./src/main/common/filter.c \
 
 # SPEED_OPTIMISED_SRC := $(SPEED_OPTIMISED_SRC) \
-#             ./src/main/common/filter.c \
+            # ./src/main/common/maths.c \
 
 endif #!F3
\ No newline at end of file
diff --git a/src/main/target/common_post.h b/src/main/target/common_post.h
index 5b4e8c7563..32b6e3531a 100644
--- a/src/main/target/common_post.h
+++ b/src/main/target/common_post.h
@@ -65,14 +65,14 @@
 #ifndef STM32F3
 #define FUNCTION_COMPILE_FOR_SIZE __attribute__((optimize("-Os")))
 #define FUNCTION_COMPILE_NORMAL __attribute__((optimize("-O2")))
-#define FUNCTION_COMPILE_FOR_SIZE __attribute__((optimize("-Ofast")))
+#define FUNCTION_COMPILE_FOR_SPEED __attribute__((optimize("-Ofast")))
 #define FILE_COMPILE_FOR_SIZE _Pragma("GCC optimize(\"Os\")")
 #define FILE_COMPILE_NORMAL _Pragma("GCC optimize(\"O2\")")
 #define FILE_COMPILE_FOR_SPEED _Pragma("GCC optimize(\"Ofast\")")
 #else
 #define FUNCTION_COMPILE_FOR_SIZE
 #define FUNCTION_COMPILE_NORMAL
-#define FUNCTION_COMPILE_FOR_SIZE
+#define FUNCTION_COMPILE_FOR_SPEED
 #define FILE_COMPILE_FOR_SIZE
 #define FILE_COMPILE_NORMAL
 #define FILE_COMPILE_FOR_SPEED

From a6bdbe6f2efe91d7f84b9c8c7436c6084324861b Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Tue, 10 Mar 2020 20:25:50 +0100
Subject: [PATCH 027/179] Fix ftoa warning

---
 src/main/common/typeconversion.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/common/typeconversion.c b/src/main/common/typeconversion.c
index 031aed6c77..add37041de 100644
--- a/src/main/common/typeconversion.c
+++ b/src/main/common/typeconversion.c
@@ -185,7 +185,7 @@ char *ftoa(float x, char *floatString)
 
     dpLocation = strlen(intString2) - 3;
 
-    strncpy(floatString, intString2, dpLocation);
+    memcpy(floatString, intString2, dpLocation);
     floatString[dpLocation] = '\0';
     strcat(floatString, decimalPoint);
     strcat(floatString, intString2 + dpLocation);

From 200d61eed0714eef32725502d7e2af0a5d39e0ae Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Tue, 10 Mar 2020 20:31:49 +0100
Subject: [PATCH 028/179] Drop minor functionality do fix warning

---
 src/main/io/dashboard.c | 20 --------------------
 src/main/io/dashboard.h |  4 +---
 2 files changed, 1 insertion(+), 23 deletions(-)

diff --git a/src/main/io/dashboard.c b/src/main/io/dashboard.c
index 619fb552d7..f14673f0df 100644
--- a/src/main/io/dashboard.c
+++ b/src/main/io/dashboard.c
@@ -418,14 +418,6 @@ static void showStatusPage(void)
         i2c_OLED_send_string(lineBuffer);
     }
 #endif
-
-    rowIndex++;
-    char rollTrim[7], pitchTrim[7];
-    formatTrimDegrees(rollTrim, boardAlignment()->rollDeciDegrees);
-    formatTrimDegrees(pitchTrim, boardAlignment()->pitchDeciDegrees);
-    tfp_sprintf(lineBuffer, "Acc: %sR, %sP", rollTrim, pitchTrim );
-    i2c_OLED_set_line(rowIndex++);
-    i2c_OLED_send_string(lineBuffer);
 }
 
 void dashboardUpdate(timeUs_t currentTimeUs)
@@ -551,16 +543,4 @@ void dashboardSetNextPageChangeAt(timeUs_t futureMicros)
     nextPageAt = futureMicros;
 }
 
-void formatTrimDegrees ( char *formattedTrim, int16_t trimValue ) {
-    char trim[6];
-    tfp_sprintf(trim, "%d", trimValue);
-    int x = strlen(trim)-1;
-    strncpy(formattedTrim,trim,x);
-    formattedTrim[x] = '\0';
-    if (trimValue !=0) {
-        strcat(formattedTrim,".");
-    }
-    strcat(formattedTrim,trim+x);
-}
-
 #endif
diff --git a/src/main/io/dashboard.h b/src/main/io/dashboard.h
index 91c3418395..06186e7eed 100644
--- a/src/main/io/dashboard.h
+++ b/src/main/io/dashboard.h
@@ -25,6 +25,4 @@ void dashboardInit(void);
 void dashboardUpdate(timeUs_t currentTimeUs);
 
 void dashboardSetPage(pageId_e newPageId);
-void dashboardSetNextPageChangeAt(timeUs_t futureMicros);
-
-void formatTrimDegrees ( char formattedTrim[7], int16_t trimValue );
+void dashboardSetNextPageChangeAt(timeUs_t futureMicros);
\ No newline at end of file

From c897f6bf476a30936eb7c2f8672dd88e03f9d87f Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Tue, 10 Mar 2020 20:39:49 +0100
Subject: [PATCH 029/179] Bump osdConfig PG version

---
 src/main/io/osd.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index e608c526ee..bd348c4823 100755
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -196,7 +196,7 @@ static bool osdDisplayHasCanvas;
 #define AH_SIDEBAR_WIDTH_POS 7
 #define AH_SIDEBAR_HEIGHT_POS 3
 
-PG_REGISTER_WITH_RESET_FN(osdConfig_t, osdConfig, PG_OSD_CONFIG, 9);
+PG_REGISTER_WITH_RESET_FN(osdConfig_t, osdConfig, PG_OSD_CONFIG, 10);
 
 static int digitCount(int32_t value)
 {

From 3b1e70ced2657293ac18d4fba540c4357cb9e7c6 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Tue, 10 Mar 2020 20:48:39 +0100
Subject: [PATCH 030/179] remove obsolete code from gyroanalyse.c

---
 src/main/flight/gyroanalyse.h | 17 -----------------
 1 file changed, 17 deletions(-)

diff --git a/src/main/flight/gyroanalyse.h b/src/main/flight/gyroanalyse.h
index 0b4e060a4d..b6de10e40a 100644
--- a/src/main/flight/gyroanalyse.h
+++ b/src/main/flight/gyroanalyse.h
@@ -51,23 +51,6 @@ typedef struct gyroAnalyseState_s {
     biquadFilter_t detectedFrequencyFilter[XYZ_AXIS_COUNT];
     uint16_t centerFreq[XYZ_AXIS_COUNT];
     uint16_t prevCenterFreq[XYZ_AXIS_COUNT];
-
-    /*
-     * Extended Dynamic Filters are 3x3 filter matrix
-     * In this approach, we assume that vibration peak on one axis
-     * can be also detected on other axises, but with lower amplitude
-     * that causes this freqency not to be attenuated.
-     * 
-     * This approach is similiar to the approach on RPM filter when motor base
-     * frequency is attenuated on every axis even tho it might not be appearing
-     * in gyro traces
-     * 
-     * extendedDynamicFilter[GYRO_AXIS][ANALYZED_AXIS]
-     * 
-     */
-    biquadFilter_t extendedDynamicFilter[XYZ_AXIS_COUNT][XYZ_AXIS_COUNT];
-    filterApplyFnPtr extendedDynamicFilterApplyFn;
-
     bool filterUpdateExecute;
     uint8_t filterUpdateAxis;
     uint16_t filterUpdateFrequency;

From 9a723680b7b64a8ae8d62a34dce4bad05e57ff0b Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Wed, 11 Mar 2020 13:18:33 +0100
Subject: [PATCH 031/179] Fix compilation on Mac

---
 Makefile | 27 +++++++++++++--------------
 1 file changed, 13 insertions(+), 14 deletions(-)

diff --git a/Makefile b/Makefile
index a81de2402c..5a6da86388 100644
--- a/Makefile
+++ b/Makefile
@@ -175,8 +175,7 @@ SIZE        = $(ARM_SDK_PREFIX)size
 #
 
 ifeq ($(DEBUG),GDB)
-OPTIMIZE    = -O0
-LTO_FLAGS   = $(OPTIMIZE)
+LTO_FLAGS   = 
 else
 LTO_FLAGS   = -flto -fuse-linker-plugin
 endif
@@ -300,21 +299,21 @@ $(TARGET_ELF): $(TARGET_OBJS)
 	$(V1) $(CROSS_CC) -o $@ $(filter %.o, $^) $(LDFLAGS)
 	$(V0) $(SIZE) $(TARGET_ELF)
 
+OPTIMIZE_FLAG_SPEED := "-Os"
+OPTIMIZE_FLAG_SIZE := "-Os"
+OPTIMIZE_FLAG_NORMAL := "-Os"
+
+ifneq ($(TARGET_MCU_GROUP), STM32F3)
+	OPTIMIZE_FLAG_SPEED := "-Ofast"
+	OPTIMIZE_FLAG_SIZE := "-Os"
+	OPTIMIZE_FLAG_NORMAL := "-O2"
+endif
+
 define compile_file
-	echo "%% $(1) $<" "$(STDOUT)" && \
+	echo "%% $(1) $(2) $<" "$(STDOUT)" && \
 	$(CROSS_CC) -c -o $@ $(CFLAGS) $(2) $<
 endef
 
-ifneq ($(TARGET),$(filter $(TARGET),$(F3_TARGETS)))
-	OPTIMIZE_FLAG_SPEED = -Ofast
-	OPTIMIZE_FLAG_SIZE = -Os
-	OPTIMIZE_FLAG_NORMAL = -O2
-else
-	OPTIMIZE_FLAG_SPEED = -Os
-	OPTIMIZE_FLAG_SIZE = -Os
-	OPTIMIZE_FLAG_NORMAL = -Os
-endif
-
 # Compile
 $(TARGET_OBJ_DIR)/%.o: %.c
 	$(V1) mkdir -p $(dir $@)
@@ -325,7 +324,7 @@ $(TARGET_OBJ_DIR)/%.o: %.c
 		$(if $(findstring $<,$(SPEED_OPTIMISED_SRC)), \
 			$(call compile_file,(speed),$(OPTIMIZE_FLAG_SPEED)) \
 		, \
-			$(call compile_file,,$(OPTIMIZE_FLAG_NORMAL)) \
+			$(call compile_file,(normal),$(OPTIMIZE_FLAG_NORMAL)) \
 		) \
 	)
 ifeq ($(GENERATE_ASM), 1)

From 0d0929aaa1fa90f9b5e0147a5ffbf48b1e75874b Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Wed, 11 Mar 2020 20:27:05 +0100
Subject: [PATCH 032/179] remove rpm filter from fast_code

---
 src/main/common/global_functions.c |  2 +-
 src/main/fc/fc_init.c              |  4 ++--
 src/main/flight/mixer.c            |  6 +++---
 src/main/flight/rpm_filter.c       | 10 ++++++----
 src/main/navigation/navigation.c   |  2 +-
 src/main/scheduler/scheduler.c     |  2 +-
 6 files changed, 14 insertions(+), 12 deletions(-)

diff --git a/src/main/common/global_functions.c b/src/main/common/global_functions.c
index 7421dab33c..b4033479e9 100644
--- a/src/main/common/global_functions.c
+++ b/src/main/common/global_functions.c
@@ -142,7 +142,7 @@ float NOINLINE getThrottleScale(float globalThrottleScale) {
     }
 }
 
-int16_t FAST_CODE getRcCommandOverride(int16_t command[], uint8_t axis) {
+int16_t getRcCommandOverride(int16_t command[], uint8_t axis) {
     int16_t outputValue = command[axis];
 
     if (GLOBAL_FUNCTION_FLAG(GLOBAL_FUNCTION_FLAG_OVERRIDE_SWAP_ROLL_YAW) && axis == FD_ROLL) {
diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c
index 990c02aa3d..2cee3e2d00 100644
--- a/src/main/fc/fc_init.c
+++ b/src/main/fc/fc_init.c
@@ -655,10 +655,10 @@ void init(void)
 
 #ifdef USE_RPM_FILTER
     disableRpmFilters();
-    if (STATE(ESC_SENSOR_ENABLED) && (rpmFilterConfig()->gyro_filter_enabled || rpmFilterConfig()->dterm_filter_enabled)) {
+    // if (STATE(ESC_SENSOR_ENABLED) && (rpmFilterConfig()->gyro_filter_enabled || rpmFilterConfig()->dterm_filter_enabled)) {
         rpmFiltersInit();
         setTaskEnabled(TASK_RPM_FILTER, true);
-    }
+    // }
 #endif
 
     systemState |= SYSTEM_STATE_READY;
diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c
index 09f7b32e8f..30d4f1f876 100755
--- a/src/main/flight/mixer.c
+++ b/src/main/flight/mixer.c
@@ -290,7 +290,7 @@ void mixerResetDisarmedMotors(void)
     }
 }
 
-static FAST_CODE NOINLINE uint16_t handleOutputScaling(
+static uint16_t handleOutputScaling(
     int16_t input,          // Input value from the mixer
     int16_t stopThreshold,  // Threshold value to check if motor should be rotating or not
     int16_t onStopValue,    // Value sent to the ESC when min rotation is required - on motor_stop it is STOP command, without motor_stop it's a value that keeps rotation
@@ -318,7 +318,7 @@ static FAST_CODE NOINLINE uint16_t handleOutputScaling(
     return value;
 }
 
-void FAST_CODE NOINLINE writeMotors(void)
+void FAST_CODE writeMotors(void)
 {
     for (int i = 0; i < motorCount; i++) {
         uint16_t motorValue;
@@ -438,7 +438,7 @@ static int getReversibleMotorsThrottleDeadband(void)
     return feature(FEATURE_MOTOR_STOP) ? reversibleMotorsConfig()->neutral : directionValue;
 }
 
-void FAST_CODE NOINLINE mixTable(const float dT)
+void FAST_CODE mixTable(const float dT)
 {
     int16_t input[3];   // RPY, range [-500:+500]
     // Allow direct stick input to motors in passthrough mode on airplanes
diff --git a/src/main/flight/rpm_filter.c b/src/main/flight/rpm_filter.c
index 7d861de498..e2b2c9849d 100644
--- a/src/main/flight/rpm_filter.c
+++ b/src/main/flight/rpm_filter.c
@@ -22,6 +22,8 @@
  * along with this program. If not, see http://www.gnu.org/licenses/.
  */
 
+#include "platform.h"
+
 #include "flight/rpm_filter.h"
 
 #include "config/parameter_group.h"
@@ -82,7 +84,7 @@ void nullRpmFilterUpdate(rpmFilterBank_t *filterBank, uint8_t motor, float baseF
     UNUSED(baseFrequency);
 }
 
-float FAST_CODE rpmFilterApply(rpmFilterBank_t *filterBank, uint8_t axis, float input)
+float rpmFilterApply(rpmFilterBank_t *filterBank, uint8_t axis, float input)
 {
     float output = input;
 
@@ -136,7 +138,7 @@ void disableRpmFilters(void) {
     rpmGyroApplyFn = (rpmFilterApplyFnPtr)nullRpmFilterApply;
 }
 
-void FAST_CODE NOINLINE rpmFilterUpdate(rpmFilterBank_t *filterBank, uint8_t motor, float baseFrequency)
+void rpmFilterUpdate(rpmFilterBank_t *filterBank, uint8_t motor, float baseFrequency)
 {
     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++)
     {
@@ -177,7 +179,7 @@ void rpmFiltersInit(void)
     }
 }
 
-void FAST_CODE NOINLINE rpmFilterUpdateTask(timeUs_t currentTimeUs)
+void rpmFilterUpdateTask(timeUs_t currentTimeUs)
 {
     UNUSED(currentTimeUs);
 
@@ -198,7 +200,7 @@ void FAST_CODE NOINLINE rpmFilterUpdateTask(timeUs_t currentTimeUs)
     }
 }
 
-float FAST_CODE rpmFilterGyroApply(uint8_t axis, float input)
+float rpmFilterGyroApply(uint8_t axis, float input)
 {
     return rpmGyroApplyFn(&gyroRpmFilters, axis, input);
 }
diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index cc9a3e7593..413c3f49a7 100755
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -3433,7 +3433,7 @@ bool navigationRTHAllowsLanding(void)
         (allow == NAV_RTH_ALLOW_LANDING_FS_ONLY && FLIGHT_MODE(FAILSAFE_MODE));
 }
 
-bool FAST_CODE isNavLaunchEnabled(void)
+bool isNavLaunchEnabled(void)
 {
     return IS_RC_MODE_ACTIVE(BOXNAVLAUNCH) || feature(FEATURE_FW_LAUNCH);
 }
diff --git a/src/main/scheduler/scheduler.c b/src/main/scheduler/scheduler.c
index ee83dd5fa0..e9c00345ea 100755
--- a/src/main/scheduler/scheduler.c
+++ b/src/main/scheduler/scheduler.c
@@ -178,7 +178,7 @@ void setTaskEnabled(cfTaskId_e taskId, bool enabled)
     }
 }
 
-timeDelta_t FAST_CODE NOINLINE getTaskDeltaTime(cfTaskId_e taskId)
+timeDelta_t getTaskDeltaTime(cfTaskId_e taskId)
 {
     if (taskId == TASK_SELF) {
         return currentTask->taskLatestDeltaTime;

From 0a443a958c41fc78dd7036d04936b85ad3b2aad9 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Wed, 11 Mar 2020 20:28:27 +0100
Subject: [PATCH 033/179] Fix always enabled RPM filter

---
 src/main/fc/fc_init.c | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c
index 2cee3e2d00..990c02aa3d 100644
--- a/src/main/fc/fc_init.c
+++ b/src/main/fc/fc_init.c
@@ -655,10 +655,10 @@ void init(void)
 
 #ifdef USE_RPM_FILTER
     disableRpmFilters();
-    // if (STATE(ESC_SENSOR_ENABLED) && (rpmFilterConfig()->gyro_filter_enabled || rpmFilterConfig()->dterm_filter_enabled)) {
+    if (STATE(ESC_SENSOR_ENABLED) && (rpmFilterConfig()->gyro_filter_enabled || rpmFilterConfig()->dterm_filter_enabled)) {
         rpmFiltersInit();
         setTaskEnabled(TASK_RPM_FILTER, true);
-    // }
+    }
 #endif
 
     systemState |= SYSTEM_STATE_READY;

From f26c189233933eb28a6cf35d03e12c7cfe60cc70 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Wed, 11 Mar 2020 20:50:24 +0100
Subject: [PATCH 034/179] gyro, imu and other

---
 src/main/sensors/boardalignment.c | 2 +-
 src/main/sensors/esc_sensor.c     | 2 +-
 src/main/sensors/gyro.c           | 4 ++--
 3 files changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/main/sensors/boardalignment.c b/src/main/sensors/boardalignment.c
index b23edb7af7..ffe4210f5c 100644
--- a/src/main/sensors/boardalignment.c
+++ b/src/main/sensors/boardalignment.c
@@ -85,7 +85,7 @@ void updateBoardAlignment(int16_t roll, int16_t pitch)
     initBoardAlignment();
 }
 
-void FAST_CODE applyBoardAlignment(int32_t *vec)
+void applyBoardAlignment(int32_t *vec)
 {
     if (standardBoardAlignment) {
         return;
diff --git a/src/main/sensors/esc_sensor.c b/src/main/sensors/esc_sensor.c
index 618db67221..8a076e643f 100644
--- a/src/main/sensors/esc_sensor.c
+++ b/src/main/sensors/esc_sensor.c
@@ -130,7 +130,7 @@ static bool escSensorDecodeFrame(void)
     return ESC_SENSOR_FRAME_PENDING;
 }
 
-uint32_t FAST_CODE computeRpm(int16_t erpm) {
+uint32_t computeRpm(int16_t erpm) {
     return lrintf((float)erpm * ERPM_PER_LSB / (motorConfig()->motorPoleCount / 2));
 }
 
diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index 27934c549c..827d7cb33c 100644
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@@ -357,7 +357,7 @@ void gyroStartCalibration(void)
     zeroCalibrationStartV(&gyroCalibration, CALIBRATING_GYRO_TIME_MS, gyroConfig()->gyroMovementCalibrationThreshold, false);
 }
 
-bool FAST_CODE NOINLINE gyroIsCalibrationComplete(void)
+bool gyroIsCalibrationComplete(void)
 {
     return zeroCalibrationIsCompleteV(&gyroCalibration) && zeroCalibrationIsSuccessfulV(&gyroCalibration);
 }
@@ -400,7 +400,7 @@ void gyroGetMeasuredRotationRate(fpVector3_t *measuredRotationRate)
     }
 }
 
-void FAST_CODE NOINLINE gyroUpdate()
+void gyroUpdate()
 {
     // range: +/- 8192; +/- 2000 deg/sec
     if (gyroDev0.readFn(&gyroDev0)) {

From 9a3e65eb6999a7ec39b6d7716a7539a7d476d7fb Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Niccol=C3=B2=20Maggioni?= <nicco.maggioni@gmail.com>
Date: Wed, 11 Mar 2020 21:02:27 +0100
Subject: [PATCH 035/179] Sync Dockerfile with 02382d4

---
 Dockerfile | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/Dockerfile b/Dockerfile
index de2b3aa084..c699617788 100644
--- a/Dockerfile
+++ b/Dockerfile
@@ -4,9 +4,9 @@ FROM ubuntu:bionic
 VOLUME /home/src/
 WORKDIR /home/src/
 ARG TOOLCHAIN_VERSION_SHORT
-ENV TOOLCHAIN_VERSION_SHORT ${TOOLCHAIN_VERSION_SHORT:-"8-2018q4"}
+ENV TOOLCHAIN_VERSION_SHORT ${TOOLCHAIN_VERSION_SHORT:-"9-2019q4"}
 ARG TOOLCHAIN_VERSION_LONG
-ENV TOOLCHAIN_VERSION_LONG ${TOOLCHAIN_VERSION_LONG:-"8-2018-q4-major"}
+ENV TOOLCHAIN_VERSION_LONG ${TOOLCHAIN_VERSION_LONG:-"9-2019-q4-major"}
 
 # Essentials
 RUN mkdir -p /home/src && \
@@ -14,10 +14,10 @@ RUN mkdir -p /home/src && \
     apt-get install -y software-properties-common ruby make git gcc wget curl bzip2
 
 # Toolchain
-RUN wget -P /tmp "https://developer.arm.com/-/media/Files/downloads/gnu-rm/$TOOLCHAIN_VERSION_SHORT/gcc-arm-none-eabi-$TOOLCHAIN_VERSION_LONG-linux.tar.bz2"
+RUN wget -P /tmp "https://developer.arm.com/-/media/Files/downloads/gnu-rm/$TOOLCHAIN_VERSION_SHORT/gcc-arm-none-eabi-$TOOLCHAIN_VERSION_LONG-x86_64-linux.tar.bz2"
 RUN mkdir -p /opt && \
 	cd /opt && \
-    tar xvjf "/tmp/gcc-arm-none-eabi-$TOOLCHAIN_VERSION_LONG-linux.tar.bz2" -C /opt && \
+    tar xvjf "/tmp/gcc-arm-none-eabi-$TOOLCHAIN_VERSION_LONG-x86_64-linux.tar.bz2" -C /opt && \
 	chmod -R -w "/opt/gcc-arm-none-eabi-$TOOLCHAIN_VERSION_LONG"
 
 ENV PATH="/opt/gcc-arm-none-eabi-$TOOLCHAIN_VERSION_LONG/bin:$PATH"

From fce005f7f988b43ff8c74908a40483607130bbda Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Wed, 11 Mar 2020 21:48:57 +0100
Subject: [PATCH 036/179] Switch compiler to _Ofast for critical files

---
 src/main/common/encoding.c        | 4 ++++
 src/main/common/filter.c          | 2 ++
 src/main/common/maths.c           | 2 ++
 src/main/common/typeconversion.c  | 2 ++
 src/main/drivers/max7456.c        | 2 ++
 src/main/drivers/pwm_output.c     | 2 ++
 src/main/fc/fc_core.c             | 2 ++
 src/main/flight/gyroanalyse.c     | 1 +
 src/main/flight/imu.c             | 4 +++-
 src/main/flight/mixer.c           | 2 ++
 src/main/flight/pid.c             | 2 ++
 src/main/io/osd.c                 | 2 ++
 src/main/rx/crsf.c                | 2 +-
 src/main/rx/fport.c               | 1 +
 src/main/rx/sbus.c                | 1 +
 src/main/scheduler/scheduler.c    | 2 ++
 src/main/sensors/acceleration.c   | 3 +++
 src/main/sensors/boardalignment.c | 2 ++
 src/main/sensors/gyro.c           | 2 ++
 src/main/telemetry/crsf.c         | 1 +
 src/main/telemetry/smartport.c    | 1 +
 21 files changed, 40 insertions(+), 2 deletions(-)

diff --git a/src/main/common/encoding.c b/src/main/common/encoding.c
index 136f628031..32d9eb5205 100644
--- a/src/main/common/encoding.c
+++ b/src/main/common/encoding.c
@@ -15,6 +15,10 @@
  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+#include "platform.h"
+
+FILE_COMPILE_FOR_SPEED
+
 #include "encoding.h"
 
 /**
diff --git a/src/main/common/filter.c b/src/main/common/filter.c
index 3226e52b3a..a436534256 100644
--- a/src/main/common/filter.c
+++ b/src/main/common/filter.c
@@ -22,6 +22,8 @@
 
 #include "platform.h"
 
+FILE_COMPILE_FOR_SPEED
+
 #include "common/filter.h"
 #include "common/maths.h"
 #include "common/utils.h"
diff --git a/src/main/common/maths.c b/src/main/common/maths.c
index e96ac7c1e6..0a148701e5 100644
--- a/src/main/common/maths.c
+++ b/src/main/common/maths.c
@@ -25,6 +25,8 @@
 #include "quaternion.h"
 #include "platform.h"
 
+FILE_COMPILE_FOR_SPEED
+
 // http://lolengine.net/blog/2011/12/21/better-function-approximations
 // Chebyshev http://stackoverflow.com/questions/345085/how-do-trigonometric-functions-work/345117#345117
 // Thanks for ledvinap for making such accuracy possible! See: https://github.com/cleanflight/cleanflight/issues/940#issuecomment-110323384
diff --git a/src/main/common/typeconversion.c b/src/main/common/typeconversion.c
index add37041de..b07f41f8a0 100644
--- a/src/main/common/typeconversion.c
+++ b/src/main/common/typeconversion.c
@@ -20,6 +20,8 @@
 #include "build/build_config.h"
 
 #include "maths.h"
+#include "platform.h"
+FILE_COMPILE_FOR_SPEED
 
 #ifdef REQUIRE_PRINTF_LONG_SUPPORT
 
diff --git a/src/main/drivers/max7456.c b/src/main/drivers/max7456.c
index 405db287c1..964166b14b 100644
--- a/src/main/drivers/max7456.c
+++ b/src/main/drivers/max7456.c
@@ -22,6 +22,8 @@
 
 #include "platform.h"
 
+FILE_COMPILE_FOR_SPEED
+
 #ifdef USE_MAX7456
 
 #include "common/bitarray.h"
diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c
index 15a68faa19..8d048c87a5 100644
--- a/src/main/drivers/pwm_output.c
+++ b/src/main/drivers/pwm_output.c
@@ -22,6 +22,8 @@
 
 #include "platform.h"
 
+FILE_COMPILE_FOR_SPEED
+
 #include "build/debug.h"
 
 #include "common/log.h"
diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c
index 64329dbe17..4528397b42 100755
--- a/src/main/fc/fc_core.c
+++ b/src/main/fc/fc_core.c
@@ -21,6 +21,8 @@
 
 #include "platform.h"
 
+FILE_COMPILE_FOR_SPEED
+
 #include "blackbox/blackbox.h"
 
 #include "build/debug.h"
diff --git a/src/main/flight/gyroanalyse.c b/src/main/flight/gyroanalyse.c
index 64da4305f1..b85d538a15 100644
--- a/src/main/flight/gyroanalyse.c
+++ b/src/main/flight/gyroanalyse.c
@@ -26,6 +26,7 @@
 #include <stdint.h>
 
 #include "platform.h"
+FILE_COMPILE_FOR_SPEED
 
 #ifdef USE_DYNAMIC_FILTERS
 
diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c
index ff9f0f9063..375a128f89 100644
--- a/src/main/flight/imu.c
+++ b/src/main/flight/imu.c
@@ -23,6 +23,8 @@
 
 #include "platform.h"
 
+FILE_COMPILE_FOR_SPEED
+
 #include "blackbox/blackbox.h"
 
 #include "build/build_config.h"
@@ -631,7 +633,7 @@ void imuCheckVibrationLevels(void)
     // DEBUG_VIBE values 4-7 are used by NAV estimator
 }
 
-void FAST_CODE NOINLINE imuUpdateAttitude(timeUs_t currentTimeUs)
+void imuUpdateAttitude(timeUs_t currentTimeUs)
 {
     /* Calculate dT */
     static timeUs_t previousIMUUpdateTimeUs;
diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c
index 30d4f1f876..c0a95ae0d8 100755
--- a/src/main/flight/mixer.c
+++ b/src/main/flight/mixer.c
@@ -21,6 +21,8 @@
 
 #include "platform.h"
 
+FILE_COMPILE_FOR_SPEED
+
 #include "build/debug.h"
 
 #include "common/axis.h"
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index eced2d6a4c..740fd28a77 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -21,6 +21,8 @@
 
 #include <platform.h>
 
+FILE_COMPILE_FOR_SPEED
+
 #include "build/build_config.h"
 #include "build/debug.h"
 
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index bd348c4823..df93a1d59f 100755
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -31,6 +31,8 @@
 
 #include "platform.h"
 
+FILE_COMPILE_FOR_SPEED
+
 #ifdef USE_OSD
 
 #include "build/debug.h"
diff --git a/src/main/rx/crsf.c b/src/main/rx/crsf.c
index 276bb956b9..6a0ad8d3f5 100755
--- a/src/main/rx/crsf.c
+++ b/src/main/rx/crsf.c
@@ -21,7 +21,7 @@
 #include <string.h>
 
 #include "platform.h"
-
+FILE_COMPILE_FOR_SPEED
 #ifdef USE_SERIALRX_CRSF
 
 #include "build/build_config.h"
diff --git a/src/main/rx/fport.c b/src/main/rx/fport.c
index e544e434e7..b9a8144b31 100644
--- a/src/main/rx/fport.c
+++ b/src/main/rx/fport.c
@@ -21,6 +21,7 @@
 #include <string.h>
 
 #include "platform.h"
+FILE_COMPILE_FOR_SPEED
 
 #if defined(USE_SERIAL_RX)
 
diff --git a/src/main/rx/sbus.c b/src/main/rx/sbus.c
index 04bc855697..16a76a8fa0 100644
--- a/src/main/rx/sbus.c
+++ b/src/main/rx/sbus.c
@@ -21,6 +21,7 @@
 #include <string.h>
 
 #include "platform.h"
+FILE_COMPILE_FOR_SPEED
 
 #ifdef USE_SERIAL_RX
 
diff --git a/src/main/scheduler/scheduler.c b/src/main/scheduler/scheduler.c
index e9c00345ea..f3de93c7b7 100755
--- a/src/main/scheduler/scheduler.c
+++ b/src/main/scheduler/scheduler.c
@@ -21,6 +21,8 @@
 
 #include "platform.h"
 
+FILE_COMPILE_FOR_SPEED
+
 #include "scheduler.h"
 
 #include "build/build_config.h"
diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c
index 5a1cdd8b6d..07ae4c067b 100644
--- a/src/main/sensors/acceleration.c
+++ b/src/main/sensors/acceleration.c
@@ -21,6 +21,9 @@
 #include <math.h>
 
 #include "platform.h"
+
+FILE_COMPILE_FOR_SPEED
+
 #include "build/debug.h"
 
 #include "common/axis.h"
diff --git a/src/main/sensors/boardalignment.c b/src/main/sensors/boardalignment.c
index ffe4210f5c..205f7c8df3 100644
--- a/src/main/sensors/boardalignment.c
+++ b/src/main/sensors/boardalignment.c
@@ -22,6 +22,8 @@
 
 #include "platform.h"
 
+FILE_COMPILE_FOR_SPEED
+
 #include "common/maths.h"
 #include "common/vector.h"
 #include "common/axis.h"
diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index 827d7cb33c..c51466322e 100644
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@@ -22,6 +22,8 @@
 
 #include "platform.h"
 
+FILE_COMPILE_FOR_SPEED
+
 #include "build/build_config.h"
 #include "build/debug.h"
 
diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c
index 0e8c9dc2a5..b17809bdd1 100755
--- a/src/main/telemetry/crsf.c
+++ b/src/main/telemetry/crsf.c
@@ -20,6 +20,7 @@
 #include <string.h>
 
 #include "platform.h"
+FILE_COMPILE_FOR_SPEED
 
 #if defined(USE_TELEMETRY) && defined(USE_SERIALRX_CRSF) && defined(USE_TELEMETRY_CRSF)
 
diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c
index 9ac0dce545..0d7a0bfe04 100755
--- a/src/main/telemetry/smartport.c
+++ b/src/main/telemetry/smartport.c
@@ -9,6 +9,7 @@
 #include <math.h>
 
 #include "platform.h"
+FILE_COMPILE_FOR_SPEED
 
 #if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_SMARTPORT)
 

From 683449600eaea88856397c071baed397d519a3af Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Thu, 12 Mar 2020 09:36:11 +0100
Subject: [PATCH 037/179] Fix tests

---
 src/test/unit/platform.h | 8 +++++++-
 1 file changed, 7 insertions(+), 1 deletion(-)

diff --git a/src/test/unit/platform.h b/src/test/unit/platform.h
index a900257c42..4dcf2092c7 100644
--- a/src/test/unit/platform.h
+++ b/src/test/unit/platform.h
@@ -81,4 +81,10 @@ extern SysTick_Type *SysTick;
 
 #define FAST_CODE 
 #define NOINLINE
-#define EXTENDED_FASTRAM
\ No newline at end of file
+#define EXTENDED_FASTRAM
+#define FUNCTION_COMPILE_FOR_SIZE
+#define FUNCTION_COMPILE_NORMAL
+#define FUNCTION_COMPILE_FOR_SPEED
+#define FILE_COMPILE_FOR_SIZE
+#define FILE_COMPILE_NORMAL
+#define FILE_COMPILE_FOR_SPEED
\ No newline at end of file

From 1c2e1b5657ac271e104f7df0ebddf934605f6b54 Mon Sep 17 00:00:00 2001
From: Oleg Borodin <onborodin@gmail.com>
Date: Fri, 13 Mar 2020 10:02:22 +0200
Subject: [PATCH 038/179] added FreeBSD OS identification as Linux

---
 make/system-id.mk | 7 +++++++
 1 file changed, 7 insertions(+)

diff --git a/make/system-id.mk b/make/system-id.mk
index c224b824e7..c4c2d81729 100644
--- a/make/system-id.mk
+++ b/make/system-id.mk
@@ -18,6 +18,13 @@ ifeq ($(UNAME), Linux)
   LINUX := 1
 endif
 
+# FreeBSD
+ifeq ($(UNAME), FreeBSD)
+  OSFAMILY := linux
+  LINUX := 1
+endif
+
+
 # Mac OSX
 ifeq ($(UNAME), Darwin)
   OSFAMILY := macosx

From abe8b7f94f3807c7a9fbde900caec5baacb71bad Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Mon, 16 Mar 2020 15:21:34 +0100
Subject: [PATCH 039/179] Revert "Merge pull request #5449 from
 iNavFlight/dzikuvx-ustick-to-fastram"

This reverts commit c451c176ee42829ae4f01190faf97a78516f80f2, reversing
changes made to 9cb3ada195a2a4c781f0457c67c41eea1f6950f8.
---
 src/main/drivers/system.c | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/main/drivers/system.c b/src/main/drivers/system.c
index 69bde9c41e..aa4bfcb2c9 100644
--- a/src/main/drivers/system.c
+++ b/src/main/drivers/system.c
@@ -49,9 +49,9 @@ void registerExtiCallbackHandler(IRQn_Type irqn, extiCallbackHandlerFunc *fn)
 }
 
 // cycles per microsecond
-STATIC_UNIT_TESTED EXTENDED_FASTRAM  timeUs_t usTicks = 0;
+STATIC_UNIT_TESTED  timeUs_t usTicks = 0;
 // current uptime for 1kHz systick timer. will rollover after 49 days. hopefully we won't care.
-STATIC_UNIT_TESTED EXTENDED_FASTRAM volatile timeMs_t sysTickUptime = 0;
+STATIC_UNIT_TESTED volatile timeMs_t sysTickUptime = 0;
 STATIC_UNIT_TESTED volatile uint32_t sysTickValStamp = 0;
 // cached value of RCC->CSR
 uint32_t cachedRccCsrValue;
@@ -76,7 +76,7 @@ void cycleCounterInit(void)
 
 // SysTick
 
-static EXTENDED_FASTRAM volatile int sysTickPending = 0;
+static volatile int sysTickPending = 0;
 
 void SysTick_Handler(void)
 {

From 416249a608081fcd283f4d189e51feab6baef595 Mon Sep 17 00:00:00 2001
From: "Konstantin (DigitalEntity) Sharlaimov"
 <konstantin.sharlaimov@gmail.com>
Date: Mon, 16 Mar 2020 20:20:46 +0100
Subject: [PATCH 040/179] [OSD/CMS] Avoid initializing CMS when OSD is disabled

---
 src/main/cms/cms.c    | 2 +-
 src/main/fc/fc_init.c | 7 -------
 2 files changed, 1 insertion(+), 8 deletions(-)

diff --git a/src/main/cms/cms.c b/src/main/cms/cms.c
index fcfb56ebea..ee3742b127 100644
--- a/src/main/cms/cms.c
+++ b/src/main/cms/cms.c
@@ -766,7 +766,7 @@ void cmsMenuOpen(void)
 {
     if (!cmsInMenu) {
         // New open
-	setServoOutputEnabled(false);
+        setServoOutputEnabled(false);
         pCurrentDisplay = cmsDisplayPortSelectCurrent();
         if (!pCurrentDisplay)
             return;
diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c
index 990c02aa3d..2ea7f7281b 100644
--- a/src/main/fc/fc_init.c
+++ b/src/main/fc/fc_init.c
@@ -536,13 +536,6 @@ void init(void)
     }
 #endif
 
-#if defined(USE_MSP_DISPLAYPORT) && defined(USE_CMS)
-    // If OSD is not active, then register MSP_DISPLAYPORT as a CMS device.
-    if (!osdDisplayPort) {
-        cmsDisplayPortRegister(displayPortMspInit());
-    }
-#endif
-
 #ifdef USE_UAV_INTERCONNECT
     uavInterconnectBusInit();
 #endif

From ee04f6fbb50f1d18bccd1cf4069a1153b2a81575 Mon Sep 17 00:00:00 2001
From: Stefan Naewe <snaewe@users.noreply.github.com>
Date: Thu, 19 Mar 2020 10:22:51 +0100
Subject: [PATCH 041/179] Document telemetry sensor 0x450

Signed-off-by: Stefan Naewe <stefan.naewe@gmail.com>
---
 docs/Telemetry.md | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/docs/Telemetry.md b/docs/Telemetry.md
index 72c593c7e7..8c8b406ccf 100644
--- a/docs/Telemetry.md
+++ b/docs/Telemetry.md
@@ -120,7 +120,7 @@ The following sensors are transmitted
 * **0420** : distance to GPS home fix, in meters
 * **0430** : if `frsky_pitch_roll = ON` set this will be pitch degrees*10
 * **0440** : if `frsky_pitch_roll = ON` set this will be roll degrees*10
-
+* **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.

From 7655d1871ec16a5fe1db6caa12228afed9160520 Mon Sep 17 00:00:00 2001
From: stronnag <jh+github@daria.co.uk>
Date: Fri, 20 Mar 2020 14:15:58 +0000
Subject: [PATCH 042/179] Support WP LAND (#5485)

Add LAND way point type
---
 src/main/fc/cli.c                |  4 ++--
 src/main/navigation/navigation.c | 22 ++++++++++++++++------
 src/main/navigation/navigation.h |  5 +++--
 3 files changed, 21 insertions(+), 10 deletions(-)

diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c
index a4cba8f4e7..cea006f24b 100755
--- a/src/main/fc/cli.c
+++ b/src/main/fc/cli.c
@@ -1322,7 +1322,7 @@ static void cliWaypoints(char *cmdline)
     } else if (sl_strcasecmp(cmdline, "save") == 0) {
         posControl.waypointListValid = false;
         for (int i = 0; i < NAV_MAX_WAYPOINTS; i++) {
-            if (!(posControl.waypointList[i].action == NAV_WP_ACTION_WAYPOINT || posControl.waypointList[i].action == NAV_WP_ACTION_JUMP || posControl.waypointList[i].action == NAV_WP_ACTION_RTH || posControl.waypointList[i].action == NAV_WP_ACTION_HOLD_TIME)) break;
+            if (!(posControl.waypointList[i].action == NAV_WP_ACTION_WAYPOINT || posControl.waypointList[i].action == NAV_WP_ACTION_JUMP || posControl.waypointList[i].action == NAV_WP_ACTION_RTH || posControl.waypointList[i].action == NAV_WP_ACTION_HOLD_TIME || posControl.waypointList[i].action == NAV_WP_ACTION_LAND)) break;
             if (posControl.waypointList[i].flag == NAV_WP_FLAG_LAST) {
                 posControl.waypointCount = i + 1;
                 posControl.waypointListValid = true;
@@ -1374,7 +1374,7 @@ static void cliWaypoints(char *cmdline)
             }
             if (validArgumentCount < 4) {
                 cliShowParseError();
-            } else if (!(action == 0 || action == NAV_WP_ACTION_WAYPOINT || action == NAV_WP_ACTION_RTH || action == NAV_WP_ACTION_HOLD_TIME) || (p1 < 0) || !(flag == 0 || flag == NAV_WP_FLAG_LAST)) {
+            } else if (!(action == 0 || action == NAV_WP_ACTION_WAYPOINT || action == NAV_WP_ACTION_RTH || action == NAV_WP_ACTION_HOLD_TIME || action == NAV_WP_ACTION_LAND) || (p1 < 0) || !(flag == 0 || flag == NAV_WP_FLAG_LAST)) {
                 cliShowParseError();
             } else {
                 posControl.waypointList[i].action = action;
diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index 413c3f49a7..0afe103fdd 100755
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -1396,6 +1396,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav
     switch ((navWaypointActions_e)posControl.waypointList[posControl.activeWaypointIndex].action) {
         case NAV_WP_ACTION_HOLD_TIME:
         case NAV_WP_ACTION_WAYPOINT:
+        case NAV_WP_ACTION_LAND:
             calculateAndSetActiveWaypoint(&posControl.waypointList[posControl.activeWaypointIndex]);
             posControl.wpInitialDistance = calculateDistanceToDestination(&posControl.activeWaypoint.pos);
             posControl.wpInitialAltitude = posControl.actualState.abs.pos.z;
@@ -1445,6 +1446,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na
         switch ((navWaypointActions_e)posControl.waypointList[posControl.activeWaypointIndex].action) {
             case NAV_WP_ACTION_HOLD_TIME:
             case NAV_WP_ACTION_WAYPOINT:
+            case NAV_WP_ACTION_LAND:
                 if (isWaypointReached(&posControl.activeWaypoint, false) || isWaypointMissed(&posControl.activeWaypoint)) {
                     return NAV_FSM_EVENT_SUCCESS;   // will switch to NAV_STATE_WAYPOINT_REACHED
                 }
@@ -1504,6 +1506,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_REACHED(naviga
             else {
                 return NAV_FSM_EVENT_SUCCESS;   // NAV_STATE_WAYPOINT_NEXT
             }
+
+        case NAV_WP_ACTION_LAND:
+            return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND;
+
         case NAV_WP_ACTION_HOLD_TIME:
             // Save the current time for the time the waypoint was reached
             posControl.wpReachedTime = millis();
@@ -2685,7 +2691,7 @@ void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData)
     }
     // WP #1 - #NAV_MAX_WAYPOINTS - common waypoints - pre-programmed mission
     else if ((wpNumber >= 1) && (wpNumber <= NAV_MAX_WAYPOINTS) && !ARMING_FLAG(ARMED)) {
-        if (wpData->action == NAV_WP_ACTION_WAYPOINT || wpData->action == NAV_WP_ACTION_JUMP || wpData->action == NAV_WP_ACTION_RTH || wpData->action == NAV_WP_ACTION_HOLD_TIME) {
+        if (wpData->action == NAV_WP_ACTION_WAYPOINT || wpData->action == NAV_WP_ACTION_JUMP || wpData->action == NAV_WP_ACTION_RTH || wpData->action == NAV_WP_ACTION_HOLD_TIME || wpData->action == NAV_WP_ACTION_LAND) {
             // Only allow upload next waypoint (continue upload mission) or first waypoint (new mission)
             if (wpNumber == (posControl.waypointCount + 1) || wpNumber == 1) {
                 posControl.waypointList[wpNumber - 1] = *wpData;
@@ -2827,12 +2833,13 @@ float getActiveWaypointSpeed(void)
         uint16_t waypointSpeed = navConfig()->general.max_auto_speed;
 
         if (navGetStateFlags(posControl.navState) & NAV_AUTO_WP) {
-            if (posControl.waypointCount > 0 && (posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_WAYPOINT || posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_HOLD_TIME)) {
+            if (posControl.waypointCount > 0 && (posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_WAYPOINT || posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_HOLD_TIME || posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_LAND)) {
                 float wpSpecificSpeed = 0.0f;
-                if(posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_WAYPOINT)
-                    wpSpecificSpeed = posControl.waypointList[posControl.activeWaypointIndex].p1;
+                if(posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_HOLD_TIME)
+                    wpSpecificSpeed = posControl.waypointList[posControl.activeWaypointIndex].p2; // P1 is hold time
                 else
-                    wpSpecificSpeed = posControl.waypointList[posControl.activeWaypointIndex].p2;
+                    wpSpecificSpeed = posControl.waypointList[posControl.activeWaypointIndex].p1; // default case
+
                 if (wpSpecificSpeed >= 50.0f && wpSpecificSpeed <= navConfig()->general.max_auto_speed) {
                     waypointSpeed = wpSpecificSpeed;
                 }
@@ -3313,7 +3320,7 @@ void navigationUsePIDs(void)
                                         0.0f,
                                         NAV_DTERM_CUT_HZ
     );
-    
+
     navPidInit(&posControl.pids.fw_heading, (float)pidProfile()->bank_fw.pid[PID_POS_HEADING].P / 10.0f,
                                         (float)pidProfile()->bank_fw.pid[PID_POS_HEADING].I / 10.0f,
                                         (float)pidProfile()->bank_fw.pid[PID_POS_HEADING].D / 100.0f,
@@ -3428,6 +3435,9 @@ bool navigationIsFlyingAutonomousMode(void)
 
 bool navigationRTHAllowsLanding(void)
 {
+    if (posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_LAND)
+        return true;
+
     navRTHAllowLanding_e allow = navConfig()->general.flags.rth_allow_landing;
     return allow == NAV_RTH_ALLOW_LANDING_ALWAYS ||
         (allow == NAV_RTH_ALLOW_LANDING_FS_ONLY && FLIGHT_MODE(FAILSAFE_MODE));
diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h
index fcb5a4649d..e9d53f8844 100755
--- a/src/main/navigation/navigation.h
+++ b/src/main/navigation/navigation.h
@@ -235,8 +235,9 @@ typedef struct gpsOrigin_s {
 typedef enum {
     NAV_WP_ACTION_WAYPOINT  = 0x01,
     NAV_WP_ACTION_HOLD_TIME = 0x03,
-    NAV_WP_ACTION_JUMP      = 0x06, 
-    NAV_WP_ACTION_RTH       = 0x04
+    NAV_WP_ACTION_RTH       = 0x04,
+    NAV_WP_ACTION_JUMP      = 0x06,
+    NAV_WP_ACTION_LAND      = 0x08
 } navWaypointActions_e;
 
 typedef enum {

From 0cb3ce4ede4ea27539a9ec0c62735fa5a3b35cb9 Mon Sep 17 00:00:00 2001
From: Michel Pastor <shellixyz@users.noreply.github.com>
Date: Sat, 21 Mar 2020 23:07:02 +0100
Subject: [PATCH 043/179] Lower pos_hold_deadband minimum and default (#5505)

---
 src/main/fc/rc_controls.c | 2 +-
 src/main/fc/settings.yaml | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c
index a160909565..1b8664972d 100644
--- a/src/main/fc/rc_controls.c
+++ b/src/main/fc/rc_controls.c
@@ -74,7 +74,7 @@ PG_REGISTER_WITH_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig, PG_RC_CONT
 PG_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig,
     .deadband = 5,
     .yaw_deadband = 5,
-    .pos_hold_deadband = 20,
+    .pos_hold_deadband = 10,
     .alt_hold_deadband = 50,
     .mid_throttle_deadband = 50,
     .airmodeHandlingType = STICK_CENTER,
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 464bf02e0b..1c0a5032b3 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -933,7 +933,7 @@ groups:
         min: 0
         max: 100
       - name: pos_hold_deadband
-        min: 10
+        min: 2
         max: 250
       - name: alt_hold_deadband
         min: 10

From d7fa48b2e6e44fe515439022ea41158218a897f4 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Sun, 22 Mar 2020 14:21:00 +0100
Subject: [PATCH 044/179] Remove PID iterm constrain for NAV PID controller

---
 src/main/fc/settings.yaml        | 2 +-
 src/main/flight/pid.h            | 2 +-
 src/main/navigation/navigation.c | 5 -----
 3 files changed, 2 insertions(+), 7 deletions(-)

diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 1c0a5032b3..66e2ec87cc 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -1235,7 +1235,7 @@ groups:
         min: 0
         max: 255
       - name: nav_fw_pos_hdg_pidsum_limit
-        field: pidSumLimitYaw
+        field: navFwPosHdgPidsumLimit
         min: PID_SUM_LIMIT_MIN
         max: PID_SUM_LIMIT_MAX
       - name: mc_iterm_relax_type
diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h
index 8bc148e2be..9290bfff91 100644
--- a/src/main/flight/pid.h
+++ b/src/main/flight/pid.h
@@ -150,7 +150,7 @@ typedef struct pidProfile_s {
     float antigravityAccelerator;
     uint8_t antigravityCutoff;
 
-    int navFwPosHdgPidsumLimit;
+    uint16_t navFwPosHdgPidsumLimit;
 } pidProfile_t;
 
 typedef struct pidAutotuneConfig_s {
diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index 0afe103fdd..90e3c05609 100755
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -1863,11 +1863,6 @@ float navPidApply3(pidController_t *pid, const float setpoint, const float measu
         }
     }
 
-    /*
-     * Limit both output and Iterm to limit windup
-     */
-    pid->integrator = constrain(pid->integrator, outMin, outMax);
-
     return outValConstrained;
 }
 

From c00c24924d6ec0f3922827c9f9621bab6d557bf1 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Fri, 20 Mar 2020 20:01:39 +0100
Subject: [PATCH 045/179] Simplify notch Q computation

---
 src/main/common/filter.c | 5 ++---
 src/main/common/filter.h | 2 +-
 2 files changed, 3 insertions(+), 4 deletions(-)

diff --git a/src/main/common/filter.c b/src/main/common/filter.c
index a436534256..3384a1188c 100644
--- a/src/main/common/filter.c
+++ b/src/main/common/filter.c
@@ -118,10 +118,9 @@ float rateLimitFilterApply4(rateLimitFilter_t *filter, float input, float rate_l
     return filter->state;
 }
 
-float filterGetNotchQ(uint16_t centerFreq, uint16_t cutoff)
+float filterGetNotchQ(uint16_t centerFrequencyHz, uint16_t cutoffFrequencyHz)
 {
-    const float octaves = log2f((float)centerFreq  / (float)cutoff) * 2;
-    return sqrtf(powf(2, octaves)) / (powf(2, octaves) - 1);
+    return centerFrequencyHz * cutoffFrequencyHz / (centerFrequencyHz * centerFrequencyHz - cutoffFrequencyHz * cutoffFrequencyHz);
 }
 
 void biquadFilterInitNotch(biquadFilter_t *filter, uint32_t samplingIntervalUs, uint16_t filterFreq, uint16_t cutoffHz)
diff --git a/src/main/common/filter.h b/src/main/common/filter.h
index 5ce181dba6..a9a49ee683 100644
--- a/src/main/common/filter.h
+++ b/src/main/common/filter.h
@@ -79,7 +79,7 @@ void biquadFilterInit(biquadFilter_t *filter, uint16_t filterFreq, uint32_t samp
 float biquadFilterApply(biquadFilter_t *filter, float sample);
 float biquadFilterReset(biquadFilter_t *filter, float value);
 float biquadFilterApplyDF1(biquadFilter_t *filter, float input);
-float filterGetNotchQ(uint16_t centerFreq, uint16_t cutoff);
+float filterGetNotchQ(uint16_t centerFrequencyHz, uint16_t cutoffFrequencyHz)
 void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType);
 
 void firFilterInit(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs);

From 1368e8e88b9362d12aaa474694f9983905c5a210 Mon Sep 17 00:00:00 2001
From: stronnag <jh+github@daria.co.uk>
Date: Sun, 22 Mar 2020 15:09:11 +0000
Subject: [PATCH 046/179] * Accept JUMP WP in CLI wp command (#5499)

* Support WP p2 and p3 in CLI wp command
---
 src/main/fc/cli.c | 35 ++++++++++++++++++++++++++++++-----
 1 file changed, 30 insertions(+), 5 deletions(-)

diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c
index cea006f24b..228dc1b4c7 100755
--- a/src/main/fc/cli.c
+++ b/src/main/fc/cli.c
@@ -1279,7 +1279,7 @@ static void cliTempSensor(char *cmdline)
 static void printWaypoints(uint8_t dumpMask, const navWaypoint_t *navWaypoint, const navWaypoint_t *defaultNavWaypoint)
 {
     cliPrintLinef("#wp %d %svalid", posControl.waypointCount, posControl.waypointListValid ? "" : "in"); //int8_t bool
-    const char *format = "wp %u %u %d %d %d %d %u"; //uint8_t action; int32_t lat; int32_t lon; int32_t alt; int16_t p1; uint8_t flag
+    const char *format = "wp %u %u %d %d %d %d %d %d %u"; //uint8_t action; int32_t lat; int32_t lon; int32_t alt; int16_t p1 int16_t p2 int16_t p3; uint8_t flag
     for (uint8_t i = 0; i < NAV_MAX_WAYPOINTS; i++) {
         bool equalsDefault = false;
         if (defaultNavWaypoint) {
@@ -1288,6 +1288,8 @@ static void printWaypoints(uint8_t dumpMask, const navWaypoint_t *navWaypoint, c
                 && navWaypoint[i].lon == defaultNavWaypoint[i].lon
                 && navWaypoint[i].alt == defaultNavWaypoint[i].alt
                 && navWaypoint[i].p1 == defaultNavWaypoint[i].p1
+                && navWaypoint[i].p2 == defaultNavWaypoint[i].p2
+                && navWaypoint[i].p3 == defaultNavWaypoint[i].p3
                 && navWaypoint[i].flag == defaultNavWaypoint[i].flag;
             cliDefaultPrintLinef(dumpMask, equalsDefault, format,
                 i,
@@ -1296,6 +1298,8 @@ static void printWaypoints(uint8_t dumpMask, const navWaypoint_t *navWaypoint, c
                 defaultNavWaypoint[i].lon,
                 defaultNavWaypoint[i].alt,
                 defaultNavWaypoint[i].p1,
+                defaultNavWaypoint[i].p2,
+                defaultNavWaypoint[i].p3,
                 defaultNavWaypoint[i].flag
             );
         }
@@ -1306,6 +1310,8 @@ static void printWaypoints(uint8_t dumpMask, const navWaypoint_t *navWaypoint, c
             navWaypoint[i].lon,
             navWaypoint[i].alt,
             navWaypoint[i].p1,
+            navWaypoint[i].p2,
+            navWaypoint[i].p3,
             navWaypoint[i].flag
         );
     }
@@ -1335,7 +1341,7 @@ static void cliWaypoints(char *cmdline)
             cliShowParseError();
         }
     } else {
-        int16_t i, p1;
+        int16_t i, p1,p2=0,p3=0,tmp;
         uint8_t action, flag;
         int32_t lat, lon, alt;
         uint8_t validArgumentCount = 0;
@@ -1369,12 +1375,29 @@ static void cliWaypoints(char *cmdline)
             }
             ptr = nextArg(ptr);
             if (ptr) {
-                flag = fastA2I(ptr);
+                tmp = fastA2I(ptr);
                 validArgumentCount++;
             }
-            if (validArgumentCount < 4) {
+                /* We support pre-2.5 6 values (... p1,flags) or
+                 *  2.5 and later, 8 values (... p1,p2,p3,flags)
+                 */
+            ptr = nextArg(ptr);
+            if (ptr) {
+                p2 = tmp;
+                p3 = fastA2I(ptr);
+                validArgumentCount++;
+                ptr = nextArg(ptr);
+                 if (ptr) {
+                    flag = fastA2I(ptr);
+                    validArgumentCount++;
+                }
+            } else {
+                flag = tmp;
+            }
+
+            if (!(validArgumentCount == 6 || validArgumentCount == 8)) {
                 cliShowParseError();
-            } else if (!(action == 0 || action == NAV_WP_ACTION_WAYPOINT || action == NAV_WP_ACTION_RTH || action == NAV_WP_ACTION_HOLD_TIME || action == NAV_WP_ACTION_LAND) || (p1 < 0) || !(flag == 0 || flag == NAV_WP_FLAG_LAST)) {
+            } else if (!(action == 0 || action == NAV_WP_ACTION_WAYPOINT || action == NAV_WP_ACTION_RTH || action == NAV_WP_ACTION_JUMP || action == NAV_WP_ACTION_HOLD_TIME || action == NAV_WP_ACTION_LAND) || (p1 < 0) || !(flag == 0 || flag == NAV_WP_FLAG_LAST)) {
                 cliShowParseError();
             } else {
                 posControl.waypointList[i].action = action;
@@ -1382,6 +1405,8 @@ static void cliWaypoints(char *cmdline)
                 posControl.waypointList[i].lon = lon;
                 posControl.waypointList[i].alt = alt;
                 posControl.waypointList[i].p1 = p1;
+                posControl.waypointList[i].p2 = p2;
+                posControl.waypointList[i].p3 = p3;
                 posControl.waypointList[i].flag = flag;
             }
         } else {

From 169d75403dec7906d488c2d5f8cbaae7572ef934 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Sun, 22 Mar 2020 16:27:22 +0100
Subject: [PATCH 047/179] Fix compilation

---
 src/main/common/filter.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/common/filter.h b/src/main/common/filter.h
index a9a49ee683..985763e459 100644
--- a/src/main/common/filter.h
+++ b/src/main/common/filter.h
@@ -79,7 +79,7 @@ void biquadFilterInit(biquadFilter_t *filter, uint16_t filterFreq, uint32_t samp
 float biquadFilterApply(biquadFilter_t *filter, float sample);
 float biquadFilterReset(biquadFilter_t *filter, float value);
 float biquadFilterApplyDF1(biquadFilter_t *filter, float input);
-float filterGetNotchQ(uint16_t centerFrequencyHz, uint16_t cutoffFrequencyHz)
+float filterGetNotchQ(uint16_t centerFrequencyHz, uint16_t cutoffFrequencyHz);
 void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType);
 
 void firFilterInit(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs);

From ea174b4edf7fee6c76cb326ef317ee4a6df70cd6 Mon Sep 17 00:00:00 2001
From: "Konstantin (DigitalEntity) Sharlaimov"
 <konstantin.sharlaimov@gmail.com>
Date: Tue, 24 Mar 2020 14:39:28 +0100
Subject: [PATCH 048/179] [ACC] Make accelerometer calibration more robust

---
 src/main/common/calibration.h   |  2 +-
 src/main/common/maths.c         | 20 ++++++++-
 src/main/common/maths.h         |  5 ++-
 src/main/sensors/acceleration.c | 72 +++++++++++++++++++++++++--------
 4 files changed, 77 insertions(+), 22 deletions(-)

diff --git a/src/main/common/calibration.h b/src/main/common/calibration.h
index a6443aae48..6c84c4437c 100644
--- a/src/main/common/calibration.h
+++ b/src/main/common/calibration.h
@@ -33,7 +33,7 @@
 #include "common/vector.h"
 
 typedef enum {
-    ZERO_CALIBRATION_NONE,
+    ZERO_CALIBRATION_NONE = 0,
     ZERO_CALIBRATION_IN_PROGRESS,
     ZERO_CALIBRATION_DONE,
     ZERO_CALIBRATION_FAIL,
diff --git a/src/main/common/maths.c b/src/main/common/maths.c
index 0a148701e5..cb7904252e 100644
--- a/src/main/common/maths.c
+++ b/src/main/common/maths.c
@@ -475,7 +475,19 @@ static void sensorCalibration_SolveLGS(float A[4][4], float x[4], float b[4]) {
     sensorCalibration_BackwardSubstitution(A, x, y);
 }
 
-void sensorCalibrationSolveForOffset(sensorCalibrationState_t * state, float result[3])
+bool sensorCalibrationValidateResult(const float result[3])
+{
+    // Validate that result is not INF and not NAN
+    for (int i = 0; i < 3; i++) {
+        if (isnan(result[i]) && isinf(result[i])) {
+            return false;
+        }
+    }
+
+    return true;
+}
+
+bool sensorCalibrationSolveForOffset(sensorCalibrationState_t * state, float result[3])
 {
     float beta[4];
     sensorCalibration_SolveLGS(state->XtX, beta, state->XtY);
@@ -483,9 +495,11 @@ void sensorCalibrationSolveForOffset(sensorCalibrationState_t * state, float res
     for (int i = 0; i < 3; i++) {
         result[i] = beta[i] / 2;
     }
+
+    return sensorCalibrationValidateResult(result);
 }
 
-void sensorCalibrationSolveForScale(sensorCalibrationState_t * state, float result[3])
+bool sensorCalibrationSolveForScale(sensorCalibrationState_t * state, float result[3])
 {
     float beta[4];
     sensorCalibration_SolveLGS(state->XtX, beta, state->XtY);
@@ -493,6 +507,8 @@ void sensorCalibrationSolveForScale(sensorCalibrationState_t * state, float resu
     for (int i = 0; i < 3; i++) {
         result[i] = sqrtf(beta[i]);
     }
+
+    return sensorCalibrationValidateResult(result);
 }
 
 float bellCurve(const float x, const float curveWidth)
diff --git a/src/main/common/maths.h b/src/main/common/maths.h
index 4a4cee9fe8..24a72c5d72 100644
--- a/src/main/common/maths.h
+++ b/src/main/common/maths.h
@@ -18,6 +18,7 @@
 #pragma once
 
 #include <stdint.h>
+#include <stdbool.h>
 
 #ifndef sq
 #define sq(x) ((x)*(x))
@@ -122,8 +123,8 @@ typedef struct {
 void sensorCalibrationResetState(sensorCalibrationState_t * state);
 void sensorCalibrationPushSampleForOffsetCalculation(sensorCalibrationState_t * state, int32_t sample[3]);
 void sensorCalibrationPushSampleForScaleCalculation(sensorCalibrationState_t * state, int axis, int32_t sample[3], int target);
-void sensorCalibrationSolveForOffset(sensorCalibrationState_t * state, float result[3]);
-void sensorCalibrationSolveForScale(sensorCalibrationState_t * state, float result[3]);
+bool sensorCalibrationSolveForOffset(sensorCalibrationState_t * state, float result[3]);
+bool sensorCalibrationSolveForScale(sensorCalibrationState_t * state, float result[3]);
 
 int gcd(int num, int denom);
 int32_t applyDeadband(int32_t value, int32_t deadband);
diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c
index 07ae4c067b..eb537daf7f 100644
--- a/src/main/sensors/acceleration.c
+++ b/src/main/sensors/acceleration.c
@@ -350,11 +350,10 @@ bool accInit(uint32_t targetLooptime)
 
 static bool calibratedPosition[6];
 static int32_t accSamples[6][3];
-static int  calibratedAxisCount = 0;
 
 uint8_t accGetCalibrationAxisFlags(void)
 {
-    if (accIsCalibrationComplete() && STATE(ACCELEROMETER_CALIBRATED)) {
+    if (STATE(ACCELEROMETER_CALIBRATED)) {
         return 0x3F;    // All 6 bits are set
     }
 
@@ -393,19 +392,20 @@ static int getPrimaryAxisIndex(int32_t accADCData[3])
         return -1;
 }
 
-bool accIsCalibrationComplete(void)
-{
-    return zeroCalibrationIsCompleteV(&zeroCalibration);
-}
-
 void accStartCalibration(void)
 {
     int positionIndex = getPrimaryAxisIndex(accADC);
 
+    // Fail if we can't detect the side
     if (positionIndex < 0) {
         return;
     }
 
+    // Fail if we have accelerometer fully calibrated and are NOT starting with TOP-UP position
+    if (STATE(ACCELEROMETER_CALIBRATED) && positionIndex != 0) {
+        return;
+    }
+
     // Top+up and first calibration cycle, reset everything
     if (positionIndex == 0) {
         for (int axis = 0; axis < 6; axis++) {
@@ -415,7 +415,6 @@ void accStartCalibration(void)
             accSamples[axis][Z] = 0;
         }
 
-        calibratedAxisCount = 0;
         DISABLE_STATE(ACCELEROMETER_CALIBRATED);
     }
 
@@ -423,8 +422,30 @@ void accStartCalibration(void)
     zeroCalibrationStartV(&zeroCalibration, CALIBRATING_ACC_TIME_MS, acc.dev.acc_1G * 0.05f, true);
 }
 
+static bool allOrientationsHaveCalibrationDataCollected(void)
+{
+    // Return true only if we have calibration data for all 6 positions
+    return calibratedPosition[0] && calibratedPosition[1] && calibratedPosition[2] &&
+           calibratedPosition[3] && calibratedPosition[4] && calibratedPosition[5];
+}
+
+bool accIsCalibrationComplete(void)
+{
+    return zeroCalibrationIsCompleteV(&zeroCalibration);
+}
+
 static void performAcclerationCalibration(void)
 {
+    // Shortcut - no need to do any math if acceleromter is marked as calibrated
+    if (STATE(ACCELEROMETER_CALIBRATED)) {
+        return;
+    }
+
+    // If zero calibration logic is finished - no need to do anything
+    if (accIsCalibrationComplete()) {
+        return;
+    }
+
     fpVector3_t v;
     int positionIndex = getPrimaryAxisIndex(accADC);
 
@@ -449,7 +470,6 @@ static void performAcclerationCalibration(void)
                 accSamples[positionIndex][Z] = v.v[Z];
 
                 calibratedPosition[positionIndex] = true;
-                calibratedAxisCount++;
             }
             else {
                 calibratedPosition[positionIndex] = false;
@@ -459,9 +479,10 @@ static void performAcclerationCalibration(void)
         }
     }
 
-    if (calibratedAxisCount == 6) {
+    if (allOrientationsHaveCalibrationDataCollected()) {
         sensorCalibrationState_t calState;
         float accTmp[3];
+        bool calFailed = false;
 
         /* Calculate offset */
         sensorCalibrationResetState(&calState);
@@ -470,7 +491,12 @@ static void performAcclerationCalibration(void)
             sensorCalibrationPushSampleForOffsetCalculation(&calState, accSamples[axis]);
         }
 
-        sensorCalibrationSolveForOffset(&calState, accTmp);
+        if (!sensorCalibrationSolveForOffset(&calState, accTmp)) {
+            accTmp[0] = 0.0f;
+            accTmp[1] = 0.0f;
+            accTmp[1] = 0.0f;
+            calFailed = true;
+        }
 
         accelerometerConfigMutable()->accZero.raw[X] = lrintf(accTmp[X]);
         accelerometerConfigMutable()->accZero.raw[Y] = lrintf(accTmp[Y]);
@@ -489,13 +515,28 @@ static void performAcclerationCalibration(void)
             sensorCalibrationPushSampleForScaleCalculation(&calState, axis / 2, accSample, acc.dev.acc_1G);
         }
 
-        sensorCalibrationSolveForScale(&calState, accTmp);
+        if (!sensorCalibrationSolveForScale(&calState, accTmp)) {
+            accTmp[0] = 1.0f;
+            accTmp[1] = 1.0f;
+            accTmp[1] = 1.0f;
+            calFailed = true;
+        }
 
         for (int axis = 0; axis < 3; axis++) {
             accelerometerConfigMutable()->accGain.raw[axis] = lrintf(accTmp[axis] * 4096);
         }
 
-        saveConfigAndNotify();
+        if (calFailed) {
+            // If failed - don't save and also invalidate the calibration data for all positions
+            for (int axis = 0; axis < 6; axis++) {
+                calibratedPosition[axis] = false;
+            }
+        }
+        else {
+            // saveConfigAndNotify will trigger eepromREAD and in turn call back the accelerometer gain validation
+            // that will set ENABLE_STATE(ACCELEROMETER_CALIBRATED) if all is good
+            saveConfigAndNotify();
+        }
     }
 }
 
@@ -540,10 +581,7 @@ void accUpdate(void)
         DEBUG_SET(DEBUG_ACC, axis, accADC[axis]);
     }
 
-    if (!accIsCalibrationComplete()) {
-        performAcclerationCalibration();
-        return;
-    }
+    performAcclerationCalibration();
 
     applyAccelerationZero(&accelerometerConfig()->accZero, &accelerometerConfig()->accGain);
 

From 659fef8338cdbff8ef7f31094cfa690d6d171f31 Mon Sep 17 00:00:00 2001
From: stronnag <jh+github@daria.co.uk>
Date: Tue, 24 Mar 2020 15:26:21 +0000
Subject: [PATCH 049/179] enable embedded / rearmed JUMPs (#5515)

* Missions may contain multiple, embedded JUMPs
*The jump counter is reset to the designed value at the end of each jump so JUMP is re-entrant
* Save to EEPROM of a completed JUMP mission is correct and safe
---
 src/main/navigation/navigation.c | 60 ++++++++++++++++++++++++++------
 1 file changed, 50 insertions(+), 10 deletions(-)

diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index 90e3c05609..109de949ca 100755
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -197,6 +197,10 @@ static void setupAltitudeController(void);
 static void resetHeadingController(void);
 void resetGCSFlags(void);
 
+static void setupJumpCounters(void);
+static void resetJumpCounter(void);
+static void clearJumpCounters(void);
+
 static void calculateAndSetActiveWaypoint(const navWaypoint_t * waypoint);
 static void calculateAndSetActiveWaypointToLocalPosition(const fpVector3_t * pos);
 void calculateInitialHoldPosition(fpVector3_t * pos);
@@ -1364,7 +1368,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHED(navigation
 
     // Prevent I-terms growing when already landed
     pidResetErrorAccumulators();
-
     return NAV_FSM_EVENT_NONE;
 }
 
@@ -1382,7 +1385,11 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_INITIALIZE(nav
         // Make sure surface tracking is not enabled - RTH uses global altitude, not AGL
         resetAltitudeController(false);
         setupAltitudeController();
-
+/*
+  Use p3 as the volatile jump counter, allowing embedded, rearmed jumps
+  Using p3 minimises the risk of saving an invalid counter if a mission is aborted.
+*/
+        setupJumpCounters();
         posControl.activeWaypointIndex = 0;
         return NAV_FSM_EVENT_SUCCESS;   // will switch to NAV_STATE_WAYPOINT_PRE_ACTION
     }
@@ -1402,9 +1409,11 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav
             posControl.wpInitialAltitude = posControl.actualState.abs.pos.z;
             return NAV_FSM_EVENT_SUCCESS;       // will switch to NAV_STATE_WAYPOINT_IN_PROGRESS
 
+                // We use p3 as the volatile jump counter (p2 is the static value)
         case NAV_WP_ACTION_JUMP:
-            if(posControl.waypointList[posControl.activeWaypointIndex].p2 != -1){
-                if(posControl.waypointList[posControl.activeWaypointIndex].p2 == 0){
+            if(posControl.waypointList[posControl.activeWaypointIndex].p3 != -1){
+                if(posControl.waypointList[posControl.activeWaypointIndex].p3 == 0){
+                    resetJumpCounter();
                     const bool isLastWaypoint = (posControl.waypointList[posControl.activeWaypointIndex].flag == NAV_WP_FLAG_LAST) ||
                           (posControl.activeWaypointIndex >= (posControl.waypointCount - 1));
 
@@ -1419,7 +1428,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav
                 }
                 else
                 {
-                    posControl.waypointList[posControl.activeWaypointIndex].p2--;
+                    posControl.waypointList[posControl.activeWaypointIndex].p3--;
                 }
             }
 
@@ -1571,6 +1580,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_FINISHED(navig
 {
     UNUSED(previousState);
 
+    clearJumpCounters();
     // If no position sensor available - land immediately
     if ((posControl.flags.estPosStatus >= EST_USABLE) && (posControl.flags.estHeadingStatus >= EST_USABLE)) {
         return NAV_FSM_EVENT_NONE;
@@ -2545,6 +2555,36 @@ static bool adjustAltitudeFromRCInput(void)
     }
 }
 
+/*-----------------------------------------------------------
+ * Jump Counter support functions
+ *-----------------------------------------------------------*/
+static void setupJumpCounters(void)
+{
+    for (uint8_t wp = 0; wp < posControl.waypointCount ; wp++) {
+        if (posControl.waypointList[wp].action == NAV_WP_ACTION_JUMP){
+            posControl.waypointList[wp].p3 = posControl.waypointList[wp].p2;
+        }
+    }
+}
+
+static void resetJumpCounter(void)
+{
+        // reset the volatile counter from the set / static value
+    posControl.waypointList[posControl.activeWaypointIndex].p3 =
+        posControl.waypointList[posControl.activeWaypointIndex].p2;
+}
+
+static void clearJumpCounters(void)
+{
+    for (uint8_t wp = 0; wp < posControl.waypointCount ; wp++) {
+        if (posControl.waypointList[wp].action == NAV_WP_ACTION_JUMP) {
+            posControl.waypointList[wp].p3 = 0;
+        }
+    }
+}
+
+
+
 /*-----------------------------------------------------------
  * Heading controller (pass-through to MAG mode)
  *-----------------------------------------------------------*/
@@ -3172,14 +3212,14 @@ navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass)
 
     // Don't allow arming if any of JUMP waypoint has invalid settings
     if (posControl.waypointCount > 0) {
-    for (uint8_t wp = 0; wp < posControl.waypointCount ; wp++){
-       if (posControl.waypointList[wp].action == NAV_WP_ACTION_JUMP){
-           if((posControl.waypointList[wp].p1 > posControl.waypointCount) || (posControl.waypointList[wp].p2 < -1)){
-               return NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR;
+        for (uint8_t wp = 0; wp < posControl.waypointCount ; wp++){
+            if (posControl.waypointList[wp].action == NAV_WP_ACTION_JUMP){
+                if((posControl.waypointList[wp].p1 > posControl.waypointCount) || (posControl.waypointList[wp].p2 < -1)){
+                    return NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR;
+                }
             }
         }
     }
-    }
 
     return NAV_ARMING_BLOCKER_NONE;
 }

From ace2c35c5ccad3c336ccafaa3a237e42224d908a Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Alberto=20Garci=CC=81a=20Hierro?= <alberto@garciahierro.com>
Date: Wed, 25 Mar 2020 22:16:38 +0000
Subject: [PATCH 050/179] [SYSTEM] Avoid performing a 64bit division in
 micros()

Since the sub-ms step fits in an uint32_t, we can do the calculation
using just 32 bits, then perform a 64 bit add to the (ms * 1000)
value. This is way faster because cortex-m[4,7] do have 32 bit
integer division in HW, but not for 64 bit.

This reduces the number of cycles used by micros() from ~177
to ~57 (there's a small 3-4 cycle variance due to pipelining).
---
 src/main/drivers/system.c | 8 ++++++--
 1 file changed, 6 insertions(+), 2 deletions(-)

diff --git a/src/main/drivers/system.c b/src/main/drivers/system.c
index aa4bfcb2c9..bfbfbf42ce 100644
--- a/src/main/drivers/system.c
+++ b/src/main/drivers/system.c
@@ -132,7 +132,9 @@ timeUs_t microsISR(void)
         pending = sysTickPending;
     }
 
-    return ((timeUs_t)(ms + pending) * 1000LL) + (usTicks * 1000LL - (timeUs_t)cycle_cnt) / usTicks;
+    // XXX: Be careful to not trigger 64 bit division
+    uint32_t partial = (((uint32_t)usTicks) * 1000U - cycle_cnt) / ((uint32_t)usTicks);
+    return ((timeUs_t)(ms + pending) * 1000LL) + partial;
 }
 
 timeUs_t micros(void)
@@ -152,7 +154,9 @@ timeUs_t micros(void)
         cycle_cnt = SysTick->VAL;
     } while (ms != sysTickUptime || cycle_cnt > sysTickValStamp);
 
-    return ((timeUs_t)ms * 1000LL) + (usTicks * 1000LL - (timeUs_t)cycle_cnt) / usTicks;
+    // XXX: Be careful to not trigger 64 bit division
+    uint32_t partial = (((uint32_t)usTicks) * 1000U - cycle_cnt) / ((uint32_t)usTicks);
+    return ((timeUs_t)ms * 1000U) + partial;
 }
 
 // Return system uptime in milliseconds (rollover in 49 days)

From 396f4cceeb92b01126111152aaef4f9da6426b17 Mon Sep 17 00:00:00 2001
From: "Konstantin (DigitalEntity) Sharlaimov"
 <konstantin.sharlaimov@gmail.com>
Date: Thu, 26 Mar 2020 10:16:13 +0100
Subject: [PATCH 051/179] [SYSTEM] Reduce type witdth  for usTicks to uint32_t
 to avoid unneccessary 64-bit math

---
 src/main/drivers/system.c | 12 ++++++------
 1 file changed, 6 insertions(+), 6 deletions(-)

diff --git a/src/main/drivers/system.c b/src/main/drivers/system.c
index bfbfbf42ce..a94de7ddd5 100644
--- a/src/main/drivers/system.c
+++ b/src/main/drivers/system.c
@@ -48,8 +48,8 @@ void registerExtiCallbackHandler(IRQn_Type irqn, extiCallbackHandlerFunc *fn)
     failureMode(FAILURE_DEVELOPER); // EXTI_CALLBACK_HANDLER_COUNT is too low for the amount of handlers required.
 }
 
-// cycles per microsecond
-STATIC_UNIT_TESTED  timeUs_t usTicks = 0;
+// cycles per microsecond, this is deliberately uint32_t to avoid type conversions
+STATIC_UNIT_TESTED  uint32_t usTicks = 0;
 // current uptime for 1kHz systick timer. will rollover after 49 days. hopefully we won't care.
 STATIC_UNIT_TESTED volatile timeMs_t sysTickUptime = 0;
 STATIC_UNIT_TESTED volatile uint32_t sysTickValStamp = 0;
@@ -133,8 +133,8 @@ timeUs_t microsISR(void)
     }
 
     // XXX: Be careful to not trigger 64 bit division
-    uint32_t partial = (((uint32_t)usTicks) * 1000U - cycle_cnt) / ((uint32_t)usTicks);
-    return ((timeUs_t)(ms + pending) * 1000LL) + partial;
+    const uint32_t partial = (usTicks * 1000U - cycle_cnt) / usTicks;
+    return ((timeUs_t)(ms + pending) * 1000LL) + ((timeUs_t)partial);
 }
 
 timeUs_t micros(void)
@@ -155,8 +155,8 @@ timeUs_t micros(void)
     } while (ms != sysTickUptime || cycle_cnt > sysTickValStamp);
 
     // XXX: Be careful to not trigger 64 bit division
-    uint32_t partial = (((uint32_t)usTicks) * 1000U - cycle_cnt) / ((uint32_t)usTicks);
-    return ((timeUs_t)ms * 1000U) + partial;
+    const uint32_t partial = (usTicks * 1000U - cycle_cnt) / usTicks;
+    return ((timeUs_t)ms * 1000LL) + ((timeUs_t)partial);
 }
 
 // Return system uptime in milliseconds (rollover in 49 days)

From 67c6e9d6c8c2890d34455d50df9d355b1290a4ff Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Alberto=20Garci=CC=81a=20Hierro?= <alberto@garciahierro.com>
Date: Thu, 26 Mar 2020 20:18:19 +0000
Subject: [PATCH 052/179] [MAKE] Avoid adding our custom CFLAGS twice

We don't clear CFLAGS becase we want to allow them to be passed
via command line. However, when building a target we add our
custom ones once in the first parsing of the Makefile and then
again a second time when we invoke make recursively for each target.
This breaks compilation with -Werror.

To workaround this, save the initial CFLAGS in a variable, then
pass them to the recursive invocation of make.
---
 Makefile | 7 ++++++-
 1 file changed, 6 insertions(+), 1 deletion(-)

diff --git a/Makefile b/Makefile
index 5a6da86388..6fa7ba763c 100644
--- a/Makefile
+++ b/Makefile
@@ -174,6 +174,11 @@ SIZE        = $(ARM_SDK_PREFIX)size
 # Tool options.
 #
 
+# Save original CFLAGS before modifying them, so we don't
+# add them twice when calling this Makefile recursively
+# for each target
+SAVED_CFLAGS	:= $(CFLAGS)
+
 ifeq ($(DEBUG),GDB)
 LTO_FLAGS   = 
 else
@@ -360,7 +365,7 @@ release: $(RELEASE_TARGETS)
 $(VALID_TARGETS):
 	$(V0) echo "" && \
 	echo "Building $@" && \
-	$(MAKE) -j 8 TARGET=$@ && \
+	CFLAGS=$(SAVED_CFLAGS) $(MAKE) -j 8 TARGET=$@ && \
 	echo "Building $@ succeeded."
 
 ## clean             : clean up all temporary / machine-generated files

From 9099634d3da53510b2a302be5243754574d5e8f5 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Alberto=20Garci=CC=81a=20Hierro?= <alberto@garciahierro.com>
Date: Thu, 26 Mar 2020 23:21:50 +0000
Subject: [PATCH 053/179] [BUILD] Fix compilation with -Werror

---
 src/main/fc/fc_init.c              | 2 +-
 src/main/flight/mixer.c            | 4 +++-
 src/main/io/osd_dji_hd.c           | 2 ++
 src/main/io/vtx.c                  | 3 +++
 src/main/target/OMNIBUSF4/target.h | 2 ++
 5 files changed, 11 insertions(+), 2 deletions(-)

diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c
index 2ea7f7281b..8d445f0289 100644
--- a/src/main/fc/fc_init.c
+++ b/src/main/fc/fc_init.c
@@ -509,7 +509,7 @@ void init(void)
 
     rxInit();
 
-#if (defined(USE_OSD) || (defined(USE_MSP_DISPLAYPORT) && defined(USE_CMS)))
+#if defined(USE_OSD)
     displayPort_t *osdDisplayPort = NULL;
 #endif
 
diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c
index c0a95ae0d8..41b4b46649 100755
--- a/src/main/flight/mixer.c
+++ b/src/main/flight/mixer.c
@@ -292,6 +292,7 @@ void mixerResetDisarmedMotors(void)
     }
 }
 
+#ifdef USE_DSHOT
 static uint16_t handleOutputScaling(
     int16_t input,          // Input value from the mixer
     int16_t stopThreshold,  // Threshold value to check if motor should be rotating or not
@@ -319,6 +320,7 @@ static uint16_t handleOutputScaling(
     }
     return value;
 }
+#endif
 
 void FAST_CODE writeMotors(void)
 {
@@ -591,4 +593,4 @@ void loadPrimaryMotorMixer(void) {
     for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
         currentMixer[i] = *primaryMotorMixer(i);
     }
-}
\ No newline at end of file
+}
diff --git a/src/main/io/osd_dji_hd.c b/src/main/io/osd_dji_hd.c
index 38e7bf52ce..638768f726 100644
--- a/src/main/io/osd_dji_hd.c
+++ b/src/main/io/osd_dji_hd.c
@@ -258,6 +258,7 @@ static uint32_t djiPackArmingDisabledFlags(void)
     return isArmingDisabled() ? (1 << 24) : 0;
 }
 
+#if defined(USE_OSD)
 static uint32_t djiEncodeOSDEnabledWarnings(void)
 {
     // TODO:
@@ -364,6 +365,7 @@ static void djiSerializeOSDConfigReply(sbuf_t *dst)
     //sbufWriteU8(dst, DJI_OSD_SCREEN_WIDTH); // osdConfig()->camera_frame_width
     //sbufWriteU8(dst, DJI_OSD_SCREEN_HEIGHT); // osdConfig()->camera_frame_height
 }
+#endif
 
 static mspResult_e djiProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn)
 {
diff --git a/src/main/io/vtx.c b/src/main/io/vtx.c
index 1971b09b04..972162264c 100644
--- a/src/main/io/vtx.c
+++ b/src/main/io/vtx.c
@@ -25,6 +25,7 @@
 
 #include "common/maths.h"
 #include "common/time.h"
+#include "common/utils.h"
 
 #include "config/parameter_group.h"
 #include "config/parameter_group_ids.h"
@@ -121,6 +122,8 @@ static bool vtxProcessPower(vtxDevice_t *vtxDevice, const vtxSettingsConfig_t *
 
 static bool vtxProcessPitMode(vtxDevice_t *vtxDevice, const vtxSettingsConfig_t * runtimeSettings)
 {
+    UNUSED(runtimeSettings);
+
     uint8_t pitOnOff;
 
     bool        currPmSwitchState = false;
diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h
index 991fb6c4ab..a4fcdf0736 100644
--- a/src/main/target/OMNIBUSF4/target.h
+++ b/src/main/target/OMNIBUSF4/target.h
@@ -140,7 +140,9 @@
 #define VBUS_SENSING_PIN        PC5
 #define VBUS_SENSING_ENABLED
 
+#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3)
 #define USE_UART_INVERTER
+#endif
 
 #define USE_UART1
 #define UART1_RX_PIN            PA10

From ace367beb1d5d2b2283c450328b794693cbdc506 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Alberto=20Garci=CC=81a=20Hierro?= <alberto@garciahierro.com>
Date: Fri, 14 Feb 2020 16:48:05 +0000
Subject: [PATCH 054/179] [CI] Add GH action for CI

Add a workflow file which is triggered on every commit, runs the
tests and tries to compile all targets with -Werror.

On each run of the actions, all the built binaries are saved as
build artifacts, so this can be helpful for providing binaries
for testing.
---
 .github/workflows/ci.yml   | 88 ++++++++++++++++++++++++++++++++++++++
 make/tools.mk              | 25 ++++++++---
 src/utils/build-targets.sh | 66 ++++++++++++++++++++++++++++
 3 files changed, 172 insertions(+), 7 deletions(-)
 create mode 100644 .github/workflows/ci.yml
 create mode 100755 src/utils/build-targets.sh

diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml
new file mode 100644
index 0000000000..b6b4bc7f83
--- /dev/null
+++ b/.github/workflows/ci.yml
@@ -0,0 +1,88 @@
+name: Build firmware
+on: [push]
+
+jobs:
+  # This downloads the GNU toolchain from ARM once, then
+  # saves it as an artifact for the remaining jobs, making
+  # the download way faster.
+  download-toolchain:
+    runs-on: ubuntu-18.04
+    steps:
+      - uses: actions/checkout@v2
+      - name: Prepare ARM toolchain environment
+        run: |
+          echo "::set-env name=ARM_SDK_FILENAME::$(make arm_sdk_print_filename)"
+          echo "::set-env name=ARM_SDK_DOWNLOAD_PATH::$(make arm_sdk_print_download_path)"
+      - name: Download ARM toolchain
+        run: make arm_sdk_download
+      - name: Save toolchain as artifact
+        uses: actions/upload-artifact@v2-preview
+        with:
+          name: ${{ env.ARM_SDK_FILENAME }}
+          path: ${{ env.ARM_SDK_DOWNLOAD_PATH }}
+
+  build:
+    needs: download-toolchain
+    runs-on: ubuntu-18.04
+    strategy:
+        matrix:
+          start: [0, 10, 20, 30, 40, 50, 60, 70, 80, 90, 100]
+          count: [10]
+
+    steps:
+        - uses: actions/checkout@v2
+        - name: Setup environment
+          run: |
+            echo "::set-env name=TARGETS::$(./src/utils/build-targets.sh -n -s ${{ matrix.start }} -c ${{ matrix.count }})"
+            echo "::set-env name=BUILD_SUFFIX::ci-$(git rev-parse --short ${{ github.ref }})"
+            echo "::set-env name=IS_LAST_JOB::$([ $(expr ${{ strategy.job-index }} + 1) = ${{ strategy.job-total }} ] && echo yes)"
+        - name: Ensure all targets will be tested
+          if: ${{ env.IS_LAST_JOB }}
+          run: |
+            UNTESTED=$(./src/utils/build-targets.sh -n -s $(expr ${{ matrix.start }} + ${{ matrix.count }}) -c 10000)
+            if ! [ -z "${UNTESTED}" ]; then
+              echo "Untested targets: ${UNTESTED}" >&2
+              exit 1
+            fi
+        - name: Prepare ARM toolchain environment
+          if: ${{ env.TARGETS }}
+          run: |
+            echo "::set-env name=ARM_SDK_FILENAME::$(make arm_sdk_print_filename)"
+            echo "::set-env name=ARM_SDK_DOWNLOAD_DIR::$(dirname $(make arm_sdk_print_download_path))"
+        - name: Download ARM toolchain
+          if: ${{ env.TARGETS }}
+          uses: actions/download-artifact@v1
+          with:
+            name: ${{ env.ARM_SDK_FILENAME }}
+            path: ${{ env.ARM_SDK_DOWNLOAD_DIR }}
+        - name: Install ARM toolchain
+          if: ${{ env.TARGETS }}
+          run: make arm_sdk_install
+        - name: Build targets (${{ matrix.start }})
+          if: ${{ env.TARGETS }}
+          run: ./src/utils/build-targets.sh -s ${{ matrix.start }} -c ${{ matrix.count }} -S ${{ env.BUILD_SUFFIX }}
+        - name: Upload artifacts
+          if: ${{ env.TARGETS }}
+          uses: actions/upload-artifact@v2-preview
+          with:
+            name: inav-${{ env.BUILD_SUFFIX }}.zip
+            path: ./obj/*.hex
+
+  test:
+    needs: download-toolchain
+    runs-on: ubuntu-18.04
+    steps:
+      - uses: actions/checkout@v2
+      - name: Prepare ARM toolchain environment
+        run: |
+          echo "::set-env name=ARM_SDK_FILENAME::$(make arm_sdk_print_filename)"
+          echo "::set-env name=ARM_SDK_DOWNLOAD_DIR::$(dirname $(make arm_sdk_print_download_path))"
+      - name: Download ARM toolchain
+        uses: actions/download-artifact@v1
+        with:
+          name: ${{ env.ARM_SDK_FILENAME }}
+          path: ${{ env.ARM_SDK_DOWNLOAD_DIR }}
+      - name: Install ARM toolchain
+        run: make arm_sdk_install
+      - name: Run Tests
+        run: make test
diff --git a/make/tools.mk b/make/tools.mk
index 28e1953b61..e24bf1bfed 100644
--- a/make/tools.mk
+++ b/make/tools.mk
@@ -32,9 +32,20 @@ ifdef WINDOWS
 endif
 
 ARM_SDK_FILE := $(notdir $(ARM_SDK_URL))
+ARM_SDK_DOWNLOAD_PATH := $(DL_DIR)/$(ARM_SDK_FILE)
 
 SDK_INSTALL_MARKER := $(ARM_SDK_DIR)/bin/arm-none-eabi-gcc-$(GCC_REQUIRED_VERSION)
 
+.PHONY: arm_sdk_print_filename
+
+arm_sdk_print_filename:
+	@echo $(ARM_SDK_FILE)
+
+.PHONY: arm_sdk_print_download_path
+
+arm_sdk_print_download_path:
+	@echo $(ARM_SDK_DOWNLOAD_PATH)
+
 arm_sdk_install: | $(TOOLS_DIR)
 
 arm_sdk_install: arm_sdk_download $(SDK_INSTALL_MARKER)
@@ -42,17 +53,17 @@ arm_sdk_install: arm_sdk_download $(SDK_INSTALL_MARKER)
 $(SDK_INSTALL_MARKER):
 ifneq ($(OSFAMILY), windows)
     # binary only release so just extract it
-	$(V1) tar -C $(TOOLS_DIR) -xjf "$(DL_DIR)/$(ARM_SDK_FILE)"
+	$(V1) tar -C $(TOOLS_DIR) -xjf "$(ARM_SDK_DOWNLOAD_PATH)"
 else
-	$(V1) unzip -q -d $(ARM_SDK_DIR) "$(DL_DIR)/$(ARM_SDK_FILE)"
+	$(V1) unzip -q -d $(ARM_SDK_DIR) "$(ARM_SDK_DOWNLOAD_PATH)"
 endif
 
 .PHONY: arm_sdk_download
 arm_sdk_download: | $(DL_DIR)
-arm_sdk_download: $(DL_DIR)/$(ARM_SDK_FILE)
-$(DL_DIR)/$(ARM_SDK_FILE):
+arm_sdk_download: $(ARM_SDK_DOWNLOAD_PATH)
+$(ARM_SDK_DOWNLOAD_PATH):
     # download the source only if it's newer than what we already have
-	$(V1) curl -L -k -o "$(DL_DIR)/$(ARM_SDK_FILE)" -z "$(DL_DIR)/$(ARM_SDK_FILE)" "$(ARM_SDK_URL)"
+	$(V1) curl -L -k -o "$(ARM_SDK_DOWNLOAD_PATH)" -z "$(ARM_SDK_DOWNLOAD_PATH)" "$(ARM_SDK_URL)"
 
 
 ## arm_sdk_clean     : Uninstall Arm SDK
@@ -69,7 +80,7 @@ arm_sdk_clean:
 
 ifeq ($(shell [ -d "$(ARM_SDK_DIR)" ] && echo "exists"), exists)
   ARM_SDK_PREFIX := $(ARM_SDK_DIR)/bin/arm-none-eabi-
-else ifeq (,$(findstring _install,$(MAKECMDGOALS)))
+else ifeq (,$(filter targets,$(MAKECMDGOALS))$(findstring arm_sdk,$(MAKECMDGOALS)))
   GCC_VERSION = $(shell arm-none-eabi-gcc -dumpversion)
   ifeq ($(GCC_VERSION),)
     $(error **ERROR** arm-none-eabi-gcc not in the PATH. Run 'make arm_sdk_install' to install automatically in the tools folder of this repo)
@@ -79,4 +90,4 @@ else ifeq (,$(findstring _install,$(MAKECMDGOALS)))
 
   # ARM tookchain is in the path, and the version is what's required.
   ARM_SDK_PREFIX ?= arm-none-eabi-
-endif
\ No newline at end of file
+endif
diff --git a/src/utils/build-targets.sh b/src/utils/build-targets.sh
new file mode 100755
index 0000000000..49f4360272
--- /dev/null
+++ b/src/utils/build-targets.sh
@@ -0,0 +1,66 @@
+#!/bin/sh
+
+set -e
+
+usage()
+{
+   echo "Usage ${0} [-n] [-S suffix] -s START -c COUNT]" >&2;
+   echo "Build COUNT targets starting at index START" >&2;
+   echo "Targets are retrieved via make targets" >&2;
+   echo "-n prints the targets instead of building them" >&2;
+   echo "-S sets the BUILD_SUFFIX" >&2;
+   exit 1;
+}
+
+is_number()
+{
+    ! [ -z ${1} ] && [ ${1} -eq ${1} ] 2>/dev/null
+}
+
+START=
+COUNT=
+DRY_RUN=
+BUILD_SUFFIX=
+
+args=`getopt nS:s:c: $*`
+set -- $args
+for i
+do
+    case "$i"
+    in
+        -n)
+            DRY_RUN=1
+            shift;;
+        -S)
+            BUILD_SUFFIX=${2}; shift;
+            shift;;
+        -s)
+            START=${2}; shift;
+            shift;;
+        -c)
+            COUNT=${2}; shift;
+            shift;;
+        --)
+            shift; break;;
+    esac
+done
+
+if ! is_number ${START} || ! is_number ${COUNT}; then
+    usage;
+fi
+
+ALL_TARGETS=$(make targets | grep Valid | awk -F' *: *' '{print $2}')
+START_IDX=$(expr ${START} + 1)
+END_IDX=$(expr ${START} + ${COUNT})
+SELECTED_TARGETS=$(echo ${ALL_TARGETS} | cut -d ' ' -f ${START_IDX}-${END_IDX})
+
+if [ -z "${DRY_RUN}" ]; then
+    # make without arguments builds the default
+    # target, so make sure ${SELECTED_TARGETS} is
+    # not empty
+    if [ -n "${SELECTED_TARGETS}" ]; then
+        BUILD_SUFFIX=${BUILD_SUFFIX} V=0 CFLAGS=-Werror make ${SELECTED_TARGETS}
+    fi
+else
+    echo "${SELECTED_TARGETS}"
+fi

From 3ba7452351513e4031987a86e088205454a00971 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Alberto=20Garci=CC=81a=20Hierro?= <alberto@garciahierro.com>
Date: Sat, 28 Mar 2020 10:08:50 +0000
Subject: [PATCH 055/179] [CI] Make the GH CI test job require all the build
 jobs first

This way we only need to mark the test job as required.
---
 .github/workflows/ci.yml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml
index b6b4bc7f83..dd6ac65ea4 100644
--- a/.github/workflows/ci.yml
+++ b/.github/workflows/ci.yml
@@ -69,7 +69,7 @@ jobs:
             path: ./obj/*.hex
 
   test:
-    needs: download-toolchain
+    needs: [download-toolchain, build]
     runs-on: ubuntu-18.04
     steps:
       - uses: actions/checkout@v2

From ba30bd9ab86d14f223f4fc5bd0a4b245a902131a Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Alberto=20Garci=CC=81a=20Hierro?= <alberto@garciahierro.com>
Date: Sat, 28 Mar 2020 14:16:51 +0000
Subject: [PATCH 056/179] [CI] Trigger CI workflow on PRs coming from forks

---
 .github/workflows/ci.yml | 7 ++++++-
 1 file changed, 6 insertions(+), 1 deletion(-)

diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml
index dd6ac65ea4..f445c58293 100644
--- a/.github/workflows/ci.yml
+++ b/.github/workflows/ci.yml
@@ -1,5 +1,10 @@
 name: Build firmware
-on: [push]
+on:
+  push:
+    branches-ignore:
+      - master
+      - development
+  pull_request:
 
 jobs:
   # This downloads the GNU toolchain from ARM once, then

From 377f5cba7f24868c7d431842e914cbca0a5f2da7 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Alberto=20Garci=CC=81a=20Hierro?= <alberto@garciahierro.com>
Date: Wed, 25 Mar 2020 16:41:54 +0000
Subject: [PATCH 057/179] [CANVAS] Add alternate AHI style with a single line

---
 src/main/fc/settings.yaml |  6 ++++
 src/main/io/osd.c         |  2 +-
 src/main/io/osd.h         |  6 ++++
 src/main/io/osd_canvas.c  | 69 ++++++++++++++++++++++++++++++++++-----
 4 files changed, 73 insertions(+), 10 deletions(-)

diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 66e2ec87cc..88ce41d6b8 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -138,6 +138,9 @@ tables:
   - name: pidTypeTable
     values: ["NONE", "PID", "PIFF", "AUTO"]
     enum: pidType_e
+  - name: osd_ahi_style
+    values: ["DEFAULT", "LINE"]
+    enum: osd_ahi_style_e
 
 groups:
   - name: PG_GYRO_CONFIG
@@ -1999,6 +2002,9 @@ groups:
         min: 10
         max: 13
 
+      - name: osd_ahi_style
+        table: osd_ahi_style
+
   - name: PG_SYSTEM_CONFIG
     type: systemConfig_t
     headers: ["fc/config.h"]
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index df93a1d59f..7da898ebc4 100755
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -198,7 +198,7 @@ static bool osdDisplayHasCanvas;
 #define AH_SIDEBAR_WIDTH_POS 7
 #define AH_SIDEBAR_HEIGHT_POS 3
 
-PG_REGISTER_WITH_RESET_FN(osdConfig_t, osdConfig, PG_OSD_CONFIG, 10);
+PG_REGISTER_WITH_RESET_FN(osdConfig_t, osdConfig, PG_OSD_CONFIG, 11);
 
 static int digitCount(int32_t value)
 {
diff --git a/src/main/io/osd.h b/src/main/io/osd.h
index 43fa41274c..7b26c452ec 100755
--- a/src/main/io/osd.h
+++ b/src/main/io/osd.h
@@ -187,6 +187,11 @@ typedef enum {
     OSD_ALIGN_RIGHT
 } osd_alignment_e;
 
+typedef enum {
+    OSD_AHI_STYLE_DEFAULT,
+    OSD_AHI_STYLE_LINE,
+} osd_ahi_style_e;
+
 typedef struct osdConfig_s {
     // Layouts
     uint16_t item_pos[OSD_LAYOUT_COUNT][OSD_ITEM_COUNT];
@@ -245,6 +250,7 @@ typedef struct osdConfig_s {
 
     bool osd_failsafe_switch_layout;
     uint8_t plus_code_digits; // Number of digits to use in OSD_PLUS_CODE
+    uint8_t osd_ahi_style;
 } osdConfig_t;
 
 PG_DECLARE(osdConfig_t, osdConfig);
diff --git a/src/main/io/osd_canvas.c b/src/main/io/osd_canvas.c
index c4d562be45..4d42b6daf4 100644
--- a/src/main/io/osd_canvas.c
+++ b/src/main/io/osd_canvas.c
@@ -32,6 +32,7 @@
 
 #define AHI_MIN_DRAW_INTERVAL_MS 100
 #define AHI_MAX_DRAW_INTERVAL_MS 1000
+#define AHI_CROSSHAIR_MARGIN 6
 
 #include "common/log.h"
 #include "common/maths.h"
@@ -45,6 +46,7 @@
 #include "drivers/time.h"
 
 #include "io/osd_common.h"
+#include "io/osd.h"
 
 void osdCanvasDrawVarioShape(displayCanvas_t *canvas, unsigned ex, unsigned ey, float zvel, bool erase)
 {
@@ -162,7 +164,6 @@ static void osdDrawArtificialHorizonShapes(displayCanvas_t *canvas, float pitchA
 {
     int barWidth = (OSD_AHI_WIDTH - 1) * canvas->gridElementWidth;
     int levelBarWidth = barWidth * (3.0/4);
-    int crosshairMargin = 6;
     float pixelsPerDegreeLevel = 3.5f;
     int borderSize = 3;
     char buf[12];
@@ -220,9 +221,9 @@ static void osdDrawArtificialHorizonShapes(displayCanvas_t *canvas, float pitchA
         if (ii == 0) {
             displayCanvasSetLineOutlineType(canvas, DISPLAY_CANVAS_OUTLINE_TYPE_BOTTOM);
             displayCanvasMoveToPoint(canvas, -barWidth / 2, 0);
-            displayCanvasStrokeLineToPoint(canvas, -crosshairMargin, 0);
+            displayCanvasStrokeLineToPoint(canvas, -AHI_CROSSHAIR_MARGIN, 0);
             displayCanvasMoveToPoint(canvas, barWidth / 2, 0);
-            displayCanvasStrokeLineToPoint(canvas, crosshairMargin, 0);
+            displayCanvasStrokeLineToPoint(canvas, AHI_CROSSHAIR_MARGIN, 0);
             continue;
         }
 
@@ -257,6 +258,48 @@ static void osdDrawArtificialHorizonShapes(displayCanvas_t *canvas, float pitchA
     displayCanvasContextPop(canvas);
 }
 
+void osdDrawArtificialHorizonLine(displayCanvas_t *canvas, float pitchAngle, float rollAngle, bool erase)
+{
+    int barWidth = (OSD_AHI_WIDTH - 1) * canvas->gridElementWidth;
+    int maxHeight = canvas->height;
+    float pixelsPerDegreeLevel = 1.5f;
+    int maxWidth = (OSD_AHI_WIDTH + 1) * canvas->gridElementWidth;
+
+    int lx = (canvas->width - maxWidth) / 2;
+
+    displayCanvasContextPush(canvas);
+
+    displayCanvasClipToRect(canvas, lx, 0, maxWidth, maxHeight);
+    osdGridBufferClearPixelRect(canvas, lx, 0, maxWidth, maxHeight);
+
+    if (erase) {
+        displayCanvasSetStrokeColor(canvas, DISPLAY_CANVAS_COLOR_TRANSPARENT);
+        displayCanvasSetLineOutlineColor(canvas, DISPLAY_CANVAS_COLOR_TRANSPARENT);
+    } else {
+        displayCanvasSetStrokeColor(canvas, DISPLAY_CANVAS_COLOR_WHITE);
+        displayCanvasSetLineOutlineColor(canvas, DISPLAY_CANVAS_COLOR_BLACK);
+    }
+
+    float pitchDegrees = RADIANS_TO_DEGREES(pitchAngle);
+    float pitchOffset = -pitchDegrees * pixelsPerDegreeLevel;
+    float translateX = canvas->width / 2;
+    float translateY = canvas->height / 2;
+
+    displayCanvasCtmTranslate(canvas, 0, pitchOffset);
+    displayCanvasCtmRotate(canvas, rollAngle);
+    displayCanvasCtmTranslate(canvas, translateX, translateY);
+
+
+    displayCanvasSetStrokeWidth(canvas, 2);
+    displayCanvasSetLineOutlineType(canvas, DISPLAY_CANVAS_OUTLINE_TYPE_BOTTOM);
+    displayCanvasMoveToPoint(canvas, -barWidth / 2, 0);
+    displayCanvasStrokeLineToPoint(canvas, -AHI_CROSSHAIR_MARGIN, 0);
+    displayCanvasMoveToPoint(canvas, barWidth / 2, 0);
+    displayCanvasStrokeLineToPoint(canvas, AHI_CROSSHAIR_MARGIN, 0);
+
+    displayCanvasContextPop(canvas);
+}
+
 void osdCanvasDrawArtificialHorizon(displayPort_t *display, displayCanvas_t *canvas, const osdDrawPoint_t *p, float pitchAngle, float rollAngle)
 {
     UNUSED(display);
@@ -271,12 +314,20 @@ void osdCanvasDrawArtificialHorizon(displayPort_t *display, displayCanvas_t *can
 
     float totalError = fabsf(prevPitchAngle - pitchAngle) + fabsf(prevRollAngle - rollAngle);
     if ((now > nextDrawMinMs && totalError > 0.05f)|| now > nextDrawMaxMs) {
-
-        int x, y, w, h;
-        osdArtificialHorizonRect(canvas, &x, &y, &w, &h);
-        displayCanvasClearRect(canvas, x, y, w, h);
-
-        osdDrawArtificialHorizonShapes(canvas, pitchAngle, rollAngle);
+        switch ((osd_ahi_style_e)osdConfig()->osd_ahi_style) {
+            case OSD_AHI_STYLE_DEFAULT:
+            {
+                int x, y, w, h;
+                osdArtificialHorizonRect(canvas, &x, &y, &w, &h);
+                displayCanvasClearRect(canvas, x, y, w, h);
+                osdDrawArtificialHorizonShapes(canvas, pitchAngle, rollAngle);
+                break;
+            }
+            case OSD_AHI_STYLE_LINE:
+                osdDrawArtificialHorizonLine(canvas, prevPitchAngle, prevRollAngle, true);
+                osdDrawArtificialHorizonLine(canvas, pitchAngle, rollAngle, false);
+                break;
+        }
         prevPitchAngle = pitchAngle;
         prevRollAngle = rollAngle;
         nextDrawMinMs = now + AHI_MIN_DRAW_INTERVAL_MS;

From 332ecc415b6bc556543101c2835e0a1337274167 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Alberto=20Garci=CC=81a=20Hierro?= <alberto@garciahierro.com>
Date: Tue, 24 Mar 2020 22:39:33 +0000
Subject: [PATCH 058/179] [OSD] Wrap stats screen into a transaction

Otherwise, the stats won't show sometimes.
---
 src/main/io/osd.c | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index df93a1d59f..12bc015828 100755
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -2936,6 +2936,7 @@ static void osdShowStats(void)
     const uint8_t statValuesX = 20;
     char buff[10];
 
+    displayBeginTransaction(osdDisplayPort, DISPLAY_TRANSACTION_OPT_RESET_DRAWING);
     displayClearScreen(osdDisplayPort);
     if (IS_DISPLAY_PAL)
         displayWrite(osdDisplayPort, statNameX, top++, "  --- STATS ---");
@@ -3031,6 +3032,7 @@ static void osdShowStats(void)
 
     displayWrite(osdDisplayPort, statNameX, top, "DISARMED BY      :");
     displayWrite(osdDisplayPort, statValuesX, top++, disarmReasonStr[getDisarmReason()]);
+    displayCommitTransaction(osdDisplayPort);
 }
 
 // called when motors armed

From 3618b11d330dee8f0348be3a03b2547d56f1f4b7 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Alberto=20Garci=CC=81a=20Hierro?= <alberto@garciahierro.com>
Date: Tue, 24 Mar 2020 22:34:44 +0000
Subject: [PATCH 059/179] [MSP] Fix crash in mspSerialSendFrame() when MSP port
 turns into CLI

msp->port can become NULL if the port stops being used for MSP
(e.g. it's MSP shared with telemetry or CLI is enabled)
---
 src/main/msp/msp_serial.c | 20 +++++++++++++-------
 1 file changed, 13 insertions(+), 7 deletions(-)

diff --git a/src/main/msp/msp_serial.c b/src/main/msp/msp_serial.c
index b57c3a4c75..1a50162737 100644
--- a/src/main/msp/msp_serial.c
+++ b/src/main/msp/msp_serial.c
@@ -268,8 +268,14 @@ static uint8_t mspSerialChecksumBuf(uint8_t checksum, const uint8_t *data, int l
 #define JUMBO_FRAME_SIZE_LIMIT 255
 static int mspSerialSendFrame(mspPort_t *msp, const uint8_t * hdr, int hdrLen, const uint8_t * data, int dataLen, const uint8_t * crc, int crcLen)
 {
+    // MSP port might be turned into a CLI port, which will make
+    // msp->port become NULL.
+    serialPort_t *port = msp->port;
+    if (!port) {
+        return 0;
+    }
     // VSP MSP port might be unconnected. To prevent blocking - check if it's connected first
-    if (!serialIsConnected(msp->port)) {
+    if (!serialIsConnected(port)) {
         return 0;
     }
 
@@ -278,15 +284,15 @@ static int mspSerialSendFrame(mspPort_t *msp, const uint8_t * hdr, int hdrLen, c
     //     this allows us to transmit jumbo frames bigger than TX buffer (serialWriteBuf will block, but for jumbo frames we don't care)
     //  b) Response fits into TX buffer
     const int totalFrameLength = hdrLen + dataLen + crcLen;
-    if (!isSerialTransmitBufferEmpty(msp->port) && ((int)serialTxBytesFree(msp->port) < totalFrameLength))
+    if (!isSerialTransmitBufferEmpty(port) && ((int)serialTxBytesFree(port) < totalFrameLength))
         return 0;
 
     // Transmit frame
-    serialBeginWrite(msp->port);
-    serialWriteBuf(msp->port, hdr, hdrLen);
-    serialWriteBuf(msp->port, data, dataLen);
-    serialWriteBuf(msp->port, crc, crcLen);
-    serialEndWrite(msp->port);
+    serialBeginWrite(port);
+    serialWriteBuf(port, hdr, hdrLen);
+    serialWriteBuf(port, data, dataLen);
+    serialWriteBuf(port, crc, crcLen);
+    serialEndWrite(port);
 
     return totalFrameLength;
 }

From c0439afb20ecc01ee1799c903b9a7606699349cd Mon Sep 17 00:00:00 2001
From: gereic <57770169+gereic@users.noreply.github.com>
Date: Mon, 30 Mar 2020 14:27:30 +0200
Subject: [PATCH 060/179] added support for BME280

---
 src/main/drivers/barometer/barometer_bmp280.c | 2 +-
 src/main/drivers/barometer/barometer_bmp280.h | 1 +
 2 files changed, 2 insertions(+), 1 deletion(-)

diff --git a/src/main/drivers/barometer/barometer_bmp280.c b/src/main/drivers/barometer/barometer_bmp280.c
index 4e93d458f8..038c5029ce 100644
--- a/src/main/drivers/barometer/barometer_bmp280.c
+++ b/src/main/drivers/barometer/barometer_bmp280.c
@@ -166,7 +166,7 @@ static bool deviceDetect(busDevice_t * busDev)
 
         bool ack = busRead(busDev, BMP280_CHIP_ID_REG, &chipId);
 
-        if (ack && chipId == BMP280_DEFAULT_CHIP_ID) {
+        if ((ack && chipId == BMP280_DEFAULT_CHIP_ID)  || (ack && chipId == BME280_DEFAULT_CHIP_ID)){
             return true;
         }
     };
diff --git a/src/main/drivers/barometer/barometer_bmp280.h b/src/main/drivers/barometer/barometer_bmp280.h
index 0060e820b7..d9827fbf0b 100644
--- a/src/main/drivers/barometer/barometer_bmp280.h
+++ b/src/main/drivers/barometer/barometer_bmp280.h
@@ -19,6 +19,7 @@
 
 #define BMP280_I2C_ADDR                      (0x76)
 #define BMP280_DEFAULT_CHIP_ID               (0x58)
+#define BME280_DEFAULT_CHIP_ID               (0x60)
 
 #define BMP280_CHIP_ID_REG                   (0xD0)  /* Chip ID Register */
 #define BMP280_RST_REG                       (0xE0)  /* Softreset Register */

From a8aa53ec6d84584bc21ed01f8043706581442536 Mon Sep 17 00:00:00 2001
From: stronnag <jh+github@daria.co.uk>
Date: Mon, 30 Mar 2020 18:42:22 +0100
Subject: [PATCH 061/179] remove obsolete `docs/development/Building in
 Ubuntu.md` which was previously replaced by
 `docs/development/Generic_Linux_development.md`, addressing the embedded SDK
 (#5547)

---
 docs/development/Building in Ubuntu.md | 111 -------------------------
 1 file changed, 111 deletions(-)
 delete mode 100644 docs/development/Building in Ubuntu.md

diff --git a/docs/development/Building in Ubuntu.md b/docs/development/Building in Ubuntu.md
deleted file mode 100644
index a76eec0c85..0000000000
--- a/docs/development/Building in Ubuntu.md	
+++ /dev/null
@@ -1,111 +0,0 @@
-# Building in Ubuntu
-
-iNav requires a reasonably modern `gcc-arm-none-eabi` compiler. As a consequence of the long term support options in Ubuntu, it is possible that the distribution compiler will be too old to build iNav firmware. For example, Ubuntu 16.04 LTS ships with version 4.9.3 which cannot compile contemporary iNav.
-
-As of August 2018, the recommendation for Ubuntu releases is:
-
-| Release | Compiler Source |
-| ------- | --------------- |
-| 16.04 or earlier | Use the 'official' PPA |
-| 17.10 | Use the 'official' PPA as the distro compiler (5.4) *may* be too old |
-| 18.04 | Use the 'official' PPA, as the distro compiler (6.3) was broken when last tested |
-
-For Ubuntu derivatives (ElementaryOS, Mint etc.), please check the distro provided version, and if it's lower than 6, use the PPA.
-
-e.g. ElementaryOS
-
-```
-$ apt show gcc-arm-none-eabi
-...
-Version: 15:4.9.3+svn231177-1
-```
-
-This 4.9.3 and will not build iNav, so we need the PPA.
-
-## Installer commands
-
-Older versions of Debian / Ubuntu and derivatives use the `apt-get` command; newer versions use `apt`. Use the appropriate command for your release.
-
-# Prerequisites
-
-Regardless of the cross-compiler version, it is necessary to install some other tools:
-
-```
-sudo apt install git
-sudo apt install gcc
-sudo apt install ruby
-```
-
-A ruby release of at least 2 or later is recommended, if your release only provides 1.9, it is necessary to install a later version:
-
-```
-sudo apt-get remove ruby
-sudo apt-add-repository ppa:brightbox/ruby-ng
-sudo apt-get update
-sudo apt-get install ruby2.4 ruby2.4-dev
-```
-
-# Using the Distro compiler
-
-In case Ubuntu ever provides a modern compiler (as of August 2018, not recommended):
-
-```
-sudo apt install gcc-arm-none-eabi
-```
-
-# Using the PPA compiler
-
-The PPA compiler is recommended for all cases:
-
-```
-sudo apt-get remove binutils-arm-none-eabi gcc-arm-none-eabi
-sudo add-apt-repository ppa:team-gcc-arm-embedded/ppa
-sudo apt-get update
-sudo apt-get install gcc-arm-embedded
-```
-
-After these steps, on Ubuntu 16.04, (at least of March 2018) you should now have:
-
-```
-$ arm-none-eabi-gcc -dumpversion
-7.2.1
-```
-
-Which is more than adequate for our purposes.
-
-# Building from the Github repository
-
-After the ARM cross-compiler toolchain from is installed, you should be able to build from source.
-
-```
-mkdir src
-cd src
-git clone https://github.com/iNavFlight/inav.git
-cd inav
-make
-```
-
-If you have a github account with registered ssh key you can replace the `git clone` command with  `git clone git@github.com:iNavFlight/inav.git` instead of the https link.
-
-By default, this builds the REVO target, to build another target, specify the target name to the make command, for example:
-```
-make TARGET=MATEKF405
-```
-The resultant hex file are in the `obj/` directory.
-
-You can use the INAV Configurator to flash the local ```obj/inav_TARGET.hex``` file, or use `stm32flash` or `dfu-util` directly from the command line.
-
-[msp-tool](https://github.com/fiam/msp-tool) and [flash.sh](https://github.com/stronnag/mwptools/blob/master/docs/MiscTools.asciidoc#flashsh) provide / describe 3rd party helper tools for command line flashing.
-
-# Updating and rebuilding
-
-In order to update your local firmware build:
-
-* Navigate to the local iNav repository
-* Use the following steps to pull the latest changes and rebuild your local version of iNav:
-
-```
-cd src/inav
-git pull
-make TARGET=<TARGET>
-```

From 6028923991b55ad21d6618df3900e17aa9093652 Mon Sep 17 00:00:00 2001
From: "Konstantin (DigitalEntity) Sharlaimov"
 <konstantin.sharlaimov@gmail.com>
Date: Mon, 30 Mar 2020 20:41:36 +0200
Subject: [PATCH 062/179] [NVIC] Refactor NVIC; Fix ATOMIC_BLOCK not being
 actually atomic

---
 src/main/build/atomic.h                   | 11 ++++-
 src/main/drivers/bus_i2c_hal.c            |  5 ++-
 src/main/drivers/dma_stm32f3xx.c          |  9 +---
 src/main/drivers/dma_stm32f4xx.c          |  9 +---
 src/main/drivers/dma_stm32f7xx.c          |  2 +-
 src/main/drivers/exti.c                   | 10 ++---
 src/main/drivers/nvic.h                   | 54 ++++++++---------------
 src/main/drivers/sdcard/sdmmc_sdio_f4xx.c |  8 +---
 src/main/drivers/sdcard/sdmmc_sdio_f7xx.c |  4 +-
 src/main/drivers/serial_uart_stm32f30x.c  | 40 +++++------------
 src/main/drivers/serial_uart_stm32f4xx.c  | 24 +++++-----
 src/main/drivers/serial_uart_stm32f7xx.c  | 18 ++++----
 src/main/drivers/timer_impl_hal.c         |  6 +--
 src/main/drivers/timer_impl_stdperiph.c   | 16 +++----
 src/main/drivers/usb_msc_f4xx.c           |  2 +-
 src/main/drivers/usb_msc_f7xx.c           |  2 +-
 src/main/telemetry/crsf.c                 |  2 +-
 src/main/vcp/hw_config.c                  | 20 ++-------
 src/main/vcpf4/usb_bsp.c                  | 44 ++++--------------
 19 files changed, 93 insertions(+), 193 deletions(-)

diff --git a/src/main/build/atomic.h b/src/main/build/atomic.h
index e0cfb5cdc3..ed11eb42f2 100644
--- a/src/main/build/atomic.h
+++ b/src/main/build/atomic.h
@@ -64,6 +64,13 @@ static inline uint8_t __basepriSetRetVal(uint8_t prio)
     return 1;
 }
 
+// The CMSIS provides the function __set_BASEPRI(priority) for changing the value of the BASEPRI register.
+// The function uses the hardware convention for the ‘priority’ argument, which means that the priority must
+// be shifted left by the number of unimplemented bits (8 – __NVIC_PRIO_BITS).
+//
+// NOTE: The priority numbering convention used in __set_BASEPRI(priority) is thus different than in the
+// NVIC_SetPriority(priority) function, which expects the “priority” argument not shifted.
+
 // Run block with elevated BASEPRI (using BASEPRI_MAX), restoring BASEPRI on exit. All exit paths are handled
 // Full memory barrier is placed at start and exit of block
 #ifdef UNIT_TEST
@@ -71,7 +78,7 @@ static inline uint8_t __basepriSetRetVal(uint8_t prio)
 #define ATOMIC_BLOCK_NB(prio) {}
 #else
 #define ATOMIC_BLOCK(prio) for ( uint8_t __basepri_save __attribute__((__cleanup__(__basepriRestoreMem))) = __get_BASEPRI(), \
-                                     __ToDo = __basepriSetMemRetVal(prio); __ToDo ; __ToDo = 0 )
+                                     __ToDo = __basepriSetMemRetVal((prio) << (8U - __NVIC_PRIO_BITS)); __ToDo ; __ToDo = 0 )
 
 // Run block with elevated BASEPRI (using BASEPRI_MAX), but do not create any (explicit) memory barrier.
 // Be careful when using this, you must use some method to prevent optimizer form breaking things
@@ -80,7 +87,7 @@ static inline uint8_t __basepriSetRetVal(uint8_t prio)
 // - gcc 4.8.4 does write all values in registers to memory before 'asm volatile', so this optimization does not help much
 //    but that can change in future versions
 #define ATOMIC_BLOCK_NB(prio) for ( uint8_t __basepri_save __attribute__((__cleanup__(__basepriRestore))) = __get_BASEPRI(), \
-                                    __ToDo = __basepriSetRetVal(prio); __ToDo ; __ToDo = 0 ) \
+                                    __ToDo = __basepriSetRetVal((prio) << (8U - __NVIC_PRIO_BITS)); __ToDo ; __ToDo = 0 ) \
 
 #endif // UNIT_TEST
 
diff --git a/src/main/drivers/bus_i2c_hal.c b/src/main/drivers/bus_i2c_hal.c
index 1f8c6f2838..e2dc6436c6 100644
--- a/src/main/drivers/bus_i2c_hal.c
+++ b/src/main/drivers/bus_i2c_hal.c
@@ -317,9 +317,10 @@ void i2cInit(I2CDevice device)
     /* Enable the Analog I2C Filter */
     HAL_I2CEx_ConfigAnalogFilter(&i2cHandle[device].Handle,I2C_ANALOGFILTER_ENABLE);
 
-    HAL_NVIC_SetPriority(i2cHardwareMap[device].er_irq, NVIC_PRIORITY_BASE(NVIC_PRIO_I2C_ER), NVIC_PRIORITY_SUB(NVIC_PRIO_I2C_ER));
+    HAL_NVIC_SetPriority(i2cHardwareMap[device].er_irq, NVIC_PRIO_I2C_ER, 0);
     HAL_NVIC_EnableIRQ(i2cHardwareMap[device].er_irq);
-    HAL_NVIC_SetPriority(i2cHardwareMap[device].ev_irq, NVIC_PRIORITY_BASE(NVIC_PRIO_I2C_EV), NVIC_PRIORITY_SUB(NVIC_PRIO_I2C_EV));
+
+    HAL_NVIC_SetPriority(i2cHardwareMap[device].ev_irq, NVIC_PRIO_I2C_EV, 0);
     HAL_NVIC_EnableIRQ(i2cHardwareMap[device].ev_irq);
     state->initialised = true;
 }
diff --git a/src/main/drivers/dma_stm32f3xx.c b/src/main/drivers/dma_stm32f3xx.c
index 8b19d8dbbb..707cf6e097 100644
--- a/src/main/drivers/dma_stm32f3xx.c
+++ b/src/main/drivers/dma_stm32f3xx.c
@@ -98,18 +98,13 @@ void dmaInit(DMA_t dma, resourceOwner_e owner, uint8_t resourceIndex)
 
 void dmaSetHandler(DMA_t dma, dmaCallbackHandlerFuncPtr callback, uint32_t priority, uint32_t userParam)
 {
-    NVIC_InitTypeDef NVIC_InitStructure;
-
     dmaEnableClock(dma);
 
     dma->irqHandlerCallback = callback;
     dma->userParam = userParam;
 
-    NVIC_InitStructure.NVIC_IRQChannel = dma->irqNumber;
-    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(priority);
-    NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(priority);
-    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
-    NVIC_Init(&NVIC_InitStructure);
+    NVIC_SetPriority(dma->irqNumber, priority);
+    NVIC_EnableIRQ(dma->irqNumber);
 }
 
 DMA_t dmaGetByRef(const DMA_Channel_TypeDef * ref)
diff --git a/src/main/drivers/dma_stm32f4xx.c b/src/main/drivers/dma_stm32f4xx.c
index 85393a99bd..955c70f3fe 100644
--- a/src/main/drivers/dma_stm32f4xx.c
+++ b/src/main/drivers/dma_stm32f4xx.c
@@ -106,18 +106,13 @@ void dmaInit(DMA_t dma, resourceOwner_e owner, uint8_t resourceIndex)
 
 void dmaSetHandler(DMA_t dma, dmaCallbackHandlerFuncPtr callback, uint32_t priority, uint32_t userParam)
 {
-    NVIC_InitTypeDef NVIC_InitStructure;
-
     dmaEnableClock(dma);
 
     dma->irqHandlerCallback = callback;
     dma->userParam = userParam;
 
-    NVIC_InitStructure.NVIC_IRQChannel = dma->irqNumber;
-    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(priority);
-    NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(priority);
-    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
-    NVIC_Init(&NVIC_InitStructure);
+    NVIC_SetPriority(dma->irqNumber, priority);
+    NVIC_EnableIRQ(dma->irqNumber);
 }
 
 uint32_t dmaGetChannelByTag(dmaTag_t tag)
diff --git a/src/main/drivers/dma_stm32f7xx.c b/src/main/drivers/dma_stm32f7xx.c
index b044cc497f..fd8b056c28 100644
--- a/src/main/drivers/dma_stm32f7xx.c
+++ b/src/main/drivers/dma_stm32f7xx.c
@@ -110,7 +110,7 @@ void dmaSetHandler(DMA_t dma, dmaCallbackHandlerFuncPtr callback, uint32_t prior
     dma->irqHandlerCallback = callback;
     dma->userParam = userParam;
 
-    HAL_NVIC_SetPriority(dma->irqNumber, NVIC_PRIORITY_BASE(priority), NVIC_PRIORITY_SUB(priority));
+    HAL_NVIC_SetPriority(dma->irqNumber, priority, 0);
     HAL_NVIC_EnableIRQ(dma->irqNumber);
 }
 
diff --git a/src/main/drivers/exti.c b/src/main/drivers/exti.c
index 16ed467254..9185257e95 100644
--- a/src/main/drivers/exti.c
+++ b/src/main/drivers/exti.c
@@ -90,7 +90,7 @@ void EXTIConfig(IO_t io, extiCallbackRec_t *cb, int irqPriority, ioConfig_t conf
 
     if (extiGroupPriority[group] > irqPriority) {
         extiGroupPriority[group] = irqPriority;
-        HAL_NVIC_SetPriority(extiGroupIRQn[group], NVIC_PRIORITY_BASE(irqPriority), NVIC_PRIORITY_SUB(irqPriority));
+        HAL_NVIC_SetPriority(extiGroupIRQn[group], irqPriority, 0);
         HAL_NVIC_EnableIRQ(extiGroupIRQn[group]);
     }
 }
@@ -131,12 +131,8 @@ void EXTIConfig(IO_t io, extiCallbackRec_t *cb, int irqPriority, EXTITrigger_Typ
     if (extiGroupPriority[group] > irqPriority) {
         extiGroupPriority[group] = irqPriority;
 
-        NVIC_InitTypeDef NVIC_InitStructure;
-        NVIC_InitStructure.NVIC_IRQChannel = extiGroupIRQn[group];
-        NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(irqPriority);
-        NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(irqPriority);
-        NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
-        NVIC_Init(&NVIC_InitStructure);
+        NVIC_SetPriority(extiGroupIRQn[group], irqPriority);
+        NVIC_EnableIRQ(extiGroupIRQn[group]);
     }
 }
 #endif
diff --git a/src/main/drivers/nvic.h b/src/main/drivers/nvic.h
index 52aa554e80..68586b7c08 100644
--- a/src/main/drivers/nvic.h
+++ b/src/main/drivers/nvic.h
@@ -2,43 +2,25 @@
 #pragma once
 
 
-// can't use 0
-#define NVIC_PRIO_MAX                      NVIC_BUILD_PRIORITY(0, 1)
-#define NVIC_PRIO_TIMER                    NVIC_BUILD_PRIORITY(1, 1)
-#define NVIC_PRIO_BARO_EXTI                NVIC_BUILD_PRIORITY(0x0f, 0x0f)
-#define NVIC_PRIO_SONAR_EXTI               NVIC_BUILD_PRIORITY(2, 0)  // maybe increate slightly
-#define NVIC_PRIO_TRANSPONDER_DMA          NVIC_BUILD_PRIORITY(3, 0)
-#define NVIC_PRIO_GYRO_INT_EXTI             NVIC_BUILD_PRIORITY(0x0f, 0x0f)
-#define NVIC_PRIO_MAG_INT_EXTI             NVIC_BUILD_PRIORITY(0x0f, 0x0f)
-#define NVIC_PRIO_WS2811_DMA               NVIC_BUILD_PRIORITY(1, 2)  // TODO - is there some reason to use high priority? (or to use DMA IRQ at all?)
-#define NVIC_PRIO_SERIALUART1              NVIC_BUILD_PRIORITY(1, 1)
-#define NVIC_PRIO_SERIALUART2              NVIC_BUILD_PRIORITY(1, 2)
-#define NVIC_PRIO_SERIALUART3              NVIC_BUILD_PRIORITY(1, 2)
-#define NVIC_PRIO_SERIALUART4              NVIC_BUILD_PRIORITY(1, 2)
-#define NVIC_PRIO_SERIALUART5              NVIC_BUILD_PRIORITY(1, 2)
-#define NVIC_PRIO_SERIALUART6              NVIC_BUILD_PRIORITY(1, 2)
-#define NVIC_PRIO_SERIALUART7              NVIC_BUILD_PRIORITY(1, 2)
-#define NVIC_PRIO_SERIALUART8              NVIC_BUILD_PRIORITY(1, 2)
-#define NVIC_PRIO_I2C_ER                   NVIC_BUILD_PRIORITY(0, 0)
-#define NVIC_PRIO_I2C_EV                   NVIC_BUILD_PRIORITY(0, 0)
-#define NVIC_PRIO_USB                      NVIC_BUILD_PRIORITY(2, 0)
-#define NVIC_PRIO_USB_WUP                  NVIC_BUILD_PRIORITY(1, 0)
-#define NVIC_PRIO_SONAR_ECHO               NVIC_BUILD_PRIORITY(0x0f, 0x0f)
-#define NVIC_PRIO_MPU_DATA_READY           NVIC_BUILD_PRIORITY(0x0f, 0x0f)
-#define NVIC_PRIO_MAG_DATA_READY           NVIC_BUILD_PRIORITY(0x0f, 0x0f)
-#define NVIC_PRIO_CALLBACK                 NVIC_BUILD_PRIORITY(0x0f, 0x0f)
-#define NVIC_PRIO_MAX7456_DMA              NVIC_BUILD_PRIORITY(3, 0)
+// NVIC_SetPriority expects priority encoded according to priority grouping
+// We allocate zero bits for sub-priority, therefore we have 16 priorities to use on STM32
 
+// can't use 0
+#define NVIC_PRIO_MAX                       1
+#define NVIC_PRIO_I2C_ER                    2
+#define NVIC_PRIO_I2C_EV                    2
+#define NVIC_PRIO_TIMER                     3
+#define NVIC_PRIO_TIMER_DMA                 3
+#define NVIC_PRIO_SDIO                      3
+#define NVIC_PRIO_GYRO_INT_EXTI             4
+#define NVIC_PRIO_USB                       5
+#define NVIC_PRIO_SERIALUART                5
+#define NVIC_PRIO_SONAR_EXTI                7
+
+
+// Use all available bits for priority and zero bits to sub-priority
 #ifdef USE_HAL_DRIVER
-// utility macros to join/split priority
-#define NVIC_PRIORITY_GROUPING NVIC_PRIORITYGROUP_2
-#define NVIC_BUILD_PRIORITY(base,sub) (((((base)<<(4-(7-(NVIC_PRIORITY_GROUPING))))|((sub)&(0x0f>>(7-(NVIC_PRIORITY_GROUPING)))))<<4)&0xf0)
-#define NVIC_PRIORITY_BASE(prio) (((prio)>>(4-(7-(NVIC_PRIORITY_GROUPING))))>>4)
-#define NVIC_PRIORITY_SUB(prio) (((prio)&(0x0f>>(7-(NVIC_PRIORITY_GROUPING))))>>4)
+#define NVIC_PRIORITY_GROUPING NVIC_PRIORITYGROUP_4
 #else
-// utility macros to join/split priority
-#define NVIC_PRIORITY_GROUPING NVIC_PriorityGroup_2
-#define NVIC_BUILD_PRIORITY(base,sub) (((((base)<<(4-(7-(NVIC_PRIORITY_GROUPING>>8))))|((sub)&(0x0f>>(7-(NVIC_PRIORITY_GROUPING>>8)))))<<4)&0xf0)
-#define NVIC_PRIORITY_BASE(prio) (((prio)>>(4-(7-(NVIC_PRIORITY_GROUPING>>8))))>>4)
-#define NVIC_PRIORITY_SUB(prio) (((prio)&(0x0f>>(7-(NVIC_PRIORITY_GROUPING>>8))))>>4)
+#define NVIC_PRIORITY_GROUPING NVIC_PriorityGroup_4
 #endif
diff --git a/src/main/drivers/sdcard/sdmmc_sdio_f4xx.c b/src/main/drivers/sdcard/sdmmc_sdio_f4xx.c
index 707b5e83d9..2734de00ad 100644
--- a/src/main/drivers/sdcard/sdmmc_sdio_f4xx.c
+++ b/src/main/drivers/sdcard/sdmmc_sdio_f4xx.c
@@ -1411,12 +1411,8 @@ bool SD_Initialize_LL(DMA_Stream_TypeDef *dmaRef)
     IOConfigGPIOAF(cmd, SDIO_CMD, GPIO_AF_SDIO);
 
     // NVIC configuration for SDIO interrupts
-    NVIC_InitTypeDef NVIC_InitStructure;
-    NVIC_InitStructure.NVIC_IRQChannel = SDIO_IRQn;
-    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(1);
-    NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(0);
-    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
-    NVIC_Init(&NVIC_InitStructure);
+    NVIC_SetPriority(SDIO_IRQn, NVIC_PRIO_SDIO);
+    NVIC_EnableIRQ(SDIO_IRQn);
 
     dma_stream = dmaRef;
     RCC->AHB1ENR |= RCC_AHB1ENR_DMA2EN;
diff --git a/src/main/drivers/sdcard/sdmmc_sdio_f7xx.c b/src/main/drivers/sdcard/sdmmc_sdio_f7xx.c
index 2c941f2ced..d5b11946b2 100644
--- a/src/main/drivers/sdcard/sdmmc_sdio_f7xx.c
+++ b/src/main/drivers/sdcard/sdmmc_sdio_f7xx.c
@@ -1329,10 +1329,8 @@ bool SD_Initialize_LL(DMA_Stream_TypeDef * dmaRef)
     IOInit(cmd, OWNER_SDCARD, RESOURCE_NONE, 0);
     IOConfigGPIOAF(cmd, SDMMC_CMD, GPIO_AF12_SDMMC1);
 
-    uint32_t PriorityGroup = NVIC_GetPriorityGrouping();
-
     // NVIC configuration for SDIO interrupts
-    NVIC_SetPriority(SDMMC1_IRQn, NVIC_EncodePriority(PriorityGroup, 1, 0));
+    NVIC_SetPriority(SDMMC1_IRQn, NVIC_PRIO_SDIO);
     NVIC_EnableIRQ(SDMMC1_IRQn);
 
     dma_stream = dmaRef;
diff --git a/src/main/drivers/serial_uart_stm32f30x.c b/src/main/drivers/serial_uart_stm32f30x.c
index b8d79fd214..5f944fb84d 100644
--- a/src/main/drivers/serial_uart_stm32f30x.c
+++ b/src/main/drivers/serial_uart_stm32f30x.c
@@ -169,7 +169,6 @@ void serialUARTInit(IO_t tx, IO_t rx, portMode_t mode, portOptions_t options, ui
 #ifdef USE_UART1
 uartPort_t *serialUART1(uint32_t baudRate, portMode_t mode, portOptions_t options)
 {
-    NVIC_InitTypeDef NVIC_InitStructure;
     uartPort_t *s;
 
     static volatile uint8_t rx1Buffer[UART1_RX_BUFFER_SIZE];
@@ -191,11 +190,8 @@ uartPort_t *serialUART1(uint32_t baudRate, portMode_t mode, portOptions_t option
 
     serialUARTInit(IOGetByTag(IO_TAG(UART1_TX_PIN)), IOGetByTag(IO_TAG(UART1_RX_PIN)), mode, options, GPIO_AF_7, 1);
 
-    NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQn;
-    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_SERIALUART1);
-    NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_SERIALUART1);
-    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
-    NVIC_Init(&NVIC_InitStructure);
+    NVIC_SetPriority(USART1_IRQn, NVIC_PRIO_SERIALUART);
+    NVIC_EnableIRQ(USART1_IRQn);
 
     return s;
 }
@@ -204,7 +200,6 @@ uartPort_t *serialUART1(uint32_t baudRate, portMode_t mode, portOptions_t option
 #ifdef USE_UART2
 uartPort_t *serialUART2(uint32_t baudRate, portMode_t mode, portOptions_t options)
 {
-    NVIC_InitTypeDef NVIC_InitStructure;
     uartPort_t *s;
 
     static volatile uint8_t rx2Buffer[UART2_RX_BUFFER_SIZE];
@@ -226,11 +221,8 @@ uartPort_t *serialUART2(uint32_t baudRate, portMode_t mode, portOptions_t option
 
     serialUARTInit(IOGetByTag(IO_TAG(UART2_TX_PIN)), IOGetByTag(IO_TAG(UART2_RX_PIN)), mode, options, GPIO_AF_7, 2);
 
-    NVIC_InitStructure.NVIC_IRQChannel = USART2_IRQn;
-    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_SERIALUART2);
-    NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_SERIALUART2);
-    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
-    NVIC_Init(&NVIC_InitStructure);
+    NVIC_SetPriority(USART2_IRQn, NVIC_PRIO_SERIALUART);
+    NVIC_EnableIRQ(USART2_IRQn);
 
     return s;
 }
@@ -239,7 +231,6 @@ uartPort_t *serialUART2(uint32_t baudRate, portMode_t mode, portOptions_t option
 #ifdef USE_UART3
 uartPort_t *serialUART3(uint32_t baudRate, portMode_t mode, portOptions_t options)
 {
-    NVIC_InitTypeDef NVIC_InitStructure;
     uartPort_t *s;
 
     static volatile uint8_t rx3Buffer[UART3_RX_BUFFER_SIZE];
@@ -261,11 +252,8 @@ uartPort_t *serialUART3(uint32_t baudRate, portMode_t mode, portOptions_t option
 
     serialUARTInit(IOGetByTag(IO_TAG(UART3_TX_PIN)), IOGetByTag(IO_TAG(UART3_RX_PIN)), mode, options, GPIO_AF_7, 3);
 
-    NVIC_InitStructure.NVIC_IRQChannel = USART3_IRQn;
-    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_SERIALUART3);
-    NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_SERIALUART3);
-    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
-    NVIC_Init(&NVIC_InitStructure);
+    NVIC_SetPriority(USART3_IRQn, NVIC_PRIO_SERIALUART);
+    NVIC_EnableIRQ(USART3_IRQn);
 
     return s;
 }
@@ -277,7 +265,6 @@ uartPort_t *serialUART4(uint32_t baudRate, portMode_t mode, portOptions_t option
     uartPort_t *s;
     static volatile uint8_t rx4Buffer[UART4_RX_BUFFER_SIZE];
     static volatile uint8_t tx4Buffer[UART4_TX_BUFFER_SIZE];
-    NVIC_InitTypeDef NVIC_InitStructure;
 
     s = &uartPort4;
     s->port.vTable = uartVTable;
@@ -295,11 +282,8 @@ uartPort_t *serialUART4(uint32_t baudRate, portMode_t mode, portOptions_t option
 
     serialUARTInit(IOGetByTag(IO_TAG(UART4_TX_PIN)), IOGetByTag(IO_TAG(UART4_RX_PIN)), mode, options, GPIO_AF_5, 4);
 
-    NVIC_InitStructure.NVIC_IRQChannel = UART4_IRQn;
-    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_SERIALUART4);
-    NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_SERIALUART4);
-    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
-    NVIC_Init(&NVIC_InitStructure);
+    NVIC_SetPriority(UART4_IRQn, NVIC_PRIO_SERIALUART);
+    NVIC_EnableIRQ(UART4_IRQn);
 
     return s;
 }
@@ -311,7 +295,6 @@ uartPort_t *serialUART5(uint32_t baudRate, portMode_t mode, portOptions_t option
     uartPort_t *s;
     static volatile uint8_t rx5Buffer[UART5_RX_BUFFER_SIZE];
     static volatile uint8_t tx5Buffer[UART5_TX_BUFFER_SIZE];
-    NVIC_InitTypeDef NVIC_InitStructure;
 
     s = &uartPort5;
     s->port.vTable = uartVTable;
@@ -329,11 +312,8 @@ uartPort_t *serialUART5(uint32_t baudRate, portMode_t mode, portOptions_t option
 
     serialUARTInit(IOGetByTag(IO_TAG(UART5_TX_PIN)), IOGetByTag(IO_TAG(UART5_RX_PIN)), mode, options, GPIO_AF_5, 5);
 
-    NVIC_InitStructure.NVIC_IRQChannel = UART5_IRQn;
-    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_SERIALUART5);
-    NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_SERIALUART5);
-    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
-    NVIC_Init(&NVIC_InitStructure);
+    NVIC_SetPriority(UART5_IRQn, NVIC_PRIO_SERIALUART);
+    NVIC_EnableIRQ(UART5_IRQn);
 
     return s;
 }
diff --git a/src/main/drivers/serial_uart_stm32f4xx.c b/src/main/drivers/serial_uart_stm32f4xx.c
index dc9f0ddc50..2b310ec575 100644
--- a/src/main/drivers/serial_uart_stm32f4xx.c
+++ b/src/main/drivers/serial_uart_stm32f4xx.c
@@ -60,7 +60,7 @@ static uartDevice_t uart1 =
 #endif
     .rcc_apb2 = RCC_APB2(USART1),
     .irq = USART1_IRQn,
-    .irqPriority = NVIC_PRIO_SERIALUART1
+    .irqPriority = NVIC_PRIO_SERIALUART
 };
 #endif
 
@@ -76,7 +76,7 @@ static uartDevice_t uart2 =
 #endif
     .rcc_apb1 = RCC_APB1(USART2),
     .irq = USART2_IRQn,
-    .irqPriority = NVIC_PRIO_SERIALUART2
+    .irqPriority = NVIC_PRIO_SERIALUART
 };
 #endif
 
@@ -92,7 +92,7 @@ static uartDevice_t uart3 =
 #endif
     .rcc_apb1 = RCC_APB1(USART3),
     .irq = USART3_IRQn,
-    .irqPriority = NVIC_PRIO_SERIALUART3
+    .irqPriority = NVIC_PRIO_SERIALUART
 };
 #endif
 
@@ -108,7 +108,7 @@ static uartDevice_t uart4 =
 #endif
     .rcc_apb1 = RCC_APB1(UART4),
     .irq = UART4_IRQn,
-    .irqPriority = NVIC_PRIO_SERIALUART4
+    .irqPriority = NVIC_PRIO_SERIALUART
 };
 #endif
 
@@ -124,7 +124,7 @@ static uartDevice_t uart5 =
 #endif
     .rcc_apb1 = RCC_APB1(UART5),
     .irq = UART5_IRQn,
-    .irqPriority = NVIC_PRIO_SERIALUART5
+    .irqPriority = NVIC_PRIO_SERIALUART
 };
 #endif
 
@@ -140,7 +140,7 @@ static uartDevice_t uart6 =
 #endif
     .rcc_apb2 = RCC_APB2(USART6),
     .irq = USART6_IRQn,
-    .irqPriority = NVIC_PRIO_SERIALUART6
+    .irqPriority = NVIC_PRIO_SERIALUART
 };
 #endif
 
@@ -153,7 +153,7 @@ static uartDevice_t uart7 =
     .af = GPIO_AF_UART7,
     .rcc_apb1 = RCC_APB1(UART7),
     .irq = UART7_IRQn,
-    .irqPriority = NVIC_PRIO_SERIALUART7
+    .irqPriority = NVIC_PRIO_SERIALUART
 };
 #endif
 
@@ -166,7 +166,7 @@ static uartDevice_t uart8 =
     .af = GPIO_AF_UART8,
     .rcc_apb1 = RCC_APB1(UART8),
     .irq = UART8_IRQn,
-    .irqPriority = NVIC_PRIO_SERIALUART8
+    .irqPriority = NVIC_PRIO_SERIALUART
 };
 #endif
 
@@ -256,7 +256,6 @@ void uartGetPortPins(UARTDevice_e device, serialPortPins_t * pins)
 uartPort_t *serialUART(UARTDevice_e device, uint32_t baudRate, portMode_t mode, portOptions_t options)
 {
     uartPort_t *s;
-    NVIC_InitTypeDef NVIC_InitStructure;
 
     uartDevice_t *uart = uartHardwareMap[device];
     if (!uart) return NULL;
@@ -304,11 +303,8 @@ uartPort_t *serialUART(UARTDevice_e device, uint32_t baudRate, portMode_t mode,
         }
     }
 
-    NVIC_InitStructure.NVIC_IRQChannel = uart->irq;
-    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(uart->irqPriority);
-    NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(uart->irqPriority);
-    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
-    NVIC_Init(&NVIC_InitStructure);
+    NVIC_SetPriority(uart->irq, uart->irqPriority);
+    NVIC_EnableIRQ(uart->irq);
 
     return s;
 }
diff --git a/src/main/drivers/serial_uart_stm32f7xx.c b/src/main/drivers/serial_uart_stm32f7xx.c
index bf22467f24..baab69b087 100644
--- a/src/main/drivers/serial_uart_stm32f7xx.c
+++ b/src/main/drivers/serial_uart_stm32f7xx.c
@@ -64,7 +64,7 @@ static uartDevice_t uart1 =
 #endif
     .rcc_apb2 = RCC_APB2(USART1),
     .irq = USART1_IRQn,
-    .irqPriority = NVIC_PRIO_SERIALUART1
+    .irqPriority = NVIC_PRIO_SERIALUART
 };
 #endif
 
@@ -84,7 +84,7 @@ static uartDevice_t uart2 =
 #endif
     .rcc_apb1 = RCC_APB1(USART2),
     .irq = USART2_IRQn,
-    .irqPriority = NVIC_PRIO_SERIALUART2
+    .irqPriority = NVIC_PRIO_SERIALUART
 };
 #endif
 
@@ -104,7 +104,7 @@ static uartDevice_t uart3 =
 #endif
     .rcc_apb1 = RCC_APB1(USART3),
     .irq = USART3_IRQn,
-    .irqPriority = NVIC_PRIO_SERIALUART3
+    .irqPriority = NVIC_PRIO_SERIALUART
 };
 #endif
 
@@ -124,7 +124,7 @@ static uartDevice_t uart4 =
 #endif
     .rcc_apb1 = RCC_APB1(UART4),
     .irq = UART4_IRQn,
-    .irqPriority = NVIC_PRIO_SERIALUART4
+    .irqPriority = NVIC_PRIO_SERIALUART
 };
 #endif
 
@@ -144,7 +144,7 @@ static uartDevice_t uart5 =
 #endif
     .rcc_apb1 = RCC_APB1(UART5),
     .irq = UART5_IRQn,
-    .irqPriority = NVIC_PRIO_SERIALUART5
+    .irqPriority = NVIC_PRIO_SERIALUART
 };
 #endif
 
@@ -164,7 +164,7 @@ static uartDevice_t uart6 =
 #endif
     .rcc_apb2 = RCC_APB2(USART6),
     .irq = USART6_IRQn,
-    .irqPriority = NVIC_PRIO_SERIALUART6
+    .irqPriority = NVIC_PRIO_SERIALUART
 };
 #endif
 
@@ -184,7 +184,7 @@ static uartDevice_t uart7 =
 #endif
     .rcc_apb1 = RCC_APB1(UART7),
     .irq = UART7_IRQn,
-    .irqPriority = NVIC_PRIO_SERIALUART7
+    .irqPriority = NVIC_PRIO_SERIALUART
 };
 #endif
 #ifdef USE_UART8
@@ -203,7 +203,7 @@ static uartDevice_t uart8 =
 #endif
     .rcc_apb1 = RCC_APB1(UART8),
     .irq = UART8_IRQn,
-    .irqPriority = NVIC_PRIO_SERIALUART8
+    .irqPriority = NVIC_PRIO_SERIALUART
 };
 #endif
 
@@ -372,7 +372,7 @@ uartPort_t *serialUART(UARTDevice_e device, uint32_t baudRate, portMode_t mode,
         }
     }
 
-    HAL_NVIC_SetPriority(uart->irq, NVIC_PRIORITY_BASE(uart->irqPriority), NVIC_PRIORITY_SUB(uart->irqPriority));
+    HAL_NVIC_SetPriority(uart->irq, uart->irqPriority, 0);
     HAL_NVIC_EnableIRQ(uart->irq);
 
     return s;
diff --git a/src/main/drivers/timer_impl_hal.c b/src/main/drivers/timer_impl_hal.c
index a4eb82941d..9f855217ee 100644
--- a/src/main/drivers/timer_impl_hal.c
+++ b/src/main/drivers/timer_impl_hal.c
@@ -62,12 +62,12 @@ void impl_timerInitContext(timHardwareContext_t * timCtx)
 void impl_timerNVICConfigure(TCH_t * tch, int irqPriority)
 {
     if (tch->timCtx->timDef->irq) {
-        HAL_NVIC_SetPriority(tch->timCtx->timDef->irq, NVIC_PRIORITY_BASE(irqPriority), NVIC_PRIORITY_SUB(irqPriority));
+        HAL_NVIC_SetPriority(tch->timCtx->timDef->irq, irqPriority, 0);
         HAL_NVIC_EnableIRQ(tch->timCtx->timDef->irq);
     }
 
     if (tch->timCtx->timDef->secondIrq) {
-        HAL_NVIC_SetPriority(tch->timCtx->timDef->secondIrq, NVIC_PRIORITY_BASE(irqPriority), NVIC_PRIORITY_SUB(irqPriority));
+        HAL_NVIC_SetPriority(tch->timCtx->timDef->secondIrq, irqPriority, 0);
         HAL_NVIC_EnableIRQ(tch->timCtx->timDef->secondIrq);
     }
 }
@@ -380,7 +380,7 @@ bool impl_timerPWMConfigChannelDMA(TCH_t * tch, void * dmaBuffer, uint8_t dmaBuf
     init.PeriphBurst = LL_DMA_PBURST_SINGLE;
 
     dmaInit(tch->dma, OWNER_TIMER, 0);
-    dmaSetHandler(tch->dma, impl_timerDMA_IRQHandler, NVIC_PRIO_WS2811_DMA, (uint32_t)tch);
+    dmaSetHandler(tch->dma, impl_timerDMA_IRQHandler, NVIC_PRIO_TIMER_DMA, (uint32_t)tch);
 
     LL_DMA_Init(tch->dma->dma, streamLL, &init);
 
diff --git a/src/main/drivers/timer_impl_stdperiph.c b/src/main/drivers/timer_impl_stdperiph.c
index 4dd428ebc5..599b8b5cb3 100644
--- a/src/main/drivers/timer_impl_stdperiph.c
+++ b/src/main/drivers/timer_impl_stdperiph.c
@@ -44,20 +44,14 @@ void impl_timerInitContext(timHardwareContext_t * timCtx)
 
 void impl_timerNVICConfigure(TCH_t * tch, int irqPriority)
 {
-    NVIC_InitTypeDef NVIC_InitStructure;
-
-    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(irqPriority);
-    NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(irqPriority);
-    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
-
     if (tch->timCtx->timDef->irq) {
-        NVIC_InitStructure.NVIC_IRQChannel = tch->timCtx->timDef->irq;
-        NVIC_Init(&NVIC_InitStructure);
+        NVIC_SetPriority(tch->timCtx->timDef->irq, irqPriority);
+        NVIC_EnableIRQ(tch->timCtx->timDef->irq);
     }
 
     if (tch->timCtx->timDef->secondIrq) {
-        NVIC_InitStructure.NVIC_IRQChannel = tch->timCtx->timDef->secondIrq;
-        NVIC_Init(&NVIC_InitStructure);
+        NVIC_SetPriority(tch->timCtx->timDef->secondIrq, irqPriority);
+        NVIC_EnableIRQ(tch->timCtx->timDef->secondIrq);
     }
 }
 
@@ -311,7 +305,7 @@ bool impl_timerPWMConfigChannelDMA(TCH_t * tch, void * dmaBuffer, uint8_t dmaBuf
     TIM_Cmd(timer, ENABLE);
 
     dmaInit(tch->dma, OWNER_TIMER, 0);
-    dmaSetHandler(tch->dma, impl_timerDMA_IRQHandler, NVIC_PRIO_WS2811_DMA, (uint32_t)tch);
+    dmaSetHandler(tch->dma, impl_timerDMA_IRQHandler, NVIC_PRIO_TIMER_DMA, (uint32_t)tch);
 
     DMA_DeInit(tch->dma->ref);
     DMA_Cmd(tch->dma->ref, DISABLE);
diff --git a/src/main/drivers/usb_msc_f4xx.c b/src/main/drivers/usb_msc_f4xx.c
index 2b9a7f847b..f0fb050061 100644
--- a/src/main/drivers/usb_msc_f4xx.c
+++ b/src/main/drivers/usb_msc_f4xx.c
@@ -112,7 +112,7 @@ uint8_t mscStart(void)
 
     // NVIC configuration for SYSTick
     NVIC_DisableIRQ(SysTick_IRQn);
-    NVIC_SetPriority(SysTick_IRQn, NVIC_BUILD_PRIORITY(0, 0));
+    NVIC_SetPriority(SysTick_IRQn, 0);
     NVIC_EnableIRQ(SysTick_IRQn);
 
     return 0;
diff --git a/src/main/drivers/usb_msc_f7xx.c b/src/main/drivers/usb_msc_f7xx.c
index bf56fd7564..41877a3362 100644
--- a/src/main/drivers/usb_msc_f7xx.c
+++ b/src/main/drivers/usb_msc_f7xx.c
@@ -110,7 +110,7 @@ uint8_t mscStart(void)
 
     // NVIC configuration for SYSTick
     NVIC_DisableIRQ(SysTick_IRQn);
-    NVIC_SetPriority(SysTick_IRQn, NVIC_BUILD_PRIORITY(0, 0));
+    NVIC_SetPriority(SysTick_IRQn, 0);
     NVIC_EnableIRQ(SysTick_IRQn);
 
     return 0;
diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c
index b17809bdd1..4f8f210707 100755
--- a/src/main/telemetry/crsf.c
+++ b/src/main/telemetry/crsf.c
@@ -119,7 +119,7 @@ bool handleCrsfMspFrameBuffer(uint8_t payloadSize, mspResponseFnPtr responseFn)
             requestHandled |= sendMspReply(payloadSize, responseFn);
         }
         pos += CRSF_MSP_LENGTH_OFFSET + mspFrameLength;
-        ATOMIC_BLOCK(NVIC_PRIO_SERIALUART1) {
+        ATOMIC_BLOCK(NVIC_PRIO_SERIALUART) {
             if (pos >= mspRxBuffer.len) {
                 mspRxBuffer.len = 0;
                 return requestHandled;
diff --git a/src/main/vcp/hw_config.c b/src/main/vcp/hw_config.c
index 27dbd11c2e..4ab0d8dad4 100644
--- a/src/main/vcp/hw_config.c
+++ b/src/main/vcp/hw_config.c
@@ -194,23 +194,11 @@ void Leave_LowPowerMode(void)
  *******************************************************************************/
 void USB_Interrupts_Config(void)
 {
-    NVIC_InitTypeDef NVIC_InitStructure;
+    NVIC_SetPriority(USB_LP_CAN1_RX0_IRQn, NVIC_PRIO_USB);
+    NVIC_EnableIRQ(USB_LP_CAN1_RX0_IRQn);
 
-    /* 2 bit for pre-emption priority, 2 bits for subpriority */
-    NVIC_PriorityGroupConfig(NVIC_PRIORITY_GROUPING);     // is this really neccesary?
-
-    /* Enable the USB interrupt */
-    NVIC_InitStructure.NVIC_IRQChannel = USB_LP_CAN1_RX0_IRQn;
-    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_USB);
-    NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_USB);
-    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
-    NVIC_Init(&NVIC_InitStructure);
-
-    /* Enable the USB Wake-up interrupt */
-    NVIC_InitStructure.NVIC_IRQChannel = USBWakeUp_IRQn;
-    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_USB_WUP);
-    NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_USB_WUP);
-    NVIC_Init(&NVIC_InitStructure);
+    NVIC_SetPriority(USBWakeUp_IRQn, NVIC_PRIO_USB);
+    NVIC_EnableIRQ(USBWakeUp_IRQn);
 }
 
 /*******************************************************************************
diff --git a/src/main/vcpf4/usb_bsp.c b/src/main/vcpf4/usb_bsp.c
index 1160c47d8a..c9f49fecb9 100644
--- a/src/main/vcpf4/usb_bsp.c
+++ b/src/main/vcpf4/usb_bsp.c
@@ -49,23 +49,14 @@ void USB_OTG_BSP_Init(USB_OTG_CORE_HANDLE *pdev)
     (void)pdev;
     GPIO_InitTypeDef GPIO_InitStructure;
 
-#ifndef USE_ULPI_PHY
-#ifdef USB_OTG_FS_LOW_PWR_MGMT_SUPPORT
-    EXTI_InitTypeDef EXTI_InitStructure;
-    NVIC_InitTypeDef NVIC_InitStructure;
-#endif
-#endif
 
-    NVIC_InitTypeDef NVIC_InitStructure;
 #ifdef USE_USB_OTG_HS
-    NVIC_InitStructure.NVIC_IRQChannel = OTG_HS_IRQn;
+    NVIC_SetPriority(OTG_HS_IRQn, NVIC_PRIO_USB);
+    NVIC_DisableIRQ(OTG_HS_IRQn);
 #else
-    NVIC_InitStructure.NVIC_IRQChannel = OTG_FS_IRQn;
+    NVIC_SetPriority(OTG_FS_IRQn, NVIC_PRIO_USB);
+    NVIC_DisableIRQ(OTG_FS_IRQn);
 #endif
-    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_USB);
-    NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_USB);
-    NVIC_InitStructure.NVIC_IRQChannelCmd = DISABLE;
-    NVIC_Init(&NVIC_InitStructure);
 
     RCC_AHB1PeriphClockCmd( RCC_AHB1Periph_GPIOA , ENABLE);
 
@@ -101,32 +92,13 @@ void USB_OTG_BSP_Init(USB_OTG_CORE_HANDLE *pdev)
 void USB_OTG_BSP_EnableInterrupt(USB_OTG_CORE_HANDLE *pdev)
 {
     (void)pdev;
-    NVIC_InitTypeDef NVIC_InitStructure;
 
-    NVIC_PriorityGroupConfig(NVIC_PRIORITY_GROUPING);
 #ifdef USE_USB_OTG_HS
-    NVIC_InitStructure.NVIC_IRQChannel = OTG_HS_IRQn;
+    NVIC_SetPriority(OTG_HS_IRQn, NVIC_PRIO_USB);
+    NVIC_EnableIRQ(OTG_HS_IRQn);
 #else
-    NVIC_InitStructure.NVIC_IRQChannel = OTG_FS_IRQn;
-#endif
-    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_USB);
-    NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_USB);
-    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
-    NVIC_Init(&NVIC_InitStructure);
-#ifdef USB_OTG_HS_DEDICATED_EP1_ENABLED
-    NVIC_PriorityGroupConfig(NVIC_PriorityGroup_1);
-    NVIC_InitStructure.NVIC_IRQChannel = OTG_HS_EP1_OUT_IRQn;
-    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
-    NVIC_InitStructure.NVIC_IRQChannelSubPriority = 2;
-    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
-    NVIC_Init(&NVIC_InitStructure);
-
-    NVIC_PriorityGroupConfig(NVIC_PriorityGroup_1);
-    NVIC_InitStructure.NVIC_IRQChannel = OTG_HS_EP1_IN_IRQn;
-    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
-    NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
-    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
-    NVIC_Init(&NVIC_InitStructure);
+    NVIC_SetPriority(OTG_FS_IRQn, NVIC_PRIO_USB);
+    NVIC_EnableIRQ(OTG_FS_IRQn);
 #endif
 }
 /**

From eb2b9d7947f423979b39a09f596edd9ec5db046f Mon Sep 17 00:00:00 2001
From: "Konstantin (DigitalEntity) Sharlaimov"
 <konstantin.sharlaimov@gmail.com>
Date: Mon, 30 Mar 2020 21:29:24 +0200
Subject: [PATCH 063/179] [UTIL] Remove dead code from atomic.h

---
 src/main/build/atomic.h | 77 -----------------------------------------
 1 file changed, 77 deletions(-)

diff --git a/src/main/build/atomic.h b/src/main/build/atomic.h
index ed11eb42f2..5fac4482a8 100644
--- a/src/main/build/atomic.h
+++ b/src/main/build/atomic.h
@@ -17,25 +17,9 @@
 
 #pragma once
 
-// only set_BASEPRI is implemented in device library. It does always create memory barrier
-// missing versions are implemented here
-
 #ifdef UNIT_TEST
 static inline void __set_BASEPRI(uint32_t basePri) {(void)basePri;}
 static inline void __set_BASEPRI_MAX(uint32_t basePri) {(void)basePri;}
-static inline void __set_BASEPRI_nb(uint32_t basePri) {(void)basePri;}
-static inline void __set_BASEPRI_MAX_nb(uint32_t basePri) {(void)basePri;}
-#else
-// set BASEPRI and BASEPRI_MAX register, but do not create memory barrier
-__attribute__( ( always_inline ) ) static inline void __set_BASEPRI_nb(uint32_t basePri)
-{
-   __ASM volatile ("\tMSR basepri, %0\n" : : "r" (basePri) );
-}
-
-__attribute__( ( always_inline ) ) static inline void __set_BASEPRI_MAX_nb(uint32_t basePri)
-{
-   __ASM volatile ("\tMSR basepri_max, %0\n" : : "r" (basePri) );
-}
 #endif // UNIT_TEST
 
 // cleanup BASEPRI restore function, with global memory barrier
@@ -51,19 +35,6 @@ static inline uint8_t __basepriSetMemRetVal(uint8_t prio)
     return 1;
 }
 
-// cleanup BASEPRI restore function, no memory barrier
-static inline void __basepriRestore(uint8_t *val)
-{
-    __set_BASEPRI_nb(*val);
-}
-
-// set BASEPRI_MAX function, no memory barrier, returns true
-static inline uint8_t __basepriSetRetVal(uint8_t prio)
-{
-    __set_BASEPRI_MAX_nb(prio);
-    return 1;
-}
-
 // The CMSIS provides the function __set_BASEPRI(priority) for changing the value of the BASEPRI register.
 // The function uses the hardware convention for the ‘priority’ argument, which means that the priority must
 // be shifted left by the number of unimplemented bits (8 – __NVIC_PRIO_BITS).
@@ -73,53 +44,5 @@ static inline uint8_t __basepriSetRetVal(uint8_t prio)
 
 // Run block with elevated BASEPRI (using BASEPRI_MAX), restoring BASEPRI on exit. All exit paths are handled
 // Full memory barrier is placed at start and exit of block
-#ifdef UNIT_TEST
-#define ATOMIC_BLOCK(prio) {}
-#define ATOMIC_BLOCK_NB(prio) {}
-#else
 #define ATOMIC_BLOCK(prio) for ( uint8_t __basepri_save __attribute__((__cleanup__(__basepriRestoreMem))) = __get_BASEPRI(), \
                                      __ToDo = __basepriSetMemRetVal((prio) << (8U - __NVIC_PRIO_BITS)); __ToDo ; __ToDo = 0 )
-
-// Run block with elevated BASEPRI (using BASEPRI_MAX), but do not create any (explicit) memory barrier.
-// Be careful when using this, you must use some method to prevent optimizer form breaking things
-// - lto is used for Cleanflight compilation, so function call is not memory barrier
-// - use ATOMIC_BARRIER or proper volatile to protect used variables
-// - gcc 4.8.4 does write all values in registers to memory before 'asm volatile', so this optimization does not help much
-//    but that can change in future versions
-#define ATOMIC_BLOCK_NB(prio) for ( uint8_t __basepri_save __attribute__((__cleanup__(__basepriRestore))) = __get_BASEPRI(), \
-                                    __ToDo = __basepriSetRetVal((prio) << (8U - __NVIC_PRIO_BITS)); __ToDo ; __ToDo = 0 ) \
-
-#endif // UNIT_TEST
-
-// ATOMIC_BARRIER
-// Create memory barrier
-// - at the beginning (all data must be reread from memory)
-// - at exit of block (all exit paths) (all data must be written, but may be cached in register for subsequent use)
-// ideally this would only protect memory passed as parameter (any type should work), but gcc is currently creating almost full barrier
-// this macro can be used only ONCE PER LINE, but multiple uses per block are fine
-
-#if (__GNUC__ > 9)
-#warning "Please verify that ATOMIC_BARRIER works as intended"
-// increment version number is BARRIER works
-// TODO - use flag to disable ATOMIC_BARRIER and use full barrier instead
-// you should check that local variable scope with cleanup spans entire block
-#endif
-
-#ifndef __UNIQL
-# define __UNIQL_CONCAT2(x,y) x ## y
-# define __UNIQL_CONCAT(x,y) __UNIQL_CONCAT2(x,y)
-# define __UNIQL(x) __UNIQL_CONCAT(x,__LINE__)
-#endif
-
-// this macro uses local function for cleanup. CLang block can be substituted
-#define ATOMIC_BARRIER(data)                                            \
-    __extension__ void  __UNIQL(__barrierEnd)(typeof(data) **__d) {     \
-        __asm__ volatile ("\t# barier(" #data ")  end\n" : : "m" (**__d));                          \
-    }                                                                   \
-    typeof(data)  __attribute__((__cleanup__(__UNIQL(__barrierEnd)))) *__UNIQL(__barrier) = &data; \
-    __asm__ volatile ("\t# barier (" #data ") start\n" : "+m" (*__UNIQL(__barrier)))
-
-
-// define these wrappers for atomic operations, use gcc buildins
-#define ATOMIC_OR(ptr, val) __sync_fetch_and_or(ptr, val)
-#define ATOMIC_AND(ptr, val) __sync_fetch_and_and(ptr, val)

From 36b06a23c5d41fb2b52572ab59772407564b58d2 Mon Sep 17 00:00:00 2001
From: "Konstantin (DigitalEntity) Sharlaimov"
 <konstantin.sharlaimov@gmail.com>
Date: Tue, 31 Mar 2020 10:16:34 +0200
Subject: [PATCH 064/179] Fix tests

---
 src/main/build/atomic.h | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/src/main/build/atomic.h b/src/main/build/atomic.h
index 5fac4482a8..53c165d3c6 100644
--- a/src/main/build/atomic.h
+++ b/src/main/build/atomic.h
@@ -44,5 +44,10 @@ static inline uint8_t __basepriSetMemRetVal(uint8_t prio)
 
 // Run block with elevated BASEPRI (using BASEPRI_MAX), restoring BASEPRI on exit. All exit paths are handled
 // Full memory barrier is placed at start and exit of block
+#ifdef UNIT_TEST
+#define ATOMIC_BLOCK(prio) {}
+#else
 #define ATOMIC_BLOCK(prio) for ( uint8_t __basepri_save __attribute__((__cleanup__(__basepriRestoreMem))) = __get_BASEPRI(), \
                                      __ToDo = __basepriSetMemRetVal((prio) << (8U - __NVIC_PRIO_BITS)); __ToDo ; __ToDo = 0 )
+
+#endif // UNIT_TEST

From 489d2af639498fc12604fab98db8b22445d88628 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Alberto=20Garci=CC=81a=20Hierro?= <alberto@garciahierro.com>
Date: Mon, 30 Mar 2020 20:51:59 +0100
Subject: [PATCH 065/179] [CI] Trigger CI on PR only

There's no way to trigger CI on push OR PR, since the CI build
on push is triggered immediately and when the PR is created the
push run is already underway.

Also, make some cosmetic changes to the artifact name to include
the date, inav version and git revision.
---
 .github/workflows/ci.yml | 24 ++++++++++++++++--------
 Makefile                 | 15 +--------------
 make/tools.mk            |  2 +-
 make/version.mk          | 18 ++++++++++++++++++
 4 files changed, 36 insertions(+), 23 deletions(-)
 create mode 100644 make/version.mk

diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml
index f445c58293..f8dddb668b 100644
--- a/.github/workflows/ci.yml
+++ b/.github/workflows/ci.yml
@@ -1,10 +1,8 @@
 name: Build firmware
-on:
-  push:
-    branches-ignore:
-      - master
-      - development
-  pull_request:
+# Don't enable CI on push, just on PR. If you
+# are working on the main repo and want to trigger
+# a CI build submit a draft PR.
+on: pull_request
 
 jobs:
   # This downloads the GNU toolchain from ARM once, then
@@ -38,8 +36,18 @@ jobs:
         - uses: actions/checkout@v2
         - name: Setup environment
           run: |
+            # This is the hash of the commit for the PR
+            # when the action is triggered by PR, empty otherwise
+            COMMIT_ID=${{ github.event.pull_request.head.sha }}
+            # This is the hash of the commit when triggered by push
+            # but the hash of refs/pull/<n>/merge, which is different
+            # from the hash of the latest commit in the PR, that's
+            # why we try github.event.pull_request.head.sha first
+            COMMIT_ID=${COMMIT_ID:-${{ github.sha }}}
+            BUILD_SUFFIX=ci-$(date '+%Y%m%d')-$(git rev-parse --short ${COMMIT_ID})
             echo "::set-env name=TARGETS::$(./src/utils/build-targets.sh -n -s ${{ matrix.start }} -c ${{ matrix.count }})"
-            echo "::set-env name=BUILD_SUFFIX::ci-$(git rev-parse --short ${{ github.ref }})"
+            echo "::set-env name=BUILD_SUFFIX::${BUILD_SUFFIX}"
+            echo "::set-env name=BUILD_NAME::inav-$(make print_version)-${BUILD_SUFFIX}"
             echo "::set-env name=IS_LAST_JOB::$([ $(expr ${{ strategy.job-index }} + 1) = ${{ strategy.job-total }} ] && echo yes)"
         - name: Ensure all targets will be tested
           if: ${{ env.IS_LAST_JOB }}
@@ -70,7 +78,7 @@ jobs:
           if: ${{ env.TARGETS }}
           uses: actions/upload-artifact@v2-preview
           with:
-            name: inav-${{ env.BUILD_SUFFIX }}.zip
+            name: ${{ env.BUILD_NAME }}.zip
             path: ./obj/*.hex
 
   test:
diff --git a/Makefile b/Makefile
index 6fa7ba763c..aa20153cca 100644
--- a/Makefile
+++ b/Makefile
@@ -64,8 +64,6 @@ endif
 # Things that need to be maintained as the source changes
 #
 
-FORKNAME      = inav
-
 # Working directories
 SRC_DIR         := $(ROOT)/src/main
 OBJECT_DIR      := $(ROOT)/obj/main
@@ -85,17 +83,9 @@ MHZ_VALUE      ?=
 # used for turning on features like VCP and SDCARD
 FEATURES        =
 
+include $(ROOT)/make/version.mk
 include $(ROOT)/make/targets.mk
 
-REVISION = $(shell git rev-parse --short HEAD)
-
-FC_VER_MAJOR := $(shell grep " FC_VERSION_MAJOR" src/main/build/version.h | awk '{print $$3}' )
-FC_VER_MINOR := $(shell grep " FC_VERSION_MINOR" src/main/build/version.h | awk '{print $$3}' )
-FC_VER_PATCH := $(shell grep " FC_VERSION_PATCH" src/main/build/version.h | awk '{print $$3}' )
-
-FC_VER := $(FC_VER_MAJOR).$(FC_VER_MINOR).$(FC_VER_PATCH)
-FC_VER_SUFFIX ?=
-
 BUILD_DATE = $(shell date +%Y%m%d)
 
 # Search path for sources
@@ -255,9 +245,6 @@ CPPCHECK        = cppcheck $(CSOURCES) --enable=all --platform=unix64 \
 # Things we will build
 #
 TARGET_BIN	:= $(BIN_DIR)/$(FORKNAME)_$(FC_VER)
-ifneq ($(FC_VER_SUFFIX),)
-    TARGET_BIN	:= $(TARGET_BIN)-$(FC_VER_SUFFIX)
-endif
 TARGET_BIN	:= $(TARGET_BIN)_$(TARGET)
 ifneq ($(BUILD_SUFFIX),)
     TARGET_BIN	:= $(TARGET_BIN)_$(BUILD_SUFFIX)
diff --git a/make/tools.mk b/make/tools.mk
index e24bf1bfed..dd56b7a286 100644
--- a/make/tools.mk
+++ b/make/tools.mk
@@ -80,7 +80,7 @@ arm_sdk_clean:
 
 ifeq ($(shell [ -d "$(ARM_SDK_DIR)" ] && echo "exists"), exists)
   ARM_SDK_PREFIX := $(ARM_SDK_DIR)/bin/arm-none-eabi-
-else ifeq (,$(filter targets,$(MAKECMDGOALS))$(findstring arm_sdk,$(MAKECMDGOALS)))
+else ifeq (,$(findstring print_,$(MAKECMDGOALS))$(filter targets,$(MAKECMDGOALS))$(findstring arm_sdk,$(MAKECMDGOALS)))
   GCC_VERSION = $(shell arm-none-eabi-gcc -dumpversion)
   ifeq ($(GCC_VERSION),)
     $(error **ERROR** arm-none-eabi-gcc not in the PATH. Run 'make arm_sdk_install' to install automatically in the tools folder of this repo)
diff --git a/make/version.mk b/make/version.mk
new file mode 100644
index 0000000000..729979023a
--- /dev/null
+++ b/make/version.mk
@@ -0,0 +1,18 @@
+FORKNAME		:= inav
+
+FC_VER_MAJOR	:= $(shell grep " FC_VERSION_MAJOR" src/main/build/version.h | awk '{print $$3}' )
+FC_VER_MINOR	:= $(shell grep " FC_VERSION_MINOR" src/main/build/version.h | awk '{print $$3}' )
+FC_VER_PATCH	:= $(shell grep " FC_VERSION_PATCH" src/main/build/version.h | awk '{print $$3}' )
+FC_VER 			:= $(FC_VER_MAJOR).$(FC_VER_MINOR).$(FC_VER_PATCH)
+
+FC_VER_SUFFIX	?=
+ifneq ($(FC_VER_SUFFIX),)
+	FC_VER += -$(FC_VER_SUFFIX)
+endif
+
+REVISION 		:= $(shell git rev-parse --short HEAD)
+
+.PHONY: print_version
+
+print_version:
+	@echo $(FC_VER)

From f30de03672de81322320f062f1d0ed65d14a165a Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Alberto=20Garci=CC=81a=20Hierro?= <alberto@garciahierro.com>
Date: Tue, 31 Mar 2020 17:05:23 +0100
Subject: [PATCH 066/179] [CI] Use actions/cache to cache the ARM toolchain

This way we avoid having the ARM toolchain as an artifact when
the run ends and it also lets us reuse the cache from different
runs.

Also, make sure the whole file is indented with 2 spaces.
---
 .github/workflows/ci.yml | 119 ++++++++++++++-------------------------
 1 file changed, 43 insertions(+), 76 deletions(-)

diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml
index f8dddb668b..6fefa21e29 100644
--- a/.github/workflows/ci.yml
+++ b/.github/workflows/ci.yml
@@ -5,27 +5,7 @@ name: Build firmware
 on: pull_request
 
 jobs:
-  # This downloads the GNU toolchain from ARM once, then
-  # saves it as an artifact for the remaining jobs, making
-  # the download way faster.
-  download-toolchain:
-    runs-on: ubuntu-18.04
-    steps:
-      - uses: actions/checkout@v2
-      - name: Prepare ARM toolchain environment
-        run: |
-          echo "::set-env name=ARM_SDK_FILENAME::$(make arm_sdk_print_filename)"
-          echo "::set-env name=ARM_SDK_DOWNLOAD_PATH::$(make arm_sdk_print_download_path)"
-      - name: Download ARM toolchain
-        run: make arm_sdk_download
-      - name: Save toolchain as artifact
-        uses: actions/upload-artifact@v2-preview
-        with:
-          name: ${{ env.ARM_SDK_FILENAME }}
-          path: ${{ env.ARM_SDK_DOWNLOAD_PATH }}
-
   build:
-    needs: download-toolchain
     runs-on: ubuntu-18.04
     strategy:
         matrix:
@@ -33,68 +13,55 @@ jobs:
           count: [10]
 
     steps:
-        - uses: actions/checkout@v2
-        - name: Setup environment
-          run: |
-            # This is the hash of the commit for the PR
-            # when the action is triggered by PR, empty otherwise
-            COMMIT_ID=${{ github.event.pull_request.head.sha }}
-            # This is the hash of the commit when triggered by push
-            # but the hash of refs/pull/<n>/merge, which is different
-            # from the hash of the latest commit in the PR, that's
-            # why we try github.event.pull_request.head.sha first
-            COMMIT_ID=${COMMIT_ID:-${{ github.sha }}}
-            BUILD_SUFFIX=ci-$(date '+%Y%m%d')-$(git rev-parse --short ${COMMIT_ID})
-            echo "::set-env name=TARGETS::$(./src/utils/build-targets.sh -n -s ${{ matrix.start }} -c ${{ matrix.count }})"
-            echo "::set-env name=BUILD_SUFFIX::${BUILD_SUFFIX}"
-            echo "::set-env name=BUILD_NAME::inav-$(make print_version)-${BUILD_SUFFIX}"
-            echo "::set-env name=IS_LAST_JOB::$([ $(expr ${{ strategy.job-index }} + 1) = ${{ strategy.job-total }} ] && echo yes)"
-        - name: Ensure all targets will be tested
-          if: ${{ env.IS_LAST_JOB }}
-          run: |
-            UNTESTED=$(./src/utils/build-targets.sh -n -s $(expr ${{ matrix.start }} + ${{ matrix.count }}) -c 10000)
-            if ! [ -z "${UNTESTED}" ]; then
-              echo "Untested targets: ${UNTESTED}" >&2
-              exit 1
-            fi
-        - name: Prepare ARM toolchain environment
-          if: ${{ env.TARGETS }}
-          run: |
-            echo "::set-env name=ARM_SDK_FILENAME::$(make arm_sdk_print_filename)"
-            echo "::set-env name=ARM_SDK_DOWNLOAD_DIR::$(dirname $(make arm_sdk_print_download_path))"
-        - name: Download ARM toolchain
-          if: ${{ env.TARGETS }}
-          uses: actions/download-artifact@v1
-          with:
-            name: ${{ env.ARM_SDK_FILENAME }}
-            path: ${{ env.ARM_SDK_DOWNLOAD_DIR }}
-        - name: Install ARM toolchain
-          if: ${{ env.TARGETS }}
-          run: make arm_sdk_install
-        - name: Build targets (${{ matrix.start }})
-          if: ${{ env.TARGETS }}
-          run: ./src/utils/build-targets.sh -s ${{ matrix.start }} -c ${{ matrix.count }} -S ${{ env.BUILD_SUFFIX }}
-        - name: Upload artifacts
-          if: ${{ env.TARGETS }}
-          uses: actions/upload-artifact@v2-preview
-          with:
-            name: ${{ env.BUILD_NAME }}.zip
-            path: ./obj/*.hex
+      - uses: actions/checkout@v2
+      - name: Setup environment
+        run: |
+          # This is the hash of the commit for the PR
+          # when the action is triggered by PR, empty otherwise
+          COMMIT_ID=${{ github.event.pull_request.head.sha }}
+          # This is the hash of the commit when triggered by push
+          # but the hash of refs/pull/<n>/merge, which is different
+          # from the hash of the latest commit in the PR, that's
+          # why we try github.event.pull_request.head.sha first
+          COMMIT_ID=${COMMIT_ID:-${{ github.sha }}}
+          BUILD_SUFFIX=ci-$(date '+%Y%m%d')-$(git rev-parse --short ${COMMIT_ID})
+          echo "::set-env name=TARGETS::$(./src/utils/build-targets.sh -n -s ${{ matrix.start }} -c ${{ matrix.count }})"
+          echo "::set-env name=BUILD_SUFFIX::${BUILD_SUFFIX}"
+          echo "::set-env name=BUILD_NAME::inav-$(make print_version)-${BUILD_SUFFIX}"
+          echo "::set-env name=IS_LAST_JOB::$([ $(expr ${{ strategy.job-index }} + 1) = ${{ strategy.job-total }} ] && echo yes)"
+      - name: Ensure all targets will be tested
+        if: ${{ env.IS_LAST_JOB }}
+        run: |
+          UNTESTED=$(./src/utils/build-targets.sh -n -s $(expr ${{ matrix.start }} + ${{ matrix.count }}) -c 10000)
+          if ! [ -z "${UNTESTED}" ]; then
+            echo "Untested targets: ${UNTESTED}" >&2
+            exit 1
+          fi
+      - uses: actions/cache@v1
+        with:
+          path: downloads
+          key: ${{ runner.os }}-downloads-${{ hashFiles('Makefile') }}-${{ hashFiles('**/make/*.mk')}}
+      - name: Install ARM toolchain
+        run: make arm_sdk_install
+      - name: Build targets (${{ matrix.start }})
+        if: ${{ env.TARGETS }}
+        run: ./src/utils/build-targets.sh -s ${{ matrix.start }} -c ${{ matrix.count }} -S ${{ env.BUILD_SUFFIX }}
+      - name: Upload artifacts
+        if: ${{ env.TARGETS }}
+        uses: actions/upload-artifact@v2-preview
+        with:
+          name: ${{ env.BUILD_NAME }}.zip
+          path: ./obj/*.hex
 
   test:
-    needs: [download-toolchain, build]
+    needs: [build]
     runs-on: ubuntu-18.04
     steps:
       - uses: actions/checkout@v2
-      - name: Prepare ARM toolchain environment
-        run: |
-          echo "::set-env name=ARM_SDK_FILENAME::$(make arm_sdk_print_filename)"
-          echo "::set-env name=ARM_SDK_DOWNLOAD_DIR::$(dirname $(make arm_sdk_print_download_path))"
-      - name: Download ARM toolchain
-        uses: actions/download-artifact@v1
+      - uses: actions/cache@v1
         with:
-          name: ${{ env.ARM_SDK_FILENAME }}
-          path: ${{ env.ARM_SDK_DOWNLOAD_DIR }}
+          path: downloads
+          key: ${{ runner.os }}-downloads-${{ hashFiles('Makefile') }}-${{ hashFiles('**/make/*.mk')}}
       - name: Install ARM toolchain
         run: make arm_sdk_install
       - name: Run Tests

From db12e87990655665d4301456e67ecd5ab4670de3 Mon Sep 17 00:00:00 2001
From: stronnag <jh+github@daria.co.uk>
Date: Sat, 4 Apr 2020 13:47:59 +0100
Subject: [PATCH 067/179] update Navigation.md for 2.5.0 features (#5564)

* update docs/Navigation.md for 2.5 capabilities
---
 docs/Navigation.md | 59 +++++++++++++++++++++++++++++++++++-----------
 1 file changed, 45 insertions(+), 14 deletions(-)

diff --git a/docs/Navigation.md b/docs/Navigation.md
index a227b90629..11cd8bec2d 100755
--- a/docs/Navigation.md
+++ b/docs/Navigation.md
@@ -1,6 +1,6 @@
 # Navigation
 
-Navigation system in INAV is responsible for assisting the pilot allowing altitude and position hold, return-to-home and waypoint flight.
+The navigation system in INAV is responsible for assisting the pilot allowing altitude and position hold, return-to-home and waypoint flight.
 
 ## NAV ALTHOLD mode - altitude hold
 
@@ -23,7 +23,7 @@ Throttle tilt compensation attempts to maintain constant vertical thrust when co
 ## NAV POSHOLD mode - position hold
 
 Position hold requires GPS, accelerometer and compass sensors. Flight modes that require a compass (POSHOLD, RTH) are locked until compass is properly calibrated.
-When activated, this mode will attempt to keep copter where it is (based on GPS coordinates). Can be activated together with ALTHOLD to get a full 3D position hold. Heading hold in this mode is assumed and activated automatically.
+When activated, this mode will attempt to keep copter where it is (based on GPS coordinates). From inav 2.0, POSHOLD is a full 3D position hold. Heading hold in this mode is assumed and activated automatically.
 
 ### CLI parameters affecting ALTHOLD mode:
 * *nav_user_control_mode* - can be set to "0" (GPS_ATTI) or "1" (GPS_CRUISE), controls how firmware will respond to roll/pitch stick movement. When in GPS_ATTI mode, right stick controls attitude, when it is released, new position is recorded and held. When in GPS_CRUISE mode right stick controls velocity and firmware calculates required attitude on its own.
@@ -36,7 +36,7 @@ PID meaning:
 
 ## NAV RTH - return to home mode
 
-Home for RTH is position, where copter was armed. RTH requires accelerometer, compass and GPS sensors.
+Home for RTH is the position where vehicle was armed. This position may be offset by the CLI settings `nav_rth_home_offset_distance` and `nav_rth_home_offset_direction`. RTH requires accelerometer, compass and GPS sensors.
 
 If barometer is NOT present, RTH will fly directly to home, altitude control here is up to pilot.
 
@@ -54,24 +54,55 @@ When deciding what altitude to maintain, RTH has 4 different modes of operation
 
 `wp load` - Load list of waypoints from EEPROM to FC.
 
-`wp <n> <action> <lat> <lon> <alt> <p1> <flag>` - Set parameters of waypoint with index `<n>`.
+`wp <n> <action> <lat> <lon> <alt> <p1> <p2> <p3> <flag>` - Set parameters of waypoint with index `<n>`. Note that prior to inav 2.5, the `p2` and `p3` parameters were not required. From 2.5, inav will accept either version but always saves and lists the later full version.
 
 Parameters:
 
-  * `<action>` - When 0 then waypoint is not used, when 1 then it is normal waypoint, when 4 then it is RTH.
-  
+  * `<action>` - The action to be taken at the WP. The following are enumerations are available in inav 2.5 and later:
+      *  0 - Unused / Unassigned
+      *  1 - WAYPOINT
+      *  3 - POSHOLD_TIME
+      *  4 - RTH
+      *  6 - JUMP
+      *  8 - LAND
+
   * `<lat>` - Latitude (WGS84), in degrees * 1E7 (for example 123456789 means 12.3456789).
-  
+
   * `<lon>` - Longitude.
-  
+
   * `<alt>` - Altitude in cm.
-  
-  * `<p1>` - For a "RTH waypoint" p1 > 0 alow landing. For a normal waypoint it means speed to this waypoint, it is taken into account only for multicopters and when > 50 and < nav_auto_speed. For POSHOLD TIME waypoint it is time time to wait in seconds.
-  
-  * `<p2>` - For a POSHOLD TIME it means speed to this waypoint, it is taken into account only for multicopters and when > 50 and < nav_auto_speed.
-  
+
+  * `<p1>` - For a RTH waypoint, p1 > 0 enables landing. For a normal waypoint it is the speed to this waypoint (cm/s), it is taken into account only for multicopters and when > 50 and < nav_auto_speed. For POSHOLD TIME waypoint it is time to loiter in seconds. For JUMP it is the target WP.
+
+  * `<p2>` - For a POSHOLD TIME it is the speed to this waypoint (cm/s), it is taken into account only for multicopters and when > 50 and < nav_auto_speed. For JUMP it is the number of iterations of the JUMP.
+
+  * `<p3>` - Reserved for future use. If `p2` is provided, then `p3` is also required.
+
   * `<flag>` - Last waypoint must have set `flag` to 165 (0xA5), otherwise 0.
-  
+
 `wp save` - Checks list of waypoints and save from FC to EEPROM (warning: it also saves all unsaved CLI settings like normal `save`).
 
 `wp reset` - Resets the list, sets the waypoints number to 0 and mark it as invalid (but doesn't delete the waypoints).
+
+### `wp` example
+
+```
+# wp load
+
+# wp
+#wp 11 valid
+wp 0 1 543533193 -45179273 3500 0 0 0 0
+wp 1 1 543535723 -45193913 3500 0 0 0 0
+wp 2 1 543544541 -45196617 5000 0 0 0 0
+wp 3 1 543546578 -45186895 5000 0 0 0 0
+wp 4 6 0 0 0 2 2 0 0
+wp 5 1 543546688 -45176009 3500 0 0 0 0
+wp 6 1 543541225 -45172673 3500 0 0 0 0
+wp 7 6 0 0 0 1 1 0 0
+wp 8 3 543531383 -45190405 3500 45 0 0 0
+wp 9 1 543548470 -45182104 3500 0 0 0 0
+wp 10 8 543540521 -45178091 6000 0 0 0 165
+wp 11 0 0 0 0 0 0 0 0
+...
+wp 59 0 0 0 0 0 0 0 0
+```

From fe5ada21add32aa49aaa389f9eb1bf346cd091aa Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Pawe=C5=82=20Spychalski?= <pspychalski@gmail.com>
Date: Sun, 5 Apr 2020 18:09:42 +0200
Subject: [PATCH 068/179] Fix expected length of MSP2_INAV_SET_GLOBAL_FUNCTIONS
 frame (#5570)

* Fix expected length of MSP2_INAV_SET_GLOBAL_FUNCTIONS frame

* MSP2_INAV_SET_GLOBAL_FUNCTIONS length adjustment
---
 src/main/fc/fc_msp.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index 212b7f01f3..d1f549b660 100644
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -1958,7 +1958,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
 #ifdef USE_GLOBAL_FUNCTIONS
     case MSP2_INAV_SET_GLOBAL_FUNCTIONS:
         sbufReadU8Safe(&tmp_u8, src);
-        if ((dataSize == 14) && (tmp_u8 < MAX_GLOBAL_FUNCTIONS)) {
+        if ((dataSize == 10) && (tmp_u8 < MAX_GLOBAL_FUNCTIONS)) {
             globalFunctionsMutable(tmp_u8)->enabled = sbufReadU8(src);
             globalFunctionsMutable(tmp_u8)->conditionId = sbufReadU8(src);
             globalFunctionsMutable(tmp_u8)->action = sbufReadU8(src);

From 2ff5ced4eec01ff3f3b57f2a535b46d412e75141 Mon Sep 17 00:00:00 2001
From: stronnag <jh+github@daria.co.uk>
Date: Mon, 6 Apr 2020 18:33:58 +0100
Subject: [PATCH 069/179] [NAV] store jump targets as indices (vice WP #) for
 consistency (#5569)

* [NAV] store jump targets as indices (vice WP #) for consistency

* [NAV] improve JUMP validity test
---
 docs/Navigation.md               | 10 ++++++----
 src/main/navigation/navigation.c | 14 +++++++++++---
 2 files changed, 17 insertions(+), 7 deletions(-)

diff --git a/docs/Navigation.md b/docs/Navigation.md
index 11cd8bec2d..1b31da1289 100755
--- a/docs/Navigation.md
+++ b/docs/Navigation.md
@@ -72,7 +72,7 @@ Parameters:
 
   * `<alt>` - Altitude in cm.
 
-  * `<p1>` - For a RTH waypoint, p1 > 0 enables landing. For a normal waypoint it is the speed to this waypoint (cm/s), it is taken into account only for multicopters and when > 50 and < nav_auto_speed. For POSHOLD TIME waypoint it is time to loiter in seconds. For JUMP it is the target WP.
+  * `<p1>` - For a RTH waypoint, p1 > 0 enables landing. For a normal waypoint it is the speed to this waypoint (cm/s), it is taken into account only for multicopters and when > 50 and < nav_auto_speed. For POSHOLD TIME waypoint it is time to loiter in seconds. For JUMP it is the target WP **index** (not number).
 
   * `<p2>` - For a POSHOLD TIME it is the speed to this waypoint (cm/s), it is taken into account only for multicopters and when > 50 and < nav_auto_speed. For JUMP it is the number of iterations of the JUMP.
 
@@ -82,7 +82,7 @@ Parameters:
 
 `wp save` - Checks list of waypoints and save from FC to EEPROM (warning: it also saves all unsaved CLI settings like normal `save`).
 
-`wp reset` - Resets the list, sets the waypoints number to 0 and mark it as invalid (but doesn't delete the waypoints).
+`wp reset` - Resets the list, sets the number of waypoints to 0 and marks the list as invalid (but doesn't delete the waypoint definitions).
 
 ### `wp` example
 
@@ -95,10 +95,10 @@ wp 0 1 543533193 -45179273 3500 0 0 0 0
 wp 1 1 543535723 -45193913 3500 0 0 0 0
 wp 2 1 543544541 -45196617 5000 0 0 0 0
 wp 3 1 543546578 -45186895 5000 0 0 0 0
-wp 4 6 0 0 0 2 2 0 0
+wp 4 6 0 0 0 1 2 0 0
 wp 5 1 543546688 -45176009 3500 0 0 0 0
 wp 6 1 543541225 -45172673 3500 0 0 0 0
-wp 7 6 0 0 0 1 1 0 0
+wp 7 6 0 0 0 0 1 0 0
 wp 8 3 543531383 -45190405 3500 45 0 0 0
 wp 9 1 543548470 -45182104 3500 0 0 0 0
 wp 10 8 543540521 -45178091 6000 0 0 0 165
@@ -106,3 +106,5 @@ wp 11 0 0 0 0 0 0 0 0
 ...
 wp 59 0 0 0 0 0 0 0 0
 ```
+
+Note that the `wp` CLI command shows waypoint list indices, while the MW-XML definition used by mwp, ezgui and the configurator use WP numbers.
diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index 109de949ca..94ae07e39b 100755
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -1432,7 +1432,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav
                 }
             }
 
-            posControl.activeWaypointIndex = posControl.waypointList[posControl.activeWaypointIndex].p1 - 1;
+            posControl.activeWaypointIndex = posControl.waypointList[posControl.activeWaypointIndex].p1;
 
             return NAV_FSM_EVENT_NONE; // re-process the state passing to the next WP
 
@@ -2679,10 +2679,14 @@ void getWaypoint(uint8_t wpNumber, navWaypoint_t * wpData)
         wpData->lon = wpLLH.lon;
         wpData->alt = wpLLH.alt;
     }
-    // WP #1 - #15 - common waypoints - pre-programmed mission
+    // WP #1 - #60 - common waypoints - pre-programmed mission
     else if ((wpNumber >= 1) && (wpNumber <= NAV_MAX_WAYPOINTS)) {
         if (wpNumber <= posControl.waypointCount) {
             *wpData = posControl.waypointList[wpNumber - 1];
+            if(wpData->action == NAV_WP_ACTION_JUMP) {
+                wpData->p1 += 1; // make WP # (vice index)
+            }
+
         }
     }
 }
@@ -2730,6 +2734,9 @@ void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData)
             // Only allow upload next waypoint (continue upload mission) or first waypoint (new mission)
             if (wpNumber == (posControl.waypointCount + 1) || wpNumber == 1) {
                 posControl.waypointList[wpNumber - 1] = *wpData;
+                if(wpData->action == NAV_WP_ACTION_JUMP) {
+                    posControl.waypointList[wpNumber - 1].p1 -= 1; // make index (vice WP #)
+                }
                 posControl.waypointCount = wpNumber;
                 posControl.waypointListValid = (wpData->flag == NAV_WP_FLAG_LAST);
             }
@@ -3211,10 +3218,11 @@ navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass)
     }
 
     // Don't allow arming if any of JUMP waypoint has invalid settings
+    // Note JUMP only goes to previous WPs, which must be 2 indices back
     if (posControl.waypointCount > 0) {
         for (uint8_t wp = 0; wp < posControl.waypointCount ; wp++){
             if (posControl.waypointList[wp].action == NAV_WP_ACTION_JUMP){
-                if((posControl.waypointList[wp].p1 > posControl.waypointCount) || (posControl.waypointList[wp].p2 < -1)){
+                if((wp < 2) || (posControl.waypointList[wp].p1 > (wp-2)) || (posControl.waypointList[wp].p2 < -1)) {
                     return NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR;
                 }
             }

From 90291bd9b77a860a7aac0c3db04ea9c5896cfd3c Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Fri, 10 Apr 2020 19:45:53 +0200
Subject: [PATCH 070/179] Add constrain for math operators

---
 src/main/common/logic_condition.c | 20 ++++++++++----------
 src/main/common/logic_condition.h |  2 +-
 2 files changed, 11 insertions(+), 11 deletions(-)

diff --git a/src/main/common/logic_condition.c b/src/main/common/logic_condition.c
index 43bedc7658..cfa1ed0ae1 100644
--- a/src/main/common/logic_condition.c
+++ b/src/main/common/logic_condition.c
@@ -141,20 +141,20 @@ static int logicConditionCompute(
             break;
 
         case LOGIC_CONDITION_ADD:
-            return operandA + operandB;
+            return constrain(operandA + operandB, INT16_MIN, INT16_MAX);
             break;
 
         case LOGIC_CONDITION_SUB:
-            return operandA - operandB;
+            return constrain(operandA - operandB, INT16_MIN, INT16_MAX);
             break;
 
         case LOGIC_CONDITION_MUL:
-            return operandA * operandB;
+            return constrain(operandA * operandB, INT16_MIN, INT16_MAX);
             break;
 
         case LOGIC_CONDITION_DIV:
             if (operandB != 0) {
-                return operandA / operandB;
+                return constrain(operandA / operandB, INT16_MIN, INT16_MAX);
             } else {
                 return operandA;
             }
@@ -203,15 +203,15 @@ static int logicConditionGetFlightOperandValue(int operand) {
     switch (operand) {
 
         case LOGIC_CONDITION_OPERAND_FLIGHT_ARM_TIMER: // in s
-            return constrain((uint32_t)getFlightTime(), 0, 32767);
+            return constrain((uint32_t)getFlightTime(), 0, INT16_MAX);
             break;
         
         case LOGIC_CONDITION_OPERAND_FLIGHT_HOME_DISTANCE: //in m
-            return constrain(GPS_distanceToHome, 0, 32767);
+            return constrain(GPS_distanceToHome, 0, INT16_MAX);
             break;
         
         case LOGIC_CONDITION_OPERAND_FLIGHT_TRIP_DISTANCE: //in m
-            return constrain(getTotalTravelDistance() / 100, 0, 32767);
+            return constrain(getTotalTravelDistance() / 100, 0, INT16_MAX);
             break;
 
         case LOGIC_CONDITION_OPERAND_FLIGHT_RSSI:
@@ -248,18 +248,18 @@ static int logicConditionGetFlightOperandValue(int operand) {
 
         case LOGIC_CONDITION_OPERAND_FLIGHT_AIR_SPEED: // cm/s
         #ifdef USE_PITOT
-            return constrain(pitot.airSpeed, 0, 32767);
+            return constrain(pitot.airSpeed, 0, INT16_MAX);
         #else
             return false;
         #endif
             break;
 
         case LOGIC_CONDITION_OPERAND_FLIGHT_ALTITUDE: // cm
-            return constrain(getEstimatedActualPosition(Z), -32678, 32767);
+            return constrain(getEstimatedActualPosition(Z), INT16_MIN, INT16_MAX);
             break;
 
         case LOGIC_CONDITION_OPERAND_FLIGHT_VERTICAL_SPEED: // cm/s
-            return constrain(getEstimatedActualVelocity(Z), 0, 32767);
+            return constrain(getEstimatedActualVelocity(Z), 0, INT16_MAX);
             break;
 
         case LOGIC_CONDITION_OPERAND_FLIGHT_TROTTLE_POS: // %
diff --git a/src/main/common/logic_condition.h b/src/main/common/logic_condition.h
index bd1aa6dec6..3dc3e7e77c 100644
--- a/src/main/common/logic_condition.h
+++ b/src/main/common/logic_condition.h
@@ -49,7 +49,7 @@ typedef enum {
     LOGIC_CONDITION_DIV,            // 17
     LOGIC_CONDITION_GVAR_SET,       // 18
     LOGIC_CONDITION_GVAR_INC,       // 19
-    LOGIC_CONDITION_GVAR_DEC,       // 21
+    LOGIC_CONDITION_GVAR_DEC,       // 20
     LOGIC_CONDITION_LAST
 } logicOperation_e;
 

From cd8da994b07e09095729a222df05f7ab64cfd862 Mon Sep 17 00:00:00 2001
From: stronnag <jh+github@daria.co.uk>
Date: Fri, 10 Apr 2020 20:09:46 +0100
Subject: [PATCH 071/179] [DOC] reference ublox update rates in Cli.md (#5585)

---
 docs/Cli.md | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/docs/Cli.md b/docs/Cli.md
index cfb8e3d065..60a3b59d6a 100644
--- a/docs/Cli.md
+++ b/docs/Cli.md
@@ -152,7 +152,7 @@ A shorter form is also supported to enable and disable functions using `serial <
 |  switch_disarm_delay | 250 | Delay before disarming when requested by switch (ms) [0-1000] |
 |  small_angle  | 25 | If the aircraft tilt angle exceed this value the copter will refuse to arm.  |
 |  reboot_character  | 82 | Special character used to trigger reboot |
-|  gps_provider  | UBLOX | Which GPS protocol to be used |
+|  gps_provider  | UBLOX | Which GPS protocol to be used, note that UBLOX is 5Hz and UBLOX7 is 10Hz (M8N). |
 |  gps_sbas_mode  | NONE | Which SBAS mode to be used |
 |  gps_dyn_model  | AIR_1G | GPS navigation model: Pedestrian, Air_1g, Air_4g. Default is AIR_1G. Use pedestrian with caution, can cause flyaways with fast flying. |
 |  gps_auto_config  | ON | Enable automatic configuration of UBlox GPS receivers. |

From 1f57afa1c5ba078071a4a3970ac0a859ab0aeecb Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Sat, 11 Apr 2020 11:23:27 +0200
Subject: [PATCH 072/179] Switch GVARS to int32_t

---
 src/main/common/global_variables.c | 10 +++++-----
 src/main/common/global_variables.h |  8 ++++----
 2 files changed, 9 insertions(+), 9 deletions(-)

diff --git a/src/main/common/global_variables.c b/src/main/common/global_variables.c
index ea0327eaeb..9e6990ec65 100644
--- a/src/main/common/global_variables.c
+++ b/src/main/common/global_variables.c
@@ -34,7 +34,7 @@
 #include "common/maths.h"
 #include "build/build_config.h"
  
-static EXTENDED_FASTRAM int globalVariableState[MAX_GLOBAL_VARIABLES];
+static EXTENDED_FASTRAM int32_t globalVariableState[MAX_GLOBAL_VARIABLES];
 
 PG_REGISTER_ARRAY_WITH_RESET_FN(globalVariableConfig_t, MAX_GLOBAL_VARIABLES, globalVariableConfigs, PG_GLOBAL_VARIABLE_CONFIG, 0);
 
@@ -42,12 +42,12 @@ void pgResetFn_globalVariableConfigs(globalVariableConfig_t *globalVariableConfi
 {
     // set default calibration to full range and 1:1 mapping
     for (int i = 0; i < MAX_GLOBAL_VARIABLES; i++) {
-        globalVariableConfigs[i].min = INT32_MIN;
-        globalVariableConfigs[i].max = INT32_MAX;
+        globalVariableConfigs[i].min = INT16_MIN;
+        globalVariableConfigs[i].max = INT16_MAX;
     }
 }
 
-int gvGet(uint8_t index) {
+int32_t gvGet(uint8_t index) {
     if (index < MAX_GLOBAL_VARIABLES) {
         return globalVariableState[index];
     } else {
@@ -55,7 +55,7 @@ int gvGet(uint8_t index) {
     }
 }
 
-void gvSet(uint8_t index, int value) {
+void gvSet(uint8_t index, int32_t value) {
     if (index < MAX_GLOBAL_VARIABLES) {
         globalVariableState[index] = constrain(value, globalVariableConfigs(index)->min, globalVariableConfigs(index)->max);
     }
diff --git a/src/main/common/global_variables.h b/src/main/common/global_variables.h
index 0a63713b1d..56875b2e99 100644
--- a/src/main/common/global_variables.h
+++ b/src/main/common/global_variables.h
@@ -29,10 +29,10 @@
 #define MAX_GLOBAL_VARIABLES 4
 
 typedef struct globalVariableConfig_s {
-    int min;
-    int max;
+    int32_t min;
+    int32_t max;
 } globalVariableConfig_t;
 PG_DECLARE_ARRAY(globalVariableConfig_t, MAX_GLOBAL_VARIABLES, globalVariableConfigs);
 
-int gvGet(uint8_t index);
-void gvSet(uint8_t index, int value);
\ No newline at end of file
+int32_t gvGet(uint8_t index);
+void gvSet(uint8_t index, int32_t value);
\ No newline at end of file

From b2eaf46f66be4894a9c98bb3b6d269de9e4451dd Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Sat, 11 Apr 2020 14:24:31 +0200
Subject: [PATCH 073/179] Add an option to have GVAR default value

---
 src/main/common/global_variables.c | 10 +++++++++-
 src/main/common/global_variables.h |  4 +++-
 src/main/fc/cli.c                  | 12 +++++++++---
 src/main/fc/fc_init.c              |  5 +++++
 4 files changed, 26 insertions(+), 5 deletions(-)

diff --git a/src/main/common/global_variables.c b/src/main/common/global_variables.c
index 9e6990ec65..72230a97ae 100644
--- a/src/main/common/global_variables.c
+++ b/src/main/common/global_variables.c
@@ -24,6 +24,8 @@
 
 #include "platform.h"
 
+FILE_COMPILE_FOR_SIZE
+
 #ifdef USE_LOGIC_CONDITIONS
 
 #include <stdint.h>
@@ -40,10 +42,10 @@ PG_REGISTER_ARRAY_WITH_RESET_FN(globalVariableConfig_t, MAX_GLOBAL_VARIABLES, gl
 
 void pgResetFn_globalVariableConfigs(globalVariableConfig_t *globalVariableConfigs)
 {
-    // set default calibration to full range and 1:1 mapping
     for (int i = 0; i < MAX_GLOBAL_VARIABLES; i++) {
         globalVariableConfigs[i].min = INT16_MIN;
         globalVariableConfigs[i].max = INT16_MAX;
+        globalVariableConfigs[i].defaultValue = 0;
     }
 }
 
@@ -61,4 +63,10 @@ void gvSet(uint8_t index, int32_t value) {
     }
 }
 
+void gvInit(void) {
+    for (int i = 0; i < MAX_GLOBAL_VARIABLES; i++) {
+        globalVariableState[i] = globalVariableConfigs(i)->defaultValue;
+    }
+}
+
 #endif
\ No newline at end of file
diff --git a/src/main/common/global_variables.h b/src/main/common/global_variables.h
index 56875b2e99..cbeb500348 100644
--- a/src/main/common/global_variables.h
+++ b/src/main/common/global_variables.h
@@ -31,8 +31,10 @@
 typedef struct globalVariableConfig_s {
     int32_t min;
     int32_t max;
+    int32_t defaultValue;
 } globalVariableConfig_t;
 PG_DECLARE_ARRAY(globalVariableConfig_t, MAX_GLOBAL_VARIABLES, globalVariableConfigs);
 
 int32_t gvGet(uint8_t index);
-void gvSet(uint8_t index, int32_t value);
\ No newline at end of file
+void gvSet(uint8_t index, int32_t value);
+void gvInit(void);
\ No newline at end of file
diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c
index 307e2a582e..107494b54a 100755
--- a/src/main/fc/cli.c
+++ b/src/main/fc/cli.c
@@ -1859,7 +1859,7 @@ static void cliLogic(char *cmdline) {
 
 static void printGvar(uint8_t dumpMask, const globalVariableConfig_t *gvars, const globalVariableConfig_t *defaultGvars)
 {
-    const char *format = "gvar %d %d %d";
+    const char *format = "gvar %d %d %d %d";
     for (uint32_t i = 0; i < MAX_GLOBAL_VARIABLES; i++) {
         const globalVariableConfig_t gvar = gvars[i];
 
@@ -1867,17 +1867,20 @@ static void printGvar(uint8_t dumpMask, const globalVariableConfig_t *gvars, con
         if (defaultGvars) {
             globalVariableConfig_t defaultValue = defaultGvars[i];
             equalsDefault =
+                gvar.defaultValue == defaultValue.defaultValue &&
                 gvar.min == defaultValue.min &&
                 gvar.max == defaultValue.max;
 
             cliDefaultPrintLinef(dumpMask, equalsDefault, format,
                 i,
+                gvar.defaultValue,
                 gvar.min,
                 gvar.max
             );
         }
         cliDumpPrintLinef(dumpMask, equalsDefault, format,
             i,
+            gvar.defaultValue,
             gvar.min,
             gvar.max
         );
@@ -1886,7 +1889,7 @@ static void printGvar(uint8_t dumpMask, const globalVariableConfig_t *gvars, con
 
 static void cliGvar(char *cmdline) {
     char * saveptr;
-    int args[3], check = 0;
+    int args[4], check = 0;
     uint8_t len = strlen(cmdline);
 
     if (len == 0) {
@@ -1896,6 +1899,7 @@ static void cliGvar(char *cmdline) {
     } else {
         enum {
             INDEX = 0,
+            DEFAULT,
             MIN,
             MAX,
             ARGS_COUNT
@@ -1914,9 +1918,11 @@ static void cliGvar(char *cmdline) {
         int32_t i = args[INDEX];
         if (
             i >= 0 && i < MAX_GLOBAL_VARIABLES &&
+            args[DEFAULT] >= INT32_MIN && args[DEFAULT] <= INT32_MAX &&  
             args[MIN] >= INT32_MIN && args[MIN] <= INT32_MAX &&  
             args[MAX] >= INT32_MIN && args[MAX] <= INT32_MAX  
         ) {
+            globalVariableConfigsMutable(i)->defaultValue = args[DEFAULT];
             globalVariableConfigsMutable(i)->min = args[MIN];
             globalVariableConfigsMutable(i)->max = args[MAX];
 
@@ -3545,7 +3551,7 @@ const clicmd_t cmdTable[] = {
         "\treset\r\n", cliLogic),
 
     CLI_COMMAND_DEF("gvar", "configure global variables",
-        "<gvar> <min> <max>\r\n"
+        "<gvar> <default> <min> <max>\r\n"
         "\treset\r\n", cliGvar),
 #endif
 #ifdef USE_GLOBAL_FUNCTIONS
diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c
index 8d445f0289..4228405a32 100644
--- a/src/main/fc/fc_init.c
+++ b/src/main/fc/fc_init.c
@@ -35,6 +35,7 @@
 #include "common/maths.h"
 #include "common/memory.h"
 #include "common/printf.h"
+#include "common/global_variables.h"
 
 #include "config/config_eeprom.h"
 #include "config/feature.h"
@@ -276,6 +277,10 @@ void init(void)
     logInit();
 #endif
 
+#ifdef USE_LOGIC_CONDITIONS
+    gvInit();
+#endif
+
     // Initialize servo and motor mixers
     // This needs to be called early to set up platform type correctly and count required motors & servos
     servosInit();

From ae8d29c02515a59c2142cd16ffa93b5fc28516ea Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Sat, 11 Apr 2020 23:10:40 +0200
Subject: [PATCH 074/179] Add activatorId to CLI and MSP

---
 src/main/common/logic_condition.c | 22 +++++++++++++++++++++-
 src/main/common/logic_condition.h |  1 +
 src/main/fc/cli.c                 | 10 ++++++++--
 src/main/fc/fc_msp.c              |  4 +++-
 4 files changed, 33 insertions(+), 4 deletions(-)

diff --git a/src/main/common/logic_condition.c b/src/main/common/logic_condition.c
index cfa1ed0ae1..e767d2eea2 100644
--- a/src/main/common/logic_condition.c
+++ b/src/main/common/logic_condition.c
@@ -44,7 +44,27 @@
 #include "navigation/navigation.h"
 #include "navigation/navigation_private.h"
 
-PG_REGISTER_ARRAY(logicCondition_t, MAX_LOGIC_CONDITIONS, logicConditions, PG_LOGIC_CONDITIONS, 0);
+PG_REGISTER_ARRAY_WITH_RESET_FN(logicCondition_t, MAX_LOGIC_CONDITIONS, logicConditions, PG_LOGIC_CONDITIONS, 1);
+
+void pgResetFn_logicConditions(logicCondition_t *instance)
+{
+    for (int i = 0; i < MAX_LOGIC_CONDITIONS; i++) {
+        RESET_CONFIG(logicCondition_t, &instance[i],
+            .enabled = 0,
+            .activatorId = -1,
+            .operation = 0,
+            .operandA = {
+                .type = LOGIC_CONDITION_OPERAND_TYPE_VALUE,
+                .value = 0
+            },
+            .operandB = {
+                .type = LOGIC_CONDITION_OPERAND_TYPE_VALUE,
+                .value = 0
+            },
+            .flags = 0
+        );
+    }
+}
 
 logicConditionState_t logicConditionStates[MAX_LOGIC_CONDITIONS];
 
diff --git a/src/main/common/logic_condition.h b/src/main/common/logic_condition.h
index 3dc3e7e77c..a158875437 100644
--- a/src/main/common/logic_condition.h
+++ b/src/main/common/logic_condition.h
@@ -115,6 +115,7 @@ typedef struct logicOperand_s {
 
 typedef struct logicCondition_s {
     uint8_t enabled;
+    int8_t activatorId;
     logicOperation_e operation;
     logicOperand_t operandA;
     logicOperand_t operandB;
diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c
index 107494b54a..427df2bbb1 100755
--- a/src/main/fc/cli.c
+++ b/src/main/fc/cli.c
@@ -1758,7 +1758,7 @@ static void cliServoMix(char *cmdline)
 
 static void printLogic(uint8_t dumpMask, const logicCondition_t *logicConditions, const logicCondition_t *defaultLogicConditions)
 {
-    const char *format = "logic %d %d %d %d %d %d %d %d";
+    const char *format = "logic %d %d %d %d %d %d %d %d %d";
     for (uint32_t i = 0; i < MAX_LOGIC_CONDITIONS; i++) {
         const logicCondition_t logic = logicConditions[i];
 
@@ -1767,6 +1767,7 @@ static void printLogic(uint8_t dumpMask, const logicCondition_t *logicConditions
             logicCondition_t defaultValue = defaultLogicConditions[i];
             equalsDefault =
                 logic.enabled == defaultValue.enabled &&
+                logic.activatorId == defaultValue.activatorId &&
                 logic.operation == defaultValue.operation &&
                 logic.operandA.type == defaultValue.operandA.type &&
                 logic.operandA.value == defaultValue.operandA.value &&
@@ -1777,6 +1778,7 @@ static void printLogic(uint8_t dumpMask, const logicCondition_t *logicConditions
             cliDefaultPrintLinef(dumpMask, equalsDefault, format,
                 i,
                 logic.enabled,
+                logic.activatorId,
                 logic.operation,
                 logic.operandA.type,
                 logic.operandA.value,
@@ -1788,6 +1790,7 @@ static void printLogic(uint8_t dumpMask, const logicCondition_t *logicConditions
         cliDumpPrintLinef(dumpMask, equalsDefault, format,
             i,
             logic.enabled,
+            logic.activatorId,
             logic.operation,
             logic.operandA.type,
             logic.operandA.value,
@@ -1800,7 +1803,7 @@ static void printLogic(uint8_t dumpMask, const logicCondition_t *logicConditions
 
 static void cliLogic(char *cmdline) {
     char * saveptr;
-    int args[8], check = 0;
+    int args[9], check = 0;
     uint8_t len = strlen(cmdline);
 
     if (len == 0) {
@@ -1811,6 +1814,7 @@ static void cliLogic(char *cmdline) {
         enum {
             INDEX = 0,
             ENABLED,
+            ACTIVATOR_ID,
             OPERATION,
             OPERAND_A_TYPE,
             OPERAND_A_VALUE,
@@ -1834,6 +1838,7 @@ static void cliLogic(char *cmdline) {
         if (
             i >= 0 && i < MAX_LOGIC_CONDITIONS &&
             args[ENABLED] >= 0 && args[ENABLED] <= 1 &&
+            args[ACTIVATOR_ID] >= -1 && args[ACTIVATOR_ID] < MAX_LOGIC_CONDITIONS &&
             args[OPERATION] >= 0 && args[OPERATION] < LOGIC_CONDITION_LAST &&
             args[OPERAND_A_TYPE] >= 0 && args[OPERAND_A_TYPE] < LOGIC_CONDITION_OPERAND_TYPE_LAST &&
             args[OPERAND_A_VALUE] >= -1000000 && args[OPERAND_A_VALUE] <= 1000000 &&
@@ -1843,6 +1848,7 @@ static void cliLogic(char *cmdline) {
 
         ) {
             logicConditionsMutable(i)->enabled = args[ENABLED];
+            logicConditionsMutable(i)->activatorId = args[ACTIVATOR_ID];
             logicConditionsMutable(i)->operation = args[OPERATION];
             logicConditionsMutable(i)->operandA.type = args[OPERAND_A_TYPE];
             logicConditionsMutable(i)->operandA.value = args[OPERAND_A_VALUE];
diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index 0e890efb1e..6dea4773db 100644
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -530,6 +530,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
     case MSP2_INAV_LOGIC_CONDITIONS:
         for (int i = 0; i < MAX_LOGIC_CONDITIONS; i++) {
             sbufWriteU8(dst, logicConditions(i)->enabled);
+            sbufWriteU8(dst, logicConditions(i)->activatorId);
             sbufWriteU8(dst, logicConditions(i)->operation);
             sbufWriteU8(dst, logicConditions(i)->operandA.type);
             sbufWriteU32(dst, logicConditions(i)->operandA.value);
@@ -1949,8 +1950,9 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
 #ifdef USE_LOGIC_CONDITIONS
     case MSP2_INAV_SET_LOGIC_CONDITIONS:
         sbufReadU8Safe(&tmp_u8, src);
-        if ((dataSize == 14) && (tmp_u8 < MAX_LOGIC_CONDITIONS)) {
+        if ((dataSize == 15) && (tmp_u8 < MAX_LOGIC_CONDITIONS)) {
             logicConditionsMutable(tmp_u8)->enabled = sbufReadU8(src);
+            logicConditionsMutable(tmp_u8)->activatorId = sbufReadU8(src);
             logicConditionsMutable(tmp_u8)->operation = sbufReadU8(src);
             logicConditionsMutable(tmp_u8)->operandA.type = sbufReadU8(src);
             logicConditionsMutable(tmp_u8)->operandA.value = sbufReadU32(src);

From d64620e93bbafe594c02a9d743987bab0e4c609f Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Sat, 11 Apr 2020 23:27:49 +0200
Subject: [PATCH 075/179] Conditionally compute logic conditions

---
 src/main/common/logic_condition.c | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/src/main/common/logic_condition.c b/src/main/common/logic_condition.c
index e767d2eea2..a4bf5483c0 100644
--- a/src/main/common/logic_condition.c
+++ b/src/main/common/logic_condition.c
@@ -188,7 +188,9 @@ static int logicConditionCompute(
 
 void logicConditionProcess(uint8_t i) {
 
-    if (logicConditions(i)->enabled) {
+    const int activatorValue = logicConditionGetValue(logicConditions(i)->activatorId);
+
+    if (logicConditions(i)->enabled && activatorValue) {
         
         /*
          * Process condition only when latch flag is not set

From 93b610863d881aaa49b15ff941486f08a9995370 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Thu, 20 Feb 2020 14:34:13 +0100
Subject: [PATCH 076/179] Modify NAV state machine for ROVER and BOAT RTH
 scenario

---
 src/main/navigation/navigation.c | 23 +++++++++++++++++++++--
 1 file changed, 21 insertions(+), 2 deletions(-)

diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index 94ae07e39b..5499151da6 100755
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -1143,6 +1143,11 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n
         return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
     }
 
+    if (!STATE(ALTITUDE_CONTROL)) {
+        //If altitude control is not a thing, switch to RTH in progress instead
+        return NAV_FSM_EVENT_SUCCESS; //Will cause NAV_STATE_RTH_HEAD_HOME
+    }
+
     // If we have valid pos sensor OR we are configured to ignore GPS loss
     if ((posControl.flags.estPosStatus >= EST_USABLE) || !checkForPositionSensorTimeout() || navConfig()->general.flags.rth_climb_ignore_emerg) {
         const uint8_t rthClimbMarginPercent = STATE(FIXED_WING_LEGACY) ? FW_RTH_CLIMB_MARGIN_PERCENT : MR_RTH_CLIMB_MARGIN_PERCENT;
@@ -1248,6 +1253,11 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LAND
         return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
     }
 
+    //On ROVER and BOAT we immediately switch to the next event
+    if (!STATE(ALTITUDE_CONTROL)) {
+        return NAV_FSM_EVENT_SUCCESS;
+    }
+
     // If position ok OR within valid timeout - continue
     if ((posControl.flags.estPosStatus >= EST_USABLE) || !checkForPositionSensorTimeout()) {
 
@@ -1310,6 +1320,11 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF
 {
     UNUSED(previousState);
 
+    //On ROVER and BOAT we immediately switch to the next event
+    if (!STATE(ALTITUDE_CONTROL)) {
+        return NAV_FSM_EVENT_SUCCESS;
+    }
+
     if (!ARMING_FLAG(ARMED)) {
         return NAV_FSM_EVENT_SUCCESS;
     }
@@ -1353,7 +1368,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHING(navigatio
 {
     UNUSED(previousState);
 
-    if (navConfig()->general.flags.disarm_on_landing) {
+    //On ROVER and BOAT disarm immediately
+    if (!STATE(ALTITUDE_CONTROL) || navConfig()->general.flags.disarm_on_landing) {
         disarm(DISARM_NAVIGATION);
     }
 
@@ -1364,8 +1380,11 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHED(navigation
 {
     // Stay in this state
     UNUSED(previousState);
-    updateClimbRateToAltitudeController(-0.3f * navConfig()->general.land_descent_rate, ROC_TO_ALT_NORMAL);  // FIXME
 
+    if (STATE(ALTITUDE_CONTROL)) {
+        updateClimbRateToAltitudeController(-0.3f * navConfig()->general.land_descent_rate, ROC_TO_ALT_NORMAL);  // FIXME
+    }
+    
     // Prevent I-terms growing when already landed
     pidResetErrorAccumulators();
     return NAV_FSM_EVENT_NONE;

From 5d787cb5c9bd1134fe0b43e13c1aa2897008a9c4 Mon Sep 17 00:00:00 2001
From: stronnag <jh+github@daria.co.uk>
Date: Mon, 13 Apr 2020 11:08:15 +0100
Subject: [PATCH 077/179] [NAV] allow forward JUMP WPs (#5591)

Alllow forward as well as backward jumps
---
 src/main/navigation/navigation.c | 12 ++++++++----
 1 file changed, 8 insertions(+), 4 deletions(-)

diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index 5499151da6..5c155676b6 100755
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -1384,7 +1384,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHED(navigation
     if (STATE(ALTITUDE_CONTROL)) {
         updateClimbRateToAltitudeController(-0.3f * navConfig()->general.land_descent_rate, ROC_TO_ALT_NORMAL);  // FIXME
     }
-    
+
     // Prevent I-terms growing when already landed
     pidResetErrorAccumulators();
     return NAV_FSM_EVENT_NONE;
@@ -3236,12 +3236,16 @@ navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass)
         }
     }
 
-    // Don't allow arming if any of JUMP waypoint has invalid settings
-    // Note JUMP only goes to previous WPs, which must be 2 indices back
+        /*
+         * Don't allow arming if any of JUMP waypoint has invalid settings
+         * First WP can't be JUMP
+         * Can't jump to immediately adjacent WPs (pointless)
+         * Can't jump beyond WP list
+         */
     if (posControl.waypointCount > 0) {
         for (uint8_t wp = 0; wp < posControl.waypointCount ; wp++){
             if (posControl.waypointList[wp].action == NAV_WP_ACTION_JUMP){
-                if((wp < 2) || (posControl.waypointList[wp].p1 > (wp-2)) || (posControl.waypointList[wp].p2 < -1)) {
+                if((wp == 0) || ((posControl.waypointList[wp].p1 > (wp-2)) && (posControl.waypointList[wp].p1 < (wp+2)) ) || (posControl.waypointList[wp].p1 >=  posControl.waypointCount) || (posControl.waypointList[wp].p2 < -1)) {
                     return NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR;
                 }
             }

From f933381d1840638df89f25da733af026ac13e2e5 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Mon, 13 Apr 2020 14:24:49 +0200
Subject: [PATCH 078/179] Simplified 2D navigation routenes

---
 make/source.mk                              |  1 +
 src/main/navigation/navigation.c            |  4 +-
 src/main/navigation/navigation_private.h    |  7 +++
 src/main/navigation/navigation_rover_boat.c | 68 +++++++++++++++++++++
 4 files changed, 79 insertions(+), 1 deletion(-)
 create mode 100644 src/main/navigation/navigation_rover_boat.c

diff --git a/make/source.mk b/make/source.mk
index f781f6f473..565c806c2b 100644
--- a/make/source.mk
+++ b/make/source.mk
@@ -201,6 +201,7 @@ COMMON_SRC = \
             navigation/navigation_pos_estimator.c \
             navigation/navigation_pos_estimator_agl.c \
             navigation/navigation_pos_estimator_flow.c \
+            navigation/navigation_rover_boat.c \
             sensors/barometer.c \
             sensors/pitotmeter.c \
             sensors/rangefinder.c \
diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index 5499151da6..a42e62903f 100755
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -2982,7 +2982,9 @@ void applyWaypointNavigationAndAltitudeHold(void)
 
     /* Process controllers */
     navigationFSMStateFlags_t navStateFlags = navGetStateFlags(posControl.navState);
-    if (STATE(FIXED_WING_LEGACY)) {
+    if (STATE(ROVER) || STATE(BOAT)) {
+        applyRoverBoatNavigationController(navStateFlags, currentTimeUs);
+    } else if (STATE(FIXED_WING_LEGACY)) {
         applyFixedWingNavigationController(navStateFlags, currentTimeUs);
     }
     else {
diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h
index 2a17449318..48beccf01f 100755
--- a/src/main/navigation/navigation_private.h
+++ b/src/main/navigation/navigation_private.h
@@ -444,6 +444,8 @@ bool adjustFixedWingAltitudeFromRCInput(void);
 bool adjustFixedWingHeadingFromRCInput(void);
 bool adjustFixedWingPositionFromRCInput(void);
 
+void applyFixedWingPositionController(timeUs_t currentTimeUs);
+
 void applyFixedWingNavigationController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs);
 
 void calculateFixedWingInitialHoldPosition(fpVector3_t * pos);
@@ -456,4 +458,9 @@ bool isFixedWingLaunchFinishedOrAborted(void);
 void abortFixedWingLaunch(void);
 void applyFixedWingLaunchController(timeUs_t currentTimeUs);
 
+/*
+ * Rover specific functions
+ */
+void applyRoverBoatNavigationController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs);
+
 #endif
diff --git a/src/main/navigation/navigation_rover_boat.c b/src/main/navigation/navigation_rover_boat.c
new file mode 100644
index 0000000000..d2b473fb67
--- /dev/null
+++ b/src/main/navigation/navigation_rover_boat.c
@@ -0,0 +1,68 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute 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.
+ *
+ * This file 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 this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#include "platform.h"
+
+FILE_COMPILE_FOR_SIZE
+
+#ifdef USE_NAV
+
+#include "common/utils.h"
+#include "fc/rc_controls.h"
+#include "flight/mixer.h"
+
+#include "navigation/navigation.h"
+#include "navigation/navigation_private.h"
+
+void applyRoverBoatPitchRollThrottleController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs)
+{
+    UNUSED(currentTimeUs);
+    rcCommand[ROLL] = 0;
+    rcCommand[PITCH] = 0;
+
+    if (navStateFlags & NAV_CTL_POS) {
+
+        // if (isYawAdjustmentValid) {
+        rcCommand[YAW] = posControl.rcAdjustment[YAW];
+        // }
+
+        // const uint16_t correctedThrottleValue = constrain(navConfig()->fw.cruise_throttle, navConfig()->fw.min_throttle, navConfig()->fw.max_throttle);
+        rcCommand[THROTTLE] = constrain(navConfig()->fw.cruise_throttle, motorConfig()->mincommand, motorConfig()->maxthrottle);
+    }
+}
+
+void applyRoverBoatNavigationController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs)
+{
+    if (navStateFlags & NAV_CTL_EMERG) {
+        rcCommand[ROLL] = 0;
+        rcCommand[PITCH] = 0;
+        rcCommand[YAW] = 0;
+        rcCommand[THROTTLE] = failsafeConfig()->failsafe_throttle;
+    } else if (navStateFlags & NAV_CTL_POS) {
+        applyFixedWingPositionController(currentTimeUs);
+        applyRoverBoatPitchRollThrottleController(navStateFlags, currentTimeUs);
+    }
+}
+
+#endif
\ No newline at end of file

From a5f0d1b1235f32f2fed0a27b511e7f884eacbf06 Mon Sep 17 00:00:00 2001
From: "Konstantin (DigitalEntity) Sharlaimov"
 <konstantin.sharlaimov@gmail.com>
Date: Tue, 7 Apr 2020 12:55:39 +0200
Subject: [PATCH 079/179] [VTX] Refactor TRAMP VTX control code [VTX] Make
 TRAMP driver capable of dynamic power table selection [VTX] Make sure TRAMP
 queries the VTX state after reset

---
 src/main/common/crc.c   |  11 +
 src/main/common/crc.h   |   2 +
 src/main/io/vtx_tramp.c | 944 ++++++++++++++++++++--------------------
 src/main/io/vtx_tramp.h |  31 +-
 4 files changed, 485 insertions(+), 503 deletions(-)

diff --git a/src/main/common/crc.c b/src/main/common/crc.c
index 87c3486eda..d11d37ae9e 100644
--- a/src/main/common/crc.c
+++ b/src/main/common/crc.c
@@ -132,3 +132,14 @@ uint8_t crc8_update(uint8_t crc, const void *data, uint32_t length)
     }
     return crc;
 }
+
+uint8_t crc8_sum_update(uint8_t crc, const void *data, uint32_t length)
+{
+    const uint8_t *p = (const uint8_t *)data;
+    const uint8_t *pend = p + length;
+
+    for (; p != pend; p++) {
+        crc += *p;
+    }
+    return crc;
+}
\ No newline at end of file
diff --git a/src/main/common/crc.h b/src/main/common/crc.h
index 2161838e34..9e02cc88d5 100644
--- a/src/main/common/crc.h
+++ b/src/main/common/crc.h
@@ -34,3 +34,5 @@ void crc8_xor_sbuf_append(struct sbuf_s *dst, uint8_t *start);
 
 uint8_t crc8(uint8_t crc, uint8_t a);
 uint8_t crc8_update(uint8_t crc, const void *data, uint32_t length);
+
+uint8_t crc8_sum_update(uint8_t crc, const void *data, uint32_t length);
\ No newline at end of file
diff --git a/src/main/io/vtx_tramp.c b/src/main/io/vtx_tramp.c
index 14f0811b31..3e95ac1ae8 100644
--- a/src/main/io/vtx_tramp.c
+++ b/src/main/io/vtx_tramp.c
@@ -30,11 +30,12 @@
 #if defined(USE_VTX_TRAMP) && defined(USE_VTX_CONTROL)
 
 #include "build/debug.h"
+#include "drivers/vtx_common.h"
+#include "drivers/time.h"
 
 #include "common/maths.h"
 #include "common/utils.h"
-
-#include "drivers/vtx_common.h"
+#include "common/crc.h"
 
 #include "io/serial.h"
 #include "io/vtx_tramp.h"
@@ -42,565 +43,556 @@
 #include "io/vtx.h"
 #include "io/vtx_string.h"
 
-const uint16_t trampPowerTable[VTX_TRAMP_POWER_COUNT] = {
-    25, 100, 200, 400, 600
-};
+#define VTX_PKT_SIZE                16
+#define VTX_PROTO_STATE_TIMEOUT_MS  1000
+#define VTX_STATUS_INTERVAL_MS      2000
 
-const char * const trampPowerNames[VTX_TRAMP_POWER_COUNT+1] = {
-    "---", "25 ", "100", "200", "400", "600"
-};
-
-static const vtxVTable_t trampVTable; // forward
-static vtxDevice_t vtxTramp = {
-    .vTable = &trampVTable,
-    .capability.bandCount = VTX_TRAMP_BAND_COUNT,
-    .capability.channelCount = VTX_TRAMP_CHANNEL_COUNT,
-    .capability.powerCount = VTX_TRAMP_POWER_COUNT,
-    .capability.bandNames = (char **)vtx58BandNames,
-    .capability.channelNames = (char **)vtx58ChannelNames,
-    .capability.powerNames = (char **)trampPowerNames,
-};
-
-static serialPort_t *trampSerialPort = NULL;
-
-static uint8_t trampRespBuffer[16];
+#define VTX_UPDATE_REQ_NONE         0x00
+#define VTX_UPDATE_REQ_FREQUENCY    0x01
+#define VTX_UPDATE_REQ_POWER        0x02
+#define VTX_UPDATE_REQ_PITMODE      0x04
 
 typedef enum {
-    TRAMP_STATUS_BAD_DEVICE = -1,
-    TRAMP_STATUS_OFFLINE = 0,
-    TRAMP_STATUS_ONLINE,
-    TRAMP_STATUS_SET_FREQ_PW,
-    TRAMP_STATUS_CHECK_FREQ_PW
-} trampStatus_e;
-
-trampStatus_e trampStatus = TRAMP_STATUS_OFFLINE;
-
-uint32_t trampRFFreqMin;
-uint32_t trampRFFreqMax;
-uint32_t trampRFPowerMax;
-
-trampData_t trampData;
-
-// Maximum number of requests sent to try a config change
-#define TRAMP_MAX_RETRIES 2
-
-uint32_t trampConfFreq = 0;
-uint8_t  trampFreqRetries = 0;
-
-uint16_t trampConfPower = 0;
-uint8_t  trampPowerRetries = 0;
-
-static void trampWriteBuf(uint8_t *buf)
-{
-    serialWriteBuf(trampSerialPort, buf, 16);
-}
-
-static uint8_t trampChecksum(uint8_t *trampBuf)
-{
-    uint8_t cksum = 0;
-
-    for (int i = 1 ; i < 14 ; i++) {
-        cksum += trampBuf[i];
-    }
-
-    return cksum;
-}
-
-void trampCmdU16(uint8_t cmd, uint16_t param)
-{
-    if (!trampSerialPort) {
-        return;
-    }
-
-    uint8_t trampReqBuffer[16];
-    memset(trampReqBuffer, 0, ARRAYLEN(trampReqBuffer));
-    trampReqBuffer[0] = 15;
-    trampReqBuffer[1] = cmd;
-    trampReqBuffer[2] = param & 0xff;
-    trampReqBuffer[3] = (param >> 8) & 0xff;
-    trampReqBuffer[14] = trampChecksum(trampReqBuffer);
-    trampWriteBuf(trampReqBuffer);
-}
-
-static void trampDevSetFreq(uint16_t freq)
-{
-    trampConfFreq = freq;
-    if (trampConfFreq != trampData.curFreq) {
-        trampFreqRetries = TRAMP_MAX_RETRIES;
-    }
-}
-
-void trampSetFreq(uint16_t freq)
-{
-    trampData.setByFreqFlag = true;         //set freq via MHz value
-    trampDevSetFreq(freq);
-}
-
-void trampSendFreq(uint16_t freq)
-{
-    trampCmdU16('F', freq);
-}
-
-static bool trampValidateBandAndChannel(uint8_t band, uint8_t channel)
-{
-    return (band >= VTX_TRAMP_MIN_BAND && band <= VTX_TRAMP_MAX_BAND &&
-            channel >= VTX_TRAMP_MIN_CHANNEL && channel <= VTX_TRAMP_MAX_CHANNEL);
-}
-
-static void trampDevSetBandAndChannel(uint8_t band, uint8_t channel)
-{
-    trampDevSetFreq(vtx58_Bandchan2Freq(band, channel));
-}
-
-void trampSetBandAndChannel(uint8_t band, uint8_t channel)
-{
-    trampData.setByFreqFlag = false;        //set freq via band/channel
-    trampDevSetBandAndChannel(band, channel);
-}
-
-void trampSetRFPower(uint16_t level)
-{
-    trampConfPower = level;
-    if (trampConfPower != trampData.power) {
-        trampPowerRetries = TRAMP_MAX_RETRIES;
-    }
-}
-
-void trampSendRFPower(uint16_t level)
-{
-    trampCmdU16('P', level);
-}
-
-// return false if error
-bool trampCommitChanges(void)
-{
-    if (trampStatus != TRAMP_STATUS_ONLINE) {
-        return false;
-    }
-
-    trampStatus = TRAMP_STATUS_SET_FREQ_PW;
-    return true;
-}
-
-// return false if index out of range
-static bool trampDevSetPowerByIndex(uint8_t index)
-{
-    if (index > 0 && index <= VTX_TRAMP_POWER_COUNT) {
-        trampSetRFPower(trampPowerTable[index - 1]);
-        trampCommitChanges();
-        return true;
-    }
-    return false;
-}
-
-void trampSetPitMode(uint8_t onoff)
-{
-    trampCmdU16('I', onoff ? 0 : 1);
-}
-
-// returns completed response code
-char trampHandleResponse(void)
-{
-    const uint8_t respCode = trampRespBuffer[1];
-
-    switch (respCode) {
-    case 'r':
-        {
-            const uint16_t min_freq = trampRespBuffer[2]|(trampRespBuffer[3] << 8);
-            if (min_freq != 0) {
-                trampRFFreqMin = min_freq;
-                trampRFFreqMax = trampRespBuffer[4]|(trampRespBuffer[5] << 8);
-                trampRFPowerMax = trampRespBuffer[6]|(trampRespBuffer[7] << 8);
-                return 'r';
-            }
-
-            // throw bytes echoed from tx to rx in bidirectional mode away
-        }
-        break;
-
-    case 'v':
-        {
-            const uint16_t freq = trampRespBuffer[2]|(trampRespBuffer[3] << 8);
-            if (freq != 0) {
-                trampData.curFreq = freq;
-                trampData.configuredPower = trampRespBuffer[4]|(trampRespBuffer[5] << 8);
-                trampData.pitMode = trampRespBuffer[7];
-                trampData.power = trampRespBuffer[8]|(trampRespBuffer[9] << 8);
-
-                // if no band/chan match then make sure set-by-freq mode is flagged
-                if (!vtx58_Freq2Bandchan(trampData.curFreq, &trampData.band, &trampData.channel)) {
-                    trampData.setByFreqFlag = true;
-                }
-
-                if (trampConfFreq == 0)  trampConfFreq  = trampData.curFreq;
-                if (trampConfPower == 0) trampConfPower = trampData.power;
-                return 'v';
-            }
-
-            // throw bytes echoed from tx to rx in bidirectional mode away
-        }
-        break;
-
-    case 's':
-        {
-            const uint16_t temp = (int16_t)(trampRespBuffer[6]|(trampRespBuffer[7] << 8));
-            if (temp != 0) {
-                trampData.temperature = temp;
-                return 's';
-            }
-        }
-        break;
-    }
-
-    return 0;
-}
+    VTX_STATE_RESET         = 0,
+    VTX_STATE_OFFILE        = 1,    // Not detected
+    VTX_STATE_DETECTING     = 2,    // 
+    VTX_STATE_IDLE          = 3,    // Idle, ready to sent commands
+    VTX_STATE_QUERY_DELAY   = 4,
+    VTX_STATE_QUERY_STATUS  = 5,
+    VTX_STATE_WAIT_STATUS   = 6,    // Wait for VTX state
+} vtxProtoState_e;
 
 typedef enum {
-    S_WAIT_LEN = 0,   // Waiting for a packet len
-    S_WAIT_CODE,      // Waiting for a response code
-    S_DATA,           // Waiting for rest of the packet.
-} trampReceiveState_e;
+    VTX_RESPONSE_TYPE_NONE,
+    VTX_RESPONSE_TYPE_CAPABILITIES,
+    VTX_RESPONSE_TYPE_STATUS,
+} vtxProtoResponseType_e;
 
-static trampReceiveState_e trampReceiveState = S_WAIT_LEN;
-static int trampReceivePos = 0;
+typedef struct {
+    vtxProtoState_e protoState;
+    timeMs_t        lastStateChangeMs;
+    timeMs_t        lastStatusQueryMs;
+    int             protoTimeoutCount;
+    unsigned        updateReqMask;
 
-static void trampResetReceiver(void)
-{
-    trampReceiveState = S_WAIT_LEN;
-    trampReceivePos = 0;
-}
+    // VTX capabilities
+    struct {
+        unsigned freqMin;   // min freq
+        unsigned freqMax;   // max freq
+        unsigned powerMax;  //
+    } capabilities;
+
+    // Requested VTX state
+    struct {
+        // Only tracking
+        int      band;
+        int      channel;
+        unsigned powerIndex;
+
+        // Actual settings to send to the VTX
+        unsigned freq;
+        unsigned power;
+    } request;
+
+    // Actual VTX state: updated from actual VTX
+    struct {
+        unsigned freq;      // Frequency in MHz
+        unsigned power;
+        unsigned temp;
+        bool     pitMode;
+    } state;
+
+    struct {
+        int              powerTableCount;
+        const uint16_t * powerTablePtr;
+    } metadata;
+
+    // Comms flags and state
+    uint8_t         sendPkt[VTX_PKT_SIZE];
+    uint8_t         recvPkt[VTX_PKT_SIZE];
+    unsigned        recvPtr;
+    serialPort_t *  port;
+} vtxProtoState_t;
+
+static vtxProtoState_t vtxState;
+
+static void vtxProtoUpdatePowerMetadata(uint16_t maxPower);
 
 static bool trampIsValidResponseCode(uint8_t code)
 {
-    if (code == 'r' || code == 'v' || code == 's') {
-        return true;
-    } else {
-        return false;
-    }
+    return (code == 'r' || code == 'v' || code == 's');
 }
 
-// returns completed response code or 0
-static char trampReceive(uint32_t currentTimeUs)
+static bool vtxProtoRecv(void)
 {
+    uint8_t * bufPtr = (uint8_t*)&vtxState.recvPkt;
+    while (serialRxBytesWaiting(vtxState.port)) {
+        const uint8_t c = serialRead(vtxState.port);
+
+        if (vtxState.recvPtr == 0) {
+            // Wait for sync byte
+            if (c == 0x0F) {
+                bufPtr[vtxState.recvPtr++] = c;
+            }
+        }
+        else if (vtxState.recvPtr == 1) {
+            // Check if we received a valid response code
+            if (trampIsValidResponseCode(c)) {
+                bufPtr[vtxState.recvPtr++] = c;
+            }
+            else {
+                vtxState.recvPtr = 0;
+            }
+        }
+        else {
+            // Consume character and check if we have got a full packet
+            if (vtxState.recvPtr < VTX_PKT_SIZE) {
+                bufPtr[vtxState.recvPtr++] = c;
+            }
+
+            if (vtxState.recvPtr == VTX_PKT_SIZE) {
+                // Full packet received - validate packet, make sure it's the one we expect
+                const bool pktValid = ((bufPtr[14] == crc8_sum_update(0, &bufPtr[1], 13)) && (bufPtr[15] == 0));
+
+                if (!pktValid) {
+                    // Reset the receiver state - keep waiting
+                    vtxState.recvPtr = 0;
+                }
+                // Make sure it's not the echo one (half-duplex serial might receive it's own data)
+                else if (memcmp(&vtxState.recvPkt, &vtxState.sendPkt, VTX_PKT_SIZE) == 0) {
+                    vtxState.recvPtr = 0;
+                }
+                // Valid receive packet
+                else {
+                    return true;
+                }
+            }
+        }
+    }
+
+    return false;
+}
+
+static void vtxProtoSend(uint8_t cmd, uint16_t param)
+{
+    // Craft the packet
+    memset(vtxState.sendPkt, 0, ARRAYLEN(vtxState.sendPkt));
+    vtxState.sendPkt[0] = 15;
+    vtxState.sendPkt[1] = cmd;
+    vtxState.sendPkt[2] = param & 0xff;
+    vtxState.sendPkt[3] = (param >> 8) & 0xff;
+    vtxState.sendPkt[14] = crc8_sum_update(0, &vtxState.sendPkt[1], 13);
+
+    // Send data 
+    serialWriteBuf(vtxState.port, (uint8_t *)&vtxState.sendPkt, sizeof(vtxState.sendPkt));
+
+    // Reset cmd response state
+    vtxState.recvPtr = 0;
+}
+
+static void vtxProtoSetState(vtxProtoState_e newState)
+{
+    vtxState.lastStateChangeMs = millis();
+    vtxState.protoState = newState;
+}
+
+static bool vtxProtoTimeout(void)
+{
+    return (millis() - vtxState.lastStateChangeMs) > VTX_PROTO_STATE_TIMEOUT_MS;
+}
+
+static void vtxProtoQueryCapabilities(void)
+{
+    vtxProtoSend(0x72, 0);
+}
+
+static void vtxProtoQueryStatus(void)
+{
+    vtxProtoSend(0x76, 0);
+    vtxState.lastStatusQueryMs = millis();
+}
+
+/*
+static void vtxProtoQueryTemperature(void)
+{
+    vtxProtoSend('s', 0);
+}
+*/
+
+static vtxProtoResponseType_e vtxProtoProcessResponse(void)
+{
+    const uint8_t respCode = vtxState.recvPkt[1];
+
+    switch (respCode) {
+        case 0x72:
+            vtxState.capabilities.freqMin = vtxState.recvPkt[2] | (vtxState.recvPkt[3] << 8);
+            vtxState.capabilities.freqMax = vtxState.recvPkt[4] | (vtxState.recvPkt[5] << 8);
+            vtxState.capabilities.powerMax = vtxState.recvPkt[6] | (vtxState.recvPkt[7] << 8);
+            if (vtxState.capabilities.freqMin != 0 && vtxState.capabilities.freqMin < vtxState.capabilities.freqMax) {
+                // Update max power metadata so OSD settings would match VTX capabiolities
+                vtxProtoUpdatePowerMetadata(vtxState.capabilities.powerMax);
+                return VTX_RESPONSE_TYPE_CAPABILITIES;
+            }
+            break;
+
+        case 0x76:
+            vtxState.state.freq = vtxState.recvPkt[2] | (vtxState.recvPkt[3] << 8);
+            vtxState.state.power = vtxState.recvPkt[4]|(vtxState.recvPkt[5] << 8);
+            vtxState.state.pitMode = vtxState.recvPkt[7];
+            //vtxState.state.power = vtxState.recvPkt[8]|(vtxState.recvPkt[9] << 8);
+            return VTX_RESPONSE_TYPE_STATUS;
+    }
+
+    return VTX_RESPONSE_TYPE_NONE;
+}
+
+static void vtxProtoSetPitMode(uint16_t mode)
+{
+    vtxProtoSend(0x73, mode);
+}
+
+static void vtxProtoSetPower(uint16_t power)
+{
+    vtxProtoSend(0x50, power);
+}
+
+static void vtxProtoSetFrequency(uint16_t freq)
+{
+    vtxProtoSend(0x46, freq);
+}
+
+static void impl_Process(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs)
+{
+    // Glue function betwen VTX VTable and actual driver protothread
+    UNUSED(vtxDevice);
     UNUSED(currentTimeUs);
 
-    if (!trampSerialPort) {
-        return 0;
+    if (!vtxState.port) {
+        return false;
     }
 
-    while (serialRxBytesWaiting(trampSerialPort)) {
-        const uint8_t c = serialRead(trampSerialPort);
-        trampRespBuffer[trampReceivePos++] = c;
+    switch((int)vtxState.protoState) {
+        case VTX_STATE_RESET:
+            vtxState.protoTimeoutCount = 0;
+            vtxState.updateReqMask = VTX_UPDATE_REQ_NONE;
+            vtxProtoSetState(VTX_STATE_OFFILE);
+            break;
 
-        switch (trampReceiveState) {
-        case S_WAIT_LEN:
-            if (c == 0x0F) {
-                trampReceiveState = S_WAIT_CODE;
-            } else {
-                trampReceivePos = 0;
+        // Send request for capabilities
+        case VTX_STATE_OFFILE:
+            vtxProtoQueryCapabilities();
+            vtxProtoSetState(VTX_STATE_DETECTING);
+            break;
+
+        // Detect VTX. We only accept VTX_RESPONSE_TYPE_CAPABILITIES here
+        case VTX_STATE_DETECTING:
+            if (vtxProtoRecv()) {
+                if (vtxProtoProcessResponse() == VTX_RESPONSE_TYPE_CAPABILITIES) {
+                    // VTX sent capabilities. Query status now
+                    vtxState.protoTimeoutCount = 0;
+                    vtxProtoSetState(VTX_STATE_QUERY_STATUS);
+                }
+                else {
+                    // Unexpected response. Re-initialize
+                    vtxProtoSetState(VTX_STATE_RESET);
+                }
+            }
+            else if (vtxProtoTimeout()) {
+                // Time-out while waiting for capabilities. Reset the state
+                vtxProtoSetState(VTX_STATE_RESET);
             }
             break;
 
-        case S_WAIT_CODE:
-            if (trampIsValidResponseCode(c)) {
-                trampReceiveState = S_DATA;
-            } else {
-                trampResetReceiver();
+        // Send requests to update freqnecy and power, periodically poll device for liveness
+        case VTX_STATE_IDLE:
+            if (vtxState.updateReqMask != VTX_UPDATE_REQ_NONE) {
+                // Updates pending. Send an appropriate command
+                if (vtxState.updateReqMask & VTX_UPDATE_REQ_PITMODE) {
+                    // Only disabling PIT mode supported
+                    vtxState.updateReqMask &= ~VTX_UPDATE_REQ_PITMODE;
+                    vtxProtoSetPitMode(0);
+                    vtxProtoSetState(VTX_STATE_QUERY_DELAY);
+                }
+                else if (vtxState.updateReqMask & VTX_UPDATE_REQ_FREQUENCY) {
+                    vtxState.updateReqMask &= ~VTX_UPDATE_REQ_FREQUENCY;
+                    vtxProtoSetFrequency(vtxState.request.freq);
+                    vtxProtoSetState(VTX_STATE_QUERY_DELAY);
+                }
+                else if (vtxState.updateReqMask & VTX_UPDATE_REQ_POWER) {
+                    vtxState.updateReqMask &= ~VTX_UPDATE_REQ_POWER;
+                    vtxProtoSetPower(vtxState.request.power);
+                    vtxProtoSetState(VTX_STATE_QUERY_DELAY);
+                }
+            }
+            else if ((millis() - vtxState.lastStatusQueryMs) > VTX_STATUS_INTERVAL_MS) {
+                // Poll VTX for status updates
+                vtxProtoSetState(VTX_STATE_QUERY_STATUS);
             }
             break;
 
-        case S_DATA:
-            if (trampReceivePos == 16) {
-                uint8_t cksum = trampChecksum(trampRespBuffer);
+        case VTX_STATE_QUERY_DELAY:
+            // We get here after sending the command. We give VTX some time to process the command
+            // and switch to VTX_STATE_QUERY_STATUS
+            if (vtxProtoTimeout()) {
+                // We gave VTX some time to process the command. Query status to confirm success
+                vtxProtoSetState(VTX_STATE_QUERY_STATUS);
+            }
+            break;
 
-                trampResetReceiver();
+        case VTX_STATE_QUERY_STATUS:
+            // Just query status, nothing special
+            vtxProtoQueryStatus();
+            vtxProtoSetState(VTX_STATE_WAIT_STATUS);
+            break;
 
-                if ((trampRespBuffer[14] == cksum) && (trampRespBuffer[15] == 0)) {
-                    return trampHandleResponse();
+        case VTX_STATE_WAIT_STATUS:
+            if (vtxProtoRecv()) {
+                vtxState.protoTimeoutCount = 0;
+
+                if (vtxProtoProcessResponse() == VTX_RESPONSE_TYPE_STATUS) {
+                    // Check if VTX state matches VTX request
+                    if (!(vtxState.updateReqMask & VTX_UPDATE_REQ_FREQUENCY) && (vtxState.state.freq != vtxState.request.freq)) {
+                        vtxState.updateReqMask |= VTX_UPDATE_REQ_FREQUENCY;
+                    }
+
+                    if (!(vtxState.updateReqMask & VTX_UPDATE_REQ_POWER) && (vtxState.state.power != vtxState.request.power)) {
+                        vtxState.updateReqMask |= VTX_UPDATE_REQ_POWER;
+                    }
+
+                    // We got the status response - proceed to IDLE
+                    vtxProtoSetState(VTX_STATE_IDLE);
+                }
+                else {
+                    // Unexpected response. Query for STATUS again
+                    vtxProtoSetState(VTX_STATE_QUERY_STATUS);
+                }
+            }
+            else if (vtxProtoTimeout()) {
+                vtxState.protoTimeoutCount++;
+                if (vtxState.protoTimeoutCount > 3) {
+                    vtxProtoSetState(VTX_STATE_RESET);
+                }
+                else {
+                    vtxProtoSetState(VTX_STATE_QUERY_STATUS);
                 }
             }
             break;
-
-        default:
-            trampResetReceiver();
-            break;
-        }
     }
-
-    return 0;
 }
 
-void trampQuery(uint8_t cmd)
-{
-    trampResetReceiver();
-    trampCmdU16(cmd, 0);
-}
-
-void trampQueryR(void)
-{
-    trampQuery('r');
-}
-
-void trampQueryV(void)
-{
-    trampQuery('v');
-}
-
-void trampQueryS(void)
-{
-    trampQuery('s');
-}
-
-static void vtxTrampProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs)
-{
-    UNUSED(vtxDevice);
-
-    static timeUs_t lastQueryTimeUs = 0;
-    static bool initSettingsDoneFlag = false;
-
-#ifdef TRAMP_DEBUG
-    static uint16_t debugFreqReqCounter = 0;
-    static uint16_t debugPowReqCounter = 0;
-#endif
-
-    if (trampStatus == TRAMP_STATUS_BAD_DEVICE) {
-        return;
-    }
-
-    const char replyCode = trampReceive(currentTimeUs);
-
-#ifdef TRAMP_DEBUG
-    debug[0] = trampStatus;
-#endif
-
-    switch (replyCode) {
-    case 'r':
-        if (trampStatus <= TRAMP_STATUS_OFFLINE) {
-            trampStatus = TRAMP_STATUS_ONLINE;
-
-            // once device is ready enter vtx settings
-            if (!initSettingsDoneFlag) {
-                initSettingsDoneFlag = true;
-                // if vtx_band!=0 then enter 'vtx_band/chan' values (and power)
-            }
-        }
-        break;
-
-    case 'v':
-         if (trampStatus == TRAMP_STATUS_CHECK_FREQ_PW) {
-             trampStatus = TRAMP_STATUS_SET_FREQ_PW;
-         }
-         break;
-    }
-
-    switch (trampStatus) {
-
-    case TRAMP_STATUS_OFFLINE:
-    case TRAMP_STATUS_ONLINE:
-        if (cmp32(currentTimeUs, lastQueryTimeUs) > 1000 * 1000) { // 1s
-
-            if (trampStatus == TRAMP_STATUS_OFFLINE) {
-                trampQueryR();
-            } else {
-                static unsigned int cnt = 0;
-                if (((cnt++) & 1) == 0) {
-                    trampQueryV();
-                } else {
-                    trampQueryS();
-                }
-            }
-
-            lastQueryTimeUs = currentTimeUs;
-        }
-        break;
-
-    case TRAMP_STATUS_SET_FREQ_PW:
-        {
-            bool done = true;
-            if (trampConfFreq && trampFreqRetries && (trampConfFreq != trampData.curFreq)) {
-                trampSendFreq(trampConfFreq);
-                trampFreqRetries--;
-#ifdef TRAMP_DEBUG
-                debugFreqReqCounter++;
-#endif
-                done = false;
-            } else if (trampConfPower && trampPowerRetries && (trampConfPower != trampData.configuredPower)) {
-                trampSendRFPower(trampConfPower);
-                trampPowerRetries--;
-#ifdef TRAMP_DEBUG
-                debugPowReqCounter++;
-#endif
-                done = false;
-            }
-
-            if (!done) {
-                trampStatus = TRAMP_STATUS_CHECK_FREQ_PW;
-
-                // delay next status query by 300ms
-                lastQueryTimeUs = currentTimeUs + 300 * 1000;
-            } else {
-                // everything has been done, let's return to original state
-                trampStatus = TRAMP_STATUS_ONLINE;
-                // reset configuration value in case it failed (no more retries)
-                trampConfFreq  = trampData.curFreq;
-                trampConfPower = trampData.power;
-                trampFreqRetries = trampPowerRetries = 0;
-            }
-        }
-        break;
-
-    case TRAMP_STATUS_CHECK_FREQ_PW:
-        if (cmp32(currentTimeUs, lastQueryTimeUs) > 200 * 1000) {
-            trampQueryV();
-            lastQueryTimeUs = currentTimeUs;
-        }
-        break;
-
-    default:
-        break;
-    }
-
-#ifdef TRAMP_DEBUG
-    debug[1] = debugFreqReqCounter;
-    debug[2] = debugPowReqCounter;
-    debug[3] = 0;
-#endif
-}
-
-
-// Interface to common VTX API
-
-static vtxDevType_e vtxTrampGetDeviceType(const vtxDevice_t *vtxDevice)
+static vtxDevType_e impl_GetDeviceType(const vtxDevice_t *vtxDevice)
 {
     UNUSED(vtxDevice);
     return VTXDEV_TRAMP;
 }
 
-static bool vtxTrampIsReady(const vtxDevice_t *vtxDevice)
+static bool impl_IsReady(const vtxDevice_t *vtxDevice)
 {
-    return vtxDevice!=NULL && trampStatus > TRAMP_STATUS_OFFLINE;
+    return vtxDevice != NULL && vtxState.port != NULL && vtxState.protoState >= VTX_STATE_IDLE;
 }
 
-static void vtxTrampSetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint8_t channel)
+static void impl_SetBandAndChannel(vtxDevice_t * vtxDevice, uint8_t band, uint8_t channel)
 {
     UNUSED(vtxDevice);
-    if (trampValidateBandAndChannel(band, channel)) {
-        trampSetBandAndChannel(band, channel);
-        trampCommitChanges();
+
+    if (!impl_IsReady(vtxDevice)) {
+        return;
+    }
+
+    // TRAMP is 5.8 GHz only
+    uint16_t newFreqMhz  = vtx58_Bandchan2Freq(band, channel);
+
+    if (newFreqMhz < vtxState.capabilities.freqMin || newFreqMhz > vtxState.capabilities.freqMax) {
+        return false;
+    }
+
+    // Cache band and channel
+    vtxState.request.band = band;
+    vtxState.request.channel = channel;
+    vtxState.request.freq = newFreqMhz;
+    vtxState.updateReqMask |= VTX_UPDATE_REQ_FREQUENCY;
+}
+
+static void impl_SetPowerByIndex(vtxDevice_t * vtxDevice, uint8_t index)
+{
+    UNUSED(vtxDevice);
+
+    if (!impl_IsReady(vtxDevice) || index < 1 || index > vtxState.metadata.powerTableCount) {
+        return;
+    }
+
+    unsigned reqPower = vtxState.metadata.powerTablePtr[index - 1];
+
+    // Cap the power to the max capability of the VTX
+    vtxState.request.power = MIN(reqPower, vtxState.capabilities.powerMax);
+    vtxState.request.powerIndex = index;
+
+    vtxState.updateReqMask |= VTX_UPDATE_REQ_POWER;
+}
+
+static void impl_SetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff)
+{
+    UNUSED(vtxDevice);
+
+    if (onoff == 0) {
+        vtxState.updateReqMask |= VTX_UPDATE_REQ_PITMODE;
     }
 }
 
-static void vtxTrampSetPowerByIndex(vtxDevice_t *vtxDevice, uint8_t index)
+static bool impl_GetBandAndChannel(const vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel)
 {
-    UNUSED(vtxDevice);
-    trampDevSetPowerByIndex(index);
-}
-
-static void vtxTrampSetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff)
-{
-    UNUSED(vtxDevice);
-    trampSetPitMode(onoff);
-}
-
-static bool vtxTrampGetBandAndChannel(const vtxDevice_t *vtxDevice, uint8_t *pBand, uint8_t *pChannel)
-{
-    if (!vtxTrampIsReady(vtxDevice)) {
+    if (!impl_IsReady(vtxDevice)) {
         return false;
     }
 
     // if in user-freq mode then report band as zero
-    *pBand = trampData.setByFreqFlag ? 0 : trampData.band;
-    *pChannel = trampData.channel;
+    *pBand = vtxState.request.band;
+    *pChannel = vtxState.request.channel;
     return true;
 }
 
-static bool vtxTrampGetPowerIndex(const vtxDevice_t *vtxDevice, uint8_t *pIndex)
+static bool impl_GetPowerIndex(const vtxDevice_t *vtxDevice, uint8_t *pIndex)
 {
-    if (!vtxTrampIsReady(vtxDevice)) {
+    if (!impl_IsReady(vtxDevice)) {
         return false;
     }
 
-    if (trampData.configuredPower > 0) {
-        for (uint8_t i = 0; i < sizeof(trampPowerTable); i++) {
-            if (trampData.configuredPower <= trampPowerTable[i]) {
-                *pIndex = i + 1;
-                break;
-            }
-        }
-    }
+    *pIndex = vtxState.request.powerIndex;
 
     return true;
 }
 
-static bool vtxTrampGetPitMode(const vtxDevice_t *vtxDevice, uint8_t *pOnOff)
+static bool impl_GetPitMode(const vtxDevice_t *vtxDevice, uint8_t *pOnOff)
 {
-    if (!vtxTrampIsReady(vtxDevice)) {
+    if (!impl_IsReady(vtxDevice)) {
         return false;
     }
 
-    *pOnOff = trampData.pitMode;
+    *pOnOff = vtxState.state.pitMode ? 1 : 0;
     return true;
 }
 
-static bool vtxTrampGetFreq(const vtxDevice_t *vtxDevice, uint16_t *pFreq)
+static bool impl_GetFreq(const vtxDevice_t *vtxDevice, uint16_t *pFreq)
 {
-    if (!vtxTrampIsReady(vtxDevice)) {
+    if (!impl_IsReady(vtxDevice)) {
         return false;
     }
 
-    *pFreq = trampData.curFreq;
+    *pFreq = vtxState.request.freq;
     return true;
 }
 
-static bool vtxTrampGetPower(const vtxDevice_t *vtxDevice, uint8_t *pIndex, uint16_t *pPowerMw)
+static bool impl_GetPower(const vtxDevice_t *vtxDevice, uint8_t *pIndex, uint16_t *pPowerMw)
 {
-    uint8_t powerIndex;
-    if (!vtxTrampGetPowerIndex(vtxDevice, &powerIndex)) {
+    if (!impl_IsReady(vtxDevice)) {
         return false;
     }
 
-    *pIndex = trampData.configuredPower ? powerIndex : 0;
-    *pPowerMw = trampData.configuredPower;
+    *pIndex = vtxState.request.powerIndex;
+    *pPowerMw = vtxState.request.power;
     return true;
 }
 
-static bool vtxTrampGetOsdInfo(const  vtxDevice_t *vtxDevice, vtxDeviceOsdInfo_t * pOsdInfo)
+static bool impl_GetOsdInfo(const  vtxDevice_t *vtxDevice, vtxDeviceOsdInfo_t * pOsdInfo)
 {
-    uint8_t powerIndex; 
-    uint16_t powerMw;
-
-    if (!vtxTrampGetPower(vtxDevice, &powerIndex, &powerMw)) {
+    if (!impl_IsReady(vtxDevice)) {
         return false;
     }
 
-    pOsdInfo->band = trampData.setByFreqFlag ? 0 : trampData.band;
-    pOsdInfo->channel = trampData.channel;
-    pOsdInfo->frequency = trampData.curFreq;
-    pOsdInfo->powerIndex = powerIndex;
-    pOsdInfo->powerMilliwatt = powerMw;
-    pOsdInfo->bandLetter = vtx58BandNames[pOsdInfo->band][0];
-    pOsdInfo->bandName = vtx58BandNames[pOsdInfo->band];
-    pOsdInfo->channelName = vtx58ChannelNames[pOsdInfo->channel];
-    pOsdInfo->powerIndexLetter = '0' + powerIndex;
+    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->bandLetter = vtx58BandNames[vtxState.request.band][0];
+    pOsdInfo->bandName = vtx58BandNames[vtxState.request.band];
+    pOsdInfo->channelName = vtx58ChannelNames[vtxState.request.channel];
+    pOsdInfo->powerIndexLetter = '0' + vtxState.request.powerIndex;
     return true;
 }
 
-
-static const vtxVTable_t trampVTable = {
-    .process = vtxTrampProcess,
-    .getDeviceType = vtxTrampGetDeviceType,
-    .isReady = vtxTrampIsReady,
-    .setBandAndChannel = vtxTrampSetBandAndChannel,
-    .setPowerByIndex = vtxTrampSetPowerByIndex,
-    .setPitMode = vtxTrampSetPitMode,
-    .getBandAndChannel = vtxTrampGetBandAndChannel,
-    .getPowerIndex = vtxTrampGetPowerIndex,
-    .getPitMode = vtxTrampGetPitMode,
-    .getFrequency = vtxTrampGetFreq,
-    .getPower = vtxTrampGetPower,
-    .getOsdInfo = vtxTrampGetOsdInfo,
+/*****************************************************************************/
+static const vtxVTable_t impl_vtxVTable = {
+    .process = impl_Process,
+    .getDeviceType = impl_GetDeviceType,
+    .isReady = impl_IsReady,
+    .setBandAndChannel = impl_SetBandAndChannel,
+    .setPowerByIndex = impl_SetPowerByIndex,
+    .setPitMode = impl_SetPitMode,
+    .getBandAndChannel = impl_GetBandAndChannel,
+    .getPowerIndex = impl_GetPowerIndex,
+    .getPitMode = impl_GetPitMode,
+    .getFrequency = impl_GetFreq,
+    .getPower = impl_GetPower,
+    .getOsdInfo = impl_GetOsdInfo,
 };
 
+static vtxDevice_t impl_vtxDevice = {
+    .vTable = &impl_vtxVTable,
+    .capability.bandCount = VTX_TRAMP_5G8_BAND_COUNT,
+    .capability.channelCount = VTX_TRAMP_5G8_CHANNEL_COUNT,
+    .capability.powerCount = VTX_TRAMP_MAX_POWER_COUNT,
+    .capability.bandNames = (char **)vtx58BandNames,
+    .capability.channelNames = (char **)vtx58ChannelNames,
+    .capability.powerNames = NULL,
+};
+
+const uint16_t trampPowerTable_200[VTX_TRAMP_MAX_POWER_COUNT]         = { 25, 100, 200, 200, 200 };
+const char * const trampPowerNames_200[VTX_TRAMP_MAX_POWER_COUNT + 1] = { "---", "25 ", "100", "200", "200", "200" };
+
+const uint16_t trampPowerTable_400[VTX_TRAMP_MAX_POWER_COUNT]         = { 25, 100, 200, 400, 400 };
+const char * const trampPowerNames_400[VTX_TRAMP_MAX_POWER_COUNT + 1] = { "---", "25 ", "100", "200", "400", "400" };
+
+const uint16_t trampPowerTable_600[VTX_TRAMP_MAX_POWER_COUNT]         = { 25, 100, 200, 400, 600 };
+const char * const trampPowerNames_600[VTX_TRAMP_MAX_POWER_COUNT + 1] = { "---", "25 ", "100", "200", "400", "600" };
+
+const uint16_t trampPowerTable_800[VTX_TRAMP_MAX_POWER_COUNT]         = { 25, 100, 200, 500, 800 };
+const char * const trampPowerNames_800[VTX_TRAMP_MAX_POWER_COUNT + 1] = { "---", "25 ", "100", "200", "500", "800" };
+
+static void vtxProtoUpdatePowerMetadata(uint16_t maxPower)
+{
+    if (maxPower >= 800) {
+        // Max power 800mW: Use 25, 100, 200, 500, 800 table
+        vtxState.metadata.powerTablePtr  = trampPowerTable_800;
+        vtxState.metadata.powerTableCount = VTX_TRAMP_MAX_POWER_COUNT;
+        
+        impl_vtxDevice.capability.powerNames = (char **)trampPowerNames_800;
+        impl_vtxDevice.capability.powerCount = VTX_TRAMP_MAX_POWER_COUNT;
+    }
+    else if (maxPower >= 600) {
+        // Max power 600mW: Use 25, 100, 200, 400, 600 table
+        vtxState.metadata.powerTablePtr  = trampPowerTable_600;
+        vtxState.metadata.powerTableCount = VTX_TRAMP_MAX_POWER_COUNT;
+
+        impl_vtxDevice.capability.powerNames = (char **)trampPowerNames_600;
+        impl_vtxDevice.capability.powerCount = VTX_TRAMP_MAX_POWER_COUNT;
+    }
+    else if (maxPower >= 400) {
+        // Max power 400mW: Use 25, 100, 200, 400 table
+        vtxState.metadata.powerTablePtr  = trampPowerTable_400;
+        vtxState.metadata.powerTableCount = 4;
+
+        impl_vtxDevice.capability.powerNames = (char **)trampPowerNames_400;
+        impl_vtxDevice.capability.powerCount = 4;
+    }
+    else if (maxPower >= 200) {
+        // Max power 200mW: Use 25, 100, 200 table
+        vtxState.metadata.powerTablePtr  = trampPowerTable_200;
+        vtxState.metadata.powerTableCount = 3;
+
+        impl_vtxDevice.capability.powerNames = (char **)trampPowerNames_200;
+        impl_vtxDevice.capability.powerCount = 3;
+    }
+    else {
+        // Default to standard TRAMP 600mW VTX
+        vtxState.metadata.powerTablePtr  = trampPowerTable_600;
+        vtxState.metadata.powerTableCount = VTX_TRAMP_MAX_POWER_COUNT;
+
+        impl_vtxDevice.capability.powerNames = (char **)trampPowerNames_600;
+        impl_vtxDevice.capability.powerCount = VTX_TRAMP_MAX_POWER_COUNT;
+    }
+
+}
 
 bool vtxTrampInit(void)
 {
@@ -609,17 +601,19 @@ bool vtxTrampInit(void)
     if (portConfig) {
         portOptions_t portOptions = 0;
         portOptions = portOptions | (vtxConfig()->halfDuplex ? SERIAL_BIDIR : SERIAL_UNIDIR);
-
-        trampSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_TRAMP, NULL, NULL, 9600, MODE_RXTX, portOptions);
+        vtxState.port = openSerialPort(portConfig->identifier, FUNCTION_VTX_TRAMP, NULL, NULL, 9600, MODE_RXTX, portOptions);
     }
 
-    if (!trampSerialPort) {
+    if (!vtxState.port) {
         return false;
     }
 
-    vtxCommonSetDevice(&vtxTramp);
+    vtxProtoUpdatePowerMetadata(600);
+    vtxCommonSetDevice(&impl_vtxDevice);
+
+    vtxState.protoState = VTX_STATE_RESET;
 
     return true;
 }
 
-#endif // VTX_TRAMP
+#endif
diff --git a/src/main/io/vtx_tramp.h b/src/main/io/vtx_tramp.h
index a9c7cd04b2..3d2643c194 100644
--- a/src/main/io/vtx_tramp.h
+++ b/src/main/io/vtx_tramp.h
@@ -19,38 +19,13 @@
 
 #include <stdint.h>
 
-#define VTX_TRAMP_MIN_BAND 1
-#define VTX_TRAMP_MAX_BAND 5
-#define VTX_TRAMP_MIN_CHANNEL 1
-#define VTX_TRAMP_MAX_CHANNEL 8
+#define VTX_TRAMP_5G8_BAND_COUNT        5
+#define VTX_TRAMP_5G8_CHANNEL_COUNT     8
 
-#define VTX_TRAMP_BAND_COUNT (VTX_TRAMP_MAX_BAND - VTX_TRAMP_MIN_BAND + 1)
-#define VTX_TRAMP_CHANNEL_COUNT (VTX_TRAMP_MAX_CHANNEL - VTX_TRAMP_MIN_CHANNEL + 1)
-
-#define VTX_TRAMP_POWER_COUNT 5
+#define VTX_TRAMP_MAX_POWER_COUNT 5
 #define VTX_TRAMP_DEFAULT_POWER 1
 
 #define VTX_TRAMP_MIN_FREQUENCY_MHZ 5000             //min freq in MHz
 #define VTX_TRAMP_MAX_FREQUENCY_MHZ 5999             //max freq in MHz
 
-extern const uint16_t trampPowerTable[VTX_TRAMP_POWER_COUNT];
-extern const char * const trampPowerNames[VTX_TRAMP_POWER_COUNT+1];
-
-typedef struct trampData_s {
-    bool setByFreqFlag;  //false = set via band/channel
-    uint8_t band;
-    uint8_t channel;
-    uint16_t power;       // Actual transmitting power
-    uint16_t curFreq;
-    uint16_t configuredPower; // Configured transmitting power
-    int16_t temperature;
-    uint8_t pitMode;
-} trampData_t;
-
-extern trampData_t trampData;
-
 bool vtxTrampInit(void);
-bool trampCommitChanges(void);
-void trampSetPitMode(uint8_t onoff);
-void trampSetBandAndChannel(uint8_t band, uint8_t channel);
-void trampSetRFPower(uint16_t level);

From eff264f515cc1bd0ccc54e8d11c0745d729e6dd3 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Mon, 13 Apr 2020 16:39:28 +0200
Subject: [PATCH 080/179] WIP: 2D position controller

---
 src/main/navigation/navigation_fixedwing.c  |  3 ++
 src/main/navigation/navigation_rover_boat.c | 57 +++++++++++++++++++--
 2 files changed, 57 insertions(+), 3 deletions(-)

diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c
index fe2e34ba18..b9cc0deb38 100755
--- a/src/main/navigation/navigation_fixedwing.c
+++ b/src/main/navigation/navigation_fixedwing.c
@@ -203,6 +203,9 @@ bool adjustFixedWingHeadingFromRCInput(void)
 static fpVector3_t virtualDesiredPosition;
 static pt1Filter_t fwPosControllerCorrectionFilterState;
 
+/*
+ * TODO Currently this function resets both FixedWing and Rover & Boat position controller 
+ */
 void resetFixedWingPositionController(void)
 {
     virtualDesiredPosition.x = 0;
diff --git a/src/main/navigation/navigation_rover_boat.c b/src/main/navigation/navigation_rover_boat.c
index d2b473fb67..6f4101b867 100644
--- a/src/main/navigation/navigation_rover_boat.c
+++ b/src/main/navigation/navigation_rover_boat.c
@@ -35,6 +35,57 @@ FILE_COMPILE_FOR_SIZE
 #include "navigation/navigation.h"
 #include "navigation/navigation_private.h"
 
+static bool isYawAdjustmentValid = false;
+
+void applyRoverBoatPositionController(timeUs_t currentTimeUs)
+{
+    static timeUs_t previousTimePositionUpdate;         // Occurs @ GPS update rate
+    static timeUs_t previousTimeUpdate;                 // Occurs @ looptime rate
+
+    const timeDelta_t deltaMicros = currentTimeUs - previousTimeUpdate;
+    previousTimeUpdate = currentTimeUs;
+
+    // If last position update was too long in the past - ignore it (likely restarting altitude controller)
+    if (deltaMicros > HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) {
+        previousTimeUpdate = currentTimeUs;
+        previousTimePositionUpdate = currentTimeUs;
+        resetFixedWingPositionController();
+        return;
+    }
+
+    // Apply controller only if position source is valid. In absence of valid pos sensor (GPS loss), we'd stick in forced ANGLE mode
+    if ((posControl.flags.estPosStatus >= EST_USABLE)) {
+        // If we have new position - update velocity and acceleration controllers
+        if (posControl.flags.horizontalPositionDataNew) {
+            const timeDelta_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate;
+            previousTimePositionUpdate = currentTimeUs;
+
+            if (deltaMicrosPositionUpdate < HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) {
+                // Calculate virtual position target at a distance of forwardVelocity * HZ2S(POSITION_TARGET_UPDATE_RATE_HZ)
+                // Account for pilot's roll input (move position target left/right at max of max_manual_speed)
+                // POSITION_TARGET_UPDATE_RATE_HZ should be chosen keeping in mind that position target shouldn't be reached until next pos update occurs
+                // FIXME: verify the above
+                // calculateVirtualPositionTarget_FW(HZ2S(MIN_POSITION_UPDATE_RATE_HZ) * 2);
+
+                // updatePositionHeadingController_FW(currentTimeUs, deltaMicrosPositionUpdate);
+
+                //FIXME build a simple 2D position controller 
+            }
+            else {
+                resetFixedWingPositionController();
+            }
+
+            // Indicate that information is no longer usable
+            posControl.flags.horizontalPositionDataConsumed = 1;
+        }
+
+        isYawAdjustmentValid = true;
+    }
+    else {
+        isYawAdjustmentValid = false;
+    }
+}
+
 void applyRoverBoatPitchRollThrottleController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs)
 {
     UNUSED(currentTimeUs);
@@ -43,9 +94,9 @@ void applyRoverBoatPitchRollThrottleController(navigationFSMStateFlags_t navStat
 
     if (navStateFlags & NAV_CTL_POS) {
 
-        // if (isYawAdjustmentValid) {
-        rcCommand[YAW] = posControl.rcAdjustment[YAW];
-        // }
+        if (isYawAdjustmentValid) {
+            rcCommand[YAW] = posControl.rcAdjustment[YAW];
+        }
 
         // const uint16_t correctedThrottleValue = constrain(navConfig()->fw.cruise_throttle, navConfig()->fw.min_throttle, navConfig()->fw.max_throttle);
         rcCommand[THROTTLE] = constrain(navConfig()->fw.cruise_throttle, motorConfig()->mincommand, motorConfig()->maxthrottle);

From c58e28c7091940c749fcd22054fe8f5f6a220f3b Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Mon, 13 Apr 2020 19:21:05 +0200
Subject: [PATCH 081/179] Heading controller

---
 src/main/navigation/navigation_fixedwing.c  | 55 +++++++++++----------
 src/main/navigation/navigation_private.h    |  2 +-
 src/main/navigation/navigation_rover_boat.c | 45 ++++++++++++-----
 3 files changed, 62 insertions(+), 40 deletions(-)

diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c
index b9cc0deb38..9b01c2c679 100755
--- a/src/main/navigation/navigation_fixedwing.c
+++ b/src/main/navigation/navigation_fixedwing.c
@@ -289,6 +289,34 @@ bool adjustFixedWingPositionFromRCInput(void)
     return (rcRollAdjustment);
 }
 
+float processHeadingYawController(timeDelta_t deltaMicros, int32_t navHeadingError, bool errorIsDecreasing) {
+    static float limit = 0.0f;
+
+    if (limit == 0.0f) {
+        limit = pidProfile()->navFwPosHdgPidsumLimit * 100.0f;
+    }
+
+    const pidControllerFlags_e yawPidFlags = errorIsDecreasing ? PID_SHRINK_INTEGRATOR : 0;
+
+    const float yawAdjustment = navPidApply2(
+        &posControl.pids.fw_heading, 
+        0, 
+        applyDeadband(navHeadingError, navConfig()->fw.yawControlDeadband * 100), 
+        US2S(deltaMicros),
+        -limit,
+        limit,
+        yawPidFlags
+        ) / 100.0f;
+
+    DEBUG_SET(DEBUG_NAV_YAW, 0, posControl.pids.fw_heading.proportional);
+    DEBUG_SET(DEBUG_NAV_YAW, 1, posControl.pids.fw_heading.integral);
+    DEBUG_SET(DEBUG_NAV_YAW, 2, posControl.pids.fw_heading.derivative);
+    DEBUG_SET(DEBUG_NAV_YAW, 3, navHeadingError);
+    DEBUG_SET(DEBUG_NAV_YAW, 4, posControl.pids.fw_heading.output_constrained);
+
+    return yawAdjustment;
+}
+
 static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta_t deltaMicros)
 {
     static timeUs_t previousTimeMonitoringUpdate;
@@ -349,32 +377,7 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
      * It is working in relative mode and we aim to keep error at zero
      */
     if (STATE(FW_HEADING_USE_YAW)) {
-
-        static float limit = 0.0f;
-
-        if (limit == 0.0f) {
-            limit = pidProfile()->navFwPosHdgPidsumLimit * 100.0f;
-        }
-
-        const pidControllerFlags_e yawPidFlags = errorIsDecreasing ? PID_SHRINK_INTEGRATOR : 0;
-
-        float yawAdjustment = navPidApply2(
-            &posControl.pids.fw_heading, 
-            0, 
-            applyDeadband(navHeadingError, navConfig()->fw.yawControlDeadband * 100), 
-            US2S(deltaMicros),
-            -limit,
-            limit,
-            yawPidFlags
-            ) / 100.0f;
-
-        DEBUG_SET(DEBUG_NAV_YAW, 0, posControl.pids.fw_heading.proportional);
-        DEBUG_SET(DEBUG_NAV_YAW, 1, posControl.pids.fw_heading.integral);
-        DEBUG_SET(DEBUG_NAV_YAW, 2, posControl.pids.fw_heading.derivative);
-        DEBUG_SET(DEBUG_NAV_YAW, 3, navHeadingError);
-        DEBUG_SET(DEBUG_NAV_YAW, 4, posControl.pids.fw_heading.output_constrained);
-
-        posControl.rcAdjustment[YAW] = yawAdjustment;
+        posControl.rcAdjustment[YAW] = processHeadingYawController(deltaMicros, navHeadingError, errorIsDecreasing);
     } else {
         posControl.rcAdjustment[YAW] = 0;
     }
diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h
index 48beccf01f..41a5b4e95a 100755
--- a/src/main/navigation/navigation_private.h
+++ b/src/main/navigation/navigation_private.h
@@ -445,7 +445,7 @@ bool adjustFixedWingHeadingFromRCInput(void);
 bool adjustFixedWingPositionFromRCInput(void);
 
 void applyFixedWingPositionController(timeUs_t currentTimeUs);
-
+float processHeadingYawController(timeDelta_t deltaMicros, int32_t navHeadingError, bool errorIsDecreasing);
 void applyFixedWingNavigationController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs);
 
 void calculateFixedWingInitialHoldPosition(fpVector3_t * pos);
diff --git a/src/main/navigation/navigation_rover_boat.c b/src/main/navigation/navigation_rover_boat.c
index 6f4101b867..ea61afaa91 100644
--- a/src/main/navigation/navigation_rover_boat.c
+++ b/src/main/navigation/navigation_rover_boat.c
@@ -28,6 +28,7 @@ FILE_COMPILE_FOR_SIZE
 
 #ifdef USE_NAV
 
+#include "build/debug.h"
 #include "common/utils.h"
 #include "fc/rc_controls.h"
 #include "flight/mixer.h"
@@ -36,6 +37,34 @@ FILE_COMPILE_FOR_SIZE
 #include "navigation/navigation_private.h"
 
 static bool isYawAdjustmentValid = false;
+static int32_t navHeadingError;
+
+static void update2DPositionHeadingController(timeUs_t currentTimeUs, timeDelta_t deltaMicros)
+{
+    static timeUs_t previousTimeMonitoringUpdate;
+    static int32_t previousHeadingError;
+    static bool errorIsDecreasing;
+
+    int32_t targetBearing = calculateBearingToDestination(&posControl.desiredState.pos);
+
+    /*
+     * Calculate NAV heading error
+     * Units are centidegrees
+     */
+    navHeadingError = wrap_18000(targetBearing - posControl.actualState.yaw);
+
+    // Slow error monitoring (2Hz rate)
+    if ((currentTimeUs - previousTimeMonitoringUpdate) >= HZ2US(NAV_FW_CONTROL_MONITORING_RATE)) {
+        // Check if error is decreasing over time
+        errorIsDecreasing = (ABS(previousHeadingError) > ABS(navHeadingError));
+
+        // Save values for next iteration
+        previousHeadingError = navHeadingError;
+        previousTimeMonitoringUpdate = currentTimeUs;
+    }
+
+    posControl.rcAdjustment[YAW] = processHeadingYawController(deltaMicros, navHeadingError, errorIsDecreasing);
+}
 
 void applyRoverBoatPositionController(timeUs_t currentTimeUs)
 {
@@ -61,17 +90,8 @@ void applyRoverBoatPositionController(timeUs_t currentTimeUs)
             previousTimePositionUpdate = currentTimeUs;
 
             if (deltaMicrosPositionUpdate < HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) {
-                // Calculate virtual position target at a distance of forwardVelocity * HZ2S(POSITION_TARGET_UPDATE_RATE_HZ)
-                // Account for pilot's roll input (move position target left/right at max of max_manual_speed)
-                // POSITION_TARGET_UPDATE_RATE_HZ should be chosen keeping in mind that position target shouldn't be reached until next pos update occurs
-                // FIXME: verify the above
-                // calculateVirtualPositionTarget_FW(HZ2S(MIN_POSITION_UPDATE_RATE_HZ) * 2);
-
-                // updatePositionHeadingController_FW(currentTimeUs, deltaMicrosPositionUpdate);
-
-                //FIXME build a simple 2D position controller 
-            }
-            else {
+                update2DPositionHeadingController(currentTimeUs, deltaMicrosPositionUpdate);
+            } else {
                 resetFixedWingPositionController();
             }
 
@@ -98,7 +118,6 @@ void applyRoverBoatPitchRollThrottleController(navigationFSMStateFlags_t navStat
             rcCommand[YAW] = posControl.rcAdjustment[YAW];
         }
 
-        // const uint16_t correctedThrottleValue = constrain(navConfig()->fw.cruise_throttle, navConfig()->fw.min_throttle, navConfig()->fw.max_throttle);
         rcCommand[THROTTLE] = constrain(navConfig()->fw.cruise_throttle, motorConfig()->mincommand, motorConfig()->maxthrottle);
     }
 }
@@ -111,7 +130,7 @@ void applyRoverBoatNavigationController(navigationFSMStateFlags_t navStateFlags,
         rcCommand[YAW] = 0;
         rcCommand[THROTTLE] = failsafeConfig()->failsafe_throttle;
     } else if (navStateFlags & NAV_CTL_POS) {
-        applyFixedWingPositionController(currentTimeUs);
+        applyRoverBoatPositionController(currentTimeUs);
         applyRoverBoatPitchRollThrottleController(navStateFlags, currentTimeUs);
     }
 }

From 1094a7b14f282ec6202534d2c643a210c141cbfb Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Tue, 14 Apr 2020 19:25:41 +0200
Subject: [PATCH 082/179] Updated docs

---
 docs/Logic Conditions.md | 1 +
 src/main/fc/cli.c        | 2 +-
 2 files changed, 2 insertions(+), 1 deletion(-)

diff --git a/docs/Logic Conditions.md b/docs/Logic Conditions.md
index ac8ae4d8fd..c04fa06353 100644
--- a/docs/Logic Conditions.md	
+++ b/docs/Logic Conditions.md	
@@ -13,6 +13,7 @@ Logic conditions can be edited using INAV Configurator user interface, of via CL
 
 * `<rule>` - ID of Logic Condition rule
 * `<enabled>` - `0` evaluates as disabled, `1` evaluates as enabled
+* `<activatorId>` - the ID of _LogicCondition_ used to activate this _Condition_. _Logic Condition_ will be evaluated only then Activator evaluates as `true`. `-1` evaluates as `true`
 * `<operation>` - See `Operations` paragraph
 * `<operand A type>` - See `Operands` paragraph
 * `<operand A value>` - See `Operands` paragraph
diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c
index 427df2bbb1..a08b000a97 100755
--- a/src/main/fc/cli.c
+++ b/src/main/fc/cli.c
@@ -3553,7 +3553,7 @@ const clicmd_t cmdTable[] = {
     CLI_COMMAND_DEF("servo", "configure servos", NULL, cliServo),
 #ifdef USE_LOGIC_CONDITIONS
     CLI_COMMAND_DEF("logic", "configure logic conditions",
-        "<rule> <enabled> <operation> <operand A type> <operand A value> <operand B type> <operand B value> <flags>\r\n"
+        "<rule> <enabled> <activatorId> <operation> <operand A type> <operand A value> <operand B type> <operand B value> <flags>\r\n"
         "\treset\r\n", cliLogic),
 
     CLI_COMMAND_DEF("gvar", "configure global variables",

From 2aa704a46b1fc56622390680d18176b80fc5b62d Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Alberto=20Garci=CC=81a=20Hierro?= <alberto@garciahierro.com>
Date: Fri, 17 Apr 2020 12:10:22 -0400
Subject: [PATCH 083/179] [OSD] Fix off-by-one in osdGridBufferConstrainRect()

Caused writes outside of the local grid buffer when the caller
tried to clear a rect as tall as the screen. Spotted and narrowed
down by @giacomo.
---
 src/main/drivers/osd.c | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/drivers/osd.c b/src/main/drivers/osd.c
index 418758f61e..231d8eec28 100644
--- a/src/main/drivers/osd.c
+++ b/src/main/drivers/osd.c
@@ -56,12 +56,12 @@ static void osdGridBufferConstrainRect(int *x, int *y, int *w, int *h, int total
         *y = 0;
     }
     int maxX = *x + *w;
-    int extraWidth = maxX - totalWidth;
+    int extraWidth = maxX - totalWidth + 1;
     if (extraWidth > 0) {
         *w -= extraWidth;
     }
     int maxY = *y + *h;
-    int extraHeight = maxY - totalHeight;
+    int extraHeight = maxY - totalHeight + 1;
     if (extraHeight > 0) {
         *h -= extraHeight;
     }

From ae035ab557cb0cc46778b6926d32b342a812d2b6 Mon Sep 17 00:00:00 2001
From: Michel Pastor <shellixyz@gmail.com>
Date: Sun, 19 Apr 2020 18:46:53 +0200
Subject: [PATCH 084/179] Fix building without I2C

---
 src/main/fc/fc_init.c | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c
index 4228405a32..c9e26afd29 100644
--- a/src/main/fc/fc_init.c
+++ b/src/main/fc/fc_init.c
@@ -211,7 +211,9 @@ void init(void)
     // Re-initialize system clock to their final values (if necessary)
     systemClockSetup(systemConfig()->cpuUnderclock);
 
+#ifdef USE_I2C
     i2cSetSpeed(systemConfig()->i2c_speed);
+#endif
 
 #ifdef USE_HARDWARE_PREBOOT_SETUP
     initialisePreBootHardware();

From 888fb50d75548e338ba233b4e2d908e1362c8f07 Mon Sep 17 00:00:00 2001
From: seriousrob <schmidt.r@posteo.de>
Date: Mon, 20 Apr 2020 18:41:07 +0200
Subject: [PATCH 085/179] Changed timers for DShot support

---
 src/main/target/SPEEDYBEEF4/target.c | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/target/SPEEDYBEEF4/target.c b/src/main/target/SPEEDYBEEF4/target.c
index 45a98da28b..8fa896dae5 100644
--- a/src/main/target/SPEEDYBEEF4/target.c
+++ b/src/main/target/SPEEDYBEEF4/target.c
@@ -26,8 +26,8 @@
 const timerHardware_t timerHardware[] = {
     DEF_TIM(TIM5, CH4, PA3,  TIM_USE_PPM, 0, 0), // PPM
 
-    DEF_TIM(TIM8, CH1, PC6,  TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1  UP(1,2)
-    DEF_TIM(TIM8, CH2, PC7,  TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1), // S2  UP(2,1)
+    DEF_TIM(TIM3, CH1, PC6,  TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1  UP(1,2)
+    DEF_TIM(TIM3, CH2, PC7,  TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2  UP(2,1)
     DEF_TIM(TIM3, CH3, PC8,  TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3  UP(2,1)
     DEF_TIM(TIM3, CH4, PC9,  TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4  UP(2,1)
 

From 5df3a3e3da2070e77a9cca806f82b5ee4e3c9523 Mon Sep 17 00:00:00 2001
From: seriousrob <schmidt.r@posteo.de>
Date: Mon, 20 Apr 2020 18:43:34 +0200
Subject: [PATCH 086/179] Added DShot, ESC sensor, SoftSerial support

SoftSerial available via pre-definded targets.

SoftSerial1 on S5(RX) / S7(TX)
SoftSerial2 on S6(RX) / DAC(TX)
---
 src/main/target/SPEEDYBEEF4/target.h | 175 +++++++++++++++------------
 1 file changed, 95 insertions(+), 80 deletions(-)

diff --git a/src/main/target/SPEEDYBEEF4/target.h b/src/main/target/SPEEDYBEEF4/target.h
index 0e7f8e2c53..982eb704c2 100644
--- a/src/main/target/SPEEDYBEEF4/target.h
+++ b/src/main/target/SPEEDYBEEF4/target.h
@@ -17,60 +17,73 @@
 
 #pragma once
 
-#define TARGET_BOARD_IDENTIFIER "SBF4"
-#define USBD_PRODUCT_STRING  "SpeedyBeeF4"
+#define TARGET_BOARD_IDENTIFIER "SBF4"		
+#define USBD_PRODUCT_STRING  "SpeedyBeeF4"	
 
-#define LED0                    PB9
+/*** Indicators ***/
+#define LED0                    PB9			
+#define BEEPER                  PC13		
+#define BEEPER_INVERTED						
 
-#define BEEPER                  PC13
-#define BEEPER_INVERTED
+/*** IMU sensors ***/
+#define USE_EXTI							
+#define USE_GYRO							
+#define USE_ACC								
 
-#define USE_I2C
-#define USE_I2C_DEVICE_1
-#define I2C1_SCL                PB6        // SCL pad
-#define I2C1_SDA                PB7        // SDA pad
-#define I2C_DEVICE              (I2CDEV_1)
+#define GYRO_INT_EXTI            PC4		
+#define USE_MPU_DATA_READY_SIGNAL			
 
-#define UG2864_I2C_BUS BUS_I2C1
+#define SENSORS_SET (SENSOR_ACC|SENSOR_MAG|SENSOR_BARO)
 
-// MPU6000 interrupts
-#define USE_EXTI
-#define GYRO_INT_EXTI            PC4
-#define USE_MPU_DATA_READY_SIGNAL
+/*** MPU6000 ***/
+#define USE_GYRO_MPU6000					
+#define USE_ACC_MPU6000						
 
-#define MPU6000_CS_PIN          PB11
-#define MPU6000_SPI_BUS         BUS_SPI1
-#define USE_GYRO
-#define USE_GYRO_MPU6000
+#define MPU6000_CS_PIN          PB11		
+#define MPU6000_SPI_BUS         BUS_SPI1	
 #define GYRO_MPU6000_ALIGN      CW0_DEG
-#define USE_ACC
-#define USE_ACC_MPU6000
 #define ACC_MPU6000_ALIGN       CW0_DEG
 
-#define USE_MAG
-#define MAG_I2C_BUS             BUS_I2C1
-#define MAG_HMC5883_ALIGN       CW90_DEG
-#define USE_MAG_HMC5883
-#define USE_MAG_QMC5883
-#define USE_MAG_IST8310
-#define USE_MAG_MAG3110
-#define USE_MAG_LIS3MDL
+/*** SPI/I2C bus ***/
+#define USE_SPI								
+#define USE_SPI_DEVICE_1					
+#define SPI1_SCK_PIN            PA5			
+#define SPI1_MISO_PIN           PA6			
+#define SPI1_MOSI_PIN           PA7			
 
-#define TEMPERATURE_I2C_BUS     BUS_I2C1
+#define USE_SPI_DEVICE_2					
+#define SPI2_NSS_PIN            PB12		
+#define SPI2_SCK_PIN            PB13		
+#define SPI2_MISO_PIN           PB14		
+#define SPI2_MOSI_PIN           PB15		
 
-#define USE_BARO
+#define USE_SPI_DEVICE_3					
+#define SPI3_SCK_PIN            PB3			
+#define SPI3_MISO_PIN           PB4			
+#define SPI3_MOSI_PIN           PB5			
 
-#define BARO_I2C_BUS          BUS_I2C1
-#define USE_BARO_BMP085
-#define USE_BARO_BMP280
-#define USE_BARO_MS5611
+#define I2C_DEVICE              (I2CDEV_1)	
+#define UG2864_I2C_BUS BUS_I2C1				
 
-#define PITOT_I2C_BUS           BUS_I2C1
+#define USE_I2C								
+#define USE_I2C_DEVICE_1					
+#define I2C1_SCL                PB6        	
+#define I2C1_SDA                PB7        	
 
-#define USE_RANGEFINDER
-#define RANGEFINDER_I2C_BUS     BUS_I2C1
-#define USE_RANGEFINDER_HCSR04_I2C
+/*** Onboard flash ***/
+#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
+#define M25P16_CS_PIN           PC0				
+#define M25P16_SPI_BUS          BUS_SPI3		
+#define USE_FLASHFS								
+#define USE_FLASH_M25P16	
 
+/*** OSD ***/
+#define USE_OSD								
+#define USE_MAX7456							
+#define MAX7456_SPI_BUS         BUS_SPI2	
+#define MAX7456_CS_PIN          PB10		
+
+/*** Serial ports ***/
 #define USE_VCP
 #define VBUS_SENSING_PIN        PB12
 #define VBUS_SENSING_ENABLED
@@ -95,40 +108,44 @@
 #define UART5_RX_PIN            PD2
 #define UART5_TX_PIN            PC12
 
+#if defined(SPEEDYBEEF4_SFTSRL1)
+#define USE_SOFTSERIAL1
+#define SOFTSERIAL_1_RX_PIN     PA15 // S5
+#define SOFTSERIAL_1_TX_PIN     PB8  // S7
+
+#define SERIAL_PORT_COUNT       7
+
+#elif defined(SPEEDYBEEF4_SFTSRL2)
+#define USE_SOFTSERIAL1
+#define SOFTSERIAL_1_RX_PIN     PA15 // S5
+#define SOFTSERIAL_1_TX_PIN     PB8  // S7
+#define USE_SOFTSERIAL2
+#define SOFTSERIAL_2_RX_PIN     PA8 // S6   
+#define SOFTSERIAL_2_TX_PIN     PA4 // DAC
+
+#define SERIAL_PORT_COUNT       8
+
+#else
 #define SERIAL_PORT_COUNT       6
+#endif
 
-#define USE_SPI
+/*** BARO & MAG ***/
+#define USE_BARO							
+#define BARO_I2C_BUS          	BUS_I2C1	
+#define USE_BARO_BMP085						
+#define USE_BARO_BMP280						
+#define USE_BARO_MS5611						
 
-#define USE_SPI_DEVICE_1
-#define SPI1_SCK_PIN            PA5
-#define SPI1_MISO_PIN           PA6
-#define SPI1_MOSI_PIN           PA7
-
-
-#define USE_SPI_DEVICE_2
-#define SPI2_NSS_PIN            PB12
-#define SPI2_SCK_PIN            PB13
-#define SPI2_MISO_PIN           PB14
-#define SPI2_MOSI_PIN           PB15
-
-#define USE_SPI_DEVICE_3
-#define SPI3_NSS_PIN            PC0
-#define SPI3_SCK_PIN            PB3
-#define SPI3_MISO_PIN           PB4
-#define SPI3_MOSI_PIN           PB5
-
-
-#define USE_OSD
-#define USE_MAX7456
-#define MAX7456_SPI_BUS         BUS_SPI2
-#define MAX7456_CS_PIN          PB10
-
-#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
-#define M25P16_CS_PIN           SPI3_NSS_PIN
-#define M25P16_SPI_BUS          BUS_SPI3
-#define USE_FLASHFS
-#define USE_FLASH_M25P16
+#define USE_MAG								
+#define MAG_I2C_BUS             BUS_I2C1	
+#define USE_MAG_HMC5883						
+#define USE_MAG_QMC5883						
+#define USE_MAG_IST8310						
+#define USE_MAG_MAG3110						
+#define USE_MAG_LIS3MDL						
+#define MAG_HMC5883_ALIGN       CW90_DEG	
 
+/*** ADC ***/
 #define USE_ADC
 #define ADC_CHANNEL_1_PIN               PC1
 #define ADC_CHANNEL_2_PIN               PC2
@@ -138,24 +155,22 @@
 #define VBAT_ADC_CHANNEL                ADC_CHN_2
 #define RSSI_ADC_CHANNEL                ADC_CHN_3
 
-#define SENSORS_SET (SENSOR_ACC|SENSOR_MAG|SENSOR_BARO)
+/*** LED STRIP ***/
 
-#define DEFAULT_RX_TYPE         RX_TYPE_PPM
-#define DISABLE_RX_PWM_FEATURE
+/*** Default settings ***/
 #define DEFAULT_FEATURES        (FEATURE_TX_PROF_SEL | FEATURE_CURRENT_METER | FEATURE_BLACKBOX | FEATURE_VBAT | FEATURE_OSD)
+#define DEFAULT_RX_TYPE         RX_TYPE_SERIAL
+#define SERIALRX_PROVIDER       SERIALRX_SBUS
+#define CURRENT_METER_SCALE     166
 
-#define USE_SPEKTRUM_BIND
-#define BIND_PIN                PC11 // USART3 RX
-
+/*** Timer/PWM output ***/
 #define USE_SERIAL_4WAY_BLHELI_INTERFACE
-
-// Number of available PWM outputs
 #define MAX_PWM_OUTPUT_PORTS    7
-#define TARGET_MOTOR_COUNT      7
+#define USE_DSHOT
+#define USE_ESC_SENSOR
 
+/*** Used pins ***/
 #define TARGET_IO_PORTA         0xffff
 #define TARGET_IO_PORTB         0xffff
 #define TARGET_IO_PORTC         0xffff
 #define TARGET_IO_PORTD         (BIT(2))
-
-#define CURRENT_METER_SCALE   302

From 0acdb8dd24c3d7c11bf6f08b17de9868b9a20c32 Mon Sep 17 00:00:00 2001
From: seriousrob <schmidt.r@posteo.de>
Date: Mon, 20 Apr 2020 18:44:35 +0200
Subject: [PATCH 087/179] Target for using 1 SoftSerial

---
 src/main/target/SPEEDYBEEF4/SPEEDYBEEF4_SFTSRL1.mk | 1 +
 1 file changed, 1 insertion(+)
 create mode 100644 src/main/target/SPEEDYBEEF4/SPEEDYBEEF4_SFTSRL1.mk

diff --git a/src/main/target/SPEEDYBEEF4/SPEEDYBEEF4_SFTSRL1.mk b/src/main/target/SPEEDYBEEF4/SPEEDYBEEF4_SFTSRL1.mk
new file mode 100644
index 0000000000..123317b4ed
--- /dev/null
+++ b/src/main/target/SPEEDYBEEF4/SPEEDYBEEF4_SFTSRL1.mk
@@ -0,0 +1 @@
+#SPEEDYBEEF4_SFTSRL1
\ No newline at end of file

From c23a463fb8393abf87dbedef71308109a0015792 Mon Sep 17 00:00:00 2001
From: seriousrob <schmidt.r@posteo.de>
Date: Mon, 20 Apr 2020 18:45:17 +0200
Subject: [PATCH 088/179] Target for using 2 SoftSerials

---
 src/main/target/SPEEDYBEEF4/SPEEDYBEEF4_SFTSRL2.mk | 1 +
 1 file changed, 1 insertion(+)
 create mode 100644 src/main/target/SPEEDYBEEF4/SPEEDYBEEF4_SFTSRL2.mk

diff --git a/src/main/target/SPEEDYBEEF4/SPEEDYBEEF4_SFTSRL2.mk b/src/main/target/SPEEDYBEEF4/SPEEDYBEEF4_SFTSRL2.mk
new file mode 100644
index 0000000000..6cc372b363
--- /dev/null
+++ b/src/main/target/SPEEDYBEEF4/SPEEDYBEEF4_SFTSRL2.mk
@@ -0,0 +1 @@
+#SPEEDYBEEF4_SFTSRL2
\ No newline at end of file

From 8a3d38a44f05f8448888a0e88d2aa53222dc47d1 Mon Sep 17 00:00:00 2001
From: "Konstantin (DigitalEntity) Sharlaimov"
 <konstantin.sharlaimov@gmail.com>
Date: Tue, 21 Apr 2020 12:29:08 +0200
Subject: [PATCH 089/179] [DJI] Don't show DJI OSD items if they don't have a
 dependency featyure enabled

---
 src/main/config/feature.c |   5 +-
 src/main/io/osd_dji_hd.c  | 129 ++++++++++++++++++++------------------
 2 files changed, 72 insertions(+), 62 deletions(-)

diff --git a/src/main/config/feature.c b/src/main/config/feature.c
index ae65df4b77..042509629d 100755
--- a/src/main/config/feature.c
+++ b/src/main/config/feature.c
@@ -31,12 +31,13 @@ void latchActiveFeatures(void)
 
 bool featureConfigured(uint32_t mask)
 {
-    return featureConfig()->enabledFeatures & mask;
+    return (featureConfig()->enabledFeatures & mask) == mask;
 }
 
 bool feature(uint32_t mask)
 {
-    return activeFeaturesLatch & mask;
+    // Check for ALL masked features
+    return (activeFeaturesLatch & mask) == mask;
 }
 
 void featureSet(uint32_t mask)
diff --git a/src/main/io/osd_dji_hd.c b/src/main/io/osd_dji_hd.c
index 638768f726..f8f2dd8b0a 100644
--- a/src/main/io/osd_dji_hd.c
+++ b/src/main/io/osd_dji_hd.c
@@ -104,64 +104,69 @@ const uint8_t djiPidIndexMap[] = {
     PID_HEADING // DJI: PID_MAG
 };
 
-const int djiOSDItemIndexMap[] = {
-    OSD_RSSI_VALUE,             // DJI: OSD_RSSI_VALUE
-    OSD_MAIN_BATT_VOLTAGE,      // DJI: OSD_MAIN_BATT_VOLTAGE
-    OSD_CROSSHAIRS,             // DJI: OSD_CROSSHAIRS
-    OSD_ARTIFICIAL_HORIZON,     // DJI: OSD_ARTIFICIAL_HORIZON
-    OSD_HORIZON_SIDEBARS,       // DJI: OSD_HORIZON_SIDEBARS
-    OSD_ONTIME,                 // DJI: OSD_ITEM_TIMER_1
-    OSD_FLYTIME,                // DJI: OSD_ITEM_TIMER_2
-    OSD_FLYMODE,                // DJI: OSD_FLYMODE
-    OSD_CRAFT_NAME,             // DJI: OSD_CRAFT_NAME
-    OSD_THROTTLE_POS,           // DJI: OSD_THROTTLE_POS
-    OSD_VTX_CHANNEL,            // DJI: OSD_VTX_CHANNEL
-    OSD_CURRENT_DRAW,           // DJI: OSD_CURRENT_DRAW
-    OSD_MAH_DRAWN,              // DJI: OSD_MAH_DRAWN
-    OSD_GPS_SPEED,              // DJI: OSD_GPS_SPEED
-    OSD_GPS_SATS,               // DJI: OSD_GPS_SATS
-    OSD_ALTITUDE,               // DJI: OSD_ALTITUDE
-    OSD_ROLL_PIDS,              // DJI: OSD_ROLL_PIDS
-    OSD_PITCH_PIDS,             // DJI: OSD_PITCH_PIDS
-    OSD_YAW_PIDS,               // DJI: OSD_YAW_PIDS
-    OSD_POWER,                  // DJI: OSD_POWER
-    -1,                         // DJI: OSD_PIDRATE_PROFILE
-    -1,                         // DJI: OSD_WARNINGS
-    OSD_MAIN_BATT_CELL_VOLTAGE, // DJI: OSD_AVG_CELL_VOLTAGE
-    OSD_GPS_LON,                // DJI: OSD_GPS_LON
-    OSD_GPS_LAT,                // DJI: OSD_GPS_LAT
-    OSD_DEBUG,                  // DJI: OSD_DEBUG
-    OSD_ATTITUDE_PITCH,         // DJI: OSD_PITCH_ANGLE
-    OSD_ATTITUDE_ROLL,          // DJI: OSD_ROLL_ANGLE
-    -1,                         // DJI: OSD_MAIN_BATT_USAGE
-    -1,                         // DJI: OSD_DISARMED
-    OSD_HOME_DIR,               // DJI: OSD_HOME_DIR
-    OSD_HOME_DIST,              // DJI: OSD_HOME_DIST
-    OSD_HEADING,                // DJI: OSD_NUMERICAL_HEADING
-    OSD_VARIO_NUM,              // DJI: OSD_NUMERICAL_VARIO
-    -1,                         // DJI: OSD_COMPASS_BAR
-    -1,                         // DJI: OSD_ESC_TMP
-    OSD_ESC_RPM,                // DJI: OSD_ESC_RPM
-    OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH,   // DJI: OSD_REMAINING_TIME_ESTIMATE
-    OSD_RTC_TIME,               // DJI: OSD_RTC_DATETIME
-    -1,                         // DJI: OSD_ADJUSTMENT_RANGE
-    -1,                         // DJI: OSD_CORE_TEMPERATURE
-    -1,                         // DJI: OSD_ANTI_GRAVITY
-    -1,                         // DJI: OSD_G_FORCE
-    -1,                         // DJI: OSD_MOTOR_DIAG
-    -1,                         // DJI: OSD_LOG_STATUS
-    -1,                         // DJI: OSD_FLIP_ARROW
-    -1,                         // DJI: OSD_LINK_QUALITY
-    OSD_TRIP_DIST,              // DJI: OSD_FLIGHT_DIST
-    -1,                         // DJI: OSD_STICK_OVERLAY_LEFT
-    -1,                         // DJI: OSD_STICK_OVERLAY_RIGHT
-    -1,                         // DJI: OSD_DISPLAY_NAME
-    -1,                         // DJI: OSD_ESC_RPM_FREQ
-    -1,                         // DJI: OSD_RATE_PROFILE_NAME
-    -1,                         // DJI: OSD_PID_PROFILE_NAME
-    -1,                         // DJI: OSD_PROFILE_NAME
-    -1,                         // DJI: OSD_RSSI_DBM_VALUE
-    -1,                         // DJI: OSD_RC_CHANNELS
+typedef struct {
+    int         itemIndex;      // INAV OSD item
+    features_e  depFeature;     // INAV feature that item is dependent on
+} djiOsdMapping_t;
+
+const djiOsdMapping_t djiOSDItemIndexMap[] = {
+    { OSD_RSSI_VALUE,                         0 }, // DJI: OSD_RSSI_VALUE
+    { OSD_MAIN_BATT_VOLTAGE,                  FEATURE_VBAT }, // DJI: OSD_MAIN_BATT_VOLTAGE
+    { OSD_CROSSHAIRS,                         0 }, // DJI: OSD_CROSSHAIRS
+    { OSD_ARTIFICIAL_HORIZON,                 0 }, // DJI: OSD_ARTIFICIAL_HORIZON
+    { OSD_HORIZON_SIDEBARS,                   0 }, // DJI: OSD_HORIZON_SIDEBARS
+    { OSD_ONTIME,                             0 }, // DJI: OSD_ITEM_TIMER_1
+    { OSD_FLYTIME,                            0 }, // DJI: OSD_ITEM_TIMER_2
+    { OSD_FLYMODE,                            0 }, // DJI: OSD_FLYMODE
+    { OSD_CRAFT_NAME,                         0 }, // DJI: OSD_CRAFT_NAME
+    { OSD_THROTTLE_POS,                       0 }, // DJI: OSD_THROTTLE_POS
+    { OSD_VTX_CHANNEL,                        0 }, // DJI: OSD_VTX_CHANNEL
+    { OSD_CURRENT_DRAW,                       FEATURE_CURRENT_METER }, // DJI: OSD_CURRENT_DRAW
+    { OSD_MAH_DRAWN,                          FEATURE_CURRENT_METER }, // DJI: OSD_MAH_DRAWN
+    { OSD_GPS_SPEED,                          FEATURE_GPS }, // DJI: OSD_GPS_SPEED
+    { OSD_GPS_SATS,                           FEATURE_GPS }, // DJI: OSD_GPS_SATS
+    { OSD_ALTITUDE,                           0 }, // DJI: OSD_ALTITUDE
+    { OSD_ROLL_PIDS,                          0 }, // DJI: OSD_ROLL_PIDS
+    { OSD_PITCH_PIDS,                         0 }, // DJI: OSD_PITCH_PIDS
+    { OSD_YAW_PIDS,                           0 }, // DJI: OSD_YAW_PIDS
+    { OSD_POWER,                              0 }, // DJI: OSD_POWER
+    { -1,                                     0 }, // DJI: OSD_PIDRATE_PROFILE
+    { -1,                                     0 }, // DJI: OSD_WARNINGS
+    { OSD_MAIN_BATT_CELL_VOLTAGE,             0 }, // DJI: OSD_AVG_CELL_VOLTAGE
+    { OSD_GPS_LON,                            FEATURE_GPS }, // DJI: OSD_GPS_LON
+    { OSD_GPS_LAT,                            FEATURE_GPS }, // DJI: OSD_GPS_LAT
+    { OSD_DEBUG,                              0 }, // DJI: OSD_DEBUG
+    { OSD_ATTITUDE_PITCH,                     0 }, // DJI: OSD_PITCH_ANGLE
+    { OSD_ATTITUDE_ROLL,                      0 }, // DJI: OSD_ROLL_ANGLE
+    { -1,                                     0 }, // DJI: OSD_MAIN_BATT_USAGE
+    { -1,                                     0 }, // DJI: OSD_DISARMED
+    { OSD_HOME_DIR,                           FEATURE_GPS }, // DJI: OSD_HOME_DIR
+    { OSD_HOME_DIST,                          FEATURE_GPS }, // DJI: OSD_HOME_DIST
+    { OSD_HEADING,                            0 }, // DJI: OSD_NUMERICAL_HEADING
+    { OSD_VARIO_NUM,                          0 }, // DJI: OSD_NUMERICAL_VARIO
+    { -1,                                     0 }, // DJI: OSD_COMPASS_BAR
+    { -1,                                     0 }, // DJI: OSD_ESC_TMP
+    { OSD_ESC_RPM,                            0 }, // DJI: OSD_ESC_RPM
+    { OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH,   FEATURE_CURRENT_METER }, // DJI: OSD_REMAINING_TIME_ESTIMATE
+    { OSD_RTC_TIME,                           0 }, // DJI: OSD_RTC_DATETIME
+    { -1,                                     0 }, // DJI: OSD_ADJUSTMENT_RANGE
+    { -1,                                     0 }, // DJI: OSD_CORE_TEMPERATURE
+    { -1,                                     0 }, // DJI: OSD_ANTI_GRAVITY
+    { -1,                                     0 }, // DJI: OSD_G_FORCE
+    { -1,                                     0 }, // DJI: OSD_MOTOR_DIAG
+    { -1,                                     0 }, // DJI: OSD_LOG_STATUS
+    { -1,                                     0 }, // DJI: OSD_FLIP_ARROW
+    { -1,                                     0 }, // DJI: OSD_LINK_QUALITY
+    { OSD_TRIP_DIST,                          FEATURE_GPS }, // DJI: OSD_FLIGHT_DIST
+    { -1,                                     0 }, // DJI: OSD_STICK_OVERLAY_LEFT
+    { -1,                                     0 }, // DJI: OSD_STICK_OVERLAY_RIGHT
+    { -1,                                     0 }, // DJI: OSD_DISPLAY_NAME
+    { -1,                                     0 }, // DJI: OSD_ESC_RPM_FREQ
+    { -1,                                     0 }, // DJI: OSD_RATE_PROFILE_NAME
+    { -1,                                     0 }, // DJI: OSD_PID_PROFILE_NAME
+    { -1,                                     0 }, // DJI: OSD_PROFILE_NAME
+    { -1,                                     0 }, // DJI: OSD_RSSI_DBM_VALUE
+    { -1,                                     0 }, // DJI: OSD_RC_CHANNELS
 };
 
 const int djiOSDStatisticsMap[] = {
@@ -289,8 +294,12 @@ static void djiSerializeOSDConfigReply(sbuf_t *dst)
 
     // OSD element position and visibility
     for (unsigned i = 0; i < ARRAYLEN(djiOSDItemIndexMap); i++) {
-        int inavOSDIdx = djiOSDItemIndexMap[i];
-        if (inavOSDIdx >= 0) {
+        const int inavOSDIdx = djiOSDItemIndexMap[i].itemIndex;
+
+        // We call OSD item supported if it doesn't have dependencies or all feature-dependencies are satistied
+        const bool itemIsSupported = ((djiOSDItemIndexMap[i].depFeature == 0) || feature(djiOSDItemIndexMap[i].depFeature));
+
+        if (inavOSDIdx >= 0 && itemIsSupported) {
             // Position & visibility encoded in 16 bits. Position encoding is the same between BF/DJI and INAV
             // However visibility is different. INAV has 3 layouts, while BF only has visibility profiles
             // Here we use only one OSD layout mapped to first OSD BF profile

From 3acb85feb81bde986853c3b5b419a576245182a5 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Wed, 22 Apr 2020 14:33:50 +0200
Subject: [PATCH 090/179] On rovers and boats stop motors when mission is done

---
 src/main/navigation/navigation.c            |  2 +-
 src/main/navigation/navigation_private.h    |  1 +
 src/main/navigation/navigation_rover_boat.c | 18 +++++++++++++-----
 3 files changed, 15 insertions(+), 6 deletions(-)

diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index 266115ff7c..a636c13d80 100755
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -743,7 +743,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
         .persistentId = NAV_PERSISTENT_ID_WAYPOINT_FINISHED,
         .onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_FINISHED,
         .timeoutMs = 0,
-        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
+        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP | NAV_AUTO_WP_DONE,
         .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
         .mwState = MW_NAV_STATE_WP_ENROUTE,
         .mwError = MW_NAV_ERROR_FINISH,
diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h
index 41a5b4e95a..7748472403 100755
--- a/src/main/navigation/navigation_private.h
+++ b/src/main/navigation/navigation_private.h
@@ -276,6 +276,7 @@ typedef enum {
 
     /* Additional flags */
     NAV_CTL_LAND            = (1 << 14),
+    NAV_AUTO_WP_DONE        = (1 << 15),    //Waypoint mission reached the last waypoint and is idling
 } navigationFSMStateFlags_t;
 
 typedef struct {
diff --git a/src/main/navigation/navigation_rover_boat.c b/src/main/navigation/navigation_rover_boat.c
index ea61afaa91..1f0a01669a 100644
--- a/src/main/navigation/navigation_rover_boat.c
+++ b/src/main/navigation/navigation_rover_boat.c
@@ -31,8 +31,8 @@ FILE_COMPILE_FOR_SIZE
 #include "build/debug.h"
 #include "common/utils.h"
 #include "fc/rc_controls.h"
+#include "fc/config.h"
 #include "flight/mixer.h"
-
 #include "navigation/navigation.h"
 #include "navigation/navigation_private.h"
 
@@ -114,11 +114,19 @@ void applyRoverBoatPitchRollThrottleController(navigationFSMStateFlags_t navStat
 
     if (navStateFlags & NAV_CTL_POS) {
 
-        if (isYawAdjustmentValid) {
-            rcCommand[YAW] = posControl.rcAdjustment[YAW];
-        }
+        if (navStateFlags & NAV_AUTO_WP_DONE) {
+            /*
+             * When WP mission is done, stop the motors
+             */
+            rcCommand[YAW] = 0;
+            rcCommand[THROTTLE] = feature(FEATURE_REVERSIBLE_MOTORS) ? reversibleMotorsConfig()->neutral : motorConfig()->mincommand;
+        } else {
+            if (isYawAdjustmentValid) {
+                rcCommand[YAW] = posControl.rcAdjustment[YAW];
+            }
 
-        rcCommand[THROTTLE] = constrain(navConfig()->fw.cruise_throttle, motorConfig()->mincommand, motorConfig()->maxthrottle);
+            rcCommand[THROTTLE] = constrain(navConfig()->fw.cruise_throttle, motorConfig()->mincommand, motorConfig()->maxthrottle);
+        }
     }
 }
 

From 4409b134ffa8e61092160d106664f056ce851960 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Thu, 23 Apr 2020 20:40:29 +0200
Subject: [PATCH 091/179] Enable DSHOT and ESC telemetry on Kakute F4

---
 src/main/target/KAKUTEF4/target.h | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/src/main/target/KAKUTEF4/target.h b/src/main/target/KAKUTEF4/target.h
index ed79614f08..a4ea4a00b7 100755
--- a/src/main/target/KAKUTEF4/target.h
+++ b/src/main/target/KAKUTEF4/target.h
@@ -169,7 +169,9 @@
 #define TARGET_IO_PORTC 0xffff
 #define TARGET_IO_PORTD        (BIT(2))
 
-
+#define USE_DSHOT
+#define USE_SERIALSHOT
+#define USE_ESC_SENSOR
 
 #ifdef KAKUTEF4V2
 #   define MAX_PWM_OUTPUT_PORTS       4

From 90a272ecf4c00fb1defb9541f67bda0139527a5f Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Fri, 24 Apr 2020 13:37:54 +0200
Subject: [PATCH 092/179] Disable Pitor and MC Braking on F3

---
 src/main/target/common.h | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/src/main/target/common.h b/src/main/target/common.h
index 5c8803245f..af81fa439f 100755
--- a/src/main/target/common.h
+++ b/src/main/target/common.h
@@ -58,8 +58,6 @@
 #define USE_TELEMETRY_LTM
 #define USE_TELEMETRY_FRSKY
 
-#define USE_MR_BRAKING_MODE
-
 #if defined(STM_FAST_TARGET)
 #define SCHEDULER_DELAY_LIMIT           10
 #else
@@ -67,6 +65,11 @@
 #endif
 
 #if (FLASH_SIZE > 256)
+#define USE_MR_BRAKING_MODE
+#define USE_PITOT
+#define USE_PITOT_ADC
+#define USE_PITOT_VIRTUAL
+
 #define USE_DYNAMIC_FILTERS
 #define USE_EXTENDED_CMS_MENUS
 #define USE_UAV_INTERCONNECT
@@ -149,9 +152,6 @@
 #define USE_SERIAL_PASSTHROUGH
 #define NAV_MAX_WAYPOINTS       60
 #define USE_RCDEVICE
-#define USE_PITOT
-#define USE_PITOT_ADC
-#define USE_PITOT_VIRTUAL
 
 //Enable VTX control
 #define USE_VTX_CONTROL

From faeca2087447a8d605522ba0f6395fccddf6f4c5 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Fri, 24 Apr 2020 13:49:42 +0200
Subject: [PATCH 093/179] Fix compilation error

---
 src/main/navigation/navigation_multicopter.c | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c
index dff676931d..90d425dfaa 100755
--- a/src/main/navigation/navigation_multicopter.c
+++ b/src/main/navigation/navigation_multicopter.c
@@ -31,6 +31,7 @@
 #include "common/axis.h"
 #include "common/maths.h"
 #include "common/filter.h"
+#include "common/utils.h"
 
 #include "sensors/sensors.h"
 #include "sensors/acceleration.h"

From 90c1544a6f6d5721e7e9ef6f364fc244e4dde7ae Mon Sep 17 00:00:00 2001
From: giacomo892 <giacomo1989@gmail.com>
Date: Thu, 16 Apr 2020 21:27:06 +0200
Subject: [PATCH 094/179]  ESC Temperature to OSD

---
 docs/Cli.md                    |  2 ++
 src/main/cms/cms_menu_osd.c    |  1 +
 src/main/drivers/osd_symbols.h |  1 +
 src/main/fc/settings.yaml      |  8 ++++++++
 src/main/io/osd.c              | 12 +++++++++++-
 src/main/io/osd.h              |  3 +++
 6 files changed, 26 insertions(+), 1 deletion(-)

diff --git a/docs/Cli.md b/docs/Cli.md
index 60a3b59d6a..8081f18968 100644
--- a/docs/Cli.md
+++ b/docs/Cli.md
@@ -339,6 +339,8 @@ A shorter form is also supported to enable and disable functions using `serial <
 |  osd_gforce_axis_alarm_max | 5  | Value above which the OSD axis g force indicators will blink (g) |
 |  osd_imu_temp_alarm_min | -200 | Temperature under which the IMU temperature OSD element will start blinking (decidegrees centigrade) |
 |  osd_imu_temp_alarm_max | 600 | Temperature above which the IMU temperature OSD element will start blinking (decidegrees centigrade) |
+|  osd_esc_temp_alarm_min | -200 | Temperature under which the IMU temperature OSD element will start blinking (decidegrees centigrade) |
+|  osd_esc_temp_alarm_max | 900 | Temperature above which the IMU temperature OSD element will start blinking (decidegrees centigrade) |
 |  osd_baro_temp_alarm_min | -200 | Temperature under which the baro temperature OSD element will start blinking (decidegrees centigrade) |
 |  osd_baro_temp_alarm_max | 600 | Temperature above which the baro temperature OSD element will start blinking (decidegrees centigrade) |
 |  osd_current_alarm | 0 | Value above which the OSD current consumption element will start blinking. Measured in full Amperes. |
diff --git a/src/main/cms/cms_menu_osd.c b/src/main/cms/cms_menu_osd.c
index cf2eabe5dd..901bb0e171 100755
--- a/src/main/cms/cms_menu_osd.c
+++ b/src/main/cms/cms_menu_osd.c
@@ -273,6 +273,7 @@ static const OSD_Entry menuOsdElemsEntries[] =
 
 #ifdef USE_ESC_SENSOR
     OSD_ELEMENT_ENTRY("ESC RPM", OSD_ESC_RPM),
+    OSD_ELEMENT_ENTRY("ESC TEMPERATURE", OSD_ESC_TEMPERATURE),
 #endif
 
     OSD_BACK_AND_END_ENTRY,
diff --git a/src/main/drivers/osd_symbols.h b/src/main/drivers/osd_symbols.h
index c289bfbf44..0b08e7b51d 100644
--- a/src/main/drivers/osd_symbols.h
+++ b/src/main/drivers/osd_symbols.h
@@ -205,6 +205,7 @@
 #define SYM_BARO_TEMP             0xF0 // 240
 #define SYM_IMU_TEMP              0xF1 // 241
 #define SYM_TEMP                  0xF2 // 242
+#define SYM_ESC_TEMP              0xF3 // 243
 
 #define SYM_TEMP_SENSOR_FIRST     0xF2 // 242
 #define SYM_TEMP_SENSOR_LAST      0xF7 // 247
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 88ce41d6b8..a611be75f2 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -1892,6 +1892,14 @@ groups:
         field: imu_temp_alarm_max
         min: -550
         max: 1250
+      - name: osd_esc_temp_alarm_max
+        field: esc_temp_alarm_max
+        min: -550
+        max: 1500
+      - name: osd_esc_temp_alarm_min
+        field: esc_temp_alarm_min
+        min: -550
+        max: 1500
       - name: osd_baro_temp_alarm_min
         field: baro_temp_alarm_min
         condition: USE_BARO
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index d83ee053f2..f7a0f2d943 100755
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -198,7 +198,7 @@ static bool osdDisplayHasCanvas;
 #define AH_SIDEBAR_WIDTH_POS 7
 #define AH_SIDEBAR_HEIGHT_POS 3
 
-PG_REGISTER_WITH_RESET_FN(osdConfig_t, osdConfig, PG_OSD_CONFIG, 11);
+PG_REGISTER_WITH_RESET_FN(osdConfig_t, osdConfig, PG_OSD_CONFIG, 12);
 
 static int digitCount(int32_t value)
 {
@@ -2480,6 +2480,13 @@ static bool osdDrawSingleElement(uint8_t item)
             }
             break;
         }
+    case OSD_ESC_TEMPERATURE:
+        {
+            escSensorData_t * escSensor = escSensorGetData();
+            bool escTemperatureValid = escSensor && escSensor->dataAge <= ESC_DATA_MAX_AGE;
+            osdDisplayTemperature(elemPosX, elemPosY, SYM_ESC_TEMP, NULL, escTemperatureValid, (escSensor->temperature)*10, osdConfig()->esc_temp_alarm_min, osdConfig()->esc_temp_alarm_max);
+            return true;
+        }
 #endif
 
     default:
@@ -2695,6 +2702,7 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig)
 
 #if defined(USE_ESC_SENSOR)
     osdConfig->item_pos[0][OSD_ESC_RPM] = OSD_POS(1, 2);
+    osdConfig->item_pos[0][OSD_ESC_TEMPERATURE] = OSD_POS(1, 3);
 #endif
 
 #if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE)
@@ -2718,6 +2726,8 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig)
     osdConfig->current_alarm = 0;
     osdConfig->imu_temp_alarm_min = -200;
     osdConfig->imu_temp_alarm_max = 600;
+    osdConfig->esc_temp_alarm_min = -200;
+    osdConfig->esc_temp_alarm_max = 900;
     osdConfig->gforce_alarm = 5;
     osdConfig->gforce_axis_alarm_min = -5;
     osdConfig->gforce_axis_alarm_max = 5;
diff --git a/src/main/io/osd.h b/src/main/io/osd.h
index 7b26c452ec..00dc2fd6d7 100755
--- a/src/main/io/osd.h
+++ b/src/main/io/osd.h
@@ -151,6 +151,7 @@ typedef enum {
     OSD_RC_SOURCE,
     OSD_VTX_POWER,
     OSD_ESC_RPM,
+    OSD_ESC_TEMPERATURE,
     OSD_ITEM_COUNT // MUST BE LAST
 } osd_items_e;
 
@@ -205,6 +206,8 @@ typedef struct osdConfig_s {
     uint8_t current_alarm; // current consumption in A
     int16_t imu_temp_alarm_min;
     int16_t imu_temp_alarm_max;
+    int16_t esc_temp_alarm_min;
+    int16_t esc_temp_alarm_max;
     float gforce_alarm;
     float gforce_axis_alarm_min;
     float gforce_axis_alarm_max;

From 566d5805506bfe12a6bd7a64da1fa86dfc17ce3d Mon Sep 17 00:00:00 2001
From: "Konstantin (DigitalEntity) Sharlaimov"
 <konstantin.sharlaimov@gmail.com>
Date: Mon, 8 Jul 2019 21:46:51 +0200
Subject: [PATCH 095/179] [GYRO] Refactor gyro driver for dual-gyro support

---
 src/main/sensors/gyro.c | 78 ++++++++++++++++++++++++++---------------
 1 file changed, 49 insertions(+), 29 deletions(-)

diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index c51466322e..3ec5128579 100644
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@@ -81,9 +81,7 @@ FASTRAM gyro_t gyro; // gyro sensor object
 
 STATIC_UNIT_TESTED gyroDev_t gyroDev0;  // Not in FASTRAM since it may hold DMA buffers
 STATIC_FASTRAM int16_t gyroTemperature0;
-
-STATIC_FASTRAM_UNIT_TESTED zeroCalibrationVector_t gyroCalibration;
-STATIC_FASTRAM int32_t gyroADC[XYZ_AXIS_COUNT];
+STATIC_FASTRAM_UNIT_TESTED zeroCalibrationVector_t gyroCalibration0;
 
 STATIC_FASTRAM filterApplyFnPtr gyroLpfApplyFn;
 STATIC_FASTRAM filter_t gyroLpfState[XYZ_AXIS_COUNT];
@@ -256,10 +254,6 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHard
         gyroHardware = GYRO_NONE;
     }
 
-    if (gyroHardware != GYRO_NONE) {
-        detectedSensors[SENSOR_INDEX_GYRO] = gyroHardware;
-        sensorsSet(SENSOR_GYRO);
-    }
     return gyroHardware;
 }
 
@@ -274,10 +268,16 @@ bool gyroInit(void)
     gyroDev0.imuSensorToUse = 0;
 #endif
 
-    if (gyroDetect(&gyroDev0, GYRO_AUTODETECT) == GYRO_NONE) {
+    // Detecting gyro0
+    gyroSensor_e gyroHardware0 = gyroDetect(&gyroDev0, GYRO_AUTODETECT);
+
+    if (gyroHardware0 == GYRO_NONE) {
         return false;
     }
 
+    detectedSensors[SENSOR_INDEX_GYRO] = gyroHardware0;
+    sensorsSet(SENSOR_GYRO);
+
     // Driver initialisation
     gyroDev0.lpf = gyroConfig()->gyro_lpf;
     gyroDev0.requestedSampleIntervalUs = gyroConfig()->looptime;
@@ -356,15 +356,15 @@ void gyroInitFilters(void)
 
 void gyroStartCalibration(void)
 {
-    zeroCalibrationStartV(&gyroCalibration, CALIBRATING_GYRO_TIME_MS, gyroConfig()->gyroMovementCalibrationThreshold, false);
+    zeroCalibrationStartV(&gyroCalibration0, CALIBRATING_GYRO_TIME_MS, gyroConfig()->gyroMovementCalibrationThreshold, false);
 }
 
 bool gyroIsCalibrationComplete(void)
 {
-    return zeroCalibrationIsCompleteV(&gyroCalibration) && zeroCalibrationIsSuccessfulV(&gyroCalibration);
+    return zeroCalibrationIsCompleteV(&gyroCalibration0) && zeroCalibrationIsSuccessfulV(&gyroCalibration0);
 }
 
-STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, zeroCalibrationVector_t *gyroCalibration)
+STATIC_UNIT_TESTED void performgyroCalibration0(gyroDev_t *dev, zeroCalibrationVector_t *gyroCalibration0)
 {
     fpVector3_t v;
 
@@ -373,11 +373,11 @@ STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, zeroCalibrationVe
     v.v[1] = dev->gyroADCRaw[1];
     v.v[2] = dev->gyroADCRaw[2];
 
-    zeroCalibrationAddValueV(gyroCalibration, &v);
+    zeroCalibrationAddValueV(gyroCalibration0, &v);
 
     // Check if calibration is complete after this cycle
-    if (zeroCalibrationIsCompleteV(gyroCalibration)) {
-        zeroCalibrationGetZeroV(gyroCalibration, &v);
+    if (zeroCalibrationIsCompleteV(gyroCalibration0)) {
+        zeroCalibrationGetZeroV(gyroCalibration0, &v);
         dev->gyroZero[0] = v.v[0];
         dev->gyroZero[1] = v.v[1];
         dev->gyroZero[2] = v.v[2];
@@ -402,33 +402,53 @@ void gyroGetMeasuredRotationRate(fpVector3_t *measuredRotationRate)
     }
 }
 
-void gyroUpdate()
+static bool FAST_CODE NOINLINE gyroUpdateAndCalibrate(gyroDev_t * gyroDev, zeroCalibrationVector_t * gyroCal, float * gyroADCf)
 {
     // range: +/- 8192; +/- 2000 deg/sec
-    if (gyroDev0.readFn(&gyroDev0)) {
-        if (zeroCalibrationIsCompleteV(&gyroCalibration)) {
+    if (gyroDev->readFn(gyroDev)) {
+        if (zeroCalibrationIsCompleteV(gyroCal)) {
+            int32_t gyroADCtmp[XYZ_AXIS_COUNT];
+
             // Copy gyro value into int32_t (to prevent overflow) and then apply calibration and alignment
-            gyroADC[X] = (int32_t)gyroDev0.gyroADCRaw[X] - (int32_t)gyroDev0.gyroZero[X];
-            gyroADC[Y] = (int32_t)gyroDev0.gyroADCRaw[Y] - (int32_t)gyroDev0.gyroZero[Y];
-            gyroADC[Z] = (int32_t)gyroDev0.gyroADCRaw[Z] - (int32_t)gyroDev0.gyroZero[Z];
-            applySensorAlignment(gyroADC, gyroADC, gyroDev0.gyroAlign);
-            applyBoardAlignment(gyroADC);
+            gyroADCtmp[X] = (int32_t)gyroDev->gyroADCRaw[X] - (int32_t)gyroDev->gyroZero[X];
+            gyroADCtmp[Y] = (int32_t)gyroDev->gyroADCRaw[Y] - (int32_t)gyroDev->gyroZero[Y];
+            gyroADCtmp[Z] = (int32_t)gyroDev->gyroADCRaw[Z] - (int32_t)gyroDev->gyroZero[Z];
+
+            // Apply sensor alignment
+            applySensorAlignment(gyroADCtmp, gyroADCtmp, gyroDev->gyroAlign);
+            applyBoardAlignment(gyroADCtmp);
+
+            // Convert to deg/s and store in unified data
+            gyroADCf[X] = (float)gyroADCtmp[X] * gyroDev->scale;
+            gyroADCf[Y] = (float)gyroADCtmp[Y] * gyroDev->scale;
+            gyroADCf[Z] = (float)gyroADCtmp[Z] * gyroDev->scale;
+
+            return true;
         } else {
-            performGyroCalibration(&gyroDev0, &gyroCalibration);
+            performgyroCalibration0(gyroDev, gyroCal);
+
             // Reset gyro values to zero to prevent other code from using uncalibrated data
-            gyro.gyroADCf[X] = 0.0f;
-            gyro.gyroADCf[Y] = 0.0f;
-            gyro.gyroADCf[Z] = 0.0f;
-            // still calibrating, so no need to further process gyro data
-            return;
+            gyroADCf[X] = 0.0f;
+            gyroADCf[Y] = 0.0f;
+            gyroADCf[Z] = 0.0f;
+
+            return false;
         }
     } else {
         // no gyro reading to process
+        return false;
+    }
+}
+
+void FAST_CODE NOINLINE gyroUpdate()
+{
+    if (!gyroUpdateAndCalibrate(&gyroDev0, &gyroCalibration0, gyro.gyroADCf)) {
         return;
     }
 
     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-        float gyroADCf = (float)gyroADC[axis] * gyroDev0.scale;
+        // At this point gyro.gyroADCf contains unfiltered gyro value
+        float gyroADCf = gyro.gyroADCf[axis];
 
         DEBUG_SET(DEBUG_GYRO, axis, lrintf(gyroADCf));
 

From 76904ecfdaef40ff187af756b28910caf26de7fd Mon Sep 17 00:00:00 2001
From: giacomo892 <giacomo1989@gmail.com>
Date: Fri, 24 Apr 2020 21:53:47 +0200
Subject: [PATCH 096/179] add ESC RPM and Temperature to BB

---
 src/main/blackbox/blackbox.c | 18 ++++++++++++++++++
 1 file changed, 18 insertions(+)

diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c
index 4095224c87..1a490d17e5 100644
--- a/src/main/blackbox/blackbox.c
+++ b/src/main/blackbox/blackbox.c
@@ -77,6 +77,7 @@
 #include "sensors/pitotmeter.h"
 #include "sensors/rangefinder.h"
 #include "sensors/sensors.h"
+#include "sensors/esc_sensor.h"
 #include "flight/wind_estimator.h"
 #include "sensors/temperature.h"
 
@@ -390,6 +391,10 @@ static const blackboxSimpleFieldDefinition_t blackboxSlowFields[] = {
     {"sens6Temp",             -1, SIGNED,   PREDICT(0),      ENCODING(SIGNED_VB)},
     {"sens7Temp",             -1, SIGNED,   PREDICT(0),      ENCODING(SIGNED_VB)},
 #endif
+#ifdef USE_ESC_SENSOR
+    {"escRPM",                -1, UNSIGNED, PREDICT(0),             ENCODING(UNSIGNED_VB)},
+    {"escTemperature",        -1, SIGNED,   PREDICT(PREVIOUS),      ENCODING(SIGNED_VB)},
+#endif
 };
 
 typedef enum BlackboxState {
@@ -498,6 +503,10 @@ typedef struct blackboxSlowState_s {
 #ifdef USE_TEMPERATURE_SENSOR
     int16_t tempSensorTemperature[MAX_TEMP_SENSORS];
 #endif
+#ifdef USE_ESC_SENSOR
+    uint32_t escRPM;
+    int8_t escTemperature;
+#endif
 } __attribute__((__packed__)) blackboxSlowState_t; // We pack this struct so that padding doesn't interfere with memcmp()
 
 //From rc_controls.c
@@ -1104,6 +1113,10 @@ static void writeSlowFrame(void)
     blackboxWriteSigned16VBArray(slowHistory.tempSensorTemperature, MAX_TEMP_SENSORS);
 #endif
 
+#ifdef USE_ESC_SENSOR
+    blackboxWriteUnsignedVB(slowHistory.escRPM);
+    blackboxWriteSignedVB(slowHistory.escTemperature);
+#endif
     blackboxSlowFrameIterationTimer = 0;
 }
 
@@ -1156,6 +1169,11 @@ static void loadSlowState(blackboxSlowState_t *slow)
     }
 #endif
 
+#ifdef USE_ESC_SENSOR
+    escSensorData_t * escSensor = escSensorGetData(); 
+    slow->escRPM = escSensor->rpm;
+    slow->escTemperature = escSensor->temperature;
+#endif
 }
 
 /**

From 6d045b27c0025135cbcd5c2419ab9509bc839427 Mon Sep 17 00:00:00 2001
From: "Konstantin (DigitalEntity) Sharlaimov"
 <konstantin.sharlaimov@gmail.com>
Date: Sat, 25 Apr 2020 14:13:21 +0200
Subject: [PATCH 097/179] [GYRO] More refactoring

---
 src/main/sensors/gyro.c | 62 ++++++++++++++++++++---------------------
 1 file changed, 31 insertions(+), 31 deletions(-)

diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index 3ec5128579..28283aa7da 100644
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@@ -79,9 +79,11 @@ FILE_COMPILE_FOR_SPEED
 
 FASTRAM gyro_t gyro; // gyro sensor object
 
-STATIC_UNIT_TESTED gyroDev_t gyroDev0;  // Not in FASTRAM since it may hold DMA buffers
-STATIC_FASTRAM int16_t gyroTemperature0;
-STATIC_FASTRAM_UNIT_TESTED zeroCalibrationVector_t gyroCalibration0;
+#define MAX_GYRO_COUNT          1
+
+STATIC_UNIT_TESTED gyroDev_t gyroDev[MAX_GYRO_COUNT];  // Not in FASTRAM since it may hold DMA buffers
+STATIC_FASTRAM int16_t gyroTemperature[MAX_GYRO_COUNT];
+STATIC_FASTRAM_UNIT_TESTED zeroCalibrationVector_t gyroCalibration[MAX_GYRO_COUNT];
 
 STATIC_FASTRAM filterApplyFnPtr gyroLpfApplyFn;
 STATIC_FASTRAM filter_t gyroLpfState[XYZ_AXIS_COUNT];
@@ -263,32 +265,30 @@ bool gyroInit(void)
 
     // Set inertial sensor tag (for dual-gyro selection)
 #ifdef USE_DUAL_GYRO
-    gyroDev0.imuSensorToUse = gyroConfig()->gyro_to_use;
+    gyroDev[0].imuSensorToUse = gyroConfig()->gyro_to_use;
 #else
-    gyroDev0.imuSensorToUse = 0;
+    gyroDev[0].imuSensorToUse = 0;
 #endif
 
     // Detecting gyro0
-    gyroSensor_e gyroHardware0 = gyroDetect(&gyroDev0, GYRO_AUTODETECT);
-
-    if (gyroHardware0 == GYRO_NONE) {
+    gyroSensor_e gyroHardware = gyroDetect(&gyroDev[0], GYRO_AUTODETECT);
+    if (gyroHardware == GYRO_NONE) {
         return false;
     }
-
-    detectedSensors[SENSOR_INDEX_GYRO] = gyroHardware0;
+    detectedSensors[SENSOR_INDEX_GYRO] = gyroHardware;
     sensorsSet(SENSOR_GYRO);
 
     // Driver initialisation
-    gyroDev0.lpf = gyroConfig()->gyro_lpf;
-    gyroDev0.requestedSampleIntervalUs = gyroConfig()->looptime;
-    gyroDev0.sampleRateIntervalUs = gyroConfig()->looptime;
-    gyroDev0.initFn(&gyroDev0);
+    gyroDev[0].lpf = gyroConfig()->gyro_lpf;
+    gyroDev[0].requestedSampleIntervalUs = gyroConfig()->looptime;
+    gyroDev[0].sampleRateIntervalUs = gyroConfig()->looptime;
+    gyroDev[0].initFn(&gyroDev[0]);
 
     // initFn will initialize sampleRateIntervalUs to actual gyro sampling rate (if driver supports it). Calculate target looptime using that value
-    gyro.targetLooptime = gyroConfig()->gyroSync ? gyroDev0.sampleRateIntervalUs : gyroConfig()->looptime;
+    gyro.targetLooptime = gyroConfig()->gyroSync ? gyroDev[0].sampleRateIntervalUs : gyroConfig()->looptime;
 
     if (gyroConfig()->gyro_align != ALIGN_DEFAULT) {
-        gyroDev0.gyroAlign = gyroConfig()->gyro_align;
+        gyroDev[0].gyroAlign = gyroConfig()->gyro_align;
     }
 
     gyroInitFilters();
@@ -356,15 +356,15 @@ void gyroInitFilters(void)
 
 void gyroStartCalibration(void)
 {
-    zeroCalibrationStartV(&gyroCalibration0, CALIBRATING_GYRO_TIME_MS, gyroConfig()->gyroMovementCalibrationThreshold, false);
+    zeroCalibrationStartV(&gyroCalibration[0], CALIBRATING_GYRO_TIME_MS, gyroConfig()->gyroMovementCalibrationThreshold, false);
 }
 
 bool gyroIsCalibrationComplete(void)
 {
-    return zeroCalibrationIsCompleteV(&gyroCalibration0) && zeroCalibrationIsSuccessfulV(&gyroCalibration0);
+    return zeroCalibrationIsCompleteV(&gyroCalibration[0]) && zeroCalibrationIsSuccessfulV(&gyroCalibration[0]);
 }
 
-STATIC_UNIT_TESTED void performgyroCalibration0(gyroDev_t *dev, zeroCalibrationVector_t *gyroCalibration0)
+STATIC_UNIT_TESTED void performgyroCalibration(gyroDev_t *dev, zeroCalibrationVector_t *gyroCalibration)
 {
     fpVector3_t v;
 
@@ -373,11 +373,11 @@ STATIC_UNIT_TESTED void performgyroCalibration0(gyroDev_t *dev, zeroCalibrationV
     v.v[1] = dev->gyroADCRaw[1];
     v.v[2] = dev->gyroADCRaw[2];
 
-    zeroCalibrationAddValueV(gyroCalibration0, &v);
+    zeroCalibrationAddValueV(gyroCalibration, &v);
 
     // Check if calibration is complete after this cycle
-    if (zeroCalibrationIsCompleteV(gyroCalibration0)) {
-        zeroCalibrationGetZeroV(gyroCalibration0, &v);
+    if (zeroCalibrationIsCompleteV(gyroCalibration)) {
+        zeroCalibrationGetZeroV(gyroCalibration, &v);
         dev->gyroZero[0] = v.v[0];
         dev->gyroZero[1] = v.v[1];
         dev->gyroZero[2] = v.v[2];
@@ -425,7 +425,7 @@ static bool FAST_CODE NOINLINE gyroUpdateAndCalibrate(gyroDev_t * gyroDev, zeroC
 
             return true;
         } else {
-            performgyroCalibration0(gyroDev, gyroCal);
+            performgyroCalibration(gyroDev, gyroCal);
 
             // Reset gyro values to zero to prevent other code from using uncalibrated data
             gyroADCf[X] = 0.0f;
@@ -442,12 +442,12 @@ static bool FAST_CODE NOINLINE gyroUpdateAndCalibrate(gyroDev_t * gyroDev, zeroC
 
 void FAST_CODE NOINLINE gyroUpdate()
 {
-    if (!gyroUpdateAndCalibrate(&gyroDev0, &gyroCalibration0, gyro.gyroADCf)) {
+    if (!gyroUpdateAndCalibrate(&gyroDev[0], &gyroCalibration[0], gyro.gyroADCf)) {
         return;
     }
 
     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-        // At this point gyro.gyroADCf contains unfiltered gyro value
+        // At this point gyro.gyroADCf contains unfiltered gyro value [deg/s]
         float gyroADCf = gyro.gyroADCf[axis];
 
         DEBUG_SET(DEBUG_GYRO, axis, lrintf(gyroADCf));
@@ -493,8 +493,8 @@ void FAST_CODE NOINLINE gyroUpdate()
 bool gyroReadTemperature(void)
 {
     // Read gyro sensor temperature. temperatureFn returns temperature in [degC * 10]
-    if (gyroDev0.temperatureFn) {
-        return gyroDev0.temperatureFn(&gyroDev0, &gyroTemperature0);
+    if (gyroDev[0].temperatureFn) {
+        return gyroDev[0].temperatureFn(&gyroDev[0], &gyroTemperature[0]);
     }
 
     return false;
@@ -502,18 +502,18 @@ bool gyroReadTemperature(void)
 
 int16_t gyroGetTemperature(void)
 {
-    return gyroTemperature0;
+    return gyroTemperature[0];
 }
 
 int16_t gyroRateDps(int axis)
 {
-    return lrintf(gyro.gyroADCf[axis] / gyroDev0.scale);
+    return lrintf(gyro.gyroADCf[axis]);
 }
 
 bool gyroSyncCheckUpdate(void)
 {
-    if (!gyroDev0.intStatusFn)
+    if (!gyroDev[0].intStatusFn)
         return false;
 
-    return gyroDev0.intStatusFn(&gyroDev0);
+    return gyroDev[0].intStatusFn(&gyroDev[0]);
 }

From 4adb123e9020171f698ed6546908966cffc07aa7 Mon Sep 17 00:00:00 2001
From: "Konstantin (DigitalEntity) Sharlaimov"
 <konstantin.sharlaimov@gmail.com>
Date: Sat, 25 Apr 2020 14:40:33 +0200
Subject: [PATCH 098/179] [GYRO] Fix tests

---
 src/main/sensors/gyro.c               |  4 +-
 src/test/unit/sensor_gyro_unittest.cc | 55 +++++++++++++--------------
 2 files changed, 29 insertions(+), 30 deletions(-)

diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index 28283aa7da..822a787a32 100644
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@@ -364,7 +364,7 @@ bool gyroIsCalibrationComplete(void)
     return zeroCalibrationIsCompleteV(&gyroCalibration[0]) && zeroCalibrationIsSuccessfulV(&gyroCalibration[0]);
 }
 
-STATIC_UNIT_TESTED void performgyroCalibration(gyroDev_t *dev, zeroCalibrationVector_t *gyroCalibration)
+STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, zeroCalibrationVector_t *gyroCalibration)
 {
     fpVector3_t v;
 
@@ -425,7 +425,7 @@ static bool FAST_CODE NOINLINE gyroUpdateAndCalibrate(gyroDev_t * gyroDev, zeroC
 
             return true;
         } else {
-            performgyroCalibration(gyroDev, gyroCal);
+            performGyroCalibration(gyroDev, gyroCal);
 
             // Reset gyro values to zero to prevent other code from using uncalibrated data
             gyroADCf[X] = 0.0f;
diff --git a/src/test/unit/sensor_gyro_unittest.cc b/src/test/unit/sensor_gyro_unittest.cc
index a17312e4dc..0e49751519 100644
--- a/src/test/unit/sensor_gyro_unittest.cc
+++ b/src/test/unit/sensor_gyro_unittest.cc
@@ -39,7 +39,7 @@ extern "C" {
     #include "sensors/sensors.h"
 
     extern zeroCalibrationVector_t gyroCalibration;
-    extern gyroDev_t gyroDev0;
+    extern gyroDev_t gyroDev[];
 
     STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHardware);
     STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, zeroCalibrationVector_t *gyroCalibration);
@@ -50,9 +50,8 @@ extern "C" {
 
 TEST(SensorGyro, Detect)
 {
-    const gyroSensor_e detected = gyroDetect(&gyroDev0, GYRO_AUTODETECT);
+    const gyroSensor_e detected = gyroDetect(&gyroDev[0], GYRO_AUTODETECT);
     EXPECT_EQ(GYRO_FAKE, detected);
-    EXPECT_EQ(GYRO_FAKE, detectedSensors[SENSOR_INDEX_GYRO]);
 }
 
 TEST(SensorGyro, Init)
@@ -67,11 +66,11 @@ TEST(SensorGyro, Read)
     gyroInit();
     EXPECT_EQ(GYRO_FAKE, detectedSensors[SENSOR_INDEX_GYRO]);
     fakeGyroSet(5, 6, 7);
-    const bool read = gyroDev0.readFn(&gyroDev0);
+    const bool read = gyroDev[0].readFn(&gyroDev[0]);
     EXPECT_EQ(true, read);
-    EXPECT_EQ(5, gyroDev0.gyroADCRaw[X]);
-    EXPECT_EQ(6, gyroDev0.gyroADCRaw[Y]);
-    EXPECT_EQ(7, gyroDev0.gyroADCRaw[Z]);
+    EXPECT_EQ(5, gyroDev[0].gyroADCRaw[X]);
+    EXPECT_EQ(6, gyroDev[0].gyroADCRaw[Y]);
+    EXPECT_EQ(7, gyroDev[0].gyroADCRaw[Z]);
 }
 
 TEST(SensorGyro, Calibrate)
@@ -79,25 +78,25 @@ TEST(SensorGyro, Calibrate)
     gyroStartCalibration();
     gyroInit();
     fakeGyroSet(5, 6, 7);
-    const bool read = gyroDev0.readFn(&gyroDev0);
+    const bool read = gyroDev[0].readFn(&gyroDev[0]);
     EXPECT_EQ(true, read);
-    EXPECT_EQ(5, gyroDev0.gyroADCRaw[X]);
-    EXPECT_EQ(6, gyroDev0.gyroADCRaw[Y]);
-    EXPECT_EQ(7, gyroDev0.gyroADCRaw[Z]);
-    gyroDev0.gyroZero[X] = 8;
-    gyroDev0.gyroZero[Y] = 9;
-    gyroDev0.gyroZero[Z] = 10;
-    performGyroCalibration(&gyroDev0, &gyroCalibration);
-    EXPECT_EQ(0, gyroDev0.gyroZero[X]);
-    EXPECT_EQ(0, gyroDev0.gyroZero[Y]);
-    EXPECT_EQ(0, gyroDev0.gyroZero[Z]);
+    EXPECT_EQ(5, gyroDev[0].gyroADCRaw[X]);
+    EXPECT_EQ(6, gyroDev[0].gyroADCRaw[Y]);
+    EXPECT_EQ(7, gyroDev[0].gyroADCRaw[Z]);
+    gyroDev[0].gyroZero[X] = 8;
+    gyroDev[0].gyroZero[Y] = 9;
+    gyroDev[0].gyroZero[Z] = 10;
+    performGyroCalibration(&gyroDev[0], &gyroCalibration);
+    EXPECT_EQ(0, gyroDev[0].gyroZero[X]);
+    EXPECT_EQ(0, gyroDev[0].gyroZero[Y]);
+    EXPECT_EQ(0, gyroDev[0].gyroZero[Z]);
     EXPECT_EQ(false, gyroIsCalibrationComplete());
     while (!gyroIsCalibrationComplete()) {
-        performGyroCalibration(&gyroDev0, &gyroCalibration);
+        performGyroCalibration(&gyroDev[0], &gyroCalibration);
     }
-    EXPECT_EQ(5, gyroDev0.gyroZero[X]);
-    EXPECT_EQ(6, gyroDev0.gyroZero[Y]);
-    EXPECT_EQ(7, gyroDev0.gyroZero[Z]);
+    EXPECT_EQ(5, gyroDev[0].gyroZero[X]);
+    EXPECT_EQ(6, gyroDev[0].gyroZero[Y]);
+    EXPECT_EQ(7, gyroDev[0].gyroZero[Z]);
 }
 
 TEST(SensorGyro, Update)
@@ -112,9 +111,9 @@ TEST(SensorGyro, Update)
         gyroUpdate();
     }
     EXPECT_EQ(true, gyroIsCalibrationComplete());
-    EXPECT_EQ(5, gyroDev0.gyroZero[X]);
-    EXPECT_EQ(6, gyroDev0.gyroZero[Y]);
-    EXPECT_EQ(7, gyroDev0.gyroZero[Z]);
+    EXPECT_EQ(5, gyroDev[0].gyroZero[X]);
+    EXPECT_EQ(6, gyroDev[0].gyroZero[Y]);
+    EXPECT_EQ(7, gyroDev[0].gyroZero[Z]);
     EXPECT_FLOAT_EQ(0, gyro.gyroADCf[X]);
     EXPECT_FLOAT_EQ(0, gyro.gyroADCf[Y]);
     EXPECT_FLOAT_EQ(0, gyro.gyroADCf[Z]);
@@ -125,9 +124,9 @@ TEST(SensorGyro, Update)
     EXPECT_FLOAT_EQ(0, gyro.gyroADCf[Z]);
     fakeGyroSet(15, 26, 97);
     gyroUpdate();
-    EXPECT_FLOAT_EQ(10 * gyroDev0.scale, gyro.gyroADCf[X]); // gyroADCf values are scaled
-    EXPECT_FLOAT_EQ(20 * gyroDev0.scale, gyro.gyroADCf[Y]);
-    EXPECT_FLOAT_EQ(90 * gyroDev0.scale, gyro.gyroADCf[Z]);
+    EXPECT_FLOAT_EQ(10 * gyroDev[0].scale, gyro.gyroADCf[X]); // gyroADCf values are scaled
+    EXPECT_FLOAT_EQ(20 * gyroDev[0].scale, gyro.gyroADCf[Y]);
+    EXPECT_FLOAT_EQ(90 * gyroDev[0].scale, gyro.gyroADCf[Z]);
 }
 
 

From 121edfc86b7e231fa0223c17c7f9376cf5e65f5b Mon Sep 17 00:00:00 2001
From: "Konstantin (DigitalEntity) Sharlaimov"
 <konstantin.sharlaimov@gmail.com>
Date: Sat, 25 Apr 2020 14:57:48 +0200
Subject: [PATCH 099/179] [GYRO] Gracefully handle gyro detection failure

---
 src/main/sensors/diagnostics.c |   6 +-
 src/main/sensors/gyro.c        | 138 ++++++++++++++++++++-------------
 src/main/sensors/gyro.h        |   2 +-
 3 files changed, 92 insertions(+), 54 deletions(-)

diff --git a/src/main/sensors/diagnostics.c b/src/main/sensors/diagnostics.c
index ca1c4c67f0..480eaa27ab 100644
--- a/src/main/sensors/diagnostics.c
+++ b/src/main/sensors/diagnostics.c
@@ -30,7 +30,11 @@ extern uint8_t detectedSensors[SENSOR_INDEX_COUNT];
 
 hardwareSensorStatus_e getHwGyroStatus(void)
 {
-    // Gyro is assumed to be always healthy
+    // Gyro is assumed to be always healthy, but it must be present
+    if (detectedSensors[SENSOR_INDEX_GYRO] == GYRO_NONE) {
+        return HW_SENSOR_UNAVAILABLE;
+    }
+
     return HW_SENSOR_OK;
 }
 
diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index 822a787a32..a12bc8ef15 100644
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@@ -259,6 +259,56 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHard
     return gyroHardware;
 }
 
+static void initGyroFilter(filterApplyFnPtr *applyFn, filter_t state[], uint8_t type, uint16_t cutoff)
+{
+    *applyFn = nullFilterApply;
+    if (cutoff > 0) {
+        switch (type) 
+        {
+            case FILTER_PT1:
+                *applyFn = (filterApplyFnPtr)pt1FilterApply;
+                for (int axis = 0; axis < 3; axis++) {
+                    pt1FilterInit(&state[axis].pt1, cutoff, getLooptime()* 1e-6f);
+                }
+                break;
+            case FILTER_BIQUAD:
+                *applyFn = (filterApplyFnPtr)biquadFilterApply;
+                for (int axis = 0; axis < 3; axis++) {
+                    biquadFilterInitLPF(&state[axis].biquad, cutoff, getLooptime());
+                }
+                break;
+        }
+    }
+}
+
+static void gyroInitFilters(void)
+{
+    STATIC_FASTRAM biquadFilter_t gyroFilterNotch_1[XYZ_AXIS_COUNT];
+    notchFilter1ApplyFn = nullFilterApply;
+    
+    STATIC_FASTRAM biquadFilter_t gyroFilterNotch_2[XYZ_AXIS_COUNT];
+    notchFilter2ApplyFn = nullFilterApply;
+    
+    initGyroFilter(&gyroLpf2ApplyFn, gyroLpf2State, gyroConfig()->gyro_stage2_lowpass_type, gyroConfig()->gyro_stage2_lowpass_hz);
+    initGyroFilter(&gyroLpfApplyFn, gyroLpfState, gyroConfig()->gyro_soft_lpf_type, gyroConfig()->gyro_soft_lpf_hz);
+
+    if (gyroConfig()->gyro_soft_notch_hz_1) {
+        notchFilter1ApplyFn = (filterApplyFnPtr)biquadFilterApply;
+        for (int axis = 0; axis < 3; axis++) {
+            notchFilter1[axis] = &gyroFilterNotch_1[axis];
+            biquadFilterInitNotch(notchFilter1[axis], getLooptime(), gyroConfig()->gyro_soft_notch_hz_1, gyroConfig()->gyro_soft_notch_cutoff_1);
+        }
+    }
+
+    if (gyroConfig()->gyro_soft_notch_hz_2) {
+        notchFilter2ApplyFn = (filterApplyFnPtr)biquadFilterApply;
+        for (int axis = 0; axis < 3; axis++) {
+            notchFilter2[axis] = &gyroFilterNotch_2[axis];
+            biquadFilterInitNotch(notchFilter2[axis], getLooptime(), gyroConfig()->gyro_soft_notch_hz_2, gyroConfig()->gyro_soft_notch_cutoff_2);
+        }
+    }
+}
+
 bool gyroInit(void)
 {
     memset(&gyro, 0, sizeof(gyro));
@@ -273,8 +323,13 @@ bool gyroInit(void)
     // Detecting gyro0
     gyroSensor_e gyroHardware = gyroDetect(&gyroDev[0], GYRO_AUTODETECT);
     if (gyroHardware == GYRO_NONE) {
-        return false;
+        gyro.initialized = false;
+        detectedSensors[SENSOR_INDEX_GYRO] = GYRO_NONE;
+        return true;
     }
+
+    // Gyro is initialized
+    gyro.initialized = true;
     detectedSensors[SENSOR_INDEX_GYRO] = gyroHardware;
     sensorsSet(SENSOR_GYRO);
 
@@ -304,63 +359,21 @@ bool gyroInit(void)
     return true;
 }
 
-static void initGyroFilter(filterApplyFnPtr *applyFn, filter_t state[], uint8_t type, uint16_t cutoff)
-{
-    *applyFn = nullFilterApply;
-    if (cutoff > 0) {
-        switch (type) 
-        {
-            case FILTER_PT1:
-                *applyFn = (filterApplyFnPtr)pt1FilterApply;
-                for (int axis = 0; axis < 3; axis++) {
-                    pt1FilterInit(&state[axis].pt1, cutoff, getLooptime()* 1e-6f);
-                }
-                break;
-            case FILTER_BIQUAD:
-                *applyFn = (filterApplyFnPtr)biquadFilterApply;
-                for (int axis = 0; axis < 3; axis++) {
-                    biquadFilterInitLPF(&state[axis].biquad, cutoff, getLooptime());
-                }
-                break;
-        }
-    }
-}
-
-void gyroInitFilters(void)
-{
-    STATIC_FASTRAM biquadFilter_t gyroFilterNotch_1[XYZ_AXIS_COUNT];
-    notchFilter1ApplyFn = nullFilterApply;
-    
-    STATIC_FASTRAM biquadFilter_t gyroFilterNotch_2[XYZ_AXIS_COUNT];
-    notchFilter2ApplyFn = nullFilterApply;
-    
-    initGyroFilter(&gyroLpf2ApplyFn, gyroLpf2State, gyroConfig()->gyro_stage2_lowpass_type, gyroConfig()->gyro_stage2_lowpass_hz);
-    initGyroFilter(&gyroLpfApplyFn, gyroLpfState, gyroConfig()->gyro_soft_lpf_type, gyroConfig()->gyro_soft_lpf_hz);
-
-    if (gyroConfig()->gyro_soft_notch_hz_1) {
-        notchFilter1ApplyFn = (filterApplyFnPtr)biquadFilterApply;
-        for (int axis = 0; axis < 3; axis++) {
-            notchFilter1[axis] = &gyroFilterNotch_1[axis];
-            biquadFilterInitNotch(notchFilter1[axis], getLooptime(), gyroConfig()->gyro_soft_notch_hz_1, gyroConfig()->gyro_soft_notch_cutoff_1);
-        }
-    }
-
-    if (gyroConfig()->gyro_soft_notch_hz_2) {
-        notchFilter2ApplyFn = (filterApplyFnPtr)biquadFilterApply;
-        for (int axis = 0; axis < 3; axis++) {
-            notchFilter2[axis] = &gyroFilterNotch_2[axis];
-            biquadFilterInitNotch(notchFilter2[axis], getLooptime(), gyroConfig()->gyro_soft_notch_hz_2, gyroConfig()->gyro_soft_notch_cutoff_2);
-        }
-    }
-}
-
 void gyroStartCalibration(void)
 {
+    if (!gyro.initialized) {
+        return;
+    }
+
     zeroCalibrationStartV(&gyroCalibration[0], CALIBRATING_GYRO_TIME_MS, gyroConfig()->gyroMovementCalibrationThreshold, false);
 }
 
 bool gyroIsCalibrationComplete(void)
 {
+    if (!gyro.initialized) {
+        return true;
+    }
+
     return zeroCalibrationIsCompleteV(&gyroCalibration[0]) && zeroCalibrationIsSuccessfulV(&gyroCalibration[0]);
 }
 
@@ -442,6 +455,10 @@ static bool FAST_CODE NOINLINE gyroUpdateAndCalibrate(gyroDev_t * gyroDev, zeroC
 
 void FAST_CODE NOINLINE gyroUpdate()
 {
+    if (!gyro.initialized) {
+        return;
+    }
+
     if (!gyroUpdateAndCalibrate(&gyroDev[0], &gyroCalibration[0], gyro.gyroADCf)) {
         return;
     }
@@ -492,6 +509,10 @@ void FAST_CODE NOINLINE gyroUpdate()
 
 bool gyroReadTemperature(void)
 {
+    if (!gyro.initialized) {
+        return false;
+    }
+
     // Read gyro sensor temperature. temperatureFn returns temperature in [degC * 10]
     if (gyroDev[0].temperatureFn) {
         return gyroDev[0].temperatureFn(&gyroDev[0], &gyroTemperature[0]);
@@ -502,18 +523,31 @@ bool gyroReadTemperature(void)
 
 int16_t gyroGetTemperature(void)
 {
+    if (!gyro.initialized) {
+        return 0;
+    }
+
     return gyroTemperature[0];
 }
 
 int16_t gyroRateDps(int axis)
 {
+    if (!gyro.initialized) {
+        return 0;
+    }
+
     return lrintf(gyro.gyroADCf[axis]);
 }
 
 bool gyroSyncCheckUpdate(void)
 {
-    if (!gyroDev[0].intStatusFn)
+    if (!gyro.initialized) {
         return false;
+    }
+
+    if (!gyroDev[0].intStatusFn) {
+        return false;
+    }
 
     return gyroDev[0].intStatusFn(&gyroDev[0]);
 }
diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h
index 51e7fda3af..414d9042cc 100644
--- a/src/main/sensors/gyro.h
+++ b/src/main/sensors/gyro.h
@@ -50,6 +50,7 @@ typedef enum {
 #define DYN_NOTCH_RANGE_HZ_LOW 1000
 
 typedef struct gyro_s {
+    bool initialized;
     uint32_t targetLooptime;
     float gyroADCf[XYZ_AXIS_COUNT];
 } gyro_t;
@@ -80,7 +81,6 @@ typedef struct gyroConfig_s {
 PG_DECLARE(gyroConfig_t, gyroConfig);
 
 bool gyroInit(void);
-void gyroInitFilters(void);
 void gyroGetMeasuredRotationRate(fpVector3_t *imuMeasuredRotationBF);
 void gyroUpdate(void);
 void gyroStartCalibration(void);

From debbf9ec43c732ac9cf2749f4305d6afa6a2299d Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Sat, 25 Apr 2020 15:08:13 +0200
Subject: [PATCH 100/179] Rename servo input ONE to MAX

---
 docs/Mixer.md            | 2 +-
 src/main/flight/servos.c | 2 +-
 src/main/flight/servos.h | 2 +-
 3 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/docs/Mixer.md b/docs/Mixer.md
index 575aba098c..1e3cc1369e 100644
--- a/docs/Mixer.md
+++ b/docs/Mixer.md
@@ -82,7 +82,7 @@ Each servo mixing rule has the following parameters:
 | 26 | Stabilized PITCH-        | Clipped between -1000 and 0 |
 | 27 | Stabilized YAW+          | Clipped between 0 and 1000 |
 | 28 | Stabilized YAW-          | Clipped between -1000 and 0 |
-| 29 | One                      | Constant value of 500 |
+| 29 | MAX                      | Constant value of 500 |
 
 The `smix reset` command removes all the existing motor mixing rules.
 
diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c
index f1dac159c7..0aefd6ec75 100755
--- a/src/main/flight/servos.c
+++ b/src/main/flight/servos.c
@@ -263,7 +263,7 @@ void servoMixer(float dT)
 
     input[INPUT_FEATURE_FLAPS] = FLIGHT_MODE(FLAPERON) ? servoConfig()->flaperon_throw_offset : 0;
 
-    input[INPUT_LOGIC_ONE] = 500;
+    input[INPUT_MAX] = 500;
 #ifdef USE_LOGIC_CONDITIONS
     input[INPUT_GVAR_0] = constrain(gvGet(0), -1000, 1000);
     input[INPUT_GVAR_1] = constrain(gvGet(1), -1000, 1000);
diff --git a/src/main/flight/servos.h b/src/main/flight/servos.h
index 8745cf239d..a6befa0269 100644
--- a/src/main/flight/servos.h
+++ b/src/main/flight/servos.h
@@ -53,7 +53,7 @@ typedef enum {
     INPUT_STABILIZED_PITCH_MINUS    = 26,
     INPUT_STABILIZED_YAW_PLUS       = 27,
     INPUT_STABILIZED_YAW_MINUS      = 28,
-    INPUT_LOGIC_ONE                 = 29,
+    INPUT_MAX                       = 29,
     INPUT_GVAR_0                    = 30,
     INPUT_GVAR_1                    = 31,
     INPUT_GVAR_2                    = 32,

From 2205f73d236f13d1e7d5b87550e527e2323b84cc Mon Sep 17 00:00:00 2001
From: Jonas Elvedal Hole <jolejonaseh@gmail.com>
Date: Sat, 25 Apr 2020 19:20:41 +0200
Subject: [PATCH 101/179] Add STABILIZED_[YAW, ROLL, PITCH] for logic_condition

---
 src/main/common/logic_condition.c | 15 ++++++++++++++-
 src/main/common/logic_condition.h |  3 +++
 2 files changed, 17 insertions(+), 1 deletion(-)

diff --git a/src/main/common/logic_condition.c b/src/main/common/logic_condition.c
index a4bf5483c0..7ba9dd2f1b 100644
--- a/src/main/common/logic_condition.c
+++ b/src/main/common/logic_condition.c
@@ -40,6 +40,7 @@
 #include "sensors/battery.h"
 #include "sensors/pitotmeter.h"
 #include "flight/imu.h"
+#include "flight/pid.h"
 
 #include "navigation/navigation.h"
 #include "navigation/navigation_private.h"
@@ -330,7 +331,19 @@ static int logicConditionGetFlightOperandValue(int operand) {
 
         case LOGIC_CONDITION_OPERAND_FLIGHT_IS_FAILSAFE: // 0/1
             return (failsafePhase() == FAILSAFE_RX_LOSS_MONITORING) ? 1 : 0;
-            break; 
+            break;
+        
+        case LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_YAW: // 
+            return axisPID[YAW];
+            break;
+        
+        case LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_ROLL: // 
+            return axisPID[ROLL];
+            break;
+        
+        case LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_PITCH: // 
+            return axisPID[PITCH];
+            break;
 
         default:
             return 0;
diff --git a/src/main/common/logic_condition.h b/src/main/common/logic_condition.h
index a158875437..d5eef12d92 100644
--- a/src/main/common/logic_condition.h
+++ b/src/main/common/logic_condition.h
@@ -90,6 +90,9 @@ typedef enum {
     LOGIC_CONDITION_OPERAND_FLIGHT_IS_WP, // 0/1                            // 23
     LOGIC_CONDITION_OPERAND_FLIGHT_IS_LANDING, // 0/1                       // 24
     LOGIC_CONDITION_OPERAND_FLIGHT_IS_FAILSAFE, // 0/1                      // 25
+    LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_YAW,                          // 26
+    LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_ROLL,                         // 27
+    LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_PITCH,                        // 28
 } logicFlightOperands_e;
 
 typedef enum {

From b7db025d43a1e9e16f2d88e27f4b3749b3480655 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Sat, 25 Apr 2020 20:57:35 +0200
Subject: [PATCH 102/179] Allow to change VTX band and channel via global
 functions

---
 docs/Global Functions.md           |  2 ++
 src/main/common/global_functions.c | 16 ++++++++++++++++
 src/main/common/global_functions.h | 18 ++++++++++--------
 3 files changed, 28 insertions(+), 8 deletions(-)

diff --git a/docs/Global Functions.md b/docs/Global Functions.md
index 53910caa85..f74d772b65 100644
--- a/docs/Global Functions.md	
+++ b/docs/Global Functions.md	
@@ -26,6 +26,8 @@ _Global Functions_ (abbr. GF) are a mechanism allowing to override certain fligh
 | 5             | INVERT_PITCH                  | Inverts PITCH axis input for PID/PIFF controller  |
 | 6             | INVERT_YAW                    | Inverts YAW axis input for PID/PIFF controller |
 | 7             | OVERRIDE_THROTTLE             | Override throttle value that is fed to the motors by mixer. Operand is scaled in us. `1000` means throttle cut, `1500` means half throttle |
+| 8             | SET_VTX_BAND           | Sets VTX band. Accepted values are `1-5` |
+| 8             | SET_VTX_CHANNEL           | Sets VTX channel. Accepted values are `1-8` |
 
 ## Flags
 
diff --git a/src/main/common/global_functions.c b/src/main/common/global_functions.c
index b4033479e9..5e12bf9761 100644
--- a/src/main/common/global_functions.c
+++ b/src/main/common/global_functions.c
@@ -98,6 +98,22 @@ void globalFunctionsProcess(int8_t functionId) {
                     }
                 }
                 break;
+            case GLOBAL_FUNCTION_ACTION_SET_VTX_BAND:
+                if (conditionValue && !previousValue) {
+                    vtxDeviceCapability_t vtxDeviceCapability;
+                    if (vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) {
+                        vtxSettingsConfigMutable()->band = constrain(globalFunctionsStates[functionId].value, VTX_SETTINGS_MIN_BAND, VTX_SETTINGS_MAX_BAND);
+                    }
+                }
+                break;
+            case GLOBAL_FUNCTION_ACTION_SET_VTX_CHANNEL:
+                if (conditionValue && !previousValue) {
+                    vtxDeviceCapability_t vtxDeviceCapability;
+                    if (vtxCommonGetDeviceCapability(vtxCommonDevice(), &vtxDeviceCapability)) {
+                        vtxSettingsConfigMutable()->channel = constrain(globalFunctionsStates[functionId].value, VTX_SETTINGS_MIN_CHANNEL, VTX_SETTINGS_MAX_CHANNEL);
+                    }
+                }
+                break;
             case GLOBAL_FUNCTION_ACTION_INVERT_ROLL:
                 if (conditionValue) {
                     GLOBAL_FUNCTION_FLAG_ENABLE(GLOBAL_FUNCTION_FLAG_OVERRIDE_INVERT_ROLL);
diff --git a/src/main/common/global_functions.h b/src/main/common/global_functions.h
index d31157a310..f8e0128eab 100644
--- a/src/main/common/global_functions.h
+++ b/src/main/common/global_functions.h
@@ -29,14 +29,16 @@
 #define MAX_GLOBAL_FUNCTIONS 8
 
 typedef enum {
-    GLOBAL_FUNCTION_ACTION_OVERRIDE_ARMING_SAFETY = 0,
-    GLOBAL_FUNCTION_ACTION_OVERRIDE_THROTTLE_SCALE,
-    GLOBAL_FUNCTION_ACTION_SWAP_ROLL_YAW,
-    GLOBAL_FUNCTION_ACTION_SET_VTX_POWER_LEVEL,
-    GLOBAL_FUNCTION_ACTION_INVERT_ROLL,
-    GLOBAL_FUNCTION_ACTION_INVERT_PITCH,
-    GLOBAL_FUNCTION_ACTION_INVERT_YAW,
-    GLOBAL_FUNCTION_ACTION_OVERRIDE_THROTTLE,
+    GLOBAL_FUNCTION_ACTION_OVERRIDE_ARMING_SAFETY = 0,      // 0
+    GLOBAL_FUNCTION_ACTION_OVERRIDE_THROTTLE_SCALE,         // 1
+    GLOBAL_FUNCTION_ACTION_SWAP_ROLL_YAW,                   // 2
+    GLOBAL_FUNCTION_ACTION_SET_VTX_POWER_LEVEL,             // 3
+    GLOBAL_FUNCTION_ACTION_INVERT_ROLL,                     // 4
+    GLOBAL_FUNCTION_ACTION_INVERT_PITCH,                    // 5
+    GLOBAL_FUNCTION_ACTION_INVERT_YAW,                      // 6
+    GLOBAL_FUNCTION_ACTION_OVERRIDE_THROTTLE,               // 7
+    GLOBAL_FUNCTION_ACTION_SET_VTX_BAND,                    // 8
+    GLOBAL_FUNCTION_ACTION_SET_VTX_CHANNEL,                 // 9
     GLOBAL_FUNCTION_ACTION_LAST
 } globalFunctionActions_e;
 

From a2893030ad6c07b7b5437cd52cd2895043d35008 Mon Sep 17 00:00:00 2001
From: Stefan Naewe <snaewe@users.noreply.github.com>
Date: Thu, 19 Mar 2020 10:22:51 +0100
Subject: [PATCH 103/179] Document telemetry sensor 0x450

Signed-off-by: Stefan Naewe <stefan.naewe@gmail.com>
---
 docs/Telemetry.md | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/docs/Telemetry.md b/docs/Telemetry.md
index 72c593c7e7..8c8b406ccf 100644
--- a/docs/Telemetry.md
+++ b/docs/Telemetry.md
@@ -120,7 +120,7 @@ The following sensors are transmitted
 * **0420** : distance to GPS home fix, in meters
 * **0430** : if `frsky_pitch_roll = ON` set this will be pitch degrees*10
 * **0440** : if `frsky_pitch_roll = ON` set this will be roll degrees*10
-
+* **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.

From a2c4823470c53e719fd1c143af5a341061e459d8 Mon Sep 17 00:00:00 2001
From: Manoel Brunnen <manoel.brunnen@gmail.com>
Date: Sun, 19 Apr 2020 09:14:52 +0200
Subject: [PATCH 104/179] Enable DSHOT and ESC sensor for OMNIBUSF7

---
 src/main/target/OMNIBUSF7/target.h | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/src/main/target/OMNIBUSF7/target.h b/src/main/target/OMNIBUSF7/target.h
index c3a7262078..9945f82f8a 100644
--- a/src/main/target/OMNIBUSF7/target.h
+++ b/src/main/target/OMNIBUSF7/target.h
@@ -202,6 +202,8 @@
 // Number of available PWM outputs
 #define MAX_PWM_OUTPUT_PORTS    4
 #define TARGET_MOTOR_COUNT      4
+#define USE_DSHOT
+#define USE_ESC_SENSOR
 
 #define TARGET_IO_PORTA 0xffff
 #define TARGET_IO_PORTB 0xffff
@@ -209,4 +211,4 @@
 #define TARGET_IO_PORTD 0xffff
 #define TARGET_IO_PORTE 0xffff
 
-#define PCA9685_I2C_BUS         BUS_I2C2
\ No newline at end of file
+#define PCA9685_I2C_BUS         BUS_I2C2

From 086b2bdc9c8c4dd43a1b2f0f4790256744de037c Mon Sep 17 00:00:00 2001
From: joleeee <jolejonaseh@gmail.com>
Date: Sat, 25 Apr 2020 23:25:23 +0200
Subject: [PATCH 105/179] Reorder to ROLL, PITCH, YAW

---
 src/main/common/logic_condition.h | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/main/common/logic_condition.h b/src/main/common/logic_condition.h
index d5eef12d92..f48383870a 100644
--- a/src/main/common/logic_condition.h
+++ b/src/main/common/logic_condition.h
@@ -90,9 +90,9 @@ typedef enum {
     LOGIC_CONDITION_OPERAND_FLIGHT_IS_WP, // 0/1                            // 23
     LOGIC_CONDITION_OPERAND_FLIGHT_IS_LANDING, // 0/1                       // 24
     LOGIC_CONDITION_OPERAND_FLIGHT_IS_FAILSAFE, // 0/1                      // 25
-    LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_YAW,                          // 26
-    LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_ROLL,                         // 27
-    LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_PITCH,                        // 28
+    LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_ROLL,                         // 26
+    LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_PITCH,                        // 27
+    LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_YAW,                          // 28
 } logicFlightOperands_e;
 
 typedef enum {
@@ -138,4 +138,4 @@ int logicConditionGetOperandValue(logicOperandType_e type, int operand);
 
 int logicConditionGetValue(int8_t conditionId);
 void logicConditionUpdateTask(timeUs_t currentTimeUs);
-void logicConditionReset(void);
\ No newline at end of file
+void logicConditionReset(void);

From d9573715502824908862b4a83f218a5876a95092 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Alberto=20Garci=CC=81a=20Hierro?= <alberto@garciahierro.com>
Date: Sun, 26 Apr 2020 21:03:43 +0100
Subject: [PATCH 106/179] [CI] Cleanup intermediate files after building each
 target

This reduces the amount of required disk space, so we don't hit
the GH CI limits. Note that hex files are kept until the end of
the run for uploading the binaries to an artifact, but they're
quite small compared to all the intermediate data.
---
 .github/workflows/ci.yml   |  2 +-
 src/utils/build-targets.sh | 16 ++++++++++------
 2 files changed, 11 insertions(+), 7 deletions(-)

diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml
index 6fefa21e29..35bfbdd88e 100644
--- a/.github/workflows/ci.yml
+++ b/.github/workflows/ci.yml
@@ -51,7 +51,7 @@ jobs:
         uses: actions/upload-artifact@v2-preview
         with:
           name: ${{ env.BUILD_NAME }}.zip
-          path: ./obj/*.hex
+          path: ./obj/dist/*.hex
 
   test:
     needs: [build]
diff --git a/src/utils/build-targets.sh b/src/utils/build-targets.sh
index 49f4360272..f8633819bf 100755
--- a/src/utils/build-targets.sh
+++ b/src/utils/build-targets.sh
@@ -55,12 +55,16 @@ END_IDX=$(expr ${START} + ${COUNT})
 SELECTED_TARGETS=$(echo ${ALL_TARGETS} | cut -d ' ' -f ${START_IDX}-${END_IDX})
 
 if [ -z "${DRY_RUN}" ]; then
-    # make without arguments builds the default
-    # target, so make sure ${SELECTED_TARGETS} is
-    # not empty
-    if [ -n "${SELECTED_TARGETS}" ]; then
-        BUILD_SUFFIX=${BUILD_SUFFIX} V=0 CFLAGS=-Werror make ${SELECTED_TARGETS}
-    fi
+    for target in ${SELECTED_TARGETS}; do
+        BUILD_SUFFIX=${BUILD_SUFFIX} V=0 CFLAGS=-Werror make ${target}
+        # Cleanup intermediate files after building each target.
+        # Otherwise we run out of space on GH CI.
+        # XXX: Make sure we save the .hex file for artifact
+        # generation
+        mkdir -p obj/dist
+        mv obj/*.hex obj/dist
+        V=0 make clean_${target}
+    done
 else
     echo "${SELECTED_TARGETS}"
 fi

From 31fffc05a8596347b5f78ce605d2d13746428aad Mon Sep 17 00:00:00 2001
From: "Konstantin (DigitalEntity) Sharlaimov"
 <konstantin.sharlaimov@gmail.com>
Date: Sun, 26 Apr 2020 17:33:50 +0200
Subject: [PATCH 107/179] [TARGET] Add AIRBOTF7 target

---
 src/main/target/AIRBOTF7/target.c  |  50 ++++++++
 src/main/target/AIRBOTF7/target.h  | 183 +++++++++++++++++++++++++++++
 src/main/target/AIRBOTF7/target.mk |  16 +++
 3 files changed, 249 insertions(+)
 create mode 100644 src/main/target/AIRBOTF7/target.c
 create mode 100644 src/main/target/AIRBOTF7/target.h
 create mode 100644 src/main/target/AIRBOTF7/target.mk

diff --git a/src/main/target/AIRBOTF7/target.c b/src/main/target/AIRBOTF7/target.c
new file mode 100644
index 0000000000..aad08be85e
--- /dev/null
+++ b/src/main/target/AIRBOTF7/target.c
@@ -0,0 +1,50 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute 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.
+ *
+ * This file 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 this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#include <stdint.h>
+
+#include <platform.h>
+#include "drivers/io.h"
+#include "drivers/pwm_mapping.h"
+#include "drivers/timer.h"
+#include "drivers/bus.h"
+
+// Board hardware definitions
+BUSDEV_REGISTER_SPI_TAG(busdev_imu0_mpu6000,    DEVHW_MPU6000,  GYRO_0_SPI_BUS,  GYRO_0_CS_PIN,   GYRO_0_EXTI_PIN,     0,  DEVFLAGS_NONE);
+BUSDEV_REGISTER_SPI_TAG(busdev_imu0_mpu6500,    DEVHW_MPU6500,  GYRO_0_SPI_BUS,  GYRO_0_CS_PIN,   GYRO_0_EXTI_PIN,     0,  DEVFLAGS_NONE);
+
+BUSDEV_REGISTER_SPI_TAG(busdev_imu1_mpu6000,    DEVHW_MPU6000,  GYRO_1_SPI_BUS,  GYRO_1_CS_PIN,   GYRO_1_EXTI_PIN,     1,  DEVFLAGS_NONE);
+BUSDEV_REGISTER_SPI_TAG(busdev_imu1_mpu6500,    DEVHW_MPU6500,  GYRO_1_SPI_BUS,  GYRO_1_CS_PIN,   GYRO_1_EXTI_PIN,     1,  DEVFLAGS_NONE);
+
+const timerHardware_t timerHardware[] = {
+    DEF_TIM(TIM2,  CH1, PA15, TIM_USE_LED,              0, 0), // LED
+    DEF_TIM(TIM1,  CH1, PA8,  TIM_USE_ANY,              0, 0), // Cam control, SS, UNUSED
+
+    DEF_TIM(TIM8,  CH3, PC8,  TIM_USE_MC_MOTOR,         0, 0), //S1
+    DEF_TIM(TIM4,  CH1, PB6,  TIM_USE_MC_MOTOR,         0, 0), //S2
+    DEF_TIM(TIM8,  CH4, PC9,  TIM_USE_MC_MOTOR,         0, 0), //S3
+    DEF_TIM(TIM4,  CH2, PB7,  TIM_USE_MC_MOTOR,         0, 0), //S4
+};
+
+const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);
diff --git a/src/main/target/AIRBOTF7/target.h b/src/main/target/AIRBOTF7/target.h
new file mode 100644
index 0000000000..ed7275dcc4
--- /dev/null
+++ b/src/main/target/AIRBOTF7/target.h
@@ -0,0 +1,183 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute 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.
+ *
+ * This file 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 this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#pragma once
+
+#define TARGET_BOARD_IDENTIFIER "ABF7"
+#define USBD_PRODUCT_STRING     "Airbot F7"
+
+// ************** Beeper and LED ******************
+#define LED0                    PB12
+#define BEEPER                  PB0
+#define BEEPER_INVERTED
+
+// *************** SPI & I2C **********************
+#define USE_SPI
+#define USE_I2C
+
+#define USE_SPI_DEVICE_3
+#define SPI3_SCK_PIN            PB3
+#define SPI3_MISO_PIN           PB4
+#define SPI3_MOSI_PIN           PB5
+
+#define USE_SPI_DEVICE_2
+#define SPI2_SCK_PIN            PB13
+#define SPI2_MISO_PIN           PB14
+#define SPI2_MOSI_PIN           PB15
+
+#define USE_SPI_DEVICE_1
+#define SPI1_SCK_PIN            PA5
+#define SPI1_MISO_PIN           PA6
+#define SPI1_MOSI_PIN           PA7
+
+#define USE_I2C_DEVICE_1
+#define I2C1_SCL                PB8
+#define I2C1_SDA                PB9 
+
+// *************** Gyro & ACC **********************
+#define USE_TARGET_IMU_HARDWARE_DESCRIPTORS
+#define USE_DUAL_GYRO
+
+#define USE_ACC
+#define USE_GYRO
+
+#define USE_ACC_MPU6000
+#define USE_ACC_MPU6500
+#define USE_GYRO_MPU6000
+#define USE_GYRO_MPU6500
+
+#define GYRO_0_CS_PIN           PD2
+#define GYRO_0_SPI_BUS          BUS_SPI3
+#define GYRO_0_EXTI_PIN         NONE
+#define GYRO_0_ALIGN            CW90_DEG    // This doesn't work yet, requires BUS refactoring
+
+#define GYRO_1_CS_PIN           PC4
+#define GYRO_1_SPI_BUS          BUS_SPI1
+#define GYRO_1_EXTI_PIN         NONE
+#define GYRO_1_ALIGN            CW0_DEG     // This doesn't work yet, requires BUS refactoring
+
+// TODO: Remove this once per-gyro alignment is supported correctly
+#define GYRO_MPU6500_ALIGN      CW90_DEG
+#define ACC_MPU6500_ALIGN       CW90_DEG
+
+// *************** OSD *****************************
+#define USE_OSD
+#define USE_MAX7456
+#define MAX7456_SPI_BUS         BUS_SPI2
+#define MAX7456_CS_PIN          PC15
+
+// *************** FLASH **************************
+#define USE_FLASHFS
+#define USE_FLASH_M25P16
+#define M25P16_CS_PIN           PA3
+#define M25P16_SPI_BUS          BUS_SPI1
+#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
+
+// *************** I2C defvices **********************
+#define USE_BARO
+#define BARO_I2C_BUS            BUS_I2C1
+#define USE_BARO_BMP280
+
+#define USE_MAG
+#define MAG_I2C_BUS             BUS_I2C1
+#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
+
+// *************** UART *****************************
+#define USE_VCP
+
+#define USE_UART1
+#define UART1_RX_PIN            PA10
+#define UART1_TX_PIN            PA9
+
+#define USE_UART2
+#define UART2_TX_PIN            PA2
+#define UART2_RX_PIN            NONE
+
+#define USE_UART3
+#define UART3_RX_PIN            PC11
+#define UART3_TX_PIN            PC10
+
+#define USE_UART4
+#define UART4_RX_PIN            PA1
+#define UART4_TX_PIN            PA0
+
+#define USE_UART5
+#define UART5_TX_PIN            PC12
+#define UART5_RX_PIN            NONE
+
+#define USE_UART6
+#define UART6_RX_PIN            PC7
+#define UART6_TX_PIN            PC6
+
+#define USE_SOFTSERIAL1
+#define SOFTSERIAL_1_TX_PIN     PA5
+#define SOFTSERIAL_1_RX_PIN     PA5
+
+#define SERIAL_PORT_COUNT       8
+
+// *************** ADC *****************************
+#define USE_ADC
+#define ADC_INSTANCE                ADC1
+#define ADC_CHANNEL_1_PIN           PC0
+#define ADC_CHANNEL_2_PIN           PC2
+#define ADC_CHANNEL_3_PIN           PC1
+
+#define VBAT_ADC_CHANNEL            ADC_CHN_1
+#define CURRENT_METER_ADC_CHANNEL   ADC_CHN_2
+#define RSSI_ADC_CHANNEL            ADC_CHN_3
+
+// ************** Other features *******************
+#define USE_LED_STRIP
+#define WS2811_PIN                  PA15
+
+#define DEFAULT_FEATURES            (FEATURE_TX_PROF_SEL | FEATURE_TELEMETRY | FEATURE_VBAT | FEATURE_OSD | FEATURE_BLACKBOX)
+
+#define DEFAULT_RX_TYPE             RX_TYPE_SERIAL
+#define SERIALRX_PROVIDER           SERIALRX_SBUS
+#define SERIALRX_UART               SERIAL_PORT_USART1
+
+#define USE_SERIAL_4WAY_BLHELI_INTERFACE
+
+#define MAX_PWM_OUTPUT_PORTS        4
+#define TARGET_MOTOR_COUNT          4
+
+#define TARGET_IO_PORTA         0xffff
+#define TARGET_IO_PORTB         0xffff
+#define TARGET_IO_PORTC         0xffff
+#define TARGET_IO_PORTD         0xffff
+
+#define USE_DSHOT
+#define USE_ESC_SENSOR
diff --git a/src/main/target/AIRBOTF7/target.mk b/src/main/target/AIRBOTF7/target.mk
new file mode 100644
index 0000000000..24cc94224d
--- /dev/null
+++ b/src/main/target/AIRBOTF7/target.mk
@@ -0,0 +1,16 @@
+F7X2RE_TARGETS += $(TARGET)
+FEATURES       += VCP ONBOARDFLASH MSC
+
+TARGET_SRC = \
+            drivers/accgyro/accgyro_mpu6500.c \
+            drivers/accgyro/accgyro_mpu6000.c \
+            drivers/barometer/barometer_bmp280.c \
+            drivers/barometer/barometer_ms56xx.c \
+            drivers/compass/compass_hmc5883l.c \
+            drivers/compass/compass_mag3110.c \
+            drivers/compass/compass_qmc5883l.c \
+            drivers/compass/compass_ist8310.c \
+            drivers/compass/compass_ist8308.c \
+            drivers/compass/compass_lis3mdl.c \
+            drivers/light_ws2811strip.c \
+            drivers/max7456.c

From f002b4e449a51db200923b1f2baf4f5bac0ad06e Mon Sep 17 00:00:00 2001
From: "Konstantin (DigitalEntity) Sharlaimov"
 <konstantin.sharlaimov@gmail.com>
Date: Mon, 27 Apr 2020 11:00:31 +0200
Subject: [PATCH 108/179] Add AIRBOTF7 to release targets

---
 make/release.mk | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/make/release.mk b/make/release.mk
index bc7c18e83e..74e5dde8f6 100644
--- a/make/release.mk
+++ b/make/release.mk
@@ -10,7 +10,7 @@ RELEASE_TARGETS += KAKUTEF4 KAKUTEF4V2 KAKUTEF7 KAKUTEF7MINI KAKUTEF7HDV
 
 RELEASE_TARGETS += SPRACINGF3 SPRACINGF3EVO SPRACINGF3EVO_1SS SPRACINGF3MINI SPRACINGF4EVO SPRACINGF7DUAL
 
-RELEASE_TARGETS += OMNIBUS AIRBOTF4 ASGARD32F4 ASGARD32F7 FIREWORKSV2
+RELEASE_TARGETS += OMNIBUS AIRBOTF4 ASGARD32F4 ASGARD32F7 FIREWORKSV2 AIRBOTF7
 RELEASE_TARGETS += OMNIBUSF4 OMNIBUSF4PRO OMNIBUSF4PRO_LEDSTRIPM5 DYSF4PRO DYSF4PROV2
 RELEASE_TARGETS += OMNIBUSF4V3 OMNIBUSF4V3_S6_SS OMNIBUSF4V3_S5S6_SS OMNIBUSF4V3_S5_S6_2SS
 RELEASE_TARGETS += OMNIBUSF7 OMNIBUSF7V2 OMNIBUSF7NXT YUPIF7

From 0551f8743a9a48c05c2d5faee44fa89c238aaead Mon Sep 17 00:00:00 2001
From: Michel Pastor <shellixyz@users.noreply.github.com>
Date: Mon, 27 Apr 2020 13:23:13 +0200
Subject: [PATCH 109/179] Add flash partitioning support (#5474)

---
 make/source.mk                                |   1 +
 src/main/cms/cms_menu_blackbox.c              |   3 +-
 src/main/drivers/flash.c                      | 288 ++++++++++++++++++
 src/main/drivers/flash.h                      |  95 +++++-
 src/main/fc/cli.c                             |  31 +-
 src/main/fc/fc_init.c                         |  14 +-
 src/main/fc/fc_msp.c                          |   5 +-
 src/main/io/flashfs.c                         |  45 ++-
 src/main/io/flashfs.h                         |   1 -
 src/main/target/BLUEJAYF4/hardware_revision.c |   2 +-
 10 files changed, 432 insertions(+), 53 deletions(-)
 create mode 100644 src/main/drivers/flash.c

diff --git a/make/source.mk b/make/source.mk
index 565c806c2b..aad3966d1d 100644
--- a/make/source.mk
+++ b/make/source.mk
@@ -236,6 +236,7 @@ TARGET_SRC   := $(filter-out $(MCU_EXCLUDES), $(TARGET_SRC))
 
 ifneq ($(filter ONBOARDFLASH,$(FEATURES)),)
 TARGET_SRC += \
+            drivers/flash.c \
             drivers/flash_m25p16.c \
             io/flashfs.c \
             $(MSC_SRC)
diff --git a/src/main/cms/cms_menu_blackbox.c b/src/main/cms/cms_menu_blackbox.c
index 6fdaae6954..d16a3c79fc 100644
--- a/src/main/cms/cms_menu_blackbox.c
+++ b/src/main/cms/cms_menu_blackbox.c
@@ -36,6 +36,7 @@
 
 #include "config/feature.h"
 
+#include "drivers/flash.h"
 #include "drivers/time.h"
 
 #include "fc/config.h"
@@ -53,7 +54,7 @@ static long cmsx_EraseFlash(displayPort_t *pDisplay, const void *ptr)
     displayResync(pDisplay); // Was max7456RefreshAll(); Why at this timing?
 
     flashfsEraseCompletely();
-    while (!flashfsIsReady()) {
+    while (!flashIsReady()) {
         delay(100);
     }
 
diff --git a/src/main/drivers/flash.c b/src/main/drivers/flash.c
new file mode 100644
index 0000000000..13a4a114e2
--- /dev/null
+++ b/src/main/drivers/flash.c
@@ -0,0 +1,288 @@
+/*
+ * This file is part of iNav.
+ *
+ * iNav is free software. You can redistribute
+ * this software and/or modify this software 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 this software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <string.h>
+
+#include "platform.h"
+
+#include "build/debug.h"
+
+#ifdef USE_FLASHFS
+
+#include "flash.h"
+#include "flash_m25p16.h"
+#include "drivers/bus_spi.h"
+#include "drivers/io.h"
+#include "drivers/time.h"
+
+static flashPartitionTable_t flashPartitionTable;
+static int flashPartitions = 0;
+
+#ifdef USE_SPI
+static bool flashSpiInit(void)
+{
+#ifdef USE_FLASH_M25P16
+    return m25p16_init(0);
+#endif
+    return false;
+}
+#endif // USE_SPI
+
+bool flashDeviceInit(void)
+{
+#ifdef USE_SPI
+    return flashSpiInit();
+#endif
+
+    return false;
+}
+
+bool flashIsReady(void)
+{
+#ifdef USE_FLASH_M25P16
+    return m25p16_isReady();
+#endif
+    return false;
+}
+
+bool flashWaitForReady(void)
+{
+#ifdef USE_FLASH_M25P16
+    return m25p16_waitForReady(10);
+#endif
+    return false;
+}
+
+void flashEraseSector(uint32_t address)
+{
+#ifdef USE_FLASH_M25P16
+    return m25p16_eraseSector(address);
+#endif
+}
+
+void flashEraseCompletely(void)
+{
+#ifdef USE_FLASH_M25P16
+    return m25p16_eraseCompletely();
+#endif
+}
+
+#if 0
+void flashPageProgramBegin(uint32_t address)
+{
+#ifdef USE_FLASH_M25P16
+    return m25p16_pageProgramBegin(address);
+#endif
+}
+
+void flashPageProgramContinue(const uint8_t *data, int length)
+{
+#ifdef USE_FLASH_M25P16
+    return m25p16_pageProgramContinue(data, length);
+#endif
+}
+
+void flashPageProgramFinish(void)
+{
+#ifdef USE_FLASH_M25P16
+    return m25p16_pageProgramFinish();
+#endif
+}
+#endif
+
+uint32_t flashPageProgram(uint32_t address, const uint8_t *data, int length)
+{
+#ifdef USE_FLASH_M25P16
+    return m25p16_pageProgram(address, data, length);
+#endif
+}
+
+int flashReadBytes(uint32_t address, uint8_t *buffer, int length)
+{
+#ifdef USE_FLASH_M25P16
+    return m25p16_readBytes(address, buffer, length);
+#endif
+    return 0;
+}
+
+void flashFlush(void)
+{
+}
+
+const flashGeometry_t *flashGetGeometry(void)
+{
+#ifdef USE_FLASH_M25P16
+    return m25p16_getGeometry();
+#endif
+
+    return NULL;
+}
+
+/*
+ * Flash partitioning
+ *
+ * Partition table is not currently stored on the flash, in-memory only.
+ *
+ * Partitions are required so that Badblock management (inc spare blocks), FlashFS (Blackbox Logging), Configuration and Firmware can be kept separate and tracked.
+ *
+ * XXX FIXME
+ * XXX Note that Flash FS must start at sector 0.
+ * XXX There is existing blackbox/flash FS code the relies on this!!!
+ * XXX This restriction can and will be fixed by creating a set of flash operation functions that take partition as an additional parameter.
+ */
+
+static __attribute__((unused)) void createPartition(flashPartitionType_e type, uint32_t size, flashSector_t *endSector)
+{
+    const flashGeometry_t *flashGeometry = flashGetGeometry();
+    flashSector_t partitionSectors = (size / flashGeometry->sectorSize);
+
+    if (size % flashGeometry->sectorSize > 0) {
+        partitionSectors++; // needs a portion of a sector.
+    }
+
+    flashSector_t startSector = (*endSector + 1) - partitionSectors; // + 1 for inclusive
+
+    flashPartitionSet(type, startSector, *endSector);
+
+    *endSector = startSector - 1;
+}
+
+static void flashConfigurePartitions(void)
+{
+    const flashGeometry_t *flashGeometry = flashGetGeometry();
+    if (flashGeometry->totalSize == 0) {
+        return;
+    }
+
+    flashSector_t startSector = 0;
+    flashSector_t endSector = flashGeometry->sectors - 1; // 0 based index
+
+    const flashPartition_t *badBlockPartition = flashPartitionFindByType(FLASH_PARTITION_TYPE_BADBLOCK_MANAGEMENT);
+    if (badBlockPartition) {
+        endSector = badBlockPartition->startSector - 1;
+    }
+
+#if defined(FIRMWARE_SIZE)
+    createPartition(FLASH_PARTITION_TYPE_FIRMWARE, FIRMWARE_SIZE*1024, &endSector);
+#endif
+
+#if defined(MSP_FIRMWARE_UPDATE)
+    createPartition(FLASH_PARTITION_TYPE_FIRMWARE_UPDATE_META, flashGeometry->sectorSize, &endSector);
+    createPartition(FLASH_PARTITION_TYPE_UPDATE_FIRMWARE, FLASH_SIZE*1024, &endSector);
+    createPartition(FLASH_PARTITION_TYPE_FULL_BACKUP, FLASH_SIZE*1024, &endSector);
+#endif
+
+#if defined(CONFIG_IN_EXTERNAL_FLASH)
+    createPartition(FLASH_PARTITION_TYPE_CONFIG, EEPROM_SIZE, &endSector);
+#endif
+
+#ifdef USE_FLASHFS
+    flashPartitionSet(FLASH_PARTITION_TYPE_FLASHFS, startSector, endSector);
+#endif
+}
+
+flashPartition_t *flashPartitionFindByType(uint8_t type)
+{
+    for (int index = 0; index < FLASH_MAX_PARTITIONS; index++) {
+        flashPartition_t *candidate = &flashPartitionTable.partitions[index];
+        if (candidate->type == type) {
+            return candidate;
+        }
+    }
+
+    return NULL;
+}
+
+const flashPartition_t *flashPartitionFindByIndex(uint8_t index)
+{
+    if (index >= flashPartitions) {
+        return NULL;
+    }
+
+    return &flashPartitionTable.partitions[index];
+}
+
+void flashPartitionSet(uint8_t type, uint32_t startSector, uint32_t endSector)
+{
+    flashPartition_t *entry = flashPartitionFindByType(type);
+
+    if (!entry) {
+        if (flashPartitions == FLASH_MAX_PARTITIONS - 1) {
+            return;
+        }
+        entry = &flashPartitionTable.partitions[flashPartitions++];
+    }
+
+    entry->type = type;
+    entry->startSector = startSector;
+    entry->endSector = endSector;
+}
+
+// Must be in sync with FLASH_PARTITION_TYPE
+static const char *flashPartitionNames[] = {
+    "UNKNOWN  ",
+    "PARTITION",
+    "FLASHFS  ",
+    "BBMGMT   ",
+    "FIRMWARE ",
+    "CONFIG   ",
+    "FW UPDT  ",
+};
+
+const char *flashPartitionGetTypeName(flashPartitionType_e type)
+{
+    if (type < ARRAYLEN(flashPartitionNames)) {
+        return flashPartitionNames[type];
+    }
+
+    return NULL;
+}
+
+bool flashInit(void)
+{
+    memset(&flashPartitionTable, 0x00, sizeof(flashPartitionTable));
+
+    bool haveFlash = flashDeviceInit();
+
+    flashConfigurePartitions();
+
+    return haveFlash;
+}
+
+int flashPartitionCount(void)
+{
+    return flashPartitions;
+}
+
+uint32_t flashPartitionSize(flashPartition_t *partition)
+{
+    const flashGeometry_t * const geometry = flashGetGeometry();
+    return (partition->endSector - partition->startSector + 1) * geometry->sectorSize;
+}
+
+void flashPartitionErase(flashPartition_t *partition)
+{
+    for (uint16_t sector = partition->startSector; sector <= partition->endSector; ++sector) {
+        flashEraseSector(sector);
+    }
+}
+#endif // USE_FLASH_CHIP
diff --git a/src/main/drivers/flash.h b/src/main/drivers/flash.h
index 21216825c1..119b391baa 100644
--- a/src/main/drivers/flash.h
+++ b/src/main/drivers/flash.h
@@ -1,31 +1,94 @@
 /*
- * This file is part of Cleanflight.
+ * This file is part of iNav.
  *
- * Cleanflight 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 free software. You can redistribute
+ * this software and/or modify this software 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.
  *
- * Cleanflight 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.
+ * 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 Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ * along with this software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
  */
 
 #pragma once
 
 #include <stdint.h>
 
+//#include "drivers/io.h"
+
+//#ifdef USE_FLASHFS
+
+typedef uint16_t flashSector_t;
+
 typedef struct flashGeometry_s {
-    uint16_t sectors; // Count of the number of erasable blocks on the device
-
-    uint16_t pagesPerSector;
-    const uint16_t pageSize; // In bytes
-
+    flashSector_t sectors; // Count of the number of erasable blocks on the device
+    uint16_t pageSize; // In bytes
     uint32_t sectorSize; // This is just pagesPerSector * pageSize
-
     uint32_t totalSize;  // This is just sectorSize * sectors
+    uint16_t pagesPerSector;
 } flashGeometry_t;
+
+bool flashInit(void);
+
+bool flashIsReady(void);
+bool flashWaitForReady(void);
+void flashEraseSector(uint32_t address);
+void flashEraseCompletely(void);
+#if 0
+void flashPageProgramBegin(uint32_t address);
+void flashPageProgramContinue(const uint8_t *data, int length);
+void flashPageProgramFinish(void);
+#endif
+uint32_t flashPageProgram(uint32_t address, const uint8_t *data, int length);
+int flashReadBytes(uint32_t address, uint8_t *buffer, int length);
+void flashFlush(void);
+const flashGeometry_t *flashGetGeometry(void);
+
+//
+// flash partitioning api
+//
+
+typedef struct flashPartition_s {
+    uint8_t type;
+    flashSector_t startSector;
+    flashSector_t endSector;
+} flashPartition_t;
+
+#define FLASH_PARTITION_SECTOR_COUNT(partition) (partition->endSector + 1 - partition->startSector) // + 1 for inclusive, start and end sector can be the same sector.
+
+// Must be in sync with flashPartitionTypeNames[]
+// Should not be deleted or reordered once the code is writing a table to a flash.
+typedef enum {
+    FLASH_PARTITION_TYPE_UNKNOWN = 0,
+    FLASH_PARTITION_TYPE_PARTITION_TABLE,
+    FLASH_PARTITION_TYPE_FLASHFS,
+    FLASH_PARTITION_TYPE_BADBLOCK_MANAGEMENT,
+    FLASH_PARTITION_TYPE_FIRMWARE,
+    FLASH_PARTITION_TYPE_CONFIG,
+    FLASH_PARTITION_TYPE_FULL_BACKUP,
+    FLASH_PARTITION_TYPE_FIRMWARE_UPDATE_META,
+    FLASH_PARTITION_TYPE_UPDATE_FIRMWARE,
+    FLASH_MAX_PARTITIONS
+} flashPartitionType_e;
+
+typedef struct flashPartitionTable_s {
+    flashPartition_t partitions[FLASH_MAX_PARTITIONS];
+} flashPartitionTable_t;
+
+void flashPartitionSet(uint8_t index, uint32_t startSector, uint32_t endSector);
+flashPartition_t *flashPartitionFindByType(flashPartitionType_e type);
+const flashPartition_t *flashPartitionFindByIndex(uint8_t index);
+const char *flashPartitionGetTypeName(flashPartitionType_e type);
+int flashPartitionCount(void);
+uint32_t flashPartitionSize(flashPartition_t *partition);
+void flashPartitionErase(flashPartition_t *partition);
+
+//#endif [> USE_FLASHFS <]
diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c
index a08b000a97..21f4762593 100755
--- a/src/main/fc/cli.c
+++ b/src/main/fc/cli.c
@@ -56,6 +56,7 @@ extern uint8_t __config_end;
 #include "drivers/buf_writer.h"
 #include "drivers/bus_i2c.h"
 #include "drivers/compass/compass.h"
+#include "drivers/flash.h"
 #include "drivers/io.h"
 #include "drivers/io_impl.h"
 #include "drivers/osd_symbols.h"
@@ -2115,12 +2116,32 @@ static void cliSdInfo(char *cmdline)
 
 static void cliFlashInfo(char *cmdline)
 {
-    const flashGeometry_t *layout = flashfsGetGeometry();
-
     UNUSED(cmdline);
 
-    cliPrintLinef("Flash sectors=%u, sectorSize=%u, pagesPerSector=%u, pageSize=%u, totalSize=%u, usedSize=%u",
-            layout->sectors, layout->sectorSize, layout->pagesPerSector, layout->pageSize, layout->totalSize, flashfsGetOffset());
+    const flashGeometry_t *layout = flashGetGeometry();
+
+    cliPrintLinef("Flash sectors=%u, sectorSize=%u, pagesPerSector=%u, pageSize=%u, totalSize=%u",
+            layout->sectors, layout->sectorSize, layout->pagesPerSector, layout->pageSize, layout->totalSize);
+
+    for (uint8_t index = 0; index < FLASH_MAX_PARTITIONS; index++) {
+        const flashPartition_t *partition;
+        if (index == 0) {
+            cliPrintLine("Paritions:");
+        }
+        partition = flashPartitionFindByIndex(index);
+        if (!partition) {
+            break;
+        }
+        cliPrintLinef("  %d: %s %u %u", index, flashPartitionGetTypeName(partition->type), partition->startSector, partition->endSector);
+    }
+#ifdef USE_FLASHFS
+    const flashPartition_t *flashPartition = flashPartitionFindByType(FLASH_PARTITION_TYPE_FLASHFS);
+
+    cliPrintLinef("FlashFS size=%u, usedSize=%u",
+            FLASH_PARTITION_SECTOR_COUNT(flashPartition) * layout->sectorSize,
+            flashfsGetOffset()
+    );
+#endif
 }
 
 static void cliFlashErase(char *cmdline)
@@ -2130,7 +2151,7 @@ static void cliFlashErase(char *cmdline)
     cliPrintLine("Erasing...");
     flashfsEraseCompletely();
 
-    while (!flashfsIsReady()) {
+    while (!flashIsReady()) {
         delay(100);
     }
 
diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c
index c9e26afd29..f0477e27d2 100644
--- a/src/main/fc/fc_init.c
+++ b/src/main/fc/fc_init.c
@@ -54,6 +54,7 @@
 #include "drivers/flash_m25p16.h"
 #include "drivers/io.h"
 #include "drivers/io_pca9685.h"
+#include "drivers/flash.h"
 #include "drivers/light_led.h"
 #include "drivers/nvic.h"
 #include "drivers/osd.h"
@@ -183,6 +184,10 @@ void flashLedsAndBeep(void)
 
 void init(void)
 {
+#if defined(USE_FLASHFS) && defined(USE_FLASH_M25P16)
+    bool flashDeviceInitialized = false;
+#endif
+
 #ifdef USE_HAL_DRIVER
     HAL_Init();
 #endif
@@ -362,7 +367,10 @@ void init(void)
         if (blackboxConfig()->device == BLACKBOX_DEVICE_FLASH) {
 #ifdef USE_FLASH_M25P16
             // Must initialise the device to read _anything_
-            m25p16_init(0);
+            /*m25p16_init(0);*/
+            if (!flashDeviceInitialized) {
+                flashDeviceInitialized = flashInit();
+            }
 #endif
             emfat_init_files();
         }
@@ -579,7 +587,9 @@ void init(void)
 #ifdef USE_FLASHFS
         case BLACKBOX_DEVICE_FLASH:
 #ifdef USE_FLASH_M25P16
-            m25p16_init(0);
+            if (!flashDeviceInitialized) {
+                flashDeviceInitialized = flashInit();
+            }
 #endif
             flashfsInit();
             break;
diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index 6dea4773db..8365dfb0d7 100644
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -44,6 +44,7 @@
 #include "drivers/bus_i2c.h"
 #include "drivers/compass/compass.h"
 #include "drivers/display.h"
+#include "drivers/flash.h"
 #include "drivers/osd.h"
 #include "drivers/osd_symbols.h"
 #include "drivers/pwm_mapping.h"
@@ -293,8 +294,8 @@ static void serializeSDCardSummaryReply(sbuf_t *dst)
 static void serializeDataflashSummaryReply(sbuf_t *dst)
 {
 #ifdef USE_FLASHFS
-    const flashGeometry_t *geometry = flashfsGetGeometry();
-    sbufWriteU8(dst, flashfsIsReady() ? 1 : 0);
+    const flashGeometry_t *geometry = flashGetGeometry();
+    sbufWriteU8(dst, flashIsReady() ? 1 : 0);
     sbufWriteU32(dst, geometry->sectors);
     sbufWriteU32(dst, geometry->totalSize);
     sbufWriteU32(dst, flashfsGetOffset()); // Effectively the current number of bytes stored on the volume
diff --git a/src/main/io/flashfs.c b/src/main/io/flashfs.c
index 545d136cd8..bb50b269f2 100644
--- a/src/main/io/flashfs.c
+++ b/src/main/io/flashfs.c
@@ -24,18 +24,17 @@
  *
  * Note that bits can only be set to 0 when writing, not back to 1 from 0. You must erase sectors in order
  * to bring bits back to 1 again.
- *
- * In future, we can add support for multiple different flash chips by adding a flash device driver vtable
- * and make calls through that, at the moment flashfs just calls m25p16_* routines explicitly.
  */
 
 #include <stdint.h>
 #include <stdbool.h>
 #include <string.h>
 
-#include "drivers/flash_m25p16.h"
+#include "drivers/flash.h"
 #include "flashfs.h"
 
+static flashPartition_t *flashPartition;
+
 static uint8_t flashWriteBuffer[FLASHFS_WRITE_BUFFER_SIZE];
 
 /* The position of our head and tail in the circular flash write buffer.
@@ -67,10 +66,8 @@ static void flashfsSetTailAddress(uint32_t address)
 
 void flashfsEraseCompletely(void)
 {
-    m25p16_eraseCompletely();
-
+    flashPartitionErase(flashPartition);
     flashfsClearBuffer();
-
     flashfsSetTailAddress(0);
 }
 
@@ -80,7 +77,7 @@ void flashfsEraseCompletely(void)
  */
 void flashfsEraseRange(uint32_t start, uint32_t end)
 {
-    const flashGeometry_t *geometry = m25p16_getGeometry();
+    const flashGeometry_t *geometry = flashGetGeometry();
 
     if (geometry->sectorSize <= 0)
         return;
@@ -97,7 +94,7 @@ void flashfsEraseRange(uint32_t start, uint32_t end)
     }
 
     for (int i = startSector; i < endSector; i++) {
-        m25p16_eraseSector(i * geometry->sectorSize);
+        flashEraseSector(i * geometry->sectorSize);
     }
 }
 
@@ -106,12 +103,12 @@ void flashfsEraseRange(uint32_t start, uint32_t end)
  */
 bool flashfsIsReady(void)
 {
-    return m25p16_isReady();
+    return !!flashPartition;
 }
 
 uint32_t flashfsGetSize(void)
 {
-    return m25p16_getGeometry()->totalSize;
+    return flashPartitionSize(flashPartition);
 }
 
 static uint32_t flashfsTransmitBufferUsed(void)
@@ -138,11 +135,6 @@ uint32_t flashfsGetWriteBufferFreeSpace(void)
     return flashfsGetWriteBufferSize() - flashfsTransmitBufferUsed();
 }
 
-const flashGeometry_t* flashfsGetGeometry(void)
-{
-    return m25p16_getGeometry();
-}
-
 /**
  * Write the given buffers to flash sequentially at the current tail address, advancing the tail address after
  * each write.
@@ -164,6 +156,8 @@ const flashGeometry_t* flashfsGetGeometry(void)
  */
 static uint32_t flashfsWriteBuffers(uint8_t const **buffers, uint32_t *bufferSizes, int bufferCount, bool sync)
 {
+    const flashGeometry_t *geometry = flashGetGeometry();
+
     uint32_t bytesTotal = 0;
 
     int i;
@@ -172,7 +166,7 @@ static uint32_t flashfsWriteBuffers(uint8_t const **buffers, uint32_t *bufferSiz
         bytesTotal += bufferSizes[i];
     }
 
-    if (!sync && !m25p16_isReady()) {
+    if (!sync && !flashIsReady()) {
         return 0;
     }
 
@@ -187,8 +181,8 @@ static uint32_t flashfsWriteBuffers(uint8_t const **buffers, uint32_t *bufferSiz
          * Each page needs to be saved in a separate program operation, so
          * if we would cross a page boundary, only write up to the boundary in this iteration:
          */
-        if (tailAddress % M25P16_PAGESIZE + bytesTotalRemaining > M25P16_PAGESIZE) {
-            bytesTotalThisIteration = M25P16_PAGESIZE - tailAddress % M25P16_PAGESIZE;
+        if (tailAddress % geometry->pageSize + bytesTotalRemaining > geometry->pageSize) {
+            bytesTotalThisIteration = geometry->pageSize - tailAddress % geometry->pageSize;
         } else {
             bytesTotalThisIteration = bytesTotalRemaining;
         }
@@ -207,7 +201,7 @@ static uint32_t flashfsWriteBuffers(uint8_t const **buffers, uint32_t *bufferSiz
             if (bufferSizes[i] > 0) {
                 // Is buffer larger than our write limit? Write our limit out of it
                 if (bufferSizes[i] >= bytesRemainThisIteration) {
-                    currentFlashAddress = m25p16_pageProgram(currentFlashAddress, buffers[i], bytesRemainThisIteration);
+                    currentFlashAddress = flashPageProgram(currentFlashAddress, buffers[i], bytesRemainThisIteration);
 
                     buffers[i] += bytesRemainThisIteration;
                     bufferSizes[i] -= bytesRemainThisIteration;
@@ -216,7 +210,7 @@ static uint32_t flashfsWriteBuffers(uint8_t const **buffers, uint32_t *bufferSiz
                     break;
                 } else {
                     // We'll still have more to write after finishing this buffer off
-                    currentFlashAddress = m25p16_pageProgram(currentFlashAddress, buffers[i], bufferSizes[i]);
+                    currentFlashAddress = flashPageProgram(currentFlashAddress, buffers[i], bufferSizes[i]);
 
                     bytesRemainThisIteration -= bufferSizes[i];
 
@@ -471,7 +465,7 @@ int flashfsReadAbs(uint32_t address, uint8_t *buffer, unsigned int len)
     // Since the read could overlap data in our dirty buffers, force a sync to clear those first
     flashfsFlushSync();
 
-    bytesRead = m25p16_readBytes(address, buffer, len);
+    bytesRead = flashReadBytes(address, buffer, len);
 
     return bytesRead;
 }
@@ -516,7 +510,7 @@ int flashfsIdentifyStartOfFreeSpace(void)
     while (left < right) {
         mid = (left + right) / 2;
 
-        if (m25p16_readBytes(mid * FREE_BLOCK_SIZE, testBuffer.bytes, FREE_BLOCK_TEST_SIZE_BYTES) < FREE_BLOCK_TEST_SIZE_BYTES) {
+        if (flashReadBytes(mid * FREE_BLOCK_SIZE, testBuffer.bytes, FREE_BLOCK_TEST_SIZE_BYTES) < FREE_BLOCK_TEST_SIZE_BYTES) {
             // Unexpected timeout from flash, so bail early (reporting the device fuller than it really is)
             break;
         }
@@ -558,8 +552,9 @@ bool flashfsIsEOF(void)
  */
 void flashfsInit(void)
 {
-    // If we have a flash chip present at all
-    if (flashfsGetSize() > 0) {
+    flashPartition = flashPartitionFindByType(FLASH_PARTITION_TYPE_FLASHFS);
+
+    if (flashPartition) {
         // Start the file pointer off at the beginning of free space so caller can start writing immediately
         flashfsSeekAbs(flashfsIdentifyStartOfFreeSpace());
     }
diff --git a/src/main/io/flashfs.h b/src/main/io/flashfs.h
index 02b79d69c3..4e14692ed2 100644
--- a/src/main/io/flashfs.h
+++ b/src/main/io/flashfs.h
@@ -35,7 +35,6 @@ uint32_t flashfsGetOffset(void);
 uint32_t flashfsGetWriteBufferFreeSpace(void);
 uint32_t flashfsGetWriteBufferSize(void);
 int flashfsIdentifyStartOfFreeSpace(void);
-const flashGeometry_t* flashfsGetGeometry(void);
 
 void flashfsSeekAbs(uint32_t offset);
 void flashfsSeekRel(int32_t offset);
diff --git a/src/main/target/BLUEJAYF4/hardware_revision.c b/src/main/target/BLUEJAYF4/hardware_revision.c
index f49c88c932..7192169d40 100644
--- a/src/main/target/BLUEJAYF4/hardware_revision.c
+++ b/src/main/target/BLUEJAYF4/hardware_revision.c
@@ -93,7 +93,7 @@ void updateHardwareRevision(void)
     }
 
     /* if flash exists on PB3 (busDevice m25p16_bjf3_rev1) then Rev1 */
-    if (m25p16_init(1)) {
+    if (flashInit()) {
         hardwareRevision = BJF4_REV1;
     } else {
         IOInit(IOGetByTag(IO_TAG(PB3)), OWNER_FREE, RESOURCE_NONE, 0);

From 90e127c6969bb0cd8f82c3329c83f6aff7528569 Mon Sep 17 00:00:00 2001
From: Michel Pastor <shellixyz@gmail.com>
Date: Thu, 5 Mar 2020 19:33:04 +0100
Subject: [PATCH 110/179] Add persistent data storage support

---
 make/mcu/STM32F4.mk                      |   1 -
 make/mcu/STM32F7.mk                      |   2 -
 make/source.mk                           |   2 +
 src/main/drivers/persistent.c            | 119 +++++++++++++++++++++++
 src/main/drivers/persistent.h            |  43 ++++++++
 src/main/drivers/system_stm32f4xx.c      |  31 +++++-
 src/main/drivers/usb_msc.c               |  27 +++++
 src/main/drivers/usb_msc.h               |   2 -
 src/main/drivers/usb_msc_f4xx.c          |  12 +--
 src/main/drivers/usb_msc_f7xx.c          |  11 +--
 src/main/fc/cli.c                        |   7 +-
 src/main/fc/fc_init.c                    |   4 +
 src/main/platform.h                      |   1 +
 src/main/startup/startup_stm32f40xx.s    |  17 +---
 src/main/startup/startup_stm32f427xx.s   |  29 +-----
 src/main/target/link/stm32_flash.ld      |   9 ++
 src/main/target/link/stm32_flash_f405.ld |   4 +-
 src/main/target/stm32f7xx_hal_conf.h     |   2 +-
 18 files changed, 251 insertions(+), 72 deletions(-)
 create mode 100644 src/main/drivers/persistent.c
 create mode 100644 src/main/drivers/persistent.h
 create mode 100644 src/main/drivers/usb_msc.c

diff --git a/make/mcu/STM32F4.mk b/make/mcu/STM32F4.mk
index 02e2548074..3c740a564b 100644
--- a/make/mcu/STM32F4.mk
+++ b/make/mcu/STM32F4.mk
@@ -29,7 +29,6 @@ EXCLUDES        = stm32f4xx_crc.c \
                   stm32f4xx_cryp_aes.c \
                   stm32f4xx_hash_md5.c \
                   stm32f4xx_cryp_des.c \
-                  stm32f4xx_rtc.c \
                   stm32f4xx_hash.c \
                   stm32f4xx_dbgmcu.c \
                   stm32f4xx_cryp_tdes.c \
diff --git a/make/mcu/STM32F7.mk b/make/mcu/STM32F7.mk
index a7f23aa58d..342a8cbf95 100644
--- a/make/mcu/STM32F7.mk
+++ b/make/mcu/STM32F7.mk
@@ -37,8 +37,6 @@ EXCLUDES        = stm32f7xx_hal_can.c \
                   stm32f7xx_hal_nor.c \
                   stm32f7xx_hal_qspi.c \
                   stm32f7xx_hal_rng.c \
-                  stm32f7xx_hal_rtc.c \
-                  stm32f7xx_hal_rtc_ex.c \
                   stm32f7xx_hal_sai.c \
                   stm32f7xx_hal_sai_ex.c \
                   stm32f7xx_hal_sd.c \
diff --git a/make/source.mk b/make/source.mk
index 565c806c2b..2637a2fb4a 100644
--- a/make/source.mk
+++ b/make/source.mk
@@ -46,6 +46,7 @@ COMMON_SRC = \
             drivers/io_pca9685.c \
             drivers/light_led.c \
             drivers/osd.c \
+            drivers/persistent.c \
             drivers/resource.c \
             drivers/rx_nrf24l01.c \
             drivers/rx_spi.c \
@@ -64,6 +65,7 @@ COMMON_SRC = \
             drivers/stack_check.c \
             drivers/system.c \
             drivers/timer.c \
+            drivers/usb_msc.c \
             drivers/lights_io.c \
             drivers/1-wire.c \
             drivers/1-wire/ds_crc.c \
diff --git a/src/main/drivers/persistent.c b/src/main/drivers/persistent.c
new file mode 100644
index 0000000000..b4ed38903a
--- /dev/null
+++ b/src/main/drivers/persistent.c
@@ -0,0 +1,119 @@
+/*
+ * This file is part of iNav.
+ *
+ * iNav free software. You can redistribute
+ * this software and/or modify this software 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 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 this software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/*
+ * An implementation of persistent data storage utilizing RTC backup data register.
+ * Retains values written across software resets and boot loader activities.
+ */
+
+#include <stdint.h>
+#include "platform.h"
+
+#include "drivers/persistent.h"
+#include "drivers/system.h"
+
+#define PERSISTENT_OBJECT_MAGIC_VALUE (('i' << 24)|('N' << 16)|('a' << 8)|('v' << 0))
+
+#ifdef USE_HAL_DRIVER
+
+uint32_t persistentObjectRead(persistentObjectId_e id)
+{
+    RTC_HandleTypeDef rtcHandle = { .Instance = RTC };
+
+    uint32_t value = HAL_RTCEx_BKUPRead(&rtcHandle, id);
+
+    return value;
+}
+
+void persistentObjectWrite(persistentObjectId_e id, uint32_t value)
+{
+    RTC_HandleTypeDef rtcHandle = { .Instance = RTC };
+
+    HAL_RTCEx_BKUPWrite(&rtcHandle, id, value);
+}
+
+void persistentObjectRTCEnable(void)
+{
+    RTC_HandleTypeDef rtcHandle = { .Instance = RTC };
+
+#if !defined(STM32H7)
+    __HAL_RCC_PWR_CLK_ENABLE(); // Enable Access to PWR
+#endif
+    HAL_PWR_EnableBkUpAccess(); // Disable backup domain protection
+
+#if defined(__HAL_RCC_RTC_CLK_ENABLE)
+    // For those MCUs with RTCAPBEN bit in RCC clock enable register, turn it on.
+    __HAL_RCC_RTC_CLK_ENABLE(); // Enable RTC module
+#endif
+
+    // We don't need a clock source for RTC itself. Skip it.
+
+    __HAL_RTC_WRITEPROTECTION_ENABLE(&rtcHandle);  // Reset sequence
+    __HAL_RTC_WRITEPROTECTION_DISABLE(&rtcHandle); // Apply sequence
+}
+
+#else
+uint32_t persistentObjectRead(persistentObjectId_e id)
+{
+    uint32_t value = RTC_ReadBackupRegister(id);
+
+    return value;
+}
+
+void persistentObjectWrite(persistentObjectId_e id, uint32_t value)
+{
+    RTC_WriteBackupRegister(id, value);
+}
+
+void persistentObjectRTCEnable(void)
+{
+    RCC_APB1PeriphClockCmd(RCC_APB1Periph_PWR, ENABLE); // Enable Access to PWR
+    PWR_BackupAccessCmd(ENABLE); // Disable backup domain protection
+
+    // We don't need a clock source for RTC itself. Skip it.
+
+    RTC_WriteProtectionCmd(ENABLE);  // Reset sequence
+    RTC_WriteProtectionCmd(DISABLE); // Apply sequence
+}
+#endif
+
+void persistentObjectInit(void)
+{
+    // Configure and enable RTC for backup register access
+
+    persistentObjectRTCEnable();
+
+    // XXX Magic value checking may be sufficient
+
+    uint32_t wasSoftReset;
+
+#ifdef STM32H7
+    wasSoftReset = RCC->RSR & RCC_RSR_SFTRSTF;
+#else
+    wasSoftReset = RCC->CSR & RCC_CSR_SFTRSTF;
+#endif
+
+    if (!wasSoftReset || (persistentObjectRead(PERSISTENT_OBJECT_MAGIC) != PERSISTENT_OBJECT_MAGIC_VALUE)) {
+        for (int i = 1; i < PERSISTENT_OBJECT_COUNT; i++) {
+            persistentObjectWrite(i, 0);
+        }
+        persistentObjectWrite(PERSISTENT_OBJECT_MAGIC, PERSISTENT_OBJECT_MAGIC_VALUE);
+    }
+}
diff --git a/src/main/drivers/persistent.h b/src/main/drivers/persistent.h
new file mode 100644
index 0000000000..ff26802788
--- /dev/null
+++ b/src/main/drivers/persistent.h
@@ -0,0 +1,43 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software 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.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * 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 this software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include <stdint.h>
+
+// Available RTC backup registers (4-byte words) per MCU type
+// F4: 20 words
+// F7: 32 words
+// H7: 32 words
+
+typedef enum {
+    PERSISTENT_OBJECT_MAGIC = 0,
+    PERSISTENT_OBJECT_RESET_REASON,
+    PERSISTENT_OBJECT_COUNT,
+} persistentObjectId_e;
+
+// Values for PERSISTENT_OBJECT_RESET_REASON
+#define RESET_NONE                                      0
+#define RESET_BOOTLOADER_REQUEST_ROM                    1
+#define RESET_MSC_REQUEST                               2  // MSC invocation was requested
+
+void persistentObjectInit(void);
+uint32_t persistentObjectRead(persistentObjectId_e id);
+void persistentObjectWrite(persistentObjectId_e id, uint32_t value);
diff --git a/src/main/drivers/system_stm32f4xx.c b/src/main/drivers/system_stm32f4xx.c
index 7da03ec009..47708e67a4 100644
--- a/src/main/drivers/system_stm32f4xx.c
+++ b/src/main/drivers/system_stm32f4xx.c
@@ -23,6 +23,7 @@
 
 #include "drivers/accgyro/accgyro_mpu.h"
 #include "drivers/exti.h"
+#include "drivers/persistent.h"
 #include "drivers/nvic.h"
 #include "drivers/system.h"
 
@@ -43,6 +44,32 @@ inline static void NVIC_DisableAllIRQs(void)
     }
 }
 
+typedef void resetHandler_t(void);
+
+typedef struct isrVector_s {
+    __I uint32_t    stackEnd;
+    resetHandler_t *resetHandler;
+} isrVector_t;
+
+#pragma GCC push_options
+#pragma GCC optimize ("O0")
+void checkForBootLoaderRequest(void)
+{
+    uint32_t bootloaderRequest = persistentObjectRead(PERSISTENT_OBJECT_RESET_REASON);
+
+    if (bootloaderRequest != RESET_BOOTLOADER_REQUEST_ROM) {
+            return;
+        }
+    persistentObjectWrite(PERSISTENT_OBJECT_RESET_REASON, RESET_NONE);
+
+    extern isrVector_t system_isr_vector_table_base;
+
+    __set_MSP(system_isr_vector_table_base.stackEnd);
+    system_isr_vector_table_base.resetHandler();
+    while (1);
+}
+#pragma GCC pop_options
+
 void systemReset(void)
 {
     __disable_irq();
@@ -52,11 +79,9 @@ void systemReset(void)
 
 void systemResetToBootloader(void)
 {
+    persistentObjectWrite(PERSISTENT_OBJECT_RESET_REASON, RESET_BOOTLOADER_REQUEST_ROM);
     __disable_irq();
     NVIC_DisableAllIRQs();
-
-    *((uint32_t *)0x2001FFFC) = 0xDEADBEEF; // 128KB SRAM STM32F4XX
-
     NVIC_SystemReset();
 }
 
diff --git a/src/main/drivers/usb_msc.c b/src/main/drivers/usb_msc.c
new file mode 100644
index 0000000000..937a88fb17
--- /dev/null
+++ b/src/main/drivers/usb_msc.c
@@ -0,0 +1,27 @@
+/*
+ * This file is part of iNav
+ *
+ * iNav is free software. You can redistribute
+ * this software and/or modify this software 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 this software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "drivers/persistent.h"
+#include <stdbool.h>
+
+bool mscCheckBoot(void)
+{
+    return (persistentObjectRead(PERSISTENT_OBJECT_RESET_REASON) == RESET_MSC_REQUEST);
+}
diff --git a/src/main/drivers/usb_msc.h b/src/main/drivers/usb_msc.h
index 3995b3c170..01334bbef0 100644
--- a/src/main/drivers/usb_msc.h
+++ b/src/main/drivers/usb_msc.h
@@ -24,8 +24,6 @@
 
 #pragma once
 
-#define MSC_MAGIC 0xDDDD1010
-
 void mscInit(void);
 bool mscCheckBoot(void);
 uint8_t mscStart(void);
diff --git a/src/main/drivers/usb_msc_f4xx.c b/src/main/drivers/usb_msc_f4xx.c
index f0fb050061..8bc9a9ade3 100644
--- a/src/main/drivers/usb_msc_f4xx.c
+++ b/src/main/drivers/usb_msc_f4xx.c
@@ -41,6 +41,7 @@
 #include "drivers/io.h"
 #include "drivers/light_led.h"
 #include "drivers/nvic.h"
+#include "drivers/persistent.h"
 #include "drivers/sdmmc_sdio.h"
 #include "drivers/time.h"
 #include "drivers/usb_msc.h"
@@ -110,6 +111,8 @@ uint8_t mscStart(void)
 
     USBD_Init(&USB_OTG_dev, USB_OTG_FS_CORE_ID, &MSC_desc, &USBD_MSC_cb, &USR_cb);
 
+    persistentObjectWrite(PERSISTENT_OBJECT_RESET_REASON, RESET_NONE);
+
     // NVIC configuration for SYSTick
     NVIC_DisableIRQ(SysTick_IRQn);
     NVIC_SetPriority(SysTick_IRQn, 0);
@@ -118,14 +121,6 @@ uint8_t mscStart(void)
     return 0;
 }
 
-bool mscCheckBoot(void)
-{
-    if (*((uint32_t *)0x2001FFF0) == MSC_MAGIC) {
-        return true;
-    }
-    return false;
-}
-
 bool mscCheckButton(void)
 {
     bool result = false;
@@ -150,7 +145,6 @@ void mscWaitForButton(void)
     while (true) {
         asm("NOP");
         if (mscCheckButton()) {
-            *((uint32_t *)0x2001FFF0) = 0xFFFFFFFF;
             delay(1);
             NVIC_SystemReset();
         }
diff --git a/src/main/drivers/usb_msc_f7xx.c b/src/main/drivers/usb_msc_f7xx.c
index 41877a3362..bfaee68bb8 100644
--- a/src/main/drivers/usb_msc_f7xx.c
+++ b/src/main/drivers/usb_msc_f7xx.c
@@ -41,6 +41,7 @@
 #include "drivers/io.h"
 #include "drivers/light_led.h"
 #include "drivers/nvic.h"
+#include "drivers/persistent.h"
 #include "drivers/time.h"
 #include "drivers/usb_msc.h"
 
@@ -108,6 +109,8 @@ uint8_t mscStart(void)
 
     USBD_Start(&USBD_Device);
 
+    persistentObjectWrite(PERSISTENT_OBJECT_RESET_REASON, RESET_NONE);
+
     // NVIC configuration for SYSTick
     NVIC_DisableIRQ(SysTick_IRQn);
     NVIC_SetPriority(SysTick_IRQn, 0);
@@ -116,14 +119,6 @@ uint8_t mscStart(void)
     return 0;
 }
 
-bool mscCheckBoot(void)
-{
-    if (*((__IO uint32_t *)BKPSRAM_BASE + 16) == MSC_MAGIC) {
-        return true;
-    }
-    return false;
-}
-
 bool mscCheckButton(void)
 {
     bool result = false;
diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c
index a08b000a97..bfa6be28f5 100755
--- a/src/main/fc/cli.c
+++ b/src/main/fc/cli.c
@@ -59,6 +59,7 @@ extern uint8_t __config_end;
 #include "drivers/io.h"
 #include "drivers/io_impl.h"
 #include "drivers/osd_symbols.h"
+#include "drivers/persistent.h"
 #include "drivers/rx_pwm.h"
 #include "drivers/sdcard/sdcard.h"
 #include "drivers/sensor.h"
@@ -3434,11 +3435,7 @@ static void cliMsc(char *cmdline)
         waitForSerialPortToFinishTransmitting(cliPort);
         stopPwmAllMotors();
 
-#ifdef STM32F7
-        *((__IO uint32_t*) BKPSRAM_BASE + 16) = MSC_MAGIC;
-#elif defined(STM32F4)
-        *((uint32_t *)0x2001FFF0) = MSC_MAGIC;
-#endif
+        persistentObjectWrite(PERSISTENT_OBJECT_RESET_REASON, RESET_MSC_REQUEST);
 
         __disable_irq();
         NVIC_SystemReset();
diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c
index c9e26afd29..240d296c89 100644
--- a/src/main/fc/fc_init.c
+++ b/src/main/fc/fc_init.c
@@ -57,6 +57,7 @@
 #include "drivers/light_led.h"
 #include "drivers/nvic.h"
 #include "drivers/osd.h"
+#include "drivers/persistent.h"
 #include "drivers/pwm_esc_detect.h"
 #include "drivers/pwm_mapping.h"
 #include "drivers/pwm_output.h"
@@ -661,5 +662,8 @@ void init(void)
     }
 #endif
 
+    // Considering that the persistent reset reason is only used during init
+    persistentObjectWrite(PERSISTENT_OBJECT_RESET_REASON, RESET_NONE);
+
     systemState |= SYSTEM_STATE_READY;
 }
diff --git a/src/main/platform.h b/src/main/platform.h
index 657b067480..640ded15aa 100644
--- a/src/main/platform.h
+++ b/src/main/platform.h
@@ -20,6 +20,7 @@
 #if defined(STM32F7)
 #include "stm32f7xx.h"
 #include "stm32f7xx_hal.h"
+#include "stm32f7xx_hal_rtc.h"
 #include "stm32f7xx_ll_spi.h"
 #include "stm32f7xx_ll_gpio.h"
 #include "stm32f7xx_ll_dma.h"
diff --git a/src/main/startup/startup_stm32f40xx.s b/src/main/startup/startup_stm32f40xx.s
index 764bc499cb..9e3a33932d 100644
--- a/src/main/startup/startup_stm32f40xx.s
+++ b/src/main/startup/startup_stm32f40xx.s
@@ -80,13 +80,9 @@ Reset_Handler:
   str     r1, [r0, #0x30]
   dsb
 
-  // Check for bootloader reboot
-  ldr r0, =0x2001FFFC         // mj666
-  ldr r1, =0xDEADBEEF         // mj666
-  ldr r2, [r0, #0]            // mj666
-  str r0, [r0, #0]            // mj666
-  cmp r2, r1                  // mj666
-  beq Reboot_Loader           // mj666
+  // Defined in C code
+  bl persistentObjectInit
+  bl checkForBootLoaderRequest
 
 /* Copy the data segment initializers from flash to SRAM */ 
   movs  r1, #0
@@ -158,13 +154,6 @@ LoopMarkHeapStack:
 LoopForever:
   b LoopForever
 
-Reboot_Loader:                // mj666
-  // Reboot to ROM            // mj666
-  ldr     r0, =0x1FFF0000     // mj666
-  ldr     sp,[r0, #0]         // mj666
-  ldr     r0,[r0, #4]         // mj666
-  bx      r0                  // mj666
-
 .size  Reset_Handler, .-Reset_Handler
 
 /**
diff --git a/src/main/startup/startup_stm32f427xx.s b/src/main/startup/startup_stm32f427xx.s
index 520a7f7222..064f187fd6 100755
--- a/src/main/startup/startup_stm32f427xx.s
+++ b/src/main/startup/startup_stm32f427xx.s
@@ -87,13 +87,9 @@ Reset_Handler:
   str     r1, [r0, #0x30]
   dsb
 
-  // Check for bootloader reboot
-  ldr r0, =0x2001FFFC         // mj666
-  ldr r1, =0xDEADBEEF         // mj666
-  ldr r2, [r0, #0]            // mj666
-  str r0, [r0, #0]            // mj666
-  cmp r2, r1                  // mj666
-  beq Reboot_Loader           // mj666
+  // Defined in C code
+  bl persistentObjectInit
+  bl checkForBootLoaderRequest
 
 /* Copy the data segment initializers from flash to SRAM */ 
   movs  r1, #0
@@ -165,25 +161,6 @@ LoopMarkHeapStack:
 LoopForever:
   b LoopForever
 
-Reboot_Loader:                // mj666
-  // RCC->APB2ENR |= RCC_APB2Periph_SYSCFG;
-  ldr     r0, =0x40023800
-  ldr     r1, [r0, #0x44]
-  orr     r1, r1, 0x00004000    // RCC_APB2Periph_SYSCFG
-  str     r1, [r0, #0x44]
-
-  // Remap system memory to 0x00000000
-  // SYSCFG->MEMRMP = SYSCFG_MemoryRemap_SystemFlash
-  ldr     r0, =0x40013800
-  ldr     r1, =0x00000001
-  str     r1, [r0]
-
-  // Reboot to ROM            // mj666
-  ldr     r0, =0x1FFF0000     // mj666
-  ldr     sp,[r0, #0]         // mj666
-  ldr     r0,[r0, #4]         // mj666
-  bx      r0                  // mj666
-
 .size  Reset_Handler, .-Reset_Handler
 
 /**
diff --git a/src/main/target/link/stm32_flash.ld b/src/main/target/link/stm32_flash.ld
index 83f6aeb32c..08af3aee4c 100644
--- a/src/main/target/link/stm32_flash.ld
+++ b/src/main/target/link/stm32_flash.ld
@@ -30,6 +30,15 @@ SECTIONS
     . = ALIGN(4);
   } >FLASH
 
+  /* System memory (read-only bootloader) interrupt vector */
+  .system_isr_vector (NOLOAD) :
+  {
+    . = ALIGN(4);
+    PROVIDE (system_isr_vector_table_base = .);
+    KEEP(*(.system_isr_vector)) /* Bootloader code */
+    . = ALIGN(4);
+  } >SYSTEM_MEMORY
+
   /* The program code and other data goes into FLASH */
   .text :
   {
diff --git a/src/main/target/link/stm32_flash_f405.ld b/src/main/target/link/stm32_flash_f405.ld
index 4c728c5ebb..4e009241d9 100644
--- a/src/main/target/link/stm32_flash_f405.ld
+++ b/src/main/target/link/stm32_flash_f405.ld
@@ -28,6 +28,8 @@ MEMORY
     FLASH (rx)        : ORIGIN = 0x08000000, LENGTH = 896K
     FLASH_CONFIG (r)  : ORIGIN = 0x080E0000, LENGTH = 128K
 
+    SYSTEM_MEMORY (rx): ORIGIN = 0x1FFF0000, LENGTH = 29K
+
     RAM (rwx)         : ORIGIN = 0x20000000, LENGTH = 128K
     CCM (rwx)         : ORIGIN = 0x10000000, LENGTH = 64K
     BACKUP_SRAM (rwx) : ORIGIN = 0x40024000, LENGTH = 4K
@@ -37,4 +39,4 @@ MEMORY
 REGION_ALIAS("STACKRAM", CCM)
 REGION_ALIAS("FASTRAM", CCM)
 
-INCLUDE "stm32_flash.ld"
\ No newline at end of file
+INCLUDE "stm32_flash.ld"
diff --git a/src/main/target/stm32f7xx_hal_conf.h b/src/main/target/stm32f7xx_hal_conf.h
index ebd30750b4..8c82cd3b95 100644
--- a/src/main/target/stm32f7xx_hal_conf.h
+++ b/src/main/target/stm32f7xx_hal_conf.h
@@ -69,7 +69,7 @@
 /* #define HAL_LTDC_MODULE_ENABLED   */
 /* #define HAL_QSPI_MODULE_ENABLED   */
 /* #define HAL_RNG_MODULE_ENABLED   */
-/* #define HAL_RTC_MODULE_ENABLED   */
+#define HAL_RTC_MODULE_ENABLED
 /* #define HAL_SAI_MODULE_ENABLED   */
 /* #define HAL_SD_MODULE_ENABLED   */
 /* #define HAL_MMC_MODULE_ENABLED   */

From 0f4245b58667e72cdfb5a8bf91710755e0a28beb Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Alberto=20Garci=CC=81a=20Hierro?= <alberto@garciahierro.com>
Date: Tue, 24 Mar 2020 19:39:22 +0000
Subject: [PATCH 111/179] [STARTUP] Call persistentObjectInit during startup
 for all chips

---
 src/main/startup/startup_stm32f30x_md_gcc.S | 11 +++++------
 src/main/startup/startup_stm32f411xe.s      | 10 +++-------
 src/main/startup/startup_stm32f446xx.s      | 10 +++-------
 src/main/startup/startup_stm32f722xx.s      |  2 ++
 src/main/startup/startup_stm32f745xx.s      |  4 ++++
 src/main/startup/startup_stm32f746xx.s      |  2 ++
 src/main/startup/startup_stm32f765xx.s      |  4 ++++
 7 files changed, 23 insertions(+), 20 deletions(-)

diff --git a/src/main/startup/startup_stm32f30x_md_gcc.S b/src/main/startup/startup_stm32f30x_md_gcc.S
index 466d2a7add..819c92734f 100644
--- a/src/main/startup/startup_stm32f30x_md_gcc.S
+++ b/src/main/startup/startup_stm32f30x_md_gcc.S
@@ -70,12 +70,11 @@ defined in linker script */
   .weak  Reset_Handler
   .type  Reset_Handler, %function
 Reset_Handler:
-  ldr r0, =0x20009FFC         // HJI 11/9/2012
-  ldr r1, =0xDEADBEEF         // HJI 11/9/2012
-  ldr r2, [r0, #0]            // HJI 11/9/2012
-  str r0, [r0, #0]            // HJI 11/9/2012
-  cmp r2, r1                  // HJI 11/9/2012
-  beq Reboot_Loader           // HJI 11/9/2012
+
+  // Defined in C code
+  bl persistentObjectInit
+  bl checkForBootLoaderRequest
+
 
 /* Copy the data segment initializers from flash to SRAM */
   movs  r1, #0
diff --git a/src/main/startup/startup_stm32f411xe.s b/src/main/startup/startup_stm32f411xe.s
index bf174f25ba..06fb51a84d 100644
--- a/src/main/startup/startup_stm32f411xe.s
+++ b/src/main/startup/startup_stm32f411xe.s
@@ -80,13 +80,9 @@ Reset_Handler:
   str     r1, [r0, #0x30]
   dsb
 
-  // Check for bootloader reboot
-  ldr r0, =0x2001FFFC         // mj666
-  ldr r1, =0xDEADBEEF         // mj666
-  ldr r2, [r0, #0]            // mj666
-  str r0, [r0, #0]            // mj666
-  cmp r2, r1                  // mj666
-  beq Reboot_Loader           // mj666
+  // Defined in C code
+  bl persistentObjectInit
+  bl checkForBootLoaderRequest
 
 /* Copy the data segment initializers from flash to SRAM */ 
   movs  r1, #0
diff --git a/src/main/startup/startup_stm32f446xx.s b/src/main/startup/startup_stm32f446xx.s
index 592a03de67..4686636823 100644
--- a/src/main/startup/startup_stm32f446xx.s
+++ b/src/main/startup/startup_stm32f446xx.s
@@ -82,13 +82,9 @@ Reset_Handler:
   str     r1, [r0, #0x30]
   dsb
 
-  // Check for bootloader reboot
-  ldr r0, =0x2001FFFC         // mj666
-  ldr r1, =0xDEADBEEF         // mj666
-  ldr r2, [r0, #0]            // mj666
-  str r0, [r0, #0]            // mj666
-  cmp r2, r1                  // mj666
-  beq Reboot_Loader           // mj666
+  // Defined in C code
+  bl persistentObjectInit
+  bl checkForBootLoaderRequest
 
 /* Copy the data segment initializers from flash to SRAM */
   movs  r1, #0
diff --git a/src/main/startup/startup_stm32f722xx.s b/src/main/startup/startup_stm32f722xx.s
index f856d04e3b..2e8d766626 100644
--- a/src/main/startup/startup_stm32f722xx.s
+++ b/src/main/startup/startup_stm32f722xx.s
@@ -79,6 +79,8 @@ defined in linker script */
 Reset_Handler:  
   ldr   sp, =_estack      /* set stack pointer */
 
+  bl persistentObjectInit
+
 /* Copy the data segment initializers from flash to SRAM */  
   movs  r1, #0
   b  LoopCopyDataInit
diff --git a/src/main/startup/startup_stm32f745xx.s b/src/main/startup/startup_stm32f745xx.s
index eb6aa25659..48b4a79657 100644
--- a/src/main/startup/startup_stm32f745xx.s
+++ b/src/main/startup/startup_stm32f745xx.s
@@ -77,6 +77,10 @@ defined in linker script */
   .weak  Reset_Handler
   .type  Reset_Handler, %function
 Reset_Handler:  
+  ldr   sp, =_estack      /* set stack pointer */
+
+  bl persistentObjectInit
+
 /* Copy the data segment initializers from flash to SRAM */  
   movs  r1, #0
   b  LoopCopyDataInit
diff --git a/src/main/startup/startup_stm32f746xx.s b/src/main/startup/startup_stm32f746xx.s
index 8ef14a877f..47d4c6841b 100644
--- a/src/main/startup/startup_stm32f746xx.s
+++ b/src/main/startup/startup_stm32f746xx.s
@@ -79,6 +79,8 @@ defined in linker script */
 Reset_Handler:  
   ldr   sp, =_estack      /* set stack pointer */
 
+  bl persistentObjectInit
+
 /* Copy the data segment initializers from flash to SRAM */  
   movs  r1, #0
   b  LoopCopyDataInit
diff --git a/src/main/startup/startup_stm32f765xx.s b/src/main/startup/startup_stm32f765xx.s
index 29c9ac1a28..84d4b30e9c 100644
--- a/src/main/startup/startup_stm32f765xx.s
+++ b/src/main/startup/startup_stm32f765xx.s
@@ -77,6 +77,10 @@ defined in linker script */
   .weak  Reset_Handler
   .type  Reset_Handler, %function
 Reset_Handler:  
+  ldr   sp, =_estack      /* set stack pointer */
+
+  bl persistentObjectInit
+
 /* Copy the data segment initializers from flash to SRAM */  
   movs  r1, #0
   b  LoopCopyDataInit

From 6809bc2b99ec0ad7568197f8968d31fe54993331 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Alberto=20Garci=CC=81a=20Hierro?= <alberto@garciahierro.com>
Date: Tue, 24 Mar 2020 19:46:40 +0000
Subject: [PATCH 112/179] [LD] Define SYSTEM_MEMORY region for all chips

SYSTEM_MEMORY contains the in-ROM factory programmed bootloader
---
 src/main/target/link/stm32_flash_f303_128k.ld | 2 ++
 src/main/target/link/stm32_flash_f303_256k.ld | 2 ++
 src/main/target/link/stm32_flash_f405_opbl.ld | 4 +++-
 src/main/target/link/stm32_flash_f411.ld      | 2 ++
 src/main/target/link/stm32_flash_f411_opbl.ld | 4 +++-
 src/main/target/link/stm32_flash_f427.ld      | 6 ++++--
 src/main/target/link/stm32_flash_f446.ld      | 4 +++-
 src/main/target/link/stm32_flash_f722.ld      | 2 ++
 src/main/target/link/stm32_flash_f745.ld      | 2 ++
 src/main/target/link/stm32_flash_f746.ld      | 2 ++
 src/main/target/link/stm32_flash_f765.ld      | 4 +++-
 src/main/target/link/stm32_flash_split.ld     | 9 +++++++++
 12 files changed, 37 insertions(+), 6 deletions(-)

diff --git a/src/main/target/link/stm32_flash_f303_128k.ld b/src/main/target/link/stm32_flash_f303_128k.ld
index 4274fb4a45..320148bc5b 100644
--- a/src/main/target/link/stm32_flash_f303_128k.ld
+++ b/src/main/target/link/stm32_flash_f303_128k.ld
@@ -19,6 +19,8 @@ MEMORY
     FLASH (rx)        : ORIGIN = 0x08000000, LENGTH = 126K
     FLASH_CONFIG (r)  : ORIGIN = 0x0801F800, LENGTH = 2K
 
+    SYSTEM_MEMORY (rx): ORIGIN = 0x1FFFD800, LENGTH = 8K
+
     RAM (xrw)         : ORIGIN = 0x20000000, LENGTH = 40K
     CCM (xrw)         : ORIGIN = 0x10000000, LENGTH = 8K
     MEMORY_B1 (rx)    : ORIGIN = 0x60000000, LENGTH = 0K
diff --git a/src/main/target/link/stm32_flash_f303_256k.ld b/src/main/target/link/stm32_flash_f303_256k.ld
index 1917843495..d3e49fe479 100644
--- a/src/main/target/link/stm32_flash_f303_256k.ld
+++ b/src/main/target/link/stm32_flash_f303_256k.ld
@@ -19,6 +19,8 @@ MEMORY
     FLASH (rx)        : ORIGIN = 0x08000000, LENGTH = 250K
     FLASH_CONFIG (r)  : ORIGIN = 0x0803E800, LENGTH = 6K
 
+    SYSTEM_MEMORY (rx): ORIGIN = 0x1FFFD800, LENGTH = 8K
+
     RAM (xrw)         : ORIGIN = 0x20000000, LENGTH = 40K
     CCM (xrw)         : ORIGIN = 0x10000000, LENGTH = 8K
     MEMORY_B1 (rx)    : ORIGIN = 0x60000000, LENGTH = 0K
diff --git a/src/main/target/link/stm32_flash_f405_opbl.ld b/src/main/target/link/stm32_flash_f405_opbl.ld
index d12578f7db..8ba55f94f1 100644
--- a/src/main/target/link/stm32_flash_f405_opbl.ld
+++ b/src/main/target/link/stm32_flash_f405_opbl.ld
@@ -29,6 +29,8 @@ MEMORY
     FLASH (rx)      : ORIGIN = 0x08004000, LENGTH = 880K
     FLASH_CONFIG (r): ORIGIN = 0x080E0000, LENGTH = 128K
 
+    SYSTEM_MEMORY (rx): ORIGIN = 0x1FFF0000, LENGTH = 29K
+
     RAM (rwx)       : ORIGIN = 0x20000000, LENGTH = 128K
     CCM (rwx)       : ORIGIN = 0x10000000, LENGTH = 64K
     MEMORY_B1 (rx)  : ORIGIN = 0x60000000, LENGTH = 0K
@@ -37,4 +39,4 @@ MEMORY
 REGION_ALIAS("STACKRAM", CCM)
 REGION_ALIAS("FASTRAM", CCM)
 
-INCLUDE "stm32_flash.ld"
\ No newline at end of file
+INCLUDE "stm32_flash.ld"
diff --git a/src/main/target/link/stm32_flash_f411.ld b/src/main/target/link/stm32_flash_f411.ld
index 0326840b34..a4ed5003a4 100644
--- a/src/main/target/link/stm32_flash_f411.ld
+++ b/src/main/target/link/stm32_flash_f411.ld
@@ -30,6 +30,8 @@ MEMORY
     FLASH_CONFIG (r)  : ORIGIN = 0x08004000, LENGTH = 16K
     FLASH1 (rx)       : ORIGIN = 0x08008000, LENGTH = 480K
 
+    SYSTEM_MEMORY (rx): ORIGIN = 0x1FFF0000, LENGTH = 29K
+
     RAM (rwx)         : ORIGIN = 0x20000000, LENGTH = 128K
     MEMORY_B1 (rx)    : ORIGIN = 0x60000000, LENGTH = 0K
 }
diff --git a/src/main/target/link/stm32_flash_f411_opbl.ld b/src/main/target/link/stm32_flash_f411_opbl.ld
index af264a87e1..b32fef22dc 100644
--- a/src/main/target/link/stm32_flash_f411_opbl.ld
+++ b/src/main/target/link/stm32_flash_f411_opbl.ld
@@ -29,6 +29,8 @@ MEMORY
     FLASH (rx)        : ORIGIN = 0x08004000, LENGTH = 368K
     FLASH_CONFIG (r)  : ORIGIN = 0x08060000, LENGTH = 128K
 
+    SYSTEM_MEMORY (rx): ORIGIN = 0x1FFF0000, LENGTH = 29K
+
     RAM (rwx)         : ORIGIN = 0x20000000, LENGTH = 128K
     CCM (xrw)         : ORIGIN = 0x10000000, LENGTH = 64K
     MEMORY_B1 (rx)    : ORIGIN = 0x60000000, LENGTH = 0K
@@ -37,4 +39,4 @@ MEMORY
 REGION_ALIAS("STACKRAM", CCM)
 REGION_ALIAS("FASTRAM", CCM)
 
-INCLUDE "stm32_flash.ld"
\ No newline at end of file
+INCLUDE "stm32_flash.ld"
diff --git a/src/main/target/link/stm32_flash_f427.ld b/src/main/target/link/stm32_flash_f427.ld
index 7cc7256337..96a0dc0969 100755
--- a/src/main/target/link/stm32_flash_f427.ld
+++ b/src/main/target/link/stm32_flash_f427.ld
@@ -15,7 +15,7 @@
 **
 **  Environment : Atollic TrueSTUDIO(R)
 **
-**  Distribution: The file is distributed �as is,� without any warranty
+**  Distribution: The file is distributed as is, without any warranty
 **                of any kind.
 **
 **  (c)Copyright Atollic AB.
@@ -48,6 +48,8 @@ MEMORY
     FLASH (rx)        : ORIGIN = 0x08000000, LENGTH = 896K
     FLASH_CONFIG (r)  : ORIGIN = 0x080E0000, LENGTH = 128K
 
+    SYSTEM_MEMORY (rx): ORIGIN = 0x1FFF0000, LENGTH = 29K
+
     RAM (rwx)         : ORIGIN = 0x20000000, LENGTH = 128K
     CCM (rwx)         : ORIGIN = 0x10000000, LENGTH = 64K
     MEMORY_B1 (rx)    : ORIGIN = 0x60000000, LENGTH = 0K
@@ -56,4 +58,4 @@ MEMORY
 REGION_ALIAS("STACKRAM", CCM)
 REGION_ALIAS("FASTRAM", CCM)
 
-INCLUDE "stm32_flash.ld"
\ No newline at end of file
+INCLUDE "stm32_flash.ld"
diff --git a/src/main/target/link/stm32_flash_f446.ld b/src/main/target/link/stm32_flash_f446.ld
index 1128fa3506..a4ed5003a4 100644
--- a/src/main/target/link/stm32_flash_f446.ld
+++ b/src/main/target/link/stm32_flash_f446.ld
@@ -30,6 +30,8 @@ MEMORY
     FLASH_CONFIG (r)  : ORIGIN = 0x08004000, LENGTH = 16K
     FLASH1 (rx)       : ORIGIN = 0x08008000, LENGTH = 480K
 
+    SYSTEM_MEMORY (rx): ORIGIN = 0x1FFF0000, LENGTH = 29K
+
     RAM (rwx)         : ORIGIN = 0x20000000, LENGTH = 128K
     MEMORY_B1 (rx)    : ORIGIN = 0x60000000, LENGTH = 0K
 }
@@ -37,4 +39,4 @@ MEMORY
 REGION_ALIAS("STACKRAM", RAM)
 REGION_ALIAS("FASTRAM", RAM)
 
-INCLUDE "stm32_flash_split.ld"
\ No newline at end of file
+INCLUDE "stm32_flash_split.ld"
diff --git a/src/main/target/link/stm32_flash_f722.ld b/src/main/target/link/stm32_flash_f722.ld
index 296708678d..457c02a330 100644
--- a/src/main/target/link/stm32_flash_f722.ld
+++ b/src/main/target/link/stm32_flash_f722.ld
@@ -37,6 +37,8 @@ MEMORY
     FLASH_CONFIG (r)   : ORIGIN = 0x08004000, LENGTH = 16K
     FLASH1 (rx)        : ORIGIN = 0x08008000, LENGTH = 480K
 
+    SYSTEM_MEMORY (rx) : ORIGIN = 0x1FF00000, LENGTH = 59K
+
     TCM (rwx)          : ORIGIN = 0x20000000, LENGTH = 64K
     RAM (rwx)          : ORIGIN = 0x20010000, LENGTH = 192K
     MEMORY_B1 (rx)     : ORIGIN = 0x60000000, LENGTH = 0K
diff --git a/src/main/target/link/stm32_flash_f745.ld b/src/main/target/link/stm32_flash_f745.ld
index a83374e4e4..2dc5eb8acb 100644
--- a/src/main/target/link/stm32_flash_f745.ld
+++ b/src/main/target/link/stm32_flash_f745.ld
@@ -37,6 +37,8 @@ MEMORY
     FLASH_CONFIG (r)  : ORIGIN = 0x08008000, LENGTH = 32K
     FLASH1 (rx)       : ORIGIN = 0x08010000, LENGTH = 960K
 
+    SYSTEM_MEMORY (rx) : ORIGIN = 0x1FF00000, LENGTH = 60K
+
     TCM (rwx)         : ORIGIN = 0x20000000, LENGTH = 64K
     RAM (rwx)         : ORIGIN = 0x20010000, LENGTH = 256K
     MEMORY_B1 (rx)    : ORIGIN = 0x60000000, LENGTH = 0K
diff --git a/src/main/target/link/stm32_flash_f746.ld b/src/main/target/link/stm32_flash_f746.ld
index 0c9041676f..b80cdef7e2 100644
--- a/src/main/target/link/stm32_flash_f746.ld
+++ b/src/main/target/link/stm32_flash_f746.ld
@@ -36,6 +36,8 @@ MEMORY
     FLASH_CONFIG (r)  : ORIGIN = 0x08008000, LENGTH = 32K
     FLASH1 (rx)       : ORIGIN = 0x08010000, LENGTH = 960K
 
+    SYSTEM_MEMORY (rx) : ORIGIN = 0x1FF00000, LENGTH = 60K
+
     TCM (rwx)         : ORIGIN = 0x20000000, LENGTH = 64K
     RAM (rwx)         : ORIGIN = 0x20010000, LENGTH = 256K
     MEMORY_B1 (rx)    : ORIGIN = 0x60000000, LENGTH = 0K
diff --git a/src/main/target/link/stm32_flash_f765.ld b/src/main/target/link/stm32_flash_f765.ld
index 86e394c673..eaf3209d45 100644
--- a/src/main/target/link/stm32_flash_f765.ld
+++ b/src/main/target/link/stm32_flash_f765.ld
@@ -38,6 +38,8 @@ MEMORY
     AXIM_FLASH_CFG (r)      : ORIGIN = 0x08008000, LENGTH = 32K
     AXIM_FLASH1 (rx)        : ORIGIN = 0x08010000, LENGTH = 1984K
 
+    SYSTEM_MEMORY (rx)      : ORIGIN = 0x1FF00000, LENGTH = 59K
+
     DTCM_RAM (rwx)          : ORIGIN = 0x20000000, LENGTH = 128K
     SRAM1 (rwx)             : ORIGIN = 0x20020000, LENGTH = 368K
     SRAM2 (rwx)             : ORIGIN = 0x2007C000, LENGTH = 16K
@@ -52,4 +54,4 @@ REGION_ALIAS("STACKRAM", DTCM_RAM)
 REGION_ALIAS("FASTRAM",  DTCM_RAM)
 REGION_ALIAS("RAM",      SRAM1)
 
-INCLUDE "stm32_flash_f7_split.ld"
\ No newline at end of file
+INCLUDE "stm32_flash_f7_split.ld"
diff --git a/src/main/target/link/stm32_flash_split.ld b/src/main/target/link/stm32_flash_split.ld
index d0f746d1da..082909493d 100644
--- a/src/main/target/link/stm32_flash_split.ld
+++ b/src/main/target/link/stm32_flash_split.ld
@@ -30,6 +30,15 @@ SECTIONS
     . = ALIGN(4);
   } >FLASH
 
+  /* System memory (read-only bootloader) interrupt vector */
+  .system_isr_vector (NOLOAD) :
+  {
+    . = ALIGN(4);
+    PROVIDE (system_isr_vector_table_base = .);
+    KEEP(*(.system_isr_vector)) /* Bootloader code */
+    . = ALIGN(4);
+  } >SYSTEM_MEMORY
+
   /* The program code and other data goes into FLASH */
   .text :
   {

From 7a32cd84175bfa6646c323288e2684068c0f68ef Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Alberto=20Garci=CC=81a=20Hierro?= <alberto@garciahierro.com>
Date: Tue, 24 Mar 2020 19:47:07 +0000
Subject: [PATCH 113/179] [USB] Avoid resetting the PWR domain during USB
 startup

This caused the RTC backup registers to de-initialize when
USB was started after persistentObjectInit
---
 src/main/vcpf4/usb_bsp.c | 3 ---
 1 file changed, 3 deletions(-)

diff --git a/src/main/vcpf4/usb_bsp.c b/src/main/vcpf4/usb_bsp.c
index c9f49fecb9..e322ec8931 100644
--- a/src/main/vcpf4/usb_bsp.c
+++ b/src/main/vcpf4/usb_bsp.c
@@ -78,9 +78,6 @@ void USB_OTG_BSP_Init(USB_OTG_CORE_HANDLE *pdev)
     RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE);
     RCC_AHB2PeriphClockCmd(RCC_AHB2Periph_OTG_FS, ENABLE) ;
 
-    /* enable the PWR clock */
-    RCC_APB1PeriphResetCmd(RCC_APB1Periph_PWR, ENABLE);
-
     EXTI_ClearITPendingBit(EXTI_Line0);
 }
 /**

From 25f483db64fd95aaf72fb86aa1cdb2e08f60a2cc Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Alberto=20Garci=CC=81a=20Hierro?= <alberto@garciahierro.com>
Date: Tue, 24 Mar 2020 20:10:02 +0000
Subject: [PATCH 114/179] [LD] Add missing SYSTEM_MEMORY section to
 flash_f7_split

---
 src/main/target/link/stm32_flash_f7_split.ld | 9 +++++++++
 1 file changed, 9 insertions(+)

diff --git a/src/main/target/link/stm32_flash_f7_split.ld b/src/main/target/link/stm32_flash_f7_split.ld
index 42d8f0205c..b3ea8ede93 100644
--- a/src/main/target/link/stm32_flash_f7_split.ld
+++ b/src/main/target/link/stm32_flash_f7_split.ld
@@ -67,6 +67,15 @@ SECTIONS
     __exidx_end = .;
   } >FLASH
 
+  /* System memory (read-only bootloader) interrupt vector */
+  .system_isr_vector (NOLOAD) :
+  {
+    . = ALIGN(4);
+    PROVIDE (system_isr_vector_table_base = .);
+    KEEP(*(.system_isr_vector)) /* Bootloader code */
+    . = ALIGN(4);
+  } >SYSTEM_MEMORY
+
   .busdev_registry :
   {
     PROVIDE_HIDDEN (__busdev_registry_start = .);

From 5b64d8c4554ded4aa37b2a71df7c2fefaa80d88a Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Alberto=20Garci=CC=81a=20Hierro?= <alberto@garciahierro.com>
Date: Tue, 24 Mar 2020 20:10:21 +0000
Subject: [PATCH 115/179] [STARTUP/BL] Unify startup/reset/bootloader handling
 across F3/F4/F7

Use MCU-independent versions of systemReset(), systemResetToBootloader()
and checkForBootLoaderRequest() based on exchanging data via persistent
storage.
---
 src/main/drivers/system.c           | 57 +++++++++++++++++++++++++++--
 src/main/drivers/system_stm32f30x.c | 38 -------------------
 src/main/drivers/system_stm32f4xx.c | 54 ---------------------------
 src/main/drivers/system_stm32f7xx.c | 53 ---------------------------
 4 files changed, 54 insertions(+), 148 deletions(-)

diff --git a/src/main/drivers/system.c b/src/main/drivers/system.c
index a94de7ddd5..e3bcdd766c 100644
--- a/src/main/drivers/system.c
+++ b/src/main/drivers/system.c
@@ -20,12 +20,13 @@
 
 #include "platform.h"
 
-#include "drivers/light_led.h"
-#include "sound_beeper.h"
-#include "drivers/nvic.h"
 #include "build/atomic.h"
 #include "build/build_config.h"
 
+#include "drivers/light_led.h"
+#include "drivers/nvic.h"
+#include "drivers/persistent.h"
+#include "drivers/sound_beeper.h"
 #include "drivers/system.h"
 #include "drivers/time.h"
 
@@ -74,6 +75,56 @@ void cycleCounterInit(void)
 }
 #endif // UNIT_TEST
 
+static inline void systemDisableAllIRQs(void)
+{
+    // We access CMSIS NVIC registers directly here
+    for (int x = 0; x < 8; x++) {
+        // Mask all IRQs controlled by a ICERx
+        NVIC->ICER[x] = 0xFFFFFFFF;
+        // Clear all pending IRQs controlled by a ICPRx
+        NVIC->ICPR[x] = 0xFFFFFFFF;
+    }
+}
+
+void systemReset(void)
+{
+    __disable_irq();
+    systemDisableAllIRQs();
+    NVIC_SystemReset();
+}
+
+void systemResetToBootloader(void)
+{
+    persistentObjectWrite(PERSISTENT_OBJECT_RESET_REASON, RESET_BOOTLOADER_REQUEST_ROM);
+    systemReset();
+}
+
+typedef void resetHandler_t(void);
+
+typedef struct isrVector_s {
+    __I uint32_t    stackEnd;
+    resetHandler_t *resetHandler;
+} isrVector_t;
+
+#pragma GCC push_options
+#pragma GCC optimize ("O0")
+void checkForBootLoaderRequest(void)
+{
+    uint32_t bootloaderRequest = persistentObjectRead(PERSISTENT_OBJECT_RESET_REASON);
+
+    if (bootloaderRequest != RESET_BOOTLOADER_REQUEST_ROM) {
+            return;
+        }
+    persistentObjectWrite(PERSISTENT_OBJECT_RESET_REASON, RESET_NONE);
+
+    extern isrVector_t system_isr_vector_table_base;
+
+    __set_MSP(system_isr_vector_table_base.stackEnd);
+    system_isr_vector_table_base.resetHandler();
+    while (1);
+}
+#pragma GCC pop_options
+
 // SysTick
 
 static volatile int sysTickPending = 0;
diff --git a/src/main/drivers/system_stm32f30x.c b/src/main/drivers/system_stm32f30x.c
index a31cb08aa9..e8d9d1c651 100644
--- a/src/main/drivers/system_stm32f30x.c
+++ b/src/main/drivers/system_stm32f30x.c
@@ -24,42 +24,8 @@
 #include "drivers/nvic.h"
 #include "drivers/system.h"
 
-#define AIRCR_VECTKEY_MASK    ((uint32_t)0x05FA0000)
 void SetSysClock(uint8_t underclock);
 
-inline static void NVIC_DisableAllIRQs(void)
-{
-    // We access CMSIS NVIC registers directly here
-    for (int x = 0; x < 8; x++) {
-        // Mask all IRQs controlled by a ICERx
-        NVIC->ICER[x] = 0xFFFFFFFF;
-        // Clear all pending IRQs controlled by a ICPRx
-        NVIC->ICPR[x] = 0xFFFFFFFF;
-    }
-}
-
-void systemReset(void)
-{
-    // Disable all NVIC interrupts
-    __disable_irq();
-    NVIC_DisableAllIRQs();
-
-    // Generate system reset
-    SCB->AIRCR = AIRCR_VECTKEY_MASK | (uint32_t)0x04;
-}
-
-void systemResetToBootloader(void)
-{
-    __disable_irq();
-    NVIC_DisableAllIRQs();
-
-    *((uint32_t *)0x20009FFC) = 0xDEADBEEF; // 40KB SRAM STM32F30X
-
-    // Generate system reset
-    SCB->AIRCR = AIRCR_VECTKEY_MASK | (uint32_t)0x04;
-}
-
-
 void enableGPIOPowerUsageAndNoiseReductions(void)
 {
     RCC_AHBPeriphClockCmd(
@@ -133,7 +99,3 @@ void systemInit(void)
     // Pre-setup SysTick and system time - final setup is done in systemClockSetup
     systemTimekeepingSetup();
 }
-
-void checkForBootLoaderRequest(void)
-{
-}
diff --git a/src/main/drivers/system_stm32f4xx.c b/src/main/drivers/system_stm32f4xx.c
index 47708e67a4..b2adb289cf 100644
--- a/src/main/drivers/system_stm32f4xx.c
+++ b/src/main/drivers/system_stm32f4xx.c
@@ -23,68 +23,14 @@
 
 #include "drivers/accgyro/accgyro_mpu.h"
 #include "drivers/exti.h"
-#include "drivers/persistent.h"
 #include "drivers/nvic.h"
 #include "drivers/system.h"
 
 #include "drivers/exti.h"
 
 
-#define AIRCR_VECTKEY_MASK    ((uint32_t)0x05FA0000)
 void SetSysClock(void);
 
-inline static void NVIC_DisableAllIRQs(void)
-{
-    // We access CMSIS NVIC registers directly here
-    for (int x = 0; x < 8; x++) {
-        // Mask all IRQs controlled by a ICERx
-        NVIC->ICER[x] = 0xFFFFFFFF;
-        // Clear all pending IRQs controlled by a ICPRx
-        NVIC->ICPR[x] = 0xFFFFFFFF;
-    }
-}
-
-typedef void resetHandler_t(void);
-
-typedef struct isrVector_s {
-    __I uint32_t    stackEnd;
-    resetHandler_t *resetHandler;
-} isrVector_t;
-
-#pragma GCC push_options
-#pragma GCC optimize ("O0")
-void checkForBootLoaderRequest(void)
-{
-    uint32_t bootloaderRequest = persistentObjectRead(PERSISTENT_OBJECT_RESET_REASON);
-
-    if (bootloaderRequest != RESET_BOOTLOADER_REQUEST_ROM) {
-            return;
-        }
-    persistentObjectWrite(PERSISTENT_OBJECT_RESET_REASON, RESET_NONE);
-
-    extern isrVector_t system_isr_vector_table_base;
-
-    __set_MSP(system_isr_vector_table_base.stackEnd);
-    system_isr_vector_table_base.resetHandler();
-    while (1);
-}
-#pragma GCC pop_options
-
-void systemReset(void)
-{
-    __disable_irq();
-    NVIC_DisableAllIRQs();
-    NVIC_SystemReset();
-}
-
-void systemResetToBootloader(void)
-{
-    persistentObjectWrite(PERSISTENT_OBJECT_RESET_REASON, RESET_BOOTLOADER_REQUEST_ROM);
-    __disable_irq();
-    NVIC_DisableAllIRQs();
-    NVIC_SystemReset();
-}
-
 void enableGPIOPowerUsageAndNoiseReductions(void)
 {
 
diff --git a/src/main/drivers/system_stm32f7xx.c b/src/main/drivers/system_stm32f7xx.c
index 62ee3b1ae1..92f417bea0 100644
--- a/src/main/drivers/system_stm32f7xx.c
+++ b/src/main/drivers/system_stm32f7xx.c
@@ -27,38 +27,8 @@
 #include "drivers/nvic.h"
 #include "drivers/system.h"
 
-
-#define AIRCR_VECTKEY_MASK    ((uint32_t)0x05FA0000)
 void SystemClock_Config(void);
 
-inline static void NVIC_DisableAllIRQs(void)
-{
-    // We access CMSIS NVIC registers directly here
-    for (int x = 0; x < 8; x++) {
-        // Mask all IRQs controlled by a ICERx
-        NVIC->ICER[x] = 0xFFFFFFFF;
-        // Clear all pending IRQs controlled by a ICPRx
-        NVIC->ICPR[x] = 0xFFFFFFFF;
-    }
-}
-
-void systemReset(void)
-{
-    __disable_irq();
-    NVIC_DisableAllIRQs();
-    NVIC_SystemReset();
-}
-
-void systemResetToBootloader(void)
-{
-    __disable_irq();
-    NVIC_DisableAllIRQs();
-
-    (*(__IO uint32_t *) (BKPSRAM_BASE + 4)) = 0xDEADBEEF;   // flag that will be readable after reboot
-
-    NVIC_SystemReset();
-}
-
 void enableGPIOPowerUsageAndNoiseReductions(void)
 {
 
@@ -128,26 +98,3 @@ void systemInit(void)
 
     HAL_SYSTICK_CLKSourceConfig(SYSTICK_CLKSOURCE_HCLK);
 }
-
-void(*bootJump)(void);
-void checkForBootLoaderRequest(void)
-{
-    uint32_t bt;
-    __PWR_CLK_ENABLE();
-    __BKPSRAM_CLK_ENABLE();
-    HAL_PWR_EnableBkUpAccess();
-
-    bt = (*(__IO uint32_t *) (BKPSRAM_BASE + 4)) ;
-    if ( bt == 0xDEADBEEF ) {
-        (*(__IO uint32_t *) (BKPSRAM_BASE + 4)) =  0xCAFEFEED; // Reset our trigger
-
-        void (*SysMemBootJump)(void);
-        __SYSCFG_CLK_ENABLE();
-        SYSCFG->MEMRMP |= SYSCFG_MEM_BOOT_ADD0 ;
-        uint32_t p =  (*((uint32_t *) 0x1ff00000));
-        __set_MSP(p); //Set the main stack pointer to its defualt values
-        SysMemBootJump = (void (*)(void)) (*((uint32_t *) 0x1ff00004)); // Point the PC to the System Memory reset vector (+4)
-        SysMemBootJump();
-        while (1);
-    }
-}

From 188de6443fd1f8023dbf2fe0200f376d68d42f00 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Alberto=20Garci=CC=81a=20Hierro?= <alberto@garciahierro.com>
Date: Tue, 24 Mar 2020 21:49:08 +0000
Subject: [PATCH 116/179] [STARTUP/BL] Use a function per MCU family to define
 the BL address

This way we avoid having to define it in all the LD scripts.
Instead, a new systemBootloaderAddress() function returns the
address where the bootloader is mapped for the current MCU.
---
 src/main/drivers/system.c                     | 17 +++++++----------
 src/main/drivers/system.h                     |  1 +
 src/main/drivers/system_stm32f30x.c           |  5 +++++
 src/main/drivers/system_stm32f4xx.c           |  5 +++++
 src/main/drivers/system_stm32f7xx.c           |  5 +++++
 src/main/target/link/stm32_flash.ld           |  9 ---------
 src/main/target/link/stm32_flash_f303_128k.ld |  2 --
 src/main/target/link/stm32_flash_f303_256k.ld |  2 --
 src/main/target/link/stm32_flash_f405.ld      |  2 --
 src/main/target/link/stm32_flash_f405_opbl.ld |  4 +---
 src/main/target/link/stm32_flash_f411.ld      |  2 --
 src/main/target/link/stm32_flash_f411_opbl.ld |  4 +---
 src/main/target/link/stm32_flash_f427.ld      |  2 --
 src/main/target/link/stm32_flash_f446.ld      |  4 +---
 src/main/target/link/stm32_flash_f722.ld      |  2 --
 src/main/target/link/stm32_flash_f745.ld      |  2 --
 src/main/target/link/stm32_flash_f746.ld      |  2 --
 src/main/target/link/stm32_flash_f765.ld      |  4 +---
 src/main/target/link/stm32_flash_f7_split.ld  |  9 ---------
 src/main/target/link/stm32_flash_split.ld     |  9 ---------
 20 files changed, 27 insertions(+), 65 deletions(-)

diff --git a/src/main/drivers/system.c b/src/main/drivers/system.c
index e3bcdd766c..f3aa27a35a 100644
--- a/src/main/drivers/system.c
+++ b/src/main/drivers/system.c
@@ -102,28 +102,25 @@ void systemResetToBootloader(void)
 typedef void resetHandler_t(void);
 
 typedef struct isrVector_s {
-    __I uint32_t    stackEnd;
+    uint32_t    stackEnd;
     resetHandler_t *resetHandler;
 } isrVector_t;
 
-#pragma GCC push_options
-#pragma GCC optimize ("O0")
+
 void checkForBootLoaderRequest(void)
 {
     uint32_t bootloaderRequest = persistentObjectRead(PERSISTENT_OBJECT_RESET_REASON);
 
     if (bootloaderRequest != RESET_BOOTLOADER_REQUEST_ROM) {
-            return;
-        }
+        return;
+    }
     persistentObjectWrite(PERSISTENT_OBJECT_RESET_REASON, RESET_NONE);
 
-    extern isrVector_t system_isr_vector_table_base;
-
-    __set_MSP(system_isr_vector_table_base.stackEnd);
-    system_isr_vector_table_base.resetHandler();
+    volatile isrVector_t *bootloaderVector = (isrVector_t *)systemBootloaderAddress();
+    __set_MSP(bootloaderVector->stackEnd);
+    bootloaderVector->resetHandler();
     while (1);
 }
-#pragma GCC pop_options
 
 // SysTick
 
diff --git a/src/main/drivers/system.h b/src/main/drivers/system.h
index 4931c0d599..87cc140690 100644
--- a/src/main/drivers/system.h
+++ b/src/main/drivers/system.h
@@ -39,6 +39,7 @@ void failureMode(failureMode_e mode);
 // bootloader/IAP
 void systemReset(void);
 void systemResetToBootloader(void);
+uint32_t systemBootloaderAddress(void);
 bool isMPUSoftReset(void);
 void cycleCounterInit(void);
 void checkForBootLoaderRequest(void);
diff --git a/src/main/drivers/system_stm32f30x.c b/src/main/drivers/system_stm32f30x.c
index e8d9d1c651..0719e576c1 100644
--- a/src/main/drivers/system_stm32f30x.c
+++ b/src/main/drivers/system_stm32f30x.c
@@ -61,6 +61,11 @@ bool isMPUSoftReset(void)
         return false;
 }
 
+uint32_t systemBootloaderAddress(void)
+{
+    return 0x1FFFD800;
+}
+
 static void systemTimekeepingSetup(void)
 {
     RCC_ClocksTypeDef clocks;
diff --git a/src/main/drivers/system_stm32f4xx.c b/src/main/drivers/system_stm32f4xx.c
index b2adb289cf..aa98183484 100644
--- a/src/main/drivers/system_stm32f4xx.c
+++ b/src/main/drivers/system_stm32f4xx.c
@@ -138,6 +138,11 @@ bool isMPUSoftReset(void)
         return false;
 }
 
+uint32_t systemBootloaderAddress(void)
+{
+    return 0x1FFF0000;
+}
+
 void systemClockSetup(uint8_t cpuUnderclock)
 {
     (void)cpuUnderclock;
diff --git a/src/main/drivers/system_stm32f7xx.c b/src/main/drivers/system_stm32f7xx.c
index 92f417bea0..9c39be23e8 100644
--- a/src/main/drivers/system_stm32f7xx.c
+++ b/src/main/drivers/system_stm32f7xx.c
@@ -61,6 +61,11 @@ bool isMPUSoftReset(void)
         return false;
 }
 
+uint32_t systemBootloaderAddress(void)
+{
+    return 0x1FF00000;
+}
+
 void systemClockSetup(uint8_t cpuUnderclock)
 {
     (void)cpuUnderclock;
diff --git a/src/main/target/link/stm32_flash.ld b/src/main/target/link/stm32_flash.ld
index 08af3aee4c..83f6aeb32c 100644
--- a/src/main/target/link/stm32_flash.ld
+++ b/src/main/target/link/stm32_flash.ld
@@ -30,15 +30,6 @@ SECTIONS
     . = ALIGN(4);
   } >FLASH
 
-  /* System memory (read-only bootloader) interrupt vector */
-  .system_isr_vector (NOLOAD) :
-  {
-    . = ALIGN(4);
-    PROVIDE (system_isr_vector_table_base = .);
-    KEEP(*(.system_isr_vector)) /* Bootloader code */
-    . = ALIGN(4);
-  } >SYSTEM_MEMORY
-
   /* The program code and other data goes into FLASH */
   .text :
   {
diff --git a/src/main/target/link/stm32_flash_f303_128k.ld b/src/main/target/link/stm32_flash_f303_128k.ld
index 320148bc5b..4274fb4a45 100644
--- a/src/main/target/link/stm32_flash_f303_128k.ld
+++ b/src/main/target/link/stm32_flash_f303_128k.ld
@@ -19,8 +19,6 @@ MEMORY
     FLASH (rx)        : ORIGIN = 0x08000000, LENGTH = 126K
     FLASH_CONFIG (r)  : ORIGIN = 0x0801F800, LENGTH = 2K
 
-    SYSTEM_MEMORY (rx): ORIGIN = 0x1FFFD800, LENGTH = 8K
-
     RAM (xrw)         : ORIGIN = 0x20000000, LENGTH = 40K
     CCM (xrw)         : ORIGIN = 0x10000000, LENGTH = 8K
     MEMORY_B1 (rx)    : ORIGIN = 0x60000000, LENGTH = 0K
diff --git a/src/main/target/link/stm32_flash_f303_256k.ld b/src/main/target/link/stm32_flash_f303_256k.ld
index d3e49fe479..1917843495 100644
--- a/src/main/target/link/stm32_flash_f303_256k.ld
+++ b/src/main/target/link/stm32_flash_f303_256k.ld
@@ -19,8 +19,6 @@ MEMORY
     FLASH (rx)        : ORIGIN = 0x08000000, LENGTH = 250K
     FLASH_CONFIG (r)  : ORIGIN = 0x0803E800, LENGTH = 6K
 
-    SYSTEM_MEMORY (rx): ORIGIN = 0x1FFFD800, LENGTH = 8K
-
     RAM (xrw)         : ORIGIN = 0x20000000, LENGTH = 40K
     CCM (xrw)         : ORIGIN = 0x10000000, LENGTH = 8K
     MEMORY_B1 (rx)    : ORIGIN = 0x60000000, LENGTH = 0K
diff --git a/src/main/target/link/stm32_flash_f405.ld b/src/main/target/link/stm32_flash_f405.ld
index 4e009241d9..66721aee5f 100644
--- a/src/main/target/link/stm32_flash_f405.ld
+++ b/src/main/target/link/stm32_flash_f405.ld
@@ -28,8 +28,6 @@ MEMORY
     FLASH (rx)        : ORIGIN = 0x08000000, LENGTH = 896K
     FLASH_CONFIG (r)  : ORIGIN = 0x080E0000, LENGTH = 128K
 
-    SYSTEM_MEMORY (rx): ORIGIN = 0x1FFF0000, LENGTH = 29K
-
     RAM (rwx)         : ORIGIN = 0x20000000, LENGTH = 128K
     CCM (rwx)         : ORIGIN = 0x10000000, LENGTH = 64K
     BACKUP_SRAM (rwx) : ORIGIN = 0x40024000, LENGTH = 4K
diff --git a/src/main/target/link/stm32_flash_f405_opbl.ld b/src/main/target/link/stm32_flash_f405_opbl.ld
index 8ba55f94f1..d12578f7db 100644
--- a/src/main/target/link/stm32_flash_f405_opbl.ld
+++ b/src/main/target/link/stm32_flash_f405_opbl.ld
@@ -29,8 +29,6 @@ MEMORY
     FLASH (rx)      : ORIGIN = 0x08004000, LENGTH = 880K
     FLASH_CONFIG (r): ORIGIN = 0x080E0000, LENGTH = 128K
 
-    SYSTEM_MEMORY (rx): ORIGIN = 0x1FFF0000, LENGTH = 29K
-
     RAM (rwx)       : ORIGIN = 0x20000000, LENGTH = 128K
     CCM (rwx)       : ORIGIN = 0x10000000, LENGTH = 64K
     MEMORY_B1 (rx)  : ORIGIN = 0x60000000, LENGTH = 0K
@@ -39,4 +37,4 @@ MEMORY
 REGION_ALIAS("STACKRAM", CCM)
 REGION_ALIAS("FASTRAM", CCM)
 
-INCLUDE "stm32_flash.ld"
+INCLUDE "stm32_flash.ld"
\ No newline at end of file
diff --git a/src/main/target/link/stm32_flash_f411.ld b/src/main/target/link/stm32_flash_f411.ld
index a4ed5003a4..0326840b34 100644
--- a/src/main/target/link/stm32_flash_f411.ld
+++ b/src/main/target/link/stm32_flash_f411.ld
@@ -30,8 +30,6 @@ MEMORY
     FLASH_CONFIG (r)  : ORIGIN = 0x08004000, LENGTH = 16K
     FLASH1 (rx)       : ORIGIN = 0x08008000, LENGTH = 480K
 
-    SYSTEM_MEMORY (rx): ORIGIN = 0x1FFF0000, LENGTH = 29K
-
     RAM (rwx)         : ORIGIN = 0x20000000, LENGTH = 128K
     MEMORY_B1 (rx)    : ORIGIN = 0x60000000, LENGTH = 0K
 }
diff --git a/src/main/target/link/stm32_flash_f411_opbl.ld b/src/main/target/link/stm32_flash_f411_opbl.ld
index b32fef22dc..af264a87e1 100644
--- a/src/main/target/link/stm32_flash_f411_opbl.ld
+++ b/src/main/target/link/stm32_flash_f411_opbl.ld
@@ -29,8 +29,6 @@ MEMORY
     FLASH (rx)        : ORIGIN = 0x08004000, LENGTH = 368K
     FLASH_CONFIG (r)  : ORIGIN = 0x08060000, LENGTH = 128K
 
-    SYSTEM_MEMORY (rx): ORIGIN = 0x1FFF0000, LENGTH = 29K
-
     RAM (rwx)         : ORIGIN = 0x20000000, LENGTH = 128K
     CCM (xrw)         : ORIGIN = 0x10000000, LENGTH = 64K
     MEMORY_B1 (rx)    : ORIGIN = 0x60000000, LENGTH = 0K
@@ -39,4 +37,4 @@ MEMORY
 REGION_ALIAS("STACKRAM", CCM)
 REGION_ALIAS("FASTRAM", CCM)
 
-INCLUDE "stm32_flash.ld"
+INCLUDE "stm32_flash.ld"
\ No newline at end of file
diff --git a/src/main/target/link/stm32_flash_f427.ld b/src/main/target/link/stm32_flash_f427.ld
index 96a0dc0969..8c080f38e7 100755
--- a/src/main/target/link/stm32_flash_f427.ld
+++ b/src/main/target/link/stm32_flash_f427.ld
@@ -48,8 +48,6 @@ MEMORY
     FLASH (rx)        : ORIGIN = 0x08000000, LENGTH = 896K
     FLASH_CONFIG (r)  : ORIGIN = 0x080E0000, LENGTH = 128K
 
-    SYSTEM_MEMORY (rx): ORIGIN = 0x1FFF0000, LENGTH = 29K
-
     RAM (rwx)         : ORIGIN = 0x20000000, LENGTH = 128K
     CCM (rwx)         : ORIGIN = 0x10000000, LENGTH = 64K
     MEMORY_B1 (rx)    : ORIGIN = 0x60000000, LENGTH = 0K
diff --git a/src/main/target/link/stm32_flash_f446.ld b/src/main/target/link/stm32_flash_f446.ld
index a4ed5003a4..1128fa3506 100644
--- a/src/main/target/link/stm32_flash_f446.ld
+++ b/src/main/target/link/stm32_flash_f446.ld
@@ -30,8 +30,6 @@ MEMORY
     FLASH_CONFIG (r)  : ORIGIN = 0x08004000, LENGTH = 16K
     FLASH1 (rx)       : ORIGIN = 0x08008000, LENGTH = 480K
 
-    SYSTEM_MEMORY (rx): ORIGIN = 0x1FFF0000, LENGTH = 29K
-
     RAM (rwx)         : ORIGIN = 0x20000000, LENGTH = 128K
     MEMORY_B1 (rx)    : ORIGIN = 0x60000000, LENGTH = 0K
 }
@@ -39,4 +37,4 @@ MEMORY
 REGION_ALIAS("STACKRAM", RAM)
 REGION_ALIAS("FASTRAM", RAM)
 
-INCLUDE "stm32_flash_split.ld"
+INCLUDE "stm32_flash_split.ld"
\ No newline at end of file
diff --git a/src/main/target/link/stm32_flash_f722.ld b/src/main/target/link/stm32_flash_f722.ld
index 457c02a330..296708678d 100644
--- a/src/main/target/link/stm32_flash_f722.ld
+++ b/src/main/target/link/stm32_flash_f722.ld
@@ -37,8 +37,6 @@ MEMORY
     FLASH_CONFIG (r)   : ORIGIN = 0x08004000, LENGTH = 16K
     FLASH1 (rx)        : ORIGIN = 0x08008000, LENGTH = 480K
 
-    SYSTEM_MEMORY (rx) : ORIGIN = 0x1FF00000, LENGTH = 59K
-
     TCM (rwx)          : ORIGIN = 0x20000000, LENGTH = 64K
     RAM (rwx)          : ORIGIN = 0x20010000, LENGTH = 192K
     MEMORY_B1 (rx)     : ORIGIN = 0x60000000, LENGTH = 0K
diff --git a/src/main/target/link/stm32_flash_f745.ld b/src/main/target/link/stm32_flash_f745.ld
index 2dc5eb8acb..a83374e4e4 100644
--- a/src/main/target/link/stm32_flash_f745.ld
+++ b/src/main/target/link/stm32_flash_f745.ld
@@ -37,8 +37,6 @@ MEMORY
     FLASH_CONFIG (r)  : ORIGIN = 0x08008000, LENGTH = 32K
     FLASH1 (rx)       : ORIGIN = 0x08010000, LENGTH = 960K
 
-    SYSTEM_MEMORY (rx) : ORIGIN = 0x1FF00000, LENGTH = 60K
-
     TCM (rwx)         : ORIGIN = 0x20000000, LENGTH = 64K
     RAM (rwx)         : ORIGIN = 0x20010000, LENGTH = 256K
     MEMORY_B1 (rx)    : ORIGIN = 0x60000000, LENGTH = 0K
diff --git a/src/main/target/link/stm32_flash_f746.ld b/src/main/target/link/stm32_flash_f746.ld
index b80cdef7e2..0c9041676f 100644
--- a/src/main/target/link/stm32_flash_f746.ld
+++ b/src/main/target/link/stm32_flash_f746.ld
@@ -36,8 +36,6 @@ MEMORY
     FLASH_CONFIG (r)  : ORIGIN = 0x08008000, LENGTH = 32K
     FLASH1 (rx)       : ORIGIN = 0x08010000, LENGTH = 960K
 
-    SYSTEM_MEMORY (rx) : ORIGIN = 0x1FF00000, LENGTH = 60K
-
     TCM (rwx)         : ORIGIN = 0x20000000, LENGTH = 64K
     RAM (rwx)         : ORIGIN = 0x20010000, LENGTH = 256K
     MEMORY_B1 (rx)    : ORIGIN = 0x60000000, LENGTH = 0K
diff --git a/src/main/target/link/stm32_flash_f765.ld b/src/main/target/link/stm32_flash_f765.ld
index eaf3209d45..86e394c673 100644
--- a/src/main/target/link/stm32_flash_f765.ld
+++ b/src/main/target/link/stm32_flash_f765.ld
@@ -38,8 +38,6 @@ MEMORY
     AXIM_FLASH_CFG (r)      : ORIGIN = 0x08008000, LENGTH = 32K
     AXIM_FLASH1 (rx)        : ORIGIN = 0x08010000, LENGTH = 1984K
 
-    SYSTEM_MEMORY (rx)      : ORIGIN = 0x1FF00000, LENGTH = 59K
-
     DTCM_RAM (rwx)          : ORIGIN = 0x20000000, LENGTH = 128K
     SRAM1 (rwx)             : ORIGIN = 0x20020000, LENGTH = 368K
     SRAM2 (rwx)             : ORIGIN = 0x2007C000, LENGTH = 16K
@@ -54,4 +52,4 @@ REGION_ALIAS("STACKRAM", DTCM_RAM)
 REGION_ALIAS("FASTRAM",  DTCM_RAM)
 REGION_ALIAS("RAM",      SRAM1)
 
-INCLUDE "stm32_flash_f7_split.ld"
+INCLUDE "stm32_flash_f7_split.ld"
\ No newline at end of file
diff --git a/src/main/target/link/stm32_flash_f7_split.ld b/src/main/target/link/stm32_flash_f7_split.ld
index b3ea8ede93..42d8f0205c 100644
--- a/src/main/target/link/stm32_flash_f7_split.ld
+++ b/src/main/target/link/stm32_flash_f7_split.ld
@@ -67,15 +67,6 @@ SECTIONS
     __exidx_end = .;
   } >FLASH
 
-  /* System memory (read-only bootloader) interrupt vector */
-  .system_isr_vector (NOLOAD) :
-  {
-    . = ALIGN(4);
-    PROVIDE (system_isr_vector_table_base = .);
-    KEEP(*(.system_isr_vector)) /* Bootloader code */
-    . = ALIGN(4);
-  } >SYSTEM_MEMORY
-
   .busdev_registry :
   {
     PROVIDE_HIDDEN (__busdev_registry_start = .);
diff --git a/src/main/target/link/stm32_flash_split.ld b/src/main/target/link/stm32_flash_split.ld
index 082909493d..d0f746d1da 100644
--- a/src/main/target/link/stm32_flash_split.ld
+++ b/src/main/target/link/stm32_flash_split.ld
@@ -30,15 +30,6 @@ SECTIONS
     . = ALIGN(4);
   } >FLASH
 
-  /* System memory (read-only bootloader) interrupt vector */
-  .system_isr_vector (NOLOAD) :
-  {
-    . = ALIGN(4);
-    PROVIDE (system_isr_vector_table_base = .);
-    KEEP(*(.system_isr_vector)) /* Bootloader code */
-    . = ALIGN(4);
-  } >SYSTEM_MEMORY
-
   /* The program code and other data goes into FLASH */
   .text :
   {

From 9a55d7dbf869f8829b82dc8bc7555216cafacd62 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Alberto=20Garci=CC=81a=20Hierro?= <alberto@garciahierro.com>
Date: Mon, 27 Apr 2020 12:04:43 +0100
Subject: [PATCH 117/179] [DRIVERS] Split time related functions from system.c
 to time.c

This allows pulling in time.c instead of system.c for the time
tests, which has less platform specific code. We alredy had the
time functions declared in drivers/time.h, so introducing a
drivers/time.c seems like the cleanest solution.
---
 make/source.mk            |   3 +-
 src/main/drivers/system.c | 141 +------------------------------
 src/main/drivers/time.c   | 172 ++++++++++++++++++++++++++++++++++++++
 src/test/Makefile         |  12 +--
 4 files changed, 181 insertions(+), 147 deletions(-)
 create mode 100644 src/main/drivers/time.c

diff --git a/make/source.mk b/make/source.mk
index 2637a2fb4a..bc9e55981c 100644
--- a/make/source.mk
+++ b/make/source.mk
@@ -64,6 +64,7 @@ COMMON_SRC = \
             drivers/sound_beeper.c \
             drivers/stack_check.c \
             drivers/system.c \
+            drivers/time.c \
             drivers/timer.c \
             drivers/usb_msc.c \
             drivers/lights_io.c \
@@ -290,4 +291,4 @@ ifneq ($(TARGET),$(filter $(TARGET),$(F3_TARGETS)))
 # SPEED_OPTIMISED_SRC := $(SPEED_OPTIMISED_SRC) \
             # ./src/main/common/maths.c \
 
-endif #!F3
\ No newline at end of file
+endif #!F3
diff --git a/src/main/drivers/system.c b/src/main/drivers/system.c
index f3aa27a35a..55691bb291 100644
--- a/src/main/drivers/system.c
+++ b/src/main/drivers/system.c
@@ -20,11 +20,9 @@
 
 #include "platform.h"
 
-#include "build/atomic.h"
 #include "build/build_config.h"
 
 #include "drivers/light_led.h"
-#include "drivers/nvic.h"
 #include "drivers/persistent.h"
 #include "drivers/sound_beeper.h"
 #include "drivers/system.h"
@@ -49,17 +47,12 @@ void registerExtiCallbackHandler(IRQn_Type irqn, extiCallbackHandlerFunc *fn)
     failureMode(FAILURE_DEVELOPER); // EXTI_CALLBACK_HANDLER_COUNT is too low for the amount of handlers required.
 }
 
-// cycles per microsecond, this is deliberately uint32_t to avoid type conversions
-STATIC_UNIT_TESTED  uint32_t usTicks = 0;
-// current uptime for 1kHz systick timer. will rollover after 49 days. hopefully we won't care.
-STATIC_UNIT_TESTED volatile timeMs_t sysTickUptime = 0;
-STATIC_UNIT_TESTED volatile uint32_t sysTickValStamp = 0;
 // cached value of RCC->CSR
 uint32_t cachedRccCsrValue;
 
-#ifndef UNIT_TEST
 void cycleCounterInit(void)
 {
+    extern uint32_t usTicks; // From drivers/time.h
 #if defined(USE_HAL_DRIVER)
     usTicks = HAL_RCC_GetSysClockFreq() / 1000000;
 #else
@@ -73,7 +66,6 @@ void cycleCounterInit(void)
     CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
     DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk;
 }
-#endif // UNIT_TEST
 
 static inline void systemDisableAllIRQs(void)
 {
@@ -122,137 +114,6 @@ void checkForBootLoaderRequest(void)
     while (1);
 }
 
-// SysTick
-
-static volatile int sysTickPending = 0;
-
-void SysTick_Handler(void)
-{
-    ATOMIC_BLOCK(NVIC_PRIO_MAX) {
-        sysTickUptime++;
-        sysTickValStamp = SysTick->VAL;
-        sysTickPending = 0;
-        (void)(SysTick->CTRL);
-    }
-#ifdef USE_HAL_DRIVER
-    // used by the HAL for some timekeeping and timeouts, should always be 1ms
-    HAL_IncTick();
-#endif
-}
-
-uint32_t ticks(void)
-{
-#ifdef UNIT_TEST
-    return 0;
-#else
-    CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
-    return DWT->CYCCNT;
-#endif
-}
-
-timeDelta_t ticks_diff_us(uint32_t begin, uint32_t end)
-{
-    return (end - begin) / usTicks;
-}
-
-// Return system uptime in microseconds
-timeUs_t microsISR(void)
-{
-    register uint32_t ms, pending, cycle_cnt;
-
-    ATOMIC_BLOCK(NVIC_PRIO_MAX) {
-        cycle_cnt = SysTick->VAL;
-
-        if (SysTick->CTRL & SysTick_CTRL_COUNTFLAG_Msk) {
-            // Update pending.
-            // Record it for multiple calls within the same rollover period
-            // (Will be cleared when serviced).
-            // Note that multiple rollovers are not considered.
-
-            sysTickPending = 1;
-
-            // Read VAL again to ensure the value is read after the rollover.
-
-            cycle_cnt = SysTick->VAL;
-        }
-
-        ms = sysTickUptime;
-        pending = sysTickPending;
-    }
-
-    // XXX: Be careful to not trigger 64 bit division
-    const uint32_t partial = (usTicks * 1000U - cycle_cnt) / usTicks;
-    return ((timeUs_t)(ms + pending) * 1000LL) + ((timeUs_t)partial);
-}
-
-timeUs_t micros(void)
-{
-    register uint32_t ms, cycle_cnt;
-
-    // Call microsISR() in interrupt and elevated (non-zero) BASEPRI context
-
-#ifndef UNIT_TEST
-    if ((SCB->ICSR & SCB_ICSR_VECTACTIVE_Msk) || (__get_BASEPRI())) {
-        return microsISR();
-    }
-#endif
-
-    do {
-        ms = sysTickUptime;
-        cycle_cnt = SysTick->VAL;
-    } while (ms != sysTickUptime || cycle_cnt > sysTickValStamp);
-
-    // XXX: Be careful to not trigger 64 bit division
-    const uint32_t partial = (usTicks * 1000U - cycle_cnt) / usTicks;
-    return ((timeUs_t)ms * 1000LL) + ((timeUs_t)partial);
-}
-
-// Return system uptime in milliseconds (rollover in 49 days)
-timeMs_t millis(void)
-{
-    return sysTickUptime;
-}
-
-#if 1
-void delayMicroseconds(timeUs_t us)
-{
-    timeUs_t now = micros();
-    while (micros() - now < us);
-}
-#else
-void delayMicroseconds(timeUs_t us)
-{
-    uint32_t elapsed = 0;
-    uint32_t lastCount = SysTick->VAL;
-
-    for (;;) {
-        register uint32_t current_count = SysTick->VAL;
-        timeUs_t elapsed_us;
-
-        // measure the time elapsed since the last time we checked
-        elapsed += current_count - lastCount;
-        lastCount = current_count;
-
-        // convert to microseconds
-        elapsed_us = elapsed / usTicks;
-        if (elapsed_us >= us)
-            break;
-
-        // reduce the delay by the elapsed time
-        us -= elapsed_us;
-
-        // keep fractional microseconds for the next iteration
-        elapsed %= usTicks;
-    }
-}
-#endif
-
-void delay(timeMs_t ms)
-{
-    while (ms--)
-        delayMicroseconds(1000);
-}
-
 #define SHORT_FLASH_DURATION 50
 #define CODE_FLASH_DURATION 250
 
diff --git a/src/main/drivers/time.c b/src/main/drivers/time.c
new file mode 100644
index 0000000000..9fcbc78625
--- /dev/null
+++ b/src/main/drivers/time.c
@@ -0,0 +1,172 @@
+/*
+ * This file is part of INAV.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute 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.
+ *
+ * This file 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 this program. If not, see http://www.gnu.org/licenses/.
+ *
+ */
+
+#include <stdint.h>
+
+#include "platform.h"
+
+
+#include "build/atomic.h"
+
+#include "drivers/nvic.h"
+#include "drivers/time.h"
+
+// cycles per microsecond, this is deliberately uint32_t to avoid type conversions
+// This is not static so system.c can set it up for us.
+uint32_t usTicks = 0;
+// current uptime for 1kHz systick timer. will rollover after 49 days. hopefully we won't care.
+STATIC_UNIT_TESTED volatile timeMs_t sysTickUptime = 0;
+STATIC_UNIT_TESTED volatile uint32_t sysTickValStamp = 0;
+
+// Return system uptime in milliseconds (rollover in 49 days)
+timeMs_t millis(void)
+{
+    return sysTickUptime;
+}
+
+// SysTick
+
+static volatile int sysTickPending = 0;
+
+void SysTick_Handler(void)
+{
+    ATOMIC_BLOCK(NVIC_PRIO_MAX) {
+        sysTickUptime++;
+        sysTickValStamp = SysTick->VAL;
+        sysTickPending = 0;
+        (void)(SysTick->CTRL);
+    }
+#ifdef USE_HAL_DRIVER
+    // used by the HAL for some timekeeping and timeouts, should always be 1ms
+    HAL_IncTick();
+#endif
+}
+
+uint32_t ticks(void)
+{
+#ifdef UNIT_TEST
+    return 0;
+#else
+    CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
+    return DWT->CYCCNT;
+#endif
+}
+
+timeDelta_t ticks_diff_us(uint32_t begin, uint32_t end)
+{
+    return (end - begin) / usTicks;
+}
+
+// Return system uptime in microseconds
+timeUs_t microsISR(void)
+{
+    register uint32_t ms, pending, cycle_cnt;
+
+    ATOMIC_BLOCK(NVIC_PRIO_MAX) {
+        cycle_cnt = SysTick->VAL;
+
+        if (SysTick->CTRL & SysTick_CTRL_COUNTFLAG_Msk) {
+            // Update pending.
+            // Record it for multiple calls within the same rollover period
+            // (Will be cleared when serviced).
+            // Note that multiple rollovers are not considered.
+
+            sysTickPending = 1;
+
+            // Read VAL again to ensure the value is read after the rollover.
+
+            cycle_cnt = SysTick->VAL;
+        }
+
+        ms = sysTickUptime;
+        pending = sysTickPending;
+    }
+
+    // XXX: Be careful to not trigger 64 bit division
+    const uint32_t partial = (usTicks * 1000U - cycle_cnt) / usTicks;
+    return ((timeUs_t)(ms + pending) * 1000LL) + ((timeUs_t)partial);
+}
+
+timeUs_t micros(void)
+{
+    register uint32_t ms, cycle_cnt;
+
+    // Call microsISR() in interrupt and elevated (non-zero) BASEPRI context
+
+#ifndef UNIT_TEST
+    if ((SCB->ICSR & SCB_ICSR_VECTACTIVE_Msk) || (__get_BASEPRI())) {
+        return microsISR();
+    }
+#endif
+
+    do {
+        ms = sysTickUptime;
+        cycle_cnt = SysTick->VAL;
+    } while (ms != sysTickUptime || cycle_cnt > sysTickValStamp);
+
+    // XXX: Be careful to not trigger 64 bit division
+    const uint32_t partial = (usTicks * 1000U - cycle_cnt) / usTicks;
+    return ((timeUs_t)ms * 1000LL) + ((timeUs_t)partial);
+}
+
+#if 1
+void delayMicroseconds(timeUs_t us)
+{
+    timeUs_t now = micros();
+    while (micros() - now < us);
+}
+#else
+void delayMicroseconds(timeUs_t us)
+{
+    uint32_t elapsed = 0;
+    uint32_t lastCount = SysTick->VAL;
+
+    for (;;) {
+        register uint32_t current_count = SysTick->VAL;
+        timeUs_t elapsed_us;
+
+        // measure the time elapsed since the last time we checked
+        elapsed += current_count - lastCount;
+        lastCount = current_count;
+
+        // convert to microseconds
+        elapsed_us = elapsed / usTicks;
+        if (elapsed_us >= us)
+            break;
+
+        // reduce the delay by the elapsed time
+        us -= elapsed_us;
+
+        // keep fractional microseconds for the next iteration
+        elapsed %= usTicks;
+    }
+}
+#endif
+
+void delay(timeMs_t ms)
+{
+    while (ms--)
+        delayMicroseconds(1000);
+}
diff --git a/src/test/Makefile b/src/test/Makefile
index 444c5ba065..676bd496e7 100644
--- a/src/test/Makefile
+++ b/src/test/Makefile
@@ -701,24 +701,24 @@ $(OBJECT_DIR)/rcdevice_unittest : \
 
 	$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
 
-$(OBJECT_DIR)/drivers/system.o : \
-	$(USER_DIR)/drivers/system.c \
-	$(USER_DIR)/drivers/system.h \
+$(OBJECT_DIR)/drivers/time.o : \
+	$(USER_DIR)/drivers/time.c \
+	$(USER_DIR)/drivers/time.h \
 	$(GTEST_HEADERS)
 
 	@mkdir -p $(dir $@)
-	$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/drivers/system.c -o $@
+	$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/drivers/time.c -o $@
 
 $(OBJECT_DIR)/time_unittest.o : \
 	$(TEST_DIR)/time_unittest.cc \
-	$(USER_DIR)/drivers/system.h \
+	$(USER_DIR)/drivers/time.h \
 	$(GTEST_HEADERS)
 
 	@mkdir -p $(dir $@)
 	$(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/time_unittest.cc -o $@
 
 $(OBJECT_DIR)/time_unittest : \
-	$(OBJECT_DIR)/drivers/system.o \
+	$(OBJECT_DIR)/drivers/time.o \
 	$(OBJECT_DIR)/time_unittest.o \
 	$(OBJECT_DIR)/gtest_main.a
 

From e9a88332a62b6b707bff95cf0866b8cb4cacb37d Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Niccol=C3=B2=20Maggioni?= <nicco.maggioni@gmail.com>
Date: Mon, 27 Apr 2020 16:20:10 +0200
Subject: [PATCH 118/179] Use non-root user in Docker build and update docs
 (#5355)

* Use non-root user in Docker build and update docs

* Exclude cruft from Docker build context
---
 .dockerignore                          |  2 ++
 Dockerfile                             |  3 +++
 docs/development/Building in Docker.md | 30 +++++++++++++++++---------
 3 files changed, 25 insertions(+), 10 deletions(-)
 create mode 100644 .dockerignore

diff --git a/.dockerignore b/.dockerignore
new file mode 100644
index 0000000000..f6718b745c
--- /dev/null
+++ b/.dockerignore
@@ -0,0 +1,2 @@
+.vagrant/
+obj/
diff --git a/Dockerfile b/Dockerfile
index c699617788..746733bbeb 100644
--- a/Dockerfile
+++ b/Dockerfile
@@ -21,3 +21,6 @@ RUN mkdir -p /opt && \
 	chmod -R -w "/opt/gcc-arm-none-eabi-$TOOLCHAIN_VERSION_LONG"
 
 ENV PATH="/opt/gcc-arm-none-eabi-$TOOLCHAIN_VERSION_LONG/bin:$PATH"
+
+RUN useradd inav
+USER inav
diff --git a/docs/development/Building in Docker.md b/docs/development/Building in Docker.md
index fee0b31acf..8fc0b3f704 100755
--- a/docs/development/Building in Docker.md	
+++ b/docs/development/Building in Docker.md	
@@ -1,24 +1,34 @@
 # Building with Docker
 
-Building in Docker is remarkably easy.
+Building with [Docker](https://www.docker.com/) is remarkably easy: an isolated container will hold all the needed compilation tools so that they won't interfere with your system and you won't need to install and manage them by yourself. You'll only need to have Docker itself [installed](https://docs.docker.com/install/).
 
-## Build a container with toolchain
+The first time that you'll run a build it will take a little more time than following executions since it will be building its base image first. Once this initial process is completed, the firmware will be always built immediately.
+
+If you want to start from scratch - _even if that's rarely needed_ - delete the `inav-build` image on your system (`docker image rm inav-build`).
+
+## Linux
+
+In the repo's root, run:
 
 ```
-docker build -t inav-build .
+./build.sh <TARGET>
 ```
 
-## Building firmware with Docker on Ubuntu
+Where `<TARGET>` must be replaced with the name of the target that you want to build. For example:
 
-Build specified target
 ```
-sh build.sh SPRACINGF3
+./build.sh MATEKF405SE
 ```
 
-## Building firmware with Docker on Windows 10
+## Windows 10
 
-Path in Docker on Windows works in a _strange_ way, so you have to provide full path for `docker run` command. For example:
+Docker on Windows requires full paths for mounting volumes in `docker run` commands. For example: `c:\Users\pspyc\Documents\Projects\inav` becomes `//c/Users/pspyc/Documents/Projects/inav` .
 
-`docker run --rm -v //c/Users/pspyc/Documents/Projects/inav:/home/src/ inav-build make TARGET=AIRBOTF4`
+You'll have to manually execute the same steps that the build script does:
 
-So, `c:\Users\pspyc\Documents\Projects\inav` becomes `//c/Users/pspyc/Documents/Projects/inav`
+1. `docker build -t inav-build .`
+   + This step is only needed the first time.
+2. `docker run --rm -v <PATH_TO_REPO>:/home/src/ inav-build make TARGET=<TARGET>`
+   + Where `<PATH_TO_REPO>` must be replaced with the absolute path of where you cloned this repo (see above), and `<TARGET>` with the name of the target that you want to build.
+
+Refer to the [Linux](#Linux) instructions or the [build script](/build.sh) for more details.
\ No newline at end of file

From 4a5442297c1240994cc946dbf17945cede4e1a6e Mon Sep 17 00:00:00 2001
From: Arnaud Morin <arnaud.morin@gmail.com>
Date: Sun, 26 Apr 2020 14:21:07 +0200
Subject: [PATCH 119/179] Fix wrong link to sparky (space issue)

Space in markdown links should be url encoded
---
 docs/Battery.md | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/docs/Battery.md b/docs/Battery.md
index 1b5e23ebd3..f7277b264a 100644
--- a/docs/Battery.md
+++ b/docs/Battery.md
@@ -26,7 +26,7 @@ On the first battery connection is always advisable to use a current limiter dev
 
 ### Sparky
 
-See the [Sparky board chapter](Board - Sparky.md).
+See the [Sparky board chapter](Board%20-%20Sparky.md).
 
 ## Voltage measurement
 

From badc6c78acc3f194b256067e7902633ef01afb0a Mon Sep 17 00:00:00 2001
From: Michel Pastor <shellixyz@users.noreply.github.com>
Date: Tue, 28 Apr 2020 18:15:30 +0200
Subject: [PATCH 120/179] Fix CRC algo for FrSky S.Port and F.Port (#5641)

---
 make/source.mk                 |  1 +
 src/main/rx/fport.c            | 17 ++----------
 src/main/rx/frsky_crc.c        | 47 ++++++++++++++++++++++++++++++++++
 src/main/rx/frsky_crc.h        | 25 ++++++++++++++++++
 src/main/telemetry/smartport.c |  8 +++---
 5 files changed, 80 insertions(+), 18 deletions(-)
 create mode 100644 src/main/rx/frsky_crc.c
 create mode 100644 src/main/rx/frsky_crc.h

diff --git a/make/source.mk b/make/source.mk
index 7c2ebd2a27..2e8106da6c 100644
--- a/make/source.mk
+++ b/make/source.mk
@@ -131,6 +131,7 @@ COMMON_SRC = \
             rx/nrf24_syma.c \
             rx/nrf24_v202.c \
             rx/pwm.c \
+            rx/frsky_crc.c \
             rx/rx.c \
             rx/rx_spi.c \
             rx/sbus.c \
diff --git a/src/main/rx/fport.c b/src/main/rx/fport.c
index b9a8144b31..59f62fe3f5 100644
--- a/src/main/rx/fport.c
+++ b/src/main/rx/fport.c
@@ -39,6 +39,7 @@ FILE_COMPILE_FOR_SPEED
 #include "telemetry/smartport.h"
 #endif
 
+#include "rx/frsky_crc.h"
 #include "rx/rx.h"
 #include "rx/sbus_channels.h"
 #include "rx/fport.h"
@@ -57,8 +58,6 @@ FILE_COMPILE_FOR_SPEED
 #define FPORT_ESCAPE_CHAR 0x7D
 #define FPORT_ESCAPE_MASK 0x20
 
-#define FPORT_CRC_VALUE 0xFF
-
 #define FPORT_BAUDRATE 115200
 
 #define FPORT_PORT_OPTIONS (SERIAL_STOPBITS_1 | SERIAL_PARITY_NO)
@@ -239,18 +238,6 @@ static void smartPortWriteFrameFport(const smartPortPayload_t *payload)
 }
 #endif
 
-static bool checkChecksum(uint8_t *data, uint8_t length)
-{
-    uint16_t checksum = 0;
-    for (unsigned i = 0; i < length; i++) {
-        checksum = checksum + *(uint8_t *)(data + i);
-    }
-
-    checksum = (checksum & 0xff) + (checksum >> 8);
-
-    return checksum == FPORT_CRC_VALUE;
-}
-
 static uint8_t fportFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
 {
 #if defined(USE_TELEMETRY_SMARTPORT)
@@ -268,7 +255,7 @@ static uint8_t fportFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
         if (frameLength != bufferLength - 2) {
             reportFrameError(DEBUG_FPORT_ERROR_SIZE);
         } else {
-            if (!checkChecksum(&rxBuffer[rxBufferReadIndex].data[0], bufferLength)) {
+            if (!frskyChecksumIsGood(&rxBuffer[rxBufferReadIndex].data[0], bufferLength)) {
                 reportFrameError(DEBUG_FPORT_ERROR_CHECKSUM);
             } else {
                 fportFrame_t *frame = (fportFrame_t *)&rxBuffer[rxBufferReadIndex].data[1];
diff --git a/src/main/rx/frsky_crc.c b/src/main/rx/frsky_crc.c
new file mode 100644
index 0000000000..fa3934e574
--- /dev/null
+++ b/src/main/rx/frsky_crc.c
@@ -0,0 +1,47 @@
+/*
+ * 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 <stdbool.h>
+#include <stdint.h>
+
+#include "platform.h"
+FILE_COMPILE_FOR_SPEED
+
+#include "rx/frsky_crc.h"
+
+void frskyCheckSumStep(uint16_t *checksum, uint8_t byte)
+{
+    *checksum += byte;
+    *checksum += (*checksum >> 8);
+    *checksum &= 0xFF;
+}
+
+void frskyCheckSumFini(uint16_t *checksum)
+{
+    *checksum = 0xFF - *checksum;
+}
+
+bool frskyChecksumIsGood(uint8_t *data, uint8_t length)
+{
+    uint16_t checksum = 0;
+    for (unsigned i = 0; i < length; i++) {
+        frskyCheckSumStep(&checksum, *data++);
+    }
+
+    return checksum == FRSKY_CHECKSUM_GOOD_VALUE;
+}
+
diff --git a/src/main/rx/frsky_crc.h b/src/main/rx/frsky_crc.h
new file mode 100644
index 0000000000..849b53bfab
--- /dev/null
+++ b/src/main/rx/frsky_crc.h
@@ -0,0 +1,25 @@
+/*
+ * 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 <stdbool.h>
+#include <stdint.h>
+
+#define FRSKY_CHECKSUM_GOOD_VALUE 0xFF
+
+bool frskyChecksumIsGood(uint8_t *data, uint8_t length);
+void frskyCheckSumStep(uint16_t *checksum, uint8_t byte);   // Add byte to checksum
+void frskyCheckSumFini(uint16_t *checksum);                 // Finalize checksum
diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c
index 0d7a0bfe04..dd980d242f 100755
--- a/src/main/telemetry/smartport.c
+++ b/src/main/telemetry/smartport.c
@@ -44,6 +44,8 @@ FILE_COMPILE_FOR_SPEED
 
 #include "navigation/navigation.h"
 
+#include "rx/frsky_crc.h"
+
 #include "sensors/boardalignment.h"
 #include "sensors/sensors.h"
 #include "sensors/battery.h"
@@ -233,7 +235,7 @@ void smartPortSendByte(uint8_t c, uint16_t *checksum, serialPort_t *port)
     }
 
     if (checksum != NULL) {
-        *checksum += c;
+        frskyCheckSumStep(checksum, c);
     }
 }
 
@@ -248,8 +250,8 @@ void smartPortWriteFrameSerial(const smartPortPayload_t *payload, serialPort_t *
     for (unsigned i = 0; i < sizeof(smartPortPayload_t); i++) {
         smartPortSendByte(*data++, &checksum, port);
     }
-    checksum = 0xff - ((checksum & 0xff) + (checksum >> 8));
-    smartPortSendByte((uint8_t)checksum, NULL, port);
+    frskyCheckSumFini(&checksum);
+    smartPortSendByte(checksum, NULL, port);
 }
 
 static void smartPortWriteFrameInternal(const smartPortPayload_t *payload)

From 7a8c7f5330a0517caef15f6179443c6773da3991 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Tue, 28 Apr 2020 21:39:04 +0200
Subject: [PATCH 121/179] Apply Dterm filters on derivative, not error

---
 src/main/flight/pid.c | 16 ++++++++--------
 1 file changed, 8 insertions(+), 8 deletions(-)

diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 740fd28a77..599ca7d438 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -716,18 +716,18 @@ static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pid
         newDTerm = 0;
     } else {
         // Calculate delta for Dterm calculation. Apply filters before derivative to minimize effects of dterm kick
-        float deltaFiltered = pidProfile()->dterm_setpoint_weight * pidState->rateTarget - pidState->gyroRate;
-
-        // Apply D-term notch
-        deltaFiltered = notchFilterApplyFn(&pidState->deltaNotchFilter, deltaFiltered);
-
-        // Apply additional lowpass
-        deltaFiltered = dTermLpfFilterApplyFn((filter_t *) &pidState->dtermLpfState, deltaFiltered);
-        deltaFiltered = dTermLpf2FilterApplyFn((filter_t *) &pidState->dtermLpf2State, deltaFiltered);
+        const float deltaFiltered = pidProfile()->dterm_setpoint_weight * pidState->rateTarget - pidState->gyroRate;
 
         firFilterUpdate(&pidState->gyroRateFilter, deltaFiltered);
         newDTerm = firFilterApply(&pidState->gyroRateFilter);
 
+        // Apply D-term notch
+        newDTerm = notchFilterApplyFn(&pidState->deltaNotchFilter, newDTerm);
+
+        // Apply additional lowpass
+        newDTerm = dTermLpfFilterApplyFn((filter_t *) &pidState->dtermLpfState, newDTerm);
+        newDTerm = dTermLpf2FilterApplyFn((filter_t *) &pidState->dtermLpf2State, newDTerm);
+
         // Calculate derivative
         newDTerm =  newDTerm * (pidState->kD / dT) * applyDBoost(pidState, dT);
 

From 974e4181341e78ad55b2da1916d3fdb0eb8cc969 Mon Sep 17 00:00:00 2001
From: Stefan Naewe <snaewe@users.noreply.github.com>
Date: Wed, 29 Apr 2020 11:13:31 +0200
Subject: [PATCH 122/179] CLI docs: update default value of inav_reset_home

Document the new default value of `inav_reset_home` that changed in commit a2fdcdb8cf ("Set default home reset to first arm", 2019-03-11).

Signed-off-by: Stefan Naewe <stefan.naewe@gmail.com>
---
 docs/Cli.md | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/docs/Cli.md b/docs/Cli.md
index 8081f18968..b292304274 100644
--- a/docs/Cli.md
+++ b/docs/Cli.md
@@ -163,7 +163,7 @@ A shorter form is also supported to enable and disable functions using `serial <
 |  inav_gravity_cal_tolerance  | 5 | Unarmed gravity calibration tolerance level. Won't finish the calibration until estimated gravity error falls below this value. |
 |  inav_use_gps_velned  | ON | Defined if iNav should use velocity data provided by GPS module for doing position and speed estimation. If set to OFF iNav will fallback to calculating velocity from GPS coordinates. Using native velocity data may improve performance on some GPS modules. Some GPS modules introduce significant delay and using native velocity may actually result in much worse performance. |
 |  inav_reset_altitude | FIRST_ARM | Defines when relative estimated altitude is reset to zero. Variants - `NEVER` (once reference is acquired it's used regardless); `FIRST_ARM` (keep altitude at zero until firstly armed), `EACH_ARM` (altitude is reset to zero on each arming) |
-|  inav_reset_home | EACH_ARM | Allows to chose when the home position is reset. Can help prevent resetting home position after accidental mid-air disarm. Possible values are: NEVER, FIRST_ARM and EACH_ARM |
+|  inav_reset_home | FIRST_ARM | Allows to chose when the home position is reset. Can help prevent resetting home position after accidental mid-air disarm. Possible values are: NEVER, FIRST_ARM and EACH_ARM |
 |  inav_max_surface_altitude  | 200 | Max allowed altitude for surface following mode. [cm] |
 |  inav_w_z_baro_p  | 0.350 | Weight of barometer measurements in estimated altitude and climb rate |
 |  inav_w_z_gps_p  | 0.200 | Weight of GPS altitude measurements in estimated altitude. Setting is used only of airplanes |

From ab8f2105cddb8720946537ffdb52c2ff6f790763 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Thu, 30 Apr 2020 14:59:05 +0200
Subject: [PATCH 123/179] Add a infor that F3 are EOL

---
 README.md | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/README.md b/README.md
index 73d8f3ce6a..aa996c6788 100644
--- a/README.md
+++ b/README.md
@@ -1,5 +1,9 @@
 # INAV - navigation capable flight controller
 
+## F3 based flight controllers
+
+> STM32 F3 flight controllers like Omnibus F3 or SP Racing F3 are deprecated and soon they will reach the end of support in INAV. If you are still using F3 boards, please migrate to F4 or F7.
+
 ![INAV](http://static.rcgroups.net/forums/attachments/6/1/0/3/7/6/a9088858-102-inav.png)
 ![Travis CI status](https://travis-ci.org/iNavFlight/inav.svg?branch=master)
 

From c8a62def8b0accaf882b6c536444789f906f69ec Mon Sep 17 00:00:00 2001
From: "Konstantin (DigitalEntity) Sharlaimov"
 <konstantin.sharlaimov@gmail.com>
Date: Thu, 30 Apr 2020 19:54:58 +0200
Subject: [PATCH 124/179] [TARGET] Add target for Omnibus F7 Nano V7

---
 make/release.mk                             |  2 +-
 src/main/target/AIRBOTF7/OMNIBUSF7NANOV7.mk |  0
 src/main/target/AIRBOTF7/target.c           |  3 +
 src/main/target/AIRBOTF7/target.h           | 51 ++-----------
 src/main/target/AIRBOTF7/target_abf7.h      | 81 +++++++++++++++++++++
 src/main/target/AIRBOTF7/target_o7v7.h      | 72 ++++++++++++++++++
 6 files changed, 162 insertions(+), 47 deletions(-)
 create mode 100644 src/main/target/AIRBOTF7/OMNIBUSF7NANOV7.mk
 create mode 100644 src/main/target/AIRBOTF7/target_abf7.h
 create mode 100644 src/main/target/AIRBOTF7/target_o7v7.h

diff --git a/make/release.mk b/make/release.mk
index 74e5dde8f6..d664fb908f 100644
--- a/make/release.mk
+++ b/make/release.mk
@@ -10,7 +10,7 @@ RELEASE_TARGETS += KAKUTEF4 KAKUTEF4V2 KAKUTEF7 KAKUTEF7MINI KAKUTEF7HDV
 
 RELEASE_TARGETS += SPRACINGF3 SPRACINGF3EVO SPRACINGF3EVO_1SS SPRACINGF3MINI SPRACINGF4EVO SPRACINGF7DUAL
 
-RELEASE_TARGETS += OMNIBUS AIRBOTF4 ASGARD32F4 ASGARD32F7 FIREWORKSV2 AIRBOTF7
+RELEASE_TARGETS += OMNIBUS AIRBOTF4 ASGARD32F4 ASGARD32F7 FIREWORKSV2 AIRBOTF7 OMNIBUSF7NANOV7
 RELEASE_TARGETS += OMNIBUSF4 OMNIBUSF4PRO OMNIBUSF4PRO_LEDSTRIPM5 DYSF4PRO DYSF4PROV2
 RELEASE_TARGETS += OMNIBUSF4V3 OMNIBUSF4V3_S6_SS OMNIBUSF4V3_S5S6_SS OMNIBUSF4V3_S5_S6_2SS
 RELEASE_TARGETS += OMNIBUSF7 OMNIBUSF7V2 OMNIBUSF7NXT YUPIF7
diff --git a/src/main/target/AIRBOTF7/OMNIBUSF7NANOV7.mk b/src/main/target/AIRBOTF7/OMNIBUSF7NANOV7.mk
new file mode 100644
index 0000000000..e69de29bb2
diff --git a/src/main/target/AIRBOTF7/target.c b/src/main/target/AIRBOTF7/target.c
index aad08be85e..72592069c5 100644
--- a/src/main/target/AIRBOTF7/target.c
+++ b/src/main/target/AIRBOTF7/target.c
@@ -34,8 +34,11 @@
 BUSDEV_REGISTER_SPI_TAG(busdev_imu0_mpu6000,    DEVHW_MPU6000,  GYRO_0_SPI_BUS,  GYRO_0_CS_PIN,   GYRO_0_EXTI_PIN,     0,  DEVFLAGS_NONE);
 BUSDEV_REGISTER_SPI_TAG(busdev_imu0_mpu6500,    DEVHW_MPU6500,  GYRO_0_SPI_BUS,  GYRO_0_CS_PIN,   GYRO_0_EXTI_PIN,     0,  DEVFLAGS_NONE);
 
+// OMNIBUSF7NANOV7 doesn't have a second gyro
+#ifndef OMNIBUSF7NANOV7
 BUSDEV_REGISTER_SPI_TAG(busdev_imu1_mpu6000,    DEVHW_MPU6000,  GYRO_1_SPI_BUS,  GYRO_1_CS_PIN,   GYRO_1_EXTI_PIN,     1,  DEVFLAGS_NONE);
 BUSDEV_REGISTER_SPI_TAG(busdev_imu1_mpu6500,    DEVHW_MPU6500,  GYRO_1_SPI_BUS,  GYRO_1_CS_PIN,   GYRO_1_EXTI_PIN,     1,  DEVFLAGS_NONE);
+#endif
 
 const timerHardware_t timerHardware[] = {
     DEF_TIM(TIM2,  CH1, PA15, TIM_USE_LED,              0, 0), // LED
diff --git a/src/main/target/AIRBOTF7/target.h b/src/main/target/AIRBOTF7/target.h
index ed7275dcc4..8b4a267710 100644
--- a/src/main/target/AIRBOTF7/target.h
+++ b/src/main/target/AIRBOTF7/target.h
@@ -24,9 +24,6 @@
 
 #pragma once
 
-#define TARGET_BOARD_IDENTIFIER "ABF7"
-#define USBD_PRODUCT_STRING     "Airbot F7"
-
 // ************** Beeper and LED ******************
 #define LED0                    PB12
 #define BEEPER                  PB0
@@ -55,31 +52,11 @@
 #define I2C1_SCL                PB8
 #define I2C1_SDA                PB9 
 
-// *************** Gyro & ACC **********************
-#define USE_TARGET_IMU_HARDWARE_DESCRIPTORS
-#define USE_DUAL_GYRO
-
-#define USE_ACC
-#define USE_GYRO
-
-#define USE_ACC_MPU6000
-#define USE_ACC_MPU6500
-#define USE_GYRO_MPU6000
-#define USE_GYRO_MPU6500
-
-#define GYRO_0_CS_PIN           PD2
-#define GYRO_0_SPI_BUS          BUS_SPI3
-#define GYRO_0_EXTI_PIN         NONE
-#define GYRO_0_ALIGN            CW90_DEG    // This doesn't work yet, requires BUS refactoring
-
-#define GYRO_1_CS_PIN           PC4
-#define GYRO_1_SPI_BUS          BUS_SPI1
-#define GYRO_1_EXTI_PIN         NONE
-#define GYRO_1_ALIGN            CW0_DEG     // This doesn't work yet, requires BUS refactoring
-
-// TODO: Remove this once per-gyro alignment is supported correctly
-#define GYRO_MPU6500_ALIGN      CW90_DEG
-#define ACC_MPU6500_ALIGN       CW90_DEG
+#ifdef OMNIBUSF7NANOV7
+#   include "target_o7v7.h"
+#else
+#   include "target_abf7.h"
+#endif
 
 // *************** OSD *****************************
 #define USE_OSD
@@ -87,13 +64,6 @@
 #define MAX7456_SPI_BUS         BUS_SPI2
 #define MAX7456_CS_PIN          PC15
 
-// *************** FLASH **************************
-#define USE_FLASHFS
-#define USE_FLASH_M25P16
-#define M25P16_CS_PIN           PA3
-#define M25P16_SPI_BUS          BUS_SPI1
-#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
-
 // *************** I2C defvices **********************
 #define USE_BARO
 #define BARO_I2C_BUS            BUS_I2C1
@@ -148,17 +118,6 @@
 
 #define SERIAL_PORT_COUNT       8
 
-// *************** ADC *****************************
-#define USE_ADC
-#define ADC_INSTANCE                ADC1
-#define ADC_CHANNEL_1_PIN           PC0
-#define ADC_CHANNEL_2_PIN           PC2
-#define ADC_CHANNEL_3_PIN           PC1
-
-#define VBAT_ADC_CHANNEL            ADC_CHN_1
-#define CURRENT_METER_ADC_CHANNEL   ADC_CHN_2
-#define RSSI_ADC_CHANNEL            ADC_CHN_3
-
 // ************** Other features *******************
 #define USE_LED_STRIP
 #define WS2811_PIN                  PA15
diff --git a/src/main/target/AIRBOTF7/target_abf7.h b/src/main/target/AIRBOTF7/target_abf7.h
new file mode 100644
index 0000000000..80753c3acb
--- /dev/null
+++ b/src/main/target/AIRBOTF7/target_abf7.h
@@ -0,0 +1,81 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute 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.
+ *
+ * This file 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 this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#pragma once
+
+#define TARGET_BOARD_IDENTIFIER "ABF7"
+#define USBD_PRODUCT_STRING     "Airbot F7"
+
+
+// *************** Gyro & ACC **********************
+#define USE_TARGET_IMU_HARDWARE_DESCRIPTORS
+#define USE_DUAL_GYRO
+
+#define USE_ACC
+#define USE_GYRO
+
+#define USE_ACC_MPU6000
+#define USE_ACC_MPU6500
+#define USE_GYRO_MPU6000
+#define USE_GYRO_MPU6500
+
+#define GYRO_0_CS_PIN           PD2
+#define GYRO_0_SPI_BUS          BUS_SPI3
+#define GYRO_0_EXTI_PIN         NONE
+#define GYRO_0_ALIGN            CW90_DEG    // This doesn't work yet, requires BUS refactoring
+
+#define GYRO_1_CS_PIN           PC4
+#define GYRO_1_SPI_BUS          BUS_SPI1
+#define GYRO_1_EXTI_PIN         NONE
+#define GYRO_1_ALIGN            CW0_DEG     // This doesn't work yet, requires BUS refactoring
+
+// TODO: Remove this once per-gyro alignment is supported correctly
+#define GYRO_MPU6500_ALIGN      CW90_DEG
+#define ACC_MPU6500_ALIGN       CW90_DEG
+
+// *************** FLASH **************************
+#define USE_FLASHFS
+#define USE_FLASH_M25P16
+#define M25P16_CS_PIN           PA3
+#define M25P16_SPI_BUS          BUS_SPI1
+#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
+
+// *************** UART *****************************
+#define USE_UART2
+#define UART2_TX_PIN            PA2
+#define UART2_RX_PIN            NONE
+
+#define SERIAL_PORT_COUNT       8
+
+// *************** ADC *****************************
+#define USE_ADC
+#define ADC_INSTANCE                ADC1
+#define ADC_CHANNEL_1_PIN           PC0
+#define ADC_CHANNEL_2_PIN           PC2
+#define ADC_CHANNEL_3_PIN           PC1
+
+#define VBAT_ADC_CHANNEL            ADC_CHN_1
+#define CURRENT_METER_ADC_CHANNEL   ADC_CHN_2
+#define RSSI_ADC_CHANNEL            ADC_CHN_3
+
diff --git a/src/main/target/AIRBOTF7/target_o7v7.h b/src/main/target/AIRBOTF7/target_o7v7.h
new file mode 100644
index 0000000000..3a6ab38854
--- /dev/null
+++ b/src/main/target/AIRBOTF7/target_o7v7.h
@@ -0,0 +1,72 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute 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.
+ *
+ * This file 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 this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#pragma once
+
+#define TARGET_BOARD_IDENTIFIER "O7V7"
+#define USBD_PRODUCT_STRING     "OmnibusF7 Nano V7"
+
+
+// *************** Gyro & ACC **********************
+#define USE_TARGET_IMU_HARDWARE_DESCRIPTORS
+
+#define USE_ACC
+#define USE_GYRO
+
+#define USE_ACC_MPU6000
+#define USE_ACC_MPU6500
+#define USE_GYRO_MPU6000
+#define USE_GYRO_MPU6500
+
+#define GYRO_0_CS_PIN           PD2
+#define GYRO_0_SPI_BUS          BUS_SPI3
+#define GYRO_0_EXTI_PIN         NONE
+
+#define GYRO_0_ALIGN            CW0_DEG
+#define GYRO_MPU6500_ALIGN      CW0_DEG
+#define ACC_MPU6500_ALIGN       CW0_DEG
+
+// *************** FLASH **************************
+#define USE_FLASHFS
+#define USE_FLASH_M25P16
+#define M25P16_CS_PIN           PA2
+#define M25P16_SPI_BUS          BUS_SPI1
+#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
+
+// *************** UART *****************************
+#define USE_UART2
+#define UART2_TX_PIN            PA2
+#define UART2_RX_PIN            NONE
+
+#define SERIAL_PORT_COUNT       8
+
+// *************** ADC *****************************
+#define USE_ADC
+#define ADC_INSTANCE                ADC1
+#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
+

From 1bc8938657a3503452774beafb1b69ec2a216514 Mon Sep 17 00:00:00 2001
From: Harry Kalogirou <harkal@nlogn.eu>
Date: Thu, 9 Apr 2020 11:58:09 +0300
Subject: [PATCH 125/179] Add control_smoothness configuration variable

---
 src/main/cms/cms_menu_navigation.c | 1 +
 src/main/fc/settings.yaml          | 4 ++++
 src/main/navigation/navigation.c   | 1 +
 src/main/navigation/navigation.h   | 1 +
 4 files changed, 7 insertions(+)

diff --git a/src/main/cms/cms_menu_navigation.c b/src/main/cms/cms_menu_navigation.c
index 01532efb8f..af5ac8574b 100644
--- a/src/main/cms/cms_menu_navigation.c
+++ b/src/main/cms/cms_menu_navigation.c
@@ -108,6 +108,7 @@ static const OSD_Entry cmsx_menuFixedWingEntries[] =
     OSD_SETTING_ENTRY("MAX DIVE ANGLE", SETTING_NAV_FW_DIVE_ANGLE),
     OSD_SETTING_ENTRY("PITCH TO THR RATIO", SETTING_NAV_FW_PITCH2THR),
     OSD_SETTING_ENTRY("LOITER RADIUS", SETTING_NAV_FW_LOITER_RADIUS),
+    OSD_SETTING_ENTRY("CONTROL SMOOTHNESS", SETTING_NAV_FW_CONTROL_SMOOTHNESS),
 
     OSD_BACK_AND_END_ENTRY,
 };
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index a611be75f2..65183912c0 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -1596,6 +1596,10 @@ groups:
         field: fw.cruise_speed
         min: 0
         max: 65535
+      - name: nav_fw_control_smoothness 
+        field: fw.control_smoothness
+        min: 0
+        max: 9
 
       - name: nav_fw_land_dive_angle
         field: fw.land_dive_angle
diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index a636c13d80..c9574a4c18 100755
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -143,6 +143,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
         .max_dive_angle = 15,                   // degrees
         .cruise_throttle = 1400,
         .cruise_speed = 0,                      // cm/s
+        .control_smoothness = 0,
         .max_throttle = 1700,
         .min_throttle = 1200,
         .pitch_to_throttle = 10,                // pwm units per degree of pitch (10pwm units ~ 1% throttle)
diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h
index e9d53f8844..57927b0687 100755
--- a/src/main/navigation/navigation.h
+++ b/src/main/navigation/navigation.h
@@ -198,6 +198,7 @@ typedef struct navConfig_s {
         uint8_t  max_dive_angle;             // Fixed wing max banking angle (deg)
         uint16_t cruise_throttle;            // Cruise throttle
         uint16_t cruise_speed;               // Speed at cruise throttle (cm/s), used for time/distance left before RTH
+        uint8_t control_smoothness;          // The amount of smoothing to apply to controls for navigation
         uint16_t min_throttle;               // Minimum allowed throttle in auto mode
         uint16_t max_throttle;               // Maximum allowed throttle in auto mode
         uint8_t  pitch_to_throttle;          // Pitch angle (in deg) to throttle gain (in 1/1000's of throttle) (*10)

From d41d1fc5104c5d17fddc4bc9040df3722106d9ba Mon Sep 17 00:00:00 2001
From: Harry Kalogirou <harkal@nlogn.eu>
Date: Thu, 9 Apr 2020 11:59:07 +0300
Subject: [PATCH 126/179] Apply low pass filters with configurable value

---
 src/main/navigation/navigation_fixedwing.c | 7 +++++--
 1 file changed, 5 insertions(+), 2 deletions(-)

diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c
index 9b01c2c679..81eaf77aa0 100755
--- a/src/main/navigation/navigation_fixedwing.c
+++ b/src/main/navigation/navigation_fixedwing.c
@@ -136,7 +136,9 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros)
 
     // PID controller to translate energy balance error [J] into pitch angle [decideg]
     float targetPitchAngle = navPidApply3(&posControl.pids.fw_alt, demSEB, estSEB, US2S(deltaMicros), minDiveDeciDeg, maxClimbDeciDeg, 0, pitchGainInv);
-    targetPitchAngle = pt1FilterApply4(&velzFilterState, targetPitchAngle, NAV_FW_PITCH_CUTOFF_FREQENCY_HZ, US2S(deltaMicros));
+
+    const float cutoff_freq = constrain((10.f - navConfig()->fw.control_smoothness) / 5.0, 0.5, 2.0); // Pitch cutoff set to 1/5 the roll cutoff
+    targetPitchAngle = pt1FilterApply4(&velzFilterState, targetPitchAngle, cutoff_freq, US2S(deltaMicros));
 
     // Reconstrain pitch angle ( >0 - climb, <0 - dive)
     targetPitchAngle = constrainf(targetPitchAngle, minDiveDeciDeg, maxClimbDeciDeg);
@@ -367,7 +369,8 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
                                         pidFlags);
 
     // Apply low-pass filter to prevent rapid correction
-    rollAdjustment = pt1FilterApply4(&fwPosControllerCorrectionFilterState, rollAdjustment, NAV_FW_ROLL_CUTOFF_FREQUENCY_HZ, US2S(deltaMicros));
+    uint8_t cutoff_freq = constrain(10 - navConfig()->fw.control_smoothness, 1, 10);
+    rollAdjustment = pt1FilterApply4(&fwPosControllerCorrectionFilterState, rollAdjustment, cutoff_freq, US2S(deltaMicros));
 
     // Convert rollAdjustment to decidegrees (rcAdjustment holds decidegrees)
     posControl.rcAdjustment[ROLL] = CENTIDEGREES_TO_DECIDEGREES(rollAdjustment);

From 99eac02087950edf7c6bc779ec6e24297ea337c4 Mon Sep 17 00:00:00 2001
From: Harry Kalogirou <harkal@nlogn.eu>
Date: Thu, 9 Apr 2020 11:59:26 +0300
Subject: [PATCH 127/179] Remove unused constants

---
 src/main/navigation/navigation_private.h | 2 --
 1 file changed, 2 deletions(-)

diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h
index 7748472403..7feaa50b1f 100755
--- a/src/main/navigation/navigation_private.h
+++ b/src/main/navigation/navigation_private.h
@@ -30,8 +30,6 @@
 #define MIN_POSITION_UPDATE_RATE_HZ         5       // Minimum position update rate at which XYZ controllers would be applied
 #define NAV_THROTTLE_CUTOFF_FREQENCY_HZ     4       // low-pass filter on throttle output
 #define NAV_FW_CONTROL_MONITORING_RATE      2
-#define NAV_FW_PITCH_CUTOFF_FREQENCY_HZ     2       // low-pass filter on Z (pitch angle) for fixed wing
-#define NAV_FW_ROLL_CUTOFF_FREQUENCY_HZ     10      // low-pass filter on roll correction for fixed wing
 #define NAV_DTERM_CUT_HZ                    10.0f
 #define NAV_ACCELERATION_XY_MAX             980.0f  // cm/s/s       // approx 45 deg lean angle
 

From 6e2de5685f6fb25a277ede292248175c4725449f Mon Sep 17 00:00:00 2001
From: Harry Kalogirou <harkal@nlogn.eu>
Date: Thu, 9 Apr 2020 11:59:41 +0300
Subject: [PATCH 128/179] Add documentation of config variable

---
 docs/Cli.md | 1 +
 1 file changed, 1 insertion(+)

diff --git a/docs/Cli.md b/docs/Cli.md
index b292304274..147a39de08 100644
--- a/docs/Cli.md
+++ b/docs/Cli.md
@@ -215,6 +215,7 @@ A shorter form is also supported to enable and disable functions using `serial <
 |  nav_fw_climb_angle  | 20 | Max pitch angle when climbing in GPS assisted modes, is also restrained by global max_angle_inclination_pit |
 |  nav_fw_dive_angle  | 15 | Max negative pitch angle when diving in GPS assisted modes, is also restrained by global max_angle_inclination_pit |
 |  nav_fw_pitch2thr  | 10 | Amount of throttle applied related to pitch attitude in GPS assisted modes. Throttle = nav_fw_cruise_throttle - (nav_fw_pitch2thr * pitch_angle). (notice that pitch_angle is in degrees and is negative when climbing and positive when diving, and throttle value is constrained between nav_fw_min_thr and nav_fw_max_thr) |
+|  nav_fw_control_smoothness | 0 | How smoothly the autopilot controls the airplane to correct the navigation error |
 |  nav_fw_loiter_radius  | 5000 | PosHold radius. 3000 to 7500 is a good value (30-75m) [cm] |
 |  nav_fw_launch_velocity  | 300 | Forward velocity threshold for swing-launch detection [cm/s] |
 |  nav_fw_launch_accel  | 1863 | Forward acceleration threshold for bungee launch of throw launch [cm/s/s], 1G = 981 cm/s/s |

From 52daaa789ea57d06939f518a546ac4e2f06dba43 Mon Sep 17 00:00:00 2001
From: Harry Kalogirou <harkal@nlogn.eu>
Date: Mon, 27 Apr 2020 15:31:54 +0300
Subject: [PATCH 129/179] more accuracy for smoothness factor at low values

---
 src/main/navigation/navigation_fixedwing.c | 10 +++++++---
 1 file changed, 7 insertions(+), 3 deletions(-)

diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c
index 81eaf77aa0..1fbfb32193 100755
--- a/src/main/navigation/navigation_fixedwing.c
+++ b/src/main/navigation/navigation_fixedwing.c
@@ -137,7 +137,10 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros)
     // PID controller to translate energy balance error [J] into pitch angle [decideg]
     float targetPitchAngle = navPidApply3(&posControl.pids.fw_alt, demSEB, estSEB, US2S(deltaMicros), minDiveDeciDeg, maxClimbDeciDeg, 0, pitchGainInv);
 
-    const float cutoff_freq = constrain((10.f - navConfig()->fw.control_smoothness) / 5.0, 0.5, 2.0); // Pitch cutoff set to 1/5 the roll cutoff
+    // Apply low-pass filter to prevent rapid correction cutoff_freq = 0.002 * (10.0-x)^3 + epsilon 
+    // Pitch cutoff set to 1/5 the roll cutoff
+    uint16_t smoothness = 10 - navConfig()->fw.control_smoothness;
+    float cutoff_freq = 0.002f * (float)(smoothness*smoothness*smoothness) + 0.1f;
     targetPitchAngle = pt1FilterApply4(&velzFilterState, targetPitchAngle, cutoff_freq, US2S(deltaMicros));
 
     // Reconstrain pitch angle ( >0 - climb, <0 - dive)
@@ -368,8 +371,9 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
                                         DEGREES_TO_CENTIDEGREES(navConfig()->fw.max_bank_angle),
                                         pidFlags);
 
-    // Apply low-pass filter to prevent rapid correction
-    uint8_t cutoff_freq = constrain(10 - navConfig()->fw.control_smoothness, 1, 10);
+    // Apply low-pass filter to prevent rapid correction cutoff_freq = 0.01 * (10.0-x)^3 + epsilon
+    uint16_t smoothness = 10 - navConfig()->fw.control_smoothness;
+    float cutoff_freq = 0.01f * (float)(smoothness*smoothness*smoothness) + 0.1f;
     rollAdjustment = pt1FilterApply4(&fwPosControllerCorrectionFilterState, rollAdjustment, cutoff_freq, US2S(deltaMicros));
 
     // Convert rollAdjustment to decidegrees (rcAdjustment holds decidegrees)

From 041f2c98515416d43bfb803c24550ae19a8d7d49 Mon Sep 17 00:00:00 2001
From: "Konstantin (DigitalEntity) Sharlaimov"
 <konstantin.sharlaimov@gmail.com>
Date: Fri, 1 May 2020 11:23:53 +0200
Subject: [PATCH 130/179] [SERVO] Option to output servos via SBUS stream

---
 make/source.mk                 |   1 +
 src/main/drivers/pwm_mapping.c |   2 +-
 src/main/drivers/pwm_mapping.h |   6 ++
 src/main/drivers/pwm_output.c  |  24 ++++--
 src/main/fc/cli.c              |   2 +-
 src/main/fc/config.c           |  10 ++-
 src/main/fc/config.h           |   4 +-
 src/main/fc/fc_init.c          |   6 --
 src/main/fc/fc_tasks.c         |  27 ++++---
 src/main/fc/settings.yaml      |   5 ++
 src/main/flight/servos.c       |   4 +-
 src/main/flight/servos.h       |   2 +-
 src/main/io/pwmdriver_i2c.c    |   4 +
 src/main/io/serial.h           |   1 +
 src/main/io/servo_sbus.c       | 133 +++++++++++++++++++++++++++++++++
 src/main/io/servo_sbus.h       |  29 +++++++
 src/main/rx/sbus.c             |  30 --------
 src/main/rx/sbus_channels.c    |  20 +++--
 src/main/rx/sbus_channels.h    |  30 +++++++-
 src/main/target/common.h       |   1 +
 20 files changed, 276 insertions(+), 65 deletions(-)
 create mode 100644 src/main/io/servo_sbus.c
 create mode 100644 src/main/io/servo_sbus.h

diff --git a/make/source.mk b/make/source.mk
index 2e8106da6c..a9feba5d99 100644
--- a/make/source.mk
+++ b/make/source.mk
@@ -105,6 +105,7 @@ COMMON_SRC = \
             flight/dynamic_gyro_notch.c \
             io/beeper.c \
             io/esc_serialshot.c \
+            io/servo_sbus.c \
             io/frsky_osd.c \
             io/osd_dji_hd.c \
             io/lights.c \
diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c
index 00e1f43489..8c6eaea463 100644
--- a/src/main/drivers/pwm_mapping.c
+++ b/src/main/drivers/pwm_mapping.c
@@ -268,7 +268,7 @@ static bool motorsUseHardwareTimers(void)
 
 static bool servosUseHardwareTimers(void)
 {
-    return !feature(FEATURE_PWM_SERVO_DRIVER);
+    return servoConfig()->servo_protocol == SERVO_TYPE_PWM;
 }
 
 static void pwmInitMotors(timMotorServoHardware_t * timOutputs)
diff --git a/src/main/drivers/pwm_mapping.h b/src/main/drivers/pwm_mapping.h
index cb4d760a05..2c8b8f83a5 100644
--- a/src/main/drivers/pwm_mapping.h
+++ b/src/main/drivers/pwm_mapping.h
@@ -49,6 +49,12 @@ typedef enum {
     PWM_TYPE_SERIALSHOT,
 } motorPwmProtocolTypes_e;
 
+typedef enum {
+    SERVO_TYPE_PWM = 0,
+    SERVO_TYPE_SERVO_DRIVER,
+    SERVO_TYPE_SBUS
+} servoProtocolType_e;
+
 typedef enum {
     PWM_INIT_ERROR_NONE = 0,
     PWM_INIT_ERROR_TOO_MANY_MOTORS,
diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c
index 8d048c87a5..f14b6a0e33 100644
--- a/src/main/drivers/pwm_output.c
+++ b/src/main/drivers/pwm_output.c
@@ -37,6 +37,7 @@ FILE_COMPILE_FOR_SPEED
 
 #include "io/pwmdriver_i2c.h"
 #include "io/esc_serialshot.h"
+#include "io/servo_sbus.h"
 #include "sensors/esc_sensor.h"
 
 #include "config/feature.h"
@@ -507,14 +508,27 @@ static void pwmServoWriteExternalDriver(uint8_t index, uint16_t value)
 
 void pwmServoPreconfigure(void)
 {
-    servoWritePtr = pwmServoWriteStandard;
+    // Protocol-specific configuration
+    switch (servoConfig()->servo_protocol) {
+        default:
+        case SERVO_TYPE_PWM:
+            servoWritePtr = pwmServoWriteStandard;
+            break;
 
 #ifdef USE_PWM_SERVO_DRIVER
-    // If PCA9685 is enabled - switch the servo write function to external
-    if (feature(FEATURE_PWM_SERVO_DRIVER)) {
-        servoWritePtr = pwmServoWriteExternalDriver;
-    }
+        case SERVO_TYPE_SERVO_DRIVER:
+            pwmDriverInitialize();
+            servoWritePtr = pwmServoWriteExternalDriver;
+            break;
 #endif
+
+#ifdef USE_SERVO_SBUS
+        case SERVO_TYPE_SBUS:
+            sbusServoInitialize();
+            servoWritePtr = sbusServoUpdate;
+            break;
+#endif
+    }
 }
 
 bool pwmServoConfig(const timerHardware_t *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse, bool enableOutput)
diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c
index 6817db0740..f132058eb2 100755
--- a/src/main/fc/cli.c
+++ b/src/main/fc/cli.c
@@ -153,7 +153,7 @@ static const char * const featureNames[] = {
     "", "TELEMETRY", "CURRENT_METER", "REVERSIBLE_MOTORS", "",
     "", "RSSI_ADC", "LED_STRIP", "DASHBOARD", "",
     "BLACKBOX", "", "TRANSPONDER", "AIRMODE",
-    "SUPEREXPO", "VTX", "", "", "PWM_SERVO_DRIVER", "PWM_OUTPUT_ENABLE",
+    "SUPEREXPO", "VTX", "", "", "", "PWM_OUTPUT_ENABLE",
     "OSD", "FW_LAUNCH", NULL
 };
 
diff --git a/src/main/fc/config.c b/src/main/fc/config.c
index 4ac930f1fa..20b08e5ca2 100755
--- a/src/main/fc/config.c
+++ b/src/main/fc/config.c
@@ -217,7 +217,15 @@ void validateAndFixConfig(void)
 #endif
 
 #ifndef USE_PWM_SERVO_DRIVER
-    featureClear(FEATURE_PWM_SERVO_DRIVER);
+    if (servoConfig()->servo_protocol == SERVO_TYPE_SERVO_DRIVER) {
+        servoConfigMutable()->servo_protocol = SERVO_TYPE_PWM;
+    }
+#endif
+
+#ifndef USE_SERVO_SBUS
+    if (servoConfig()->servo_protocol == SERVO_TYPE_SBUS) {
+        servoConfigMutable()->servo_protocol = SERVO_TYPE_PWM;
+    }
 #endif
 
     if (!isSerialConfigValid(serialConfigMutable())) {
diff --git a/src/main/fc/config.h b/src/main/fc/config.h
index a12fc63a07..bab2d96eef 100644
--- a/src/main/fc/config.h
+++ b/src/main/fc/config.h
@@ -62,8 +62,8 @@ typedef enum {
     FEATURE_SUPEREXPO_RATES = 1 << 23,
     FEATURE_VTX = 1 << 24,
     FEATURE_UNUSED_8 = 1 << 25,         // RX_SPI
-    FEATURE_UNUSED_9 = 1 << 26,         //SOFTSPI
-    FEATURE_PWM_SERVO_DRIVER = 1 << 27,
+    FEATURE_UNUSED_9 = 1 << 26,         // SOFTSPI
+    FEATURE_UNUSED_11 = 1 << 27,        // FEATURE_PWM_SERVO_DRIVER
     FEATURE_PWM_OUTPUT_ENABLE = 1 << 28,
     FEATURE_OSD = 1 << 29,
     FEATURE_FW_LAUNCH = 1 << 30,
diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c
index e7c4b4b026..09295d94f9 100644
--- a/src/main/fc/fc_init.c
+++ b/src/main/fc/fc_init.c
@@ -643,12 +643,6 @@ void init(void)
     if (feature(FEATURE_VBAT | FEATURE_CURRENT_METER))
         batteryInit();
 
-#ifdef USE_PWM_SERVO_DRIVER
-    if (feature(FEATURE_PWM_SERVO_DRIVER)) {
-        pwmDriverInitialize();
-    }
-#endif
-
 #ifdef USE_RCDEVICE
     rcdeviceInit();
 #endif // USE_RCDEVICE
diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c
index e8ba6f432a..cf0cad508d 100755
--- a/src/main/fc/fc_tasks.c
+++ b/src/main/fc/fc_tasks.c
@@ -34,6 +34,7 @@
 #include "drivers/sensor.h"
 #include "drivers/serial.h"
 #include "drivers/stack_check.h"
+#include "drivers/pwm_mapping.h"
 
 #include "fc/cli.h"
 #include "fc/config.h"
@@ -48,6 +49,7 @@
 #include "flight/pid.h"
 #include "flight/wind_estimator.h"
 #include "flight/rpm_filter.h"
+#include "flight/servos.h"
 
 #include "navigation/navigation.h"
 
@@ -62,6 +64,7 @@
 #include "io/rcdevice_cam.h"
 #include "io/vtx.h"
 #include "io/osd_dji_hd.h"
+#include "io/servo_sbus.h"
 
 #include "msp/msp_serial.h"
 
@@ -249,17 +252,19 @@ void taskLedStrip(timeUs_t currentTimeUs)
 }
 #endif
 
-#ifdef USE_PWM_SERVO_DRIVER
-void taskSyncPwmDriver(timeUs_t currentTimeUs)
+void taskSyncServoDriver(timeUs_t currentTimeUs)
 {
     UNUSED(currentTimeUs);
 
-    if (feature(FEATURE_PWM_SERVO_DRIVER)) {
-        pwmDriverSync();
-    }
-}
+#if defined(USE_SERVO_SBUS)
+    sbusServoSendUpdate();
 #endif
 
+#ifdef USE_PWM_SERVO_DRIVER
+    pwmDriverSync();
+#endif
+}
+
 #ifdef USE_OSD
 void taskUpdateOsd(timeUs_t currentTimeUs)
 {
@@ -317,8 +322,8 @@ void fcTasksInit(void)
 #ifdef STACK_CHECK
     setTaskEnabled(TASK_STACK_CHECK, true);
 #endif
-#ifdef USE_PWM_SERVO_DRIVER
-    setTaskEnabled(TASK_PWMDRIVER, feature(FEATURE_PWM_SERVO_DRIVER));
+#if defined(USE_PWM_SERVO_DRIVER) || defined(USE_SERVO_SBUS)
+    setTaskEnabled(TASK_PWMDRIVER, (servoConfig()->servo_protocol == SERVO_TYPE_SERVO_DRIVER) || (servoConfig()->servo_protocol == SERVO_TYPE_SBUS));
 #endif
 #ifdef USE_CMS
 #ifdef USE_MSP_DISPLAYPORT
@@ -481,10 +486,10 @@ cfTask_t cfTasks[TASK_COUNT] = {
     },
 #endif
 
-#ifdef USE_PWM_SERVO_DRIVER
+#if defined(USE_PWM_SERVO_DRIVER) || defined(USE_SERVO_SBUS)
     [TASK_PWMDRIVER] = {
-        .taskName = "PWMDRIVER",
-        .taskFunc = taskSyncPwmDriver,
+        .taskName = "SERVOS",
+        .taskFunc = taskSyncServoDriver,
         .desiredPeriod = TASK_PERIOD_HZ(200),         // 200 Hz
         .staticPriority = TASK_PRIORITY_HIGH,
     },
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index a611be75f2..f8303f4934 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -33,6 +33,8 @@ tables:
     values: ["SERIAL", "SPIFLASH", "SDCARD"]
   - name: motor_pwm_protocol
     values: ["STANDARD", "ONESHOT125", "ONESHOT42", "MULTISHOT", "BRUSHED", "DSHOT150", "DSHOT300", "DSHOT600", "DSHOT1200", "SERIALSHOT"]
+  - name: servo_protocol
+    values: ["PWM", "SERVO_DRIVER", "SBUS"]
   - name: failsafe_procedure
     values: ["SET-THR", "DROP", "RTH", "NONE"]
   - name: current_sensor
@@ -726,6 +728,9 @@ groups:
     type: servoConfig_t
     headers: ["flight/servos.h"]
     members:
+      - name: servo_protocol
+        field: servo_protocol
+        table: servo_protocol
       - name: servo_center_pulse
         field: servoCenterPulse
         min: PWM_RANGE_MIN
diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c
index 0aefd6ec75..375eacb288 100755
--- a/src/main/flight/servos.c
+++ b/src/main/flight/servos.c
@@ -36,6 +36,7 @@
 #include "config/parameter_group_ids.h"
 
 #include "drivers/pwm_output.h"
+#include "drivers/pwm_mapping.h"
 #include "drivers/time.h"
 
 #include "fc/config.h"
@@ -53,12 +54,13 @@
 
 #include "sensors/gyro.h"
 
-PG_REGISTER_WITH_RESET_TEMPLATE(servoConfig_t, servoConfig, PG_SERVO_CONFIG, 0);
+PG_REGISTER_WITH_RESET_TEMPLATE(servoConfig_t, servoConfig, PG_SERVO_CONFIG, 1);
 
 PG_RESET_TEMPLATE(servoConfig_t, servoConfig,
     .servoCenterPulse = 1500,
     .servoPwmRate = 50,             // Default for analog servos
     .servo_lowpass_freq = 20,       // Default servo update rate is 50Hz, everything above Nyquist frequency (25Hz) is going to fold and cause distortions
+    .servo_protocol = SERVO_TYPE_PWM,
     .flaperon_throw_offset = FLAPERON_THROW_DEFAULT,
     .tri_unarmed_servo = 1
 );
diff --git a/src/main/flight/servos.h b/src/main/flight/servos.h
index a6befa0269..f8ca7b5112 100644
--- a/src/main/flight/servos.h
+++ b/src/main/flight/servos.h
@@ -132,7 +132,7 @@ typedef struct servoConfig_s {
     uint16_t servoPwmRate;                  // The update rate of servo outputs (50-498Hz)
     int16_t servo_lowpass_freq;             // lowpass servo filter frequency selection; 1/1000ths of loop freq
     uint16_t flaperon_throw_offset;
-    uint8_t __reserved;
+    uint8_t servo_protocol;                 // See servoProtocolType_e
     uint8_t tri_unarmed_servo;              // send tail servo correction pulses even when unarmed
 } servoConfig_t;
 
diff --git a/src/main/io/pwmdriver_i2c.c b/src/main/io/pwmdriver_i2c.c
index 727b8428f9..dc5e930457 100644
--- a/src/main/io/pwmdriver_i2c.c
+++ b/src/main/io/pwmdriver_i2c.c
@@ -50,6 +50,10 @@ void pwmDriverInitialize(void) {
 }
 
 void pwmDriverSync(void) {
+    if (!STATE(PWM_DRIVER_AVAILABLE)) {
+        return;
+    }
+
     static uint8_t cycle = 0;
 
     (pwmDrivers[driverImplementationIndex].syncFunction)(cycle);
diff --git a/src/main/io/serial.h b/src/main/io/serial.h
index bea6abb001..6f77480f73 100644
--- a/src/main/io/serial.h
+++ b/src/main/io/serial.h
@@ -53,6 +53,7 @@ typedef enum {
     FUNCTION_TELEMETRY_SIM       = (1 << 19), // 524288
     FUNCTION_FRSKY_OSD           = (1 << 20), // 1048576
     FUNCTION_DJI_HD_OSD          = (1 << 21), // 2097152
+    FUNCTION_SERVO_SERIAL        = (1 << 22), // 4194304
 } serialPortFunction_e;
 
 typedef enum {
diff --git a/src/main/io/servo_sbus.c b/src/main/io/servo_sbus.c
new file mode 100644
index 0000000000..aac649cb50
--- /dev/null
+++ b/src/main/io/servo_sbus.c
@@ -0,0 +1,133 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute 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.
+ *
+ * This file 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 this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <ctype.h>
+#include <math.h>
+
+#include "platform.h"
+
+#include "build/build_config.h"
+#include "build/debug.h"
+
+#include "common/maths.h"
+#include "common/crc.h"
+
+#include "io/serial.h"
+#include "io/servo_sbus.h"
+#include "rx/sbus_channels.h"
+
+#if defined(USE_SERVO_SBUS)
+
+#define SERVO_SBUS_UART_BAUD            100000
+#define SERVO_SBUS_OPTIONS              (SERIAL_INVERTED | SERIAL_UNIDIR)
+
+static serialPort_t * servoSbusPort = NULL;
+static sbusFrame_t sbusFrame;
+
+bool sbusServoInitialize(void)
+{
+    serialPortConfig_t * portConfig;
+
+    // Avoid double initialization
+    if (servoSbusPort) {
+        return true;
+    }
+
+    portConfig = findSerialPortConfig(FUNCTION_SERVO_SERIAL);
+    if (!portConfig) {
+        return false;
+    }
+
+    servoSbusPort = openSerialPort(portConfig->identifier, FUNCTION_SERVO_SERIAL, NULL, NULL, SERVO_SBUS_UART_BAUD, MODE_TX, SERVO_SBUS_OPTIONS);
+    if (!servoSbusPort) {
+        return false;
+    }
+
+    // SBUS V1 magical values
+    sbusFrame.syncByte = 0x0F;
+    sbusFrame.channels.flags = 0;
+    sbusFrame.channels.chan0  = sbusEncodeChannelValue(1500);
+    sbusFrame.channels.chan1  = sbusEncodeChannelValue(1500);
+    sbusFrame.channels.chan2  = sbusEncodeChannelValue(1500);
+    sbusFrame.channels.chan3  = sbusEncodeChannelValue(1500);
+    sbusFrame.channels.chan4  = sbusEncodeChannelValue(1500);
+    sbusFrame.channels.chan5  = sbusEncodeChannelValue(1500);
+    sbusFrame.channels.chan6  = sbusEncodeChannelValue(1500);
+    sbusFrame.channels.chan7  = sbusEncodeChannelValue(1500);
+    sbusFrame.channels.chan8  = sbusEncodeChannelValue(1500);
+    sbusFrame.channels.chan9  = sbusEncodeChannelValue(1500);
+    sbusFrame.channels.chan10 = sbusEncodeChannelValue(1500);
+    sbusFrame.channels.chan11 = sbusEncodeChannelValue(1500);
+    sbusFrame.channels.chan12 = sbusEncodeChannelValue(1500);
+    sbusFrame.channels.chan13 = sbusEncodeChannelValue(1500);
+    sbusFrame.channels.chan14 = sbusEncodeChannelValue(1500);
+    sbusFrame.channels.chan15 = sbusEncodeChannelValue(1500);
+    sbusFrame.endByte = 0x00;
+
+    return true;
+}
+
+void sbusServoUpdate(uint8_t index, uint16_t value)
+{
+    switch(index) {
+        case 0: sbusFrame.channels.chan0 = sbusEncodeChannelValue(value); break;
+        case 1: sbusFrame.channels.chan1 = sbusEncodeChannelValue(value); break;
+        case 2: sbusFrame.channels.chan2 = sbusEncodeChannelValue(value); break;
+        case 3: sbusFrame.channels.chan3 = sbusEncodeChannelValue(value); break;
+        case 4: sbusFrame.channels.chan4 = sbusEncodeChannelValue(value); break;
+        case 5: sbusFrame.channels.chan5 = sbusEncodeChannelValue(value); break;
+        case 6: sbusFrame.channels.chan6 = sbusEncodeChannelValue(value); break;
+        case 7: sbusFrame.channels.chan7 = sbusEncodeChannelValue(value); break;
+        case 8: sbusFrame.channels.chan8 = sbusEncodeChannelValue(value); break;
+        case 9: sbusFrame.channels.chan9 = sbusEncodeChannelValue(value); break;
+        case 10: sbusFrame.channels.chan10 = sbusEncodeChannelValue(value); break;
+        case 11: sbusFrame.channels.chan11 = sbusEncodeChannelValue(value); break;
+        case 12: sbusFrame.channels.chan12 = sbusEncodeChannelValue(value); break;
+        case 13: sbusFrame.channels.chan13 = sbusEncodeChannelValue(value); break;
+        case 14: sbusFrame.channels.chan14 = sbusEncodeChannelValue(value); break;
+        case 15: sbusFrame.channels.chan15 = sbusEncodeChannelValue(value); break;
+        default:
+            break;
+    }
+}
+
+void sbusServoSendUpdate(void)
+{
+    // Check if the port is initialized
+    if (!servoSbusPort) {
+        return;
+    }
+
+    // Skip update if previous one is not yet fully sent
+    // This helps to avoid buffer overflow and evenyually the data corruption
+    if (!isSerialTransmitBufferEmpty(servoSbusPort)) {
+        return;
+    }
+
+    serialWriteBuf(servoSbusPort, (const uint8_t *)&sbusFrame, sizeof(sbusFrame));
+}
+
+#endif
diff --git a/src/main/io/servo_sbus.h b/src/main/io/servo_sbus.h
new file mode 100644
index 0000000000..0dcc14ac8d
--- /dev/null
+++ b/src/main/io/servo_sbus.h
@@ -0,0 +1,29 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute 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.
+ *
+ * This file 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 this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#pragma once
+
+bool sbusServoInitialize(void);
+void sbusServoUpdate(uint8_t index, uint16_t value);
+void sbusServoSendUpdate(void);
diff --git a/src/main/rx/sbus.c b/src/main/rx/sbus.c
index 16a76a8fa0..a41e264c2a 100644
--- a/src/main/rx/sbus.c
+++ b/src/main/rx/sbus.c
@@ -53,21 +53,6 @@ FILE_COMPILE_FOR_SPEED
  * time to send frame: 3ms.
  */
 
-#define SBUS_FRAME_SIZE (SBUS_CHANNEL_DATA_LENGTH + 2)
-
-#define SBUS_FRAME_BEGIN_BYTE 0x0F
-
-#define SBUS_BAUDRATE       100000
-#define SBUS_BAUDRATE_FAST  200000
-
-#if !defined(SBUS_PORT_OPTIONS)
-#define SBUS_PORT_OPTIONS (SERIAL_STOPBITS_2 | SERIAL_PARITY_EVEN)
-#endif
-
-#define SBUS_DIGITAL_CHANNEL_MIN 173
-#define SBUS_DIGITAL_CHANNEL_MAX 1812
-
-
 enum {
     DEBUG_SBUS_INTERFRAME_TIME = 0,
     DEBUG_SBUS_FRAME_FLAGS = 1,
@@ -80,19 +65,6 @@ typedef enum {
     STATE_SBUS_WAIT_SYNC
 } sbusDecoderState_e;
 
-typedef struct sbusFrame_s {
-    uint8_t syncByte;
-    sbusChannels_t channels;
-    /**
-     * The endByte is 0x00 on FrSky and some futaba RX's, on Some SBUS2 RX's the value indicates the telemetry byte that is sent after every 4th sbus frame.
-     *
-     * See https://github.com/cleanflight/cleanflight/issues/590#issuecomment-101027349
-     * and
-     * https://github.com/cleanflight/cleanflight/issues/590#issuecomment-101706023
-     */
-    uint8_t endByte;
-} __attribute__ ((__packed__)) sbusFrame_t;
-
 typedef struct sbusFrameData_s {
     sbusDecoderState_e state;
     volatile sbusFrame_t frame;
@@ -102,8 +74,6 @@ typedef struct sbusFrameData_s {
     timeUs_t lastActivityTimeUs;
 } sbusFrameData_t;
 
-STATIC_ASSERT(SBUS_FRAME_SIZE == sizeof(sbusFrame_t), SBUS_FRAME_SIZE_doesnt_match_sbusFrame_t);
-
 // Receive ISR callback
 static void sbusDataReceive(uint16_t c, void *data)
 {
diff --git a/src/main/rx/sbus_channels.c b/src/main/rx/sbus_channels.c
index 6e25ed719e..ba87343e84 100644
--- a/src/main/rx/sbus_channels.c
+++ b/src/main/rx/sbus_channels.c
@@ -24,18 +24,18 @@
 #ifdef USE_SERIAL_RX
 
 #include "common/utils.h"
+#include "common/maths.h"
 
-#include "rx/rx.h"
 #include "rx/sbus_channels.h"
 
-#define DEBUG_SBUS_FRAME_INTERVAL 3
-
 #define SBUS_FLAG_CHANNEL_17        (1 << 0)
 #define SBUS_FLAG_CHANNEL_18        (1 << 1)
 
 #define SBUS_DIGITAL_CHANNEL_MIN 173
 #define SBUS_DIGITAL_CHANNEL_MAX 1812
 
+STATIC_ASSERT(SBUS_FRAME_SIZE == sizeof(sbusFrame_t), SBUS_FRAME_SIZE_doesnt_match_sbusFrame_t);
+
 uint8_t sbusChannelsDecode(rxRuntimeConfig_t *rxRuntimeConfig, const sbusChannels_t *channels)
 {
     uint16_t *sbusChannelData = rxRuntimeConfig->channelData;
@@ -82,11 +82,21 @@ uint8_t sbusChannelsDecode(rxRuntimeConfig_t *rxRuntimeConfig, const sbusChannel
     return RX_FRAME_COMPLETE;
 }
 
-static uint16_t sbusChannelsReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan)
+uint16_t sbusDecodeChannelValue(uint16_t sbusValue)
 {
     // Linear fitting values read from OpenTX-ppmus and comparing with values received by X4R
     // http://www.wolframalpha.com/input/?i=linear+fit+%7B173%2C+988%7D%2C+%7B1812%2C+2012%7D%2C+%7B993%2C+1500%7D
-    return (5 * rxRuntimeConfig->channelData[chan] / 8) + 880;
+    return (5 * constrain(sbusValue, SBUS_DIGITAL_CHANNEL_MIN, SBUS_DIGITAL_CHANNEL_MAX) / 8) + 880;
+}
+
+uint16_t sbusEncodeChannelValue(uint16_t rcValue)
+{
+    return constrain((((int)rcValue - 880) * 8 + 4) / 5, SBUS_DIGITAL_CHANNEL_MIN, SBUS_DIGITAL_CHANNEL_MAX);
+}
+
+static uint16_t sbusChannelsReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan)
+{
+    return sbusDecodeChannelValue(rxRuntimeConfig->channelData[chan]);
 }
 
 void sbusChannelsInit(rxRuntimeConfig_t *rxRuntimeConfig)
diff --git a/src/main/rx/sbus_channels.h b/src/main/rx/sbus_channels.h
index 962e0fa9e2..1354d82fe3 100644
--- a/src/main/rx/sbus_channels.h
+++ b/src/main/rx/sbus_channels.h
@@ -18,12 +18,25 @@
 #pragma once
 
 #include <stdint.h>
+#include "rx/rx.h"
 
 #define SBUS_MAX_CHANNEL 18
 
 #define SBUS_FLAG_SIGNAL_LOSS       (1 << 2)
 #define SBUS_FLAG_FAILSAFE_ACTIVE   (1 << 3)
 
+#define SBUS_CHANNEL_DATA_LENGTH sizeof(sbusChannels_t)
+#define SBUS_FRAME_SIZE (SBUS_CHANNEL_DATA_LENGTH + 2)
+
+#define SBUS_FRAME_BEGIN_BYTE 0x0F
+
+#define SBUS_BAUDRATE       100000
+#define SBUS_BAUDRATE_FAST  200000
+
+#if !defined(SBUS_PORT_OPTIONS)
+#define SBUS_PORT_OPTIONS (SERIAL_STOPBITS_2 | SERIAL_PARITY_EVEN)
+#endif
+
 typedef struct sbusChannels_s {
     // 176 bits of data (11 bits per channel * 16 channels) = 22 bytes.
     unsigned int chan0 : 11;
@@ -45,7 +58,22 @@ typedef struct sbusChannels_s {
     uint8_t flags;
 } __attribute__((__packed__)) sbusChannels_t;
 
-#define SBUS_CHANNEL_DATA_LENGTH sizeof(sbusChannels_t)
+typedef struct sbusFrame_s {
+    uint8_t syncByte;
+    sbusChannels_t channels;
+    /**
+     * The endByte is 0x00 on FrSky and some futaba RX's, on Some SBUS2 RX's the value indicates the telemetry byte that is sent after every 4th sbus frame.
+     *
+     * See https://github.com/cleanflight/cleanflight/issues/590#issuecomment-101027349
+     * and
+     * https://github.com/cleanflight/cleanflight/issues/590#issuecomment-101706023
+     */
+    uint8_t endByte;
+} __attribute__ ((__packed__)) sbusFrame_t;
+
+
+uint16_t sbusDecodeChannelValue(uint16_t sbusValue);
+uint16_t sbusEncodeChannelValue(uint16_t rcValue);
 
 uint8_t sbusChannelsDecode(rxRuntimeConfig_t *rxRuntimeConfig, const sbusChannels_t *channels);
 
diff --git a/src/main/target/common.h b/src/main/target/common.h
index af81fa439f..52eb6eb819 100755
--- a/src/main/target/common.h
+++ b/src/main/target/common.h
@@ -46,6 +46,7 @@
 
 #if defined(STM32F4) || defined(STM32F7)
 #define USE_USB_MSC
+#define USE_SERVO_SBUS
 #endif
 
 #define USE_ADC_AVERAGING

From 0a176993ddc8a894e330c95974c295842a12a99f Mon Sep 17 00:00:00 2001
From: "Konstantin (DigitalEntity) Sharlaimov"
 <konstantin.sharlaimov@gmail.com>
Date: Fri, 1 May 2020 11:30:22 +0200
Subject: [PATCH 131/179] CLI documentation for new servo_protocol parameter

---
 docs/Cli.md | 1 +
 1 file changed, 1 insertion(+)

diff --git a/docs/Cli.md b/docs/Cli.md
index b292304274..90a98683a8 100644
--- a/docs/Cli.md
+++ b/docs/Cli.md
@@ -291,6 +291,7 @@ A shorter form is also supported to enable and disable functions using `serial <
 |  alt_hold_deadband  | 50 | Defines the deadband of throttle during alt_hold [r/c points] |
 |  yaw_motor_direction  | 1 | Use if you need to inverse yaw motor direction. |
 |  tri_unarmed_servo  | ON | On tricopter mix only, if this is set to ON, servo will always be correcting regardless of armed state. to disable this, set it to OFF. |
+|  servo_protocol  | PWM | An option to chose the protocol/option that would be used to output servo data. Possible options `PWM` (FC servo outputs), `SERVO_DRIVER` (I2C PCA9685 peripheral), `SBUS` (S.Bus protocol output via a configured serial port) |
 |  servo_lpf_hz  | 20 | Selects the servo PWM output cutoff frequency. Value is in [Hz] |
 |  servo_center_pulse  | 1500 | Servo midpoint |
 |  servo_pwm_rate  | 50 | Output frequency (in Hz) servo pins. When using tricopters or gimbal with digital servo, this rate can be increased. Max of 498Hz (for 500Hz pwm period), and min of 50Hz. Most digital servos will support for example 330Hz. |

From ac39fe1cc73dc34b08d716bd2b9211a173bac2ab Mon Sep 17 00:00:00 2001
From: Harry Kalogirou <harkal@nlogn.eu>
Date: Fri, 1 May 2020 14:56:41 +0300
Subject: [PATCH 132/179] Refactor code to use common function that take a base
 frequency

---
 src/main/navigation/navigation_fixedwing.c | 24 ++++++++++++++--------
 1 file changed, 15 insertions(+), 9 deletions(-)

diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c
index 1fbfb32193..7af61ae6ce 100755
--- a/src/main/navigation/navigation_fixedwing.c
+++ b/src/main/navigation/navigation_fixedwing.c
@@ -51,6 +51,9 @@
 
 #include "rx/rx.h"
 
+// Base frequencies for smoothing pitch and roll 
+#define NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ     2.0f      
+#define NAV_FW_BASE_ROLL_CUTOFF_FREQUENCY_HZ     10.0f
 
 // If we are going slower than NAV_FW_MIN_VEL_SPEED_BOOST - boost throttle to fight against the wind
 #define NAV_FW_THROTTLE_SPEED_BOOST_GAIN        1.5f
@@ -67,6 +70,14 @@ static bool isAutoThrottleManuallyIncreased = false;
 static int32_t navHeadingError;
 static int8_t loiterDirYaw = 1;
 
+// Calculates the cutoff frequency for smoothing out roll/pitch commands 
+// control_smoothness valid range from 0 to 9
+// resulting cutoff_freq ranging from baseFreq downwards to ~0.11Hz 
+static float getSmoothnessCutoffFreq(float baseFreq)
+{
+    uint16_t smoothness = 10 - navConfig()->fw.control_smoothness;
+    return 0.001f * baseFreq * (float)(smoothness*smoothness*smoothness) + 0.1f;
+}
 
 /*-----------------------------------------------------------
  * Altitude controller
@@ -137,11 +148,8 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros)
     // PID controller to translate energy balance error [J] into pitch angle [decideg]
     float targetPitchAngle = navPidApply3(&posControl.pids.fw_alt, demSEB, estSEB, US2S(deltaMicros), minDiveDeciDeg, maxClimbDeciDeg, 0, pitchGainInv);
 
-    // Apply low-pass filter to prevent rapid correction cutoff_freq = 0.002 * (10.0-x)^3 + epsilon 
-    // Pitch cutoff set to 1/5 the roll cutoff
-    uint16_t smoothness = 10 - navConfig()->fw.control_smoothness;
-    float cutoff_freq = 0.002f * (float)(smoothness*smoothness*smoothness) + 0.1f;
-    targetPitchAngle = pt1FilterApply4(&velzFilterState, targetPitchAngle, cutoff_freq, US2S(deltaMicros));
+    // Apply low-pass filter to prevent rapid correction
+    targetPitchAngle = pt1FilterApply4(&velzFilterState, targetPitchAngle, getSmoothnessCutoffFreq(NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ), US2S(deltaMicros));
 
     // Reconstrain pitch angle ( >0 - climb, <0 - dive)
     targetPitchAngle = constrainf(targetPitchAngle, minDiveDeciDeg, maxClimbDeciDeg);
@@ -371,10 +379,8 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
                                         DEGREES_TO_CENTIDEGREES(navConfig()->fw.max_bank_angle),
                                         pidFlags);
 
-    // Apply low-pass filter to prevent rapid correction cutoff_freq = 0.01 * (10.0-x)^3 + epsilon
-    uint16_t smoothness = 10 - navConfig()->fw.control_smoothness;
-    float cutoff_freq = 0.01f * (float)(smoothness*smoothness*smoothness) + 0.1f;
-    rollAdjustment = pt1FilterApply4(&fwPosControllerCorrectionFilterState, rollAdjustment, cutoff_freq, US2S(deltaMicros));
+    // Apply low-pass filter to prevent rapid correction
+    rollAdjustment = pt1FilterApply4(&fwPosControllerCorrectionFilterState, rollAdjustment, getSmoothnessCutoffFreq(NAV_FW_BASE_ROLL_CUTOFF_FREQUENCY_HZ), US2S(deltaMicros));
 
     // Convert rollAdjustment to decidegrees (rcAdjustment holds decidegrees)
     posControl.rcAdjustment[ROLL] = CENTIDEGREES_TO_DECIDEGREES(rollAdjustment);

From 6906d0986847b388dc1e52ac511653cd65b8b927 Mon Sep 17 00:00:00 2001
From: Harry Kalogirou <harkal@nlogn.eu>
Date: Fri, 1 May 2020 15:27:56 +0300
Subject: [PATCH 133/179] Update nav config version

---
 src/main/navigation/navigation.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index c9574a4c18..2429bb6662 100755
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -80,7 +80,7 @@ radar_pois_t radar_pois[RADAR_MAX_POIS];
 PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 0);
 #endif
 
-PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 6);
+PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 7);
 
 PG_RESET_TEMPLATE(navConfig_t, navConfig,
     .general = {

From 93e349ec3bcf1ec4de3d3a66e7a0c33ba72190ce Mon Sep 17 00:00:00 2001
From: stronnag <jh+github@daria.co.uk>
Date: Sun, 3 May 2020 07:29:18 +0100
Subject: [PATCH 134/179] check that jump target WPs are geo-referenced (#5666)

---
 src/main/navigation/navigation.c | 6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index a636c13d80..9e4871931f 100755
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -3243,6 +3243,7 @@ navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass)
          * First WP can't be JUMP
          * Can't jump to immediately adjacent WPs (pointless)
          * Can't jump beyond WP list
+         * Only jump to geo-referenced WP types
          */
     if (posControl.waypointCount > 0) {
         for (uint8_t wp = 0; wp < posControl.waypointCount ; wp++){
@@ -3250,6 +3251,11 @@ navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass)
                 if((wp == 0) || ((posControl.waypointList[wp].p1 > (wp-2)) && (posControl.waypointList[wp].p1 < (wp+2)) ) || (posControl.waypointList[wp].p1 >=  posControl.waypointCount) || (posControl.waypointList[wp].p2 < -1)) {
                     return NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR;
                 }
+                    /* check for target geo-ref sanity */
+                uint16_t target = posControl.waypointList[wp].p1;
+                if(!(posControl.waypointList[target].action == NAV_WP_ACTION_WAYPOINT || posControl.waypointList[target].action == NAV_WP_ACTION_HOLD_TIME || posControl.waypointList[target].action == NAV_WP_ACTION_LAND)) {
+                    return NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR;
+                }
             }
         }
     }

From 878503c2fd5f99c641db63e417f7e138670d08ff Mon Sep 17 00:00:00 2001
From: stronnag <jh+github@daria.co.uk>
Date: Sun, 3 May 2020 07:31:40 +0100
Subject: [PATCH 135/179] fix minor errors in document (#5667)

---
 docs/development/serial_printf_debugging.md | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/docs/development/serial_printf_debugging.md b/docs/development/serial_printf_debugging.md
index 0d6bc015ab..7aa4e919a0 100644
--- a/docs/development/serial_printf_debugging.md
+++ b/docs/development/serial_printf_debugging.md
@@ -74,7 +74,7 @@ If the CLI `log_topics` is non-zero, then all topics matching the mask will be d
 
 ## Code usage
 
-A set of macros `LOG_S()` (log system) through `LOG_D()` (log debug) may be used, subject to compile time log level constraints. These provide `printf` style logging for a given topic.
+A set of macros `LOG_E()` (log error) through `LOG_D()` (log debug) may be used, subject to compile time log level constraints. These provide `printf` style logging for a given topic.
 
 ```
 //  LOG_D(topic, fmt, ...)

From 3714d51e02aba8e23f949576770a33903a482887 Mon Sep 17 00:00:00 2001
From: Ilya guterman <amfernusus@gmail.com>
Date: Sun, 3 May 2020 23:37:26 +0300
Subject: [PATCH 136/179] cli: aux command print permamentId (#5654)

*Breaking change* change aux modes from boxid to permanentId
---
 src/main/fc/cli.c | 17 ++++++++++++-----
 1 file changed, 12 insertions(+), 5 deletions(-)

diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c
index 6817db0740..e4c9802c78 100755
--- a/src/main/fc/cli.c
+++ b/src/main/fc/cli.c
@@ -96,6 +96,8 @@ extern uint8_t __config_end;
 #include "io/osd.h"
 #include "io/serial.h"
 
+#include "fc/fc_msp_box.h"
+
 #include "navigation/navigation.h"
 #include "navigation/navigation_private.h"
 
@@ -648,17 +650,19 @@ static void printAux(uint8_t dumpMask, const modeActivationCondition_t *modeActi
                 && mac->auxChannelIndex == macDefault->auxChannelIndex
                 && mac->range.startStep == macDefault->range.startStep
                 && mac->range.endStep == macDefault->range.endStep;
+            const box_t *box = findBoxByActiveBoxId(macDefault->modeId);
             cliDefaultPrintLinef(dumpMask, equalsDefault, format,
                 i,
-                macDefault->modeId,
+                box->permanentId,
                 macDefault->auxChannelIndex,
                 MODE_STEP_TO_CHANNEL_VALUE(macDefault->range.startStep),
                 MODE_STEP_TO_CHANNEL_VALUE(macDefault->range.endStep)
             );
         }
+        const box_t *box = findBoxByActiveBoxId(mac->modeId);
         cliDumpPrintLinef(dumpMask, equalsDefault, format,
             i,
-            mac->modeId,
+            box->permanentId,
             mac->auxChannelIndex,
             MODE_STEP_TO_CHANNEL_VALUE(mac->range.startStep),
             MODE_STEP_TO_CHANNEL_VALUE(mac->range.endStep)
@@ -682,9 +686,12 @@ static void cliAux(char *cmdline)
             ptr = nextArg(ptr);
             if (ptr) {
                 val = fastA2I(ptr);
-                if (val >= 0 && val < CHECKBOX_ITEM_COUNT) {
-                    mac->modeId = val;
-                    validArgumentCount++;
+                if (val >= 0) {
+                    const box_t *box = findBoxByPermanentId(val);
+                    if (box != NULL) {
+                        mac->modeId = box->boxId;
+                        validArgumentCount++;
+                    }
                 }
             }
             ptr = nextArg(ptr);

From 7a2b63abfb2f98ade76ea59533aa4f9a26a1c5d4 Mon Sep 17 00:00:00 2001
From: "Konstantin (DigitalEntity) Sharlaimov"
 <konstantin.sharlaimov@gmail.com>
Date: Mon, 4 May 2020 12:31:12 +0200
Subject: [PATCH 137/179] Fix S.Bus parity and stop bit count

---
 src/main/io/servo_sbus.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/io/servo_sbus.c b/src/main/io/servo_sbus.c
index aac649cb50..ba0edcc532 100644
--- a/src/main/io/servo_sbus.c
+++ b/src/main/io/servo_sbus.c
@@ -42,7 +42,7 @@
 #if defined(USE_SERVO_SBUS)
 
 #define SERVO_SBUS_UART_BAUD            100000
-#define SERVO_SBUS_OPTIONS              (SERIAL_INVERTED | SERIAL_UNIDIR)
+#define SERVO_SBUS_OPTIONS              (SBUS_PORT_OPTIONS | SERIAL_INVERTED | SERIAL_UNIDIR)
 
 static serialPort_t * servoSbusPort = NULL;
 static sbusFrame_t sbusFrame;

From 0408358e04a6eccfb35e1486757ab47d17de6c86 Mon Sep 17 00:00:00 2001
From: "Konstantin (DigitalEntity) Sharlaimov"
 <konstantin.sharlaimov@gmail.com>
Date: Sun, 3 May 2020 14:50:08 +0200
Subject: [PATCH 138/179] Initial cut on per-gyro alignment setting. Will allow
 dual-gyro with different alignment to be used

---
 src/main/drivers/accgyro/accgyro_adxl345.c    |   1 +
 src/main/drivers/accgyro/accgyro_bma280.c     |   1 +
 src/main/drivers/accgyro/accgyro_bmi160.c     |   1 +
 src/main/drivers/accgyro/accgyro_fake.c       |   2 +
 src/main/drivers/accgyro/accgyro_icm20689.c   |   2 +
 src/main/drivers/accgyro/accgyro_l3g4200d.c   |   1 +
 src/main/drivers/accgyro/accgyro_l3gd20.c     |   1 +
 src/main/drivers/accgyro/accgyro_lsm303dlhc.c |   1 +
 src/main/drivers/accgyro/accgyro_mma845x.c    |   1 +
 src/main/drivers/accgyro/accgyro_mpu3050.c    |   1 +
 src/main/drivers/accgyro/accgyro_mpu6000.c    |   2 +
 src/main/drivers/accgyro/accgyro_mpu6050.c    |   2 +
 src/main/drivers/accgyro/accgyro_mpu6500.c    |   2 +
 src/main/drivers/accgyro/accgyro_mpu9250.c    |   2 +
 src/main/drivers/bus.c                        |   3 +-
 src/main/drivers/bus.h                        |  88 +++++++-------
 src/main/platform.h                           |   1 +
 src/main/sensors/acceleration.c               |  30 +----
 src/main/sensors/gyro.c                       |  29 +----
 src/main/target/AIRBOTF7/target.c             |   9 +-
 src/main/target/AIRBOTF7/target_abf7.h        |   4 -
 src/main/target/common_hardware.c             | 108 +++++++++---------
 src/main/target/sanity_check.h                |  71 ++++++++++++
 23 files changed, 209 insertions(+), 154 deletions(-)
 create mode 100644 src/main/target/sanity_check.h

diff --git a/src/main/drivers/accgyro/accgyro_adxl345.c b/src/main/drivers/accgyro/accgyro_adxl345.c
index e2e5dc3c2c..a2b73c66ba 100644
--- a/src/main/drivers/accgyro/accgyro_adxl345.c
+++ b/src/main/drivers/accgyro/accgyro_adxl345.c
@@ -110,6 +110,7 @@ bool adxl345Detect(accDev_t *acc)
 
     acc->initFn = adxl345Init;
     acc->readFn = adxl345Read;
+    acc->accAlign = acc->busDev->param;
     return true;
 }
 
diff --git a/src/main/drivers/accgyro/accgyro_bma280.c b/src/main/drivers/accgyro/accgyro_bma280.c
index 0f57186bcb..6fcb4008bb 100644
--- a/src/main/drivers/accgyro/accgyro_bma280.c
+++ b/src/main/drivers/accgyro/accgyro_bma280.c
@@ -86,6 +86,7 @@ bool bma280Detect(accDev_t *acc)
 
     acc->initFn = bma280Init;
     acc->readFn = bma280Read;
+    acc->accAlign = acc->busDev->param;
     return true;
 }
 
diff --git a/src/main/drivers/accgyro/accgyro_bmi160.c b/src/main/drivers/accgyro/accgyro_bmi160.c
index e84a9046fc..15ed8634e1 100644
--- a/src/main/drivers/accgyro/accgyro_bmi160.c
+++ b/src/main/drivers/accgyro/accgyro_bmi160.c
@@ -270,6 +270,7 @@ bool bmi160GyroDetect(gyroDev_t *gyro)
     gyro->intStatusFn = gyroCheckDataReady;
     gyro->temperatureFn = NULL;
     gyro->scale = 1.0f / 16.4f;     // 16.4 dps/lsb scalefactor
+    gyro->gyroAlign = gyro->busDev->param;
 
     return true;
 }
diff --git a/src/main/drivers/accgyro/accgyro_fake.c b/src/main/drivers/accgyro/accgyro_fake.c
index 473691c20e..19d4f2498b 100644
--- a/src/main/drivers/accgyro/accgyro_fake.c
+++ b/src/main/drivers/accgyro/accgyro_fake.c
@@ -71,6 +71,7 @@ bool fakeGyroDetect(gyroDev_t *gyro)
     gyro->readFn = fakeGyroRead;
     gyro->temperatureFn = fakeGyroReadTemperature;
     gyro->scale = 1.0f / 16.4f;
+    gyro->gyroAlign = 0;
     return true;
 }
 #endif // USE_FAKE_GYRO
@@ -104,6 +105,7 @@ bool fakeAccDetect(accDev_t *acc)
 {
     acc->initFn = fakeAccInit;
     acc->readFn = fakeAccRead;
+    acc->accAlign = 0;
     return true;
 }
 #endif // USE_FAKE_ACC
diff --git a/src/main/drivers/accgyro/accgyro_icm20689.c b/src/main/drivers/accgyro/accgyro_icm20689.c
index 4766e5aa94..de7f33d967 100644
--- a/src/main/drivers/accgyro/accgyro_icm20689.c
+++ b/src/main/drivers/accgyro/accgyro_icm20689.c
@@ -82,6 +82,7 @@ bool icm20689AccDetect(accDev_t *acc)
 
     acc->initFn = icm20689AccInit;
     acc->readFn = mpuAccReadScratchpad;
+    acc->accAlign = acc->busDev->param;
 
     return true;
 }
@@ -142,6 +143,7 @@ bool icm20689GyroDetect(gyroDev_t *gyro)
     gyro->intStatusFn = gyroCheckDataReady;
     gyro->temperatureFn = mpuTemperatureReadScratchpad;
     gyro->scale = 1.0f / 16.4f;     // 16.4 dps/lsb scalefactor
+    gyro->gyroAlign = gyro->busDev->param;
 
     return true;
 }
diff --git a/src/main/drivers/accgyro/accgyro_l3g4200d.c b/src/main/drivers/accgyro/accgyro_l3g4200d.c
index 1fee8f712f..e674973bf2 100644
--- a/src/main/drivers/accgyro/accgyro_l3g4200d.c
+++ b/src/main/drivers/accgyro/accgyro_l3g4200d.c
@@ -139,6 +139,7 @@ bool l3g4200dDetect(gyroDev_t *gyro)
     gyro->initFn = l3g4200dInit;
     gyro->readFn = l3g4200dRead;
     gyro->scale = 1.0f / 14.2857f;      // 14.2857dps/lsb scalefactor
+    gyro->gyroAlign = gyro->busDev->param;
 
     return true;
 }
diff --git a/src/main/drivers/accgyro/accgyro_l3gd20.c b/src/main/drivers/accgyro/accgyro_l3gd20.c
index 29e78fe231..2705d3dc3b 100644
--- a/src/main/drivers/accgyro/accgyro_l3gd20.c
+++ b/src/main/drivers/accgyro/accgyro_l3gd20.c
@@ -116,6 +116,7 @@ bool l3gd20Detect(gyroDev_t *gyro)
 
     // Page 9 in datasheet, So - Sensitivity, Full Scale = 2000, 70 mdps/digit
     gyro->scale = 0.07f;
+    gyro->gyroAlign = gyro->busDev->param;
 
     return true;
 }
diff --git a/src/main/drivers/accgyro/accgyro_lsm303dlhc.c b/src/main/drivers/accgyro/accgyro_lsm303dlhc.c
index 6f7f84b830..596453f6ef 100644
--- a/src/main/drivers/accgyro/accgyro_lsm303dlhc.c
+++ b/src/main/drivers/accgyro/accgyro_lsm303dlhc.c
@@ -163,6 +163,7 @@ bool lsm303dlhcAccDetect(accDev_t *acc)
 
     acc->initFn = lsm303dlhcAccInit;
     acc->readFn = lsm303dlhcAccRead;
+    acc->accAlign = acc->busDev->param;
     return true;
 }
 
diff --git a/src/main/drivers/accgyro/accgyro_mma845x.c b/src/main/drivers/accgyro/accgyro_mma845x.c
index 1f41910f56..0a5a9844aa 100644
--- a/src/main/drivers/accgyro/accgyro_mma845x.c
+++ b/src/main/drivers/accgyro/accgyro_mma845x.c
@@ -139,5 +139,6 @@ bool mma8452Detect(accDev_t *acc)
 
     acc->initFn = mma8452Init;
     acc->readFn = mma8452Read;
+    acc->accAlign = acc->busDev->param;
     return true;
 }
diff --git a/src/main/drivers/accgyro/accgyro_mpu3050.c b/src/main/drivers/accgyro/accgyro_mpu3050.c
index bb3d919fd7..4cebe2c79f 100644
--- a/src/main/drivers/accgyro/accgyro_mpu3050.c
+++ b/src/main/drivers/accgyro/accgyro_mpu3050.c
@@ -131,6 +131,7 @@ bool mpu3050Detect(gyroDev_t *gyro)
     gyro->readFn = mpu3050GyroRead;
     gyro->intStatusFn = gyroCheckDataReady;
     gyro->scale = 1.0f / 16.4f;     // 16.4 dps/lsb scalefactor
+    gyro->gyroAlign = gyro->busDev->param;
 
     return true;
 }
diff --git a/src/main/drivers/accgyro/accgyro_mpu6000.c b/src/main/drivers/accgyro/accgyro_mpu6000.c
index e0396f29ec..d447138e58 100644
--- a/src/main/drivers/accgyro/accgyro_mpu6000.c
+++ b/src/main/drivers/accgyro/accgyro_mpu6000.c
@@ -150,6 +150,7 @@ bool mpu6000AccDetect(accDev_t *acc)
 
     acc->initFn = mpu6000AccInit;
     acc->readFn = mpuAccReadScratchpad;
+    acc->accAlign = acc->busDev->param;
 
     return true;
 }
@@ -218,6 +219,7 @@ bool mpu6000GyroDetect(gyroDev_t *gyro)
     gyro->intStatusFn = gyroCheckDataReady;
     gyro->temperatureFn = mpuTemperatureReadScratchpad;
     gyro->scale = 1.0f / 16.4f;     // 16.4 dps/lsb scalefactor
+    gyro->gyroAlign = gyro->busDev->param;
 
     return true;
 }
diff --git a/src/main/drivers/accgyro/accgyro_mpu6050.c b/src/main/drivers/accgyro/accgyro_mpu6050.c
index b6c8671691..0a160d59ef 100755
--- a/src/main/drivers/accgyro/accgyro_mpu6050.c
+++ b/src/main/drivers/accgyro/accgyro_mpu6050.c
@@ -127,6 +127,7 @@ bool mpu6050AccDetect(accDev_t *acc)
     if (ctx->chipMagicNumber == 0x6850 || ctx->chipMagicNumber == 0x6050) {
         acc->initFn = mpu6050AccInit;
         acc->readFn = mpuAccReadScratchpad;
+        acc->accAlign = acc->busDev->param;
         return true;
     }
 
@@ -213,6 +214,7 @@ bool mpu6050GyroDetect(gyroDev_t *gyro)
     gyro->intStatusFn = gyroCheckDataReady;
     gyro->temperatureFn = mpuTemperatureReadScratchpad;
     gyro->scale = 1.0f / 16.4f;     // 16.4 dps/lsb scalefactor
+    gyro->gyroAlign = gyro->busDev->param;
 
     return true;
 }
diff --git a/src/main/drivers/accgyro/accgyro_mpu6500.c b/src/main/drivers/accgyro/accgyro_mpu6500.c
index 532fd03606..79e457c649 100755
--- a/src/main/drivers/accgyro/accgyro_mpu6500.c
+++ b/src/main/drivers/accgyro/accgyro_mpu6500.c
@@ -60,6 +60,7 @@ bool mpu6500AccDetect(accDev_t *acc)
 
     acc->initFn = mpu6500AccInit;
     acc->readFn = mpuAccReadScratchpad;
+    acc->accAlign = acc->busDev->param;
 
     return true;
 }
@@ -163,6 +164,7 @@ bool mpu6500GyroDetect(gyroDev_t *gyro)
     gyro->intStatusFn = gyroCheckDataReady;
     gyro->temperatureFn = mpuTemperatureReadScratchpad;
     gyro->scale = 1.0f / 16.4f;     // 16.4 dps/lsb scalefactor
+    gyro->gyroAlign = gyro->busDev->param;
 
     return true;
 }
diff --git a/src/main/drivers/accgyro/accgyro_mpu9250.c b/src/main/drivers/accgyro/accgyro_mpu9250.c
index 03801eeb89..fe020b3047 100755
--- a/src/main/drivers/accgyro/accgyro_mpu9250.c
+++ b/src/main/drivers/accgyro/accgyro_mpu9250.c
@@ -60,6 +60,7 @@ bool mpu9250AccDetect(accDev_t *acc)
 
     acc->initFn = mpu9250AccInit;
     acc->readFn = mpuAccReadScratchpad;
+    acc->accAlign = acc->busDev->param;
 
     return true;
 }
@@ -163,6 +164,7 @@ bool mpu9250GyroDetect(gyroDev_t *gyro)
     gyro->intStatusFn = gyroCheckDataReady;
     gyro->temperatureFn = mpuTemperatureReadScratchpad;
     gyro->scale = 1.0f / 16.4f;     // 16.4 dps/lsb scalefactor
+    gyro->gyroAlign = gyro->busDev->param;
 
     return true;
 }
diff --git a/src/main/drivers/bus.c b/src/main/drivers/bus.c
index e9b8b8f433..62126fe025 100755
--- a/src/main/drivers/bus.c
+++ b/src/main/drivers/bus.c
@@ -32,7 +32,7 @@
 #include "drivers/bus.h"
 #include "drivers/io.h"
 
-#define BUSDEV_MAX_DEVICES 8
+#define BUSDEV_MAX_DEVICES 16
 
 #ifdef USE_SPI
 static void busDevPreInit_SPI(const busDeviceDescriptor_t * descriptor)
@@ -124,6 +124,7 @@ busDevice_t * busDeviceInit(busType_e bus, devHardwareType_e hw, uint8_t tag, re
                 dev->descriptorPtr = descriptor;
                 dev->busType = descriptor->busType;
                 dev->flags = descriptor->flags;
+                dev->param = descriptor->param;
 
                 switch (descriptor->busType) {
                     default:
diff --git a/src/main/drivers/bus.h b/src/main/drivers/bus.h
index 843404573f..18bea5b63b 100755
--- a/src/main/drivers/bus.h
+++ b/src/main/drivers/bus.h
@@ -148,6 +148,8 @@ typedef enum {
 typedef enum {
     DEVFLAGS_NONE                       = 0,
     DEVFLAGS_USE_RAW_REGISTERS          = (1 << 0),     // Don't manipulate MSB for R/W selection (SPI), allow using 0xFF register to raw i2c reads/writes
+
+    // SPI-only
     DEVFLAGS_USE_MANUAL_DEVICE_SELECT   = (1 << 1),     // (SPI only) Don't automatically select/deselect device
     DEVFLAGS_SPI_MODE_0                 = (1 << 2),     // (SPI only) Use CPOL=0/CPHA=0 (if unset MODE3 is used - CPOL=1/CPHA=1)
 } deviceFlags_e;
@@ -156,8 +158,9 @@ typedef struct busDeviceDescriptor_s {
     void *              devicePtr;
     busType_e           busType;
     devHardwareType_e   devHwType;
-    uint16_t            flags;
+    uint8_t             flags;
     uint8_t             tag;
+    uint8_t             param;      // Driver-specific parameter
     union {
 #ifdef USE_SPI
         struct {
@@ -179,6 +182,7 @@ typedef struct busDevice_s {
     const busDeviceDescriptor_t * descriptorPtr;
     busType_e busType;              // Copy of busType to avoid additional pointer dereferencing
     uint32_t flags;                 // Copy of flags
+    uint32_t param;                 // Copy of param
     union {
 #ifdef USE_SPI
         struct {
@@ -209,53 +213,55 @@ extern const busDeviceDescriptor_t __busdev_registry_end[];
 #define BUSDEV_REGISTER_ATTRIBUTES __attribute__ ((section(".busdev_registry"), used, aligned(4)))
 #endif
 
-#define BUSDEV_REGISTER_SPI_F(_name, _devHw, _spiBus, _csnPin, _irqPin, _tag, _flags)       \
-    extern const busDeviceDescriptor_t _name ## _registry;                                  \
-    static busDevice_t _name ## _memory;                                                    \
-    const busDeviceDescriptor_t _name ## _registry BUSDEV_REGISTER_ATTRIBUTES = {           \
-        .devicePtr = (void *) & _name ## _memory,                                           \
-        .busType = BUSTYPE_SPI,                                                             \
-        .devHwType = _devHw,                                                                \
-        .flags = _flags,                                                                    \
-        .tag = _tag,                                                                        \
-        .busdev.spi = {                                                                     \
-            .spiBus = _spiBus,                                                              \
-            .csnPin = IO_TAG(_csnPin)                                                       \
-        },                                                                                  \
-        .irqPin = IO_TAG(_irqPin)                                                           \
-    };                                                                                      \
-    struct _dummy                                                                           \
+#define BUSDEV_REGISTER_SPI_F(_name, _devHw, _spiBus, _csnPin, _irqPin, _tag, _flags, _param)   \
+    extern const busDeviceDescriptor_t _name ## _registry;                                      \
+    static busDevice_t _name ## _memory;                                                        \
+    const busDeviceDescriptor_t _name ## _registry BUSDEV_REGISTER_ATTRIBUTES = {               \
+        .devicePtr = (void *) & _name ## _memory,                                               \
+        .busType = BUSTYPE_SPI,                                                                 \
+        .devHwType = _devHw,                                                                    \
+        .flags = _flags,                                                                        \
+        .tag = _tag,                                                                            \
+        .param = _param,                                                                        \
+        .busdev.spi = {                                                                         \
+            .spiBus = _spiBus,                                                                  \
+            .csnPin = IO_TAG(_csnPin)                                                           \
+        },                                                                                      \
+        .irqPin = IO_TAG(_irqPin)                                                               \
+    };                                                                                          \
+    struct _dummy                                                                               \
     /**/
 
-#define BUSDEV_REGISTER_I2C_F(_name, _devHw, _i2cBus, _devAddr, _irqPin, _tag, _flags)      \
-    extern const busDeviceDescriptor_t _name ## _registry;                                  \
-    static busDevice_t _name ## _memory;                                                    \
-    const busDeviceDescriptor_t _name ## _registry BUSDEV_REGISTER_ATTRIBUTES = {           \
-        .devicePtr = (void *) & _name ## _memory,                                           \
-        .busType = BUSTYPE_I2C,                                                             \
-        .devHwType = _devHw,                                                                \
-        .flags = _flags,                                                                    \
-        .tag = _tag,                                                                        \
-        .busdev.i2c = {                                                                     \
-            .i2cBus = _i2cBus,                                                              \
-            .address = _devAddr                                                             \
-        },                                                                                  \
-        .irqPin = IO_TAG(_irqPin)                                                           \
-    };                                                                                      \
-    struct _dummy                                                                           \
+#define BUSDEV_REGISTER_I2C_F(_name, _devHw, _i2cBus, _devAddr, _irqPin, _tag, _flags, _param)  \
+    extern const busDeviceDescriptor_t _name ## _registry;                                      \
+    static busDevice_t _name ## _memory;                                                        \
+    const busDeviceDescriptor_t _name ## _registry BUSDEV_REGISTER_ATTRIBUTES = {               \
+        .devicePtr = (void *) & _name ## _memory,                                               \
+        .busType = BUSTYPE_I2C,                                                                 \
+        .devHwType = _devHw,                                                                    \
+        .flags = _flags,                                                                        \
+        .tag = _tag,                                                                            \
+        .param = _param,                                                                        \
+        .busdev.i2c = {                                                                         \
+            .i2cBus = _i2cBus,                                                                  \
+            .address = _devAddr                                                                 \
+        },                                                                                      \
+        .irqPin = IO_TAG(_irqPin)                                                               \
+    };                                                                                          \
+    struct _dummy                                                                               \
     /**/
 
-#define BUSDEV_REGISTER_SPI(_name, _devHw, _spiBus, _csnPin, _irqPin, _flags)               \
-    BUSDEV_REGISTER_SPI_F(_name, _devHw, _spiBus, _csnPin, _irqPin, 0, _flags)
+#define BUSDEV_REGISTER_SPI(_name, _devHw, _spiBus, _csnPin, _irqPin, _flags, _param)           \
+    BUSDEV_REGISTER_SPI_F(_name, _devHw, _spiBus, _csnPin, _irqPin, 0, _flags, _param)
 
-#define BUSDEV_REGISTER_SPI_TAG(_name, _devHw, _spiBus, _csnPin, _irqPin, _tag, _flags)     \
-    BUSDEV_REGISTER_SPI_F(_name, _devHw, _spiBus, _csnPin, _irqPin, _tag, _flags)
+#define BUSDEV_REGISTER_SPI_TAG(_name, _devHw, _spiBus, _csnPin, _irqPin, _tag, _flags, _param) \
+    BUSDEV_REGISTER_SPI_F(_name, _devHw, _spiBus, _csnPin, _irqPin, _tag, _flags, _param)
 
-#define BUSDEV_REGISTER_I2C(_name, _devHw, _i2cBus, _devAddr, _irqPin, _flags)              \
-    BUSDEV_REGISTER_I2C_F(_name, _devHw, _i2cBus, _devAddr, _irqPin, 0, _flags)
+#define BUSDEV_REGISTER_I2C(_name, _devHw, _i2cBus, _devAddr, _irqPin, _flags, _param)          \
+    BUSDEV_REGISTER_I2C_F(_name, _devHw, _i2cBus, _devAddr, _irqPin, 0, _flags, _param)
 
-#define BUSDEV_REGISTER_I2C_TAG(_name, _devHw, _i2cBus, _devAddr, _irqPin, _tag, _flags)    \
-    BUSDEV_REGISTER_I2C_F(_name, _devHw, _i2cBus, _devAddr, _irqPin, _tag, _flags)
+#define BUSDEV_REGISTER_I2C_TAG(_name, _devHw, _i2cBus, _devAddr, _irqPin, _tag, _flags, _param)\
+    BUSDEV_REGISTER_I2C_F(_name, _devHw, _i2cBus, _devAddr, _irqPin, _tag, _flags, _param)
 
 
 // busTransfer and busTransferMultiple are supported only on full-duplex SPI bus
diff --git a/src/main/platform.h b/src/main/platform.h
index 640ded15aa..110ea783d0 100644
--- a/src/main/platform.h
+++ b/src/main/platform.h
@@ -62,6 +62,7 @@
 
 #include "target/common.h"
 #include "target.h"
+#include "target/sanity_check.h"
 #include "target/common_post.h"
 
 // Remove the unaligned packed structure member pointer access warning
diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c
index eb537daf7f..5e7b0b9072 100644
--- a/src/main/sensors/acceleration.c
+++ b/src/main/sensors/acceleration.c
@@ -129,7 +129,7 @@ static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
     case ACC_ADXL345: {
         if (adxl345Detect(dev)) {
 #ifdef ACC_ADXL345_ALIGN
-            dev->accAlign = ACC_ADXL345_ALIGN;
+             = ACC_ADXL345_ALIGN;
 #endif
             accHardware = ACC_ADXL345;
             break;
@@ -146,7 +146,7 @@ static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
     case ACC_LSM303DLHC:
         if (lsm303dlhcAccDetect(dev)) {
 #ifdef ACC_LSM303DLHC_ALIGN
-            dev->accAlign = ACC_LSM303DLHC_ALIGN;
+             = ACC_LSM303DLHC_ALIGN;
 #endif
             accHardware = ACC_LSM303DLHC;
             break;
@@ -161,9 +161,6 @@ static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
 #ifdef USE_ACC_MPU6050
     case ACC_MPU6050: // MPU6050
         if (mpu6050AccDetect(dev)) {
-#ifdef ACC_MPU6050_ALIGN
-            dev->accAlign = ACC_MPU6050_ALIGN;
-#endif
             accHardware = ACC_MPU6050;
             break;
         }
@@ -178,9 +175,6 @@ static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
     case ACC_MMA8452: // MMA8452
 
         if (mma8452Detect(dev)) {
-#ifdef ACC_MMA8452_ALIGN
-            dev->accAlign = ACC_MMA8452_ALIGN;
-#endif
             accHardware = ACC_MMA8452;
             break;
         }
@@ -194,9 +188,6 @@ static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
 #ifdef USE_ACC_BMA280
     case ACC_BMA280: // BMA280
         if (bma280Detect(dev)) {
-#ifdef ACC_BMA280_ALIGN
-            dev->accAlign = ACC_BMA280_ALIGN;
-#endif
             accHardware = ACC_BMA280;
             break;
         }
@@ -210,9 +201,6 @@ static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
 #ifdef USE_ACC_MPU6000
     case ACC_MPU6000:
         if (mpu6000AccDetect(dev)) {
-#ifdef ACC_MPU6000_ALIGN
-            dev->accAlign = ACC_MPU6000_ALIGN;
-#endif
             accHardware = ACC_MPU6000;
             break;
         }
@@ -226,9 +214,6 @@ static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
 #if defined(USE_ACC_MPU6500)
     case ACC_MPU6500:
         if (mpu6500AccDetect(dev)) {
-#ifdef ACC_MPU6500_ALIGN
-            dev->accAlign = ACC_MPU6500_ALIGN;
-#endif
             accHardware = ACC_MPU6500;
             break;
         }
@@ -242,9 +227,6 @@ static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
 #if defined(USE_ACC_MPU9250)
     case ACC_MPU9250:
         if (mpu9250AccDetect(dev)) {
-#ifdef ACC_MPU9250_ALIGN
-            dev->accAlign = ACC_MPU9250_ALIGN;
-#endif
             accHardware = ACC_MPU9250;
             break;
         }
@@ -258,9 +240,6 @@ static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
 #if defined(USE_ACC_BMI160)
     case ACC_BMI160:
         if (bmi160AccDetect(dev)) {
-#ifdef ACC_BMI160_ALIGN
-            dev->accAlign = ACC_BMI160_ALIGN;
-#endif
             accHardware = ACC_BMI160;
             break;
         }
@@ -274,9 +253,6 @@ static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
 #ifdef USE_ACC_ICM20689
     case ACC_ICM20689:
         if (icm20689AccDetect(dev)) {
-#ifdef ACC_ICM20689_ALIGN
-            dev->accAlign = ACC_ICM20689_ALIGN;
-#endif
             accHardware = ACC_ICM20689;
             break;
         }
@@ -342,6 +318,8 @@ bool accInit(uint32_t targetLooptime)
         acc.extremes[axis].max = -100;
     }
 
+    // At this poinrt acc.dev.accAlign was set up by the driver from the busDev record
+    // If configuration says different - override
     if (accelerometerConfig()->acc_align != ALIGN_DEFAULT) {
         acc.dev.accAlign = accelerometerConfig()->acc_align;
     }
diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index a12bc8ef15..dd6188bb06 100644
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@@ -138,9 +138,6 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHard
     case GYRO_MPU6050:
         if (mpu6050GyroDetect(dev)) {
             gyroHardware = GYRO_MPU6050;
-#ifdef GYRO_MPU6050_ALIGN
-            dev->gyroAlign = GYRO_MPU6050_ALIGN;
-#endif
             break;
         }
         FALLTHROUGH;
@@ -150,9 +147,6 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHard
     case GYRO_L3G4200D:
         if (l3g4200dDetect(dev)) {
             gyroHardware = GYRO_L3G4200D;
-#ifdef GYRO_L3G4200D_ALIGN
-            dev->gyroAlign = GYRO_L3G4200D_ALIGN;
-#endif
             break;
         }
         FALLTHROUGH;
@@ -162,9 +156,6 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHard
     case GYRO_MPU3050:
         if (mpu3050Detect(dev)) {
             gyroHardware = GYRO_MPU3050;
-#ifdef GYRO_MPU3050_ALIGN
-            dev->gyroAlign = GYRO_MPU3050_ALIGN;
-#endif
             break;
         }
         FALLTHROUGH;
@@ -174,9 +165,6 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHard
     case GYRO_L3GD20:
         if (l3gd20Detect(dev)) {
             gyroHardware = GYRO_L3GD20;
-#ifdef GYRO_L3GD20_ALIGN
-            dev->gyroAlign = GYRO_L3GD20_ALIGN;
-#endif
             break;
         }
         FALLTHROUGH;
@@ -186,9 +174,6 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHard
     case GYRO_MPU6000:
         if (mpu6000GyroDetect(dev)) {
             gyroHardware = GYRO_MPU6000;
-#ifdef GYRO_MPU6000_ALIGN
-            dev->gyroAlign = GYRO_MPU6000_ALIGN;
-#endif
             break;
         }
         FALLTHROUGH;
@@ -198,9 +183,6 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHard
     case GYRO_MPU6500:
         if (mpu6500GyroDetect(dev)) {
             gyroHardware = GYRO_MPU6500;
-#ifdef GYRO_MPU6500_ALIGN
-            dev->gyroAlign = GYRO_MPU6500_ALIGN;
-#endif
             break;
         }
         FALLTHROUGH;
@@ -210,9 +192,6 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHard
     case GYRO_MPU9250:
         if (mpu9250GyroDetect(dev)) {
             gyroHardware = GYRO_MPU9250;
-#ifdef GYRO_MPU9250_ALIGN
-            dev->gyroAlign = GYRO_MPU9250_ALIGN;
-#endif
             break;
         }
         FALLTHROUGH;
@@ -222,9 +201,6 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHard
     case GYRO_BMI160:
         if (bmi160GyroDetect(dev)) {
             gyroHardware = GYRO_BMI160;
-#ifdef GYRO_BMI160_ALIGN
-            dev->gyroAlign = GYRO_BMI160_ALIGN;
-#endif
             break;
         }
         FALLTHROUGH;
@@ -234,9 +210,6 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHard
     case GYRO_ICM20689:
         if (icm20689GyroDetect(dev)) {
             gyroHardware = GYRO_ICM20689;
-#ifdef GYRO_ICM20689_ALIGN
-            dev->gyroAlign = GYRO_ICM20689_ALIGN;
-#endif
             break;
         }
         FALLTHROUGH;
@@ -342,6 +315,8 @@ bool gyroInit(void)
     // initFn will initialize sampleRateIntervalUs to actual gyro sampling rate (if driver supports it). Calculate target looptime using that value
     gyro.targetLooptime = gyroConfig()->gyroSync ? gyroDev[0].sampleRateIntervalUs : gyroConfig()->looptime;
 
+    // At this poinrt gyroDev[0].gyroAlign was set up by the driver from the busDev record
+    // If configuration says different - override
     if (gyroConfig()->gyro_align != ALIGN_DEFAULT) {
         gyroDev[0].gyroAlign = gyroConfig()->gyro_align;
     }
diff --git a/src/main/target/AIRBOTF7/target.c b/src/main/target/AIRBOTF7/target.c
index 72592069c5..a24923a5cb 100644
--- a/src/main/target/AIRBOTF7/target.c
+++ b/src/main/target/AIRBOTF7/target.c
@@ -29,15 +29,16 @@
 #include "drivers/pwm_mapping.h"
 #include "drivers/timer.h"
 #include "drivers/bus.h"
+#include "drivers/sensor.h"
 
 // Board hardware definitions
-BUSDEV_REGISTER_SPI_TAG(busdev_imu0_mpu6000,    DEVHW_MPU6000,  GYRO_0_SPI_BUS,  GYRO_0_CS_PIN,   GYRO_0_EXTI_PIN,     0,  DEVFLAGS_NONE);
-BUSDEV_REGISTER_SPI_TAG(busdev_imu0_mpu6500,    DEVHW_MPU6500,  GYRO_0_SPI_BUS,  GYRO_0_CS_PIN,   GYRO_0_EXTI_PIN,     0,  DEVFLAGS_NONE);
+BUSDEV_REGISTER_SPI_TAG(busdev_imu0_mpu6000,    DEVHW_MPU6000,  GYRO_0_SPI_BUS,  GYRO_0_CS_PIN,   GYRO_0_EXTI_PIN,     0,  DEVFLAGS_NONE,   GYRO_0_ALIGN);
+BUSDEV_REGISTER_SPI_TAG(busdev_imu0_mpu6500,    DEVHW_MPU6500,  GYRO_0_SPI_BUS,  GYRO_0_CS_PIN,   GYRO_0_EXTI_PIN,     0,  DEVFLAGS_NONE,   GYRO_0_ALIGN);
 
 // OMNIBUSF7NANOV7 doesn't have a second gyro
 #ifndef OMNIBUSF7NANOV7
-BUSDEV_REGISTER_SPI_TAG(busdev_imu1_mpu6000,    DEVHW_MPU6000,  GYRO_1_SPI_BUS,  GYRO_1_CS_PIN,   GYRO_1_EXTI_PIN,     1,  DEVFLAGS_NONE);
-BUSDEV_REGISTER_SPI_TAG(busdev_imu1_mpu6500,    DEVHW_MPU6500,  GYRO_1_SPI_BUS,  GYRO_1_CS_PIN,   GYRO_1_EXTI_PIN,     1,  DEVFLAGS_NONE);
+BUSDEV_REGISTER_SPI_TAG(busdev_imu1_mpu6000,    DEVHW_MPU6000,  GYRO_1_SPI_BUS,  GYRO_1_CS_PIN,   GYRO_1_EXTI_PIN,     1,  DEVFLAGS_NONE,   GYRO_1_ALIGN);
+BUSDEV_REGISTER_SPI_TAG(busdev_imu1_mpu6500,    DEVHW_MPU6500,  GYRO_1_SPI_BUS,  GYRO_1_CS_PIN,   GYRO_1_EXTI_PIN,     1,  DEVFLAGS_NONE,   GYRO_1_ALIGN);
 #endif
 
 const timerHardware_t timerHardware[] = {
diff --git a/src/main/target/AIRBOTF7/target_abf7.h b/src/main/target/AIRBOTF7/target_abf7.h
index 80753c3acb..bced32c4b0 100644
--- a/src/main/target/AIRBOTF7/target_abf7.h
+++ b/src/main/target/AIRBOTF7/target_abf7.h
@@ -50,10 +50,6 @@
 #define GYRO_1_EXTI_PIN         NONE
 #define GYRO_1_ALIGN            CW0_DEG     // This doesn't work yet, requires BUS refactoring
 
-// TODO: Remove this once per-gyro alignment is supported correctly
-#define GYRO_MPU6500_ALIGN      CW90_DEG
-#define ACC_MPU6500_ALIGN       CW90_DEG
-
 // *************** FLASH **************************
 #define USE_FLASHFS
 #define USE_FLASH_M25P16
diff --git a/src/main/target/common_hardware.c b/src/main/target/common_hardware.c
index b4fadfca59..aac63ddd76 100755
--- a/src/main/target/common_hardware.c
+++ b/src/main/target/common_hardware.c
@@ -18,6 +18,7 @@
 #include <platform.h>
 #include "drivers/io.h"
 #include "drivers/bus.h"
+#include "drivers/sensor.h"
 
 #if !defined(USE_TARGET_HARDWARE_DESCRIPTORS)
 
@@ -32,46 +33,51 @@
     #endif
 
     #if defined(USE_GYRO_L3GD20)
-        BUSDEV_REGISTER_SPI(busdev_l3gd20,      DEVHW_L3GD20,       L3GD20_SPI_BUS,     L3GD20_CS_PIN,      NONE,           DEVFLAGS_NONE);
+        #if defined(GYRO_L3GD20_ALIGN)
+            #define GYRO_0_ALIGN    GYRO_L3GD20_ALIGN
+        #else
+            #define GYRO_0_ALIGN    ALIGN_DEFAULT
+        #endif
+        BUSDEV_REGISTER_SPI(busdev_l3gd20,      DEVHW_L3GD20,       L3GD20_SPI_BUS,     L3GD20_CS_PIN,      NONE,           DEVFLAGS_NONE,  IMU_L3GD20_ALIGN);
     #endif
 
     #if defined(USE_ACC_LSM303DLHC)
-        BUSDEV_REGISTER_I2C(busdev_lsm303,      DEVHW_LSM303DLHC,   LSM303DLHC_I2C_BUS, 0x19,               NONE,           DEVFLAGS_NONE);
+        BUSDEV_REGISTER_I2C(busdev_lsm303,      DEVHW_LSM303DLHC,   LSM303DLHC_I2C_BUS, 0x19,               NONE,           DEVFLAGS_NONE,  IMU_LSM303DLHC_ALIGN);
     #endif
 
     #if defined(USE_GYRO_MPU6000)
-        BUSDEV_REGISTER_SPI(busdev_mpu6000,     DEVHW_MPU6000,      MPU6000_SPI_BUS,    MPU6000_CS_PIN,     GYRO_INT_EXTI,  DEVFLAGS_NONE);
+        BUSDEV_REGISTER_SPI(busdev_mpu6000,     DEVHW_MPU6000,      MPU6000_SPI_BUS,    MPU6000_CS_PIN,     GYRO_INT_EXTI,  DEVFLAGS_NONE,  IMU_MPU6000_ALIGN);
     #endif
 
     #if defined(USE_GYRO_MPU6050)
-        BUSDEV_REGISTER_I2C(busdev_mpu6050,     DEVHW_MPU6050,      MPU6050_I2C_BUS,    MPU_ADDRESS,        GYRO_INT_EXTI,  DEVFLAGS_NONE);
+        BUSDEV_REGISTER_I2C(busdev_mpu6050,     DEVHW_MPU6050,      MPU6050_I2C_BUS,    MPU_ADDRESS,        GYRO_INT_EXTI,  DEVFLAGS_NONE,  IMU_MPU6050_ALIGN);
     #endif
 
     #if defined(USE_GYRO_MPU6500)
         #if defined(MPU6500_SPI_BUS)
-        BUSDEV_REGISTER_SPI(busdev_mpu6500,     DEVHW_MPU6500,      MPU6500_SPI_BUS,    MPU6500_CS_PIN,     GYRO_INT_EXTI,  DEVFLAGS_NONE);
+        BUSDEV_REGISTER_SPI(busdev_mpu6500,     DEVHW_MPU6500,      MPU6500_SPI_BUS,    MPU6500_CS_PIN,     GYRO_INT_EXTI,  DEVFLAGS_NONE,  IMU_MPU6500_ALIGN);
         #elif defined(MPU6500_I2C_BUS)
-        BUSDEV_REGISTER_I2C(busdev_mpu6500,     DEVHW_MPU6500,      MPU6500_I2C_BUS,    MPU_ADDRESS,        GYRO_INT_EXTI,  DEVFLAGS_NONE);
+        BUSDEV_REGISTER_I2C(busdev_mpu6500,     DEVHW_MPU6500,      MPU6500_I2C_BUS,    MPU_ADDRESS,        GYRO_INT_EXTI,  DEVFLAGS_NONE,  IMU_MPU6500_ALIGN);
         #endif
     #endif
 
     #if defined(USE_GYRO_MPU9250)
         #if defined(MPU9250_SPI_BUS)
-        BUSDEV_REGISTER_SPI(busdev_mpu9250,     DEVHW_MPU9250,      MPU9250_SPI_BUS,    MPU9250_CS_PIN,     GYRO_INT_EXTI,  DEVFLAGS_NONE);
+        BUSDEV_REGISTER_SPI(busdev_mpu9250,     DEVHW_MPU9250,      MPU9250_SPI_BUS,    MPU9250_CS_PIN,     GYRO_INT_EXTI,  DEVFLAGS_NONE,  IMU_MPU9250_ALIGN);
         #elif defined(MPU9250_I2C_BUS)
-        BUSDEV_REGISTER_I2C(busdev_mpu9250,     DEVHW_MPU9250,      MPU9250_I2C_BUS,    MPU_ADDRESS,        GYRO_INT_EXTI,  DEVFLAGS_NONE);
+        BUSDEV_REGISTER_I2C(busdev_mpu9250,     DEVHW_MPU9250,      MPU9250_I2C_BUS,    MPU_ADDRESS,        GYRO_INT_EXTI,  DEVFLAGS_NONE,  IMU_MPU9250_ALIGN);
         #endif
     #endif
 
     #if defined(USE_GYRO_ICM20689)
-        BUSDEV_REGISTER_SPI(busdev_icm20689,    DEVHW_ICM20689,     ICM20689_SPI_BUS,   ICM20689_CS_PIN,    GYRO_INT_EXTI,  DEVFLAGS_NONE);
+        BUSDEV_REGISTER_SPI(busdev_icm20689,    DEVHW_ICM20689,     ICM20689_SPI_BUS,   ICM20689_CS_PIN,    GYRO_INT_EXTI,  DEVFLAGS_NONE,  IMU_ICM20689_ALIGN);
     #endif
 
     #if defined(USE_GYRO_BMI160)
         #if defined(BMI160_SPI_BUS)
-        BUSDEV_REGISTER_SPI(busdev_bmi160,      DEVHW_BMI160,       BMI160_SPI_BUS,     BMI160_CS_PIN,      GYRO_INT_EXTI,  DEVFLAGS_NONE);
+        BUSDEV_REGISTER_SPI(busdev_bmi160,      DEVHW_BMI160,       BMI160_SPI_BUS,     BMI160_CS_PIN,      GYRO_INT_EXTI,  DEVFLAGS_NONE,  IMU_BMI160_ALIGN);
         #elif defined(BMI160_I2C_BUS)
-        BUSDEV_REGISTER_I2C(busdev_bmi160,      DEVHW_BMI160,       BMI160_I2C_BUS,     0x68,               GYRO_INT_EXTI,  DEVFLAGS_NONE);
+        BUSDEV_REGISTER_I2C(busdev_bmi160,      DEVHW_BMI160,       BMI160_I2C_BUS,     0x68,               GYRO_INT_EXTI,  DEVFLAGS_NONE,  IMU_BMI160_ALIGN);
         #endif
     #endif
 #endif
@@ -83,45 +89,45 @@
     #if !defined(BMP085_I2C_BUS)
         #define BMP085_I2C_BUS BARO_I2C_BUS
     #endif
-    BUSDEV_REGISTER_I2C(busdev_bmp085,      DEVHW_BMP085,       BMP085_I2C_BUS,     0x77,               NONE,           DEVFLAGS_NONE);
+    BUSDEV_REGISTER_I2C(busdev_bmp085,      DEVHW_BMP085,       BMP085_I2C_BUS,     0x77,               NONE,           DEVFLAGS_NONE,      0);
 #endif
 
 #if defined(USE_BARO_BMP280)
     #if defined(BMP280_SPI_BUS)
-    BUSDEV_REGISTER_SPI(busdev_bmp280,      DEVHW_BMP280,       BMP280_SPI_BUS,     BMP280_CS_PIN,      NONE,           DEVFLAGS_NONE);
+    BUSDEV_REGISTER_SPI(busdev_bmp280,      DEVHW_BMP280,       BMP280_SPI_BUS,     BMP280_CS_PIN,      NONE,           DEVFLAGS_NONE,      0);
     #elif defined(BMP280_I2C_BUS) || defined(BARO_I2C_BUS)
     #if !defined(BMP280_I2C_BUS)
         #define BMP280_I2C_BUS BARO_I2C_BUS
     #endif
-    BUSDEV_REGISTER_I2C(busdev_bmp280,      DEVHW_BMP280,       BMP280_I2C_BUS,     0x76,               NONE,           DEVFLAGS_NONE);
+    BUSDEV_REGISTER_I2C(busdev_bmp280,      DEVHW_BMP280,       BMP280_I2C_BUS,     0x76,               NONE,           DEVFLAGS_NONE,      0);
     #endif
 #endif
 
 #if defined(USE_BARO_BMP388)
     #if defined(BMP388_SPI_BUS)
-    BUSDEV_REGISTER_SPI(busdev_bmp388,      DEVHW_BMP388,       BMP388_SPI_BUS,     BMP388_CS_PIN,      NONE,           DEVFLAGS_NONE);
+    BUSDEV_REGISTER_SPI(busdev_bmp388,      DEVHW_BMP388,       BMP388_SPI_BUS,     BMP388_CS_PIN,      NONE,           DEVFLAGS_NONE,      0);
     #elif defined(BMP388_I2C_BUS) || defined(BARO_I2C_BUS)
     #if !defined(BMP388_I2C_BUS)
         #define BMP388_I2C_BUS BARO_I2C_BUS
     #endif
-    BUSDEV_REGISTER_I2C(busdev_bmp388,      DEVHW_BMP388,       BMP388_I2C_BUS,     0x76,               NONE,           DEVFLAGS_NONE);
+    BUSDEV_REGISTER_I2C(busdev_bmp388,      DEVHW_BMP388,       BMP388_I2C_BUS,     0x76,               NONE,           DEVFLAGS_NONE,      0);
     #endif
 #endif
 
 #if defined(USE_BARO_SPL06)
     #if defined(SPL06_SPI_BUS)
-      BUSDEV_REGISTER_SPI(busdev_spl06,     DEVHW_SPL06,        SPL06_SPI_BUS,      SPL06_CS_PIN,       NONE,           DEVFLAGS_NONE);
+      BUSDEV_REGISTER_SPI(busdev_spl06,     DEVHW_SPL06,        SPL06_SPI_BUS,      SPL06_CS_PIN,       NONE,           DEVFLAGS_NONE,      0);
     #elif defined(SPL06_I2C_BUS) || defined(BARO_I2C_BUS)
       #if !defined(SPL06_I2C_BUS)
         #define SPL06_I2C_BUS BARO_I2C_BUS
       #endif
-      BUSDEV_REGISTER_I2C(busdev_spl06,     DEVHW_SPL06,        SPL06_I2C_BUS,      0x76,               NONE,           DEVFLAGS_NONE);
+      BUSDEV_REGISTER_I2C(busdev_spl06,     DEVHW_SPL06,        SPL06_I2C_BUS,      0x76,               NONE,           DEVFLAGS_NONE,      0);
     #endif
 #endif
 
 #if defined(USE_BARO_LPS25H)
     #if defined(LPS25H_SPI_BUS)
-    BUSDEV_REGISTER_SPI(busdev_lps25h,      DEVHW_LPS25H,       LPS25H_SPI_BUS,     LPS25H_CS_PIN,      NONE,           DEVFLAGS_NONE);
+    BUSDEV_REGISTER_SPI(busdev_lps25h,      DEVHW_LPS25H,       LPS25H_SPI_BUS,     LPS25H_CS_PIN,      NONE,           DEVFLAGS_NONE,      0);
     #endif
 #endif
 
@@ -129,17 +135,17 @@
     #if !defined(MS5607_I2C_BUS)
         #define MS5607_I2C_BUS BARO_I2C_BUS
     #endif
-    BUSDEV_REGISTER_I2C(busdev_ms5607,      DEVHW_MS5607,       MS5607_I2C_BUS,     0x77,               NONE,           DEVFLAGS_USE_RAW_REGISTERS);
+    BUSDEV_REGISTER_I2C(busdev_ms5607,      DEVHW_MS5607,       MS5607_I2C_BUS,     0x77,               NONE,           DEVFLAGS_USE_RAW_REGISTERS, 0);
 #endif
 
 #if defined(USE_BARO_MS5611)
     #if defined(MS5611_SPI_BUS)
-    BUSDEV_REGISTER_SPI(busdev_ms5611,      DEVHW_MS5611,       MS5611_SPI_BUS,     MS5611_CS_PIN,      NONE,           DEVFLAGS_USE_RAW_REGISTERS);
+    BUSDEV_REGISTER_SPI(busdev_ms5611,      DEVHW_MS5611,       MS5611_SPI_BUS,     MS5611_CS_PIN,      NONE,           DEVFLAGS_USE_RAW_REGISTERS, 0);
     #elif defined(MS5611_I2C_BUS) || defined(BARO_I2C_BUS)
     #if !defined(MS5611_I2C_BUS)
         #define MS5611_I2C_BUS BARO_I2C_BUS
     #endif
-    BUSDEV_REGISTER_I2C(busdev_ms5611,      DEVHW_MS5611,       MS5611_I2C_BUS,     0x77,               NONE,           DEVFLAGS_USE_RAW_REGISTERS);
+    BUSDEV_REGISTER_I2C(busdev_ms5611,      DEVHW_MS5611,       MS5611_I2C_BUS,     0x77,               NONE,           DEVFLAGS_USE_RAW_REGISTERS, 0);
     #endif
 #endif
 
@@ -149,35 +155,35 @@
     #if !defined(HMC5883_I2C_BUS)
         #define HMC5883_I2C_BUS MAG_I2C_BUS
     #endif
-    BUSDEV_REGISTER_I2C(busdev_hmc5883,     DEVHW_HMC5883,      HMC5883_I2C_BUS,    0x1E,               NONE,           DEVFLAGS_NONE);
+    BUSDEV_REGISTER_I2C(busdev_hmc5883,     DEVHW_HMC5883,      HMC5883_I2C_BUS,    0x1E,               NONE,           DEVFLAGS_NONE,  0);
 #endif
 
 #if defined(USE_MAG_QMC5883)
     #if !defined(QMC5883_I2C_BUS)
         #define QMC5883_I2C_BUS MAG_I2C_BUS
     #endif
-    BUSDEV_REGISTER_I2C(busdev_qmc5883,     DEVHW_QMC5883,      QMC5883_I2C_BUS,    0x0D,               NONE,           DEVFLAGS_NONE);
+    BUSDEV_REGISTER_I2C(busdev_qmc5883,     DEVHW_QMC5883,      QMC5883_I2C_BUS,    0x0D,               NONE,           DEVFLAGS_NONE,  0);
 #endif
 
 #if defined(USE_MAG_AK8963)
     #if defined(AK8963_SPI_BUS)
-    BUSDEV_REGISTER_SPI(busdev_ak8963,      DEVHW_AK8963,       AK8963_SPI_BUS,     AK8963_CS_PIN,      NONE,           DEVFLAGS_NONE);
+    BUSDEV_REGISTER_SPI(busdev_ak8963,      DEVHW_AK8963,       AK8963_SPI_BUS,     AK8963_CS_PIN,      NONE,           DEVFLAGS_NONE,  0);
     #elif defined(AK8963_I2C_BUS) || defined(MAG_I2C_BUS)
     #if !defined(AK8963_I2C_BUS)
         #define AK8963_I2C_BUS MAG_I2C_BUS
     #endif
-    BUSDEV_REGISTER_I2C(busdev_ak8963,      DEVHW_AK8963,       AK8963_I2C_BUS,     0x0C,               NONE,           DEVFLAGS_NONE);
+    BUSDEV_REGISTER_I2C(busdev_ak8963,      DEVHW_AK8963,       AK8963_I2C_BUS,     0x0C,               NONE,           DEVFLAGS_NONE,  0);
     #endif
 #endif
 
 #if defined(USE_MAG_AK8975)
     #if defined(AK8975_SPI_BUS)
-    BUSDEV_REGISTER_SPI(busdev_ak8975,      DEVHW_AK8975,       AK8975_SPI_BUS,     AK8975_CS_PIN,      NONE,           DEVFLAGS_NONE);
+    BUSDEV_REGISTER_SPI(busdev_ak8975,      DEVHW_AK8975,       AK8975_SPI_BUS,     AK8975_CS_PIN,      NONE,           DEVFLAGS_NONE,  0);
     #elif defined(AK8975_I2C_BUS) || defined(MAG_I2C_BUS)
     #if !defined(AK8975_I2C_BUS)
         #define AK8975_I2C_BUS MAG_I2C_BUS
     #endif
-    BUSDEV_REGISTER_I2C(busdev_ak8975,      DEVHW_AK8975,       AK8975_I2C_BUS,     0x0C,               NONE,           DEVFLAGS_NONE);
+    BUSDEV_REGISTER_I2C(busdev_ak8975,      DEVHW_AK8975,       AK8975_I2C_BUS,     0x0C,               NONE,           DEVFLAGS_NONE,  0);
     #endif
 #endif
 
@@ -185,14 +191,14 @@
     #if !defined(MAG3110_I2C_BUS)
         #define MAG3110_I2C_BUS MAG_I2C_BUS
     #endif
-    BUSDEV_REGISTER_I2C(busdev_mag3110,     DEVHW_MAG3110,      MAG3110_I2C_BUS,    0x0E,               NONE,           DEVFLAGS_NONE);
+    BUSDEV_REGISTER_I2C(busdev_mag3110,     DEVHW_MAG3110,      MAG3110_I2C_BUS,    0x0E,               NONE,           DEVFLAGS_NONE,  0);
 #endif
 
 #if defined(USE_MAG_LIS3MDL)
     #if !defined(LIS3MDL_I2C_BUS)
         #define LIS3MDL_I2C_BUS MAG_I2C_BUS
     #endif
-    BUSDEV_REGISTER_I2C(busdev_lis3mdl,     DEVHW_LIS3MDL,      LIS3MDL_I2C_BUS,    0x1E,               NONE,           DEVFLAGS_NONE);
+    BUSDEV_REGISTER_I2C(busdev_lis3mdl,     DEVHW_LIS3MDL,      LIS3MDL_I2C_BUS,    0x1E,               NONE,           DEVFLAGS_NONE,  0);
 //  ST LIS3MDL address can be changed by connecting it's SDO/SA1 pin to either supply or ground.
 //  BUSDEV_REGISTER_I2C(busdev_lis3mdl,     DEVHW_LIS3MDL,      LIS3MDL_I2C_BUS,    0x1C,               NONE,           DEVFLAGS_NONE);
 #endif
@@ -201,15 +207,15 @@
     #if !defined(IST8310_I2C_BUS)
         #define IST8310_I2C_BUS MAG_I2C_BUS
     #endif
-    BUSDEV_REGISTER_I2C(busdev_ist8310_0,   DEVHW_IST8310_0,    IST8310_I2C_BUS,    0x0C,               NONE,           DEVFLAGS_NONE);
-    BUSDEV_REGISTER_I2C(busdev_ist8310_1,   DEVHW_IST8310_1,    IST8310_I2C_BUS,    0x0E,               NONE,           DEVFLAGS_NONE);
+    BUSDEV_REGISTER_I2C(busdev_ist8310_0,   DEVHW_IST8310_0,    IST8310_I2C_BUS,    0x0C,               NONE,           DEVFLAGS_NONE,  0);
+    BUSDEV_REGISTER_I2C(busdev_ist8310_1,   DEVHW_IST8310_1,    IST8310_I2C_BUS,    0x0E,               NONE,           DEVFLAGS_NONE,  0);
 #endif
 
 #if defined(USE_MAG_IST8308)
     #if !defined(IST8308_I2C_BUS)
         #define IST8308_I2C_BUS MAG_I2C_BUS
     #endif
-    BUSDEV_REGISTER_I2C(busdev_ist8308,     DEVHW_IST8308,      IST8308_I2C_BUS,    0x0C,               NONE,           DEVFLAGS_NONE);
+    BUSDEV_REGISTER_I2C(busdev_ist8308,     DEVHW_IST8308,      IST8308_I2C_BUS,    0x0C,               NONE,           DEVFLAGS_NONE,  0);
 #endif
 #endif
 
@@ -223,7 +229,7 @@
 #endif
 
 #if defined(USE_1WIRE_DS2482) && defined(DS2482_I2C_BUS)
-    BUSDEV_REGISTER_I2C(busdev_ds2482,      DEVHW_DS2482,       DS2482_I2C_BUS,     0x18,               NONE,           DEVFLAGS_USE_RAW_REGISTERS);
+    BUSDEV_REGISTER_I2C(busdev_ds2482,      DEVHW_DS2482,       DS2482_I2C_BUS,     0x18,               NONE,           DEVFLAGS_USE_RAW_REGISTERS,  0);
 #endif
 
 #endif
@@ -236,14 +242,14 @@
 #endif
 
 #if defined(USE_TEMPERATURE_LM75) && defined(LM75_I2C_BUS)
-    BUSDEV_REGISTER_I2C(busdev_lm75_0,      DEVHW_LM75_0,         LM75_I2C_BUS,     0x48,               NONE,           DEVFLAGS_NONE);
-    BUSDEV_REGISTER_I2C(busdev_lm75_1,      DEVHW_LM75_1,         LM75_I2C_BUS,     0x49,               NONE,           DEVFLAGS_NONE);
-    BUSDEV_REGISTER_I2C(busdev_lm75_2,      DEVHW_LM75_2,         LM75_I2C_BUS,     0x4A,               NONE,           DEVFLAGS_NONE);
-    BUSDEV_REGISTER_I2C(busdev_lm75_3,      DEVHW_LM75_3,         LM75_I2C_BUS,     0x4B,               NONE,           DEVFLAGS_NONE);
-    BUSDEV_REGISTER_I2C(busdev_lm75_4,      DEVHW_LM75_4,         LM75_I2C_BUS,     0x4C,               NONE,           DEVFLAGS_NONE);
-    BUSDEV_REGISTER_I2C(busdev_lm75_5,      DEVHW_LM75_5,         LM75_I2C_BUS,     0x4D,               NONE,           DEVFLAGS_NONE);
-    BUSDEV_REGISTER_I2C(busdev_lm75_6,      DEVHW_LM75_6,         LM75_I2C_BUS,     0x4E,               NONE,           DEVFLAGS_NONE);
-    BUSDEV_REGISTER_I2C(busdev_lm75_7,      DEVHW_LM75_7,         LM75_I2C_BUS,     0x4F,               NONE,           DEVFLAGS_NONE);
+    BUSDEV_REGISTER_I2C(busdev_lm75_0,      DEVHW_LM75_0,         LM75_I2C_BUS,     0x48,               NONE,           DEVFLAGS_NONE,  0);
+    BUSDEV_REGISTER_I2C(busdev_lm75_1,      DEVHW_LM75_1,         LM75_I2C_BUS,     0x49,               NONE,           DEVFLAGS_NONE,  0);
+    BUSDEV_REGISTER_I2C(busdev_lm75_2,      DEVHW_LM75_2,         LM75_I2C_BUS,     0x4A,               NONE,           DEVFLAGS_NONE,  0);
+    BUSDEV_REGISTER_I2C(busdev_lm75_3,      DEVHW_LM75_3,         LM75_I2C_BUS,     0x4B,               NONE,           DEVFLAGS_NONE,  0);
+    BUSDEV_REGISTER_I2C(busdev_lm75_4,      DEVHW_LM75_4,         LM75_I2C_BUS,     0x4C,               NONE,           DEVFLAGS_NONE,  0);
+    BUSDEV_REGISTER_I2C(busdev_lm75_5,      DEVHW_LM75_5,         LM75_I2C_BUS,     0x4D,               NONE,           DEVFLAGS_NONE,  0);
+    BUSDEV_REGISTER_I2C(busdev_lm75_6,      DEVHW_LM75_6,         LM75_I2C_BUS,     0x4E,               NONE,           DEVFLAGS_NONE,  0);
+    BUSDEV_REGISTER_I2C(busdev_lm75_7,      DEVHW_LM75_7,         LM75_I2C_BUS,     0x4F,               NONE,           DEVFLAGS_NONE,  0);
 #endif
 
 
@@ -254,7 +260,7 @@
         #define SRF10_I2C_BUS RANGEFINDER_I2C_BUS
     #endif
     #if defined(SRF10_I2C_BUS)
-    BUSDEV_REGISTER_I2C(busdev_srf10,       DEVHW_SRF10,        SRF10_I2C_BUS,      0x70,               NONE,           DEVFLAGS_NONE);
+    BUSDEV_REGISTER_I2C(busdev_srf10,       DEVHW_SRF10,        SRF10_I2C_BUS,      0x70,               NONE,           DEVFLAGS_NONE,  0);
     #endif
 #endif
 
@@ -263,7 +269,7 @@
         #define HCSR04_I2C_BUS RANGEFINDER_I2C_BUS
     #endif
     #if defined(HCSR04_I2C_BUS)
-    BUSDEV_REGISTER_I2C(busdev_hcsr04,      DEVHW_HCSR04_I2C,   HCSR04_I2C_BUS,     0x14,               NONE,           DEVFLAGS_NONE);
+    BUSDEV_REGISTER_I2C(busdev_hcsr04,      DEVHW_HCSR04_I2C,   HCSR04_I2C_BUS,     0x14,               NONE,           DEVFLAGS_NONE,  0);
     #endif
 #endif
 
@@ -273,7 +279,7 @@
     #endif
 
     #if defined(VL53L0X_I2C_BUS)
-    BUSDEV_REGISTER_I2C(busdev_vl53l0x,     DEVHW_VL53L0X,      VL53L0X_I2C_BUS,    0x29,               NONE,           DEVFLAGS_NONE);
+    BUSDEV_REGISTER_I2C(busdev_vl53l0x,     DEVHW_VL53L0X,      VL53L0X_I2C_BUS,    0x29,               NONE,           DEVFLAGS_NONE,  0);
     #endif
 #endif
 
@@ -285,22 +291,22 @@
 #endif
 
 #if defined(USE_PITOT_MS4525) && defined(MS4525_I2C_BUS)
-    BUSDEV_REGISTER_I2C(busdev_ms5425,      DEVHW_MS4525,       MS4525_I2C_BUS,     0x28,               NONE,           DEVFLAGS_USE_RAW_REGISTERS);    // Requires 0xFF to passthrough
+    BUSDEV_REGISTER_I2C(busdev_ms5425,      DEVHW_MS4525,       MS4525_I2C_BUS,     0x28,               NONE,           DEVFLAGS_USE_RAW_REGISTERS,  0);    // Requires 0xFF to passthrough
 #endif
 
 
 /** OTHER HARDWARE **/
 
 #if defined(USE_MAX7456)
-    BUSDEV_REGISTER_SPI(busdev_max7456,     DEVHW_MAX7456,      MAX7456_SPI_BUS,    MAX7456_CS_PIN,     NONE,           DEVFLAGS_USE_RAW_REGISTERS);
+    BUSDEV_REGISTER_SPI(busdev_max7456,     DEVHW_MAX7456,      MAX7456_SPI_BUS,    MAX7456_CS_PIN,     NONE,           DEVFLAGS_USE_RAW_REGISTERS,  0);
 #endif
 
 #if defined(USE_FLASH_M25P16)
-    BUSDEV_REGISTER_SPI(busdev_m25p16,      DEVHW_M25P16,       M25P16_SPI_BUS,     M25P16_CS_PIN,      NONE,           DEVFLAGS_NONE);
+    BUSDEV_REGISTER_SPI(busdev_m25p16,      DEVHW_M25P16,       M25P16_SPI_BUS,     M25P16_CS_PIN,      NONE,           DEVFLAGS_NONE,  0);
 #endif
 
 #if defined(USE_SDCARD) && defined(USE_SDCARD_SPI)
-    BUSDEV_REGISTER_SPI(busdev_sdcard_spi,  DEVHW_SDCARD,       SDCARD_SPI_BUS,     SDCARD_CS_PIN,      NONE,           DEVFLAGS_USE_MANUAL_DEVICE_SELECT | DEVFLAGS_SPI_MODE_0);
+    BUSDEV_REGISTER_SPI(busdev_sdcard_spi,  DEVHW_SDCARD,       SDCARD_SPI_BUS,     SDCARD_CS_PIN,      NONE,           DEVFLAGS_USE_MANUAL_DEVICE_SELECT | DEVFLAGS_SPI_MODE_0,  0);
 #endif
 
 /*
@@ -314,7 +320,7 @@
     #if !defined(UG2864_I2C_BUS)
         #define UG2864_I2C_BUS BUS_I2C1
     #endif
-    BUSDEV_REGISTER_I2C(busdev_ug2864,      DEVHW_UG2864,       UG2864_I2C_BUS,     0x3C,               NONE,           DEVFLAGS_NONE);
+    BUSDEV_REGISTER_I2C(busdev_ug2864,      DEVHW_UG2864,       UG2864_I2C_BUS,     0x3C,               NONE,           DEVFLAGS_NONE,  0);
 #endif
 
 #if defined(USE_PWM_SERVO_DRIVER)
@@ -322,7 +328,7 @@
         #if !defined(PCA9685_I2C_BUS)
             #define PCA9685_I2C_BUS BUS_I2C1
         #endif
-        BUSDEV_REGISTER_I2C(busdev_pca9685,      DEVHW_PCA9685,       PCA9685_I2C_BUS,     0x40,               NONE,           DEVFLAGS_NONE);
+        BUSDEV_REGISTER_I2C(busdev_pca9685,      DEVHW_PCA9685,       PCA9685_I2C_BUS,     0x40,               NONE,           DEVFLAGS_NONE,  0);
     #endif
 #endif
 
diff --git a/src/main/target/sanity_check.h b/src/main/target/sanity_check.h
new file mode 100644
index 0000000000..57989b4001
--- /dev/null
+++ b/src/main/target/sanity_check.h
@@ -0,0 +1,71 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute 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.
+ *
+ * This file 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 this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#pragma once
+
+// Make sure IMU alignments are migrated to IMU_xxx_ALIGN
+#if defined (GYRO_MPU6050_ALIGN) || defined (ACC_MPU6050_ALIGN)
+#error "Replace GYRO_MPU6050_ALIGN and ACC_MPU6050_ALIGN with IMU_MPU6050_ALIGN"
+#endif
+
+#if defined (GYRO_MPU6000_ALIGN) || defined (ACC_MPU6000_ALIGN)
+#error "Replace GYRO_MPU6000_ALIGN and ACC_MPU6000_ALIGN with IMU_MPU6000_ALIGN"
+#endif
+
+#if defined (GYRO_MPU6500_ALIGN) || defined (ACC_MPU6500_ALIGN)
+#error "Replace GYRO_MPU6500_ALIGN and ACC_MPU6500_ALIGN with IMU_MPU6500_ALIGN"
+#endif
+
+#if defined (GYRO_MPU9250_ALIGN) || defined (ACC_MPU9250_ALIGN)
+#error "Replace GYRO_MPU9250_ALIGN and ACC_MPU9250_ALIGN with IMU_MPU9250_ALIGN"
+#endif
+
+#if defined (GYRO_BMI160_ALIGN) || defined (ACC_BMI160_ALIGN)
+#error "Replace GYRO_BMI160_ALIGN and ACC_BMI160_ALIGN with IMU_BMI160_ALIGN"
+#endif
+
+#if defined (GYRO_ICM20689_ALIGN) || defined (ACC_ICM20689_ALIGN)
+#error "Replace GYRO_ICM20689_ALIGN and ACC_ICM20689_ALIGN with IMU_ICM20689_ALIGN"
+#endif
+
+#if defined (GYRO_L3G4200D_ALIGN)
+#error "Replace GYRO_L3G4200D_ALIGN with IMU_L3G4200D_ALIGN"
+#endif
+
+#if defined (GYRO_MPU3050_ALIGN)
+#error "Replace GYRO_MPU3050_ALIGN with IMU_MPU3050_ALIGN"
+#endif
+
+#if defined (GYRO_L3GD20_ALIGN)
+#error "Replace GYRO_L3GD20_ALIGN with IMU_L3GD20_ALIGN"
+#endif
+
+#if defined (ACC_BMA280_ALIGN)
+#error "Replace ACC_BMA280_ALIGN with IMU_BMA280_ALIGN"
+#endif
+
+#if defined (ACC_MMA8452_ALIGN)
+#error "Replace ACC_MMA8452_ALIGN with IMU_MMA8452_ALIGN"
+#endif
+

From 99f6c42d7cba9a82758bc25cec45e3cac51a2415 Mon Sep 17 00:00:00 2001
From: "Konstantin (DigitalEntity) Sharlaimov"
 <konstantin.sharlaimov@gmail.com>
Date: Tue, 5 May 2020 15:03:35 +0200
Subject: [PATCH 139/179] Deprecate and remove USE_ACC and USE_GYRO; Replace
 ACC_xxx_ALIGN/GYRO_xxx_ALIGN with IMU_xxx_ALIGN

---
 src/main/drivers/accgyro/accgyro_adxl345.c    |  2 +-
 src/main/drivers/accgyro/accgyro_bma280.c     |  2 +-
 src/main/drivers/accgyro/accgyro_bmi160.c     |  2 +-
 src/main/drivers/accgyro/accgyro_fake.c       |  2 +-
 src/main/drivers/accgyro/accgyro_icm20689.c   |  2 +-
 src/main/drivers/accgyro/accgyro_icm20689.h   |  4 ---
 src/main/drivers/accgyro/accgyro_l3gd20.c     |  2 +-
 src/main/drivers/accgyro/accgyro_lsm303dlhc.c |  2 +-
 src/main/drivers/accgyro/accgyro_mpu3050.c    |  2 +-
 src/main/drivers/accgyro/accgyro_mpu6000.c    |  2 +-
 src/main/drivers/accgyro/accgyro_mpu6050.c    |  2 +-
 src/main/drivers/accgyro/accgyro_mpu6500.c    |  2 +-
 src/main/drivers/accgyro/accgyro_mpu9250.c    |  2 +-
 src/main/drivers/compass/compass_mpu9250.c    |  2 +-
 src/main/fc/fc_msp.c                          | 19 -----------
 src/main/sensors/acceleration.c               | 32 +++++++-----------
 src/main/sensors/diagnostics.c                |  4 ---
 src/main/sensors/gyro.c                       | 18 +++++-----
 src/main/target/AIKONF4/target.h              | 22 +++++--------
 src/main/target/AIRBOTF4/target.h             | 16 +++------
 src/main/target/AIRBOTF7/target_abf7.h        | 13 +++-----
 src/main/target/AIRBOTF7/target_o7v7.h        | 12 ++-----
 src/main/target/AIRHEROF3/target.h            | 10 ++----
 src/main/target/ALIENFLIGHTF3/target.h        | 22 ++++---------
 src/main/target/ALIENFLIGHTF4/target.h        | 14 +++-----
 src/main/target/ALIENFLIGHTNGF7/target.h      | 14 +++-----
 src/main/target/ANYFC/target.h                |  9 ++---
 src/main/target/ANYFCF7/target.h              |  9 ++---
 src/main/target/ANYFCM7/target.h              |  9 ++---
 src/main/target/ASGARD32F4/target.h           |  9 ++---
 src/main/target/ASGARD32F7/target.h           |  9 ++---
 src/main/target/BEEROTORF4/target.h           |  9 ++---
 src/main/target/BETAFLIGHTF3/target.h         |  9 ++---
 src/main/target/BETAFLIGHTF4/target.h         | 10 ++----
 src/main/target/BLUEJAYF4/hardware_revision.c |  2 +-
 src/main/target/BLUEJAYF4/target.h            |  9 ++---
 src/main/target/CHEBUZZF3/target.h            | 15 +++------
 src/main/target/CLRACINGF4AIR/target.h        | 10 +++---
 src/main/target/COLIBRI/target.h              | 12 ++-----
 src/main/target/COLIBRI_RACE/target.h         | 21 ++++--------
 src/main/target/DALRCF405/target.h            | 15 +++------
 src/main/target/DALRCF722DUAL/target.h        |  9 ++---
 src/main/target/F4BY/target.h                 | 16 +++------
 src/main/target/FALCORE/target.h              |  9 ++---
 src/main/target/FF_F35_LIGHTNING/target.h     |  9 ++---
 src/main/target/FF_FORTINIF4/target.h         |  9 ++---
 src/main/target/FF_PIKOF4/target.h            | 16 +++------
 src/main/target/FIREWORKSV2/target.h          | 18 +++-------
 src/main/target/FISHDRONEF4/target.h          | 14 +++-----
 src/main/target/FOXEERF405/target.h           | 14 +++-----
 src/main/target/FOXEERF722DUAL/target.c       |  5 +--
 src/main/target/FOXEERF722DUAL/target.h       | 16 +++------
 src/main/target/FRSKYF3/target.h              | 10 ++----
 src/main/target/FRSKYF4/target.h              |  9 ++---
 src/main/target/FURYF3/target.h               | 22 ++++---------
 src/main/target/FURYF4OSD/target.h            | 18 +++-------
 src/main/target/IFLIGHTF4_TWING/target.c      |  5 +--
 src/main/target/IFLIGHTF4_TWING/target.h      | 10 ++----
 src/main/target/IFLIGHTF7_TWING/target.c      |  5 +--
 src/main/target/IFLIGHTF7_TWING/target.h      |  9 ++---
 src/main/target/KAKUTEF4/target.h             |  9 ++---
 src/main/target/KAKUTEF7/target.h             | 16 +++------
 src/main/target/KFC32F3_INAV/target.h         |  9 ++---
 src/main/target/KISSFC/target.h               |  9 ++---
 src/main/target/KROOZX/target.h               |  9 ++---
 src/main/target/LUX_RACE/target.h             | 15 +++------
 src/main/target/MAMBAF405US/target.h          | 10 ++----
 src/main/target/MAMBAF722/target.h            | 10 ++----
 src/main/target/MATEKF405/target.h            | 16 +++------
 src/main/target/MATEKF405SE/target.h          |  9 ++---
 src/main/target/MATEKF411/target.h            | 15 +++------
 src/main/target/MATEKF411SE/target.h          |  9 ++---
 src/main/target/MATEKF722/target.h            |  9 ++---
 src/main/target/MATEKF722PX/target.h          |  8 ++---
 src/main/target/MATEKF722SE/target.c          |  5 +--
 src/main/target/MATEKF722SE/target.h          | 14 +++-----
 src/main/target/MATEKF765/target.c            |  5 +--
 src/main/target/MATEKF765/target.h            | 15 +++------
 src/main/target/MOTOLAB/target.h              | 15 +++------
 src/main/target/NOX/target.h                  |  7 ++--
 src/main/target/OMNIBUS/target.h              | 10 ++----
 src/main/target/OMNIBUSF4/target.h            | 25 ++++----------
 src/main/target/OMNIBUSF7/target.c            | 15 +++++----
 src/main/target/OMNIBUSF7/target.h            | 27 +++++----------
 src/main/target/OMNIBUSF7NXT/target.c         |  5 +--
 src/main/target/OMNIBUSF7NXT/target.h         | 14 +++-----
 src/main/target/PIKOBLX/target.h              | 12 ++-----
 src/main/target/PIXRACER/target.c             | 15 +++++----
 src/main/target/PIXRACER/target.h             | 15 +++------
 src/main/target/RADIX/target.h                |  9 ++---
 src/main/target/RCEXPLORERF3/target.h         |  9 ++---
 src/main/target/REVO/target.c                 | 12 +++----
 src/main/target/REVO/target.h                 | 13 ++------
 src/main/target/RMDO/target.h                 |  9 ++---
 src/main/target/SPARKY/target.h               | 10 ++----
 src/main/target/SPARKY2/target.h              |  9 ++---
 src/main/target/SPEEDYBEEF4/target.h          | 11 +++----
 src/main/target/SPRACINGF3/target.h           | 10 ++----
 src/main/target/SPRACINGF3EVO/target.h        | 15 +++------
 src/main/target/SPRACINGF3MINI/target.h       | 15 +++------
 src/main/target/SPRACINGF3NEO/target.h        | 15 +++------
 src/main/target/SPRACINGF4EVO/target.h        | 16 +++------
 src/main/target/SPRACINGF7DUAL/target.c       |  5 +--
 src/main/target/SPRACINGF7DUAL/target.h       | 31 ++++-------------
 src/main/target/YUPIF4/target.h               | 13 ++------
 src/main/target/YUPIF7/target.h               | 12 ++-----
 src/main/target/common_hardware.c             | 16 ++++-----
 src/main/target/sanity_check.h                | 33 +++++++++++++++++++
 108 files changed, 368 insertions(+), 853 deletions(-)

diff --git a/src/main/drivers/accgyro/accgyro_adxl345.c b/src/main/drivers/accgyro/accgyro_adxl345.c
index a2b73c66ba..b867f85b4a 100644
--- a/src/main/drivers/accgyro/accgyro_adxl345.c
+++ b/src/main/drivers/accgyro/accgyro_adxl345.c
@@ -28,7 +28,7 @@
 #include "drivers/accgyro/accgyro.h"
 #include "drivers/accgyro/accgyro_adxl345.h"
 
-#ifdef USE_ACC_ADXL345
+#ifdef USE_IMU_ADXL345
 
 // ADXL345, Alternative address mode 0x53
 #define ADXL345_ADDRESS     0x53
diff --git a/src/main/drivers/accgyro/accgyro_bma280.c b/src/main/drivers/accgyro/accgyro_bma280.c
index 6fcb4008bb..a97b831bd2 100644
--- a/src/main/drivers/accgyro/accgyro_bma280.c
+++ b/src/main/drivers/accgyro/accgyro_bma280.c
@@ -27,7 +27,7 @@
 #include "drivers/accgyro/accgyro.h"
 #include "drivers/accgyro/accgyro_bma280.h"
 
-#ifdef USE_ACC_BMA280
+#ifdef USE_IMU_BMA280
 
 // BMA280, default I2C address mode 0x18
 #define BMA280_WHOAMI       0x00
diff --git a/src/main/drivers/accgyro/accgyro_bmi160.c b/src/main/drivers/accgyro/accgyro_bmi160.c
index 15ed8634e1..60b54f7f8c 100644
--- a/src/main/drivers/accgyro/accgyro_bmi160.c
+++ b/src/main/drivers/accgyro/accgyro_bmi160.c
@@ -42,7 +42,7 @@
 #include "drivers/accgyro/accgyro.h"
 #include "drivers/accgyro/accgyro_bmi160.h"
 
-#if defined(USE_GYRO_BMI160) || defined(USE_ACC_BMI160)
+#if defined(USE_IMU_BMI160)
 
 /* BMI160 Registers */
 #define BMI160_REG_CHIPID           0x00
diff --git a/src/main/drivers/accgyro/accgyro_fake.c b/src/main/drivers/accgyro/accgyro_fake.c
index 19d4f2498b..0cd9addfae 100644
--- a/src/main/drivers/accgyro/accgyro_fake.c
+++ b/src/main/drivers/accgyro/accgyro_fake.c
@@ -27,7 +27,7 @@
 #include "drivers/accgyro/accgyro_fake.h"
 
 
-#ifdef USE_FAKE_GYRO
+#ifdef USE_IMU_FAKE
 
 static int16_t fakeGyroADC[XYZ_AXIS_COUNT];
 
diff --git a/src/main/drivers/accgyro/accgyro_icm20689.c b/src/main/drivers/accgyro/accgyro_icm20689.c
index de7f33d967..6ed891dac3 100644
--- a/src/main/drivers/accgyro/accgyro_icm20689.c
+++ b/src/main/drivers/accgyro/accgyro_icm20689.c
@@ -38,7 +38,7 @@
 #include "drivers/accgyro/accgyro_mpu.h"
 #include "drivers/accgyro/accgyro_icm20689.h"
 
-#if (defined(USE_GYRO_ICM20689) || defined(USE_ACC_ICM20689))
+#if defined(USE_IMU_ICM20689)
 
 static uint8_t icm20689DeviceDetect(const busDevice_t *busDev)
 {
diff --git a/src/main/drivers/accgyro/accgyro_icm20689.h b/src/main/drivers/accgyro/accgyro_icm20689.h
index bf37b1d4ad..44911daebd 100644
--- a/src/main/drivers/accgyro/accgyro_icm20689.h
+++ b/src/main/drivers/accgyro/accgyro_icm20689.h
@@ -24,9 +24,5 @@
 
 #define ICM20689_BIT_RESET                  (0x80)
 
-#if (defined(USE_GYRO_ICM20689) || defined(USE_ACC_ICM20689))
-
 bool icm20689AccDetect(accDev_t *acc);
 bool icm20689GyroDetect(gyroDev_t *gyro);
-
-#endif
diff --git a/src/main/drivers/accgyro/accgyro_l3gd20.c b/src/main/drivers/accgyro/accgyro_l3gd20.c
index 2705d3dc3b..9fc09556cf 100644
--- a/src/main/drivers/accgyro/accgyro_l3gd20.c
+++ b/src/main/drivers/accgyro/accgyro_l3gd20.c
@@ -32,7 +32,7 @@
 #include "drivers/accgyro/accgyro.h"
 #include "drivers/accgyro/accgyro_l3gd20.h"
 
-#ifdef USE_GYRO_L3GD20
+#ifdef USE_IMU_L3GD20
 
 #define READ_CMD               ((uint8_t)0x80)
 #define MULTIPLEBYTE_CMD       ((uint8_t)0x40)
diff --git a/src/main/drivers/accgyro/accgyro_lsm303dlhc.c b/src/main/drivers/accgyro/accgyro_lsm303dlhc.c
index 596453f6ef..5421d8e337 100644
--- a/src/main/drivers/accgyro/accgyro_lsm303dlhc.c
+++ b/src/main/drivers/accgyro/accgyro_lsm303dlhc.c
@@ -20,7 +20,7 @@
 
 #include "platform.h"
 
-#ifdef USE_ACC_LSM303DLHC
+#ifdef USE_IMU_LSM303DLHC
 
 #include "build/debug.h"
 
diff --git a/src/main/drivers/accgyro/accgyro_mpu3050.c b/src/main/drivers/accgyro/accgyro_mpu3050.c
index 4cebe2c79f..082aa795f5 100644
--- a/src/main/drivers/accgyro/accgyro_mpu3050.c
+++ b/src/main/drivers/accgyro/accgyro_mpu3050.c
@@ -33,7 +33,7 @@
 #include "drivers/accgyro/accgyro_mpu.h"
 #include "drivers/accgyro/accgyro_mpu3050.h"
 
-#ifdef USE_GYRO_MPU3050
+#ifdef USE_IMU_MPU3050
 
 // MPU3050, Standard address 0x68
 #define MPU3050_ADDRESS         0x68
diff --git a/src/main/drivers/accgyro/accgyro_mpu6000.c b/src/main/drivers/accgyro/accgyro_mpu6000.c
index d447138e58..6f8555c0b3 100644
--- a/src/main/drivers/accgyro/accgyro_mpu6000.c
+++ b/src/main/drivers/accgyro/accgyro_mpu6000.c
@@ -42,7 +42,7 @@
 #include "drivers/accgyro/accgyro_mpu.h"
 #include "drivers/accgyro/accgyro_mpu6000.h"
 
-#if (defined(USE_GYRO_MPU6000) || defined(USE_ACC_MPU6000))
+#if defined(USE_IMU_MPU6000)
 
 // Bits
 #define BIT_H_RESET                 0x80
diff --git a/src/main/drivers/accgyro/accgyro_mpu6050.c b/src/main/drivers/accgyro/accgyro_mpu6050.c
index 0a160d59ef..5c83f977d7 100755
--- a/src/main/drivers/accgyro/accgyro_mpu6050.c
+++ b/src/main/drivers/accgyro/accgyro_mpu6050.c
@@ -42,7 +42,7 @@
 #include "drivers/accgyro/accgyro_mpu.h"
 #include "drivers/accgyro/accgyro_mpu6050.h"
 
-#if defined(USE_GYRO_MPU6050) || defined(USE_ACC_MPU6050)
+#if defined(USE_IMU_MPU6050)
 
 #define BIT_H_RESET                 0x80
 #define MPU_CLK_SEL_PLLGYROZ        0x03
diff --git a/src/main/drivers/accgyro/accgyro_mpu6500.c b/src/main/drivers/accgyro/accgyro_mpu6500.c
index 79e457c649..5d381acbab 100755
--- a/src/main/drivers/accgyro/accgyro_mpu6500.c
+++ b/src/main/drivers/accgyro/accgyro_mpu6500.c
@@ -33,7 +33,7 @@
 #include "drivers/accgyro/accgyro_mpu.h"
 #include "drivers/accgyro/accgyro_mpu6500.h"
 
-#if defined(USE_GYRO_MPU6500) || defined(USE_ACC_MPU6500)
+#if defined(USE_IMU_MPU6500)
 
 #define MPU6500_BIT_RESET                   (0x80)
 #define MPU6500_BIT_INT_ANYRD_2CLEAR        (1 << 4)
diff --git a/src/main/drivers/accgyro/accgyro_mpu9250.c b/src/main/drivers/accgyro/accgyro_mpu9250.c
index fe020b3047..32d7d5e7a2 100755
--- a/src/main/drivers/accgyro/accgyro_mpu9250.c
+++ b/src/main/drivers/accgyro/accgyro_mpu9250.c
@@ -33,7 +33,7 @@
 #include "drivers/accgyro/accgyro_mpu.h"
 #include "drivers/accgyro/accgyro_mpu9250.h"
 
-#if defined(USE_GYRO_MPU9250) || defined(USE_ACC_MPU9250)
+#if defined(USE_IMU_MPU9250)
 
 #define MPU9250_BIT_RESET                   (0x80)
 #define MPU9250_BIT_INT_ANYRD_2CLEAR        (1 << 4)
diff --git a/src/main/drivers/compass/compass_mpu9250.c b/src/main/drivers/compass/compass_mpu9250.c
index 1e7d6aa178..111a899e17 100755
--- a/src/main/drivers/compass/compass_mpu9250.c
+++ b/src/main/drivers/compass/compass_mpu9250.c
@@ -40,7 +40,7 @@
 #include "drivers/compass/compass.h"
 #include "drivers/compass/compass_mpu9250.h"
 
-#if defined(USE_MAG_MPU9250) && defined(USE_GYRO_MPU9250)
+#if defined(USE_MAG_MPU9250) && defined(USE_IMU_MPU9250)
 
 // No separate hardware descriptor needed. Hardware descriptor initialization is handled by GYRO driver
 
diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index 8365dfb0d7..36427cc095 100644
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -1273,7 +1273,6 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
 #endif
 
     case MSP_CALIBRATION_DATA:
-    #ifdef USE_ACC
         sbufWriteU8(dst, accGetCalibrationAxisFlags());
         sbufWriteU16(dst, accelerometerConfig()->accZero.raw[X]);
         sbufWriteU16(dst, accelerometerConfig()->accZero.raw[Y]);
@@ -1281,15 +1280,6 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
         sbufWriteU16(dst, accelerometerConfig()->accGain.raw[X]);
         sbufWriteU16(dst, accelerometerConfig()->accGain.raw[Y]);
         sbufWriteU16(dst, accelerometerConfig()->accGain.raw[Z]);
-    #else
-        sbufWriteU8(dst, 0);
-        sbufWriteU16(dst, 0);
-        sbufWriteU16(dst, 0);
-        sbufWriteU16(dst, 0);
-        sbufWriteU16(dst, 0);
-        sbufWriteU16(dst, 0);
-        sbufWriteU16(dst, 0);
-    #endif
 
     #ifdef USE_MAG
         sbufWriteU16(dst, compassConfig()->magZero.raw[X]);
@@ -2207,21 +2197,12 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
 
     case MSP_SET_CALIBRATION_DATA:
         if (dataSize >= 18) {
-#ifdef USE_ACC
             accelerometerConfigMutable()->accZero.raw[X] = sbufReadU16(src);
             accelerometerConfigMutable()->accZero.raw[Y] = sbufReadU16(src);
             accelerometerConfigMutable()->accZero.raw[Z] = sbufReadU16(src);
             accelerometerConfigMutable()->accGain.raw[X] = sbufReadU16(src);
             accelerometerConfigMutable()->accGain.raw[Y] = sbufReadU16(src);
             accelerometerConfigMutable()->accGain.raw[Z] = sbufReadU16(src);
-#else
-            sbufReadU16(src);
-            sbufReadU16(src);
-            sbufReadU16(src);
-            sbufReadU16(src);
-            sbufReadU16(src);
-            sbufReadU16(src);
-#endif
 
 #ifdef USE_MAG
             compassConfigMutable()->magZero.raw[X] = sbufReadU16(src);
diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c
index 5e7b0b9072..015d55f359 100644
--- a/src/main/sensors/acceleration.c
+++ b/src/main/sensors/acceleration.c
@@ -115,9 +115,6 @@ static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
 {
     accelerationSensor_e accHardware = ACC_NONE;
 
-#ifdef USE_ACC_ADXL345
-#endif
-
     dev->accAlign = ALIGN_DEFAULT;
 
     requestedSensors[SENSOR_INDEX_ACC] = accHardwareToUse;
@@ -125,12 +122,9 @@ static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
     switch (accHardwareToUse) {
     case ACC_AUTODETECT:
         FALLTHROUGH;
-#ifdef USE_ACC_ADXL345
+#ifdef USE_IMU_ADXL345
     case ACC_ADXL345: {
         if (adxl345Detect(dev)) {
-#ifdef ACC_ADXL345_ALIGN
-             = ACC_ADXL345_ALIGN;
-#endif
             accHardware = ACC_ADXL345;
             break;
         }
@@ -142,12 +136,9 @@ static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
     FALLTHROUGH;
 #endif
 
-#ifdef USE_ACC_LSM303DLHC
+#ifdef USE_IMU_LSM303DLHC
     case ACC_LSM303DLHC:
         if (lsm303dlhcAccDetect(dev)) {
-#ifdef ACC_LSM303DLHC_ALIGN
-             = ACC_LSM303DLHC_ALIGN;
-#endif
             accHardware = ACC_LSM303DLHC;
             break;
         }
@@ -158,7 +149,7 @@ static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
         FALLTHROUGH;
 #endif
 
-#ifdef USE_ACC_MPU6050
+#ifdef USE_IMU_MPU6050
     case ACC_MPU6050: // MPU6050
         if (mpu6050AccDetect(dev)) {
             accHardware = ACC_MPU6050;
@@ -171,9 +162,8 @@ static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
         FALLTHROUGH;
 #endif
 
-#ifdef USE_ACC_MMA8452
+#ifdef USE_IMU_MMA8452
     case ACC_MMA8452: // MMA8452
-
         if (mma8452Detect(dev)) {
             accHardware = ACC_MMA8452;
             break;
@@ -185,7 +175,7 @@ static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
         FALLTHROUGH;
 #endif
 
-#ifdef USE_ACC_BMA280
+#ifdef USE_IMU_BMA280
     case ACC_BMA280: // BMA280
         if (bma280Detect(dev)) {
             accHardware = ACC_BMA280;
@@ -198,7 +188,7 @@ static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
         FALLTHROUGH;
 #endif
 
-#ifdef USE_ACC_MPU6000
+#ifdef USE_IMU_MPU6000
     case ACC_MPU6000:
         if (mpu6000AccDetect(dev)) {
             accHardware = ACC_MPU6000;
@@ -211,7 +201,7 @@ static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
         FALLTHROUGH;
 #endif
 
-#if defined(USE_ACC_MPU6500)
+#if defined(USE_IMU_MPU6500)
     case ACC_MPU6500:
         if (mpu6500AccDetect(dev)) {
             accHardware = ACC_MPU6500;
@@ -224,7 +214,7 @@ static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
         FALLTHROUGH;
 #endif
 
-#if defined(USE_ACC_MPU9250)
+#if defined(USE_IMU_MPU9250)
     case ACC_MPU9250:
         if (mpu9250AccDetect(dev)) {
             accHardware = ACC_MPU9250;
@@ -237,7 +227,7 @@ static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
         FALLTHROUGH;
 #endif
 
-#if defined(USE_ACC_BMI160)
+#if defined(USE_IMU_BMI160)
     case ACC_BMI160:
         if (bmi160AccDetect(dev)) {
             accHardware = ACC_BMI160;
@@ -250,7 +240,7 @@ static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
         FALLTHROUGH;
 #endif
 
-#ifdef USE_ACC_ICM20689
+#ifdef USE_IMU_ICM20689
     case ACC_ICM20689:
         if (icm20689AccDetect(dev)) {
             accHardware = ACC_ICM20689;
@@ -264,7 +254,7 @@ static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
 #endif
 
 
-#ifdef USE_FAKE_ACC
+#ifdef USE_IMU_FAKE
     case ACC_FAKE:
         if (fakeAccDetect(dev)) {
             accHardware = ACC_FAKE;
diff --git a/src/main/sensors/diagnostics.c b/src/main/sensors/diagnostics.c
index 480eaa27ab..e00859a196 100644
--- a/src/main/sensors/diagnostics.c
+++ b/src/main/sensors/diagnostics.c
@@ -40,7 +40,6 @@ hardwareSensorStatus_e getHwGyroStatus(void)
 
 hardwareSensorStatus_e getHwAccelerometerStatus(void)
 {
-#if defined(USE_ACC)
     if (detectedSensors[SENSOR_INDEX_ACC] != ACC_NONE) {
         if (accIsHealthy()) {
             return HW_SENSOR_OK;
@@ -59,9 +58,6 @@ hardwareSensorStatus_e getHwAccelerometerStatus(void)
             return HW_SENSOR_NONE;
         }
     }
-#else
-    return HW_SENSOR_NONE;
-#endif
 }
 
 hardwareSensorStatus_e getHwCompassStatus(void)
diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index dd6188bb06..72d3557a8e 100644
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@@ -134,7 +134,7 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHard
     case GYRO_AUTODETECT:
         FALLTHROUGH;
 
-#ifdef USE_GYRO_MPU6050
+#ifdef USE_IMU_MPU6050
     case GYRO_MPU6050:
         if (mpu6050GyroDetect(dev)) {
             gyroHardware = GYRO_MPU6050;
@@ -143,7 +143,7 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHard
         FALLTHROUGH;
 #endif
 
-#ifdef USE_GYRO_L3G4200D
+#ifdef USE_IMU_L3G4200D
     case GYRO_L3G4200D:
         if (l3g4200dDetect(dev)) {
             gyroHardware = GYRO_L3G4200D;
@@ -152,7 +152,7 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHard
         FALLTHROUGH;
 #endif
 
-#ifdef USE_GYRO_MPU3050
+#ifdef USE_IMU_MPU3050
     case GYRO_MPU3050:
         if (mpu3050Detect(dev)) {
             gyroHardware = GYRO_MPU3050;
@@ -161,7 +161,7 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHard
         FALLTHROUGH;
 #endif
 
-#ifdef USE_GYRO_L3GD20
+#ifdef USE_IMU_L3GD20
     case GYRO_L3GD20:
         if (l3gd20Detect(dev)) {
             gyroHardware = GYRO_L3GD20;
@@ -170,7 +170,7 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHard
         FALLTHROUGH;
 #endif
 
-#ifdef USE_GYRO_MPU6000
+#ifdef USE_IMU_MPU6000
     case GYRO_MPU6000:
         if (mpu6000GyroDetect(dev)) {
             gyroHardware = GYRO_MPU6000;
@@ -179,7 +179,7 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHard
         FALLTHROUGH;
 #endif
 
-#if defined(USE_GYRO_MPU6500)
+#if defined(USE_IMU_MPU6500)
     case GYRO_MPU6500:
         if (mpu6500GyroDetect(dev)) {
             gyroHardware = GYRO_MPU6500;
@@ -188,7 +188,7 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHard
         FALLTHROUGH;
 #endif
 
-#ifdef USE_GYRO_MPU9250
+#ifdef USE_IMU_MPU9250
     case GYRO_MPU9250:
         if (mpu9250GyroDetect(dev)) {
             gyroHardware = GYRO_MPU9250;
@@ -197,7 +197,7 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHard
         FALLTHROUGH;
 #endif
 
-#ifdef USE_GYRO_BMI160
+#ifdef USE_IMU_BMI160
     case GYRO_BMI160:
         if (bmi160GyroDetect(dev)) {
             gyroHardware = GYRO_BMI160;
@@ -206,7 +206,7 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHard
         FALLTHROUGH;
 #endif
 
-#ifdef USE_GYRO_ICM20689
+#ifdef USE_IMU_ICM20689
     case GYRO_ICM20689:
         if (icm20689GyroDetect(dev)) {
             gyroHardware = GYRO_ICM20689;
diff --git a/src/main/target/AIKONF4/target.h b/src/main/target/AIKONF4/target.h
index 2ab53fca08..6e77fde6ad 100644
--- a/src/main/target/AIKONF4/target.h
+++ b/src/main/target/AIKONF4/target.h
@@ -31,21 +31,15 @@
 #define GYRO_INT_EXTI            PC4
 #define USE_MPU_DATA_READY_SIGNAL
 
-#define USE_GYRO
-#define USE_GYRO_MPU6000
-#define USE_GYRO_MPU6500
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW0_DEG
+#define MPU6000_CS_PIN          SPI1_NSS_PIN
+#define MPU6000_SPI_BUS         BUS_SPI1
 
-#define MPU6000_CS_PIN           SPI1_NSS_PIN
-#define MPU6000_SPI_BUS          BUS_SPI1
-#define GYRO_MPU6000_ALIGN       CW0_DEG
-
-#define MPU6500_CS_PIN          MPU6000_CS_PIN
-#define MPU6500_SPI_BUS         MPU6000_SPI_BUS
-#define GYRO_MPU6500_ALIGN      CW0_DEG
-
-#define USE_ACC
-#define USE_ACC_MPU6000
-#define USE_ACC_MPU6500
+#define USE_IMU_MPU6500
+#define IMU_MPU6500_ALIGN       CW0_DEG
+#define MPU6500_CS_PIN          SPI1_NSS_PIN
+#define MPU6500_SPI_BUS         BUS_SPI1
 
 #define SERIAL_PORT_COUNT       7   //VCP, UART1, UART2, UART3, UART4, SOFTSERIAL1, SOFTSERIAL2
 
diff --git a/src/main/target/AIRBOTF4/target.h b/src/main/target/AIRBOTF4/target.h
index 85c4fcbd98..edf6973b54 100644
--- a/src/main/target/AIRBOTF4/target.h
+++ b/src/main/target/AIRBOTF4/target.h
@@ -34,24 +34,16 @@
 #define GYRO_INT_EXTI            PC4
 #define USE_MPU_DATA_READY_SIGNAL
 
-#define USE_GYRO
-#define USE_GYRO_MPU6000
-#define GYRO_MPU6000_ALIGN      CW270_DEG
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW270_DEG
 #define MPU6000_CS_PIN          PA4
 #define MPU6000_SPI_BUS         BUS_SPI1
 
-#define USE_GYRO_MPU6500
-#define GYRO_MPU6500_ALIGN      CW270_DEG
+#define USE_IMU_MPU6500
+#define IMU_MPU6500_ALIGN       CW270_DEG
 #define MPU6500_CS_PIN          PA4
 #define MPU6500_SPI_BUS         BUS_SPI1
 
-#define USE_ACC
-#define USE_ACC_MPU6000
-#define ACC_MPU6000_ALIGN       CW270_DEG
-
-#define USE_ACC_MPU6500
-#define ACC_MPU6500_ALIGN       CW270_DEG
-
 #define USE_MAG
 #define MAG_I2C_BUS             BUS_I2C2
 #define MAG_HMC5883_ALIGN       CW90_DEG
diff --git a/src/main/target/AIRBOTF7/target_abf7.h b/src/main/target/AIRBOTF7/target_abf7.h
index bced32c4b0..861d04f493 100644
--- a/src/main/target/AIRBOTF7/target_abf7.h
+++ b/src/main/target/AIRBOTF7/target_abf7.h
@@ -32,23 +32,18 @@
 #define USE_TARGET_IMU_HARDWARE_DESCRIPTORS
 #define USE_DUAL_GYRO
 
-#define USE_ACC
-#define USE_GYRO
-
-#define USE_ACC_MPU6000
-#define USE_ACC_MPU6500
-#define USE_GYRO_MPU6000
-#define USE_GYRO_MPU6500
+#define USE_IMU_MPU6000
+#define USE_IMU_MPU6500
 
 #define GYRO_0_CS_PIN           PD2
 #define GYRO_0_SPI_BUS          BUS_SPI3
 #define GYRO_0_EXTI_PIN         NONE
-#define GYRO_0_ALIGN            CW90_DEG    // This doesn't work yet, requires BUS refactoring
+#define GYRO_0_ALIGN            CW90_DEG
 
 #define GYRO_1_CS_PIN           PC4
 #define GYRO_1_SPI_BUS          BUS_SPI1
 #define GYRO_1_EXTI_PIN         NONE
-#define GYRO_1_ALIGN            CW0_DEG     // This doesn't work yet, requires BUS refactoring
+#define GYRO_1_ALIGN            CW0_DEG
 
 // *************** FLASH **************************
 #define USE_FLASHFS
diff --git a/src/main/target/AIRBOTF7/target_o7v7.h b/src/main/target/AIRBOTF7/target_o7v7.h
index 3a6ab38854..c808ef6cdb 100644
--- a/src/main/target/AIRBOTF7/target_o7v7.h
+++ b/src/main/target/AIRBOTF7/target_o7v7.h
@@ -31,21 +31,13 @@
 // *************** Gyro & ACC **********************
 #define USE_TARGET_IMU_HARDWARE_DESCRIPTORS
 
-#define USE_ACC
-#define USE_GYRO
-
-#define USE_ACC_MPU6000
-#define USE_ACC_MPU6500
-#define USE_GYRO_MPU6000
-#define USE_GYRO_MPU6500
+#define USE_IMU_MPU6000
+#define USE_IMU_MPU6500
 
 #define GYRO_0_CS_PIN           PD2
 #define GYRO_0_SPI_BUS          BUS_SPI3
 #define GYRO_0_EXTI_PIN         NONE
-
 #define GYRO_0_ALIGN            CW0_DEG
-#define GYRO_MPU6500_ALIGN      CW0_DEG
-#define ACC_MPU6500_ALIGN       CW0_DEG
 
 // *************** FLASH **************************
 #define USE_FLASHFS
diff --git a/src/main/target/AIRHEROF3/target.h b/src/main/target/AIRHEROF3/target.h
index 53f1a52acf..9356bf0cb2 100755
--- a/src/main/target/AIRHEROF3/target.h
+++ b/src/main/target/AIRHEROF3/target.h
@@ -34,20 +34,14 @@
 #define USE_SPI
 #define USE_SPI_DEVICE_2
 
+#define USE_IMU_MPU6500
+#define IMU_MPU6500_ALIGN       CW0_DEG
 #define MPU6500_SPI_BUS         BUS_SPI2
 #define MPU6500_CS_PIN          PB12
 
 #define BMP280_SPI_BUS          BUS_SPI2
 #define BMP280_CS_PIN           PB5
 
-#define USE_GYRO
-#define USE_GYRO_MPU6500
-#define GYRO_MPU6500_ALIGN      CW0_DEG
-
-#define USE_ACC
-#define USE_ACC_MPU6500
-#define ACC_MPU6500_ALIGN       CW0_DEG
-
 #define USE_BARO
 #define USE_BARO_BMP280
 
diff --git a/src/main/target/ALIENFLIGHTF3/target.h b/src/main/target/ALIENFLIGHTF3/target.h
index 128364d1eb..ba8de96eea 100644
--- a/src/main/target/ALIENFLIGHTF3/target.h
+++ b/src/main/target/ALIENFLIGHTF3/target.h
@@ -38,28 +38,20 @@
 //#define DEBUG_MPU_DATA_READY_INTERRUPT
 
 // Using MPU6050 for the moment.
-#define USE_GYRO
-
-#define USE_GYRO_MPU6050
+#define USE_IMU_MPU6050
+#define IMU_MPU6050_ALIGN       CW270_DEG
 #define MPU6050_I2C_BUS         BUS_I2C2
-#define GYRO_MPU6050_ALIGN      CW270_DEG
 
-#define USE_GYRO_MPU9250
-#define GYRO_MPU6500_ALIGN      CW270_DEG
+#define USE_IMU_MPU6500
+#define IMU_MPU6500_ALIGN       CW270_DEG
 #define MPU6500_CS_PIN          PA15
 #define MPU6500_SPI_BUS         BUS_SPI3
-#define GYRO_MPU9250_ALIGN      CW270_DEG
+
+#define USE_IMU_MPU9250
+#define IMU_MPU9250_ALIGN       CW270_DEG
 #define MPU9250_CS_PIN          PA15
 #define MPU9250_SPI_BUS         BUS_SPI3
 
-#define USE_ACC
-#define USE_ACC_MPU6050
-#define ACC_MPU6050_ALIGN       CW270_DEG
-#define USE_ACC_MPU6500
-#define ACC_MPU6500_ALIGN       CW270_DEG
-#define USE_ACC_MPU9250
-#define ACC_MPU9250_ALIGN       CW270_DEG
-
 // No baro support.
 //#define USE_BARO
 //#define USE_BARO_MS5611
diff --git a/src/main/target/ALIENFLIGHTF4/target.h b/src/main/target/ALIENFLIGHTF4/target.h
index 39bb130d0c..0e375b2652 100644
--- a/src/main/target/ALIENFLIGHTF4/target.h
+++ b/src/main/target/ALIENFLIGHTF4/target.h
@@ -45,17 +45,11 @@
 #define MPU9250_CS_PIN          SPI1_NSS_PIN
 #define MPU9250_SPI_BUS         BUS_SPI1
 
-#define USE_ACC
-#define USE_ACC_MPU6500
-#define ACC_MPU6500_ALIGN       CW270_DEG
-#define USE_ACC_MPU9250
-#define ACC_MPU9250_ALIGN       CW270_DEG
+#define USE_IMU_MPU6500
+#define IMU_MPU6500_ALIGN       CW270_DEG
+#define USE_IMU_MPU9250
+#define IMU_MPU9250_ALIGN       CW270_DEG
 
-#define USE_GYRO
-#define USE_GYRO_MPU6500
-#define GYRO_MPU6500_ALIGN      CW270_DEG
-#define USE_GYRO_MPU9250
-#define GYRO_MPU9250_ALIGN      CW270_DEG
 
 #define USE_MAG
 #define MAG_I2C_BUS             BUS_I2C1
diff --git a/src/main/target/ALIENFLIGHTNGF7/target.h b/src/main/target/ALIENFLIGHTNGF7/target.h
index 116cf92d5f..197d8a5875 100644
--- a/src/main/target/ALIENFLIGHTNGF7/target.h
+++ b/src/main/target/ALIENFLIGHTNGF7/target.h
@@ -42,17 +42,11 @@
 #define MPU9250_CS_PIN          SPI1_NSS_PIN
 #define MPU9250_SPI_BUS         BUS_SPI1
 
-#define USE_ACC
-#define USE_ACC_MPU6500
-#define ACC_MPU6500_ALIGN       CW270_DEG
-#define USE_ACC_MPU9250
-#define ACC_MPU9250_ALIGN       CW270_DEG
+#define USE_IMU_MPU6500
+#define IMU_MPU6500_ALIGN       CW270_DEG
 
-#define USE_GYRO
-#define USE_GYRO_MPU6500
-#define GYRO_MPU6500_ALIGN      CW270_DEG
-#define USE_GYRO_MPU9250
-#define GYRO_MPU9250_ALIGN      CW270_DEG
+#define USE_IMU_MPU9250
+#define IMU_MPU9250_ALIGN       CW270_DEG
 
 #define USE_MAG
 #define MAG_I2C_BUS             BUS_I2C1
diff --git a/src/main/target/ANYFC/target.h b/src/main/target/ANYFC/target.h
index 6959a3856b..78d1cab751 100644
--- a/src/main/target/ANYFC/target.h
+++ b/src/main/target/ANYFC/target.h
@@ -30,13 +30,8 @@
 #define MPU6000_CS_PIN          PA4
 #define MPU6000_SPI_BUS         BUS_SPI1
 
-#define USE_GYRO
-#define USE_GYRO_MPU6000
-#define GYRO_MPU6000_ALIGN      CW270_DEG
-
-#define USE_ACC
-#define USE_ACC_MPU6000
-#define ACC_MPU6000_ALIGN       CW270_DEG
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW270_DEG
 
 // MPU6000 interrupts
 #define USE_EXTI
diff --git a/src/main/target/ANYFCF7/target.h b/src/main/target/ANYFCF7/target.h
index 288c8136f2..16baaca9cc 100644
--- a/src/main/target/ANYFCF7/target.h
+++ b/src/main/target/ANYFCF7/target.h
@@ -30,13 +30,8 @@
 #define MPU6000_CS_PIN        PA4
 #define MPU6000_SPI_BUS       BUS_SPI1
 
-#define USE_ACC
-#define USE_ACC_MPU6000
-#define ACC_MPU6000_ALIGN CW270_DEG
-
-#define USE_GYRO
-#define USE_GYRO_MPU6000
-#define GYRO_MPU6000_ALIGN CW270_DEG
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN CW270_DEG
 
 // MPU6000 interrupts
 #define USE_MPU_DATA_READY_SIGNAL
diff --git a/src/main/target/ANYFCM7/target.h b/src/main/target/ANYFCM7/target.h
index c591f5691b..ca42a0ad2e 100644
--- a/src/main/target/ANYFCM7/target.h
+++ b/src/main/target/ANYFCM7/target.h
@@ -30,13 +30,8 @@
 #define MPU6000_CS_PIN          PA4
 #define MPU6000_SPI_BUS         BUS_SPI1
 
-#define USE_ACC
-#define USE_ACC_MPU6000
-#define ACC_MPU6000_ALIGN CW270_DEG
-
-#define USE_GYRO
-#define USE_GYRO_MPU6000
-#define GYRO_MPU6000_ALIGN CW270_DEG
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN CW270_DEG
 
 // MPU6000 interrupts
 #define USE_MPU_DATA_READY_SIGNAL
diff --git a/src/main/target/ASGARD32F4/target.h b/src/main/target/ASGARD32F4/target.h
index 6351de47d4..49c8dd10c3 100644
--- a/src/main/target/ASGARD32F4/target.h
+++ b/src/main/target/ASGARD32F4/target.h
@@ -39,15 +39,10 @@
 // #define GYRO_INT_EXTI            PC8
 // #define USE_MPU_DATA_READY_SIGNAL        // Not connected on FireworksV2
 
-#define USE_GYRO
-#define USE_ACC
-
-#define USE_GYRO_MPU6000
-#define USE_ACC_MPU6000
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW90_DEG
 #define MPU6000_CS_PIN          PA4
 #define MPU6000_SPI_BUS         BUS_SPI1
-#define GYRO_MPU6000_ALIGN      CW90_DEG
-#define ACC_MPU6000_ALIGN       CW90_DEG
 
 // #define USE_MAG
 // #define MAG_I2C_BUS             BUS_I2C2
diff --git a/src/main/target/ASGARD32F7/target.h b/src/main/target/ASGARD32F7/target.h
index 76c58145a7..f26650eeeb 100644
--- a/src/main/target/ASGARD32F7/target.h
+++ b/src/main/target/ASGARD32F7/target.h
@@ -39,15 +39,10 @@
 // #define GYRO_INT_EXTI            PC8
 // #define USE_MPU_DATA_READY_SIGNAL        // Not connected on FireworksV2
 
-#define USE_GYRO
-#define USE_ACC
-
-#define USE_GYRO_MPU6000
-#define USE_ACC_MPU6000
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW90_DEG
 #define MPU6000_CS_PIN          PA4
 #define MPU6000_SPI_BUS         BUS_SPI1
-#define GYRO_MPU6000_ALIGN      CW90_DEG
-#define ACC_MPU6000_ALIGN       CW90_DEG
 
 #define USE_MAG
 #define MAG_I2C_BUS             BUS_I2C2
diff --git a/src/main/target/BEEROTORF4/target.h b/src/main/target/BEEROTORF4/target.h
index 1c11634ba8..7f7ed3dea6 100644
--- a/src/main/target/BEEROTORF4/target.h
+++ b/src/main/target/BEEROTORF4/target.h
@@ -36,13 +36,8 @@
 #define MPU6500_CS_PIN          SPI1_NSS_PIN
 #define MPU6500_SPI_BUS         BUS_SPI1
 
-#define USE_GYRO
-#define USE_GYRO_MPU6500
-#define GYRO_MPU6500_ALIGN      CW270_DEG
-
-#define USE_ACC
-#define USE_ACC_MPU6500
-#define ACC_MPU6500_ALIGN       CW270_DEG
+#define USE_IMU_MPU6500
+#define IMU_MPU6500_ALIGN       CW270_DEG
 
 #define USE_BARO
 #define BARO_I2C_BUS            BUS_I2C1
diff --git a/src/main/target/BETAFLIGHTF3/target.h b/src/main/target/BETAFLIGHTF3/target.h
index 3b70e722fc..2f50dc866e 100755
--- a/src/main/target/BETAFLIGHTF3/target.h
+++ b/src/main/target/BETAFLIGHTF3/target.h
@@ -26,13 +26,8 @@
 #define MPU6000_CS_PIN          PA15
 #define MPU6000_SPI_BUS    BUS_SPI1
 
-#define USE_GYRO
-#define USE_GYRO_MPU6000
-#define GYRO_MPU6000_ALIGN      CW180_DEG
-
-#define USE_ACC
-#define USE_ACC_MPU6000
-#define ACC_MPU6000_ALIGN       CW180_DEG
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW180_DEG
 
 // MPU6000 interrupts
 #define USE_MPU_DATA_READY_SIGNAL
diff --git a/src/main/target/BETAFLIGHTF4/target.h b/src/main/target/BETAFLIGHTF4/target.h
index 0fa8b8beec..cf4727c7b3 100755
--- a/src/main/target/BETAFLIGHTF4/target.h
+++ b/src/main/target/BETAFLIGHTF4/target.h
@@ -30,14 +30,8 @@
 #define MPU6000_CS_PIN          PA4
 #define MPU6000_SPI_BUS		BUS_SPI1
 
-#define USE_ACC
-#define USE_ACC_MPU6000
-#define ACC_MPU6000_ALIGN       CW180_DEG
-
-#define USE_GYRO
-#define USE_GYRO_MPU6000
-#define GYRO_MPU6000_ALIGN      CW180_DEG
-
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW180_DEG
 
 
 // MPU6000 interrupts
diff --git a/src/main/target/BLUEJAYF4/hardware_revision.c b/src/main/target/BLUEJAYF4/hardware_revision.c
index 7192169d40..adfb5a81a5 100644
--- a/src/main/target/BLUEJAYF4/hardware_revision.c
+++ b/src/main/target/BLUEJAYF4/hardware_revision.c
@@ -84,7 +84,7 @@ void detectHardwareRevision(void)
 }
 
 /* BJF4_REV1 has different connection of memory chip */
-BUSDEV_REGISTER_SPI_TAG(m25p16_bjf3_rev1, DEVHW_M25P16, M25P16_SPI_BUS, PB3, NONE, 1, DEVFLAGS_NONE);
+BUSDEV_REGISTER_SPI_TAG(m25p16_bjf3_rev1, DEVHW_M25P16, M25P16_SPI_BUS, PB3, NONE, 1, DEVFLAGS_NONE, 0);
 
 void updateHardwareRevision(void)
 {
diff --git a/src/main/target/BLUEJAYF4/target.h b/src/main/target/BLUEJAYF4/target.h
index 92184c0930..06cca4cf11 100644
--- a/src/main/target/BLUEJAYF4/target.h
+++ b/src/main/target/BLUEJAYF4/target.h
@@ -40,13 +40,8 @@
 #define MPU6500_CS_PIN          PC4
 #define MPU6500_SPI_BUS         BUS_SPI1
 
-#define USE_ACC
-#define USE_ACC_MPU6500
-#define ACC_MPU6500_ALIGN       CW0_DEG
-
-#define USE_GYRO
-#define USE_GYRO_MPU6500
-#define GYRO_MPU6500_ALIGN      CW0_DEG
+#define USE_IMU_MPU6500
+#define IMU_MPU6500_ALIGN       CW0_DEG
 
 #define USE_MAG
 #define MAG_I2C_BUS             BUS_I2C1
diff --git a/src/main/target/CHEBUZZF3/target.h b/src/main/target/CHEBUZZF3/target.h
index 3fdacef844..ad03b71bcb 100644
--- a/src/main/target/CHEBUZZF3/target.h
+++ b/src/main/target/CHEBUZZF3/target.h
@@ -29,22 +29,17 @@
 #define USE_SPI
 #define USE_SPI_DEVICE_1
 
-#define USE_GYRO
-#define USE_GYRO_L3GD20
-#define USE_GYRO_MPU6050
+#define USE_IMU_L3GD20
+#define USE_IMU_MPU6050
+#define USE_IMU_LSM303DLHC
 
 #define MPU6050_I2C_BUS                 BUS_I2C1
 #define LSM303DLHC_I2C_BUS              BUS_I2C1
 #define L3GD20_SPI_BUS                  BUS_SPI1
 #define L3GD20_CS_PIN                   PE3
 
-#define GYRO_L3GD20_ALIGN CW270_DEG
-#define GYRO_MPU6050_ALIGN CW0_DEG
-
-#define USE_ACC
-#define USE_ACC_MPU6050
-#define USE_ACC_LSM303DLHC
-#define ACC_MPU6050_ALIGN       CW0_DEG
+#define IMU_L3GD20_ALIGN       CW270_DEG
+#define IMU_MPU6050_ALIGN      CW0_DEG
 
 #define USE_BARO
 #define BARO_I2C_BUS            BUS_I2C1
diff --git a/src/main/target/CLRACINGF4AIR/target.h b/src/main/target/CLRACINGF4AIR/target.h
index 54955f2910..992e7e79c4 100644
--- a/src/main/target/CLRACINGF4AIR/target.h
+++ b/src/main/target/CLRACINGF4AIR/target.h
@@ -43,15 +43,13 @@
 #else
 #define SPI3_MOSI_PIN           PC12
 #endif
+
 //MPU-9250
 #define MPU9250_CS_PIN          PA4
 #define MPU9250_SPI_BUS         BUS_SPI1
-#define USE_GYRO
-#define USE_GYRO_MPU9250
-#define GYRO_MPU9250_ALIGN      CW0_DEG
-#define USE_ACC
-#define USE_ACC_MPU9250
-#define ACC_MPU9250_ALIGN       CW0_DEG
+#define USE_IMU_MPU9250
+#define IMU_MPU9250_ALIGN       CW0_DEG
+
 #define USE_MAG
 #define USE_MAG_MPU9250
 #define MAG_MPU9250_ALIGN       CW90_DEG
diff --git a/src/main/target/COLIBRI/target.h b/src/main/target/COLIBRI/target.h
index 53d6dd0be5..d852c19a06 100755
--- a/src/main/target/COLIBRI/target.h
+++ b/src/main/target/COLIBRI/target.h
@@ -39,11 +39,7 @@
 #define MPU6000_CS_PIN          PC4
 #define MPU6000_SPI_BUS         BUS_SPI1
 
-#define USE_ACC
-#define USE_ACC_MPU6000
-
-#define USE_GYRO
-#define USE_GYRO_MPU6000
+#define USE_IMU_MPU6000
 
 // MPU6000 interrupts
 #define USE_EXTI
@@ -62,12 +58,10 @@
 #define TEMPERATURE_I2C_BUS     BUS_I2C3
 
 #ifdef QUANTON
-#define ACC_MPU6000_ALIGN       CW90_DEG
-#define GYRO_MPU6000_ALIGN      CW90_DEG
+#define IMU_MPU6000_ALIGN       CW90_DEG
 #define MAG_HMC5883_ALIGN       CW90_DEG
 #else
-#define ACC_MPU6000_ALIGN       CW270_DEG_FLIP
-#define GYRO_MPU6000_ALIGN      CW270_DEG_FLIP
+#define IMU_MPU6000_ALIGN       CW270_DEG_FLIP
 #define MAG_HMC5883_ALIGN       CW270_DEG_FLIP
 #endif
 
diff --git a/src/main/target/COLIBRI_RACE/target.h b/src/main/target/COLIBRI_RACE/target.h
index 82cc819192..57e6a98eee 100755
--- a/src/main/target/COLIBRI_RACE/target.h
+++ b/src/main/target/COLIBRI_RACE/target.h
@@ -52,21 +52,12 @@
 #define MPU9250_CS_PIN          SPI1_NSS_PIN
 #define MPU9250_SPI_BUS         BUS_SPI1
 
-#define USE_GYRO
-#define USE_GYRO_MPU6000
-#define GYRO_MPU6000_ALIGN      CW270_DEG
-#define USE_GYRO_MPU6500
-#define GYRO_MPU6500_ALIGN      CW270_DEG
-#define USE_GYRO_MPU9250
-#define GYRO_MPU9250_ALIGN      CW270_DEG
-
-#define USE_ACC
-#define USE_ACC_MPU6000
-#define ACC_MPU6000_ALIGN       CW270_DEG
-#define USE_ACC_MPU6500
-#define ACC_MPU6500_ALIGN       CW270_DEG
-#define USE_ACC_MPU9250
-#define ACC_MPU9250_ALIGN       CW270_DEG
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW270_DEG
+#define USE_IMU_MPU6500
+#define IMU_MPU6500_ALIGN       CW270_DEG
+#define USE_IMU_MPU9250
+#define IMU_MPU9250_ALIGN       CW270_DEG
 
 #define USE_BARO
 #define BARO_I2C_BUS            BUS_I2C2
diff --git a/src/main/target/DALRCF405/target.h b/src/main/target/DALRCF405/target.h
index 538aac3394..b478f8e238 100644
--- a/src/main/target/DALRCF405/target.h
+++ b/src/main/target/DALRCF405/target.h
@@ -36,22 +36,15 @@
 #define GYRO_INT_EXTI            PC4
 #define USE_MPU_DATA_READY_SIGNAL
 
-#define USE_GYRO
-#define USE_ACC
-
 #define MPU6500_CS_PIN          PA4
 #define MPU6500_SPI_BUS         BUS_SPI1
-#define USE_GYRO_MPU6500
-#define GYRO_MPU6500_ALIGN      CW90_DEG
-#define USE_ACC_MPU6500
-#define ACC_MPU6500_ALIGN       CW90_DEG
+#define USE_IMU_MPU6500
+#define IMU_MPU6500_ALIGN       CW90_DEG
 
 #define MPU6000_CS_PIN          PA4
 #define MPU6000_SPI_BUS         BUS_SPI1
-#define USE_GYRO_MPU6000
-#define GYRO_MPU6000_ALIGN      CW90_DEG
-#define USE_ACC_MPU6000
-#define ACC_MPU6000_ALIGN       CW90_DEG
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW90_DEG
 
 
 //FLASH
diff --git a/src/main/target/DALRCF722DUAL/target.h b/src/main/target/DALRCF722DUAL/target.h
index ccf6ac6138..534042b8da 100644
--- a/src/main/target/DALRCF722DUAL/target.h
+++ b/src/main/target/DALRCF722DUAL/target.h
@@ -26,16 +26,11 @@
 #define BEEPER                  PC13
 #define BEEPER_INVERTED
 
-#define USE_GYRO
-#define USE_ACC
-
 // MPU6000
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW180_DEG
 #define MPU6000_CS_PIN          PB0
 #define MPU6000_SPI_BUS         BUS_SPI1
-#define USE_GYRO_MPU6000
-#define GYRO_MPU6000_ALIGN      CW180_DEG
-#define USE_ACC_MPU6000
-#define ACC_MPU6000_ALIGN       CW180_DEG
 
 #define USE_EXTI
 #define GYRO_INT_EXTI            PB10
diff --git a/src/main/target/F4BY/target.h b/src/main/target/F4BY/target.h
index 9231b8a808..6c2ebc7925 100644
--- a/src/main/target/F4BY/target.h
+++ b/src/main/target/F4BY/target.h
@@ -39,19 +39,11 @@
 #define ICM20689_SPI_BUS        BUS_SPI1
 
 
-#define USE_ACC
-#define USE_ACC_MPU6000
-#define ACC_MPU6000_ALIGN       CW90_DEG
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW90_DEG
 
-#define USE_GYRO
-#define USE_GYRO_MPU6000
-#define GYRO_MPU6000_ALIGN      CW90_DEG
-
-#define USE_ACC_ICM20689
-#define ACC_ICM20689_ALIGN       CW90_DEG
-
-#define USE_GYRO_ICM20689
-#define GYRO_ICM20689_ALIGN      CW90_DEG
+#define USE_IMU_ICM20689
+#define IMU_ICM20689_ALIGN       CW90_DEG
 
 #define USE_MAG
 #define MAG_I2C_BUS             BUS_I2C2
diff --git a/src/main/target/FALCORE/target.h b/src/main/target/FALCORE/target.h
index 8e370c958e..d8480eda73 100755
--- a/src/main/target/FALCORE/target.h
+++ b/src/main/target/FALCORE/target.h
@@ -35,13 +35,8 @@
 #define GYRO_INT_EXTI            PB0
 #define USE_MPU_DATA_READY_SIGNAL
 
-#define USE_GYRO
-#define USE_GYRO_MPU6500
-#define GYRO_MPU6500_ALIGN CW0_DEG
-
-#define USE_ACC
-#define USE_ACC_MPU6500
-#define ACC_MPU6500_ALIGN CW0_DEG
+#define USE_IMU_MPU6500
+#define IMU_MPU6500_ALIGN CW0_DEG
 
 #define USE_BARO
 #define BARO_I2C_BUS            BUS_I2C1
diff --git a/src/main/target/FF_F35_LIGHTNING/target.h b/src/main/target/FF_F35_LIGHTNING/target.h
index dec7d460e6..df981eeb99 100644
--- a/src/main/target/FF_F35_LIGHTNING/target.h
+++ b/src/main/target/FF_F35_LIGHTNING/target.h
@@ -35,13 +35,8 @@
 #define MPU9250_CS_PIN          PC0
 #define MPU9250_SPI_BUS         BUS_SPI3
 
-#define USE_ACC
-#define USE_ACC_MPU9250
-#define ACC_MPU9250_ALIGN       CW180_DEG
-
-#define USE_GYRO
-#define USE_GYRO_MPU9250
-#define GYRO_MPU9250_ALIGN      CW180_DEG
+#define USE_IMU_MPU9250
+#define IMU_MPU9250_ALIGN       CW180_DEG
 
 #define USE_MAG
 #define USE_MAG_MPU9250
diff --git a/src/main/target/FF_FORTINIF4/target.h b/src/main/target/FF_FORTINIF4/target.h
index 44a87b5e69..669f4bd440 100644
--- a/src/main/target/FF_FORTINIF4/target.h
+++ b/src/main/target/FF_FORTINIF4/target.h
@@ -45,13 +45,8 @@
 #define MPU6500_CS_PIN          PA8
 #define MPU6500_SPI_BUS         BUS_SPI1
 
-#define USE_GYRO
-#define USE_GYRO_MPU6500
-#define GYRO_MPU6500_ALIGN      CW180_DEG
-
-#define USE_ACC
-#define USE_ACC_MPU6500
-#define ACC_MPU6500_ALIGN       CW180_DEG
+#define USE_IMU_MPU6500
+#define IMU_MPU6500_ALIGN       CW180_DEG
 /*---------------------------------*/
 
 /*------------FLASH----------------*/
diff --git a/src/main/target/FF_PIKOF4/target.h b/src/main/target/FF_PIKOF4/target.h
index 5ec9a8f634..59337ad53b 100644
--- a/src/main/target/FF_PIKOF4/target.h
+++ b/src/main/target/FF_PIKOF4/target.h
@@ -64,19 +64,11 @@
 #define MPU6500_SPI_BUS         BUS_SPI1
 #endif
 
-#define USE_GYRO
-#define USE_GYRO_MPU6000
-#define GYRO_MPU6000_ALIGN      CW180_DEG
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW180_DEG
 
-#define USE_GYRO_MPU6500
-#define GYRO_MPU6500_ALIGN      CW180_DEG
-
-#define USE_ACC
-#define USE_ACC_MPU6000
-#define ACC_MPU6000_ALIGN       CW180_DEG
-
-#define USE_ACC_MPU6500
-#define ACC_MPU6500_ALIGN       CW180_DEG
+#define USE_IMU_MPU6500
+#define IMU_MPU6500_ALIGN       CW180_DEG
 /*---------------------------------*/
 
 #if defined(FF_PIKOF4OSD)
diff --git a/src/main/target/FIREWORKSV2/target.h b/src/main/target/FIREWORKSV2/target.h
index c11eeda584..6e707a1f28 100644
--- a/src/main/target/FIREWORKSV2/target.h
+++ b/src/main/target/FIREWORKSV2/target.h
@@ -62,31 +62,23 @@
 #define GYRO_INT_EXTI            PC8
 // #define USE_MPU_DATA_READY_SIGNAL        // Not connected on FireworksV2
 
-#define USE_GYRO
-#define USE_ACC
-
-#define USE_GYRO_MPU6500
-#define USE_ACC_MPU6500
+#define USE_IMU_MPU6500
 
 #if defined(OMNIBUSF4V6)
 #define MPU6500_CS_PIN          PC14
 #define MPU6500_SPI_BUS         BUS_SPI1
-#define GYRO_MPU6500_ALIGN      CW0_DEG
-#define ACC_MPU6500_ALIGN       CW0_DEG
+#define IMU_MPU6500_ALIGN       CW0_DEG
 #else
 #define MPU6500_CS_PIN          PD2
 #define MPU6500_SPI_BUS         BUS_SPI3
-#define GYRO_MPU6500_ALIGN      CW180_DEG
-#define ACC_MPU6500_ALIGN       CW180_DEG
+#define IMU_MPU6500_ALIGN       CW180_DEG
 #endif
 
 // OmnibusF4 Nano v6 and OmnibusF4 V6 has a MPU6000
-#define USE_GYRO_MPU6000
-#define USE_ACC_MPU6000
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW180_DEG
 #define MPU6000_CS_PIN          PA4
 #define MPU6000_SPI_BUS         BUS_SPI1
-#define GYRO_MPU6000_ALIGN      CW180_DEG
-#define ACC_MPU6000_ALIGN       CW180_DEG
 
 #define USE_MAG
 #if defined(OMNIBUSF4V6)
diff --git a/src/main/target/FISHDRONEF4/target.h b/src/main/target/FISHDRONEF4/target.h
index bbea161301..91fd29491b 100644
--- a/src/main/target/FISHDRONEF4/target.h
+++ b/src/main/target/FISHDRONEF4/target.h
@@ -38,17 +38,11 @@
 #define MPU9250_CS_PIN          PA4
 #define MPU9250_SPI_BUS         BUS_SPI1
 
-#define USE_ACC
-#define USE_ACC_MPU6500
-#define GYRO_MPU6500_ALIGN      CW180_DEG
-#define USE_ACC_MPU9250
-#define GYRO_MPU9250_ALIGN      CW180_DEG
+#define USE_IMU_MPU6500
+#define IMU_MPU6500_ALIGN       CW180_DEG
 
-#define USE_GYRO
-#define USE_GYRO_MPU6500
-#define ACC_MPU6500_ALIGN       CW180_DEG
-#define USE_GYRO_MPU9250
-#define ACC_MPU9250_ALIGN       CW180_DEG
+#define USE_IMU_MPU9250
+#define IMU_MPU9250_ALIGN       CW180_DEG
 
 // MPU6500 interrupts
 #define USE_EXTI
diff --git a/src/main/target/FOXEERF405/target.h b/src/main/target/FOXEERF405/target.h
index dc8b964c7e..c4c38aa3c7 100644
--- a/src/main/target/FOXEERF405/target.h
+++ b/src/main/target/FOXEERF405/target.h
@@ -27,27 +27,21 @@
 
 /*** IMU sensors ***/
 #define USE_EXTI
-#define USE_GYRO
-#define USE_ACC
 
 #define GYRO_INT_EXTI            PC4
 #define USE_MPU_DATA_READY_SIGNAL
 
 // MPU6000
-#define USE_GYRO_MPU6000
-#define USE_ACC_MPU6000
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW90_DEG
 #define MPU6000_CS_PIN          PB2
 #define MPU6000_SPI_BUS         BUS_SPI1
-#define GYRO_MPU6000_ALIGN      CW90_DEG
-#define ACC_MPU6000_ALIGN       CW90_DEG
 
 // ICM20689 - handled by MPU6500 driver
-#define USE_GYRO_MPU6500
-#define USE_ACC_MPU6500
+#define USE_IMU_MPU6500
+#define IMU_MPU6500_ALIGN       CW90_DEG
 #define MPU6500_CS_PIN          PB2
 #define MPU6500_SPI_BUS         BUS_SPI1
-#define GYRO_MPU6500_ALIGN      CW90_DEG
-#define ACC_MPU6500_ALIGN       CW90_DEG
 
 /*** SPI/I2C bus ***/
 #define USE_SPI
diff --git a/src/main/target/FOXEERF722DUAL/target.c b/src/main/target/FOXEERF722DUAL/target.c
index 27d080d663..80285de5a5 100644
--- a/src/main/target/FOXEERF722DUAL/target.c
+++ b/src/main/target/FOXEERF722DUAL/target.c
@@ -20,10 +20,11 @@
 #include "drivers/io.h"
 #include "drivers/bus.h"
 #include "drivers/timer.h"
+#include "drivers/sensor.h"
 #include "drivers/pwm_mapping.h"
 
-BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000,     DEVHW_MPU6000,      MPU6000_SPI_BUS,    MPU6000_CS_PIN,     MPU6000_EXTI_PIN,       0,  DEVFLAGS_NONE);
-BUSDEV_REGISTER_SPI_TAG(busdev_mpu6500,     DEVHW_MPU6500,      MPU6500_SPI_BUS,    MPU6500_CS_PIN,     MPU6500_EXTI_PIN,       1,  DEVFLAGS_NONE);
+BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000,     DEVHW_MPU6000,      MPU6000_SPI_BUS,    MPU6000_CS_PIN,     MPU6000_EXTI_PIN,       0,  DEVFLAGS_NONE,  IMU_MPU6000_ALIGN);
+BUSDEV_REGISTER_SPI_TAG(busdev_mpu6500,     DEVHW_MPU6500,      MPU6500_SPI_BUS,    MPU6500_CS_PIN,     MPU6500_EXTI_PIN,       1,  DEVFLAGS_NONE,  IMU_MPU6500_ALIGN);
 
 const timerHardware_t timerHardware[] = {
     DEF_TIM(TIM4,  CH2, PB7,  TIM_USE_PPM,                          0, 0),  // PPM&SBUS  
diff --git a/src/main/target/FOXEERF722DUAL/target.h b/src/main/target/FOXEERF722DUAL/target.h
index a770e605b5..75eec081ff 100644
--- a/src/main/target/FOXEERF722DUAL/target.h
+++ b/src/main/target/FOXEERF722DUAL/target.h
@@ -27,8 +27,6 @@
 
 /*** IMU sensors ***/
 #define USE_EXTI
-#define USE_ACC
-#define USE_GYRO
 
 // We use dual IMU sensors, they have to be described in the target file
 #define USE_TARGET_IMU_HARDWARE_DESCRIPTORS
@@ -36,24 +34,18 @@
 #define USE_DUAL_GYRO
 
 // MPU6000
-#define USE_GYRO_MPU6000
-#define USE_ACC_MPU6000
-
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW270_DEG
 #define MPU6000_CS_PIN          PB2
 #define MPU6000_SPI_BUS         BUS_SPI1
 #define MPU6000_EXTI_PIN        PC4
-#define GYRO_MPU6000_ALIGN      CW270_DEG
-#define ACC_MPU6000_ALIGN       CW270_DEG
 
 // ICM20602 - handled by MPU6500 driver
-#define USE_GYRO_MPU6500
-#define USE_ACC_MPU6500
-
+#define USE_IMU_MPU6500
+#define IMU_MPU6500_ALIGN       CW180_DEG
 #define MPU6500_CS_PIN          PB1
 #define MPU6500_SPI_BUS         BUS_SPI1
 #define MPU6500_EXTI_PIN        PB0
-#define GYRO_MPU6500_ALIGN      CW180_DEG
-#define ACC_MPU6500_ALIGN       CW180_DEG
 
 /*** SPI/I2C bus ***/
 #define USE_SPI
diff --git a/src/main/target/FRSKYF3/target.h b/src/main/target/FRSKYF3/target.h
index 6fe5507157..b046c399cb 100644
--- a/src/main/target/FRSKYF3/target.h
+++ b/src/main/target/FRSKYF3/target.h
@@ -28,16 +28,10 @@
 #define USE_MPU_DATA_READY_SIGNAL
 #define MPU_ADDRESS             0x69
 
-#define USE_GYRO
-#define USE_ACC
-
 #define MPU6050_I2C_BUS         BUS_I2C1
 
-#define USE_GYRO_MPU6050
-#define GYRO_MPU6050_ALIGN      CW270_DEG
-
-#define USE_ACC_MPU6050
-#define ACC_MPU6050_ALIGN       CW270_DEG
+#define USE_IMU_MPU6050
+#define IMU_MPU6050_ALIGN       CW270_DEG
 
 #define USE_VCP
 #define USE_UART1
diff --git a/src/main/target/FRSKYF4/target.h b/src/main/target/FRSKYF4/target.h
index b727701ace..4d950a18a7 100755
--- a/src/main/target/FRSKYF4/target.h
+++ b/src/main/target/FRSKYF4/target.h
@@ -25,13 +25,8 @@
 #define MPU6000_CS_PIN          PA4
 #define MPU6000_SPI_BUS         BUS_SPI1
 
-#define USE_GYRO
-#define USE_GYRO_MPU6000
-#define GYRO_MPU6000_ALIGN      CW270_DEG
-
-#define USE_ACC
-#define USE_ACC_MPU6000
-#define ACC_MPU6000_ALIGN       CW270_DEG
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW270_DEG
 
 #define USE_EXTI
 #define GYRO_INT_EXTI            PC4
diff --git a/src/main/target/FURYF3/target.h b/src/main/target/FURYF3/target.h
index 114ee507c3..9091457131 100755
--- a/src/main/target/FURYF3/target.h
+++ b/src/main/target/FURYF3/target.h
@@ -29,31 +29,21 @@
 #define USE_MPU_DATA_READY_SIGNAL
 #define ENSURE_MPU_DATA_READY_IS_LOW
 
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW180_DEG  // changedkb 270
 #define MPU6000_CS_PIN          PA4
 #define MPU6000_SPI_BUS         BUS_SPI1
 
+#define USE_IMU_MPU6500
+#define IMU_MPU6500_ALIGN       CW90_DEG  // changedkb 270
 #define MPU6500_CS_PIN          PA4
 #define MPU6500_SPI_BUS         BUS_SPI1
 
+#define USE_IMU_MPU9250
+#define IMU_MPU9250_ALIGN       CW90_DEG  // changedkb 270
 #define MPU9250_CS_PIN          PA4
 #define MPU9250_SPI_BUS         BUS_SPI1
 
-#define USE_GYRO
-#define USE_GYRO_MPU6000
-#define GYRO_MPU6000_ALIGN      CW180_DEG  // changedkb 270
-#define USE_GYRO_MPU6500
-#define GYRO_MPU6500_ALIGN      CW90_DEG  // changedkb 270
-#define USE_GYRO_MPU9250
-#define GYRO_MPU9250_ALIGN      CW90_DEG  // changedkb 270
-
-#define USE_ACC
-#define USE_ACC_MPU6000
-#define ACC_MPU6000_ALIGN       CW180_DEG  // changedkb 270
-#define USE_ACC_MPU6500
-#define ACC_MPU6500_ALIGN       CW90_DEG  // changedkb 270
-#define USE_ACC_MPU9250
-#define ACC_MPU9250_ALIGN       CW90_DEG  // changedkb 270
-
 #define USE_MAG
 #define MAG_I2C_BUS             BUS_I2C1
 #define USE_MAG_MPU9250
diff --git a/src/main/target/FURYF4OSD/target.h b/src/main/target/FURYF4OSD/target.h
index d380ac7bed..c4603911cb 100644
--- a/src/main/target/FURYF4OSD/target.h
+++ b/src/main/target/FURYF4OSD/target.h
@@ -35,9 +35,13 @@
 #define USE_SPI
 #define USE_SPI_DEVICE_1
 
+#define USE_IMU_MPU6500
+#define IMU_MPU6500_ALIGN       CW180_DEG
 #define MPU6500_CS_PIN          PA4
 #define MPU6500_SPI_BUS         BUS_SPI1
 
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW180_DEG
 #define MPU6000_CS_PIN          PA4
 #define MPU6000_SPI_BUS         BUS_SPI1
 
@@ -45,20 +49,6 @@
 #define GYRO_INT_EXTI            PC4
 #define USE_MPU_DATA_READY_SIGNAL
 
-#define USE_GYRO
-#define USE_GYRO_MPU6500
-#define GYRO_MPU6500_ALIGN      CW180_DEG
-
-#define USE_GYRO_MPU6000
-#define GYRO_MPU6000_ALIGN      CW180_DEG
-
-#define USE_ACC
-#define USE_ACC_MPU6500
-#define ACC_MPU6500_ALIGN       CW180_DEG
-
-#define USE_ACC_MPU6000
-#define ACC_MPU6000_ALIGN       CW180_DEG
-
 #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
 #define USE_SPI_DEVICE_3
 #define SPI3_SCK_PIN            PC10
diff --git a/src/main/target/IFLIGHTF4_TWING/target.c b/src/main/target/IFLIGHTF4_TWING/target.c
index d6566965b0..b4f212ddd6 100644
--- a/src/main/target/IFLIGHTF4_TWING/target.c
+++ b/src/main/target/IFLIGHTF4_TWING/target.c
@@ -23,10 +23,11 @@
 #include "drivers/io.h"
 #include "drivers/pwm_mapping.h"
 #include "drivers/timer.h"
+#include "drivers/sensor.h"
 
 // Board hardware definitions
-BUSDEV_REGISTER_SPI_TAG(busdev_imu0,    DEVHW_MPU6500,  MPU6500_0_SPI_BUS,  MPU6500_0_CS_PIN,   MPU6500_0_EXTI_PIN,     0,  DEVFLAGS_NONE);
-BUSDEV_REGISTER_SPI_TAG(busdev_imu1,    DEVHW_MPU6500,  MPU6500_1_SPI_BUS,  MPU6500_1_CS_PIN,   MPU6500_1_EXTI_PIN,     1,  DEVFLAGS_NONE);
+BUSDEV_REGISTER_SPI_TAG(busdev_imu0,    DEVHW_MPU6500,  MPU6500_0_SPI_BUS,  MPU6500_0_CS_PIN,   MPU6500_0_EXTI_PIN,     0,  DEVFLAGS_NONE,  IMU_0_ALIGN);
+BUSDEV_REGISTER_SPI_TAG(busdev_imu1,    DEVHW_MPU6500,  MPU6500_1_SPI_BUS,  MPU6500_1_CS_PIN,   MPU6500_1_EXTI_PIN,     1,  DEVFLAGS_NONE,  IMU_1_ALIGN);
 
 const timerHardware_t timerHardware[] = {
     DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), // PPM
diff --git a/src/main/target/IFLIGHTF4_TWING/target.h b/src/main/target/IFLIGHTF4_TWING/target.h
index 6f5d6521bb..27d99bdcf7 100644
--- a/src/main/target/IFLIGHTF4_TWING/target.h
+++ b/src/main/target/IFLIGHTF4_TWING/target.h
@@ -34,12 +34,11 @@
 #define SPI1_MISO_PIN           PA6
 #define SPI1_MOSI_PIN           PA7
 
-#define USE_ACC
-#define USE_GYRO 
 #define USE_DUAL_GYRO
 
-#define USE_ACC_MPU6500
-#define USE_GYRO_MPU6500
+#define USE_IMU_MPU6500
+#define IMU_0_ALIGN             CW0_DEG
+#define IMU_1_ALIGN             CW0_DEG
 
 #define MPU6500_0_CS_PIN        PA4
 #define MPU6500_0_SPI_BUS       BUS_SPI1
@@ -49,9 +48,6 @@
 #define MPU6500_1_SPI_BUS       BUS_SPI1
 #define MPU6500_1_EXTI_PIN      PA8
 
-#define GYRO_MPU6500_ALIGN      CW0_DEG
-#define ACC_MPU6500_ALIGN       CW0_DEG
-
 #define USE_EXTI
 #define USE_MPU_DATA_READY_SIGNAL
 
diff --git a/src/main/target/IFLIGHTF7_TWING/target.c b/src/main/target/IFLIGHTF7_TWING/target.c
index 46285e6857..83090b5c94 100644
--- a/src/main/target/IFLIGHTF7_TWING/target.c
+++ b/src/main/target/IFLIGHTF7_TWING/target.c
@@ -23,10 +23,11 @@
 #include "drivers/io.h"
 #include "drivers/pwm_mapping.h"
 #include "drivers/timer.h"
+#include "drivers/sensor.h"
 
 // Board hardware definitions
-BUSDEV_REGISTER_SPI_TAG(busdev_imu0,    DEVHW_MPU6500,  MPU6500_0_SPI_BUS,  MPU6500_0_CS_PIN,   MPU6500_0_EXTI_PIN,     0,  DEVFLAGS_NONE);
-BUSDEV_REGISTER_SPI_TAG(busdev_imu1,    DEVHW_MPU6500,  MPU6500_1_SPI_BUS,  MPU6500_1_CS_PIN,   MPU6500_1_EXTI_PIN,     1,  DEVFLAGS_NONE);
+BUSDEV_REGISTER_SPI_TAG(busdev_imu0,    DEVHW_MPU6500,  MPU6500_0_SPI_BUS,  MPU6500_0_CS_PIN,   MPU6500_0_EXTI_PIN,     0,  DEVFLAGS_NONE,  IMU_0_ALIGN);
+BUSDEV_REGISTER_SPI_TAG(busdev_imu1,    DEVHW_MPU6500,  MPU6500_1_SPI_BUS,  MPU6500_1_CS_PIN,   MPU6500_1_EXTI_PIN,     1,  DEVFLAGS_NONE,  IMU_1_ALIGN);
 
 const timerHardware_t timerHardware[] = {
     DEF_TIM(TIM5, CH4, PA3, TIM_USE_PPM,                         0, 0),
diff --git a/src/main/target/IFLIGHTF7_TWING/target.h b/src/main/target/IFLIGHTF7_TWING/target.h
index 7741e9617a..6c67e21e27 100644
--- a/src/main/target/IFLIGHTF7_TWING/target.h
+++ b/src/main/target/IFLIGHTF7_TWING/target.h
@@ -36,12 +36,11 @@
 #define SPI1_MISO_PIN           PA6
 #define SPI1_MOSI_PIN           PA7
 
-#define USE_ACC
-#define USE_GYRO 
 #define USE_DUAL_GYRO
 
-#define USE_ACC_MPU6500
-#define USE_GYRO_MPU6500
+#define USE_IMU_MPU6500
+#define IMU_0_ALIGN       CW90_DEG
+#define IMU_1_ALIGN       CW90_DEG
 
 #define MPU6500_0_CS_PIN        PC3
 #define MPU6500_0_SPI_BUS       BUS_SPI1
@@ -51,8 +50,6 @@
 #define MPU6500_1_SPI_BUS       BUS_SPI1
 #define MPU6500_1_EXTI_PIN      PA8
 
-#define GYRO_MPU6500_ALIGN      CW90_DEG
-#define ACC_MPU6500_ALIGN       CW90_DEG
 
 #define USE_EXTI
 #define USE_MPU_DATA_READY_SIGNAL
diff --git a/src/main/target/KAKUTEF4/target.h b/src/main/target/KAKUTEF4/target.h
index a4ea4a00b7..0bc68d3e3d 100755
--- a/src/main/target/KAKUTEF4/target.h
+++ b/src/main/target/KAKUTEF4/target.h
@@ -42,16 +42,11 @@
 #define GYRO_INT_EXTI            PC5
 #define USE_MPU_DATA_READY_SIGNAL
 
+#define USE_IMU_MPU6500
+#define IMU_MPU6500_ALIGN       CW270_DEG
 #define MPU6500_CS_PIN          PC4
 #define MPU6500_SPI_BUS         BUS_SPI1
 
-#define USE_GYRO
-#define USE_GYRO_MPU6500
-#define GYRO_MPU6500_ALIGN      CW270_DEG
-
-#define USE_ACC
-#define USE_ACC_MPU6500
-#define ACC_MPU6500_ALIGN       CW270_DEG
 
 #ifdef KAKUTEF4V2
 #   define USE_I2C
diff --git a/src/main/target/KAKUTEF7/target.h b/src/main/target/KAKUTEF7/target.h
index 82be9590a0..02d398e6b6 100644
--- a/src/main/target/KAKUTEF7/target.h
+++ b/src/main/target/KAKUTEF7/target.h
@@ -36,30 +36,22 @@
 #define USE_DSHOT
 #define USE_ESC_SENSOR
 
-#define USE_ACC
-#define USE_GYRO
-
 #define USE_MPU_DATA_READY_SIGNAL
 #define USE_EXTI
 
 // ICM-20689
-#define USE_ACC_ICM20689
-#define USE_GYRO_ICM20689
-#define GYRO_ICM20689_ALIGN      CW270_DEG
-#define ACC_ICM20689_ALIGN       CW270_DEG
-
+#define USE_IMU_ICM20689
+#define IMU_ICM20689_ALIGN       CW270_DEG
 #define GYRO_INT_EXTI            PE1
 #define ICM20689_CS_PIN          SPI4_NSS_PIN
 #define ICM20689_SPI_BUS         BUS_SPI4
 
-#define USE_GYRO_MPU6000
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW270_DEG
 #define GYRO_INT_EXTI           PE1
-#define GYRO_MPU6000_ALIGN      CW270_DEG
 #define MPU6000_CS_PIN          SPI4_NSS_PIN
 #define MPU6000_SPI_BUS         BUS_SPI4
 
-#define USE_ACC_MPU6000
-#define ACC_MPU6000_ALIGN       CW270_DEG
 
 #define USB_IO
 #define USE_VCP
diff --git a/src/main/target/KFC32F3_INAV/target.h b/src/main/target/KFC32F3_INAV/target.h
index 40808156df..26e6ebeecb 100755
--- a/src/main/target/KFC32F3_INAV/target.h
+++ b/src/main/target/KFC32F3_INAV/target.h
@@ -36,16 +36,11 @@
 
 #define BUS_SPI_SPEED_MAX BUS_SPEED_SLOW
 
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW90_DEG
 #define MPU6000_CS_PIN          PB5
 #define MPU6000_SPI_BUS         BUS_SPI2
 
-#define USE_GYRO
-#define USE_GYRO_MPU6000
-#define GYRO_MPU6000_ALIGN CW90_DEG
-
-#define USE_ACC
-#define USE_ACC_MPU6000
-#define ACC_MPU6000_ALIGN CW90_DEG
 
 #define USE_MAG
 #define MAG_I2C_BUS             BUS_I2C1
diff --git a/src/main/target/KISSFC/target.h b/src/main/target/KISSFC/target.h
index bf37520717..a97d2b6166 100644
--- a/src/main/target/KISSFC/target.h
+++ b/src/main/target/KISSFC/target.h
@@ -29,15 +29,10 @@
 #define BEEPER              PB13
 #define BEEPER_INVERTED
 
-#define USE_ACC
-#define USE_ACC_MPU6050
-#define ACC_MPU6050_ALIGN       CW180_DEG
+#define USE_IMU_MPU6050
+#define IMU_MPU6050_ALIGN       CW180_DEG
 #define MPU6050_I2C_BUS         BUS_I2C1
 
-#define USE_GYRO
-#define USE_GYRO_MPU6050
-#define GYRO_MPU6050_ALIGN      CW180_DEG
-
 #define USE_EXTI
 #define MPU_INT_EXTI            PB2
 #define USE_MPU_DATA_READY_SIGNAL
diff --git a/src/main/target/KROOZX/target.h b/src/main/target/KROOZX/target.h
index 924b619277..d0b86ce00c 100755
--- a/src/main/target/KROOZX/target.h
+++ b/src/main/target/KROOZX/target.h
@@ -36,13 +36,8 @@
 #define USE_MPU_DATA_READY_SIGNAL
 #define GYRO_INT_EXTI            PA4
 
-#define USE_GYRO
-#define USE_GYRO_MPU6000
-#define GYRO_MPU6000_ALIGN      CW90_DEG_FLIP
-
-#define USE_ACC
-#define USE_ACC_MPU6000
-#define ACC_MPU6000_ALIGN       CW90_DEG_FLIP
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW90_DEG_FLIP
 
 #define USE_MAG
 #define MAG_I2C_BUS             BUS_I2C1
diff --git a/src/main/target/LUX_RACE/target.h b/src/main/target/LUX_RACE/target.h
index ca13846a86..5c64316a83 100644
--- a/src/main/target/LUX_RACE/target.h
+++ b/src/main/target/LUX_RACE/target.h
@@ -42,23 +42,16 @@
 #define SPI1_MOSI_PIN           PB5
 #define SPI1_NSS_PIN            PA4
 
+#define USE_IMU_MPU6500
+#define IMU_MPU6500_ALIGN       CW270_DEG
 #define MPU6500_CS_PIN          SPI1_NSS_PIN
 #define MPU6500_SPI_BUS         BUS_SPI1
 
+#define USE_IMU_MPU9250
+#define IMU_MPU9250_ALIGN       CW270_DEG
 #define MPU9250_CS_PIN          SPI1_NSS_PIN
 #define MPU9250_SPI_BUS         BUS_SPI1
 
-#define USE_GYRO
-#define USE_GYRO_MPU6500
-#define GYRO_MPU6500_ALIGN      CW270_DEG
-#define USE_GYRO_MPU9250
-#define GYRO_MPU9250_ALIGN      CW270_DEG
-
-#define USE_ACC
-#define USE_ACC_MPU6500
-#define ACC_MPU6500_ALIGN       CW270_DEG
-#define USE_ACC_MPU9250
-#define ACC_MPU9250_ALIGN       CW270_DEG
 
 #define USE_VCP
 #define USE_UART1
diff --git a/src/main/target/MAMBAF405US/target.h b/src/main/target/MAMBAF405US/target.h
index 67b45f39e4..f4312c06fc 100644
--- a/src/main/target/MAMBAF405US/target.h
+++ b/src/main/target/MAMBAF405US/target.h
@@ -33,20 +33,14 @@
 
 // ******* GYRO and ACC ********
 #define USE_EXTI
-#define USE_GYRO_EXTI
 #define GYRO_INT_EXTI                   PC4
 #define USE_MPU_DATA_READY_SIGNAL
 
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN               CW180_DEG
 #define MPU6000_SPI_BUS                 BUS_SPI1
 #define MPU6000_CS_PIN                  PA4
 
-#define USE_GYRO
-#define USE_GYRO_MPU6000
-#define GYRO_MPU6000_ALIGN              CW180_DEG
-
-#define USE_ACC
-#define USE_ACC_MPU6000
-#define ACC_MPU6500_ALIGN               CW180_DEG
 
 // *************** Baro **************************
 #define USE_I2C
diff --git a/src/main/target/MAMBAF722/target.h b/src/main/target/MAMBAF722/target.h
index 09c3074756..aa324c685c 100644
--- a/src/main/target/MAMBAF722/target.h
+++ b/src/main/target/MAMBAF722/target.h
@@ -33,20 +33,14 @@
 
 // ******* GYRO and ACC ********
 #define USE_EXTI
-#define USE_GYRO_EXTI
 #define GYRO_INT_EXTI                   PC4
 #define USE_MPU_DATA_READY_SIGNAL
 
 #define MPU6000_CS_PIN                  SPI1_NSS_PIN
 #define MPU6000_SPI_BUS                 BUS_SPI1
 
-#define USE_GYRO
-#define USE_GYRO_MPU6000
-#define GYRO_MPU6000_ALIGN              CW180_DEG
-
-#define USE_ACC
-#define USE_ACC_MPU6000
-#define ACC_MPU6000_ALIGN               CW180_DEG
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN               CW180_DEG
 
 // *************** Baro **************************
 #define USE_I2C
diff --git a/src/main/target/MATEKF405/target.h b/src/main/target/MATEKF405/target.h
index df11ef86f7..077155b1ff 100644
--- a/src/main/target/MATEKF405/target.h
+++ b/src/main/target/MATEKF405/target.h
@@ -45,19 +45,11 @@
 #define GYRO_INT_EXTI            PC3
 #define USE_MPU_DATA_READY_SIGNAL
 
-#define USE_GYRO
-#define USE_GYRO_MPU6500
-#define GYRO_MPU6500_ALIGN      CW180_DEG
+#define USE_IMU_MPU6500
+#define IMU_MPU6500_ALIGN       CW180_DEG
 
-#define USE_GYRO_MPU6000
-#define GYRO_MPU6000_ALIGN      CW270_DEG
-
-#define USE_ACC
-#define USE_ACC_MPU6500
-#define ACC_MPU6500_ALIGN       CW180_DEG
-
-#define USE_ACC_MPU6000
-#define ACC_MPU6000_ALIGN       CW270_DEG
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW270_DEG
 
 // *************** SD Card **************************
 #define USE_SDCARD
diff --git a/src/main/target/MATEKF405SE/target.h b/src/main/target/MATEKF405SE/target.h
index 9c9cd393a0..513b14a8b2 100644
--- a/src/main/target/MATEKF405SE/target.h
+++ b/src/main/target/MATEKF405SE/target.h
@@ -33,6 +33,8 @@
 #define SPI1_MISO_PIN   	    PA6
 #define SPI1_MOSI_PIN   	    PA7
 
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW270_DEG
 #define MPU6000_CS_PIN          PA4
 #define MPU6000_SPI_BUS         BUS_SPI1
 
@@ -40,13 +42,6 @@
 #define GYRO_INT_EXTI            PC4
 #define USE_MPU_DATA_READY_SIGNAL
 
-#define USE_GYRO
-#define USE_GYRO_MPU6000
-#define GYRO_MPU6000_ALIGN      CW270_DEG
-
-#define USE_ACC
-#define USE_ACC_MPU6000
-#define ACC_MPU6000_ALIGN       CW270_DEG
 
 // *************** I2C /Baro/Mag *********************
 #define USE_I2C
diff --git a/src/main/target/MATEKF411/target.h b/src/main/target/MATEKF411/target.h
index 2a9bcbe83d..7063d18c38 100755
--- a/src/main/target/MATEKF411/target.h
+++ b/src/main/target/MATEKF411/target.h
@@ -35,27 +35,20 @@
 #define SPI1_MISO_PIN           PA6
 #define SPI1_MOSI_PIN           PA7
 
-#define USE_GYRO_MPU6000
-#define USE_ACC_MPU6000
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW180_DEG
 #define MPU6000_CS_PIN          PA4
 #define MPU6000_SPI_BUS         BUS_SPI1
-#define GYRO_MPU6000_ALIGN      CW180_DEG
-#define ACC_MPU6000_ALIGN       CW180_DEG
 
-#define USE_GYRO_MPU6500
-#define USE_ACC_MPU6500
+#define USE_IMU_MPU6500
+#define IMU_MPU6500_ALIGN       CW180_DEG
 #define MPU6500_CS_PIN          PA4
 #define MPU6500_SPI_BUS         BUS_SPI1
-#define GYRO_MPU6500_ALIGN      CW180_DEG
-#define ACC_MPU6500_ALIGN       CW180_DEG
 
 #define USE_EXTI
 #define GYRO_INT_EXTI            PA1
 #define USE_MPU_DATA_READY_SIGNAL
 
-#define USE_GYRO
-#define USE_ACC
-
 // *************** SPI2 OSD *****************************
 #define USE_SPI_DEVICE_2
 #define SPI2_SCK_PIN            PB13
diff --git a/src/main/target/MATEKF411SE/target.h b/src/main/target/MATEKF411SE/target.h
index 9a0a3c5ef1..cbffcd1a2c 100755
--- a/src/main/target/MATEKF411SE/target.h
+++ b/src/main/target/MATEKF411SE/target.h
@@ -34,6 +34,8 @@
 #define SPI1_MISO_PIN           PA6
 #define SPI1_MOSI_PIN           PA7
 
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW180_DEG
 #define MPU6000_CS_PIN          PA4
 #define MPU6000_SPI_BUS         BUS_SPI1
 
@@ -41,13 +43,6 @@
 #define GYRO_INT_EXTI            PA14
 #define USE_MPU_DATA_READY_SIGNAL
 
-#define USE_GYRO
-#define USE_GYRO_MPU6000
-#define GYRO_MPU6000_ALIGN      CW180_DEG
-
-#define USE_ACC
-#define USE_ACC_MPU6000
-#define ACC_MPU6000_ALIGN       CW180_DEG
 
 // *************** SPI2 OSD *****************************
 #define USE_SPI_DEVICE_2
diff --git a/src/main/target/MATEKF722/target.h b/src/main/target/MATEKF722/target.h
index 51ef9cdf90..62227998e8 100755
--- a/src/main/target/MATEKF722/target.h
+++ b/src/main/target/MATEKF722/target.h
@@ -35,6 +35,8 @@
 #define SPI1_MISO_PIN           PA6
 #define SPI1_MOSI_PIN           PA7
 
+#define USE_IMU_MPU6500
+#define IMU_MPU6500_ALIGN       CW180_DEG
 #define MPU6500_CS_PIN          PC2
 #define MPU6500_SPI_BUS         BUS_SPI1
 
@@ -42,13 +44,6 @@
 #define GYRO_INT_EXTI            PC3
 #define USE_MPU_DATA_READY_SIGNAL
 
-#define USE_GYRO
-#define USE_GYRO_MPU6500
-#define GYRO_MPU6500_ALIGN      CW180_DEG
-
-#define USE_ACC
-#define USE_ACC_MPU6500
-#define ACC_MPU6500_ALIGN       CW180_DEG
 
 // *************** I2C/Baro/Mag *********************
 #define USE_I2C
diff --git a/src/main/target/MATEKF722PX/target.h b/src/main/target/MATEKF722PX/target.h
index 9c0be7255b..cc7d33cb42 100755
--- a/src/main/target/MATEKF722PX/target.h
+++ b/src/main/target/MATEKF722PX/target.h
@@ -34,19 +34,15 @@
 #define SPI1_MISO_PIN           PA6
 #define SPI1_MOSI_PIN           PA7
 
-#define USE_ACC
-#define USE_GYRO
 #define USE_EXTI
 #define USE_MPU_DATA_READY_SIGNAL
 
-#define USE_ACC_MPU6000
-#define USE_GYRO_MPU6000
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW180_DEG_FLIP
 #define MPU6000_CS_PIN          PB2
 #define MPU6000_SPI_BUS         BUS_SPI1
 #define MPU6000_EXTI_PIN        PC4
 
-#define GYRO_MPU6000_ALIGN      CW180_DEG_FLIP
-#define ACC_MPU6000_ALIGN       CW180_DEG_FLIP
 
 // *************** I2C /Baro/Mag *********************
 #define USE_I2C
diff --git a/src/main/target/MATEKF722SE/target.c b/src/main/target/MATEKF722SE/target.c
index d792cec4bf..2c4170a1ce 100644
--- a/src/main/target/MATEKF722SE/target.c
+++ b/src/main/target/MATEKF722SE/target.c
@@ -24,9 +24,10 @@
 #include "drivers/pwm_mapping.h"
 #include "drivers/timer.h"
 #include "drivers/pinio.h"
+#include "drivers/sensor.h"
 
-BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000,     DEVHW_MPU6000,      MPU6000_SPI_BUS,    MPU6000_CS_PIN,     MPU6000_EXTI_PIN,       0,  DEVFLAGS_NONE);
-BUSDEV_REGISTER_SPI_TAG(busdev_mpu6500,     DEVHW_MPU6500,      MPU6500_SPI_BUS,    MPU6500_CS_PIN,     MPU6500_EXTI_PIN,       1,  DEVFLAGS_NONE);
+BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000,     DEVHW_MPU6000,      MPU6000_SPI_BUS,    MPU6000_CS_PIN,     MPU6000_EXTI_PIN,       0,  DEVFLAGS_NONE,  IMU_MPU6000_ALIGN);
+BUSDEV_REGISTER_SPI_TAG(busdev_mpu6500,     DEVHW_MPU6500,      MPU6500_SPI_BUS,    MPU6500_CS_PIN,     MPU6500_EXTI_PIN,       1,  DEVFLAGS_NONE,  IMU_MPU6500_ALIGN);
 
 const timerHardware_t timerHardware[] = {
     DEF_TIM(TIM3, CH1, PB4,   TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0),   // S1   UP1-2   D(1, 4, 5)
diff --git a/src/main/target/MATEKF722SE/target.h b/src/main/target/MATEKF722SE/target.h
index 28169a3ab1..56dcdcc886 100644
--- a/src/main/target/MATEKF722SE/target.h
+++ b/src/main/target/MATEKF722SE/target.h
@@ -41,27 +41,21 @@
 #define SPI1_MISO_PIN           PA6
 #define SPI1_MOSI_PIN           PA7
 
-#define USE_ACC
-#define USE_GYRO
 #define USE_DUAL_GYRO
 
-#define USE_ACC_MPU6000
-#define USE_GYRO_MPU6000
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW180_DEG_FLIP
 #define MPU6000_CS_PIN          PB2
 #define MPU6000_SPI_BUS         BUS_SPI1
 #define MPU6000_EXTI_PIN        PC4
 
-#define USE_ACC_MPU6500
-#define USE_GYRO_MPU6500
+#define USE_IMU_MPU6500
+#define IMU_MPU6500_ALIGN       CW90_DEG
 #define MPU6500_CS_PIN          PC15
 #define MPU6500_SPI_BUS         BUS_SPI1
 #define MPU6500_EXTI_PIN        PC3
 
-#define GYRO_MPU6000_ALIGN      CW180_DEG_FLIP
-#define ACC_MPU6000_ALIGN       CW180_DEG_FLIP
 
-#define GYRO_MPU6500_ALIGN      CW90_DEG
-#define ACC_MPU6500_ALIGN       CW90_DEG
 
 #define USE_EXTI
 #define USE_MPU_DATA_READY_SIGNAL
diff --git a/src/main/target/MATEKF765/target.c b/src/main/target/MATEKF765/target.c
index 72455ac600..af157cad89 100644
--- a/src/main/target/MATEKF765/target.c
+++ b/src/main/target/MATEKF765/target.c
@@ -24,9 +24,10 @@
 #include "drivers/pwm_mapping.h"
 #include "drivers/timer.h"
 #include "drivers/pinio.h"
+#include "drivers/sensor.h"
 
-BUSDEV_REGISTER_SPI_TAG(busdev_imu0,    DEVHW_MPU6000,      MPU6000_SPI_BUS,    MPU6000_CS_PIN,     MPU6000_EXTI_PIN,       0,  DEVFLAGS_NONE);
-BUSDEV_REGISTER_SPI_TAG(busdev_imu1,    DEVHW_MPU6500,      MPU6500_SPI_BUS,    MPU6500_CS_PIN,     MPU6500_EXTI_PIN,       1,  DEVFLAGS_NONE);
+BUSDEV_REGISTER_SPI_TAG(busdev_imu0,    DEVHW_MPU6000,      MPU6000_SPI_BUS,    MPU6000_CS_PIN,     MPU6000_EXTI_PIN,       0,  DEVFLAGS_NONE,  IMU_MPU6000_ALIGN);
+BUSDEV_REGISTER_SPI_TAG(busdev_imu1,    DEVHW_MPU6500,      MPU6500_SPI_BUS,    MPU6500_CS_PIN,     MPU6500_EXTI_PIN,       1,  DEVFLAGS_NONE,  IMU_MPU6500_ALIGN);
 
 const timerHardware_t timerHardware[] = {
     DEF_TIM(TIM2,  CH1, PA0,  TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0),   // S1  UP(1,7), D(1,5,3)
diff --git a/src/main/target/MATEKF765/target.h b/src/main/target/MATEKF765/target.h
index 5329c419ae..fcddab2693 100644
--- a/src/main/target/MATEKF765/target.h
+++ b/src/main/target/MATEKF765/target.h
@@ -42,27 +42,20 @@
 #define SPI3_MISO_PIN           PB4
 #define SPI3_MOSI_PIN           PB5
 
-#define USE_ACC
-#define USE_GYRO
 #define USE_DUAL_GYRO
 
-#define USE_ACC_MPU6000
-#define USE_GYRO_MPU6000
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW90_DEG_FLIP
 #define MPU6000_SPI_BUS         BUS_SPI1
 #define MPU6000_CS_PIN          PC4
 #define MPU6000_EXTI_PIN        PB2
 
-#define USE_ACC_MPU6500
-#define USE_GYRO_MPU6500
+#define USE_IMU_MPU6500
+#define IMU_MPU6500_ALIGN       CW270_DEG_FLIP
 #define MPU6500_SPI_BUS         BUS_SPI3
 #define MPU6500_CS_PIN          PD7
 #define MPU6500_EXTI_PIN        PD4
 
-#define GYRO_MPU6000_ALIGN      CW90_DEG_FLIP
-#define ACC_MPU6000_ALIGN       CW90_DEG_FLIP
-
-#define GYRO_MPU6500_ALIGN      CW270_DEG_FLIP
-#define ACC_MPU6500_ALIGN       CW270_DEG_FLIP
 
 #define USE_EXTI
 #define USE_MPU_DATA_READY_SIGNAL
diff --git a/src/main/target/MOTOLAB/target.h b/src/main/target/MOTOLAB/target.h
index 278bd9654d..386eadf120 100644
--- a/src/main/target/MOTOLAB/target.h
+++ b/src/main/target/MOTOLAB/target.h
@@ -30,18 +30,11 @@
 #define GYRO_INT_EXTI            PA15
 #define USE_MPU_DATA_READY_SIGNAL
 
-#define USE_GYRO
-#define USE_GYRO_MPU6050
-#define GYRO_MPU6050_ALIGN      CW180_DEG
+#define USE_IMU_MPU6050
+#define IMU_MPU6050_ALIGN       CW180_DEG
 
-#define USE_ACC
-#define USE_ACC_MPU6050
-#define ACC_MPU6050_ALIGN       CW180_DEG
-
-#define USE_GYRO_MPU6000
-#define GYRO_MPU6000_ALIGN      CW180_DEG
-#define USE_ACC_MPU6000
-#define ACC_MPU6000_ALIGN       CW180_DEG
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW180_DEG
 
 #define MPU6050_I2C_BUS         BUS_I2C2
 #define MPU6000_CS_PIN          PB12
diff --git a/src/main/target/NOX/target.h b/src/main/target/NOX/target.h
index 7951a76183..9a40ea5050 100755
--- a/src/main/target/NOX/target.h
+++ b/src/main/target/NOX/target.h
@@ -43,6 +43,8 @@
 
 
 // *************** SPI Gyro & ACC **********************
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW0_DEG
 #define MPU6000_CS_PIN          PB12
 #define MPU6000_SPI_BUS         BUS_SPI2
 
@@ -50,11 +52,6 @@
 #define GYRO_INT_EXTI           PA8
 #define USE_MPU_DATA_READY_SIGNAL
 
-#define USE_GYRO
-#define USE_GYRO_MPU6000
-
-#define USE_ACC
-#define USE_ACC_MPU6000
 
 // *************** SPI BARO *****************************
 #define USE_BARO
diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h
index 1127030379..63f7c37209 100644
--- a/src/main/target/OMNIBUS/target.h
+++ b/src/main/target/OMNIBUS/target.h
@@ -19,8 +19,6 @@
 
 #define TARGET_BOARD_IDENTIFIER "OMNI" // https://en.wikipedia.org/wiki/Omnibus
 
-#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE
-
 #define BEEPER                  PC15
 #define BEEPER_INVERTED
 
@@ -34,17 +32,13 @@
 
 #define USE_EXTI
 
-#define USE_GYRO
-#define USE_GYRO_MPU6000
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW90_DEG
 #define MPU6000_SPI_BUS         BUS_SPI1
 #define MPU6000_CS_PIN          PA4
 #define GYRO_INT_EXTI            PC13
 #define USE_MPU_DATA_READY_SIGNAL
-#define GYRO_MPU6000_ALIGN      CW90_DEG
 
-#define USE_ACC
-#define USE_ACC_MPU6000
-#define ACC_MPU6000_ALIGN       CW90_DEG
 
 #define USE_BARO
 #define USE_BARO_BMP280
diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h
index a4fcdf0736..0c3666a9d3 100644
--- a/src/main/target/OMNIBUSF4/target.h
+++ b/src/main/target/OMNIBUSF4/target.h
@@ -69,36 +69,23 @@
 #define GYRO_INT_EXTI            PC4
 #define USE_MPU_DATA_READY_SIGNAL
 
-#define USE_GYRO
-#define USE_ACC
-
 #define MPU6000_CS_PIN          PA4
 #define MPU6000_SPI_BUS         BUS_SPI1
 
 #if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3)
-  #define USE_GYRO_MPU6000
-  #define GYRO_MPU6000_ALIGN      CW270_DEG
-
-  #define USE_ACC_MPU6000
-  #define ACC_MPU6000_ALIGN       CW270_DEG
+  #define USE_IMU_MPU6000
+  #define IMU_MPU6000_ALIGN       CW270_DEG
 #else
-  #define USE_GYRO_MPU6000
-  #define GYRO_MPU6000_ALIGN      CW180_DEG
-
-  #define USE_ACC_MPU6000
-  #define ACC_MPU6000_ALIGN       CW180_DEG
+  #define USE_IMU_MPU6000
+  #define IMU_MPU6000_ALIGN       CW180_DEG
 #endif
 
 // Support for OMNIBUS F4 PRO CORNER - it has ICM20608 instead of MPU6000
 #if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3)
   #define MPU6500_CS_PIN          MPU6000_CS_PIN
   #define MPU6500_SPI_BUS         MPU6000_SPI_BUS
-
-  #define USE_GYRO_MPU6500
-  #define GYRO_MPU6500_ALIGN      GYRO_MPU6000_ALIGN
-
-  #define USE_ACC_MPU6500
-  #define ACC_MPU6500_ALIGN       ACC_MPU6000_ALIGN
+  #define USE_IMU_MPU6500
+  #define IMU_MPU6500_ALIGN       IMU_MPU6000_ALIGN
 #endif
 
 #define USE_MAG
diff --git a/src/main/target/OMNIBUSF7/target.c b/src/main/target/OMNIBUSF7/target.c
index 3132b03ac6..ffa46dad9c 100644
--- a/src/main/target/OMNIBUSF7/target.c
+++ b/src/main/target/OMNIBUSF7/target.c
@@ -20,18 +20,19 @@
 #include "drivers/pwm_mapping.h"
 #include "drivers/timer.h"
 #include "drivers/bus.h"
+#include "drivers/sensor.h"
 
 /* GYRO */
-BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000,     DEVHW_MPU6000,      MPU6000_SPI_BUS,    MPU6000_CS_PIN,     MPU6000_EXTI_PIN,       0,  DEVFLAGS_NONE);
-BUSDEV_REGISTER_SPI_TAG(busdev_mpu6500,     DEVHW_MPU6500,      MPU6500_SPI_BUS,    MPU6500_CS_PIN,     MPU6500_EXTI_PIN,       1,  DEVFLAGS_NONE);
+BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000, DEVHW_MPU6000,  MPU6000_SPI_BUS,    MPU6000_CS_PIN, MPU6000_EXTI_PIN,   0,  DEVFLAGS_NONE,  IMU_MPU6000_ALIGN);
+BUSDEV_REGISTER_SPI_TAG(busdev_mpu6500, DEVHW_MPU6500,  MPU6500_SPI_BUS,    MPU6500_CS_PIN, MPU6500_EXTI_PIN,   1,  DEVFLAGS_NONE,  IMU_MPU6500_ALIGN);
 
-BUSDEV_REGISTER_SPI(    busdev_bmp280,      DEVHW_BMP280,       BMP280_SPI_BUS,     BMP280_CS_PIN,      NONE,                       DEVFLAGS_NONE);
+BUSDEV_REGISTER_SPI(    busdev_bmp280,  DEVHW_BMP280,   BMP280_SPI_BUS,     BMP280_CS_PIN,  NONE,                   DEVFLAGS_NONE,  0);
 
-BUSDEV_REGISTER_I2C(    busdev_hmc5883,     DEVHW_HMC5883,      MAG_I2C_BUS,        0x1E,               NONE,                       DEVFLAGS_NONE);
-BUSDEV_REGISTER_I2C(    busdev_qmc5883,     DEVHW_QMC5883,      MAG_I2C_BUS,        0x0D,               NONE,                       DEVFLAGS_NONE);
-BUSDEV_REGISTER_I2C(    busdev_mag3110,     DEVHW_MAG3110,      MAG_I2C_BUS,        0x0E,               NONE,                       DEVFLAGS_NONE);
+BUSDEV_REGISTER_I2C(    busdev_hmc5883, DEVHW_HMC5883,  MAG_I2C_BUS,        0x1E,           NONE,                   DEVFLAGS_NONE,  0);
+BUSDEV_REGISTER_I2C(    busdev_qmc5883, DEVHW_QMC5883,  MAG_I2C_BUS,        0x0D,           NONE,                   DEVFLAGS_NONE,  0);
+BUSDEV_REGISTER_I2C(    busdev_mag3110, DEVHW_MAG3110,  MAG_I2C_BUS,        0x0E,           NONE,                   DEVFLAGS_NONE,  0);
 
-BUSDEV_REGISTER_SPI(    busdev_max7456,     DEVHW_MAX7456,      MAX7456_SPI_BUS,    MAX7456_CS_PIN,     NONE,                       DEVFLAGS_USE_RAW_REGISTERS);
+BUSDEV_REGISTER_SPI(    busdev_max7456, DEVHW_MAX7456,  MAX7456_SPI_BUS,    MAX7456_CS_PIN, NONE,                   DEVFLAGS_USE_RAW_REGISTERS,  0);
 
 const timerHardware_t timerHardware[] = {
 
diff --git a/src/main/target/OMNIBUSF7/target.h b/src/main/target/OMNIBUSF7/target.h
index 9945f82f8a..d013a36a6c 100644
--- a/src/main/target/OMNIBUSF7/target.h
+++ b/src/main/target/OMNIBUSF7/target.h
@@ -31,47 +31,36 @@
 #define BEEPER   PD15
 #define BEEPER_INVERTED
 
-#define USE_ACC
-#define USE_GYRO
 #define USE_DUAL_GYRO
 
 // ICM-20608-G
-#define USE_ACC_MPU6500
-#define USE_GYRO_MPU6500
 
 // MPU6000
-#define USE_ACC_MPU6000
-#define USE_GYRO_MPU6000
 
 #ifdef OMNIBUSF7V2
+#   define USE_IMU_MPU6000
+#   define IMU_MPU6000_ALIGN        CW0_DEG
 #   define MPU6000_CS_PIN           SPI1_NSS_PIN
 #   define MPU6000_SPI_BUS          BUS_SPI1
 #   define MPU6000_EXTI_PIN         PE8
 
+#   define USE_IMU_MPU6500
+#   define IMU_MPU6500_ALIGN        CW90_DEG
 #   define MPU6500_CS_PIN           SPI3_NSS_PIN
 #   define MPU6500_SPI_BUS          BUS_SPI3
 #   define MPU6500_EXTI_PIN         PD0
-// #   define GYRO_1_CS_PIN           MPU6500_CS_PIN
-// #   define GYRO_0_CS_PIN           MPU6000_CS_PIN
-// #   define GYRO_1_INT_EXTI         PD0
-// #   define GYRO_0_INT_EXTI         PE8
-#   define GYRO_MPU6500_ALIGN       CW90_DEG
-#   define ACC_MPU6500_ALIGN        CW90_DEG
 #else
+#   define USE_IMU_MPU6000
+#   define IMU_MPU6000_ALIGN        CW0_DEG
 #   define MPU6000_CS_PIN           SPI3_NSS_PIN
 #   define MPU6000_SPI_BUS          BUS_SPI3
 #   define MPU6000_EXTI_PIN         PD0
 
+#   define USE_IMU_MPU6500
+#   define IMU_MPU6500_ALIGN        CW0_DEG
 #   define MPU6500_CS_PIN           SPI1_NSS_PIN
 #   define MPU6500_SPI_BUS          BUS_SPI1
 #   define MPU6500_EXTI_PIN         PE8
-
-// #   define GYRO_0_CS_PIN           MPU6000_CS_PIN
-// #   define GYRO_1_CS_PIN           MPU6500_CS_PIN
-// #   define GYRO_0_INT_EXTI         PD0
-// #   define GYRO_1_INT_EXTI         PE8
-#   define GYRO_MPU6000_ALIGN      CW0_DEG
-#   define ACC_MPU6000_ALIGN       CW0_DEG
 #endif
 
 #define USE_EXTI
diff --git a/src/main/target/OMNIBUSF7NXT/target.c b/src/main/target/OMNIBUSF7NXT/target.c
index e6439d4ac1..0222914f0e 100644
--- a/src/main/target/OMNIBUSF7NXT/target.c
+++ b/src/main/target/OMNIBUSF7NXT/target.c
@@ -29,10 +29,11 @@
 #include "drivers/pwm_mapping.h"
 #include "drivers/timer.h"
 #include "drivers/bus.h"
+#include "drivers/sensor.h"
 
 // Board hardware definitions
-BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000,     DEVHW_MPU6000,      MPU6000_SPI_BUS,    MPU6000_CS_PIN,     NONE,       0,  DEVFLAGS_NONE);
-BUSDEV_REGISTER_SPI_TAG(busdev_mpu6500,     DEVHW_MPU6500,      MPU6500_SPI_BUS,    MPU6500_CS_PIN,     NONE,       1,  DEVFLAGS_NONE);
+BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000,     DEVHW_MPU6000,      MPU6000_SPI_BUS,    MPU6000_CS_PIN,     NONE,       0,  DEVFLAGS_NONE,  IMU_MPU6000_ALIGN);
+BUSDEV_REGISTER_SPI_TAG(busdev_mpu6500,     DEVHW_MPU6500,      MPU6500_SPI_BUS,    MPU6500_CS_PIN,     NONE,       1,  DEVFLAGS_NONE,  IMU_MPU6500_ALIGN);
 
 const timerHardware_t timerHardware[] = {
     DEF_TIM(TIM4, CH2, PB7, TIM_USE_PPM,                            0, 0), // PPM / UART1_RX
diff --git a/src/main/target/OMNIBUSF7NXT/target.h b/src/main/target/OMNIBUSF7NXT/target.h
index 21257e9e86..b14e0c219e 100644
--- a/src/main/target/OMNIBUSF7NXT/target.h
+++ b/src/main/target/OMNIBUSF7NXT/target.h
@@ -37,24 +37,18 @@
 #define BEEPER                  PC13
 #define BEEPER_INVERTED
 
-#define USE_ACC
-#define USE_GYRO
 #define USE_DUAL_GYRO
 
 // OMNIBUS F7 NEXT has two IMUs - MPU6000 onboard and ICM20608 (MPU6500) in the vibration dampened box
-#define USE_GYRO_MPU6000
-#define USE_ACC_MPU6000
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW180_DEG
 #define MPU6000_CS_PIN          PB12
 #define MPU6000_SPI_BUS         BUS_SPI1
-#define GYRO_MPU6000_ALIGN      CW180_DEG
-#define ACC_MPU6000_ALIGN       CW180_DEG
 
-#define USE_GYRO_MPU6500
-#define USE_ACC_MPU6500
+#define USE_IMU_MPU6500
+#define IMU_MPU6500_ALIGN       CW90_DEG
 #define MPU6500_CS_PIN          PA8
 #define MPU6500_SPI_BUS         BUS_SPI1
-#define GYRO_MPU6500_ALIGN      CW90_DEG
-#define ACC_MPU6500_ALIGN       CW90_DEG
 
 #define USE_MAG
 #define MAG_I2C_BUS             BUS_I2C1
diff --git a/src/main/target/PIKOBLX/target.h b/src/main/target/PIKOBLX/target.h
index 2d167eaeac..1d7f31c531 100644
--- a/src/main/target/PIKOBLX/target.h
+++ b/src/main/target/PIKOBLX/target.h
@@ -19,7 +19,7 @@
 
 #define TARGET_BOARD_IDENTIFIER "PIKO" // Furious FPV Piko BLX
 
-#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT
+#define CONFIG_FASTLOOP_PREFERRED_ACC IMU_DEFAULT
 
 #define LED0                    PB9
 #define LED1                    PB5
@@ -32,14 +32,8 @@
 #define GYRO_INT_EXTI            PA15
 #define USE_MPU_DATA_READY_SIGNAL
 
-#define USE_GYRO
-#define USE_GYRO_MPU6000
-#define GYRO_MPU6000_ALIGN      CW180_DEG
-
-#define USE_ACC
-#define USE_ACC_MPU6000
-#define ACC_MPU6000_ALIGN       CW180_DEG
-
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW180_DEG
 #define MPU6000_CS_PIN          PB12
 #define MPU6000_SPI_BUS         BUS_SPI2
 
diff --git a/src/main/target/PIXRACER/target.c b/src/main/target/PIXRACER/target.c
index fcd15cde95..b881223337 100755
--- a/src/main/target/PIXRACER/target.c
+++ b/src/main/target/PIXRACER/target.c
@@ -21,19 +21,20 @@
 #include "drivers/io.h"
 #include "drivers/pwm_mapping.h"
 #include "drivers/timer.h"
+#include "drivers/sensor.h"
 #include "drivers/bus.h"
 
-BUSDEV_REGISTER_SPI_TAG(busdev_mpu9250,     DEVHW_MPU9250,      MPU9250_SPI_BUS,    MPU9250_CS_PIN,     MPU9250_EXTI_PIN,       0,  DEVFLAGS_NONE);
-BUSDEV_REGISTER_SPI_TAG(busdev_icm20608,    DEVHW_MPU6500,      ICM20608_SPI_BUS,   ICM20608_CS_PIN,    ICM20608_EXTI_PIN,      1,  DEVFLAGS_NONE);
+BUSDEV_REGISTER_SPI_TAG(busdev_mpu9250,     DEVHW_MPU9250,      MPU9250_SPI_BUS,    MPU9250_CS_PIN,     MPU9250_EXTI_PIN,       0,  DEVFLAGS_NONE,  IMU_MPU9250_ALIGN);
+BUSDEV_REGISTER_SPI_TAG(busdev_icm20608,    DEVHW_MPU6500,      ICM20608_SPI_BUS,   ICM20608_CS_PIN,    ICM20608_EXTI_PIN,      1,  DEVFLAGS_NONE,  IMU_MPU6500_ALIGN);
 
-BUSDEV_REGISTER_SPI_TAG(busdev_ms5611,      DEVHW_MS5611,       MS5611_SPI_BUS,     MS5611_CS_PIN,      NONE,                   0,   DEVFLAGS_USE_RAW_REGISTERS);
+BUSDEV_REGISTER_SPI_TAG(busdev_ms5611,      DEVHW_MS5611,       MS5611_SPI_BUS,     MS5611_CS_PIN,      NONE,                   0,   DEVFLAGS_USE_RAW_REGISTERS,    0);
 
-BUSDEV_REGISTER_I2C_TAG(busdev_hmc5883,     DEVHW_HMC5883,      MAG_I2C_BUS,        0x1E,               NONE,                   0,  DEVFLAGS_NONE);
-BUSDEV_REGISTER_I2C_TAG(busdev_qmc5883,     DEVHW_QMC5883,      MAG_I2C_BUS,        0x0D,               NONE,                   0,  DEVFLAGS_NONE);
-BUSDEV_REGISTER_I2C_TAG(busdev_mag3110,     DEVHW_MAG3110,      MAG_I2C_BUS,        0x0E,               NONE,                   0,  DEVFLAGS_NONE);
+BUSDEV_REGISTER_I2C_TAG(busdev_hmc5883,     DEVHW_HMC5883,      MAG_I2C_BUS,        0x1E,               NONE,                   0,  DEVFLAGS_NONE,    0);
+BUSDEV_REGISTER_I2C_TAG(busdev_qmc5883,     DEVHW_QMC5883,      MAG_I2C_BUS,        0x0D,               NONE,                   0,  DEVFLAGS_NONE,    0);
+BUSDEV_REGISTER_I2C_TAG(busdev_mag3110,     DEVHW_MAG3110,      MAG_I2C_BUS,        0x0E,               NONE,                   0,  DEVFLAGS_NONE,    0);
 
 // PixRacer has built-in HMC5983 compass on the same SPI bus as MPU9250
-BUSDEV_REGISTER_SPI_TAG(busdev_hmc5983_spi, DEVHW_HMC5883,      MPU9250_SPI_BUS,    PE15,               NONE,                   1,  DEVFLAGS_NONE);
+BUSDEV_REGISTER_SPI_TAG(busdev_hmc5983_spi, DEVHW_HMC5883,      MPU9250_SPI_BUS,    PE15,               NONE,                   1,  DEVFLAGS_NONE,    0);
 
 const timerHardware_t timerHardware[] = {
     DEF_TIM(TIM3, CH3, PB0,  TIM_USE_PPM,         0, 0 ),   // PPM shared uart6 pc7
diff --git a/src/main/target/PIXRACER/target.h b/src/main/target/PIXRACER/target.h
index a695ebcc85..5d6f71b1c8 100755
--- a/src/main/target/PIXRACER/target.h
+++ b/src/main/target/PIXRACER/target.h
@@ -39,15 +39,11 @@
 
 #define INVERTER_PIN_UART       PC13
 
-#define USE_GYRO_MPU6500
-#define USE_ACC_MPU6500
-#define USE_GYRO_MPU9250
-#define USE_ACC_MPU9250
+#define USE_IMU_MPU6500
+#define IMU_MPU6500_ALIGN       CW180_DEG_FLIP
 
-#define GYRO_MPU6500_ALIGN      CW180_DEG_FLIP
-#define ACC_MPU6500_ALIGN       CW180_DEG_FLIP
-#define GYRO_MPU9250_ALIGN      CW180_DEG_FLIP
-#define ACC_MPU9250_ALIGN       CW180_DEG_FLIP
+#define USE_IMU_MPU9250
+#define IMU_MPU9250_ALIGN       CW180_DEG_FLIP
 #define MAG_MPU9250_ALIGN       CW90_DEG
 
 #define USE_DUAL_GYRO
@@ -62,9 +58,6 @@
 #define MPU9250_EXTI_PIN        PD15
 #define MPU9250_SPI_BUS         BUS_SPI1
 
-#define USE_ACC
-#define USE_GYRO
-
 #define USE_MAG
 #define MAG_I2C_BUS             BUS_I2C1
 #define USE_MAG_MPU9250
diff --git a/src/main/target/RADIX/target.h b/src/main/target/RADIX/target.h
index fbf2339a88..2f778992b5 100644
--- a/src/main/target/RADIX/target.h
+++ b/src/main/target/RADIX/target.h
@@ -31,13 +31,8 @@
 #define BMI160_CS_PIN           PB4
 #define GYRO_EXTI_PIN           PC13
 
-#define USE_GYRO
-#define USE_GYRO_BMI160
-#define GYRO_BMI160_ALIGN       CW0_DEG
-
-#define USE_ACC
-#define USE_ACC_BMI160
-#define ACC_BMI160_ALIGN        CW0_DEG
+#define USE_IMU_BMI160
+#define IMU_BMI160_ALIGN        CW0_DEG
 
 // #define USE_MAG
 // #define MAG_I2C_BUS             BUS_I2C1
diff --git a/src/main/target/RCEXPLORERF3/target.h b/src/main/target/RCEXPLORERF3/target.h
index 53cc0371dd..738ed44638 100644
--- a/src/main/target/RCEXPLORERF3/target.h
+++ b/src/main/target/RCEXPLORERF3/target.h
@@ -29,16 +29,11 @@
 #define GYRO_INT_EXTI            PA15
 #define USE_MPU_DATA_READY_SIGNAL
 
-#define USE_GYRO
-#define USE_GYRO_MPU6000
-#define GYRO_MPU6000_ALIGN      CW180_DEG
-
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW180_DEG
 #define MPU6000_CS_PIN          PB12
 #define MPU6000_SPI_BUS         BUS_SPI2
 
-#define USE_ACC
-#define USE_ACC_MPU6000
-#define ACC_MPU6000_ALIGN       CW180_DEG
 
 #define USE_BARO
 #define BARO_I2C_BUS            BUS_I2C2
diff --git a/src/main/target/REVO/target.c b/src/main/target/REVO/target.c
index 3f7431af59..eb02a3eb59 100644
--- a/src/main/target/REVO/target.c
+++ b/src/main/target/REVO/target.c
@@ -23,13 +23,13 @@
 #include "drivers/timer.h"
 #include "drivers/timer_def.h"
 #include "drivers/pwm_mapping.h"
+#include "drivers/sensor.h"
 
-/* GYRO */
-BUSDEV_REGISTER_I2C_TAG(busdev_hmc5883_int, DEVHW_HMC5883,      MAG_I2C_BUS_INT,    0x1E,               NONE,                   0,  DEVFLAGS_NONE);
-
-BUSDEV_REGISTER_I2C_TAG(busdev_hmc5883,     DEVHW_HMC5883,      MAG_I2C_BUS_EXT,    0x1E,               NONE,                   1,  DEVFLAGS_NONE);
-BUSDEV_REGISTER_I2C_TAG(busdev_qmc5883,     DEVHW_QMC5883,      MAG_I2C_BUS_EXT,    0x0D,               NONE,                   1,  DEVFLAGS_NONE);
-BUSDEV_REGISTER_I2C_TAG(busdev_mag3110,     DEVHW_MAG3110,      MAG_I2C_BUS_EXT,    0x0E,               NONE,                   1,  DEVFLAGS_NONE);
+/* COMPAS */
+BUSDEV_REGISTER_I2C_TAG(busdev_hmc5883_int, DEVHW_HMC5883,      MAG_I2C_BUS_INT,    0x1E,               NONE,                   0,  DEVFLAGS_NONE,  0);
+BUSDEV_REGISTER_I2C_TAG(busdev_hmc5883,     DEVHW_HMC5883,      MAG_I2C_BUS_EXT,    0x1E,               NONE,                   1,  DEVFLAGS_NONE,  0);
+BUSDEV_REGISTER_I2C_TAG(busdev_qmc5883,     DEVHW_QMC5883,      MAG_I2C_BUS_EXT,    0x0D,               NONE,                   1,  DEVFLAGS_NONE,  0);
+BUSDEV_REGISTER_I2C_TAG(busdev_mag3110,     DEVHW_MAG3110,      MAG_I2C_BUS_EXT,    0x0E,               NONE,                   1,  DEVFLAGS_NONE,  0);
 
 
 /* TIMERS */
diff --git a/src/main/target/REVO/target.h b/src/main/target/REVO/target.h
index 3c1c952e54..be2c511c19 100644
--- a/src/main/target/REVO/target.h
+++ b/src/main/target/REVO/target.h
@@ -20,9 +20,6 @@
 #define TARGET_BOARD_IDENTIFIER "REVO"
 
 #define USBD_PRODUCT_STRING     "Revolution"
-#ifdef OPBL
-#define USBD_SERIALNUMBER_STRING "0x8020000"
-#endif
 
 // Use target-specific MAG hardware descriptors (don't use common_hardware.h)
 #define USE_TARGET_MAG_HARDWARE_DESCRIPTORS
@@ -41,17 +38,11 @@
 #define GYRO_INT_EXTI           PC4
 #define USE_MPU_DATA_READY_SIGNAL
 
+#define USE_IMU_MPU6000
+#define IMU_MPU6000_ALIGN       CW270_DEG
 #define MPU6000_CS_PIN          PA4
 #define MPU6000_SPI_BUS         BUS_SPI1
 
-#define USE_GYRO
-#define USE_GYRO_MPU6000
-#define ACC_MPU6000_ALIGN       CW270_DEG
-
-#define USE_ACC
-#define USE_ACC_MPU6000
-#define GYRO_MPU6000_ALIGN      CW270_DEG
-
 #define USE_MAG
 #define USE_DUAL_MAG
 #define MAG_I2C_BUS_EXT         BUS_I2C2
diff --git a/src/main/target/RMDO/target.h b/src/main/target/RMDO/target.h
index 1e78f6df93..fe467f8296 100644
--- a/src/main/target/RMDO/target.h
+++ b/src/main/target/RMDO/target.h
@@ -28,15 +28,10 @@
 #define USE_MPU_DATA_READY_SIGNAL
 #define ENSURE_MPU_DATA_READY_IS_LOW
 
-#define USE_GYRO
-#define USE_ACC
-
-#define USE_GYRO_MPU6050
-#define USE_ACC_MPU6050
+#define USE_IMU_MPU6050
+#define IMU_MPU6050_ALIGN       CW270_DEG
 #define MPU6050_I2C_BUS         BUS_I2C1
 
-#define GYRO_MPU6050_ALIGN      CW270_DEG
-#define ACC_MPU6050_ALIGN       CW270_DEG
 
 #define USE_BARO
 #define BARO_I2C_BUS            BUS_I2C1
diff --git a/src/main/target/SPARKY/target.h b/src/main/target/SPARKY/target.h
index 6b028cb0d6..26baef81c2 100644
--- a/src/main/target/SPARKY/target.h
+++ b/src/main/target/SPARKY/target.h
@@ -31,14 +31,8 @@
 #define USE_MPU_DATA_READY_SIGNAL
 
 // MPU 9150 INT connected to PA15, pulled up to VCC by 10K Resistor, contains MPU6050 and AK8975 in single component.
-#define USE_GYRO
-#define USE_GYRO_MPU6050
-#define GYRO_MPU6050_ALIGN      CW270_DEG
-
-#define USE_ACC
-#define USE_ACC_MPU6050
-#define ACC_MPU6050_ALIGN       CW270_DEG
-
+#define USE_IMU_MPU6050
+#define IMU_MPU6050_ALIGN       CW270_DEG
 #define MPU6050_I2C_BUS         BUS_I2C2
 
 #define USE_BARO
diff --git a/src/main/target/SPARKY2/target.h b/src/main/target/SPARKY2/target.h
index adff656ca7..f716369b74 100644
--- a/src/main/target/SPARKY2/target.h
+++ b/src/main/target/SPARKY2/target.h
@@ -33,13 +33,8 @@
 #define USE_MPU_DATA_READY_SIGNAL
 #define ENSURE_MPU_DATA_READY_IS_LOW
 
-#define USE_GYRO
-#define USE_GYRO_MPU9250
-#define GYRO_MPU9250_ALIGN      CW270_DEG
-
-#define USE_ACC
-#define USE_ACC_MPU9250
-#define ACC_MPU9250_ALIGN       CW270_DEG
+#define USE_IMU_MPU9250
+#define IMU_MPU9250_ALIGN       CW270_DEG
 
 #define MPU9250_SPI_BUS         BUS_SPI1
 #define MPU9250_CS_PIN          PC4
diff --git a/src/main/target/SPEEDYBEEF4/target.h b/src/main/target/SPEEDYBEEF4/target.h
index 982eb704c2..865980439c 100644
--- a/src/main/target/SPEEDYBEEF4/target.h
+++ b/src/main/target/SPEEDYBEEF4/target.h
@@ -36,13 +36,10 @@
 #define SENSORS_SET (SENSOR_ACC|SENSOR_MAG|SENSOR_BARO)
 
 /*** MPU6000 ***/
-#define USE_GYRO_MPU6000					
-#define USE_ACC_MPU6000						
-
-#define MPU6000_CS_PIN          PB11		
-#define MPU6000_SPI_BUS         BUS_SPI1	
-#define GYRO_MPU6000_ALIGN      CW0_DEG
-#define ACC_MPU6000_ALIGN       CW0_DEG
+#define USE_IMU_MPU6000
+#define MPU6000_CS_PIN          PB11
+#define MPU6000_SPI_BUS         BUS_SPI1
+#define IMU_MPU6000_ALIGN       CW0_DEG
 
 /*** SPI/I2C bus ***/
 #define USE_SPI								
diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h
index 4717440c16..05727f5d42 100644
--- a/src/main/target/SPRACINGF3/target.h
+++ b/src/main/target/SPRACINGF3/target.h
@@ -30,14 +30,8 @@
 #define USE_MPU_DATA_READY_SIGNAL
 #define ENSURE_MPU_DATA_READY_IS_LOW
 
-#define USE_GYRO
-#define USE_GYRO_MPU6050
-#define GYRO_MPU6050_ALIGN      CW270_DEG
-
-#define USE_ACC
-#define USE_ACC_MPU6050
-#define ACC_MPU6050_ALIGN       CW270_DEG
-
+#define USE_IMU_MPU6050
+#define IMU_MPU6050_ALIGN       CW270_DEG
 #define MPU6050_I2C_BUS         BUS_I2C1
 
 #define USE_BARO
diff --git a/src/main/target/SPRACINGF3EVO/target.h b/src/main/target/SPRACINGF3EVO/target.h
index 7a4f57490a..c2b139a044 100755
--- a/src/main/target/SPRACINGF3EVO/target.h
+++ b/src/main/target/SPRACINGF3EVO/target.h
@@ -35,17 +35,10 @@
 #define MPU9250_CS_PIN          PB9
 #define MPU9250_SPI_BUS         BUS_SPI1
 
-#define USE_GYRO
-#define USE_GYRO_MPU6500
-#define GYRO_MPU6500_ALIGN      CW180_DEG
-#define USE_GYRO_MPU9250
-#define GYRO_MPU9250_ALIGN      CW180_DEG
-
-#define USE_ACC
-#define USE_ACC_MPU6500
-#define ACC_MPU6500_ALIGN       CW180_DEG
-#define USE_ACC_MPU9250
-#define ACC_MPU9250_ALIGN       CW180_DEG
+#define USE_IMU_MPU6500
+#define IMU_MPU6500_ALIGN       CW180_DEG
+#define USE_IMU_MPU9250
+#define IMU_MPU9250_ALIGN       CW180_DEG
 
 #define USE_BARO
 #define BARO_I2C_BUS            BUS_I2C1
diff --git a/src/main/target/SPRACINGF3MINI/target.h b/src/main/target/SPRACINGF3MINI/target.h
index 758bd30577..6a7ec647f1 100644
--- a/src/main/target/SPRACINGF3MINI/target.h
+++ b/src/main/target/SPRACINGF3MINI/target.h
@@ -33,17 +33,10 @@
 #define USE_MPU_DATA_READY_SIGNAL
 #define ENSURE_MPU_DATA_READY_IS_LOW
 
-#define USE_GYRO
-#define USE_GYRO_MPU6500
-#define GYRO_MPU6500_ALIGN      CW180_DEG
-#define USE_GYRO_MPU9250
-#define GYRO_MPU9250_ALIGN      CW180_DEG
-
-#define USE_ACC
-#define USE_ACC_MPU6500
-#define ACC_MPU6500_ALIGN       CW180_DEG
-#define USE_ACC_MPU9250
-#define ACC_MPU9250_ALIGN       CW180_DEG
+#define USE_IMU_MPU6500
+#define IMU_MPU6500_ALIGN       CW180_DEG
+#define USE_IMU_MPU9250
+#define IMU_MPU9250_ALIGN       CW180_DEG
 
 #define MPU6500_I2C_BUS         BUS_I2C1
 #define MPU9250_I2C_BUS         BUS_I2C1
diff --git a/src/main/target/SPRACINGF3NEO/target.h b/src/main/target/SPRACINGF3NEO/target.h
index e8ff5509e1..9bec2fda3c 100755
--- a/src/main/target/SPRACINGF3NEO/target.h
+++ b/src/main/target/SPRACINGF3NEO/target.h
@@ -30,18 +30,11 @@
 #define USE_MPU_DATA_READY_SIGNAL
 #define ENSURE_MPU_DATA_READY_IS_LOW
 
-#define USE_GYRO
-#define USE_GYRO_MPU6500
-#define USE_GYRO_MPU9250
+#define USE_IMU_MPU6500
+#define IMU_MPU6500_ALIGN       CW0_DEG
 
-#define USE_ACC
-#define USE_ACC_MPU6500
-#define USE_ACC_MPU9250
-
-#define ACC_MPU6500_ALIGN       CW0_DEG
-#define GYRO_MPU6500_ALIGN      CW0_DEG
-#define ACC_MPU9250_ALIGN       CW0_DEG
-#define GYRO_MPU9250_ALIGN      CW0_DEG
+#define USE_IMU_MPU9250
+#define IMU_MPU9250_ALIGN       CW0_DEG
 
 #define MPU6500_CS_PIN          SPI1_NSS_PIN
 #define MPU6500_SPI_BUS         BUS_SPI1
diff --git a/src/main/target/SPRACINGF4EVO/target.h b/src/main/target/SPRACINGF4EVO/target.h
index 04f92232f4..c1da04f60b 100755
--- a/src/main/target/SPRACINGF4EVO/target.h
+++ b/src/main/target/SPRACINGF4EVO/target.h
@@ -37,19 +37,11 @@
 #define USE_MAG_DATA_READY_SIGNAL
 #define ENSURE_MAG_DATA_READY_IS_HIGH
 
-#define USE_GYRO
-#define USE_GYRO_MPU6500
-#define USE_GYRO_MPU9250
+#define USE_IMU_MPU6500
+#define IMU_MPU6500_ALIGN       CW0_DEG
 
-#define USE_ACC
-#define USE_ACC_MPU6500
-#define USE_ACC_MPU9250
-
-#define ACC_MPU6500_ALIGN       CW0_DEG
-#define GYRO_MPU6500_ALIGN      CW0_DEG
-
-#define ACC_MPU9250_ALIGN       CW0_DEG
-#define GYRO_MPU9250_ALIGN      CW0_DEG
+#define USE_IMU_MPU9250
+#define IMU_MPU9250_ALIGN       CW0_DEG
 
 #define USE_BARO
 #define BARO_I2C_BUS            BUS_I2C1
diff --git a/src/main/target/SPRACINGF7DUAL/target.c b/src/main/target/SPRACINGF7DUAL/target.c
index d9c1c21b9a..8fb1c781f5 100644
--- a/src/main/target/SPRACINGF7DUAL/target.c
+++ b/src/main/target/SPRACINGF7DUAL/target.c
@@ -27,10 +27,11 @@
 #include "drivers/timer_def.h"
 #include "drivers/dma.h"
 #include "drivers/bus.h"
+#include "drivers/sensor.h"
 
 // Register both MPU6500
-BUSDEV_REGISTER_SPI(busdev_mpu6500_1,     DEVHW_MPU6500,      MPU6500_1_SPI_BUS,    MPU6500_1_CS_PIN,     GYRO_1_EXTI_PIN,  DEVFLAGS_NONE);
-BUSDEV_REGISTER_SPI(busdev_mpu6500_2,     DEVHW_MPU6500,      MPU6500_2_SPI_BUS,    MPU6500_2_CS_PIN,     GYRO_2_EXTI_PIN,  DEVFLAGS_NONE);
+BUSDEV_REGISTER_SPI_TAG(busdev_mpu6500_1,     DEVHW_MPU6500,      MPU6500_1_SPI_BUS,    MPU6500_1_CS_PIN,     GYRO_1_EXTI_PIN,  0,  DEVFLAGS_NONE,  IMU_1_ALIGN);
+BUSDEV_REGISTER_SPI_TAG(busdev_mpu6500_2,     DEVHW_MPU6500,      MPU6500_2_SPI_BUS,    MPU6500_2_CS_PIN,     GYRO_2_EXTI_PIN,  0,  DEVFLAGS_NONE,  IMU_2_ALIGN);
 
 const timerHardware_t timerHardware[] = {
 
diff --git a/src/main/target/SPRACINGF7DUAL/target.h b/src/main/target/SPRACINGF7DUAL/target.h
index 3f65888364..aead3cbe59 100644
--- a/src/main/target/SPRACINGF7DUAL/target.h
+++ b/src/main/target/SPRACINGF7DUAL/target.h
@@ -39,41 +39,24 @@
 #define USE_EXTI
 #define GYRO_1_EXTI_PIN         PC13
 #define GYRO_2_EXTI_PIN         PC14
-#define GYRO_INT_EXTI           GYRO_1_EXTI_PIN
 
 #define USE_MPU_DATA_READY_SIGNAL
 #define ENSURE_MPU_DATA_READY_IS_LOW
 
 #define USE_TARGET_IMU_HARDWARE_DESCRIPTORS
-#define USE_GYRO
-#define USE_GYRO_MPU6500
 
-#define USE_ACC
-#define USE_ACC_MPU6500
+#define USE_IMU_MPU6500
+#define IMU_1_ALIGN                 IMU_MPU6500_1_ALIGN
+#define IMU_2_ALIGN                 IMU_MPU6500_2_ALIGN
 
 #if (SPRACINGF7DUAL_REV >= 2)
-#define ACC_MPU6500_1_ALIGN           CW0_DEG
-#define GYRO_MPU6500_1_ALIGN          CW0_DEG
-
-#define ACC_MPU6500_2_ALIGN         CW270_DEG
-#define GYRO_MPU6500_2_ALIGN        CW270_DEG
+#define IMU_MPU6500_1_ALIGN         CW0_DEG
+#define IMU_MPU6500_2_ALIGN         CW270_DEG
 #else
-#define ACC_MPU6500_1_ALIGN           CW180_DEG
-#define GYRO_MPU6500_1_ALIGN          CW180_DEG
-
-#define ACC_MPU6500_2_ALIGN         CW270_DEG
-#define GYRO_MPU6500_2_ALIGN        CW270_DEG
+#define IMU_MPU6500_1_ALIGN         CW180_DEG
+#define IMU_MPU6500_2_ALIGN         CW270_DEG
 #endif
 
-
-#define GYRO_1_ALIGN                GYRO_MPU6500_1_ALIGN
-#define GYRO_2_ALIGN                GYRO_MPU6500_2_ALIGN
-#define ACC_1_ALIGN                 ACC_MPU6500_1_ALIGN
-#define ACC_2_ALIGN                 ACC_MPU6500_2_ALIGN
-
-#define ACC_MPU6500_ALIGN       ACC_1_ALIGN
-#define GYRO_MPU6500_ALIGN      GYRO_1_ALIGN
-
 #define USE_BARO
 #define BARO_I2C_BUS            BUS_I2C1
 #define USE_BARO_BMP280
diff --git a/src/main/target/YUPIF4/target.h b/src/main/target/YUPIF4/target.h
index 0e95781fd6..dc423616f2 100644
--- a/src/main/target/YUPIF4/target.h
+++ b/src/main/target/YUPIF4/target.h
@@ -53,17 +53,8 @@
 #define MPU6500_CS_PIN          SPI1_NSS_PIN
 #define MPU6500_SPI_BUS         BUS_SPI1
 
-#define USE_GYRO_MPU6500
-#define USE_ACC_MPU6500
-
-#define USE_ACC
-#define USE_ACC_SPI_MPU6500
-#define ACC_MPU6500_ALIGN       CW90_DEG
-
-#define USE_GYRO
-#define USE_GYRO_SPI_MPU6500
-#define GYRO_MPU6500_ALIGN      CW90_DEG
-
+#define USE_IMU_MPU6500
+#define IMU_MPU6500_ALIGN       CW90_DEG
 
 #define USE_I2C
 #define USE_I2C_DEVICE_2
diff --git a/src/main/target/YUPIF7/target.h b/src/main/target/YUPIF7/target.h
index 84a9c2dd08..b7799bfb81 100644
--- a/src/main/target/YUPIF7/target.h
+++ b/src/main/target/YUPIF7/target.h
@@ -40,16 +40,8 @@
 #define MPU6500_CS_PIN          SPI1_NSS_PIN
 #define MPU6500_SPI_BUS         BUS_SPI1
 
-#define USE_GYRO_MPU6500
-#define USE_ACC_MPU6500
-
-#define USE_ACC
-#define USE_ACC_SPI_MPU6500
-#define ACC_MPU6500_ALIGN       CW90_DEG
-
-#define USE_GYRO
-#define USE_GYRO_SPI_MPU6500
-#define GYRO_MPU6500_ALIGN      CW90_DEG
+#define USE_IMU_MPU6500
+#define IMU_MPU6500_ALIGN       CW90_DEG
 
 #define USE_MAG
 #define MAG_I2C_BUS             BUS_I2C1
diff --git a/src/main/target/common_hardware.c b/src/main/target/common_hardware.c
index aac63ddd76..22bee2465c 100755
--- a/src/main/target/common_hardware.c
+++ b/src/main/target/common_hardware.c
@@ -32,7 +32,7 @@
     #define MPU_ADDRESS 0x68
     #endif
 
-    #if defined(USE_GYRO_L3GD20)
+    #if defined(USE_IMU_L3GD20)
         #if defined(GYRO_L3GD20_ALIGN)
             #define GYRO_0_ALIGN    GYRO_L3GD20_ALIGN
         #else
@@ -41,19 +41,19 @@
         BUSDEV_REGISTER_SPI(busdev_l3gd20,      DEVHW_L3GD20,       L3GD20_SPI_BUS,     L3GD20_CS_PIN,      NONE,           DEVFLAGS_NONE,  IMU_L3GD20_ALIGN);
     #endif
 
-    #if defined(USE_ACC_LSM303DLHC)
+    #if defined(USE_IMU_LSM303DLHC)
         BUSDEV_REGISTER_I2C(busdev_lsm303,      DEVHW_LSM303DLHC,   LSM303DLHC_I2C_BUS, 0x19,               NONE,           DEVFLAGS_NONE,  IMU_LSM303DLHC_ALIGN);
     #endif
 
-    #if defined(USE_GYRO_MPU6000)
+    #if defined(USE_IMU_MPU6000)
         BUSDEV_REGISTER_SPI(busdev_mpu6000,     DEVHW_MPU6000,      MPU6000_SPI_BUS,    MPU6000_CS_PIN,     GYRO_INT_EXTI,  DEVFLAGS_NONE,  IMU_MPU6000_ALIGN);
     #endif
 
-    #if defined(USE_GYRO_MPU6050)
+    #if defined(USE_IMU_MPU6050)
         BUSDEV_REGISTER_I2C(busdev_mpu6050,     DEVHW_MPU6050,      MPU6050_I2C_BUS,    MPU_ADDRESS,        GYRO_INT_EXTI,  DEVFLAGS_NONE,  IMU_MPU6050_ALIGN);
     #endif
 
-    #if defined(USE_GYRO_MPU6500)
+    #if defined(USE_IMU_MPU6500)
         #if defined(MPU6500_SPI_BUS)
         BUSDEV_REGISTER_SPI(busdev_mpu6500,     DEVHW_MPU6500,      MPU6500_SPI_BUS,    MPU6500_CS_PIN,     GYRO_INT_EXTI,  DEVFLAGS_NONE,  IMU_MPU6500_ALIGN);
         #elif defined(MPU6500_I2C_BUS)
@@ -61,7 +61,7 @@
         #endif
     #endif
 
-    #if defined(USE_GYRO_MPU9250)
+    #if defined(USE_IMU_MPU9250)
         #if defined(MPU9250_SPI_BUS)
         BUSDEV_REGISTER_SPI(busdev_mpu9250,     DEVHW_MPU9250,      MPU9250_SPI_BUS,    MPU9250_CS_PIN,     GYRO_INT_EXTI,  DEVFLAGS_NONE,  IMU_MPU9250_ALIGN);
         #elif defined(MPU9250_I2C_BUS)
@@ -69,11 +69,11 @@
         #endif
     #endif
 
-    #if defined(USE_GYRO_ICM20689)
+    #if defined(USE_IMU_ICM20689)
         BUSDEV_REGISTER_SPI(busdev_icm20689,    DEVHW_ICM20689,     ICM20689_SPI_BUS,   ICM20689_CS_PIN,    GYRO_INT_EXTI,  DEVFLAGS_NONE,  IMU_ICM20689_ALIGN);
     #endif
 
-    #if defined(USE_GYRO_BMI160)
+    #if defined(USE_IMU_BMI160)
         #if defined(BMI160_SPI_BUS)
         BUSDEV_REGISTER_SPI(busdev_bmi160,      DEVHW_BMI160,       BMI160_SPI_BUS,     BMI160_CS_PIN,      GYRO_INT_EXTI,  DEVFLAGS_NONE,  IMU_BMI160_ALIGN);
         #elif defined(BMI160_I2C_BUS)
diff --git a/src/main/target/sanity_check.h b/src/main/target/sanity_check.h
index 57989b4001..8b78b89d4d 100644
--- a/src/main/target/sanity_check.h
+++ b/src/main/target/sanity_check.h
@@ -24,6 +24,31 @@
 
 #pragma once
 
+// Deprecated USE_GYRO/ACC defines
+#if defined (USE_GYRO) || defined (USE_ACC)
+#error "Unnecessary USE_ACC and/or USE_GYRO"
+#endif
+
+#if defined (USE_GYRO_MPU6000) || defined (USE_ACC_MPU6000) || defined (USE_GYRO_MPU6050) || defined (USE_ACC_MPU6050)
+#error "Replace USE_GYRO_xxx and USE_ACC_xxx with USE_IMU_xxx"
+#endif
+
+#if defined (USE_GYRO_MPU6500) || defined (USE_ACC_MPU6500) || defined (USE_GYRO_MPU9250) || defined (USE_ACC_MPU9250)
+#error "Replace USE_GYRO_xxx and USE_ACC_xxx with USE_IMU_xxx"
+#endif
+
+#if defined (USE_GYRO_ICM20689) || defined (USE_ACC_ICM20689)
+#error "Replace USE_GYRO_xxx and USE_ACC_xxx with USE_IMU_xxx"
+#endif
+
+#if defined (USE_FAKE_GYRO) || defined (USE_FAKE_ACC)
+#error "Replace USE_GYRO_xxx and USE_ACC_xxx with USE_IMU_xxx"
+#endif
+
+#if defined (USE_ACC_ADXL345) || defined (USE_ACC_LSM303DLHC) || defined (USE_ACC_MMA8452) || defined (USE_ACC_BMA280) || defined (USE_ACC_BMI160)
+#error "Replace USE_GYRO_xxx and USE_ACC_xxx with USE_IMU_xxx"
+#endif
+
 // Make sure IMU alignments are migrated to IMU_xxx_ALIGN
 #if defined (GYRO_MPU6050_ALIGN) || defined (ACC_MPU6050_ALIGN)
 #error "Replace GYRO_MPU6050_ALIGN and ACC_MPU6050_ALIGN with IMU_MPU6050_ALIGN"
@@ -69,3 +94,11 @@
 #error "Replace ACC_MMA8452_ALIGN with IMU_MMA8452_ALIGN"
 #endif
 
+#if defined (ACC_ADXL345_ALIGN)
+#error "Replace ACC_ADXL345_ALIGN with IMU_ADXL345_ALIGN"
+#endif
+
+#if defined (ACC_LSM303DLHC_ALIGN)
+#error "Replace ACC_LSM303DLHC_ALIGN with IMU_LSM303DLHC_ALIGN"
+#endif
+

From c30b18de32992fa2a65913e9edb89c38f18ee568 Mon Sep 17 00:00:00 2001
From: "Konstantin (DigitalEntity) Sharlaimov"
 <konstantin.sharlaimov@gmail.com>
Date: Tue, 5 May 2020 15:38:29 +0200
Subject: [PATCH 140/179] Fix failing tests and targets

---
 src/main/sensors/gyro.c              | 2 +-
 src/main/target/SPEEDYBEEF4/target.h | 2 --
 src/test/unit/target.h               | 2 +-
 3 files changed, 2 insertions(+), 4 deletions(-)

diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index 72d3557a8e..5849833f29 100644
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@@ -215,7 +215,7 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHard
         FALLTHROUGH;
 #endif
 
-#ifdef USE_FAKE_GYRO
+#ifdef USE_IMU_FAKE
     case GYRO_FAKE:
         if (fakeGyroDetect(dev)) {
             gyroHardware = GYRO_FAKE;
diff --git a/src/main/target/SPEEDYBEEF4/target.h b/src/main/target/SPEEDYBEEF4/target.h
index 865980439c..1365b7aec5 100644
--- a/src/main/target/SPEEDYBEEF4/target.h
+++ b/src/main/target/SPEEDYBEEF4/target.h
@@ -27,8 +27,6 @@
 
 /*** IMU sensors ***/
 #define USE_EXTI							
-#define USE_GYRO							
-#define USE_ACC								
 
 #define GYRO_INT_EXTI            PC4		
 #define USE_MPU_DATA_READY_SIGNAL			
diff --git a/src/test/unit/target.h b/src/test/unit/target.h
index 23987c9aab..ad0ad7a21d 100755
--- a/src/test/unit/target.h
+++ b/src/test/unit/target.h
@@ -35,7 +35,7 @@
 #define USE_TELEMETRY_LTM
 #define USE_LED_STRIP
 #define TRANSPONDER
-#define USE_FAKE_GYRO
+#define USE_IMU_FAKE
 #define USE_VCP
 #define USE_UART1
 #define USE_UART2

From 6665c7e877fe5451a6d5824c75da54d1e05b916f Mon Sep 17 00:00:00 2001
From: "Konstantin (DigitalEntity) Sharlaimov"
 <konstantin.sharlaimov@gmail.com>
Date: Tue, 5 May 2020 16:31:19 +0200
Subject: [PATCH 141/179] [ESC_SENSOR] Add listen_only mode

---
 src/main/drivers/pwm_output.c | 12 ++++--------
 src/main/fc/fc_core.c         |  5 +++++
 src/main/fc/fc_init.c         |  7 +++++++
 src/main/fc/settings.yaml     | 11 ++++++++++-
 src/main/sensors/esc_sensor.c | 31 +++++++++++++++++++++++++------
 src/main/sensors/esc_sensor.h |  1 +
 6 files changed, 52 insertions(+), 15 deletions(-)

diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c
index 8d048c87a5..a8c90c3eeb 100644
--- a/src/main/drivers/pwm_output.c
+++ b/src/main/drivers/pwm_output.c
@@ -327,6 +327,10 @@ bool isMotorProtocolDigital(void)
 
 void pwmRequestMotorTelemetry(int motorIndex)
 {
+    if (!isMotorProtocolDigital()) {
+        return;
+    }
+
     const int motorCount = getMotorCount();
     for (int index = 0; index < motorCount; index++) {
         if (motors[index].pwmPort && motors[index].pwmPort->configured && index == motorIndex) {
@@ -345,10 +349,6 @@ void pwmCompleteMotorUpdate(void)
     int motorCount = getMotorCount();
     timeUs_t currentTimeUs = micros();
 
-#ifdef USE_ESC_SENSOR
-    escSensorUpdate(currentTimeUs);
-#endif
-
     // Enforce motor update rate
     if ((digitalMotorUpdateIntervalUs == 0) || ((currentTimeUs - digitalMotorLastUpdateUs) <= digitalMotorUpdateIntervalUs)) {
         return;
@@ -417,10 +417,6 @@ void pwmMotorPreconfigure(void)
         case PWM_TYPE_DSHOT600:
         case PWM_TYPE_DSHOT300:
         case PWM_TYPE_DSHOT150:
-#ifdef USE_ESC_SENSOR
-            // DSHOT supports a dedicated wire ESC telemetry. Kick off the ESC-sensor receiver initialization
-            escSensorInitialize();
-#endif
             motorConfigDigitalUpdateInterval(motorConfig()->motorPwmRate);
             motorWritePtr = pwmWriteDigital;
             break;
diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c
index 4528397b42..dc50817963 100755
--- a/src/main/fc/fc_core.c
+++ b/src/main/fc/fc_core.c
@@ -50,6 +50,7 @@ FILE_COMPILE_FOR_SPEED
 #include "sensors/battery.h"
 #include "sensors/rangefinder.h"
 #include "sensors/opflow.h"
+#include "sensors/esc_sensor.h"
 
 #include "fc/fc_core.h"
 #include "fc/cli.h"
@@ -856,6 +857,10 @@ void taskRunRealtimeCallbacks(timeUs_t currentTimeUs)
 #ifdef USE_DSHOT
     pwmCompleteMotorUpdate();
 #endif
+
+#ifdef USE_ESC_SENSOR
+    escSensorUpdate(currentTimeUs);
+#endif
 }
 
 bool taskUpdateRxCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTime)
diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c
index e7c4b4b026..6b3c9f0020 100644
--- a/src/main/fc/fc_init.c
+++ b/src/main/fc/fc_init.c
@@ -138,6 +138,7 @@
 #include "sensors/pitotmeter.h"
 #include "sensors/rangefinder.h"
 #include "sensors/sensors.h"
+#include "sensors/esc_sensor.h"
 
 #include "scheduler/scheduler.h"
 
@@ -274,6 +275,12 @@ void init(void)
     // to run after the sensors have been detected.
     mspSerialInit();
 
+#ifdef USE_ESC_SENSOR
+    // DSHOT supports a dedicated wire ESC telemetry. Kick off the ESC-sensor receiver initialization
+    // We may, however, do listen_only, so need to init this anyway
+    escSensorInitialize();
+#endif
+
 #if defined(USE_DJI_HD_OSD)
     // DJI OSD uses a special flavour of MSP (subset of Betaflight 4.1.1 MSP) - process as part of serial task
     djiOsdSerialInit();
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 65183912c0..d328c3d813 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -2150,5 +2150,14 @@ groups:
           table: log_level
         - name: log_topics
           field: topics
-          min: 0,
+          min: 0
           max: UINT32_MAX
+
+  - name: PG_ESC_SENSOR_CONFIG
+    type: escSensorConfig_t
+    headers: ["sensors/esc_sensor.h"]
+    condition: USE_ESC_SENSOR
+    members:
+        - name: esc_sensor_listen_only
+          field: listenOnly
+          type: bool
diff --git a/src/main/sensors/esc_sensor.c b/src/main/sensors/esc_sensor.c
index 8a076e643f..f4bde550f1 100644
--- a/src/main/sensors/esc_sensor.c
+++ b/src/main/sensors/esc_sensor.c
@@ -75,14 +75,30 @@ static escSensorData_t  escSensorData[MAX_SUPPORTED_MOTORS];
 static escSensorData_t  escSensorDataCombined;
 static bool             escSensorDataNeedsUpdate;
 
-PG_REGISTER_WITH_RESET_TEMPLATE(escSensorConfig_t, escSensorConfig, PG_ESC_SENSOR_CONFIG, 0);
+PG_REGISTER_WITH_RESET_TEMPLATE(escSensorConfig_t, escSensorConfig, PG_ESC_SENSOR_CONFIG, 1);
 PG_RESET_TEMPLATE(escSensorConfig_t, escSensorConfig,
-    .currentOffset = 0,
+    .currentOffset = 0, // UNUSED
+    .listenOnly = 0,
 );
 
+static int getTelemetryMotorCount(void)
+{
+    if (escSensorConfig()->listenOnly) {
+        return 1;
+    }
+    else {
+        return getMotorCount();
+    }
+}
+
 static void escSensorSelectNextMotor(void)
 {
-    escSensorMotor = (escSensorMotor + 1) % getMotorCount();
+    if (escSensorConfig()->listenOnly) {
+        escSensorMotor = 0;
+    }
+    else {
+        escSensorMotor = (escSensorMotor + 1) % getTelemetryMotorCount();
+    }
 }
 
 static void escSensorIncreaseDataAge(void)
@@ -154,7 +170,7 @@ escSensorData_t * escSensorGetData(void)
 
         // Combine data only from active sensors, ignore stale sensors
         int usedEscSensorCount = 0;
-        for (int i = 0; i < getMotorCount(); i++) {
+        for (int i = 0; i < getTelemetryMotorCount(); i++) {
             if (escSensorData[i].dataAge < ESC_DATA_INVALID) {
                 usedEscSensorCount++;
                 escSensorDataCombined.dataAge = MAX(escSensorDataCombined.dataAge, escSensorData[i].dataAge);
@@ -167,7 +183,7 @@ escSensorData_t * escSensorGetData(void)
 
         // Make sure we calculate our sensor values only from non-stale values
         if (usedEscSensorCount) {
-            escSensorDataCombined.current = (uint32_t)escSensorDataCombined.current * getMotorCount() / usedEscSensorCount + escSensorConfig()->currentOffset;
+            escSensorDataCombined.current = (uint32_t)escSensorDataCombined.current * getTelemetryMotorCount() / usedEscSensorCount + escSensorConfig()->currentOffset;
             escSensorDataCombined.voltage = (uint32_t)escSensorDataCombined.voltage / usedEscSensorCount;
             escSensorDataCombined.rpm = (float)escSensorDataCombined.rpm / usedEscSensorCount;
         }
@@ -229,7 +245,9 @@ void escSensorUpdate(timeUs_t currentTimeUs)
             break;
 
         case ESC_SENSOR_READY:
-            pwmRequestMotorTelemetry(escSensorMotor);
+            if (!escSensorConfig()->listenOnly) {
+                pwmRequestMotorTelemetry(escSensorMotor);
+            }
             bufferPosition = 0;
             escTriggerTimeMs = currentTimeMs;
             escSensorState = ESC_SENSOR_WAITING;
@@ -238,6 +256,7 @@ void escSensorUpdate(timeUs_t currentTimeUs)
         case ESC_SENSOR_WAITING:
             if ((currentTimeMs - escTriggerTimeMs) >= ESC_REQUEST_TIMEOUT_MS) {
                 // Timed out. Select next motor and move on
+                escSensorIncreaseDataAge();
                 escSensorSelectNextMotor();
                 escSensorState = ESC_SENSOR_READY;
             }
diff --git a/src/main/sensors/esc_sensor.h b/src/main/sensors/esc_sensor.h
index 97d5671d42..e205af981d 100644
--- a/src/main/sensors/esc_sensor.h
+++ b/src/main/sensors/esc_sensor.h
@@ -34,6 +34,7 @@ typedef struct {
 
 typedef struct escSensorConfig_s {
     uint16_t currentOffset;             // offset consumed by the flight controller / VTX / cam / ... in mA
+    uint8_t  listenOnly;
 } escSensorConfig_t;
 
 PG_DECLARE(escSensorConfig_t, escSensorConfig);

From 94543da48f50afac8eef1851921621b74e44e666 Mon Sep 17 00:00:00 2001
From: "Konstantin (DigitalEntity) Sharlaimov"
 <konstantin.sharlaimov@gmail.com>
Date: Tue, 5 May 2020 18:01:02 +0200
Subject: [PATCH 142/179] Fix DSHOT-less builds

---
 src/main/drivers/pwm_output.c | 9 +++++++++
 1 file changed, 9 insertions(+)

diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c
index a8c90c3eeb..74c74f62f8 100644
--- a/src/main/drivers/pwm_output.c
+++ b/src/main/drivers/pwm_output.c
@@ -387,6 +387,15 @@ void pwmCompleteMotorUpdate(void)
     }
 #endif
 }
+
+#else // digital motor protocol
+
+// This stub is needed to avoid ESC_SENSOR dependency on DSHOT
+void pwmRequestMotorTelemetry(int motorIndex)
+{
+    UNUSED(motorIndex);
+}
+
 #endif
 
 void pwmMotorPreconfigure(void)

From df26e6e9f4173bd7c647765723c1902257e4720e Mon Sep 17 00:00:00 2001
From: Reto Hasler <Reto.hasler@asciich.ch>
Date: Tue, 5 May 2020 21:16:12 +0200
Subject: [PATCH 143/179] Fixed number of input cells

MatekF722-SE is able to handle 2-8S LiPO according to their webpage: http://www.mateksys.com/?portfolio=f722-se#tab-id-1
---
 docs/Board - MatekF722-SE.md | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/docs/Board - MatekF722-SE.md b/docs/Board - MatekF722-SE.md
index 6a14d973fa..8fcf3177fe 100644
--- a/docs/Board - MatekF722-SE.md	
+++ b/docs/Board - MatekF722-SE.md	
@@ -22,7 +22,7 @@ Full details on the MATEKSYS F722-SE can be found on the Matek Website: [mateksy
 
 ### Integrated PDB Specs
 
-* *Input:* 6-36v (3-8S LiPo) w/TVS protection
+* *Input:* 6-36v (2-8S LiPo) w/TVS protection
 * *ESC Pads:* Rated 4x30A per ESC pad set (4x46A burst)
 * Voltage Regulators:
   * *5v BEC:* 2A continuous load (3A burst)

From 854816e1f42e89cb53a6d5898d8b32208f769fed Mon Sep 17 00:00:00 2001
From: giacomo892 <giacomo1989@gmail.com>
Date: Wed, 6 May 2020 09:17:50 +0200
Subject: [PATCH 144/179] Update Bug_report.md

---
 .github/ISSUE_TEMPLATE/Bug_report.md | 12 ++++++++++++
 .github/ISSUE_TEMPLATE/Question.md   | 23 -----------------------
 2 files changed, 12 insertions(+), 23 deletions(-)
 delete mode 100644 .github/ISSUE_TEMPLATE/Question.md

diff --git a/.github/ISSUE_TEMPLATE/Bug_report.md b/.github/ISSUE_TEMPLATE/Bug_report.md
index 6f6952875f..0af416b84a 100644
--- a/.github/ISSUE_TEMPLATE/Bug_report.md
+++ b/.github/ISSUE_TEMPLATE/Bug_report.md
@@ -7,6 +7,18 @@ assignees: ''
 
 ---
 
+**IF YOU DON'T REMOVE AND UNDERSTAND THE NEXT FEW LINES, THEN YOUR REPORT WILL BE CLOSED**
+
+Questions and user problems should be directed at our social media accounts:
+* [Telegram channel](https://t.me/INAVFlight)
+* [Facebook group](https://www.facebook.com/groups/INAVOfficial)
+* [RC Groups thread](https://www.rcgroups.com/forums/showthread.php?2495732-Cleanflight-iNav-%28navigation-rewrite%29-project)
+
+**Please be very sure you have found a bug and no others have already reported it, when opening this issue**
+
+****
+
+
 ## Current Behavior
 <!-- If applicable, add screenshots, videos and blackbox logs to help explain your problem. -->
 
diff --git a/.github/ISSUE_TEMPLATE/Question.md b/.github/ISSUE_TEMPLATE/Question.md
deleted file mode 100644
index 2850f8b410..0000000000
--- a/.github/ISSUE_TEMPLATE/Question.md
+++ /dev/null
@@ -1,23 +0,0 @@
----
-name: "❓Question"
-about: Have a question?
-title: ''
-labels: ''
-assignees: ''
-
----
-
-For immediate help, just ask your question on one of the following platforms:
-
-* [Telegram channel](https://t.me/INAVFlight)
-* [Facebook group](https://www.facebook.com/groups/INAVOfficial)
-* [RC Groups thread](https://www.rcgroups.com/forums/showthread.php?2495732-Cleanflight-iNav-%28navigation-rewrite%29-project)
-
-You can also read public documentations or watch video tutorials:
-
-* [Official documentation](https://github.com/iNavFlight/inav/tree/master/docs)
-* [Official Wiki](https://github.com/iNavFlight/inav/wiki)
-* [Video series by Painless360](https://www.youtube.com/playlist?list=PLYsWjANuAm4qdXEGFSeUhOZ10-H8YTSnH)
-* [Video series by Paweł Spychalski](https://www.youtube.com/playlist?list=PLOUQ8o2_nCLloACrA6f1_daCjhqY2x0fB)
-* [How to setup INAV on a flying wing](https://www.youtube.com/playlist?list=PLOUQ8o2_nCLn_lCX7UB2OPLzR4w5G4el3)
-* [How to setup INAV on a 5" quad](https://www.youtube.com/playlist?list=PLOUQ8o2_nCLl9M6Vm-ZNLMCr6_9yNVHQG)

From a7cc75fb245d77e5f72dbe63263a1db6116eb3df Mon Sep 17 00:00:00 2001
From: giacomo892 <giacomo1989@gmail.com>
Date: Wed, 6 May 2020 10:26:36 +0200
Subject: [PATCH 145/179] Update Bug_report.md

---
 .github/ISSUE_TEMPLATE/Bug_report.md | 7 ++++---
 1 file changed, 4 insertions(+), 3 deletions(-)

diff --git a/.github/ISSUE_TEMPLATE/Bug_report.md b/.github/ISSUE_TEMPLATE/Bug_report.md
index 0af416b84a..eb7f126be9 100644
--- a/.github/ISSUE_TEMPLATE/Bug_report.md
+++ b/.github/ISSUE_TEMPLATE/Bug_report.md
@@ -7,14 +7,15 @@ assignees: ''
 
 ---
 
-**IF YOU DON'T REMOVE AND UNDERSTAND THE NEXT FEW LINES, THEN YOUR REPORT WILL BE CLOSED**
+**PLEASE MAKE SURE YOU READ AND UNDERSTAND THE SOCIAL MEDIA SUPPORT CHANNELS. QUESTIONS ABOUT FLASHING, CONFIGURING, PILOTING MAY BE CLOSED WITHOUT FURTHER INTERACTION.**
 
-Questions and user problems should be directed at our social media accounts:
 * [Telegram channel](https://t.me/INAVFlight)
 * [Facebook group](https://www.facebook.com/groups/INAVOfficial)
 * [RC Groups thread](https://www.rcgroups.com/forums/showthread.php?2495732-Cleanflight-iNav-%28navigation-rewrite%29-project)
 
-**Please be very sure you have found a bug and no others have already reported it, when opening this issue**
+**Please double-check that nobody reported the issue before by using search in this bug tracker.**
+
+**PLEASE DELETE THE TEXT ABOVE AFTER READING AND UNDERSTANDING IT**
 
 ****
 

From aa928bdffffb928d3d9f86ec07f9ad4293cbad6a Mon Sep 17 00:00:00 2001
From: Michel Pastor <shellixyz@users.noreply.github.com>
Date: Wed, 6 May 2020 13:38:06 +0200
Subject: [PATCH 146/179] Add IR-Lock optical positioning system support
 (#5677)

---
 make/source.mk                    |   4 +-
 src/main/build/debug.h            |   1 +
 src/main/common/maths.h           |   2 +
 src/main/drivers/bus.h            |   1 +
 src/main/drivers/irlock.c         |  88 +++++++++++++++++++
 src/main/drivers/irlock.h         |  45 ++++++++++
 src/main/drivers/resource.c       |   2 +-
 src/main/drivers/resource.h       |   1 +
 src/main/fc/fc_tasks.c            |  21 +++++
 src/main/fc/settings.yaml         |   3 +-
 src/main/scheduler/scheduler.h    |   3 +
 src/main/sensors/initialisation.c |  17 ++--
 src/main/sensors/irlock.c         | 136 ++++++++++++++++++++++++++++++
 src/main/sensors/irlock.h         |  29 +++++++
 src/main/target/common_hardware.c |   9 ++
 15 files changed, 353 insertions(+), 9 deletions(-)
 create mode 100644 src/main/drivers/irlock.c
 create mode 100644 src/main/drivers/irlock.h
 create mode 100644 src/main/sensors/irlock.c
 create mode 100644 src/main/sensors/irlock.h

diff --git a/make/source.mk b/make/source.mk
index 2e8106da6c..5c706dc07d 100644
--- a/make/source.mk
+++ b/make/source.mk
@@ -44,6 +44,7 @@ COMMON_SRC = \
             drivers/exti.c \
             drivers/io.c \
             drivers/io_pca9685.c \
+            drivers/irlock.c \
             drivers/light_led.c \
             drivers/osd.c \
             drivers/persistent.c \
@@ -144,13 +145,14 @@ COMMON_SRC = \
             scheduler/scheduler.c \
             sensors/acceleration.c \
             sensors/battery.c \
-            sensors/temperature.c \
             sensors/boardalignment.c \
             sensors/compass.c \
             sensors/diagnostics.c \
             sensors/gyro.c \
             sensors/initialisation.c \
             sensors/esc_sensor.c \
+            sensors/irlock.c \
+            sensors/temperature.c \
             uav_interconnect/uav_interconnect_bus.c \
             uav_interconnect/uav_interconnect_rangefinder.c \
             blackbox/blackbox.c \
diff --git a/src/main/build/debug.h b/src/main/build/debug.h
index a300b70ade..283032b297 100644
--- a/src/main/build/debug.h
+++ b/src/main/build/debug.h
@@ -70,5 +70,6 @@ typedef enum {
     DEBUG_NAV_YAW,
     DEBUG_DYNAMIC_FILTER,
     DEBUG_DYNAMIC_FILTER_FREQUENCY,
+    DEBUG_IRLOCK,
     DEBUG_COUNT
 } debugType_e;
diff --git a/src/main/common/maths.h b/src/main/common/maths.h
index 24a72c5d72..728df5e57a 100644
--- a/src/main/common/maths.h
+++ b/src/main/common/maths.h
@@ -60,6 +60,8 @@
 #define CENTIMETERS_TO_FEET(cm)                 (cm * (328 / 10000.0))
 #define CENTIMETERS_TO_METERS(cm)               (cm / 100)
 
+#define METERS_TO_CENTIMETERS(m)                (m * 100)
+
 // copied from https://code.google.com/p/cxutil/source/browse/include/cxutil/utility.h#70
 #define _CHOOSE2(binoper, lexpr, lvar, rexpr, rvar)         \
     ( __extension__ ({                                      \
diff --git a/src/main/drivers/bus.h b/src/main/drivers/bus.h
index 18bea5b63b..7f7d06d092 100755
--- a/src/main/drivers/bus.h
+++ b/src/main/drivers/bus.h
@@ -143,6 +143,7 @@ typedef enum {
     DEVHW_M25P16,       // SPI NOR flash
     DEVHW_UG2864,       // I2C OLED display
     DEVHW_SDCARD,       // Generic SD-Card
+    DEVHW_IRLOCK,       // IR-Lock visual positioning hardware
 } devHardwareType_e;
 
 typedef enum {
diff --git a/src/main/drivers/irlock.c b/src/main/drivers/irlock.c
new file mode 100644
index 0000000000..179fe30c61
--- /dev/null
+++ b/src/main/drivers/irlock.c
@@ -0,0 +1,88 @@
+/*
+ * 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 <stdbool.h>
+#include <stdint.h>
+
+#include <math.h>
+
+#include "platform.h"
+
+#include "drivers/sensor.h"
+#include "drivers/irlock.h"
+#include "drivers/time.h"
+
+#define IRLOCK_OBJECT_SYNC ((uint16_t)0xaa55)
+#define IRLOCK_FRAME_SYNC ((uint32_t)(IRLOCK_OBJECT_SYNC | (IRLOCK_OBJECT_SYNC << 16)))
+
+
+#if defined(USE_NAV) && defined(USE_IRLOCK)
+
+static bool irlockHealthy = false;
+
+static bool irlockFrameSync(irlockDev_t *irlockDev)
+{
+    uint32_t sync_word = 0;
+    uint8_t count = 10;
+    while (count-- && sync_word != IRLOCK_FRAME_SYNC) {
+        uint8_t sync_byte;
+        irlockHealthy = busRead(irlockDev->busDev, 0xFF, &sync_byte);
+        if (!(irlockHealthy && sync_byte)) return false;
+        sync_word = (sync_word >> 8) | (((uint32_t)sync_byte) << 24);
+    }
+    return sync_word == IRLOCK_FRAME_SYNC;
+}
+
+static bool irlockRead(irlockDev_t *irlockDev, irlockData_t *irlockData)
+{
+    if (irlockFrameSync(irlockDev) && busReadBuf(irlockDev->busDev, 0xFF, (void*)irlockData, sizeof(*irlockData))) {
+        uint16_t cksum = irlockData->signature + irlockData->posX + irlockData->posY + irlockData->sizeX + irlockData->sizeY;
+        if (irlockData->cksum == cksum) return true;
+    }
+    return false;
+}
+
+static bool deviceDetect(irlockDev_t *irlockDev)
+{
+    uint8_t buf;
+    bool detected = busRead(irlockDev->busDev, 0xFF, &buf);
+    return !!detected;
+}
+
+bool irlockDetect(irlockDev_t *irlockDev)
+{
+    irlockDev->busDev = busDeviceInit(BUSTYPE_I2C, DEVHW_IRLOCK, 0, OWNER_IRLOCK);
+    if (irlockDev->busDev == NULL) {
+        return false;
+    }
+
+    if (!deviceDetect(irlockDev)) {
+        busDeviceDeInit(irlockDev->busDev);
+        return false;
+    }
+
+    irlockDev->read = irlockRead;
+
+    return true;
+}
+
+bool irlockIsHealthy(void)
+{
+    return irlockHealthy;
+}
+
+#endif /* USE_IRLOCK */
diff --git a/src/main/drivers/irlock.h b/src/main/drivers/irlock.h
new file mode 100644
index 0000000000..a91a39552f
--- /dev/null
+++ b/src/main/drivers/irlock.h
@@ -0,0 +1,45 @@
+/*
+ * 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
+
+#include "drivers/sensor.h"
+#include "drivers/io_types.h"
+
+#if defined(USE_NAV) && defined(USE_IRLOCK)
+
+#define IRLOCK_RES_X 320
+#define IRLOCK_RES_Y 200
+
+typedef struct {
+    uint16_t cksum;
+    uint16_t signature;
+    uint16_t posX;
+    uint16_t posY;
+    uint16_t sizeX;
+    uint16_t sizeY;
+} irlockData_t;
+
+typedef struct irlockDev_s {
+    busDevice_t *busDev;
+    bool (*read)(struct irlockDev_s *irlockDev, irlockData_t *irlockData);
+} irlockDev_t;
+
+bool irlockDetect(irlockDev_t *irlockDev);
+bool irlockIsHealthy(void);
+
+#endif /* USE_IRLOCK */
diff --git a/src/main/drivers/resource.c b/src/main/drivers/resource.c
index 1169f15290..0f12ce3bc9 100644
--- a/src/main/drivers/resource.c
+++ b/src/main/drivers/resource.c
@@ -22,7 +22,7 @@ const char * const ownerNames[OWNER_TOTAL_COUNT] = {
     "RANGEFINDER", "SYSTEM", "SPI", "I2C", "SDCARD", "FLASH", "USB", "BEEPER", "OSD",
     "BARO", "MPU", "INVERTER", "LED STRIP", "LED", "RECEIVER", "TRANSMITTER",
     "NRF24", "VTX", "SPI_PREINIT", "COMPASS", "TEMPERATURE", "1-WIRE", "AIRSPEED", "OLED DISPLAY",
-    "PINIO"
+    "PINIO", "IRLOCK"
 };
 
 const char * const resourceNames[RESOURCE_TOTAL_COUNT] = {
diff --git a/src/main/drivers/resource.h b/src/main/drivers/resource.h
index fbbbbdba27..67153a61f2 100644
--- a/src/main/drivers/resource.h
+++ b/src/main/drivers/resource.h
@@ -56,6 +56,7 @@ typedef enum {
     OWNER_AIRSPEED,
     OWNER_OLED_DISPLAY,
     OWNER_PINIO,
+    OWNER_IRLOCK,
     OWNER_TOTAL_COUNT
 } resourceOwner_e;
 
diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c
index e8ba6f432a..d902eb7199 100755
--- a/src/main/fc/fc_tasks.c
+++ b/src/main/fc/fc_tasks.c
@@ -78,6 +78,7 @@
 #include "sensors/battery.h"
 #include "sensors/compass.h"
 #include "sensors/gyro.h"
+#include "sensors/irlock.h"
 #include "sensors/pitotmeter.h"
 #include "sensors/rangefinder.h"
 #include "sensors/opflow.h"
@@ -209,6 +210,14 @@ void taskUpdateRangefinder(timeUs_t currentTimeUs)
 }
 #endif
 
+#if defined(USE_NAV) && defined(USE_IRLOCK)
+void taskUpdateIrlock(timeUs_t currentTimeUs)
+{
+    UNUSED(currentTimeUs);
+    irlockUpdate();
+}
+#endif
+
 #ifdef USE_OPFLOW
 void taskUpdateOpticalFlow(timeUs_t currentTimeUs)
 {
@@ -347,6 +356,9 @@ void fcTasksInit(void)
 #ifdef USE_GLOBAL_FUNCTIONS
     setTaskEnabled(TASK_GLOBAL_FUNCTIONS, true);
 #endif
+#ifdef USE_IRLOCK
+    setTaskEnabled(TASK_IRLOCK, irlockHasBeenDetected());
+#endif
 }
 
 cfTask_t cfTasks[TASK_COUNT] = {
@@ -454,6 +466,15 @@ cfTask_t cfTasks[TASK_COUNT] = {
     },
 #endif
 
+#ifdef USE_IRLOCK
+    [TASK_IRLOCK] = {
+        .taskName = "IRLOCK",
+        .taskFunc = taskUpdateIrlock,
+        .desiredPeriod = TASK_PERIOD_HZ(100),
+        .staticPriority = TASK_PRIORITY_MEDIUM,
+    },
+#endif
+
 #ifdef USE_DASHBOARD
     [TASK_DASHBOARD] = {
         .taskName = "DASHBOARD",
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 65183912c0..95ca9fcde1 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -81,7 +81,8 @@ tables:
     values: ["NONE", "GYRO", "AGL", "FLOW_RAW",
       "FLOW", "SBUS", "FPORT", "ALWAYS", "SAG_COMP_VOLTAGE",
       "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC", "ITERM_RELAX",
-      "ERPM", "RPM_FILTER", "RPM_FREQ", "NAV_YAW", "DYNAMIC_FILTER", "DYNAMIC_FILTER_FREQUENCY"]
+      "ERPM", "RPM_FILTER", "RPM_FREQ", "NAV_YAW", "DYNAMIC_FILTER", "DYNAMIC_FILTER_FREQUENCY",
+      "IRLOCK"]
   - name: async_mode
     values: ["NONE", "GYRO", "ALL"]
   - name: aux_operator
diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h
index b9cf88bd21..a9d13ef5ef 100755
--- a/src/main/scheduler/scheduler.h
+++ b/src/main/scheduler/scheduler.h
@@ -118,6 +118,9 @@ typedef enum {
 #endif
 #ifdef USE_RPM_FILTER
     TASK_RPM_FILTER,
+#endif
+#ifdef USE_IRLOCK
+    TASK_IRLOCK,
 #endif
     /* Count of real tasks */
     TASK_COUNT,
diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c
index 394c627c69..274b76eb86 100755
--- a/src/main/sensors/initialisation.c
+++ b/src/main/sensors/initialisation.c
@@ -27,16 +27,17 @@
 #include "fc/config.h"
 #include "fc/runtime_config.h"
 
-#include "sensors/sensors.h"
 #include "sensors/acceleration.h"
 #include "sensors/barometer.h"
-#include "sensors/pitotmeter.h"
-#include "sensors/gyro.h"
 #include "sensors/compass.h"
-#include "sensors/rangefinder.h"
-#include "sensors/opflow.h"
-#include "sensors/temperature.h"
+#include "sensors/gyro.h"
 #include "sensors/initialisation.h"
+#include "sensors/irlock.h"
+#include "sensors/opflow.h"
+#include "sensors/pitotmeter.h"
+#include "sensors/rangefinder.h"
+#include "sensors/sensors.h"
+#include "sensors/temperature.h"
 
 uint8_t requestedSensors[SENSOR_INDEX_COUNT] = { GYRO_AUTODETECT, ACC_NONE, BARO_NONE, MAG_NONE, RANGEFINDER_NONE, PITOT_NONE, OPFLOW_NONE };
 uint8_t detectedSensors[SENSOR_INDEX_COUNT] = { GYRO_NONE, ACC_NONE, BARO_NONE, MAG_NONE, RANGEFINDER_NONE, PITOT_NONE, OPFLOW_NONE };
@@ -102,6 +103,10 @@ bool sensorsAutodetect(void)
     }
 #endif
 
+#ifdef USE_IRLOCK
+    irlockInit();
+#endif
+
     if (eepromUpdatePending) {
         writeEEPROM();
     }
diff --git a/src/main/sensors/irlock.c b/src/main/sensors/irlock.c
new file mode 100644
index 0000000000..186ae3d10f
--- /dev/null
+++ b/src/main/sensors/irlock.c
@@ -0,0 +1,136 @@
+/*
+ * 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 "stdbool.h"
+#include "stdint.h"
+#include <string.h>
+#include <math.h>
+
+#include "platform.h"
+
+#include "build/debug.h"
+
+#include "common/maths.h"
+#include "common/log.h"
+#include "common/printf.h"
+
+#include "drivers/irlock.h"
+#include "drivers/time.h"
+
+#include "fc/runtime_config.h"
+
+#include "flight/imu.h"
+
+#include "navigation/navigation.h"
+
+#include "sensors/sensors.h"
+#include "sensors/barometer.h"
+#include "sensors/irlock.h"
+
+
+#define IRLOCK_TIMEOUT 100
+
+#if defined(USE_NAV) && defined(USE_IRLOCK)
+
+enum {
+    DEBUG_IRLOCK_DETECTED,
+    DEBUG_IRLOCK_MEAS_VALID,
+    DEBUG_IRLOCK_POS_X,
+    DEBUG_IRLOCK_POS_Y
+};
+
+static irlockDev_t irlockDev;
+static bool irlockDetected = false;
+static bool measurementValid = false;
+static irlockData_t irlockData;
+static timeMs_t lastUpdateMs = 0;
+
+void irlockInit(void)
+{
+    irlockDetected = irlockDetect(&irlockDev);
+    DEBUG_SET(DEBUG_IRLOCK, DEBUG_IRLOCK_DETECTED, irlockDetected);
+}
+
+bool irlockHasBeenDetected(void)
+{
+    return irlockDetected;
+}
+
+bool irlockMeasurementIsValid(void) {
+    return measurementValid;
+}
+
+void irlockUpdate(void)
+{
+    if (irlockDetected && irlockDev.read(&irlockDev, &irlockData)) lastUpdateMs = millis();
+    measurementValid = millis() - lastUpdateMs < IRLOCK_TIMEOUT;
+
+    if (debugMode == DEBUG_IRLOCK) {
+        float distX, distY;
+        bool valid = irlockGetPosition(&distX, &distY);
+        debug[DEBUG_IRLOCK_MEAS_VALID] = valid;
+        debug[DEBUG_IRLOCK_POS_X] = lrintf(distX * 100);
+        debug[DEBUG_IRLOCK_POS_Y] = lrintf(distY * 100);
+    }
+}
+
+#define X_TO_DISTANCE_FACTOR -0.0029387573f
+#define X_TO_DISTANCE_OFFSET 0.4702011635f
+#define Y_TO_DISTANCE_FACTOR -0.0030568431f
+#define Y_TO_DISTANCE_OFFSET 0.3056843086f
+
+#define LENS_X_CORRECTION 4.4301355e-6f
+#define LENS_Y_CORRECTION 4.7933139e-6f
+
+// calculate distance relative to center at 1 meter distance from absolute object coordinates and taking into account lens distortion
+static void irlockCoordinatesToDistance(uint16_t pixX, uint16_t pixY, float *distX, float *distY)
+{
+    int16_t xCenterOffset = pixX - IRLOCK_RES_X / 2;
+    int16_t yCenterOffset = pixY - IRLOCK_RES_Y / 2;
+    float lensDistortionCorrectionFactor = LENS_X_CORRECTION * xCenterOffset * xCenterOffset + LENS_Y_CORRECTION * yCenterOffset * yCenterOffset - 1.0f;
+    *distX = (X_TO_DISTANCE_FACTOR * pixX + X_TO_DISTANCE_OFFSET) / lensDistortionCorrectionFactor;
+    *distY = (Y_TO_DISTANCE_FACTOR * pixY + Y_TO_DISTANCE_OFFSET) / lensDistortionCorrectionFactor;
+}
+
+bool irlockGetPosition(float *distX, float *distY)
+{
+    if (!measurementValid) return false;
+
+    // calculate edges of the object
+    int16_t corner1X = irlockData.posX - irlockData.sizeX / 2;
+    int16_t corner1Y = irlockData.posY - irlockData.sizeY / 2;
+    int16_t corner2X = irlockData.posX + irlockData.sizeX / 2;
+    int16_t corner2Y = irlockData.posY + irlockData.sizeY / 2;
+
+    // convert pixel position to distance
+    float corner1DistX, corner1DistY, corner2DistX, corner2DistY;
+    irlockCoordinatesToDistance(corner1X, corner1Y, &corner1DistX, &corner1DistY);
+    irlockCoordinatesToDistance(corner2X, corner2Y, &corner2DistX, &corner2DistY);
+
+    // return center of object
+    float uDistX = (corner1DistX + corner2DistX) / 2.0f; // lateral movement, positive means the aircraft is to the left of the beacon
+    float uDistY = (corner1DistY + corner2DistY) / 2.0f; // longitudinal movement, positive means the aircraft is in front of the beacon
+
+    // compensate for altitude and attitude
+    float altitude = CENTIMETERS_TO_METERS(getEstimatedActualPosition(Z));
+    *distX = altitude * tan_approx(atan2_approx(uDistX, 1.0f) - DECIDEGREES_TO_RADIANS(attitude.values.roll));
+    *distY = altitude * tan_approx(atan2_approx(uDistY, 1.0f) + DECIDEGREES_TO_RADIANS(attitude.values.pitch));
+
+    return true;
+}
+
+#endif /* USE_IRLOCK */
diff --git a/src/main/sensors/irlock.h b/src/main/sensors/irlock.h
new file mode 100644
index 0000000000..03270b6c5c
--- /dev/null
+++ b/src/main/sensors/irlock.h
@@ -0,0 +1,29 @@
+/*
+ * 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
+
+#if defined(USE_NAV) && defined(USE_IRLOCK)
+
+void irlockInit(void);
+bool irlockHasBeenDetected(void);
+
+void irlockUpdate(void);
+bool irlockMeasurementIsValid(void);
+bool irlockGetPosition(float *distX, float *distY);
+
+#endif /* USE_IRLOCK */
diff --git a/src/main/target/common_hardware.c b/src/main/target/common_hardware.c
index 22bee2465c..1e35d2baef 100755
--- a/src/main/target/common_hardware.c
+++ b/src/main/target/common_hardware.c
@@ -332,4 +332,13 @@
     #endif
 #endif
 
+#if defined(USE_IRLOCK) && defined(USE_I2C)
+    #if !defined(IRLOCK_I2C_BUS) && defined(EXTERNAL_I2C_BUS)
+        #define IRLOCK_I2C_BUS EXTERNAL_I2C_BUS
+    #else
+        #define IRLOCK_I2C_BUS BUS_I2C1
+    #endif
+    BUSDEV_REGISTER_I2C(busdev_irlock,      DEVHW_IRLOCK,       IRLOCK_I2C_BUS,     0x54,               NONE,           DEVFLAGS_USE_RAW_REGISTERS);
+#endif
+
 #endif  // USE_TARGET_HARDWARE_DESCRIPTORS

From b409b007d7c97d4f8a621964cf8cea95c6810360 Mon Sep 17 00:00:00 2001
From: Michel Pastor <shellixyz@gmail.com>
Date: Thu, 7 May 2020 00:37:49 +0200
Subject: [PATCH 147/179] Improve FrSky' CRC lib

---
 src/main/rx/fport.c     |  2 +-
 src/main/rx/frsky_crc.c | 17 ++++++++++++-----
 src/main/rx/frsky_crc.h |  5 ++---
 3 files changed, 15 insertions(+), 9 deletions(-)

diff --git a/src/main/rx/fport.c b/src/main/rx/fport.c
index 59f62fe3f5..4b607373c4 100644
--- a/src/main/rx/fport.c
+++ b/src/main/rx/fport.c
@@ -255,7 +255,7 @@ static uint8_t fportFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
         if (frameLength != bufferLength - 2) {
             reportFrameError(DEBUG_FPORT_ERROR_SIZE);
         } else {
-            if (!frskyChecksumIsGood(&rxBuffer[rxBufferReadIndex].data[0], bufferLength)) {
+            if (!frskyCheckSumIsGood(&rxBuffer[rxBufferReadIndex].data[0], bufferLength)) {
                 reportFrameError(DEBUG_FPORT_ERROR_CHECKSUM);
             } else {
                 fportFrame_t *frame = (fportFrame_t *)&rxBuffer[rxBufferReadIndex].data[1];
diff --git a/src/main/rx/frsky_crc.c b/src/main/rx/frsky_crc.c
index fa3934e574..f001e0cc2f 100644
--- a/src/main/rx/frsky_crc.c
+++ b/src/main/rx/frsky_crc.c
@@ -26,22 +26,29 @@ FILE_COMPILE_FOR_SPEED
 void frskyCheckSumStep(uint16_t *checksum, uint8_t byte)
 {
     *checksum += byte;
-    *checksum += (*checksum >> 8);
-    *checksum &= 0xFF;
 }
 
 void frskyCheckSumFini(uint16_t *checksum)
 {
+    while (*checksum > 0xFF) {
+        *checksum = (*checksum & 0xFF) + (*checksum >> 8);
+    }
+
     *checksum = 0xFF - *checksum;
 }
 
-bool frskyChecksumIsGood(uint8_t *data, uint8_t length)
+uint8_t frskyCheckSum(uint8_t *data, uint8_t length)
 {
     uint16_t checksum = 0;
     for (unsigned i = 0; i < length; i++) {
         frskyCheckSumStep(&checksum, *data++);
     }
-
-    return checksum == FRSKY_CHECKSUM_GOOD_VALUE;
+    frskyCheckSumFini(&checksum);
+    return checksum;
 }
 
+bool frskyCheckSumIsGood(uint8_t *data, uint8_t length)
+{
+    uint16_t checksum = frskyCheckSum(data, length);
+    return checksum == 0xFF;
+}
diff --git a/src/main/rx/frsky_crc.h b/src/main/rx/frsky_crc.h
index 849b53bfab..c0a026ba47 100644
--- a/src/main/rx/frsky_crc.h
+++ b/src/main/rx/frsky_crc.h
@@ -18,8 +18,7 @@
 #include <stdbool.h>
 #include <stdint.h>
 
-#define FRSKY_CHECKSUM_GOOD_VALUE 0xFF
-
-bool frskyChecksumIsGood(uint8_t *data, uint8_t length);
+uint8_t frskyCheckSum(uint8_t *data, uint8_t length);
+bool frskyCheckSumIsGood(uint8_t *data, uint8_t length);
 void frskyCheckSumStep(uint16_t *checksum, uint8_t byte);   // Add byte to checksum
 void frskyCheckSumFini(uint16_t *checksum);                 // Finalize checksum

From b38f0dfd4869fca0d0250becd3eff745e113aaa1 Mon Sep 17 00:00:00 2001
From: jihlein <jihlein@hotmail.com>
Date: Wed, 6 May 2020 18:20:10 -0600
Subject: [PATCH 148/179] Timer resource change to support independent output
 protocols for tricopter use.

---
 src/main/target/KAKUTEF4/target.c | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/main/target/KAKUTEF4/target.c b/src/main/target/KAKUTEF4/target.c
index 48082d288a..469c0d63b0 100755
--- a/src/main/target/KAKUTEF4/target.c
+++ b/src/main/target/KAKUTEF4/target.c
@@ -31,10 +31,10 @@
 const timerHardware_t timerHardware[] = {
     DEF_TIM(TIM8, CH2, PC7, TIM_USE_PPM,   0, 0), // PPM IN
 
-    DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1_OUT - DMA1_ST7
-    DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2_OUT - DMA1_ST2
-    DEF_TIM(TIM2, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S3_OUT - DMA1_ST6
-    DEF_TIM(TIM2, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4_OUT - DMA1_ST1
+    DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR |                    TIM_USE_FW_MOTOR, 0, 0), // S1_OUT - DMA1_ST7
+    DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR |                    TIM_USE_FW_MOTOR, 0, 0), // S2_OUT - DMA1_ST2
+    DEF_TIM(TIM2, CH4, PA3, TIM_USE_MC_MOTOR |                    TIM_USE_FW_SERVO, 0, 1), // S3_OUT - DMA1_ST6
+    DEF_TIM(TIM5, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S4_OUT - DMA1_ST1
 
 #if defined(KAKUTEF4V2)
     DEF_TIM(TIM8, CH3, PC8, TIM_USE_LED,   0, 0), // LED_STRIP - DMA2_ST2

From a6985fd393528bed860a1634efac75639fe43011 Mon Sep 17 00:00:00 2001
From: Michel Pastor <shellixyz@gmail.com>
Date: Thu, 7 May 2020 15:42:05 +0200
Subject: [PATCH 149/179] Fix OSD hang due to compiler optimization bug

Introduced by fce005f

Fixes #5690
---
 src/main/drivers/max7456.c | 2 --
 1 file changed, 2 deletions(-)

diff --git a/src/main/drivers/max7456.c b/src/main/drivers/max7456.c
index 964166b14b..405db287c1 100644
--- a/src/main/drivers/max7456.c
+++ b/src/main/drivers/max7456.c
@@ -22,8 +22,6 @@
 
 #include "platform.h"
 
-FILE_COMPILE_FOR_SPEED
-
 #ifdef USE_MAX7456
 
 #include "common/bitarray.h"

From e02b8cac39788f91ab430cac027a9d165010e878 Mon Sep 17 00:00:00 2001
From: joleeee <jolejonaseh@gmail.com>
Date: Fri, 8 May 2020 16:33:26 +0200
Subject: [PATCH 150/179] Replace PASSTHROUGH with MANUAL

Replace PASSTHROUGH with MANUAL in Autotune - fixedwing.md
---
 docs/Autotune - fixedwing.md | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/docs/Autotune - fixedwing.md b/docs/Autotune - fixedwing.md
index e3d9c11966..3579e8e58d 100755
--- a/docs/Autotune - fixedwing.md	
+++ b/docs/Autotune - fixedwing.md	
@@ -30,7 +30,7 @@ For most hobby-sized airplanes roll/pitch rate limits should be in range 70-120
 
 Other things to check:
 
-* It's highly recommended that you fly in PASTHROUGH and trim your servo midpoints for stable flight
+* It's highly recommended that you fly in MANUAL and trim your servo midpoints for stable flight
 * Make sure you have center of gravity according to manual to your aircraft
 * Check that your failsafe activates correctly (test on the ground with propeller off for safety)
 
@@ -54,6 +54,6 @@ The more you fly the better it will get. Let autotune analyze how your airplane
 
 ## Completing the tune
 
-Once you have tuned reasonable PIFF parameters with AUTOTUNE you should complete the tune by switching out of AUTOTUNE to ANGLE or PASTHROUGH and landing the airplane.
+Once you have tuned reasonable PIFF parameters with AUTOTUNE you should complete the tune by switching out of AUTOTUNE to ANGLE or MANUAL and landing the airplane.
 
 Note that AUTOTUNE mode doesn't automatically save parameters to EEPROM. You need to disarm and issue a [stick command](Controls.md) to save configuration parameters.

From c0a87cdcc2db00a84ee6fad1d5a2469a93f030bb Mon Sep 17 00:00:00 2001
From: Konstantin Sharlaimov <konstantin.sharlaimov@gmail.com>
Date: Sat, 9 May 2020 09:13:51 +0200
Subject: [PATCH 151/179] Revert "Improve FrSky's CRC lib"

---
 src/main/rx/fport.c     |  2 +-
 src/main/rx/frsky_crc.c | 17 +++++------------
 src/main/rx/frsky_crc.h |  5 +++--
 3 files changed, 9 insertions(+), 15 deletions(-)

diff --git a/src/main/rx/fport.c b/src/main/rx/fport.c
index 4b607373c4..59f62fe3f5 100644
--- a/src/main/rx/fport.c
+++ b/src/main/rx/fport.c
@@ -255,7 +255,7 @@ static uint8_t fportFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
         if (frameLength != bufferLength - 2) {
             reportFrameError(DEBUG_FPORT_ERROR_SIZE);
         } else {
-            if (!frskyCheckSumIsGood(&rxBuffer[rxBufferReadIndex].data[0], bufferLength)) {
+            if (!frskyChecksumIsGood(&rxBuffer[rxBufferReadIndex].data[0], bufferLength)) {
                 reportFrameError(DEBUG_FPORT_ERROR_CHECKSUM);
             } else {
                 fportFrame_t *frame = (fportFrame_t *)&rxBuffer[rxBufferReadIndex].data[1];
diff --git a/src/main/rx/frsky_crc.c b/src/main/rx/frsky_crc.c
index f001e0cc2f..fa3934e574 100644
--- a/src/main/rx/frsky_crc.c
+++ b/src/main/rx/frsky_crc.c
@@ -26,29 +26,22 @@ FILE_COMPILE_FOR_SPEED
 void frskyCheckSumStep(uint16_t *checksum, uint8_t byte)
 {
     *checksum += byte;
+    *checksum += (*checksum >> 8);
+    *checksum &= 0xFF;
 }
 
 void frskyCheckSumFini(uint16_t *checksum)
 {
-    while (*checksum > 0xFF) {
-        *checksum = (*checksum & 0xFF) + (*checksum >> 8);
-    }
-
     *checksum = 0xFF - *checksum;
 }
 
-uint8_t frskyCheckSum(uint8_t *data, uint8_t length)
+bool frskyChecksumIsGood(uint8_t *data, uint8_t length)
 {
     uint16_t checksum = 0;
     for (unsigned i = 0; i < length; i++) {
         frskyCheckSumStep(&checksum, *data++);
     }
-    frskyCheckSumFini(&checksum);
-    return checksum;
+
+    return checksum == FRSKY_CHECKSUM_GOOD_VALUE;
 }
 
-bool frskyCheckSumIsGood(uint8_t *data, uint8_t length)
-{
-    uint16_t checksum = frskyCheckSum(data, length);
-    return checksum == 0xFF;
-}
diff --git a/src/main/rx/frsky_crc.h b/src/main/rx/frsky_crc.h
index c0a026ba47..849b53bfab 100644
--- a/src/main/rx/frsky_crc.h
+++ b/src/main/rx/frsky_crc.h
@@ -18,7 +18,8 @@
 #include <stdbool.h>
 #include <stdint.h>
 
-uint8_t frskyCheckSum(uint8_t *data, uint8_t length);
-bool frskyCheckSumIsGood(uint8_t *data, uint8_t length);
+#define FRSKY_CHECKSUM_GOOD_VALUE 0xFF
+
+bool frskyChecksumIsGood(uint8_t *data, uint8_t length);
 void frskyCheckSumStep(uint16_t *checksum, uint8_t byte);   // Add byte to checksum
 void frskyCheckSumFini(uint16_t *checksum);                 // Finalize checksum

From 296d6058e9b5728696904b8cadb492c62ff4aaef Mon Sep 17 00:00:00 2001
From: "Konstantin (DigitalEntity) Sharlaimov"
 <konstantin.sharlaimov@gmail.com>
Date: Sat, 9 May 2020 12:09:42 +0200
Subject: [PATCH 152/179] Revert c00c24924d6ec0f3922827c9f9621bab6d557bf1 that
 broke biquad notch

---
 src/main/common/filter.c | 5 +++--
 src/main/common/filter.h | 2 +-
 2 files changed, 4 insertions(+), 3 deletions(-)

diff --git a/src/main/common/filter.c b/src/main/common/filter.c
index 3384a1188c..a436534256 100644
--- a/src/main/common/filter.c
+++ b/src/main/common/filter.c
@@ -118,9 +118,10 @@ float rateLimitFilterApply4(rateLimitFilter_t *filter, float input, float rate_l
     return filter->state;
 }
 
-float filterGetNotchQ(uint16_t centerFrequencyHz, uint16_t cutoffFrequencyHz)
+float filterGetNotchQ(uint16_t centerFreq, uint16_t cutoff)
 {
-    return centerFrequencyHz * cutoffFrequencyHz / (centerFrequencyHz * centerFrequencyHz - cutoffFrequencyHz * cutoffFrequencyHz);
+    const float octaves = log2f((float)centerFreq  / (float)cutoff) * 2;
+    return sqrtf(powf(2, octaves)) / (powf(2, octaves) - 1);
 }
 
 void biquadFilterInitNotch(biquadFilter_t *filter, uint32_t samplingIntervalUs, uint16_t filterFreq, uint16_t cutoffHz)
diff --git a/src/main/common/filter.h b/src/main/common/filter.h
index 985763e459..5ce181dba6 100644
--- a/src/main/common/filter.h
+++ b/src/main/common/filter.h
@@ -79,7 +79,7 @@ void biquadFilterInit(biquadFilter_t *filter, uint16_t filterFreq, uint32_t samp
 float biquadFilterApply(biquadFilter_t *filter, float sample);
 float biquadFilterReset(biquadFilter_t *filter, float value);
 float biquadFilterApplyDF1(biquadFilter_t *filter, float input);
-float filterGetNotchQ(uint16_t centerFrequencyHz, uint16_t cutoffFrequencyHz);
+float filterGetNotchQ(uint16_t centerFreq, uint16_t cutoff);
 void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType);
 
 void firFilterInit(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs);

From 308e0723baf2eb0f2a6bfe3e18264b0078fafd0a Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Sat, 9 May 2020 13:50:30 +0200
Subject: [PATCH 153/179] Revert "Merge pull request #5700 from
 iNavFlight/de_revert_notch_q"

This reverts commit 33319a145df38f840d25997f1b98953c5b1ad58f, reversing
changes made to 5d7904be8936b9d2245c1784b440741f5e3ca255.
---
 src/main/common/filter.c | 5 ++---
 src/main/common/filter.h | 2 +-
 2 files changed, 3 insertions(+), 4 deletions(-)

diff --git a/src/main/common/filter.c b/src/main/common/filter.c
index a436534256..3384a1188c 100644
--- a/src/main/common/filter.c
+++ b/src/main/common/filter.c
@@ -118,10 +118,9 @@ float rateLimitFilterApply4(rateLimitFilter_t *filter, float input, float rate_l
     return filter->state;
 }
 
-float filterGetNotchQ(uint16_t centerFreq, uint16_t cutoff)
+float filterGetNotchQ(uint16_t centerFrequencyHz, uint16_t cutoffFrequencyHz)
 {
-    const float octaves = log2f((float)centerFreq  / (float)cutoff) * 2;
-    return sqrtf(powf(2, octaves)) / (powf(2, octaves) - 1);
+    return centerFrequencyHz * cutoffFrequencyHz / (centerFrequencyHz * centerFrequencyHz - cutoffFrequencyHz * cutoffFrequencyHz);
 }
 
 void biquadFilterInitNotch(biquadFilter_t *filter, uint32_t samplingIntervalUs, uint16_t filterFreq, uint16_t cutoffHz)
diff --git a/src/main/common/filter.h b/src/main/common/filter.h
index 5ce181dba6..985763e459 100644
--- a/src/main/common/filter.h
+++ b/src/main/common/filter.h
@@ -79,7 +79,7 @@ void biquadFilterInit(biquadFilter_t *filter, uint16_t filterFreq, uint32_t samp
 float biquadFilterApply(biquadFilter_t *filter, float sample);
 float biquadFilterReset(biquadFilter_t *filter, float value);
 float biquadFilterApplyDF1(biquadFilter_t *filter, float input);
-float filterGetNotchQ(uint16_t centerFreq, uint16_t cutoff);
+float filterGetNotchQ(uint16_t centerFrequencyHz, uint16_t cutoffFrequencyHz);
 void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType);
 
 void firFilterInit(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs);

From b3e0ea3c45f4d9488fd6eb4d82c2394f1743cd2a Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Sat, 9 May 2020 14:04:52 +0200
Subject: [PATCH 154/179] Fix static notch initialization

---
 src/main/common/filter.c             | 2 +-
 src/main/common/filter.h             | 2 +-
 src/main/flight/dynamic_gyro_notch.c | 9 ++++-----
 src/main/flight/dynamic_gyro_notch.h | 1 -
 4 files changed, 6 insertions(+), 8 deletions(-)

diff --git a/src/main/common/filter.c b/src/main/common/filter.c
index 3384a1188c..c2eb1cf4cc 100644
--- a/src/main/common/filter.c
+++ b/src/main/common/filter.c
@@ -118,7 +118,7 @@ float rateLimitFilterApply4(rateLimitFilter_t *filter, float input, float rate_l
     return filter->state;
 }
 
-float filterGetNotchQ(uint16_t centerFrequencyHz, uint16_t cutoffFrequencyHz)
+float filterGetNotchQ(float centerFrequencyHz, float cutoffFrequencyHz)
 {
     return centerFrequencyHz * cutoffFrequencyHz / (centerFrequencyHz * centerFrequencyHz - cutoffFrequencyHz * cutoffFrequencyHz);
 }
diff --git a/src/main/common/filter.h b/src/main/common/filter.h
index 985763e459..fadae7ba48 100644
--- a/src/main/common/filter.h
+++ b/src/main/common/filter.h
@@ -79,7 +79,7 @@ void biquadFilterInit(biquadFilter_t *filter, uint16_t filterFreq, uint32_t samp
 float biquadFilterApply(biquadFilter_t *filter, float sample);
 float biquadFilterReset(biquadFilter_t *filter, float value);
 float biquadFilterApplyDF1(biquadFilter_t *filter, float input);
-float filterGetNotchQ(uint16_t centerFrequencyHz, uint16_t cutoffFrequencyHz);
+float filterGetNotchQ(float centerFrequencyHz, float cutoffFrequencyHz);
 void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType);
 
 void firFilterInit(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs);
diff --git a/src/main/flight/dynamic_gyro_notch.c b/src/main/flight/dynamic_gyro_notch.c
index f45b3b6c09..7e6c12afbc 100644
--- a/src/main/flight/dynamic_gyro_notch.c
+++ b/src/main/flight/dynamic_gyro_notch.c
@@ -40,15 +40,14 @@ void dynamicGyroNotchFiltersInit(dynamicGyroNotchState_t *state) {
     state->looptime = getLooptime();
 
     if (state->enabled) {
-        const float notchQ = filterGetNotchQ(DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, DYNAMIC_NOTCH_DEFAULT_CUTOFF_HZ); // any defaults OK here
-
         /*
          * Step 1 - init all filters even if they will not be used further down the road
          */
         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-            biquadFilterInit(&state->filters[axis][0], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, state->looptime, notchQ, FILTER_NOTCH);
-            biquadFilterInit(&state->filters[axis][1], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, state->looptime, notchQ, FILTER_NOTCH);
-            biquadFilterInit(&state->filters[axis][2], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, state->looptime, notchQ, FILTER_NOTCH);
+            //Any initial notch Q is valid sice it will be updated immediately after
+            biquadFilterInit(&state->filters[axis][0], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, state->looptime, 1.0f, FILTER_NOTCH);
+            biquadFilterInit(&state->filters[axis][1], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, state->looptime, 1.0f, FILTER_NOTCH);
+            biquadFilterInit(&state->filters[axis][2], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, state->looptime, 1.0f, FILTER_NOTCH);
         }
 
         state->filtersApplyFn = (filterApplyFnPtr)biquadFilterApplyDF1;
diff --git a/src/main/flight/dynamic_gyro_notch.h b/src/main/flight/dynamic_gyro_notch.h
index f6ae3b7779..87d923fb06 100644
--- a/src/main/flight/dynamic_gyro_notch.h
+++ b/src/main/flight/dynamic_gyro_notch.h
@@ -29,7 +29,6 @@
 #include "common/filter.h"
 
 #define DYNAMIC_NOTCH_DEFAULT_CENTER_HZ 350
-#define DYNAMIC_NOTCH_DEFAULT_CUTOFF_HZ 300
 
 typedef struct dynamicGyroNotchState_s {
     uint16_t frequency[XYZ_AXIS_COUNT];

From 214f7a7ccb0b4f6e8272fd11b54f0cd48db1a8d1 Mon Sep 17 00:00:00 2001
From: Michel Pastor <shellixyz@gmail.com>
Date: Sat, 9 May 2020 17:09:16 +0200
Subject: [PATCH 155/179] Fix flash log partition erasing

---
 src/main/drivers/flash.c        | 12 ++++++++----
 src/main/drivers/flash.h        |  2 +-
 src/main/drivers/flash_m25p16.c |  2 +-
 3 files changed, 10 insertions(+), 6 deletions(-)

diff --git a/src/main/drivers/flash.c b/src/main/drivers/flash.c
index 13a4a114e2..67669ade06 100644
--- a/src/main/drivers/flash.c
+++ b/src/main/drivers/flash.c
@@ -64,10 +64,10 @@ bool flashIsReady(void)
     return false;
 }
 
-bool flashWaitForReady(void)
+bool flashWaitForReady(uint32_t timeoutMillis)
 {
 #ifdef USE_FLASH_M25P16
-    return m25p16_waitForReady(10);
+    return m25p16_waitForReady(timeoutMillis);
 #endif
     return false;
 }
@@ -281,8 +281,12 @@ uint32_t flashPartitionSize(flashPartition_t *partition)
 
 void flashPartitionErase(flashPartition_t *partition)
 {
-    for (uint16_t sector = partition->startSector; sector <= partition->endSector; ++sector) {
-        flashEraseSector(sector);
+    const flashGeometry_t * const geometry = flashGetGeometry();
+
+    for (unsigned i = partition->startSector; i <= partition->endSector; i++) {
+        uint32_t flashAddress = geometry->sectorSize * i;
+        flashEraseSector(flashAddress);
+        flashWaitForReady(0);
     }
 }
 #endif // USE_FLASH_CHIP
diff --git a/src/main/drivers/flash.h b/src/main/drivers/flash.h
index 119b391baa..2a608fcb4d 100644
--- a/src/main/drivers/flash.h
+++ b/src/main/drivers/flash.h
@@ -39,7 +39,7 @@ typedef struct flashGeometry_s {
 bool flashInit(void);
 
 bool flashIsReady(void);
-bool flashWaitForReady(void);
+bool flashWaitForReady(uint32_t timeoutMillis);
 void flashEraseSector(uint32_t address);
 void flashEraseCompletely(void);
 #if 0
diff --git a/src/main/drivers/flash_m25p16.c b/src/main/drivers/flash_m25p16.c
index 35d0a75505..c2529b436b 100644
--- a/src/main/drivers/flash_m25p16.c
+++ b/src/main/drivers/flash_m25p16.c
@@ -118,7 +118,7 @@ bool m25p16_waitForReady(uint32_t timeoutMillis)
 {
     uint32_t time = millis();
     while (!m25p16_isReady()) {
-        if (millis() - time > timeoutMillis) {
+        if (timeoutMillis && (millis() - time > timeoutMillis)) {
             return false;
         }
     }

From 8afad90df67e03e632a73b1928c94ff9b249bc9d Mon Sep 17 00:00:00 2001
From: Andi Kanzler <andi_kanzler@gmx.de>
Date: Mon, 11 May 2020 00:21:35 +0200
Subject: [PATCH 156/179] Basic Smartaudio 2.1 Support

---
 src/main/io/vtx_smartaudio.c | 295 +++++++++++++++++++++++------------
 src/main/io/vtx_smartaudio.h |  23 ++-
 2 files changed, 212 insertions(+), 106 deletions(-)

diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c
index a23d590201..a89dc2796e 100644
--- a/src/main/io/vtx_smartaudio.c
+++ b/src/main/io/vtx_smartaudio.c
@@ -24,9 +24,11 @@
 #include <stdint.h>
 #include <string.h>
 #include <ctype.h>
+#include <math.h>
 
 #include "platform.h"
 
+// Todo: Comment in before commit!
 #if defined(USE_VTX_SMARTAUDIO) && defined(USE_VTX_CONTROL)
 
 #include "build/debug.h"
@@ -37,6 +39,7 @@
 #include "common/maths.h"
 #include "common/printf.h"
 #include "common/utils.h"
+#include "common/typeconversion.h"
 
 #include "drivers/time.h"
 #include "drivers/vtx_common.h"
@@ -56,19 +59,23 @@
 
 static serialPort_t *smartAudioSerialPort = NULL;
 
-const char * const saPowerNames[VTX_SMARTAUDIO_POWER_COUNT+1] = {
-    "---", "25 ", "200", "500", "800",
+uint8_t saPowerCount = VTX_SMARTAUDIO_DEFAULT_POWER_COUNT;
+const char * saPowerNames[VTX_SMARTAUDIO_MAX_POWER_COUNT + 1] = {
+    "----", "25  ", "200 ", "500 ", "800 ", "    "
 };
 
+// Save powerlevels reported from SA 2.1 devices here
+char sa21PowerNames[VTX_SMARTAUDIO_MAX_POWER_COUNT][5];
+
 static const vtxVTable_t saVTable;    // Forward
 static vtxDevice_t vtxSmartAudio = {
     .vTable = &saVTable,
     .capability.bandCount = VTX_SMARTAUDIO_BAND_COUNT,
     .capability.channelCount = VTX_SMARTAUDIO_CHANNEL_COUNT,
-    .capability.powerCount = VTX_SMARTAUDIO_POWER_COUNT,
+    .capability.powerCount = VTX_SMARTAUDIO_MAX_POWER_COUNT,
     .capability.bandNames = (char **)vtx58BandNames,
     .capability.channelNames = (char **)vtx58ChannelNames,
-    .capability.powerNames = (char **)saPowerNames,
+    .capability.powerNames = (char**)saPowerNames
 };
 
 // SmartAudio command and response codes
@@ -110,22 +117,25 @@ smartAudioStat_t saStat = {
     .badcode = 0,
 };
 
-saPowerTable_t saPowerTable[VTX_SMARTAUDIO_POWER_COUNT] = {
-    {  25,   7,   0 },
-    { 200,  16,   1 },
-    { 500,  25,   2 },
-    { 800,  40,   3 },
+// Fill table with standard values for SA 1.0 and 2.0
+saPowerTable_t saPowerTable[VTX_SMARTAUDIO_MAX_POWER_COUNT] = {
+    {  25,   7 },
+    { 200,  16 },
+    { 500,  25 },
+    { 800,  40 },
+    {   0,   0 } // Placeholder
 };
 
 // Last received device ('hard') states
 
 smartAudioDevice_t saDevice = {
-    .version = 0,
+    .version = SA_UNKNOWN,
     .channel = -1,
     .power = -1,
     .mode = 0,
     .freq = 0,
     .orfreq = 0,
+    .willBootIntoPitMode = false
 };
 
 static smartAudioDevice_t saDevicePrev = {
@@ -139,7 +149,7 @@ static uint8_t saLockMode = SA_MODE_SET_UNLOCK; // saCms variable?
 bool saDeferred = true; // saCms variable?
 
 // Receive frame reassembly buffer
-#define SA_MAX_RCVLEN 15
+#define SA_MAX_RCVLEN 21
 static uint8_t sa_rbuf[SA_MAX_RCVLEN+4]; // XXX delete 4 byte guard
 
 //
@@ -183,18 +193,31 @@ static void saPrintSettings(void)
     LOG_D(VTX, "freq: %d ", saDevice.freq);
     LOG_D(VTX, "power: %d ", saDevice.power);
     LOG_D(VTX, "pitfreq: %d ", saDevice.orfreq);
+    LOG_D(VTX, "BootIntoPitMode: %s", saDevice.willBootIntoPitMode ? "yes" : "no");
 }
 
 int saDacToPowerIndex(int dac)
 {
-    for (int idx = VTX_SMARTAUDIO_POWER_COUNT - 1 ; idx >= 0 ; idx--) {
-        if (saPowerTable[idx].valueV1 <= dac) {
+    for (int idx = saPowerCount - 1 ; idx >= 0 ; idx--) {
+        if (saPowerTable[idx].dbi <= dac) {
             return idx;
         }
     }
     return 0;
 }
 
+int saDbiToMw(uint16_t dbi) {
+
+    uint16_t mw = (uint16_t)pow(10.0, dbi / 10.0);
+
+    if (dbi > 14) {
+        // For powers greater than 25mW round up to a multiple of 50 to match expectations
+        mw = 50 * ((mw + 25) / 50);
+     }
+
+    return mw;
+}
+
 //
 // Autobauding
 //
@@ -272,14 +295,67 @@ static void saProcessResponse(uint8_t *buf, int len)
             break;
         }
 
-        // XXX(fujin): For now handle V21 saDevice.version as '2'.
         // From spec: "Bit 7-3 is holding the Smart audio version where 0 is V1, 1 is V2, 2 is V2.1"
-        saDevice.version = (buf[0] == SA_CMD_GET_SETTINGS) ? 1 : 2;
+        // saDevice.version = 0 means unknown, 1 means Smart audio V1, 2 means Smart audio V2 and 3 means Smart audio V2.1
+        saDevice.version = (buf[0] == SA_CMD_GET_SETTINGS) ? 1 : ((buf[0] == SA_CMD_GET_SETTINGS_V2) ? 2 : 3);
         saDevice.channel = buf[2];
-        saDevice.power = buf[3];
+        uint8_t rawPowerValue = buf[3];
         saDevice.mode = buf[4];
-        saDevice.freq = (buf[5] << 8)|buf[6];
-        // XXX(fujin): Receive additional SA2.1 fields here for dBm based power level.
+        saDevice.freq = (buf[5] << 8) | buf[6];
+
+        // read pir and por flags to detect if the device will boot into pitmode.
+        // note that "quit pitmode without unsetting the pitmode flag" clears pir and por flags but the device will still boot into pitmode.
+        // therefore we ignore the pir and por flags while the device is not in pitmode
+        // actually, this is the whole reason the variable saDevice.willBootIntoPitMode exists.
+        // otherwise we could use saDevice.mode directly
+        if (saDevice.mode & SA_MODE_GET_PITMODE) {
+            bool newBootMode = (saDevice.mode & SA_MODE_GET_IN_RANGE_PITMODE) || (saDevice.mode & SA_MODE_GET_OUT_RANGE_PITMODE);
+            if (newBootMode != saDevice.willBootIntoPitMode) {
+                LOG_D(VTX, "saProcessResponse: willBootIntoPitMode is now %s\r\n", newBootMode  ? "true" : "false");
+            }
+            saDevice.willBootIntoPitMode = newBootMode;
+        }
+
+        if(saDevice.version == SA_2_1) {
+            //read dbm based power levels
+            if(len < 10) { //current power level in dbm field missing or power level length field missing or zero power levels reported
+                LOG_D(VTX, "processResponse: V2.1 vtx didn't report any power levels\r\n");
+                break;
+            }
+            saPowerCount = constrain((int8_t)buf[8], 0, VTX_SMARTAUDIO_MAX_POWER_COUNT);
+            vtxSmartAudio.capability.powerCount = saPowerCount;
+            //SmartAudio seems to report buf[8] + 1 power levels, but one of them is zero.
+            //zero is indeed a valid power level to set the vtx to, but it activates pit mode.
+            //crucially, after sending 0 dbm, the vtx does NOT report its power level to be 0 dbm.
+            //instead, it reports whatever value was set previously and it reports to be in pit mode.
+            //for this reason, zero shouldn't be used as a normal power level in iNav.
+            for (int8_t i = 0; i < saPowerCount; i++ ) {
+                saPowerTable[i].dbi = buf[9 + i + 1]; //+ 1 to skip the first power level, as mentioned above
+                saPowerTable[i].mW = saDbiToMw(saPowerTable[i].dbi);
+                if (i <= VTX_SMARTAUDIO_MAX_POWER_COUNT) {
+                    char strbuf[5];
+                    itoa(saPowerTable[i].mW, strbuf, 10);
+                    strcpy(sa21PowerNames[i], strbuf);
+                    saPowerNames[i + 1] = sa21PowerNames[i];
+                }
+            }
+
+            LOG_D(VTX, "processResponse: %d power values: %d, %d, %d, %d\r\n",
+                    saPowerCount, saPowerTable[0].dbi, saPowerTable[1].dbi,
+                    saPowerTable[2].dbi, saPowerTable[3].dbi);
+            //LOG_D(VTX, "processResponse: V2.1 received vtx power value %d\r\n",buf[7]);
+            rawPowerValue = buf[7];
+
+            saDevice.power = 0; //set to unknown power level if the reported one doesnt match any of the known ones
+            LOG_D(VTX, "processResponse: rawPowerValue is %d, legacy power is %d\r\n", rawPowerValue, buf[3]);
+            for (int8_t i = 0; i < saPowerCount; i++) {
+                if (rawPowerValue == saPowerTable[i].dbi) {
+                    saDevice.power = i + 1;
+                }
+            }
+        } else {
+            saDevice.power = rawPowerValue + 1;
+        }
 
         DEBUG_SET(DEBUG_SMARTAUDIO, 0, saDevice.version * 100 + saDevice.mode);
         DEBUG_SET(DEBUG_SMARTAUDIO, 1, saDevice.channel);
@@ -288,14 +364,6 @@ static void saProcessResponse(uint8_t *buf, int len)
         break;
 
     case SA_CMD_SET_POWER: // Set Power
-        if (len == 5) {
-            // Improperly implemented S.Audio devices that
-            // omit the channel
-            saDevice.power = buf[2];
-        } else if (len >= 6) {
-            // S.Audio v2 spec
-            saDevice.power = buf[3];
-        }
         break;
 
     case SA_CMD_SET_CHAN: // Set Channel
@@ -321,7 +389,9 @@ static void saProcessResponse(uint8_t *buf, int len)
         break;
 
     case SA_CMD_SET_MODE: // Set Mode
-        LOG_D(VTX, "saProcessResponse: SET_MODE 0x%x", buf[2]);
+        LOG_D(VTX, "saProcessResponse: SET_MODE 0x%x, (pir %s, por %s, pitdsbl %s, %s)\r\n",
+            buf[2], (buf[2] & 1) ? "on" : "off", (buf[2] & 2) ? "on" : "off", (buf[3] & 4) ? "on" : "off",
+            (buf[4] & 8) ? "unlocked" : "locked");
         break;
 
     default:
@@ -330,18 +400,11 @@ static void saProcessResponse(uint8_t *buf, int len)
     }
 
     if (memcmp(&saDevice, &saDevicePrev, sizeof(smartAudioDevice_t))) {
-#ifdef USE_CMS    //if changes then trigger saCms update
-        //saCmsResetOpmodel();
-#endif
-    // Debug
-    saPrintSettings();
+        // Debug
+        saPrintSettings();
     }
-    saDevicePrev = saDevice;
 
-#ifdef USE_CMS
-    // Export current device status for CMS
-    //saCmsUpdate();
-#endif
+    saDevicePrev = saDevice;
 }
 
 //
@@ -536,10 +599,11 @@ static void saGetSettings(void)
 {
     static uint8_t bufGetSettings[5] = {0xAA, 0x55, SACMD(SA_CMD_GET_SETTINGS), 0x00, 0x9F};
 
+    LOG_D(VTX, "smartAudioGetSettings\r\n");
     saQueueCmd(bufGetSettings, 5);
 }
 
-static void saDoDevSetFreq(uint16_t freq)
+void saSetFreq(uint16_t freq)
 {
     static uint8_t buf[7] = { 0xAA, 0x55, SACMD(SA_CMD_SET_FREQ), 2 };
     static uint8_t switchBuf[7];
@@ -572,14 +636,9 @@ static void saDoDevSetFreq(uint16_t freq)
     saQueueCmd(buf, 7);
 }
 
-void saSetFreq(uint16_t freq)
-{
-    saDoDevSetFreq(freq);
-}
-
 void saSetPitFreq(uint16_t freq)
 {
-    saDoDevSetFreq(freq | SA_FREQ_SETPIT);
+    saSetFreq(freq | SA_FREQ_SETPIT);
 }
 
 #if 0
@@ -592,58 +651,35 @@ static void saGetPitFreq(void)
 static bool saValidateBandAndChannel(uint8_t band, uint8_t channel)
 {
     return (band >= VTX_SMARTAUDIO_MIN_BAND && band <= VTX_SMARTAUDIO_MAX_BAND &&
-             channel >= VTX_SMARTAUDIO_MIN_CHANNEL && channel <= VTX_SMARTAUDIO_MAX_CHANNEL);
+            channel >= VTX_SMARTAUDIO_MIN_CHANNEL && channel <= VTX_SMARTAUDIO_MAX_CHANNEL);
 }
 
-static void saDevSetBandAndChannel(uint8_t band, uint8_t channel)
+void saSetBandAndChannel(uint8_t band, uint8_t channel)
 {
     static uint8_t buf[6] = { 0xAA, 0x55, SACMD(SA_CMD_SET_CHAN), 1 };
 
     buf[4] = SA_BANDCHAN_TO_DEVICE_CHVAL(band, channel);
     buf[5] = CRC8(buf, 5);
+    LOG_D(VTX, "vtxSASetBandAndChannel set index band %d channel %d value sent 0x%x\r\n", band, channel, buf[4]);
 
+    //this will clear saDevice.mode & SA_MODE_GET_FREQ_BY_FREQ
     saQueueCmd(buf, 6);
 }
 
-void saSetBandAndChannel(uint8_t band, uint8_t channel)
-{
-    saDevSetBandAndChannel(band, channel);
-}
-
 void saSetMode(int mode)
 {
     static uint8_t buf[6] = { 0xAA, 0x55, SACMD(SA_CMD_SET_MODE), 1 };
 
     buf[4] = (mode & 0x3f) | saLockMode;
-    buf[5] = CRC8(buf, 5);
-
-    saQueueCmd(buf, 6);
-}
-
-static void saDevSetPowerByIndex(uint8_t index)
-{
-    static uint8_t buf[6] = { 0xAA, 0x55, SACMD(SA_CMD_SET_POWER), 1 };
-
-    LOG_D(VTX, "saSetPowerByIndex: index %d", index);
-
-    if (index == 0) {
-        // SmartAudio doesn't support power off.
-        return;
+    if (saDevice.version >= SA_2_1 && (mode & SA_MODE_CLR_PITMODE) &&
+        ((mode & SA_MODE_SET_IN_RANGE_PITMODE) || (mode & SA_MODE_SET_OUT_RANGE_PITMODE))) {
+        saDevice.willBootIntoPitMode = true;//quit pitmode without unsetting flag.
+        //the response will just say pit=off but the device will still go into pitmode on reboot.
+        //therefore we have to memorize this change here.
     }
+    LOG_D(VTX, "saSetMode(0x%x): pir=%s por=%s pitdsbl=%s %s\r\n", mode, (mode & 1) ? "on " : "off", (mode & 2) ? "on " : "off",
+            (mode & 4)? "on " : "off", (mode & 8) ? "locked" : "unlocked");
 
-    if (index > VTX_SMARTAUDIO_POWER_COUNT) {
-        // Invalid power level
-        return;
-    }
-
-    if (saDevice.version == 0) {
-        // Unknown or yet unknown version.
-        return;
-    }
-
-    unsigned entry = index - 1;
-
-    buf[4] = (saDevice.version == 1) ? saPowerTable[entry].valueV1 : saPowerTable[entry].valueV2;
     buf[5] = CRC8(buf, 5);
     saQueueCmd(buf, 6);
 }
@@ -701,12 +737,17 @@ static void vtxSAProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs)
         // Don't send SA_FREQ_GETPIT to V1 device; it act as plain SA_CMD_SET_FREQ,
         // and put the device into user frequency mode with uninitialized freq.
         if (saDevice.version) {
-            if (saDevice.version == 2) {
-                saDoDevSetFreq(SA_FREQ_GETPIT);
+            if (saDevice.version == SA_2_0) {
+                saSetFreq(SA_FREQ_GETPIT);
                 initPhase = SA_INITPHASE_WAIT_PITFREQ;
             } else {
                 initPhase = SA_INITPHASE_DONE;
             }
+            if (saDevice.version >= SA_2_0 ) {
+                //did the device boot up in pit mode on its own?
+                saDevice.willBootIntoPitMode = (saDevice.mode & SA_MODE_GET_PITMODE) ? true : false;
+                LOG_D(VTX, "sainit: willBootIntoPitMode is %s\r\n", saDevice.willBootIntoPitMode ? "true" : "false");
+            }
         }
         break;
 
@@ -735,7 +776,7 @@ static void vtxSAProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs)
         // Command pending. Send it.
         // LOG_D(VTX, "process: sending queue");
         saSendQueue();
-    lastCommandSentMs = nowMs;
+        lastCommandSentMs = nowMs;
     } else if ((nowMs - lastCommandSentMs < SMARTAUDIO_POLLING_WINDOW) && (nowMs - sa_lastTransmissionMs >= SMARTAUDIO_POLLING_INTERVAL)) {
     //LOG_D(VTX, "process: sending status change polling");
     saGetSettings();
@@ -753,45 +794,100 @@ vtxDevType_e vtxSAGetDeviceType(const vtxDevice_t *vtxDevice)
 
 static bool vtxSAIsReady(const vtxDevice_t *vtxDevice)
 {
-    return vtxDevice!=NULL && !(saDevice.version == 0);
+    return vtxDevice != NULL && !(saDevice.power == 0);
+    //wait until power reading exists
 }
 
-static void vtxSASetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint8_t channel)
+void vtxSASetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint8_t channel)
 {
     UNUSED(vtxDevice);
     if (saValidateBandAndChannel(band, channel)) {
         saSetBandAndChannel(band - 1, channel - 1);
     }
 }
-
-static void vtxSASetPowerByIndex(vtxDevice_t *vtxDevice, uint8_t index)
+ static void vtxSASetPowerByIndex(vtxDevice_t *vtxDevice, uint8_t index)
 {
-    UNUSED(vtxDevice);
-    saDevSetPowerByIndex(index);
+    static uint8_t buf[6] = { 0xAA, 0x55, SACMD(SA_CMD_SET_POWER), 1 };
+
+    if (!vtxSAIsReady(vtxDevice)) {
+        return;
+    }
+
+    if (index == 0) {
+        LOG_D(VTX, "SmartAudio doesn't support power off");
+        return;
+    }
+
+    if (index > saPowerCount) {
+        LOG_D(VTX, "Invalid power level");
+        return;
+    }
+
+    LOG_D(VTX, "saSetPowerByIndex: index %d, value %d\r\n", index, buf[4]);
+
+    index--;
+    switch (saDevice.version) {
+        case SA_1_0:
+            buf[4] = saPowerTable[index].dbi;
+            break;
+        case SA_2_0:
+            buf[4] = index;
+            break;
+        case SA_2_1:
+            buf[4] = saPowerTable[index].dbi;
+            buf[4] |= 128; //set MSB to indicate set power by dbm
+            break;
+        default:
+            break;
+    }
+
+    buf[5] = CRC8(buf, 5);
+    saQueueCmd(buf, 6);
 }
 
 static void vtxSASetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff)
 {
-    if (!(vtxSAIsReady(vtxDevice) && (saDevice.version == 2))) {
+    if (!vtxSAIsReady(vtxDevice) || saDevice.version < SA_1_0) {
         return;
     }
 
-    if (onoff) {
-        // SmartAudio can not turn pit mode on by software.
+    if (onoff && saDevice.version < SA_2_1) {
+        // Smart Audio prior to V2.1 can not turn pit mode on by software.
         return;
     }
 
-    uint8_t newmode = SA_MODE_CLR_PITMODE;
-
-    if (saDevice.mode & SA_MODE_GET_IN_RANGE_PITMODE) {
-        newmode |= SA_MODE_SET_IN_RANGE_PITMODE;
+    if (saDevice.version >= SA_2_1 && !saDevice.willBootIntoPitMode) {
+        if (onoff) {
+            // enable pitmode using SET_POWER command with 0 dbm.
+            // This enables pitmode without causing the device to boot into pitmode next power-up
+            static uint8_t buf[6] = { 0xAA, 0x55, SACMD(SA_CMD_SET_POWER), 1 };
+            buf[4] = 0 | 128;
+            buf[5] = CRC8(buf, 5);
+            saQueueCmd(buf, 6);
+            LOG_D(VTX, "vtxSASetPitMode: set power to 0 dbm\r\n");
+        } else {
+            saSetMode(SA_MODE_CLR_PITMODE);
+            LOG_D(VTX, "vtxSASetPitMode: clear pitmode permanently");
+        }
+       return;
     }
 
+    uint8_t newMode = onoff ? 0 : SA_MODE_CLR_PITMODE;
+
     if (saDevice.mode & SA_MODE_GET_OUT_RANGE_PITMODE) {
-        newmode |= SA_MODE_SET_OUT_RANGE_PITMODE;
+        newMode |= SA_MODE_SET_OUT_RANGE_PITMODE;
     }
 
-    saSetMode(newmode);
+    if ((saDevice.mode & SA_MODE_GET_IN_RANGE_PITMODE) || (onoff && newMode == 0)) {
+        // ensure when turning on pit mode that pit mode gets actually enabled
+        newMode |= SA_MODE_SET_IN_RANGE_PITMODE;
+    }
+    LOG_D(VTX, "vtxSASetPitMode %s with stored mode 0x%x por %s, pir %s, newMode 0x%x\r\n", onoff ? "on" : "off", saDevice.mode,
+            (saDevice.mode & SA_MODE_GET_OUT_RANGE_PITMODE) ? "on" : "off",
+            (saDevice.mode & SA_MODE_GET_IN_RANGE_PITMODE) ? "on" : "off" , newMode);
+
+
+    saSetMode(newMode);
 
     return;
 }
@@ -806,6 +902,7 @@ static bool vtxSAGetBandAndChannel(const vtxDevice_t *vtxDevice, uint8_t *pBand,
     *pBand = (saDevice.mode & SA_MODE_GET_FREQ_BY_FREQ) ? 0 :
         (SA_DEVICE_CHVAL_TO_BAND(saDevice.channel) + 1);
     *pChannel = SA_DEVICE_CHVAL_TO_CHANNEL(saDevice.channel) + 1;
+
     return true;
 }
 
@@ -815,13 +912,13 @@ static bool vtxSAGetPowerIndex(const vtxDevice_t *vtxDevice, uint8_t *pIndex)
         return false;
     }
 
-    *pIndex = ((saDevice.version == 1) ? saDacToPowerIndex(saDevice.power) : saDevice.power) + 1;
+    *pIndex = ((saDevice.version == SA_1_0) ? saDacToPowerIndex(saDevice.power) : saDevice.power);
     return true;
 }
 
 static bool vtxSAGetPitMode(const vtxDevice_t *vtxDevice, uint8_t *pOnOff)
 {
-    if (!(vtxSAIsReady(vtxDevice) && (saDevice.version == 2))) {
+    if (!(vtxSAIsReady(vtxDevice) && (saDevice.version < SA_2_0))) {
         return false;
     }
 
@@ -851,7 +948,7 @@ static bool vtxSAGetPower(const vtxDevice_t *vtxDevice, uint8_t *pIndex, uint16_
     }
 
     *pIndex = powerIndex;
-    *pPowerMw = (powerIndex > 0) ? saPowerTable[powerIndex-1].rfpower : 0;
+    *pPowerMw = (powerIndex > 0) ? saPowerTable[powerIndex - 1].mW : 0;
     return true;
 }
 
diff --git a/src/main/io/vtx_smartaudio.h b/src/main/io/vtx_smartaudio.h
index ef3e147ace..c817f05a79 100644
--- a/src/main/io/vtx_smartaudio.h
+++ b/src/main/io/vtx_smartaudio.h
@@ -33,7 +33,8 @@
 #define VTX_SMARTAUDIO_BAND_COUNT (VTX_SMARTAUDIO_MAX_BAND - VTX_SMARTAUDIO_MIN_BAND + 1)
 #define VTX_SMARTAUDIO_CHANNEL_COUNT (VTX_SMARTAUDIO_MAX_CHANNEL - VTX_SMARTAUDIO_MIN_CHANNEL + 1)
 
-#define VTX_SMARTAUDIO_POWER_COUNT 4
+#define VTX_SMARTAUDIO_MAX_POWER_COUNT 5
+#define VTX_SMARTAUDIO_DEFAULT_POWER_COUNT 4
 #define VTX_SMARTAUDIO_DEFAULT_POWER 1
 
 #define VTX_SMARTAUDIO_MIN_FREQUENCY_MHZ 5000        //min freq in MHz
@@ -49,7 +50,7 @@
 
 // opmode flags, SET side
 #define SA_MODE_SET_IN_RANGE_PITMODE        1 // Immediate
-#define SA_MODE_SET_OUT_RANGE_PITMODE        2 // Immediate
+#define SA_MODE_SET_OUT_RANGE_PITMODE       2 // Immediate
 #define SA_MODE_CLR_PITMODE                 4 // Immediate
 #define SA_MODE_SET_UNLOCK                  8
 #define SA_MODE_SET_LOCK                    0 // ~UNLOCK
@@ -61,20 +62,26 @@
 #define SA_FREQ_MASK                        (~(SA_FREQ_GETPIT|SA_FREQ_SETPIT))
 
 // For generic API use, but here for now
+typedef enum {
+    SA_UNKNOWN, // or offline
+    SA_1_0,
+    SA_2_0,
+    SA_2_1
+} smartAudioVersion_e;
 
 typedef struct smartAudioDevice_s {
-    int8_t version;
+    smartAudioVersion_e version;
     int8_t channel;
     int8_t power;
     int8_t mode;
     uint16_t freq;
     uint16_t orfreq;
+    bool willBootIntoPitMode;
 } smartAudioDevice_t;
 
 typedef struct saPowerTable_s {
-    int rfpower;
-    int16_t valueV1;
-    int16_t valueV2;
+    int mW;         // rfpower
+    int16_t dbi;    // valueV1
 } saPowerTable_t;
 
 typedef struct smartAudioStat_s {
@@ -89,7 +96,9 @@ typedef struct smartAudioStat_s {
 
 extern smartAudioDevice_t saDevice;
 extern saPowerTable_t saPowerTable[];
-extern const char * const saPowerNames[];
+
+extern uint8_t saPowerCount;
+extern const char * saPowerNames[VTX_SMARTAUDIO_MAX_POWER_COUNT + 1];
 extern smartAudioStat_t saStat;
 
 extern uint16_t sa_smartbaud;

From b4517d8e54605d0c9b48657dea91b56f9ca33dd0 Mon Sep 17 00:00:00 2001
From: Andi Kanzler <andi_kanzler@gmx.de>
Date: Mon, 11 May 2020 00:22:47 +0200
Subject: [PATCH 157/179] ...

---
 src/main/io/vtx_smartaudio.c | 1 -
 1 file changed, 1 deletion(-)

diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c
index a89dc2796e..cb60d447ad 100644
--- a/src/main/io/vtx_smartaudio.c
+++ b/src/main/io/vtx_smartaudio.c
@@ -28,7 +28,6 @@
 
 #include "platform.h"
 
-// Todo: Comment in before commit!
 #if defined(USE_VTX_SMARTAUDIO) && defined(USE_VTX_CONTROL)
 
 #include "build/debug.h"

From e04584c8db502c500d9b38606336ccdcad5f1b86 Mon Sep 17 00:00:00 2001
From: Andi Kanzler <andi_kanzler@gmx.de>
Date: Mon, 11 May 2020 00:21:35 +0200
Subject: [PATCH 158/179] Basic Smartaudio 2.1 Support

---
 src/main/io/vtx_smartaudio.c | 294 +++++++++++++++++++++++------------
 src/main/io/vtx_smartaudio.h |  23 ++-
 2 files changed, 211 insertions(+), 106 deletions(-)

diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c
index a23d590201..cb60d447ad 100644
--- a/src/main/io/vtx_smartaudio.c
+++ b/src/main/io/vtx_smartaudio.c
@@ -24,6 +24,7 @@
 #include <stdint.h>
 #include <string.h>
 #include <ctype.h>
+#include <math.h>
 
 #include "platform.h"
 
@@ -37,6 +38,7 @@
 #include "common/maths.h"
 #include "common/printf.h"
 #include "common/utils.h"
+#include "common/typeconversion.h"
 
 #include "drivers/time.h"
 #include "drivers/vtx_common.h"
@@ -56,19 +58,23 @@
 
 static serialPort_t *smartAudioSerialPort = NULL;
 
-const char * const saPowerNames[VTX_SMARTAUDIO_POWER_COUNT+1] = {
-    "---", "25 ", "200", "500", "800",
+uint8_t saPowerCount = VTX_SMARTAUDIO_DEFAULT_POWER_COUNT;
+const char * saPowerNames[VTX_SMARTAUDIO_MAX_POWER_COUNT + 1] = {
+    "----", "25  ", "200 ", "500 ", "800 ", "    "
 };
 
+// Save powerlevels reported from SA 2.1 devices here
+char sa21PowerNames[VTX_SMARTAUDIO_MAX_POWER_COUNT][5];
+
 static const vtxVTable_t saVTable;    // Forward
 static vtxDevice_t vtxSmartAudio = {
     .vTable = &saVTable,
     .capability.bandCount = VTX_SMARTAUDIO_BAND_COUNT,
     .capability.channelCount = VTX_SMARTAUDIO_CHANNEL_COUNT,
-    .capability.powerCount = VTX_SMARTAUDIO_POWER_COUNT,
+    .capability.powerCount = VTX_SMARTAUDIO_MAX_POWER_COUNT,
     .capability.bandNames = (char **)vtx58BandNames,
     .capability.channelNames = (char **)vtx58ChannelNames,
-    .capability.powerNames = (char **)saPowerNames,
+    .capability.powerNames = (char**)saPowerNames
 };
 
 // SmartAudio command and response codes
@@ -110,22 +116,25 @@ smartAudioStat_t saStat = {
     .badcode = 0,
 };
 
-saPowerTable_t saPowerTable[VTX_SMARTAUDIO_POWER_COUNT] = {
-    {  25,   7,   0 },
-    { 200,  16,   1 },
-    { 500,  25,   2 },
-    { 800,  40,   3 },
+// Fill table with standard values for SA 1.0 and 2.0
+saPowerTable_t saPowerTable[VTX_SMARTAUDIO_MAX_POWER_COUNT] = {
+    {  25,   7 },
+    { 200,  16 },
+    { 500,  25 },
+    { 800,  40 },
+    {   0,   0 } // Placeholder
 };
 
 // Last received device ('hard') states
 
 smartAudioDevice_t saDevice = {
-    .version = 0,
+    .version = SA_UNKNOWN,
     .channel = -1,
     .power = -1,
     .mode = 0,
     .freq = 0,
     .orfreq = 0,
+    .willBootIntoPitMode = false
 };
 
 static smartAudioDevice_t saDevicePrev = {
@@ -139,7 +148,7 @@ static uint8_t saLockMode = SA_MODE_SET_UNLOCK; // saCms variable?
 bool saDeferred = true; // saCms variable?
 
 // Receive frame reassembly buffer
-#define SA_MAX_RCVLEN 15
+#define SA_MAX_RCVLEN 21
 static uint8_t sa_rbuf[SA_MAX_RCVLEN+4]; // XXX delete 4 byte guard
 
 //
@@ -183,18 +192,31 @@ static void saPrintSettings(void)
     LOG_D(VTX, "freq: %d ", saDevice.freq);
     LOG_D(VTX, "power: %d ", saDevice.power);
     LOG_D(VTX, "pitfreq: %d ", saDevice.orfreq);
+    LOG_D(VTX, "BootIntoPitMode: %s", saDevice.willBootIntoPitMode ? "yes" : "no");
 }
 
 int saDacToPowerIndex(int dac)
 {
-    for (int idx = VTX_SMARTAUDIO_POWER_COUNT - 1 ; idx >= 0 ; idx--) {
-        if (saPowerTable[idx].valueV1 <= dac) {
+    for (int idx = saPowerCount - 1 ; idx >= 0 ; idx--) {
+        if (saPowerTable[idx].dbi <= dac) {
             return idx;
         }
     }
     return 0;
 }
 
+int saDbiToMw(uint16_t dbi) {
+
+    uint16_t mw = (uint16_t)pow(10.0, dbi / 10.0);
+
+    if (dbi > 14) {
+        // For powers greater than 25mW round up to a multiple of 50 to match expectations
+        mw = 50 * ((mw + 25) / 50);
+     }
+
+    return mw;
+}
+
 //
 // Autobauding
 //
@@ -272,14 +294,67 @@ static void saProcessResponse(uint8_t *buf, int len)
             break;
         }
 
-        // XXX(fujin): For now handle V21 saDevice.version as '2'.
         // From spec: "Bit 7-3 is holding the Smart audio version where 0 is V1, 1 is V2, 2 is V2.1"
-        saDevice.version = (buf[0] == SA_CMD_GET_SETTINGS) ? 1 : 2;
+        // saDevice.version = 0 means unknown, 1 means Smart audio V1, 2 means Smart audio V2 and 3 means Smart audio V2.1
+        saDevice.version = (buf[0] == SA_CMD_GET_SETTINGS) ? 1 : ((buf[0] == SA_CMD_GET_SETTINGS_V2) ? 2 : 3);
         saDevice.channel = buf[2];
-        saDevice.power = buf[3];
+        uint8_t rawPowerValue = buf[3];
         saDevice.mode = buf[4];
-        saDevice.freq = (buf[5] << 8)|buf[6];
-        // XXX(fujin): Receive additional SA2.1 fields here for dBm based power level.
+        saDevice.freq = (buf[5] << 8) | buf[6];
+
+        // read pir and por flags to detect if the device will boot into pitmode.
+        // note that "quit pitmode without unsetting the pitmode flag" clears pir and por flags but the device will still boot into pitmode.
+        // therefore we ignore the pir and por flags while the device is not in pitmode
+        // actually, this is the whole reason the variable saDevice.willBootIntoPitMode exists.
+        // otherwise we could use saDevice.mode directly
+        if (saDevice.mode & SA_MODE_GET_PITMODE) {
+            bool newBootMode = (saDevice.mode & SA_MODE_GET_IN_RANGE_PITMODE) || (saDevice.mode & SA_MODE_GET_OUT_RANGE_PITMODE);
+            if (newBootMode != saDevice.willBootIntoPitMode) {
+                LOG_D(VTX, "saProcessResponse: willBootIntoPitMode is now %s\r\n", newBootMode  ? "true" : "false");
+            }
+            saDevice.willBootIntoPitMode = newBootMode;
+        }
+
+        if(saDevice.version == SA_2_1) {
+            //read dbm based power levels
+            if(len < 10) { //current power level in dbm field missing or power level length field missing or zero power levels reported
+                LOG_D(VTX, "processResponse: V2.1 vtx didn't report any power levels\r\n");
+                break;
+            }
+            saPowerCount = constrain((int8_t)buf[8], 0, VTX_SMARTAUDIO_MAX_POWER_COUNT);
+            vtxSmartAudio.capability.powerCount = saPowerCount;
+            //SmartAudio seems to report buf[8] + 1 power levels, but one of them is zero.
+            //zero is indeed a valid power level to set the vtx to, but it activates pit mode.
+            //crucially, after sending 0 dbm, the vtx does NOT report its power level to be 0 dbm.
+            //instead, it reports whatever value was set previously and it reports to be in pit mode.
+            //for this reason, zero shouldn't be used as a normal power level in iNav.
+            for (int8_t i = 0; i < saPowerCount; i++ ) {
+                saPowerTable[i].dbi = buf[9 + i + 1]; //+ 1 to skip the first power level, as mentioned above
+                saPowerTable[i].mW = saDbiToMw(saPowerTable[i].dbi);
+                if (i <= VTX_SMARTAUDIO_MAX_POWER_COUNT) {
+                    char strbuf[5];
+                    itoa(saPowerTable[i].mW, strbuf, 10);
+                    strcpy(sa21PowerNames[i], strbuf);
+                    saPowerNames[i + 1] = sa21PowerNames[i];
+                }
+            }
+
+            LOG_D(VTX, "processResponse: %d power values: %d, %d, %d, %d\r\n",
+                    saPowerCount, saPowerTable[0].dbi, saPowerTable[1].dbi,
+                    saPowerTable[2].dbi, saPowerTable[3].dbi);
+            //LOG_D(VTX, "processResponse: V2.1 received vtx power value %d\r\n",buf[7]);
+            rawPowerValue = buf[7];
+
+            saDevice.power = 0; //set to unknown power level if the reported one doesnt match any of the known ones
+            LOG_D(VTX, "processResponse: rawPowerValue is %d, legacy power is %d\r\n", rawPowerValue, buf[3]);
+            for (int8_t i = 0; i < saPowerCount; i++) {
+                if (rawPowerValue == saPowerTable[i].dbi) {
+                    saDevice.power = i + 1;
+                }
+            }
+        } else {
+            saDevice.power = rawPowerValue + 1;
+        }
 
         DEBUG_SET(DEBUG_SMARTAUDIO, 0, saDevice.version * 100 + saDevice.mode);
         DEBUG_SET(DEBUG_SMARTAUDIO, 1, saDevice.channel);
@@ -288,14 +363,6 @@ static void saProcessResponse(uint8_t *buf, int len)
         break;
 
     case SA_CMD_SET_POWER: // Set Power
-        if (len == 5) {
-            // Improperly implemented S.Audio devices that
-            // omit the channel
-            saDevice.power = buf[2];
-        } else if (len >= 6) {
-            // S.Audio v2 spec
-            saDevice.power = buf[3];
-        }
         break;
 
     case SA_CMD_SET_CHAN: // Set Channel
@@ -321,7 +388,9 @@ static void saProcessResponse(uint8_t *buf, int len)
         break;
 
     case SA_CMD_SET_MODE: // Set Mode
-        LOG_D(VTX, "saProcessResponse: SET_MODE 0x%x", buf[2]);
+        LOG_D(VTX, "saProcessResponse: SET_MODE 0x%x, (pir %s, por %s, pitdsbl %s, %s)\r\n",
+            buf[2], (buf[2] & 1) ? "on" : "off", (buf[2] & 2) ? "on" : "off", (buf[3] & 4) ? "on" : "off",
+            (buf[4] & 8) ? "unlocked" : "locked");
         break;
 
     default:
@@ -330,18 +399,11 @@ static void saProcessResponse(uint8_t *buf, int len)
     }
 
     if (memcmp(&saDevice, &saDevicePrev, sizeof(smartAudioDevice_t))) {
-#ifdef USE_CMS    //if changes then trigger saCms update
-        //saCmsResetOpmodel();
-#endif
-    // Debug
-    saPrintSettings();
+        // Debug
+        saPrintSettings();
     }
-    saDevicePrev = saDevice;
 
-#ifdef USE_CMS
-    // Export current device status for CMS
-    //saCmsUpdate();
-#endif
+    saDevicePrev = saDevice;
 }
 
 //
@@ -536,10 +598,11 @@ static void saGetSettings(void)
 {
     static uint8_t bufGetSettings[5] = {0xAA, 0x55, SACMD(SA_CMD_GET_SETTINGS), 0x00, 0x9F};
 
+    LOG_D(VTX, "smartAudioGetSettings\r\n");
     saQueueCmd(bufGetSettings, 5);
 }
 
-static void saDoDevSetFreq(uint16_t freq)
+void saSetFreq(uint16_t freq)
 {
     static uint8_t buf[7] = { 0xAA, 0x55, SACMD(SA_CMD_SET_FREQ), 2 };
     static uint8_t switchBuf[7];
@@ -572,14 +635,9 @@ static void saDoDevSetFreq(uint16_t freq)
     saQueueCmd(buf, 7);
 }
 
-void saSetFreq(uint16_t freq)
-{
-    saDoDevSetFreq(freq);
-}
-
 void saSetPitFreq(uint16_t freq)
 {
-    saDoDevSetFreq(freq | SA_FREQ_SETPIT);
+    saSetFreq(freq | SA_FREQ_SETPIT);
 }
 
 #if 0
@@ -592,58 +650,35 @@ static void saGetPitFreq(void)
 static bool saValidateBandAndChannel(uint8_t band, uint8_t channel)
 {
     return (band >= VTX_SMARTAUDIO_MIN_BAND && band <= VTX_SMARTAUDIO_MAX_BAND &&
-             channel >= VTX_SMARTAUDIO_MIN_CHANNEL && channel <= VTX_SMARTAUDIO_MAX_CHANNEL);
+            channel >= VTX_SMARTAUDIO_MIN_CHANNEL && channel <= VTX_SMARTAUDIO_MAX_CHANNEL);
 }
 
-static void saDevSetBandAndChannel(uint8_t band, uint8_t channel)
+void saSetBandAndChannel(uint8_t band, uint8_t channel)
 {
     static uint8_t buf[6] = { 0xAA, 0x55, SACMD(SA_CMD_SET_CHAN), 1 };
 
     buf[4] = SA_BANDCHAN_TO_DEVICE_CHVAL(band, channel);
     buf[5] = CRC8(buf, 5);
+    LOG_D(VTX, "vtxSASetBandAndChannel set index band %d channel %d value sent 0x%x\r\n", band, channel, buf[4]);
 
+    //this will clear saDevice.mode & SA_MODE_GET_FREQ_BY_FREQ
     saQueueCmd(buf, 6);
 }
 
-void saSetBandAndChannel(uint8_t band, uint8_t channel)
-{
-    saDevSetBandAndChannel(band, channel);
-}
-
 void saSetMode(int mode)
 {
     static uint8_t buf[6] = { 0xAA, 0x55, SACMD(SA_CMD_SET_MODE), 1 };
 
     buf[4] = (mode & 0x3f) | saLockMode;
-    buf[5] = CRC8(buf, 5);
-
-    saQueueCmd(buf, 6);
-}
-
-static void saDevSetPowerByIndex(uint8_t index)
-{
-    static uint8_t buf[6] = { 0xAA, 0x55, SACMD(SA_CMD_SET_POWER), 1 };
-
-    LOG_D(VTX, "saSetPowerByIndex: index %d", index);
-
-    if (index == 0) {
-        // SmartAudio doesn't support power off.
-        return;
+    if (saDevice.version >= SA_2_1 && (mode & SA_MODE_CLR_PITMODE) &&
+        ((mode & SA_MODE_SET_IN_RANGE_PITMODE) || (mode & SA_MODE_SET_OUT_RANGE_PITMODE))) {
+        saDevice.willBootIntoPitMode = true;//quit pitmode without unsetting flag.
+        //the response will just say pit=off but the device will still go into pitmode on reboot.
+        //therefore we have to memorize this change here.
     }
+    LOG_D(VTX, "saSetMode(0x%x): pir=%s por=%s pitdsbl=%s %s\r\n", mode, (mode & 1) ? "on " : "off", (mode & 2) ? "on " : "off",
+            (mode & 4)? "on " : "off", (mode & 8) ? "locked" : "unlocked");
 
-    if (index > VTX_SMARTAUDIO_POWER_COUNT) {
-        // Invalid power level
-        return;
-    }
-
-    if (saDevice.version == 0) {
-        // Unknown or yet unknown version.
-        return;
-    }
-
-    unsigned entry = index - 1;
-
-    buf[4] = (saDevice.version == 1) ? saPowerTable[entry].valueV1 : saPowerTable[entry].valueV2;
     buf[5] = CRC8(buf, 5);
     saQueueCmd(buf, 6);
 }
@@ -701,12 +736,17 @@ static void vtxSAProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs)
         // Don't send SA_FREQ_GETPIT to V1 device; it act as plain SA_CMD_SET_FREQ,
         // and put the device into user frequency mode with uninitialized freq.
         if (saDevice.version) {
-            if (saDevice.version == 2) {
-                saDoDevSetFreq(SA_FREQ_GETPIT);
+            if (saDevice.version == SA_2_0) {
+                saSetFreq(SA_FREQ_GETPIT);
                 initPhase = SA_INITPHASE_WAIT_PITFREQ;
             } else {
                 initPhase = SA_INITPHASE_DONE;
             }
+            if (saDevice.version >= SA_2_0 ) {
+                //did the device boot up in pit mode on its own?
+                saDevice.willBootIntoPitMode = (saDevice.mode & SA_MODE_GET_PITMODE) ? true : false;
+                LOG_D(VTX, "sainit: willBootIntoPitMode is %s\r\n", saDevice.willBootIntoPitMode ? "true" : "false");
+            }
         }
         break;
 
@@ -735,7 +775,7 @@ static void vtxSAProcess(vtxDevice_t *vtxDevice, timeUs_t currentTimeUs)
         // Command pending. Send it.
         // LOG_D(VTX, "process: sending queue");
         saSendQueue();
-    lastCommandSentMs = nowMs;
+        lastCommandSentMs = nowMs;
     } else if ((nowMs - lastCommandSentMs < SMARTAUDIO_POLLING_WINDOW) && (nowMs - sa_lastTransmissionMs >= SMARTAUDIO_POLLING_INTERVAL)) {
     //LOG_D(VTX, "process: sending status change polling");
     saGetSettings();
@@ -753,45 +793,100 @@ vtxDevType_e vtxSAGetDeviceType(const vtxDevice_t *vtxDevice)
 
 static bool vtxSAIsReady(const vtxDevice_t *vtxDevice)
 {
-    return vtxDevice!=NULL && !(saDevice.version == 0);
+    return vtxDevice != NULL && !(saDevice.power == 0);
+    //wait until power reading exists
 }
 
-static void vtxSASetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint8_t channel)
+void vtxSASetBandAndChannel(vtxDevice_t *vtxDevice, uint8_t band, uint8_t channel)
 {
     UNUSED(vtxDevice);
     if (saValidateBandAndChannel(band, channel)) {
         saSetBandAndChannel(band - 1, channel - 1);
     }
 }
-
-static void vtxSASetPowerByIndex(vtxDevice_t *vtxDevice, uint8_t index)
+ static void vtxSASetPowerByIndex(vtxDevice_t *vtxDevice, uint8_t index)
 {
-    UNUSED(vtxDevice);
-    saDevSetPowerByIndex(index);
+    static uint8_t buf[6] = { 0xAA, 0x55, SACMD(SA_CMD_SET_POWER), 1 };
+
+    if (!vtxSAIsReady(vtxDevice)) {
+        return;
+    }
+
+    if (index == 0) {
+        LOG_D(VTX, "SmartAudio doesn't support power off");
+        return;
+    }
+
+    if (index > saPowerCount) {
+        LOG_D(VTX, "Invalid power level");
+        return;
+    }
+
+    LOG_D(VTX, "saSetPowerByIndex: index %d, value %d\r\n", index, buf[4]);
+
+    index--;
+    switch (saDevice.version) {
+        case SA_1_0:
+            buf[4] = saPowerTable[index].dbi;
+            break;
+        case SA_2_0:
+            buf[4] = index;
+            break;
+        case SA_2_1:
+            buf[4] = saPowerTable[index].dbi;
+            buf[4] |= 128; //set MSB to indicate set power by dbm
+            break;
+        default:
+            break;
+    }
+
+    buf[5] = CRC8(buf, 5);
+    saQueueCmd(buf, 6);
 }
 
 static void vtxSASetPitMode(vtxDevice_t *vtxDevice, uint8_t onoff)
 {
-    if (!(vtxSAIsReady(vtxDevice) && (saDevice.version == 2))) {
+    if (!vtxSAIsReady(vtxDevice) || saDevice.version < SA_1_0) {
         return;
     }
 
-    if (onoff) {
-        // SmartAudio can not turn pit mode on by software.
+    if (onoff && saDevice.version < SA_2_1) {
+        // Smart Audio prior to V2.1 can not turn pit mode on by software.
         return;
     }
 
-    uint8_t newmode = SA_MODE_CLR_PITMODE;
-
-    if (saDevice.mode & SA_MODE_GET_IN_RANGE_PITMODE) {
-        newmode |= SA_MODE_SET_IN_RANGE_PITMODE;
+    if (saDevice.version >= SA_2_1 && !saDevice.willBootIntoPitMode) {
+        if (onoff) {
+            // enable pitmode using SET_POWER command with 0 dbm.
+            // This enables pitmode without causing the device to boot into pitmode next power-up
+            static uint8_t buf[6] = { 0xAA, 0x55, SACMD(SA_CMD_SET_POWER), 1 };
+            buf[4] = 0 | 128;
+            buf[5] = CRC8(buf, 5);
+            saQueueCmd(buf, 6);
+            LOG_D(VTX, "vtxSASetPitMode: set power to 0 dbm\r\n");
+        } else {
+            saSetMode(SA_MODE_CLR_PITMODE);
+            LOG_D(VTX, "vtxSASetPitMode: clear pitmode permanently");
+        }
+       return;
     }
 
+    uint8_t newMode = onoff ? 0 : SA_MODE_CLR_PITMODE;
+
     if (saDevice.mode & SA_MODE_GET_OUT_RANGE_PITMODE) {
-        newmode |= SA_MODE_SET_OUT_RANGE_PITMODE;
+        newMode |= SA_MODE_SET_OUT_RANGE_PITMODE;
     }
 
-    saSetMode(newmode);
+    if ((saDevice.mode & SA_MODE_GET_IN_RANGE_PITMODE) || (onoff && newMode == 0)) {
+        // ensure when turning on pit mode that pit mode gets actually enabled
+        newMode |= SA_MODE_SET_IN_RANGE_PITMODE;
+    }
+    LOG_D(VTX, "vtxSASetPitMode %s with stored mode 0x%x por %s, pir %s, newMode 0x%x\r\n", onoff ? "on" : "off", saDevice.mode,
+            (saDevice.mode & SA_MODE_GET_OUT_RANGE_PITMODE) ? "on" : "off",
+            (saDevice.mode & SA_MODE_GET_IN_RANGE_PITMODE) ? "on" : "off" , newMode);
+
+
+    saSetMode(newMode);
 
     return;
 }
@@ -806,6 +901,7 @@ static bool vtxSAGetBandAndChannel(const vtxDevice_t *vtxDevice, uint8_t *pBand,
     *pBand = (saDevice.mode & SA_MODE_GET_FREQ_BY_FREQ) ? 0 :
         (SA_DEVICE_CHVAL_TO_BAND(saDevice.channel) + 1);
     *pChannel = SA_DEVICE_CHVAL_TO_CHANNEL(saDevice.channel) + 1;
+
     return true;
 }
 
@@ -815,13 +911,13 @@ static bool vtxSAGetPowerIndex(const vtxDevice_t *vtxDevice, uint8_t *pIndex)
         return false;
     }
 
-    *pIndex = ((saDevice.version == 1) ? saDacToPowerIndex(saDevice.power) : saDevice.power) + 1;
+    *pIndex = ((saDevice.version == SA_1_0) ? saDacToPowerIndex(saDevice.power) : saDevice.power);
     return true;
 }
 
 static bool vtxSAGetPitMode(const vtxDevice_t *vtxDevice, uint8_t *pOnOff)
 {
-    if (!(vtxSAIsReady(vtxDevice) && (saDevice.version == 2))) {
+    if (!(vtxSAIsReady(vtxDevice) && (saDevice.version < SA_2_0))) {
         return false;
     }
 
@@ -851,7 +947,7 @@ static bool vtxSAGetPower(const vtxDevice_t *vtxDevice, uint8_t *pIndex, uint16_
     }
 
     *pIndex = powerIndex;
-    *pPowerMw = (powerIndex > 0) ? saPowerTable[powerIndex-1].rfpower : 0;
+    *pPowerMw = (powerIndex > 0) ? saPowerTable[powerIndex - 1].mW : 0;
     return true;
 }
 
diff --git a/src/main/io/vtx_smartaudio.h b/src/main/io/vtx_smartaudio.h
index ef3e147ace..c817f05a79 100644
--- a/src/main/io/vtx_smartaudio.h
+++ b/src/main/io/vtx_smartaudio.h
@@ -33,7 +33,8 @@
 #define VTX_SMARTAUDIO_BAND_COUNT (VTX_SMARTAUDIO_MAX_BAND - VTX_SMARTAUDIO_MIN_BAND + 1)
 #define VTX_SMARTAUDIO_CHANNEL_COUNT (VTX_SMARTAUDIO_MAX_CHANNEL - VTX_SMARTAUDIO_MIN_CHANNEL + 1)
 
-#define VTX_SMARTAUDIO_POWER_COUNT 4
+#define VTX_SMARTAUDIO_MAX_POWER_COUNT 5
+#define VTX_SMARTAUDIO_DEFAULT_POWER_COUNT 4
 #define VTX_SMARTAUDIO_DEFAULT_POWER 1
 
 #define VTX_SMARTAUDIO_MIN_FREQUENCY_MHZ 5000        //min freq in MHz
@@ -49,7 +50,7 @@
 
 // opmode flags, SET side
 #define SA_MODE_SET_IN_RANGE_PITMODE        1 // Immediate
-#define SA_MODE_SET_OUT_RANGE_PITMODE        2 // Immediate
+#define SA_MODE_SET_OUT_RANGE_PITMODE       2 // Immediate
 #define SA_MODE_CLR_PITMODE                 4 // Immediate
 #define SA_MODE_SET_UNLOCK                  8
 #define SA_MODE_SET_LOCK                    0 // ~UNLOCK
@@ -61,20 +62,26 @@
 #define SA_FREQ_MASK                        (~(SA_FREQ_GETPIT|SA_FREQ_SETPIT))
 
 // For generic API use, but here for now
+typedef enum {
+    SA_UNKNOWN, // or offline
+    SA_1_0,
+    SA_2_0,
+    SA_2_1
+} smartAudioVersion_e;
 
 typedef struct smartAudioDevice_s {
-    int8_t version;
+    smartAudioVersion_e version;
     int8_t channel;
     int8_t power;
     int8_t mode;
     uint16_t freq;
     uint16_t orfreq;
+    bool willBootIntoPitMode;
 } smartAudioDevice_t;
 
 typedef struct saPowerTable_s {
-    int rfpower;
-    int16_t valueV1;
-    int16_t valueV2;
+    int mW;         // rfpower
+    int16_t dbi;    // valueV1
 } saPowerTable_t;
 
 typedef struct smartAudioStat_s {
@@ -89,7 +96,9 @@ typedef struct smartAudioStat_s {
 
 extern smartAudioDevice_t saDevice;
 extern saPowerTable_t saPowerTable[];
-extern const char * const saPowerNames[];
+
+extern uint8_t saPowerCount;
+extern const char * saPowerNames[VTX_SMARTAUDIO_MAX_POWER_COUNT + 1];
 extern smartAudioStat_t saStat;
 
 extern uint16_t sa_smartbaud;

From f5f13749fcdb87fc4c92d3f0b06e59ddc9498e96 Mon Sep 17 00:00:00 2001
From: "Konstantin (DigitalEntity) Sharlaimov"
 <konstantin.sharlaimov@gmail.com>
Date: Mon, 11 May 2020 21:42:36 +0200
Subject: [PATCH 159/179] [GYRO] Add support for MPU6000 to FIREWORKS V2 board;
 Enable DUAL_GYRO to use both SPI CS lines

---
 src/main/target/FIREWORKSV2/target.c |  8 ++++++++
 src/main/target/FIREWORKSV2/target.h | 28 ++++++++++++++++------------
 2 files changed, 24 insertions(+), 12 deletions(-)

diff --git a/src/main/target/FIREWORKSV2/target.c b/src/main/target/FIREWORKSV2/target.c
index 1eb592444d..3866b61fc0 100644
--- a/src/main/target/FIREWORKSV2/target.c
+++ b/src/main/target/FIREWORKSV2/target.c
@@ -29,11 +29,19 @@
 #include "drivers/pwm_mapping.h"
 #include "drivers/timer.h"
 #include "drivers/bus.h"
+#include "drivers/sensor.h"
 
 #include "drivers/pwm_output.h"
 #include "common/maths.h"
 #include "fc/config.h"
 
+// Board hardware definitions - IMU1 slot
+BUSDEV_REGISTER_SPI_TAG(busdev_1_mpu6000,   DEVHW_MPU6000,  IMU_1_SPI_BUS,  IMU_1_CS_PIN,   NONE,   0,  DEVFLAGS_NONE,  IMU_1_ALIGN);
+BUSDEV_REGISTER_SPI_TAG(busdev_1_mpu6500,   DEVHW_MPU6500,  IMU_1_SPI_BUS,  IMU_1_CS_PIN,   NONE,   0,  DEVFLAGS_NONE,  IMU_1_ALIGN);
+
+// Board hardware definitions - IMU2 slot
+BUSDEV_REGISTER_SPI_TAG(busdev_2_mpu6000,   DEVHW_MPU6000,  IMU_2_SPI_BUS,  IMU_2_CS_PIN,   NONE,   1,  DEVFLAGS_NONE,  IMU_2_ALIGN);
+BUSDEV_REGISTER_SPI_TAG(busdev_2_mpu6500,   DEVHW_MPU6500,  IMU_2_SPI_BUS,  IMU_2_CS_PIN,   NONE,   1,  DEVFLAGS_NONE,  IMU_2_ALIGN);
 
 const timerHardware_t timerHardware[] = {
     DEF_TIM(TIM10, CH1, PB8, TIM_USE_PPM,                           0, 0), // PPM
diff --git a/src/main/target/FIREWORKSV2/target.h b/src/main/target/FIREWORKSV2/target.h
index 6e707a1f28..b8dd5c6ef6 100644
--- a/src/main/target/FIREWORKSV2/target.h
+++ b/src/main/target/FIREWORKSV2/target.h
@@ -62,24 +62,28 @@
 #define GYRO_INT_EXTI            PC8
 // #define USE_MPU_DATA_READY_SIGNAL        // Not connected on FireworksV2
 
+#define USE_DUAL_GYRO
+#define USE_TARGET_IMU_HARDWARE_DESCRIPTORS     // Don't use common busdev descriptors for IMU
 #define USE_IMU_MPU6500
+#define USE_IMU_MPU6000
 
 #if defined(OMNIBUSF4V6)
-#define MPU6500_CS_PIN          PC14
-#define MPU6500_SPI_BUS         BUS_SPI1
-#define IMU_MPU6500_ALIGN       CW0_DEG
+#   define IMU_1_CS_PIN            PA4
+#   define IMU_1_SPI_BUS           BUS_SPI1
+#   define IMU_1_ALIGN             CW180_DEG
+#   define IMU_2_CS_PIN            PC14
+#   define IMU_2_SPI_BUS           BUS_SPI1
+#   define IMU_2_ALIGN             CW0_DEG
 #else
-#define MPU6500_CS_PIN          PD2
-#define MPU6500_SPI_BUS         BUS_SPI3
-#define IMU_MPU6500_ALIGN       CW180_DEG
+    // FIREWORKS V2
+#   define IMU_1_CS_PIN            PD2
+#   define IMU_1_SPI_BUS           BUS_SPI3
+#   define IMU_1_ALIGN             CW180_DEG
+#   define IMU_2_CS_PIN            PA4
+#   define IMU_2_SPI_BUS           BUS_SPI1
+#   define IMU_2_ALIGN             CW0_DEG_FLIP
 #endif
 
-// OmnibusF4 Nano v6 and OmnibusF4 V6 has a MPU6000
-#define USE_IMU_MPU6000
-#define IMU_MPU6000_ALIGN       CW180_DEG
-#define MPU6000_CS_PIN          PA4
-#define MPU6000_SPI_BUS         BUS_SPI1
-
 #define USE_MAG
 #if defined(OMNIBUSF4V6)
 #define MAG_I2C_BUS             BUS_I2C1

From bf4115a0ecfb64867c00b654eded3f7ed5b2d141 Mon Sep 17 00:00:00 2001
From: Jukka Aakko <aakko.jukka@gmail.com>
Date: Wed, 13 May 2020 21:08:07 +0300
Subject: [PATCH 160/179] IFLIGHT7_TWING second IMU orientation fix.

---
 src/main/target/IFLIGHTF7_TWING/target.h | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/target/IFLIGHTF7_TWING/target.h b/src/main/target/IFLIGHTF7_TWING/target.h
index 6c67e21e27..a6a9e63de2 100644
--- a/src/main/target/IFLIGHTF7_TWING/target.h
+++ b/src/main/target/IFLIGHTF7_TWING/target.h
@@ -40,7 +40,7 @@
 
 #define USE_IMU_MPU6500
 #define IMU_0_ALIGN       CW90_DEG
-#define IMU_1_ALIGN       CW90_DEG
+#define IMU_1_ALIGN       CW0_DEG
 
 #define MPU6500_0_CS_PIN        PC3
 #define MPU6500_0_SPI_BUS       BUS_SPI1
@@ -155,4 +155,4 @@
 #define MAX_PWM_OUTPUT_PORTS        8
 
 #define USE_DSHOT
-#define USE_ESC_SENSOR
\ No newline at end of file
+#define USE_ESC_SENSOR

From 3e22221816d5561f470578920c9d5a92a1c71a6c Mon Sep 17 00:00:00 2001
From: Michel Pastor <shellixyz@users.noreply.github.com>
Date: Thu, 14 May 2020 12:21:45 +0200
Subject: [PATCH 161/179] Improve FrSky's CRC lib (#5703)

---
 src/main/rx/fport.c     |  2 +-
 src/main/rx/frsky_crc.c | 16 +++++++++++-----
 src/main/rx/frsky_crc.h |  5 ++---
 3 files changed, 14 insertions(+), 9 deletions(-)

diff --git a/src/main/rx/fport.c b/src/main/rx/fport.c
index 59f62fe3f5..4b607373c4 100644
--- a/src/main/rx/fport.c
+++ b/src/main/rx/fport.c
@@ -255,7 +255,7 @@ static uint8_t fportFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
         if (frameLength != bufferLength - 2) {
             reportFrameError(DEBUG_FPORT_ERROR_SIZE);
         } else {
-            if (!frskyChecksumIsGood(&rxBuffer[rxBufferReadIndex].data[0], bufferLength)) {
+            if (!frskyCheckSumIsGood(&rxBuffer[rxBufferReadIndex].data[0], bufferLength)) {
                 reportFrameError(DEBUG_FPORT_ERROR_CHECKSUM);
             } else {
                 fportFrame_t *frame = (fportFrame_t *)&rxBuffer[rxBufferReadIndex].data[1];
diff --git a/src/main/rx/frsky_crc.c b/src/main/rx/frsky_crc.c
index fa3934e574..32411ecc39 100644
--- a/src/main/rx/frsky_crc.c
+++ b/src/main/rx/frsky_crc.c
@@ -26,22 +26,28 @@ FILE_COMPILE_FOR_SPEED
 void frskyCheckSumStep(uint16_t *checksum, uint8_t byte)
 {
     *checksum += byte;
-    *checksum += (*checksum >> 8);
-    *checksum &= 0xFF;
 }
 
 void frskyCheckSumFini(uint16_t *checksum)
 {
+    while (*checksum > 0xFF) {
+        *checksum = (*checksum & 0xFF) + (*checksum >> 8);
+    }
+
     *checksum = 0xFF - *checksum;
 }
 
-bool frskyChecksumIsGood(uint8_t *data, uint8_t length)
+uint8_t frskyCheckSum(uint8_t *data, uint8_t length)
 {
     uint16_t checksum = 0;
     for (unsigned i = 0; i < length; i++) {
         frskyCheckSumStep(&checksum, *data++);
     }
-
-    return checksum == FRSKY_CHECKSUM_GOOD_VALUE;
+    frskyCheckSumFini(&checksum);
+    return checksum;
 }
 
+bool frskyCheckSumIsGood(uint8_t *data, uint8_t length)
+{
+    return !frskyCheckSum(data, length);
+}
diff --git a/src/main/rx/frsky_crc.h b/src/main/rx/frsky_crc.h
index 849b53bfab..c0a026ba47 100644
--- a/src/main/rx/frsky_crc.h
+++ b/src/main/rx/frsky_crc.h
@@ -18,8 +18,7 @@
 #include <stdbool.h>
 #include <stdint.h>
 
-#define FRSKY_CHECKSUM_GOOD_VALUE 0xFF
-
-bool frskyChecksumIsGood(uint8_t *data, uint8_t length);
+uint8_t frskyCheckSum(uint8_t *data, uint8_t length);
+bool frskyCheckSumIsGood(uint8_t *data, uint8_t length);
 void frskyCheckSumStep(uint16_t *checksum, uint8_t byte);   // Add byte to checksum
 void frskyCheckSumFini(uint16_t *checksum);                 // Finalize checksum

From 552b6f53293a6745dbd065ba9f344539712477ea Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Thu, 14 May 2020 13:59:53 +0200
Subject: [PATCH 162/179] Fix incorrect mixer throttle constrains

---
 src/main/flight/mixer.c | 9 ++++-----
 1 file changed, 4 insertions(+), 5 deletions(-)

diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c
index 41b4b46649..24aab18727 100755
--- a/src/main/flight/mixer.c
+++ b/src/main/flight/mixer.c
@@ -477,14 +477,13 @@ void FAST_CODE mixTable(const float dT)
     int16_t rpyMixRange = rpyMixMax - rpyMixMin;
     int16_t throttleRange;
     int16_t throttleMin, throttleMax;
-    // static int16_t throttlePrevious = 0;   // Store the last throttle direction for deadband transitions
 
     // Find min and max throttle based on condition.
 #ifdef USE_GLOBAL_FUNCTIONS
     if (GLOBAL_FUNCTION_FLAG(GLOBAL_FUNCTION_FLAG_OVERRIDE_THROTTLE)) {
         throttleRangeMin = throttleIdleValue;
         throttleRangeMax = motorConfig()->maxthrottle;
-        mixerThrottleCommand = constrain(globalFunctionValues[GLOBAL_FUNCTION_ACTION_OVERRIDE_THROTTLE], throttleMin, throttleMax); 
+        mixerThrottleCommand = constrain(globalFunctionValues[GLOBAL_FUNCTION_ACTION_OVERRIDE_THROTTLE], throttleRangeMin, throttleRangeMax); 
     } else
 #endif
     if (feature(FEATURE_REVERSIBLE_MOTORS)) {
@@ -516,13 +515,13 @@ void FAST_CODE mixTable(const float dT)
 
         // Throttle scaling to limit max throttle when battery is full
     #ifdef USE_GLOBAL_FUNCTIONS
-        mixerThrottleCommand = ((mixerThrottleCommand - throttleMin) * getThrottleScale(motorConfig()->throttleScale)) + throttleMin;
+        mixerThrottleCommand = ((mixerThrottleCommand - throttleRangeMin) * getThrottleScale(motorConfig()->throttleScale)) + throttleRangeMin;
     #else
-        mixerThrottleCommand = ((mixerThrottleCommand - throttleMin) * motorConfig()->throttleScale) + throttleMin;
+        mixerThrottleCommand = ((mixerThrottleCommand - throttleRangeMin) * motorConfig()->throttleScale) + throttleRangeMin;
     #endif
         // Throttle compensation based on battery voltage
         if (feature(FEATURE_THR_VBAT_COMP) && isAmperageConfigured() && feature(FEATURE_VBAT)) {                
-            mixerThrottleCommand = MIN(throttleMin + (mixerThrottleCommand - throttleMin) * calculateThrottleCompensationFactor(), throttleMax);
+            mixerThrottleCommand = MIN(throttleRangeMin + (mixerThrottleCommand - throttleRangeMin) * calculateThrottleCompensationFactor(), throttleRangeMax);
         }
     }
 

From 1d03477f627c4d96838a7e819744eae2a06763f2 Mon Sep 17 00:00:00 2001
From: "Konstantin (DigitalEntity) Sharlaimov"
 <konstantin.sharlaimov@gmail.com>
Date: Thu, 14 May 2020 14:02:47 +0200
Subject: [PATCH 163/179] [POLICY] Amend the hardware policy with guidelines
 for contacting the development team

---
 docs/policies/NEW_HARDWARE_POLICY.md | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/docs/policies/NEW_HARDWARE_POLICY.md b/docs/policies/NEW_HARDWARE_POLICY.md
index 6d38333a60..2d8794be3d 100644
--- a/docs/policies/NEW_HARDWARE_POLICY.md
+++ b/docs/policies/NEW_HARDWARE_POLICY.md
@@ -46,4 +46,8 @@ If one of the core developers has the hardware in possession they may opt in and
 
 5. It's advised to provide more than one sample to avoid issues with damaged or dead on arrival hardware.
 
+# Guidelines on contacting the team
 
+1. Manufacturers are advised to open a feature request to add support for certain hardware to INAV by following [this link](https://github.com/iNavFlight/inav/issues/new/choose)
+
+2. After opening a feature request, author of the feature request is advised to contact the core development team by [email](mailto:coredev@inavflight.com) mentioning the open feature request and communicate with developer team via email to arrange hardware and specsifications delivery.
\ No newline at end of file

From a54adea74c437e660cf83b0a4819488a34ad7cc2 Mon Sep 17 00:00:00 2001
From: Andi Kanzler <andi_kanzler@gmx.de>
Date: Thu, 14 May 2020 22:11:30 +0200
Subject: [PATCH 164/179] Disable Smartaudio for target OMNIBUS, due to flash
 size.

---
 src/main/target/OMNIBUS/target.h | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h
index 63f7c37209..cdd1b8d33a 100644
--- a/src/main/target/OMNIBUS/target.h
+++ b/src/main/target/OMNIBUS/target.h
@@ -30,6 +30,8 @@
 #define SPI1_MISO_PIN           PA6
 #define SPI1_MOSI_PIN           PA7
 
+#undef USE_VTX_SMARTAUDIO       // Disabled due to flash size
+
 #define USE_EXTI
 
 #define USE_IMU_MPU6000

From fe67103fae0cb5674a81cd343d60d157ce8799e0 Mon Sep 17 00:00:00 2001
From: "Konstantin (DigitalEntity) Sharlaimov"
 <konstantin.sharlaimov@gmail.com>
Date: Fri, 15 May 2020 08:11:57 +0200
Subject: [PATCH 165/179] [BLACKBOX] Fix 32-bit array writes

---
 src/main/blackbox/blackbox.c | 28 +++++++++++++++++++++-------
 1 file changed, 21 insertions(+), 7 deletions(-)

diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c
index 1a490d17e5..d023e9c287 100644
--- a/src/main/blackbox/blackbox.c
+++ b/src/main/blackbox/blackbox.c
@@ -875,7 +875,7 @@ static void writeIntraframe(void)
     blackboxLoggedAnyFrames = true;
 }
 
-static void blackboxWriteMainStateArrayUsingAveragePredictor(int arrOffsetInHistory, int count)
+static void blackboxWriteArrayUsingAveragePredictor16(int arrOffsetInHistory, int count)
 {
     int16_t *curr  = (int16_t*) ((char*) (blackboxHistory[0]) + arrOffsetInHistory);
     int16_t *prev1 = (int16_t*) ((char*) (blackboxHistory[1]) + arrOffsetInHistory);
@@ -889,6 +889,20 @@ static void blackboxWriteMainStateArrayUsingAveragePredictor(int arrOffsetInHist
     }
 }
 
+static void blackboxWriteArrayUsingAveragePredictor32(int arrOffsetInHistory, int count)
+{
+    int32_t *curr  = (int32_t*) ((char*) (blackboxHistory[0]) + arrOffsetInHistory);
+    int32_t *prev1 = (int32_t*) ((char*) (blackboxHistory[1]) + arrOffsetInHistory);
+    int32_t *prev2 = (int32_t*) ((char*) (blackboxHistory[2]) + arrOffsetInHistory);
+
+    for (int i = 0; i < count; i++) {
+        // Predictor is the average of the previous two history states
+        int32_t predictor = ((int64_t)prev1[i] + (int64_t)prev2[i]) / 2;
+
+        blackboxWriteSignedVB(curr[i] - predictor);
+    }
+}
+
 static void writeInterframe(void)
 {
     blackboxMainState_t *blackboxCurrent = blackboxHistory[0];
@@ -1023,16 +1037,16 @@ static void writeInterframe(void)
     blackboxWriteTag8_8SVB(deltas, optionalFieldCount);
 
     //Since gyros, accs and motors are noisy, base their predictions on the average of the history:
-    blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, gyroADC),   XYZ_AXIS_COUNT);
-    blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, accADC), XYZ_AXIS_COUNT);
-    blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, attitude), XYZ_AXIS_COUNT);
+    blackboxWriteArrayUsingAveragePredictor16(offsetof(blackboxMainState_t, gyroADC),   XYZ_AXIS_COUNT);
+    blackboxWriteArrayUsingAveragePredictor16(offsetof(blackboxMainState_t, accADC), XYZ_AXIS_COUNT);
+    blackboxWriteArrayUsingAveragePredictor16(offsetof(blackboxMainState_t, attitude), XYZ_AXIS_COUNT);
     if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_DEBUG)) {
-        blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, debug), DEBUG32_VALUE_COUNT);
+        blackboxWriteArrayUsingAveragePredictor32(offsetof(blackboxMainState_t, debug), DEBUG32_VALUE_COUNT);
     }
-    blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, motor),     getMotorCount());
+    blackboxWriteArrayUsingAveragePredictor16(offsetof(blackboxMainState_t, motor),     getMotorCount());
 
     if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_SERVOS)) {
-        blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, servo),     MAX_SUPPORTED_SERVOS);
+        blackboxWriteArrayUsingAveragePredictor16(offsetof(blackboxMainState_t, servo),     MAX_SUPPORTED_SERVOS);
     }
 
 #ifdef NAV_BLACKBOX

From 7f1a7d5a0bf771b29a7cb592e221536459601fce Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Alberto=20Garc=C3=ADa=20Hierro?= <alberto@garciahierro.com>
Date: Sat, 16 May 2020 16:38:31 +0100
Subject: [PATCH 166/179] [BITARRAY] Bounds check in the first iteration in
 bitArrayFindFirstSet() (#5707)

We already had bounds checking for the 2nd and subsequent 32 bit
blocks, but since the first block was handled separately (due to the
masking needed if it doesn't fall on a 32 bit boundary) it skipped
the check. With this change, any start bit >= the number of bits
in the array will return -1.
---
 src/main/common/bitarray.c         | 18 ++++++++++--------
 src/test/unit/bitarray_unittest.cc | 11 +++++++++++
 2 files changed, 21 insertions(+), 8 deletions(-)

diff --git a/src/main/common/bitarray.c b/src/main/common/bitarray.c
index 92332b9e33..a46a536e3d 100755
--- a/src/main/common/bitarray.c
+++ b/src/main/common/bitarray.c
@@ -75,17 +75,19 @@ int bitArrayFindFirstSet(const bitarrayElement_t *array, unsigned start, size_t
     const uint32_t *end = ptr + (size / 4);
     const uint32_t *p = ptr + start / (8 * 4);
     int ret;
-    // First iteration might need to mask some bits
-    uint32_t mask = 0xFFFFFFFF << (start % (8 * 4));
-    if ((ret = __CTZ(*p & mask)) != 32) {
-        return (((char *)p) - ((char *)ptr)) * 8 + ret;
-    }
-    p++;
-    while (p < end) {
-        if ((ret = __CTZ(*p)) != 32) {
+    if (p < end) {
+        // First iteration might need to mask some bits
+        uint32_t mask = 0xFFFFFFFF << (start % (8 * 4));
+        if ((ret = __CTZ(*p & mask)) != 32) {
             return (((char *)p) - ((char *)ptr)) * 8 + ret;
         }
         p++;
+        while (p < end) {
+            if ((ret = __CTZ(*p)) != 32) {
+                return (((char *)p) - ((char *)ptr)) * 8 + ret;
+            }
+            p++;
+        }
     }
     return -1;
 }
diff --git a/src/test/unit/bitarray_unittest.cc b/src/test/unit/bitarray_unittest.cc
index 6375d828ae..d80f380d82 100644
--- a/src/test/unit/bitarray_unittest.cc
+++ b/src/test/unit/bitarray_unittest.cc
@@ -83,3 +83,14 @@ TEST(BitArrayTest, TestSetClrAll)
         EXPECT_EQ(ii, BITARRAY_FIND_FIRST_SET(p, ii));
     }
 }
+
+TEST(BitArrayTest, TestOutOfBounds)
+{
+    const int bits = 32 * 4;
+
+    BITARRAY_DECLARE(p, bits);
+    BITARRAY_CLR_ALL(p);
+    EXPECT_EQ(-1, BITARRAY_FIND_FIRST_SET(p, 0));
+    EXPECT_EQ(-1, BITARRAY_FIND_FIRST_SET(p, bits));
+    EXPECT_EQ(-1, BITARRAY_FIND_FIRST_SET(p, bits + 1));
+}

From 307f32ac633fb970a37d67afdc5711229a74938d Mon Sep 17 00:00:00 2001
From: Michel Pastor <shellixyz@users.noreply.github.com>
Date: Sat, 16 May 2020 17:38:45 +0200
Subject: [PATCH 167/179] Fix HAL USB vcp serial 64 byte packet receive bug
 (#5732)

---
 src/main/vcp_hal/usbd_cdc_interface.c | 6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/src/main/vcp_hal/usbd_cdc_interface.c b/src/main/vcp_hal/usbd_cdc_interface.c
index 26cf9e9c88..82207670af 100644
--- a/src/main/vcp_hal/usbd_cdc_interface.c
+++ b/src/main/vcp_hal/usbd_cdc_interface.c
@@ -284,6 +284,12 @@ static int8_t CDC_Itf_Receive(uint8_t* Buf, uint32_t *Len)
 {
     rxAvailable = *Len;
     rxBuffPtr = Buf;
+    if (!rxAvailable) {
+        // Received an empty packet, trigger receiving the next packet.
+        // This will happen after a packet that's exactly 64 bytes is received.
+        // The USB protocol requires that an empty (0 byte) packet immediately follow.
+        USBD_CDC_ReceivePacket(&USBD_Device);
+    }
     return (USBD_OK);
 }
 

From 787feb1bf9a79ac031d07d3f4e49832ea820e4ac Mon Sep 17 00:00:00 2001
From: "Konstantin (DigitalEntity) Sharlaimov"
 <konstantin.sharlaimov@gmail.com>
Date: Sat, 16 May 2020 21:20:58 +0200
Subject: [PATCH 168/179] Amend the hardware policy

---
 docs/policies/NEW_HARDWARE_POLICY.md | 46 +++++++++++++++++++---------
 1 file changed, 31 insertions(+), 15 deletions(-)

diff --git a/docs/policies/NEW_HARDWARE_POLICY.md b/docs/policies/NEW_HARDWARE_POLICY.md
index 2d8794be3d..21ea93933f 100644
--- a/docs/policies/NEW_HARDWARE_POLICY.md
+++ b/docs/policies/NEW_HARDWARE_POLICY.md
@@ -1,4 +1,4 @@
-# INAV New Hardware policies
+# INAV Hardware Policy
 
 ## General
 
@@ -10,33 +10,41 @@ To prevent explosive growth of different target and feature count and ensure rea
 
 "Hardware" - physical hardware requiring support from firmware side.
 
+"Requestor" - manufacturer or community member seeking addition of new hardware or target
+
 ## New target additions
 
 New targets are accepted into INAV code if any of the following conditions is satisfied:
 
-1. Board manufacturer provides specs, schematics and production samples are provided to at least two core developers. In this case the new target becomes part of official INAV release.
+1. Requestor is a manufacturer of the hardware or an affiliated community member. Requestor provides specs, schematics and production samples are provided to at least two core developers. In this case the new target becomes part of official INAV release.
 
-2. Community member or board manufacturer provides board samples to at least one core developer and the target already exists in official Cleanflight or Betaflight firmware. In this case the new target may or may not become part of official release based on the decision made by core developers.
+2. Requestor is a community member not affiliated with a manufacturer of the hardware. Requestor provides board samples to at least one core developer and the target is already supported in official Cleanflight or Betaflight firmware. In this case the new target may or may not become part of official release based on the decision made by core developers.
 
-# New hardware support
+3. The new target must meet the following minimal requirements:
+
+  * Onboard sensors include at least the IMU (gyroscope + accelerometer) and barometer
+  * At least 2 hardware serial ports are available with both TX and RX pins
+  * At least 512K of firmware flash memory and at least of 64K of RAM available
+
+## New hardware support
 
 For the hardware which does not require a new target, but still require support from the firmware side the following rules apply:
 
-1. Hardware manufacturer provides specs and production samples for the unsupported hardware to at least two core developers.
+1. Requestor is a manufacturer of the hardware or an affiliated community member. Requestor provides specs and production samples for the unsupported hardware to at least two core developers.
 
-2. Community member or hardware manufacturer provides hardware samples to at least one core developer and the firmware support already exists in official Cleanflight or Betaflight firmware.
+2. Requestor is a community member not affiliated with a manufacturer of the hardware. Requestor provides hardware samples to at least one core developer and the firmware support already exists in official Cleanflight or Betaflight firmware.
 
-# Using own hardware exception
+## Using own hardware exception
 
-If one of the core developers has the hardware in possession they may opt in and submit support for it anyway. In this case the support is not official and is generally not included in official releases.
+If one of the core developers has the hardware in possession they may opt in and submit support for it anyway. In this case the support is not official and may not be included in official releases.
 
-# Providing samples to developers
+## Providing samples to developers
 
-1. Hardware provided to the developers would be considered a donation to the INAV project. Under no circumstances developer will be obliged to return the hardware.
+1. Hardware provided to the developers would be considered a donation to the INAV project. Under no circumstances developer will be obliged to return the hardware or pay for it.
 
-2. Manufacturer or community member providing the hardware bears all the costs of the hardware, delivery and VAT/customs duties. Hardware manufacturer also assumes the responsibility to provide up to date specs, documentation and firmware (if applicable).
+2. Requestor bears all the costs of the hardware, delivery and VAT/customs duties. Hardware manufacturer also assumes the responsibility to provide up to date specs, documentation and firmware (if applicable).
 
-3. Before sending samples the providing party should reach out to developers and agree on specific terms of implementing support for the unsupported hardware. Developers may place additional requirements on a case by case basis and at their sole discretion.
+3. Before sending samples the Requstor should reach out to developers and agree on specific terms of implementing support for the unsupported hardware. Developers may place additional requirements on a case by case basis and at their sole discretion.
 
 4. The new target and new hardware policies do not apply in the following cases. Developers may still chose to apply the "own hardware exception" at their own discretion.
 
@@ -46,8 +54,16 @@ If one of the core developers has the hardware in possession they may opt in and
 
 5. It's advised to provide more than one sample to avoid issues with damaged or dead on arrival hardware.
 
-# Guidelines on contacting the team
+## Implementing support for the new target or hardware
 
-1. Manufacturers are advised to open a feature request to add support for certain hardware to INAV by following [this link](https://github.com/iNavFlight/inav/issues/new/choose)
+1. Pull request to add the new target or hardware may be authored by a contributor outside INAV team or by one of the core developers. 
 
-2. After opening a feature request, author of the feature request is advised to contact the core development team by [email](mailto:coredev@inavflight.com) mentioning the open feature request and communicate with developer team via email to arrange hardware and specsifications delivery.
\ No newline at end of file
+2. There is no obligation to accept a pull request made by an outside contributor. INAV team reserves the right to reject that pull request and re-implement the support or take that pull request as a baseline and amend it.
+
+3. INAV team reserves the right to reject the new target or hardware or remove the support for an unforseen reason including, but not limited to violation of [INAV Code of Conduct](CODE_OF_CONDUCT.md) by the manufacturer or an affiliated outside contributor.
+
+## Guidelines on contacting the team
+
+1. Requstor is advised to open a feature request to add support for certain hardware to INAV by following [this link](https://github.com/iNavFlight/inav/issues/new/choose)
+
+2. After opening a feature request, Requestor is advised to contact the core development team by [email](mailto:coredev@inavflight.com) mentioning the open feature request and communicate with developer team via email to arrange hardware and specsifications delivery.
\ No newline at end of file

From 1bd4364ec1f1bab670335ee4514600850159b44b Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Sat, 16 May 2020 22:15:33 +0200
Subject: [PATCH 169/179] Add I2C requirement

---
 docs/policies/NEW_HARDWARE_POLICY.md | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/docs/policies/NEW_HARDWARE_POLICY.md b/docs/policies/NEW_HARDWARE_POLICY.md
index 21ea93933f..05d1739454 100644
--- a/docs/policies/NEW_HARDWARE_POLICY.md
+++ b/docs/policies/NEW_HARDWARE_POLICY.md
@@ -22,9 +22,10 @@ New targets are accepted into INAV code if any of the following conditions is sa
 
 3. The new target must meet the following minimal requirements:
 
-  * Onboard sensors include at least the IMU (gyroscope + accelerometer) and barometer
+  * Onboard sensors include at least the IMU (gyroscope + accelerometer)
   * At least 2 hardware serial ports are available with both TX and RX pins
   * At least 512K of firmware flash memory and at least of 64K of RAM available
+  * At least one I2C bus broken out (SCL and SDA pins) and not shared with other functions 
 
 ## New hardware support
 

From 0728da487fcb301a366bf0f05f00c5368ef0db7a Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Sun, 17 May 2020 15:18:25 +0200
Subject: [PATCH 170/179] Configure I2C rangefinder on Kakure F7 target

---
 src/main/target/KAKUTEF7/target.h | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/src/main/target/KAKUTEF7/target.h b/src/main/target/KAKUTEF7/target.h
index 82be9590a0..ad86225627 100644
--- a/src/main/target/KAKUTEF7/target.h
+++ b/src/main/target/KAKUTEF7/target.h
@@ -156,6 +156,8 @@
 
 #define TEMPERATURE_I2C_BUS     BUS_I2C1
 
+#define RANGEFINDER_I2C_BUS     BUS_I2C1
+
 #define USE_ADC
 #define ADC_CHANNEL_1_PIN           PC2
 #define ADC_CHANNEL_2_PIN           PC3

From 20c71437642869b9288235fac422b1e8a050132e Mon Sep 17 00:00:00 2001
From: giacomo892 <giacomo1989@gmail.com>
Date: Tue, 19 May 2020 16:34:15 +0200
Subject: [PATCH 171/179] cleanup F722PX target

---
 src/main/target/MATEKF722PX/target.h  | 9 +--------
 src/main/target/MATEKF722PX/target.mk | 2 +-
 2 files changed, 2 insertions(+), 9 deletions(-)

diff --git a/src/main/target/MATEKF722PX/target.h b/src/main/target/MATEKF722PX/target.h
index cc7d33cb42..c5c438faf1 100755
--- a/src/main/target/MATEKF722PX/target.h
+++ b/src/main/target/MATEKF722PX/target.h
@@ -84,11 +84,6 @@
 #define M25P16_CS_PIN           PB12
 #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
 
-#define USE_SDCARD
-#define USE_SDCARD_SPI
-#define SDCARD_SPI_BUS          BUS_SPI2
-#define SDCARD_CS_PIN           PC15
-
 // *************** UART *****************************
 #define USE_VCP
 #define USB_DETECT_PIN          PC14
@@ -120,7 +115,7 @@
      
 #define USE_SOFTSERIAL1
 #define SOFTSERIAL_1_TX_PIN      PA2 //TX2 pad
-#define SOFTSERIAL_1_RX_PIN      PA2
+#define SOFTSERIAL_1_RX_PIN      NONE
 
 #define SERIAL_PORT_COUNT       8
 
@@ -169,5 +164,3 @@
 #define USE_SERIALSHOT
 #define USE_ESC_SENSOR
 
-//#define USE_CAMERA_CONTROL
-//#define CAMERA_CONTROL_PIN         	PB15
diff --git a/src/main/target/MATEKF722PX/target.mk b/src/main/target/MATEKF722PX/target.mk
index 5c061571fb..0db5e55e78 100755
--- a/src/main/target/MATEKF722PX/target.mk
+++ b/src/main/target/MATEKF722PX/target.mk
@@ -1,5 +1,5 @@
 F7X2RE_TARGETS += $(TARGET)
-FEATURES       += SDCARD VCP ONBOARDFLASH MSC
+FEATURES       += VCP ONBOARDFLASH MSC
 
 TARGET_SRC = \
             drivers/accgyro/accgyro_mpu6000.c \

From 37c564c05204dd70b9369fe00cacf596b478b483 Mon Sep 17 00:00:00 2001
From: "Konstantin (DigitalEntity) Sharlaimov"
 <konstantin.sharlaimov@gmail.com>
Date: Thu, 21 May 2020 14:37:44 +0200
Subject: [PATCH 172/179] Check arming blocker flags in a certain priority to
 make sure more important warnings are shown first

---
 src/main/fc/runtime_config.c | 42 +++++++++++++++++++++++++++++++++---
 1 file changed, 39 insertions(+), 3 deletions(-)

diff --git a/src/main/fc/runtime_config.c b/src/main/fc/runtime_config.c
index 60f911bd6b..cb7af593bb 100644
--- a/src/main/fc/runtime_config.c
+++ b/src/main/fc/runtime_config.c
@@ -20,6 +20,7 @@
 
 #include "platform.h"
 
+#include "common/utils.h"
 #include "fc/runtime_config.h"
 
 #include "io/beeper.h"
@@ -39,16 +40,51 @@ const char *armingDisableFlagNames[]= {
 };
 #endif
 
+const armingFlag_e armDisableReasonsChecklist[] = {
+    ARMING_DISABLED_INVALID_SETTING,
+    ARMING_DISABLED_HARDWARE_FAILURE,
+    ARMING_DISABLED_PWM_OUTPUT_ERROR,
+    ARMING_DISABLED_COMPASS_NOT_CALIBRATED,
+    ARMING_DISABLED_ACCELEROMETER_NOT_CALIBRATED,
+    ARMING_DISABLED_RC_LINK,
+    ARMING_DISABLED_NAVIGATION_UNSAFE,
+    ARMING_DISABLED_ARM_SWITCH,
+    ARMING_DISABLED_BOXFAILSAFE,
+    ARMING_DISABLED_BOXKILLSWITCH,
+    ARMING_DISABLED_THROTTLE,
+    ARMING_DISABLED_CLI,
+    ARMING_DISABLED_CMS_MENU,
+    ARMING_DISABLED_OSD_MENU,
+    ARMING_DISABLED_ROLLPITCH_NOT_CENTERED,
+    ARMING_DISABLED_SERVO_AUTOTRIM,
+    ARMING_DISABLED_OOM
+};
+
 armingFlag_e isArmingDisabledReason(void)
 {
-    armingFlag_e flag;
+    // Shortcut, if we don't block arming at all
+    if (!isArmingDisabled()) {
+        return 0;
+    }
+
     armingFlag_e reasons = armingFlags & ARMING_DISABLED_ALL_FLAGS;
-    for (unsigned ii = 0; ii < sizeof(armingFlag_e) * 8; ii++) {
-        flag = 1u << ii;
+
+    // First check for "more important reasons"
+    for (unsigned ii = 0; ii < ARRAYLEN(armDisableReasonsChecklist); ii++) {
+        armingFlag_e flag = armDisableReasonsChecklist[ii];
         if (flag & reasons) {
             return flag;
         }
     }
+
+    // Fallback, we ended up with a blocker flag not included in armDisableReasonsChecklist[]
+    for (unsigned ii = 0; ii < sizeof(armingFlag_e) * 8; ii++) {
+        armingFlag_e flag = 1u << ii;
+        if (flag & reasons) {
+            return flag;
+        }
+    }
+
     return 0;
 }
 

From baa8129432b5833ee5775791959416f0c1901d1f Mon Sep 17 00:00:00 2001
From: "Konstantin (DigitalEntity) Sharlaimov"
 <konstantin.sharlaimov@gmail.com>
Date: Fri, 22 May 2020 14:31:33 +0200
Subject: [PATCH 173/179] Fix incorrect EXTI define on MATEK722PX

---
 src/main/target/MATEKF722PX/target.h | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/src/main/target/MATEKF722PX/target.h b/src/main/target/MATEKF722PX/target.h
index c5c438faf1..38e9a8f380 100755
--- a/src/main/target/MATEKF722PX/target.h
+++ b/src/main/target/MATEKF722PX/target.h
@@ -36,13 +36,12 @@
 
 #define USE_EXTI
 #define USE_MPU_DATA_READY_SIGNAL
+#define GYRO_INT_EXTI           PC4
 
 #define USE_IMU_MPU6000
 #define IMU_MPU6000_ALIGN       CW180_DEG_FLIP
 #define MPU6000_CS_PIN          PB2
 #define MPU6000_SPI_BUS         BUS_SPI1
-#define MPU6000_EXTI_PIN        PC4
-
 
 // *************** I2C /Baro/Mag *********************
 #define USE_I2C

From 16d331df42f431661d706fa496d679904744bca5 Mon Sep 17 00:00:00 2001
From: Jonathan Hudson <jh+github@daria.co.uk>
Date: Fri, 22 May 2020 15:52:24 +0100
Subject: [PATCH 174/179] fix gcc 10 warnings

---
 src/main/drivers/display.c          | 7 +++----
 src/main/io/displayport_frsky_osd.c | 4 ++--
 2 files changed, 5 insertions(+), 6 deletions(-)

diff --git a/src/main/drivers/display.c b/src/main/drivers/display.c
index 1d5e4c3772..5afe475184 100644
--- a/src/main/drivers/display.c
+++ b/src/main/drivers/display.c
@@ -192,9 +192,9 @@ int displayWriteCharWithAttr(displayPort_t *instance, uint8_t x, uint8_t y, uint
         return -1;
     }
     if (displayAttributesRequireEmulation(instance, attr)) {
-        char ec;
-        if (displayEmulateTextAttributes(instance, &ec, 1, &attr)) {
-            c = ec;
+        char ec[2];
+        if (displayEmulateTextAttributes(instance, ec, 1, &attr)) {
+            c = ec[0];
         }
     }
     instance->posX = x + 1;
@@ -317,4 +317,3 @@ void displayInit(displayPort_t *instance, const displayPortVTable_t *vTable)
     instance->maxChar = 0;
     displayUpdateMaxChar(instance);
 }
-
diff --git a/src/main/io/displayport_frsky_osd.c b/src/main/io/displayport_frsky_osd.c
index 8aa0b26061..f5b02d6a50 100644
--- a/src/main/io/displayport_frsky_osd.c
+++ b/src/main/io/displayport_frsky_osd.c
@@ -259,14 +259,14 @@ static void setLineOutlineType(displayCanvas_t *displayCanvas, displayCanvasOutl
 {
     UNUSED(displayCanvas);
 
-    frskyOSDSetLineOutlineType(outlineType);
+    frskyOSDSetLineOutlineType((frskyOSDLineOutlineType_e)outlineType);
 }
 
 static void setLineOutlineColor(displayCanvas_t *displayCanvas, displayCanvasColor_e outlineColor)
 {
     UNUSED(displayCanvas);
 
-    frskyOSDSetLineOutlineColor(outlineColor);
+    frskyOSDSetLineOutlineColor((frskyOSDColor_e)outlineColor);
 }
 
 static void clipToRect(displayCanvas_t *displayCanvas, int x, int y, int w, int h)

From a3253333a79e142d91745384a94ec9732fa86603 Mon Sep 17 00:00:00 2001
From: 4712 <4712@outlook.de>
Date: Sat, 23 May 2020 23:40:26 +0200
Subject: [PATCH 175/179] Update serial 4way-if to v20003

Added STM32L431x and STM32G071x MCU.
Increased timout for flash erase.
---
 src/main/io/serial_4way.c              | 5 +++--
 src/main/io/serial_4way_avrootloader.c | 2 +-
 2 files changed, 4 insertions(+), 3 deletions(-)

diff --git a/src/main/io/serial_4way.c b/src/main/io/serial_4way.c
index 95ef85a465..7cf24890b7 100644
--- a/src/main/io/serial_4way.c
+++ b/src/main/io/serial_4way.c
@@ -72,7 +72,7 @@
 // *** change to adapt Revision
 #define SERIAL_4WAY_VER_MAIN 20
 #define SERIAL_4WAY_VER_SUB_1 (uint8_t) 0
-#define SERIAL_4WAY_VER_SUB_2 (uint8_t) 02
+#define SERIAL_4WAY_VER_SUB_2 (uint8_t) 03
 
 #define SERIAL_4WAY_PROTOCOL_VER 107
 // *** end
@@ -332,7 +332,8 @@ uint16_t _crc_xmodem_update (uint16_t crc, uint8_t data) {
         (pDeviceInfo->words[0] == 0xE8B2))
 
 #define ARM_DEVICE_MATCH ((pDeviceInfo->words[0] == 0x1F06) || \
-        (pDeviceInfo->words[0] == 0x3306) || (pDeviceInfo->words[0] == 0x3406) || (pDeviceInfo->words[0] == 0x3506))
+        (pDeviceInfo->words[0] == 0x3306) || (pDeviceInfo->words[0] == 0x3406) || (pDeviceInfo->words[0] == 0x3506) || \
+        (pDeviceInfo->words[0] == 0x2B06) || (pDeviceInfo->words[0] == 0x4706))
 
 static uint8_t CurrentInterfaceMode;
 
diff --git a/src/main/io/serial_4way_avrootloader.c b/src/main/io/serial_4way_avrootloader.c
index 3b711a1c55..5823006870 100644
--- a/src/main/io/serial_4way_avrootloader.c
+++ b/src/main/io/serial_4way_avrootloader.c
@@ -311,7 +311,7 @@ uint8_t BL_PageErase(ioMem_t *pMem)
     if (BL_SendCMDSetAddress(pMem)) {
         uint8_t sCMD[] = {CMD_ERASE_FLASH, 0x01};
         BL_SendBuf(sCMD, 2);
-        return (BL_GetACK((1400 / START_BIT_TIMEOUT_MS)) == brSUCCESS);
+        return (BL_GetACK((3000 / START_BIT_TIMEOUT_MS)) == brSUCCESS);
     }
     return 0;
 }

From d3d20fd8cdf8ae4655ad09069ab17ba8b5364e37 Mon Sep 17 00:00:00 2001
From: "Konstantin (DigitalEntity) Sharlaimov"
 <konstantin.sharlaimov@gmail.com>
Date: Sun, 24 May 2020 14:22:22 +0200
Subject: [PATCH 176/179] Update CoC to version 2.0 and adapt some wording

---
 docs/policies/CODE_OF_CONDUCT.md | 35 +++++++++++++++++---------------
 1 file changed, 19 insertions(+), 16 deletions(-)

diff --git a/docs/policies/CODE_OF_CONDUCT.md b/docs/policies/CODE_OF_CONDUCT.md
index 9da50525b6..60c654f66a 100644
--- a/docs/policies/CODE_OF_CONDUCT.md
+++ b/docs/policies/CODE_OF_CONDUCT.md
@@ -2,35 +2,38 @@
 
 ## Our Pledge
 
-In the interest of fostering an open and welcoming environment, we as contributors and maintainers pledge to making participation in our project and our community a harassment-free experience for everyone, regardless of age, body size, disability, ethnicity, gender identity and expression, level of experience, nationality, personal appearance, race, religion, or sexual identity and orientation.
+We as members, contributors, and leaders pledge to make participation in our community a harassment-free experience for everyone, regardless of age, body size, visible or invisible disability, ethnicity, sex characteristics, gender identity and expression, level of experience, education, socio-economic status, nationality, personal appearance, race, religion, or sexual identity and orientation.
+
+We pledge to act and interact in ways that contribute to an open, welcoming, diverse, inclusive, and healthy community.
 
 ## Our Standards
 
-Examples of behavior that contributes to creating a positive environment include:
+Examples of behavior that contributes to creating a positive environment include but are not limited to:
 
-* Using welcoming and inclusive language
-* Being respectful of differing viewpoints and experiences
-* Gracefully accepting constructive criticism
-* Focusing on what is best for the community
-* Showing empathy towards other community members
+* Demonstrating empathy and kindness toward other people
+* Being respectful of differing opinions, viewpoints, and experiences
+* Giving and gracefully accepting constructive feedback
+* Accepting responsibility and apologizing to those affected by our mistakes, and learning from the experience
+* Focusing on what is best not just for us as individuals, but for the overall community
 
-Examples of unacceptable behavior by participants include:
+Examples of unacceptable behavior include but are not limited to:
 
-* The use of sexualized language or imagery and unwelcome sexual attention or advances
-* Trolling, insulting/derogatory comments, and personal or political attacks
+* The use of sexualized language or imagery, and sexual attention or advances of any kind
+* Trolling, insulting or derogatory comments, and personal or political attacks
 * Public or private harassment
-* Publishing others' private information, such as a physical or electronic address, without explicit permission
+* Publishing others’ private information, such as a physical or email address, without their explicit permission
 * Other conduct which could reasonably be considered inappropriate in a professional setting
+* Acts of unacceptable behavior on other public resources outside of but in relation to this project
 
 ## Our Responsibilities
 
-Project maintainers are responsible for clarifying the standards of acceptable behavior and are expected to take appropriate and fair corrective action in response to any instances of unacceptable behavior.
+Project maintainers and community leaders are responsible for clarifying and enforcing our standards of acceptable behavior and will take appropriate and fair corrective action in response to any behavior that they deem inappropriate, threatening, offensive, or harmful.
 
-Project maintainers have the right and responsibility to remove, edit, or reject comments, commits, code, wiki edits, issues, and other contributions that are not aligned to this Code of Conduct, or to ban temporarily or permanently any contributor for other behaviors that they deem inappropriate, threatening, offensive, or harmful.
+Project maintainers and community leaders have the right and responsibility to remove, edit, or reject comments, commits, code, wiki edits, issues, and other contributions that are not aligned to this Code of Conduct, and will communicate reasons for moderation decisions when appropriate.
 
 ## Scope
 
-This Code of Conduct applies both within project spaces and in public spaces when an individual is representing the project or its community. Examples of representing a project or community include using an official project e-mail address, posting via an official social media account, or acting as an appointed representative at an online or offline event. Representation of a project may be further defined and clarified by project maintainers.
+This Code of Conduct applies within all community spaces, and also applies when an individual is officially representing the community in public spaces. Examples of representing our community include using an official e-mail address, posting via an official social media account, or acting as an appointed representative at an online or offline event.
 
 ## Enforcement
 
@@ -40,7 +43,7 @@ Project maintainers who do not follow or enforce the Code of Conduct in good fai
 
 ## Attribution
 
-This Code of Conduct is adapted from the [Contributor Covenant][homepage], version 1.4, available at [http://contributor-covenant.org/version/1/4][version]
+This Code of Conduct is adapted from the [Contributor Covenant][homepage],
+version 2.0, available at https://www.contributor-covenant.org/version/2/0/code_of_conduct.html.
 
 [homepage]: http://contributor-covenant.org
-[version]: http://contributor-covenant.org/version/1/4/
\ No newline at end of file

From 333760f4f22b7ca2aef5bdc1c9d191ef8d7b032c Mon Sep 17 00:00:00 2001
From: "Konstantin (DigitalEntity) Sharlaimov"
 <konstantin.sharlaimov@gmail.com>
Date: Sun, 24 May 2020 14:46:46 +0200
Subject: [PATCH 177/179] Fix typos

---
 docs/policies/NEW_HARDWARE_POLICY.md | 22 +++++++++++-----------
 1 file changed, 11 insertions(+), 11 deletions(-)

diff --git a/docs/policies/NEW_HARDWARE_POLICY.md b/docs/policies/NEW_HARDWARE_POLICY.md
index 05d1739454..1e97413480 100644
--- a/docs/policies/NEW_HARDWARE_POLICY.md
+++ b/docs/policies/NEW_HARDWARE_POLICY.md
@@ -10,19 +10,19 @@ To prevent explosive growth of different target and feature count and ensure rea
 
 "Hardware" - physical hardware requiring support from firmware side.
 
-"Requestor" - manufacturer or community member seeking addition of new hardware or target
+"Requester" - manufacturer or community member seeking addition of new hardware or target
 
 ## New target additions
 
 New targets are accepted into INAV code if any of the following conditions is satisfied:
 
-1. Requestor is a manufacturer of the hardware or an affiliated community member. Requestor provides specs, schematics and production samples are provided to at least two core developers. In this case the new target becomes part of official INAV release.
+1. Requester is a manufacturer of the hardware or an affiliated community member. Requester provides specs, schematics and production samples are provided to at least two core developers. In this case the new target becomes part of official INAV release.
 
-2. Requestor is a community member not affiliated with a manufacturer of the hardware. Requestor provides board samples to at least one core developer and the target is already supported in official Cleanflight or Betaflight firmware. In this case the new target may or may not become part of official release based on the decision made by core developers.
+2. Requester is a community member not affiliated with a manufacturer of the hardware. Requester provides board samples to at least one core developer and the target is already supported in official Cleanflight or Betaflight firmware. In this case the new target may or may not become part of official release based on the decision made by core developers.
 
 3. The new target must meet the following minimal requirements:
 
-  * Onboard sensors include at least the IMU (gyroscope + accelerometer)
+  * On-board sensors include at least the IMU (gyroscope + accelerometer)
   * At least 2 hardware serial ports are available with both TX and RX pins
   * At least 512K of firmware flash memory and at least of 64K of RAM available
   * At least one I2C bus broken out (SCL and SDA pins) and not shared with other functions 
@@ -31,9 +31,9 @@ New targets are accepted into INAV code if any of the following conditions is sa
 
 For the hardware which does not require a new target, but still require support from the firmware side the following rules apply:
 
-1. Requestor is a manufacturer of the hardware or an affiliated community member. Requestor provides specs and production samples for the unsupported hardware to at least two core developers.
+1. Requester is a manufacturer of the hardware or an affiliated community member. Requester provides specs and production samples for the unsupported hardware to at least two core developers.
 
-2. Requestor is a community member not affiliated with a manufacturer of the hardware. Requestor provides hardware samples to at least one core developer and the firmware support already exists in official Cleanflight or Betaflight firmware.
+2. Requester is a community member not affiliated with a manufacturer of the hardware. Requester provides hardware samples to at least one core developer and the firmware support already exists in official Cleanflight or Betaflight firmware.
 
 ## Using own hardware exception
 
@@ -43,9 +43,9 @@ If one of the core developers has the hardware in possession they may opt in and
 
 1. Hardware provided to the developers would be considered a donation to the INAV project. Under no circumstances developer will be obliged to return the hardware or pay for it.
 
-2. Requestor bears all the costs of the hardware, delivery and VAT/customs duties. Hardware manufacturer also assumes the responsibility to provide up to date specs, documentation and firmware (if applicable).
+2. Requester bears all the costs of the hardware, delivery and VAT/customs duties. Hardware manufacturer also assumes the responsibility to provide up to date specs, documentation and firmware (if applicable).
 
-3. Before sending samples the Requstor should reach out to developers and agree on specific terms of implementing support for the unsupported hardware. Developers may place additional requirements on a case by case basis and at their sole discretion.
+3. Before sending samples the Requester should reach out to developers and agree on specific terms of implementing support for the unsupported hardware. Developers may place additional requirements on a case by case basis and at their sole discretion.
 
 4. The new target and new hardware policies do not apply in the following cases. Developers may still chose to apply the "own hardware exception" at their own discretion.
 
@@ -61,10 +61,10 @@ If one of the core developers has the hardware in possession they may opt in and
 
 2. There is no obligation to accept a pull request made by an outside contributor. INAV team reserves the right to reject that pull request and re-implement the support or take that pull request as a baseline and amend it.
 
-3. INAV team reserves the right to reject the new target or hardware or remove the support for an unforseen reason including, but not limited to violation of [INAV Code of Conduct](CODE_OF_CONDUCT.md) by the manufacturer or an affiliated outside contributor.
+3. INAV team reserves the right to reject the new target or hardware or remove the support for an unforeseen reason including, but not limited to violation of [INAV Code of Conduct](CODE_OF_CONDUCT.md) by the manufacturer or an affiliated outside contributor.
 
 ## Guidelines on contacting the team
 
-1. Requstor is advised to open a feature request to add support for certain hardware to INAV by following [this link](https://github.com/iNavFlight/inav/issues/new/choose)
+1. Requester is advised to open a feature request to add support for certain hardware to INAV by following [this link](https://github.com/iNavFlight/inav/issues/new/choose)
 
-2. After opening a feature request, Requestor is advised to contact the core development team by [email](mailto:coredev@inavflight.com) mentioning the open feature request and communicate with developer team via email to arrange hardware and specsifications delivery.
\ No newline at end of file
+2. After opening a feature request, Requester is advised to contact the core development team by [email](mailto:coredev@inavflight.com) mentioning the open feature request and communicate with developer team via email to arrange hardware and specifications delivery.

From 8bf1f11866aba4877e9a68784396fd815c5937dc Mon Sep 17 00:00:00 2001
From: giacomo892 <giacomo1989@gmail.com>
Date: Mon, 25 May 2020 09:38:31 +0200
Subject: [PATCH 178/179] Update Cli.md

---
 docs/Cli.md | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/docs/Cli.md b/docs/Cli.md
index d5f5e2b2d2..717037223d 100644
--- a/docs/Cli.md
+++ b/docs/Cli.md
@@ -349,6 +349,7 @@ A shorter form is also supported to enable and disable functions using `serial <
 |  osd_estimations_wind_compensation  | ON | Use wind estimation for remaining flight time/distance estimation |
 |  osd_failsafe_switch_layout  | OFF | If enabled the OSD automatically switches to the first layout during failsafe |
 |  osd_temp_label_align | LEFT | Allows to chose between left and right alignment for the OSD temperature sensor labels. Valid values are `LEFT` and `RIGHT` |
+|  osd_ahi_style | DEFAULT | Sets OSD Artificial Horizon style "DEFAULT" or "LINE" for the FrSky Graphical OSD. |
 |  display_force_sw_blink  | OFF | OFF = OSD hardware blink / ON = OSD software blink. If OSD warning text/values are invisible, try setting this to ON |
 |  magzero_x  | 0 | Magnetometer calibration X offset. If its 0 none offset has been applied and calibration is failed. |
 |  magzero_y  | 0 | Magnetometer calibration Y offset. If its 0 none offset has been applied and calibration is failed. |
@@ -373,6 +374,9 @@ A shorter form is also supported to enable and disable functions using `serial <
 |  nav_fw_pos_xy_p  | 75 | P gain of 2D trajectory PID controller. Play with this to get a straight line between waypoints or a straight RTH |
 |  nav_fw_pos_xy_i  | 5 | I gain of 2D trajectory PID controller. Too high and there will be overshoot in trajectory. Better start tuning with zero |
 |  nav_fw_pos_xy_d  | 8 | D gain of 2D trajectory PID controller. Too high and there will be overshoot in trajectory. Better start tuning with zero |
+|  nav_fw_pos_hdg_p  | 60 | P gain of heading PID controller. (Fixedwing) |
+|  nav_fw_pos_hdg_i  | 0 | I gain of heading trajectory PID controller. (Fixedwing) |
+|  nav_fw_pos_hdg_d  | 0 | D gain of heading trajectory PID controller. (Fixedwing) |
 |  nav_mc_heading_p  | 60 | P gain of Heading Hold controller (Multirotor) |
 |  nav_fw_heading_p  | 60 | P gain of Heading Hold controller (Fixedwing) |
 |  deadband  | 5 | These are values (in us) by how much RC input can be different before it's considered valid. For transmitters with jitter on outputs, this value can be increased. Defaults are zero, but can be increased up to 10 or so if rc inputs twitch while idle. |

From e9c5d59c3a99885adff7ed87130873b950674e58 Mon Sep 17 00:00:00 2001
From: giacomo892 <giacomo1989@gmail.com>
Date: Mon, 25 May 2020 09:41:48 +0200
Subject: [PATCH 179/179] Update Cli.md

---
 docs/Cli.md | 1 +
 1 file changed, 1 insertion(+)

diff --git a/docs/Cli.md b/docs/Cli.md
index 717037223d..19a5f0aa9c 100644
--- a/docs/Cli.md
+++ b/docs/Cli.md
@@ -232,6 +232,7 @@ A shorter form is also supported to enable and disable functions using `serial <
 |  nav_fw_launch_max_altitude | 0 | Altitude (centimeters) at which LAUNCH mode will be turned off and regular flight mode will take over [0-60000]. |
 |  nav_fw_land_dive_angle  | 2 | Dive angle that airplane will use during final landing phase. During dive phase, motor is stopped or IDLE and roll control is locked to 0 degrees |
 |  nav_fw_cruise_yaw_rate  | 20 | Max YAW rate when NAV CRUISE mode is enabled (0=disable control via yaw stick) [dps]|
+| nav_use_fw_yaw_control | OFF | Enables or Disables the use of the heading PID controller on fixed wing |
 |  serialrx_provider  | SPEK1024 | When feature SERIALRX is enabled, this allows connection to several receivers which output data via digital interface resembling serial. See RX section. |
 |  serialrx_halfduplex  | OFF | Allow serial receiver to operate on UART TX pin. With some receivers will allow control and telemetry over a single wire |
 |  serialrx_inverted     | OFF | Reverse the serial inversion of the  serial RX protocol. When this value is OFF, each protocol will use its default signal (e.g. SBUS will use an inverted signal). Some OpenLRS receivers produce a non-inverted SBUS signal. This setting supports this type of receivers (including modified FrSKY). |