HAL_AVR: use hal.console instead of hal.uart0

This commit is contained in:
Andrew Tridgell 2012-10-27 11:26:19 +11:00
parent ecd67ad79b
commit d133f15f57
8 changed files with 91 additions and 91 deletions

View File

@ -44,7 +44,7 @@ void setup()
{
hal.uart1->begin(38400);
hal.uart0->println("GPS AUTO library test");
hal.console->println("GPS AUTO library test");
gps = &GPS;
gps->init(GPS::GPS_ENGINE_AIRBORNE_2G);
}
@ -54,11 +54,11 @@ void loop()
gps->update();
if (gps->new_data) {
if (gps->fix) {
hal.uart0->print("Lat: ");
hal.console->print("Lat: ");
print_latlon(hal.uart0,gps->latitude);
hal.uart0->print(" Lon: ");
hal.console->print(" Lon: ");
print_latlon(hal.uart0,gps->longitude);
hal.uart0->printf(" Alt: %.2fm GSP: %.2fm/s CoG: %d SAT: %d TIM: %lu STATUS: %u\n",
hal.console->printf(" Alt: %.2fm GSP: %.2fm/s CoG: %d SAT: %d TIM: %lu STATUS: %u\n",
(float)gps->altitude / 100.0,
(float)gps->ground_speed / 100.0,
(int)gps->ground_course / 100,
@ -66,7 +66,7 @@ void loop()
gps->time,
gps->status());
} else {
hal.uart0->println("No fix");
hal.console->println("No fix");
}
gps->new_data = false;
}

View File

@ -25,7 +25,7 @@ void setup()
hal.uart1->begin(38400);
gps.print_errors = true;
hal.uart0->println("GPS MTK library test");
hal.console->println("GPS MTK library test");
gps.init(); // GPS Initialization
}
@ -34,24 +34,24 @@ void loop()
hal.scheduler->delay(20);
gps.update();
if (gps.new_data) {
hal.uart0->print("gps:");
hal.uart0->print(" Lat:");
hal.uart0->print((float)gps.latitude / T7, DEC);
hal.uart0->print(" Lon:");
hal.uart0->print((float)gps.longitude / T7, DEC);
hal.uart0->print(" Alt:");
hal.uart0->print((float)gps.altitude / 100.0, DEC);
hal.uart0->print(" GSP:");
hal.uart0->print(gps.ground_speed / 100.0);
hal.uart0->print(" COG:");
hal.uart0->print(gps.ground_course / 100.0, DEC);
hal.uart0->print(" SAT:");
hal.uart0->print(gps.num_sats, DEC);
hal.uart0->print(" FIX:");
hal.uart0->print(gps.fix, DEC);
hal.uart0->print(" TIM:");
hal.uart0->print(gps.time, DEC);
hal.uart0->println();
hal.console->print("gps:");
hal.console->print(" Lat:");
hal.console->print((float)gps.latitude / T7, DEC);
hal.console->print(" Lon:");
hal.console->print((float)gps.longitude / T7, DEC);
hal.console->print(" Alt:");
hal.console->print((float)gps.altitude / 100.0, DEC);
hal.console->print(" GSP:");
hal.console->print(gps.ground_speed / 100.0);
hal.console->print(" COG:");
hal.console->print(gps.ground_course / 100.0, DEC);
hal.console->print(" SAT:");
hal.console->print(gps.num_sats, DEC);
hal.console->print(" FIX:");
hal.console->print(gps.fix, DEC);
hal.console->print(" TIM:");
hal.console->print(gps.time, DEC);
hal.console->println();
gps.new_data = 0; // We have readed the data
}
}

View File

