P.I.D. Stands for Proportional Integral Derivative (Controller)

Now that we can see what it's going on with the collected data, we can focus on the "intelligent" part of the autopilot.

So off to study control systems we go. For the beginning, the simplest implementation of a PID controller. And the key word is simple. No boundary checking, trial and error parameters, ...

And it didn't work half that bad (running FlightGear and recording the flight did introduce some lag and jitter):

For the development and tests: FlightGear with Rascal110-JSBSim aircraft, connected trough wireless network to the Samsung Galaxy S2 which is running the autopilot software.

The flight: starting the engine, controlling the plane with a mouse while taking off, banking to the left side, turning on the autopilot, the plane levels (some oscillation because of the lag introduced with running the simulator and recording it), using just the rudder to turn right, plane turns right while being stabilized with no roll, reset all controlling surfaces to default position, using just the rudder to turn left, plane turns left while being stabilized with no roll, reset all controlling surfaces to default position, fly straight, turn off autopilot, plane "crashes".

And that's how the flight looked, location, speed, and altitude (yes, the "heat map" still needs a legend, it's a work in progress).

Now, just fro fun, let's look at some graphs, these are always fun:

Time stamps of network packets with data received. Almost completely linear, with some variation. In some other tests, where the variation was significant, the autopilot would fly smooth, then correct violently (error being calculated was wrong due to incorrect timing of the event and the received data stamp), oscillate a bit and the settle again.

Roll graph: showing target attitude, current attitude and error.

Roll graph: showing the PID controller output, not limited to reasonable boundary’s.

Roll graph: showing integral part of the error, not very useful  for calculating offsets at the moment since it's not limited to reasonable boundary’s.

Roll graph: derivative part of the error, showing where were the quickest changes.

Roll stabilization was computed with these weights: Kp=0.1111, Ki=0.0, Kd=0.5

Pitch graph: showing target attitude, current attitude and error.

Pitch graph: showing the PID controller output, not limited to reasonable boundary’s. Zooming closer to the latter part, there would be visible some minor oscillation, as is visible in the previous graph.

Pitch graph: showing integral part of the error, not very useful  for calculating offsets at the moment since it's not limited to reasonable boundary’s.

Roll graph: derivative part of the error, showing where were the quickest changes.

Roll stabilization was computed with these weights: Kp=-0.019, Ki=-0.025, Kd=0.75

It's not perfect, but it's a start.

Visualization Makes Data in to Pretty Pictures

Things may look like they're progressing slowly, but looks can be deceiving.

Take large datasets for example. Wall of numbers like this is an awesome thing for a computer, not so much for a human:

And this is just the navigational data from a quick flight in FlightGear with the RC2USB module, someday to be piped in to an autopilot.  Forget about the amount of attitude data used to keep the planed levelled.

To better "see" what's going on, we plot the data in to google earth:

With the code prom the previous project, we can see where and when, but not exactly what is going on. We can do better.

With some work, we can get a nice "heat map" of speed.

And once we know how to do that, we can visualize altitude. Can you guess what was the highest point of the flight?

Mix it all together, and we get a lot more information from the same image. Now, when we can "see better", we're ready to tackle on the autopilot.

P.S. Want to make pretty pictures like this? Start reading: https://developers.google.com/kml/documentation/

Programming rc2usb Module

Playing with the module for a while now, making sure all the desired features work:

- read 8 servo PWM signals from an RC receiver
- convert the PWM signal to a value between -1.00 and 1.00
- output a formatted string that something like FlightGear can parse (25 times a second)
- output a separate PWM signal for servo testing (change servo position every second to: far left, center, far right, center, ... with an LED indicator when the servo is in center position)

To program it, you'll need avr-gcc to compile the code and avrdude & an AVR programmer to flash the firmware.

And now, the code:

#include <avr/io.h>
#include <util/delay.h>
#include <avr/interrupt.h>
#include <stdlib.h>

// USART definitions
#define BAUD 57600
#define FOSC 20000000            // clock Speed
#define MYUBRR FOSC/16/BAUD-1

// global variables
volatile unsigned char port_c_last = 0x00;        // pin change interrupts PC0-5
volatile unsigned char port_c_change = 0x00;    // pin change interrupts PC0-5
volatile unsigned char port_d_last = 0x00;        // pin change interrupts PD3-4
volatile unsigned char port_d_change = 0x00;    // pin change interrupts PD3-4

