MegaClock/rtos.c

#include "rtos.h"

/**
 * Переменные модуля
 */
static volatile task TaskArray[MAX_TASKS]; // очередь задач
static volatile uint8_t arrayTail;         // "хвост" очереди
static volatile uint8_t interruptStatus;

#define DISABLE_INTERRUPT       {interruptStatus = __save_interrupt(); __disable_interrupt();}
#define RESTORE_INTERRUPT       __restore_interrupt(interruptStatus)

// счётчик задержки
static volatile uint16_t TDelay;

/**
 * Инициализация РТОС и t_delay, время тика - 1 мс
 */
inline void RTOS_Init()
{
  // настраиваем основной таймер
  TIMER_TCCR = TIMER_CSB;
  TIMER_TCNT = TIMER_CNT;
  TIMER_TIMSK |= TIMER_INT;

  // "хвост" в 0
  arrayTail = 0;
}

/**
 * Добавление задачи в список
 */
void RTOS_SetTask (void (*taskFunc)(void), uint16_t taskDelay, uint16_t taskPeriod)
{
   uint8_t i;

   if(!taskFunc) return;
   for(i = 0; i < arrayTail; i++)               // поиск задачи в текущем списке
   {
      if(TaskArray[i].pFunc == taskFunc)        // если нашли, то обновляем переменные
      {
         DISABLE_INTERRUPT;

         TaskArray[i].delay  = taskDelay;
         TaskArray[i].period = taskPeriod;
         TaskArray[i].run    = 0;

         RESTORE_INTERRUPT;
         return;                                // обновив, выходим
      }
   }

   if (arrayTail < MAX_TASKS)                   // если такой задачи в списке нет
   {                                            // и есть место,то добавляем
      DISABLE_INTERRUPT;

      TaskArray[arrayTail].pFunc  = taskFunc;
      TaskArray[arrayTail].delay  = taskDelay;
      TaskArray[arrayTail].period = taskPeriod;
      TaskArray[arrayTail].run    = 0;

      arrayTail++;                              // увеличиваем "хвост"
      RESTORE_INTERRUPT;
   }
}

/**
 * Удаление задачи из списка
 */
inline void RTOS_DeleteTask (void (*taskFunc)(void))
{
   uint8_t i;

   for (i=0; i<arrayTail; i++)                  // проходим по списку задач
   {
      if(TaskArray[i].pFunc == taskFunc)        // если задача в списке найдена
      {

         DISABLE_INTERRUPT;
         if(i != (arrayTail - 1))               // переносим последнюю задачу
         {                                      // на место удаляемой
            TaskArray[i] = TaskArray[arrayTail - 1];
         }
         arrayTail--;                           // уменьшаем указатель "хвоста"
         RESTORE_INTERRUPT;
         return;
      }
   }
}

/**
 * Диспетчер РТОС, вызывается в main
 */

void RTOS_DispatchTask(void)
{
   uint8_t i;
   void (*function) (void);
   for (i=0; i<arrayTail; i++)                  // проходим по списку задач
   {
      if (TaskArray[i].run == 1)                // если флаг на выполнение взведен,
      {                                         // запоминаем задачу, т.к. во
         function = TaskArray[i].pFunc;         // время выполнения может
                                                // измениться индекс
         if(TaskArray[i].period == 0)
         {                                      // если период равен 0
            RTOS_DeleteTask(TaskArray[i].pFunc);// удаляем задачу из списка,

         }
         else
         {
            TaskArray[i].run = 0;               // иначе снимаем флаг запуска
            if(!TaskArray[i].delay)             // если задача не изменила задержку
            {                                   // задаем ее
               TaskArray[i].delay = TaskArray[i].period-1;
            }                                   // задача для себя может сделать паузу
         }
         (*function)();                         // выполняем задачу
      }
   }
}

/**
 * Delay, msek
 * также, что-бы зря не терять время -- крутим диспетчер
 */
void tdelay_ms(uint16_t msek) {

  TDelay = msek;

  do {
    RTOS_DispatchTask();
    // делать нехрен -- спим, ждём прерывание
    MCUCR = 1<<SE;
    __sleep();
  } while (TDelay>0);

}

/**
 * Таймерная служба РТОС и tdelay (прерывание аппаратного таймера)
 */
#pragma vector=TIMER_OVF_VECT
__interrupt void TIMER_OVF_ISR(void) {

  // перезарядим таймер
  TIMER_TCNT = TIMER_CNT;

  /* RTOS_Timer */
  uint8_t i;

  // проходим по списку задач
  for (i=0; i<arrayTail; i++) {
    // если время до выполнения истекло
    if (TaskArray[i].delay == 0) {
      // взводим флаг запуска,
      TaskArray[i].run = 1;
    } else {
      // иначе уменьшаем время
      TaskArray[i].delay--;
    }
  }

  /* TDelay_Timer -- уменьшим счётчик */
  if (TDelay > 0) {
    TDelay --;
  }

}