Merge pull request #5857 from iNavFlight/agh_frskyosd_v2

Add support for FrSkyOSD version 2

make/source.mk

@@ -50,6 +50,7 @@ MAIN_SRC = \
             drivers/display.c \
             drivers/display_canvas.c \
             drivers/display_font_metadata.c \
+            drivers/display_widgets.c \
             drivers/exti.c \
             drivers/io_pca9685.c \
             drivers/io_pcf8574.c \

src/main/cms/cms.c

@@ -1314,6 +1314,7 @@ void cmsUpdate(uint32_t currentTimeUs)
             rcDelayMs = BUTTON_PAUSE;    // Tends to overshoot if BUTTON_TIME
         }
     } else {
+        displayBeginTransaction(pCurrentDisplay, DISPLAY_TRANSACTION_OPT_RESET_DRAWING);
 
         // Check if we're yielding and its's time to stop it
         if (cmsYieldUntil > 0 && currentTimeMs > cmsYieldUntil) {
@@ -1339,6 +1340,7 @@ void cmsUpdate(uint32_t currentTimeUs)
             displayHeartbeat(pCurrentDisplay);
             lastCmsHeartBeatMs = currentTimeMs;
         }
+        displayCommitTransaction(pCurrentDisplay);
     }
 
     // Some key (command), notably flash erase, takes too long to use the

src/main/cms/cms_menu_osd.c

@@ -75,7 +75,7 @@ static long cmsx_osdElementOnChange(displayPort_t *displayPort, const void *ptr)
 {
     UNUSED(ptr);
 
-    uint16_t *pos = &osdConfigMutable()->item_pos[osdCurrentLayout][osdCurrentItem];
+    uint16_t *pos = &osdLayoutsConfigMutable()->item_pos[osdCurrentLayout][osdCurrentItem];
     *pos = OSD_POS(osdCurrentElementColumn, osdCurrentElementRow);
     if (osdCurrentElementVisible) {
         *pos |= OSD_VISIBLE_FLAG;
@@ -125,7 +125,7 @@ static CMS_Menu cmsx_menuOsdElementActions = {
 static long osdElemActionsOnEnter(const OSD_Entry *from)
 {
     osdCurrentItem = from->itemId;
-    uint16_t pos = osdConfig()->item_pos[osdCurrentLayout][osdCurrentItem];
+    uint16_t pos = osdLayoutsConfig()->item_pos[osdCurrentLayout][osdCurrentItem];
     osdCurrentElementColumn = OSD_X(pos);
     osdCurrentElementRow = OSD_Y(pos);
     osdCurrentElementVisible = OSD_VISIBLE(pos) ? 1 : 0;

src/main/config/parameter_group_ids.h

@@ -112,7 +112,8 @@
 #define PG_RPM_FILTER_CONFIG 1022
 #define PG_GLOBAL_VARIABLE_CONFIG 1023
 #define PG_SMARTPORT_MASTER_CONFIG 1024
-#define PG_INAV_END 1024
+#define PG_OSD_LAYOUTS_CONFIG 1025
+#define PG_INAV_END 1025
 
 // OSD configuration (subject to change)
 //#define PG_OSD_FONT_CONFIG 2047

src/main/drivers/display_canvas.c

@@ -273,3 +273,8 @@ void displayCanvasContextPop(displayCanvas_t *displayCanvas)
         displayCanvas->vTable->contextPop(displayCanvas);
     }
 }
+
+bool displayCanvasGetWidgets(displayWidgets_t *widgets, const displayCanvas_t *displayCanvas)
+{
+    return displayCanvas && displayCanvas->vTable->getWidgets ? displayCanvas->vTable->getWidgets(widgets, displayCanvas) : false;
+}

src/main/drivers/display_canvas.h

@@ -29,6 +29,8 @@
 #include <stdbool.h>
 #include <stdint.h>
 
+typedef struct displayWidgets_s displayWidgets_t;
+
 typedef enum {
     DISPLAY_CANVAS_BITMAP_OPT_INVERT_COLORS = 1 << 0,
     DISPLAY_CANVAS_BITMAP_OPT_SOLID_BACKGROUND = 1 << 1,
@@ -100,8 +102,9 @@ typedef struct displayCanvasVTable_s {
 
     void (*contextPush)(displayCanvas_t *displayCanvas);
     void (*contextPop)(displayCanvas_t *displayCanvas);
-} displayCanvasVTable_t;
 
+    bool (*getWidgets)(displayWidgets_t *widgets, const displayCanvas_t *displayCanvas);
+} displayCanvasVTable_t;
 
 void displayCanvasSetStrokeColor(displayCanvas_t *displayCanvas, displayCanvasColor_e color);
 void displayCanvasSetFillColor(displayCanvas_t *displayCanvas, displayCanvasColor_e color);
@@ -141,3 +144,5 @@ void displayCanvasCtmRotate(displayCanvas_t *displayCanvas, float r);
 
 void displayCanvasContextPush(displayCanvas_t *displayCanvas);
 void displayCanvasContextPop(displayCanvas_t *displayCanvas);
+
+bool displayCanvasGetWidgets(displayWidgets_t *widgets, const displayCanvas_t *displayCanvas);

src/main/drivers/display_widgets.c

@@ -0,0 +1,33 @@
+
+#include "platform.h"
+
+#if defined(USE_CANVAS)
+
+#include "drivers/display_widgets.h"
+
+int displayWidgetsSupportedInstances(displayWidgets_t *widgets, displayWidgetType_e widgetType)
+{
+    return widgets->vTable->supportedInstances ? widgets->vTable->supportedInstances(widgets, widgetType) : 0;
+}
+
+bool displayWidgetsConfigureAHI(displayWidgets_t *widgets, unsigned instance, const widgetAHIConfiguration_t *config)
+{
+    return widgets->vTable->configureAHI ? widgets->vTable->configureAHI(widgets, instance, config) : false;
+}
+
+bool displayWidgetsDrawAHI(displayWidgets_t *widgets, unsigned instance, const widgetAHIData_t *data)
+{
+    return widgets->vTable->drawAHI ? widgets->vTable->drawAHI(widgets, instance, data) : false;
+}
+
+bool displayWidgetsConfigureSidebar(displayWidgets_t *widgets, unsigned instance, const widgetSidebarConfiguration_t *config)
+{
+    return widgets->vTable->configureSidebar ? widgets->vTable->configureSidebar(widgets, instance, config) : false;
+}
+
+bool displayWidgetsDrawSidebar(displayWidgets_t *widgets, unsigned instance, int32_t data)
+{
+    return widgets->vTable->drawSidebar ? widgets->vTable->drawSidebar(widgets, instance, data) : false;
+}
+
+#endif

src/main/drivers/display_widgets.h

@@ -0,0 +1,77 @@
+#pragma once
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "drivers/osd.h"
+
+typedef struct widgetRect_s {
+    int x;
+    int y;
+    unsigned w;
+    unsigned h;
+} widgetRect_t;
+
+typedef enum {
+    DISPLAY_WIDGET_TYPE_AHI,
+    DISPLAY_WIDGET_TYPE_SIDEBAR,
+} displayWidgetType_e;
+
+typedef enum {
+    DISPLAY_WIDGET_AHI_STYLE_STAIRCASE = 0,
+    DISPLAY_WIDGET_AHI_STYLE_LINE = 1,
+} widgetAHIStyle_e;
+
+typedef enum {
+    DISPLAY_WIDGET_AHI_OPTION_SHOW_CORNERS = 1 << 0,
+} widgetAHIOptions_t;
+
+typedef struct widgetAHIConfiguration_s {
+    widgetRect_t rect;
+    widgetAHIStyle_e style;
+    widgetAHIOptions_t options;
+    unsigned crosshairMargin;
+    unsigned strokeWidth;
+} widgetAHIConfiguration_t;
+
+typedef struct widgetAHIData_s {
+    float pitch; // radians
+    float roll; // radians
+} widgetAHIData_t;
+
+typedef enum
+{
+    DISPLAY_WIDGET_SIDEBAR_OPTION_LEFT = 1 << 0,      // Display the sidebar oriented to the left. Default is to the right
+    DISPLAY_WIDGET_SIDEBAR_OPTION_REVERSE = 1 << 1,   // Reverse the sidebar direction, so positive values move it down
+    DISPLAY_WIDGET_SIDEBAR_OPTION_UNLABELED = 1 << 2, // Don't display the central label with the value.
+    DISPLAY_WIDGET_SIDEBAR_OPTION_STATIC = 1 << 3,    // The sidebar doesn't scroll nor display values along it.
+} widgetSidebarOptions_t;
+
+typedef struct widgetSidebarConfiguration_s {
+    widgetRect_t rect;
+    widgetSidebarOptions_t options;
+    uint8_t divisions;                  // How many divisions the sidebar will have
+    uint16_t counts_per_step;           // How much the value increases/decreases per division BEFORE applying the unit scale
+    osdUnit_t unit;                     // The unit used to display the values in the sidebar
+} widgetSidebarConfiguration_t;
+
+typedef struct displayWidgetsVTable_s displayWidgetsVTable_t;
+
+typedef struct displayWidgets_s {
+    const displayWidgetsVTable_t *vTable;
+    void *device;
+} displayWidgets_t;
+
+typedef struct displayWidgetsVTable_s {
+    int (*supportedInstances)(displayWidgets_t *widgets, displayWidgetType_e widgetType);
+    bool (*configureAHI)(displayWidgets_t *widgets, unsigned instance, const widgetAHIConfiguration_t *config);
+    bool (*drawAHI)(displayWidgets_t *widgets, unsigned instance, const widgetAHIData_t *data);
+    bool (*configureSidebar)(displayWidgets_t *widgets, unsigned instance, const widgetSidebarConfiguration_t *config);
+    bool (*drawSidebar)(displayWidgets_t *widgets, unsigned instance, int32_t data);
+} displayWidgetsVTable_t;
+
+int displayWidgetsSupportedInstances(displayWidgets_t *widgets, displayWidgetType_e widgetType);
+bool displayWidgetsConfigureAHI(displayWidgets_t *widgets, unsigned instance, const widgetAHIConfiguration_t *config);
+bool displayWidgetsDrawAHI(displayWidgets_t *widgets, unsigned instance, const widgetAHIData_t *data);
+bool displayWidgetsConfigureSidebar(displayWidgets_t *widgets, unsigned instance, const widgetSidebarConfiguration_t *config);
+bool displayWidgetsDrawSidebar(displayWidgets_t *widgets, unsigned instance, int32_t data);

src/main/drivers/osd.c

@@ -23,8 +23,13 @@
  *
  */
 
+#include "platform.h"
+
+#if defined(USE_OSD)
+
 #include "drivers/display_canvas.h"
 #include "drivers/osd.h"
+#include "drivers/osd_symbols.h"
 
 uint16_t osdCharacterGridBuffer[OSD_CHARACTER_GRID_BUFFER_SIZE] ALIGNED(4);
 
@@ -32,8 +37,9 @@ void osdCharacterGridBufferClear(void)
 {
     uint32_t *ptr = (uint32_t *)osdCharacterGridBuffer;
     uint32_t *end = (uint32_t *)(ARRAYEND(osdCharacterGridBuffer));
+    uint32_t blank32 = SYM_BLANK << 24 | SYM_BLANK << 16 | SYM_BLANK << 8 | SYM_BLANK;
     for (; ptr < end; ptr++) {
-        *ptr = 0;
+        *ptr = blank32;
     }
 }
 
@@ -74,7 +80,7 @@ void osdGridBufferClearGridRect(int x, int y, int w, int h)
     int maxY = y + h;
     for (int ii = x; ii <= maxX; ii++) {
         for (int jj = y; jj <= maxY; jj++) {
-            *osdCharacterGridBufferGetEntryPtr(ii, jj) = 0;
+            *osdCharacterGridBufferGetEntryPtr(ii, jj) = SYM_BLANK;
         }
     }
 }
@@ -102,3 +108,5 @@ uint16_t *osdCharacterGridBufferGetEntryPtr(unsigned x, unsigned y)
     unsigned pos = y * OSD_CHARACTER_GRID_MAX_WIDTH + x;
     return &osdCharacterGridBuffer[pos];
 }
+
+#endif

src/main/drivers/osd.h

@@ -61,14 +61,23 @@ typedef struct osdCharacter_s {
     uint8_t data[OSD_CHAR_BYTES];
 } osdCharacter_t;
 
+typedef struct osdUnit_t
+{
+    uint16_t scale;             // The scale between the value and the represented unit. e.g. if you're providing cms but you want to draw meters this should be 100 ([0, 1023])
+    uint16_t symbol;            // Symbol to append/prepend to the value when it's not scaled [0, 511]
+    uint16_t divisor;           // If abs(value) > divisor, divide it by this. e.g. for meters and km you'd set this to 1000 [0, UINT16_MAX)
+    uint16_t divided_symbol;    // Symbol to append/prepend to the value when it's divided (e.g. the km symbol) [0, 511]
+} osdUnit_t;
+
 #define OSD_CHARACTER_GRID_MAX_WIDTH 30
 #define OSD_CHARACTER_GRID_MAX_HEIGHT 16
 #define OSD_CHARACTER_GRID_BUFFER_SIZE (OSD_CHARACTER_GRID_MAX_WIDTH * OSD_CHARACTER_GRID_MAX_HEIGHT)
 
 extern uint16_t osdCharacterGridBuffer[OSD_CHARACTER_GRID_BUFFER_SIZE] ALIGNED(4);
 
-// Sets all buffer entries to 0
+// Sets all buffer entries to SYM_BLANK
 void osdCharacterGridBufferClear(void);
 void osdGridBufferClearGridRect(int x, int y, int w, int h);
 void osdGridBufferClearPixelRect(displayCanvas_t *canvas, int x, int y, int w, int h);
+
 uint16_t *osdCharacterGridBufferGetEntryPtr(unsigned x, unsigned y);

src/main/fc/cli.c

@@ -810,27 +810,19 @@ static void cliSerial(char *cmdline)
 
         switch (i) {
         case 0:
-            if (baudRateIndex < BAUD_1200 || baudRateIndex > BAUD_2470000) {
+            baudRateIndex = constrain(baudRateIndex, BAUD_MIN, BAUD_MAX);
-                continue;
-            }
             portConfig.msp_baudrateIndex = baudRateIndex;
             break;
         case 1:
-            if (baudRateIndex < BAUD_9600 || baudRateIndex > BAUD_115200) {
+            baudRateIndex = constrain(baudRateIndex, BAUD_MIN, BAUD_MAX);
-                continue;
-            }
             portConfig.gps_baudrateIndex = baudRateIndex;
             break;
         case 2:
-            if (baudRateIndex != BAUD_AUTO && baudRateIndex > BAUD_115200) {
+            baudRateIndex = constrain(baudRateIndex, BAUD_MIN, BAUD_MAX);
-                continue;
-            }
             portConfig.telemetry_baudrateIndex = baudRateIndex;
             break;
         case 3:
-            if (baudRateIndex < BAUD_19200 || baudRateIndex > BAUD_250000) {
+            baudRateIndex = constrain(baudRateIndex, BAUD_MIN, BAUD_MAX);
-                continue;
-            }
             portConfig.peripheral_baudrateIndex = baudRateIndex;
             break;
         }
@@ -2122,7 +2114,7 @@ static void cliFlashRead(char *cmdline)
 #endif
 
 #ifdef USE_OSD
