feat: Formation Flight - possibility to show peer name

docs/development/msp/msp_messages.json

@@ -6469,6 +6469,31 @@
             "notes": "Returns the stored GPS coordinates for all radar POIs (`radar_pois[i].gps`).",
             "description": "Provides the GPS positions (latitude, longitude, altitude) for each radar point of interest." 
         },
+        "MSP2_COMMON_SET_RADAR_PEER_NAME": {
+            "code": 4112,
+            "mspv": 2,
+            "request": {
+                "payload": [
+                    {
+                        "name": "poiIndex",
+                        "ctype": "uint8_t",
+                        "desc": "Index of the POI slot (0 to `RADAR_MAX_POIS - 1`)",
+                        "units": "Index"
+                    },
+                    {
+                        "name": "peerName",
+                        "ctype": "char",
+                        "desc": "Peer name, copied verbatim into `radar_pois[i].name`, up to `RADAR_PEER_NAME_LENGTH` (3) bytes, length implied by the payload size. The name is not null-terminated if all `RADAR_PEER_NAME_LENGTH` bytes are non-zero",
+                        "array": true,
+                        "array_size": 0
+                    }
+                ]
+            },
+            "reply": null,
+            "variable_len": true,
+            "notes": "Expects 1 to `RADAR_PEER_NAME_LENGTH + 1` (4) bytes. Returns an error if POI index is not less than `RADAR_MAX_POIS`. Used to display the peer name instead of the peer ID letter in the formation flight OSD element when `osdConfig()->show_radar_peer_name` (`MSP2_INAV_OSD_PREFERENCES` / `MSP2_INAV_OSD_SET_PREFERENCES`) is enabled.",
+            "description": "Sets the display name for a \"radar\" Point of Interest (POI), used for the formation flight OSD element."
+        },
         "MSP2_SENSOR_RANGEFINDER": {
             "code": 7937,
             "mspv": 2,
@@ -8355,10 +8380,23 @@
                         "desc": "Enum `osd_stats_energy_unit_e`: Unit for energy display in post-flight stats (`osdConfig()->stats_energy_unit`)",
                         "units": "",
                         "enum": "osd_stats_energy_unit_e"
+                    },
+                    {
+                        "name": "showRadarPeerName",
+                        "ctype": "uint8_t",
+                        "desc": "Boolean: Show the radar peer name instead of the peer ID letter in the formation flight OSD element (`osdConfig()->show_radar_peer_name`)",
+                        "units": ""
+                    },
+                    {
+                        "name": "adsbWarningStyle",
+                        "ctype": "uint8_t",
+                        "desc": "Enum `osd_adsb_warning_style_e`: ADSB warning element style, one or two lines (`osdConfig()->adsb_warning_style`). 0 if `USE_ADSB` disabled",
+                        "units": "",
+                        "enum": "osd_adsb_warning_style_e"
                     }
                 ]
             },
-            "notes": "Requires `USE_OSD`.",
+            "notes": "Requires `USE_OSD`. Expects 11 bytes.",
             "description": "Retrieves OSD display preferences (video system, units, styles, etc.)."
         },
         "MSP2_INAV_OSD_SET_PREFERENCES": {
@@ -8425,11 +8463,25 @@
                         "desc": "Sets `osdConfigMutable()->stats_energy_unit`",
                         "units": "",
                         "enum": "osd_stats_energy_unit_e"
+                    },
+                    {
+                        "name": "showRadarPeerName",
+                        "ctype": "uint8_t",
+                        "desc": "Sets `osdConfigMutable()->show_radar_peer_name`",
+                        "units": ""
+                    },
+                    {
+                        "name": "adsbWarningStyle",
+                        "ctype": "uint8_t",
+                        "desc": "Sets `osdConfigMutable()->adsb_warning_style`. Only read if `USE_ADSB` is compiled in and `dataSize` includes this trailing byte",
+                        "units": "",
+                        "enum": "osd_adsb_warning_style_e",
+                        "optional": true
                     }
                 ]
             },
             "reply": null,
