Tiny Whoop Angle Mode Crash Causea ACC "freakout" in 3.2.x

[ProjectMockingbird]

This issue might be with other quads in angle mode, but I only run Angle in Whoops with High PID values. Tested on BetaFPV Betaflight F3, BeeCore v2 (Omnibus) and various other TW FC's

Upon spinning or tumbling crashes in brushed tiny whoop sized quads, if you don't wait at least 4-5 seconds to re-arm, your Whoop will flip over, scoot off, and generally act like the ACC is messed up. You can try to re-cal the ACC, but it doesn't seem to wipe out the calculations, and it also takes very long to do the stick commands, which is killer in a Whoop race.

Issue happens, but is lessened if you switch out of Angle, go to Acro, fly a little and then flip back to Angle (also not great for racing).

Solved if you unplug and plug back in...also not great for a race.

FEATURE REQUEST: Would be great if the ACC could be turned OFF when disarmed, and then ON when armed, that way you could wipe out all bad calculations on a bad crash.

STEPS TO REPRODUCE

1. Plug in Whoop (I have mine not to acc calibrate on plug-in)
2. arm in Angle mode and fly just a little and land and disarm
3. while disarmed, spin and toss into the air
4. quickly put on ground and arm
5. watch it as it flips over and goes crazy

# Betaflight / OMNIBUS (OMNI) 3.2.4 Jan 10 2018 / 23:01:30 (90d32fd56) MSP API: 1.36
name BeeCoreWhoop

map TAER1234
serial 2 64 115200 57600 0 115200
aux 0 0 0 1450 2100
aux 1 1 1 1300 1700
aux 2 2 1 900 1250
aux 3 28 0 1300 1925
aux 4 30 0 1300 1700
adjrange 0 1 1 1450 1550 12 1
rxrange 0 1009 1990
rxrange 1 1009 1990
rxrange 2 1074 1924
rxrange 3 1162 1836
set gyro_sync_denom = 2
set gyro_notch1_hz = 0
set gyro_notch2_hz = 0
set baro_hardware = NONE
set rc_interp = OFF
set rc_interp_int = 1
set fpv_mix_degrees = 20
set serialrx_provider = SPEK2048
set min_throttle = 1040
set use_unsynced_pwm = ON
set motor_pwm_protocol = BRUSHED
set motor_pwm_rate = 16000
set yaw_motors_reversed = ON
set small_angle = 180
set deadband = 1
set yaw_deadband = 1
set pid_process_denom = 1
profile 0

set dterm_lowpass_type = PT1
set dterm_notch_hz = 0
set dterm_setpoint_weight = 100
set p_pitch = 100
set i_pitch = 130
set d_pitch = 60
set p_roll = 90
set i_roll = 110
set d_roll = 60
set p_yaw = 180
set i_yaw = 120
set p_level = 80
set i_level = 80
set d_level = 0
set level_limit = 60
set horizon_tilt_effect = 25
rateprofile 1

set rc_rate = 221
set rc_rate_yaw = 207
set roll_srate = 0
set pitch_srate = 0
set yaw_srate = 0

# 

[brucesdad13]

I'll try to post some logs in the next few days. I also experience this and didn't think much of it until competing at the 2018 Tiny Whoop Invitational. After a little crash and tumble I might end up right side up. Having disarmed I would rearm and, if the quad was in angle mode with airmode enabled, it would simply flip upside down. Someone would set it right. I even at times gave the stick commands to recalibrate acc. We resorted to taking off in acro mode and then at some point several seconds later switching to angle. Those flying the Inductrix flight controller were able to get back into the air almost immediately. Hoping I get some data that might indicate a workaround or means to quicken the acc recovery or wipe the slate clean on disarm as you mentioned. Happened with 3.2 maint branch and 3.3 RCs

[jflyper]

@brucesdad13 Log speaks for all.

[ProjectMockingbird]