@ -25,7 +25,7 @@ void setup()
hal.uart1->begin(38400);
gps.print_errors = true;
hal.uart0->println("GPS UBLOX library test");
hal.console->println("GPS UBLOX library test");
gps.init(GPS::GPS_ENGINE_AIRBORNE_2G); // GPS Initialization
}
@ -34,28 +34,28 @@ void loop()
hal.scheduler->delay(20);
gps.update();
if (gps.new_data) {
hal.uart0->print("gps:");
hal.uart0->print(" Lat:");
hal.uart0->print((float)gps.latitude / T7, DEC);
hal.uart0->print(" Lon:");
hal.uart0->print((float)gps.longitude / T7, DEC);
hal.uart0->print(" Alt:");
hal.uart0->print((float)gps.altitude / 100.0, DEC);
hal.uart0->print(" GSP:");
hal.uart0->print(gps.ground_speed / 100.0);
hal.uart0->print(" COG:");
hal.uart0->print(gps.ground_course / 100.0, DEC);
hal.uart0->printf(" VEL: %.2f %.2f %.2f",
hal.console->print("gps:");
hal.console->print(" Lat:");
hal.console->print((float)gps.latitude / T7, DEC);
hal.console->print(" Lon:");
hal.console->print((float)gps.longitude / T7, DEC);
hal.console->print(" Alt:");
hal.console->print((float)gps.altitude / 100.0, DEC);
hal.console->print(" GSP:");
hal.console->print(gps.ground_speed / 100.0);
hal.console->print(" COG:");
hal.console->print(gps.ground_course / 100.0, DEC);
hal.console->printf(" VEL: %.2f %.2f %.2f",
gps.velocity_north(),
gps.velocity_east(),
sqrt(sq(gps.velocity_north())+sq(gps.velocity_east())));
hal.uart0->print(" SAT:");
hal.uart0->print(gps.num_sats, DEC);
hal.uart0->print(" FIX:");
hal.uart0->print(gps.fix, DEC);
hal.uart0->print(" TIM:");
hal.uart0->print(gps.time, DEC);
hal.uart0->println();
hal.console->print(" SAT:");
hal.console->print(gps.num_sats, DEC);
hal.console->print(" FIX:");
hal.console->print(gps.fix, DEC);
hal.console->print(" TIM:");
hal.console->print(gps.time, DEC);
hal.console->println();
gps.new_data = 0; // We have readed the data
}
}

View File

