Remote Station & Gamepad Control

In order to control the quadcopter with the Logitech gamepad, a laptop computer was used as an intermediary to take inputs from the controller and send them via serial to the remote station Arduino. Similarly to Project 1, a simple python script made reading the controller inputs easy.

Gamepad Python Script

For copter controls on the gamepad, we used a similar scheme to the one used by Aeroquad for simplicity.  The gamepad controls worked according to this diagram:

Since the throttle movement on the gamepad always returns to center, however, we used the right and left triggers to adjust throttle positions instead of the joystick.  Sometimes the joysticks become stuck slightly in one direction.  To fix this, the python script used a range of values to determine a neutral position.  The script is shown below.

'''
basic_control.py
@author: Garrett Owen
@date: April 27, 2012

    for python verson 2.7.1
'''

import serial
import pygame
import time
import struct

serialPort = '/dev/tty.usbmodemfd1221'       # Arduino Uno
baudRate = 38400

# Open Serial Connection to Arduino Board
ser = serial.Serial(serialPort, baudRate, timeout=1);

'''
Gets joystick data and prints it
'''
pygame.init()
#joystick.init()
j = pygame.joystick.Joystick(0)
j.init()
print 'Initialized Joystick : %s' % j.get_name()

# Keeps a history of buttons pressed so that one press does
# not send multiple presses to the Arduino Board
axis = [0.0,0.0,0.0,0.0]
throttle = 1000
mode = 1000 # rate mode is < 1500, else it is attitude
aux = 1000

try:
    while True:
        pygame.event.pump()

        # Minimum change in joystick position to send a new command
        diff = 0.005
        if j.get_axis(0) < axis[0] - diff or j.get_axis(0) > axis[0] + diff:
            axis[0] = j.get_axis(0)

        if j.get_axis(2) < axis[2] - diff or j.get_axis(2) > axis[2] + diff:
            axis[2] = j.get_axis(2)

        if j.get_axis(3) < axis[3] - diff or j.get_axis(3) > axis[3] + diff:
            axis[3] = j.get_axis(3)

        # Get Controller Values
        zaxis = axis[0] * 150 + 1500 # Yaw   (z-axis)
        yaxis = axis[3] * -1 * 150 + 1515 # Pitch (y-axis)
        xaxis = axis[2] * 150 + 1500 # Roll  (x-axis)

        # If values are close to neutral set them to 1500
        if zaxis < 1523 and zaxis > 1490:
            zaxis = 1500
        if yaxis < 1495 and yaxis > 1520:
            yaxis = 1500  # temporary fix for rate mode
        if xaxis < 1518 and xaxis > 1481:
            xaxis = 1500  

        # Get throttle position
        # throttle += j.get_axis(1) * -1 * 25 # Throttle
        if j.get_button(4) == 1:
            throttle -= 40
        elif j.get_button(5) == 1:
            throttle += 40
        if j.get_button(6) == 1:
            throttle -= 10
        elif j.get_button(7) == 1:
            throttle += 10

        if throttle > 1700:
            throttle = 1700
        elif throttle < 1000:
            throttle = 1000

        # Calibrate Sensors
        if j.get_button(8) == 1:
            throttle = 1000
            zaxis = 1000
            yaxis = 1000
            xaxis = 2000

        # Arm Motors
        if j.get_button(9) == 1:
            throttle = 1000
            zaxis = 2000

        # Disarm Motors
        if j.get_button(3) == 1:
            throttle = 1000
            zaxis = 1000

        # Change mode
        if j.get_button(0) == 1:
            mode = 1000
        elif j.get_button(1) == 1:
            mode = 2000

        # Turn off motors
        if j.get_button(2) == 1:
            throttle = 1000

        '''
        Number for each command
        XAXIS '0'
        YAXIS '1'
        ZAXIS '2'
        THROTTLE '3'
        MODE '4'
        AUX '5'
        '''

        # Put Values into a command string and convert ints to bytes
        command =   "c" + \
                    ";0;" + str(int(round(xaxis))) + \
                    ";1;" + str(int(round(yaxis))) + \
                    ";2;" + str(int(round(zaxis))) + \
                    ";3;" + str(int(round(throttle))) + \
                    ";4;" + str(int(round(mode))) + \
                    ";5;" + str(int(round(aux)))
        print(command)

        # Send Commands
        ser.write('c')
        ser.write('0');
        ser.write( struct.pack('h', int(round(xaxis))) )
        ser.write('1');
        ser.write( struct.pack('h', int(round(yaxis))) )
        ser.write('2');
        ser.write( struct.pack('h', int(round(zaxis))) )
        ser.write('3');
        ser.write( struct.pack('h', int(round(throttle))) )
        ser.write('4');
        ser.write( struct.pack('h', int(round(mode))) )
        ser.write('5');
        ser.write( struct.pack('h', int(round(aux))) )

        # Only send 5 times per second
        time.sleep(.2)

