New sonar alt and vel. estim. TBC.
Sonar PID working
Optical flow velocity estimate obtained.

Todo: get position too (without acc because precision is good. maybe
just LPF). Make code for PID.
Stableish... Flow + Sonar
Rewrite PID for flybarless system
Sonar & FBL & Flow working together. Tuning req.
Improved sonar tracking filter. Completely stable.

Sonar filter: added sonar reading delay compensation before calculating
accelerometer bias.

Needs work on:
-IMU rotation matrix
-Tuning
-Replace MPU6050 by the one with magnetometer.
Added flow navigation & throttle batt compensation

NEEDS WORK. FALLS WHEN SONAR MODE ACTIVATED.
Started to write code for FBL100... TBC
In progress....
Motor output for FBL100
Servo output for FBL100
Added default PID in code. Fixed code for both FBL100 and CX2
Started fixed sonar for sporadic readings. tbc
Fixed long cycle time. Rewriting sonar IMU. TBC
Merge remote-tracking branch 'origin/IMU-filter' into upstream_shared

Conflicts:
	MultiWii/IMU.cpp
	MultiWii/Output.cpp
	MultiWii/config.h
	MultiWii/def.h