Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 1085 → Rev 1086

/branches/salvo_gps/Basis_V0071h/trunk/fc.c
51,6 → 51,12
// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// + POSSIBILITY OF SUCH DAMAGE.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Aenderungen von Peter Muehlenbrock ("Salvo") Stand 9.11.2008
/*
Driftkompensation fuer Gyros verbessert
Linearsensor optional mit fixem Neutralwert
Ersatzkompass abgeleitet aus Magnetkompass und Giergyro fuer nahezu neigungsunabhaengige Kompassfunktion
*/
 
#include "main.h"
#include "eeprom.c"
85,6 → 91,25
long ErsatzKompass;
int ErsatzKompassInGrad; // Kompasswert in Grad
int GierGyroFehler = 0;
//Salvo 12.10.2007
uint8_t magkompass_ok=0;
uint8_t gps_cmd = GPS_CMD_STOP;
static int ubat_cnt =0;
static int gas_actual,gas_mittel; //Parameter fuer Gasreduzierung bei unterspannung
int w,v;
//Salvo End
 
//Salvo 15.12.2007 Ersatzkompass und Giergyrokompensation
long GyroKomp_Int;
long int GyroGier_Comp;
int GyroKomp_Value; // Der ermittelte Kompasswert aus Gyro und Magnetkompass
short int cnt_stickgier_zero =0;
int gyrogier_kompass;
//Salvo End
 
//Salvo 2.1.2008 Allgemeine Debugvariablen
int debug_gp_0,debug_gp_1,debug_gp_2,debug_gp_3,debug_gp_4,debug_gp_5,debug_gp_6,debug_gp_7;
//Salvo End
float GyroFaktor;
float IntegralFaktor;
volatile int DiffNick,DiffRoll;
132,6 → 157,7
unsigned char Parameter_NaviGpsI;
unsigned char Parameter_NaviGpsD;
unsigned char Parameter_NaviGpsACC;
unsigned char Parameter_NaviStickThreshold; //salvo 16.10.2008
unsigned char Parameter_NaviOperatingRadius;
unsigned char Parameter_NaviWindCorrection;
unsigned char Parameter_NaviSpeedCompensation;
142,6 → 168,13
unsigned int modell_fliegt = 0;
unsigned char MikroKopterFlags = 0;
 
