diff --git a/README.md b/README.md index 69d37c0..47819d1 100644 --- 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 index 8e62813..9629762 100644 --- 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 index e10c341..c4cf50e 100644 --- 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 -#define CRTP_TX_TASK_PRI 2 -#define CRTP_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 +// system state tasks +#define SYSTEM_TASK_PRI 1 +#define PM_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 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 +// communication TX tasks +#define UDP_TX_TASK_PRI 2 +#define CRTP_TX_TASK_PRI 2 +// communication RX tasks #define UDP_RX_TASK_PRI 2 +#define EXTRX_TASK_PRI 2 +#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 -// Not compiled -#if 0 - #define INFO_TASK_PRI 2 - #define PID_CTRL_TASK_PRI 2 +// 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 #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 index eb9fa50..8fda252 100644 --- 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") \ No newline at end of file + "-Wno-stringop-truncation" + "-Wno-format") \ No newline at end of file diff --git a/components/core/crazyflie/hal/src/wifilink.c b/components/core/crazyflie/hal/src/wifilink.c index 3c461bd..ff0ff98 100644 --- 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; } diff --git a/components/core/crazyflie/modules/src/comm.c b/components/core/crazyflie/modules/src/comm.c index 6b03521..3651e6e 100644 --- a/components/core/crazyflie/modules/src/comm.c +++ b/components/core/crazyflie/modules/src/comm.c @@ -45,28 +45,29 @@ static bool isInit; void commInit(void) { if (isInit) { - return;} + return; + } - /* These functions are moved to be initialized early so - * that DEBUG_PRINTD can be used early */ - //wifilinkInit(); - //crtpInit(); - //consoleInit(); + /* These functions are moved to be initialized early so + * that DEBUG_PRINTD can be used early */ + //wifilinkInit(); + //crtpInit(); + //consoleInit(); - crtpSetLink(wifilinkGetLink()); - crtpserviceInit(); - platformserviceInit(); - logInit(); - paramInit(); - //locSrvInit(); + crtpSetLink(wifilinkGetLink()); + crtpserviceInit(); + platformserviceInit(); + logInit(); + paramInit(); + //locSrvInit(); - //setup CRTP communication channel //TODO: check for USB first and prefer USB over radio + //setup CRTP communication channel //if (usbTest()) // crtpSetLink(usbGetLink); //else if(radiolinkTest()) // crtpSetLink(radiolinkGetLink()); - isInit = true; + isInit = true; } bool commTest(void) @@ -84,7 +85,6 @@ bool commTest(void) DEBUG_PRINTI("consoleTest = %d ", pass); pass &= paramTest(); DEBUG_PRINTI("paramTest = %d ", pass); - - return pass; + return pass; } diff --git a/components/core/crazyflie/modules/src/crtp.c b/components/core/crazyflie/modules/src/crtp.c index 23c7563..2d23c0d 100644 --- a/components/core/crazyflie/modules/src/crtp.c +++ b/components/core/crazyflie/modules/src/crtp.c @@ -186,6 +186,8 @@ void crtpRxTask(void *param) if (xQueueSend(queues[p.port], &p, 0) == errQUEUE_FULL) { // We should never drop packet + printf("CRTP RX queue full\n"); + printf("Port: %d\n", p.port); ASSERT(0); } } diff --git a/components/core/crazyflie/modules/src/crtp_commander_high_level.c b/components/core/crazyflie/modules/src/crtp_commander_high_level.c index 18e1a8e..818b146 100644 --- a/components/core/crazyflie/modules/src/crtp_commander_high_level.c +++ b/components/core/crazyflie/modules/src/crtp_commander_high_level.c @@ -109,17 +109,16 @@ static StaticSemaphore_t lockTrajBuffer; static float defaultTakeoffVelocity = 0.5f; static float defaultLandingVelocity = 0.5f; -// TODO // Trajectory memory handling from the memory module -// static uint32_t handleMemGetSize(void) { return crtpCommanderHighLevelTrajectoryMemSize(); } -// static bool handleMemRead(const uint32_t memAddr, const uint8_t readLen, uint8_t* buffer); -// static bool handleMemWrite(const uint32_t memAddr, const uint8_t writeLen, const uint8_t* buffer); -// static const MemoryHandlerDef_t memDef = { -// .type = MEM_TYPE_TRAJ, -// .getSize = handleMemGetSize, -// .read = handleMemRead, -// .write = handleMemWrite, -// }; +static uint32_t handleMemGetSize(void) { return crtpCommanderHighLevelTrajectoryMemSize(); } +static bool handleMemRead(const uint32_t memAddr, const uint8_t readLen, uint8_t* buffer); +static bool handleMemWrite(const uint32_t memAddr, const uint8_t writeLen, const uint8_t* buffer); +static const MemoryHandlerDef_t memDef = { + .type = MEM_TYPE_TRAJ, + .getSize = handleMemGetSize, + .read = handleMemRead, + .write = handleMemWrite, +}; STATIC_MEM_TASK_ALLOC(crtpCommanderHighLevelTask, CMD_HIGH_LEVEL_TASK_STACKSIZE); @@ -262,7 +261,7 @@ void crtpCommanderHighLevelInit(void) return; } - //memoryRegisterHandler(&memDef); + memoryRegisterHandler(&memDef); plan_init(&planner); //Start the trajectory task @@ -629,13 +628,13 @@ int define_trajectory(const struct data_define_trajectory* data) return 0; } -// static bool handleMemRead(const uint32_t memAddr, const uint8_t readLen, uint8_t* buffer) { -// return crtpCommanderHighLevelReadTrajectory(memAddr, readLen, buffer); -// } +static bool handleMemRead(const uint32_t memAddr, const uint8_t readLen, uint8_t* buffer) { + return crtpCommanderHighLevelReadTrajectory(memAddr, readLen, buffer); +} -// static bool handleMemWrite(const uint32_t memAddr, const uint8_t writeLen, const uint8_t* buffer) { -// return crtpCommanderHighLevelWriteTrajectory(memAddr, writeLen, buffer); -// } +static bool handleMemWrite(const uint32_t memAddr, const uint8_t writeLen, const uint8_t* buffer) { + return crtpCommanderHighLevelWriteTrajectory(memAddr, writeLen, buffer); +} uint8_t* initCrtpPacket(CRTPPacket* packet, const enum TrajectoryCommand_e command) { diff --git a/components/core/crazyflie/modules/src/estimator_kalman.c b/components/core/crazyflie/modules/src/estimator_kalman.c index 9abf5ce..c8ad8fa 100644 --- a/components/core/crazyflie/modules/src/estimator_kalman.c +++ b/components/core/crazyflie/modules/src/estimator_kalman.c @@ -263,7 +263,7 @@ static void kalmanTask(void* parameters); static bool predictStateForward(uint32_t osTick, float dt); static bool updateQueuedMeasurments(const Axis3f *gyro, const uint32_t tick); -STATIC_MEM_TASK_ALLOC_STACK_NO_DMA_CCM_SAFE(kalmanTask, 3 * configMINIMAL_STACK_SIZE); +STATIC_MEM_TASK_ALLOC_STACK_NO_DMA_CCM_SAFE(kalmanTask, KALMAN_TASK_STACKSIZE); // -------------------------------------------------- diff --git a/components/core/crazyflie/modules/src/mem.c b/components/core/crazyflie/modules/src/mem.c index 79f0be0..c183c86 100644 --- a/components/core/crazyflie/modules/src/mem.c +++ b/components/core/crazyflie/modules/src/mem.c @@ -125,7 +125,9 @@ bool memTest(void) { } if (owMemHandler == 0) { - return false; + printf("No one-wire memory handler registered\n"); + //TODO: + return true; } return true; @@ -218,7 +220,7 @@ static void createInfoResponse(CRTPPacket* p, uint8_t memId) { uint8_t serialNr[MEMORY_SERIAL_LENGTH]; // No error code if we fail, just send an empty packet back - if (owMemHandler->getSerialNr(selectedMem, serialNr)) { + if (owMemHandler->getSerialNr && owMemHandler->getSerialNr(selectedMem, serialNr)) { createInfoResponseBody(p, MEM_TYPE_OW, owMemHandler->size, serialNr); } } diff --git a/components/core/crazyflie/modules/src/system.c b/components/core/crazyflie/modules/src/system.c index 36e7c04..a2955e7 100644 --- a/components/core/crazyflie/modules/src/system.c +++ b/components/core/crazyflie/modules/src/system.c @@ -223,8 +223,9 @@ void systemTask(void *arg) pass &= estimatorKalmanTaskTest(); DEBUG_PRINTI("estimatorKalmanTaskTest = %d ", pass); //pass &= deckTest(); - //pass &= soundTest(); - //pass &= memTest(); + pass &= soundTest(); + DEBUG_PRINTI("soundTest = %d ", pass); + pass &= memTest(); DEBUG_PRINTI("memTest = %d ", pass); //pass &= watchdogNormalStartTest(); pass &= cfAssertNormalStartTest(); @@ -273,7 +274,6 @@ void systemTask(void *arg) vTaskDelay(portMAX_DELAY); } - /* Global system variables */ void systemStart() { diff --git a/components/drivers/general/wifi/include/wifi_esp32.h b/components/drivers/general/wifi/include/wifi_esp32.h index a125dea..c5416ad 100644 --- a/components/drivers/general/wifi/include/wifi_esp32.h +++ b/components/drivers/general/wifi/include/wifi_esp32.h @@ -3,7 +3,8 @@ #include #include -#define WIFI_RX_TX_PACKET_SIZE (64) +// 32 bytes is enough for CRTP packets (30+1) + checksum 1 +#define WIFI_RX_TX_PACKET_SIZE (32) /* Structure used for in/out data via USB */ typedef struct diff --git a/components/drivers/general/wifi/wifi_esp32.c b/components/drivers/general/wifi/wifi_esp32.c index 89ff95d..fe90988 100644 --- a/components/drivers/general/wifi/wifi_esp32.c +++ b/components/drivers/general/wifi/wifi_esp32.c @@ -27,9 +27,9 @@ #include "espnow_utils.h" #define UDP_SERVER_PORT 2390 -#define UDP_SERVER_BUFSIZE 128 +#define UDP_SERVER_BUFSIZE 64 -static struct sockaddr_in6 source_addr; +static struct sockaddr_storage source_addr; static char WIFI_SSID[32] = ""; static char WIFI_PWD[64] = CONFIG_WIFI_PASSWORD; @@ -41,17 +41,9 @@ static uint8_t WIFI_CH = CONFIG_WIFI_CHANNEL; #define MACSTR "%02x:%02x:%02x:%02x:%02x:%02x" #endif -static char rx_buffer[UDP_SERVER_BUFSIZE]; -static char tx_buffer[UDP_SERVER_BUFSIZE]; -const int addr_family = (int)AF_INET; -const int ip_protocol = IPPROTO_IP; -static struct sockaddr_in dest_addr; static int sock; - static xQueueHandle udpDataRx; static xQueueHandle udpDataTx; -static UDPPacket inPacket; -static UDPPacket outPacket; static bool isInit = false; static bool isUDPInit = false; @@ -94,7 +86,7 @@ bool wifiGetDataBlocking(UDPPacket *in) { /* command step - receive 02 from udp rx queue */ while (xQueueReceive(udpDataRx, in, portMAX_DELAY) != pdTRUE) { - vTaskDelay(1); + vTaskDelay(M2T(10)); }; // Don't return until we get some data on the UDP return true; @@ -102,7 +94,7 @@ bool wifiGetDataBlocking(UDPPacket *in) bool wifiSendData(uint32_t size, uint8_t *data) { - static UDPPacket outStage; + UDPPacket outStage = {0}; outStage.size = size; memcpy(outStage.data, data, size); // Dont' block when sending @@ -115,12 +107,12 @@ static esp_err_t udp_server_create(void *arg) return ESP_OK; } - struct sockaddr_in *pdest_addr = &dest_addr; - pdest_addr->sin_addr.s_addr = htonl(INADDR_ANY); - pdest_addr->sin_family = AF_INET; - pdest_addr->sin_port = htons(UDP_SERVER_PORT); + static struct sockaddr_in dest_addr = {0}; + dest_addr.sin_addr.s_addr = htonl(INADDR_ANY); + dest_addr.sin_family = AF_INET; + dest_addr.sin_port = htons(UDP_SERVER_PORT); - sock = socket(addr_family, SOCK_DGRAM, ip_protocol); + sock = socket(AF_INET, SOCK_DGRAM, IPPROTO_IP); if (sock < 0) { DEBUG_PRINT_LOCAL("Unable to create socket: errno %d", errno); return ESP_FAIL; @@ -139,41 +131,43 @@ static esp_err_t udp_server_create(void *arg) static void udp_server_rx_task(void *pvParameters) { - uint8_t cksum = 0; socklen_t socklen = sizeof(source_addr); + char rx_buffer[UDP_SERVER_BUFSIZE]; + UDPPacket inPacket = {0}; while (true) { if(isUDPInit == false) { vTaskDelay(20); continue; } - int len = recvfrom(sock, rx_buffer, sizeof(rx_buffer) - 1, 0, (struct sockaddr *)&source_addr, &socklen); + int len = recvfrom(sock, rx_buffer, sizeof(rx_buffer), 0, (struct sockaddr *)&source_addr, &socklen); /* command step - receive 01 from Wi-Fi UDP */ if (len < 0) { DEBUG_PRINT_LOCAL("recvfrom failed: errno %d", errno); - break; - } else if(len > WIFI_RX_TX_PACKET_SIZE - 4) { - DEBUG_PRINT_LOCAL("Received data length = %d > 64", len); + continue; + } else if(len > WIFI_RX_TX_PACKET_SIZE) { + DEBUG_PRINT_LOCAL("Received data length = %d > %d", len, WIFI_RX_TX_PACKET_SIZE); + continue; } else { - //copy part of the UDP packet - rx_buffer[len] = 0;// Null-terminate whatever we received and treat like a string... - memcpy(inPacket.data, rx_buffer, len); - cksum = inPacket.data[len - 1]; + uint8_t cksum = rx_buffer[len - 1]; //remove cksum, do not belong to CRTP - inPacket.size = len - 1; //check packet - if (cksum == calculate_cksum(inPacket.data, len - 1) && inPacket.size < 64){ - xQueueSend(udpDataRx, &inPacket, M2T(2)); + if (cksum == calculate_cksum(rx_buffer, len - 1)) { + //copy part of the UDP packet, the size not include cksum + inPacket.size = len - 1; + memcpy(inPacket.data, rx_buffer, inPacket.size); + xQueueSend(udpDataRx, &inPacket, M2T(10)); if(!isUDPConnected) isUDPConnected = true; }else{ DEBUG_PRINT_LOCAL("udp packet cksum unmatched"); } #ifdef DEBUG_UDP - DEBUG_PRINT_LOCAL("1.Received data size = %d %02X \n cksum = %02X", len, inPacket.data[0], cksum); - for (size_t i = 0; i < len; i++) { - DEBUG_PRINT_LOCAL(" data[%d] = %02X ", i, inPacket.data[i]); + printf("\nReceived size = %d cksum = %02X\n", inPacket.size, cksum); + for (size_t i = 0; i < inPacket.size; i++) { + printf("%02X ", inPacket.data[i]); } + printf("\n"); #endif } } @@ -181,26 +175,28 @@ static void udp_server_rx_task(void *pvParameters) static void udp_server_tx_task(void *pvParameters) { + UDPPacket outPacket = {0}; while (TRUE) { if(isUDPInit == false) { vTaskDelay(20); continue; } - if ((xQueueReceive(udpDataTx, &outPacket, 5) == pdTRUE) && isUDPConnected) { - memcpy(tx_buffer, outPacket.data, outPacket.size); - tx_buffer[outPacket.size] = calculate_cksum(tx_buffer, outPacket.size); - tx_buffer[outPacket.size + 1] = 0; + if ((xQueueReceive(udpDataTx, &outPacket, portMAX_DELAY) == pdTRUE) && isUDPConnected) { + // append cksum to the packet + outPacket.data[outPacket.size] = calculate_cksum(outPacket.data, outPacket.size); + outPacket.size += 1; - int err = sendto(sock, tx_buffer, outPacket.size + 1, 0, (struct sockaddr *)&source_addr, sizeof(source_addr)); + int err = sendto(sock, outPacket.data, outPacket.size, 0, (struct sockaddr *)&source_addr, sizeof(source_addr)); if (err < 0) { DEBUG_PRINT_LOCAL("Error occurred during sending: errno %d", errno); continue; } #ifdef DEBUG_UDP - DEBUG_PRINT_LOCAL("Send data to"); - for (size_t i = 0; i < outPacket.size + 1; i++) { - DEBUG_PRINT_LOCAL(" data_send[%d] = %02X ", i, tx_buffer[i]); + printf("\nSend size = %d checksum = %02X\n", outPacket.size, outPacket.data[outPacket.size - 1]); + for (size_t i = 0; i < outPacket.size; i++) { + printf("%02X ", outPacket.data[i]); } + printf("\n"); #endif } } @@ -259,9 +255,9 @@ void wifiInit(void) return; } // This should probably be reduced to a CRTP packet size - udpDataRx = xQueueCreate(5, sizeof(UDPPacket)); /* Buffer packets (max 64 bytes) */ + udpDataRx = xQueueCreate(16, sizeof(UDPPacket)); DEBUG_QUEUE_MONITOR_REGISTER(udpDataRx); - udpDataTx = xQueueCreate(5, sizeof(UDPPacket)); /* Buffer packets (max 64 bytes) */ + udpDataTx = xQueueCreate(16, sizeof(UDPPacket)); DEBUG_QUEUE_MONITOR_REGISTER(udpDataTx); espnow_storage_init(); diff --git a/components/drivers/i2c_devices/eeprom/eeprom.c b/components/drivers/i2c_devices/eeprom/eeprom.c index b383f1d..10a2943 100644 --- a/components/drivers/i2c_devices/eeprom/eeprom.c +++ b/components/drivers/i2c_devices/eeprom/eeprom.c @@ -101,7 +101,8 @@ bool eepromTestConnection(void) bool eepromReadBuffer(uint8_t *buffer, size_t readAddr, size_t len) { bool status = false; - esp_err_t err = ESP_OK; //TODO spi_flash_read(readAddr, buffer, len); + esp_err_t err = ESP_OK; + //TODO spi_flash_read(readAddr, buffer, len); if (err == ESP_OK) { @@ -119,7 +120,6 @@ bool eepromWriteBuffer(uint8_t *buffer, size_t writeAddr, size_t len) { bool status = false; writeAddr += EEPROM_IN_FLASH_ADDR; - //TODO if (spi_flash_write(writeAddr, buffer, len) == ESP_OK) { if (1) { diff --git a/components/drivers/i2c_devices/vl53l1/zranger2.c b/components/drivers/i2c_devices/vl53l1/zranger2.c index 4570d39..660be48 100644 --- a/components/drivers/i2c_devices/vl53l1/zranger2.c +++ b/components/drivers/i2c_devices/vl53l1/zranger2.c @@ -142,6 +142,10 @@ void zRanger2Task(void* arg) } } +static uint8_t disable = 0; + PARAM_GROUP_START(deck) PARAM_ADD(PARAM_UINT8 | PARAM_RONLY, bcZRanger2, &isInit) +PARAM_ADD(PARAM_UINT8 | PARAM_RONLY, bcZRanger, &disable) +PARAM_ADD(PARAM_UINT8 | PARAM_RONLY, deck, &disable) PARAM_GROUP_STOP(deck) diff --git a/components/drivers/spi_devices/pmw3901/flowdeck_v1v2.c b/components/drivers/spi_devices/pmw3901/flowdeck_v1v2.c index c54fc4b..09e979b 100755 --- a/components/drivers/spi_devices/pmw3901/flowdeck_v1v2.c +++ b/components/drivers/spi_devices/pmw3901/flowdeck_v1v2.c @@ -86,9 +86,9 @@ static void flowdeckTask(void *param) while (1) { // if task watchdog triggered,flow frequency should set lower -#ifdef CONFIG_IDF_TARGET_ESP32S2 +#if CONFIG_FREERTOS_UNICORE vTaskDelay(10); -#elif defined(CONFIG_IDF_TARGET_ESP32S3) || defined(CONFIG_IDF_TARGET_ESP32) +#else vTaskDelay(5); #endif @@ -122,9 +122,9 @@ static void flowdeckTask(void *param) flowData.stdDevY = stdFlow; // [pixels] should perhaps be made larger? // if task watchdog triggered,flow frequency should set lower -#ifdef CONFIG_IDF_TARGET_ESP32S2 +#if CONFIG_FREERTOS_UNICORE flowData.dt = 0.01; -#elif defined(CONFIG_IDF_TARGET_ESP32S3) || defined(CONFIG_IDF_TARGET_ESP32) +#else flowData.dt = 0.005; #endif diff --git a/components/lib/dsp_lib/CMakeLists.txt b/components/lib/dsp_lib/CMakeLists.txt index 039cf86..5f86302 100644 --- a/components/lib/dsp_lib/CMakeLists.txt +++ b/components/lib/dsp_lib/CMakeLists.txt @@ -10,4 +10,4 @@ idf_component_register(SRC_DIRS "BasicMathFunctions" INCLUDE_DIRS "include" ) -target_compile_options(${COMPONENT_LIB} PUBLIC "-fno-strict-aliasing") \ No newline at end of file +target_compile_options(${COMPONENT_LIB} PUBLIC "-fno-strict-aliasing" "-Wno-format") \ No newline at end of file diff --git a/docs/en/rst/gettingstarted.rst b/docs/en/rst/gettingstarted.rst index 6d1e6e5..a42c86d 100755 --- a/docs/en/rst/gettingstarted.rst +++ b/docs/en/rst/gettingstarted.rst @@ -156,53 +156,28 @@ This step is optional, only for advanced debugging. cfclient Interface -**1. Install CRTP protocol package** +**1. Install cfclient** 1.1 Download the source code .. code:: text - git clone -b esp-drone https://github.com/qljz1993/crazyflie-lib-python.git + git clone https://github.com/qljz1993/crazyflie-clients-python.git -1.2 Navigate to the source code directory, and install the requirements -.. code:: text - - pip3 install -r requirements.txt - -1.3 Install CRTP package +1.2 Install cfclient .. code:: text pip3 install -e . -**2. Install cfclient** - -2.1 Download the source code - -.. code:: text - - git clone -b esp-drone https://github.com/qljz1993/crazyflie-clients-python.git - -2.2 Navigate to the source code directory, and install the requirements - -.. code:: text - - sudo apt-get install python3 python3-pip python3-pyqt5 python3-pyqt5.qtsvg - -2.3 Install cfclient - -.. code:: text - - pip3 install -e . - -2.4 Start cfclient +1.3 Start cfclient .. code:: text python3 ./bin/cfclient -**3. Configure the controllers** +**2. Configure the controllers** .. figure:: ../../_static/gamepad_settings.png :align: center @@ -211,9 +186,9 @@ This step is optional, only for advanced debugging. Controller Configuration -3.1 Configure the four main dimensions of controls: ``Roll, Pitch, Yaw, Thrust``. +2.1 Configure the four main dimensions of controls: ``Roll, Pitch, Yaw, Thrust``. -3.2 Configure button ``Assisted control`` for flight mode switching. +2.2 Configure button ``Assisted control`` for flight mode switching. ESP-Drone APP Guide ===================== diff --git a/docs/zh_CN/rst/gettingstarted.rst b/docs/zh_CN/rst/gettingstarted.rst index baa0bb7..ef63a2a 100644 --- a/docs/zh_CN/rst/gettingstarted.rst +++ b/docs/zh_CN/rst/gettingstarted.rst @@ -134,53 +134,27 @@ ESP-Drone APP 同时支持 Android 系统和 iOS 系统。 cfclient 上位机界面 -**1. 安装 CRTP 协议支持包** +**1. 安装 cfclient** 1.1 下载源代码 .. code:: text - git clone -b esp-drone https://github.com/qljz1993/crazyflie-lib-python.git + git clone https://github.com/qljz1993/crazyflie-clients-python.git -1.2 进入源码目录，安装依赖 - -.. code:: text - - pip3 install -r requirements.txt - -1.3 安装 CRTP 包 +1.2 安装 cfclient 客户端 .. code:: text pip3 install -e . -**2. 安装 cfclient** - -2.1 下载源代码 - -.. code:: text - - git clone -b esp-drone https://github.com/qljz1993/crazyflie-clients-python.git - -2.2 进入源码目录，安装依赖 - -.. code:: text - - sudo apt-get install python3 python3-pip python3-pyqt5 python3-pyqt5.qtsvg - -2.3 安装 cfclient 客户端 - -.. code:: text - - pip3 install -e . - -2.4 启动客户端 +1.3 启动客户端 .. code:: text python3 ./bin/cfclient -**3. 配置遥控器** +**2. 配置遥控器** .. figure:: ../../_static/gamepad_settings.png :align: center @@ -189,9 +163,9 @@ ESP-Drone APP 同时支持 Android 系统和 iOS 系统。 遥控器配置 -3.1 配置 4 个控制轴：``Roll 、Pitch、Yaw、Thrust``。 +2.1 配置 4 个控制轴：``Roll 、Pitch、Yaw、Thrust``。 -3.2 配置一个按键为 ``Assisted control``，用于飞行模式切换。 +2.2 配置一个按键为 ``Assisted control``，用于飞行模式切换。 手机 APP 使用指南 ======================== diff --git a/main/Kconfig.projbuild b/main/Kconfig.projbuild index dbe46a4..f7c043c 100644 --- a/main/Kconfig.projbuild +++ b/main/Kconfig.projbuild @@ -64,7 +64,7 @@ menu "ESPDrone Config" config WIFI_CHANNEL int "Wi-Fi/ESP-Now Channel" range 1 13 - default 13 + default 6 help Wi-Fi/ESP-Now Channel, 1-13 config WIFI_MAX_STA_CONN diff --git a/sdkconfig.defaults b/sdkconfig.defaults index ca10619..9e7ca0f 100644 --- a/sdkconfig.defaults +++ b/sdkconfig.defaults @@ -7,9 +7,6 @@ CONFIG_FREERTOS_CORETIMER_0=y CONFIG_FREERTOS_OPTIMIZED_SCHEDULER=y CONFIG_FREERTOS_HZ=1000 CONFIG_FREERTOS_SUPPORT_STATIC_ALLOCATION=y -CONFIG_FREERTOS_USE_TRACE_FACILITY=y -CONFIG_FREERTOS_GENERATE_RUN_TIME_STATS=y -CONFIG_FREERTOS_VTASKLIST_INCLUDE_COREID=y CONFIG_FREERTOS_USE_STATS_FORMATTING_FUNCTIONS=y CONFIG_FREERTOS_IDLE_TASK_STACKSIZE=2048 CONFIG_FREERTOS_ISR_STACKSIZE=2048 @@ -23,3 +20,6 @@ CONFIG_FREERTOS_TIMER_TASK_STACK_DEPTH=4096 CONFIG_ESPTOOLPY_FLASHMODE_QIO=y CONFIG_ESPTOOLPY_FLASHFREQ_80M=y +CONFIG_FREERTOS_VTASKLIST_INCLUDE_COREID=y +CONFIG_FREERTOS_USE_TRACE_FACILITY=y +CONFIG_FREERTOS_GENERATE_RUN_TIME_STATS=y