Ground Station for Self-Driving Car

I had to do quite a lot of field testing while working on the software for my Self-Driving RC Car and one of the frustrations I had was not being able to observe key code variables and sensor info in realtime while the car was actually driving.  I spent a lot of time putting an LCD-based info display on the car, but I could only use this to view and change settings before and after a test run.  I tried adding remote telemetry using XBee modules, but the signal tended to get jammed the minute I turned on the RC transmitter I used to manually take back control of the car when the autonomous software invariably hiccups.  I consider various solutions to this problem, such as using 900 MHz XBee modules, or some other kind of telemetry link, but never got as far as implementing these ideas and resorted, instead, to analyzing the log file after I'd completed a series of fields tests.

Then, a few weeks back, I stumbled upon a new type of RC control transmitter module made by a company called FrSky that advertised a two-way control system that provided a way to receive data back from the RC "receiver".  After a little more research, I found several open source projects that were hacking these TX modules, and the companion transmitter units to display this remote telemetry on the transmitter's built-in LCD display.  One such project, TH9X, is based on an inexpensive transmitter unit sold under various brand names, such as the Turnigy 9X. The Turnigy 9X doesn't come equipped with two way capabiities as a standard feature, but it's designed to have its transmitter module replaced, so it can be fitted with an FrSky TX module that supports two-way telemetry.  With a  bit of careful shopping, I was able to get the Turnigy 9X and the FrSky TX module, and companion telemetry-enabled receiver for about $120, total price.

In addition to being fairly inexpensive, the FrSky system is also makes for a rather impressive RC control setup, as the matching, telemetry-enabled  receiver I chose, the D8R-II Plus, is a full diversity receiver (inside, there are two receivers that select for best signal) that uses ACCST (Advanced Continuous Channel Shifting Technology) which shifts the frequency hundreds of times per second.  FrSky claims that this means there is no signal conflicts and interruptions.  I have yet to fully verify this claim, but I have read some good reports about it.  I did run some informal range tests, but more about those in a later post.

The Plan

My idea was fairly simple; the Turnigy 9X transmitter has a fairly nice 128x64 pixel graphics LCD display, as well as a set of control buttons (cursor, select, exit) on the front panel.  Together, these would let me to create a simple way to process and review realtime telemetry data from the car using a set of status panes I could select using the  front panel controls.  In addition, I could probably even draw simple graphs, or graphical indicaors on the LCD display.  All I have to do is write the code.  The ATMega64 is not supported by the Arduino environment, but it's based on the same 8 bit AVR architecture as the ATMegas used by the Arduino, so I knew the same toolchain could compile code for the ATMega64, too.  Then, rather than using a bootloader to upload code, I could use the hardware ICSP port to program the ATMega64.  It all seemed doable, so I opened up the transmitter and started hacking...

Step 1: Add an ISCP Connector to the Transmitter

The Turnigy 9X internally uses an ATMega64 to perform all it's RC functions.  In fact, other than the removable TX module, the internal electronics consist mostly of just this AVR processor combined with a 128x64 pixel graphic LCD and a collection of buttons, switches, pots wired up as inputs.  So, essentially, everything it does is defined by software.  but, in order to reprogram the ATMega64 with my own code, I first needed to add a ICSP programming port.  There are a bunch of step by step articles on-line in the TH9X forums about how to do this, but here's a list of the connections needed to use the 6 Pin ICSP socket:

The following image shows how I was able to make these connections using fine, 30 gauge wire wrap wire to connect to test pads that seemed to be conveniently designed into the board for this purpose:

However, I found that I also needed to make additional changes before the ICSP programmer would work.  In the original board design, each IO pin is connected to an RC network that consists of a 200 Ohm resistor that is then connected Gnd via a .1 uFd ceramic cap (you can see these components on the image above.)  This RC network introduced just enough load that my AVRICSP MKII programmer could not reliably talk to the ATMega64.  So, I removed the .1 uFd caps from the MISO, MOSI and SCK lines and, to avoid problems that migth result should the switches these I/O line connect to be in the wrong position, I also replaced the 200 Ohm reistors with 10K resistors on the same lines.  In addition, I also needed to removed the large, 47 uFd cap from the RESET line.  I was concerned that this might cause a problem with the normal reset of the processor but, so far, it's worked fine.  Note: the above image shows the PCB before these components were removed, or changed.  The image below shows where to replace, or remove components:

Step 2: Write Some Code

The hardest part of this project turned out to be trying to unravel the complex TH9X code so i could understand how to replace it with my own, much simpler code.  People using the Turnigy 9X use it to fly planes and helicopters and these need all sorts of advanced features, such as servo mixing, that I just don't care about, but which add a lot of complexity to the code.  But, after a bit of study, I was able to pick out the essential portions I wanted, which includes code to:

