增加了大量注释

This commit is contained in:
NeoZng 2023-01-01 17:32:22 +08:00
parent c2f8b5c8c3
commit c05513587c
56 changed files with 773 additions and 598 deletions

View File

@ -103,27 +103,25 @@ HAL_N_Middlewares/Middlewares/Third_Party/SEGGER/RTT/SEGGER_RTT_printf.c \
HAL_N_Middlewares/Middlewares/Third_Party/SEGGER/RTT/SEGGER_RTT.c \
bsp/dwt/bsp_dwt.c \
bsp/pwm/bsp_pwm.c \
bsp/bsp_temperature.c \
bsp/bsp_led.c \
bsp/bsp_legacy_support/bsp_temperature.c \
bsp/bsp_legacy_support/bsp_buzzer.c \
bsp/bsp_legacy_support/bsp_led.c \
bsp/spi/bsp_spi.c \
bsp/iic/bsp_iic.c \
bsp/can/bsp_can.c \
bsp/bsp_buzzer.c \
bsp/usart/bsp_usart.c \
bsp/log/bsp_log.c \
bsp/bsp_init.c \
bsp/gpio/bsp_gpio.c \
modules/algorithm/controller.c \
modules/algorithm/kalman_filter.c \
modules/algorithm/QuaternionEKF.c \
modules/algorithm/crc8.c \
modules/algorithm/crc16.c \
modules/algorithm/user_lib.c \
modules/BMI088/bmi088.c \
modules/imu/BMI088driver.c \
modules/imu/BMI088Middleware.c \
modules/imu/ins_task.c \
modules/led_light/led_task.c \
modules/led/led_task.c \
modules/master_machine/master_process.c \
modules/master_machine/seasky_protocol.c \
modules/motor/dji_motor.c \
@ -229,16 +227,16 @@ C_INCLUDES = \
-Iapplication \
-Ibsp/dwt \
-Ibsp/can \
-Ibsp/gpio \
-Ibsp/usart \
-Ibsp/spi \
-Ibsp/iic \
-Ibsp/log \
-Ibsp/pwm \
-Ibsp/bsp_legacy_support \
-Ibsp \
-Imodules/algorithm \
-Imodules/imu \
-Imodules/led_light \
-Imodules/led \
-Imodules/master_machine \
-Imodules/motor \
-Imodules/referee \

234
README.md
View File

@ -197,24 +197,27 @@ Module层主要存放的是类型定义和实例指针数组在该层没有
板级支持包的每个组件,每个moduel,每个app都有对应的说明文档.
```shell
ROOT:.
│ .gitignore # git版本管理忽略文件
│ .mxproject # CubeMX项目文件
│ basic_framework.ioc # CubeMX初始化配置文件
│ Makefile # 编译管理文件,为make(mingw32-make)命令的目标
│ openocd_dap.cfg # 用于OpenOCD调试使用的配置文件
│ openocd_jlink.cfg # 同上
│ README.md # 本说明文档
│ .gitignore # git版本管理忽略文件
│ .mxproject # CubeMX项目文件
│ basic_framework.ioc # CubeMX初始化配置文件
│ LICENSE # 开源协议文件
│ Makefile # 编译管理文件,为make(mingw32-make)命令的目标
│ openocd_dap.cfg # 用于OpenOCD调试使用的配置文件,dap用
│ openocd_jlink.cfg # 用于OpenOCD调试使用的配置文件,jlink用
│ README.md # 本说明文档
│ startup_stm32f407xx.s # F407汇编启动文件
│ STM32F407.svd # F407外设地址映射文件,用于调试
│ STM32F407IGHx_FLASH.ld # F407IGH(C板使用的MCU)的文件目标FLASH地址,用于烧录和调试
│ stm32.jflash # 烧录的配置文件,一键下载用
│ STM32F407.svd # F407外设地址映射文件,用于调试
│ STM32F407IGHx_FLASH.ld # 包含了F407IGH(C板使用的MCU)的文件目标FLASH地址,用于编译(作为链接阶段的链接器),烧录和调试
│ TODO.md # 项目待完成的任务
│ VSCode+Ozone使用方法.md # 开发环境配置和前置知识介绍
|
├─.vscode
│ launch.json # 用于VSCode插件CORTEX-DEBUG调试的配置文件
settings.json# 工作区配置文件,设置了代码缩进和format风格等
tasks.json # 启动编译的任务配置文件
├─assets # markdown存放图片和外链文件夹
|
│ 修改HAL配置时文件目录的更改.md # 重新配置CubeMX时的步骤和注意事项
├─.vscode
launch.json # 调试的配置文件
settings.json # 工作区配置文件,根据自己的需要配置
│ tasks.json # 任务配置文件,包括一键编译下载调试等
├─application
│ │ application.md
│ │ APP层应用编写指引.md
@ -228,10 +231,9 @@ ROOT:.
│ │ chassis.md
│ │
│ ├─cmd
│ │ chassis_cmd.c
│ │ chassis_cmd.h
│ │ gimbal_cmd.c
│ │ gimbal_cmd.h
│ │ robot_cmd.c
│ │ robot_cmd.h
│ │ robot_cmd.md
│ │
│ ├─gimbal
│ │ gimbal.c
@ -243,108 +245,168 @@ ROOT:.
│ shoot.h
│ shoot.md
├─bsp # 板级支持包,提供对硬件的封装,将接口暴露给module层
│ bsp.md
│ bsp_buzzer.c
│ bsp_buzzer.h
│ bsp_can.c
│ bsp_can.h
│ bsp_can.md
│ bsp_dwt.c
│ bsp_dwt.h
│ bsp_init.c # bsp初始化
│ bsp_init.h
│ bsp_led.c
│ bsp_led.h
│ bsp_log.c
│ bsp_log.h
│ bsp_log.md
│ bsp_temperature.c
│ bsp_temperature.h
│ bsp_usart.c
│ bsp_usart.h
│ bsp_usart.md
├─assets # 说明文档的图片
|
├─HAL_N_Middlewares # HAL库对寄存器操作的封装,以及FreeRTOS/Segger RTT等中间件
|
|
└─modules # 模块层,使用BSP提供的接口构建对应的功能模块,将模块实例提供给应用层
| module.md
|
├─algorithm # 算法
├─bsp
│ │ bsp.md
│ │ bsp_buzzer.c
│ │ bsp_buzzer.h
│ │ bsp_init.c
│ │ bsp_init.h
│ │ bsp_led.c
│ │ bsp_led.h
│ │ bsp_spi.md
│ │ bsp_temperature.c
│ │ bsp_temperature.h
│ │
│ ├─adc
│ │ bsp_adc.c
│ │ bsp_adc.h
│ │ bsp_adc.md
│ │
│ ├─can
│ │ bsp_can.c
│ │ bsp_can.h
│ │ bsp_can.md
│ │
│ ├─dwt
│ │ bsp_dwt.c
│ │ bsp_dwt.h
│ │ bsp_dwt.md
│ │
│ ├─gpio
│ │ bsp_gpio.c
│ │ bsp_gpio.h
│ │ bsp_gpio.md
│ │
│ ├─iic
│ │ bsp_iic.c
│ │ bsp_iic.h
│ │ bsp_iic.md
│ │
│ ├─log
│ │ bsp_log.c
│ │ bsp_log.h
│ │ bsp_log.md
│ │
│ ├─pwm
│ │ bsp_pwm.c
│ │ bsp_pwm.h
│ │ bsp_pwm.md
│ │
│ ├─spi
│ │ bsp_spi.c
│ │ bsp_spi.h
│ │
│ ├─usart
│ │ bsp_usart.c
│ │ bsp_usart.h
│ │ bsp_usart.md
│ │
│ └─usb
└─modules
│ general_def.h
│ module.md
├─algorithm
│ algorithm.md
│ controller.c # 控制器
│ controller.c
│ controller.h
│ crc16.c # 循环冗余校验
│ crc16.c
│ crc16.h
│ crc8.c
│ crc8.h
│ kalman_filter.c # KF
│ kalman_filter.c
│ kalman_filter.h
│ LQR.c # LQR控制器
│ LQR.c
│ LQR.h
│ QuaternionEKF.c # 四元数EKF融合
│ QuaternionEKF.c
│ QuaternionEKF.h
│ user_lib.c # 多个模块都会使用到的函数
│ user_lib.c
│ user_lib.h
├─can_comm # 双板CAN通信组件
├─BMI088
│ bmi088.c
│ bmi088.h
│ bmi088_regNdef.h
├─can_comm
│ can_comm.c
│ can_comm.h
│ can_comm.md
|
├─imu # 考虑到使用SPI的设备较少,这里没有对SPI提供bsp支持,直接于此实现
│ can_comm.md
├─daemon
│ daemon.c
│ daemon.h
│ daemon.md
├─imu
│ BMI088driver.c
│ BMI088driver.h
│ BMI088Middleware.c
│ BMI088Middleware.h
│ BMI088reg.h
│ ins_task.c # 姿态解算任务,在RTOS中以1kHz运行
│ ins_task.c
│ ins_task.h
│ ins_task.md
├─led_light
│ led_task.c # 用于指示错误和主控是否正常运行,流水灯任务
│ led.md
│ led_task.c
│ led_task.h
├─master_machine # 和上位机(视觉PC)通信的模块
├─master_machine
│ master_process.c
│ master_process.h
│ master_process.md
│ seasky_protocol.c
│ master_process.md
│ seasky_protocol.c
│ seasky_protocol.h
│ 湖南大学RoboMaster电控组通信协议.md
├─message_center # 发布-订阅机制,app层应用之间交换数据用
├─message_center
│ message_center.c
│ message_center.h
│ message_center.md
|
├─motor # 电机模块
│ dji_motor.c # DJI智能电机
│ dji_motor.h
│ HT04.c # 海泰-04关节电机
│ HT04.h
│ LK9025.c # 瓴控9025驱动轮电机
│ LK9025.h
│ motor_def.h # 电机通用定义
│ motor_task.c # 电机控制任务,1kHz运行在RTOS上
│ motor_task.h
├─referee # 裁判系统模块
│ referee.c # 接收裁判系统信息
├─motor
│ dji_motor.c
│ dji_motor.h
│ dji_motor.md
│ HT04.c
│ HT04.h
│ LK9025.c
│ LK9025.h
│ motor_def.h
│ motor_task.c
│ motor_task.h
│ servo_motor.c
│ servo_motor.h
│ servo_motor.md
│ step_motor.c
│ step_motor.h
├─referee
│ crc.c
│ crc.h
│ referee.c
│ referee.h
│ referee_UI.c # UI绘制(发送)
│ referee_communication.c # 多机通信
|
├─remote # 遥控器模块
│ referee.md
│ referee_communication.c
│ referee_UI.c
├─remote
│ remote.md
│ remote_control.c
│ remote_control.h
└─super_cap # 超级电容
super_cap.c
super_cap.h
super_cap.md
├─super_cap
│ super_cap.c
│ super_cap.h
│ super_cap.md
└─vofa
vofa.c
vofa.h
```

View File

@ -23,9 +23,9 @@
#include "arm_math.h"
/* 根据robot_def.h中的macro自动计算的参数 */
#define HALF_WHEEL_BASE (WHEEL_BASE / 2.0f)
#define HALF_TRACK_WIDTH (TRACK_WIDTH / 2.0f)
#define PERIMETER_WHEEL (RADIUS_WHEEL * 2 * PI)
#define HALF_WHEEL_BASE (WHEEL_BASE / 2.0f) // 半轴距
#define HALF_TRACK_WIDTH (TRACK_WIDTH / 2.0f) // 半轮距
#define PERIMETER_WHEEL (RADIUS_WHEEL * 2 * PI) // 轮子周长
/* 底盘应用包含的模块和信息存储,底盘是单例模式,因此不需要为底盘建立单独的结构体 */
#ifdef CHASSIS_BOARD // 如果是底盘板,使用板载IMU获取底盘转动角速度
@ -35,16 +35,16 @@ static CANCommInstance *chasiss_can_comm; // 双板通信CAN comm
attitude_t *Chassis_IMU_data;
#endif // CHASSIS_BOARD
#ifdef ONE_BOARD
static Publisher_t *chassis_pub;
static Subscriber_t *chassis_sub;
static Publisher_t *chassis_pub; // 用于发布底盘的数据
static Subscriber_t *chassis_sub; // 用于订阅底盘的控制命令
#endif // ONE_BOARD
static Chassis_Ctrl_Cmd_s chassis_cmd_recv;
static Chassis_Upload_Data_s chassis_feedback_data;
static Chassis_Ctrl_Cmd_s chassis_cmd_recv; // 底盘接收到的控制命令
static Chassis_Upload_Data_s chassis_feedback_data; // 底盘回传的反馈数据
static referee_info_t *referee_data; // 裁判系统的数据
static SuperCapInstance *cap; // 超级电容
static DJIMotorInstance *motor_lf; // left right forward back
static DJIMotorInstance *motor_rf;
static DJIMotorInstance *motor_rf;
static DJIMotorInstance *motor_lb;
static DJIMotorInstance *motor_rb;
@ -125,7 +125,7 @@ void ChassisInit()
chasiss_can_comm = CANCommInit(&comm_conf); // can comm初始化
#endif // CHASSIS_BOARD
#ifdef ONE_BOARD
#ifdef ONE_BOARD // 单板控制整车,则通过pubsub来传递消息
chassis_sub = SubRegister("chassis_cmd", sizeof(Chassis_Ctrl_Cmd_s));
chassis_pub = PubRegister("chassis_feed", sizeof(Chassis_Upload_Data_s));
#endif // ONE_BOARD
@ -157,6 +157,7 @@ static void LimitChassisOutput()
// referee_data->PowerHeatData.chassis_power;
// referee_data->PowerHeatData.chassis_power_buffer;
// 完成功率限制后进行电机参考输入设定
DJIMotorSetRef(motor_lf, vt_lf);
DJIMotorSetRef(motor_rf, vt_rf);
DJIMotorSetRef(motor_lb, vt_lb);
@ -170,14 +171,15 @@ static void LimitChassisOutput()
*/
static void EstimateSpeed()
{
// 根据电机速度和imu的速度解算,利用加速度计判断是否打滑(如果有)
// 根据电机速度和陀螺仪的角速度进行解算,还可以利用加速度计判断是否打滑(如果有)
// chassis_feedback_data.vx vy wz =
// ...
}
/* 机器人底盘控制核心任务 */
void ChassisTask()
{
// 后续增加没收到消息的处理
// 后续增加没收到消息的处理(双板的情况)
// 获取新的控制信息
#ifdef ONE_BOARD
SubGetMessage(chassis_sub, &chassis_cmd_recv);
@ -204,14 +206,14 @@ void ChassisTask()
// 根据控制模式设定旋转速度
switch (chassis_cmd_recv.chassis_mode)
{
case CHASSIS_NO_FOLLOW:
chassis_cmd_recv.wz = 0; // 底盘不旋转,但维持全向机动,一般用于调整云台姿态
case CHASSIS_NO_FOLLOW: // 底盘不旋转,但维持全向机动,一般用于调整云台姿态
chassis_cmd_recv.wz = 0;
break;
case CHASSIS_FOLLOW_GIMBAL_YAW:
chassis_cmd_recv.wz = 0.05f * powf(chassis_cmd_recv.wz, 2.0f); // 跟随,不单独设置pid,以误差角度平方为速度输出
case CHASSIS_FOLLOW_GIMBAL_YAW: // 跟随云台,不单独设置pid,以误差角度平方为速度输出
chassis_cmd_recv.wz = 0.05f * powf(chassis_cmd_recv.wz, 2.0f);
break;
case CHASSIS_ROTATE:
// chassis_cmd_recv.wz=sin(t) // 自旋,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略
case CHASSIS_ROTATE: // 自旋,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略
// chassis_cmd_recv.wz=sin(t)
break;
default:
break;
@ -237,7 +239,7 @@ void ChassisTask()
// 获取裁判系统数据
// 我方颜色id小于7是红色,大于7是蓝色,注意这里发送的是对方的颜色, 0:blue , 1:red
chassis_feedback_data.enemy_color = referee_data->GameRobotStat.robot_id > 7 ? 1 : 0;
// 当前只做了17mm的数据获取,后续根据robot_def中的宏切换双枪管和英雄42mm的情况
// 当前只做了17mm热量的数据获取,后续根据robot_def中的宏切换双枪管和英雄42mm的情况
chassis_feedback_data.bullet_speed = referee_data->GameRobotStat.shooter_id1_17mm_speed_limit;
chassis_feedback_data.rest_heat = referee_data->PowerHeatData.shooter_heat0;

View File

@ -8,8 +8,8 @@
#include "dji_motor.h"
// 私有宏,自动将编码器转换成角度值
#define YAW_ALIGN_ANGLE (YAW_CHASSIS_ALIGN_ECD * ECD_ANGLE_COEF_DJI)
#define PTICH_HORIZON_ANGLE (PITCH_HORIZON_ECD * ECD_ANGLE_COEF_DJI)
#define YAW_ALIGN_ANGLE (YAW_CHASSIS_ALIGN_ECD * ECD_ANGLE_COEF_DJI) // 对齐时的角度,0-360
#define PTICH_HORIZON_ANGLE (PITCH_HORIZON_ECD * ECD_ANGLE_COEF_DJI) // pitch水平时电机的角度,0-360
/* gimbal_cmd应用包含的模块实例指针和交互信息存储*/
#ifdef GIMBAL_BOARD // 对双板的兼容,条件编译
@ -40,7 +40,7 @@ static Shoot_Upload_Data_s shoot_fetch_data; // 从发射获取的反馈信息
static Robot_Status_e robot_state; // 机器人整体工作状态
void GimbalCMDInit()
void RobotCMDInit()
{
rc_data = RemoteControlInit(&huart3); // 修改为对应串口,注意如果是自研板dbus协议串口需选用添加了反相器的那个
vision_recv_data = VisionInit(&huart1); // 视觉通信串口
@ -77,8 +77,9 @@ void GimbalCMDInit()
*/
static void CalcOffsetAngle()
{
static float angle; // 提高可读性,不然太长了不好看,虽然基本不会动这个函数
angle = gimbal_fetch_data.yaw_motor_single_round_angle;
// 别名angle提高可读性,不然太长了不好看,虽然基本不会动这个函数
static float angle;
angle = gimbal_fetch_data.yaw_motor_single_round_angle; // 从云台获取的当前yaw电机单圈角度
#if YAW_ECD_GREATER_THAN_4096 // 如果大于180度
if (angle > YAW_ALIGN_ANGLE && angle <= 180.0f + YAW_ALIGN_ANGLE)
chassis_cmd_send.offset_angle = angle - YAW_ALIGN_ANGLE;
@ -122,17 +123,17 @@ static void RemoteControlSet()
gimbal_cmd_send.gimbal_mode = GIMBAL_FREE_MODE;
}
// 底盘参数,目前没有加入小陀螺(调试似乎没有必要),系数需要调整
chassis_cmd_send.vx = 10.0f * (float)rc_data[TEMP].rc.rocker_r_;
chassis_cmd_send.vy = 10.0f * (float)rc_data[TEMP].rc.rocker_r1;
// 底盘参数,目前没有加入小陀螺(调试似乎暂时没有必要),系数需要调整
chassis_cmd_send.vx = 10.0f * (float)rc_data[TEMP].rc.rocker_r_; // _水平方向
chassis_cmd_send.vy = 10.0f * (float)rc_data[TEMP].rc.rocker_r1; // 1数值方向
// 发射参数
if (switch_is_up(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[上],弹舱打开
{ // 弹舱舵机控制,待添加servo_motor模块,开启
}
; // 弹舱舵机控制,待添加servo_motor模块,开启
else
; // 弹舱舵机控制,待添加servo_motor模块,关闭
// 摩擦轮控制,后续可以根据左侧拨轮的值大小切换射频
// 摩擦轮控制,拨轮向上打为负,向下为正
if (rc_data[TEMP].rc.dial < -100)
shoot_cmd_send.friction_mode = FRICTION_ON;
else
@ -142,6 +143,7 @@ static void RemoteControlSet()
shoot_cmd_send.load_mode = LOAD_BURSTFIRE;
else
shoot_cmd_send.load_mode = LOAD_STOP;
// 射频控制,固定每秒1发,后续可以根据左侧拨轮的值大小切换射频,
shoot_cmd_send.shoot_rate = 1;
}
@ -151,31 +153,36 @@ static void RemoteControlSet()
*/
static void MouseKeySet()
{
// 待添加键鼠控制
// ...
}
/**
* @brief ,/线/
* '300',
* @todo 线()
* '300',.
*
* @todo 线(),daemon实现
*
*/
static void EmergencyHandler()
{
// 拨轮的向下拨超过一半,注意向打时下拨轮是正
// 拨轮的向下拨超过一半进入急停模式.注意向打时下拨轮是正
if (rc_data[TEMP].rc.dial > 300 || robot_state == ROBOT_STOP) // 还需添加重要应用和模块离线的判断
{
robot_state = ROBOT_STOP; // 遥控器左上侧拨轮打满,进入紧急停止模式
robot_state = ROBOT_STOP;
gimbal_cmd_send.gimbal_mode = GIMBAL_ZERO_FORCE;
chassis_cmd_send.chassis_mode = CHASSIS_ZERO_FORCE;
shoot_cmd_send.shoot_mode = SHOOT_OFF;
}
// 遥控器右侧开关为[上],恢复正常运行
if (switch_is_up(rc_data[TEMP].rc.switch_right))
{
robot_state = ROBOT_READY; // 遥控器右侧开关为[上],恢复正常运行
robot_state = ROBOT_READY;
shoot_cmd_send.shoot_mode = SHOOT_ON;
}
}
/* 机器人核心控制任务,200Hz频率运行(必须高于视觉发送频率) */
void RobotCMDTask()
{
// 从其他应用获取回传数据
@ -188,9 +195,10 @@ void RobotCMDTask()
SubGetMessage(shoot_feed_sub, &shoot_fetch_data);
SubGetMessage(gimbal_feed_sub, &gimbal_fetch_data);
// 根据gimbal的反馈值计算云台和底盘正方向的夹角,不需要传参,通过私有变量完成
// 根据gimbal的反馈值计算云台和底盘正方向的夹角,不需要传参,通过static私有变量完成
CalcOffsetAngle();
// 根据遥控器左侧开关,确定当前使用的控制模式为遥控器调试还是键鼠
if (switch_is_down(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[下],遥控器控制
RemoteControlSet();
else if (switch_is_up(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[上],键盘控制
@ -206,7 +214,7 @@ void RobotCMDTask()
vision_send_data.roll = gimbal_fetch_data.gimbal_imu_data.Roll;
// 推送消息,双板通信,视觉通信等
// 应用所需的控制数据在remotecontrolsetmode和mousekeysetmode中完成设置
// 其他应用所需的控制数据在remotecontrolsetmode和mousekeysetmode中完成设置
#ifdef ONE_BOARD
PubPushMessage(chassis_cmd_pub, (void *)&chassis_cmd_send);
#endif // ONE_BOARD

View File

@ -1,7 +1,7 @@
#ifndef GIMBAL_CMD_H
#define GIMBAL_CMD_H
void GimbalCMDInit();
void RobotCMDInit();
void RobotCMDTask();

View File

@ -91,6 +91,7 @@ void GimbalInit()
gimbal_sub = SubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s));
}
/* 机器人云台控制核心任务,后续考虑只保留IMU控制,不再需要电机的反馈 */
void GimbalTask()
{
// 获取云台控制数据
@ -106,7 +107,7 @@ void GimbalTask()
DJIMotorStop(pitch_motor);
break;
// 使用陀螺仪的反馈,底盘根据yaw电机的offset跟随云台或视觉模式采用
case GIMBAL_GYRO_MODE:
case GIMBAL_GYRO_MODE: // 后续只保留此模式
DJIMotorEnable(yaw_motor);
DJIMotorEnable(pitch_motor);
DJIMotorChangeFeed(yaw_motor, ANGLE_LOOP, OTHER_FEED);
@ -117,7 +118,7 @@ void GimbalTask()
DJIMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch);
break;
// 云台自由模式,使用编码器反馈,底盘和云台分离,仅云台旋转,一般用于调整云台姿态(英雄吊射等)/能量机关
case GIMBAL_FREE_MODE:
case GIMBAL_FREE_MODE: // 后续删除,或加入云台追地盘的跟随模式(响应速度更快)
DJIMotorEnable(yaw_motor);
DJIMotorEnable(pitch_motor);
DJIMotorChangeFeed(yaw_motor, ANGLE_LOOP, MOTOR_FEED);
@ -131,7 +132,7 @@ void GimbalTask()
break;
}
// 设置反馈数据
// 设置反馈数据,主要是imu和yaw的ecd
gimbal_feedback_data.gimbal_imu_data = *Gimbal_IMU_data;
gimbal_feedback_data.yaw_motor_single_round_angle = yaw_motor->motor_measure.angle_single_round;

View File

@ -1,8 +1,16 @@
#ifndef GIMBAL_H
#define GIMBAL_H
/**
* @brief
*
*/
void GimbalInit();
/**
* @brief
*
*/
void GimbalTask();
#endif // GIMBAL_H

View File

@ -16,7 +16,7 @@ void RobotInit()
BSPInit();
#if defined(ONE_BOARD) || defined(GIMBAL_BOARD)
GimbalCMDInit();
RobotCMDInit();
GimbalInit();
ShootInit();
#endif

View File

@ -21,10 +21,18 @@
// #define CHASSIS_BOARD //底盘板
// #define GIMBAL_BOARD //云台板
/* 机器人类型定义 */
// #define ROBOT_HERO 1 // 英雄机器人
// #define ROBOT_ENINEER 2 // 工程机器人
#define ROBOT_INFANTRY 3 // 步兵机器人3
// #define ROBOT_INFANTRY 4 // 步兵机器人4
// #define ROBOT_INFANTRY 5 // 步兵机器人5
// #define ROBOT_SENTRY 6 // 哨兵机器人
/* 机器人重要参数定义,注意根据不同机器人进行修改,浮点数需要以.0或f结尾,无符号以u结尾 */
// 云台参数
#define YAW_CHASSIS_ALIGN_ECD 4000 // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改
#define YAW_ECD_GREATER_THAN_4096 0 // yaw电机的初始编码器值是否大于4096,是为1,否为0
#define YAW_CHASSIS_ALIGN_ECD 4000 // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改
#define YAW_ECD_GREATER_THAN_4096 0 // ALIGN_ECD值是否大于4096,是为1,否为0;用于计算云台偏转角度
#define PITCH_HORIZON_ECD 0 // 云台处于水平位置时编码器值,若对云台有机械改动需要修改
// 发射参数
#define ONE_BULLET_DELTA_ANGLE 0 // 发射一发弹丸拨盘转动的距离,由机械设计图纸给出
@ -55,14 +63,14 @@
// 机器人状态
typedef enum
{
ROBOT_STOP=0,
ROBOT_STOP = 0,
ROBOT_READY,
} Robot_Status_e;
// 应用状态
typedef enum
{
APP_OFFLINE=0,
APP_OFFLINE = 0,
APP_ONLINE,
APP_ERROR,
} App_Status_e;
@ -74,7 +82,7 @@ typedef enum
*/
typedef enum
{
CHASSIS_ZERO_FORCE=0, // 电流零输入
CHASSIS_ZERO_FORCE = 0, // 电流零输入
CHASSIS_ROTATE, // 小陀螺模式
CHASSIS_NO_FOLLOW, // 不跟随,允许全向平移
CHASSIS_FOLLOW_GIMBAL_YAW, // 跟随模式,底盘叠加角度环控制
@ -83,9 +91,9 @@ typedef enum
// 云台模式设置
typedef enum
{
GIMBAL_ZERO_FORCE=0, // 电流零输入
GIMBAL_FREE_MODE, // 云台自由运动模式,即与底盘分离(底盘此时应为NO_FOLLOW)反馈值为电机total_angle;似乎可以改为全部用IMU数据?
GIMBAL_GYRO_MODE, // 云台陀螺仪反馈模式,反馈值为陀螺仪pitch,total_yaw_angle,底盘可以为小陀螺和跟随模式
GIMBAL_ZERO_FORCE = 0, // 电流零输入
GIMBAL_FREE_MODE, // 云台自由运动模式,即与底盘分离(底盘此时应为NO_FOLLOW)反馈值为电机total_angle;似乎可以改为全部用IMU数据?
GIMBAL_GYRO_MODE, // 云台陀螺仪反馈模式,反馈值为陀螺仪pitch,total_yaw_angle,底盘可以为小陀螺和跟随模式
} gimbal_mode_e;
// 发射模式设置
@ -96,19 +104,19 @@ typedef enum
} shoot_mode_e;
typedef enum
{
FRICTION_OFF=0, // 摩擦轮关闭
FRICTION_ON, // 摩擦轮开启
FRICTION_OFF = 0, // 摩擦轮关闭
FRICTION_ON, // 摩擦轮开启
} friction_mode_e;
typedef enum
{
LID_OPEN=0, // 弹舱盖打开
LID_CLOSE, // 弹舱盖关闭
LID_OPEN = 0, // 弹舱盖打开
LID_CLOSE, // 弹舱盖关闭
} lid_mode_e;
typedef enum
{
LOAD_STOP=0, // 停止发射
LOAD_STOP = 0, // 停止发射
LOAD_REVERSE, // 反转
LOAD_1_BULLET, // 单发
LOAD_3_BULLET, // 三发

View File

@ -108,7 +108,7 @@ void ShootInit()
.controller_setting_init_config = {
.angle_feedback_source = MOTOR_FEED, .speed_feedback_source = MOTOR_FEED,
.outer_loop_type = SPEED_LOOP, // 初始化成SPEED_LOOP,让拨盘停在原地,防止拨盘上电时乱转
.close_loop_type = ANGLE_LOOP | SPEED_LOOP ,
.close_loop_type = ANGLE_LOOP | SPEED_LOOP,
.reverse_flag = MOTOR_DIRECTION_NORMAL, // 注意方向设置为拨盘的拨出的击发方向
},
.motor_type = M2006 // 英雄使用m3508
@ -122,12 +122,13 @@ void ShootInit()
shoot_sub = SubRegister("shoot_cmd", sizeof(Shoot_Ctrl_Cmd_s));
}
/* 机器人发射机构控制核心任务 */
void ShootTask()
{
// 从cmd获取控制数据
SubGetMessage(shoot_sub, &shoot_cmd_recv);
// 对shoot mode等于SHOOT_STOP的情况特殊处理,直接停止所有电机
// 对shoot mode等于SHOOT_STOP的情况特殊处理,直接停止所有电机(紧急停止)
if (shoot_cmd_recv.shoot_mode == SHOOT_OFF)
{
DJIMotorStop(friction_l);
@ -139,77 +140,83 @@ void ShootTask()
DJIMotorEnable(friction_l);
DJIMotorEnable(friction_r);
DJIMotorEnable(loader);
}
// 如果上一次触发单发或3发指令的时间加上不应期仍然大于当前时间(尚未休眠完毕),直接返回即可
// 单发模式主要提供给能量机关激活使用(以及英雄的射击大部分处于单发)
if (hibernate_time + dead_time > DWT_GetTimeline_ms())
return;
// 若不在休眠状态,根据控制模式进行拨盘电机参考值设定和模式切换
// 若不在休眠状态,根据robotCMD传来的控制模式进行拨盘电机参考值设定和模式切换
switch (shoot_cmd_recv.load_mode)
{
// 停止拨盘
case LOAD_STOP:
DJIMotorOuterLoop(loader, SPEED_LOOP);
DJIMotorSetRef(loader, 0);
DJIMotorOuterLoop(loader, SPEED_LOOP); // 切换到速度环
DJIMotorSetRef(loader, 0); // 同时设定参考值为0,这样停止的速度最快
break;
// 单发模式,根据鼠标按下的时间,触发一次之后需要进入不响应输入的状态(否则按下的时间内可能多次进入)
// 单发模式,根据鼠标按下的时间,触发一次之后需要进入不响应输入的状态(否则按下的时间内可能多次进入,导致多次发射)
case LOAD_1_BULLET: // 激活能量机关/干扰对方用,英雄用.
DJIMotorOuterLoop(loader, ANGLE_LOOP);
DJIMotorSetRef(loader, loader->motor_measure.total_angle + ONE_BULLET_DELTA_ANGLE); // 增加一发弹丸的角度
DJIMotorOuterLoop(loader, ANGLE_LOOP); // 切换到角度环
DJIMotorSetRef(loader, loader->motor_measure.total_angle + ONE_BULLET_DELTA_ANGLE); // 控制量增加一发弹丸的角度
hibernate_time = DWT_GetTimeline_ms(); // 记录触发指令的时间
dead_time = 150; // 完成1发弹丸发射的时间
break;
// 三连发,如果不需要后续可能删除
case LOAD_3_BULLET:
DJIMotorOuterLoop(loader, ANGLE_LOOP);
DJIMotorOuterLoop(loader, ANGLE_LOOP); // 切换到速度环
DJIMotorSetRef(loader, loader->motor_measure.total_angle + 3 * ONE_BULLET_DELTA_ANGLE); // 增加3发
hibernate_time = DWT_GetTimeline_ms(); // 记录触发指令的时间
dead_time = 300; // 完成3发弹丸发射的时间
break;
// 连发模式,对速度闭环,射频后续修改为可变
// 连发模式,对速度闭环,射频后续修改为可变,目前固定为1Hz
case LOAD_BURSTFIRE:
DJIMotorOuterLoop(loader, SPEED_LOOP);
DJIMotorSetRef(loader, shoot_cmd_recv.shoot_rate * 360 * REDUCTION_RATIO_LOADER / 8);
// x颗/秒换算成速度: 已知一圈的载弹量,由此计算出1s需要转的角度,注意换算角速度
// x颗/秒换算成速度: 已知一圈的载弹量,由此计算出1s需要转的角度,注意换算角速度(DJIMotor的速度单位是angle per second)
break;
// 拨盘反转,对速度闭环,后续增加卡弹检测(通过裁判系统剩余热量反馈)
// 可能需要从switch-case中独立出来
// 拨盘反转,对速度闭环,后续增加卡弹检测(通过裁判系统剩余热量反馈和电机电流)
// 也有可能需要从switch-case中独立出来
case LOAD_REVERSE:
DJIMotorOuterLoop(loader, SPEED_LOOP);
// ...
break;
default:
break;
while (1)
; // 未知模式,停止运行,检查指针越界,内存溢出等问题
}
// 根据收到的弹速设置设定摩擦轮电机参考值,需实测后填入
switch (shoot_cmd_recv.bullet_speed)
// 确定是否开启摩擦轮,后续可能修改为键鼠模式下始终开启摩擦轮(上场时建议一直开启)
if (shoot_cmd_recv.friction_mode == FRICTION_ON)
{
case SMALL_AMU_15:
DJIMotorSetRef(friction_l, 0);
DJIMotorSetRef(friction_r, 0);
break;
case SMALL_AMU_18:
DJIMotorSetRef(friction_l, 0);
DJIMotorSetRef(friction_r, 0);
break;
case SMALL_AMU_30:
DJIMotorSetRef(friction_l, 0);
DJIMotorSetRef(friction_r, 0);
break;
default:
DJIMotorSetRef(friction_l, 4000);
DJIMotorSetRef(friction_r, 4000);
break;
} // 关闭摩擦轮
if (shoot_cmd_recv.friction_mode==FRICTION_OFF)
// 根据收到的弹速设置设定摩擦轮电机参考值,需实测后填入
switch (shoot_cmd_recv.bullet_speed)
{
case SMALL_AMU_15:
DJIMotorSetRef(friction_l, 0);
DJIMotorSetRef(friction_r, 0);
break;
case SMALL_AMU_18:
DJIMotorSetRef(friction_l, 0);
DJIMotorSetRef(friction_r, 0);
break;
case SMALL_AMU_30:
DJIMotorSetRef(friction_l, 0);
DJIMotorSetRef(friction_r, 0);
break;
default: // 当前为了调试设定的4000,因为还没有加入裁判系统无法读取弹速.后续修改为while(1);表明模式错误
DJIMotorSetRef(friction_l, 4000);
DJIMotorSetRef(friction_r, 4000);
break;
}
}
else // 关闭摩擦轮
{
DJIMotorSetRef(friction_l, 0);
DJIMotorSetRef(friction_r, 0);
}
// 开关弹舱盖
if (shoot_cmd_recv.lid_mode == LID_CLOSE)
{
@ -220,6 +227,6 @@ void ShootTask()
//...
}
// 反馈数据
PubPushMessage(shoot_pub,(void*)&shoot_feedback_data);
// 反馈数据,目前暂时没有要设定的反馈数据,后续可能增加应用离线监测以及卡弹反馈
PubPushMessage(shoot_pub, (void *)&shoot_feedback_data);
}

View File

@ -1,8 +1,16 @@
#ifndef SHOOT_H
#define SHOOT_H
/**
* @brief
*
*/
void ShootInit();
/**
* @brief
*
*/
void ShootTask();
#endif // SHOOT_H

View File

@ -0,0 +1,3 @@
待添加adc的bsp支持,应该提供阻塞/IT/DMA的量测接口
是否需要bsp_adc?由于功能非常简单,似乎可以直接使用HAL的接口,没有必要多此一举进行封装?

View File

@ -10,6 +10,8 @@ void BSPInit()
{
DWT_Init(168);
BSPLogInit();
// 下面都是待删除的,将在实现了module之后移动到app层
LEDInit();
IMUTempInit();
BuzzerInit();

View File

@ -3,7 +3,7 @@
/**
* @brief bsp层初始化统一入口
* @brief bsp层初始化统一入口,bsp组件,
*
*/
void BSPInit();

View File

@ -0,0 +1,3 @@
# legacy bsp
这些bsp实现将在v0.1删除,因为他们实际上都是用pwm实现的,应当放在module层,以彻底隔离bsp和CubeMX的初始化代码.之后在修改CubeMX的初始化配置之后就不再需要修改bsp的内容了,所有的修改都会通过app层的初始化配置`xxx_Init_Config_s`来实现.

View File

@ -5,23 +5,21 @@
/* can instance ptrs storage, used for recv callback */
// 在CAN产生接收中断会遍历数组,选出hcan和rxid与发生中断的实例相同的那个,调用其回调函数
static CANInstance *can_instance[MX_REGISTER_DEVICE_CNT] = {NULL};
static CANInstance *can_instance[CAN_MX_REGISTER_CNT] = {NULL};
static uint8_t idx; // 全局CAN实例索引,每次有新的模块注册会自增
/* ----------------two static function called by CANRegister()-------------------- */
/**
* @brief add filter to receive mesg with specific ID,called by CANRegister()
* @brief id的报文的接收,CANRegister()
* CAN添加过滤器后,BxCAN会根据接收到的报文的id进行消息过滤,id会被填入FIFO触发中断
*
* @note there are total 28 filter and 2 FIFO in bxCAN of STM32F4 series product.
* here, we assign the former 14 to CAN1 and the rest for CAN2
* when initializing, module with odd ID will be assigned to FIFO0 while even one to FIFO1
* those modules which registered in CAN1 would use Filter0-13, while CAN2 use Filter14-27
* @note f407的bxCAN有28个过滤器,14CAN1使用,14CAN2使用
* ,id的模块会被分配到FIFO0,id的模块会被分配到FIFO1
* CAN1的模块使用过滤器0-13,CAN2使用过滤器14-27
*
* @attention you don't have to fully understand what this function done, cause it is basically
* for initialization.Enjoy developing without caring about the infrastructure!
* if you really want to know what is happeng, contact author.
* @attention ,,
* !,(reference manual)
*
* @param _instance can instance owned by specific module
*/
@ -42,11 +40,9 @@ static void CANAddFilter(CANInstance *_instance)
}
/**
* @brief called by CANRegister before the first module being registered
* CAN实例初始化的时候会自动调用此函数,CAN服务
* @brief CAN实例初始化的时候会自动调用此函数,CAN服务
*
* @note this func will handle all these thing automatically
* there is no need to worry about hardware initialization, we do these for you!
* @note CAN1和CAN2,CAN1和CAN2的FIFO0 & FIFO1溢出通知
*
*/
static void CANServiceInit()
@ -67,13 +63,16 @@ CANInstance *CANRegister(CAN_Init_Config_s *config)
{
CANServiceInit(); // 第一次注册,先进行硬件初始化
}
if (idx >= CAN_MX_REGISTER_CNT) // 超过最大实例数
while (1)
;
CANInstance *instance = (CANInstance *)malloc(sizeof(CANInstance)); // 分配空间
memset(instance, 0, sizeof(CANInstance));
memset(instance, 0, sizeof(CANInstance)); // 分配的空间未必是0,所以要先清空
// 进行发送报文的配置
instance->txconf.StdId = config->tx_id;
instance->txconf.IDE = CAN_ID_STD;
instance->txconf.RTR = CAN_RTR_DATA;
instance->txconf.DLC = 0x08; // 默认发送长度为8
instance->txconf.StdId = config->tx_id; // 发送id
instance->txconf.IDE = CAN_ID_STD; // 使用标准id,扩展id则使用CAN_ID_EXT(目前没有需求)
instance->txconf.RTR = CAN_RTR_DATA; // 发送数据帧
instance->txconf.DLC = 0x08; // 默认发送长度为8
// 设置回调函数和接收发送id
instance->can_handle = config->can_handle;
instance->tx_id = config->tx_id; // 好像没用,可以删掉
@ -87,10 +86,11 @@ CANInstance *CANRegister(CAN_Init_Config_s *config)
return instance; // 返回can实例指针
}
/* TODO:目前似乎封装过度,应该添加一个指向tx_buff的指针,tx_buff不应该由CAN instance保存 */
/* @todo 目前似乎封装过度,应该添加一个指向tx_buff的指针,tx_buff不应该由CAN instance保存 */
/* 如果让CANinstance保存txbuff,会增加一次复制的开销 */
void CANTransmit(CANInstance *_instance)
{
while (HAL_CAN_GetTxMailboxesFreeLevel(_instance->can_handle) == 0)
while (HAL_CAN_GetTxMailboxesFreeLevel(_instance->can_handle) == 0) // 等待邮箱空闲
;
// tx_mailbox会保存实际填入了这一帧消息的邮箱,但是知道是哪个邮箱发的似乎也没啥用
HAL_CAN_AddTxMessage(_instance->can_handle, &_instance->txconf, _instance->tx_buff, &_instance->tx_mailbox);
@ -107,22 +107,23 @@ void CANSetDLC(CANInstance *_instance, uint8_t length)
/* -----------------------belows are callback definitions--------------------------*/
/**
* @brief this func will recv data from @param:fifox to a tmp can_rx_buff
* then, all the instances will be polling to check which should recv this pack of data
* @brief ,FIFO0和FIFO1溢出中断()
* ,can_handle和rx_id相等的实例时,
*
* @param _hcan
* @param fifox passed to HAL_CAN_GetRxMessage() to get mesg from a specific fifo
*/
static void CANFIFOxCallback(CAN_HandleTypeDef *_hcan, uint32_t fifox)
{
static uint8_t can_rx_buff[8];
static CAN_RxHeaderTypeDef rxconf;
HAL_CAN_GetRxMessage(_hcan, fifox, &rxconf, can_rx_buff);
static uint8_t can_rx_buff[8]; // 用于保存接收到的数据,static是为了减少栈空间占用,避免重复分配
static CAN_RxHeaderTypeDef rxconf; // 同上
HAL_CAN_GetRxMessage(_hcan, fifox, &rxconf, can_rx_buff); // 从FIFO中获取数据
for (size_t i = 0; i < idx; ++i)
{ // 两者相等说明这是要找的实例
if (_hcan == can_instance[i]->can_handle && rxconf.StdId == can_instance[i]->rx_id)
{
if (can_instance[i]->can_module_callback != NULL)
if (can_instance[i]->can_module_callback != NULL) // 回调函数不为空就调用
{
can_instance[i]->rx_len = rxconf.DLC; // 保存接收到的数据长度
memcpy(can_instance[i]->rx_buff, can_rx_buff, rxconf.DLC); // 消息拷贝到对应实例
@ -133,8 +134,13 @@ static void CANFIFOxCallback(CAN_HandleTypeDef *_hcan, uint32_t fifox)
}
}
/* ATTENTION: two CAN devices in STM32 share two FIFOs */
/* functions below will call CANFIFOxCallback() to further process message from a specific CAN device */
/**
* @brief ,STM32的两个CAN设备共享两个FIFO
* HAL库中的回调函数,__weak,()
* FIFO0或FIFO1溢出时会调用这两个函数
*/
// 下面的函数会调用CANFIFOxCallback()来进一步处理来自特定CAN设备的消息
/**
* @brief rx fifo callback. Once FIFO_0 is full,this func would be called
*
@ -142,7 +148,7 @@ static void CANFIFOxCallback(CAN_HandleTypeDef *_hcan, uint32_t fifox)
*/
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
{
CANFIFOxCallback(hcan, CAN_RX_FIFO0);
CANFIFOxCallback(hcan, CAN_RX_FIFO0); // 调用我们自己写的函数来处理消息
}
/**
@ -152,5 +158,5 @@ void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
*/
void HAL_CAN_RxFifo1MsgPendingCallback(CAN_HandleTypeDef *hcan)
{
CANFIFOxCallback(hcan, CAN_RX_FIFO1);
CANFIFOxCallback(hcan, CAN_RX_FIFO1); // 调用我们自己写的函数来处理消息
}

View File

@ -4,10 +4,10 @@
#include <stdint.h>
#include "can.h"
#define MX_REGISTER_DEVICE_CNT 12 // maximum number of device can be registered to CAN service
// this number depends on the load of CAN bus.
#define MX_CAN_FILTER_CNT (2 * 14) // temporarily useless
#define DEVICE_CAN_CNT 2 // CAN1,CAN2
// 最多能够支持的CAN设备数
#define CAN_MX_REGISTER_CNT 16 // 这个数量取决于CAN总线的负载
#define MX_CAN_FILTER_CNT (2 * 14) // 最多可以使用的CAN过滤器数量,目前远不会用到这么多
#define DEVICE_CAN_CNT 2 // 根据板子设定,F407IG有CAN1,CAN2,因此为2;F334只有一个,则设为1
/* can instance typedef, every module registered to CAN should have this variable */
#pragma pack(1)
@ -23,20 +23,28 @@ typedef struct _
uint8_t rx_len; // 接收长度,可能为0-8
// 接收的回调函数,用于解析接收到的数据
void (*can_module_callback)(struct _ *); // callback needs an instance to tell among registered ones
void* id; // 使用can外设的
void *id; // 使用can外设的
} CANInstance;
#pragma pack()
/* this structure is used for initialization */
/* CAN实例初始化结构体,将此结构体指针传入注册函数 */
typedef struct
{
CAN_HandleTypeDef *can_handle;
uint32_t tx_id;
uint32_t rx_id;
void (*can_module_callback)(CANInstance *);
void* id;
CAN_HandleTypeDef *can_handle; // can句柄
uint32_t tx_id; // 发送id
uint32_t rx_id; // 接收id
void (*can_module_callback)(CANInstance *); // 处理接收数据的回调函数
void *id; // 拥有can实例的模块地址,用于区分不同的模块(如果有需要的话),如果不需要可以不传入
} CAN_Init_Config_s;
/**
* @brief Register a module to CAN service,remember to call this before using a CAN device
* ()can实例,.
* @param config init config
* @return CANInstance* can instance owned by module
*/
CANInstance *CANRegister(CAN_Init_Config_s *config);
/**
* @brief CAN发送报文的数据帧长度;8,,8
*
@ -53,12 +61,4 @@ void CANSetDLC(CANInstance *_instance, uint8_t length);
*/
void CANTransmit(CANInstance *_instance);
/**
* @brief Register a module to CAN service,remember to call this before using a CAN device
* ()can实例,.
* @param config init config
* @return CANInstance* can instance owned by module
*/
CANInstance *CANRegister(CAN_Init_Config_s *config);
#endif

View File

@ -34,6 +34,7 @@ typedef struct _
uint8_t rx_buff[8];
uint32_t rx_id;
void (*can_module_callback)(struct _*);
void* id;
} can_instance;
typedef struct
@ -42,6 +43,7 @@ typedef struct
uint32_t tx_id;
uint32_t rx_id;
void (*can_module_callback)(can_instance*);
void* id;
} can_instance_config;
typedef void (*can_callback)(can_instance*);

View File

@ -0,0 +1,3 @@
# bsp_dwt
DWT是stm32内部的一个"隐藏资源",他的用途是给下载器提供准确的定时,从而为调试信息加上时间戳.

View File

@ -0,0 +1,2 @@
#include "gpio.h"

View File

@ -5,37 +5,11 @@
static uint8_t idx = 0; // 配合中断以及初始化
static IICInstance *iic_instance[IIC_DEVICE_CNT] = {NULL};
/**
* @brief
*
* @param hi2c handle
*/
void HAL_I2C_MasterRxCpltCallback(I2C_HandleTypeDef *hi2c)
{
// 如果是当前i2c硬件发出的complete,且dev_address和之前发起接收的地址相同,同时回到函数不为空, 则调用回调函数
for (uint8_t i = 0; i < idx; i++)
{
if (iic_instance[i]->handle == hi2c && hi2c->Devaddress == iic_instance[i]->dev_address)
{
if (iic_instance[i]->callback != NULL)
iic_instance[i]->callback(iic_instance[i]);
return;
}
}
}
/**
* @brief ,使HAL_I2C_MasterRxCpltCallback
*
* @param hi2c handle
*/
void HAL_I2C_MemRxCpltCallback(I2C_HandleTypeDef *hi2c)
{
HAL_I2C_MasterRxCpltCallback(hi2c);
}
IICInstance *IICRegister(IIC_Init_Config_s *conf)
{
if (idx >= MX_IIC_SLAVE_CNT) // 超过最大实例数
while (1) // 酌情增加允许的实例上限,也有可能是内存泄漏
;
// 申请到的空间未必是0, 所以需要手动初始化
IICInstance *instance = (IICInstance *)malloc(sizeof(IICInstance));
instance = (IICInstance *)malloc(sizeof(IICInstance));
@ -53,9 +27,9 @@ IICInstance *IICRegister(IIC_Init_Config_s *conf)
void IICSetMode(IICInstance *iic, IIC_Work_Mode_e mode)
{ // HAL自带重入保护,不需要手动终止或等待传输完成
if (iic->work_mode != mode) // 如果不同才需要修改
if (iic->work_mode != mode)
{
iic->work_mode = mode;
iic->work_mode = mode; // 如果不同才需要修改
}
}
@ -65,13 +39,14 @@ void IICTransmit(IICInstance *iic, uint8_t *data, uint16_t size, IIC_Seq_Mode_e
while (1)
; // 未知传输模式, 程序停止
// 根据不同的工作模式进行不同的传输
switch (iic->work_mode)
{
case IIC_BLOCK_MODE:
if (seq_mode != IIC_RELEASE)
while (1)
; // 阻塞模式下不支持HOLD ON模式!!!
HAL_I2C_Master_Transmit(iic->handle, iic->dev_address, data, size, 100);
; // 阻塞模式下不支持HOLD ON模式!!!只能传输完成后立刻释放总线
HAL_I2C_Master_Transmit(iic->handle, iic->dev_address, data, size, 100); // 默认超时时间100ms
break;
case IIC_IT_MODE:
if (seq_mode == IIC_RELEASE)
@ -88,7 +63,6 @@ void IICTransmit(IICInstance *iic, uint8_t *data, uint16_t size, IIC_Seq_Mode_e
default:
while (1)
; // 未知传输模式, 程序停止
break;
}
}
@ -108,7 +82,7 @@ void IICReceive(IICInstance *iic, uint8_t *data, uint16_t size, IIC_Seq_Mode_e s
if (seq_mode != IIC_RELEASE)
while (1)
; // 阻塞模式下不支持HOLD ON模式!!!
HAL_I2C_Master_Receive(iic->handle, iic->dev_address, data, size, 100);
HAL_I2C_Master_Receive(iic->handle, iic->dev_address, data, size, 100); // 默认超时时间100ms
break;
case IIC_IT_MODE:
if (seq_mode == IIC_RELEASE)
@ -133,11 +107,11 @@ void IICAcessMem(IICInstance *iic, uint8_t mem_addr, uint8_t *data, uint16_t siz
{
if (mem_mode == IIC_WRITE_MEM)
{
HAL_I2C_Mem_Write(iic->handle, iic->dev_address, mem_addr, I2C_MEMADD_SIZE_8BIT, data, size, 1000);
HAL_I2C_Mem_Write(iic->handle, iic->dev_address, mem_addr, I2C_MEMADD_SIZE_8BIT, data, size, 100);
}
else if (mem_mode == IIC_READ_MEM)
{
HAL_I2C_Mem_Read(iic->handle, iic->dev_address, mem_addr, I2C_MEMADD_SIZE_8BIT, data, size, 1000);
HAL_I2C_Mem_Read(iic->handle, iic->dev_address, mem_addr, I2C_MEMADD_SIZE_8BIT, data, size, 100);
}
else
{
@ -145,3 +119,32 @@ void IICAcessMem(IICInstance *iic, uint8_t mem_addr, uint8_t *data, uint16_t siz
; // 未知模式, 程序停止
}
}
/**
* @brief IIC接收完成回调函数
*
* @param hi2c handle
*/
void HAL_I2C_MasterRxCpltCallback(I2C_HandleTypeDef *hi2c)
{
// 如果是当前i2c硬件发出的complete,且dev_address和之前发起接收的地址相同,同时回到函数不为空, 则调用回调函数
for (uint8_t i = 0; i < idx; i++)
{
if (iic_instance[i]->handle == hi2c && hi2c->Devaddress == iic_instance[i]->dev_address)
{
if (iic_instance[i]->callback != NULL) // 回调函数不为空
iic_instance[i]->callback(iic_instance[i]);
return;
}
}
}
/**
* @brief ,使HAL_I2C_MasterRxCpltCallback
*
* @param hi2c handle
*/
void HAL_I2C_MemRxCpltCallback(I2C_HandleTypeDef *hi2c)
{
HAL_I2C_MasterRxCpltCallback(hi2c);
}

View File

@ -0,0 +1,7 @@
# bsp iic
关于I2C的序列传输,Restart condition和总线仲裁,请看:
https://blog.csdn.net/NeoZng/article/details/128496694
https://blog.csdn.net/NeoZng/article/details/128486366

View File

@ -1,10 +1,27 @@
#ifndef _BSP_LOG_H
#define _BSP_LOG_H
/**
* @brief ,
*
*/
void BSPLogInit();
/**
* @brief segger RTT打印日志,,printf
*
* @param fmt
* @param ...
* @return int
*/
int PrintLog(const char *fmt, ...);
/**
* @brief sprintf(),float转换为字符串进行打印
*
* @param str
* @param va float
*/
void Float2Str(char *str, float va);
#endif

View File

@ -13,9 +13,8 @@ void HAL_TIM_PWM_PulseFinishedCallback(TIM_HandleTypeDef *htim)
if (pwm_instance[i]->htim == htim && htim->Channel == pwm_instance[i]->channel)
{
if (pwm_instance[i]->callback) // 如果有回调函数
{
pwm_instance[i]->callback(pwm_instance[i]);
}
return; // 一次只能有一个通道的中断,所以直接返回
}
}
@ -23,6 +22,9 @@ void HAL_TIM_PWM_PulseFinishedCallback(TIM_HandleTypeDef *htim)
PWMInstance *PWMRegister(PWM_Init_Config_s *config)
{
if (idx >= PWM_DEVICE_CNT) // 超过最大实例数,考虑增加或查看是否有内存泄漏
while (1)
;
PWMInstance *pwm = (PWMInstance *)malloc(sizeof(PWMInstance));
memset(pwm, 0, sizeof(PWMInstance));
@ -33,8 +35,8 @@ PWMInstance *PWMRegister(PWM_Init_Config_s *config)
pwm->callback = config->callback;
pwm->id = config->id;
HAL_TIM_PWM_Start(pwm->htim, pwm->channel);
__HAL_TIM_SetCompare(pwm->htim, pwm->channel, pwm->pulse);
HAL_TIM_PWM_Start(pwm->htim, pwm->channel); // 启动PWM
__HAL_TIM_SetCompare(pwm->htim, pwm->channel, pwm->pulse); // 设置占空比,初始为0
pwm_instance[idx++] = pwm;
return pwm;

View File

@ -4,7 +4,7 @@
#include "tim.h"
#include "stdint.h"
#define PWM_DEVICE_CNT 16 // PWM实例数量
#define PWM_DEVICE_CNT 16 // 最大支持的PWM实例数量
/* pwm实例结构体 */
typedef struct pwm_ins_temp
@ -15,6 +15,9 @@ typedef struct pwm_ins_temp
uint32_t pulse; // 脉宽
void (*callback)(struct pwm_ins_temp *); // DMA传输完成回调函数
void *id; // 实例ID
// 后续还要添加更多的参数,以提供更直观的封装,比如直接按照百分比设置占空比,直接设置频率等
// ...
} PWMInstance;
typedef struct
@ -49,6 +52,7 @@ void PWMStart(PWMInstance *pwm);
*/
void PWMStop(PWMInstance *pwm);
// @todo 这三个函数还需要进一步封装,协调好三者之间的关系
/**
* @brief pwm脉宽
*
@ -56,6 +60,8 @@ void PWMStop(PWMInstance *pwm);
* @param pulse
*/
void PWMSetPulse(PWMInstance *pwm, uint32_t pulse);
void PWMSetPeriod(PWMInstance *pwm, uint32_t period); // 未实现
void PWMSetPrescaler(PWMInstance *pwm, uint32_t prescaler); // 未实现
/**
* @brief pwm dma传输

View File

@ -6,44 +6,14 @@
static SPIInstance *spi_instance[SPI_DEVICE_CNT] = {NULL};
static uint8_t idx = 0; // 配合中断以及初始化
/**
* @brief SPI接收完成,,
*
* @param hspi spi handle
*/
void HAL_SPI_RxCpltCallback(SPI_HandleTypeDef *hspi)
{
for (size_t i = 0; i < idx; i++)
{
// 如果是当前spi硬件发出的complete,且cs_pin为低电平(说明正在传输),则尝试调用回调函数
if (spi_instance[i]->spi_handle == hspi &&
HAL_GPIO_ReadPin(spi_instance[i]->GPIO_cs, spi_instance[i]->cs_pin) == GPIO_PIN_RESET)
{
if (spi_instance[i]->callback) // 回调函数不为空, 则调用回调函数
{
// 先拉高片选,结束传输
HAL_GPIO_WritePin(spi_instance[i]->GPIO_cs, spi_instance[i]->cs_pin, GPIO_PIN_SET);
spi_instance[i]->callback(spi_instance[i]);
}
return;
}
}
}
/**
* @brief RxCpltCallback共用解析即可,,
*
* @param hspi spi handle
*/
void HAL_SPI_TxRxCpltCallback(SPI_HandleTypeDef *hspi)
{
HAL_SPI_RxCpltCallback(hspi);
}
SPIInstance *SPIRegister(SPI_Init_Config_s *conf)
{
if (idx >= MX_SPI_BUS_SLAVE_CNT) // 超过最大实例数
while (1)
;
SPIInstance *instance = (SPIInstance *)malloc(sizeof(SPIInstance));
memset(instance, 0, sizeof(SPIInstance));
instance->spi_handle = conf->spi_handle;
instance->GPIO_cs = conf->GPIO_cs;
instance->cs_pin = conf->cs_pin;
@ -57,7 +27,7 @@ SPIInstance *SPIRegister(SPI_Init_Config_s *conf)
void SPITransmit(SPIInstance *spi_ins, uint8_t *ptr_data, uint8_t len)
{
// 拉低片选,开始传输
// 拉低片选,开始传输(选中从机)
HAL_GPIO_WritePin(spi_ins->GPIO_cs, spi_ins->cs_pin, GPIO_PIN_RESET);
switch (spi_ins->spi_work_mode)
{
@ -68,7 +38,7 @@ void SPITransmit(SPIInstance *spi_ins, uint8_t *ptr_data, uint8_t len)
HAL_SPI_Transmit_IT(spi_ins->spi_handle, ptr_data, len);
break;
case SPI_BLOCK_MODE:
HAL_SPI_Transmit(spi_ins->spi_handle, ptr_data, len, 10);
HAL_SPI_Transmit(spi_ins->spi_handle, ptr_data, len, 50); // 默认50ms超时
// 阻塞模式不会调用回调函数,传输完成后直接拉高片选结束
HAL_GPIO_WritePin(spi_ins->GPIO_cs, spi_ins->cs_pin, GPIO_PIN_SET);
break;
@ -122,7 +92,7 @@ void SPITransRecv(SPIInstance *spi_ins, uint8_t *ptr_data_rx, uint8_t *ptr_data_
HAL_SPI_TransmitReceive_IT(spi_ins->spi_handle, ptr_data_tx, ptr_data_rx, len);
break;
case SPI_BLOCK_MODE:
HAL_SPI_TransmitReceive(spi_ins->spi_handle, ptr_data_tx, ptr_data_rx, len, 10);
HAL_SPI_TransmitReceive(spi_ins->spi_handle, ptr_data_tx, ptr_data_rx, len, 50); // 默认50ms超时
// 阻塞模式不会调用回调函数,传输完成后直接拉高片选结束
HAL_GPIO_WritePin(spi_ins->GPIO_cs, spi_ins->cs_pin, GPIO_PIN_SET);
break;
@ -144,3 +114,37 @@ void SPISetMode(SPIInstance *spi_ins, SPI_TXRX_MODE_e spi_mode)
spi_ins->spi_work_mode = spi_mode;
}
}
/**
* @brief SPI接收完成,,
*
* @param hspi spi handle
*/
void HAL_SPI_RxCpltCallback(SPI_HandleTypeDef *hspi)
{
for (size_t i = 0; i < idx; i++)
{
// 如果是当前spi硬件发出的complete,且cs_pin为低电平(说明正在传输),则尝试调用回调函数
if (spi_instance[i]->spi_handle == hspi && // 显然同一时间一条总线只能有一个从机在接收数据
HAL_GPIO_ReadPin(spi_instance[i]->GPIO_cs, spi_instance[i]->cs_pin) == GPIO_PIN_RESET)
{
if (spi_instance[i]->callback != NULL) // 回调函数不为空, 则调用回调函数
{
// 先拉高片选,结束传输
HAL_GPIO_WritePin(spi_instance[i]->GPIO_cs, spi_instance[i]->cs_pin, GPIO_PIN_SET);
spi_instance[i]->callback(spi_instance[i]);
}
return;
}
}
}
/**
* @brief RxCpltCallback共用解析即可,,
* HAL库的__weak函数的重写,使IT或DMA模式,
* @param hspi spi handle
*/
void HAL_SPI_TxRxCpltCallback(SPI_HandleTypeDef *hspi)
{
HAL_SPI_RxCpltCallback(hspi); // 直接调用接收完成的回调函数
}

View File

@ -21,15 +21,15 @@ typedef struct spi_ins_temp
GPIO_TypeDef *GPIO_cs; // 片选信号对应的GPIO,如GPIOA,GPIOB等等
uint16_t cs_pin; // 片选信号对应的引脚号,GPIO_PIN_1,GPIO_PIN_2等等
SPI_TXRX_MODE_e spi_work_mode; // 传输工作模式
uint8_t rx_size; // 本次接收的数据长度
uint8_t *rx_buffer; // 本次接收的数据缓冲区
void (*callback)(struct spi_ins_temp *); // 接收回调函数
SPI_TXRX_MODE_e spi_work_mode; // 传输工作模式
uint8_t rx_size; // 本次接收的数据长度
uint8_t *rx_buffer; // 本次接收的数据缓冲区
void *id; // 模块指针
void (*callback)(struct spi_ins_temp *); // 接收回调函数
void *id; // 模块指针
} SPIInstance;
/* rx data resolve callback*/
/* 接收回调函数定义,包含SPI的module按照此格式构建回调函数 */
typedef void (*spi_rx_callback)(SPIInstance *);
/* SPI初始化配置,其实基本和SPIIstance一模一样,为了代码风格统一因此再次定义 */

View File

@ -34,6 +34,9 @@ static void USARTServiceInit(USARTInstance *_instance)
USARTInstance *USARTRegister(USART_Init_Config_s *init_config)
{
if (idx >= DEVICE_USART_CNT) // 超过最大实例数
while (1)
;
USARTInstance *instance = (USARTInstance *)malloc(sizeof(USARTInstance));
memset(instance, 0, sizeof(USARTInstance));

View File

@ -5,15 +5,14 @@
#include "main.h"
#define DEVICE_USART_CNT 3 // C板至多分配3个串口
#define USART_RXBUFF_LIMIT 256 // if your protocol needs bigger buff, modify here
#define USART_RXBUFF_LIMIT 256 // 如果协议需要更大的buff,请修改这里
/* application callback,which resolves specific protocol,解析协议的回调函数 */
// 模块回调函数,用于解析协议
typedef void (*usart_module_callback)();
/* USARTInstance struct,each app would have one instance */
// 串口实例结构体,每个module都要包含一个实例
typedef struct
{
// 更新:弃用malloc方案,使用了固定大小的数组方便debug时查看
uint8_t recv_buff[USART_RXBUFF_LIMIT]; // 预先定义的最大buff大小,如果太小请修改USART_RXBUFF_LIMIT
uint8_t recv_buff_size; // 模块接收一包数据的大小
UART_HandleTypeDef *usart_handle; // 实例对应的usart_handle
@ -36,13 +35,10 @@ typedef struct
USARTInstance *USARTRegister(USART_Init_Config_s *init_config);
/**
* @todo buff和size,?
* ,,buffer大小以及何时发送.
* @brief ,usart实例,buff以及这一帧的长度
*
* @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 _instance
* @param send_buf buffer
* @param send_size how many bytes to send
*/
void USARTSend(USARTInstance *_instance, uint8_t *send_buf, uint16_t send_size);

View File

@ -11,112 +11,26 @@
#include "controller.h"
#include <memory.h>
// PID优化环节函数声明
static void f_Trapezoid_Intergral(PIDInstance *pid); // 梯形积分
static void f_Integral_Limit(PIDInstance *pid); // 积分限幅
static void f_Derivative_On_Measurement(PIDInstance *pid); // 微分先行(仅使用反馈值而不计参考输入的微分)
static void f_Changing_Integration_Rate(PIDInstance *pid); // 变速积分(误差小时积分作用更强)
static void f_Output_Filter(PIDInstance *pid); // 输出滤波(平滑输出)
static void f_Derivative_Filter(PIDInstance *pid); // 微分滤波(采集时,滤除高频噪声)
static void f_Output_Limit(PIDInstance *pid); // 输出限幅
static void f_PID_ErrorHandle(PIDInstance *pid); // 堵转保护
/**
* @brief PID,,
*
* @param pid PID实例
* @param config PID初始化设置
*/
void PID_Init(PIDInstance *pid, PID_Init_config_s *config)
{
memset(pid, 0, sizeof(PIDInstance));
// utilize the quality of struct that its memeory is continuous
memcpy(pid, config, sizeof(PID_Init_config_s));
// set rest of memory to 0
}
/**
* @brief PID计算
* @param[in] PID结构体
* @param[in]
* @param[in]
* @retval
*/
float PID_Calculate(PIDInstance *pid, float measure, float ref)
{
if (pid->Improve & ErrorHandle)
f_PID_ErrorHandle(pid);
pid->dt = DWT_GetDeltaT((void *)&pid->DWT_CNT);
pid->Measure = measure;
pid->Ref = ref;
pid->Err = pid->Ref - pid->Measure;
if (abs(pid->Err) > pid->DeadBand)
{
pid->Pout = pid->Kp * pid->Err;
pid->ITerm = pid->Ki * pid->Err * pid->dt;
pid->Dout = pid->Kd * (pid->Err - pid->Last_Err) / pid->dt;
// 梯形积分
if (pid->Improve & Trapezoid_Intergral)
f_Trapezoid_Intergral(pid);
// 变速积分
if (pid->Improve & ChangingIntegrationRate)
f_Changing_Integration_Rate(pid);
// 微分先行
if (pid->Improve & Derivative_On_Measurement)
f_Derivative_On_Measurement(pid);
// 微分滤波器
if (pid->Improve & DerivativeFilter)
f_Derivative_Filter(pid);
// 积分限幅
if (pid->Improve & Integral_Limit)
f_Integral_Limit(pid);
pid->Iout += pid->ITerm;
pid->Output = pid->Pout + pid->Iout + pid->Dout;
// 输出滤波
if (pid->Improve & OutputFilter)
f_Output_Filter(pid);
// 输出限幅
f_Output_Limit(pid);
}
else // 进入死区,清空积分和输出
{
pid->Output=0;
pid->ITerm=0;
}
pid->Last_Measure = pid->Measure;
pid->Last_Output = pid->Output;
pid->Last_Dout = pid->Dout;
pid->Last_Err = pid->Err;
pid->Last_ITerm = pid->ITerm;
return pid->Output;
}
/* ----------------------------下面是pid优化环节的实现---------------------------- */
// 梯形积分
static void f_Trapezoid_Intergral(PIDInstance *pid)
{
// 计算梯形的面积,(上底+下底)*高/2
pid->ITerm = pid->Ki * ((pid->Err + pid->Last_Err) / 2) * pid->dt;
}
// 变速积分(误差小时积分作用更强)
static void f_Changing_Integration_Rate(PIDInstance *pid)
{
if (pid->Err * pid->Iout > 0)
{
// 积分呈累积趋势
// Integral still increasing
if (abs(pid->Err) <= pid->CoefB)
return; // Full integral
if (abs(pid->Err) <= (pid->CoefA + pid->CoefB))
pid->ITerm *= (pid->CoefA - abs(pid->Err) + pid->CoefB) / pid->CoefA;
else
else // 最大阈值,不使用积分
pid->ITerm = 0;
}
}
@ -126,13 +40,11 @@ static void f_Integral_Limit(PIDInstance *pid)
static float temp_Output, temp_Iout;
temp_Iout = pid->Iout + pid->ITerm;
temp_Output = pid->Pout + pid->Iout + pid->Dout;
if (abs(temp_Output) > pid->MaxOut)
if (abs(temp_Output) > pid->MaxOut)
{
if (pid->Err * pid->Iout > 0)
if (pid->Err * pid->Iout > 0) // 积分却还在累积
{
// 积分呈累积趋势
// Integral still increasing
pid->ITerm = 0;
pid->ITerm = 0; // 当前积分项置零
}
}
@ -148,23 +60,27 @@ static void f_Integral_Limit(PIDInstance *pid)
}
}
// 微分先行(仅使用反馈值而不计参考输入的微分)
static void f_Derivative_On_Measurement(PIDInstance *pid)
{
pid->Dout = pid->Kd * (pid->Last_Measure - pid->Measure) / pid->dt;
}
// 微分滤波(采集微分时,滤除高频噪声)
static void f_Derivative_Filter(PIDInstance *pid)
{
pid->Dout = pid->Dout * pid->dt / (pid->Derivative_LPF_RC + pid->dt) +
pid->Last_Dout * pid->Derivative_LPF_RC / (pid->Derivative_LPF_RC + pid->dt);
}
// 输出滤波
static void f_Output_Filter(PIDInstance *pid)
{
pid->Output = pid->Output * pid->dt / (pid->Output_LPF_RC + pid->dt) +
pid->Last_Output * pid->Output_LPF_RC / (pid->Output_LPF_RC + pid->dt);
}
// 输出限幅
static void f_Output_Limit(PIDInstance *pid)
{
if (pid->Output > pid->MaxOut)
@ -177,7 +93,7 @@ static void f_Output_Limit(PIDInstance *pid)
}
}
// PID ERRORHandle Function
// 电机堵转检测
static void f_PID_ErrorHandle(PIDInstance *pid)
{
/*Motor Blocked Handle*/
@ -199,4 +115,95 @@ static void f_PID_ErrorHandle(PIDInstance *pid)
// Motor blocked over 1000times
pid->ERRORHandler.ERRORType = Motor_Blocked;
}
}
/* ---------------------------下面是PID的外部算法接口--------------------------- */
/**
* @brief PID,,
*
* @param pid PID实例
* @param config PID初始化设置
*/
void PID_Init(PIDInstance *pid, PID_Init_config_s *config)
{
// config的数据和pid的部分数据是连续且相同的的,所以可以直接用memcpy
// @todo: 不建议这样做,可扩展性差,不知道的开发者可能会误以为pid和config是同一个结构体
// 后续修改为逐个赋值
memset(pid, 0, sizeof(PIDInstance));
// utilize the quality of struct that its memeory is continuous
memcpy(pid, config, sizeof(PID_Init_config_s));
// set rest of memory to 0
}
/**
* @brief PID计算
* @param[in] PID结构体
* @param[in]
* @param[in]
* @retval
*/
float PID_Calculate(PIDInstance *pid, float measure, float ref)
{
// 堵转检测
if (pid->Improve & ErrorHandle)
f_PID_ErrorHandle(pid);
pid->dt = DWT_GetDeltaT((void *)&pid->DWT_CNT); //获取两次pid计算的时间间隔,用于积分和微分
// 保存上次的测量值和误差,计算当前error
pid->Measure = measure;
pid->Ref = ref;
pid->Err = pid->Ref - pid->Measure;
// 如果在死区外,则计算PID
if (abs(pid->Err) > pid->DeadBand)
{
// 基本的pid计算,使用位置式
pid->Pout = pid->Kp * pid->Err;
pid->ITerm = pid->Ki * pid->Err * pid->dt;
pid->Dout = pid->Kd * (pid->Err - pid->Last_Err) / pid->dt;
// 梯形积分
if (pid->Improve & Trapezoid_Intergral)
f_Trapezoid_Intergral(pid);
// 变速积分
if (pid->Improve & ChangingIntegrationRate)
f_Changing_Integration_Rate(pid);
// 微分先行
if (pid->Improve & Derivative_On_Measurement)
f_Derivative_On_Measurement(pid);
// 微分滤波器
if (pid->Improve & DerivativeFilter)
f_Derivative_Filter(pid);
// 积分限幅
if (pid->Improve & Integral_Limit)
f_Integral_Limit(pid);
pid->Iout += pid->ITerm; // 累加积分
pid->Output = pid->Pout + pid->Iout + pid->Dout; // 计算输出
// 输出滤波
if (pid->Improve & OutputFilter)
f_Output_Filter(pid);
// 输出限幅
f_Output_Limit(pid);
}
else // 进入死区, 则清空积分和输出
{
pid->Output=0;
pid->ITerm=0;
}
// 保存当前数据,用于下次计算
pid->Last_Measure = pid->Measure;
pid->Last_Output = pid->Output;
pid->Last_Dout = pid->Dout;
pid->Last_Err = pid->Err;
pid->Last_ITerm = pid->ITerm;
return pid->Output;
}

View File

@ -89,15 +89,6 @@ float float_deadband(float Value, float minValue, float maxValue)
return Value;
}
// int26死区
int16_t int16_deadline(int16_t Value, int16_t minValue, int16_t maxValue)
{
if (Value < maxValue && Value > minValue)
{
Value = 0;
}
return Value;
}
// 限幅函数
float float_constrain(float Value, float minValue, float maxValue)

View File

@ -96,8 +96,6 @@ float abs_limit(float num, float Limit);
float sign(float value);
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float float_deadband(float Value, float minValue, float maxValue);
// int26<32><36><EFBFBD><EFBFBD>
int16_t int16_deadline(int16_t Value, int16_t minValue, int16_t maxValue);
//<2F>޷<EFBFBD><DEB7><EFBFBD><EFBFBD><EFBFBD>
float float_constrain(float Value, float minValue, float maxValue);
//<2F>޷<EFBFBD><DEB7><EFBFBD><EFBFBD><EFBFBD>

View File

@ -10,9 +10,10 @@
*/
static void CANCommResetRx(CANCommInstance *ins)
{
// 当前已经收到的buffer清零
memset(ins->raw_recvbuf, 0, ins->cur_recv_len);
ins->recv_state = 0;
ins->cur_recv_len = 0;
ins->recv_state = 0; // 接收状态重置
ins->cur_recv_len = 0; // 当前已经收到的长度重置
}
/**
@ -23,33 +24,36 @@ static void CANCommResetRx(CANCommInstance *ins)
static void CANCommRxCallback(CANInstance *_instance)
{
static CANCommInstance *comm;
comm = (CANCommInstance *)_instance->id;
comm = (CANCommInstance *)_instance->id; // 注意写法,将can instance的id强制转换为CANCommInstance*类型
/* 接收状态判断 */
if (_instance->rx_buff[0] == CAN_COMM_HEADER && comm->recv_state == 0) // 尚未开始接收且新的一包里有帧头
if (_instance->rx_buff[0] == CAN_COMM_HEADER && comm->recv_state == 0) // 之前尚未开始接收且此次包里第一个位置是帧头
{
if (_instance->rx_buff[1] == comm->recv_data_len) // 接收长度等于设定接收长度
if (_instance->rx_buff[1] == comm->recv_data_len) // 如果这一包里的datalen也等于我们设定接收长度(这是因为暂时不支持动态包长)
{
comm->recv_state = 1;
comm->recv_state = 1; // 设置接收状态为1,说明已经开始接收
}
else
return; // 直接跳过即可
}
if (comm->recv_state) // 已经收到过帧头
{ // 如果已经接收到的长度加上当前一包的长度大于总buf len,说明接收错误
{
// 如果已经接收到的长度加上当前一包的长度大于总buf len,说明接收错误
if (comm->cur_recv_len + _instance->rx_len > comm->recv_buf_len)
{
CANCommResetRx(comm);
return; // 重置状态然后返回
}
// 直接拷贝到当前的接收buffer后面
// 直接把当前接收到的数据接到buffer后面
memcpy(comm->raw_recvbuf + comm->cur_recv_len, _instance->rx_buff, _instance->rx_len);
comm->cur_recv_len += _instance->rx_len;
// 当前已经收满
// 收完这一包以后刚好等于总buf len,说明已经收完了
if (comm->cur_recv_len == comm->recv_buf_len)
{ // buff里本该是tail的位置不等于CAN_COMM_TAIL
{
// 如果buff里本该是tail的位置不等于CAN_COMM_TAIL
if (comm->raw_recvbuf[comm->recv_buf_len - 1] != CAN_COMM_TAIL)
{
CANCommResetRx(comm);
@ -59,8 +63,8 @@ static void CANCommRxCallback(CANInstance *_instance)
{
if (comm->raw_recvbuf[comm->recv_buf_len - 2] ==
crc_8(comm->raw_recvbuf + 2, comm->recv_data_len))
{ // 通过校验,复制数据到unpack_data中
memcpy(comm->unpacked_recv_data, comm->raw_recvbuf + 2, comm->recv_data_len);
{ // 通过校验,复制数据到unpack_data中
memcpy(comm->unpacked_recv_data, comm->raw_recvbuf + 2, comm->recv_data_len); // 数据量大的话考虑使用DMA
comm->update_flag = 1; // 数据更新flag置为1
}
CANCommResetRx(comm);
@ -73,17 +77,18 @@ static void CANCommRxCallback(CANInstance *_instance)
CANCommInstance *CANCommInit(CANComm_Init_Config_s *comm_config)
{
CANCommInstance* ins = (CANCommInstance *)malloc(sizeof(CANCommInstance));
CANCommInstance *ins = (CANCommInstance *)malloc(sizeof(CANCommInstance));
memset(ins, 0, sizeof(CANCommInstance));
ins->recv_data_len = comm_config->recv_data_len;
ins->recv_buf_len = comm_config->recv_data_len + CAN_COMM_OFFSET_BYTES;
ins->recv_buf_len = comm_config->recv_data_len + CAN_COMM_OFFSET_BYTES; // head + datalen + crc8 + tail
ins->send_data_len = comm_config->send_data_len;
ins->send_buf_len = comm_config->send_data_len + CAN_COMM_OFFSET_BYTES;
ins->raw_sendbuf[0] = CAN_COMM_HEADER;
ins->raw_sendbuf[1] = comm_config->send_data_len;
ins->raw_sendbuf[0] = CAN_COMM_HEADER; // head,直接设置避免每次发送都要重新赋值,下面的tail同理
ins->raw_sendbuf[1] = comm_config->send_data_len; // datalen
ins->raw_sendbuf[comm_config->send_data_len + CAN_COMM_OFFSET_BYTES - 1] = CAN_COMM_TAIL;
comm_config->can_config.id = ins;
// can instance的设置
comm_config->can_config.id = ins; // CANComm的实例指针作为CANInstance的id,回调函数中会用到
comm_config->can_config.can_module_callback = CANCommRxCallback;
ins->can_ins = CANRegister(&comm_config->can_config);
return ins;
@ -93,10 +98,12 @@ void CANCommSend(CANCommInstance *instance, uint8_t *data)
{
static uint8_t crc8;
static uint8_t send_len;
// 将data copy到raw_sendbuf中,计算crc8
memcpy(instance->raw_sendbuf + 2, data, instance->send_data_len);
crc8 = crc_8(data, instance->send_data_len);
instance->raw_sendbuf[2 + instance->send_data_len] = crc8;
// CAN单次发送最大为8字节,如果超过8字节,需要分包发送
for (size_t i = 0; i < instance->send_buf_len; i += 8)
{ // 如果是最后一包,send len将会小于8,要修改CAN的txconf中的DLC位,调用bsp_can提供的接口即可
send_len = instance->send_buf_len - i >= 8 ? 8 : instance->send_buf_len - i;
@ -108,6 +115,6 @@ void CANCommSend(CANCommInstance *instance, uint8_t *data)
void *CANCommGet(CANCommInstance *instance)
{
instance->update_flag = 0;
instance->update_flag = 0; // 读取后将更新flag置为0
return instance->unpacked_recv_data;
}

View File

@ -16,9 +16,9 @@
#define MX_CAN_COMM_COUNT 4 // 注意均衡负载,一条总线上不要挂载过多的外设
#define CAN_COMM_MAX_BUFFSIZE 60 // 最大发送/接收字节数,如果不够可以增加此数值
#define CAN_COMM_HEADER 's'
#define CAN_COMM_TAIL 'e'
#define CAN_COMM_OFFSET_BYTES 4 // 's'+datalen+'e'+crc8
#define CAN_COMM_HEADER 's' // 帧头
#define CAN_COMM_TAIL 'e' // 帧尾
#define CAN_COMM_OFFSET_BYTES 4 // 's'+ datalen + 'e' + crc8
#pragma pack(1)
/* CAN comm 结构体, 拥有CAN comm的app应该包含一个CAN comm指针 */
@ -35,42 +35,44 @@ typedef struct
uint8_t raw_recvbuf[CAN_COMM_MAX_BUFFSIZE + CAN_COMM_OFFSET_BYTES]; // 额外4个bytes保存帧头帧尾和校验和
uint8_t unpacked_recv_data[CAN_COMM_MAX_BUFFSIZE]; // 解包后的数据,调用CANCommGet()后cast成对应的类型通过指针读取即可
/* 接收和更新标志位*/
uint8_t recv_state; // 接收状态,
uint8_t cur_recv_len;
uint8_t update_flag;
uint8_t recv_state; // 接收状态,
uint8_t cur_recv_len; // 当前已经接收到的数据长度(包括帧头帧尾datalen和校验和)
uint8_t update_flag; // 数据更新标志位,当接收到新数据时,会将此标志位置1,调用CANCommGet()后会将此标志位置0
} CANCommInstance;
#pragma pack()
/* CAN comm 初始化结构体 */
typedef struct
{
CAN_Init_Config_s can_config;
uint8_t send_data_len;
uint8_t recv_data_len;
CAN_Init_Config_s can_config; // CAN初始化结构体
uint8_t send_data_len; // 发送数据长度
uint8_t recv_data_len; // 接收数据长度
} CANComm_Init_Config_s;
/**
* @brief
* @brief CANComm
*
* @param config
* @param config CANComm初始化结构体
* @return CANCommInstance*
*/
CANCommInstance *CANCommInit(CANComm_Init_Config_s *comm_config);
/**
* @brief
* @brief CANComm发送数
*
* @param instance can comm实例
* @param instance cancomm实例
* @param data datalen相同
*/
void CANCommSend(CANCommInstance *instance, uint8_t *data);
/**
* @brief CAN COMM接收的数据,使void指针转换成指定类型
* @brief CANComm接收的数据,使void指针转换成指定类型
*
* @return void*
* @attention 访,union或struct,使pack(n)
* CAN COMM接收到的数据可以看作是pack(1),
* CANComm接收到的数据可以看作是pack(1),.
* 使pack(n),使访,.
* CANComm传输的数据使用pack(1)
*/
void *CANCommGet(CANCommInstance *instance);

View File

@ -20,6 +20,7 @@ DaemonInstance *DaemonRegister(Daemon_Init_Config_s *config)
return instance;
}
/* "喂狗"函数 */
void DaemonReload(DaemonInstance *instance)
{
instance->temp_count = instance->reload_count;
@ -36,11 +37,15 @@ void DaemonTask()
for (size_t i = 0; i < idx; ++i)
{
dins = daemon_instances[i];
if (dins->temp_count > 0)
if (dins->temp_count > 0) // 如果计数器还有值,说明上一次喂狗后还没有超时,则计数器减一
dins->temp_count--;
else if (dins->callback) // 如果有callback
else if (dins->callback) // 等于零说明超时了,调用回调函数(如果有的话)
{
dins->callback(dins->owner_id); // module内可以将owner_id强制类型转换成自身类型从而调用自身的offline callback
dins->callback(dins->owner_id); // module内可以将owner_id强制类型转换成自身类型从而调用特定module的offline callback
}
}
}
// (需要id的原因是什么?) 下面是copilot的回答!
// 需要id的原因是因为有些module可能有多个实例,而我们需要知道具体是哪个实例offline
// 如果只有一个实例,则可以不用id,直接调用callback即可
// 比如: 有一个module叫做"电机",它有两个实例,分别是"电机1"和"电机2",那么我们调用电机的离线处理函数时就需要知道是哪个电机offline

View File

@ -16,15 +16,14 @@ typedef struct daemon_ins
uint16_t temp_count; // 当前值,减为零说明模块离线或异常
void *owner_id; // daemon实例的地址,初始化的时候填入
} DaemonInstance;
/* daemon初始化配置 */
typedef struct
{
uint16_t reload_count; // 实际上这是app唯一需要设置的值?
offline_callback callback;
void *owner_id; // id取拥有daemon的实例的地址,如DJIMotorInstance*,cast成void*类型
uint16_t reload_count; // 实际上这是app唯一需要设置的值?
offline_callback callback; // 异常处理函数,当模块发生异常时会被调用
void *owner_id; // id取拥有daemon的实例的地址,如DJIMotorInstance*,cast成void*类型
} Daemon_Init_Config_s;
/**

View File

@ -4,3 +4,5 @@
> TODO:
> 1. 预计添加不同错误标志,将错误类型和灯的闪烁频率或颜色等对应起来,方便调试
这是legacy support,后续将使用bsp_pwm进行重构

View File

@ -13,7 +13,6 @@
#include "usart.h"
#include "seasky_protocol.h"
/* use usart1 as vision communication*/
static Vision_Recv_s recv_data;
// @todo:由于后续需要进行IMU-Cam的硬件触发采集控制,因此可能需要将发送设置为定时任务,或由IMU采集完成产生的中断唤醒的任务,
// 使得时间戳对齐. 因此,在send_data中设定其他的标志位数据,让ins_task填充姿态值.
@ -58,7 +57,8 @@ void VisionSend(Vision_Send_s *send)
static uint8_t send_buff[VISION_SEND_SIZE];
static uint16_t tx_len;
// TODO: code to set flag_register
// 将数据转化为seasky协议的数据包
get_protocol_send_data(0x02, flag_register, &send->yaw, 3, send_buff, &tx_len);
USARTSend(vision_usart_instance, send_buff, tx_len);
}

View File

@ -4,9 +4,10 @@
#include "bsp_usart.h"
#include "seasky_protocol.h"
#define VISION_RECV_SIZE 36u
#define VISION_RECV_SIZE 36u // 当前为固定值,36字节
#define VISION_SEND_SIZE 36u
#pragma pack(1)
typedef enum
{
NO_FIRE = 0,
@ -79,6 +80,7 @@ typedef struct
// uint32_t time_stamp; // @todo 用于和相机的时间戳对齐
} Vision_Send_s;
#pragma pack()
/**
* @brief

View File

@ -8,7 +8,7 @@ static char sname[MAX_EVENT_COUNT][MAX_EVENT_NAME_LEN + 1];
static void *p_ptr[MAX_EVENT_COUNT] = {NULL};
static void **s_pptr[MAX_EVENT_COUNT] = {NULL}; // 因为要修改指针,所以需要二重指针
/* ----------------------------------第三方指针传递版的实现----------------------------------- */
/* ----------------------------------第三方指针传递版的实现,deprecated----------------------------------- */
void MessageInit()
{
@ -68,11 +68,12 @@ void SubscribeEvent(char *name, void **data_ptr)
/* ----------------------------------链表-队列版的实现----------------------------------- */
/* message_center是fake node,是方便链表编写的技巧,这样不需要处理链表头的特殊情况 */
/* message_center是fake head node,是方便链表编写的技巧,这样不需要处理链表头的特殊情况 */
static Publisher_t message_center = {
.event_name = "Message_Manager",
.first_subs = NULL,
.next_event_node = NULL};
.next_event_node = NULL
};
static void CheckName(char *name)
{
@ -88,7 +89,7 @@ static void CheckLen(uint8_t len1, uint8_t len2)
if (len1 != len2)
{
while (1)
; // 相同事件的消息长度不同
; // 进入这里说明相同事件的消息长度不同
}
}
@ -111,14 +112,14 @@ Subscriber_t *SubRegister(char *name, uint8_t data_len)
{ // 给消息队列的每一个元素分配空间,queue里保存的实际上是数据执指针,这样可以兼容不同的数据长度
ret->queue[i] = malloc(sizeof(data_len));
}
//如果之前没有订阅者,特殊处理一下
//如果是第一个订阅者,特殊处理一下
if(node->first_subs==NULL)
{
node->first_subs=ret;
return ret;
}
// 遍历订阅者链表,直到到达尾部
Subscriber_t *sub = node->first_subs; // iterator
Subscriber_t *sub = node->first_subs; // 作为iterator
while (sub->next_subs_queue) // 遍历订阅了该事件的订阅者链表
{
sub = sub->next_subs_queue; // 移动到下一个订阅者,遇到空指针停下,说明到了链表尾部
@ -182,11 +183,14 @@ uint8_t SubGetMessage(Subscriber_t *sub, void *data_ptr)
uint8_t PubPushMessage(Publisher_t *pub, void *data_ptr)
{
Subscriber_t *iter = pub->first_subs; // iter作为订阅者指针,遍历订阅该事件的所有订阅者;如果为空说明遍历结束
while (iter) // 遍历订阅了当前事件的所有订阅者,依次填入最新消息
static Subscriber_t *iter;
iter = pub->first_subs; // iter作为订阅者指针,遍历订阅该事件的所有订阅者;如果为空说明遍历结束
// 遍历订阅了当前事件的所有订阅者,依次填入最新消息
while (iter)
{
if (iter->temp_size == QUEUE_SIZE) // 如果队列已满,则需要删除最老的数据(头部),再填入
{ // 队列头索引前移动,相当于抛弃前一个位置,被抛弃的位置稍后会被写入新的数据
{
// 队列头索引前移动,相当于抛弃前一个位置的数据,被抛弃的位置稍后会被写入新的数据
iter->front_idx = (iter->front_idx + 1) % QUEUE_SIZE;
iter->temp_size--; // 相当于出队,size-1
}

View File

@ -5,9 +5,9 @@
* @todo -pubsub机制
* @version 0.1
* @date 2022-11-30
*
*
* @copyright Copyright (c) 2022
*
*
*/
#ifndef PUBSUB_H
@ -15,102 +15,93 @@
#include "stdint.h"
#define MAX_EVENT_NAME_LEN 32 //最大的事件名长度,每个事件都有字符串来命名
#define MAX_EVENT_COUNT 12 //最多支持的事件数量
#define MAX_EVENT_NAME_LEN 32 // 最大的事件名长度,每个事件都有字符串来命名
#define MAX_EVENT_COUNT 12 // 最多支持的事件数量
#define QUEUE_SIZE 1
/**
* @brief ,app的"回调函数"
*
*
*/
void MessageInit();
/**
* @brief
*
*
* @param name ,MAX_EVENT_NAME_LEN
* @param data
*/
void PublisherRegister(char* name,void* data);
void PublisherRegister(char *name, void *data);
/**
* @brief ,
*
*
* @param name
* @param data ,,(&)
*/
void SubscribeEvent(char* name,void** data);
void SubscribeEvent(char *name, void **data);
#endif // !PUBSUB_H
/* 以下是队列版的pubsub,TODO */
typedef struct mqt
{
/* 用数组模拟FIFO队列 */
void* queue[QUEUE_SIZE];
void *queue[QUEUE_SIZE];
uint8_t data_len;
uint8_t front_idx;
uint8_t back_idx;
uint8_t temp_size; // 当前队列长度
/* 指向下一个订阅了相同的事件的订阅者的指针 */
struct mqt* next_subs_queue;
struct mqt *next_subs_queue; // 使得发布者可以通过链表访问所有订阅了相同事件的订阅者
} Subscriber_t;
/**
* @brief .,
*
* @brief .,访
*
*/
typedef struct ent
{
typedef struct ent
{
/* 事件名称 */
char event_name[MAX_EVENT_NAME_LEN+2];
uint8_t data_len;
char event_name[MAX_EVENT_NAME_LEN + 1]; // 1个字节用于存放字符串结束符 '\0'
uint8_t data_len; // 该事件的数据长度
/* 指向第一个订阅了该事件的订阅者,通过链表访问所有订阅者 */
Subscriber_t* first_subs;
Subscriber_t *first_subs;
/* 指向下一个Publisher的指针 */
struct ent* next_event_node;
struct ent *next_event_node;
} Publisher_t;
/**
* @brief name的事件消息
*
*
* @param name
* @param data_len ,sizeof()
* @return Subscriber_t*
*/
Subscriber_t* SubRegister(char* name,uint8_t data_len);
Subscriber_t *SubRegister(char *name, uint8_t data_len);
/**
* @brief
*
*
* @param name ()
* @return Publisher_t*
*/
Publisher_t* PubRegister(char* name,uint8_t data_len);
Publisher_t *PubRegister(char *name, uint8_t data_len);
/**
* @brief
*
*
* @param sub
* @param data_ptr ,
* @return uint8_t 0(),1
*/
uint8_t SubGetMessage(Subscriber_t* sub,void* data_ptr);
uint8_t SubGetMessage(Subscriber_t *sub, void *data_ptr);
/**
* @brief
*
*
* @param pub
* @param data_ptr
* @return uint8_t
*/
uint8_t PubPushMessage(Publisher_t* pub,void* data_ptr);
uint8_t PubPushMessage(Publisher_t *pub, void *data_ptr);

View File

@ -10,23 +10,23 @@ static DJIMotorInstance *dji_motor_instance[DJI_MOTOR_CNT] = {NULL};
* @brief DJI电机发送以四个一组的形式进行,,6(2can*3group)can_instance专门负责发送
* DJIMotorControl() 使, MotorSenderGrouping()
*
* C610(m2006)/C620(m3508):0x1ff,0x200; GM6020:0x1ff,0x2ff
* : GM6020: 0x204+id ; C610/C620: 0x200+id
* C610(m2006)/C620(m3508):0x1ff,0x200;
* GM6020:0x1ff,0x2ff
* (rx_id): GM6020: 0x204+id ; C610/C620: 0x200+id
* can1: [0]:0x1FF,[1]:0x200,[2]:0x2FF
* can2: [3]:0x1FF,[4]:0x200,[5]:0x2FF
*/
static CANInstance sender_assignment[6] =
{
[0] = {.can_handle = &hcan1, .txconf.StdId = 0x1ff, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
[1] = {.can_handle = &hcan1, .txconf.StdId = 0x200, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
[2] = {.can_handle = &hcan1, .txconf.StdId = 0x2ff, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
[3] = {.can_handle = &hcan2, .txconf.StdId = 0x1ff, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
[4] = {.can_handle = &hcan2, .txconf.StdId = 0x200, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
[5] = {.can_handle = &hcan2, .txconf.StdId = 0x2ff, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
static CANInstance sender_assignment[6] = {
[0] = {.can_handle = &hcan1, .txconf.StdId = 0x1ff, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
[1] = {.can_handle = &hcan1, .txconf.StdId = 0x200, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
[2] = {.can_handle = &hcan1, .txconf.StdId = 0x2ff, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
[3] = {.can_handle = &hcan2, .txconf.StdId = 0x1ff, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
[4] = {.can_handle = &hcan2, .txconf.StdId = 0x200, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
[5] = {.can_handle = &hcan2, .txconf.StdId = 0x2ff, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
};
/**
* @brief 6sender_assignment中的标志位,, DJIMotorControl() 使
* @brief 6sender_assignment中的标志位,,DJIMotorControl()使
* flag的初始化在 MotorSenderGrouping()
*
*/
@ -38,9 +38,7 @@ static uint8_t sender_enable_flag[6] = {0};
*/
static void IDcrash_Handler(uint8_t conflict_motor_idx, uint8_t temp_motor_idx)
{
while (1)
{
};
while (1);
}
/**
@ -108,7 +106,7 @@ static void MotorSenderGrouping(CAN_Init_Config_s *config)
break;
default: // other motors should not be registered here
break;
while(1); // 其他电机不应该在这里注册
}
}
@ -124,9 +122,10 @@ static void DecodeDJIMotor(CANInstance *_instance)
static uint8_t *rxbuff;
static DJI_Motor_Measure_s *measure;
rxbuff = _instance->rx_buff;
// 这里对can instance的id进行了强制转换,从而获得电机的instance实例地址
measure = &((DJIMotorInstance *)_instance->id)->motor_measure; // 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->angle_single_round = ECD_ANGLE_COEF_DJI * (float)measure->ecd;
@ -136,7 +135,7 @@ static void DecodeDJIMotor(CANInstance *_instance)
CURRENT_SMOOTH_COEF * (float)((int16_t)(rxbuff[4] << 8 | rxbuff[5]));
measure->temperate = rxbuff[6];
// multi rounds calc,计算的前提是两次采样间电机转过的角度小于180°
// 多圈角度计算,计算的前提是两次采样间电机转过的角度小于180°
if (measure->ecd - measure->last_ecd > 4096)
measure->total_round--;
else if (measure->ecd - measure->last_ecd < -4096)
@ -150,23 +149,23 @@ DJIMotorInstance *DJIMotorInit(Motor_Init_Config_s *config)
DJIMotorInstance *instance = (DJIMotorInstance *)malloc(sizeof(DJIMotorInstance));
memset(instance, 0, sizeof(DJIMotorInstance));
// motor basic setting
// motor basic setting 电机基本设置
instance->motor_type = config->motor_type;
instance->motor_settings = config->controller_setting_init_config;
// motor controller init
// motor controller init 电机控制器初始化
PID_Init(&instance->motor_controller.current_PID, &config->controller_param_init_config.current_PID);
PID_Init(&instance->motor_controller.speed_PID, &config->controller_param_init_config.speed_PID);
PID_Init(&instance->motor_controller.angle_PID, &config->controller_param_init_config.angle_PID);
instance->motor_controller.other_angle_feedback_ptr = config->controller_param_init_config.other_angle_feedback_ptr;
instance->motor_controller.other_speed_feedback_ptr = config->controller_param_init_config.other_speed_feedback_ptr;
// group motors, because 4 motors share the same CAN control message
// 电机分组,因为至多4个电机可以共用一帧CAN控制报文
MotorSenderGrouping(&config->can_init_config);
// register motor to CAN bus
// 注册电机到CAN总线
config->can_init_config.can_module_callback = DecodeDJIMotor; // set callback
config->can_init_config.id = instance; // set id,eq to address(it is identity)
config->can_init_config.id = instance;// set id,eq to address(it is identity)
instance->motor_can_instance = CANRegister(&config->can_init_config);
DJIMotorEnable(instance);
@ -228,7 +227,7 @@ void DJIMotorControl()
static float pid_measure, pid_ref; // 电机PID测量值和设定值
// 遍历所有电机实例,进行串级PID的计算并设置发送报文的值
for (size_t i = 0; i < idx; ++i)
{ // 减小访存开销,先保存指针引用
{ // 减小访存开销,先保存指针引用
motor = dji_motor_instance[i];
motor_setting = &motor->motor_settings;
motor_controller = &motor->motor_controller;
@ -280,7 +279,6 @@ void DJIMotorControl()
{ // 若该电机处于停止状态,直接将buff置零
memset(sender_assignment[group].tx_buff + 2 * num, 0, 16u);
}
}
// 遍历flag,检查是否要发送这一帧报文

View File

@ -22,22 +22,22 @@
#define DJI_MOTOR_CNT 12
/* 滤波系数设置为1的时候即关闭滤波 */
#define SPEED_SMOOTH_COEF 0.85f // better to be greater than 0.85
#define CURRENT_SMOOTH_COEF 0.9f // this coef *must* be greater than 0.9
#define ECD_ANGLE_COEF_DJI (360.0f/8192.0f) // ,将编码器值转化为角度制
#define SPEED_SMOOTH_COEF 0.85f // 最好大于0.85
#define CURRENT_SMOOTH_COEF 0.9f // 必须大于0.9
#define ECD_ANGLE_COEF_DJI 0.043945f // (360/8192),将编码器值转化为角度制
/* DJI电机CAN反馈信息*/
typedef struct
{
uint16_t last_ecd; // 上一次读取的编码器值
uint16_t ecd; // 0-8191,刻度总共有8192格
uint16_t last_ecd; // 上一次读取的编码器值
uint16_t ecd; // 0-8191,刻度总共有8192格
float angle_single_round; // 单圈角度
float speed_aps; // 角速度,单位为:度/秒 rounds per minute
int16_t real_current; // 实际电流
uint8_t temperate; // 温度 Celsius
int16_t real_current; // 实际电流
uint8_t temperate; // 温度 Celsius
float total_angle; // 总角度,注意方向
int32_t total_round; // 总圈数,注意方向
float total_angle; // 总角度,注意方向
int32_t total_round; // 总圈数,注意方向
} DJI_Motor_Measure_s;
/**
@ -46,25 +46,18 @@ typedef struct
*/
typedef struct
{
/* motor measurement recv from CAN feedback */
DJI_Motor_Measure_s motor_measure;
/* basic config of a motor*/
Motor_Control_Setting_s motor_settings;
DJI_Motor_Measure_s motor_measure; // 电机测量值
Motor_Control_Setting_s motor_settings; // 电机设置
Motor_Controller_s motor_controller; // 电机控制器
/* controller used in the motor (3 loops)*/
Motor_Controller_s motor_controller;
/* the CAN instance own by motor instance*/
CANInstance *motor_can_instance;
/* sender assigment*/
CANInstance *motor_can_instance; // 电机CAN实例
// 分组发送设置
uint8_t sender_group;
uint8_t message_num;
Motor_Type_e motor_type; // 电机类型
Motor_Working_Type_e stop_flag; // 启停标志
} DJIMotorInstance;
/**

View File

@ -7,7 +7,6 @@
// 参考深圳大学 Infantry_X-master
#define RE_RX_BUFFER_SIZE 200
// static USARTInstance referee_usart_instance;
static USARTInstance *referee_usart_instance;
/**************裁判系统数据******************/
@ -17,7 +16,7 @@ static uint16_t Judge_SelfClient_ID; // 发送者机器人对应的客户端ID
/**
* @brief ,
* @param
* @param ReadFromUsart:
* @retval
* @attention CRC校验,
*/

View File

@ -2,11 +2,12 @@
#include "string.h"
#include "bsp_usart.h"
#include "memory.h"
#include "stdlib.h"
#define REMOTE_CONTROL_FRAME_SIZE 18u // 遥控器接收的buffer大小
// 遥控器数据
static RC_ctrl_t rc_ctrl[2]; //[0]:当前数据,[1]:上一次的数据.用于按键判断
// 遥控器拥有的串口实例
static RC_ctrl_t rc_ctrl[2]; //[0]:当前数据TEMP,[1]:上一次的数据LAST.用于按键持续按下和切换的判断
// 遥控器拥有的串口实例,因为遥控器是单例,所以这里只有一个,就不封装了
static USARTInstance *rc_usart_instance;
/**
@ -17,8 +18,8 @@ static void RectifyRCjoystick()
{
for (uint8_t i = 0; i < 5; ++i)
{
if (*(&rc_ctrl[TEMP].rc.rocker_l_+i) > 660 || *(&rc_ctrl[TEMP].rc.rocker_l_+i) < -660)
*(&rc_ctrl[TEMP].rc.rocker_l_+i) = 0;
if (abs(*(&rc_ctrl[TEMP].rc.rocker_l_ + i)) > 660)
*(&rc_ctrl[TEMP].rc.rocker_l_ + i) = 0;
}
}
@ -28,7 +29,7 @@ static void RectifyRCjoystick()
* @param[out] rc_ctrl: remote control data struct point
* @retval none
*/
static void sbus_to_rc(volatile const uint8_t *sbus_buf)
static void sbus_to_rc(const uint8_t *sbus_buf)
{
memcpy(&rc_ctrl[1], &rc_ctrl[TEMP], sizeof(RC_ctrl_t)); // 保存上一次的数据
// 摇杆,直接解算时减去偏置
@ -68,7 +69,7 @@ static void sbus_to_rc(volatile const uint8_t *sbus_buf)
rc_ctrl[TEMP].key[KEY_PRESS_WITH_CTRL][j] = rc_ctrl[TEMP].key_temp & i;
}
// 位域的按键值解算,直接memcpy即可,注意小端低字节在前,即lsb在第一位
// 位域的按键值解算,直接memcpy即可,注意小端低字节在前,即lsb在第一位,msb在最后. 尚未测试
// *(uint16_t *)&rc_ctrl[TEMP].key_test[KEY_PRESS] = (uint16_t)(sbus_buf[14] | (sbus_buf[15] << 8));
// *(uint16_t *)&rc_ctrl[TEMP].key_test[KEY_STATE] = *(uint16_t *)&rc_ctrl[TEMP].key_test[KEY_PRESS] & ~(*(uint16_t *)&(rc_ctrl[1].key_test[KEY_PRESS]));
// if (rc_ctrl[TEMP].key_test[KEY_PRESS].ctrl)

View File

@ -17,7 +17,7 @@
#include "main.h"
#include "usart.h"
//
// 用于遥控器数据读取,遥控器数据是一个大小为2的数组
#define LAST 1
#define TEMP 0
@ -33,17 +33,18 @@
#define RC_CH_VALUE_MAX ((uint16_t)1684)
/* ----------------------- RC Switch Definition----------------------------- */
#define RC_SW_UP ((uint16_t)1)
#define RC_SW_MID ((uint16_t)3)
#define RC_SW_DOWN ((uint16_t)2)
#define RC_SW_UP ((uint16_t)1) // 开关向上时的值
#define RC_SW_MID ((uint16_t)3) // 开关中间时的值
#define RC_SW_DOWN ((uint16_t)2) // 开关向下时的值
// 三个判断开关状态的宏
#define switch_is_down(s) (s == RC_SW_DOWN)
#define switch_is_mid(s) (s == RC_SW_MID)
#define switch_is_up(s) (s == RC_SW_UP)
#define LEFT_SW 1
#define RIGHT_SW 0
#define LEFT_SW 1 // 左侧开关
#define RIGHT_SW 0 // 右侧开关
/* ----------------------- PC Key Definition-------------------------------- */
// 对应key[x][0~16],获取对应的键;例如通过key[KEY_PRESS][Key_W]获取W键是否按下
// 对应key[x][0~16],获取对应的键;例如通过key[KEY_PRESS][Key_W]获取W键是否按下,后续改为位域后删除
#define Key_W 0
#define Key_S 1
#define Key_D 2

View File

@ -10,28 +10,42 @@
#include "bsp_can.h"
typedef struct
{
uint16_t vol;
uint16_t current;
uint16_t power;
} SuperCap_Msg_s;
#pragma pack(1)
typedef struct
{
CANInstance *can_ins;
SuperCap_Msg_s cap_msg;
} SuperCapInstance;
uint16_t vol; // 电压
uint16_t current; // 电流
uint16_t power; // 功率
} SuperCap_Msg_s;
#pragma pack()
/* 超级电容实例 */
typedef struct
{
CANInstance *can_ins; // CAN实例
SuperCap_Msg_s cap_msg; // 超级电容信息
} SuperCapInstance;
/* 超级电容初始化配置 */
typedef struct
{
CAN_Init_Config_s can_config;
} SuperCap_Init_Config_s;
/**
* @brief
*
* @param supercap_config
* @return SuperCapInstance*
*/
SuperCapInstance *SuperCapInit(SuperCap_Init_Config_s *supercap_config);
/**
* @brief
*
* @param instance
* @param data
*/
void SuperCapSend(SuperCapInstance *instance, uint8_t *data);
#endif // !SUPER_CAP_Hd