From 9ad058783afe0f28d824341a73d9eff3dd3b19f9 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Sat, 17 Mar 2018 17:56:57 +1100
Subject: [PATCH] AP_AccelCal: stop using mavlink_snoop for target traffic

---
 libraries/AP_AccelCal/AP_AccelCal.cpp | 12 ++++++------
 libraries/AP_AccelCal/AP_AccelCal.h   |  3 +++
 2 files changed, 9 insertions(+), 6 deletions(-)

diff --git a/libraries/AP_AccelCal/AP_AccelCal.cpp b/libraries/AP_AccelCal/AP_AccelCal.cpp
index 3062608f7e..226ffe53e3 100644
--- a/libraries/AP_AccelCal/AP_AccelCal.cpp
+++ b/libraries/AP_AccelCal/AP_AccelCal.cpp
@@ -21,7 +21,6 @@
 
 const extern AP_HAL::HAL& hal;
 static bool _start_collect_sample;
-static void _snoop(const mavlink_message_t* msg);
 
 uint8_t AP_AccelCal::_num_clients = 0;
 AP_AccelCal_Client* AP_AccelCal::_clients[AP_ACCELCAL_MAX_NUM_CLIENTS] {};
@@ -97,8 +96,7 @@ void AP_AccelCal::update()
                                 return;
                         }
                         _printf("Place vehicle %s and press any key.", msg);
-                        // setup snooping of packets so we can see the COMMAND_ACK
-                        _gcs->set_snoop(_snoop);
+                        _waiting_for_mavlink_ack = true;
                     }
                 }
 
@@ -277,8 +275,6 @@ void AP_AccelCal::collect_sample()
     for(uint8_t i=0 ; (cal = get_calibrator(i))  ; i++) {
         cal->collect_sample();
     }
-    // setup snooping of packets so we can see the COMMAND_ACK
-    _gcs->set_snoop(nullptr);
     _start_collect_sample = false;
     update_status();
 }
@@ -357,8 +353,12 @@ bool AP_AccelCal::client_active(uint8_t client_num)
     return (bool)_clients[client_num]->_acal_get_calibrator(0);
 }
 
-static void _snoop(const mavlink_message_t* msg)
+void AP_AccelCal::handleMessage(const mavlink_message_t* msg)
 {
+    if (!_waiting_for_mavlink_ack) {
+        return;
+    }
+    _waiting_for_mavlink_ack = false;
     if (msg->msgid == MAVLINK_MSG_ID_COMMAND_ACK) {
         _start_collect_sample = true;
     }
diff --git a/libraries/AP_AccelCal/AP_AccelCal.h b/libraries/AP_AccelCal/AP_AccelCal.h
index a7ea8e156a..2504e2a423 100644
--- a/libraries/AP_AccelCal/AP_AccelCal.h
+++ b/libraries/AP_AccelCal/AP_AccelCal.h
@@ -33,9 +33,12 @@ public:
     // interface to the clients for registration
     static void register_client(AP_AccelCal_Client* client);
 
+    void handleMessage(const mavlink_message_t * msg);
+
 private:
     GCS_MAVLINK *_gcs;
     bool _use_gcs_snoop;
+    bool _waiting_for_mavlink_ack = false;
     uint32_t _last_position_request_ms;
     uint8_t _step;
     accel_cal_status_t _status;