Switch crtp service and platform service to be tasks

These services respond to request and therefore should use the blocking version of sendPacket that waits if the outgoing queue is full. There are two other files that use the CRTP callback functionality (commander and location service), but neither of them requires blocking calls. Part of fixes for issue #681.
bitcraze · Feb 6, 2021 · d6fe422 · d6fe422
1 parent 6b4cb3a
commit d6fe422
Show file tree

Hide file tree

Showing 3 changed files with 79 additions and 49 deletions.
diff --git a/src/config/config.h b/src/config/config.h
@@ -102,6 +102,8 @@
 #define ACTIVE_MARKER_TASK_PRI  3
 #define AI_DECK_TASK_PRI        3
 #define UART2_TASK_PRI          3
+#define CRTP_SRV_TASK_PRI       0
+#define PLATFORM_SRV_TASK_PRI   0
 
 // Not compiled
 #if 0
@@ -150,6 +152,8 @@
 #define AI_DECK_GAP_TASK_NAME   "AI-DECK-GAP"
 #define AI_DECK_NINA_TASK_NAME  "AI-DECK-NINA"
 #define UART2_TASK_NAME         "UART2"
+#define CRTP_SRV_TASK_NAME      "CRTP-SRV"
+#define PLATFORM_SRV_TASK_NAME  "PLATFORM-SRV"
 
 //Task stack sizes
 #define SYSTEM_TASK_STACKSIZE         (2* configMINIMAL_STACK_SIZE)
@@ -182,6 +186,8 @@
 #define ACTIVEMARKER_TASK_STACKSIZE   configMINIMAL_STACK_SIZE
 #define AI_DECK_TASK_STACKSIZE        configMINIMAL_STACK_SIZE
 #define UART2_TASK_STACKSIZE          configMINIMAL_STACK_SIZE
+#define CRTP_SRV_TASK_STACKSIZE       configMINIMAL_STACK_SIZE
+#define PLATFORM_SRV_TASK_STACKSIZE   configMINIMAL_STACK_SIZE
 
 //The radio channel. From 0 to 125
 #define RADIO_CHANNEL 80

diff --git a/src/modules/src/crtpservice.c b/src/modules/src/crtpservice.c
@@ -7,7 +7,7 @@
  *
  * Crazyflie control firmware
  *
- * Copyright (C) 2011-2012 Bitcraze AB
+ * Copyright (C) 2011-2021 Bitcraze AB
  *
  * This program is free software: you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -29,27 +29,32 @@
 
 /* FreeRtos includes */
 #include "FreeRTOS.h"
+#include "task.h"
 
 #include "crtp.h"
 #include "crtpservice.h"
+#include "static_mem.h"
 
-static bool isInit=false;
 
 typedef enum {
   linkEcho   = 0x00,
   linkSource = 0x01,
   linkSink   = 0x02,
 } LinkNbr;
 
-void crtpserviceHandler(CRTPPacket *p);
+
+static bool isInit=false;
+STATIC_MEM_TASK_ALLOC_STACK_NO_DMA_CCM_SAFE(crtpSrvTask, CRTP_SRV_TASK_STACKSIZE);
+
+static void crtpSrvTask(void*);
 
 void crtpserviceInit(void)
 {
   if (isInit)
     return;
 
-  // Register a callback to service the Link port
-  crtpRegisterPortCB(CRTP_PORT_LINK, crtpserviceHandler);
+  //Start the task
+  STATIC_MEM_TASK_CREATE(crtpSrvTask, crtpSrvTask, CRTP_SRV_TASK_NAME, NULL, CRTP_SRV_TASK_PRI);
 
   isInit = true;
 }
@@ -59,23 +64,31 @@ bool crtpserviceTest(void)
   return isInit;
 }
 
