Reading the RC Channels

Right, with the sensors (nearly) sorted out, I hope, I’m moving on to the next bit of the programming: the input from the RC receiver – the little board that receives the radio signal from the transmitter.

Here’s mine, a cheap one from HobbyKing, where most of my other stuff comes from too:

HobbyKing 6Ch Receiver
HobbyKing 6Ch Receiver

There are 6 channels of information: Throttle, Yaw (Rudder), Pitch (Elevator), Roll (Aileron), and 2 knobs that act as auxiliary channels. The receiver gets these from 2.4Ghz radio waves sent by the transmitter, and converts them into digital PWM signals to send on the flight controller via wires.

PWM stands for pulse width modulation and is a clever way of faking an analog signal using a digital channel. Basically, the length of an ‘on’ pulse dictates the value of the signal.

The RC receiver outputs pulses between roughly 1000μs and 2000μs long, with (for each channel) 1000μs signifying the joystick being at one extreme, and 2000μs showing the other end. Here’s a great diagram from Dr G Owenson’s blog:

Radio Signals

I think he means ms (milliseconds) for the labels, but you get the picture.

Now Arduino has a built in function for this, `pulseIn()`. For example, `pulseIn(7, HIGH)` would read the length of time, in microseconds, that the pulse is high for.

This seems ideal, right? Well, the problem is that it freezes everything while waiting for a pulse to occur and timing it. So none of the rest of the program can run while it’s measuring which means that everything is blocked for between 1000μs and 0.02s (because the pulse runs at 50Hz).

That’s devastating for my program, especially on a quadcopter which needs to be constantly adjusting everything to maintain balance. The Arduino runs at 16MHz – that’s 16 million clock cycles per second, which equates to roughly 8 million instructions per second, or so. That’s a rough maximum of 160,000 instructions lost every time `pulseIn()` runs. Especially since I have 6 channels to check, so in reality the figure would be a lot higher.

Basically I need to find a better way. After a lot of research, the best way I could see to do this is using software interrupts. Basically an interrupt is when as soon as a certain pin goes high (or low), a block of code runs, wherever the processor is in the main loop, and then it carries on with the main program. This means that we can wait for the pin to go high, measure the timestamp, carry on with the main loop, and then when it goes low again measure how long it was high for. That doesn’t block out any other code from happening.

The Arduino Uno only supports hardware interrupts – where this process is actually hardwired onto the board – on pins 2 and 3, but one can more or less equate them using software. I found the extremely helpful EnableInterrupt Library which makes this simple to do.

So what I have set up is as follows. Let’s just use the throttle as an example, but I’ve done it for all 6 channels.

Firstly, I initialise the pin and enable an interrupt on it:

void setup() {
  pinMode(RCTHR, INPUT); //Set throttle pin as INPUT
  enableInterrupt(RCTHR, interruptThrottleRise, RISING); //Assigns an interrupt to throttle pin
}

The first statement, `pinmode(RCTHR, INPUT);` is self-explanatory. `RCTHR` is the pin number that the throttle channel is plugged into. In the next line, I tell the `enableInterrupt` library to call the function `interruptThrottleRise` as soon as the signal goes high.

Here’s that function:

void interruptThrottleRise() {
  enableInterrupt(RCTHR, interruptThrottleFall, FALLING); //Attach the other function for when it next falls again
  throttleStart = micros();
}

As soon as it the throttle signal goes high, I firstly reassign the interrupt, saying ‘now when it goes low again, call function `interruptThrottleFall`’. Then I record the current timestamp with global variable throttleStart.

So now the code gets on with itself in the main loop, and when the high signal ends `interruptThrottleFall` is called:

void interruptThrottleFall() {
  enableInterrupt(RCTHR, interruptThrottleRise, RISING); //Attach the other function for when it next rises again
  throttleRaw = micros() - throttleStart;
}

I reassign the first interrupt back to the pin, ready to start again, and then record the time passed since the signal went high, with `throttleRaw`. This variable should now contain the length of the pulse, in microseconds, roughly between 1000 and 2000.

This happens with all 5 other channels and works very well.

The next thing to do is make this value into something usable. We have a number somewhere between 1000 and 2000, but because it comes from an analog joystick these numbers are not exact at all. So first of all I printed off `throttleRaw` on serial and measured the max and min values it output. In this case, with the throttle stick at the bottom the value was 1152 and at the top it was 1844.

Now I actually want to leave this as it is for the throttle, because I can callibrate the ESCs to these endpoints and directly dictate the motor power, but for all the other channels I need to turn them into something more reasonable.

For pitch and roll, I need angles between -45° and 45°, because these are going to be the attitude limits of the quadcopter. For yaw, I want an angle between -150° and 150°, which will allow good control of the rate of rotation later (I got this figure from Owenson’s article), and for the auxiliary channels I want an easy-to-work-with number between 0 and 100.

So I’m just going to use the Arduino built-in `map()` function to do this, along with `constrain()` to keep the values within these endpoints should the minima/maxima of the joysticks drift.

Here’s what I’ve got:

void rcMap() {
  currentRC.throttle = throttleRaw; //Leave throttle untouched for direct ESC callibration and motor drive
  currentRC.yaw = constrain(map(yawRaw, 1132, 1940, -150, 150), -150, 150);
  currentRC.pitch = -constrain(map(pitchRaw, 1160, 1816, -45, 45), -45, 45);
  currentRC.roll = constrain(map(rollRaw, 1144, 1896, -45, 45), -45, 45);
  currentRC.aux1 = constrain(map(aux1Raw, 1000, 2048, 0, 100), 0, 100);
  currentRC.aux2 = constrain(map(aux2Raw, 1000, 2048, 0, 100), 0, 100);
}

I added a minus to pitch because without it the value is positive when the stick is forwards, and I need it to be the other way round to work with orientation angles properly later.

`currentRC` is just a data structure containing 6 floats, similar to the one I used in the sensor fusion setup. I just call `rcMap()` once a cycle to properly convert the data, and then anywhere throughout the cycle I can access the most recent RC inputs via `currentRC.whatever`.

Next up I need to take the RC inputs, and the sensor inputs, and use an awful lot of PID loops to control the motors!

Leave a Reply

Your email address will not be published. Required fields are marked *