mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-09 09:28:31 -04:00
AP_HAL: fixed example build
This commit is contained in:
parent
a04c056598
commit
fc7f3c8aa6
@ -61,45 +61,6 @@ void stream_loopback(AP_HAL::Stream* s, uint32_t time) {
|
||||
}
|
||||
}
|
||||
|
||||
void stream_console_loopback(AP_HAL::Stream* s, AP_HAL::ConsoleDriver* c,
|
||||
uint32_t time) {
|
||||
uint32_t end = hal.scheduler->millis() + time;
|
||||
for(;;) {
|
||||
if (hal.scheduler->millis() >= end && time != 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
/* Read the uart, write to the console backend. */
|
||||
if (s->available() > 0) {
|
||||
int b;
|
||||
b = s->read();
|
||||
if (-1 != b) {
|
||||
uint8_t tmp[1];
|
||||
tmp[0] = (uint8_t) b;
|
||||
c->backend_write(tmp, 1);
|
||||
}
|
||||
}
|
||||
/* Loop back the console upon itself. */
|
||||
{
|
||||
int b;
|
||||
b = c->read();
|
||||
if (-1 != b) {
|
||||
c->write((uint8_t)b);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/* Read the console backend, print to the uart. */
|
||||
{
|
||||
uint8_t tmp[1];
|
||||
int readback = c->backend_read(tmp, 1);
|
||||
if (readback > 0) {
|
||||
s->write(tmp[0]);
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
void setup(void)
|
||||
{
|
||||
//
|
||||
@ -114,11 +75,6 @@ void setup(void)
|
||||
hal.console->println_P(PSTR("progmem"));
|
||||
hal.console->printf("printf %d %u %#x %p %f %S\n", -1000, 1000, 1000, 1000, 1.2345, PSTR("progmem"));
|
||||
hal.console->printf_P(PSTR("printf_P %d %u %#x %p %f %S\n"), -1000, 1000, 1000, 1000, 1.2345, PSTR("progmem"));
|
||||
|
||||
hal.console->println("loopback for 10sec:");
|
||||
stream_loopback(hal.console, 10000);
|
||||
hal.console->println("loopback done");
|
||||
|
||||
hal.console->println("done.");
|
||||
for(;;);
|
||||
|
||||
|
@ -41,61 +41,6 @@ const AP_HAL::HAL& hal = AP_HAL_AVR_APM1;
|
||||
#endif
|
||||
|
||||
|
||||
void stream_loopback(AP_HAL::Stream* s, uint32_t time) {
|
||||
uint32_t end = hal.scheduler->millis() + time;
|
||||
for(;;) {
|
||||
if (hal.scheduler->millis() >= end && time != 0) {
|
||||
return;
|
||||
}
|
||||
if (s->available() > 0) {
|
||||
int c;
|
||||
c = s->read();
|
||||
if (-1 != c) {
|
||||
s->write((uint8_t)c);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void stream_console_loopback(AP_HAL::Stream* s, AP_HAL::ConsoleDriver* c,
|
||||
uint32_t time) {
|
||||
uint32_t end = hal.scheduler->millis() + time;
|
||||
for(;;) {
|
||||
if (hal.scheduler->millis() >= end && time != 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
/* Read the uart, write to the console backend. */
|
||||
if (s->available() > 0) {
|
||||
int b;
|
||||
b = s->read();
|
||||
if (-1 != b) {
|
||||
uint8_t tmp[1];
|
||||
tmp[0] = (uint8_t) b;
|
||||
c->backend_write(tmp, 1);
|
||||
}
|
||||
}
|
||||
/* Loop back the console upon itself. */
|
||||
{
|
||||
int b;
|
||||
b = c->read();
|
||||
if (-1 != b) {
|
||||
c->write((uint8_t)b);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/* Read the console backend, print to the uart. */
|
||||
{
|
||||
uint8_t tmp[1];
|
||||
int readback = c->backend_read(tmp, 1);
|
||||
if (readback > 0) {
|
||||
s->write(tmp[0]);
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
void setup(void)
|
||||
{
|
||||
//
|
||||
@ -111,10 +56,6 @@ void setup(void)
|
||||
hal.console->printf("printf %d %u %#x %p %f %S\n", -1000, 1000, 1000, 1000, 1.2345, PSTR("progmem"));
|
||||
hal.console->printf_P(PSTR("printf_P %d %u %#x %p %f %S\n"), -1000, 1000, 1000, 1000, 1.2345, PSTR("progmem"));
|
||||
|
||||
hal.console->println("loopback for 10sec:");
|
||||
stream_loopback(hal.console, 10000);
|
||||
hal.console->println("loopback done");
|
||||
|
||||
for(;;);
|
||||
|
||||
}
|
||||
|
@ -20,61 +20,6 @@ const AP_HAL::HAL& hal = AP_HAL_AVR_APM2;
|
||||
const AP_HAL::HAL& hal = AP_HAL_AVR_APM1;
|
||||
#endif
|
||||
|
||||
void stream_loopback(AP_HAL::Stream* s, uint32_t time) {
|
||||
uint32_t end = hal.scheduler->millis() + time;
|
||||
for(;;) {
|
||||
if (hal.scheduler->millis() >= end && time != 0) {
|
||||
return;
|
||||
}
|
||||
if (s->available() > 0) {
|
||||
int c;
|
||||
c = s->read();
|
||||
if (-1 != c) {
|
||||
s->write((uint8_t)c);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void stream_console_loopback(AP_HAL::Stream* s, AP_HAL::ConsoleDriver* c,
|
||||
uint32_t time) {
|
||||
uint32_t end = hal.scheduler->millis() + time;
|
||||
for(;;) {
|
||||
if (hal.scheduler->millis() >= end && time != 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
/* Read the uart, write to the console backend. */
|
||||
if (s->available() > 0) {
|
||||
int b;
|
||||
b = s->read();
|
||||
if (-1 != b) {
|
||||
uint8_t tmp[1];
|
||||
tmp[0] = (uint8_t) b;
|
||||
c->backend_write(tmp, 1);
|
||||
}
|
||||
}
|
||||
/* Loop back the console upon itself. */
|
||||
{
|
||||
int b;
|
||||
b = c->read();
|
||||
if (-1 != b) {
|
||||
c->write((uint8_t)b);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/* Read the console backend, print to the uart. */
|
||||
{
|
||||
uint8_t tmp[1];
|
||||
int readback = c->backend_read(tmp, 1);
|
||||
if (readback > 0) {
|
||||
s->write(tmp[0]);
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
void setup(void)
|
||||
{
|
||||
//
|
||||
@ -90,10 +35,6 @@ void setup(void)
|
||||
hal.console->printf("printf %d %u %#x %p %f %S\n", -1000, 1000, 1000, 1000, 1.2345, PSTR("progmem"));
|
||||
hal.console->printf_P(PSTR("printf_P %d %u %#x %p %f %S\n"), -1000, 1000, 1000, 1000, 1.2345, PSTR("progmem"));
|
||||
|
||||
hal.console->println("loopback for 10sec:");
|
||||
stream_loopback(hal.console, 10000);
|
||||
hal.console->println("loopback done");
|
||||
|
||||
for(;;);
|
||||
|
||||
}
|
||||
|
@ -17,62 +17,6 @@
|
||||
|
||||
const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
|
||||
|
||||
void stream_loopback(AP_HAL::Stream* s, uint32_t time) {
|
||||
uint32_t end = hal.scheduler->millis() + time;
|
||||
for(;;) {
|
||||
if (hal.scheduler->millis() >= end && time != 0) {
|
||||
return;
|
||||
}
|
||||
if (s->available() > 0) {
|
||||
int c;
|
||||
c = s->read();
|
||||
if (-1 != c) {
|
||||
s->write((uint8_t)c);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void stream_console_loopback(AP_HAL::Stream* s, AP_HAL::ConsoleDriver* c,
|
||||
uint32_t time) {
|
||||
uint32_t end = hal.scheduler->millis() + time;
|
||||
for(;;) {
|
||||
if (hal.scheduler->millis() >= end && time != 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
/* Read the uart, write to the console backend. */
|
||||
if (s->available() > 0) {
|
||||
int b;
|
||||
b = s->read();
|
||||
if (-1 != b) {
|
||||
uint8_t tmp[1];
|
||||
tmp[0] = (uint8_t) b;
|
||||
c->backend_write(tmp, 1);
|
||||
}
|
||||
}
|
||||
|
||||
/* Loop back the console upon itself. */
|
||||
{
|
||||
int b;
|
||||
b = c->read();
|
||||
if (-1 != b) {
|
||||
c->write((uint8_t)b);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/* Read the console backend, print to the uart. */
|
||||
{
|
||||
uint8_t tmp[1];
|
||||
int readback = c->backend_read(tmp, 1);
|
||||
if (readback > 0) {
|
||||
s->write(tmp[0]);
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
void setup(void)
|
||||
{
|
||||
//
|
||||
|
Loading…
Reference in New Issue
Block a user