flight/modules/CallbackTest/callbacktest.c

   1 /**
   2  ******************************************************************************
   3  *
   4  * @file       callbacktest.c
   5  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
   6  * @brief      Example module to be used as a template for actual modules.
   7  *             Event callback version.
   8  *
   9  * @see        The GNU Public License (GPL) Version 3
  10  *
  11  *****************************************************************************/
  12 /*
  13  * This program is free software; you can redistribute it and/or modify
  14  * it under the terms of the GNU General Public License as published by
  15  * the Free Software Foundation; either version 3 of the License, or
  16  * (at your option) any later version.
  17  *
  18  * This program is distributed in the hope that it will be useful, but
  19  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  20  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  21  * for more details.
  22  *
  23  * You should have received a copy of the GNU General Public License along
  24  * with this program; if not, write to the Free Software Foundation, Inc.,
  25  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  26  */
  27
  28 /**
  29  *
  30  * This is a test suite to test the callback scheduler,
  31  * including its forward scheduling ability
  32  *
  33  */
  34
  35 #include "openpilot.h"
  36
  37 // Private constants
  38 #define STACK_SIZE        configMINIMAL_STACK_SIZE
  39 #define CALLBACK_PRIORITY CALLBACK_PRIORITY_LOW
  40 #define TASK_PRIORITY     CALLBACK_TASKPRIORITY_AUXILIARY
  41 // Private types
  42
  43 // #define DEBUGPRINT(...) fprintf (stderr, __VA_ARGS__)
  44 #define DEBUGPRINT(...) xSemaphoreTakeRecursive(mutex, portMAX_DELAY); fprintf(stderr, __VA_ARGS__); xSemaphoreGiveRecursive(mutex);
  45
  46 static xSemaphoreHandle mutex;
  47
  48 // Private variables
  49 static DelayedCallbackInfo *cbinfo[10];
  50
  51 static volatile int32_t counter[10] = { 0 };
  52
  53 static void DelayedCb0();
  54 static void DelayedCb1();
  55 static void DelayedCb2();
  56 static void DelayedCb3();
  57 static void DelayedCb4();
  58 static void DelayedCb5();
  59 static void DelayedCb6();
  60 /**
  61  * Initialise the module, called on startup.
  62  * \returns 0 on success or -1 if initialisation failed
  63  */
  64 int32_t CallbackTestInitialize()
  65 {
  66     mutex     = xSemaphoreCreateRecursiveMutex();
  67
  68     cbinfo[0] = PIOS_CALLBACKSCHEDULER_Create(&DelayedCb0, CALLBACK_PRIORITY_LOW, tskIDLE_PRIORITY + 2, -1, STACK_SIZE);
  69     cbinfo[1] = PIOS_CALLBACKSCHEDULER_Create(&DelayedCb1, CALLBACK_PRIORITY_LOW, tskIDLE_PRIORITY + 2, -1, STACK_SIZE);
  70     cbinfo[2] = PIOS_CALLBACKSCHEDULER_Create(&DelayedCb2, CALLBACK_PRIORITY_CRITICAL, tskIDLE_PRIORITY + 2, -1, STACK_SIZE);
  71     cbinfo[3] = PIOS_CALLBACKSCHEDULER_Create(&DelayedCb3, CALLBACK_PRIORITY_CRITICAL, tskIDLE_PRIORITY + 2, -1, STACK_SIZE);
  72     cbinfo[4] = PIOS_CALLBACKSCHEDULER_Create(&DelayedCb4, CALLBACK_PRIORITY_LOW, tskIDLE_PRIORITY + 2, -1, STACK_SIZE);
  73     cbinfo[5] = PIOS_CALLBACKSCHEDULER_Create(&DelayedCb5, CALLBACK_PRIORITY_LOW, tskIDLE_PRIORITY + 2, -1, STACK_SIZE);
  74     cbinfo[6] = PIOS_CALLBACKSCHEDULER_Create(&DelayedCb6, CALLBACK_PRIORITY_LOW, tskIDLE_PRIORITY + 20, -1, STACK_SIZE);
  75
  76
  77     return 0;
  78 }
  79 int32_t CallbackTestStart()
  80 {
  81     xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
  82     PIOS_CALLBACKSCHEDULER_Dispatch(cbinfo[3]);
  83     PIOS_CALLBACKSCHEDULER_Dispatch(cbinfo[2]);
  84     PIOS_CALLBACKSCHEDULER_Dispatch(cbinfo[1]);
  85     PIOS_CALLBACKSCHEDULER_Dispatch(cbinfo[0]);
  86     // different callback priorities within a taskpriority
  87     PIOS_CALLBACKSCHEDULER_Schedule(cbinfo[4], 30000, CALLBACK_UPDATEMODE_NONE);
  88     PIOS_CALLBACKSCHEDULER_Schedule(cbinfo[4], 5000, CALLBACK_UPDATEMODE_OVERRIDE);
  89     PIOS_CALLBACKSCHEDULER_Schedule(cbinfo[4], 4000, CALLBACK_UPDATEMODE_SOONER);
  90     PIOS_CALLBACKSCHEDULER_Schedule(cbinfo[4], 10000, CALLBACK_UPDATEMODE_SOONER);
  91     PIOS_CALLBACKSCHEDULER_Schedule(cbinfo[4], 1000, CALLBACK_UPDATEMODE_LATER);
  92     PIOS_CALLBACKSCHEDULER_Schedule(cbinfo[4], 4800, CALLBACK_UPDATEMODE_LATER);
  93     PIOS_CALLBACKSCHEDULER_Schedule(cbinfo[4], 48000, CALLBACK_UPDATEMODE_NONE);
  94     // should be at 4.8 seconds after this, allowing for exactly 9 prints of the following
  95     PIOS_CALLBACKSCHEDULER_Schedule(cbinfo[5], 500, CALLBACK_UPDATEMODE_NONE);
  96     // delayed counter with 500 ms
  97     PIOS_CALLBACKSCHEDULER_Dispatch(cbinfo[6]);
  98     // high task prio
  99     xSemaphoreGiveRecursive(mutex);
 100     return 0;
 101 }
 102
 103 static void DelayedCb0()
 104 {
 105     DEBUGPRINT("delayed counter low prio 0 updated: %i\n", counter[0]);
 106     if (++counter[0] < 10) {
 107         PIOS_CALLBACKSCHEDULER_Dispatch(cbinfo[0]);
 108     }
 109 }
 110 static void DelayedCb1()
 111 {
 112     DEBUGPRINT("delayed counter low prio 1 updated: %i\n", counter[1]);
 113     if (++counter[1] < 10) {
 114         PIOS_CALLBACKSCHEDULER_Dispatch(cbinfo[1]);
 115     }
 116 }
 117 static void DelayedCb2()
 118 {
 119     DEBUGPRINT("delayed counter high prio 2 updated: %i\n", counter[2]);
 120     if (++counter[2] < 10) {
 121         PIOS_CALLBACKSCHEDULER_Dispatch(cbinfo[2]);
 122     }
 123 }
 124 static void DelayedCb3()
 125 {
 126     DEBUGPRINT("delayed counter high prio 3 updated: %i\n", counter[3]);
 127     if (++counter[3] < 10) {
 128         PIOS_CALLBACKSCHEDULER_Dispatch(cbinfo[3]);
 129     }
 130 }
 131 static void DelayedCb4()
 132 {
 133     DEBUGPRINT("delayed scheduled callback 4 reached!\n");
 134     exit(0);
 135 }
 136 static void DelayedCb5()
 137 {
 138     DEBUGPRINT("delayed scheduled counter 5 updated: %i\n", counter[5]);
 139     if (++counter[5] < 10) {
 140         PIOS_CALLBACKSCHEDULER_Schedule(cbinfo[5], 500, CALLBACK_UPDATEMODE_NONE);
 141     }
 142     // it will likely only reach 8 due to cb4 aborting the run
 143 }
 144 static void DelayedCb6()
 145 {
 146     DEBUGPRINT("delayed counter 6 (high task prio) updated: %i\n", counter[6]);
 147     if (++counter[6] < 10) {
 148         PIOS_CALLBACKSCHEDULER_Dispatch(cbinfo[6]);
 149     }
 150 }