From c021b4794c94b1a599bfb14f5447da4631311e4a Mon Sep 17 00:00:00 2001 From: TuxMonkey <8196772+tuxmonkey@user.noreply.gitee.com> Date: Fri, 5 Dec 2025 21:47:00 +0800 Subject: [PATCH] videotransmiter --- User_Code/bsp/pwm/bsp_pwm.c | 235 +++---- User_Code/bsp/pwm/bsp_pwm.h | 186 +++--- User_Code/module/algorithm/crc/crc.c | 191 ++++++ User_Code/module/algorithm/crc/crc.h | 33 + User_Code/module/algorithm/crc/crc16.c | 5 - User_Code/module/algorithm/crc/crc16.h | 8 - User_Code/module/algorithm/crc/crc8.c | 5 - User_Code/module/algorithm/crc/crc8.h | 8 - .../periph/remote_control/videotransmiter.c | 144 +++++ .../periph/remote_control/videotransmiter.h | 179 ++++++ .../module/software/powercontrol/Martix.hpp | 416 ++++++++++++ .../module/software/powercontrol/RLS.hpp | 128 ++++ .../software/powercontrol/powercontrol.c | 594 +++++++++++++++++- .../software/powercontrol/powercontrol.h | 269 +++++++- .../software/powercontrol/powercontrol.md | 2 + 15 files changed, 2164 insertions(+), 239 deletions(-) create mode 100644 User_Code/module/algorithm/crc/crc.c create mode 100644 User_Code/module/algorithm/crc/crc.h delete mode 100644 User_Code/module/algorithm/crc/crc16.c delete mode 100644 User_Code/module/algorithm/crc/crc16.h delete mode 100644 User_Code/module/algorithm/crc/crc8.c delete mode 100644 User_Code/module/algorithm/crc/crc8.h create mode 100644 User_Code/module/periph/remote_control/videotransmiter.c create mode 100644 User_Code/module/periph/remote_control/videotransmiter.h create mode 100644 User_Code/module/software/powercontrol/Martix.hpp create mode 100644 User_Code/module/software/powercontrol/RLS.hpp create mode 100644 User_Code/module/software/powercontrol/powercontrol.md diff --git a/User_Code/bsp/pwm/bsp_pwm.c b/User_Code/bsp/pwm/bsp_pwm.c index 1e98063..b1ce07a 100644 --- a/User_Code/bsp/pwm/bsp_pwm.c +++ b/User_Code/bsp/pwm/bsp_pwm.c @@ -1,113 +1,122 @@ -// #include "bsp_pwm.h" -// #include "stdlib.h" -// #include "memory.h" -// -// // 配合中断以及初始化 -// static uint8_t idx; -// static PWMInstance *pwm_instance[PWM_DEVICE_CNT] = {NULL}; // 所有的pwm instance保存于此,用于callback时判断中断来源 -// static uint32_t PWMSelectTclk(TIM_HandleTypeDef *htim); -// -// /** -// * @brief pwm dma传输完成回调函数 -// * -// * @param htim 发生中断的定时器句柄 -// */ -// void HAL_TIM_PWM_PulseFinishedCallback(TIM_HandleTypeDef *htim) -// { -// for (uint8_t i = 0; i < idx; i++) -// { -// // 来自同一个定时器的中断且通道相同 -// if (pwm_instance[i]->htim == htim && htim->Channel == (1 << (pwm_instance[i]->channel / 4))) -// { -// if (pwm_instance[i]->callback) // 如果有回调函数 -// pwm_instance[i]->callback(pwm_instance[i]); -// return; // 一次只能有一个通道的中断,所以直接返回 -// } -// } -// } -// -// PWMInstance *PWMRegister(PWM_Init_Config_s *config) -// { -// if (idx >= PWM_DEVICE_CNT) // 超过最大实例数,考虑增加或查看是否有内存泄漏 -// while (1); -// PWMInstance *pwm = (PWMInstance *) malloc(sizeof(PWMInstance)); -// memset(pwm, 0, sizeof(PWMInstance)); -// -// pwm->htim = config->htim; -// pwm->channel = config->channel; -// pwm->period = config->period; -// pwm->dutyratio = config->dutyratio; -// pwm->callback = config->callback; -// pwm->id = config->id; -// pwm->tclk = PWMSelectTclk(pwm->htim); -// // 启动PWM -// HAL_TIM_PWM_Start(pwm->htim, pwm->channel); -// PWMSetPeriod(pwm, pwm->period); -// PWMSetDutyRatio(pwm, pwm->dutyratio); -// pwm_instance[idx++] = pwm; -// return pwm; -// } -// -// /* 只是对HAL的函数进行了形式上的封装 */ -// void PWMStart(PWMInstance *pwm) -// { -// HAL_TIM_PWM_Start(pwm->htim, pwm->channel); -// } -// -// /* 只是对HAL的函数进行了形式上的封装 */ -// void PWMStop(PWMInstance *pwm) -// { -// HAL_TIM_PWM_Stop(pwm->htim, pwm->channel); -// } -// -// /* -// * @brief 设置pwm周期 -// * -// * @param pwm pwm实例 -// * @param period 周期 单位 s -// */ -// void PWMSetPeriod(PWMInstance *pwm, float period) -// { -// __HAL_TIM_SetAutoreload(pwm->htim, period * ((pwm->tclk) / (pwm->htim->Init.Prescaler + 1))); -// } -// -// /* -// * @brief 设置pwm占空比 -// * -// * @param pwm pwm实例 -// * @param dutyratio 占空比 0~1 -// */ -// void PWMSetDutyRatio(PWMInstance *pwm, float dutyratio) -// { -// __HAL_TIM_SetCompare(pwm->htim, pwm->channel, dutyratio * (pwm->htim->Instance->ARR)); -// } -// -// /* 只是对HAL的函数进行了形式上的封装 */ -// void PWMStartDMA(PWMInstance *pwm, uint32_t *pData, uint32_t Size) -// { -// HAL_TIM_PWM_Start_DMA(pwm->htim, pwm->channel, pData, Size); -// } -// -// // 设置pwm对应定时器时钟源频率 -// //tim2~7,12~14:APB1 tim1,8~11:APB2 -// static uint32_t PWMSelectTclk(TIM_HandleTypeDef *htim) -// { -// uintptr_t tclk_temp = ((uintptr_t) ((htim)->Instance)); -// if ( -// (tclk_temp <= (APB1PERIPH_BASE + 0x2000UL)) && -// (tclk_temp >= (APB1PERIPH_BASE + 0x0000UL))) -// { -// return (HAL_RCC_GetPCLK1Freq() * ( -// APBPrescTable[(RCC->CFGR & RCC_CFGR_PPRE1) >> RCC_CFGR_PPRE1_Pos] == 0 ? 1 : 2)); -// } -// if ( -// ((tclk_temp <= (APB2PERIPH_BASE + 0x0400UL)) && -// (tclk_temp >= (APB2PERIPH_BASE + 0x0000UL))) || -// ((tclk_temp <= (APB2PERIPH_BASE + 0x4800UL)) && -// (tclk_temp >= (APB2PERIPH_BASE + 0x4000UL)))) -// { -// return (HAL_RCC_GetPCLK2Freq() * ( -// APBPrescTable[(RCC->CFGR & RCC_CFGR_PPRE1) >> RCC_CFGR_PPRE1_Pos] == 0 ? 1 : 2)); -// } -// return 0; -// } +#include "bsp_pwm.h" +#include "stdlib.h" +#include "memory.h" + +// 配合中断以及初始化 +static uint8_t idx; +static PWMInstance *pwm_instance[PWM_DEVICE_CNT] = {NULL}; // 所有的pwm instance保存于此,用于callback时判断中断来源 +static uint32_t PWMSelectTclk(TIM_HandleTypeDef *htim); + +/** + * @brief pwm dma传输完成回调函数 + * + * @param htim 发生中断的定时器句柄 + */ +void HAL_TIM_PWM_PulseFinishedCallback(TIM_HandleTypeDef *htim) +{ + for (uint8_t i = 0; i < idx; i++) + { + // 来自同一个定时器的中断且通道相同 + if (pwm_instance[i]->htim == htim && htim->Channel == (1 << (pwm_instance[i]->channel / 4))) + { + if (pwm_instance[i]->callback) // 如果有回调函数 + pwm_instance[i]->callback(pwm_instance[i]); + return; // 一次只能有一个通道的中断,所以直接返回 + } + } +} + +PWMInstance *PWMRegister(PWM_Init_Config_s *config) +{ + if (idx >= PWM_DEVICE_CNT) // 超过最大实例数,考虑增加或查看是否有内存泄漏 + while (1); + PWMInstance *pwm = (PWMInstance *) malloc(sizeof(PWMInstance)); + memset(pwm, 0, sizeof(PWMInstance)); + + pwm->htim = config->htim; + pwm->channel = config->channel; + pwm->period = config->period; + pwm->dutyratio = config->dutyratio; + pwm->callback = config->callback; + pwm->id = config->id; + pwm->tclk = PWMSelectTclk(pwm->htim); + // 启动PWM + HAL_TIM_PWM_Start(pwm->htim, pwm->channel); + PWMSetPeriod(pwm, pwm->period); + PWMSetDutyRatio(pwm, pwm->dutyratio); + pwm_instance[idx++] = pwm; + return pwm; +} + +/* 只是对HAL的函数进行了形式上的封装 */ +void PWMStart(PWMInstance *pwm) +{ + HAL_TIM_PWM_Start(pwm->htim, pwm->channel); +} + +/* 只是对HAL的函数进行了形式上的封装 */ +void PWMStop(PWMInstance *pwm) +{ + HAL_TIM_PWM_Stop(pwm->htim, pwm->channel); +} + +/* + * @brief 设置pwm周期 + * + * @param pwm pwm实例 + * @param period 周期 单位 s + */ +void PWMSetPeriod(PWMInstance *pwm, float period) +{ + __HAL_TIM_SetAutoreload(pwm->htim, period * ((pwm->tclk) / (pwm->htim->Init.Prescaler + 1))); +} + +/* + * @brief 设置pwm占空比 + * + * @param pwm pwm实例 + * @param dutyratio 占空比 0~1 +*/ +void PWMSetDutyRatio(PWMInstance *pwm, float dutyratio) +{ + __HAL_TIM_SetCompare(pwm->htim, pwm->channel, dutyratio * (pwm->htim->Instance->ARR)); +} + +/* 只是对HAL的函数进行了形式上的封装 */ +void PWMStartDMA(PWMInstance *pwm, uint32_t *pData, uint32_t Size) +{ + HAL_TIM_PWM_Start_DMA(pwm->htim, pwm->channel, pData, Size); +} + +// STM32H7 PWM时钟源选择函数（简洁版） +static uint32_t PWMSelectTclk(TIM_HandleTypeDef *htim) +{ + // 获取APB预分频器值 + uint32_t ppre1 = ((RCC->D2CFGR & RCC_D2CFGR_D2PPRE1) >> RCC_D2CFGR_D2PPRE1_Pos); + uint32_t ppre2 = ((RCC->D2CFGR & RCC_D2CFGR_D2PPRE2) >> RCC_D2CFGR_D2PPRE2_Pos); + + // 计算倍频因子 + uint32_t tim_mult1 = (ppre1 == 0b000 || ppre1 == 0b100) ? 1 : 2; + uint32_t tim_mult2 = (ppre2 == 0b000 || ppre2 == 0b100) ? 1 : 2; + + // 根据定时器实例判断 + TIM_TypeDef *tim = htim->Instance; + + if (tim == TIM1 || tim == TIM8 || tim == TIM15 || + tim == TIM16 || tim == TIM17) // 某些H7型号有TIM20 + { + // APB2上的定时器 + return HAL_RCC_GetPCLK2Freq() * tim_mult2; + } + else if (tim == TIM2 || tim == TIM3 || tim == TIM4 || + tim == TIM5 || tim == TIM12 || tim == TIM13 || tim == TIM14 || tim == TIM6 || tim == TIM7) + { + // APB1上的定时器 + return HAL_RCC_GetPCLK1Freq() * tim_mult1; + } + else if (tim == TIM23 || tim == TIM24) // 某些H7型号有这些定时器 + { + } + + return 0; // 未识别的定时器 +} diff --git a/User_Code/bsp/pwm/bsp_pwm.h b/User_Code/bsp/pwm/bsp_pwm.h index 1e61d42..a03a8a1 100644 --- a/User_Code/bsp/pwm/bsp_pwm.h +++ b/User_Code/bsp/pwm/bsp_pwm.h @@ -1,93 +1,93 @@ -// /** -// * @file bsp_pwm.h -// * @author TuxMonkey -// * @brief -// * @version 0.1 -// * @date 2023-02-14 -// * -// * @copyright Copyright (c) 2023 -// * -// */ -// -// #ifndef BSP_PWM_H -// #define BSP_PWM_H -// -// #include "tim.h" -// #include "stdint.h" -// #include "stm32h7xx_hal_rcc.h" -// #include "stm32h723xx.h" -// #define PWM_DEVICE_CNT 16 // 最大支持的PWM实例数量 -// -// /* pwm实例结构体 */ -// typedef struct pwm_ins_temp -// { -// TIM_HandleTypeDef *htim; // TIM句柄 -// uint32_t channel; // 通道 -// uint32_t tclk; // 时钟频率 -// float period; // 周期 -// float dutyratio; // 占空比 -// void (*callback)(struct pwm_ins_temp *); // DMA传输完成回调函数 -// void *id; // 实例ID -// } PWMInstance; -// -// typedef struct -// { -// TIM_HandleTypeDef *htim; // TIM句柄 -// uint32_t channel; // 通道 -// float period; // 周期 -// float dutyratio; // 占空比 -// void (*callback)(PWMInstance *); // DMA传输完成回调函数 -// void *id; // 实例ID -// } PWM_Init_Config_s; -// -// /** -// * @brief 注册一个pwm实例 -// * -// * @param config 初始化配置 -// * @return PWMInstance* -// */ -// PWMInstance *PWMRegister(PWM_Init_Config_s *config); -// -// /** -// * @brief 启动pwm -// * -// * @param pwm pwm实例 -// */ -// void PWMStart(PWMInstance *pwm); -// -// /** -// * @brief 设置pwm占空比 -// * -// * @param pwm pwm实例 -// * @param dutyratio 占空比 0~1 -// */ -// -// void PWMSetDutyRatio(PWMInstance *pwm, float dutyratio); -// -// /** -// * @brief 停止pwm -// * -// * @param pwm pwm实例 -// */ -// void PWMStop(PWMInstance *pwm); -// -// /** -// * @brief 设置pwm周期 -// * -// * @param pwm pwm实例 -// * @param period 周期 单位 s -// */ -// void PWMSetPeriod(PWMInstance *pwm, float period); -// -// /** -// * @brief 启动pwm dma传输 -// * -// * @param pwm pwm实例 -// * @param pData 数据首地址指针,注意数据的位数必须和CubeMX配置的DMA传输位数(字长)一致 -// * @param Size 数据长度 -// * @note 如果使用此函数,则需要在CubeMX中配置DMA传输位数为对应位数 -// * 例如:使用16位数据,则需要配置DMA传输位数为16位(half word),配置错误会导致指针越界或数据错误 -// */ -// void PWMStartDMA(PWMInstance *pwm, uint32_t *pData, uint32_t Size); -// -// #endif // BSP_PWM_H +/** +* @file bsp_pwm.h + * @author TuxMonkey + * @brief + * @version 0.1 + * @date 2023-02-14 + * + * @copyright Copyright (c) 2023 + * + */ + +#ifndef BSP_PWM_H +#define BSP_PWM_H + +#include "tim.h" +#include "stdint.h" +#include "stm32h7xx_hal_rcc.h" +#include "stm32h723xx.h" +#define PWM_DEVICE_CNT 16 // 最大支持的PWM实例数量 + +/* pwm实例结构体 */ +typedef struct pwm_ins_temp +{ + TIM_HandleTypeDef *htim; // TIM句柄 + uint32_t channel; // 通道 + uint32_t tclk; // 时钟频率 + float period; // 周期 + float dutyratio; // 占空比 + void (*callback)(struct pwm_ins_temp *); // DMA传输完成回调函数 + void *id; // 实例ID +} PWMInstance; + +typedef struct +{ + TIM_HandleTypeDef *htim; // TIM句柄 + uint32_t channel; // 通道 + float period; // 周期 + float dutyratio; // 占空比 + void (*callback)(PWMInstance *); // DMA传输完成回调函数 + void *id; // 实例ID +} PWM_Init_Config_s; + +/** + * @brief 注册一个pwm实例 + * + * @param config 初始化配置 + * @return PWMInstance* + */ +PWMInstance *PWMRegister(PWM_Init_Config_s *config); + +/** + * @brief 启动pwm + * + * @param pwm pwm实例 + */ +void PWMStart(PWMInstance *pwm); + +/** + * @brief 设置pwm占空比 + * + * @param pwm pwm实例 + * @param dutyratio 占空比 0~1 + */ + +void PWMSetDutyRatio(PWMInstance *pwm, float dutyratio); + +/** + * @brief 停止pwm + * + * @param pwm pwm实例 + */ +void PWMStop(PWMInstance *pwm); + +/** + * @brief 设置pwm周期 + * + * @param pwm pwm实例 + * @param period 周期 单位 s + */ +void PWMSetPeriod(PWMInstance *pwm, float period); + +/** + * @brief 启动pwm dma传输 + * + * @param pwm pwm实例 + * @param pData 数据首地址指针,注意数据的位数必须和CubeMX配置的DMA传输位数(字长)一致 + * @param Size 数据长度 + * @note 如果使用此函数,则需要在CubeMX中配置DMA传输位数为对应位数 + * 例如:使用16位数据,则需要配置DMA传输位数为16位(half word),配置错误会导致指针越界或数据错误 + */ +void PWMStartDMA(PWMInstance *pwm, uint32_t *pData, uint32_t Size); + +#endif // BSP_PWM_H diff --git a/User_Code/module/algorithm/crc/crc.c b/User_Code/module/algorithm/crc/crc.c new file mode 100644 index 0000000..154d2db --- /dev/null +++ b/User_Code/module/algorithm/crc/crc.c @@ -0,0 +1,191 @@ +/* USER CODE BEGIN Header */ +/** + ****************************************************************************** + * @file : CRC.c + * @brief : CRC check + * @author : + * @date : 2025/01/22 + * @version : v1.0 + ****************************************************************************** + * @attention : To be tested + ****************************************************************************** + */ +/* USER CODE END Header */ + +#include "CRC.h" +#include "string.h" + +//crc8 generator polynomial:G(x=x8+x5+x4+1 +const uint8_t CRC8_INIT = 0xff; + +const uint8_t CRC8_TAB[256] = +{ + 0x00, 0x5e, 0xbc, 0xe2, 0x61, 0x3f, 0xdd, 0x83, 0xc2, 0x9c, 0x7e, 0x20, 0xa3, 0xfd, 0x1f, 0x41, + 0x9d, 0xc3, 0x21, 0x7f, 0xfc, 0xa2, 0x40, 0x1e, 0x5f, 0x01, 0xe3, 0xbd, 0x3e, 0x60, 0x82, 0xdc, + 0x23, 0x7d, 0x9f, 0xc1, 0x42, 0x1c, 0xfe, 0xa0, 0xe1, 0xbf, 0x5d, 0x03, 0x80, 0xde, 0x3c, 0x62, + 0xbe, 0xe0, 0x02, 0x5c, 0xdf, 0x81, 0x63, 0x3d, 0x7c, 0x22, 0xc0, 0x9e, 0x1d, 0x43, 0xa1, 0xff, + 0x46, 0x18, 0xfa, 0xa4, 0x27, 0x79, 0x9b, 0xc5, 0x84, 0xda, 0x38, 0x66, 0xe5, 0xbb, 0x59, 0x07, + 0xdb, 0x85, 0x67, 0x39, 0xba, 0xe4, 0x06, 0x58, 0x19, 0x47, 0xa5, 0xfb, 0x78, 0x26, 0xc4, 0x9a, + 0x65, 0x3b, 0xd9, 0x87, 0x04, 0x5a, 0xb8, 0xe6, 0xa7, 0xf9, 0x1b, 0x45, 0xc6, 0x98, 0x7a, 0x24, + 0xf8, 0xa6, 0x44, 0x1a, 0x99, 0xc7, 0x25, 0x7b, 0x3a, 0x64, 0x86, 0xd8, 0x5b, 0x05, 0xe7, 0xb9, + 0x8c, 0xd2, 0x30, 0x6e, 0xed, 0xb3, 0x51, 0x0f, 0x4e, 0x10, 0xf2, 0xac, 0x2f, 0x71, 0x93, 0xcd, + 0x11, 0x4f, 0xad, 0xf3, 0x70, 0x2e, 0xcc, 0x92, 0xd3, 0x8d, 0x6f, 0x31, 0xb2, 0xec, 0x0e, 0x50, + 0xaf, 0xf1, 0x13, 0x4d, 0xce, 0x90, 0x72, 0x2c, 0x6d, 0x33, 0xd1, 0x8f, 0x0c, 0x52, 0xb0, 0xee, + 0x32, 0x6c, 0x8e, 0xd0, 0x53, 0x0d, 0xef, 0xb1, 0xf0, 0xae, 0x4c, 0x12, 0x91, 0xcf, 0x2d, 0x73, + 0xca, 0x94, 0x76, 0x28, 0xab, 0xf5, 0x17, 0x49, 0x08, 0x56, 0xb4, 0xea, 0x69, 0x37, 0xd5, 0x8b, + 0x57, 0x09, 0xeb, 0xb5, 0x36, 0x68, 0x8a, 0xd4, 0x95, 0xcb, 0x29, 0x77, 0xf4, 0xaa, 0x48, 0x16, + 0xe9, 0xb7, 0x55, 0x0b, 0x88, 0xd6, 0x34, 0x6a, 0x2b, 0x75, 0x97, 0xc9, 0x4a, 0x14, 0xf6, 0xa8, + 0x74, 0x2a, 0xc8, 0x96, 0x15, 0x4b, 0xa9, 0xf7, 0xb6, 0xe8, 0x0a, 0x54, 0xd7, 0x89, 0x6b, 0x35, +}; + + +const uint16_t CRC16_INIT = 0xffff; + +const uint16_t wCRC_Table[256] = +{ + 0x0000, 0x1189, 0x2312, 0x329b, 0x4624, 0x57ad, 0x6536, 0x74bf, + 0x8c48, 0x9dc1, 0xaf5a, 0xbed3, 0xca6c, 0xdbe5, 0xe97e, 0xf8f7, + 0x1081, 0x0108, 0x3393, 0x221a, 0x56a5, 0x472c, 0x75b7, 0x643e, + 0x9cc9, 0x8d40, 0xbfdb, 0xae52, 0xdaed, 0xcb64, 0xf9ff, 0xe876, + 0x2102, 0x308b, 0x0210, 0x1399, 0x6726, 0x76af, 0x4434, 0x55bd, + 0xad4a, 0xbcc3, 0x8e58, 0x9fd1, 0xeb6e, 0xfae7, 0xc87c, 0xd9f5, + 0x3183, 0x200a, 0x1291, 0x0318, 0x77a7, 0x662e, 0x54b5, 0x453c, + 0xbdcb, 0xac42, 0x9ed9, 0x8f50, 0xfbef, 0xea66, 0xd8fd, 0xc974, + 0x4204, 0x538d, 0x6116, 0x709f, 0x0420, 0x15a9, 0x2732, 0x36bb, + 0xce4c, 0xdfc5, 0xed5e, 0xfcd7, 0x8868, 0x99e1, 0xab7a, 0xbaf3, + 0x5285, 0x430c, 0x7197, 0x601e, 0x14a1, 0x0528, 0x37b3, 0x263a, + 0xdecd, 0xcf44, 0xfddf, 0xec56, 0x98e9, 0x8960, 0xbbfb, 0xaa72, + 0x6306, 0x728f, 0x4014, 0x519d, 0x2522, 0x34ab, 0x0630, 0x17b9, + 0xef4e, 0xfec7, 0xcc5c, 0xddd5, 0xa96a, 0xb8e3, 0x8a78, 0x9bf1, + 0x7387, 0x620e, 0x5095, 0x411c, 0x35a3, 0x242a, 0x16b1, 0x0738, + 0xffcf, 0xee46, 0xdcdd, 0xcd54, 0xb9eb, 0xa862, 0x9af9, 0x8b70, + 0x8408, 0x9581, 0xa71a, 0xb693, 0xc22c, 0xd3a5, 0xe13e, 0xf0b7, + 0x0840, 0x19c9, 0x2b52, 0x3adb, 0x4e64, 0x5fed, 0x6d76, 0x7cff, + 0x9489, 0x8500, 0xb79b, 0xa612, 0xd2ad, 0xc324, 0xf1bf, 0xe036, + 0x18c1, 0x0948, 0x3bd3, 0x2a5a, 0x5ee5, 0x4f6c, 0x7df7, 0x6c7e, + 0xa50a, 0xb483, 0x8618, 0x9791, 0xe32e, 0xf2a7, 0xc03c, 0xd1b5, + 0x2942, 0x38cb, 0x0a50, 0x1bd9, 0x6f66, 0x7eef, 0x4c74, 0x5dfd, + 0xb58b, 0xa402, 0x9699, 0x8710, 0xf3af, 0xe226, 0xd0bd, 0xc134, + 0x39c3, 0x284a, 0x1ad1, 0x0b58, 0x7fe7, 0x6e6e, 0x5cf5, 0x4d7c, + 0xc60c, 0xd785, 0xe51e, 0xf497, 0x8028, 0x91a1, 0xa33a, 0xb2b3, + 0x4a44, 0x5bcd, 0x6956, 0x78df, 0x0c60, 0x1de9, 0x2f72, 0x3efb, + 0xd68d, 0xc704, 0xf59f, 0xe416, 0x90a9, 0x8120, 0xb3bb, 0xa232, + 0x5ac5, 0x4b4c, 0x79d7, 0x685e, 0x1ce1, 0x0d68, 0x3ff3, 0x2e7a, + 0xe70e, 0xf687, 0xc41c, 0xd595, 0xa12a, 0xb0a3, 0x8238, 0x93b1, + 0x6b46, 0x7acf, 0x4854, 0x59dd, 0x2d62, 0x3ceb, 0x0e70, 0x1ff9, + 0xf78f, 0xe606, 0xd49d, 0xc514, 0xb1ab, 0xa022, 0x92b9, 0x8330, + 0x7bc7, 0x6a4e, 0x58d5, 0x495c, 0x3de3, 0x2c6a, 0x1ef1, 0x0f78 +}; + +/** + * @brief calculate the crc8 + * @param[in] pch_message: data + * @param[in] dw_length: stream length = data + checksum + * @param[in] ucCRC8: init CRC8 + * @retval calculated crc8 + */ +uint8_t Get_CRC8_Check_Sum(unsigned char *pch_message, unsigned int dw_length, unsigned char ucCRC8) +{ + unsigned char uc_index; + while (dw_length--) + { + uc_index = ucCRC8 ^ (*pch_message++); + ucCRC8 = CRC8_TAB[uc_index]; + } + return (ucCRC8); +} + +/** + * @brief CRC8 verify function + * @param[in] pch_message: data + * @param[in] dw_length:stream length = data + checksum + * @retval true of false + */ +bool Verify_CRC8_Check_Sum(unsigned char *pch_message, unsigned int dw_length) +{ + unsigned char ucExpected = 0; + if ((pch_message == 0) || (dw_length <= 2)) + { + return 0; + } + ucExpected = Get_CRC8_Check_Sum(pch_message, dw_length - 1, CRC8_INIT); + return (ucExpected == pch_message[dw_length - 1]); +} + +/** + * @brief append CRC8 to the end of data + * @param[in] pch_message: data + * @param[in] dw_length:stream length = data + checksum + * @retval none + */ +void Append_CRC8_Check_Sum(unsigned char *pch_message, unsigned int dw_length) +{ + unsigned char ucCRC = 0; + if ((pch_message == 0) || (dw_length <= 2)) + { + return; + } + ucCRC = Get_CRC8_Check_Sum((unsigned char *) pch_message, dw_length - 1, CRC8_INIT); + pch_message[dw_length - 1] = ucCRC; +} + +/** + * @brief calculate the crc16 + * @param[in] pch_message: data + * @param[in] dw_length: stream length = data + checksum + * @param[in] wCRC: init CRC16 + * @retval calculated crc16 + */ +uint16_t Get_CRC16_Check_Sum(uint8_t *pch_message, uint32_t dw_length, uint16_t wCRC) +{ + uint8_t chData; + if (pch_message == NULL) + { + return 0xFFFF; + } + while (dw_length--) + { + chData = *pch_message++; + (wCRC) = ((uint16_t) (wCRC) >> 8) ^ wCRC_Table[((uint16_t) (wCRC) ^ (uint16_t) (chData)) & 0x00ff]; + } + return wCRC; +} + +/** + * @brief CRC16 verify function + * @param[in] pch_message: data + * @param[in] dw_length:stream length = data + checksum + * @retval true of false + */ +bool Verify_CRC16_Check_Sum(uint8_t *pchMessage, uint32_t dwLength) +{ + uint16_t wExpected = 0; + if ((pchMessage == NULL) || (dwLength <= 2)) + { + return 0; + } + wExpected = Get_CRC16_Check_Sum(pchMessage, dwLength - 2, CRC16_INIT); + return ((wExpected & 0xff) == pchMessage[dwLength - 2] && ((wExpected >> 8) & 0xff) == pchMessage[dwLength - 1]); +} + +/** + * @brief append CRC16 to the end of data + * @param[in] pch_message: data + * @param[in] dw_length:stream length = data + checksum + * @retval none + */ +void Append_CRC16_Check_Sum(uint8_t *pchMessage, uint32_t dwLength) +{ + uint16_t wCRC = 0; + if ((pchMessage == NULL) || (dwLength <= 2)) + { + return; + } + wCRC = Get_CRC16_Check_Sum((uint8_t *) pchMessage, dwLength - 2, CRC16_INIT); + pchMessage[dwLength - 2] = (uint8_t) (wCRC & 0x00ff); + pchMessage[dwLength - 1] = (uint8_t) ((wCRC >> 8) & 0x00ff); +} + +//------------------------------------------------------------------------------ + + diff --git a/User_Code/module/algorithm/crc/crc.h b/User_Code/module/algorithm/crc/crc.h new file mode 100644 index 0000000..19e978a --- /dev/null +++ b/User_Code/module/algorithm/crc/crc.h @@ -0,0 +1,33 @@ +/* USER CODE BEGIN Header */ +/** + ****************************************************************************** + * @file : crc.h + * @brief : crc check + * @author : + * @date : 2023/01/22 + * @version : v1.0 + ****************************************************************************** + * @attention : None + ****************************************************************************** + */ +/* USER CODE END Header */ + +#ifndef CRC_H +#define CRC_H + +#include "stdint.h" +#include "stdbool.h" + + +extern uint8_t Get_CRC8_Check_Sum(unsigned char *pchMessage, unsigned int dwLength, unsigned char ucCRC8); + +extern bool Verify_CRC8_Check_Sum(unsigned char *pchMessage, unsigned int dwLength); + +extern void Append_CRC8_Check_Sum(unsigned char *pchMessage, unsigned int dwLength); + +extern uint16_t Get_CRC16_Check_Sum(uint8_t *pchMessage, uint32_t dwLength, uint16_t wCRC); + +extern bool Verify_CRC16_Check_Sum(uint8_t *pchMessage, uint32_t dwLength); + +extern void Append_CRC16_Check_Sum(uint8_t *pchMessage, uint32_t dwLength); +#endif diff --git a/User_Code/module/algorithm/crc/crc16.c b/User_Code/module/algorithm/crc/crc16.c deleted file mode 100644 index 1a42885..0000000 --- a/User_Code/module/algorithm/crc/crc16.c +++ /dev/null @@ -1,5 +0,0 @@ -// -// Created by ASUS on 2025/11/15. -// - -#include "crc16.h" diff --git a/User_Code/module/algorithm/crc/crc16.h b/User_Code/module/algorithm/crc/crc16.h deleted file mode 100644 index 61f9162..0000000 --- a/User_Code/module/algorithm/crc/crc16.h +++ /dev/null @@ -1,8 +0,0 @@ -// -// Created by ASUS on 2025/11/15. -// - -#ifndef TRONONEH7_SCAFFOLD_CRC16_H -#define TRONONEH7_SCAFFOLD_CRC16_H - -#endif //TRONONEH7_SCAFFOLD_CRC16_H diff --git a/User_Code/module/algorithm/crc/crc8.c b/User_Code/module/algorithm/crc/crc8.c deleted file mode 100644 index 0db8866..0000000 --- a/User_Code/module/algorithm/crc/crc8.c +++ /dev/null @@ -1,5 +0,0 @@ -// -// Created by ASUS on 2025/11/15. -// - -#include "crc8.h" diff --git a/User_Code/module/algorithm/crc/crc8.h b/User_Code/module/algorithm/crc/crc8.h deleted file mode 100644 index c2e4544..0000000 --- a/User_Code/module/algorithm/crc/crc8.h +++ /dev/null @@ -1,8 +0,0 @@ -// -// Created by ASUS on 2025/11/15. -// - -#ifndef TRONONEH7_SCAFFOLD_CRC8_H -#define TRONONEH7_SCAFFOLD_CRC8_H - -#endif //TRONONEH7_SCAFFOLD_CRC8_H diff --git a/User_Code/module/periph/remote_control/videotransmiter.c b/User_Code/module/periph/remote_control/videotransmiter.c new file mode 100644 index 0000000..32eb621 --- /dev/null +++ b/User_Code/module/periph/remote_control/videotransmiter.c @@ -0,0 +1,144 @@ +// +// Created by ASUS on 2025/12/5. +// + + +/* USER CODE BEGIN Header */ +/** + ****************************************************************************** + * @file : Image_Transmission.c + * @brief : Image_Transmission_Info interfaces functions + * @author : + * @date : 2025/4/20 + * @version : v1.0 + ****************************************************************************** + * @attention : to be tested + ****************************************************************************** + */ +/* USER CODE END Header */ +#include "videotransmiter.h" +#include "CRC.h" +#include "usart.h" + +__attribute__((section (".AXI_SRAM"))) uint8_t Image_Trans_MultiRx_Buff[2][39]; + +Image_Transmission_Info_TypeDef Image_Transmission_Info; +VT13_Info_TypeDef VT13_Info; + +static int16_t bit8TObit16(uint8_t change_info[2]); + +void Image_Transmission_Info_Update(uint8_t *Buff) +{ + Image_Transmission_Info.Index = 0; + Image_Transmission_Info.DataLength = 0; + /*Check the header frame */ + + if (Buff[0] == 0xA5) + { + + if (Verify_CRC8_Check_Sum(&Buff[0], 5) == true) + { + + Image_Transmission_Info.DataLength = + (uint16_t) (Buff[Image_Transmission_Info.Index + 2] << 8 | Buff[Image_Transmission_Info.Index + 1]) + + FrameHeader_Length + CMDID_Length + CRC16_Length; + + if (Verify_CRC16_Check_Sum(&Buff[Image_Transmission_Info.Index], Image_Transmission_Info.DataLength) == + true) + { + +#ifdef CUSTOM_ROBOT_DATA_ID + + if (Image_Transmission_Info.DataLength == 39) + { + for (uint8_t i = 0; i < 30; i++) + { + Image_Transmission_Info.custom_robot_data.data[i] = Buff[ + Image_Transmission_Info.Index + FrameHeader_Length + CMDID_Length + i]; + } + } +#endif + +#ifdef REMOTE_CONTROL_ID + + if (Image_Transmission_Info.DataLength == 21) + { + Image_Transmission_Info.remote_control.mouse_x = bit8TObit16( + &Buff[Image_Transmission_Info.Index + FrameHeader_Length + CMDID_Length]); + Image_Transmission_Info.remote_control.mouse_y = bit8TObit16( + &Buff[Image_Transmission_Info.Index + FrameHeader_Length + CMDID_Length + 2]); + Image_Transmission_Info.remote_control.mouse_z = bit8TObit16( + &Buff[Image_Transmission_Info.Index + FrameHeader_Length + CMDID_Length + 4]); + Image_Transmission_Info.remote_control.left_button_down = Buff[ + Image_Transmission_Info.Index + FrameHeader_Length + CMDID_Length + 6]; + Image_Transmission_Info.remote_control.right_button_down = Buff[ + Image_Transmission_Info.Index + FrameHeader_Length + CMDID_Length + 7]; + Image_Transmission_Info.remote_control.Key.keyboard_value = bit8TObit16( + &Buff[Image_Transmission_Info.Index + FrameHeader_Length + CMDID_Length + 8]); + } +#endif + } + } + + } + else if (Buff[0] == 0xA9) + { + + if (Buff[1] == 0x53) + { + + VT13_Info_Update(Buff, &VT13_Info); + + } + } +} + +void VT13_Info_Update(uint8_t *Buff, VT13_Info_TypeDef *VT13_Info) +{ + + if (Verify_CRC16_Check_Sum(&Buff[0], 21) == true) + { + + VT13_Info->RC.Channel[0] = ((Buff[2]) | (Buff[3] << 8)) & 0x07FF; + VT13_Info->RC.Channel[1] = ((Buff[3] >> 3) | (Buff[4] << 5)) & 0x07FF; + VT13_Info->RC.Channel[2] = ((Buff[4] >> 6) | (Buff[5] << 2) | (Buff[6] << 10)) & 0x07FF; + VT13_Info->RC.Channel[3] = ((Buff[6] >> 1) | (Buff[7] << 7)) & 0x07FF; + VT13_Info->RC.Switch = (Buff[7] >> 4) & 0x03; + VT13_Info->RC.Stop = (Buff[7] >> 6) & 0x01; + VT13_Info->RC.Left = (Buff[7] >> 7) & 0x01; + VT13_Info->RC.Right = (Buff[8]) & 0x01; + VT13_Info->RC.Wheel = ((Buff[8] >> 1) | (Buff[9] << 7)) & 0x07FF; + VT13_Info->RC.Trigger = (Buff[9] >> 4) & 0x01; + + VT13_Info->RC.Channel[0] -= 1024; + VT13_Info->RC.Channel[1] -= 1024; + VT13_Info->RC.Channel[2] -= 1024; + VT13_Info->RC.Channel[3] -= 1024; + VT13_Info->RC.Wheel -= 1024; + + VT13_Info->Mouse.X = (Buff[10] | (Buff[11] << 8)); + VT13_Info->Mouse.Y = (Buff[12] | (Buff[13] << 8)); + VT13_Info->Mouse.Z = (Buff[14] | (Buff[15] << 8)); + + VT13_Info->Mouse.Press_L = (Buff[16]) & 0x03; + VT13_Info->Mouse.Press_R = (Buff[16] >> 2) & 0x03; + VT13_Info->Mouse.Press_M = (Buff[16] >> 4) & 0x03; + + VT13_Info->Key.V = (Buff[17] | (Buff[18] << 8)); + } + +} + +static int16_t bit8TObit16(uint8_t change_info[2]) +{ + union + { + int16_t bit16; + uint8_t byte[2]; + } u16val; + + u16val.byte[0] = change_info[0]; + u16val.byte[1] = change_info[1]; + + return u16val.bit16; +} diff --git a/User_Code/module/periph/remote_control/videotransmiter.h b/User_Code/module/periph/remote_control/videotransmiter.h new file mode 100644 index 0000000..f0c5a2d --- /dev/null +++ b/User_Code/module/periph/remote_control/videotransmiter.h @@ -0,0 +1,179 @@ +/* USER CODE BEGIN Header */ +/** + ****************************************************************************** + * @file : Image_Transmission.h + * @brief : The header file of Image_Transmission.c + * @author : + * @date : 2025/04/20 + * @version : v1.0 + ****************************************************************************** + * @attention : None + ****************************************************************************** + */ +/* USER CODE END Header */ + +#ifndef VIDEOTRANSMITER_H +#define VIDEOTRANSMITER_H + +#include "stdint.h" +#include "stdbool.h" +#include "stdlib.h" +#include "string.h" +#include "math.h" +#include +#include + +#define IMAGE_TRANS_RX_LENGTH 39 //frame_header 5bytes , cmd_id 2bytes , data_max 30bytes , crc16 2bytes = bytes + +#define FrameHeader_Length 5U /*!< the length of frame header */ +#define CMDID_Length 2U /*!< the length of CMD ID */ +#define CRC16_Length 2U /*!< the length of CRC ID */ + +#define CUSTOM_ROBOT_DATA_ID 0x0302U +#define ROBOT_CUSTOM_DATA_ID 0x0309U +#define REMOTE_CONTROL_ID 0x0304U + +/** + * @brief The custom controller sends data to the corresponding robot through the image transmission, id: 0x0302U + */ +typedef struct +{ + + uint8_t data[30]; + +} custom_robot_data_t; + +/** + * @brief The robot sends data to the custom controller connected to the player end through the image transmission, id: 0x0309U + */ +typedef struct +{ + + uint8_t data[30]; + +} robot_custom_data_t; + +/** + * @brief The keyboard and mouse remote control data sent to the robot through the image transmission, id: 0x0304U + */ +typedef struct +{ + + int16_t mouse_x; + int16_t mouse_y; + int16_t mouse_z; + int8_t left_button_down; + int8_t right_button_down; + + union + { + uint16_t keyboard_value; + + struct + { + uint16_t W: 1; + uint16_t S: 1; + uint16_t A: 1; + uint16_t D: 1; + uint16_t SHIFT: 1; + uint16_t CTRL: 1; + uint16_t Q: 1; + uint16_t E: 1; + uint16_t R: 1; + uint16_t F: 1; + uint16_t G: 1; + uint16_t Z: 1; + uint16_t X: 1; + uint16_t C: 1; + uint16_t V: 1; + uint16_t B: 1; + } Set; + } Key; + + uint16_t reserved; + +} remote_control_t; + +typedef struct +{ + + uint8_t Index; + uint16_t DataLength; + +#ifdef CUSTOM_ROBOT_DATA_ID + custom_robot_data_t custom_robot_data; +#endif + +#ifdef ROBOT_CUSTOM_DATA_ID + robot_custom_data_t robot_custom_data; +#endif + +#ifdef REMOTE_CONTROL_ID + remote_control_t remote_control; +#endif +} Image_Transmission_Info_TypeDef; + + +typedef struct +{ + + struct + { + int16_t Channel[4]; + uint8_t Switch; + uint8_t Stop; + uint8_t Left; + uint8_t Right; + int16_t Wheel; + uint8_t Trigger; + } RC; + + struct + { + int16_t X; + int16_t Y; + int16_t Z; + uint8_t Press_L; + uint8_t Press_R; + uint8_t Press_M; + } Mouse; + + union + { + uint16_t V; + + struct + { + uint16_t W: 1; + uint16_t S: 1; + uint16_t A: 1; + uint16_t D: 1; + uint16_t SHIFT: 1; + uint16_t CTRL: 1; + uint16_t Q: 1; + uint16_t E: 1; + uint16_t R: 1; + uint16_t F: 1; + uint16_t G: 1; + uint16_t Z: 1; + uint16_t X: 1; + uint16_t C: 1; + uint16_t V: 1; + uint16_t B: 1; + } Set; + } Key; + +} VT13_Info_TypeDef; + +/* Exported variables ---------------------------------------------------------*/ +extern uint8_t Image_Trans_MultiRx_Buff[2][39]; +extern VT13_Info_TypeDef VT13_Info; +extern Image_Transmission_Info_TypeDef Image_Transmission_Info; + +extern void Robot_Data_to_Custom_(uint8_t *Data); + +extern void Image_Transmission_Info_Update(uint8_t *Buff); + +extern void VT13_Info_Update(uint8_t *Data, VT13_Info_TypeDef *VT13_Info); + +#endif diff --git a/User_Code/module/software/powercontrol/Martix.hpp b/User_Code/module/software/powercontrol/Martix.hpp new file mode 100644 index 0000000..4b03db8 --- /dev/null +++ b/User_Code/module/software/powercontrol/Martix.hpp @@ -0,0 +1,416 @@ +/** + ****************************************************************************** + * @file matrix.cpp/h + * @brief Matrix/vector calculation. 矩阵/向量运算 + * @author Spoon Guan + ****************************************************************************** + * Copyright (c) 2023 Team JiaoLong-SJTU + * All rights reserved. + ****************************************************************************** + */ + +#include "AppConfig.h" +#include "EnableIf.hpp" +#include "arm_math.h" +#pragma once + +template + +class Matrixf +{ +public: + /** + * @brief Constructor without input data + * @param + */ + constexpr Matrixf ( +void + ) + : + rows_(_rows), cols_(_cols) { + arm_mat_init_f32(&arm_mat_, _rows, _cols, this->data_); + } + + /** + * @brief Constructor with input data + * @param data A 2-D array buffer that stores the data + */ + constexpr Matrixf ( + float data[_rows * _cols] + ) + : + Matrixf() { + memcpy(this->data_, data, _rows * _cols * sizeof(float)); + arm_mat_init_f32(&arm_mat_, _rows, _cols, this->data_); + } + + /** + * @brief Copy Constructor + * @param mat The copied matrix + */ + constexpr Matrixf ( + const Matrixf<_rows, _cols> + &mat + ) + : + Matrixf() { + memcpy(this->data_, mat.data_, _rows * _cols * sizeof(float)); + arm_mat_init_f32(&arm_mat_, _rows, _cols, this->data_); + } + + /** + * @brief Destructor + */ + ~Matrixf(void) { + } + + /** + * @brief returns the row size of the matrix + * @return _rows The row size of the matrix + */ + uint32_t rows(void) const { + return _rows; + } + + /** + * @brief return the column size of the matrix + * @return _cols The column size of the matrix + */ + uint32_t cols(void) const { + return _cols; + } + + /** + * @brief Return the element of the matrix + * @param row The row + */ + float *operator[](const int&row) { + return &this->data_[row * _cols]; + } + + /** + * @brief Copy assignment of the matrix(row * size) instance + * @param mat The copied prototype + * @return *this matrix + */ + Matrixf < _rows, _cols > &operator = (const Matrixf<_rows, _cols> + mat + ) { + memcpy(this->data_, mat.data_, _rows * _cols * sizeof(float)); + return *this; + } + + /** + * @brief Additional operator of two matrices(row * size) + * @param mat The matrix on the right hand side + * @note This function returns itself as the result + * @return The sum of two matrices + */ + Matrixf < _rows, _cols > &operator += (const Matrixf<_rows, _cols> + mat + ) { + arm_mat_add_f32(&this->arm_mat_, &mat.arm_mat_, &this->arm_mat_); + return *this; + } + + /** + * @brief Substraction operator of two matrices(row * size) + * @param mat The matrix on the left hand side + * @note This function returns itself as the result + * @return The difference of two matrices + */ + Matrixf < _rows, _cols > &operator -= (const Matrixf<_rows, _cols> + mat + ) { + arm_mat_sub_f32(&this->arm_mat_, &mat.arm_mat_, &this->arm_mat_); + return *this; + } + + /** + * @brief Scalar operator of the matrix and a scaling factor + * @param val The scaling factor + * @note This function returns itself as the result + * @return THe scaled matrix + */ + Matrixf < _rows, _cols > &operator *= (const float & val + ) { + arm_mat_scale_f32(&this->arm_mat_, val, &this->arm_mat_); + return *this; + } + + /** + * @brief Scalar operator of the matrix and a division factor + * @param val The division factor + * @note This function returns itself as the result + * @retval matrix / val + * @return The scaled matrix + */ + Matrixf < _rows, _cols > &operator /= (const float & val + ) { + arm_mat_scale_f32(&this->arm_mat_, 1.f / val, &this->arm_mat_); + return *this; + } + + /** + * @brief Additonal operator + * @note This function doesn't return itself but instead a new matrix instance + * @param mat The matrix on the right hand side + * @return The sum of the additional matrix + */ + Matrixf < _rows, _cols > operator + (const Matrixf<_rows, _cols> & mat + ) + const { + Matrixf < _rows, _cols > res; + arm_mat_add_f32(&this->arm_mat_, &mat.arm_mat_, &res.arm_mat_); + return res; + } + + /** + * @brief Substraction matrix + * @note This function does not return itself but instead a new matrix instance + * @param mat matrix on the right hand side + * @return The sum of the substracted matrix + */ + Matrixf < _rows, _cols > operator - (const Matrixf<_rows, _cols> & mat + ) + const { + Matrixf < _rows, _cols > res; + arm_mat_sub_f32(&this->arm_mat_, &mat.arm_mat_, &res.arm_mat_); + return res; + } + + /** + * @brief Scalar operator of the matrix and a scaling factor + * @param val The scaling factor + * @note This function does not return itself + * @return THe scaled matrix + */ + Matrixf < _rows, _cols > operator * (const float & val + ) + const { + Matrixf < _rows, _cols > res; + arm_mat_scale_f32(&this->arm_mat_, val, &res.arm_mat_); + return res; + } + + /** + * @brief Scalar operator of the matrix and a scaling factor + * @param val The scaling factor on the left hand side + * @note This function does not return itself + * @note This time the scaling factor is on the left hand side + * @return THe scaled matrix + */ + friend Matrixf<_rows, _cols> operator*(const float&val, const Matrixf<_rows, _cols>&mat) { + arm_status s; + Matrixf < _rows, _cols > res; + s = arm_mat_scale_f32(&mat.arm_mat_, val, &res.arm_mat_); + return res; + } + + /** + * @brief Scalar operator of the matrix and a division factor + * @param val The division factor + * @note This function returns itself as the result + * @retval matrix / val + * @return The scaled matrix + */ + Matrixf < _rows, _cols > operator / (const float & val + ) + const { + Matrixf < _rows, _cols > res; + arm_mat_scale_f32(&this->arm_mat_, 1.f / val, &res.arm_mat_); + return res; + } + + /** + * @brief The matrix multiplication + * @param mat1 the matrix on the LHS + * @param mat2 the matrix on the RHS + * @return The multiplication result + */ + template < int + cols2 > + friend Matrixf<_rows, cols2> operator*(const Matrixf<_rows, _cols>&mat1, const Matrixf<_cols, cols2>&mat2) { + Matrixf < _rows, cols2 > res; + arm_mat_mult_f32(&mat1.arm_mat_, &mat2.arm_mat_, &res.arm_mat_); + return res; + } + + /** + * @brief Compare whether two matrices are identical + * + */ + bool operator + == + (const Matrixf<_rows, _cols> + &mat + ) + const { + for (int i = 0; i < _rows * _cols; i++) + { + if (this->data_[i] != mat.data_[i]) + return false; + } + return true; + } + + // Submatrix + template < int + rows, int + cols > + Matrixf < rows, cols > block(const int &start_row, const int &start_col) + const { + Matrixf < rows, cols > res; + for (int row = start_row; row < start_row + rows; row++) + { + memcpy((float *) res[0] + (row - start_row) * cols, (float *) this->data_ + row * _cols + start_col, + cols * sizeof(float)); + } + return res; + } + + /** + * @brief Return the specific row of the matrix + * @param row The row index + * @retval The row vector presented in the matrix from + */ + Matrixf < 1, _cols > row(const int &row) + const { + return block < 1, _cols > (row, 0); + } + + /** + * @brief Return the specific row of the matrix + * @param col The column index + * @retval The column vector presented in the matrix from + */ + Matrixf < _rows, 1 > col(const int &col) + const { + return block<_rows, 1>(0, col); + } + + /** + * @brief Get the transpose of the matrix + * @param + * @retval the transposed matrix + */ + Matrixf < _cols, _rows > trans(void) + const { + Matrixf < _cols, _rows > res; + arm_mat_trans_f32(&arm_mat_, &res.arm_mat_); + return res; + } + // Trace + + /** + * @brief Get the trace of the matrix + * @param + * @retval The trace of the matrix + */ + float trace(void) const { + float res = 0; + for (int i = 0; i < fmin(_rows, _cols); i++) + { + res += (*this)[i][i]; + } + return res; + } + + /** + * @brief Get the norm of the matrix + * @param + * @retval The norm of the matrix + */ + float norm(void) const { + return sqrtf((this->trans() * *this)[0][0]); + } + + /** + * @brief Get the inverse of the matrix + * @param + * @retval The inverse of the matrix + */ + Matrixf < _cols, _rows > inv(void) + const { + if (_cols != _rows) + return Matrixf < _cols, _rows > ::zeros(); + + Matrixf < _cols, _rows > res; + arm_status status = arm_mat_inverse_f32(&this->arm_mat_, &res); + + if (status == ARM_MATH_SINGULAR) + return Matrixf < _cols, _rows > ::zeros(); + + return res; + } + + /*==============================================================*/ + // Static function + /** + * @brief Returns a _rows x _cols zero matrix + * @tparam _rows The row size + * @tparam _cols The column size + * @retval The zero matrix + */ + static Matrixf<_rows, _cols> zeros(void) { + float data[_rows * _cols] = {0}; + return Matrixf < _rows, _cols > (data); + } + + /** + * @brief Returns a _rows x _cols one matrix + * @tparam _rows The row size + * @tparam _cols The column size + * @retval The one matrix + */ + static Matrixf<_rows, _cols> ones(void) { + float data[_rows * _cols] = {0}; + for (int i = 0; i < _rows * _cols; i++) + { + data[i] = 1; + } + return Matrixf < _rows, _cols > (data); + } + + /** + * @brief Returns a _rows * columns matrix + * @tparam _rows The row size + * @tparam _cols The column size + * @retval The identity matrix + */ + static Matrixf<_rows, _cols> eye(void) { + float data[_rows * _cols] = {0}; + for (int i = 0; i < fmin(_rows, _cols); i++) + { + data[i * _cols + i] = 1; + } + return Matrixf < _rows, _cols > (data); + } + + /** + * @brief Returns a _rows x _cols diagonal matrix + * @tparam _rows The row size + * @tparam _cols The column size + * @param vec The diagnoal entries + * @retval The diagnoanl matrix + */ + static Matrixf<_rows, _cols> diag(Matrixf<_rows, 1> vec) { + Matrixf < _rows, _cols > res = Matrixf < _rows, _cols > ::zeros(); + for (int i = 0; i < fmin(_rows, _cols); i++) + { + res[i][i] = vec[i][0]; + } + return res; + } + +public: + arm_matrix_instance_f32 arm_mat_; // The arm math instance + +protected: + // The size + int rows_, cols_; + // Data buffer + float data_[_rows * _cols]; +}; + +// namespace matrixf diff --git a/User_Code/module/software/powercontrol/RLS.hpp b/User_Code/module/software/powercontrol/RLS.hpp new file mode 100644 index 0000000..2fe72f6 --- /dev/null +++ b/User_Code/module/software/powercontrol/RLS.hpp @@ -0,0 +1,128 @@ +#include "FreeRTOS.h" +#include "Matrix.hpp" +#include "task.h" + +#pragma once + +namespace Core +{ +namespace Control + { +namespace Math + { + + template < uint32_t dim > +class RLS + { + public: + /** + * @brief Delete the default constructor + */ + RLS() = delete; + + /** + * @brief The constructor + * @param delta_ The intialized non-singular value of the transfer matrix + * @param lambda_ The forgotten index + */ + constexpr RLS(float delta_, float lambda_) + : dimension(dim), lambda(lambda_), delta(delta_), lastUpdate(0), updateCnt(0), + defaultParamsVector(Matrixf::zeros()) + { + this->reset(); + this->validate(); + } + + constexpr RLS(float delta_, float lambda_, Matrixf initParam): RLS(delta_, lambda_) + {defaultParamsVector = initParam; } + + /** + * @brief Reset the RLS module + * @retval None + */ + void reset() + { + transMatrix = Matrixf::eye() * delta; + gainVector = Matrixf::zeros(); + paramsVector = Matrixf::zeros(); + } + + /** + * @brief Proccess a cycle of RLS update + * @param sampleVector The new samples input expressed in n x 1 dimensionasl vector form + * @param actualOutput The actual feedback real output + * @retval paramsVector + */ + const Matrixf & update(Matrixf & sampleVector, float actualOutput) + { + gainVector = + (transMatrix * sampleVector) / ( + 1.0f + (sampleVector.trans() * transMatrix * sampleVector)[0][0] / lambda) / + lambda; // Get gain vector + paramsVector += gainVector * ( + actualOutput - (sampleVector.trans() * paramsVector)[0][0]) +; // Get params vector + transMatrix = + (transMatrix - gainVector * sampleVector.trans() * transMatrix) / lambda +; // Get transferred matrix + + updateCnt++; + lastUpdate = xTaskGetTickCount(); + return paramsVector; + } + + /** + * @brief Set the default regression parameters + * @param updatedParams + * @retval None + */ + void setParamVector(const Matrixf & updatedParams) + { + paramsVector = updatedParams; + defaultParamsVector = updatedParams; + } + /** + * @brief The getter function of the params vector + * @param None + * @retval paramsVector + */ + constexpr Matrixf & getParamsVector() const{return paramsVector; } + + /** + * @brief The getter function of the output vector + * @param None + * @retval The estimated / filterd output of the RLS module + */ + const float &getOutput() const{return output; } + + private: + /** + * @brief Lambda and delta validate check + * @param None + * @retval None + */ + void validate() const + { + configASSERT(lambda >= 0.0f || lambda <= 1.0f); + configASSERT(delta > 0); + } + + uint32_t dimension; // Dimension of the RLS space + float lambda; // The forget index + float delta; // Intialized value of the transferred matrix + + TickType_t lastUpdate; // Last update tick + uint32_t updateCnt; // Total update Count + + /*RLS relvant matrix*/ + Matrixf transMatrix; // Transfer matrix instance + Matrixf gainVector; // Gain vector for params update + Matrixf paramsVector; // Params vector + Matrixf defaultParamsVector; + float output; // Estimated / filtered output + } +; + + } // namespace Math + } // namespace Control +} // namespace Core diff --git a/User_Code/module/software/powercontrol/powercontrol.c b/User_Code/module/software/powercontrol/powercontrol.c index bacfe2d..478ad8c 100644 --- a/User_Code/module/software/powercontrol/powercontrol.c +++ b/User_Code/module/software/powercontrol/powercontrol.c @@ -2,4 +2,596 @@ // Created by tux on 2025/11/5. // -#include "powercontrol.h" \ No newline at end of file +#include "powercontrol.h" + +// #define USE_POWER_CONTROLLER + +#if USE_POWER_CONTROLLER + +namespace Core +{ +namespace Control + { +namespace Power + { + +#define POWER_PD_KP 50.0f + +Manager manager(Manager::Motors(nullptr, nullptr, nullptr, nullptr), Division::HERO); +PowerStatus powerStatus; +static uint8_t LATEST_FEEDBACK_JUDGE_ROBOT_LEVEL; +static bool isCapEnergyOut = false; +static uint16_t motorDisconnectCounter[4] = {0U, 0U, 0U, 0U}; +static float MIN_MAXPOWER_CONFIGURED = 30.0f; + +static inline bool floatEqual(float a, float b) {return fabs(a - b) < 1e-5f; } + +static inline float rpm2av(float rpm) {return rpm * (float) M_PI / 30.0f; } + +static inline float av2rpm(float av) {return av * 30.0f / (float) M_PI; } + +static inline void setErrorFlag(uint8_t & curFlag, Manager::ErrorFlags setFlag) + {curFlag |= static_cast(setFlag); } + +static inline void clearErrorFlag(uint8_t & curFlag, Manager::ErrorFlags clearFlag) + {curFlag &= (~static_cast(clearFlag)); } + +static inline bool isFlagged(uint8_t & curFlag, Manager::ErrorFlags flag) + {return (curFlag & static_cast(flag)) != 0; } + +static inline bool isMotorConnected(const AbstractFeedbackMotor * motor) + {return motor != nullptr && motor->getDisconnectCounter() < 10; } + +static inline bool isAllMotorConnected() + { + for (int i = 0; i < 4; i++) + if (not isMotorConnected(manager.motors[i])) + return false; + return true; + } + + Manager::Manager( + const Motors & motors_, const Division division_, RLSEnabled rlsEnabled_, const float k1_, const float k2_, + const float k3_, const float lambda_) + + : rlsEnabled(rlsEnabled_), + error(0UL), + motors(motors_), + division(division_), + powerBuff(0.0f), + fullBuffSet(0.0f), + baseBuffSet(0.0f), + fullMaxPower(0.0f), + baseMaxPower(0.0f), + powerUpperLimit(0.0f), + refereeMaxPower(0.0f), + userConfiguredMaxPower(0.0f), + callback(nullptr), + torqueConst(0.0f), + k1(k1_), + k2(k2_), + k3(k3_), + lastUpdateTick(0), + rls(1e-5f, 0.99999f) + { + configASSERT(k1_ >= 0); + configASSERT(k2_ >= 0); + configASSERT(k3_ >= 0); + + float initParams[2] = {k1_, k2_}; + rls.setParamVector(Matrixf < 2, 1 > (initParams)); + } + +static bool isInitialized; +StackType_t xPowerTaskStack[1024]; +StaticTask_t uxPowerTaskTCB; + +using namespace Core::Communication; +RefereeSystem::RefereeSystemMessageReceive powerMessage; +RefereeSystem::RefereeSystemMessageReceive robotMessage; + +#if USE_SUPER_CAPACITOR +const Core::Control::SuperCapacitor::CapacitorStatus&capStatus = Core::Control::SuperCapacitor::getStatus(); +#endif + +Core::Control::PID::Param powerPDParam(POWER_PD_KP, 0.0f, 0.2f, 0.0f, MAX_CAP_POWER_OUT, 0.0f, 0.0f, 0.2f, 100UL); +Core::Control::PID powerPD_base(powerPDParam); // ensure the system does not die +Core::Control::PID powerPD_full(powerPDParam); // ensure the capacitor's energy keeps under 90% + +/** + * @implements + */ +void setMaxPowerConfigured(float maxPower) +{ + manager.userConfiguredMaxPower = Utils::Math::clamp(maxPower, MIN_MAXPOWER_CONFIGURED, manager.powerUpperLimit); +} + +void setMode(uint8_t mode) +{ + setMaxPowerConfigured(mode == 1 ? manager.powerUpperLimit : manager.refereeMaxPower); +} + +void registerPowerCallbackFunc(float (*callback)(void)) +{ + manager.callback = callback; +} + +/** + * @implements + */ +void setRLSEnabled(uint8_t enable) +{ + manager.rlsEnabled = static_cast(enable); +} + +const volatile PowerStatus&getPowerStatus() +{ + return powerStatus; +} + +float getLatestFeedbackJudgePowerLimit() +{ + return manager.refereeMaxPower; +} + +/** + * @implements + */ + +float *getControlledOutput(PowerObj *objs[4]) +{ + const float k0 = manager.torqueConst * manager.motors[0]->getCurrentLimit() / + manager.motors[0]->getOutputLimit(); // torque current rate of the motor, defined as Nm/Output + + static float newTorqueCurrent[4]; + + float sumCmdPower = 0.0f; + float cmdPower[4]; + + float sumError = 0.0f; + float error[4]; + + float maxPower = Utils::Math::clamp(manager.userConfiguredMaxPower, manager.fullMaxPower, manager.baseMaxPower); + + float allocatablePower = maxPower; + float sumPowerRequired = 0.0f; +#if USE_DEBUG +static float newCmdPower; +#endif + +for (int i = 0; i<4; i++) + { + if (isMotorConnected(manager.motors[i])) + { + PowerObj *p = objs[i]; + cmdPower[i] = p->pidOutput * k0 * p->curAv + fabs(p->curAv) * manager.k1 + p->pidOutput * k0 * p->pidOutput * k0 * manager.k2 + + manager.k3 / static_cast(4); + sumCmdPower += cmdPower[i]; + error[i] = fabs(p->setAv - p->curAv); + if (floatEqual(cmdPower[i], 0.0f) || cmdPower[i] < 0.0f) + { + allocatablePower += -cmdPower[i]; + } + else + { + sumError += error[i]; + sumPowerRequired += cmdPower[i]; + } + } + else if (motorDisconnectCounter[i] < 1000U) + { + cmdPower[i] = manager.motors[i]->getTorqueFeedback() * rpm2av(manager.motors[i]->getRPMFeedback()) + + fabs(rpm2av(manager.motors[i]->getRPMFeedback())) * manager.k1 + + manager.motors[i]->getTorqueFeedback() * manager.motors[i]->getTorqueFeedback() * manager.k2 + manager.k3 / 4.0f; + error[i] = 0.0f; + } + else + { + cmdPower[i] = 0.0f; + error[i] = 0.0f; + } + } + +// update power status +powerStatus.maxPowerLimited = maxPower; +powerStatus.sumPowerCmd_before_clamp = sumCmdPower; + + if (sumCmdPower> maxPower) + { + float errorConfidence; + if (sumError > error_powerDistribution_set) + { + errorConfidence = 1.0f; + } + else if (sumError > prop_powerDistribution_set) + { + errorConfidence = + Utils::Math::clamp((sumError - prop_powerDistribution_set) / (error_powerDistribution_set - prop_powerDistribution_set), 0.0f, 1.0f); + } + else + { + errorConfidence = 0.0f; + } + for (int i = 0; i < 4; i++) + { + PowerObj *p = objs[i]; + if (isMotorConnected(manager.motors[i])) + { + if (floatEqual(cmdPower[i], 0.0f) || cmdPower[i] < 0.0f) + { + newTorqueCurrent[i] = p->pidOutput; + continue; + } + float powerWeight_Error = fabs(p->setAv - p->curAv) / sumError; + float powerWeight_Prop = cmdPower[i] / sumPowerRequired; + float powerWeight = errorConfidence * powerWeight_Error + (1.0f - errorConfidence) * powerWeight_Prop; + float delta = p->curAv * p->curAv - + 4.0f * manager.k2 * (manager.k1 * fabs(p->curAv) + manager.k3 / static_cast(4) - powerWeight * allocatablePower); + if (floatEqual(delta, 0.0f)) // repeat roots + { + newTorqueCurrent[i] = -p->curAv / (2.0f * manager.k2) / k0; + } + else if (delta > 0.0f) // distinct roots + { + newTorqueCurrent[i] = p->pidOutput > 0.0f ? (-p->curAv + sqrtf(delta)) / (2.0f * manager.k2) / k0 + : (-p->curAv - sqrtf(delta)) / (2.0f * manager.k2) / k0; + } + else // imaginary roots + { + newTorqueCurrent[i] = -p->curAv / (2.0f * manager.k2) / k0; + } + newTorqueCurrent[i] = Utils::Math::clamp(newTorqueCurrent[i], p->pidMaxOutput); + } + else + { + newTorqueCurrent[i] = 0.0f; + } + } + } + else + { + for (int i = 0; i < 4; i++) + { + if (isMotorConnected(manager.motors[i])) + { + newTorqueCurrent[i] = objs[i]->pidOutput; + } + else + { + newTorqueCurrent[i] = 0.0f; + } + } + } + +#if USE_DEBUG +newCmdPower = 0.0f; + for (int i = 0; i<4; i++) + { + PowerObj *p = objs[i]; + newCmdPower += newTorqueCurrent[i] * k0 * p->curAv + fabs(p->curAv) * manager.k1 + + newTorqueCurrent[i] * k0 * newTorqueCurrent[i] * k0 * manager.k2 + manager.k3 / 4.0f; + } +#endif + +return newTorqueCurrent; +} + +static inline void setErrorFlag() +{ + + + +/*Judge the error status*/ +#if USE_SUPER_CAPACITOR +if (not capStatus.isConnected|| not capStatus.capacitorTx.enableDCDC|| not capStatus.capacitorRx.errorCode== 0) +setErrorFlag(manager.error, Manager::CAPDisConnect); + else +clearErrorFlag(manager.error, Manager::CAPDisConnect); +#else +setErrorFlag(manager.error, Manager::CAPDisConnect); +#endif + +if (not RefereeSystem::isConnected()) +setErrorFlag(manager.error, Manager::RefereeDisConnect); + else +clearErrorFlag(manager.error, Manager::RefereeDisConnect); + + if (not isAllMotorConnected()) +setErrorFlag(manager.error, Manager::MotorDisconnect); + else +clearErrorFlag(manager.error, Manager::MotorDisconnect); +} + +void powerDaemon [[noreturn]](void *pvParam) +{ + static Matrixf<2, 1> samples; + static Matrixf<2, 1> params; + static float effectivePower = 0; + + manager.torqueConst = manager.motors[0]->getKA() * manager.motors[0]->getReductionRatio(); + isInitialized = true; + + vTaskDelay(1000); + + manager.lastUpdateTick = xTaskGetTickCount(); + + while (true) + { + setErrorFlag(); + TickType_t now = xTaskGetTickCount(); + +// update rls state and check whether cap energy is out even when cap disconnect to utilize credible data from referee system for the rls +// model +// estimate the cap energy if cap disconnect +// estimated cap energy = cap energy feedback when cap is connected +#if USE_SUPER_CAPACITOR +// If super capacitor is disconnected from the circuit, disable the rls update +if (isFlagged(manager.error, Manager::CAPDisConnect)) +{ + // Judge whether the cap energy is used-up + if (not isFlagged(manager.error, Manager::RefereeDisConnect)) + { + if (powerMessage.getData().bufferEnergy < MAX_POEWR_REFEREE_BUFF && + powerMessage.getData().chassisPower > CAP_OFFLINE_ENERGY_RUNOUT_POWER_THRESHOLD) + { + isCapEnergyOut = true; + manager.estimatedCapEnergy = 0.0f; + } + else + { + isCapEnergyOut = false; + manager.rlsEnabled = Manager::Disable; + if (powerMessage.getData().chassisPower < MIN_MAXPOWER_CONFIGURED && powerMessage.getData().bufferEnergy == + 60U) + { + manager.estimatedCapEnergy = 2100.0f; + } + else + { + manager.estimatedCapEnergy += (powerMessage.getData().chassisPower - manager.estimatedPower) * + static_cast((now - manager.lastUpdateTick) / configTICK_RATE_HZ); + manager.estimatedCapEnergy = Utils::Math::clamp(manager.estimatedCapEnergy, 0.0f, 2100.0f); + } + } + } + else + { + isCapEnergyOut = false; + manager.rlsEnabled = Manager::Disable; + manager.estimatedCapEnergy += (CAP_OFFLINE_ENERGY_TARGET_POWER - manager.estimatedPower) * + static_cast((now - manager.lastUpdateTick) / configTICK_RATE_HZ); + manager.estimatedCapEnergy = Utils::Math::clamp(manager.estimatedCapEnergy, 0.0f, 2100.0f); + } +} + else + { + isCapEnergyOut = false; + manager.estimatedCapEnergy = capStatus.capacitorRx.capEnergy / 255.0f * 2100.0f; + } +#else // Only Use Referee System, disable the rls update if referee data is invalid +if (isFlagged(manager.error, Manager::RefereeDisConnect)) +{ + manager.rlsEnabled = Manager::Disable; +} +isCapEnergyOut = false; +manager.estimatedCapEnergy = 0.0f +#endif + +// Set the power buff and buff set based on the current state +// Take cap message as priority +// If disconnect from cap or disable the cap, then take the referee system's power buffer as feedback +// If referee system is disconnected, then we need to disable the energy loop and treat power loop conservatively +// When both cap and referee are disconnected, we disable the energy loop and therefore no need to update the powerBuff and buffSet +// +// Set the energy feedback based on the current error status +#if USE_SUPER_CAPACITOR +if (not isFlagged(manager.error, Manager::CAPDisConnect)) +manager.powerBuff = capStatus.capacitorRx.capEnergy; + else if (not isFlagged(manager.error, Manager::RefereeDisConnect)) +manager.powerBuff = powerMessage.getData().bufferEnergy; +#else +if (not isFlagged(manager.error, Manager::RefereeDisConnect)) +manager.powerBuff = powerMessage.getData().bufferEnergy; +#endif +// Set the energy target based on the current error status +#if USE_SUPER_CAPACITOR +// If the Super Capacitor is in the circuit +if (not isFlagged(manager.error, Manager::CAPDisConnect)) +{ + manager.fullBuffSet = capFullBuffSet; + manager.baseBuffSet = capBaseBuffSet; +} + else + { + // if referee data is not valid, we do not enable the energy loop, so that we do not have to update fullbuffset and basebuffset + manager.fullBuffSet = refereeFullBuffSet; + manager.baseBuffSet = refereeBaseBuffSet; + } +#else +// Only Use Referee System +// if referee data is not valid, we do not enable the energy loop, so that we do not have to updating fullbuffset and basebuffset +manager.fullBuffSet = refereeFullBuffSet; +manager.baseBuffSet = refereeBaseBuffSet; +#endif + +// Update the referee maximum power limit and user configured power limit +// If disconnected, then restore the last robot level and find corresponding chassis power limit +if (not isFlagged(manager.error, Manager::RefereeDisConnect)) +{ + manager.refereeMaxPower = fmax(robotMessage.getData().chassis_power_limit, + CAP_OFFLINE_ENERGY_RUNOUT_POWER_THRESHOLD); + if (robotMessage.getData().robot_level > 10U) + LATEST_FEEDBACK_JUDGE_ROBOT_LEVEL = 1U; + else + LATEST_FEEDBACK_JUDGE_ROBOT_LEVEL = fmax(1U, robotMessage.getData().robot_level); +#if USE_SUPER_CAPACITOR +if (isFlagged(manager.error, Manager::CAPDisConnect)) +manager.powerUpperLimit = manager.refereeMaxPower + POWER_PD_KP * ( + sqrtf(refereeFullBuffSet) - sqrtf(refereeBaseBuffSet)); + else +manager.powerUpperLimit = manager.refereeMaxPower + MAX_CAP_POWER_OUT; +#else +manager.powerUpperLimit = manager.refereeMaxPower; +#endif +} + else + { + switch (manager.division) + { + case Division::HERO: + manager.refereeMaxPower = HeroChassisPowerLimit_HP_FIRST[LATEST_FEEDBACK_JUDGE_ROBOT_LEVEL - 1U]; + break; + case Division::INFANTRY: + manager.refereeMaxPower = InfantryChassisPowerLimit_HP_FIRST[LATEST_FEEDBACK_JUDGE_ROBOT_LEVEL - 1U]; + break; + case Division::SENTRY: + manager.refereeMaxPower = SentryChassisPowerLimit; + break; + default: + configASSERT(0) break; + } +// Since we have less available feedback, we constrain the power conservatively +#if USE_SUPER_CAPACITOR +if (isFlagged(manager.error, Manager::CAPDisConnect)) +{ + manager.powerUpperLimit = manager.refereeMaxPower * CAP_REFEREE_BOTH_GG_COE; +} + else + { + manager.powerUpperLimit = manager.refereeMaxPower + MAX_CAP_POWER_OUT; + } +#else +manager.powerUpperLimit = manager.refereeMaxPower * CAP_REFEREE_BOTH_GG_COE; +#endif +} + +MIN_MAXPOWER_CONFIGURED = manager.refereeMaxPower * 0.8f; + +// energy loop +// if cap and referee both gg, set the max power to latest power limit * 0.85 and disable energy loop +// if referee gg, set the max power to latest power limit * 0.95, enable energy loop when cap energy out + if (isFlagged(manager.error, Manager::CAPDisConnect) && isFlagged(manager.error, Manager::RefereeDisConnect)) + { + manager.baseMaxPower = manager.fullMaxPower = manager.refereeMaxPower * CAP_REFEREE_BOTH_GG_COE; + powerPD_base.reset(); + powerPD_full.reset(); + } + else + { + manager.baseMaxPower = + fmax(manager.refereeMaxPower - powerPD_base(sqrtf(manager.baseBuffSet), sqrtf(manager.powerBuff)), MIN_MAXPOWER_CONFIGURED); + manager.fullMaxPower = + fmax(manager.refereeMaxPower - powerPD_full(sqrtf(manager.fullBuffSet), sqrtf(manager.powerBuff)), MIN_MAXPOWER_CONFIGURED); + } + +// if user has self defined power curve, use it + if (manager.callback!= nullptr) +setMaxPowerConfigured(manager.callback()); + +// Estimate the power based on the current model +effectivePower = 0; +samples[0][0] = 0; +samples[1][0] = 0; + for (int i = 0; i<4; i++) + { + if (isMotorConnected(manager.motors[i])) + { + motorDisconnectCounter[i] = 0U; + } + else + { + motorDisconnectCounter[i]++; + } + if (motorDisconnectCounter[i] < 1000U) // We consider motor that is just disconnected as still using power, by assuming the motor keep + // latest output and rpm by 1 second, otherwise it is not safe if we only use energy loop + { + effectivePower += manager.motors[i]->getTorqueFeedback() * rpm2av(manager.motors[i]->getRPMFeedback()); + samples[0][0] += fabsf(rpm2av(manager.motors[i]->getRPMFeedback())); + samples[1][0] += manager.motors[i]->getTorqueFeedback() * manager.motors[i]->getTorqueFeedback(); + } + else + { + motorDisconnectCounter[i] = 1000U; + } + } +manager.estimatedPower = manager.k1 * samples[0][0] + manager.k2 * samples[1][0] + effectivePower + manager.k3; + +// Get the measured power from cap +// If cap is disconnected, get measured power from referee feedback if cap energy is out +// Otherwise, set it to estimated power +#if USE_SUPER_CAPACITOR +if (not isFlagged(manager.error, Manager::CAPDisConnect)) +{ + manager.measuredPower = capStatus.capacitorRx.chassisPower; +} + else if (not isFlagged(manager.error, Manager::RefereeDisConnect) && isCapEnergyOut) + { + // If the capacitor energy is used up, we could trust the data from the referee system + manager.measuredPower = powerMessage.getData().chassisPower; + } + else + { + manager.measuredPower = manager.estimatedPower; + } +#else +if (not isFlagged(manager.error, Manager::RefereeDisConnect)) +{ + manager.measuredPower = powerMessage.getData().chassisPower; +} + else + { + manager.measuredPower = manager.estimatedPower; + } +#endif + +// update power status +powerStatus.userConfiguredMaxPower = manager.userConfiguredMaxPower; +powerStatus.effectivePower = effectivePower; +powerStatus.powerLoss = manager.measuredPower - effectivePower; +powerStatus.efficiency = Utils::Math::clamp(effectivePower / manager.measuredPower, 0.0f, 1.0f); +powerStatus.estimatedCapEnergy = static_cast(manager.estimatedCapEnergy / 2100.0f * 255.0f); +powerStatus.error = static_cast(manager.error); + +// Update the RLS parameters AND +// Add dead zone AND +// The Referee System could not detect negative power, leading to failure of real measurement. +// So use estimated power to evaluate this situtation + if (manager.rlsEnabled== Manager::Enable&& fabs(manager.measuredPower)> 5.0f && +not(isFlagged(manager.error, Manager::CAPDisConnect) && manager.estimatedPower<0)) + { + params = manager.rls.update(samples, manager.measuredPower - effectivePower - manager.k3); + manager.k1 = fmax(params[0][0], 1e-5f); // In case the k1 diverge to negative number + manager.k2 = fmax(params[1][0], 1e-5f); // In case the k2 diverge to negative number + } + +manager.lastUpdateTick = now; + +vTaskDelay(pdMS_TO_TICKS(1)); + } +} + +/** + * @implements + */ +void init(const Manager&mana) +{ + if (isInitialized) + return; + manager = mana; + + // default value + LATEST_FEEDBACK_JUDGE_ROBOT_LEVEL = manager.division == Division::SENTRY ? 10U : 1U; + MIN_MAXPOWER_CONFIGURED = 30.0f; + manager.powerUpperLimit = CAP_OFFLINE_ENERGY_RUNOUT_POWER_THRESHOLD + MAX_CAP_POWER_OUT; + + Core::Communication::RefereeSystem::subscribeMessage(&powerMessage); + Core::Communication::RefereeSystem::subscribeMessage(&robotMessage); + + xTaskCreateStatic(powerDaemon, "power", 1024, nullptr, 10, xPowerTaskStack, &uxPowerTaskTCB); +} + +} // namespace Power +} // namespace Control +} // namespace Core + +#endif diff --git a/User_Code/module/software/powercontrol/powercontrol.h b/User_Code/module/software/powercontrol/powercontrol.h index d04477c..10c285b 100644 --- a/User_Code/module/software/powercontrol/powercontrol.h +++ b/User_Code/module/software/powercontrol/powercontrol.h @@ -1,8 +1,265 @@ -// -// Created by tux on 2025/11/5. -// +/** + * @file PowerController.hpp + * @version 2.0 + * @note The estimated power formula: P = τΩ + k1|Ω| + k2τ^2 + */ +#pragma once +// #include "AppConfig.h" -#ifndef TRONONEH7_SCAFFOLD_POWERCONTROL_H -#define TRONONEH7_SCAFFOLD_POWERCONTROL_H +#ifndef USE_POWER_CONTROLLER +#define USE_POWER_CONTROLLER FALSE +#endif -#endif //TRONONEH7_SCAFFOLD_POWERCONTROL_H \ No newline at end of file +#if USE_POWER_CONTROLLER + +// If the capacitor is plugged into the circuit, make sure you enable the super cap module successfully +// Otherwise, it will cause unexpected behavior of the RLS model +#ifndef USE_SUPER_CAPACITOR +#define USE_SUPER_CAPACITOR FALSE +#endif + +#ifndef USE_REFEREE_SYSTEM_COMM +#define USE_REFEREE_SYSTEM_COMM FALSE +#endif + +#if !(USE_REFEREE_SYSTEM_COMM) +#error Referee System Communication is not defined! +#endif + +#if USE_SUPER_CAPACITOR +#include "SuperCapaManager.hpp" +#endif + +#include "AbstractFeedbackMotor.hpp" +#include "Math.hpp" +#include "PID.hpp" +#include "RLS.hpp" +#include "RefereeSystemComm.hpp" +#include "RefereeSystemManager.hpp" +#include "RefereeSystemMessage.hpp" + +namespace Core +{ +namespace Control + { +namespace Power + { + +constexpr static float refereeFullBuffSet = 60.0f; +constexpr static float refereeBaseBuffSet = 50.0f; +constexpr static float capFullBuffSet = 230.0f; +constexpr static float capBaseBuffSet = 30.0f; +constexpr static float error_powerDistribution_set = 20.0f; +constexpr static float prop_powerDistribution_set = 15.0f; + + // constexpr float MIN_MAXPOWER_CONFIGURED = 15.0f; +constexpr float MAX_CAP_POWER_OUT = 300.0f; +constexpr float CAP_OFFLINE_ENERGY_RUNOUT_POWER_THRESHOLD = 43.0f; +constexpr float CAP_OFFLINE_ENERGY_TARGET_POWER = 37.0f; +constexpr float MAX_POEWR_REFEREE_BUFF = 60.0f; +constexpr float REFEREE_GG_COE = 0.95f; +constexpr float CAP_REFEREE_BOTH_GG_COE = 0.85f; + + /** + * @brief The Power Limit and max HP enumeration attributed by division, chassis type and level + * @note Copy from RM2024 Official Rule Manual + * @attention The infantry data list only suits for standard infantry, but not balanced infantry + * @attention if the pilot changes the chassis type before the game officially start, and simultaneously the referee system is disconnected before + * chassis type changed, there will be problem of distinguishing the chassis type, so we choose HP_FIRST chassis type conservatively, except for + * sentry + */ +constexpr static uint8_t maxLevel = 10U; +constexpr static uint8_t HeroChassisPowerLimit_HP_FIRST[maxLevel] = {55U, 60U, 65U, 70U, 75U, 80U, 85U, 90U, 100U, 120U} +; +constexpr static uint8_t + + InfantryChassisPowerLimit_HP_FIRST[maxLevel] = {45U, 50U, 55U, 60U, 65U, 70U, 75U, 80U, 90U, + 100U}; +constexpr static uint8_t SentryChassisPowerLimit = 100U; + +enum class Division + { + INFANTRY = 0, + HERO, + SENTRY + }; + +struct Manager + { + enum RLSEnabled: bool + { + Disable = 0, + Enable = 1 + } rlsEnabled; + + enum ErrorFlags + { + MotorDisconnect = 1U, + RefereeDisConnect = 2U, + CAPDisConnect = 4U + }; + + struct Motors // For overloading + { + const AbstractFeedbackMotor * motorRf; + const AbstractFeedbackMotor * motorLf; + const AbstractFeedbackMotor * motorLb; + const AbstractFeedbackMotor * motorRb; + + const AbstractFeedbackMotor * &operator[](int index) + { + switch (index) + { + case 0: + return motorRf; + case 1: + return motorLf; + case 2: + return motorLb; + case 3: + return motorRb; + default: + return motorRf; + } + } + + Motors(const AbstractFeedbackMotor * motorRf_, + const AbstractFeedbackMotor * motorLf_, + const AbstractFeedbackMotor * motorLb_, + const AbstractFeedbackMotor * motorRb_) + : motorRf(motorRf_), motorLf(motorLf_), motorLb(motorLb_), motorRb(motorRb_) + { + } + }; + + uint8_t error; + + /** + * @remark In case of initialization without explicit datas + */ + Manager() = delete; + + /** + * @brief The constructor of the power manager object + * @param motors_ The motor objects + * @todo This will change to the type of "AbstractFeedbackMotor*" + * @param division_ The type of robot + * @param rlsEnabled_ Enable or disable the RLS adaptive param mode + * @param torqueConst_ The torque const (KA) of the motor, measured by (N.m / A) + * @param k1_ The frequency-dissipate params on the power estimation motor + * @param k2_ The current-dissipate's square params on the power estimation motor + * @param k3_ The constant power loss + * @param lambda_ The RLS update forgetting factor + */ + Manager(const Motors & motors_, + const Division division_, + RLSEnabled rlsEnabled_ = Enable, + const float k1_ = 0.22f, + const float k2_ = 1.2f, + const float k3_ = 2.78f, + const float lambda_ = 0.9999f); + + Motors motors; + Division division; + + float powerBuff; + float fullBuffSet; + float baseBuffSet; + float fullMaxPower; + float baseMaxPower; + + float powerUpperLimit; + float refereeMaxPower; + + float userConfiguredMaxPower; + float(*callback)(void); + + float measuredPower; + float estimatedPower; + float estimatedCapEnergy; + + float torqueConst; + + float k1; + float k2; + float k3; + + TickType_t lastUpdateTick; + Math::RLS < 2 > rls; + } +; + +struct PowerObj + { + public: + float pidOutput; // torque current command, [-maxOutput, maxOutput], no unit + float curAv; // Measured angular velocity, [-maxAv, maxAv], rad/s + float setAv; // target angular velocity, [-maxAv, maxAv], rad/s + float pidMaxOutput; // pid max output + } +; + + /** + * @brief Storing the power status of the chassis + */ +struct PowerStatus + { + public: + float userConfiguredMaxPower; + float maxPowerLimited; + float sumPowerCmd_before_clamp; + float effectivePower; + float powerLoss; + float efficiency; + uint8_t estimatedCapEnergy; + Manager::ErrorFlags error; + } +; + + // return the latest feedback referee power limit(before referee disconnected), according to the robot level +float getLatestFeedbackJudgePowerLimit(); + + /** + * @brief Get the controlled output torque current based on current model + * @param objs The collections of power objects from four wheels, recording the necessary data from the PID controller + * @retval The controlled output torque current + */ +float *getControlledOutput(PowerObj * objs[4]); + + /** + * @brief return the power status of the chassis + * @retval The power status object + */ +const volatile PowerStatus & getPowerStatus(); + + /** + * @brief The power controller module initialization function + * @param manager The manager object + * @note This function should be called before the scheduler starts + */ +void init(const Manager & manager); + + /** + * @brief set the user configured max power + * @param maxPower The max power value + * @note The max power configured by this function will compete with the basic energy limitation, to ensure system does not die + */ +void setMaxPowerConfigured(float maxPower); + +void setMode(uint8_t mode); + +void registerPowerCallbackFunc(float(*callback)(void)); + + /** + * @brief Enable for disable the automatically parameters update process + * @param isUpdate disable with 0, enable with 1 + * @note The system will automatically disable the update when both referee system and cap is disconnect from the power module + * @retval None + */ +void setRLSEnabled(uint8_t isUpdate); + + } // namespace Power + } // namespace Control +} // namespace Core + +#endif diff --git a/User_Code/module/software/powercontrol/powercontrol.md b/User_Code/module/software/powercontrol/powercontrol.md new file mode 100644 index 0000000..00627bf --- /dev/null +++ b/User_Code/module/software/powercontrol/powercontrol.md @@ -0,0 +1,2 @@ +# 功率控制模块 +