Quadrotor from scratch
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
 
 
 
 
 

75 lines
1.4 KiB

#include "event.h"
#include "interrupt.h"
#include "types.h"
event_handler *event_handler_table[EVENT_MAX+1];
#define EVENT_WORDS ((EVENT_MAX+1+31)/32)
unsigned int event_pending[EVENT_WORDS];
#define EVENT_WORDLEN 32
#define EVENT_WORD(x) (x/EVENT_WORDLEN)
#define EVENT_BIT(x) (x%EVENT_WORDLEN)
#define EVENT_MASK(x) (1<<EVENT_BIT(x))
void event_set(unsigned int event)
{
if (event > EVENT_MAX)
return;
interrupt_block();
event_pending[EVENT_WORD(event)] |= EVENT_MASK(event);
interrupt_unblock();
}
static int bitset(unsigned int x)
{
x = 0xffffffff - (x-1);
x = x - ((x >> 1) & 0x55555555);
x = (x & 0x33333333) + ((x >> 2) & 0x33333333);
x = (x + (x >> 4)) & 0x0F0F0F0F;
x = x + (x << 8);
x = x + (x << 16);
return 32 - (x >> 24);
}
bool event_get(unsigned int *event)
{
int i;
unsigned int p;
for (i = 0; i < EVENT_WORDS; i++) {
if ((p = event_pending[i]) != 0) {
p = p & (-p);
*event = bitset(p) + EVENT_WORDLEN*i;
return TRUE;
}
}
return FALSE;
}
void event_clear(unsigned int event)
{
interrupt_block();
event_pending[EVENT_WORD(event)] &= ~EVENT_MASK(event);
interrupt_unblock();
}
void event_dispatch(void)
{
unsigned int event;
if (event_get(&event)) {
event_clear(event);
if (event_handler_table[event] != NULL)
(event_handler_table[event])();
}
}
void event_register(unsigned int event, event_handler *handler)
{
if (event > EVENT_MAX)
return;
event_handler_table[event] = handler;
}