volatile unsigned short timer_temp = 0x0000;
volatile unsigned short timer_temp_result[8] = {0x0EA6, 0x0EA6, 0x0EA6, 0x0EA6, 0x0EA6, 0x0EA6, 0x0EA6, 0x0EA6};    // 0x0EA6=3750 - after conversion the output is 0
volatile unsigned short timer_temp_storage[8] = {0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000};    // storage like any other

volatile unsigned char counter_low = 0x00;
volatile unsigned char counter_high = 0x00;
volatile unsigned short temp_storage = 0x0000;

volatile float data_out = 0.0;                    // output data in the right format for flightgear to parse
char buffer_out[10];                            // flightgear likes strings
unsigned char buffer_out_pos;                    // char by char eaquals string

// USART init
void USART_Init( unsigned int ubrr) {
    // set baud rate
    UBRR0H = (unsigned char)(ubrr>>8);
    UBRR0L = (unsigned char)ubrr;

    // enable receiver and transmitter
    UCSR0B = (1<<RXEN0)|(1<<TXEN0);
    
    // set frame format: 8data, 2stop bit
    UCSR0C = (1<<USBS0)|(3<<UCSZ00);
}

// USART transmit
void USART_Transmit( unsigned char data ) {
    // wait for empty transmit buffer
    while ( !( UCSR0A & (1<<UDRE0)) );
    
    // put data into buffer, sends the data
    UDR0 = data;
}

// pin change interrupt alias
ISR(PCINT2_vect, ISR_ALIASOF(PCINT1_vect));

// pin change interrupt
ISR(PCINT1_vect) {
    // get the timer, get the timer NOW!
    timer_temp = TCNT1;    
    
    // check which pin triggerd the interrupt
    port_c_change = (PINC & PCMSK1) ^ port_c_last;
    port_d_change = (PIND & PCMSK2) ^ port_d_last;
    port_c_last = (PINC & PCMSK1);
    port_d_last = (PIND & PCMSK2);
    
    // find and service the apropriate pins
    if (port_c_change & 0x20) {            // channel 1 (PC5 - PCINT 13 - PCMSK1 bit 5)
        if (port_c_last & 0x20) {        // low to hight
            timer_temp_storage[0] = timer_temp;
        }
        else {                            // high to low
            if (timer_temp >= timer_temp_storage[0]) {
                timer_temp_result[0] = timer_temp - timer_temp_storage[0];
            }
            else {
                timer_temp_result[0] = (ICR1 - timer_temp_storage[0]) + timer_temp;
            }
        }
    }
    if (port_c_change & 0x10) {            // channel 2 (PC4 - PCINT 12 - PCMSK1 bit 4)
        if (port_c_last & 0x10) {        // low to hight
            timer_temp_storage[1] = timer_temp;
        }
        else {                            // high to low
            if (timer_temp >= timer_temp_storage[1]) {
                timer_temp_result[1] = timer_temp - timer_temp_storage[1];
            }
            else {
                timer_temp_result[1] = (ICR1 - timer_temp_storage[1]) + timer_temp;
            }
        }
    }
    if (port_c_change & 0x08) {            // channel 3 (PC3 - PCINT 11 - PCMSK1 bit 3)
        if (port_c_last & 0x08) {        // low to hight
            timer_temp_storage[2] = timer_temp;
        }
        else {                            // high to low            
            if (timer_temp >= timer_temp_storage[2]) {
                timer_temp_result[2] = timer_temp - timer_temp_storage[2];
            }
            else {
                timer_temp_result[2] = (ICR1 - timer_temp_storage[2]) + timer_temp;
            }
        }
    }
    if (port_c_change & 0x04) {            // channel 4 (PC2 - PCINT 10 - PCMSK1 bit 2)
        if (port_c_last & 0x04) {        // low to hight
            timer_temp_storage[3] = timer_temp;
        }
        else {                            // high to low            
            if (timer_temp >= timer_temp_storage[3]) {
                timer_temp_result[3] = timer_temp - timer_temp_storage[3];
            }
            else {
                timer_temp_result[3] = (ICR1 - timer_temp_storage[3]) + timer_temp;
            }
        }
    }
    if (port_c_change & 0x02) {            // channel 5 (PC1 - PCINT 9 - PCMSK1 bit 1)
        if (port_c_last & 0x02) {        // low to hight
            timer_temp_storage[4] = timer_temp;
        }
        else {                            // high to low            
            if (timer_temp >= timer_temp_storage[4]) {
                timer_temp_result[4] = timer_temp - timer_temp_storage[4];
            }
            else {
                timer_temp_result[4] = (ICR1 - timer_temp_storage[4]) + timer_temp;
            }
        }
    }
    if (port_c_change & 0x01) {            // channel 6 (PC0 - PCINT 8 - PCMSK1 bit 0)
        if (port_c_last & 0x01) {        // low to hight
            timer_temp_storage[5] = timer_temp;
        }
        else {                            // high to low
            if (timer_temp >= timer_temp_storage[5]) {
                timer_temp_result[5] = timer_temp - timer_temp_storage[5];
            }
            else {
                timer_temp_result[5] = (ICR1 - timer_temp_storage[5]) + timer_temp;
            }
        }
    }
    if (port_d_change & 0x08) {            // channel 7 (PD3 - PCINT 19 - PCMSK2 bit 3)
        if (port_d_last & 0x08) {        // low to hight
            timer_temp_storage[6] = timer_temp;
        }
        else {                            // high to low
            if (timer_temp >= timer_temp_storage[6]) {
                timer_temp_result[6] = timer_temp - timer_temp_storage[6];
            }
            else {
                timer_temp_result[6] = (ICR1 - timer_temp_storage[6]) + timer_temp;
            }
        }
    }
    if (port_d_change & 0x10) {            // channel 8 (PD4 - PCINT 20 - PCMSK2 bit 4)
        if (port_d_last & 0x10) {        // low to hight
            timer_temp_storage[7] = timer_temp;
        }
        else {                            // high to low            
            if (timer_temp >= timer_temp_storage[7]) {
                timer_temp_result[7] = timer_temp - timer_temp_storage[7];
            }
            else {
                timer_temp_result[7] = (ICR1 - timer_temp_storage[7]) + timer_temp;
            }
        }
    }    
}

