Subversion Repositories FlightCtrl

Rev

Details | Last modification | View Log | RSS feed

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