From 587ab7178a45cb4fee196dfdb45bd206139204ac Mon Sep 17 00:00:00 2001 From: Zap Andersson Date: Sat, 19 Mar 2016 11:14:38 +0100 Subject: [PATCH 1/6] Removed the hard accelerometer cutoff, replaced my a smooth weighting. Also got rid of a square root :) --- src/main/flight/imu.c | 37 +++++++++++++++++++------------------ 1 file changed, 19 insertions(+), 18 deletions(-) diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index d72331ddd1..77590eb96c 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -224,7 +224,7 @@ static float imuGetPGainScaleFactor(void) } static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz, - bool useAcc, float ax, float ay, float az, + float useAcc, float ax, float ay, float az, bool useMag, float mx, float my, float mz, bool useYaw, float yawError) { @@ -272,19 +272,21 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz, ez += rMat[2][2] * ez_ef; } - // Use measured acceleration vector - recipNorm = sq(ax) + sq(ay) + sq(az); - if (useAcc && recipNorm > 0.01f) { - // Normalise accelerometer measurement - recipNorm = invSqrt(recipNorm); + if (useAcc > 0.0f) { + // Just scale by 1G length - That's our vector adjustment. Rather than + // using one-over-exact length (which needs a costly square root), we already + // know the vector is enough "roughly unit length" and since it is only weighted + // in by a certain amount anyway later, having that exact is meaningless. + recipNorm = 1.0f / acc_1G;; + ax *= recipNorm; ay *= recipNorm; az *= recipNorm; + - // Error is sum of cross product between estimated direction and measured direction of gravity - ex += (ay * rMat[2][2] - az * rMat[2][1]); - ey += (az * rMat[2][0] - ax * rMat[2][2]); - ez += (ax * rMat[2][1] - ay * rMat[2][0]); + ex += (ay * rMat[2][2] - az * rMat[2][1]) * useAcc; + ey += (az * rMat[2][0] - ax * rMat[2][2]) * useAcc; + ez += (ax * rMat[2][1] - ay * rMat[2][0]) * useAcc; } // Compute and apply integral feedback if enabled @@ -353,7 +355,7 @@ STATIC_UNIT_TESTED void imuUpdateEulerAngles(void) } } -static bool imuIsAccelerometerHealthy(void) +static float imuAccelerometerWeight(void) { int32_t axis; int32_t accMagnitude = 0; @@ -362,10 +364,11 @@ static bool imuIsAccelerometerHealthy(void) accMagnitude += (int32_t)accSmooth[axis] * accSmooth[axis]; } - accMagnitude = accMagnitude * 100 / (sq((int32_t)acc_1G)); + accMagnitude = accMagnitude * 100 / ((((int32_t)acc_1G)*((int32_t)acc_1G))); - // Accept accel readings only in range 0.90g - 1.10g - return (81 < accMagnitude) && (accMagnitude < 121); + int32_t nearness = ABS(100 - accMagnitude); + if (nearness > 25) return 0.0f; + return 1.0f - (nearness / 25.0f); } static bool isMagnetometerHealthy(void) @@ -377,7 +380,7 @@ static void imuCalculateEstimatedAttitude(void) { static uint32_t previousIMUUpdateTime; float rawYawError = 0; - bool useAcc = false; + float useAcc = 0.0f; bool useMag = false; bool useYaw = false; @@ -385,9 +388,7 @@ static void imuCalculateEstimatedAttitude(void) uint32_t deltaT = currentTime - previousIMUUpdateTime; previousIMUUpdateTime = currentTime; - if (imuIsAccelerometerHealthy()) { - useAcc = true; - } + useAcc = imuAccelerometerWeight(); if (sensors(SENSOR_MAG) && isMagnetometerHealthy()) { useMag = true; From ca0fb6c9cfc27fbca6e87b678a22d3c5cc148739 Mon Sep 17 00:00:00 2001 From: Zap Andersson Date: Sat, 19 Mar 2016 13:01:53 +0100 Subject: [PATCH 2/6] Made the beeper for "RX Lost" shut up if RX was never gained in the 1st place. Shuts quad up when you just put it down on the ground, or just plug it into USB or whatever. --- src/main/flight/failsafe.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index 5c8872fa2c..66a0a6cc39 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -172,7 +172,8 @@ void failsafeUpdateState(void) bool failsafeSwitchIsOn = IS_RC_MODE_ACTIVE(BOXFAILSAFE); beeperMode_e beeperMode = BEEPER_SILENCE; - if (!receivingRxData) { + // Beep RX lost only if we are not seeing data, but we have seen it in the past. + if (!receivingRxData && failsafeState.validRxDataReceivedAt) { beeperMode = BEEPER_RX_LOST; } From c0e01e8d2b11a762b242f5d80d5a1ac0f5ea2f54 Mon Sep 17 00:00:00 2001 From: Zap Andersson Date: Sat, 19 Mar 2016 16:16:49 +0100 Subject: [PATCH 3/6] Add 'WAS_EVER_ARMED' arming flag --- src/main/config/runtime_config.h | 3 ++- src/main/flight/failsafe.c | 4 ++-- src/main/mw.c | 1 + 3 files changed, 5 insertions(+), 3 deletions(-) diff --git a/src/main/config/runtime_config.h b/src/main/config/runtime_config.h index 6b4dee82e5..9bb81fe11f 100644 --- a/src/main/config/runtime_config.h +++ b/src/main/config/runtime_config.h @@ -21,7 +21,8 @@ typedef enum { OK_TO_ARM = (1 << 0), PREVENT_ARMING = (1 << 1), - ARMED = (1 << 2) + ARMED = (1 << 2), + WAS_EVER_ARMED = (1 << 3) } armingFlag_e; extern uint8_t armingFlags; diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index 66a0a6cc39..86384212ed 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -172,8 +172,8 @@ void failsafeUpdateState(void) bool failsafeSwitchIsOn = IS_RC_MODE_ACTIVE(BOXFAILSAFE); beeperMode_e beeperMode = BEEPER_SILENCE; - // Beep RX lost only if we are not seeing data, but we have seen it in the past. - if (!receivingRxData && failsafeState.validRxDataReceivedAt) { + // Beep RX lost only if we are not seeing data and we have been armed earlier + if (!receivingRxData && ARMING_FLAG(WAS_EVER_ARMED)) { beeperMode = BEEPER_RX_LOST; } diff --git a/src/main/mw.c b/src/main/mw.c index d8e2f30820..f4e823b035 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -366,6 +366,7 @@ void mwArm(void) } if (!ARMING_FLAG(PREVENT_ARMING)) { ENABLE_ARMING_FLAG(ARMED); + ENABLE_ARMING_FLAG(WAS_EVER_ARMED); headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw); #ifdef BLACKBOX From 451060a8aba5cda496642b08e0aba0290a56a84f Mon Sep 17 00:00:00 2001 From: Zap Andersson Date: Sat, 19 Mar 2016 16:58:28 +0100 Subject: [PATCH 4/6] Revert "Removed the hard accelerometer cutoff, replaced my a smooth weighting." This reverts commit 587ab7178a45cb4fee196dfdb45bd206139204ac. --- src/main/flight/imu.c | 37 ++++++++++++++++++------------------- 1 file changed, 18 insertions(+), 19 deletions(-) diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 77590eb96c..d72331ddd1 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -224,7 +224,7 @@ static float imuGetPGainScaleFactor(void) } static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz, - float useAcc, float ax, float ay, float az, + bool useAcc, float ax, float ay, float az, bool useMag, float mx, float my, float mz, bool useYaw, float yawError) { @@ -272,21 +272,19 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz, ez += rMat[2][2] * ez_ef; } - if (useAcc > 0.0f) { - // Just scale by 1G length - That's our vector adjustment. Rather than - // using one-over-exact length (which needs a costly square root), we already - // know the vector is enough "roughly unit length" and since it is only weighted - // in by a certain amount anyway later, having that exact is meaningless. - recipNorm = 1.0f / acc_1G;; - + // Use measured acceleration vector + recipNorm = sq(ax) + sq(ay) + sq(az); + if (useAcc && recipNorm > 0.01f) { + // Normalise accelerometer measurement + recipNorm = invSqrt(recipNorm); ax *= recipNorm; ay *= recipNorm; az *= recipNorm; - - ex += (ay * rMat[2][2] - az * rMat[2][1]) * useAcc; - ey += (az * rMat[2][0] - ax * rMat[2][2]) * useAcc; - ez += (ax * rMat[2][1] - ay * rMat[2][0]) * useAcc; + // Error is sum of cross product between estimated direction and measured direction of gravity + ex += (ay * rMat[2][2] - az * rMat[2][1]); + ey += (az * rMat[2][0] - ax * rMat[2][2]); + ez += (ax * rMat[2][1] - ay * rMat[2][0]); } // Compute and apply integral feedback if enabled @@ -355,7 +353,7 @@ STATIC_UNIT_TESTED void imuUpdateEulerAngles(void) } } -static float imuAccelerometerWeight(void) +static bool imuIsAccelerometerHealthy(void) { int32_t axis; int32_t accMagnitude = 0; @@ -364,11 +362,10 @@ static float imuAccelerometerWeight(void) accMagnitude += (int32_t)accSmooth[axis] * accSmooth[axis]; } - accMagnitude = accMagnitude * 100 / ((((int32_t)acc_1G)*((int32_t)acc_1G))); + accMagnitude = accMagnitude * 100 / (sq((int32_t)acc_1G)); - int32_t nearness = ABS(100 - accMagnitude); - if (nearness > 25) return 0.0f; - return 1.0f - (nearness / 25.0f); + // Accept accel readings only in range 0.90g - 1.10g + return (81 < accMagnitude) && (accMagnitude < 121); } static bool isMagnetometerHealthy(void) @@ -380,7 +377,7 @@ static void imuCalculateEstimatedAttitude(void) { static uint32_t previousIMUUpdateTime; float rawYawError = 0; - float useAcc = 0.0f; + bool useAcc = false; bool useMag = false; bool useYaw = false; @@ -388,7 +385,9 @@ static void imuCalculateEstimatedAttitude(void) uint32_t deltaT = currentTime - previousIMUUpdateTime; previousIMUUpdateTime = currentTime; - useAcc = imuAccelerometerWeight(); + if (imuIsAccelerometerHealthy()) { + useAcc = true; + } if (sensors(SENSOR_MAG) && isMagnetometerHealthy()) { useMag = true; From d82b3a44eb165f8a8c02a7f18745aa4054f8a1c3 Mon Sep 17 00:00:00 2001 From: Zap Andersson Date: Sun, 20 Mar 2016 13:24:04 +0100 Subject: [PATCH 5/6] Fix tabs --- src/main/config/runtime_config.h | 2 +- src/main/mw.c | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/config/runtime_config.h b/src/main/config/runtime_config.h index 9bb81fe11f..8366f96067 100644 --- a/src/main/config/runtime_config.h +++ b/src/main/config/runtime_config.h @@ -22,7 +22,7 @@ typedef enum { OK_TO_ARM = (1 << 0), PREVENT_ARMING = (1 << 1), ARMED = (1 << 2), - WAS_EVER_ARMED = (1 << 3) + WAS_EVER_ARMED = (1 << 3) } armingFlag_e; extern uint8_t armingFlags; diff --git a/src/main/mw.c b/src/main/mw.c index f4e823b035..c65734413a 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -366,7 +366,7 @@ void mwArm(void) } if (!ARMING_FLAG(PREVENT_ARMING)) { ENABLE_ARMING_FLAG(ARMED); - ENABLE_ARMING_FLAG(WAS_EVER_ARMED); + ENABLE_ARMING_FLAG(WAS_EVER_ARMED); headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw); #ifdef BLACKBOX From 0fb34a64b52c92c8ae21328fd6c1d520aed14d92 Mon Sep 17 00:00:00 2001 From: Zap Andersson Date: Sun, 20 Mar 2016 13:25:40 +0100 Subject: [PATCH 6/6] Fix tabs again --- src/main/flight/failsafe.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index 86384212ed..a553cb58f9 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -172,7 +172,7 @@ void failsafeUpdateState(void) bool failsafeSwitchIsOn = IS_RC_MODE_ACTIVE(BOXFAILSAFE); beeperMode_e beeperMode = BEEPER_SILENCE; - // Beep RX lost only if we are not seeing data and we have been armed earlier + // Beep RX lost only if we are not seeing data and we have been armed earlier if (!receivingRxData && ARMING_FLAG(WAS_EVER_ARMED)) { beeperMode = BEEPER_RX_LOST; }