//Salvo 2.1.2008 Allgemeine Debugvariablen
int debug_gp_0,debug_gp_1,debug_gp_2,debug_gp_3,debug_gp_4,debug_gp_5,debug_gp_6,debug_gp_7;
//Salvo End
 
 
 
 
void Piep(unsigned char Anzahl)
{
while(Anzahl--)
157,7 → 190,10
void SetNeutral(void)
//############################################################################
{
NeutralAccX = 0;
// Salvo 9.12.2007
RX_SWTCH_ON; //GPS Daten auf RX eingang schalten
// Salvo End
NeutralAccX = 0;
NeutralAccY = 0;
NeutralAccZ = 0;
AdNeutralNick = 0;
220,6 → 256,13
FromNaviCtrl_Value.Kalman_K = -1;
FromNaviCtrl_Value.Kalman_MaxDrift = EE_Parameter.Driftkomp * 16;
FromNaviCtrl_Value.Kalman_MaxFusion = 32;
//Salvo 13.10.2007 Ersatzkompass und Gas
GyroKomp_Int = KompassValue * GIER_GRAD_FAKTOR; //Neu ab 15.10.2008
// gas_mittel = 30;
// gas_actual = gas_mittel;
// Salvo End
 
}
 
//############################################################################
234,7 → 277,7
MesswertNick = (signed int) AdWertNick - AdNeutralNick;
 
//DebugOut.Analog[26] = MesswertNick;
DebugOut.Analog[28] = MesswertRoll;
//DebugOut.Analog[28] = MesswertRoll;
 
// Beschleunigungssensor ++++++++++++++++++++++++++++++++++++++++++++++++
Mittelwert_AccNick = ((long)Mittelwert_AccNick * 1 + ((ACC_AMPLIFY * (long)AdWertAccNick))) / 2L;
248,6 → 291,10
IntegralAccZ += Aktuell_az - NeutralAccZ;
// Gier ++++++++++++++++++++++++++++++++++++++++++++++++
ErsatzKompass += MesswertGier;
//Salvo 12.11.2007
GyroKomp_Int += (long)MesswertGier;
GyroGier_Comp += (long)MesswertGier;
//Salvo End
Mess_Integral_Gier += MesswertGier;
// Mess_Integral_Gier2 += MesswertGier;
if(ErsatzKompass >= (360L * GIER_GRAD_FAKTOR)) ErsatzKompass -= 360L * GIER_GRAD_FAKTOR; // 360° Umschlag
438,12 → 485,12
CHK_POTI_MM(Parameter_J16Timing,EE_Parameter.J16Timing,1,255);
CHK_POTI_MM(Parameter_J17Timing,EE_Parameter.J17Timing,1,255);
 
// CHK_POTI(Parameter_NaviGpsModeControl,EE_Parameter.NaviGpsModeControl,0,255);
//CHK_POTI(Parameter_NaviGpsGain,EE_Parameter.NaviGpsGain,0,255);
// CHK_POTI(Parameter_NaviGpsP,EE_Parameter.NaviGpsP,0,255);
// CHK_POTI(Parameter_NaviGpsI,EE_Parameter.NaviGpsI,0,255);
// CHK_POTI(Parameter_NaviGpsD,EE_Parameter.NaviGpsD,0,255);
// CHK_POTI(Parameter_NaviGpsACC,EE_Parameter.NaviGpsACC,0,255);
CHK_POTI(Parameter_NaviGpsModeControl,EE_Parameter.NaviGpsModeControl,0,255);
CHK_POTI(Parameter_NaviGpsGain,EE_Parameter.NaviGpsGain,0,255);
CHK_POTI(Parameter_NaviGpsP,EE_Parameter.NaviGpsP,0,255);
CHK_POTI(Parameter_NaviGpsI,EE_Parameter.NaviGpsI,0,255);
CHK_POTI(Parameter_NaviGpsD,EE_Parameter.NaviGpsD,0,255);
CHK_POTI(Parameter_NaviGpsACC,EE_Parameter.NaviGpsACC,0,255);
// CHK_POTI_MM(Parameter_NaviOperatingRadius,EE_Parameter.NaviOperatingRadius,10,255);
// CHK_POTI(Parameter_NaviWindCorrection,EE_Parameter.NaviWindCorrection,0,255);
// CHK_POTI(Parameter_NaviSpeedCompensation,EE_Parameter.NaviSpeedCompensation,0,255);
475,9 → 522,17
static char TimerWerteausgabe = 0;
static char NeueKompassRichtungMerken = 0;
static long ausgleichNick, ausgleichRoll;
 
Mittelwert();
 
Mittelwert();
//****** GPS Daten holen ***************
short int n;
if (gps_alive_cnt > 0) gps_alive_cnt--; //Dekrementieren. Wenn 0 kommen keine ausreichenden GPS Meldungen (Timeout)
n = Get_Rel_Position();
if (n == 0)
{
ROT_ON; //led blitzen lassen
}
//******PROVISORISCH***************
GRN_ON;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Gaswert ermitteln
551,6 → 606,7
if(++delay_neutral > 200) // nicht sofort
{
GRN_OFF;
SetNeutral();
MotorenEin = 0;
delay_neutral = 0;
modell_fliegt = 0;
579,6 → 635,13
}
SetNeutral();
Piep(GetActiveParamSetNumber());
GPS_Save_Home(); //Daten sind jetzt hoffentlich verfuegbar
if (gps_home_position.status > 0 )
{
Delay_ms(1000); //akustisch verkuenden dass GPS Home Daten da sind
beeptime = 1000;
Delay_ms(500);
}
}
}
}
617,6 → 680,10
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(++delay_einschalten > 200)
{
int n;
// Salvo 9.12.2007
RX_SWTCH_ON; //GPS Daten auf RX eingang schalten
// Salvo End
delay_einschalten = 200;
modell_fliegt = 1;
MotorenEin = 1;
629,6 → 696,8
Mess_IntegralRoll2 = IntegralRoll;
SummeNick = 0;
SummeRoll = 0;
n= GPS_CRTL(GPS_CMD_STOP); //GPS Lageregelung beenden
 
MikroKopterFlags |= FLAG_START;
}
}
641,6 → 710,10
{
if(++delay_ausschalten > 200) // nicht sofort
{
// Salvo 9.12.2007
RX_SWTCH_OFF; //Bluetooth Daten auf RX eingang schalten
// Salvo End
MotorenEin = 0;
delay_ausschalten = 200;
modell_fliegt = 0;
897,10 → 970,15
if(GierGyroFehler > ABGLEICH_ANZAHL/2) { AdNeutralGier++; AdNeutralGierBias++; }
if(GierGyroFehler <-ABGLEICH_ANZAHL/2) { AdNeutralGier--; AdNeutralGierBias--; }
 
DebugOut.Analog[22] = MittelIntegralRoll / 26;
//DebugOut.Analog[22] = MittelIntegralRoll / 26;
//DebugOut.Analog[24] = GierGyroFehler;
GierGyroFehler = 0;
 
//Salvo Ersatzkompass Ueberlauf korrigieren
if (GyroKomp_Int >= ((long)360 * GIER_GRAD_FAKTOR)) GyroKomp_Int = GyroKomp_Int - (GIER_GRAD_FAKTOR *(long)360); //neu ab 3.11.2007
if (GyroKomp_Int < 0) GyroKomp_Int = GyroKomp_Int + (GIER_GRAD_FAKTOR *(long)360); //neu ab 3.11.2007
ROT_OFF;
// Salvo End
 
/*DebugOut.Analog[17] = IntegralAccNick / 26;
DebugOut.Analog[18] = IntegralAccRoll / 26;
1015,8 → 1093,118
ZaehlMessungen = 0;
}
//DebugOut.Analog[31] = StickRoll / (26*IntegralFaktor);
// Salvo Ersatzkompass und Giergyrokompensation 15.12.2007 **********************
if ((Kompass_Neuer_Wert > 0)) //nur wenn Kompass einen neuen Wert geliefert hat
{
Kompass_Neuer_Wert = 0;
w = (abs(Mittelwert_AccNick));
v = (abs(Mittelwert_AccRoll));
if ((w < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT)) //Ersatzkompass und Giergyro nur mit Magnetkompass aktualisieren wenn alles ok
{
if ((abs(KompassValue - Kompass_Value_Old)) <= 5) // Aufeinanderfolgende Werte duerfen nur minimal abweichen
{
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if ((abs(StickGier) < 20)) //Giergyroabgleich mit Kompass
{
if (cnt_stickgier_zero < 255) cnt_stickgier_zero += 1;
if (cnt_stickgier_zero > 2) // nur Abgleichen wenn keine Stickbewegung da
{
w = (int) (GyroGier_Comp/(long)GIER_GRAD_FAKTOR);
v = KompassValue - gyrogier_kompass; // realen Drehwinkel seit letztem Ruecksetzen von GyroGier_Comp bestimmen
if (v <-180) v +=360; // Uberlaufkorrektur
if (v > 180) v -=360; // Uberlaufkorrektur
 
v = w-v; //Differenz Gyro zu Kompass ist der Driftfehler
 
#define GIER_COMP_MAX 4
if (v > GIER_COMP_MAX) v= GIER_COMP_MAX;
if (v < -GIER_COMP_MAX) v= - GIER_COMP_MAX;
if (abs(w) > 1)
{
GyroGier_Comp = 0;
gyrogier_kompass = KompassValue; // Kompasswert merken
AdNeutralGier -= v;
}
}
}
else
{
gyrogier_kompass = KompassValue; // Kompasswert merken
cnt_stickgier_zero = 0;
GyroGier_Comp = 0;
}
 
magkompass_ok = 1; // Flag dass Magnetkompass stabil arbeitet
GyroKomp_Int = (GyroKomp_Int )/(long)GIER_GRAD_FAKTOR;
 
w = KompassValue - GyroKomp_Int;
if ((w > 0) && (w < 180))
{
++GyroKomp_Int;
}
else if ((w > 0) && (w >= 180))
{
--GyroKomp_Int;
}
else if ((w < 0) && (w >= -180))
{
--GyroKomp_Int;
}
else if ((w < 0) && (w < -180))
{
++GyroKomp_Int;
}
if (GyroKomp_Int < 0) GyroKomp_Int = GyroKomp_Int + 360L;
GyroKomp_Int = (GyroKomp_Int%360L) * (long)GIER_GRAD_FAKTOR; // An Magnetkompasswert annaehern
}
}
else //Kompassfehler
{
magkompass_ok = 0;
GyroGier_Comp = 0;
}
Kompass_Value_Old = KompassValue;
}
// Salvo End *************************
 
// Salvo 6.10.2007
// GPS Home aktivieren wenn Knueppel in Ruhelage und Hoehenschalter aktiviert ist
//GPS Hold Aktiveren wenn Knueppel in Ruhelage sind
if ((abs(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]) < Parameter_NaviStickThreshold)
&& (abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) < Parameter_NaviStickThreshold) && (gps_alive_cnt > 0))
{
if ((Parameter_NaviGpsModeControl > 170) && (EE_Parameter.GlobalConfig & CFG_GPS_AKTIV)) //Hoehenschalter und GPS Flag aktiv
{
if ( gps_cmd == GPS_CMD_REQ_HOLD) gps_cmd = GPS_CMD_STOP; // erst mal stoppen, denn altes Kommando wurde noch nicht beendet
else gps_cmd = GPS_CMD_REQ_HOME;
n = GPS_CRTL(gps_cmd);
}
else if ((Parameter_NaviGpsModeControl < 170) && (EE_Parameter.GlobalConfig & CFG_GPS_AKTIV)) //Hoehenschalter Mittelstellung und GPS Flag aktiv
{
if ( gps_cmd == GPS_CMD_REQ_HOME) gps_cmd = GPS_CMD_STOP; // erst mal stoppen, denn altes Kommando wurde noch nicht beendet
else gps_cmd = GPS_CMD_REQ_HOLD;
n= GPS_CRTL(gps_cmd);
}
else // GPS komplett aus
{
if (gps_cmd != GPS_CMD_STOP)
{
gps_cmd = GPS_CMD_STOP;
n= GPS_CRTL(gps_cmd); //GPS Lageregelung beenden
}
}
}
else
{
if (gps_cmd != GPS_CMD_STOP)
{
gps_cmd = GPS_CMD_STOP;
n= GPS_CRTL(gps_cmd); //GPS Lageregelung beenden
}
}
if (gps_state != GPS_CRTL_IDLE) if (TimerWerteausgabe == 12) LED_J16_OFF; //led im GPS Mode schnell blinken lassen
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Gieren
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// if(GasMischanteil < 35) { if(StickGier > 10) StickGier = 10; else if(StickGier < -10) StickGier = -10;};
1040,53 → 1228,27
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//DebugOut.Analog[29] = (MaxStickNick + MaxStickRoll);
 
if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV))
{
int w,v,r,fehler,korrektur;
w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln
v = abs(IntegralRoll /512);
if(v > w) w = v; // grösste Neigung ermitteln
korrektur = w / 8 + 1;
fehler = ((540 + KompassValue - (ErsatzKompass/GIER_GRAD_FAKTOR)) % 360) - 180;
if(NeueKompassRichtungMerken)
{
fehler = 0;
ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR;
}
if(!KompassSignalSchlecht && w < 25)
{
GierGyroFehler += fehler;
if(NeueKompassRichtungMerken)
{
beeptime = 200;
// KompassStartwert = KompassValue;
KompassStartwert = (ErsatzKompass/GIER_GRAD_FAKTOR);
NeueKompassRichtungMerken = 0;
}
// Salvo 13.9.2007 Nur wenn Magnetkompass ordentliche Werte liefert
if ((magkompass_ok > 0) && NeueKompassRichtungMerken)
{
KompassStartwert = KompassValue;
NeueKompassRichtungMerken = 0;
}
ErsatzKompass += (fehler * 8) / korrektur;
w = (w * Parameter_KompassWirkung) / 32; // auf die Wirkung normieren
// Salvo 13.9.2007
w=0;
// Salvo End
w = (w * Parameter_KompassWirkung) / 64; // auf die Wirkung normieren
w = Parameter_KompassWirkung - w; // Wirkung ggf drosseln
if(w >= 0)
{
if(!KompassSignalSchlecht)
{
v = 64 + ((MaxStickNick + MaxStickRoll)) / 8;
r = ((540 + (ErsatzKompass/GIER_GRAD_FAKTOR) - KompassStartwert) % 360) - 180;
// r = KompassRichtung;
v = (r * w) / v; // nach Kompass ausrichten
w = 3 * Parameter_KompassWirkung;
if(v > w) v = w; // Begrenzen
else
if(v < -w) v = -w;
Mess_Integral_Gier += v;
}
if(KompassSignalSchlecht) KompassSignalSchlecht--;
}
else KompassSignalSchlecht = 500; // so lange das Signal taub stellen --> ca. 1 sek
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Salvo Kompasssteuerung **********************
if (magkompass_ok > 0) Mess_Integral_Gier += (KompassRichtung * w) / 32; // nach Kompass ausrichten
// Salvo End
}
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Debugwerte zuordnen
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1093,6 → 1255,11
if(!TimerWerteausgabe--)
{
TimerWerteausgabe = 24;
// Salvo 13.12.2007 Beleuchtung steuern
if (!(beeptime & BeepMuster)) LED_J16_FLASH;
else if (MotorenEin) LED_J16_ON;
else LED_J16_OFF;
// Salvo End
 
DebugOut.Analog[0] = IntegralNick / EE_Parameter.GyroAccFaktor;
DebugOut.Analog[1] = IntegralRoll / EE_Parameter.GyroAccFaktor;
1100,25 → 1267,34
DebugOut.Analog[3] = Mittelwert_AccRoll;
DebugOut.Analog[4] = MesswertGier;
DebugOut.Analog[5] = HoehenWert;
DebugOut.Analog[6] = (Mess_Integral_Hoch / 512);
// DebugOut.Analog[6] = (Mess_Integral_Hoch / 512);
DebugOut.Analog[8] = KompassValue;
DebugOut.Analog[9] = UBat;
DebugOut.Analog[11] = ErsatzKompass / GIER_GRAD_FAKTOR;
DebugOut.Analog[10] = SenderOkay;
DebugOut.Analog[10] = Mess_Integral_Gier / 128;
// DebugOut.Analog[10] = SenderOkay;
DebugOut.Analog[11] = GyroKomp_Int / GIER_GRAD_FAKTOR;
//DebugOut.Analog[16] = Mittelwert_AccHoch;
DebugOut.Analog[17] = FromNaviCtrl_Value.Distance;
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;
// DebugOut.Analog[17] = FromNaviCtrl_Value.Distance;
// DebugOut.Analog[18] = (int)FromNaviCtrl_Value.OsdBar;
// DebugOut.Analog[19] = WinkelOut.CalcState;
// DebugOut.Analog[20] = ServoValue;
 
 
// DebugOut.Analog[19] -= DebugOut.Analog[19]/128;
// if(DebugOut.Analog[19] > 0) DebugOut.Analog[19]--; else DebugOut.Analog[19]++;
DebugOut.Analog[23] = debug_gp_0;
DebugOut.Analog[24] = debug_gp_1;
DebugOut.Analog[25] = debug_gp_2;
DebugOut.Analog[26] = gps_rel_act_position.utm_east; //in 10cm ausgeben
DebugOut.Analog[27] = gps_rel_act_position.utm_north;
DebugOut.Analog[28] = gps_rel_act_position.utm_alt;
DebugOut.Analog[29] = gps_state + (gps_sub_state*10)+(50*gps_cmd);
 
DebugOut.Analog[30] = GPS_Nick;
DebugOut.Analog[31] = GPS_Roll;
 
 
/* DebugOut.Analog[16] = motor_rx[0];
DebugOut.Analog[17] = motor_rx[1];
DebugOut.Analog[18] = motor_rx[2];