diff --git a/docs/Settings.md b/docs/Settings.md index 21155e280b..fcc44ca76c 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -4692,6 +4692,16 @@ Horizontal margin (both left and right) for OSD 2D map borders. The map items wo --- +### osd_map2d_ref_line_heading + +Draws a dashed line on the center of the map display, if using "up is north" setting. The line is aligned at the heading set by this parameter. This should be set to the same heading as an easily recognizable feature nearby the place you are flying, for example the same heading as the runway (if flying from a club), street or a nearby fence. So now you are able to compare the craft's position on the OSD map with this reference, making it easier to orientate yourself, better align directional antennas and so on. Default setting of -1 disables this line. + +| Default | Min | Max | +| --- | --- | --- | +| -1 | -1 | 359 | + +--- + ### osd_map2d_vmargin Vertical margin (both top and bottom) for OSD 2D map borders. The map items won't be drawn into any of the lines within this number. diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index d5b93b2465..958f22a9c1 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -3737,8 +3737,15 @@ groups: field: map2d_hmargin default_value: 5 min: 0 - max: 15 - + max: 15 + + - name: osd_map2d_ref_line_heading + description: OSD 2D Map reference line heading (0 is north, 90 east, 180 south and so on, -1 disabled) + field: map2d_ref_line_heading + default_value: -1 + min: -1 + max: 359 + - name: PG_OSD_COMMON_CONFIG type: osdCommonConfig_t headers: ["io/osd_common.h"] diff --git a/src/main/io/osd.c b/src/main/io/osd.c index fa02459b98..142d095310 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1290,6 +1290,41 @@ int osdGetHeadingAngle(int angle) #if defined(USE_GPS) +/* Draws a reference line in the center of screen, based on configurable heading (assuming north is up). + * Code borrowed and simplified from osdGridDrawArtificialHorizon(), since it's essentially the same thing + * as the artificial horizon line, but fixed on the same position and with roll only, no pitch. + */ +static void osdDrawMapReferenceLine(uint8_t midX, uint8_t midY) +{ + float heading = osdConfig()->map2d_ref_line_heading; + float rollAngle = DEGREES_TO_RADIANS(90.f - (heading > 180.0 ? heading - 180.f: heading)); + const float ky = sin_approx(rollAngle); + const float kx = cos_approx(rollAngle); + const float ratio = osdDisplayIsPAL() ? 12.0f/15.0f : 12.0f/18.46f; + + // Reduce the size so as not to be confused with the AHI, if both are used at the same time + static const uint8_t ref_line_width = OSD_AHI_WIDTH - 6U; + static const uint8_t ref_line_height = OSD_AHI_HEIGHT - 4U; + + if (fabsf(ky) < fabsf(kx)) { + for (int8_t dx = -ref_line_width / 2; dx <= ref_line_width / 2; dx++) { + float fy = (ratio * dx) * (ky / kx) + 0.49f; + int8_t dy = floorf(fy); + const uint8_t chX = midX + dx, chY = midY - dy; + uint16_t c = SYM_AH_H_START + ((OSD_AHI_H_SYM_COUNT - 1) - (uint8_t)((fy - dy) * OSD_AHI_H_SYM_COUNT)); + displayWriteChar(osdDisplayPort, chX, chY, c); + } + } else { + for (int8_t dy = -ref_line_height / 2; dy <= ref_line_height / 2; dy++) { + const float fx = (dy / ratio) * (kx / ky) + 0.5f; + const int8_t dx = floorf(fx); + const uint8_t chX = midX + dx, chY = midY - dy; + uint16_t c = SYM_AH_V_START + (fx - dx) * OSD_AHI_V_SYM_COUNT; + displayWriteChar(osdDisplayPort, chX, chY, c); + } + } +} + /* Draws a map with the given symbol in the center and given point of interest * defined by its distance in meters and direction in degrees. * referenceHeading indicates the up direction in the map, in degrees, while @@ -1317,7 +1352,14 @@ static void osdDrawMap(int referenceHeading, uint16_t referenceSym, uint16_t cen uint8_t midX = osdDisplayPort->cols / 2; uint8_t midY = osdDisplayPort->rows / 2; - // Fixed marks + // Fixed marks + if(osdConfig()->map2d_ref_line_heading >= 0 && referenceSym == 'N') { + // Reference line is only drawn if enabled (ref heading == -1 disables it) + // and using North as the map reference + osdDrawMapReferenceLine(midX, midY); + } + + // Draw the center symbol after the reference line so it overwrites the line center character displayWriteChar(osdDisplayPort, midX, midY, centerSym); // First, erase the previous drawing. @@ -3988,7 +4030,8 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig, .stats_show_metric_efficiency = SETTING_OSD_STATS_SHOW_METRIC_EFFICIENCY_DEFAULT, .map2d_vmargin = SETTING_OSD_MAP2D_VMARGIN_DEFAULT, - .map2d_hmargin = SETTING_OSD_MAP2D_HMARGIN_DEFAULT + .map2d_hmargin = SETTING_OSD_MAP2D_HMARGIN_DEFAULT, + .map2d_ref_line_heading = SETTING_OSD_MAP2D_REF_LINE_HEADING_DEFAULT ); void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig) diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 9637fce58f..7593886cb1 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -465,6 +465,7 @@ typedef struct osdConfig_s { uint8_t map2d_vmargin; // Vertical lines margin for 2D map (lines where nothing will be drawn) uint8_t map2d_hmargin; // Horizontal lines margin for 2D map + int16_t map2d_ref_line_heading; // Reference line heading (0 to 360, -1 to disable) } osdConfig_t; PG_DECLARE(osdConfig_t, osdConfig);