as soon as we hit the SPEED_TURN_DIST we lower target speed to the
specified gain

we need to ensure that prev_WP_loc is not used as it could be
completely different from current_loc. As a precuation, this also sets
up next_WP_loc so that when the set_next_WP() is first called that
prev_WP_loc is set to current_loc

Don't do bias estimation if tilted by more than 60 degrees to prevent scale
factor errors affecting result unnecessarily.
Prevent Kalman gain from having the wrong sign due to numerical errors
associated with small process noise values.
Allow smaller Z accel bias process noise values to be set

Bring Plane glitch protection thresholds into alignment with copter and
rover
Slight increase in accelerometer bias process noise to prevent bias
estimate divergence into limits (Rover and Plane only as Copter does not
seem respond as well to this change)
effective increase in threshold on divergence test to allow increased
margin for bad GPS velocities

Fix #1068. When receiving guided waypoints do not change to GUIDED mode. This serves as a safety precaution for GCS, since they must switch to guided mode before sending the waypoints.

this ensures drivers can run

This provides consistent behaviour for a range of gyro bias process
noise settings and automatically adjusts sensitivity as filter learns bias

This is required to stop the bias estimate from becoming too static
towards the end of longer flights.

At the moment, the copter sonar set up does not have access to the
raw sonar readings so this code sets the voltage field to zero.

this provides a common way of dealing with UART setup for a GCS
instance. It includes code to cope with SiK radios stuck in bootloader
mode.

this prevents an initialisation error, and is reasonable in flight too

Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>

we need to check health of each accelerometer separately

Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>

will be used to indicate AHRS health

Divergence is now detected by looking for very large changes in the gyro
bias. This will cause the filter to be reset and declared unhealthy for
10 seconds.

Don't reset filterDiverged status immediately during reset

Set filterDiverged true if covariance blows up

Add fault status reporting

this will be used to report when the AHRS subsystem becomes unhealthy

logs all accel data at high rate to SD card