testArduino/fsfw/globalfunctions/AsciiConverter.cpp

231 lines
5.3 KiB
C++

#include "AsciiConverter.h"
#include <limits>
#include <cmath>
template<typename T>
ReturnValue_t AsciiConverter::scanAsciiDecimalNumber(const uint8_t** dataPtr,
uint8_t len, T* value) {
if (len > std::numeric_limits<T>().digits10) {
return TOO_LONG_FOR_TARGET_TYPE;
}
double temp;
ReturnValue_t result = scanAsciiDecimalNumber_(dataPtr, len, &temp);
*value = temp;
return result;
}
ReturnValue_t AsciiConverter::scanAsciiHexByte(const uint8_t** dataPtr,
uint8_t* value) {
int8_t tmp;
tmp = convertHexChar(*dataPtr);
(*dataPtr)++;
if (tmp == -1) {
return INVALID_CHARACTERS;
}
if (tmp == -2) {
tmp = 0;
}
*value = tmp << 4;
tmp = convertHexChar(*dataPtr);
(*dataPtr)++;
if (tmp == -1) {
return INVALID_CHARACTERS;
}
if (tmp != -2) {
*value += tmp;
} else {
*value = *value >> 4;
}
return RETURN_OK;
}
ReturnValue_t AsciiConverter::scanAsciiDecimalNumber_(uint8_t const ** dataPtr,
uint8_t len, double* value) {
uint8_t const *ptr = *dataPtr;
int8_t sign = 1;
float decimal = 0;
bool abort = false;
*value = 0;
//ignore leading space
ptr = clearSpace(ptr, len);
while ((ptr - *dataPtr < len) && !abort) {
switch (*ptr) {
case '+':
sign = 1;
break;
case '-':
sign = -1;
break;
case '.':
decimal = 1;
break;
case ' ':
case 0x0d:
case 0x0a:
//ignore trailing space
ptr = clearSpace(ptr, len - (ptr - *dataPtr)) - 1; //before aborting the loop, ptr will be incremented
abort = true;
break;
default:
if ((*ptr < 0x30) || (*ptr > 0x39)) {
return INVALID_CHARACTERS;
}
*value = *value * 10 + (*ptr - 0x30);
if (decimal > 0) {
decimal *= 10;
}
break;
}
ptr++;
}
if (decimal == 0) {
decimal = 1;
}
*value = *value / (decimal) * sign;
*dataPtr = ptr;
return RETURN_OK;
}
ReturnValue_t AsciiConverter::printFloat(uint8_t* buffer, uint32_t bufferLength,
float value, uint8_t decimalPlaces, uint32_t *printedSize) {
*printedSize = 0;
uint32_t streamposition = 0, integerSize;
bool negative = (value < 0);
int32_t digits = bufferLength - decimalPlaces - 1;
if (digits <= 0) {
return BUFFER_TOO_SMALL;
}
if (negative) {
digits -= 1;
buffer[streamposition++] = '-';
value = -value;
}
float maximumNumber = pow(10, digits);
if (value >= maximumNumber) {
return BUFFER_TOO_SMALL;
}
//print the numbers before the decimal point;
ReturnValue_t result = printInteger(buffer + streamposition,
bufferLength - streamposition - decimalPlaces - 1, value,
&integerSize);
if (result != RETURN_OK) {
return result;
}
streamposition += integerSize;
//The decimal Point
buffer[streamposition++] = '.';
//Print the decimals
uint32_t integerValue = value;
value -= integerValue;
value = value * pow(10, decimalPlaces);
result = printInteger(buffer + streamposition, decimalPlaces, round(value),
&integerSize, true);
*printedSize = integerSize + streamposition;
return result;
}
ReturnValue_t AsciiConverter::printInteger(uint8_t* buffer,
uint32_t bufferLength, uint32_t value, uint32_t *printedSize,
bool leadingZeros) {
*printedSize = 0;
if (bufferLength == 0) {
return BUFFER_TOO_SMALL;
}
uint32_t maximumNumber = -1;
if (bufferLength < 10) {
maximumNumber = pow(10, bufferLength);
if (value >= maximumNumber) {
return BUFFER_TOO_SMALL;
}
maximumNumber /= 10;
} else {
if (!(value <= maximumNumber)) {
return BUFFER_TOO_SMALL;
}
maximumNumber = 1000000000;
}
if (!leadingZeros && (value == 0)) {
buffer[(*printedSize)++] = '0';
return RETURN_OK;
}
while (maximumNumber >= 1) {
uint8_t number = value / maximumNumber;
value = value - (number * maximumNumber);
if (!leadingZeros && number == 0) {
maximumNumber /= 10;
} else {
leadingZeros = true;
buffer[(*printedSize)++] = '0' + number;
maximumNumber /= 10;
}
}
return RETURN_OK;
}
ReturnValue_t AsciiConverter::printSignedInteger(uint8_t* buffer,
uint32_t bufferLength, int32_t value, uint32_t *printedSize) {
bool negative = false;
if ((bufferLength > 0) && (value < 0)) {
*buffer++ = '-';
bufferLength--;
value = -value;
negative = true;
}
ReturnValue_t result = printInteger(buffer, bufferLength, value,
printedSize);
if (negative) {
(*printedSize)++;
}
return result;
}
int8_t AsciiConverter::convertHexChar(const uint8_t* character) {
if ((*character > 0x60) && (*character < 0x67)) {
return *character - 0x61 + 10;
} else if ((*character > 0x40) && (*character < 0x47)) {
return *character - 0x41 + 10;
} else if ((*character > 0x2F) && (*character < 0x3A)) {
return *character - 0x30;
} else if (*character == ' ') {
return -2;
}
return -1;
}
template ReturnValue_t AsciiConverter::scanAsciiDecimalNumber<float>(
const uint8_t** dataPtr, uint8_t len, float* value);
template ReturnValue_t AsciiConverter::scanAsciiDecimalNumber<uint8_t>(
const uint8_t** dataPtr, uint8_t len, uint8_t* value);
template ReturnValue_t AsciiConverter::scanAsciiDecimalNumber<uint16_t>(
const uint8_t** dataPtr, uint8_t len, uint16_t* value);
template ReturnValue_t AsciiConverter::scanAsciiDecimalNumber<double>(
const uint8_t** dataPtr, uint8_t len, double* value);
const uint8_t* AsciiConverter::clearSpace(const uint8_t* data, uint8_t len) {
while (len > 0) {
if ((*data != ' ') && (*data != 0x0a) && (*data != 0x0d)) {
return data;
}
data++;
len--;
}
return data;
}