Posts | Comments

Planet Arduino

Archive for the ‘yellow plane’ Category

Nov
29

Arduino Aircraft Stabilization Code

gallery, plane, stabilization, update, yellow plane Commenti disabilitati su Arduino Aircraft Stabilization Code 

We remember [nickatredbox] for the [Yellow Plane],  an Arduino project. Today we have an update to the code. Click on [more] for the full code update.

Revised Yellow FPV Plane with gyro stability system added, Worked best with analogue inputs from Murata piezo gyro sensors of a dead KK board filtered taking a 10 point average. Yaw compensation is currently not used as there is no rudder presently.

Here is a video of the functionality

Here is the Arduino code that calculates the trim values

//Use the pot as the gain for all channels for now
float GainPot = (float)(TxVal[2]) * 0.001f;

//Get the target values from the TX
int PitchTarg = (TxVal[3] / 10);
int RollTarg = (TxVal[4] / 10);
int YawTarg = (TxVal[6] / 10);

//Prime the Target WOZ values
if(PitchTargWOZ == 9999)
PitchTargWOZ = PitchTarg;

if(RollTargWOZ == 9999)
RollTargWOZ = RollTarg;

if(YawTargWOZ == 9999)
YawTargWOZ = YawTarg;

//Get the Centered target values
float PitchTargCentred = (float)(PitchTarg – PitchTargWOZ);
float RollTargCentred = (float)(RollTarg – RollTargWOZ);
float YawTargCentred = (float)(YawTarg – YawTargWOZ);

//Calculate gains
float PitchGain = GainPot * 1.0f;
float RollGain = GainPot * 1.0f;
float YawGain = GainPot * 1.0f;

//Get Gyro values
float PitchGyro = (float)(AnIn[2] – AnInWOZ[2]);
float RollGyro = (float)(AnIn[1] – AnInWOZ[1]);
float YawGyro = (float)(AnIn[0] – AnInWOZ[0]);

//Calc P error
float PitchError = (float)PitchTargCentred + PitchGyro;
float RollError = (float)RollTargCentred + RollGyro;
float YawError = (float)YawTargCentred + YawGyro;

//Apply gains
int PitchTrim = (int)(PitchError * PitchGain);
int RollTrim = (int)(RollError * RollGain);
int YawTrim = (int)(YawError * YawGain);

//Constaring trim authority
PitchTrim = constrain(PitchTrim, -30, 30);
RollTrim = constrain(RollTrim, -30, 30);
YawTrim = constrain(YawTrim, -30, 30);

//Dump the trim value
if((TxVal[9] & 0×4) == 0)
{
PitchTrim = 0;
RollTrim = 0;
YawTrim = 0;
}

Here is all the RX the code

#define MAX_CHAN 12
#define MAX_IN_STR 200
#define MAX_SETTINGS 20
#define MAX_NAV_VALS 50
#define MAX_SAMPLE 10

#include

char buf[255] = {0, };
String str = “”;
char *p;

Servo servo[7]; // create servo object to control a servo
int val = 0; // variable to read the value from the analog pin
int AnInWOZ[MAX_CHAN] = {0, };
int AnIn[MAX_CHAN] = {0, };
int AnInBuf[MAX_CHAN] = {0, };

//Get the target values from the TX at rest
int PitchTargWOZ = 9999;
int RollTargWOZ = 9999;
int YawTargWOZ = 9999;

String inputString = “”; // a string to hold incoming data

int Sample = 0;
int TxTemp[MAX_CHAN + 1] = {0, };
int TxVal[MAX_CHAN + 1] = {0, };

int NavVal[MAX_NAV_VALS] = {0, };
int SettingVal[MAX_SETTINGS] = {0, };

int rssiDur = 0;
int DigBits = 0;
int ComState = 0;
long PacketCount = 0;
long NoPacketCount = 0;

//Digital inputs TX code helper
//TxVal[8] |= (digitalRead(5) << 0);//joy 2 push
//TxVal[8] |= (digitalRead(6) << 1);//pb
//TxVal[8] |= (digitalRead(7) << 2);//slide
//TxVal[8] |= (digitalRead(8) << 3);//toggle

