Merge pull request #10417 from bobbycisneros/master

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