-void crtpserviceHandler(CRTPPacket *p)
+static void crtpSrvTask(void* prm)
 {
-  switch (p->channel)
-  {
-    case linkEcho:
-      crtpSendPacket(p);
-      break;
-    case linkSource:
-      p->size = CRTP_MAX_DATA_SIZE;
-      bzero(p->data, CRTP_MAX_DATA_SIZE);
-      strcpy((char*)p->data, "Bitcraze Crazyflie");
-      crtpSendPacket(p);
-      break;
-    case linkSink:
-      /* Ignore packet */
-      break;
-    default:
-      break;
+  static CRTPPacket p;
+
+  crtpInitTaskQueue(CRTP_PORT_LINK);
+
+  while(1) {
+    crtpReceivePacketBlock(CRTP_PORT_LINK, &p);
+
+    switch (p.channel)
+    {
+      case linkEcho:
+        crtpSendPacketBlock(&p);
+        break;
+      case linkSource:
+        p.size = CRTP_MAX_DATA_SIZE;
+        bzero(p.data, CRTP_MAX_DATA_SIZE);
+        strcpy((char*)p.data, "Bitcraze Crazyflie");
+        crtpSendPacketBlock(&p);
+        break;
+      case linkSink:
+        /* Ignore packet */
+        break;
+      default:
+        break;
+    }
   }
 }
diff --git a/src/modules/src/platformservice.c b/src/modules/src/platformservice.c
@@ -7,7 +7,7 @@
  *
  * Crazyflie control firmware
  *
- * Copyright (C) 2011-2012 Bitcraze AB
+ * Copyright (C) 2011-2021 Bitcraze AB
  *
  * This program is free software: you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -21,15 +21,16 @@
  * You should have received a copy of the GNU General Public License
  * along with this program. If not, see <http://www.gnu.org/licenses/>.
  *
- * crtpservice.c - Implements low level services for CRTP
+ * platformservice.c - Implements platform services for CRTP
  */
 
 #include <stdbool.h>
+#include <stdint.h>
+#include <string.h>
 
 /* FreeRtos includes */
 #include "FreeRTOS.h"
-#include <stdint.h>
-#include <string.h>
+#include "task.h"
 
 #include "config.h"
 #include "crtp.h"
@@ -38,8 +39,10 @@
 #include "version.h"
 #include "platform.h"
 #include "app_channel.h"
+#include "static_mem.h"
 
 static bool isInit=false;
+STATIC_MEM_TASK_ALLOC_STACK_NO_DMA_CCM_SAFE(platformSrvTask, PLATFORM_SRV_TASK_STACKSIZE);
 
 typedef enum {
   platformCommand   = 0x00,
@@ -57,7 +60,7 @@ typedef enum {
   getDeviceTypeName  = 0x02,
 } VersionCommand;
 
-void platformserviceHandler(CRTPPacket *p);
+static void platformSrvTask(void*);
 static void platformCommandProcess(uint8_t command, uint8_t *data);
 static void versionCommandProcess(CRTPPacket *p);
 
@@ -68,8 +71,8 @@ void platformserviceInit(void)
 
   appchannelInit();
 
-  // Register a callback to service the Platform port
-  crtpRegisterPortCB(CRTP_PORT_PLATFORM, platformserviceHandler);
+  //Start the task
+  STATIC_MEM_TASK_CREATE(platformSrvTask, platformSrvTask, PLATFORM_SRV_TASK_NAME, NULL, PLATFORM_SRV_TASK_PRI);
 
   isInit = true;
 }
@@ -79,28 +82,36 @@ bool platformserviceTest(void)
   return isInit;
 }
 
-void platformserviceHandler(CRTPPacket *p)
+static void platformSrvTask(void* prm)
 {
-  switch (p->channel)
-  {
-    case platformCommand:
-      platformCommandProcess(p->data[0], &p->data[1]);
-      crtpSendPacket(p);
-      break;
-    case versionCommand:
-      versionCommandProcess(p);
-      break;
-    case appChannel:
-      appchannelIncomingPacket(p);
-      break;
-    default:
-      break;
+  static CRTPPacket p;
+
+  crtpInitTaskQueue(CRTP_PORT_PLATFORM);
+
+  while(1) {
+    crtpReceivePacketBlock(CRTP_PORT_PLATFORM, &p);
+
+    switch (p.channel)
+    {
+      case platformCommand:
+        platformCommandProcess(p.data[0], &p.data[1]);
+        crtpSendPacketBlock(&p);
+        break;
+      case versionCommand:
+        versionCommandProcess(&p);
+        break;
+      case appChannel:
+        appchannelIncomingPacket(&p);
+        break;
+      default:
+        break;
+    }
   }
 }
 
 static void platformCommandProcess(uint8_t command, uint8_t *data)
 {
-  SyslinkPacket slp;
+  static SyslinkPacket slp;
 
   switch (command) {
     case setContinousWave:
@@ -118,7 +129,7 @@ void platformserviceSendAppchannelPacket(CRTPPacket *p)
 {
   p->port = CRTP_PORT_PLATFORM;
   p->channel = appChannel;
-  crtpSendPacket(p);
+  crtpSendPacketBlock(p);
 }
 
 static void versionCommandProcess(CRTPPacket *p)
@@ -127,19 +138,19 @@ static void versionCommandProcess(CRTPPacket *p)
     case getProtocolVersion:
       *(int*)&p->data[1] = PROTOCOL_VERSION;
       p->size = 5;
-      crtpSendPacket(p);
+      crtpSendPacketBlock(p);
       break;
     case getFirmwareVersion:
       strncpy((char*)&p->data[1], V_STAG, CRTP_MAX_DATA_SIZE-1);
       p->size = (strlen(V_STAG)>CRTP_MAX_DATA_SIZE-1)?CRTP_MAX_DATA_SIZE:strlen(V_STAG)+1;
-      crtpSendPacket(p);
+      crtpSendPacketBlock(p);
       break;
     case getDeviceTypeName:
       {
       const char* name = platformConfigGetDeviceTypeName();
       strncpy((char*)&p->data[1], name, CRTP_MAX_DATA_SIZE-1);
       p->size = (strlen(name)>CRTP_MAX_DATA_SIZE-1)?CRTP_MAX_DATA_SIZE:strlen(name)+1;
-      crtpSendPacket(p);
+      crtpSendPacketBlock(p);
       }
       break;
     default: