80,7 → 80,6 |
volatile long Mess_Integral_Hoch = 0; |
int KompassValue = 0; |
int KompassStartwert = 0; |
int KompassEinschaltStartwert = 0; |
int KompassRichtung = 0; |
unsigned int KompassSignalSchlecht = 500; |
unsigned char MAX_GAS,MIN_GAS; |
163,7 → 162,14 |
#define LIMIT_MIN(value, min) {if(value < min) value = min;} |
#define LIMIT_MAX(value, max) {if(value > max) value = max;} |
#define LIMIT_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;} |
uint8_t servoValues[6] = { 127, 127, 127, 127, 127 , 127 }; |
|
uint8_t MakeByte(int16_t i) { |
if (i < 0) return 0; |
if (i > 255) return 255; |
return i; |
} |
|
int MotorSmoothing(int neu, int alt) |
{ |
int motor; |
251,7 → 257,6 |
VarioMeter = 0; |
Mess_Integral_Hoch = 0; |
KompassStartwert = KompassValue; |
KompassEinschaltStartwert = KompassValue; |
GPS_Neutral(); |
beeptime = 50; |
Umschlag180Nick = ((long) EE_Parameter.WinkelUmschlagNick * 2500L) + 15000L; |
473,21 → 478,10 |
{ |
unsigned char i; |
if(!MotorenEin) |
{ |
MikroKopterFlags &= ~(FLAG_MOTOR_RUN | FLAG_FLY); |
for(i=0;i<MAX_MOTORS;i++) |
{ |
if(!PC_MotortestActive) MotorTest[i] = 0; |
Motor[i] = MotorTest[i]; |
} |
if(PC_MotortestActive) PC_MotortestActive--; |
} |
else MikroKopterFlags |= FLAG_MOTOR_RUN; |
else |
MikroKopterFlags |= FLAG_MOTOR_RUN; |
|
DebugOut.Analog[12] = Motor[0]; |
DebugOut.Analog[13] = Motor[1]; |
DebugOut.Analog[14] = Motor[3]; |
DebugOut.Analog[15] = Motor[2]; |
|
//Start I2C Interrupt Mode |
twi_state = 0; |
540,7 → 534,6 |
MIN_GAS = EE_Parameter.Gas_Min; |
} |
|
|
//############################################################################ |
// |
void MotorRegler(void) |
599,6 → 592,7 |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(SenderOkay > 140) |
{ |
|
MikroKopterFlags &= ~FLAG_NOTLANDUNG; |
RcLostTimer = EE_Parameter.NotGasZeit * 50; |
if(GasMischanteil > 40 && MotorenEin) |
697,6 → 691,7 |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(++delay_einschalten > 200) |
{ |
beeptime = 1000; |
delay_einschalten = 200; |
modell_fliegt = 1; |
MotorenEin = 1; |
710,7 → 705,6 |
SummeNick = 0; |
SummeRoll = 0; |
MikroKopterFlags |= FLAG_START; |
KompassEinschaltStartwert = KompassValue; |
} |
} |
else delay_einschalten = 0; |
722,6 → 716,7 |
{ |
if(++delay_ausschalten > 200) // nicht sofort |
{ |
beeptime = 1000; |
MotorenEin = 0; |
delay_ausschalten = 200; |
modell_fliegt = 0; |
737,77 → 732,13 |
if(!NewPpmData-- || (MikroKopterFlags & FLAG_NOTLANDUNG)) |
{ |
static int stick_nick,stick_roll; |
int e_stick_nick, e_stick_nick_diff, e_stick_roll, e_stick_roll_diff; |
int stick_p, stick_d; |
|
ParameterZuordnung(); |
|
stick_p = EE_Parameter.Stick_P; |
stick_d = EE_Parameter.Stick_D; |
|
if (Parameter_UserParam8) { |
|
int angle, stick_nick_tmp, stick_roll_tmp; |
|
if (Parameter_UserParam8 == 254) { |
angle = -45; |
} else if (Parameter_UserParam8 > 128) { |
angle = KompassValue; |
} else { |
angle = (ErsatzKompass / GIER_GRAD_FAKTOR); |
} |
|
angle = ((KompassEinschaltStartwert - angle + 360) % 360); |
|
stick_nick_tmp = (long)c_cos_8192(angle) * (long)PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] / 8192L; |
stick_roll_tmp = (long)c_sin_8192(angle) * (long)PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] / 8192L; |
|
e_stick_nick = stick_roll_tmp + stick_nick_tmp; |
e_stick_roll = stick_roll_tmp - stick_nick_tmp; |
|
stick_nick_tmp = (long)c_cos_8192(angle) * (long)PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] / 8192L; |
stick_roll_tmp = (long)c_sin_8192(angle) * (long)PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] / 8192L; |
|
e_stick_nick_diff = stick_roll_tmp + stick_nick_tmp; |
e_stick_roll_diff = stick_roll_tmp - stick_nick_tmp; |
|
if (Parameter_UserParam8 == 255) { |
|
int gier = abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]); |
if (gier > 100) { |
stick_d /= 3; |
} else if (gier > 50) { |
stick_d /= 2; |
} |
|
if (stick_d < 1) { |
stick_d = 1; |
} |
|
} |
|
/* |
DebugOut.Analog[12] = PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]; |
DebugOut.Analog[13] = e_stick_nick; |
DebugOut.Analog[14] = PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]; |
DebugOut.Analog[15] = e_stick_roll; |
*/ |
|
} else { |
|
e_stick_nick = PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]; |
e_stick_roll = PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]; |
e_stick_nick_diff = PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]]; |
e_stick_roll_diff = PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]]; |
|
} |
|
stick_nick = (stick_nick * 3 + e_stick_nick * stick_p) / 4; |
stick_nick += e_stick_nick_diff * stick_d; |
stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4; |
stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D; |
StickNick = stick_nick - (GPS_Nick + GPS_Nick2); |
|
stick_roll = (stick_roll * 3 + e_stick_roll * stick_p) / 4; |
stick_roll += e_stick_roll_diff * stick_d; |
stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 4; |
stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D; |
StickRoll = stick_roll - (GPS_Roll + GPS_Roll2); |
|
StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]; |
1233,7 → 1164,7 |
DebugOut.Analog[3] = Mittelwert_AccRoll / 4; |
DebugOut.Analog[4] = MesswertGier; |
DebugOut.Analog[5] = HoehenWert/5; |
DebugOut.Analog[6] = Aktuell_az;//(Mess_Integral_Hoch / 512);//Aktuell_az; |
//DebugOut.Analog[6] = Aktuell_az;//(Mess_Integral_Hoch / 512);//Aktuell_az; |
DebugOut.Analog[8] = KompassValue; |
DebugOut.Analog[9] = UBat; |
DebugOut.Analog[10] = SenderOkay; |
1594,7 → 1525,7 |
modell_fliegt = 1; |
GasMischanteil = MIN_GAS; |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
/*// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Mischer und PI-Regler |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
DebugOut.Analog[7] = GasMischanteil; |
1642,31 → 1573,69 |
pd_ergebnis_roll = DiffRoll + SummeRoll / Ki; // PI-Regler für Roll |
tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64; |
if(pd_ergebnis_roll > tmp_int) pd_ergebnis_roll = tmp_int; |
if(pd_ergebnis_roll < -tmp_int) pd_ergebnis_roll = -tmp_int; |
if(pd_ergebnis_roll < -tmp_int) pd_ergebnis_roll = -tmp_int;*/ |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Universal Mixer |
// Servo Mixer |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
for(i=0; i<MAX_MOTORS; i++) |
{ |
signed int tmp_int; |
if(Mixer.Motor[i][0] > 0) |
{ |
tmp_int = ((long)GasMischanteil * Mixer.Motor[i][0]) / 64L; |
tmp_int += ((long)pd_ergebnis_nick * Mixer.Motor[i][1]) / 64L; |
tmp_int += ((long)pd_ergebnis_roll * Mixer.Motor[i][2]) / 64L; |
tmp_int += ((long)GierMischanteil * Mixer.Motor[i][3]) / 64L; |
tmp_motorwert[i] = MotorSmoothing(tmp_int,tmp_motorwert[i]); // Filter |
tmp_int = tmp_motorwert[i] / STICK_GAIN; |
CHECK_MIN_MAX(tmp_int,MIN_GAS,MAX_GAS); |
Motor[i] = tmp_int; |
int16_t nick, roll, tmp, pitch, throttle; |
int16_t servo[6]; |
|
// throttle |
if (MotorenEin == 0 || MikroKopterFlags & FLAG_NOTLANDUNG) { |
tmp = 0; |
} else { |
throttle = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + Mixer.Motor[2][0]; |
if (throttle < 0) throttle = 0; |
tmp = throttle / Mixer.Motor[3][0]; |
if (tmp > Mixer.Motor[3][1] * 2) tmp = Mixer.Motor[3][1] * 2; |
} |
else Motor[i] = 0; |
servo[4] = tmp; |
|
// pitch |
pitch = throttle / Mixer.Motor[4][0] + Mixer.Motor[4][2]; |
if (pitch > Mixer.Motor[4][1]) pitch = Mixer.Motor[4][1]; |
|
servo[0] = -pitch; |
servo[1] = pitch; |
servo[2] = pitch; |
|
|
nick = IntegralNick / (EE_Parameter.GyroAccFaktor * 4); |
roll = IntegralRoll / (EE_Parameter.GyroAccFaktor * 4); |
|
// nick |
nick /= Mixer.Motor[0][1]; |
nick -= PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] / Mixer.Motor[5][0]; |
servo[0] += 127 + Mixer.Motor[0][0] - nick; |
servo[1] += 127 + Mixer.Motor[0][0] + nick; |
servo[2] += 127 + Mixer.Motor[0][0] - nick; |
|
// roll |
roll /= Mixer.Motor[0][2]; |
roll -= PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] / Mixer.Motor[6][0]; |
servo[0] += roll; |
servo[1] += roll; |
|
// gier |
tmp = MesswertGier / Mixer.Motor[0][3]; |
servo[3] = 127 + Mixer.Motor[1][1] + tmp + (PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] / Mixer.Motor[7][0]); |
|
// not used |
servo[5] = 127; |
|
for(tmp = 0; tmp < 6; tmp++) { |
servoValues[tmp] = MakeByte(servo[tmp]); |
} |
/* |
if(Poti1 > 20) Motor1 = 0; |
if(Poti1 > 90) Motor6 = 0; |
if(Poti1 > 140) Motor2 = 0; |
//if(Poti1 > 200) Motor7 = 0; |
*/ |
|
DebugOut.Analog[12] = servo[0]; |
DebugOut.Analog[13] = servo[1]; |
DebugOut.Analog[14] = servo[2]; |
DebugOut.Analog[15] = servo[3]; |
DebugOut.Analog[6] = pitch; |
DebugOut.Analog[7] = servo[4]; |
|
} |
|
} |