337 lines
7.8 KiB
C
337 lines
7.8 KiB
C
#include "stm32f4xx.h"
|
|
|
|
#include <stdarg.h>
|
|
#include <stddef.h>
|
|
#include <stdint.h>
|
|
#include <stdio.h>
|
|
|
|
typedef struct
|
|
{
|
|
char *buffer;
|
|
size_t capacity;
|
|
size_t length;
|
|
} OutputBuffer;
|
|
|
|
static void output_char(OutputBuffer *output, char value)
|
|
{
|
|
if (output->capacity != 0U && output->length + 1U < output->capacity)
|
|
{
|
|
output->buffer[output->length] = value;
|
|
}
|
|
output->length++;
|
|
}
|
|
|
|
static void output_unsigned(OutputBuffer *output, uint32_t value, unsigned int base,
|
|
unsigned int width, char pad, int uppercase)
|
|
{
|
|
char digits[16];
|
|
unsigned int count = 0U;
|
|
const char *alphabet = uppercase ? "0123456789ABCDEF" : "0123456789abcdef";
|
|
|
|
do
|
|
{
|
|
digits[count++] = alphabet[value % base];
|
|
value /= base;
|
|
} while (value != 0U);
|
|
|
|
while (count < width)
|
|
{
|
|
output_char(output, pad);
|
|
width--;
|
|
}
|
|
while (count != 0U)
|
|
{
|
|
output_char(output, digits[--count]);
|
|
}
|
|
}
|
|
|
|
int vsnprintf(char *buffer, size_t size, const char *format, va_list args)
|
|
{
|
|
OutputBuffer output = {buffer, size, 0U};
|
|
|
|
while (*format != '\0')
|
|
{
|
|
unsigned int width = 0U;
|
|
char pad = ' ';
|
|
int is_long = 0;
|
|
char specifier;
|
|
|
|
if (*format != '%')
|
|
{
|
|
output_char(&output, *format++);
|
|
continue;
|
|
}
|
|
format++;
|
|
if (*format == '%')
|
|
{
|
|
output_char(&output, *format++);
|
|
continue;
|
|
}
|
|
if (*format == '0')
|
|
{
|
|
pad = '0';
|
|
format++;
|
|
}
|
|
while (*format >= '0' && *format <= '9')
|
|
{
|
|
width = width * 10U + (unsigned int)(*format++ - '0');
|
|
}
|
|
if (*format == 'l')
|
|
{
|
|
is_long = 1;
|
|
format++;
|
|
}
|
|
specifier = *format++;
|
|
if (specifier == 's')
|
|
{
|
|
const char *text = va_arg(args, const char *);
|
|
if (text == NULL)
|
|
{
|
|
text = "(null)";
|
|
}
|
|
while (*text != '\0')
|
|
{
|
|
output_char(&output, *text++);
|
|
}
|
|
}
|
|
else if (specifier == 'c')
|
|
{
|
|
output_char(&output, (char)va_arg(args, int));
|
|
}
|
|
else if (specifier == 'd' || specifier == 'i')
|
|
{
|
|
int32_t value = is_long ? (int32_t)va_arg(args, long) : (int32_t)va_arg(args, int);
|
|
uint32_t magnitude;
|
|
if (value < 0)
|
|
{
|
|
output_char(&output, '-');
|
|
magnitude = 0U - (uint32_t)value;
|
|
if (width != 0U)
|
|
{
|
|
width--;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
magnitude = (uint32_t)value;
|
|
}
|
|
output_unsigned(&output, magnitude, 10U, width, pad, 0);
|
|
}
|
|
else if (specifier == 'u' || specifier == 'x' || specifier == 'X')
|
|
{
|
|
uint32_t value = is_long ? (uint32_t)va_arg(args, unsigned long)
|
|
: (uint32_t)va_arg(args, unsigned int);
|
|
unsigned int base = specifier == 'u' ? 10U : 16U;
|
|
output_unsigned(&output, value, base, width, pad, specifier == 'X');
|
|
}
|
|
else if (specifier == 'p')
|
|
{
|
|
uintptr_t value = (uintptr_t)va_arg(args, void *);
|
|
output_char(&output, '0');
|
|
output_char(&output, 'x');
|
|
output_unsigned(&output, (uint32_t)value, 16U, 8U, '0', 0);
|
|
}
|
|
else
|
|
{
|
|
output_char(&output, '%');
|
|
output_char(&output, specifier);
|
|
}
|
|
}
|
|
|
|
if (output.capacity != 0U)
|
|
{
|
|
size_t terminator = output.length < output.capacity ? output.length : output.capacity - 1U;
|
|
output.buffer[terminator] = '\0';
|
|
}
|
|
return (int)output.length;
|
|
}
|
|
|
|
int snprintf(char *buffer, size_t size, const char *format, ...)
|
|
{
|
|
int result;
|
|
va_list args;
|
|
va_start(args, format);
|
|
result = vsnprintf(buffer, size, format, args);
|
|
va_end(args);
|
|
return result;
|
|
}
|
|
|
|
int sprintf(char *buffer, const char *format, ...)
|
|
{
|
|
int result;
|
|
va_list args;
|
|
va_start(args, format);
|
|
result = vsnprintf(buffer, (size_t)-1, format, args);
|
|
va_end(args);
|
|
return result;
|
|
}
|
|
|
|
int printf(const char *format, ...)
|
|
{
|
|
char buffer[192];
|
|
int result;
|
|
size_t count;
|
|
va_list args;
|
|
|
|
va_start(args, format);
|
|
result = vsnprintf(buffer, sizeof(buffer), format, args);
|
|
va_end(args);
|
|
count = result < 0 ? 0U : (size_t)result;
|
|
if (count >= sizeof(buffer))
|
|
{
|
|
count = sizeof(buffer) - 1U;
|
|
}
|
|
for (size_t index = 0U; index < count; index++)
|
|
{
|
|
while ((USART2->SR & USART_SR_TC) == 0U)
|
|
{
|
|
}
|
|
USART2->DR = (uint8_t)buffer[index];
|
|
}
|
|
return result;
|
|
}
|
|
|
|
void *memcpy(void *destination, const void *source, size_t count)
|
|
{
|
|
uint8_t *to = (uint8_t *)destination;
|
|
const uint8_t *from = (const uint8_t *)source;
|
|
while (count-- != 0U)
|
|
{
|
|
*to++ = *from++;
|
|
}
|
|
return destination;
|
|
}
|
|
|
|
void *memset(void *destination, int value, size_t count)
|
|
{
|
|
uint8_t *to = (uint8_t *)destination;
|
|
while (count-- != 0U)
|
|
{
|
|
*to++ = (uint8_t)value;
|
|
}
|
|
return destination;
|
|
}
|
|
|
|
int memcmp(const void *left, const void *right, size_t count)
|
|
{
|
|
const uint8_t *lhs = (const uint8_t *)left;
|
|
const uint8_t *rhs = (const uint8_t *)right;
|
|
while (count-- != 0U)
|
|
{
|
|
if (*lhs != *rhs)
|
|
{
|
|
return (int)*lhs - (int)*rhs;
|
|
}
|
|
lhs++;
|
|
rhs++;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
size_t strlen(const char *string)
|
|
{
|
|
const char *end = string;
|
|
while (*end != '\0')
|
|
{
|
|
end++;
|
|
}
|
|
return (size_t)(end - string);
|
|
}
|
|
|
|
char *strcpy(char *destination, const char *source)
|
|
{
|
|
char *result = destination;
|
|
do
|
|
{
|
|
*destination++ = *source;
|
|
} while (*source++ != '\0');
|
|
return result;
|
|
}
|
|
|
|
int abs(int value)
|
|
{
|
|
return value < 0 ? -value : value;
|
|
}
|
|
|
|
double sqrt(double value)
|
|
{
|
|
double estimate;
|
|
if (value <= 0.0)
|
|
{
|
|
return value == 0.0 ? 0.0 : (0.0 / 0.0);
|
|
}
|
|
estimate = value > 1.0 ? value : 1.0;
|
|
for (unsigned int iteration = 0U; iteration < 12U; iteration++)
|
|
{
|
|
estimate = 0.5 * (estimate + value / estimate);
|
|
}
|
|
return estimate;
|
|
}
|
|
|
|
double pow(double base, double exponent)
|
|
{
|
|
int power = (int)exponent;
|
|
unsigned int magnitude;
|
|
double result = 1.0;
|
|
if ((double)power != exponent)
|
|
{
|
|
return 0.0 / 0.0;
|
|
}
|
|
magnitude = power < 0 ? (unsigned int)(-power) : (unsigned int)power;
|
|
while (magnitude != 0U)
|
|
{
|
|
if ((magnitude & 1U) != 0U)
|
|
{
|
|
result *= base;
|
|
}
|
|
base *= base;
|
|
magnitude >>= 1U;
|
|
}
|
|
return power < 0 ? 1.0 / result : result;
|
|
}
|
|
|
|
double atan(double value)
|
|
{
|
|
const double half_pi = 1.5707963267948966;
|
|
const double quarter_pi = 0.7853981633974483;
|
|
double sign = 1.0;
|
|
double result;
|
|
if (value < 0.0)
|
|
{
|
|
value = -value;
|
|
sign = -1.0;
|
|
}
|
|
if (value > 1.0)
|
|
{
|
|
double reciprocal = 1.0 / value;
|
|
result = half_pi - reciprocal * (quarter_pi + 0.273 * (1.0 - reciprocal));
|
|
}
|
|
else
|
|
{
|
|
result = value * (quarter_pi + 0.273 * (1.0 - value));
|
|
}
|
|
return sign * result;
|
|
}
|
|
|
|
double tan(double value)
|
|
{
|
|
const double pi = 3.1415926535897932;
|
|
double squared;
|
|
double sine;
|
|
double cosine;
|
|
while (value > 1.5707963267948966)
|
|
{
|
|
value -= pi;
|
|
}
|
|
while (value < -1.5707963267948966)
|
|
{
|
|
value += pi;
|
|
}
|
|
squared = value * value;
|
|
sine = value * (1.0 + squared * (-1.0 / 6.0 + squared * (1.0 / 120.0 +
|
|
squared * (-1.0 / 5040.0 + squared * (1.0 / 362880.0)))));
|
|
cosine = 1.0 + squared * (-1.0 / 2.0 + squared * (1.0 / 24.0 +
|
|
squared * (-1.0 / 720.0 + squared * (1.0 / 40320.0))));
|
|
return sine / cosine;
|
|
}
|