Optitrack implementation and Ardrone 2 fixes

Full optitrack implementation, only AHRS GPS frequency scaling needs to be implemented to fix the issues with using the optitrack for heading.
- Added gps based on information gathered through the datalink
- Added ground module to transmit optitrack packets to the drone

Some small Ardrone 2 fixes:
- Implemented sonar
- Fix of baro spikes