• • Generate the PPM Pulse sequence that sends servo signals to the receiver

  • Write text and graphics on the LCD

  • Read and debounce buttons

  • Read analog inputs from the joysticks

  • Use USART 0 to receive and process async serial telemetry data (more on ths later)

The first big mystery to unravel was how the ATMega64 generated the PPM signal that sends servo position infromation through the TX module.  Fortunately, this is a rather standardized process, so I was able to find some good documentation about how this works.  In effect, the processor needs to generate a sequence of pulses like this:

Where the timing between pulses, such as Ch1, Ch2, etc. varies in proportion to the servo position desired.  For example, the width of the gap after the Ch1 pulse will be 1.0 ms when the servo is centered and will shorten to .5 ms, or lengthen to 1.5 ms as the servo moves from one end position to another.  The TH9H code creates this pulse train by using Timer 1 in CTC (Clear Timer on Compare) mode and setting it to count at a rate of 2 MHz.  The initialization code to do this looks like this:

  // Setup Timer 1 to count at 2 MHz rate for RC pulse generation

  TCNT1 = 0;                                       // Timer 1 initial count = 0

  TCCR1A = 0;                                      // Clear Timer 1 on Compare Match (WGM12:11:10 = 4)

  TCCR1B = (1<<WGM12) | (1<<CS11);                 // Divide 16 MHz by 8 (CS12:11:10 = 2)

  // Critical: must set OCR1A *after* setting TCCR1A and TCCR1B or timer 1 will misbehave

  OCR1A = 18000;                                   // Compare register A = 3000 (interrupt every after 1.5ms)

  TIMSK |= (1 << OCIE1A);                          // Enable Compare Match Interrupt (TIMER1_COMPA_vect)

