Fix decimal point bugs found by unit test
Fix decimal point bugs found by unit test

ethz-asl/qgc_asl,Hunter522/qgroundcontrol,ethz-asl/qgc_asl,Hunter522/qgroundcontrol,Hunter522/qgroundcontrol,ethz-asl/qgc_asl,ethz-asl/qgc_asl,Hunter522/qgroundcontrol,Hunter522/qgroundcontrol,ethz-asl/qgc_asl,Hunter522/qgroundcontrol