From 9e5cdbf2be0ab0dfee6820a578d5ffd38bd25d36 Mon Sep 17 00:00:00 2001 From: Robert Cisneros Date: Sun, 20 Dec 2020 11:46:55 -0600 Subject: [PATCH] Enable Up/Down reference for artificial horizon in OSD This mod adds an arrow symbol showing the location of the up/down on the OSD. The arrow direction points in the direction of up or down. Update osd_elements.c Making the Up/Down OSD reference its own element Removing the Up/Down reference from the artificial horizon and making a specific element for this feature. Activation via CLI (for now) Updates to Up/Down refernce OSD element Changed the up/down reference so that it is its own separarte element. The position is absolute on the screen and immutable by the user for consistent operation. Updated settings to include CLI option for turning the feature on and off. Update cms_menu_osd.c Updated file to add Up/Down reference selection from OSD menu Updates to simplify/correct Up/Down OSD element Simplified/corrected logic and removed arcsine function. OSD up/down reference is now positionable by the user in the configurator. Tweaks and codestyle updates Feedback from Michael Keller Updates to Up/Down OSD indicator Created a bounding box of +/- 25 degrees for indicator Coding style correction minor correction to match BF coding style Enable Up/Down reference in OSD This mod adds an arrow symbol showing the location of the up/down on the OSD. The arrow direction points in the direction of up or down. Update osd_elements.c Making the Up/Down OSD reference its own element Removing the Up/Down reference from the artificial horizon and making a specific element for this feature. Activation via CLI (for now) Updates to Up/Down refernce OSD element Changed the up/down reference so that it is its own separarte element. The position is absolute on the screen and immutable by the user for consistent operation. Updated settings to include CLI option for turning the feature on and off. Updates to simplify/correct Up/Down OSD element Simplified/corrected logic and removed arcsine function. OSD up/down reference is now positionable by the user in the configurator. Tweaks and codestyle updates Feedback from Michael Keller Enable Up/Down reference for artificial horizon in OSD This mod adds an arrow symbol showing the location of the up/down on the OSD. The arrow direction points in the direction of up or down. Update osd_elements.c Making the Up/Down OSD reference its own element Removing the Up/Down reference from the artificial horizon and making a specific element for this feature. Activation via CLI (for now) Updates to Up/Down refernce OSD element Changed the up/down reference so that it is its own separarte element. The position is absolute on the screen and immutable by the user for consistent operation. Updated settings to include CLI option for turning the feature on and off. Updates to simplify/correct Up/Down OSD element Simplified/corrected logic and removed arcsine function. OSD up/down reference is now positionable by the user in the configurator. Tweaks and codestyle updates Feedback from Michael Keller Coding style correction minor correction to match BF coding style Enable Up/Down reference for artificial horizon in OSD This mod adds an arrow symbol showing the location of the up/down on the OSD. The arrow direction points in the direction of up or down. Update osd_elements.c Making the Up/Down OSD reference its own element Removing the Up/Down reference from the artificial horizon and making a specific element for this feature. Activation via CLI (for now) Updates to Up/Down refernce OSD element Changed the up/down reference so that it is its own separarte element. The position is absolute on the screen and immutable by the user for consistent operation. Updated settings to include CLI option for turning the feature on and off. Updates to simplify/correct Up/Down OSD element Simplified/corrected logic and removed arcsine function. OSD up/down reference is now positionable by the user in the configurator. Tweaks and codestyle updates Feedback from Michael Keller Corrections to unit tests Corrected unit tests link failing due to not being able to find rMat[3][3] --- src/main/cli/settings.c | 1 + src/main/cms/cms_menu_osd.c | 1 + src/main/flight/imu.c | 2 +- src/main/flight/imu.h | 1 + src/main/osd/osd.c | 1 + src/main/osd/osd.h | 1 + src/main/osd/osd_elements.c | 40 +++++++++++++++++++++++++- src/test/unit/link_quality_unittest.cc | 2 ++ src/test/unit/osd_unittest.cc | 2 ++ 9 files changed, 49 insertions(+), 2 deletions(-) diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 07aa97b34e..e9d0afa4f5 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -1391,6 +1391,7 @@ const clivalue_t valueTable[] = { { "osd_battery_usage_pos", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, OSD_POSCFG_MAX }, PG_OSD_ELEMENT_CONFIG, offsetof(osdElementConfig_t, item_pos[OSD_MAIN_BATT_USAGE]) }, { "osd_disarmed_pos", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, OSD_POSCFG_MAX }, PG_OSD_ELEMENT_CONFIG, offsetof(osdElementConfig_t, item_pos[OSD_DISARMED]) }, { "osd_nheading_pos", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, OSD_POSCFG_MAX }, PG_OSD_ELEMENT_CONFIG, offsetof(osdElementConfig_t, item_pos[OSD_NUMERICAL_HEADING]) }, + { "osd_up_down_reference_pos", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, OSD_POSCFG_MAX }, PG_OSD_ELEMENT_CONFIG, offsetof(osdElementConfig_t, item_pos[OSD_UP_DOWN_REFERENCE]) }, #ifdef USE_VARIO { "osd_nvario_pos", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, OSD_POSCFG_MAX }, PG_OSD_ELEMENT_CONFIG, offsetof(osdElementConfig_t, item_pos[OSD_NUMERICAL_VARIO]) }, #endif diff --git a/src/main/cms/cms_menu_osd.c b/src/main/cms/cms_menu_osd.c index 5f17a97475..70021a8987 100644 --- a/src/main/cms/cms_menu_osd.c +++ b/src/main/cms/cms_menu_osd.c @@ -88,6 +88,7 @@ const OSD_Entry menuOsdActiveElemsEntries[] = {"CROSSHAIRS", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_CROSSHAIRS], DYNAMIC}, {"HORIZON", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_ARTIFICIAL_HORIZON], DYNAMIC}, {"HORIZON SIDEBARS", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_HORIZON_SIDEBARS], DYNAMIC}, + {"UP/DOWN REFERENCE", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_UP_DOWN_REFERENCE], DYNAMIC}, {"TIMER 1", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_ITEM_TIMER_1], DYNAMIC}, {"TIMER 2", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_ITEM_TIMER_2], DYNAMIC}, {"REMAINING TIME ESTIMATE", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_REMAINING_TIME_ESTIMATE], DYNAMIC}, diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index d65448f5c5..0eb2f91339 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -96,7 +96,7 @@ static float smallAngleCosZ = 0; static imuRuntimeConfig_t imuRuntimeConfig; -STATIC_UNIT_TESTED float rMat[3][3]; +float rMat[3][3]; STATIC_UNIT_TESTED bool attitudeIsEstablished = false; diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h index bf3502c2b1..3aba95b8c7 100644 --- a/src/main/flight/imu.h +++ b/src/main/flight/imu.h @@ -51,6 +51,7 @@ typedef union { #define EULER_INITIALIZE { { 0, 0, 0 } } extern attitudeEulerAngles_t attitude; +extern float rMat[3][3]; typedef struct imuConfig_s { uint16_t dcm_kp; // DCM filter proportional gain ( x 10000) diff --git a/src/main/osd/osd.c b/src/main/osd/osd.c index a10f8755d9..da39035e93 100644 --- a/src/main/osd/osd.c +++ b/src/main/osd/osd.c @@ -363,6 +363,7 @@ void pgResetFn_osdElementConfig(osdElementConfig_t *osdElementConfig) osdElementConfig->item_pos[OSD_ARTIFICIAL_HORIZON] = OSD_POS(14, 2); osdElementConfig->item_pos[OSD_HORIZON_SIDEBARS] = OSD_POS(14, 6); osdElementConfig->item_pos[OSD_CAMERA_FRAME] = OSD_POS(3, 1); + osdElementConfig->item_pos[OSD_UP_DOWN_REFERENCE] = OSD_POS(13, 6); } static void osdDrawLogo(int x, int y) diff --git a/src/main/osd/osd.h b/src/main/osd/osd.h index 518ed72c0f..bf7660bc0d 100644 --- a/src/main/osd/osd.h +++ b/src/main/osd/osd.h @@ -157,6 +157,7 @@ typedef enum { OSD_CAMERA_FRAME, OSD_EFFICIENCY, OSD_TOTAL_FLIGHTS, + OSD_UP_DOWN_REFERENCE, OSD_ITEM_COUNT // MUST BE LAST } osd_items_e; diff --git a/src/main/osd/osd_elements.c b/src/main/osd/osd_elements.c index 8fe95f0f59..229456df1a 100644 --- a/src/main/osd/osd_elements.c +++ b/src/main/osd/osd_elements.c @@ -126,6 +126,7 @@ #include "sensors/esc_sensor.h" #include "sensors/sensors.h" +#include "common/maths.h" #define AH_SYMBOL_COUNT 9 #define AH_SIDEBAR_WIDTH_POS 7 @@ -142,6 +143,8 @@ #define MOTOR_STOPPED_THRESHOLD_RPM 1000 +#define SINE_25_DEG 0.422618261740699f + #ifdef USE_OSD_STICK_OVERLAY typedef struct radioControls_s { uint8_t left_vertical; @@ -185,6 +188,8 @@ static uint32_t blinkBits[(OSD_ITEM_COUNT + 31) / 32]; #define IS_BLINK(item) (blinkBits[(item) / 32] & (1 << ((item) % 32))) #define BLINK(item) (IS_BLINK(item) && blinkState) +enum {UP, DOWN}; + static int osdDisplayWrite(osdElementParms_t *element, uint8_t x, uint8_t y, uint8_t attr, const char *s) { if (IS_BLINK(element->item)) { @@ -577,6 +582,7 @@ static void osdElementAntiGravity(osdElementParms_t *element) } #ifdef USE_ACC + static void osdElementArtificialHorizon(osdElementParms_t *element) { // Get pitch and roll limits in tenths of degrees @@ -601,6 +607,34 @@ static void osdElementArtificialHorizon(osdElementParms_t *element) element->drawElement = false; // element already drawn } + +static void osdElementUpDownReference(osdElementParms_t *element) +{ +// Up/Down reference feature displays reference points on the OSD at Zenith and Nadir + const float earthUpinBodyFrame[3] = {-rMat[2][0], -rMat[2][1], -rMat[2][2]}; //transforum the up vector to the body frame + + if (ABS(earthUpinBodyFrame[2]) < SINE_25_DEG && ABS(earthUpinBodyFrame[1]) < SINE_25_DEG) { + float thetaB; // pitch from body frame to zenith/nadir + float psiB; // psi from body frame to zenith/nadir + char *symbol[2] = {"U", "D"}; // character buffer + int direction; + + if(attitude.values.pitch>0.0){ //nose down + thetaB = -earthUpinBodyFrame[2]; // get pitch w/re to nadir (use small angle approx for sine) + psiB = -earthUpinBodyFrame[1]; // calculate the yaw w/re to nadir (use small angle approx for sine) + direction = DOWN; + } else { // nose up + thetaB = earthUpinBodyFrame[2]; // get pitch w/re to zenith (use small angle approx for sine) + psiB = earthUpinBodyFrame[1]; // calculate the yaw w/re to zenith (use small angle approx for sine) + direction = UP; + } + int posX = element->elemPosX + round(scaleRangef(psiB, -M_PIf / 4, M_PIf / 4, -14, 14)); + int posY = element->elemPosY + round(scaleRangef(thetaB, -M_PIf / 4, M_PIf / 4, -8, 8)); + + osdDisplayWrite(element, posX, posY, DISPLAYPORT_ATTR_NONE, symbol[direction]); + } + element->drawElement = false; // element already drawn +} #endif // USE_ACC static void osdElementAverageCellVoltage(osdElementParms_t *element) @@ -1570,6 +1604,7 @@ static const uint8_t osdElementDisplayOrder[] = { OSD_RSSI_VALUE, OSD_CROSSHAIRS, OSD_HORIZON_SIDEBARS, + OSD_UP_DOWN_REFERENCE, OSD_ITEM_TIMER_1, OSD_ITEM_TIMER_2, OSD_REMAINING_TIME_ESTIMATE, @@ -1648,6 +1683,7 @@ const osdElementDrawFn osdElementDrawFunction[OSD_ITEM_COUNT] = { [OSD_CROSSHAIRS] = osdElementCrosshairs, // only has background, but needs to be over other elements (like artificial horizon) #ifdef USE_ACC [OSD_ARTIFICIAL_HORIZON] = osdElementArtificialHorizon, + [OSD_UP_DOWN_REFERENCE] = osdElementUpDownReference, #endif [OSD_HORIZON_SIDEBARS] = NULL, // only has background [OSD_ITEM_TIMER_1] = osdElementTimer, @@ -1784,6 +1820,7 @@ void osdAddActiveElements(void) if (sensors(SENSOR_ACC)) { osdAddActiveElement(OSD_ARTIFICIAL_HORIZON); osdAddActiveElement(OSD_G_FORCE); + osdAddActiveElement(OSD_UP_DOWN_REFERENCE); } #endif @@ -2042,7 +2079,8 @@ bool osdElementsNeedAccelerometer(void) osdElementIsActive(OSD_PITCH_ANGLE) || osdElementIsActive(OSD_ROLL_ANGLE) || osdElementIsActive(OSD_G_FORCE) || - osdElementIsActive(OSD_FLIP_ARROW); + osdElementIsActive(OSD_FLIP_ARROW) || + osdElementIsActive(OSD_UP_DOWN_REFERENCE); } #endif // USE_ACC diff --git a/src/test/unit/link_quality_unittest.cc b/src/test/unit/link_quality_unittest.cc index 26d4619f1e..e26c3bc7ce 100644 --- a/src/test/unit/link_quality_unittest.cc +++ b/src/test/unit/link_quality_unittest.cc @@ -64,6 +64,8 @@ extern "C" { #include "sensors/battery.h" attitudeEulerAngles_t attitude; + float rMat[3][3]; + pidProfile_t *currentPidProfile; int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; uint8_t GPS_numSat; diff --git a/src/test/unit/osd_unittest.cc b/src/test/unit/osd_unittest.cc index 22d57d3f14..e448f38974 100644 --- a/src/test/unit/osd_unittest.cc +++ b/src/test/unit/osd_unittest.cc @@ -68,6 +68,8 @@ extern "C" { uint16_t rssi; attitudeEulerAngles_t attitude; + float rMat[3][3]; + pidProfile_t *currentPidProfile; int16_t debug[DEBUG16_VALUE_COUNT]; int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];