mirror of https://github.com/ArduPilot/ardupilot
AP_HAL:examples:Printf: Improvements in the Printf example
This commit is contained in:
parent
e13a35abc1
commit
18f56fbac5
|
@ -1,18 +1,40 @@
|
||||||
|
/*
|
||||||
|
This program is free software: you can redistribute it and/or modify
|
||||||
|
it under the terms of the GNU General Public License as published by
|
||||||
|
the Free Software Foundation, either version 3 of the License, or
|
||||||
|
(at your option) any later version.
|
||||||
|
|
||||||
|
This program is distributed in the hope that it will be useful,
|
||||||
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||||
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||||
|
GNU General Public License for more details.
|
||||||
|
|
||||||
|
You should have received a copy of the GNU General Public License
|
||||||
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||||
|
*/
|
||||||
|
/*
|
||||||
|
Printf.cpp: We demonstrate the use of the printf() and snprintf() functions
|
||||||
|
*/
|
||||||
#include <AP_Common/AP_Common.h>
|
#include <AP_Common/AP_Common.h>
|
||||||
#include <AP_HAL/AP_HAL.h>
|
#include <AP_HAL/AP_HAL.h>
|
||||||
|
|
||||||
void setup();
|
void setup(); // declaration of the setup() function
|
||||||
void loop();
|
void loop(); // declaration of the loop() function
|
||||||
|
|
||||||
const AP_HAL::HAL& hal = AP_HAL::get_HAL();
|
const AP_HAL::HAL& hal = AP_HAL::get_HAL(); // create a reference to AP_HAL::HAL object to get access to hardware specific functions. For more info see <https://ardupilot.org/dev/docs/learning-ardupilot-the-example-sketches.html/>
|
||||||
|
|
||||||
|
// the setup function runs once when the board powers up
|
||||||
void setup(void) {
|
void setup(void) {
|
||||||
hal.console->printf("Starting Printf test\n");
|
hal.console->printf("Starting Printf test\n"); // print a starting message
|
||||||
}
|
}
|
||||||
|
|
||||||
|
//create a struct array float_tests
|
||||||
static const struct {
|
static const struct {
|
||||||
|
// holds the char sequence representing the format specifier
|
||||||
const char *fmt;
|
const char *fmt;
|
||||||
|
// holds a float value
|
||||||
float v;
|
float v;
|
||||||
|
// holds the char sequence that would be printed in accordance with the format specifier defined(fmt)
|
||||||
const char *result;
|
const char *result;
|
||||||
} float_tests[] = {
|
} float_tests[] = {
|
||||||
{ "%f", 3.71f, "3.710000" },
|
{ "%f", 3.71f, "3.710000" },
|
||||||
|
@ -47,16 +69,20 @@ static const struct {
|
||||||
{ "%.1f", 10.6f, "10.6" },
|
{ "%.1f", 10.6f, "10.6" },
|
||||||
};
|
};
|
||||||
|
|
||||||
|
// test_printf_floats(void) : tests the printf() and snprintf() function against the float numbers defined in float_tests
|
||||||
static void test_printf_floats(void)
|
static void test_printf_floats(void)
|
||||||
{
|
{
|
||||||
hal.console->printf("Starting Printf floats test\n");
|
hal.console->printf("Starting Printf floats test\n");
|
||||||
|
|
||||||
uint8_t i;
|
uint8_t i;
|
||||||
|
// 30 bytes long char buffer(Expected length of string : 29)
|
||||||
char buf[30];
|
char buf[30];
|
||||||
uint8_t failures = 0;
|
uint8_t failures = 0;
|
||||||
hal.console->printf("Running printf tests\n");
|
hal.console->printf("Running printf tests\n");
|
||||||
for (i=0; i < ARRAY_SIZE(float_tests); i++) {
|
for (i=0; i < ARRAY_SIZE(float_tests); i++) {
|
||||||
int ret = hal.util->snprintf(buf, sizeof(buf), float_tests[i].fmt, (double)float_tests[i].v);
|
// create a format string(buf) in accordance with the format specifier and the float values defined for every element of float_tests[]
|
||||||
|
int ret = hal.util->snprintf(buf, sizeof(buf), float_tests[i].fmt, (double)float_tests[i].v); //For more info, see : http://www.cplusplus.com/reference/cstdio/snprintf/
|
||||||
|
// comparing the results of the snprintf() and results defined in the float_tests[] :
|
||||||
|
// 1. check whether the strings are equal or not
|
||||||
if (strcmp(buf, float_tests[i].result) != 0) {
|
if (strcmp(buf, float_tests[i].result) != 0) {
|
||||||
hal.console->printf("Failed float_tests[%u] '%s' -> '%s' should be '%s'\n",
|
hal.console->printf("Failed float_tests[%u] '%s' -> '%s' should be '%s'\n",
|
||||||
(unsigned)i,
|
(unsigned)i,
|
||||||
|
@ -65,6 +91,7 @@ static void test_printf_floats(void)
|
||||||
float_tests[i].result);
|
float_tests[i].result);
|
||||||
failures++;
|
failures++;
|
||||||
}
|
}
|
||||||
|
// 2. check whether the len of the strings is equal or not
|
||||||
if (ret != (int)strlen(float_tests[i].result)) {
|
if (ret != (int)strlen(float_tests[i].result)) {
|
||||||
hal.console->printf("Failed float_tests[%u] ret=%d/%d '%s' should be '%s'\n",
|
hal.console->printf("Failed float_tests[%u] ret=%d/%d '%s' should be '%s'\n",
|
||||||
(unsigned)i,
|
(unsigned)i,
|
||||||
|
@ -77,33 +104,26 @@ static void test_printf_floats(void)
|
||||||
hal.console->printf("%u failures\n", (unsigned)failures);
|
hal.console->printf("%u failures\n", (unsigned)failures);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// test_printf_null_termination(void) : tests the printf() and the snprintf() function against a char sequence and whether they consider the terminating null character '\0'
|
||||||
static void test_printf_null_termination(void)
|
static void test_printf_null_termination(void)
|
||||||
{
|
{
|
||||||
hal.console->printf("Starting Printf null-termination tests\n");
|
hal.console->printf("Starting Printf null-termination tests\n");
|
||||||
|
// 10 bytes long char buffer(Expected length of string : 9)
|
||||||
{
|
|
||||||
char buf[10];
|
char buf[10];
|
||||||
int ret = hal.util->snprintf(buf,sizeof(buf), "%s", "ABCDEABCDE");
|
// create a format string(buf) in accordance with "ABCDEABCDE" and length of buf[]
|
||||||
|
int ret = hal.util->snprintf(buf,sizeof(buf), "%s", "ABCDEABCDE"); //For more info, see : http://www.cplusplus.com/reference/cstdio/snprintf/
|
||||||
|
// store the expected length of string
|
||||||
const int want = 9;
|
const int want = 9;
|
||||||
|
// comparing the results of the snprintf() function :
|
||||||
|
// 1. check whether the expected length of the string is equal to the buffer(buf) length or not
|
||||||
if (ret != want) {
|
if (ret != want) {
|
||||||
hal.console->printf("snprintf returned %d expected %d\n", ret, want);
|
hal.console->printf("snprintf returned %d expected %d\n", ret, want);
|
||||||
}
|
}
|
||||||
|
// 2. check whether the buffer(buf) is equal to "ABCDEABCD" or not
|
||||||
if (!strncmp(buf, "ABCDEABCD", sizeof(buf))) {
|
if (!strncmp(buf, "ABCDEABCD", sizeof(buf))) {
|
||||||
hal.console->printf("Bad snprintf string (%s)\n", buf);
|
hal.console->printf("Bad snprintf string (%s)\n", buf);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
{
|
|
||||||
char buf[10];
|
|
||||||
int ret = hal.util->snprintf(buf,sizeof(buf), "ABCDEABCDE");
|
|
||||||
const int want = 9;
|
|
||||||
if (ret != want) {
|
|
||||||
hal.console->printf("snprintf returned %d expected %d\n", ret, want);
|
|
||||||
}
|
|
||||||
if (!strncmp(buf, "ABCDEABCD", sizeof(buf))) {
|
|
||||||
hal.console->printf("Bad snprintf string (%s)\n", buf);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
static void test_printf(void)
|
static void test_printf(void)
|
||||||
{
|
{
|
||||||
|
@ -111,10 +131,12 @@ static void test_printf(void)
|
||||||
test_printf_null_termination();
|
test_printf_null_termination();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// the loop function runs over and over again forever
|
||||||
void loop(void)
|
void loop(void)
|
||||||
{
|
{
|
||||||
test_printf();
|
test_printf();
|
||||||
|
// give a delay of 1000ms or 1s
|
||||||
hal.scheduler->delay(1000);
|
hal.scheduler->delay(1000);
|
||||||
}
|
}
|
||||||
|
|
||||||
AP_HAL_MAIN();
|
AP_HAL_MAIN(); // HAL Macro that declares the main function. For more info see <https://ardupilot.org/dev/docs/learning-ardupilot-the-example-sketches.html/>
|
||||||
|
|
Loading…
Reference in New Issue