-static void printOsdLayout(uint8_t dumpMask, const osdConfig_t *osdConfig, const osdConfig_t *osdConfigDefault, int layout, int item)
+static void printOsdLayout(uint8_t dumpMask, const osdLayoutsConfig_t *config, const osdLayoutsConfig_t *configDefault, int layout, int item)
 {
     // "<layout> <item> <col> <row> <visible>"
     const char *format = "osd_layout %d %d %d %d %c";
@@ -2130,8 +2122,8 @@ static void printOsdLayout(uint8_t dumpMask, const osdConfig_t *osdConfig, const
         if (layout >= 0 && layout != ii) {
             continue;
         }
-        const uint16_t *layoutItems = osdConfig->item_pos[ii];
+        const uint16_t *layoutItems = config->item_pos[ii];
-        const uint16_t *defaultLayoutItems = osdConfigDefault->item_pos[ii];
+        const uint16_t *defaultLayoutItems = configDefault->item_pos[ii];
         for (int jj = 0; jj < OSD_ITEM_COUNT; jj++) {
             if (item >= 0 && item != jj) {
                 continue;
@@ -2223,15 +2215,15 @@ static void cliOsdLayout(char *cmdline)
             // No args, or just layout or layout and item. If any of them not provided,
             // it will be the -1 that we used during initialization, so printOsdLayout()
             // won't use them for filtering.
-            printOsdLayout(DUMP_MASTER, osdConfig(), osdConfig(), layout, item);
+            printOsdLayout(DUMP_MASTER, osdLayoutsConfig(), osdLayoutsConfig(), layout, item);
             break;
         case 4:
             // No visibility provided. Keep the previous one.
-            visible = OSD_VISIBLE(osdConfig()->item_pos[layout][item]);
+            visible = OSD_VISIBLE(osdLayoutsConfig()->item_pos[layout][item]);
             FALLTHROUGH;
         case 5:
             // Layout, item, pos and visibility. Set the item.
-            osdConfigMutable()->item_pos[layout][item] = OSD_POS(col, row) | (visible ? OSD_VISIBLE_FLAG : 0);
+            osdLayoutsConfigMutable()->item_pos[layout][item] = OSD_POS(col, row) | (visible ? OSD_VISIBLE_FLAG : 0);
             break;
         default:
             // Unhandled
@@ -3276,7 +3268,7 @@ static void printConfig(const char *cmdline, bool doDiff)
 
 #ifdef USE_OSD
         cliPrintHashLine("osd_layout");
-        printOsdLayout(dumpMask, &osdConfig_Copy, osdConfig(), -1, -1);
+        printOsdLayout(dumpMask, &osdLayoutsConfig_Copy, osdLayoutsConfig(), -1, -1);
 #endif
 
         cliPrintHashLine("master");

src/main/fc/fc_msp.c

@@ -1092,7 +1092,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
         sbufWriteU16(dst, osdConfig()->dist_alarm);
         sbufWriteU16(dst, osdConfig()->neg_alt_alarm);
         for (int i = 0; i < OSD_ITEM_COUNT; i++) {
-            sbufWriteU16(dst, osdConfig()->item_pos[0][i]);
+            sbufWriteU16(dst, osdLayoutsConfig()->item_pos[0][i]);
         }
 #else
         sbufWriteU8(dst, OSD_DRIVER_NONE); // OSD not supported
@@ -2281,7 +2281,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
         } else {
             // set a position setting
             if ((dataSize >= 3) && (tmp_u8 < OSD_ITEM_COUNT)) // tmp_u8 == addr
-                osdConfigMutable()->item_pos[0][tmp_u8] = sbufReadU16(src);
+                osdLayoutsConfigMutable()->item_pos[0][tmp_u8] = sbufReadU16(src);
             else
                 return MSP_RESULT_ERROR;
         }
@@ -2575,10 +2575,10 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
 
                 portConfig->identifier = identifier;
                 portConfig->functionMask = sbufReadU16(src);
-                portConfig->msp_baudrateIndex = sbufReadU8(src);
+                portConfig->msp_baudrateIndex = constrain(sbufReadU8(src), BAUD_MIN, BAUD_MAX);
-                portConfig->gps_baudrateIndex = sbufReadU8(src);
+                portConfig->gps_baudrateIndex = constrain(sbufReadU8(src), BAUD_MIN, BAUD_MAX);
-                portConfig->telemetry_baudrateIndex = sbufReadU8(src);
+                portConfig->telemetry_baudrateIndex = constrain(sbufReadU8(src), BAUD_MIN, BAUD_MAX);
-                portConfig->peripheral_baudrateIndex = sbufReadU8(src);
+                portConfig->peripheral_baudrateIndex = constrain(sbufReadU8(src), BAUD_MIN, BAUD_MAX);
             }
         }
         break;
@@ -2603,10 +2603,10 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
 
                 portConfig->identifier = identifier;
                 portConfig->functionMask = sbufReadU32(src);
-                portConfig->msp_baudrateIndex = sbufReadU8(src);
+                portConfig->msp_baudrateIndex = constrain(sbufReadU8(src), BAUD_MIN, BAUD_MAX);
-                portConfig->gps_baudrateIndex = sbufReadU8(src);
+                portConfig->gps_baudrateIndex = constrain(sbufReadU8(src), BAUD_MIN, BAUD_MAX);
-                portConfig->telemetry_baudrateIndex = sbufReadU8(src);
+                portConfig->telemetry_baudrateIndex = constrain(sbufReadU8(src), BAUD_MIN, BAUD_MAX);
-                portConfig->peripheral_baudrateIndex = sbufReadU8(src);
+                portConfig->peripheral_baudrateIndex = constrain(sbufReadU8(src), BAUD_MIN, BAUD_MAX);
             }
         }
         break;
@@ -2728,7 +2728,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
             if (!sbufReadU8Safe(&item, src)) {
                 return MSP_RESULT_ERROR;
             }
-            if (!sbufReadU16Safe(&osdConfigMutable()->item_pos[layout][item], src)) {
+            if (!sbufReadU16Safe(&osdLayoutsConfigMutable()->item_pos[layout][item], src)) {
                 return MSP_RESULT_ERROR;
             }
             // If the layout is not already overriden and it's different
@@ -3148,11 +3148,11 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
                     *ret = MSP_RESULT_ERROR;
                     break;
                 }
-                sbufWriteU16(dst, osdConfig()->item_pos[layout][item]);
+                sbufWriteU16(dst, osdLayoutsConfig()->item_pos[layout][item]);
             } else {
                 // Asking for an specific layout
                 for (unsigned ii = 0; ii < OSD_ITEM_COUNT; ii++) {
-                    sbufWriteU16(dst, osdConfig()->item_pos[layout][ii]);
+                    sbufWriteU16(dst, osdLayoutsConfig()->item_pos[layout][ii]);
                 }
             }
         } else {

src/main/fc/settings.yaml

@@ -2838,6 +2838,57 @@ groups:
         default_value: "DEFAULT"
         table: osd_ahi_style
 
+      - name: osd_force_grid
+        field: force_grid
+        type: bool
+        default_value: "OFF"
+        description: Force OSD to work in grid mode even if the OSD device supports pixel level access (mainly used for development)
+
+      - name: osd_ahi_bordered
+        field: ahi_bordered
+        type: bool
+        description: Shows a border/corners around the AHI region (pixel OSD only)
+        default_value: "OFF"
+
+      - name: osd_ahi_width
+        field: ahi_width
+        max: 255
+        description: AHI width in pixels (pixel OSD only)
+        default_value: 132
+
+      - name: osd_ahi_height
+        field: ahi_height
+        max: 255
+        description: AHI height in pixels (pixel OSD only)
+        default_value: 162
+
+      - name: osd_ahi_vertical_offset
+        field: ahi_vertical_offset
+        min: -128
+        max: 127
+        description: AHI vertical offset from center (pixel OSD only)
+        default_value: 0
+
+      - name: osd_sidebar_horizontal_offset
+        field: sidebar_horizontal_offset
+        min: -128
+        max: 127
+        default_value: 0
+        description: Sidebar horizontal offset from default position. Positive values move the sidebars closer to the edges.
+
+      - name: osd_left_sidebar_scroll_step
+        field: left_sidebar_scroll_step
+        max: 255
+        default_value: 0
+        description: How many units each sidebar step represents. 0 means the default value for the scroll type.
+
+      - name: osd_right_sidebar_scroll_step
+        field: right_sidebar_scroll_step
+        max: 255
+        default_value: 0
+        description: Same as left_sidebar_scroll_step, but for the right sidebar
+
+
   - name: PG_SYSTEM_CONFIG
     type: systemConfig_t
     headers: ["fc/config.h"]

src/main/io/displayport_frsky_osd.c

@@ -22,11 +22,13 @@
 
 #if defined(USE_FRSKYOSD)
 
+#include "common/maths.h"
 #include "common/utils.h"
 
 #include "drivers/display.h"
 #include "drivers/display_canvas.h"
 #include "drivers/display_font_metadata.h"
+#include "drivers/display_widgets.h"
 
 #include "io/displayport_frsky_osd.h"
 #include "io/frsky_osd.h"
@@ -444,6 +446,111 @@ static void contextPop(displayCanvas_t *displayCanvas)
     frskyOSDContextPop();
 }
 
