Mag current calibration

I made a calibration script for the magnetometer, to calibrate for interference from currents in the wires. I tested it in my transition branch, there it works very well. At thrust settings where I would normally get 30 degrees deviation from the magnetic north, this is now only 3 degrees.

The calibration is done by slowly ramping up the motors with the uav held steady on the ground. It is done best without the control laws to avoid feedback on the heading which will then influence the magnetometer reading again.

Theoretically a better implementation would be to calibrate for each motor individually, but that would require separate calibration code to spin each motor up. This could be a next step.

I also flight tested with the correction in place and this resulted in a much better autonomous flight.