Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 2313 → Rev 2314

/test_branches/FC2_2/analog.c
181,19 → 181,26
17 L
*/
 
long my_ACC_AltitudeFusion(void)
long my_ACC_AltitudeFusion(unsigned char init)
{
static long HoeheAlt, HoehenDiff, VerticalVelocitySum, AccVertical;
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
if(init)
{
VerticalVelocitySum = 0;
SummenHoehe = HoehenWert * SA_FILTER;
}
else
{
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
}
return(SummenHoehe);
}
 
 
//#######################################################################################
//
ISR(ADC_vect)
200,7 → 207,7
//#######################################################################################
{
static unsigned char kanal=0,state = 0;
static signed char subcount = 0;
static signed int subcount = 0;
static signed int gier1, roll1, nick1, nick_filter, roll_filter;
static signed int accy, accx;
static long tmpLuftdruck = 0;
247,7 → 254,7
break;
case 8:
Aktuell_az = ADC;
AdWertAccHoch = Aktuell_az - NeutralAccZ;
AdWertAccHoch = Aktuell_az - NeutralAccZ - (int) NeutralAccZfine;
if(!ACC_AltitudeControl) // The Offset must be corrected, because of the ACC-Drift from vibrations
{
if(AdWertAccHoch > 1)
256,8 → 263,8
{
subcount += 5;
if(modell_fliegt < 500) subcount += 10;
if(subcount > 100) { NeutralAccZ++; subcount -= 100;}
}
if(subcount > 100) { NeutralAccZ++; subcount -= 100;}
}
else if(AdWertAccHoch < -1)
{
269,6 → 276,29
}
}
}
#if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__))
else
if(CosAttitude > 8192 - 50) // horizontal leveled within 6°
{
if(AdWertAccHoch > 1)
{
if(++subcount > 5000)
{
if(NeutralAccZfine < 6) NeutralAccZfine++;
subcount -= 5000;
}
}
else
if(AdWertAccHoch < -1)
{
if(--subcount < -5000)
{
if(NeutralAccZfine > -6) NeutralAccZfine--;
subcount += 5000;
}
}
}
#endif
Mess_Integral_Hoch += AdWertAccHoch; // Integrieren
Mess_Integral_Hoch -= Mess_Integral_Hoch / 1024; // dämfen
kanal = AD_DRUCK;
321,7 → 351,7
#if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__))
if(ACC_AltitudeControl)
{
HoehenWertF = (my_ACC_AltitudeFusion() + SA_FILTER/2)/SA_FILTER; // cm
HoehenWertF = (my_ACC_AltitudeFusion(0) + SA_FILTER/2)/SA_FILTER; // cm
}
else HoehenWertF = HoehenWert;
#else
341,7 → 371,7
Luftdruck -= Luftdruck/16;
Luftdruck += tmpLuftdruck;
HoehenWert = StartLuftdruck - Luftdruck; // cm
HoehenWert = HoehenWert + AdWertAccHoch * 41 / 256; // Weight-compsensation <-------
//HoehenWert = HoehenWert + AdWertAccHoch * 41 / 256; // Weight-compsensation <-------
}
else
#endif
/test_branches/FC2_2/analog.h
3,7 → 3,9
/*#######################################################################################
 
#######################################################################################*/
extern long my_ACC_AltitudeFusion(unsigned char init);
 
 
#define SM_FILTER 16
#define SA_FILTER 512
 
/test_branches/FC2_2/fc.c
67,6 → 67,7
unsigned int NeutralAccX=0, NeutralAccY=0;
int NaviAccNick, NaviAccRoll,NaviCntAcc = 0;
int NeutralAccZ = 0;
signed char NeutralAccZfine = 0;
unsigned char ControlHeading = 0;// in 2°
long IntegralNick = 0,IntegralNick2 = 0;
long IntegralRoll = 0,IntegralRoll2 = 0;
215,9 → 216,12
DebugOut.Analog[16] = Variance;
DebugOut.Analog[17] = VerticalVelocity;
DebugOut.Analog[18] = HoehenWertF;
DebugOut.Analog[25] = Parameter_Hoehe_P;
DebugOut.Analog[26] = Parameter_Luftdruck_D;
 
DebugOut.Analog[25] = CosAttitude;///8192;
DebugOut.Analog[26] = NeutralAccZfine;
//DebugOut.Analog[25] = Parameter_Hoehe_P;
//DebugOut.Analog[26] = Parameter_Luftdruck_D;
 
}
 
 
282,6 → 286,7
NeutralAccX = 0;
NeutralAccY = 0;
NeutralAccZ = 0;
NeutralAccZfine = 0;
 
AdNeutralNick = 0;
AdNeutralRoll = 0;
393,7 → 398,7
CosAttitude = c_cos_8192(tilt1);
NeutralAccZ = (long)((long) (NeutralAccZ - 512) * 8192 + 4096) / CosAttitude + 512;
if(tilt1 > 20) sucess = 0; // calibration must be within 20° Tilt angle
if(ACC_AltitudeControl) if((NeutralAccZ < 680 - 20) || (NeutralAccZ > 680 + 20)) { VersionInfo.HardwareError[0] |= FC_ERROR0_ACC_TOP; };
if(ACC_AltitudeControl) if((NeutralAccZ < 682 - 25) || (NeutralAccZ > 682 + 25)) { VersionInfo.HardwareError[0] |= FC_ERROR0_ACC_TOP; };
 
#else
NeutralAccZ = (int16_t)GetParamWord(PID_ACC_TOP);
409,6 → 414,7
carefree_old = 70;
#if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__))
LIBFC_HoTT_Clear();
my_ACC_AltitudeFusion(1); // initalisation
#endif
return(sucess);
}
/test_branches/FC2_2/fc.h
87,6 → 87,7
extern unsigned int NeutralAccX, NeutralAccY;
extern unsigned char HoehenReglerAktiv;
extern int NeutralAccZ;
extern signed char NeutralAccZfine;
extern long Umschlag180Nick, Umschlag180Roll;
extern signed int ExternStickNick,ExternStickRoll,ExternStickGier;
extern unsigned char Parameter_UserParam1,Parameter_UserParam2,Parameter_UserParam3,Parameter_UserParam4,Parameter_UserParam5,Parameter_UserParam6,Parameter_UserParam7,Parameter_UserParam8;
/test_branches/FC2_2/libfc.h
8,7 → 8,7
#define CPU_ATMEGA1284P 4
 
extern void LIBFC_Init(unsigned char);
extern long ACC_AltitudeFusion(void);
extern long ACC_AltitudeFusion(unsigned char init);
extern void LIBFC_Polling(void);
extern void LIBFC_ReceiverInit(unsigned char rtype);
 
/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/uart.c
137,8 → 137,8
"Current [0.1A] ",
"Capacity [mAh] ",
"Height Setpoint ",
"25 P ", //25
"26 D ", //"26 CPU OverLoad ",
"25 cos 8192 ", //25
"26 AccZfine ", //"26 CPU OverLoad ",
"Compass Setpoint",
"I2C-Error ",
"BL Limit ",