+static int supportedInstances(displayWidgets_t *widgets, displayWidgetType_e widgetType)
+{
+    UNUSED(widgets);
+
+    if (frskyOSDSupportsWidgets()) {
+        switch (widgetType) {
+            case DISPLAY_WIDGET_TYPE_AHI:
+                return 1;
+            case DISPLAY_WIDGET_TYPE_SIDEBAR:
+                return FRSKY_OSD_WIDGET_ID_SIDEBAR_LAST - FRSKY_OSD_WIDGET_ID_SIDEBAR_FIRST + 1;
+        }
+    }
+    return 0;
+}
+
+static bool configureAHI(displayWidgets_t *widgets, unsigned instance, const widgetAHIConfiguration_t *config)
+{
+    UNUSED(widgets);
+
+    if (frskyOSDSupportsWidgets() && instance == 0) {
+        frskyOSDWidgetAHIConfig_t cfg = {
+            .rect.origin.x = config->rect.x,
+            .rect.origin.y = config->rect.y,
+            .rect.size.w = config->rect.w,
+            .rect.size.h = config->rect.h,
+            .style = config->style,
+            .options = config->options,
+            .crosshairMargin = config->crosshairMargin,
+            .strokeWidth = config->strokeWidth,
+        };
+        return frskyOSDSetWidgetConfig(FRSKY_OSD_WIDGET_ID_AHI, &cfg, sizeof(cfg));
+    }
+    return false;
+}
+
+static bool drawAHI(displayWidgets_t *widgets, unsigned instance, const widgetAHIData_t *data)
+{
+    UNUSED(widgets);
+
+    if (frskyOSDSupportsWidgets() && instance == 0) {
+        frskyOSDWidgetAHIData_t ahiData = {
+            .pitch = frskyOSDQuantize(data->pitch, 0, 2 * M_PIf, 12),
+            .roll = frskyOSDQuantize(data->roll, 0, 2 * M_PIf, 12),
+        };
+        return frskyOSDDrawWidget(FRSKY_OSD_WIDGET_ID_AHI, &ahiData, sizeof(ahiData));
+    }
+    return false;
+}
+
+static bool configureSidebar(displayWidgets_t *widgets, unsigned instance, const widgetSidebarConfiguration_t *config)
+{
+    UNUSED(widgets);
+
+    if (frskyOSDSupportsWidgets()) {
+        frskyOSDWidgetID_e id = FRSKY_OSD_WIDGET_ID_SIDEBAR_FIRST + instance;
+        if (id <= FRSKY_OSD_WIDGET_ID_SIDEBAR_LAST) {
+            frskyOSDWidgetSidebarConfig_t cfg = {
+                .rect.origin.x = config->rect.x,
+                .rect.origin.y = config->rect.y,
+                .rect.size.w = config->rect.w,
+                .rect.size.h = config->rect.h,
+                .options = config->options,
+                .divisions = config->divisions,
+                .counts_per_step = config->counts_per_step,
+                .unit = config->unit,
+            };
+            return frskyOSDSetWidgetConfig(id, &cfg, sizeof(cfg));
+        }
+    }
+    return false;
+}
+
+static bool drawSidebar(displayWidgets_t *widgets, unsigned instance, int32_t data)
+{
+    UNUSED(widgets);
+
+    if (frskyOSDSupportsWidgets()) {
+        frskyOSDWidgetID_e id = FRSKY_OSD_WIDGET_ID_SIDEBAR_FIRST + instance;
+        if (id <= FRSKY_OSD_WIDGET_ID_SIDEBAR_LAST) {
+            frskyOSDWidgetSidebarData_t sidebarData = {
+                .value = data,
+            };
+            return frskyOSDDrawWidget(id, &sidebarData, sizeof(sidebarData));
+        }
+    }
+    return false;
+}
+
+static const displayWidgetsVTable_t frskyOSDWidgetsVTable = {
+    .supportedInstances = supportedInstances,
+    .configureAHI = configureAHI,
+    .drawAHI = drawAHI,
+    .configureSidebar = configureSidebar,
+    .drawSidebar = drawSidebar,
+};
+
+static bool getWidgets(displayWidgets_t *widgets, const displayCanvas_t *displayCanvas)
+{
+    if (frskyOSDSupportsWidgets()) {
+        widgets->device = displayCanvas->device;
+        widgets->vTable = &frskyOSDWidgetsVTable;
+        return true;
+    }
+    return false;
+}
 
 static const displayCanvasVTable_t frskyOSDCanvasVTable = {
     .setStrokeColor = setStrokeColor,
@@ -484,12 +591,13 @@ static const displayCanvasVTable_t frskyOSDCanvasVTable = {
 
     .contextPush = contextPush,
     .contextPop = contextPop,
+
+    .getWidgets = getWidgets,
 };
 
 static bool getCanvas(displayCanvas_t *canvas, const displayPort_t *instance)
 {
-    UNUSED(instance);
+    canvas->device = instance->device;
-
     canvas->vTable = &frskyOSDCanvasVTable;
     canvas->width = frskyOSDGetPixelWidth();
     canvas->height = frskyOSDGetPixelHeight();

src/main/io/frsky_osd.c

@@ -18,8 +18,8 @@
 #include "io/frsky_osd.h"
 #include "io/serial.h"
 
-#define FRSKY_OSD_BAUDRATE 115200
+#define FRSKY_OSD_DEFAULT_BAUDRATE_INDEX BAUD_115200
-#define FRSKY_OSD_SUPPORTED_API_VERSION 1
+#define FRSKY_OSD_SUPPORTED_API_VERSION 2
 
 #define FRSKY_OSD_PREAMBLE_BYTE_0 '$'
 #define FRSKY_OSD_PREAMBLE_BYTE_1 'A'
@@ -40,7 +40,8 @@
 
 #define FRSKY_OSD_CMD_RESPONSE_ERROR 0
 
-#define FRSKY_OSD_INFO_INTERVAL_MS 1000
+#define FRSKY_OSD_INFO_INTERVAL_MS 100
+#define FRSKY_OSD_INFO_READY_INTERVAL_MS 5000
 
 #define FRSKY_OSD_TRACE(fmt, ...)
 #define FRSKY_OSD_DEBUG(fmt, ...) LOG_D(OSD, "FrSky OSD: " fmt,  ##__VA_ARGS__)
@@ -114,6 +115,14 @@ typedef enum
     // MAX7456 emulation commands
     OSD_CMD_DRAW_GRID_CHR = 110,
     OSD_CMD_DRAW_GRID_STR = 111,
+    OSD_CMD_DRAW_GRID_CHR_2 = 112,                                  // API2
+    OSD_CMD_DRAW_GRID_STR_2 = 113,                                  // API2
+
+    OSD_CMD_WIDGET_SET_CONFIG = 115,                                // API2
+    OSD_CMD_WIDGET_DRAW = 116,                                      // API2
+    OSD_CMD_WIDGET_ERASE = 117,                                     // API2
+
+    OSD_CMD_SET_DATA_RATE = 122,
 } osdCommand_e;
 
 typedef enum {
@@ -159,23 +168,19 @@ typedef struct frskyOSDDrawGridStrHeaderCmd_s {
     uint8_t gx;
     uint8_t gy;
     uint8_t opts;
-    // uvarint with size and blob folow
+    // uvarint with size and blob follow
+    // string IS null-terminated
 } __attribute__((packed)) frskyOSDDrawGridStrHeaderCmd_t;
 
-typedef struct frskyOSDPoint_s {
+typedef struct frskyOSDDrawGridStrV2HeaderCmd_s {
-    int x : 12;
+    unsigned gx : 5;   // +5 = 5
-    int y : 12;
+    unsigned gy : 4;   // +4 = 9
-} __attribute__((packed)) frskyOSDPoint_t;
+    unsigned opts : 3; // +3 = 12
-
+    unsigned size : 4; // +4 = 16 = 2 bytes
-typedef struct frskyOSDSize_s {
+    // if size == 0, uvarint with size follows
-    int w : 12;
+    // blob with the given size follows
-    int h : 12;
+    // string IS NOT null terminated
-} __attribute__((packed)) frskyOSDSize_t;
+} __attribute__((packed)) frskyOSDDrawGridStrV2HeaderCmd_t;
-
-typedef struct frskyOSDRect_s {
-    frskyOSDPoint_t origin;
-    frskyOSDSize_t size;
-} __attribute__((packed)) frskyOSDRect_t;
 
 typedef struct frskyOSDTriangle_s {
     frskyOSDPoint_t p1;
@@ -212,6 +217,21 @@ typedef struct frskyOSDDrawStrMaskCommandHeaderCmd_s {
     // uvarint with size and blob follow
 } __attribute__((packed)) frskyOSDDrawStrMaskCommandHeaderCmd_t;
 
+typedef struct frskyOSDDrawGridChrV2Cmd_s
+{
+    unsigned gx : 5;        // +5 = 5
+    unsigned gy : 4;        // +4 = 9
+    unsigned chr : 9;       // +9 = 18
+    unsigned opts : 3;      // +3 = 21, from osd_bitmap_opt_t
+    unsigned as_mask : 1;   // +1 = 22
+    unsigned color : 2;     // +2 = 24 = 3 bytes, only used when drawn as as_mask = 1
+} __attribute__((packed)) frskyOSDDrawGridChrV2Cmd_t;
+
+typedef struct frskyOSDError_s {
+    uint8_t command;
+    int8_t code;
+} frskyOSDError_t;
+
 
 typedef struct frskyOSDState_s {
     struct {
@@ -244,17 +264,27 @@ typedef struct frskyOSDState_s {
         osdCharacter_t *chr;
     } recvOsdCharacter;
     serialPort_t *port;
+    baudRate_e baudrate;
+    bool keepBaudrate;
     bool initialized;
+    frskyOSDError_t error;
     timeMs_t nextInfoRequest;
 } frskyOSDState_t;
 
 static frskyOSDState_t state;
 
+static bool frskyOSDDispatchResponse(void);
+
 static uint8_t frskyOSDChecksum(uint8_t crc, uint8_t c)
 {
     return crc8_dvb_s2(crc, c);
 }
 
+static bool frskyOSDSpeaksV2(void)
+{
+    return state.info.major >= 2 || (state.info.major == 1 && state.info.minor >= 99);
+}
+
 static void frskyOSDResetReceiveBuffer(void)
 {
     state.recvBuffer.state = RECV_STATE_NONE;
@@ -294,7 +324,7 @@ static void frskyOSDSendCommand(uint8_t cmd, const void *payload, size_t size)
     }
 }
 
-static void frskyOSDStateReset(serialPort_t *port)
+static void frskyOSDStateReset(serialPort_t *port, baudRate_e baudrate)
 {
     frskyOSDResetReceiveBuffer();
     frskyOSDResetSendBuffer();
@@ -304,6 +334,8 @@ static void frskyOSDStateReset(serialPort_t *port)
     state.info.viewport.height = 0;
 
     state.port = port;
+    state.baudrate = baudrate;
+    state.keepBaudrate = false;
     state.initialized = false;
 }
 
@@ -369,14 +401,55 @@ static bool frskyOSDIsResponseAvailable(void)
     return state.recvBuffer.state == RECV_STATE_DONE;
 }
 
+static void frskyOSDClearReceiveBuffer(void)
+{
+    frskyOSDUpdateReceiveBuffer();
+
+    if (frskyOSDIsResponseAvailable()) {
+        frskyOSDDispatchResponse();
+    } else if (state.recvBuffer.pos > 0) {
+        FRSKY_OSD_DEBUG("Discarding receive buffer with %u bytes", state.recvBuffer.pos);
+        frskyOSDResetReceiveBuffer();
+    }
+}
+
+static void frskyOSDSendAsyncCommand(uint8_t cmd, const void *data, size_t size)
+{
+    FRSKY_OSD_TRACE("Send async cmd %u", cmd);
+    frskyOSDSendCommand(cmd, data, size);
+}
+
+static bool frskyOSDSendSyncCommand(uint8_t cmd, const void *data, size_t size, timeMs_t timeout)
+{
+    FRSKY_OSD_TRACE("Send sync cmd %u", cmd);
+    frskyOSDClearReceiveBuffer();
+    frskyOSDSendCommand(cmd, data, size);
+    frskyOSDFlushSendBuffer();
+    timeMs_t end = millis() + timeout;
+    while (millis() < end) {
+        frskyOSDUpdateReceiveBuffer();
+        if (frskyOSDIsResponseAvailable() && frskyOSDDispatchResponse()) {
+            FRSKY_OSD_TRACE("Got sync response");
+            return true;
+        }
+    }
+    FRSKY_OSD_DEBUG("Sync response failed");
+    return false;
+}
+
 static bool frskyOSDHandleCommand(osdCommand_e cmd, const void *payload, size_t size)
 {
     const uint8_t *ptr = payload;
 
+    state.error.command = 0;
+    state.error.code = 0;
+
     switch (cmd) {
         case OSD_CMD_RESPONSE_ERROR:
         {
             if (size >= 2) {
+                state.error.command = *ptr;
+                state.error.code = *(ptr + 1);
                 FRSKY_OSD_ERROR("Received an error %02x in response to command %u", *(ptr + 1), *ptr);
                 return true;
             }
@@ -393,6 +466,21 @@ static bool frskyOSDHandleCommand(osdCommand_e cmd, const void *payload, size_t
                     resp->magic[0], resp->magic[1], resp->magic[2]);
                 return false;
             }
+            FRSKY_OSD_TRACE("received OSD_CMD_INFO at %u", (unsigned)baudRates[state.baudrate]);
+            if (!state.keepBaudrate) {
+                const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_FRSKY_OSD);
+                if (portConfig && portConfig->peripheral_baudrateIndex > FRSKY_OSD_DEFAULT_BAUDRATE_INDEX &&
+                    portConfig->peripheral_baudrateIndex != state.baudrate) {
+
+                    // Try switching baudrates
+                    uint32_t portBaudrate = baudRates[portConfig->peripheral_baudrateIndex];
+                    FRSKY_OSD_TRACE("requesting baudrate switch from %u to %u",
+                        (unsigned)baudRates[state.baudrate], (unsigned)portBaudrate);
+                    frskyOSDSendAsyncCommand(OSD_CMD_SET_DATA_RATE, &portBaudrate, sizeof(portBaudrate));
+                    frskyOSDFlushSendBuffer();
+                    return true;
+                }
+            }
             state.info.major = resp->versionMajor;
             state.info.minor = resp->versionMinor;
             state.info.grid.rows = resp->gridRows;
@@ -431,6 +519,39 @@ static bool frskyOSDHandleCommand(osdCommand_e cmd, const void *payload, size_t
             // We only wait for the confirmation, we're not interested in the data
             return true;
         }
+        case OSD_CMD_WIDGET_SET_CONFIG:
+        {
+            return true;
+        }
+        case OSD_CMD_SET_DATA_RATE:
+        {
+            if (size < sizeof(uint32_t)) {
+                break;
+            }
+            const uint32_t *newBaudrate = payload;
+            if (*newBaudrate && *newBaudrate != baudRates[state.baudrate]) {
+                FRSKY_OSD_TRACE("changed baudrate from %u to %u", (unsigned)baudRates[state.baudrate],
+                                (unsigned)*newBaudrate);
+                serialSetBaudRate(state.port, *newBaudrate);
+                // OSD might have returned a different baudrate from our
+                // predefined ones. Be ready to handle that
+                state.baudrate = 0;
+                for (baudRate_e ii = 0; ii <= BAUD_MAX; ii++) {
+                    if (baudRates[ii] == *newBaudrate) {
+                        state.baudrate = ii;
+                        break;
+                    }
+                }
+            } else {
+                FRSKY_OSD_TRACE("baudrate refused, returned %u", (unsigned)*newBaudrate);
+            }
+            // Make sure we request OSD_CMD_INFO again as soon
+            // as possible and don't try to change the baudrate
+            // anymore.
+            state.nextInfoRequest = 0;
+            state.keepBaudrate = true;
+            return true;
+        }
         default:
             break;
     }
@@ -458,42 +579,6 @@ static bool frskyOSDDispatchResponse(void)
     return ok;
 }
 
-static void frskyOSDClearReceiveBuffer(void)
-{
-    frskyOSDUpdateReceiveBuffer();
-
-    if (frskyOSDIsResponseAvailable()) {
-        frskyOSDDispatchResponse();
-    } else if (state.recvBuffer.pos > 0) {
-        FRSKY_OSD_DEBUG("Discarding receive buffer with %u bytes", state.recvBuffer.pos);
-        frskyOSDResetReceiveBuffer();
-    }
-}
-
-static void frskyOSDSendAsyncCommand(uint8_t cmd, const void *data, size_t size)
-{
-    FRSKY_OSD_TRACE("Send async cmd %u", cmd);
-    frskyOSDSendCommand(cmd, data, size);
-}
-
-static bool frskyOSDSendSyncCommand(uint8_t cmd, const void *data, size_t size, timeMs_t timeout)
-{
-    FRSKY_OSD_TRACE("Send sync cmd %u", cmd);
-    frskyOSDClearReceiveBuffer();
-    frskyOSDSendCommand(cmd, data, size);
-    frskyOSDFlushSendBuffer();
-    timeMs_t end = millis() + timeout;
-    while (millis() < end) {
-        frskyOSDUpdateReceiveBuffer();
-        if (frskyOSDIsResponseAvailable() && frskyOSDDispatchResponse()) {
-            FRSKY_OSD_DEBUG("Got sync response");
-            return true;
-        }
-    }
-    FRSKY_OSD_DEBUG("Sync response failed");
-    return false;
-}
-
 static bool frskyOSDShouldRequestInfo(void)
 {
     return !frskyOSDIsReady() || millis() > state.nextInfoRequest;
@@ -503,13 +588,52 @@ static void frskyOSDRequestInfo(void)
 {
     timeMs_t now = millis();
     if (state.info.nextRequest < now) {
+        timeMs_t interval;
+        if (frskyOSDIsReady()) {
+            // We already contacted the OSD, so we're just
+            // polling it to see if the video changed.
+            interval = FRSKY_OSD_INFO_READY_INTERVAL_MS;
+        } else {
+            // We haven't yet heard from the OSD. If this is not
+            // the first request, switch to the next baudrate.
+            if (state.info.nextRequest > 0 && !state.keepBaudrate) {
+                if (state.baudrate == BAUD_MAX) {
+                    state.baudrate = FRSKY_OSD_DEFAULT_BAUDRATE_INDEX;
+                } else {
+                    state.baudrate++;
+                }
+                serialSetBaudRate(state.port, baudRates[state.baudrate]);
+            }
+            interval = FRSKY_OSD_INFO_INTERVAL_MS;
+        }
+        state.info.nextRequest = now + interval;
+
         uint8_t version = FRSKY_OSD_SUPPORTED_API_VERSION;
+        FRSKY_OSD_TRACE("request OSD_CMD_INFO at %u", (unsigned)baudRates[state.baudrate]);
         frskyOSDSendAsyncCommand(OSD_CMD_INFO, &version, sizeof(version));
         frskyOSDFlushSendBuffer();
-        state.info.nextRequest = now + FRSKY_OSD_INFO_INTERVAL_MS;
     }
 }
 
+static bool frskyOSDOpenPort(baudRate_e baudrate)
+{
+    const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_FRSKY_OSD);
+    if (portConfig) {
+        FRSKY_OSD_TRACE("configured, trying to connect...");
+        portOptions_t portOptions = SERIAL_STOPBITS_1 | SERIAL_PARITY_NO;
+        serialPort_t *port = openSerialPort(portConfig->identifier,
+            FUNCTION_FRSKY_OSD, NULL, NULL, baudRates[baudrate],
+            MODE_RXTX, portOptions);
+
+        if (port) {
+            frskyOSDStateReset(port, baudrate);
+            frskyOSDRequestInfo();
+            return true;
+        }
+    }
+    return false;
+}
+
 static uint8_t frskyOSDEncodeAttr(textAttributes_t attr)
 {
     uint8_t frskyOSDAttr = 0;
@@ -525,23 +649,8 @@ static uint8_t frskyOSDEncodeAttr(textAttributes_t attr)
 bool frskyOSDInit(videoSystem_e videoSystem)
 {
     UNUSED(videoSystem);
-    // TODO: Use videoSystem to set the signal standard when
-    // no input is detected.
-    const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_FRSKY_OSD);
-    if (portConfig) {
-        FRSKY_OSD_TRACE("configured, trying to connect...");
-        portOptions_t portOptions = 0;
-        serialPort_t *port = openSerialPort(portConfig->identifier,
-            FUNCTION_FRSKY_OSD, NULL, NULL, FRSKY_OSD_BAUDRATE,
-            MODE_RXTX, portOptions);
 
-        if (port) {
+    return frskyOSDOpenPort(FRSKY_OSD_DEFAULT_BAUDRATE_INDEX);
-            frskyOSDStateReset(port);
-            frskyOSDRequestInfo();
-            return true;
-        }
-    }
-    return false;
 }
 
 bool frskyOSDIsReady(void)
@@ -675,7 +784,33 @@ unsigned frskyOSDGetPixelHeight(void)
     return state.info.viewport.height;
 }
 
-static void frskyOSDSendCharInGrid(unsigned x, unsigned y, uint16_t chr, textAttributes_t attr)
+static void frskyOSDSendAsyncBlobCommand(uint8_t cmd, const void *header, size_t headerSize, const void *blob, size_t blobSize, bool explicitBlobSize)
+{
+    uint8_t payload[128];
+
+    memcpy(payload, header, headerSize);
+
+    int uvarintSize;
+    if (explicitBlobSize) {
+        uvarintSize = uvarintEncode(blobSize, &payload[headerSize], sizeof(payload) - headerSize);
+    } else {
+        uvarintSize = 0;
+    }
+    memcpy(&payload[headerSize + uvarintSize], blob, blobSize);
+    frskyOSDSendAsyncCommand(cmd, payload,  headerSize + uvarintSize + blobSize);
+}
+
+static void frskyOSDSendAsyncBlobWithExplicitSizeCommand(uint8_t cmd, const void *header, size_t headerSize, const void *blob, size_t blobSize)
+{
+    frskyOSDSendAsyncBlobCommand(cmd, header, headerSize, blob, blobSize, true);
+}
+
+static void frskyOSDSendAsyncBlobWithoutExplicitSizeCommand(uint8_t cmd, const void *header, size_t headerSize, const void *blob, size_t blobSize)
+{
+    frskyOSDSendAsyncBlobCommand(cmd, header, headerSize, blob, blobSize, false);
+}
+
+static void frskyOSDSendCharInGrid_V1(unsigned x, unsigned y, uint16_t chr, textAttributes_t attr)
 {
     uint8_t payload[] = {
         x,
@@ -687,15 +822,59 @@ static void frskyOSDSendCharInGrid(unsigned x, unsigned y, uint16_t chr, textAtt
     frskyOSDSendAsyncCommand(OSD_CMD_DRAW_GRID_CHR, payload, sizeof(payload));
 }
 
-static void frskyOSDSendAsyncBlobCommand(uint8_t cmd, const void *header, size_t headerSize, const void *blob, size_t blobSize)
+static void frskyOSDSendCharInGrid_V2(unsigned x, unsigned y, uint16_t chr, textAttributes_t attr)
 {
-    uint8_t payload[128];
+    frskyOSDDrawGridChrV2Cmd_t payload = {
+        .gx = x,
+        .gy = y,
+        .chr = chr,
+        .opts = frskyOSDEncodeAttr(attr),
+    };
+    frskyOSDSendAsyncCommand(OSD_CMD_DRAW_GRID_CHR_2, &payload, sizeof(payload));
+}
 
-    memcpy(payload, header, headerSize);
+static void frskyOSDSendCharInGrid(unsigned x, unsigned y, uint16_t chr, textAttributes_t attr)
+{
+    if (frskyOSDSpeaksV2()) {
+        frskyOSDSendCharInGrid_V2(x, y, chr, attr);
+    } else {
+        frskyOSDSendCharInGrid_V1(x, y, chr, attr);
+    }
+}
 
-    int uvarintSize = uvarintEncode(blobSize, &payload[headerSize], sizeof(payload) - headerSize);
+static void frskyOSDSendCharSendStringInGrid_V1(unsigned x, unsigned y, const char *buff, textAttributes_t attr)
-    memcpy(&payload[headerSize + uvarintSize], blob, blobSize);
+{
-    frskyOSDSendAsyncCommand(cmd, payload,  headerSize + uvarintSize + blobSize);
+    frskyOSDDrawGridStrHeaderCmd_t cmd = {
+        .gx = x,
+        .gy = y,
+        .opts = frskyOSDEncodeAttr(attr),
+    };
+    frskyOSDSendAsyncBlobWithExplicitSizeCommand(OSD_CMD_DRAW_GRID_STR, &cmd, sizeof(cmd), buff, strlen(buff) + 1);
+}
+
+static void frskyOSDSendCharSendStringInGrid_V2(unsigned x, unsigned y, const char *buff, textAttributes_t attr)
+{
+    frskyOSDDrawGridStrV2HeaderCmd_t cmd = {
+        .gx = x,
+        .gy = y,
+        .opts = frskyOSDEncodeAttr(attr),
+    };
+    size_t len = strlen(buff);
+    if (len <= 15) {
+        cmd.size = len;
+        frskyOSDSendAsyncBlobWithoutExplicitSizeCommand(OSD_CMD_DRAW_GRID_STR_2, &cmd, sizeof(cmd), buff, len);
+    } else {
+        frskyOSDSendAsyncBlobWithExplicitSizeCommand(OSD_CMD_DRAW_GRID_STR_2, &cmd, sizeof(cmd), buff, len);
+    }
+}
+
+static void frskyOSDSendCharSendStringInGrid(unsigned x, unsigned y, const char *buff, textAttributes_t attr)
+{
+    if (frskyOSDSpeaksV2()) {
+        frskyOSDSendCharSendStringInGrid_V2(x, y, buff, attr);
+    } else {
+        frskyOSDSendCharSendStringInGrid_V1(x, y, buff, attr);
+    }
 }
 
 void frskyOSDDrawStringInGrid(unsigned x, unsigned y, const char *buff, textAttributes_t attr)
@@ -726,12 +905,7 @@ void frskyOSDDrawStringInGrid(unsigned x, unsigned y, const char *buff, textAttr
         return;
     }
 
-    frskyOSDDrawGridStrHeaderCmd_t cmd;
+    frskyOSDSendCharSendStringInGrid(x, y, buff, attr);
-    cmd.gx = x;
-    cmd.gy = y;
-    cmd.opts = frskyOSDEncodeAttr(attr);
-
-    frskyOSDSendAsyncBlobCommand(OSD_CMD_DRAW_GRID_STR, &cmd, sizeof(cmd), buff, strlen(buff) + 1);
 }
 
 void frskyOSDDrawCharInGrid(unsigned x, unsigned y, uint16_t chr, textAttributes_t attr)
@@ -859,7 +1033,7 @@ void frskyOSDDrawString(int x, int y, const char *s, uint8_t opts)
     cmd.p.y = y;
     cmd.opts = opts;
 
-    frskyOSDSendAsyncBlobCommand(OSD_CMD_DRAWING_DRAW_STRING, &cmd, sizeof(cmd), s, strlen(s) + 1);
+    frskyOSDSendAsyncBlobWithExplicitSizeCommand(OSD_CMD_DRAWING_DRAW_STRING, &cmd, sizeof(cmd), s, strlen(s) + 1);
 }
 
 void frskyOSDDrawStringMask(int x, int y, const char *s, frskyOSDColor_e color, uint8_t opts)
