From cc8b8d3bf6842fba7b4d8c4821b18df58b151e40 Mon Sep 17 00:00:00 2001 From: mikeller Date: Sun, 17 Nov 2019 13:40:30 +1300 Subject: [PATCH] Improved detection of upright / 'SMALL_ANGLE' state. --- src/main/fc/core.c | 10 +- src/main/fc/init.c | 2 - src/main/fc/runtime_config.h | 1 - src/main/flight/imu.c | 141 +++++++++++--------- src/main/flight/imu.h | 3 +- src/main/osd/osd_elements.c | 4 +- src/test/unit/arming_prevention_unittest.cc | 24 ++-- src/test/unit/flight_imu_unittest.cc | 15 ++- src/test/unit/link_quality_unittest.cc | 1 + src/test/unit/osd_unittest.cc | 1 + src/test/unit/vtx_unittest.cc | 1 + 11 files changed, 108 insertions(+), 95 deletions(-) diff --git a/src/main/fc/core.c b/src/main/fc/core.c index 4a2b7f3816..6710f68d01 100644 --- a/src/main/fc/core.c +++ b/src/main/fc/core.c @@ -320,7 +320,7 @@ void updateArmingStatus(void) unsetArmingDisabled(ARMING_DISABLED_THROTTLE); } - if (!STATE(SMALL_ANGLE) && !IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH)) { + if (!isUpright() && !IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH)) { setArmingDisabled(ARMING_DISABLED_ANGLE); } else { unsetArmingDisabled(ARMING_DISABLED_ANGLE); @@ -627,7 +627,7 @@ static void updateInflightCalibrationState(void) } #if defined(USE_GPS) || defined(USE_MAG) -void updateMagHold(void) +static void updateMagHold(void) { if (fabsf(rcCommand[YAW]) < 15 && FLIGHT_MODE(MAG_MODE)) { int16_t dif = DECIDEGREES_TO_DEGREES(attitude.values.yaw) - magHold; @@ -636,7 +636,7 @@ void updateMagHold(void) if (dif >= +180) dif -= 360; dif *= -GET_DIRECTION(rcControlsConfig()->yaw_control_reversed); - if (STATE(SMALL_ANGLE)) { + if (isUpright()) { rcCommand[YAW] -= dif * currentPidProfile->pid[PID_MAG].P / 30; // 18 deg } } else @@ -1130,8 +1130,8 @@ static FAST_CODE_NOINLINE void subTaskPidSubprocesses(timeUs_t currentTimeUs) startTime = micros(); } -#ifdef USE_MAG - if (sensors(SENSOR_MAG)) { +#if defined(USE_GPS) || defined(USE_MAG) + if (sensors(SENSOR_GPS) || sensors(SENSOR_MAG)) { updateMagHold(); } #endif diff --git a/src/main/fc/init.c b/src/main/fc/init.c index e84f6a1920..7b15cff140 100644 --- a/src/main/fc/init.c +++ b/src/main/fc/init.c @@ -915,8 +915,6 @@ void init(void) timerStart(); #endif - ENABLE_STATE(SMALL_ANGLE); - #ifdef SOFTSERIAL_LOOPBACK // FIXME this is a hack, perhaps add a FUNCTION_LOOPBACK to support it properly loopbackPort = (serialPort_t*)&(softSerialPorts[0]); diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index 0569d3430d..4771204ee4 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -114,7 +114,6 @@ typedef enum { GPS_FIX_HOME = (1 << 0), GPS_FIX = (1 << 1), CALIBRATE_MAG = (1 << 2), - SMALL_ANGLE = (1 << 3), } stateFlags_t; #define DISABLE_STATE(mask) (stateFlags &= ~(mask)) diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 5dc516d5c6..5f3c2b3efd 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -101,6 +101,8 @@ static imuRuntimeConfig_t imuRuntimeConfig; STATIC_UNIT_TESTED float rMat[3][3]; +STATIC_UNIT_TESTED bool attitudeIsEstablished = false; + // quaternion of sensor frame relative to earth frame STATIC_UNIT_TESTED quaternion q = QUATERNION_INITIALIZE; STATIC_UNIT_TESTED quaternionProducts qP = QUATERNION_PRODUCTS_INITIALIZE; @@ -119,6 +121,20 @@ PG_RESET_TEMPLATE(imuConfig_t, imuConfig, .small_angle = 25, ); +static void imuQuaternionComputeProducts(quaternion *quat, quaternionProducts *quatProd) +{ + quatProd->ww = quat->w * quat->w; + quatProd->wx = quat->w * quat->x; + quatProd->wy = quat->w * quat->y; + quatProd->wz = quat->w * quat->z; + quatProd->xx = quat->x * quat->x; + quatProd->xy = quat->x * quat->y; + quatProd->xz = quat->x * quat->z; + quatProd->yy = quat->y * quat->y; + quatProd->yz = quat->y * quat->z; + quatProd->zz = quat->z * quat->z; +} + STATIC_UNIT_TESTED void imuComputeRotationMatrix(void){ imuQuaternionComputeProducts(&q, &qP); @@ -320,6 +336,8 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz, // Pre-compute rotation matrix from quaternion imuComputeRotationMatrix(); + + attitudeIsEstablished = true; } STATIC_UNIT_TESTED void imuUpdateEulerAngles(void) @@ -338,14 +356,8 @@ STATIC_UNIT_TESTED void imuUpdateEulerAngles(void) attitude.values.yaw = lrintf((-atan2_approx(rMat[1][0], rMat[0][0]) * (1800.0f / M_PIf))); } - if (attitude.values.yaw < 0) + if (attitude.values.yaw < 0) { attitude.values.yaw += 3600; - - // Update small angle state - if (rMat[2][2] > smallAngleCosZ) { - ENABLE_STATE(SMALL_ANGLE); - } else { - DISABLE_STATE(SMALL_ANGLE); } } @@ -428,6 +440,51 @@ static float imuCalcKpGain(timeUs_t currentTimeUs, bool useAcc, float *gyroAvera return ret; } +static void imuComputeQuaternionFromRPY(quaternionProducts *quatProd, int16_t initialRoll, int16_t initialPitch, int16_t initialYaw) +{ + if (initialRoll > 1800) { + initialRoll -= 3600; + } + + if (initialPitch > 1800) { + initialPitch -= 3600; + } + + if (initialYaw > 1800) { + initialYaw -= 3600; + } + + const float cosRoll = cos_approx(DECIDEGREES_TO_RADIANS(initialRoll) * 0.5f); + const float sinRoll = sin_approx(DECIDEGREES_TO_RADIANS(initialRoll) * 0.5f); + + const float cosPitch = cos_approx(DECIDEGREES_TO_RADIANS(initialPitch) * 0.5f); + const float sinPitch = sin_approx(DECIDEGREES_TO_RADIANS(initialPitch) * 0.5f); + + const float cosYaw = cos_approx(DECIDEGREES_TO_RADIANS(-initialYaw) * 0.5f); + const float sinYaw = sin_approx(DECIDEGREES_TO_RADIANS(-initialYaw) * 0.5f); + + const float q0 = cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw; + const float q1 = sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw; + const float q2 = cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw; + const float q3 = cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw; + + quatProd->xx = sq(q1); + quatProd->yy = sq(q2); + quatProd->zz = sq(q3); + + quatProd->xy = q1 * q2; + quatProd->xz = q1 * q3; + quatProd->yz = q2 * q3; + + quatProd->wx = q0 * q1; + quatProd->wy = q0 * q2; + quatProd->wz = q0 * q3; + + imuComputeRotationMatrix(); + + attitudeIsEstablished = true; +} + static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs) { static timeUs_t previousIMUUpdateTime; @@ -510,10 +567,10 @@ static int calculateThrottleAngleCorrection(void) * small angle < 0.86 deg * TODO: Define this small angle in config. */ - if (rMat[2][2] <= 0.015f) { + if (getCosTiltAngle() <= 0.015f) { return 0; } - int angle = lrintf(acos_approx(rMat[2][2]) * throttleAngleScale); + int angle = lrintf(acos_approx(getCosTiltAngle()) * throttleAngleScale); if (angle > 900) angle = 900; return lrintf(throttleAngleValue * sin_approx(angle / (900.0f * M_PIf / 2.0f))); @@ -574,49 +631,6 @@ void getQuaternion(quaternion *quat) quat->z = q.z; } -void imuComputeQuaternionFromRPY(quaternionProducts *quatProd, int16_t initialRoll, int16_t initialPitch, int16_t initialYaw) -{ - if (initialRoll > 1800) { - initialRoll -= 3600; - } - - if (initialPitch > 1800) { - initialPitch -= 3600; - } - - if (initialYaw > 1800) { - initialYaw -= 3600; - } - - const float cosRoll = cos_approx(DECIDEGREES_TO_RADIANS(initialRoll) * 0.5f); - const float sinRoll = sin_approx(DECIDEGREES_TO_RADIANS(initialRoll) * 0.5f); - - const float cosPitch = cos_approx(DECIDEGREES_TO_RADIANS(initialPitch) * 0.5f); - const float sinPitch = sin_approx(DECIDEGREES_TO_RADIANS(initialPitch) * 0.5f); - - const float cosYaw = cos_approx(DECIDEGREES_TO_RADIANS(-initialYaw) * 0.5f); - const float sinYaw = sin_approx(DECIDEGREES_TO_RADIANS(-initialYaw) * 0.5f); - - const float q0 = cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw; - const float q1 = sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw; - const float q2 = cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw; - const float q3 = cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw; - - quatProd->xx = sq(q1); - quatProd->yy = sq(q2); - quatProd->zz = sq(q3); - - quatProd->xy = q1 * q2; - quatProd->xz = q1 * q3; - quatProd->yz = q2 * q3; - - quatProd->wx = q0 * q1; - quatProd->wy = q0 * q2; - quatProd->wz = q0 * q3; - - imuComputeRotationMatrix(); -} - #ifdef SIMULATOR_BUILD void imuSetAttitudeRPY(float roll, float pitch, float yaw) { @@ -628,6 +642,7 @@ void imuSetAttitudeRPY(float roll, float pitch, float yaw) IMU_UNLOCK; } + void imuSetAttitudeQuat(float w, float x, float y, float z) { IMU_LOCK; @@ -638,6 +653,9 @@ void imuSetAttitudeQuat(float w, float x, float y, float z) q.z = z; imuComputeRotationMatrix(); + + attitudeIsEstablished = true; + imuUpdateEulerAngles(); IMU_UNLOCK; @@ -655,20 +673,6 @@ void imuSetHasNewData(uint32_t dt) } #endif -void imuQuaternionComputeProducts(quaternion *quat, quaternionProducts *quatProd) -{ - quatProd->ww = quat->w * quat->w; - quatProd->wx = quat->w * quat->x; - quatProd->wy = quat->w * quat->y; - quatProd->wz = quat->w * quat->z; - quatProd->xx = quat->x * quat->x; - quatProd->xy = quat->x * quat->y; - quatProd->xz = quat->x * quat->z; - quatProd->yy = quat->y * quat->y; - quatProd->yz = quat->y * quat->z; - quatProd->zz = quat->z * quat->z; -} - bool imuQuaternionHeadfreeOffsetSet(void) { if ((ABS(attitude.values.roll) < 450) && (ABS(attitude.values.pitch) < 450)) { @@ -717,3 +721,8 @@ void imuQuaternionHeadfreeTransformVectorEarthToBody(t_fp_vector_def *v) v->Y = y; v->Z = z; } + +bool isUpright(void) +{ + return attitudeIsEstablished && getCosTiltAngle() <= smallAngleCosZ; +} diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h index 10bec00d40..42995510f9 100644 --- a/src/main/flight/imu.h +++ b/src/main/flight/imu.h @@ -85,8 +85,7 @@ void imuSetHasNewData(uint32_t dt); #endif #endif -void imuQuaternionComputeProducts(quaternion *quat, quaternionProducts *quatProd); bool imuQuaternionHeadfreeOffsetSet(void); void imuQuaternionHeadfreeTransformVectorEarthToBody(t_fp_vector_def * v); -void imuComputeQuaternionFromRPY(quaternionProducts *qP, int16_t initialRoll, int16_t initialPitch, int16_t initialYaw); bool shouldInitializeGPSHeading(void); +bool isUpright(void); diff --git a/src/main/osd/osd_elements.c b/src/main/osd/osd_elements.c index 2405834bc5..46973723fb 100644 --- a/src/main/osd/osd_elements.c +++ b/src/main/osd/osd_elements.c @@ -81,6 +81,7 @@ #include "common/typeconversion.h" #include "common/utils.h" +#include "config/config.h" #include "config/feature.h" #include "drivers/display.h" @@ -89,7 +90,6 @@ #include "drivers/time.h" #include "drivers/vtx_common.h" -#include "config/config.h" #include "fc/controlrate_profile.h" #include "fc/core.h" #include "fc/rc_adjustments.h" @@ -622,7 +622,7 @@ static void osdElementCrashFlipArrow(osdElementParms_t *element) rollAngle = (rollAngle < 0 ? -180 : 180) - rollAngle; } - if ((isFlipOverAfterCrashActive() || (!ARMING_FLAG(ARMED) && !STATE(SMALL_ANGLE))) && !((imuConfig()->small_angle < 180) && STATE(SMALL_ANGLE)) && (rollAngle || pitchAngle)) { + if ((isFlipOverAfterCrashActive() || (!ARMING_FLAG(ARMED) && !isUpright())) && !((imuConfig()->small_angle < 180 && isUpright()) || (rollAngle == 0 && pitchAngle == 0))) { if (abs(pitchAngle) < 2 * abs(rollAngle) && abs(rollAngle) < 2 * abs(pitchAngle)) { if (pitchAngle > 0) { if (rollAngle > 0) { diff --git a/src/test/unit/arming_prevention_unittest.cc b/src/test/unit/arming_prevention_unittest.cc index e0f2390096..1b8fea0840 100644 --- a/src/test/unit/arming_prevention_unittest.cc +++ b/src/test/unit/arming_prevention_unittest.cc @@ -73,6 +73,7 @@ extern "C" { uint16_t GPS_distanceToHome = 0; int16_t GPS_directionToHome = 0; acc_t acc = {}; + bool mockIsUpright = false; } uint32_t simulationFeatureFlags = 0; @@ -129,7 +130,7 @@ TEST(ArmingPreventionTest, CalibrationPowerOnGraceAngleThrottleArmSwitch) // given // quad is level - ENABLE_STATE(SMALL_ANGLE); + mockIsUpright = true; // when updateArmingStatus(); @@ -190,7 +191,7 @@ TEST(ArmingPreventionTest, ArmingGuardRadioLeftOnAndArmed) // and rcData[THROTTLE] = 1000; - ENABLE_STATE(SMALL_ANGLE); + mockIsUpright = true; // when updateActivatedModes(); @@ -268,7 +269,7 @@ TEST(ArmingPreventionTest, Prearm) // given rcData[THROTTLE] = 1000; - ENABLE_STATE(SMALL_ANGLE); + mockIsUpright = true; // when updateActivatedModes(); @@ -311,7 +312,7 @@ TEST(ArmingPreventionTest, RadioTurnedOnAtAnyTimeArmed) // and rcData[THROTTLE] = 1000; - ENABLE_STATE(SMALL_ANGLE); + mockIsUpright = true; // and // RX has no link to radio @@ -378,7 +379,7 @@ TEST(ArmingPreventionTest, In3DModeAllowArmingWhenEnteringThrottleDeadband) // and rcData[THROTTLE] = 1400; - ENABLE_STATE(SMALL_ANGLE); + mockIsUpright = true; simulationHaveRx = true; // and @@ -447,7 +448,7 @@ TEST(ArmingPreventionTest, When3DModeDisabledThenNormalThrottleArmingConditionAp // and // safe throttle value for 3D mode rcData[THROTTLE] = 1500; - ENABLE_STATE(SMALL_ANGLE); + mockIsUpright = true; simulationHaveRx = true; // and @@ -545,7 +546,7 @@ TEST(ArmingPreventionTest, WhenUsingSwitched3DModeThenNormalThrottleArmingCondit // and rcData[THROTTLE] = 1000; - ENABLE_STATE(SMALL_ANGLE); + mockIsUpright = true; simulationHaveRx = true; // and @@ -641,7 +642,7 @@ TEST(ArmingPreventionTest, GPSRescueWithoutFixPreventsArm) rcData[THROTTLE] = 1000; rcData[AUX1] = 1000; rcData[AUX2] = 1000; - ENABLE_STATE(SMALL_ANGLE); + mockIsUpright = true; // when updateActivatedModes(); @@ -755,7 +756,7 @@ TEST(ArmingPreventionTest, GPSRescueSwitchPreventsArm) rcData[THROTTLE] = 1000; rcData[AUX1] = 1000; rcData[AUX2] = 1800; // Start out with rescue enabled - ENABLE_STATE(SMALL_ANGLE); + mockIsUpright = true; // when updateActivatedModes(); @@ -867,7 +868,7 @@ TEST(ArmingPreventionTest, ParalyzeOnAtBoot) rcData[THROTTLE] = 1000; rcData[AUX1] = 1000; rcData[AUX2] = 1800; // Paralyze on at boot - ENABLE_STATE(SMALL_ANGLE); + mockIsUpright = true; // when updateActivatedModes(); @@ -918,7 +919,7 @@ TEST(ArmingPreventionTest, Paralyze) rcData[AUX1] = 1000; rcData[AUX2] = 1800; // Start out with paralyze enabled rcData[AUX3] = 1000; - ENABLE_STATE(SMALL_ANGLE); + mockIsUpright = true; // when updateActivatedModes(); @@ -1091,4 +1092,5 @@ extern "C" { void pidSetItermReset(bool) {} void applyAccelerometerTrimsDelta(rollAndPitchTrims_t*) {} bool isFixedWing(void) { return false; } + bool isUpright(void) { return mockIsUpright; } } diff --git a/src/test/unit/flight_imu_unittest.cc b/src/test/unit/flight_imu_unittest.cc index c31b3c986f..c4790e6cc2 100644 --- a/src/test/unit/flight_imu_unittest.cc +++ b/src/test/unit/flight_imu_unittest.cc @@ -59,6 +59,7 @@ extern "C" { extern quaternion q; extern float rMat[3][3]; + extern bool attitudeIsEstablished; PG_REGISTER(rcControlsConfig_t, rcControlsConfig, PG_RC_CONTROLS_CONFIG, 0); PG_REGISTER(barometerConfig_t, barometerConfig, PG_BAROMETER_CONFIG, 0); @@ -164,15 +165,17 @@ TEST(FlightImuTest, TestSmallAngle) // given imuConfigMutable()->small_angle = 25; + imuConfigure(0, 0); + attitudeIsEstablished = true; // and memset(rMat, 0.0, sizeof(float) * 9); // when - imuUpdateEulerAngles(); + imuComputeRotationMatrix(); // expect - EXPECT_EQ(0, STATE(SMALL_ANGLE)); + EXPECT_EQ(true, isUpright()); // given rMat[0][0] = r1; @@ -181,19 +184,19 @@ TEST(FlightImuTest, TestSmallAngle) rMat[2][2] = r1; // when - imuUpdateEulerAngles(); + imuComputeRotationMatrix(); // expect - EXPECT_EQ(SMALL_ANGLE, STATE(SMALL_ANGLE)); + EXPECT_EQ(true, isUpright()); // given memset(rMat, 0.0, sizeof(float) * 9); // when - imuUpdateEulerAngles(); + imuComputeRotationMatrix(); // expect - EXPECT_EQ(0, STATE(SMALL_ANGLE)); + EXPECT_EQ(true, isUpright()); } // STUBS diff --git a/src/test/unit/link_quality_unittest.cc b/src/test/unit/link_quality_unittest.cc index c22ae1c6d6..918109f0ad 100644 --- a/src/test/unit/link_quality_unittest.cc +++ b/src/test/unit/link_quality_unittest.cc @@ -543,4 +543,5 @@ extern "C" { return 0.0; } + bool isUpright(void) { return true; } } diff --git a/src/test/unit/osd_unittest.cc b/src/test/unit/osd_unittest.cc index 60bfb93f4f..7e54bcbc91 100644 --- a/src/test/unit/osd_unittest.cc +++ b/src/test/unit/osd_unittest.cc @@ -1173,4 +1173,5 @@ extern "C" { int8_t calculateThrottlePercent(void) { return 0; } uint32_t persistentObjectRead(persistentObjectId_e) { return 0; } void persistentObjectWrite(persistentObjectId_e, uint32_t) {} + bool isUpright(void) { return true; } } diff --git a/src/test/unit/vtx_unittest.cc b/src/test/unit/vtx_unittest.cc index 62662b4e17..33c9a8112d 100644 --- a/src/test/unit/vtx_unittest.cc +++ b/src/test/unit/vtx_unittest.cc @@ -179,4 +179,5 @@ extern "C" { void pidSetItermReset(bool) {} void applyAccelerometerTrimsDelta(rollAndPitchTrims_t*) {} bool isFixedWing(void) { return false; } + bool isUpright(void) { return true; } }