Subversion Repositories FlightCtrl

Rev

Rev 466 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 466 Rev 479
Line 56... Line 56...
56 
#include "main.h"
 56 
#include "main.h"
57 
#include "eeprom.c"
 57 
#include "eeprom.c"
Line 58... Line 58...
58 
 
 58 
 
59 
unsigned char h,m,s;
 59 
unsigned char h,m,s;
60 
volatile unsigned int I2CTimeout = 100;
 60 
volatile unsigned int I2CTimeout = 100;
61 
volatile int MesswertNick,MesswertRoll,MesswertGier;
 61 
int MesswertNick,MesswertRoll,MesswertGier;
62 
volatile int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0,StartNeutralRoll = 0,StartNeutralNick = 0;
 62 
int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0, StartNeutralRoll = 0, StartNeutralNick = 0;
63 
volatile int Mittelwert_AccNick, Mittelwert_AccRoll,Mittelwert_AccHoch, NeutralAccX=0, NeutralAccY=0;
 - 
 
64 
volatile float NeutralAccZ = 0;
 - 
 
- 
 
 63 
int Mittelwert_AccNick, Mittelwert_AccRoll,Mittelwert_AccHoch;
65 
unsigned char CosinusNickWinkel = 0, CosinusRollWinkel = 0;
 64 
 
66 
volatile long IntegralNick = 0,IntegralNick2 = 0;
 65 
long IntegralNick = 0,IntegralNick2 = 0;
67 
volatile long IntegralRoll = 0,IntegralRoll2 = 0;
 66 
long IntegralRoll = 0,IntegralRoll2 = 0;
68 
volatile long IntegralAccNick = 0,IntegralAccRoll = 0,IntegralAccZ = 0;
 67 
long IntegralAccNick = 0,IntegralAccRoll = 0,IntegralAccZ = 0;
69 
volatile long Integral_Gier = 0;
 68 
long Integral_Gier = 0;
70 
volatile long Mess_IntegralNick = 0,Mess_IntegralNick2 = 0;
 69 
volatile long Mess_IntegralNick = 0,Mess_IntegralNick2 = 0;
71 
volatile long Mess_IntegralRoll = 0,Mess_IntegralRoll2 = 0;
 70 
volatile long Mess_IntegralRoll = 0,Mess_IntegralRoll2 = 0;
72 
volatile long Mess_Integral_Gier = 0,Mess_Integral_Gier2 = 0;
 71 
volatile long Mess_Integral_Gier = 0,Mess_Integral_Gier2 = 0;
73 
volatile long MittelIntegralNick,MittelIntegralRoll,MittelIntegralNick2,MittelIntegralRoll2;
 72 
volatile long MittelIntegralNick,MittelIntegralRoll,MittelIntegralNick2,MittelIntegralRoll2;
74 
volatile long Mess_Integral_Hoch = 0;
 73 
volatile long Mess_Integral_Hoch = 0;
75 
int  KompassValue = 0;
 74 
int KompassValue = 0;
76 
int  KompassStartwert = 0;
 75 
int KompassStartwert = 0;
77 
int  KompassRichtung = 0;
 76 
int KompassRichtung = 0;
78 
uint8_t updKompass = 0;
 77 
uint8_t updKompass = 0;
79 
unsigned char MAX_GAS,MIN_GAS;
 78 
unsigned char MAX_GAS,MIN_GAS;
80 
unsigned char Notlandung = 0;
 79 
unsigned char Notlandung = 0;
81 
unsigned char HoehenReglerAktiv = 0;
 80 
unsigned char HoehenReglerAktiv = 0;
Line 82... Line 81...
82 
long Umschlag180Nick = 250000L, Umschlag180Roll = 250000L;
 81 
long Umschlag180Nick = 250000L, Umschlag180Roll = 250000L;
83 
 
 82 
 
Line 84... Line 83...
84 
float GyroFaktor;
 83 
float GyroFaktor;
85 
float IntegralFaktor;
 84 