As a test last night with a Whoop with OSD, I did the steps to reproduce, but after spinning the quad and setting it down, I quickly went into the OSD menu and hit "Save and Reboot" and 100% of the time the quad re-armed normally and no freak out. ACC_Calibration on stick commands does not have the same result.

[brucesdad13]

Ok I managed to squeeze a few in a row into this log. I modified the code slightly to prevent disarm from closing the blackbox log.

if (blackboxConfig()->device && blackboxConfig()->mode != 2) { // 2 being BLACKBOX_MODE_ALWAYS_ON

Classic example at 01:29.664 arm after crash and it flips over without rc input. Another good one at 2:20.456

BTFL_BLACKBOX_LOG_BRUCESDAD_20180228_012827.zip

[brucesdad13]

[brucesdad13]

The ACC actually doesn't seem to be freaked out at all. It's very stable. The PID_sum values for pitch and roll seems pretty tremendous at rearm e.g. PID_sum[roll] | -3206 | -320.6%

[mikeller]

Good finding, @brucesdad13. There were discussions in other places already about stopping the PID loop when not armed, or at least resetting the PIDs when arming. This makes a strong point in favour of this.

I think the above modification for blackbox should go into Betaflight in any case, can you please open a pull request?

[etracer65]

The problem is not in the PID controller continuing to run as iterm is reset and not accumulating. The problem is all about P reacting to the source of error and that doesn't matter if it wasn't running while disarmed. It would just immediately reset to the same high value on arming anyway.

I really think the problem is in the imu orientation estimation. I think it gets overwhelmed by the high and seemingly random gyro rates seen during a crash. Even hard flying in acro and then switching to self-level can show this as the imu orientation takes time to "settle". You can see the settling or convergence delay when even calibrating the ACC. Put a high angle on the quad and click calibrate in the Configurator and watch how slowly the 3D model takes to converge to level. The imu logic is seeing an instant change and has to converge to it.

It really seems like we simply need to reset the imu's estimate of orientation on arming - maybe as simple as calling imuInit() but I'm not sure. imu.c is full of scary matrix and quaternion math that I'm not even pretending to understand. Also resetting wouldn't fix the situation where the estimate gets significantly off in flight and then messes up when switching to angle mode, but it would fix the crash/rearm problem (I think).

[mikeller]

@etracer65: Ok, makes sense. But wouldn't resetting the IMU on arming wreak havoc with stabilised modes if it was done when the craft was at an angle while arming (which is probably the norm after crashes)?

[etracer65]

Well I'm not even pretending to understand the math in imu.c but basically we need to throw out the "estimate" and reinitialize based on the current absolute ACC readings. No motion so no reason to converge to an estimate. Besides, the orientation is set correctly when the FC is initialized on power up.

[etracer65]

"Resetting" the estimated orientation is kind of fixing the symptom though as the problem is in the orientation logic not being able to deal or "keep up" with the high rates seen by the gyro. If we could only get faster ACC sampling then we wouoldn't even need the imu estimation logic integrating gyro rates. But unfortunately that's no possible with current hardware.

Who is our "expert" on imu.c and the self-leveling code?

[etracer65]

Although resetting the imu on arming would have a very negative effect on the use case of disarming and then attempting to rearm during flight if the quad was in self-leveling or some other imu-related mode like altitude hold.

[ProjectMockingbird]

Could you have the reset tied to Prearm instead of arm if that was a concern?

It totally makes sense what @etracer65 said about how slow calibration is and it's relationship to slow crash recovery...I bet if I timed it and how long it takes the Whoop to get back into flying condition, it would probably be very close. Awesome insight, guys!

[etracer65]

So here's a video showing what happens to the imu orientation estimate when the FC sees high gyro rates. This is directly applicable to the scenario of hard acro flight and then switching to self-level and having the ACC level being way off.

In this test I have a bare FC connected via USB. I start with the board flat on the table and calibrate the ACC just to be sure. Then at 0:10 I pick up the FC by the USB cable and "twirl" the cable between my fingers to cause high pitch gyro rates (the animation doesn't do the movement justice). Then at 0:16 the FC is returned to level on the table. Note how slowly the orientation estimate converges back to level - over 20 seconds!

https://youtu.be/wKAT6-8YmC4

So in the case of the tiny whoop crash I think the high gyro rates caused by the crash are doing the same thing. This also is confirmed by the observation of "waiting a while" before rearming will not experience the problem.

I also observed that if you supply a lot of high gyro rates to the imu it will sometimes lose the ability to converge and remain significantly variant from level. Interestingly doing a ACC calibration doesn't seem to fix this so some offset is getting accumulated that doesn't return to zero. But this doesn't seem easy to reproduce and I've only observed it a few times.

[etracer65]

Regarding resetting the orientation estimate on arming...

The problem is that the only sensor you have to initially calculate orientation is the ACC. But you can't trust that if the quad is moving because we can't distinguish acceleration caused by gravity vs. caused by motion.

So perhaps we check at arming and if the gyro is at very low rates for all axes then we reset, otherwise not. This would fix the tiny whoop crash issue as long as the quad came to full rest before attempting to re-arm. In the use-case of disarming in flight and then re-arming any rotation coefficients would cancel the reset. In the unlikely case that the quad was falling perfectly still with no rotation resetting wouldn't be a problem (I'm thinking) because no motion affecting the acceleration felt by the axes.

Though still not sure "how" to reset the orientation short of a full imuInit() what other effects that would have.

[brucesdad13]

I'm not entirely certain I'm following you @etracer65. The logs are showing 0 deg/s gyro movement on all three axes and accelerometer is reporting basically 0g on X and Y and 1g in Z. 😕 Edit: reading further I think I'm understanding you now... the estimate and absolute values are slow to converge.

@mikeller I'll submit a PR for blackbox separately 😄

[etracer65]

@brucesdad13 Yes, that's partly the point. The imu estimate of the orientation gets dramatically incorrect from the high gyro rates seen during the crash. It then takes a long time for the orientation to converge back to reality (see the video I posted above). So if you re-arm before the orientation gets back to near reality the self-leveling code tries to adjust to the false orientation information.

We can't reset the orientation simply based on the accelerometer IF the quad is moving. Thus I was suggesting that might be a way forward - look for no (or very little) rotation movement on the gyro axes. This would be inline with what your log is showing.

[brucesdad13]

Yep. Whoop crashes and pilot disarms. Someone runs over and places it level on a hard flat surface. Gyro and Acc reading no movement. Pilot arms and flies away.

[etracer65]

Yes, but you're missing the point that if you were to connect the quad to the configurator after it was flipped over and before taking off, you would see the orientation incorrect on the setup screen with it slowly converging to level. Quad thinks it's not level (when it is) so it tries to adjust - causing the flip-over crash.

[brucesdad13]

I understood you @etracer65 😄 I was agreeing with your suggestion. If the IMU shows the quad is not moving allow some sort of soft reset of the estimates. Maybe in tryArm() near imuQuaternionHeadfreeOffsetSet();?

[etracer65]

Yes, I'm setting up a testing build to try out different options.

[ledvinap]

@etracer65 : Kalman is mathematically sound way to implement it (some trick would be necessary to handle covariance degradation on overflow)

But it would be enough to keep estimate how trustworthy orientation is (state estimate covariance) and set IMU gain accordingly (from measurement noise).

[etracer65]

@ledvinap For now I'm just testing out ways to reset the attitude estimate on arming when no gyro motion is detected rather than fix the drift issue on high rates. Treating the symptom rather than the cause. Been too long since I tried to understand maths like this...

I can conditionally on arming increase dcmKpGain and get it to converge much faster, but that's probably not good enough. Any ideas how to reset the attitude estimate to match orientation suggested by just the accelerometer without having to converge?

I guess alternately we could speed up the convergence when disarmed and no gyro activity is detected. It already does this during initial power on.

[etracer65]

Ok, I think I have something. Looked in iNav to see if they were doing anything differently and their imu.c is related but somewhat different. But got lucky and found this:

static void imuResetOrientationQuaternion(const float ax, const float ay, const float az)
{
    const float accNorm = sqrtf(ax * ax + ay * ay + az * az);

    q0 = az + accNorm;
    q1 = ay;
    q2 = -ax;
    q3 = 0.0f;

    const float recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);

    q0 *= recipNorm;
    q1 *= recipNorm;
    q2 *= recipNorm;
    q3 *= recipNorm;
}

