I made a kOS quadcopter script (WIP). Can someone help me finish it?

[MAFman]

I'm trying to make a quadcopter that's actually flyable intuitively, and to that end I'm developing a kOS script to control it. I'm using the Breaking Ground rotors and controlling them using their max RPM fields. Here's the code: 

// Fly-by-wire for a quadcopter

// Declare variables representing the four motors, which are tagged
set frontLeft to ship:partsTagged("front left")[0].
set frontRight to ship:partsTagged("front right")[0].
set rearLeft to ship:partsTagged("rear left")[0].
set rearRight to ship:partsTagged("rear right")[0].

// Use setField(rpmLimit)
set frontLeftThrottle to frontLeft:getModule("ModuleRoboticServoRotor").
set frontRightThrottle to frontRight:getModule("ModuleRoboticServoRotor").
set rearLeftThrottle to rearLeft:getModule("ModuleRoboticServoRotor").
set rearRightThrottle to rearRight:getModule("ModuleRoboticServoRotor").

set motors to list("frontLeft", "frontRight", "rearLeft", "rearRight").

function throttleMotor {
  parameter motor, throt. // throt is a float between 0 and 1
  if motor = "frontLeft" {
    frontLeftThrottle:setField(throt * 460).
  }
  else if motor = "frontRight" {
    frontRightThrottle:setField(throt * 460).
  }
  else if motor = "rearLeft" {
    rearLeftThrottle:setField(throt * 460).
  }
  else if motor = "rearRight" {
    rearRightThrottle:setField(throt * 460).
  }
  else { // Catch-all, this should never happen
    print("Error: invalid motor!").
  }
}

function calcMotorSpeed {
  // Make the speed of each motor proportional to the main throttle plus the input from steering
  set rotorSpeed to 230 * pilotMainThrottle.
  set speedOffset to rotorSpeed * 0.25.

  // Pitch: If negative, front rotors slow down and rear rotors speed up
  //        If positive, front rotors speed up and rear rotors slow down
  //        This happens until the measured pitch is +/- 15 degrees,
  //        at which point the four motors briefly change speed to stop the pitch
  //        If zero, reverse pitch until forward speed is zero
  set pitchOutput to list(0, 0, 0, 0).
  if pilotPitch = -1 {
    set pitchOutput[0] to -speedOffset.
    set pitchOutput[1] to -speedOffset.
    set pitchOutput[2] to speedOffset.
    set pitchOutput[3] to speedOffset.
  }
  else if pilotPitch = 1 {
      set pitchOutput[0] to speedOffset.
      set pitchOutput[1] to speedOffset.
      set pitchOutput[2] to -speedOffset.
      set pitchOutput[3] to -speedOffset.
    }
  else {
    // Reverse outputs until measured forward speed is zero, then set all input offsets to 0.
  }

  // Yaw: If negative, front left and rear right rotors speed up, and rear left and front right rotors slow down
  //      If positive, front right and rear left rotors speed up, and rear right and front left rotors slow down
  //      If zero, set all rotors to equal speed.
  set yawOutput to list(0, 0, 0, 0).
  if pilotYaw = -1 {
    set pitchOutput[0] to -speedOffset.
    set pitchOutput[2] to -speedOffset.
    set pitchOutput[1] to speedOffset.
    set pitchOutput[3] to speedOffset.
  }
  else if pilotYaw = 1 {
      set yawOutput[0] to speedOffset.
      set yawOutput[2] to speedOffset.
      set yawOutput[1] to -speedOffset.
      set yawOutput[3] to -speedOffset.
    }
  else {
    set yawOutput[0] to 0.
    set yawOutput[1] to 0.
    set yawOutput[2] to 0.
    set yawOutput[3] to 0.
  }

  // Roll: If positive, left rotors speed up and right rotors slow down
  //       If negative, left rotors slow down and right rotors speed up
  //       This happens until the measured roll is +/- 15 degrees,
  //       at which point the rotors briefly change speed to stop the roll
  //       If zero, reverse roll until lateral speed is zero
  set rollOutput to list(0, 0, 0, 0).
  if pilotRoll = -1 {
    set rollOutput[0] to -speedOffset.
    set rollOutput[2] to -speedOffset.
    set rollOutput[1] to speedOffset.
    set rollOutput[3] to speedOffset.
  }
  else if pilotPitch = 1 {
      set rollOutput[0] to speedOffset.
      set rollOutput[2] to speedOffset.
      set rollOutput[1] to -speedOffset.
      set rollOutput[3] to -speedOffset.
    }
  else {
    // Reverse outputs until measured forward speed is zero, then set all input offsets to 0.
  }

  // Total: Sum all inputs and that's the final output.
  set totalOutput to list(0, 0, 0, 0).
  for element in pitchOutput {
    set totalOutput[element] to pitchOutput[element] + yawOutput[element] + rollOutput[element] + rotorSpeed.
  }
  for m in motors {
    throttleMotor(m, totalOutput[indexOf(m)]).
  }
}

I have yet to implement the auto-leveling, which I have no idea how to do. How do real drones pull it off?

[Snark]

Moving to Add-on Discussions.