mirror of https://github.com/ArduPilot/ardupilot
58 lines
1.5 KiB
C++
58 lines
1.5 KiB
C++
|
|
#include "AP_TimerProcess.h"
|
|
|
|
extern "C" {
|
|
#include <inttypes.h>
|
|
#include <stdint.h>
|
|
#include "WConstants.h"
|
|
#include <avr/interrupt.h>
|
|
}
|
|
|
|
int AP_TimerProcess::_period;
|
|
ap_procedure AP_TimerProcess::_proc[AP_TIMERPROCESS_MAX_PROCS];
|
|
int AP_TimerProcess::_pidx = 0;
|
|
|
|
AP_TimerProcess::AP_TimerProcess(int period)
|
|
{
|
|
_period = period;
|
|
}
|
|
|
|
void AP_TimerProcess::init( Arduino_Mega_ISR_Registry * isr_reg )
|
|
{
|
|
// Enable Timer2 Overflow interrupt to trigger process.
|
|
TIMSK2 = 0; // Disable interrupts
|
|
TCCR2A = 0; // normal counting mode
|
|
TCCR2B = _BV(CS21) | _BV(CS22); // Set prescaler of clk/256
|
|
TCNT2 = 0; // Set count to zero, so it goes off right away.
|
|
TIFR2 = _BV(TOV2); // clear pending interrupts;
|
|
TIMSK2 = _BV(TOIE2); // enable the overflow interrupt
|
|
|
|
for (int i = 0; i < AP_TIMERPROCESS_MAX_PROCS; i++)
|
|
_proc[i] = NULL;
|
|
|
|
isr_reg->register_signal( ISR_REGISTRY_TIMER2_OVF, AP_TimerProcess::run);
|
|
}
|
|
|
|
void AP_TimerProcess::register_process(void (*proc)(void) )
|
|
{
|
|
if (_pidx < AP_TIMERPROCESS_MAX_PROCS)
|
|
_proc[_pidx++] = proc;
|
|
}
|
|
|
|
void AP_TimerProcess::run(void)
|
|
{
|
|
// we re-enable interrupts here as some of these functions
|
|
// take a long time, and we don't want to block other critical
|
|
// interrupts, especially the serial interrupt
|
|
sei();
|
|
|
|
for (int i = 0; i < _pidx; i++) {
|
|
if (_proc[i] != NULL)
|
|
_proc[i]();
|
|
}
|
|
|
|
// to prevent recursion, we don't enable the timer interrupt
|
|
// again until we've completed the tasks
|
|
TCNT2 = _period;
|
|
}
|