Not a direct drop-in because they use q0, q1, etc for the quaternion where we use q.w, q.x, etc. So I translated it to:

static void imuResetOrientationQuaternion(const float ax, const float ay, const float az)
{
    const float accNorm = sqrtf(ax * ax + ay * ay + az * az);

    q.w = az + accNorm;
    q.x = ay;
    q.y = -ax;
    q.z = 0.0f;

    const float recipNorm = invSqrt(q.w * q.w + q.x * q.x + q.y * q.y + q.z * q.z);

    q.w *= recipNorm;
    q.x *= recipNorm;
    q.y *= recipNorm;
    q.z *= recipNorm;
}

But this needs review because I don't fully understand all the math in imu.c yet.

However I rigged up a test platform where I could trigger the reset off a mode switch to see the effect. Recreated the scenario in the example video above and triggered the reset while the attitude was slowly converging. The attitude reset to the correct orientation immediately.

So people with more math skills than me need to chime to see if there's anything wrong with this approach. The plan would be to reset the quaternion to current accelerometer reading of gravity on arming, but only if the gyro rates are very low (quad not moving). This will prevent resetting in the case where somebody disarmed and re-armed in flight.

[etracer65]

Well under the assumption that I'm the right track, I made a version that resets the quaternion based on the current accelerometer on arming - but only when the gyro rates are low for all axes.

