diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index 9c5aa5b9bb..967320fa38 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -11,7 +11,7 @@ #include #include #include - +#include "AP_InertialSensor_UserInteract.h" /* AP_InertialSensor is an abstraction for gyro and accel measurements * which are correctly aligned to the body axes and scaled to SI units. * diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_UserInteract.h b/libraries/AP_InertialSensor/AP_InertialSensor_UserInteract.h new file mode 100644 index 0000000000..282d74a5f3 --- /dev/null +++ b/libraries/AP_InertialSensor/AP_InertialSensor_UserInteract.h @@ -0,0 +1,16 @@ + +#ifndef __AP_INERTIAL_SENSOR_USER_INTERACT_H__ +#define __AP_INERTIAL_SENSOR_USER_INTERACT_H__ + +#include + +/* Pure virtual interface class */ +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; +}; + +#endif // __AP_INERTIAL_SENSOR_USER_INTERACT_H__ + diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_UserInteract_Stream.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_UserInteract_Stream.cpp new file mode 100644 index 0000000000..1277631f19 --- /dev/null +++ b/libraries/AP_InertialSensor/AP_InertialSensor_UserInteract_Stream.cpp @@ -0,0 +1,32 @@ +#include +#include +#include "AP_InertialSensor_UserInteract_Stream.h" + +extern const AP_HAL::HAL& hal; + +uint8_t AP_InertialSensor_UserInteractStream::blocking_read() { + /* Wait for input to be available */ + while(!_s->available()) { + hal.scheduler->delay(20); + } + /* Grab first character */ + uint8_t ret = (uint8_t) _s->read(); + /* Clear all available input */ + while (_s->available()) { + _s->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; + va_start(ap, fmt); + _s->vprintf_P(fmt, ap); + va_end(ap); +} + diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_UserInteract_Stream.h b/libraries/AP_InertialSensor/AP_InertialSensor_UserInteract_Stream.h new file mode 100644 index 0000000000..2f42d82aea --- /dev/null +++ b/libraries/AP_InertialSensor/AP_InertialSensor_UserInteract_Stream.h @@ -0,0 +1,25 @@ + +#ifndef __AP_INERTIAL_SENSOR_USER_INTERACT_STREAM_H__ +#define __AP_INERTIAL_SENSOR_USER_INTERACT_STREAM_H__ + +#include +#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) {} + + uint8_t blocking_read(); + void println_P(const prog_char_t *); + void _printf_P(const prog_char *, ...); +private: + AP_HAL::BetterStream *_s; +}; + +#endif // __AP_INERTIAL_SENSOR_USER_INTERACT_STREAM_H__ +