Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 790 → Rev 791

/branches/V0.68d Code Redesign killagreg/fc.c
122,7 → 122,7
 
// stick values derived by rc channels readings
int16_t StickPitch = 0, StickRoll = 0, StickYaw = 0, StickThrust = 0;
int16_t MaxStickPitch = 0, MaxStickRoll = 0, MaxStickYaw = 0;
int16_t MaxStickPitch = 0, MaxStickRoll = 0;
// stick values derived by uart inputs
int16_t ExternStickPitch = 0, ExternStickRoll = 0, ExternStickYaw = 0, ExternHeightValue = -20;
 
328,16 → 328,6
if(Reading_GyroRoll > 200) Reading_GyroRoll += 4 * (Reading_GyroRoll - 200);
else if(Reading_GyroRoll < -200) Reading_GyroRoll += 4 * (Reading_GyroRoll + 200);
}
//update poti values by rc-signals
if(Poti1 < PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110 && Poti1) Poti1--;
if(Poti2 < PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110 && Poti2) Poti2--;
if(Poti3 < PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110 && Poti3) Poti3--;
if(Poti4 < PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + 110 && Poti4) Poti4--;
//limit poti values
if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255;
if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255;
if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255;
if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255;
}
 
/************************************************************************/
358,29 → 348,7
// start ADC (enables internal trigger so that the ISR in analog.c
// updates the readings once)
ADC_Enable();
//update poti values by rc-signals
if(Poti1 < PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110 && Poti1) Poti1--;
if(Poti2 < PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110 && Poti2) Poti2--;
if(Poti3 < PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110 && Poti3) Poti3--;
if(Poti4 < PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + 110 && Poti4) Poti4--;
 
//PPM24-Extension
if(Poti5 < PPM_in[9] + 110) Poti5++; else if(Poti5 > PPM_in[9] + 110 && Poti5) Poti5--;
if(Poti6 < PPM_in[10] + 110) Poti6++; else if(Poti6 > PPM_in[10] + 110 && Poti6) Poti6--;
if(Poti7 < PPM_in[11] + 110) Poti7++; else if(Poti7 > PPM_in[11] + 110 && Poti7) Poti7--;
if(Poti8 < PPM_in[12] + 110) Poti8++; else if(Poti8 > PPM_in[12] + 110 && Poti8) Poti8--;
 
//limit poti values
if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255;
if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255;
if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255;
if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255;
//PPM24-Extension
if(Poti5 < 0) Poti5 = 0; else if(Poti5 > 255) Poti5 = 255;
if(Poti6 < 0) Poti6 = 0; else if(Poti6 > 255) Poti6 = 255;
if(Poti7 < 0) Poti7 = 0; else if(Poti7 > 255) Poti7 = 255;
if(Poti8 < 0) Poti8 = 0; else if(Poti8 > 255) Poti8 = 255;
 
TurnOver180Pitch = (int32_t) ParamSet.AngleTurnOverPitch * 2500L;
TurnOver180Roll = (int32_t) ParamSet.AngleTurnOverRoll * 2500L;
}
423,6 → 391,7
if(SenderOkay > 140) // do the mapping of RC-Potis only if the rc-signal is ok
// else the last updated values are used
{
//update poti values by rc-signals
#define CHK_POTI(b,a,min,max) { if(a > 250) { if(a == 251) b = Poti1; else if(a == 252) b = Poti2; else if(a == 253) b = Poti3; else if(a == 254) b = Poti4;} else b = a; if(b <= min) b = min; else if(b >= max) b = max;}
CHK_POTI(FCParam.MaxHeight,ParamSet.MaxHeight,0,255);
CHK_POTI(FCParam.Height_D,ParamSet.Height_D,0,100);
510,6 → 479,7
// set neutral rc inputs
PPM_diff[ParamSet.ChannelAssignment[CH_PITCH]] = 0;
PPM_diff[ParamSet.ChannelAssignment[CH_ROLL]] = 0;
PPM_diff[ParamSet.ChannelAssignment[CH_YAW]] = 0;
PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] = 0;
PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] = 0;
PPM_in[ParamSet.ChannelAssignment[CH_YAW]] = 0;
536,6 → 506,27
Reading_IntegralGyroYaw = 0;
Reading_IntegralGyroYaw2 = 0;
}
 
if(Poti1 < PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[ParamSet.ChannelAssignment[CH_POTI1]] + 110 && Poti1) Poti1--;
if(Poti2 < PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[ParamSet.ChannelAssignment[CH_POTI2]] + 110 && Poti2) Poti2--;
if(Poti3 < PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[ParamSet.ChannelAssignment[CH_POTI3]] + 110 && Poti3) Poti3--;
if(Poti4 < PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[ParamSet.ChannelAssignment[CH_POTI4]] + 110 && Poti4) Poti4--;
//PPM24-Extension
if(Poti5 < PPM_in[9] + 110) Poti5++; else if(Poti5 > PPM_in[9] + 110 && Poti5) Poti5--;
if(Poti6 < PPM_in[10] + 110) Poti6++; else if(Poti6 > PPM_in[10] + 110 && Poti6) Poti6--;
if(Poti7 < PPM_in[11] + 110) Poti7++; else if(Poti7 > PPM_in[11] + 110 && Poti7) Poti7--;
if(Poti8 < PPM_in[12] + 110) Poti8++; else if(Poti8 > PPM_in[12] + 110 && Poti8) Poti8--;
//limit poti values
if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255;
if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255;
if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255;
if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255;
//PPM24-Extension
if(Poti5 < 0) Poti5 = 0; else if(Poti5 > 255) Poti5 = 255;
if(Poti6 < 0) Poti6 = 0; else if(Poti6 > 255) Poti6 = 255;
if(Poti7 < 0) Poti7 = 0; else if(Poti7 > 255) Poti7 = 255;
if(Poti8 < 0) Poti8 = 0; else if(Poti8 > 255) Poti8 = 255;
 
