HFUT_Firmament_EC
/
sentry_chassis_hzz
11
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

finish all bsp utilities

This commit is contained in:
NeoZng 2022-11-01 22:32:15 +08:00
parent c113ca81e0
commit a2b7558047
13 changed files with 514 additions and 436 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

230
Src/main.c
View File

@ -79,90 +79,90 @@ void MX_FREERTOS_Init(void);
*/ */
int main(void) int main(void)
{ {
/* USER CODE BEGIN 1 */ /* USER CODE BEGIN 1 */
/* USER CODE END 1 */ /* USER CODE END 1 */
/* MCU Configuration--------------------------------------------------------*/ /* MCU Configuration--------------------------------------------------------*/
/* Reset of all peripherals, Initializes the Flash interface and the Systick. */ /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
HAL_Init(); HAL_Init();
/* USER CODE BEGIN Init */ /* USER CODE BEGIN Init */
/* USER CODE END Init */ /* USER CODE END Init */
/* Configure the system clock */ /* Configure the system clock */
SystemClock_Config(); SystemClock_Config();
/* USER CODE BEGIN SysInit */ /* USER CODE BEGIN SysInit */
/* USER CODE END SysInit */ /* USER CODE END SysInit */
/* Initialize all configured peripherals */ /* Initialize all configured peripherals */
MX_GPIO_Init(); MX_GPIO_Init();
MX_DMA_Init(); MX_DMA_Init();
MX_ADC1_Init(); MX_ADC1_Init();
MX_CAN1_Init(); MX_CAN1_Init();
MX_CAN2_Init(); MX_CAN2_Init();
MX_SPI1_Init(); MX_SPI1_Init();
MX_TIM4_Init(); MX_TIM4_Init();
MX_TIM5_Init(); MX_TIM5_Init();
MX_USART3_UART_Init(); MX_USART3_UART_Init();
MX_RNG_Init(); MX_RNG_Init();
MX_RTC_Init(); MX_RTC_Init();
MX_TIM1_Init(); MX_TIM1_Init();
MX_TIM10_Init(); MX_TIM10_Init();
MX_USART1_UART_Init(); MX_USART1_UART_Init();
MX_USART6_UART_Init(); MX_USART6_UART_Init();
/* USER CODE BEGIN 2 */ /* USER CODE BEGIN 2 */
RC_init(&huart3); RC_init(&huart3);
can_instance_config can_config = {.can_handle = &hcan1, can_instance_config can_config = {.can_handle = &hcan1,
.tx_id = 1}; .tx_id = 1};
Motor_Control_Setting_s motor_config = {.angle_feedback_source = MOTOR_FEED, Motor_Control_Setting_s motor_config = {.angle_feedback_source = MOTOR_FEED,
.close_loop_type = SPEED_LOOP | CURRENT_LOOP, .close_loop_type = SPEED_LOOP | CURRENT_LOOP,
.speed_feedback_source = MOTOR_FEED, .speed_feedback_source = MOTOR_FEED,
.reverse_flag = MOTOR_DIRECTION_NORMAL}; .reverse_flag = MOTOR_DIRECTION_NORMAL};
Motor_Controller_Init_s controller_init = {.current_PID = { Motor_Controller_Init_s controller_init = {.current_PID = {
.Improve = 0, .Improve = 0,
.Kp = 1, .Kp = 1,
.Ki = 0, .Ki = 0,
.Kd = 0, .Kd = 0,
.DeadBand = 0, .DeadBand = 0,
.MaxOut = 4000, .MaxOut = 4000,
}, },
.speed_PID = { .speed_PID = {
.Improve = 0, .Improve = 0,
.Kp = 1, .Kp = 1,
.Ki = 0, .Ki = 0,
.Kd = 0, .Kd = 0,
.DeadBand = 0, .DeadBand = 0,
.MaxOut = 4000, .MaxOut = 4000,
}}; }};
Motor_Type_e type = M3508; Motor_Type_e type = M3508;
DWT_Init(168); DWT_Init(168);
dji_motor_instance *djimotor = DJIMotorInit(can_config, motor_config, controller_init, type); dji_motor_instance *djimotor = DJIMotorInit(can_config, motor_config, controller_init, type);
/* USER CODE END 2 */ /* USER CODE END 2 */
/* Call init function for freertos objects (in freertos.c) */ /* Call init function for freertos objects (in freertos.c) */
// MX_FREERTOS_Init(); // MX_FREERTOS_Init();
// /* Start scheduler */ // /* Start scheduler */
// osKernelStart(); // osKernelStart();
/* We should never get here as control is now taken by the scheduler */ /* We should never get here as control is now taken by the scheduler */
/* Infinite loop */ /* Infinite loop */
/* USER CODE BEGIN WHILE */ /* USER CODE BEGIN WHILE */
while (1) while (1)
{ {
/* USER CODE END WHILE */ /* USER CODE END WHILE */
DJIMotorSetRef(djimotor, get_remote_control_point()->rc.ch[0]); DJIMotorSetRef(djimotor, get_remote_control_point()->rc.ch[0]);
MotorControlTask(); MotorControlTask();
HAL_Delay(20); HAL_Delay(20);
/* USER CODE BEGIN 3 */ /* USER CODE BEGIN 3 */
} }
/* USER CODE END 3 */ /* USER CODE END 3 */
} }
/** /**
@ -171,42 +171,42 @@ int main(void)
*/ */
void SystemClock_Config(void) void SystemClock_Config(void)
{ {
RCC_OscInitTypeDef RCC_OscInitStruct = {0}; RCC_OscInitTypeDef RCC_OscInitStruct = {0};
RCC_ClkInitTypeDef RCC_ClkInitStruct = {0}; RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
/** Configure the main internal regulator output voltage /** Configure the main internal regulator output voltage
*/ */
__HAL_RCC_PWR_CLK_ENABLE(); __HAL_RCC_PWR_CLK_ENABLE();
__HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1); __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1);
/** Initializes the RCC Oscillators according to the specified parameters /** Initializes the RCC Oscillators according to the specified parameters
* in the RCC_OscInitTypeDef structure. * in the RCC_OscInitTypeDef structure.
*/ */
RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE; RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
RCC_OscInitStruct.HSEState = RCC_HSE_ON; RCC_OscInitStruct.HSEState = RCC_HSE_ON;
RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON; RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE; RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
RCC_OscInitStruct.PLL.PLLM = 6; RCC_OscInitStruct.PLL.PLLM = 6;
RCC_OscInitStruct.PLL.PLLN = 168; RCC_OscInitStruct.PLL.PLLN = 168;
RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2; RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
RCC_OscInitStruct.PLL.PLLQ = 7; RCC_OscInitStruct.PLL.PLLQ = 7;
if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
{ {
Error_Handler(); Error_Handler();
} }
/** Initializes the CPU, AHB and APB buses clocks /** Initializes the CPU, AHB and APB buses clocks
*/ */
RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_SYSCLK | RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2; RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_SYSCLK | RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2;
RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK; RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1; RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV4; RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV4;
RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2; RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2;
if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_5) != HAL_OK) if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_5) != HAL_OK)
{ {
Error_Handler(); Error_Handler();
} }
} }
/* USER CODE BEGIN 4 */ /* USER CODE BEGIN 4 */
@ -219,13 +219,13 @@ void SystemClock_Config(void)
*/ */
void Error_Handler(void) void Error_Handler(void)
{ {
/* USER CODE BEGIN Error_Handler_Debug */ /* USER CODE BEGIN Error_Handler_Debug */
/* User can add his own implementation to report the HAL error return state */ /* User can add his own implementation to report the HAL error return state */
__disable_irq(); __disable_irq();
while (1) while (1)
{ {
} }
/* USER CODE END Error_Handler_Debug */ /* USER CODE END Error_Handler_Debug */
} }
#ifdef USE_FULL_ASSERT #ifdef USE_FULL_ASSERT
@ -238,9 +238,9 @@ void Error_Handler(void)
*/ */
void assert_failed(uint8_t *file, uint32_t line) void assert_failed(uint8_t *file, uint32_t line)
{ {
/* USER CODE BEGIN 6 */ /* USER CODE BEGIN 6 */
/* User can add his own implementation to report the file name and line number, /* User can add his own implementation to report the file name and line number,
ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */ ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
/* USER CODE END 6 */ /* USER CODE END 6 */
} }
#endif /* USE_FULL_ASSERT */ #endif /* USE_FULL_ASSERT */

31
bsp/bsp_usart.c
View File

@ -1,17 +1,30 @@
/**
* @file bsp_usart.c
* @author neozng
* @brief 串口bsp层的实现
* @version beta
* @date 2022-11-01
*
* @copyright Copyright (c) 2022
*
*/
#include "bsp_usart.h" #include "bsp_usart.h"
#include "stdlib.h" #include "stdlib.h"
/* usart service instance,modules' info would be recoreded here using ModuleRegister() */ /* usart service instance, modules' info would be recoreded here using USARTRegister() */
/* usart服务实例,所有注册了usart的模块信息会被保存在这里 */
static usart_instance *instance[DEVICE_USART_CNT]; static usart_instance *instance[DEVICE_USART_CNT];
/** /**
* @brief usart service will start automatically, after each module registered * @brief usart service will start automatically, after each module registered
* 串口服务会在每个实例注册之后自动启用
* *
* @param _instance instance owned by module * @param _instance instance owned by module,模块拥有的串口实例
*/ */
static void USARTServiceInit(usart_instance *_instance) static void USARTServiceInit(usart_instance *_instance)
{ {
HAL_UARTEx_ReceiveToIdle_DMA(_instance->usart_handle, _instance->recv_buff, _instance->recv_buff_size); HAL_UARTEx_ReceiveToIdle_DMA(_instance->usart_handle, _instance->recv_buff, _instance->recv_buff_size);
// 关闭dma half transfer中断防止两次进入HAL_UARTEx_RxEventCallback()
__HAL_DMA_DISABLE_IT(_instance->usart_handle->hdmarx, DMA_IT_HT); __HAL_DMA_DISABLE_IT(_instance->usart_handle->hdmarx, DMA_IT_HT);
} }
@ -22,21 +35,21 @@ void USARTRegister(usart_instance *_instance)
instance[instance_idx++] = _instance; instance[instance_idx++] = _instance;
} }
/* @todo 当前仅进行了形式上的封装,后续要进一步考虑是否将module的行为与bsp完全分离 */
void USARTSend(usart_instance *_instance, uint8_t *send_buf, uint16_t send_size) void USARTSend(usart_instance *_instance, uint8_t *send_buf, uint16_t send_size)
{ {
HAL_UART_Transmit_DMA(_instance->usart_handle, send_buf, send_size); HAL_UART_Transmit_DMA(_instance->usart_handle, send_buf, send_size);
} }
/** /**
* @brief everytiem when dma/idle happens,this function will be called * @brief 每次dma/idle中断发生时，都会调用此函数.对于每个uart实例会调用对应的回调进行进一步的处理
* here, for each uart, specific callback is refered for further process * 例如:视觉协议解析/遥控器解析/裁判系统解析
* etc:visual protocol resolve/remote control resolve/referee protocol resolve
* *
* @note because DMA half transfer iterrupt(DMA_IT_HT) would call this function too, so we just * @note because DMA half transfer iterrupt(DMA_IT_HT) would call this callback function too, so we just
* disable it when transfer complete using macro: __HAL_DMA_DISABLE_IT(huart->hdmarx,DMA_IT_HT) * disable it when transfer complete using macro: __HAL_DMA_DISABLE_IT(huart->hdmarx,DMA_IT_HT)
* *
* @param huart uart handle indicate which uart is being handled * @param huart uart handle indicate which uart is being handled 发生中断的串口
* @param Size not used temporarily * @param Size not used temporarily,此次接收到的总数居量,暂时没用
*/ */
void HAL_UARTEx_RxEventCallback(UART_HandleTypeDef *huart, uint16_t Size) void HAL_UARTEx_RxEventCallback(UART_HandleTypeDef *huart, uint16_t Size)
{ {
@ -58,7 +71,7 @@ void HAL_UARTEx_RxEventCallback(UART_HandleTypeDef *huart, uint16_t Size)
* *
* @note most frequent error ex: parity/overrrun/frame error * @note most frequent error ex: parity/overrrun/frame error
* *
* @param huart uart handle type,indicate where error comes from * @param huart uart handle type, indicate where error comes from
*/ */
void HAL_UART_ErrorCallback(UART_HandleTypeDef *huart) void HAL_UART_ErrorCallback(UART_HandleTypeDef *huart)
{ {

33
bsp/bsp_usart.h
View File

@ -4,41 +4,38 @@
#include "struct_typedef.h" #include "struct_typedef.h"
#include "main.h" #include "main.h"
#define DEVICE_USART_CNT 3 #define DEVICE_USART_CNT 3 // C板至多分配3个串口
#define USART_RX_LIMIT 128 // if your protocol needs bigger buff, modify here #define USART_RXBUFF_LIMIT 128 // if your protocol needs bigger buff, modify here
/* application callback,which resolves specific protocol */ /* application callback,which resolves specific protocol,解析协议的回调函数 */
typedef void (*usart_module_callback)(); typedef void (*usart_module_callback)();
/* usart_instance struct,each app would have one instance */ /* usart_instance struct,each app would have one instance */
typedef struct typedef struct
{ {
uint8_t recv_buff[USART_RX_LIMIT]; uint8_t recv_buff[USART_RXBUFF_LIMIT]; // 预先定义的最大buff大小,如果太小请修改USART_RXBUFF_LIMIT
uint8_t recv_buff_size; uint8_t recv_buff_size; // 模块接收一包数据的大小
UART_HandleTypeDef *usart_handle; UART_HandleTypeDef *usart_handle; // 实例对应的usart_handle
usart_module_callback module_callback; usart_module_callback module_callback; // 解析收到的数据的回调函数
} usart_instance; } usart_instance;
/** /**
* @brief calling this func would regiter a module to usart_service * @brief 注册一个串口实例.
* this function provides completely wrap and abstract for those module who use usart as commu method
*
* @param id module id.attention,each id is mapped to a specific usart port(ex:remote control->usart3)
* @param prbuff pointer to where the raw recv data stored
* @param rbuffsize size of recv data buff
* @param psbuff pointer to where the data to be sent stored
* @param callback func pointer,this func would be called in USART_service.c/HAL_UARTEx_RxEventCallback()
* to resolve raw data using protocol provides by specific app calling Moduleregister()
* *
* @param _instance 传入一个由module拥有的串口实例,注意在传入前进行基本信息的配置,详见usart_instance的struct定义
*/ */
void USARTRegister(usart_instance* _instance); void USARTRegister(usart_instance *_instance);
/** /**
* @todo 是否需要进一步封装发送buff和size,并创建一个串口任务以一定频率自动发送?
* 若采用此方法,则串口实例的拥有者仅需要在自己的任务中设置发送值,不需要关心发送buffer大小以及何时发送.
*
* @brief api for sending data through a specific serial port,indicated by the first parameter:id * @brief api for sending data through a specific serial port,indicated by the first parameter:id
* 通过调用该函数可以发送一帧数据,需要传入一个usart实例,发送buff以及这一帧的长度
* *
* @param id specify which usart would be used * @param id specify which usart would be used
* @param send_size how many bytes to send * @param send_size how many bytes to send
*/ */
void USARTSend(usart_instance* _instance, uint8_t* send_buf,uint16_t send_size); void USARTSend(usart_instance *_instance, uint8_t *send_buf, uint16_t send_size);
#endif #endif

49
modules/algorithm/controller.c
View File

@ -1,37 +1,38 @@
/**
* @file controller.c
* @author wanghongxi
* @author modified by neozng
* @brief PID控制器定义
* @version beta
* @date 2022-11-01
*
* @copyrightCopyright (c) 2022 HNU YueLu EC all rights reserved
*/
#include "controller.h" #include "controller.h"
#include <memory.h> #include <memory.h>
/******************************* PID CONTROL *********************************/
// PID优化环节函数声明 // PID优化环节函数声明
static void f_Trapezoid_Intergral(PID_t *pid); static void f_Trapezoid_Intergral(PID_t *pid); // 梯形积分
static void f_Integral_Limit(PID_t *pid); static void f_Integral_Limit(PID_t *pid); // 积分限幅
static void f_Derivative_On_Measurement(PID_t *pid); static void f_Derivative_On_Measurement(PID_t *pid); // 微分先行(仅使用反馈微分)
static void f_Changing_Integration_Rate(PID_t *pid); static void f_Changing_Integration_Rate(PID_t *pid); // 变速积分
static void f_Output_Filter(PID_t *pid); static void f_Output_Filter(PID_t *pid); // 输出滤波
static void f_Derivative_Filter(PID_t *pid); static void f_Derivative_Filter(PID_t *pid); // 微分滤波(采集时)
static void f_Output_Limit(PID_t *pid); static void f_Output_Limit(PID_t *pid); // 输出限幅
static void f_Proportion_Limit(PID_t *pid); static void f_PID_ErrorHandle(PID_t *pid); // 堵转保护
static void f_PID_ErrorHandle(PID_t *pid);
/** /**
* @brief * @brief 初始化PID,设置参数和启用的优化环节,将其他数据置零
* *
* @param pid * @param pid PID实例
* @param config * @param config PID初始化设置
*/ */
void PID_Init(PID_t* pid,PID_Init_config_s *config) void PID_Init(PID_t *pid, PID_Init_config_s *config)
{ {
// utilize the quality of struct that its memeory is continuous
memcpy(pid, config, sizeof(PID_Init_config_s)); memcpy(pid, config, sizeof(PID_Init_config_s));
memset(&pid->Measure,0,sizeof(PID_t)-sizeof(PID_Init_config_s)); // set rest of memory to 0
memset(&pid->Measure, 0, sizeof(PID_t) - sizeof(PID_Init_config_s));
// // DWT定时器计数变量清零
// // reset DWT Timer count counter
// pid->DWT_CNT = 0;
// // 设置PID异常处理 目前仅包含电机堵转保护
// pid->ERRORHandler.ERRORCount = 0;
// pid->ERRORHandler.ERRORType = PID_ERROR_NONE;
} }
/** /**

106
modules/algorithm/controller.h
View File

@ -1,19 +1,18 @@
/** /**
****************************************************************************** ******************************************************************************
* @file controller.h * @file controller.h
* @author Wang Hongxi * @author Wang Hongxi
* @version V1.1.3 * @version V1.1.3
* @date 2021/7/3 * @date 2021/7/3
* @brief * @brief
****************************************************************************** ******************************************************************************
* @attention * @attention
* *
****************************************************************************** ******************************************************************************
*/ */
#ifndef _CONTROLLER_H #ifndef _CONTROLLER_H
#define _CONTROLLER_H #define _CONTROLLER_H
#include "main.h" #include "main.h"
#include "stdint.h" #include "stdint.h"
#include "string.h" #include "string.h"
@ -26,36 +25,37 @@
#define abs(x) ((x > 0) ? x : -x) #define abs(x) ((x > 0) ? x : -x)
#endif #endif
// PID 优化环节使能标志位
/******************************* PID CONTROL *********************************/
typedef enum pid_Improvement_e typedef enum pid_Improvement_e
{ {
NONE = 0X00, //0000 0000 NONE = 0b00000000, // 0000 0000
Integral_Limit = 0x01, //0000 0001 Integral_Limit = 0b00000001, // 0000 0001
Derivative_On_Measurement = 0x02, //0000 0010 Derivative_On_Measurement = 0b00000010, // 0000 0010
Trapezoid_Intergral = 0x04, //0000 0100 Trapezoid_Intergral = 0b00000100, // 0000 0100
Proportional_On_Measurement = 0x08, //0000 1000 Proportional_On_Measurement = 0b00001000, // 0000 1000
OutputFilter = 0x10, //0001 0000 OutputFilter = 0b00010000, // 0001 0000
ChangingIntegrationRate = 0x20, //0010 0000 ChangingIntegrationRate = 0b00100000, // 0010 0000
DerivativeFilter = 0x40, //0100 0000 DerivativeFilter = 0b01000000, // 0100 0000
ErrorHandle = 0x80, //1000 0000 ErrorHandle = 0b10000000, // 1000 0000
} PID_Improvement_e; } PID_Improvement_e;
/* PID 报错类型枚举*/
typedef enum errorType_e typedef enum errorType_e
{ {
PID_ERROR_NONE = 0x00U, PID_ERROR_NONE = 0x00U,
Motor_Blocked = 0x01U Motor_Blocked = 0x01U
} ErrorType_e; } ErrorType_e;
typedef struct typedef struct
{ {
uint64_t ERRORCount; uint64_t ERRORCount;
ErrorType_e ERRORType; ErrorType_e ERRORType;
} PID_ErrorHandler_t; } PID_ErrorHandler_t;
typedef struct /* PID结构体 */
typedef struct
{ {
//---------------------------------- init config block //---------------------------------- init config block
// config parameter // config parameter
float Kp; float Kp;
float Ki; float Ki;
@ -64,13 +64,13 @@ typedef struct
float MaxOut; float MaxOut;
float IntegralLimit; float IntegralLimit;
float DeadBand; float DeadBand;
float CoefA; //For Changing Integral float CoefA; // For Changing Integral
float CoefB; //ITerm = Err*((A-abs(err)+B)/A) when B<|err|<A+B float CoefB; // ITerm = Err*((A-abs(err)+B)/A) when B<|err|<A+B
float Output_LPF_RC; // RC = 1/omegac float Output_LPF_RC; // RC = 1/omegac
float Derivative_LPF_RC; float Derivative_LPF_RC;
uint8_t Improve; uint8_t Improve;
//----------------------------------- //-----------------------------------
// for calculating // for calculating
float Measure; float Measure;
float Last_Measure; float Last_Measure;
@ -96,7 +96,7 @@ typedef struct
} PID_t; } PID_t;
/* 用于PID初始化的结构体*/ /* 用于PID初始化的结构体*/
typedef struct typedef struct
{ {
// config parameter // config parameter
float Kp; float Kp;
@ -106,8 +106,8 @@ typedef struct
float MaxOut; float MaxOut;
float IntegralLimit; float IntegralLimit;
float DeadBand; float DeadBand;
float CoefA; //For Changing Integral float CoefA; // For Changing Integral
float CoefB; //ITerm = Err*((A-abs(err)+B)/A) when B<|err|<A+B float CoefB; // ITerm = Err*((A-abs(err)+B)/A) when B<|err|<A+B
float Output_LPF_RC; // RC = 1/omegac float Output_LPF_RC; // RC = 1/omegac
float Derivative_LPF_RC; float Derivative_LPF_RC;
@ -115,32 +115,22 @@ typedef struct
} PID_Init_config_s; } PID_Init_config_s;
/**
* @brief 初始化PID实例
*
* @param pid PID实例指针
* @param config PID初始化配置
*/
void PID_Init(PID_t *pid, PID_Init_config_s *config);
/**
void PID_Init(PID_t* pid,PID_Init_config_s* config); * @brief 计算PID输出
*
* @param pid PID实例指针
* @param measure 反馈值
* @param ref 设定值
* @return float PID计算输出
*/
float PID_Calculate(PID_t *pid, float measure, float ref); float PID_Calculate(PID_t *pid, float measure, float ref);
#endif
/*************************** Tracking Differentiator ***************************/
typedef struct
{
float Input;
float h0;
float r;
float x;
float dx;
float ddx;
float last_dx;
float last_ddx;
uint32_t DWT_CNT;
float dt;
} TD_t;
void TD_Init(TD_t *td, float r, float h0);
float TD_Calculate(TD_t *td, float input);
#endif

9
modules/imu/ins_task.c
View File

@ -16,7 +16,7 @@
#include "bsp_temperature.h" #include "bsp_temperature.h"
#include "spi.h" #include "spi.h"
INS_t INS; static INS_t INS;
IMU_Param_t IMU_Param; IMU_Param_t IMU_Param;
PID_t TempCtrl = {0}; PID_t TempCtrl = {0};
@ -31,6 +31,11 @@ float RefTemp = 40;
static void IMU_Param_Correction(IMU_Param_t *param, float gyro[3], float accel[3]); static void IMU_Param_Correction(IMU_Param_t *param, float gyro[3], float accel[3]);
attitude_t *GetINSptr()
{
return (attitude_t *)&INS.Roll;
}
void INS_Init(void) void INS_Init(void)
{ {
// while (BMI088_init(&hspi1, 1) != BMI088_NO_ERROR); // while (BMI088_init(&hspi1, 1) != BMI088_NO_ERROR);
@ -51,7 +56,7 @@ void INS_Init(void)
.Ki = 20, .Ki = 20,
.Kd = 0, .Kd = 0,
.Improve = 0x01}; // enable integratiaon limit .Improve = 0x01}; // enable integratiaon limit
PID_Init(&TempCtrl,&config); PID_Init(&TempCtrl, &config);
// noise of accel is relatively big and of high freq,thus lpf is used // noise of accel is relatively big and of high freq,thus lpf is used
INS.AccelLPF = 0.0085; INS.AccelLPF = 0.0085;

82
modules/imu/ins_task.h
View File

@ -23,12 +23,20 @@
#define INS_TASK_PERIOD 1 #define INS_TASK_PERIOD 1
typedef struct
{
float Roll;
float Pitch;
float Yaw;
float YawTotalAngle;
} attitude_t;
typedef struct typedef struct
{ {
float q[4]; // 四元数估计值 float q[4]; // 四元数估计值
float Gyro[3]; // 角速度 float Gyro[3]; // 角速度
float Accel[3]; // 加速度 float Accel[3]; // 加速度
float MotionAccel_b[3]; // 机体坐标加速度 float MotionAccel_b[3]; // 机体坐标加速度
float MotionAccel_n[3]; // 绝对系加速度 float MotionAccel_n[3]; // 绝对系加速度
@ -49,10 +57,9 @@ typedef struct
float YawTotalAngle; float YawTotalAngle;
} INS_t; } INS_t;
/** /**
* @brief 用于修正安装误差的参数,demo中可无视 * @brief 用于修正安装误差的参数
* *
*/ */
typedef struct typedef struct
{ {
@ -65,16 +72,79 @@ typedef struct
float Roll; float Roll;
} IMU_Param_t; } IMU_Param_t;
extern INS_t INS; /**
* @brief 反馈位姿结构体指针,apps通过初始化时保存该指针以实现数据访问
*
* @return attitude_t*
*/
attitude_t *GetINSptr();
/**
* @brief 初始化惯导解算系统
*
*/
void INS_Init(void); void INS_Init(void);
/**
* @brief 此函数放入实时系统中,以1kHz频率运行
* p.s. osDelay(1);
*
*/
void INS_Task(void); void INS_Task(void);
/**
* @brief 温控函数
*
*/
void IMU_Temperature_Ctrl(void); void IMU_Temperature_Ctrl(void);
/**
* @brief 四元数更新函数,即实现dq/dt=0.5Ωq
*
* @param q 四元数
* @param gx
* @param gy
* @param gz
* @param dt 距离上次调用的时间间隔
*/
void QuaternionUpdate(float *q, float gx, float gy, float gz, float dt); void QuaternionUpdate(float *q, float gx, float gy, float gz, float dt);
/**
* @brief 四元数转换成欧拉角 ZYX
*
* @param q
* @param Yaw
* @param Pitch
* @param Roll
*/
void QuaternionToEularAngle(float *q, float *Yaw, float *Pitch, float *Roll); void QuaternionToEularAngle(float *q, float *Yaw, float *Pitch, float *Roll);
/**
* @brief ZYX欧拉角转换为四元数
*
* @param Yaw
* @param Pitch
* @param Roll
* @param q
*/
void EularAngleToQuaternion(float Yaw, float Pitch, float Roll, float *q); void EularAngleToQuaternion(float Yaw, float Pitch, float Roll, float *q);
/**
* @brief 机体系到惯性系的变换函数
*
* @param vecBF body frame
* @param vecEF earth frame
* @param q
*/
void BodyFrameToEarthFrame(const float *vecBF, float *vecEF, float *q); void BodyFrameToEarthFrame(const float *vecBF, float *vecEF, float *q);
/**
* @brief 惯性系转换到机体系
*
* @param vecEF
* @param vecBF
* @param q
*/
void EarthFrameToBodyFrame(const float *vecEF, float *vecBF, float *q); void EarthFrameToBodyFrame(const float *vecEF, float *vecBF, float *q);
#endif #endif

78
modules/motor/dji_motor.c
View File

@ -1,5 +1,8 @@
#include "dji_motor.h" #include "dji_motor.h"
#define PI2 3.141592f
#define ECD_ANGLE_COEF 3.835e-4 // ecd/8192*pi
static uint8_t idx = 0; // register idx,是该文件的全局电机索引,在注册时使用 static uint8_t idx = 0; // register idx,是该文件的全局电机索引,在注册时使用
/* DJI电机的实例,此处仅保存指针,内存的分配将通过电机实例初始化时通过malloc()进行 */ /* DJI电机的实例,此处仅保存指针,内存的分配将通过电机实例初始化时通过malloc()进行 */
@ -33,7 +36,7 @@ static uint8_t sender_enable_flag[6] = {0};
* @brief 当注册的电机id冲突时,会进入这个函数并提示冲突的ID * @brief 当注册的电机id冲突时,会进入这个函数并提示冲突的ID
* @todo 通过segger jlink 发送日志 * @todo 通过segger jlink 发送日志
*/ */
static void IDcrash_Handler() static void IDcrash_Handler(uint8_t conflict_motor_idx, uint8_t temp_motor_idx)
{ {
while (1) while (1)
{ {
@ -67,7 +70,7 @@ static void MotorSenderGrouping(can_instance_config *config)
} }
// 计算接收id并设置分组发送id // 计算接收id并设置分组发送id
config->rx_id = 0x200 + motor_id+1; config->rx_id = 0x200 + motor_id + 1;
sender_enable_flag[motor_grouping] = 1; sender_enable_flag[motor_grouping] = 1;
dji_motor_info[idx]->message_num = motor_send_num; dji_motor_info[idx]->message_num = motor_send_num;
dji_motor_info[idx]->sender_group = motor_grouping; dji_motor_info[idx]->sender_group = motor_grouping;
@ -76,7 +79,7 @@ static void MotorSenderGrouping(can_instance_config *config)
for (size_t i = 0; i < idx; i++) for (size_t i = 0; i < idx; i++)
{ {
if (dji_motor_info[i]->motor_can_instance.can_handle == config->can_handle && dji_motor_info[i]->motor_can_instance.rx_id == config->rx_id) if (dji_motor_info[i]->motor_can_instance.can_handle == config->can_handle && dji_motor_info[i]->motor_can_instance.rx_id == config->rx_id)
IDcrash_Handler(); IDcrash_Handler(i, idx);
} }
break; break;
@ -100,7 +103,7 @@ static void MotorSenderGrouping(can_instance_config *config)
for (size_t i = 0; i < idx; i++) for (size_t i = 0; i < idx; i++)
{ {
if (dji_motor_info[i]->motor_can_instance.can_handle == config->can_handle && dji_motor_info[i]->motor_can_instance.rx_id == config->rx_id) if (dji_motor_info[i]->motor_can_instance.can_handle == config->can_handle && dji_motor_info[i]->motor_can_instance.rx_id == config->rx_id)
IDcrash_Handler(); IDcrash_Handler(i, idx);
} }
break; break;
@ -110,22 +113,33 @@ static void MotorSenderGrouping(can_instance_config *config)
} }
/** /**
* @todo 是否可以简化多圈角度的计算？
* @brief 根据返回的can_instance对反馈报文进行解析 * @brief 根据返回的can_instance对反馈报文进行解析
* *
* @param _instance 收到数据的instance,通过遍历与所有电机进行对比以选择正确的实例 * @param _instance 收到数据的instance,通过遍历与所有电机进行对比以选择正确的实例
*/ */
static void DecodeDJIMotor(can_instance *_instance) static void DecodeDJIMotor(can_instance *_instance)
{ {
uint8_t *rxbuff = _instance->rx_buff; static uint8_t *rxbuff;
static dji_motor_measure *measure;
for (size_t i = 0; i < DJI_MOTOR_CNT; i++) for (size_t i = 0; i < DJI_MOTOR_CNT; i++)
{ {
if (&dji_motor_info[i]->motor_can_instance == _instance) if (&dji_motor_info[i]->motor_can_instance == _instance)
{ {
dji_motor_info[i]->motor_measure.last_ecd = dji_motor_info[i]->motor_measure.ecd; rxbuff = _instance->rx_buff;
dji_motor_info[i]->motor_measure.ecd = (uint16_t)(rxbuff[0] << 8 | rxbuff[1]); measure = &dji_motor_info[i]->motor_measure;
dji_motor_info[i]->motor_measure.speed_rpm = (uint16_t)(rxbuff[2] << 8 | rxbuff[3]); // resolve data and apply filter to current and speed
dji_motor_info[i]->motor_measure.given_current = (uint16_t)(rxbuff[4] << 8 | rxbuff[5]); measure->last_ecd = measure->ecd;
dji_motor_info[i]->motor_measure.temperate = rxbuff[6]; measure->ecd = (uint16_t)(rxbuff[0] << 8 | rxbuff[1]);
measure->speed_rpm = (1 - SPEED_SMOOTH_COEF) * measure->speed_rpm + SPEED_SMOOTH_COEF * (int16_t)(rxbuff[2] << 8 | rxbuff[3]);
measure->given_current = (1 - CURRENT_SMOOTH_COEF) * measure->given_current + CURRENT_SMOOTH_COEF * (uint16_t)(rxbuff[4] << 8 | rxbuff[5]);
measure->temperate = rxbuff[6];
// multi round calc
if (measure->ecd - measure->last_ecd > 4096)
measure->total_round--;
else if (measure->ecd - measure->last_ecd < -4096)
measure->total_round++;
measure->total_angle = measure->total_round * PI2 + measure->ecd * ECD_ANGLE_COEF; // @todo simplify the calculation
break; break;
} }
} }
@ -138,6 +152,7 @@ dji_motor_instance *DJIMotorInit(can_instance_config can_config,
Motor_Type_e type) Motor_Type_e type)
{ {
dji_motor_info[idx] = (dji_motor_instance *)malloc(sizeof(dji_motor_instance)); dji_motor_info[idx] = (dji_motor_instance *)malloc(sizeof(dji_motor_instance));
memset(dji_motor_info[idx], 0, sizeof(dji_motor_instance));
// motor basic setting // motor basic setting
dji_motor_info[idx]->motor_type = type; dji_motor_info[idx]->motor_type = type;
@ -159,15 +174,15 @@ dji_motor_instance *DJIMotorInit(can_instance_config can_config,
} }
// 改变反馈来源 // 改变反馈来源
void DJIMotorChangeFeed(dji_motor_instance *motor,Closeloop_Type_e loop,Feedback_Source_e type) void DJIMotorChangeFeed(dji_motor_instance *motor, Closeloop_Type_e loop, Feedback_Source_e type)
{ {
if(loop==ANGLE_LOOP) if (loop == ANGLE_LOOP)
{ {
motor->motor_settings.angle_feedback_source=type; motor->motor_settings.angle_feedback_source = type;
} }
if(loop==SPEED_LOOP) if (loop == SPEED_LOOP)
{ {
motor->motor_settings.speed_feedback_source=type; motor->motor_settings.speed_feedback_source = type;
} }
} }
@ -176,14 +191,13 @@ void DJIMotorSetRef(dji_motor_instance *motor, float ref)
{ {
motor->motor_controller.pid_ref = ref; motor->motor_controller.pid_ref = ref;
} }
// 计算三环PID,发送控制报文
void DJIMotorControl() void DJIMotorControl()
{ {
// 预先通过静态变量定义避免反复释放分配栈空间,直接保存一次指针引用从而减小访存的开销 // 预先通过静态变量定义避免反复释放分配栈空间,直接保存一次指针引用从而减小访存的开销
static uint8_t group, num; static uint8_t group, num;
static int16_t set; static int16_t set;
static dji_motor_instance* motor; static dji_motor_instance *motor;
static Motor_Control_Setting_s *motor_setting; static Motor_Control_Setting_s *motor_setting;
static Motor_Controller_s *motor_controller; static Motor_Controller_s *motor_controller;
static dji_motor_measure *motor_measure; static dji_motor_measure *motor_measure;
@ -193,33 +207,33 @@ void DJIMotorControl()
{ {
if (dji_motor_info[i]) if (dji_motor_info[i])
{ {
motor=dji_motor_info[i]; motor = dji_motor_info[i];
motor_setting=&motor->motor_settings; motor_setting = &motor->motor_settings;
motor_controller=&motor->motor_controller; motor_controller = &motor->motor_controller;
motor_measure=&motor->motor_measure; motor_measure = &motor->motor_measure;
if (motor_setting->close_loop_type & ANGLE_LOOP) // 计算位置环 if (motor_setting->close_loop_type & ANGLE_LOOP) // 计算位置环
{ {
if (motor_setting->angle_feedback_source == OTHER_FEED) if (motor_setting->angle_feedback_source == OTHER_FEED)
pid_measure=*motor_controller->other_angle_feedback_ptr; pid_measure = *motor_controller->other_angle_feedback_ptr;
else // MOTOR_FEED else // MOTOR_FEED
pid_measure=motor_measure->total_angle; pid_measure = motor_measure->total_angle;
// 更新pid_ref进入下一个环 // 更新pid_ref进入下一个环
motor_controller->pid_ref=PID_Calculate(&motor_controller->angle_PID,pid_measure,motor_controller->pid_ref); motor_controller->pid_ref = PID_Calculate(&motor_controller->angle_PID, pid_measure, motor_controller->pid_ref);
} }
if (motor_setting->close_loop_type & SPEED_LOOP) // 计算速度环 if (motor_setting->close_loop_type & SPEED_LOOP) // 计算速度环
{ {
if (motor_setting->speed_feedback_source == OTHER_FEED) if (motor_setting->speed_feedback_source == OTHER_FEED)
pid_measure=*motor_controller->other_speed_feedback_ptr; pid_measure = *motor_controller->other_speed_feedback_ptr;
else else
pid_measure=motor_measure->speed_rpm; pid_measure = motor_measure->speed_rpm;
motor_controller->pid_ref=PID_Calculate(&motor_controller->speed_PID,pid_measure,motor_controller->pid_ref); motor_controller->pid_ref = PID_Calculate(&motor_controller->speed_PID, pid_measure, motor_controller->pid_ref);
} }
if (motor_setting->close_loop_type & CURRENT_LOOP) //计算电流环 if (motor_setting->close_loop_type & CURRENT_LOOP) //计算电流环
{ {
motor_controller->pid_ref=PID_Calculate(&motor_controller->current_PID,motor_measure->given_current,motor_controller->pid_ref); motor_controller->pid_ref = PID_Calculate(&motor_controller->current_PID, motor_measure->given_current, motor_controller->pid_ref);
} }
set = (int16_t)motor_controller->pid_ref; set = (int16_t)motor_controller->pid_ref;
@ -230,7 +244,7 @@ void DJIMotorControl()
group = motor->sender_group; group = motor->sender_group;
num = motor->message_num; num = motor->message_num;
sender_assignment[group].tx_buff[num] = 0xff & set >> 8; sender_assignment[group].tx_buff[num] = 0xff & set >> 8;
sender_assignment[group].tx_buff[num+1] = 0xff & set; sender_assignment[group].tx_buff[num + 1] = 0xff & set;
} }
else // 遇到空指针说明所有遍历结束,退出循环 else // 遇到空指针说明所有遍历结束,退出循环
break; break;

38
modules/motor/dji_motor.h
View File

@ -1,7 +1,20 @@
/**
* @file dji_motor.h
* @author neozng
* @brief DJI智能电机头文件
* @version 0.1
* @date 2022-11-01
*
* @copyright Copyright (c) 2022 HNU YueLu EC all rights reserved
*
*/
#ifndef DJI_MOTOR_H #ifndef DJI_MOTOR_H
#define DJI_MOTOR_H #define DJI_MOTOR_H
#define DJI_MOTOR_CNT 12 #define DJI_MOTOR_CNT 12
#define SPEED_SMOOTH_COEF 0.85f // better to be greater than 0.8
#define CURRENT_SMOOTH_COEF 0.98f // this coef must be greater than 0.95
#include "bsp_can.h" #include "bsp_can.h"
#include "controller.h" #include "controller.h"
@ -45,7 +58,6 @@ typedef struct
} dji_motor_instance; } dji_motor_instance;
/** /**
* @brief 调用此函数注册一个DJI智能电机,需要传递较多的初始化参数,请在application初始化的时候调用此函数 * @brief 调用此函数注册一个DJI智能电机,需要传递较多的初始化参数,请在application初始化的时候调用此函数
* 推荐传参时像标准库一样构造initStructure然后传入此函数. * 推荐传参时像标准库一样构造initStructure然后传入此函数.
@ -55,20 +67,20 @@ typedef struct
* ....}; * ....};
* 请注意不要在一条总线上挂载过多的电机(超过6个),若一定要这么做,请降低每个电机的反馈频率(设为500Hz), * 请注意不要在一条总线上挂载过多的电机(超过6个),若一定要这么做,请降低每个电机的反馈频率(设为500Hz),
* 并减小DJIMotorControl()任务的运行频率. * 并减小DJIMotorControl()任务的运行频率.
* *
* @attention M3508和M2006的反馈报文都是0x200+id,而GM6020的反馈是0x204+id,请注意前两者和后者的id不要冲突. * @attention M3508和M2006的反馈报文都是0x200+id,而GM6020的反馈是0x204+id,请注意前两者和后者的id不要冲突.
* 如果产生冲突,在初始化电机的时候会进入IDcrash_Handler(),可以通过debug来判断是否出现冲突. * 如果产生冲突,在初始化电机的时候会进入IDcrash_Handler(),可以通过debug来判断是否出现冲突.
* *
* @param config 电机can设置,对于DJI电机仅需要将tx_id设置为电调闪动次数(c620,615,610)或拨码开关的值(GM6020) * @param config 电机can设置,对于DJI电机仅需要将tx_id设置为电调闪动次数(c620,615,610)或拨码开关的值(GM6020)
* 你不需要自己查表计算发送和接收id,我们会帮你处理好! * 你不需要自己查表计算发送和接收id,我们会帮你处理好!
* *
* @param motor_setting 电机的控制设置,包括是否反转,闭环类型和是否使用编码器之外的反馈值 * @param motor_setting 电机的控制设置,包括是否反转,闭环类型和是否使用编码器之外的反馈值
* *
* @param controller_init 电机控制器的参数设置,包括其他的反馈来源数据指针和三环PID的参数. * @param controller_init 电机控制器的参数设置,包括其他的反馈来源数据指针和三环PID的参数.
* 如果不需要其他数据来源或不需要三个环,将不需要指针置为NULL即可 * 如果不需要其他数据来源或不需要三个环,将不需要指针置为NULL即可
* *
* @param type 电机的类型枚举,包括m2006,m3508和gm6020 * @param type 电机的类型枚举,包括m2006,m3508和gm6020
* *
* @return dji_motor_instance* 返回一个电机实例指针给应用,方便其对电机的参考值进行设置,即调用DJIMotorSetRef() * @return dji_motor_instance* 返回一个电机实例指针给应用,方便其对电机的参考值进行设置,即调用DJIMotorSetRef()
*/ */
dji_motor_instance *DJIMotorInit(can_instance_config config, dji_motor_instance *DJIMotorInit(can_instance_config config,
@ -76,32 +88,28 @@ dji_motor_instance *DJIMotorInit(can_instance_config config,
Motor_Controller_Init_s controller_init, Motor_Controller_Init_s controller_init,
Motor_Type_e type); Motor_Type_e type);
/** /**
* @brief 被application层的应用调用,给电机设定参考值. * @brief 被application层的应用调用,给电机设定参考值.
* 对于应用,可以将电机视为传递函数为1的设备,不需要关心底层的闭环 * 对于应用,可以将电机视为传递函数为1的设备,不需要关心底层的闭环
* *
* @param motor 要设置的电机 * @param motor 要设置的电机
* @param ref 设定参考值 * @param ref 设定参考值
*/ */
void DJIMotorSetRef(dji_motor_instance *motor, float ref); void DJIMotorSetRef(dji_motor_instance *motor, float ref);
/** /**
* @brief 切换反馈的目标来源,如将角速度和角度的来源换为IMU(小陀螺模式常用) * @brief 切换反馈的目标来源,如将角速度和角度的来源换为IMU(小陀螺模式常用)
* *
* @param motor 要切换反馈数据来源的电机 * @param motor 要切换反馈数据来源的电机
* @param loop 要切换反馈数据来源的控制闭环 * @param loop 要切换反馈数据来源的控制闭环
* @param type 目标反馈模式 * @param type 目标反馈模式
*/ */
void DJIMotorChangeFeed(dji_motor_instance *motor,Closeloop_Type_e loop,Feedback_Source_e type); void DJIMotorChangeFeed(dji_motor_instance *motor, Closeloop_Type_e loop, Feedback_Source_e type);
/** /**
* @brief 该函数被motor_task调用运行在rtos上,motor_stask内通过osDelay()确定控制频率 * @brief 该函数被motor_task调用运行在rtos上,motor_stask内通过osDelay()确定控制频率
* @todo 增加前馈功能 * @todo 增加前馈功能
*/ */
void DJIMotorControl(); void DJIMotorControl();
#endif // !DJI_MOTOR_H #endif // !DJI_MOTOR_H

28
modules/motor/motor_def.h
View File

@ -1,6 +1,21 @@
/**
* @file motor_def.h
* @author neozng
* @brief 电机通用的数据结构定义
* @version beta
* @date 2022-11-01
*
* @copyright Copyright (c) 2022 HNU YueLu EC all rights reserved
*
*/
#ifndef MOTOR_DEF_H #ifndef MOTOR_DEF_H
#define MOTOR_DEF_H #define MOTOR_DEF_H
/**
* @brief 闭环类型,如果需要多个闭环,则使用或运算
* 例如需要速度环和电流环: CURRENT_LOOP|SPEED_LOOP
*/
typedef enum typedef enum
{ {
CURRENT_LOOP = 0b0001, CURRENT_LOOP = 0b0001,
@ -13,18 +28,21 @@ typedef enum
___ = 0b0111 ___ = 0b0111
} Closeloop_Type_e; } Closeloop_Type_e;
/* 反馈来源设定,若设为OTHER_FEED则需要指定数据来源指针,详见Motor_Controller_s*/
typedef enum typedef enum
{ {
MOTOR_FEED = 0, MOTOR_FEED = 0,
OTHER_FEED = 1 OTHER_FEED = 1
} Feedback_Source_e; } Feedback_Source_e;
/* 电机正反转标志 */
typedef enum typedef enum
{ {
MOTOR_DIRECTION_NORMAL = 0, MOTOR_DIRECTION_NORMAL = 0,
MOTOR_DIRECTION_REVERSE = 1 MOTOR_DIRECTION_REVERSE = 1
} Reverse_Flag_e; } Reverse_Flag_e;
/* 电机控制设置,包括闭环类型,反转标志和反馈来源 */
typedef struct typedef struct
{ {
Closeloop_Type_e close_loop_type; Closeloop_Type_e close_loop_type;
@ -34,6 +52,7 @@ typedef struct
} Motor_Control_Setting_s; } Motor_Control_Setting_s;
/* 电机控制器,包括其他来源的反馈数据指针,3环控制器和电机的参考输入*/
typedef struct typedef struct
{ {
float *other_angle_feedback_ptr; float *other_angle_feedback_ptr;
@ -43,10 +62,10 @@ typedef struct
PID_t speed_PID; PID_t speed_PID;
PID_t angle_PID; PID_t angle_PID;
// 将会作为每个环的输入和输出 float pid_ref; // 将会作为每个环的输入和输出顺次通过串级闭环
float pid_ref;
} Motor_Controller_s; } Motor_Controller_s;
/* 电机类型枚举 */
typedef enum typedef enum
{ {
GM6020 = 0, GM6020 = 0,
@ -56,6 +75,11 @@ typedef enum
HT04 = 4 HT04 = 4
} Motor_Type_e; } Motor_Type_e;
/**
* @brief 电机控制器初始化结构体,包括三环PID的配置以及两个反馈数据来源指针
* 如果不需要某个控制环,可以不设置对应的pid config
* 需要其他数据来源进行反馈闭环,不仅要设置这里的指针还需要在Motor_Control_Setting_s启用其他数据来源标志
*/
typedef struct typedef struct
{ {
float *other_angle_feedback_ptr; float *other_angle_feedback_ptr;

10
modules/motor/motor_task.h
View File

@ -1,3 +1,13 @@
/**
* @file motor_task.h
* @author neozng
* @brief 对所有电机,舵机等控制任务的进一步封装,MotorControlTask()将在操作系统中按一定频率调用
* @version beta
* @date 2022-11-01
*
* @copyright Copyright (c) 2022
*
*/
#ifndef MOTOR_TASK_H #ifndef MOTOR_TASK_H
#define MOTOR_TASK_H #define MOTOR_TASK_H

117
modules/remote/remote_control.c
View File

@ -2,90 +2,19 @@
#include "string.h" #include "string.h"
#include "bsp_usart.h" #include "bsp_usart.h"
#define RC_CHANNAL_ERROR_VALUE 700 #define REMOTE_CONTROL_FRAME_SIZE 18u // 遥控器接收的buffer大小
#define REMOTE_CONTROL_FRAME_SIZE 18u
#define RC_abs(x) (x)>0?(x):(-x)
#define RC_USART_SERVICE_ID 1
//remote control data // 遥控器数据
static RC_ctrl_t rc_ctrl; static RC_ctrl_t rc_ctrl;
// 遥控器拥有的串口实例
static usart_instance rc_usart_instance; static usart_instance rc_usart_instance;
/** /**
* @brief remote control protocol resolution * @brief remote control protocol resolution
* @param[in] sbus_buf: raw data point * @param[in] sbus_buf: raw data point
* @param[out] rc_ctrl: remote control data struct point * @param[out] rc_ctrl: remote control data struct point
* @retval none * @retval none
*/
static void sbus_to_rc(volatile const uint8_t *sbus_buf, RC_ctrl_t *rc_ctrl);
/**
* @brief protocol resolve callback
* this func would be called when usart3 idle interrupt happens
*
*/ */
static void ReceiveCallback()
{
sbus_to_rc(rc_usart_instance.recv_buff,&rc_ctrl);
}
void RC_init(UART_HandleTypeDef* rc_usart_handle)
{
rc_usart_instance.module_callback=ReceiveCallback;
rc_usart_instance.usart_handle=rc_usart_handle;
rc_usart_instance.recv_buff_size=REMOTE_CONTROL_FRAME_SIZE;
USARTRegister(&rc_usart_instance);
}
const RC_ctrl_t *get_remote_control_point(void)
{
return &rc_ctrl;
}
uint8_t RC_data_is_error(void)
{
if (RC_abs(rc_ctrl.rc.ch[0]) > RC_CHANNAL_ERROR_VALUE)
{
goto error;
}
if (RC_abs(rc_ctrl.rc.ch[1]) > RC_CHANNAL_ERROR_VALUE)
{
goto error;
}
if (RC_abs(rc_ctrl.rc.ch[2]) > RC_CHANNAL_ERROR_VALUE)
{
goto error;
}
if (RC_abs(rc_ctrl.rc.ch[3]) > RC_CHANNAL_ERROR_VALUE)
{
goto error;
}
if (rc_ctrl.rc.s[0] == 0)
{
goto error;
}
if (rc_ctrl.rc.s[1] == 0)
{
goto error;
}
return 0;
error:
rc_ctrl.rc.ch[0] = 0;
rc_ctrl.rc.ch[1] = 0;
rc_ctrl.rc.ch[2] = 0;
rc_ctrl.rc.ch[3] = 0;
rc_ctrl.rc.ch[4] = 0;
rc_ctrl.rc.s[0] = RC_SW_DOWN;
rc_ctrl.rc.s[1] = RC_SW_DOWN;
rc_ctrl.mouse.x = 0;
rc_ctrl.mouse.y = 0;
rc_ctrl.mouse.z = 0;
rc_ctrl.mouse.press_l = 0;
rc_ctrl.mouse.press_r = 0;
rc_ctrl.key.v = 0;
return 1;
}
static void sbus_to_rc(volatile const uint8_t *sbus_buf, RC_ctrl_t *rc_ctrl) static void sbus_to_rc(volatile const uint8_t *sbus_buf, RC_ctrl_t *rc_ctrl)
{ {
if (sbus_buf == NULL || rc_ctrl == NULL) if (sbus_buf == NULL || rc_ctrl == NULL)
@ -95,17 +24,18 @@ static void sbus_to_rc(volatile const uint8_t *sbus_buf, RC_ctrl_t *rc_ctrl)
rc_ctrl->rc.ch[0] = (sbus_buf[0] | (sbus_buf[1] << 8)) & 0x07ff; //!< Channel 0 rc_ctrl->rc.ch[0] = (sbus_buf[0] | (sbus_buf[1] << 8)) & 0x07ff; //!< Channel 0
rc_ctrl->rc.ch[1] = ((sbus_buf[1] >> 3) | (sbus_buf[2] << 5)) & 0x07ff; //!< Channel 1 rc_ctrl->rc.ch[1] = ((sbus_buf[1] >> 3) | (sbus_buf[2] << 5)) & 0x07ff; //!< Channel 1
rc_ctrl->rc.ch[2] = ((sbus_buf[2] >> 6) | (sbus_buf[3] << 2) | //!< Channel 2 rc_ctrl->rc.ch[2] = ((sbus_buf[2] >> 6) | (sbus_buf[3] << 2) | //!< Channel 2
(sbus_buf[4] << 10)) &0x07ff; (sbus_buf[4] << 10)) &
0x07ff;
rc_ctrl->rc.ch[3] = ((sbus_buf[4] >> 1) | (sbus_buf[5] << 7)) & 0x07ff; //!< Channel 3 rc_ctrl->rc.ch[3] = ((sbus_buf[4] >> 1) | (sbus_buf[5] << 7)) & 0x07ff; //!< Channel 3
rc_ctrl->rc.s[0] = ((sbus_buf[5] >> 4) & 0x0003); //!< Switch left rc_ctrl->rc.s[0] = ((sbus_buf[5] >> 4) & 0x0003); //!< Switch left
rc_ctrl->rc.s[1] = ((sbus_buf[5] >> 4) & 0x000C) >> 2; //!< Switch right rc_ctrl->rc.s[1] = ((sbus_buf[5] >> 4) & 0x000C) >> 2; //!< Switch right
rc_ctrl->mouse.x = sbus_buf[6] | (sbus_buf[7] << 8); //!< Mouse X axis rc_ctrl->mouse.x = sbus_buf[6] | (sbus_buf[7] << 8); //!< Mouse X axis
rc_ctrl->mouse.y = sbus_buf[8] | (sbus_buf[9] << 8); //!< Mouse Y axis rc_ctrl->mouse.y = sbus_buf[8] | (sbus_buf[9] << 8); //!< Mouse Y axis
rc_ctrl->mouse.z = sbus_buf[10] | (sbus_buf[11] << 8); //!< Mouse Z axis rc_ctrl->mouse.z = sbus_buf[10] | (sbus_buf[11] << 8); //!< Mouse Z axis
rc_ctrl->mouse.press_l = sbus_buf[12]; //!< Mouse Left Is Press ? rc_ctrl->mouse.press_l = sbus_buf[12]; //!< Mouse Left Is Press ?
rc_ctrl->mouse.press_r = sbus_buf[13]; //!< Mouse Right Is Press ? rc_ctrl->mouse.press_r = sbus_buf[13]; //!< Mouse Right Is Press ?
rc_ctrl->key.v = sbus_buf[14] | (sbus_buf[15] << 8); //!< KeyBoard value rc_ctrl->key.v = sbus_buf[14] | (sbus_buf[15] << 8); //!< KeyBoard value
rc_ctrl->rc.ch[4] = sbus_buf[16] | (sbus_buf[17] << 8); //NULL rc_ctrl->rc.ch[4] = sbus_buf[16] | (sbus_buf[17] << 8); // NULL
rc_ctrl->rc.ch[0] -= RC_CH_VALUE_OFFSET; rc_ctrl->rc.ch[0] -= RC_CH_VALUE_OFFSET;
rc_ctrl->rc.ch[1] -= RC_CH_VALUE_OFFSET; rc_ctrl->rc.ch[1] -= RC_CH_VALUE_OFFSET;
@ -114,3 +44,26 @@ static void sbus_to_rc(volatile const uint8_t *sbus_buf, RC_ctrl_t *rc_ctrl)
rc_ctrl->rc.ch[4] -= RC_CH_VALUE_OFFSET; rc_ctrl->rc.ch[4] -= RC_CH_VALUE_OFFSET;
} }
/**
* @brief protocol resolve callback
* this func would be called when usart3 idle interrupt happens
* 对sbus_to_rc的简单封装
*
*/
static void ReceiveCallback()
{
sbus_to_rc(rc_usart_instance.recv_buff, &rc_ctrl);
}
void RC_init(UART_HandleTypeDef *rc_usart_handle)
{
rc_usart_instance.module_callback = ReceiveCallback;
rc_usart_instance.usart_handle = rc_usart_handle;
rc_usart_instance.recv_buff_size = REMOTE_CONTROL_FRAME_SIZE;
USARTRegister(&rc_usart_instance);
}
const RC_ctrl_t *get_remote_control_point(void)
{
return &rc_ctrl;
}

139
modules/remote/remote_control.h
View File

@ -1,98 +1,91 @@
/** /**
****************************(C) COPYRIGHT 2016 DJI**************************** * @file remote_control.h
* @file remote_control.c/h * @author DJI 2016
* @brief 遥控器处理，遥控器是通过类似SBUS的协议传输，利用DMA传输方式节约CPU * @author modified by neozng
* 资源，利用串口空闲中断来拉起处理函数，同时提供一些掉线重启DMA，串口 * @brief 遥控器模块定义头文件
* 的方式保证热插拔的稳定性。 * @version beta
* @note * @date 2022-11-01
* @history *
* Version Date Author Modification * @copyright Copyright (c) 2016 DJI corp
* V1.0.0 Dec-26-2018 RM 1. done * @copyright Copyright (c) 2022 HNU YueLu EC all rights reserved
* V1.0.0 Nov-11-2019 RM 1. support development board tpye c *
* */
@verbatim
==============================================================================
==============================================================================
@endverbatim
****************************(C) COPYRIGHT 2016 DJI****************************
*/
#ifndef REMOTE_CONTROL_H #ifndef REMOTE_CONTROL_H
#define REMOTE_CONTROL_H #define REMOTE_CONTROL_H
#include "struct_typedef.h" #include "struct_typedef.h"
#include "main.h" #include "main.h"
#define RC_CH_VALUE_MIN ((uint16_t)364) #define RC_CH_VALUE_MIN ((uint16_t)364)
#define RC_CH_VALUE_OFFSET ((uint16_t)1024) #define RC_CH_VALUE_OFFSET ((uint16_t)1024)
#define RC_CH_VALUE_MAX ((uint16_t)1684) #define RC_CH_VALUE_MAX ((uint16_t)1684)
/* ----------------------- RC Switch Definition----------------------------- */ /* ----------------------- RC Switch Definition----------------------------- */
#define RC_SW_UP ((uint16_t)1) #define RC_SW_UP ((uint16_t)1)
#define RC_SW_MID ((uint16_t)3) #define RC_SW_MID ((uint16_t)3)
#define RC_SW_DOWN ((uint16_t)2) #define RC_SW_DOWN ((uint16_t)2)
#define switch_is_down(s) (s == RC_SW_DOWN) #define switch_is_down(s) (s == RC_SW_DOWN)
#define switch_is_mid(s) (s == RC_SW_MID) #define switch_is_mid(s) (s == RC_SW_MID)
#define switch_is_up(s) (s == RC_SW_UP) #define switch_is_up(s) (s == RC_SW_UP)
/* ----------------------- PC Key Definition-------------------------------- */ /* ----------------------- PC Key Definition-------------------------------- */
#define KEY_PRESSED_OFFSET_W ((uint16_t)1 << 0) #define KEY_PRESSED_OFFSET_W ((uint16_t)1 << 0)
#define KEY_PRESSED_OFFSET_S ((uint16_t)1 << 1) #define KEY_PRESSED_OFFSET_S ((uint16_t)1 << 1)
#define KEY_PRESSED_OFFSET_A ((uint16_t)1 << 2) #define KEY_PRESSED_OFFSET_A ((uint16_t)1 << 2)
#define KEY_PRESSED_OFFSET_D ((uint16_t)1 << 3) #define KEY_PRESSED_OFFSET_D ((uint16_t)1 << 3)
#define KEY_PRESSED_OFFSET_SHIFT ((uint16_t)1 << 4) #define KEY_PRESSED_OFFSET_SHIFT ((uint16_t)1 << 4)
#define KEY_PRESSED_OFFSET_CTRL ((uint16_t)1 << 5) #define KEY_PRESSED_OFFSET_CTRL ((uint16_t)1 << 5)
#define KEY_PRESSED_OFFSET_E ((uint16_t)1 << 7) #define KEY_PRESSED_OFFSET_E ((uint16_t)1 << 7)
#define KEY_PRESSED_OFFSET_Q ((uint16_t)1 << 6) #define KEY_PRESSED_OFFSET_Q ((uint16_t)1 << 6)
#define KEY_PRESSED_OFFSET_R ((uint16_t)1 << 8) #define KEY_PRESSED_OFFSET_R ((uint16_t)1 << 8)
#define KEY_PRESSED_OFFSET_F ((uint16_t)1 << 9) #define KEY_PRESSED_OFFSET_F ((uint16_t)1 << 9)
#define KEY_PRESSED_OFFSET_G ((uint16_t)1 << 10) #define KEY_PRESSED_OFFSET_G ((uint16_t)1 << 10)
#define KEY_PRESSED_OFFSET_Z ((uint16_t)1 << 11) #define KEY_PRESSED_OFFSET_Z ((uint16_t)1 << 11)
#define KEY_PRESSED_OFFSET_X ((uint16_t)1 << 12) #define KEY_PRESSED_OFFSET_X ((uint16_t)1 << 12)
#define KEY_PRESSED_OFFSET_C ((uint16_t)1 << 13) #define KEY_PRESSED_OFFSET_C ((uint16_t)1 << 13)
#define KEY_PRESSED_OFFSET_V ((uint16_t)1 << 14) #define KEY_PRESSED_OFFSET_V ((uint16_t)1 << 14)
#define KEY_PRESSED_OFFSET_B ((uint16_t)1 << 15) #define KEY_PRESSED_OFFSET_B ((uint16_t)1 << 15)
/* ----------------------- Data Struct ------------------------------------- */ /* ----------------------- Data Struct ------------------------------------- */
typedef struct typedef struct
{ {
struct struct
{ {
int16_t ch[5]; int16_t ch[5];
char s[2]; char s[2];
} rc; } rc;
struct struct
{ {
int16_t x; int16_t x;
int16_t y; int16_t y;
int16_t z; int16_t z;
uint8_t press_l; uint8_t press_l;
uint8_t press_r; uint8_t press_r;
} mouse; } mouse;
struct struct
{ {
uint16_t v; uint16_t v;
} key; } key;
} RC_ctrl_t; } RC_ctrl_t;
/* ----------------------- Internal Data ----------------------------------- */
/* ------------------------- Internal Data ----------------------------------- */
/** /**
* @brief register remote control into usart_serive * @brief 初始化遥控器,该函数会将遥控器注册到串口
* *
* @attention remember to assign the correct handler * @attention 注意分配正确的串口硬件,遥控器在C板上使用USART3
* *
*/ */
void RC_init(UART_HandleTypeDef* rc_usart_handle); void RC_init(UART_HandleTypeDef *rc_usart_handle);
/** /**
* @brief Get the remote control point object,then you can access data * @brief 获取遥控器的数据指针
* *
* @return const RC_ctrl_t* * @return const RC_ctrl_t*
*/ */
extern const RC_ctrl_t *get_remote_control_point(void); extern const RC_ctrl_t *get_remote_control_point(void);
/** /**
* @brief check whether there is an error in rc data * @brief 检查遥控器数据是否出错
* *
* @return uint8_t * @return uint8_t
*/ */
extern uint8_t RC_data_is_error(void); extern uint8_t RC_data_is_error(void);