mirror of
https://github.com/ttrftech/NanoVNA.git
synced 2025-12-06 03:31:59 +01:00
595 lines
16 KiB
C
595 lines
16 KiB
C
/*
|
|
ChibiOS - Copyright (C) 2006..2016 Giovanni Di Sirio
|
|
|
|
Licensed under the Apache License, Version 2.0 (the "License");
|
|
you may not use this file except in compliance with the License.
|
|
You may obtain a copy of the License at
|
|
|
|
http://www.apache.org/licenses/LICENSE-2.0
|
|
|
|
Unless required by applicable law or agreed to in writing, software
|
|
distributed under the License is distributed on an "AS IS" BASIS,
|
|
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
See the License for the specific language governing permissions and
|
|
limitations under the License.
|
|
*/
|
|
|
|
/*
|
|
Concepts and parts of this file have been contributed by Fabio Utzig,
|
|
chvprintf() added by Brent Roman.
|
|
*/
|
|
|
|
/**
|
|
* @file chprintf.c
|
|
* @brief Mini printf-like functionality.
|
|
*
|
|
* @addtogroup chprintf
|
|
* @{
|
|
*/
|
|
|
|
#include "hal.h"
|
|
#include "chprintf.h"
|
|
//#include "memstreams.h"
|
|
#include <math.h>
|
|
|
|
// Enable [flags], support:
|
|
// ' ' Prepends a space for positive signed-numeric types. positive = ' ', negative = '-'. This flag is ignored if the + flag exists.
|
|
//#define CHPRINTF_USE_SPACE_FLAG
|
|
|
|
// Force putting trailing zeros on float value
|
|
#define CHPRINTF_FORCE_TRAILING_ZEROS
|
|
|
|
#define MAX_FILLER 11
|
|
#define FLOAT_PRECISION 9
|
|
|
|
#pragma pack(push, 2)
|
|
|
|
static const uint32_t pow10[FLOAT_PRECISION+1] = {
|
|
1, 10, 100, 1000, 10000, 100000, 1000000, 10000000, 100000000, 1000000000
|
|
};
|
|
// Prefixes for values bigger then 1000.0
|
|
// 1 1e3, 1e6, 1e9, 1e12, 1e15, 1e18, 1e21, 1e24
|
|
static char bigPrefix[] = {' ', 'k', 'M', 'G', 'T', 'P', 'E', 'Z', 'Y', 0};
|
|
// Prefixes for values less then 1.0
|
|
// 1e-3, 1e-6, 1e-9, 1e-12, 1e-15, 1e-18, 1e-21, 1e-24
|
|
static char smallPrefix[] = {'m', 0x1d, 'n', 'p', 'f', 'a', 'z', 'y', 0};
|
|
|
|
#pragma pack(pop)
|
|
|
|
static char *long_to_string_with_divisor(char *p,
|
|
uint32_t num,
|
|
uint32_t radix,
|
|
uint32_t precision) {
|
|
char *q = p + MAX_FILLER;
|
|
char *b = q;
|
|
// convert to string from end buffer to begin
|
|
do {
|
|
uint8_t c = num % radix;
|
|
num /= radix;
|
|
*--q = c + ((c > 9) ? ('A'-10) : '0');
|
|
}while((precision && --precision) || num);
|
|
// copy string at begin
|
|
int i = (int)(b - q);
|
|
do
|
|
*p++ = *q++;
|
|
while (--i);
|
|
return p;
|
|
}
|
|
|
|
// default prescision = 13
|
|
// g.mmm kkk hhh
|
|
#define MAX_FREQ_PRESCISION 13
|
|
#define FREQ_PSET 1
|
|
#define FREQ_NO_SPACE 2
|
|
#define FREQ_PREFIX_SPACE 4
|
|
|
|
static char *
|
|
ulong_freq(char *p, uint32_t freq, uint32_t precision)
|
|
{
|
|
uint8_t flag = FREQ_PSET;
|
|
if (precision == 0)
|
|
flag|=FREQ_PREFIX_SPACE;
|
|
if (precision == 0 || precision > MAX_FREQ_PRESCISION)
|
|
precision = MAX_FREQ_PRESCISION;
|
|
char *q = p + MAX_FREQ_PRESCISION;
|
|
char *b = q;
|
|
// Prefix counter
|
|
uint32_t s = 0;
|
|
// Set format (every 3 digits add ' ' up to GHz)
|
|
uint32_t format = 0b00100100100;
|
|
do {
|
|
#if 0
|
|
uint8_t c = freq % 10;
|
|
freq/= 10;
|
|
#else
|
|
// Fast and compact division uint32_t on 10, using shifts, result:
|
|
// c = freq % 10
|
|
// freq = freq / 10;
|
|
uint32_t c = freq;
|
|
freq >>= 1;
|
|
freq += freq >> 1;
|
|
freq += freq >> 4;
|
|
freq += freq >> 8;
|
|
freq += freq >> 16; // freq = 858993459*freq/1073741824 = freq *
|
|
// 0,799999999813735485076904296875
|
|
freq >>= 3; // freq/=8; freq = freq * 0,09999999997671693563461303710938
|
|
c -= freq * 10; // freq*10 = (freq*4+freq)*2 = ((freq<<2)+freq)<<1
|
|
while (c >= 10) {
|
|
freq++;
|
|
c -= 10;
|
|
}
|
|
#endif
|
|
*--q = c + '0';
|
|
if (freq == 0) break;
|
|
// Add spaces, calculate prefix
|
|
if (format & 1) {
|
|
*--q = ' ';
|
|
s++;
|
|
}
|
|
format >>= 1;
|
|
} while (1);
|
|
s = bigPrefix[s];
|
|
|
|
// Get string size
|
|
uint32_t i = (b - q);
|
|
// Limit string size, max size is - precision
|
|
if (precision && i > precision) {
|
|
i = precision;
|
|
flag |= FREQ_NO_SPACE;
|
|
}
|
|
// copy string
|
|
// Replace first ' ' by '.', remove ' ' if size too big
|
|
do {
|
|
char c = *q++;
|
|
// replace first ' ' on '.'
|
|
if (c == ' ') {
|
|
if (flag & FREQ_PSET) {
|
|
c = '.';
|
|
flag &= ~FREQ_PSET;
|
|
} else if (flag & FREQ_NO_SPACE)
|
|
c = *q++;
|
|
}
|
|
*p++ = c;
|
|
} while (--i);
|
|
// Put pref (amd space before it if need)
|
|
if (flag & FREQ_PREFIX_SPACE && s != ' ')
|
|
*p++ = ' ';
|
|
*p++ = s;
|
|
return p;
|
|
}
|
|
|
|
#if CHPRINTF_USE_FLOAT
|
|
static char *ftoa(char *p, float num, uint32_t precision) {
|
|
// Check precision limit
|
|
if (precision > FLOAT_PRECISION)
|
|
precision = FLOAT_PRECISION;
|
|
uint32_t multi = pow10[precision];
|
|
uint32_t l = num;
|
|
// Round value
|
|
uint32_t k = ((num-l)*multi+0.5);
|
|
// Fix rounding error if get
|
|
if (k>=multi){k-=multi;l++;}
|
|
p = long_to_string_with_divisor(p, l, 10, 0);
|
|
if (precision) {
|
|
*p++ = '.';
|
|
p=long_to_string_with_divisor(p, k, 10, precision);
|
|
#ifndef CHPRINTF_FORCE_TRAILING_ZEROS
|
|
// remove zeros at end
|
|
while (p[-1]=='0') p--;
|
|
if (p[-1]=='.') p--;
|
|
#endif
|
|
}
|
|
return p;
|
|
}
|
|
|
|
static char *ftoaS(char *p, float num, uint32_t precision) {
|
|
char prefix=0;
|
|
char *ptr;
|
|
if (num > 1000.0){
|
|
for (ptr = bigPrefix+1; *ptr && num > 1000.0; num/=1000, ptr++)
|
|
;
|
|
prefix = ptr[-1];
|
|
}
|
|
else if (num < 1){
|
|
for (ptr = smallPrefix; *ptr && num < 1.0; num*=1000, ptr++)
|
|
;
|
|
prefix = num > 1e-3 ? ptr[-1] : 0;
|
|
}
|
|
// Auto set prescision
|
|
uint32_t l = num;
|
|
if (l < 10)
|
|
precision+=2;
|
|
else if (l < 100)
|
|
precision+=1;
|
|
p=ftoa(p, num, precision);
|
|
if (prefix)
|
|
*p++ = prefix;
|
|
return p;
|
|
}
|
|
#endif
|
|
|
|
/**
|
|
* @brief System formatted output function.
|
|
* @details This function implements a minimal @p vprintf()-like functionality
|
|
* with output on a @p BaseSequentialStream.
|
|
* The general parameters format is: %[-][width|*][.precision|*][l|L]p.
|
|
* The following parameter types (p) are supported:
|
|
* - <b>x</b> hexadecimal integer.
|
|
* - <b>X</b> hexadecimal long.
|
|
* - <b>o</b> octal integer.
|
|
* - <b>O</b> octal long.
|
|
* - <b>d</b> decimal signed integer.
|
|
* - <b>D</b> decimal signed long.
|
|
* - <b>u</b> decimal unsigned integer.
|
|
* - <b>U</b> decimal unsigned long.
|
|
* - <b>c</b> character.
|
|
* - <b>s</b> string.
|
|
* .
|
|
*
|
|
* @param[in] chp pointer to a @p BaseSequentialStream implementing object
|
|
* @param[in] fmt formatting string
|
|
* @param[in] ap list of parameters
|
|
* @return The number of bytes that would have been
|
|
* written to @p chp if no stream error occurs
|
|
*
|
|
* @api
|
|
*/
|
|
#define IS_LONG 1
|
|
#define LEFT_ALIGN 2
|
|
#define POSITIVE 4
|
|
#define NEGATIVE 8
|
|
#define PAD_ZERO 16
|
|
#define PLUS_SPACE 32
|
|
#define DEFAULT_PRESCISION 64
|
|
|
|
int chvprintf(BaseSequentialStream *chp, const char *fmt, va_list ap) {
|
|
char *p, *s, c, filler=' ';
|
|
int precision, width;
|
|
int n = 0;
|
|
uint32_t state;
|
|
union {
|
|
uint32_t u;
|
|
int32_t l;
|
|
float f;
|
|
}value;
|
|
#if CHPRINTF_USE_FLOAT
|
|
char tmpbuf[2*MAX_FILLER + 1];
|
|
#else
|
|
char tmpbuf[MAX_FREQ_PRESCISION + 1];
|
|
#endif
|
|
|
|
while (true) {
|
|
c = *fmt++;
|
|
if (c == 0)
|
|
return n;
|
|
if (c != '%') {
|
|
streamPut(chp, (uint8_t)c);
|
|
n++;
|
|
continue;
|
|
}
|
|
// Parse %[flags][width][.precision][length]type
|
|
p = tmpbuf;
|
|
s = tmpbuf;
|
|
state = 0;
|
|
width = 0;
|
|
precision = 0;
|
|
|
|
// Get [flags], support:
|
|
// '-' Left-align the output of this placeholder. (The default is to right-align the output.)
|
|
// '+' Prepends a plus for positive signed-numeric types. positive = '+', negative = '-'.
|
|
// ' ' Prepends a space for positive signed-numeric types. positive = ' ', negative = '-'. This flag is ignored if the + flag exists.
|
|
// '0' When the 'width' option is specified, prepends zeros for numeric types. (The default prepends spaces.)
|
|
while (true){
|
|
if (*fmt == '-')
|
|
state|=LEFT_ALIGN;
|
|
else if (*fmt == '+')
|
|
state|=POSITIVE;
|
|
else if (*fmt == '0')
|
|
state|=PAD_ZERO;
|
|
#ifdef CHPRINTF_USE_SPACE_FLAG
|
|
else if (*fmt == ' ')
|
|
state|=PLUS_SPACE;
|
|
#endif
|
|
else
|
|
break;
|
|
fmt++;
|
|
}
|
|
// Get [width] - The Width field specifies a minimum number of characters to output
|
|
// if set *, get width as argument
|
|
while (true) {
|
|
c = *fmt++;
|
|
if (c >= '0' && c <= '9')
|
|
c -= '0';
|
|
else if (c == '*')
|
|
c = va_arg(ap, int);
|
|
else
|
|
break;
|
|
width = width * 10 + c;
|
|
}
|
|
// Get [.precision]
|
|
if (c == '.') {
|
|
while (true) {
|
|
c = *fmt++;
|
|
if (c >= '0' && c <= '9')
|
|
c -= '0';
|
|
else if (c == '*')
|
|
c = va_arg(ap, int);
|
|
else
|
|
break;
|
|
precision = precision * 10 + c;
|
|
}
|
|
}
|
|
else
|
|
state|=DEFAULT_PRESCISION;
|
|
//Get [length]
|
|
/*
|
|
if (c == 'l' || c == 'L') {
|
|
state|=IS_LONG;
|
|
if (*fmt)
|
|
c = *fmt++;
|
|
}
|
|
else if ((c >= 'A') && (c <= 'Z'))
|
|
state|=IS_LONG;
|
|
*/
|
|
// Parse type
|
|
switch (c) {
|
|
case 'c':
|
|
state&=~PAD_ZERO;
|
|
*p++ = va_arg(ap, int);
|
|
break;
|
|
case 's':
|
|
state&=~PAD_ZERO;
|
|
if ((s = va_arg(ap, char *)) == 0)
|
|
s = "(null)";
|
|
if (state&DEFAULT_PRESCISION)
|
|
precision = 32767;
|
|
for (p = s; *p && (--precision >= 0); p++)
|
|
;
|
|
break;
|
|
case 'D':
|
|
case 'd':
|
|
case 'I':
|
|
case 'i':/*
|
|
if (state & IS_LONG)
|
|
value.l = va_arg(ap, long);
|
|
else*/
|
|
value.l = va_arg(ap, uint32_t);
|
|
if (value.l < 0) {
|
|
state|=NEGATIVE;
|
|
*p++ = '-';
|
|
value.l = -value.l;
|
|
}
|
|
else if (state & POSITIVE)
|
|
*p++ = '+';
|
|
#ifdef CHPRINTF_USE_SPACE_FLAG
|
|
else if (state & PLUS_SPACE)
|
|
*p++ = ' ';
|
|
#endif
|
|
p = long_to_string_with_divisor(p, value.l, 10, 0);
|
|
break;
|
|
case 'q':
|
|
value.u = va_arg(ap, uint32_t);
|
|
p=ulong_freq(p, value.u, precision);
|
|
break;
|
|
#if CHPRINTF_USE_FLOAT
|
|
case 'F':
|
|
case 'f':
|
|
value.f = va_arg(ap, double);
|
|
if (value.f < 0) {
|
|
state|=NEGATIVE;
|
|
*p++ = '-';
|
|
value.f = -value.f;
|
|
}
|
|
else if (state & POSITIVE)
|
|
*p++ = '+';
|
|
#ifdef CHPRINTF_USE_SPACE_FLAG
|
|
else if (state & PLUS_SPACE)
|
|
*p++ = ' ';
|
|
#endif
|
|
if (value.f == INFINITY){
|
|
*p++ = 0x19;
|
|
break;
|
|
}
|
|
p = (c=='F') ? ftoaS(p, value.f, precision) : ftoa(p, value.f, state&DEFAULT_PRESCISION ? FLOAT_PRECISION : precision);
|
|
break;
|
|
#endif
|
|
case 'X':
|
|
case 'x':
|
|
c = 16;
|
|
goto unsigned_common;
|
|
case 'U':
|
|
case 'u':
|
|
c = 10;
|
|
goto unsigned_common;
|
|
case 'O':
|
|
case 'o':
|
|
c = 8;
|
|
unsigned_common:/*
|
|
if (state & IS_LONG)
|
|
value.u = va_arg(ap, unsigned long);
|
|
else*/
|
|
value.u = va_arg(ap, uint32_t);
|
|
p = long_to_string_with_divisor(p, value.u, c, 0);
|
|
break;
|
|
default:
|
|
*p++ = c;
|
|
break;
|
|
}
|
|
// Now need print buffer s[{sign}XXXXXXXXXXXX]p and align it on width
|
|
// Check filler width (if buffer less then width) and prepare filler if need fill
|
|
if ((width -=(int)(p - s)) < 0)
|
|
width = 0;
|
|
else
|
|
filler = (state&PAD_ZERO) ? '0' : ' ';
|
|
// if left align, put sign digit, and fill
|
|
// [{sign}ffffffXXXXXXXXXXXX]
|
|
if (!(state&LEFT_ALIGN)) {
|
|
// Put '+' or '-' or ' ' first if need
|
|
if ((state&(NEGATIVE|POSITIVE|PLUS_SPACE)) && (state&PAD_ZERO)) {
|
|
streamPut(chp, (uint8_t)*s++);
|
|
n++;
|
|
}
|
|
// fill from left
|
|
while (width){
|
|
streamPut(chp, (uint8_t)filler);
|
|
n++;
|
|
width--;
|
|
}
|
|
}
|
|
// put data
|
|
while (s < p) {
|
|
streamPut(chp, (uint8_t)*s++);
|
|
n++;
|
|
}
|
|
// Put filler from right (if need)
|
|
while (width) {
|
|
streamPut(chp, (uint8_t)filler);
|
|
n++;
|
|
width--;
|
|
}
|
|
}
|
|
}
|
|
|
|
/**
|
|
* @brief System formatted output function.
|
|
* @details This function implements a minimal @p printf() like functionality
|
|
* with output on a @p BaseSequentialStream.
|
|
* The general parameters format is: %[-][width|*][.precision|*][l|L]p.
|
|
* The following parameter types (p) are supported:
|
|
* - <b>x</b> hexadecimal integer.
|
|
* - <b>X</b> hexadecimal long.
|
|
* - <b>o</b> octal integer.
|
|
* - <b>O</b> octal long.
|
|
* - <b>d</b> decimal signed integer.
|
|
* - <b>D</b> decimal signed long.
|
|
* - <b>u</b> decimal unsigned integer.
|
|
* - <b>U</b> decimal unsigned long.
|
|
* - <b>c</b> character.
|
|
* - <b>s</b> string.
|
|
* .
|
|
*
|
|
* @param[in] chp pointer to a @p BaseSequentialStream implementing object
|
|
* @param[in] fmt formatting string
|
|
*
|
|
* @api
|
|
*/
|
|
#if 0
|
|
int chprintf(BaseSequentialStream *chp, const char *fmt, ...) {
|
|
va_list ap;
|
|
int formatted_bytes;
|
|
|
|
va_start(ap, fmt);
|
|
formatted_bytes = chvprintf(chp, fmt, ap);
|
|
va_end(ap);
|
|
|
|
return formatted_bytes;
|
|
}
|
|
#endif
|
|
/**
|
|
* @brief System formatted output function.
|
|
* @details This function implements a minimal @p vprintf()-like functionality
|
|
* with output on a @p BaseSequentialStream.
|
|
* The general parameters format is: %[-][width|*][.precision|*][l|L]p.
|
|
* The following parameter types (p) are supported:
|
|
* - <b>x</b> hexadecimal integer.
|
|
* - <b>X</b> hexadecimal long.
|
|
* - <b>o</b> octal integer.
|
|
* - <b>O</b> octal long.
|
|
* - <b>d</b> decimal signed integer.
|
|
* - <b>D</b> decimal signed long.
|
|
* - <b>u</b> decimal unsigned integer.
|
|
* - <b>U</b> decimal unsigned long.
|
|
* - <b>c</b> character.
|
|
* - <b>s</b> string.
|
|
* .
|
|
* @post @p str is NUL-terminated, unless @p size is 0.
|
|
*
|
|
* @param[in] str pointer to a buffer
|
|
* @param[in] size maximum size of the buffer
|
|
* @param[in] fmt formatting string
|
|
* @return The number of characters (excluding the
|
|
* terminating NUL byte) that would have been
|
|
* stored in @p str if there was room.
|
|
*
|
|
* @api
|
|
*/
|
|
#if 0
|
|
int chsnprintf(char *str, size_t size, const char *fmt, ...) {
|
|
va_list ap;
|
|
MemoryStream ms;
|
|
BaseSequentialStream *chp;
|
|
size_t size_wo_nul;
|
|
int retval;
|
|
|
|
if (size > 0)
|
|
size_wo_nul = size - 1;
|
|
else
|
|
size_wo_nul = 0;
|
|
|
|
/* Memory stream object to be used as a string writer, reserving one
|
|
byte for the final zero.*/
|
|
msObjectInit(&ms, (uint8_t *)str, size_wo_nul, 0);
|
|
|
|
/* Performing the print operation using the common code.*/
|
|
chp = (BaseSequentialStream *)(void *)&ms;
|
|
va_start(ap, fmt);
|
|
retval = chvprintf(chp, fmt, ap);
|
|
va_end(ap);
|
|
|
|
/* Terminate with a zero, unless size==0.*/
|
|
if (ms.eos < size)
|
|
str[ms.eos] = 0;
|
|
|
|
/* Return number of bytes that would have been written.*/
|
|
return retval;
|
|
}
|
|
#endif
|
|
|
|
//
|
|
// Small memory stream object, only put function
|
|
//
|
|
struct printStreamVMT {
|
|
_base_sequential_stream_methods
|
|
};
|
|
|
|
typedef struct {
|
|
const struct printStreamVMT *vmt;
|
|
uint8_t *buffer;
|
|
uint16_t size;
|
|
} printStream;
|
|
|
|
static msg_t put(void *ip, uint8_t b) {
|
|
printStream *ps = ip;
|
|
if (ps->size > 1){
|
|
*(ps->buffer++) = b;
|
|
ps->size--;
|
|
}
|
|
return MSG_OK;
|
|
}
|
|
|
|
static const struct printStreamVMT vmt = {NULL, NULL, put, NULL};
|
|
void printObjectInit(printStream *ps, int size, uint8_t *buffer){
|
|
ps->vmt = &vmt;
|
|
ps->buffer = buffer;
|
|
ps->size = size;
|
|
}
|
|
// Simple print in buffer function
|
|
int plot_printf(char *str, int size, const char *fmt, ...) {
|
|
va_list ap;
|
|
printStream ps;
|
|
int retval;
|
|
if (size <= 0) return 0;
|
|
// Init small memory stream for print
|
|
printObjectInit(&ps, size, (uint8_t *)str);
|
|
// Performing the print operation using the common code.
|
|
va_start(ap, fmt);
|
|
retval = chvprintf((BaseSequentialStream *)(void *)&ps, fmt, ap);
|
|
va_end(ap);
|
|
*(ps.buffer)=0;
|
|
if (retval > size-1) retval = size-1;
|
|
// Return number of bytes that would have been written.
|
|
return retval;
|
|
}
|
|
|
|
/** @} */
|