Merge branch 'master' of gitee.com:hnuyuelurm/basic_framework

2023-03-23 19:01:39 +08:00 · 2023-03-23 19:01:39 +08:00 · b85eff52ba
parent 59e21dc25d 57a5ec33b9
commit b85eff52ba
6 changed files with 75 additions and 47 deletions
--- a/HAL_N_Middlewares/Src/main.c
+++ b/HAL_N_Middlewares/Src/main.c
@ -83,7 +83,7 @@ int main(void)
 	/* MCU Configuration--------------------------------------------------------*/

 	/* Reset of all peripherals, Initializes the Flash interface and the Systick. */
-	HAL_Init();
+	   HAL_Init();

 	/* USER CODE BEGIN Init */

--- a/2
+++ b/2
@ -141,6 +141,7 @@ modules/can_comm/can_comm.c \
 modules/message_center/message_center.c \
 modules/daemon/daemon.c \
 modules/vofa/vofa.c \
+modules/ps_handle/ps_handle.c \
 application/gimbal/gimbal.c \
 application/chassis/chassis.c \
 application/shoot/shoot.c \
@ -262,6 +263,7 @@ C_INCLUDES =  \
 -Imodules/message_center \
 -Imodules/daemon \
 -Imodules/vofa \
+-Imodules/ps_handle \
 -Imodules

 # compile gcc flags
--- a/application/cmd/robot_cmd.c
+++ b/application/cmd/robot_cmd.c
@ -69,6 +69,7 @@ void RobotCMDInit()
    cmd_can_comm = CANCommInit(&comm_conf);
 #endif // GIMBAL_BOARD
    gimbal_cmd_send.pitch = 0;
+gimbal_cmd_send.pitch = 0;

    robot_state = ROBOT_READY; // 启动时机器人进入工作模式,后续加入所有应用初始化完成之后再进入
 }
@ -100,6 +101,17 @@ static void CalcOffsetAngle()
 #endif
 }

