AP_InsertialSensor: added support for MAVLink user interaction

allows APM to ask user to print enter to continue via MAVLink messages
during accel calibration

libraries/AP_InertialSensor/AP_InertialSensor.h

@@ -183,5 +183,6 @@ protected:
 #include "AP_InertialSensor_Stub.h"
 #include "AP_InertialSensor_PX4.h"
 #include "AP_InertialSensor_UserInteract_Stream.h"
+#include "AP_InertialSensor_UserInteract_MAVLink.h"
 
 #endif // __AP_INERTIAL_SENSOR_H__

libraries/AP_InertialSensor/AP_InertialSensor_UserInteract.h

@@ -8,7 +8,6 @@
 class AP_InertialSensor_UserInteract {
 public:
     virtual uint8_t blocking_read() = 0;
-    virtual void println_P(const prog_char_t *) = 0;
     virtual void _printf_P(const prog_char *, ...) = 0;
 };
 

libraries/AP_InertialSensor/AP_InertialSensor_UserInteract_MAVLink.cpp

@@ -0,0 +1,47 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#include <stdarg.h>
+#include <AP_HAL.h>
+#include <GCS_MAVLink.h>
+#include "AP_InertialSensor_UserInteract_MAVLink.h"
+
+extern const AP_HAL::HAL& hal;
+
+uint8_t AP_InertialSensor_UserInteract_MAVLink::blocking_read(void) 
+{
+    uint32_t start_ms = hal.scheduler->millis();
+    /* Wait for a COMMAND_ACK message to be received from the ground station */
+    while (hal.scheduler->millis() - start_ms < 30000U) {
+        while (!comm_get_available(_chan)) {
+            hal.scheduler->delay(1);
+        }
+        uint8_t c = comm_receive_ch(_chan);
+        mavlink_message_t msg;
+        mavlink_status_t status;
+        if (mavlink_parse_char(_chan, c, &msg, &status)) {
+            if (msg.msgid == MAVLINK_MSG_ID_COMMAND_ACK) {
+                return 0;
+            }
+        }
+    }
+    hal.console->println_P(PSTR("Timed out waiting for user response"));
+    return 0;
+}
+
+void AP_InertialSensor_UserInteract_MAVLink::_printf_P(const prog_char* fmt, ...) 
+{
+    char msg[50];
+    va_list ap;
+    va_start(ap, fmt);
+    hal.util->vsnprintf_P(msg, sizeof(msg), (const prog_char_t *)fmt, ap);
+    va_end(ap);
+    if (msg[strlen(msg)-1] == '\n') {
+        // STATUSTEXT messages should not add linefeed
+        msg[strlen(msg)-1] = 0;
+    }
+    while (comm_get_txspace(_chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES < sizeof(mavlink_statustext_t)) {
+        hal.scheduler->delay(1);        
+    }
+    mavlink_msg_statustext_send(_chan, 1, msg);
+}
+

libraries/AP_InertialSensor/AP_InertialSensor_UserInteract_MAVLink.h

@@ -0,0 +1,24 @@
+
+#ifndef __AP_INERTIAL_SENSOR_USER_INTERACT_MAVLINK_H__
+#define __AP_INERTIAL_SENSOR_USER_INTERACT_MAVLINK_H__
+
+#include <AP_HAL.h>
+#include <GCS_MAVLink.h>
+#include "AP_InertialSensor_UserInteract.h"
+
+/**
+ * AP_InertialSensor_UserInteract, implemented in terms of a MAVLink connection
+ */
+class AP_InertialSensor_UserInteract_MAVLink : public AP_InertialSensor_UserInteract {
+public:
+    AP_InertialSensor_UserInteract_MAVLink(mavlink_channel_t chan) :
+        _chan(chan) {}
+
+    uint8_t blocking_read();
+    void _printf_P(const prog_char *, ...);
+private:
+    mavlink_channel_t _chan;
+};
+
+#endif // __AP_INERTIAL_SENSOR_USER_INTERACT_MAVLINK_H__
+

libraries/AP_InertialSensor/AP_InertialSensor_UserInteract_Stream.cpp

@@ -18,10 +18,6 @@ uint8_t AP_InertialSensor_UserInteractStream::blocking_read() {
     return ret;
 }
 
-void AP_InertialSensor_UserInteractStream::println_P(const prog_char_t* str) {
-    _s->println_P(str);
-}
-
 void AP_InertialSensor_UserInteractStream::_printf_P(
         const prog_char* fmt, ...) {
     va_list ap;

libraries/AP_InertialSensor/AP_InertialSensor_UserInteract_Stream.h

@@ -15,7 +15,6 @@ public:
         _s(s) {}
 
     uint8_t blocking_read();
-    void println_P(const prog_char_t *);
     void _printf_P(const prog_char *, ...);
 private:
     AP_HAL::BetterStream *_s;