碳板步兵云台yaw轴调试完成

application/chassis/chassis.c

@@ -252,10 +252,10 @@ void ChassisTask()
 
     // 根据裁判系统的反馈数据和电容数据对输出限幅并设定闭环参考值
     LimitChassisOutput();
-    float vofa_send_data[2];
+//    float vofa_send_data[2];
-    vofa_send_data[0]=motor_lb->motor_controller.speed_PID.Ref;
+//    vofa_send_data[0]=motor_lb->motor_controller.speed_PID.Ref;
-    vofa_send_data[1]=motor_lb->motor_controller.speed_PID.Measure;
+//    vofa_send_data[1]=motor_lb->motor_controller.speed_PID.Measure;
-    vofa_justfloat_output(vofa_send_data,8,&huart1);
+//    vofa_justfloat_output(vofa_send_data,8,&huart1);
 
     // 根据电机的反馈速度和IMU(如果有)计算真实速度
     EstimateSpeed();

application/gimbal/gimbal.c

@@ -6,6 +6,7 @@
 #include "general_def.h"
 
 #include "bmi088.h"
+#include "vofa.h"
 
 static attitude_t *gimba_IMU_data; // 云台IMU数据
 static DJIMotorInstance *yaw_motor, *pitch_motor;
@@ -26,7 +27,7 @@ void GimbalInit()
         },
         .controller_param_init_config = {
             .angle_PID = {
-                .Kp = 8, // 8
+                .Kp = 0.5, // 8
                 .Ki = 0,
                 .Kd = 0,
                 .DeadBand = 0.1,
@@ -36,8 +37,8 @@ void GimbalInit()
                 .MaxOut = 500,
             },
             .speed_PID = {
-                .Kp = 50,  // 50
+                .Kp = 12000,  // 50
-                .Ki = 200, // 200
+                .Ki = 3000, // 200
                 .Kd = 0,
                 .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
                 .IntegralLimit = 3000,
@@ -51,7 +52,7 @@ void GimbalInit()
             .angle_feedback_source = OTHER_FEED,
             .speed_feedback_source = OTHER_FEED,
             .outer_loop_type = ANGLE_LOOP,
-            .close_loop_type = ANGLE_LOOP | SPEED_LOOP,
+            .close_loop_type = SPEED_LOOP | ANGLE_LOOP,
             .motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
         },
         .motor_type = GM6020};
@@ -59,7 +60,7 @@ void GimbalInit()
     Motor_Init_Config_s pitch_config = {
         .can_init_config = {
             .can_handle = &hcan2,
-            .tx_id = 2,
+            .tx_id = 4,
         },
         .controller_param_init_config = {
             .angle_PID = {
@@ -93,7 +94,7 @@ void GimbalInit()
     };
     // 电机对total_angle闭环,上电时为零,会保持静止,收到遥控器数据再动
     yaw_motor = DJIMotorInit(&yaw_config);
-    pitch_motor = DJIMotorInit(&pitch_config);
+//    pitch_motor = DJIMotorInit(&pitch_config);
 
     gimbal_pub = PubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s));
     gimbal_sub = SubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s));
@@ -124,7 +125,7 @@ void GimbalTask()
         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);
+//        DJIMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch);
         break;
     // 云台自由模式,使用编码器反馈,底盘和云台分离,仅云台旋转,一般用于调整云台姿态(英雄吊射等)/能量机关
     case GIMBAL_FREE_MODE: // 后续删除,或加入云台追地盘的跟随模式(响应速度更快)
@@ -135,7 +136,7 @@ void GimbalTask()
         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);
+//        DJIMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch);
         break;
     default:
         break;
@@ -145,6 +146,14 @@ void GimbalTask()
     // 根据IMU姿态/pitch电机角度反馈计算出当前配重下的重力矩
     // ...
 
+        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;

application/robot.c

@@ -32,7 +32,7 @@ void RobotInit()
 #if defined(ONE_BOARD) || defined(GIMBAL_BOARD)
     RobotCMDInit();
     //暂时注释云台和发射任务  调试底盘
-//    GimbalInit();
+    GimbalInit();
 //    ShootInit();
 #endif
 
@@ -51,7 +51,7 @@ void RobotTask()
 #if defined(ONE_BOARD) || defined(GIMBAL_BOARD)
     RobotCMDTask();
     //暂时注释云台和发射任务  调试底盘
-//    GimbalTask();
+    GimbalTask();
 //    ShootTask();
 #endif
 

modules/imu/ins_task.c

@@ -20,13 +20,19 @@
 #include "general_def.h"
 #include "master_process.h"
 
+#include "vofa.h"
+
 static INS_t INS;
 static IMU_Param_t IMU_Param;
 static PIDInstance TempCtrl = {0};
 
-const float xb[3] = {1, 0, 0};
+//const float xb[3] = {1, 0, 0};
+//const float yb[3] = {0, 1, 0};
+//const float zb[3] = {0, 0, 1};
+
+const float xb[3] = {0, 0, 1};
 const float yb[3] = {0, 1, 0};
-const float zb[3] = {0, 0, 1};
+const float zb[3] = {-1, 0, 0};
 
 // 用于获取两次采样之间的时间间隔
 static uint32_t INS_DWT_Count = 0;
