Merge pull request iNavFlight#9883 from error414/fixed-formationflight

Fixed position for formation flight / inav radar

Author: mmosca

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

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)

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);

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

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, &currentPeer->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)

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);