Embedded/Controls

Two rotors, each actuated by a servo. STM32-based Flight Controller. PID closed loop control with FF. Very tricky to tune pitch axis since rotor tilt controls two degrees of freedom: both vehicle pitch and forward lateral movement. Tuned heuristically from approximated step responses derived from black-box logging.

This hexacopter was built as a subscale demonstrator of an experimental rotor configuration for a professional role. I’ve posted it here as an example of my experience with PX4 firmware. This vehicle required a custom build of PX4 to define the actuator matrix in three dimensions due to the vertical separation of the rotors.

Power from the battery will be split between electronic speed controllers(ESCs) for the motors, a flight controller board, and a 5v BEC. The ESCs run an open source software called BL Heli. This is critical because I will be signaling the ESCs with raw PWM signal from the Arduino. Typically the ESC signal is bidirectional. This allows for RPM filtering with a standard flight controller. Without RPM filtering BL Heli allows me to tune the ESC for my specific motors. I wont go too much into motor tuning methodology because it’s widely applicable to quadcopters and nothing unique on this project. The motors spun best with a slightly higher timing angle (around 18 deg) and with medium demag compensation.
The flight controller (FC) allows access to the ESCs with it’s USB port. Most critically the FC also provides gyro data to the Arduino for the pitch and roll axis. It has a built in BEC as well, but with this many 5v outputs it wont be able to handle everything, hence the dedicated BEC. All the control surfaces will be handled by the receiver. It’s possible to drive the servos using the flight controller or the Arduino directly, but the receiver offers the most direct means of control. Its the “dumbest” electrical component, so if anything goes wrong with software in the Arduino or FC, I’ll still be able to glide to the ground. The RX also relays GPS and Battery telemetry to the transmitter.

#include "PWM.hpp"
#include <Servo.h>
#include <Wire.h>
 

//output variables for motors and tilt
int leftmotor;
int rightmotor;
int tiltOUT;

//input variables from FC
int throttle;
int roll;
int pitch;
int yaw;
int tiltIN;

//intermediate variables for transitioning
int rollMIX;
int yawMIX;
int pitchMIX;


//Outputs
Servo leftmotoroutput; 
Servo rightmotoroutput; 
Servo tiltoutput;

//pins in
PWM ch1(0);
PWM ch2(2);
PWM ch3(3);
PWM ch4(7);
PWM ch5(1);


void setup () {
  
  ch1.begin(true);
  ch2.begin(true);
  ch3.begin(true);
  ch4.begin(true);
  ch5.begin(true);   
  //Output Pins
  leftmotoroutput.attach(5);   // throttleleft
  rightmotoroutput.attach(9);  // throttleright
  tiltoutput.attach(6);      // tiltservo
}

void loop () {

  //making math easier  for mixing(setting the midpoint to zero)
    if (ch1.getValue() > 1000 && ch1.getValue() < 2000) {
    throttle = ch1.getValue() - 1500;
  } else {
    throttle = -500;
  }
  
  tiltIN = ch5.getValue() - 1500;  
  roll = ch2.getValue() - 1500;
  pitch = ch3.getValue() - 1500;
  yaw = ch4.getValue() - 1500;
  
   
  //transitions
  tiltIN = constrain(tiltIN, -500, 500);
  //reduces roll and pitch as motors tilt forward, while introducing yaw
  rollMIX = roll * (1 - ((tiltIN + 500) * 0.001));
  yawMIX = yaw * ((tiltIN + 500) * 0.001);
  pitchMIX = pitch * (1 - ((tiltIN + 500) * 0.001));
  //adding the axis together for throttle values
  leftmotor = throttle + (rollMIX * 0.2) - (yawMIX * 0.2) + 1500;
  rightmotor = throttle - (rollMIX * 0.2) + (yawMIX * 0.2) + 1500;
  tiltOUT = - tiltIN - pitchMIX + 1500;
  //PWM ranges
  tiltOUT = constrain(tiltOUT, 1200, 1800);
  leftmotor = constrain(leftmotor, 1000, 2000);
  rightmotor = constrain(rightmotor, 1000, 2000);
  //outputs
  tiltoutput.writeMicroseconds(tiltOUT);
  leftmotoroutput.writeMicroseconds(leftmotor);
  rightmotoroutput.writeMicroseconds(rightmotor);
  
  delay(30);
}

This code switches between inputs as the rotors transition. While hovering differential thrust is used for stabilized roll and the tilt servo for stabilized pitch. As the blades pitch forward the combined PID and control input from these axis is reduced. Simultaneously, yaw input replaces roll input in differential thrust. The yaw has no PID stabilization as the aircraft is passively stable with its vertical stabilizers. The transmitter itself has a simple Lua script that temporarily raises the angle of incidence of the canard during transitions.

Without active stabilization, a hovering multirotor has neutral static stability. It may fly for a few seconds but it will be nearly impossible to keep in the air, let alone go where the pilot wants. Active stabilization assists the pilot. I will be implementing a basic control loop, known as a PID loop. A PID loop has proportional, integral and differential components. Each part is a “loop” meaning it continually reacts to inputs that are influenced by its own outputs as well as outside disturbances and pilot inputs. A PID loop will be assigned to the pitch and roll axis. Below is the bare-bones equation:

//basic PID math
unsigned long past;
double kp, ki, kd;
double In, Out, Setpoint;
double errorSum, pastError;

void Compute()
{
   unsigned long now = millis();
   double timeChange = (double)(now - past);
   double error = Setpoint - In;
   errorSum += (error * timeChange);
   double dErr = (error - pastError) / timeChange;
   Out = kp * error + ki * errorSum + kd * dErr;  
   pastError = error;
   past = now;
}

The purpose of the PID loop is to continually output an error value, in this case the difference between a given setpoint(or pilot input) and data from the gyro. The proportional term looks at the angle of the aircraft in a given axis, allowing for motor inputs that move the aircraft towards the setpoint. The derivative term looks at angular velocity. This allows for dampening of angular momentum. As the P term moves the aircraft towards the setpoint, the D term will grow stronger in opposition, eliminating oscillation. The integral term looks at the angle over time, growing in strength over the course of continuous disturbances like wind or imbalances in the aircraft. Key additions to the basic PID include feed-forward and auto leveling. Feed-forward reduces the authority of the PID loop, prioritizing pilot inputs to a given degree. Auto-leveling acts as a proportional filter to pilot inputs, influencing the setpoint so that the aircraft is “attracted” towards level. The key challenge is to tune the strength of these additions and the respective PID loops for each axis.