Browse Source

AP_HAL_AVR_SITL: implement betterstream functions in AP_HAL

this gives more consistancy between ports
mission-4.1.18
Andrew Tridgell 12 years ago
parent
commit
dbb70fc57c
  1. 43
      libraries/AP_HAL_AVR_SITL/Console.cpp
  2. 11
      libraries/AP_HAL_AVR_SITL/Console.h
  3. 38
      libraries/AP_HAL_AVR_SITL/UARTDriver.cpp
  4. 10
      libraries/AP_HAL_AVR_SITL/UARTDriver.h

43
libraries/AP_HAL_AVR_SITL/Console.cpp

@ -5,7 +5,6 @@ @@ -5,7 +5,6 @@
#include <limits.h>
#include <stdarg.h>
#include "utility/print_vprintf.h"
#include "Console.h"
using namespace AVR_SITL;
@ -37,48 +36,6 @@ size_t SITLConsoleDriver::backend_write(const uint8_t *data, size_t len) @@ -37,48 +36,6 @@ size_t SITLConsoleDriver::backend_write(const uint8_t *data, size_t len)
return 0;
}
// SITLConsoleDriver private method implementations ////////////////////////////
// BetterStream method implementations /////////////////////////////////////////
void SITLConsoleDriver::print_P(const prog_char_t *s)
{
char c;
while ('\0' != (c = pgm_read_byte((const prog_char *)s++)))
write(c);
}
void SITLConsoleDriver::println_P(const prog_char_t *s)
{
print_P(s);
println();
}
void SITLConsoleDriver::printf(const char *fmt, ...)
{
va_list ap;
va_start(ap, fmt);
vprintf(fmt, ap);
va_end(ap);
}
void SITLConsoleDriver::vprintf(const char *fmt, va_list ap)
{
print_vprintf((AP_HAL::Print*)this, 0, fmt, ap);
}
void SITLConsoleDriver::_printf_P(const prog_char *fmt, ...)
{
va_list ap;
va_start(ap, fmt);
vprintf_P(fmt, ap);
va_end(ap);
}
void SITLConsoleDriver::vprintf_P(const prog_char *fmt, va_list ap)
{
print_vprintf((AP_HAL::Print*)this, 1, fmt, ap);
}
// Stream method implementations /////////////////////////////////////////
int16_t SITLConsoleDriver::available(void)
{

11
libraries/AP_HAL_AVR_SITL/Console.h

@ -17,17 +17,6 @@ public: @@ -17,17 +17,6 @@ public:
size_t backend_read(uint8_t *data, size_t len);
size_t backend_write(const uint8_t *data, size_t len);
/* Implementations of BetterStream virtual methods */
void print_P(const prog_char_t *s);
void println_P(const prog_char_t *s);
void printf(const char *s, ...)
__attribute__ ((format(__printf__, 2, 3)));
void _printf_P(const prog_char *s, ...)
__attribute__ ((format(__printf__, 2, 3)));
void vprintf(const char *s, va_list ap);
void vprintf_P(const prog_char *s, va_list ap);
/* Implementations of Stream virtual methods */
int16_t available();
int16_t txspace();

38
libraries/AP_HAL_AVR_SITL/UARTDriver.cpp

@ -35,7 +35,6 @@ @@ -35,7 +35,6 @@
#include <netinet/in.h>
#include <netinet/tcp.h>
#include "utility/print_vprintf.h"
#include "UARTDriver.h"
#include "SITL_State.h"
@ -175,43 +174,6 @@ size_t SITLUARTDriver::write(uint8_t c) @@ -175,43 +174,6 @@ size_t SITLUARTDriver::write(uint8_t c)
return send(_fd, &c, 1, flags);
}
// BetterStream method implementations /////////////////////////////////////////
void SITLUARTDriver::print_P(const prog_char_t *s)
{
char c;
while ('\0' != (c = pgm_read_byte((const prog_char *)s++)))
write(c);
}
void SITLUARTDriver::println_P(const prog_char_t *s)
{
print_P(s);
println();
}
void SITLUARTDriver::printf(const char *fmt, ...)
{
va_list ap;
va_start(ap, fmt);
vprintf(fmt, ap);
va_end(ap);
}
void SITLUARTDriver::vprintf(const char *fmt, va_list ap) {
print_vprintf((AP_HAL::Print*)this, 0, fmt, ap);
}
void SITLUARTDriver::_printf_P(const prog_char *fmt, ...)
{
va_list ap;
va_start(ap, fmt);
vprintf_P(fmt, ap);
va_end(ap);
}
void SITLUARTDriver::vprintf_P(const prog_char *fmt, va_list ap) {
print_vprintf((AP_HAL::Print*)this, 1, fmt, ap);
}
/*
start a TCP connection for the serial port. If wait_for_connection
is true then block until a client connects

10
libraries/AP_HAL_AVR_SITL/UARTDriver.h

@ -39,16 +39,6 @@ public: @@ -39,16 +39,6 @@ public:
return false;
}
/* Implementations of BetterStream virtual methods */
void print_P(const prog_char_t *s);
void println_P(const prog_char_t *s);
void printf(const char *s, ...)
__attribute__ ((format(__printf__, 2, 3)));
void _printf_P(const prog_char *s, ...)
__attribute__ ((format(__printf__, 2, 3)));
void vprintf(const char *s, va_list ap);
void vprintf_P(const prog_char *s, va_list ap);
/* Implementations of Stream virtual methods */
int16_t available();
int16_t txspace();

Loading…
Cancel
Save