diff --git a/make/source.mk b/make/source.mk
index e38323f355..30450ce7b3 100644
--- a/make/source.mk
+++ b/make/source.mk
@@ -175,6 +175,7 @@ COMMON_SRC = \
io/gps_nmea.c \
io/gps_naza.c \
io/ledstrip.c \
+ io/osd_hud.c \
io/osd.c \
navigation/navigation.c \
navigation/navigation_fixedwing.c \
diff --git a/src/main/cms/cms_menu_osd.c b/src/main/cms/cms_menu_osd.c
index e509eb9132..6253034872 100755
--- a/src/main/cms/cms_menu_osd.c
+++ b/src/main/cms/cms_menu_osd.c
@@ -194,7 +194,7 @@ static const OSD_Entry menuOsdElemsEntries[] =
OSD_ELEMENT_ENTRY("VARIO NUM", OSD_VARIO_NUM),
#endif // defined
OSD_ELEMENT_ENTRY("ALTITUDE", OSD_ALTITUDE),
- OSD_ELEMENT_ENTRY("ALTITUDE MSL", OSD_ALTITUDE_MSL),
+ OSD_ELEMENT_ENTRY("ALTITUDE MSL", OSD_ALTITUDE_MSL),
#if defined(USE_PITOT)
OSD_ELEMENT_ENTRY("AIR SPEED", OSD_AIR_SPEED),
#endif
@@ -345,7 +345,6 @@ static const OSD_Entry menuOsdSettingsEntries[] = {
OSD_SETTING_ENTRY("VOLT. DECIMALS", SETTING_OSD_MAIN_VOLTAGE_DECIMALS),
OSD_SETTING_ENTRY("COORD. DIGITS", SETTING_OSD_COORDINATE_DIGITS),
- OSD_SETTING_ENTRY("CROSSHAIRS STYLE", SETTING_OSD_CROSSHAIRS_STYLE),
OSD_SETTING_ENTRY("LEFT SCROLL", SETTING_OSD_LEFT_SIDEBAR_SCROLL),
OSD_SETTING_ENTRY("RIGHT SCROLL", SETTING_OSD_RIGHT_SIDEBAR_SCROLL),
OSD_SETTING_ENTRY("SCROLL ARROWS", SETTING_OSD_SIDEBAR_SCROLL_ARROWS),
@@ -364,12 +363,64 @@ static const CMS_Menu cmsx_menuOsdSettings = {
.entries = menuOsdSettingsEntries,
};
+static const OSD_Entry menuOsdHud2Entries[] = {
+ OSD_LABEL_ENTRY("--- HUD ITEMS ---"),
+
+ OSD_SETTING_ENTRY("HOMING ARROWS", SETTING_OSD_HUD_HOMING),
+ OSD_SETTING_ENTRY("HOME POINT", SETTING_OSD_HUD_HOMEPOINT),
+ OSD_SETTING_ENTRY("RADAR MAX AIRCRAFT", SETTING_OSD_HUD_RADAR_DISP),
+ OSD_SETTING_ENTRY("RADAR MIN RANGE", SETTING_OSD_HUD_RADAR_RANGE_MIN),
+ OSD_SETTING_ENTRY("RADAR MAX RANGE", SETTING_OSD_HUD_RADAR_RANGE_MAX),
+ OSD_SETTING_ENTRY("RADAR DET. NEAREST", SETTING_OSD_HUD_RADAR_NEAREST),
+ OSD_BACK_ENTRY,
+ OSD_END_ENTRY,
+};
+
+static const CMS_Menu cmsx_menuOsdHud2 = {
+#ifdef CMS_MENU_DEBUG
+ .GUARD_text = "MENUOSDH2",
+ .GUARD_type = OME_MENU,
+#endif
+ .onEnter = NULL,
+ .onExit = NULL,
+ .onGlobalExit = NULL,
+ .entries = menuOsdHud2Entries,
+};
+
+static const OSD_Entry menuOsdHudEntries[] = {
+ OSD_LABEL_ENTRY("--- HUD ---"),
+
+ OSD_SETTING_ENTRY("CROSSHAIRS STYLE", SETTING_OSD_CROSSHAIRS_STYLE),
+ OSD_SETTING_ENTRY("HORIZON OFFSET", SETTING_OSD_HORIZON_OFFSET),
+ OSD_SETTING_ENTRY("CAMERA UPTILT", SETTING_OSD_CAMERA_UPTILT),
+ OSD_SETTING_ENTRY("CAMERA FOV HORI", SETTING_OSD_CAMERA_FOV_H),
+ OSD_SETTING_ENTRY("CAMERA FOV VERT", SETTING_OSD_CAMERA_FOV_V),
+ OSD_SETTING_ENTRY("HUD MARGIN HORI", SETTING_OSD_HUD_MARGIN_H),
+ OSD_SETTING_ENTRY("HUD MARGIN VERT", SETTING_OSD_HUD_MARGIN_V),
+ OSD_SUBMENU_ENTRY("DISPLAYED ITEMS", &cmsx_menuOsdHud2),
+
+ OSD_BACK_ENTRY,
+ OSD_END_ENTRY,
+};
+
+static const CMS_Menu cmsx_menuOsdHud = {
+#ifdef CMS_MENU_DEBUG
+ .GUARD_text = "MENUOSDH",
+ .GUARD_type = OME_MENU,
+#endif
+ .onEnter = NULL,
+ .onExit = NULL,
+ .onGlobalExit = NULL,
+ .entries = menuOsdHudEntries,
+};
+
static const OSD_Entry menuOsdEntries[] = {
OSD_LABEL_ENTRY("--- OSD ---"),
OSD_SUBMENU_ENTRY("LAYOUTS", &cmsx_menuOsdLayout),
OSD_SUBMENU_ENTRY("SETTINGS", &cmsx_menuOsdSettings),
OSD_SUBMENU_ENTRY("ALARMS", &cmsx_menuAlarms),
+ OSD_SUBMENU_ENTRY("HUD", &cmsx_menuOsdHud),
OSD_BACK_AND_END_ENTRY,
};
diff --git a/src/main/common/constants.h b/src/main/common/constants.h
new file mode 100644
index 0000000000..6b2f9f69b6
--- /dev/null
+++ b/src/main/common/constants.h
@@ -0,0 +1,24 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight. If not, see .
+ */
+
+#pragma once
+
+#define FEET_PER_MILE 5280
+#define FEET_PER_KILOFEET 1000 // Used for altitude
+#define METERS_PER_KILOMETER 1000
+#define METERS_PER_MILE 1609
+
diff --git a/src/main/drivers/osd_symbols.h b/src/main/drivers/osd_symbols.h
index b15abb2963..7d7ec32f34 100644
--- a/src/main/drivers/osd_symbols.h
+++ b/src/main/drivers/osd_symbols.h
@@ -197,20 +197,45 @@
#define SYM_AH_V_START 0xE0 // 224 to 229 Vertical AHI
-#define SYM_BARO_TEMP 0xF0
-#define SYM_IMU_TEMP 0xF1
-#define SYM_TEMP 0xF2
+#define SYM_BARO_TEMP 0xF0 // 240
+#define SYM_IMU_TEMP 0xF1 // 241
+#define SYM_TEMP 0xF2 // 242
-#define SYM_TEMP_SENSOR_FIRST 0xF2
-#define SYM_TEMP_SENSOR_LAST 0xF7
+#define SYM_TEMP_SENSOR_FIRST 0xF2 // 242
+#define SYM_TEMP_SENSOR_LAST 0xF7 // 247
#define TEMP_SENSOR_SYM_COUNT (SYM_TEMP_SENSOR_LAST - SYM_TEMP_SENSOR_FIRST + 1)
+#define SYM_HUD_SIGNAL_0 0xF8 // 248 Hud signal icon Lost
+#define SYM_HUD_SIGNAL_1 0xF9 // 249 Hud signal icon 25%
+#define SYM_HUD_SIGNAL_2 0xFA // 250 Hud signal icon 50%
+#define SYM_HUD_SIGNAL_3 0xFB // 251 Hud signal icon 75%
+#define SYM_HUD_SIGNAL_4 0xFC // 252 Hud signal icon 100%
+
#define SYM_LOGO_START 0x101 // 257 to 280, INAV logo
#define SYM_LOGO_WIDTH 6
#define SYM_LOGO_HEIGHT 4
+#define SYM_AH_CH_TYPE3 0x190 // 400 to 402, crosshair 3
+#define SYM_AH_CH_TYPE4 0x193 // 403 to 405, crosshair 4
+#define SYM_AH_CH_TYPE5 0x196 // 406 to 408, crosshair 5
+#define SYM_AH_CH_TYPE6 0x199 // 409 to 411, crosshair 6
+#define SYM_AH_CH_TYPE7 0x19C // 412 to 414, crosshair 7
+
+#define SYM_HUD_ARROWS_L1 0x1A2 // 418 1 arrow left
+#define SYM_HUD_ARROWS_L2 0x1A3 // 419 2 arrows left
+#define SYM_HUD_ARROWS_L3 0x1A4 // 420 3 arrows left
+#define SYM_HUD_ARROWS_R1 0x1A5 // 421 1 arrow right
+#define SYM_HUD_ARROWS_R2 0x1A6 // 422 2 arrows right
+#define SYM_HUD_ARROWS_R3 0x1A7 // 423 3 arrows right
+#define SYM_HUD_ARROWS_U1 0x1A8 // 424 1 arrow up
+#define SYM_HUD_ARROWS_U2 0x1A9 // 425 2 arrows up
+#define SYM_HUD_ARROWS_U3 0x1AA // 426 3 arrows up
+#define SYM_HUD_ARROWS_D1 0x1AB // 427 1 arrow down
+#define SYM_HUD_ARROWS_D2 0x1AC // 428 2 arrows down
+#define SYM_HUD_ARROWS_D3 0x1AD // 429 3 arrows down
+
#else
#define TEMP_SENSOR_SYM_COUNT 0
-#endif // USE_MAX7456
+#endif // USE_OSD
diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index e52deab64b..1bd830eab7 100644
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -2395,6 +2395,19 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
} else
return MSP_RESULT_ERROR;
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
+ return MSP_RESULT_ERROR;
+ break;
#endif
case MSP_SET_FEATURE:
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index c9769b26b5..d4ac1da577 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -84,7 +84,7 @@ tables:
values: ["OR", "AND"]
enum: modeActivationOperator_e
- name: osd_crosshairs_style
- values: ["DEFAULT", "AIRCRAFT"]
+ values: ["DEFAULT", "AIRCRAFT", "TYPE3", "TYPE4", "TYPE5", "TYPE6", "TYPE7"]
enum: osd_crosshairs_style_e
- name: osd_sidebar_scroll
values: ["NONE", "ALTITUDE", "GROUND_SPEED", "HOME_DISTANCE"]
@@ -1713,6 +1713,52 @@ groups:
field: crosshairs_style
table: osd_crosshairs_style
type: uint8_t
+ - name: osd_horizon_offset
+ field: horizon_offset
+ min: -2
+ max: 2
+ - name: osd_camera_uptilt
+ field: camera_uptilt
+ min: -40
+ max: 80
+ - name: osd_camera_fov_h
+ field: camera_fov_h
+ min: 60
+ max: 150
+ - name: osd_camera_fov_v
+ field: camera_fov_v
+ min: 30
+ max: 120
+ - name: osd_hud_margin_h
+ field: hud_margin_h
+ min: 0
+ max: 4
+ - name: osd_hud_margin_v
+ field: hud_margin_v
+ min: 0
+ max: 4
+ - name: osd_hud_homing
+ field: hud_homing
+ type: bool
+ - name: osd_hud_homepoint
+ field: hud_homepoint
+ type: bool
+ - name: osd_hud_radar_disp
+ field: hud_radar_disp
+ min: 0
+ max: 4
+ - name: osd_hud_radar_range_min
+ field: hud_radar_range_min
+ min: 1
+ max: 30
+ - name: osd_hud_radar_range_max
+ field: hud_radar_range_max
+ min: 100
+ max: 9990
+ - name: osd_hud_radar_nearest
+ field: hud_radar_nearest
+ min: 0
+ max: 4000
- name: osd_left_sidebar_scroll
field: left_sidebar_scroll
table: osd_sidebar_scroll
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index 407e2ddbd1..77c2d1ca34 100755
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -41,6 +41,7 @@
#include "cms/cms_menu_osd.h"
#include "common/axis.h"
+#include "common/constants.h"
#include "common/filter.h"
#include "common/olc.h"
#include "common/printf.h"
@@ -61,6 +62,7 @@
#include "io/flashfs.h"
#include "io/gps.h"
#include "io/osd.h"
+#include "io/osd_hud.h"
#include "io/vtx_string.h"
#include "fc/config.h"
@@ -100,11 +102,6 @@
#define VIDEO_BUFFER_CHARS_PAL 480
#define IS_DISPLAY_PAL (displayScreenSize(osdDisplayPort) == VIDEO_BUFFER_CHARS_PAL)
-#define FEET_PER_MILE 5280
-#define FEET_PER_KILOFEET 1000 // Used for altitude
-#define METERS_PER_KILOMETER 1000
-#define METERS_PER_MILE 1609
-
#define GFORCE_FILTER_TC 0.2
#define DELAYED_REFRESH_RESUME_COMMAND (checkStickPosition(THR_HI) || checkStickPosition(PIT_HI))
@@ -213,8 +210,8 @@ static int digitCount(int32_t value)
* of the same length. If the value doesn't fit into the provided length
* it will be divided by scale and true will be returned.
*/
- static bool osdFormatCentiNumber(char *buff, int32_t centivalue, uint32_t scale, int maxDecimals, int maxScaledDecimals, int length)
- {
+bool osdFormatCentiNumber(char *buff, int32_t centivalue, uint32_t scale, int maxDecimals, int maxScaledDecimals, int length)
+{
char *ptr = buff;
char *dec;
int decimals = maxDecimals;
@@ -321,35 +318,35 @@ static void osdFormatDistanceSymbol(char *buff, int32_t dist)
* Converts distance into a string based on the current unit system.
* @param dist Distance in centimeters
*/
- static void osdFormatDistanceStr(char *buff, int32_t dist)
- {
- int32_t centifeet;
- switch ((osd_unit_e)osdConfig()->units) {
- case OSD_UNIT_IMPERIAL:
- centifeet = CENTIMETERS_TO_CENTIFEET(dist);
- if (abs(centifeet) < FEET_PER_MILE * 100 / 2) {
- // Show feet when dist < 0.5mi
- tfp_sprintf(buff, "%d%c", (int)(centifeet / 100), SYM_FT);
- } else {
- // Show miles when dist >= 0.5mi
- tfp_sprintf(buff, "%d.%02d%c", (int)(centifeet / (100*FEET_PER_MILE)),
- (abs(centifeet) % (100 * FEET_PER_MILE)) / FEET_PER_MILE, SYM_MI);
- }
- break;
- case OSD_UNIT_UK:
- FALLTHROUGH;
- case OSD_UNIT_METRIC:
- if (abs(dist) < METERS_PER_KILOMETER * 100) {
- // Show meters when dist < 1km
- tfp_sprintf(buff, "%d%c", (int)(dist / 100), SYM_M);
- } else {
- // Show kilometers when dist >= 1km
- tfp_sprintf(buff, "%d.%02d%c", (int)(dist / (100*METERS_PER_KILOMETER)),
- (abs(dist) % (100 * METERS_PER_KILOMETER)) / METERS_PER_KILOMETER, SYM_KM);
- }
- break;
+static void osdFormatDistanceStr(char *buff, int32_t dist)
+{
+ int32_t centifeet;
+ switch ((osd_unit_e)osdConfig()->units) {
+ case OSD_UNIT_IMPERIAL:
+ centifeet = CENTIMETERS_TO_CENTIFEET(dist);
+ if (abs(centifeet) < FEET_PER_MILE * 100 / 2) {
+ // Show feet when dist < 0.5mi
+ tfp_sprintf(buff, "%d%c", (int)(centifeet / 100), SYM_FT);
+ } else {
+ // Show miles when dist >= 0.5mi
+ tfp_sprintf(buff, "%d.%02d%c", (int)(centifeet / (100*FEET_PER_MILE)),
+ (abs(centifeet) % (100 * FEET_PER_MILE)) / FEET_PER_MILE, SYM_MI);
+ }
+ break;
+ case OSD_UNIT_UK:
+ FALLTHROUGH;
+ case OSD_UNIT_METRIC:
+ if (abs(dist) < METERS_PER_KILOMETER * 100) {
+ // Show meters when dist < 1km
+ tfp_sprintf(buff, "%d%c", (int)(dist / 100), SYM_M);
+ } else {
+ // Show kilometers when dist >= 1km
+ tfp_sprintf(buff, "%d.%02d%c", (int)(dist / (100*METERS_PER_KILOMETER)),
+ (abs(dist) % (100 * METERS_PER_KILOMETER)) / METERS_PER_KILOMETER, SYM_KM);
}
+ break;
}
+}
/**
* Converts velocity based on the current unit system (kmh or mph).
@@ -373,7 +370,7 @@ static int32_t osdConvertVelocityToUnit(int32_t vel)
* Converts velocity into a string based on the current unit system.
* @param alt Raw velocity (i.e. as taken from gpsSol.groundSpeed in centimeters/seconds)
*/
-static void osdFormatVelocityStr(char* buff, int32_t vel, bool _3D)
+void osdFormatVelocityStr(char* buff, int32_t vel, bool _3D)
{
switch ((osd_unit_e)osdConfig()->units) {
case OSD_UNIT_UK:
@@ -425,7 +422,7 @@ static void osdFormatWindSpeedStr(char *buff, int32_t ws, bool isValid)
* prefixed by a a symbol to indicate the unit used.
* @param alt Raw altitude/distance (i.e. as taken from baro.BaroAlt in centimeters)
*/
-static void osdFormatAltitudeSymbol(char *buff, int32_t alt)
+void osdFormatAltitudeSymbol(char *buff, int32_t alt)
{
switch ((osd_unit_e)osdConfig()->units) {
case OSD_UNIT_UK:
@@ -832,21 +829,11 @@ static void osdUpdateBatteryCapacityOrVoltageTextAttributes(textAttributes_t *at
TEXT_ATTRIBUTES_ADD_BLINK(*attr);
}
-static void osdCrosshairsBounds(uint8_t *x, uint8_t *y, uint8_t *length)
+void osdCrosshairPosition(uint8_t *x, uint8_t *y)
{
- *x = 14 - 1; // Offset for 1 char to the left
- *y = 6;
- if (IS_DISPLAY_PAL) {
- ++(*y);
- }
- int size = 3;
- if (osdConfig()->crosshairs_style == OSD_CROSSHAIRS_STYLE_AIRCRAFT) {
- (*x)--;
- size = 5;
- }
- if (length) {
- *length = size;
- }
+ *x = osdDisplayPort->cols / 2;
+ *y = osdDisplayPort->rows / 2;
+ *y += osdConfig()->horizon_offset;
}
/**
@@ -870,7 +857,7 @@ static void osdFormatThrottlePosition(char *buff, bool autoThr, textAttributes_t
tfp_sprintf(buff + 2, "%3d", (constrain(thr, PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN));
}
-static inline int32_t osdGetAltitude(void)
+int32_t osdGetAltitude(void)
{
#if defined(USE_NAV)
return getEstimatedActualPosition(Z);
@@ -961,7 +948,7 @@ static bool osdIsHeadingValid(void)
return isImuHeadingValid();
}
-static int16_t osdGetHeading(void)
+int16_t osdGetHeading(void)
{
return attitude.values.yaw;
}
@@ -994,8 +981,8 @@ static void osdDrawMap(int referenceHeading, uint8_t referenceSym, uint8_t cente
{
// TODO: These need to be tested with several setups. We might
// need to make them configurable.
- const int hMargin = 1;
- const int vMargin = 1;
+ const int hMargin = 5;
+ const int vMargin = 3;
// TODO: Get this from the display driver?
const int charWidth = 12;
@@ -1225,7 +1212,6 @@ static bool osdDrawSingleElement(uint8_t item)
if (!OSD_VISIBLE(pos)) {
return false;
}
-
uint8_t elemPosX = OSD_X(pos);
uint8_t elemPosY = OSD_Y(pos);
textAttributes_t elemAttr = TEXT_ATTRIBUTES_NONE;
@@ -1671,24 +1657,48 @@ static bool osdDrawSingleElement(uint8_t item)
#endif
break;
- case OSD_CROSSHAIRS:
- osdCrosshairsBounds(&elemPosX, &elemPosY, NULL);
- switch ((osd_crosshairs_style_e)osdConfig()->crosshairs_style) {
- case OSD_CROSSHAIRS_STYLE_DEFAULT:
- buff[0] = SYM_AH_CH_LEFT;
- buff[1] = SYM_AH_CH_CENTER;
- buff[2] = SYM_AH_CH_RIGHT;
- buff[3] = '\0';
- break;
- case OSD_CROSSHAIRS_STYLE_AIRCRAFT:
- buff[0] = SYM_AH_CH_AIRCRAFT0;
- buff[1] = SYM_AH_CH_AIRCRAFT1;
- buff[2] = SYM_AH_CH_AIRCRAFT2;
- buff[3] = SYM_AH_CH_AIRCRAFT3;
- buff[4] = SYM_AH_CH_AIRCRAFT4;
- buff[5] = '\0';
- break;
+ case OSD_CROSSHAIRS: // Hud is a sub-element of the crosshair
+
+ osdCrosshairPosition(&elemPosX, &elemPosY);
+ osdHudDrawCrosshair(elemPosX, elemPosY);
+
+ if (osdConfig()->hud_homing && STATE(GPS_FIX) && STATE(GPS_FIX_HOME) && isImuHeadingValid()) {
+ osdHudDrawHoming(elemPosX, elemPosY);
}
+
+ if (STATE(GPS_FIX) && isImuHeadingValid()) {
+
+ if (osdConfig()->hud_homepoint || osdConfig()->hud_radar_disp > 0) {
+ osdHudClear();
+ }
+
+ if (osdConfig()->hud_homepoint) { // Display the home point (H)
+ osdHudDrawPoi(GPS_distanceToHome, GPS_directionToHome, -osdGetAltitude() / 100, 0, 5, SYM_HOME);
+ }
+
+ if (osdConfig()->hud_radar_disp > 0) { // Display the POI from the radar
+ for (uint8_t i = 0; i < osdConfig()->hud_radar_disp; i++) {
+ fpVector3_t poi;
+ geoConvertGeodeticToLocal(&poi, &posControl.gpsOrigin, &radar_pois[i].gps, GEO_ALT_RELATIVE);
+ radar_pois[i].distance = calculateDistanceToDestination(&poi) / 100; // In meters
+
+ if ((radar_pois[i].distance >= (osdConfig()->hud_radar_range_min)) && (radar_pois[i].distance <= (osdConfig()->hud_radar_range_max))) {
+ radar_pois[i].direction = calculateBearingToDestination(&poi) / 100; // In °
+ radar_pois[i].altitude = (radar_pois[i].gps.alt - osdGetAltitudeMsl()) / 100;
+ osdHudDrawPoi(radar_pois[i].distance, osdGetHeadingAngle(radar_pois[i].direction), radar_pois[i].altitude, radar_pois[i].heading, radar_pois[i].lq, 65 + i);
+ }
+ }
+
+ if (osdConfig()->hud_radar_nearest > 0) { // Display extra datas for 1 POI closer than a set distance
+ int poi_id = radarGetNearestPOI();
+ if (poi_id >= 0 && radar_pois[poi_id].distance <= osdConfig()->hud_radar_nearest) {
+ osdHudDrawExtras(poi_id);
+ }
+ }
+ }
+ }
+
+ return true;
break;
case OSD_ATTITUDE_ROLL:
@@ -1710,9 +1720,7 @@ static bool osdDrawSingleElement(uint8_t item)
case OSD_ARTIFICIAL_HORIZON:
{
-
- elemPosX = 14;
- elemPosY = 6; // Center of the AH area
+ osdCrosshairPosition(&elemPosX, &elemPosY);
// Store the positions we draw over to erase only these at the next iteration
static int8_t previous_written[AH_PREV_SIZE];
@@ -1727,10 +1735,6 @@ static bool osdDrawSingleElement(uint8_t item)
rollAngle = -rollAngle;
}
- if (IS_DISPLAY_PAL) {
- ++elemPosY;
- }
-
float ky = sin_approx(rollAngle);
float kx = cos_approx(rollAngle);
@@ -1789,12 +1793,7 @@ static bool osdDrawSingleElement(uint8_t item)
case OSD_HORIZON_SIDEBARS:
{
- elemPosX = 14;
- elemPosY = 6;
-
- if (IS_DISPLAY_PAL) {
- ++elemPosY;
- }
+ osdCrosshairPosition(&elemPosX, &elemPosY);
static osd_sidebar_t left;
static osd_sidebar_t right;
@@ -2757,6 +2756,18 @@ void pgResetFn_osdConfig(osdConfig_t *osdConfig)
osdConfig->ahi_reverse_roll = 0;
osdConfig->ahi_max_pitch = AH_MAX_PITCH_DEFAULT;
osdConfig->crosshairs_style = OSD_CROSSHAIRS_STYLE_DEFAULT;
+ osdConfig->horizon_offset = 0;
+ osdConfig->camera_uptilt = 0;
+ osdConfig->camera_fov_h = 135;
+ osdConfig->camera_fov_v = 85;
+ osdConfig->hud_margin_h = 3;
+ osdConfig->hud_margin_v = 3;
+ osdConfig->hud_homing = 0;
+ osdConfig->hud_homepoint = 0;
+ osdConfig->hud_radar_disp = 0;
+ osdConfig->hud_radar_range_min = 1;
+ osdConfig->hud_radar_range_max = 4000;
+ osdConfig->hud_radar_nearest = 0;
osdConfig->left_sidebar_scroll = OSD_SIDEBAR_SCROLL_NONE;
osdConfig->right_sidebar_scroll = OSD_SIDEBAR_SCROLL_NONE;
osdConfig->sidebar_scroll_arrows = 0;
@@ -3041,7 +3052,7 @@ static void osdShowArmed(void)
displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y + 1, buf);
int digits = osdConfig()->plus_code_digits;
olc_encode(GPS_home.lat, GPS_home.lon, digits, buf, sizeof(buf));
- displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y + 2, buf);
+ displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y + 2, buf);
y += 4;
} else {
strcpy(buf, "!NO HOME POSITION!");
diff --git a/src/main/io/osd.h b/src/main/io/osd.h
index d729824786..2453796432 100755
--- a/src/main/io/osd.h
+++ b/src/main/io/osd.h
@@ -36,6 +36,13 @@
#define OSD_POS_MAX 0x3FF
#define OSD_POS_MAX_CLI (OSD_POS_MAX | OSD_VISIBLE_FLAG)
+#define OSD_HOMING_LIM_H1 6
+#define OSD_HOMING_LIM_H2 16
+#define OSD_HOMING_LIM_H3 38
+#define OSD_HOMING_LIM_V1 5
+#define OSD_HOMING_LIM_V2 10
+#define OSD_HOMING_LIM_V3 15
+
typedef enum {
OSD_RSSI_VALUE,
OSD_MAIN_BATT_VOLTAGE,
@@ -159,6 +166,11 @@ typedef enum {
typedef enum {
OSD_CROSSHAIRS_STYLE_DEFAULT,
OSD_CROSSHAIRS_STYLE_AIRCRAFT,
+ OSD_CROSSHAIRS_STYLE_TYPE3,
+ OSD_CROSSHAIRS_STYLE_TYPE4,
+ OSD_CROSSHAIRS_STYLE_TYPE5,
+ OSD_CROSSHAIRS_STYLE_TYPE6,
+ OSD_CROSSHAIRS_STYLE_TYPE7,
} osd_crosshairs_style_e;
typedef enum {
@@ -205,6 +217,19 @@ typedef struct osdConfig_s {
uint8_t ahi_reverse_roll;
uint8_t ahi_max_pitch;
uint8_t crosshairs_style; // from osd_crosshairs_style_e
+ int8_t horizon_offset;
+ int8_t camera_uptilt;
+ uint8_t camera_fov_h;
+ uint8_t camera_fov_v;
+ uint8_t hud_margin_h;
+ uint8_t hud_margin_v;
+ bool hud_homing;
+ bool hud_homepoint;
+ uint8_t hud_radar_disp;
+ uint16_t hud_radar_range_min;
+ uint16_t hud_radar_range_max;
+ uint16_t hud_radar_nearest;
+
uint8_t left_sidebar_scroll; // from osd_sidebar_scroll_e
uint8_t right_sidebar_scroll; // from osd_sidebar_scroll_e
uint8_t sidebar_scroll_arrows;
@@ -236,4 +261,12 @@ void osdOverrideLayout(int layout, timeMs_t duration);
// set by the user configuration (modes, etc..) or by overriding it.
int osdGetActiveLayout(bool *overridden);
bool osdItemIsFixed(osd_items_e item);
+
displayPort_t *osdGetDisplayPort(void);
+
+int16_t osdGetHeading(void);
+int32_t osdGetAltitude(void);
+void osdCrosshairPosition(uint8_t *x, uint8_t *y);
+bool osdFormatCentiNumber(char *buff, int32_t centivalue, uint32_t scale, int maxDecimals, int maxScaledDecimals, int length);
+void osdFormatAltitudeSymbol(char *buff, int32_t alt);
+void osdFormatVelocityStr(char* buff, int32_t vel, bool _3D);
diff --git a/src/main/io/osd_hud.c b/src/main/io/osd_hud.c
new file mode 100644
index 0000000000..ecea0268b8
--- /dev/null
+++ b/src/main/io/osd_hud.c
@@ -0,0 +1,340 @@
+/*
+ * This file is part of INAV.
+ *
+ * INAV is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * INAV is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with INAV. If not, see .
+ */
+
+#include
+
+#include "platform.h"
+
+#include "common/constants.h"
+#include "common/printf.h"
+
+#include "flight/imu.h"
+
+#include "io/osd.h"
+#include "io/osd_hud.h"
+
+#include "drivers/display.h"
+#include "drivers/osd.h"
+#include "drivers/osd_symbols.h"
+#include "drivers/time.h"
+
+#include "navigation/navigation.h"
+
+#ifdef USE_OSD
+
+#define HUD_DRAWN_MAXCHARS 40 // 6 POI (1 home, 5 radar) x 7 chars max for each minus 2 for H (no icons for heading and signal)
+
+static int8_t hud_drawn[HUD_DRAWN_MAXCHARS][2];
+static int8_t hud_drawn_pt;
+
+/*
+ * Overwrite all previously written positions on the OSD with a blank
+ */
+void osdHudClear(void)
+{
+ for (int i = 0; i < HUD_DRAWN_MAXCHARS; i++) {
+ if (hud_drawn[i][0] > -1) {
+ displayWriteChar(osdGetDisplayPort(), hud_drawn[i][0], hud_drawn[i][1], SYM_BLANK);
+ hud_drawn[i][0] = -1;
+ }
+ }
+ hud_drawn_pt = 0;
+}
+
+/*
+ * Write a single char on the OSD, and record the position for the next loop
+ */
+static int osdHudWrite(uint8_t px, uint8_t py, uint16_t symb, bool crush)
+{
+ if (!crush) {
+ uint16_t c;
+ if (displayReadCharWithAttr(osdGetDisplayPort(), px, py, &c, NULL) && c != SYM_BLANK) {
+ return false;
+ }
+ }
+
+ displayWriteChar(osdGetDisplayPort(), px, py, symb);
+ hud_drawn[hud_drawn_pt][0] = px;
+ hud_drawn[hud_drawn_pt][1] = py;
+ hud_drawn_pt++;
+ if (hud_drawn_pt >= HUD_DRAWN_MAXCHARS) hud_drawn_pt = 0;
+ return true;
+}
+
+/*
+ * Constrain an angle between -180 and +180°
+ */
+static int16_t hudWrap180(int16_t angle)
+{
+ while (angle < -179) angle += 360;
+ while (angle > 180) angle -= 360;
+ return angle;
+}
+
+/*
+ * Constrain an angle between 0 and +360°
+ */
+static int16_t hudWrap360(int16_t angle)
+{
+ while (angle < 0) angle += 360;
+ while (angle > 360) angle -= 360;
+ return angle;
+}
+
+/*
+ * Radar, get the nearest POI
+ */
+int8_t radarGetNearestPOI(void)
+{
+ int8_t poi = -1;
+ uint16_t min = 10000; // 10kms
+
+ for (int i = 0; i < RADAR_MAX_POIS; i++) {
+ if ((radar_pois[i].distance > 0) && (radar_pois[i].distance < min)) { // (radar_pois[i].state == 1)
+ min = radar_pois[i].distance;
+ poi = i;
+ }
+ }
+ return poi;
+}
+
+/*
+ * Display one POI on the hud, centered on crosshair position.
+ * Distance (m), Direction (°), Altitude (relative, m, negative means below), Heading (°), Signal 0 to 5, Symbol 0 to 480
+ */
+void osdHudDrawPoi(uint32_t poiDistance, int16_t poiDirection, int32_t poiAltitude, int16_t poiHeading, uint8_t poiSignal, uint16_t poiSymbol)
+{
+ int poi_x;
+ int poi_y;
+ uint8_t center_x;
+ uint8_t center_y;
+ bool poi_is_oos = 0;
+
+ 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(¢er_x, ¢er_y);
+
+ 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)) { // POI might be in sight, extra geometry needed
+ 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 ) { // In camera view, but out of the hud area
+ poi_is_oos = 1;
+ }
+ else { // POI is on sight, compute the vertical
+ float poi_angle = atan2_approx(-poiAltitude, poiDistance);
+ 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);
+ }
+ }
+ else {
+ poi_is_oos = 1; // POI is out of camera view for sure
+ }
+
+ // Out-of-sight arrows and stacking
+
+ if (poi_is_oos) {
+ uint16_t d;
+ uint16_t c;
+
+ poi_x = (error_x > 0 ) ? maxX : minX;
+ poi_y = center_y;
+
+ if (displayReadCharWithAttr(osdGetDisplayPort(), poi_x, poi_y, &c, NULL) && c != SYM_BLANK) {
+ poi_y = center_y - 2;
+ while (displayReadCharWithAttr(osdGetDisplayPort(), poi_x, poi_y, &c, NULL) && c != SYM_BLANK && poi_y < maxY - 3) { // Stacks the out-of-sight POI from top to bottom
+ poi_y += 2;
+ }
+ }
+
+ if (error_x > 0 ) {
+ d = SYM_HUD_ARROWS_R3 - constrain ((180 - error_x) / 45, 0, 2);
+ osdHudWrite(poi_x + 2, poi_y, d, 1);
+ }
+ else {
+ d = SYM_HUD_ARROWS_L3 - constrain ((180 + error_x) / 45, 0, 2);
+ osdHudWrite(poi_x - 2, poi_y, d, 1);
+ }
+ }
+
+ // POI marker (A B C ...)
+
+ osdHudWrite(poi_x, poi_y, poiSymbol, 1);
+
+ // Signal on the right, heading on the left
+
+ if (poiSignal < 5) { // 0 to 4 = signal bars, 5 = No LQ and no heading displayed
+ error_x = hudWrap360(poiHeading - DECIDEGREES_TO_DEGREES(osdGetHeading()));
+ osdHudWrite(poi_x - 1, poi_y, SYM_DIRECTION + ((error_x + 22) / 45) % 8, 1);
+ osdHudWrite(poi_x + 1, poi_y, SYM_HUD_SIGNAL_0 + poiSignal, 1);
+ }
+
+ // Distance
+
+ char buff[3];
+ if ((osd_unit_e)osdConfig()->units == OSD_UNIT_IMPERIAL) {
+ osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(poiDistance * 100), FEET_PER_MILE, 0, 3, 3);
+ }
+ else {
+ osdFormatCentiNumber(buff, poiDistance * 100, METERS_PER_KILOMETER, 0, 3, 3);
+ }
+
+ osdHudWrite(poi_x - 1, poi_y + 1, buff[0], 1);
+ osdHudWrite(poi_x , poi_y + 1, buff[1], 1);
+ osdHudWrite(poi_x + 1, poi_y + 1, buff[2], 1);
+}
+
+/*
+ * Draw the crosshair
+ */
+void osdHudDrawCrosshair(uint8_t px, uint8_t py)
+{
+ static const uint16_t crh_style_all[] = {
+ SYM_AH_CH_LEFT, SYM_AH_CH_CENTER, SYM_AH_CH_RIGHT,
+ SYM_AH_CH_AIRCRAFT1, SYM_AH_CH_AIRCRAFT2, SYM_AH_CH_AIRCRAFT3,
+ SYM_AH_CH_TYPE3, SYM_AH_CH_TYPE3 + 1, SYM_AH_CH_TYPE3 + 2,
+ SYM_AH_CH_TYPE4, SYM_AH_CH_TYPE4 + 1, SYM_AH_CH_TYPE4 + 2,
+ SYM_AH_CH_TYPE5, SYM_AH_CH_TYPE5 + 1, SYM_AH_CH_TYPE5 + 2,
+ SYM_AH_CH_TYPE6, SYM_AH_CH_TYPE6 + 1, SYM_AH_CH_TYPE6 + 2,
+ SYM_AH_CH_TYPE7, SYM_AH_CH_TYPE7 + 1, SYM_AH_CH_TYPE7 + 2,
+ };
+
+ uint8_t crh_crosshair = (osd_crosshairs_style_e)osdConfig()->crosshairs_style;
+
+ displayWriteChar(osdGetDisplayPort(), px - 1, py,crh_style_all[crh_crosshair * 3]);
+ displayWriteChar(osdGetDisplayPort(), px, py, crh_style_all[crh_crosshair * 3 + 1]);
+ displayWriteChar(osdGetDisplayPort(), px + 1, py, crh_style_all[crh_crosshair * 3 + 2]);
+}
+
+
+/*
+ * Draw the homing arrows around the crosshair
+ */
+void osdHudDrawHoming(uint8_t px, uint8_t py)
+{
+ int crh_l = SYM_BLANK;
+ int crh_r = SYM_BLANK;
+ int crh_u = SYM_BLANK;
+ int crh_d = SYM_BLANK;
+
+ int16_t crh_diff_head = hudWrap180(GPS_directionToHome - DECIDEGREES_TO_DEGREES(osdGetHeading()));
+
+ if (crh_diff_head <= -162 || crh_diff_head >= 162) {
+ crh_l = SYM_HUD_ARROWS_L3;
+ crh_r = SYM_HUD_ARROWS_R3;
+ } else if (crh_diff_head > -162 && crh_diff_head <= -126) {
+ crh_l = SYM_HUD_ARROWS_L3;
+ crh_r = SYM_HUD_ARROWS_R2;
+ } else if (crh_diff_head > -126 && crh_diff_head <= -90) {
+ crh_l = SYM_HUD_ARROWS_L3;
+ crh_r = SYM_HUD_ARROWS_R1;
+ } else if (crh_diff_head > -90 && crh_diff_head <= -OSD_HOMING_LIM_H3) {
+ crh_l = SYM_HUD_ARROWS_L3;
+ } else if (crh_diff_head > -OSD_HOMING_LIM_H3 && crh_diff_head <= -OSD_HOMING_LIM_H2) {
+ crh_l = SYM_HUD_ARROWS_L2;
+ } else if (crh_diff_head > -OSD_HOMING_LIM_H2 && crh_diff_head <= -OSD_HOMING_LIM_H1) {
+ crh_l = SYM_HUD_ARROWS_L1;
+ } else if (crh_diff_head >= OSD_HOMING_LIM_H1 && crh_diff_head < OSD_HOMING_LIM_H2) {
+ crh_r = SYM_HUD_ARROWS_R1;
+ } else if (crh_diff_head >= OSD_HOMING_LIM_H2 && crh_diff_head < OSD_HOMING_LIM_H3) {
+ crh_r = SYM_HUD_ARROWS_R2;
+ } else if (crh_diff_head >= OSD_HOMING_LIM_H3 && crh_diff_head < 90) {
+ crh_r = SYM_HUD_ARROWS_R3;
+ } else if (crh_diff_head >= 90 && crh_diff_head < 126) {
+ crh_l = SYM_HUD_ARROWS_L1;
+ crh_r = SYM_HUD_ARROWS_R3;
+ } else if (crh_diff_head >= 126 && crh_diff_head < 162) {
+ crh_l = SYM_HUD_ARROWS_L2;
+ crh_r = SYM_HUD_ARROWS_R3;
+ }
+
+ if (ABS(crh_diff_head) < 90) {
+
+ int32_t crh_altitude = osdGetAltitude() / 100;
+ int32_t crh_distance = GPS_distanceToHome;
+
+ float crh_home_angle = atan2_approx(crh_altitude, crh_distance);
+ crh_home_angle = RADIANS_TO_DEGREES(crh_home_angle);
+ int crh_plane_angle = attitude.values.pitch / 10;
+ int crh_camera_angle = osdConfig()->camera_uptilt;
+ int crh_diff_vert = crh_home_angle - crh_plane_angle + crh_camera_angle;
+
+ if (crh_diff_vert > -OSD_HOMING_LIM_V2 && crh_diff_vert <= -OSD_HOMING_LIM_V1 ) {
+ crh_u = SYM_HUD_ARROWS_U1;
+ } else if (crh_diff_vert > -OSD_HOMING_LIM_V3 && crh_diff_vert <= -OSD_HOMING_LIM_V2) {
+ crh_u = SYM_HUD_ARROWS_U2;
+ } else if (crh_diff_vert <= -OSD_HOMING_LIM_V3) {
+ crh_u = SYM_HUD_ARROWS_U3;
+ } else if (crh_diff_vert >= OSD_HOMING_LIM_V1 && crh_diff_vert < OSD_HOMING_LIM_V2) {
+ crh_d = SYM_HUD_ARROWS_D1;
+ } else if (crh_diff_vert >= OSD_HOMING_LIM_V2 && crh_diff_vert < OSD_HOMING_LIM_V3) {
+ crh_d = SYM_HUD_ARROWS_D2;
+ } else if (crh_diff_vert >= OSD_HOMING_LIM_V3) {
+ crh_d = SYM_HUD_ARROWS_D3;
+ }
+ }
+
+ displayWriteChar(osdGetDisplayPort(), px - 2, py, crh_l);
+ displayWriteChar(osdGetDisplayPort(), px + 2, py, crh_r);
+ displayWriteChar(osdGetDisplayPort(), px, py - 1, crh_u);
+ displayWriteChar(osdGetDisplayPort(), px, py + 1, crh_d);
+}
+
+
+/*
+ * Draw extra datas for a radar POI
+ */
+void osdHudDrawExtras(uint8_t poi_id)
+{
+ char buftmp[6];
+
+ uint8_t minX = osdConfig()->hud_margin_h + 1;
+ uint8_t maxX = osdGetDisplayPort()->cols - osdConfig()->hud_margin_h - 2;
+ uint8_t lineY = osdGetDisplayPort()->rows - osdConfig()->hud_margin_v - 1;
+
+ displayWriteChar(osdGetDisplayPort(), minX + 1, lineY, 65 + poi_id);
+ displayWriteChar(osdGetDisplayPort(), minX + 2, lineY, SYM_HUD_SIGNAL_0 + radar_pois[poi_id].lq);
+
+ if (radar_pois[poi_id].altitude < 0) {
+ osdFormatAltitudeSymbol(buftmp, -radar_pois[poi_id].altitude * 100);
+ displayWriteChar(osdGetDisplayPort(), minX + 8, lineY, SYM_HUD_ARROWS_D2);
+ }
+ else {
+ osdFormatAltitudeSymbol(buftmp, radar_pois[poi_id].altitude * 100);
+ displayWriteChar(osdGetDisplayPort(), minX + 8, lineY, SYM_HUD_ARROWS_U2);
+ }
+
+ displayWrite(osdGetDisplayPort(), minX + 4, lineY, buftmp);
+
+ osdFormatVelocityStr(buftmp, radar_pois[poi_id].speed, false);
+ displayWrite(osdGetDisplayPort(), maxX - 9, lineY, buftmp);
+
+ tfp_sprintf(buftmp, "%3d%c", radar_pois[poi_id].heading, SYM_HEADING);
+ displayWrite(osdGetDisplayPort(), maxX - 4, lineY, buftmp);
+
+}
+
+#endif // USE_OSD
diff --git a/src/main/io/osd_hud.h b/src/main/io/osd_hud.h
new file mode 100644
index 0000000000..9744ad0ec4
--- /dev/null
+++ b/src/main/io/osd_hud.h
@@ -0,0 +1,29 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight. If not, see .
+ */
+
+#pragma once
+
+#include
+
+
+void osdHudClear(void);
+void osdHudDrawCrosshair(uint8_t px, uint8_t py);
+void osdHudDrawHoming(uint8_t px, uint8_t py);
+void osdHudDrawPoi(uint32_t poiDistance, int16_t poiDirection, int32_t poiAltitude, int16_t poiHeading, uint8_t poiSignal, uint16_t poiSymbol);
+void osdHudDrawExtras(uint8_t poi_id);
+int8_t radarGetNearestPOI(void);
+
diff --git a/src/main/msp/msp_protocol_v2_common.h b/src/main/msp/msp_protocol_v2_common.h
index 80a4c8faf4..82b7f75407 100644
--- a/src/main/msp/msp_protocol_v2_common.h
+++ b/src/main/msp/msp_protocol_v2_common.h
@@ -15,16 +15,21 @@
* along with INAV. If not, see .
*/
-#define MSP2_COMMON_TZ 0x1001 //out message Gets the TZ offset for the local time (returns: minutes(i16))
-#define MSP2_COMMON_SET_TZ 0x1002 //in message Sets the TZ offset for the local time (args: minutes(i16))
-#define MSP2_COMMON_SETTING 0x1003 //in/out message Returns the value for a setting
-#define MSP2_COMMON_SET_SETTING 0x1004 //in message Sets the value for a setting
+#define MSP2_COMMON_TZ 0x1001 //out message Gets the TZ offset for the local time (returns: minutes(i16))
+#define MSP2_COMMON_SET_TZ 0x1002 //in message Sets the TZ offset for the local time (args: minutes(i16))
+#define MSP2_COMMON_SETTING 0x1003 //in/out message Returns the value for a setting
+#define MSP2_COMMON_SET_SETTING 0x1004 //in message Sets the value for a setting
-#define MSP2_COMMON_MOTOR_MIXER 0x1005
-#define MSP2_COMMON_SET_MOTOR_MIXER 0x1006
+#define MSP2_COMMON_MOTOR_MIXER 0x1005
+#define MSP2_COMMON_SET_MOTOR_MIXER 0x1006
-#define MSP2_COMMON_SETTING_INFO 0x1007 //in/out message Returns info about a setting (PG, type, flags, min/max, etc..).
-#define MSP2_COMMON_PG_LIST 0x1008 //in/out message Returns a list of the PG ids used by the settings
+#define MSP2_COMMON_SETTING_INFO 0x1007 //in/out message Returns info about a setting (PG, type, flags, min/max, etc..).
+#define MSP2_COMMON_PG_LIST 0x1008 //in/out message Returns a list of the PG ids used by the settings
#define MSP2_COMMON_SERIAL_CONFIG 0x1009
-#define MSP2_COMMON_SET_SERIAL_CONFIG 0x100A
\ No newline at end of file
+#define MSP2_COMMON_SET_SERIAL_CONFIG 0x100A
+
+// 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
+
diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index 13c1f4634c..f53b7a9571 100755
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -73,6 +73,8 @@ gpsLocation_t GPS_home;
uint32_t GPS_distanceToHome; // distance to home point in meters
int16_t GPS_directionToHome; // direction to home point in degrees
+radar_pois_t radar_pois[RADAR_MAX_POIS];
+
#if defined(USE_NAV)
#if defined(NAV_NON_VOLATILE_WAYPOINT_STORAGE)
PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 0);
diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h
index 9ef37ccddb..ac0219537c 100755
--- a/src/main/navigation/navigation.h
+++ b/src/main/navigation/navigation.h
@@ -245,6 +245,21 @@ typedef struct {
uint8_t flag;
} navWaypoint_t;
+typedef struct radar_pois_s {
+ gpsLocation_t gps;
+ uint8_t state;
+ uint16_t heading; // °
+ uint16_t speed; // cm/s
+ uint8_t lq; // from 0 t o 4
+ uint16_t distance; // m
+ int16_t altitude; // m
+ int16_t direction; // °
+} radar_pois_t;
+
+#define RADAR_MAX_POIS 5
+
+extern radar_pois_t radar_pois[RADAR_MAX_POIS];
+
typedef struct {
fpVector3_t pos;
int32_t yaw; // deg * 100