Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 760 → Rev 761

/branches/V0.68d Code Redesign killagreg/fc.c
123,13 → 123,13
int16_t StickPitch = 0, StickRoll = 0, StickYaw = 0, StickThrust = 0;
int16_t MaxStickPitch = 0, MaxStickRoll = 0, MaxStickYaw = 0;
// stick values derived by uart inputs
int16_t ExternStickPitch = 0, ExternStickRoll = 0, ExternStickYaw = 0, ExternHightValue = -20;
int16_t ExternStickPitch = 0, ExternStickRoll = 0, ExternStickYaw = 0, ExternHeightValue = -20;
 
 
 
 
int16_t ReadingHight = 0;
int16_t SetPointHight = 0;
int16_t ReadingHeight = 0;
int16_t SetPointHeight = 0;
 
int16_t AttitudeCorrectionRoll = 0, AttitudeCorrectionPitch = 0;
 
173,7 → 173,7
CalibMean();
Delay_ms_Mess(100);
CalibMean();
if((ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL)) // Hight Control activated?
if((ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL)) // Height Control activated?
{
if((ReadingAirPressure > 950) || (ReadingAirPressure < 750)) SearchAirPressureOffset();
}
203,13 → 203,13
Reading_GyroRoll = 0;
Reading_GyroYaw = 0;
StartAirPressure = AirPressure;
HightD = 0;
HeightD = 0;
Reading_Integral_Top = 0;
CompassCourse = CompassHeading;
BeepTime = 50;
TurnOver180Pitch = (int32_t) ParamSet.AngleTurnOverPitch * 2500L;
TurnOver180Roll = (int32_t) ParamSet.AngleTurnOverRoll * 2500L;
ExternHightValue = 0;
ExternHeightValue = 0;
GPS_Neutral();
}
 
409,10 → 409,10
{
 
#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.MaxHight,ParamSet.MaxHight,0,255);
CHK_POTI(FCParam.Hight_D,ParamSet.Hight_D,0,100);
CHK_POTI(FCParam.Hight_P,ParamSet.Hight_P,0,100);
CHK_POTI(FCParam.Hight_ACC_Effect,ParamSet.Hight_ACC_Effect,0,255);
CHK_POTI(FCParam.MaxHeight,ParamSet.MaxHeight,0,255);
CHK_POTI(FCParam.Height_D,ParamSet.Height_D,0,100);
CHK_POTI(FCParam.Height_P,ParamSet.Height_P,0,100);
CHK_POTI(FCParam.Height_ACC_Effect,ParamSet.Height_ACC_Effect,0,255);
CHK_POTI(FCParam.CompassYawEffect,ParamSet.CompassYawEffect,0,255);
CHK_POTI(FCParam.Gyro_P,ParamSet.Gyro_P,10,255);
CHK_POTI(FCParam.Gyro_I,ParamSet.Gyro_I,0,255);
449,8 → 449,8
static uint16_t RcLostTimer;
static uint8_t delay_neutral = 0, delay_startmotors = 0, delay_stopmotors = 0;
static uint16_t Modell_Is_Flying = 0;
static uint8_t HightControlActive = 0;
static int16_t HightControlThrust = 0;
static uint8_t HeightControlActive = 0;
static int16_t HeightControlThrust = 0;
static int8_t TimerDebugOut = 0;
static int8_t StoreNewCompassCourse = 0;
static int32_t CorrectionPitch, CorrectionRoll;
681,8 → 681,8
 
if(DubWiseKeys[0] & 8) ExternStickYaw = 50;else
if(DubWiseKeys[0] & 4) ExternStickYaw =-50;else ExternStickYaw = 0;
if(DubWiseKeys[0] & 2) ExternHightValue++;
if(DubWiseKeys[0] & 16) ExternHightValue--;
if(DubWiseKeys[0] & 2) ExternHeightValue++;
if(DubWiseKeys[0] & 16) ExternHeightValue--;
 
StickPitch += ExternStickPitch / 8;
StickRoll += ExternStickRoll / 8;
697,7 → 697,7
StickPitch += (int16_t) ExternControl.Pitch * (int16_t) ParamSet.Stick_P;
StickRoll += (int16_t) ExternControl.Roll * (int16_t) ParamSet.Stick_P;
StickYaw += ExternControl.Yaw;
ExternHightValue = (int16_t) ExternControl.Hight * (int16_t)ParamSet.Hight_Gain;
ExternHeightValue = (int16_t) ExternControl.Height * (int16_t)ParamSet.Height_Gain;
if(ExternControl.Thrust < StickThrust) StickThrust = ExternControl.Thrust;
}
// disable I part of gyro control feedback
1078,7 → 1078,7
DebugOut.Analog[2] = Mean_AccPitch;
DebugOut.Analog[3] = Mean_AccRoll;
DebugOut.Analog[4] = Reading_GyroYaw;
DebugOut.Analog[5] = ReadingHight;
DebugOut.Analog[5] = ReadingHeight;
DebugOut.Analog[6] = (Reading_Integral_Top / 512);
DebugOut.Analog[8] = CompassHeading;
DebugOut.Analog[9] = UBat;
1098,7 → 1098,7
DebugOut.Analog[25] = motor_rx[4] + motor_rx[5] + motor_rx[6] + motor_rx[7];
 
