add as extra icon

Author: druckgott

docs/OSD.md

@@ -197,6 +197,7 @@ Here are the OSD Elements provided by INAV.
 | 163 | OSD_COURSE_TO_FENCE                              | 8.0.0  |       |
 | 164 | OSD_H_DIST_TO_FENCE                              | 8.0.0  |       |
 | 165 | OSD_V_DIST_TO_FENCE                              | 8.0.0  |       |
+| 166 | OSD_FLIGHT_DIR                                   | 9.0.0  |       |
 
 # Pilot Logos
 

src/main/cms/cms_menu_osd.c

@@ -204,6 +204,7 @@ static const OSD_Entry menuOsdElemsEntries[] =
     OSD_ELEMENT_ENTRY("BATT % REM", OSD_BATTERY_REMAINING_PERCENT),
 #ifdef USE_GPS
     OSD_ELEMENT_ENTRY("HOME DIR", OSD_HOME_DIR),
+    OSD_ELEMENT_ENTRY("FLIGHT_DIR", OSD_FLIGHT_DIR),
     OSD_ELEMENT_ENTRY("HOME HEAD. ERR", OSD_HOME_HEADING_ERROR),
     OSD_ELEMENT_ENTRY("HOME DIST", OSD_HOME_DIST),
     OSD_ELEMENT_ENTRY("TRIP DIST", OSD_TRIP_DIST),

src/main/io/osd.c

@@ -1958,6 +1958,18 @@ static bool osdDrawSingleElement(uint8_t item)
             return true;
         }
 
+    case OSD_FLIGHT_DIR:
+        {
+            int16_t vx = getEstimatedActualVelocity(X);  // in cm/s
+            int16_t vy = getEstimatedActualVelocity(Y);  // in cm/s
+            int16_t vz = getEstimatedActualVelocity(Z);  // in cm/s
+            float direction_deg = RADIANS_TO_DEGREES(atan2f((float)vy, (float)vx));
+            int16_t altitude_relative = (vz / 100);  // in m
+            //osdHudDrawPoi(0, (int16_t)direction_deg, altitude_relative, 0, SYM_ALERT, 0, 0);
+            osdHudDrawDirection((int16_t)direction_deg, altitude_relative, SYM_ALERT);
+            return true;
+        }
+        
     case OSD_HOME_HEADING_ERROR:
         {
             buff[0] = SYM_HOME;
@@ -4251,6 +4263,7 @@ void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig)
     // 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_FLIGHT_DIR] = OSD_POS(14, 12);
     osdLayoutsConfig->item_pos[0][OSD_ARTIFICIAL_HORIZON] = OSD_POS(8, 6);
     osdLayoutsConfig->item_pos[0][OSD_HORIZON_SIDEBARS] = OSD_POS(8, 6);
 

src/main/io/osd.h

@@ -169,6 +169,7 @@ typedef enum {
     OSD_GPS_LON,
     OSD_GPS_LAT,
     OSD_HOME_DIR,
+    OSD_FLIGHT_DIR,
     OSD_HOME_DIST,
     OSD_HEADING,
     OSD_VARIO,

src/main/io/osd_dji_hd.c

@@ -190,6 +190,7 @@ const djiOsdMapping_t djiOSDItemIndexMap[] = {
     { -1,                                     0 }, // DJI: OSD_MAIN_BATT_USAGE
     { -1,                                     0 }, // DJI: OSD_DISARMED
     { OSD_HOME_DIR,                           FEATURE_GPS }, // DJI: OSD_HOME_DIR
+    { OSD_FLIGHT_DIR,                         FEATURE_GPS }, // DJI: OSD_FLIGHT_DIR
     { OSD_HOME_DIST,                          FEATURE_GPS }, // DJI: OSD_HOME_DIST
     { OSD_HEADING,                            0 }, // DJI: OSD_NUMERICAL_HEADING
     { OSD_VARIO_NUM,                          0 }, // DJI: OSD_NUMERICAL_VARIO

src/main/io/osd_hud.c

@@ -113,6 +113,46 @@ int8_t radarGetNearestPOI(void)
     return poi;
 }
 
+void osdHudDrawDirection(int16_t poiDirection, int32_t poiAltitude, uint16_t poiSymbol)
+{
+    int poi_x = -1;
+    int poi_y = -1;
+    uint8_t center_x, center_y;
+
+    uint8_t minX = osdConfig()->hud_margin_h + 2;
+    uint8_t maxX = osdGetDisplayPort()->cols - osdConfig()->hud_margin_h - 3;
+    uint8_t minY = osdConfig()->hud_margin_v;
+    uint8_t maxY = osdGetDisplayPort()->rows - osdConfig()->hud_margin_v - 2;
+
+    osdCrosshairPosition(&center_x, &center_y);
+
+    if (osdConfig()->pan_servo_pwm2centideg != 0) {
+        poiDirection += osdGetPanServoOffset();
+    }
+
+    int16_t error_x = hudWrap180(poiDirection - DECIDEGREES_TO_DEGREES(osdGetHeading()));
+
+    if ((error_x > -(osdConfig()->camera_fov_h / 2)) && (error_x < osdConfig()->camera_fov_h / 2)) {
+        // Horizontalposition berechnen
+        float scaled_x = sin_approx(DEGREES_TO_RADIANS(error_x)) / sin_approx(DEGREES_TO_RADIANS(osdConfig()->camera_fov_h / 2));
+        poi_x = center_x + 15 * scaled_x;
+
+        if (poi_x >= minX && poi_x <= maxX) {
+            // Vertikalposition berechnen
+            float poi_angle = atan2_approx(-poiAltitude, 1.0f);  // Abstand fest auf 1
+            poi_angle = RADIANS_TO_DEGREES(poi_angle);
+            int16_t plane_angle = attitude.values.pitch / 10;
+            int camera_angle = osdConfig()->camera_uptilt;
+            int16_t error_y = poi_angle - plane_angle + camera_angle;
+            float scaled_y = sin_approx(DEGREES_TO_RADIANS(error_y)) / sin_approx(DEGREES_TO_RADIANS(osdConfig()->camera_fov_v / 2));
+            poi_y = constrain(center_y + (osdGetDisplayPort()->rows / 2) * scaled_y, minY, maxY - 1);
+
+            // Symbol zeichnen
+            osdHudWrite(poi_x, poi_y, poiSymbol, 1);
+        }
+    }
+}
+
 /*
  * Display a POI as a 3D-marker on the hud
  * Distance (m), Direction (°), Altitude (relative, m, negative means below), Heading (°),

src/main/io/osd_hud.h

@@ -26,5 +26,6 @@ typedef struct displayCanvas_s displayCanvas_t;
 void osdHudClear(void);
 void osdHudDrawCrosshair(displayCanvas_t *canvas, uint8_t px, uint8_t py);
 void osdHudDrawHoming(uint8_t px, uint8_t py);
+void osdHudDrawDirection(int16_t poiDirection, int32_t poiAltitude, uint16_t poiSymbol);
 void osdHudDrawPoi(uint32_t poiDistance, int16_t poiDirection, int32_t poiAltitude, uint8_t poiType, uint16_t poiSymbol, int16_t poiP1, int16_t poiP2);
 int8_t radarGetNearestPOI(void);