5 changed files with 0 additions and 141 deletions
@ -1,10 +0,0 @@
@@ -1,10 +0,0 @@
|
||||
#pragma once |
||||
|
||||
#include <AP_Common/AP_Common.h> |
||||
|
||||
/* Pure virtual interface class */ |
||||
class AP_InertialSensor_UserInteract { |
||||
public: |
||||
virtual bool blocking_read() = 0; |
||||
virtual void printf(const char *, ...) FMT_PRINTF(2, 3) = 0; |
||||
}; |
@ -1,60 +0,0 @@
@@ -1,60 +0,0 @@
|
||||
#include <stdarg.h> |
||||
#include <AP_HAL/AP_HAL.h> |
||||
#include <GCS_MAVLink/GCS_MAVLink.h> |
||||
#include "AP_InertialSensor_UserInteract_MAVLink.h" |
||||
#include <GCS_MAVLink/GCS.h> |
||||
|
||||
extern const AP_HAL::HAL& hal; |
||||
|
||||
// set by _snoop on COMMAND_ACK
|
||||
static bool _got_ack; |
||||
|
||||
/*
|
||||
watch for COMMAND_ACK messages |
||||
*/ |
||||
static void _snoop(const mavlink_message_t* msg) |
||||
{ |
||||
if (msg->msgid == MAVLINK_MSG_ID_COMMAND_ACK) { |
||||
_got_ack = true; |
||||
} |
||||
} |
||||
|
||||
bool AP_InertialSensor_UserInteract_MAVLink::blocking_read(void)
|
||||
{ |
||||
uint32_t start_ms = AP_HAL::millis(); |
||||
// setup snooping of packets so we can see the COMMAND_ACK
|
||||
_gcs->set_snoop(_snoop); |
||||
_got_ack = false; |
||||
while (AP_HAL::millis() - start_ms < 30000U) { |
||||
hal.scheduler->delay(10); |
||||
if (_got_ack) { |
||||
_gcs->set_snoop(nullptr); |
||||
return true;
|
||||
} |
||||
} |
||||
hal.console->printf("Timed out waiting for user response\n"); |
||||
_gcs->set_snoop(nullptr); |
||||
return false; |
||||
} |
||||
|
||||
void AP_InertialSensor_UserInteract_MAVLink::printf(const char* fmt, ...) |
||||
{ |
||||
char msg[50]; |
||||
va_list ap; |
||||
va_start(ap, fmt); |
||||
hal.util->vsnprintf(msg, sizeof(msg), fmt, ap); |
||||
va_end(ap); |
||||
if (msg[strlen(msg)-1] == '\n') { |
||||
// STATUSTEXT messages should not add linefeed
|
||||
msg[strlen(msg)-1] = 0; |
||||
} |
||||
AP_HAL::UARTDriver *uart = _gcs->get_uart(); |
||||
/*
|
||||
to ensure these messages get to the user we need to wait for the |
||||
port send buffer to have enough room |
||||
*/ |
||||
while (uart->txspace() < MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_MSG_ID_STATUSTEXT_LEN) { |
||||
hal.scheduler->delay(1); |
||||
} |
||||
_gcs->send_text(MAV_SEVERITY_CRITICAL, msg); |
||||
} |
@ -1,24 +0,0 @@
@@ -1,24 +0,0 @@
|
||||
#pragma once |
||||
|
||||
#include <AP_Common/AP_Common.h> |
||||
#include <AP_HAL/AP_HAL.h> |
||||
#include <GCS_MAVLink/GCS_MAVLink.h> |
||||
|
||||
|
||||
#include "AP_InertialSensor_UserInteract.h" |
||||
|
||||
class GCS_MAVLINK; |
||||
|
||||
/**
|
||||
* AP_InertialSensor_UserInteract, implemented in terms of a MAVLink connection |
||||
*/ |
||||
class AP_InertialSensor_UserInteract_MAVLink : public AP_InertialSensor_UserInteract { |
||||
public: |
||||
AP_InertialSensor_UserInteract_MAVLink(GCS_MAVLINK *gcs) : |
||||
_gcs(gcs) {} |
||||
|
||||
bool blocking_read(); |
||||
void printf(const char *, ...) FMT_PRINTF(2, 3); |
||||
private: |
||||
GCS_MAVLINK *_gcs; |
||||
}; |
@ -1,27 +0,0 @@
@@ -1,27 +0,0 @@
|
||||
#include <stdarg.h> |
||||
#include <AP_HAL/AP_HAL.h> |
||||
#include "AP_InertialSensor_UserInteract_Stream.h" |
||||
|
||||
extern const AP_HAL::HAL& hal; |
||||
|
||||
bool AP_InertialSensor_UserInteractStream::blocking_read()
|
||||
{ |
||||
/* Wait for input to be available */ |
||||
while(!_s->available()) { |
||||
hal.scheduler->delay(20); |
||||
} |
||||
/* Clear all available input */ |
||||
while (_s->available()) { |
||||
_s->read(); |
||||
} |
||||
return true; |
||||
} |
||||
|
||||
void AP_InertialSensor_UserInteractStream::printf( |
||||
const char* fmt, ...) { |
||||
va_list ap; |
||||
va_start(ap, fmt); |
||||
_s->vprintf(fmt, ap); |
||||
va_end(ap); |
||||
} |
||||
|
@ -1,20 +0,0 @@
@@ -1,20 +0,0 @@
|
||||
#pragma once |
||||
|
||||
#include <AP_Common/AP_Common.h> |
||||
#include <AP_HAL/AP_HAL.h> |
||||
|
||||
#include "AP_InertialSensor_UserInteract.h" |
||||
|
||||
/**
|
||||
* AP_InertialSensor_UserInteract, implemented in terms of a BetterStream. |
||||
*/ |
||||
class AP_InertialSensor_UserInteractStream : public AP_InertialSensor_UserInteract { |
||||
public: |
||||
AP_InertialSensor_UserInteractStream(AP_HAL::BetterStream *s) : |
||||
_s(s) {} |
||||
|
||||
bool blocking_read(); |
||||
void printf(const char*, ...) FMT_PRINTF(2, 3); |
||||
private: |
||||
AP_HAL::BetterStream *_s; |
||||
}; |
Loading…
Reference in new issue