From 6830a8b86c15789d8a05144a60bb37842dad8093 Mon Sep 17 00:00:00 2001
From: Michael du Breuil <wicked.shell.scripts@gmail.com>
Date: Mon, 29 May 2017 14:03:36 -0700
Subject: [PATCH] AccelCal: Continously report success/failure to the GCS that
 requested the calibration

---
 libraries/AP_AccelCal/AP_AccelCal.cpp | 27 +++++++++++++++++++++++++--
 libraries/AP_AccelCal/AP_AccelCal.h   |  1 +
 2 files changed, 26 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_AccelCal/AP_AccelCal.cpp b/libraries/AP_AccelCal/AP_AccelCal.cpp
index af20d7250f..3062608f7e 100644
--- a/libraries/AP_AccelCal/AP_AccelCal.cpp
+++ b/libraries/AP_AccelCal/AP_AccelCal.cpp
@@ -156,6 +156,23 @@ void AP_AccelCal::update()
                 fail();
                 return;
         }
+    } else if (_last_result != ACCEL_CAL_NOT_STARTED) {
+        // only continuously report if we have ever completed a calibration
+        uint32_t now = AP_HAL::millis();
+        if (now - _last_position_request_ms > AP_ACCELCAL_POSITION_REQUEST_INTERVAL_MS) {
+            _last_position_request_ms = now;
+            switch (_last_result) {
+                case ACCEL_CAL_SUCCESS:
+                    _gcs->send_accelcal_vehicle_position(ACCELCAL_VEHICLE_POS_SUCCESS);
+                    break;
+                case ACCEL_CAL_FAILED:
+                    _gcs->send_accelcal_vehicle_position(ACCELCAL_VEHICLE_POS_FAILED);
+                    break;
+                default:
+                    // should never hit this state
+                    break;
+            }
+        }
     }
 }
 
@@ -181,6 +198,8 @@ void AP_AccelCal::start(GCS_MAVLINK *gcs)
     _last_position_request_ms = 0;
     _step = 0;
 
+    _last_result = ACCEL_CAL_NOT_STARTED;
+
     update_status();
 }
 
@@ -192,6 +211,8 @@ void AP_AccelCal::success()
         _clients[i]->_acal_event_success();
     }
 
+    _last_result = ACCEL_CAL_SUCCESS;
+
     clear();
 }
 
@@ -203,6 +224,8 @@ void AP_AccelCal::cancel()
         _clients[i]->_acal_event_cancellation();
     }
 
+    _last_result = ACCEL_CAL_NOT_STARTED;
+
     clear();
 }
 
@@ -214,6 +237,8 @@ void AP_AccelCal::fail()
         _clients[i]->_acal_event_failure();
     }
 
+    _last_result = ACCEL_CAL_FAILED;
+
     clear();
 }
 
@@ -228,8 +253,6 @@ void AP_AccelCal::clear()
         cal->clear();
     }
 
-    _gcs = nullptr;
-
     _step = 0;
     _started = false;
     _saving = false;
diff --git a/libraries/AP_AccelCal/AP_AccelCal.h b/libraries/AP_AccelCal/AP_AccelCal.h
index 195553dd1e..a7ea8e156a 100644
--- a/libraries/AP_AccelCal/AP_AccelCal.h
+++ b/libraries/AP_AccelCal/AP_AccelCal.h
@@ -39,6 +39,7 @@ private:
     uint32_t _last_position_request_ms;
     uint8_t _step;
     accel_cal_status_t _status;
+    accel_cal_status_t _last_result;
 
     static uint8_t _num_clients;
     static AP_AccelCal_Client* _clients[AP_ACCELCAL_MAX_NUM_CLIENTS];