diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000000000000000000000000000000000000..368f69766cf601dd0e3f9e85107d5ac2090093eb --- /dev/null +++ b/.gitignore @@ -0,0 +1,3 @@ +.vscode/ +.cache/ +build/ \ No newline at end of file diff --git a/.idea/workspace.xml b/.idea/workspace.xml new file mode 100644 index 0000000000000000000000000000000000000000..ad4f8450e9f97474ee88f7bc82b8b834460a4355 --- /dev/null +++ b/.idea/workspace.xml @@ -0,0 +1,94 @@ + + + + + + + { + "useNewFormat": true +} + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1692777502148 + + + + + + + + + + + + \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000000000000000000000000000000000000..bc8687f4604c8afc4f3464aefae3e98161669794 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,38 @@ + +cmake_minimum_required(VERSION 3.21) +project(UIML) + +if(MSVC) + add_compile_options("/utf-8") +endif() +set(CMAKE_C_STANDARD 99) +set(CMAKE_CXX_STANDARD 20) + +# 子模块添加 +add_subdirectory(tools) +add_subdirectory(softbus) +add_subdirectory(conf) + +# UIML 静态库部分 +add_library(uiml_static OBJECT + ${TOOLS_MODULE_SRC} + ${SOFTBUS_MODULE_SRC} + ${CONFIG_MODULE_SRC} +) + +target_include_directories(uiml_static PUBLIC + ${TOOLS_MODULE_INCL} + ${SOFTBUS_MODULE_INCL} + ${CONFIG_MODULE_INCL} +) + +if(PROJECT_IS_TOP_LEVEL) + target_include_directories(uiml_static PRIVATE test) + + file(GLOB UIML_TEST_SRC test/*.cpp) + add_executable(${PROJECT_NAME}_test ${UIML_TEST_SRC} conf/yamlparser.cpp tools/hasher/hasher.c tools/universal_vector/vector.c) + # target_link_libraries(${PROJECT_NAME}_test uiml_static) + target_include_directories(${PROJECT_NAME}_test PRIVATE test tools/include) + add_test(RunTest ${PROJECT_NAME}_test) + set_tests_properties(RunTest PROPERTIES FAIL_REGULAR_EXPRESSION "^FAIL:") +endif() diff --git a/cmake/UimlUtil.cmake b/cmake/UimlUtil.cmake new file mode 100644 index 0000000000000000000000000000000000000000..c3ad1126020f0c6359c2cd297f1337661cbde0a5 --- /dev/null +++ b/cmake/UimlUtil.cmake @@ -0,0 +1,8 @@ + +function(uiml_link_to_stm32_target TARGET_NAME CPU_TYPE) + get_target_property(TARGET_INCL ${TARGET_NAME} INCLUDE_DIRECTORIES) + target_include_directories(uiml_static PRIVATE ${TARGET_INCL}) + target_compile_options(uiml_static PUBLIC -mcpu=${CPU_TYPE}) + + target_link_libraries(${TARGET_NAME} PRIVATE uiml_static) +endfunction() diff --git a/conf/CMakeLists.txt b/conf/CMakeLists.txt new file mode 100644 index 0000000000000000000000000000000000000000..2f89024e6e46cbd7ae34c96bf387e41b66094c9f --- /dev/null +++ b/conf/CMakeLists.txt @@ -0,0 +1,7 @@ + +# 在上级作用域设置配置模块源码列表 +file(GLOB CONFIG_SRC *.c *.cpp) +set(CONFIG_MODULE_SRC ${CONFIG_SRC} PARENT_SCOPE) + +# 添加包含目录 +set(CONFIG_MODULE_INCL ${CMAKE_CURRENT_LIST_DIR} PARENT_SCOPE) diff --git a/conf/config.c b/conf/config.c index 6ffcc0acef81b954aecf4cb05ebe89bc75c84c90..deda8d6b695a63d2793500013ca62677d9a2d02c 100644 --- a/conf/config.c +++ b/conf/config.c @@ -1,13 +1,16 @@ + +#include "stddef.h" +#include "string.h" #include "config.h" #include "sys_conf.h" #include "cmsis_os.h" -//ģص +//声明服务模块回调函数 #define SERVICE(service,callback,priority,stackSize) extern void callback(void const *); SERVICE_LIST #undef SERVICE -//бö +//服务列表枚举 typedef enum { #define SERVICE(service,callback,priority,stackSize) service, @@ -16,10 +19,10 @@ typedef enum serviceNum }Module; -// +//服务任务句柄 osThreadId serviceTaskHandle[serviceNum]; -//ȡñиֵûҵ򷵻NULL +//取出给定配置表中给定配置项的值，没找到则返回NULL void* _Conf_GetValue(ConfItem* dict, const char* name) { if(!dict) @@ -50,15 +53,15 @@ void* _Conf_GetValue(ConfItem* dict, const char* name) return NULL; } -//FreeRTOSĬ +//FreeRTOS默认任务 void StartDefaultTask(void const * argument) { - //з񣬽ñֱΪ + //创建所有服务任务，将配置表分别作为参数传入 #define SERVICE(service,callback,priority,stackSize) \ osThreadDef(service, callback, priority, 0, stackSize); \ serviceTaskHandle[service] = osThreadCreate(osThread(service), Conf_GetPtr(systemConfig,#service,void)); SERVICE_LIST #undef SERVICE - //Լ + //销毁自己 vTaskDelete(NULL); } diff --git a/conf/config.h b/conf/config.h index d1b15ce223274023944a49fd6ac23e53728daaa8..7521ed2719c2590bc2126bd50d0a9469946ed870 100644 --- a/conf/config.h +++ b/conf/config.h @@ -1,53 +1,67 @@ #ifndef _CONFIG_H_ #define _CONFIG_H_ -#include -#include -#include #include #include -// +//弱符号，定义来自STM32 HAL库 +#if defined (__ARMCC_VERSION) && (__ARMCC_VERSION >= 6010050) /* ARM Compiler V6 */ + #ifndef __weak + #define __weak __attribute__((weak)) + #endif + #ifndef __packed + #define __packed __attribute__((packed)) + #endif +#elif defined ( __GNUC__ ) && !defined (__CC_ARM) /* GNU Compiler */ + #ifndef __weak + #define __weak __attribute__((weak)) + #endif /* __weak */ + #ifndef __packed + #define __packed __attribute__((__packed__)) + #endif /* __packed */ +#endif /* __GNUC__ */ + +//配置项类型 typedef struct { - char *name; // - void *value; //ֵ + char *name; //配置名 + void *value; //配置值 } ConfItem; -//ûļõĺװ +//给用户配置文件用的宏封装 #define CF_DICT (ConfItem[]) #define CF_DICT_END {NULL,NULL} #ifndef IM_PTR -#define IM_PTR(type,...) (&(type){__VA_ARGS__}) //ȡĵַ +#define IM_PTR(type,...) (&(type){__VA_ARGS__}) //取立即数的地址 #endif -//ȡֵӦֱӵãӦʹ·ķװ +//获取配置值，不应直接调用，应使用下方的封装宏 void* _Conf_GetValue(ConfItem* dict, const char* name); /* - @brief жֵǷ - @param dict:Ŀֵ - @param name:Ҫҵͨ'/'ָԲڲֵ - @retval 0: 1: + @brief 判断字典中配置名是否存在 + @param dict:目标字典 + @param name:要查找的配置名，可通过'/'分隔以查找内层字典 + @retval 0:不存在 1:存在 */ #define Conf_ItemExist(dict,name) (_Conf_GetValue((dict),(name))!=NULL) /* - @brief ȡֵָ - @param dict:Ŀֵ - @param name:Ҫҵͨ'/'ָԲڲֵ - @param type:ֵ - @retval ֵָ(type*)ָ룬򷵻NULL + @brief 获取配置值指针 + @param dict:目标字典 + @param name:要查找的配置名，可通过'/'分隔以查找内层字典 + @param type:配置值的类型 + @retval 指向配置值的(type*)型指针，若配置名不存在则返回NULL */ #define Conf_GetPtr(dict,name,type) ((type*)_Conf_GetValue((dict),(name))) /* - @brief ȡֵ - @param dict:Ŀֵ - @param name:Ҫҵͨ'/'ָԲڲֵ - @param type:ֵ - @param def:Ĭֵ - @retval typeֵݣ򷵻def + @brief 获取配置值 + @param dict:目标字典 + @param name:要查找的配置名，可通过'/'分隔以查找内层字典 + @param type:配置值的类型 + @param def:默认值 + @retval type型配置值数据，若配置名不存在则返回def */ #define Conf_GetValue(dict,name,type,def) (_Conf_GetValue((dict),(name))?(*(type*)(_Conf_GetValue((dict),(name)))):(def)) diff --git a/conf/sys_conf.h b/conf/sys_conf.h index ce804ef6aff2e9685754639843acf20686212256..e96d9c0c5c84ef634dd5696c903e263db53ff399 100644 --- a/conf/sys_conf.h +++ b/conf/sys_conf.h @@ -1,6 +1,6 @@ /* sys_conf.h - ϵͳļ + 系统配置文件 */ #ifndef _SYSCONF_H_ @@ -8,8 +8,8 @@ #include "config.h" -/****************** ******************/ -//ʹConfiguration Wizard +/****************** 外设配置 ******************/ +//可使用Configuration Wizard配置 //<<< Use Configuration Wizard in Context Menu >>> //BSP Config @@ -46,9 +46,9 @@ #endif // <<< end of configuration section >>> -/****************** б (X-MACRO) ******************/ +/****************** 服务列表配置 (X-MACRO) ******************/ -//ÿʽ(,,ȼ,ջС) +//每项格式(服务名,服务任务函数,任务优先级,任务栈大小) #define SERVICE_LIST \ SERVICE(can, BSP_CAN_TaskCallback, osPriorityRealtime,128) \ SERVICE(uart, BSP_UART_TaskCallback, osPriorityNormal,128) \ @@ -64,13 +64,13 @@ //SERVICE(exti, BSP_EXTI_TaskCallback, osPriorityNormal,256) -/****************** ñ ******************/ +/****************** 各服务配置表 ******************/ ConfItem* systemConfig = CF_DICT{ - //û߼ + //用户逻辑服务配置 {"sys",CF_DICT{ - //̸PID + //底盘跟随PID配置 {"rotate-pid",CF_DICT{ {"p", IM_PTR(float, 1.5)}, {"i", IM_PTR(float, 0)}, @@ -82,11 +82,11 @@ ConfItem* systemConfig = CF_DICT{ CF_DICT_END }}, - //̷ + //底盘服务配置 {"chassis", CF_DICT{ - //ѭ + //任务循环周期 {"task-interval", IM_PTR(uint16_t, 2)}, - //̳ߴϢ + //底盘尺寸信息 {"info", CF_DICT{ {"wheelbase", IM_PTR(float, 100)}, {"wheeltrack", IM_PTR(float, 100)}, @@ -95,7 +95,7 @@ ConfItem* systemConfig = CF_DICT{ {"offset-y", IM_PTR(float, 0)}, CF_DICT_END }}, - //ƶٶ/ٶ + //底盘移动速度/加速度配置 {"move", CF_DICT{ {"max-vx", IM_PTR(float, 2000)}, {"max-vy", IM_PTR(float, 2000)}, @@ -104,7 +104,7 @@ ConfItem* systemConfig = CF_DICT{ {"y-acc", IM_PTR(float, 1000)}, CF_DICT_END }}, - //ǰ + //左前电机配置 {"motor-fl", CF_DICT{ {"type", "M3508"}, {"id", IM_PTR(uint16_t, 1)}, @@ -119,7 +119,7 @@ ConfItem* systemConfig = CF_DICT{ }}, CF_DICT_END }}, - //ǰ + //右前电机配置 {"motor-fr", CF_DICT{ {"type", "M3508"}, {"id", IM_PTR(uint16_t, 2)}, @@ -134,7 +134,7 @@ ConfItem* systemConfig = CF_DICT{ }}, CF_DICT_END }}, - // + //左后电机配置 {"motor-bl", CF_DICT{ {"type", "M3508"}, {"id", IM_PTR(uint16_t, 3)}, @@ -149,7 +149,7 @@ ConfItem* systemConfig = CF_DICT{ }}, CF_DICT_END }}, - //Һ + //右后电机配置 {"motor-br", CF_DICT{ {"type", "M3508"}, {"id", IM_PTR(uint16_t, 4)}, @@ -167,14 +167,14 @@ ConfItem* systemConfig = CF_DICT{ CF_DICT_END }}, - //̨ + //云台服务配置 {"gimbal", CF_DICT{ - //yaw pitch е + //yaw pitch 机械零点 {"zero-yaw",IM_PTR(uint16_t,4010)}, {"zero-pitch",IM_PTR(uint16_t,5300)}, - //ѭ + //任务循环周期 {"task-interval", IM_PTR(uint16_t, 10)}, - //pidãΪǶ⻷ + //陀螺仪pid参数设置，作为角度外环 {"yaw-imu-pid",CF_DICT{ {"p", IM_PTR(float, -90)}, {"i", IM_PTR(float, 0)}, @@ -183,12 +183,12 @@ ConfItem* systemConfig = CF_DICT{ {"max-out", IM_PTR(float, 1000)}, CF_DICT_END }}, - //̨ + //云台电机配置 {"motor-yaw", CF_DICT{ {"type", "M6020"}, {"id", IM_PTR(uint16_t, 1)}, {"can-x", IM_PTR(uint8_t, 1)}, - //ڻ + //内环参数设置 {"speed-pid", CF_DICT{ {"p", IM_PTR(float, 15)}, {"i", IM_PTR(float, 0)}, @@ -199,7 +199,7 @@ ConfItem* systemConfig = CF_DICT{ }}, CF_DICT_END }}, - //pidãΪǶ⻷ + //陀螺仪pid参数设置，作为角度外环 {"pitch-imu-pid",CF_DICT{ {"p", IM_PTR(float, 63)}, {"i", IM_PTR(float, 0)}, @@ -212,7 +212,7 @@ ConfItem* systemConfig = CF_DICT{ {"type", "M6020"}, {"id", IM_PTR(uint16_t, 4)}, {"can-x", IM_PTR(uint8_t, 2)}, - //ڻ + //内环参数设置 {"speed-pid", CF_DICT{ {"p", IM_PTR(float, 15)}, {"i", IM_PTR(float, 0)}, @@ -226,13 +226,13 @@ ConfItem* systemConfig = CF_DICT{ CF_DICT_END }}, - // + //发射服务配置 {"shooter", CF_DICT{ - //ѭ + //任务循环周期 {"task-interval", IM_PTR(uint16_t, 10)}, - //һĽǶ + //拨一发弹丸的角度 {"trigger-angle",IM_PTR(float,45)}, - // + //发射机构电机配置 {"fric-motor-left", CF_DICT{ {"type", "M3508"}, {"id", IM_PTR(uint16_t, 2)}, @@ -292,9 +292,9 @@ ConfItem* systemConfig = CF_DICT{ CF_DICT_END }}, - //CAN + //CAN服务配置 {"can", CF_DICT{ - //CANϢ + //CAN控制器信息 {"cans", CF_DICT{ {"0", CF_DICT{ {"hcan", &hcan1}, @@ -308,7 +308,7 @@ ConfItem* systemConfig = CF_DICT{ }}, CF_DICT_END }}, - //ʱ֡ + //定时帧配置 {"repeat-buffers", CF_DICT{ {"0", CF_DICT{ {"can-x", IM_PTR(uint8_t, 1)}, @@ -339,13 +339,13 @@ ConfItem* systemConfig = CF_DICT{ CF_DICT_END }}, - //ңط + //遥控服务配置 {"rc",CF_DICT{ {"uart-x",IM_PTR(uint8_t, 3)}, CF_DICT_END }}, - //ϵͳ + //裁判系统服务配置 {"judge",CF_DICT{ {"max-tx-queue-length",IM_PTR(uint16_t,20)}, {"task-interval",IM_PTR(uint16_t,150)}, @@ -353,7 +353,7 @@ ConfItem* systemConfig = CF_DICT{ CF_DICT_END }}, - //ڷ + //串口服务配置 {"uart",CF_DICT{ {"uarts",CF_DICT{ {"0",CF_DICT{ @@ -373,12 +373,12 @@ ConfItem* systemConfig = CF_DICT{ CF_DICT_END }}, - //ߵ + //惯导服务配置 {"ins",CF_DICT{ {"spi-x",IM_PTR(uint8_t,1)}, {"tim-x",IM_PTR(uint8_t,10)}, {"channel-x",IM_PTR(uint8_t,1)}, - //¶ȿPID + //温度控制PID配置 {"tmp-pid", CF_DICT{ {"p", IM_PTR(float, 0.15)}, {"i", IM_PTR(float, 0.01)}, @@ -390,7 +390,7 @@ ConfItem* systemConfig = CF_DICT{ CF_DICT_END }}, - //SPI + //SPI驱动服务配置 {"spi",CF_DICT{ {"spis",CF_DICT{ {"0",CF_DICT{ @@ -419,7 +419,7 @@ ConfItem* systemConfig = CF_DICT{ CF_DICT_END }}, - //ʱ + //定时器服务配置 {"tim",CF_DICT{ {"tims",CF_DICT{ {"0",CF_DICT{ @@ -433,7 +433,7 @@ ConfItem* systemConfig = CF_DICT{ CF_DICT_END }}, - //ⲿжϷ + //外部中断服务配置 // {"exti",CF_DICT{ // {"extis",CF_DICT{ // {"0",CF_DICT{ diff --git a/conf/yamlparser.cpp b/conf/yamlparser.cpp new file mode 100644 index 0000000000000000000000000000000000000000..da25920ac67cdf5f819db73edf9d4c5dafd48a2c --- /dev/null +++ b/conf/yamlparser.cpp @@ -0,0 +1,299 @@ + +/** +实现一个最小化的、类YAML的，在C板上消耗最小化资源的文本解析器。 + +限制： +只能使用最基础的数据类型：int32, uint32, float, 字符串；字典。 +字符串不支持任何形式的转义，而且必须以双引号包围。 +根对象必须为字典。 +一个键只能占用一行。 +元素节点不保存其值类型，读取者读取时必须自行选取与解析器输出行为一致的类型。 + +只支持小数形式的浮点数（123.456），不支持指数形式。 +如数字被写作浮点形式，即使它的小数部分在当前精度下会变为0，值也会被按float存储。 + +键名不得包含空格或者冒号（其他任意字符均可）。 + +没有错误返回机制，因为将Children（字典）与其他32位长度的类型union起来时，已无法自动释放资源。 +检测到未预期的字符时，会直接试图跳转至结尾状态。 +*/ + +#include +#include "yamlparser.h" +#include "hasher.h" +extern "C" { +#include "vector.h" +} +#include "cmsis_os.h" + +static UimlYamlNode* CreateYamlNode() { + UimlYamlNode* ret = (UimlYamlNode*)pvPortMalloc(sizeof(UimlYamlNode)); + ret->Children = NULL; + ret->Next = NULL; + ret->NameRef = NULL; + ret->NameHash = 0; + ret->I32 = 0; + return ret; +} + +static size_t UimlParseYamlDictIndent(const char* input, UimlYamlNode** output, int indentSpace) { + UimlYamlNode* out = nullptr, *current = nullptr; + auto inputLength = strlen(input); + + enum { SkipWhitespace, Key, Colon, TryValue, SkipToNewLine, ValueString, ValueNumberBegin, ValueNumberInteger, ValueNumberDecimal, ValueDict } state = SkipWhitespace; + enum { NextKey, NextColon, NextValue, NextLine } nextElem = NextKey; + enum { Undetermined, vtU32, vtNeg32, vtF32, vtNegF32, vtString, vtDict } valueType = Undetermined; + int spacesToSkip = 0; + bool decrementSpacesToSkip = true; + const char * keyNameRef = nullptr; + size_t keyNameLength = 0; + union { + const char * strValueBegin = nullptr; + uint32_t integerPart; + }; + union { + char * strValueRef = nullptr; + float decimalPart; + }; + float decimalCoeff = 1.0f; + + auto gotoNextElem = [&](){ + switch (nextElem) { + case NextKey: state = Key; nextElem = NextColon; break; + case NextColon: state = Colon; nextElem = NextValue; break; + case NextValue: state = TryValue; nextElem = NextLine; break; + case NextLine: state = SkipToNewLine; break; + } + }; + + spacesToSkip = indentSpace; + char c; + size_t i, lasti = 0; + for (i = 0; i < inputLength; i++) { + c = input[i]; + if (c == '#' && state != ValueString) state = SkipToNewLine; // 注释起始 + switch (state) { + case SkipWhitespace: + if (c == '\n') { + if (nextElem == NextValue) { + state = ValueDict; + } + break; + } else if (c != ' ') { + if (spacesToSkip == 0) { + spacesToSkip = 0; + decrementSpacesToSkip = false; + i--; + } else if (spacesToSkip == 2) { // 少一格缩进，回退 + goto BreakLoop; + } else { + state = SkipToNewLine; + } + gotoNextElem(); + } else if (spacesToSkip < 0) { + state = SkipToNewLine; + } else { + if (decrementSpacesToSkip) spacesToSkip--; + } + break; + + case Key: + if (keyNameLength == 0) { // 初始状态 + keyNameRef = input + i; + keyNameLength++; + // 新建节点，串入链表 + auto newNode = CreateYamlNode(); + if (current != nullptr) current->Next = newNode; + current = newNode; + current->NameRef = keyNameRef; + if (out == nullptr) out = current; + } else { // 探测键名 + if (c == ' ' || c == ':') { // 结束键名 + i--; + state = SkipWhitespace; + current->NameHash = Hasher_UIML32((uint8_t*)keyNameRef, keyNameLength); + } else { // 键名继续 + keyNameLength++; + } + } + break; + + case Colon: + if (c != ':') { + state = SkipToNewLine; + } + state = SkipWhitespace; + break; + + case TryValue: + if (c == '"') state = ValueString; + else if ((c >= '0' && c <= '9') || c == '-') { state = ValueNumberBegin; i--; } + else if (c == ' ') state = SkipWhitespace; + else if (c == '\n') state = ValueDict; + else state = SkipToNewLine; + break; + + case ValueString: + if (strValueBegin == nullptr) { + strValueBegin = input + i; + } else if (c == '"') { + auto length = input + i - strValueBegin; + valueType = vtString; + strValueRef = (char*)pvPortMalloc(length + 1); + memcpy(strValueRef, strValueBegin, length); + strValueRef[length] = '\0'; + state = SkipToNewLine; + } + break; + + case ValueNumberBegin: + integerPart = 0; + decimalPart = 0.0f; + valueType = vtU32; + if (c == '-') { valueType = vtNeg32; } // 拿Neg32表示是负数 + else i--; // 没有负号就回退一位，保证下一位读出来是数字 + state = ValueNumberInteger; // 直接交给专门处理数字的部分处理 + break; + + case ValueNumberInteger: + if (c >= '0' && c <= '9') { + integerPart *= 10; + integerPart += (c - '0'); + } else if (c == '.') { + state = ValueNumberDecimal; // 去处理小数 + valueType = (valueType == vtNeg32) ? vtNegF32 : vtF32; + } else if (c == ' ' || c == '\n') { + i--; + state = SkipToNewLine; // 尝试结束 + } else { + // return UimlYamlInvalidValue; + state = SkipToNewLine; + } + break; + + case ValueNumberDecimal: + if (c >= '0' && c <= '9') { + decimalCoeff *= 0.1f; + decimalPart += decimalCoeff * (c - '0'); + } else if (c == ' ' || c == '\n') { + i--; + state = SkipToNewLine; // 尝试结束 + } else { + // return UimlYamlInvalidValue; + state = SkipToNewLine; + } + break; + + case ValueDict: + { + valueType = vtDict; + auto dictResult = UimlParseYamlDictIndent(input + i, ¤t->Children, indentSpace + 2); + if ((int)dictResult < 0) { // 如果子级出错，递归退出 + return dictResult; + } + i += dictResult; + i--; // 加上lasti之后会指向\n，向前退一个字节，确保可以正确结束行 + state = SkipToNewLine; // 复用这部分代码做好下一个元素的准备 + break; + } + + case SkipToNewLine: + if (c == '\n') { // 一行结束 + if (strValueRef) { + } // TODO + + switch (valueType) { + case Undetermined: // 没有值也没有Key的空行 + break; + case vtU32: // 没有负号的整数 + current->U32 = integerPart; + break; + case vtNeg32: // 负的整数 + current->I32 = -(int32_t)integerPart; + break; + case vtF32: // 没有负号的小数 + current->F32 = integerPart + decimalPart; + break; + case vtNegF32: // 负的小数 + current->F32 = -(integerPart + decimalPart); + break; + case vtString: + current->Str = strValueRef; + strValueRef = nullptr; + break; + case vtDict: break; + } + + spacesToSkip = indentSpace; // 解析下一个键值对 + state = SkipWhitespace; + nextElem = NextKey; + decrementSpacesToSkip = true; + keyNameLength = 0; + strValueBegin = 0; + strValueRef = nullptr; + decimalCoeff = 1.0f; + lasti = i; + break; + } else if (c != ' ') { + // return UimlYamlInvalidValue; + } + break; + } + } + +BreakLoop: + + *output = out; + + return lasti == 0 ? i : lasti; +} + +size_t UimlYamlParse(const char* input, UimlYamlNode** output) { + *output = CreateYamlNode(); + return UimlParseYamlDictIndent(input, &((*output)->Children), 0); +} + +UimlYamlNode* UimlYamlGetValue(UimlYamlNode* input, const char* childName) { + auto nameLength = strlen(childName); + auto nameHash = Hasher_UIML32((uint8_t*)childName, nameLength); + UimlYamlNode* ret = input; + + while (ret != NULL) { + if (ret->NameHash == nameHash && !strncmp(ret->NameRef, childName, nameLength)) { + return ret; + } + ret = ret->Next; + } + + return NULL; +} + +UimlYamlNode* UimlYamlGetValueByPath(UimlYamlNode* input, const char* path) { + const char *begin = path, *end; + UimlYamlNode *ret = input; + + do { + end = strchr(begin, '/'); + if (end > begin || end == NULL) { + auto nameLength = (end == NULL) ? strlen(begin) : (end - begin); + auto nameHash = Hasher_UIML32((uint8_t*)begin, nameLength); + + ret = ret->Children; + + while (ret != NULL) { + if (ret->NameHash == nameHash && !strncmp(ret->NameRef, begin, nameLength)) { + goto Found; // 跳出内循环并跳过错误情况处理 + } + ret = ret->Next; + } + + return NULL; +Found: + begin = end + 1; + } else if (end == begin) { + begin = end + 1; // 没用的单/，只跳过 + } + } while (end != NULL); + + return ret; +} diff --git a/conf/yamlparser.h b/conf/yamlparser.h new file mode 100644 index 0000000000000000000000000000000000000000..d0b00bdd49a362b20e3fbf9b0c476fbad2a384aa --- /dev/null +++ b/conf/yamlparser.h @@ -0,0 +1,29 @@ + +#include +#include + +struct UimlYamlNode { + uint32_t NameHash; + const char* NameRef; + UimlYamlNode* Next; + union { + UimlYamlNode* Children; + uint32_t U32; + int32_t I32; + float F32; + char* Str; + }; +}; + +#ifdef __cplusplus +extern "C" { +#endif + +size_t UimlYamlParse(const char* input, struct UimlYamlNode** output); + +struct UimlYamlNode* UimlYamlGetValue(struct UimlYamlNode* input, const char* childName); +struct UimlYamlNode* UimlYamlGetValueByPath(struct UimlYamlNode* input, const char* path); + +#ifdef __cplusplus +} // extern "C" { +#endif diff --git a/inc/bmi088/AHRS.h b/inc/bmi088/AHRS.h new file mode 100644 index 0000000000000000000000000000000000000000..4beba342324318cef69aa650d946c30a97a28af2 --- /dev/null +++ b/inc/bmi088/AHRS.h @@ -0,0 +1,61 @@ +#ifndef AHRS_H +#define AHRS_H + +#include "AHRS_MiddleWare.h" + +/** + * @brief 根据加速度的数据，磁力计的数据进行四元数初始化 + * @param[in] 需要初始化的四元数数组 + * @param[in] 用于初始化的加速度计,(x,y,z)不为空 单位 m/s2 + * @param[in] 用于初始化的磁力计计,(x,y,z)不为空 单位 uT + * @retval 返回空 + */ +extern void AHRS_init(fp32 quat[4], const fp32 accel[3], const fp32 mag[3]); + +/** + * @brief 根据陀螺仪的数据，加速度的数据，磁力计的数据进行四元数更新 + * @param[in] 需要更新的四元数数组 + * @param[in] 更新定时时间，固定定时调用，例如1000Hz，传入的数据为0.001f, + * @param[in] 用于更新的陀螺仪数据,数组顺序(x,y,z) 单位 rad + * @param[in] 用于初始化的加速度数据,数组顺序(x,y,z) 单位 m/s2 + * @param[in] 用于初始化的磁力计数据,数组顺序(x,y,z) 单位 uT + * @retval 1:更新成功, 0:更新失败 + */ +extern bool_t AHRS_update(fp32 quat[4], const fp32 timing_time, const fp32 gyro[3], const fp32 accel[3], const fp32 mag[3]); + +/** + * @brief 根据四元数大小计算对应的欧拉角偏航yaw + * @param[in] 四元数数组，不为NULL + * @retval 返回的偏航角yaw 单位 rad + */ +extern fp32 get_yaw(const fp32 quat[4]); + +/** + * @brief 根据四元数大小计算对应的欧拉角俯仰角 pitch + * @param[in] 四元数数组，不为NULL + * @retval 返回的俯仰角 pitch 单位 rad + */ +extern fp32 get_pitch(const fp32 quat[4]); +/** + * @brief 根据四元数大小计算对应的欧拉角横滚角 roll + * @param[in] 四元数数组，不为NULL + * @retval 返回的横滚角 roll 单位 rad + */ +extern fp32 get_roll(const fp32 quat[4]); + +/** + * @brief 根据四元数大小计算对应的欧拉角yaw，pitch，roll + * @param[in] 四元数数组，不为NULL + * @param[in] 返回的偏航角yaw 单位 rad + * @param[in] 返回的俯仰角pitch 单位 rad + * @param[in] 返回的横滚角roll 单位 rad + */ +extern void get_angle(const fp32 quat[4], fp32 *yaw, fp32 *pitch, fp32 *roll); +/** + * @brief 返回当前的重力加速度 + * @param[in] 空 + * @retval 返回重力加速度 单位 m/s2 + */ +extern fp32 get_carrier_gravity(void); + +#endif diff --git a/services/ins/bmi088/AHRS_middleware.h b/inc/bmi088/AHRS_middleware.h similarity index 66% rename from services/ins/bmi088/AHRS_middleware.h rename to inc/bmi088/AHRS_middleware.h index 2351731bbb2c4443a2e28e4c7c294fda7eaafcd0..ab9010af50ebd9b8324bcf0405ff3d184337a2ac 100644 --- a/services/ins/bmi088/AHRS_middleware.h +++ b/inc/bmi088/AHRS_middleware.h @@ -2,11 +2,11 @@ /** ****************************(C) COPYRIGHT 2019 DJI**************************** * @file AHRS_MiddleWare.c/h - * @brief ̬м㣬Ϊ̬ṩغ + * @brief 姿态解算中间层，为姿态解算提供相关函数 * @note * @history * Version Date Author Modification - * V1.0.0 Dec-26-2018 RM 1. + * V1.0.0 Dec-26-2018 RM 1. 完成 * @verbatim ============================================================================== @@ -19,37 +19,37 @@ #ifndef AHRS_MIDDLEWARE_H #define AHRS_MIDDLEWARE_H -//¶Ӧ -typedef signed char int8_t; -typedef signed short int int16_t; -typedef signed int int32_t; -typedef signed long long int64_t; +// //重新对应的数据类型 +// typedef signed char int8_t; +// typedef signed short int int16_t; +// typedef signed int int32_t; +// typedef signed long long int64_t; -/* exact-width unsigned integer types */ -typedef unsigned char uint8_t; -typedef unsigned short int uint16_t; -typedef unsigned int uint32_t; -typedef unsigned long long uint64_t; +// /* exact-width unsigned integer types */ +// typedef unsigned char uint8_t; +// typedef unsigned short int uint16_t; +// typedef unsigned int uint32_t; +// typedef unsigned long long uint64_t; typedef unsigned char bool_t; typedef float fp32; typedef double fp64; -// NULL +//定义 NULL #ifndef NULL #define NULL 0 #endif -//PI ֵ +//定义PI 值 #ifndef PI #define PI 3.14159265358979f #endif -// Ƕ()ת ȵı +//定义 角度(度)转换到 弧度的比例 #ifndef ANGLE_TO_RAD #define ANGLE_TO_RAD 0.01745329251994329576923690768489f #endif -// ת Ƕȵı +//定义 弧度 转换到 角度的比例 #ifndef RAD_TO_ANGLE #define RAD_TO_ANGLE 57.295779513082320876798154814105f #endif diff --git a/services/ins/bmi088/BMI088reg.h b/inc/bmi088/BMI088reg.h similarity index 100% rename from services/ins/bmi088/BMI088reg.h rename to inc/bmi088/BMI088reg.h diff --git a/services/ins/bmi088/bmi088_driver.h b/inc/bmi088/bmi088_driver.h similarity index 100% rename from services/ins/bmi088/bmi088_driver.h rename to inc/bmi088/bmi088_driver.h diff --git a/services/ins/bmi088/AHRS.lib b/lib/AHRS.lib similarity index 100% rename from services/ins/bmi088/AHRS.lib rename to lib/AHRS.lib diff --git a/services/bsp/bsp_can.c b/services/bsp/bsp_can.c index 8e7eefb672b664ef42dfa5e68408c5d826e2e4e1..a1738c8dfd2393a0d38506b5351dabdc2f58c7ed 100644 --- a/services/bsp/bsp_can.c +++ b/services/bsp/bsp_can.c @@ -1,34 +1,34 @@ #include "config.h" #include "softbus.h" #include "cmsis_os.h" - +#include #include "can.h" -//CANϢ +//CAN句柄信息 typedef struct { CAN_HandleTypeDef* hcan; - uint8_t number; //canXеX - SoftBusReceiverHandle fastHandle; //ٹ㲥 + uint8_t number; //canX中的X + SoftBusReceiverHandle fastHandle; //快速广播句柄 }CANInfo; -//ѭͻ +//循环发送缓冲区 typedef struct { - CANInfo* canInfo; //ָ󶨵CANInfo - uint16_t frameID; //can֡ID - uint8_t* data; //can֡8ֽ + CANInfo* canInfo; //指向所绑定的CANInfo + uint16_t frameID; //can帧ID + uint8_t* data; //can帧8字节数据 }CANRepeatBuffer; -//CAN +//本CAN服务数据 typedef struct { - CANInfo* canList; //canб - uint8_t canNum; //can - CANRepeatBuffer* repeatBuffers; //ѭͻ - uint8_t bufferNum; //ѭͻ - uint8_t initFinished; //ʼɱ־ + CANInfo* canList; //can列表 + uint8_t canNum; //can数量 + CANRepeatBuffer* repeatBuffers; //循环发送缓冲区 + uint8_t bufferNum; //循环发送缓冲区数量 + uint8_t initFinished; //初始化完成标志 }CANService; CANService canService = {0}; -// +//函数声明 void BSP_CAN_Init(ConfItem* dict); void BSP_CAN_InitInfo(CANInfo* info, ConfItem* dict); void BSP_CAN_InitHardware(CANInfo* info); @@ -38,7 +38,7 @@ bool BSP_CAN_SendOnceCallback(const char* name, SoftBusFrame* frame, void* bindD void BSP_CAN_TimerCallback(void const *argument); uint8_t BSP_CAN_SendFrame(CAN_HandleTypeDef* hcan,uint16_t StdId,uint8_t* data); -//canսж +//can接收结束中断 void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan) { CAN_RxHeaderTypeDef header; @@ -52,19 +52,19 @@ void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan) for(uint8_t i = 0; i < canService.canNum; i++) { CANInfo* canInfo = &canService.canList[i]; - if(hcan == canInfo->hcan) //ҵжϻصжӦcanбcan + if(hcan == canInfo->hcan) //找到中断回调函数中对应can列表的can { uint16_t frameID = header.StdId; - Bus_FastBroadcastSend(canInfo->fastHandle, {&frameID, rx_data}); //ٹ㲥 + Bus_FastBroadcastSend(canInfo->fastHandle, {&frameID, rx_data}); //快速广播发布数据 break; } } } -//canص +//can任务回调函数 void BSP_CAN_TaskCallback(void const * argument) { - //ٽ + //进入临界区 portENTER_CRITICAL(); BSP_CAN_Init((ConfItem*)argument); portEXIT_CRITICAL(); @@ -74,7 +74,7 @@ void BSP_CAN_TaskCallback(void const * argument) void BSP_CAN_Init(ConfItem* dict) { - //ûõcan + //计算用户配置的can数量 canService.canNum = 0; for(uint8_t num = 0; ; num++) { @@ -85,7 +85,7 @@ void BSP_CAN_Init(ConfItem* dict) else break; } - //ʼcanϢ + //初始化各can信息 canService.canList = pvPortMalloc(canService.canNum * sizeof(CANInfo)); for(uint8_t num = 0; num < canService.canNum; num++) { @@ -93,12 +93,12 @@ void BSP_CAN_Init(ConfItem* dict) confName[5] = num + '0'; BSP_CAN_InitInfo(&canService.canList[num], Conf_GetPtr(dict, confName, ConfItem)); } - //ʼCANӲ + //初始化CAN硬件参数 for(uint8_t num = 0; num < canService.canNum; num++) { BSP_CAN_InitHardware(&canService.canList[num]); } - //ûõѭͻ + //计算用户配置的循环发送缓冲区数量 canService.bufferNum = 0; for(uint8_t num = 0; ; num++) { @@ -109,7 +109,7 @@ void BSP_CAN_Init(ConfItem* dict) else break; } - //ʼѭ + //初始化各循环缓冲区 canService.repeatBuffers = pvPortMalloc(canService.bufferNum * sizeof(CANRepeatBuffer)); for(uint8_t num = 0; num < canService.bufferNum; num++) { @@ -117,14 +117,14 @@ void BSP_CAN_Init(ConfItem* dict) confName[15] = num + '0'; BSP_CAN_InitRepeatBuffer(&canService.repeatBuffers[num], Conf_GetPtr(dict, confName, ConfItem)); } - //Ĺ㲥 + //订阅广播 Bus_RegisterRemoteFunc(NULL, BSP_CAN_SetBufCallback, "/can/set-buf"); Bus_RegisterRemoteFunc(NULL, BSP_CAN_SetBufCallback, "/can/send-once"); canService.initFinished = 1; } -//ʼCANϢ +//初始化CAN信息 void BSP_CAN_InitInfo(CANInfo* info, ConfItem* dict) { info->hcan = Conf_GetPtr(dict, "hcan", CAN_HandleTypeDef); @@ -134,10 +134,10 @@ void BSP_CAN_InitInfo(CANInfo* info, ConfItem* dict) info->fastHandle = Bus_CreateReceiverHandle(name); } -//ʼӲ +//初始化硬件参数 void BSP_CAN_InitHardware(CANInfo* info) { - //CANʼ + //CAN过滤器初始化 CAN_FilterTypeDef canFilter = {0}; canFilter.FilterActivation = ENABLE; canFilter.FilterMode = CAN_FILTERMODE_IDMASK; @@ -157,30 +157,30 @@ void BSP_CAN_InitHardware(CANInfo* info) canFilter.FilterBank = 0; } HAL_CAN_ConfigFilter(info->hcan, &canFilter); - //CAN + //开启CAN HAL_CAN_Start(info->hcan); HAL_CAN_ActivateNotification(info->hcan, CAN_IT_RX_FIFO0_MSG_PENDING); } -//ʼѭͻ +//初始化循环发送缓冲区 void BSP_CAN_InitRepeatBuffer(CANRepeatBuffer* buffer, ConfItem* dict) { - //ظ֡can + //重复帧绑定can uint8_t canX = Conf_GetValue(dict, "can-x", uint8_t, 0); for(uint8_t i = 0; i < canService.canNum; ++i) if(canService.canList[i].number == canX) buffer->canInfo = &canService.canList[i]; - //ظ֡canid + //设置重复帧can的id域 buffer->frameID = Conf_GetValue(dict, "id", uint16_t, 0x00); buffer->data = pvPortMalloc(8); memset(buffer->data, 0, 8); - //ʱʱظ֡ + //开启软件定时器定时发送重复帧 uint16_t sendInterval = Conf_GetValue(dict, "interval", uint16_t, 100); osTimerDef(CAN, BSP_CAN_TimerCallback); osTimerStart(osTimerCreate(osTimer(CAN), osTimerPeriodic, buffer), sendInterval); } -//ϵͳʱص +//系统定时器回调 void BSP_CAN_TimerCallback(void const *argument) { if(!canService.initFinished) @@ -189,7 +189,7 @@ void BSP_CAN_TimerCallback(void const *argument) BSP_CAN_SendFrame(buffer->canInfo->hcan, buffer->frameID, buffer->data); } -//CAN֡ +//CAN发送数据帧 uint8_t BSP_CAN_SendFrame(CAN_HandleTypeDef* hcan,uint16_t stdId,uint8_t* data) { CAN_TxHeaderTypeDef txHeader; @@ -205,7 +205,7 @@ uint8_t BSP_CAN_SendFrame(CAN_HandleTypeDef* hcan,uint16_t stdId,uint8_t* data) return retVal; } -//ѭijֽԶ̺ص +//设置循环缓存区某部分字节数据远程函数回调 bool BSP_CAN_SetBufCallback(const char* name, SoftBusFrame* frame, void* bindData) { if(!Bus_CheckMapKeys(frame, {"can-x", "id", "pos", "len", "data"})) @@ -228,7 +228,7 @@ bool BSP_CAN_SetBufCallback(const char* name, SoftBusFrame* frame, void* bindDat } return false; } -//һ֡canԶ̺ص +//发送一帧can数据远程函数回调 bool BSP_CAN_SendOnceCallback(const char* name, SoftBusFrame* frame, void* bindData) { if(!Bus_CheckMapKeys(frame, {"can-x", "id", "data"})) diff --git a/services/bsp/bsp_exti.c b/services/bsp/bsp_exti.c index 225e57c1c9b793975c4902f4384d5d3d73e1f5f4..dd5bfa3fbe170c05822394c6a7525a4644297705 100644 --- a/services/bsp/bsp_exti.c +++ b/services/bsp/bsp_exti.c @@ -4,7 +4,7 @@ #include "gpio.h" #include "stdio.h" -//EXTI GPIOϢ +//EXTI GPIO信息 typedef struct { GPIO_TypeDef* gpioX; @@ -12,43 +12,43 @@ typedef struct SoftBusReceiverHandle fastHandle; }EXTIInfo; -//EXTI +//EXTI服务数据 typedef struct { EXTIInfo extiList[16]; uint8_t extiNum; uint8_t initFinished; }EXTIService; -// +//函数声明 void BSP_EXTI_Init(ConfItem* dict); void BSP_EXIT_InitInfo(EXTIInfo* info, ConfItem* dict); EXTIService extiService={0}; -//ⲿжϷ +//外部中断服务函数 void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) { if(!extiService.initFinished) return; - uint8_t pin = 31 - __clz((uint32_t)GPIO_Pin);//ʹں˺__clzGPIO_Pinǰ0ĸӶõжߺ + uint8_t pin = 31 - __CLZ((uint32_t)GPIO_Pin);//使用内核函数__clz就算GPIO_Pin前导0的个数，从而得到中断线号 EXTIInfo* extiInfo = &extiService.extiList[pin]; GPIO_PinState state = HAL_GPIO_ReadPin(extiInfo->gpioX, GPIO_Pin); Bus_FastBroadcastSend(extiInfo->fastHandle,{&state}); } -//EXTIص +//EXTI任务回调函数 void BSP_EXTI_TaskCallback(void const * argument) { - //ٽ + //进入临界区 portENTER_CRITICAL(); BSP_EXTI_Init((ConfItem*)argument); portEXIT_CRITICAL(); vTaskDelete(NULL); } -//EXTIʼ +//EXTI初始化 void BSP_EXTI_Init(ConfItem* dict) { - //ûõexit + //计算用户配置的exit数量 extiService.extiNum = 0; for(uint8_t num = 0; ; num++) { @@ -59,7 +59,7 @@ void BSP_EXTI_Init(ConfItem* dict) else break; } - //ʼextiϢ + //初始化各exti信息 for(uint8_t num = 0; num < extiService.extiNum; num++) { char confName[9] = {0}; @@ -69,14 +69,14 @@ void BSP_EXTI_Init(ConfItem* dict) extiService.initFinished=1; } -//ʼEXTIϢ +//初始化EXTI信息 void BSP_EXIT_InitInfo(EXTIInfo* info, ConfItem* dict) { uint8_t pin = Conf_GetValue(dict, "pin-x", uint8_t, 0); info[pin].gpioX = Conf_GetPtr(dict, "gpio-x", GPIO_TypeDef); char name[12] = {0}; sprintf(name,"/exti/pin%d",pin); - //ӳGPIO_PIN=2^pin + //重新映射至GPIO_PIN=2^pin info[pin].pin = 1 << pin; info[pin].fastHandle = Bus_CreateReceiverHandle(name); } diff --git a/services/bsp/bsp_spi.c b/services/bsp/bsp_spi.c index f1f8879ee82fab6a88ddbc3ae53e22543648f129..b9806ba053e65fa739bfec1f98e42149b15bde24 100644 --- a/services/bsp/bsp_spi.c +++ b/services/bsp/bsp_spi.c @@ -5,6 +5,7 @@ #include "gpio.h" #include "spi.h" #include +#include typedef struct { @@ -13,10 +14,10 @@ typedef struct uint16_t pin; }CSInfo; -//SPIϢ +//SPI句柄信息 typedef struct { SPI_HandleTypeDef* hspi; - uint8_t number; //SPIXеX + uint8_t number; //SPIX中的X osSemaphoreId lock; CSInfo* csList; @@ -30,7 +31,7 @@ typedef struct { SoftBusReceiverHandle fastHandle; }SPIInfo; -//SPI +//SPI服务数据 typedef struct { SPIInfo* spiList; uint8_t spiNum; @@ -39,7 +40,7 @@ typedef struct { SPIService spiService = {0}; -// +//函数声明 void BSP_SPI_Init(ConfItem* dict); void BSP_SPI_InitInfo(SPIInfo* info, ConfItem* dict); void BSP_SPI_InitCS(SPIInfo* info, ConfItem* dict); @@ -51,25 +52,25 @@ void HAL_SPI_TxRxCpltCallback(SPI_HandleTypeDef *hspi) for(uint8_t num = 0; num < spiService.spiNum; num++) { SPIInfo* spiInfo = &spiService.spiList[num]; - if(hspi == spiInfo->hspi) //ҵжϻصжӦspiбspi + if(hspi == spiInfo->hspi) //找到中断回调函数中对应spi列表的spi { - Bus_FastBroadcastSend(spiInfo->fastHandle, {spiService.spiList->recvBuffer.data, &spiService.spiList->recvBuffer.dataLen}); // - //Ƭѡȫ + Bus_FastBroadcastSend(spiInfo->fastHandle, {spiService.spiList->recvBuffer.data, &spiService.spiList->recvBuffer.dataLen}); //发送数据 + //将片选全部拉高 for(uint8_t i = 0; i < spiService.spiList[num].csNum; i++) { HAL_GPIO_WritePin(spiService.spiList[num].csList[i].gpioX, spiService.spiList[num].csList[i].pin, GPIO_PIN_SET); } - // + //解锁 osSemaphoreRelease(spiService.spiList[num].lock); break; } } } -//SPIص +//SPI任务回调函数 void BSP_SPI_TaskCallback(void const * argument) { - //ٽ + //进入临界区 portENTER_CRITICAL(); BSP_SPI_Init((ConfItem*)argument); portEXIT_CRITICAL(); @@ -78,7 +79,7 @@ void BSP_SPI_TaskCallback(void const * argument) } void BSP_SPI_Init(ConfItem* dict) { - //ûõspi + //计算用户配置的spi数量 spiService.spiNum = 0; for(uint8_t num = 0; ; num++) { @@ -90,7 +91,7 @@ void BSP_SPI_Init(ConfItem* dict) break; } - //ʼspiϢ + //初始化各spi信息 spiService.spiList = pvPortMalloc(spiService.spiNum * sizeof(SPIInfo)); for(uint8_t num = 0; num < spiService.spiNum; num++) { @@ -99,12 +100,12 @@ void BSP_SPI_Init(ConfItem* dict) BSP_SPI_InitInfo(&spiService.spiList[num], Conf_GetPtr(dict, confName, ConfItem)); } - //עԶ̷ + //注册远程服务 Bus_RegisterRemoteFunc(NULL, BSP_SPI_BlockCallback, "/spi/block"); Bus_RegisterRemoteFunc(NULL,BSP_SPI_DMACallback, "/spi/trans/dma"); spiService.initFinished = 1; } -//ʼspiϢ +//初始化spi信息 void BSP_SPI_InitInfo(SPIInfo* info, ConfItem* dict) { info->hspi = Conf_GetPtr(dict, "hspi", SPI_HandleTypeDef); @@ -115,15 +116,15 @@ void BSP_SPI_InitInfo(SPIInfo* info, ConfItem* dict) info->fastHandle=Bus_CreateReceiverHandle(name); osSemaphoreDef(lock); info->lock = osSemaphoreCreate(osSemaphore(lock), 1); - //ʼƬѡ + //初始化片选引脚 BSP_SPI_InitCS(info, Conf_GetPtr(dict, "cs", ConfItem)); - //ʼ + //初始化缓冲区 info->recvBuffer.maxBufSize = Conf_GetValue(dict, "max-recv-size", uint16_t, 1); info->recvBuffer.data = pvPortMalloc(info->recvBuffer.maxBufSize); memset(info->recvBuffer.data,0,info->recvBuffer.maxBufSize); } -//ʼƬѡ +//初始化片选引脚 void BSP_SPI_InitCS(SPIInfo* info, ConfItem* dict) { for(uint8_t num = 0; ; num++) @@ -135,13 +136,13 @@ void BSP_SPI_InitCS(SPIInfo* info, ConfItem* dict) else break; } - //ʼspiϢ + //初始化各spi信息 info->csList = pvPortMalloc(info->csNum * sizeof(CSInfo)); for(uint8_t num = 0; num < info->csNum; num++) { char confName[11] = {0}; sprintf(confName, "%d/pin", num); - //ӳGPIO_PIN=2^pin + //重新映射至GPIO_PIN=2^pin info->csList[num].pin =1<csList[num].name = Conf_GetPtr(dict, confName, char); @@ -158,27 +159,27 @@ bool BSP_SPI_DMACallback(const char* name, SoftBusFrame* frame, void* bindData) uint8_t* txData = (uint8_t*)Bus_GetMapValue(frame, "tx-data"); uint8_t* rxData = NULL; if(Bus_IsMapKeyExist(frame, "rx-data")) - rxData = (uint8_t*)Bus_GetMapValue(frame, "rx-data"); //ΪΪnullָspi + rxData = (uint8_t*)Bus_GetMapValue(frame, "rx-data"); //不检查该项是因为若为null则指向spi缓冲区 uint16_t len = *(uint16_t*)Bus_GetMapValue(frame, "len"); char* csName = (char*)Bus_GetMapValue(frame, "cs-name"); uint32_t waitTime = (*(bool*)Bus_GetMapValue(frame, "is-block"))? osWaitForever: 0; for(uint8_t num = 0; num < spiService.spiNum; num++) { - if(spiX == spiService.spiList[num].number) //ҵӦspi + if(spiX == spiService.spiList[num].number) //找到对应的spi { - if(rxData == NULL) //δָջָspi + if(rxData == NULL) //若未指定接收缓冲区则指向spi缓冲区 rxData = spiService.spiList[num].recvBuffer.data; - if(len > spiService.spiList[num].recvBuffer.maxBufSize) //ճȴڻȣ򷵻ش + if(len > spiService.spiList[num].recvBuffer.maxBufSize) //若接收长度大于缓冲区长度，则返回错误 return false; for (uint8_t i = 0; i < spiService.spiList[num].csNum; i++) { - if(!strcmp(csName, spiService.spiList[num].csList[i].name)) //ҵӦƬѡ + if(!strcmp(csName, spiService.spiList[num].csList[i].name)) //找到对应的片选引脚 { - // + //上锁 if(osSemaphoreWait(spiService.spiList[num].lock, waitTime) != osOK) return false; - HAL_GPIO_WritePin(spiService.spiList[num].csList[i].gpioX, spiService.spiList[num].csList[i].pin, GPIO_PIN_RESET); //Ƭѡ,ʼͨ - HAL_SPI_TransmitReceive_DMA(spiService.spiList[num].hspi, txData, rxData, len); //ʼ + HAL_GPIO_WritePin(spiService.spiList[num].csList[i].gpioX, spiService.spiList[num].csList[i].pin, GPIO_PIN_RESET); //拉低片选,开始通信 + HAL_SPI_TransmitReceive_DMA(spiService.spiList[num].hspi, txData, rxData, len); //开始传输 return true; } } @@ -196,31 +197,31 @@ bool BSP_SPI_BlockCallback(const char* name, SoftBusFrame* frame, void* bindData uint8_t* txData = (uint8_t*)Bus_GetMapValue(frame, "tx-data"); uint8_t* rxData = NULL; if(Bus_IsMapKeyExist(frame, "rx-data")) - rxData = (uint8_t*)Bus_GetMapValue(frame, "rx-data"); //ΪΪnullָspi + rxData = (uint8_t*)Bus_GetMapValue(frame, "rx-data"); //不检查该项是因为若为null则指向spi缓冲区 uint16_t len = *(uint16_t*)Bus_GetMapValue(frame, "len"); uint32_t timeout = *(uint32_t*)Bus_GetMapValue(frame, "timeout"); char* csName = (char*)Bus_GetMapValue(frame, "cs-name"); uint32_t waitTime = (*(bool*)Bus_GetMapValue(frame, "is-block"))? osWaitForever: 0; for(uint8_t num = 0; num < spiService.spiNum; num++) { - if(spiX == spiService.spiList[num].number) //ҵӦspi + if(spiX == spiService.spiList[num].number) //找到对应的spi { - if(rxData == NULL) //δָջָspi + if(rxData == NULL) //若未指定接收缓冲区则指向spi缓冲区 rxData = spiService.spiList[num].recvBuffer.data; - if(len > spiService.spiList[num].recvBuffer.maxBufSize) //ճȴڻȣ򷵻ش + if(len > spiService.spiList[num].recvBuffer.maxBufSize) //若接收长度大于缓冲区长度，则返回错误 return false; spiService.spiList[num].recvBuffer.dataLen = len; - for (uint8_t i = 0; i < spiService.spiList[num].csNum; i++) //ҵӦƬѡ + for (uint8_t i = 0; i < spiService.spiList[num].csNum; i++) //找到对应的片选引脚 { if(!strcmp(csName, spiService.spiList[num].csList[i].name)) { - // + //上锁 if(osSemaphoreWait(spiService.spiList[num].lock, waitTime) != osOK) return false; - HAL_GPIO_WritePin(spiService.spiList[num].csList[i].gpioX, spiService.spiList[num].csList[i].pin, GPIO_PIN_RESET); //Ƭѡ,ʼͨ - HAL_SPI_TransmitReceive(spiService.spiList[num].hspi, txData, rxData, len, timeout); //ʼ - HAL_GPIO_WritePin(spiService.spiList[num].csList[i].gpioX, spiService.spiList[num].csList[i].pin, GPIO_PIN_SET); //Ƭѡ,ͨ - // + HAL_GPIO_WritePin(spiService.spiList[num].csList[i].gpioX, spiService.spiList[num].csList[i].pin, GPIO_PIN_RESET); //拉低片选,开始通信 + HAL_SPI_TransmitReceive(spiService.spiList[num].hspi, txData, rxData, len, timeout); //开始传输 + HAL_GPIO_WritePin(spiService.spiList[num].csList[i].gpioX, spiService.spiList[num].csList[i].pin, GPIO_PIN_SET); //拉高片选,结束通信 + //解锁 osSemaphoreRelease(spiService.spiList[num].lock); return true; } diff --git a/services/bsp/bsp_tim.c b/services/bsp/bsp_tim.c index 086d8d6f86bb4d93bec09306b2f59804fa174790..654aa46ee16114fc5c5dc0799a48ac2cc12980a4 100644 --- a/services/bsp/bsp_tim.c +++ b/services/bsp/bsp_tim.c @@ -1,6 +1,8 @@ #include "config.h" #include "softbus.h" #include "cmsis_os.h" +#include +#include #include "tim.h" @@ -8,7 +10,7 @@ #define LIMIT(x,min,max) (x)=(((x)<=(min))?(min):(((x)>=(max))?(max):(x))) #endif -//TIMϢ +//TIM句柄信息 typedef struct { TIM_HandleTypeDef *htim; @@ -16,7 +18,7 @@ typedef struct SoftBusReceiverHandle fastHandle; }TIMInfo; -//TIM +//TIM服务数据 typedef struct { TIMInfo *timList; @@ -24,7 +26,7 @@ typedef struct uint8_t initFinished; }TIMService; -// +//函数声明 void BSP_TIM_Init(ConfItem* dict); void BSP_TIM_InitInfo(TIMInfo* info,ConfItem* dict); void BSP_TIM_StartHardware(TIMInfo* info,ConfItem* dict); @@ -38,7 +40,7 @@ void BSP_TIM_UpdateCallback(TIM_HandleTypeDef *htim) { for(uint8_t num = 0;numfastHandle,{""}); @@ -46,10 +48,10 @@ void BSP_TIM_UpdateCallback(TIM_HandleTypeDef *htim) } } -//TIMص +//TIM任务回调函数 void BSP_TIM_TaskCallback(void const * argument) { - //ٽ + //进入临界区 portENTER_CRITICAL(); BSP_TIM_Init((ConfItem*)argument); portEXIT_CRITICAL(); @@ -57,10 +59,10 @@ void BSP_TIM_TaskCallback(void const * argument) vTaskDelete(NULL); } -//TIMʼ +//TIM初始化 void BSP_TIM_Init(ConfItem* dict) { - //ûõtim + //计算用户配置的tim数量 timService.timNum = 0; for(uint8_t num = 0; ; num++) { @@ -80,14 +82,14 @@ void BSP_TIM_Init(ConfItem* dict) BSP_TIM_InitInfo(&timService.timList[num], Conf_GetPtr(dict, confName, ConfItem)); BSP_TIM_StartHardware(&timService.timList[num], Conf_GetPtr(dict, confName, ConfItem)); } - //עԶ̷ + //注册远程服务 Bus_RegisterRemoteFunc(NULL,BSP_TIM_SettingCallback,"/tim/setting"); Bus_RegisterRemoteFunc(NULL,BSP_TIM_SetDutyCallback,"/tim/pwm/set-duty"); Bus_RegisterRemoteFunc(NULL,BSP_TIM_GetEncodeCallback,"/tim/encode"); timService.initFinished=1; } -//ʼTIMϢ +//初始化TIM信息 void BSP_TIM_InitInfo(TIMInfo* info,ConfItem* dict) { info->htim = Conf_GetPtr(dict,"htim",TIM_HandleTypeDef); @@ -98,7 +100,7 @@ void BSP_TIM_InitInfo(TIMInfo* info,ConfItem* dict) info->fastHandle=Bus_CreateReceiverHandle(name); } -//TIMӲ +//开启TIM硬件 void BSP_TIM_StartHardware(TIMInfo* info,ConfItem* dict) { if(!strcmp(Conf_GetPtr(dict,"mode",char),"encode")) @@ -114,13 +116,13 @@ void BSP_TIM_StartHardware(TIMInfo* info,ConfItem* dict) } else if(!strcmp(Conf_GetPtr(dict,"mode",char),"update-interrupted")) { - //ʱжϱ־λһж + //清除定时器中断标志位，避免一开启就中断 __HAL_TIM_CLEAR_IT(info->htim, TIM_IT_UPDATE); HAL_TIM_Base_Start_IT(info->htim); } } -//TIMԶ̺ص +//TIM设置配置远程函数回调 bool BSP_TIM_SettingCallback(const char* name, SoftBusFrame* frame, void* bindData) { if(!Bus_IsMapKeyExist(frame,"tim-x")) @@ -129,7 +131,7 @@ bool BSP_TIM_SettingCallback(const char* name, SoftBusFrame* frame, void* bindDa TIMInfo* timInfo = NULL; for(uint8_t num = 0;num #include "usart.h" #define UART_IRQ \ @@ -16,10 +16,10 @@ #define UART_TOTAL_NUM 8 -//UARTϢ +//UART句柄信息 typedef struct { UART_HandleTypeDef* huart; - uint8_t number; //uartXеX + uint8_t number; //uartX中的X struct { uint8_t *data; @@ -29,7 +29,7 @@ typedef struct { SoftBusReceiverHandle fastHandle; }UARTInfo; -//UART +//UART服务数据 typedef struct { UARTInfo uartList[UART_TOTAL_NUM]; uint8_t uartNum; @@ -37,7 +37,7 @@ typedef struct { }UARTService; UARTService uartService = {0}; -// +//函数声明 void BSP_UART_Init(ConfItem* dict); void BSP_UART_InitInfo(UARTInfo* info, ConfItem* dict); void BSP_UART_Start_IT(UARTInfo* info); @@ -46,12 +46,12 @@ bool BSP_UART_ItCallback(const char* name, SoftBusFrame* frame, void* bindData); bool BSP_UART_DMACallback(const char* name, SoftBusFrame* frame, void* bindData); void BSP_UART_InitRecvBuffer(UARTInfo* info); -//uartжϻص +//uart接收中断回调函数 void BSP_UART_IRQCallback(uint8_t huartX) { UARTInfo* uartInfo = &uartService.uartList[huartX - 1]; - if(!uartService.initFinished)//ʼδ־λ + if(!uartService.initFinished)//如果初始化未完成则清除标志位 { (void)uartInfo->huart->Instance->SR; (void)uartInfo->huart->Instance->DR; @@ -60,7 +60,7 @@ void BSP_UART_IRQCallback(uint8_t huartX) if (__HAL_UART_GET_FLAG(uartInfo->huart, UART_FLAG_RXNE)) { - //ֹԽ + //防止数组越界 if(uartInfo->recvBuffer.pos < uartInfo->recvBuffer.maxBufSize) uartInfo->recvBuffer.data[uartInfo->recvBuffer.pos++] = uartInfo->huart->Instance->DR; } @@ -69,16 +69,16 @@ void BSP_UART_IRQCallback(uint8_t huartX) { /* clear idle it flag avoid idle interrupt all the time */ __HAL_UART_CLEAR_IDLEFLAG(uartInfo->huart); - uint16_t recSize=uartInfo->recvBuffer.pos; //ʱposֵΪһ֡ݵij - Bus_FastBroadcastSend(uartInfo->fastHandle, {uartInfo->recvBuffer.data, &recSize}); //жΪһ֡һ֡ + uint16_t recSize=uartInfo->recvBuffer.pos; //此时pos值为一帧数据的长度 + Bus_FastBroadcastSend(uartInfo->fastHandle, {uartInfo->recvBuffer.data, &recSize}); //空闲中断为一帧，发送一帧数据 uartInfo->recvBuffer.pos = 0; } } -//uartص +//uart任务回调函数 void BSP_UART_TaskCallback(void const * argument) { - //ٽ + //进入临界区 portENTER_CRITICAL(); BSP_UART_Init((ConfItem*)argument); portEXIT_CRITICAL(); @@ -88,7 +88,7 @@ void BSP_UART_TaskCallback(void const * argument) void BSP_UART_Init(ConfItem* dict) { - //ûõuart + //计算用户配置的uart数量 uartService.uartNum = 0; for(uint8_t num = 0; ; num++) { @@ -99,7 +99,7 @@ void BSP_UART_Init(ConfItem* dict) else break; } - //ʼuartϢ + //初始化各uart信息 for(uint8_t num = 0; num < uartService.uartNum; num++) { char confName[] = "uarts/_"; @@ -107,14 +107,14 @@ void BSP_UART_Init(ConfItem* dict) BSP_UART_InitInfo(uartService.uartList, Conf_GetPtr(dict, confName, ConfItem)); } - //עԶ̺ + //注册远程函数 Bus_RegisterRemoteFunc(NULL, BSP_UART_BlockCallback, "/uart/trans/block"); Bus_RegisterRemoteFunc(NULL, BSP_UART_ItCallback, "/uart/trans/it"); Bus_RegisterRemoteFunc(NULL, BSP_UART_DMACallback, "/uart/trans/dma"); uartService.initFinished = 1; } -//ʼuartϢ +//初始化uart信息 void BSP_UART_InitInfo(UARTInfo* info, ConfItem* dict) { uint8_t number = Conf_GetValue(dict, "number", uint8_t, 0); @@ -124,19 +124,19 @@ void BSP_UART_InitInfo(UARTInfo* info, ConfItem* dict) char name[] = "/uart_/recv"; name[5] = info[number-1].number + '0'; info[number-1].fastHandle = Bus_CreateReceiverHandle(name); - //ʼջ + //初始化接收缓冲区 BSP_UART_InitRecvBuffer(&info[number-1]); - //uartж + //开启uart中断 BSP_UART_Start_IT(&info[number-1]); } -//uartж +//开启uart中断 void BSP_UART_Start_IT(UARTInfo* info) { __HAL_UART_ENABLE_IT(info->huart, UART_IT_RXNE); __HAL_UART_ENABLE_IT(info->huart, UART_IT_IDLE); } -//ʼջ +//初始化接收缓冲区 void BSP_UART_InitRecvBuffer(UARTInfo* info) { info->recvBuffer.pos=0; @@ -144,7 +144,7 @@ void BSP_UART_InitRecvBuffer(UARTInfo* info) memset(info->recvBuffer.data,0,info->recvBuffer.maxBufSize); } -//ص +//阻塞回调 bool BSP_UART_BlockCallback(const char* name, SoftBusFrame* frame, void* bindData) { if(!Bus_CheckMapKeys(frame, {"uart-x", "data", "trans-size", "timeout"})) @@ -158,7 +158,7 @@ bool BSP_UART_BlockCallback(const char* name, SoftBusFrame* frame, void* bindDat return true; } -//жϷͻص +//中断发送回调 bool BSP_UART_ItCallback(const char* name, SoftBusFrame* frame, void* bindData) { if(!Bus_CheckMapKeys(frame, {"uart-x","data","trans-size"})) @@ -171,7 +171,7 @@ bool BSP_UART_ItCallback(const char* name, SoftBusFrame* frame, void* bindData) return true; } -//DMAͻص +//DMA发送回调 bool BSP_UART_DMACallback(const char* name, SoftBusFrame* frame, void* bindData) { if(!Bus_CheckMapKeys(frame, {"uart-x","data","trans-size"})) @@ -184,7 +184,7 @@ bool BSP_UART_DMACallback(const char* name, SoftBusFrame* frame, void* bindData) return true; } -//жϷ +//生成中断服务函数 #define IRQ_FUN(irq, number) \ void irq(void) \ { \ diff --git a/services/chassis/chassis_mecanum.c b/services/chassis/chassis_mecanum.c index 14e3dda1489f0993f8b0f29c1d13f7c2d5011c96..1d4a6cd42e5e8864266186d94d83322014780f35 100644 --- a/services/chassis/chassis_mecanum.c +++ b/services/chassis/chassis_mecanum.c @@ -3,6 +3,7 @@ #include "softbus.h" #include "motor.h" #include "cmsis_os.h" +#include "stm32f4xx.h" #include "arm_math.h" #ifndef LIMIT @@ -12,29 +13,29 @@ typedef struct _Chassis { - //̳ߴϢ + //底盘尺寸信息 struct Info { - float wheelbase;// - float wheeltrack;//־ - float wheelRadius;//ְ뾶 - float offsetX;//xyϵƫ + float wheelbase;//轴距 + float wheeltrack;//轮距 + float wheelRadius;//轮半径 + float offsetX;//重心在xy轴上的偏移 float offsetY; }info; - //4 + //4个电机 Motor* motors[4]; - //ƶϢ + //底盘移动信息 struct Move { - float vx;//ǰƽٶ mm/s - float vy;//ǰǰƶٶ mm/s - float vw;//ǰתٶ rad/s + float vx;//当前左右平移速度 mm/s + float vy;//当前前后移动速度 mm/s + float vw;//当前旋转速度 rad/s - float maxVx,maxVy,maxVw; //ٶ - Slope xSlope,ySlope; //б + float maxVx,maxVy,maxVw; //三个分量最大速度 + Slope xSlope,ySlope; //斜坡 }move; - float relativeAngle; //̵ƫǣλ + float relativeAngle; //与底盘的偏离角，单位度 uint16_t taskInterval; @@ -51,23 +52,27 @@ bool Chassis_SetAccCallback(const char* name, SoftBusFrame* frame, void* bindDat bool Chassis_SetRelativeAngleCallback(const char* name, SoftBusFrame* frame, void* bindData); void Chassis_StopCallback(const char* name, SoftBusFrame* frame, void* bindData); -//ص +static Chassis* GlobalChassis; + +//底盘任务回调函数 void Chassis_TaskCallback(void const * argument) { - //ٽ + //进入临界区 portENTER_CRITICAL(); Chassis chassis={0}; Chassis_Init(&chassis, (ConfItem*)argument); portEXIT_CRITICAL(); + + GlobalChassis = &chassis; osDelay(2000); while(1) { - /*************ƽٶ**************/ + /*************计算底盘平移速度**************/ - Chassis_UpdateSlope(&chassis);//˶бº + Chassis_UpdateSlope(&chassis);//更新运动斜坡函数数据 - //̨ϵƽٶȽ㵽ƽٶ(̨ƫ) + //将云台坐标系下平移速度解算到底盘平移速度(根据云台偏离角) float gimbalAngleSin=arm_sin_f32(chassis.relativeAngle*PI/180); float gimbalAngleCos=arm_cos_f32(chassis.relativeAngle*PI/180); chassis.move.vx=Slope_GetVal(&chassis.move.xSlope) * gimbalAngleCos @@ -76,7 +81,7 @@ void Chassis_TaskCallback(void const * argument) +Slope_GetVal(&chassis.move.ySlope) * gimbalAngleCos; float vw = chassis.move.vw/180*PI; - /*************ת**************/ + /*************解算各轮子转速**************/ float rotateRatio[4]; rotateRatio[0]=(chassis.info.wheelbase+chassis.info.wheeltrack)/2.0f-chassis.info.offsetY+chassis.info.offsetX; @@ -100,47 +105,47 @@ void Chassis_TaskCallback(void const * argument) void Chassis_Init(Chassis* chassis, ConfItem* dict) { - // + //任务间隔 chassis->taskInterval = Conf_GetValue(dict, "task-interval", uint16_t, 2); - //̳ߴϢڽ٣ + //底盘尺寸信息（用于解算轮速） chassis->info.wheelbase = Conf_GetValue(dict, "info/wheelbase", float, 0); chassis->info.wheeltrack = Conf_GetValue(dict, "info/wheeltrack", float, 0); chassis->info.wheelRadius = Conf_GetValue(dict, "info/wheel-radius", float, 76); chassis->info.offsetX = Conf_GetValue(dict, "info/offset-x", float, 0); chassis->info.offsetY = Conf_GetValue(dict, "info/offset-y", float, 0); - //ƶʼ + //移动参数初始化 chassis->move.maxVx = Conf_GetValue(dict, "move/max-vx", float, 2000); chassis->move.maxVy = Conf_GetValue(dict, "move/max-vy", float, 2000); chassis->move.maxVw = Conf_GetValue(dict, "move/max-vw", float, 180); - //̼ٶȳʼ + //底盘加速度初始化 float xAcc = Conf_GetValue(dict, "move/x-acc", float, 1000); float yAcc = Conf_GetValue(dict, "move/y-acc", float, 1000); Slope_Init(&chassis->move.xSlope, CHASSIS_ACC2SLOPE(chassis->taskInterval, xAcc),0); Slope_Init(&chassis->move.ySlope, CHASSIS_ACC2SLOPE(chassis->taskInterval, yAcc),0); - //̵ʼ + //底盘电机初始化 chassis->motors[0] = Motor_Init(Conf_GetPtr(dict, "motor-fl", ConfItem)); chassis->motors[1] = Motor_Init(Conf_GetPtr(dict, "motor-fr", ConfItem)); chassis->motors[2] = Motor_Init(Conf_GetPtr(dict, "motor-bl", ConfItem)); chassis->motors[3] = Motor_Init(Conf_GetPtr(dict, "motor-br", ConfItem)); - //õ̵Ϊٶģʽ + //设置底盘电机为速度模式 for(uint8_t i = 0; i<4; i++) { chassis->motors[i]->changeMode(chassis->motors[i], MOTOR_SPEED_MODE); } - //߹㲥Զ̺nameӳ + //软总线广播、远程函数name重映射 char* temp = Conf_GetPtr(dict, "name", char); temp = temp ? temp : "chassis"; uint8_t len = strlen(temp); - chassis->speedName = pvPortMalloc(len + 7+ 1); //7Ϊ"/ /speed"ijȣ1Ϊ'\0'ij + chassis->speedName = pvPortMalloc(len + 7+ 1); //7为"/ /speed"的长度，1为'\0'的长度 sprintf(chassis->speedName, "/%s/speed", temp); - chassis->accName = pvPortMalloc(len + 5+ 1); //5Ϊ"/ /acc"ijȣ1Ϊ'\0'ij + chassis->accName = pvPortMalloc(len + 5+ 1); //5为"/ /acc"的长度，1为'\0'的长度 sprintf(chassis->accName, "/%s/acc", temp); - chassis->relAngleName = pvPortMalloc(len + 16+ 1); //16Ϊ"/ /relative-angle"ijȣ1Ϊ'\0'ij + chassis->relAngleName = pvPortMalloc(len + 16+ 1); //16为"/ /relative-angle"的长度，1为'\0'的长度 sprintf(chassis->relAngleName, "/%s/relative-angle", temp); - //עԶ̺ + //注册远程函数 Bus_RegisterRemoteFunc(chassis, Chassis_SetSpeedCallback, chassis->speedName); Bus_RegisterRemoteFunc(chassis, Chassis_SetAccCallback, chassis->accName); Bus_RegisterRemoteFunc(chassis, Chassis_SetRelativeAngleCallback, chassis->relAngleName); @@ -148,13 +153,13 @@ void Chassis_Init(Chassis* chassis, ConfItem* dict) } -//б¼ٶ +//更新斜坡计算速度 void Chassis_UpdateSlope(Chassis* chassis) { Slope_NextVal(&chassis->move.xSlope); Slope_NextVal(&chassis->move.ySlope); } -//õٶȻص +//设置底盘速度回调 bool Chassis_SetSpeedCallback(const char* name, SoftBusFrame* frame, void* bindData) { Chassis* chassis = (Chassis*)bindData; @@ -177,7 +182,7 @@ bool Chassis_SetSpeedCallback(const char* name, SoftBusFrame* frame, void* bindD } return true; } -//õ̼ٶȻص +//设置底盘加速度回调 bool Chassis_SetAccCallback(const char* name, SoftBusFrame* frame, void* bindData) { Chassis* chassis = (Chassis*)bindData; @@ -187,7 +192,7 @@ bool Chassis_SetAccCallback(const char* name, SoftBusFrame* frame, void* bindDat Slope_SetStep(&chassis->move.ySlope, CHASSIS_ACC2SLOPE(chassis->taskInterval, *(float*)Bus_GetMapValue(frame, "ay"))); return true; } -//õϵǶȻص +//设置底盘坐标系分离角度回调 bool Chassis_SetRelativeAngleCallback(const char* name, SoftBusFrame* frame, void* bindData) { Chassis* chassis = (Chassis*)bindData; @@ -195,7 +200,7 @@ bool Chassis_SetRelativeAngleCallback(const char* name, SoftBusFrame* frame, voi chassis->relativeAngle = *(float*)Bus_GetMapValue(frame, "angle"); return true; } -//̼ͣص +//底盘急停回调 void Chassis_StopCallback(const char* name, SoftBusFrame* frame, void* bindData) { Chassis* chassis = (Chassis*)bindData; diff --git a/services/control/README.md b/services/control/README.md index 470a6256d43f5f89d545ff1c1b874960193a02d9..95d55f0a9d9ee20c40108abce926506cd781ba49 100644 --- a/services/control/README.md +++ b/services/control/README.md @@ -1,7 +1,7 @@ -# ϵͳۺϿģ +# 系统综合控制模块 --- -## ģ +## 模块介绍 -ģΪϲ߲ģ飬ңݺ󣬽һЩ߼ٷֱ̨ģݽִУͬ˸ԻӦڴļнбд`sys_control.c`ļֻʾԱдڸҪƵuiͬ˱ʾļδuiƣʵϴڱļнв䡣 \ No newline at end of file +该模块为最上层决策层模块，负责接收遥控器数据后，进行一些逻辑处理后再分别给底盘云台等模块数据进行执行，不同机器人个性化操作应当在此文件中进行编写，`sys_control.c`文件只进行示例性编写，由于各个机器人需要绘制的ui并不相同，因此本示例文件并未进行ui绘制，实际上次需在本文件中进行补充。 \ No newline at end of file diff --git a/services/control/sys_control.c b/services/control/sys_control.c index 43e4fbd10328c7791d90b3eaa8253b6ec9e209e4..099ce1718165c99ef9c0cdbf142412ddc8d9789d 100644 --- a/services/control/sys_control.c +++ b/services/control/sys_control.c @@ -3,37 +3,38 @@ #include "config.h" #include "pid.h" #include "main.h" +#include typedef enum { - SYS_FOLLOW_MODE, - SYS_SPIN_MODE, - SYS_SEPARATE_MODE + SYS_FOLLOW_MODE, + SYS_SPIN_MODE, + SYS_SEPARATE_MODE }SysCtrlMode; typedef struct { - struct - { - float vx,vy,vw; - float ax,ay; - }chassisData; // + struct + { + float vx,vy,vw; + float ax,ay; + } chassisData; //底盘数据 - struct - { - float yaw,pitch; - float relativeAngle; //̨ƫǶ - }gimbalData; //̨ + struct + { + float targetYaw, targetPitch; // 操纵云台欲达到的目标偏离角度 + float relativeAngle; //云台偏离角度 + } gimbalData; //云台数据 - uint8_t mode; - bool rockerCtrl; // ңƱ־λ - bool errFlag; // ͣ־λ - PID rotatePID; + uint8_t mode; + bool rockerCtrl; // 遥控器控制标志位 + bool errFlag; // 急停标志位 + PID rotatePID; }SysControl; SysControl sysCtrl={0}; -// +//函数声明 void Sys_InitInfo(ConfItem *dict); void Sys_InitReceiver(void); void Sys_Broadcast(void); @@ -45,254 +46,258 @@ void Sys_Gimbal_RotateCallback(const char* name, SoftBusFrame* frame, void* bind void Sys_Shoot_Callback(const char* name, SoftBusFrame* frame, void* bindData); void Sys_ErrorHandle(void); -//ʼϢ +//初始化控制信息 void Sys_InitInfo(ConfItem *dict) { - sysCtrl.mode = Conf_GetValue(dict, "init-mode", uint8_t, SYS_FOLLOW_MODE); //Ĭϸģʽ - sysCtrl.rockerCtrl = Conf_GetValue(dict, "rocker-ctrl", bool, false); //Ĭϼ - PID_Init(&sysCtrl.rotatePID, Conf_GetPtr(dict, "rotate-pid", ConfItem)); + sysCtrl.mode = Conf_GetValue(dict, "init-mode", uint8_t, SYS_FOLLOW_MODE); //默认跟随模式 + sysCtrl.rockerCtrl = Conf_GetValue(dict, "rocker-ctrl", bool, false); //默认键鼠控制 + PID_Init(&sysCtrl.rotatePID, Conf_GetPtr(dict, "rotate-pid", ConfItem)); } -//ʼ +//初始化接收 void Sys_InitReceiver() { - // - Bus_MultiRegisterReceiver(NULL, Sys_Chassis_MoveCallback, {"/rc/key/on-pressing","/rc/left-stick"}); - Bus_RegisterReceiver(NULL, Sys_Chassis_StopCallback, "/rc/key/on-up"); - //̨ - Bus_MultiRegisterReceiver(NULL, Sys_Gimbal_RotateCallback, {"/rc/mouse-move", - "/rc/right-stick", - "/gimbal/yaw/relative-angle"}); - //ģʽл - Bus_MultiRegisterReceiver(NULL, Sys_Mode_ChangeCallback, {"/rc/key/on-click","/rc/switch"}); - // - Bus_MultiRegisterReceiver(NULL, Sys_Shoot_Callback, {"/rc/key/on-click", - "/rc/key/on-long-press", - "/rc/key/on-up", - "/rc/wheel"}); + //底盘 + Bus_MultiRegisterReceiver(NULL, Sys_Chassis_MoveCallback, {"/rc/key/on-pressing","/rc/left-stick"}); + Bus_RegisterReceiver(NULL, Sys_Chassis_StopCallback, "/rc/key/on-up"); + //云台 + Bus_MultiRegisterReceiver(NULL, Sys_Gimbal_RotateCallback, {"/rc/mouse-move", + "/rc/right-stick", + "/gimbal/yaw/relative-angle"}); + //模式切换 + Bus_MultiRegisterReceiver(NULL, Sys_Mode_ChangeCallback, {"/rc/key/on-click","/rc/switch"}); + //发射 + Bus_MultiRegisterReceiver(NULL, Sys_Shoot_Callback, {"/rc/key/on-click", + "/rc/key/on-long-press", + "/rc/key/on-up", + "/rc/wheel"}); } void SYS_CTRL_TaskCallback(void const * argument) { - //ٽ - portENTER_CRITICAL(); - Sys_InitInfo((ConfItem *)argument); - Sys_InitReceiver(); - portEXIT_CRITICAL(); - while(1) - { - if(sysCtrl.errFlag==1) - Sys_ErrorHandle(); + //进入临界区 + portENTER_CRITICAL(); + Sys_InitInfo((ConfItem *)argument); + Sys_InitReceiver(); + portEXIT_CRITICAL(); + while(1) + { + if(sysCtrl.errFlag==1) + Sys_ErrorHandle(); - if(sysCtrl.mode==SYS_FOLLOW_MODE)//ģʽ - { - PID_SingleCalc(&sysCtrl.rotatePID, 0, sysCtrl.gimbalData.relativeAngle); - sysCtrl.chassisData.vw = sysCtrl.rotatePID.output; - } - else if(sysCtrl.mode==SYS_SPIN_MODE)//Сģʽ - { - sysCtrl.chassisData.vw = 240; - } - else if(sysCtrl.mode==SYS_SEPARATE_MODE)// ģʽ - { - sysCtrl.chassisData.vw = 0; - } - Sys_Broadcast(); - osDelay(10); - } + if(sysCtrl.mode==SYS_FOLLOW_MODE)//跟随模式 + { + PID_SingleCalc(&sysCtrl.rotatePID, 0, sysCtrl.gimbalData.relativeAngle); + sysCtrl.chassisData.vw = sysCtrl.rotatePID.output; + } + else if(sysCtrl.mode==SYS_SPIN_MODE)//小陀螺模式 + { + sysCtrl.chassisData.vw = 240; + } + else if(sysCtrl.mode==SYS_SEPARATE_MODE)// 分离模式 + { + sysCtrl.chassisData.vw = 0; + } + Sys_Broadcast(); + osDelay(10); + } } -//͹㲥 +//发送广播 void Sys_Broadcast() { - Bus_RemoteCall("/chassis/speed", {{"vx", &sysCtrl.chassisData.vx}, - {"vy", &sysCtrl.chassisData.vy}, - {"vw", &sysCtrl.chassisData.vw}}); - Bus_RemoteCall("/chassis/relative-angle", {{"angle", &sysCtrl.gimbalData.relativeAngle}}); - Bus_RemoteCall("/gimbal/setting", {{"yaw", &sysCtrl.gimbalData.yaw},{"pitch", &sysCtrl.gimbalData.pitch}}); + Bus_RemoteCall("/chassis/speed", {{"vx", {.F32 = sysCtrl.chassisData.vx}}, + {"vy", {.F32 = sysCtrl.chassisData.vy}}, + {"vw", {.F32 = sysCtrl.chassisData.vw}}}); + Bus_RemoteCall("/chassis/relative-angle", {{"angle", {.F32 = sysCtrl.gimbalData.relativeAngle}}}); + Bus_RemoteCall("/gimbal/setting", {{"yaw", {.F32 = sysCtrl.gimbalData.targetYaw}}, + {"pitch", {.F32 = sysCtrl.gimbalData.targetPitch}}}); } -//˶ֹͣص +//底盘运动及停止回调函数 void Sys_Chassis_MoveCallback(const char* name, SoftBusFrame* frame, void* bindData) { - float speedRatio=0; - if(!strcmp(name,"/rc/key/on-pressing") && !sysCtrl.rockerCtrl) // - { - if(!Bus_CheckMapKeys(frame,{"combine-key","key"})) - return; - if(!strcmp(Bus_GetMapValue(frame,"combine-key"), "none")) // - speedRatio=1; - else if(!strcmp(Bus_GetMapValue(frame,"combine-key"), "shift")) // - speedRatio=5; - else if(!strcmp(Bus_GetMapValue(frame,"combine-key"), "ctrl")) // - speedRatio=0.2; - switch(*(char*)Bus_GetMapValue(frame,"key")) - { - case 'A': - sysCtrl.chassisData.vx = 200*speedRatio; - break; - case 'D': - sysCtrl.chassisData.vx = -200*speedRatio; - break; - case 'W': - sysCtrl.chassisData.vy = 200*speedRatio; - break; - case 'S': - sysCtrl.chassisData.vy = -200*speedRatio; - break; - } - } - else if(!strcmp(name,"/rc/left-stick") && sysCtrl.rockerCtrl) //ң - { - if(!Bus_CheckMapKeys(frame,{"x","y"})) - return; - sysCtrl.chassisData.vx = *(int16_t*)Bus_GetMapValue(frame,"x"); - sysCtrl.chassisData.vy = *(int16_t*)Bus_GetMapValue(frame,"y"); - } + float speedRatio=0; + if(!strcmp(name,"/rc/key/on-pressing") && !sysCtrl.rockerCtrl) //键鼠控制 + { + if(!Bus_CheckMapKeys(frame,{"combine-key","key"})) + return; + if(!strcmp(Bus_GetMapValue(frame,"combine-key"), "none")) //正常 + speedRatio=1; + else if(!strcmp(Bus_GetMapValue(frame,"combine-key"), "shift")) //快速 + speedRatio=5; + else if(!strcmp(Bus_GetMapValue(frame,"combine-key"), "ctrl")) //慢速 + speedRatio=0.2; + switch(*(char*)Bus_GetMapValue(frame,"key")) + { + case 'A': + sysCtrl.chassisData.vx = 200*speedRatio; + break; + case 'D': + sysCtrl.chassisData.vx = -200*speedRatio; + break; + case 'W': + sysCtrl.chassisData.vy = 200*speedRatio; + break; + case 'S': + sysCtrl.chassisData.vy = -200*speedRatio; + break; + } + } + else if(!strcmp(name,"/rc/left-stick") && sysCtrl.rockerCtrl) //遥控器控制 + { + if(!Bus_CheckMapKeys(frame,{"x","y"})) + return; + sysCtrl.chassisData.vx = *(int16_t*)Bus_GetMapValue(frame,"x"); + sysCtrl.chassisData.vy = *(int16_t*)Bus_GetMapValue(frame,"y"); + } } void Sys_Chassis_StopCallback(const char* name, SoftBusFrame* frame, void* bindData) { - if(sysCtrl.rockerCtrl || !Bus_IsMapKeyExist(frame, "key")) - return; - switch(*(char*)Bus_GetMapValue(frame,"key")) - { - case 'A': - case 'D': - sysCtrl.chassisData.vx = 0; - break; - case 'W': - case 'S': - sysCtrl.chassisData.vy = 0; - break; - } + if(sysCtrl.rockerCtrl || !Bus_IsMapKeyExist(frame, "key")) + return; + switch(*(char*)Bus_GetMapValue(frame,"key")) + { + case 'A': + case 'D': + sysCtrl.chassisData.vx = 0; + break; + case 'W': + case 'S': + sysCtrl.chassisData.vy = 0; + break; + } } -//̨תص +//云台旋转回调函数 void Sys_Gimbal_RotateCallback(const char* name, SoftBusFrame* frame, void* bindData) { - if(!strcmp(name,"/rc/mouse-move") && !sysCtrl.rockerCtrl) // - { - if(!Bus_CheckMapKeys(frame,{"x","y"})) - return; - sysCtrl.gimbalData.yaw +=*(int16_t*)Bus_GetMapValue(frame,"x"); - sysCtrl.gimbalData.pitch +=*(int16_t*)Bus_GetMapValue(frame,"y"); - } - else if(!strcmp(name,"/rc/right-stick") && sysCtrl.rockerCtrl) //ң - { - if(!Bus_CheckMapKeys(frame,{"x","y"})) - return; - sysCtrl.gimbalData.yaw +=*(int16_t*)Bus_GetMapValue(frame,"x")*0.01; - sysCtrl.gimbalData.pitch +=*(int16_t*)Bus_GetMapValue(frame,"y")*0.01; - } - else if(!strcmp(name,"/gimbal/yaw/relative-angle")) - { - if(!Bus_IsMapKeyExist(frame,"angle")) - return; - sysCtrl.gimbalData.relativeAngle = *(float*)Bus_GetMapValue(frame,"angle"); - } + if(!strcmp(name,"/rc/mouse-move") && !sysCtrl.rockerCtrl) //键鼠控制 + { + if(!Bus_CheckMapKeys(frame,{"x","y"})) + return; + // FIXME:鼠标能打多少量未知 + // sysCtrl.gimbalData.targetYaw +=*(int16_t*)Bus_GetMapValue(frame,"x"); + // sysCtrl.gimbalData.targetPitch +=*(int16_t*)Bus_GetMapValue(frame,"y"); + } + else if(!strcmp(name,"/rc/right-stick") && sysCtrl.rockerCtrl) //遥控器控制 + { + if(!Bus_CheckMapKeys(frame,{"x","y"})) + return; + // Yaw：【1 / 660 * 30 = 0.04545】（遥控器满杆量时，期望云台能转到偏移角30度位置） + // Pitch：【1 / 660 * 10 = 0.01515】（满杆量时相对转动10度）//FIXME + sysCtrl.gimbalData.targetYaw = *(int16_t*)Bus_GetMapValue(frame,"x")*0.045; + sysCtrl.gimbalData.targetPitch = *(int16_t*)Bus_GetMapValue(frame,"y")*0.015; + } + else if(!strcmp(name,"/gimbal/yaw/relative-angle")) + { + if(!Bus_IsMapKeyExist(frame,"angle")) + return; + sysCtrl.gimbalData.relativeAngle = *(float*)Bus_GetMapValue(frame,"angle"); + } } -//ģʽлص +//模式切换回调 void Sys_Mode_ChangeCallback(const char* name, SoftBusFrame* frame, void* bindData) { - if(!strcmp(name,"/rc/key/on-click") && !sysCtrl.rockerCtrl) // - { - if(!Bus_IsMapKeyExist(frame,"key")) - return; - switch(*(char*)Bus_GetMapValue(frame,"key")) - { - case 'Q': - sysCtrl.mode = SYS_SPIN_MODE; //Сģʽ - break; - case 'E': - sysCtrl.mode = SYS_FOLLOW_MODE; //ģʽ - break; - case 'R': - sysCtrl.mode = SYS_SEPARATE_MODE; //ģʽ - break; - } - } - else if(!strcmp(name,"/rc/switch") ) //ң - { - if(Bus_IsMapKeyExist(frame, "right")&& sysCtrl.rockerCtrl) - { - switch(*(uint8_t*)Bus_GetMapValue(frame, "right")) - { - case 1: - sysCtrl.mode = SYS_SPIN_MODE; //Сģʽ - break; - case 3: - sysCtrl.mode = SYS_FOLLOW_MODE; //ģʽ - break; - case 2: - sysCtrl.mode = SYS_SEPARATE_MODE; //ģʽ - break; - } - } - if(Bus_IsMapKeyExist(frame, "left")) - { - switch(*(uint8_t*)Bus_GetMapValue(frame, "left")) - { - case 1: - sysCtrl.rockerCtrl = true; //лң - sysCtrl.errFlag = 0; - break; - case 3: - sysCtrl.rockerCtrl = false; //л - sysCtrl.errFlag = 0; - break; - case 2: - sysCtrl.errFlag = 1; - break; - } - } - } + if(!strcmp(name,"/rc/key/on-click") && !sysCtrl.rockerCtrl) //键鼠控制 + { + if(!Bus_IsMapKeyExist(frame,"key")) + return; + switch(*(char*)Bus_GetMapValue(frame,"key")) + { + case 'Q': + sysCtrl.mode = SYS_SPIN_MODE; //小陀螺模式 + break; + case 'E': + sysCtrl.mode = SYS_FOLLOW_MODE; //跟随模式 + break; + case 'R': + sysCtrl.mode = SYS_SEPARATE_MODE; //分离模式 + break; + } + } + else if(!strcmp(name,"/rc/switch") ) //遥控器控制 + { + if(Bus_IsMapKeyExist(frame, "right")&& sysCtrl.rockerCtrl) + { + switch(*(uint8_t*)Bus_GetMapValue(frame, "right")) + { + case 1: + sysCtrl.mode = SYS_SPIN_MODE; //小陀螺模式 + break; + case 3: + sysCtrl.mode = SYS_FOLLOW_MODE; //跟随模式 + break; + case 2: + sysCtrl.mode = SYS_SEPARATE_MODE; //分离模式 + break; + } + } + if(Bus_IsMapKeyExist(frame, "left")) + { + switch(*(uint8_t*)Bus_GetMapValue(frame, "left")) + { + case 1: + sysCtrl.rockerCtrl = true; //切换至遥控器控制 + sysCtrl.errFlag = 0; + break; + case 3: + sysCtrl.rockerCtrl = false; //切换至键鼠控制 + sysCtrl.errFlag = 0; + break; + case 2: + sysCtrl.errFlag = 1; + break; + } + } + } } -//ص +//发射回调函数 void Sys_Shoot_Callback(const char* name, SoftBusFrame* frame, void* bindData) { - if(!strcmp(name,"/rc/key/on-click") && !sysCtrl.rockerCtrl)// - { - if(!Bus_IsMapKeyExist(frame,"left")) - return; - Bus_RemoteCall("/shooter/mode",{{"mode", "once"}}); // - } - else if(!strcmp(name,"/rc/key/on-long-press") && !sysCtrl.rockerCtrl) - { - if(!Bus_IsMapKeyExist(frame,"left")) - return; - Bus_RemoteCall("/shooter/mode",{{"mode", "continue"}, {"interval-time", IM_PTR(uint16_t, 100)}}); // - } - else if(!strcmp(name,"/rc/key/on-up") && !sysCtrl.rockerCtrl) - { - if(!Bus_IsMapKeyExist(frame,"left")) - return; - Bus_RemoteCall("/shooter/mode",{{"mode", "idle"}}); // - } - else if(!strcmp(name,"/rc/wheel") && sysCtrl.rockerCtrl)//ң - { - if(!Bus_IsMapKeyExist(frame,"value")) - return; - int16_t wheel = *(int16_t*)Bus_GetMapValue(frame,"value"); + if(!strcmp(name,"/rc/key/on-click") && !sysCtrl.rockerCtrl)//键鼠控制 + { + if(!Bus_IsMapKeyExist(frame,"left")) + return; + Bus_RemoteCall("/shooter/mode",{{"mode", {"once"}}}); //点射 + } + else if(!strcmp(name,"/rc/key/on-long-press") && !sysCtrl.rockerCtrl) + { + if(!Bus_IsMapKeyExist(frame,"left")) + return; + Bus_RemoteCall("/shooter/mode",{{"mode", {"continue"}}, {"interval-time", {.U32 = 100}}}); //连发 + } + else if(!strcmp(name,"/rc/key/on-up") && !sysCtrl.rockerCtrl) + { + if(!Bus_IsMapKeyExist(frame,"left")) + return; + Bus_RemoteCall("/shooter/mode",{{"mode", {"idle"}}}); //连发 + } + else if(!strcmp(name,"/rc/wheel") && sysCtrl.rockerCtrl)//遥控器控制 + { + if(!Bus_IsMapKeyExist(frame,"value")) + return; + int16_t wheel = *(int16_t*)Bus_GetMapValue(frame, "value"); - if(wheel > 600) - Bus_RemoteCall("/shooter/mode", {{"mode","once"}}); // - } + if(wheel > 600) + Bus_RemoteCall("/shooter/mode", {{"mode", {"once"}}}); //点射 + } } -//ͣ +//急停 void Sys_ErrorHandle(void) { - Bus_BroadcastSend("/system/stop",{"",0}); - while(1) - { - if(!sysCtrl.errFlag) - { - __disable_irq(); - NVIC_SystemReset(); - } - osDelay(2); - } + Bus_BroadcastSend("/system/stop",{"",0}); + while(1) + { + if(!sysCtrl.errFlag) + { + __disable_irq(); + NVIC_SystemReset(); + } + osDelay(2); + } } diff --git a/services/gimbal/gimbal.c b/services/gimbal/gimbal.c index eb85b027eb82359e1261bb954e1535952864b17a..c3d30aeaceee251d51d43090dc5845a1d5f4d528 100644 --- a/services/gimbal/gimbal.c +++ b/services/gimbal/gimbal.c @@ -10,21 +10,21 @@ typedef struct _Gimbal { - //yawpitch + //yaw、pitch电机 Motor* motors[2]; - uint16_t zeroAngle[2]; // - float relativeAngle; //̨ƫǶ + uint16_t zeroAngle[2]; //零点 + float relativeAngle; //云台偏离角度 struct { - // float eulerAngle[3]; //ŷ + // float eulerAngle[3]; //欧拉角 float lastEulerAngle[3]; float totalEulerAngle[3]; PID pid[2]; }imu; - float angle[2]; //̨Ƕ + float angle[2]; //云台角度 uint16_t taskInterval; - //߹㲥Զ̺name + //软总线广播、远程函数name char* yawRelAngleName; char* imuEulerAngleName; char* settingName; @@ -38,72 +38,77 @@ void Gimbal_BroadcastCallback(const char* name, SoftBusFrame* frame, void* bindD bool Gimbal_SettingCallback(const char* name, SoftBusFrame* frame, void* bindData); void Gimbal_StopCallback(const char* name, SoftBusFrame* frame, void* bindData); +static Gimbal* GlobalGimbal; + void Gimbal_TaskCallback(void const * argument) { - //ٽ + //进入临界区 portENTER_CRITICAL(); Gimbal gimbal={0}; Gimbal_Init(&gimbal, (ConfItem*)argument); portEXIT_CRITICAL(); osDelay(2000); - Gimbal_TotalAngleInit(&gimbal); //̨ - //̨󣬸ĵģʽimuǶ⻷ٶȷڻ + Gimbal_TotalAngleInit(&gimbal); //计算云台零点 + //计算好云台零点后，更改电机模式，imu反馈做角度外环电机速度反馈做内环 gimbal.motors[0]->changeMode(gimbal.motors[0], MOTOR_SPEED_MODE); gimbal.motors[1]->changeMode(gimbal.motors[1], MOTOR_SPEED_MODE); + + GlobalGimbal = &gimbal; + while(1) { - //Ƕȴpid - PID_SingleCalc(&gimbal.imu.pid[0], gimbal.angle[0], gimbal.imu.totalEulerAngle[0]); + //计算角度串级pid + PID_SingleCalc(&gimbal.imu.pid[0], gimbal.angle[0], gimbal.relativeAngle/*gimbal.imu.totalEulerAngle[0]*/); PID_SingleCalc(&gimbal.imu.pid[1], gimbal.angle[1], gimbal.imu.totalEulerAngle[1]); gimbal.motors[0]->setTarget(gimbal.motors[0], gimbal.imu.pid[0].output); gimbal.motors[1]->setTarget(gimbal.motors[1], gimbal.imu.pid[1].output); - //̨ĽǶ + //解算云台距离零点的角度 gimbal.relativeAngle = gimbal.motors[0]->getData(gimbal.motors[0], "totalAngle"); - int16_t turns = (int32_t)gimbal.relativeAngle / 360; //ת - turns = turns < 0 ? turns - 1 : turns; //ǸһȦʹƫDZ - gimbal.relativeAngle -= turns*360; //0-360 - Bus_BroadcastSend(gimbal.yawRelAngleName, {{"angle", &gimbal.relativeAngle}}); //㲥̨ƫ + int16_t turns = (int32_t)gimbal.relativeAngle / 360; //转数 + turns = turns < 0 ? turns - 1 : turns; //如果是负数多减一圈使偏离角变成正数 + gimbal.relativeAngle -= turns*360; //0-360度 + Bus_BroadcastSend(gimbal.yawRelAngleName, {{"angle", &gimbal.relativeAngle}}); //广播云台偏离角 osDelay(gimbal.taskInterval); } } void Gimbal_Init(Gimbal* gimbal, ConfItem* dict) { - // + //任务间隔 gimbal->taskInterval = Conf_GetValue(dict, "task-interval", uint16_t, 2); - //̨ + //云台零点 gimbal->zeroAngle[0] = Conf_GetValue(dict, "zero-yaw", uint16_t, 0); gimbal->zeroAngle[1] = Conf_GetValue(dict, "zero-pitch", uint16_t, 0); - //̨ʼ + //云台电机初始化 gimbal->motors[0] = Motor_Init(Conf_GetPtr(dict, "motor-yaw", ConfItem)); gimbal->motors[1] = Motor_Init(Conf_GetPtr(dict, "motor-pitch", ConfItem)); PID_Init(&gimbal->imu.pid[0], Conf_GetPtr(dict, "yaw-imu-pid", ConfItem)); PID_Init(&gimbal->imu.pid[1], Conf_GetPtr(dict, "pitch-imu-pid", ConfItem)); - //㲥Զ̺nameӳ + //广播、远程函数name重映射 char* temp = Conf_GetPtr(dict, "name", char); temp = temp ? temp : "gimbal"; uint8_t len = strlen(temp); - gimbal->settingName = pvPortMalloc(len + 9+ 1); //9Ϊ"/ /setting"ijȣ1Ϊ'\0'ij + gimbal->settingName = pvPortMalloc(len + 9+ 1); //9为"/ /setting"的长度，1为'\0'的长度 sprintf(gimbal->settingName, "/%s/setting", temp); - gimbal->yawRelAngleName = pvPortMalloc(len + 20+ 1); //20Ϊ"/ /yaw/relative-angle"ijȣ1Ϊ'\0'ij + gimbal->yawRelAngleName = pvPortMalloc(len + 20+ 1); //20为"/ /yaw/relative-angle"的长度，1为'\0'的长度 sprintf(gimbal->yawRelAngleName, "/%s/yaw/relative-angle", temp); temp = Conf_GetPtr(dict, "ins-name", char); temp = temp ? temp : "ins"; len = strlen(temp); - gimbal->imuEulerAngleName = pvPortMalloc(len + 13+ 1); //13Ϊ"/ /euler-angle"ijȣ1Ϊ'\0'ij + gimbal->imuEulerAngleName = pvPortMalloc(len + 13+ 1); //13为"/ /euler-angle"的长度，1为'\0'的长度 sprintf(gimbal->imuEulerAngleName, "/%s/euler-angle", temp); - //õģʽΪδúǰpidʹﵽimuijʼ + //不在这里设置电机模式，因为在未设置好零点前，pid会驱使电机达到编码器的零点或者imu的初始化零点 - //ע㲥Զ̺ص + //注册广播、远程函数回调函数 Bus_RegisterReceiver(gimbal, Gimbal_BroadcastCallback, gimbal->imuEulerAngleName); Bus_RegisterRemoteFunc(gimbal, Gimbal_SettingCallback, gimbal->settingName); - Bus_RegisterReceiver(gimbal, Gimbal_StopCallback, "/system/stop"); //ͣ + Bus_RegisterReceiver(gimbal, Gimbal_StopCallback, "/system/stop"); //急停 } void Gimbal_BroadcastCallback(const char* name, SoftBusFrame* frame, void* bindData) @@ -117,7 +122,7 @@ void Gimbal_BroadcastCallback(const char* name, SoftBusFrame* frame, void* bindD float yaw = *(float*)Bus_GetMapValue(frame, "yaw"); float pitch = *(float*)Bus_GetMapValue(frame, "pitch"); float roll = *(float*)Bus_GetMapValue(frame, "roll"); - Gimbal_StatAngle(gimbal, yaw, pitch, roll); //ͳ̨Ƕ + Gimbal_StatAngle(gimbal, yaw, pitch, roll); //统计云台角度 } } bool Gimbal_SettingCallback(const char* name, SoftBusFrame* frame, void* bindData) @@ -135,7 +140,7 @@ bool Gimbal_SettingCallback(const char* name, SoftBusFrame* frame, void* bindDat return true; } -void Gimbal_StopCallback(const char* name, SoftBusFrame* frame, void* bindData) //ͣ +void Gimbal_StopCallback(const char* name, SoftBusFrame* frame, void* bindData) //急停 { Gimbal* gimbal = (Gimbal*)bindData; for(uint8_t i = 0; i<2; i++) @@ -156,9 +161,9 @@ void Gimbal_StatAngle(Gimbal* gimbal, float yaw, float pitch, float roll) dAngle = -gimbal->imu.lastEulerAngle[i] - (360 - eulerAngle[i]); else dAngle = eulerAngle[i] - gimbal->imu.lastEulerAngle[i]; - //Ƕ + //将角度增量加入计数器 gimbal->imu.totalEulerAngle[i] += dAngle; - //¼Ƕ + //记录角度 gimbal->imu.lastEulerAngle[i] = eulerAngle[i]; } } @@ -169,12 +174,12 @@ void Gimbal_TotalAngleInit(Gimbal* gimbal) { float angle = 0; angle = gimbal->motors[i]->getData(gimbal->motors[i], "angle"); - angle = angle - (float)gimbal->zeroAngle[i]*360/8191; //ĽǶ - if(angle < -180) //Ƕת-180~180ȣʹ̨ת + angle = angle - (float)gimbal->zeroAngle[i]*360/8191; //计算距离零点的角度 + if(angle < -180) //将角度转化到-180~180度，这样可以使云台以最近距离旋转至零点 angle += 360; else if(angle > 180) angle -= 360; gimbal->imu.totalEulerAngle[i] = angle; - gimbal->motors[i]->initTotalAngle(gimbal->motors[i], angle); //õʼǶ + gimbal->motors[i]->initTotalAngle(gimbal->motors[i], angle); //设置电机的起始角度 } } diff --git a/services/ins/bmi088/AHRS.h b/services/ins/bmi088/AHRS.h deleted file mode 100644 index 3ad6a343cad64bb26768eb9cad182c28ec92400c..0000000000000000000000000000000000000000 --- a/services/ins/bmi088/AHRS.h +++ /dev/null @@ -1,61 +0,0 @@ -#ifndef AHRS_H -#define AHRS_H - -#include "AHRS_MiddleWare.h" - -/** - * @brief ݼٶȵݣƵݽԪʼ - * @param[in] ҪʼԪ - * @param[in] ڳʼļٶȼ,(x,y,z)Ϊ λ m/s2 - * @param[in] ڳʼĴƼ,(x,y,z)Ϊ λ uT - * @retval ؿ - */ -extern void AHRS_init(fp32 quat[4], const fp32 accel[3], const fp32 mag[3]); - -/** - * @brief ǵݣٶȵݣƵݽԪ - * @param[in] ҪµԪ - * @param[in] ¶ʱʱ䣬̶ʱã1000HzΪ0.001f, - * @param[in] ڸµ,˳(x,y,z) λ rad - * @param[in] ڳʼļٶ,˳(x,y,z) λ m/s2 - * @param[in] ڳʼĴ,˳(x,y,z) λ uT - * @retval 1:³ɹ, 0:ʧ - */ -extern bool_t AHRS_update(fp32 quat[4], const fp32 timing_time, const fp32 gyro[3], const fp32 accel[3], const fp32 mag[3]); - -/** - * @brief ԪСӦŷƫyaw - * @param[in] Ԫ飬ΪNULL - * @retval صƫyaw λ rad - */ -extern fp32 get_yaw(const fp32 quat[4]); - -/** - * @brief ԪСӦŷǸ pitch - * @param[in] Ԫ飬ΪNULL - * @retval صĸ pitch λ rad - */ -extern fp32 get_pitch(const fp32 quat[4]); -/** - * @brief ԪСӦŷǺ roll - * @param[in] Ԫ飬ΪNULL - * @retval صĺ roll λ rad - */ -extern fp32 get_roll(const fp32 quat[4]); - -/** - * @brief ԪСӦŷyawpitchroll - * @param[in] Ԫ飬ΪNULL - * @param[in] صƫyaw λ rad - * @param[in] صĸpitch λ rad - * @param[in] صĺroll λ rad - */ -extern void get_angle(const fp32 quat[4], fp32 *yaw, fp32 *pitch, fp32 *roll); -/** - * @brief صǰٶ - * @param[in] - * @retval ٶ λ m/s2 - */ -extern fp32 get_carrier_gravity(void); - -#endif diff --git a/services/ins/bmi088/AHRS_middleware.c b/services/ins/bmi088/AHRS_middleware.c index 9e370e8e1811bf1184b221b661ac57fbbdda7120..413acd2677b3dd16d20ab8418531d77033b9d653 100644 --- a/services/ins/bmi088/AHRS_middleware.c +++ b/services/ins/bmi088/AHRS_middleware.c @@ -1,11 +1,11 @@ /** ****************************(C) COPYRIGHT 2019 DJI**************************** * @file AHRS_MiddleWare.c/h - * @brief ̬м㣬Ϊ̬ṩغ + * @brief 姿态解算中间层，为姿态解算提供相关函数 * @note * @history * Version Date Author Modification - * V1.0.0 Dec-26-2018 RM 1. + * V1.0.0 Dec-26-2018 RM 1. 完成 * @verbatim ============================================================================== @@ -17,13 +17,14 @@ #include "AHRS_MiddleWare.h" #include "AHRS.h" +#include "stm32f4xx.h" #include "arm_math.h" #include "main.h" /** - * @brief ڻȡǰ߶ + * @brief 用于获取当前高度 * @author RM - * @param[in] ߶ȵָ룬fp32 - * @retval ؿ + * @param[in] 高度的指针，fp32 + * @retval 返回空 */ void AHRS_get_height(fp32* high) @@ -35,10 +36,10 @@ void AHRS_get_height(fp32* high) } /** - * @brief ڻȡǰγ + * @brief 用于获取当前纬度 * @author RM - * @param[in] γȵָ룬fp32 - * @retval ؿ + * @param[in] 纬度的指针，fp32 + * @retval 返回空 */ void AHRS_get_latitude(fp32* latitude) @@ -50,10 +51,10 @@ void AHRS_get_latitude(fp32* latitude) } /** - * @brief ٿ + * @brief 快速开方函数， * @author RM - * @param[in] Ҫĸfp32 - * @retval 1/sqrt ĵ + * @param[in] 输入需要开方的浮点数，fp32 + * @retval 返回1/sqrt 开方后的倒数 */ fp32 AHRS_invSqrt(fp32 num) @@ -71,10 +72,10 @@ fp32 AHRS_invSqrt(fp32 num) } /** - * @brief sin + * @brief sin函数 * @author RM - * @param[in] Ƕ λ rad - * @retval ضӦǶȵsinֵ + * @param[in] 角度 单位 rad + * @retval 返回对应角度的sin值 */ fp32 AHRS_sinf(fp32 angle) @@ -82,10 +83,10 @@ fp32 AHRS_sinf(fp32 angle) return sin(angle); } /** - * @brief cos + * @brief cos函数 * @author RM - * @param[in] Ƕ λ rad - * @retval ضӦǶȵcosֵ + * @param[in] 角度 单位 rad + * @retval 返回对应角度的cos值 */ fp32 AHRS_cosf(fp32 angle) @@ -94,10 +95,10 @@ fp32 AHRS_cosf(fp32 angle) } /** - * @brief tan + * @brief tan函数 * @author RM - * @param[in] Ƕ λ rad - * @retval ضӦǶȵtanֵ + * @param[in] 角度 单位 rad + * @retval 返回对应角度的tan值 */ fp32 AHRS_tanf(fp32 angle) @@ -105,10 +106,10 @@ fp32 AHRS_tanf(fp32 angle) return tanf(angle); } /** - * @brief 32λķǺ asin + * @brief 用于32位浮点数的反三角函数 asin函数 * @author RM - * @param[in] sinֵ1.0fС-1.0f - * @retval ؽǶ λ + * @param[in] 输入sin值，最大1.0f，最小-1.0f + * @retval 返回角度 单位弧度 */ fp32 AHRS_asinf(fp32 sin) @@ -118,10 +119,10 @@ fp32 AHRS_asinf(fp32 sin) } /** - * @brief Ǻacos + * @brief 反三角函数acos函数 * @author RM - * @param[in] cosֵ1.0fС-1.0f - * @retval ضӦĽǶ λ + * @param[in] 输入cos值，最大1.0f，最小-1.0f + * @retval 返回对应的角度 单位弧度 */ fp32 AHRS_acosf(fp32 cos) @@ -131,11 +132,11 @@ fp32 AHRS_acosf(fp32 cos) } /** - * @brief Ǻatan + * @brief 反三角函数atan函数 * @author RM - * @param[in] tanֵеyֵ С - * @param[in] tanֵеxֵ С - * @retval ضӦĽǶ λ + * @param[in] 输入tan值中的y值 最大正无穷，最小负无穷 + * @param[in] 输入tan值中的x值 最大正无穷，最小负无穷 + * @retval 返回对应的角度 单位弧度 */ fp32 AHRS_atan2f(fp32 y, fp32 x) diff --git a/services/ins/bmi088/bmi088_driver.c b/services/ins/bmi088/bmi088_driver.c index fe8c2d97c8a46afee0bbc3a465ea324266ca11e8..824f32415c3161583887e897089de85c892aec91 100644 --- a/services/ins/bmi088/bmi088_driver.c +++ b/services/ins/bmi088/bmi088_driver.c @@ -2,21 +2,24 @@ #include "BMI088reg.h" #include "softbus.h" #include "cmsis_os.h" +#include "spi.h" +#include "stm32f4xx_hal_spi.h" +#include -#define spiWriteAddr(addr) ((addr)&0x7f) -#define spiReadAddr(addr) ((addr)|0x80) +#define spiWriteAddr(addr) ((addr) & (~0x80)) +#define spiReadAddr(addr) ((addr) | 0x80) #define BMI088_ACCEL_SEN BMI088_ACCEL_6G_SEN; #define BMI088_GYRO_SEN BMI088_GYRO_2000_SEN; -//ٶȼơλ(ֻд2ֽĿǰbug3ֽھûˣĿǰûҵԭҴ3ֽ) +//加速度计、陀螺仪软复位命令(只写2个字节目前测出bug，但传3个字节就没问题了，目前没找到原因暂且传3个字节) uint8_t accSoftResetCmd[] = {spiWriteAddr(BMI088_ACC_SOFTRESET), BMI088_ACC_SOFTRESET_VALUE, 0}; uint8_t gyroSoftResetCmd[] = {spiWriteAddr(BMI088_ACC_SOFTRESET), BMI088_ACC_SOFTRESET_VALUE, 0}; -//ٶȼơǶid +//加速度计、陀螺仪读id命令 uint8_t accIdCmd[] = {spiReadAddr(BMI088_ACC_CHIP_ID), 0, 0}; uint8_t gyroIdCmd[] = {spiReadAddr(BMI088_GYRO_CHIP_ID), 0}; -//ٶȼ +//加速度计配置命令 uint8_t accPwrCtrlCmd[]={spiWriteAddr(BMI088_ACC_PWR_CTRL), BMI088_ACC_ENABLE_ACC_ON}; uint8_t accPwrConfCmd[]={spiWriteAddr(BMI088_ACC_PWR_CONF), BMI088_ACC_PWR_ACTIVE_MODE}; uint8_t accConfCmd[]={spiWriteAddr(BMI088_ACC_CONF), BMI088_ACC_NORMAL| BMI088_ACC_800_HZ | BMI088_ACC_CONF_MUST_Set}; @@ -24,7 +27,7 @@ uint8_t accRangeCmd[]={spiWriteAddr(BMI088_ACC_RANGE), BMI088_ACC_RANGE_6G}; uint8_t accIOConfCmd[]={spiWriteAddr(BMI088_INT1_IO_CTRL), BMI088_ACC_INT1_IO_ENABLE | BMI088_ACC_INT1_GPIO_PP | BMI088_ACC_INT1_GPIO_LOW}; uint8_t accIOMapCmd[]={spiWriteAddr(BMI088_INT_MAP_DATA), BMI088_ACC_INT1_DRDY_INTERRUPT}; -// +//陀螺仪配置命令 uint8_t gyroRangeCmd[]={spiWriteAddr(BMI088_GYRO_RANGE), BMI088_GYRO_2000}; uint8_t gyroBandWidthCmd[]={spiWriteAddr(BMI088_GYRO_BANDWIDTH), BMI088_GYRO_1000_116_HZ | BMI088_GYRO_BANDWIDTH_MUST_Set}; uint8_t gyroPwrConfCmd[]={spiWriteAddr(BMI088_GYRO_LPM1), BMI088_GYRO_NORMAL_MODE}; @@ -32,7 +35,7 @@ uint8_t gyroPwrCtrlCmd[]={spiWriteAddr(BMI088_GYRO_CTRL), BMI088_DRDY_ON}; uint8_t gyroIOConfCmd[]={spiWriteAddr(BMI088_GYRO_INT3_INT4_IO_CONF), BMI088_GYRO_INT3_GPIO_PP | BMI088_GYRO_INT3_GPIO_LOW}; uint8_t gyroIOMapCmd[]={spiWriteAddr(BMI088_GYRO_INT3_INT4_IO_MAP), BMI088_GYRO_DRDY_IO_INT3}; -//ٶȼǻȡ +//加速度计陀螺仪获取数据命令 uint8_t accGetDataCmd[8]={spiReadAddr(BMI088_ACCEL_XOUT_L)}; uint8_t gyroGetDataCmd[7]={spiReadAddr(BMI088_GYRO_X_L)}; uint8_t tmpGetDataCmd[4]={spiReadAddr(BMI088_TEMP_M)}; @@ -41,26 +44,26 @@ uint8_t tmpGetDataCmd[4]={spiReadAddr(BMI088_TEMP_M)}; bool BMI088_AccelInit(uint8_t spiX) { uint8_t accId[3]= {0}; - Bus_RemoteCall("/spi/block", {{"spi-x", &spiX}, - {"tx-data", accIdCmd}, - {"len", IM_PTR(uint16_t, 3)}, - {"timeout", IM_PTR(uint32_t, 1000)}, - {"cs-name", "acc"}, - {"is-block", IM_PTR(bool, true)}}); //ٶȼid + Bus_RemoteCall("/spi/block", {{"spi-x", {&spiX}}, + {"tx-data", {accIdCmd}}, + {"len", {.U32 = 3}}, + {"timeout", {.U32 = 1000}}, + {"cs-name", {"acc"}}, + {"is-block", {.Bool = true}}}); //读加速度计id osDelay(1); Bus_RemoteCall("/spi/block", {{"spi-x", &spiX}, {"tx-data", accPwrConfCmd}, {"len", IM_PTR(uint16_t, 2)}, {"timeout", IM_PTR(uint32_t, 1000)}, {"cs-name", "acc"}, - {"is-block", IM_PTR(bool, true)}}); //ýģʽ + {"is-block", IM_PTR(bool, true)}}); //配置进入活动模式 osDelay(1); Bus_RemoteCall("/spi/block", {{"spi-x", &spiX}, {"tx-data", accPwrCtrlCmd}, {"len", IM_PTR(uint16_t, 2)}, {"timeout", IM_PTR(uint32_t, 1000)}, {"cs-name", "acc"}, - {"is-block", IM_PTR(bool, true)}}); //򿪼ٶȼƵԴ + {"is-block", IM_PTR(bool, true)}}); //打开加速度计电源 osDelay(1); Bus_RemoteCall("/spi/block", {{"spi-x", &spiX}, {"tx-data", accIdCmd}, @@ -68,17 +71,17 @@ bool BMI088_AccelInit(uint8_t spiX) {"len", IM_PTR(uint16_t, 3)}, {"timeout", IM_PTR(uint32_t, 1000)}, {"cs-name", "acc"}, - {"is-block", IM_PTR(bool, true)}}); //ٶȼid + {"is-block", IM_PTR(bool, true)}}); //读加速度计id osDelay(1); - if(accId[2] != BMI088_ACC_CHIP_ID_VALUE) //ٶȼid + if(accId[2] != BMI088_ACC_CHIP_ID_VALUE) //如果加速度计id不对 { Bus_RemoteCall("/spi/block", {{"spi-x", &spiX}, {"tx-data", accSoftResetCmd}, {"len", IM_PTR(uint16_t, 3)}, {"timeout", IM_PTR(uint32_t, 1000)}, {"cs-name", "acc"}, - {"is-block", IM_PTR(bool, true)}}); //λ + {"is-block", IM_PTR(bool, true)}}); //软复位 osDelay(50); return true; } @@ -88,28 +91,28 @@ bool BMI088_AccelInit(uint8_t spiX) {"len", IM_PTR(uint16_t, 2)}, {"timeout", IM_PTR(uint32_t, 1000)}, {"cs-name", "acc"}, - {"is-block", IM_PTR(bool, true)}}); //üٶȼ800Hz, Normalģʽ + {"is-block", IM_PTR(bool, true)}}); //配置加速度计输出速率800Hz, Normal输出模式 osDelay(1); Bus_RemoteCall("/spi/block", {{"spi-x", &spiX}, {"tx-data", accRangeCmd}, {"len", IM_PTR(uint16_t, 2)}, {"timeout", IM_PTR(uint32_t, 1000)}, {"cs-name", "acc"}, - {"is-block", IM_PTR(bool, true)}}); //üٶȼ+-3g + {"is-block", IM_PTR(bool, true)}}); //配置加速度计量程+-3g osDelay(1); Bus_RemoteCall("/spi/block", {{"spi-x", &spiX}, {"tx-data", accPwrConfCmd}, {"len", IM_PTR(uint16_t, 2)}, {"timeout", IM_PTR(uint32_t, 1000)}, {"cs-name", "acc"}, - {"is-block", IM_PTR(bool, true)}}); //ýģʽ + {"is-block", IM_PTR(bool, true)}}); //配置进入正常模式 osDelay(1); Bus_RemoteCall("/spi/block", {{"spi-x", &spiX}, {"tx-data", accPwrCtrlCmd}, {"len", IM_PTR(uint16_t, 2)}, {"timeout", IM_PTR(uint32_t, 1000)}, {"cs-name", "acc"}, - {"is-block", IM_PTR(bool, true)}}); //򿪼ٶȼƵԴ + {"is-block", IM_PTR(bool, true)}}); //打开加速度计电源 osDelay(1); Bus_RemoteCall("/spi/block", {{"spi-x", &spiX}, {"tx-data", accIOConfCmd}, @@ -136,7 +139,7 @@ bool BMI088_GyroInit(uint8_t spiX) {"len", IM_PTR(uint16_t, 2)}, {"timeout", IM_PTR(uint32_t, 1000)}, {"cs-name", "gyro"}, - {"is-block", IM_PTR(bool, true)}}); //id + {"is-block", IM_PTR(bool, true)}}); //读陀螺仪id osDelay(1); Bus_RemoteCall("/spi/block", {{"spi-x", &spiX}, {"tx-data", gyroIdCmd}, @@ -144,17 +147,17 @@ bool BMI088_GyroInit(uint8_t spiX) {"len", IM_PTR(uint16_t, 2)}, {"timeout", IM_PTR(uint32_t, 1000)}, {"cs-name", "gyro"}, - {"is-block", IM_PTR(bool, true)}}); //id + {"is-block", IM_PTR(bool, true)}}); //读陀螺仪id osDelay(1); - if(gyroId[1] != BMI088_GYRO_CHIP_ID_VALUE) //id + if(gyroId[1] != BMI088_GYRO_CHIP_ID_VALUE) //如果陀螺仪id不对 { Bus_RemoteCall("/spi/block", {{"spi-x", &spiX}, {"tx-data", gyroSoftResetCmd}, {"len", IM_PTR(uint16_t, 3)}, {"timeout", IM_PTR(uint32_t, 1000)}, {"cs-name", "gyro"}, - {"is-block", IM_PTR(bool, true)}}); //λ + {"is-block", IM_PTR(bool, true)}}); //软复位 osDelay(50); return true; } @@ -164,21 +167,21 @@ bool BMI088_GyroInit(uint8_t spiX) {"len", IM_PTR(uint16_t, 2)}, {"timeout", IM_PTR(uint32_t, 1000)}, {"cs-name", "gyro"}, - {"is-block", IM_PTR(bool, true)}}); //1000Hz, 116Hz + {"is-block", IM_PTR(bool, true)}}); //配置陀螺仪输出速率1000Hz, 带宽116Hz osDelay(1); Bus_RemoteCall("/spi/block", {{"spi-x", &spiX}, {"tx-data", gyroRangeCmd}, {"len", IM_PTR(uint16_t, 2)}, {"timeout", IM_PTR(uint32_t, 1000)}, {"cs-name", "gyro"}, - {"is-block", IM_PTR(bool, true)}}); //Χ+-2000/s + {"is-block", IM_PTR(bool, true)}}); //配置陀螺仪输出范围+-2000°/s osDelay(1); Bus_RemoteCall("/spi/block", {{"spi-x", &spiX}, {"tx-data", gyroPwrConfCmd}, {"len", IM_PTR(uint16_t, 2)}, {"timeout", IM_PTR(uint32_t, 1000)}, {"cs-name", "gyro"}, - {"is-block", IM_PTR(bool, true)}}); //ýģʽ + {"is-block", IM_PTR(bool, true)}}); //配置进入活动模式 osDelay(1); Bus_RemoteCall("/spi/block", {{"spi-x", &spiX}, {"tx-data", gyroIOConfCmd}, @@ -208,7 +211,7 @@ void BMI088_ReadData(uint8_t spiX, float gyro[3], float accel[3], float *tempera {"len", IM_PTR(uint16_t, 8)}, {"timeout", IM_PTR(uint32_t, 1000)}, {"cs-name", "acc"}, - {"is-block", IM_PTR(bool, true)}}); //ȡٶȼ + {"is-block", IM_PTR(bool, true)}}); //获取加速度计数据 temp = (int16_t)((buf[3]) << 8) | buf[2]; accel[0] = temp * BMI088_ACCEL_SEN; temp = (int16_t)((buf[5]) << 8) | buf[4]; @@ -222,7 +225,7 @@ void BMI088_ReadData(uint8_t spiX, float gyro[3], float accel[3], float *tempera {"len", IM_PTR(uint16_t, 7)}, {"timeout", IM_PTR(uint32_t, 1000)}, {"cs-name", "gyro"}, - {"is-block", IM_PTR(bool, true)}}); //ȡ + {"is-block", IM_PTR(bool, true)}}); //获取陀螺仪数据 // if(buf[0] == BMI088_GYRO_CHIP_ID_VALUE) // { temp = (int16_t)((buf[2]) << 8) | buf[1]; @@ -239,7 +242,7 @@ void BMI088_ReadData(uint8_t spiX, float gyro[3], float accel[3], float *tempera {"len", IM_PTR(uint16_t, 4)}, {"timeout", IM_PTR(uint32_t, 1000)}, {"cs-name", "acc"}, - {"is-block", IM_PTR(bool, true)}}); //ȡ¶ + {"is-block", IM_PTR(bool, true)}}); //获取温度数据 temp = (int16_t)((buf[2] << 3) | (buf[3] >> 5)); if (temp > 1023) diff --git a/services/ins/ins.c b/services/ins/ins.c index c4d49f3b83c54708b904e8982392aa55a8e3bbd3..ef8f14a4ea29220ca885d3b4c70efaefe9dbef64 100644 --- a/services/ins/ins.c +++ b/services/ins/ins.c @@ -5,6 +5,7 @@ #include "AHRS.h" #include "pid.h" #include "filter.h" +#include typedef struct { @@ -26,7 +27,7 @@ typedef struct PID tmpPID; Filter *filter; - uint16_t taskInterval; //ִм + uint16_t taskInterval; //任务执行间隔 char* eulerAngleName; }INS; @@ -42,7 +43,7 @@ void INS_TaskCallback(void const * argument) osDelay(50); INS_Init(&ins, (ConfItem*)argument); AHRS_init(ins.imu.quat,ins.imu.accel,ins.imu.mag); - //У׼ƫ + //校准零偏 // for(int i=0;i<10000;i++) // { // BMI088_ReadData(ins.spiX, ins.imu.gyro,ins.imu.accel, &ins.imu.tmp); @@ -55,7 +56,7 @@ void INS_TaskCallback(void const * argument) // ins.imu.gyroOffset[1] = ins.imu.gyroOffset[1]/10000.0f; // ins.imu.gyroOffset[2] = ins.imu.gyroOffset[2]/10000.0f; - ins.imu.gyroOffset[0] = -0.000767869; //10У׼ȡֵ + ins.imu.gyroOffset[0] = -0.000767869; //10次校准取均值 ins.imu.gyroOffset[1] = 0.000771033; ins.imu.gyroOffset[2] = 0.001439746; @@ -66,16 +67,16 @@ void INS_TaskCallback(void const * argument) for(uint8_t i=0;i<3;i++) ins.imu.gyro[i] -= ins.imu.gyroOffset[i]; - //˲ + //滤波 // for(uint8_t i=0;i<3;i++) // ins.imu.accel[i] = ins.filter->cala(ins.filter , ins.imu.accel[i]); - //ں + //数据融合 AHRS_update(ins.imu.quat,ins.taskInterval/1000.0f,ins.imu.gyro,ins.imu.accel,ins.imu.mag); get_angle(ins.imu.quat,&ins.yaw,&ins.pitch,&ins.roll); ins.yaw = ins.yaw/PI*180; ins.pitch = ins.pitch/PI*180; ins.roll = ins.roll/PI*180; - // + //发布数据 Bus_BroadcastSend(ins.eulerAngleName, {{"yaw",&ins.yaw}, {"pitch",&ins.pitch}, {"roll",&ins.roll}}); osDelay(ins.taskInterval); } @@ -96,7 +97,7 @@ void INS_Init(INS* ins, ConfItem* dict) char* temp = Conf_GetPtr(dict, "name", char); temp = temp ? temp : "ins"; uint8_t len = strlen(temp); - ins->eulerAngleName = pvPortMalloc(len + 13+ 1); //13Ϊ"/ /euler-angle"ijȣ1Ϊ'\0'ij + ins->eulerAngleName = pvPortMalloc(len + 13+ 1); //13为"/ /euler-angle"的长度，1为'\0'的长度 sprintf(ins->eulerAngleName, "/%s/euler-angle", temp); while(BMI088_AccelInit(ins->spiX) || BMI088_GyroInit(ins->spiX)) @@ -106,16 +107,18 @@ void INS_Init(INS* ins, ConfItem* dict) BMI088_ReadData(ins->spiX, ins->imu.gyro,ins->imu.accel, &ins->imu.tmp); - //ʱ¶pid + //创建定时器进行温度pid控制 osTimerDef(tmp, INS_TmpPIDTimerCallback); osTimerStart(osTimerCreate(osTimer(tmp), osTimerPeriodic, ins), 2); } -//ʱص +//软件定时器回调函数 void INS_TmpPIDTimerCallback(void const *argument) { INS* ins = pvTimerGetTimerID((TimerHandle_t)argument); PID_SingleCalc(&ins->tmpPID, ins->targetTmp, ins->imu.tmp); ins->tmpPID.output = ins->tmpPID.output > 0? ins->tmpPID.output : 0; - Bus_RemoteCall("/tim/pwm/set-duty", {{"tim-x", &ins->timX}, {"channel-x", &ins->channelX}, {"duty", &ins->tmpPID.output}}); + Bus_RemoteCall("/tim/pwm/set-duty", {{"tim-x", {.U8 = ins->timX}}, + {"channel-x", {.U8 = ins->channelX}}, + {"duty", {.F32 = ins->tmpPID.output}}}); } diff --git a/services/judgement/judge.c b/services/judgement/judge.c index 3c52e2d508658ee63272bb410c54c1d13392d29b..cf4605dcff5b69aa7095c9587e74031bc931621b 100644 --- a/services/judgement/judge.c +++ b/services/judgement/judge.c @@ -3,38 +3,39 @@ #include "softbus.h" #include "cmsis_os.h" #include "my_queue.h" +#include typedef struct _Judge { - JudgeRecInfo judgeRecInfo; //Ӳϵͳյ - bool dataTF; //Ƿ, - Queue txQueue; //Ͷ + JudgeRecInfo judgeRecInfo; //从裁判系统接收到的数据 + bool dataTF; //裁判数据是否可用,辅助函数调用 + Queue txQueue; //发送队列 JudgeTxFrame *queueBuf; uint8_t uartX; - uint16_t taskInterval; //ִм + uint16_t taskInterval; //任务执行间隔 }Judge; void Judge_Init(Judge *judge,ConfItem* dict); void Judge_Recv_SoftBusCallback(const char* topic, SoftBusFrame* frame, void* bindData); -bool Judge_UiDrawTextCallback(const char* topic, SoftBusFrame* frame, void* bindData); //ı -bool Judge_UiDrawLineCallback(const char* topic, SoftBusFrame* frame, void* bindData); //ֱ -bool Judge_UiDrawRectCallback(const char* topic, SoftBusFrame* frame, void* bindData); // -bool Judge_UiDrawCircleCallback(const char* topic, SoftBusFrame* frame, void* bindData); //Բ -bool Judge_UiDrawOvalCallback(const char* topic, SoftBusFrame* frame, void* bindData); //Բ -bool Judge_UiDrawArcCallback(const char* topic, SoftBusFrame* frame, void* bindData);//Բ +bool Judge_UiDrawTextCallback(const char* topic, SoftBusFrame* frame, void* bindData); //画文本 +bool Judge_UiDrawLineCallback(const char* topic, SoftBusFrame* frame, void* bindData); //画直线 +bool Judge_UiDrawRectCallback(const char* topic, SoftBusFrame* frame, void* bindData); //画矩形 +bool Judge_UiDrawCircleCallback(const char* topic, SoftBusFrame* frame, void* bindData); //画圆 +bool Judge_UiDrawOvalCallback(const char* topic, SoftBusFrame* frame, void* bindData); //画椭圆 +bool Judge_UiDrawArcCallback(const char* topic, SoftBusFrame* frame, void* bindData);//画圆弧 bool Judge_UiDrawFloatCallback(const char* topic, SoftBusFrame* frame, void* bindData); bool Judge_UiDrawIntCallback(const char* topic, SoftBusFrame* frame, void* bindData); bool JUDGE_Read_Data(Judge *judge,uint8_t *ReadFromUsart); void Judge_publishData(Judge * judge); void Judge_TimerCallback(void const *argument); -Judge judge={0}; //ڴ ʺΪľֲ +Judge judge={0}; //大内存 不适合作为任务的局部变量 JudgeTxFrame debug; void Judge_TaskCallback(void const * argument) { - //ٽ + //进入临界区 portENTER_CRITICAL(); Judge_Init(&judge, (ConfItem*)argument); portEXIT_CRITICAL(); @@ -43,27 +44,27 @@ void Judge_TaskCallback(void const * argument) { if(!Queue_IsEmpty(&judge.txQueue)) { - //ȡͷϢ + //取队头的消息发送 JudgeTxFrame *txframe=(JudgeTxFrame*)Queue_Dequeue(&judge.txQueue); - //ui - Bus_RemoteCall("/uart/trans/dma",{{"uart-x",&judge.uartX}, - {"data",(uint8_t *)txframe->data}, - {"trans-size",&txframe->frameLength}}); + //发送ui + Bus_RemoteCall("/uart/trans/dma",{{"uart-x", {.U8 = judge.uartX}}, + {"data", {txframe->data}}, + {"trans-size", {.U16 = txframe->frameLength}}}); } osDelay(judge.taskInterval); } } -//ʼ +//初始化 void Judge_Init(Judge* judge,ConfItem* dict) { - //ʼͶ - uint16_t maxTxQueueLen = Conf_GetValue(dict, "max-tx-queue-length", uint16_t, 20); //Ͷ + //初始化发送队列 + uint16_t maxTxQueueLen = Conf_GetValue(dict, "max-tx-queue-length", uint16_t, 20); //最大发送队列 judge->queueBuf=(JudgeTxFrame*)pvPortMalloc(maxTxQueueLen*sizeof(JudgeTxFrame)); Queue_Init(&judge->txQueue,maxTxQueueLen); Queue_AttachBuffer(&judge->txQueue,judge->queueBuf,sizeof(JudgeTxFrame)); - judge->taskInterval = Conf_GetValue(dict, "task-interval", uint16_t, 150); //ִм + judge->taskInterval = Conf_GetValue(dict, "task-interval", uint16_t, 150); //任务执行间隔 char name[] = "/uart_/recv"; judge->uartX = Conf_GetValue(dict, "uart-x", uint8_t, 0); name[5] = judge->uartX + '0'; @@ -77,16 +78,16 @@ void Judge_Init(Judge* judge,ConfItem* dict) Bus_RegisterRemoteFunc(judge, Judge_UiDrawArcCallback, "/judge/send/ui/arc"); Bus_RegisterRemoteFunc(judge, Judge_UiDrawFloatCallback, "/judge/send/ui/float"); Bus_RegisterRemoteFunc(judge, Judge_UiDrawIntCallback, "/judge/send/ui/int"); - //ʱ ʱ㲥յ + //开启软件定时器 定时广播接收到的数据 osTimerDef(judge, Judge_TimerCallback); osTimerStart(osTimerCreate(osTimer(judge), osTimerPeriodic, judge), 20); } -//ϵͳʱص +//系统定时器回调 void Judge_TimerCallback(void const *argument) { Judge *judge =(Judge*)argument; - Judge_publishData(judge); //㲥յ + Judge_publishData(judge); //广播接收到的数据 } @@ -100,9 +101,9 @@ void Judge_Recv_SoftBusCallback(const char* topic, SoftBusFrame* frame, void* bi /* -uiͼnameڲУΪͻ˵Ψһֻ3ַ -㲥uiƵӦС5hzӦݸʱ㲥 -Ӧͼβ +ui的图形名（name）在操作中，作为客户端的唯一索引，有且只有3个字符 +各任务广播ui的频率应小于5hz，应仅在数据更新时广播 +立即数对应的图形操作 typedef enum _GraphOperation { Operation_Null=0, @@ -110,12 +111,12 @@ typedef enum _GraphOperation Operation_Change, Operation_Delete }GraphOperation; -***UIʱOperation_AddOperation_ChangeOperation_Delete +***添加UI时必须先Operation_Add，才能Operation_Change、Operation_Delete -Ӧͼɫ +立即数对应的图形颜色 typedef enum _GraphColor { - Color_Self=0,//ɫ + Color_Self=0,//己方主色 Color_Yellow, Color_Green, Color_Orange, @@ -127,10 +128,10 @@ typedef enum _GraphColor }GraphColor; */ -bool Judge_UiDrawTextCallback(const char* topic, SoftBusFrame* frame, void* bindData) //ı +bool Judge_UiDrawTextCallback(const char* topic, SoftBusFrame* frame, void* bindData) //画文本 { Judge *judge = (Judge*)bindData; - // ı ɫ ͼ С ʽ + // 名字 文本 颜色 宽度 图层 坐标 字体大小 长度 操作方式 if(!Bus_CheckMapKeys(frame,{"name","text","color","width","layer","start-x","start-y","size","len","opera"})) return false; graphic_data_struct_t text; @@ -153,10 +154,10 @@ bool Judge_UiDrawTextCallback(const char* topic, SoftBusFrame* frame, void* bind return true; } -bool Judge_UiDrawLineCallback(const char* topic, SoftBusFrame* frame, void* bindData) //ֱ +bool Judge_UiDrawLineCallback(const char* topic, SoftBusFrame* frame, void* bindData) //画直线 { Judge *judge = (Judge*)bindData; - // ɫ ͼ ʼ ʽ + // 名字 颜色 宽度 图层 开始坐标 结束坐标 操作方式 if(!Bus_CheckMapKeys(frame,{"name","color","width","layer","start-x","start-y","end-x","end-y","opera"})) return false; graphic_data_struct_t line; @@ -170,17 +171,17 @@ bool Judge_UiDrawLineCallback(const char* topic, SoftBusFrame* frame, void* bind line.start_y = *(int16_t*)Bus_GetMapValue(frame,"start-y"); line.end_x = *(int16_t*)Bus_GetMapValue(frame,"end-x"); line.end_y = *(int16_t*)Bus_GetMapValue(frame,"end-y"); - JudgeTxFrame txframe = JUDGE_PackGraphData(judge->judgeRecInfo.GameRobotStat.robot_id, //ID - 0x100+judge->judgeRecInfo.GameRobotStat.robot_id, //IDͻˣ + JudgeTxFrame txframe = JUDGE_PackGraphData(judge->judgeRecInfo.GameRobotStat.robot_id, //发送者ID + 0x100+judge->judgeRecInfo.GameRobotStat.robot_id, //接收者ID（客户端） &line); Queue_Enqueue(&judge->txQueue,&txframe); return true; } -bool Judge_UiDrawRectCallback(const char* topic, SoftBusFrame* frame, void* bindData) // +bool Judge_UiDrawRectCallback(const char* topic, SoftBusFrame* frame, void* bindData) //画矩形 { Judge *judge = (Judge*)bindData; - // ɫ ͼ ʼ Խ ʽ + // 名字 颜色 宽度 图层 开始坐标 对角坐标 操作方式 if(!Bus_CheckMapKeys(frame,{"name","color","width","layer","start-x","start-y","end-x","end-y","opera"})) return false; graphic_data_struct_t rect; @@ -194,17 +195,17 @@ bool Judge_UiDrawRectCallback(const char* topic, SoftBusFrame* frame, void* bind rect.start_y=*(int16_t*)Bus_GetMapValue(frame,"start-y"); rect.end_x=*(int16_t*)Bus_GetMapValue(frame,"end-x"); rect.end_y=*(int16_t*)Bus_GetMapValue(frame,"end-y"); - JudgeTxFrame txframe = JUDGE_PackGraphData(judge->judgeRecInfo.GameRobotStat.robot_id,//ID - 0x100+judge->judgeRecInfo.GameRobotStat.robot_id,//IDͻˣ + JudgeTxFrame txframe = JUDGE_PackGraphData(judge->judgeRecInfo.GameRobotStat.robot_id,//发送者ID + 0x100+judge->judgeRecInfo.GameRobotStat.robot_id,//接收者ID（客户端） &rect); Queue_Enqueue(&judge->txQueue,&txframe); return true; } -bool Judge_UiDrawCircleCallback(const char* topic, SoftBusFrame* frame, void* bindData) //Բ +bool Judge_UiDrawCircleCallback(const char* topic, SoftBusFrame* frame, void* bindData) //画圆 { Judge *judge = (Judge*)bindData; - // ɫ ͼ 뾶 ʽ + // 名字 颜色 宽度 图层 中心坐标 半径 操作方式 if(!Bus_CheckMapKeys(frame,{"name","color","width","layer","cent-x","cent-y","radius","opera"})) return false; graphic_data_struct_t circle; @@ -217,17 +218,17 @@ bool Judge_UiDrawCircleCallback(const char* topic, SoftBusFrame* frame, void* bi circle.start_x=*(int16_t*)Bus_GetMapValue(frame,"cent-x"); circle.start_y=*(int16_t*)Bus_GetMapValue(frame,"cent-y"); circle.radius=*(uint16_t*)Bus_GetMapValue(frame,"radius"); - JudgeTxFrame txframe = JUDGE_PackGraphData(judge->judgeRecInfo.GameRobotStat.robot_id,//ID - 0x100+judge->judgeRecInfo.GameRobotStat.robot_id,//IDͻˣ + JudgeTxFrame txframe = JUDGE_PackGraphData(judge->judgeRecInfo.GameRobotStat.robot_id,//发送者ID + 0x100+judge->judgeRecInfo.GameRobotStat.robot_id,//接收者ID（客户端） &circle); Queue_Enqueue(&judge->txQueue,&txframe); return true; } -bool Judge_UiDrawOvalCallback(const char* topic, SoftBusFrame* frame, void* bindData) //Բ +bool Judge_UiDrawOvalCallback(const char* topic, SoftBusFrame* frame, void* bindData) //画椭圆 { Judge *judge = (Judge*)bindData; - // ɫ ͼ xy᳤ ʽ + // 名字 颜色 宽度 图层 中心坐标 xy半轴长 操作方式 if(!Bus_CheckMapKeys(frame,{"name","color","width","layer","cent-x","cent-y","semiaxis-x","semiaxis-y","opera"})) return false; graphic_data_struct_t oval; @@ -241,17 +242,17 @@ bool Judge_UiDrawOvalCallback(const char* topic, SoftBusFrame* frame, void* bind oval.start_y=*(int16_t*)Bus_GetMapValue(frame,"cent-y"); oval.end_x=*(int16_t*)Bus_GetMapValue(frame,"semiaxis-x"); oval.end_y=*(int16_t*)Bus_GetMapValue(frame,"semiaxis-y"); - JudgeTxFrame txframe = JUDGE_PackGraphData(judge->judgeRecInfo.GameRobotStat.robot_id,//ID - 0x100+judge->judgeRecInfo.GameRobotStat.robot_id,//IDͻˣ + JudgeTxFrame txframe = JUDGE_PackGraphData(judge->judgeRecInfo.GameRobotStat.robot_id,//发送者ID + 0x100+judge->judgeRecInfo.GameRobotStat.robot_id,//接收者ID（客户端） &oval); Queue_Enqueue(&judge->txQueue,&txframe); return true; } -bool Judge_UiDrawArcCallback(const char* topic, SoftBusFrame* frame, void* bindData) //Բ +bool Judge_UiDrawArcCallback(const char* topic, SoftBusFrame* frame, void* bindData) //画圆弧 { Judge *judge = (Judge*)bindData; - // ɫ ͼ xy᳤ ʼֹǶ ʽ + // 名字 颜色 宽度 图层 中心坐标 xy半轴长 起始、终止角度 操作方式 if(!Bus_CheckMapKeys(frame,{"name","color","width","layer","cent-x","cent-y","semiaxis-x","semiaxis-y","start-angle","end-angle","opera"})) return false; graphic_data_struct_t arc; @@ -267,8 +268,8 @@ bool Judge_UiDrawArcCallback(const char* topic, SoftBusFrame* frame, void* bindD arc.end_y=*(int16_t*)Bus_GetMapValue(frame,"semiaxis-y"); arc.start_angle=*(int16_t*)Bus_GetMapValue(frame,"start-angle"); arc.end_angle=*(int16_t*)Bus_GetMapValue(frame,"end-angle"); - JudgeTxFrame txframe = JUDGE_PackGraphData(judge->judgeRecInfo.GameRobotStat.robot_id,//ID - 0x100+judge->judgeRecInfo.GameRobotStat.robot_id,//IDͻˣ + JudgeTxFrame txframe = JUDGE_PackGraphData(judge->judgeRecInfo.GameRobotStat.robot_id,//发送者ID + 0x100+judge->judgeRecInfo.GameRobotStat.robot_id,//接收者ID（客户端） &arc); Queue_Enqueue(&judge->txQueue,&txframe); return true; @@ -277,7 +278,7 @@ bool Judge_UiDrawArcCallback(const char* topic, SoftBusFrame* frame, void* bindD bool Judge_UiDrawFloatCallback(const char* topic, SoftBusFrame* frame, void* bindData) { Judge *judge = (Judge*)bindData; - // ɫ ͼ ֵ С Чλ ʽ + // 名字 颜色 宽度 图层 值 坐标 大小 有效位数 操作方式 if(!Bus_CheckMapKeys(frame,{"name","color","width","layer","value","start-x","start-y","size","digit","opera"})) return false; graphic_data_struct_t float_num; @@ -295,8 +296,8 @@ bool Judge_UiDrawFloatCallback(const char* topic, SoftBusFrame* frame, void* bin float_num.radius=((int32_t)(value*1000))&0x3FF; float_num.end_x=((int32_t)(value*1000)>>10)&0x7FF; float_num.end_y=((int32_t)(value*1000)>>21)&0x7FF; - JudgeTxFrame txframe = JUDGE_PackGraphData(judge->judgeRecInfo.GameRobotStat.robot_id,//ID - 0x100+judge->judgeRecInfo.GameRobotStat.robot_id,//IDͻˣ + JudgeTxFrame txframe = JUDGE_PackGraphData(judge->judgeRecInfo.GameRobotStat.robot_id,//发送者ID + 0x100+judge->judgeRecInfo.GameRobotStat.robot_id,//接收者ID（客户端） &float_num); Queue_Enqueue(&judge->txQueue,&txframe); return true; @@ -305,7 +306,7 @@ bool Judge_UiDrawFloatCallback(const char* topic, SoftBusFrame* frame, void* bin bool Judge_UiDrawIntCallback(const char* topic, SoftBusFrame* frame, void* bindData) { Judge *judge = (Judge*)bindData; - // ɫ ͼ ֵ С ʽ + // 名字 颜色 宽度 图层 值 坐标 大小 操作方式 if(!Bus_CheckMapKeys(frame,{"name","color","width","layer","value","start-x","start-y","size","opera"})) return false; graphic_data_struct_t int_num; @@ -322,33 +323,33 @@ bool Judge_UiDrawIntCallback(const char* topic, SoftBusFrame* frame, void* bindD int_num.radius= value&0x3FF; int_num.end_x=( value>>10)&0x7FF; int_num.end_y=( value>>21)&0x7FF; - JudgeTxFrame txframe = JUDGE_PackGraphData(judge->judgeRecInfo.GameRobotStat.robot_id,//ID - 0x100+judge->judgeRecInfo.GameRobotStat.robot_id,//IDͻˣ + JudgeTxFrame txframe = JUDGE_PackGraphData(judge->judgeRecInfo.GameRobotStat.robot_id,//发送者ID + 0x100+judge->judgeRecInfo.GameRobotStat.robot_id,//接收者ID（客户端） &int_num); Queue_Enqueue(&judge->txQueue,&txframe); return true; } -//㲥յ +//广播接收到的数据 void Judge_publishData(Judge* judge) { // if(!judge->dataTF) // return; - //׼ + //准备带发布的数据 uint8_t robot_id = judge->judgeRecInfo.GameRobotStat.robot_id; - uint8_t robot_color = robot_id<10?RobotColor_Blue:RobotColor_Red;//ɫ - uint16_t chassis_power_limit = judge->judgeRecInfo.GameRobotStat.chassis_power_limit; //̹ - bool isShooterPowerOutput = judge->judgeRecInfo.GameRobotStat.mains_power_shooter_output; //ܷǷϵ - bool isChassisPowerOutput = judge->judgeRecInfo.GameRobotStat.mains_power_chassis_output; //ܵǷϵ - float chassis_power = judge->judgeRecInfo.PowerHeatData.chassis_power; //̹ - uint16_t chassis_power_buffer = judge->judgeRecInfo.PowerHeatData.chassis_power_buffer; //̻ - float bullet_speed = judge->judgeRecInfo.ShootData.bullet_speed; //䵯ٶ - //ݷ - if(robot_id == 1|| robot_id == 101) //Ӣ + uint8_t robot_color = robot_id<10?RobotColor_Blue:RobotColor_Red;//机器人颜色 + uint16_t chassis_power_limit = judge->judgeRecInfo.GameRobotStat.chassis_power_limit; //底盘功率限制 + bool isShooterPowerOutput = judge->judgeRecInfo.GameRobotStat.mains_power_shooter_output; //电管发射机构是否断电 + bool isChassisPowerOutput = judge->judgeRecInfo.GameRobotStat.mains_power_chassis_output; //电管底盘是否断电 + float chassis_power = judge->judgeRecInfo.PowerHeatData.chassis_power; //底盘功率 + uint16_t chassis_power_buffer = judge->judgeRecInfo.PowerHeatData.chassis_power_buffer; //底盘缓冲 + float bullet_speed = judge->judgeRecInfo.ShootData.bullet_speed; //发射弹丸速度 + //数据发布 + if(robot_id == 1|| robot_id == 101) //英雄 { - uint16_t shooter_id1_42mm_speed_limit = judge->judgeRecInfo.GameRobotStat.shooter_id1_42mm_speed_limit; //42mm - uint16_t shooter_id1_42mm_cooling_heat = judge->judgeRecInfo.PowerHeatData.shooter_id1_42mm_cooling_heat; //42mmǹ - uint16_t bullet_remaining_num_42mm = judge->judgeRecInfo.BulletRemaining.bullet_remaining_num_42mm; //42mmʣ൯ + uint16_t shooter_id1_42mm_speed_limit = judge->judgeRecInfo.GameRobotStat.shooter_id1_42mm_speed_limit; //42mm弹速上限 + uint16_t shooter_id1_42mm_cooling_heat = judge->judgeRecInfo.PowerHeatData.shooter_id1_42mm_cooling_heat; //42mm枪口热量 + uint16_t bullet_remaining_num_42mm = judge->judgeRecInfo.BulletRemaining.bullet_remaining_num_42mm; //42mm剩余弹丸数量 Bus_BroadcastSend("/judge/recv/robot-state",{{"color",&robot_color}, {"42mm-speed-limit",&shooter_id1_42mm_speed_limit}, {"chassis-power-limit",&chassis_power_limit}, @@ -362,12 +363,12 @@ void Judge_publishData(Judge* judge) Bus_BroadcastSend("/judge/recv/shoot",{{"bullet-speed",&bullet_speed}}); Bus_BroadcastSend("/judge/recv/bullet",{ {"42mm-bullet-remain",&bullet_remaining_num_42mm}}); } - else //Ӣ۵λ + else //非英雄单位 { - uint16_t shooter_id1_17mm_speed_limit = judge->judgeRecInfo.GameRobotStat.shooter_id1_17mm_speed_limit; //17mm - uint16_t shooter_id1_17mm_cooling_heat = judge->judgeRecInfo.PowerHeatData.shooter_id1_17mm_cooling_heat; //17mmǹ - uint16_t bullet_remaining_num_17mm = judge->judgeRecInfo.BulletRemaining.bullet_remaining_num_17mm; //17mmʣ൯ - bool isGimbalPowerOutput = judge->judgeRecInfo.GameRobotStat.mains_power_gimbal_output; //̨Ƿϵ + uint16_t shooter_id1_17mm_speed_limit = judge->judgeRecInfo.GameRobotStat.shooter_id1_17mm_speed_limit; //17mm弹速上限 + uint16_t shooter_id1_17mm_cooling_heat = judge->judgeRecInfo.PowerHeatData.shooter_id1_17mm_cooling_heat; //17mm枪口热量 + uint16_t bullet_remaining_num_17mm = judge->judgeRecInfo.BulletRemaining.bullet_remaining_num_17mm; //17mm剩余弹丸数量 + bool isGimbalPowerOutput = judge->judgeRecInfo.GameRobotStat.mains_power_gimbal_output; //电管云台是否断电 Bus_BroadcastSend("/judge/recv/robot-state",{{"color",&robot_color}, {"17mm-speed-limit",&shooter_id1_17mm_speed_limit}, {"chassis-power-limit",&chassis_power_limit}, diff --git a/services/judgement/judge_drive.c b/services/judgement/judge_drive.c index 762a5cf0171c31d1a372948f4cae92fd7a4225db..71e4209cdfd8b13c96155ff57b8c756e1f0dcf01 100644 --- a/services/judgement/judge_drive.c +++ b/services/judgement/judge_drive.c @@ -1,45 +1,45 @@ #include "judge_drive.h" #include "string.h" #include "crc_dji.h" -/**************ϵͳݸ****************/ +/**************裁判系统数据辅助****************/ /** - * @brief ȡ,жжȡ֤ٶ - * @param - * @retval Ƿж - * @attention ڴж֡ͷCRCУ,дݣظж֡ͷ + * @brief 读取裁判数据,中断中读取保证速度 + * @param 缓存数据 + * @retval 是否对正误判断做处理 + * @attention 在此判断帧头和CRC校验,无误再写入数据，不重复判断帧头 */ bool JUDGE_Read_Data(JudgeRecInfo *judge,uint8_t *ReadFromUsart) { - uint16_t judge_length;//ͳһ֡ݳ + uint16_t judge_length;//统计一帧数据长度 - int CmdID = 0;// + int CmdID = 0;//数据命令码解析 /***------------------*****/ - //ݰκδ + //无数据包，则不作任何处理 if (ReadFromUsart == NULL) { return false; } - //д֡ͷ,жǷʼ洢 + //写入帧头数据,用于判断是否开始存储裁判数据 memcpy(&judge->FrameHeader, ReadFromUsart, LEN_HEADER); - //ж֡ͷǷΪ0xA5 + //判断帧头数据是否为0xA5 if(ReadFromUsart[ SOF ] == JUDGE_FRAME_HEADER) { - //֡ͷCRC8У + //帧头CRC8校验 if (!Verify_CRC8_Check_Sum( ReadFromUsart, LEN_HEADER )) return false; - //ͳһ֡ݳ,CR16У + //统计一帧数据长度,用于CR16校验 judge_length = ReadFromUsart[ DATA_LENGTH ] + LEN_HEADER + LEN_CMDID + LEN_TAIL; - //֡βCRC16У + //帧尾CRC16校验 if(!Verify_CRC16_Check_Sum(ReadFromUsart,judge_length)) return false; CmdID = (ReadFromUsart[6] << 8 | ReadFromUsart[5]); - //,ݿӦṹ(ע⿽ݵij) + //解析数据命令码,将数据拷贝到相应结构体中(注意拷贝数据的长度) switch(CmdID) { case ID_game_state: //0x0001 @@ -111,18 +111,18 @@ bool JUDGE_Read_Data(JudgeRecInfo *judge,uint8_t *ReadFromUsart) memcpy(&judge->DartClientCmd, (ReadFromUsart + DATA), LEN_dart_client_cmd); break; } - //׵ַ֡,ָCRC16һֽ,жǷΪ0xA5,жһݰǷж֡ + //首地址加帧长度,指向CRC16下一字节,用来判断是否为0xA5,用来判断一个数据包是否有多帧数据 if(*(ReadFromUsart + sizeof(xFrameHeader) + LEN_CMDID + judge->FrameHeader.DataLength + LEN_TAIL) == 0xA5) { - //һݰ˶֡,ٴζȡ + //如果一个数据包出现了多帧数据,则再次读取 JUDGE_Read_Data(judge,ReadFromUsart + sizeof(xFrameHeader) + LEN_CMDID + judge->FrameHeader.DataLength + LEN_TAIL); } - return true;//У˵ݿ + return true;//都校验过了则说明数据可用 } - return false;//֡ͷ + return false;//帧头有问题 } -//ıݣΪı֡ͷУϢγһ֡ +//打包文本数据，为文本数据添加帧头，校验码等信息，形成一个完整的帧 JudgeTxFrame JUDGE_PackTextData(uint8_t sendID,uint8_t receiveID,graphic_data_struct_t *textConf,uint8_t text[30]) { JudgeTxFrame txFrame; @@ -130,13 +130,13 @@ JudgeTxFrame JUDGE_PackTextData(uint8_t sendID,uint8_t receiveID,graphic_data_st textData.txFrameHeader.SOF=0xA5; textData.txFrameHeader.DataLength=sizeof(ext_student_interactive_header_data_t)+sizeof(ext_client_custom_character_t); textData.txFrameHeader.Seq=0; - memcpy(txFrame.data, &textData.txFrameHeader, sizeof(xFrameHeader));//д֡ͷ - Append_CRC8_Check_Sum(txFrame.data, sizeof(xFrameHeader));//д֡ͷCRC8У + memcpy(txFrame.data, &textData.txFrameHeader, sizeof(xFrameHeader));//写入帧头数据 + Append_CRC8_Check_Sum(txFrame.data, sizeof(xFrameHeader));//写入帧头CRC8校验码 - textData.CmdID=0x301;//֡ID - textData.dataFrameHeader.data_cmd_id=0x0110;//ݶID - textData.dataFrameHeader.send_ID = sendID;//ߵID - textData.dataFrameHeader.receiver_ID = receiveID; //ߵID + textData.CmdID=0x301;//数据帧ID + textData.dataFrameHeader.data_cmd_id=0x0110;//数据段ID + textData.dataFrameHeader.send_ID = sendID;//发送者的ID + textData.dataFrameHeader.receiver_ID = receiveID; //接收者的ID textData.textData.grapic_data_struct=*textConf; memcpy(textData.textData.data,text,30); @@ -149,7 +149,7 @@ JudgeTxFrame JUDGE_PackTextData(uint8_t sendID,uint8_t receiveID,graphic_data_st txFrame.frameLength=sizeof(textData); return txFrame; } -//ͼݣΪͼ֡ͷУϢγһ֡ +//打包图像数据，为图像数据添加帧头，校验码等信息，形成一个完整的帧 JudgeTxFrame JUDGE_PackGraphData(uint8_t sendID,uint8_t receiveID,graphic_data_struct_t *data) { JudgeTxFrame txFrame; @@ -157,13 +157,13 @@ JudgeTxFrame JUDGE_PackGraphData(uint8_t sendID,uint8_t receiveID,graphic_data_s graphData.txFrameHeader.SOF=0xA5; graphData.txFrameHeader.DataLength=sizeof(ext_student_interactive_header_data_t)+sizeof(ext_client_custom_graphic_single_t); graphData.txFrameHeader.Seq=0; - memcpy(txFrame.data, &graphData.txFrameHeader, sizeof(xFrameHeader));//д֡ͷ - Append_CRC8_Check_Sum(txFrame.data, sizeof(xFrameHeader));//д֡ͷCRC8У + memcpy(txFrame.data, &graphData.txFrameHeader, sizeof(xFrameHeader));//写入帧头数据 + Append_CRC8_Check_Sum(txFrame.data, sizeof(xFrameHeader));//写入帧头CRC8校验码 - graphData.CmdID=0x301;//֡ID - graphData.dataFrameHeader.data_cmd_id=0x0101;//ݶID - graphData.dataFrameHeader.send_ID = sendID;//ߵID - graphData.dataFrameHeader.receiver_ID = receiveID;//ͻ˵IDֻΪ߻˶ӦĿͻ + graphData.CmdID=0x301;//数据帧ID + graphData.dataFrameHeader.data_cmd_id=0x0101;//数据段ID + graphData.dataFrameHeader.send_ID = sendID;//发送者的ID + graphData.dataFrameHeader.receiver_ID = receiveID;//客户端的ID，只能为发送者机器人对应的客户端 graphData.graphData.grapic_data_struct=*data; diff --git a/services/judgement/judge_drive.h b/services/judgement/judge_drive.h index 4679b7e087d3e1ea5c0e53d6ae0efbe49ceca10d..6bdcabd9d8026c77aa88fa233572bbf15b7f2092 100644 --- a/services/judgement/judge_drive.h +++ b/services/judgement/judge_drive.h @@ -3,18 +3,19 @@ #include "stdint.h" #include "stdbool.h" +#include "config.h" #define JUDGE_DATA_ERROR 0 #define JUDGE_DATA_CORRECT 1 -#define LEN_HEADER 5 //֡ͷ -#define LEN_CMDID 2 //볤 -#define LEN_TAIL 2 //֡βCRC16 +#define LEN_HEADER 5 //帧头长 +#define LEN_CMDID 2 //命令码长度 +#define LEN_TAIL 2 //帧尾CRC16 -//ʼֽ,Э̶Ϊ0xA5 +//起始字节,协议固定为0xA5 #define JUDGE_FRAME_HEADER (0xA5) #define JUDGE_MAX_FRAME_LENGTH 128 -//ɫ +//机器人颜色 typedef enum { RobotColor_Red, @@ -28,64 +29,64 @@ typedef enum DATA = 7, }JudgeFrameOffset; -//5ֽ֡ͷ,ƫλ +//5字节帧头,偏移位置 typedef enum { - SOF = 0,//ʼλ - DATA_LENGTH = 1,//֡ݳ,ȡݳ - SEQ = 3,// + SOF = 0,//起始位 + DATA_LENGTH = 1,//帧内数据长度,根据这个来获取数据长度 + SEQ = 3,//包序号 CRC8 = 4 //CRC8 }FrameHeaderOffset; -/***************ID********************/ +/***************命令码ID********************/ /* - ID: 0x0001 Byte: 11 ״̬ Ƶ 1Hz - ID: 0x0002 Byte: 1 - ID: 0x0003 Byte: 32 Ѫ 1Hz - ID: 0x0101 Byte: 4 ¼ ¼ı - ID: 0x0102 Byte: 4 زվʶ ı - ID: 0x0104 Byte: 2 оϢ - ID: 0x0105 Byte: 1 ڷڵʱ - ID: 0X0201 Byte: 27 ״̬ 10Hz - ID: 0X0202 Byte: 14 ʵʱ 50Hz - ID: 0x0203 Byte: 16 λ 10Hz - ID: 0x0204 Byte: 1 ״̬ı - ID: 0x0205 Byte: 1 л״̬ 10Hz - ID: 0x0206 Byte: 1 ˺״̬ ˺ - ID: 0x0207 Byte: 7 ʵʱ ӵ - ID: 0x0208 Byte: 6 ӵʣ෢ - ID: 0x0209 Byte: 4 RFID״̬ - ID: 0x020A Byte: 6 ڻ˿ͻָ - ID: 0x0301 Byte: n ˼佻 ͷ,10Hz + ID: 0x0001 Byte: 11 比赛状态数据 发送频率 1Hz + ID: 0x0002 Byte: 1 比赛结果数据 比赛结束后发送 + ID: 0x0003 Byte: 32 比赛机器人血量数据 1Hz发送 + ID: 0x0101 Byte: 4 场地事件数据 事件改变后发送 + ID: 0x0102 Byte: 4 场地补给站动作标识数据 动作改变后发送 + ID: 0x0104 Byte: 2 裁判警告信息 + ID: 0x0105 Byte: 1 飞镖发射口倒计时 + ID: 0X0201 Byte: 27 机器人状态数据 10Hz + ID: 0X0202 Byte: 14 实时功率热量数据 50Hz + ID: 0x0203 Byte: 16 机器人位置数据 10Hz + ID: 0x0204 Byte: 1 机器人增益数据 增益状态改变后发送 + ID: 0x0205 Byte: 1 空中机器人能量状态数据 10Hz + ID: 0x0206 Byte: 1 伤害状态数据 伤害发生后发送 + ID: 0x0207 Byte: 7 实时射击数据 子弹发射后发送 + ID: 0x0208 Byte: 6 子弹剩余发射数 + ID: 0x0209 Byte: 4 机器人RFID状态 + ID: 0x020A Byte: 6 飞镖机器人客户端指令数据 + ID: 0x0301 Byte: n 机器人间交互数据 发送方触发发送,10Hz */ -//ID,жϽյʲô +//命令码ID,用来判断接收的是什么数据 typedef enum { - ID_game_state = 0x0001,//״̬ - ID_game_result = 0x0002,// - ID_game_robot_HP = 0x0003,//Ѫ - ID_event_data = 0x0101,//¼ - ID_supply_projectile_action = 0x0102,//زվʶ - ID_referee_warning = 0x0104,//оϢ - ID_dart_remaining_time = 0x0105,//ڷڵʱ - ID_game_robot_state = 0x0201,//״̬ - ID_power_heat_data = 0x0202,//ʵʱ - ID_game_robot_pos = 0x0203,//λ - ID_buff_musk = 0x0204,// - ID_aerial_robot_energy = 0x0205,//л״̬ - ID_robot_hurt = 0x0206,//˺״̬ - ID_shoot_data = 0x0207,//ʵʱ - ID_bullet_remaining = 0x0208,//ӵʣ෢ - ID_rfid_status = 0x0209,//RFID״̬ - ID_dart_client_cmd = 0x020A,//ڻ˿ͻָ + ID_game_state = 0x0001,//比赛状态数据 + ID_game_result = 0x0002,//比赛结果数据 + ID_game_robot_HP = 0x0003,//比赛机器人血量数据 + ID_event_data = 0x0101,//场地事件数据 + ID_supply_projectile_action = 0x0102,//场地补给站动作标识数据 + ID_referee_warning = 0x0104,//裁判警告信息 + ID_dart_remaining_time = 0x0105,//飞镖发射口倒计时 + ID_game_robot_state = 0x0201,//机器人状态数据 + ID_power_heat_data = 0x0202,//实时功率热量数据 + ID_game_robot_pos = 0x0203,//机器人位置数据 + ID_buff_musk = 0x0204,//机器人增益数据 + ID_aerial_robot_energy = 0x0205,//空中机器人能量状态数据 + ID_robot_hurt = 0x0206,//伤害状态数据 + ID_shoot_data = 0x0207,//实时射击数据 + ID_bullet_remaining = 0x0208,//子弹剩余发射数 + ID_rfid_status = 0x0209,//机器人RFID状态 + ID_dart_client_cmd = 0x020A,//飞镖机器人客户端指令数据 } CmdID; -//ݶγ,ݹٷЭ峤 +//命令码数据段长,根据官方协议来定义长度 typedef enum { LEN_game_state = 11, //0x0001 @@ -107,8 +108,8 @@ typedef enum LEN_dart_client_cmd = 6, //0x020A } JudgeDataLength; -/* Զ֡ͷ */ -typedef __packed struct +/* 自定义帧头 */ +typedef struct __packed { uint8_t SOF; uint16_t DataLength; @@ -116,8 +117,8 @@ typedef __packed struct uint8_t CRC8; } xFrameHeader; -/* ID: 0x0001 Byte: 11 ״̬ */ -typedef __packed struct +/* ID: 0x0001 Byte: 11 比赛状态数据 */ +typedef struct __packed { uint8_t game_type : 4; uint8_t game_progress : 4; @@ -125,14 +126,14 @@ typedef __packed struct uint64_t SyncTimeStamp; } ext_game_status_t; -/* ID: 0x0002 Byte: 1 */ -typedef __packed struct +/* ID: 0x0002 Byte: 1 比赛结果数据 */ +typedef struct __packed { uint8_t winner; } ext_game_result_t; -/* ID: 0x0003 Byte: 32 Ѫ */ -typedef __packed struct +/* ID: 0x0003 Byte: 32 比赛机器人血量数据 */ +typedef struct __packed { uint16_t red_1_robot_HP; uint16_t red_2_robot_HP; @@ -152,14 +153,14 @@ typedef __packed struct uint16_t blue_base_HP; } ext_game_robot_HP_t; -/* ID: 0x0101 Byte: 4 ¼ */ -typedef __packed struct +/* ID: 0x0101 Byte: 4 场地事件数据 */ +typedef struct __packed { uint32_t event_type; } ext_event_data_t; -/* ID: 0x0102 Byte: 4 زվʶ */ -typedef __packed struct +/* ID: 0x0102 Byte: 4 场地补给站动作标识数据 */ +typedef struct __packed { uint8_t supply_projectile_id; uint8_t supply_robot_id; @@ -167,21 +168,21 @@ typedef __packed struct uint8_t supply_projectile_num; } ext_supply_projectile_action_t; -/* ID: 0x104 Byte: 2 оϢ */ -typedef __packed struct +/* ID: 0x104 Byte: 2 裁判警告信息 */ +typedef struct __packed { uint8_t level; uint8_t foul_robot_id; } ext_referee_warning_t; -/* ID: 0x105 Byte: 1 ڷڵʱ */ -typedef __packed struct +/* ID: 0x105 Byte: 1 飞镖发射口倒计时 */ +typedef struct __packed { uint8_t dart_remaining_time; } ext_dart_remaining_time_t; -/* ID: 0X0201 Byte: 27 ״̬ */ -typedef __packed struct +/* ID: 0X0201 Byte: 27 机器人状态数据 */ +typedef struct __packed { uint8_t robot_id; uint8_t robot_level; @@ -203,8 +204,8 @@ typedef __packed struct } ext_game_robot_status_t; -/* ID: 0X0202 Byte: 16 ʵʱ */ -typedef __packed struct +/* ID: 0X0202 Byte: 16 实时功率热量数据 */ +typedef struct __packed { uint16_t chassis_volt; uint16_t chassis_current; @@ -216,8 +217,8 @@ typedef __packed struct } ext_power_heat_data_t; -/* ID: 0x0203 Byte: 16 λ */ -typedef __packed struct +/* ID: 0x0203 Byte: 16 机器人位置数据 */ +typedef struct __packed { float x; float y; @@ -226,30 +227,30 @@ typedef __packed struct } ext_game_robot_pos_t; -/* ID: 0x0204 Byte: 1 */ -typedef __packed struct +/* ID: 0x0204 Byte: 1 机器人增益数据 */ +typedef struct __packed { uint8_t power_rune_buff; } ext_buff_musk_t; -/* ID: 0x0205 Byte: 1 л״̬ */ -typedef __packed struct +/* ID: 0x0205 Byte: 1 空中机器人能量状态数据 */ +typedef struct __packed { uint8_t attack_time; } aerial_robot_energy_t; -/* ID: 0x0206 Byte: 1 ˺״̬ */ -typedef __packed struct +/* ID: 0x0206 Byte: 1 伤害状态数据 */ +typedef struct __packed { uint8_t armor_id : 4; uint8_t hurt_type : 4; } ext_robot_hurt_t; -/* ID: 0x0207 Byte: 7 ʵʱ */ -typedef __packed struct +/* ID: 0x0207 Byte: 7 实时射击数据 */ +typedef struct __packed { uint8_t bullet_type; uint8_t shooter_id; @@ -257,22 +258,22 @@ typedef __packed struct float bullet_speed; } ext_shoot_data_t; -/* ID: 0x0208 Byte: 6 ӵʣ෢ */ -typedef __packed struct +/* ID: 0x0208 Byte: 6 子弹剩余发射数 */ +typedef struct __packed { uint16_t bullet_remaining_num_17mm; uint16_t bullet_remaining_num_42mm; uint16_t coin_remaining_num; } ext_bullet_remaining_t; -/* ID: 0x0209 Byte: 4 RFID״̬ */ -typedef __packed struct +/* ID: 0x0209 Byte: 4 机器人RFID状态 */ +typedef struct __packed { uint32_t rfid_status; } ext_rfid_status_t; -/* ID: 0x020A Byte: 6 ڻ˿ͻָ */ -typedef __packed struct +/* ID: 0x020A Byte: 6 飞镖机器人客户端指令数据 */ +typedef struct __packed { uint8_t dart_launch_opening_status; uint8_t dart_attack_target; @@ -282,36 +283,36 @@ typedef __packed struct /* - ݣһͳһݶͷṹ - IDԼߵ ID ݶΣ - ݵİܹΪ 128 ֽڣ - ȥ frame_header,cmd_id,frame_tail Լݶͷṹ 6 ֽڣ - ʶ͵ݶΪ 113 - 0x0301 İƵΪ 10Hz - - ID - 1Ӣ() - 2() - 3/4/5() - 6() - 7ڱ() - 11Ӣ() - 12() - 13/14/15() - 16() - 17ڱ() - ͻ ID - 0x0101 ΪӢ۲ֿͻ( ) - 0x0102 ֿ̲ͻ (( ) - 0x0103/0x0104/0x0105ֿͻ() - 0x0106вֿͻ(() - 0x0111Ӣ۲ֿͻ() - 0x0112ֿ̲ͻ() - 0x0113/0x0114/0x0115ֿͻ˲() - 0x0116вֿͻ() + 交互数据，包括一个统一的数据段头结构， + 包含了内容 ID，发送者以及接受者的 ID 和内容数据段， + 整个交互数据的包总共长最大为 128 个字节， + 减去 frame_header,cmd_id,frame_tail 以及数据段头结构的 6 个字节， + 故而发送的内容数据段最大为 113。 + 整个交互数据 0x0301 的包上行频率为 10Hz。 + + 机器人 ID： + 1，英雄(红)； + 2，工程(红)； + 3/4/5，步兵(红)； + 6，空中(红)； + 7，哨兵(红)； + 11，英雄(蓝)； + 12，工程(蓝)； + 13/14/15，步兵(蓝)； + 16，空中(蓝)； + 17，哨兵(蓝)。 + 客户端 ID： + 0x0101 为英雄操作手客户端( 红) ； + 0x0102 ，工程操作手客户端 ((红 )； + 0x0103/0x0104/0x0105，步兵操作手客户端(红)； + 0x0106，空中操作手客户端((红)； + 0x0111，英雄操作手客户端(蓝)； + 0x0112，工程操作手客户端(蓝)； + 0x0113/0x0114/0x0115，操作手客户端步兵(蓝)； + 0x0116，空中操作手客户端(蓝)。 */ -/* ݽϢ0x0301 */ -typedef __packed struct +/* 交互数据接收信息：0x0301 */ +typedef struct __packed { uint16_t data_cmd_id; uint16_t send_ID; @@ -319,32 +320,32 @@ typedef __packed struct } ext_student_interactive_header_data_t; /* - ѧ˼ͨ cmd_id 0x0301 ID:0x0200~0x02FF - ˼ͨţ0x0301 - Ƶʣ 10Hz + 学生机器人间通信 cmd_id 0x0301，内容 ID:0x0200~0x02FF + 交互数据 机器人间通信：0x0301。 + 发送频率：上限 10Hz - ֽƫ С ˵ ע - 0 2 ݵ ID 0x0200~0x02FF - ID ѡȡ ID ɲԶ + 字节偏移量 大小 说明 备注 + 0 2 数据的内容 ID 0x0200~0x02FF + 可以在以上 ID 段选取，具体 ID 含义由参赛队自定义 - 2 2 ߵ ID ҪУ鷢ߵ ID ȷԣ + 2 2 发送者的 ID 需要校验发送者的 ID 正确性， - 4 2 ߵ ID ҪУߵ ID ȷԣ - 粻ܷ͵жԻ˵ID + 4 2 接收者的 ID 需要校验接收者的 ID 正确性， + 例如不能发送到敌对机器人的ID - 6 n ݶ n ҪС 113 + 6 n 数据段 n 需要小于 113 */ -typedef __packed struct +typedef struct __packed { - uint8_t data[100]; //ݶ,nҪС113 + uint8_t data[100]; //数据段,n需要小于113 } robot_interactive_data_t; -/* ͻ ͻԶͼΣcmd_id:0x030 */ +/* 客户端 客户端自定义图形：cmd_id:0x030 */ -//ͼ -typedef __packed struct { +//图形数据 +typedef struct __packed { uint8_t graphic_name[3]; uint32_t operate_tpye:3; uint32_t graphic_tpye:3; @@ -360,76 +361,76 @@ typedef __packed struct { uint32_t end_y:11; } graphic_data_struct_t; -//ɾͼ data_cmd_id=0x0100 -typedef __packed struct +//删除图形 data_cmd_id=0x0100 +typedef struct __packed { uint8_t operate_tpye; uint8_t layer; } ext_client_custom_graphic_delete_t; -//һͼ data_cmd_id=0x0101 -typedef __packed struct +//绘制一个图形 data_cmd_id=0x0101 +typedef struct __packed { graphic_data_struct_t grapic_data_struct; } ext_client_custom_graphic_single_t; -// data_cmd_id=0x0110 -typedef __packed struct { +//文字数据 data_cmd_id=0x0110 +typedef struct __packed { graphic_data_struct_t grapic_data_struct; uint8_t data[30]; } ext_client_custom_character_t; -//˽Ϣ -typedef __packed struct +//机器人交互信息 +typedef struct __packed { - xFrameHeader txFrameHeader;//֡ͷ - uint16_t CmdID;// - ext_student_interactive_header_data_t dataFrameHeader;//ݶͷṹ - robot_interactive_data_t interactData;//ݶ - uint16_t FrameTail;//֡β + xFrameHeader txFrameHeader;//帧头 + uint16_t CmdID;//命令码 + ext_student_interactive_header_data_t dataFrameHeader;//数据段头结构 + robot_interactive_data_t interactData;//数据段 + uint16_t FrameTail;//帧尾 }ext_CommunatianData_t; -//ͻԶͼϢ -typedef __packed struct +//客户端自定义图形信息 +typedef struct __packed { - xFrameHeader txFrameHeader;//֡ͷ - uint16_t CmdID;// - ext_student_interactive_header_data_t dataFrameHeader;//ݶͷṹ - ext_client_custom_graphic_single_t graphData;//ݶ - uint16_t FrameTail;//֡β + xFrameHeader txFrameHeader;//帧头 + uint16_t CmdID;//命令码 + ext_student_interactive_header_data_t dataFrameHeader;//数据段头结构 + ext_client_custom_graphic_single_t graphData;//数据段 + uint16_t FrameTail;//帧尾 }ext_GraphData_t; -//ͻԶϢ -typedef __packed struct +//客户端自定义文字信息 +typedef struct __packed { - xFrameHeader txFrameHeader;//֡ͷ - uint16_t CmdID;// - ext_student_interactive_header_data_t dataFrameHeader;//ݶͷṹ - ext_client_custom_character_t textData;//ݶ - uint16_t FrameTail;//֡β + xFrameHeader txFrameHeader;//帧头 + uint16_t CmdID;//命令码 + ext_student_interactive_header_data_t dataFrameHeader;//数据段头结构 + ext_client_custom_character_t textData;//数据段 + uint16_t FrameTail;//帧尾 }ext_TextData_t; -//ͻԶUIɾ״ -typedef __packed struct +//客户端自定义UI删除形状 +typedef struct __packed { - xFrameHeader txFrameHeader;//֡ͷ - uint16_t CmdID;// - ext_student_interactive_header_data_t dataFrameHeader;//ݶͷṹ - ext_client_custom_graphic_delete_t deleteData;//ݶ - uint16_t FrameTail;//֡β + xFrameHeader txFrameHeader;//帧头 + uint16_t CmdID;//命令码 + ext_student_interactive_header_data_t dataFrameHeader;//数据段头结构 + ext_client_custom_graphic_delete_t deleteData;//数据段 + uint16_t FrameTail;//帧尾 }ext_DeleteData_t; -//ϵͳ֡ +//裁判系统发送数据帧 typedef struct { uint8_t data[JUDGE_MAX_FRAME_LENGTH]; uint16_t frameLength; }JudgeTxFrame; -/*****************ϵͳݶ**********************/ +/*****************系统数据定义**********************/ typedef struct _judge { - xFrameHeader FrameHeader; //֡ͷϢ + xFrameHeader FrameHeader; //帧头信息 ext_game_status_t GameState; //0x0001 ext_game_result_t GameResult; //0x0002 ext_game_robot_HP_t GameRobotHP; //0x0003 @@ -450,7 +451,7 @@ typedef struct _judge }JudgeRecInfo; /****************************************************/ -//ͼβ +//图形操作 typedef enum _GraphOperation { Operation_Null=0, @@ -459,7 +460,7 @@ typedef enum _GraphOperation Operation_Delete }GraphOperation; -//ͼ״ +//图形形状 typedef enum _GraphShape { Shape_Line=0, @@ -472,10 +473,10 @@ typedef enum _GraphShape Shape_Text }GraphShape; -//ͼɫ +//图形颜色 typedef enum _GraphColor { - Color_Self=0,//ɫ + Color_Self=0,//己方主色 Color_Yellow, Color_Green, Color_Orange, diff --git a/services/rc/rc.c b/services/rc/rc.c index ace8a9eb4c36a51d0021c32e0da1d8cacf9ac083..6d4190d9b20d2c21396d0050973b1ac96ca7ff1f 100644 --- a/services/rc/rc.c +++ b/services/rc/rc.c @@ -1,6 +1,8 @@ #include "config.h" #include "softbus.h" #include "cmsis_os.h" +#include +#include #define KEY_NUM 18 //X-MACRO @@ -24,7 +26,7 @@ MOUSE_KEY(left) \ MOUSE_KEY(right) -// +//主键 char* keyType[] = { #define KEY(x) #x, #define MOUSE_KEY(x) #x, @@ -34,66 +36,66 @@ char* keyType[] = { }; -//ңݽṹ +//遥控数据结构体 typedef struct { - //ҡ ȡֵ[-660,660] + //摇杆数据 取值[-660,660] int16_t ch1; int16_t ch2; int16_t ch3; int16_t ch4; - //Ҳ뿪 1/3/2 + //左右拨码开关 上1/中3/下2 uint8_t left; uint8_t right; - //Ϣ + //鼠标信息 struct { - //ƶٶ + //移动速度 int16_t x; int16_t y; int16_t z; - //Ҽ + //左右键 uint8_t l; uint8_t r; } mouse; - //Ϣ + //键盘信息 union { - uint16_t key_code;// + uint16_t key_code;//按键编码 struct { #define KEY(x) uint16_t x : 1; #define MOUSE_KEY(x) KEY_TPYE #undef KEY #undef MOUSE_KEY - } bit;//λӦļλ + } bit;//各个位代表对应的键位 } kb; - // ȡֵ[-660,660] + //拨轮数据 取值[-660,660] int16_t wheel; }RC_TypeDef; -//ṹ壬ڼ/ݼ +//按键结构体，用于计算键盘/鼠标的按键事件 typedef struct{ - //ҪõIJ - uint16_t clickDelayTime;//¶ò㵥һ - uint16_t longPressTime;//¶ò㳤 + //需要配置的参数 + uint16_t clickDelayTime;//按下多久才算单击一次 + uint16_t longPressTime;//按下多久才算长按 - //ʹõIJڶӦЧһ˲Ϊ1 + //用来使用的参数，仅在对应条件有效的一瞬间为1 uint8_t isClicked; uint8_t isLongPressed; uint8_t isUp; uint8_t isPressing; - //м - uint8_t lastState;//1/0Ϊ/ɿ + //中间变量 + uint8_t lastState;//1/0为按下/松开 uint32_t startPressTime; }Key; typedef struct { - RC_TypeDef rcInfo;//ң - Key keyList[KEY_NUM];//б(пṵ̈Ҽ) + RC_TypeDef rcInfo;//遥控器数据 + Key keyList[KEY_NUM];//按键列表(包含所有可用键盘按键和鼠标左右键) uint8_t uartX; }RC; -//ڲ -//ʼжʱ +//内部函数 +//初始化判定时间 void RC_InitKeys(RC* rc); void RC_InitKeyJudgeTime(RC* rc, uint32_t key,uint16_t clickDelay,uint16_t longPressDelay); void RC_PublishData(RC *rc); @@ -101,9 +103,11 @@ void RC_UpdateKeys(RC* rc); void RC_ParseData(RC* rc, uint8_t* buff); void RC_SoftBusCallback(const char* name, SoftBusFrame* frame, void* bindData); +static RC* GlobalRC; // 调试用 + void RC_TaskCallback(void const * argument) { - //ٽ + //进入临界区 portENTER_CRITICAL(); RC rc={0}; rc.uartX = Conf_GetValue((ConfItem*)argument, "uart-x", uint8_t, 0); @@ -112,6 +116,8 @@ void RC_TaskCallback(void const * argument) name[5] = rc.uartX + '0'; Bus_RegisterReceiver(&rc, RC_SoftBusCallback, name); portEXIT_CRITICAL(); + + GlobalRC = &rc; osDelay(2000); while(1) @@ -121,14 +127,14 @@ void RC_TaskCallback(void const * argument) } } -//ʼажʱ +//初始化所有按键的判定时间 void RC_InitKeys(RC* rc) { - //ʼȫ(0x3ffff) + //初始化全部按键(0x3ffff) RC_InitKeyJudgeTime(rc,0x3ffff,50,500); } -//ʼһжʱ(λIDжʱ䣬жʱ) +//初始化一个按键的判定时间(键位ID，单击判定时间，长按判定时间) void RC_InitKeyJudgeTime(RC* rc, uint32_t key,uint16_t clickDelay,uint16_t longPressDelay) { for(uint8_t i=0;i<18;i++) @@ -142,15 +148,15 @@ void RC_InitKeyJudgeTime(RC* rc, uint32_t key,uint16_t clickDelay,uint16_t longP } -//ݸ򷢲rc +//若有数据更新则发布rc数据 void RC_PublishData(RC *rc) { static RC_TypeDef lastData={0}; - if(lastData.ch1 != rc->rcInfo.ch1 || lastData.ch2 != rc->rcInfo.ch2) + if(abs(lastData.ch1) > 5 || abs(lastData.ch2) > 5) Bus_BroadcastSend("/rc/right-stick",{{"x",&rc->rcInfo.ch1},{"y",&rc->rcInfo.ch2}}); - if(lastData.ch3 != rc->rcInfo.ch3 || lastData.ch4 != rc->rcInfo.ch4) + if(abs(lastData.ch3) > 5 || abs(lastData.ch4) > 5) Bus_BroadcastSend("/rc/left-stick",{{"x",&rc->rcInfo.ch3},{"y",&rc->rcInfo.ch4}}); if(lastData.mouse.x != rc->rcInfo.mouse.x || lastData.mouse.y != rc->rcInfo.mouse.y) @@ -165,31 +171,31 @@ void RC_PublishData(RC *rc) if(lastData.wheel != rc->rcInfo.wheel) Bus_BroadcastSend("/rc/wheel",{{"value",&rc->rcInfo.wheel}}); - //°״̬ + //更新按键状态并发布 RC_UpdateKeys(rc); lastData =rc->rcInfo; } -//°״̬ҪʱãΪ14ms +//更新按键状态，需要定时调用，建议间隔为14ms void RC_UpdateKeys(RC* rc) { static uint32_t lastUpdateTime; - uint32_t presentTime=osKernelSysTick();//ȡϵͳʱ + uint32_t presentTime=osKernelSysTick();//获取系统时间戳 - //ϼ + //检查组合键 char* combineKey="none"; if(rc->rcInfo.kb.bit.Ctrl) combineKey="ctrl"; else if(rc->rcInfo.kb.bit.Shift) combineKey="shift"; for(uint8_t key=0;key<18;key++) { - //ȡ״̬ + //读取按键状态 uint8_t thisState=0; if(key==4||key==5) continue; char name[22] = "/rc/key/"; if(key<16) { - thisState=(rc->rcInfo.kb.key_code&(0x01<rcInfo.kb.key_code&(0x01<rcInfo.mouse.r; } - uint16_t lastPressTime=lastUpdateTime-rc->keyList[key].startPressTime;//ϴθʱµʱ - uint16_t pressTime=presentTime-rc->keyList[key].startPressTime;//ǰµʱ + uint16_t lastPressTime=lastUpdateTime-rc->keyList[key].startPressTime;//上次更新时按下的时间 + uint16_t pressTime=presentTime-rc->keyList[key].startPressTime;//当前按下的时间 - //״̬ж - /*******µһ˲********/ + //按键状态判定 + /*******按下的一瞬间********/ if(!rc->keyList[key].lastState && thisState) { - rc->keyList[key].startPressTime=presentTime;//¼ʱ + rc->keyList[key].startPressTime=presentTime;//记录按下时间 rc->keyList[key].isPressing=1; - //topic + //发布topic memcpy(name+8, "on-down", 8); Bus_BroadcastSend(name, { {"key", keyType[key]}, {"combine-key", combineKey} }); } - /*******ɿһ˲********/ + /*******松开的一瞬间********/ else if(rc->keyList[key].lastState && !thisState) { rc->keyList[key].isLongPressed=0; rc->keyList[key].isPressing=0; - //̧ + //按键抬起 rc->keyList[key].isUp=1; - //topic + //发布topic memcpy(name+8, "on-up", 6); Bus_BroadcastSend(name, { {"key", keyType[key]}, {"combine-key", combineKey} }); - //ж + //单击判定 if(pressTime>rc->keyList[key].clickDelayTime && pressTimekeyList[key].longPressTime) { rc->keyList[key].isClicked=1; - //topic + //发布topic memcpy(name+8, "on-click", 9); Bus_BroadcastSend(name, { {"key", keyType[key]}, @@ -243,22 +249,22 @@ void RC_UpdateKeys(RC* rc) }); } } - /***************/ + /*******按键持续按下********/ else if(rc->keyList[key].lastState && thisState) { - //ִһֱµ¼ص - //topic + //执行一直按下的事件回调 + //发布topic memcpy(name+8, "on-pressing", 12); Bus_BroadcastSend(name, { {"key", keyType[key]}, {"combine-key", combineKey} }); - //ж + //长按判定 if(lastPressTime<=rc->keyList[key].longPressTime && pressTime>rc->keyList[key].longPressTime) { rc->keyList[key].isLongPressed=1; - //topic + //发布topic memcpy(name+8, "on-long-press", 14); Bus_BroadcastSend(name, { {"key", keyType[key]}, @@ -267,7 +273,7 @@ void RC_UpdateKeys(RC* rc) } else rc->keyList[key].isLongPressed=0; } - /*******ɿ********/ + /*******按键持续松开********/ else { rc->keyList[key].isClicked=0; @@ -275,10 +281,10 @@ void RC_UpdateKeys(RC* rc) rc->keyList[key].isUp=0; } - rc->keyList[key].lastState=thisState;//¼״̬ + rc->keyList[key].lastState=thisState;//记录按键状态 } - lastUpdateTime=presentTime;//¼¼ + lastUpdateTime=presentTime;//记录更新事件 } @@ -287,11 +293,12 @@ void RC_SoftBusCallback(const char* name, SoftBusFrame* frame, void* bindData) RC* rc = (RC*)bindData; uint8_t* data = (uint8_t*)Bus_GetListValue(frame, 0); - if(data) + uint16_t* len = (uint16_t*)Bus_GetListValue(frame, 1); + if(data && len) RC_ParseData(rc, data); } -// +//解析串口数据 void RC_ParseData(RC* rc, uint8_t* buff) { rc->rcInfo.ch1 = (buff[0] | buff[1] << 8) & 0x07FF; diff --git a/services/shoot/shoot.c b/services/shoot/shoot.c index 4d32cf334a8fafb445f8d9d32503e55d1de07c97..4916c511615dd53c6944d74faa6b49e34f95d84a 100644 --- a/services/shoot/shoot.c +++ b/services/shoot/shoot.c @@ -15,9 +15,9 @@ typedef struct { Motor *fricMotors[2],*triggerMotor; bool fricEnable; - float fricSpeed; //Ħٶ - float triggerAngle,targetTrigAngle; //һνǶȼۼƽǶ - uint16_t intervalTime; //ms + float fricSpeed; //摩擦轮速度 + float triggerAngle,targetTrigAngle; //拨弹一次角度及累计角度 + uint16_t intervalTime; //连发间隔ms uint8_t mode; uint16_t taskInterval; @@ -35,7 +35,7 @@ void Shoot_StopCallback(const char* name, SoftBusFrame* frame, void* bindData); void Shooter_TaskCallback(void const * argument) { portENTER_CRITICAL(); - float totalAngle; //ΪwarningCбǩһ䲻ǶıʽcaseǡþDZǩ + float totalAngle; //为解决warning，C语言中标签的下一条语句不能是定义变量的表达式，而case恰好就是标签 Shooter shooter={0}; Shooter_Init(&shooter, (ConfItem*)argument); portEXIT_CRITICAL(); @@ -49,29 +49,29 @@ void Shooter_TaskCallback(void const * argument) osDelay(shooter.taskInterval); break; case SHOOTER_MODE_BLOCK: - totalAngle = shooter.triggerMotor->getData(shooter.triggerMotor, "totalAngle"); //õǶΪǰۼƽǶ - shooter.targetTrigAngle = totalAngle - shooter.triggerAngle*0.5f; //ת + totalAngle = shooter.triggerMotor->getData(shooter.triggerMotor, "totalAngle"); //重置电机角度为当前累计角度 + shooter.targetTrigAngle = totalAngle - shooter.triggerAngle*0.5f; //反转 shooter.triggerMotor->setTarget(shooter.triggerMotor,shooter.targetTrigAngle); - osDelay(500); //ȴתָ + osDelay(500); //等待电机堵转恢复 shooter.mode = SHOOTER_MODE_IDLE; break; - case SHOOTER_MODE_ONCE: // - if(shooter.fricEnable == false) //Ħδȿ + case SHOOTER_MODE_ONCE: //单发 + if(shooter.fricEnable == false) //若摩擦轮未开启则先开启 { - Bus_RemoteCall("/shooter/setting",{"fric-enable",IM_PTR(bool,true)}); - osDelay(200); //ȴĦתȶ + Bus_RemoteCall("/shooter/setting",{{"fric-enable",{.Bool = true}}}); + osDelay(200); //等待摩擦轮转速稳定 } shooter.targetTrigAngle += shooter.triggerAngle; shooter.triggerMotor->setTarget(shooter.triggerMotor,shooter.targetTrigAngle); shooter.mode = SHOOTER_MODE_IDLE; break; - case SHOOTER_MODE_CONTINUE: //һʱ - if(shooter.fricEnable == false) //Ħδȿ + case SHOOTER_MODE_CONTINUE: //以一定的时间间隔连续发射 + if(shooter.fricEnable == false) //若摩擦轮未开启则先开启 { - Bus_RemoteCall("/shooter/setting",{"fric-enable",IM_PTR(bool,true)}); - osDelay(200); //ȴĦתȶ + Bus_RemoteCall("/shooter/setting",{{"fric-enable",{.Bool = true}}}); + osDelay(200); //等待摩擦轮转速稳定 } - shooter.targetTrigAngle += shooter.triggerAngle; //ӲĿǶ + shooter.targetTrigAngle += shooter.triggerAngle; //增加拨弹电机目标角度 shooter.triggerMotor->setTarget(shooter.triggerMotor,shooter.targetTrigAngle); osDelay(shooter.intervalTime); break; @@ -83,17 +83,17 @@ void Shooter_TaskCallback(void const * argument) void Shooter_Init(Shooter* shooter, ConfItem* dict) { - // + //任务间隔 shooter->taskInterval = Conf_GetValue(dict, "task-interval", uint16_t, 20); - //ʼ + //初始弹速 shooter->fricSpeed = Conf_GetValue(dict,"fric-speed",float,5450); - //ֲһת + //拨弹轮拨出一发弹丸转角 shooter->triggerAngle = Conf_GetValue(dict,"trigger-angle",float,1/7.0*360); - //ʼ + //发射机构电机初始化 shooter->fricMotors[0] = Motor_Init(Conf_GetPtr(dict, "fric-motor-left", ConfItem)); shooter->fricMotors[1] = Motor_Init(Conf_GetPtr(dict, "fric-motor-right", ConfItem)); shooter->triggerMotor = Motor_Init(Conf_GetPtr(dict, "trigger-motor", ConfItem)); - //÷ģʽ + //设置发射机构电机模式 for(uint8_t i = 0; i<2; i++) { shooter->fricMotors[i]->changeMode(shooter->fricMotors[i], MOTOR_SPEED_MODE); @@ -103,26 +103,26 @@ void Shooter_Init(Shooter* shooter, ConfItem* dict) char* temp = Conf_GetPtr(dict, "name", char); temp = temp ? temp : "shooter"; uint8_t len = strlen(temp); - shooter->settingName = pvPortMalloc(len + 9+ 1); //9Ϊ"/ /setting"ijȣ1Ϊ'\0'ij + shooter->settingName = pvPortMalloc(len + 9+ 1); //9为"/ /setting"的长度，1为'\0'的长度 sprintf(shooter->settingName, "/%s/setting", temp); - shooter->changeModeName = pvPortMalloc(len + 6+ 1); //6Ϊ"/ /mode"ijȣ1Ϊ'\0'ij + shooter->changeModeName = pvPortMalloc(len + 6+ 1); //6为"/ /mode"的长度，1为'\0'的长度 sprintf(shooter->changeModeName, "/%s/mode", temp); temp = Conf_GetPtr(dict, "trigger-motor/name", char); temp = temp ? temp : "trigger-motor"; len = strlen(temp); - shooter->triggerStallName = pvPortMalloc(len + 7+ 1); //7Ϊ"/ /stall"ijȣ1Ϊ'\0'ij + shooter->triggerStallName = pvPortMalloc(len + 7+ 1); //7为"/ /stall"的长度，1为'\0'的长度 sprintf(shooter->triggerStallName, "/%s/stall", temp); - //עص + //注册回调函数 Bus_RegisterRemoteFunc(shooter,Shooter_SettingCallback, shooter->settingName); Bus_RegisterRemoteFunc(shooter,Shoot_ChangeModeCallback, shooter->changeModeName); Bus_RegisterReceiver(shooter,Shoot_StopCallback,"/system/stop"); Bus_RegisterReceiver(shooter,Shooter_BlockCallback, shooter->triggerStallName); } -//ģʽ +//射击模式 bool Shooter_SettingCallback(const char* name, SoftBusFrame* frame, void* bindData) { Shooter *shooter = (Shooter*)bindData ; @@ -158,7 +158,7 @@ bool Shoot_ChangeModeCallback(const char* name, SoftBusFrame* frame, void* bindD if(Bus_IsMapKeyExist(frame,"mode")) { char* mode = (char*)Bus_GetMapValue(frame,"mode"); - if(!strcmp(mode,"once") && shooter->mode == SHOOTER_MODE_IDLE) //ʱ޸ģʽ + if(!strcmp(mode,"once") && shooter->mode == SHOOTER_MODE_IDLE) //空闲时才允许修改模式 { shooter->mode = SHOOTER_MODE_ONCE; return true; @@ -180,13 +180,13 @@ bool Shoot_ChangeModeCallback(const char* name, SoftBusFrame* frame, void* bindD return false; } -//ת +//堵转 void Shooter_BlockCallback(const char* name, SoftBusFrame* frame, void* bindData) { Shooter *shooter = (Shooter*)bindData; shooter->mode = SHOOTER_MODE_BLOCK; } -//ͣ +//急停 void Shoot_StopCallback(const char* name, SoftBusFrame* frame, void* bindData) { Shooter *shooter = (Shooter*)bindData; diff --git a/softbus/CMakeLists.txt b/softbus/CMakeLists.txt new file mode 100644 index 0000000000000000000000000000000000000000..732ff6e231cc121f8eda8a9bf805f0a53ce9c8f2 --- /dev/null +++ b/softbus/CMakeLists.txt @@ -0,0 +1,7 @@ + +# 软总线源码列表 +file(GLOB SOFTBUS_SRC *.c) +set(SOFTBUS_MODULE_SRC ${SOFTBUS_SRC} PARENT_SCOPE) + +# 添加包含目录 +set(SOFTBUS_MODULE_INCL ${CMAKE_CURRENT_LIST_DIR} PARENT_SCOPE) diff --git a/softbus/README.md b/softbus/README.md index b4abd83ba90273832172fb53dcb2ef62da13974f..e0b86e526f4e91525719917c08e9cbaf7da5e18e 100644 --- a/softbus/README.md +++ b/softbus/README.md @@ -184,10 +184,10 @@ float value2 = 1.0f; //要传入的第二个参数 float result1; //用于接收远程函数第一个返回值 uint8_t result2; //用于接收远程函数第二个返回值 Bus_RemoteCall("fun1", { - {"arg1", &value1}, - {"arg2", &value2}, - {"ret1", &result1}, - {"ret2", &result2} + {"arg1", {.U8 = &value1}}, + {"arg2", {.F32 = &value2}}, + {"ret1", {&result1}}, + {"ret2", {&result2}} }); //调用远程函数fun1 ``` diff --git a/softbus/softbus.c b/softbus/softbus.c index aed779048c6889161e15c126ff8502f3f8597fd2..fe2d6c56907c4f0e6b3374245fb11237aa2a8dc1 100644 --- a/softbus/softbus.c +++ b/softbus/softbus.c @@ -15,35 +15,35 @@ typedef struct{ void* bindData; void* callback; -}CallbackNode;//صڵ +}CallbackNode;//回调函数节点 typedef struct{ char* name; Vector callbackNodes; -}ReceiverNode;//receiverڵ +}ReceiverNode;//receiver节点 typedef struct { char* name; CallbackNode callbackNode; -}RemoteNode;//remoteڵ +}RemoteNode;//remote节点 typedef struct{ uint32_t hash; Vector receiverNodes; Vector remoteNodes; -}HashNode;//hashڵ +}HashNode;//hash节点 -int8_t Bus_Init(void);//ʼ,0:ɹ -1:ʧ -uint32_t SoftBus_Str2Hash_8(const char* str);//8λhashַС20ַʱʹ -uint32_t SoftBus_Str2Hash_32(const char* str);//32λhashַС20ַʱʹ -void _Bus_BroadcastSend(const char* name, SoftBusFrame* frame);//Ϣ -bool _Bus_RemoteCall(const char* name, SoftBusFrame* frame);// -void Bus_EmptyBroadcastReceiver(const char* name, SoftBusFrame* frame, void* bindData);//ջص -bool Bus_EmptyRemoteFunction(const char* name, SoftBusFrame* frame, void* bindData);//ջص +int8_t Bus_Init(void);//初始化软总线,返回0:成功 -1:失败 +uint32_t SoftBus_Str2Hash_8(const char* str);//8位处理的hash函数，在字符串长度小于20个字符时使用 +uint32_t SoftBus_Str2Hash_32(const char* str);//32位处理的hash函数，在字符串长度小于20个字符时使用 +void _Bus_BroadcastSend(const char* name, SoftBusFrame* frame);//发布消息 +bool _Bus_RemoteCall(const char* name, SoftBusFrame* frame);//请求服务 +void Bus_EmptyBroadcastReceiver(const char* name, SoftBusFrame* frame, void* bindData);//空回调函数 +bool Bus_EmptyRemoteFunction(const char* name, SoftBusFrame* frame, void* bindData);//空回调函数 Vector hashList={0}; -//ʼhash +//初始化hash树 int8_t Bus_Init() { return Vector_Init(hashList,HashNode); @@ -53,38 +53,38 @@ int8_t Bus_RegisterReceiver(void* bindData, SoftBusBroadcastReceiver callback, c { if(!name || !callback) return -2; - if(hashList.data == NULL)//δʼʼ + if(hashList.data == NULL)//如果软总线未初始化则初始化软总线 { if(Bus_Init()) return -1; } - uint32_t hash = SoftBus_Str2Hash(name);//ַhashֵ - Vector_ForEach(hashList, hashNode, HashNode)//hashڵ + uint32_t hash = SoftBus_Str2Hash(name);//计算字符串hash值 + Vector_ForEach(hashList, hashNode, HashNode)//遍历所有hash节点 { if(hash == hashNode->hash) { - Vector_ForEach(hashNode->receiverNodes, receiverNode, ReceiverNode)//hashڵreceiver + Vector_ForEach(hashNode->receiverNodes, receiverNode, ReceiverNode)//遍历该hash节点下所有receiver { - if(strcmp(name, receiverNode->name) == 0)//ƥ䵽receiverעص + if(strcmp(name, receiverNode->name) == 0)//匹配到已有receiver注册回调函数 { - if(Vector_GetFront(receiverNode->callbackNodes, CallbackNode)->callback == Bus_EmptyBroadcastReceiver)//receiverпջص + if(Vector_GetFront(receiverNode->callbackNodes, CallbackNode)->callback == Bus_EmptyBroadcastReceiver)//如果该receiver下有空回调函数 { CallbackNode* callbackNode = Vector_GetFront(receiverNode->callbackNodes, CallbackNode); - callbackNode->bindData = bindData;//° - callbackNode->callback = callback;//»ص + callbackNode->bindData = bindData;//更新绑定数据 + callbackNode->callback = callback;//更新回调函数 return 0; } return Vector_PushBack(receiverNode->callbackNodes, ((CallbackNode){bindData, callback})); } } - Vector callbackNodes = Vector_Create(CallbackNode);//δƥ䵽receiverhashͻڸhashڵ㴦һreceiverڵhashͻ + Vector callbackNodes = Vector_Create(CallbackNode);//未匹配到receiver产生hash冲突，在该hash节点处添加一个receiver节点解决hash冲突 Vector_PushBack(callbackNodes, ((CallbackNode){bindData, callback})); - char* nameCpy = SOFTBUS_MALLOC_PORT(SOFTBUS_STRLEN_PORT(name)+1);//ֹnameǾֲռ䱣浽hash + char* nameCpy = SOFTBUS_MALLOC_PORT(SOFTBUS_STRLEN_PORT(name)+1);//防止name是局部变量，分配空间保存到hash树中 SOFTBUS_MEMCPY_PORT(nameCpy, name, SOFTBUS_STRLEN_PORT(name)+1); return Vector_PushBack(hashNode->receiverNodes,((ReceiverNode){nameCpy, callbackNodes})); } } - Vector callbackNodes = Vector_Create(CallbackNode);//µhashڵ + Vector callbackNodes = Vector_Create(CallbackNode);//新的hash节点 Vector_PushBack(callbackNodes, ((CallbackNode){bindData, callback})); Vector receiverV = Vector_Create(ReceiverNode); Vector remoteV = Vector_Create(RemoteNode); @@ -101,7 +101,7 @@ int8_t _Bus_MultiRegisterReceiver(void* bindData, SoftBusBroadcastReceiver callb return -2; for (uint16_t i = 0; i < namesNum; i++) { - uint8_t retval = Bus_RegisterReceiver(bindData, callback, names[i]); //Ļ + uint8_t retval = Bus_RegisterReceiver(bindData, callback, names[i]); //逐个订阅话题 if(retval) return retval; } @@ -132,13 +132,13 @@ void _Bus_BroadcastSend(const char* name, SoftBusFrame* frame) if(!hashList.data ||!name || !frame) return; uint32_t hash = SoftBus_Str2Hash(name); - Vector_ForEach(hashList, hashNode, HashNode)//hashڵ + Vector_ForEach(hashList, hashNode, HashNode)//遍历所有hash节点 { - if(hash == hashNode->hash)//ƥ䵽hashֵ + if(hash == hashNode->hash)//匹配到hash值 { - Vector_ForEach(hashNode->receiverNodes, receiverNode, ReceiverNode)//hashڵreceiver + Vector_ForEach(hashNode->receiverNodes, receiverNode, ReceiverNode)//遍历改hash节点的所有receiver { - if(strcmp(name, receiverNode->name) == 0)//ƥ䵽receiver׳receiverлص + if(strcmp(name, receiverNode->name) == 0)//匹配到receiver，抛出该receiver所有回调函数 { Vector_ForEach(receiverNode->callbackNodes, callbackNode, CallbackNode) { @@ -152,7 +152,7 @@ void _Bus_BroadcastSend(const char* name, SoftBusFrame* frame) } } -void _Bus_BroadcastSendMap(const char* name, uint16_t itemNum, SoftBusItem* items) +void _Bus_BroadcastSendMap(const char* name, uint16_t itemNum, SoftBusItemX* items) { if(!hashList.data ||!name || !itemNum || !items) return; @@ -166,7 +166,7 @@ void _Bus_BroadcastSendList(SoftBusReceiverHandle receiverHandle, uint16_t listN return; ReceiverNode* receiverNode = (ReceiverNode*)receiverHandle; SoftBusFrame frame = {list, listNum}; - Vector_ForEach(receiverNode->callbackNodes, callbackNode, CallbackNode)//׳ÿپлص + Vector_ForEach(receiverNode->callbackNodes, callbackNode, CallbackNode)//抛出该快速句柄下所有回调函数 { (*((SoftBusBroadcastReceiver)callbackNode->callback))(receiverNode->name, &frame, callbackNode->bindData); } @@ -176,58 +176,58 @@ SoftBusReceiverHandle Bus_CreateReceiverHandle(const char* name) { if(!name) return NULL; - uint32_t hash = SoftBus_Str2Hash(name);//ַhashֵ - Vector_ForEach(hashList, hashNode, HashNode)//hashڵ + uint32_t hash = SoftBus_Str2Hash(name);//计算字符串hash值 + Vector_ForEach(hashList, hashNode, HashNode)//遍历所有hash节点 { if(hash == hashNode->hash) { - Vector_ForEach(hashNode->receiverNodes, receiverNode, ReceiverNode)//hashڵreceiver + Vector_ForEach(hashNode->receiverNodes, receiverNode, ReceiverNode)//遍历该hash节点下所有receiver { - if(strcmp(name, receiverNode->name) == 0)//ƥ䵽receiverעص + if(strcmp(name, receiverNode->name) == 0)//匹配到已有receiver注册回调函数 { return receiverNode; } } } } - Bus_RegisterReceiver(NULL, Bus_EmptyBroadcastReceiver, name);//δƥ䵽receiver,עһջص - return Bus_CreateReceiverHandle(name);//ݹ + Bus_RegisterReceiver(NULL, Bus_EmptyBroadcastReceiver, name);//未匹配到receiver,注册一个空回调函数 + return Bus_CreateReceiverHandle(name);//递归调用 } int8_t Bus_RegisterRemoteFunc(void* bindData, SoftBusRemoteFunction callback, const char* name) { if(!name || !callback) return -2; - if(hashList.data == NULL)//δʼʼ + if(hashList.data == NULL)//如果软总线未初始化则初始化软总线 { if(Bus_Init()) return -1; } - uint32_t hash = SoftBus_Str2Hash(name);//ַhashֵ - Vector_ForEach(hashList, hashNode, HashNode)//hashڵ + uint32_t hash = SoftBus_Str2Hash(name);//计算字符串hash值 + Vector_ForEach(hashList, hashNode, HashNode)//遍历所有hash节点 { if(hash == hashNode->hash) { - Vector_ForEach(hashNode->remoteNodes, remoteNode, RemoteNode)//hashڵremote + Vector_ForEach(hashNode->remoteNodes, remoteNode, RemoteNode)//遍历该hash节点下所有remote { - if(strcmp(name, remoteNode->name) == 0)//ƥ䵽remoteעص + if(strcmp(name, remoteNode->name) == 0)//匹配到已有remote注册回调函数 { - if(remoteNode->callbackNode.callback == Bus_EmptyRemoteFunction)//remoteΪջص + if(remoteNode->callbackNode.callback == Bus_EmptyRemoteFunction)//如果该remote下为空回调函数 { - remoteNode->callbackNode = ((CallbackNode){bindData, callback});//»ص + remoteNode->callbackNode = ((CallbackNode){bindData, callback});//更新回调函数 return 0; } - return -3; //÷עһж + return -3; //该服务已注册过服务器，不允许一个服务有多个服务器 } } - CallbackNode callbackNode = {bindData, callback};//δƥ䵽remotehashͻڸhashڵ㴦һremoteڵhashͻ + CallbackNode callbackNode = {bindData, callback};//未匹配到remote产生hash冲突，在该hash节点处添加一个remote节点解决hash冲突 char* remoteCpy = SOFTBUS_MALLOC_PORT(SOFTBUS_STRLEN_PORT(name)+1); SOFTBUS_MEMCPY_PORT(remoteCpy, name, SOFTBUS_STRLEN_PORT(name)+1); return Vector_PushBack(hashNode->remoteNodes,((RemoteNode){remoteCpy, callbackNode})); } } - Vector remoteV = Vector_Create(RemoteNode);//µhashڵ + Vector remoteV = Vector_Create(RemoteNode);//新的hash节点 char* nameCpy = SOFTBUS_MALLOC_PORT(SOFTBUS_STRLEN_PORT(name)+1); SOFTBUS_MEMCPY_PORT(nameCpy, name, SOFTBUS_STRLEN_PORT(name)+1); Vector callbackNodes = Vector_Create(CallbackNode); @@ -243,13 +243,13 @@ bool _Bus_RemoteCall(const char* name, SoftBusFrame* frame) if(!hashList.data ||!name || !frame) return false; uint32_t hash = SoftBus_Str2Hash(name); - Vector_ForEach(hashList, hashNode, HashNode)//hashڵ + Vector_ForEach(hashList, hashNode, HashNode)//遍历所有hash节点 { - if(hash == hashNode->hash)//ƥ䵽hashֵ + if(hash == hashNode->hash)//匹配到hash值 { - Vector_ForEach(hashNode->remoteNodes, remoteNode, RemoteNode)//hashڵremote + Vector_ForEach(hashNode->remoteNodes, remoteNode, RemoteNode)//遍历改hash节点的所有remote { - if(strcmp(name, remoteNode->name) == 0)//ƥ䵽remote׳remoteĻص + if(strcmp(name, remoteNode->name) == 0)//匹配到remote，抛出该remote的回调函数 { CallbackNode callbackNode = remoteNode->callbackNode; return (*((SoftBusRemoteFunction)callbackNode.callback))(name, frame, callbackNode.bindData); @@ -261,7 +261,7 @@ bool _Bus_RemoteCall(const char* name, SoftBusFrame* frame) return false; } -bool _Bus_RemoteCallMap(const char* name, uint16_t itemNum, SoftBusItem* items) +bool _Bus_RemoteCallMap(const char* name, uint16_t itemNum, SoftBusItemX* items) { if(!hashList.data ||!name || !itemNum || !items) return false; @@ -286,7 +286,7 @@ const SoftBusItem* Bus_GetMapItem(SoftBusFrame* frame, char* key) for(uint16_t i = 0; i < frame->size; ++i) { SoftBusItem* item = (SoftBusItem*)frame->data + i; - if(strcmp(key, item->key) == 0)//keyֵ֡Ӧֶƥ򷵻 + if(strcmp(key, item->key) == 0)//如果key值与数据帧中相应的字段匹配上则返回它 return item; } return NULL; diff --git a/softbus/softbus.h b/softbus/softbus.h index 9356cb69ddb283800bc8ce825d15c976076cdda5..8ff6d84b9922f4a320cb7ab63c5df45e08641b6a 100644 --- a/softbus/softbus.h +++ b/softbus/softbus.h @@ -7,136 +7,153 @@ typedef struct{ void* data; uint16_t size; -}SoftBusFrame;//֡ +}SoftBusFrame;//数据帧 typedef struct{ char* key; void* data; -}SoftBusItem;//ֶ +}SoftBusItem;//数据字段 + +typedef struct _SoftBusItemX { + const char* key; + union { + void* Ptr; + const char* Str; + uint32_t U32; + uint16_t U16; + uint8_t U8; + int32_t I32; + int16_t I16; + int8_t I8; + float F32; + _Bool Bool; + struct _SoftBusItemX* Child; + }; // 字长以内的数据均可以直接指定 +} SoftBusItemX; #ifndef IM_PTR -#define IM_PTR(type,...) (&(type){__VA_ARGS__}) //ȡĵַ +#define IM_PTR(type,...) (&(type){__VA_ARGS__}) //取立即数的地址 #endif -typedef void* SoftBusReceiverHandle;//߿پ -typedef void (*SoftBusBroadcastReceiver)(const char* name, SoftBusFrame* frame, void* bindData);//㲥صָ -typedef bool (*SoftBusRemoteFunction)(const char* name, SoftBusFrame* frame, void* bindData);//Զ̺صָ +typedef void* SoftBusReceiverHandle;//软总线快速句柄 +typedef void (*SoftBusBroadcastReceiver)(const char* name, SoftBusFrame* frame, void* bindData);//广播回调函数指针 +typedef bool (*SoftBusRemoteFunction)(const char* name, SoftBusFrame* frame, void* bindData);//远程函数回调函数指针 -//(ֱӵãӦʹ·defineĽӿ) +//操作函数声明(不直接调用，应使用下方define定义的接口) int8_t _Bus_MultiRegisterReceiver(void* bindData, SoftBusBroadcastReceiver callback, uint16_t namesNum, char** names); -void _Bus_BroadcastSendMap(const char* name, uint16_t itemNum, SoftBusItem* items); +void _Bus_BroadcastSendMap(const char* name, uint16_t itemNum, SoftBusItemX* items); void _Bus_BroadcastSendList(SoftBusReceiverHandle receiverHandle, uint16_t listNum, void** list); -bool _Bus_RemoteCallMap(const char* name, uint16_t itemNum, SoftBusItem* items); +bool _Bus_RemoteCallMap(const char* name, uint16_t itemNum, SoftBusItemX* items); uint8_t _Bus_CheckMapKeys(SoftBusFrame* frame, uint16_t keysNum, char** keys); /* - @brief ϵһ㲥 - @param callback:㲥ʱĻص - @param name:㲥 - @retval 0:ɹ -1:ѿռ䲻 -2:Ϊ - @note صʽӦΪvoid callback(const char* name, SoftBusFrame* frame, void* bindData) + @brief 订阅软总线上的一个广播 + @param callback:广播发布时的回调函数 + @param name:广播名 + @retval 0:成功 -1:堆空间不足 -2:参数为空 + @note 回调函数的形式应为void callback(const char* name, SoftBusFrame* frame, void* bindData) */ int8_t Bus_RegisterReceiver(void* bindData, SoftBusBroadcastReceiver callback, const char* name); /* - @brief ϵĶ㲥 - @param bindData: - @param callback:㲥ʱĻص - @param ...:㲥ַб - @retval 0:ɹ -1:ѿռ䲻 -2:Ϊ + @brief 订阅软总线上的多个广播 + @param bindData:绑定数据 + @param callback:广播发布时的回调函数 + @param ...:广播字符串列表 + @retval 0:成功 -1:堆空间不足 -2:参数为空 @example Bus_MultiRegisterReceiver(NULL, callback, {"name1", "name2"}); */ #define Bus_MultiRegisterReceiver(bindData, callback,...) _Bus_MultiRegisterReceiver((bindData),(callback),(sizeof((char*[])__VA_ARGS__)/sizeof(char*)),((char*[])__VA_ARGS__)) /* - @brief 㲥ӳ֡ - @param name:㲥 - @param ...:ӳ + @brief 广播映射表数据帧 + @param name:广播名 + @param ...:映射表 @retval void @example Bus_BroadcastSend("name", {{"key1", data1}, {"key2", data2}}); */ -#define Bus_BroadcastSend(name,...) _Bus_BroadcastSendMap((name),(sizeof((SoftBusItem[])__VA_ARGS__)/sizeof(SoftBusItem)),((SoftBusItem[])__VA_ARGS__)) +#define Bus_BroadcastSend(name,...) _Bus_BroadcastSendMap((name),(sizeof((SoftBusItemX[])__VA_ARGS__)/sizeof(SoftBusItemX)),((SoftBusItemX[])__VA_ARGS__)) /* - @brief ͨپ㲥б֡ - @param handle:پ - @param ...:ָб + @brief 通过快速句柄广播列表数据帧 + @param handle:快速句柄 + @param ...:数据指针列表 @retval void @example float value1,value2; Bus_FastBroadcastSend(handle, {&value1, &value2}); */ #define Bus_FastBroadcastSend(handle,...) _Bus_BroadcastSendList((handle),(sizeof((void*[])__VA_ARGS__)/sizeof(void*)),((void*[])__VA_ARGS__)) /* - @brief ϵһԶ̺ - @param callback:ӦԶ̺ʱĻص - @param name:Զ̺ - @retval 0:ɹ -1:ѿռ䲻 -2:Ϊ -3:Զ̺Ѵ - @note صʽӦΪbool callback(const char* name, SoftBusFrame* frame, void* bindData) + @brief 创建软总线上的一个远程函数 + @param callback:响应远程函数时的回调函数 + @param name:远程函数名 + @retval 0:成功 -1:堆空间不足 -2:参数为空 -3:远程函数已存在 + @note 回调函数的形式应为bool callback(const char* name, SoftBusFrame* frame, void* bindData) */ int8_t Bus_RegisterRemoteFunc(void* bindData, SoftBusRemoteFunction callback, const char* name); /* - @brief ͨӳ֡Զ̺ - @param name:Զ̺ - @param ...:Զ̺б(ͷֵ) - @retval true:ɹ false:ʧ - @example Bus_RemoteCall("name", {{"key1", data1}, {"key2", data2}}); + @brief 通过映射表数据帧调用远程函数 + @param name:远程函数名 + @param ...:远程函数参数列表(包含参数和返回值) + @retval true:成功 false:失败 + @example Bus_RemoteCall("name", {{"key1", {data1}}, {"key2", {data2}}}); */ -#define Bus_RemoteCall(name,...) _Bus_RemoteCallMap((name),(sizeof((SoftBusItem[])__VA_ARGS__)/sizeof(SoftBusItem)),((SoftBusItem[])__VA_ARGS__)) +#define Bus_RemoteCall(name,...) _Bus_RemoteCallMap((name),(sizeof((SoftBusItemX[])__VA_ARGS__)/sizeof(SoftBusItemX)),((SoftBusItemX[])__VA_ARGS__)) /* - @brief ӳ֡еֶ - @param frame:ָ֡ - @param key:ֶε - @retval ֶָָεconstָ,ѯkey򷵻NULL - @note ӦԷص֡޸ + @brief 查找映射表数据帧中的数据字段 + @param frame:数据帧的指针 + @param key:数据字段的名字 + @retval 指向指定数据字段的const指针,若查询不到key则返回NULL + @note 不应对返回的数据帧进行修改 */ const SoftBusItem* Bus_GetMapItem(SoftBusFrame* frame, char* key); /* - @brief жӳ֡Ƿֶָ - @param frame:ָ֡ - @param key:ֶε - @retval 0: 1: + @brief 判断映射表数据帧中是否存在指定字段 + @param frame:数据帧的指针 + @param key:数据字段的名字 + @retval 0:不存在 1:存在 */ #define Bus_IsMapKeyExist(frame,key) (Bus_GetMapItem((frame),(key)) != NULL) /* - @brief жϸkeyбǷȫӳ֡ - @param frame:ָ֡ - @param ...:Ҫжϵkeyб - @retval 0:һkey 1:key + @brief 判断给定key列表是否全部存在于映射表数据帧中 + @param frame:数据帧的指针 + @param ...:要判断的key列表 + @retval 0:任意一个key不存在 1:所有key都存在 @example if(Bus_CheckMapKeys(frame, {"key1", "key2", "key3"})) { ... } */ #define Bus_CheckMapKeys(frame,...) _Bus_CheckMapKeys((frame),(sizeof((char*[])__VA_ARGS__)/sizeof(char*)),((char*[])__VA_ARGS__)) /* - @brief ȡӳֶָ֡εֵ - @param frame:ָ֡ - @param key:ֶε - @retval ֵָ(void*)ָ - @note ȷkey֡УӦؽӿڽм - @note Ӧͨصָ޸ָ + @brief 获取映射表数据帧中指定字段的值 + @param frame:数据帧的指针 + @param key:数据字段的名字 + @retval 指向值的(void*)型指针 + @note 必须确保传入的key存在于数据帧中，应先用相关接口进行检查 + @note 不应通过返回的指针修改指向的数据 @example float value = *(float*)Bus_GetMapValue(frame, "key"); */ #define Bus_GetMapValue(frame,key) (Bus_GetMapItem((frame),(key))->data) /* - @brief ͨ㲥ٹ㲥 - @param name:㲥 - @retval Ŀپ + @brief 通过广播名创建快速广播句柄 + @param name:广播名 + @retval 创建出的快速句柄 @example SoftBusReceiverHandle handle = Bus_CreateReceiverHandle("name"); - @note ӦڳʼʱһΣÿηǰ + @note 应仅在程序初始化时创建一次，而不是每次发布前创建 */ SoftBusReceiverHandle Bus_CreateReceiverHandle(const char* name); /* - @brief ȡбָ֡ - @param frame:ָ֡ - @param pos:беλ - @retval ָݵ(void*)ָ룬򷵻NULL - @note Ӧͨصָ޸ָ - @example float value = *(float*)Bus_GetListValue(frame, 0); //ȡбеһֵ + @brief 获取列表数据帧中指定索引的数据 + @param frame:数据帧的指针 + @param pos:数据在列表中的位置 + @retval 指向数据的(void*)型指针，若不存在则返回NULL + @note 不应通过返回的指针修改指向的数据 + @example float value = *(float*)Bus_GetListValue(frame, 0); //获取列表中第一个值 */ #define Bus_GetListValue(frame,pos) (((pos) < (frame)->size)?((void**)(frame)->data)[(pos)]:NULL) diff --git a/test/cmsis_os.h b/test/cmsis_os.h new file mode 100644 index 0000000000000000000000000000000000000000..4942798a8038de75978cc5a5b24ff7899926fb8a --- /dev/null +++ b/test/cmsis_os.h @@ -0,0 +1,6 @@ + +// 假的CMSIS OS/FreeRTOS API，为测试而生。 + +#include +#define pvPortMalloc malloc +#define vPortFree free diff --git a/test/main.cpp b/test/main.cpp new file mode 100644 index 0000000000000000000000000000000000000000..15220fc0bcb03235c8452bcb609f1bbc1ab16d87 --- /dev/null +++ b/test/main.cpp @@ -0,0 +1,9 @@ + +#include "tests.hpp" + +#include +#include + +int main() { + TestYamlParser(); +} diff --git a/test/tests.hpp b/test/tests.hpp new file mode 100644 index 0000000000000000000000000000000000000000..16c6c4b51d1b0ab51b988977112be96061071578 --- /dev/null +++ b/test/tests.hpp @@ -0,0 +1,2 @@ + +void TestYamlParser(); diff --git a/test/yamlparsertest.cpp b/test/yamlparsertest.cpp new file mode 100644 index 0000000000000000000000000000000000000000..5a360b598779934c8cd7875779bdba8486ddfb07 --- /dev/null +++ b/test/yamlparsertest.cpp @@ -0,0 +1,85 @@ + +#include "../conf/yamlparser.h" +#include +#include + +constexpr const char* yaml_simple = +R"( +sys: + rotate-pid: + p: 1.5 + i: 0.0 + d: 0.0 + max-i: 100.0 + max-out: 200.0 + +chassis: + task-interval: 2 + info: + wheelbase: 100.0 #这里是注释 + wheeltrack: 100.0 + wheel-radius: 76.0 + offset-x: 0.0 + offset-y: 0.0 + #ThisIsAComment: 0 + move: + max-vx: 2000.0 + max-vy: 2000.0 + +testcase: + minus: + int: -65537 + float: -123.456 + positive: + int: 65536 + float: 12.3456 + string: + hello: "world" +)"; + +template bool CompareYamlValue(UimlYamlNode* node, T value) { + return !memcmp(&(node->I32), &value, sizeof(T)); +} +template <> bool CompareYamlValue(UimlYamlNode* node, const char* value) { + return !strcmp(node->Str, value); +} +template <> bool CompareYamlValue(UimlYamlNode* node, float value) { + return (node->F32 - value <= 1e-6); +} +template <> bool CompareYamlValue(UimlYamlNode* node, double value) { + return (node->F32 - value <= 1e-6); +} + +template void AssertYamlValue(UimlYamlNode* root, const char* path, T value) { + auto node = UimlYamlGetValueByPath(root, path); + if (node == NULL) { + std::cerr << "FAIL: Requested value of \"" << path << "\" but the item does not exist!\n"; + return; + } + if (!CompareYamlValue(node, value)) { + std::cerr << "FAIL: Value of \"" << path << "\" expected: " << value << ", got: " << *reinterpret_cast(&(node->I32)) << '\n'; + } +} + +void TestYamlParser() { + UimlYamlNode* root; + + UimlYamlParse(yaml_simple, &root); + auto pChassis = UimlYamlGetValue(root->Children, "chassis"); + auto pInfo = UimlYamlGetValue(pChassis->Children, "info"); + auto pOffsetX = UimlYamlGetValue(pInfo->Children, "wheel-radius"); + + std::cerr << "YAML test didn't crash." << std::endl; + + AssertYamlValue(root, "/chassis/info/wheel-radius", 76.0); + AssertYamlValue(root, "/testcase/minus/int", -65537); + AssertYamlValue(root, "/testcase/minus/float", -123.456); + AssertYamlValue(root, "/testcase/positive/int", 65536); + AssertYamlValue(root, "/testcase/positive/float", 12.3456); + AssertYamlValue(root, "/testcase/string/hello", "world"); + + std::cerr << "chassis.info.wheel-radius: " << pOffsetX->F32 << std::endl; + + std::cerr << "Comment test: " << UimlYamlGetValueByPath(root, "/chassis/#ThisIsAComment") << std::endl; + +} diff --git a/tools/CMakeLists.txt b/tools/CMakeLists.txt new file mode 100644 index 0000000000000000000000000000000000000000..382ce57a14cac7685417a17b8f259424b3126ac3 --- /dev/null +++ b/tools/CMakeLists.txt @@ -0,0 +1,9 @@ + +# 递归获取所有源码文件 +file(GLOB_RECURSE TOOLS_SRC *.c) + +# 传递源码文件到上一级 CMakeLists 作用域 +set(TOOLS_MODULE_SRC ${TOOLS_SRC} PARENT_SCOPE) + +# 添加 include 目录 +set(TOOLS_MODULE_INCL ${CMAKE_CURRENT_LIST_DIR}/include PARENT_SCOPE) diff --git a/tools/controller/pid.c b/tools/controller/pid.c deleted file mode 100644 index cc5682e9a31e6616bed1e0091c3781cc15284bb0..0000000000000000000000000000000000000000 --- a/tools/controller/pid.c +++ /dev/null @@ -1,64 +0,0 @@ -/****************PID****************/ - -#include "pid.h" - -//ʼpid -void PID_Init(PID *pid, ConfItem* conf) -{ - pid->kp=Conf_GetValue(conf, "p", float, 0); - pid->ki=Conf_GetValue(conf, "i", float, 0); - pid->kd=Conf_GetValue(conf, "d", float, 0); - pid->maxIntegral=Conf_GetValue(conf, "max-i", float, 0); - pid->maxOutput=Conf_GetValue(conf, "max-out", float, 0); - pid->deadzone=0; -} - -//pid -void PID_SingleCalc(PID *pid,float reference,float feedback) -{ - // - pid->lastError=pid->error; - if(ABS(reference-feedback) < pid->deadzone)//errorֱ0 - pid->error=0; - else - pid->error=reference-feedback; - //΢ - pid->output=(pid->error-pid->lastError)*pid->kd; - // - pid->output+=pid->error*pid->kp; - // - pid->integral+=pid->error*pid->ki; - LIMIT(pid->integral,-pid->maxIntegral,pid->maxIntegral);//޷ - pid->output+=pid->integral; - //޷ - LIMIT(pid->output,-pid->maxOutput,pid->maxOutput); -} - -//pid -void PID_CascadeCalc(CascadePID *pid,float angleRef,float angleFdb,float speedFdb) -{ - PID_SingleCalc(&pid->outer,angleRef,angleFdb);//⻷(ǶȻ) - PID_SingleCalc(&pid->inner,pid->outer.output,speedFdb);//ڻ(ٶȻ) - pid->output=pid->inner.output; -} - -//һpidʷ -void PID_Clear(PID *pid) -{ - pid->error=0; - pid->lastError=0; - pid->integral=0; - pid->output=0; -} - -//趨pid޷ -void PID_SetMaxOutput(PID *pid,float maxOut) -{ - pid->maxOutput=maxOut; -} - -//PID -void PID_SetDeadzone(PID *pid,float deadzone) -{ - pid->deadzone=deadzone; -} diff --git a/tools/controller/pid.cpp b/tools/controller/pid.cpp new file mode 100644 index 0000000000000000000000000000000000000000..75d03ebf47fbefcccd098b151750d1d933c0338a --- /dev/null +++ b/tools/controller/pid.cpp @@ -0,0 +1,112 @@ +/****************PID运算****************/ + +#include "../include/pid.h" + + +//初始化pid参数 +PID::PID(ConfItem* conf) : + kp(Conf_GetValue(conf, "p", float, 0)), + ki(ki=Conf_GetValue(conf, "i", float, 0)), + kd(Conf_GetValue(conf, "d", float, 0)), + maxIntegral(Conf_GetValue(conf, "max-i", float, 0)), + maxOutput(Conf_GetValue(conf, "max-out", float, 0)), + deadzone(0) +{ + +} + +//单级pid计算 +void PID::SingleCalc(float reference, float feedback) +{ + //更新数据 + lastError = error; + if(ABS(reference - feedback) < deadzone)//若误差在死区内则error直接置0 + error = 0; + else + error = reference - feedback; + //计算微分 + output = (error - lastError) * kd; + //计算比例 + output += error * kp; + //计算积分 + integral += error * ki; + LIMIT(integral, -maxIntegral, maxIntegral);//积分限幅 + output += integral; + //输出限幅 + LIMIT(output, -maxOutput, maxOutput); +} + +//前馈pid计算 +/* +当逻辑不复杂，没必要或不想给函数起名的时候，我们会推荐用Lambda表达式来代替函数指针Lambda表达式的结构 +[capture-list] (params) -> ret(optional) { body } +captures: 捕捉子句，作用是捕捉外部变量 +[]，啥都不捕捉，也就是Lambda表达式body中不能出现没在body中声明的变量 +[=]，按值捕捉所有变里，也就是lambda表达式前的所有变里，都可以以值传递(拷贝)的方式出现在body中（性能问题，不建议使用） +[&]，按引用捕捉所有变量，也就是Lambda表达式前的所有变量，都可以以引用传递的方式出现在body中（性能问题，不建议使用） +[&a]，按引用捕捉变量a，按值捕捉其他变量 +[&，a]，按值捕捉变量a，按引用捕捉其他变量 +[=，a]，[&，&a]这样的写法是错误的 +params: 和普通函数一样的参数(设置本地参数) +ret: 返回值类型，也可以省略，让编译器通过 return 语句自动推导 +body: 函数的具体逻辑 +样例： + [] (int a) ->float { return (a * 10);}; + a * 10可以改成自己所需要的数学模型，然后将lambda表达式整体传入FeedbackCalc函数中， +*/ +void PID::FeedbackCalc(float reference, float feedback, const std::function& lambda) +{ + //更新数据 + lastError = error; + if(ABS(reference - feedback) < deadzone)//若误差在死区内则error直接置0 + error = 0; + else + error = reference - feedback; + //计算微分 + output = (error - lastError) * kd; + //计算比例 + output += error * kp; + //计算积分 + integral += error * ki; + LIMIT(integral, -maxIntegral, maxIntegral);//积分限幅 + output += integral; + //加入前馈 + output += lambda(reference); + //输出限幅 + LIMIT(output, -maxOutput, maxOutput); +} + +//串级pid计算 +void CascadePID::CascadeCalc(float angleRef,float angleFdb,float speedFdb) +{ + outer.SingleCalc(angleRef, angleFdb);//计算外环(角度环) + inner.SingleCalc(outer.output, speedFdb);//计算内环(速度环) + output = inner.output; +} + +//清空一个pid的历史数据 +void PID::Clear() +{ + error = 0; + lastError = 0; + integral = 0; + output = 0; +} + +//重新设定pid输出限幅 +void PID::SetMaxOutput(float maxOutput) +{ + this->maxOutput = maxOutput; +} + +void PID::SetMaxIntegral(float maxIntegral) +{ + this->maxIntegral = maxIntegral; +} + +//设置PID死区 +void PID::SetDeadzone(float deadzone) +{ + this->deadzone = deadzone; +} + diff --git a/tools/controller/pid.h b/tools/controller/pid.h deleted file mode 100644 index 7226ec93100e6a3fa4ad0003f22a27689be1b1d7..0000000000000000000000000000000000000000 --- a/tools/controller/pid.h +++ /dev/null @@ -1,37 +0,0 @@ -#ifndef _USER_PID_H_ -#define _USER_PID_H_ - -#include "config.h" - -#ifndef LIMIT -#define LIMIT(x,min,max) (x)=(((x)<=(min))?(min):(((x)>=(max))?(max):(x))) -#endif - -#ifndef ABS -#define ABS(x) ((x)>=0?(x):-(x)) -#endif - -typedef struct _PID -{ - float kp,ki,kd; - float error,lastError;//ϴ - float integral,maxIntegral;//֡޷ - float output,maxOutput;//޷ - float deadzone;// -}PID; - -typedef struct _CascadePID -{ - PID inner;//ڻ - PID outer;//⻷ - float output;//inner.output -}CascadePID; - -void PID_Init(PID *pid, ConfItem* conf); -void PID_SingleCalc(PID *pid,float reference,float feedback); -void PID_CascadeCalc(CascadePID *pid,float angleRef,float angleFdb,float speedFdb); -void PID_Clear(PID *pid); -void PID_SetMaxOutput(PID *pid,float maxOut); -void PID_SetDeadzone(PID *pid,float deadzone); - -#endif diff --git a/tools/crc/crc16_modbus.c b/tools/crc/crc16_modbus.c index 9fa160d047ec192be21806c0b8b27db828508886..e43bb250d354b353e87fc9ba896e9e4d24c2d9b1 100644 --- a/tools/crc/crc16_modbus.c +++ b/tools/crc/crc16_modbus.c @@ -1,7 +1,7 @@ -#include "crc_modbus.h" +#include "crc16_modbus.h" #include -const uint8_t auchCRCHi[] = /* Table of CRC values for highCorder byte */ +const uint8_t auchCRCHi[] = /* Table of CRC values for high¨Corder byte */ { 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, @@ -21,7 +21,7 @@ const uint8_t auchCRCHi[] = /* Table of CRC values for high 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40 } ; -const uint8_t auchCRCLo[] = /* Table of CRC values for lowCorder byte */ +const uint8_t auchCRCLo[] = /* Table of CRC values for low¨Corder byte */ { 0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06, 0x07, 0xC7, 0x05, 0xC5, 0xC4, 0x04, 0xCC, 0x0C, 0x0D, 0xCD, 0x0F, 0xCF, 0xCE, 0x0E, 0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 0x09, 0x08, 0xC8, 0xD8, @@ -42,7 +42,7 @@ const uint8_t auchCRCLo[] = /* Table of CRC values for low } ; /****************************************************************************** - CRC + CRC计算 *******************************************************************************/ uint16_t CRC_Calculate(uint8_t *pdata, uint8_t num) { diff --git a/tools/first_order_filter/filter.c b/tools/first_order_filter/filter.c index 48e1ee1cc4e64354569af7acec62f4eb09bb57f1..b84bc43266ef5f262be8425a45b89add38f94f67 100644 --- a/tools/first_order_filter/filter.c +++ b/tools/first_order_filter/filter.c @@ -1,18 +1,18 @@ #include "filter.h" //X-MACRO -//бÿһʽΪ(,ʼ) +//子类列表，每一项格式为(类型名,初始化函数名) #define FILTER_CHILD_LIST \ FILTER_TYPE("kalman",Kalman_Init) \ FILTER_TYPE("mean",Mean_Init) \ FILTER_TYPE("low-pass",LowPass_Init) -//ʼ +//声明子类初始化函数 #define FILTER_TYPE(name,initFunc) extern Filter* initFunc(ConfItem*); FILTER_CHILD_LIST #undef FILTER_TYPE -//ڲ +//内部函数声明 void Filter_InitDefault(Filter* filter); float Filter_Cala(Filter *filter, float data); @@ -21,7 +21,7 @@ Filter* Filter_Init(ConfItem* dict) char* filterType = Conf_GetPtr(dict, "type", char); Filter *filter = NULL; - //жĸ + //判断属于哪个子类 #define FILTER_TYPE(name,initFunc) \ if(!strcmp(filterType, name)) \ filter = initFunc(dict); @@ -30,7 +30,7 @@ Filter* Filter_Init(ConfItem* dict) if(!filter) filter = FILTER_MALLOC_PORT(sizeof(Filter)); - //δķΪպ + //将子类未定义的方法设置为空函数 Filter_InitDefault(filter); return filter; @@ -42,5 +42,5 @@ void Filter_InitDefault(Filter* filter) filter->cala = Filter_Cala; } -//麯 +//纯虚函数 float Filter_Cala(Filter *filter, float data) {return 0;} diff --git a/tools/first_order_filter/kalman.c b/tools/first_order_filter/kalman.c index a27306a5cb6a92820dd65885a0e7ca061b547d55..cd757a710bddd9ac5977889698d220440c40cb3b 100644 --- a/tools/first_order_filter/kalman.c +++ b/tools/first_order_filter/kalman.c @@ -1,17 +1,17 @@ #include "filter.h" -//һ׿˲ṹ +//一阶卡尔曼滤波结构体 typedef struct { Filter filter; - float xLast; //һʱ̵Ž X(k|k-1) - float xPre; //ǰʱ̵Ԥ X(k|k-1) - float xNow; //ǰʱ̵Ž X(k|k) - float pPre; //ǰʱԤЭ P(k|k-1) - float pNow; //ǰʱŽЭ P(k|k) - float pLast; //һʱŽЭ P(k-1|k-1) - float kg; //kalman + float xLast; //上一时刻的最优结果 X(k|k-1) + float xPre; //当前时刻的预测结果 X(k|k-1) + float xNow; //当前时刻的最优结果 X(k|k) + float pPre; //当前时刻预测结果的协方差 P(k|k-1) + float pNow; //当前时刻最优结果的协方差 P(k|k) + float pLast; //上一时刻最优结果的协方差 P(k-1|k-1) + float kg; //kalman增益 float Q; float R; }KalmanFilter; @@ -21,13 +21,13 @@ Filter* Kalman_Init(ConfItem* dict); float Kalman_Cala(Filter* filter, float data); /** - * @brief ʼһ˲ - * @param kalman: ˲ - * @param T_Q:ϵͳЭ - * @param T_R:Э + * @brief 初始化一个卡尔曼滤波器 + * @param kalman: 滤波器 + * @param T_Q:系统噪声协方差 + * @param T_R:测量噪声协方差 * @retval None - * @attention R̶QԽ󣬴ԽβֵQֻòֵ - * ֮QԽСԽģԤֵQΪֻģԤ + * @attention R固定，Q越大，代表越信任侧量值，Q无穷代表只用测量值 + * 反之，Q越小代表越信任模型预测值，Q为零则是只用模型预测 */ Filter* Kalman_Init(ConfItem* dict) { @@ -45,14 +45,14 @@ Filter* Kalman_Init(ConfItem* dict) /** - * @brief ˲ - * @param kalman: ˲ - * @param data:˲ - * @retval ˲ - * @attention Z(k)ϵͳ,ֵ X(k|k)ǿ˲ֵ, - * A=1 B=0 H=1 I=1 W(K) V(k)Ǹ˹,ڲֵ,Բù - * ǿ5Ĺʽ - * һH'Ϊ,Ϊתþ + * @brief 卡尔曼滤波器 + * @param kalman: 滤波器 + * @param data:待滤波数据 + * @retval 滤波后的数据 + * @attention Z(k)是系统输入,即测量值 X(k|k)是卡尔曼滤波后的值,即最终输出 + * A=1 B=0 H=1 I=1 W(K) V(k)是高斯白噪声,叠加在测量值上了,可以不用管 + * 以下是卡尔曼的5个核心公式 + * 一阶H'即为它本身,否则为转置矩阵 */ float Kalman_Cala(Filter* filter, float data) { @@ -63,7 +63,7 @@ float Kalman_Cala(Filter* filter, float data) kalman->kg = kalman->pPre / (kalman->pPre + kalman->R); //kg(k) = p(k|k-1)*H'/(H*p(k|k-1)*H'+R) kalman->xNow = kalman->xPre + kalman->kg * (data - kalman->xPre); //x(k|k) = X(k|k-1)+kg(k)*(Z(k)-H*X(k|k-1)) kalman->pNow = (1 - kalman->kg) * kalman->pPre; //p(k|k) = (I-kg(k)*H)*P(k|k-1) - kalman->pLast = kalman->pNow; //״̬ - kalman->xLast = kalman->xNow; //״̬ - return kalman->xNow; //Ԥx(k|k) + kalman->pLast = kalman->pNow; //状态更新 + kalman->xLast = kalman->xNow; //状态更新 + return kalman->xNow; //输出预测结果x(k|k) } diff --git a/tools/first_order_filter/low_pass.c b/tools/first_order_filter/low_pass.c index f079edfebaa5d8f5fb3edb1d648cc746fc1aecd0..6500c04d6a6e696c0d6a1e43cfca6a2e31333a24 100644 --- a/tools/first_order_filter/low_pass.c +++ b/tools/first_order_filter/low_pass.c @@ -1,5 +1,5 @@ #include "filter.h" -//ͨ˲ṹ +//低通滤波结构体 typedef struct { Filter filter; diff --git a/tools/first_order_filter/mean.c b/tools/first_order_filter/mean.c index 4dc379f926dcbd19bde2d4ca1602f20b73e83e06..980e70e61ff6eb04ea34f5aa140cedc84d95e056 100644 --- a/tools/first_order_filter/mean.c +++ b/tools/first_order_filter/mean.c @@ -1,6 +1,6 @@ #include "filter.h" -//ֵ˲ṹ +//均值滤波结构体 typedef struct { Filter filter; @@ -14,10 +14,10 @@ typedef struct float Mean_Cala(Filter* filter, float data); /** - * @brief ʼһֵ˲ - * @param ˲ṹ - * @param Ҫ˲ṹ - * @param ˲С˲ڲڴ洢ݵĴС + * @brief 初始化一个均值滤波器 + * @param 滤波器结构体 + * @param 需要关联到滤波器结构体的数组 + * @param 滤波器缓存区大小（滤波器内部用于存储数据的数组的大小） * @retval None */ Filter* Mean_Init(ConfItem* dict) @@ -36,9 +36,9 @@ Filter* Mean_Init(ConfItem* dict) } /** - * @brief ֵ˲ - * @param ˲ṹ壬Ҫ˲ - * @retval ˲ + * @brief 均值滤波函数 + * @param 滤波器结构体，需要滤波的数据 + * @retval 滤波输出量 * @attention None */ float Mean_Cala(Filter* filter, float data) diff --git a/tools/hasher/hasher.c b/tools/hasher/hasher.c new file mode 100644 index 0000000000000000000000000000000000000000..d3c315f643fad6463bbc29fe2c633742cb5fddab --- /dev/null +++ b/tools/hasher/hasher.c @@ -0,0 +1,25 @@ + +#include "hasher.h" +#include + +uint32_t Hasher_UIML32(const uint8_t *data, size_t length) +{ + uint32_t h = 0; + uint16_t strLength = length, alignedLen = strLength / sizeof(uint32_t); + for(uint16_t i = 0; i < alignedLen; ++i) + h = (h << 5) - h + ((uint32_t*)data)[i]; + for(uint16_t i = alignedLen << 2; i < strLength; ++i) + h = (h << 5) - h + data[i]; + return h; +} + +uint32_t Hasher_FNV1A(const uint8_t *data, size_t length) +{ + uint32_t prime = 16777619U; + uint32_t ret = 2166136261U; + for (size_t i = 0; i < length; i++) { + ret ^= (uint32_t)(data[i]); + ret *= prime; + } + return ret; +} diff --git a/tools/crc/crc16_modbus.h b/tools/include/crc16_modbus.h similarity index 100% rename from tools/crc/crc16_modbus.h rename to tools/include/crc16_modbus.h diff --git a/tools/crc/crc_dji.h b/tools/include/crc_dji.h similarity index 100% rename from tools/crc/crc_dji.h rename to tools/include/crc_dji.h diff --git a/tools/first_order_filter/filter.h b/tools/include/filter.h similarity index 94% rename from tools/first_order_filter/filter.h rename to tools/include/filter.h index 0f7c347a8ffe49559de31ac47d28808ffc115704..d4cd489effd105a3289e3fa4b79b637658a8859c 100644 --- a/tools/first_order_filter/filter.h +++ b/tools/include/filter.h @@ -3,6 +3,7 @@ #include "config.h" #include "cmsis_os.h" +#include #define FILTER_MALLOC_PORT(len) pvPortMalloc(len) #define FILTER_FREE_PORT(ptr) vPortFree(ptr) diff --git a/tools/include/hasher.h b/tools/include/hasher.h new file mode 100644 index 0000000000000000000000000000000000000000..4d9502f82c5a65982537198183865dad6a62bf0a --- /dev/null +++ b/tools/include/hasher.h @@ -0,0 +1,13 @@ + +#include + +#ifdef __cplusplus +extern "C" { +#endif + +uint32_t Hasher_UIML32(const uint8_t* data, size_t length); +uint32_t Hasher_FNV1A(const uint8_t* data, size_t length); + +#ifdef __cplusplus +} // extern "C" { +#endif diff --git a/tools/motor/motor.h b/tools/include/motor.h similarity index 85% rename from tools/motor/motor.h rename to tools/include/motor.h index 9eb7d17a1314334eb8fa2bc048453d1f52b232e2..8abf0162ca24fa4fccd351c17bd48ba00c25432b 100644 --- a/tools/motor/motor.h +++ b/tools/include/motor.h @@ -2,10 +2,13 @@ #define _MOTOR_H_ #include "config.h" #include "cmsis_os.h" +#include +#include +#include #define MOTOR_MALLOC_PORT(len) pvPortMalloc(len) #define MOTOR_FREE_PORT(ptr) vPortFree(ptr) -//ģʽ +//模式 typedef enum { MOTOR_TORQUE_MODE, @@ -13,7 +16,7 @@ typedef enum MOTOR_ANGLE_MODE, MOTOR_STOP_MODE }MotorCtrlMode; -//࣬ķ +//父类，包含所有子类的方法 typedef struct _Motor { void (*changeMode)(struct _Motor* motor, MotorCtrlMode mode); diff --git a/tools/universal_queue/my_queue.h b/tools/include/my_queue.h similarity index 69% rename from tools/universal_queue/my_queue.h rename to tools/include/my_queue.h index 7686497ce976fb151a5889cd5c5fce51b6bd686a..a30027f50a685e885a7d04995bc11fe018a68451 100644 --- a/tools/universal_queue/my_queue.h +++ b/tools/include/my_queue.h @@ -5,21 +5,21 @@ #define EMPTY_QUEUE {NULL,0,0,0,NULL,0} -/*************ݽṹ**************/ -//нṹ +/*************数据结构**************/ +//队列结构体 typedef struct _Queue { - // - void **data;//ָֻ룬Ҫͬʱָ븽ӱ + //队列数据 + void **data;//只保存指针，若要同时保存指向的数据请附加保存区 uint16_t maxSize; uint16_t front,rear; uint8_t initialized; - //ݱ(ѡ) + //数据保存区(可选用) void *buffer; - uint8_t bufElemSize;//ÿԪصĴС + uint8_t bufElemSize;//每个元素的大小 }Queue; -/**************ӿں***************/ +/**************接口函数***************/ void Queue_Init(Queue *queue,uint16_t maxSize); void Queue_AttachBuffer(Queue *queue,void *buffer,uint8_t elemSize); void Queue_Destroy(Queue *queue); diff --git a/tools/include/pid.h b/tools/include/pid.h new file mode 100644 index 0000000000000000000000000000000000000000..53a2355a9dc30d48562e95cdeba08c8560711e1d --- /dev/null +++ b/tools/include/pid.h @@ -0,0 +1,45 @@ +#ifndef _USER_PID_H_ +#define _USER_PID_H_ + +#include "../../conf/config.h" + +#include + +#ifndef LIMIT +#define LIMIT(x,min,max) (x)=(((x)<=(min))?(min):(((x)>=(max))?(max):(x))) +#endif + +#ifndef ABS +#define ABS(x) ((x)>=0?(x):-(x)) +#endif + +class PID +{ +public: + float kp,ki,kd; + float error,lastError;//误差、上次误差 + float integral,maxIntegral;//积分、积分限幅 + float output,maxOutput;//输出、输出限幅 + float deadzone;//死区 + + PID(ConfItem* conf); + void SingleCalc(float reference,float feedback); + void FeedbackCalc(float reference, float feedback, const std::function& lambda); + void Clear(); + void SetMaxOutput(float maxOutput); + void SetMaxIntegral(float maxIntegral); + void SetDeadzone(float deadzone); +}; + +class CascadePID +{ +public: + PID inner;//内环 + PID outer;//外环 + float output;//串级输出，等于inner.output + + void CascadeCalc(float angleRef,float angleFdb,float speedFdb); +}; + + +#endif diff --git a/tools/slope/slope.h b/tools/include/slope.h similarity index 67% rename from tools/slope/slope.h rename to tools/include/slope.h index 5971eb2359f680fa0ae66014ebc865cac27b2ecc..5b1ff31e29b48e704fb274cef9ed8bace0e7cc52 100644 --- a/tools/slope/slope.h +++ b/tools/include/slope.h @@ -5,12 +5,12 @@ #define ABS(x) ((x)>=0?(x):-(x)) #endif -//б½ṹ +//斜坡结构体 typedef struct{ - float target; //Ŀֵ - float step; //ֵ - float value; //ǰֵ - float deadzone; //ֵСڸֵ򲻽 + float target; //目标值 + float step; //步进值 + float value; //当前值 + float deadzone; //死区，若差值小于该值则不进行增减 }Slope; void Slope_Init(Slope *slope,float step,float deadzone); diff --git a/tools/universal_vector/vector.h b/tools/include/vector.h similarity index 40% rename from tools/universal_vector/vector.h rename to tools/include/vector.h index 6541324afc8b26155d2d36a6ad8f1ec228b7aa43..f317081b21a2101d2850f9fefa6735f883d29e78 100644 --- a/tools/universal_vector/vector.h +++ b/tools/include/vector.h @@ -1,18 +1,18 @@ -/* CԵͨͶ̬ģ飬C++еSTLģvector */ +/* 用于C语言的通用类型动态数组模块，类似于C++中的STL模板库vector */ #ifndef _VECTOR_H_ #define _VECTOR_H_ #include -//Vectorݽṹ +//Vector数据结构体 typedef struct{ uint8_t *data; uint8_t elementSize; uint32_t size,capacity; }Vector; -//(ֱӵãӦʹ·defineĽӿ) +//操作函数声明(不直接调用，应使用下方define定义的接口) int _Vector_Init(Vector *vector,int _elementSize); Vector _Vector_Create(int _elementSize); void _Vector_Destroy(Vector *vector); @@ -23,154 +23,154 @@ int _Vector_SetValue(Vector *vector, uint32_t index, void *element); int _Vector_TrimCapacity(Vector *vector); /* - @brief ʼһָ͵Vector - @param vector:ҪʼVector - type:VectorнŵԪ - @retval 0:ɹ -1:ʧ + @brief 初始化一个指定类型的Vector + @param vector:需要初始化的Vector对象 + type:该Vector中将会存放的元素类型 + @retval 0:成功 -1:失败 */ #define Vector_Init(vector,type) (_Vector_Init(&(vector),sizeof(type))) /* - @brief һָ͵Vector - @param type:VectorнŵԪ - @retval ѳʼõVector (ڴʧdata=NULL) + @brief 创建一个指定类型的Vector + @param type:该Vector中将会存放的元素类型 + @retval 已初始化好的Vector对象 (若内存分配失败则其中data=NULL) */ #define Vector_Create(type) (_Vector_Create(sizeof(type))) /* - @brief һVector - @param vector:ҪٵVector + @brief 销毁一个Vector + @param vector:需要销毁的Vector @retval void */ #define Vector_Destroy(vector) (_Vector_Destroy(&(vector))) /* - @brief ȡѲԪ - @param vector:ĿVector - @retval vectorеԪ + @brief 获取已插入的元素数量 + @param vector:目标Vector对象 + @retval 该vector中的元素数量 */ #define Vector_Size(vector) ((vector).size) /* - @brief ȡǰѷ - @param vector:ĿVector - @retval ΪvectorĿռܴ洢Ԫ + @brief 获取当前已分配的容量 + @param vector:目标Vector对象 + @retval 已为该vector分配的空间所能存储的元素数量 */ #define Vector_Capacity(vector) ((vector).capacity) /* - @brief ɾ - @param vector:ĿVector - @retval 0:ɹ -1:ʧ + @brief 删除空余容量 + @param vector:目标Vector对象 + @retval 0:成功 -1:失败 */ #define Vector_TrimCapacity(vector) (_Vector_TrimCapacity(&(vector))) /* - @brief VectorתΪԪ - @param vector:Ŀvector - type:Ԫ - @retval type*͵ָ룬ָ׵ֱַΪʹ + @brief 将Vector转为元素数组 + @param vector:目标vector对象 + type:元素类型 + @retval type*类型的指针，指向数据首地址，可直接作为数组名使用 @example int num = Vector_ToArray(vector,int)[0]; */ #define Vector_ToArray(vector,type) ((type*)((vector).data)) /* - @brief ȡָ±괦Ԫصָ - @param vector:Ŀvector - index:ָ± - type:Ԫ - @retval ָָ±괦Ԫص(type*)ָ룬±겻򷵻NULL + @brief 获取指定下标处元素的指针 + @param vector:目标vector对象 + index:指定的下标 + type:元素类型 + @retval 指向指定下标处元素的(type*)型指针，若下标不存在则返回NULL */ #define Vector_GetByIndex(vector,index,type) ((type*)_Vector_GetByIndex(&(vector),(index))) /* - @brief ȡԪָ - @param vector:Ŀvector - type:Ԫ - @retval ָԪص(type*)ָ룬򷵻NULL + @brief 获取首元素指针 + @param vector:目标vector对象 + type:元素类型 + @retval 指向首元素的(type*)型指针，若不存在则返回NULL */ #define Vector_GetFront(vector,type) Vector_GetByIndex(vector,0,type) /* - @brief ȡβԪָ - @param vector:Ŀvector - type:Ԫ - @retval ָβԪص(type*)ָ룬򷵻NULL + @brief 获取尾元素指针 + @param vector:目标vector对象 + type:元素类型 + @retval 指向尾元素的(type*)型指针，若不存在则返回NULL */ #define Vector_GetBack(vector,type) Vector_GetByIndex(vector,(vector).size-1,type) /* - @brief ޸ָ±ֵ - @param vector:Ŀvector - index:ָ± - val:Ҫ޸ĵֵ - @retval 0:ɹ -1:ʧ - @notice ϣvalӦʹ紫100Ӧд"(int){100}" + @brief 修改指定下标的值 + @param vector:目标vector对象 + index:指定的下标 + val:需要修改的新值 + @retval 0:成功 -1:失败 + @notice 若希望在val处传入立即数则应使用字面量，如传入100应写"(int){100}" */ #define Vector_SetValue(vector,index,val) (_Vector_SetValue(&(vector),(index),&(val))) /* - @brief ɾָ±Ԫ - @param vector:Ҫvector - index:ָ± - @retval 0:ɹ -1:ʧ + @brief 删除指定下标的元素 + @param vector:要操作的vector对象 + index:指定的下标 + @retval 0:成功 -1:失败 */ #define Vector_Remove(vector,index) (_Vector_Remove(&(vector),(index))) /* - @brief ɾβԪ - @param vector:Ҫvector - @retval 0:ɹ -1:ʧ + @brief 删除尾元素 + @param vector:要操作的vector对象 + @retval 0:成功 -1:失败 */ #define Vector_PopBack(vector) Vector_Remove(vector,(vector).size-1) /* - @brief ɾԪ - @param vector:Ҫvector - @retval 0:ɹ -1:ʧ + @brief 删除首元素 + @param vector:要操作的vector对象 + @retval 0:成功 -1:失败 */ #define Vector_PopFront(vector) Vector_Remove(vector,0) /* - @brief ɾԪ - @param vector:Ҫvector + @brief 删除所有元素 + @param vector:要操作的vector对象 @retval 0 */ #define Vector_Clear(vector) ((vector).size=0) /* - @brief ָ±괦Ԫأ±괦ԭԪؼԪƶ - @param vector:Ŀvector - index:ָ± - val:Ҫֵ - @retval 0:ɹ -1:ʧ - @notice ϣvalӦʹ紫100Ӧд"(int){100}" + @brief 在指定下标处插入元素，该下标处原有元素及其后续元素向后移动 + @param vector:目标vector对象 + index:指定的下标 + val:需要插入的值 + @retval 0:成功 -1:失败 + @notice 若希望在val处传入立即数则应使用字面量，如传入100应写"(int){100}" */ #define Vector_Insert(vector,index,val) (_Vector_Insert(&(vector),(index),&(val))) /* - @brief βԪ - @param vector:Ŀvector - val:Ҫֵ - @retval 0:ɹ -1:ʧ - @notice ϣvalӦʹ紫100Ӧд"(int){100}" + @brief 在尾部插入元素 + @param vector:目标vector对象 + val:需要插入的值 + @retval 0:成功 -1:失败 + @notice 若希望在val处传入立即数则应使用字面量，如传入100应写"(int){100}" */ #define Vector_PushBack(vector,val) Vector_Insert(vector,(vector).size,(val)) /* - @brief ͷԪ - @param vector:Ŀvector - val:Ҫֵ - @retval 0:ɹ -1:ʧ - @notice ϣvalӦʹ紫100Ӧд"(int){100}" + @brief 在头部插入元素 + @param vector:目标vector对象 + val:需要插入的值 + @retval 0:成功 -1:失败 + @notice 若希望在val处传入立即数则应使用字面量，如传入100应写"(int){100}" */ #define Vector_PushFront(vector,val) Vector_Insert(vector,0,(val)) /* - @brief 䣬еfor - @param vector:Ŀvector - iter:ʱıָԪص(type*)ָ - type:Ԫص + @brief 遍历迭代语句，替代遍历过程中的for语句 + @param vector:目标vector对象 + iter:遍历时迭代器的变量名，是指向所遍历到的元素的(type*)型指针 + type:元素的类型 @example Vector_ForEach(vector,ptr,int) { printf("%d",*ptr); } */ #define Vector_ForEach(vector,iter,type) for(type *iter=(type*)(vector).data; iter<((type*)(vector).data+(vector).size); iter++) diff --git a/tools/motor/motor.c b/tools/motor/motor.c index 23a94fff80158a8b7760e79a16e512303072d8f3..bcb26ed97372690bae33926cb619daa3740c3582 100644 --- a/tools/motor/motor.c +++ b/tools/motor/motor.c @@ -1,7 +1,7 @@ #include "motor.h" //X-MACRO -//бÿһʽΪ(,ʼ) +//子类列表，每一项格式为(类型名,初始化函数名) #define MOTOR_CHILD_LIST \ MOTOR_TYPE("M3508",M3508_Init) \ MOTOR_TYPE("M2006",M2006_Init) \ @@ -9,7 +9,7 @@ MOTOR_TYPE("Servo",Servo_Init) \ MOTOR_TYPE("DcMotor",DcMotor_Init) -//ڲ +//内部函数声明 void Motor_SetTarget(Motor* motor, float targetValue); void Motor_ChangeCtrler(Motor* motor, MotorCtrlMode ctrlerType); void Motor_InitTotalAngle(Motor* motor, float angle); @@ -17,7 +17,7 @@ float Motor_GetData(Motor* motor, const char* data); void Motor_Stop(Motor* motor); void Motor_InitDefault(Motor* motor); -//ʼ +//声明子类初始化函数 #define MOTOR_TYPE(name,initFunc) __weak Motor* initFunc(ConfItem* dict) {return NULL;} MOTOR_CHILD_LIST #undef MOTOR_TYPE @@ -27,7 +27,7 @@ Motor* Motor_Init(ConfItem* dict) char* motorType = Conf_GetPtr(dict, "type", char); Motor *motor = NULL; - //жĸ + //判断属于哪个子类 #define MOTOR_TYPE(name,initFunc) \ if(!strcmp(motorType, name)) \ motor = initFunc(dict); @@ -36,7 +36,7 @@ Motor* Motor_Init(ConfItem* dict) if(!motor) motor = MOTOR_MALLOC_PORT(sizeof(Motor)); - //δķΪպ + //将子类未定义的方法设置为空函数 Motor_InitDefault(motor); return motor; @@ -56,7 +56,7 @@ void Motor_InitDefault(Motor* motor) motor->stop = Motor_Stop; } -//麯 +//纯虚函数 void Motor_SetTarget(Motor* motor, float targetValue) { } void Motor_ChangeCtrler(Motor* motor, MotorCtrlMode mode) { } diff --git a/tools/motor/motor_can/m2006.c b/tools/motor/motor_can/m2006.c index 795ad470af7a87f486a3a2390871119d2f57f941..d14b67b76e30ac7e22ef88cbc9b5838760a63eb9 100644 --- a/tools/motor/motor_can/m2006.c +++ b/tools/motor/motor_can/m2006.c @@ -3,37 +3,37 @@ #include "pid.h" #include "config.h" -//ֵֵǶȵĻ -#define M2006_DGR2CODE(dgr,rdcr) ((int32_t)((dgr)*22.7528f*(rdcr))) //ٱ*8191/360 +//各种电机编码值与角度的换算 +#define M2006_DGR2CODE(dgr,rdcr) ((int32_t)((dgr)*22.7528f*(rdcr))) //减速比*8191/360 #define M2006_CODE2DGR(code,rdcr) ((float)((code)/(22.7528f*(rdcr)))) typedef struct _M2006 { - Motor motor; - - float reductionRatio; - struct - { - uint16_t sendID,recvID; - uint8_t canX; - uint8_t bufIndex; - }canInfo; - MotorCtrlMode mode; - - int16_t angle,speed,torque; - - int16_t lastAngle;//¼һεõĽǶ - - int32_t totalAngle;//ۼתıֵ - - float targetValue;//Ŀֵ(Ťؾ/ٶ/Ƕ(λ)) - - uint16_t stallTime;//תʱ - - PID speedPID;//ٶpid() - CascadePID anglePID;//Ƕpid - - char* stallName; + Motor motor; + + float reductionRatio; + struct + { + uint16_t sendID,recvID; + uint8_t canX; + uint8_t bufIndex; + }canInfo; + MotorCtrlMode mode; + + int16_t angle,speed,torque; + + int16_t lastAngle;//记录上一次得到的角度 + + int32_t totalAngle;//累计转过的编码器值 + + float targetValue;//目标值(输出轴扭矩矩/速度/角度(单位度)) + + uint16_t stallTime;//堵转时间 + + PID speedPID;//速度pid(单级) + CascadePID anglePID;//角度pid，串级 + + char* stallName; }M2006; Motor* M2006_Init(ConfItem* dict); @@ -50,198 +50,197 @@ void M2006_PIDInit(M2006* m2006, ConfItem* dict); void M2006_StatAngle(M2006* m2006); void M2006_CtrlerCalc(M2006* m2006, float reference); -//ʱص +//软件定时器回调函数 void M2006_TimerCallback(void const *argument) { - M2006* m2006 = pvTimerGetTimerID((TimerHandle_t)argument); - M2006_StatAngle(m2006); - M2006_CtrlerCalc(m2006, m2006->targetValue); - m2006->stallTime = (m2006->speed == 0 && abs(m2006->torque) > 500) ? m2006->stallTime + 2 : 0; - if (m2006->stallTime > 500) - { - Bus_BroadcastSend(m2006->stallName, {{""}}); - m2006->stallTime = 0; - } + M2006* m2006 = pvTimerGetTimerID((TimerHandle_t)argument); + M2006_StatAngle(m2006); + M2006_CtrlerCalc(m2006, m2006->targetValue); + m2006->stallTime = (m2006->speed == 0 && abs(m2006->torque) > 500) ? m2006->stallTime + 2 : 0; + if (m2006->stallTime > 500) + { + Bus_BroadcastSend(m2006->stallName, {{""}}); + m2006->stallTime = 0; + } } Motor* M2006_Init(ConfItem* dict) { - //ڴռ - M2006* m2006 = MOTOR_MALLOC_PORT(sizeof(M2006)); - memset(m2006,0,sizeof(M2006)); - //̬ - m2006->motor.setTarget = M2006_SetTarget; - m2006->motor.changeMode = M2006_ChangeMode; - m2006->motor.initTotalAngle = M2006_InitTotalAngle; - m2006->motor.getData = M2006_GetData; - m2006->motor.stop = M2006_Stop; - //ٱ - m2006->reductionRatio = Conf_GetValue(dict, "reduction-ratio", float, 36);//δõٱȲʹԭװĬϼٱ - //ʼcanϢ - uint16_t id = Conf_GetValue(dict, "id", uint16_t, 0); - m2006->canInfo.recvID = id + 0x200; - m2006->canInfo.sendID = (id <= 4) ? 0x200 : 0x1FF; - m2006->canInfo.bufIndex = ((id - 1)%4) * 2; - m2006->canInfo.canX = Conf_GetValue(dict, "can-x", uint8_t, 0); - //õĬģʽΪŤģʽ - m2006->mode = MOTOR_TORQUE_MODE; - //ʼpid - M2006_PIDInit(m2006, dict); - //canϢ - char name[] = "/can_/recv"; - name[4] = m2006->canInfo.canX + '0'; - Bus_RegisterReceiver(m2006, M2006_SoftBusCallback, name); - - char* motorName = Conf_GetPtr(dict, "name", char); - motorName = motorName ? motorName : "motor"; - uint8_t len = strlen(motorName); - m2006->stallName = MOTOR_MALLOC_PORT(len + 7+ 1); //7Ϊ"/ /stall"ijȣ1Ϊ'\0'ij - sprintf(m2006->stallName, "/%s/stall", motorName); - //ʱ - osTimerDef(M2006, M2006_TimerCallback); - osTimerStart(osTimerCreate(osTimer(M2006), osTimerPeriodic, m2006), 2); - - return (Motor*)m2006; + //分配子类内存空间 + M2006* m2006 = MOTOR_MALLOC_PORT(sizeof(M2006)); + memset(m2006,0,sizeof(M2006)); + //子类多态 + m2006->motor.setTarget = M2006_SetTarget; + m2006->motor.changeMode = M2006_ChangeMode; + m2006->motor.initTotalAngle = M2006_InitTotalAngle; + m2006->motor.getData = M2006_GetData; + m2006->motor.stop = M2006_Stop; + //电机减速比 + m2006->reductionRatio = Conf_GetValue(dict, "reduction-ratio", float, 36);//如果未配置电机减速比参数，则使用原装电机默认减速比 + //初始化电机绑定can信息 + uint16_t id = Conf_GetValue(dict, "id", uint16_t, 0); + m2006->canInfo.recvID = id + 0x200; + m2006->canInfo.sendID = (id <= 4) ? 0x200 : 0x1FF; + m2006->canInfo.bufIndex = ((id - 1)%4) * 2; + m2006->canInfo.canX = Conf_GetValue(dict, "can-x", uint8_t, 0); + //设置电机默认模式为扭矩模式 + m2006->mode = MOTOR_TORQUE_MODE; + //初始化电机pid + M2006_PIDInit(m2006, dict); + //订阅can信息 + char name[] = "/can_/recv"; + name[4] = m2006->canInfo.canX + '0'; + Bus_RegisterReceiver(m2006, M2006_SoftBusCallback, name); + + char* motorName = Conf_GetPtr(dict, "name", char); + motorName = motorName ? motorName : "motor"; + uint8_t len = strlen(motorName); + m2006->stallName = MOTOR_MALLOC_PORT(len + 7+ 1); //7为"/ /stall"的长度，1为'\0'的长度 + sprintf(m2006->stallName, "/%s/stall", motorName); + //开启软件定时器 + osTimerDef(M2006, M2006_TimerCallback); + osTimerStart(osTimerCreate(osTimer(M2006), osTimerPeriodic, m2006), 2); + + return (Motor*)m2006; } -//ʼpid +//初始化pid void M2006_PIDInit(M2006* m2006, ConfItem* dict) { - PID_Init(&m2006->speedPID, Conf_GetPtr(dict, "speed-pid", ConfItem)); - PID_Init(&m2006->anglePID.inner, Conf_GetPtr(dict, "angle-pid/inner", ConfItem)); - PID_Init(&m2006->anglePID.outer, Conf_GetPtr(dict, "angle-pid/outer", ConfItem)); - PID_SetMaxOutput(&m2006->anglePID.outer, m2006->anglePID.outer.maxOutput*m2006->reductionRatio);//ٶ޷Ŵת + PID_Init(&m2006->speedPID, Conf_GetPtr(dict, "speed-pid", ConfItem)); + PID_Init(&m2006->anglePID.inner, Conf_GetPtr(dict, "angle-pid/inner", ConfItem)); + PID_Init(&m2006->anglePID.outer, Conf_GetPtr(dict, "angle-pid/outer", ConfItem)); + PID_SetMaxOutput(&m2006->anglePID.outer, m2006->anglePID.outer.maxOutput*m2006->reductionRatio);//将输出轴速度限幅放大到转子上 } -//߻ص +//软总线回调函数 void M2006_SoftBusCallback(const char* name, SoftBusFrame* frame, void* bindData) { - M2006* m2006 = (M2006*)bindData; - - uint16_t id = *(uint16_t*)Bus_GetListValue(frame, 0); - if(id != m2006->canInfo.recvID) - return; - - uint8_t* data = (uint8_t*)Bus_GetListValue(frame, 1); - if(data) - M2006_Update(m2006, data); + M2006* m2006 = (M2006*)bindData; + + uint16_t id = *(uint16_t*)Bus_GetListValue(frame, 0); + if(id != m2006->canInfo.recvID) + return; + + uint8_t* data = (uint8_t*)Bus_GetListValue(frame, 1); + if(data) + M2006_Update(m2006, data); } -//ʼͳƵۼƽǶ +//开始统计电机累计角度 void M2006_InitTotalAngle(Motor *motor, float startAngle) { - M2006* m2006 = (M2006*)motor; - - m2006->totalAngle= M2006_DGR2CODE(startAngle, m2006->reductionRatio); - m2006->lastAngle=m2006->angle; + M2006* m2006 = (M2006*)motor; + + m2006->totalAngle= M2006_DGR2CODE(startAngle, m2006->reductionRatio); + m2006->lastAngle=m2006->angle; } -//ͳƵۼתȦ +//统计电机累计转过的圈数 void M2006_StatAngle(M2006* m2006) { - int32_t dAngle=0; - if(m2006->angle-m2006->lastAngle<-4000) - dAngle=m2006->angle+(8191-m2006->lastAngle); - else if(m2006->angle-m2006->lastAngle>4000) - dAngle=-m2006->lastAngle-(8191-m2006->angle); - else - dAngle=m2006->angle-m2006->lastAngle; - //Ƕ - m2006->totalAngle+=dAngle; - //¼Ƕ - m2006->lastAngle=m2006->angle; + int32_t dAngle=0; + if(m2006->angle-m2006->lastAngle<-4000) + dAngle=m2006->angle+(8191-m2006->lastAngle); + else if(m2006->angle-m2006->lastAngle>4000) + dAngle=-m2006->lastAngle-(8191-m2006->angle); + else + dAngle=m2006->angle-m2006->lastAngle; + //将角度增量加入计数器 + m2006->totalAngle+=dAngle; + //记录角度 + m2006->lastAngle=m2006->angle; } -//ģʽ +//控制器根据模式计算输出 void M2006_CtrlerCalc(M2006* m2006, float reference) { - int16_t output=0; - uint8_t buffer[2]={0}; - if(m2006->mode == MOTOR_SPEED_MODE) - { - PID_SingleCalc(&m2006->speedPID, reference, m2006->speed); - output = m2006->speedPID.output; - } - else if(m2006->mode == MOTOR_ANGLE_MODE) - { - PID_CascadeCalc(&m2006->anglePID, reference, m2006->totalAngle, m2006->speed); - output = m2006->anglePID.output; - } - else if(m2006->mode == MOTOR_TORQUE_MODE) - { - output = (int16_t)reference; - } - buffer[0] = (output>>8)&0xff; - buffer[1] = (output)&0xff; - //canϢ - Bus_RemoteCall("/can/set-buf",{ - {"can-x", &m2006->canInfo.canX}, - {"id", &m2006->canInfo.sendID}, - {"pos", &m2006->canInfo.bufIndex}, - {"len", &(uint8_t){2}}, - {"data", buffer} - }); + int16_t output=0; + uint8_t buffer[2]={0}; + if(m2006->mode == MOTOR_SPEED_MODE) + { + PID_SingleCalc(&m2006->speedPID, reference, m2006->speed); + output = m2006->speedPID.output; + } + else if(m2006->mode == MOTOR_ANGLE_MODE) + { + PID_CascadeCalc(&m2006->anglePID, reference, m2006->totalAngle, m2006->speed); + output = m2006->anglePID.output; + } + else if(m2006->mode == MOTOR_TORQUE_MODE) + { + output = (int16_t)reference; + } + buffer[0] = (output>>8)&0xff; + buffer[1] = (output)&0xff; + //发布can信息 + Bus_RemoteCall("/can/set-buf",{{"can-x", {.U8 = m2006->canInfo.canX}}, + {"id", {.U16 = m2006->canInfo.sendID}}, + {"pos", {.U8 = m2006->canInfo.bufIndex}}, + {"len", {.U8 = 2}}, + {"data", {buffer}} + }); } -//õֵ +//设置电机期望值 void M2006_SetTarget(Motor* motor, float targetValue) { - M2006* m2006 = (M2006*)motor; - if(m2006->mode == MOTOR_ANGLE_MODE) - { - m2006->targetValue = M2006_DGR2CODE(targetValue, m2006->reductionRatio); - } - else if(m2006->mode == MOTOR_SPEED_MODE) - { - m2006->targetValue = targetValue*m2006->reductionRatio; - } - else - { - m2006->targetValue = targetValue; - } + M2006* m2006 = (M2006*)motor; + if(m2006->mode == MOTOR_ANGLE_MODE) + { + m2006->targetValue = M2006_DGR2CODE(targetValue, m2006->reductionRatio); + } + else if(m2006->mode == MOTOR_SPEED_MODE) + { + m2006->targetValue = targetValue*m2006->reductionRatio; + } + else + { + m2006->targetValue = targetValue; + } } -//лģʽ +//切换电机模式 void M2006_ChangeMode(Motor* motor, MotorCtrlMode mode) { - M2006* m2006 = (M2006*)motor; - if(m2006->mode == MOTOR_STOP_MODE) //ͣģʽ²лģʽ - return; - - if(m2006->mode == MOTOR_SPEED_MODE) - { - PID_Clear(&m2006->speedPID); - } - else if(m2006->mode == MOTOR_ANGLE_MODE) - { - PID_Clear(&m2006->anglePID.inner); - PID_Clear(&m2006->anglePID.outer); - } - m2006->mode = mode; + M2006* m2006 = (M2006*)motor; + if(m2006->mode == MOTOR_STOP_MODE) //急停模式下不允许切换模式 + return; + + if(m2006->mode == MOTOR_SPEED_MODE) + { + PID_Clear(&m2006->speedPID); + } + else if(m2006->mode == MOTOR_ANGLE_MODE) + { + PID_Clear(&m2006->anglePID.inner); + PID_Clear(&m2006->anglePID.outer); + } + m2006->mode = mode; } -//ȡ +//获取电机数据 float M2006_GetData(Motor* motor, const char* data) { - M2006* m2006 = (M2006*)motor; - if(!strcmp(data, "angle")) - { - return M2006_CODE2DGR(m2006->angle, 1); - } - else if(!strcmp(data, "totalAngle")) - { - return M2006_CODE2DGR(m2006->totalAngle, m2006->reductionRatio); - } - return 0; + M2006* m2006 = (M2006*)motor; + if(!strcmp(data, "angle")) + { + return M2006_CODE2DGR(m2006->angle, 1); + } + else if(!strcmp(data, "totalAngle")) + { + return M2006_CODE2DGR(m2006->totalAngle, m2006->reductionRatio); + } + return 0; } -//ͣ +//电机急停函数 void M2006_Stop(Motor* motor) { - M2006* m2006 = (M2006*)motor; - m2006->targetValue = 0; - m2006->mode = MOTOR_STOP_MODE; + M2006* m2006 = (M2006*)motor; + m2006->targetValue = 0; + m2006->mode = MOTOR_STOP_MODE; } -//µ(ܽ˲) +//更新电机数据(可能进行滤波) void M2006_Update(M2006* m2006,uint8_t* data) { - m2006->angle = (data[0]<<8 | data[1]); - m2006->speed = (data[2]<<8 | data[3]); - m2006->torque = (data[4]<<8 | data[5]); + m2006->angle = (data[0]<<8 | data[1]); + m2006->speed = (data[2]<<8 | data[3]); + m2006->torque = (data[4]<<8 | data[5]); } diff --git a/tools/motor/motor_can/m3508.c b/tools/motor/motor_can/m3508.c index b8a841c596f79d7497e106caedefb8b64d151403..a167b06511e624ec0027ae91acd12036666cb6ca 100644 --- a/tools/motor/motor_can/m3508.c +++ b/tools/motor/motor_can/m3508.c @@ -3,8 +3,8 @@ #include "pid.h" #include "config.h" -//ֵֵǶȵĻ -#define M3508_DGR2CODE(dgr,rdcr) ((int32_t)((dgr)*22.7528f*(rdcr))) //ٱ*8191/360 +//各种电机编码值与角度的换算 +#define M3508_DGR2CODE(dgr,rdcr) ((int32_t)((dgr)*22.7528f*(rdcr))) //减速比*8191/360 #define M3508_CODE2DGR(code,rdcr) ((float)((code)/(22.7528f*(rdcr)))) typedef struct _M3508 @@ -22,16 +22,16 @@ typedef struct _M3508 int16_t angle,speed,torque; - int16_t lastAngle;//¼һεõĽǶ + int16_t lastAngle;//记录上一次得到的角度 - int32_t totalAngle;//ۼתıֵ + int32_t totalAngle;//累计转过的编码器值 - float targetValue;//Ŀֵ(Ťؾ/ٶ/Ƕ(λ)) + float targetValue;//目标值(输出轴扭矩矩/速度/角度(单位度)) - uint16_t stallTime;//תʱ + uint16_t stallTime;//堵转时间 - PID speedPID;//ٶpid() - CascadePID anglePID;//Ƕpid + PID speedPID;//速度pid(单级) + CascadePID anglePID;//角度pid，串级 char* stallName; }M3508; @@ -50,7 +50,7 @@ void M3508_PIDInit(M3508* m3508, ConfItem* dict); void M3508_StatAngle(M3508* m3508); void M3508_CtrlerCalc(M3508* m3508, float reference); -//ʱص +//软件定时器回调函数 void M3508_TimerCallback(void const *argument) { M3508* m3508 = pvTimerGetTimerID((TimerHandle_t)argument); @@ -66,28 +66,28 @@ void M3508_TimerCallback(void const *argument) Motor* M3508_Init(ConfItem* dict) { - //ڴռ + //分配子类内存空间 M3508* m3508 = MOTOR_MALLOC_PORT(sizeof(M3508)); memset(m3508,0,sizeof(M3508)); - //̬ + //子类多态 m3508->motor.setTarget = M3508_SetTarget; m3508->motor.changeMode = M3508_ChangeMode; m3508->motor.initTotalAngle = M3508_InitTotalAngle; m3508->motor.getData = M3508_GetData; m3508->motor.stop = M3508_Stop; - //ٱ - m3508->reductionRatio = Conf_GetValue(dict, "reduction-ratio", float, 19.2f);//δõٱȲʹԭװĬϼٱ - //ʼcanϢ + //电机减速比 + m3508->reductionRatio = Conf_GetValue(dict, "reduction-ratio", float, 19.2f);//如果未配置电机减速比参数，则使用原装电机默认减速比 + //初始化电机绑定can信息 uint16_t id = Conf_GetValue(dict, "id", uint16_t, 0); m3508->canInfo.recvID = id + 0x200; m3508->canInfo.sendID = (id <= 4) ? 0x200 : 0x1FF; m3508->canInfo.bufIndex = ((id - 1)%4) * 2; m3508->canInfo.canX = Conf_GetValue(dict, "can-x", uint8_t, 0); - //õĬģʽΪŤģʽ + //设置电机默认模式为扭矩模式 m3508->mode = MOTOR_TORQUE_MODE; - //ʼpid + //初始化电机pid M3508_PIDInit(m3508, dict); - //canϢ + //订阅can信息 char name[] = "/can_/recv"; name[4] = m3508->canInfo.canX + '0'; Bus_RegisterReceiver(m3508, M3508_SoftBusCallback, name); @@ -95,23 +95,23 @@ Motor* M3508_Init(ConfItem* dict) char* motorName = Conf_GetPtr(dict, "name", char); motorName = motorName ? motorName : "motor"; uint8_t len = strlen(motorName); - m3508->stallName = MOTOR_MALLOC_PORT(len + 7+ 1); //7Ϊ"/ /stall"ijȣ1Ϊ'\0'ij + m3508->stallName = MOTOR_MALLOC_PORT(len + 7+ 1); //7为"/ /stall"的长度，1为'\0'的长度 sprintf(m3508->stallName, "/%s/stall", motorName); - //ʱ + //开启软件定时器 osTimerDef(M3508, M3508_TimerCallback); osTimerStart(osTimerCreate(osTimer(M3508), osTimerPeriodic, m3508), 2); return (Motor*)m3508; } -//ʼpid +//初始化pid void M3508_PIDInit(M3508* m3508, ConfItem* dict) { PID_Init(&m3508->speedPID, Conf_GetPtr(dict, "speed-pid", ConfItem)); PID_Init(&m3508->anglePID.inner, Conf_GetPtr(dict, "angle-pid/inner", ConfItem)); PID_Init(&m3508->anglePID.outer, Conf_GetPtr(dict, "angle-pid/outer", ConfItem)); - PID_SetMaxOutput(&m3508->anglePID.outer, m3508->anglePID.outer.maxOutput*m3508->reductionRatio);//ٶ޷Ŵת + PID_SetMaxOutput(&m3508->anglePID.outer, m3508->anglePID.outer.maxOutput*m3508->reductionRatio);//将输出轴速度限幅放大到转子上 } -//߻ص +//软总线回调函数 void M3508_SoftBusCallback(const char* name, SoftBusFrame* frame, void* bindData) { M3508* m3508 = (M3508*)bindData; @@ -125,7 +125,7 @@ void M3508_SoftBusCallback(const char* name, SoftBusFrame* frame, void* bindData M3508_Update(m3508, data); } -//ͣص +//电机急停回调函数 void M3508_StopCallback(const char* name, SoftBusFrame* frame, void* bindData) { M3508* m3508 = (M3508*)bindData; @@ -133,7 +133,7 @@ void M3508_StopCallback(const char* name, SoftBusFrame* frame, void* bindData) m3508->mode = MOTOR_STOP_MODE; } -//ʼͳƵۼƽǶ +//开始统计电机累计角度 void M3508_InitTotalAngle(Motor *motor, float startAngle) { M3508* m3508 = (M3508*)motor; @@ -142,7 +142,7 @@ void M3508_InitTotalAngle(Motor *motor, float startAngle) m3508->lastAngle=m3508->angle; } -//ͳƵۼתȦ +//统计电机累计转过的圈数 void M3508_StatAngle(M3508* m3508) { int32_t dAngle=0; @@ -152,12 +152,12 @@ void M3508_StatAngle(M3508* m3508) dAngle=-m3508->lastAngle-(8191-m3508->angle); else dAngle=m3508->angle-m3508->lastAngle; - //Ƕ + //将角度增量加入计数器 m3508->totalAngle+=dAngle; - //¼Ƕ + //记录角度 m3508->lastAngle=m3508->angle; } -//ģʽ +//控制器根据模式计算输出 void M3508_CtrlerCalc(M3508* m3508, float reference) { int16_t output=0; @@ -178,15 +178,14 @@ void M3508_CtrlerCalc(M3508* m3508, float reference) } buffer[0] = (output>>8)&0xff; buffer[1] = (output)&0xff; - Bus_RemoteCall("/can/set-buf",{ - {"can-x", &m3508->canInfo.canX}, - {"id", &m3508->canInfo.sendID}, - {"pos", &m3508->canInfo.bufIndex}, - {"len", &(uint8_t){2}}, - {"data", buffer} + Bus_RemoteCall("/can/set-buf",{{"can-x", {.U8 = m3508->canInfo.canX}}, + {"id", {.U16 = m3508->canInfo.sendID}}, + {"pos", {.U8 = m3508->canInfo.bufIndex}}, + {"len", {.U8 = 2}}, + {"data", {buffer}} }); } -//õֵ +//设置电机期望值 void M3508_SetTarget(Motor* motor, float targetValue) { M3508* m3508 = (M3508*)motor; @@ -203,11 +202,11 @@ void M3508_SetTarget(Motor* motor, float targetValue) m3508->targetValue = targetValue; } } -//лģʽ +//切换电机模式 void M3508_ChangeMode(Motor* motor, MotorCtrlMode mode) { M3508* m3508 = (M3508*)motor; - if(m3508->mode == MOTOR_STOP_MODE) //ͣģʽ²лģʽ + if(m3508->mode == MOTOR_STOP_MODE) //急停模式下不允许切换模式 return; if(m3508->mode == MOTOR_SPEED_MODE) @@ -222,7 +221,7 @@ void M3508_ChangeMode(Motor* motor, MotorCtrlMode mode) m3508->mode = mode; } -//ȡ +//获取电机数据 float M3508_GetData(Motor* motor, const char* data) { M3508* m3508 = (M3508*)motor; @@ -238,7 +237,7 @@ float M3508_GetData(Motor* motor, const char* data) return 0; } -//ͣ +//电机急停函数 void M3508_Stop(Motor* motor) { M3508* m3508 = (M3508*)motor; @@ -246,7 +245,7 @@ void M3508_Stop(Motor* motor) m3508->mode = MOTOR_STOP_MODE; } -//µ(ܽ˲) +//更新电机数据(可能进行滤波) void M3508_Update(M3508* m3508,uint8_t* data) { m3508->angle = (data[0]<<8 | data[1]); diff --git a/tools/motor/motor_can/m6020.c b/tools/motor/motor_can/m6020.c index 880be2221c5bf2c3d37e2216258ecd9abce93ded..4944327f86bc8f9a44b027f14c3f006ac3189700 100644 --- a/tools/motor/motor_can/m6020.c +++ b/tools/motor/motor_can/m6020.c @@ -3,33 +3,33 @@ #include "pid.h" #include "config.h" -//ֵֵǶȵĻ -#define M6020_DGR2CODE(dgr,rdcr) ((int32_t)((dgr)*22.7528f*(rdcr))) //ٱ*8191/360 +//各种电机编码值与角度的换算 +#define M6020_DGR2CODE(dgr,rdcr) ((int32_t)((dgr)*22.7528f*(rdcr))) //减速比*8191/360 #define M6020_CODE2DGR(code,rdcr) ((float)((code)/(22.7528f*(rdcr)))) typedef struct _M6020 { - Motor motor; - - float reductionRatio; - struct - { - uint16_t sendID,recvID; - uint8_t canX; - uint8_t bufIndex; - }canInfo; - MotorCtrlMode mode; - - int16_t angle,speed; - - int16_t lastAngle;//¼һεõĽǶ - - int32_t totalAngle;//ۼתıֵ - - float targetValue;//Ŀֵ(Ťؾ/ٶ/Ƕ(λ)) - - PID speedPID;//ٶpid() - CascadePID anglePID;//Ƕpid + Motor motor; + + float reductionRatio; + struct + { + uint16_t sendID,recvID; + uint8_t canX; + uint8_t bufIndex; + }canInfo; + MotorCtrlMode mode; + + int16_t angle,speed; + + int16_t lastAngle;//记录上一次得到的角度 + + int32_t totalAngle;//累计转过的编码器值 + + float targetValue;//目标值(输出轴扭矩矩/速度/角度(单位度)) + + PID speedPID;//速度pid(单级) + CascadePID anglePID;//角度pid，串级 }M6020; Motor* M6020_Init(ConfItem* dict); @@ -46,193 +46,191 @@ void M6020_PIDInit(M6020* m6020, ConfItem* dict); void M6020_StatAngle(M6020* m6020); void M6020_CtrlerCalc(M6020* m6020, float reference); -//ʱص +//软件定时器回调函数 void M6020_TimerCallback(void const *argument) { - M6020* m6020 = pvTimerGetTimerID((TimerHandle_t)argument); - M6020_StatAngle(m6020); - M6020_CtrlerCalc(m6020, m6020->targetValue); + M6020* m6020 = pvTimerGetTimerID((TimerHandle_t)argument); + M6020_StatAngle(m6020); + M6020_CtrlerCalc(m6020, m6020->targetValue); } Motor* M6020_Init(ConfItem* dict) { - //ڴռ - M6020* m6020 = MOTOR_MALLOC_PORT(sizeof(M6020)); - memset(m6020,0,sizeof(M6020)); - //̬ - m6020->motor.setTarget = M6020_SetTarget; - m6020->motor.changeMode = M6020_ChangeMode; - m6020->motor.initTotalAngle = M6020_InitTotalAngle; - m6020->motor.getData = M6020_GetData; - m6020->motor.stop = M6020_Stop; - //ٱ - m6020->reductionRatio = Conf_GetValue(dict, "reduction-ratio", float, 1);//δõٱȲʹԭװĬϼٱ - //ʼcanϢ - uint16_t id = Conf_GetValue(dict, "id", uint16_t, 0); - m6020->canInfo.recvID = id + 0x204; - m6020->canInfo.sendID = (id <= 4) ? 0x1FF : 0x2FF; - m6020->canInfo.bufIndex = ((id - 1)%4) * 2; - m6020->canInfo.canX = Conf_GetValue(dict, "can-x", uint8_t, 0); - //õĬģʽΪŤģʽ - m6020->mode = MOTOR_TORQUE_MODE; - //ʼpid - M6020_PIDInit(m6020, dict); - //canϢ - char name[] = "/can_/recv"; - name[4] = m6020->canInfo.canX + '0'; - Bus_RegisterReceiver(m6020, M6020_SoftBusCallback, name); - //ʱ - osTimerDef(M6020, M6020_TimerCallback); - osTimerStart(osTimerCreate(osTimer(M6020), osTimerPeriodic, m6020), 2); - - return (Motor*)m6020; + //分配子类内存空间 + M6020* m6020 = MOTOR_MALLOC_PORT(sizeof(M6020)); + memset(m6020,0,sizeof(M6020)); + //子类多态 + m6020->motor.setTarget = M6020_SetTarget; + m6020->motor.changeMode = M6020_ChangeMode; + m6020->motor.initTotalAngle = M6020_InitTotalAngle; + m6020->motor.getData = M6020_GetData; + m6020->motor.stop = M6020_Stop; + //电机减速比 + m6020->reductionRatio = Conf_GetValue(dict, "reduction-ratio", float, 1);//如果未配置电机减速比参数，则使用原装电机默认减速比 + //初始化电机绑定can信息 + uint16_t id = Conf_GetValue(dict, "id", uint16_t, 0); + m6020->canInfo.recvID = id + 0x204; + m6020->canInfo.sendID = (id <= 4) ? 0x1FF : 0x2FF; + m6020->canInfo.bufIndex = ((id - 1)%4) * 2; + m6020->canInfo.canX = Conf_GetValue(dict, "can-x", uint8_t, 0); + //设置电机默认模式为扭矩模式 + m6020->mode = MOTOR_TORQUE_MODE; + //初始化电机pid + M6020_PIDInit(m6020, dict); + //订阅can信息 + char name[] = "/can_/recv"; + name[4] = m6020->canInfo.canX + '0'; + Bus_RegisterReceiver(m6020, M6020_SoftBusCallback, name); + //开启软件定时器 + osTimerDef(M6020, M6020_TimerCallback); + osTimerStart(osTimerCreate(osTimer(M6020), osTimerPeriodic, m6020), 2); + + return (Motor*)m6020; } -//ʼpid +//初始化pid void M6020_PIDInit(M6020* m6020, ConfItem* dict) { - PID_Init(&m6020->speedPID, Conf_GetPtr(dict, "speed-pid", ConfItem)); - PID_Init(&m6020->anglePID.inner, Conf_GetPtr(dict, "angle-pid/inner", ConfItem)); - PID_Init(&m6020->anglePID.outer, Conf_GetPtr(dict, "angle-pid/outer", ConfItem)); - PID_SetMaxOutput(&m6020->anglePID.outer, m6020->anglePID.outer.maxOutput*m6020->reductionRatio);//ٶ޷Ŵת + PID_Init(&m6020->speedPID, Conf_GetPtr(dict, "speed-pid", ConfItem)); + PID_Init(&m6020->anglePID.inner, Conf_GetPtr(dict, "angle-pid/inner", ConfItem)); + PID_Init(&m6020->anglePID.outer, Conf_GetPtr(dict, "angle-pid/outer", ConfItem)); + PID_SetMaxOutput(&m6020->anglePID.outer, m6020->anglePID.outer.maxOutput*m6020->reductionRatio);//将输出轴速度限幅放大到转子上 } -//߻ص +//软总线回调函数 void M6020_SoftBusCallback(const char* name, SoftBusFrame* frame, void* bindData) { - M6020* m6020 = (M6020*)bindData; - - uint16_t id = *(uint16_t*)Bus_GetListValue(frame, 0); - if(id != m6020->canInfo.recvID) - return; - - uint8_t* data = (uint8_t*)Bus_GetListValue(frame, 1); - if(data) - M6020_Update(m6020, data); + M6020* m6020 = (M6020*)bindData; + + uint16_t id = *(uint16_t*)Bus_GetListValue(frame, 0); + if(id != m6020->canInfo.recvID) + return; + + uint8_t* data = (uint8_t*)Bus_GetListValue(frame, 1); + if(data) + M6020_Update(m6020, data); } -//ͣص +//电机急停回调函数 void M6020_StopCallback(const char* name, SoftBusFrame* frame, void* bindData) { - M6020* m6020 = (M6020*)bindData; - m6020->targetValue = 0; - m6020->mode = MOTOR_STOP_MODE; + M6020* m6020 = (M6020*)bindData; + m6020->targetValue = 0; + m6020->mode = MOTOR_STOP_MODE; } -//ʼͳƵۼƽǶ +//开始统计电机累计角度 void M6020_InitTotalAngle(Motor *motor, float startAngle) { - M6020* m6020 = (M6020*)motor; - - m6020->totalAngle=M6020_DGR2CODE(startAngle, m6020->reductionRatio); - m6020->lastAngle=m6020->angle; + M6020* m6020 = (M6020*)motor; + + m6020->totalAngle=M6020_DGR2CODE(startAngle, m6020->reductionRatio); + m6020->lastAngle=m6020->angle; } -//ͳƵۼתȦ +//统计电机累计转过的圈数 void M6020_StatAngle(M6020* m6020) { - int32_t dAngle=0; - if(m6020->angle-m6020->lastAngle<-4000) - dAngle=m6020->angle+(8191-m6020->lastAngle); - else if(m6020->angle-m6020->lastAngle>4000) - dAngle=-m6020->lastAngle-(8191-m6020->angle); - else - dAngle=m6020->angle-m6020->lastAngle; - //Ƕ - m6020->totalAngle+=dAngle; - //¼Ƕ - m6020->lastAngle=m6020->angle; + int32_t dAngle=0; + if(m6020->angle-m6020->lastAngle<-4000) + dAngle=m6020->angle+(8191-m6020->lastAngle); + else if(m6020->angle-m6020->lastAngle>4000) + dAngle=-m6020->lastAngle-(8191-m6020->angle); + else + dAngle=m6020->angle-m6020->lastAngle; + //将角度增量加入计数器 + m6020->totalAngle+=dAngle; + //记录角度 + m6020->lastAngle=m6020->angle; } -//ģʽ +//控制器根据模式计算输出 void M6020_CtrlerCalc(M6020* m6020, float reference) { - int16_t output=0; - uint8_t buffer[2]={0}; - if(m6020->mode == MOTOR_SPEED_MODE) - { - PID_SingleCalc(&m6020->speedPID, reference, m6020->speed); - output = m6020->speedPID.output; - } - else if(m6020->mode == MOTOR_ANGLE_MODE) - { - PID_CascadeCalc(&m6020->anglePID, reference, m6020->totalAngle, m6020->speed); - output = m6020->anglePID.output; - } - else if(m6020->mode == MOTOR_TORQUE_MODE) - { - output = (int16_t)reference; - } - buffer[0] = (output>>8)&0xff; - buffer[1] = (output)&0xff; - Bus_RemoteCall("/can/set-buf",{ - {"can-x", &m6020->canInfo.canX}, - {"id", &m6020->canInfo.sendID}, - {"pos", &m6020->canInfo.bufIndex}, - {"len", &(uint8_t){2}}, - {"data", buffer} - }); + int16_t output=0; + uint8_t buffer[2]={0}; + if(m6020->mode == MOTOR_SPEED_MODE) + { + PID_SingleCalc(&m6020->speedPID, reference, m6020->speed); + output = m6020->speedPID.output; + } + else if(m6020->mode == MOTOR_ANGLE_MODE) + { + PID_CascadeCalc(&m6020->anglePID, reference, m6020->totalAngle, m6020->speed); + output = m6020->anglePID.output; + } + else if(m6020->mode == MOTOR_TORQUE_MODE) + { + output = (int16_t)reference; + } + buffer[0] = (output>>8)&0xff; + buffer[1] = (output)&0xff; + Bus_RemoteCall("/can/set-buf",{{"can-x", {.U8 = m6020->canInfo.canX}}, + {"id", {.U16 = m6020->canInfo.sendID}}, + {"pos", {.U8 = m6020->canInfo.bufIndex}}, + {"len", {.U8 = 2}}, + {"data", {buffer}}}); } -//õֵ +//设置电机期望值 void M6020_SetTarget(Motor* motor, float targetValue) { - M6020* m6020 = (M6020*)motor; - if(m6020->mode == MOTOR_ANGLE_MODE) - { - m6020->targetValue = M6020_DGR2CODE(targetValue, m6020->reductionRatio); - } - else if(m6020->mode == MOTOR_SPEED_MODE) - { - m6020->targetValue = targetValue*m6020->reductionRatio; - } - else - { - m6020->targetValue = targetValue; - } + M6020* m6020 = (M6020*)motor; + if(m6020->mode == MOTOR_ANGLE_MODE) + { + m6020->targetValue = M6020_DGR2CODE(targetValue, m6020->reductionRatio); + } + else if(m6020->mode == MOTOR_SPEED_MODE) + { + m6020->targetValue = targetValue*m6020->reductionRatio; + } + else + { + m6020->targetValue = targetValue; + } } -//лģʽ +//切换电机模式 void M6020_ChangeMode(Motor* motor, MotorCtrlMode mode) { - M6020* m6020 = (M6020*)motor; - if(m6020->mode == MOTOR_STOP_MODE) //ͣģʽ²лģʽ - return; - - if(m6020->mode == MOTOR_SPEED_MODE) - { - PID_Clear(&m6020->speedPID); - } - else if(m6020->mode == MOTOR_ANGLE_MODE) - { - PID_Clear(&m6020->anglePID.inner); - PID_Clear(&m6020->anglePID.outer); - } - m6020->mode = mode; + M6020* m6020 = (M6020*)motor; + if(m6020->mode == MOTOR_STOP_MODE) //急停模式下不允许切换模式 + return; + + if(m6020->mode == MOTOR_SPEED_MODE) + { + PID_Clear(&m6020->speedPID); + } + else if(m6020->mode == MOTOR_ANGLE_MODE) + { + PID_Clear(&m6020->anglePID.inner); + PID_Clear(&m6020->anglePID.outer); + } + m6020->mode = mode; } -//ȡ +//获取电机数据 float M6020_GetData(Motor* motor, const char* data) { - M6020* m6020 = (M6020*)motor; - - if(!strcmp(data, "angle")) - { - return M6020_CODE2DGR(m6020->angle, 1); - } - else if(!strcmp(data, "totalAngle")) - { - return M6020_CODE2DGR(m6020->totalAngle, m6020->reductionRatio); - } - return 0; + M6020* m6020 = (M6020*)motor; + + if(!strcmp(data, "angle")) + { + return M6020_CODE2DGR(m6020->angle, 1); + } + else if(!strcmp(data, "totalAngle")) + { + return M6020_CODE2DGR(m6020->totalAngle, m6020->reductionRatio); + } + return 0; } -//ͣ +//电机急停函数 void M6020_Stop(Motor* motor) { - M6020* m6020 = (M6020*)motor; - m6020->targetValue = 0; - m6020->mode = MOTOR_STOP_MODE; + M6020* m6020 = (M6020*)motor; + m6020->targetValue = 0; + m6020->mode = MOTOR_STOP_MODE; } -//µ(ܽ˲) +//更新电机数据(可能进行滤波) void M6020_Update(M6020* m6020,uint8_t* data) { - m6020->angle = (data[0]<<8 | data[1]); - m6020->speed = (data[2]<<8 | data[3]); + m6020->angle = (data[0]<<8 | data[1]); + m6020->speed = (data[2]<<8 | data[3]); } diff --git a/tools/motor/motor_pwm/dc_motor.c b/tools/motor/motor_pwm/dc_motor.c index d715ef8ad85142fe7d7be3e15b29e69d3b11bfce..9b5d9b00d0a0a04078b1549ecba9ea1acadab011 100644 --- a/tools/motor/motor_pwm/dc_motor.c +++ b/tools/motor/motor_pwm/dc_motor.c @@ -4,8 +4,8 @@ #include "pid.h" #include -//ֵֵǶȵĻ -#define DCMOTOR_DGR2CODE(dgr,rdcr,encode) ((int32_t)((dgr)*encode/360.0f*(rdcr))) //ٱ*һȦֵ/360 +//各种电机编码值与角度的换算 +#define DCMOTOR_DGR2CODE(dgr,rdcr,encode) ((int32_t)((dgr)*encode/360.0f*(rdcr))) //减速比*编码器一圈值/360 #define DCMOTOR_CODE2DGR(code,rdcr,encode) ((float)((code)/(encode/360.0f*(rdcr)))) typedef struct @@ -23,16 +23,16 @@ typedef struct _DCmotor MotorCtrlMode mode; - uint32_t angle,lastAngle;//¼һεõĽǶ + uint32_t angle,lastAngle;//记录上一次得到的角度 int16_t speed; - int32_t totalAngle;//ۼתıֵ + int32_t totalAngle;//累计转过的编码器值 - float targetValue;//Ŀֵ(/ٶ/Ƕ(λ)) + float targetValue;//目标值(输出轴/速度/角度(单位度)) - PID speedPID;//ٶpid() - CascadePID anglePID;//Ƕpid + PID speedPID;//速度pid(单级) + CascadePID anglePID;//角度pid，串级 }DcMotor; @@ -45,7 +45,7 @@ void DcMotor_ChangeMode(Motor* motor, MotorCtrlMode mode); void DcMotor_StatAngle(DcMotor* dcMotor); void DcMotor_CtrlerCalc(DcMotor* dcMotor, float reference); -//ʱص +//软件定时器回调函数 void DcMotor_TimerCallback(void const *argument) { DcMotor* dcMotor = pvTimerGetTimerID((TimerHandle_t)argument); @@ -55,40 +55,40 @@ void DcMotor_TimerCallback(void const *argument) Motor* DcMotor_Init(ConfItem* dict) { - //ڴռ + //分配子类内存空间 DcMotor* dcMotor = MOTOR_MALLOC_PORT(sizeof(DcMotor)); memset(dcMotor,0,sizeof(DcMotor)); - //̬ + //子类多态 dcMotor->motor.setTarget = DcMotor_SetTarget; dcMotor->motor.changeMode = DcMotor_ChangeMode; dcMotor->motor.initTotalAngle = DcMotor_InitTotalAngle; - //ٱ - dcMotor->reductionRatio = Conf_GetValue(dict, "reduction-ratio", float, 19.0f);//ٱȲ - dcMotor->circleEncode = Conf_GetValue(dict, "max-encode", float, 48.0f); //ƵתһȦֵ - //ʼtimϢ + //电机减速比 + dcMotor->reductionRatio = Conf_GetValue(dict, "reduction-ratio", float, 19.0f);//电机减速比参数 + dcMotor->circleEncode = Conf_GetValue(dict, "max-encode", float, 48.0f); //倍频后编码器转一圈的最大值 + //初始化电机绑定tim信息 DcMotor_TimInit(dcMotor, dict); - //õĬģʽΪٶģʽ + //设置电机默认模式为速度模式 dcMotor->mode = MOTOR_TORQUE_MODE; - //ʼpid + //初始化电机pid DcMotor_PIDInit(dcMotor, dict); - //ʱ + //开启软件定时器 osTimerDef(DCMOTOR, DcMotor_TimerCallback); osTimerStart(osTimerCreate(osTimer(DCMOTOR), osTimerPeriodic,dcMotor), 2); return (Motor*)dcMotor; } -//ʼpid +//初始化pid void DcMotor_PIDInit(DcMotor* dcMotor, ConfItem* dict) { PID_Init(&dcMotor->speedPID, Conf_GetPtr(dict, "speed-pid", ConfItem)); PID_Init(&dcMotor->anglePID.inner, Conf_GetPtr(dict, "angle-pid/inner", ConfItem)); PID_Init(&dcMotor->anglePID.outer, Conf_GetPtr(dict, "angle-pid/outer", ConfItem)); - PID_SetMaxOutput(&dcMotor->anglePID.outer, dcMotor->anglePID.outer.maxOutput*dcMotor->reductionRatio);//ٶ޷Ŵת + PID_SetMaxOutput(&dcMotor->anglePID.outer, dcMotor->anglePID.outer.maxOutput*dcMotor->reductionRatio);//将输出轴速度限幅放大到转子上 } -//ʼtim +//初始化tim void DcMotor_TimInit(DcMotor* dcMotor, ConfItem* dict) { dcMotor->posRotateTim.timX = Conf_GetValue(dict,"pos-rotate-tim/tim-x",uint8_t,0); @@ -98,7 +98,7 @@ void DcMotor_TimInit(DcMotor* dcMotor, ConfItem* dict) dcMotor->encodeTim.channelX = Conf_GetValue(dict,"encode-tim/tim-x",uint8_t,0); } -//ʼͳƵۼƽǶ +//开始统计电机累计角度 void DcMotor_InitTotalAngle(Motor *motor, float startAngle) { DcMotor* dcMotor = (DcMotor*)motor; @@ -107,13 +107,15 @@ void DcMotor_InitTotalAngle(Motor *motor, float startAngle) dcMotor->lastAngle=dcMotor->angle; } -//ͳƵۼתȦ +//统计电机累计转过的圈数 void DcMotor_StatAngle(DcMotor* dcMotor) { int32_t dAngle=0; uint32_t autoReload=0; - Bus_RemoteCall("/tim/encode",{{"tim-x",&dcMotor->encodeTim.timX},{"count",&dcMotor->angle},{"auto-reload",&autoReload}}); + Bus_RemoteCall("/tim/encode",{{"tim-x", {.U8 = dcMotor->encodeTim.timX}}, + {"count", {.U32 = dcMotor->angle}}, + {"auto-reload", {.U32 = autoReload}}}); if(dcMotor->angle - dcMotor->lastAngle < -(autoReload/2.0f)) dAngle = dcMotor->angle+(autoReload-dcMotor->lastAngle); @@ -121,15 +123,15 @@ void DcMotor_StatAngle(DcMotor* dcMotor) dAngle = -dcMotor->lastAngle - (autoReload - dcMotor->angle); else dAngle = dcMotor->angle - dcMotor->lastAngle; - //Ƕ + //将角度增量加入计数器 dcMotor->totalAngle += dAngle; - //ٶ + //计算速度 dcMotor->speed = (float)dAngle/(dcMotor->circleEncode*dcMotor->reductionRatio)*500*60;//rpm - //¼Ƕ + //记录角度 dcMotor->lastAngle=dcMotor->angle; } -//ģʽ +//控制器根据模式计算输出 void DcMotor_CtrlerCalc(DcMotor* dcMotor, float reference) { float output=0; @@ -150,19 +152,27 @@ void DcMotor_CtrlerCalc(DcMotor* dcMotor, float reference) if(output>0) { - Bus_RemoteCall("/tim/pwm/set-duty",{{"tim-x",&dcMotor->posRotateTim.timX},{"channel-x",&dcMotor->posRotateTim.channelX},{"duty",&output}}); - Bus_RemoteCall("/tim/pwm/set-duty",{{"tim-x",&dcMotor->posRotateTim.timX},{"channel-x",&dcMotor->negRotateTim.channelX},{"duty",IM_PTR(float, 0)}}); + Bus_RemoteCall("/tim/pwm/set-duty", {{"tim-x", {.U8 = dcMotor->posRotateTim.timX}}, + {"channel-x", {.U8 = dcMotor->posRotateTim.channelX}}, + {"duty", {.F32 = output}}}); + Bus_RemoteCall("/tim/pwm/set-duty", {{"tim-x", {.U8 = dcMotor->posRotateTim.timX}}, + {"channel-x", {.U8 = dcMotor->negRotateTim.channelX}}, + {"duty", {.F32 = 0.0f}}}); } else { output = ABS(output); - Bus_RemoteCall("/tim/pwm/set-duty",{{"tim-x",&dcMotor->posRotateTim.timX},{"channel-x",&dcMotor->posRotateTim.channelX},{"duty",IM_PTR(float, 0)}}); - Bus_RemoteCall("/tim/pwm/set-duty",{{"tim-x",&dcMotor->posRotateTim.timX},{"channel-x",&dcMotor->negRotateTim.channelX},{"duty",&output}}); + Bus_RemoteCall("/tim/pwm/set-duty", {{"tim-x", {.U8 = dcMotor->posRotateTim.timX}}, + {"channel-x", {.U8 = dcMotor->posRotateTim.channelX}}, + {"duty", {.F32 = 0.0f}}}); + Bus_RemoteCall("/tim/pwm/set-duty", {{"tim-x", {.U8 = dcMotor->posRotateTim.timX}}, + {"channel-x", {.U8 = dcMotor->negRotateTim.channelX}}, + {"duty", {.F32 = output}}}); } } -//õֵ +//设置电机期望值 void DcMotor_SetTarget(Motor* motor, float targetValue) { DcMotor* dcMotor = (DcMotor*)motor; @@ -180,7 +190,7 @@ void DcMotor_SetTarget(Motor* motor, float targetValue) } } -//лģʽ +//切换电机模式 void DcMotor_ChangeMode(Motor* motor, MotorCtrlMode mode) { DcMotor* dcMotor = (DcMotor*)motor; diff --git a/tools/motor/motor_pwm/servo.c b/tools/motor/motor_pwm/servo.c index 835a3cd9dea00cabb6fcabaad1967fe885e80ff2..5069d9c63d6e39c2075b37964e449dc1bf8ef151 100644 --- a/tools/motor/motor_pwm/servo.c +++ b/tools/motor/motor_pwm/servo.c @@ -10,7 +10,7 @@ typedef struct _Servo uint8_t timX; uint8_t channelX; }timInfo; - float targetAngle;//ĿǶ(λ) + float targetAngle;//目标角度(单位度) float maxAngle; float maxDuty,minDuty; }Servo; @@ -21,12 +21,12 @@ void Servo_SetTarget(Motor* motor,float targetValue); Motor* Servo_Init(ConfItem* dict) { - //ڴռ + //分配子类内存空间 Servo* servo = MOTOR_MALLOC_PORT(sizeof(Servo)); memset(servo,0,sizeof(Servo)); - //̬ + //子类多态 servo->motor.setTarget = Servo_SetTarget; - //ʼTIMϢ + //初始化电机绑定TIM信息 servo->timInfo.timX = Conf_GetValue(dict,"tim-x",uint8_t,0); servo->timInfo.channelX = Conf_GetValue(dict,"channel-x",uint8_t,0); servo->maxAngle = Conf_GetValue(dict,"max-angle",float,180); @@ -36,13 +36,15 @@ Motor* Servo_Init(ConfItem* dict) return (Motor*)servo; } -//öǶ +//设置舵机角度 void Servo_SetTarget(Motor* motor,float targetValue) { Servo* servo=(Servo*) motor; - servo->targetAngle = targetValue; //ʵ;debug + servo->targetAngle = targetValue; //无实际用途，可用于debug float pwmDuty = servo->targetAngle / servo->maxAngle * (servo->maxDuty - servo->minDuty) + servo->minDuty; - Bus_RemoteCall("/tim/pwm/set-duty", {{"tim-x", &servo->timInfo.timX}, {"channel-x", &servo->timInfo.channelX}, {"duty", &pwmDuty}}); + Bus_RemoteCall("/tim/pwm/set-duty", {{"tim-x", {.U8 = servo->timInfo.timX}}, + {"channel-x", {.U8 = servo->timInfo.channelX}}, + {"duty", {.F32 = pwmDuty}}}); } diff --git a/tools/slope/slope.c b/tools/slope/slope.c index 6b9641f9b01fcc37d9e37c0745826fd29ec791d1..b1a5b346cef30a167431b2f029b8d1e2bc2416ec 100644 --- a/tools/slope/slope.c +++ b/tools/slope/slope.c @@ -1,6 +1,6 @@ #include "slope.h" -//ʼб² +//初始化斜坡参数 void Slope_Init(Slope *slope,float step,float deadzone) { slope->target=0; @@ -9,27 +9,27 @@ void Slope_Init(Slope *slope,float step,float deadzone) slope->deadzone=deadzone; } -//趨бĿ +//设定斜坡目标 void Slope_SetTarget(Slope *slope,float target) { slope->target=target; } -//趨б² +//设定斜坡步长 void Slope_SetStep(Slope *slope,float step) { slope->step=step; } -//һбֵslope->valueظֵ +//计算下一个斜坡值，更新slope->value并返回该值 float Slope_NextVal(Slope *slope) { - float error=slope->value-slope->target;//ǰֵĿֵIJֵ + float error=slope->value-slope->target;//当前值与目标值的差值 - if(ABS(error)deadzone)//ǰֵ仯 + if(ABS(error)deadzone)//若误差在死区内则当前值不发生变化 return slope->value; - if(ABS(error)step)//㲽ǰֱֵΪĿֵ + if(ABS(error)step)//若误差不足步长则当前值直接设为目标值 slope->value=slope->target; else if(error<0) slope->value+=slope->step; @@ -38,7 +38,7 @@ float Slope_NextVal(Slope *slope) return slope->value; } -//ȡбµǰֵ +//获取斜坡当前值 float Slope_GetVal(Slope *slope) { return slope->value; diff --git a/tools/universal_queue/my_queue.c b/tools/universal_queue/my_queue.c index 5c61bbc05d5a2b0ac0c20b6097a9fef8d5591cd7..ecdfb54daed43113d6d709609188bfbca428456d 100644 --- a/tools/universal_queue/my_queue.c +++ b/tools/universal_queue/my_queue.c @@ -1,10 +1,10 @@ -/****************ͨѭ*****************/ +/****************通用循环队列*****************/ #include "my_queue.h" #include #include -//ʼ +//初始化队列 void Queue_Init(Queue *queue,uint16_t maxSize) { queue->data=malloc(maxSize*sizeof(void*)); @@ -13,8 +13,8 @@ void Queue_Init(Queue *queue,uint16_t maxSize) queue->maxSize=maxSize; } -//һݱ(ʱὫݸƵӦλ) -//עⱣССڶгȣԽ +//附加上一个数据保存区(入队时会将数据复制到保存区对应位置) +//注意保存区大小不得小于队列长度，否则会越界 void Queue_AttachBuffer(Queue *queue,void *buffer,uint8_t elemSize) { if(!queue->initialized) return; @@ -22,7 +22,7 @@ void Queue_AttachBuffer(Queue *queue,void *buffer,uint8_t elemSize) queue->bufElemSize=elemSize; } -//һ +//销毁一个队列 void Queue_Destroy(Queue *queue) { if(!queue->initialized) return; @@ -30,28 +30,28 @@ void Queue_Destroy(Queue *queue) memset(queue,0,sizeof(Queue)); } -//ȡг +//获取队列长度 uint16_t Queue_Size(Queue *queue) { if(!queue->initialized) return 0; return (queue->rear+queue->maxSize-queue->front)%queue->maxSize; } -//ǷΪ +//队列是否为满 uint8_t Queue_IsFull(Queue *queue) { if(!queue->initialized) return 1; return (queue->front==(queue->rear+1)%queue->maxSize)?1:0; } -//ǷΪ +//队列是否为空 uint8_t Queue_IsEmpty(Queue *queue) { if(!queue->initialized) return 1; return (queue->front==queue->rear)?1:0; } -//ȡָλõԪ(ͷԪλΪ0) +//获取队列指定位置的元素(队头元素位置为0) void* Queue_GetElement(Queue *queue,uint16_t position) { if(!queue->initialized) return NULL; @@ -61,7 +61,7 @@ void* Queue_GetElement(Queue *queue,uint16_t position) return queue->data[(queue->front+position)%queue->maxSize]; } -//ȡͷԪ() +//获取队头元素(但不出队) void* Queue_Top(Queue *queue) { if(!queue->initialized) return NULL; @@ -71,7 +71,7 @@ void* Queue_Top(Queue *queue) return queue->data[queue->front]; } -// +//出队 void* Queue_Dequeue(Queue *queue) { if(!queue->initialized) return NULL; @@ -83,19 +83,19 @@ void* Queue_Dequeue(Queue *queue) return res; } -// +//入队 void Queue_Enqueue(Queue *queue,void* data) { if(!queue->initialized) return; if(Queue_IsFull(queue)) return; - //ָΪ˵δӱԭʼָ + //如果缓冲区指针为空说明未附加保存区，入队原始指针 if(queue->buffer == NULL) { queue->data[queue->rear]=data; } - else//˱ݸƹӱָ + else//若加了保存区则将数据复制过来并入队保存区的指针 { memcpy((uint8_t*)(queue->buffer) + (queue->rear)*(queue->bufElemSize), data, diff --git a/tools/universal_vector/vector.c b/tools/universal_vector/vector.c index 3cea1b097fedbfb6bbfaa0854e88d49b69c82778..85366701840f8f4ba9221bde39a3dfb6605d8d26 100644 --- a/tools/universal_vector/vector.c +++ b/tools/universal_vector/vector.c @@ -1,14 +1,14 @@ #include "vector.h" -//ñ׼⺯ڴͨ޸ĺ궨ֲ -#include +//引用标准库函数用于内存操作，可通过修改宏定义进行移植 +#include #include #include "cmsis_os.h" #define VECTOR_MALLOC_PORT(len) pvPortMalloc(len) #define VECTOR_FREE_PORT(ptr) vPortFree(ptr) #define VECTOR_MEMCPY_PORT(dst,src,len) memcpy(dst,src,len) -//ʼ, ɹ0ʧܷ-1 +//初始化, 成功返回0失败返回-1 int _Vector_Init(Vector *vector, int _elementSize) { vector->elementSize=_elementSize; @@ -20,7 +20,7 @@ int _Vector_Init(Vector *vector, int _elementSize) return 0; } -//ʼһvector +//创建并初始化一个vector Vector _Vector_Create(int _elementSize) { Vector vector; @@ -28,19 +28,19 @@ Vector _Vector_Create(int _elementSize) return vector; } -//һvector +//销毁一个vector void _Vector_Destroy(Vector *vector) { if(vector->data) VECTOR_FREE_PORT(vector->data); } -//Ԫ, ɹ0ʧܷ-1 +//插入元素, 成功返回0失败返回-1 int _Vector_Insert(Vector *vector, uint32_t index, void *element) { if(!vector->data || index > vector->size) return -1; - if(vector->size+1 > vector->capacity) //ʣռ䲻㣬·ռ䲢 + if(vector->size+1 > vector->capacity) //若剩余空间不足，重新分配两倍空间并拷贝数据 { uint8_t *newBuf=VECTOR_MALLOC_PORT(vector->capacity * 2 * vector->elementSize); if(!newBuf) @@ -50,57 +50,57 @@ int _Vector_Insert(Vector *vector, uint32_t index, void *element) VECTOR_FREE_PORT(vector->data); vector->data=newBuf; } - if(vector->size > 0 && index <= vector->size-1) //ݺ + if(vector->size > 0 && index <= vector->size-1) //将插入点后的数据后移 { for(int32_t pos=vector->size-1; pos>=(int32_t)index; pos--) VECTOR_MEMCPY_PORT(vector->data + (pos+1) * vector->elementSize, vector->data + pos * vector->elementSize, vector->elementSize); } - VECTOR_MEMCPY_PORT(vector->data + index * vector->elementSize, element, vector->elementSize); //ûֵ + VECTOR_MEMCPY_PORT(vector->data + index * vector->elementSize, element, vector->elementSize); //拷贝用户传入的值 vector->size++; return 0; } -//ɾԪ, ɹ0ʧܷ-1 +//删除元素, 成功返回0失败返回-1 int _Vector_Remove(Vector *vector, uint32_t index) { if(!vector->data || vector->size==0 || index >= vector->size) return -1; - for(uint32_t pos=index; pos<(vector->size-1); pos++) //ɾǰ + for(uint32_t pos=index; pos<(vector->size-1); pos++) //将删除点后的数据前移 VECTOR_MEMCPY_PORT(vector->data + pos * vector->elementSize, vector->data + (pos+1) * vector->elementSize, vector->elementSize); vector->size--; return 0; } -//ȡָλϵԪָ, ʧܷNULL +//获取指定位置上的元素指针, 若失败返回NULL void *_Vector_GetByIndex(Vector *vector, uint32_t index) { if(index >= vector->size) return NULL; - return vector->data + index * vector->elementSize; //ƫƣصַ + return vector->data + index * vector->elementSize; //计算偏移，返回地址 } -//޸ָλϵԪ, ɹ0ʧܷ-1 +//修改指定位置上的元素, 成功返回0失败返回-1 int _Vector_SetValue(Vector *vector, uint32_t index, void *element) { if(index >= vector->size || !vector->data) return -1; - VECTOR_MEMCPY_PORT(vector->data + index * vector->elementSize, element, vector->elementSize); //ûֵĿַ + VECTOR_MEMCPY_PORT(vector->data + index * vector->elementSize, element, vector->elementSize); //拷贝用户传入的值到目标地址 return 0; } -//ɾĿռ, ɹ0ʧܷ-1 +//删除多余的空间, 成功返回0失败返回-1 int _Vector_TrimCapacity(Vector *vector) { if(!vector->data) return -1; if(vector->capacity <= vector->size) return 0; - int newCapacity=(vector->size ? vector->size : 1); //¿ռС - uint8_t *newBuf=VECTOR_MALLOC_PORT(newCapacity * vector->elementSize); //¿ռ + int newCapacity=(vector->size ? vector->size : 1); //计算新空间大小 + uint8_t *newBuf=VECTOR_MALLOC_PORT(newCapacity * vector->elementSize); //分配新空间 if(!newBuf) return -1; if(vector->size) - VECTOR_MEMCPY_PORT(newBuf, vector->data, vector->size * vector->elementSize); //ԭ + VECTOR_MEMCPY_PORT(newBuf, vector->data, vector->size * vector->elementSize); //拷贝原有数据 VECTOR_FREE_PORT(vector->data); vector->data=newBuf; vector->capacity=newCapacity;