mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-12-01 09:24:10 +01:00
a2d8544931
to make the flight code compile again. Needs careful review, particularly all the fixes for the signed vs unsigned comparisons. +review OPReview-459
634 lines
18 KiB
C
634 lines
18 KiB
C
/*******************************************************************************
|
||
Copyright 2001, 2002 Georges Menie (<URL snipped>)
|
||
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, just remove the following
|
||
define. If the function should be called something else,
|
||
replace outbyte(c) by your own function call.
|
||
*/
|
||
//*******************************************************************************
|
||
// Updated by Daniel D Miller. Changes to the original Menie code are
|
||
// Copyright 2009-2012 Daniel D Miller
|
||
// All such changes are distributed under the same license as the original,
|
||
// as described above.
|
||
// 11/06/09 - adding floating-point support
|
||
// 03/19/10 - pad fractional portion of floating-point number with 0s
|
||
// 03/30/10 - document the \% bug
|
||
// 07/20/10 - Fix a round-off bug in floating-point conversions
|
||
// ( 0.99 with %.1f did not round to 1.0 )
|
||
// 10/25/11 - Add support for %+ format (always show + on positive numbers)
|
||
// 01/19/12 - fix handling of %f with no decimal; it was defaulting to 0
|
||
// decimal places, rather than printf's 6.
|
||
//*******************************************************************************
|
||
// BUGS
|
||
// If '%' is included in a format string, in the form \% with the intent
|
||
// of including the percent sign in the output string, this function will
|
||
// mis-handle the data entirely!!
|
||
// Essentially, it will just discard the character following the percent sign.
|
||
// This bug is not easy to fix in the existing code;
|
||
// for now, I'll just try to remember to use %% instead of \% ...
|
||
//*******************************************************************************
|
||
|
||
//lint -esym(752, debug_output)
|
||
//lint -esym(766, stdio.h)
|
||
|
||
// #define TEST_PRINTF 1
|
||
|
||
#include <pios.h>
|
||
|
||
static uint use_leading_plus = 0 ;
|
||
|
||
/* based on a example-code from Keil for CS G++ */
|
||
|
||
/* for caddr_t (typedef char * caddr_t;) */
|
||
#include <sys/types.h>
|
||
|
||
//* NEWLIB STUBS *//
|
||
#include <stdlib.h>
|
||
#include <sys/unistd.h>
|
||
#include <sys/stat.h>
|
||
#include <sys/times.h>
|
||
#include <errno.h>
|
||
|
||
/*==============================================================================
|
||
* Environment variables.
|
||
* A pointer to a list of environment variables and their values. For a minimal
|
||
* environment, this empty list is adequate:
|
||
*/
|
||
char *__env[1] = { 0 };
|
||
char **environ = __env;
|
||
|
||
/*==============================================================================
|
||
* Close a file.
|
||
*/
|
||
int _close(__attribute__((unused)) int file)
|
||
{
|
||
return -1;
|
||
}
|
||
|
||
/*==============================================================================
|
||
* Transfer control to a new process.
|
||
*/
|
||
int _execve(__attribute__((unused)) char *name, __attribute__((unused)) char **argv, __attribute__((unused)) char **env)
|
||
{
|
||
errno = ENOMEM;
|
||
return -1;
|
||
}
|
||
|
||
/*==============================================================================
|
||
* Exit a program without cleaning up files.
|
||
*/
|
||
void _exit( __attribute__((unused)) int code )
|
||
{
|
||
/* Should we force a system reset? */
|
||
while( 1 )
|
||
{
|
||
;
|
||
}
|
||
}
|
||
|
||
/*==============================================================================
|
||
* Create a new process.
|
||
*/
|
||
int _fork(void)
|
||
{
|
||
errno = EAGAIN;
|
||
return -1;
|
||
}
|
||
|
||
/*==============================================================================
|
||
* Status of an open file.
|
||
*/
|
||
int _fstat(__attribute__((unused)) int file, struct stat *st)
|
||
{
|
||
st->st_mode = S_IFCHR;
|
||
return 0;
|
||
}
|
||
|
||
/*==============================================================================
|
||
* Process-ID
|
||
*/
|
||
int _getpid(void)
|
||
{
|
||
return 1;
|
||
}
|
||
|
||
/*==============================================================================
|
||
* Query whether output stream is a terminal.
|
||
*/
|
||
int _isatty(__attribute__((unused)) int file)
|
||
{
|
||
return 1;
|
||
}
|
||
|
||
/*==============================================================================
|
||
* Send a signal.
|
||
*/
|
||
int _kill(__attribute__((unused)) int pid, __attribute__((unused)) int sig)
|
||
{
|
||
errno = EINVAL;
|
||
return -1;
|
||
}
|
||
|
||
/*==============================================================================
|
||
* Establish a new name for an existing file.
|
||
*/
|
||
int _link(__attribute__((unused)) char *old, __attribute__((unused)) char *new)
|
||
{
|
||
errno = EMLINK;
|
||
return -1;
|
||
}
|
||
|
||
/*==============================================================================
|
||
* Set position in a file.
|
||
*/
|
||
int _lseek(__attribute__((unused)) int file, __attribute__((unused)) int ptr, __attribute__((unused)) int dir)
|
||
{
|
||
return 0;
|
||
}
|
||
|
||
/*==============================================================================
|
||
* Open a file.
|
||
*/
|
||
int _open(__attribute__((unused)) const char *name, __attribute__((unused)) int flags, __attribute__((unused)) int mode)
|
||
{
|
||
return -1;
|
||
}
|
||
|
||
/*==============================================================================
|
||
* Read from a file.
|
||
*/
|
||
int _read(__attribute__((unused)) int file, __attribute__((unused)) char *ptr, __attribute__((unused)) int len)
|
||
{
|
||
return 0;
|
||
}
|
||
|
||
/*==============================================================================
|
||
* Write to a file. libc subroutines will use this system routine for output to
|
||
* all files, including stdout<75>so if you need to generate any output, for
|
||
* example to a serial port for debugging, you should make your minimal write
|
||
* capable of doing this.
|
||
*/
|
||
int _write_r( __attribute__((unused)) void * reent, __attribute__((unused)) int file, __attribute__((unused)) char * ptr, __attribute__((unused)) int len )
|
||
{
|
||
return 0;
|
||
}
|
||
|
||
/*==============================================================================
|
||
* Increase program data space. As malloc and related functions depend on this,
|
||
* it is useful to have a working implementation. The following suffices for a
|
||
* standalone system; it exploits the symbol _end automatically defined by the
|
||
* GNU linker.
|
||
*/
|
||
caddr_t _sbrk(int incr)
|
||
{
|
||
extern char _end; /* Defined by the linker */
|
||
static char *heap_end;
|
||
char *prev_heap_end;
|
||
char * stack_ptr;
|
||
|
||
if (heap_end == 0)
|
||
{
|
||
heap_end = &_end;
|
||
}
|
||
|
||
prev_heap_end = heap_end;
|
||
asm volatile ("MRS %0, msp" : "=r" (stack_ptr) );
|
||
if (heap_end + incr > stack_ptr)
|
||
{
|
||
_write_r ((void *)0, 1, "Heap and stack collision\n", 25);
|
||
_exit (1);
|
||
}
|
||
|
||
heap_end += incr;
|
||
return (caddr_t) prev_heap_end;
|
||
}
|
||
|
||
/*==============================================================================
|
||
* Status of a file (by name).
|
||
*/
|
||
int _stat(__attribute__((unused)) char *file, struct stat *st)
|
||
{
|
||
st->st_mode = S_IFCHR;
|
||
return 0;
|
||
}
|
||
|
||
/*==============================================================================
|
||
* Timing information for current process.
|
||
*/
|
||
int _times(__attribute__((unused)) struct tms *buf)
|
||
{
|
||
return -1;
|
||
}
|
||
|
||
/*==============================================================================
|
||
* Remove a file's directory entry.
|
||
*/
|
||
int _unlink(__attribute__((unused)) char *name)
|
||
{
|
||
errno = ENOENT;
|
||
return -1;
|
||
}
|
||
|
||
/*==============================================================================
|
||
* Wait for a child process.
|
||
*/
|
||
int _wait(__attribute__((unused)) int *status)
|
||
{
|
||
errno = ECHILD;
|
||
return -1;
|
||
}
|
||
//* NEWLIB STUBS *//
|
||
|
||
|
||
//****************************************************************************
|
||
static void printchar (char **str, int c)
|
||
{
|
||
if (str) {
|
||
**str = c;
|
||
++(*str);
|
||
}
|
||
#ifdef TEST_PRINTF
|
||
else {
|
||
extern int putchar (int c);
|
||
(void) putchar (c);
|
||
}
|
||
#endif
|
||
}
|
||
|
||
//****************************************************************************
|
||
static uint my_strlen(char *str)
|
||
{
|
||
if (str == 0)
|
||
return 0;
|
||
uint slen = 0 ;
|
||
while (*str != 0) {
|
||
slen++ ;
|
||
str++ ;
|
||
}
|
||
return slen;
|
||
}
|
||
|
||
//****************************************************************************
|
||
// This version returns the length of the output string.
|
||
// It is more useful when implementing a walking-string function.
|
||
//****************************************************************************
|
||
static const double round_nums[8] = {
|
||
0.5L,
|
||
0.05L,
|
||
0.005L,
|
||
0.0005L,
|
||
0.00005L,
|
||
0.000005L,
|
||
0.0000005L,
|
||
0.00000005L
|
||
} ;
|
||
|
||
static unsigned dbl2stri(char *outbfr, double dbl, unsigned dec_digits)
|
||
{
|
||
static char local_bfr[128] ;
|
||
char *output = (outbfr == 0) ? local_bfr : outbfr ;
|
||
|
||
//*******************************************
|
||
// extract negative info
|
||
//*******************************************
|
||
if (dbl < 0.0L) {
|
||
*output++ = '-' ;
|
||
dbl *= -1.0L ;
|
||
} else {
|
||
if (use_leading_plus) {
|
||
*output++ = '+' ;
|
||
}
|
||
|
||
}
|
||
|
||
// handling rounding by adding .5LSB to the floating-point data
|
||
if (dec_digits < 8) {
|
||
dbl += round_nums[dec_digits] ;
|
||
}
|
||
|
||
//**************************************************************************
|
||
// construct fractional multiplier for specified number of digits.
|
||
//**************************************************************************
|
||
uint mult = 1 ;
|
||
uint idx ;
|
||
for (idx=0; idx < dec_digits; idx++)
|
||
mult *= 10 ;
|
||
|
||
// printf("mult=%u\n", mult) ;
|
||
uint wholeNum = (uint) dbl ;
|
||
uint decimalNum = (uint) ((dbl - wholeNum) * mult);
|
||
|
||
//*******************************************
|
||
// convert integer portion
|
||
//*******************************************
|
||
char tbfr[40] ;
|
||
idx = 0 ;
|
||
while (wholeNum != 0) {
|
||
tbfr[idx++] = '0' + (wholeNum % 10) ;
|
||
wholeNum /= 10 ;
|
||
}
|
||
// printf("%.3f: whole=%s, dec=%d\n", dbl, tbfr, decimalNum) ;
|
||
if (idx == 0) {
|
||
*output++ = '0' ;
|
||
} else {
|
||
while (idx > 0) {
|
||
*output++ = tbfr[idx-1] ; //lint !e771
|
||
idx-- ;
|
||
}
|
||
}
|
||
if (dec_digits > 0) {
|
||
*output++ = '.' ;
|
||
|
||
//*******************************************
|
||
// convert fractional portion
|
||
//*******************************************
|
||
idx = 0 ;
|
||
while (decimalNum != 0) {
|
||
tbfr[idx++] = '0' + (decimalNum % 10) ;
|
||
decimalNum /= 10 ;
|
||
}
|
||
// pad the decimal portion with 0s as necessary;
|
||
// We wouldn't want to report 3.093 as 3.93, would we??
|
||
while (idx < dec_digits) {
|
||
tbfr[idx++] = '0' ;
|
||
}
|
||
// printf("decimal=%s\n", tbfr) ;
|
||
if (idx == 0) {
|
||
*output++ = '0' ;
|
||
} else {
|
||
while (idx > 0) {
|
||
*output++ = tbfr[idx-1] ;
|
||
idx-- ;
|
||
}
|
||
}
|
||
}
|
||
*output = 0 ;
|
||
|
||
// prepare output
|
||
output = (outbfr == 0) ? local_bfr : outbfr ;
|
||
return my_strlen(output) ;
|
||
}
|
||
|
||
//****************************************************************************
|
||
#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) {
|
||
int len = 0;
|
||
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];
|
||
char *s;
|
||
int t, neg = 0, pc = 0;
|
||
unsigned u = (unsigned) 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 = (unsigned) -i;
|
||
}
|
||
// make sure print_buf is NULL-term
|
||
s = print_buf + PRINT_BUF_LEN - 1;
|
||
*s = '\0';
|
||
|
||
|
||
while (u) {
|
||
t = u % b; //lint !e573 Warning 573: Signed-unsigned mix with divide
|
||
if (t >= 10)
|
||
t += letbase - '0' - 10;
|
||
*--s = t + '0';
|
||
u /= b; //lint !e573 Warning 573: Signed-unsigned mix with divide
|
||
}
|
||
if (neg) {
|
||
if (width && (pad & PAD_ZERO)) {
|
||
printchar (out, '-');
|
||
++pc;
|
||
--width;
|
||
}
|
||
else {
|
||
*--s = '-';
|
||
}
|
||
} else {
|
||
if (use_leading_plus) {
|
||
*--s = '+';
|
||
}
|
||
}
|
||
return pc + prints (out, s, width, pad);
|
||
}
|
||
|
||
//****************************************************************************
|
||
static int print (char **out, int *varg)
|
||
{
|
||
int post_decimal ;
|
||
int width, pad ;
|
||
unsigned dec_width = 6 ;
|
||
int pc = 0;
|
||
char *format = (char *) (*varg++);
|
||
char scr[2];
|
||
use_leading_plus = 0 ; // start out with this clear
|
||
for (; *format != 0; ++format) {
|
||
if (*format == '%') {
|
||
dec_width = 6 ;
|
||
++format;
|
||
width = pad = 0;
|
||
if (*format == '\0')
|
||
break;
|
||
if (*format == '%')
|
||
goto out_lbl;
|
||
if (*format == '-') {
|
||
++format;
|
||
pad = PAD_RIGHT;
|
||
}
|
||
if (*format == '+') {
|
||
++format;
|
||
use_leading_plus = 1 ;
|
||
}
|
||
while (*format == '0') {
|
||
++format;
|
||
pad |= PAD_ZERO;
|
||
}
|
||
post_decimal = 0 ;
|
||
if (*format == '.' ||
|
||
(*format >= '0' && *format <= '9')) {
|
||
|
||
while (1) {
|
||
if (*format == '.') {
|
||
post_decimal = 1 ;
|
||
dec_width = 0 ;
|
||
format++ ;
|
||
} else if ((*format >= '0' && *format <= '9')) {
|
||
if (post_decimal) {
|
||
dec_width *= 10;
|
||
dec_width += *format - '0';
|
||
} else {
|
||
width *= 10;
|
||
width += *format - '0';
|
||
}
|
||
format++ ;
|
||
} else {
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
if (*format == 'l')
|
||
++format;
|
||
switch (*format) {
|
||
case 's':
|
||
{
|
||
// char *s = *((char **) varg++); //lint !e740
|
||
char *s = (char *) *varg++ ; //lint !e740 !e826 convert to double pointer
|
||
pc += prints (out, s ? s : "(null)", width, pad);
|
||
use_leading_plus = 0 ; // reset this flag after printing one value
|
||
}
|
||
break;
|
||
case 'd':
|
||
pc += printi (out, *varg++, 10, 1, width, pad, 'a');
|
||
use_leading_plus = 0 ; // reset this flag after printing one value
|
||
break;
|
||
case 'x':
|
||
pc += printi (out, *varg++, 16, 0, width, pad, 'a');
|
||
use_leading_plus = 0 ; // reset this flag after printing one value
|
||
break;
|
||
case 'X':
|
||
pc += printi (out, *varg++, 16, 0, width, pad, 'A');
|
||
use_leading_plus = 0 ; // reset this flag after printing one value
|
||
break;
|
||
case 'u':
|
||
pc += printi (out, *varg++, 10, 0, width, pad, 'a');
|
||
use_leading_plus = 0 ; // reset this flag after printing one value
|
||
break;
|
||
case 'c':
|
||
/* char are converted to int then pushed on the stack */
|
||
scr[0] = *varg++;
|
||
scr[1] = '\0';
|
||
pc += prints (out, scr, width, pad);
|
||
use_leading_plus = 0 ; // reset this flag after printing one value
|
||
break;
|
||
|
||
case 'f':
|
||
{
|
||
// http://wiki.debian.org/ArmEabiPort#Structpackingandalignment
|
||
// Stack alignment
|
||
//
|
||
// The ARM EABI requires 8-byte stack alignment at public function entry points,
|
||
// compared to the previous 4-byte alignment.
|
||
#ifdef USE_NEWLIB
|
||
char *cptr = (char *) varg ; //lint !e740 !e826 convert to double pointer
|
||
uint caddr = (uint) cptr ;
|
||
if ((caddr & 0xF) != 0) {
|
||
cptr += 4 ;
|
||
}
|
||
double *dblptr = (double *) cptr ; //lint !e740 !e826 convert to double pointer
|
||
#else
|
||
double *dblptr = (double *) varg ; //lint !e740 !e826 convert to double pointer
|
||
#endif
|
||
double dbl = *dblptr++ ; // increment double pointer
|
||
varg = (int *) dblptr ; //lint !e740 copy updated pointer back to base pointer
|
||
char bfr[81] ;
|
||
// unsigned slen =
|
||
dbl2stri(bfr, dbl, dec_width) ;
|
||
// stuff_talkf("[%s], width=%u, dec_width=%u\n", bfr, width, dec_width) ;
|
||
pc += prints (out, bfr, width, pad);
|
||
use_leading_plus = 0 ; // reset this flag after printing one value
|
||
}
|
||
break;
|
||
|
||
default:
|
||
printchar (out, '%');
|
||
printchar (out, *format);
|
||
use_leading_plus = 0 ; // reset this flag after printing one value
|
||
break;
|
||
}
|
||
} else
|
||
// if (*format == '\\') {
|
||
//
|
||
// } else
|
||
{
|
||
out_lbl:
|
||
printchar (out, *format);
|
||
++pc;
|
||
}
|
||
} // for each char in format string
|
||
if (out) //lint !e850
|
||
**out = '\0';
|
||
return pc;
|
||
}
|
||
|
||
//****************************************************************************
|
||
int stringf (char *out, const char *format, ...)
|
||
{
|
||
// create a pointer into the stack.
|
||
// Thematically this should be a void*, since the actual usage of the
|
||
// pointer will vary. However, int* works fine too.
|
||
// Either way, the called function will need to re-cast the pointer
|
||
// for any type which isn't sizeof(int)
|
||
int *varg = (int *) (char *) (&format);
|
||
return print (&out, varg);
|
||
}
|
||
|
||
int printf(const char *format, ...)
|
||
{
|
||
int *varg = (int *) (char *) (&format);
|
||
return print (0, varg);
|
||
}
|
||
|
||
int sprintf(char *out, const char *format, ...)
|
||
{
|
||
int *varg = (int *) (char *) (&format);
|
||
return print (&out, varg);
|
||
}
|
||
|
||
/**
|
||
* @}
|
||
*/
|