TuxMonkey/half-steering-basic-framework
1
0
Fork 0
mirror of https://gitee.com/dlmu-cone/half-steering-basic-framework synced 2026-10-23 17:42:20 +08:00
Code Issues Packages Projects Releases Wiki Activity

零点感觉还可以...

Browse Source
This commit is contained in:
fcl
2026-03-21 18:44:25 +08:00
committed by guoxy06
parent 277b3a11c0
commit df0e1f4b1f
5 changed files with 64 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
application/gimbal/gimbal.c
View File

@@ -14,7 +14,9 @@ static Publisher_t *gimbal_pub; // 云台应用消息发布者
static Subscriber_t *gimbal_sub; // cmd控制消息订阅者
static Gimbal_Upload_Data_s gimbal_feedback_data; // 回传给cmd的云台状态信息
static Gimbal_Ctrl_Cmd_s gimbal_cmd_recv; // 来自cmd的控制信息
static float last_yaw_angle = 0.0f; // 保护模式前的yaw角度
// static float last_pitch_angle = 0.0f; // 保护模式前的pitch角度
uint8_t in_protection_mode = 0; // 是否在保护模式
void GimbalInit()
{
gimba_IMU_data = INS_Init(); // IMU先初始化,获取姿态数据指针赋给yaw电机的其他数据来源
@@ -117,11 +119,25 @@ void GimbalTask()
{
// 停止
case GIMBAL_ZERO_FORCE:
// 进入保护模式时记录当前角度
if (!in_protection_mode) {
last_yaw_angle = gimba_IMU_data->YawTotalAngle;
// last_pitch_angle = gimba_IMU_data->Pitch;
in_protection_mode = 1;
}
DJIMotorStop(yaw_motor);
DJIMotorStop(pitch_motor);
break;
// 使用陀螺仪的反馈,底盘根据yaw电机的offset跟随云台或视觉模式采用
case GIMBAL_GYRO_MODE: // 后续只保留此模式
// 从保护模式退出时
if (in_protection_mode) {
// 计算零飘导致的角度误差
float angle_error = gimba_IMU_data->YawTotalAngle - last_yaw_angle;
// 补偿目标角度，避免突然旋转
gimbal_cmd_recv.yaw -= angle_error;
in_protection_mode = 0;
}
DJIMotorEnable(yaw_motor);
DJIMotorEnable(pitch_motor);
DJIMotorChangeFeed(yaw_motor, ANGLE_LOOP, OTHER_FEED);