Subversion Repositories FlightCtrl

Rev

Rev 858 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 858 Rev 887
Line 146... Line 146...
146
//############################################################################
146
//############################################################################
147
{
147
{
148
        acc_neutral.X = 0;
148
        acc_neutral.X = 0;
149
        acc_neutral.Y = 0;
149
        acc_neutral.Y = 0;
150
        acc_neutral.Z = 0;
150
        acc_neutral.Z = 0;
-
 
151
 
-
 
152
        Delay_ms(500);
-
 
153
        beeptime = 100;
-
 
154
        while (PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > -100)       //Warten, bis Benutzer den Kopter neu ausgerichtet hat
-
 
155
       
151
    CalibrierMittelwert();     
156
    CalibrierMittelwert();     
152
    Delay_ms_Mess(100);
157
    Delay_ms_Mess(100);
153
        CalibrierMittelwert();
158
        CalibrierMittelwert();
Line 154... Line 159...
154
               
159
               
155
        acc_neutral.X = abs(Mittelwert_AccNick) / ACC_AMPLIFY;
160
        acc_neutral.X = abs(Mittelwert_AccNick) / ACC_AMPLIFY;
156
        acc_neutral.Y = abs(Mittelwert_AccRoll) / ACC_AMPLIFY;
161
        acc_neutral.Y = abs(Mittelwert_AccRoll) / ACC_AMPLIFY;
157
        acc_neutral.Z = Aktuell_az;
-
 
158
/*     
-
 
159
        while (PPM_in_gas > 100)        //Warten, bis Benutzer den Kopter neu ausgerichtet hat
-
 
160
        {
-
 
161
                uint8_t delay=9;
162
        acc_neutral.Z = Aktuell_az;
162
               
163
       
163
                Delay_ms(10);
-
 
164
                if (!delay--)
-
 
165
                {
164
        beeptime = 100;
166
                        delay = 9;
165
        Delay_ms(500);
167
                        beeptime = 100;
-
 
-
 
166
        beeptime = 100;
168
                }
167
        while (PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > -100)       //Warten, bis Benutzer den Kopter neu ausgerichtet hat
169
        }
168
       
170
        Delay_ms_Mess(100);
169
        Delay_ms_Mess(100);
171
        acc_neutral.C = Aktuell_az;
170
        acc_neutral.compass = Aktuell_az;
172
*/     
171
       
173
        eeprom_write_block(&acc_neutral,&ee_acc_neutral,sizeof(struct acc_neutral_struct));
172
        eeprom_write_block(&acc_neutral,&ee_acc_neutral,sizeof(struct acc_neutral_struct));
Line 174... Line 173...
174
}
173
}
175
 
174
 
Line 192... Line 191...
192
    if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung aktiviert?
191
    if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung aktiviert?
193
     {    
192
     {    
194
      if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
193
      if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
195
     }
194
     }
Line 196... Line 195...
196
 
195
 
197
     AdNeutralNick= AdWertNick;
196
     AdNeutralNick = AdWertNick;       
198
         AdNeutralRoll= AdWertRoll;    
197
         AdNeutralRoll = AdWertRoll;   
199
         AdNeutralGier= AdWertGier;
198
         AdNeutralGier = AdWertGier;
200
     StartNeutralRoll = AdNeutralRoll;
199
     StartNeutralRoll = AdNeutralRoll;
Line 201... Line 200...
201
     StartNeutralNick = AdNeutralNick;
200
     StartNeutralNick = AdNeutralNick;
