AP_Notify: make APM2 ToshibaLED driver more efficient

use one I2C transfer, not 3
AP_AHRS: prevent GPS yaw resets on compass errors

when we switch from compass heading to GPS heading we don't want to
trigger a sudden GPS yaw reset
AP_AHRS: fixed wrapping bug in GPS based heading error

this bug could cause the GPS based yaw to use an expensive reset far
too often
AP_AHRS: removed vertical G limit on GPS velocity correction

this limit could lead to a bias in the accel drift correction, and is
frequenctly exceeded
AP_AHRS: removed limit on normalisation of accel reference vectors

this could lead to a bias in the accel drift correction
AP_AHRS: save memory and reduce pointer references

use a refence for ins, and don't save gyro and accel between updates
AC_Fence: update for AHRS API changes
AP_YawController: update for AHRS API change
DataFlash: use const ins reference

save some pointer dereferences
AP_TECS: update for AHRS API change
AP_InertialSensor: use const reference returns

saves some vector copies
AP_InertialNav: update for AHRS API changes
VARTest: update for AHRS API change
Plane: update for API changes
Copter: update for AHRS API changes
Rover: update for AHRS API changes
AP_AHRS: added accel sum delay buffer to account for GPS lag
DataFlash: added timestamp to IMU and VelZ to GPS logging

both are very useful for analysis
APM_Control: reduce roll compensation in pitch controller by cos(pitch)

This reduces the use of the pitch compensation when in a steep climb
or dive

Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
AP_L1_Control: reduced demanded bank angle by cos(pitch)

this prevents attempts to do too tight turns while at a steep pitch

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