Subversion Repositories FlightCtrl

Rev

Go to most recent revision | Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
2248 - 1
//----------------------------------------------------------------------------------------------------------------------------------
2
// Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist nicht von der Lizenz für den MikroKopter-Teil unterstellt
3
 
4
/*
5
Copyright (C) 1993 Free Software Foundation
6
 
7
This file is part of the GNU IO Library.  This library is free
8
software; you can redistribute it and/or modify it under the
9
terms of the GNU General Public License as published by the
10
Free Software Foundation; either version 2, or (at your option)
11
any later version.
12
 
13
This library is distributed in the hope that it will be useful,
14
but WITHOUT ANY WARRANTY; without even the implied warranty of
15
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
16
GNU General Public License for more details.
17
 
18
You should have received a copy of the GNU General Public License
19
along with this library; see the file COPYING.  If not, write to the Free
20
Software Foundation, 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
21
 
22
As a special exception, if you link this library with files
23
compiled with a GNU compiler to produce an executable, this does not cause
24
the resulting executable to be covered by the GNU General Public License.
25
This exception does not however invalidate any other reasons why
26
the executable file might be covered by the GNU General Public License. */
27
 
28
/*
29
 * Copyright (c) 1990 Regents of the University of California.
30
 * All rights reserved.
31
 *
32
 * Redistribution and use in source and binary forms, with or without
33
 * modification, are permitted provided that the following conditions
34
 * are met:
35
 * 1. Redistributions of source code must retain the above copyright
36
 *    notice, this list of conditions and the following disclaimer.
37
 * 2. Redistributions in binary form must reproduce the above copyright
38
 *    notice, this list of conditions and the following disclaimer in the
39
 *    documentation and/or other materials provided with the distribution.
40
 * 3. [rescinded 22 July 1999]
41
 * 4. Neither the name of the University nor the names of its contributors
42
 *    may be used to endorse or promote products derived from this software
43
 *    without specific prior written permission.
44
 *
45
 * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND
46
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
47
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
48
 * ARE DISCLAIMED.  IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE
49
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
50
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
51
 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
52
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
53
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
54
 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
55
 * SUCH DAMAGE.
56
 */
57
 
58
/******************************************************************************
59
 This file is a patched version of printf called _printf_P
60
 It is made to work with avr-gcc for Atmel AVR MCUs.
61
 There are some differences from standard printf:
62
        1. There is no floating point support (with fp the code is about 8K!)
63
        2. Return type is void
64
        3. Format string must be in program memory (by using macro printf this is
65
           done automaticaly)
66
        4. %n is not implemented (just remove the comment around it if you need it)
67
        5. If LIGHTPRINTF is defined, the code is about 550 bytes smaller and the
68
           folowing specifiers are disabled :
69
                space # * . - + p s o O
70
        6. A function void uart_sendchar(char c) is used for output. The UART must
71
                be initialized before using printf.
72
 
73
 Alexander Popov
74
 sasho@vip.orbitel.bg
75
******************************************************************************/
76
 
77
/*
78
 * Actual printf innards.
79
 *
80
 * This code is large and complicated...
81
 */
82
 
83
#include <string.h>
84
#ifdef __STDC__
85
#include <stdarg.h>
86
#else
87
#include <varargs.h>
88
#endif
89
 
90
#include "main.h"
91
 
92
 
93
//#define LIGHTPRINTF
94
char PrintZiel;
95
 
96
 
97
char Putchar(char zeichen)
98
{
99
 if(PrintZiel == OUT_LCD) { DisplayBuff[DispPtr++] = zeichen; return(1);}
100
 else                       return(uart_putchar(zeichen));
101
}
102
 
103
 
104
void PRINT(const char * ptr, unsigned int len)
105
{
106
 for(;len;len--) Putchar(*ptr++);
107
}
108
 
109
void PRINTP(const char * ptr, unsigned int len)
110
{
111
 for(;len;len--) Putchar(pgm_read_byte(ptr++));
112
}
113
 
114
void PAD_SP(signed char howmany)
115
{
116
        for(;howmany>0;howmany--) Putchar(' ');
117
}
118
 
119
void PAD_0(signed char howmany)
120
{
121
        for(;howmany>0;howmany--) Putchar('0');
122
}
123
 
124
#define BUF             40
125
 
126
/*
127
 * Macros for converting digits to letters and vice versa
128
 */
129
#define to_digit(c)     ((c) - '0')
130
#define  is_digit(c)    ((c)<='9' && (c)>='0')
131
#define to_char(n)      ((n) + '0')
132
 
