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

@@ -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

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},

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;
 

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)

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)

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;
 

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

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;

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];