ISR(TIMER1_COMPB_vect) {
    // turn off other interrupts (servo duty cycle is 50Hz, so we get at least every other one while we're not sending)
    cli();    // optional - works without, but why waste cycles reading new servo state if we read it at least once more before we send it
    
    // update the conter, update the counter now
    counter_low++;
    
    // run 25 times per second - transmit and more
    if (counter_low >= 2) {
        // transmmit - the slowest part of them all (one by one, divide and convert)
        temp_storage = timer_temp_result[0];                // get the last known servo state, so it's not overwritten if we got an interrupt in an interrupt
        data_out = (temp_storage / 1250.0) - 3.0 ;            // formula for converting 2500 to 5000 counter cycles in to -1.0 to 1.0 range (flightgear likes)
        dtostrf(data_out, 0, 2, buffer_out);                // flightgear likes strings, converting float to string (2 decimal precisin)
        buffer_out_pos = 0;                                    // char in string counter
        while (buffer_out[buffer_out_pos] != '\0') {        // send data
            USART_Transmit(buffer_out[buffer_out_pos]);
            buffer_out_pos++;    
        }
        USART_Transmit('\t');                                // send delimiter
        
        temp_storage = timer_temp_result[1];
        data_out = (temp_storage / 1250.0) - 3.0 ;
        dtostrf(data_out, 0, 2, buffer_out);
        buffer_out_pos = 0;
        while (buffer_out[buffer_out_pos] != '\0') {
            USART_Transmit(buffer_out[buffer_out_pos]);
            buffer_out_pos++;    
        }
        USART_Transmit('\t');
        
        temp_storage = timer_temp_result[2];
        data_out = (temp_storage / 1250.0) - 3.0 ;
        dtostrf(data_out, 0, 2, buffer_out);
        buffer_out_pos = 0;
        while (buffer_out[buffer_out_pos] != '\0') {
            USART_Transmit(buffer_out[buffer_out_pos]);
            buffer_out_pos++;    
        }
        USART_Transmit('\t');
        
        temp_storage = timer_temp_result[3];
        data_out = (temp_storage / 1250.0) - 3.0 ;
        dtostrf(data_out, 0, 2, buffer_out);
        buffer_out_pos = 0;
        while (buffer_out[buffer_out_pos] != '\0') {
            USART_Transmit(buffer_out[buffer_out_pos]);
            buffer_out_pos++;    
        }
        USART_Transmit('\t');
        
        temp_storage = timer_temp_result[4];
        data_out = (temp_storage / 1250.0) - 3.0 ;
        dtostrf(data_out, 0, 2, buffer_out);
        buffer_out_pos = 0;
        while (buffer_out[buffer_out_pos] != '\0') {
            USART_Transmit(buffer_out[buffer_out_pos]);
            buffer_out_pos++;    
        }
        USART_Transmit('\t');
        
        temp_storage = timer_temp_result[5];
        data_out = (temp_storage / 1250.0) - 3.0 ;
        dtostrf(data_out, 0, 2, buffer_out);
        buffer_out_pos = 0;
        while (buffer_out[buffer_out_pos] != '\0') {
            USART_Transmit(buffer_out[buffer_out_pos]);
            buffer_out_pos++;    
        }
        USART_Transmit('\t');
        
        temp_storage = timer_temp_result[6];
        data_out = (temp_storage / 1250.0) - 3.0 ;
        dtostrf(data_out, 0, 2, buffer_out);
        buffer_out_pos = 0;
        while (buffer_out[buffer_out_pos] != '\0') {
            USART_Transmit(buffer_out[buffer_out_pos]);
            buffer_out_pos++;    
        }
        USART_Transmit('\t');
        
        temp_storage = timer_temp_result[7];
        data_out = (temp_storage / 1250.0) - 3.0 ;
        dtostrf(data_out, 0, 2, buffer_out);
        buffer_out_pos = 0;
        while (buffer_out[buffer_out_pos] != '\0') {
            USART_Transmit(buffer_out[buffer_out_pos]);
            buffer_out_pos++;    
        }
        USART_Transmit('\n');
        
        counter_low = 0;
        counter_high++;
        
        // run 1 time per second - set servo position and LED status
        if (counter_high == 25) {
            OCR1A = (2500*1.5);        // servo center position
            PORTD |= 0x04;            // LED on
        }
        else if (counter_high == 50) {
            OCR1A = (2500*1.0);        // servo left position
            PORTD &= ~0x04;            // LED off
        }
        else if (counter_high == 75) {
            OCR1A = (2500*1.5);        // servo center position
            PORTD |= 0x04;            // LED on
        }
        else if (counter_high == 100) {
            OCR1A = (2500*2.0);        // servo right position
            PORTD &= ~0x04;            // LED off
            counter_high = 0;        // restart the cycle
        }
        
    }
    // enable back all interrupts
    sei();    // optional - use if interrupts disabled at the beginning
}