@@ -870,7 +1044,7 @@ void frskyOSDDrawStringMask(int x, int y, const char *s, frskyOSDColor_e color,
     cmd.opts = opts;
     cmd.maskColor = color;
 
-    frskyOSDSendAsyncBlobCommand(OSD_CMD_DRAWING_DRAW_STRING_MASK, &cmd, sizeof(cmd), s, strlen(s) + 1);
+    frskyOSDSendAsyncBlobWithExplicitSizeCommand(OSD_CMD_DRAWING_DRAW_STRING_MASK, &cmd, sizeof(cmd), s, strlen(s) + 1);
 }
 
 void frskyOSDMoveToPoint(int x, int y)
@@ -987,5 +1161,45 @@ void frskyOSDContextPop(void)
     frskyOSDSendAsyncCommand(OSD_CMD_CONTEXT_POP, NULL, 0);
 }
 
+bool frskyOSDSupportsWidgets(void)
+{
+    return frskyOSDSpeaksV2();
+}
+
+bool frskyOSDSetWidgetConfig(frskyOSDWidgetID_e widget, const void *config, size_t configSize)
+{
+    if (!frskyOSDSupportsWidgets()) {
+        return false;
+    }
+
+    uint8_t buffer[configSize + 1];
+    buffer[0] = widget;
+    memcpy(buffer + 1, config, configSize);
+    bool ok = frskyOSDSendSyncCommand(OSD_CMD_WIDGET_SET_CONFIG, buffer, sizeof(buffer), 500);
+    return ok && state.error.code == 0;
+}
+
+bool frskyOSDDrawWidget(frskyOSDWidgetID_e widget, const void *data, size_t dataSize)
+{
+    if (!frskyOSDSupportsWidgets()) {
+        return false;
+    }
+
+    uint8_t buffer[dataSize + 1];
+    buffer[0] = widget;
+    memcpy(buffer + 1, data, dataSize);
+    frskyOSDSendAsyncCommand(OSD_CMD_WIDGET_DRAW, buffer, sizeof(buffer));
+    return true;
+}
+
+uint32_t frskyOSDQuantize(float val, float min, float max, int bits)
+{
+    if (val < min) {
+        val = max - (min - val);
+    } else if (val > max) {
+        val = min + (val - max);
+    }
+    return (val * (1 << bits)) / max;
+}
 
 #endif

src/main/io/frsky_osd.h

@@ -26,6 +26,77 @@ typedef enum {
     FRSKY_OSD_OUTLINE_TYPE_LEFT = 1 << 3,
 } frskyOSDLineOutlineType_e;
 
+typedef enum
+{
+    FRSKY_OSD_WIDGET_ID_AHI = 0,
+
+    FRSKY_OSD_WIDGET_ID_SIDEBAR_0 = 1,
+    FRSKY_OSD_WIDGET_ID_SIDEBAR_1 = 2,
+
+    FRSKY_OSD_WIDGET_ID_GRAPH_0 = 3,
+    FRSKY_OSD_WIDGET_ID_GRAPH_1 = 4,
+    FRSKY_OSD_WIDGET_ID_GRAPH_2 = 5,
+    FRSKY_OSD_WIDGET_ID_GRAPH_3 = 6,
+
+    FRSKY_OSD_WIDGET_ID_CHARGAUGE_0 = 7,
+    FRSKY_OSD_WIDGET_ID_CHARGAUGE_1 = 8,
+    FRSKY_OSD_WIDGET_ID_CHARGAUGE_2 = 9,
+    FRSKY_OSD_WIDGET_ID_CHARGAUGE_3 = 10,
+
+    FRSKY_OSD_WIDGET_ID_SIDEBAR_FIRST = FRSKY_OSD_WIDGET_ID_SIDEBAR_0,
+    FRSKY_OSD_WIDGET_ID_SIDEBAR_LAST = FRSKY_OSD_WIDGET_ID_SIDEBAR_1,
+
+    FRSKY_OSD_WIDGET_ID_GRAPH_FIRST = FRSKY_OSD_WIDGET_ID_GRAPH_0,
+    FRSKY_OSD_WIDGET_ID_GRAPH_LAST = FRSKY_OSD_WIDGET_ID_GRAPH_3,
+
+    FRSKY_OSD_WIDGET_ID_CHARGAUGE_FIRST = FRSKY_OSD_WIDGET_ID_CHARGAUGE_0,
+    FRSKY_OSD_WIDGET_ID_CHARGAUGE_LAST = FRSKY_OSD_WIDGET_ID_CHARGAUGE_3,
+} frskyOSDWidgetID_e;
+
+typedef struct frskyOSDPoint_s {
+    int x : 12;
+    int y : 12;
+} __attribute__((packed)) frskyOSDPoint_t;
+
+typedef struct frskyOSDSize_s {
+    int w : 12;
+    int h : 12;
+} __attribute__((packed)) frskyOSDSize_t;
+
+typedef struct frskyOSDRect_s {
+    frskyOSDPoint_t origin;
+    frskyOSDSize_t size;
+} __attribute__((packed)) frskyOSDRect_t;
+
+typedef struct frskyOSDWidgetAHIData_s
+{
+    uint16_t pitch : 12;
+    uint16_t roll : 12;
+} __attribute__((packed)) frskyOSDWidgetAHIData_t;
+
+typedef struct frskyOSDWidgetAHIConfig_s
+{
+    frskyOSDRect_t rect;
+    uint8_t style;
+    uint8_t options;
+    uint8_t crosshairMargin;
+    uint8_t strokeWidth;
+} __attribute__((packed)) frskyOSDWidgetAHIConfig_t;
+
+typedef struct frskyOSDWidgetSidebarData_s
+{
+    int32_t value : 24;
+} __attribute__((packed)) frskyOSDWidgetSidebarData_t;
+
+typedef struct frskyOSDWidgetSidebarConfig_s
+{
+    frskyOSDRect_t rect;
+    uint8_t options;
+    uint8_t divisions;
+    uint16_t counts_per_step;
+    osdUnit_t unit;
+} __attribute__((packed)) frskyOSDWidgetSidebarConfig_t;
+
 bool frskyOSDInit(videoSystem_e videoSystem);
 bool frskyOSDIsReady(void);
 void frskyOSDUpdate(void);
@@ -84,3 +155,9 @@ void frskyOSDCtmRotate(float r);
 
 void frskyOSDContextPush(void);
 void frskyOSDContextPop(void);
+
+bool frskyOSDSupportsWidgets(void);
+bool frskyOSDSetWidgetConfig(frskyOSDWidgetID_e widget, const void *config, size_t configSize);
+bool frskyOSDDrawWidget(frskyOSDWidgetID_e widget, const void *data, size_t dataSize);
+
+uint32_t frskyOSDQuantize(float val, float min, float max, int bits);

src/main/io/osd.c

@@ -160,19 +160,6 @@ typedef struct statistic_s {
 
 static statistic_t stats;
 
-typedef enum {
-    OSD_SIDEBAR_ARROW_NONE,
-    OSD_SIDEBAR_ARROW_UP,
-    OSD_SIDEBAR_ARROW_DOWN,
-} osd_sidebar_arrow_e;
-
-typedef struct osd_sidebar_s {
-    int32_t offset;
-    timeMs_t updated;
-    osd_sidebar_arrow_e arrow;
-    uint8_t idle;
-} osd_sidebar_t;
-
 static timeUs_t resumeRefreshAt = 0;
 static bool refreshWaitForResumeCmdRelease;
 
@@ -197,10 +184,9 @@ static bool osdDisplayHasCanvas;
 #endif
 
 #define AH_MAX_PITCH_DEFAULT 20 // Specify default maximum AHI pitch value displayed (degrees)
-#define AH_SIDEBAR_WIDTH_POS 7
-#define AH_SIDEBAR_HEIGHT_POS 3
 
-PG_REGISTER_WITH_RESET_FN(osdConfig_t, osdConfig, PG_OSD_CONFIG, 12);
+PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 13);
+PG_REGISTER_WITH_RESET_FN(osdLayoutsConfig_t, osdLayoutsConfig, PG_OSD_LAYOUTS_CONFIG, 0);
 
 static int digitCount(int32_t value)
 {
@@ -926,70 +912,6 @@ static inline int32_t osdGetAltitudeMsl(void)
 #endif
 }
 
