Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 991 → Rev 992

/trunk/fc.c
213,6 → 213,9
SendVersionToNavi = 1;
LED_Init();
MikroKopterFlags |= FLAG_CALIBRATE;
FromNaviCtrl_Value.Kalman_K = -1;
FromNaviCtrl_Value.Kalman_MaxDrift = EE_Parameter.Driftkomp * 16;
FromNaviCtrl_Value.Kalman_MaxFusion = 32;
}
 
//############################################################################
811,32 → 814,54
if(!Looping_Nick && !Looping_Roll)
{
long tmp_long, tmp_long2;
tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick);
tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll);
tmp_long /= 16;
tmp_long2 /= 16;
if((MaxStickNick > 64) || (MaxStickRoll > 64))
{
tmp_long /= 3;
tmp_long2 /= 3;
}
if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25)
{
tmp_long /= 3;
tmp_long2 /= 3;
}
 
if(FromNaviCtrl_Value.Kalman_K != -1)
{
tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick);
tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll);
tmp_long = (tmp_long * FromNaviCtrl_Value.Kalman_K) / (32 * 16);
tmp_long2 = (tmp_long2 * FromNaviCtrl_Value.Kalman_K) / (32 * 16);
if((MaxStickNick > 64) || (MaxStickRoll > 64))
{
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;
if(tmp_long2 < (long)-FromNaviCtrl_Value.Kalman_MaxFusion) tmp_long2 = (long)-FromNaviCtrl_Value.Kalman_MaxFusion;
}
else
{
tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick);
tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll);
tmp_long /= 16;
tmp_long2 /= 16;
if((MaxStickNick > 64) || (MaxStickRoll > 64))
{
tmp_long /= 3;
tmp_long2 /= 3;
}
if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25)
{
tmp_long /= 3;
tmp_long2 /= 3;
}
#define AUSGLEICH 32
if(tmp_long > AUSGLEICH) tmp_long = AUSGLEICH;
if(tmp_long < -AUSGLEICH) tmp_long =-AUSGLEICH;
if(tmp_long2 > AUSGLEICH) tmp_long2 = AUSGLEICH;
if(tmp_long2 <-AUSGLEICH) tmp_long2 =-AUSGLEICH;
 
