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