mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-11 02:18:29 -04:00
HAL_AVR: updates for new scheduler API
This commit is contained in:
parent
f0f5761e8d
commit
0ade02a624
libraries/AP_HAL_AVR
@ -70,13 +70,13 @@ public:
|
||||
AP_HAL::AnalogSource* channel(int16_t n);
|
||||
|
||||
protected:
|
||||
static ADCSource* _create_channel(int16_t num);
|
||||
static void _register_channel(ADCSource*);
|
||||
static void _timer_event(uint32_t);
|
||||
static ADCSource* _channels[AVR_INPUT_MAX_CHANNELS];
|
||||
static int16_t _num_channels;
|
||||
static int16_t _active_channel;
|
||||
static uint16_t _channel_repeat_count;
|
||||
ADCSource* _create_channel(int16_t num);
|
||||
void _register_channel(ADCSource*);
|
||||
void _timer_event(void);
|
||||
ADCSource* _channels[AVR_INPUT_MAX_CHANNELS];
|
||||
int16_t _num_channels;
|
||||
int16_t _active_channel;
|
||||
uint16_t _channel_repeat_count;
|
||||
|
||||
private:
|
||||
ADCSource _vcc;
|
||||
|
@ -16,19 +16,14 @@ extern const AP_HAL::HAL& hal;
|
||||
* This seems to be determined empirically */
|
||||
#define CHANNEL_READ_REPEAT 2
|
||||
|
||||
/* Static variable instances */
|
||||
ADCSource* AVRAnalogIn::_channels[AVR_INPUT_MAX_CHANNELS] = {NULL};
|
||||
int16_t AVRAnalogIn::_num_channels = 0;
|
||||
int16_t AVRAnalogIn::_active_channel = 0;
|
||||
uint16_t AVRAnalogIn::_channel_repeat_count = 0;
|
||||
|
||||
AVRAnalogIn::AVRAnalogIn() :
|
||||
_vcc(ADCSource(ANALOG_INPUT_BOARD_VCC))
|
||||
{}
|
||||
|
||||
void AVRAnalogIn::init(void* machtnichts) {
|
||||
void AVRAnalogIn::init(void* machtnichts)
|
||||
{
|
||||
/* Register AVRAnalogIn::_timer_event with the scheduler. */
|
||||
hal.scheduler->register_timer_process(_timer_event);
|
||||
hal.scheduler->register_timer_process(reinterpret_cast<AP_HAL::TimedProc>(&AVRAnalogIn::_timer_event), this);
|
||||
/* Register each private channel with AVRAnalogIn. */
|
||||
_register_channel(&_vcc);
|
||||
}
|
||||
@ -62,7 +57,7 @@ void AVRAnalogIn::_register_channel(ADCSource* ch) {
|
||||
}
|
||||
}
|
||||
|
||||
void AVRAnalogIn::_timer_event(uint32_t t)
|
||||
void AVRAnalogIn::_timer_event(void)
|
||||
{
|
||||
if (_channels[_active_channel]->_pin == ANALOG_INPUT_NONE) {
|
||||
_channels[_active_channel]->new_sample(0);
|
||||
|
@ -25,6 +25,7 @@ volatile bool AVRScheduler::_timer_suspended = false;
|
||||
volatile bool AVRScheduler::_timer_event_missed = false;
|
||||
volatile bool AVRScheduler::_in_timer_proc = false;
|
||||
AP_HAL::TimedProc AVRScheduler::_timer_proc[AVR_SCHEDULER_MAX_TIMER_PROCS] = {NULL};
|
||||
void *AVRScheduler::_timer_arg[AVR_SCHEDULER_MAX_TIMER_PROCS] = {NULL};
|
||||
uint8_t AVRScheduler::_num_timer_procs = 0;
|
||||
|
||||
|
||||
@ -88,7 +89,8 @@ void AVRScheduler::register_delay_callback(AP_HAL::Proc proc,
|
||||
_min_delay_cb_ms = min_time_ms;
|
||||
}
|
||||
|
||||
void AVRScheduler::register_timer_process(AP_HAL::TimedProc proc) {
|
||||
void AVRScheduler::register_timer_process(AP_HAL::TimedProc proc, void *arg)
|
||||
{
|
||||
for (int i = 0; i < _num_timer_procs; i++) {
|
||||
if (_timer_proc[i] == proc) {
|
||||
return;
|
||||
@ -100,21 +102,22 @@ void AVRScheduler::register_timer_process(AP_HAL::TimedProc proc) {
|
||||
* because that memory won't be used until _num_timer_procs is
|
||||
* incremented. */
|
||||
_timer_proc[_num_timer_procs] = proc;
|
||||
_timer_arg[_num_timer_procs] = arg;
|
||||
/* _num_timer_procs is used from interrupt, and multiple bytes long. */
|
||||
uint8_t sreg = SREG;
|
||||
cli();
|
||||
_num_timer_procs++;
|
||||
sei();
|
||||
SREG = sreg;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void AVRScheduler::register_io_process(AP_HAL::TimedProc proc)
|
||||
void AVRScheduler::register_io_process(AP_HAL::TimedProc proc, void *arg)
|
||||
{
|
||||
// IO processes not supported on AVR
|
||||
}
|
||||
|
||||
void AVRScheduler::register_timer_failsafe(
|
||||
AP_HAL::TimedProc failsafe, uint32_t period_us) {
|
||||
void AVRScheduler::register_timer_failsafe(AP_HAL::TimedProc failsafe, uint32_t period_us) {
|
||||
/* XXX Assert period_us == 1000 */
|
||||
_failsafe = failsafe;
|
||||
}
|
||||
@ -150,7 +153,6 @@ void AVRScheduler::_timer_isr_event() {
|
||||
|
||||
void AVRScheduler::_run_timer_procs(bool called_from_isr) {
|
||||
|
||||
uint32_t tnow = _timer.micros();
|
||||
if (_in_timer_proc) {
|
||||
// the timer calls took longer than the period of the
|
||||
// timer. This is bad, and may indicate a serious
|
||||
@ -163,7 +165,7 @@ void AVRScheduler::_run_timer_procs(bool called_from_isr) {
|
||||
// block. If it does then we will recurse and die when
|
||||
// we run out of stack
|
||||
if (_failsafe != NULL) {
|
||||
_failsafe(tnow);
|
||||
_failsafe(NULL);
|
||||
}
|
||||
return;
|
||||
}
|
||||
@ -174,7 +176,7 @@ void AVRScheduler::_run_timer_procs(bool called_from_isr) {
|
||||
// now call the timer based drivers
|
||||
for (int i = 0; i < _num_timer_procs; i++) {
|
||||
if (_timer_proc[i] != NULL) {
|
||||
_timer_proc[i](tnow);
|
||||
_timer_proc[i](_timer_arg[i]);
|
||||
}
|
||||
}
|
||||
} else if (called_from_isr) {
|
||||
@ -183,7 +185,7 @@ void AVRScheduler::_run_timer_procs(bool called_from_isr) {
|
||||
|
||||
// and the failsafe, if one is setup
|
||||
if (_failsafe != NULL) {
|
||||
_failsafe(tnow);
|
||||
_failsafe(NULL);
|
||||
}
|
||||
|
||||
_in_timer_proc = false;
|
||||
|
@ -31,8 +31,8 @@ public:
|
||||
void delay_microseconds(uint16_t us);
|
||||
void register_delay_callback(AP_HAL::Proc, uint16_t min_time_ms);
|
||||
|
||||
void register_timer_process(AP_HAL::TimedProc);
|
||||
void register_io_process(AP_HAL::TimedProc);
|
||||
void register_timer_process(AP_HAL::TimedProc, void *);
|
||||
void register_io_process(AP_HAL::TimedProc, void *);
|
||||
void suspend_timer_procs();
|
||||
void resume_timer_procs();
|
||||
|
||||
@ -65,6 +65,7 @@ private:
|
||||
static volatile bool _timer_suspended;
|
||||
static volatile bool _timer_event_missed;
|
||||
static AP_HAL::TimedProc _timer_proc[AVR_SCHEDULER_MAX_TIMER_PROCS];
|
||||
static void *_timer_arg[AVR_SCHEDULER_MAX_TIMER_PROCS];
|
||||
static uint8_t _num_timer_procs;
|
||||
|
||||
};
|
||||
|
@ -30,7 +30,7 @@ void delay_toggle() {
|
||||
hal.gpio->write(DELAY_TOGGLE_PIN, 0);
|
||||
}
|
||||
|
||||
void failsafe_toggle(uint32_t machtnichts) {
|
||||
void failsafe_toggle(void *) {
|
||||
volatile int i;
|
||||
hal.gpio->write(FAILSAFE_TOGGLE_PIN, 1);
|
||||
for (i = 0; i < 10; i++);
|
||||
@ -38,21 +38,21 @@ void failsafe_toggle(uint32_t machtnichts) {
|
||||
}
|
||||
|
||||
|
||||
void schedule_toggle_1(uint32_t machtnichts) {
|
||||
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(uint32_t machtnichts) {
|
||||
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(uint32_t machtnichts) {
|
||||
void schedule_toggle_hang(void *) {
|
||||
hal.gpio->write(SCHEDULED_TOGGLE_PIN_2, 1);
|
||||
for(;;);
|
||||
}
|
||||
@ -95,8 +95,8 @@ void setup (void) {
|
||||
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->register_timer_process(schedule_toggle_1, NULL);
|
||||
hal.scheduler->register_timer_process(schedule_toggle_2, NULL);
|
||||
|
||||
hal.scheduler->delay(100);
|
||||
|
||||
@ -105,7 +105,7 @@ void setup (void) {
|
||||
"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->register_timer_process(schedule_toggle_hang);
|
||||
hal.scheduler->register_timer_process(schedule_toggle_hang, NULL);
|
||||
}
|
||||
|
||||
void loop (void) { }
|
||||
|
@ -82,15 +82,19 @@ void setup (void) {
|
||||
hal.scheduler->panic(PSTR("Error: No SPIDeviceDriver semaphore!"));
|
||||
}
|
||||
|
||||
hal.scheduler->register_timer_process(async_blinker);
|
||||
hal.scheduler->register_timer_process(async_blinker, NULL);
|
||||
}
|
||||
|
||||
|
||||
uint32_t async_last_run = 0;
|
||||
void async_blinker(uint32_t millis) {
|
||||
if (async_last_run - millis < 5) {
|
||||
static uint32_t async_last_run;
|
||||
|
||||
void async_blinker(void *)
|
||||
{
|
||||
uint32_t now = hal.scheduler->millis();
|
||||
if ((now - async_last_run) < 5) {
|
||||
return;
|
||||
}
|
||||
async_last_run = now;
|
||||
|
||||
if (sem->take_nonblocking()) {
|
||||
blink_a0();
|
||||
|
Loading…
Reference in New Issue
Block a user