mirror of https://github.com/ArduPilot/ardupilot
HAL_PX4: handle %S in format strings
This commit is contained in:
parent
c16db01395
commit
30447018d5
|
@ -74,23 +74,23 @@ void PX4UARTDriver::println_P(const prog_char_t *pstr) {
|
||||||
void PX4UARTDriver::printf(const char *fmt, ...) {
|
void PX4UARTDriver::printf(const char *fmt, ...) {
|
||||||
va_list ap;
|
va_list ap;
|
||||||
va_start(ap, fmt);
|
va_start(ap, fmt);
|
||||||
vdprintf(_fd, fmt, ap);
|
_vdprintf(_fd, fmt, ap);
|
||||||
va_end(ap);
|
va_end(ap);
|
||||||
}
|
}
|
||||||
|
|
||||||
void PX4UARTDriver::_printf_P(const prog_char *fmt, ...) {
|
void PX4UARTDriver::_printf_P(const prog_char *fmt, ...) {
|
||||||
va_list ap;
|
va_list ap;
|
||||||
va_start(ap, fmt);
|
va_start(ap, fmt);
|
||||||
vdprintf(_fd, fmt, ap);
|
_vdprintf(_fd, fmt, ap);
|
||||||
va_end(ap);
|
va_end(ap);
|
||||||
}
|
}
|
||||||
|
|
||||||
void PX4UARTDriver::vprintf(const char *fmt, va_list ap) {
|
void PX4UARTDriver::vprintf(const char *fmt, va_list ap) {
|
||||||
vdprintf(_fd, fmt, ap);
|
_vdprintf(_fd, fmt, ap);
|
||||||
}
|
}
|
||||||
|
|
||||||
void PX4UARTDriver::vprintf_P(const prog_char *fmt, va_list ap) {
|
void PX4UARTDriver::vprintf_P(const prog_char *fmt, va_list ap) {
|
||||||
vdprintf(_fd, fmt, ap);
|
_vdprintf(_fd, fmt, ap);
|
||||||
}
|
}
|
||||||
|
|
||||||
/* PX4 implementations of Stream virtual methods */
|
/* PX4 implementations of Stream virtual methods */
|
||||||
|
@ -143,5 +143,24 @@ size_t PX4UARTDriver::write(uint8_t c) {
|
||||||
return ::write(_fd, &c, 1);
|
return ::write(_fd, &c, 1);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// handle %S -> %s
|
||||||
|
void PX4UARTDriver::_vdprintf(int fd, const char *fmt, va_list ap)
|
||||||
|
{
|
||||||
|
if (strstr(fmt, "%S")) {
|
||||||
|
char *fmt2 = strdup(fmt);
|
||||||
|
if (fmt2 != NULL) {
|
||||||
|
for (uint16_t i=0; fmt2[i]; i++) {
|
||||||
|
if (fmt2[i] == '%' && fmt2[i+1] == 'S') {
|
||||||
|
fmt2[i+1] = 's';
|
||||||
|
}
|
||||||
|
}
|
||||||
|
vdprintf(fd, fmt2, ap);
|
||||||
|
free(fmt2);
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
vdprintf(fd, fmt, ap);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
#endif // CONFIG_HAL_BOARD
|
#endif // CONFIG_HAL_BOARD
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue