mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-19 22:35:19 +03:00
OSD Hud + LoRa radar (nearby aircrafts) (#4558)
This commit is contained in:
Olivier C
Michel Pastor
parent
39d2a956e9
commit
468a040801
13 changed files with 699 additions and 104 deletions
|
|
@ -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 \
|
||||
|
|
|
|||
|
|
@ -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,
|
||||
};
|
||||
|
|
|
|||
24
src/main/common/constants.h
Normal file
24
src/main/common/constants.h
Normal file
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#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
|
||||
|
||||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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:
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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!");
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
340
src/main/io/osd_hud.c
Normal file
340
src/main/io/osd_hud.c
Normal file
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#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
|
||||
29
src/main/io/osd_hud.h
Normal file
29
src/main/io/osd_hud.h
Normal file
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
|
||||
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);
|
||||
|
||||
|
|
@ -15,16 +15,21 @@
|
|||
* along with INAV. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#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
|
||||
#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
|
||||
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue