diff --git a/APMrover2/APMrover2.cpp b/APMrover2/APMrover2.cpp index dcb78800aa..037b135e7e 100644 --- a/APMrover2/APMrover2.cpp +++ b/APMrover2/APMrover2.cpp @@ -205,20 +205,6 @@ void Rover::gcs_failsafe_check(void) failsafe_trigger(FAILSAFE_EVENT_GCS, failsafe.last_heartbeat_ms != 0 && (millis() - failsafe.last_heartbeat_ms) > 2000); } -/* - check for new compass data - 10Hz - */ -void Rover::update_compass(void) -{ - if (g.compass_enabled && compass.read()) { - ahrs.set_compass(&compass); - // update offsets - if (should_log(MASK_LOG_COMPASS)) { - logger.Write_Compass(); - } - } -} - /* log some key data - 10Hz */ diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h index ed0b700338..d1aa60ceb0 100644 --- a/APMrover2/Rover.h +++ b/APMrover2/Rover.h @@ -386,7 +386,6 @@ private: void stats_update(); void ahrs_update(); void gcs_failsafe_check(void); - void update_compass(void); void update_logging1(void); void update_logging2(void); void one_second_loop(void); @@ -481,11 +480,12 @@ private: // sensors.cpp void init_compass(void); void init_compass_location(void); + void update_compass(void); + void compass_cal_update(void); + void compass_save(void); void init_beacon(); void init_visual_odom(); void update_wheel_encoder(); - void compass_cal_update(void); - void compass_save(void); void accel_cal_update(void); void read_rangefinders(void); void init_proximity(); diff --git a/APMrover2/sensors.cpp b/APMrover2/sensors.cpp index ff5dc37529..126b99e223 100644 --- a/APMrover2/sensors.cpp +++ b/APMrover2/sensors.cpp @@ -34,6 +34,34 @@ void Rover::init_compass_location(void) } } +// check for new compass data - 10Hz +void Rover::update_compass(void) +{ + if (g.compass_enabled && compass.read()) { + ahrs.set_compass(&compass); + // update offsets + if (should_log(MASK_LOG_COMPASS)) { + logger.Write_Compass(); + } + } +} + +// Calibrate compass +void Rover::compass_cal_update() { + if (!hal.util->get_soft_armed()) { + compass.compass_cal_update(); + } +} + +// Save compass offsets +void Rover::compass_save() { + if (g.compass_enabled && + compass.get_learn_type() >= Compass::LEARN_INTERNAL && + !arming.is_armed()) { + compass.save_offsets(); + } +} + // init beacons used for non-gps position estimates void Rover::init_beacon() { @@ -110,22 +138,6 @@ void Rover::update_wheel_encoder() wheel_encoder_last_ekf_update_ms = now; } -// Calibrate compass -void Rover::compass_cal_update() { - if (!hal.util->get_soft_armed()) { - compass.compass_cal_update(); - } -} - -// Save compass offsets -void Rover::compass_save() { - if (g.compass_enabled && - compass.get_learn_type() >= Compass::LEARN_INTERNAL && - !arming.is_armed()) { - compass.save_offsets(); - } -} - // Accel calibration void Rover::accel_cal_update() {