-            "notes": "Requires `USE_OSD`. Expects 9 bytes. Triggers a full OSD redraw.",
+            "notes": "Requires `USE_OSD`. Expects 10 bytes, or 11 bytes if `USE_ADSB` is compiled in and `adsbWarningStyle` is included. Triggers a full OSD redraw.",
             "description": "Sets OSD display preferences."
         },
         "MSP2_INAV_SELECT_BATTERY_PROFILE": {

src/main/fc/fc_msp.c

@@ -1726,6 +1726,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
         sbufWriteU8(dst, osdConfig()->sidebar_scroll_arrows);
         sbufWriteU8(dst, osdConfig()->units);
         sbufWriteU8(dst, osdConfig()->stats_energy_unit);
+        sbufWriteU8(dst, osdConfig()->show_radar_peer_name);
 #ifdef USE_ADSB
         sbufWriteU8(dst, osdConfig()->adsb_warning_style);
 #else
@@ -3257,16 +3258,45 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
         break;
     case MSP2_COMMON_SET_RADAR_POS:
         if (dataSize == 19) {
-            const uint8_t msp_radar_no = MIN(sbufReadU8(src), RADAR_MAX_POIS - 1); // Radar poi number, 0 to 3
-            radar_pois[msp_radar_no].state = sbufReadU8(src);                      // 0=undefined, 1=armed, 2=lost
-            radar_pois[msp_radar_no].gps.lat = sbufReadU32(src);                   // lat 10E7
-            radar_pois[msp_radar_no].gps.lon = sbufReadU32(src);                   // lon 10E7
-            radar_pois[msp_radar_no].gps.alt = sbufReadU32(src);                   // altitude (cm)
-            radar_pois[msp_radar_no].heading = sbufReadU16(src);                   // °
-            radar_pois[msp_radar_no].speed = sbufReadU16(src);                     // cm/s
-            radar_pois[msp_radar_no].lq = sbufReadU8(src);                         // Link quality, from 0 to 4
-        } else
+            const uint8_t msp_radar_no = sbufReadU8(src); // Radar poi number, 0 to 5
+            if(msp_radar_no < RADAR_MAX_POIS) {
+                radar_pois[msp_radar_no].state = sbufReadU8(src);                      // 0=undefined, 1=armed, 2=lost
+
+                if (radar_pois[msp_radar_no].state == 0) {
+                    memset(radar_pois[msp_radar_no].name, 0, sizeof(radar_pois[msp_radar_no].name));
+                }
+
+                radar_pois[msp_radar_no].gps.lat = sbufReadU32(src);                   // lat 10E7
+                radar_pois[msp_radar_no].gps.lon = sbufReadU32(src);                   // lon 10E7
+                radar_pois[msp_radar_no].gps.alt = sbufReadU32(src);                   // altitude (cm)
+                radar_pois[msp_radar_no].heading = sbufReadU16(src);                   // °
+                radar_pois[msp_radar_no].speed = sbufReadU16(src);                     // cm/s
+                radar_pois[msp_radar_no].lq = sbufReadU8(src);                         // Link quality, from 0 to 4
+            }else{
+                return MSP_RESULT_ERROR;
+            }
+        } else {
             return MSP_RESULT_ERROR;
+        }
+        break;
+
+    case MSP2_COMMON_SET_RADAR_PEER_NAME:
+        if (dataSize > 3) {
+            const uint8_t msp_radar_no = sbufReadU8(src); // Radar poi number, 0 to 5
+            if (msp_radar_no < RADAR_MAX_POIS) {
+                const uint8_t msp_peer_max_length = sbufReadU8(src);
+                const uint8_t nameLength = MIN(msp_peer_max_length, (uint8_t)(sizeof(radar_pois[msp_radar_no].name)));
+
+                // clear current name, leaves the trailing '\0' in place regardless of nameLength
+                memset(radar_pois[msp_radar_no].name, 0, sizeof(radar_pois[msp_radar_no].name));
+                // ignored (no-op) if the peer's claimed length exceeds what was actually sent
+                sbufReadDataSafe(src, radar_pois[msp_radar_no].name, nameLength);
+            } else {
+                return MSP_RESULT_ERROR;
+            }
+        } else {
+            return MSP_RESULT_ERROR;
+        }
         break;
 
     case MSP_SET_FEATURE:
