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, TxVal, TxVal, TxVal, TxVal, TxVal, AnIn, AnIn, AnIn, AnIn, AnIn, AnIn, AnIn, AnIn, DigBits);
//sprintf(buf, “T %d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,n", PacketType, rssiDur, PacketCount, NoPacketCount, AnIn, AnIn, AnIn, AnIn, AnIn, AnIn, AnIn, AnIn, DigBits, TxVal);
sprintf(buf, “T%02X%02X%04X%02X%03X%03X%03X%03X%03X%03X%03X%03X%03X%02X%02Xn", PacketType, rssiDur, PacketCount, NoPacketCount, AnIn, AnIn, AnIn, AnIn, AnIn, AnIn, AnIn, AnIn, DigBits, TxVal);
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 |= (digitalRead(5) << 0);//joy 2 push
//TxVal |= (digitalRead(6) << 1);//pb
//TxVal |= (digitalRead(7) << 2);//slide
//TxVal |= (digitalRead(8) << 3);//toggle
//Throttle TxVal
//Rotary pot TxVal
//Joy 1 X TxVal
//Joy 1 Y TxVal
//Joy 2 X TxVal
//Joy 2 Y TxVal
//rssi TxVal
//digital TxVal
//micros() TxVal
//Use the pot as the gain for all channels for now
float GainPot = (float)(TxVal) * 0.001f;
//Get the target values from the TX
int PitchTarg = (TxVal / 10);
int RollTarg = (TxVal / 10);
int YawTarg = (TxVal / 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 - AnInWOZ);
float RollGyro = (float)(AnIn - AnInWOZ);
float YawGyro = (float)(AnIn - AnInWOZ);
//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 & 0x4) == 0)
{
PitchTrim = 0;
RollTrim = 0;
YawTrim = 0;
}
//Calc flap anglke
int Flaps = 0;
//Apply flaps
if((TxVal & 0x8) != 0)
Flaps = 25;
//Throttle
val = TxVal / 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.write(val); // sets the servo position according to the scaled value
//Elevator Joy 1 Y TxVal
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.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.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.write(val); // sets the servo position according to the scaled value
//Joy 2 x nose Wheel / rudder
val = (TxVal / 10);
val = map(val, 0, 179, 55, 125);
servo.write(val); // sets the servo position according to the scaled value
//Joy 2 Y
val = TxVal / 10;
val = constrain(val, 15, 165); // scale it to use it with the servo (value between 0 and 180)
servo.write(val); // sets the servo position according to the scaled value
}
void NullServos()
{
//Throttle TxVal
//Rotary pot TxVal
//Joy 1 X TxVal
//Joy 1 Y TxVal
//Joy 2 X TxVal
//Joy 2 Y TxVal
//rssi TxVal
//digital TxVal
//micros() TxVal
//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.write(val); // sets the servo position according to the scaled value
//Elevator Joy 1 Y TxVal
val = 90;
val = constrain(val, 1, 179); // scale it to use it with the servo (value between 0 and 180)
servo.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.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.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.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.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");
}