mirror of https://github.com/ArduPilot/ardupilot
AP_HAL_AVR: implement betterstream functions in AP_HAL
this gives more consistancy between ports
This commit is contained in:
parent
d46f5f6943
commit
33fe27a104
|
@ -4,10 +4,8 @@
|
||||||
#if CONFIG_HAL_BOARD == HAL_BOARD_APM1 || CONFIG_HAL_BOARD == HAL_BOARD_APM2
|
#if CONFIG_HAL_BOARD == HAL_BOARD_APM1 || CONFIG_HAL_BOARD == HAL_BOARD_APM2
|
||||||
|
|
||||||
#include <limits.h>
|
#include <limits.h>
|
||||||
|
|
||||||
#include "utility/print_vprintf.h"
|
|
||||||
|
|
||||||
#include "Console.h"
|
#include "Console.h"
|
||||||
|
|
||||||
using namespace AP_HAL_AVR;
|
using namespace AP_HAL_AVR;
|
||||||
|
|
||||||
// ConsoleDriver method implementations ///////////////////////////////////////
|
// ConsoleDriver method implementations ///////////////////////////////////////
|
||||||
|
@ -30,39 +28,6 @@ size_t AVRConsoleDriver::backend_write(const uint8_t *data, size_t len) {
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
// AVRConsoleDriver private method implementations ////////////////////////////
|
|
||||||
|
|
||||||
// BetterStream method implementations /////////////////////////////////////////
|
|
||||||
void AVRConsoleDriver::print_P(const prog_char_t *s) {
|
|
||||||
_base_uart->print_P(s);
|
|
||||||
}
|
|
||||||
|
|
||||||
void AVRConsoleDriver::println_P(const prog_char_t *s) {
|
|
||||||
_base_uart->println_P(s);
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
void AVRConsoleDriver::printf(const char *fmt, ...) {
|
|
||||||
va_list ap;
|
|
||||||
va_start(ap, fmt);
|
|
||||||
vprintf(fmt, ap);
|
|
||||||
va_end(ap);
|
|
||||||
}
|
|
||||||
|
|
||||||
void AVRConsoleDriver::_printf_P(const prog_char *fmt, ...) {
|
|
||||||
va_list ap;
|
|
||||||
va_start(ap, fmt);
|
|
||||||
vprintf_P(fmt, ap);
|
|
||||||
va_end(ap);
|
|
||||||
}
|
|
||||||
|
|
||||||
void AVRConsoleDriver::vprintf(const char *fmt, va_list ap){
|
|
||||||
print_vprintf((AP_HAL::Print*)this, 0, fmt, ap);
|
|
||||||
}
|
|
||||||
|
|
||||||
void AVRConsoleDriver::vprintf_P(const prog_char *fmt, va_list ap){
|
|
||||||
print_vprintf((AP_HAL::Print*)this, 1, fmt, ap);
|
|
||||||
}
|
|
||||||
|
|
||||||
// Stream method implementations /////////////////////////////////////////
|
// Stream method implementations /////////////////////////////////////////
|
||||||
int16_t AVRConsoleDriver::available(void) {
|
int16_t AVRConsoleDriver::available(void) {
|
||||||
|
|
|
@ -15,17 +15,6 @@ public:
|
||||||
size_t backend_read(uint8_t *data, size_t len);
|
size_t backend_read(uint8_t *data, size_t len);
|
||||||
size_t backend_write(const 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 */
|
/* Implementations of Stream virtual methods */
|
||||||
int16_t available();
|
int16_t available();
|
||||||
int16_t txspace();
|
int16_t txspace();
|
||||||
|
|
|
@ -28,9 +28,8 @@
|
||||||
|
|
||||||
#include <AP_HAL.h>
|
#include <AP_HAL.h>
|
||||||
#include <AP_Math.h>
|
#include <AP_Math.h>
|
||||||
|
|
||||||
#include "utility/print_vprintf.h"
|
|
||||||
#include "UARTDriver.h"
|
#include "UARTDriver.h"
|
||||||
|
|
||||||
using namespace AP_HAL_AVR;
|
using namespace AP_HAL_AVR;
|
||||||
|
|
||||||
#define FS_MAX_PORTS 4
|
#define FS_MAX_PORTS 4
|
||||||
|
@ -256,38 +255,4 @@ void AVRUARTDriver::_freeBuffer(Buffer *buffer)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// BetterStream method implementations /////////////////////////////////////////
|
|
||||||
void AVRUARTDriver::print_P(const prog_char_t *s) {
|
|
||||||
char c;
|
|
||||||
while ('\0' != (c = pgm_read_byte((const prog_char *)s++)))
|
|
||||||
write(c);
|
|
||||||
}
|
|
||||||
|
|
||||||
void AVRUARTDriver::println_P(const prog_char_t *s) {
|
|
||||||
print_P(s);
|
|
||||||
println();
|
|
||||||
}
|
|
||||||
|
|
||||||
void AVRUARTDriver::printf(const char *fmt, ...) {
|
|
||||||
va_list ap;
|
|
||||||
va_start(ap, fmt);
|
|
||||||
vprintf(fmt, ap);
|
|
||||||
va_end(ap);
|
|
||||||
}
|
|
||||||
|
|
||||||
void AVRUARTDriver::vprintf(const char *fmt, va_list ap) {
|
|
||||||
print_vprintf((AP_HAL::Print*)this, 0, fmt, ap);
|
|
||||||
}
|
|
||||||
|
|
||||||
void AVRUARTDriver::_printf_P(const prog_char *fmt, ...) {
|
|
||||||
va_list ap;
|
|
||||||
va_start(ap, fmt);
|
|
||||||
vprintf_P(fmt, ap);
|
|
||||||
va_end(ap);
|
|
||||||
}
|
|
||||||
|
|
||||||
void AVRUARTDriver::vprintf_P(const prog_char *fmt, va_list ap) {
|
|
||||||
print_vprintf((AP_HAL::Print*)this, 1, fmt, ap);
|
|
||||||
}
|
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
|
@ -40,17 +40,6 @@ public:
|
||||||
return (_txBuffer->head != _txBuffer->tail);
|
return (_txBuffer->head != _txBuffer->tail);
|
||||||
}
|
}
|
||||||
|
|
||||||
/* 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 */
|
/* Implementations of Stream virtual methods */
|
||||||
int16_t available();
|
int16_t available();
|
||||||
int16_t txspace();
|
int16_t txspace();
|
||||||
|
|
Loading…
Reference in New Issue