• MikroKopter.de
• Wiki
• SHOP
• Datenschutz
• Impressum

FlightCtrlBL-CtrlMK3MagNaviCtrlProjects

Català-Valencià – Catalan中文 – Chinese (Simplified)中文 – Chinese (Traditional)Česky – CzechDansk – DanishNederlands – DutchEnglish – EnglishSuomi – FinnishFrançais – FrenchDeutsch – Germanעברית – Hebrewहिंदी – HindiMagyar – HungarianBahasa Indonesia – IndonesianItaliano – Italian日本語 – Japanese한국어 – KoreanМакедонски – Macedonianमराठी – MarathiNorsk – NorwegianPolski – PolishPortuguês – PortuguesePortuguês – Portuguese (Brazil)Русский – RussianSlovenčina – SlovakSlovenščina – SlovenianEspañol – SpanishSvenska – SwedishTürkçe – TurkishУкраїнська – UkrainianOëzbekcha – Uzbek

Compare Revisions

• This comparison shows the changes necessary to convert path

  → / @ 2307

  → / @ 2308

  ↔ Reverse comparison

• Compare Path:	Rev

  With Path:	Rev

Ignore whitespace Rev 2307 → Rev 2308

/test_branches/FC2_2/analog.c
69,8 → 69,7
unsigned char AnalogOffsetNick = 115,AnalogOffsetRoll = 115,AnalogOffsetGier = 115;
volatile unsigned char AdReady = 1;
volatile long HoehenWertF = 0, HoehenDiff, HoeheAlt;
volatile long vv, vvSum, AccVertical;
//signed int AccZSum = 0;
volatile long VerticalVelocity, VerticalVelocitySum, AccVertical;
//#######################################################################################
void ADC_Init(void)
192,11 → 191,7
static signed int gier1, roll1, nick1, nick_filter, roll_filter;
static signed int accy, accx;
static long tmpLuftdruck = 0;
// static char messanzahl_Druck = 0;
#define ANZ_RING 32
//static long VarioRing[ANZ_RING];
//static unsigned char RingPtr = 0;
static char messanzahl_Druck = 0;
switch(state++)
{
case 0:
238,43 → 233,33
kanal = AD_ACC_Z;
break;
case 8:
{
// signed int tmp;
Aktuell_az = ADC;
//if(PlatinenVersion < 22)
//else Aktuell_az = 1024 - ADC;
// tmp = (signed int) Aktuell_az - NeutralAccZ;
AdWertAccHoch = Aktuell_az - NeutralAccZ;
}
if(PlatinenVersion < 22)
{
if(AdWertAccHoch > 1)
{
if(NeutralAccZ < 750)
if(!ACC_AltitudeControl) // The Offset must be corrected, because of the ACC-Drift from vibrations
{
if(AdWertAccHoch > 1)
{
subcount += 5;
if(modell_fliegt < 500) subcount += 10;
if(NeutralAccZ < 750)
{
subcount += 5;
if(modell_fliegt < 500) subcount += 10;
}
if(subcount > 100) { NeutralAccZ++; subcount -= 100;}
}
if(subcount > 100) { NeutralAccZ++; subcount -= 100;}
else if(AdWertAccHoch < -1)
{
if(NeutralAccZ > 550)
{
subcount -= 5;
if(modell_fliegt < 500) subcount -= 10;
if(subcount < -100) { NeutralAccZ--; subcount += 100;}
}
}
}
else if(AdWertAccHoch < -1)
{
if(NeutralAccZ > 550)
{
subcount -= 5;
if(modell_fliegt < 500) subcount -= 10;
if(subcount < -100) { NeutralAccZ--; subcount += 100;}
}
}
AdWertAccHoch = 0;
}
// messanzahl_AccHoch = 1;
Mess_Integral_Hoch += AdWertAccHoch; // Integrieren
Mess_Integral_Hoch -= Mess_Integral_Hoch / 1024; // dämfen
kanal = AD_DRUCK;
break;
// "case 9:" fehlt hier absichtlich
case 10:
nick1 += ADC;
319,70 → 304,23
AdWertRollFilter = (AdWertRollFilter + HiResRoll) / 2;
kanal = AD_DRUCK;
break;
// - neu -------------------------------------------------
case 17:
HoehenDiff = HoehenWert - HoeheAlt;
AccVertical = (long)(AdWertAccHoch + 172)*CosAttitude/8192 - 172;
vvSum = vvSum - vv + AccVertical*13 + HoehenDiff*500; // Fusion vert. velocity, T=2s
vv = (vvSum + 512)/1024; // cm/s
HoeheAlt = HoehenWert;
SummenHoehe = SummenHoehe - HoehenWertF + vv + HoehenWert; //Fusion Hoehe
HoehenWertF = (SummenHoehe + SM_FILTER/2)/SM_FILTER; // cm
state = 0;
AdReady = 1;
ZaehlMessungen++;
// "break" fehlt hier absichtlich
case 9:
MessLuftdruck = ADC;
tmpLuftdruck = MessLuftdruck - 523 * (long)ExpandBaro; // -523 counts per offset step
Luftdruck -= Luftdruck/16;
Luftdruck += tmpLuftdruck;
HoehenWert = StartLuftdruck - Luftdruck; // cm
#if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__))
if(ACC_AltitudeControl)
{
HoehenDiff = HoehenWert - HoeheAlt;
AccVertical = (long)(AdWertAccHoch + 172)*CosAttitude/8192 - 172;
VerticalVelocitySum = VerticalVelocitySum - VerticalVelocity + AccVertical*13 + HoehenDiff*500; // Fusion vert. velocity, T=2s
VerticalVelocity = (VerticalVelocitySum + 512)/1024; // cm/s
HoeheAlt = HoehenWert;
SummenHoehe = SummenHoehe - HoehenWertF + VerticalVelocity + HoehenWert; //Fusion Hoehe
HoehenWertF = (SummenHoehe + SA_FILTER/2)/SA_FILTER; // cm
}
else HoehenWertF = HoehenWert;
#else
HoehenWertF = HoehenWert;
#endif
if(PlatinenVersion > 21) HoehenWert = HoehenWert + AdWertAccHoch*41/256;
/* // Qopter: deaktivieren Anfang
if(PlatinenVersion > 21 && NeutralAccZ) MessLuftdruck -= (Aktuell_az - NeutralAccZ)/12;
tmpLuftdruck += MessLuftdruck;
if(++messanzahl_Druck >= 16) // war bis 0.86 "18"
{
signed int tmp;
long tmp_long;
Luftdruck = (7 * Luftdruck + tmpLuftdruck - (16 * 523) * (long)ExpandBaro + 4) / 8; // -523.19 counts per 10 counts offset step
HoehenWert = StartLuftdruck - Luftdruck;
if(PlatinenVersion > 21) HoehenWert = HoehenWert/3 + AdWertAccHoch/200;
VarioRing[RingPtr++] = HoehenWert;
RingPtr %= ANZ_RING;
tmp_long = (VarioRing[RingPtr] + VarioRing[(RingPtr+ANZ_RING/2)%ANZ_RING]) / 2;
DebugOut.Analog[27] = tmp_long;
// SummenHoehe -= SummenHoehe/SM_FILTER;
// SummenHoehe += HoehenWert;
//DebugOut.Analog[27] = SummenHoehe/SM_FILTER;
// tmp = (HoehenWert - SummenHoehe/SM_FILTER);
tmp = HoehenWert - DebugOut.Analog[27];//VarioRing[RingPtr];
if(tmp > 1024) tmp = 1024; else if(tmp < -1024) tmp = -1024;
//VarioMeter -= VarioMeter/8;
//VarioMeter += tmp;
// if(abs(VarioMeter) > 700) VarioMeter = (15 * VarioMeter + 8 * tmp)/16;
// else VarioMeter = (31 * VarioMeter + 8 * tmp)/32;
VarioMeter = (15 * VarioMeter + 4 * tmp)/16;
DebugOut.Analog[25] += (Aktuell_az - NeutralAccZ) - AccShorter;
DebugOut.Analog[25] -= (DebugOut.Analog[25] - VarioMeter*8)/16;
tmpLuftdruck /= 2;
messanzahl_Druck = 16/2;
}
*/ // Qopter: deaktivieren Ende
kanal = AD_NICK;
//DebugOut.Analog[16] = VarioMeter * 8;
break;
// - neu -------------------------------------------------
/*
case 17:
state = 0;
AdReady = 1;
ZaehlMessungen++;
389,9 → 327,21
// "break" fehlt hier absichtlich
case 9:
MessLuftdruck = ADC;
tmpLuftdruck += MessLuftdruck;
if(++messanzahl_Druck >= 16) // war bis 0.86 "18"
{
#if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__))
if(ACC_AltitudeControl)
{
tmpLuftdruck = MessLuftdruck - 523 * (long)ExpandBaro; // -523 counts per offset step
Luftdruck -= Luftdruck/16;
Luftdruck += tmpLuftdruck;
HoehenWert = StartLuftdruck - Luftdruck; // cm
HoehenWert = HoehenWert + AdWertAccHoch * 41 / 256; // Weight-compsensation <-------
}
else
#endif
{ // old version (until FC V2.1)
tmpLuftdruck += MessLuftdruck;
if(++messanzahl_Druck >= 16) // war bis 0.86 "18"
{
signed int tmp;
Luftdruck = (7 * Luftdruck + tmpLuftdruck - (16 * 523) * (long)ExpandBaro + 4) / 8; // -523.19 counts per 10 counts offset step
HoehenWert = StartLuftdruck - Luftdruck;
403,10 → 353,11
else VarioMeter = (31 * VarioMeter + 8 * tmp)/32;
tmpLuftdruck /= 2;
messanzahl_Druck = 16/2;
}
}
}
kanal = AD_NICK;
break;
*/ default:
default:
kanal = 0; state = 0; kanal = AD_NICK;
break;
}

/test_branches/FC2_2/analog.h
4,8 → 4,8
#######################################################################################*/
#define SM_FILTER 512
#define ACC_Z_FILTER 16
#define SM_FILTER 16
#define SA_FILTER 512
extern volatile int UBat;
extern volatile int AdWertNick, AdWertRoll, AdWertGier;
25,8 → 25,7
extern volatile char MessanzahlNick;
extern unsigned char AnalogOffsetNick,AnalogOffsetRoll,AnalogOffsetGier;
extern volatile unsigned char AdReady;
extern volatile long HoehenWertF;
extern volatile long vv, vvSum;
volatile long HoehenWertF, VerticalVelocity, VerticalVelocitySum, AccVertical;
unsigned int ReadADC(unsigned char adc_input);
void ADC_Init(void);

/test_branches/FC2_2/eeprom.c
150,11 → 150,25
EE_Parameter.Hoehe_MinGas = 30;
EE_Parameter.MaxHoehe = 255; // Wert : 0-247 255 -> Poti1
EE_Parameter.Hoehe_P = 15; // Wert : 0-32
EE_Parameter.Luftdruck_D = 30; // Wert : 0-247
EE_Parameter.Hoehe_ACC_Wirkung = 0; // Wert : 0-247
EE_Parameter.Hoehe_HoverBand = 8; // Wert : 0-247
EE_Parameter.Hoehe_GPS_Z = 20; // Wert : 0-247
#if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__))
if(ACC_AltitudeControl)
{
EE_Parameter.Hoehe_P = 20; // Wert : 0-32
EE_Parameter.Luftdruck_D = 40; // Wert : 0-247
EE_Parameter.Hoehe_ACC_Wirkung = 50; // Wert : 0-247
EE_Parameter.Hoehe_HoverBand = 1; // Wert : 0-247
EE_Parameter.Hoehe_GPS_Z = 0; // Wert : 0-247
}
else
#endif
{
EE_Parameter.Hoehe_P = 15; // Wert : 0-32
EE_Parameter.Luftdruck_D = 30; // Wert : 0-247
EE_Parameter.Hoehe_ACC_Wirkung = 0; // Wert : 0-247
EE_Parameter.Hoehe_HoverBand = 8; // Wert : 0-247
EE_Parameter.Hoehe_GPS_Z = 20; // Wert : 0-247
}
EE_Parameter.Hoehe_StickNeutralPoint = 0;// Wert : 0-247 (0 = Hover-Estimation)
EE_Parameter.Hoehe_Verstaerkung = 15; // Wert : 0-50 (15 -> ca. +/- 5m/sek bei Stick-Voll-Ausschlag)
219,8 → 233,8
EE_Parameter.ComingHomeAltitude = 0; // 0 = don't change
EE_Parameter.FailSafeTime = 0; // 0 = off
EE_Parameter.MaxAltitude = 150; // 0 = off
EE_Parameter.AchsKopplung1 = 90;
EE_Parameter.AchsKopplung2 = 55;
EE_Parameter.AchsKopplung1 = 125;
EE_Parameter.AchsKopplung2 = 52;
EE_Parameter.FailsafeChannel = 0;
EE_Parameter.ServoFilterNick = 0;
EE_Parameter.ServoFilterRoll = 0;
557,13 → 571,10
{
ee_default = 1; // software update or forced by mktool
}
// 1st check for a valid channel backup in eeprom
i = EEProm_Checksum(EEPROM_ADR_CHANNELS, sizeof(EE_Parameter.Kanalbelegung));
if(i == eeprom_read_byte((uint8_t*)(EEPROM_ADR_CHANNELS + sizeof(EE_Parameter.Kanalbelegung))) ) channel_backup = 1;
// parameter check
// check all 5 parameter settings

/test_branches/FC2_2/eeprom.h
4,7 → 4,7
#include <inttypes.h>
#include "twimaster.h"
#define EEPARAM_REVISION 92 // is count up, if paramater stucture has changed (compatibility)
#define EEPARAM_REVISION 93 // is count up, if paramater stucture has changed (compatibility)
#define EEMIXER_REVISION 1 // is count up, if mixer stucture has changed (compatibility)
#define EEPROM_ADR_PARAM_BEGIN 0

/test_branches/FC2_2/fc.c
60,7 → 60,6
unsigned char h,m,s;
unsigned int BaroExpandActive = 0;
signed int ACC_AMPLIFY = 6;
int MesswertNick,MesswertRoll,MesswertGier,MesswertGierBias, RohMesswertNick,RohMesswertRoll;
int TrimNick, TrimRoll;
int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0,StartNeutralRoll = 0,StartNeutralNick = 0;
118,7 → 117,7
unsigned char Looping_Links = 0, Looping_Rechts = 0, Looping_Unten = 0, Looping_Oben = 0;
unsigned char Parameter_Luftdruck_D = 48; // Wert : 0-250
unsigned char Parameter_HoehenSchalter = 251; // Wert : 0-250
unsigned char Parameter_HoehenSchalter = 251; // Wert : 0-250
unsigned char Parameter_Hoehe_P = 16; // Wert : 0-32
unsigned char Parameter_Hoehe_ACC_Wirkung = 58; // Wert : 0-250
unsigned char Parameter_KompassWirkung = 64; // Wert : 0-250
140,6 → 139,8
unsigned char Parameter_UserParam8 = 0;
unsigned char Parameter_ServoNickControl = 100;
unsigned char Parameter_ServoRollControl = 100;
unsigned char Parameter_ServoNickComp = 50;
unsigned char Parameter_ServoRollComp = 85;
unsigned char Parameter_LoopGasLimit = 70;
unsigned char Parameter_AchsKopplung1 = 90;
unsigned char Parameter_AchsKopplung2 = 65;
165,7 → 166,6
unsigned char Parameter_MaximumAltitude;
unsigned char Parameter_Servo3,Parameter_Servo4,Parameter_Servo5;
unsigned char CareFree = 0;
//signed char AccShorter = 0;
const signed char sintab[31] = { 0, 2, 4, 6, 7, 8, 8, 8, 7, 6, 4, 2, 0, -2, -4, -6, -7, -8, -8, -8, -7, -6, -4, -2, 0, 2, 4, 6, 7, 8, 8}; // 15° steps
signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20;
178,8 → 178,9
char VarioCharacter = ' ';
unsigned int HooverGasEmergencyPercent = 0; // The gas value for Emergency landing
unsigned int GasIsZeroCnt = 0; // to detect that the gas-stick is down for a while
int Variance = 0;
int CosAttitude; // for projection of hoover gas
signed int Variance = 0;
signed int CosAttitude; // for projection of hoover gas
unsigned char ACC_AltitudeControl = 0;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Debugwerte zuordnen
191,9 → 192,9
DebugOut.Analog[2] = Mittelwert_AccNick / 4;
DebugOut.Analog[3] = Mittelwert_AccRoll / 4;
DebugOut.Analog[4] = (signed int) AdNeutralGier - AdWertGier;
DebugOut.Analog[5] = HoehenWert;///5;
DebugOut.Analog[5] = HoehenWert/10;
DebugOut.Analog[6] = Aktuell_az;//AdWertAccHoch;//(Mess_Integral_Hoch / 512);
DebugOut.Analog[8] = AdWertAccHoch;//KompassValue;
DebugOut.Analog[8] = KompassValue;
DebugOut.Analog[9] = UBat;
DebugOut.Analog[10] = SenderOkay;
DebugOut.Analog[11] = ErsatzKompassInGrad;
201,26 → 202,25
DebugOut.Analog[13] = Motor[1].SetPoint;
DebugOut.Analog[14] = Motor[2].SetPoint;
DebugOut.Analog[15] = Motor[3].SetPoint;
DebugOut.Analog[16] = Variance;
DebugOut.Analog[17] = vv;
DebugOut.Analog[18] = HoehenWertF;
DebugOut.Analog[20] = ServoNickValue;
DebugOut.Analog[22] = Capacity.ActualCurrent;
DebugOut.Analog[23] = Capacity.UsedCapacity;
DebugOut.Analog[24] = SollHoehe;
DebugOut.Analog[25] = Parameter_Hoehe_P;
DebugOut.Analog[26] = Parameter_Luftdruck_D;
// DebugOut.Analog[22] = FromNaviCtrl_Value.GpsZ;
// DebugOut.Analog[29] = FromNaviCtrl_Value.SerialDataOkay;
DebugOut.Analog[24] = SollHoehe/10;
DebugOut.Analog[27] = KompassSollWert;
DebugOut.Analog[29] = Capacity.MinOfMaxPWM;
DebugOut.Analog[30] = GPS_Nick;
DebugOut.Analog[31] = GPS_Roll;
if(VersionInfo.HardwareError[0] || VersionInfo.HardwareError[1]) DebugOut.Status[1] |= 1; else DebugOut.Status[1] &= 0xfe;
DebugOut.Analog[16] = Variance;
DebugOut.Analog[17] = VerticalVelocity;
DebugOut.Analog[18] = HoehenWertF;
DebugOut.Analog[25] = Parameter_Hoehe_P;
DebugOut.Analog[26] = Parameter_Luftdruck_D;
}
void Piep(unsigned char Anzahl, unsigned int dauer)
{
unsigned int wait = 0;
356,9 → 356,8
Mess_Integral_Gier = 0;
StartLuftdruck = Luftdruck;
VarioMeter = 0;
vvSum = 0;
SummenHoehe = 0;
Mess_Integral_Hoch = 0;
VerticalVelocitySum = 0;
SummenHoehe = 0; Mess_Integral_Hoch = 0;
KompassSollWert = KompassValue;
KompassSignalSchlecht = 100;
beeptime = 50;
372,7 → 371,6
FromNaviCtrl_Value.Kalman_K = -1;
FromNaviCtrl_Value.Kalman_MaxDrift = 0;
FromNaviCtrl_Value.Kalman_MaxFusion = 32;
for(i=0;i<8;i++)
{
Poti[i] = PPM_in[EE_Parameter.Kanalbelegung[K_POTI1 + i]] + 127;
388,7 → 386,7
// else
NickServoValue = ((128 - 60) * 4 * 16); // neutral position = lower 1/4
}
if((AdNeutralNick < 150 * 16) || (AdNeutralNick > 850 * 16)) { VersionInfo.HardwareError[0] |= FC_ERROR0_GYRO_NICK; };
if((AdNeutralRoll < 150 * 16) || (AdNeutralRoll > 850 * 16)) { VersionInfo.HardwareError[0] |= FC_ERROR0_GYRO_ROLL; };
if((AdNeutralGier < 150 * 2) || (AdNeutralGier > 850 * 2)) { VersionInfo.HardwareError[0] |= FC_ERROR0_GYRO_YAW; };
559,6 → 557,7
if(!MotorenEin)
{
FC_StatusFlags &= ~(FC_STATUS_MOTOR_RUN | FC_STATUS_FLY);
FC_StatusFlags2 &= ~FC_STATUS2_WAIT_FOR_TAKEOFF;
for(i=0;i<MAX_MOTORS;i++)
{
if(!PC_MotortestActive) MotorTest[i] = 0;
642,6 → 641,8
CHK_POTI(Parameter_UserParam8,EE_Parameter.UserParam8);
CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl);
CHK_POTI(Parameter_ServoRollControl,EE_Parameter.ServoRollControl);
CHK_POTI(Parameter_ServoNickComp,EE_Parameter.ServoNickComp);
CHK_POTI(Parameter_ServoRollComp,EE_Parameter.ServoRollComp);
CHK_POTI(Parameter_LoopGasLimit,EE_Parameter.LoopGasLimit);
CHK_POTI(Parameter_AchsKopplung1,EE_Parameter.AchsKopplung1);
CHK_POTI(Parameter_AchsKopplung2,EE_Parameter.AchsKopplung2);
779,21 → 780,26
SummeRoll = 0;
sollGier = 0;
Mess_Integral_Gier = 0;
FC_StatusFlags2 |= FC_STATUS2_WAIT_FOR_TAKEOFF;
}
else
{
FC_StatusFlags |= FC_STATUS_FLY;
if(!(FC_StatusFlags2 & FC_STATUS2_TAKEOFF))
if(FC_StatusFlags2 & FC_STATUS2_WAIT_FOR_TAKEOFF)
{
if(HoehenWert > 150)
if(abs(HoehenWert) > 150 || !(Parameter_GlobalConfig & CFG_HOEHENREGELUNG))
{
FC_StatusFlags2 |= FC_STATUS2_TAKEOFF;
FC_StatusFlags2 &= ~FC_STATUS2_WAIT_FOR_TAKEOFF;
#if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__))
SpeakHoTT = SPEAK_RISING;
#endif
beeptime = 5000;
}
}
SummeNick = 0;
SummeRoll = 0;
Mess_Integral_Gier = 0;
if(modell_fliegt < 256) sollGier = 0;
// sollGier = 0;
if(modell_fliegt > 1000) modell_fliegt = 1000; // for the Hooverpoint-Estimation
}
}
if((PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] > 80) && MotorenEin == 0)
855,7 → 861,7
SetNeutral(1);
CalibrationDone = 1;
#if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__))
SpeakHoTT = SPEAK_CALIBRATE;
SpeakHoTT = SPEAK_CALIBRATE;
#endif
Piep(GetActiveParamSet(),120);
}
880,6 → 886,10
// Einschalten
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(CalibrationDone) FC_StatusFlags |= FC_STATUS_START;
StartLuftdruck = Luftdruck;
HoehenWertF = 0;
HoehenWert = 0;
SummenHoehe = 0;
if(++delay_einschalten > 253)
{
delay_einschalten = 0;
1085,9 → 1095,6
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Bei Empfangsausfall im Flug
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//DebugOut.Analog[16] = GasIsZeroCnt;
//DebugOut.Analog[17] = VarioMeter;
if(FC_StatusFlags2 & FC_STATUS2_RC_FAILSAVE_ACTIVE)
{
StickNick = -GPS_Nick;
1152,13 → 1159,6
tmp_long /= 2;
tmp_long2 /= 2;
}
/*
if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25)
{
tmp_long /= 3;
tmp_long2 /= 3;
}
*/
if(tmp_long > (long) FromNaviCtrl_Value.Kalman_MaxFusion) tmp_long = (long) FromNaviCtrl_Value.Kalman_MaxFusion;
if(tmp_long < (long)-FromNaviCtrl_Value.Kalman_MaxFusion) tmp_long = (long)-FromNaviCtrl_Value.Kalman_MaxFusion;
if(tmp_long2 > (long) FromNaviCtrl_Value.Kalman_MaxFusion) tmp_long2 = (long) FromNaviCtrl_Value.Kalman_MaxFusion;
1456,7 → 1456,6
static int HeightDeviation = 0, FilterHCGas = 0;
static unsigned long HoverGasFilter = 0;
static unsigned char delay = 100, BaroAtUpperLimit = 0, BaroAtLowerLimit = 0;
// int CosAttitude; // for projection of hoover gas
// get the current hooverpoint
DebugOut.Analog[21] = HoverGas;
1504,8 → 1503,14
else // delay, because of expanding the Baro-Range
{
// now clear the D-values
SummenHoehe = HoehenWert * SM_FILTER;
vvSum = 0;
#if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__))
if(ACC_AltitudeControl) SummenHoehe = HoehenWert * SA_FILTER;
else SummenHoehe = HoehenWert * SM_FILTER;
#else
SummenHoehe = HoehenWert * SM_FILTER;
#endif
VerticalVelocitySum = 0;
VarioMeter = 0;
BaroExpandActive--;
}
1518,10 → 1523,10
if(!delay--)
{
#if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__))
if(HoehenReglerAktiv && !SpeakHoTT) SpeakHoTT = SPEAK_ALTITUDE_OFF;
if(!SpeakHoTT && HoehenReglerAktiv) SpeakHoTT = SPEAK_ALTITUDE_OFF;
#endif
HoehenReglerAktiv = 0; // disable height control
SollHoehe = HoehenWertF; // update SetPoint with current reading
SollHoehe = HoehenWert; // update SetPoint with current reading
delay = 1;
}
}
1529,10 → 1534,10
if(Parameter_HoehenSchalter > 70)
{ //height control is activated
#if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__))
if(!HoehenReglerAktiv && !SpeakHoTT) SpeakHoTT = SPEAK_ALTITUDE_ON;
if(!SpeakHoTT && !HoehenReglerAktiv) SpeakHoTT = SPEAK_ALTITUDE_ON;
#endif
delay = 200;
HoehenReglerAktiv = 1; // enable height control
delay = 200;
}
}
else // no switchable height control
1540,7 → 1545,6
SollHoehe = ((int16_t) ExternHoehenValue + (int16_t) Parameter_HoehenSchalter) * (int)EE_Parameter.Hoehe_Verstaerkung;
HoehenReglerAktiv = 1;
}
// calculate cos of nick and roll angle used for projection of the vertical hoover gas
tmp_int = (int)(IntegralNick/GIER_GRAD_FAKTOR); // nick angle in deg
tmp_int2 = (int)(IntegralRoll/GIER_GRAD_FAKTOR); // roll angle in deg
1547,8 → 1551,6
CosAttitude = (int16_t)ihypot(tmp_int, tmp_int2); // phytagoras gives effective attitude angle in deg
LIMIT_MAX(CosAttitude, 60); // limit effective attitude angle
CosAttitude = c_cos_8192(CosAttitude); // cos of actual attitude
//AccShorter = CosAttitude / 32 - 256;
//AccShorter = (CosAttitude - 8192) / 40;
VarioCharacter = ' ';
AltitudeSetpointTrimming = 0;
if(HoehenReglerAktiv && !(FC_StatusFlags & FC_STATUS_EMERGENCY_LANDING))
1578,7 → 1580,7
{ // gas stick is above hoover point
if(StickGas > (StickGasHover + HEIGHT_CONTROL_STICKTHRESHOLD) && !BaroAtUpperLimit)
{
if(HeightDeviation > 20) SollHoehe = HoehenWertF; // update setpoint to current heigth
if(HeightDeviation > 20) SollHoehe = HoehenWertF; // update setpoint to current heigth
if(FC_StatusFlags & FC_STATUS_VARIO_TRIM_DOWN)
{
FC_StatusFlags &= ~FC_STATUS_VARIO_TRIM_DOWN;
1693,6 → 1695,8
}
if(HoehenWertF > SollHoehe || !(Parameter_ExtraConfig & CFG2_HEIGHT_LIMIT))
{
if(!ACC_AltitudeControl)
{
// from this point the Heigth Control Algorithm is identical for both versions
if(BaroExpandActive) // baro range expanding active
{
1706,25 → 1710,8
LIMIT_MIN_MAX(tmp_long, -32767L, 32767L); // avoid overflov when casting to int16_t
HeightDeviation = (int)(tmp_long); // positive when too high
tmp_long = (tmp_long * (long)Parameter_Hoehe_P) / 32L; // p-part
LIMIT_MIN_MAX(tmp_long, -511 * STICK_GAIN, 512 * STICK_GAIN); // more than full range makes sense
LIMIT_MIN_MAX(tmp_long, -127 * STICK_GAIN, 256 * STICK_GAIN); // more than the full range makes no sense
GasReduction = tmp_long;
// ------------------------- D-Part ---------------------------- Qopter: neu
tmp_long = vv + AdWertAccHoch * Parameter_Hoehe_ACC_Wirkung/256;
if(WaypointTrimming) {
Variance = AltitudeSetpointTrimming*8;
} else {
Variance = AltitudeSetpointTrimming * EE_Parameter.Hoehe_Verstaerkung*9/32;
}
tmp_long -= (long)Variance;
tmp_long = (tmp_long * (long)Parameter_Luftdruck_D) / 32; // scale to d-gain parameter
LIMIT_MIN_MAX(tmp_long,-511 * STICK_GAIN, 512 * STICK_GAIN);
GasReduction += tmp_long;
} // EOF no baro range expanding
HCGas -= GasReduction;
LIMIT_MIN_MAX(HCGas, HoverGasMin, HoverGasMax); // limits gas around hover point
/* Qopter: deaktiviert
// ------------------------- D-Part 1: Vario Meter ----------------------------
tmp_int = VarioMeter / 8;
LIMIT_MIN_MAX(tmp_int, -127, 128);
1775,7 → 1762,6
}
}
}
*/
// strech control output by inverse attitude projection 1/cos
// + 1/cos(angle) ++++++++++++++++++++++++++
tmp_long2 = (int32_t)HCGas;
1792,8 → 1778,59
LIMIT_MAX(FilterHCGas, GasMischanteil); // nicht mehr als Gas
GasMischanteil = FilterHCGas;
}
// else GasMischanteil = FilterHCGas + (GasMischanteil - HoverGas) / 4; // only in Vario-Mode
else GasMischanteil = FilterHCGas + (GasMischanteil - HoverGas) / 4; // only in Vario-Mode
}
else
{
// from this point the Heigth Control Algorithm is identical for both versions
if(BaroExpandActive) // baro range expanding active
{
HCGas = HoverGas; // hover while expanding baro adc range
HeightDeviation = 0;
} // EOF // baro range expanding active
else // valid data from air pressure sensor
{
// ------------------------- P-Part ----------------------------
tmp_long = (HoehenWertF - SollHoehe); // positive when too high
LIMIT_MIN_MAX(tmp_long, -32767L, 32767L); // avoid overflov when casting to int16_t
HeightDeviation = (int)(tmp_long); // positive when too high
tmp_long = (tmp_long * (long)Parameter_Hoehe_P) / 32L; // p-part
LIMIT_MIN_MAX(tmp_long, -511 * STICK_GAIN, 512 * STICK_GAIN); // more than full range makes sense
GasReduction = tmp_long;
// ------------------------ D-Part: ACC-Z Integral ------------------------
tmp_long = VerticalVelocity + AdWertAccHoch * Parameter_Hoehe_ACC_Wirkung/256;
// ------------------------- D-Part: Vario Meter ----------------------------
if(WaypointTrimming) {
Variance = AltitudeSetpointTrimming * 8;
} else {
Variance = AltitudeSetpointTrimming * EE_Parameter.Hoehe_Verstaerkung*9/32;
}
tmp_long -= (long)Variance;
tmp_long = (tmp_long * (long)Parameter_Luftdruck_D) / 32; // scale to d-gain parameter
LIMIT_MIN_MAX(tmp_long,-511 * STICK_GAIN, 512 * STICK_GAIN);
GasReduction += tmp_long;
} // EOF no baro range expanding
DebugOut.Analog[19] = -GasReduction;
HCGas -= GasReduction;
LIMIT_MIN_MAX(HCGas, HoverGasMin, HoverGasMax); // limits gas around hover point
// strech control output by inverse attitude projection 1/cos
// + 1/cos(angle) ++++++++++++++++++++++++++
tmp_long2 = (int32_t)HCGas;
tmp_long2 *= 8192L;
tmp_long2 /= CosAttitude;
HCGas = (int16_t)tmp_long2;
// update height control gas averaging
FilterHCGas = (FilterHCGas * (HC_GAS_AVERAGE - 1) + HCGas) / HC_GAS_AVERAGE;
// limit height control gas pd-control output
LIMIT_MIN_MAX(FilterHCGas, EE_Parameter.Hoehe_MinGas * STICK_GAIN, (MAX_GAS - 20) * STICK_GAIN);
// set GasMischanteil to HeightControlGasFilter
if(Parameter_ExtraConfig & CFG2_HEIGHT_LIMIT)
{ // old version
LIMIT_MAX(FilterHCGas, GasMischanteil); // nicht mehr als Gas
GasMischanteil = FilterHCGas;
}
else GasMischanteil = FilterHCGas;// + (GasMischanteil - HoverGas) / 4; // Qopter: geändert
}
}
}// EOF height control active
else // HC not active
1815,8 → 1852,8
// this is done only if height contol option is selected in global config and aircraft is flying
if((FC_StatusFlags & FC_STATUS_FLY))// && !(FC_SatusFlags & FC_STATUS_EMERGENCY_LANDING))
{
// if(HoverGasFilter == 0 || StartTrigger == 1) HoverGasFilter = HOVER_GAS_AVERAGE * (unsigned long)(GasMischanteil); // init estimation
if(HoverGasFilter == 0 || StartTrigger == 1) HoverGasFilter = HOVER_GAS_AVERAGE * (unsigned long)(HoverGas); // Qopter: geändert
//if(HoverGasFilter == 0 || StartTrigger == 1) HoverGasFilter = HOVER_GAS_AVERAGE * (unsigned long)(GasMischanteil); // init estimation
if(HoverGasFilter == 0 || StartTrigger == 1) HoverGasFilter = HOVER_GAS_AVERAGE * (unsigned long)(HoverGas); // Qopter: geändert
if(StartTrigger == 1) StartTrigger = 2;
tmp_long2 = (int32_t)GasMischanteil; // take current thrust
tmp_long2 *= CosAttitude; // apply attitude projection
1833,8 → 1870,8
HoverGasFilter += 4L * tmp_long2;
}
else //later
// if(abs(VarioMeter) < 100 && abs(HoehenWert - SollHoehe) < 256) // only on small vertical speed & difference is small (only descending)
if(abs(vv) < 50 && abs(HoehenWertF - SollHoehe) < 256) // Qopter: geändert, Anpassung nötig?
//if(abs(VerticalVelocity) < 50 && abs(HoehenWertF - SollHoehe) < 256) // Qopter: geändert, Anpassung nötig?
if(abs(VarioMeter) < 100 && abs(HoehenWert - SollHoehe) < 256) // only on small vertical speed & difference is small (only descending)
{
HoverGasFilter -= HoverGasFilter/HOVER_GAS_AVERAGE;
HoverGasFilter += tmp_long2;

/test_branches/FC2_2/fc.h
8,7 → 8,7
//#define GIER_GRAD_FAKTOR 1160L
extern long GIER_GRAD_FAKTOR; // Abhängigkeit zwischen GyroIntegral und Winkel
#define STICK_GAIN 4
extern signed int ACC_AMPLIFY;
#define ACC_AMPLIFY 6
// FC_StatusFlags
#define FC_STATUS_MOTOR_RUN 0x01
26,7 → 26,7
#define FC_STATUS2_RC_FAILSAVE_ACTIVE 0x04
#define FC_STATUS2_OUT1_ACTIVE 0x08
#define FC_STATUS2_OUT2_ACTIVE 0x10
#define FC_STATUS2_TAKEOFF 0x20
#define FC_STATUS2_WAIT_FOR_TAKEOFF 0x20 // Motor Running, but still on the ground
//NC_To_FC_Flags
#define NC_TO_FC_FLYING_RANGE 0x01
34,7 → 34,6
extern volatile unsigned char FC_StatusFlags, FC_StatusFlags2;
extern void ParameterZuordnung(void);
//extern signed char AccShorter;
#define Poti1 Poti[0]
#define Poti2 Poti[1]
100,6 → 99,8
extern void SetNeutral(unsigned char AccAdjustment);
extern void Piep(unsigned char Anzahl, unsigned int dauer);
extern void CopyDebugValues(void);
extern unsigned char ACC_AltitudeControl;
extern signed int CosAttitude; // for projection of hoover gas
extern unsigned char h,m,s;
extern int StickNick,StickRoll,StickGier,StickGas;
126,6 → 127,8
extern unsigned char Parameter_Gier_P;
extern unsigned char Parameter_ServoNickControl;
extern unsigned char Parameter_ServoRollControl;
extern unsigned char Parameter_ServoNickComp;
extern unsigned char Parameter_ServoRollComp;
extern unsigned char Parameter_AchsKopplung1;
extern unsigned char Parameter_AchsKopplung2;
//extern unsigned char Parameter_AchsGegenKopplung1;
137,6 → 140,5
extern unsigned char Parameter_ExtraConfig;
extern signed char MixerTable[MAX_MOTORS][4];
extern const signed char sintab[31];
extern int CosAttitude;
#endif //_FC_H

/test_branches/FC2_2/libfc1284.a
Cannot display: file marked as a binary type.
svn:mime-type = application/octet-stream

/test_branches/FC2_2/libfc644.a
Cannot display: file marked as a binary type.
svn:mime-type = application/octet-stream

/test_branches/FC2_2/main.c
81,7 → 81,6
}
else Piep(WinkelOut.CalcState,150);
}
// DebugOut.Analog[19] = WinkelOut.CalcState;
}
127,7 → 126,7
else
{
PlatinenVersion = 22;
ACC_AMPLIFY = 7; // der ACC-Sensor hat etwa 16% weniger Ausschlag
ACC_AltitudeControl = 1;
}
#else
401,12 → 400,12
{
second = 0;
if(FC_StatusFlags & FC_STATUS_FLY) FlugSekunden++;
else
else timer2 = 1450; // 0,5 Minuten aufrunden
if(modell_fliegt < 1024)
{
timer2 = 1450; // 0,5 Minuten aufrunden
if(StartLuftdruck < Luftdruck) StartLuftdruck += 2;
if(StartLuftdruck < Luftdruck) StartLuftdruck += 5;
else
if(StartLuftdruck > Luftdruck) StartLuftdruck -= 2;
if(StartLuftdruck > Luftdruck) StartLuftdruck -= 5;
}
}
// +++++++++++++++++++++++++++++++++