int main(void) {
    // set PD2 as output - LED
    DDRD |= 0x04;
    PORTD &= ~0x04;        // LED (PD2) off
    
    // set PB1 as output - PWM
    DDRB |= 0x02;
    
    // set PC0-5 (PCI1) & PD3-4 (PCI2) as input - PIN CHANGE INTERRUPT
    DDRC &= ~0x3F;
    DDRD &= ~0x19;    // includes the RX to FT230X
    
    // set internal pull up resistors
    PORTC |= 0x3F;
    PORTD |= 0x19;
    
    
    // init USART
    USART_Init(MYUBRR);
    
    // set Timer/Counter1 with PWM
    TCCR1A |= 1<<WGM11;                    // set Waveform Generation Mode to 14 (fast PWM - ICR1)
    TCCR1B |= 1<<WGM12 | 1<<WGM13;        // set Waveform Generation Mode to 14 (fast PWM - ICR1)
    //TCCR1A |= 1<<COM1A1 | 1<<COM1A0;    // set inverted mode (starts at 0, jumps to 1 when OCR1A is reached)
    TCCR1A |= 1<<COM1A1;                // set non-inverted mode (starts at 1, jumps to 0 when OCR1A is reached)
    TCCR1B |= 1<<CS11;                    // set prescaler to 8 (50Hz duty cycle for servo - ((20000000/8)/50) = 50000)    
    ICR1 = 49999;                        // set the Input Capture Register for 50Hz duty cycle (0-49999)
    // set Timer/Counter1 with interrupt
    TIMSK1 |= 1<<OCIE1B;                // set interrupt on CTC
    OCR1B = 49999;                        // set Output Compare Register for 50Hz (once every 20ms)
    
    // set PIN CHANGE INTERRUPT
    PCICR |= 1<<PCIE1 | 1<<PCIE2;        // enable pin change interrupts PC0-5 (PCI1) & PD3-4 (PCI2)
    PCMSK1 |= 1<<PCINT13 | 1<<PCINT12 | 1<<PCINT11 | 1<<PCINT10 | 1<<PCINT9 | 1<<PCINT8;    // enable pins PC0-5 (PCI1)
    PCMSK2 |= 1<<PCINT20 | 1<<PCINT19;                                                        // enable pins PD3-4 (PCI2)
    
    // enable interrups
    sei();    
    
    // do nothing and repeat - interrupts do all the work
    for(;;);
}