except KeyboardInterrupt:
    j.quit()

Remote Station

The remote station only utilized two components: an Arduino Uno and a nRF24l01 Wireless Radio.  Reading the controller commands as a byte array from a laptop, the remote station simply compiles the controller commands into the radio packet structure and sends it to the quadcopter. In addition to reading controller commands, the remote station can also request the PID values in use from the copter and send updated values.  This process is explained in the section on PID tuning.

Remote station source code:


// Do not remove the include below
#include "Aero_Remote.h"

#define XAXIS 0
#define YAXIS 1
#define ZAXIS 2
#define THROTTLE 3
#define MODE 4
#define AUX 5

#define RATE_XAXIS_PID_IDX          0
#define RATE_YAXIS_PID_IDX          1
#define ZAXIS_PID_IDX               2
#define ATTITUDE_XAXIS_PID_IDX      3
#define ATTITUDE_YAXIS_PID_IDX      4
#define HEADING_HOLD_PID_IDX        5
#define ATTITUDE_GYRO_XAXIS_PID_IDX 6
#define ATTITUDE_GYRO_YAXIS_PID_IDX 7
#define ALTITUDE_HOLD_PID_IDX       8
#define ZDAMPENING_PID_IDX          9

int debug = 0;

// Used to read floating point values from the serial port
float readFloatSerial() {
  #define SERIALFLOATSIZE 15
  byte index = 0;
  byte timeout = 0;
  char data[SERIALFLOATSIZE] = "";

  do {
    if (Serial.available() == 0) {
      delay(10);
      timeout++;
    }
    else {
      data[index] = Serial.read();
      timeout = 0;
      index++;
    }
  } while ((index == 0 || data[index-1] != ';') && (timeout < 10) && (index < sizeof(data)-1));
  data[index] = '\0';

  return atof(data);
}

/**
 * Waits for timeout*10 milliseconds for Serial data to be available.
 * Return 0 if serial data not available or 1 if it is.
 */
int waitSerialAvailable(int timeout) {
	while ( !Serial.available() ) {
		timeout++;
		if (timeout > timeout) {
			return 0;
		}
		delay(10);
	}
	return 1;
}

// Reads an integer given in bytes
unsigned int readIntSerialBytes() {
	union u_tag {
	    byte b[2];
	    unsigned int iVal;
	} value;

	// Wait for input
	if ( !waitSerialAvailable(10) )
		return -1;

	value.b[0] = Serial.read();

	if ( !waitSerialAvailable(10) )
		return -1;

	value.b[1] = Serial.read();

	return value.iVal;
}

