Move compass logging into library, remove scheduler wrapper functions

APMrover2/APMrover2.cpp

@@ -85,7 +85,7 @@ const AP_Scheduler::Task Rover::scheduler_tasks[] = {
     SCHED_TASK_CLASS(AP_Notify,           &rover.notify,           update,         50,  300),
     SCHED_TASK(one_second_loop,         1,   1500),
     SCHED_TASK_CLASS(AC_Sprayer,          &rover.g2.sprayer,           update,      3,  90),
-    SCHED_TASK(compass_cal_update,     50,    200),
+    SCHED_TASK_CLASS(Compass,          &rover.compass,              cal_update, 50, 200),
     SCHED_TASK(compass_save,           0.1,   200),
     SCHED_TASK(accel_cal_update,       10,    200),
 #if LOGGING_ENABLED == ENABLED

APMrover2/Rover.h

@@ -463,7 +463,6 @@ class Rover : public AP_HAL::HAL::Callbacks {
     // sensors.cpp
     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();

APMrover2/sensors.cpp

@@ -23,17 +23,6 @@ void Rover::update_compass(void)
 {
     if (AP::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();
     }
 }
 

APMrover2/system.cpp

@@ -90,6 +90,7 @@ void Rover::init_ardupilot()
 #endif
 
     // initialise compass
+    AP::compass().set_log_bit(MASK_LOG_COMPASS);
     AP::compass().init();
 
     // initialise rangefinder

AntennaTracker/AntennaTracker.cpp

@@ -49,7 +49,7 @@ const AP_Scheduler::Task Tracker::scheduler_tasks[] = {
     SCHED_TASK_CLASS(AP_InertialSensor, &tracker.ins,       periodic,       50,   50),
     SCHED_TASK_CLASS(AP_Notify,         &tracker.notify,    update,         50,  100),
     SCHED_TASK(one_second_loop,         1,   3900),
-    SCHED_TASK(compass_cal_update,     50,    100),
+    SCHED_TASK_CLASS(Compass,          &tracker.compass,              cal_update, 50, 100),
     SCHED_TASK(accel_cal_update,       10,    100)
 };
 

AntennaTracker/Tracker.h

@@ -247,7 +247,6 @@ class Tracker : public AP_HAL::HAL::Callbacks {
     void compass_save();
     void init_compass_location();
     void update_compass(void);
-    void compass_cal_update();
     void accel_cal_update(void);
     void update_GPS(void);
     void handle_battery_failsafe(const char* type_str, const int8_t action);

AntennaTracker/sensors.cpp

@@ -30,18 +30,6 @@ void Tracker::update_compass(void)
 {
     if (AP::compass().enabled() && compass.read()) {
         ahrs.set_compass(&compass);
-        if (should_log(MASK_LOG_COMPASS)) {
-            logger.Write_Compass();
-        }
-    }
-}
-
-/*
- calibrate compass
-*/
-void Tracker::compass_cal_update() {
-    if (!hal.util->get_soft_armed()) {
-        compass.compass_cal_update();
     }
 }
 

AntennaTracker/system.cpp

@@ -60,6 +60,7 @@ void Tracker::init_tracker()
 #endif // ENABLE_SCRIPTING
 
     // initialise compass
+    AP::compass().set_log_bit(MASK_LOG_COMPASS);
     AP::compass().init();
 
     // GPS Initialization

ArduCopter/ArduCopter.cpp

@@ -310,10 +310,6 @@ void Copter::update_batt_compass(void)
         compass.set_throttle(motors->get_throttle());
         compass.set_voltage(battery.voltage());
         compass.read();
-        // log compass information
-        if (should_log(MASK_LOG_COMPASS) && !ahrs.have_ekf_logging()) {
-            logger.Write_Compass();
-        }
     }
 }
 

ArduCopter/sensors.cpp

@@ -107,12 +107,14 @@ void Copter::init_optflow()
 
 void Copter::compass_cal_update()
 {
-    static uint32_t compass_cal_stick_gesture_begin = 0;
+    compass.cal_update();
 
-    if (!hal.util->get_soft_armed()) {
-        compass.compass_cal_update();
+    if (hal.util->get_soft_armed()) {
+        return;
     }
 
+    static uint32_t compass_cal_stick_gesture_begin = 0;
+
     if (compass.is_calibrating()) {
         if (channel_yaw->get_control_in() < -4000 && channel_throttle->get_control_in() > 900) {
             compass.cancel_calibration_all();

ArduCopter/system.cpp

@@ -137,6 +137,7 @@ void Copter::init_ardupilot()
     gps.set_log_gps_bit(MASK_LOG_GPS);
     gps.init(serial_manager);
 
+    AP::compass().set_log_bit(MASK_LOG_COMPASS);
     AP::compass().init();
 
 #if OPTFLOW == ENABLED

ArduPlane/ArduPlane.cpp

@@ -59,7 +59,7 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = {
     SCHED_TASK_CLASS(AP_Notify,      &plane.notify,  update, 50, 300),
     SCHED_TASK(read_rangefinder,       50,    100),
     SCHED_TASK_CLASS(AP_ICEngine, &plane.g2.ice_control, update, 10, 100),
-    SCHED_TASK(compass_cal_update,     50,    50),
+    SCHED_TASK_CLASS(Compass,          &plane.compass,              cal_update, 50, 50),
     SCHED_TASK(accel_cal_update,       10,    50),
 #if OPTFLOW == ENABLED
     SCHED_TASK_CLASS(OpticalFlow, &plane.optflow, update,    50,    50),
@@ -202,9 +202,6 @@ void Plane::update_compass(void)
 {
     if (AP::compass().enabled() && compass.read()) {
         ahrs.set_compass(&compass);
-        if (should_log(MASK_LOG_COMPASS) && !ahrs.have_ekf_logging()) {
-            logger.Write_Compass();
-        }
     }
 }
 

ArduPlane/Plane.h

@@ -966,7 +966,6 @@ class Plane : public AP_HAL::HAL::Callbacks {
 #if ADVANCED_FAILSAFE == ENABLED
     void afs_fs_check(void);
 #endif
-    void compass_cal_update();
     void update_optical_flow(void);
     void one_second_loop(void);
     void airspeed_ratio_update(void);

ArduPlane/sensors.cpp

@@ -38,15 +38,6 @@ void Plane::read_rangefinder(void)
     rangefinder_height_update();
 }
 
-/*
-  calibrate compass
-*/
-void Plane::compass_cal_update() {
-    if (!hal.util->get_soft_armed()) {
-        compass.compass_cal_update();
-    }
-}
-
 /*
     Accel calibration
 */

ArduPlane/system.cpp

@@ -109,6 +109,7 @@ void Plane::init_ardupilot()
     // initialise airspeed sensor
     airspeed.init();
 
+    AP::compass().set_log_bit(MASK_LOG_COMPASS);
     AP::compass().init();
 
 #if OPTFLOW == ENABLED

ArduSub/ArduSub.cpp

@@ -55,7 +55,7 @@ const AP_Scheduler::Task Sub::scheduler_tasks[] = {
 #if RPM_ENABLED == ENABLED
     SCHED_TASK(rpm_update,            10,    200),
 #endif
-    SCHED_TASK(compass_cal_update,   100,    100),
+    SCHED_TASK_CLASS(Compass,          &sub.compass,              cal_update, 100, 100),
     SCHED_TASK(accel_cal_update,      10,    100),
     SCHED_TASK(terrain_update,        10,    100),
 #if GRIPPER_ENABLED == ENABLED
@@ -171,10 +171,6 @@ void Sub::update_batt_compass()
         // update compass with throttle value - used for compassmot
         compass.set_throttle(motors.get_throttle());
         compass.read();
-        // log compass information
-        if (should_log(MASK_LOG_COMPASS) && !ahrs.have_ekf_logging()) {
-            logger.Write_Compass();
-        }
     }
 }
 

ArduSub/Sub.h

@@ -455,7 +455,6 @@ class Sub : public AP_HAL::HAL::Callbacks {
     static const struct LogStructure log_structure[];
 
     void init_compass_location();
-    void compass_cal_update(void);
     void fast_loop();
     void fifty_hz_loop();
     void update_batt_compass(void);

ArduSub/sensors.cpp

@@ -104,13 +104,6 @@ void Sub::init_optflow()
 }
 #endif      // OPTFLOW == ENABLED
 
-void Sub::compass_cal_update()
-{
-    if (!hal.util->get_soft_armed()) {
-        compass.compass_cal_update();
-    }
-}
-
 void Sub::accel_cal_update()
 {
     if (hal.util->get_soft_armed()) {

ArduSub/system.cpp

@@ -104,6 +104,7 @@ void Sub::init_ardupilot()
     gps.set_log_gps_bit(MASK_LOG_GPS);
     gps.init(serial_manager);
 
+    AP::compass().set_log_bit(MASK_LOG_COMPASS);
     AP::compass().init();
 
 #if OPTFLOW == ENABLED

Tools/autotest/arducopter.py

@@ -2846,6 +2846,76 @@ def fly_throw_mode(self):
                                        (tdelta, max_good_tdelta))
         self.progress("Vehicle returned")
 
+    def test_onboard_compass_calibration(self, timeout=240):
+        twist_x = 2.1
+        twist_y = 2.2
+        twist_z = 2.3
+        ex = None
+        self.context_push()
+        try:
+            self.set_parameter("SIM_GND_BEHAV", 0)
+            self.set_parameter("AHRS_EKF_TYPE", 10)
+            self.reboot_sitl()
+
+            report = self.mav.messages.get("MAG_CAL_REPORT", None)
+            if report is not None:
+                raise PreconditionFailedException("MAG_CAL_REPORT found")
+
+            self.run_cmd(mavutil.mavlink.MAV_CMD_DO_START_MAG_CAL,
+                         1, # bitmask of compasses to calibrate
+                         0,
+                         0,
+                         0,
+                         0,
+                         0,
+                         0,
+                         timeout=1)
+            tstart = self.get_sim_time()
+            last_twist_time = 0
+            while True:
+                now = self.get_sim_time_cached()
+                if now - tstart > timeout:
+                    raise NotAchievedException("timeout before cal complete")
+                report = self.mav.messages.get("MAG_CAL_REPORT", None)
+                if report is not None:
+                    print("Report: %s" % str(report))
+                    break
+                if now - last_twist_time > 5:
+                    last_twist_time = now
+                    twist_x *= 1.1
+                    twist_y *= 1.2
+                    twist_z *= 1.3
+                    if abs(twist_x) > 10:
+                        twist_x /= -2
+                    if abs(twist_y) > 10:
+                        twist_y /= -2
+                    if abs(twist_z) > 10:
+                        twist_z /= -2
+                    self.set_parameter("SIM_TWIST_X", twist_x)
+                    self.set_parameter("SIM_TWIST_Y", twist_y)
+                    self.set_parameter("SIM_TWIST_Z", twist_z)
+                    try:
+                        self.set_parameter("SIM_TWIST_TIME", 100)
+                    except ValueError as e:
+                        # the shove resets this to zero
+                        pass
+
+                m = self.mav.recv_match(type="MAG_CAL_PROGRESS", timeout=1)
+                self.progress("progress: %s" % str(m))
+                if m is None:
+                    continue
+                att = self.mav.messages.get("ATTITUDE", None)
+                self.progress("Attitude: %f %f %f" %
+                              (math.degrees(att.roll), math.degrees(att.pitch), math.degrees(att.yaw)))
+        except Exception as e:
+            print("Exception caught: %s" % str(e))
+            ex = e
+
+        self.context_pop()
+        self.reboot_sitl()
+        if ex is not None:
+            raise ex
+
     def fly_brake_mode(self):
         # test brake mode
         self.progress("Testing brake mode")
@@ -3237,6 +3307,10 @@ def tests(self):
              "Fly POSHOLD takeoff",
              self.fly_poshold_takeoff),
 
+            ("OnboardCompassCalibration",
+             "Test onboard compass calibration",
+             self.test_onboard_compass_calibration),
+
             ("LogDownLoad",
              "Log download",
              lambda: self.log_download(

Tools/autotest/common.py

@@ -998,7 +998,7 @@ def should_fetch_all_for_parameter_change(param_name):
             return True
         return False
 
-    def set_parameter(self, name, value, add_to_context=True, epsilon=0.00001):
+    def set_parameter(self, name, value, add_to_context=True, epsilon=0.0002):
         """Set parameters from vehicle."""
         self.progress("Setting %s to %f" % (name, value))
         old_value = self.get_parameter(name, retry=2)

libraries/AP_Compass/AP_Compass.cpp

@@ -981,7 +981,11 @@ Compass::read(void)
     if (_learn == LEARN_INFLIGHT && learn != nullptr) {
         learn->update();
     }
-    return healthy();
+    bool ret = healthy();
+    if (ret && _log_bit != (uint32_t)-1 && AP::logger().should_log(_log_bit) && !AP::ahrs().have_ekf_logging()) {
+        AP::logger().Write_Compass();
+    }
+    return ret;
 }
 
 uint8_t

libraries/AP_Compass/AP_Compass.h

@@ -123,7 +123,7 @@ friend class AP_Compass_Backend;
     const Vector3f &get_field(void) const { return get_field(get_primary()); }
 
     // compass calibrator interface
-    void compass_cal_update();
+    void cal_update();
 
     // per-motor calibration access
     void per_motor_calibration_start(void) {
@@ -143,6 +143,9 @@ friend class AP_Compass_Backend;
     bool compass_cal_requires_reboot() const { return _cal_complete_requires_reboot; }
     bool is_calibrating() const;
 
+    // indicate which bit in LOG_BITMASK indicates we should log compass readings
+    void set_log_bit(uint32_t log_bit) { _log_bit = log_bit; }
+
     /*
       handle an incoming MAG_CAL command
     */
@@ -404,6 +407,9 @@ friend class AP_Compass_Backend;
     // first-time-around flag used by offset nulling
     bool        _null_init_done;
 
+    // stores which bit is used to indicate we should log compass readings
+    uint32_t _log_bit = -1;
+
     // used by offset correction
     static const uint8_t _mag_history_size = 20;
 

libraries/AP_Compass/AP_Compass_Calibration.cpp

@@ -7,8 +7,12 @@
 extern AP_HAL::HAL& hal;
 
 void
-Compass::compass_cal_update()
+Compass::cal_update()
 {
+    if (hal.util->get_soft_armed()) {
+        return;
+    }
+
     bool running = false;
 
     for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {