悟空四轴姿态控制

XiePeiQuan

悟空大四轴的姿态控制，用到了位置PID，有什么作用。

//姿态控制
void stateControl(const sensorData_t *sensorData, const state_t *state, setpoint_t *setpoint, control_t *control, const u32 tick)
{
        //位置PID
        if (RATE_DO_EXECUTE(POSITION_PID_RATE, tick))
        {
                //Z轴
                if (setpoint->mode.z == modeAbs)
                {
                        setpoint->velocity.z = pidUpdate(&pid[POSHOLD_Z], setpoint->position.z - state->position.z);
                }
                //XY轴
        }
       
        //速度PID
        if (RATE_DO_EXECUTE(VELOCITY_PID_RATE, tick))
        {
                //Z轴
                if (setpoint->mode.z != modeDisable)
                {
                        altThrustAdj = pidUpdate(&pid[VELOCITY_Z], setpoint->velocity.z - state->velocity.z);
                       
                        altHoldThrust = altThrustAdj + commanderGetALtHoldThrottle();
                }
                //XY轴
        }
       
        //角度PID（外环）
        if (RATE_DO_EXECUTE(ANGLE_PID_RATE, tick))
        {
                if (setpoint->mode.x == modeDisable || setpoint->mode.y == modeDisable)
                {
                        attitudeDesired.roll = setpoint->attitude.roll;
                        attitudeDesired.pitch = setpoint->attitude.pitch;
                }
                rateDesired.roll = pidUpdate(&pid[ANGLE_ROLL], attitudeDesired.roll - state->attitude.roll);
                rateDesired.pitch = pidUpdate(&pid[ANGLE_PITCH], attitudeDesired.pitch - state->attitude.pitch);
               
                if (setpoint->attitudeRate.yaw == 0)//yaw遥杆在中位
                {
                        if (attitudeDesired.yaw == 0)//偏航后yaw遥杆并回中时
                        {
                                attitudeDesired.yaw = state->attitude.yaw;//锁定当前航向角
                        }
                        float yawError = attitudeDesired.yaw - state->attitude.yaw;
                        if (yawError >= +180)
                                yawError -= 360;
                        if (yawError <= -180)
                                yawError += 360;
                        rateDesired.yaw = pidUpdate(&pid[ANGLE_YAW], yawError);
                }
                else//偏航时只进行角速度环控制
                {
                        attitudeDesired.yaw = 0;
                        rateDesired.yaw = setpoint->attitudeRate.yaw;
                }
        }
       
        //角速度PID（内环）
        if (RATE_DO_EXECUTE(RATE_PID_RATE, tick))
        {
                //速率模式则断开外环
                if (setpoint->mode.roll == modeVelocity || setpoint->mode.pitch == modeVelocity)
                {
                        rateDesired.roll = setpoint->attitudeRate.roll;
                        rateDesired.pitch = setpoint->attitudeRate.pitch;
                        pidReset(&pid[ANGLE_ROLL]);
                        pidReset(&pid[ANGLE_PITCH]);
                }
     //如果油门值小于MINCHECK时即怠速状态，清除PID积分，防止积分累计导致电机转速不一致
                if(rcCommand[THROTTLE] <= RC_COMMANDER_MINCHECK)
                {
                        pidResetIntegral(&pid[RATE_ROLL]);
                        pidResetIntegral(&pid[RATE_PITCH]);
                        pidResetIntegral(&pid[RATE_YAW]);
                        stateControlResetYawHolding();//复位航向角锁定
                }
               
                control->roll = pidUpdate(&pid[RATE_ROLL], rateDesired.roll - sensorData->gyro.x);
                control->pitch = pidUpdate(&pid[RATE_PITCH], rateDesired.pitch - sensorData->gyro.y);
                control->yaw = pidUpdate(&pid[RATE_YAW], rateDesired.yaw - sensorData->gyro.z);
        }
       
        //油门
        if (setpoint->mode.z == modeDisable)
        {
                control->thrust = setpoint->thrust;
        }
        else
        {
                control->thrust = altHoldThrust;
        }
}

二九结狐六体

多谢楼主分享啊！