1/11/2026 1 PM - Decided on some parts

after doing some research i found out about the teensy 4.0 and how at fast it is i also found about about drhem flight so i can use that to he
lp me learn

1/11/2026 5 PM - FINISHED PCB

I based my pcb off Nicholas Rehm's flight controller drhemFlight VTOl i did however make it easier to add more sensors and tried to make it in as small a package as possible one thing that really annoys me was the empty space on the board so i just added some silkscreen. I also spent a while making my own pcb footprints for the pin headers as i couldnt seem to find any in the right size.

1/11/2026 9 PM - Made a render

i started to make a render of the drone i found all the parts for the frame and assmebled them I also added the moters props and i also added my pcb to see where it would fit. It will be attached with double sided tape.

1/12/2026 - did code

#include
#include
#include
#include

// Pin definitions (adjust as needed)
const int motorPin1 = 5; // Front-left motor (ESC)
const int motorPin2 = 6; // Front-right motor (ESC)
const int motorPin3 = 9; // Rear-right motor (ESC)
const int motorPin4 = 10; // Rear-left motor (ESC)

const int throttlePin = 2; // RC PWM input: Throttle channel
const int rollPin = 3; // RC PWM input: Roll channel
const int pitchPin = 4; // RC PWM input: Pitch channel
const int yawPin = 7; // RC PWM input: Yaw channel

// RC input pulse widths (in microseconds)
volatile unsigned long pwmThrottle = 1000;
volatile unsigned long pwmRoll = 1500;
volatile unsigned long pwmPitch = 1500;
volatile unsigned long pwmYaw = 1500;
volatile unsigned long startThrottle = 0, startRoll = 0, startPitch = 0, startYaw = 0;

// Track last RC signal time for failsafe
volatile unsigned long lastSignalTime = 0;

// IMU and control variables
MPU6050 mpu;
float pitchAngle = 0.0, rollAngle = 0.0;
float pitchErr=0, pitchPrevErr=0, pitchI=0;
float rollErr=0, rollPrevErr=0, rollI=0;
float yawErr=0, yawPrevErr=0, yawI=0;

// PID gain (tune these)
float Kp = 1.5, Ki = 0.0, Kd = 0.5; // Roll/Pitch PID
float Kpyaw = 1.0, Kiyaw = 0.0, Kd_yaw = 0.3; // Yaw PID

Servo esc1, esc2, esc3, esc4; // ESC signal outputs

// Interrupts to measure PWM pulse widths from RC receiver
void throttleISR() {
if (digitalRead(throttlePin) == HIGH) {
startThrottle = micros();
} else {
pwmThrottle = micros() - startThrottle;
lastSignalTime = millis();
}
}
void rollISR() {
if (digitalRead(rollPin) == HIGH) {
startRoll = micros();
} else {
pwmRoll = micros() - startRoll;
lastSignalTime = millis();
}
}
void pitchISR() {
if (digitalRead(pitchPin) == HIGH) {
startPitch = micros();
} else {
pwmPitch = micros() - startPitch;
lastSignalTime = millis();
}
}
void yawISR() {
if (digitalRead(yawPin) == HIGH) {
startYaw = micros();
} else {
pwmYaw = micros() - startYaw;
lastSignalTime = millis();
}
}

void setup() {
Serial.begin(9600);
Wire.begin();
mpu.initialize();
if (!mpu.testConnection()) {
// Sensor failed to initialize – stop here
while (1) { /* could flash an LED here */ }
}

// Attach ESCs
esc1.attach(motorPin1);
esc2.attach(motorPin2);
esc3.attach(motorPin3);
esc4.attach(motorPin4);
// Initialize ESCs to minimum throttle
esc1.writeMicroseconds(1000);
esc2.writeMicroseconds(1000);
esc3.writeMicroseconds(1000);
esc4.writeMicroseconds(1000);
delay(2000); // Wait for ESCs to arm

// Setup RC input pins and interrupts
pinMode(throttlePin, INPUT);
pinMode(rollPin, INPUT);
pinMode(pitchPin, INPUT);
pinMode(yawPin, INPUT);
attachInterrupt(digitalPinToInterrupt(throttlePin), throttleISR, CHANGE);
attachInterrupt(digitalPinToInterrupt(rollPin), rollISR, CHANGE);
attachInterrupt(digitalPinToInterrupt(pitchPin), pitchISR, CHANGE);
attachInterrupt(digitalPinToInterrupt(yawPin), yawISR, CHANGE);
lastSignalTime = millis();
}