+static void OffsetAnglePidCalc()
+{
+    // float pid_measure,pid_ref;
+    // static PIDInstance AngleCal = {
+    //         .Kp = -1,
+    //         .Ki = 0,
+    //         .Kd = 0,
+    //         .MaxOut = 10000,
+    // };
+    // chassis_cmd_send.offset_angle = PIDCalculate(&AngleCal,chassis_cmd_send.offset_angle,0);  
+}
 /**
 * @brief 控制输入为遥控器(调试时)的模式和控制量设置
 *
@ -134,6 +146,7 @@ static void RemoteControlSet()
    // 底盘参数,目前没有加入小陀螺(调试似乎暂时没有必要),系数需要调整
    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.wz = 300;

    // 发射参数
    if (switch_is_up(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[上],弹舱打开
@ -181,6 +194,8 @@ static void EmergencyHandler()
        gimbal_cmd_send.gimbal_mode = GIMBAL_ZERO_FORCE;    // 云台模式改为零力矩
        chassis_cmd_send.chassis_mode = CHASSIS_ZERO_FORCE; // 底盘模式改为零力矩
        shoot_cmd_send.shoot_mode = SHOOT_OFF;              // 射击模式改为关闭
+        shoot_cmd_send.friction_mode = FRICTION_OFF;
+        shoot_cmd_send.load_mode = LOAD_STOP;
    }
    // 遥控器右侧开关为[上],恢复正常运行
    if (switch_is_up(rc_data[TEMP].rc.switch_right))
@ -218,7 +233,7 @@ void RobotCMDTask()
    vision_send_data.enemy_color = 0;
    vision_send_data.pitch = gimbal_fetch_data.gimbal_imu_data.Pitch;
    vision_send_data.yaw = gimbal_fetch_data.gimbal_imu_data.Yaw;
-    vision_send_data.roll = gimbal_fetch_data.gimbal_imu_data.Roll;
+    vision_send_data.roll = gimbal_fetch_data.gimbal_imu_data.Roll;;

    // 推送消息,双板通信,视觉通信等
    // 其他应用所需的控制数据在remotecontrolsetmode和mousekeysetmode中完成设置
--- a/application/gimbal/gimbal.c
+++ b/application/gimbal/gimbal.c
@ -6,9 +6,10 @@
 #include "ins_task.h"
 #include "message_center.h"
 #include "general_def.h"
+
 #include "bmi088.h"

-static attitude_t *gimba_IMU_data;   // 云台IMU数据
+static attitude_t *gimba_IMU_data;    // 云台IMU数据
 static DJIMotorInstance *yaw_motor;   // yaw电机
 static DJIMotorInstance *pitch_motor; // pitch电机

@ -17,45 +18,45 @@ static Subscriber_t *gimbal_sub;                  // cmd控制消息订阅者
 static Gimbal_Upload_Data_s gimbal_feedback_data; // 回传给cmd的云台状态信息
 static Gimbal_Ctrl_Cmd_s gimbal_cmd_recv;         // 来自cmd的控制信息

-BMI088Instance* imu;
+BMI088Instance *imu;
 void GimbalInit()
 {
    BMI088_Init_Config_s imu_config = {
-        .spi_acc_config={
-            .GPIOx=CS1_ACCEL_GPIO_Port,
-            .cs_pin=CS1_ACCEL_Pin,
-            .spi_handle=&hspi1,
+        .spi_acc_config = {
+            .GPIOx = CS1_ACCEL_GPIO_Port,
+            .cs_pin = CS1_ACCEL_Pin,
+            .spi_handle = &hspi1,
        },
-        .spi_gyro_config={
-            .GPIOx=CS1_GYRO_GPIO_Port,
-            .cs_pin=CS1_GYRO_Pin,
-            .spi_handle=&hspi1,
+        .spi_gyro_config = {
+            .GPIOx = CS1_GYRO_GPIO_Port,
+            .cs_pin = CS1_GYRO_Pin,
+            .spi_handle = &hspi1,
        },
-        .acc_int_config={
-            .exti_mode=EXTI_TRIGGER_FALLING,
-            .GPIO_Pin=INT_ACC_Pin,
-            .GPIOx=INT_ACC_GPIO_Port,
+        .acc_int_config = {
+            .exti_mode = EXTI_TRIGGER_FALLING,
+            .GPIO_Pin = INT_ACC_Pin,
+            .GPIOx = INT_ACC_GPIO_Port,
        },
-        .gyro_int_config={
-            .exti_mode=EXTI_TRIGGER_FALLING,
-            .GPIO_Pin=INT_GYRO_Pin,
-            .GPIOx=INT_GYRO_GPIO_Port,
+        .gyro_int_config = {
+            .exti_mode = EXTI_TRIGGER_FALLING,
+            .GPIO_Pin = INT_GYRO_Pin,
+            .GPIOx = INT_GYRO_GPIO_Port,
        },
-        .heat_pid_config={
-            .Kp=0.0f,
-            .Kd=0.0f,
-            .Ki=0.0f,
-            .MaxOut=0.0f,
-            .DeadBand=0.0f,
+        .heat_pid_config = {
+            .Kp = 0.0f,
+            .Kd = 0.0f,
+            .Ki = 0.0f,
+            .MaxOut = 0.0f,
+            .DeadBand = 0.0f,
        },
-        .heat_pwm_config={
-            .channel=TIM_CHANNEL_1,
-            .htim=&htim1,
+        .heat_pwm_config = {
+            .channel = TIM_CHANNEL_1,
+            .htim = &htim1,
        },
-        .cali_mode=BMI088_CALIBRATE_ONLINE_MODE,
-        .work_mode=BMI088_BLOCK_PERIODIC_MODE,
+        .cali_mode = BMI088_CALIBRATE_ONLINE_MODE,
+        .work_mode = BMI088_BLOCK_PERIODIC_MODE,
    };
-   // imu=BMI088Register(&imu_config);
+    // imu=BMI088Register(&imu_config);
    gimba_IMU_data = INS_Init(); // IMU先初始化,获取姿态数据指针赋给yaw电机的其他数据来源
    // YAW
    Motor_Init_Config_s yaw_config = {
@ -65,26 +66,26 @@ void GimbalInit()
        },
        .controller_param_init_config = {
            .angle_PID = {
-                .Kp = 8, //8
+                .Kp = 8, // 8
                .Ki = 0,
                .Kd = 0,
                .DeadBand = 0.1,
-                .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit |PID_Derivative_On_Measurement,
+                .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
                .IntegralLimit = 100,

                .MaxOut = 500,
            },
            .speed_PID = {
-                .Kp = 50,//40
+                .Kp = 50, // 40
                .Ki = 200,
                .Kd = 0,
-                .Improve = PID_Trapezoid_Intergral |PID_Integral_Limit |PID_Derivative_On_Measurement,
+                .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
                .IntegralLimit = 3000,
                .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_speed_feedback_ptr = &gimba_IMU_data->Gyro[2],
        },
        .controller_setting_init_config = {
            .angle_feedback_source = OTHER_FEED,
@ -102,24 +103,24 @@ void GimbalInit()
        },
        .controller_param_init_config = {
            .angle_PID = {
-                .Kp =10,//10
+                .Kp = 10, // 10
                .Ki = 0,
                .Kd = 0,
-                .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit |PID_Derivative_On_Measurement,
-                .IntegralLimit =100,
+                .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
+                .IntegralLimit = 100,
                .MaxOut = 500,
            },
            .speed_PID = {
-                .Kp=50,//50
-                .Ki =350,//350
-                .Kd =0,//0.1
-                .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit |PID_Derivative_On_Measurement,
-                .IntegralLimit =2500,
+                .Kp = 50,  // 50
+                .Ki = 350, // 350
+                .Kd = 0,   // 0.1
+                .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]),
+            .other_speed_feedback_ptr = (&gimba_IMU_data->Gyro[0]),
        },
        .controller_setting_init_config = {
            .angle_feedback_source = OTHER_FEED,
--- a/modules/algorithm/controller.h
+++ b/modules/algorithm/controller.h
@ -113,7 +113,6 @@ typedef struct // config parameter
    float CoefB;         // ITerm = Err*((A-abs(err)+B)/A)  when B<|err|<A+B
    float Output_LPF_RC; // RC = 1/omegac
    float Derivative_LPF_RC;
-
 } PID_Init_Config_s;

 /**
--- a/modules/master_machine/seasky_protocol.c
+++ b/modules/master_machine/seasky_protocol.c
@ -15,6 +15,11 @@
 #include "crc16.h"
 #include "memory.h"

+/*获取CRC8校验码*/
+uint8_t Get_CRC8_Check(uint8_t *pchMessage,uint16_t dwLength)
+{
+    return crc_8(pchMessage,dwLength);
+}
 /*检验CRC8数据段*/
 static uint8_t CRC8_Check_Sum(uint8_t *pchMessage, uint16_t dwLength)
 {
@ -25,6 +30,12 @@ static uint8_t CRC8_Check_Sum(uint8_t *pchMessage, uint16_t dwLength)
    return (ucExpected == pchMessage[dwLength - 1]);
 }

+/*获取CRC16校验码*/
+uint16_t Get_CRC16_Check(uint8_t *pchMessage,uint32_t dwLength)
+{
+    return crc_16(pchMessage,dwLength);
+}
+
 /*检验CRC16数据段*/
 static uint16_t CRC16_Check_Sum(uint8_t *pchMessage, uint32_t dwLength)
 {