Four-channel (pitch, roll, yaw, speedbrake) servo mixing algorthim for flying wing designs

servo_4mixer.pde

int windowWidth;
int windowHeight;
float surfaceHeight = 50;

// sum should be 1
float kYaw = .5, kBrake = .5;

float[] mix(float pitch, float yaw, float roll, float brake) {
  float brakeSeparation = kBrake * (1 + brake) / 2;
  float yawMix = Math.max(kYaw, 1 - brakeSeparation);
  float separation0 = brakeSeparation + yawMix * Math.max(0, yaw);
  float separation1 = brakeSeparation + yawMix * Math.max(0, -yaw);

  float angle0 = (pitch + roll) / 2;
  float angle1 = (pitch - roll) / 2;

  println("sepration0 = " + separation0);
  println("separation1 = " + separation1);

  println("angle0 = " + angle0);
  println("angle1 = " + angle1);

  return new float[] {
    (separation0 + angle0) / 2,
    (separation0 - angle0) / 2,
    (separation1 + angle1) / 2,
    (separation1 - angle1) / 2
  };
}

void draw() {
    
  // range is (-1, 1)
  float pitch = 0;
  float yaw = (float)mouseX / windowWidth * 2 - 1;
  float roll = 0;
  float brake = (float)mouseY / windowHeight * 2 - 1; // -1 is no brake, 1 is full brake

  println("pitch = " + pitch);
  println("yaw = " + yaw);
  println("roll = " + roll);
  println("brake = " + brake);

  float[] servos = mix(pitch, yaw, roll, brake);

  // range is (-1, 1)
  println("servo0 = " + servos[0]);
  println("servo1 = " + servos[1]);
  println("servo2 = " + servos[2]);
  println("servo3 = " + servos[3]);

  background(0);

  // draw a red background if servos are smashing into each other
  if ((servos[0] < 0 && servos[1] < 0) || (servos[2] < 0 && servos[3] < 0))
    background(255, 0, 0);

  fill(128, 128, 0, 128);
  rect(0, windowHeight / 2 - servos[0] * surfaceHeight, windowWidth / 2, servos[0] * surfaceHeight);
  rect(windowWidth / 2, windowHeight / 2 - servos[2] * surfaceHeight, windowWidth / 2, servos[2] * surfaceHeight);

  fill(0, 128, 128, 128);
  rect(0, windowHeight / 2, windowWidth / 2, servos[1] * surfaceHeight);
  rect(windowWidth / 2, windowHeight / 2, windowWidth / 2, servos[3] * surfaceHeight);
}

void setup() {
  windowWidth = windowHeight = 480;
  size(windowWidth, windowHeight);
}