Using XBox One Controller for command input

One of my requirements was to achieve fully autonomous flight. It dawned to me that before I get there, I'll probably need to manually control the quad to perform flight tests and improve the control subsystems. Hence I need a controller, but I don't want to invest in a costly radio-controlled solution.

I already have WiFi onboard the Pi. I have an Xbox One Controller. And I already need to develop a control app to run on my computer. I'll simply interface with the Xbox One controller, which is extremely versatile.

The control mapping will surely change, but it could look like the above.

• Throttles: control motor throttle in manual mode, control altitude in altitude stabilization mode
• Select: switch between manual, altitude stabilized, position hold and fully automated
• Start: start/stop onboard control
• Right joystick: adjust yaw
• Left joystick: adjust pitch (up / down) and roll (left / right)
• A-button: automated lift-off and landing activation

The controller can be plugged into any Windows system with a USB cord and is instantaneously recognized. Plus there are DirectX APIs to interface with it. Since I'll be developing in C# on the Windows side, I'll be using the SharpDX.XInput library, a convenient managed wrapper around the DX native XInput APIs.

I'm using Visual Studio Community 2015. In the package manager consoler, install the latest beta version of SharpDX.XInput

Install-Package SharpDX.XInput -Pre

I'm using the beta version as the latest stable release is based on DX11 and I ended up with missing DX DLLs (Windows 10 comes with DX12).

Once you have that imported, getting a Controller object is quite simple.. In the example below, in a simple Form app, when I update the text field with the X/Y coordinates of the left joystick anytime the form button is pressed.

using SharpDX.XInput;

namespace QuadRemote
{
    public partial class Form1 : Form
    {
        public Controller _controller;

        public Form1()
        {
            InitializeComponent();
            _controller = new Controller(SharpDX.XInput.UserIndex.One);
        }

        private void button1_Click(object sender, EventArgs e)
        {
            if (_controller.IsConnected)
            {
                var state = _controller.GetState();
                var x = state.Gamepad.LeftThumbX;
                var y = state.Gamepad.LeftThumbY;

                richTextBox1.Text = x.ToString() + " " + y.ToString();
            }
        }
    }
}

Pulse Width Modulation (PWM) with PCA9685

The theory

PWM (Pulse Width Modulation) is a modulation technique where the duration of a signal pulse is used to encode information. In the case of a quadcopter it's usually used to modulate the rotational speed of the motors/propellers. The signal is periodic and the duty cycle represent the proportion of the period where the signal is "on" (logical 1).

To generate such a signal, I'll be using the NXP PCA9685 chip on a pre-assembled Adafruit PCB module. The output channels will eventually be connected to ESC circuits (electronic speed controllers) that will drive the motors with a higher voltage/amperage than the Pi or the PCA9685 could deliver.

The signals could be generated on the GPIO pins of the Pi, but there is only 1 PWM dedicated hardware pin. Since we need 4 PWM signals (one for each motor), three of the signals would need to be generated in software - which would gobble up a lot of CPU power at high frequencies. It's therefore better to use a dedicated IC like the PCA9685.

A longer pulse width will increase propeller rotational speed. A shorter pulse width will decrease the propeller rotational speed. Some baselining and calibration will be required when I get there. For the moment I want to make sure that I can properly interface with the circuit and generate a PWM signal on any output pins.

What we need

You can find the NXP PCA9685 specification here. There are 3 things I need to code:

1. Initialization routine of the PCA to make sure the IC is running in the proper mode and is ready to go.
2. Function to modify the PWM frequency
1. The IC has an internal oscillator (clock) of 25 MHz, but the output frequency of the PWM signal can only be set between 24Hz and 1526Hz
2. To adjust the PWM frequency, a special register is used (prescale)
3. The output frequency will need to be set according to what can be taken as input in my future ESC motor drivers (usually maximum of 500Hz according to what I gathered so far)
3. Function to modify the duty cycle of the PWM signal
1. The duty cycle will be changed according to the input throttle value - i.e. the speed at which we want the motors to spin

By the way, the duty cycle doesn't have to start exactly at the beginning of a period. You can have a delay before the duty cycle starts. That's why the PCA9685 allows you to define both an ON and an OFF value. The circuit uses 12 bits registers for those. That means that each period is divided in 4096 sub intervals.

