From 8eb9b39a44cdbd7d605a7c9fda5d3677d7bc5d6f Mon Sep 17 00:00:00 2001 From: Pat Hickey Date: Tue, 21 Aug 2012 14:11:24 -0700 Subject: [PATCH] AP_HAL_AVR move library classes to their own AP_HAL_AVR namespace --- libraries/AP_HAL/AP_HAL_Namespace.h | 16 +- libraries/AP_HAL/UARTDriver.h | 20 +- libraries/AP_HAL/utility/BetterStream.h | 28 +- libraries/AP_HAL_AVR/AP_HAL_AVR.cpp | 30 +- libraries/AP_HAL_AVR/AP_HAL_AVR.h | 6 + libraries/AP_HAL_AVR/AP_HAL_AVR_Namespace.h | 22 + libraries/AP_HAL_AVR/AnalogIn.h | 3 +- libraries/AP_HAL_AVR/Console.h | 3 +- libraries/AP_HAL_AVR/GPIO.h | 3 +- libraries/AP_HAL_AVR/I2CDriver.h | 3 +- libraries/AP_HAL_AVR/Log.h | 5 +- libraries/AP_HAL_AVR/PPMInput.h | 5 +- libraries/AP_HAL_AVR/PWMOutput.h | 4 +- libraries/AP_HAL_AVR/SPIDriver.h | 2 +- libraries/AP_HAL_AVR/Storage.h | 2 +- libraries/AP_HAL_AVR/UARTDriver.cpp | 78 +++ libraries/AP_HAL_AVR/UARTDriver.h | 219 +++++++- libraries/AP_HAL_AVR/examples/APM1/APM1.pde | 10 +- libraries/AP_HAL_AVR/utility/ftoa_engine.S | 532 ++++++++++++++++++++ libraries/AP_HAL_AVR/utility/ftoa_engine.h | 48 ++ libraries/AP_HAL_AVR/utility/macros.inc | 365 ++++++++++++++ libraries/AP_HAL_AVR/utility/ntz.h | 54 ++ libraries/AP_HAL_AVR/utility/ultoa_invert.S | 216 ++++++++ libraries/AP_HAL_AVR/utility/vprintf.cpp | 518 +++++++++++++++++++ libraries/AP_HAL_AVR/utility/xtoa_fast.h | 48 ++ 25 files changed, 2171 insertions(+), 69 deletions(-) create mode 100644 libraries/AP_HAL_AVR/AP_HAL_AVR_Namespace.h create mode 100644 libraries/AP_HAL_AVR/UARTDriver.cpp create mode 100644 libraries/AP_HAL_AVR/utility/ftoa_engine.S create mode 100644 libraries/AP_HAL_AVR/utility/ftoa_engine.h create mode 100644 libraries/AP_HAL_AVR/utility/macros.inc create mode 100644 libraries/AP_HAL_AVR/utility/ntz.h create mode 100644 libraries/AP_HAL_AVR/utility/ultoa_invert.S create mode 100644 libraries/AP_HAL_AVR/utility/vprintf.cpp create mode 100644 libraries/AP_HAL_AVR/utility/xtoa_fast.h diff --git a/libraries/AP_HAL/AP_HAL_Namespace.h b/libraries/AP_HAL/AP_HAL_Namespace.h index 8e50df992a..d143c6021d 100644 --- a/libraries/AP_HAL/AP_HAL_Namespace.h +++ b/libraries/AP_HAL/AP_HAL_Namespace.h @@ -19,21 +19,7 @@ namespace AP_HAL { class PPMInput; class PWMOutput; - /* AVR / Arduino Implementations for APM1 and APM2 */ - - class AVRUARTDriver; - class AVRI2CDriver; - class ArduinoSPIDriver; - class ArduinoAnalogIn; - class AVREEPROMStorage; - class DataFlashAPM1Log; - class DataFlashAPM2Log; - class AVRUARTConsole; - class ArduinoGPIO; - class APM1PPMInput; - class APM2PPMInput; - class APM1PWMOutput; - class APM2PWMOutput; + class EmptyUARTDriver; /* Utility Classes */ class Print; diff --git a/libraries/AP_HAL/UARTDriver.h b/libraries/AP_HAL/UARTDriver.h index b365af3160..85a12e6582 100644 --- a/libraries/AP_HAL/UARTDriver.h +++ b/libraries/AP_HAL/UARTDriver.h @@ -11,12 +11,15 @@ class AP_HAL::UARTDriver : public AP_HAL::BetterStream { public: UARTDriver() {} - virtual void begin(long baud) = 0; - virtual void begin(long baud, - unsigned int rxSpace, - unsigned int txSpace) = 0; - virtual void end() = 0; - virtual void flush() = 0; + virtual void begin(long baud) = 0; + virtual void begin(long baud, + unsigned int rxSpace, + unsigned int txSpace) = 0; + virtual void end() = 0; + virtual void flush() = 0; + virtual bool is_initialized() = 0; + virtual void set_blocking_writes(bool blocking) = 0; + virtual bool tx_pending() = 0; }; /* Concrete EmptyUARTDriver class provided for convenience */ @@ -28,12 +31,15 @@ public: void begin(long b, unsigned int rxS, unsigned int txS) {} void end() {} void flush() {} + bool is_initialized() { return false; } + void set_blocking_writes(bool blocking) {} + bool tx_pending() { return false; } /* Empty implementations of BetterStream virtual methods */ void print_P(const prog_char_t *pstr) {} void println_P(const prog_char_t *pstr) {} void printf(const char *pstr, ...) {} - void _printf_P(const prog_char_t *pstr, ...) {} + void _printf_P(const prog_char *pstr, ...) {} int txspace() { return 1; } /* Empty implementations of Stream virtual methods */ diff --git a/libraries/AP_HAL/utility/BetterStream.h b/libraries/AP_HAL/utility/BetterStream.h index 402e49051e..d7160b0c75 100644 --- a/libraries/AP_HAL/utility/BetterStream.h +++ b/libraries/AP_HAL/utility/BetterStream.h @@ -17,9 +17,12 @@ #include #include -/* AP_HAL::BetterStream is derived from Michael Smith's FastSerial library for - * Arduino. Mike's library has an implementation of _vprintf() for AVR. - * Please provide your own platform-specic implementation for this library. +/* AP_HAL::BetterStream is a pure virtual interface. It resembles + * Michael Smith's BetterStream library for Arduino. + * The Michael Smith BetterStream provided some implementations for AVR based + * on _vprintf(). + * Please provide your own platform-specic implementation of vprintf, sprintf, + * etc. to implement the printf functions. * * TODO: Segregate prog_char_t dependent functions to be available on AVR * platform only, with default implementations elsewhere. @@ -30,22 +33,17 @@ public: BetterStream(void) {} // Stream extensions - virtual void print_P(const prog_char_t *); - virtual void println_P(const prog_char_t *); - virtual void printf(const char *, ...) - __attribute__ ((format(__printf__, 2, 3))); - virtual void _printf_P(const prog_char *, ...) - __attribute__ ((format(__printf__, 2, 3))); + virtual int txspace(void) = 0; - virtual int txspace(void); + virtual void print_P(const prog_char_t *) = 0; + virtual void println_P(const prog_char_t *) = 0; + virtual void printf(const char *, ...) + __attribute__ ((format(__printf__, 2, 3))) = 0; + virtual void _printf_P(const prog_char *, ...) + __attribute__ ((format(__printf__, 2, 3))) = 0; #define printf_P(fmt, ...) _printf_P((const prog_char *)fmt, ## __VA_ARGS__) -private: - virtual void _vprintf(unsigned char, const char *, va_list) - __attribute__ ((format(__printf__, 3, 0))); - - }; #endif // __AP_HAL_UTILITY_BETTERSTREAM_H__ diff --git a/libraries/AP_HAL_AVR/AP_HAL_AVR.cpp b/libraries/AP_HAL_AVR/AP_HAL_AVR.cpp index 68c036120a..623bd6a204 100644 --- a/libraries/AP_HAL_AVR/AP_HAL_AVR.cpp +++ b/libraries/AP_HAL_AVR/AP_HAL_AVR.cpp @@ -15,11 +15,17 @@ #include "PWMOutput.h" using namespace AP_HAL; +using namespace AP_HAL_AVR; + +AVRUARTDriverISRs(0); +AVRUARTDriverISRs(1); +AVRUARTDriverISRs(3); + +static AVRUARTDriverInstance(avrUart0Driver, 0); +static AVRUARTDriverInstance(avrUart1Driver, 1); +static EmptyUARTDriver avrUart2Driver; +static AVRUARTDriverInstance(avrUart3Driver, 3); -static AVRUARTDriver avrUart0Driver( 0 ); -static AVRUARTDriver avrUart1Driver( 1 ); -static AVRUARTDriver avrUart2Driver( 2 ); -static AVRUARTDriver avrUart3Driver( 3 ); static AVRI2CDriver avrI2CDriver; static ArduinoSPIDriver arduinoSPIDriver; static ArduinoAnalogIn arduinoAnalogIn; @@ -34,10 +40,10 @@ static APM1PWMOutput apm1PWMOutput; static APM2PWMOutput apm2PWMOutput; const HAL AP_HAL_AVR_APM1( - &avrUart0Driver, - &avrUart1Driver, - &avrUart2Driver, - &avrUart3Driver, + (UARTDriver*) &avrUart0Driver, + (UARTDriver*) &avrUart1Driver, + (UARTDriver*) &avrUart2Driver, + (UARTDriver*) &avrUart3Driver, &avrI2CDriver, &arduinoSPIDriver, &arduinoAnalogIn, @@ -49,10 +55,10 @@ const HAL AP_HAL_AVR_APM1( &apm1PWMOutput ); const HAL AP_HAL_AVR_APM2( - &avrUart0Driver, - &avrUart1Driver, - &avrUart2Driver, - &avrUart3Driver, + (UARTDriver*) &avrUart0Driver, + (UARTDriver*) &avrUart1Driver, + (UARTDriver*) &avrUart2Driver, + (UARTDriver*) &avrUart3Driver, &avrI2CDriver, &arduinoSPIDriver, &arduinoAnalogIn, diff --git a/libraries/AP_HAL_AVR/AP_HAL_AVR.h b/libraries/AP_HAL_AVR/AP_HAL_AVR.h index a9fe6e6e1a..661e92210e 100644 --- a/libraries/AP_HAL_AVR/AP_HAL_AVR.h +++ b/libraries/AP_HAL_AVR/AP_HAL_AVR.h @@ -4,6 +4,12 @@ #include +/** + * This module exports AP_HAL instances only. + * All internal drivers must conform to AP_HAL interfaces + * and not expose implementation details. + */ + extern const AP_HAL::HAL AP_HAL_AVR_APM1; extern const AP_HAL::HAL AP_HAL_AVR_APM2; diff --git a/libraries/AP_HAL_AVR/AP_HAL_AVR_Namespace.h b/libraries/AP_HAL_AVR/AP_HAL_AVR_Namespace.h new file mode 100644 index 0000000000..90e53275ec --- /dev/null +++ b/libraries/AP_HAL_AVR/AP_HAL_AVR_Namespace.h @@ -0,0 +1,22 @@ + +#ifndef __AP_HAL_AVR_NAMESPACE_H__ +#define __AP_HAL_AVR_NAMESPACE_H__ + +namespace AP_HAL_AVR { + class AVRUARTDriver; + class AVRI2CDriver; + class ArduinoSPIDriver; + class ArduinoAnalogIn; + class AVREEPROMStorage; + class DataFlashAPM1Log; + class DataFlashAPM2Log; + class AVRUARTConsole; + class ArduinoGPIO; + class APM1PPMInput; + class APM2PPMInput; + class APM1PWMOutput; + class APM2PWMOutput; +} + +#endif //__AP_HAL_AVR_NAMESPACE_H__ + diff --git a/libraries/AP_HAL_AVR/AnalogIn.h b/libraries/AP_HAL_AVR/AnalogIn.h index 83f51860de..f0cc757006 100644 --- a/libraries/AP_HAL_AVR/AnalogIn.h +++ b/libraries/AP_HAL_AVR/AnalogIn.h @@ -3,8 +3,9 @@ #define __AP_HAL_AVR_ANALOG_IN_H__ #include +#include "AP_HAL_AVR_Namespace.h" -class AP_HAL::ArduinoAnalogIn : public AP_HAL::AnalogIn { +class AP_HAL_AVR::ArduinoAnalogIn : public AP_HAL::AnalogIn { public: ArduinoAnalogIn() : _init(0) {} void init(int machtnicht) { _init = 1; } diff --git a/libraries/AP_HAL_AVR/Console.h b/libraries/AP_HAL_AVR/Console.h index 0f8bbe2dee..1c0b11b40c 100644 --- a/libraries/AP_HAL_AVR/Console.h +++ b/libraries/AP_HAL_AVR/Console.h @@ -3,9 +3,10 @@ #define __AP_HAL_AVR_CONSOLE_H__ #include +#include "AP_HAL_AVR_Namespace.h" #include "UARTDriver.h" -class AP_HAL::AVRUARTConsole : public AP_HAL::Console { +class AP_HAL_AVR::AVRUARTConsole : public AP_HAL::Console { public: AVRUARTConsole( AVRUARTDriver* driver ) : _driver(driver), _init(0) {} void init(int machtnicht) { _init = 1; } diff --git a/libraries/AP_HAL_AVR/GPIO.h b/libraries/AP_HAL_AVR/GPIO.h index 1364e922fd..3d9d86d355 100644 --- a/libraries/AP_HAL_AVR/GPIO.h +++ b/libraries/AP_HAL_AVR/GPIO.h @@ -3,8 +3,9 @@ #define __AP_HAL_AVR_GPIO_H__ #include +#include "AP_HAL_AVR_Namespace.h" -class AP_HAL::ArduinoGPIO : public AP_HAL::GPIO { +class AP_HAL_AVR::ArduinoGPIO : public AP_HAL::GPIO { public: ArduinoGPIO() : _init(0) {} void init(int machtnicht) { _init = machtnicht; } diff --git a/libraries/AP_HAL_AVR/I2CDriver.h b/libraries/AP_HAL_AVR/I2CDriver.h index 67728eb281..840a1947a7 100644 --- a/libraries/AP_HAL_AVR/I2CDriver.h +++ b/libraries/AP_HAL_AVR/I2CDriver.h @@ -3,8 +3,9 @@ #define __AP_HAL_AVR_I2C_DRIVER_H__ #include +#include "AP_HAL_AVR_Namespace.h" -class AP_HAL::AVRI2CDriver : public AP_HAL::I2CDriver { +class AP_HAL_AVR::AVRI2CDriver : public AP_HAL::I2CDriver { public: AVRI2CDriver(): _init(0) {} void init() { _init = 1; } diff --git a/libraries/AP_HAL_AVR/Log.h b/libraries/AP_HAL_AVR/Log.h index 90722772c1..336ca626e5 100644 --- a/libraries/AP_HAL_AVR/Log.h +++ b/libraries/AP_HAL_AVR/Log.h @@ -3,8 +3,9 @@ #define __AP_HAL_AVR_LOG_H__ #include +#include "AP_HAL_AVR_Namespace.h" -class AP_HAL::DataFlashAPM1Log : public AP_HAL::Log { +class AP_HAL_AVR::DataFlashAPM1Log : public AP_HAL::Log { public: DataFlashAPM1Log() : _init(0) {} void init(int machtnicht) { _init = 1; } @@ -12,7 +13,7 @@ private: int _init; }; -class AP_HAL::DataFlashAPM2Log : public AP_HAL::Log { +class AP_HAL_AVR::DataFlashAPM2Log : public AP_HAL::Log { public: DataFlashAPM2Log() : _init(0) {} void init(int machtnicht) { _init = 2; } diff --git a/libraries/AP_HAL_AVR/PPMInput.h b/libraries/AP_HAL_AVR/PPMInput.h index 05bf0099cd..7ac60c7a76 100644 --- a/libraries/AP_HAL_AVR/PPMInput.h +++ b/libraries/AP_HAL_AVR/PPMInput.h @@ -3,8 +3,9 @@ #define __AP_HAL_AVR_PPM_INPUT_H__ #include +#include "AP_HAL_AVR_Namespace.h" -class AP_HAL::APM1PPMInput : public AP_HAL::PPMInput { +class AP_HAL_AVR::APM1PPMInput : public AP_HAL::PPMInput { public: APM1PPMInput() : _init(0) {} void init(int machtnicht) { _init = 1; } @@ -12,7 +13,7 @@ private: int _init; }; -class AP_HAL::APM2PPMInput : public AP_HAL::PPMInput { +class AP_HAL_AVR::APM2PPMInput : public AP_HAL::PPMInput { public: APM2PPMInput() : _init(0) {} void init(int machtnicht) { _init = 2; } diff --git a/libraries/AP_HAL_AVR/PWMOutput.h b/libraries/AP_HAL_AVR/PWMOutput.h index b6d4a41836..cb1b9a7136 100644 --- a/libraries/AP_HAL_AVR/PWMOutput.h +++ b/libraries/AP_HAL_AVR/PWMOutput.h @@ -4,7 +4,7 @@ #include -class AP_HAL::APM1PWMOutput : public AP_HAL::PWMOutput { +class AP_HAL_AVR::APM1PWMOutput : public AP_HAL::PWMOutput { public: APM1PWMOutput() : _init(0) {} void init(int machtnicht) { _init = 1; } @@ -12,7 +12,7 @@ private: int _init; }; -class AP_HAL::APM2PWMOutput : public AP_HAL::PWMOutput { +class AP_HAL_AVR::APM2PWMOutput : public AP_HAL::PWMOutput { public: APM2PWMOutput() : _init(0) {} void init(int machtnicht) { _init = 2; } diff --git a/libraries/AP_HAL_AVR/SPIDriver.h b/libraries/AP_HAL_AVR/SPIDriver.h index 49e6a9aca1..0b0131a5da 100644 --- a/libraries/AP_HAL_AVR/SPIDriver.h +++ b/libraries/AP_HAL_AVR/SPIDriver.h @@ -4,7 +4,7 @@ #include -class AP_HAL::ArduinoSPIDriver : public AP_HAL::SPIDriver { +class AP_HAL_AVR::ArduinoSPIDriver : public AP_HAL::SPIDriver { public: ArduinoSPIDriver() : _init(0) {} void init() { _init = 1; } diff --git a/libraries/AP_HAL_AVR/Storage.h b/libraries/AP_HAL_AVR/Storage.h index 5a0dfc0af9..cbd959a08a 100644 --- a/libraries/AP_HAL_AVR/Storage.h +++ b/libraries/AP_HAL_AVR/Storage.h @@ -5,7 +5,7 @@ #include -class AP_HAL::AVREEPROMStorage : public AP_HAL::Storage { +class AP_HAL_AVR::AVREEPROMStorage : public AP_HAL::Storage { public: AVREEPROMStorage() : _init(0) {} void init(int machtnicht) { _init = 1; } diff --git a/libraries/AP_HAL_AVR/UARTDriver.cpp b/libraries/AP_HAL_AVR/UARTDriver.cpp new file mode 100644 index 0000000000..58a3dd7dbe --- /dev/null +++ b/libraries/AP_HAL_AVR/UARTDriver.cpp @@ -0,0 +1,78 @@ +// -*- Mode: C++; c-basic-offset: 8; indent-tabs-mode: nil -*- +// +// Copyright (c) 2010 Michael Smith. All rights reserved. +// +// This is free software; you can redistribute it and/or modify it under +// the terms of the GNU Lesser General Public License as published by the +// Free Software Foundation; either version 2.1 of the License, or (at +// your option) any later version. +// + +// +// Enhancements to the Arduino Stream class. +// + +#include +#include +#include +#include + +#include "UARTDriver.h" +using namespace AP_HAL_AVR; + +#define FS_MAX_PORTS 4 +AVRUARTDriver::Buffer __AVRUARTDriver__rxBuffer[FS_MAX_PORTS]; +AVRUARTDriver::Buffer __AVRUARTDriver__txBuffer[FS_MAX_PORTS]; + +AVRUARTDriver::AVRUARTDriver( + const uint8_t portNumber, volatile uint8_t *ubrrh, + volatile uint8_t *ubrrl, volatile uint8_t *ucsra, + volatile uint8_t *ucsrb, const uint8_t u2x, + const uint8_t portEnableBits, const uint8_t portTxBits) : + _ubrrh(ubrrh), + _ubrrl(ubrrl), + _ucsra(ucsra), + _ucsrb(ucsrb), + _u2x(u2x), + _portEnableBits(portEnableBits), + _portTxBits(portTxBits), + _rxBuffer(&__AVRUARTDriver__rxBuffer[portNumber]), + _txBuffer(&__AVRUARTDriver__txBuffer[portNumber]) +{ + _initialized = true; + begin(57600); +} +/* */ + +/* BetterStream 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(0, fmt, ap); + va_end(ap); +} + +void AVRUARTDriver::_printf_P(const prog_char *fmt, ...) +{ + va_list ap; + + va_start(ap, fmt); + _vprintf(1, fmt, ap); + va_end(ap); +} diff --git a/libraries/AP_HAL_AVR/UARTDriver.h b/libraries/AP_HAL_AVR/UARTDriver.h index 6886ba7572..2454aaffd7 100644 --- a/libraries/AP_HAL_AVR/UARTDriver.h +++ b/libraries/AP_HAL_AVR/UARTDriver.h @@ -4,23 +4,230 @@ #include +#include #include +#include "AP_HAL_AVR_Namespace.h" /** * AVRUARTDriver is an implementation of UARTDriver for the AVR. * It will be a thin wrapper on FastSerial. */ -class AP_HAL::AVRUARTDriver : public AP_HAL::UARTDriver { +class AP_HAL_AVR::AVRUARTDriver : public AP_HAL::UARTDriver { public: - AVRUARTDriver(int num) : _num(num) {} - void init(uint16_t baud) { _baud = baud; } - /* XXX stub implementation! */ + AVRUARTDriver( + const uint8_t portNumber, volatile uint8_t *ubrrh, + volatile uint8_t *ubrrl, volatile uint8_t *ucsra, + volatile uint8_t *ucsrb, const uint8_t u2x, + const uint8_t portEnableBits, const uint8_t portTxBits); + + /* Implementations of UARTDriver virtual methods */ + void begin(long b) {} + /// Extended port open method + /// + /// Allows for both opening with specified buffer sizes, and re-opening + /// to adjust a subset of the port's settings. + /// + /// @note Buffer sizes greater than ::_max_buffer_size will be rounded + /// down. + /// + /// @param baud Selects the speed that the port will be + /// configured to. If zero, the port speed is left + /// unchanged. + /// @param rxSpace Sets the receive buffer size for the port. If zero + /// then the buffer size is left unchanged if the port + /// is open, or set to ::_default_rx_buffer_size if it is + /// currently closed. + /// @param txSpace Sets the transmit buffer size for the port. If zero + /// then the buffer size is left unchanged if the port + /// is open, or set to ::_default_tx_buffer_size if it + /// is currently closed. + /// + void begin(long b, unsigned int rxS, unsigned int txS) {} + void end() {} + void flush() {} + bool is_initialized() { return _initialized; } + void set_blocking_writes(bool blocking) {} + bool tx_pending() { return false; } + + /* Implementations of BetterStream virtual methods */ + int txspace() { return 10; } + 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))); + + /* Implementations of Stream virtual methods */ + int available() { return 0; } + int read() { return -1; } + int peek() { return -1; } + + /* Implementations of Print virtual methods */ + size_t write(uint8_t c) { return 0; } + + /// Transmit/receive buffer descriptor. + /// + /// Public so the interrupt handlers can see it + struct Buffer { + volatile uint16_t head, tail; ///< head and tail pointers + uint16_t mask; ///< buffer size mask for pointer wrap + uint8_t *bytes; ///< pointer to allocated buffer + }; private: - int _num; - uint16_t _baud; + /* Helper function _vprintf for implementing BetterStream's printfs */ + void _vprintf(unsigned char, const char *, va_list) + __attribute__((format(__printf__,3,0))); + + /* Instance Variables */ + bool _initialized; + + // register accessors + volatile uint8_t * const _ubrrh; + volatile uint8_t * const _ubrrl; + volatile uint8_t * const _ucsra; + volatile uint8_t * const _ucsrb; + + // register magic numbers + const uint8_t _u2x; + const uint8_t _portEnableBits; ///< rx, tx and rx interrupt enables + const uint8_t _portTxBits; ///< tx data and completion interrupt enables + + // ring buffers + Buffer * const _rxBuffer; + Buffer * const _txBuffer; + bool _open; + + // whether writes to the port should block waiting + // for enough space to appear + bool _nonblocking_writes; + + /* Class Variables */ + + /// Allocates a buffer of the given size + /// + /// @param buffer The buffer descriptor for which the buffer will + /// will be allocated. + /// @param size The desired buffer size. + /// @returns True if the buffer was allocated successfully. + /// + static bool _allocBuffer(Buffer *buffer, unsigned int size); + + /// Frees the allocated buffer in a descriptor + /// + /// @param buffer The descriptor whose buffer should be freed. + /// + static void _freeBuffer(Buffer *buffer); + + /// default receive buffer size + static const unsigned int _default_rx_buffer_size = 128; + + /// default transmit buffer size + static const unsigned int _default_tx_buffer_size = 16; + + /// maxium tx/rx buffer size + /// @note if we could bring the max size down to 256, the mask and head/tail + /// pointers in the buffer could become uint8_t. + /// + static const unsigned int _max_buffer_size = 512; }; +extern AP_HAL_AVR::AVRUARTDriver::Buffer __AVRUARTDriver__rxBuffer[]; +extern AP_HAL_AVR::AVRUARTDriver::Buffer __AVRUARTDriver__txBuffer[]; + +/// Generic Rx/Tx vectors for a serial port - needs to know magic numbers +/// +#define AVRUARTDriverHandler(_PORT, _RXVECTOR, _TXVECTOR, _UDR, _UCSRB, _TXBITS) \ +ISR(_RXVECTOR, ISR_BLOCK) \ +{ \ + uint8_t c; \ + uint16_t i; \ + \ + /* read the byte as quickly as possible */ \ + c = _UDR; \ + /* work out where the head will go next */ \ + i = (__AVRUARTDriver__rxBuffer[_PORT].head + 1) & __AVRUARTDriver__rxBuffer[_PORT].mask; \ + /* decide whether we have space for another byte */ \ + if (i != __AVRUARTDriver__rxBuffer[_PORT].tail) { \ + /* we do, move the head */ \ + __AVRUARTDriver__rxBuffer[_PORT].bytes[__AVRUARTDriver__rxBuffer[_PORT].head] = c; \ + __AVRUARTDriver__rxBuffer[_PORT].head = i; \ + } \ +} \ +ISR(_TXVECTOR, ISR_BLOCK) \ +{ \ + /* if there is another character to send */ \ + if (__AVRUARTDriver__txBuffer[_PORT].tail != __AVRUARTDriver__txBuffer[_PORT].head) { \ + _UDR = __AVRUARTDriver__txBuffer[_PORT].bytes[__AVRUARTDriver__txBuffer[_PORT].tail]; \ + /* increment the tail */ \ + __AVRUARTDriver__txBuffer[_PORT].tail = \ + (__AVRUARTDriver__txBuffer[_PORT].tail + 1) & __AVRUARTDriver__txBuffer[_PORT].mask; \ + } else { \ + /* there are no more bytes to send, disable the interrupt */ \ + if (__AVRUARTDriver__txBuffer[_PORT].head == __AVRUARTDriver__txBuffer[_PORT].tail) \ + _UCSRB &= ~_TXBITS; \ + } \ +} \ +struct hack + +// +// Portability; convert various older sets of defines for U(S)ART0 up +// to match the definitions for the 1280 and later devices. +// +#if !defined(USART0_RX_vect) +# if defined(USART_RX_vect) +# define USART0_RX_vect USART_RX_vect +# define USART0_UDRE_vect USART_UDRE_vect +# elif defined(UART0_RX_vect) +# define USART0_RX_vect UART0_RX_vect +# define USART0_UDRE_vect UART0_UDRE_vect +# endif +#endif + +#if !defined(USART1_RX_vect) +# if defined(UART1_RX_vect) +# define USART1_RX_vect UART1_RX_vect +# define USART1_UDRE_vect UART1_UDRE_vect +# endif +#endif + +#if !defined(UDR0) +# if defined(UDR) +# define UDR0 UDR +# define UBRR0H UBRRH +# define UBRR0L UBRRL +# define UCSR0A UCSRA +# define UCSR0B UCSRB +# define U2X0 U2X +# define RXEN0 RXEN +# define TXEN0 TXEN +# define RXCIE0 RXCIE +# define UDRIE0 UDRIE +# endif +#endif + +/// +/// Macro defining a AVRUARTDriver port instance. +/// +#define AVRUARTDriverInstance(_name, _num) \ + AVRUARTDriver _name(_num, \ + &UBRR##_num##H, \ + &UBRR##_num##L, \ + &UCSR##_num##A, \ + &UCSR##_num##B, \ + U2X##_num, \ + (_BV(RXEN##_num) | _BV(TXEN##_num) | _BV(RXCIE##_num)), \ + (_BV(UDRIE##_num))); + +#define AVRUARTDriverISRs(_num) \ + AVRUARTDriverHandler(_num, \ + USART##_num##_RX_vect, \ + USART##_num##_UDRE_vect, \ + UDR##_num, \ + UCSR##_num##B, \ + _BV(UDRIE##_num)) + #endif // __AP_HAL_AVR_UART_DRIVER_H__ diff --git a/libraries/AP_HAL_AVR/examples/APM1/APM1.pde b/libraries/AP_HAL_AVR/examples/APM1/APM1.pde index 2a6d8c752f..7b86ebbb14 100644 --- a/libraries/AP_HAL_AVR/examples/APM1/APM1.pde +++ b/libraries/AP_HAL_AVR/examples/APM1/APM1.pde @@ -11,8 +11,14 @@ void loop (void) { } void setup (void) { - hal1.uart0->init(9600); - hal2.uart0->init(9600); + hal1.uart0->begin(9600); + hal2.uart0->begin(9600); + hal1.uart1->begin(9600); + hal2.uart1->begin(9600); + hal1.uart2->begin(9600); + hal2.uart2->begin(9600); + hal1.uart3->begin(9600); + hal2.uart3->begin(9600); } diff --git a/libraries/AP_HAL_AVR/utility/ftoa_engine.S b/libraries/AP_HAL_AVR/utility/ftoa_engine.S new file mode 100644 index 0000000000..4d35d0688b --- /dev/null +++ b/libraries/AP_HAL_AVR/utility/ftoa_engine.S @@ -0,0 +1,532 @@ +/* Copyright (c) 2005, Dmitry Xmelkov + All rights reserved. + + Redistribution and use in source and binary forms, with or without + modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in + the documentation and/or other materials provided with the + distribution. + * Neither the name of the copyright holders nor the names of + contributors may be used to endorse or promote products derived + from this software without specific prior written permission. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + POSSIBILITY OF SUCH DAMAGE. */ + +/* $Id: ftoa_engine.S,v 1.3 2009/04/01 23:11:00 arcanum Exp $ */ + +#ifndef __DOXYGEN__ + +#include "macros.inc" +#include "ftoa_engine.h" + +#if defined(__AVR_HAVE_LPMX__) && __AVR_HAVE_LPMX__ +# define AVR_ENH_LPM 1 +#else +# define AVR_ENH_LPM 0 +#endif + +/* + int __ftoa_engine (double val, char *buf, + unsigned char prec, unsigned char maxdgs) + Input: + val - value to convert + buf - output buffer address + prec - precision: number of decimal digits is 'prec + 1' + maxdgs - (0 if unused) precision restriction for "%f" specification + + Output: + return - decimal exponent of first digit + buf[0] - flags (FTOA_***) + buf[1],... - decimal digits + Number of digits: + maxdgs == 0 ? prec+1 : + (buf[0] & FTOA_CARRY) == 0 || buf[1] != '1' ? + aver(1, maxdgs+exp, prec+1) : + aver(1, masdgs+exp-1, prec+1) + + Notes: + * Output string is not 0-terminated. For possibility of user's buffer + usage in any case. + * If used, 'maxdgs' is a number of digits for value with zero exponent. +*/ + + /* Input */ +#define maxdgs r16 +#define prec r18 +#define buf_lo r20 +#define buf_hi r21 +#define val_lo r22 +#define val_hi r23 +#define val_hlo r24 +#define val_hhi r25 + + /* Float value parse */ +#define flag r19 + + /* Multiplication of mantisses */ +#define exp_sv r17 +#define mlt_1 r19 /* lowest result byte */ +#define mlt_2 r14 +#define mlt_3 r15 +#define mlt_4 r20 +#define mlt_5 r21 +#define mlt_6 r28 +#define mlt_7 r29 + + /* Conversion to string */ +#define pwr_2 r1 /* lowest byte of 'powr10' element */ +#define pwr_3 r17 +#define pwr_4 r19 +#define pwr_5 r22 +#define pwr_6 r25 +#define pwr_7 r0 +#define digit r23 +#define exp10 r24 + + /* Fixed */ +#define zero r1 + +/* ASSEMBLY_CLIB_SECTION */ + + .global __ftoa_engine + .type __ftoa_engine, "function" +__ftoa_engine: + +/* -------------------------------------------------------------------- + Float value parse. +*/ + ; limit 'prec' + cpi prec, 8 + brlo 1f + ldi prec, 7 +1: + ; init. + clr flag + X_movw XL, buf_lo + ; val_hhi := exponent, sign test and remove +#if FTOA_MINUS != 1 +# error FTOA_MINUS must be 1: add with carry used +#endif + lsl val_hhi + adc flag, zero ; FTOA_MINUS + sbrc val_hlo, 7 + ori val_hhi, 1 + ; zero test + adiw val_hlo, 0 + cpc val_lo, zero + cpc val_hi, zero + brne 3f + ; return 0 + ori flag, FTOA_ZERO + subi prec, -2 +2: st X+, flag + ldi flag, '0' + dec prec + brne 2b + ret ; r24,r25 == 0 +3: + ; infinity, NaN ? +#if FTOA_NAN != 2 * FTOA_INF +# error Must: FTOA_NAN == 2*FTOA_INF: 'rjmp' is absent +#endif + cpi val_hhi, 0xff + brlo 6f + cpi val_hlo, 0x80 + cpc val_hi, zero + cpc val_lo, zero + breq 5f + subi flag, -FTOA_INF ; FTOA_NAN +5: subi flag, -FTOA_INF +6: + ; write flags byte + st X+, flag + ; hidden bit + cpi val_hhi, 1 + brlo 7f ; if subnormal value + ori val_hlo, 0x80 +7: adc val_hhi, zero + ; pushes + push r29 + push r28 + push r17 + push r16 + push r15 + push r14 + +/* -------------------------------------------------------------------- + Multiplication of mantisses (val and table). + At the begin: + val_hlo .. val_lo - input value mantisse + val_hhi - input value exponent + X - second byte address (string begin) + At the end: + mlt_7 .. mlt_2 - multiplication result + exp10 - decimal exponent +*/ + + ; save + mov exp_sv, val_hhi + ; Z := & base10[exp / 8] (sizeof(base10[0]) == 5) + andi val_hhi, ~7 + lsr val_hhi ; (exp/8) * 4 + mov ZL, val_hhi + lsr val_hhi + lsr val_hhi ; exp/8 + add ZL, val_hhi ; (exp/8) * 5 + clr ZH + subi ZL, lo8(-(.L_base10)) + sbci ZH, hi8(-(.L_base10)) + ; highest mantissa byte (mult. shifting prepare) + clr val_hhi + ; result initializ. + clr mlt_1 + clr mlt_2 + clr mlt_3 + X_movw mlt_4, mlt_2 + X_movw mlt_6, mlt_2 + + ; multiply to 1-st table byte +#if AVR_ENH_LPM + lpm r0, Z+ +#else + lpm + adiw ZL, 1 +#endif + sec ; for loop end control + ror r0 + ; addition +10: brcc 11f + add mlt_1, val_lo + adc mlt_2, val_hi + adc mlt_3, val_hlo + adc mlt_4, val_hhi + adc mlt_5, zero + ; arg shift +11: lsl val_lo + rol val_hi + rol val_hlo + rol val_hhi + ; next bit + lsr r0 + brne 10b + + ; second table byte +#if AVR_ENH_LPM + lpm r0, Z+ ; C flag is stay 1 +#else + lpm + adiw ZL, 1 + sec +#endif + ror r0 + ; addition +12: brcc 13f + add mlt_2, val_hi ; val_hi is the least byte now + adc mlt_3, val_hlo + adc mlt_4, val_hhi + adc mlt_5, val_lo + adc mlt_6, zero + ; arg shift +13: lsl val_hi + rol val_hlo + rol val_hhi + rol val_lo + ; next bit + lsr r0 + brne 12b + + ; 3-t table byte +#if AVR_ENH_LPM + lpm r0, Z+ ; C flag is stay 1 +#else + lpm + adiw ZL, 1 + sec +#endif + ror r0 + ; addition +14: brcc 15f + add mlt_3, val_hlo ; val_hlo is the least byte now + adc mlt_4, val_hhi + adc mlt_5, val_lo + adc mlt_6, val_hi + adc mlt_7, zero + ; arg shift +15: lsl val_hlo + rol val_hhi + rol val_lo + rol val_hi + ; next bit + lsr r0 + brne 14b + + ; 4-t table byte +#if AVR_ENH_LPM + lpm r0, Z+ ; C flag is stay 1 +#else + lpm +#endif + ror r0 + ; addition +16: brcc 17f + add mlt_4, val_hhi ; val_hhi is the least byte now + adc mlt_5, val_lo + adc mlt_6, val_hi + adc mlt_7, val_hlo + ; arg shift +17: lsl val_hhi + rol val_lo + rol val_hi + rol val_hlo + ; next bit + lsr r0 + brne 16b + + ; decimal exponent +#if AVR_ENH_LPM + lpm exp10, Z +#else + adiw ZL, 1 + lpm + mov exp10, r0 +#endif + + ; result shift: mlt_7..2 >>= (~exp & 7) + com exp_sv + andi exp_sv, 7 + breq 19f +18: lsr mlt_7 + ror mlt_6 + ror mlt_5 + ror mlt_4 + ror mlt_3 + ror mlt_2 + dec exp_sv + brne 18b +19: + +/* -------------------------------------------------------------------- + Conversion to string. + + Registers usage: + mlt_7 .. mlt_2 - new mantissa (multiplication result) + pwr_7 .. pwr_2 - 'powr10' table element + Z - 'powr10' table pointer + X - output string pointer + maxdgs - number of digits + prec - number of digits stays to output + exp10 - decimal exponent + digit - conversion process + + At the end: + X - end of buffer (nonfilled byte) + exp10 - corrected dec. exponent + mlt_7 .. mlt_2 - remainder + pwr_7 .. pwr_2 - last powr10[] element + + Notes: + * It is possible to leave out powr10'x table with subnormal value. + Result: accuracy degrease on the rounding phase. No matter: high + precision with subnormals is not needed. (Now 0x00000001 is converted + exactly on prec = 5, i.e. 6 digits.) +*/ + + ; to find first digit + ldi ZL, lo8(.L_powr10) + ldi ZH, hi8(.L_powr10) + set + ; 'pwr10' element reading +.L_digit: + X_lpm pwr_2, Z+ + X_lpm pwr_3, Z+ + X_lpm pwr_4, Z+ + X_lpm pwr_5, Z+ + X_lpm pwr_6, Z+ + X_lpm pwr_7, Z+ + ; 'digit' init. + ldi digit, '0' - 1 + ; subtraction loop +20: inc digit + sub mlt_2, pwr_2 + sbc mlt_3, pwr_3 + sbc mlt_4, pwr_4 + sbc mlt_5, pwr_5 + sbc mlt_6, pwr_6 + sbc mlt_7, pwr_7 + brsh 20b + ; restore mult + add mlt_2, pwr_2 + adc mlt_3, pwr_3 + adc mlt_4, pwr_4 + adc mlt_5, pwr_5 + adc mlt_6, pwr_6 + adc mlt_7, pwr_7 + ; analisys + brtc 25f + cpi digit, '0' + brne 21f ; this is the first digit finded + dec exp10 + rjmp .L_digit + ; now is the first digit +21: clt + ; number of digits + subi maxdgs, 1 + brlo 23f ; maxdgs was 0 + add maxdgs, exp10 + brpl 22f + clr maxdgs +22: cp maxdgs, prec + brsh 23f + mov prec, maxdgs +23: inc prec + mov maxdgs, prec + ; operate digit +25: cpi digit, '0' + 10 + brlo 27f + ; overflow, digit > '9' + ldi digit, '9' +26: st X+, digit + dec prec + brne 26b + rjmp .L_up + ; write digit +27: st X+, digit + dec prec + brne .L_digit + +/* -------------------------------------------------------------------- + Rounding. +*/ +.L_round: + ; pwr10 /= 2 + lsr pwr_7 + ror pwr_6 + ror pwr_5 + ror pwr_4 + ror pwr_3 + ror pwr_2 + ; mult -= pwr10 (half of last 'pwr10' value) + sub mlt_2, pwr_2 + sbc mlt_3, pwr_3 + sbc mlt_4, pwr_4 + sbc mlt_5, pwr_5 + sbc mlt_6, pwr_6 + sbc mlt_7, pwr_7 + ; rounding direction? + brlo .L_rest + ; round to up +.L_up: + inc prec + ld digit, -X + inc digit + cpi digit, '9' + 1 + brlo 31f + ldi digit, '0' +31: st X, digit + cpse prec, maxdgs + brsh .L_up + ; it was a carry to master digit + ld digit, -X ; flags + ori digit, FTOA_CARRY ; 'C' is not changed + st X+, digit + brlo .L_rest ; above comparison + ; overflow + inc exp10 + ldi digit, '1' +32: st X+, digit + ldi digit, '0' + dec prec + brne 32b + ; restore +.L_rest: + clr zero + pop r14 + pop r15 + pop r16 + pop r17 + pop r28 + pop r29 + ; return + clr r25 + sbrc exp10, 7 ; high byte + com r25 + ret + + .size __ftoa_engine, . - __ftoa_engine + +/* -------------------------------------------------------------------- + Tables. '.L_powr10' is placed first -- for subnormals stability. +*/ + .section .progmem.data,"a",@progbits + + .type .L_powr10, "object" +.L_powr10: + .byte 0, 64, 122, 16, 243, 90 ; 100000000000000 + .byte 0, 160, 114, 78, 24, 9 ; 10000000000000 + .byte 0, 16, 165, 212, 232, 0 ; 1000000000000 + .byte 0, 232, 118, 72, 23, 0 ; 100000000000 + .byte 0, 228, 11, 84, 2, 0 ; 10000000000 + .byte 0, 202, 154, 59, 0, 0 ; 1000000000 + .byte 0, 225, 245, 5, 0, 0 ; 100000000 + .byte 128, 150, 152, 0, 0, 0 ; 10000000 + .byte 64, 66, 15, 0, 0, 0 ; 1000000 + .byte 160, 134, 1, 0, 0, 0 ; 100000 + .byte 16, 39, 0, 0, 0, 0 ; 10000 + .byte 232, 3, 0, 0, 0, 0 ; 1000 + .byte 100, 0, 0, 0, 0, 0 ; 100 + .byte 10, 0, 0, 0, 0, 0 ; 10 + .byte 1, 0, 0, 0, 0, 0 ; 1 + .size .L_powr10, . - .L_powr10 + + .type .L_base10, "object" +.L_base10: + .byte 44, 118, 216, 136, -36 ; 2295887404 + .byte 103, 79, 8, 35, -33 ; 587747175 + .byte 193, 223, 174, 89, -31 ; 1504632769 + .byte 177, 183, 150, 229, -29 ; 3851859889 + .byte 228, 83, 198, 58, -26 ; 986076132 + .byte 81, 153, 118, 150, -24 ; 2524354897 + .byte 230, 194, 132, 38, -21 ; 646234854 + .byte 137, 140, 155, 98, -19 ; 1654361225 + .byte 64, 124, 111, 252, -17 ; 4235164736 + .byte 188, 156, 159, 64, -14 ; 1084202172 + .byte 186, 165, 111, 165, -12 ; 2775557562 + .byte 144, 5, 90, 42, -9 ; 710542736 + .byte 92, 147, 107, 108, -7 ; 1818989404 + .byte 103, 109, 193, 27, -4 ; 465661287 + .byte 224, 228, 13, 71, -2 ; 1192092896 + .byte 245, 32, 230, 181, 0 ; 3051757813 + .byte 208, 237, 144, 46, 3 ; 781250000 + .byte 0, 148, 53, 119, 5 ; 2000000000 + .byte 0, 128, 132, 30, 8 ; 512000000 + .byte 0, 0, 32, 78, 10 ; 1310720000 + .byte 0, 0, 0, 200, 12 ; 3355443200 + .byte 51, 51, 51, 51, 15 ; 858993459 + .byte 152, 110, 18, 131, 17 ; 2199023256 + .byte 65, 239, 141, 33, 20 ; 562949953 + .byte 137, 59, 230, 85, 22 ; 1441151881 + .byte 207, 254, 230, 219, 24 ; 3689348815 + .byte 209, 132, 75, 56, 27 ; 944473297 + .byte 247, 124, 29, 144, 29 ; 2417851639 + .byte 164, 187, 228, 36, 32 ; 618970020 + .byte 50, 132, 114, 94, 34 ; 1584563250 + .byte 129, 0, 201, 241, 36 ; 4056481921 + .byte 236, 161, 229, 61, 39 ; 1038459372 + .size .L_base10, . - .L_base10 + + .end +#endif /* !__DOXYGEN__ */ \ No newline at end of file diff --git a/libraries/AP_HAL_AVR/utility/ftoa_engine.h b/libraries/AP_HAL_AVR/utility/ftoa_engine.h new file mode 100644 index 0000000000..be8d221d2e --- /dev/null +++ b/libraries/AP_HAL_AVR/utility/ftoa_engine.h @@ -0,0 +1,48 @@ +/* Copyright (c) 2005, Dmitry Xmelkov + All rights reserved. + + Redistribution and use in source and binary forms, with or without + modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in + the documentation and/or other materials provided with the + distribution. + * Neither the name of the copyright holders nor the names of + contributors may be used to endorse or promote products derived + from this software without specific prior written permission. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + POSSIBILITY OF SUCH DAMAGE. */ + +/* $Id: ftoa_engine.h 1218 2007-02-18 13:18:41Z dmix $ */ + +#ifndef _FTOA_ENGINE_H +#define _FTOA_ENGINE_H + +#ifndef __ASSEMBLER__ + +int __ftoa_engine (double val, char *buf, + unsigned char prec, unsigned char maxdgs); + +#endif + +/* '__ftoa_engine' return next flags (in buf[0]): */ +#define FTOA_MINUS 1 +#define FTOA_ZERO 2 +#define FTOA_INF 4 +#define FTOA_NAN 8 +#define FTOA_CARRY 16 /* Carry was to master position. */ + +#endif /* !_FTOA_ENGINE_H */ diff --git a/libraries/AP_HAL_AVR/utility/macros.inc b/libraries/AP_HAL_AVR/utility/macros.inc new file mode 100644 index 0000000000..a25b746085 --- /dev/null +++ b/libraries/AP_HAL_AVR/utility/macros.inc @@ -0,0 +1,365 @@ +/* Copyright (c) 2002, 2005, 2006, 2007 Marek Michalkiewicz + Copyright (c) 2006 Dmitry Xmelkov + All rights reserved. + + Redistribution and use in source and binary forms, with or without + modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in + the documentation and/or other materials provided with the + distribution. + + * Neither the name of the copyright holders nor the names of + contributors may be used to endorse or promote products derived + from this software without specific prior written permission. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + POSSIBILITY OF SUCH DAMAGE. */ + +/* + macros.inc - macros for use in assembler sources + + Contributors: + Created by Marek Michalkiewicz + */ + +#include +//#include "sectionname.h" + +/* if not defined, assume old version with underscores */ +#ifndef __USER_LABEL_PREFIX__ +#define __USER_LABEL_PREFIX__ _ +#endif + +#ifndef __REGISTER_PREFIX__ +#define __REGISTER_PREFIX__ +#endif + +/* the assembler line separator (just in case it ever changes) */ +#define _L $ + +#define CONCAT1(a, b) CONCAT2(a, b) +#define CONCAT2(a, b) a ## b + +#define _U(x) CONCAT1(__USER_LABEL_PREFIX__, x) + +#define _R(x) CONCAT1(__REGISTER_PREFIX__, x) + +/* these should help to fix the "can't have function named r1()" bug + which may require adding '%' in front of register names. */ + +#define r0 _R(r0) +#define r1 _R(r1) +#define r2 _R(r2) +#define r3 _R(r3) +#define r4 _R(r4) +#define r5 _R(r5) +#define r6 _R(r6) +#define r7 _R(r7) +#define r8 _R(r8) +#define r9 _R(r9) +#define r10 _R(r10) +#define r11 _R(r11) +#define r12 _R(r12) +#define r13 _R(r13) +#define r14 _R(r14) +#define r15 _R(r15) +#define r16 _R(r16) +#define r17 _R(r17) +#define r18 _R(r18) +#define r19 _R(r19) +#define r20 _R(r20) +#define r21 _R(r21) +#define r22 _R(r22) +#define r23 _R(r23) +#define r24 _R(r24) +#define r25 _R(r25) +#define r26 _R(r26) +#define r27 _R(r27) +#define r28 _R(r28) +#define r29 _R(r29) +#define r30 _R(r30) +#define r31 _R(r31) + +#ifndef __tmp_reg__ +#define __tmp_reg__ r0 +#endif + +#ifndef __zero_reg__ +#define __zero_reg__ r1 +#endif + +#if __AVR_MEGA__ + #define XJMP jmp + #define XCALL call +#else + #define XJMP rjmp + #define XCALL rcall +#endif + +/* used only by fplib/strtod.S - libgcc internal function calls */ +#define PROLOGUE_SAVES(offset) XJMP (__prologue_saves__ + 2 * (offset)) +#define EPILOGUE_RESTORES(offset) XJMP (__epilogue_restores__ + 2 * (offset)) + +#if FLASHEND > 0x10000 /* ATmega103 */ + #define BIG_CODE 1 +#else + #define BIG_CODE 0 +#endif + +#ifndef __AVR_HAVE_MOVW__ +# if defined(__AVR_ENHANCED__) && __AVR_ENHANCED__ +# define __AVR_HAVE_MOVW__ 1 +# endif +#endif + +#ifndef __AVR_HAVE_LPMX__ +# if defined(__AVR_ENHANCED__) && __AVR_ENHANCED__ +# define __AVR_HAVE_LPMX__ 1 +# endif +#endif + +#ifndef __AVR_HAVE_MUL__ +# if defined(__AVR_ENHANCED__) && __AVR_ENHANCED__ +# define __AVR_HAVE_MUL__ 1 +# endif +#endif + +/* + Smart version of movw: + - uses "movw" if possible (supported by MCU, and both registers even) + - handles overlapping register pairs correctly + - no instruction generated if source and destination are the same + (may expand to 0, 1 or 2 instructions). + */ + +.macro X_movw dst src + .L_movw_dst = -1 + .L_movw_src = -1 + .L_movw_n = 0 + .irp reg, r0, r1, r2, r3, r4, r5, r6, r7, r8, r9, \ + r10,r11,r12,r13,r14,r15,r16,r17,r18,r19, \ + r20,r21,r22,r23,r24,r25,r26,r27,r28,r29, \ + r30,r31 + .ifc \reg,\dst + .L_movw_dst = .L_movw_n + .endif + .ifc \reg,\src + .L_movw_src = .L_movw_n + .endif + .L_movw_n = .L_movw_n + 1 + .endr + .L_movw_n = 0 + .irp reg, R0, R1, R2, R3, R4, R5, R6, R7, R8, R9, \ + R10,R11,R12,R13,R14,R15,R16,R17,R18,R19, \ + R20,R21,R22,R23,R24,R25,R26,R27,R28,R29, \ + R30,R31 + .ifc \reg,\dst + .L_movw_dst = .L_movw_n + .endif + .ifc \reg,\src + .L_movw_src = .L_movw_n + .endif + .L_movw_n = .L_movw_n + 1 + .endr + .if .L_movw_dst < 0 + .L_movw_n = 0 + .rept 32 + .if \dst == .L_movw_n + .L_movw_dst = .L_movw_n + .endif + .L_movw_n = .L_movw_n + 1 + .endr + .endif + .if .L_movw_src < 0 + .L_movw_n = 0 + .rept 32 + .if \src == .L_movw_n + .L_movw_src = .L_movw_n + .endif + .L_movw_n = .L_movw_n + 1 + .endr + .endif + .if (.L_movw_dst < 0) || (.L_movw_src < 0) + .err ; Invalid 'X_movw' arg. + .endif + + .if ((.L_movw_src) - (.L_movw_dst)) /* different registers */ + .if (((.L_movw_src) | (.L_movw_dst)) & 0x01) + .if (((.L_movw_src)-(.L_movw_dst)) & 0x80) /* src < dest */ + mov (.L_movw_dst)+1, (.L_movw_src)+1 + mov (.L_movw_dst), (.L_movw_src) + .else /* src > dest */ + mov (.L_movw_dst), (.L_movw_src) + mov (.L_movw_dst)+1, (.L_movw_src)+1 + .endif + .else /* both even -> overlap not possible */ +#if defined(__AVR_HAVE_MOVW__) && __AVR_HAVE_MOVW__ + movw \dst, \src +#else + mov (.L_movw_dst), (.L_movw_src) + mov (.L_movw_dst)+1, (.L_movw_src)+1 +#endif + .endif + .endif +.endm + +/* Macro 'X_lpm' extends enhanced lpm instruction for classic chips. + Usage: + X_lpm reg, dst + where + reg is 0..31, r0..r31 or R0..R31 + dst is z, Z, z+ or Z+ + It is possible to omit both arguments. + + Possible results for classic chips: + lpm + lpm / mov Rd,r0 + lpm / adiw ZL,1 + lpm / mov Rd,r0 / adiw ZL,1 + + For enhanced chips it is one instruction always. + + ATTENTION: unlike enhanced chips SREG (S,V,N,Z,C) flags are + changed in case of 'Z+' dst. R0 is scratch. + */ +.macro X_lpm dst=r0, src=Z + + /* dst evaluation */ + .L_lpm_dst = -1 + + .L_lpm_n = 0 + .irp reg, r0, r1, r2, r3, r4, r5, r6, r7, r8, r9, \ + r10,r11,r12,r13,r14,r15,r16,r17,r18,r19, \ + r20,r21,r22,r23,r24,r25,r26,r27,r28,r29, \ + r30,r31 + .ifc \reg,\dst + .L_lpm_dst = .L_lpm_n + .endif + .L_lpm_n = .L_lpm_n + 1 + .endr + + .L_lpm_n = 0 + .irp reg, R0, R1, R2, R3, R4, R5, R6, R7, R8, R9, \ + R10,R11,R12,R13,R14,R15,R16,R17,R18,R19, \ + R20,R21,R22,R23,R24,R25,R26,R27,R28,R29, \ + R30,R31 + .ifc \reg,\dst + .L_lpm_dst = .L_lpm_n + .endif + .L_lpm_n = .L_lpm_n + 1 + .endr + + .if .L_lpm_dst < 0 + .L_lpm_n = 0 + .rept 32 + .if \dst == .L_lpm_n + .L_lpm_dst = .L_lpm_n + .endif + .L_lpm_n = .L_lpm_n + 1 + .endr + .endif + + .if (.L_lpm_dst < 0) + .err ; Invalid dst arg of 'X_lpm' macro. + .endif + + /* src evaluation */ + .L_lpm_src = -1 + .L_lpm_n = 0 + .irp reg, z,Z,z+,Z+ + .ifc \reg,\src + .L_lpm_src = .L_lpm_n + .endif + .L_lpm_n = .L_lpm_n + 1 + .endr + + .if (.L_lpm_src < 0) + .err ; Invalid src arg of 'X_lpm' macro. + .endif + + /* instruction(s) */ + .if .L_lpm_src < 2 + .if .L_lpm_dst == 0 + lpm + .else +#if defined(__AVR_HAVE_LPMX__) && __AVR_HAVE_LPMX__ + lpm .L_lpm_dst, Z +#else + lpm + mov .L_lpm_dst, r0 +#endif + .endif + .else + .if (.L_lpm_dst >= 30) + .err ; Registers 30 and 31 are inhibited as 'X_lpm *,Z+' dst. + .endif +#if defined(__AVR_HAVE_LPMX__) && __AVR_HAVE_LPMX__ + lpm .L_lpm_dst, Z+ +#else + lpm + .if .L_lpm_dst + mov .L_lpm_dst, r0 + .endif + adiw r30, 1 +#endif + .endif +.endm + +/* + LPM_R0_ZPLUS_INIT is used before the loop to initialize RAMPZ + for future devices with RAMPZ:Z auto-increment - [e]lpm r0, Z+. + + LPM_R0_ZPLUS_NEXT is used inside the loop to load a byte from + the program memory at [RAMPZ:]Z to R0, and increment [RAMPZ:]Z. + + The argument in both macros is a register that contains the + high byte (bits 23-16) of the address, bits 15-0 should be in + the Z (r31:r30) register. It can be any register except for: + r0, r1 (__zero_reg__ - assumed to always contain 0), r30, r31. + */ + + .macro LPM_R0_ZPLUS_INIT hhi +#if __AVR_ENHANCED__ + #if BIG_CODE + out AVR_RAMPZ_ADDR, \hhi + #endif +#endif + .endm + + .macro LPM_R0_ZPLUS_NEXT hhi +#if __AVR_ENHANCED__ + #if BIG_CODE + /* ELPM with RAMPZ:Z post-increment, load RAMPZ only once */ + elpm r0, Z+ + #else + /* LPM with Z post-increment, max 64K, no RAMPZ (ATmega83/161/163/32) */ + lpm r0, Z+ + #endif +#else + #if BIG_CODE + /* ELPM without post-increment, load RAMPZ each time (ATmega103) */ + out AVR_RAMPZ_ADDR, \hhi + elpm + adiw r30,1 + adc \hhi, __zero_reg__ + #else + /* LPM without post-increment, max 64K, no RAMPZ (AT90S*) */ + lpm + adiw r30,1 + #endif +#endif + .endm diff --git a/libraries/AP_HAL_AVR/utility/ntz.h b/libraries/AP_HAL_AVR/utility/ntz.h new file mode 100644 index 0000000000..08e2586519 --- /dev/null +++ b/libraries/AP_HAL_AVR/utility/ntz.h @@ -0,0 +1,54 @@ +/* Copyright (c) 2007, Dmitry Xmelkov + All rights reserved. + + Redistribution and use in source and binary forms, with or without + modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in + the documentation and/or other materials provided with the + distribution. + * Neither the name of the copyright holders nor the names of + contributors may be used to endorse or promote products derived + from this software without specific prior written permission. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + POSSIBILITY OF SUCH DAMAGE. */ + +/* $Id: ntz.h 1217 2007-02-18 13:18:05Z dmix $ */ + +#ifndef _NTZ_H +#define _NTZ_H + +/* Number of Tail Zeros: ntz(x)= (ffs(x) ? ffs(x)-1 : 16) + It works with all: cpp, gcc and gas expressions. */ +#define ntz(x) \ + ( (1 & (((x) & 1) == 0)) \ + + (1 & (((x) & 3) == 0)) \ + + (1 & (((x) & 7) == 0)) \ + + (1 & (((x) & 017) == 0)) \ + + (1 & (((x) & 037) == 0)) \ + + (1 & (((x) & 077) == 0)) \ + + (1 & (((x) & 0177) == 0)) \ + + (1 & (((x) & 0377) == 0)) \ + + (1 & (((x) & 0777) == 0)) \ + + (1 & (((x) & 01777) == 0)) \ + + (1 & (((x) & 03777) == 0)) \ + + (1 & (((x) & 07777) == 0)) \ + + (1 & (((x) & 017777) == 0)) \ + + (1 & (((x) & 037777) == 0)) \ + + (1 & (((x) & 077777) == 0)) \ + + (1 & (((x) & 0177777) == 0)) ) + +#endif /* !_NTZ_H */ diff --git a/libraries/AP_HAL_AVR/utility/ultoa_invert.S b/libraries/AP_HAL_AVR/utility/ultoa_invert.S new file mode 100644 index 0000000000..934e0a7c66 --- /dev/null +++ b/libraries/AP_HAL_AVR/utility/ultoa_invert.S @@ -0,0 +1,216 @@ +/* Copyright (c) 2005,2007 Dmitry Xmelkov + All rights reserved. + + Redistribution and use in source and binary forms, with or without + modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in + the documentation and/or other materials provided with the + distribution. + * Neither the name of the copyright holders nor the names of + contributors may be used to endorse or promote products derived + from this software without specific prior written permission. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + POSSIBILITY OF SUCH DAMAGE. */ + +/* $Id: ultoa_invert.S 1944 2009-04-01 23:12:20Z arcanum $ */ + +#ifndef __DOXYGEN__ + +#include "macros.inc" +#include "ntz.h" +#include "xtoa_fast.h" + +/* -------------------------------------------------------------------- + char * __ultoa_invert (unsigned long val, char * str, int base) + + This function is intended for usage as internal printf's one. + It differs from others of `xtoa_fast' family: + * srt[] will NOT 0 terminated. + * Sequence of digits is inverted. + * It returns pointer to first byte after a string. + * Only `XTOA_UPPER' flag is operated. + Notes: + * base: check only 8 and 16, all others are treated as 10. + (internal printf's function). +*/ + + /* Input */ +#define v_lo r22 +#define v_hi r23 +#define v_hlo r24 +#define v_hhi r25 +#define str_lo r20 +#define str_hi r21 +#define base r18 +#define flags r19 + + /* Used */ +#define v_fifth r26 /* val: bits 39..32 */ +#define t_lo r18 /* temporary for shifted `val' */ +#define t_hi r19 +#define t_hlo r20 +#define t_hhi r21 +#define symb r20 /* write to string */ +#define cnt r27 /* shift loop counter, local arg */ + + /* Fixed */ +#define rzero r1 + +/* ASSEMBLY_CLIB_SECTION */ + .global __ultoa_invert + .type __ultoa_invert, "function" + +__ultoa_invert: + X_movw ZL, str_lo + clr v_fifth ; needed for all (ultoa_lsr) + cpi base, 8 + breq .L_oct + cpi base, 16 + breq .L_hex + + ; decimal format + clt ; flag of val == 0 +.L_dec_loop: + push v_lo ; to calculate remander + ; val &= ~1 + andi v_lo, ~1 + ; val += 2 + subi v_lo, lo8(-2) + sbci v_hi, hi8(-2) + sbci v_hlo, hlo8(-2) + sbci v_hhi, hhi8(-2) + sbci v_fifth, hhi8(-2) + ; val += val/2 + ldi cnt, 1 + rcall .L_div_add + ; val += val/16 + ldi cnt, 4 + rcall .L_div_add + ; val += val/256 + add v_lo, v_hi + adc v_hi, v_hlo + adc v_hlo, v_hhi + adc v_hhi, v_fifth + adc v_fifth, rzero + ; val += val/65536 + add v_lo, v_hlo + adc v_hi, v_hhi + adc v_hlo, v_fifth + adc v_hhi, rzero + adc v_fifth, rzero + ; val += val >> 32 + add v_lo, v_fifth + adc v_hi, rzero + adc v_hlo, rzero + adc v_hhi, rzero + adc v_fifth, rzero + ; division result: val /= 16 + rcall .L_lsr_4 ; v_fitth := 0 + brne 1f + set ; T := Z flag +1: + ; rem: val_original - 10*val + pop t_hi +#if defined(__AVR_ENHANCED__) && __AVR_ENHANCED__ + ldi t_lo, 10 + mul t_lo, v_lo + clr r1 +#else + mov r0, v_lo + lsl r0 + sub t_hi, r0 + lsl r0 + lsl r0 +#endif + sub t_hi, r0 + ; output digit + subi t_hi, lo8(-'0') + st Z+, t_hi + ; quotient == 0 ? + brtc .L_dec_loop + ; end of string +.L_eos: + X_movw r24, ZL + ret + + ; octal format +.L_oct: + mov symb, v_lo + andi symb, 7 + subi symb, lo8(-'0') + st Z+, symb + ldi cnt, 3 + rcall .L_lsr + brne .L_oct + rjmp .L_eos + + ; hex format +.L_hex: + mov symb, v_lo + andi symb, 0x0f + subi symb, lo8(-'0') + cpi symb, '9' + 1 + brlo 3f + subi symb, lo8('9' + 1 - 'a') + sbrc flags, ntz(XTOA_UPPER) - 8 + subi symb, lo8('a' - 'A') +3: st Z+, symb + rcall .L_lsr_4 + brne .L_hex + rjmp .L_eos + +.L_lsr_4: + ldi cnt, 4 +.L_lsr: + lsr v_fifth + ror v_hhi + ror v_hlo + ror v_hi + ror v_lo + dec cnt + brne .L_lsr + ; tst + sbiw v_hlo, 0 ; only Z flag is needed + cpc v_lo, rzero + cpc v_hi, rzero + ret + +.L_div_add: + ; copy to temporary + X_movw t_lo, v_lo + X_movw t_hlo, v_hlo + mov r0, v_fifth + ; lsr temporary +7: lsr r0 + ror t_hhi + ror t_hlo + ror t_hi + ror t_lo + dec cnt + brne 7b + ; add + add v_lo, t_lo + adc v_hi, t_hi + adc v_hlo, t_hlo + adc v_hhi, t_hhi + adc v_fifth, r0 ; here r0 == 0 + ret + + .size __ultoa_invert, . - __ultoa_invert + .end + +#endif /* !__DOXYGEN__ */ diff --git a/libraries/AP_HAL_AVR/utility/vprintf.cpp b/libraries/AP_HAL_AVR/utility/vprintf.cpp new file mode 100644 index 0000000000..ef7d9481b8 --- /dev/null +++ b/libraries/AP_HAL_AVR/utility/vprintf.cpp @@ -0,0 +1,518 @@ +// -*- Mode: C++; c-basic-offset: 8; indent-tabs-mode: nil -*- +/* + Adapted from the avr-libc vfprintf: + + Copyright (c) 2002, Alexander Popov (sasho@vip.bg) + Copyright (c) 2002,2004,2005 Joerg Wunsch + Copyright (c) 2005, Helmut Wallner + Copyright (c) 2007, Dmitry Xmelkov + All rights reserved. + + Redistribution and use in source and binary forms, with or without + modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in + the documentation and/or other materials provided with the + distribution. + * Neither the name of the copyright holders nor the names of + contributors may be used to endorse or promote products derived + from this software without specific prior written permission. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + POSSIBILITY OF SUCH DAMAGE. +*/ + +/* From: Id: printf_p_new.c,v 1.1.1.9 2002/10/15 20:10:28 joerg_wunsch Exp */ +/* $Id: vfprintf.c,v 1.18.2.1 2009/04/01 23:12:06 arcanum Exp $ */ + + +#include +#include +#include +extern "C" { +#include "ftoa_engine.h" +#include "ntz.h" +#include "xtoa_fast.h" +} + +#include +#include "../UARTDriver.h" +using namespace AP_HAL_AVR; + +// workaround for GCC bug c++/34734 +#undef PROGMEM +#define PROGMEM __attribute__(( section(".progmem.data") )) +#undef PSTR +#define PSTR(s) (__extension__({static prog_char __c[] PROGMEM = (s); &__c[0];})) + +#ifdef GETBYTE +#undef GETBYTE +#endif +#define GETBYTE(flag, mask, pnt) ({ \ + unsigned char __c; \ + asm ( \ + "sbrc %2,%3 \n\t" \ + "lpm %0,Z+ \n\t" \ + "sbrs %2,%3 \n\t" \ + "ld %0,Z+ " \ + : "=r" (__c), \ + "+z" (pnt) \ + : "r" (flag), \ + "I" (ntz(mask)) \ + ); \ + __c; \ + }) + +#define FL_ZFILL 0x01 +#define FL_PLUS 0x02 +#define FL_SPACE 0x04 +#define FL_LPAD 0x08 +#define FL_ALT 0x10 +#define FL_WIDTH 0x20 +#define FL_PREC 0x40 +#define FL_LONG 0x80 + +#define FL_PGMSTRING FL_LONG +#define FL_NEGATIVE FL_LONG + +#define FL_ALTUPP FL_PLUS +#define FL_ALTHEX FL_SPACE + +#define FL_FLTUPP FL_ALT +#define FL_FLTEXP FL_PREC +#define FL_FLTFIX FL_LONG + +void +AVRUARTDriver::_vprintf (unsigned char in_progmem, const char *fmt, va_list ap) +{ + unsigned char c; /* holds a char from the format string */ + unsigned char flags; + unsigned char width; + unsigned char prec; + unsigned char buf[11]; /* size for -1 in octal, without '\0' */ + + for (;;) { + + /* + * Process non-format characters + */ + for (;;) { + c = GETBYTE (in_progmem, 1, fmt); + if (!c) return; + if (c == '%') { + c = GETBYTE (in_progmem, 1, fmt); + if (c != '%') break; + } + /* emit cr before lf to make most terminals happy */ + if (c == '\n') + write('\r'); + write(c); + } + + flags = 0; + width = 0; + prec = 0; + + /* + * Process format adjustment characters, precision, width. + */ + do { + if (flags < FL_WIDTH) { + switch (c) { + case '0': + flags |= FL_ZFILL; + continue; + case '+': + flags |= FL_PLUS; + /* FALLTHROUGH */ + case ' ': + flags |= FL_SPACE; + continue; + case '-': + flags |= FL_LPAD; + continue; + case '#': + flags |= FL_ALT; + continue; + } + } + + if (flags < FL_LONG) { + if (c >= '0' && c <= '9') { + c -= '0'; + if (flags & FL_PREC) { + prec = 10*prec + c; + continue; + } + width = 10*width + c; + flags |= FL_WIDTH; + continue; + } + if (c == '.') { + if (flags & FL_PREC) + return; + flags |= FL_PREC; + continue; + } + if (c == 'l') { + flags |= FL_LONG; + continue; + } + if (c == 'h') + continue; + } + + break; + } while ( (c = GETBYTE (in_progmem, 1, fmt)) != 0); + + /* + * Handle floating-point formats E, F, G, e, f, g. + */ + if (c >= 'E' && c <= 'G') { + flags |= FL_FLTUPP; + c += 'e' - 'E'; + goto flt_oper; + + } else if (c >= 'e' && c <= 'g') { + + int exp; /* exponent of master decimal digit */ + int n; + unsigned char vtype; /* result of float value parse */ + unsigned char sign; /* sign character (or 0) */ + unsigned char ndigs; + + flags &= ~FL_FLTUPP; + + flt_oper: + if (!(flags & FL_PREC)) + prec = 6; + flags &= ~(FL_FLTEXP | FL_FLTFIX); + if (c == 'e') + flags |= FL_FLTEXP; + else if (c == 'f') + flags |= FL_FLTFIX; + else if (prec > 0) + prec -= 1; + + if (flags & FL_FLTFIX) { + vtype = 7; /* 'prec' arg for 'ftoa_engine' */ + ndigs = prec < 60 ? prec + 1 : 60; + } else { + if (prec > 7) prec = 7; + vtype = prec; + ndigs = 0; + } + exp = __ftoa_engine (va_arg(ap,double), (char *)buf, vtype, ndigs); + vtype = buf[0]; + + sign = 0; + if ((vtype & FTOA_MINUS) && !(vtype & FTOA_NAN)) + sign = '-'; + else if (flags & FL_PLUS) + sign = '+'; + else if (flags & FL_SPACE) + sign = ' '; + + if (vtype & (FTOA_NAN | FTOA_INF)) { + const char *p; + ndigs = sign ? 4 : 3; + if (width > ndigs) { + width -= ndigs; + if (!(flags & FL_LPAD)) { + do { + write(' '); + } while (--width); + } + } else { + width = 0; + } + if (sign) + write(sign); + p = PSTR("inf"); + if (vtype & FTOA_NAN) + p = PSTR("nan"); + while ( (ndigs = pgm_read_byte((const prog_char *)p)) != 0) { + if (flags & FL_FLTUPP) + ndigs += 'I' - 'i'; + write(ndigs); + p++; + } + goto tail; + } + + /* Output format adjustment, number of decimal digits in buf[] */ + if (flags & FL_FLTFIX) { + ndigs += exp; + if ((vtype & FTOA_CARRY) && buf[1] == '1') + ndigs -= 1; + if ((signed char)ndigs < 1) + ndigs = 1; + else if (ndigs > 8) + ndigs = 8; + } else if (!(flags & FL_FLTEXP)) { /* 'g(G)' format */ + if (exp <= prec && exp >= -4) + flags |= FL_FLTFIX; + while (prec && buf[1+prec] == '0') + prec--; + if (flags & FL_FLTFIX) { + ndigs = prec + 1; /* number of digits in buf */ + prec = prec > exp + ? prec - exp : 0; /* fractional part length */ + } + } + + /* Conversion result length, width := free space length */ + if (flags & FL_FLTFIX) + n = (exp>0 ? exp+1 : 1); + else + n = 5; /* 1e+00 */ + if (sign) n += 1; + if (prec) n += prec + 1; + width = width > n ? width - n : 0; + + /* Output before first digit */ + if (!(flags & (FL_LPAD | FL_ZFILL))) { + while (width) { + write(' '); + width--; + } + } + if (sign) write(sign); + if (!(flags & FL_LPAD)) { + while (width) { + write('0'); + width--; + } + } + + if (flags & FL_FLTFIX) { /* 'f' format */ + + n = exp > 0 ? exp : 0; /* exponent of left digit */ + do { + if (n == -1) + write('.'); + flags = (n <= exp && n > exp - ndigs) + ? buf[exp - n + 1] : '0'; + if (--n < -prec) + break; + write(flags); + } while (1); + if (n == exp + && (buf[1] > '5' + || (buf[1] == '5' && !(vtype & FTOA_CARRY))) ) + { + flags = '1'; + } + write(flags); + + } else { /* 'e(E)' format */ + + /* mantissa */ + if (buf[1] != '1') + vtype &= ~FTOA_CARRY; + write(buf[1]); + if (prec) { + write('.'); + sign = 2; + do { + write(buf[sign++]); + } while (--prec); + } + + /* exponent */ + write(flags & FL_FLTUPP ? 'E' : 'e'); + ndigs = '+'; + if (exp < 0 || (exp == 0 && (vtype & FTOA_CARRY) != 0)) { + exp = -exp; + ndigs = '-'; + } + write(ndigs); + for (ndigs = '0'; exp >= 10; exp -= 10) + ndigs += 1; + write(ndigs); + write('0' + exp); + } + + goto tail; + } + + /* + * Handle string formats c, s, S. + */ + { + const char * pnt; + size_t size; + + switch (c) { + + case 'c': + buf[0] = va_arg (ap, int); + pnt = (char *)buf; + size = 1; + goto no_pgmstring; + + case 's': + pnt = va_arg (ap, char *); + size = strnlen (pnt, (flags & FL_PREC) ? prec : ~0); + no_pgmstring: + flags &= ~FL_PGMSTRING; + goto str_lpad; + + case 'S': + // pgmstring: // not yet used + pnt = va_arg (ap, char *); + size = strnlen_P (pnt, (flags & FL_PREC) ? prec : ~0); + flags |= FL_PGMSTRING; + + str_lpad: + if (!(flags & FL_LPAD)) { + while (size < width) { + write(' '); + width--; + } + } + while (size) { + write(GETBYTE (flags, FL_PGMSTRING, pnt)); + if (width) width -= 1; + size -= 1; + } + goto tail; + } + } + + /* + * Handle integer formats variations for d/i, u, o, p, x, X. + */ + if (c == 'd' || c == 'i') { + long x = (flags & FL_LONG) ? va_arg(ap,long) : va_arg(ap,int); + flags &= ~(FL_NEGATIVE | FL_ALT); + if (x < 0) { + x = -x; + flags |= FL_NEGATIVE; + } + c = __ultoa_invert (x, (char *)buf, 10) - (char *)buf; + + } else { + int base; + + if (c == 'u') { + flags &= ~FL_ALT; + base = 10; + goto ultoa; + } + + flags &= ~(FL_PLUS | FL_SPACE); + + switch (c) { + case 'o': + base = 8; + goto ultoa; + case 'p': + flags |= FL_ALT; + /* no break */ + case 'x': + if (flags & FL_ALT) + flags |= FL_ALTHEX; + base = 16; + goto ultoa; + case 'X': + if (flags & FL_ALT) + flags |= (FL_ALTHEX | FL_ALTUPP); + base = 16 | XTOA_UPPER; + ultoa: + c = __ultoa_invert ((flags & FL_LONG) + ? va_arg(ap, unsigned long) + : va_arg(ap, unsigned int), + (char *)buf, base) - (char *)buf; + flags &= ~FL_NEGATIVE; + break; + + default: + return; + } + } + + /* + * Format integers. + */ + { + unsigned char len; + + len = c; + if (flags & FL_PREC) { + flags &= ~FL_ZFILL; + if (len < prec) { + len = prec; + if ((flags & FL_ALT) && !(flags & FL_ALTHEX)) + flags &= ~FL_ALT; + } + } + if (flags & FL_ALT) { + if (buf[c-1] == '0') { + flags &= ~(FL_ALT | FL_ALTHEX | FL_ALTUPP); + } else { + len += 1; + if (flags & FL_ALTHEX) + len += 1; + } + } else if (flags & (FL_NEGATIVE | FL_PLUS | FL_SPACE)) { + len += 1; + } + + if (!(flags & FL_LPAD)) { + if (flags & FL_ZFILL) { + prec = c; + if (len < width) { + prec += width - len; + len = width; + } + } + while (len < width) { + write(' '); + len++; + } + } + + width = (len < width) ? width - len : 0; + + if (flags & FL_ALT) { + write('0'); + if (flags & FL_ALTHEX) + write(flags & FL_ALTUPP ? 'X' : 'x'); + } else if (flags & (FL_NEGATIVE | FL_PLUS | FL_SPACE)) { + unsigned char z = ' '; + if (flags & FL_PLUS) z = '+'; + if (flags & FL_NEGATIVE) z = '-'; + write(z); + } + + while (prec > c) { + write('0'); + prec--; + } + + do { + write(buf[--c]); + } while (c); + } + + tail: + /* Tail is possible. */ + while (width) { + write(' '); + width--; + } + } /* for (;;) */ +} diff --git a/libraries/AP_HAL_AVR/utility/xtoa_fast.h b/libraries/AP_HAL_AVR/utility/xtoa_fast.h new file mode 100644 index 0000000000..4a022b05ae --- /dev/null +++ b/libraries/AP_HAL_AVR/utility/xtoa_fast.h @@ -0,0 +1,48 @@ +/* + Adapted from avr-libc: + + Copyright (c) 2005, Dmitry Xmelkov + All rights reserved. + + Redistribution and use in source and binary forms, with or without + modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in + the documentation and/or other materials provided with the + distribution. + * Neither the name of the copyright holders nor the names of + contributors may be used to endorse or promote products derived + from this software without specific prior written permission. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + POSSIBILITY OF SUCH DAMAGE. */ + +/* $Id: xtoa_fast.h 1223 2007-02-18 13:33:09Z dmix $ */ + +#ifndef _XTOA_FAST_H_ +#define _XTOA_FAST_H_ + +#ifndef __ASSEMBLER__ + +/* Internal function for use from `printf'. */ +char * __ultoa_invert (unsigned long val, char *s, int base); + +#endif /* ifndef __ASSEMBLER__ */ + +/* Next flags are to use with `base'. Unused fields are reserved. */ +#define XTOA_PREFIX 0x0100 /* put prefix for octal or hex */ +#define XTOA_UPPER 0x0200 /* use upper case letters */ + +#endif /* _XTOA_FAST_H_ */