// if motors are off and the thrust stick is in the upper position
if((PPM_in[ParamSet.ChannelAssignment[CH_THRUST]] > 80) && MotorsOn == 0)
{
669,7 → 660,7
StickYaw = -PPM_in[ParamSet.ChannelAssignment[CH_YAW]];
StickThrust = PPM_in[ParamSet.ChannelAssignment[CH_THRUST]] + 120;// shift to positive numbers
 
// update max stick positions for pitch, roll and yaw
// update max stick positions for pitch and roll
if(abs(PPM_in[ParamSet.ChannelAssignment[CH_PITCH]]) > MaxStickPitch)
MaxStickPitch = abs(PPM_in[ParamSet.ChannelAssignment[CH_PITCH]]);
else MaxStickPitch--;
676,9 → 667,6
if(abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]) > MaxStickRoll)
MaxStickRoll = abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]);
else MaxStickRoll--;
if(abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]) > MaxStickYaw)
MaxStickYaw = abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]);
else MaxStickYaw--;
 
// update gyro control loop factors
 
798,7 → 786,6
Looping_Pitch = 0;
MaxStickPitch = 0;
MaxStickRoll = 0;
MaxStickYaw = 0;
}
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
843,7 → 830,7
tmp_long /= 3;
tmp_long2 /= 3;
}
if(MaxStickYaw > 25) // reduce further is yaw stick is active
if(abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]) > 25) // reduce further is yaw stick is active
{
tmp_long /= 3;
tmp_long2 /= 3;
890,7 → 877,7
CorrectionRoll = IntegralErrorRoll / ParamSet.GyroAccTrim;
AttitudeCorrectionRoll = CorrectionRoll / BALANCE_NUMBER;
 
if((MaxStickPitch > 15) || (MaxStickRoll > 15) || (MaxStickYaw > 25))
if((MaxStickPitch > 15) || (MaxStickRoll > 15) || (abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]) > 25))
{
AttitudeCorrectionPitch /= 2;
AttitudeCorrectionRoll /= 2;
1024,7 → 1011,7
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Yawing
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(MaxStickYaw > 20) // yaw stick is activated
if(abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]) > 20) // yaw stick is activated
{ // if not fixed compass course is set update compass course
if(!(ParamSet.GlobalConfig & CFG_COMPASS_FIX)) StoreNewCompassCourse = 1;
}
1165,36 → 1152,37
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Height Control
// The higtht control algorithm reduces the thrust but does not increase the thrust.
// The height control algorithm reduces the thrust but does not increase the thrust.
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// If hight control is activated and no emergency landing is active
// If height control is activated and no emergency landing is active
if((ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL) && (!EmergencyLanding) )
{
int tmp_int;
// if hight control is activated by an rc channel
// if height control is activated by an rc channel
if(ParamSet.GlobalConfig & CFG_HEIGHT_SWITCH)
{ // check if parameter is less than activation threshold
if(FCParam.MaxHeight < 50)
{
SetPointHeight = ReadingHeight - 20; // update SetPoint with current reading
HeightControlActive = 0; // disable hight control
HeightControlActive = 0; // disable height control
}
else HeightControlActive = 1; // enable hight control
else HeightControlActive = 1; // enable height control
}
else // no switchable hight control
else // no switchable height control
{
SetPointHeight = ((int16_t) ExternHeightValue + (int16_t) FCParam.MaxHeight) * (int16_t)ParamSet.Height_Gain - 20;
HeightControlActive = 1;
}
// get current hight
// get current height
h = ReadingHeight;
// if current hight is above the setpoint reduce thrust
// if current height is above the setpoint reduce thrust
if((h > SetPointHeight) && HeightControlActive)
{
// hight difference -> P control part
// ThrustMixFraction - HightDeviation * P - HeightChange * D - ACCTop * DACC
// height difference -> P control part
h = ((h - SetPointHeight) * (int16_t) FCParam.Height_P) / 16;
h = ThrustMixFraction - h; // reduce gas
// higth gradient --> D control part
// height gradient --> D control part
h -= (HeightD * FCParam.Height_D) / 8; // D control part
// acceleration sensor effect
tmp_int = ((Reading_Integral_Top / 512) * (int32_t) FCParam.Height_ACC_Effect) / 32;
1201,13 → 1189,13
if(tmp_int > 50) tmp_int = 50;
if(tmp_int < -50) tmp_int = -50;
h -= tmp_int;
// update hight control thrust
// update height control thrust
HeightControlThrust = (HeightControlThrust*15 + h) / 16;
// limit thrust reduction
if(HeightControlThrust < ParamSet.Height_MinThrust)
{
if(ThrustMixFraction >= ParamSet.Height_MinThrust) HeightControlThrust = ParamSet.Height_MinThrust;
// allows landing also if thrust stick is reduced below min thrust on hight control
// allows landing also if thrust stick is reduced below min thrust on height control
if(ThrustMixFraction < ParamSet.Height_MinThrust) HeightControlThrust = ThrustMixFraction;
}
// limit thrust to stick setting