void sendControllerData() {
	radio_packet packet;
	int i, command;

	// Get all values
	for(i = 0; i < 6; i++) {
		if ( !waitSerialAvailable(10) )
			return;

		command = Serial.read() - '0';

		// Check if it is out of phase
		if (command < 0 || command > 5)
			return;

		switch (command) {
		case XAXIS:
			packet.vars.receiverCommand[0] = readIntSerialBytes();
			break;
		case YAXIS:
			packet.vars.receiverCommand[1] = readIntSerialBytes();
			break;
		case ZAXIS:
			packet.vars.receiverCommand[2] = readIntSerialBytes();
			break;
		case THROTTLE:
			packet.vars.receiverCommand[3] = readIntSerialBytes();
			break;
		case MODE:
			packet.vars.receiverCommand[4] = readIntSerialBytes();
			break;
		case AUX:
			packet.vars.receiverCommand[5] = readIntSerialBytes();
			break;
		default:
			return;
		}
	}

	// This is not a PID calibration packet
	packet.vars.flag = CONTROL_DATA;

	radio_send_wait(&packet, COPTER_ADDR);

	if (debug == 1) {
		for (i = 0; i < 6; i++) {
			Serial.print(packet.vars.receiverCommand[i]);
			Serial.print(" ");
		}
		Serial.println();
	}
}

void sendPidData() {
	float p, i, d, command;
	uint8_t pidIndex;
	radio_packet packet;

	pidIndex = int(readFloatSerial());
	p = readFloatSerial();
	i = readFloatSerial();
	d = readFloatSerial();

	Serial.print("Index: ");
	Serial.print(pidIndex);
	Serial.print(" P: ");
	Serial.print(p);
	Serial.print(" I: ");
	Serial.print(i);
	Serial.print(" D: ");
	Serial.println(d);

	Serial.println("Set these values? (y/n)");
	while (!Serial.available());
	command = Serial.read();
	if (command == 'n') {
		Serial.println("Calibration canceled.");

	} else if (command == 'y') {
		// Put data into packet and send
		Serial.println("Sending values.");
		packet.vars.pidIndex = pidIndex;
		packet.vars.p = p;
		packet.vars.i = i;
		packet.vars.d = d;
		packet.vars.flag = WRITE_PID;
		radio_send_wait(&packet, COPTER_ADDR);
	}
}

void readPidData(uint8_t pidIndex) {

	radio_packet packet;

	// Request read from index
	packet.vars.pidIndex = pidIndex;
	packet.vars.flag = READ_PID;
	radio_send_wait(&packet, COPTER_ADDR);

	if ( !radio_recv(1000, &packet) ) {
		Serial.println("No response received.");
		return;
	}

	// Print PID Data
	Serial.print("Index: ");
	Serial.print(packet.vars.pidIndex);
	Serial.print(" P: ");
	Serial.print(packet.vars.p);
	Serial.print(" I: ");
	Serial.print(packet.vars.i);
	Serial.print(" D: ");
	Serial.println(packet.vars.d);
}

void commitPid() {
	radio_packet packet;

	Serial.println("Committing PID values to EEPROM.");
	packet.vars.flag = COMMIT_PID;
	radio_send_wait(&packet, COPTER_ADDR);
}

//The setup function is called once at startup of the sketch
void setup() {
	Serial.begin(38400);

	// init radio address, channel, payload size
	radio_init(BASE_ADDR, RADIO_CHANNEL, RADIO_PAYLOAD);

}

// The loop function is called in an endless loop
void loop() {
	int command, i;

	if (!Serial.available()) {
		return;
	}

	// What kind of data is to be sent? Read first character.
	if ((command = Serial.read()) == 'c') { 		// send controller commands
		sendControllerData();
	} else if (command == 'p') { 					// set PID
		sendPidData();
	} else if (command == 'r') { 					// read PID
		if ( !waitSerialAvailable(10) )
			return;
		if (Serial.peek() == 'a') { 				// read all PID values
			Serial.read();
			for (i=0; i< 10; i++) {
				delay(20);
				readPidData(i);
			}
			return;
		} else { 									// read specific PID value
			readPidData( int(readFloatSerial()) );
		}
	} else if (command == 's') { 					// save current PID values to EEPROM
		commitPid();
	} else if (command == '#') {
		debug = !debug;
	}
}