Video Demonstration (Test Flight)

Pictures

、

Controller MATLAB Simulation Script

clear; clc; close all;

%Constants
g = 9.81;
m = 6.0;
W = m*g;

%Geometry
x_top  = 0.19;               % m
x_bot = 0.55;               % m
k_arm = 2*(x_top + x_bot);   % m

% Motor spec
T50  = (611/1000)*g;        % N in 50% throttle
T100 = (1651/1000)*g;       % N in 100% throttle

k1 = 4*T50 - T100;
k2 = 2*T100 - 4*T50;

dTdd = @(d) 2*k2*d + k1;    % N per throttle fraction

% Hover throttle estimate
delta0 = max(real(roots([k2 k1 -W/4])));      % positive root
dTdd0  = dTdd(delta0);                         % slope at hover

% Gain
k_tau = k_arm * dTdd0;                          % N*m per deltaThrottle

% Inertia
Ixx  = 0.50;     % kg*m^2 estimated
bphi = 0.05;     % N*m*s/rad 
tau_m = 0.05;    % s 

% Plant state-space 
% phi_dot = p
% p_dot   = (tau - bphi*p)/Ixx
% tau_dot = (-1/tau_m)*tau + (k_tau/tau_m)*u
%
% Output y = phi

A = [ 0         1           0;
      0   -bphi/Ixx     1/Ixx;
      0         0      -1/tau_m ];

B = [ 0;
      0;
      k_tau/tau_m ];

C = [ 1 0 0 ];
D = 0;

P = ss(A,B,C,D);

% PD controller 
% Use u = -(Kp*phi + Kd*p)
% 2nd order differential equation: Ixx*s^2 + (bphi + k_tau*Kd)*s + k_tau*Kp = 0
zeta = 0.4;   % modify if needs change currently 25% overshoot
wn   = 5.0;   % rad/s modify if needs change currently 2s settling time

Kp = (Ixx*wn^2)/k_tau;
Kd = (2*zeta*Ixx*wn - bphi)/k_tau;

fprintf('delta0 (hover) = %.3f\n', delta0);
fprintf('dT/d(delta)@hover = %.2f N\n', dTdd0);
fprintf('k_tau = %.2f N*m \n', k_tau);
fprintf('PD gains for regulation: Kp = %.6f, Kd = %.6f\n', Kp, Kd);

% Closed-loop
K = [Kp Kd 0];
Acl = A - B*K;

% Closed-loop system 
Pcl = ss(Acl, zeros(3,1), eye(3), zeros(3,1));   % outputs = states

% Initial condition
x0 = [1; 0; 0];   % phi=1 rad, p=0, tau=0
t  = linspace(0, 5, 2000);

x = initial(Pcl, x0, t);  % returns states because outputs = states
phi = x(:,1);
p   = x(:,2);
tau = x(:,3);

% Reconstruct control command u(t) = -(Kp*phi + Kd*p)
u = -(Kp*phi + Kd*p);

% Open loop response
P_ol = ss(A, zeros(3,1), eye(3), zeros(3,1));   % outputs = states, no input
x_ol = initial(P_ol, x0, t);
phi_ol = x_ol(:,1);

figure;
plot(t, phi_ol, 'LineWidth', 1.5); grid on;
title('Open loop: \phi(0)=1 rad, u=0');
xlabel('Time (s)'); ylabel('\phi (rad)');

% Closed loop response
figure;
plot(t, phi, 'LineWidth', 1.5); grid on;
title('Closed loop: stabilization from \phi(0)=1 rad');
xlabel('Time (s)'); ylabel('\phi (rad)');

figure;
plot(t, u, 'LineWidth', 1.5); grid on;
title('Controller output u(t)');
xlabel('Time (s)'); ylabel('u = \Delta\delta');

Roll Control PD Controller Script

#include <Wire.h>
#include <Servo.h>

float RateRoll, RatePitch, RateYaw;
float RateCalibrationRoll, RateCalibrationPitch, RateCalibrationYaw;
int RateCalibrationNumber;
float AccX, AccY, AccZ;
float AngleRoll, AnglePitch;
uint32_t LoopTimer;

