TuxMonkey/tronone-h7-scaffold
1
0
Fork 0
mirror of https://gitee.com/dlmu-cone/tronone-h7-scaffold synced 2026-07-24 03:27:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

videotransmiter

Browse Source
This commit is contained in:
TuxMonkey
2025-12-05 21:47:00 +08:00
parent 7522bc9974
commit c021b4794c
15 changed files with 2164 additions and 239 deletions
Download Patch File Download Diff File Expand all files Collapse all files

191
User_Code/module/algorithm/crc/crc.c Normal file
View File

@@ -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);
}
//------------------------------------------------------------------------------

33
User_Code/module/algorithm/crc/crc.h Normal file
View File

@@ -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

5
User_Code/module/algorithm/crc/crc16.c
View File

@@ -1,5 +0,0 @@
//
// Created by ASUS on 2025/11/15.
//
#include "crc16.h"

8
User_Code/module/algorithm/crc/crc16.h
View File

@@ -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

5
User_Code/module/algorithm/crc/crc8.c
View File

@@ -1,5 +0,0 @@
//
// Created by ASUS on 2025/11/15.
//
#include "crc8.h"

8
User_Code/module/algorithm/crc/crc8.h
View File

@@ -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

144
User_Code/module/periph/remote_control/videotransmiter.c Normal file
View File

@@ -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;
}

179
User_Code/module/periph/remote_control/videotransmiter.h Normal file
View File

@@ -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 <stdio.h>
#include <string.h>
#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

416
User_Code/module/software/powercontrol/Martix.hpp Normal file
View File

@@ -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<int _rows, int _cols>
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

128
User_Code/module/software/powercontrol/RLS.hpp Normal file
View File

@@ -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<dim, 1>::zeros())
{
this->reset();
this->validate();
}
constexpr RLS(float delta_, float lambda_, Matrixf<dim, 1> initParam): RLS(delta_, lambda_)
{defaultParamsVector = initParam; }
/**
* @brief Reset the RLS module
* @retval None
*/
void reset()
{
transMatrix = Matrixf<dim, dim>::eye() * delta;
gainVector = Matrixf<dim, 1>::zeros();
paramsVector = Matrixf<dim, 1>::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<dim, 1> & update(Matrixf<dim, 1> & 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<dim, 1> & updatedParams)
{
paramsVector = updatedParams;
defaultParamsVector = updatedParams;
}
/**
* @brief The getter function of the params vector
* @param None
* @retval paramsVector
*/
constexpr Matrixf<dim, 1> & 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<dim, dim > transMatrix; // Transfer matrix instance
Matrixf<dim, 1 > gainVector; // Gain vector for params update
Matrixf<dim, 1 > paramsVector; // Params vector
Matrixf<dim, 1 > defaultParamsVector;
float output; // Estimated / filtered output
}
;
} // namespace Math
} // namespace Control
} // namespace Core

594
User_Code/module/software/powercontrol/powercontrol.c
View File

@@ -2,4 +2,596 @@
// Created by tux on 2025/11/5.
//
#include "powercontrol.h"
#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<uint8_t>(setFlag); }
static inline void clearErrorFlag(uint8_t & curFlag, Manager::ErrorFlags clearFlag)
{curFlag &= (~static_cast<uint8_t>(clearFlag)); }
static inline bool isFlagged(uint8_t & curFlag, Manager::ErrorFlags flag)
{return (curFlag & static_cast<uint8_t>(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<RefereeSystem::RefereePowerHeatMessageData> powerMessage;
RefereeSystem::RefereeSystemMessageReceive<RefereeSystem::RefereeRobotStatusMessageData> 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<Manager::RLSEnabled>(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<float>(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<float>(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<float>((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<float>((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<uint8_t>(manager.estimatedCapEnergy / 2100.0f * 255.0f);
powerStatus.error = static_cast<Manager::ErrorFlags>(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

269
User_Code/module/software/powercontrol/powercontrol.h
View File

@@ -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
#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

2
User_Code/module/software/powercontrol/powercontrol.md Normal file
View File

@@ -0,0 +1,2 @@
# 功率控制模块