Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 1451 → Rev 1452

/branches/V0.76g-acid/fc.c
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;
}
else Motor[i] = 0;
}
/*
if(Poti1 > 20) Motor1 = 0;
if(Poti1 > 90) Motor6 = 0;
if(Poti1 > 140) Motor2 = 0;
//if(Poti1 > 200) Motor7 = 0;
*/
{
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;
}
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]);
}
 
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];
 
}
 
}