ardupilot/libraries/AP_HAL/Util.cpp

49 lines
1.1 KiB
C++
Raw Normal View History

#include "AP_HAL.h"
#include "Util.h"
#include "utility/print_vprintf.h"
/* Helper class implements AP_HAL::Print so we can use utility/vprintf */
class BufferPrinter : public AP_HAL::Print {
public:
BufferPrinter(char* str, size_t size) : _offs(0), _str(str), _size(size) {}
size_t write(uint8_t c) {
if (_offs < _size) {
_str[_offs] = c;
_offs++;
return 1;
} else {
return 0;
}
}
size_t write(const uint8_t *buffer, size_t size) {
size_t n = 0;
while (size--) {
n += write(*buffer++);
}
return n;
}
size_t _offs;
char* const _str;
const size_t _size;
};
int AP_HAL::Util::snprintf(char* str, size_t size, const char *format, ...)
{
va_list ap;
va_start(ap, format);
int res = vsnprintf(str, size, format, ap);
va_end(ap);
return res;
}
int AP_HAL::Util::vsnprintf(char* str, size_t size, const char *format, va_list ap)
{
BufferPrinter buf(str, size);
print_vprintf(&buf, format, ap);
// null terminate if possible
int ret = buf._offs;
buf.write(0);
return ret;
}