mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-13 11:29:58 +03:00
update round, update convert integer part to string without buff
This commit is contained in:
parent
399807c054
commit
256bf21099
1 changed files with 53 additions and 30 deletions
|
@ -47,6 +47,8 @@
|
||||||
|
|
||||||
#ifdef REQUIRE_CC_ARM_PRINTF_SUPPORT
|
#ifdef REQUIRE_CC_ARM_PRINTF_SUPPORT
|
||||||
|
|
||||||
|
#define DEFAULT_FLOAT_PRECISION 6
|
||||||
|
|
||||||
putcf stdout_putf;
|
putcf stdout_putf;
|
||||||
void *stdout_putp;
|
void *stdout_putp;
|
||||||
|
|
||||||
|
@ -72,6 +74,7 @@ static int putchw(void *putp, putcf putf, int n, char z, char *bf)
|
||||||
// function to convert float to string
|
// function to convert float to string
|
||||||
static void tfp_ftoa(double f, char *buf, int buf_size, int precision) {
|
static void tfp_ftoa(double f, char *buf, int buf_size, int precision) {
|
||||||
int i = 0;
|
int i = 0;
|
||||||
|
const double scales[] = { 1e0, 1e1, 1e2, 1e3, 1e4, 1e5, 1e6 };
|
||||||
|
|
||||||
// Handle negative numbers
|
// Handle negative numbers
|
||||||
if (f < 0) {
|
if (f < 0) {
|
||||||
|
@ -80,43 +83,63 @@ static void tfp_ftoa(double f, char *buf, int buf_size, int precision) {
|
||||||
}
|
}
|
||||||
|
|
||||||
// Extract integer part
|
// Extract integer part
|
||||||
int ipart = (int)f;
|
long int_part = (long)f;
|
||||||
double fpart = f - (double)ipart;
|
|
||||||
|
|
||||||
// Convert integer part
|
// Extract fractional part
|
||||||
if (ipart == 0) {
|
double frac_part = f - (double)int_part;
|
||||||
|
|
||||||
|
// Scale the fractional part for rounding
|
||||||
|
double scaled_frac_part = frac_part * scales[precision];
|
||||||
|
|
||||||
|
// Get the rounded fractional part
|
||||||
|
long rounded_frac = (long)(scaled_frac_part);
|
||||||
|
double last_digit = scaled_frac_part - rounded_frac; // Get last digit for rounding
|
||||||
|
|
||||||
|
// Handle rounding based on the last digit
|
||||||
|
if (last_digit >= (double)0.5) {
|
||||||
|
rounded_frac++; // Round up if the last digit is 5 or more
|
||||||
|
}
|
||||||
|
|
||||||
|
// Check for overflow in the fractional part
|
||||||
|
if (rounded_frac >= scales[precision]) {
|
||||||
|
rounded_frac = 0;
|
||||||
|
int_part++; // Increment the integer part if necessary
|
||||||
|
}
|
||||||
|
|
||||||
|
// Convert the integer part to string
|
||||||
|
if (int_part == 0) {
|
||||||
buf[i++] = '0';
|
buf[i++] = '0';
|
||||||
} else {
|
} else {
|
||||||
char temp[10];
|
long num = int_part;
|
||||||
int j = 0;
|
int digits = 0;
|
||||||
while (ipart) {
|
|
||||||
temp[j++] = (ipart % 10) + '0'; // part + '0' ASCII 48
|
// Calculate the number of digits in the integer part
|
||||||
ipart /= 10;
|
long temp = num;
|
||||||
|
while (temp > 0) {
|
||||||
|
temp /= 10;
|
||||||
|
digits++;
|
||||||
}
|
}
|
||||||
while (j > 0) {
|
|
||||||
if (i < buf_size) {
|
// Write digits starting from the highest digit
|
||||||
buf[i++] = temp[--j];
|
for (int j = digits - 1; j >= 0; j--) {
|
||||||
} else {
|
if (i < buf_size - 1) {
|
||||||
break; // buffer overflow
|
buf[i + j] = (num % 10) + '0';
|
||||||
|
num /= 10;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
i += digits;
|
||||||
}
|
}
|
||||||
|
|
||||||
buf[i++] = '.';
|
// Add decimal point and fractional part if precision > 0
|
||||||
|
if (precision > 0 && i < buf_size - 1) {
|
||||||
|
buf[i++] = '.';
|
||||||
|
|
||||||
// Convert fractional part
|
// Convert the fractional part to string
|
||||||
for (int j = 0; j < precision; j++) {
|
for (int j = 0; j < precision && i < buf_size - 1; j++) {
|
||||||
fpart *= 10;
|
rounded_frac *= 10; // Shift left
|
||||||
int digit = (int)fpart;
|
buf[i++] = (char)(rounded_frac / scales[precision]) + '0';
|
||||||
if (i < buf_size) {
|
rounded_frac %= (long)scales[precision];
|
||||||
buf[i++] = digit + '0'; // part + '0' ASCII 48
|
|
||||||
}
|
}
|
||||||
fpart -= digit;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Round last digit in case of precision overflow
|
|
||||||
if ((int)(fpart * 10) >= 5 && i > 0 && i < buf_size) {
|
|
||||||
buf[i - 1] += 1; // Round up the last digit
|
|
||||||
}
|
}
|
||||||
|
|
||||||
if (i < buf_size) {
|
if (i < buf_size) {
|
||||||
|
@ -132,7 +155,6 @@ int tfp_format(void *putp, putcf putf, const char *fmt, va_list va)
|
||||||
char bf[32];
|
char bf[32];
|
||||||
int written = 0;
|
int written = 0;
|
||||||
char ch;
|
char ch;
|
||||||
const int buffer_size = sizeof(bf) - 1; // Reserve one byte for '\0'
|
|
||||||
|
|
||||||
while ((ch = *(fmt++))) {
|
while ((ch = *(fmt++))) {
|
||||||
if (ch != '%') {
|
if (ch != '%') {
|
||||||
|
@ -143,7 +165,7 @@ int tfp_format(void *putp, putcf putf, const char *fmt, va_list va)
|
||||||
char lng = 0;
|
char lng = 0;
|
||||||
#endif
|
#endif
|
||||||
int w = 0;
|
int w = 0;
|
||||||
int precision = 6; // Default float precision
|
int precision = DEFAULT_FLOAT_PRECISION; // Default float precision
|
||||||
ch = *(fmt++);
|
ch = *(fmt++);
|
||||||
if (ch == '0') {
|
if (ch == '0') {
|
||||||
ch = *(fmt++);
|
ch = *(fmt++);
|
||||||
|
@ -211,7 +233,8 @@ int tfp_format(void *putp, putcf putf, const char *fmt, va_list va)
|
||||||
break;
|
break;
|
||||||
case 'f': {
|
case 'f': {
|
||||||
double f = va_arg(va, double);
|
double f = va_arg(va, double);
|
||||||
tfp_ftoa(f, bf, buffer_size, precision);
|
if (precision > DEFAULT_FLOAT_PRECISION) precision = DEFAULT_FLOAT_PRECISION;
|
||||||
|
tfp_ftoa(f, bf, sizeof(bf) - 1, precision);
|
||||||
written = putchw(putp, putf, w, lz, bf);
|
written = putchw(putp, putf, w, lz, bf);
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue