C++ Quadcopter Flight Controller & Physics Simulator
A high-fidelity 6-DOF drone simulator and custom flight controller written in C++.

Project Overview This project implements a quadcopter physics engine and a PID flight controller without relying on external physics engines. The goal was to create a simplified simulation of a quadcopter by implementing rigid body dynamics, control allocations, and numerical integration methods.
This project offers the option of using Euler or Runge-Kutta 4th-order integration and Attitude Control (Thrust Vectoring), enabling stable flight even during violent maneuvers.

Key Features Custom Physics Engine: Derived 6-DOF rigid body dynamics using Newton-Euler equations. High-Stability Integration: Implemented Runge-Kutta 4 (RK4) numerical integration to eliminate "energy drift". Geometric Flight Control: Replaced standard Roll/Pitch PID loops with Thrust Vector Alignment (Cross-Product Error). Safety-Critical Motor Mixer: Features a custom allocation algorithm that prioritizes altitude ($F_z$) and scales down turning requests when motor saturation is detected. Data Analysis Pipeline: CSV logging pipeline with Python/Matplotlib visualization for 3D trajectory and velocity profiles.

Technical Implementation

1. The Physics Model (RK4 & Quaternions)The simulator does not use a black-box physics library. Instead, I implemented:

   Kinematics: Quaternion integration using the using first-order linear approximation .
   Dynamics: Gyroscopic precession modeling ($\omega \times I\omega$) to account for the resistance to axis changes at high RPM.
   Integration: Moved from First-Order Euler to Fourth-Order Runge-Kutta (RK4). This solved critical instability issues where the simulation would "explode" during fast spins.

2. Geometric Control Architecture
   Instead of asking "What is my Pitch Angle?", the controller asks "Where is my thrust vector pointing?"

   Outer Loop (Position): Calculates a target 3D acceleration vector in the World Frame.
   Inner Loop (Attitude): Uses the cross-product of the Current Thrust Vector and Target Thrust Vector ($v_{current} \times v_{target}$) to generate an error signal.
   Benefit: This allows the drone to recover safely even if inverted, a scenario where Euler-angle based controllers would fail due to singularities.

3. The "Anti-Flyaway" Mixer
   A common issue in drone control is "Saturation," where a turn request exceeds the motor's remaining headroom.

   The Algorithm: I implemented a Scaling Strategy. If a requested maneuver requires 110% motor power, the mixer calculates the maximum available headroom and linearly scales down the rotational command while preserving the base throttle (Altitude).
   Result: The drone turns slower than requested but maintains perfect altitude, preventing the "Fly-Away" bug where prioritizing rotation causes uncommanded climbs.

Simplification
While this simulator achieves high fidelity in kinematics and integration, several idealizations were made to keep the scope manageable. These differences would need to be addressed when porting this controller to physical hardware.

1. Actuator Dynamics & Power

   Instant Motors: The simulator models motors as ideal actuators that reach the desired force instantly. Real motors would have some sort of lag. Impact: The simulator tolerates much higher P-Gains ($K_p$) than real hardware, which would oscillate due to lag. Idealized Power Train: Thrust is modeled as linear to the command signal (0.0 - 1.0). In reality, thrust is proportional to RPM squared ($T \propto \omega^2$), and battery voltage sags under load. Impact: Vertical performance in the sim is perfectly consistent, ignoring the "sluggishness" of a depleted battery.

2. Physics & Aerodynamics

   Linear Drag Model: Drag is calculated as $F_{drag} = -C \cdot v$. Real aerodynamics follow the Quadratic Drag Equation ($F_d = \frac{1}{2} \rho v^2 C_d A$). Impact: The drone creates less drag at high speeds than a real vehicle. Rigid Point Mass: The drone is modeled as a perfectly rigid object. Real frames flex and vibrate, introducing high-frequency noise to the IMU (Gyro). Impact: Real drones require Notch Filters and Low-Pass Filters to prevent "D-Term Heating" (amplification of noise by the Derivative term). Ignored Aerodynamic Effects: The simulation does not model Blade Flapping (pitch-up moment during forward flight) or Ground Effect (increased lift near terrain).

3. Sensors & Timing

   Perfect State Estimation: The controller receives ground-truth position and orientation directly from the physics engine. A real flight controller requires an Extended Kalman Filter (EKF) to fuse noisy data from accelerometers, gyroscopes, and GPS. Perfect Timing: The physics loop runs at a mathematically precise dt. Embedded systems suffer from Loop Jitter (e.g., a 2ms loop taking 2.1ms) and protocol latency (PWM/DShot). Impact: The simulation is immune to aliasing and synchronization bugs that plague real embedded flight stacks.

Engineering Challenges & Solutions

1. Actuator Saturation ("Space Program" Bug)

   Challenge: Early mixer logic used a "boost" strategy to prevent negative motor commands, unintentionally adding total thrust during turns and causing uncommanded climbs.
   Solution: Engineered a Headroom-Based Scaling Mixer that linearly reduces turning authority when motors reach saturation, ensuring altitude control is always prioritized over rotation speed.

2. Geometric Singularities

   Challenge: Euler angle approximations (atan2) caused gimbal lock and control instability at tilt angles >30°.
   Solution: Migrated to Thrust Vector Control (TVC). The controller now calculates orientation error via the cross-product of the current and target thrust vectors ($v_{body} \times v_{target}$), enabling stable flight in any orientation.

3. Coordinate Frame Consistency

   Challenge: Mixing mathematical conventions (NED vs. ENU) led to inverted torque and control fighting.
   Solution: Standardized the entire simulation stack (Physics, Control, Telemetry) to the FLU (Forward-Left-Up) convention, ensuring consistent sign conventions for all vector operations.

4. Simulation Stability

   Challenge: PID noise caused false positives for waypoint arrival, and missed waypoints led to infinite simulation loops.
   Solution: Implemented a Continuous Dwell Timer (requiring 5s stability within tolerance range) for arrival confirmation and added a global timer to guarantee simulation termination.

Future Improvements

• Port the C++ controller logic to a more realistic simulator like Gazebo and ROS
• Add multiple sensors with noise
• Implement Extended Kalman Filter (EKF) for sensor fusion (Simulating noisy IMU data).