use new vector2 cross product, and factor out the z component of a dcm
mul_transpose() to reduce the number of floating point operations for
a yaw drift correction cycle
this switches to the GPS for yaw if the compass has dragged us off by
more than 45 degrees from the GPS heading, and the wind speed is less
than 80% of the ground speed.
this uses a new formulation of the GPS based drift correction from
Bill Premerlani that rotates the error vector to avoid relying on
accurate yaw. This means we should get accurate roll/pitch correction
even with lots of magnetometer interference
It also makes it possible to fly a multicopter with no compass. It can
even navigate and correct yaw (slowly!)
this follows the method that Bill developed in his fastRotations
paper. We've demonstrated that this is indeed needed in APM, as we
were able to produce the 'dizzy' effects in both the ArduPlane and
ArduCopter simulator
omega_I applied continuously. _ki larger. Stop integrating when _omega.length()>20
The key change was the scaling of ge to ensure the error is not
quadratic
This makes 3 major changes:
1) fixes the scaling of the yaw drift correction term to fix the time
constant
2) don't integrate the mag vector over multiple readings
3) accumulate omega_I changes over 15 seconds before applying, to try
to prevent omega_I picking up short term responses
this buffers up _omega_I changes in _omega_I_sum over a period of 10
seconds, applying the slope limit only when _omega_I_sum is
transferred to _omega_I.
The result is a huge improvement in the ability of _omega_I to track
gyro drift over the long term.
when a user first connects with USB, and later switches to the
telemetry port without restarting we were getting zero for error_yaw
in the logs, as AHRS.get_error_yaw() was being called twice.
This ensures we give the last value after the counter is reset
this cleans up the separation of drift rates and proportional
correction from yaw source and accelerometers, allow the yaw
to run at a different rate to the accel correction
in_motion is not a good name now it is also used for the compass
The error_course and heading component values don't need to be part of
the DCM object, they can be on the stack to reduce the memory usage a bit
when we get a bad DCM error we can recover a matrix corresponding to
the current attitude, making it more likely that the aircraft will be
able to recover
Andrew Tridgell
Randy Mackay
Pat Hickey
uncrustify
Jason Short
Jonathan Challinger