I don't have a tiny whoop but I tested on my 8.5mm brushed motor quad and everything seemed reasonable. No apparent change in self-leveling stability between multiple armings.

The branch is here if you want to check it out: https://github.com/etracer65/betaflight/tree/imu_quaternion_reset

@brucesdad13 If you want to try it out and can't build it yourself, let me know what target you need and I'll build one for you. If you need SPRACINGF3EVO you probably won't be able to build it yourself as it's overflowing by 100 bytes (again!), but I can give you a build with some things disabled for testing.

[brucesdad13]

Thanks @etracer65 😄 I have compiler and everything here. Working on the blackbox logging bit for @mikeller 's request but then will give yours a spin. Thank you so much 👍

[ProjectMockingbird]

Hey @etracer65, I'd love an omnibus f3 or betaflight f3 build to run it through the scenarios I've done... Thank you all so much for the work on this so far!

[etracer65]

@ProjectMockingbird Here you go. Treat these as extremely beta.

betaflight_3.3.0_BETAFLIGHTF3.hex.zip betaflight_3.3.0_OMNIBUS.hex.zip

[ProjectMockingbird]

Awesome! Will load it up as soon as I get out of traffic and home.

[ProjectMockingbird]

@etracer65 First thing I noticed is that calibration is much faster (watching the numbers and model in the config) Spinning and twisting the Whoop while plugged in resolved faster. So, that's better.

Then went on to my test

• Arm and fly around in Angle mode
• Land, take whoop and toss in air spinning around ROLL
• catch, toss in air spinning around PITCH
• catch, and toss in air spinning around YAW
• then set on level surface and arm

It still flipped out unfortunately...but, it seemed to resolve more quickly than before.

I then did some real-world testing simulating the bigger two story crashes we'd have at the invitational, and yeah, it would still flip on re-arm if I did it right away, but instead of 5-10 seconds to resolve, it was more like 3-5 seconds.