DebugOut.Analog[9] = Reading_GyroPitch;
DebugOut.Analog[9] = SetPointHight;
DebugOut.Analog[9] = SetPointHeight;
DebugOut.Analog[10] = Reading_IntegralGyroYaw / 128;
DebugOut.Analog[11] = CompassCourse;
DebugOut.Analog[10] = FCParam.Gyro_I;
1105,8 → 1105,8
DebugOut.Analog[10] = ParamSet.Gyro_I;
DebugOut.Analog[9] = CompassOffCourse;
DebugOut.Analog[10] = ThrustMixFraction;
DebugOut.Analog[3] = HightD * 32;
DebugOut.Analog[4] = HightControlThrust;
DebugOut.Analog[3] = HeightD * 32;
DebugOut.Analog[4] = HeightControlThrust;
*/
}
 
1134,7 → 1134,7
if(Reading_GyroYaw < -MAX_SENSOR) Reading_GyroYaw = -MAX_SENSOR;
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Hight Control
// Height Control
// The higth control algorithm reduces the thrust but does not increase the thrust.
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// If hight control is activated and no emergency landing is active
1144,45 → 1144,45
// if hight control is activated by an rc channel
if(ParamSet.GlobalConfig & CFG_HEIGHT_SWITCH)
{ // check if parameter is less than activation threshold
if(FCParam.MaxHight < 50)
if(FCParam.MaxHeight < 50)
{
SetPointHight = ReadingHight - 20; // update SetPoint with current reading
HightControlActive = 0; // disable hight control
SetPointHeight = ReadingHeight - 20; // update SetPoint with current reading
HeightControlActive = 0; // disable hight control
}
else HightControlActive = 1; // enable hight control
else HeightControlActive = 1; // enable hight control
}
else // no switchable hight control
{
SetPointHight = ((int16_t) ExternHightValue + (int16_t) FCParam.MaxHight) * (int16_t)ParamSet.Hight_Gain - 20;
HightControlActive = 1;
SetPointHeight = ((int16_t) ExternHeightValue + (int16_t) FCParam.MaxHeight) * (int16_t)ParamSet.Height_Gain - 20;
HeightControlActive = 1;
}
// get current hight
h = ReadingHight;
h = ReadingHeight;
// if current hight is above the setpoint reduce thrust
if((h > SetPointHight) && HightControlActive)
if((h > SetPointHeight) && HeightControlActive)
{
// hight difference -> P control part
h = ((h - SetPointHight) * (int16_t) FCParam.Hight_P) / 16;
h = ((h - SetPointHeight) * (int16_t) FCParam.Height_P) / 16;
h = ThrustMixFraction - h; // reduce gas
// higth gradient --> D control part
h -= (HightD * FCParam.Hight_D) / 8; // D control part
h -= (HeightD * FCParam.Height_D) / 8; // D control part
// acceleration sensor effect
tmp_int = ((Reading_Integral_Top / 512) * (int32_t) FCParam.Hight_ACC_Effect) / 32;
tmp_int = ((Reading_Integral_Top / 512) * (int32_t) FCParam.Height_ACC_Effect) / 32;
if(tmp_int > 50) tmp_int = 50;
if(tmp_int < -50) tmp_int = -50;
h -= tmp_int;
// update hight control thrust
HightControlThrust = (HightControlThrust*15 + h) / 16;
HeightControlThrust = (HeightControlThrust*15 + h) / 16;
// limit thrust reduction
if(HightControlThrust < ParamSet.Hight_MinThrust)
if(HeightControlThrust < ParamSet.Height_MinThrust)
{
if(ThrustMixFraction >= ParamSet.Hight_MinThrust) HightControlThrust = ParamSet.Hight_MinThrust;
if(ThrustMixFraction >= ParamSet.Height_MinThrust) HeightControlThrust = ParamSet.Height_MinThrust;
// allows landing also if thrust stick is reduced below min thrust on hight control
if(ThrustMixFraction < ParamSet.Hight_MinThrust) HightControlThrust = ThrustMixFraction;
if(ThrustMixFraction < ParamSet.Height_MinThrust) HeightControlThrust = ThrustMixFraction;
}
// limit thrust to stick setting
if(HightControlThrust > ThrustMixFraction) HightControlThrust = ThrustMixFraction;
ThrustMixFraction = HightControlThrust;
if(HeightControlThrust > ThrustMixFraction) HeightControlThrust = ThrustMixFraction;
ThrustMixFraction = HeightControlThrust;
}
}
// limit thrust to parameter setting