@@ -60,9 +66,9 @@ static void InitQuaternion(float *init_q4)
     for (uint8_t i = 0; i < 100; ++i)
     {
         BMI088_Read(&BMI088);
-        acc_init[X] += BMI088.Accel[X];
+        acc_init[X] += -BMI088.Accel[2];
         acc_init[Y] += BMI088.Accel[Y];
-        acc_init[Z] += BMI088.Accel[Z];
+        acc_init[Z] += BMI088.Accel[0];
         DWT_Delay(0.001);
     }
     for (uint8_t i = 0; i < 3; ++i)
@@ -72,6 +78,9 @@ static void InitQuaternion(float *init_q4)
     float angle = acosf(Dot3d(acc_init, gravity_norm));
     Cross3d(acc_init, gravity_norm, axis_rot);
     Norm3d(axis_rot);
+
+    //q = cos (a/2) + i(x * sin(a/2)) + j(y * sin(a/2)) + k(z * sin(a/2))
+    //其中a表示旋转角度，(x,y,z)表示旋转轴  计算由实际重力加速度方向(0,0,1)到当前加速度方向的旋转
     init_q4[0] = cosf(angle / 2.0f);
     for (uint8_t i = 0; i < 2; ++i)
         init_q4[i + 1] = axis_rot[i] * sinf(angle / 2.0f); // 轴角公式,第三轴为0(没有z轴分量)
@@ -97,7 +106,7 @@ attitude_t *INS_Init(void)
     IMU_Param.flag = 1;
 
     float init_quaternion[4] = {0};
-    InitQuaternion(init_quaternion);
+    InitQuaternion(init_quaternion);//计算由实际重力加速度方向(0,0,1)到当前加速度方向的旋转
     IMU_QuaternionEKF_Init(init_quaternion, 10, 0.001, 1000000, 1, 0);
     // imu heat init
     PID_Init_Config_s config = {.MaxOut = 2000,
@@ -129,12 +138,12 @@ void INS_Task(void)
     {
         BMI088_Read(&BMI088);
 
-        INS.Accel[X] = BMI088.Accel[X];
+        INS.Accel[X] = -BMI088.Accel[2];
-        INS.Accel[Y] = BMI088.Accel[Y];
+        INS.Accel[Y] = BMI088.Accel[1];
-        INS.Accel[Z] = BMI088.Accel[Z];
+        INS.Accel[Z] = BMI088.Accel[0];
-        INS.Gyro[X] = BMI088.Gyro[X];
+        INS.Gyro[X] = -BMI088.Gyro[2];
-        INS.Gyro[Y] = BMI088.Gyro[Y];
+        INS.Gyro[Y] = BMI088.Gyro[1];
-        INS.Gyro[Z] = BMI088.Gyro[Z];
+        INS.Gyro[Z] = BMI088.Gyro[0];
 
         // demo function,用于修正安装误差,可以不管,本demo暂时没用
         IMU_Param_Correction(&IMU_Param, INS.Gyro, INS.Accel);
@@ -162,11 +171,35 @@ void INS_Task(void)
         }
         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;
+
+//        // get Yaw total, yaw数据可能会超过360,处理一下方便其他功能使用(如小陀螺)
+//        if (INS.Yaw - INS.YawAngleLast > 180.0f)
+//        {
+//            INS.YawRoundCount--;
+//        }
+//        else if (INS.Yaw - INS.YawAngleLast < -180.0f)
+//        {
+//            INS.YawRoundCount++;
+//        }
+//        INS.YawTotalAngle = 360.0f * INS.YawRoundCount + INS.Yaw;
         INS.YawTotalAngle = QEKF_INS.YawTotalAngle;
 
+//        float vofa_send_data[9];
+//        vofa_send_data[0]=INS.Yaw;
+//        vofa_send_data[1]=INS.Pitch;
+//        vofa_send_data[2]=INS.Roll;
+//        vofa_send_data[3]=INS.Gyro[X];
+//        vofa_send_data[4]=INS.Gyro[Y];
+//        vofa_send_data[5]=INS.Gyro[Z];
+//        vofa_send_data[6]=BMI088.Accel[X];
+//        vofa_send_data[7]=BMI088.Accel[Y];
+//        vofa_send_data[8]=BMI088.Accel[Z];
+//        vofa_justfloat_output(vofa_send_data,36,&huart1);
+
         VisionSetAltitude(INS.Yaw, INS.Pitch, INS.Roll);
     }
 

modules/imu/ins_task.h

@@ -23,6 +23,10 @@
 #define Y 1
 #define Z 2
 
+//#define X 2
+//#define Y 1
+//#define Z 0
+
 #define INS_TASK_PERIOD 1
 
 typedef struct
@@ -34,6 +38,7 @@ typedef struct
     float Pitch;
     float Yaw;
     float YawTotalAngle;
+
 } attitude_t; // 最终解算得到的角度,以及yaw转动的总角度(方便多圈控制)
 
 typedef struct
@@ -63,6 +68,9 @@ typedef struct
     float Yaw;
     float YawTotalAngle;
 
+    float YawAngleLast;
+    float YawRoundCount;
+
     uint8_t init;
 } INS_t;