if(tmp_long > AUSGLEICH) tmp_long = AUSGLEICH;
if(tmp_long < -AUSGLEICH) tmp_long =-AUSGLEICH;
if(tmp_long2 > AUSGLEICH) tmp_long2 = AUSGLEICH;
if(tmp_long2 <-AUSGLEICH) tmp_long2 =-AUSGLEICH;
}
Mess_IntegralNick -= tmp_long;
Mess_IntegralRoll -= tmp_long2;
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
if(ZaehlMessungen >= ABGLEICH_ANZAHL)
{
static int cnt = 0;
860,7 → 885,7
LageKorrekturNick = ausgleichNick / ABGLEICH_ANZAHL;
LageKorrekturRoll = ausgleichRoll / ABGLEICH_ANZAHL;
 
if((MaxStickNick > 64) || (MaxStickRoll > 64) || (abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25))
if(((MaxStickNick > 64) || (MaxStickRoll > 64) || (abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25)) && (FromNaviCtrl_Value.Kalman_K == -1))
{
LageKorrekturNick /= 2;
LageKorrekturRoll /= 2;
908,7 → 933,7
#define BEWEGUNGS_LIMIT 20000
// Nick +++++++++++++++++++++++++++++++++++++++++++++++++
cnt = 1;// + labs(IntegralFehlerNick) / 4096;
if(labs(MittelIntegralNick_Alt - MittelIntegralNick) < BEWEGUNGS_LIMIT)
if(labs(MittelIntegralNick_Alt - MittelIntegralNick) < BEWEGUNGS_LIMIT || (FromNaviCtrl_Value.Kalman_MaxDrift > 3*16))
{
if(IntegralFehlerNick > FEHLER_LIMIT2)
{
939,6 → 964,7
KompassSignalSchlecht = 1000;
}
if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;
if(cnt * 16 > FromNaviCtrl_Value.Kalman_MaxDrift) cnt = FromNaviCtrl_Value.Kalman_MaxDrift/16;
if(IntegralFehlerNick > FEHLER_LIMIT) AdNeutralNick += cnt;
if(IntegralFehlerNick < -FEHLER_LIMIT) AdNeutralNick -= cnt;
 
946,7 → 972,7
cnt = 1;// + labs(IntegralFehlerNick) / 4096;
 
ausgleichRoll = 0;
if(labs(MittelIntegralRoll_Alt - MittelIntegralRoll) < BEWEGUNGS_LIMIT)
if(labs(MittelIntegralRoll_Alt - MittelIntegralRoll) < BEWEGUNGS_LIMIT || (FromNaviCtrl_Value.Kalman_MaxDrift > 3*16))
{
if(IntegralFehlerRoll > FEHLER_LIMIT2)
{
975,8 → 1001,8
cnt = 0;
KompassSignalSchlecht = 1000;
}
 
if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;
if(cnt * 16 > FromNaviCtrl_Value.Kalman_MaxDrift) cnt = FromNaviCtrl_Value.Kalman_MaxDrift/16;
if(IntegralFehlerRoll > FEHLER_LIMIT) AdNeutralRoll += cnt;
if(IntegralFehlerRoll < -FEHLER_LIMIT) AdNeutralRoll -= cnt;
}
1093,7 → 1119,8
DebugOut.Analog[18] = (int)FromNaviCtrl_Value.OsdBar;
DebugOut.Analog[19] = WinkelOut.CalcState;
DebugOut.Analog[20] = ServoValue;
 
DebugOut.Analog[27] = (int)FromNaviCtrl_Value.Kalman_MaxDrift;
DebugOut.Analog[29] = (int)FromNaviCtrl_Value.Kalman_K;
DebugOut.Analog[30] = GPS_Nick;
DebugOut.Analog[31] = GPS_Roll;
 
/trunk/makefile
5,7 → 5,7
#-------------------------------------------------------------------
HAUPT_VERSION = 0
NEBEN_VERSION = 71
VERSION_INDEX = 0
VERSION_INDEX = 1
 
VERSION_KOMPATIBEL = 8 # PC-Kompatibilität
#-------------------------------------------------------------------
/trunk/spi.c
242,10 → 242,16
switch (FromNaviCtrl.Command)
{
case SPI_KALMAN:
FromNaviCtrl_Value.Kalman_K = FromNaviCtrl.Param.Byte[0];
FromNaviCtrl_Value.Kalman_MaxFusion = FromNaviCtrl.Param.Byte[1];
FromNaviCtrl_Value.Kalman_MaxDrift = FromNaviCtrl.Param.Byte[2];
break;
case SPI_CMD_OSD_DATA:
FromNaviCtrl_Value.OsdBar = FromNaviCtrl.Param.Byte[0];
FromNaviCtrl_Value.Distance = FromNaviCtrl.Param.Int[1];
break;
break;
 
case SPI_CMD_GPS_POS:
// ToFlightCtrl.Param.Long[0] = GPS_Data.Longitude;
/trunk/spi.h
79,7 → 79,9
 
#define SPI_CMD_OSD_DATA 100
#define SPI_CMD_GPS_POS 101
#define SPI_CMD_GPS_TARGET 102
#define SPI_CMD_GPS_TARGET 102
#define SPI_KALMAN 103
 
struct str_FromNaviCtrl
{
unsigned char Command;
102,6 → 104,9
{
signed int OsdBar;
signed int Distance;
signed char Kalman_K;
signed char Kalman_MaxDrift;
signed char Kalman_MaxFusion;
};
 
#ifdef USE_SPI_COMMUNICATION
/trunk/uart.c
64,9 → 64,9
" ",
" ", //25
" ",
"Kalman_MaxDrift ",
" ",
" ",
" ",
"Kalman K ",
"GPS_Nick ", //30
"GPS_Roll "
};
/trunk/version.txt
155,4 → 155,8
V0.70d H.Buss 02.08.2008
- Transistorausgänge: das oberste Bit der Blinkmaske (im KopterTool linkes Bit) gibt nun den Zustand des Ausgangs im Schalterbetrieb an
0.71b: H.Buss 19.10.2008
Kommunikation zum Navi erweitert
- Beeptime jetzt 32Bit
- Datenfusion und Driftkopensation wird durch NaviBoard unterstützt