@ -12,7 +12,7 @@ AP_HAL::AnalogSource* ch1;
AP_HAL::AnalogSource* vdd;
void setup (void) {
hal.uart0->printf_P(PSTR("Starting AP_HAL_AVR::AnalogIn test\r\n"));
hal.console->printf_P(PSTR("Starting AP_HAL_AVR::AnalogIn test\r\n"));
ch0 = hal.analogin->channel(0);
ch1 = hal.analogin->channel(1);
vdd = hal.analogin->channel(2);
@ -22,7 +22,7 @@ void loop (void) {
float meas_ch0 = ch0->read();
float meas_ch1 = ch1->read();
float meas_vdd = vdd->read();
hal.uart0->printf_P(PSTR("read ch0: %f, ch1: %f, vdd: %f\r\n"),
hal.console->printf_P(PSTR("read ch0: %f, ch1: %f, vdd: %f\r\n"),
meas_ch0, meas_ch1, meas_vdd);
hal.scheduler->delay(10);
}

View File

@ -10,14 +10,14 @@ const AP_HAL::HAL& hal = AP_HAL_AVR_APM2;
bool dbg = true;
void erase() {
hal.uart0->println("Erasing dataflash");
hal.console->println("Erasing dataflash");
hal.dataflash->erase_all();
}
void readback_page(int i);
void single_page_readwrite() {
hal.uart0->println("Writing simple sequence to page 1");
hal.console->println("Writing simple sequence to page 1");
hal.dataflash->start_write(1);
hal.dataflash->write_byte(1);
hal.dataflash->write_byte(2);
@ -34,7 +34,7 @@ void single_page_readwrite() {
}
void readback_page(int i) {
hal.uart0->printf_P(PSTR("Reading back sequence from page %d\r\n"), i);
hal.console->printf_P(PSTR("Reading back sequence from page %d\r\n"), i);
hal.dataflash->start_read(i);
uint8_t v1 = hal.dataflash->read_byte();
uint8_t v2 = hal.dataflash->read_byte();
@ -43,12 +43,12 @@ void readback_page(int i) {
uint8_t v5 = hal.dataflash->read_byte();
uint8_t v6 = hal.dataflash->read_byte();
uint8_t v7 = hal.dataflash->read_byte();
hal.uart0->printf_P(PSTR("vals on page %d: %d %d %d %d %d 0x%x 0x%x\r\n"),
hal.console->printf_P(PSTR("vals on page %d: %d %d %d %d %d 0x%x 0x%x\r\n"),
i, (int) v1, (int) v2, (int) v3, (int) v4, (int) v5,
(int) v6, (int) v7);
}
void two_page_readwrite() {
hal.uart0->println("Writing simple sequence to page 1");
hal.console->println("Writing simple sequence to page 1");
hal.dataflash->start_write(1);
hal.dataflash->write_byte(1);
hal.dataflash->write_byte(2);
@ -64,7 +64,7 @@ void two_page_readwrite() {
hal.scheduler->delay(100);
readback_page(1);
hal.uart0->println("Writing simple sequence to page 2");
hal.console->println("Writing simple sequence to page 2");
hal.dataflash->start_write(2);
hal.dataflash->write_byte(21);
hal.dataflash->write_byte(22);
@ -99,9 +99,9 @@ void two_page_readwrite() {
void longtest_write() {
// We start to write some info (sequentialy) starting from page 1
// This is similar to what we will do...
hal.uart0->println("After testing perform erase before using hal.dataflash->for logging!");
hal.uart0->println("");
hal.uart0->println("Writing to flash... wait...");
hal.console->println("After testing perform erase before using hal.dataflash->for logging!");
hal.console->println("");
hal.console->println("Writing to flash... wait...");
hal.dataflash->start_write(1);
for (int i = 0; i < 40; i++) {
// Write 1000 packets...
@ -126,7 +126,7 @@ void longtest_readback()
uint8_t tmp_byte1, tmp_byte2;
long tmp_long;
hal.uart0->println("Start reading page 1...");
hal.console->println("Start reading page 1...");
hal.dataflash->start_read(1); // We start reading from page 1
for (int i = 0; i < 40; i++) { // Read 200 packets...
@ -148,7 +148,7 @@ void longtest_readback()
int8_t cs1 = hal.dataflash->read_byte();
int8_t cs2 = hal.dataflash->read_byte();
hal.uart0->printf_P(PSTR("sync 0x%x 0x%x ints %d %d %d %d, "
hal.console->printf_P(PSTR("sync 0x%x 0x%x ints %d %d %d %d, "
"longs %d %d, cksm %d %d\r\n"),
(int) sync1, (int) sync2,
w1, w2, w3, w4,
@ -163,14 +163,14 @@ void setup()
{
hal.dataflash->init(NULL);
hal.uart0->println("Dataflash Log Test 1.0");
hal.console->println("Dataflash Log Test 1.0");
hal.scheduler->delay(20);
hal.uart0->print("Manufacturer:");
hal.uart0->print((int)hal.dataflash->mfg_id());
hal.uart0->print(",");
hal.uart0->print((int)hal.dataflash->device_id());
hal.uart0->println();
hal.console->print("Manufacturer:");
hal.console->print((int)hal.dataflash->mfg_id());
hal.console->print(",");
hal.console->print((int)hal.dataflash->device_id());
hal.console->println();
erase();

View File

@ -23,16 +23,16 @@ void setup(void)
//
// Test printing things
//
hal.uart0->print("test");
hal.uart0->println(" begin");
hal.uart0->println(1000);
hal.uart0->println(1000, 8);
hal.uart0->println(1000, 10);
hal.uart0->println(1000, 16);
hal.uart0->println_P(PSTR("progmem"));
hal.uart0->printf("printf %d %u %#x %p %f %S\n", -1000, 1000, 1000, 1000, 1.2345, PSTR("progmem"));
hal.uart0->printf_P(PSTR("printf_P %d %u %#x %p %f %S\n"), -1000, 1000, 1000, 1000, 1.2345, PSTR("progmem"));
hal.uart0->println("done");
hal.console->print("test");
hal.console->println(" begin");
hal.console->println(1000);
hal.console->println(1000, 8);
hal.console->println(1000, 10);
hal.console->println(1000, 16);
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("done");
}
void loop(void)

View File

@ -52,7 +52,7 @@ void schedule_toggle_hang(uint32_t machtnichts) {
}
void setup_pin(int pin_num) {
hal.uart0->printf_P(PSTR("Setup pin %d\r\n"), pin_num);
hal.console->printf_P(PSTR("Setup pin %d\r\n"), pin_num);
hal.gpio->pinMode(pin_num,GPIO_OUTPUT);
/* Blink so we can see setup on the logic analyzer.*/
hal.gpio->write(pin_num,1);
@ -60,14 +60,14 @@ void setup_pin(int pin_num) {
}
void setup (void) {
hal.uart0->printf_P(PSTR("Starting AP_HAL_AVR::Scheduler test\r\n"));
hal.console->printf_P(PSTR("Starting AP_HAL_AVR::Scheduler test\r\n"));
setup_pin(DELAY_TOGGLE_PIN);
setup_pin(FAILSAFE_TOGGLE_PIN);
setup_pin(SCHEDULED_TOGGLE_PIN_1);
setup_pin(SCHEDULED_TOGGLE_PIN_2);
hal.uart0->printf_P(PSTR("Testing delay callback. "
hal.console->printf_P(PSTR("Testing delay callback. "
"Pin %d should toggle at 1khz:\r\n"),
(int) DELAY_TOGGLE_PIN);
@ -75,7 +75,7 @@ void setup (void) {
hal.scheduler->delay(100);
hal.uart0->printf_P(PSTR("Testing failsafe callback. "
hal.console->printf_P(PSTR("Testing failsafe callback. "
"Pin %d should toggle at 1khz:\r\n"),
(int) FAILSAFE_TOGGLE_PIN);
@ -83,10 +83,10 @@ void setup (void) {
hal.scheduler->delay(100);
hal.uart0->printf_P(PSTR("Testing running timer processes.\r\n"));
hal.uart0->printf_P(PSTR("Pin %d should toggle at 1khz.\r\n"),
hal.console->printf_P(PSTR("Testing running timer processes.\r\n"));
hal.console->printf_P(PSTR("Pin %d should toggle at 1khz.\r\n"),
(int) SCHEDULED_TOGGLE_PIN_1);
hal.uart0->printf_P(PSTR("Pin %d should toggle at 1khz.\r\n"),
hal.console->printf_P(PSTR("Pin %d should toggle at 1khz.\r\n"),
(int) SCHEDULED_TOGGLE_PIN_2);
hal.scheduler->register_timer_process(schedule_toggle_1, 1000, 0);
@ -94,10 +94,10 @@ void setup (void) {
hal.scheduler->delay(100);
hal.uart0->printf_P(PSTR("Test running a pathological timer process.\r\n"
hal.console->printf_P(PSTR("Test running a pathological timer process.\r\n"
"Failsafe should continue even as pathological process "
"dominates the processor."));
hal.uart0->printf_P(PSTR("Pin %d should toggle then go high forever.\r\n"),
hal.console->printf_P(PSTR("Pin %d should toggle then go high forever.\r\n"),
(int) SCHEDULED_TOGGLE_PIN_2);
hal.scheduler->register_timer_process(schedule_toggle_hang, 1000, 0);
}

View File

@ -10,39 +10,39 @@ const AP_HAL::HAL& hal = AP_HAL_AVR_APM1;
uint8_t fibs[12] = { 1, 1, 2, 3, 5, 8, 13, 21, 34, 55, 89, 144 };
void test_erase() {
hal.uart0->printf_P(PSTR("erasing... "));
hal.console->printf_P(PSTR("erasing... "));
for(int i = 0; i < 100; i++) {
hal.storage->write_byte((uint8_t*)i, 0);
}
hal.uart0->printf_P(PSTR(" done.\r\n"));
hal.console->printf_P(PSTR(" done.\r\n"));
}
void test_write() {
hal.uart0->printf_P(PSTR("writing... "));
hal.console->printf_P(PSTR("writing... "));
hal.storage->write_block(fibs, (uint8_t*)0, 12);
hal.uart0->printf_P(PSTR(" done.\r\n"));
hal.console->printf_P(PSTR(" done.\r\n"));
}
void test_readback() {
hal.uart0->printf_P(PSTR("reading back...\r\n"));
hal.console->printf_P(PSTR("reading back...\r\n"));
uint8_t readback[12];
bool success = true;
hal.storage->read_block(readback, (uint8_t*)0, 12);
for (int i = 0; i < 12; i++) {
if (readback[i] != fibs[i]) {
success = false;
hal.uart0->printf_P(PSTR("At index %d expected %d got %d\r\n"),
hal.console->printf_P(PSTR("At index %d expected %d got %d\r\n"),
i, (int) fibs[i], (int) readback[i]);
}
}
if (success) {
hal.uart0->printf_P(PSTR("all bytes read successfully\r\n"));
hal.console->printf_P(PSTR("all bytes read successfully\r\n"));
}
hal.uart0->printf_P(PSTR("done reading back.\r\n"));
hal.console->printf_P(PSTR("done reading back.\r\n"));
}
void setup (void) {
hal.uart0->printf_P(PSTR("Starting AP_HAL_AVR::Storage test\r\n"));
hal.console->printf_P(PSTR("Starting AP_HAL_AVR::Storage test\r\n"));
test_erase();
test_write();
test_readback();