Hopefully @brucesdad13 did some blackbox to see if his Alienwhoop did the same thing.

This was with a BetaFPV Betaflight F3 board.

[etracer65]

Well, back to the drawing board I guess. There were no changes that affected "calibration" or how quickly the attitude estimation converged on it's own (the slow return to level in the configurator 3D model) so I think that part was wishful thinking. The actual change is that if you watched in the configurator as the model slowly returned to level and then armed, you would see the 3D model "snap" back to the actual orientation immediately.

I still convinced that the attitude estimation being incorrect is related - but maybe only part of the cause. I'll keep looking.

[ledvinap]

@etracer65: imuResetOrientationQuaternion is problematic - it seems to provide correct quaternion (so that gravity points down), but it resets heading (with following transient if compass is installed) Also acc noise may be quite problematic.

It would be cleaner to increase gain so that IMU converges quickly. Ideally with gain scheduling, so that noise is suppressed better ...

[etracer65]

Well I'm quite sure that the slow attitude convergence after heavy gyro activity is part of the problem - maybe not all but definitely a big part.

Here's a blackbox of a test without the reset code. Note the wild activity (me whipping the FC around to get lots of gyro activity), then I set it back down level. The pidSums are still high but gradually decreasing as the attitude estimate is slowly converging. The mode was angle mode so that the PID controller is "pushing" trying to get the quad level based on what it sees from the attitude estimation. So as the attitude slowly returns to level the pidSum decreases proportionally. Before the attitude estimation gets completely level I arm. You can see the motors instantly go crazy trying to correct the (false) level - instant flip.

The next blackbox is with the reset code. Same procedure but notice how the pidSums reset at arming and the motors don't go crazy. This is because the attitude estimate was reset to a correct reading when arming occurred.

[etracer65]

@ledvinap Yeah, I thought it might be problematic. To go with the increased gain approach we'd have to base the logic on the disarm even rather than arming because even with increasing the gain substantially the settle time would be too slow to do at arming (and prevent the PID controller from reacting).

What would you perceive as an upper limit for the gain scaling? Currently the default is 1.0 but it is increased to 10.0 when disarmed and during the first 20 seconds after board power to expedite convergence. Maybe just overthinking this entirely and instead we just always increase the gain when disarmed to achieve faster convergence?

[ledvinap]

@etracer65 : You can probably boost gain quite a lot. It is set very low because it must filter quad acceleration from ACC reading during flight - the gain should be as low as possible, just removing gyro drift.

When disarmed, gain should only filter ACC noise (and possibly acceleration if arming after throw is desirable).

Covariance approach is still preferable - it can handle in-flight gyro saturation and gain error.

[etracer65]

@ledvinap Increasing the scale for the gain to 100.0 when disarmed seems to work pretty well. The attitude estimation converges quite quickly (< 1 second). Tried going even higher like 1000.0 but that probably led to overflow somewhere as the attitude was flipping around crazily.

I think I'll try a test build where the gain is increased whenever disarmed and reset to the default 1.0 scale when armed.

Back to the reset logic: Couldn't the Z component be recalculated from the absolute MAG readings rather than setting it to 0? Not saying I know the math to do that, but just wondering out loud.

[ledvinap]

@etracer65 : quaternion/ z component - yes, you can solve equation for ACC + MAG and set orientation accordingly. IIRC math is in Mahony paper. But it will still have problems with acc/mag noise ...

[ledvinap]

@etracer65 : High gain is problematic with current implementation, it does assume that gain is relatively small, so (1-alpha) ~ 1 (not true complementary filter) and that cross product is decent approximation of error (sin(x) ~ x). With too high gain, estimation update will overshoot 'true' orientation (with slightly higher gain, it will overshoot so much that error is actually larger) and cause oscillation.

