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 |