float IntegralFaktor;
86 
 
 85 
 
87 
volatile int  DiffNick,DiffRoll;
 86 
int DiffNick,DiffRoll;
88 
int  Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0;
 87 
int  Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0;
89 
volatile unsigned char Motor_Vorne,Motor_Hinten,Motor_Rechts,Motor_Links, Count;
 88 
volatile unsigned char Motor_Vorne,Motor_Hinten,Motor_Rechts,Motor_Links, Count;
Line 114... Line 113...
114 
unsigned char Parameter_UserParam4 = 0;
 113 
unsigned char Parameter_UserParam4 = 0;
115 
unsigned char Parameter_ServoNickControl = 100;
 114 
unsigned char Parameter_ServoNickControl = 100;
116 
unsigned char Parameter_LoopGasLimit = 70;
 115 
unsigned char Parameter_LoopGasLimit = 70;
117 
unsigned char Parameter_AchsKopplung1 = 0;
 116 
unsigned char Parameter_AchsKopplung1 = 0;
118 
unsigned char Parameter_AchsGegenKopplung1 = 0;
 117 
unsigned char Parameter_AchsGegenKopplung1 = 0;
- 
 
 118 
 
119 
struct mk_param_struct EE_Parameter;
 119 
struct mk_param_struct EE_Parameter;
- 
 
 120 
struct acc_neutral_struct ee_acc_neutral EEMEM;         // Reservierung im EEPROM
- 
 
 121 
struct acc_neutral_struct acc_neutral;
- 
 
 122 
 
- 
 
 123 
 
Line 120... Line 124...
120 
 
 124 
 
121 
void Piep(unsigned char Anzahl)
 125 
void Piep(unsigned char Anzahl)
122 
{
 126 
{
123 
 while(Anzahl--)
 127 
 while(Anzahl--)
Line 127... Line 131...
127 
  Delay_ms(250);
 131 
  Delay_ms(250);
128 
 }
 132 
 }
129 
}
 133 
}
Line 130... Line 134...
130 
 
 134 
 
- 
 
 135 
//############################################################################
- 
 
 136 
//  Neutrallage kalibrieren und fest im EEPROM abspeichern
- 
 
 137 
void calib_acc(void)
- 
 
 138 
//############################################################################
- 
 
 139 
{
- 
 
 140 
        acc_neutral.X = 0;
- 
 
 141 
        acc_neutral.Y = 0;
- 
 
 142 
        acc_neutral.Z = 0;
- 
 
 143 
    CalibrierMittelwert();     
- 
 
 144 
    Delay_ms_Mess(100);
- 
 
 145 
        CalibrierMittelwert();
- 
 
 146 
               
- 
 
 147 
        acc_neutral.X = abs(Mittelwert_AccNick) / ACC_AMPLIFY;
- 
 
 148 
        acc_neutral.Y = abs(Mittelwert_AccRoll) / ACC_AMPLIFY;
- 
 
 149 
        acc_neutral.Z = Aktuell_az;
- 
 
 150 
       
- 
 
 151 
        eeprom_write_block(&acc_neutral,&ee_acc_neutral,sizeof(struct acc_neutral_struct));
- 
 
 152 
}
- 
 
 153 
 
131 
//############################################################################
 154 
//############################################################################
132 
//  Nullwerte ermitteln
 155 
//  Nullwerte ermitteln
133 
void SetNeutral(void)
 156 
void SetNeutral(void)
134 
//############################################################################
 157 