Line 224... Line 223...
224
//############################################################################
223
//############################################################################
225
// Bearbeitet die Messwerte
224
// Bearbeitet die Messwerte
226
void Mittelwert(void)
225
void Mittelwert(void)
227
//############################################################################
226
//############################################################################
228
{      
227
{      
229
    static signed long tmpl,tmpl2;     
228
    static signed long tmpl,tmpl2;
-
 
229
       
-
 
230
        uint8_t tmp_sreg = SREG;
-
 
231
    cli();
230
    MesswertGier = (signed int) AdNeutralGier - AdWertGier;
232
     MesswertGier = (int16_t)AdNeutralGier - AdWertGier;
231
    MesswertRoll = (signed int) AdWertRoll - AdNeutralRoll;
233
     MesswertRoll = (int16_t)AdWertRoll - AdNeutralRoll;
232
    MesswertNick = (signed int) AdWertNick - AdNeutralNick;
234
     MesswertNick = (int16_t)AdWertNick - AdNeutralNick;
-
 
235
         int16_t AdWertAccNick_temp = AdWertAccNick;
-
 
236
         int16_t AdWertAccRoll_temp = AdWertAccRoll;
-
 
237
        SREG = tmp_sreg;
233
 
238
       
234
//DebugOut.Analog[26] = MesswertNick;
239
//DebugOut.Analog[26] = MesswertNick;
235
//DebugOut.Analog[28] = MesswertRoll;
240
//DebugOut.Analog[28] = MesswertRoll;
Line 236... Line 241...
236
 
241
 
237
// Beschleunigungssensor  ++++++++++++++++++++++++++++++++++++++++++++++++
242
// Beschleunigungssensor  ++++++++++++++++++++++++++++++++++++++++++++++++
238
        Mittelwert_AccNick = ((long)Mittelwert_AccNick * 1 + ((ACC_AMPLIFY * (long)AdWertAccNick))) / 2L;
243
        Mittelwert_AccNick = ((long)Mittelwert_AccNick * 1 + ((ACC_AMPLIFY * (long)AdWertAccNick_temp))) / 2L;
239
        Mittelwert_AccRoll = ((long)Mittelwert_AccRoll * 1 + ((ACC_AMPLIFY * (long)AdWertAccRoll))) / 2L;
244
        Mittelwert_AccRoll = ((long)Mittelwert_AccRoll * 1 + ((ACC_AMPLIFY * (long)AdWertAccRoll_temp))) / 2L;
240
    IntegralAccNick += ACC_AMPLIFY * AdWertAccNick;
245
    IntegralAccNick += ACC_AMPLIFY * AdWertAccNick_temp;
241
    IntegralAccRoll += ACC_AMPLIFY * AdWertAccRoll;
246
    IntegralAccRoll += ACC_AMPLIFY * AdWertAccRoll_temp;
242
// Gier  ++++++++++++++++++++++++++++++++++++++++++++++++
247
// Gier  ++++++++++++++++++++++++++++++++++++++++++++++++
243
            Mess_Integral_Gier +=  MesswertGier;
248
            Mess_Integral_Gier +=  MesswertGier;
244
            Mess_Integral_Gier2 += MesswertGier;
249
            Mess_Integral_Gier2 += MesswertGier;
245
// Kopplungsanteil  +++++++++++++++++++++++++++++++++++++
250
// Kopplungsanteil  +++++++++++++++++++++++++++++++++++++
Line 330... Line 335...
330
 
335
 
331
//############################################################################
336
//############################################################################
332
// Messwerte beim Ermitteln der Nullage
337
// Messwerte beim Ermitteln der Nullage
333
void CalibrierMittelwert(void)
338
void CalibrierMittelwert(void)
334
//############################################################################
339
//############################################################################
335
{                
340
{
336
    // ADC auschalten, damit die Werte sich nicht während der Berechnung ändern
341
        uint8_t tmp_sreg = SREG;
337
        ANALOG_OFF;
342
    cli();
338
        MesswertNick = AdWertNick;
343
         MesswertNick = AdWertNick;
339
        MesswertRoll = AdWertRoll;
344
         MesswertRoll = AdWertRoll;
340
        MesswertGier = AdWertGier;
345
         MesswertGier = AdWertGier;
341
        Mittelwert_AccNick = ACC_AMPLIFY * (long)AdWertAccNick;
346
         Mittelwert_AccNick = ACC_AMPLIFY * (long)AdWertAccNick;
342
        Mittelwert_AccRoll = ACC_AMPLIFY * (long)AdWertAccRoll;
347
         Mittelwert_AccRoll = ACC_AMPLIFY * (long)AdWertAccRoll;
343
   // ADC einschalten
348
    SREG = tmp_sreg;
344
    ANALOG_ON; 
349
       
345
    if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 128 && Poti1 < 255) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 128 && Poti1 > 0) Poti1--;
350
    if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 128 && Poti1 < 255) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 128 && Poti1 > 0) Poti1--;
346
    if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 128 && Poti2 < 255) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 128 && Poti2 > 0) Poti2--;
351
    if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 128 && Poti2 < 255) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 128 && Poti2 > 0) Poti2--;
347
    if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 128 && Poti3 < 255) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 128 && Poti3 > 0) Poti3--;
352
    if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 128 && Poti3 < 255) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 128 && Poti3 > 0) Poti3--;
348
    if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 128 && Poti4 < 255) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 128 && Poti4 > 0) Poti4--;
353
    if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 128 && Poti4 < 255) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 128 && Poti4 > 0) Poti4--;
Line 444... Line 449...
444
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
449
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
445
        int16_t PPM_in_nick, PPM_in_roll, PPM_in_gier, PPM_diff_nick, PPM_diff_roll, PPM_in_gas;
450
        int16_t PPM_in_nick, PPM_in_roll, PPM_in_gier, PPM_diff_nick, PPM_diff_roll, PPM_in_gas;
Line 446... Line 451...
446
       
451
       
447
        uint8_t tmp_sreg = SREG;
452
        uint8_t tmp_sreg = SREG;
448
    cli();  
453
    cli();  
449
    PPM_in_nick = PPM_in[EE_Parameter.Kanalbelegung[K_NICK]];
454
     PPM_in_nick = PPM_in[EE_Parameter.Kanalbelegung[K_NICK]];
