S107G Helicopter Control via Arduino
I have worked with two types of S107G helicopters. One is a 2-channel (A and B) and the other is a 3-channel (A, B, and C) version. Their protocols differ significantly. The more common 2-channel (32-bit) version’s protocol is well documented elsewhere, so here I will only document the 3-channel (30-bit) version.
(First posted at rcgroups.)
The protocol for this is 30 bits long.
- To send a bit you flash the IR lights 16 times for a 0 and 32 times for a 1.
- Each flash is off for 8-9 microseconds and on for 8-9 microseconds.
- Between bits you wait for 300 microsecond
- Between 30-bit packets you delay an amount depending on the channel you are using.
- Channel A: 136500 us
- Channel B: 105200 us
- Channel C: 168700 us
The order of the bits is as follows:
CC00 PPPP TTTT TTTT YYYY XXXX RRRR RR
C – channel
P – pitch
T – throttle
Y – yaw
X – checksum
R – trim
There are a few other things to note:
1) It has a checksum. The 21-24th bits are a bitwise XOR of 4-bit words with the two zeros appended to the end of the bitstring. Thus you can compute the checksum for the first packet:
1000 0000 1000 1100 0000 1111 1111 11
with:
1000 ^ 0000 ^ 1000 ^ 1100 ^ 0000 ^ 1111 ^ 1100 = 1111
and for the second packet:
1000 0000 0011 1001 0000 0001 1111 11
with
1000 ^ 0000 ^ 0011 ^ 1001 ^ 0000 ^ 1111 ^ 1100 = 0001
2) The bits in the throttle, pitch, and yaw are reversed, so for throttle bit 16 is the most significant bit with bit 9 being the least significant for throttle.
3) Bit 20 is the direction bit for yaw and then bits 19-17 (aka reversed) are the speed bits. So for full yaw in one direction you do 1111 and for full yaw in the other direction you do 1110
4) Bit 8 is the direction bit for pitch with the speed bits being just like yaw.
5) The trim bits are probably reversed or something like that too but I just set them to 0 and didn’t worry.
Here’s my code for an Arduino. To use the code, open the Serial Monitor (Tools > Serial Monitor) and use the following commands:
- 0-9: throttle
- w: forward
- a: left
- s: backwards
- d: right
- t: take off
- u: increase throttle
- j: decrease throttle
- r: reset pitch and yaw
/* S107 3-channel with checksum helicopter control code
* Copyright (C) 2012, Andrew Barry, Dan Barry
*
* Uses an Arduino to control a S107 helicopter
*
*
* Instructions:
* Connect an IR LED array to pin 8 (using a FET to amplify the signal)
* and use the serial monitor to send commands to the system
*
*/
#define LED 8
#define STATUS 13
//#define TAKEOFF_THROTTLE 240
//#define HOLDING_THROTTLE 130
byte yawCmd, pitchCmd, throttleCmd, trimCmd;
// Set this value for the default channel
// A = 0
// B = 1
// C = 2
byte channel = 0;
/*
* Setup function that initializes the serial port and
* sets some default values for the control variables.
* Also sets up the pins we'll be using.
*/
void setup()
{
Serial.begin(9600);
pinMode(STATUS,OUTPUT);
digitalWrite(STATUS,LOW);
pinMode(LED,OUTPUT);
digitalWrite(LED,LOW);
yawCmd = 8;
pitchCmd = 8;
trimCmd = 0;
throttleCmd = 0;
Serial.println("throttle = 0, standing by for commands.");
}
/*
* Function that does the actual work of converting commands into
* IR LED pulses and changes the pins in the appropriate manner.
*/
byte sendPacket(byte yaw, byte pitch, byte throttle, byte trim)
{
int packetData[100];
int pulseNum;
digitalWrite(STATUS,HIGH);
float channelDelayValue = 136500;
// channel A B or C
// A is 10 with 136500us packet delay
// B is 01 with 105200us packet delay
// C is 11 with 168700us packet delay
if (channel == 0)
{
packetData[0] = 1;
packetData[1] = 0;
channelDelayValue = 136500;
} else if (channel == 1)
{
packetData[0] = 0;
packetData[1] = 1;
channelDelayValue = 105200;
} else {
packetData[0] = 1;
packetData[1] = 1;
channelDelayValue = 168700;
}
packetData[2] = 0;
packetData[3] = 0;
// pitch
packetData[7] = (pitch & 0b1000) >> 3; // direction bit
if (pitch < 8)
{
pitch = 8 - pitch;
}
packetData[6] = (pitch & 0b0100) >> 2; // others are speed bits, note that they are reversed
packetData[5] = (pitch & 0b0010) >> 1;
packetData[4] = (pitch & 0b0001);
// throttle
// bits are reversed in the throttle command
packetData[15] = (throttle & 0b10000000) >> 7;
packetData[14] = (throttle & 0b01000000) >> 6;
packetData[13] = (throttle & 0b00100000) >> 5;
packetData[12] = (throttle & 0b00010000) >> 4;
packetData[11] = (throttle & 0b00001000) >> 3;
packetData[10] = (throttle & 0b00000100) >> 2;
packetData[9] = (throttle & 0b00000010) >> 1;
packetData[8] = (throttle & 0b00000001);
// yaw
packetData[19] = (yaw & 0b1000) >> 3; // direction bit
if (yaw < 8)
{
yaw = 8 - yaw;
}
packetData[18] = (yaw & 0b0100) >> 2;
packetData[17] = (yaw & 0b0010) >> 1;
packetData[16] = (yaw & 0b0001);
// these 4 bits are the checksum, so make sure they
// are 0s so they don't change the XOR later on
packetData[20] = 0;
packetData[21] = 0;
packetData[22] = 0;
packetData[23] = 0;
// yaw trim / yaw adjust (the little dial on the controller)
// 6 bits
packetData[24] = 0;
packetData[25] = 0;
packetData[26] = 0;
packetData[27] = 0;
packetData[28] = 0;
packetData[29] = 0;
// these bits are never sent but we do the checksum
// computation in 4-bit chunks, with the trailing two
// bits set to zero, so we set them to zero here to make
// the checksum a bit easier to compute
packetData[30] = 0;
packetData[31] = 0;
int i;
int checksum[10];
checksum[0] = 0;
checksum[1] = 0;
checksum[2] = 0;
checksum[3] = 0;
// compute checksum -- bitwise XOR of 4-bit chuncks
// with two zeros padding the *end* of the last two bits
for (i=0; i<32; i+=4)
{
// XOR
checksum[0] = packetData[i + 0] ^ checksum[0]; // the "^" operator is bitwise XOR (exclusive OR)
checksum[1] = packetData[i + 1] ^ checksum[1];
checksum[2] = packetData[i + 2] ^ checksum[2];
checksum[3] = packetData[i + 3] ^ checksum[3];
}
// now set bits 21-24 (array values 20-23) to the checksum
packetData[20] = checksum[0];
packetData[21] = checksum[1];
packetData[22] = checksum[2];
packetData[23] = checksum[3];
/*
* Uncomment for realtime display of packet data
*/
/*
for (i=0; i<30; i++)
{
Serial.print(packetData[i]);
if ((i+1)%4 == 0)
{
Serial.print(" ");
}
}
Serial.println(" ");
*/
// Send the packet by flashing the LEDs. Also remember how long the packet takes to send
// so we can properly compute how long to wait before sending the next packet.
int bitsum = 0;
for (i=0; i<30; i++)
{
// a "0" bit is 16 pulses and a "1" bit is 32 pulses
if (packetData[i] == 1)
{
bitsum ++;
pulseNum = 32;
} else {
pulseNum = 16;
}
// flash pulseNum times
// a "0" bit is 16 pulses and a "1" bit is 32 pulses
while(pulseNum--)
{
digitalWrite(LED,LOW);
delayMicroseconds(9);
digitalWrite(LED,HIGH);
delayMicroseconds(8);
}
// there is a 300 microsecond delay between pulses of the LED
delayMicroseconds(300);
}
// channel A B or C
// A is 10 with 136500us packet delay
// B is 01 with 105200us packet delay
// C is 11 with 168700us packet delay
//
// that is the delay between sending 30 bit packets
// note that this does not change if our packets are longer
// or shorter, so we must take that into account
return((channelDelayValue - bitsum * 272)/1000); // in ms.
}
void HoldCommand(int yawIn, int pitchIn, int throttleIn, int delayTime)
{
Serial.print("Holding: Yaw:");
Serial.print(yawIn);
Serial.print(" Pitch: ");
Serial.print(pitchIn);
Serial.print(" Throttle: ");
Serial.print(throttleIn);
Serial.print(" for ");
Serial.print(delayTime);
Serial.println("ms");
int i;
int delayConst = 50;
int delayAmount = delayTime/delayConst;
int packetDelay;
while (delayTime > 0)
{
if (Serial.available() == true)
{
Serial.println("HOLD ABORTED");
break;
}
packetDelay = sendPacket(yawIn, pitchIn, throttleIn, trimCmd);
delayTime = delayTime - packetDelay;
delay(packetDelay);
delay(delayAmount);
delayTime = delayTime - delayAmount;
}
Serial.println("Done holding.");
}
void Land()
{
static int i;
Serial.println("Landing");
for(i=throttleCmd;i>0;i--){
HoldCommand(8,8,throttleCmd,50);
}
throttleCmd = 0;
}
/*
* Function that manages recieving data from the serial port.
* Mostly changes the global variables that are passed to the
* control functions.
*/
void serialEvent()
{
char cmd = Serial.read();
Serial.println();
Serial.print("command received is ");
Serial.println(cmd);
switch (cmd)
{
// Take off with 't'
case 't':
Serial.println("Taking Off");
// Yaw: 1-15
// 8 = no turn
// 1 = max right turn
// 15 = max left turn
//
// Pitch: 1-15
// 8 = no pitch
// 15 = max forward
// 1 = max backwards
//
// Throttle: 0-255
// 0 = off
// ~130 = steady flight
// ~240 = fast climb
// First, go up with lots of throttle for 650ms
// yaw: 8 --> no yaw
// pitch: 8 --> no pitch
// throttle: 240 --> fast climb
// delay: 650ms --> enough time to climb, not too long so won't hit ceiling
// HoldCommand: a function that sends the same data for a given amount of time
// HoldCommand(yaw, pitch, throttle, time-to-hold-in-ms);
HoldCommand(8, 8, 240, 650);
// set the *global* throttle to steady flight throttle
throttleCmd = 130;
break;
// land with 'x' or 'q'
case 'x':
case 'q':
Land();
break;
// throttle commands
case '0':
case '1':
case '2':
case '3':
case '4':
case '5':
case '6':
case '7':
case '8':
case '9':
throttleCmd = atoi(&cmd) * 25; //single character, so we can go from 0 to 255 by inputting 0 to 9 in the serial monitor
break;
// turn left
case 'a':
if (yawCmd < 15)
{
yawCmd ++;
}
Serial.print("Yaw is ");
Serial.println(yawCmd);
break;
// turn right
case 'd':
if (yawCmd > 1)
{
yawCmd --;
}
Serial.print("Yaw is ");
Serial.println(yawCmd);
break;
// move forwards
case 'w':
if (pitchCmd < 15){
pitchCmd ++; // moves forward
}
Serial.print("Pitch is ");
Serial.println(pitchCmd);
break;
// move backwards
case 's':
if (pitchCmd > 1)
{
pitchCmd --; // moves backward
}
Serial.print("Pitch is ");
Serial.println(pitchCmd);
break;
// increase throttle
case 'u':
if (throttleCmd < 255 - 6)
{
throttleCmd += 6;
}
Serial.print("Throttle is ");
Serial.println(throttleCmd);
break;
// decrease throttle
case 'j':
if (throttleCmd > 6)
{
throttleCmd -= 6;
}
Serial.print("Trottle is ");
Serial.println(throttleCmd);
break;
// change channel
case 'c':
Serial.println("Changing channel");
if (channel >= 2)
{
channel = 0;
} else
{
channel ++;
}
Serial.print("Channel is: ");
Serial.println(channel);
break;
// reset yaw and pitch
case 'r':
Serial.println("resetting yaw and pitch");
yawCmd = 8;
pitchCmd = 8;
break;
default:
Serial.println("Unknown command");
}
Serial.print("Throttle is at ");
Serial.println(throttleCmd);
}
/*
* Loops continuously sending and delaying for the transmission
*/
void loop()
{
// Note that serialEvent() gets called on each path of the loop
// and runs if there is data at the serial port
// we call delay here on the return value of sendPacket because that will
// cause us to put the right amount of time between packets. The delay is
// not constant, but is instead based on how long the packet was
// that we sent
delay(sendPacket(yawCmd, pitchCmd, throttleCmd, trimCmd));
}
03/21/12