From f25d20f54996cf6f950622edc7dcd847b5f7a583 Mon Sep 17 00:00:00 2001
From: Michael du Breuil <wicked.shell.scripts@gmail.com>
Date: Thu, 11 Jun 2020 17:35:32 -0700
Subject: [PATCH] AP_Compass: Only send a single MAG_CAL_* message per poll

this fairly allocates bandwidth between the calibrators
---
 libraries/AP_Compass/AP_Compass.h             |  2 +
 .../AP_Compass/AP_Compass_Calibration.cpp     | 56 +++++++++++--------
 2 files changed, 36 insertions(+), 22 deletions(-)

diff --git a/libraries/AP_Compass/AP_Compass.h b/libraries/AP_Compass/AP_Compass.h
index f9762dda1a..66273f7c6c 100644
--- a/libraries/AP_Compass/AP_Compass.h
+++ b/libraries/AP_Compass/AP_Compass.h
@@ -377,6 +377,8 @@ private:
     bool _start_calibration(uint8_t i, bool retry=false, float delay_sec=0.0f);
     bool _start_calibration_mask(uint8_t mask, bool retry=false, bool autosave=false, float delay_sec=0.0f, bool autoreboot=false);
     bool _auto_reboot() { return _compass_cal_autoreboot; }
+    Priority next_cal_progress_idx[MAVLINK_COMM_NUM_BUFFERS];
+    Priority next_cal_report_idx[MAVLINK_COMM_NUM_BUFFERS];
 
     // see if we already have probed a i2c driver by bus number and address
     bool _have_i2c_driver(uint8_t bus_num, uint8_t address) const;
diff --git a/libraries/AP_Compass/AP_Compass_Calibration.cpp b/libraries/AP_Compass/AP_Compass_Calibration.cpp
index 38f7fe5a9b..a919a3f848 100644
--- a/libraries/AP_Compass/AP_Compass_Calibration.cpp
+++ b/libraries/AP_Compass/AP_Compass_Calibration.cpp
@@ -230,19 +230,14 @@ bool Compass::_accept_calibration_mask(uint8_t mask)
 
 bool Compass::send_mag_cal_progress(const GCS_MAVLINK& link)
 {
-    uint8_t cal_mask = _get_cal_mask();
+    const mavlink_channel_t chan = link.get_chan();
 
-    for (Priority compass_id(0); compass_id<COMPASS_MAX_INSTANCES; compass_id++) {
-        // ensure we don't try to send with no space available
-        if (!HAVE_PAYLOAD_SPACE(link.get_chan(), MAG_CAL_PROGRESS)) {
-            // ideally we would only send one progress message per
-            // call.  If we don't return true here we may end up
-            // hogging *all* the bandwidth
-            return true;
-        }
+    for (uint8_t i = 0; i < COMPASS_MAX_INSTANCES; i++) {
+        const Priority compass_id = (next_cal_progress_idx[chan] + 1) % COMPASS_MAX_INSTANCES;
         
         auto& calibrator = _calibrator[compass_id];
         if (calibrator == nullptr) {
+            next_cal_progress_idx[chan] = compass_id;
             continue;
         }
         const CompassCalibrator::State cal_state = calibrator->get_state();
@@ -250,12 +245,22 @@ bool Compass::send_mag_cal_progress(const GCS_MAVLINK& link)
         if (cal_state.status == CompassCalibrator::Status::WAITING_TO_START  ||
             cal_state.status == CompassCalibrator::Status::RUNNING_STEP_ONE ||
             cal_state.status == CompassCalibrator::Status::RUNNING_STEP_TWO) {
+            // ensure we don't try to send with no space available
+            if (!HAVE_PAYLOAD_SPACE(chan, MAG_CAL_PROGRESS)) {
+                return false;
+            }
+
+            next_cal_progress_idx[chan] = compass_id;
+
             mavlink_msg_mag_cal_progress_send(
                 link.get_chan(),
-                uint8_t(compass_id), cal_mask,
+                uint8_t(compass_id),
+                _get_cal_mask(),
                 (uint8_t)cal_state.status, cal_state.attempt, cal_state.completion_pct, cal_state.completion_mask,
                 0.0f, 0.0f, 0.0f
             );
+        } else {
+            next_cal_progress_idx[chan] = compass_id;
         }
     }
 
@@ -264,28 +269,33 @@ bool Compass::send_mag_cal_progress(const GCS_MAVLINK& link)
 
 bool Compass::send_mag_cal_report(const GCS_MAVLINK& link)
 {
-    uint8_t cal_mask = _get_cal_mask();
+    const mavlink_channel_t chan = link.get_chan();
+
+    for (uint8_t i = 0; i < COMPASS_MAX_INSTANCES; i++) {
+        const Priority compass_id = (next_cal_report_idx[chan] + 1) % COMPASS_MAX_INSTANCES;
 
-    for (Priority compass_id(0); compass_id<COMPASS_MAX_INSTANCES; compass_id++) {
-        // ensure we don't try to send with no space available
-        if (!HAVE_PAYLOAD_SPACE(link.get_chan(), MAG_CAL_REPORT)) {
-            // ideally we would only send one progress message per
-            // call.  If we don't return true here we may end up
-            // hogging *all* the bandwidth
-            return true;
-        }
         if (_calibrator[compass_id] == nullptr) {
+            next_cal_report_idx[chan] = compass_id;
             continue;
         }
         const CompassCalibrator::Report cal_report = _calibrator[compass_id]->get_report();
         if (cal_report.status == CompassCalibrator::Status::SUCCESS ||
             cal_report.status == CompassCalibrator::Status::FAILED ||
             cal_report.status == CompassCalibrator::Status::BAD_ORIENTATION) {
-            uint8_t autosaved = _cal_saved[compass_id];
+
+            // ensure we don't try to send with no space available
+            if (!HAVE_PAYLOAD_SPACE(chan, MAG_CAL_REPORT)) {
+                return false;
+            }
+
+            next_cal_report_idx[chan] = compass_id;
+
             mavlink_msg_mag_cal_report_send(
                 link.get_chan(),
-                uint8_t(compass_id), cal_mask,
-                (uint8_t)cal_report.status, autosaved,
+                uint8_t(compass_id),
+                _get_cal_mask(),
+                (uint8_t)cal_report.status,
+                _cal_saved[compass_id],
                 cal_report.fitness,
                 cal_report.ofs.x, cal_report.ofs.y, cal_report.ofs.z,
                 cal_report.diag.x, cal_report.diag.y, cal_report.diag.z,
@@ -295,6 +305,8 @@ bool Compass::send_mag_cal_report(const GCS_MAVLINK& link)
                 cal_report.orientation,
                 cal_report.scale_factor
             );
+        } else {
+            next_cal_report_idx[chan] = compass_id;
         }
     }
     return true;