In CTC mode, Timer 1 will reset the time count to zero and interrupt the processor when the value in compare register A matches the current count so, to generate the proper pulse sequence, all the interrupt code has to do is toggle the output bit and update the value in compare register A from an array of preset values after each interrupt until a complete "frame" of pulses is generated.  Then, after each frame is complete, it can update the appropriate servo channel values from an array of values taken from the ADC converter (using another interrupt task I'll describe next.)  Finally, here's the code for the interrupt handler:

volatile unsigned char pdx = 0;

volatile unsigned int pulses[18] = {1000, 2000, 1000, 2000, 1000, 2000, 1000, 2000,

                                    1000, 2000, 1000, 2000, 1000, 2000, 1000, 2000,

                                    1000, 18000};

long map (long x, long in_min, long in_max, long out_min, long out_max) {

  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;

}

ISR (TIMER1_COMPA_vect) {

  OCR1A = pulses[pdx++];                        // Update Timer 1 compare value

  PORTB ^= 0x01;                                // Toggle pulse output pin

  if (pdx >= sizeof(pulses) / 2) {

    pdx = 0;

    // Update 4 joystick channel values from ADC array (values 0-1023)

    unsigned int accum = 0;

    accum += pulses[1] = map(adcValues[0], 0, 1023, 1000, 3000);

    accum += pulses[3] = map(adcValues[2], 0, 1023, 1000, 3000);

    accum += pulses[5] = map(adcValues[1], 0, 1023, 1000, 3000);

    accum += pulses[7] = map(adcValues[3], 0, 1023, 1000, 3000);

    // Set other 4 channels to 1.5 ms neutral position (placeholder)

    accum += pulses[9]  = 2000;

    accum += pulses[11] = 2000;

    accum += pulses[13] = 2000;

    accum += pulses[15] = 2000;

    // Compute value for sync pulse to maintain 50 Hz frame rate

    pulses[17] = 40000 - accum;

  }

}

Values from the joysticks and the trim pots are read by setting up a periodic ADC interrupt.  The initilization code to do this is:

  // Setup periodic ADC interrupt

  ADMUX = (1 << REFS0);

  ADCSRA = (1<<ADEN) | (1<<ADPS0) | (1<<ADPS1) | (1<<ADPS2) | (1<<ADSC) | (1 << ADIE);

  adcChan = 0;

and the corresponding ADC interrupt handler looks like this:

volatile unsigned char  adcChan;

volatile unsigned int      adcValues[8];

ISR (ADC_vect, ISR_NOBLOCK) {

  adcValues[adcChan] = ADC;                     // Read ADC Value

  ADCSRA = 0;                                   // Reset adconv to force 25 cycles

  adcChan = (adcChan + 1) & 0x7;

  ADMUX = adcChan | (1 << REFS0);

  ADCSRA = (1<<ADEN) | (1<<ADPS0) | (1<<ADPS1) | (1<<ADPS2) | (1<<ADSC) | (1 << ADIE);

}

Notice that ISR_NOBLOCK is used to allow the pulse generation interrupt to interrupt the ADC handler interrupt.  Thsi is done to reduce jitter on the pulse train timing that could result in servo chatter.

Step 2: Install the FrSky TX module

The code listed above will, effect, give you a very basic, 4 channel RC control transmitter and should function with the one-way TX module and receievr that comes with the Turnigy 9X kit.  However, the whole purpose of this exercise is to get telemetry data back from the remote receiver and, before we can do that, we have to make some additional hardware changes to the FrSky TX module and to the Turnigy 9X transmitter.  First, we need to free up the RX input line to USART0 in the ATMega64.  In the Turnigy 9X, this line (which is pin 2 on the ATMega64) is connected to the TCut (throttle cut) switch, which is rearmost toggle switches on the top left side of the transmitter case.  To free up this line, I simply unsoldered the wires at the switch, but you could rewire it to one of the unused pins on the ATMega64, such as bits 0 or 7 of PORTC.

The telemetry receive signal is available from an external connector on the FrSky TX module but, unfortunately, this signal is an inverted, RS-232 +/- polarity swing signal, not a 5 volt logic level signal like we need to connect to the USART0 RX pin on the ATMega64.  However, the non inverted, logic level signal is available on the PCB inside the TX module.  And, there are two free pins available on the 5 pin connector that's used to connect the TX module when its plugged into the back of the transmitter.  I chose to use the bottom most pin, as the other pin is shorted to ground inside the transmitter.  The first step is to open up the FrSky TX module and gently pry out the curcuit board (note: mine was glued on one end, so this took a bit of careful cutting with an xacto knife to free it.)  Once it's out, install a jumper like the one shown in the picture below.  The connection to the PCB is made by gently scraping away the solder mask to expose the copper trace below it, then soldering to it with some 30 gauge wire wrap wire.  Connect the other end to the lower pin of the connector, check continuity from the connector pin to pin xx of the xx IC on the reverse side of the board.

Click image for larger view

As this point, the reassembled transmiter module should be ready to plug into the transmitter case, but I found that the module would not fit in all the way due to a plastic protrustion around the pins that connect to the TX module not matching up with the space available around the matching connector in the TX module.  My solution was to use my xacto knife to expand the cutout around the connector in the TX module case before I reassembled it:

After the TX module is installed, you'll need to add an additional jumper wire to the PCB in the back of the transmitter case.  This connects from the lower pin coming from the TX module to the line formerly used to bring in the signal from the TCut toggle switch.  The lower pin is connected to a section of the PCB that seems to be unused on my version of the Turnigy 9x, but I went ahead and cut the pin free, just to be sure. The result should look like this:

Step 3: Sending Telemetry Data to the FrSky D8R-II Plus Receiver

The FrSky D8R-II Plus receiver has a port on the side of the receiver where you can connect up to two voltage inputs and another RX input that a accepts async serial data at 9600 baud (subject to some rate limits I'll describe later.)  However, in another odd design decision, the serial RX input to to the D8R-II Plus is designed to take an inverted, RS-232-type signal, so it can't be connected directly to a standard, logic level output TX signal from a UART.  Some microcontrollers provide a way to invert the UART signals but, as I wanted to generate test data from an Arduino, I chose to wire up a simple inverter circuit using two resistors, and a 2N3904 NPN transistor, as show below:

Now, in theory, I was ready to send telemetry data back to the transmitter, but I hadn't yet written code for the ATMega to receive and process this data.  So, as a first step, I wired up another inverter circuit like the one shown above and used it to connect the TX signal available on the back of the FrSky TX module to the Arduino's RX pin.  In this way, I could write cdoe that would send data through the telemetry link and then back to the Arduino.  But, before I tried to send anything, I wanetd to see what kind of telemetry data the receiver was sending on its own (I figured it had to be because it had the two inputs for monitoring battery voltage.)  Using a simple Arduino Leonardo sketch to echo out the bytes received at 9600 baud in hex format (note: I chose the Leonardo because its hardware serial port is available, but you could probably use SoftwareSerial, too), I was able to determine that the receiver, by itself, continually spits out a series of 11 byte data packets like this:

    7E FE 11 22 33 44 00 00 00 00 7E     

where bytes 3-6 seem to change with each frame.  Then, after some more research, I found a document, which seemed to confirmed my analysis.  In addition, the document described a "byte stuffing" scheme used to avoid having bytes inside a packet that used the reserved packet sync code of 0x7E (each packet begins and ends with this reserved byte code.)  To avoid this, the protocol requries receiving software to look for the byte value 0x5D and when found, ignore this byte but XOR the value of the following byte with 0x5D.  The document further described how the 3rd bytes of the packet sent by the receiver is the voltage for receiver port 1, the 4 byte is the voltage for port 2, the 5th byte indicates the uplink quality (using a value from 40-110) and the 6th byte indicates downlink quality.  I'm not sure how the voltage values are scaled, but I'll leave this for future investigation. 

FrSky Receiver test setup

However, the document also indicated that data sent into the receiver's RX serial port would also be formatted into 11 byte packets, byte with a 0xFD value in the 2nd byte and a length of data value in the 3rd byte.  The 4th byte contains a module 16 sequence number and, finally, the data is packed into bytes 5-10, for a maximum of 6 data bytes per packet.  Apparently, these packets are generated very 36 ms, or so, and whatever data has been received by the RX input is packed into a pack of this format and sent along,  This means that some of the slots for data bytes will be unfilled in some packets.  So, I altered my Arduino sketch to send a sequence of 3 ASCII digits, plus a CR and LF every 100 ms, or so (based on a recommendation in the document referenced above.)  Then looking at the output from the transmitter, I saw another type of packet show up, one formatted exactly as the document had described.  Here's a sample sequence:

    7E FD 01 01 0A 32 0D 0A EB 00 7E     Packet containing 1 byte of data (0x32, or '2")

    7E FD 04 02 33 30 0D 0A 79 3D 7E     Packet with 4 bytes of data (0x33, 0x30, 0x0D, 0x0A, or "30\n\r")

    7E FD 04 03 33 31 0D 0A 0F 65 7E     Four more bytes of data

    7E FD 04 04 33 32 0D 0A AC EB 7E     And 4 more...

Step 4: Displaying the Telemetry Data on the Transmitter's LCD

So, while this was starting to get a bit more complex than I had first hoped it would be, I believed my scheme for displaying telemetry data from and autonomous vehicle still had potential, so I pressed on.  The next step was to create code to receive and process the telemetry packets inside the transmitter.  To do this, I needed to put the ATMega64's UART0 into receive mode and configure it to interrupt each time a byte of data was received.  The init code to do this is:

  // Setup USART Rx0 for Telemetry reception

  UBRR0H = BAUD_PRESCALE >> 8;                             // Set baud rate High Byte

  UBRR0L = BAUD_PRESCALE & 0xFF;                           // Set baud rate Low Byte

  UCSR0C |= (1<<UCSZ0) | (1<<UCSZ1);                       // 8-bits, no parity

  UCSR0B |= (1<<RXEN0);                                    // Enable Rx0 reception

  UCSR0B |= (1<<RXCIE0);                                   // Enable Rx0 Interrupt

Next, here's code for a simple interrupt handler that processes all the received packets bytes, implements the required byte stuffing, ignores packets coming from the receiver (although I eventually plan to process them, too) and then looks for LF-terminated text strings inside the data bytes:

volatile bool unstuff = false;

volatile unsigned int rxBuf[16];

volatile unsigned char rxdx = 0;

volatile unsigned char lastCc = 0;

volatile unsigned char msg[16];

volatile unsigned char msgIdx = 0;

volatile bool msgAvail = false;

ISR (USART0_RX_vect) {

  unsigned char cc = UDR0;

  if (unstuff) {

    cc ^= 0x5D;

    unstuff = false;

  }

  if (cc == 0x5D) {

     unstuff = true;

  } else {

    if (lastCc == 0x7E and cc == 0x7E) {

      // process incoming packet

      if (rxBuf[1] == 0xFD) {

        // data bytes are in rxBuf[4] - rxBuf[4 + len - 1]

        unsigned char len = rxBuf[2];

        for (unsigned ii = 0; ii < len; ii++) {

          unsigned char tmp = rxBuf[4 + ii];

          if (tmp >= 0x20) {

            if (msgIdx < (sizeof(msg) - 1)) {

              msg[msgIdx++] = tmp;

              msg[msgIdx] = 0;

            }

          } else if (tmp == 0x0A) {

             msgIdx = 0;

             msgAvail = true;

          }

        }               

      }

      rxdx = 0;

      if (rxdx < sizeof(rxBuf))

        rxBuf[rxdx++] = cc;

    } else {

       if (rxdx < sizeof(rxBuf))

         rxBuf[rxdx++] = cc;

    }

    lastCc = cc;

  }

}

Received text messages are accumulated in the 16 byte msg[] buffer and, each time a LF is processed, the msgAvail flag is set.  I won't try to explain all the LCD-related code here, as it's all fairly standard and well covered in othe places, but here's the bit of code I added inside the main() loop which displays the received msg on the Turnigy's display:

        if (msgAvail) {

          msgAvail = false;

          lcd_clear();

          unsigned char col = 2;

          for (unsigned char ii = 0; ii < sizeof(msg); ii++) {

            if (msg[ii] < 0x20)

              break;

            lcd_putc(2, col++, msg[ii]);

          }

          refreshDisplay();

        }

For further details on the code, such as how to read and debounce button presses, draw boxes and lines on the LCD, and other details, please refer to the source code available at the bottom of this page.  This code is still mostly a rough testbad, but I'll update the code as I progress to my final design.