//############################################################################
135 
{
 158 
{
136 
        NeutralAccX = 0;
 159 
        acc_neutral.X = 0;
137 
        NeutralAccY = 0;
 160 
        acc_neutral.Y = 0;
138 
        NeutralAccZ = 0;
 161 
        acc_neutral.Z = 0;
139 
    AdNeutralNick = 0; 
 162 
    AdNeutralNick = 0; 
140 
        AdNeutralRoll = 0;     
 163 
        AdNeutralRoll = 0;     
141 
        AdNeutralGier = 0;
 164 
        AdNeutralGier = 0;
142 
    Parameter_AchsKopplung1 = 0;
 165 
    Parameter_AchsKopplung1 = 0;
Line 152... Line 175...
152 
     AdNeutralNick= AdWertNick;
 175 
     AdNeutralNick= AdWertNick;
153 
         AdNeutralRoll= AdWertRoll;    
 176 
         AdNeutralRoll= AdWertRoll;    
154 
         AdNeutralGier= AdWertGier;
 177 
         AdNeutralGier= AdWertGier;
155 
     StartNeutralRoll = AdNeutralRoll;
 178 
     StartNeutralRoll = AdNeutralRoll;
156 
     StartNeutralNick = AdNeutralNick;
 179 
     StartNeutralNick = AdNeutralNick;
- 
 
 180 
         
157 
    NeutralAccY = abs(Mittelwert_AccRoll) / ACC_AMPLIFY;
 181 
    eeprom_read_block(&acc_neutral,&ee_acc_neutral,sizeof(struct acc_neutral_struct));
158 
        NeutralAccX = abs(Mittelwert_AccNick) / ACC_AMPLIFY;
 - 
 
159 
        NeutralAccZ = Aktuell_az;
 - 
 
Line 160... Line 182...
160 
   
 182 
   
161 
        Mess_IntegralNick = 0; 
 183 
        Mess_IntegralNick = 0; 
162 
    Mess_IntegralNick2 = 0;
 184 
    Mess_IntegralNick2 = 0;
163 
    Mess_IntegralRoll = 0;     
 185 
    Mess_IntegralRoll = 0;     
Line 190... Line 212...
190 
        Mittelwert_AccNick = ((long)Mittelwert_AccNick * 1 + ((ACC_AMPLIFY * (long)AdWertAccNick))) / 2L;
 212 
        Mittelwert_AccNick = ((long)Mittelwert_AccNick * 1 + ((ACC_AMPLIFY * (long)AdWertAccNick))) / 2L;
191 
        Mittelwert_AccRoll = ((long)Mittelwert_AccRoll * 1 + ((ACC_AMPLIFY * (long)AdWertAccRoll))) / 2L;
 213 
        Mittelwert_AccRoll = ((long)Mittelwert_AccRoll * 1 + ((ACC_AMPLIFY * (long)AdWertAccRoll))) / 2L;
192 
        Mittelwert_AccHoch = ((long)Mittelwert_AccHoch * 1 + ((long)AdWertAccHoch)) / 2L;
 214 
        Mittelwert_AccHoch = ((long)Mittelwert_AccHoch * 1 + ((long)AdWertAccHoch)) / 2L;
193 
    IntegralAccNick += ACC_AMPLIFY * AdWertAccNick;
 215 
    IntegralAccNick += ACC_AMPLIFY * AdWertAccNick;
194 
    IntegralAccRoll += ACC_AMPLIFY * AdWertAccRoll;
 216 
    IntegralAccRoll += ACC_AMPLIFY * AdWertAccRoll;
195 
    IntegralAccZ    += Aktuell_az - 704;//NeutralAccZ;
 217 
    IntegralAccZ    += Aktuell_az - 704;//acc_neutral.Z;
196 
// Gier  ++++++++++++++++++++++++++++++++++++++++++++++++
 218 
// Gier  ++++++++++++++++++++++++++++++++++++++++++++++++
197 
            Mess_Integral_Gier +=  MesswertGier;
 219 
            Mess_Integral_Gier +=  MesswertGier;
198 
            Mess_Integral_Gier2 += MesswertGier;
 220 
            Mess_Integral_Gier2 += MesswertGier;
199 
// Kopplungsanteil  +++++++++++++++++++++++++++++++++++++
 221 
// Kopplungsanteil  +++++++++++++++++++++++++++++++++++++
200 
      if(!Looping_Nick && !Looping_Roll && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV))
 222 
      if(!Looping_Nick && !Looping_Roll && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV))