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

29
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 "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];
/**
* @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)
{
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);
}
@ -22,21 +35,21 @@ void USARTRegister(usart_instance *_instance)
instance[instance_idx++] = _instance;
}
/* @todo 当前仅进行了形式上的封装,后续要进一步考虑是否将module的行为与bsp完全分离 */
void USARTSend(usart_instance *_instance, uint8_t *send_buf, uint16_t send_size)
{
HAL_UART_Transmit_DMA(_instance->usart_handle, send_buf, send_size);
}
/**
* @brief everytiem when dma/idle happens,this function will be called
* here, for each uart, specific callback is refered for further process
* etc:visual protocol resolve/remote control resolve/referee protocol resolve
* @brief 每次dma/idle中断发生时，都会调用此函数.对于每个uart实例会调用对应的回调进行进一步的处理
* 例如:视觉协议解析/遥控器解析/裁判系统解析
*
* @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)
*
* @param huart uart handle indicate which uart is being handled
* @param Size not used temporarily
* @param huart uart handle indicate which uart is being handled 发生中断的串口
* @param Size not used temporarily,此次接收到的总数居量,暂时没用
*/
void HAL_UARTEx_RxEventCallback(UART_HandleTypeDef *huart, uint16_t Size)
{

29
bsp/bsp_usart.h
View File

@ -4,37 +4,34 @@
#include "struct_typedef.h"
#include "main.h"
#define DEVICE_USART_CNT 3
#define USART_RX_LIMIT 128 // if your protocol needs bigger buff, modify here
#define DEVICE_USART_CNT 3 // C板至多分配3个串口
#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)();
/* usart_instance struct,each app would have one instance */
typedef struct
{
uint8_t recv_buff[USART_RX_LIMIT];
uint8_t recv_buff_size;
UART_HandleTypeDef *usart_handle;
usart_module_callback module_callback;
uint8_t recv_buff[USART_RXBUFF_LIMIT]; // 预先定义的最大buff大小,如果太小请修改USART_RXBUFF_LIMIT
uint8_t recv_buff_size; // 模块接收一包数据的大小
UART_HandleTypeDef *usart_handle; // 实例对应的usart_handle
usart_module_callback module_callback; // 解析收到的数据的回调函数
} usart_instance;
/**
* @brief calling this func would regiter a module to usart_service
* 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()
* @brief 注册一个串口实例.
*
* @param _instance 传入一个由module拥有的串口实例,注意在传入前进行基本信息的配置,详见usart_instance的struct定义
*/
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
* 通过调用该函数可以发送一帧数据,需要传入一个usart实例,发送buff以及这一帧的长度
*
* @param id specify which usart would be used
* @param send_size how many bytes to send

45
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 <memory.h>
/******************************* PID CONTROL *********************************/
// PID优化环节函数声明
static void f_Trapezoid_Intergral(PID_t *pid);
static void f_Integral_Limit(PID_t *pid);
static void f_Derivative_On_Measurement(PID_t *pid);
static void f_Changing_Integration_Rate(PID_t *pid);
static void f_Output_Filter(PID_t *pid);
static void f_Derivative_Filter(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_Trapezoid_Intergral(PID_t *pid); // 梯形积分
static void f_Integral_Limit(PID_t *pid); // 积分限幅
static void f_Derivative_On_Measurement(PID_t *pid); // 微分先行(仅使用反馈微分)
static void f_Changing_Integration_Rate(PID_t *pid); // 变速积分
static void f_Output_Filter(PID_t *pid); // 输出滤波
static void f_Derivative_Filter(PID_t *pid); // 微分滤波(采集时)
static void f_Output_Limit(PID_t *pid); // 输出限幅
static void f_PID_ErrorHandle(PID_t *pid); // 堵转保护
/**
* @brief
* @brief 初始化PID,设置参数和启用的优化环节,将其他数据置零
*
* @param pid
* @param config
* @param pid PID实例
* @param config PID初始化设置
*/
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));
// 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;
}
/**

64
modules/algorithm/controller.h
View File

@ -13,7 +13,6 @@
#ifndef _CONTROLLER_H
#define _CONTROLLER_H
#include "main.h"
#include "stdint.h"
#include "string.h"
@ -26,21 +25,21 @@
#define abs(x) ((x > 0) ? x : -x)
#endif
/******************************* PID CONTROL *********************************/
// PID 优化环节使能标志位
typedef enum pid_Improvement_e
{
NONE = 0X00, //0000 0000
Integral_Limit = 0x01, //0000 0001
Derivative_On_Measurement = 0x02, //0000 0010
Trapezoid_Intergral = 0x04, //0000 0100
Proportional_On_Measurement = 0x08, //0000 1000
OutputFilter = 0x10, //0001 0000
ChangingIntegrationRate = 0x20, //0010 0000
DerivativeFilter = 0x40, //0100 0000
ErrorHandle = 0x80, //1000 0000
NONE = 0b00000000, // 0000 0000
Integral_Limit = 0b00000001, // 0000 0001
Derivative_On_Measurement = 0b00000010, // 0000 0010
Trapezoid_Intergral = 0b00000100, // 0000 0100
Proportional_On_Measurement = 0b00001000, // 0000 1000
OutputFilter = 0b00010000, // 0001 0000
ChangingIntegrationRate = 0b00100000, // 0010 0000
DerivativeFilter = 0b01000000, // 0100 0000
ErrorHandle = 0b10000000, // 1000 0000
} PID_Improvement_e;
/* PID 报错类型枚举*/
typedef enum errorType_e
{
PID_ERROR_NONE = 0x00U,
@ -53,6 +52,7 @@ typedef struct
ErrorType_e ERRORType;
} PID_ErrorHandler_t;
/* PID结构体 */
typedef struct
{
//---------------------------------- init config block
@ -115,32 +115,22 @@ typedef struct
} PID_Init_config_s;
/**
* @brief 初始化PID实例
*
* @param pid PID实例指针
* @param config PID初始化配置
*/
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);
/*************************** 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

7
modules/imu/ins_task.c
View File

@ -16,7 +16,7 @@
#include "bsp_temperature.h"
#include "spi.h"
INS_t INS;
static INS_t INS;
IMU_Param_t IMU_Param;
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]);
attitude_t *GetINSptr()
{
return (attitude_t *)&INS.Roll;
}
void INS_Init(void)
{
// while (BMI088_init(&hspi1, 1) != BMI088_NO_ERROR);

76
modules/imu/ins_task.h
View File

@ -23,6 +23,14 @@
#define INS_TASK_PERIOD 1
typedef struct
{
float Roll;
float Pitch;
float Yaw;
float YawTotalAngle;
} attitude_t;
typedef struct
{
float q[4]; // 四元数估计值
@ -49,9 +57,8 @@ typedef struct
float YawTotalAngle;
} INS_t;
/**
* @brief 用于修正安装误差的参数,demo中可无视
* @brief 用于修正安装误差的参数
*
*/
typedef struct
@ -65,16 +72,79 @@ typedef struct
float Roll;
} IMU_Param_t;
extern INS_t INS;
/**
* @brief 反馈位姿结构体指针,apps通过初始化时保存该指针以实现数据访问
*
* @return attitude_t*
*/
attitude_t *GetINSptr();
/**
* @brief 初始化惯导解算系统
*
*/
void INS_Init(void);
/**
* @brief 此函数放入实时系统中,以1kHz频率运行
* p.s. osDelay(1);
*
*/
void INS_Task(void);
/**
* @brief 温控函数
*
*/
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);
/**
* @brief 四元数转换成欧拉角 ZYX
*
* @param q
* @param Yaw
* @param Pitch
* @param 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);
/**
* @brief 机体系到惯性系的变换函数
*
* @param vecBF body frame
* @param vecEF earth frame
* @param 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);
#endif

36
modules/motor/dji_motor.c
View File

@ -1,5 +1,8 @@
#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,是该文件的全局电机索引,在注册时使用
/* DJI电机的实例,此处仅保存指针,内存的分配将通过电机实例初始化时通过malloc()进行 */
@ -33,7 +36,7 @@ static uint8_t sender_enable_flag[6] = {0};
* @brief 当注册的电机id冲突时,会进入这个函数并提示冲突的ID
* @todo 通过segger jlink 发送日志
*/
static void IDcrash_Handler()
static void IDcrash_Handler(uint8_t conflict_motor_idx, uint8_t temp_motor_idx)
{
while (1)
{
@ -76,7 +79,7 @@ static void MotorSenderGrouping(can_instance_config *config)
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)
IDcrash_Handler();
IDcrash_Handler(i, idx);
}
break;
@ -100,7 +103,7 @@ static void MotorSenderGrouping(can_instance_config *config)
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)
IDcrash_Handler();
IDcrash_Handler(i, idx);
}
break;
@ -110,22 +113,33 @@ static void MotorSenderGrouping(can_instance_config *config)
}
/**
* @todo 是否可以简化多圈角度的计算？
* @brief 根据返回的can_instance对反馈报文进行解析
*
* @param _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++)
{
if (&dji_motor_info[i]->motor_can_instance == _instance)
{
dji_motor_info[i]->motor_measure.last_ecd = dji_motor_info[i]->motor_measure.ecd;
dji_motor_info[i]->motor_measure.ecd = (uint16_t)(rxbuff[0] << 8 | rxbuff[1]);
dji_motor_info[i]->motor_measure.speed_rpm = (uint16_t)(rxbuff[2] << 8 | rxbuff[3]);
dji_motor_info[i]->motor_measure.given_current = (uint16_t)(rxbuff[4] << 8 | rxbuff[5]);
dji_motor_info[i]->motor_measure.temperate = rxbuff[6];
rxbuff = _instance->rx_buff;
measure = &dji_motor_info[i]->motor_measure;
// resolve data and apply filter to current and speed
measure->last_ecd = measure->ecd;
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;
}
}
@ -138,6 +152,7 @@ dji_motor_instance *DJIMotorInit(can_instance_config can_config,
Motor_Type_e type)
{
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
dji_motor_info[idx]->motor_type = type;
@ -176,8 +191,7 @@ void DJIMotorSetRef(dji_motor_instance *motor, float ref)
{
motor->motor_controller.pid_ref = ref;
}
// 计算三环PID,发送控制报文
void DJIMotorControl()
{
// 预先通过静态变量定义避免反复释放分配栈空间,直接保存一次指针引用从而减小访存的开销

18
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
#define DJI_MOTOR_H
#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 "controller.h"
@ -45,7 +58,6 @@ typedef struct
} dji_motor_instance;
/**
* @brief 调用此函数注册一个DJI智能电机,需要传递较多的初始化参数,请在application初始化的时候调用此函数
* 推荐传参时像标准库一样构造initStructure然后传入此函数.
@ -76,7 +88,6 @@ dji_motor_instance *DJIMotorInit(can_instance_config config,
Motor_Controller_Init_s controller_init,
Motor_Type_e type);
/**
* @brief 被application层的应用调用,给电机设定参考值.
* 对于应用,可以将电机视为传递函数为1的设备,不需要关心底层的闭环
@ -86,7 +97,6 @@ dji_motor_instance *DJIMotorInit(can_instance_config config,
*/
void DJIMotorSetRef(dji_motor_instance *motor, float ref);
/**
* @brief 切换反馈的目标来源,如将角速度和角度的来源换为IMU(小陀螺模式常用)
*
@ -96,12 +106,10 @@ void DJIMotorSetRef(dji_motor_instance *motor, float ref);
*/
void DJIMotorChangeFeed(dji_motor_instance *motor, Closeloop_Type_e loop, Feedback_Source_e type);
/**
* @brief 该函数被motor_task调用运行在rtos上,motor_stask内通过osDelay()确定控制频率
* @todo 增加前馈功能
*/
void DJIMotorControl();
#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
#define MOTOR_DEF_H
/**
* @brief 闭环类型,如果需要多个闭环,则使用或运算
* 例如需要速度环和电流环: CURRENT_LOOP|SPEED_LOOP
*/
typedef enum
{
CURRENT_LOOP = 0b0001,
@ -13,18 +28,21 @@ typedef enum
___ = 0b0111
} Closeloop_Type_e;
/* 反馈来源设定,若设为OTHER_FEED则需要指定数据来源指针,详见Motor_Controller_s*/
typedef enum
{
MOTOR_FEED = 0,
OTHER_FEED = 1
} Feedback_Source_e;
/* 电机正反转标志 */
typedef enum
{
MOTOR_DIRECTION_NORMAL = 0,
MOTOR_DIRECTION_REVERSE = 1
} Reverse_Flag_e;
/* 电机控制设置,包括闭环类型,反转标志和反馈来源 */
typedef struct
{
Closeloop_Type_e close_loop_type;
@ -34,6 +52,7 @@ typedef struct
} Motor_Control_Setting_s;
/* 电机控制器,包括其他来源的反馈数据指针,3环控制器和电机的参考输入*/
typedef struct
{
float *other_angle_feedback_ptr;
@ -43,10 +62,10 @@ typedef struct
PID_t speed_PID;
PID_t angle_PID;
// 将会作为每个环的输入和输出
float pid_ref;
float pid_ref; // 将会作为每个环的输入和输出顺次通过串级闭环
} Motor_Controller_s;
/* 电机类型枚举 */
typedef enum
{
GM6020 = 0,
@ -56,6 +75,11 @@ typedef enum
HT04 = 4
} Motor_Type_e;
/**
* @brief 电机控制器初始化结构体,包括三环PID的配置以及两个反馈数据来源指针
* 如果不需要某个控制环,可以不设置对应的pid config
* 需要其他数据来源进行反馈闭环,不仅要设置这里的指针还需要在Motor_Control_Setting_s启用其他数据来源标志
*/
typedef struct
{
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
#define MOTOR_TASK_H

111
modules/remote/remote_control.c
View File

@ -2,13 +2,11 @@
#include "string.h"
#include "bsp_usart.h"
#define RC_CHANNAL_ERROR_VALUE 700
#define REMOTE_CONTROL_FRAME_SIZE 18u
#define RC_abs(x) (x)>0?(x):(-x)
#define RC_USART_SERVICE_ID 1
#define REMOTE_CONTROL_FRAME_SIZE 18u // 遥控器接收的buffer大小
//remote control data
// 遥控器数据
static RC_ctrl_t rc_ctrl;
// 遥控器拥有的串口实例
static usart_instance rc_usart_instance;
/**
@ -17,11 +15,39 @@ static usart_instance rc_usart_instance;
* @param[out] rc_ctrl: remote control data struct point
* @retval none
*/
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)
{
return;
}
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[2] = ((sbus_buf[2] >> 6) | (sbus_buf[3] << 2) | //!< Channel 2
(sbus_buf[4] << 10)) &
0x07ff;
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[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.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.press_l = sbus_buf[12]; //!< Mouse Left 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->rc.ch[4] = sbus_buf[16] | (sbus_buf[17] << 8); // NULL
rc_ctrl->rc.ch[0] -= RC_CH_VALUE_OFFSET;
rc_ctrl->rc.ch[1] -= RC_CH_VALUE_OFFSET;
rc_ctrl->rc.ch[2] -= RC_CH_VALUE_OFFSET;
rc_ctrl->rc.ch[3] -= 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()
@ -41,76 +67,3 @@ 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)
{
if (sbus_buf == NULL || rc_ctrl == NULL)
{
return;
}
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[2] = ((sbus_buf[2] >> 6) | (sbus_buf[3] << 2) | //!< Channel 2
(sbus_buf[4] << 10)) &0x07ff;
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[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.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.press_l = sbus_buf[12]; //!< Mouse Left 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->rc.ch[4] = sbus_buf[16] | (sbus_buf[17] << 8); //NULL
rc_ctrl->rc.ch[0] -= RC_CH_VALUE_OFFSET;
rc_ctrl->rc.ch[1] -= RC_CH_VALUE_OFFSET;
rc_ctrl->rc.ch[2] -= RC_CH_VALUE_OFFSET;
rc_ctrl->rc.ch[3] -= RC_CH_VALUE_OFFSET;
rc_ctrl->rc.ch[4] -= RC_CH_VALUE_OFFSET;
}

37
modules/remote/remote_control.h
View File

@ -1,21 +1,14 @@
/**
****************************(C) COPYRIGHT 2016 DJI****************************
* @file remote_control.c/h
* @brief 遥控器处理，遥控器是通过类似SBUS的协议传输，利用DMA传输方式节约CPU
* 资源，利用串口空闲中断来拉起处理函数，同时提供一些掉线重启DMA，串口
* 的方式保证热插拔的稳定性。
* @note
* @history
* Version Date Author Modification
* V1.0.0 Dec-26-2018 RM 1. done
* V1.0.0 Nov-11-2019 RM 1. support development board tpye c
* @file remote_control.h
* @author DJI 2016
* @author modified by neozng
* @brief 遥控器模块定义头文件
* @version beta
* @date 2022-11-01
*
* @copyright Copyright (c) 2016 DJI corp
* @copyright Copyright (c) 2022 HNU YueLu EC all rights reserved
*
@verbatim
==============================================================================
==============================================================================
@endverbatim
****************************(C) COPYRIGHT 2016 DJI****************************
*/
#ifndef REMOTE_CONTROL_H
#define REMOTE_CONTROL_H
@ -70,27 +63,27 @@ typedef struct
{
uint16_t v;
} key;
} 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);
/**
* @brief Get the remote control point object,then you can access data
* @brief 获取遥控器的数据指针
*
* @return const RC_ctrl_t*
*/
extern const RC_ctrl_t *get_remote_control_point(void);
/**
* @brief check whether there is an error in rc data
* @brief 检查遥控器数据是否出错
*
* @return uint8_t
*/