bool queue_push_u16(queue_t* q, uint16_t data)
{
return queue_push_value(q, &data, 2);
}
bool timer_insert_value_in_queue(timer_event* event)
{
uint16_t current_timer = hal_timer_getvalue();
DPRINT("timer_insert current_time %d", current_timer);
DPRINT(" - next %d queue len %d", event->next_event, event_queue.length);
// TODO: substract time already gone
uint8_t position = 0;
uint16_t next_event = event->next_event;
event->next_event = current_timer + event->next_event;
while (position < event_queue.length)
{
DPRINT(" - position %d queue len %d", position, event_queue.length);
timer_event *temp_event = (timer_event*) queue_read_value(&event_queue, position, sizeof(timer_event));
DPRINT(" - tempevent next_event %d event %p", temp_event->next_event, temp_event->f);
DPRINT(" - tempevent ne - ct %d", temp_event->next_event - current_timer);
if ((temp_event->next_event - current_timer) > next_event)
{
DPRINT(" - ne-ct > ne");
if (position == 0)
{
DPRINT(" - position == 0");
hal_timer_disable_interrupt();
started = false;
}
queue_insert_value(&event_queue, (void*) event, position, sizeof(timer_event));
position = 0;
break;
}
position++;
}
if (position == event_queue.length)
{
if (!queue_push_value(&event_queue, (void*) event, sizeof(timer_event)))
return false;
}
#ifdef LOG_FWK_ENABLED
position = 0;
for (;position<event_queue.length; position++)
{
timer_event *temp_event = (timer_event*) queue_read_value(&event_queue, position, sizeof(timer_event));
DPRINT("Queue: %d", temp_event->next_event);
}
DPRINT("timer_insert E - Timer current value: %d", hal_timer_getvalue());
#endif
/*
// code when timer was up instead of continous
int16_t sum_next_event = - hal_timer_getvalue();
while (position < event_queue.length)
{
timer_event *temp_event = (timer_event*) queue_read_value(&event_queue, position, sizeof(timer_event));
if (event->next_event > sum_next_event + temp_event->next_event)
{
sum_next_event += temp_event->next_event;
} else {
uint16_t elapsed = 0;
if (position == 0)
{
elapsed = hal_timer_getvalue();
hal_timer_disable_interrupt();
started = false;
} else {c
event->next_event -= sum_next_event;
}
queue_insert_value(&event_queue, (void*) event, position, sizeof(timer_event));
temp_event = (timer_event*) queue_read_value(&event_queue, position+1, sizeof(timer_event));
temp_event->next_event -= (event->next_event + elapsed);
return true;
}
position++;
}
if (position == event_queue.length)
{
if (started) event->next_event -= sum_next_event;
return queue_push_value(&event_queue, (void*) event, sizeof(timer_event));
}
*/
return true;
}
bool queue_push_u8(queue_t* q, uint8_t data)
{
return queue_push_value(q, &data, 1);
}
bool timer_insert_value_in_queue(timer_event* event)
{
uint16_t current_timer = hal_timer_getvalue();
#ifdef LOG_FWK_ENABLED
log_print_stack_string(LOG_FWK, "timer_insert S - Timer current value: %d.%d", current_timer/1000, current_timer%1000);
#endif
// TODO: substract time already gone
uint8_t position = 0;
uint16_t next_event = event->next_event;
event->next_event = current_timer + event->next_event;
while (position < event_queue.length)
{
timer_event *temp_event = (timer_event*) queue_read_value(&event_queue, position, sizeof(timer_event));
if ((temp_event->next_event - current_timer) > next_event)
{
if (position == 0)
{
hal_timer_disable_interrupt();
started = false;
}
queue_insert_value(&event_queue, (void*) event, position, sizeof(timer_event));
position = 0;
break;
}
position++;
}
if (position == event_queue.length)
{
if (!queue_push_value(&event_queue, (void*) event, sizeof(timer_event)))
return false;
}
#ifdef LOG_FWK_ENABLED
position = 0;
for (;position<event_queue.length; position++)
{
timer_event *temp_event = (timer_event*) queue_read_value(&event_queue, position, sizeof(timer_event));
log_print_stack_string(LOG_FWK, "Queue: %d.%d", temp_event->next_event/1000, temp_event->next_event%1000);
}
current_timer = hal_timer_getvalue();
log_print_stack_string(LOG_FWK, "timer_insert E - Timer current value: %d.%d", current_timer/1000, current_timer%1000);
#endif
/*
// code when timer was up instead of continous
int16_t sum_next_event = - hal_timer_getvalue();
while (position < event_queue.length)
{
timer_event *temp_event = (timer_event*) queue_read_value(&event_queue, position, sizeof(timer_event));
if (event->next_event > sum_next_event + temp_event->next_event)
{
sum_next_event += temp_event->next_event;
} else {
uint16_t elapsed = 0;
if (position == 0)
{
elapsed = hal_timer_getvalue();
hal_timer_disable_interrupt();
started = false;
} else {
event->next_event -= sum_next_event;
}
queue_insert_value(&event_queue, (void*) event, position, sizeof(timer_event));
temp_event = (timer_event*) queue_read_value(&event_queue, position+1, sizeof(timer_event));
temp_event->next_event -= (event->next_event + elapsed);
return true;
}
position++;
}
if (position == event_queue.length)
{
if (started) event->next_event -= sum_next_event;
return queue_push_value(&event_queue, (void*) event, sizeof(timer_event));
}
*/
return true;
}