/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 ", |