Another approach to get high gain is to take estimator as P regulator and use PID tuning heuristics.

[etracer65]

I'm going to focus on just the symptom of the attitude estimate being incorrect after a crash rather than try and tackle improvements to the the estimate algorithms in general.

It seems there are at least two things contributing to large errors in the attitude after a crash - both high gyro rate related:

1. We're only calculating the attitude estimate at 1KHz which makes sense for the ACC, but leads to using a 8 point moving average for the gyro at 8KHz and obviously worse at 32KHz.
2. During a crash the physical gyro rates may be exceeding 2000dps and are being clipped.

Item 1 is also likely the primary cause of attitude drift in flight after periods of hard acro. But unless we shift the attitude calculations into the gyro loop I'm not sure how to improve this.

So back to dealing with the crash scenario... I think my approach will trigger on disarm and do the following:

• Wait for a minimum time period of idle gyros to ensure the quad has stopped tumbling or is still not in flight (disarm in the air).
• Accelerate the attitude convergence by increasing the gain for a period long enough to get very close to reality with the goal to get the convergence to happen in < 1 second.
• Reset the gain back to the standard setting.

Also since the actual gain used is a product of the scale and user configured imu_dcm_kp so I'll need to select and appropriate hardcoded gain value rather than simply adjusting the scale. Otherwise user changes to imu_dcm_kp could prevent the convergence completing (if set very low) or leading to oscillation (when set very high).

[ledvinap]

IMU update is called at 300Hz. The difference is negligible from my preliminary tests (may have slight effect on heading, but not attitude).

Gain error is causing large drift after full roll (gyro values does not integrate to full circle)

Your approach is sound for disarm state. Gain value should be quite independent on configuration, so universal value should be fine. It is probably possible (and easy - just calculate gain that will bring error to zero and decrease it a bit) to calculate/simulate it, but it may be better to aim for reasonable time constant.

[brucesdad13]

Let me know if there's anything I can do to help. I don't have comprehension of the math involved. 😕 I am able to record short blackbox logs on my dataflash chip. 😄

[brucesdad13]

Thanks @etracer65 for the "reset_attitude_on_disarm" test branch. Please find attached a blackbox log with set debug_mode = ATTITUDE_RESET and set blackbox_mode = ALWAYS from my AlienWhoop F4 V3 and 6mm brushed motors. I lowered the logging rate to 500Hz per your suggestion.

The disarm triggered attitude reset functionality seems to be working great. Once, I didn't wait quite long enough and the quad flipped on arm but, in general, by the time I've walked over to the quad and flipped it and let it settle briefly I am able to arm and fly away! 🏆

I don't believe I captured it on this log but there still seems, as you noted previously, the issue where hard acro maneuvers are performed and then the mode is changed in flight to angle and the quad veers off or flips and crashes.

Regardless, this disarm code will help countless micro brushed pilots no doubt. Cheers! 👍 💯 😁

BTFL_BLACKBOX_LOG_BRUCESDAD_20180306_003614.zip

[ProjectMockingbird]

Whoa!? Is there an Omnibus F3 build that I can try? This is exciting news!

[etracer65]

@ledvinap Any thoughts on imuIsAccelerometerHealthy() in imu.c?

static bool imuIsAccelerometerHealthy(void)
{
    float accMagnitude = 0;
    for (int axis = 0; axis < 3; axis++) {
        const float a = acc.accADC[axis];
        accMagnitude += a * a;
    }

    accMagnitude = accMagnitude * 100 / (sq((int32_t)acc.dev.acc_1G));

    // Accept accel readings only in range 0.90g - 1.10g
    return (81 < accMagnitude) && (accMagnitude < 121);
}

I ran into a case where it returned false while the flight controller was sitting stationary after a simulated crash (I tossed the flight controller on the ground). From the blackbox during the period the function returned false, I had the following:

