mirror of https://github.com/ArduPilot/ardupilot
40 lines
1.0 KiB
C++
40 lines
1.0 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 "vprintf.h"
|
||
|
|
#include <stdio.h>
|
||
|
|
#include <stdlib.h>
|
||
|
|
|
||
|
|
void 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);
|
||
|
|
}
|
||
|
|
#endif
|