@@ -3653,9 +3683,9 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
     case MSP2_INAV_OSD_SET_PREFERENCES:
         {
             if (
-                    dataSize == 9
+                    dataSize == 10
 #ifdef USE_ADSB
-                    || dataSize == 10
+                    || dataSize == 11
 #endif
             ) {
                 osdConfigMutable()->video_system = sbufReadU8(src);
@@ -3667,8 +3697,9 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
                 osdConfigMutable()->sidebar_scroll_arrows = sbufReadU8(src);
                 osdConfigMutable()->units = sbufReadU8(src);
                 osdConfigMutable()->stats_energy_unit = sbufReadU8(src);
+                osdConfigMutable()->show_radar_peer_name = sbufReadU8(src);
 #ifdef USE_ADSB
-                if(dataSize == 10) {
+                if(dataSize == 11) {
                     osdConfigMutable()->adsb_warning_style = sbufReadU8(src);
                 }
 #endif

src/main/fc/settings.yaml

@@ -3683,6 +3683,11 @@ groups:
         min: 1
         max: 10
         default_value: 3
+      - name: osd_show_radar_peer_name
+        description: "Show the radar peer name (set with MSP2_COMMON_SET_RADAR_PEER_NAME) in the formation flight OSD element instead of the peer ID letter (A, B, C...)"
+        default_value: OFF
+        field: show_radar_peer_name
+        type: bool
       - name: osd_hud_wp_disp
         description: "How many navigation waypoints are displayed, set to 0 (zero) to disable. As sample, if set to 2, and you just passed the 3rd waypoint of the mission, you'll see markers for the 4th waypoint (marked 1) and the 5th waypoint (marked 2)"
         default_value: 0

src/main/io/osd.c

@@ -224,7 +224,7 @@ static bool osdDisplayHasCanvas;
 
 #define AH_MAX_PITCH_DEFAULT 20 // Specify default maximum AHI pitch value displayed (degrees)
 
-PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 0);
+PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 1);
 PG_REGISTER_WITH_RESET_FN(osdLayoutsConfig_t, osdLayoutsConfig, PG_OSD_LAYOUTS_CONFIG, 3);
 
 /* OSD formatting helpers replacing common tfp_sprintf patterns
@@ -2824,8 +2824,8 @@ static bool osdDrawSingleElement(uint8_t item)
             if ((radar_pois[currentPeerIndex].gps.lat == 0 || radar_pois[currentPeerIndex].gps.lon == 0 || radar_pois[currentPeerIndex].state >= 2) || (millis() > (osdConfig()->radar_peers_display_time * 1000) + lastPeerSwitch)) {
                 lastPeerSwitch = millis();
 
-                for(uint8_t i = 1; i < RADAR_MAX_POIS - 1; i++) {
-                    uint8_t nextPeerIndex = (currentPeerIndex + i) % (RADAR_MAX_POIS - 1);
+                for(uint8_t i = 0; i < RADAR_MAX_POIS; i++) {
+                    uint8_t nextPeerIndex = (currentPeerIndex + i + 1) % RADAR_MAX_POIS;
                     if (radar_pois[nextPeerIndex].gps.lat != 0 && radar_pois[nextPeerIndex].gps.lon != 0 && radar_pois[nextPeerIndex].state < 2) {
                         currentPeerIndex = nextPeerIndex;
                         break;
@@ -2848,21 +2848,36 @@ static bool osdDrawSingleElement(uint8_t item)
                 }
 
                 //line 1
-                //[peer heading][peer ID][LQ][direction to peer]
+                //[peer heading][peer ID or name][LQ][direction to peer]
 
                 //[peer heading]
                 int relativePeerHeading = osdGetHeadingAngle(currentPeer->heading - (int)DECIDEGREES_TO_DEGREES(osdGetHeading()));
                 displayWriteChar(osdDisplayPort, elemPosX, elemPosY, SYM_DECORATION + ((relativePeerHeading + 22) / 45) % 8);
 
-                //[peer ID]
-                displayWriteChar(osdDisplayPort, elemPosX + 1, elemPosY, 65 + currentPeerIndex);
+                //[peer ID or name]
+                uint8_t peerIdWidth = 1;
+                if (osdConfig()->show_radar_peer_name && currentPeer->name[0] != '\0') {
+                    peerIdWidth = RADAR_PEER_NAME_LENGTH;
+                    for (uint8_t i = 0; i < RADAR_PEER_NAME_LENGTH; i++) {
+                        const char nameChar = currentPeer->name[i];
+                        displayWriteChar(osdDisplayPort, elemPosX + 1 + i, elemPosY, nameChar != '\0' ? sl_toupper((unsigned char)nameChar) : SYM_BLANK);
+                    }
+                } else if (osdConfig()->show_radar_peer_name) {
+                    peerIdWidth = RADAR_PEER_NAME_LENGTH; // keep LQ and direction at fixed positions while cycling through peers
+                    displayWriteChar(osdDisplayPort, elemPosX + 1, elemPosY, 65 + currentPeerIndex);
+                    for (uint8_t i = 1; i < RADAR_PEER_NAME_LENGTH; i++) {
+                        displayWriteChar(osdDisplayPort, elemPosX + 1 + i, elemPosY, SYM_BLANK);
+                    }
+                } else {
+                    displayWriteChar(osdDisplayPort, elemPosX + 1, elemPosY, 65 + currentPeerIndex);
+                }
 
                 //[LQ]
-                displayWriteChar(osdDisplayPort, elemPosX + 2, elemPosY, SYM_HUD_SIGNAL_0 + currentPeer->lq);
+                displayWriteChar(osdDisplayPort, elemPosX + 1 + peerIdWidth, elemPosY, SYM_HUD_SIGNAL_0 + currentPeer->lq);
 
                 //[direction to peer]
                 int directionToPeerError = osdGetHeadingAngle(currentPeer->direction) + panServoDirOffset - (int)DECIDEGREES_TO_DEGREES(osdGetHeading());
-                osdDrawDirCardinal(osdDisplayPort, elemPosX + 3, elemPosY, directionToPeerError, elemAttr);
+                osdDrawDirCardinal(osdDisplayPort, elemPosX + 2 + peerIdWidth, elemPosY, directionToPeerError, elemAttr);
 
                 //line 2
                 switch ((osd_unit_e)osdConfig()->units) {
@@ -2915,8 +2930,9 @@ static bool osdDrawSingleElement(uint8_t item)
             }
         }
 
-        //clear screen
-        for(uint8_t i = 0; i < 4; i++){
+        //clear screen (use the widest possible layout so a mid-flight config change can't leave stale glyphs)
+        const uint8_t elemWidth = 3 + RADAR_PEER_NAME_LENGTH;
+        for(uint8_t i = 0; i < elemWidth; i++){
             displayWriteChar(osdDisplayPort, elemPosX + i, elemPosY, SYM_BLANK);
             displayWriteChar(osdDisplayPort, elemPosX + i, elemPosY + 1, SYM_BLANK);
             displayWriteChar(osdDisplayPort, elemPosX + i, elemPosY + 2, SYM_BLANK);
@@ -4345,7 +4361,8 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig,
     .stats_page_auto_swap_time = SETTING_OSD_STATS_PAGE_AUTO_SWAP_TIME_DEFAULT,
     .stats_show_metric_efficiency = SETTING_OSD_STATS_SHOW_METRIC_EFFICIENCY_DEFAULT,
 
-    .radar_peers_display_time = SETTING_OSD_RADAR_PEERS_DISPLAY_TIME_DEFAULT
+    .radar_peers_display_time = SETTING_OSD_RADAR_PEERS_DISPLAY_TIME_DEFAULT,
+    .show_radar_peer_name = SETTING_OSD_SHOW_RADAR_PEER_NAME_DEFAULT
 );
 
 void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig)

src/main/io/osd.h

@@ -541,6 +541,7 @@ typedef struct osdConfig_s {
     bool                        adsb_calculation_use_cpa;                  // adsb calculation type, the closest or the closest approach
 #endif
     uint8_t  radar_peers_display_time;                  // in seconds
+    bool     show_radar_peer_name;                      // show the radar peer name instead of the peer ID letter
 #ifdef USE_GEOZONE
     uint8_t geozoneDistanceWarning;                     // Distance to fence or action
     bool geozoneDistanceType;                            // Shows a countdown timer or distance to fence/action

src/main/msp/msp_protocol_v2_common.h

@@ -32,6 +32,7 @@
 // radar commands
 #define MSP2_COMMON_SET_RADAR_POS           0x100B //SET radar position information
 #define MSP2_COMMON_SET_RADAR_ITD           0x100C //SET radar information to display
+#define MSP2_COMMON_SET_RADAR_PEER_NAME     0x1010
 
 #define MSP2_COMMON_SET_MSP_RC_LINK_STATS   0x100D //in message        Sets the MSP RC stats
 #define MSP2_COMMON_SET_MSP_RC_INFO         0x100E //in message        Sets the MSP RC info

src/main/navigation/navigation.h

@@ -561,6 +561,8 @@ typedef struct {
     fpVector3_t poi_pos; // POI location in local coordinates (SET_POI)
 } navWapointHeading_t;
 
+#define RADAR_MAX_POIS 6
+#define RADAR_PEER_NAME_LENGTH 3
 typedef struct radar_pois_s {
     gpsLocation_t gps;
     uint8_t state;
@@ -570,10 +572,9 @@ typedef struct radar_pois_s {
     uint16_t distance; // m
     int16_t altitude; // m
     int16_t direction; // °
+    char name[RADAR_PEER_NAME_LENGTH];
 } radar_pois_t;
 
-#define RADAR_MAX_POIS 5
-
 extern radar_pois_t radar_pois[RADAR_MAX_POIS];
 
 typedef struct {