# MicroMultiRotor **Repository Path**: hallo_frank/micro-multi-rotor ## Basic Information - **Project Name**: MicroMultiRotor - **Description**: LoongDoor MicroMultiRotor Codes - **Primary Language**: Unknown - **License**: Not specified - **Default Branch**: master - **Homepage**: None - **GVP Project**: No ## Statistics - **Stars**: 0 - **Forks**: 0 - **Created**: 2023-12-15 - **Last Updated**: 2024-11-05 ## Categories & Tags **Categories**: Uncategorized **Tags**: None ## README # MultiRotor ## ## 1、环境依赖 windows10 STM32cubeIDE:Version: 1.4.2(该软件为免费软件，可以前往ST官网下载) ## 2、目录结构描述 >readme.md //help > >Core > >Drivers > >eigen3：C++矩阵运算库源文件 > >Middlewares > >Release：编译生成的文件 > >Src：自定义源代码 >>drivers > >>>spi：SPI驱动函数，包括SPI发送数据，SPI接收数据，数据以字节为单位。在这里定义了一个类，作为ICM20602(IMU)/LSM303D(MAG)/MS5611(气压计)等类的基类。正是由于IMU、MAG和气压计都使用到了SPI这个外设来读取数据，都需要使用SPI的驱动程序，为了不重复编写SPI的驱动程序，所以加了这个文件夹，定义了这样一个基类。 >>>system：这个文件夹可以说是最重要的，因为所有的任务的创建，任务的优先级设置，任务的调度，用于任务之间通信的队列的创建，以及控制定时任务定时执行的软件定时器都是在这里实现。 >>>userlib：一些常用的数据类型的声明，一些常用函数的定义，比如滑窗均值滤波、最小二乘滤波等等。 >>>queueMessage.h：所有的消息队列的数据类型在这里声明。 > >>modules >>>ahrs：用于解算飞行器姿态角。 >>>lsm303d：用于采集磁力计数据，并将数据发送到磁力计消息队列(queueMagDat)，以便其他任务可以随时读取。 >>>control：用于飞行器的底层控制，包括位置、速度、姿态、角速度的控制，以及控制分配。 >>>icm20602:用于采集加速度计、陀螺仪数据，对数据进行滤波，并将数据发送到IMU消息队列(queueGyrDat/queueAccDat/queueAccDat)，以便其他任务可以随时读取。 >>>motor：读取control任务控制分配好的各电机或舵机的输出，并驱动电机、舵机 >>>power：读取电池电压，并发送到电池电压消息队列中去(queueBattery)。 >>>rc：遥控器信号接收以及信号解析，通过串口空闲中断实现数据的接收，接收到完整的遥控器信号后立即恢复信号解析任务，待解析完挂起这任务。 >>>transfer：这里包含两个任务，一个是飞行数据发送至上位机任务，另一个是接收上位机指令任务。 >>>ms5611：用于采集气压计数据，并将数据发送到气压计消息队列(queueBaroAlt)，以便其他任务可以随时读取。 > >.cproject > >.mxproject > >.project > >multirotor.ioc > >STM32F767VITX_FLASH.ld > >STM32F767VITX_RAM.ld > ## 3、eigen C++数学库 > **如果未添加，请按下面的步骤将C++数学库添加到工程中去** 1. 先去Eigen官网下载源码（这里下载的是3.3.7版本） 2. 将源码里面的eigen3文件夹复制到工程目录下 3. 打开工程，按步骤Project->Properties->C/C++ General->Paths and Symbols->Source Location，点击Add folder,勾选Link to folder in the file system,点击Brower,选择eigen3文件夹下面的Eigen文件夹，点击OK 4. 按步骤Project->Properties->C/C++ General->Paths and Symbols-> Includes，点击GNU C++,点击右侧的Add directory path,点击Workspace，选择eigen3文件夹下面的Eigen文件夹，点击OK 5. 至此完成了添加Eigen库 ## 4、代码说明 ## 让我们先从main()函数开始，主要是进行外设和时钟的初始化，调用freertos系统初始化函数，在初始化完之后，再调用**MX_FREERTOS_Init()，进行任务的创建、信号量的创建、队列的创建、软件定时器的创建（后面会说明各自的用处）**，最后启动内核。 ```cpp int main(void) { /* MCU Configuration--------------------------------------------------------*/ /* Reset of all peripherals, Initializes the Flash interface and the Systick. */ HAL_Init(); /* Configure the system clock */ SystemClock_Config(); /* Initialize all configured peripherals */ MX_GPIO_Init(); MX_DMA_Init(); MX_ADC1_Init(); MX_TIM1_Init(); MX_SPI1_Init(); MX_SPI2_Init(); MX_TIM2_Init(); MX_TIM5_Init(); MX_USART1_UART_Init(); MX_USART2_UART_Init(); MX_USART3_UART_Init(); MX_UART4_Init(); MX_UART5_Init(); MX_USART6_UART_Init(); /* Init scheduler */ osKernelInitialize(); /* Call init function for freertos objects (in freertos.c) */ MX_FREERTOS_Init(); /* Start scheduler */ osKernelStart(); /* We should never get here as control is now taken by the scheduler */ /* Infinite loop */ while (1) { } } ``` **MX_FREERTOS_Init()**：该函数定义在freertos.c文件中，上面已经说了它的主要作用，为了后续修改方便，这里将所有自己创建的任务、信号量、队列和软件定时器都封装成一个函数，即**mySystemInit()**，该函数在**system/system.hpp**文件中申明，在**system/system.cpp**文件中定义 ==#system/system.hpp：== ```cpp void mySystemInit(void); ``` ==#system/system.cpp：== ```cpp SYSTEM system; //创建一个SYSTEM对象 /* 在启动实时操作系统之前调用该函数 */ void mySystemInit(void) { system.taskCreat(); //任务的创建 system.semCreat(); //信号量的创建,用于任务的同步 system.timerCreat(); //软件定时器的创建,用于任务的时间片调度 system.queueCreat(); //消息队列的创建,用于任务之间的通信 } ``` 如上面代码所示，任务的创建、信号量的创建、队列的创建、软件定时器的创建是通过创建SYSTEM这个类的对象来实现的，而SYSYTEM这个类的申明和定义如下： ==#system/system.hpp：== ```cpp using namespace std; class SYSTEM { public: SYSTEM(); ~SYSTEM(); void taskCreat(); //任务创建 void semCreat(); //信号量创建 void timerCreat(); //软件定时器创建 void queueCreat(); //消息队列创建 private: BaseType_t isIcm20602TaskCreatSuccess; //陀螺仪数据采集任务创建成功标志位 BaseType_t isMagTaskCreatSuccess; //磁力计数据采集任务创建成功标志位 BaseType_t isAhrsTaskCreatSuccess; //姿态解算任务创建成功标志位 BaseType_t isMahonyAhrsTaskCreatSuccess; //姿态解算任务创建成功标志位 BaseType_t isEkfTaskCreatSuccess; //姿态解算任务创建成功标志位 BaseType_t isEkfUwbTaskCreatSuccess; //位置速度解算任务创建成功标志位 BaseType_t isTranTaskCreatSuccess; //数据传输任务创建成功标志位 BaseType_t isTranReceiveTaskCreatSuccess; //数据传输任务创建成功标志位 BaseType_t isBatteryTaskCreatSuccess; //电池电压测量任务创建成功标志位 BaseType_t isRcTaskCreatSuccess; //遥控器解析任务创建成功标志位 BaseType_t isMotorTaskCreatSuccess; //电机驱动任务创建成功标志位 BaseType_t isAttitudeCtrlTaskCreatSuccess; //姿态控制任务创建成功标志位 BaseType_t isPositionCtrlTaskCreatSuccess; //位置控制任务创建成功标志位 BaseType_t isFlowTaskCreatSuccess; //姿态控制任务创建成功标志位 BaseType_t isMs5611TaskCreatSuccess; //气压高度采集任务创建成功标志位 BaseType_t isUwbTaskCreatSuccess; //uwb任务创建成功标志位 BaseType_t isAnotcFlowTaskCreatSuccess; //匿名光流任务创建成功标志位 }; ``` ==#system/system.cpp：== ```cpp SYSTEM::SYSTEM() { } SYSTEM::~SYSTEM() { } /********************************** * function : 创建用户任务 * * task list : 任务句柄 任务功能 执行时间 * icm20602Task : 陀螺仪加速度计数据采集 31us * motorTask : 电机驱动 24us * lsm303dTask : 磁力计数据采集 15us * ahrsTask : 姿态解算 195us * attitudeCtrlTask : 内环控制（角度角速度串级PID控制） * tranTask : 飞控数据发送到地面站 * tranReceiveTask : 地面站数据发送到飞控 * batteryTask : 电池电压采集 * rcTask : 遥控器 * * para :void * * return :void * * *************************************/ void SYSTEM::taskCreat() { isIcm20602TaskCreatSuccess = xTaskCreate ((TaskFunction_t)Icm20602_main, "icm20602Task", (uint16_t)(128 * 4), NULL, (osPriority_t) osPriorityRealtime5, &icm20602TaskHandle); isMotorTaskCreatSuccess = xTaskCreate ((TaskFunction_t)motor_main, "motorTask", (uint16_t)(128 * 4), NULL, (osPriority_t) osPriorityHigh3, &motorTaskHandle); isMagTaskCreatSuccess = xTaskCreate ((TaskFunction_t)lsm303d_main, "lsm303dTask", (uint16_t)(128 * 4), NULL, (osPriority_t) osPriorityRealtime3, &lsm303dTaskHandle); isMs5611TaskCreatSuccess = xTaskCreate ((TaskFunction_t)ms5611_main, "ms5611Task", (uint16_t)(128 * 4), NULL, (osPriority_t) osPriorityRealtime2, &ms5611TaskHandle); // isAhrsTaskCreatSuccess = xTaskCreate ((TaskFunction_t)ahrs_main, "ahrsTask", (uint16_t)(256 * 4), NULL, (osPriority_t) osPriorityHigh7, &ahrsTaskHandle); isMahonyAhrsTaskCreatSuccess = xTaskCreate ((TaskFunction_t)mahonyAhrs_main, "mahonyAhrsTask", (uint16_t)(128 * 4), NULL, (osPriority_t) osPriorityHigh7, &mahonyAhrsTaskHandle); // isEkfTaskCreatSuccess = xTaskCreate ((TaskFunction_t)ekf_main, "ekfTask", (uint16_t)(256 * 4), NULL, (osPriority_t) osPriorityHigh7, &ekfTaskHandle); isEkfUwbTaskCreatSuccess = xTaskCreate ((TaskFunction_t)ekfUwb_main, "ekfUwbTask", (uint16_t)(128 * 4), NULL, (osPriority_t) osPriorityHigh5, &ekfUwbTaskHandle); isAttitudeCtrlTaskCreatSuccess = xTaskCreate ((TaskFunction_t)attitudeCtrl_main, "attitudeCtrlTask", (uint16_t)(128 * 4), NULL, (osPriority_t) osPriorityHigh6, &attitudeCtrlTaskHandle); isPositionCtrlTaskCreatSuccess = xTaskCreate ((TaskFunction_t)positionCtrl_main, "positionCtrlTask", (uint16_t)(128 * 4), NULL, (osPriority_t) osPriorityHigh2, &positionCtrlTaskHandle); isTranTaskCreatSuccess = xTaskCreate ((TaskFunction_t)tran_main, "tranTask", (uint16_t)(128 * 4), NULL, (osPriority_t) osPriorityAboveNormal3, &tranTaskHandle); isTranReceiveTaskCreatSuccess = xTaskCreate ((TaskFunction_t)tranReceive_main, "tranReceiveTask", (uint16_t)(128 * 4), NULL, (osPriority_t) osPriorityAboveNormal4, &tranReceiveTaskHandle); isBatteryTaskCreatSuccess = xTaskCreate ((TaskFunction_t)battery_main, "batteryTask", (uint16_t)(128 * 4), NULL, (osPriority_t) osPriorityBelowNormal1, &batteryTaskHandle); isRcTaskCreatSuccess = xTaskCreate ((TaskFunction_t)rc_main, "rcTask", (uint16_t)(128 * 4), NULL, (osPriority_t) osPriorityAboveNormal6, &rcTaskHandle); isFlowTaskCreatSuccess = xTaskCreate ((TaskFunction_t)flow_main, "flowTask", (uint16_t)(128 * 4), NULL, (osPriority_t) osPriorityRealtime1, &flowTaskHandle); // isAnotcFlowTaskCreatSuccess = xTaskCreate ((TaskFunction_t)anotcFlow_main, "anotcFlowTask", (uint16_t)(128 * 4), NULL, (osPriority_t) osPriorityRealtime1, &anotcFlowTaskHandle); isUwbTaskCreatSuccess = xTaskCreate ((TaskFunction_t)uwb_main, "uwbTask", (uint16_t)(128 * 4), NULL, (osPriority_t) osPriorityRealtime, &uwbTaskHandle); /* 在这里添加你的任务 */ /* 在这里添加你的任务 */ } void SYSTEM::semCreat() { semIcm20602 = xSemaphoreCreateBinary(); semLsm303d = xSemaphoreCreateBinary(); semMs5611 = xSemaphoreCreateBinary(); semAhrs = xSemaphoreCreateBinary(); semMahonyAhrs = xSemaphoreCreateBinary(); semEkf = xSemaphoreCreateBinary(); semEkfUwb = xSemaphoreCreateBinary(); semAttitudeCtrl = xSemaphoreCreateBinary(); semPositionCtrl = xSemaphoreCreateBinary(); semMotor = xSemaphoreCreateBinary(); semTran = xSemaphoreCreateBinary(); semBattery = xSemaphoreCreateBinary(); /* 在这里添加你的信号量 */ /* 在这里添加你的信号量 */ } void SYSTEM::timerCreat() { myTimer1ms = xTimerCreate ("myTimer", 1, pdTRUE, (void *)0, (TimerCallbackFunction_t)Timer1msCallback); xTimerStart(myTimer1ms,0); } void SYSTEM::queueCreat() { queueGyrDat = xQueueCreate(1,sizeof(sensor_gyro_msg)); queueGyrDatFil = xQueueCreate(1,sizeof(sensor_gyro_filter_msg)); queueAccDat = xQueueCreate(1,sizeof(sensor_acc_msg)); queueMagDat = xQueueCreate(1,sizeof(sensor_mag_msg)); queueBaroAlt = xQueueCreate(1,sizeof(sensor_baroAlt_msg)); queueAhrsEuler = xQueueCreate(1,sizeof(ahrs_euler_msg)); queueBattery = xQueueCreate(1,sizeof(battery_msg)); queueRCData = xQueueCreate(1,sizeof(RCData)); queueRCCommand = xQueueCreate(1,sizeof(RC_command_msg)); queueMotorPWM = xQueueCreate(1,sizeof(Motor_PWM_msg)); queuePID = xQueueCreate(1,sizeof(Pid_msg)); queueInloopControl = xQueueCreate(1,sizeof(Inloop_control_msg)); queueFlow = xQueueCreate(1,sizeof(flow_msg)); queueHeight = xQueueCreate(1,sizeof(height_msg)); queueEkf = xQueueCreate(1,sizeof(ekf_msg)); queueMahonyAhrs = xQueueCreate(1,sizeof(mahonyAhrs_msg));; queueUWB = xQueueCreate(1,sizeof(uwb_pos_msg)); queueCb2n = xQueueCreate(1,sizeof(Cb2n_msg)); queueUwbPosVel = xQueueCreate(1,sizeof(ekf_uwb_pos_vel_msg)); /* 在这里添加你的消息队列 */ /* 在这里添加你的消息队列 */ } ``` SYSTEM的成员函数除了构造函数和虚构函数以外，为 ```cpp void taskCreat(); //任务创建 void semCreat(); //信号量创建 void timerCreat(); //软件定时器创建 void queueCreat(); //消息队列创建 ``` 其后面的注释已经说明了各成员函数的作用。对于飞控系统来说，可能需要多个功能模块来共同完成，比如说，IMU数据的采集可以看做是一个功能模块、姿态解算是一个功能模块、遥控器接收是一个功能模块等等，也就是说一个完整的飞控软件系统是由一个个功能模块组成，从而构成比较复杂的飞控软件系统。每一个功能模块都可以看成是一个独立的任务，它们之间是互不影响，只可能存在数据的交换。这个的软件设计方法也叫模块化设计，功能模块化使得软件系统清晰明了，代码移植和新功能添加非常简单。这里先回到上面四个成员函数： ### void taskCreat(); //任务创建 ### 对于飞控软件系统中的每一个功能模块（后面统一叫任务），要让它按照自己的意愿跑起来，必须事先创建好该任务，这里以imu数据采集任务为例进行说明： ```cpp /* Definitions for icm20602Task */ TaskHandle_t icm20602TaskHandle; void Icm20602_main(void *argument); SemaphoreHandle_t semIcm20602; isIcm20602TaskCreatSuccess = xTaskCreate ((TaskFunction_t)Icm20602_main, "icm20602Task", (uint16_t)(128 * 4), NULL, (osPriority_t) osPriorityRealtime5, &icm20602TaskHandle); ``` icm20602TaskHandle：任务句柄（通俗点就是一个任务的身份证）： > 句柄的含义：在操作系统中任务包括任务控制块、任务堆栈和任务代码组成，其本质是一个结构体。在创建任务的时候，就是申请了一个任务结构体，在后面如果需要对任务的属性进行设置，就需要用到任务句柄，其本质就是指向任务控制块的指针。有了这个句柄可以随意操作和设置任务的属性，就像把柄一样，掌握它，就掌握了任务的行为。 isIcm20602TaskCreatSuccess：任务创建成功标志位，1表示任务创建成功；0表示任务创建失败。 xTaskCreat函数：创建任务，函数原型： ```c BaseType_t xTaskCreate( TaskFunction_t pxTaskCode, const char * const pcName, /*lint !e971 Unqualified char types are allowed for strings and single characters only. */ const configSTACK_DEPTH_TYPE usStackDepth, void * const pvParameters, UBaseType_t uxPriority, TaskHandle_t * const pxCreatedTask ) ``` 创建一个任务需要指明任务函数入口、任务名、任务堆栈大小（以字为单位，不能小于128word）、任务优先级（数字越大，优先级越高，如下）、任务句柄 ```c /// Priority values. typedef enum { osPriorityNone = 0, ///< No priority (not initialized). osPriorityIdle = 1, ///< Reserved for Idle thread. osPriorityLow = 8, ///< Priority: low osPriorityLow1 = 8+1, ///< Priority: low + 1 osPriorityLow2 = 8+2, ///< Priority: low + 2 osPriorityLow3 = 8+3, ///< Priority: low + 3 osPriorityLow4 = 8+4, ///< Priority: low + 4 osPriorityLow5 = 8+5, ///< Priority: low + 5 osPriorityLow6 = 8+6, ///< Priority: low + 6 osPriorityLow7 = 8+7, ///< Priority: low + 7 osPriorityBelowNormal = 16, ///< Priority: below normal osPriorityBelowNormal1 = 16+1, ///< Priority: below normal + 1 osPriorityBelowNormal2 = 16+2, ///< Priority: below normal + 2 osPriorityBelowNormal3 = 16+3, ///< Priority: below normal + 3 osPriorityBelowNormal4 = 16+4, ///< Priority: below normal + 4 osPriorityBelowNormal5 = 16+5, ///< Priority: below normal + 5 osPriorityBelowNormal6 = 16+6, ///< Priority: below normal + 6 osPriorityBelowNormal7 = 16+7, ///< Priority: below normal + 7 osPriorityNormal= 24, ///< Priority: normal osPriorityNormal1 = 24+1, ///< Priority: normal + 1 osPriorityNormal2 = 24+2, ///< Priority: normal + 2 osPriorityNormal3 = 24+3, ///< Priority: normal + 3 osPriorityNormal4 = 24+4, ///< Priority: normal + 4 osPriorityNormal5 = 24+5, ///< Priority: normal + 5 osPriorityNormal6 = 24+6, ///< Priority: normal + 6 osPriorityNormal7 = 24+7, ///< Priority: normal + 7 osPriorityAboveNormal = 32, ///< Priority: above normal osPriorityAboveNormal1 = 32+1, ///< Priority: above normal + 1 osPriorityAboveNormal2 = 32+2, ///< Priority: above normal + 2 osPriorityAboveNormal3 = 32+3, ///< Priority: above normal + 3 osPriorityAboveNormal4 = 32+4, ///< Priority: above normal + 4 osPriorityAboveNormal5 = 32+5, ///< Priority: above normal + 5 osPriorityAboveNormal6 = 32+6, ///< Priority: above normal + 6 osPriorityAboveNormal7 = 32+7, ///< Priority: above normal + 7 osPriorityHigh = 40, ///< Priority: high osPriorityHigh1 = 40+1, ///< Priority: high + 1 osPriorityHigh2 = 40+2, ///< Priority: high + 2 osPriorityHigh3 = 40+3, ///< Priority: high + 3 osPriorityHigh4 = 40+4, ///< Priority: high + 4 osPriorityHigh5 = 40+5, ///< Priority: high + 5 osPriorityHigh6 = 40+6, ///< Priority: high + 6 osPriorityHigh7 = 40+7, ///< Priority: high + 7 osPriorityRealtime = 48, ///< Priority: realtime osPriorityRealtime1 = 48+1, ///< Priority: realtime + 1 osPriorityRealtime2 = 48+2, ///< Priority: realtime + 2 osPriorityRealtime3 = 48+3, ///< Priority: realtime + 3 osPriorityRealtime4 = 48+4, ///< Priority: realtime + 4 osPriorityRealtime5 = 48+5, ///< Priority: realtime + 5 osPriorityRealtime6 = 48+6, ///< Priority: realtime + 6 osPriorityRealtime7 = 48+7, ///< Priority: realtime + 7 osPriorityISR = 56, ///< Reserved for ISR deferred thread. osPriorityError = -1, ///< System cannot determine priority or illegal priority. osPriorityReserved = 0x7FFFFFFF ///< Prevents enum down-size compiler optimization. } osPriority_t; ``` 当xTaskCreat函数执行完并且返回1，那么任务创建成功，只要freertos内核一旦启动，那么就会进入任务入口函数进行执行，对于IMU数据采集任务，当然是进入 ```c void Icm20602_main(void *argument); ``` ### void semCreat(); //信号量创建 ### 上面的IMU数据采集任务还有一个变量没有讲到，那就是二进制信号量， ```cpp SemaphoreHandle_t semIcm20602; ``` 那么它是用来干嘛的呢？让我们从飞控软件系统讲起，前面已经说过飞控软件系统是由一个个独立的任务组成，每个任务实现一个功能，还是以IMU数据采集任务为例，那采集IMU数据当然需要规定数据的采集频率，不能让CPU一直在那里采集IMU数据，它还需要完成其他任务，也就是说我需要让CPU安照一个固定的频率（按照一个固定的时间间隔）去采集IMU数据，采集完成之后就释放CPU资源，以便其他任务使用CPU资源。那么这里就需要用到二进制信号量了，创建一个二进制信号量很简单，只需要执行下面的API函数就可： semIcm20602 = xSemaphoreCreateBinary(); 我们继续进入IMU采集任务去看，在初始化完成之后就进入了一个死循环，在死循环中后面的两行就是采集IMU数据（分别为采集加速度计数据和采集陀螺仪数据），重点来看死循环中的第一行代码，该行代码是在请求一个二进制信号量（semIcm20602），并且阻塞等待时间为0xffffffff,相当于一只阻塞等待，意思就是说，如果信号量semIcm20602没有被释放，它将一直被阻塞下去，直到信号量semIcm20602被释放，才能继续执行死循环中的后两行代码，说到这里，你应该知道怎样去实现按固定频率（固定时间间隔）执行任务了吧，就是如果我们定时去释放这个信号量semIcm20602不就可以做到了吗。（这里有必要说明一下，二进制信号量被释放之后只能被请求一次，如果需要再次请求，需要等到信号量再次被释放，否则进入阻塞等待，等待时间为0xffffffff,等待时间可以更改） ```cpp ICM20602 icm20602(SPI1,(char *)"ICM20602",(char *) "-X-Y+Z"); extern "C" void Icm20602_main(void *argument) { icm20602.Icm20602_Init(); for(;;) { osSemaphoreAcquire(semIcm20602,0xffffffff); icm20602.Icm20602_Update_Gyro(); icm20602.Icm20602_Update_Acceleration(); } } ``` ### void timerCreat(); //软件定时器创建 ### 上面说到定时去释放二进制信号量就可以实现任务按固定频率（固定时间间隔）执行，所以需要创建一个定时器，freertos提供了软件定时器，相对于STM32提供的硬件定时器，它的优势是不占用外设资源。 ```c TimerHandle_t myTimer1ms; void Timer1msCallback(void *argument); myTimer1ms = xTimerCreate ("myTimer", 1, pdTRUE, (void *)0, (TimerCallbackFunction_t)Timer1msCallback); xTimerStart(myTimer1ms,0); ``` myTimer1ms：软件定时器句柄，理解同任务句柄。 xTimerCreate：创建软件定时器函数原型： ```c TimerHandle_t xTimerCreate( const char * const pcTimerName, /*lint !e971 Unqualified char types are allowed for strings and single characters only. */ const TickType_t xTimerPeriodInTicks, const UBaseType_t uxAutoReload, void * const pvTimerID, TimerCallbackFunction_t pxCallbackFunction ) ``` 函数中的第二个参数表示的是定时器的周期（单位ms），最后一个参数是指定定时器回调函数，当启动定时器之后： ```c xTimerStart(myTimer1ms,0); ``` 每过1ms，就会调用一次回调函数void Timer1msCallback(void *argument) ```cpp void Timer1msCallback(void *argument) { //任务时间片调度,设定定时任务时间片长度,单位为ms static uint32_t _cnt = 0; const uint32_t taskPeriod[20] = {1,5,10,20,200}; if(_cnt%taskPeriod[0] == taskPeriod[0]-1) { osSemaphoreRelease(semIcm20602); osSemaphoreRelease(semAhrs); osSemaphoreRelease(semMahonyAhrs); osSemaphoreRelease(semEkf); osSemaphoreRelease(semAttitudeCtrl); osSemaphoreRelease(semMotor); } if(_cnt%taskPeriod[1] == taskPeriod[1]-1) { osSemaphoreRelease(semPositionCtrl); // osSemaphoreRelease(semEkf); osSemaphoreRelease(semEkfUwb); } if(_cnt%taskPeriod[2] == taskPeriod[2]-1) { osSemaphoreRelease(semTran); osSemaphoreRelease(semMs5611); } if(_cnt%taskPeriod[3] == taskPeriod[3]-1) { osSemaphoreRelease(semLsm303d); } if(_cnt%taskPeriod[4] == taskPeriod[4]-1) { osSemaphoreRelease(semBattery); } _cnt++; } ``` 显然，信号量semIcm20602每1ms释放一次，也就是说IMU数据采集任务每过1ms执行一次，当然这个执行周期可以自行更改。其他定时任务也同理。 ### void queueCreat(); //消息队列创建 ### 前面说过不同的任务之间相互独立，但是可能存在数据交换，这也叫做任务间通信（IPC），这里任务间通信通过队列来实现。 ## 5、双向DSHOT ## 目前飞控板与电调之间的通信采用的是DSHOT300数字电调协议，而所谓的双向DSHOT指的是飞控既可以发送油门信号给电调，同时电调也可以将电机转速、电压、温度等数据遥测给飞控，并且每个电机只使用一个信号线，简单地说飞控和电调可以进行双向通信。 目前最新一代的32位电调(BLHeli_32)将固件升级到32.7版本及以上，即可支持双向DSHOT，而这里因为之前的选型，电流要求、尺寸要求、重量要求最终选择了BLHeli_s电调，BLHeli_s电调官方没有提供支持双向DSHOT的固件版本，但是幸运的是有支持双向DSHOT的第三方固件，主要有两个，分别是 JESC(付费)和Jazzmaverick(免费)，所以这里选择Jazzmaverick固件，刷固件的方法教程网上很多。 双向Dshot使用反向信号电平（空闲为1）。飞控到电调使用dshot帧，但是最低的4位是其他几位异或运算的补码（正常dshot不对异或数进行补码）。电调根据反转检测到必须发送遥测数据包（电机转速）。 收到dshot帧后30us，电调响应遥测帧。从逻辑上讲，遥测帧是一个16位值，而最低的4位是其他几位异或运算的补码。 高12位编码如下所示（1 / erps）： e e e m m m m m m m m m 9位值M需要左移E次才能获得以微秒为单位的周期。这样的范围是1 us至65408 us。转换为15.29 Hz的最小e频率，或14极电机的3.82 hz。 然后，通过每4位应用以下映射，将该16位值通过GCR编码为20位值： Data Encoded 0000 11001 0001 11011 0010 10010 0011 10011 0100 11101 0101 10101 0110 10110 0111 10111 1000 11010 1001 01001 1010 01010 1011 01011 1100 11110 1101 01101 1110 01110 1111 01111 这将创建一个不超过两个连续零的20位值。在这20位值前面加上一个0，然后接下来的每一位如果是1，那么21位值对应的那一位就相对前一位就需要变化，否则重复上一个位值，从而将该值映射到一个新的21位值。例： 1 0 1 1 1 0 0 1 1 0会变成0 1 1 0 1 0 0 0 1 0 0。 然后以5/4 * dshot比特率发送此21比特值。因此Dshot300使用375kbit，dshot600使用750kbit。 但是在飞控代码中我们接收到的数据是编码后的21位比特值，所以需要进行解码从而得到电机转速，代码如下（详见motor.cpp）： ==motor.hpp:== ```cpp #define NUM_OF_MOTOR_POLE_PAIR 6 #define PWM_OUT_NUM 4 #define iv 0xffffffff static const uint32_t decode[32] = { iv, iv, iv, iv, iv, iv, iv, iv, iv, 9, 10, 11, iv, 13, 14, 15, iv, iv, 2, 3, iv, 5, 6, 7, iv, 0, 8, 1, iv, 4, 12, iv }; ``` ==motor.cpp:== ```cpp for(uint8_t i=0; i30)?((deltaTL>55)?3:2):1; //上升沿到下降沿之间为高电平，通过时间间隔计算连续高电平个数，一个电平时间约为22us~23us deltaTH = erpm_Pwm_In[i][2*index+2] - erpm_Pwm_In[i][2*index+1]; n_H = (deltaTH>10)?((deltaTH>30)?((deltaTH>55)?3:2):1):0; //如果n_H等于0，说明已经计算结束 //因为value的初始值为0xffffffff，所以只需要将低电平的那些位置位0即可 switch(n_L) { case 1: value = value & ~((uint32_t)1<<(20-m)); break; case 2: value = value & ~((uint32_t)3<<(19-m)); break; case 3: value = value & ~((uint32_t)7<<(18-m)); break; } index = index +1; } erpm_Code_Edge[i] = value & 0x001fffff; //只取后21bits //转成GCR码 uint32_t codeValue = 0; for(uint8_t j=0; j<20; j++) { if((((erpm_Code_Edge[i]>>(20-j))^(erpm_Code_Edge[i]>>(19-j)))&0x00000001) == 1) //从左边的第二位开始，依次与其前一位比较，如果电平有变化，则该位为1，否则为0 { codeValue |= 1<<(19-j); } } erpm_Code_GCR[i] = codeValue; // First bit is start bit so discard it. erpm_Code_GCR[i] &= 0xfffff; //取后面20bits //将GCR码转成16位格式(eeem mmmm mmmm 4bit_CRC) uint32_t decodedValue = decode[erpm_Code_GCR[i] & 0x1f]; decodedValue |= decode[(erpm_Code_GCR[i] >> 5) & 0x1f] << 4; decodedValue |= decode[(erpm_Code_GCR[i] >> 10) & 0x1f] << 8; decodedValue |= decode[(erpm_Code_GCR[i] >> 15) & 0x1f] << 12; //crc校验 uint32_t ccsum = decodedValue>>4; ccsum = ccsum ^ (decodedValue >> 8); // xor bytes ccsum = ccsum ^ (decodedValue >> 12); // xor nibbles ccsum = ~ccsum; if ((ccsum & 0xf) != (decodedValue & 0xf) || decodedValue > 0xffff) //CRC校验不成功 { } else//CRC校验成功 { erpm_Code[i] = decodedValue >> 4; //去掉CRC校验码，取前12bits if (erpm_Code[i] == 0x0fff) { rpm[i] = 0.0f; } else { // Convert erpm_Code to 16 bit from the GCR telemetry format (eeem mmmm mmmm) erpm_us[i] = (erpm_Code[i] & 0x000001ff) << ((erpm_Code[i] & 0xfffffe00) >> 9); // Convert period to erpm * 100 erpm[i] = (1000000 * 60 / 100 + erpm_us[i] / 2) / erpm_us[i]; // Convert erpm*100 to rpm rpm[i] = (float)erpm[i]*100.0f/NUM_OF_MOTOR_POLE_PAIR; } motorRpm.motor_rpm[i] = rpm[i]; } } memset(erpm_Pwm_In,0,sizeof(erpm_Pwm_In)); //将接受跳边沿时间的数组重新置为0,以便定时器输入捕获再次接受新的数据 ```