zyx push 键鼠pitch限位

.idea/runConfigurations/OCD_basic_framework.xml

@@ -1,6 +1,6 @@
 <component name="ProjectRunConfigurationManager">
-  <configuration default="false" name="OCD basic_framework" type="com.jetbrains.cidr.embedded.openocd.conf.type" factoryName="com.jetbrains.cidr.embedded.openocd.conf.factory" REDIRECT_INPUT="false" ELEVATE="false" USE_EXTERNAL_CONSOLE="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="basic_framework" TARGET_NAME="basic_framework.elf" CONFIG_NAME="Debug" version="1" RUN_TARGET_PROJECT_NAME="basic_framework" RUN_TARGET_NAME="basic_framework.elf">
-    <openocd version="1" gdb-port="3333" telnet-port="4444" board-config="D:\CLion\Project\basic_framework\daplink.cfg" reset-type="INIT" download-type="UPDATED_ONLY">
+  <configuration default="false" name="OCD basic_framework" type="com.jetbrains.cidr.embedded.openocd.conf.type" factoryName="com.jetbrains.cidr.embedded.openocd.conf.factory" REDIRECT_INPUT="false" ELEVATE="false" USE_EXTERNAL_CONSOLE="false" EMULATE_TERMINAL="false" PASS_PARENT_ENVS_2="true" PROJECT_NAME="basic_framework" TARGET_NAME="basic_framework.elf" CONFIG_NAME="Debug" version="1" RUN_TARGET_PROJECT_NAME="basic_framework" RUN_TARGET_NAME="basic_framework.elf">
+    <openocd version="1" gdb-port="3333" telnet-port="4444" board-config="$PROJECT_DIR$/daplink.cfg" reset-type="INIT" download-type="UPDATED_ONLY">
       <debugger kind="GDB" isBundled="true" />
     </openocd>
     <method v="2">

CMakeLists.txt

@@ -53,7 +53,7 @@ include_directories(Inc Drivers/STM32F4xx_HAL_Driver/Inc Drivers/STM32F4xx_HAL_D
         bsp bsp/adc bsp/can bsp/dwt bsp/flash bsp/gpio bsp/iic bsp/log bsp/pwm bsp/spi bsp/usart bsp/usb
         modules modules/alarm modules/algorithm modules/BMI088 modules/can_comm modules/daemon modules/encoder modules/imu modules/ist8310
         modules/led modules/master_machine modules/message_center modules/motor modules/oled modules/referee modules/remote modules/standard_cmd
-        modules/super_cap modules/TFminiPlus modules/unicomm modules/vofa
+        modules/super_cap modules/TFminiPlus modules/unicomm modules/vofa modules/auto_aim
         modules/motor/DJImotor modules/motor/HTmotor modules/motor/LKmotor modules/motor/servo_motor modules/motor/step_motor
         application application/chassis application/cmd application/gimbal application/shoot
         Middlewares/Third_Party/SEGGER/RTT Middlewares/Third_Party/SEGGER/Config)

application/chassis/chassis.c

@@ -23,11 +23,15 @@
 #include "referee_UI.h"
 #include "arm_math.h"
 
+#include "vofa.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 P_MAX 43.0f         //功率控制 单位：W
+
 /* 底盘应用包含的模块和信息存储,底盘是单例模式,因此不需要为底盘建立单独的结构体 */
 #ifdef CHASSIS_BOARD // 如果是底盘板,使用板载IMU获取底盘转动角速度
 #include "can_comm.h"
@@ -42,8 +46,8 @@ static Subscriber_t *chassis_sub;                   // 用于订阅底盘的控
 static Chassis_Ctrl_Cmd_s chassis_cmd_recv;         // 底盘接收到的控制命令
 static Chassis_Upload_Data_s chassis_feedback_data; // 底盘回传的反馈数据
 
-static referee_info_t* referee_data; // 用于获取裁判系统的数据
-static Referee_Interactive_info_t ui_data; // UI数据，将底盘中的数据传入此结构体的对应变量中，UI会自动检测是否变化，对应显示UI
+
+
 
 static SuperCapInstance *cap;                                       // 超级电容
 static DJIMotorInstance *motor_lf, *motor_rf, *motor_lb, *motor_rb; // left right forward back
@@ -55,64 +59,66 @@ static DJIMotorInstance *motor_lf, *motor_rf, *motor_lb, *motor_rb; // left righ
 static float chassis_vx, chassis_vy;     // 将云台系的速度投影到底盘
 static float vt_lf, vt_rf, vt_lb, vt_rb; // 底盘速度解算后的临时输出,待进行限幅
 
