diff --git a/docs/development/msp/msp_messages.json b/docs/development/msp/msp_messages.json index e8f64d0cb93..ab97f7cfcd5 100644 --- a/docs/development/msp/msp_messages.json +++ b/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": { diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 50ddc2d9f09..f3010640db5 100644 --- a/src/main/fc/fc_msp.c +++ b/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 diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 0f1c239f1f0..d8ee8216305 100644 --- a/src/main/fc/settings.yaml +++ b/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 diff --git a/src/main/io/osd.c b/src/main/io/osd.c index dc592dca874..ee47a47b5d1 100644 --- a/src/main/io/osd.c +++ b/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) diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 4767ae035d6..39fd13d2a23 100644 --- a/src/main/io/osd.h +++ b/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 diff --git a/src/main/msp/msp_protocol_v2_common.h b/src/main/msp/msp_protocol_v2_common.h index 98f782b4896..be22187fdca 100644 --- a/src/main/msp/msp_protocol_v2_common.h +++ b/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 diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 65b66d778ce..7771ca0af19 100644 --- a/src/main/navigation/navigation.h +++ b/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 {