The ON value will specify the interval when the signal goes to "1", the OFF value will specify the interval when the signal goes back to "0". For example: ON = 0 and OFF = 2048 means that the signal will be active for the first 50% of the period.

You'll see that I'm using the I2C interface I coded in the previous blog post to send the commands.

this->devAddress was initialized to 0x40 (address of the PCA9685 on the I2C bus).
All the registers and bit masks are defined in an enum (e.g. ALL_LED_ON, MODE2, etc.) and the names are fully aligned with the NXP spec.

Initialization routine

The init routine does three things:

1. Turn off all output channels (effectively setting a duty cycle of 0%)
1. Writes 0 to the ALL_LED_ON and ALL_LED_OFF registers
2. Set the right modes of operations
3. Restart the module

As per the spec, we need to wait 500us after some operations to make sure the oscillator stabilizes.

int PCA9685::init() 
{ 
 debug_print("[PCA9685] Init...\n");
 debug_print("[PCA9685] Turning off all PWM channels\n");
 this->pBus->WriteRegisterByte(this->devAddress, ALL_LED_ON, 0);
 this->pBus->WriteRegisterByte(this->devAddress, ALL_LED_ON+1, 0);
 this->pBus->WriteRegisterByte(this->devAddress, ALL_LED_OFF, 0);
 this->pBus->WriteRegisterByte(this->devAddress, ALL_LED_OFF+1, 0);
 
 debug_print("[PCA9685] Set mode registers\n");
 // configure totem pole structured output
 this->pBus->WriteRegisterByte(this->devAddress, MODE2, OUTDRV);
 // respond to ALL_LED register changes 
 this->pBus->WriteRegisterByte(this->devAddress, MODE1, ALLCALL);
 
 usleep(5000); // wait for oscillator
 
 debug_print("[PCA9685] Reset\n");
 int mode = this->pBus->ReadRegisterByte(this->devAddress, MODE1);
 this->pBus->WriteRegisterByte(this->devAddress, MODE1, mode & ~SLEEP);
 
 usleep(5000); // wait for oscillator
 
 return 0;
}

Set the PWM frequency

We need a routine to modify the periodicity of the PWM signal. This is done by tweaking the prescale register value according to this formula (from the NXP spec).

The frequency of the internal clock is 25MHz. The IC also accepts an external clock up to 50MHz.
The refresh rate will be set according to what we need to drive the motors ESC - this will be the function's input parameter (e.g. 250 Hz)

int PCA9685::setPWMFequency (int frequency)
{
 debug_print("[PCA9685] Setting PWM frequency @ %dHz...\n", frequency);
 int prescale = floor((PCA9685_CLOCK_HZ / (float)(PCA9685_RESOLUTION * frequency)) + 0.5) - 1;
 
 debug_print("[PCA9685] Prescale @ %d...\n", prescale);
 
 // preserve original mode
 int mode = this->pBus->ReadRegisterByte(this->devAddress, MODE1);
 
 // disable restart, sleep, set prescale and reset
 this->pBus->WriteRegisterByte(this->devAddress, MODE1, (mode & ~RESTART) | SLEEP);
 this->pBus->WriteRegisterByte(this->devAddress, PRESCALE, prescale);
 this->pBus->WriteRegisterByte(this->devAddress, MODE1, mode);
 
 // wait for oscillator
 usleep(5000);
 
 // enable restart
 this->pBus->WriteRegisterByte(this->devAddress, MODE1, mode | RESTART);
 
 return 0;
}

Set the PWM duty cycle

This function takes 3 parameters: the channel id (between 0 and 15 as the PCA9685 has 16 output pins), and the interval values for when to turn the PWM signal on and off during a period (between 0 and 4095).

The channel id is used to determine what registers to write to as an offset from the base LED0_ON register at the bottom of the addressable register space.

int PCA9685::setPWM (int channel, int on, int off)
{
 int reg = LED0_ON + 4*channel;
  
 this->pBus->WriteRegisterByte(this->devAddress, reg, on & 0xFF);
 this->pBus->WriteRegisterByte(this->devAddress, reg+1, (on & 0xFF00) >> 8);
 this->pBus->WriteRegisterByte(this->devAddress, reg+2, off & 0xFF);
 this->pBus->WriteRegisterByte(this->devAddress, reg+3, (off & 0xFF00) >> 8);

 return 0;
}