-void ChassisInit()
-{
+void ChassisInit() {
     // 四个轮子的参数一样,改tx_id和反转标志位即可
     Motor_Init_Config_s chassis_motor_config = {
-        .can_init_config.can_handle = &hcan1,
-        .controller_param_init_config = {
-            .speed_PID = {
-                .Kp = 10, // 4.5
-                .Ki = 0,  // 0
-                .Kd = 0,  // 0
-                .IntegralLimit = 3000,
-                .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
-                .MaxOut = 12000,
+            .can_init_config.can_handle = &hcan1,
+            .controller_param_init_config = {
+                    .speed_PID = {
+                            .Kp = 4.0f, // 4.5
+                            .Ki = 3.0f,  // 1.5
+                            .Kd = 0,  // 0
+                            .IntegralLimit = 3000,
+                            .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
+                            .MaxOut = 12000,
+                    },
+                    .current_PID = {
+                            .Kp = 1, // 0.4
+                            .Ki = 0,   // 0
+                            .Kd = 0,
+                            .IntegralLimit = 3000,
+                            .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
+                            .MaxOut = 15000,
+                    },
             },
-            .current_PID = {
-                .Kp = 0.5, // 0.4
-                .Ki = 0,   // 0
-                .Kd = 0,
-                .IntegralLimit = 3000,
-                .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
-                .MaxOut = 15000,
+            .controller_setting_init_config = {
+                    .angle_feedback_source = MOTOR_FEED,
+                    .speed_feedback_source = MOTOR_FEED,
+                    .outer_loop_type = SPEED_LOOP,
+                    .close_loop_type = SPEED_LOOP, //| CURRENT_LOOP,
+                    .power_limit_flag = POWER_LIMIT_ON,
             },
-        },
-        .controller_setting_init_config = {
-            .angle_feedback_source = MOTOR_FEED,
-            .speed_feedback_source = MOTOR_FEED,
-            .outer_loop_type = SPEED_LOOP,
-            .close_loop_type = SPEED_LOOP | CURRENT_LOOP,
-        },
-        .motor_type = M3508,
+            .motor_type = M3508,
     };
     //  @todo: 当前还没有设置电机的正反转,仍然需要手动添加reference的正负号,需要电机module的支持,待修改.
     chassis_motor_config.can_init_config.tx_id = 1;
-    chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
-    motor_lf = DJIMotorInit(&chassis_motor_config);
-
-    chassis_motor_config.can_init_config.tx_id = 2;
-    chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
+    chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL;
     motor_rf = DJIMotorInit(&chassis_motor_config);
 
-    chassis_motor_config.can_init_config.tx_id = 4;
-    chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
-    motor_lb = DJIMotorInit(&chassis_motor_config);
+    chassis_motor_config.can_init_config.tx_id = 2;
+    chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL;
+    motor_lf = DJIMotorInit(&chassis_motor_config);
 
     chassis_motor_config.can_init_config.tx_id = 3;
-    chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
+    chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL;
+    motor_lb = DJIMotorInit(&chassis_motor_config);
+
+    chassis_motor_config.can_init_config.tx_id = 4;
+    chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL;
     motor_rb = DJIMotorInit(&chassis_motor_config);
 
-    referee_data = UITaskInit(&huart6,&ui_data); // 裁判系统初始化,会同时初始化UI
+
 
     SuperCap_Init_Config_s cap_conf = {
-        .can_config = {
-            .can_handle = &hcan2,
-            .tx_id = 0x302, // 超级电容默认接收id
-            .rx_id = 0x301, // 超级电容默认发送id,注意tx和rx在其他人看来是反的
-        }};
+            .can_config = {
+                    .can_handle = &hcan2,
+                    .tx_id = 0x302, // 超级电容默认接收id
+                    .rx_id = 0x301, // 超级电容默认发送id,注意tx和rx在其他人看来是反的
+            }};
     cap = SuperCapInit(&cap_conf); // 超级电容初始化
 
+   // PowerMeter_Init_Config_s
+
     // 发布订阅初始化,如果为双板,则需要can comm来传递消息
 #ifdef CHASSIS_BOARD
     Chassis_IMU_data = INS_Init(); // 底盘IMU初始化
@@ -139,33 +145,72 @@ void ChassisInit()
 #define RF_CENTER ((HALF_TRACK_WIDTH - CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE - CENTER_GIMBAL_OFFSET_Y) * DEGREE_2_RAD)
 #define LB_CENTER ((HALF_TRACK_WIDTH + CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE + CENTER_GIMBAL_OFFSET_Y) * DEGREE_2_RAD)
 #define RB_CENTER ((HALF_TRACK_WIDTH - CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE + CENTER_GIMBAL_OFFSET_Y) * DEGREE_2_RAD)
+
 /**
  * @brief 计算每个轮毂电机的输出,正运动学解算
  *        用宏进行预替换减小开销,运动解算具体过程参考教程
  */
-static void MecanumCalculate()
-{
+static void MecanumCalculate() {
     vt_lf = -chassis_vx - chassis_vy - chassis_cmd_recv.wz * LF_CENTER;
     vt_rf = -chassis_vx + chassis_vy - chassis_cmd_recv.wz * RF_CENTER;
     vt_lb = chassis_vx - chassis_vy - chassis_cmd_recv.wz * LB_CENTER;
     vt_rb = chassis_vx + chassis_vy - chassis_cmd_recv.wz * RB_CENTER;
 }
 
+//全向轮解算
+static void OmniCalculate() {
+    vt_rf = -(HALF_TRACK_WIDTH + HALF_WHEEL_BASE) * chassis_cmd_recv.wz + chassis_vx - chassis_vy;
+    vt_rb = -(HALF_TRACK_WIDTH + HALF_WHEEL_BASE) * chassis_cmd_recv.wz - chassis_vx - chassis_vy;
+    vt_lb = -(HALF_TRACK_WIDTH + HALF_WHEEL_BASE) * chassis_cmd_recv.wz - chassis_vx + chassis_vy;
+    vt_lf = -(HALF_TRACK_WIDTH + HALF_WHEEL_BASE) * chassis_cmd_recv.wz + chassis_vx + chassis_vy;
+
+    vt_rf /= (WHEEL_BASE * 1.414f);
+    vt_rb /= (WHEEL_BASE * 1.414f);
+    vt_lb /= (WHEEL_BASE * 1.414f);
+    vt_lf /= (WHEEL_BASE * 1.414f);
+}
+
+static const float motor_power_K[3] = {1.6301e-6f,5.7501e-7f,2.5863e-7f};
+///依据3508电机功率模型，预测电机输出功率
+static float EstimatePower(DJIMotorInstance* chassis_motor)
+{
+
+    float I_cmd = chassis_motor->motor_controller.current_PID.Output;
+    float w = chassis_motor->measure.speed_aps /6 ;//aps to rpm
+
+    float power = motor_power_K[0] * I_cmd * w + motor_power_K[1]*w*w  + motor_power_K[2]*I_cmd*I_cmd;
+
+    return power;
+}
+float vofa_send_data[6];
 /**
  * @brief 根据裁判系统和电容剩余容量对输出进行限制并设置电机参考值
  *
  */
 static void LimitChassisOutput()
 {
-    // 功率限制待添加
-    // referee_data->PowerHeatData.chassis_power;
-    // referee_data->PowerHeatData.chassis_power_buffer;
+    float P_cmd = motor_rf->motor_controller.motor_power_predict +
+                  motor_rb->motor_controller.motor_power_predict +
+                  motor_lb->motor_controller.motor_power_predict +
+                  motor_lf->motor_controller.motor_power_predict + 3.6f;
+
+    float K = (float)(chassis_cmd_recv.chassis_power_limit - 5) / P_cmd;
+    //vofa_send_data[2] = P_cmd;
+
+    motor_rf->motor_controller.motor_power_scale = K;
+    motor_rb->motor_controller.motor_power_scale = K;
+    motor_lf->motor_controller.motor_power_scale = K;
+    motor_lb->motor_controller.motor_power_scale = K;
+
+    {
+        DJIMotorSetRef(motor_lf, vt_lf);
+        DJIMotorSetRef(motor_rf, vt_rf);
+        DJIMotorSetRef(motor_lb, vt_lb);
+        DJIMotorSetRef(motor_rb, vt_rb);
+    }
+
+
 
-    // 完成功率限制后进行电机参考输入设定
-    DJIMotorSetRef(motor_lf, vt_lf);
-    DJIMotorSetRef(motor_rf, vt_rf);
-    DJIMotorSetRef(motor_lb, vt_lb);
-    DJIMotorSetRef(motor_rb, vt_rb);
 }
 
 /**
@@ -173,16 +218,14 @@ static void LimitChassisOutput()
  *        对于双板的情况,考虑增加来自底盘板IMU的数据
  *
  */
-static void EstimateSpeed()
-{
+static void EstimateSpeed() {
     // 根据电机速度和陀螺仪的角速度进行解算,还可以利用加速度计判断是否打滑(如果有)
     // chassis_feedback_data.vx vy wz =
     //  ...
 }
 
 /* 机器人底盘控制核心任务 */
-void ChassisTask()
-{
+void ChassisTask() {
     // 后续增加没收到消息的处理(双板的情况)
     // 获取新的控制信息
 #ifdef ONE_BOARD
@@ -192,15 +235,12 @@ void ChassisTask()
     chassis_cmd_recv = *(Chassis_Ctrl_Cmd_s *)CANCommGet(chasiss_can_comm);
 #endif // CHASSIS_BOARD
 
-    if (chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE)
-    { // 如果出现重要模块离线或遥控器设置为急停,让电机停止
+    if (chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE) { // 如果出现重要模块离线或遥控器设置为急停,让电机停止
         DJIMotorStop(motor_lf);
         DJIMotorStop(motor_rf);
         DJIMotorStop(motor_lb);
         DJIMotorStop(motor_rb);
-    }
-    else
-    { // 正常工作
+    } else { // 正常工作
         DJIMotorEnable(motor_lf);
         DJIMotorEnable(motor_rf);
         DJIMotorEnable(motor_lb);
@@ -208,19 +248,18 @@ void ChassisTask()
     }
 
     // 根据控制模式设定旋转速度
-    switch (chassis_cmd_recv.chassis_mode)
-    {
-    case CHASSIS_NO_FOLLOW: // 底盘不旋转,但维持全向机动,一般用于调整云台姿态
-        chassis_cmd_recv.wz = 0;
-        break;
-    case CHASSIS_FOLLOW_GIMBAL_YAW: // 跟随云台,不单独设置pid,以误差角度平方为速度输出
-        chassis_cmd_recv.wz = -1.5f * chassis_cmd_recv.offset_angle * abs(chassis_cmd_recv.offset_angle);
-        break;
-    case CHASSIS_ROTATE: // 自旋,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略
-        chassis_cmd_recv.wz = 4000;
-        break;
-    default:
-        break;
+    switch (chassis_cmd_recv.chassis_mode) {
+        case CHASSIS_NO_FOLLOW: // 底盘不旋转,但维持全向机动,一般用于调整云台姿态
+            chassis_cmd_recv.wz = 0;
+            break;
+        case CHASSIS_FOLLOW_GIMBAL_YAW: // 跟随云台,不单独设置pid,以误差角度平方为速度输出
+            chassis_cmd_recv.wz = 10.0f * chassis_cmd_recv.offset_angle * abs(chassis_cmd_recv.offset_angle);
+            break;
+        case CHASSIS_ROTATE: // 自旋,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略
+            chassis_cmd_recv.wz = 30000;
+            break;
+        default:
+            break;
     }
 
     // 根据云台和底盘的角度offset将控制量映射到底盘坐标系上
@@ -228,28 +267,36 @@ void ChassisTask()
     static float sin_theta, cos_theta;
     cos_theta = arm_cos_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD);
     sin_theta = arm_sin_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD);
+
     chassis_vx = chassis_cmd_recv.vx * cos_theta - chassis_cmd_recv.vy * sin_theta;
     chassis_vy = chassis_cmd_recv.vx * sin_theta + chassis_cmd_recv.vy * cos_theta;
 
     // 根据控制模式进行正运动学解算,计算底盘输出
-    MecanumCalculate();
+    //MecanumCalculate();
+    OmniCalculate();
 
     // 根据裁判系统的反馈数据和电容数据对输出限幅并设定闭环参考值
     LimitChassisOutput();
 
+//    float vofa_send_data[2];
+//    vofa_send_data[0] = motor_lb->motor_controller.speed_PID.Ref;
+//    vofa_send_data[1] = motor_lb->motor_controller.speed_PID.Measure;
+//    vofa_justfloat_output(vofa_send_data, 8, &huart1);
+
     // 根据电机的反馈速度和IMU(如果有)计算真实速度
     EstimateSpeed();
 
-    // // 获取裁判系统数据   建议将裁判系统与底盘分离，所以此处数据应使用消息中心发送
-    // // 我方颜色id小于7是红色,大于7是蓝色,注意这里发送的是对方的颜色, 0:blue , 1:red
-    // chassis_feedback_data.enemy_color = referee_data->GameRobotState.robot_id > 7 ? 1 : 0;
-    // // 当前只做了17mm热量的数据获取,后续根据robot_def中的宏切换双枪管和英雄42mm的情况
-    // chassis_feedback_data.bullet_speed = referee_data->GameRobotState.shooter_id1_17mm_speed_limit;
+    //todo: 裁判系统信息移植到消息中心发送
+    // 获取裁判系统数据   建议将裁判系统与底盘分离，所以此处数据应使用消息中心发送
+    // 发送敌方方颜色id
+    //chassis_feedback_data.enemy_color = !referee_data->referee_id.Robot_Color;
+    // 当前只做了17mm热量的数据获取,后续根据robot_def中的宏切换双枪管和英雄42mm的情况
+    //chassis_feedback_data.bullet_speed = referee_data->GameRobotState.shooter_id1_17mm_speed_limit;
     // chassis_feedback_data.rest_heat = referee_data->PowerHeatData.shooter_heat0;
 
     // 推送反馈消息
 #ifdef ONE_BOARD
-    PubPushMessage(chassis_pub, (void *)&chassis_feedback_data);
+    PubPushMessage(chassis_pub, (void *) &chassis_feedback_data);
 #endif
 #ifdef CHASSIS_BOARD
     CANCommSend(chasiss_can_comm, (void *)&chassis_feedback_data);

application/cmd/robot_cmd.c

@@ -8,9 +8,11 @@
 #include "message_center.h"
 #include "general_def.h"
 #include "dji_motor.h"
+#include "auto_aim.h"
 // bsp
 #include "bsp_dwt.h"
 #include "bsp_log.h"
+#include "referee_task.h"
 
 // 私有宏,自动将编码器转换成角度值
 #define YAW_ALIGN_ANGLE (YAW_CHASSIS_ALIGN_ECD * ECD_ANGLE_COEF_DJI) // 对齐时的角度,0-360
@@ -30,8 +32,16 @@ static Chassis_Ctrl_Cmd_s chassis_cmd_send;      // 发送给底盘应用的信
 static Chassis_Upload_Data_s chassis_fetch_data; // 从底盘应用接收的反馈信息信息,底盘功率枪口热量与底盘运动状态等
 
 static RC_ctrl_t *rc_data;              // 遥控器数据,初始化时返回
-static Vision_Recv_s *vision_recv_data; // 视觉接收数据指针,初始化时返回
-static Vision_Send_s vision_send_data;  // 视觉发送数据
+//static Vision_Recv_s *vision_recv_data; // 视觉接收数据指针,初始化时返回
+//static Vision_Send_s vision_send_data;  // 视觉发送数据
+static RecievePacket_t *vision_recv_data; // 视觉接收数据指针,初始化时返回
+static SendPacket_t vision_send_data;  // 视觉发送数据
+
+//自瞄相关信息
+static Trajectory_Type_t trajectory_cal;
+static Aim_Select_Type_t aim_select;
+static uint32_t no_find_cnt; // 未发现目标计数
+static uint8_t auto_aim_flag = 0; //辅助瞄准标志位  视野内有目标开启 目标丢失关闭
 
 static Publisher_t *gimbal_cmd_pub;            // 云台控制消息发布者
 static Subscriber_t *gimbal_feed_sub;          // 云台反馈信息订阅者
@@ -45,11 +55,14 @@ static Shoot_Upload_Data_s shoot_fetch_data; // 从发射获取的反馈信息
 
 static Robot_Status_e robot_state; // 机器人整体工作状态
 
-void RobotCMDInit()
-{
+static referee_info_t *referee_data; // 用于获取裁判系统的数据
+
+void RobotCMDInit() {
     rc_data = RemoteControlInit(&huart3);   // 修改为对应串口,注意如果是自研板dbus协议串口需选用添加了反相器的那个
     vision_recv_data = VisionInit(&huart1); // 视觉通信串口
 
+    referee_data = UITaskInit(&huart6, &ui_data); // 裁判系统初始化,会同时初始化UI
+
     gimbal_cmd_pub = PubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s));
     gimbal_feed_sub = SubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s));
     shoot_cmd_pub = PubRegister("shoot_cmd", sizeof(Shoot_Ctrl_Cmd_s));
@@ -81,8 +94,7 @@ void RobotCMDInit()
  *        单圈绝对角度的范围是0~360,说明文档中有图示
  *
  */
-static void CalcOffsetAngle()
-{
+static void CalcOffsetAngle() {
     // 别名angle提高可读性,不然太长了不好看,虽然基本不会动这个函数
     static float angle;
     angle = gimbal_fetch_data.yaw_motor_single_round_angle; // 从云台获取的当前yaw电机单圈角度
@@ -107,140 +119,226 @@ static void CalcOffsetAngle()
  * @brief 控制输入为遥控器(调试时)的模式和控制量设置
  *
  */
-static void RemoteControlSet()
-{
+static void update_ui_data() {
+    ui_data.chassis_mode = chassis_cmd_send.chassis_mode;
+    ui_data.gimbal_mode = gimbal_cmd_send.gimbal_mode;
+    ui_data.friction_mode = shoot_cmd_send.friction_mode;
+    ui_data.shoot_mode = shoot_cmd_send.shoot_mode;
+    ui_data.Chassis_Power_Data.chassis_power_mx = referee_data->GameRobotState.chassis_power_limit;
+}
+
+static void auto_aim_mode() {
+    trajectory_cal.v0 = 25; //弹速30
+    if (vision_recv_data->x == 0 && vision_recv_data->y == 0 && vision_recv_data->z == 0
+        && vision_recv_data->vx == 0 && vision_recv_data->vy == 0 && vision_recv_data->vz == 0) {
+        aim_select.suggest_fire = 0;
+        //未发现目标
+        no_find_cnt++;
+
+        if (no_find_cnt >= 2000) {
+            //gimbal_scan_flag = 1;
+            //auto_aim_flag = 0;
+        }
+        //else
+        //auto_aim_flag = 1;
+    } else {
+        //弹道解算
+        no_find_cnt = 0;
+        auto_aim_flag = 1;
+
+        auto_aim(&aim_select, &trajectory_cal, vision_recv_data);
+
+        VisionSetAim(aim_select.aim_point[0], aim_select.aim_point[1], aim_select.aim_point[2]);
+
+        float single_angle_yaw_now = gimbal_fetch_data.gimbal_imu_data.Yaw;
+        float diff_yaw = trajectory_cal.cmd_yaw * 180 / PI - single_angle_yaw_now;
+        float yaw_err = diff_yaw;
+
+        if(diff_yaw>180)
+            diff_yaw -= 360;
+        else if(diff_yaw<-180)
+            diff_yaw += 360;
+
+        gimbal_cmd_send.yaw = gimbal_fetch_data.gimbal_imu_data.YawTotalAngle + diff_yaw;
+
+        gimbal_cmd_send.pitch = trajectory_cal.cmd_pitch * 180 / PI;
+
+        if (yaw_err <= 3) //3度
+        {
+            aim_select.suggest_fire = 1;
+        }
+        else
+            aim_select.suggest_fire = 0;
+    }
+}
+
+static void RemoteControlSet() {
     // 控制底盘和云台运行模式,云台待添加,云台是否始终使用IMU数据?
-    if (switch_is_down(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[下],底盘跟随云台
+    if (switch_is_down(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[下],小陀螺
     {
         chassis_cmd_send.chassis_mode = CHASSIS_ROTATE;
         gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;
-    }
-    else if (switch_is_mid(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[中],底盘和云台分离,底盘保持不转动
+
+    } else if (switch_is_mid(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[中],,底盘跟随云台
     {
-        chassis_cmd_send.chassis_mode = CHASSIS_NO_FOLLOW;
-        gimbal_cmd_send.gimbal_mode = GIMBAL_FREE_MODE;
+
+        chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
+        gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;
     }
+//    else if (switch_is_up(rc_data[TEMP].rc.switch_right))// 右侧开关状态[上],小陀螺
+//    {
+//        chassis_cmd_send.chassis_mode = CHASSIS_ROTATE;
+//        gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;
+//    }
 
     // 云台参数,确定云台控制数据
-    if (switch_is_mid(rc_data[TEMP].rc.switch_left)) // 左侧开关状态为[中],视觉模式
+    if (switch_is_mid(rc_data[TEMP].rc.switch_left) ||
+        (vision_recv_data->x == 0 && vision_recv_data->y == 0 && vision_recv_data->z == 0
+         && vision_recv_data->vx == 0 && vision_recv_data->vy == 0 &&
+         vision_recv_data->vz == 0)) // 左侧开关状态为[中],或视觉未识别到目标,纯遥控器拨杆控制
     {
         // 待添加,视觉会发来和目标的误差,同样将其转化为total angle的增量进行控制
         // ...
+        aim_select.suggest_fire = 0;
+        gimbal_cmd_send.yaw -= 0.0025f * (float) rc_data[TEMP].rc.rocker_l_;
+        gimbal_cmd_send.pitch -= 0.001f * (float) rc_data[TEMP].rc.rocker_l1;
+
+        if (gimbal_cmd_send.pitch >= PITCH_MAX_ANGLE) gimbal_cmd_send.pitch = PITCH_MAX_ANGLE;
+        if (gimbal_cmd_send.pitch <= PITCH_MIN_ANGLE) gimbal_cmd_send.pitch = PITCH_MIN_ANGLE;
+
+
     }
-    // 左侧开关状态为[下],或视觉未识别到目标,纯遥控器拨杆控制
-    if (switch_is_down(rc_data[TEMP].rc.switch_left) || vision_recv_data->target_state == NO_TARGET)
-    { // 按照摇杆的输出大小进行角度增量,增益系数需调整
-        gimbal_cmd_send.yaw += 0.005f * (float)rc_data[TEMP].rc.rocker_l_;
-        gimbal_cmd_send.pitch += 0.001f * (float)rc_data[TEMP].rc.rocker_l1;
+    // 左侧开关状态为[下],视觉模式
+    if (switch_is_down(rc_data[TEMP].rc.switch_left)) {
+        auto_aim_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; // 1数值方向
+    // 底盘参数,目前没有加入小陀螺(调试似乎暂时没有必要),系数需要调整，遥控器输入灵敏度
+    chassis_cmd_send.vx = 8.0f * (float) rc_data[TEMP].rc.rocker_r_; // _水平方向
+    chassis_cmd_send.vy = 8.0f * (float) rc_data[TEMP].rc.rocker_r1; // 1数值方向
 
     // 发射参数
     if (switch_is_up(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[上],弹舱打开
         ;                                            // 弹舱舵机控制,待添加servo_motor模块,开启
-    else
-        ; // 弹舱舵机控制,待添加servo_motor模块,关闭
+    else; // 弹舱舵机控制,待添加servo_motor模块,关闭
+
 
     // 摩擦轮控制,拨轮向上打为负,向下为正
     if (rc_data[TEMP].rc.dial < -100) // 向上超过100,打开摩擦轮
         shoot_cmd_send.friction_mode = FRICTION_ON;
     else
         shoot_cmd_send.friction_mode = FRICTION_OFF;
+
     // 拨弹控制,遥控器固定为一种拨弹模式,可自行选择
-    if (rc_data[TEMP].rc.dial < -500)
+    if (rc_data[TEMP].rc.dial < -500)//
         shoot_cmd_send.load_mode = LOAD_BURSTFIRE;
     else
         shoot_cmd_send.load_mode = LOAD_STOP;
+
+    // 视觉控制发射
+//    if (aim_select.suggest_fire == 1) {
+//        //shoot_cmd_send.friction_mode = FRICTION_ON;
+//        //shoot_cmd_send.shoot_mode = SHOOT_ON;
+//        shoot_cmd_send.load_mode = LOAD_BURSTFIRE;
+//    } else {
+//        //shoot_cmd_send.friction_mode = FRICTION_OFF;
+//        shoot_cmd_send.load_mode = LOAD_STOP;
+//    }
+
     // 射频控制,固定每秒1发,后续可以根据左侧拨轮的值大小切换射频,
     shoot_cmd_send.shoot_rate = 8;
 }
 
+
 /**
  * @brief 输入为键鼠时模式和控制量设置
  *
  */
-static void MouseKeySet()
-{
-    chassis_cmd_send.vx = rc_data[TEMP].key[KEY_PRESS].w * 300 - rc_data[TEMP].key[KEY_PRESS].s * 300; // 系数待测
-    chassis_cmd_send.vy = rc_data[TEMP].key[KEY_PRESS].s * 300 - rc_data[TEMP].key[KEY_PRESS].d * 300;
+static void MouseKeySet() {
+    chassis_cmd_send.vy = rc_data[TEMP].key[KEY_PRESS].w * 3000 - rc_data[TEMP].key[KEY_PRESS].s * 3000; // 系数待测
+    chassis_cmd_send.vx = rc_data[TEMP].key[KEY_PRESS].a * 3000 - rc_data[TEMP].key[KEY_PRESS].d * 3000;
 
-    gimbal_cmd_send.yaw += (float)rc_data[TEMP].mouse.x / 660 * 10; // 系数待测
-    gimbal_cmd_send.pitch += (float)rc_data[TEMP].mouse.y / 660 * 10;
+    gimbal_cmd_send.yaw -= (float) rc_data[TEMP].mouse.x / 660 * 4; // 系数待测
+    gimbal_cmd_send.pitch -= (float) rc_data[TEMP].mouse.y / 660 * 6;
 
-    switch (rc_data[TEMP].key_count[KEY_PRESS][Key_Z] % 3) // Z键设置弹速
+    if (gimbal_cmd_send.pitch >= PITCH_MAX_ANGLE) gimbal_cmd_send.pitch = PITCH_MAX_ANGLE;
+    if (gimbal_cmd_send.pitch <= PITCH_MIN_ANGLE) gimbal_cmd_send.pitch = PITCH_MIN_ANGLE;
+
+
+    if (rc_data[TEMP].mouse.press_l && (!rc_data[TEMP].mouse.press_r)) // 左键发射模式
     {
-    case 0:
-        shoot_cmd_send.bullet_speed = 15;
-        break;
-    case 1:
-        shoot_cmd_send.bullet_speed = 18;
-        break;
-    default:
-        shoot_cmd_send.bullet_speed = 30;
-        break;
-    }
-    switch (rc_data[TEMP].key_count[KEY_PRESS][Key_E] % 4) // E键设置发射模式
-    {
-    case 0:
+        if (shoot_cmd_send.friction_mode == FRICTION_ON) {
+            shoot_cmd_send.shoot_mode = SHOOT_ON;
+            shoot_cmd_send.load_mode = LOAD_BURSTFIRE;
+        }
+    } else if ((!rc_data[TEMP].mouse.press_l) && (!rc_data[TEMP].mouse.press_r)) {
         shoot_cmd_send.load_mode = LOAD_STOP;
-        break;
-    case 1:
-        shoot_cmd_send.load_mode = LOAD_1_BULLET;
-        break;
-    case 2:
-        shoot_cmd_send.load_mode = LOAD_3_BULLET;
-        break;
-    default:
-        shoot_cmd_send.load_mode = LOAD_BURSTFIRE;
-        break;
+
     }
-    switch (rc_data[TEMP].key_count[KEY_PRESS][Key_R] % 2) // R键开关弹舱
+    if (rc_data[TEMP].mouse.press_r) // 右键自瞄模式
     {
-    case 0:
-        shoot_cmd_send.lid_mode = LID_OPEN;
-        break;
-    default:
-        shoot_cmd_send.lid_mode = LID_CLOSE;
-        break;
+
+        if ((vision_recv_data->x == 0 && vision_recv_data->y == 0 && vision_recv_data->z == 0
+             && vision_recv_data->vx == 0 && vision_recv_data->vy == 0 &&
+             vision_recv_data->vz == 0)) {
+            shoot_cmd_send.load_mode = LOAD_STOP;
+        } else {
+            auto_aim_mode();
+            if (aim_select.suggest_fire == 1 && rc_data[TEMP].mouse.press_l &&
+                shoot_cmd_send.friction_mode == FRICTION_ON) {
+                shoot_cmd_send.shoot_mode = SHOOT_ON;
+                shoot_cmd_send.load_mode = LOAD_BURSTFIRE;
+            } else {
+                shoot_cmd_send.load_mode = LOAD_STOP;
+            }
+        }
+    }
+
+
+    switch (rc_data[TEMP].key_count[KEY_PRESS][Key_R] % 2) // R键手动刷新UI
+    {
+        case 1:
+            MyUIInit();
+            rc_data[TEMP].key_count[KEY_PRESS][Key_R]++;
+            break;
+        default:
+            break;
     }
     switch (rc_data[TEMP].key_count[KEY_PRESS][Key_F] % 2) // F键开关摩擦轮
     {
-    case 0:
-        shoot_cmd_send.friction_mode = FRICTION_OFF;
-        break;
-    default:
-        shoot_cmd_send.friction_mode = FRICTION_ON;
-        break;
+        case 0:
+            shoot_cmd_send.friction_mode = FRICTION_OFF;
+            break;
+        default:
+            shoot_cmd_send.shoot_mode = SHOOT_ON;
+            shoot_cmd_send.friction_mode = FRICTION_ON;
+            break;
     }
-    switch (rc_data[TEMP].key_count[KEY_PRESS][Key_C] % 4) // C键设置底盘速度
+    switch (rc_data[TEMP].key_count[KEY_PRESS][Key_E] % 2) // E键开关小陀螺
     {
-    case 0:
-        chassis_cmd_send.chassis_speed_buff = 40;
-        break;
-    case 1:
-        chassis_cmd_send.chassis_speed_buff = 60;
-        break;
-    case 2:
-        chassis_cmd_send.chassis_speed_buff = 80;
-        break;
-    default:
-        chassis_cmd_send.chassis_speed_buff = 100;
-        break;
+        case 0:
+            chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
+            gimbal_cmd_send.gimbal_mode = GIMBAL_FREE_MODE;
+            break;
+        default:
+            chassis_cmd_send.chassis_mode = CHASSIS_ROTATE;
+            gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;
+            break;
     }
+
     switch (rc_data[TEMP].key[KEY_PRESS].shift) // 待添加 按shift允许超功率 消耗缓冲能量
     {
-    case 1:
+        case 1:
 
-        break;
+            break;
 
-    default:
+        default:
 
-        break;
+            break;
     }
+
+    shoot_cmd_send.shoot_rate = 8;
 }
 
 /**
@@ -250,8 +348,7 @@ static void MouseKeySet()
  * @todo   后续修改为遥控器离线则电机停止(关闭遥控器急停),通过给遥控器模块添加daemon实现
  *
  */
-static void EmergencyHandler()
-{
+static void EmergencyHandler() {
     // 拨轮的向下拨超过一半进入急停模式.注意向打时下拨轮是正
     if (rc_data[TEMP].rc.dial > 300 || robot_state == ROBOT_STOP) // 还需添加重要应用和模块离线的判断
     {
@@ -264,8 +361,7 @@ static void EmergencyHandler()
         LOGERROR("[CMD] emergency stop!");
     }
     // 遥控器右侧开关为[上],恢复正常运行
-    if (switch_is_up(rc_data[TEMP].rc.switch_right))
-    {
+    if (switch_is_up(rc_data[TEMP].rc.switch_right)) {
         robot_state = ROBOT_READY;
         shoot_cmd_send.shoot_mode = SHOOT_ON;
         LOGINFO("[CMD] reinstate, robot ready");
@@ -273,22 +369,22 @@ static void EmergencyHandler()
 }
 
 /* 机器人核心控制任务,200Hz频率运行(必须高于视觉发送频率) */
-void RobotCMDTask()
-{
+void RobotCMDTask() {
     // 从其他应用获取回传数据
 #ifdef ONE_BOARD
-    SubGetMessage(chassis_feed_sub, (void *)&chassis_fetch_data);
+    SubGetMessage(chassis_feed_sub, (void *) &chassis_fetch_data);
 #endif // ONE_BOARD
 #ifdef GIMBAL_BOARD
     chassis_fetch_data = *(Chassis_Upload_Data_s *)CANCommGet(cmd_can_comm);
 #endif // GIMBAL_BOARD
     SubGetMessage(shoot_feed_sub, &shoot_fetch_data);
     SubGetMessage(gimbal_feed_sub, &gimbal_fetch_data);
-
+    update_ui_data();
     // 根据gimbal的反馈值计算云台和底盘正方向的夹角,不需要传参,通过static私有变量完成
     CalcOffsetAngle();
     // 根据遥控器左侧开关,确定当前使用的控制模式为遥控器调试还是键鼠
-    if (switch_is_down(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[下],遥控器控制
+    if (switch_is_down(rc_data[TEMP].rc.switch_left) ||
+        switch_is_mid(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[下],[中],遥控器控制
         RemoteControlSet();
     else if (switch_is_up(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[上],键盘控制
         MouseKeySet();
@@ -296,17 +392,20 @@ void RobotCMDTask()
     EmergencyHandler(); // 处理模块离线和遥控器急停等紧急情况
 
     // 设置视觉发送数据,还需增加加速度和角速度数据
-    // VisionSetFlag(chassis_fetch_data.enemy_color,,chassis_fetch_data.bullet_speed)
+    VisionSetFlag(!referee_data->referee_id.Robot_Color);
+
+    //根据裁判系统反馈确定底盘功率上限
+    chassis_cmd_send.chassis_power_limit = referee_data->GameRobotState.chassis_power_limit;
 
     // 推送消息,双板通信,视觉通信等
     // 其他应用所需的控制数据在remotecontrolsetmode和mousekeysetmode中完成设置
 #ifdef ONE_BOARD
-    PubPushMessage(chassis_cmd_pub, (void *)&chassis_cmd_send);
+    PubPushMessage(chassis_cmd_pub, (void *) &chassis_cmd_send);
 #endif // ONE_BOARD
 #ifdef GIMBAL_BOARD
     CANCommSend(cmd_can_comm, (void *)&chassis_cmd_send);
 #endif // GIMBAL_BOARD
-    PubPushMessage(shoot_cmd_pub, (void *)&shoot_cmd_send);
-    PubPushMessage(gimbal_cmd_pub, (void *)&gimbal_cmd_send);
+    PubPushMessage(shoot_cmd_pub, (void *) &shoot_cmd_send);
+    PubPushMessage(gimbal_cmd_pub, (void *) &gimbal_cmd_send);
     VisionSend(&vision_send_data);
 }

application/gimbal/gimbal.c

@@ -4,8 +4,8 @@
 #include "ins_task.h"
 #include "message_center.h"
 #include "general_def.h"
-
 #include "bmi088.h"
+#include "vofa.h"
 
 static attitude_t *gimba_IMU_data; // 云台IMU数据
 static DJIMotorInstance *yaw_motor, *pitch_motor;
@@ -15,81 +15,110 @@ static Subscriber_t *gimbal_sub;                  // cmd控制消息订阅者
 static Gimbal_Upload_Data_s gimbal_feedback_data; // 回传给cmd的云台状态信息
 static Gimbal_Ctrl_Cmd_s gimbal_cmd_recv;         // 来自cmd的控制信息
 
-void GimbalInit()
-{
+static float gravity_current = 0;
+#define GRAVITY_PARAM 7000
+
+static void LimitPitchAngleAuto() {
+    /** 注意电机角度与陀螺仪角度方向是否相同
+     * 目前 add > 0, 向下转动
+     * 电机角度值减小
+     * 陀螺仪角度值增大
+     **/
+    float add;
+    float angle_set;
+
+    add = gimbal_cmd_recv.pitch - gimba_IMU_data->Pitch;
+
+    if (pitch_motor->measure.angle_single_round - add > PITCH_MAX_RELATIVE_ANGLE) {
+        if (add < 0.0f) {
+            add = PITCH_MAX_ANGLE - pitch_motor->measure.angle_single_round;
+        }
+    } else if (pitch_motor->measure.angle_single_round - add < PITCH_MIN_RELATIVE_ANGLE) {
+        if (add > 0.0f) {
+            add = PITCH_MIN_RELATIVE_ANGLE - pitch_motor->measure.angle_single_round;
+        }
+    }
+    angle_set = gimba_IMU_data->Pitch;
+    DJIMotorSetRef(pitch_motor, angle_set + add);
+}
+
+void GimbalInit() {
     gimba_IMU_data = INS_Init(); // IMU先初始化,获取姿态数据指针赋给yaw电机的其他数据来源
     // YAW
     Motor_Init_Config_s yaw_config = {
-        .can_init_config = {
-            .can_handle = &hcan1,
-            .tx_id = 1,
-        },
-        .controller_param_init_config = {
-            .angle_PID = {
-                .Kp = 8, // 8
-                .Ki = 0,
-                .Kd = 0,
-                .DeadBand = 0.1,
-                .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
-                .IntegralLimit = 100,
-
-                .MaxOut = 500,
+            .can_init_config = {
+                    .can_handle = &hcan1,
+                    .tx_id = 1,
             },
-            .speed_PID = {
-                .Kp = 50,  // 50
-                .Ki = 200, // 200
-                .Kd = 0,
-                .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
-                .IntegralLimit = 3000,
-                .MaxOut = 20000,
+            .controller_param_init_config = {
+                    .angle_PID = {
+                            .Kp = 0.5, // 8
+                            .Ki = 0,
+                            .Kd = 0.02,
+                            .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
+                            .IntegralLimit = 100,
+                            .MaxOut = 500,
+                    },
+                    .speed_PID = {
+                            .Kp = 5000,  // 50
+                            .Ki = 1000, // 200
+                            .Kd = 0,
+                            .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
+                            .IntegralLimit = 10000,
+                            .MaxOut = 20000,
+                    },
+                    .other_angle_feedback_ptr = &gimba_IMU_data->YawTotalAngle,
+                    // 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明
+                    .other_speed_feedback_ptr = &gimba_IMU_data->Gyro[2],
             },
-            .other_angle_feedback_ptr = &gimba_IMU_data->YawTotalAngle,
-            // 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明
-            .other_speed_feedback_ptr = &gimba_IMU_data->Gyro[2],
-        },
-        .controller_setting_init_config = {
-            .angle_feedback_source = OTHER_FEED,
-            .speed_feedback_source = OTHER_FEED,
-            .outer_loop_type = ANGLE_LOOP,
-            .close_loop_type = ANGLE_LOOP | SPEED_LOOP,
-            .motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
-        },
-        .motor_type = GM6020};
+            .controller_setting_init_config = {
+                    .angle_feedback_source = OTHER_FEED,
+                    .speed_feedback_source = OTHER_FEED,
+                    .outer_loop_type = ANGLE_LOOP,
+                    .close_loop_type = SPEED_LOOP | ANGLE_LOOP,
+                    .motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
+            },
+            .motor_type = GM6020,
+            .motor_control_type = CURRENT_CONTROL
+    };
     // PITCH
     Motor_Init_Config_s pitch_config = {
-        .can_init_config = {
-            .can_handle = &hcan2,
-            .tx_id = 2,
-        },
-        .controller_param_init_config = {
-            .angle_PID = {
-                .Kp = 10, // 10
-                .Ki = 0,
-                .Kd = 0,
-                .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
-                .IntegralLimit = 100,
-                .MaxOut = 500,
+            .can_init_config = {
+                    .can_handle = &hcan2,
+                    .tx_id = 1,
             },
-            .speed_PID = {
-                .Kp = 50,  // 50
-                .Ki = 350, // 350
-                .Kd = 0,   // 0
-                .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
-                .IntegralLimit = 2500,
-                .MaxOut = 20000,
+            .controller_param_init_config = {
+                    .angle_PID = {
+                            .Kp = 1.5f,// 3, // 10
+                            .Ki = 0, //0,
+                            .Kd = 0.1f,// 0.05,
+                            .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
+                            .IntegralLimit = 100,
+                            .MaxOut = 500,
+                    },
+                    .speed_PID = {
+                            .Kp = 0,//800,
+                            .Ki = 0, // 350
+                            .Kd = 0,   // 0
+                            .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
+                            .IntegralLimit = 2500,
+                            .MaxOut = 20000,
+                    },
+                    .other_angle_feedback_ptr = &gimba_IMU_data->Pitch,
+                    // 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明
+                    .other_speed_feedback_ptr = &gimba_IMU_data->Gyro[0],
+                    .current_feedforward_ptr = &gravity_current,
             },
-            .other_angle_feedback_ptr = &gimba_IMU_data->Pitch,
-            // 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明
-            .other_speed_feedback_ptr = (&gimba_IMU_data->Gyro[0]),
-        },
-        .controller_setting_init_config = {
-            .angle_feedback_source = OTHER_FEED,
-            .speed_feedback_source = OTHER_FEED,
-            .outer_loop_type = ANGLE_LOOP,
-            .close_loop_type = SPEED_LOOP | ANGLE_LOOP,
-            .motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
-        },
-        .motor_type = GM6020,
+            .controller_setting_init_config = {
+                    .angle_feedback_source = OTHER_FEED,
+                    .speed_feedback_source = OTHER_FEED,
+                    .outer_loop_type = ANGLE_LOOP,
+                    .close_loop_type = SPEED_LOOP | ANGLE_LOOP,
+                    .motor_reverse_flag = MOTOR_DIRECTION_REVERSE,
+                    .feedforward_flag = CURRENT_FEEDFORWARD,
+            },
+            .motor_type = GM6020,
+            .motor_control_type = CURRENT_CONTROL
     };
     // 电机对total_angle闭环,上电时为零,会保持静止,收到遥控器数据再动
     yaw_motor = DJIMotorInit(&yaw_config);
@@ -100,55 +129,71 @@ void GimbalInit()
 }
 
 /* 机器人云台控制核心任务,后续考虑只保留IMU控制,不再需要电机的反馈 */
-void GimbalTask()
-{
+void GimbalTask() {
     // 获取云台控制数据
     // 后续增加未收到数据的处理
     SubGetMessage(gimbal_sub, &gimbal_cmd_recv);
 
     // @todo:现在已不再需要电机反馈,实际上可以始终使用IMU的姿态数据来作为云台的反馈,yaw电机的offset只是用来跟随底盘
     // 根据控制模式进行电机反馈切换和过渡,视觉模式在robot_cmd模块就已经设置好,gimbal只看yaw_ref和pitch_ref
-    switch (gimbal_cmd_recv.gimbal_mode)
-    {
-    // 停止
-    case GIMBAL_ZERO_FORCE:
-        DJIMotorStop(yaw_motor);
-        DJIMotorStop(pitch_motor);
-        break;
-    // 使用陀螺仪的反馈,底盘根据yaw电机的offset跟随云台或视觉模式采用
-    case GIMBAL_GYRO_MODE: // 后续只保留此模式
-        DJIMotorEnable(yaw_motor);
-        DJIMotorEnable(pitch_motor);
-        DJIMotorChangeFeed(yaw_motor, ANGLE_LOOP, OTHER_FEED);
-        DJIMotorChangeFeed(yaw_motor, SPEED_LOOP, OTHER_FEED);
-        DJIMotorChangeFeed(pitch_motor, ANGLE_LOOP, OTHER_FEED);
-        DJIMotorChangeFeed(pitch_motor, SPEED_LOOP, OTHER_FEED);
-        DJIMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw); // yaw和pitch会在robot_cmd中处理好多圈和单圈
-        DJIMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch);
-        break;
-    // 云台自由模式,使用编码器反馈,底盘和云台分离,仅云台旋转,一般用于调整云台姿态(英雄吊射等)/能量机关
-    case GIMBAL_FREE_MODE: // 后续删除,或加入云台追地盘的跟随模式(响应速度更快)
-        DJIMotorEnable(yaw_motor);
-        DJIMotorEnable(pitch_motor);
-        DJIMotorChangeFeed(yaw_motor, ANGLE_LOOP, OTHER_FEED);
-        DJIMotorChangeFeed(yaw_motor, SPEED_LOOP, OTHER_FEED);
-        DJIMotorChangeFeed(pitch_motor, ANGLE_LOOP, OTHER_FEED);
-        DJIMotorChangeFeed(pitch_motor, SPEED_LOOP, OTHER_FEED);
-        DJIMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw); // yaw和pitch会在robot_cmd中处理好多圈和单圈
-        DJIMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch);
-        break;
-    default:
-        break;
+    switch (gimbal_cmd_recv.gimbal_mode) {
+        // 停止
+        case GIMBAL_ZERO_FORCE:
+            DJIMotorStop(yaw_motor);
+            DJIMotorStop(pitch_motor);
+            break;
+            // 使用陀螺仪的反馈,底盘根据yaw电机的offset跟随云台或视觉模式采用
+        case GIMBAL_GYRO_MODE: // 后续只保留此模式
+            DJIMotorEnable(yaw_motor);
+            DJIMotorEnable(pitch_motor);
+            DJIMotorChangeFeed(yaw_motor, ANGLE_LOOP, OTHER_FEED);
+            DJIMotorChangeFeed(yaw_motor, SPEED_LOOP, OTHER_FEED);
+            DJIMotorChangeFeed(pitch_motor, ANGLE_LOOP, OTHER_FEED);
+            DJIMotorChangeFeed(pitch_motor, SPEED_LOOP, OTHER_FEED);
+            DJIMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw); // yaw和pitch会在robot_cmd中处理好多圈和单圈
+            DJIMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch);
+            //LimitPitchAngleAuto();
+            break;
+            // 云台自由模式,使用编码器反馈,底盘和云台分离,仅云台旋转,一般用于调整云台姿态(英雄吊射等)/能量机关
+        case GIMBAL_FREE_MODE: // 后续删除,或加入云台追地盘的跟随模式(响应速度更快)
+            DJIMotorEnable(yaw_motor);
+            DJIMotorEnable(pitch_motor);
+            DJIMotorChangeFeed(yaw_motor, ANGLE_LOOP, OTHER_FEED);
+            DJIMotorChangeFeed(yaw_motor, SPEED_LOOP, OTHER_FEED);
+            DJIMotorChangeFeed(pitch_motor, ANGLE_LOOP, OTHER_FEED);
+            DJIMotorChangeFeed(pitch_motor, SPEED_LOOP, OTHER_FEED);
+            DJIMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw); // yaw和pitch会在robot_cmd中处理好多圈和单圈
+            DJIMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch);
+            break;
+        default:
+            break;
     }
 
     // 在合适的地方添加pitch重力补偿前馈力矩
     // 根据IMU姿态/pitch电机角度反馈计算出当前配重下的重力矩
     // ...
+    float theta = gimba_IMU_data->Pitch / 180 * PI;
+    gravity_current = (GRAVITY_PARAM) * arm_cos_f32(theta);
+
+//    float vofa_send_data[4];
+//    vofa_send_data[0] = pitch_motor->motor_controller.speed_PID.Ref;
+//    vofa_send_data[1] = pitch_motor->motor_controller.speed_PID.Measure;
+//    vofa_send_data[2] = pitch_motor->motor_controller.angle_PID.Ref;
+//    vofa_send_data[3] = pitch_motor->motor_controller.angle_PID.Measure;
+//    vofa_justfloat_output(vofa_send_data, 16, &huart1);
+
+//    float vofa_send_data[4];
+//    vofa_send_data[0] = yaw_motor->motor_controller.speed_PID.Ref;
+//    vofa_send_data[1] = yaw_motor->motor_controller.speed_PID.Measure;
+//    vofa_send_data[2] = yaw_motor->motor_controller.angle_PID.Ref;
+//    vofa_send_data[3] = yaw_motor->motor_controller.angle_PID.Measure;
+//    vofa_justfloat_output(vofa_send_data, 16, &huart1);
+
 
     // 设置反馈数据,主要是imu和yaw的ecd
     gimbal_feedback_data.gimbal_imu_data = *gimba_IMU_data;
     gimbal_feedback_data.yaw_motor_single_round_angle = yaw_motor->measure.angle_single_round;
 
     // 推送消息
-    PubPushMessage(gimbal_pub, (void *)&gimbal_feedback_data);
+    PubPushMessage(gimbal_pub, (void *) &gimbal_feedback_data);
 }

application/robot_def.h

@@ -26,21 +26,26 @@
 
 /* 机器人重要参数定义,注意根据不同机器人进行修改,浮点数需要以.0或f结尾,无符号以u结尾 */
 // 云台参数
-#define YAW_CHASSIS_ALIGN_ECD 2711  // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改
+#define YAW_CHASSIS_ALIGN_ECD 7856  // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改
 #define YAW_ECD_GREATER_THAN_4096 0 // ALIGN_ECD值是否大于4096,是为1,否为0;用于计算云台偏转角度
-#define PITCH_HORIZON_ECD 3412      // 云台处于水平位置时编码器值,若对云台有机械改动需要修改
-#define PITCH_MAX_ANGLE 0           // 云台竖直方向最大角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
-#define PITCH_MIN_ANGLE 0           // 云台竖直方向最小角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
+#define PITCH_HORIZON_ECD 1670      // 云台处于水平位置时编码器值,若对云台有机械改动需要修改
+#define PITCH_MAX_ANGLE 25           // 云台竖直方向最大角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
+#define PITCH_MIN_ANGLE -20          // 云台竖直方向最小角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
+
+#define PITCH_MAX_RELATIVE_ANGLE 123        // 云台相对底盘最大角度
+#define PITCH_MIN_RELATIVE_ANGLE 80          // 云台相对底盘最小角度
+
+
 // 发射参数
-#define ONE_BULLET_DELTA_ANGLE 36    // 发射一发弹丸拨盘转动的距离,由机械设计图纸给出
+#define ONE_BULLET_DELTA_ANGLE 45    // 发射一发弹丸拨盘转动的距离,由机械设计图纸给出
 #define REDUCTION_RATIO_LOADER 49.0f // 拨盘电机的减速比,英雄需要修改为3508的19.0f
-#define NUM_PER_CIRCLE 10            // 拨盘一圈的装载量
-// 机器人底盘修改的参数,单位为mm(毫米)
-#define WHEEL_BASE 350              // 纵向轴距(前进后退方向)
-#define TRACK_WIDTH 300             // 横向轮距(左右平移方向)
+#define NUM_PER_CIRCLE 8            // 拨盘一圈的装载量
+// 机器人底盘修改的参数,单位为m(米)
+#define WHEEL_BASE 0.1f              // 纵向轴距(前进后退方向)
+#define TRACK_WIDTH 0.1f              // 横向轮距(左右平移方向)
 #define CENTER_GIMBAL_OFFSET_X 0    // 云台旋转中心距底盘几何中心的距离,前后方向,云台位于正中心时默认设为0
 #define CENTER_GIMBAL_OFFSET_Y 0    // 云台旋转中心距底盘几何中心的距离,左右方向,云台位于正中心时默认设为0
-#define RADIUS_WHEEL 60             // 轮子半径
+#define RADIUS_WHEEL 0.08f            // 轮子半径
 #define REDUCTION_RATIO_WHEEL 19.0f // 电机减速比,因为编码器量测的是转子的速度而不是输出轴的速度故需进行转换
 
 #define GYRO2GIMBAL_DIR_YAW 1   // 陀螺仪数据相较于云台的yaw的方向,1为相同,-1为相反
@@ -126,7 +131,9 @@ typedef enum
 // 功率限制,从裁判系统获取,是否有必要保留?
 typedef struct
 { // 功率控制
-    float chassis_power_mx;
+    uint16_t chassis_power_mx;
+    uint16_t last_power_mx;
+
 } Chassis_Power_Data_s;
 
 /* ----------------CMD应用发布的控制数据,应当由gimbal/chassis/shoot订阅---------------- */
@@ -144,6 +151,8 @@ typedef struct
     float offset_angle; // 底盘和归中位置的夹角
     chassis_mode_e chassis_mode;
     int chassis_speed_buff;
+
+    uint16_t chassis_power_limit;
     // UI部分
     //  ...
 
@@ -189,21 +198,22 @@ typedef struct
 
     uint8_t rest_heat;           // 剩余枪口热量
     Bullet_Speed_e bullet_speed; // 弹速限制
-    Enemy_Color_e enemy_color;   // 0 for blue, 1 for red
-
+    Enemy_Color_e enemy_color;   // 0 for red, 1 for blue
+    Chassis_Power_Data_s chassis_power_mx;
 } Chassis_Upload_Data_s;
 
 
 typedef struct
 {
     attitude_t gimbal_imu_data;
-    uint16_t yaw_motor_single_round_angle;
+    float yaw_motor_single_round_angle;
 } Gimbal_Upload_Data_s;
 
 typedef struct
 {
     // code to go here
     // ...
+    uint8_t stalled_flag;  //堵转标志
 } Shoot_Upload_Data_s;
 
 #pragma pack() // 开启字节对齐,结束前面的#pragma pack(1)

application/shoot/shoot.c

@@ -16,98 +16,123 @@ static Subscriber_t *shoot_sub;
 static Shoot_Upload_Data_s shoot_feedback_data; // 来自cmd的发射控制信息
 
 // dwt定时,计算冷却用
-static float hibernate_time = 0, dead_time = 0;
+static float hibernate_time = 0,last_hibernate_time = 0, dead_time = 0;
 
 void ShootInit()
 {
     // 左摩擦轮
     Motor_Init_Config_s friction_config = {
-        .can_init_config = {
-            .can_handle = &hcan2,
-        },
-        .controller_param_init_config = {
-            .speed_PID = {
-                .Kp = 0, // 20
-                .Ki = 0, // 1
-                .Kd = 0,
-                .Improve = PID_Integral_Limit,
-                .IntegralLimit = 10000,
-                .MaxOut = 15000,
+            .can_init_config = {
+                    .can_handle = &hcan2,
             },
-            .current_PID = {
-                .Kp = 0, // 0.7
-                .Ki = 0, // 0.1
-                .Kd = 0,
-                .Improve = PID_Integral_Limit,
-                .IntegralLimit = 10000,
-                .MaxOut = 15000,
+            .controller_param_init_config = {
+                    .speed_PID = {
+                            .Kp = 1.5f, // 20
+                            .Ki = 0.2f, // 1
+                            .Kd = 0,
+                            .Improve = PID_Integral_Limit,
+                            .IntegralLimit = 10000,
+                            .MaxOut = 15000,
+                    },
+                    .current_PID = {
+                            .Kp = 0, // 0.7
+                            .Ki = 0, // 0.1
+                            .Kd = 0,
+                            .Improve = PID_Integral_Limit,
+                            .IntegralLimit = 10000,
+                            .MaxOut = 15000,
+                    },
             },
-        },
-        .controller_setting_init_config = {
-            .angle_feedback_source = MOTOR_FEED,
-            .speed_feedback_source = MOTOR_FEED,
+            .controller_setting_init_config = {
+                    .angle_feedback_source = MOTOR_FEED,
+                    .speed_feedback_source = MOTOR_FEED,
 
-            .outer_loop_type = SPEED_LOOP,
-            .close_loop_type = SPEED_LOOP | CURRENT_LOOP,
-            .motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
-        },
-        .motor_type = M3508};
-    friction_config.can_init_config.tx_id = 1,
+                    .outer_loop_type = SPEED_LOOP,
+                    .close_loop_type = SPEED_LOOP,
+                    .motor_reverse_flag = MOTOR_DIRECTION_REVERSE,
+            },
+            .motor_type = M3508};
+    friction_config.can_init_config.tx_id = 3;
     friction_l = DJIMotorInit(&friction_config);
 
     friction_config.can_init_config.tx_id = 2; // 右摩擦轮,改txid和方向就行
-    friction_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
+    friction_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL;
     friction_r = DJIMotorInit(&friction_config);
 
     // 拨盘电机
     Motor_Init_Config_s loader_config = {
-        .can_init_config = {
-            .can_handle = &hcan2,
-            .tx_id = 3,
-        },
-        .controller_param_init_config = {
-            .angle_PID = {
-                // 如果启用位置环来控制发弹,需要较大的I值保证输出力矩的线性度否则出现接近拨出的力矩大幅下降
-                .Kp = 0, // 10
-                .Ki = 0,
-                .Kd = 0,
-                .MaxOut = 200,
+            .can_init_config = {
+                    .can_handle = &hcan2,
+                    .tx_id = 1
             },
-            .speed_PID = {
-                .Kp = 0, // 10
-                .Ki = 0, // 1
-                .Kd = 0,
-                .Improve = PID_Integral_Limit,
-                .IntegralLimit = 5000,
-                .MaxOut = 5000,
+            .controller_param_init_config = {
+                    .angle_PID = {
+                            // 如果启用位置环来控制发弹,需要较大的I值保证输出力矩的线性度否则出现接近拨出的力矩大幅下降
+                            .Kp = 0, // 10
+                            .Ki = 0,
+                            .Kd = 0,
+                            .MaxOut = 200,
+                    },
+                    .speed_PID = {
+                            .Kp = 3, // 10
+                            .Ki = 0, // 1
+                            .Kd = 0,
+                            .Improve = PID_Integral_Limit,
+                            .IntegralLimit = 5000,
+                            .MaxOut = 10000,
+                    },
+                    .current_PID = {
+                            .Kp = 0, // 0.7
+                            .Ki = 0, // 0.1
+                            .Kd = 0,
+                            .Improve = PID_Integral_Limit,
+                            .IntegralLimit = 5000,
+                            .MaxOut = 5000,
+                    },
             },
-            .current_PID = {
-                .Kp = 0, // 0.7
-                .Ki = 0, // 0.1
-                .Kd = 0,
-                .Improve = PID_Integral_Limit,
-                .IntegralLimit = 5000,
-                .MaxOut = 5000,
+            .controller_setting_init_config = {
+                    .angle_feedback_source = MOTOR_FEED, .speed_feedback_source = MOTOR_FEED,
+                    .outer_loop_type = SPEED_LOOP, // 初始化成SPEED_LOOP,让拨盘停在原地,防止拨盘上电时乱转
+                    .close_loop_type = CURRENT_LOOP | SPEED_LOOP,
+                    .motor_reverse_flag = MOTOR_DIRECTION_NORMAL, // 注意方向设置为拨盘的拨出的击发方向
             },
-        },
-        .controller_setting_init_config = {
-            .angle_feedback_source = MOTOR_FEED, .speed_feedback_source = MOTOR_FEED,
-            .outer_loop_type = SPEED_LOOP, // 初始化成SPEED_LOOP,让拨盘停在原地,防止拨盘上电时乱转
-            .close_loop_type = CURRENT_LOOP | SPEED_LOOP,
-            .motor_reverse_flag = MOTOR_DIRECTION_NORMAL, // 注意方向设置为拨盘的拨出的击发方向
-        },
-        .motor_type = M2006 // 英雄使用m3508
+            .motor_type = M2006 // 英雄使用m3508
     };
     loader = DJIMotorInit(&loader_config);
 
     shoot_pub = PubRegister("shoot_feed", sizeof(Shoot_Upload_Data_s));
     shoot_sub = SubRegister("shoot_cmd", sizeof(Shoot_Ctrl_Cmd_s));
 }
+//卡弹检测
+void stalled_detect()
+{
+    static float last_detect_time = 0,detect_time = 0;
+    static float last_total_angle = 0;
+    static uint8_t stalled_cnt = 0;
+
+    last_detect_time = detect_time;
+    detect_time = DWT_GetTimeline_ms();
+
+    if(detect_time - last_detect_time < 200)  // 200ms 检测一次
+        return;
+
+    if(shoot_cmd_recv.load_mode == LOAD_BURSTFIRE)
+    {
+        float reference_angle = (detect_time - last_detect_time) * loader->motor_controller.pid_ref * 1e3f;
+        float real_angle = loader->measure.total_angle - last_total_angle;
+        if(real_angle < reference_angle * 0.2f)
+        {
+            //stalled_cnt ++;
+            shoot_feedback_data.stalled_flag = 1;
+        }
+    }
+}
+
 
 /* 机器人发射机构控制核心任务 */
 void ShootTask()
 {
-    // 从cmd获取控制数据
+     //从cmd获取控制数据
     SubGetMessage(shoot_sub, &shoot_cmd_recv);
 
     // 对shoot mode等于SHOOT_STOP的情况特殊处理,直接停止所有电机(紧急停止)
@@ -126,71 +151,76 @@ void ShootTask()
 
     // 如果上一次触发单发或3发指令的时间加上不应期仍然大于当前时间(尚未休眠完毕),直接返回即可
     // 单发模式主要提供给能量机关激活使用(以及英雄的射击大部分处于单发)
-    // if (hibernate_time + dead_time > DWT_GetTimeline_ms())
-    //     return;
+    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);             // 同时设定参考值为0,这样停止的速度最快
-        break;
-    // 单发模式,根据鼠标按下的时间,触发一次之后需要进入不响应输入的状态(否则按下的时间内可能多次进入,导致多次发射)
-    case LOAD_1_BULLET:                                                                     // 激活能量机关/干扰对方用,英雄用.
-        DJIMotorOuterLoop(loader, ANGLE_LOOP);                                              // 切换到角度环
-        DJIMotorSetRef(loader, loader->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);                                                  // 切换到速度环
-        DJIMotorSetRef(loader, loader->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需要转的角度,注意换算角速度(DJIMotor的速度单位是angle per second)
-        break;
-    // 拨盘反转,对速度闭环,后续增加卡弹检测(通过裁判系统剩余热量反馈和电机电流)
-    // 也有可能需要从switch-case中独立出来
-    case LOAD_REVERSE:
-        DJIMotorOuterLoop(loader, SPEED_LOOP);
-        // ...
-        break;
-    default:
-        while (1)
-            ; // 未知模式,停止运行,检查指针越界,内存溢出等问题
+        // 停止拨盘
+        case LOAD_STOP:
+            DJIMotorOuterLoop(loader, SPEED_LOOP); // 切换到速度环
+            DJIMotorSetRef(loader, 0);             // 同时设定参考值为0,这样停止的速度最快
+            break;
+            // 单发模式,根据鼠标按下的时间,触发一次之后需要进入不响应输入的状态(否则按下的时间内可能多次进入,导致多次发射)
+        case LOAD_1_BULLET:                                                                     // 激活能量机关/干扰对方用,英雄用.
+            DJIMotorOuterLoop(loader, ANGLE_LOOP);                                              // 切换到角度环
+            DJIMotorSetRef(loader, loader->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);                                                  // 切换到速度环
+            DJIMotorSetRef(loader, loader->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需要转的角度,注意换算角速度(DJIMotor的速度单位是angle per second)
+            break;
+            // 拨盘反转,对速度闭环,后续增加卡弹检测(通过裁判系统剩余热量反馈和电机电流)
+            // 也有可能需要从switch-case中独立出来
+        case LOAD_REVERSE:
+            DJIMotorOuterLoop(loader, SPEED_LOOP);
+            DJIMotorSetRef(loader, 8 * 360 * REDUCTION_RATIO_LOADER / 8);             // 固定速度反转
+            hibernate_time = DWT_GetTimeline_ms();                                              // 记录触发指令的时间
+            dead_time = 500;                                                                    // 翻转500ms
+            shoot_feedback_data.stalled_flag = 0 ; //清除堵转标志
+            // ...
+            break;
+        default:
+            while (1)
+                ; // 未知模式,停止运行,检查指针越界,内存溢出等问题
     }
 
     // 确定是否开启摩擦轮,后续可能修改为键鼠模式下始终开启摩擦轮(上场时建议一直开启)
     if (shoot_cmd_recv.friction_mode == FRICTION_ON)
     {
         // 根据收到的弹速设置设定摩擦轮电机参考值,需实测后填入
-        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,因为还没有加入裁判系统无法读取弹速.
-            DJIMotorSetRef(friction_l, 30000);
-            DJIMotorSetRef(friction_r, 30000);
-            break;
-        }
+//        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,因为还没有加入裁判系统无法读取弹速.
+                DJIMotorSetRef(friction_l, 40000);
+                DJIMotorSetRef(friction_r, 40000);
+//                break;
+//        }
     }
     else // 关闭摩擦轮
     {
@@ -207,6 +237,14 @@ void ShootTask()
     {
         //...
     }
+    //卡弹检测
+    stalled_detect();
+
+
+//    DJIMotorEnable(friction_l);
+//    DJIMotorEnable(friction_r);
+//    DJIMotorSetRef(friction_l, 30000);
+//    DJIMotorSetRef(friction_r, 30000);
 
     // 反馈数据,目前暂时没有要设定的反馈数据,后续可能增加应用离线监测以及卡弹反馈
     PubPushMessage(shoot_pub, (void *)&shoot_feedback_data);

modules/algorithm/crc16.c

@@ -1,7 +1,29 @@
 #include "crc16.h"
 
 static uint8_t crc_tab16_init = 0;
-static uint16_t crc_tab16[256];
+static uint16_t crc_tab16[256] = {
+        0x0000, 0x1189, 0x2312, 0x329b, 0x4624, 0x57ad, 0x6536, 0x74bf, 0x8c48, 0x9dc1, 0xaf5a, 0xbed3,
+        0xca6c, 0xdbe5, 0xe97e, 0xf8f7, 0x1081, 0x0108, 0x3393, 0x221a, 0x56a5, 0x472c, 0x75b7, 0x643e,
+        0x9cc9, 0x8d40, 0xbfdb, 0xae52, 0xdaed, 0xcb64, 0xf9ff, 0xe876, 0x2102, 0x308b, 0x0210, 0x1399,
+        0x6726, 0x76af, 0x4434, 0x55bd, 0xad4a, 0xbcc3, 0x8e58, 0x9fd1, 0xeb6e, 0xfae7, 0xc87c, 0xd9f5,
+        0x3183, 0x200a, 0x1291, 0x0318, 0x77a7, 0x662e, 0x54b5, 0x453c, 0xbdcb, 0xac42, 0x9ed9, 0x8f50,
+        0xfbef, 0xea66, 0xd8fd, 0xc974, 0x4204, 0x538d, 0x6116, 0x709f, 0x0420, 0x15a9, 0x2732, 0x36bb,
+        0xce4c, 0xdfc5, 0xed5e, 0xfcd7, 0x8868, 0x99e1, 0xab7a, 0xbaf3, 0x5285, 0x430c, 0x7197, 0x601e,
+        0x14a1, 0x0528, 0x37b3, 0x263a, 0xdecd, 0xcf44, 0xfddf, 0xec56, 0x98e9, 0x8960, 0xbbfb, 0xaa72,
+        0x6306, 0x728f, 0x4014, 0x519d, 0x2522, 0x34ab, 0x0630, 0x17b9, 0xef4e, 0xfec7, 0xcc5c, 0xddd5,
+        0xa96a, 0xb8e3, 0x8a78, 0x9bf1, 0x7387, 0x620e, 0x5095, 0x411c, 0x35a3, 0x242a, 0x16b1, 0x0738,
+        0xffcf, 0xee46, 0xdcdd, 0xcd54, 0xb9eb, 0xa862, 0x9af9, 0x8b70, 0x8408, 0x9581, 0xa71a, 0xb693,
+        0xc22c, 0xd3a5, 0xe13e, 0xf0b7, 0x0840, 0x19c9, 0x2b52, 0x3adb, 0x4e64, 0x5fed, 0x6d76, 0x7cff,
+        0x9489, 0x8500, 0xb79b, 0xa612, 0xd2ad, 0xc324, 0xf1bf, 0xe036, 0x18c1, 0x0948, 0x3bd3, 0x2a5a,
+        0x5ee5, 0x4f6c, 0x7df7, 0x6c7e, 0xa50a, 0xb483, 0x8618, 0x9791, 0xe32e, 0xf2a7, 0xc03c, 0xd1b5,
+        0x2942, 0x38cb, 0x0a50, 0x1bd9, 0x6f66, 0x7eef, 0x4c74, 0x5dfd, 0xb58b, 0xa402, 0x9699, 0x8710,
+        0xf3af, 0xe226, 0xd0bd, 0xc134, 0x39c3, 0x284a, 0x1ad1, 0x0b58, 0x7fe7, 0x6e6e, 0x5cf5, 0x4d7c,
+        0xc60c, 0xd785, 0xe51e, 0xf497, 0x8028, 0x91a1, 0xa33a, 0xb2b3, 0x4a44, 0x5bcd, 0x6956, 0x78df,
+        0x0c60, 0x1de9, 0x2f72, 0x3efb, 0xd68d, 0xc704, 0xf59f, 0xe416, 0x90a9, 0x8120, 0xb3bb, 0xa232,
+        0x5ac5, 0x4b4c, 0x79d7, 0x685e, 0x1ce1, 0x0d68, 0x3ff3, 0x2e7a, 0xe70e, 0xf687, 0xc41c, 0xd595,
+        0xa12a, 0xb0a3, 0x8238, 0x93b1, 0x6b46, 0x7acf, 0x4854, 0x59dd, 0x2d62, 0x3ceb, 0x0e70, 0x1ff9,
+        0xf78f, 0xe606, 0xd49d, 0xc514, 0xb1ab, 0xa022, 0x92b9, 0x8330, 0x7bc7, 0x6a4e, 0x58d5, 0x495c,
+        0x3de3, 0x2c6a, 0x1ef1, 0x0f78};
 
 /*
  * uint16_t crc_16( const unsigned char *input_str, size_t num_bytes );
@@ -11,21 +33,29 @@ static uint16_t crc_tab16[256];
  *要检查的字节也是一个参数。字符串中的字节数为
  *受恒定大小最大值的限制。
  */
-uint16_t crc_16(const uint8_t *input_str, uint16_t num_bytes)
-{
-    uint16_t crc;
-    const uint8_t *ptr;
-    uint16_t a;
-    if (!crc_tab16_init)
-        init_crc16_tab();
-    crc = CRC_START_16;
-    ptr = input_str;
-    if (ptr != NULL)
-        for (a = 0; a < num_bytes; a++)
-        {
-            crc = (crc >> 8) ^ crc_tab16[(crc ^ (uint16_t)*ptr++) & 0x00FF];
-        }
-    return crc;
+uint16_t crc_16(const uint8_t *input_str, uint16_t num_bytes) {
+//    uint16_t crc;
+//    const uint8_t *ptr;
+//    uint16_t a;
+////    if (!crc_tab16_init)
+////        init_crc16_tab();
+//    crc = CRC_START_16;
+//    ptr = input_str;
+//    if (ptr != NULL)
+//        for (a = 0; a < num_bytes; a++) {
+//            crc = (crc >> 8) ^ crc_tab16[(crc ^ (uint16_t) *ptr++) & 0x00FF];
+//        }
+//    return crc;
+    uint8_t ch_data;
+    uint16_t wCRC = 0xFFFF;
+
+    if (input_str == NULL) return 0xFFFF;
+    while (num_bytes--) {
+        ch_data = *input_str++;
+        (wCRC) =
+                ((uint16_t)(wCRC) >> 8) ^ crc_tab16[((uint16_t)(wCRC) ^ (uint16_t)(ch_data)) & 0x00ff];
+    }
+    return wCRC;
 }
 
 /*
@@ -36,8 +66,7 @@ uint16_t crc_16(const uint8_t *input_str, uint16_t num_bytes)
  *要检查的字节数也是一个参数。
  */
 
-uint16_t crc_modbus(const uint8_t *input_str, uint16_t num_bytes)
-{
+uint16_t crc_modbus(const uint8_t *input_str, uint16_t num_bytes) {
     uint16_t crc;
     const uint8_t *ptr;
     uint16_t a;
@@ -48,10 +77,9 @@ uint16_t crc_modbus(const uint8_t *input_str, uint16_t num_bytes)
     crc = CRC_START_MODBUS;
     ptr = input_str;
     if (ptr != NULL)
-        for (a = 0; a < num_bytes; a++)
-        {
+        for (a = 0; a < num_bytes; a++) {
 
-            crc = (crc >> 8) ^ crc_tab16[(crc ^ (uint16_t)*ptr++) & 0x00FF];
+            crc = (crc >> 8) ^ crc_tab16[(crc ^ (uint16_t) *ptr++) & 0x00FF];
         }
     return crc;
 }
@@ -62,11 +90,10 @@ uint16_t crc_modbus(const uint8_t *input_str, uint16_t num_bytes)
  *函数update_crc_16()根据
  *前一个循环冗余校验值和下一个要检查的数据字节。
  */
-uint16_t update_crc_16(uint16_t crc, uint8_t c)
-{
+uint16_t update_crc_16(uint16_t crc, uint8_t c) {
     if (!crc_tab16_init)
         init_crc16_tab();
-    return (crc >> 8) ^ crc_tab16[(crc ^ (uint16_t)c) & 0x00FF];
+    return (crc >> 8) ^ crc_tab16[(crc ^ (uint16_t) c) & 0x00FF];
 }
 
 /*
@@ -77,18 +104,15 @@ uint16_t update_crc_16(uint16_t crc, uint8_t c)
  *查找表首次由init_crc16_tab()例程计算
  *调用循环冗余校验函数。
  */
-void init_crc16_tab(void)
-{
+void init_crc16_tab(void) {
     uint16_t i;
     uint16_t j;
     uint16_t crc;
     uint16_t c;
-    for (i = 0; i < 256; i++)
-    {
+    for (i = 0; i < 256; i++) {
         crc = 0;
         c = i;
-        for (j = 0; j < 8; j++)
-        {
+        for (j = 0; j < 8; j++) {
             if ((crc ^ c) & 0x0001)
                 crc = (crc >> 1) ^ CRC_POLY_16;
             else
@@ -99,3 +123,12 @@ void init_crc16_tab(void)
     }
     crc_tab16_init = 1;
 }
+
+uint32_t VerifyCRC16CheckSum(uint8_t *pchMessage, uint32_t dwLength) {
+    uint16_t wExpected = 0;
+    if ((pchMessage == NULL) || (dwLength <= 2)) {
+        return 0;
+    }
+    wExpected = crc_16(pchMessage, dwLength - 2);
+    return ((wExpected & 0xff) == pchMessage[dwLength - 2] && ((wExpected >> 8) & 0xff) == pchMessage[dwLength - 1]);
+}

modules/algorithm/crc16.h

@@ -10,5 +10,5 @@ uint16_t crc_16(const uint8_t *input_str, uint16_t num_bytes);
 uint16_t crc_modbus(const uint8_t *input_str, uint16_t num_bytes);
 uint16_t update_crc_16(uint16_t crc, uint8_t c);
 void init_crc16_tab(void);
-
+uint32_t VerifyCRC16CheckSum(uint8_t *pchMessage, uint32_t dwLength);
 #endif

modules/auto_aim/auto_aim.c

@@ -0,0 +1,290 @@
+//
+// Created by sph on 2024/1/21.
+//
+#include "auto_aim.h"
+#include "arm_math.h"
+
+/**
+ * @brief          选择目标装甲板
+ * @author         SJQ
+ * @param[in]      trajectory_cal:弹道解算结构体
+ * @retval         返回空
+ */
+int aim_armor_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_cal) {
+    aim_sel->delay_time = trajectory_cal->fly_time + trajectory_cal->extra_delay_time;
+    aim_sel->target_state.yaw += aim_sel->target_state.v_yaw * aim_sel->delay_time;
+
+    //计算四块装甲板的位置
+    int use_1 = 1;
+    int i = 0;
+    int idx = 0; // 选择的装甲板
+    // 为平衡步兵
+    if (aim_sel->target_state.armor_num == 2) {
+        for (i = 0; i < 2; i++) {
+            float tmp_yaw = aim_sel->target_state.yaw + i * PI;
+            float r = aim_sel->target_state.r1;
+
+            aim_sel->armor_pose[i].x = aim_sel->target_state.x - r * arm_cos_f32(tmp_yaw);
+            aim_sel->armor_pose[i].y = aim_sel->target_state.y - r * arm_sin_f32(tmp_yaw);
+            aim_sel->armor_pose[i].z = aim_sel->target_state.z;
+            aim_sel->armor_pose[i].yaw = aim_sel->target_state.yaw + i * PI;
+        }
+
+//        float yaw_diff_min = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[0].yaw);
+//
+//        //因为是平衡步兵 只需判断两块装甲板即可
+//        float temp_yaw_diff = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[1].yaw);
+//        if (temp_yaw_diff < yaw_diff_min) {
+//            yaw_diff_min = temp_yaw_diff;
+//            idx = 1;
+//        }
+        // 平衡步兵选择两块装甲板中较近的装甲板
+        float distance_min = powf(aim_sel->armor_pose[0].x, 2) + powf(aim_sel->armor_pose[0].y, 2);
+        float distance_temp = powf(aim_sel->armor_pose[1].x, 2) + powf(aim_sel->armor_pose[1].y, 2);
+        if (distance_temp < distance_min) {
+            distance_min = distance_temp;
+            idx = 1;
+        }
+
+    } else if (aim_sel->target_state.armor_num == 3)//前哨站
+    {
+        for (i = 0; i < 3; i++) {
+            float tmp_yaw;
+            if (aim_sel->target_state.v_yaw <= 0)
+                tmp_yaw = aim_sel->target_state.yaw + i * (2.0 * PI / 3.0);
+            else
+                tmp_yaw = aim_sel->target_state.yaw - i * (2.0 * PI / 3.0);
+            float r = use_1 ? aim_sel->target_state.r1 : aim_sel->target_state.r2;
+            aim_sel->armor_pose[i].x = aim_sel->target_state.x - r * cos(tmp_yaw);
+            aim_sel->armor_pose[i].y = aim_sel->target_state.y - r * sin(tmp_yaw);
+            aim_sel->armor_pose[i].z = use_1 ? aim_sel->target_state.z : aim_sel->target_state.z +
+                                                                         aim_sel->target_state.dz;
+            aim_sel->armor_pose[i].yaw = aim_sel->target_state.yaw + i * (2.0 * PI / 3.0);
+            use_1 = !use_1;
+        }
+
+//        //计算枪管到目标装甲板yaw最小的那个装甲板
+//        float yaw_diff_min = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[0].yaw);
+//        for (i = 1; i < 3; i++) {
+//            float temp_yaw_diff = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[i].yaw);
+//            if (temp_yaw_diff < yaw_diff_min) {
+//                yaw_diff_min = temp_yaw_diff;
+//                idx = i;
+//            }
+//        }
+        // 选择两块较近的装甲板，再选择两块装甲板与自身位置连线，同旋转中心自身位置连线，夹角较小的为目标装甲板
+//         float distance_temp0 = powf(aim_sel->armor_pose[0].x, 2) + powf(aim_sel->armor_pose[0].y, 2);
+//         float distance_temp1 = powf(aim_sel->armor_pose[1].x, 2) + powf(aim_sel->armor_pose[1].y, 2);
+//         float distance_temp2 = powf(aim_sel->armor_pose[2].x, 2) + powf(aim_sel->armor_pose[2].y, 2);
+
+//        int label_first = 0;
+//        int label_second = 1;
+//        if (distance_temp0 > distance_temp1) {
+//            label_first = 1;
+//            if (distance_temp0 > distance_temp2) {
+//                label_second = 2;
+//            } else label_second = 0;
+//        } else {
+//            label_first = 0;
+//            if (distance_temp1 > distance_temp2) {
+//                label_second = 2;
+//            } else label_second = 1;
+//        }
+
+        // 选择两块较近的装甲板
+        float distance[3];
+        for (i = 0; i < 3; i++) {
+            distance[i] = powf(aim_sel->armor_pose[i].x, 2) + powf(aim_sel->armor_pose[i].y, 2);
+        }
+
+        int label_first = 0;
+        int label_second = 0;
+
+        for (i = 1; i < 3; i++) {
+            if (distance[i] < distance[label_first]) {
+                label_second = label_first;
+                label_first = i;
+            }else if (distance[i] < distance[label_second] && distance[i] < distance[label_first]) {
+                label_second = i;
+            } else if (distance[i] < distance[label_second]) {
+                label_second = i;
+            }
+        }
+
+        //再选择两块装甲板与自身位置连线，同旋转中心自身位置连线，夹角较小的为目标装甲板
+        float center_length = sqrtf(powf(aim_sel->target_state.x, 2) + powf(aim_sel->target_state.y, 2));
+
+        float cos_theta_first = (aim_sel->armor_pose[label_first].x * aim_sel->target_state.x +
+                                 aim_sel->armor_pose[label_first].y * aim_sel->target_state.y) /
+                                (sqrtf(powf(aim_sel->armor_pose[label_first].x, 2) +
+                                       powf(aim_sel->armor_pose[label_first].y, 2)) * center_length);
+        float cos_theta_second = (aim_sel->armor_pose[label_second].x * aim_sel->target_state.x +
+                                  aim_sel->armor_pose[label_second].y * aim_sel->target_state.y) /
+                                 (sqrtf(powf(aim_sel->armor_pose[label_second].x, 2) +
+                                        powf(aim_sel->armor_pose[label_second].y, 2)) * center_length);
+
+        if (cos_theta_first > cos_theta_second)
+            idx = label_first;
+        else
+            idx = label_second;
+
+    } else {
+        for (i = 0; i < 4; i++) {
+            float tmp_yaw = aim_sel->target_state.yaw + i * PI / 2.0;
+            float r = use_1 ? aim_sel->target_state.r1 : aim_sel->target_state.r2;
+
+            aim_sel->armor_pose[i].x = aim_sel->target_state.x - r * arm_cos_f32(tmp_yaw);
+            aim_sel->armor_pose[i].y = aim_sel->target_state.y - r * arm_sin_f32(tmp_yaw);
+            aim_sel->armor_pose[i].z = use_1 ? aim_sel->target_state.z : aim_sel->target_state.z +
+                                                                         aim_sel->target_state.dz;
+            aim_sel->armor_pose[i].yaw = tmp_yaw;
+            use_1 = !use_1;
+        }
+
+//        //计算枪管到目标装甲板yaw最小的那个装甲板
+//        float yaw_diff_min = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[0].yaw);
+//        for (i = 1; i < 4; i++) {
+//            float temp_yaw_diff = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[i].yaw);
+//            if (temp_yaw_diff < yaw_diff_min) {
+//                yaw_diff_min = temp_yaw_diff;
+//                idx = i;
+//            }
+//        }
+
+        // 选择两块较近的装甲板
+        float distance[4];
+        for (i = 0; i < 4; i++) {
+            distance[i] = powf(aim_sel->armor_pose[i].x, 2) + powf(aim_sel->armor_pose[i].y, 2);
+        }
+
+        int label_first = 0;
+        int label_second = 0;
+
+        for (i = 1; i < 4; i++) {
+            if (distance[i] < distance[label_first]) {
+                label_second = label_first;
+                label_first = i;
+            } else if (distance[i] < distance[label_second] && distance[i] < distance[label_first]) {
+                label_second = i;
+            }else if (distance[i] < distance[label_second]) {
+                label_second = i;
+            }
+        }
+
+        //再选择两块装甲板与自身位置连线，同旋转中心自身位置连线，夹角较小的为目标装甲板
+        float center_length = sqrtf(powf(aim_sel->target_state.x, 2) + powf(aim_sel->target_state.y, 2));
+
+        float cos_theta_first = (aim_sel->armor_pose[label_first].x * aim_sel->target_state.x +
+                                 aim_sel->armor_pose[label_first].y * aim_sel->target_state.y) /
+                                (sqrtf(powf(aim_sel->armor_pose[label_first].x, 2) +
+                                       powf(aim_sel->armor_pose[label_first].y, 2)) * center_length);
+        float cos_theta_second = (aim_sel->armor_pose[label_second].x * aim_sel->target_state.x +
+                                  aim_sel->armor_pose[label_second].y * aim_sel->target_state.y) /
+                                 (sqrtf(powf(aim_sel->armor_pose[label_second].x, 2) +
+                                        powf(aim_sel->armor_pose[label_second].y, 2)) * center_length);
+
+        if (cos_theta_first > cos_theta_second)
+            idx = label_first;
+        else
+            idx = label_second;
+    }
+    aim_sel->aim_point[0] = aim_sel->armor_pose[idx].x + aim_sel->target_state.vx * aim_sel->delay_time;
+    aim_sel->aim_point[1] = aim_sel->armor_pose[idx].y + aim_sel->target_state.vy * aim_sel->delay_time;
+    aim_sel->aim_point[2] = aim_sel->armor_pose[idx].z + aim_sel->target_state.vz * aim_sel->delay_time;
+    return idx;
+}
+
+/**
+  * @brief          子弹飞行时间解算
+  * @author         SJQ
+  * @param[in]      x:瞄准时shooter_link下目标x坐标
+  * @param[in]      vx:瞄准时shooter_link下目标x速度
+  * @param[in]      v_x0:弹速水平分量
+  * @retval         返回空
+  */
+const float k1 = 0.0761; //标准大气压25度下空气阻力系数
+float get_fly_time(float x, float vx, float v_x0) {
+    float t = 0;
+    float f_ti = 0, df_ti = 0;
+    for (int i = 0; i < 5; i++) {
+        f_ti = log(k1 * v_x0 * t + 1) / k1 - x - vx * t;
+        df_ti = v_x0 / (k1 * v_x0 * t + 1) - vx;
+        t = t - f_ti / df_ti;
+    }
+    return t;
+}
+
+/**
+ * @brief          解算期望云台角度（考虑子弹飞行时间）
+ * @author         SJQ
+ * @param[in]      trajectory_cal:弹道解算结构体
+ * @retval         返回空
+ */
+void get_cmd_angle(Trajectory_Type_t *trajectory_cal) {
+
+    arm_power_f32(trajectory_cal->position_xy, 2, &trajectory_cal->dis2);
+    arm_sqrt_f32(trajectory_cal->dis2, &trajectory_cal->dis);
+
+    trajectory_cal->dis = trajectory_cal->dis - 0.20;
+
+    trajectory_cal->theta_0 = atan2f(trajectory_cal->z, trajectory_cal->dis);
+    trajectory_cal->alpha = atan2f(trajectory_cal->position_xy[1], trajectory_cal->position_xy[0]);
+    //将目标的xyz速度转化为平行枪管与垂直枪管的速度
+    trajectory_cal->vx = trajectory_cal->velocity[0] * arm_cos_f32(trajectory_cal->alpha)
+                         + trajectory_cal->velocity[1] * arm_sin_f32(trajectory_cal->alpha);
+    trajectory_cal->vy = -trajectory_cal->velocity[0] * arm_sin_f32(trajectory_cal->alpha)
+                         + trajectory_cal->velocity[1] * arm_cos_f32(trajectory_cal->alpha);
+
+    float v_x0 = trajectory_cal->v0 * arm_cos_f32(trajectory_cal->theta_0);//水平方向弹速
+    float v_y0 = trajectory_cal->v0 * arm_sin_f32(trajectory_cal->theta_0);//竖直方向弹速
+
+    trajectory_cal->fly_time = get_fly_time(trajectory_cal->dis, trajectory_cal->vx, v_x0);
+    arm_power_f32(&trajectory_cal->fly_time, 1, &trajectory_cal->fly_time2);
+    trajectory_cal->h_r = trajectory_cal->z + trajectory_cal->velocity[2] * trajectory_cal->fly_time;
+    //开始迭代
+    trajectory_cal->theta_k = trajectory_cal->theta_0;
+    trajectory_cal->k = 0;
+    while (trajectory_cal->k < 10) {
+        v_y0 = trajectory_cal->v0 * arm_sin_f32(trajectory_cal->theta_k);//竖直方向弹速
+        trajectory_cal->h_k = v_y0 * trajectory_cal->fly_time - 0.5 * 9.8 * trajectory_cal->fly_time2;
+        trajectory_cal->err_k = trajectory_cal->h_r - trajectory_cal->h_k;
+        trajectory_cal->theta_k += 0.1 * trajectory_cal->err_k;
+        trajectory_cal->k++;
+        if (trajectory_cal->err_k < 0.005) break;
+    }
+
+    trajectory_cal->cmd_yaw = atan2f(trajectory_cal->position_xy[1],
+                                     trajectory_cal->position_xy[0]);
+
+    trajectory_cal->cmd_pitch = trajectory_cal->theta_k;
+}
+
+int auto_aim(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_cal, RecievePacket_t *receive_packet) {
+    trajectory_cal->extra_delay_time = 0.035;//0.025
+
+    aim_sel->target_state.armor_type = receive_packet->id;
+    aim_sel->target_state.armor_num = receive_packet->armors_num;
+    aim_sel->target_state.x = receive_packet->x;
+    aim_sel->target_state.y = receive_packet->y;
+    aim_sel->target_state.z = receive_packet->z;
+    aim_sel->target_state.vx = receive_packet->vx;
+    aim_sel->target_state.vy = receive_packet->vy;
+    aim_sel->target_state.vz = receive_packet->vz;
+    aim_sel->target_state.yaw = receive_packet->yaw;
+    aim_sel->target_state.v_yaw = receive_packet->v_yaw;
+    aim_sel->target_state.r1 = receive_packet->r1;
+    aim_sel->target_state.r2 = receive_packet->r2;
+    aim_sel->target_state.dz = receive_packet->dz;
+
+    int idx = aim_armor_select(aim_sel, trajectory_cal);
+
+    trajectory_cal->position_xy[0] = aim_sel->aim_point[0];
+    trajectory_cal->position_xy[1] = aim_sel->aim_point[1];
+    trajectory_cal->z = aim_sel->aim_point[2];
+    trajectory_cal->velocity[0] = aim_sel->target_state.vx;
+    trajectory_cal->velocity[1] = aim_sel->target_state.vy;
+    trajectory_cal->velocity[2] = aim_sel->target_state.vz;
+
+    get_cmd_angle(trajectory_cal);
+    return idx;
+}

modules/auto_aim/auto_aim.h

@@ -0,0 +1,77 @@
+//
+// Created by sph on 2024/1/21.
+//
+
+#ifndef BASIC_FRAMEWORK_AUTO_AIM_H
+#define BASIC_FRAMEWORK_AUTO_AIM_H
+
+#include "master_process.h"
+//弹道解算
+typedef  struct
+{
+    float v0;        //子弹射速
+    float velocity[3];//目标xyz速度
+    float vx;         //目标相对枪口方向的速度
+    float vy;
+    float alpha;      //目标初始航向角
+    float position_xy[2];//目标xy坐标
+    float z;         //目标z坐标
+    float fly_time;  //子弹飞行时间
+    float fly_time2;  //子弹飞行时间平方
+    float extra_delay_time ;
+    float theta_0;   //初始目标角度
+    float theta_k;   //迭代目标角度
+    float dis;       //目标距离
+    float dis2;       //目标距离平方
+    float err_k;      //迭代误差
+    uint8_t k;      //迭代次数
+    float h_k;       //迭代高度
+    float h_r;       //目标真实高度
+
+    float cmd_yaw;
+    float cmd_pitch;
+} Trajectory_Type_t;
+
+//整车状态
+typedef struct
+{
+    float x;
+    float y;
+    float z;
+    float yaw;
+    float vx;
+    float vy;
+    float vz;
+    float v_yaw;
+    float r1;
+    float r2;
+    float dz;
+    uint8_t armor_type;
+    uint8_t armor_num;
+}Target_State_Type_t;
+
+//预瞄点
+typedef struct
+{
+    float x;
+    float y;
+    float z;
+    float yaw;
+}Armor_Pose_Type_t;
+
+typedef struct
+{
+    Target_State_Type_t target_state; //整车状态
+    Armor_Pose_Type_t armor_pose[4]; //四个装甲板状态
+    float aim_point[3];                //预瞄点
+    float delay_time; //预瞄时间差
+    uint8_t suggest_fire;
+}Aim_Select_Type_t;
+
+
+int aim_armor_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_cal);
+float get_fly_time(float x, float vx, float v_x0);
+void get_cmd_angle(Trajectory_Type_t *trajectory_cal);
+int auto_aim(Aim_Select_Type_t *aim_sel,Trajectory_Type_t *trajectory_cal,RecievePacket_t *receive_packet);
+
+#endif //BASIC_FRAMEWORK_AUTO_AIM_H

modules/imu/ins_task.c

@@ -19,6 +19,7 @@
 #include "user_lib.h"
 #include "general_def.h"
 #include "master_process.h"
+#include "crc16.h"
 
 static INS_t INS;
 static IMU_Param_t IMU_Param;
@@ -35,30 +36,26 @@ static float RefTemp = 40; // 恒温设定温度
 
 static void IMU_Param_Correction(IMU_Param_t *param, float gyro[3], float accel[3]);
 
-static void IMUPWMSet(uint16_t pwm)
-{
-    __HAL_TIM_SetCompare(&htim10, TIM_CHANNEL_1, pwm);
+static void IMUPWMSet(uint16_t pwm) {
+            __HAL_TIM_SetCompare(&htim10, TIM_CHANNEL_1, pwm);
 }
 
 /**
  * @brief 温度控制
  *
  */
-static void IMU_Temperature_Ctrl(void)
-{
+static void IMU_Temperature_Ctrl(void) {
     PIDCalculate(&TempCtrl, BMI088.Temperature, RefTemp);
     IMUPWMSet(float_constrain(float_rounding(TempCtrl.Output), 0, UINT32_MAX));
 }
 
 // 使用加速度计的数据初始化Roll和Pitch,而Yaw置0,这样可以避免在初始时候的姿态估计误差
-static void InitQuaternion(float *init_q4)
-{
+static void InitQuaternion(float *init_q4) {
     float acc_init[3] = {0};
     float gravity_norm[3] = {0, 0, 1}; // 导航系重力加速度矢量,归一化后为(0,0,1)
     float axis_rot[3] = {0};           // 旋转轴
     // 读取100次加速度计数据,取平均值作为初始值
-    for (uint8_t i = 0; i < 100; ++i)
-    {
+    for (uint8_t i = 0; i < 100; ++i) {
         BMI088_Read(&BMI088);
         acc_init[X] += BMI088.Accel[X];
         acc_init[Y] += BMI088.Accel[Y];
@@ -77,17 +74,15 @@ static void InitQuaternion(float *init_q4)
         init_q4[i + 1] = axis_rot[i] * sinf(angle / 2.0f); // 轴角公式,第三轴为0(没有z轴分量)
 }
 
-attitude_t *INS_Init(void)
-{
+attitude_t *INS_Init(void) {
     if (!INS.init)
         INS.init = 1;
     else
-        return (attitude_t *)&INS.Gyro;
+        return (attitude_t *) &INS.Gyro;
 
     HAL_TIM_PWM_Start(&htim10, TIM_CHANNEL_1);
 
-    while (BMI088Init(&hspi1, 1) != BMI088_NO_ERROR)
-        ;
+    while (BMI088Init(&hspi1, 1) != BMI088_NO_ERROR);
     IMU_Param.scale[X] = 1;
     IMU_Param.scale[Y] = 1;
     IMU_Param.scale[Z] = 1;
@@ -101,23 +96,22 @@ attitude_t *INS_Init(void)
     IMU_QuaternionEKF_Init(init_quaternion, 10, 0.001, 1000000, 1, 0);
     // imu heat init
     PID_Init_Config_s config = {.MaxOut = 2000,
-                                .IntegralLimit = 300,
-                                .DeadBand = 0,
-                                .Kp = 1000,
-                                .Ki = 20,
-                                .Kd = 0,
-                                .Improve = 0x01}; // enable integratiaon limit
+            .IntegralLimit = 300,
+            .DeadBand = 0,
+            .Kp = 1000,
+            .Ki = 20,
+            .Kd = 0,
+            .Improve = 0x01}; // enable integratiaon limit
     PIDInit(&TempCtrl, &config);
 
     // noise of accel is relatively big and of high freq,thus lpf is used
     INS.AccelLPF = 0.0085;
     DWT_GetDeltaT(&INS_DWT_Count);
-    return (attitude_t *)&INS.Gyro; // @todo: 这里偷懒了,不要这样做! 修改INT_t结构体可能会导致异常,待修复.
+    return (attitude_t *) &INS.Gyro; // @todo: 这里偷懒了,不要这样做! 修改INT_t结构体可能会导致异常,待修复.
 }
 
 /* 注意以1kHz的频率运行此任务 */
-void INS_Task(void)
-{
+void INS_Task(void) {
     static uint32_t count = 0;
     const float gravity[3] = {0, 0, 9.81f};
 
@@ -125,8 +119,7 @@ void INS_Task(void)
     t += dt;
 
     // ins update
-    if ((count % 1) == 0)
-    {
+    if ((count % 1) == 0) {
         BMI088_Read(&BMI088);
 
         INS.Accel[X] = BMI088.Accel[X];
@@ -158,27 +151,27 @@ void INS_Task(void)
         EarthFrameToBodyFrame(gravity, gravity_b, INS.q);
         for (uint8_t i = 0; i < 3; ++i) // 同样过一个低通滤波
         {
-            INS.MotionAccel_b[i] = (INS.Accel[i] - gravity_b[i]) * dt / (INS.AccelLPF + dt) + INS.MotionAccel_b[i] * INS.AccelLPF / (INS.AccelLPF + dt);
+            INS.MotionAccel_b[i] = (INS.Accel[i] - gravity_b[i]) * dt / (INS.AccelLPF + dt) +
+                                   INS.MotionAccel_b[i] * INS.AccelLPF / (INS.AccelLPF + dt);
         }
         BodyFrameToEarthFrame(INS.MotionAccel_b, INS.MotionAccel_n, INS.q); // 转换回导航系n
 
         INS.Yaw = QEKF_INS.Yaw;
-        INS.Pitch = QEKF_INS.Pitch;
-        INS.Roll = QEKF_INS.Roll;
+        INS.Pitch = -QEKF_INS.Pitch;
+        INS.Roll = -QEKF_INS.Roll;
         INS.YawTotalAngle = QEKF_INS.YawTotalAngle;
 
-        VisionSetAltitude(INS.Yaw, INS.Pitch, INS.Roll);
+        //VisionSetAltitude(INS.Yaw, INS.Pitch, INS.Roll);
+        VisionSetAltitude(INS.Yaw * PI / 180, INS.Pitch * PI / 180);
     }
 
     // temperature control
-    if ((count % 2) == 0)
-    {
+    if ((count % 2) == 0) {
         // 500hz
         IMU_Temperature_Ctrl();
     }
 
-    if ((count++ % 1000) == 0)
-    {
+    if ((count++ % 1000) == 0) {
         // 1Hz 可以加入monitor函数,检查IMU是否正常运行/离线
     }
 }
@@ -189,8 +182,7 @@ void INS_Task(void)
  * @param[2]       vector in EarthFrame
  * @param[3]       quaternion
  */
-void BodyFrameToEarthFrame(const float *vecBF, float *vecEF, float *q)
-{
+void BodyFrameToEarthFrame(const float *vecBF, float *vecEF, float *q) {
     vecEF[0] = 2.0f * ((0.5f - q[2] * q[2] - q[3] * q[3]) * vecBF[0] +
                        (q[1] * q[2] - q[0] * q[3]) * vecBF[1] +
                        (q[1] * q[3] + q[0] * q[2]) * vecBF[2]);
@@ -210,8 +202,7 @@ void BodyFrameToEarthFrame(const float *vecBF, float *vecEF, float *q)
  * @param[2]       vector in BodyFrame
  * @param[3]       quaternion
  */
-void EarthFrameToBodyFrame(const float *vecEF, float *vecBF, float *q)
-{
+void EarthFrameToBodyFrame(const float *vecEF, float *vecBF, float *q) {
     vecBF[0] = 2.0f * ((0.5f - q[2] * q[2] - q[3] * q[3]) * vecEF[0] +
                        (q[1] * q[2] + q[0] * q[3]) * vecEF[1] +
                        (q[1] * q[3] - q[0] * q[2]) * vecEF[2]);
@@ -233,16 +224,14 @@ void EarthFrameToBodyFrame(const float *vecEF, float *vecBF, float *q)
  * @param gyro  角速度
  * @param accel 加速度
  */
-static void IMU_Param_Correction(IMU_Param_t *param, float gyro[3], float accel[3])
-{
+static void IMU_Param_Correction(IMU_Param_t *param, float gyro[3], float accel[3]) {
     static float lastYawOffset, lastPitchOffset, lastRollOffset;
     static float c_11, c_12, c_13, c_21, c_22, c_23, c_31, c_32, c_33;
     float cosPitch, cosYaw, cosRoll, sinPitch, sinYaw, sinRoll;
 
     if (fabsf(param->Yaw - lastYawOffset) > 0.001f ||
         fabsf(param->Pitch - lastPitchOffset) > 0.001f ||
-        fabsf(param->Roll - lastRollOffset) > 0.001f || param->flag)
-    {
+        fabsf(param->Roll - lastRollOffset) > 0.001f || param->flag) {
         cosYaw = arm_cos_f32(param->Yaw / 57.295779513f);
         cosPitch = arm_cos_f32(param->Pitch / 57.295779513f);
         cosRoll = arm_cos_f32(param->Roll / 57.295779513f);
@@ -302,8 +291,7 @@ static void IMU_Param_Correction(IMU_Param_t *param, float gyro[3], float accel[
 /**
  * @brief        Update quaternion
  */
-void QuaternionUpdate(float *q, float gx, float gy, float gz, float dt)
-{
+void QuaternionUpdate(float *q, float gx, float gy, float gz, float dt) {
     float qa, qb, qc;
 
     gx *= 0.5f * dt;
@@ -321,8 +309,7 @@ void QuaternionUpdate(float *q, float gx, float gy, float gz, float dt)
 /**
  * @brief        Convert quaternion to eular angle
  */
-void QuaternionToEularAngle(float *q, float *Yaw, float *Pitch, float *Roll)
-{
+void QuaternionToEularAngle(float *q, float *Yaw, float *Pitch, float *Roll) {
     *Yaw = atan2f(2.0f * (q[0] * q[3] + q[1] * q[2]), 2.0f * (q[0] * q[0] + q[1] * q[1]) - 1.0f) * 57.295779513f;
     *Pitch = atan2f(2.0f * (q[0] * q[1] + q[2] * q[3]), 2.0f * (q[0] * q[0] + q[3] * q[3]) - 1.0f) * 57.295779513f;
     *Roll = asinf(2.0f * (q[0] * q[2] - q[1] * q[3])) * 57.295779513f;
@@ -331,8 +318,7 @@ void QuaternionToEularAngle(float *q, float *Yaw, float *Pitch, float *Roll)
 /**
  * @brief        Convert eular angle to quaternion
  */
-void EularAngleToQuaternion(float Yaw, float Pitch, float Roll, float *q)
-{
+void EularAngleToQuaternion(float Yaw, float Pitch, float Roll, float *q) {
     float cosPitch, cosYaw, cosRoll, sinPitch, sinYaw, sinRoll;
     Yaw /= 57.295779513f;
     Pitch /= 57.295779513f;

modules/master_machine/master_process.c

@@ -9,44 +9,46 @@
  *
  */
 #include "master_process.h"
-#include "seasky_protocol.h"
 #include "daemon.h"
 #include "bsp_log.h"
 #include "robot_def.h"
+#include "crc16.h"
 
-static Vision_Recv_s recv_data;
-static Vision_Send_s send_data;
+static RecievePacket_t recv_data;
+static SendPacket_t send_data;
 static DaemonInstance *vision_daemon_instance;
 
-void VisionSetFlag(Enemy_Color_e enemy_color, Work_Mode_e work_mode, Bullet_Speed_e bullet_speed)
+//void VisionSetFlag(Enemy_Color_e enemy_color, Work_Mode_e work_mode, Bullet_Speed_e bullet_speed)
+//{
+//    send_data.enemy_color = enemy_color;
+//    send_data.work_mode = work_mode;
+//    send_data.bullet_speed = bullet_speed;
+//}
+void VisionSetFlag(Enemy_Color_e enemy_color)
 {
-    send_data.enemy_color = enemy_color;
-    send_data.work_mode = work_mode;
-    send_data.bullet_speed = bullet_speed;
+    send_data.detect_color = enemy_color;
+    send_data.reserved = 0;
 }
 
-void VisionSetAltitude(float yaw, float pitch, float roll)
+//void VisionSetAltitude(float yaw, float pitch)
+//{
+//    send_data.yaw = yaw;
+//    send_data.pitch = pitch;
+//}
+
+void VisionSetAltitude(float yaw, float pitch)
 {
     send_data.yaw = yaw;
     send_data.pitch = pitch;
-    send_data.roll = roll;
 }
 
-/**
- * @brief 离线回调函数,将在daemon.c中被daemon task调用
- * @attention 由于HAL库的设计问题,串口开启DMA接收之后同时发送有概率出现__HAL_LOCK()导致的死锁,使得无法
- *            进入接收中断.通过daemon判断数据更新,重新调用服务启动函数以解决此问题.
- *
- * @param id vision_usart_instance的地址,此处没用.
- */
-static void VisionOfflineCallback(void *id)
-{
-#ifdef VISION_USE_UART
-    USARTServiceInit(vision_usart_instance);
-#endif // !VISION_USE_UART
-    LOGWARNING("[vision] vision offline, restart communication.");
+void VisionSetAim(float aim_x, float aim_y, float aim_z) {
+    send_data.aim_x = aim_x;
+    send_data.aim_y = aim_y;
+    send_data.aim_z = aim_z;
 }
 
+
 #ifdef VISION_USE_UART
 
 #include "bsp_usart.h"
@@ -113,17 +115,45 @@ void VisionSend()
 #ifdef VISION_USE_VCP
 
 #include "bsp_usb.h"
-static uint8_t *vis_recv_buff;
+static uint8_t *vis_recv_buff; //接收到的原始数据
+
 
 static void DecodeVision(uint16_t recv_len)
 {
-    uint16_t flag_register;
-    get_protocol_info(vis_recv_buff, &flag_register, (uint8_t *)&recv_data.pitch);
-    // TODO: code to resolve flag_register;
+//    uint16_t flag_register;
+//    get_protocol_info(vis_recv_buff, &flag_register, (uint8_t *)&recv_data.pitch);
+//    // TODO: code to resolve flag_register;
+    if(vis_recv_buff[0]==0xA5)
+    {
+        if(VerifyCRC16CheckSum(vis_recv_buff,sizeof(recv_data)))
+        {
+            memcpy(&recv_data,vis_recv_buff,sizeof(recv_data));
+            DaemonReload(vision_daemon_instance);
+        }
+    }
+}
+
+/**
+ * @brief 离线回调函数,将在daemon.c中被daemon task调用
+ * @attention 由于HAL库的设计问题,串口开启DMA接收之后同时发送有概率出现__HAL_LOCK()导致的死锁,使得无法
+ *            进入接收中断.通过daemon判断数据更新,重新调用服务启动函数以解决此问题.
+ *
+ * @param id vision_usart_instance的地址,此处没用.
+ */
+static void VisionOfflineCallback(void *id)
+{
+#ifdef VISION_USE_UART
+    USARTServiceInit(vision_usart_instance);
+#endif // !VISION_USE_UART
+    memset(&recv_data,0,sizeof (recv_data));
+    memset(vis_recv_buff,0,sizeof (recv_data));
+    USB_Init_Config_s conf = {.rx_cbk = DecodeVision};
+    vis_recv_buff = USBInit(conf);
+    LOGWARNING("[vision] vision offline, restart communication.");
 }
 
 /* 视觉通信初始化 */
-Vision_Recv_s *VisionInit(UART_HandleTypeDef *_handle)
+RecievePacket_t *VisionInit(UART_HandleTypeDef *_handle)
 {
     UNUSED(_handle); // 仅为了消除警告
     USB_Init_Config_s conf = {.rx_cbk = DecodeVision};
@@ -142,14 +172,23 @@ Vision_Recv_s *VisionInit(UART_HandleTypeDef *_handle)
 
 void VisionSend()
 {
-    static uint16_t flag_register;
-    static uint8_t send_buff[VISION_SEND_SIZE];
-    static uint16_t tx_len;
-    // TODO: code to set flag_register
-    flag_register = 30 << 8 | 0b00000001;
-    // 将数据转化为seasky协议的数据包
-    get_protocol_send_data(0x02, flag_register, &send_data.yaw, 3, send_buff, &tx_len);
-    USBTransmit(send_buff, tx_len);
+//    static uint16_t flag_register;
+//    static uint8_t send_buff[VISION_SEND_SIZE];
+//    static uint16_t tx_len;
+//    // TODO: code to set flag_register
+//    flag_register = 30 << 8 | 0b00000001;
+//    // 将数据转化为seasky协议的数据包
+//    get_protocol_send_data(0x02, flag_register, &send_data.yaw, 3, send_buff, &tx_len);
+//    USBTransmit(send_buff, tx_len);
+    static uint8_t send_buffer[24]={0};
+
+    send_data.header = 0x5A;
+//    VisionSetFlag(data->detect_color);
+//    VisionSetAim(data->aim_x,data->aim_y,data->aim_z);
+    send_data.checksum = crc_16(&send_data.header,sizeof(send_data)-2);
+
+    memcpy(send_buffer,&send_data,sizeof(send_data));
+    USBTransmit(send_buffer, sizeof(send_data));
 }
 
 #endif // VISION_USE_VCP

modules/master_machine/master_process.h

@@ -8,77 +8,103 @@
 #define VISION_SEND_SIZE 36u
 
 #pragma pack(1)
-typedef enum
-{
-	NO_FIRE = 0,
-	AUTO_FIRE = 1,
-	AUTO_AIM = 2
+typedef enum {
+    NO_FIRE = 0,
+    AUTO_FIRE = 1,
+    AUTO_AIM = 2
 } Fire_Mode_e;
 
-typedef enum
-{
-	NO_TARGET = 0,
-	TARGET_CONVERGING = 1,
-	READY_TO_FIRE = 2
+typedef enum {
+    NO_TARGET = 0,
+    TARGET_CONVERGING = 1,
+    READY_TO_FIRE = 2
 } Target_State_e;
 
-typedef enum
-{
-	NO_TARGET_NUM = 0,
-	HERO1 = 1,
-	ENGINEER2 = 2,
-	INFANTRY3 = 3,
-	INFANTRY4 = 4,
-	INFANTRY5 = 5,
-	OUTPOST = 6,
-	SENTRY = 7,
-	BASE = 8
+typedef enum {
+    NO_TARGET_NUM = 0,
+    HERO1 = 1,
+    ENGINEER2 = 2,
+    INFANTRY3 = 3,
+    INFANTRY4 = 4,
+    INFANTRY5 = 5,
+    OUTPOST = 6,
+    SENTRY = 7,
+    BASE = 8
 } Target_Type_e;
 
-typedef struct
-{
-	Fire_Mode_e fire_mode;
-	Target_State_e target_state;
-	Target_Type_e target_type;
+typedef struct {
+    Fire_Mode_e fire_mode;
+    Target_State_e target_state;
+    Target_Type_e target_type;
 
-	float pitch;
-	float yaw;
+    float pitch;
+    float yaw;
 } Vision_Recv_s;
 
-typedef enum
-{
-	COLOR_NONE = 0,
-	COLOR_BLUE = 1,
-	COLOR_RED = 2,
+typedef enum {
+//	COLOR_NONE = 0,
+//	COLOR_BLUE = 1,
+//	COLOR_RED = 2,
+    ENEMY_COLOR_RED = 0,
+    ENEMY_COLOR_BLUE = 1,
 } Enemy_Color_e;
 
-typedef enum
-{
-	VISION_MODE_AIM = 0,
-	VISION_MODE_SMALL_BUFF = 1,
-	VISION_MODE_BIG_BUFF = 2
+typedef enum {
+    VISION_MODE_AIM = 0,
+    VISION_MODE_SMALL_BUFF = 1,
+    VISION_MODE_BIG_BUFF = 2
 } Work_Mode_e;
 
-typedef enum
-{
-	BULLET_SPEED_NONE = 0,
-	BIG_AMU_10 = 10,
-	SMALL_AMU_15 = 15,
-	BIG_AMU_16 = 16,
-	SMALL_AMU_18 = 18,
-	SMALL_AMU_30 = 30,
+typedef enum {
+    BULLET_SPEED_NONE = 0,
+    BIG_AMU_10 = 10,
+    SMALL_AMU_15 = 15,
+    BIG_AMU_16 = 16,
+    SMALL_AMU_18 = 18,
+    SMALL_AMU_30 = 30,
 } Bullet_Speed_e;
 
-typedef struct
-{
-	Enemy_Color_e enemy_color;
-	Work_Mode_e work_mode;
-	Bullet_Speed_e bullet_speed;
+typedef struct {
+    Enemy_Color_e enemy_color;
+    Work_Mode_e work_mode;
+    Bullet_Speed_e bullet_speed;
 
-	float yaw;
-	float pitch;
-	float roll;
+    float yaw;
+    float pitch;
+    float roll;
 } Vision_Send_s;
+
+typedef __packed struct {
+    uint8_t header;//0x5A
+    uint8_t detect_color: 1;
+    uint8_t reserved: 7;
+    float pitch;
+    float yaw;
+    float aim_x;
+    float aim_y;
+    float aim_z;
+    uint16_t checksum;
+} SendPacket_t;
+
+typedef __packed struct {
+    uint8_t header; //= 0xA5;
+    uint8_t tracking: 1;
+    uint8_t id: 3;          // 0-outpost 6-guard 7-base
+    uint8_t armors_num: 3;  // 2-balance 3-outpost 4-normal
+    uint8_t reserved: 1;
+    float x;
+    float y;
+    float z;
+    float yaw;
+    float vx;
+    float vy;
+    float vz;
+    float v_yaw;
+    float r1;
+    float r2;
+    float dz;
+    uint16_t checksum;
+} RecievePacket_t;
 #pragma pack()
 
 /**
@@ -86,7 +112,8 @@ typedef struct
  *
  * @param handle 用于和视觉通信的串口handle(C板上一般为USART1,丝印为USART2,4pin)
  */
-Vision_Recv_s *VisionInit(UART_HandleTypeDef *_handle);
+//Vision_Recv_s *VisionInit(UART_HandleTypeDef *_handle);
+RecievePacket_t *VisionInit(UART_HandleTypeDef *_handle);
 
 /**
  * @brief 发送视觉数据
@@ -101,7 +128,8 @@ void VisionSend();
  * @param work_mode
  * @param bullet_speed
  */
-void VisionSetFlag(Enemy_Color_e enemy_color, Work_Mode_e work_mode, Bullet_Speed_e bullet_speed);
+//void VisionSetFlag(Enemy_Color_e enemy_color, Work_Mode_e work_mode, Bullet_Speed_e bullet_speed);
+void VisionSetFlag(Enemy_Color_e enemy_color);
 
 /**
  * @brief 设置发送数据的姿态部分
@@ -109,6 +137,8 @@ void VisionSetFlag(Enemy_Color_e enemy_color, Work_Mode_e work_mode, Bullet_Spee
  * @param yaw
  * @param pitch
  */
-void VisionSetAltitude(float yaw, float pitch, float roll);
+//void VisionSetAltitude(float yaw, float pitch, float roll);
+void VisionSetAltitude(float yaw, float pitch);
 
+void VisionSetAim(float aim_x, float aim_y,float aim_z);
 #endif // !MASTER_PROCESS_H

modules/motor/DJImotor/dji_motor.c

@@ -2,6 +2,7 @@
 #include "general_def.h"
 #include "bsp_dwt.h"
 #include "bsp_log.h"
+#include "user_lib.h"
 
 static uint8_t idx = 0; // register idx,是该文件的全局电机索引,在注册时使用
 /* DJI电机的实例,此处仅保存指针,内存的分配将通过电机实例初始化时通过malloc()进行 */
@@ -19,13 +20,27 @@ static DJIMotorInstance *dji_motor_instance[DJI_MOTOR_CNT] = {NULL}; // 会在co
  * 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[10] = {
+        [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}},
+        [6] = {.can_handle = &hcan1, .txconf.StdId = 0x1fe, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {
+                0}},
+        [7] = {.can_handle = &hcan1, .txconf.StdId = 0x2fe, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {
+                0}},
+        [8] = {.can_handle = &hcan2, .txconf.StdId = 0x1fe, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {
+                0}},
+        [9] = {.can_handle = &hcan2, .txconf.StdId = 0x2fe, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {
+                0}}
 };
 
 /**
@@ -38,78 +53,88 @@ static uint8_t sender_enable_flag[6] = {0};
  * @brief 根据电调/拨码开关上的ID,根据说明书的默认id分配方式计算发送ID和接收ID,
  *        并对电机进行分组以便处理多电机控制命令
  */
-static void MotorSenderGrouping(DJIMotorInstance *motor, CAN_Init_Config_s *config)
-{
+static void MotorSenderGrouping(DJIMotorInstance *motor, CAN_Init_Config_s *config) {
     uint8_t motor_id = config->tx_id - 1; // 下标从零开始,先减一方便赋值
     uint8_t motor_send_num;
     uint8_t motor_grouping;
 
-    switch (motor->motor_type)
-    {
-    case M2006:
-    case M3508:
-        if (motor_id < 4) // 根据ID分组
-        {
-            motor_send_num = motor_id;
-            motor_grouping = config->can_handle == &hcan1 ? 1 : 4;
-        }
-        else
-        {
-            motor_send_num = motor_id - 4;
-            motor_grouping = config->can_handle == &hcan1 ? 0 : 3;
-        }
-
-        // 计算接收id并设置分组发送id
-        config->rx_id = 0x200 + motor_id + 1;   // 把ID+1,进行分组设置
-        sender_enable_flag[motor_grouping] = 1; // 设置发送标志位,防止发送空帧
-        motor->message_num = motor_send_num;
-        motor->sender_group = motor_grouping;
-
-        // 检查是否发生id冲突
-        for (size_t i = 0; i < idx; ++i)
-        {
-            if (dji_motor_instance[i]->motor_can_instance->can_handle == config->can_handle && dji_motor_instance[i]->motor_can_instance->rx_id == config->rx_id)
+    switch (motor->motor_type) {
+        case M2006:
+        case M3508:
+            if (motor_id < 4) // 根据ID分组
             {
-                LOGERROR("[dji_motor] ID crash. Check in debug mode, add dji_motor_instance to watch to get more information.");
-                uint16_t can_bus = config->can_handle == &hcan1 ? 1 : 2;
-                while (1) // 6020的id 1-4和2006/3508的id 5-8会发生冲突(若有注册,即1!5,2!6,3!7,4!8) (1!5!,LTC! (((不是)
-                    LOGERROR("[dji_motor] id [%d], can_bus [%d]", config->rx_id, can_bus);
+                motor_send_num = motor_id;
+                motor_grouping = config->can_handle == &hcan1 ? 1 : 4;
+            } else {
+                motor_send_num = motor_id - 4;
+                motor_grouping = config->can_handle == &hcan1 ? 0 : 3;
             }
-        }
-        break;
 
-    case GM6020:
-        if (motor_id < 4)
-        {
-            motor_send_num = motor_id;
-            motor_grouping = config->can_handle == &hcan1 ? 0 : 3;
-        }
-        else
-        {
-            motor_send_num = motor_id - 4;
-            motor_grouping = config->can_handle == &hcan1 ? 2 : 5;
-        }
+            // 计算接收id并设置分组发送id
+            config->rx_id = 0x200 + motor_id + 1;   // 把ID+1,进行分组设置
+            sender_enable_flag[motor_grouping] = 1; // 设置发送标志位,防止发送空帧
+            motor->message_num = motor_send_num;
+            motor->sender_group = motor_grouping;
 
-        config->rx_id = 0x204 + motor_id + 1;   // 把ID+1,进行分组设置
-        sender_enable_flag[motor_grouping] = 1; // 只要有电机注册到这个分组,置为1;在发送函数中会通过此标志判断是否有电机注册
-        motor->message_num = motor_send_num;
-        motor->sender_group = motor_grouping;
+            // 检查是否发生id冲突
+            for (size_t i = 0; i < idx; ++i) {
+                if (dji_motor_instance[i]->motor_can_instance->can_handle == config->can_handle &&
+                    dji_motor_instance[i]->motor_can_instance->rx_id == config->rx_id) {
+                    LOGERROR(
+                            "[dji_motor] ID crash. Check in debug mode, add dji_motor_instance to watch to get more information.");
+                    uint16_t can_bus = config->can_handle == &hcan1 ? 1 : 2;
+                    while (1) // 6020的id 1-4和2006/3508的id 5-8会发生冲突(若有注册,即1!5,2!6,3!7,4!8) (1!5!,LTC! (((不是)
+                        LOGERROR("[dji_motor] id [%d], can_bus [%d]", config->rx_id, can_bus);
+                }
+            }
+            break;
 
-        for (size_t i = 0; i < idx; ++i)
-        {
-            if (dji_motor_instance[i]->motor_can_instance->can_handle == config->can_handle && dji_motor_instance[i]->motor_can_instance->rx_id == config->rx_id)
+        case GM6020:
+            if(motor->motor_control_type == CURRENT_CONTROL) //6020电流控制帧id  0x1fe  0x2fe
             {
-                LOGERROR("[dji_motor] ID crash. Check in debug mode, add dji_motor_instance to watch to get more information.");
-                uint16_t can_bus = config->can_handle == &hcan1 ? 1 : 2;
-                while (1) // 6020的id 1-4和2006/3508的id 5-8会发生冲突(若有注册,即1!5,2!6,3!7,4!8) (1!5!,LTC! (((不是)
-                    LOGERROR("[dji_motor] id [%d], can_bus [%d]", config->rx_id, can_bus);
+                if (motor_id < 4)
+                {
+                    motor_send_num = motor_id;
+                    motor_grouping = config->can_handle == &hcan1 ? 6 : 8;
+                }
+                else
+                {
+                    motor_send_num = motor_id - 4;
+                    motor_grouping = config->can_handle == &hcan1 ? 7 : 9;
+                }
+            }else{
+                if (motor_id < 4)
+                {
+                    motor_send_num = motor_id;
+                    motor_grouping = config->can_handle == &hcan1 ? 0 : 3;
+                }
+                else
+                {
+                    motor_send_num = motor_id - 4;
+                    motor_grouping = config->can_handle == &hcan1 ? 2 : 5;
+                }
             }
-        }
-        break;
 
-    default: // other motors should not be registered here
-        while (1)
-            LOGERROR("[dji_motor]You must not register other motors using the API of DJI motor."); // 其他电机不应该在这里注册
+            config->rx_id = 0x204 + motor_id + 1;   // 把ID+1,进行分组设置
+            sender_enable_flag[motor_grouping] = 1; // 只要有电机注册到这个分组,置为1;在发送函数中会通过此标志判断是否有电机注册
+            motor->message_num = motor_send_num;
+            motor->sender_group = motor_grouping;
+
+            for (size_t i = 0; i < idx; ++i) {
+                if (dji_motor_instance[i]->motor_can_instance->can_handle == config->can_handle &&
+                    dji_motor_instance[i]->motor_can_instance->rx_id == config->rx_id) {
+                    LOGERROR(
+                            "[dji_motor] ID crash. Check in debug mode, add dji_motor_instance to watch to get more information.");
+                    uint16_t can_bus = config->can_handle == &hcan1 ? 1 : 2;
+                    while (1) // 6020的id 1-4和2006/3508的id 5-8会发生冲突(若有注册,即1!5,2!6,3!7,4!8) (1!5!,LTC! (((不是)
+                        LOGERROR("[dji_motor] id [%d], can_bus [%d]", config->rx_id, can_bus);
+                }
+            }
+            break;
+
+        default: // other motors should not be registered here
+            while (1)
+                LOGERROR("[dji_motor]You must not register other motors using the API of DJI motor."); // 其他电机不应该在这里注册
     }
 }
 
@@ -119,12 +144,11 @@ static void MotorSenderGrouping(DJIMotorInstance *motor, CAN_Init_Config_s *conf
  *
  * @param _instance 收到数据的instance,通过遍历与所有电机进行对比以选择正确的实例
  */
-static void DecodeDJIMotor(CANInstance *_instance)
-{
+static void DecodeDJIMotor(CANInstance *_instance) {
     // 这里对can instance的id进行了强制转换,从而获得电机的instance实例地址
     // _instance指针指向的id是对应电机instance的地址,通过强制转换为电机instance的指针,再通过->运算符访问电机的成员motor_measure,最后取地址获得指针
     uint8_t *rxbuff = _instance->rx_buff;
-    DJIMotorInstance *motor = (DJIMotorInstance *)_instance->id;
+    DJIMotorInstance *motor = (DJIMotorInstance *) _instance->id;
     DJI_Motor_Measure_s *measure = &motor->measure; // measure要多次使用,保存指针减小访存开销
 
     DaemonReload(motor->daemon);
@@ -132,12 +156,12 @@ static void DecodeDJIMotor(CANInstance *_instance)
 
     // 解析数据并对电流和速度进行滤波,电机的反馈报文具体格式见电机说明手册
     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;
+    measure->ecd = ((uint16_t) rxbuff[0]) << 8 | rxbuff[1];
+    measure->angle_single_round = ECD_ANGLE_COEF_DJI * (float) measure->ecd;
     measure->speed_aps = (1.0f - SPEED_SMOOTH_COEF) * measure->speed_aps +
-                         RPM_2_ANGLE_PER_SEC * SPEED_SMOOTH_COEF * (float)((int16_t)(rxbuff[2] << 8 | rxbuff[3]));
+                         RPM_2_ANGLE_PER_SEC * SPEED_SMOOTH_COEF * (float) ((int16_t) (rxbuff[2] << 8 | rxbuff[3]));
     measure->real_current = (1.0f - CURRENT_SMOOTH_COEF) * measure->real_current +
-                            CURRENT_SMOOTH_COEF * (float)((int16_t)(rxbuff[4] << 8 | rxbuff[5]));
+                            CURRENT_SMOOTH_COEF * (float) ((int16_t) (rxbuff[4] << 8 | rxbuff[5]));
     measure->temperature = rxbuff[6];
 
     // 多圈角度计算,前提是假设两次采样间电机转过的角度小于180°,自己画个图就清楚计算过程了
@@ -148,22 +172,21 @@ static void DecodeDJIMotor(CANInstance *_instance)
     measure->total_angle = measure->total_round * 360 + measure->angle_single_round;
 }
 
-static void DJIMotorLostCallback(void *motor_ptr)
-{
-    DJIMotorInstance *motor = (DJIMotorInstance *)motor_ptr;
+static void DJIMotorLostCallback(void *motor_ptr) {
+    DJIMotorInstance *motor = (DJIMotorInstance *) motor_ptr;
     uint16_t can_bus = motor->motor_can_instance->can_handle == &hcan1 ? 1 : 2;
     LOGWARNING("[dji_motor] Motor lost, can bus [%d] , id [%d]", can_bus, motor->motor_can_instance->tx_id);
 }
 
 // 电机初始化,返回一个电机实例
-DJIMotorInstance *DJIMotorInit(Motor_Init_Config_s *config)
-{
-    DJIMotorInstance *instance = (DJIMotorInstance *)malloc(sizeof(DJIMotorInstance));
+DJIMotorInstance *DJIMotorInit(Motor_Init_Config_s *config) {
+    DJIMotorInstance *instance = (DJIMotorInstance *) malloc(sizeof(DJIMotorInstance));
     memset(instance, 0, sizeof(DJIMotorInstance));
 
     // motor basic setting 电机基本设置
     instance->motor_type = config->motor_type;                         // 6020 or 2006 or 3508
     instance->motor_settings = config->controller_setting_init_config; // 正反转,闭环类型等
+    instance->motor_control_type = config->motor_control_type; //电流控制or电压控制
 
     // motor controller init 电机控制器初始化
     PIDInit(&instance->motor_controller.current_PID, &config->controller_param_init_config.current_PID);
@@ -185,9 +208,9 @@ DJIMotorInstance *DJIMotorInit(Motor_Init_Config_s *config)
 
     // 注册守护线程
     Daemon_Init_Config_s daemon_config = {
-        .callback = DJIMotorLostCallback,
-        .owner_id = instance,
-        .reload_count = 2, // 20ms未收到数据则丢失
+            .callback = DJIMotorLostCallback,
+            .owner_id = instance,
+            .reload_count = 2, // 20ms未收到数据则丢失
     };
     instance->daemon = DaemonRegister(&daemon_config);
 
@@ -197,8 +220,7 @@ DJIMotorInstance *DJIMotorInit(Motor_Init_Config_s *config)
 }
 
 /* 电流只能通过电机自带传感器监测,后续考虑加入力矩传感器应变片等 */
-void DJIMotorChangeFeed(DJIMotorInstance *motor, Closeloop_Type_e loop, Feedback_Source_e type)
-{
+void DJIMotorChangeFeed(DJIMotorInstance *motor, Closeloop_Type_e loop, Feedback_Source_e type) {
     if (loop == ANGLE_LOOP)
         motor->motor_settings.angle_feedback_source = type;
     else if (loop == SPEED_LOOP)
@@ -207,31 +229,38 @@ void DJIMotorChangeFeed(DJIMotorInstance *motor, Closeloop_Type_e loop, Feedback
         LOGERROR("[dji_motor] loop type error, check memory access and func param"); // 检查是否传入了正确的LOOP类型,或发生了指针越界
 }
 
-void DJIMotorStop(DJIMotorInstance *motor)
-{
+void DJIMotorStop(DJIMotorInstance *motor) {
     motor->stop_flag = MOTOR_STOP;
 }
 
-void DJIMotorEnable(DJIMotorInstance *motor)
-{
+void DJIMotorEnable(DJIMotorInstance *motor) {
     motor->stop_flag = MOTOR_ENALBED;
 }
 
 /* 修改电机的实际闭环对象 */
-void DJIMotorOuterLoop(DJIMotorInstance *motor, Closeloop_Type_e outer_loop)
-{
+void DJIMotorOuterLoop(DJIMotorInstance *motor, Closeloop_Type_e outer_loop) {
     motor->motor_settings.outer_loop_type = outer_loop;
 }
 
 // 设置参考值
-void DJIMotorSetRef(DJIMotorInstance *motor, float ref)
-{
+void DJIMotorSetRef(DJIMotorInstance *motor, float ref) {
     motor->motor_controller.pid_ref = ref;
 }
 
-// 为所有电机实例计算三环PID,发送控制报文
-void DJIMotorControl()
+static const float motor_power_K[3] = {1.6301e-6f,5.7501e-7f,2.5863e-7f};
+//依据3508电机功率模型，预测电机输出功率
+static float EstimatePower(DJIMotorInstance* chassis_motor,float output)
 {
+    float I_cmd = chassis_motor->motor_controller.current_PID.Output;
+    float w = chassis_motor->measure.speed_aps /6 ;//aps to rpm
+
+    float power = motor_power_K[0] * I_cmd * w + motor_power_K[1]*w*w  + motor_power_K[2]*I_cmd*I_cmd;
+    return power;
+}
+
+
+// 为所有电机实例计算三环PID,发送控制报文
+void DJIMotorControl() {
     // 直接保存一次指针引用从而减小访存的开销,同样可以提高可读性
     uint8_t group, num; // 电机组号和组内编号
     int16_t set;        // 电机控制CAN发送设定值
@@ -242,8 +271,7 @@ void DJIMotorControl()
     float pid_measure, pid_ref;             // 电机PID测量值和设定值
 
     // 遍历所有电机实例,进行串级PID的计算并设置发送报文的值
-    for (size_t i = 0; i < idx; ++i)
-    { // 减小访存开销,先保存指针引用
+    for (size_t i = 0; i < idx; ++i) { // 减小访存开销,先保存指针引用
         motor = dji_motor_instance[i];
         motor_setting = &motor->motor_settings;
         motor_controller = &motor->motor_controller;
@@ -254,8 +282,7 @@ void DJIMotorControl()
 
         // pid_ref会顺次通过被启用的闭环充当数据的载体
         // 计算位置环,只有启用位置环且外层闭环为位置时会计算速度环输出
-        if ((motor_setting->close_loop_type & ANGLE_LOOP) && motor_setting->outer_loop_type == ANGLE_LOOP)
-        {
+        if ((motor_setting->close_loop_type & ANGLE_LOOP) && motor_setting->outer_loop_type == ANGLE_LOOP) {
             if (motor_setting->angle_feedback_source == OTHER_FEED)
                 pid_measure = *motor_controller->other_angle_feedback_ptr;
             else
@@ -265,8 +292,8 @@ void DJIMotorControl()
         }
 
         // 计算速度环,(外层闭环为速度或位置)且(启用速度环)时会计算速度环
-        if ((motor_setting->close_loop_type & SPEED_LOOP) && (motor_setting->outer_loop_type & (ANGLE_LOOP | SPEED_LOOP)))
-        {
+        if ((motor_setting->close_loop_type & SPEED_LOOP) &&
+            (motor_setting->outer_loop_type & (ANGLE_LOOP | SPEED_LOOP))) {
             if (motor_setting->feedforward_flag & SPEED_FEEDFORWARD)
                 pid_ref += *motor_controller->speed_feedforward_ptr;
 
@@ -281,22 +308,54 @@ void DJIMotorControl()
         // 计算电流环,目前只要启用了电流环就计算,不管外层闭环是什么,并且电流只有电机自身传感器的反馈
         if (motor_setting->feedforward_flag & CURRENT_FEEDFORWARD)
             pid_ref += *motor_controller->current_feedforward_ptr;
-        if (motor_setting->close_loop_type & CURRENT_LOOP)
-        {
-            pid_ref = PIDCalculate(&motor_controller->current_PID, measure->real_current, pid_ref);
+        if (motor_setting->close_loop_type & CURRENT_LOOP) {
+            //现在电调都有内置电流环，无需pid计算
+            //pid_ref = PIDCalculate(&motor_controller->current_PID, measure->real_current, pid_ref);
         }
 
         if (motor_setting->feedback_reverse_flag == FEEDBACK_DIRECTION_REVERSE)
             pid_ref *= -1;
 
+        //功率限制
+        if(motor_setting->power_limit_flag == POWER_LIMIT_ON)
+        {
+            float I_cmd = pid_ref;
+            float w = measure->speed_aps /6 ;//aps to rpm
+            motor_controller->motor_power_predict = motor_power_K[0] * I_cmd * w + motor_power_K[1]*w*w  + motor_power_K[2]*I_cmd*I_cmd;
+            //这里K应该使用所有底盘电机一起计算 (在底盘任务中)
+            //float K = motor_controller->motor_power_max / motor_controller->motor_power_predict;
+            float K = motor_controller->motor_power_scale;
+            if(K>=1 || motor_controller->motor_power_predict < 0)//未超过最大功率 或做负功的电机 不做限制
+            {
+
+            }
+            else if(K<1)
+            {
+                float P_cmd = K * motor_controller->motor_power_predict; //对输出功率进行缩放
+
+                float a = motor_power_K[2] ;
+                float b = motor_power_K[0] * w;
+                float c = motor_power_K[1] * w * w - P_cmd;
+
+                if(pid_ref < 0)
+                    pid_ref = (-b - sqrtf(b*b-4*a*c))/(2*a);
+                else
+                    pid_ref = (-b + sqrtf(b*b-4*a*c))/(2*a);
+            }
+            //if( motor_controller->motor_power_predict < )
+        }
+
+        // 获取最终输出  此处注意set不要超过int16_t能表达的最大数 +-32767
+
+        pid_ref = float_constrain(pid_ref,-16384,16384);
         // 获取最终输出
-        set = (int16_t)pid_ref;
+        set = (int16_t) pid_ref;
 
         // 分组填入发送数据
         group = motor->sender_group;
         num = motor->message_num;
-        sender_assignment[group].tx_buff[2 * num] = (uint8_t)(set >> 8);         // 低八位
-        sender_assignment[group].tx_buff[2 * num + 1] = (uint8_t)(set & 0x00ff); // 高八位
+        sender_assignment[group].tx_buff[2 * num] = (uint8_t) (set >> 8);         // 低八位
+        sender_assignment[group].tx_buff[2 * num + 1] = (uint8_t) (set & 0x00ff); // 高八位
 
         // 若该电机处于停止状态,直接将buff置零
         if (motor->stop_flag == MOTOR_STOP)
@@ -304,10 +363,8 @@ void DJIMotorControl()
     }
 
     // 遍历flag,检查是否要发送这一帧报文
-    for (size_t i = 0; i < 6; ++i)
-    {
-        if (sender_enable_flag[i])
-        {
+    for (size_t i = 0; i < 10; ++i) {
+        if (sender_enable_flag[i]) {
             CANTransmit(&sender_assignment[i], 1);
         }
     }

modules/motor/DJImotor/dji_motor.h

@@ -58,6 +58,7 @@ typedef struct
     uint8_t message_num;
 
     Motor_Type_e motor_type;        // 电机类型
+    Motor_Control_Type_e motor_control_type;//电机控制方式
     Motor_Working_Type_e stop_flag; // 启停标志
 
     DaemonInstance* daemon;

modules/motor/motor_def.h

@@ -68,6 +68,12 @@ typedef enum
     MOTOR_ENALBED = 1,
 } Motor_Working_Type_e;
 
+typedef enum
+{
+    NO_POWER_LIMIT = 0,
+    POWER_LIMIT_ON = 1,
+} Power_Limit_Type_e;
+
 /* 电机控制设置,包括闭环类型,反转标志和反馈来源 */
 typedef struct
 {
@@ -78,9 +84,19 @@ typedef struct
     Feedback_Source_e angle_feedback_source;       // 角度反馈类型
     Feedback_Source_e speed_feedback_source;       // 速度反馈类型
     Feedfoward_Type_e feedforward_flag;            // 前馈标志
+    Power_Limit_Type_e power_limit_flag;            //功率限制标志
 
 } Motor_Control_Setting_s;
 
+/* 电机控制方式枚举 */
+typedef enum
+{
+    CONTROL_TYPE_NONE = 0,
+    CURRENT_CONTROL,
+    VOLTAGE_CONTROL,
+} Motor_Control_Type_e;
+
+
 /* 电机控制器,包括其他来源的反馈数据指针,3环控制器和电机的参考输入*/
 // 后续增加前馈数据指针
 typedef struct
@@ -95,6 +111,10 @@ typedef struct
     PIDInstance angle_PID;
 
     float pid_ref; // 将会作为每个环的输入和输出顺次通过串级闭环
+
+    float motor_power_max;     //每个电机分配的功率上限
+    float motor_power_predict; //根据模型预测的电机功率
+    float motor_power_scale;   //电机功率缩放比例
 } Motor_Controller_s;
 
 /* 电机类型枚举 */
@@ -133,6 +153,7 @@ typedef struct
     Motor_Control_Setting_s controller_setting_init_config;
     Motor_Type_e motor_type;
     CAN_Init_Config_s can_init_config;
+    Motor_Control_Type_e motor_control_type;
 } Motor_Init_Config_s;
 
 #endif // !MOTOR_DEF_H

modules/motor/servo_motor/servo_motor.h

@@ -14,7 +14,7 @@
 
 #include "main.h"
 #include "tim.h"
-#include <stdint-gcc.h>
+#include <stdint.h>
 
 #define SERVO_MOTOR_CNT 7
 

modules/referee/referee_protocol.h

@@ -157,37 +157,32 @@ typedef struct
 	uint8_t supply_projectile_num;
 } ext_supply_projectile_action_t;
 
-/* ID: 0X0201  Byte: 27    机器人状态数据 */
+/* ID: 0X0201  Byte: 13    V1.6.1 机器人性能体系数据 */
 typedef struct
 {
-	uint8_t robot_id;
-	uint8_t robot_level;
-	uint16_t remain_HP;
-	uint16_t max_HP;
-	uint16_t shooter_id1_17mm_cooling_rate;
-	uint16_t shooter_id1_17mm_cooling_limit;
-	uint16_t shooter_id1_17mm_speed_limit;
-	uint16_t shooter_id2_17mm_cooling_rate;
-	uint16_t shooter_id2_17mm_cooling_limit;
-	uint16_t shooter_id2_17mm_speed_limit;
-	uint16_t shooter_id1_42mm_cooling_rate;
-	uint16_t shooter_id1_42mm_cooling_limit;
-	uint16_t shooter_id1_42mm_speed_limit;
-	uint16_t chassis_power_limit;
-	uint8_t mains_power_gimbal_output : 1;
-	uint8_t mains_power_chassis_output : 1;
-	uint8_t mains_power_shooter_output : 1;
+    uint8_t robot_id;
+    uint8_t robot_level;
+    uint16_t current_HP;
+    uint16_t maximum_HP;
+    uint16_t shooter_barrel_cooling_value;
+    uint16_t shooter_barrel_heat_limit;
+    uint16_t chassis_power_limit;
+
+    uint8_t power_management_gimbal_output : 1;
+    uint8_t power_management_chassis_output : 1;
+    uint8_t power_management_shooter_output : 1;
 } ext_game_robot_state_t;
 
-/* ID: 0X0202  Byte: 14    实时功率热量数据 */
+/* ID: 0X0202  Byte: 14    v1.6.1 实时功率热量数据 */
 typedef struct
 {
-	uint16_t chassis_volt;
-	uint16_t chassis_current;
-	float chassis_power;		   // 瞬时功率
-	uint16_t chassis_power_buffer; // 60焦耳缓冲能量
-	uint16_t shooter_heat0;		   // 17mm
-	uint16_t shooter_heat1;
+    uint16_t chassis_voltage;
+    uint16_t chassis_current;
+    float chassis_power;
+    uint16_t buffer_energy;
+    uint16_t shooter_17mm_1_barrel_heat;
+    uint16_t shooter_17mm_2_barrel_heat;
+    uint16_t shooter_42mm_barrel_heat;
 } ext_power_heat_data_t;
 
 /* ID: 0x0203  Byte: 16    机器人位置数据 */

modules/referee/referee_task.c

@@ -17,6 +17,7 @@
 
 static Referee_Interactive_info_t *Interactive_data; // UI绘制需要的机器人状态数据
 static referee_info_t *referee_recv_info;            // 接收到的裁判系统数据
+Referee_Interactive_info_t ui_data;
 uint8_t UI_Seq;                                      // 包序号，供整个referee文件使用
 // @todo 不应该使用全局变量
 
@@ -28,7 +29,7 @@ uint8_t UI_Seq;                                      // 包序号，供整个ref
  */
 static void DeterminRobotID()
 {
-    // id小于7是红色,大于7是蓝色,0为红色，1为蓝色   #define Robot_Red 0    #define Robot_Blue 1
+    // id小于7是红色,大于7是蓝色   #define Robot_Red 0    #define Robot_Blue 1
     referee_recv_info->referee_id.Robot_Color = referee_recv_info->GameRobotState.robot_id > 7 ? Robot_Blue : Robot_Red;
     referee_recv_info->referee_id.Robot_ID = referee_recv_info->GameRobotState.robot_id;
     referee_recv_info->referee_id.Cilent_ID = 0x0100 + referee_recv_info->referee_id.Robot_ID; // 计算客户端ID
@@ -49,14 +50,14 @@ referee_info_t *UITaskInit(UART_HandleTypeDef *referee_usart_handle, Referee_Int
 
 void UITask()
 {
-    RobotModeTest(Interactive_data); // 测试用函数，实现模式自动变化,用于检查该任务和裁判系统是否连接正常
+    //RobotModeTest(Interactive_data); // 测试用函数，实现模式自动变化,用于检查该任务和裁判系统是否连接正常
     MyUIRefresh(referee_recv_info, Interactive_data);
 }
 
 static Graph_Data_t UI_shoot_line[10]; // 射击准线
-static Graph_Data_t UI_Energy[3];      // 电容能量条
-static String_Data_t UI_State_sta[6];  // 机器人状态,静态只需画一次
-static String_Data_t UI_State_dyn[6];  // 机器人状态,动态先add才能change
+static Graph_Data_t UI_Energy[4];      // 电容能量条
+static String_Data_t UI_State_sta[7];  // 机器人状态,静态只需画一次
+static String_Data_t UI_State_dyn[7];  // 机器人状态,动态先add才能change
 static uint32_t shoot_line_location[10] = {540, 960, 490, 515, 565};
 
 void MyUIInit()
@@ -103,16 +104,25 @@ void MyUIInit()
     UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[4]);
 
     // 底盘功率显示，静态
-    UICharDraw(&UI_State_sta[5], "ss5", UI_Graph_ADD, 7, UI_Color_Green, 18, 2, 620, 230, "Power:");
+    UICharDraw(&UI_State_sta[5], "ss5", UI_Graph_ADD, 8, UI_Color_Green, 18, 2, 620, 230, "Power_MAX:");
     UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[5]);
+
+    //底盘功率上限
+//    UICharDraw(&UI_State_sta[6], "ss7", UI_Graph_ADD, 8, UI_Color_Pink, 18, 2, 620, 300, "Power_MAX:");
+//    UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[6]);
+
     // 能量条框
     UIRectangleDraw(&UI_Energy[0], "ss6", UI_Graph_ADD, 7, UI_Color_Green, 2, 720, 140, 1220, 180);
     UIGraphRefresh(&referee_recv_info->referee_id, 1, UI_Energy[0]);
 
     // 底盘功率显示,动态
-    UIFloatDraw(&UI_Energy[1], "sd5", UI_Graph_ADD, 8, UI_Color_Green, 18, 2, 2, 750, 230, 24000);
+    UIFloatDraw(&UI_Energy[1], "sd5", UI_Graph_ADD, 8, UI_Color_Green, 18, 2, 2, 850, 230, referee_recv_info->GameRobotState.chassis_power_limit*1000);
     // 能量条初始状态
     UILineDraw(&UI_Energy[2], "sd6", UI_Graph_ADD, 8, UI_Color_Pink, 30, 720, 160, 1020, 160);
+
+
+    //UIIntDraw(&UI_Energy[3],"sd7",UI_Graph_ADD,8,UI_Color_Pink,18,2,1000,300,referee_recv_info->GameRobotState.chassis_power_limit);
+    //UIFloatDraw(&UI_Energy[3], "sd7", UI_Graph_ADD, 8, UI_Color_Green, 18, 2, 2, 1000, 1000, referee_recv_info->GameRobotState.chassis_power_limit);
     UIGraphRefresh(&referee_recv_info->referee_id, 2, UI_Energy[1], UI_Energy[2]);
 }
 
@@ -246,8 +256,8 @@ static void MyUIRefresh(referee_info_t *referee_recv_info, Referee_Interactive_i
     // power
     if (_Interactive_data->Referee_Interactive_Flag.Power_flag == 1)
     {
-        UIFloatDraw(&UI_Energy[1], "sd5", UI_Graph_Change, 8, UI_Color_Green, 18, 2, 2, 750, 230, _Interactive_data->Chassis_Power_Data.chassis_power_mx * 1000);
-        UILineDraw(&UI_Energy[2], "sd6", UI_Graph_Change, 8, UI_Color_Pink, 30, 720, 160, (uint32_t)750 + _Interactive_data->Chassis_Power_Data.chassis_power_mx * 30, 160);
+        UIFloatDraw(&UI_Energy[1], "sd5", UI_Graph_Change, 8, UI_Color_Green, 18, 2, 2, 850, 230, _Interactive_data->Chassis_Power_Data.chassis_power_mx * 1000);
+        //UILineDraw(&UI_Energy[2], "sd6", UI_Graph_Change, 8, UI_Color_Pink, 30, 720, 160, (uint32_t)750 + _Interactive_data->Chassis_Power_Data.chassis_power_mx * 30, 160);
         UIGraphRefresh(&referee_recv_info->referee_id, 2, UI_Energy[1], UI_Energy[2]);
         _Interactive_data->Referee_Interactive_Flag.Power_flag = 0;
     }
@@ -291,9 +301,9 @@ static void UIChangeCheck(Referee_Interactive_info_t *_Interactive_data)
         _Interactive_data->lid_last_mode = _Interactive_data->lid_mode;
     }
 
-    if (_Interactive_data->Chassis_Power_Data.chassis_power_mx != _Interactive_data->Chassis_last_Power_Data.chassis_power_mx)
+    if (_Interactive_data->Chassis_Power_Data.chassis_power_mx != _Interactive_data->Chassis_last_Power_Data.last_power_mx)
     {
         _Interactive_data->Referee_Interactive_Flag.Power_flag = 1;
-        _Interactive_data->Chassis_last_Power_Data.chassis_power_mx = _Interactive_data->Chassis_Power_Data.chassis_power_mx;
+        _Interactive_data->Chassis_last_Power_Data.last_power_mx = _Interactive_data->Chassis_Power_Data.chassis_power_mx;
     }
 }

modules/referee/referee_task.h

@@ -8,6 +8,7 @@
  * @brief 初始化裁判系统交互任务(UI和多机通信)
  *
  */
+extern Referee_Interactive_info_t ui_data; // UI数据，将底盘中的数据传入此结构体的对应变量中，UI会自动检测是否变化，对应显示UI
 referee_info_t *UITaskInit(UART_HandleTypeDef *referee_usart_handle, Referee_Interactive_info_t *UI_data);
 
 /**

modules/remote/remote_control.c

@@ -74,6 +74,7 @@ static void sbus_to_rc(const uint8_t *sbus_buf)
         if (i == 4 || i == 5) // 4,5位为ctrl和shift,直接跳过
             continue;
         // 如果当前按键按下,上一次按键没有按下,且ctrl和shift组合键没有按下,则按键按下计数加1(检测到上升沿)
+
         if ((key_now & j) && !(key_last & j) && !(key_with_ctrl & j) && !(key_with_shift & j))
             rc_ctrl[TEMP].key_count[KEY_PRESS][i]++;
         // 当前ctrl组合键按下,上一次ctrl组合键没有按下,则ctrl组合键按下计数加1(检测到上升沿)

modules/vofa/vofa.c

@@ -6,6 +6,7 @@
  * @LastEditTime: 2022-12-05 14:15:53
  */
 #include "vofa.h"
+#include "usbd_cdc_if.h"
 
 /*VOFA浮点协议*/
 void vofa_justfloat_output(float *data, uint8_t num , UART_HandleTypeDef *huart )
@@ -30,4 +31,5 @@ void vofa_justfloat_output(float *data, uint8_t num , UART_HandleTypeDef *huart
     send_data[4 * num + 3] = 0x7f; //加上协议要求的4个尾巴
 
     HAL_UART_Transmit(huart, (uint8_t *)send_data, 4 * num + 4, 100);
+    //CDC_Transmit_FS((uint8_t *)send_data,4 * num + 4);
 }