/test_branches/FC2_2/makefile
5,14 → 5,14
F_CPU = 20000000
#-------------------------------------------------------------------
VERSION_MAJOR = 0
VERSION_MINOR = 89
VERSION_PATCH = 2
VERSION_MINOR = 90
VERSION_PATCH = 4
VERSION_SERIAL_MAJOR = 11 # Serial Protocol
VERSION_SERIAL_MINOR = 0 # Serial Protocol
NC_SPI_COMPATIBLE = 52 # Navi-Kompatibilität
#-------------------------------------------------------------------
# ATMEGA644: 63487 is maximum
#-------------------------------------------------------------------#-------------------------------------------------------------------
#-------------------------------------------------------------------
# 0 a
# 1 b
# 2 c

/test_branches/FC2_2/menu.c
126,6 → 126,9
LCD_printfxy(0,1,"Setpoint:%5i",(int)(SollHoehe/5));
LCD_printfxy(0,2,"Pressure:%5i",MessLuftdruck);
LCD_printfxy(0,3,"Offset: %5i",OCR0A);
#if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__))
if(ACC_AltitudeControl) LCD_printfxy(17,3,"(A)",OCR0A);
#endif
}
else
{

/test_branches/FC2_2/spi.c
190,9 → 190,7
ToNaviCtrl.Param.Byte[6] = Parameter_UserParam7;
ToNaviCtrl.Param.Byte[7] = Parameter_UserParam8;
ToNaviCtrl.Param.Byte[8] = FC_StatusFlags;
if(!FC_StatusFlags2 & FC_STATUS2_TAKEOFF) ToNaviCtrl.Param.Byte[8] &= ~FC_STATUS_FLY; // Damit die NC noch keine I-Anteile aufbaut - muss später anders gelöst werden
//if(FC_StatusFlags2 & FC_STATUS2_WAIT_FOR_TAKEOFF) ToNaviCtrl.Param.Byte[8] &= ~FC_STATUS_FLY;
FC_StatusFlags &= ~(FC_STATUS_CALIBRATE | FC_STATUS_START);
ToNaviCtrl.Param.Byte[9] = GetActiveParamSet();
ToNaviCtrl.Param.Byte[10] = EE_Parameter.ComingHomeAltitude;

/test_branches/FC2_2/timer0.c
267,7 → 267,7
{
nick = (cosinus * IntegralNick) / 128L - (sinus * IntegralRoll) / 128L;
nick -= POI_KameraNick * 7;
nick = ((long)EE_Parameter.ServoNickComp * nick) / 512L;
nick = ((long)Parameter_ServoNickComp * nick) / 512L;
// offset (Range from 0 to 255 * 3 = 765)
if(EE_Parameter.ServoCompInvert & SERVO_RELATIVE) ServoNickOffset = NickServoValue;
else ServoNickOffset += (NickServoValue - ServoNickOffset) / EE_Parameter.ServoManualControlSpeed;
297,7 → 297,7
else
{
roll = (cosinus * IntegralRoll) / 128L + (sinus * IntegralNick) / 128L;
roll = ((long)EE_Parameter.ServoRollComp * roll) / 512L;
roll = ((long)Parameter_ServoRollComp * roll) / 512L;
ServoRollOffset += ((int16_t)Parameter_ServoRollControl * (MULTIPLYER*16) - ServoRollOffset) / EE_Parameter.ServoManualControlSpeed;
if(EE_Parameter.ServoCompInvert & SERVO_ROLL_INV)
{ // inverting movement of servo

/test_branches/FC2_2/uart.c
117,10 → 117,10
"AccNick ",
"AccRoll ",
"YawGyro ",
"Height Value ", //5
"Altitude [0,1m] ", //5
"AccZ ",
"Gas ",
"AdWertAccHoch ",
"Compass Value ",
"Voltage [0.1V] ",
"Receiver Level ", //10
"Gyro Compass ",
128,17 → 128,17
"Motor 2 ",
"Motor 3 ",
"Motor 4 ", //15
"Variance ",
"vv ",
"HoehenWertF ",
"19 ACC-Parameter",
"16 Variance ",
"17 vv ",
"18 HoehenWertF ",
"19 GasEinwirkung",
"Servo ", //20
"Hovergas ",
"Current [0.1A] ",
"Capacity [mAh] ",
"Height Setpoint ",
"P-Term ", //25
"D-Term ", //"26 CPU OverLoad ",
"25 P ", //25
"26 D ", //"26 CPU OverLoad ",
"Compass Setpoint",
"I2C-Error ",
"BL Limit ",

/test_branches/FC2_2/version.txt
553,9 → 553,14
0.88n H.Buss 06.07.2012
- Bugfix: Der letzte angesteuerte Servo-Puls war zu kurz
0.89a
0.90 H.Buss 04.03.2013
- Anzeige WP x/Y in der HoTT-Telemetrie
- Schalter und WP-Event gleichzeitig
- Photo-Auslösung als Entfernungsintervalle
- Jeti +
- RC-Lost am Startpunkt, wenn GAS auf Null > 1,5sek macht dann kein Failsafe
- Kamera-Neitung als Integral
- RC-Lost am Startpunkt macht kein Failsafe, wenn GAS auf Null für 1,5sek war
- Auswahl:Speak-All nun in der KopterTool-Config
0.90e H.Buss 04.03.2013
- Parameter_ServoRollComp jetzt auf Poti /Kompatibilität auf 93 erhöht)