void setup() {

// initialize serial:
Serial.begin(38400);

// reserve 200 bytes for the inputString:
inputString.reserve(MAX_IN_STR);

pinMode(2,INPUT);//rssi
digitalWrite(2, HIGH);

servo[0].attach(3); // attaches the servo on pin 3 to the servo object
servo[1].attach(5); // attaches the servo on pin 5 to the servo object
servo[2].attach(6); // attaches the servo on pin 6 to the servo object
servo[3].attach(9); // attaches the servo on pin 9 to the servo object
servo[4].attach(10); // attaches the servo on pin 10 to the servo object
servo[5].attach(11); // attaches the servo on pin 11 to the servo object

NullServos();

//Get all the analogue signals
//Do a wind off zero

for(int i = 0;i < 8;i++)
AnInWOZ[i] = 0;

for(int i = 0;i < MAX_SAMPLE;i++){

AnIn[0] += analogRead(A0);
AnIn[1] += analogRead(A1);
AnIn[2] += analogRead(A2);
AnIn[3] += analogRead(A3);
AnIn[4] += analogRead(A4);
AnIn[5] += analogRead(A5);
AnIn[6] += analogRead(A6);
AnIn[7] += analogRead(A7);

delayMicroseconds(10);
}

for(int i = 0;i < 8;i++)
AnInWOZ[i] = (AnIn[i] / MAX_SAMPLE);

//Prime the WOZ values
PitchTargWOZ = 9999;
RollTargWOZ = 9999;
YawTargWOZ = 9999;

}