void loop() {
unsigned long now = millis();
// Failsafe: if no signal >1s, stop motors
if (now - lastSignalTime > 1000) {
esc1.writeMicroseconds(1000);
esc2.writeMicroseconds(1000);
esc3.writeMicroseconds(1000);
esc4.writeMicroseconds(1000);
return;
}

// Compute time delta (dt)
static unsigned long prevTime = micros();
unsigned long currTime = micros();
float dt = (currTime - prevTime) / 1000000.0;
prevTime = currTime;

// Read raw MPU6050 data
int16_t ax, ay, az, gx, gy, gz;
mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

// Convert to physical units
float accX = ax / 16384.0;
float accY = ay / 16384.0;
float accZ = az / 16384.0;
float gyroX = gx / 131.0; // deg/s (assuming default 250 dps range)
float gyroY = gy / 131.0;
float gyroZ = gz / 131.0;

// Complementary filter for roll/pitch angle estimation
float accPitch = atan2(accY, accZ) * 180.0 / PI;
float accRoll = atan2(-accX, accZ) * 180.0 / PI;
pitchAngle = 0.98(pitchAngle + gyroX * dt) + 0.02accPitch;
rollAngle = 0.98(rollAngle + gyroY * dt) + 0.02accRoll;
// (Yaw angle could be integrated from gyroZ if needed)

// Map RC inputs to desired setpoints
float throttle = constrain(pwmThrottle, 1000UL, 2000UL);
float targetPitch = (pwmPitch - 1500UL) / 500.0 * 30.0; // +/-30 degrees
float targetRoll = (pwmRoll - 1500UL) / 500.0 * 30.0;
float targetYawRate = (pwmYaw - 1500UL) / 500.0 * 90.0; // +/-90 deg/s

// PID errors
pitchErr = targetPitch - pitchAngle;
rollErr = targetRoll - rollAngle;
yawErr = targetYawRate - gyroZ;

// PID integrals
pitchI += pitchErr * dt;
rollI += rollErr * dt;
yawI += yawErr * dt;
// PID derivatives
float pitchD = (pitchErr - pitchPrevErr) / dt;
float rollD = (rollErr - rollPrevErr) / dt;
float yawD = (yawErr - yawPrevErr) / dt;
pitchPrevErr = pitchErr;
rollPrevErr = rollErr;
yawPrevErr = yawErr;

// Compute PID outputs
float pitchPID = KppitchErr + KipitchI + KdpitchD;
float rollPID = KprollErr + KirollI + KdrollD;
float yawPID = Kpyaw*yawErr + KiyawyawI + Kd_yawyawD;

// Motor mixing (X configuration)
int m1 = (int)throttle + pitchPID + rollPID - yawPID; // Front-left
int m2 = (int)throttle + pitchPID - rollPID + yawPID; // Front-right
int m3 = (int)throttle - pitchPID - rollPID - yawPID; // Rear-right
int m4 = (int)throttle - pitchPID + rollPID + yawPID; // Rear-left

// Constrain and send to ESCs
m1 = constrain(m1, 1000, 2000);
m2 = constrain(m2, 1000, 2000);
m3 = constrain(m3, 1000, 2000);
m4 = constrain(m4, 1000, 2000);
esc1.writeMicroseconds(m1);
esc2.writeMicroseconds(m2);
esc3.writeMicroseconds(m3);
esc4.writeMicroseconds(m4);

// (Optional: debug output over Serial, disabled in final build)
// Serial.print("Pitch: "); Serial.print(pitchAngle);
// Serial.print(" Roll: "); Serial.print(rollAngle);
// Serial.print(" m1: "); Serial.print(m1);
// Serial.print(" m2: "); Serial.print(m2);
// Serial.print(" m3: "); Serial.print(m3);
// Serial.print(" m4: "); Serial.print(m4);
// Serial.println();
}