Hey guys, im trying to implement quadcopter mechanics and PID support through Unreal Engine 5. All of my code is written in c++. Im fairly new to control theory and implementation of controllers like this. The following code contains two controllers in its function block, one is for position, and the other is attitude. In between the controllers im calculating my desired angle and using my desired acceleration given by the position PID. For my derivative im just using the velocity error and angular velocity error for my attitude control.
My position controller works perfectly for hovering and altitude control. If i have my setpoint set to 0,0,1000cm then it will hover there just how i want it to. But when testing out just my pitch and I do a setpoint of 1000,0,1000, the drone moves in the positive x direction as desired, but doesnt correct itself once its getting to its setpoint. It goes way past my setpoint and the error is negative at this point which I would assume cause some change in my motor speeds which the PID controller would manipulate, but nothing happens. My drone just goes in that direction indefinitely until it crashes. I have 18 gains total, 9 for my position and 9 for my attitude. Theres 3 gains for each P, I, and D, each corresponding to x y z axis. The following code is just my PID controller block and also where its referenced in my main quadcopter cpp file. If you want my header files then please let me know.
FPIDResult UPIDController::UpdatePID(const FPIDConfig& Config, FPIDState& State, FVector PositionError, FVector VelocityError, FRotator DroneOrientation, FVector CurrentAngVel, FVector DroneInertiaTensor, float Mass, float Grav, float DeltaTime)
{
FVector DroneOrientationVector = FVector(DroneOrientation.Roll, DroneOrientation.Pitch, DroneOrientation.Yaw);
UE_LOG(LogTemp, Log, TEXT(" Drone Orientation Vector: %f, %f, %f"), DroneOrientationVector.X, DroneOrientationVector.Y, DroneOrientationVector.Z);
UE_LOG(LogTemp, Log, TEXT(" Drone Orientation Degrees not Vector: %f, %f, %f"), DroneOrientation.Roll, DroneOrientation.Pitch, DroneOrientation.Yaw);
const float CT = 0.000010546f;
float gravity = 9.8f;
float length = 0.25f;
float b_drag = 1e-9f;
float weight = Mass * gravity;
// Position Controller
FVector PTerm = Config.ProportionalGain * PositionError; // k1*e
UE_LOG(LogTemp, Log, TEXT(" Velocity Error: %f, %f, %f"), VelocityError.X, VelocityError.Y, VelocityError.Z);
State.ControllerIntegralTerm += DeltaTime * PositionError;
FVector ITerm = Config.IntegralGain * State.ControllerIntegralTerm; // k3*int(e)
FVector DTerm = Config.DerivativeGain * VelocityError; // k2*VelocityError as the derivative term
FVector Des_Acc = PTerm + ITerm + DTerm; // Will return value in cm/s^2
Des_Acc.Z = (gravity + Des_Acc.Z) / (FMath::Cos(DroneOrientationVector.X) * FMath::Cos(DroneOrientationVector.Y));
float Des_Thrust = Des_Acc.Z * Mass;
UE_LOG(LogTemp, Log, TEXT("Des_Thrust: %f"), Des_Thrust);
float Mag_Acc = Des_Acc.Size();
if (Mag_Acc == 0)
{
Mag_Acc = 1;
}
// Calculate desired angles based on desired acceleration
FVector Ang_Des;
Ang_Des.Y = FMath::Asin(-Des_Acc.Y / Mag_Acc / FMath::Cos(DroneOrientationVector.Y));
Ang_Des.X = FMath::Asin(Des_Acc.X / Mag_Acc);
Ang_Des.Z = 0.0f;
////Will make each component of Desired Acceleration into a force, dividing by 100 to make it into m/s^2
//float U1 = (Mass * Des_Acc.X) / 100;
//float U2 = (Mass * Des_Acc.Y) / 100;
//float U3 = (Mass * Des_Acc.Z) / 100;
////Creating Maximized values so that we make sure it doesnt go negative in the sqrt later
//float U1_max = FMath::Max((Mass * Des_Acc.X) / 100, 0);
//float U2_max = FMath::Max((Mass * Des_Acc.Y) / 100, 0);
//float U3_max = FMath::Max((Mass * Des_Acc.Z) / 100, 0);
////float Des_Thrust = FMath::Sqrt(FMath::Pow(U1_max, 2) + FMath::Pow(U2_max, 2) + FMath::Pow(U3_max + weight, 2));
//float Des_Thrust = U3_max + weight;
//float WeightRPM = FMath::Sqrt(Des_Thrust / CT) * 7.5 / PI;
//UE_LOG(LogTemp, Log, TEXT("weight: %f"), weight);
//UE_LOG(LogTemp, Log, TEXT("Des_Thrust: %f"), Des_Thrust);
//
//// Finding Desired Attitude
//FVector Angle_Des;
//Angle_Des.X = FMath::Atan2(U1, U3);
//Angle_Des.Y = FMath::Atan2(-U2 * FMath::Cos(DroneOrientationVector.X), U3);
//Angle_Des.Z = 0.0f;
UE_LOG(LogTemp, Log, TEXT("Angle Des: %f %f %f"), Ang_Des.X, Ang_Des.Y, Ang_Des.Z);
//Making sure the angle doesnt surpass ~15 degrees
const float MaxAngleDegrees = 15.0f;
//Ang_Des.X = FMath::ClampAngle(Ang_Des.X, -MaxAngleDegrees, MaxAngleDegrees);
//Ang_Des.Y = FMath::ClampAngle(Ang_Des.Y, -MaxAngleDegrees, MaxAngleDegrees);
//Ang_Des.Z = FMath::ClampAngle(Ang_Des.Z, -MaxAngleDegrees, MaxAngleDegrees);
//UE_LOG(LogTemp, Log, TEXT("Clamped Angle Des: %s"), *Angle_Des.ToString());
float Mag_Angle_Des = Ang_Des.Size();
if (Mag_Angle_Des > MaxAngleDegrees)
{
Ang_Des = (Ang_Des / Mag_Angle_Des) * MaxAngleDegrees;
}
const float RollInertia = 0.0000811858f; // Ixx in kg*cm^2
const float PitchInertia = 0.0000811858f; // Iyy in kg*cm^2
const float YawInertia = 0.0000612233f; // Izz in kg*cm^2
FVector AngleError = Ang_Des - DroneOrientationVector;
// Angular Velocity Controller
FVector AnglePTerm = Config.AngleProportionalGain * AngleError;
FVector AngularVelError = -CurrentAngVel;
//EnqueueAndMaintainSize(AngleControllerBuffer, AngularVelError, State.AngleControllerCurQueueSize, Config.AngleBuffer_Size, State.Angle_ControllerSumBuffer);
State.AngleIntegralTerm += AngleError * DeltaTime;
FVector AngleITerm = Config.AngleIntegralGain * State.AngleIntegralTerm;
FVector AngleDTerm = Config.AngleDerivativeGain * AngularVelError;
FVector Ang_Acc = AnglePTerm + AngleITerm + AngleDTerm;
// Each component of the Angle PID controller is multiplied with its respective rotational moment
float RollTorque = Ang_Acc.Y * RollInertia;
float PitchTorque = Ang_Acc.X * PitchInertia;
float YawTorque = Ang_Acc.Z * YawInertia;
float Pitch = PitchTorque / (4* CT * length);
UE_LOG(LogTemp, Log, TEXT("Pitch: %f"), Pitch);
// Motors Set
Des_Thrust = FMath::Clamp(Des_Thrust, 0.0, 10467.0);
//UE_LOG(LogTemp, Log, TEXT("Weight RPM: %f"), WeightRPM);
FVector4 MotorRPMs;
MotorRPMs.X = Des_Thrust - Pitch;// - RollTorque / (4) - YawTorque / (4);
MotorRPMs.Y = Des_Thrust - Pitch;// + RollTorque / (4) + YawTorque / (4);
MotorRPMs.Z = Des_Thrust + Pitch;// + RollTorque / (4) - YawTorque / (4);
MotorRPMs.W = Des_Thrust + Pitch;// - RollTorque / (4) + YawTorque / (4);
MotorRPMs.X = FMath::Clamp(MotorRPMs.X, 0.0, 10467.0);
MotorRPMs.Y = FMath::Clamp(MotorRPMs.Y, 0.0, 10467.0);
MotorRPMs.Z = FMath::Clamp(MotorRPMs.Z, 0.0, 10467.0);
MotorRPMs.W = FMath::Clamp(MotorRPMs.W, 0.0, 10467.0);
// Create the result struct and return it
FPIDResult Result;
Result.MotorRPMs = MotorRPMs;
//Log_Messages(RollTorque, PitchTorque, YawTorque,VelocityError,U1,U2,U3,MotorRPMs, Angle_Des, AngleError, AngularVelError, CurrentAngVel, Des_Acc, AngleITerm, AnglePTerm, AngleDTerm, PTerm, ITerm, DTerm);
return Result;
}
Here is where I execute it
void ASentinelDrone::ExecutePIDControl(float DeltaTime)
{
FVector MyPosition = GetActorLocation(); //in cm
FVector MyVelocity = DroneBody->GetComponentVelocity(); //in cm/s^2
UE_LOG(LogTemp, Log, TEXT("Velocity : %f %f %f"), MyVelocity.X, MyVelocity.Y, MyVelocity.Z);
FRotator CurrentOrientation = GetActorRotation(); //In degrees
FRotator CurrentOrientationRadians; //Converting into radians
CurrentOrientationRadians.Roll = FMath::DegreesToRadians(CurrentOrientation.Roll);
CurrentOrientationRadians.Pitch = FMath::DegreesToRadians(CurrentOrientation.Pitch);
CurrentOrientationRadians.Yaw = FMath::DegreesToRadians(CurrentOrientation.Yaw);
GEngine->AddOnScreenDebugMessage(-1, 5, FColor::Red, FString::Printf(TEXT("Rotation: %f %f %f"), CurrentOrientationRadians.Roll, CurrentOrientationRadians.Pitch, CurrentOrientationRadians.Yaw));
UE_LOG(LogTemp, Log, TEXT("Rotation : %f %f %f"), CurrentOrientationRadians.Roll, CurrentOrientationRadians.Pitch, CurrentOrientationRadians.Yaw);
FVector CurrentAngularVelocity = DroneBody->GetPhysicsAngularVelocityInRadians(); // rad/s^2
FVector InertiaTensor = DroneBody->GetInertiaTensor(); // kg/cm^2
float DroneMass = DroneBody->GetMass();
float GravityZ = GetWorld()->GetGravityZ();
FVector PositionError = Pos_Setpoint - MyPosition;
FVector VelocityError = -MyVelocity;
UE_LOG(LogTemp, Log, TEXT("Inertia Setpoint: %f %f %f"), InertiaTensor.X, InertiaTensor.Y, InertiaTensor.Z);
//Change Begins now
FPIDResult PIDResult = PIDController->UpdatePID(PIDConfig, PIDState, PositionError, VelocityError, CurrentOrientationRadians, CurrentAngularVelocity, InertiaTensor,DroneMass,GravityZ,DeltaTime);
RPMs[0] = PIDResult.MotorRPMs.X;
RPMs[1] = PIDResult.MotorRPMs.Y;
RPMs[2] = PIDResult.MotorRPMs.Z;
RPMs[3] = PIDResult.MotorRPMs.W;
CalculateAllAngularVelocities();
CalculateAllThrusts();
ApplyAllThrusts();
//Log data with the correct velocity values and PID values
LogDataToCSV(this->TotalElapsedTime, Pos_Setpoint, MyPosition, PositionError,VelocityError, PIDResult.MotorRPMs,DroneMass);
LogMessages(this->TotalElapsedTime, GravityZ, this->Thrusts, Pos_Setpoint, MyPosition, PositionError, PIDResult.MotorRPMs);
}
Go to 2:01:00 for the PD block diagram im kinda copying
