diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c
index ba21a2acea..397ebf9273 100644
--- a/src/main/blackbox/blackbox.c
+++ b/src/main/blackbox/blackbox.c
@@ -449,6 +449,7 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition)
 
     case FLIGHT_LOG_FIELD_CONDITION_NEVER:
         return false;
+
     default:
         return false;
     }
@@ -456,11 +457,8 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition)
 
 static void blackboxBuildConditionCache(void)
 {
-    FlightLogFieldCondition cond;
-
     blackboxConditionCache = 0;
-
-    for (cond = FLIGHT_LOG_FIELD_CONDITION_FIRST; cond <= FLIGHT_LOG_FIELD_CONDITION_LAST; cond++) {
+    for (FlightLogFieldCondition cond = FLIGHT_LOG_FIELD_CONDITION_FIRST; cond <= FLIGHT_LOG_FIELD_CONDITION_LAST; cond++) {
         if (testBlackboxConditionUncached(cond)) {
             blackboxConditionCache |= 1 << cond;
         }
@@ -765,10 +763,10 @@ static void loadSlowState(blackboxSlowState_t *slow)
  * If allowPeriodicWrite is true, the frame is also logged if it has been more than SLOW_FRAME_INTERVAL logging iterations
  * since the field was last logged.
  */
-static void writeSlowFrameIfNeeded(bool allowPeriodicWrite)
+static bool writeSlowFrameIfNeeded(void)
 {
     // Write the slow frame peridocially so it can be recovered if we ever lose sync
-    bool shouldWrite = allowPeriodicWrite && blackboxSlowFrameIterationTimer >= SLOW_FRAME_INTERVAL;
+    bool shouldWrite = blackboxSlowFrameIterationTimer >= SLOW_FRAME_INTERVAL;
 
     if (shouldWrite) {
         loadSlowState(&slowHistory);
@@ -788,15 +786,7 @@ static void writeSlowFrameIfNeeded(bool allowPeriodicWrite)
     if (shouldWrite) {
         writeSlowFrame();
     }
-}
-
-static int gcd(int num, int denom)
-{
-    if (denom == 0) {
-        return num;
-    }
-
-    return gcd(denom, num % denom);
+    return shouldWrite;
 }
 
 void blackboxValidateConfig(void)
@@ -832,6 +822,13 @@ void blackboxValidateConfig(void)
     }
 }
 
+static void blackboxResetIterationTimers(void)
+{
+    blackboxIteration = 0;
+    blackboxPFrameIndex = 0;
+    blackboxIFrameIndex = 0;
+}
+
 /**
  * Start Blackbox logging if it is not already running. Intended to be called upon arming.
  */
@@ -861,11 +858,9 @@ static void blackboxStart(void)
      */
     blackboxBuildConditionCache();
 
-	blackboxModeActivationConditionPresent = isModeActivationConditionPresent(modeActivationConditions(0), BOXBLACKBOX);
+    blackboxModeActivationConditionPresent = isModeActivationConditionPresent(modeActivationConditions(0), BOXBLACKBOX);
 
-    blackboxIteration = 0;
-    blackboxPFrameIndex = 0;
-    blackboxIFrameIndex = 0;
+    blackboxResetIterationTimers();
 
     /*
      * Record the beeper's current idea of the last arming beep time, so that we can detect it changing when
@@ -1011,50 +1006,36 @@ static void writeGPSFrame(timeUs_t currentTimeUs)
 static void loadMainState(timeUs_t currentTimeUs)
 {
     blackboxMainState_t *blackboxCurrent = blackboxHistory[0];
-    int i;
 
     blackboxCurrent->time = currentTimeUs;
 
-    for (i = 0; i < XYZ_AXIS_COUNT; i++) {
+    for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
         blackboxCurrent->axisPID_P[i] = axisPID_P[i];
-    }
-    for (i = 0; i < XYZ_AXIS_COUNT; i++) {
         blackboxCurrent->axisPID_I[i] = axisPID_I[i];
-    }
-    for (i = 0; i < XYZ_AXIS_COUNT; i++) {
         blackboxCurrent->axisPID_D[i] = axisPID_D[i];
+        blackboxCurrent->gyroADC[i] = lrintf(gyro.gyroADCf[i]);
+        blackboxCurrent->accSmooth[i] = acc.accSmooth[i];
+#ifdef MAG
+        blackboxCurrent->magADC[i] = mag.magADC[i];
+#endif
     }
 
-    for (i = 0; i < 4; i++) {
+    for (int i = 0; i < 4; i++) {
         blackboxCurrent->rcCommand[i] = rcCommand[i];
     }
 
-    for (i = 0; i < XYZ_AXIS_COUNT; i++) {
-        blackboxCurrent->gyroADC[i] = lrintf(gyro.gyroADCf[i]);
-    }
-
-    for (i = 0; i < XYZ_AXIS_COUNT; i++) {
-        blackboxCurrent->accSmooth[i] = acc.accSmooth[i];
-    }
-
-    for (i = 0; i < 4; i++) {
+    for (int i = 0; i < DEBUG16_VALUE_COUNT; i++) {
         blackboxCurrent->debug[i] = debug[i];
     }
 
     const int motorCount = getMotorCount();
-    for (i = 0; i < motorCount; i++) {
+    for (int i = 0; i < motorCount; i++) {
         blackboxCurrent->motor[i] = motor[i];
     }
 
     blackboxCurrent->vbatLatest = getBatteryVoltageLatest();
     blackboxCurrent->amperageLatest = getAmperageLatest();
 
-#ifdef MAG
-    for (i = 0; i < XYZ_AXIS_COUNT; i++) {
-        blackboxCurrent->magADC[i] = mag.magADC[i];
-    }
-#endif
-
 #ifdef BARO
     blackboxCurrent->BaroAlt = baro.BaroAlt;
 #endif
@@ -1425,6 +1406,24 @@ static bool blackboxShouldLogIFrame(void)
     return blackboxPFrameIndex == 0;
 }
 
+/*
+ * If the GPS home point has been updated, or every 128 I-frames (~10 seconds), write the
+ * GPS home position.
+ *
+ * We write it periodically so that if one Home Frame goes missing, the GPS coordinates can
+ * still be interpreted correctly.
+ */
+#ifdef GPS
+STATIC_UNIT_TESTED bool blackboxShouldLogGpsHomeFrame(void)
+{
+    if (GPS_home[0] != gpsHistory.GPS_home[0] || GPS_home[1] != gpsHistory.GPS_home[1]
+        || (blackboxPFrameIndex == BLACKBOX_I_INTERVAL / 2 && blackboxIFrameIndex % 128 == 0)) {
+        return true;
+    }
+    return false;
+}
+#endif
+
 // Called once every FC loop in order to keep track of how many FC loop iterations have passed
 static void blackboxAdvanceIterationTimers(void)
 {
@@ -1447,7 +1446,9 @@ static void blackboxLogIteration(timeUs_t currentTimeUs)
          * Don't log a slow frame if the slow data didn't change ("I" frames are already large enough without adding
          * an additional item to write at the same time). Unless we're *only* logging "I" frames, then we have no choice.
          */
-        writeSlowFrameIfNeeded(blackboxIsOnlyLoggingIntraframes());
+        if (blackboxIsOnlyLoggingIntraframes()) {
+            writeSlowFrameIfNeeded();
+        }
 
         loadMainState(currentTimeUs);
         writeIntraframe();
@@ -1460,23 +1461,14 @@ static void blackboxLogIteration(timeUs_t currentTimeUs)
              * We assume that slow frames are only interesting in that they aid the interpretation of the main data stream.
              * So only log slow frames during loop iterations where we log a main frame.
              */
-            writeSlowFrameIfNeeded(true);
+            writeSlowFrameIfNeeded();
 
             loadMainState(currentTimeUs);
             writeInterframe();
         }
 #ifdef GPS
         if (feature(FEATURE_GPS)) {
-            /*
-             * If the GPS home point has been updated, or every 128 intraframes (~10 seconds), write the
-             * GPS home position.
-             *
-             * We write it periodically so that if one Home Frame goes missing, the GPS coordinates can
-             * still be interpreted correctly.
-             */
-            if (GPS_home[0] != gpsHistory.GPS_home[0] || GPS_home[1] != gpsHistory.GPS_home[1]
-                || (blackboxPFrameIndex == BLACKBOX_I_INTERVAL / 2 && blackboxIFrameIndex % 128 == 0)) {
-
+            if (blackboxShouldLogGpsHomeFrame()) {
                 writeGPSHomeFrame();
                 writeGPSFrame(currentTimeUs);
             } else if (GPS_numSat != gpsHistory.GPS_numSat || GPS_coord[0] != gpsHistory.GPS_coord[0]
@@ -1497,8 +1489,6 @@ static void blackboxLogIteration(timeUs_t currentTimeUs)
  */
 void blackboxUpdate(timeUs_t currentTimeUs)
 {
-    int i;
-  
     switch (blackboxState) {
     case BLACKBOX_STATE_STOPPED:
         if (ARMING_FLAG(ARMED)) {
@@ -1526,7 +1516,7 @@ void blackboxUpdate(timeUs_t currentTimeUs)
          */
         if (millis() > xmitState.u.startTime + 100) {
             if (blackboxDeviceReserveBufferSpace(BLACKBOX_TARGET_HEADER_BUDGET_PER_ITERATION) == BLACKBOX_RESERVE_SUCCESS) {
-                for (i = 0; i < BLACKBOX_TARGET_HEADER_BUDGET_PER_ITERATION && blackboxHeader[xmitState.headerIndex] != '\0'; i++, xmitState.headerIndex++) {
+                for (int i = 0; i < BLACKBOX_TARGET_HEADER_BUDGET_PER_ITERATION && blackboxHeader[xmitState.headerIndex] != '\0'; i++, xmitState.headerIndex++) {
                     blackboxWrite(blackboxHeader[xmitState.headerIndex]);
                     blackboxHeaderBudget--;
                 }
diff --git a/src/main/blackbox/blackbox_encoding.c b/src/main/blackbox/blackbox_encoding.c
index dfa708e145..d0893bca81 100644
--- a/src/main/blackbox/blackbox_encoding.c
+++ b/src/main/blackbox/blackbox_encoding.c
@@ -231,6 +231,129 @@ void blackboxWriteTag2_3S32(int32_t *values)
     }
 }
 
+/**
+ * Write a 2 bit tag followed by 3 signed fields of 2, 554, 877 or 32 bits
+ */
+int blackboxWriteTag2_3SVariable(int32_t *values)
+{
+    static const int FIELD_COUNT = 3;
+    enum {
+        BITS_2  = 0,
+        BITS_554  = 1,
+        BITS_877  = 2,
+        BITS_32 = 3
+    };
+
+    enum {
+        BYTES_1  = 0,
+        BYTES_2  = 1,
+        BYTES_3  = 2,
+        BYTES_4  = 3
+    };
+
+
+    /*
+     * Find out how many bits the largest value requires to encode, and use it to choose one of the packing schemes
+     * below:
+     *
+     * Selector possibilities
+     *
+     * 2 bits per field  ss11 2233,
+     * 554 bits per field  ss11 1112 2222 3333
+     * 877 bits per field  ss11 1111 1122 2222 2333 3333
+     * 32 bits per field sstt tttt followed by fields of various byte counts
+     */
+    int selector = BITS_2;
+    int selector2 = 0;
+    // Require more than 877 bits?
+    if (values[0] >= 256 || values[0] < -256
+            || values[1] >= 128 || values[1] < -128
+            || values[2] >= 128 || values[2] < -128) {
+        selector = BITS_32;
+   // Require more than 554 bits?
+    } else if (values[0] >= 16 || values[0] < -16
+            || values[1] >= 16 || values[1] < -16
+            || values[2] >= 8 || values[2] < -8) {
+        selector = BITS_877;
+        // Require more than 2 bits?
+    } else if (values[0] >= 2 || values[0] < -2
+            || values[1] >= 2 || values[1] < -2
+            || values[2] >= 2 || values[2] < -2) {
+        selector = BITS_554;
+    }
+
+    switch (selector) {
+    case BITS_2:
+        blackboxWrite((selector << 6) | ((values[0] & 0x03) << 4) | ((values[1] & 0x03) << 2) | (values[2] & 0x03));
+        break;
+    case BITS_554:
+        // 554 bits per field  ss11 1112 2222 3333
+        blackboxWrite((selector << 6) | ((values[0] & 0x1F) << 1) | ((values[1] & 0x1F) >> 4));
+        blackboxWrite(((values[1] & 0x0F) << 4) | (values[2] & 0x0F));
+        break;
+    case BITS_877:
+        // 877 bits per field  ss11 1111 1122 2222 2333 3333
+        blackboxWrite((selector << 6) | ((values[0] & 0xFF) >> 2));
+        blackboxWrite(((values[0] & 0x03) << 6) | ((values[1] & 0x7F) >> 1));
+        blackboxWrite(((values[1] & 0x01) << 7) | (values[2] & 0x7F));
+        break;
+    case BITS_32:
+        /*
+         * Do another round to compute a selector for each field, assuming that they are at least 8 bits each
+         *
+         * Selector2 field possibilities
+         * 0 - 8 bits
+         * 1 - 16 bits
+         * 2 - 24 bits
+         * 3 - 32 bits
+         */
+        selector2 = 0;
+        //Encode in reverse order so the first field is in the low bits:
+        for (int x = FIELD_COUNT - 1; x >= 0; x--) {
+            selector2 <<= 2;
+
+            if (values[x] < 128 && values[x] >= -128) {
+                selector2 |= BYTES_1;
+            } else if (values[x] < 32768 && values[x] >= -32768) {
+                selector2 |= BYTES_2;
+            } else if (values[x] < 8388608 && values[x] >= -8388608) {
+                selector2 |= BYTES_3;
+            } else {
+                selector2 |= BYTES_4;
+            }
+        }
+
+        //Write the selectors
+        blackboxWrite((selector << 6) | selector2);
+
+        //And now the values according to the selectors we picked for them
+        for (int x = 0; x < FIELD_COUNT; x++, selector2 >>= 2) {
+            switch (selector2 & 0x03) {
+            case BYTES_1:
+                blackboxWrite(values[x]);
+                break;
+            case BYTES_2:
+                blackboxWrite(values[x]);
+                blackboxWrite(values[x] >> 8);
+                break;
+            case BYTES_3:
+                blackboxWrite(values[x]);
+                blackboxWrite(values[x] >> 8);
+                blackboxWrite(values[x] >> 16);
+                break;
+            case BYTES_4:
+                blackboxWrite(values[x]);
+                blackboxWrite(values[x] >> 8);
+                blackboxWrite(values[x] >> 16);
+                blackboxWrite(values[x] >> 24);
+                break;
+            }
+        }
+    break;
+    }
+    return selector;
+}
+
 /**
  * Write an 8-bit selector followed by four signed fields of size 0, 4, 8 or 16 bits.
  */
diff --git a/src/main/blackbox/blackbox_encoding.h b/src/main/blackbox/blackbox_encoding.h
index 1293ed45ce..2cdb6d8388 100644
--- a/src/main/blackbox/blackbox_encoding.h
+++ b/src/main/blackbox/blackbox_encoding.h
@@ -27,6 +27,7 @@ void blackboxWriteSignedVBArray(int32_t *array, int count);
 void blackboxWriteSigned16VBArray(int16_t *array, int count);
 void blackboxWriteS16(int16_t value);
 void blackboxWriteTag2_3S32(int32_t *values);
+int blackboxWriteTag2_3SVariable(int32_t *values);
 void blackboxWriteTag8_4S16(int32_t *values);
 void blackboxWriteTag8_8SVB(int32_t *values, int valueCount);
 void blackboxWriteU32(int32_t value);
diff --git a/src/main/blackbox/blackbox_fielddefs.h b/src/main/blackbox/blackbox_fielddefs.h
index 3d34216c5a..b1b9af001e 100644
--- a/src/main/blackbox/blackbox_fielddefs.h
+++ b/src/main/blackbox/blackbox_fielddefs.h
@@ -97,7 +97,8 @@ typedef enum FlightLogFieldEncoding {
     FLIGHT_LOG_FIELD_ENCODING_TAG8_8SVB       = 6,
     FLIGHT_LOG_FIELD_ENCODING_TAG2_3S32       = 7,
     FLIGHT_LOG_FIELD_ENCODING_TAG8_4S16       = 8,
-    FLIGHT_LOG_FIELD_ENCODING_NULL            = 9 // Nothing is written to the file, take value to be zero
+    FLIGHT_LOG_FIELD_ENCODING_NULL            = 9, // Nothing is written to the file, take value to be zero
+    FLIGHT_LOG_FIELD_ENCODING_TAG2_3SVARIABLE = 10
 } FlightLogFieldEncoding;
 
 typedef enum FlightLogFieldSign {
diff --git a/src/main/cms/cms.c b/src/main/cms/cms.c
index cf0ce0bf35..a1ac4c6854 100644
--- a/src/main/cms/cms.c
+++ b/src/main/cms/cms.c
@@ -21,6 +21,7 @@
  CMS-displayPort separation by jflyper and martinbudden
  */
 
+//#define CMS_PAGE_DEBUG // For multi-page/menu debugging
 //#define CMS_MENU_DEBUG // For external menu content creators
 
 #include <stdbool.h>
@@ -129,19 +130,20 @@ static displayPort_t *cmsDisplayPortSelectNext(void)
 
 static bool cmsInMenu = false;
 
-STATIC_UNIT_TESTED const CMS_Menu *currentMenu;    // Points to top entry of the current page
+typedef struct cmsCtx_s {
+    const CMS_Menu *menu;         // menu for this context
+    uint8_t page;                 // page in the menu
+    int8_t cursorRow;             // cursorRow in the page
+} cmsCtx_t;
 
-// XXX Does menu backing support backing into second page???
-
-static const CMS_Menu *menuStack[10];  // Stack to save menu transition
-static uint8_t menuStackHistory[10];// cursorRow in a stacked menu
+static cmsCtx_t menuStack[10];
 static uint8_t menuStackIdx = 0;
 
-static OSD_Entry *pageTop;       // Points to top entry of the current page
-static OSD_Entry *pageTopAlt;    // Only 2 pages are allowed (for now)
-static uint8_t maxRow;           // Max row in the current page
+static int8_t pageCount;         // Number of pages in the current menu
+static OSD_Entry *pageTop;       // First entry for the current page
+static uint8_t pageMaxRow;       // Max row in the current page
 
-static int8_t cursorRow;
+static cmsCtx_t currentCtx;
 
 #ifdef CMS_MENU_DEBUG // For external menu content creators
 
@@ -164,19 +166,52 @@ static CMS_Menu menuErr = {
 };
 #endif
 
+#ifdef CMS_PAGE_DEBUG
+#define cmsPageDebug() { \
+    debug[0] = pageCount; \
+    debug[1] = currentCtx.page; \
+    debug[2] = pageMaxRow; \
+    debug[3] = currentCtx.cursorRow; } struct _dummy
+#else
+#define cmsPageDebug()
+#endif
+
 static void cmsUpdateMaxRow(displayPort_t *instance)
 {
-    maxRow = 0;
+    pageMaxRow = 0;
 
     for (const OSD_Entry *ptr = pageTop; ptr->type != OME_END; ptr++) {
-        maxRow++;
+        pageMaxRow++;
     }
 
-    if (maxRow >  MAX_MENU_ITEMS(instance)) {
-        maxRow = MAX_MENU_ITEMS(instance);
+    if (pageMaxRow >  MAX_MENU_ITEMS(instance)) {
+        pageMaxRow = MAX_MENU_ITEMS(instance);
     }
 
-    maxRow--;
+    pageMaxRow--;
+}
+
+static uint8_t cmsCursorAbsolute(displayPort_t *instance)
+{
+    return currentCtx.cursorRow + currentCtx.page * MAX_MENU_ITEMS(instance);
+}
+
+static void cmsPageSelect(displayPort_t *instance, int8_t newpage)
+{
+    currentCtx.page = (newpage + pageCount) % pageCount;
+    pageTop = &currentCtx.menu->entries[currentCtx.page * MAX_MENU_ITEMS(instance)];
+    cmsUpdateMaxRow(instance);
+    displayClearScreen(instance);
+}
+
+static void cmsPageNext(displayPort_t *instance)
+{
+    cmsPageSelect(instance, currentCtx.page + 1);
+}
+
+static void cmsPagePrev(displayPort_t *instance)
+{
+    cmsPageSelect(instance, currentCtx.page - 1);
 }
 
 static void cmsFormatFloat(int32_t value, char *floatString)
@@ -377,7 +412,7 @@ static void cmsDrawMenu(displayPort_t *pDisplay, uint32_t currentTimeUs)
 
     uint8_t i;
     OSD_Entry *p;
-    uint8_t top = (pDisplay->rows - maxRow) / 2 - 1;
+    uint8_t top = (pDisplay->rows - pageMaxRow) / 2 - 1;
 
     // Polled (dynamic) value display denominator.
 
@@ -396,17 +431,9 @@ static void cmsDrawMenu(displayPort_t *pDisplay, uint32_t currentTimeUs)
             SET_PRINTLABEL(p);
             SET_PRINTVALUE(p);
         }
-
-        if (i > MAX_MENU_ITEMS(pDisplay)) // max per page
-        {
-            pageTopAlt = pageTop + MAX_MENU_ITEMS(pDisplay);
-            if (pageTopAlt->type == OME_END)
-                pageTopAlt = NULL;
-        }
-
         pDisplay->cleared = false;
     } else if (drawPolled) {
-        for (p = pageTop ; p <= pageTop + maxRow ; p++) {
+        for (p = pageTop ; p <= pageTop + pageMaxRow ; p++) {
             if (IS_DYNAMIC(p))
                 SET_PRINTVALUE(p);
         }
@@ -414,19 +441,21 @@ static void cmsDrawMenu(displayPort_t *pDisplay, uint32_t currentTimeUs)
 
     // Cursor manipulation
 
-    while ((pageTop + cursorRow)->type == OME_Label) // skip label
-        cursorRow++;
+    while ((pageTop + currentCtx.cursorRow)->type == OME_Label) // skip label
+        currentCtx.cursorRow++;
 
-    if (pDisplay->cursorRow >= 0 && cursorRow != pDisplay->cursorRow) {
+    cmsPageDebug();
+
+    if (pDisplay->cursorRow >= 0 && currentCtx.cursorRow != pDisplay->cursorRow) {
         room -= displayWrite(pDisplay, LEFT_MENU_COLUMN, pDisplay->cursorRow + top, "  ");
     }
 
     if (room < 30)
         return;
 
-    if (pDisplay->cursorRow != cursorRow) {
-        room -= displayWrite(pDisplay, LEFT_MENU_COLUMN, cursorRow + top, " >");
-        pDisplay->cursorRow = cursorRow;
+    if (pDisplay->cursorRow != currentCtx.cursorRow) {
+        room -= displayWrite(pDisplay, LEFT_MENU_COLUMN, currentCtx.cursorRow + top, " >");
+        pDisplay->cursorRow = currentCtx.cursorRow;
     }
 
     if (room < 30)
@@ -458,46 +487,58 @@ static void cmsDrawMenu(displayPort_t *pDisplay, uint32_t currentTimeUs)
     }
 }
 
+static void cmsMenuCountPage(displayPort_t *pDisplay)
+{
+    OSD_Entry *p;
+    for (p = currentCtx.menu->entries; p->type != OME_END; p++);
+    pageCount = (p - currentCtx.menu->entries - 1) / MAX_MENU_ITEMS(pDisplay) + 1;
+}
+
 long cmsMenuChange(displayPort_t *pDisplay, const void *ptr)
 {
     CMS_Menu *pMenu = (CMS_Menu *)ptr;
 
-    if (pMenu) {
+    if (!pMenu) {
+        return 0;
+    }
+
 #ifdef CMS_MENU_DEBUG
-        if (pMenu->GUARD_type != OME_MENU) {
-            // ptr isn't pointing to a CMS_Menu.
-            if (pMenu->GUARD_type <= OME_MAX) {
-                strncpy(menuErrLabel, pMenu->GUARD_text, sizeof(menuErrLabel) - 1);
-            } else {
-                strncpy(menuErrLabel, "LABEL UNKNOWN", sizeof(menuErrLabel) - 1);
-            }
-            pMenu = &menuErr;
+    if (pMenu->GUARD_type != OME_MENU) {
+        // ptr isn't pointing to a CMS_Menu.
+        if (pMenu->GUARD_type <= OME_MAX) {
+            strncpy(menuErrLabel, pMenu->GUARD_text, sizeof(menuErrLabel) - 1);
+        } else {
+            strncpy(menuErrLabel, "LABEL UNKNOWN", sizeof(menuErrLabel) - 1);
         }
+        pMenu = &menuErr;
+    }
 #endif
 
+    if (pMenu != currentCtx.menu) {
         // Stack the current menu and move to a new menu.
-        // The (pMenu == curretMenu) case occurs when reopening for display sw
 
-        if (pMenu != currentMenu) {
-            menuStack[menuStackIdx] = currentMenu;
-            cursorRow += pageTop - currentMenu->entries; // Convert cursorRow to absolute value
-            menuStackHistory[menuStackIdx] = cursorRow;
-            menuStackIdx++;
+        menuStack[menuStackIdx++] = currentCtx;
 
-            currentMenu = pMenu;
-            cursorRow = 0;
+        currentCtx.menu = pMenu;
+        currentCtx.cursorRow = 0;
 
-            if (pMenu->onEnter)
-                pMenu->onEnter();
-        }
+        if (pMenu->onEnter)
+            pMenu->onEnter();
 
-        pageTop = currentMenu->entries;
-        pageTopAlt = NULL;
+        cmsMenuCountPage(pDisplay);
+        cmsPageSelect(pDisplay, 0);
+    } else {
+        // The (pMenu == curretMenu) case occurs when reopening for display cycling
+        // currentCtx.cursorRow has been saved as absolute; convert it back to page + relative
 
-        displayClearScreen(pDisplay);
-        cmsUpdateMaxRow(pDisplay);
+        int8_t cursorAbs = currentCtx.cursorRow;
+        currentCtx.cursorRow = cursorAbs % MAX_MENU_ITEMS(pDisplay);
+        cmsMenuCountPage(pDisplay);
+        cmsPageSelect(pDisplay, cursorAbs / MAX_MENU_ITEMS(pDisplay));
     }
 
+    cmsPageDebug();
+
     return 0;
 }
 
@@ -505,30 +546,21 @@ STATIC_UNIT_TESTED long cmsMenuBack(displayPort_t *pDisplay)
 {
     // Let onExit function decide whether to allow exit or not.
 
-    if (currentMenu->onExit && currentMenu->onExit(pageTop + cursorRow) < 0)
+    if (currentCtx.menu->onExit && currentCtx.menu->onExit(pageTop + currentCtx.cursorRow) < 0) {
         return -1;
-
-    if (menuStackIdx) {
-        displayClearScreen(pDisplay);
-        menuStackIdx--;
-        currentMenu = menuStack[menuStackIdx];
-        cursorRow = menuStackHistory[menuStackIdx];
-
-        // cursorRow is absolute offset of a focused entry when stacked.
-        // Convert it back to page and relative offset.
-
-        pageTop = currentMenu->entries; // Temporary for cmsUpdateMaxRow()
-        cmsUpdateMaxRow(pDisplay);
-
-        if (cursorRow > maxRow) {
-            // Cursor was in the second page.
-            pageTopAlt = currentMenu->entries;
-            pageTop = pageTopAlt + maxRow + 1;
-            cursorRow -= (maxRow + 1);
-            cmsUpdateMaxRow(pDisplay); // Update maxRow for the second page
-        }
     }
 
+    if (!menuStackIdx) {
+        return 0;
+    }
+
+    currentCtx = menuStack[--menuStackIdx];
+
+    cmsMenuCountPage(pDisplay);
+    cmsPageSelect(pDisplay, currentCtx.page);
+
+    cmsPageDebug();
+
     return 0;
 }
 
@@ -540,12 +572,15 @@ STATIC_UNIT_TESTED void cmsMenuOpen(void)
         if (!pCurrentDisplay)
             return;
         cmsInMenu = true;
-        currentMenu = &menuMain;
+        currentCtx = (cmsCtx_t){ &menuMain, 0, 0 };
         DISABLE_ARMING_FLAG(OK_TO_ARM);
     } else {
         // Switch display
         displayPort_t *pNextDisplay = cmsDisplayPortSelectNext();
         if (pNextDisplay != pCurrentDisplay) {
+            // DisplayPort has been changed.
+            // Convert cursorRow to absolute value
+            currentCtx.cursorRow = cmsCursorAbsolute(pCurrentDisplay);
             displayRelease(pCurrentDisplay);
             pCurrentDisplay = pNextDisplay;
         } else {
@@ -553,7 +588,7 @@ STATIC_UNIT_TESTED void cmsMenuOpen(void)
         }
     }
     displayGrab(pCurrentDisplay); // grab the display for use by the CMS
-    cmsMenuChange(pCurrentDisplay, currentMenu);
+    cmsMenuChange(pCurrentDisplay, currentCtx.menu);
 }
 
 static void cmsTraverseGlobalExit(const CMS_Menu *pMenu)
@@ -570,39 +605,47 @@ static void cmsTraverseGlobalExit(const CMS_Menu *pMenu)
 }
 
 long cmsMenuExit(displayPort_t *pDisplay, const void *ptr)
-{
-    if (ptr) {
-        displayClearScreen(pDisplay);
+{   
+    int exitType = (int)ptr;
+    switch (exitType) {
+    case CMS_EXIT_SAVE:
+    case CMS_EXIT_SAVEREBOOT:
 
+        cmsTraverseGlobalExit(&menuMain);
+
+        if (currentCtx.menu->onExit)
+            currentCtx.menu->onExit((OSD_Entry *)NULL); // Forced exit
+
+        saveConfigAndNotify();
+        break;
+
+    case CMS_EXIT:
+        break;
+    }
+
+    cmsInMenu = false;
+
+    displayRelease(pDisplay);
+    currentCtx.menu = NULL;
+
+    if (exitType == CMS_EXIT_SAVEREBOOT) {
+        displayClearScreen(pDisplay);
         displayWrite(pDisplay, 5, 3, "REBOOTING...");
+
         displayResync(pDisplay); // Was max7456RefreshAll(); why at this timing?
 
         stopMotors();
         stopPwmAllMotors();
         delay(200);
 
-        cmsTraverseGlobalExit(&menuMain);
-
-        if (currentMenu->onExit)
-            currentMenu->onExit((OSD_Entry *)NULL); // Forced exit
-
-        saveConfigAndNotify();
-    }
-
-    cmsInMenu = false;
-
-    displayRelease(pDisplay);
-    currentMenu = NULL;
-
-    if (ptr)
         systemReset();
+    }
 
     ENABLE_ARMING_FLAG(OK_TO_ARM);
 
     return 0;
 }
 
-
 // Stick/key detection and key codes
 
 #define IS_HI(X)  (rcData[X] > 1750)
@@ -625,7 +668,7 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key)
     uint16_t res = BUTTON_TIME;
     OSD_Entry *p;
 
-    if (!currentMenu)
+    if (!currentCtx.menu)
         return res;
 
     if (key == KEY_MENU) {
@@ -639,42 +682,32 @@ STATIC_UNIT_TESTED uint16_t cmsHandleKey(displayPort_t *pDisplay, uint8_t key)
     }
 
     if (key == KEY_DOWN) {
-        if (cursorRow < maxRow) {
-            cursorRow++;
+        if (currentCtx.cursorRow < pageMaxRow) {
+            currentCtx.cursorRow++;
         } else {
-            if (pageTopAlt) { // we have another page
-                displayClearScreen(pDisplay);
-                p = pageTopAlt;
-                pageTopAlt = pageTop;
-                pageTop = (OSD_Entry *)p;
-                cmsUpdateMaxRow(pDisplay);
-            }
-            cursorRow = 0;    // Goto top in any case
+            cmsPageNext(pDisplay);
+            currentCtx.cursorRow = 0;    // Goto top in any case
         }
     }
 
     if (key == KEY_UP) {
-        cursorRow--;
+        currentCtx.cursorRow--;
 
-        if ((pageTop + cursorRow)->type == OME_Label && cursorRow > 0)
-            cursorRow--;
+        // Skip non-title labels
+        if ((pageTop + currentCtx.cursorRow)->type == OME_Label && currentCtx.cursorRow > 0)
+            currentCtx.cursorRow--;
 
-        if (cursorRow == -1 || (pageTop + cursorRow)->type == OME_Label) {
-            if (pageTopAlt) {
-                displayClearScreen(pDisplay);
-                p = pageTopAlt;
-                pageTopAlt = pageTop;
-                pageTop = (OSD_Entry *)p;
-                cmsUpdateMaxRow(pDisplay);
-            }
-            cursorRow = maxRow;    // Goto bottom in any case
+        if (currentCtx.cursorRow == -1 || (pageTop + currentCtx.cursorRow)->type == OME_Label) {
+            // Goto previous page
+            cmsPagePrev(pDisplay);
+            currentCtx.cursorRow = pageMaxRow;
         }
     }
 
     if (key == KEY_DOWN || key == KEY_UP)
         return res;
 
-    p = pageTop + cursorRow;
+    p = pageTop + currentCtx.cursorRow;
 
     switch (p->type) {
         case OME_Submenu:
@@ -878,19 +911,19 @@ void cmsUpdate(uint32_t currentTimeUs)
         if (IS_MID(THROTTLE) && IS_LO(YAW) && IS_HI(PITCH) && !ARMING_FLAG(ARMED)) {
             key = KEY_MENU;
         }
-        else if (IS_HI(PITCH)) {
+        else if (IS_MID(THROTTLE) && IS_MID(YAW) && IS_MID(ROLL) && IS_HI(PITCH)) {
             key = KEY_UP;
         }
-        else if (IS_LO(PITCH)) {
+        else if (IS_MID(THROTTLE) && IS_MID(YAW) && IS_MID(ROLL) && IS_LO(PITCH)) {
             key = KEY_DOWN;
         }
-        else if (IS_LO(ROLL)) {
+        else if (IS_MID(THROTTLE) && IS_MID(YAW) && IS_LO(ROLL) && IS_MID(PITCH)) {
             key = KEY_LEFT;
         }
-        else if (IS_HI(ROLL)) {
+        else if (IS_MID(THROTTLE) && IS_MID(YAW) && IS_HI(ROLL) && IS_MID(PITCH)) {
             key = KEY_RIGHT;
         }
-        else if (IS_HI(YAW) || IS_LO(YAW))
+        else if ((IS_HI(YAW) || IS_LO(YAW)) && IS_MID(THROTTLE) && IS_MID(ROLL) && IS_MID(PITCH))
         {
             key = KEY_ESC;
         }
@@ -969,8 +1002,6 @@ void cmsHandler(timeUs_t currentTimeUs)
     }
 }
 
-// Is initializing with menuMain better?
-// Can it be done with the current main()?
 void cmsInit(void)
 {
     cmsDeviceCount = 0;
diff --git a/src/main/cms/cms.h b/src/main/cms/cms.h
index 8bd35c47b5..6667c3d3cb 100644
--- a/src/main/cms/cms.h
+++ b/src/main/cms/cms.h
@@ -18,3 +18,9 @@ void cmsUpdate(uint32_t currentTimeUs);
 #define CMS_STARTUP_HELP_TEXT1 "MENU: THR MID"
 #define CMS_STARTUP_HELP_TEXT2     "+ YAW LEFT"
 #define CMS_STARTUP_HELP_TEXT3     "+ PITCH UP"
+
+// cmsMenuExit special ptr values
+#define CMS_EXIT             (0)
+#define CMS_EXIT_SAVE        (1)
+#define CMS_EXIT_SAVEREBOOT  (2)
+
diff --git a/src/main/cms/cms_menu_builtin.c b/src/main/cms/cms_menu_builtin.c
index 680800faef..abc3da37b0 100644
--- a/src/main/cms/cms_menu_builtin.c
+++ b/src/main/cms/cms_menu_builtin.c
@@ -138,8 +138,9 @@ static OSD_Entry menuMainEntries[] =
 #endif
     {"FC&FW INFO",  OME_Submenu,  cmsMenuChange, &menuInfo, 0},
     {"MISC",        OME_Submenu,  cmsMenuChange, &cmsx_menuMisc, 0},
-    {"SAVE&REBOOT", OME_OSD_Exit, cmsMenuExit,   (void*)1, 0},
-    {"EXIT",        OME_OSD_Exit, cmsMenuExit,   (void*)0, 0},
+    {"EXIT",        OME_OSD_Exit, cmsMenuExit,   (void *)CMS_EXIT, 0},
+    {"SAVE&EXIT",   OME_OSD_Exit, cmsMenuExit,   (void *)CMS_EXIT_SAVE, 0},
+    {"SAVE&REBOOT", OME_OSD_Exit, cmsMenuExit,   (void *)CMS_EXIT_SAVEREBOOT, 0},
 #ifdef CMS_MENU_DEBUG
     {"ERR SAMPLE",  OME_Submenu,  cmsMenuChange, &menuInfoEntries[0], 0},
 #endif
diff --git a/src/main/common/maths.c b/src/main/common/maths.c
index 97fc286798..acfbf0cc7a 100644
--- a/src/main/common/maths.c
+++ b/src/main/common/maths.c
@@ -99,6 +99,15 @@ float acos_approx(float x)
 }
 #endif
 
+int gcd(int num, int denom)
+{
+    if (denom == 0) {
+        return num;
+    }
+
+    return gcd(denom, num % denom);
+}
+
 float powerf(float base, int exp) {
     float result = base;
     for (int count = 1; count < exp; count++) result *= base;
diff --git a/src/main/common/maths.h b/src/main/common/maths.h
index a7839a4c4e..3cdf33705a 100644
--- a/src/main/common/maths.h
+++ b/src/main/common/maths.h
@@ -76,6 +76,7 @@ typedef union {
     fp_angles_def angles;
 } fp_angles_t;
 
+int gcd(int num, int denom);
 float powerf(float base, int exp);
 int32_t applyDeadband(int32_t value, int32_t deadband);
 
diff --git a/src/main/drivers/light_ws2811strip_hal.c b/src/main/drivers/light_ws2811strip_hal.c
index 1a501abbed..10930142bc 100644
--- a/src/main/drivers/light_ws2811strip_hal.c
+++ b/src/main/drivers/light_ws2811strip_hal.c
@@ -129,6 +129,7 @@ void ws2811LedStripHardwareInit(ioTag_t ioTag)
     TIM_OCInitStructure.OCMode = TIM_OCMODE_PWM1;
     TIM_OCInitStructure.Pulse = 0;
     TIM_OCInitStructure.OCPolarity = TIM_OCPOLARITY_HIGH;
+    TIM_OCInitStructure.OCNPolarity = TIM_OCPOLARITY_HIGH;
     TIM_OCInitStructure.OCIdleState = TIM_OCIDLESTATE_RESET;
     TIM_OCInitStructure.OCNIdleState  = TIM_OCNIDLESTATE_RESET;
     TIM_OCInitStructure.OCFastMode = TIM_OCFAST_DISABLE;
diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c
index 53ae693e95..035688529d 100644
--- a/src/main/drivers/pwm_output.c
+++ b/src/main/drivers/pwm_output.c
@@ -59,11 +59,15 @@ static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8
     TIM_OCInitStructure.OCMode = TIM_OCMODE_PWM1;
 
     if (output & TIMER_OUTPUT_N_CHANNEL) {
+        TIM_OCInitStructure.OCIdleState = TIM_OCIDLESTATE_RESET;
+        TIM_OCInitStructure.OCPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCPOLARITY_HIGH: TIM_OCPOLARITY_LOW;
         TIM_OCInitStructure.OCNIdleState = TIM_OCNIDLESTATE_RESET;
         TIM_OCInitStructure.OCNPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPOLARITY_HIGH : TIM_OCNPOLARITY_LOW;
     } else {
         TIM_OCInitStructure.OCIdleState = TIM_OCIDLESTATE_SET;
-        TIM_OCInitStructure.OCPolarity =  (output & TIMER_OUTPUT_INVERTED) ? TIM_OCPOLARITY_LOW : TIM_OCPOLARITY_HIGH;
+        TIM_OCInitStructure.OCPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCPOLARITY_LOW : TIM_OCPOLARITY_HIGH;
+        TIM_OCInitStructure.OCNIdleState = TIM_OCNIDLESTATE_SET;
+        TIM_OCInitStructure.OCNPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPOLARITY_LOW : TIM_OCNPOLARITY_HIGH;
     }
 
     TIM_OCInitStructure.Pulse = value;
diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index 7f967bf710..1f06610296 100644
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -792,7 +792,7 @@ static bool mspOsdSlaveProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostPro
 
     switch (cmdMSP) {
     case MSP_STATUS_EX:
-        sbufWriteU16(dst, 0); // task delta
+        sbufWriteU16(dst, getTaskDeltaTime(TASK_SERIAL));
 #ifdef USE_I2C
         sbufWriteU16(dst, i2cGetErrorCounter());
 #else
@@ -807,7 +807,7 @@ static bool mspOsdSlaveProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostPro
         break;
 
     case MSP_STATUS:
-        sbufWriteU16(dst, 0); // task delta
+        sbufWriteU16(dst, getTaskDeltaTime(TASK_SERIAL));
 #ifdef USE_I2C
         sbufWriteU16(dst, i2cGetErrorCounter());
 #else
@@ -1375,7 +1375,6 @@ static void mspFcDataFlashReadCommand(sbuf_t *dst, sbuf_t *src)
 static mspResult_e mspOsdSlaveProcessInCommand(uint8_t cmdMSP, sbuf_t *src) {
     UNUSED(cmdMSP);
     UNUSED(src);
-    // Nothing OSD SLAVE specific yet.
     return MSP_RESULT_ERROR;
 }
 #endif
@@ -2145,8 +2144,31 @@ void mspFcProcessReply(mspPacket_t *reply)
     UNUSED(src); // potentially unused depending on compile options.
 
     switch (reply->cmd) {
-        case MSP_DISPLAYPORT: {
+#ifndef OSD_SLAVE
+    case MSP_ANALOG:
+        {
+            uint8_t batteryVoltage = sbufReadU8(src);
+            uint16_t mAhDrawn = sbufReadU16(src);
+            uint16_t rssi = sbufReadU16(src);
+            uint16_t amperage = sbufReadU16(src);
+
+            UNUSED(rssi);
+            UNUSED(batteryVoltage);
+            UNUSED(amperage);
+            UNUSED(mAhDrawn);
+
+#ifdef USE_MSP_CURRENT_METER
+            currentMeterMSPSet(amperage, mAhDrawn);
+#endif
+            break;
+        }
+#endif
+
 #ifdef USE_OSD_SLAVE
+    case MSP_DISPLAYPORT:
+        {
+            osdSlaveIsLocked = true; // lock it as soon as a MSP_DISPLAYPORT message is received to prevent accidental CLI/DFU mode.
+
             int subCmd = sbufReadU8(src);
 
             switch (subCmd) {
@@ -2184,9 +2206,9 @@ void mspFcProcessReply(mspPacket_t *reply)
                     osdSlaveDrawScreen();
                 }
             }
-#endif
             break;
         }
+#endif
     }
 }
 
diff --git a/src/main/fc/fc_msp.h b/src/main/fc/fc_msp.h
index c5ccb1f1c4..577969c349 100644
--- a/src/main/fc/fc_msp.h
+++ b/src/main/fc/fc_msp.h
@@ -33,3 +33,5 @@ void mspFcInit(void);
 void mspOsdSlaveInit(void);
 mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn);
 void mspFcProcessReply(mspPacket_t *reply);
+
+void mspSerialProcessStreamSchedule(void);
diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c
index a01e833546..a34c582f8f 100644
--- a/src/main/fc/fc_tasks.c
+++ b/src/main/fc/fc_tasks.c
@@ -23,6 +23,8 @@
 
 #include "cms/cms.h"
 
+#include "build/debug.h"
+
 #include "common/axis.h"
 #include "common/color.h"
 #include "common/utils.h"
@@ -81,6 +83,8 @@
 
 #include "telemetry/telemetry.h"
 
+#include "io/osd_slave.h"
+
 #ifdef USE_BST
 void taskBstMasterProcess(timeUs_t currentTimeUs);
 #endif
@@ -115,7 +119,12 @@ static void taskHandleSerial(timeUs_t currentTimeUs)
         return;
     }
 #endif
-    mspSerialProcess(ARMING_FLAG(ARMED) ? MSP_SKIP_NON_MSP_DATA : MSP_EVALUATE_NON_MSP_DATA, mspFcProcessCommand, mspFcProcessReply);
+#ifndef OSD_SLAVE
+    bool evaluateMspData = ARMING_FLAG(ARMED) ? MSP_SKIP_NON_MSP_DATA : MSP_EVALUATE_NON_MSP_DATA;
+#else
+    bool evaluateMspData = osdSlaveIsLocked ?  MSP_SKIP_NON_MSP_DATA : MSP_EVALUATE_NON_MSP_DATA;;
+#endif
+    mspSerialProcess(evaluateMspData, mspFcProcessCommand, mspFcProcessReply);
 }
 
 void taskBatteryAlerts(timeUs_t currentTimeUs)
diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c
index 8d27688460..1527ed4fc3 100644
--- a/src/main/flight/failsafe.c
+++ b/src/main/flight/failsafe.c
@@ -88,7 +88,7 @@ void failsafeInit(void)
     return;
 }
 
-failsafePhase_e failsafePhase()
+failsafePhase_e failsafePhase(void)
 {
     return failsafeState.phase;
 }
diff --git a/src/main/io/displayport_msp.c b/src/main/io/displayport_msp.c
index d977ca75ca..dc8088ef4c 100644
--- a/src/main/io/displayport_msp.c
+++ b/src/main/io/displayport_msp.c
@@ -58,7 +58,7 @@ static int output(displayPort_t *displayPort, uint8_t cmd, uint8_t *buf, int len
         return 0;
     }
 #endif
-    return mspSerialPush(cmd, buf, len);
+    return mspSerialPush(cmd, buf, len, MSP_DIRECTION_REPLY);
 }
 
 static int heartbeat(displayPort_t *displayPort)
diff --git a/src/main/io/osd_slave.c b/src/main/io/osd_slave.c
index ff148e83e8..b8a6ff6ca0 100644
--- a/src/main/io/osd_slave.c
+++ b/src/main/io/osd_slave.c
@@ -40,6 +40,9 @@
 
 //#define OSD_SLAVE_DEBUG
 
+// when locked the system ignores requests to enter cli or bootloader mode via serial connection.
+bool osdSlaveIsLocked = false;
+
 static displayPort_t *osdDisplayPort;
 
 static void osdDrawLogo(int x, int y)
diff --git a/src/main/io/osd_slave.h b/src/main/io/osd_slave.h
index c7974f84dc..7d9e945afc 100644
--- a/src/main/io/osd_slave.h
+++ b/src/main/io/osd_slave.h
@@ -22,6 +22,8 @@
 
 struct displayPort_s;
 
+extern bool osdSlaveIsLocked;
+
 // init
 void osdSlaveInit(struct displayPort_s *osdDisplayPort);
 
diff --git a/src/main/msp/msp.h b/src/main/msp/msp.h
index 5e6a14ac58..9a9ddb0f4b 100644
--- a/src/main/msp/msp.h
+++ b/src/main/msp/msp.h
@@ -26,10 +26,16 @@ typedef enum {
     MSP_RESULT_NO_REPLY = 0
 } mspResult_e;
 
+typedef enum {
+    MSP_DIRECTION_REPLY = 0,
+    MSP_DIRECTION_REQUEST = 1
+} mspDirection_e;
+
 typedef struct mspPacket_s {
     sbuf_t buf;
     int16_t cmd;
     int16_t result;
+    uint8_t direction;
 } mspPacket_t;
 
 struct serialPort_s;
diff --git a/src/main/msp/msp_protocol.h b/src/main/msp/msp_protocol.h
index f8c8eadf18..3574f99ea8 100644
--- a/src/main/msp/msp_protocol.h
+++ b/src/main/msp/msp_protocol.h
@@ -59,7 +59,7 @@
 #define MSP_PROTOCOL_VERSION                0
 
 #define API_VERSION_MAJOR                   1  // increment when major changes are made
-#define API_VERSION_MINOR                   35 // increment after a release, to set the version for all changes to go into the following release (if no changes to MSP are made between the releases, this can be reverted before the release)
+#define API_VERSION_MINOR                   36 // increment after a release, to set the version for all changes to go into the following release (if no changes to MSP are made between the releases, this can be reverted before the release)
 
 #define API_VERSION_LENGTH                  2
 
@@ -101,7 +101,6 @@
 #define MSP_NAME                        10   //out message          Returns user set board name - betaflight
 #define MSP_SET_NAME                    11   //in message           Sets board name - betaflight
 
-
 //
 // MSP commands for Cleanflight original features
 //
@@ -312,3 +311,4 @@
 #define MSP_SERVO_MIX_RULES      241    //out message         Returns servo mixer configuration
 #define MSP_SET_SERVO_MIX_RULE   242    //in message          Sets servo mixer configuration
 #define MSP_SET_4WAY_IF          245    //in message          Sets 4way interface
+
diff --git a/src/main/msp/msp_serial.c b/src/main/msp/msp_serial.c
index 7a233ca876..08cb7d7e4a 100644
--- a/src/main/msp/msp_serial.c
+++ b/src/main/msp/msp_serial.c
@@ -27,12 +27,13 @@
 
 #include "io/serial.h"
 
+#include "drivers/system.h"
+
 #include "msp/msp.h"
 #include "msp/msp_serial.h"
 
 static mspPort_t mspPorts[MAX_MSP_PORT_COUNT];
 
-
 static void resetMspPort(mspPort_t *mspPortToReset, serialPort_t *serialPort)
 {
     memset(mspPortToReset, 0, sizeof(mspPort_t));
@@ -138,7 +139,13 @@ static int mspSerialEncode(mspPort_t *msp, mspPacket_t *packet)
     serialBeginWrite(msp->port);
     const int len = sbufBytesRemaining(&packet->buf);
     const int mspLen = len < JUMBO_FRAME_SIZE_LIMIT ? len : JUMBO_FRAME_SIZE_LIMIT;
-    uint8_t hdr[8] = {'$', 'M', packet->result == MSP_RESULT_ERROR ? '!' : '>', mspLen, packet->cmd};
+    uint8_t hdr[8] = {
+        '$',
+        'M',
+        packet->result == MSP_RESULT_ERROR ? '!' : packet->direction == MSP_DIRECTION_REPLY ? '>' : '<',
+        mspLen,
+        packet->cmd
+    };
     int hdrLen = 5;
 #define CHECKSUM_STARTPOS 3  // checksum starts from mspLen field
     if (len >= JUMBO_FRAME_SIZE_LIMIT) {
@@ -165,6 +172,7 @@ static mspPostProcessFnPtr mspSerialProcessReceivedCommand(mspPort_t *msp, mspPr
         .buf = { .ptr = outBuf, .end = ARRAYEND(outBuf), },
         .cmd = -1,
         .result = 0,
+        .direction = MSP_DIRECTION_REPLY,
     };
     uint8_t *outBufHead = reply.buf.ptr;
 
@@ -172,6 +180,7 @@ static mspPostProcessFnPtr mspSerialProcessReceivedCommand(mspPort_t *msp, mspPr
         .buf = { .ptr = msp->inBuf, .end = msp->inBuf + msp->dataSize, },
         .cmd = msp->cmdMSP,
         .result = 0,
+        .direction = MSP_DIRECTION_REQUEST,
     };
 
     mspPostProcessFnPtr mspPostProcessFn = NULL;
@@ -182,7 +191,6 @@ static mspPostProcessFnPtr mspSerialProcessReceivedCommand(mspPort_t *msp, mspPr
         mspSerialEncode(msp, &reply);
     }
 
-    msp->c_state = MSP_IDLE;
     return mspPostProcessFn;
 }
 
@@ -215,7 +223,9 @@ void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData, mspProcessComm
         if (!mspPort->port) {
             continue;
         }
+
         mspPostProcessFnPtr mspPostProcessFn = NULL;
+
         while (serialRxBytesWaiting(mspPort->port)) {
 
             const uint8_t c = serialRead(mspPort->port);
@@ -231,9 +241,12 @@ void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData, mspProcessComm
                 } else if (mspPort->packetType == MSP_PACKET_REPLY) {
                     mspSerialProcessReceivedReply(mspPort, mspProcessReplyFn);
                 }
+
+                mspPort->c_state = MSP_IDLE;
                 break; // process one command at a time so as not to block.
             }
         }
+
         if (mspPostProcessFn) {
             waitForSerialPortToFinishTransmitting(mspPort->port);
             mspPostProcessFn(mspPort->port);
@@ -262,7 +275,7 @@ void mspSerialInit(void)
     mspSerialAllocatePorts();
 }
 
-int mspSerialPush(uint8_t cmd, uint8_t *data, int datalen)
+int mspSerialPush(uint8_t cmd, uint8_t *data, int datalen, mspDirection_e direction)
 {
     int ret = 0;
 
@@ -281,6 +294,7 @@ int mspSerialPush(uint8_t cmd, uint8_t *data, int datalen)
             .buf = { .ptr = data, .end = data + datalen, },
             .cmd = cmd,
             .result = 0,
+            .direction = direction,
         };
 
         ret = mspSerialEncode(mspPort, &push);
@@ -288,6 +302,7 @@ int mspSerialPush(uint8_t cmd, uint8_t *data, int datalen)
     return ret; // return the number of bytes written
 }
 
+
 uint32_t mspSerialTxBytesFree()
 {
     uint32_t ret = UINT32_MAX;
diff --git a/src/main/msp/msp_serial.h b/src/main/msp/msp_serial.h
index e6136420cc..0032937363 100644
--- a/src/main/msp/msp_serial.h
+++ b/src/main/msp/msp_serial.h
@@ -67,11 +67,10 @@ typedef struct mspPort_s {
     uint8_t inBuf[MSP_PORT_INBUF_SIZE];
 } mspPort_t;
 
-
 void mspSerialInit(void);
 bool mspSerialWaiting(void);
 void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData, mspProcessCommandFnPtr mspProcessCommandFn, mspProcessReplyFnPtr mspProcessReplyFn);
 void mspSerialAllocatePorts(void);
 void mspSerialReleasePortIfAllocated(struct serialPort_s *serialPort);
-int mspSerialPush(uint8_t cmd, uint8_t *data, int datalen);
+int mspSerialPush(uint8_t cmd, uint8_t *data, int datalen, mspDirection_e direction);
 uint32_t mspSerialTxBytesFree(void);
diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c
index ed289aaecb..a0e7af7956 100644
--- a/src/main/sensors/battery.c
+++ b/src/main/sensors/battery.c
@@ -76,9 +76,13 @@ static batteryState_e consumptionState;
 #ifdef USE_VIRTUAL_CURRENT_METER
 #define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_VIRTUAL
 #else
+#ifdef USE_MSP_CURRENT_METER
+#define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_MSP
+#else
 #define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_NONE
 #endif
 #endif
+#endif
 
 #ifdef BOARD_HAS_VOLTAGE_DIVIDER
 #define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ADC
@@ -302,6 +306,11 @@ void batteryInit(void)
         case CURRENT_METER_ESC:
 #ifdef ESC_SENSOR
             currentMeterESCInit();
+#endif
+            break;
+        case CURRENT_METER_MSP:
+#ifdef USE_MSP_CURRENT_METER
+            currentMeterMSPInit();
 #endif
             break;
 
@@ -362,6 +371,12 @@ void batteryUpdateCurrentMeter(timeUs_t currentTimeUs)
                 currentMeterESCRefresh(lastUpdateAt);
                 currentMeterESCReadCombined(&currentMeter);
             }
+#endif
+            break;
+        case CURRENT_METER_MSP:
+#ifdef USE_MSP_CURRENT_METER
+            currentMeterMSPRefresh(currentTimeUs);
+            currentMeterMSPRead(&currentMeter);
 #endif
             break;
 
diff --git a/src/main/sensors/current.c b/src/main/sensors/current.c
index 67b170fd32..da7654357c 100644
--- a/src/main/sensors/current.c
+++ b/src/main/sensors/current.c
@@ -56,12 +56,15 @@ const uint8_t currentMeterIds[] = {
     CURRENT_METER_ID_ESC_MOTOR_11,
     CURRENT_METER_ID_ESC_MOTOR_12,
 #endif
+#ifdef USE_MSP_CURRENT_METER
+    CURRENT_METER_ID_MSP_1,
+#endif
 };
 
 const uint8_t supportedCurrentMeterCount = ARRAYLEN(currentMeterIds);
 
 //
-// ADC/Virtual/ESC shared
+// ADC/Virtual/ESC/MSP shared
 //
 
 void currentMeterReset(currentMeter_t *meter)
@@ -225,6 +228,45 @@ void currentMeterESCReadMotor(uint8_t motorNumber, currentMeter_t *meter)
 }
 #endif
 
+
+#ifdef USE_MSP_CURRENT_METER
+#include "common/streambuf.h"
+#include "msp/msp_protocol.h"
+#include "msp/msp_serial.h"
+
+currentMeterMSPState_t currentMeterMSPState;
+
+void currentMeterMSPSet(uint16_t amperage, uint16_t mAhDrawn)
+{
+    // We expect the FC's MSP_ANALOG response handler to call this function
+    currentMeterMSPState.amperage = amperage;
+    currentMeterMSPState.mAhDrawn = mAhDrawn;
+}
+
+void currentMeterMSPInit(void)
+{
+    memset(&currentMeterMSPState, 0, sizeof(currentMeterMSPState_t));
+}
+
+void currentMeterMSPRefresh(timeUs_t currentTimeUs)
+{
+    // periodically request MSP_ANALOG
+    static timeUs_t streamRequestAt = 0;
+    if (cmp32(currentTimeUs, streamRequestAt) > 0) {
+        streamRequestAt = currentTimeUs + ((1000 * 1000) / 10); // 10hz
+
+        mspSerialPush(MSP_ANALOG, NULL, 0, MSP_DIRECTION_REQUEST);
+    }
+}
+
+void currentMeterMSPRead(currentMeter_t *meter)
+{
+    meter->amperageLatest = currentMeterMSPState.amperage;
+    meter->amperage = currentMeterMSPState.amperage;
+    meter->mAhDrawn = currentMeterMSPState.mAhDrawn;
+}
+#endif
+
 //
 // API for current meters using IDs
 //
@@ -241,6 +283,11 @@ void currentMeterRead(currentMeterId_e id, currentMeter_t *meter)
         currentMeterVirtualRead(meter);
     }
 #endif
+#ifdef USE_MSP_CURRENT_METER
+    else if (id == CURRENT_METER_ID_MSP_1) {
+        currentMeterMSPRead(meter);
+    }
+#endif
 #ifdef USE_ESC_SENSOR
     else if (id == CURRENT_METER_ID_ESC_COMBINED_1) {
         currentMeterESCReadCombined(meter);
diff --git a/src/main/sensors/current.h b/src/main/sensors/current.h
index 7da02fa6b5..48c0db2bab 100644
--- a/src/main/sensors/current.h
+++ b/src/main/sensors/current.h
@@ -17,6 +17,7 @@
 
 #pragma once
 
+#include "common/time.h"
 #include "current_ids.h"
 
 typedef enum {
@@ -24,6 +25,7 @@ typedef enum {
     CURRENT_METER_ADC,
     CURRENT_METER_VIRTUAL,
     CURRENT_METER_ESC,
+    CURRENT_METER_MSP,
     CURRENT_METER_MAX = CURRENT_METER_ESC
 } currentMeterSource_e;
 
@@ -48,6 +50,7 @@ typedef enum {
     CURRENT_SENSOR_VIRTUAL = 0,
     CURRENT_SENSOR_ADC,
     CURRENT_SENSOR_ESC,
+    CURRENT_SENSOR_MSP
 } currentSensor_e;
 
 
@@ -93,6 +96,21 @@ typedef struct currentMeterESCState_s {
     int32_t amperage;           // current read by current sensor in centiampere (1/100th A)
 } currentMeterESCState_t;
 
+
+//
+// MSP
+//
+
+typedef struct currentMeterMSPState_s {
+    int32_t mAhDrawn;           // milliampere hours drawn from the battery since start
+    int32_t amperage;           // current read by current sensor in centiampere (1/100th A)
+} currentMeterMSPState_t;
+
+
+//
+// Current Meter API
+//
+
 void currentMeterReset(currentMeter_t *meter);
 
 void currentMeterADCInit(void);
@@ -108,6 +126,11 @@ void currentMeterESCRefresh(int32_t lastUpdateAt);
 void currentMeterESCReadCombined(currentMeter_t *meter);
 void currentMeterESCReadMotor(uint8_t motorNumber, currentMeter_t *meter);
 
+void currentMeterMSPInit(void);
+void currentMeterMSPRefresh(timeUs_t currentTimeUs);
+void currentMeterMSPRead(currentMeter_t *meter);
+void currentMeterMSPSet(uint16_t amperage, uint16_t mAhDrawn);
+
 //
 // API for reading current meters by id.
 //
diff --git a/src/main/sensors/current_ids.h b/src/main/sensors/current_ids.h
index 2f366c6f1f..f7bb3029e3 100644
--- a/src/main/sensors/current_ids.h
+++ b/src/main/sensors/current_ids.h
@@ -66,4 +66,7 @@ typedef enum {
     CURRENT_METER_ID_VIRTUAL_1 = 80,       // 80-89 for virtual meters
     CURRENT_METER_ID_VIRTUAL_2,
 
+    CURRENT_METER_ID_MSP_1 = 90,       // 90-99 for MSP meters
+    CURRENT_METER_ID_MSP_2,
+
 } currentMeterId_e;
diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index 9dc785d9a7..88e728c8c9 100644
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@@ -74,7 +74,13 @@ gyro_t gyro;
 STATIC_UNIT_TESTED gyroDev_t gyroDev0;
 static int16_t gyroTemperature0;
 
-static uint16_t calibratingG = 0;
+typedef struct gyroCalibration_s {
+    int32_t g[XYZ_AXIS_COUNT];
+    stdev_t var[XYZ_AXIS_COUNT];
+    uint16_t calibratingG;
+} gyroCalibration_t;
+
+STATIC_UNIT_TESTED gyroCalibration_t gyroCalibration;
 
 static filterApplyFnPtr softLpfFilterApplyFn;
 static void *softLpfFilter[3];
@@ -412,12 +418,12 @@ void gyroInitFilters(void)
 
 bool isGyroCalibrationComplete(void)
 {
-    return calibratingG == 0;
+    return gyroCalibration.calibratingG == 0;
 }
 
-static bool isOnFinalGyroCalibrationCycle(void)
+static bool isOnFinalGyroCalibrationCycle(const gyroCalibration_t *gyroCalibration)
 {
-    return calibratingG == 1;
+    return gyroCalibration->calibratingG == 1;
 }
 
 static uint16_t gyroCalculateCalibratingCycles(void)
@@ -425,56 +431,50 @@ static uint16_t gyroCalculateCalibratingCycles(void)
     return (CALIBRATING_GYRO_CYCLES / gyro.targetLooptime) * CALIBRATING_GYRO_CYCLES;
 }
 
-static bool isOnFirstGyroCalibrationCycle(void)
+static bool isOnFirstGyroCalibrationCycle(const gyroCalibration_t *gyroCalibration)
 {
-    return calibratingG == gyroCalculateCalibratingCycles();
+    return gyroCalibration->calibratingG == gyroCalculateCalibratingCycles();
 }
 
 void gyroSetCalibrationCycles(void)
 {
-    calibratingG = gyroCalculateCalibratingCycles();
+    gyroCalibration.calibratingG = gyroCalculateCalibratingCycles();
 }
 
-STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *gyroDev, uint8_t gyroMovementCalibrationThreshold)
+STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *gyroDev, gyroCalibration_t *gyroCalibration, uint8_t gyroMovementCalibrationThreshold)
 {
-    static int32_t g[3];
-    static stdev_t var[3];
-
     for (int axis = 0; axis < 3; axis++) {
-
         // Reset g[axis] at start of calibration
-        if (isOnFirstGyroCalibrationCycle()) {
-            g[axis] = 0;
-            devClear(&var[axis]);
+        if (isOnFirstGyroCalibrationCycle(gyroCalibration)) {
+            gyroCalibration->g[axis] = 0;
+            devClear(&gyroCalibration->var[axis]);
+            // gyroZero is set to zero until calibration complete
+            gyroDev->gyroZero[axis] = 0;
         }
 
         // Sum up CALIBRATING_GYRO_CYCLES readings
-        g[axis] += gyroDev->gyroADC[axis];
-        devPush(&var[axis], gyroDev->gyroADC[axis]);
+        gyroCalibration->g[axis] += gyroDev->gyroADCRaw[axis];
+        devPush(&gyroCalibration->var[axis], gyroDev->gyroADCRaw[axis]);
 
-        // Reset global variables to prevent other code from using un-calibrated data
-        gyroDev->gyroADC[axis] = 0;
-        gyroDev->gyroZero[axis] = 0;
+        if (isOnFinalGyroCalibrationCycle(gyroCalibration)) {
+            const float stddev = devStandardDeviation(&gyroCalibration->var[axis]);
 
-        if (isOnFinalGyroCalibrationCycle()) {
-            const float dev = devStandardDeviation(&var[axis]);
-
-            DEBUG_SET(DEBUG_GYRO, DEBUG_GYRO_CALIBRATION, lrintf(dev));
+            DEBUG_SET(DEBUG_GYRO, DEBUG_GYRO_CALIBRATION, lrintf(stddev));
 
             // check deviation and startover in case the model was moved
-            if (gyroMovementCalibrationThreshold && dev > gyroMovementCalibrationThreshold) {
+            if (gyroMovementCalibrationThreshold && stddev > gyroMovementCalibrationThreshold) {
                 gyroSetCalibrationCycles();
                 return;
             }
-            gyroDev->gyroZero[axis] = (g[axis] + (gyroCalculateCalibratingCycles() / 2)) / gyroCalculateCalibratingCycles();
+            gyroDev->gyroZero[axis] = (gyroCalibration->g[axis] + (gyroCalculateCalibratingCycles() / 2)) / gyroCalculateCalibratingCycles();
         }
     }
 
-    if (isOnFinalGyroCalibrationCycle()) {
+    if (isOnFinalGyroCalibrationCycle(gyroCalibration)) {
         schedulerResetTaskStatistics(TASK_SELF); // so calibration cycles do not pollute tasks statistics
         beeper(BEEPER_GYRO_CALIBRATED);
     }
-    calibratingG--;
+    gyroCalibration->calibratingG--;
 
 }
 
@@ -489,14 +489,13 @@ static bool gyroUpdateISR(gyroDev_t* gyroDev)
 #endif
     gyroDev->dataReady = false;
     // move gyro data into 32-bit variables to avoid overflows in calculations
-    gyroDev->gyroADC[X] = gyroDev->gyroADCRaw[X];
-    gyroDev->gyroADC[Y] = gyroDev->gyroADCRaw[Y];
-    gyroDev->gyroADC[Z] = gyroDev->gyroADCRaw[Z];
+    gyroDev->gyroADC[X] = (int32_t)gyroDev->gyroADCRaw[X] - (int32_t)gyroDev->gyroZero[X];
+    gyroDev->gyroADC[Y] = (int32_t)gyroDev->gyroADCRaw[Y] - (int32_t)gyroDev->gyroZero[Y];
+    gyroDev->gyroADC[Z] = (int32_t)gyroDev->gyroADCRaw[Z] - (int32_t)gyroDev->gyroZero[Z];
 
     alignSensors(gyroDev->gyroADC, gyroDev->gyroAlign);
 
     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-        gyroDev->gyroADC[axis] -= gyroDev->gyroZero[axis];
         // scale gyro output to degrees per second
         float gyroADCf = (float)gyroDev->gyroADC[axis] * gyroDev->scale;
         gyroADCf = softLpfFilterApplyFn(softLpfFilter[axis], gyroADCf);
@@ -522,15 +521,8 @@ void gyroUpdate(void)
         return;
     }
     gyroDev0.dataReady = false;
-    // move gyro data into 32-bit variables to avoid overflows in calculations
-    gyroDev0.gyroADC[X] = gyroDev0.gyroADCRaw[X];
-    gyroDev0.gyroADC[Y] = gyroDev0.gyroADCRaw[Y];
-    gyroDev0.gyroADC[Z] = gyroDev0.gyroADCRaw[Z];
 
-    alignSensors(gyroDev0.gyroADC, gyroDev0.gyroAlign);
-
-    const bool calibrationComplete = isGyroCalibrationComplete();
-    if (calibrationComplete) {
+    if (isGyroCalibrationComplete()) {
 #if defined(GYRO_USES_SPI) && defined(USE_MPU_DATA_READY_SIGNAL)
         // SPI-based gyro so can read and update in ISR
         if (gyroConfig()->gyro_isr_update) {
@@ -541,12 +533,23 @@ void gyroUpdate(void)
 #ifdef DEBUG_MPU_DATA_READY_INTERRUPT
         debug[3] = (uint16_t)(micros() & 0xffff);
 #endif
+        // move gyro data into 32-bit variables to avoid overflows in calculations
+        gyroDev0.gyroADC[X] = (int32_t)gyroDev0.gyroADCRaw[X] - (int32_t)gyroDev0.gyroZero[X];
+        gyroDev0.gyroADC[Y] = (int32_t)gyroDev0.gyroADCRaw[Y] - (int32_t)gyroDev0.gyroZero[Y];
+        gyroDev0.gyroADC[Z] = (int32_t)gyroDev0.gyroADCRaw[Z] - (int32_t)gyroDev0.gyroZero[Z];
+
+        alignSensors(gyroDev0.gyroADC, gyroDev0.gyroAlign);
     } else {
-        performGyroCalibration(&gyroDev0, gyroConfig()->gyroMovementCalibrationThreshold);
+        performGyroCalibration(&gyroDev0, &gyroCalibration, gyroConfig()->gyroMovementCalibrationThreshold);
+        // 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;
     }
 
     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-        gyroDev0.gyroADC[axis] -= gyroDev0.gyroZero[axis];
         // scale gyro output to degrees per second
         float gyroADCf = (float)gyroDev0.gyroADC[axis] * gyroDev0.scale;
 
@@ -561,11 +564,6 @@ void gyroUpdate(void)
         gyro.gyroADCf[axis] = gyroADCf;
     }
 
-    if (!calibrationComplete) {
-        gyroDev0.gyroADC[X] = lrintf(gyro.gyroADCf[X] / gyroDev0.scale);
-        gyroDev0.gyroADC[Y] = lrintf(gyro.gyroADCf[Y] / gyroDev0.scale);
-        gyroDev0.gyroADC[Z] = lrintf(gyro.gyroADCf[Z] / gyroDev0.scale);
-    }
 #ifdef USE_GYRO_DATA_ANALYSE
     gyroDataAnalyse(&gyroDev0, &gyro);
 #endif
diff --git a/src/main/target/ANYFCF7/target.h b/src/main/target/ANYFCF7/target.h
index bbf000f326..72cfb6a43a 100644
--- a/src/main/target/ANYFCF7/target.h
+++ b/src/main/target/ANYFCF7/target.h
@@ -21,8 +21,6 @@
 
 #define USBD_PRODUCT_STRING "AnyFCF7"
 
-#define USE_ESC_SENSOR
-
 #define LED0   PB7
 #define LED1   PB6
 
diff --git a/src/main/target/ANYFCM7/target.h b/src/main/target/ANYFCM7/target.h
index 7845ab59de..9f9e278d94 100644
--- a/src/main/target/ANYFCM7/target.h
+++ b/src/main/target/ANYFCM7/target.h
@@ -86,7 +86,7 @@
 #define USE_SOFTSERIAL1
 #define USE_SOFTSERIAL2
 
-#define SERIAL_PORT_COUNT 5 //VCP, USART1, USART2, USART3, UART4, UART5, USART6, USART7, USART8, SOFTSERIAL x 2
+#define SERIAL_PORT_COUNT 9 //VCP, USART1, USART2, USART3, UART4, UART5, USART6, SOFTSERIAL x 2
 
 #define USE_SPI
 #define USE_SPI_DEVICE_1
diff --git a/src/main/target/BLUEJAYF4/target.h b/src/main/target/BLUEJAYF4/target.h
index a807a6f538..027eb69797 100644
--- a/src/main/target/BLUEJAYF4/target.h
+++ b/src/main/target/BLUEJAYF4/target.h
@@ -144,8 +144,6 @@
 #define VBAT_ADC_PIN            PC3
 #define CURRENT_METER_ADC_PIN   PC2
 
-#define USE_ESC_SENSOR
-
 #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT
 
 #define DEFAULT_RX_FEATURE      FEATURE_RX_SERIAL
diff --git a/src/main/target/FURYF4/target.h b/src/main/target/FURYF4/target.h
index f0e39218fa..fe691c1a71 100644
--- a/src/main/target/FURYF4/target.h
+++ b/src/main/target/FURYF4/target.h
@@ -183,8 +183,6 @@
 
 #define USE_SERIAL_4WAY_BLHELI_INTERFACE
 
-#define USE_ESC_SENSOR
-
 #define TARGET_IO_PORTA         0xffff
 #define TARGET_IO_PORTB         0xffff
 #define TARGET_IO_PORTC         0xffff
diff --git a/src/main/target/FURYF7/target.h b/src/main/target/FURYF7/target.h
index 458aad7d42..26577662d8 100644
--- a/src/main/target/FURYF7/target.h
+++ b/src/main/target/FURYF7/target.h
@@ -21,8 +21,6 @@
 
 #define USBD_PRODUCT_STRING "FuryF7"
 
-#define USE_ESC_SENSOR
-
 #define LED0                    PB5
 #define LED1                    PB4
 
diff --git a/src/main/target/NERO/target.h b/src/main/target/NERO/target.h
index 67234d1913..a5ae8e5336 100644
--- a/src/main/target/NERO/target.h
+++ b/src/main/target/NERO/target.h
@@ -120,7 +120,6 @@
 #define USE_ADC
 #define VBAT_ADC_PIN            PC3
 
-//#define USE_ESC_SENSOR
 #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT
 
 #define DEFAULT_RX_FEATURE      FEATURE_RX_SERIAL
diff --git a/src/main/target/NUCLEOF722/target.h b/src/main/target/NUCLEOF722/target.h
index bfbaa605a8..82e09fe950 100644
--- a/src/main/target/NUCLEOF722/target.h
+++ b/src/main/target/NUCLEOF722/target.h
@@ -93,7 +93,7 @@
 #define USE_SOFTSERIAL1
 #define USE_SOFTSERIAL2
 
-#define SERIAL_PORT_COUNT 4 //VCP, USART2, USART3, UART4, UART5, USART6, USART7, USART8, SOFTSERIAL x 2
+#define SERIAL_PORT_COUNT 6 //VCP, USART2, USART3, UART4,SOFTSERIAL x 2
 
 #define USE_SPI
 #define USE_SPI_DEVICE_1
diff --git a/src/main/target/OMNIBUSF4/target.c b/src/main/target/OMNIBUSF4/target.c
index 6f495b5f49..7fce82b65a 100644
--- a/src/main/target/OMNIBUSF4/target.c
+++ b/src/main/target/OMNIBUSF4/target.c
@@ -45,6 +45,7 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
     DEF_TIM(TIM2,  CH4, PA3,  TIM_USE_MOTOR,               TIMER_OUTPUT_STANDARD, 1), // S3_OUT D1_ST6
     DEF_TIM(TIM2,  CH3, PA2,  TIM_USE_MOTOR,               TIMER_OUTPUT_STANDARD, 0), // S4_OUT D1_ST1
 #if defined(CL_RACINGF4)
+    DEF_TIM(TIM3,  CH1, PB4,  TIM_USE_BEEPER,               TIMER_OUTPUT_ENABLED, 0 ), // BEEPER PWM
     DEF_TIM(TIM4,  CH4, PB9,  TIM_USE_MOTOR | TIM_USE_LED, TIMER_OUTPUT_STANDARD, 0), // S5_OUT
 #elif defined(OMNIBUSF4SD)
     DEF_TIM(TIM5,  CH2, PA1,  TIM_USE_MOTOR,               TIMER_OUTPUT_STANDARD, 0), // S5_OUT
diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h
index 735a66982f..5895cb8e2b 100644
--- a/src/main/target/OMNIBUSF4/target.h
+++ b/src/main/target/OMNIBUSF4/target.h
@@ -41,6 +41,7 @@
 //#define LED1                    PB4 // Remove this at the next major release
 #define BEEPER                  PB4
 #define BEEPER_INVERTED
+#define BEEPER_PWM_HZ           3800 // Beeper PWM frequency in Hz
 
 #ifdef OMNIBUSF4SD
 #define INVERTER_PIN_UART6      PC8 // Omnibus F4 V3 and later
@@ -191,16 +192,16 @@
 //#define RSSI_ADC_PIN            PA0
 #endif
 
-#define USE_ESC_SENSOR
 #define DEFAULT_RX_FEATURE      FEATURE_RX_SERIAL
-#define AVOID_UART1_FOR_PWM_PPM
 #if defined(CL_RACINGF4)
 #define DEFAULT_FEATURES        (FEATURE_TELEMETRY | FEATURE_OSD )
+#define SPEKTRUM_BIND_PIN       UART6_RX_PIN
 #else
 #define DEFAULT_FEATURES        (FEATURE_OSD)
+#define AVOID_UART1_FOR_PWM_PPM
+#define SPEKTRUM_BIND_PIN       UART1_RX_PIN
 #endif
 
-#define SPEKTRUM_BIND_PIN       UART1_RX_PIN
 #define USE_SERIAL_4WAY_BLHELI_INTERFACE
 
 #define TARGET_IO_PORTA (0xffff & ~(BIT(14)|BIT(13)))
@@ -209,13 +210,13 @@
 #define TARGET_IO_PORTD BIT(2)
 
 #ifdef CL_RACINGF4
-#define USABLE_TIMER_CHANNEL_COUNT 6
-#define USED_TIMERS  ( TIM_N(4) | TIM_N(8) )
+#define USABLE_TIMER_CHANNEL_COUNT 7
+#define USED_TIMERS  (  TIM_N(3) | TIM_N(4) | TIM_N(4) | TIM_N(8) )
 #else
 #ifdef OMNIBUSF4SD
 #define USABLE_TIMER_CHANNEL_COUNT 13
 #else
 #define USABLE_TIMER_CHANNEL_COUNT 12
 #endif
-#define USED_TIMERS  ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(5) | TIM_N(12) | TIM_N(8) | TIM_N(9))
+#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(5) | TIM_N(12) | TIM_N(8) | TIM_N(9))
 #endif
diff --git a/src/main/target/REVO/target.h b/src/main/target/REVO/target.h
index 665cc54ffa..979ca7fc01 100644
--- a/src/main/target/REVO/target.h
+++ b/src/main/target/REVO/target.h
@@ -48,8 +48,6 @@
 
 #endif
 
-#define USE_ESC_SENSOR
-
 #define LED0                    PB5
 #if defined(PODIUMF4)
 #define LED1                    PB4
diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h
index 12542977eb..c1ab6f9830 100644
--- a/src/main/target/SPRACINGF3/target.h
+++ b/src/main/target/SPRACINGF3/target.h
@@ -180,6 +180,8 @@
 #define USE_OSD_OVER_MSP_DISPLAYPORT
 #define USE_SLOW_MSP_DISPLAYPORT_RATE_WHEN_UNARMED
 
+#define USE_MSP_CURRENT_METER
+
 #define USE_ESC_SENSOR
 #define REMAP_TIM17_DMA
 
diff --git a/src/main/target/SPRACINGF3EVO/target.h b/src/main/target/SPRACINGF3EVO/target.h
index c9884f42f5..f4acbf5b98 100755
--- a/src/main/target/SPRACINGF3EVO/target.h
+++ b/src/main/target/SPRACINGF3EVO/target.h
@@ -163,6 +163,8 @@
 
 #define OSD
 #define USE_OSD_OVER_MSP_DISPLAYPORT
+#define USE_MSP_CURRENT_METER
+
 #undef USE_DASHBOARD
 
 #define TRANSPONDER
diff --git a/src/main/target/SPRACINGF4EVO/target.h b/src/main/target/SPRACINGF4EVO/target.h
index f4a0bd2d8a..b3c70e5bac 100644
--- a/src/main/target/SPRACINGF4EVO/target.h
+++ b/src/main/target/SPRACINGF4EVO/target.h
@@ -162,6 +162,7 @@
 
 #define OSD
 #define USE_OSD_OVER_MSP_DISPLAYPORT
+#define USE_MSP_CURRENT_METER
 
 #define LED_STRIP
 #define TRANSPONDER
diff --git a/src/main/target/common_fc_pre.h b/src/main/target/common_fc_pre.h
index 0d2054ea0f..112471a92b 100644
--- a/src/main/target/common_fc_pre.h
+++ b/src/main/target/common_fc_pre.h
@@ -45,12 +45,14 @@
 
 #ifdef STM32F4
 #define USE_DSHOT
+#define USE_ESC_SENSOR
 #define I2C3_OVERCLOCK true
 #define TELEMETRY_IBUS
 #endif
 
 #ifdef STM32F7
 #define USE_DSHOT
+#define USE_ESC_SENSOR
 #define I2C3_OVERCLOCK true
 #define I2C4_OVERCLOCK true
 #define TELEMETRY_IBUS
diff --git a/src/test/unit/blackbox_encoding_unittest.cc b/src/test/unit/blackbox_encoding_unittest.cc
index 836b219ee9..16ba59aa65 100644
--- a/src/test/unit/blackbox_encoding_unittest.cc
+++ b/src/test/unit/blackbox_encoding_unittest.cc
@@ -29,15 +29,6 @@ extern "C" {
 
     #include "drivers/serial.h"
     #include "io/serial.h"
-
-    PG_REGISTER_WITH_RESET_TEMPLATE(blackboxConfig_t, blackboxConfig, PG_BLACKBOX_CONFIG, 0);
-
-    PG_RESET_TEMPLATE(blackboxConfig_t, blackboxConfig,
-        .device = DEFAULT_BLACKBOX_DEVICE,
-        .rate_num = 1,
-        .rate_denom = 1,
-        .on_motor_test = 0 // default off
-    );
 }
 
 #include "unittest_macros.h"
@@ -108,6 +99,7 @@ bool isSerialTransmitBufferEmpty(const serialPort_t *instance)
 
 void serialTestResetBuffers()
 {
+    blackboxPort = &serialTestInstance;
     memset(&serialReadBuffer, 0, sizeof(serialReadBuffer));
     serialReadPos = 0;
     serialReadEnd = 0;
@@ -115,9 +107,10 @@ void serialTestResetBuffers()
     serialWritePos = 0;
 }
 
-TEST(BlackboxTest, Test1)
+TEST(BlackboxEncodingTest, TestWriteUnsignedVB)
 {
-    blackboxPort = &serialTestInstance;
+    serialTestResetBuffers();
+
     blackboxWriteUnsignedVB(0);
     EXPECT_EQ(0, serialWriteBuffer[0]);
     blackboxWriteUnsignedVB(128);
@@ -125,8 +118,131 @@ TEST(BlackboxTest, Test1)
     EXPECT_EQ(1, serialWriteBuffer[2]);
 }
 
+TEST(BlackboxTest, TestWriteTag2_3SVariable_BITS2)
+{
+    serialTestResetBuffers();
+    uint8_t *buf = &serialWriteBuffer[0];
+    int selector;
+    int32_t v[3];
+
+    // 2 bits per field  ss11 2233,
+    v[0] = 0;
+    v[1] = 0;
+    v[2] = 0;
+    selector = blackboxWriteTag2_3SVariable(v);
+    EXPECT_EQ(0, selector);
+    EXPECT_EQ(0, buf[0]);
+    EXPECT_EQ(0, buf[1]); // ensure next byte has not been written
+    ++buf;
+
+    v[0] = 1;
+    selector = blackboxWriteTag2_3SVariable(v);
+    EXPECT_EQ(0x10, buf[0]); // 00010000
+    EXPECT_EQ(0, buf[1]); // ensure next byte has not been written
+    ++buf;
+
+    v[0] = 1;
+    v[1] = 1;
+    v[2] = 1;
+    selector = blackboxWriteTag2_3SVariable(v);
+    EXPECT_EQ(0, selector);
+    EXPECT_EQ(0x15, buf[0]); // 00010101
+    EXPECT_EQ(0, buf[1]); // ensure next byte has not been written
+    ++buf;
+
+    v[0] = -1;
+    v[1] = -1;
+    v[2] = -1;
+    selector = blackboxWriteTag2_3SVariable(v);
+    EXPECT_EQ(0, selector);
+    EXPECT_EQ(0x3F, buf[0]); // 00111111
+    EXPECT_EQ(0, buf[1]); // ensure next byte has not been written
+    ++buf;
+
+    v[0] = -2;
+    v[1] = -2;
+    v[2] = -2;
+    selector = blackboxWriteTag2_3SVariable(v);
+    EXPECT_EQ(0, selector);
+    EXPECT_EQ(0x2A, buf[0]); // 00101010
+    EXPECT_EQ(0, buf[1]); // ensure next byte has not been written
+    ++buf;
+}
+
+TEST(BlackboxTest, TestWriteTag2_3SVariable_BITS554)
+{
+    serialTestResetBuffers();
+    uint8_t *buf = &serialWriteBuffer[0];
+    int selector;
+    int32_t v[3];
+
+    // 554 bits per field  ss11 1112 2222 3333
+    // 5 bits per field [-16, 15], 4 bits per field [-8, 7]
+    v[0] = 15;
+    v[1] = 15;
+    v[2] = 7;
+    selector = blackboxWriteTag2_3SVariable(v);
+    EXPECT_EQ(1, selector);
+    EXPECT_EQ(0x5E, buf[0]); // 0101 1110
+    EXPECT_EQ(0xF7, buf[1]); // 1111 0111
+    EXPECT_EQ(0, buf[2]); // ensure next byte has not been written
+    buf += 2;
+
+    v[0] = -16;
+    v[1] = -16;
+    v[2] = -8;
+    selector = blackboxWriteTag2_3SVariable(v);
+    EXPECT_EQ(1, selector);
+    EXPECT_EQ(0x61, buf[0]); // 0110 0001
+    EXPECT_EQ(0x08, buf[1]); // 0000 1000
+    EXPECT_EQ(0, buf[2]); // ensure next byte has not been written
+    buf += 2;
+
+    v[0] = 7;
+    v[1] = 8;
+    v[2] = 5;
+    selector = blackboxWriteTag2_3SVariable(v);
+    EXPECT_EQ(1, selector);
+    EXPECT_EQ(0x4E, buf[0]); // 0100 1110
+    EXPECT_EQ(0x85, buf[1]); // 1000 0101
+    EXPECT_EQ(0, buf[2]); // ensure next byte has not been written
+    buf += 2;
+}
+
+TEST(BlackboxTest, TestWriteTag2_3SVariable_BITS887)
+{
+    serialTestResetBuffers();
+    uint8_t *buf = &serialWriteBuffer[0];
+    int selector;
+    int32_t v[3];
+
+    // 877 bits per field  ss11 1111 1122 2222 2333 3333
+    // 8 bits per field [-128, 127], 7 bits per field [-64, 63]
+    v[0] = 127;
+    v[1] = 63;
+    v[2] = 63;
+    selector = blackboxWriteTag2_3SVariable(v);
+    EXPECT_EQ(2, selector);
+    EXPECT_EQ(0x9F, buf[0]); // 1001 1111
+    EXPECT_EQ(0xDF, buf[1]); // 1101 1111
+    EXPECT_EQ(0xBF, buf[2]); // 1011 1111
+    EXPECT_EQ(0, buf[3]); // ensure next byte has not been written
+    buf += 3;
+
+    v[0] = -128;
+    v[1] = -64;
+    v[2] = -64;
+    selector = blackboxWriteTag2_3SVariable(v);
+    EXPECT_EQ(2, selector);
+    EXPECT_EQ(0xA0, buf[0]); // 1010 0000
+    EXPECT_EQ(0x20, buf[1]); // 0010 0000
+    EXPECT_EQ(0x40, buf[2]); // 0100 0000
+    EXPECT_EQ(0, buf[3]); // ensure next byte has not been written
+    buf += 3;
+}
 // STUBS
 extern "C" {
+PG_REGISTER(blackboxConfig_t, blackboxConfig, PG_BLACKBOX_CONFIG, 0);
 int32_t blackboxHeaderBudget;
 void mspSerialAllocatePorts(void) {}
 void blackboxWrite(uint8_t value) {serialWrite(blackboxPort, value);}
@@ -140,5 +256,4 @@ int blackboxPrint(const char *s)
     const int length = pos - (uint8_t*)s;
     return length;
 }
-
 }