From e763896b6a8db57582936f7b8a7f3a9259fa2016 Mon Sep 17 00:00:00 2001
From: Francisco Ferreira <francisco-ferreira@mail.telepac.pt>
Date: Sat, 12 Nov 2016 09:49:08 +0000
Subject: [PATCH] AP_AccelCal: send and receive MAV_CMD_ACCELCAL_VEHICLE_POS

Message is sent at a maximum of 1Hz
Status text is sent, but if command is received it stops and only uses commands
---
 libraries/AP_AccelCal/AP_AccelCal.cpp | 76 ++++++++++++++++++---------
 libraries/AP_AccelCal/AP_AccelCal.h   |  6 +++
 2 files changed, 56 insertions(+), 26 deletions(-)

diff --git a/libraries/AP_AccelCal/AP_AccelCal.cpp b/libraries/AP_AccelCal/AP_AccelCal.cpp
index 758594b62c..af20d7250f 100644
--- a/libraries/AP_AccelCal/AP_AccelCal.cpp
+++ b/libraries/AP_AccelCal/AP_AccelCal.cpp
@@ -17,6 +17,8 @@
 #include <GCS_MAVLink/GCS_MAVLink.h>
 #include <AP_HAL/AP_HAL.h>
 
+#define AP_ACCELCAL_POSITION_REQUEST_INTERVAL_MS 1000
+
 const extern AP_HAL::HAL& hal;
 static bool _start_collect_sample;
 static void _snoop(const mavlink_message_t* msg);
@@ -69,33 +71,41 @@ void AP_AccelCal::update()
                 if (step != _step) {
                     _step = step;
 
-                    const char *msg;
-                    switch (step) {
-                        case ACCELCAL_VEHICLE_POS_LEVEL:
-                            msg = "level";
-                            break;
-                        case ACCELCAL_VEHICLE_POS_LEFT:
-                            msg = "on its LEFT side";
-                            break;
-                        case ACCELCAL_VEHICLE_POS_RIGHT:
-                            msg = "on its RIGHT side";
-                            break;
-                        case ACCELCAL_VEHICLE_POS_NOSEDOWN:
-                            msg = "nose DOWN";
-                            break;
-                        case ACCELCAL_VEHICLE_POS_NOSEUP:
-                            msg = "nose UP";
-                            break;
-                        case ACCELCAL_VEHICLE_POS_BACK:
-                            msg = "on its BACK";
-                            break;
-                        default:
-                            fail();
-                            return;
+                    if(_use_gcs_snoop) {
+                        const char *msg;
+                        switch (step) {
+                            case ACCELCAL_VEHICLE_POS_LEVEL:
+                                msg = "level";
+                                break;
+                            case ACCELCAL_VEHICLE_POS_LEFT:
+                                msg = "on its LEFT side";
+                                break;
+                            case ACCELCAL_VEHICLE_POS_RIGHT:
+                                msg = "on its RIGHT side";
+                                break;
+                            case ACCELCAL_VEHICLE_POS_NOSEDOWN:
+                                msg = "nose DOWN";
+                                break;
+                            case ACCELCAL_VEHICLE_POS_NOSEUP:
+                                msg = "nose UP";
+                                break;
+                            case ACCELCAL_VEHICLE_POS_BACK:
+                                msg = "on its BACK";
+                                break;
+                            default:
+                                fail();
+                                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);
                     }
-                    _printf("Place vehicle %s and press any key.", msg);
-                    // setup snooping of packets so we can see the COMMAND_ACK
-                    _gcs->set_snoop(_snoop);
+                }
+
+                uint32_t now = AP_HAL::millis();
+                if (now - _last_position_request_ms > AP_ACCELCAL_POSITION_REQUEST_INTERVAL_MS) {
+                    _last_position_request_ms = now;
+                    _gcs->send_accelcal_vehicle_position(step);
                 }
                 break;
             }
@@ -167,6 +177,8 @@ void AP_AccelCal::start(GCS_MAVLINK *gcs)
     _started = true;
     _saving = false;
     _gcs = gcs;
+    _use_gcs_snoop = true;
+    _last_position_request_ms = 0;
     _step = 0;
 
     update_status();
@@ -329,6 +341,18 @@ static void _snoop(const mavlink_message_t* msg)
     }
 }
 
+bool AP_AccelCal::gcs_vehicle_position(float position)
+{
+    _use_gcs_snoop = false;
+
+    if (_status == ACCEL_CAL_WAITING_FOR_ORIENTATION && is_equal((float) _step, position)) {
+        _start_collect_sample = true;
+        return true;
+    }
+
+    return false;
+}
+
 void AP_AccelCal::_printf(const char* fmt, ...)
 {
     if (!_gcs) {
diff --git a/libraries/AP_AccelCal/AP_AccelCal.h b/libraries/AP_AccelCal/AP_AccelCal.h
index 58266b614d..195553dd1e 100644
--- a/libraries/AP_AccelCal/AP_AccelCal.h
+++ b/libraries/AP_AccelCal/AP_AccelCal.h
@@ -10,6 +10,7 @@ class AP_AccelCal_Client;
 class AP_AccelCal {
 public:
     AP_AccelCal():
+    _use_gcs_snoop(true),
     _started(false),
     _saving(false)
     { update_status(); }
@@ -25,12 +26,17 @@ public:
 
     // get the status of the calibrator server as a whole
     accel_cal_status_t get_status() { return _status; }
+    
+    // Set vehicle position sent by the GCS
+    bool gcs_vehicle_position(float position);
 
     // interface to the clients for registration
     static void register_client(AP_AccelCal_Client* client);
 
 private:
     GCS_MAVLINK *_gcs;
+    bool _use_gcs_snoop;
+    uint32_t _last_position_request_ms;
     uint8_t _step;
     accel_cal_status_t _status;