Merge branch 'fix/cfclient_ui' into 'master'

fix: communication with cfclient See merge request ae_group/esp-college/esp-drone!39
espressif · Aug 13, 2024 · 9438f50 · 9438f50
2 parents f1178fb + 9a14c4a
commit 9438f50
Show file tree

Hide file tree

Showing 21 changed files with 186 additions and 300 deletions.
diff --git a/README.md b/README.md
@@ -23,7 +23,7 @@ For more information, please check the sections below:
 2. Height-hold Mode
 3. Position-hold Mode
 4. APP Control
-5. CFclient Supported
+5. cfclient Supported, refer https://github.com/leeebo/crazyflie-clients-python
 6. ESP-BOX3 Joystick Control (through esp-now)
 
 Note: to implement Height-hold/Position-hold mode, extension boards are needed. For more information, see Hardware Reference. 
@@ -34,7 +34,7 @@ Additional third party copyrighted code is included under the following licenses
 
 | Component | License | Origin |Commit ID |
 | :---:  | :---: | :---: |:---: |
-| core/crazyflie | GPL3.0  |[Crazyflie](https://github.com/bitcraze/crazyflie-firmware) |tag_2021_01 b448553|
+| core/crazyflie | GPL3.0  |[Crazyflie](https://github.com/bitcraze/crazyflie-firmware/tree/2021.01) |tag_2021_01 b448553|
 | lib/dsp_lib |  | [esp32-lin](https://github.com/whyengineer/esp32-lin/tree/master/components/dsp_lib) |6fa39f4c|
 
 ### Support Policy

diff --git a/README_cn.md b/README_cn.md
@@ -22,7 +22,7 @@ ESP-Drone 是基于乐鑫 ESP32/ESP32-S2/ESP32-S3 开发的小型无人机解决
 2. 定高模式
 3. 定点模式
 4. APP 控制
-5. 适配 cfclient 上位机 
+5. 适配 cfclient 上位机，参考 https://github.com/leeebo/crazyflie-clients-python
 
 注意，定高模式及定点模式需外接扩展板，详情见硬件参考。
 
@@ -36,7 +36,7 @@ ESP-Drone 是基于乐鑫 ESP32/ESP32-S2/ESP32-S3 开发的小型无人机解决
 
 | 组件 | License | 源代码 |Commit ID |
 | :---:  | :---: | :---: |:---: |
-| core/crazyflie | GPL-3.0 |[Crazyflie](https://github.com/bitcraze/crazyflie-firmware) |tag_2021_01 b448553|
+| core/crazyflie | GPL-3.0 |[Crazyflie](https://github.com/bitcraze/crazyflie-firmware/tree/2021.01) |tag_2021_01 b448553|
 | lib/dsp_lib |  | [esp32-lin](https://github.com/whyengineer/esp32-lin/tree/master/components/dsp_lib) |6fa39f4c|
 
 ### 致谢

diff --git a/components/config/include/config.h b/components/config/include/config.h
@@ -41,7 +41,7 @@
 
 //#include "trace.h"
 #include "usec_time.h"
-
+#include "sdkconfig.h"
 
 #define PROTOCOL_VERSION 4
 #define QUAD_FORMATION_X
@@ -61,7 +61,6 @@
 #endif
 
 #ifdef STM32F4XX 
-
   #define CONFIG_BLOCK_ADDRESS    (2048 * (64-1))
   #define MCU_ID_ADDRESS          0x1FFF7A10
   #define MCU_FLASH_SIZE_ADDRESS  0x1FFF7A22
@@ -81,147 +80,94 @@
 //#define DEBUG_EP2
 
 // Task priorities. Higher number higher priority
-#define STABILIZER_TASK_PRI     5
-#define SENSORS_TASK_PRI        4
-#define ADC_TASK_PRI            3
-#define FLOW_TASK_PRI           3
-#define MULTIRANGER_TASK_PRI    3
-#define SYSTEM_TASK_PRI         2
+// system state tasks
+#define SYSTEM_TASK_PRI         1
+#define PM_TASK_PRI             1
+#define LEDSEQCMD_TASK_PRI      1
+// communication TX tasks
+#define UDP_TX_TASK_PRI         2
 #define CRTP_TX_TASK_PRI        2
-#define CRTP_RX_TASK_PRI        2
+// communication RX tasks
+#define UDP_RX_TASK_PRI         2
 #define EXTRX_TASK_PRI          2
-#define ZRANGER_TASK_PRI        2
-#define ZRANGER2_TASK_PRI       2
-#define PROXIMITY_TASK_PRI      0
-#define PM_TASK_PRI             0
-#define USDLOG_TASK_PRI         1
-#define USDWRITE_TASK_PRI       0
-#define PCA9685_TASK_PRI        2
-#define CMD_HIGH_LEVEL_TASK_PRI 2
-#define BQ_OSD_TASK_PRI         1
-#define GTGPS_DECK_TASK_PRI     1
-#define LIGHTHOUSE_TASK_PRI     3
-#define LPS_DECK_TASK_PRI       5
-#define OA_DECK_TASK_PRI        3
-#define UART1_TEST_TASK_PRI     1
-#define UART2_TEST_TASK_PRI     1
-#define LEDSEQCMD_TASK_PRI      1
-//if task watchdog triggered,KALMAN_TASK_PRI should set lower or set lower flow frequency
-#ifdef TARGET_MCU_ESP32
-  #define KALMAN_TASK_PRI         2
-  #define LOG_TASK_PRI            1
-  #define MEM_TASK_PRI            1
-  #define PARAM_TASK_PRI          1
-#else
+#define UART2_TASK_PRI          2
+#define SYSLINK_TASK_PRI        2
+#define USBLINK_TASK_PRI        2
+#define WIFILINK_TASK_PRI       2
+#define CRTP_RX_TASK_PRI        2
+#define CMD_HIGH_LEVEL_TASK_PRI 3
+#define INFO_TASK_PRI           2
+#define LOG_TASK_PRI            2
+#define MEM_TASK_PRI            2
+#define PARAM_TASK_PRI          2
+// sensors and stabilize related tasks
+#define PROXIMITY_TASK_PRI      5
+#define FLOW_TASK_PRI           5
+#define ZRANGER2_TASK_PRI       5
+#define ZRANGER_TASK_PRI        5
+#define SENSORS_TASK_PRI        6
+#define STABILIZER_TASK_PRI     7
+#define KALMAN_TASK_PRI         4
+
+// the kalman filter consumes a lot of CPU
+// for single core systems, we need to lower the priority
+#if CONFIG_FREERTOS_UNICORE
+  #undef KALMAN_TASK_PRI
   #define KALMAN_TASK_PRI         1
-  #define LOG_TASK_PRI            2
-  #define MEM_TASK_PRI            2
-  #define PARAM_TASK_PRI          2
 #endif
 
-#define SYSLINK_TASK_PRI        3
-#define USBLINK_TASK_PRI        3
-#define ACTIVE_MARKER_TASK_PRI  3
-#define AI_DECK_TASK_PRI        3
-#define UART2_TASK_PRI          3
-#define WIFILINK_TASK_PRI       3
-#define UDP_TX_TASK_PRI         3
-#define UDP_RX_TASK_PRI         2
-
-// Not compiled
-#if 0
-  #define INFO_TASK_PRI           2
-  #define PID_CTRL_TASK_PRI       2
-#endif
-
-
 // Task names
-#define SYSTEM_TASK_NAME        "SYSTEM"
-#define LEDSEQCMD_TASK_NAME     "LEDSEQCMD"
-#define ADC_TASK_NAME           "ADC"
-#define PM_TASK_NAME            "PWRMGNT"
-#define CRTP_TX_TASK_NAME       "CRTP-TX"
+#define CMD_HIGH_LEVEL_TASK_NAME "CMDHL"
 #define CRTP_RX_TASK_NAME       "CRTP-RX"
-#define CRTP_RXTX_TASK_NAME     "CRTP-RXTX"
+#define CRTP_TX_TASK_NAME       "CRTP-TX"
+#define EXTRX_TASK_NAME         "EXTRX"
+#define FLOW_TASK_NAME          "FLOW"
+#define KALMAN_TASK_NAME        "KALMAN"
+#define LEDSEQCMD_TASK_NAME     "LEDSEQCMD"
 #define LOG_TASK_NAME           "LOG"
 #define MEM_TASK_NAME           "MEM"
 #define PARAM_TASK_NAME         "PARAM"
+#define PM_TASK_NAME            "PWRMGNT"
+#define PROXIMITY_TASK_NAME     "PROXIMITY"
 #define SENSORS_TASK_NAME       "SENSORS"
 #define STABILIZER_TASK_NAME    "STABILIZER"
-#define NRF24LINK_TASK_NAME     "NRF24LINK"
-#define ESKYLINK_TASK_NAME      "ESKYLINK"
 #define SYSLINK_TASK_NAME       "SYSLINK"
+#define SYSTEM_TASK_NAME        "SYSTEM"
+#define UART2_TASK_NAME         "UART2"
+#define UDP_RX_TASK_NAME        "UDP_RX"
+#define UDP_TX_TASK_NAME        "UDP_TX"
 #define USBLINK_TASK_NAME       "USBLINK"
 #define WIFILINK_TASK_NAME      "WIFILINK"
-#define UDP_TX_TASK_NAME        "UDP_TX"
-#define UDP_RX_TASK_NAME        "UDP_RX"
-#define UDP_RX2_TASK_NAME       "UDP_RX2"
-#define PROXIMITY_TASK_NAME     "PROXIMITY"
-#define EXTRX_TASK_NAME         "EXTRX"
-#define UART_RX_TASK_NAME       "UART"
-#define ZRANGER_TASK_NAME       "ZRANGER"
 #define ZRANGER2_TASK_NAME      "ZRANGER2"
-#define FLOW_TASK_NAME          "FLOW"
-#define USDLOG_TASK_NAME        "USDLOG"
-#define USDWRITE_TASK_NAME      "USDWRITE"
-#define PCA9685_TASK_NAME       "PCA9685"
-#define CMD_HIGH_LEVEL_TASK_NAME "CMDHL"
-#define MULTIRANGER_TASK_NAME   "MR"
-#define BQ_OSD_TASK_NAME        "BQ_OSDTASK"
-#define GTGPS_DECK_TASK_NAME    "GTGPS"
-#define LIGHTHOUSE_TASK_NAME    "LH"
-#define LPS_DECK_TASK_NAME      "LPS"
-#define OA_DECK_TASK_NAME       "OA"
-#define UART1_TEST_TASK_NAME    "UART1TEST"
-#define UART2_TEST_TASK_NAME    "UART2TEST"
-#define KALMAN_TASK_NAME        "KALMAN"
-#define ACTIVE_MARKER_TASK_NAME "ACTIVEMARKER-DECK"
-#define AI_DECK_GAP_TASK_NAME   "AI-DECK-GAP"
-#define AI_DECK_NINA_TASK_NAME  "AI-DECK-NINA"
-#define UART2_TASK_NAME         "UART2"
-
-#define configBASE_STACK_SIZE CONFIG_BASE_STACK_SIZE
+#define ZRANGER_TASK_NAME       "ZRANGER"
 
 //Task stack sizes
-#define SYSTEM_TASK_STACKSIZE         (6 * configBASE_STACK_SIZE)
+#define configBASE_STACK_SIZE CONFIG_BASE_STACK_SIZE
+#define CMD_HIGH_LEVEL_TASK_STACKSIZE (2 * configBASE_STACK_SIZE)
+#define CRTP_RX_TASK_STACKSIZE        (3 * configBASE_STACK_SIZE)
+#define CRTP_TX_TASK_STACKSIZE        (3 * configBASE_STACK_SIZE)
+#define EXTRX_TASK_STACKSIZE          (1 * configBASE_STACK_SIZE)
+#define FLOW_TASK_STACKSIZE           (3 * configBASE_STACK_SIZE)
+#define KALMAN_TASK_STACKSIZE         (3 * configBASE_STACK_SIZE)
 #define LEDSEQCMD_TASK_STACKSIZE      (2 * configBASE_STACK_SIZE)
-#define ADC_TASK_STACKSIZE            (1 * configBASE_STACK_SIZE)
-#define PM_TASK_STACKSIZE             (4 * configBASE_STACK_SIZE)
-#define CRTP_TX_TASK_STACKSIZE        (2 * configBASE_STACK_SIZE)
-#define CRTP_RX_TASK_STACKSIZE        (6 * configBASE_STACK_SIZE)
-#define CRTP_RXTX_TASK_STACKSIZE      (1 * configBASE_STACK_SIZE)
 #define LOG_TASK_STACKSIZE            (3 * configBASE_STACK_SIZE)
 #define MEM_TASK_STACKSIZE            (2 * configBASE_STACK_SIZE)
 #define PARAM_TASK_STACKSIZE          (2 * configBASE_STACK_SIZE)
+#define PM_TASK_STACKSIZE             (4 * configBASE_STACK_SIZE)
 #define SENSORS_TASK_STACKSIZE        (5 * configBASE_STACK_SIZE)
 #define STABILIZER_TASK_STACKSIZE     (5 * configBASE_STACK_SIZE)
-#define NRF24LINK_TASK_STACKSIZE      (1 * configBASE_STACK_SIZE)
-#define ESKYLINK_TASK_STACKSIZE       (1 * configBASE_STACK_SIZE)
 #define SYSLINK_TASK_STACKSIZE        (1 * configBASE_STACK_SIZE)
+#define SYSTEM_TASK_STACKSIZE         (6 * configBASE_STACK_SIZE)
+#define UART2_TASK_STACKSIZE          (1 * configBASE_STACK_SIZE)
+#define UDP_RX_TASK_STACKSIZE         (4 * configBASE_STACK_SIZE)
+#define UDP_TX_TASK_STACKSIZE         (4 * configBASE_STACK_SIZE)
 #define USBLINK_TASK_STACKSIZE        (1 * configBASE_STACK_SIZE)
 #define WIFILINK_TASK_STACKSIZE       (4 * configBASE_STACK_SIZE)
-#define UDP_TX_TASK_STACKSIZE         (2 * configBASE_STACK_SIZE)
-#define UDP_RX_TASK_STACKSIZE         (4 * configBASE_STACK_SIZE)
-#define UDP_RX2_TASK_STACKSIZE        (1 * configBASE_STACK_SIZE)
-#define PROXIMITY_TASK_STACKSIZE      (1 * configBASE_STACK_SIZE)
-#define EXTRX_TASK_STACKSIZE          (1 * configBASE_STACK_SIZE)
-#define UART_RX_TASK_STACKSIZE        (1 * configBASE_STACK_SIZE)
-#define ZRANGER_TASK_STACKSIZE        (2 * configBASE_STACK_SIZE)
 #define ZRANGER2_TASK_STACKSIZE       (4 * configBASE_STACK_SIZE)
-#define FLOW_TASK_STACKSIZE           (3 * configBASE_STACK_SIZE)
-#define USDLOG_TASK_STACKSIZE         (2 * configBASE_STACK_SIZE)
-#define USDWRITE_TASK_STACKSIZE       (2 * configBASE_STACK_SIZE)
-#define PCA9685_TASK_STACKSIZE        (2 * configBASE_STACK_SIZE)
-#define CMD_HIGH_LEVEL_TASK_STACKSIZE (2 * configBASE_STACK_SIZE)
-#define MULTIRANGER_TASK_STACKSIZE    (2 * configBASE_STACK_SIZE)
-#define ACTIVEMARKER_TASK_STACKSIZE   (1 * configBASE_STACK_SIZE)
-#define AI_DECK_TASK_STACKSIZE        (1 * configBASE_STACK_SIZE)
-#define UART2_TASK_STACKSIZE          (1 * configBASE_STACK_SIZE)
+#define ZRANGER_TASK_STACKSIZE        (2 * configBASE_STACK_SIZE)
 
 //The radio channel. From 0 to 125
-//TODO:
 #define RADIO_RATE_2M 2
-
 #define RADIO_CHANNEL 80
 #define RADIO_DATARATE RADIO_RATE_2M
 #define RADIO_ADDRESS 0xE7E7E7E7E7ULL

diff --git a/components/core/crazyflie/CMakeLists.txt b/components/core/crazyflie/CMakeLists.txt
@@ -79,4 +79,5 @@ target_compile_options(${COMPONENT_LIB} PRIVATE "-fno-strict-aliasing"
                                                 "-Wno-error=stringop-truncation"
                                                 "-Wno-absolute-value"
                                                 "-Wno-address-of-packed-member"
-                                                "-Wno-stringop-truncation")
+                                                "-Wno-stringop-truncation"
+                                                "-Wno-format")
diff --git a/components/core/crazyflie/hal/src/wifilink.c b/components/core/crazyflie/hal/src/wifilink.c
@@ -51,10 +51,7 @@
 static bool isInit = false;
 static xQueueHandle crtpPacketDelivery;
 STATIC_MEM_QUEUE_ALLOC(crtpPacketDelivery, 16, sizeof(CRTPPacket));
-static uint8_t sendBuffer[64];
-
 static UDPPacket wifiIn;
-static CRTPPacket p;
 static uint32_t lastPacketTick;
 
 static int wifilinkSendPacket(CRTPPacket *p);
@@ -99,10 +96,12 @@ static bool detectEspNow(UDPPacket *in)
 
 static void wifilinkTask(void *param)
 {
+    CRTPPacket p = {0};
     while (1) {
         /* command step - receive  03 Fetch a wifi packet off the queue */
         wifiGetDataBlocking(&wifiIn);
         lastPacketTick = xTaskGetTickCount();
+        uint16_t sendWaitMs = 0;
 #ifdef CONFIG_ENABLE_LEGACY_APP
         float rch, pch, ych;
         uint16_t tch;
@@ -111,7 +110,7 @@ static void wifilinkTask(void *param)
             pch  = (-1.0) * (float)(((((uint16_t)wifiIn.data[3] << 8) + (uint16_t)wifiIn.data[4]) - 296) * 15.0 / 150.0); //-15~+15
             tch  = (((uint16_t)wifiIn.data[5] << 8) + (uint16_t)wifiIn.data[6]) * 59000.0 / 600.0;
             ych  = (float)(((((uint16_t)wifiIn.data[7] << 8) + (uint16_t)wifiIn.data[8]) - 296) * 15.0 / 150.0); //-15~+15
-            p.size = wifiIn.size + 1 ; //add cksum size
+            p.size = wifiIn.size - 1;
             p.header = CRTP_HEADER(CRTP_PORT_SETPOINT, 0x00); //head redefine
 
             memcpy(&p.data[0], &rch, 4);
@@ -131,11 +130,10 @@ static void wifilinkTask(void *param)
                 tch  = (int8_t)wifiIn.data[4] * 59000.0 / 128;
             }
             ych  = (float)((int8_t)wifiIn.data[3] * 15.0 / 128); //-15~+15
-            p.size = wifiIn.size + 1 ; //add cksum size
+            p.size = wifiIn.size - 1;
             p.header = CRTP_HEADER(CRTP_PORT_SETPOINT, 0x00); //head redefine
 
             //printf("rch: %.2f, pch: %.2f, tch: %d, ych: %.2f\n", rch, pch, tch, ych);
-
             memcpy(&p.data[0], &rch, 4);
             memcpy(&p.data[4], &pch, 4);
             memcpy(&p.data[8], &ych, 4);
@@ -144,11 +142,13 @@ static void wifilinkTask(void *param)
         {
             /* command step - receive  04 copy CRTP part from packet, the size not contain head */
             p.size = wifiIn.size - 1;
+            ASSERT(p.size <= CRTP_MAX_DATA_SIZE);
             memcpy(&p.raw, wifiIn.data, wifiIn.size);
+            sendWaitMs = 10;
         }
 
         /* command step - receive 05 send to crtpPacketDelivery queue */
-        xQueueSend(crtpPacketDelivery, &p, 0) ;
+        xQueueSend(crtpPacketDelivery, &p, M2T(sendWaitMs));
     }
 
 }
@@ -165,22 +165,9 @@ static int wifilinkReceiveCRTPPacket(CRTPPacket *p)
 
 static int wifilinkSendPacket(CRTPPacket *p)
 {
-    int dataSize;
-
-    ASSERT(p->size < SYSLINK_MTU);
-
-    sendBuffer[0] = p->header;
-
-    if (p->size <= CRTP_MAX_DATA_SIZE) {
-        memcpy(&sendBuffer[1], p->data, p->size);
-    }
-
-    dataSize = p->size + 1;
-
-
-    /*ledseqRun(&seq_linkDown);*/
-
-    return wifiSendData(dataSize, sendBuffer);
+    ASSERT(p->size <= CRTP_MAX_DATA_SIZE);
+    ledseqRun(&seq_linkDown);
+    return wifiSendData(p->size + 1, p->raw);
 }
 
 static int wifilinkSetEnable(bool enable)
@@ -198,11 +185,10 @@ void wifilinkInit()
         return;
     }
 
-
     crtpPacketDelivery = STATIC_MEM_QUEUE_CREATE(crtpPacketDelivery);
     DEBUG_QUEUE_MONITOR_REGISTER(crtpPacketDelivery);
 
-    STATIC_MEM_TASK_CREATE(wifilinkTask, wifilinkTask,WIFILINK_TASK_NAME,NULL, WIFILINK_TASK_PRI);
+    STATIC_MEM_TASK_CREATE(wifilinkTask, wifilinkTask, WIFILINK_TASK_NAME,NULL, WIFILINK_TASK_PRI);
 
     isInit = true;
 }