450
    PPM_diff_nick = PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]];
455
     PPM_diff_nick = PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]];
451
    PPM_in_roll = PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]];
456
     PPM_in_roll = PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]];
452
    PPM_diff_roll = PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]];
457
     PPM_diff_roll = PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]];
453
    PPM_in_gier = PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];
458
     PPM_in_gier = PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];
454
    PPM_in_gas = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]];
459
     PPM_in_gas = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]];
Line 455... Line 460...
455
    SREG = tmp_sreg;
460
    SREG = tmp_sreg;
456
 
461
 
457
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
462
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
Line 602... Line 607...
602
 
607
 
603
   if(abs(PPM_in_nick) > MaxStickNick)
608
   if(abs(PPM_in_nick) > MaxStickNick)
604
     MaxStickNick = abs(PPM_in_nick); else MaxStickNick--;
609
     MaxStickNick = abs(PPM_in_nick); else MaxStickNick--;
605
   if(abs(PPM_in_roll) > MaxStickRoll)
610
   if(abs(PPM_in_roll) > MaxStickRoll)
606
     MaxStickRoll = abs(PPM_in_roll); else MaxStickRoll--;
611
     MaxStickRoll = abs(PPM_in_roll); else MaxStickRoll--;
Line 607... Line 612...
607
   if(Notlandung)  {MaxStickNick = 0; MaxStickRoll = 0;}
612
   if(Notlandung) {MaxStickNick = 0; MaxStickRoll = 0;}
608
 
613
 
Line 609... Line 614...
609
    GyroFaktor     = ((float)Parameter_Gyro_P + 10.0) / 256.0;
614
    GyroFaktor     = ((float)Parameter_Gyro_P + 10.0) / 256.0;
Line 910... Line 915...
910
//DebugOut.Analog[31] = StickRoll / (26*IntegralFaktor);
915
//DebugOut.Analog[31] = StickRoll / (26*IntegralFaktor);
Line 911... Line 916...
911
 
916
 
912
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
917
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
913
//  Gieren
918
//  Gieren
914
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
919
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
915
    if(abs(StickGier) > 20)
920
    if(abs(StickGier) > 15)
916
     {
921
     {
917
      if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) NeueKompassRichtungMerken = 1;
922
      if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) NeueKompassRichtungMerken = 1;
918
     }
923
     }
919
    tmp_int  = (long)EE_Parameter.Gier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo  y = ax + bx²
924
    tmp_int  = (long)EE_Parameter.Gier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo  y = ax + bx²
Line 926... Line 931...
926
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
931
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
927
//  Kompass
932
//  Kompass
928
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
933
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
929
        if(EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)
934
        if(EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)
930
        {
935
        {
931
                if (!updKompass--)              // Aufruf mit ~15 Hz
936
                if (!updKompass--)              // Aufruf mit ~20 Hz
932
                {
937
                {
933
                        updKompass = 33;
938
                        updKompass = 25;
Line 934... Line 939...
934
                       
939
                       
935
                        if ((MaxStickNick < 50) && (MaxStickRoll < 50))         // Bei extremen Flugmanövern keine Kompassauswertung
940
                        if ((MaxStickNick < 50) && (MaxStickRoll < 50))         // Bei extremen Flugmanövern keine Kompassauswertung
936
                        {
941
                        {
937
                                KompassValue = heading_MM3();
942
                                KompassValue = heading_MM3();
Line 950... Line 955...
950
//  Debugwerte zuordnen
955
//  Debugwerte zuordnen
951
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
956
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
952
        if(!TimerWerteausgabe--)
957
        if(!TimerWerteausgabe--)
953
        {
958
        {
954
        TimerWerteausgabe = 24;
959
        TimerWerteausgabe = 24;
955
        //DebugOut.Analog[0] = IntegralNick / EE_Parameter.GyroAccFaktor;
960
        DebugOut.Analog[0] = IntegralNick / EE_Parameter.GyroAccFaktor;
956
    //DebugOut.Analog[1] = IntegralRoll / EE_Parameter.GyroAccFaktor;
961
    DebugOut.Analog[1] = IntegralRoll / EE_Parameter.GyroAccFaktor;
957
    DebugOut.Analog[2] = Mittelwert_AccNick;
962
    DebugOut.Analog[2] = Mittelwert_AccNick;
958
    DebugOut.Analog[3] = Mittelwert_AccRoll;
963
    DebugOut.Analog[3] = Mittelwert_AccRoll;
959
    DebugOut.Analog[4] = MesswertGier;
964
    DebugOut.Analog[4] = MesswertGier;
960
    DebugOut.Analog[5] = HoehenWert;
965
    DebugOut.Analog[5] = HoehenWert;
961
    DebugOut.Analog[6] = Mess_Integral_Hoch / 512;
966
    DebugOut.Analog[6] = Mess_Integral_Hoch / 512;