mirror of https://github.com/ArduPilot/ardupilot
67 lines
1.7 KiB
C++
67 lines
1.7 KiB
C++
// -*- Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
/*
|
|
* vprintf for SITL
|
|
*
|
|
* This firmware is free software; you can redistribute it and/or
|
|
* modify it under the terms of the GNU Lesser General Public
|
|
* License as published by the Free Software Foundation; either
|
|
* version 2.1 of the License, or (at your option) any later version.
|
|
*/
|
|
|
|
#include <AP_HAL.h>
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
|
|
|
|
#include <AP_Progmem.h>
|
|
#include <stdarg.h>
|
|
#include <string.h>
|
|
#include "print_vprintf.h"
|
|
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
|
|
/*
|
|
implement vprintf with support for %S meaning a progmem string
|
|
*/
|
|
void print_vprintf(AP_HAL::Print *s, unsigned char in_progmem, const char *fmt, va_list ap)
|
|
{
|
|
char *str = NULL;
|
|
int i;
|
|
char *fmt2 = strdup(fmt);
|
|
for (i=0; fmt2[i]; i++) {
|
|
// cope with %S
|
|
if (fmt2[i] == '%' && fmt2[i+1] == 'S') {
|
|
fmt2[i+1] = 's';
|
|
}
|
|
}
|
|
(void)vasprintf(&str, fmt2, ap);
|
|
for (i=0; str[i]; i++) {
|
|
s->write(str[i]);
|
|
}
|
|
free(str);
|
|
free(fmt2);
|
|
}
|
|
|
|
|
|
/*
|
|
implement vsnprintf with support for %S meaning a progmem string
|
|
*/
|
|
int print_vsnprintf(char *s, size_t n, const char *fmt, va_list ap)
|
|
{
|
|
int i, ret;
|
|
char *fmt2 = (char *)fmt;
|
|
if (strstr(fmt2, "%S") != NULL) {
|
|
fmt2 = strdup(fmt);
|
|
for (i=0; fmt2[i]; i++) {
|
|
// cope with %S
|
|
if (fmt2[i] == '%' && fmt2[i+1] == 'S') {
|
|
fmt2[i+1] = 's';
|
|
}
|
|
}
|
|
}
|
|
ret = vsnprintf(s, n, fmt2, ap);
|
|
if (fmt2 != fmt) {
|
|
free(fmt2);
|
|
}
|
|
return ret;
|
|
}
|
|
#endif
|