133
/*
134
 * Flags used during conversion.
135
 */
136
#define LONGINT         0x01            /* long integer */
137
#define LONGDBL         0x02            /* long double; unimplemented */
138
#define SHORTINT                0x04            /* short integer */
139
#define ALT                     0x08            /* alternate form */
140
#define LADJUST         0x10            /* left adjustment */
141
#define ZEROPAD         0x20            /* zero (as opposed to blank) pad */
142
#define HEXPREFIX       0x40            /* add 0x or 0X prefix */
143
 
144
void _printf_P (char ziel,char const *fmt0, ...)      /* Works with string from FLASH */
145
{
146
        va_list ap;
147
        register const char *fmt; /* format string */
148
        register char ch;       /* character from fmt */
149
        register int n;         /* handy integer (short term usage) */
150
        register char *cp;      /* handy char pointer (short term usage) */
151
        const char *fmark;      /* for remembering a place in fmt */
152
        register unsigned char flags;   /* flags as above */
153
        signed char width;              /* width from format (%8d), or 0 */
154
        signed char prec;               /* precision from format (%.3d), or -1 */
155
        char sign;                              /* sign prefix (' ', '+', '-', or \0) */
156
        unsigned long _ulong=0; /* integer arguments %[diouxX] */
157
#define OCT 8
158
#define DEC 10
159
#define HEX 16
160
        unsigned char base;             /* base for [diouxX] conversion */
161
        signed char dprec;              /* a copy of prec if [diouxX], 0 otherwise */
162
        signed char dpad;                       /* extra 0 padding needed for integers */
163
        signed char fieldsz;            /* field size expanded by sign, dpad etc */
164
        /* The initialization of 'size' is to suppress a warning that
165
           'size' might be used unitialized.  It seems gcc can't
166
           quite grok this spaghetti code ... */
167
        signed char size = 0;           /* size of converted field or string */
168
        char buf[BUF];          /* space for %c, %[diouxX], %[eEfgG] */
169
        char ox[2];                     /* space for 0x hex-prefix */
170
 
171
    PrintZiel = ziel;  // bestimmt, LCD oder UART
172
        va_start(ap, fmt0);
173
 
174
        fmt = fmt0;
175
 
176
        /*
177
         * Scan the format for conversions (`%' character).
178
         */
179
        for (;;) {
180
                for (fmark = fmt; (ch = pgm_read_byte(fmt)) != '\0' && ch != '%'; fmt++)
181
                        /* void */;
182
                if ((n = fmt - fmark) != 0) {
183
                        PRINTP(fmark, n);
184
                }
185
                if (ch == '\0')
186
                        goto done;
187
                fmt++;          /* skip over '%' */
188
 
189
                flags = 0;
190
                dprec = 0;
191
                width = 0;
192
                prec = -1;
193
                sign = '\0';
194
 
195
rflag:          ch = PRG_RDB(fmt++);
196
reswitch:
197
#ifdef LIGHTPRINTF
198
        if (ch=='o' || ch=='u' || (ch|0x20)=='x') {
199
#else
200
        if (ch=='u' || (ch|0x20)=='x') {
201
#endif
202
                if (flags&LONGINT) {
203
                        _ulong=va_arg(ap, unsigned long);
204
                } else {
205
                        register unsigned int _d;
206
                        _d=va_arg(ap, unsigned int);
207
                        _ulong = flags&SHORTINT ? (unsigned long)(unsigned short)_d : (unsigned long)_d;
208
                }
209
        }
210
 
211
#ifndef LIGHTPRINTF
212
                if(ch==' ') {
213
                        /*
214
                         * ``If the space and + flags both appear, the space
215
                         * flag will be ignored.''
216
                         *      -- ANSI X3J11
217
                         */
218
                        if (!sign)
219
                                sign = ' ';
220
                        goto rflag;
221
                } else if (ch=='#') {
222
                        flags |= ALT;
223
                        goto rflag;
224
                } else if (ch=='*'||ch=='-') {
225
                        if (ch=='*') {
226
                                /*
227
                                 * ``A negative field width argument is taken as a
228
                                 * - flag followed by a positive field width.''
229
                                 *      -- ANSI X3J11
230
                                 * They don't exclude field widths read from args.
231
                                 */
232
                                if ((width = va_arg(ap, int)) >= 0)
233
                                        goto rflag;
234
                                width = -width;
235
                        }
236
                        flags |= LADJUST;
237
                        flags &= ~ZEROPAD; /* '-' disables '0' */
238
                        goto rflag;
239
                } else if (ch=='+') {
240
                        sign = '+';
241
                        goto rflag;
242
                } else if (ch=='.') {
243
                        if ((ch = PRG_RDB(fmt++)) == '*') {
244
                                n = va_arg(ap, int);
245
                                prec = n < 0 ? -1 : n;
246
                                goto rflag;
247
                        }
248
                        n = 0;
249
                        while (is_digit(ch)) {
250
                                n = n*10 + to_digit(ch);
251
                                ch = PRG_RDB(fmt++);
252
                        }
253
                        prec = n < 0 ? -1 : n;
254
                        goto reswitch;
255
                } else
256
#endif /* LIGHTPRINTF */
257
                if (ch=='0') {
258
                        /*
259
                         * ``Note that 0 is taken as a flag, not as the
260
                         * beginning of a field width.''
261
                         *      -- ANSI X3J11
262
                         */
263
                        if (!(flags & LADJUST))
264
                            flags |= ZEROPAD; /* '-' disables '0' */
265
                        goto rflag;
266
                } else if (ch>='1' && ch<='9') {
267
                        n = 0;
268
                        do {
269
                                n = 10 * n + to_digit(ch);
270
                                ch = PRG_RDB(fmt++);
271
                        } while (is_digit(ch));
272
                        width = n;
273
                        goto reswitch;
274
                } else if (ch=='h') {
275
                        flags |= SHORTINT;
276
                        goto rflag;
277
                } else if (ch=='l') {
278
                        flags |= LONGINT;
279
                        goto rflag;
280
                } else if (ch=='c') {
281
                        *(cp = buf) = va_arg(ap, int);
282
                        size = 1;
283
                        sign = '\0';
284
                } else if (ch=='D'||ch=='d'||ch=='i') {
285
                        if(ch=='D')
286
                                flags |= LONGINT;
287
                        if (flags&LONGINT) {
288
                                _ulong=va_arg(ap, long);
289
                        } else {
290
                                register int _d;
291
                                _d=va_arg(ap, int);
292
                                _ulong = flags&SHORTINT ? (long)(short)_d : (long)_d;
293
                        }
294
 
295
                        if ((long)_ulong < 0) {
296
                                _ulong = -_ulong;
297
                                sign = '-';
298
                        }
299
                        base = DEC;
300
                        goto number;
301
                } else
302
/*             
303
                if (ch=='n') {
304
                        if (flags & LONGINT)
305
                                *va_arg(ap, long *) = ret;
306
                        else if (flags & SHORTINT)
307
                                *va_arg(ap, short *) = ret;
308
                        else
309
                                *va_arg(ap, int *) = ret;
310
                        continue;       // no output
311
                } else
312
*/
313
#ifndef LIGHTPRINTF                     
314
                if (ch=='O'||ch=='o') {
315
                        if (ch=='O')
316
                                flags |= LONGINT;
317
                        base = OCT;
318
                        goto nosign;
319
                } else if (ch=='p') {
320
                        /*
321
                         * ``The argument shall be a pointer to void.  The
322
                         * value of the pointer is converted to a sequence
323
                         * of printable characters, in an implementation-
324
                         * defined manner.''
325
                         *      -- ANSI X3J11
326
                         */
327
                        /* NOSTRICT */
328
                        _ulong = (unsigned int)va_arg(ap, void *);
329
                        base = HEX;
330
                        flags |= HEXPREFIX;
331
                        ch = 'x';
332
                        goto nosign;
333
                } else if (ch=='s') {  // print a string from RAM
334
                        if ((cp = va_arg(ap, char *)) == NULL) {
335
                                cp=buf;
336
                                cp[0] = '(';
337
                                cp[1] = 'n';
338
                                cp[2] = 'u';
339
                                cp[4] = cp[3] = 'l';
340
                                cp[5] = ')';
341
                                cp[6] = '\0';
342
                        }
343
                        if (prec >= 0) {
344
                                /*
345
                                 * can't use strlen; can only look for the
346
                                 * NUL in the first `prec' characters, and
347
                                 * strlen() will go further.
348
                                 */
349
                                char *p = (char*)memchr(cp, 0, prec);
350
 
351
                                if (p != NULL) {
352
                                        size = p - cp;
353
                                        if (size > prec)
354
                                                size = prec;
355
                                } else
356
                                        size = prec;
357
                        } else
358
                                size = strlen(cp);
359
                        sign = '\0';
360
                } else
361
#endif /* LIGHTPRINTF */                        
362
                if(ch=='U'||ch=='u') {
363
                        if (ch=='U')
364
                                flags |= LONGINT;
365
                        base = DEC;
366
                        goto nosign;
367
                } else if (ch=='X'||ch=='x') {
368
                        base = HEX;
369
                        /* leading 0x/X only if non-zero */
370
                        if (flags & ALT && _ulong != 0)
371
                                flags |= HEXPREFIX;
372
 
373
                        /* unsigned conversions */
374
nosign:                 sign = '\0';
375
                        /*
376
                         * ``... diouXx conversions ... if a precision is
377
                         * specified, the 0 flag will be ignored.''
378
                         *      -- ANSI X3J11
379
                         */
380
number: if ((dprec = prec) >= 0)
381
                                flags &= ~ZEROPAD;
382
 
383
                        /*
384
                         * ``The result of converting a zero value with an
385
                         * explicit precision of zero is no characters.''
386
                         *      -- ANSI X3J11
387
                         */
388
                        cp = buf + BUF;
389
                        if (_ulong != 0 || prec != 0) {
390
                                register unsigned char _d,notlastdigit;
391
                                do {
392
                                        notlastdigit=(_ulong>=base);
393
                                        _d = _ulong % base;
394
 
395
                                        if (_d<10) {
396
                                                _d+='0';
397
                                        } else {
398
                                                _d+='a'-10;
399
                                                if (ch=='X') _d&=~0x20;
400
                                        }
401
                                        *--cp=_d;
402
                                        _ulong /= base;
403
                                } while (notlastdigit);
404
#ifndef LIGHTPRINTF
405
                                // handle octal leading 0 
406
                                if (base==OCT && flags & ALT && *cp != '0')
407
                                        *--cp = '0';
408
#endif
409
                        }
410
 
411
                        size = buf + BUF - cp;
412
        } else {  //default
413
                /* "%?" prints ?, unless ? is NUL */
414
                        if (ch == '\0')
415
                                goto done;
416
                        /* pretend it was %c with argument ch */
417
                        cp = buf;
418
                        *cp = ch;
419
                        size = 1;
420
                        sign = '\0';
421
                }
422
 
423
                /*
424
                 * All reasonable formats wind up here.  At this point,
425
                 * `cp' points to a string which (if not flags&LADJUST)
426
                 * should be padded out to `width' places.  If
427
                 * flags&ZEROPAD, it should first be prefixed by any
428
                 * sign or other prefix; otherwise, it should be blank
429
                 * padded before the prefix is emitted.  After any
430
                 * left-hand padding and prefixing, emit zeroes
431
                 * required by a decimal [diouxX] precision, then print
432
                 * the string proper, then emit zeroes required by any
433
                 * leftover floating precision; finally, if LADJUST,
434
                 * pad with blanks.
435
                 */
436
 
437
                /*
438
                 * compute actual size, so we know how much to pad.
439
                 */
440
                fieldsz = size;
441
 
442
                dpad = dprec - size;
443
                if (dpad < 0)
444
                    dpad = 0;
445
 
446
                if (sign)
447
                        fieldsz++;
448
                else if (flags & HEXPREFIX)
449
                        fieldsz += 2;
450
                fieldsz += dpad;
451
 
452
                /* right-adjusting blank padding */
453
                if ((flags & (LADJUST|ZEROPAD)) == 0)
454
                        PAD_SP(width - fieldsz);
455
 
456
                /* prefix */
457
                if (sign) {
458
                        PRINT(&sign, 1);
459
                } else if (flags & HEXPREFIX) {
460
                        ox[0] = '0';
461
                        ox[1] = ch;
462
                        PRINT(ox, 2);
463
                }
464
 
465
                /* right-adjusting zero padding */
466
                if ((flags & (LADJUST|ZEROPAD)) == ZEROPAD)
467
                        PAD_0(width - fieldsz);
468
 
469
                /* leading zeroes from decimal precision */
470
                PAD_0(dpad);
471
 
472
                /* the string or number proper */
473
                PRINT(cp, size);
474
 
475
                /* left-adjusting padding (always blank) */
476
                if (flags & LADJUST)
477
                        PAD_SP(width - fieldsz);
478
        }
479
done:
480
        va_end(ap);
481
}
482
//----------------------------------------------------------------------------------------------------------------------------------