void loop(){

//*Get all the analogue signals
for(int i = 0;i < 8;i++)
AnInBuf[i] = 0;

for(int i = 0;i < MAX_SAMPLE;i++){

AnInBuf[0] += analogRead(A0);
AnInBuf[1] += analogRead(A1);
AnInBuf[2] += analogRead(A2);
AnInBuf[3] += analogRead(A3);
AnInBuf[4] += analogRead(A4);
AnInBuf[5] += analogRead(A5);
AnInBuf[6] += analogRead(A6);
AnInBuf[7] += analogRead(A7);

delayMicroseconds(10);
}

for(int i = 0;i < 8;i++)
AnIn[i]= (AnInBuf[i] / MAX_SAMPLE);

//Capture the Xbee comms
int CharCount = 0;

while ((Serial.available()) && ((++CharCount) 0)
inputString += inChar;

if(inputString.length() >= MAX_IN_STR)
break;

//Detect end of packet
if ( (inChar == ‘\n’) && (ComState > 0) )
{
//Serial.println(inputString);

//Count packets
PacketCount++;

int NumChan = ExtractPacket();

//Tramsmitter
if( ComState == 1)
{
for(int i = 0 ;i < NumChan;i++)
TxVal[i] = TxTemp[i];

DoTelemetery();

UpdateServos();

}//Navigator
else if( ComState == 2)
{
for(int i = 0 ;i < NumChan;i++)
NavVal[i] = TxTemp[i];

}//Settings
else if( ComState == 3)
{
for(int i = 0 ;i 50)
{
NullServos();
SoftReset();
}

//delayMicroseconds(1000);
}

int ExtractPacket()
{

int Lchk = 0;
int channel = 0; //initialise the channel count

p = &inputString[0];

while ((str = strtok_r(p, “,”, &p)) != NULL) // delimiter is the comma
{

TxTemp[channel] = str.toInt(); //use the channel as an index to add each value to the array

Lchk += TxTemp[channel];

channel++; //increment the channel

if(channel > MAX_CHAN)
break;
}

p = NULL;
inputString = “”;

//Process in comming data
if(channel > 2)
{
//Remove the remote chk from the total
Lchk -= TxTemp[channel-2];

//Checksum
if((Lchk – TxTemp[channel-2]) == 0)
return channel;

}

return -1;
}

void DoTelemetery()
{

//Send back a telemetery packet
if((PacketCount % 5) == 0)
{
rssiDur = pulseIn(5, LOW, 200);

int PacketType = 12;

//sprintf(buf, “T%02X,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,\n”, PacketType, rssiDur, PacketCount, NoPacketCount, TxVal[1], TxVal[2], TxVal[3], TxVal[4], TxVal[5], TxVal[6], AnIn[0], AnIn[1], AnIn[2], AnIn[3], AnIn[4], AnIn[5], AnIn[6], AnIn[7], DigBits);
//sprintf(buf, “T %d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,\n”, PacketType, rssiDur, PacketCount, NoPacketCount, AnIn[0], AnIn[1], AnIn[2], AnIn[3], AnIn[4], AnIn[5], AnIn[6], AnIn[7], DigBits, TxVal[9]);
sprintf(buf, “T%02X%02X%04X%02X%03X%03X%03X%03X%03X%03X%03X%03X%03X%02X%02X\n”, PacketType, rssiDur, PacketCount, NoPacketCount, AnIn[0], AnIn[1], AnIn[2], AnIn[3], AnIn[4], AnIn[5], AnIn[6], AnIn[7], DigBits, TxVal[9]);

Serial.write(buf);

if(digitalRead(13) == false)
digitalWrite(13, HIGH); // set the LED on
else
digitalWrite(13, LOW); // set the LED off

}
}

void UpdateServos()
{

//Digital inputs TX code helper
//TxVal[8] |= (digitalRead(5) << 0);//joy 2 push
//TxVal[8] |= (digitalRead(6) << 1);//pb
//TxVal[8] |= (digitalRead(7) << 2);//slide
//TxVal[8] |= (digitalRead(8) << 3);//toggle

//Throttle TxVal[1]
//Rotary pot TxVal[2]
//Joy 1 X TxVal[3]
//Joy 1 Y TxVal[4]
//Joy 2 X TxVal[5]
//Joy 2 Y TxVal[6]
//rssi TxVal[7]
//digital TxVal[8]
//micros() TxVal[9]

//Use the pot as the gain for all channels for now
float GainPot = (float)(TxVal[2]) * 0.001f;

//Get the target values from the TX
int PitchTarg = (TxVal[3] / 10);
int RollTarg = (TxVal[4] / 10);
int YawTarg = (TxVal[6] / 10);

//Prime the Target WOZ values
if(PitchTargWOZ == 9999)
PitchTargWOZ = PitchTarg;

if(RollTargWOZ == 9999)
RollTargWOZ = RollTarg;

if(YawTargWOZ == 9999)
YawTargWOZ = YawTarg;

//Get the Centered target values
float PitchTargCentred = (float)(PitchTarg – PitchTargWOZ);
float RollTargCentred = (float)(RollTarg – RollTargWOZ);
float YawTargCentred = (float)(YawTarg – YawTargWOZ);

//Calculate gains
float PitchGain = GainPot * 1.0f;
float RollGain = GainPot * 1.0f;
float YawGain = GainPot * 1.0f;

//Get Gyro values
float PitchGyro = (float)(AnIn[2] – AnInWOZ[2]);
float RollGyro = (float)(AnIn[1] – AnInWOZ[1]);
float YawGyro = (float)(AnIn[0] – AnInWOZ[0]);

//Calc P error
float PitchError = (float)PitchTargCentred + PitchGyro;
float RollError = (float)RollTargCentred + RollGyro;
float YawError = (float)YawTargCentred + YawGyro;

//Apply gains
int PitchTrim = (int)(PitchError * PitchGain);
int RollTrim = (int)(RollError * RollGain);
int YawTrim = (int)(YawError * YawGain);

//Constaring trim authority
PitchTrim = constrain(PitchTrim, -30, 30);
RollTrim = constrain(RollTrim, -30, 30);
YawTrim = constrain(YawTrim, -30, 30);

//Dump the trim value
if((TxVal[9] & 0×4) == 0)
{
PitchTrim = 0;
RollTrim = 0;
YawTrim = 0;
}

//Calc flap anglke
int Flaps = 0;

//Apply flaps
if((TxVal[9] & 0×8) != 0)
Flaps = 25;

//Throttle
val = TxVal[1] / 10;
val = map(val, 1, 179, 30, 179);
val = constrain(val, 1, 165); // scale it to use it with the servo (value between 0 and 180)
servo[0].write(val); // sets the servo position according to the scaled value

//Elevator Joy 1 Y TxVal[4]
val = PitchTarg + PitchTrim;
val = constrain(val, 15, 165);
val = map(val, 0, 179, 135, 45); // scale it to use it with the servo (value between 0 and 180)
servo[1].write(val); // sets the servo position according to the scaled value

//Left Flaperon
val = RollTarg + Flaps + RollTrim;
val = constrain(val, 15, 165);
val = map(val, 0, 179, 165, 15); // scale it to use it with the servo (value between 0 and 180)
servo[2].write(val); // sets the servo position according to the scaled value

//Right Flaperon
val = RollTarg – Flaps + RollTrim;
val = constrain(val, 15, 165);
val = map(val, 0, 179, 165, 15); // scale it to use it with the servo (value between 0 and 180)
servo[3].write(val); // sets the servo position according to the scaled value

//Joy 2 x nose Wheel / rudder
val = (TxVal[6] / 10);
val = map(val, 0, 179, 55, 125);
servo[4].write(val); // sets the servo position according to the scaled value

//Joy 2 Y
val = TxVal[5] / 10;
val = constrain(val, 15, 165); // scale it to use it with the servo (value between 0 and 180)
servo[5].write(val); // sets the servo position according to the scaled value

}

void NullServos()
{

//Throttle TxVal[1]
//Rotary pot TxVal[2]
//Joy 1 X TxVal[3]
//Joy 1 Y TxVal[4]
//Joy 2 X TxVal[5]
//Joy 2 Y TxVal[6]
//rssi TxVal[7]
//digital TxVal[8]
//micros() TxVal[9]

//Throttle
val = 0;
val = map(val, 1, 179, 30, 179);
val = constrain(val, 1, 179); // scale it to use it with the servo (value between 0 and 180)
servo[0].write(val); // sets the servo position according to the scaled value

//Elevator Joy 1 Y TxVal[4]
val = 90;
val = constrain(val, 1, 179); // scale it to use it with the servo (value between 0 and 180)
servo[1].write(val); // sets the servo position according to the scaled value

//Left Flaperon
val = 90;
val = map(val, 0, 179, 1, 179); // scale it to use it with the servo (value between 0 and 180)
servo[2].write(val); // sets the servo position according to the scaled value

//Right Flaperon
val = 90;
val = constrain(val, 1, 179); // scale it to use it with the servo (value between 0 and 180)
servo[3].write(val); // sets the servo position according to the scaled value

//Joy 2 X
val = 90;
val = constrain(val, 1, 179); // scale it to use it with the servo (value between 0 and 180)
servo[4].write(val); // sets the servo position according to the scaled value

//Joy 2 Y
val = 90;
val = constrain(val, 1, 179); // scale it to use it with the servo (value between 0 and 180)
servo[5].write(val); // sets the servo position according to the scaled value

}

void SoftReset() // Restarts program from beginning but does not reset the peripherals and registers
{

//Prime the WOZ values
PitchTargWOZ = 9999;
RollTargWOZ = 9999;
YawTargWOZ = 9999;

NoPacketCount = 0;

asm volatile (” jmp 0″);
}

Found out why the maiden flight ended with Issac Newton attacking my plane. The pilot said it seemed to loose power as she turned into the wind, and he was flat out for whole flight due to the excessive weight of all that Corriboard, I have a lighter air frame in the works 1300 Grams wing loading 12.53 oz/ft², 250 Watts per Kilo Please see the spreadsheet. I think its highly likely she just ran out of juice?

This is why I suspect the battery state



Turnigy Accucel-6 50W 6A Balancer/Charger


Spec.
Input Voltage: 11~17v
Circuit power: Max Charge: 50W / Max Discharge: 5W
Charge Current Range: .1~6.0A
Ni-MH/NiCd cells: 1~15
Li-ion/Poly cells: 1~6
Pb battery voltage: 2~20v



Some how or other my charger was set to a max CAPA of 200mAh so my batteries were never charged. My wife helpfully pointed out that I should have read the manual (Here is the manual) but that just isn't the bloke way, why do girls not get that?



Maiden flight of Yellow Plane flown by a local pilot Richard to avoid instant crash I would cause. A little tail heavy but flew pretty well on a gusty day, I'm told control was good. Ended in a crash but not too badly damaged, learned a lot. I'm just chuffed if flew looks pretty good on such a windy, which is pretty standard for the south island this time of year.


My scratch built FPV platform. Took around 30 hours to build mostly Corriboard with some ply, aluminium and carbon fiber spars. Total material cost around 50 US$. Got a DT700 (see tests data here) which hopefully will be an adequate power source. Its a modular design based around an armature, so wings and tail etc are bolted on and can be exchanged for testing parts and ideas. Have a pair of KM3 wings and the Corriboard ones shown below.


Please see the spread sheet here Yellow Plane Data 


More Links
The RX Build
Power Tests
More Test Data
Xbee's and Arduinos
Controller using Tiny CLR
Xbee Helpers


Based around a stiff wooden armature and two aluminium tail spars the wings are removable for transportation. According to my calculations the wing loading is 15.5 Oz/Ft² at a flight weight of 1700 Grams. A glass fiber nose has been molded and is curing now which is around 80 grams, which will contain the FPV gear and the main battery.


Home brew Arduino Xbee remote control 




  • Newsletter

    Sign up for the PlanetArduino Newsletter, which delivers the most popular articles via e-mail to your inbox every week. Just fill in the information below and submit.

  • Like Us on Facebook