For detailed explanation, follow the comments in the code.

Under Linux, program the fuse bits (includes the settings for a 20 MHz crystal) like so:

sudo avrdude -c avrispmkII -P usb -p m328p -U lfuse:w:0xd6:m -U hfuse:w:0xd9:m

And flash the firmware like so:

sudo avrdude -c avrispmkII -P usb -p m328p -U flash:w:main.hex

But for the module to be useful, you need to connect it with FlightGear. And you do this with defining your own input protocol:

<?xml version="1.0"?>
<PropertyList>
    <generic>
        <input>
            <binary_mode>false</binary_mode>
            <var_separator>tab</var_separator>
            <line_separator>newline</line_separator>
            <preamble></preamble>
            <postamble></postamble>
            <chunk>
                <name>throttle</name>
                <type>float</type>
                <node>/controls/engines/engine/throttle</node>
                <factor>0.625</factor>
                <offset>0.5</offset>
            </chunk>
            <chunk>
                <name>aileron</name>
                <type>float</type>
                <node>/controls/flight/aileron</node>
                <factor>1.25</factor>
            </chunk>
            <chunk>
                <name>elevator</name>
                <type>float</type>
                <node>/controls/flight/elevator</node>
                <factor>1.25</factor>
            </chunk>
            <chunk>
                <name>rudder</name>
                <type>float</type>
                <node>/controls/flight/rudder</node>
                <factor>1.25</factor>
            </chunk>
            <chunk>
                <name>junk</name>
                <type>float</type>
            </chunk>
            <chunk>
                <name>junk</name>
                <type>float</type>
            </chunk>
            <chunk>
                <name>junk</name>
                <type>float</type>
            </chunk>
            <chunk>
                <name>junk</name>
                <type>float</type>
            </chunk>
        </input>
    </generic>
</PropertyList>

Set up, to read the output from the module and map the first four to aircraft controls. Pay attention to the modification factors!

For rudder, elevator and ailerons, the scaling factor for me is 1.25, since my RC controller doesn't output full 1-2ms range (1.5ms is still zero), but slightly less, which results in the module output from -0.70 to 0.70. Which is far left and right for me in the real world, but FlightGear wants the extreme positions to be from -1.0 to 1.0. Yours might vary!

And the engine factor and offset? Same story with the addition that FlightGear accepts engine input from 0.0 to 1.0. So scale the -0.7 to 0.7 output to -0.5 to 0.5 ad the add 0.5 to get the 0.0 to 1.0 input. Again, yours might vary!

Now save the protocol file as "RC2USB_in_controls.xml" in the appropriate location and run FlightGear like this:

fgfs --generic=serial,in,25,/dev/ttyUSB0,57600,RC2USB_in_controls

 
And how does it perform? See for your self:

Servo tester part with auto generated signal (LED light for center) and pa pass-trough from the eight RC connector for manual testing. No power needed, just plug in the USB cable.

Controlling the plane in FlightGear with your RC controller! Works on RC models to Boeing 787 (just donwload more models for FlightGear). Happy practising!

P.S. Source code formatted with http://formatmysourcecode.blogspot.com