100,14 → 100,12 |
volatile int16_t CompassOffCourse = 0; |
|
// flags |
uint8_t EmergencyLanding = 0; |
uint8_t HightControlActive = 0; |
uint8_t MotorsOn = 0; |
|
int32_t TurnOver180Pitch = 250000L, TurnOver180Roll = 250000L; |
|
float GyroFactor; |
float IntegralFactor; |
float Gyro_P_Factor; |
float Gyro_I_Factor; |
|
volatile int16_t DiffPitch, DiffRoll; |
|
164,8 → 162,8 |
AdNeutralPitch = 0; |
AdNeutralRoll = 0; |
AdNeutralYaw = 0; |
FCParam.AchsKopplung1 = 0; |
FCParam.AchsGegenKopplung1 = 0; |
FCParam.Yaw_PosFeedback = 0; |
FCParam.Yaw_NegFeedback = 0; |
CalibMean(); |
Delay_ms_Mess(100); |
CalibMean(); |
242,17 → 240,17 |
{ |
tmpl = Reading_IntegralPitch / 4096L; |
tmpl *= ReadingYaw; |
tmpl *= FCParam.AchsKopplung1; //125 |
tmpl *= FCParam.Yaw_PosFeedback; //125 |
tmpl /= 2048L; |
tmpl2 = Reading_IntegralRoll / 4096L; |
tmpl2 *= ReadingYaw; |
tmpl2 *= FCParam.AchsKopplung1; |
tmpl2 *= FCParam.Yaw_PosFeedback; |
tmpl2 /= 2048L; |
} |
else tmpl = tmpl2 = 0; |
// Roll |
ReadingRoll += tmpl; |
ReadingRoll += (tmpl2 * FCParam.AchsGegenKopplung1) / 512L; //109 |
ReadingRoll += (tmpl2 * FCParam.Yaw_NegFeedback) / 512L; //109 |
Reading_IntegralRoll2 += ReadingRoll; |
Reading_IntegralRoll += ReadingRoll - AttitudeCorrectionRoll; |
if(Reading_IntegralRoll > TurnOver180Roll) |
279,7 → 277,7 |
} |
// Pitch |
ReadingPitch -= tmpl2; |
ReadingPitch -= (tmpl*FCParam.AchsGegenKopplung1) / 512L; |
ReadingPitch -= (tmpl*FCParam.Yaw_NegFeedback) / 512L; |
Reading_IntegralPitch2 += ReadingPitch; |
Reading_IntegralPitch += ReadingPitch - AttitudeCorrectionPitch; |
if(Reading_IntegralPitch > TurnOver180Pitch) |
420,8 → 418,8 |
CHK_POTI(FCParam.UserParam8,ParamSet.UserParam8,0,255); |
CHK_POTI(FCParam.ServoPitchControl,ParamSet.ServoPitchControl,0,255); |
CHK_POTI(FCParam.LoopGasLimit,ParamSet.LoopGasLimit,0,255); |
CHK_POTI(FCParam.AchsKopplung1, ParamSet.AchsKopplung1,0,255); |
CHK_POTI(FCParam.AchsGegenKopplung1,ParamSet.AchsGegenKopplung1,0,255); |
CHK_POTI(FCParam.Yaw_PosFeedback,ParamSet.Yaw_PosFeedback,0,255); |
CHK_POTI(FCParam.Yaw_NegFeedback,ParamSet.Yaw_NegFeedback,0,255); |
CHK_POTI(FCParam.DynamicStability,ParamSet.DynamicStability,0,255); |
|
Ki = (float) FCParam.I_Factor * FACTOR_I; |
428,354 → 426,393 |
} |
|
|
//############################################################################ |
// |
/************************************************************************/ |
/* MotorControl */ |
/************************************************************************/ |
void MotorRegler(void) |
//############################################################################ |
{ |
int MotorValue, pd_ergebnis, h, tmp_int; |
int YawMixingFraction, GasMixingFraction; |
static long SumPitch = 0, SumRoll = 0; |
static long SetPointYaw = 0, tmp_long, tmp_long2; |
static long IntegralErrorPitch = 0; |
static long IntegralErrorRoll = 0; |
static unsigned int RcLostTimer; |
static unsigned char delay_neutral = 0; |
static unsigned char delay_einschalten = 0,delay_ausschalten = 0; |
static unsigned int modell_fliegt = 0; |
static int hoehenregler = 0; |
static char TimerDebugOut = 0; |
static char StoreNewCompassCourse = 0; |
static long CorrectionPitch, CorrectionRoll; |
int16_t MotorValue, pd_ergebnis, h, tmp_int; |
int16_t YawMixingFraction, GasMixingFraction; |
static int32_t SumPitch = 0, SumRoll = 0; |
static int32_t SetPointYaw = 0, tmp_long, tmp_long2; |
static int32_t IntegralErrorPitch = 0; |
static int32_t IntegralErrorRoll = 0; |
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 EmergencyLanding = 0; |
static uint8_t HightControlActive = 0; |
static int16_t hoehenregler = 0; |
static int8_t TimerDebugOut = 0; |
static int8_t StoreNewCompassCourse = 0; |
static int32_t CorrectionPitch, CorrectionRoll; |
|
Mean(); |
|
GRN_ON; |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Gaswert ermitteln |
// determine GAS value |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
GasMixingFraction = StickGas; |
if(GasMixingFraction < 0) GasMixingFraction = 0; |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Emfang schlecht |
// RC-signal is bad |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(SenderOkay < 100) |
{ |
if(!PcZugriff) |
{ |
if(BeepModulation == 0xFFFF) |
{ |
BeepTime = 15000; // 1.5 seconds |
BeepModulation = 0x0C00; |
} |
} |
if(RcLostTimer) RcLostTimer--; |
else |
{ |
MotorsOn = 0; |
EmergencyLanding = 0; |
} |
ROT_ON; |
if(modell_fliegt > 2000) // wahrscheinlich in der Luft --> langsam absenken |
{ |
GasMixingFraction = ParamSet.EmergencyGas; |
EmergencyLanding = 1; |
PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] = 0; |
PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] = 0; |
PPM_in[ParamSet.ChannelAssignment[CH_YAW]] = 0; |
} |
else MotorsOn = 0; |
} |
else |
// SenderOkay is incremented at good rc-level, i.e. if the ppm-signal deviation |
// of a channel to previous frame is less than 1% the SenderOkay is incremented by 10. |
// Typicaly within a frame of 8 channels (22.5ms) the SenderOkay is incremented by 8 * 10 = 80 |
// The decremtation of 1 in the mainloop is done every 2 ms, i.e. within a time of one rc frame |
// the main loop is running 11 times that decrements the SenderOkay by 11. |
if(SenderOkay < 100) // the rc-frame signal is not reveived or noisy |
{ |
if(!PcAccess) // if also no PC-Access via UART |
{ |
if(BeepModulation == 0xFFFF) |
{ |
BeepTime = 15000; // 1.5 seconds |
BeepModulation = 0x0C00; |
} |
} |
if(RcLostTimer) RcLostTimer--; // decremtent timer after rc sigal lost |
else // rc lost countdown finished |
{ |
MotorsOn = 0; // stop all motors |
EmergencyLanding = 0; // emergency landing is over |
} |
ROT_ON; // set red led |
if(Modell_Is_Flying > 2000) // wahrscheinlich in der Luft --> langsam absenken |
{ |
GasMixingFraction = ParamSet.EmergencyGas; // set emergency gas |
EmergencyLanding = 1; // enable emergency landing |
// set neutral rc inputs |
PPM_diff[ParamSet.ChannelAssignment[CH_PITCH]] = 0; |
PPM_diff[ParamSet.ChannelAssignment[CH_ROLL]] = 0; |
PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] = 0; |
PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] = 0; |
PPM_in[ParamSet.ChannelAssignment[CH_YAW]] = 0; |
} |
else MotorsOn = 0; // switch of all motors |
} |
else |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Emfang gut |
// RC-signal is good |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(SenderOkay > 140) |
{ |
EmergencyLanding = 0; |
RcLostTimer = ParamSet.EmergencyGasDuration * 50; |
if(GasMixingFraction > 40) |
{ |
if(modell_fliegt < 0xffff) modell_fliegt++; |
} |
if((modell_fliegt < 200) || (GasMixingFraction < 40)) |
{ |
SumPitch = 0; |
SumRoll = 0; |
Reading_IntegralYaw = 0; |
Reading_IntegralYaw2 = 0; |
} |
if((PPM_in[ParamSet.ChannelAssignment[CH_GAS]] > 80) && MotorsOn == 0) |
{ |
if(SenderOkay > 140) |
{ |
EmergencyLanding = 0; // switch off emergency landing if RC-signal is okay |
// reset emergency timer |
RcLostTimer = ParamSet.EmergencyGasDuration * 50; |
if(GasMixingFraction > 40) |
{ |
if(Modell_Is_Flying < 0xFFFF) Modell_Is_Flying++; |
} |
if((Modell_Is_Flying < 200) || (GasMixingFraction < 40)) |
{ |
SumPitch = 0; |
SumRoll = 0; |
Reading_IntegralYaw = 0; |
Reading_IntegralYaw2 = 0; |
} |
// if motors are off and the thrust stick is in the upper position |
if((PPM_in[ParamSet.ChannelAssignment[CH_GAS]] > 80) && MotorsOn == 0) |
{ |
// and if the yaw stick is in the leftmost position |
if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] > 75) |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// auf Nullwerte kalibrieren |
// calibrate the neutral readings of all attitude sensors |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] > 75) // Neutralwerte |
{ |
if(++delay_neutral > 200) // nicht sofort |
{ |
GRN_OFF; |
MotorsOn = 0; |
delay_neutral = 0; |
modell_fliegt = 0; |
if(PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] > 70 || abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]) > 70) |
{ |
unsigned char setting=1; |
if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > 70 && PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] < 70) setting = 1; |
if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > 70 && PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] > 70) setting = 2; |
if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] < 70 && PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] > 70) setting = 3; |
if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] <-70 && PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] > 70) setting = 4; |
if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] <-70 && PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] < 70) setting = 5; |
SetActiveParamSet(setting); // aktiven Datensatz merken |
} |
if((ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL)) // Höhenregelung aktiviert? |
{ |
if((ReadingAirPressure > 950) || (ReadingAirPressure < 750)) SearchAirPressureOffset(); |
} |
ParamSet_ReadFromEEProm(GetActiveParamSet()); |
SetNeutral(); |
Beep(GetActiveParamSet()); |
} |
} |
else |
if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] < -75) // ACC Neutralwerte speichern |
{ |
if(++delay_neutral > 200) // nicht sofort |
{ |
GRN_OFF; |
SetParamWord(PID_ACC_PITCH,0xFFFF); // Werte löschen |
MotorsOn = 0; |
delay_neutral = 0; |
modell_fliegt = 0; |
SetNeutral(); |
// ACC-NeutralWerte speichern |
SetParamWord(PID_ACC_PITCH, (uint16_t)NeutralAccX); |
SetParamWord(PID_ACC_ROLL, (uint16_t)NeutralAccY); |
SetParamWord(PID_ACC_Z, (uint16_t)NeutralAccZ); |
Beep(GetActiveParamSet()); |
} |
} |
else delay_neutral = 0; |
} |
{ |
if(++delay_neutral > 200) // not immediately (wait 200 loops = 200 * 2ms = 0.4 s) |
{ |
delay_neutral = 0; |
GRN_OFF; |
Modell_Is_Flying = 0; |
// check roll/pitch stick position |
// if pitch stick is topmost or roll stick is leftmost --> change parameter setting |
// according to roll/pitch stick position |
if(PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] > 70 || abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]) > 70) |
{ |
uint8_t setting = 1; // default |
// _________ |
// |2 3 4| |
// | | |
// |1 5| |
// | | |
// |_________| |
// |
// roll stick leftmost and pitch stick centered --> setting 1 |
if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > 70 && PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] < 70) setting = 1; |
// roll stick leftmost and pitch stick topmost --> setting 2 |
if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > 70 && PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] > 70) setting = 2; |
// roll stick centered an pitch stick topmost --> setting 3 |
if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] < 70 && PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] > 70) setting = 3; |
// roll stick rightmost and pitch stick topmost --> setting 4 |
if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] <-70 && PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] > 70) setting = 4; |
// roll stick rightmost and pitch stick centered --> setting 5 |
if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] <-70 && PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] < 70) setting = 5; |
// update active parameter set in eeprom |
SetActiveParamSet(setting); |
} |
ParamSet_ReadFromEEProm(GetActiveParamSet()); |
SetNeutral(); |
Beep(GetActiveParamSet()); |
} |
} |
// and if the yaw stick is in the rightmost position |
// save the ACC neutral setting to eeprom |
else if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] < -75) |
{ |
if(++delay_neutral > 200) // not immediately (wait 200 loops = 200 * 2ms = 0.4 s) |
{ |
delay_neutral = 0; |
GRN_OFF; |
SetParamWord(PID_ACC_PITCH, 0xFFFF); // make value invalid |
Modell_Is_Flying = 0; |
SetNeutral(); |
// Save ACC neutral settings to eeprom |
SetParamWord(PID_ACC_PITCH, (uint16_t)NeutralAccX); |
SetParamWord(PID_ACC_ROLL, (uint16_t)NeutralAccY); |
SetParamWord(PID_ACC_Z, (uint16_t)NeutralAccZ); |
Beep(GetActiveParamSet()); |
} |
} |
else delay_neutral = 0; |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Gas ist unten |
// thrust stick is down |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(PPM_in[ParamSet.ChannelAssignment[CH_GAS]] < 35-120) |
{ |
// Starten |
if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] < -75) |
{ |
if(PPM_in[ParamSet.ChannelAssignment[CH_GAS]] < -85) |
{ |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Einschalten |
// and yaw stick is rightmost --> start motors |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(++delay_einschalten > 200) |
{ |
delay_einschalten = 200; |
modell_fliegt = 1; |
MotorsOn = 1; |
SetPointYaw = 0; |
Reading_IntegralYaw = 0; |
Reading_IntegralYaw2 = 0; |
Reading_IntegralPitch = 0; |
Reading_IntegralRoll = 0; |
Reading_IntegralPitch2 = IntegralPitch; |
Reading_IntegralRoll2 = IntegralRoll; |
SumPitch = 0; |
SumRoll = 0; |
} |
} |
else delay_einschalten = 0; |
//Auf Neutralwerte setzen |
if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] < -75) |
{ |
if(++delay_startmotors > 200) // not immediately (wait 200 loops = 200 * 2ms = 0.4 s) |
{ |
delay_startmotors = 200; // do not repeat if once executed |
Modell_Is_Flying = 1; |
MotorsOn = 1; |
SetPointYaw = 0; |
Reading_IntegralYaw = 0; |
Reading_IntegralYaw2 = 0; |
Reading_IntegralPitch = 0; |
Reading_IntegralRoll = 0; |
Reading_IntegralPitch2 = IntegralPitch; |
Reading_IntegralRoll2 = IntegralRoll; |
SumPitch = 0; |
SumRoll = 0; |
} |
} |
else delay_startmotors = 0; // reset delay timer if sticks are not in this position |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Auschalten |
// and yaw stick is leftmost --> stop motors |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] > 75) |
{ |
if(++delay_ausschalten > 200) // nicht sofort |
{ |
MotorsOn = 0; |
delay_ausschalten = 200; |
modell_fliegt = 0; |
} |
} |
else delay_ausschalten = 0; |
} |
} |
if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] > 75) |
{ |
if(++delay_stopmotors > 200) // not immediately (wait 200 loops = 200 * 2ms = 0.4 s) |
{ |
delay_stopmotors = 200; // do not repeat if once executed |
Modell_Is_Flying = 0; |
MotorsOn = 0; |
|
} |
} |
else delay_stopmotors = 0; // reset delay timer if sticks are not in this position |
} |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// neue Werte von der Funke |
// new values from RC |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(!NewPpmData-- || EmergencyLanding) |
{ |
int tmp_int; |
ParameterMapping(); |
StickPitch = (StickPitch * 3 + PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] * ParamSet.Stick_P) / 4; |
StickPitch += PPM_diff[ParamSet.ChannelAssignment[CH_PITCH]] * ParamSet.Stick_D; |
StickRoll = (StickRoll * 3 + PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] * ParamSet.Stick_P) / 4; |
StickRoll += PPM_diff[ParamSet.ChannelAssignment[CH_ROLL]] * ParamSet.Stick_D; |
if(!NewPpmData-- || EmergencyLanding) // NewData = 0 means new data from RC |
{ |
int tmp_int; |
ParameterMapping(); // remapping params (online poti replacement) |
|
StickYaw = -PPM_in[ParamSet.ChannelAssignment[CH_YAW]]; |
StickGas = PPM_in[ParamSet.ChannelAssignment[CH_GAS]] + 120; |
// calculate Stick inputs by rc channels (P) and changing of rc channels (D) |
StickPitch = (StickPitch * 3 + PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] * ParamSet.Stick_P) / 4; |
StickPitch += PPM_diff[ParamSet.ChannelAssignment[CH_PITCH]] * ParamSet.Stick_D; |
StickRoll = (StickRoll * 3 + PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] * ParamSet.Stick_P) / 4; |
StickRoll += PPM_diff[ParamSet.ChannelAssignment[CH_ROLL]] * ParamSet.Stick_D; |
|
if(abs(PPM_in[ParamSet.ChannelAssignment[CH_PITCH]]) > MaxStickPitch) |
MaxStickPitch = abs(PPM_in[ParamSet.ChannelAssignment[CH_PITCH]]); else MaxStickPitch--; |
if(abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]) > MaxStickRoll) |
MaxStickRoll = abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]); else MaxStickRoll--; |
if(EmergencyLanding) {MaxStickPitch = 0; MaxStickRoll = 0;} |
// direct mapping of yaw and gas |
StickYaw = -PPM_in[ParamSet.ChannelAssignment[CH_YAW]]; |
StickGas = PPM_in[ParamSet.ChannelAssignment[CH_GAS]] + 120;// shift to positive numbers |
|
GyroFactor = ((float)FCParam.Gyro_P + 10.0) / 256.0; |
IntegralFactor = ((float) FCParam.Gyro_I) / 44000; |
// 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--; |
if(abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]) > MaxStickRoll) |
MaxStickRoll = abs(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]); |
else MaxStickRoll--; |
|
// update gyro control loop factors |
|
Gyro_P_Factor = ((float) FCParam.Gyro_P + 10.0) / 256.0; |
Gyro_I_Factor = ((float) FCParam.Gyro_I) / 44000; |
|
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//+ Digitale Steuerung per DubWise |
// Digital Control via DubWise |
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
#define KEY_VALUE (FCParam.UserParam1 * 4) //(Poti3 * 8) |
if(DubWiseKeys[1]) BeepTime = 10; |
if(DubWiseKeys[1] & DUB_KEY_UP) tmp_int = KEY_VALUE; else |
if(DubWiseKeys[1] & DUB_KEY_DOWN) tmp_int = -KEY_VALUE; else tmp_int = 0; |
ExternStickPitch= (ExternStickPitch* 7 + tmp_int) / 8; |
if(DubWiseKeys[1] & DUB_KEY_LEFT) tmp_int = KEY_VALUE; else |
if(DubWiseKeys[1] & DUB_KEY_RIGHT) tmp_int = -KEY_VALUE; else tmp_int = 0; |
ExternStickRoll = (ExternStickRoll * 7 + tmp_int) / 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--; |
#define KEY_VALUE (FCParam.UserParam1 * 4) // step width |
if(DubWiseKeys[1]) BeepTime = 10; |
if(DubWiseKeys[1] & DUB_KEY_UP) tmp_int = KEY_VALUE; |
else if(DubWiseKeys[1] & DUB_KEY_DOWN) tmp_int = -KEY_VALUE; |
else tmp_int = 0; |
ExternStickPitch = (ExternStickPitch * 7 + tmp_int) / 8; |
if(DubWiseKeys[1] & DUB_KEY_LEFT) tmp_int = KEY_VALUE; |
else if(DubWiseKeys[1] & DUB_KEY_RIGHT) tmp_int = -KEY_VALUE; |
else tmp_int = 0; |
ExternStickRoll = (ExternStickRoll * 7 + tmp_int) / 8; |
|
StickPitch += ExternStickPitch/ 8; |
StickRoll += ExternStickRoll / 8; |
StickYaw += ExternStickYaw; |
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--; |
|
StickPitch += ExternStickPitch / 8; |
StickRoll += ExternStickRoll / 8; |
StickYaw += ExternStickYaw; |
|
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
//+ Analoge Steuerung per Seriell |
//+ Analoge Control via serial communication |
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(ExternControl.Config & 0x01 && FCParam.UserParam1 > 128) |
{ |
StickPitch += (int) ExternControl.Pitch * (int) ParamSet.Stick_P; |
StickRoll += (int) ExternControl.Roll * (int) ParamSet.Stick_P; |
StickYaw += ExternControl.Yaw; |
ExternHightValue = (int) ExternControl.Hight * (int)ParamSet.Hight_Gain; |
if(ExternControl.Gas < StickGas) StickGas = ExternControl.Gas; |
} |
|
if(ParamSet.GlobalConfig & CFG_HEADING_HOLD) IntegralFactor = 0; |
if(GyroFactor < 0) GyroFactor = 0; |
if(IntegralFactor < 0) IntegralFactor = 0; |
if(ExternControl.Config & 0x01 && FCParam.UserParam1 > 128) |
{ |
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; |
if(ExternControl.Gas < StickGas) StickGas = ExternControl.Gas; |
} |
// disable I part of gyro control feedback |
if(ParamSet.GlobalConfig & CFG_HEADING_HOLD) Gyro_I_Factor = 0; |
// avoid negative scaling factors |
if(Gyro_P_Factor < 0) Gyro_P_Factor = 0; |
if(Gyro_I_Factor < 0) Gyro_I_Factor = 0; |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Looping? |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if((PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_LEFT) Looping_Left = 1; |
else |
{ |
{ |
if((PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] < (ParamSet.LoopThreshold - ParamSet.LoopHysteresis))) Looping_Left = 0; |
} |
} |
if((PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] < -ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_RIGHT) Looping_Right = 1; |
else |
{ |
if(Looping_Right) // Hysterese |
{ |
if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > -(ParamSet.LoopThreshold - ParamSet.LoopHysteresis)) Looping_Right = 0; |
} |
} |
|
if((PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] > ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_UP) Looping_Top = 1; |
else |
{ |
if(Looping_Top) // Hysterese |
{ |
if((PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] < (ParamSet.LoopThreshold - ParamSet.LoopHysteresis))) Looping_Top = 0; |
} |
} |
if((PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] < -ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_DOWN) Looping_Down = 1; |
else |
{ |
if(Looping_Down) // Hysterese |
{ |
if(PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] > -(ParamSet.LoopThreshold - ParamSet.LoopHysteresis)) Looping_Down = 0; |
} |
} |
if((PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_LEFT) Looping_Left = 1; |
else |
{ |
{ |
if((PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] < (ParamSet.LoopThreshold - ParamSet.LoopHysteresis))) Looping_Left = 0; |
} |
} |
if((PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] < -ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_RIGHT) Looping_Right = 1; |
else |
{ |
if(Looping_Right) // Hysterese |
{ |
if(PPM_in[ParamSet.ChannelAssignment[CH_ROLL]] > -(ParamSet.LoopThreshold - ParamSet.LoopHysteresis)) Looping_Right = 0; |
} |
} |
|
if(Looping_Left || Looping_Right) Looping_Roll = 1; else Looping_Roll = 0; |
if(Looping_Top || Looping_Down) {Looping_Pitch = 1; Looping_Roll = 0; Looping_Left = 0; Looping_Right = 0;} else Looping_Pitch = 0; |
} // Ende neue Funken-Werte |
if((PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] > ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_UP) Looping_Top = 1; |
else |
{ |
if(Looping_Top) // Hysterese |
{ |
if((PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] < (ParamSet.LoopThreshold - ParamSet.LoopHysteresis))) Looping_Top = 0; |
} |
} |
if((PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] < -ParamSet.LoopThreshold) && ParamSet.LoopConfig & CFG_LOOP_DOWN) Looping_Down = 1; |
else |
{ |
if(Looping_Down) // Hysterese |
{ |
if(PPM_in[ParamSet.ChannelAssignment[CH_PITCH]] > -(ParamSet.LoopThreshold - ParamSet.LoopHysteresis)) Looping_Down = 0; |
} |
} |
|
if(Looping_Roll) BeepTime = 100; |
if(Looping_Roll || Looping_Pitch) |
{ |
if(GasMixingFraction > ParamSet.LoopGasLimit) GasMixingFraction = ParamSet.LoopGasLimit; |
} |
if(Looping_Left || Looping_Right) Looping_Roll = 1; else Looping_Roll = 0; |
if(Looping_Top || Looping_Down) {Looping_Pitch = 1; Looping_Roll = 0; Looping_Left = 0; Looping_Right = 0;} else Looping_Pitch = 0; |
} // End of new RC values |
|
|
if(Looping_Roll) BeepTime = 100; |
if(Looping_Roll || Looping_Pitch) |
{ |
if(GasMixingFraction > ParamSet.LoopGasLimit) GasMixingFraction = ParamSet.LoopGasLimit; |
} |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Bei Empfangsausfall im Flug |
// in case of emergency landing |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(EmergencyLanding) |
{ |
StickYaw = 0; |
StickPitch = 0; |
StickRoll = 0; |
GyroFactor = 0.5;//Originalwert von Holger 0.1; |
IntegralFactor = 0.003; //Originalwert von Holger 0.005; |
Looping_Roll = 0; |
Looping_Pitch = 0; |
} |
|
if(EmergencyLanding) |
{ |
StickYaw = 0; |
StickPitch = 0; |
StickRoll = 0; |
Gyro_P_Factor = 0.5; |
Gyro_I_Factor = 0.003; |
Looping_Roll = 0; |
Looping_Pitch = 0; |
MaxStickPitch = 0; |
MaxStickRoll = 0; |
} |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Integrale auf ACC-Signal abgleichen |
// Trim Gyro-Integrals to ACC-Signals |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
#define ABGLEICH_ANZAHL 256L |
|
MeanIntegralPitch += IntegralPitch; // Für die Mittelwertbildung aufsummieren |
MeanIntegralRoll += IntegralRoll; |
MeanIntegralPitch2 += IntegralPitch2; |
MeanIntegralRoll2 += IntegralRoll2; |
#define ABGLEICH_ANZAHL 256L |
|
if(Looping_Pitch || Looping_Roll) |
{ |
IntegralAccPitch = 0; |
IntegralAccRoll = 0; |
MeanIntegralPitch = 0; |
MeanIntegralRoll = 0; |
MeanIntegralPitch2 = 0; |
MeanIntegralRoll2 = 0; |
Reading_IntegralPitch2 = Reading_IntegralPitch; |
Reading_IntegralRoll2 = Reading_IntegralRoll; |
ZaehlMessungen = 0; |
AttitudeCorrectionPitch = 0; |
AttitudeCorrectionRoll = 0; |
} |
MeanIntegralPitch += IntegralPitch; // sum for averaging |
MeanIntegralRoll += IntegralRoll; |
MeanIntegralPitch2 += IntegralPitch2; |
MeanIntegralRoll2 += IntegralRoll2; |
|
if(Looping_Pitch || Looping_Roll) // if looping in any direction |
{ |
IntegralAccPitch = 0; |
IntegralAccRoll = 0; |
MeanIntegralPitch = 0; |
MeanIntegralRoll = 0; |
MeanIntegralPitch2 = 0; |
MeanIntegralRoll2 = 0; |
Reading_IntegralPitch2 = Reading_IntegralPitch; |
Reading_IntegralRoll2 = Reading_IntegralRoll; |
ZaehlMessungen = 0; |
AttitudeCorrectionPitch = 0; |
AttitudeCorrectionRoll = 0; |
} |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(!Looping_Pitch && !Looping_Roll) |
{ |
long tmp_long, tmp_long2; |
tmp_long = (long)(IntegralPitch / ParamSet.GyroAccFaktor - (long)Mean_AccPitch); |
tmp_long2 = (long)(IntegralRoll / ParamSet.GyroAccFaktor - (long)Mean_AccRoll); |
tmp_long /= 16; |
tmp_long2 /= 16; |
if((MaxStickPitch > 15) || (MaxStickRoll > 15)) |
{ |
tmp_long /= 3; |
tmp_long2 /= 3; |
} |
if(abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]) > 25) |
{ |
tmp_long /= 3; |
tmp_long2 /= 3; |
} |
if(!Looping_Pitch && !Looping_Roll) // if not lopping in any direction |
{ |
int32_t tmp_long, tmp_long2; |
tmp_long = (int32_t)(IntegralPitch / ParamSet.GyroAccFaktor - (int32_t)Mean_AccPitch); |
tmp_long2 = (int32_t)(IntegralRoll / ParamSet.GyroAccFaktor - (int32_t)Mean_AccRoll); |
tmp_long /= 16; |
tmp_long2 /= 16; |
if((MaxStickPitch > 15) || (MaxStickRoll > 15)) |
{ |
tmp_long /= 3; |
tmp_long2 /= 3; |
} |
if(abs(PPM_in[ParamSet.ChannelAssignment[CH_YAW]]) > 25) |
{ |
tmp_long /= 3; |
tmp_long2 /= 3; |
} |
|
#define AUSGLEICH 32 |
if(tmp_long > AUSGLEICH) tmp_long = AUSGLEICH; |
if(tmp_long < -AUSGLEICH) tmp_long =-AUSGLEICH; |
if(tmp_long2 > AUSGLEICH) tmp_long2 = AUSGLEICH; |
if(tmp_long2 <-AUSGLEICH) tmp_long2 =-AUSGLEICH; |
#define AUSGLEICH 32 |
if(tmp_long > AUSGLEICH) tmp_long = AUSGLEICH; |
if(tmp_long < -AUSGLEICH) tmp_long =-AUSGLEICH; |
if(tmp_long2 > AUSGLEICH) tmp_long2 = AUSGLEICH; |
if(tmp_long2 <-AUSGLEICH) tmp_long2 =-AUSGLEICH; |
|
Reading_IntegralPitch -= tmp_long; |
Reading_IntegralRoll -= tmp_long2; |
} |
Reading_IntegralPitch -= tmp_long; |
Reading_IntegralRoll -= tmp_long2; |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
if(ZaehlMessungen >= ABGLEICH_ANZAHL) |
915,7 → 952,7 |
AttitudeCorrectionPitch = 0; |
} |
|
if(!IntegralFactor) { AttitudeCorrectionRoll = 0; AttitudeCorrectionPitch = 0;} // z.B. bei HH |
if(!Gyro_I_Factor) { AttitudeCorrectionRoll = 0; AttitudeCorrectionPitch = 0;} // z.B. bei HH |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++ |
MeanIntegralPitch_old = MeanIntegralPitch; |
MeanIntegralRoll_old = MeanIntegralRoll; |
929,7 → 966,7 |
MeanIntegralRoll2 = 0; |
ZaehlMessungen = 0; |
} |
//DebugOut.Analog[31] = StickRoll / (26*IntegralFactor); |
//DebugOut.Analog[31] = StickRoll / (26*Gyro_I_Factor); |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Gieren |
1025,14 → 1062,14 |
//DebugOut.Analog[26] = ReadingPitch; |
//DebugOut.Analog[28] = ReadingRoll; |
|
if(Looping_Pitch) ReadingPitch = ReadingPitch * GyroFactor; |
else ReadingPitch = IntegralPitch * IntegralFactor + ReadingPitch * GyroFactor; |
if(Looping_Roll) ReadingRoll = ReadingRoll * GyroFactor; |
else ReadingRoll = IntegralRoll * IntegralFactor + ReadingRoll * GyroFactor; |
ReadingYaw = ReadingYaw * (2 * GyroFactor) + IntegralYaw * IntegralFactor / 2; |
if(Looping_Pitch) ReadingPitch = ReadingPitch * Gyro_P_Factor; |
else ReadingPitch = IntegralPitch * Gyro_I_Factor + ReadingPitch * Gyro_P_Factor; |
if(Looping_Roll) ReadingRoll = ReadingRoll * Gyro_P_Factor; |
else ReadingRoll = IntegralRoll * Gyro_I_Factor + ReadingRoll * Gyro_P_Factor; |
ReadingYaw = ReadingYaw * (2 * Gyro_P_Factor) + IntegralYaw * Gyro_I_Factor / 2; |
|
DebugOut.Analog[25] = IntegralRoll * IntegralFactor; |
DebugOut.Analog[31] = StickRoll;// / (26*IntegralFactor); |
DebugOut.Analog[25] = IntegralRoll * Gyro_I_Factor; |
DebugOut.Analog[31] = StickRoll;// / (26*Gyro_I_Factor); |
DebugOut.Analog[28] = ReadingRoll; |
|
// Maximalwerte abfangen |
1111,7 → 1148,7 |
// Pitch-Achse |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
DiffPitch = ReadingPitch - (StickPitch - GPS_Pitch); // Differenz bestimmen |
if(IntegralFactor) SumPitch += IntegralPitch * IntegralFactor - (StickPitch - GPS_Pitch); // I-Anteil bei Winkelregelung |
if(Gyro_I_Factor) SumPitch += IntegralPitch * Gyro_I_Factor - (StickPitch - GPS_Pitch); // I-Anteil bei Winkelregelung |
else SumPitch += DiffPitch; // I-Anteil bei HH |
if(SumPitch > 16000) SumPitch = 16000; |
if(SumPitch < -16000) SumPitch = -16000; |
1136,7 → 1173,7 |
// Roll-Achse |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
DiffRoll = ReadingRoll - (StickRoll - GPS_Roll); // Differenz bestimmen |
if(IntegralFactor) SumRoll += IntegralRoll * IntegralFactor - (StickRoll - GPS_Roll);// I-Anteil bei Winkelregelung |
if(Gyro_I_Factor) SumRoll += IntegralRoll * Gyro_I_Factor - (StickRoll - GPS_Roll);// I-Anteil bei Winkelregelung |
else SumRoll += DiffRoll; // I-Anteil bei HH |
if(SumRoll > 16000) SumRoll = 16000; |
if(SumRoll < -16000) SumRoll = -16000; |