mirror of https://github.com/ArduPilot/ardupilot
AP_PeriodicProcess: AP_TimerProcess can register multiple callbacks.
This commit is contained in:
parent
67436d7ff2
commit
a7b9c8b9cd
|
@ -34,6 +34,8 @@ void AP_TimerAperiodicProcess::run(void)
|
|||
_timer_offset = (_timer_offset + 49) % 32;
|
||||
_period = TCNT2_781_HZ + _timer_offset;
|
||||
TCNT2 = _period;
|
||||
if (_proc != NULL)
|
||||
_proc();
|
||||
for (int i = 0; i < _pidx; i++) {
|
||||
if (_proc[i] != NULL)
|
||||
_proc[i]();
|
||||
}
|
||||
}
|
||||
|
|
|
@ -8,12 +8,12 @@ extern "C" {
|
|||
}
|
||||
|
||||
int AP_TimerProcess::_period;
|
||||
void (*AP_TimerProcess::_proc)(void);
|
||||
ap_procedure AP_TimerProcess::_proc[AP_TIMERPROCESS_MAX_PROCS];
|
||||
int AP_TimerProcess::_pidx = 0;
|
||||
|
||||
AP_TimerProcess::AP_TimerProcess(int period)
|
||||
{
|
||||
_period = period;
|
||||
_proc = NULL;
|
||||
}
|
||||
|
||||
void AP_TimerProcess::init( Arduino_Mega_ISR_Registry * isr_reg )
|
||||
|
@ -26,18 +26,23 @@ void AP_TimerProcess::init( Arduino_Mega_ISR_Registry * isr_reg )
|
|||
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) )
|
||||
{
|
||||
_proc = proc;
|
||||
TCNT2 = 1; // Should go off almost immediately.
|
||||
if (_pidx < AP_TIMERPROCESS_MAX_PROCS)
|
||||
_proc[_pidx++] = proc;
|
||||
}
|
||||
|
||||
void AP_TimerProcess::run(void)
|
||||
{
|
||||
TCNT2 = _period;
|
||||
if (_proc != NULL)
|
||||
_proc();
|
||||
for (int i = 0; i < _pidx; i++) {
|
||||
if (_proc[i] != NULL)
|
||||
_proc[i]();
|
||||
}
|
||||
}
|
||||
|
|
|
@ -8,6 +8,8 @@
|
|||
/* XXX this value is a total guess, will look up. */
|
||||
#define TIMERPROCESS_PER_DEFAULT (256)
|
||||
|
||||
#define AP_TIMERPROCESS_MAX_PROCS 3
|
||||
|
||||
class AP_TimerProcess : public AP_PeriodicProcess
|
||||
{
|
||||
public:
|
||||
|
@ -16,8 +18,9 @@ class AP_TimerProcess : public AP_PeriodicProcess
|
|||
void register_process(void (* proc)(void));
|
||||
static void run(void);
|
||||
protected:
|
||||
static int _period;
|
||||
static void (*_proc)(void);
|
||||
static int _period;
|
||||
static ap_procedure _proc[AP_TIMERPROCESS_MAX_PROCS];
|
||||
static int _pidx;
|
||||
};
|
||||
|
||||
#endif // __AP_TIMERPROCESS_H__
|
||||
|
|
|
@ -2,6 +2,8 @@
|
|||
#ifndef __PERIODICPROCESS_H__
|
||||
#define __PERIODICPROCESS_H__
|
||||
|
||||
typedef void (*ap_procedure)(void);
|
||||
|
||||
class AP_PeriodicProcess
|
||||
{
|
||||
public:
|
||||
|
|
Loading…
Reference in New Issue