From f30d999e0baa11915be0255dfa02f50579acd0d5 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 2 Aug 2018 11:36:52 +0900 Subject: [PATCH] Copter: remove compassmot's compass init and accumulate when compass mot is started, the main loop has been running so the compass will already have been initialised and have good readings Also remove unnecessary call to get_primary compass --- ArduCopter/compassmot.cpp | 9 +-------- 1 file changed, 1 insertion(+), 8 deletions(-) diff --git a/ArduCopter/compassmot.cpp b/ArduCopter/compassmot.cpp index 8be9a6fa29..b020df599e 100644 --- a/ArduCopter/compassmot.cpp +++ b/ArduCopter/compassmot.cpp @@ -82,9 +82,6 @@ MAV_RESULT Copter::mavlink_compassmot(mavlink_channel_t chan) // disable cpu failsafe failsafe_disable(); - // initialise compass - init_compass(); - // default compensation type to use current if possible if (battery.has_current()) { comp_type = AP_COMPASS_MOT_COMP_CURRENT; @@ -118,10 +115,6 @@ MAV_RESULT Copter::mavlink_compassmot(mavlink_channel_t chan) } // get initial compass readings - last_run_time = millis(); - while ( millis() - last_run_time < 500 ) { - compass.accumulate(); - } compass.read(); // store initial x,y,z compass values @@ -141,7 +134,7 @@ MAV_RESULT Copter::mavlink_compassmot(mavlink_channel_t chan) last_send_time = millis(); // main run while there is no user input and the compass is healthy - while (command_ack_start == command_ack_counter && compass.healthy(compass.get_primary()) && motors->armed()) { + while (command_ack_start == command_ack_counter && compass.healthy() && motors->armed()) { // 50hz loop if (millis() - last_run_time < 20) { // grab some compass values