float KalmanAngleRoll = 0, KalmanUncertaintyAngleRoll = 4;
float KalmanAnglePitch = 0, KalmanUncertaintyAnglePitch = 4;
float Kalman1DOutput[] = {0, 0};

Servo escLeft;
Servo escRight;
int baseThrottle = 1200;
int throttleOffset = 200;

bool motorsArmed = false;
String serialCommand = "";

float Kp = 0.25;    // <-- replace with MATLAB Kp
float Kd = 0.084;    // <-- replace with MATLAB Kd

float k_pwm = 500.0;   // [us per unit-u], keep as a single scaling knob

float dt = 0.004;      // 4 ms loop

float safe_sqrt(float x) {
  if (x <= 0.0001) return 0.0001;
  return sqrt(x);
}

void kalman_1d(float KalmanState, float KalmanUncertainty, float KalmanInput, float KalmanMeasurement) {
  KalmanState = KalmanState + dt * KalmanInput;
  KalmanUncertainty = KalmanUncertainty + dt * dt * 4 * 4;
  float KalmanGain = KalmanUncertainty / (KalmanUncertainty + 9);
  KalmanState = KalmanState + KalmanGain * (KalmanMeasurement - KalmanState);
  KalmanUncertainty = (1 - KalmanGain) * KalmanUncertainty;
  Kalman1DOutput[0] = KalmanState;
  Kalman1DOutput[1] = KalmanUncertainty;
}

void gyro_signals() {
  Wire.beginTransmission(0x68);
  Wire.write(0x1A); Wire.write(0x05); Wire.endTransmission();
  Wire.beginTransmission(0x68);
  Wire.write(0x1C); Wire.write(0x10); Wire.endTransmission();

  Wire.beginTransmission(0x68);
  Wire.write(0x3B); Wire.endTransmission();
  Wire.requestFrom(0x68, 6);
  int16_t AccXLSB = Wire.read() << 8 | Wire.read();
  int16_t AccYLSB = Wire.read() << 8 | Wire.read();
  int16_t AccZLSB = Wire.read() << 8 | Wire.read();

  Wire.beginTransmission(0x68);
  Wire.write(0x1B); Wire.write(0x08); Wire.endTransmission();
  Wire.beginTransmission(0x68);
  Wire.write(0x43); Wire.endTransmission();
  Wire.requestFrom(0x68, 6);
  int16_t GyroX = Wire.read() << 8 | Wire.read();
  int16_t GyroY = Wire.read() << 8 | Wire.read();
  int16_t GyroZ = Wire.read() << 8 | Wire.read();

  RateRoll = (float)GyroX / 65.5;   // deg/s
  RatePitch = (float)GyroY / 65.5;
  RateYaw = (float)GyroZ / 65.5;

  AccX = (float)AccXLSB / 4096;
  AccY = (float)AccYLSB / 4096;
  AccZ = (float)AccZLSB / 4096;

  AngleRoll = atan(AccY / safe_sqrt(AccX * AccX + AccZ * AccZ)) * 180 / PI;      // deg
  AnglePitch = -atan(AccX / safe_sqrt(AccY * AccY + AccZ * AccZ)) * 180 / PI;    // deg
}