All of the code is on bitbucket.

Performance

For the sake of making sure that running this on the Pi is performant enough, I've measured the elapsed time to set a new PWM duty cycle 5000 times in a loop.

I obtained 770 times per second. Assuming that the routine has its own thread running on one of the four cores, that means that in the best case scenario I can update the speed of all 4 motors 193 times/s, half the maximum input frequency of some ESC circuits I've looked into.  That frequency will be lower once we add the PID control loops....

Hopefully that will be enough. I've read on a forum that we need at least 50Hz for marginal performance and flight stability. I'll look into optimizing the I2C interface code just to make sure.

UPDATE

I've optimized the code to achieve 2500 Hz in PWM duty cycle setting instead of 770Hz.

I've done two things to achieve this:

1. Used the PCA9685 register auto increment feature. It allows me to write an array of bytes to a base register, and the address will auto increment after each byte.
2. Removed writing the "On" values. Since I don't need a delay before the start of the duty cycle, it can always be 0 and left untouched. I only write the "Off" values to modify the duty cycle length.

UPDATE 8/24

I've been trying to increase the performance of those routines and figured out that I'm bumping into a bottleneck: the I2C bus clock frequency.
The default frequency on the Pi is 100kHz - but it's possible to tweak it !
I've cranked it to 400kHz and now I've achieved 8.3kHz in updating the PWM.

sudo nano /boot/config.txt

Add this line:

dtparam=i2c1_baudrate=400000

Making the code public

I've added a mutex using the POSIX pthread librairy to make sure the I2C address selection and IO operations are safe in a multi-threaded environment.

And I'm also making the code public on BitBucket.

https://bitbucket.org/sygnet/quadrasppi_public/src

I2C interface class in C++

Now that I have my PCA9685 I2C module properly connected to the Pi I2C bus comes the time of interfacing with it.

I decided to have a pure virtual IBus interface that could be used no matter what the underlying implementation is. The bus doesn't even have to be I2C, could be serial, or whatever. This way you just pass a pointer to the bus object to any routine that needs to communicate on the bus and it just calls pretty generic functions to read/write data to a device address/register pair (the register value could even be abstracted to be just a control command).

Here's what the class looks like:

class IBus
{
protected:

 bool debug;
 
public: 
 IBus (bool debug = false) {
  this->debug = debug;
 }
 virtual int ReadRegisterByte (int addr, int reg)= 0;
 virtual int WriteRegisterByte (int addr, int reg, int data) = 0;
};

The Raspian I2C implementation class derives from it.

class I2CRaspbian: public virtual IBus
{
private:
 
 int fd; // file descriptor 
 int selectedAddr; // currently selected device address
 int SelectAddr (int addr); // change device address selection
 
public:
 
 I2CRaspbian (int port, bool debug = false);
 virtual int ReadRegisterByte (int addr, int reg);
 virtual int WriteRegisterByte (int addr, int reg, int data);
};

