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 7785ec47dc..149b67383e 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; @@ -488,7 +489,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 5bec324109..b3e62acf8a 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 68d6765e7e..7706ddeb30 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -926,6 +926,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 0f3ef95e4c..71aed686e4 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -86,6 +86,7 @@ #include "rx/rx.h" +#include "sensors/acceleration.h" #include "sensors/adcinternal.h" #include "sensors/barometer.h" #include "sensors/battery.h" @@ -687,6 +688,18 @@ static bool osdDrawSingleElement(uint8_t item) return true; } + case OSD_G_FORCE: + { + float osdGForce = 0; + for (int axis = 0; axis < 3; axis++) { + const float a = accAverage[axis]; + osdGForce += a * a; + } + osdGForce = sqrtf(osdGForce) / acc.dev.acc_1G * 10; + tfp_sprintf(buff, "%01d.%01d%c", (uint8_t)osdGForce / 10, (uint8_t)osdGForce % 10, "G"); + break; + } + case OSD_ROLL_PIDS: osdFormatPID(buff, "ROL", ¤tPidProfile->pid[PID_ROLL]); break; @@ -963,6 +976,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 644ed0718e..6edf3f8705 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;