void controlMotorsWithRoll_PD() {
  float phi = KalmanAngleRoll * (PI / 180.0f);   // rad
  float p   = RateRoll        * (PI / 180.0f);   // rad/s

  float u = -(Kp * phi + Kd * p);

  int adjustment = constrain((int)(k_pwm * u), -throttleOffset, throttleOffset);

  int leftThrottle  = constrain(baseThrottle - adjustment, 1000, 2000);
  int rightThrottle = constrain(baseThrottle + adjustment, 1000, 2000);

  escLeft.writeMicroseconds(leftThrottle);
  escRight.writeMicroseconds(rightThrottle);

  Serial.println();              // USB debug only
  Serial.print("phi(deg): ");
  Serial.print(KalmanAngleRoll);
  Serial.print(" | p(deg/s): ");
  Serial.print(RateRoll);
  Serial.print(" | adj(us): ");
  Serial.print(adjustment);
  Serial.print(" | L_PWM: ");
  Serial.print(leftThrottle);
  Serial.print(" | R_PWM: ");
  Serial.println(rightThrottle);

  Serial1.print("phi(deg): ");   // Bluetooth output
  Serial1.print(KalmanAngleRoll);
  Serial1.print(" | p(deg/s): ");
  Serial1.print(RateRoll);
  Serial1.print(" | adj(us): ");
  Serial1.print(adjustment);
  Serial1.print(" | L_PWM: ");
  Serial1.print(leftThrottle);
  Serial1.print(" | R_PWM: ");
  Serial1.println(rightThrottle);
}

void setup() {
  Serial.begin(57600);
  Serial1.begin(57600);

  Serial1.println("Bluetooth Ready.");
  Serial.println("USB Ready.");

  Wire.setClock(400000);
  Wire.begin();
  delay(250);

  Wire.beginTransmission(0x68);
  Wire.write(0x6B);
  Wire.write(0x00);
  Wire.endTransmission();

  escLeft.attach(9);
  escRight.attach(10);
  escLeft.writeMicroseconds(1000);
  escRight.writeMicroseconds(1000);

  for (RateCalibrationNumber = 0; RateCalibrationNumber < 2000; RateCalibrationNumber++) {
    gyro_signals();
    RateCalibrationRoll += RateRoll;
    RateCalibrationPitch += RatePitch;
    RateCalibrationYaw += RateYaw;
    delay(1);
  }

  RateCalibrationRoll /= 2000;
  RateCalibrationPitch /= 2000;
  RateCalibrationYaw /= 2000;

  LoopTimer = micros();
}

void loop() {
  gyro_signals();

if (Serial.available()) {
  char c = Serial.read();
  if (c == '\n' || c == '\r') {
    serialCommand.trim();

    if (serialCommand.equalsIgnoreCase("1") && !motorsArmed) {
      Serial.println("Arming motors...");
      escLeft.writeMicroseconds(1000);
      escRight.writeMicroseconds(1000);
      delay(3000);
      motorsArmed = true;
      Serial.println("Motors armed.");
    }
    else if (serialCommand.equalsIgnoreCase("2") && motorsArmed) {
      Serial.println("Disarming motors...");
      escLeft.writeMicroseconds(1000);
      escRight.writeMicroseconds(1000);
      motorsArmed = false;
      Serial.println("Motors disarmed.");
    }
    else if (serialCommand.startsWith("throttle")) {
      int newThrottle = serialCommand.substring(8).toInt();
      if (newThrottle >= 1000 && newThrottle <= 2000) {
        baseThrottle = newThrottle;
        Serial.print("Base throttle set to: ");
        Serial.println(baseThrottle);
      } else {
        Serial.println("Invalid throttle. Must be 1000–2000.");
      }
    }

    serialCommand = "";
  } else {
    serialCommand += c;
  }
}
  if (motorsArmed) {
    RateRoll  -= RateCalibrationRoll;
    RatePitch -= RateCalibrationPitch;
    RateYaw   -= RateCalibrationYaw;

    kalman_1d(KalmanAngleRoll, KalmanUncertaintyAngleRoll, RateRoll, AngleRoll);
    KalmanAngleRoll = Kalman1DOutput[0];
    KalmanUncertaintyAngleRoll = Kalman1DOutput[1];

    kalman_1d(KalmanAnglePitch, KalmanUncertaintyAnglePitch, RatePitch, AnglePitch);
    KalmanAnglePitch = Kalman1DOutput[0];
    KalmanUncertaintyAnglePitch = Kalman1DOutput[1];

    controlMotorsWithRoll_PD();
  } else {
    escLeft.writeMicroseconds(1000);
    escRight.writeMicroseconds(1000);
  }

  while (micros() - LoopTimer < 4000);
  LoopTimer = micros();
}