-static uint8_t osdUpdateSidebar(osd_sidebar_scroll_e scroll, osd_sidebar_t *sidebar, timeMs_t currentTimeMs)
-{
-    // Scroll between SYM_AH_DECORATION_MIN and SYM_AH_DECORATION_MAX.
-    // Zero scrolling should draw SYM_AH_DECORATION.
-    uint8_t decoration = SYM_AH_DECORATION;
-    int offset;
-    int steps;
-    switch (scroll) {
-        case OSD_SIDEBAR_SCROLL_NONE:
-            sidebar->arrow = OSD_SIDEBAR_ARROW_NONE;
-            sidebar->offset = 0;
-            return decoration;
-        case OSD_SIDEBAR_SCROLL_ALTITUDE:
-            // Move 1 char for every 20cm
-            offset = osdGetAltitude();
-            steps = offset / 20;
-            break;
-        case OSD_SIDEBAR_SCROLL_GROUND_SPEED:
-#if defined(USE_GPS)
-            offset = gpsSol.groundSpeed;
-#else
-            offset = 0;
-#endif
-            // Move 1 char for every 20 cm/s
-            steps = offset / 20;
-            break;
-        case OSD_SIDEBAR_SCROLL_HOME_DISTANCE:
-#if defined(USE_GPS)
-            offset = GPS_distanceToHome;
-#else
-            offset = 0;
-#endif
-            // Move 1 char for every 5m
-            steps = offset / 5;
-            break;
-    }
-    if (offset) {
-        decoration -= steps % SYM_AH_DECORATION_COUNT;
-        if (decoration > SYM_AH_DECORATION_MAX) {
-            decoration -= SYM_AH_DECORATION_COUNT;
-        } else if (decoration < SYM_AH_DECORATION_MIN) {
-            decoration += SYM_AH_DECORATION_COUNT;
-        }
-    }
-    if (currentTimeMs - sidebar->updated > 100) {
-        if (offset > sidebar->offset) {
-            sidebar->arrow = OSD_SIDEBAR_ARROW_UP;
-            sidebar->idle = 0;
-        } else if (offset < sidebar->offset) {
-            sidebar->arrow = OSD_SIDEBAR_ARROW_DOWN;
-            sidebar->idle = 0;
-        } else {
-            if (sidebar->idle > 3) {
-                sidebar->arrow = OSD_SIDEBAR_ARROW_NONE;
-            } else {
-                sidebar->idle++;
-            }
-        }
-        sidebar->offset = offset;
-        sidebar->updated = currentTimeMs;
-    }
-    return decoration;
-}
-
 static bool osdIsHeadingValid(void)
 {
     return isImuHeadingValid();
@@ -1265,7 +1187,7 @@ static void osdDisplayAdjustableDecimalValue(uint8_t elemPosX, uint8_t elemPosY,
 
 static bool osdDrawSingleElement(uint8_t item)
 {
-    uint16_t pos = osdConfig()->item_pos[currentLayout][item];
+    uint16_t pos = osdLayoutsConfig()->item_pos[currentLayout][item];
     if (!OSD_VISIBLE(pos)) {
         return false;
     }
@@ -1389,7 +1311,7 @@ static bool osdDrawSingleElement(uint8_t item)
                 else
                 {
                     int homeDirection = GPS_directionToHome - DECIDEGREES_TO_DEGREES(osdGetHeading());
-                    osdDrawDirArrow(osdDisplayPort, osdGetDisplayPortCanvas(), OSD_DRAW_POINT_GRID(elemPosX, elemPosY), homeDirection, true);
+                    osdDrawDirArrow(osdDisplayPort, osdGetDisplayPortCanvas(), OSD_DRAW_POINT_GRID(elemPosX, elemPosY), homeDirection);
                 }
             } else {
                 // No home or no fix or unknown heading, blink.
@@ -1707,7 +1629,7 @@ static bool osdDrawSingleElement(uint8_t item)
     case OSD_CROSSHAIRS: // Hud is a sub-element of the crosshair
 
         osdCrosshairPosition(&elemPosX, &elemPosY);
-        osdHudDrawCrosshair(elemPosX, elemPosY);
+        osdHudDrawCrosshair(osdGetDisplayPortCanvas(), elemPosX, elemPosY);
 
         if (osdConfig()->hud_homing && STATE(GPS_FIX) && STATE(GPS_FIX_HOME) && isImuHeadingValid()) {
             osdHudDrawHoming(elemPosX, elemPosY);
@@ -1812,43 +1734,7 @@ static bool osdDrawSingleElement(uint8_t item)
 
     case OSD_HORIZON_SIDEBARS:
         {
-            osdCrosshairPosition(&elemPosX, &elemPosY);
+            osdDrawSidebars(osdDisplayPort, osdGetDisplayPortCanvas());
-
-            static osd_sidebar_t left;
-            static osd_sidebar_t right;
-
-            timeMs_t currentTimeMs = millis();
-            uint8_t leftDecoration = osdUpdateSidebar(osdConfig()->left_sidebar_scroll, &left, currentTimeMs);
-            uint8_t rightDecoration = osdUpdateSidebar(osdConfig()->right_sidebar_scroll, &right, currentTimeMs);
-
-            const int8_t hudwidth = AH_SIDEBAR_WIDTH_POS;
-            const int8_t hudheight = AH_SIDEBAR_HEIGHT_POS;
-
-            // Arrows
-            if (osdConfig()->sidebar_scroll_arrows) {
-                displayWriteChar(osdDisplayPort, elemPosX - hudwidth, elemPosY - hudheight - 1,
-                    left.arrow == OSD_SIDEBAR_ARROW_UP ? SYM_AH_DECORATION_UP : SYM_BLANK);
-
-                displayWriteChar(osdDisplayPort, elemPosX + hudwidth, elemPosY - hudheight - 1,
-                    right.arrow == OSD_SIDEBAR_ARROW_UP ? SYM_AH_DECORATION_UP : SYM_BLANK);
-
-                displayWriteChar(osdDisplayPort, elemPosX - hudwidth, elemPosY + hudheight + 1,
-                    left.arrow == OSD_SIDEBAR_ARROW_DOWN ? SYM_AH_DECORATION_DOWN : SYM_BLANK);
-
-                displayWriteChar(osdDisplayPort, elemPosX + hudwidth, elemPosY + hudheight + 1,
-                    right.arrow == OSD_SIDEBAR_ARROW_DOWN ? SYM_AH_DECORATION_DOWN : SYM_BLANK);
-            }
-
-            // Draw AH sides
-            for (int y = -hudheight; y <= hudheight; y++) {
-                displayWriteChar(osdDisplayPort, elemPosX - hudwidth, elemPosY + y, leftDecoration);
-                displayWriteChar(osdDisplayPort, elemPosX + hudwidth, elemPosY + y, rightDecoration);
-            }
-
-            // AH level indicators
-            displayWriteChar(osdDisplayPort, elemPosX - hudwidth + 1, elemPosY, SYM_AH_RIGHT);
-            displayWriteChar(osdDisplayPort, elemPosX + hudwidth - 1, elemPosY, SYM_AH_LEFT);
-
             return true;
         }
 
@@ -2604,197 +2490,204 @@ void osdDrawNextElement(void)
     osdDrawSingleElement(OSD_ARTIFICIAL_HORIZON);
 }
 
-void pgResetFn_osdConfig(osdConfig_t *osdConfig)
+PG_RESET_TEMPLATE(osdConfig_t, osdConfig,
-{
+    .rssi_alarm = 20,
-    osdConfig->item_pos[0][OSD_ALTITUDE] = OSD_POS(1, 0) | OSD_VISIBLE_FLAG;
+    .time_alarm = 10,
-    osdConfig->item_pos[0][OSD_MAIN_BATT_VOLTAGE] = OSD_POS(12, 0) | OSD_VISIBLE_FLAG;
+    .alt_alarm = 100,
-    osdConfig->item_pos[0][OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE] = OSD_POS(12, 1);
+    .dist_alarm = 1000,
-
+    .neg_alt_alarm = 5,
-    osdConfig->item_pos[0][OSD_RSSI_VALUE] = OSD_POS(23, 0) | OSD_VISIBLE_FLAG;
+    .current_alarm = 0,
-    //line 2
+    .imu_temp_alarm_min = -200,
-    osdConfig->item_pos[0][OSD_HOME_DIST] = OSD_POS(1, 1);
+    .imu_temp_alarm_max = 600,
-    osdConfig->item_pos[0][OSD_TRIP_DIST] = OSD_POS(1, 2);
+    .esc_temp_alarm_min = -200,
-    osdConfig->item_pos[0][OSD_MAIN_BATT_CELL_VOLTAGE] = OSD_POS(12, 1);
+    .esc_temp_alarm_max = 900,
-    osdConfig->item_pos[0][OSD_MAIN_BATT_SAG_COMPENSATED_CELL_VOLTAGE] = OSD_POS(12, 1);
+    .gforce_alarm = 5,
-    osdConfig->item_pos[0][OSD_GPS_SPEED] = OSD_POS(23, 1);
+    .gforce_axis_alarm_min = -5,
-    osdConfig->item_pos[0][OSD_3D_SPEED] = OSD_POS(23, 1);
+    .gforce_axis_alarm_max = 5,
-
-    osdConfig->item_pos[0][OSD_THROTTLE_POS] = OSD_POS(1, 2) | OSD_VISIBLE_FLAG;
-    osdConfig->item_pos[0][OSD_THROTTLE_POS_AUTO_THR] = OSD_POS(6, 2);
-    osdConfig->item_pos[0][OSD_HEADING] = OSD_POS(12, 2);
-    osdConfig->item_pos[0][OSD_CRUISE_HEADING_ERROR] = OSD_POS(12, 2);
-    osdConfig->item_pos[0][OSD_CRUISE_HEADING_ADJUSTMENT] = OSD_POS(12, 2);
-    osdConfig->item_pos[0][OSD_HEADING_GRAPH] = OSD_POS(18, 2);
-    osdConfig->item_pos[0][OSD_CURRENT_DRAW] = OSD_POS(2, 3) | OSD_VISIBLE_FLAG;
-    osdConfig->item_pos[0][OSD_MAH_DRAWN] = OSD_POS(1, 4) | OSD_VISIBLE_FLAG;
-    osdConfig->item_pos[0][OSD_WH_DRAWN] = OSD_POS(1, 5);
-    osdConfig->item_pos[0][OSD_BATTERY_REMAINING_CAPACITY] = OSD_POS(1, 6);
-    osdConfig->item_pos[0][OSD_BATTERY_REMAINING_PERCENT] = OSD_POS(1, 7);
-    osdConfig->item_pos[0][OSD_POWER_SUPPLY_IMPEDANCE] = OSD_POS(1, 8);
-
-    osdConfig->item_pos[0][OSD_EFFICIENCY_MAH_PER_KM] = OSD_POS(1, 5);
-    osdConfig->item_pos[0][OSD_EFFICIENCY_WH_PER_KM] = OSD_POS(1, 5);
-
-    osdConfig->item_pos[0][OSD_ATTITUDE_ROLL] = OSD_POS(1, 7);
-    osdConfig->item_pos[0][OSD_ATTITUDE_PITCH] = OSD_POS(1, 8);
-
-    // avoid OSD_VARIO under OSD_CROSSHAIRS
-    osdConfig->item_pos[0][OSD_VARIO] = OSD_POS(23, 5);
-    // OSD_VARIO_NUM at the right of OSD_VARIO
-    osdConfig->item_pos[0][OSD_VARIO_NUM] = OSD_POS(24, 7);
-    osdConfig->item_pos[0][OSD_HOME_DIR] = OSD_POS(14, 11);
-    osdConfig->item_pos[0][OSD_ARTIFICIAL_HORIZON] = OSD_POS(8, 6) | OSD_VISIBLE_FLAG;
-    osdConfig->item_pos[0][OSD_HORIZON_SIDEBARS] = OSD_POS(8, 6) | OSD_VISIBLE_FLAG;
-
-    osdConfig->item_pos[0][OSD_CRAFT_NAME] = OSD_POS(20, 2);
-    osdConfig->item_pos[0][OSD_VTX_CHANNEL] = OSD_POS(8, 6);
-
-    osdConfig->item_pos[0][OSD_ONTIME] = OSD_POS(23, 8);
-    osdConfig->item_pos[0][OSD_FLYTIME] = OSD_POS(23, 9);
-    osdConfig->item_pos[0][OSD_ONTIME_FLYTIME] = OSD_POS(23, 11) | OSD_VISIBLE_FLAG;
-    osdConfig->item_pos[0][OSD_RTC_TIME] = OSD_POS(23, 12);
-    osdConfig->item_pos[0][OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH] = OSD_POS(23, 7);
-    osdConfig->item_pos[0][OSD_REMAINING_DISTANCE_BEFORE_RTH] = OSD_POS(23, 6);
-
-    osdConfig->item_pos[0][OSD_GPS_SATS] = OSD_POS(0, 11) | OSD_VISIBLE_FLAG;
-    osdConfig->item_pos[0][OSD_GPS_HDOP] = OSD_POS(0, 10);
-
-    osdConfig->item_pos[0][OSD_GPS_LAT] = OSD_POS(0, 12);
-    // Put this on top of the latitude, since it's very unlikely
-    // that users will want to use both at the same time.
-    osdConfig->item_pos[0][OSD_PLUS_CODE] = OSD_POS(0, 12);
-    osdConfig->item_pos[0][OSD_FLYMODE] = OSD_POS(13, 12) | OSD_VISIBLE_FLAG;
-    osdConfig->item_pos[0][OSD_GPS_LON] = OSD_POS(18, 12);
-
-    osdConfig->item_pos[0][OSD_AZIMUTH] = OSD_POS(2, 12);
-
-    osdConfig->item_pos[0][OSD_ROLL_PIDS] = OSD_POS(2, 10);
-    osdConfig->item_pos[0][OSD_PITCH_PIDS] = OSD_POS(2, 11);
-    osdConfig->item_pos[0][OSD_YAW_PIDS] = OSD_POS(2, 12);
-    osdConfig->item_pos[0][OSD_LEVEL_PIDS] = OSD_POS(2, 12);
-    osdConfig->item_pos[0][OSD_POS_XY_PIDS] = OSD_POS(2, 12);
-    osdConfig->item_pos[0][OSD_POS_Z_PIDS] = OSD_POS(2, 12);
-    osdConfig->item_pos[0][OSD_VEL_XY_PIDS] = OSD_POS(2, 12);
-    osdConfig->item_pos[0][OSD_VEL_Z_PIDS] = OSD_POS(2, 12);
-    osdConfig->item_pos[0][OSD_HEADING_P] = OSD_POS(2, 12);
-    osdConfig->item_pos[0][OSD_BOARD_ALIGN_ROLL] = OSD_POS(2, 10);
-    osdConfig->item_pos[0][OSD_BOARD_ALIGN_PITCH] = OSD_POS(2, 11);
-    osdConfig->item_pos[0][OSD_RC_EXPO] = OSD_POS(2, 12);
-    osdConfig->item_pos[0][OSD_RC_YAW_EXPO] = OSD_POS(2, 12);
-    osdConfig->item_pos[0][OSD_THROTTLE_EXPO] = OSD_POS(2, 12);
-    osdConfig->item_pos[0][OSD_PITCH_RATE] = OSD_POS(2, 12);
-    osdConfig->item_pos[0][OSD_ROLL_RATE] = OSD_POS(2, 12);
-    osdConfig->item_pos[0][OSD_YAW_RATE] = OSD_POS(2, 12);
-    osdConfig->item_pos[0][OSD_MANUAL_RC_EXPO] = OSD_POS(2, 12);
-    osdConfig->item_pos[0][OSD_MANUAL_RC_YAW_EXPO] = OSD_POS(2, 12);
-    osdConfig->item_pos[0][OSD_MANUAL_PITCH_RATE] = OSD_POS(2, 12);
-    osdConfig->item_pos[0][OSD_MANUAL_ROLL_RATE] = OSD_POS(2, 12);
-    osdConfig->item_pos[0][OSD_MANUAL_YAW_RATE] = OSD_POS(2, 12);
-    osdConfig->item_pos[0][OSD_NAV_FW_CRUISE_THR] = OSD_POS(2, 12);
-    osdConfig->item_pos[0][OSD_NAV_FW_PITCH2THR] = OSD_POS(2, 12);
-    osdConfig->item_pos[0][OSD_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE] = OSD_POS(2, 12);
-    osdConfig->item_pos[0][OSD_FW_ALT_PID_OUTPUTS] = OSD_POS(2, 12);
-    osdConfig->item_pos[0][OSD_FW_POS_PID_OUTPUTS] = OSD_POS(2, 12);
-    osdConfig->item_pos[0][OSD_MC_VEL_X_PID_OUTPUTS] = OSD_POS(2, 12);
-    osdConfig->item_pos[0][OSD_MC_VEL_Y_PID_OUTPUTS] = OSD_POS(2, 12);
-    osdConfig->item_pos[0][OSD_MC_VEL_Z_PID_OUTPUTS] = OSD_POS(2, 12);
-    osdConfig->item_pos[0][OSD_MC_POS_XYZ_P_OUTPUTS] = OSD_POS(2, 12);
-
-    osdConfig->item_pos[0][OSD_POWER] = OSD_POS(15, 1);
-
-    osdConfig->item_pos[0][OSD_IMU_TEMPERATURE] = OSD_POS(19, 2);
-    osdConfig->item_pos[0][OSD_BARO_TEMPERATURE] = OSD_POS(19, 3);
-    osdConfig->item_pos[0][OSD_TEMP_SENSOR_0_TEMPERATURE] = OSD_POS(19, 4);
-    osdConfig->item_pos[0][OSD_TEMP_SENSOR_1_TEMPERATURE] = OSD_POS(19, 5);
-    osdConfig->item_pos[0][OSD_TEMP_SENSOR_2_TEMPERATURE] = OSD_POS(19, 6);
-    osdConfig->item_pos[0][OSD_TEMP_SENSOR_3_TEMPERATURE] = OSD_POS(19, 7);
-    osdConfig->item_pos[0][OSD_TEMP_SENSOR_4_TEMPERATURE] = OSD_POS(19, 8);
-    osdConfig->item_pos[0][OSD_TEMP_SENSOR_5_TEMPERATURE] = OSD_POS(19, 9);
-    osdConfig->item_pos[0][OSD_TEMP_SENSOR_6_TEMPERATURE] = OSD_POS(19, 10);
-    osdConfig->item_pos[0][OSD_TEMP_SENSOR_7_TEMPERATURE] = OSD_POS(19, 11);
-
-    osdConfig->item_pos[0][OSD_AIR_SPEED] = OSD_POS(3, 5);
-    osdConfig->item_pos[0][OSD_WIND_SPEED_HORIZONTAL] = OSD_POS(3, 6);
-    osdConfig->item_pos[0][OSD_WIND_SPEED_VERTICAL] = OSD_POS(3, 7);
-
-    osdConfig->item_pos[0][OSD_GFORCE] = OSD_POS(12, 4);
-    osdConfig->item_pos[0][OSD_GFORCE_X] = OSD_POS(12, 5);
-    osdConfig->item_pos[0][OSD_GFORCE_Y] = OSD_POS(12, 6);
-    osdConfig->item_pos[0][OSD_GFORCE_Z] = OSD_POS(12, 7);
-
-    osdConfig->item_pos[0][OSD_VTX_POWER] = OSD_POS(3, 5);
-
-#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)
-    osdConfig->item_pos[0][OSD_RC_SOURCE] = OSD_POS(3, 4);
-#endif
-
-    // Under OSD_FLYMODE. TODO: Might not be visible on NTSC?
-    osdConfig->item_pos[0][OSD_MESSAGES] = OSD_POS(1, 13) | OSD_VISIBLE_FLAG;
-
-    for (unsigned ii = 1; ii < OSD_LAYOUT_COUNT; ii++) {
-        for (unsigned jj = 0; jj < ARRAYLEN(osdConfig->item_pos[0]); jj++) {
-             osdConfig->item_pos[ii][jj] = osdConfig->item_pos[0][jj] & ~OSD_VISIBLE_FLAG;
-        }
-    }
-
-    osdConfig->rssi_alarm = 20;
-    osdConfig->time_alarm = 10;
-    osdConfig->alt_alarm = 100;
-    osdConfig->dist_alarm = 1000;
-    osdConfig->neg_alt_alarm = 5;
-    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;
 #ifdef USE_BARO
-    osdConfig->baro_temp_alarm_min = -200;
+    .baro_temp_alarm_min = -200,
-    osdConfig->baro_temp_alarm_max = 600;
+    .baro_temp_alarm_max = 600,
 #endif
 
 #ifdef USE_TEMPERATURE_SENSOR
-    osdConfig->temp_label_align = OSD_ALIGN_LEFT;
+    .temp_label_align = OSD_ALIGN_LEFT,
 #endif
 
-    osdConfig->video_system = VIDEO_SYSTEM_AUTO;
+    .video_system = VIDEO_SYSTEM_AUTO,
 
-    osdConfig->ahi_reverse_roll = 0;
+    .ahi_reverse_roll = 0,
-    osdConfig->ahi_max_pitch = AH_MAX_PITCH_DEFAULT;
+    .ahi_max_pitch = AH_MAX_PITCH_DEFAULT,
-    osdConfig->crosshairs_style = OSD_CROSSHAIRS_STYLE_DEFAULT;
+    .crosshairs_style = OSD_CROSSHAIRS_STYLE_DEFAULT,
-    osdConfig->horizon_offset = 0;
+    .horizon_offset = 0,
-    osdConfig->camera_uptilt = 0;
+    .camera_uptilt = 0,
-    osdConfig->camera_fov_h = 135;
+    .camera_fov_h = 135,
-    osdConfig->camera_fov_v = 85;
+    .camera_fov_v = 85,
-    osdConfig->hud_margin_h = 3;
+    .hud_margin_h = 3,
-    osdConfig->hud_margin_v = 3;
+    .hud_margin_v = 3,
-    osdConfig->hud_homing = 0;
+    .hud_homing = 0,
-    osdConfig->hud_homepoint = 0;
+    .hud_homepoint = 0,
-    osdConfig->hud_radar_disp = 0;
+    .hud_radar_disp = 0,
-    osdConfig->hud_radar_range_min = 3;
+    .hud_radar_range_min = 3,
-    osdConfig->hud_radar_range_max = 4000;
+    .hud_radar_range_max = 4000,
-    osdConfig->hud_radar_nearest = 0;
+    .hud_radar_nearest = 0,
-    osdConfig->hud_wp_disp = 0;
+    .hud_wp_disp = 0,
-    osdConfig->left_sidebar_scroll = OSD_SIDEBAR_SCROLL_NONE;
+    .left_sidebar_scroll = OSD_SIDEBAR_SCROLL_NONE,
-    osdConfig->right_sidebar_scroll = OSD_SIDEBAR_SCROLL_NONE;
+    .right_sidebar_scroll = OSD_SIDEBAR_SCROLL_NONE,
-    osdConfig->sidebar_scroll_arrows = 0;
+    .sidebar_scroll_arrows = 0,
 
-    osdConfig->units = OSD_UNIT_METRIC;
+    .units = OSD_UNIT_METRIC,
-    osdConfig->main_voltage_decimals = 1;
+    .main_voltage_decimals = 1,
 
-    osdConfig->estimations_wind_compensation = true;
+    .estimations_wind_compensation = true,
-    osdConfig->coordinate_digits = 9;
+    .coordinate_digits = 9,
 
-    osdConfig->osd_failsafe_switch_layout = false;
+    .osd_failsafe_switch_layout = false,
 
-    osdConfig->plus_code_digits = 11;
+    .plus_code_digits = 11,
+
+    .ahi_width = OSD_AHI_WIDTH * OSD_CHAR_WIDTH,
+    .ahi_height = OSD_AHI_HEIGHT * OSD_CHAR_HEIGHT,
+    .ahi_vertical_offset = -OSD_CHAR_HEIGHT,
+    .sidebar_horizontal_offset = OSD_AH_SIDEBAR_WIDTH_POS * OSD_CHAR_WIDTH,
+);
+
+void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig)
+{
+    osdLayoutsConfig->item_pos[0][OSD_ALTITUDE] = OSD_POS(1, 0) | OSD_VISIBLE_FLAG;
+    osdLayoutsConfig->item_pos[0][OSD_MAIN_BATT_VOLTAGE] = OSD_POS(12, 0) | OSD_VISIBLE_FLAG;
+    osdLayoutsConfig->item_pos[0][OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE] = OSD_POS(12, 1);
+
+    osdLayoutsConfig->item_pos[0][OSD_RSSI_VALUE] = OSD_POS(23, 0) | OSD_VISIBLE_FLAG;
+    //line 2
+    osdLayoutsConfig->item_pos[0][OSD_HOME_DIST] = OSD_POS(1, 1);
+    osdLayoutsConfig->item_pos[0][OSD_TRIP_DIST] = OSD_POS(1, 2);
+    osdLayoutsConfig->item_pos[0][OSD_MAIN_BATT_CELL_VOLTAGE] = OSD_POS(12, 1);
+    osdLayoutsConfig->item_pos[0][OSD_MAIN_BATT_SAG_COMPENSATED_CELL_VOLTAGE] = OSD_POS(12, 1);
+    osdLayoutsConfig->item_pos[0][OSD_GPS_SPEED] = OSD_POS(23, 1);
+    osdLayoutsConfig->item_pos[0][OSD_3D_SPEED] = OSD_POS(23, 1);
+
+    osdLayoutsConfig->item_pos[0][OSD_THROTTLE_POS] = OSD_POS(1, 2) | OSD_VISIBLE_FLAG;
+    osdLayoutsConfig->item_pos[0][OSD_THROTTLE_POS_AUTO_THR] = OSD_POS(6, 2);
+    osdLayoutsConfig->item_pos[0][OSD_HEADING] = OSD_POS(12, 2);
+    osdLayoutsConfig->item_pos[0][OSD_CRUISE_HEADING_ERROR] = OSD_POS(12, 2);
+    osdLayoutsConfig->item_pos[0][OSD_CRUISE_HEADING_ADJUSTMENT] = OSD_POS(12, 2);
+    osdLayoutsConfig->item_pos[0][OSD_HEADING_GRAPH] = OSD_POS(18, 2);
+    osdLayoutsConfig->item_pos[0][OSD_CURRENT_DRAW] = OSD_POS(2, 3) | OSD_VISIBLE_FLAG;
+    osdLayoutsConfig->item_pos[0][OSD_MAH_DRAWN] = OSD_POS(1, 4) | OSD_VISIBLE_FLAG;
+    osdLayoutsConfig->item_pos[0][OSD_WH_DRAWN] = OSD_POS(1, 5);
+    osdLayoutsConfig->item_pos[0][OSD_BATTERY_REMAINING_CAPACITY] = OSD_POS(1, 6);
+    osdLayoutsConfig->item_pos[0][OSD_BATTERY_REMAINING_PERCENT] = OSD_POS(1, 7);
+    osdLayoutsConfig->item_pos[0][OSD_POWER_SUPPLY_IMPEDANCE] = OSD_POS(1, 8);
+
+    osdLayoutsConfig->item_pos[0][OSD_EFFICIENCY_MAH_PER_KM] = OSD_POS(1, 5);
+    osdLayoutsConfig->item_pos[0][OSD_EFFICIENCY_WH_PER_KM] = OSD_POS(1, 5);
+
+    osdLayoutsConfig->item_pos[0][OSD_ATTITUDE_ROLL] = OSD_POS(1, 7);
+    osdLayoutsConfig->item_pos[0][OSD_ATTITUDE_PITCH] = OSD_POS(1, 8);
+
+    // avoid OSD_VARIO under OSD_CROSSHAIRS
+    osdLayoutsConfig->item_pos[0][OSD_VARIO] = OSD_POS(23, 5);
+    // OSD_VARIO_NUM at the right of OSD_VARIO
+    osdLayoutsConfig->item_pos[0][OSD_VARIO_NUM] = OSD_POS(24, 7);
+    osdLayoutsConfig->item_pos[0][OSD_HOME_DIR] = OSD_POS(14, 11);
+    osdLayoutsConfig->item_pos[0][OSD_ARTIFICIAL_HORIZON] = OSD_POS(8, 6) | OSD_VISIBLE_FLAG;
+    osdLayoutsConfig->item_pos[0][OSD_HORIZON_SIDEBARS] = OSD_POS(8, 6) | OSD_VISIBLE_FLAG;
+
+    osdLayoutsConfig->item_pos[0][OSD_CRAFT_NAME] = OSD_POS(20, 2);
+    osdLayoutsConfig->item_pos[0][OSD_VTX_CHANNEL] = OSD_POS(8, 6);
+
+    osdLayoutsConfig->item_pos[0][OSD_ONTIME] = OSD_POS(23, 8);
+    osdLayoutsConfig->item_pos[0][OSD_FLYTIME] = OSD_POS(23, 9);
+    osdLayoutsConfig->item_pos[0][OSD_ONTIME_FLYTIME] = OSD_POS(23, 11) | OSD_VISIBLE_FLAG;
+    osdLayoutsConfig->item_pos[0][OSD_RTC_TIME] = OSD_POS(23, 12);
+    osdLayoutsConfig->item_pos[0][OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH] = OSD_POS(23, 7);
+    osdLayoutsConfig->item_pos[0][OSD_REMAINING_DISTANCE_BEFORE_RTH] = OSD_POS(23, 6);
+
+    osdLayoutsConfig->item_pos[0][OSD_GPS_SATS] = OSD_POS(0, 11) | OSD_VISIBLE_FLAG;
+    osdLayoutsConfig->item_pos[0][OSD_GPS_HDOP] = OSD_POS(0, 10);
+
+    osdLayoutsConfig->item_pos[0][OSD_GPS_LAT] = OSD_POS(0, 12);
+    // Put this on top of the latitude, since it's very unlikely
+    // that users will want to use both at the same time.
+    osdLayoutsConfig->item_pos[0][OSD_PLUS_CODE] = OSD_POS(0, 12);
+    osdLayoutsConfig->item_pos[0][OSD_FLYMODE] = OSD_POS(13, 12) | OSD_VISIBLE_FLAG;
+    osdLayoutsConfig->item_pos[0][OSD_GPS_LON] = OSD_POS(18, 12);
+
+    osdLayoutsConfig->item_pos[0][OSD_AZIMUTH] = OSD_POS(2, 12);
+
+    osdLayoutsConfig->item_pos[0][OSD_ROLL_PIDS] = OSD_POS(2, 10);
+    osdLayoutsConfig->item_pos[0][OSD_PITCH_PIDS] = OSD_POS(2, 11);
+    osdLayoutsConfig->item_pos[0][OSD_YAW_PIDS] = OSD_POS(2, 12);
+    osdLayoutsConfig->item_pos[0][OSD_LEVEL_PIDS] = OSD_POS(2, 12);
+    osdLayoutsConfig->item_pos[0][OSD_POS_XY_PIDS] = OSD_POS(2, 12);
+    osdLayoutsConfig->item_pos[0][OSD_POS_Z_PIDS] = OSD_POS(2, 12);
+    osdLayoutsConfig->item_pos[0][OSD_VEL_XY_PIDS] = OSD_POS(2, 12);
+    osdLayoutsConfig->item_pos[0][OSD_VEL_Z_PIDS] = OSD_POS(2, 12);
+    osdLayoutsConfig->item_pos[0][OSD_HEADING_P] = OSD_POS(2, 12);
+    osdLayoutsConfig->item_pos[0][OSD_BOARD_ALIGN_ROLL] = OSD_POS(2, 10);
+    osdLayoutsConfig->item_pos[0][OSD_BOARD_ALIGN_PITCH] = OSD_POS(2, 11);
+    osdLayoutsConfig->item_pos[0][OSD_RC_EXPO] = OSD_POS(2, 12);
+    osdLayoutsConfig->item_pos[0][OSD_RC_YAW_EXPO] = OSD_POS(2, 12);
+    osdLayoutsConfig->item_pos[0][OSD_THROTTLE_EXPO] = OSD_POS(2, 12);
+    osdLayoutsConfig->item_pos[0][OSD_PITCH_RATE] = OSD_POS(2, 12);
+    osdLayoutsConfig->item_pos[0][OSD_ROLL_RATE] = OSD_POS(2, 12);
+    osdLayoutsConfig->item_pos[0][OSD_YAW_RATE] = OSD_POS(2, 12);
+    osdLayoutsConfig->item_pos[0][OSD_MANUAL_RC_EXPO] = OSD_POS(2, 12);
+    osdLayoutsConfig->item_pos[0][OSD_MANUAL_RC_YAW_EXPO] = OSD_POS(2, 12);
+    osdLayoutsConfig->item_pos[0][OSD_MANUAL_PITCH_RATE] = OSD_POS(2, 12);
+    osdLayoutsConfig->item_pos[0][OSD_MANUAL_ROLL_RATE] = OSD_POS(2, 12);
+    osdLayoutsConfig->item_pos[0][OSD_MANUAL_YAW_RATE] = OSD_POS(2, 12);
+    osdLayoutsConfig->item_pos[0][OSD_NAV_FW_CRUISE_THR] = OSD_POS(2, 12);
+    osdLayoutsConfig->item_pos[0][OSD_NAV_FW_PITCH2THR] = OSD_POS(2, 12);
+    osdLayoutsConfig->item_pos[0][OSD_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE] = OSD_POS(2, 12);
+    osdLayoutsConfig->item_pos[0][OSD_FW_ALT_PID_OUTPUTS] = OSD_POS(2, 12);
+    osdLayoutsConfig->item_pos[0][OSD_FW_POS_PID_OUTPUTS] = OSD_POS(2, 12);
+    osdLayoutsConfig->item_pos[0][OSD_MC_VEL_X_PID_OUTPUTS] = OSD_POS(2, 12);
+    osdLayoutsConfig->item_pos[0][OSD_MC_VEL_Y_PID_OUTPUTS] = OSD_POS(2, 12);
+    osdLayoutsConfig->item_pos[0][OSD_MC_VEL_Z_PID_OUTPUTS] = OSD_POS(2, 12);
+    osdLayoutsConfig->item_pos[0][OSD_MC_POS_XYZ_P_OUTPUTS] = OSD_POS(2, 12);
+
+    osdLayoutsConfig->item_pos[0][OSD_POWER] = OSD_POS(15, 1);
+
+    osdLayoutsConfig->item_pos[0][OSD_IMU_TEMPERATURE] = OSD_POS(19, 2);
+    osdLayoutsConfig->item_pos[0][OSD_BARO_TEMPERATURE] = OSD_POS(19, 3);
+    osdLayoutsConfig->item_pos[0][OSD_TEMP_SENSOR_0_TEMPERATURE] = OSD_POS(19, 4);
+    osdLayoutsConfig->item_pos[0][OSD_TEMP_SENSOR_1_TEMPERATURE] = OSD_POS(19, 5);
+    osdLayoutsConfig->item_pos[0][OSD_TEMP_SENSOR_2_TEMPERATURE] = OSD_POS(19, 6);
+    osdLayoutsConfig->item_pos[0][OSD_TEMP_SENSOR_3_TEMPERATURE] = OSD_POS(19, 7);
+    osdLayoutsConfig->item_pos[0][OSD_TEMP_SENSOR_4_TEMPERATURE] = OSD_POS(19, 8);
+    osdLayoutsConfig->item_pos[0][OSD_TEMP_SENSOR_5_TEMPERATURE] = OSD_POS(19, 9);
+    osdLayoutsConfig->item_pos[0][OSD_TEMP_SENSOR_6_TEMPERATURE] = OSD_POS(19, 10);
+    osdLayoutsConfig->item_pos[0][OSD_TEMP_SENSOR_7_TEMPERATURE] = OSD_POS(19, 11);
+
+    osdLayoutsConfig->item_pos[0][OSD_AIR_SPEED] = OSD_POS(3, 5);
+    osdLayoutsConfig->item_pos[0][OSD_WIND_SPEED_HORIZONTAL] = OSD_POS(3, 6);
+    osdLayoutsConfig->item_pos[0][OSD_WIND_SPEED_VERTICAL] = OSD_POS(3, 7);
+
+    osdLayoutsConfig->item_pos[0][OSD_GFORCE] = OSD_POS(12, 4);
+    osdLayoutsConfig->item_pos[0][OSD_GFORCE_X] = OSD_POS(12, 5);
+    osdLayoutsConfig->item_pos[0][OSD_GFORCE_Y] = OSD_POS(12, 6);
+    osdLayoutsConfig->item_pos[0][OSD_GFORCE_Z] = OSD_POS(12, 7);
+
+    osdLayoutsConfig->item_pos[0][OSD_VTX_POWER] = OSD_POS(3, 5);
+
+#if defined(USE_ESC_SENSOR)
+    osdLayoutsConfig->item_pos[0][OSD_ESC_RPM] = OSD_POS(1, 2);
+    osdLayoutsConfig->item_pos[0][OSD_ESC_TEMPERATURE] = OSD_POS(1, 3);
+#endif
+
+#if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE)
+    osdLayoutsConfig->item_pos[0][OSD_RC_SOURCE] = OSD_POS(3, 4);
+#endif
+
+    // Under OSD_FLYMODE. TODO: Might not be visible on NTSC?
+    osdLayoutsConfig->item_pos[0][OSD_MESSAGES] = OSD_POS(1, 13) | OSD_VISIBLE_FLAG;
+
+    for (unsigned ii = 1; ii < OSD_LAYOUT_COUNT; ii++) {
+        for (unsigned jj = 0; jj < ARRAYLEN(osdLayoutsConfig->item_pos[0]); jj++) {
+             osdLayoutsConfig->item_pos[ii][jj] = osdLayoutsConfig->item_pos[0][jj] & ~OSD_VISIBLE_FLAG;
+        }
+    }
 }
 
 static void osdSetNextRefreshIn(uint32_t timeMs) {
@@ -2815,7 +2708,11 @@ static void osdCompleteAsyncInitialization(void)
     osdDisplayIsReady = true;
 
 #if defined(USE_CANVAS)
-    osdDisplayHasCanvas = displayGetCanvas(&osdCanvas, osdDisplayPort);
+    if (osdConfig()->force_grid) {
+        osdDisplayHasCanvas = false;
+    } else {
+        osdDisplayHasCanvas = displayGetCanvas(&osdCanvas, osdDisplayPort);
+    }
 #endif
 
     displayBeginTransaction(osdDisplayPort, DISPLAY_TRANSACTION_OPT_RESET_DRAWING);
@@ -3289,7 +3186,7 @@ void osdStartFullRedraw(void)
 
 void osdOverrideLayout(int layout, timeMs_t duration)
 {
-    layoutOverride = constrain(layout, -1, ARRAYLEN(osdConfig()->item_pos) - 1);
+    layoutOverride = constrain(layout, -1, ARRAYLEN(osdLayoutsConfig()->item_pos) - 1);
     if (layoutOverride >= 0 && duration > 0) {
         layoutOverrideUntil = millis() + duration;
     } else {

src/main/io/osd.h

@@ -160,6 +160,8 @@ typedef enum {
     OSD_UNIT_IMPERIAL,
     OSD_UNIT_METRIC,
     OSD_UNIT_UK, // Show speed in mp/h, other values in metric
+
+    OSD_UNIT_MAX = OSD_UNIT_UK,
 } osd_unit_e;
 
 typedef enum {
@@ -182,6 +184,8 @@ typedef enum {
     OSD_SIDEBAR_SCROLL_ALTITUDE,
     OSD_SIDEBAR_SCROLL_GROUND_SPEED,
     OSD_SIDEBAR_SCROLL_HOME_DISTANCE,
+
+    OSD_SIDEBAR_SCROLL_MAX = OSD_SIDEBAR_SCROLL_HOME_DISTANCE,
 } osd_sidebar_scroll_e;
 
 typedef enum {
@@ -194,10 +198,14 @@ typedef enum {
     OSD_AHI_STYLE_LINE,
 } osd_ahi_style_e;
 
-typedef struct osdConfig_s {
+typedef struct osdLayoutsConfig_s {
     // Layouts
     uint16_t item_pos[OSD_LAYOUT_COUNT][OSD_ITEM_COUNT];
+} osdLayoutsConfig_t;
 
+PG_DECLARE(osdLayoutsConfig_t, osdLayoutsConfig);
+
+typedef struct osdConfig_s {
     // Alarms
     uint8_t rssi_alarm; // rssi %
     uint16_t time_alarm; // fly minutes
@@ -255,6 +263,15 @@ 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;
+    uint8_t force_grid;                 // Force a pixel based OSD to use grid mode.
+    uint8_t ahi_bordered;               // Only used by the AHI widget
+    uint8_t ahi_width;                  // In pixels, only used by the AHI widget
+    uint8_t ahi_height;                 // In pixels, only used by the AHI widget
+    int8_t  ahi_vertical_offset;        // Offset from center in pixels. Positive moves the AHI down. Widget only.
+    int8_t sidebar_horizontal_offset;   // Horizontal offset from default position. Units are grid slots for grid OSDs, pixels for pixel based OSDs. Positive values move sidebars closer to the edges.
+    uint8_t left_sidebar_scroll_step;   // How many units each sidebar step represents. 0 means the default value for the scroll type.
+    uint8_t right_sidebar_scroll_step;  // Same as left_sidebar_scroll_step, but for the right sidebar.
+
 } osdConfig_t;
 
 PG_DECLARE(osdConfig_t, osdConfig);

src/main/io/osd_canvas.c

@@ -30,10 +30,15 @@
 
 #if defined(USE_CANVAS)
 
-#define AHI_MIN_DRAW_INTERVAL_MS 100
+#define AHI_MIN_DRAW_INTERVAL_MS 50
 #define AHI_MAX_DRAW_INTERVAL_MS 1000
 #define AHI_CROSSHAIR_MARGIN 6
 
+#define SIDEBAR_REDRAW_INTERVAL_MS 100
+#define WIDGET_SIDEBAR_LEFT_INSTANCE 0
+#define WIDGET_SIDEBAR_RIGHT_INSTANCE 1
+
+#include "common/constants.h"
 #include "common/log.h"
 #include "common/maths.h"
 #include "common/typeconversion.h"
@@ -41,6 +46,7 @@
 
 #include "drivers/display.h"
 #include "drivers/display_canvas.h"
+#include "drivers/display_widgets.h"
 #include "drivers/osd.h"
 #include "drivers/osd_symbols.h"
 #include "drivers/time.h"
@@ -48,36 +54,21 @@
 #include "io/osd_common.h"
 #include "io/osd.h"
 
-void osdCanvasDrawVarioShape(displayCanvas_t *canvas, unsigned ex, unsigned ey, float zvel, bool erase)
+#include "navigation/navigation.h"
+
+#define OSD_CANVAS_VARIO_ARROWS_PER_SLOT 2.0f
+
+static void osdCanvasVarioRect(int *y, int *h, displayCanvas_t *canvas, int midY, float zvel)
 {
-    char sym;
+    int maxHeight = ceilf(OSD_VARIO_HEIGHT_ROWS /OSD_CANVAS_VARIO_ARROWS_PER_SLOT) * canvas->gridElementHeight;
-    float ratio = zvel / (OSD_VARIO_CM_S_PER_ARROW * 2);
+    // We use font characters with 2 arrows per slot
-    int height = -ratio * canvas->gridElementHeight;
+    int height = MIN(fabsf(zvel) / (OSD_VARIO_CM_S_PER_ARROW * OSD_CANVAS_VARIO_ARROWS_PER_SLOT) * canvas->gridElementHeight, maxHeight);
-    int step;
+    if (zvel >= 0) {
-    int x = ex * canvas->gridElementWidth;
+        *y = midY - height;
-    int start;
-    int dstart;
-    if (zvel > 0) {
-        sym = SYM_VARIO_UP_2A;
-        start = ceilf(OSD_VARIO_HEIGHT_ROWS / 2.0f);
-        dstart = start - 1;
-        step = -canvas->gridElementHeight;
     } else {
-        sym = SYM_VARIO_DOWN_2A;
+        *y = midY;
-        start = floorf(OSD_VARIO_HEIGHT_ROWS / 2.0f);
-        dstart = start;
-        step = canvas->gridElementHeight;
-    }
-    int y = (start + ey) * canvas->gridElementHeight;
-    displayCanvasClipToRect(canvas, x, y, canvas->gridElementWidth, height);
-    int dy = (dstart + ey) * canvas->gridElementHeight;
-    for (int ii = 0, yy = dy; ii < (OSD_VARIO_HEIGHT_ROWS + 1) / 2; ii++, yy += step) {
-        if (erase) {
-            displayCanvasDrawCharacterMask(canvas, x, yy, sym, DISPLAY_CANVAS_COLOR_TRANSPARENT, 0);
-        } else {
-            displayCanvasDrawCharacter(canvas, x, yy, sym, 0);
-        }
     }
+    *h = height;
 }
 
 void osdCanvasDrawVario(displayPort_t *display, displayCanvas_t *canvas, const osdDrawPoint_t *p, float zvel)
@@ -86,46 +77,84 @@ void osdCanvasDrawVario(displayPort_t *display, displayCanvas_t *canvas, const o
 
     static float prev = 0;
 
-    if (fabsf(prev - zvel) < (OSD_VARIO_CM_S_PER_ARROW / 20.0f)) {
+    if (fabsf(prev - zvel) < (OSD_VARIO_CM_S_PER_ARROW / (OSD_CANVAS_VARIO_ARROWS_PER_SLOT * 10))) {
         return;
     }
 
-    uint8_t ex;
+    // Make sure we clear the grid buffer
-    uint8_t ey;
+    uint8_t gx;
+    uint8_t gy;
+    osdDrawPointGetGrid(&gx, &gy, display, canvas, p);
+    osdGridBufferClearGridRect(gx, gy, 1, OSD_VARIO_HEIGHT_ROWS);
 
-    osdDrawPointGetGrid(&ex, &ey, display, canvas, p);
+    int midY = gy * canvas->gridElementHeight + (OSD_VARIO_HEIGHT_ROWS * canvas->gridElementHeight) / 2;
+    // Make sure we're aligned with the center-ish of the grid based variant
+    midY -= canvas->gridElementHeight;
 
-    osdCanvasDrawVarioShape(canvas, ex, ey, prev, true);
+    int x = gx * canvas->gridElementWidth;
-    osdCanvasDrawVarioShape(canvas, ex, ey, zvel, false);
+    int w = canvas->gridElementWidth;
+    int y;
+    int h;
+
+    osdCanvasVarioRect(&y, &h, canvas, midY, zvel);
+
+    if (signbit(prev) != signbit(zvel) || fabsf(prev) > fabsf(zvel)) {
+        // New rectangle doesn't overwrite the old one, we need to erase
+        int py;
+        int ph;
+        osdCanvasVarioRect(&py, &ph, canvas, midY, prev);
+        if (ph != h) {
+            displayCanvasClearRect(canvas, x, py, w, ph);
+        }
+    }
+    displayCanvasClipToRect(canvas, x, y, w, h);
+    if (zvel > 0) {
+        for (int yy = midY - canvas->gridElementHeight; yy > midY - h - canvas->gridElementHeight; yy -= canvas->gridElementHeight) {
+            displayCanvasDrawCharacter(canvas, x, yy, SYM_VARIO_UP_2A, DISPLAY_CANVAS_BITMAP_OPT_ERASE_TRANSPARENT);
+        }
+    } else {
+        for (int yy = midY; yy < midY + h; yy += canvas->gridElementHeight) {
+            displayCanvasDrawCharacter(canvas, x, yy, SYM_VARIO_DOWN_2A, DISPLAY_CANVAS_BITMAP_OPT_ERASE_TRANSPARENT);
+        }
+    }
     prev = zvel;
 }
 
-void osdCanvasDrawDirArrow(displayPort_t *display, displayCanvas_t *canvas, const osdDrawPoint_t *p, float degrees, bool eraseBefore)
+void osdCanvasDrawDirArrow(displayPort_t *display, displayCanvas_t *canvas, const osdDrawPoint_t *p, float degrees)
 {
     UNUSED(display);
 
+    const int top = 6;
+    const int topInset = -2;
+    const int bottom = -6;
+    const int width = 5;
+    // Since grid slots are not square, when we rotate the arrow
+    // it overflows horizontally a bit. Making a square-ish arrow
+    // doesn't look good, so it's better to overstep the grid slot
+    // boundaries a bit and then clean after ourselves.
+    const int overflow = 3;
+
     int px;
     int py;
     osdDrawPointGetPixels(&px, &py, display, canvas, p);
 
-    displayCanvasClipToRect(canvas, px, py, canvas->gridElementWidth, canvas->gridElementHeight);
+    displayCanvasClearRect(canvas, px - overflow, py, canvas->gridElementWidth + overflow * 2, canvas->gridElementHeight);
-
-    if (eraseBefore) {
-        displayCanvasSetFillColor(canvas, DISPLAY_CANVAS_COLOR_TRANSPARENT);
-        displayCanvasFillRect(canvas, px, py, canvas->gridElementWidth, canvas->gridElementHeight);
-    }
 
     displayCanvasSetFillColor(canvas, DISPLAY_CANVAS_COLOR_WHITE);
     displayCanvasSetStrokeColor(canvas, DISPLAY_CANVAS_COLOR_BLACK);
 
     displayCanvasCtmRotate(canvas, -DEGREES_TO_RADIANS(180 + degrees));
     displayCanvasCtmTranslate(canvas, px + canvas->gridElementWidth / 2, py + canvas->gridElementHeight / 2);
-    displayCanvasFillStrokeTriangle(canvas, 0, 6, 5, -6, -5, -6);
+
+    // Main triangle
+    displayCanvasFillStrokeTriangle(canvas, 0, top, width, bottom, -width, bottom);
+    // Inset triangle
     displayCanvasSetFillColor(canvas, DISPLAY_CANVAS_COLOR_TRANSPARENT);
-    displayCanvasFillStrokeTriangle(canvas, 0, -2, 6, -7, -6, -7);
+    displayCanvasFillTriangle(canvas, 0, topInset, width + 1, bottom - 1, -width, bottom - 1);
-    displayCanvasMoveToPoint(canvas, 6, -7);
+    // Draw bottom strokes of the triangle
-    displayCanvasSetStrokeColor(canvas, DISPLAY_CANVAS_COLOR_TRANSPARENT);
+    displayCanvasMoveToPoint(canvas, -width, bottom - 1);
-    displayCanvasStrokeLineToPoint(canvas, -6, -7);
+    displayCanvasStrokeLineToPoint(canvas, 0, topInset);
+    displayCanvasStrokeLineToPoint(canvas, width, bottom - 1);
 }
 
 static void osdDrawArtificialHorizonLevelLine(displayCanvas_t *canvas, int width, int pos, int margin)
@@ -300,6 +329,69 @@ void osdDrawArtificialHorizonLine(displayCanvas_t *canvas, float pitchAngle, flo
     displayCanvasContextPop(canvas);
 }
 
+static bool osdCanvasDrawArtificialHorizonWidget(displayPort_t *display, displayCanvas_t *canvas, const osdDrawPoint_t *p, float pitchAngle, float rollAngle)
+{
+    UNUSED(display);
+    UNUSED(p);
+
+    const int instance = 0;
+    static int iterations = 0;
+    static bool configured = false;
+
+    displayWidgets_t widgets;
+    if (displayCanvasGetWidgets(&widgets, canvas) &&
+        displayWidgetsSupportedInstances(&widgets, DISPLAY_WIDGET_TYPE_AHI) >= instance) {
+
+        int ahiWidth = osdConfig()->ahi_width;
+        int ahiX = (canvas->width - ahiWidth) / 2;
+        int ahiHeight = osdConfig()->ahi_height;
+        int ahiY = ((canvas->height - ahiHeight) / 2) + osdConfig()->ahi_vertical_offset;
+        if (ahiY < 0) {
+            ahiY = 0;
+        }
+        if (ahiY + ahiHeight >= canvas->height) {
+            ahiY = canvas->height - ahiHeight - 1;
+        }
+        if (!configured) {
+            widgetAHIStyle_e ahiStyle = 0;
+            switch ((osd_ahi_style_e)osdConfig()->osd_ahi_style) {
+                case OSD_AHI_STYLE_DEFAULT:
+                    ahiStyle = DISPLAY_WIDGET_AHI_STYLE_STAIRCASE;
+                    break;
+                case OSD_AHI_STYLE_LINE:
+                    ahiStyle = DISPLAY_WIDGET_AHI_STYLE_LINE;
+                    break;
+            }
+            widgetAHIConfiguration_t config = {
+                .rect.x = ahiX,
+                .rect.y = ahiY,
+                .rect.w = ahiWidth,
+                .rect.h = ahiHeight,
+                .style = ahiStyle,
+                .options = osdConfig()->ahi_bordered ? DISPLAY_WIDGET_AHI_OPTION_SHOW_CORNERS : 0,
+                .crosshairMargin = AHI_CROSSHAIR_MARGIN,
+                .strokeWidth = 1,
+            };
+            if (!displayWidgetsConfigureAHI(&widgets, instance, &config)) {
+                return false;
+            }
+            configured = true;
+        }
+        widgetAHIData_t data = {
+            .pitch = pitchAngle,
+            .roll = rollAngle,
+        };
+        if (displayWidgetsDrawAHI(&widgets, instance, &data)) {
+            if (++iterations == 10) {
+                iterations = 0;
+                osdGridBufferClearPixelRect(canvas, ahiX, ahiY, ahiWidth, ahiHeight);
+            }
+            return true;
+        }
+    }
+    return false;
+}
+
 void osdCanvasDrawArtificialHorizon(displayPort_t *display, displayCanvas_t *canvas, const osdDrawPoint_t *p, float pitchAngle, float rollAngle)
 {
     UNUSED(display);
@@ -314,20 +406,24 @@ void osdCanvasDrawArtificialHorizon(displayPort_t *display, displayCanvas_t *can
 
     float totalError = fabsf(prevPitchAngle - pitchAngle) + fabsf(prevRollAngle - rollAngle);
     if ((now > nextDrawMinMs && totalError > 0.05f)|| now > nextDrawMaxMs) {
-        switch ((osd_ahi_style_e)osdConfig()->osd_ahi_style) {
+
-            case OSD_AHI_STYLE_DEFAULT:
+        if (!osdCanvasDrawArtificialHorizonWidget(display, canvas, p, pitchAngle, rollAngle)) {
-            {
+            switch ((osd_ahi_style_e)osdConfig()->osd_ahi_style) {
-                int x, y, w, h;
+                case OSD_AHI_STYLE_DEFAULT:
-                osdArtificialHorizonRect(canvas, &x, &y, &w, &h);
+                {
-                displayCanvasClearRect(canvas, x, y, w, h);
+                    int x, y, w, h;
-                osdDrawArtificialHorizonShapes(canvas, pitchAngle, rollAngle);
+                    osdArtificialHorizonRect(canvas, &x, &y, &w, &h);
-                break;
+                    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;
             }
-            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;
@@ -394,4 +490,223 @@ void osdCanvasDrawHeadingGraph(displayPort_t *display, displayCanvas_t *canvas,
     displayCanvasFillStrokeTriangle(canvas, rmx - 2, py - 1, rmx + 2, py - 1, rmx, py + 1);
 }
 
+static int32_t osdCanvasSidebarGetValue(osd_sidebar_scroll_e scroll)
+{
+    switch (scroll) {
+        case OSD_SIDEBAR_SCROLL_NONE:
+            break;
+        case OSD_SIDEBAR_SCROLL_ALTITUDE:
+            switch ((osd_unit_e)osdConfig()->units) {
+                case OSD_UNIT_IMPERIAL:
+                    return CENTIMETERS_TO_CENTIFEET(osdGetAltitude());
+                case OSD_UNIT_UK:
+                    FALLTHROUGH;
+                case OSD_UNIT_METRIC:
+                    return osdGetAltitude();
+            }
+            break;
+        case OSD_SIDEBAR_SCROLL_GROUND_SPEED:
+#if defined(USE_GPS)
+            switch ((osd_unit_e)osdConfig()->units) {
+                case OSD_UNIT_UK:
+                    FALLTHROUGH;
+                case OSD_UNIT_IMPERIAL:
+                    // cms/s to (mi/h) * 100
+                    return gpsSol.groundSpeed * 224 / 100;
+                case OSD_UNIT_METRIC:
+                    // cm/s to (km/h) * 100
+                    return gpsSol.groundSpeed * 36 / 10;
+            }
+#endif
+            break;
+        case OSD_SIDEBAR_SCROLL_HOME_DISTANCE:
+#if defined(USE_GPS)
+            switch ((osd_unit_e)osdConfig()->units) {
+                case OSD_UNIT_IMPERIAL:
+                    return CENTIMETERS_TO_CENTIFEET(GPS_distanceToHome * 100);
+                case OSD_UNIT_UK:
+                    FALLTHROUGH;
+                case OSD_UNIT_METRIC:
+                    return GPS_distanceToHome * 100;
+#endif
+            }
+            break;
+    }
+    return 0;
+}
+
+static uint8_t osdCanvasSidebarGetOptions(int *width, osd_sidebar_scroll_e scroll)
+{
+    switch (scroll) {
+        case OSD_SIDEBAR_SCROLL_NONE:
+            break;
+        case OSD_SIDEBAR_SCROLL_ALTITUDE:
+            FALLTHROUGH;
+        case OSD_SIDEBAR_SCROLL_GROUND_SPEED:
+            FALLTHROUGH;
+        case OSD_SIDEBAR_SCROLL_HOME_DISTANCE:
+            *width = OSD_CHAR_WIDTH * 5; // 4 numbers + unit
+            return 0;
+    }
+    *width = OSD_CHAR_WIDTH;
+    return DISPLAY_WIDGET_SIDEBAR_OPTION_STATIC | DISPLAY_WIDGET_SIDEBAR_OPTION_UNLABELED;
+}
+
+static void osdCanvasSidebarGetUnit(osdUnit_t *unit, uint16_t *countsPerStep, osd_sidebar_scroll_e scroll)
+{
+    // We always count in 1/100 units, converting to
+    // "centifeet" when displaying imperial units
+    unit->scale = 100;
+
+    switch (scroll) {
+        case OSD_SIDEBAR_SCROLL_NONE:
+            unit->symbol = 0;
+            unit->divisor = 0;
+            unit->divided_symbol = 0;
+            *countsPerStep = 1;
+            break;
+        case OSD_SIDEBAR_SCROLL_ALTITUDE:
+            switch ((osd_unit_e)osdConfig()->units) {
+                case OSD_UNIT_IMPERIAL:
+                    unit->symbol = SYM_ALT_FT;
+                    unit->divisor = FEET_PER_KILOFEET;
+                    unit->divided_symbol = SYM_ALT_KFT;
+                    *countsPerStep = 50;
+                    break;
+                case OSD_UNIT_UK:
+                    FALLTHROUGH;
+                case OSD_UNIT_METRIC:
+                    unit->symbol = SYM_ALT_M;
+                    unit->divisor = METERS_PER_KILOMETER;
+                    unit->divided_symbol = SYM_ALT_KM;
+                    *countsPerStep = 20;
+                    break;
+            }
+            break;
+        case OSD_SIDEBAR_SCROLL_GROUND_SPEED:
+            switch ((osd_unit_e)osdConfig()->units) {
+                case OSD_UNIT_UK:
+                    FALLTHROUGH;
+                case OSD_UNIT_IMPERIAL:
+                    unit->symbol = SYM_MPH;
+                    unit->divisor = 0;
+                    unit->divided_symbol = 0;
+                    *countsPerStep = 5;
+                    break;
+                case OSD_UNIT_METRIC:
+                    unit->symbol = SYM_KMH;
+                    unit->divisor = 0;
+                    unit->divided_symbol = 0;
+                    *countsPerStep = 10;
+                    break;
+            }
+            break;
+        case OSD_SIDEBAR_SCROLL_HOME_DISTANCE:
+            switch ((osd_unit_e)osdConfig()->units) {
+                case OSD_UNIT_IMPERIAL:
+                    unit->symbol = SYM_FT;
+                    unit->divisor = FEET_PER_MILE;
+                    unit->divided_symbol = SYM_MI;
+                    *countsPerStep = 300;
+                    break;
+                case OSD_UNIT_UK:
+                    FALLTHROUGH;
+                case OSD_UNIT_METRIC:
+                    unit->symbol = SYM_M;
+                    unit->divisor = METERS_PER_KILOMETER;
+                    unit->divided_symbol = SYM_KM;
+                    *countsPerStep = 100;
+                    break;
+            }
+            break;
+    }
+}
+
+static bool osdCanvasDrawSidebar(uint32_t *configured, displayWidgets_t *widgets,
+                                displayCanvas_t *canvas,
+                                int instance,
+                                osd_sidebar_scroll_e scroll, unsigned scrollStep)
+{
+    STATIC_ASSERT(OSD_SIDEBAR_SCROLL_MAX <= 3, adjust_scroll_shift);
+    STATIC_ASSERT(OSD_UNIT_MAX <= 3, adjust_units_shift);
+    // Configuration
+    uint32_t configuration = scrollStep << 16 | (unsigned)osdConfig()->sidebar_horizontal_offset << 8 | scroll << 6 | osdConfig()->units << 4;
+    if (configuration != *configured) {
+        int width;
+        uint8_t options = osdCanvasSidebarGetOptions(&width, scroll);
+        uint8_t ex;
+        uint8_t ey;
+        osdCrosshairPosition(&ex, &ey);
+        const int height = 2 * OSD_AH_SIDEBAR_HEIGHT_POS * OSD_CHAR_HEIGHT;
+        const int y = (ey - OSD_AH_SIDEBAR_HEIGHT_POS) * OSD_CHAR_HEIGHT;
+
+        widgetSidebarConfiguration_t config = {
+            .rect.y = y,
+            .rect.w = width,
+            .rect.h = height,
+            .options = options,
+            .divisions = OSD_AH_SIDEBAR_HEIGHT_POS * 2,
+        };
+        uint16_t countsPerStep;
+        osdCanvasSidebarGetUnit(&config.unit, &countsPerStep, scroll);
+
+        int center = ex * OSD_CHAR_WIDTH;
+        int horizontalOffset = OSD_AH_SIDEBAR_WIDTH_POS * OSD_CHAR_WIDTH + osdConfig()->sidebar_horizontal_offset;
+        if (instance == WIDGET_SIDEBAR_LEFT_INSTANCE) {
+            config.rect.x = MAX(center - horizontalOffset - width, 0);
+            config.options |= DISPLAY_WIDGET_SIDEBAR_OPTION_LEFT;
+        } else {
+            config.rect.x = MIN(center + horizontalOffset, canvas->width - width - 1);
+        }
+
+        if (scrollStep > 0) {
+            countsPerStep = scrollStep;
+        }
+        config.counts_per_step = config.unit.scale * countsPerStep;
+
+        if (!displayWidgetsConfigureSidebar(widgets, instance, &config)) {
+            return false;
+        }
+
+        *configured = configuration;
+    }
+    // Actual drawing
+    int32_t data = osdCanvasSidebarGetValue(scroll);
+    return displayWidgetsDrawSidebar(widgets, instance, data);
+}
+
+bool osdCanvasDrawSidebars(displayPort_t *display, displayCanvas_t *canvas)
+{
+    UNUSED(display);
+
+    static uint32_t leftConfigured = UINT32_MAX;
+    static uint32_t rightConfigured = UINT32_MAX;
+    static timeMs_t nextRedraw = 0;
+
+    timeMs_t now = millis();
+
+    if (now < nextRedraw) {
+        return true;
+    }
+
+    displayWidgets_t widgets;
+    if (displayCanvasGetWidgets(&widgets, canvas)) {
+        if (!osdCanvasDrawSidebar(&leftConfigured, &widgets, canvas,
+            WIDGET_SIDEBAR_LEFT_INSTANCE,
+            osdConfig()->left_sidebar_scroll,
+            osdConfig()->left_sidebar_scroll_step)) {
+            return false;
+        }
+        if (!osdCanvasDrawSidebar(&rightConfigured, &widgets, canvas,
+            WIDGET_SIDEBAR_RIGHT_INSTANCE,
+            osdConfig()->right_sidebar_scroll,
+            osdConfig()->right_sidebar_scroll_step)) {
+            return false;
+        }
+        nextRedraw = now + SIDEBAR_REDRAW_INTERVAL_MS;
+        return true;
+    }
+    return false;
+}
+
 #endif

src/main/io/osd_canvas.h

@@ -33,6 +33,7 @@ typedef struct displayCanvas_s displayCanvas_t;
 typedef struct osdDrawPoint_s osdDrawPoint_t;
 
 void osdCanvasDrawVario(displayPort_t *display, displayCanvas_t *canvas, const osdDrawPoint_t *p, float zvel);
-void osdCanvasDrawDirArrow(displayPort_t *display, displayCanvas_t *canvas, const osdDrawPoint_t *p, float degrees, bool eraseBefore);
+void osdCanvasDrawDirArrow(displayPort_t *display, displayCanvas_t *canvas, const osdDrawPoint_t *p, float degrees);
 void osdCanvasDrawArtificialHorizon(displayPort_t *display, displayCanvas_t *canvas, const osdDrawPoint_t *p, float pitchAngle, float rollAngle);
 void osdCanvasDrawHeadingGraph(displayPort_t *display, displayCanvas_t *canvas, const osdDrawPoint_t *p, int heading);
+bool osdCanvasDrawSidebars(displayPort_t *display, displayCanvas_t *canvas);

src/main/io/osd_common.c

@@ -88,17 +88,14 @@ void osdDrawVario(displayPort_t *display, displayCanvas_t *canvas, const osdDraw
 #endif
 }
 
-void osdDrawDirArrow(displayPort_t *display, displayCanvas_t *canvas, const osdDrawPoint_t *p, float degrees, bool eraseBefore)
+void osdDrawDirArrow(displayPort_t *display, displayCanvas_t *canvas, const osdDrawPoint_t *p, float degrees)
 {
-#if !defined(USE_CANVAS)
-    UNUSED(eraseBefore);
-#endif
     uint8_t gx;
     uint8_t gy;
 
 #if defined(USE_CANVAS)
     if (canvas) {
-        osdCanvasDrawDirArrow(display, canvas, p, degrees, eraseBefore);
+        osdCanvasDrawDirArrow(display, canvas, p, degrees);
     } else {
 #endif
         osdDrawPointGetGrid(&gx, &gy, display, canvas, p);
@@ -139,3 +136,15 @@ void osdDrawHeadingGraph(displayPort_t *display, displayCanvas_t *canvas, const
     }
 #endif
 }
+
+void osdDrawSidebars(displayPort_t *display, displayCanvas_t *canvas)
+{
+#if defined(USE_CANVAS)
+    if (osdCanvasDrawSidebars(display, canvas))  {
+        return;
+    }
+#else
+    UNUSED(canvas);
+#endif
+    osdGridDrawSidebars(display);
+}

src/main/io/osd_common.h

@@ -41,6 +41,9 @@
 #define OSD_HEADING_GRAPH_WIDTH 9
 #define OSD_HEADING_GRAPH_DECIDEGREES_PER_CHAR 225
 
+#define OSD_AH_SIDEBAR_WIDTH_POS 7
+#define OSD_AH_SIDEBAR_HEIGHT_POS 3
+
 typedef struct displayPort_s displayPort_t;
 typedef struct displayCanvas_s displayCanvas_t;
 
@@ -73,8 +76,9 @@ void osdDrawVario(displayPort_t *display, displayCanvas_t *canvas, const osdDraw
 // Draws an arrow at the given point, where 0 degrees points to the top of the viewport and
 // positive angles result in clockwise rotation. If eraseBefore is true, the rect surrouing
 // the arrow will be erased first (need for e.g. the home arrow, since it uses multiple symbols)
-void osdDrawDirArrow(displayPort_t *display, displayCanvas_t *canvas, const osdDrawPoint_t *p, float degrees, bool eraseBefore);
+void osdDrawDirArrow(displayPort_t *display, displayCanvas_t *canvas, const osdDrawPoint_t *p, float degrees);
 void osdDrawArtificialHorizon(displayPort_t *display, displayCanvas_t *canvas, const osdDrawPoint_t *p, float rollAngle, float pitchAngle);
 // Draws a heading graph with heading given as 0.1 degree steps i.e. [0, 3600). It uses 9 horizontal
 // grid slots.
 void osdDrawHeadingGraph(displayPort_t *display, displayCanvas_t *canvas, const osdDrawPoint_t *p, int heading);
+void osdDrawSidebars(displayPort_t *display, displayCanvas_t *canvas);

src/main/io/osd_dji_hd.c

@@ -303,7 +303,7 @@ static void djiSerializeOSDConfigReply(sbuf_t *dst)
             // 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
-            uint16_t itemPos = osdConfig()->item_pos[0][inavOSDIdx];
+            uint16_t itemPos = osdLayoutsConfig()->item_pos[0][inavOSDIdx];
 
             // Workarounds for certain OSD element positions
             // INAV calculates these dynamically, while DJI expects the config to have defined coordinates

src/main/io/osd_grid.c

@@ -35,10 +35,26 @@
 
 #include "drivers/display.h"
 #include "drivers/osd_symbols.h"
+#include "drivers/time.h"
 
 #include "io/osd.h"
 #include "io/osd_common.h"
 
+#include "navigation/navigation.h"
+
+typedef enum {
+    OSD_SIDEBAR_ARROW_NONE,
+    OSD_SIDEBAR_ARROW_UP,
+    OSD_SIDEBAR_ARROW_DOWN,
+} osd_sidebar_arrow_e;
+
+typedef struct osd_sidebar_s {
+    int32_t offset;
+    timeMs_t updated;
+    osd_sidebar_arrow_e arrow;
+    uint8_t idle;
+} osd_sidebar_t;
+
 void osdGridDrawVario(displayPort_t *display, unsigned gx, unsigned gy, float zvel)
 {
     int v = zvel / OSD_VARIO_CM_S_PER_ARROW;
@@ -197,4 +213,113 @@ void osdGridDrawHeadingGraph(displayPort_t *display, unsigned gx, unsigned gy, i
     displayWrite(display, gx, gy, buf);
 }
 
+static uint8_t osdUpdateSidebar(osd_sidebar_scroll_e scroll, osd_sidebar_t *sidebar, timeMs_t currentTimeMs)
+{
+    // Scroll between SYM_AH_DECORATION_MIN and SYM_AH_DECORATION_MAX.
+    // Zero scrolling should draw SYM_AH_DECORATION.
+    uint8_t decoration = SYM_AH_DECORATION;
+    int offset;
+    int steps;
+    switch (scroll) {
+        case OSD_SIDEBAR_SCROLL_NONE:
+            sidebar->arrow = OSD_SIDEBAR_ARROW_NONE;
+            sidebar->offset = 0;
+            return decoration;
+        case OSD_SIDEBAR_SCROLL_ALTITUDE:
+            // Move 1 char for every 20cm
+            offset = osdGetAltitude();
+            steps = offset / 20;
+            break;
+        case OSD_SIDEBAR_SCROLL_GROUND_SPEED:
+#if defined(USE_GPS)
+            offset = gpsSol.groundSpeed;
+#else
+            offset = 0;
+#endif
+            // Move 1 char for every 20 cm/s
+            steps = offset / 20;
+            break;
+        case OSD_SIDEBAR_SCROLL_HOME_DISTANCE:
+#if defined(USE_GPS)
+            offset = GPS_distanceToHome;
+#else
+            offset = 0;
+#endif
+            // Move 1 char for every 5m
+            steps = offset / 5;
+            break;
+    }
+    if (offset) {
+        decoration -= steps % SYM_AH_DECORATION_COUNT;
+        if (decoration > SYM_AH_DECORATION_MAX) {
+            decoration -= SYM_AH_DECORATION_COUNT;
+        } else if (decoration < SYM_AH_DECORATION_MIN) {
+            decoration += SYM_AH_DECORATION_COUNT;
+        }
+    }
+    if (currentTimeMs - sidebar->updated > 100) {
+        if (offset > sidebar->offset) {
+            sidebar->arrow = OSD_SIDEBAR_ARROW_UP;
+            sidebar->idle = 0;
+        } else if (offset < sidebar->offset) {
+            sidebar->arrow = OSD_SIDEBAR_ARROW_DOWN;
+            sidebar->idle = 0;
+        } else {
+            if (sidebar->idle > 3) {
+                sidebar->arrow = OSD_SIDEBAR_ARROW_NONE;
+            } else {
+                sidebar->idle++;
+            }
+        }
+        sidebar->offset = offset;
+        sidebar->updated = currentTimeMs;
+    }
+    return decoration;
+}
+
+void osdGridDrawSidebars(displayPort_t *display)
+{
+    uint8_t elemPosX;
+    uint8_t elemPosY;
+
+    osdCrosshairPosition(&elemPosX, &elemPosY);
+
+    static osd_sidebar_t left;
+    static osd_sidebar_t right;
+
+    timeMs_t currentTimeMs = millis();
+    uint8_t leftDecoration = osdUpdateSidebar(osdConfig()->left_sidebar_scroll, &left, currentTimeMs);
+    uint8_t rightDecoration = osdUpdateSidebar(osdConfig()->right_sidebar_scroll, &right, currentTimeMs);
+
+    const int hudwidth = OSD_AH_SIDEBAR_WIDTH_POS;
+    const int hudheight = OSD_AH_SIDEBAR_HEIGHT_POS;
+
+    // Arrows
+    if (osdConfig()->sidebar_scroll_arrows) {
+        displayWriteChar(display, elemPosX - hudwidth, elemPosY - hudheight - 1,
+            left.arrow == OSD_SIDEBAR_ARROW_UP ? SYM_AH_DECORATION_UP : SYM_BLANK);
+
+        displayWriteChar(display, elemPosX + hudwidth, elemPosY - hudheight - 1,
+            right.arrow == OSD_SIDEBAR_ARROW_UP ? SYM_AH_DECORATION_UP : SYM_BLANK);
+
+        displayWriteChar(display, elemPosX - hudwidth, elemPosY + hudheight + 1,
+            left.arrow == OSD_SIDEBAR_ARROW_DOWN ? SYM_AH_DECORATION_DOWN : SYM_BLANK);
+
+        displayWriteChar(display, elemPosX + hudwidth, elemPosY + hudheight + 1,
+            right.arrow == OSD_SIDEBAR_ARROW_DOWN ? SYM_AH_DECORATION_DOWN : SYM_BLANK);
+    }
+
+    // Draw AH sides
+    int leftX = MAX(elemPosX - hudwidth - osdConfig()->sidebar_horizontal_offset, 0);
+    int rightX = MIN(elemPosX + hudwidth + osdConfig()->sidebar_horizontal_offset, display->cols - 1);
+    for (int y = -hudheight; y <= hudheight; y++) {
+        displayWriteChar(display, leftX, elemPosY + y, leftDecoration);
+        displayWriteChar(display, rightX, elemPosY + y, rightDecoration);
+    }
+
+    // AH level indicators
+    displayWriteChar(display, leftX + 1, elemPosY, SYM_AH_RIGHT);
+    displayWriteChar(display, rightX - 1, elemPosY, SYM_AH_LEFT);
+}
+
 #endif

src/main/io/osd_grid.h

@@ -32,3 +32,4 @@ void osdGridDrawVario(displayPort_t *display, unsigned gx, unsigned gy, float zv
 void osdGridDrawDirArrow(displayPort_t *display, unsigned gx, unsigned gy, float degrees);
 void osdGridDrawArtificialHorizon(displayPort_t *display, unsigned gx, unsigned gy, float pitchAngle, float rollAngle);
 void osdGridDrawHeadingGraph(displayPort_t *display, unsigned gx, unsigned gy, int heading);
+void osdGridDrawSidebars(displayPort_t *display);

src/main/io/osd_hud.c

@@ -28,6 +28,7 @@
 #include "io/osd_hud.h"
 
 #include "drivers/display.h"
+#include "drivers/display_canvas.h"
 #include "drivers/osd.h"
 #include "drivers/osd_symbols.h"
 #include "drivers/time.h"
@@ -215,7 +216,7 @@ void osdHudDrawPoi(uint32_t poiDistance, int16_t poiDirection, int32_t poiAltitu
 /*
  * Draw the crosshair
  */
-void osdHudDrawCrosshair(uint8_t px, uint8_t py)
+void osdHudDrawCrosshair(displayCanvas_t *canvas, uint8_t px, uint8_t py)
 {
     static const uint16_t crh_style_all[] = {
         SYM_AH_CH_LEFT, SYM_AH_CH_CENTER, SYM_AH_CH_RIGHT,
@@ -227,11 +228,21 @@ void osdHudDrawCrosshair(uint8_t px, uint8_t py)
         SYM_AH_CH_TYPE7, SYM_AH_CH_TYPE7 + 1, SYM_AH_CH_TYPE7 + 2,
     };
 
+    // Center on the screen
+    if (canvas) {
+        displayCanvasContextPush(canvas);
+        displayCanvasCtmTranslate(canvas, -canvas->gridElementWidth / 2, -canvas->gridElementHeight / 2);
+    }
+
     uint8_t crh_crosshair = (osd_crosshairs_style_e)osdConfig()->crosshairs_style;
 
     displayWriteChar(osdGetDisplayPort(), px - 1, py,crh_style_all[crh_crosshair * 3]);
     displayWriteChar(osdGetDisplayPort(), px, py, crh_style_all[crh_crosshair * 3 + 1]);
     displayWriteChar(osdGetDisplayPort(), px + 1, py, crh_style_all[crh_crosshair * 3 + 2]);
+
+    if (canvas) {
+        displayCanvasContextPop(canvas);
+    }
 }
 
 

src/main/io/osd_hud.h

@@ -19,9 +19,11 @@
 
 #include <stdint.h>
 
+typedef struct displayCanvas_s displayCanvas_t;
+
 
 void osdHudClear(void);
-void osdHudDrawCrosshair(uint8_t px, uint8_t py);
+void osdHudDrawCrosshair(displayCanvas_t *canvas, 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, uint8_t poiType, uint16_t poiSymbol, int16_t poiP1, int16_t poiP2);
 void osdHudDrawExtras(uint8_t poi_id);

src/main/io/serial.h

@@ -74,7 +74,10 @@ typedef enum {
     BAUD_1000000,
     BAUD_1500000,
     BAUD_2000000,
-    BAUD_2470000
+    BAUD_2470000,
+
+    BAUD_MIN = BAUD_AUTO,
+    BAUD_MAX = BAUD_2470000,
 } baudRate_e;
 
 extern const uint32_t baudRates[];