Performing the calculation on this data I come up with 122.04683423 which is just outside the range of a "healthy" acc based on the function. Clearly the acc was healthy and the data usable as the fc was stationary on the floor and had been for over 5 seconds so it wasn't a transient event due to the dynamics of the crash. Seems like the range for determining a "healthy" acc needs to be adjusted. Thoughts?

[etracer65]

@ledvinap And a follow-up regarding the use of imuIsAccelerometerHealthy() in imuMahonyAHRSupdate()...

The result of imuIsAccelerometerHealthy() is passed to imuMahonyAHRSupdate() via the variable useAcc. As a result imuMahonyAHRSupdate() is only using the ACC data if the acceleration vector is withing the limited range.

    // Use measured acceleration vector
    float recipAccNorm = sq(ax) + sq(ay) + sq(az);
    if (useAcc && recipAccNorm > 0.01f) {
        // Normalise accelerometer measurement
        recipAccNorm = invSqrt(recipAccNorm);
        ax *= recipAccNorm;
        ay *= recipAccNorm;
        az *= recipAccNorm;

        // 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]);
    }

Is it true we want to exclude ACC from the calculation when the vector is not within the narrow range? Hard turns or punching out can easily exceed 1.21G.

[etracer65]

@brucesdad13 Thanks for the testing and log. Good that you saw improvement. Thanks to your log I noticed some cases where the reset didn't trigger as well as it could have so I was able to make a few more improvements. After a little more testing and discussion about the acceleration vectors above I'll make another update to try. @ProjectMockingbird I'll build you an OMNIBUS target as well once it's ready.

[brucesdad13]

I'm all for honoring the accelerometer data to the same level we do gyro data. Or perhaps, as with crash recovery, the acc magnitude limits could be configurable. I know flash space is limited for f3 targets. I'm inclined to comment out that the call to ishealthy or return true and see what happens tonight 😁 I sent @ProjectMockingbird targets for OMNIBUS and BETAFLIGHTF3 last night. Is there some case of truly unhealthy accelerometers?

[etracer65]

And if imuIsAccelerometerHealthy() is trying to calculate the acceleration vector it seems like it's not doing it correctly. The formula used is:

But shouldn't it be:

[ledvinap]

@etracer65 : It's complicated ;-)

imuIsAccelerometerHealthy() tries to return true when gravity vector is pointing down - when flight-induced acceleration is low. It is just a heuristic and possibly bad one.

Without it, quad acceleration will affect IMU estimate. It is noticeable in ANGLE mode when stopping from forward flight - with sticks centered, quad will end up 'reversing' for a while.

The range is probably too narrow, it is causing problems with noisier quads when almost no acc readings are used.

Removing it completely (and possibly increasing IMU time constant) may work quite well.

Probably best way to handle this is to model expected acceleration based on throttle output and use difference to update attitude estimate (read Kalman / EKF .... )

---

After-crash reading is 1.1G - 10% relative error, it is quite large (and outside gyro specs). Proper ACC calibration may help (current 'calibration' is terrible) ..

---

imuIsAccelerometerHealthy is computing square of ACC vector to save expensive sqrt

[etracer65]

@ledvinap Thanks. Based on this I'm going to remove imuIsAccelerometerHealthy() consideration and simply check to see that the ACC is enabled in the disarmed attitude reset (accelerated gain) logic.

Looking to change the range or possibly eliminating altogether should be a separate analysis and PR as it affects armed in-flight dynamics.

One other question: Any concerns about generally increasing the kP gain when disarmed. Currently the logic uses a scale factor of 10.0 during the first 20 seconds to speed up initial convergence. After that it switches to a scale of 1.0. I'm proposing eliminating the 20 second aspect and just use a scale of 10.0 all the time (when disarmed). What I'm seeing is the attitude reset logic working well and reaching convergence after a disarm, then the pilot flips the crashed quad back over to take off. The manual reorientation is sometimes enough to cause substantial error in the attitude due to the slow kP gain.