From f3f4b7205c6162007cec946f0b4aa914273fb802 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 8 May 2013 16:18:40 +1000 Subject: [PATCH] AP_InsertialSensor: added support for MAVLink user interaction allows APM to ask user to print enter to continue via MAVLink messages during accel calibration --- .../AP_InertialSensor/AP_InertialSensor.h | 1 + .../AP_InertialSensor_UserInteract.h | 1 - ...AP_InertialSensor_UserInteract_MAVLink.cpp | 47 +++++++++++++++++++ .../AP_InertialSensor_UserInteract_MAVLink.h | 24 ++++++++++ .../AP_InertialSensor_UserInteract_Stream.cpp | 4 -- .../AP_InertialSensor_UserInteract_Stream.h | 1 - 6 files changed, 72 insertions(+), 6 deletions(-) create mode 100644 libraries/AP_InertialSensor/AP_InertialSensor_UserInteract_MAVLink.cpp create mode 100644 libraries/AP_InertialSensor/AP_InertialSensor_UserInteract_MAVLink.h diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index b0291b3375..fb87056b12 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/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__ diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_UserInteract.h b/libraries/AP_InertialSensor/AP_InertialSensor_UserInteract.h index 282d74a5f3..f72eeffb4b 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_UserInteract.h +++ b/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; }; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_UserInteract_MAVLink.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_UserInteract_MAVLink.cpp new file mode 100644 index 0000000000..28dba3d696 --- /dev/null +++ b/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 +#include +#include +#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); +} + diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_UserInteract_MAVLink.h b/libraries/AP_InertialSensor/AP_InertialSensor_UserInteract_MAVLink.h new file mode 100644 index 0000000000..89fb07ea52 --- /dev/null +++ b/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 +#include +#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__ + diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_UserInteract_Stream.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_UserInteract_Stream.cpp index 1277631f19..35e5a245cf 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_UserInteract_Stream.cpp +++ b/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; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_UserInteract_Stream.h b/libraries/AP_InertialSensor/AP_InertialSensor_UserInteract_Stream.h index 2f42d82aea..c645df2e11 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_UserInteract_Stream.h +++ b/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;