mirror of https://github.com/ArduPilot/ardupilot
117 lines
3.4 KiB
C++
117 lines
3.4 KiB
C++
|
|
#include <AP_Common.h>
|
|
#include <AP_Math.h>
|
|
#include <AP_Param.h>
|
|
#include <AP_Progmem.h>
|
|
|
|
#include <AP_HAL.h>
|
|
#include <AP_HAL_FLYMAPLE.h>
|
|
|
|
const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
|
|
|
|
/**
|
|
* You'll want to use a logic analyzer to watch the effects of this test.
|
|
* On the APM2 its pretty easy to hook up an analyzer to pins A0 through A3.
|
|
*/
|
|
#define DELAY_TOGGLE_PIN 15 /* A0 = pin 15 */
|
|
#define FAILSAFE_TOGGLE_PIN 16 /* A1 = pin 16 */
|
|
#define SCHEDULED_TOGGLE_PIN_1 17 /* A2 = pin 17 */
|
|
#define SCHEDULED_TOGGLE_PIN_2 18 /* A3 = pin 18 */
|
|
|
|
void delay_toggle() {
|
|
volatile int i;
|
|
hal.gpio->write(DELAY_TOGGLE_PIN, 1);
|
|
for (i = 0; i < 10; i++);
|
|
hal.gpio->write(DELAY_TOGGLE_PIN, 0);
|
|
}
|
|
|
|
void failsafe_toggle(void) {
|
|
volatile int i;
|
|
hal.gpio->write(FAILSAFE_TOGGLE_PIN, 1);
|
|
for (i = 0; i < 10; i++);
|
|
hal.gpio->write(FAILSAFE_TOGGLE_PIN, 0);
|
|
}
|
|
|
|
|
|
void schedule_toggle_1(void) {
|
|
volatile int i;
|
|
hal.gpio->write(SCHEDULED_TOGGLE_PIN_1, 1);
|
|
for (i = 0; i < 10; i++);
|
|
hal.gpio->write(SCHEDULED_TOGGLE_PIN_1, 0);
|
|
}
|
|
|
|
void schedule_toggle_2(void) {
|
|
volatile int i;
|
|
hal.gpio->write(SCHEDULED_TOGGLE_PIN_2, 1);
|
|
for (i = 0; i < 10; i++);
|
|
hal.gpio->write(SCHEDULED_TOGGLE_PIN_2, 0);
|
|
}
|
|
|
|
void schedule_toggle_hang(void) {
|
|
hal.gpio->write(SCHEDULED_TOGGLE_PIN_2, 1);
|
|
for(;;);
|
|
}
|
|
|
|
void setup_pin(int pin_num) {
|
|
hal.console->printf_P(PSTR("Setup pin %d\r\n"), pin_num);
|
|
hal.gpio->pinMode(pin_num,HAL_GPIO_OUTPUT);
|
|
/* Blink so we can see setup on the logic analyzer.*/
|
|
hal.gpio->write(pin_num,1);
|
|
hal.gpio->write(pin_num,0);
|
|
}
|
|
|
|
void setup (void) {
|
|
// hal.scheduler->delay(5000);
|
|
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.console->printf_P(PSTR("Testing delay callback. "
|
|
"Pin %d should toggle at 1khz:\r\n"),
|
|
(int) DELAY_TOGGLE_PIN);
|
|
|
|
// hal.scheduler->register_delay_callback(delay_toggle,0);
|
|
|
|
hal.scheduler->delay(2000);
|
|
hal.console->printf_P(PSTR("Testing failsafe callback. "
|
|
"Pin %d should toggle at 1khz:\r\n"),
|
|
(int) FAILSAFE_TOGGLE_PIN);
|
|
|
|
hal.scheduler->register_timer_failsafe(failsafe_toggle, 1000);
|
|
|
|
hal.scheduler->delay(2000);
|
|
|
|
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.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);
|
|
hal.scheduler->register_timer_process(schedule_toggle_2);
|
|
|
|
hal.scheduler->delay(2000);
|
|
|
|
// not yet working on flymaple: see FLYMAPLEScheduler::_timer_procs_timer_event()
|
|
#if 1
|
|
hal.console->printf_P(PSTR("Test running a pathological timer process.\r\n"
|
|
"Failsafe should continue even as pathological process "
|
|
"dominates the processor."));
|
|
hal.console->printf_P(PSTR("Pin %d should toggle then go high forever.\r\n"),
|
|
(int) SCHEDULED_TOGGLE_PIN_2);
|
|
hal.scheduler->delay(200);
|
|
hal.scheduler->register_timer_process(schedule_toggle_hang);
|
|
#endif
|
|
|
|
// Wait a little then test reboot
|
|
// hal.scheduler->delay(5000);
|
|
// hal.scheduler->reboot();
|
|
}
|
|
|
|
void loop (void) { }
|
|
|
|
AP_HAL_MAIN();
|