diff --git a/flight/Makefile b/flight/Makefile index 324bedf69..07cc11dbe 100644 --- a/flight/Makefile +++ b/flight/Makefile @@ -86,6 +86,7 @@ SRC += $(SYSDIR)/pios_led.c SRC += $(SYSDIR)/pios_uart.c SRC += $(SYSDIR)/pios_irq.c SRC += $(SYSDIR)/pios_com.c +SRC += $(SYSDIR)/printf-stdarg.c ## CMSIS for STM32 SRC += $(CMSISDIR)/core_cm3.c diff --git a/flight/sys/pios.h b/flight/sys/pios.h index 350f134d7..b35e9f285 100644 --- a/flight/sys/pios.h +++ b/flight/sys/pios.h @@ -30,8 +30,10 @@ /* C Lib Includes */ -#include #include +#include +#include +#include /* STM32 Std Perf Lib */ #include diff --git a/flight/sys/pios_com.c b/flight/sys/pios_com.c index c6f52935f..dec44e892 100644 --- a/flight/sys/pios_com.c +++ b/flight/sys/pios_com.c @@ -55,20 +55,6 @@ int32_t COMInit(void) return -ret; } - -/** -* This function checks the availability of a COM port -* \param[in] port COM port (COM_GPS_UART, COM_TELEM_UART, COM_AUX_UART) -* \return 1: port available -* \return 0: port not available -* \note Deprecated since our hardware is a constant -*/ -int32_t COMCheckAvailable(COMPortTypeDef port) -{ - /* Deprecated since our hardware is a constant */ -} - - /** * Sends a package over given port * \param[in] port COM port (COM_GPS_UART, COM_TELEM_UART, COM_AUX_UART) @@ -165,7 +151,7 @@ int32_t COMSendChar(COMPortTypeDef port, char c) * caller should retry until buffer is free again * \return 0 on success */ -int32_t COMSendString_NonBlocking(COMPortTypeDef port, char *str) +int32_t COMSendStringNonBlocking(COMPortTypeDef port, char *str) { return COMSendBufferNonBlocking(port, (uint8_t *)str, (uint16_t)strlen(str)); } @@ -179,9 +165,9 @@ int32_t COMSendString_NonBlocking(COMPortTypeDef port, char *str) * \return -1 if port not available * \return 0 on success */ -int32_t MIOS32_COM_SendString(COMPortTypeDef port, char *str) +int32_t COMSendString(COMPortTypeDef port, char *str) { - /* return COMSendBuffer(port, (uint8_t *)str, strlen(str)); */ + return COMSendBuffer(port, (uint8_t *)str, strlen(str)); } @@ -195,16 +181,15 @@ int32_t MIOS32_COM_SendString(COMPortTypeDef port, char *str) * caller should retry until buffer is free again * \return 0 on success */ -int32_t MIOS32_COM_SendFormattedStringNonBlocking(COMPortTypeDef port, char *format, ...) +int32_t COMSendFormattedStringNonBlocking(COMPortTypeDef port, char *format, ...) { uint8_t buffer[128]; // TODO: tmp!!! Provide a streamed COM method later! - /* + va_list args; va_start(args, format); vsprintf((char *)buffer, format, args); return COMSendBufferNonBlocking(port, buffer, (uint16_t)strlen((char *)buffer)); - */ } @@ -217,16 +202,14 @@ int32_t MIOS32_COM_SendFormattedStringNonBlocking(COMPortTypeDef port, char *for * \return -1 if port not available * \return 0 on success */ -int32_t MIOS32_COM_SendFormattedString(COMPortTypeDef port, char *format, ...) +int32_t COMSendFormattedString(COMPortTypeDef port, char *format, ...) { uint8_t buffer[128]; // TODO: tmp!!! Provide a streamed COM method later! - /* va_list args; va_start(args, format); vsprintf((char *)buffer, format, args); return COMSendBuffer(port, buffer, (uint16_t)strlen((char *)buffer)); - */ } diff --git a/flight/sys/pios_com.h b/flight/sys/pios_com.h index ec0cbba38..c07d24ec3 100644 --- a/flight/sys/pios_com.h +++ b/flight/sys/pios_com.h @@ -37,11 +37,9 @@ typedef enum { /* Public Functions */ extern int32_t COMInit(void); -extern int32_t COMCheckAvailable(COMPortTypeDef port); - -extern int32_t COMSendChar_NonBlocking(COMPortTypeDef port, char c); +extern int32_t COMSendCharNonBlocking(COMPortTypeDef port, char c); extern int32_t COMSendChar(COMPortTypeDef port, char c); -extern int32_t COMSendBuffer_NonBlocking(COMPortTypeDef port, uint8_t *buffer, uint16_t len); +extern int32_t COMSendBufferNonBlocking(COMPortTypeDef port, uint8_t *buffer, uint16_t len); extern int32_t COMSendBuffer(COMPortTypeDef port, uint8_t *buffer, uint16_t len); extern int32_t COMSendStringNonBlocking(COMPortTypeDef port, char *str); extern int32_t COMSendString(COMPortTypeDef port, char *str); @@ -49,6 +47,6 @@ extern int32_t COMSendFormattedStringNonBlocking(COMPortTypeDef port, char *form extern int32_t COMSendFormattedString(COMPortTypeDef port, char *format, ...); extern int32_t COMReceiveHandler(void); -extern int32_t COMReceiveCallback_Init(void *callback_receive); +extern int32_t COMReceiveCallbackInit(void *callback_receive); #endif /* PIOS_COM_H */ diff --git a/flight/sys/pios_uart.c b/flight/sys/pios_uart.c index 04831bff2..a8cee564a 100644 --- a/flight/sys/pios_uart.c +++ b/flight/sys/pios_uart.c @@ -505,9 +505,7 @@ GPS_IRQHANDLER_FUNC if(GPS_UART->SR & (1 << 5)) { uint8_t b = GPS_UART->DR; - int status = 0;//MIOS32_MIDI_SendByteToRxCallback(UART0, b); - - if(status == 0 && UARTRxBufferPut(0, b) < 0) { + if(UARTRxBufferPut(0, b) < 0) { /* Here we could add some error handling */ } } @@ -536,9 +534,7 @@ TELEM_IRQHANDLER_FUNC if(TELEM_UART->SR & (1 << 5)) { uint8_t b = TELEM_UART->DR; - int status = 0;//MIOS32_MIDI_SendByteToRxCallback(UART1, b); - - if(status == 0 && UARTRxBufferPut(1, b) < 0) { + if(UARTRxBufferPut(1, b) < 0) { /* Here we could add some error handling */ } } @@ -566,9 +562,7 @@ AUX_UART_IRQHANDLER_FUNC if(AUX_UART_UART->SR & (1 << 5)) { uint8_t b = AUX_UART_UART->DR; - int status = 0;//MIOS32_MIDI_SendByteToRxCallback(UART1, b); - - if(status == 0 && UARTRxBufferPut(1, b) < 0) { + if(UARTRxBufferPut(1, b) < 0) { /* Here we could add some error handling */ } } diff --git a/flight/sys/printf-stdarg.c b/flight/sys/printf-stdarg.c new file mode 100644 index 000000000..43f149512 --- /dev/null +++ b/flight/sys/printf-stdarg.c @@ -0,0 +1,228 @@ +// $Id: printf-stdarg.c 398 2009-03-22 21:47:53Z tk $ +/* + Copyright 2001, 2002 Georges Menie (www.menie.org) + stdarg version contributed by Christian Ettinger + + This program 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 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public License + along with this program; if not, write to the Free Software + Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA +*/ + +/* + putchar is the only external dependency for this file, + if you have a working putchar, leave it commented out. + If not, uncomment the define below and + replace outbyte(c) by your own function call. + +#define putchar(c) outbyte(c) +*/ +//#define putchar(c) COMSendChar(c) + +#include + +static void printchar(char **str, int c) +{ + // extern int putchar(int c); + + if (str) { + **str = c; + ++(*str); + } + + else COMSendChar(1, c); // (void)putchar(c); + +} + +#define PAD_RIGHT 1 +#define PAD_ZERO 2 + +static int prints(char **out, const char *string, int width, int pad) +{ + register int pc = 0, padchar = ' '; + + if (width > 0) { + register int len = 0; + register const char *ptr; + for (ptr = string; *ptr; ++ptr) ++len; + if (len >= width) width = 0; + else width -= len; + if (pad & PAD_ZERO) padchar = '0'; + } + if (!(pad & PAD_RIGHT)) { + for ( ; width > 0; --width) { + printchar (out, padchar); + ++pc; + } + } + for ( ; *string ; ++string) { + printchar (out, *string); + ++pc; + } + for ( ; width > 0; --width) { + printchar (out, padchar); + ++pc; + } + + return pc; +} + +/* the following should be enough for 32 bit int */ +#define PRINT_BUF_LEN 12 + +static int printi(char **out, int i, int b, int sg, int width, int pad, int letbase) +{ + char print_buf[PRINT_BUF_LEN]; + register char *s; + register int t, neg = 0, pc = 0; + register unsigned int u = i; + + if (i == 0) { + print_buf[0] = '0'; + print_buf[1] = '\0'; + return prints (out, print_buf, width, pad); + } + + if (sg && b == 10 && i < 0) { + neg = 1; + u = -i; + } + + s = print_buf + PRINT_BUF_LEN-1; + *s = '\0'; + + while (u) { + t = u % b; + if( t >= 10 ) + t += letbase - '0' - 10; + *--s = t + '0'; + u /= b; + } + + if (neg) { + if( width && (pad & PAD_ZERO) ) { + printchar (out, '-'); + ++pc; + --width; + } + else { + *--s = '-'; + } + } + + return pc + prints (out, s, width, pad); +} + +static int print( char **out, const char *format, va_list args ) +{ + register int width, pad; + register int pc = 0; + char scr[2]; + + for (; *format != 0; ++format) { + if (*format == '%') { + ++format; + width = pad = 0; + if (*format == '\0') break; + if (*format == '%') goto out; + if (*format == '-') { + ++format; + pad = PAD_RIGHT; + } + while (*format == '0') { + ++format; + pad |= PAD_ZERO; + } + for ( ; *format >= '0' && *format <= '9'; ++format) { + width *= 10; + width += *format - '0'; + } + if( *format == 's' ) { + register char *s = (char *)va_arg( args, int ); + pc += prints (out, s?s:"(null)", width, pad); + continue; + } + if( *format == 'd' ) { + pc += printi (out, va_arg( args, int ), 10, 1, width, pad, 'a'); + continue; + } + if( *format == 'x' ) { + pc += printi (out, va_arg( args, int ), 16, 0, width, pad, 'a'); + continue; + } + if( *format == 'X' ) { + pc += printi (out, va_arg( args, int ), 16, 0, width, pad, 'A'); + continue; + } + if( *format == 'u' ) { + pc += printi (out, va_arg( args, int ), 10, 0, width, pad, 'a'); + continue; + } + if( *format == 'c' ) { + /* char are converted to int then pushed on the stack */ + scr[0] = (char)va_arg( args, int ); + scr[1] = '\0'; + pc += prints (out, scr, width, pad); + continue; + } + } + else { + out: + printchar (out, *format); + ++pc; + } + } + if (out) **out = '\0'; + va_end( args ); + return pc; +} + +int printf(const char *format, ...) +{ + va_list args; + + va_start( args, format ); + return print( 0, format, args ); +} + +// TK: added for alternative parameter passing +int vprintf(const char *format, va_list args) +{ + return print( 0, format, args ); +} + +int sprintf(char *out, const char *format, ...) +{ + va_list args; + + va_start( args, format ); + return print( &out, format, args ); +} + +// TK: added for alternative parameter passing +int vsprintf(char *out, const char *format, va_list args) +{ + char *_out; + _out = out; + return print( &_out, format, args ); +} + + +int snprintf( char *buf, size_t count, const char *format, ... ) +{ + va_list args; + + ( void ) count; + + va_start( args, format ); + return print( &buf, format, args ); +}