diff --git a/docs/Settings.md b/docs/Settings.md index 5695c1ff32b..f5c246d0871 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -4972,6 +4972,16 @@ Number of leading digits removed from plus code. Removing 2, 4 and 6 digits requ --- +### osd_radar_peers_display_time + +Time in seconds to display next peer + +| Default | Min | Max | +| --- | --- | --- | +| 3 | 1 | 10 | + +--- + ### osd_right_sidebar_scroll Scroll type for the right sidebar diff --git a/src/main/common/maths.c b/src/main/common/maths.c index b8ec59c33a2..993634d902d 100644 --- a/src/main/common/maths.c +++ b/src/main/common/maths.c @@ -123,6 +123,15 @@ int32_t wrap_18000(int32_t angle) return angle; } +int16_t wrap_180(int16_t angle) +{ + if (angle > 180) + angle -= 360; + if (angle < -180) + angle += 360; + return angle; +} + int32_t wrap_36000(int32_t angle) { if (angle >= 36000) diff --git a/src/main/common/maths.h b/src/main/common/maths.h index ce96b7064fd..b8d3803613f 100644 --- a/src/main/common/maths.h +++ b/src/main/common/maths.h @@ -166,6 +166,7 @@ int scaleRange(int x, int srcMin, int srcMax, int destMin, int destMax); float scaleRangef(float x, float srcMin, float srcMax, float destMin, float destMax); int32_t wrap_18000(int32_t angle); +int16_t wrap_180(int16_t angle); int32_t wrap_36000(int32_t angle); int32_t quickMedianFilter3(int32_t * v); diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 80fa21048a8..d8f15f0921c 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -3486,6 +3486,12 @@ groups: min: 1 max: 10 default_value: 3 + - name: osd_radar_peers_display_time + description: "Time in seconds to display next peer " + field: radar_peers_display_time + min: 1 + max: 10 + default_value: 3 - 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 1b183ff58f7..d682c0299ff 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, 11); +PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 12); PG_REGISTER_WITH_RESET_FN(osdLayoutsConfig_t, osdLayoutsConfig, PG_OSD_LAYOUTS_CONFIG, 1); void osdStartedSaveProcess(void) { @@ -2513,6 +2513,121 @@ static bool osdDrawSingleElement(uint8_t item) } #endif + case OSD_FORMATION_FLIGHT: + { + static uint8_t currentPeerIndex = 0; + static timeMs_t lastPeerSwitch; + + if ((STATE(GPS_FIX) && isImuHeadingValid())) { + 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); + if (radar_pois[nextPeerIndex].gps.lat != 0 && radar_pois[nextPeerIndex].gps.lon != 0 && radar_pois[nextPeerIndex].state < 2) { + currentPeerIndex = nextPeerIndex; + break; + } + } + } + + radar_pois_t *currentPeer = &(radar_pois[currentPeerIndex]); + if (currentPeer->gps.lat != 0 && currentPeer->gps.lon != 0 && currentPeer->state < 2) { + fpVector3_t poi; + geoConvertGeodeticToLocal(&poi, &posControl.gpsOrigin, ¤tPeer->gps, GEO_ALT_RELATIVE); + + currentPeer->distance = calculateDistanceToDestination(&poi) / 100; // In m + currentPeer->altitude = (int16_t )((currentPeer->gps.alt - osdGetAltitudeMsl()) / 100); + currentPeer->direction = (int16_t )(calculateBearingToDestination(&poi) / 100); // In ° + + int16_t panServoDirOffset = 0; + if (osdConfig()->pan_servo_pwm2centideg != 0){ + panServoDirOffset = osdGetPanServoOffset(); + } + + //line 1 + //[peer heading][peer ID][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); + + //[LQ] + displayWriteChar(osdDisplayPort, elemPosX + 2, elemPosY, SYM_HUD_SIGNAL_0 + currentPeer->lq); + + //[direction to peer] + int directionToPeerError = wrap_180(osdGetHeadingAngle(currentPeer->direction) + panServoDirOffset - (int)DECIDEGREES_TO_DEGREES(osdGetHeading())); + uint16_t iconIndexOffset = constrain(((directionToPeerError + 180) / 30), 0, 12); + if (iconIndexOffset == 12) { + iconIndexOffset = 0; // Directly behind + } + displayWriteChar(osdDisplayPort, elemPosX + 3, elemPosY, SYM_HUD_CARDINAL + iconIndexOffset); + + + //line 2 + switch ((osd_unit_e)osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_IMPERIAL: + osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(currentPeer->distance * 100), FEET_PER_MILE, 0, 4, 4, false); + break; + case OSD_UNIT_GA: + osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(currentPeer->distance * 100), (uint32_t)FEET_PER_NAUTICALMILE, 0, 4, 4, false); + break; + default: + FALLTHROUGH; + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; + case OSD_UNIT_METRIC: + osdFormatCentiNumber(buff, currentPeer->distance * 100, METERS_PER_KILOMETER, 0, 4, 4, false); + break; + } + displayWrite(osdDisplayPort, elemPosX, elemPosY + 1, buff); + + + //line 3 + displayWriteChar(osdDisplayPort, elemPosX, elemPosY + 2, (currentPeer->altitude >= 0) ? SYM_AH_DECORATION_UP : SYM_AH_DECORATION_DOWN); + + int altc = currentPeer->altitude; + switch ((osd_unit_e)osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_GA: + FALLTHROUGH; + case OSD_UNIT_IMPERIAL: + // Convert to feet + altc = CENTIMETERS_TO_FEET(altc * 100); + break; + default: + FALLTHROUGH; + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; + case OSD_UNIT_METRIC: + // Already in metres + break; + } + + altc = ABS(constrain(altc, -999, 999)); + tfp_sprintf(buff, "%3d", altc); + displayWrite(osdDisplayPort, elemPosX + 1, elemPosY + 2, buff); + + return true; + } + } + + //clear screen + for(uint8_t i = 0; i < 4; i++){ + displayWriteChar(osdDisplayPort, elemPosX + i, elemPosY, SYM_BLANK); + displayWriteChar(osdDisplayPort, elemPosX + i, elemPosY + 1, SYM_BLANK); + displayWriteChar(osdDisplayPort, elemPosX + i, elemPosY + 2, SYM_BLANK); + } + + return true; + } + case OSD_CROSSHAIRS: // Hud is a sub-element of the crosshair osdCrosshairPosition(&elemPosX, &elemPosY); @@ -3930,7 +4045,9 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig, .stats_energy_unit = SETTING_OSD_STATS_ENERGY_UNIT_DEFAULT, .stats_page_auto_swap_time = SETTING_OSD_STATS_PAGE_AUTO_SWAP_TIME_DEFAULT, - .stats_show_metric_efficiency = SETTING_OSD_STATS_SHOW_METRIC_EFFICIENCY_DEFAULT + .stats_show_metric_efficiency = SETTING_OSD_STATS_SHOW_METRIC_EFFICIENCY_DEFAULT, + + .radar_peers_display_time = SETTING_OSD_RADAR_PEERS_DISPLAY_TIME_DEFAULT ); void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig) diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 7ac14054994..a73db015793 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -283,9 +283,10 @@ typedef enum { OSD_CUSTOM_ELEMENT_1, OSD_CUSTOM_ELEMENT_2, OSD_CUSTOM_ELEMENT_3, - OSD_ADSB_WARNING, + OSD_ADSB_WARNING, //150 OSD_ADSB_INFO, OSD_BLACKBOX, + OSD_FORMATION_FLIGHT, //153 OSD_ITEM_COUNT // MUST BE LAST } osd_items_e; @@ -460,11 +461,12 @@ typedef struct osdConfig_s { #ifndef DISABLE_MSP_DJI_COMPAT bool highlight_djis_missing_characters; // If enabled, show question marks where there is no character in DJI's font to represent an OSD element symbol #endif -#ifdef USE_ADSB + #ifdef USE_ADSB uint16_t adsb_distance_warning; // in metres uint16_t adsb_distance_alert; // in metres uint16_t adsb_ignore_plane_above_me_limit; // in metres -#endif + #endif + uint8_t radar_peers_display_time; // in seconds } osdConfig_t; PG_DECLARE(osdConfig_t, osdConfig);