Quadcopter Lowlevel control - latency issues

Hi all!

I want to control a quadcopter running a lowlevel control algorithm on a ground based PC. Therefore, I need to uplink the four rotor commands at high frequency (>10^2 Hz) and low latency (<10^1 ms).

The idea is to use a 3DR radio https://store.3drobotics.com/products/3dr-radio to send serial commands.
I wrote my own very simple protocol: Each command consists of 2 byte payload (that includes 2 bit rotor ID and 1 parity bit) + 1 byte delimiter (’~’ → 126).

The rotors receive the commands correctly, but the latency seems to be far to high… =(
As I am not an experienced ARDUINO user, the code might not be efficient!?

// Controlling four ecs' using a serial input

// include libs
#include <Servo.h>

// declare and initialize variables
byte serialdata[3] = {0}; // 2 bytes of data + 1 endbyte
int rotorID = 0; // rotor ID
int sum = 0; // sum over bits
unsigned serialint[4] = {0}; // rotor signals
Servo rotor1, rotor2, rotor3, rotor4;

void setup() 
{ 
  // initialize serial port
  Serial.begin(115200); // set baud rate
  
  // initialize servos
  rotor1.attach(A0);
  rotor1.write(0);
  rotor2.attach(A1);
  rotor2.write(0);
  rotor3.attach(A2);
  rotor3.write(0);
  rotor4.attach(A3);
  rotor4.write(0);   
} 

void loop() 
{ 
  // clear variables
  sum = 0;
  rotorID = 0;
 
   if (Serial.available() >= 3){
     
    // read 3 bytes from serial buffer 
    for (int i=0; i<3; i++){
      serialdata[i] = Serial.read();
    }
    
    // check for correct byte order
    if (serialdata[2] == 126){
      // sum bits
      for (int j=0; j<8; j++){
        sum = sum + bitRead(serialdata[0],j) + bitRead(serialdata[1],j);
      }
      
      // validity check
      if (sum%2 == 0){
        
        // set rotor ID
        if (bitRead(serialdata[1],5) == 0 && bitRead(serialdata[1],6) == 0){
         rotorID = 1;
        }
        else if (bitRead(serialdata[1],5) == 1 && bitRead(serialdata[1],6) == 0){
         rotorID = 2;
        }
        else if (bitRead(serialdata[1],5) == 0 && bitRead(serialdata[1],6) == 1){
         rotorID = 3;
        }
        else if (bitRead(serialdata[1],5) == 1 && bitRead(serialdata[1],6) == 1){
         rotorID = 4;
        }
        else{
          // no valid rotor ID found
        }
        
        // calculate updated servo position
        for (int k=0; k<3; k++){
          bitClear(serialdata[1],5+k);
        }
        serialint[rotorID-1] = serialdata[0]+serialdata[1]*256; // assemble bytes
        serialint[rotorID-1] = map(serialint[rotorID-1], 0, 800, 30, 130); // map to valid range  
      }
  
      else{
        // data not valid

      }
    }
  }
  
  else {
    // no data available
  }
 
  // set servo positions
  rotor1.write(serialint[0]);
  rotor2.write(serialint[1]);
  rotor3.write(serialint[2]);
  rotor4.write(serialint[3]);
}

Any help is grately appreciated.

Thanks,
Stefan

        serialint[rotorID-1] = map(serialint[rotorID-1], 0, 800, 30, 130); // map to valid range

You'd have better performance if you did this on the sending end. It's pointless sending unusable data.

I'm very doubtful you could do this over a radio link, the latency will just be too high.

You might get better results by using one of the third party ESC firmware's available depending on the ESC's you're using, but that will only speed up the link from your on board controller to the motors which is unlikely to be your biggest problem

This sums up your problems fairly nicely, in short, your radio is designed for telemetry, which is about getting larger data packets moved reliably over large distances rather than a control link which is very small packets with low latency and if one it doesn't matter too much as the next update coming very soon will be fine

Hi WendoNZ,

thanks for you reply.

This helps a lot. I hoped I could do it with the 3DR radio but I reckon I need to switch to another transmitter.
If there is anyone out there who did something like that before and has a smart solution to the problem, please let me know.

Thanks,
Stefan

One more thing to think about here, how are you keeping the quad stable when it's in the air if you have nothing on-board to do so?

Typically quads have an acceleromoter and gyro on board to keep them level in flight. Setting the 4 rotors to the same speed will not keep a quadcopter under control. If you do have them on board then you must be sending their data back over the radio link, interpreting it and then sending the correcting commands back. Even with the right radio setup I'm still not sure you could do that round trip fast enough to keep the quad under control