diff --git a/src/main/fc/core.c b/src/main/fc/core.c index 7fd4ea661c..8b80f258c4 100644 --- a/src/main/fc/core.c +++ b/src/main/fc/core.c @@ -519,8 +519,9 @@ void updateMagHold(void) if (dif >= +180) dif -= 360; dif *= -GET_DIRECTION(rcControlsConfig()->yaw_control_reversed); - if (STATE(SMALL_ANGLE)) + if (STATE(SMALL_ANGLE)) { rcCommand[YAW] -= dif * currentPidProfile->pid[PID_MAG].P / 30; // 18 deg + } } else magHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw); } diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 98291b5deb..5b17b71400 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -157,7 +157,8 @@ void imuConfigure(uint16_t throttle_correction_angle, uint8_t throttle_correctio { imuRuntimeConfig.dcm_kp = imuConfig()->dcm_kp / 10000.0f; imuRuntimeConfig.dcm_ki = imuConfig()->dcm_ki / 10000.0f; - imuRuntimeConfig.small_angle = imuConfig()->small_angle; + + smallAngleCosZ = cos_approx(degreesToRadians(imuConfig()->small_angle)); fc_acc = calculateAccZLowPassFilterRCTimeConstant(5.0f); // Set to fix value throttleAngleScale = calculateThrottleAngleScale(throttle_correction_angle); @@ -167,7 +168,6 @@ void imuConfigure(uint16_t throttle_correction_angle, uint8_t throttle_correctio void imuInit(void) { - smallAngleCosZ = cos_approx(degreesToRadians(imuRuntimeConfig.small_angle)); accVelScale = 9.80665f * acc.dev.acc_1G_rec / 10000.0f; #ifdef USE_GPS diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h index 6d8472a5a1..d8aa71bb94 100644 --- a/src/main/flight/imu.h +++ b/src/main/flight/imu.h @@ -67,7 +67,6 @@ PG_DECLARE(imuConfig_t, imuConfig); typedef struct imuRuntimeConfig_s { float dcm_ki; float dcm_kp; - uint8_t small_angle; } imuRuntimeConfig_t; void imuConfigure(uint16_t throttle_correction_angle, uint8_t throttle_correction_value); diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 8dd65b9669..2942001ac4 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -528,7 +528,6 @@ void changeOsdProfileIndex(uint8_t profileIndex) } #endif - static bool osdDrawSingleElement(uint8_t item) { if (!VISIBLE(osdConfig()->item_pos[item]) || BLINK(item)) { @@ -542,25 +541,46 @@ static bool osdDrawSingleElement(uint8_t item) switch (item) { case OSD_FLIP_ARROW: { - const int angleR = attitude.values.roll / 10; - const int angleP = attitude.values.pitch / 10; // still gotta update all angleR and angleP pointers. - if (isFlipOverAfterCrashActive()) { - if (angleP > 0 && ((angleR > 175 && angleR < 180) || (angleR > -180 && angleR < -175))) { - buff[0] = SYM_ARROW_SOUTH; - } else if (angleP > 0 && angleR > 0 && angleR < 175) { - buff[0] = (SYM_ARROW_EAST + 2); - } else if (angleP > 0 && angleR < 0 && angleR > -175) { - buff[0] = (SYM_ARROW_WEST + 2); - } else if (angleP <= 0 && ((angleR > 175 && angleR < 180) || (angleR > -180 && angleR < -175))) { - buff[0] = SYM_ARROW_NORTH; - } else if (angleP <= 0 && angleR > 0 && angleR < 175) { - buff[0] = (SYM_ARROW_NORTH + 2); - } else if (angleP <= 0 && angleR < 0 && angleR > -175) { - buff[0] = (SYM_ARROW_SOUTH + 2); + int rollAngle = attitude.values.roll / 10; + const int pitchAngle = attitude.values.pitch / 10; + if (abs(rollAngle) > 90) { + rollAngle = (rollAngle < 0 ? -180 : 180) - rollAngle; + } + + if ((isFlipOverAfterCrashActive() || (!ARMING_FLAG(ARMED) && !STATE(SMALL_ANGLE))) && !((imuConfig()->small_angle < 180) && STATE(SMALL_ANGLE)) && (rollAngle || pitchAngle)) { + if (abs(pitchAngle) < 2 * abs(rollAngle) && abs(rollAngle) < 2 * abs(pitchAngle)) { + if (pitchAngle > 0) { + if (rollAngle > 0) { + buff[0] = SYM_ARROW_WEST + 2; + } else { + buff[0] = SYM_ARROW_EAST - 2; + } + } else { + if (rollAngle > 0) { + buff[0] = SYM_ARROW_WEST - 2; + } else { + buff[0] = SYM_ARROW_EAST + 2; + } + } + } else { + if (abs(pitchAngle) > abs(rollAngle)) { + if (pitchAngle > 0) { + buff[0] = SYM_ARROW_SOUTH; + } else { + buff[0] = SYM_ARROW_NORTH; + } + } else { + if (rollAngle > 0) { + buff[0] = SYM_ARROW_WEST; + } else { + buff[0] = SYM_ARROW_EAST; + } + } } } else { buff[0] = ' '; } + buff[1] = '\0'; break; } diff --git a/src/test/unit/osd_unittest.cc b/src/test/unit/osd_unittest.cc index 86e2578674..02f63166f3 100644 --- a/src/test/unit/osd_unittest.cc +++ b/src/test/unit/osd_unittest.cc @@ -89,6 +89,7 @@ extern "C" { PG_REGISTER(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 0); PG_REGISTER(pilotConfig_t, pilotConfig, PG_PILOT_CONFIG, 0); PG_REGISTER(gpsRescueConfig_t, gpsRescueConfig, PG_GPS_RESCUE, 0); + PG_REGISTER(imuConfig_t, imuConfig, PG_IMU_CONFIG, 0); timeUs_t simulationTime = 0; batteryState_e simulationBatteryState;