Added the Madgwick complementary filter for comparison. Added time measurements to measure dt
Sync of test code
Merge remote-tracking branch 'origin/master'

Conflicts:
	modules/control/src/control.c