The Linux/Raspbian implementation of I2C relies on the concept of device objects that are accessible like all files on the file system through read/write/IOCtl commands. You just need to have the right modules loaded in memory (that's what we did in the last post) and the right headers files to start coding.

#include <errno.h>
#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <linux/i2c-dev.h>
#include <sys/ioctl.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>

The libi2c-dev module we installed contains user mode SMBUS function implementation that would normally only be available to a driver in kernel mode - the implementation relies solely on reading/writing through IOCTL ops. This is required to read from a device register. You don't need it if you only want to read data from a register-less device.

The I2C bus device file is opened in R/W access in the class constructor. The port number depends on your Pi revision.

I2CRaspbian::I2CRaspbian(int port, bool debug) : IBus(debug)
{
 // no address currently selected
 this->selectedAddr = 0; 

 // generate I2C device filename based on port number
 char filename[40];
 sprintf(filename, "/dev/i2c-%i", port);
 debug_print("Opening I2C channel on %s\n", filename);

 // open a the device file in R/W
 if ((this->fd = open(filename, O_RDWR)) < 0) {
  printf("Failed to open the bus.\n");
  exit(1);
 }
}

Before writing to or reading from a device, we need to select its address on the bus using IOCTL commands. The I2C_SLAVE is defined in the header we got when installing i2c-dev.

int I2CRaspbian::SelectAddr (int addr)
{
 // make sure to change the device address selection if its different from
 // the previous I/O
 if (this->selectedAddr != addr) 
 {
  debug_print("Changing device address selection.\n");
  if (ioctl(this->fd, I2C_SLAVE, addr) < 0) {
   printf("Failed to acquire bus access and/or talk to slave.\n");
   exit(errno);
  }
  this->selectedAddr = addr;
 }
 return 0;
}

To read from a device register, use the SMBUS interface (translate the read into an IOCTL):

int I2CRaspbian::ReadRegisterByte (int addr, int reg){

 // Select device
 SelectAddr(addr);

 // Read data using SMBUS interface
 int data = i2c_smbus_read_byte_data(this->fd, reg);
 debug_print("Read data from reg 0x%02X: 0x%02X\n", reg, data);
 
 return data;
}

To write to a device register, you can use a standard write command:

int I2CRaspbian::WriteRegisterByte (int addr, int reg, int data) 
{
 // Select device
 SelectAddr(addr);

 // Set buffer to write
 char buf[2] = {0};
 buf[0] = reg;
 buf[1] = data;

 // Write data using standard file write interface
 debug_print("Write data to reg 0x%02X: 0x%02X.\n", reg, data);
 if (write(this->fd, buf, 2) != 2) {
  printf("Failed to write to the i2c bus.\n");
  return errno;
 }

 return 0;
}

Important Note: Due to my implementation where a single pointer to a I2C class object is expected to be used everywhere in the code, we need to pay attention to the case where the process is multithreaded. If two threads are trying to access different devices on the bus, then we must insure that the selected address doesn't change (modified by a parallel thread) until the I/O is performed on the bus. To do this, I'll implement a POSIX mutex to make sure the Address Selection + IO operation becomes atomic.

Setting up the development environment

My workstation runs Windows 10 and my Linux college days were a long time ago, so I'd like to do the C++ software stack development on Windows. Additionally, based on what I gathered reading blogs and forums, the recommended environment for C++ on the Pi is NetBeans.

Since cross-compiling for ARM on my Intel box is non-trivial, the approach I've chosen is to develop on Windows using NetBeans and use the Pi as a compilation host using SSH tunneling. This way all binaries are compiled and built natively on the target execution platform. No hassle ! :-)

I also want to check my code on a cloud repository for backup, share the code and get contributions from my electrical engineer friend.  I decided to go with BitBucket as it's free and Git-based. NetBeans natively supports Git so linking the project to BitBucket is quite simple and I'll skip documenting this step.

Step 1 - Enable Root login for SSH

You first need to tweak your Pi's SSH configuration a little to allow root (like admin account on Windows) login. Root permissions will be required to achieve the next steps.

On the Pi's console, first set a password for the root user:

sudo passwd root

You should now be able to login as the root user using Putty. If it doesn't work, check the SSH daemon configuration to make sure it allows root login.

sudo nano /etc/ssh/sshd_config

Make sure you have this line somewhere in the file:

PermitRootLogin yes

Restart the daemon if you've change the config file:

sudo /etc/init.d/ssh restart

If you need to remove the root login at a later time (for security purposes), run this command:

sudo passwd -l root

Step 2 - Install and Configure

I downloaded NetBeans 8 from https://netbeans.org/downloads/ (get the one that says C/C++) and proceeded to install it on my workstation.

Once in NetBeans and a new project is created, you'll need to define the Raspberry Pi compilation host settings. Before you proceed further, make sure your Pi is up and running, connected to your network and that you have its IP address noted.

Navigate to the Services tab in the left window pane. Right-click on the C/C++ Build Host node and select Add new host.

Enter the IP address of the Pi in the hostname. The default SSH port is 22. Change the port number if you've modified the SSH default configuration.

Enter root as the login and select password as the authentication method, then click next.

You'll be asked to enter the password you've previously created for root. 

Once you click Ok, NetBeans will start configuring the Pi to handle the build process.

In NetBeans, click the compile button and make sure that the compilation process completes successfully. You should see BUILD SUCCESSFUL in the console window.