From e25cbdf2a30ef0a02a92638e55d07d8366007214 Mon Sep 17 00:00:00 2001 From: HshineO <1491134420@qq.com> Date: Thu, 12 Oct 2023 19:29:48 +0800 Subject: [PATCH] =?UTF-8?q?=E7=A2=B3=E6=9D=BF=E6=AD=A5=E5=85=B5=E4=BA=91?= =?UTF-8?q?=E5=8F=B0yaw=E8=BD=B4=E8=B0=83=E8=AF=95=E5=AE=8C=E6=88=90?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- application/chassis/chassis.c | 8 ++--- application/gimbal/gimbal.c | 25 +++++++++++----- application/robot.c | 4 +-- modules/imu/ins_task.c | 55 ++++++++++++++++++++++++++++------- modules/imu/ins_task.h | 8 +++++ 5 files changed, 75 insertions(+), 25 deletions(-) diff --git a/application/chassis/chassis.c b/application/chassis/chassis.c index c769987..29b5401 100644 --- a/application/chassis/chassis.c +++ b/application/chassis/chassis.c @@ -252,10 +252,10 @@ void ChassisTask() // 根据裁判系统的反馈数据和电容数据对输出限幅并设定闭环参考值 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); +// 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(); diff --git a/application/gimbal/gimbal.c b/application/gimbal/gimbal.c index e1a070f..5063698 100644 --- a/application/gimbal/gimbal.c +++ b/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 - .Ki = 200, // 200 + .Kp = 12000, // 50 + .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; diff --git a/application/robot.c b/application/robot.c index 25ce504..44bc884 100644 --- a/application/robot.c +++ b/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 diff --git a/modules/imu/ins_task.c b/modules/imu/ins_task.c index ec084b6..efdcced 100644 --- a/modules/imu/ins_task.c +++ b/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[Y] = BMI088.Accel[Y]; - INS.Accel[Z] = BMI088.Accel[Z]; - INS.Gyro[X] = BMI088.Gyro[X]; - INS.Gyro[Y] = BMI088.Gyro[Y]; - INS.Gyro[Z] = BMI088.Gyro[Z]; + INS.Accel[X] = -BMI088.Accel[2]; + INS.Accel[Y] = BMI088.Accel[1]; + INS.Accel[Z] = BMI088.Accel[0]; + INS.Gyro[X] = -BMI088.Gyro[2]; + INS.Gyro[Y] = BMI088.Gyro[1]; + 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); } diff --git a/modules/imu/ins_task.h b/modules/imu/ins_task.h index 4acac19..f42decf 100644 --- a/modules/imu/ins_task.h +++ b/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;