diff --git a/src/main/cms/cms_menu_osd.c b/src/main/cms/cms_menu_osd.c index abbf80b0a0..95a7e603bd 100644 --- a/src/main/cms/cms_menu_osd.c +++ b/src/main/cms/cms_menu_osd.c @@ -102,6 +102,7 @@ OSD_Entry menuOsdActiveElemsEntries[] = {"ROL ANG", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_ROLL_ANGLE], 0}, {"HEADING", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_NUMERICAL_HEADING], 0}, {"VARIO", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_NUMERICAL_VARIO], 0}, + {"G-FORCE", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_G_FORCE], 0}, {"BACK", OME_Back, NULL, NULL, 0}, {NULL, OME_END, NULL, NULL, 0} }; diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 6c92a1b59e..7b5011b827 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -85,6 +85,7 @@ static bool imuUpdated = false; #define GPS_COG_MIN_GROUNDSPEED 500 // 500cm/s minimum groundspeed for a gps heading to be considered valid int32_t accSum[XYZ_AXIS_COUNT]; +float accAverage[XYZ_AXIS_COUNT]; uint32_t accTimeSum = 0; // keep track for integration of acc int accSumCount = 0; @@ -491,7 +492,6 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs) #endif float gyroAverage[XYZ_AXIS_COUNT]; gyroGetAccumulationAverage(gyroAverage); - float accAverage[XYZ_AXIS_COUNT]; if (accGetAccumulationAverage(accAverage)) { useAcc = imuIsAccelerometerHealthy(accAverage); } diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h index 5f39d13bc7..f97e3d2cbb 100644 --- a/src/main/flight/imu.h +++ b/src/main/flight/imu.h @@ -31,6 +31,7 @@ extern int accSumCount; extern float accVelScale; extern int32_t accSum[XYZ_AXIS_COUNT]; extern bool canUseGPSHeading; +extern float accAverage[XYZ_AXIS_COUNT]; typedef struct { float w,x,y,z; diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index 3c33d0754d..0844343695 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -927,6 +927,7 @@ const clivalue_t valueTable[] = { { "osd_remaining_time_estimate_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_REMAINING_TIME_ESTIMATE]) }, { "osd_flymode_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_FLYMODE]) }, { "osd_anti_gravity_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_ANTI_GRAVITY]) }, + { "osd_g_force_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_G_FORCE]) }, { "osd_throttle_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_THROTTLE_POS]) }, { "osd_vtx_channel_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_VTX_CHANNEL]) }, { "osd_crosshairs_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_CROSSHAIRS]) }, diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 19fa110c00..6c59416c7e 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -46,6 +46,7 @@ #include "cms/cms.h" #include "cms/cms_types.h" +#include "common/axis.h" #include "common/maths.h" #include "common/printf.h" #include "common/typeconversion.h" @@ -87,6 +88,7 @@ #include "rx/rx.h" +#include "sensors/acceleration.h" #include "sensors/adcinternal.h" #include "sensors/barometer.h" #include "sensors/battery.h" @@ -690,6 +692,18 @@ static bool osdDrawSingleElement(uint8_t item) return true; } + case OSD_G_FORCE: + { + float osdGForce = 0; + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + const float a = accAverage[axis]; + osdGForce += a * a; + } + osdGForce = sqrtf(osdGForce) / acc.dev.acc_1G; + tfp_sprintf(buff, "%01d.%01dG", (int)osdGForce, (int)(osdGForce * 10) % 10); + break; + } + case OSD_ROLL_PIDS: osdFormatPID(buff, "ROL", ¤tPidProfile->pid[PID_ROLL]); break; @@ -976,6 +990,7 @@ static void osdDrawElements(void) if (sensors(SENSOR_ACC)) { osdDrawSingleElement(OSD_ARTIFICIAL_HORIZON); + osdDrawSingleElement(OSD_G_FORCE); } diff --git a/src/main/io/osd.h b/src/main/io/osd.h index b1fcdb0832..6addd762b2 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -95,6 +95,7 @@ typedef enum { OSD_ADJUSTMENT_RANGE, OSD_CORE_TEMPERATURE, OSD_ANTI_GRAVITY, + OSD_G_FORCE, OSD_ITEM_COUNT // MUST BE LAST } osd_items_e; diff --git a/src/main/target/BEEBRAIN_V2F/config.c b/src/main/target/BEEBRAIN_V2F/config.c index 70def80078..a81dfac074 100644 --- a/src/main/target/BEEBRAIN_V2F/config.c +++ b/src/main/target/BEEBRAIN_V2F/config.c @@ -150,6 +150,7 @@ void targetConfiguration(void) osdConfigMutable()->item_pos[OSD_NUMERICAL_VARIO] &= ~VISIBLE_FLAG; osdConfigMutable()->item_pos[OSD_ESC_TMP] &= ~VISIBLE_FLAG; osdConfigMutable()->item_pos[OSD_ESC_RPM] &= ~VISIBLE_FLAG; + osdConfigMutable()->item_pos[OSD_G_FORCE] &= ~VISIBLE_FLAG; modeActivationConditionsMutable(0)->modeId = BOXANGLE; modeActivationConditionsMutable(0)->auxChannelIndex = AUX2 - NON_AUX_CHANNEL_COUNT; diff --git a/src/test/unit/osd_unittest.cc b/src/test/unit/osd_unittest.cc index 28d076ac6e..e766d2f789 100644 --- a/src/test/unit/osd_unittest.cc +++ b/src/test/unit/osd_unittest.cc @@ -49,6 +49,7 @@ extern "C" { #include "io/gps.h" #include "io/osd.h" + #include "sensors/acceleration.h" #include "sensors/battery.h" #include "rx/rx.h" @@ -69,6 +70,9 @@ extern "C" { int32_t GPS_coord[2]; gpsSolutionData_t gpsSol; + acc_t acc; + float accAverage[XYZ_AXIS_COUNT]; + PG_REGISTER(batteryConfig_t, batteryConfig, PG_BATTERY_CONFIG, 0); PG_REGISTER(blackboxConfig_t, blackboxConfig, PG_BLACKBOX_CONFIG, 0); PG_REGISTER(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 0);