Subversion Repositories BL-Ctrl

Rev

Rev 80 | Rev 82 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed

Rev Author Line No. Line
60 hbuss 1
/*#######################################################################################
2
Flight Control
3
#######################################################################################*/
4
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
5
// + Regler für Brushless-Motoren
6
// + ATMEGA8 mit 8MHz
7
// + Nur für den privaten Gebrauch
8
// + Copyright (c) 12.2007 Holger Buss
9
// + www.MikroKopter.com
10
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
11
// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation), 
12
// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist. 
13
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt 
14
// + bzgl. der Nutzungsbedingungen aufzunehmen. 
15
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
16
// + Verkauf von Luftbildaufnahmen, usw.
17
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
18
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht, 
19
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen
20
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
21
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
22
// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
23
// + eindeutig als Ursprung verlinkt werden
24
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
25
// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
26
// + Benutzung auf eigene Gefahr
27
// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
28
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
29
// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur 
30
// + mit unserer Zustimmung zulässig
31
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
32
// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
33
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
34
// + Redistributions of source code (with or without modifications) must retain the above copyright notice, 
35
// + this list of conditions and the following disclaimer.
36
// +   * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
37
// +     from this software without specific prior written permission.
38
// +   * The use of this project (hardware, software, binary files, sources and documentation) is only permittet 
39
// +     for non-commercial use (directly or indirectly)
40
// +     Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted 
41
// +     with our written permission
73 hbuss 42
// +   * If sources or documentations are redistributet on other webpages, our webpage (http://www.MikroKopter.de) must be 
60 hbuss 43
// +     clearly linked as origin 
44
// +   * porting to systems other than hardware from www.mikrokopter.de is not allowed
45
// +  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
46
// +  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
47
// +  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
48
// +  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
49
// +  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
50
// +  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
51
// +  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
52
// +  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
53
// +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
54
// +  POSSIBILITY OF SUCH DAMAGE. 
55
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1 ingob 56
 
57
#include "main.h"
58
 
63 hbuss 59
unsigned int  PWM = 0;
58 hbuss 60
unsigned int  Strom = 0,RuheStrom; //ca. in 0,1A
1 ingob 61
unsigned char Strom_max = 0;
51 holgerb 62
unsigned char Mittelstrom = 0;
1 ingob 63
unsigned int  Drehzahl = 0;  // in 100UPM  60 = 6000
64
unsigned int  KommutierDelay = 10;
65
unsigned int  I2C_Timeout = 0;
23 ingob 66
unsigned int SIO_Timeout = 0;
1 ingob 67
unsigned int  SollDrehzahl = 0;
68
unsigned int  IstDrehzahl = 0;
69
unsigned int  DrehZahlTabelle[256];//vorberechnete Werte zur Drehzahlerfassung
70
unsigned char ZeitFuerBerechnungen = 1;
71
unsigned char MotorAnwerfen = 0;
72
unsigned char MotorGestoppt = 1;
73
unsigned char MaxPWM = MAX_PWM;
74
unsigned int  CntKommutierungen = 0;
75
unsigned int  SIO_Drehzahl = 0;
76
unsigned char ZeitZumAdWandeln = 1;
24 ingob 77
unsigned char MotorAdresse = 1;
50 holgerb 78
unsigned char PPM_Betrieb = 1;
73 hbuss 79
unsigned char HwVersion;
80
unsigned char IntRef = 0;
81
unsigned int MinUpmPulse;
1 ingob 82
//############################################################################
83
//
84
void SetPWM(void)
85
//############################################################################
86
{
87
    unsigned char tmp_pwm;
88
    tmp_pwm = PWM;
89
    if(tmp_pwm > MaxPWM)    // Strombegrenzung
90
        {
91
        tmp_pwm = MaxPWM;
92
        PORTC |= ROT;
93
        }
94
    if(Strom > MAX_STROM)   // Strombegrenzung
95
        {
73 hbuss 96
        OCR1A = 0; OCR1B = 0; OCR2 = 0;
97
        PORTD &= ~0x38;
1 ingob 98
        PORTC |= ROT;
73 hbuss 99
        DebugOut.Analog[6]++;
1 ingob 100
        Strom--;
101
        }
102
    else
103
        {
104
        #ifdef  _32KHZ 
105
        OCR1A =  tmp_pwm; OCR1B =  tmp_pwm; OCR2  = tmp_pwm;
106
        #endif 
107
 
108
        #ifdef  _16KHZ 
50 holgerb 109
        //OCR1A = 2 * (int)tmp_pwm; OCR1B = 2 * (int)tmp_pwm; OCR2  = tmp_pwm;
110
        OCR1A =  tmp_pwm; OCR1B =  tmp_pwm; OCR2  = tmp_pwm;
1 ingob 111
        #endif 
112
        }
113
}
114
 
73 hbuss 115
void DebugAusgaben(void)
116
{
117
    DebugOut.Analog[0] = Strom;
118
    DebugOut.Analog[1] = Mittelstrom;
119
    DebugOut.Analog[2] = SIO_Drehzahl;
120
    DebugOut.Analog[3] = PPM_Signal;
121
    DebugOut.Analog[4] = OCR2;
122
//    DebugOut.Analog[5] = PWM;
123
}
124
 
1 ingob 125
//############################################################################
126
//
127
void PWM_Init(void)
128
//############################################################################
129
{
130
    PWM_OFF;
50 holgerb 131
    TCCR1B = (1 << CS10) | (0 << CS11) | (0 << CS12) | (0 << WGM12) |
1 ingob 132
             (0 << WGM13) | (0<< ICES1) | (0 << ICNC1);
50 holgerb 133
/*    TCCR1B = (1 << CS10) | (0 << CS11) | (0 << CS12) | (1 << WGM12) |
134
             (0 << WGM13) | (0<< ICES1) | (0 << ICNC1);
135
*/
1 ingob 136
}
137
 
138
//############################################################################
139
//
140
void Wait(unsigned char dauer)
141
//############################################################################
142
{
143
    dauer = (unsigned char)TCNT0 + dauer;
144
    while((TCNT0 - dauer) & 0x80);
145
}
146
 
73 hbuss 147
void RotBlink(unsigned char anz)
148
{
149
sei(); // Interrupts ein
150
 while(anz--)
151
  {
152
   PORTC |= ROT;
153
   Delay_ms(300);    
154
   PORTC &= ~ROT;
155
   Delay_ms(300);    
156
  }
157
   Delay_ms(1000);    
158
}
159
 
1 ingob 160
//############################################################################
161
//
73 hbuss 162
char Anwerfen(unsigned char pwm)
1 ingob 163
//############################################################################
164
{
165
    unsigned long timer = 300,i;
166
    DISABLE_SENSE_INT;
167
    PWM = 5;
168
    SetPWM();
169
    Manuell();
73 hbuss 170
//    Delay_ms(200);
171
                    MinUpmPulse = SetDelay(300);
172
                    while(!CheckDelay(MinUpmPulse))
173
                    {
174
                     FastADConvert();
175
                      if(Strom > 120)
176
                      {
177
                        STEUER_OFF; // Abschalten wegen Kurzschluss
178
                        RotBlink(10);
179
                        return(0);
180
                      }  
181
                    }
1 ingob 182
    PWM = pwm;
183
    while(1)
184
        {
185
        for(i=0;i<timer; i++)
186
            {
187
            if(!UebertragungAbgeschlossen)  SendUart();
188
            else DatenUebertragung();
189
            Wait(100);  // warten
190
            }
73 hbuss 191
        DebugAusgaben();
192
        FastADConvert();
193
        if(Strom > 60)
194
          {
195
            STEUER_OFF; // Abschalten wegen Kurzschluss
196
            RotBlink(10);
197
            return(0);
198
          }  
199
 
1 ingob 200
        timer-= timer/15+1;
73 hbuss 201
        if(timer < 25) { if(TEST_MANUELL) timer = 25; else return(1); }
1 ingob 202
        Manuell();
203
        Phase++;
204
        Phase %= 6;
205
        AdConvert();
206
        PWM = pwm;
207
        SetPWM();
208
        if(SENSE)
209
            {
210
            PORTD ^= GRUEN;
211
            }
212
        }
213
}
214
 
50 holgerb 215
/*
216
#define SENSE_A ADMUX = 0;
217
#define SENSE_B ADMUX = 1;
218
#define SENSE_C ADMUX = 2;
219
 
220
#define ClrSENSE            ACSR |= 0x10
221
#define SENSE               ((ACSR & 0x10))
222
#define SENSE_L             (!(ACSR & 0x20))
223
#define SENSE_H             ((ACSR & 0x20))
224
*/
225
 
226
 
58 hbuss 227
#define TEST_STROMGRENZE 120
50 holgerb 228
unsigned char DelayM(unsigned int timer)
229
{
230
 while(timer--)
231
  {
232
   FastADConvert();
58 hbuss 233
   if(Strom > (TEST_STROMGRENZE + RuheStrom))
50 holgerb 234
       {
51 holgerb 235
        FETS_OFF;
50 holgerb 236
        return(1);
237
       }
238
  }
239
 return(0);  
240
}
241
 
242
unsigned char Delay(unsigned int timer)
243
{
244
 while(timer--)
245
  {
51 holgerb 246
//   if(SENSE_H) { PORTC |= ROT; } else { PORTC &= ~ROT;}
81 pangu 247
        asm volatile("");
50 holgerb 248
  }
249
 return(0);  
250
}
251
 
58 hbuss 252
/*
51 holgerb 253
void ShowSense(void)
254
{
255
 if(SENSE_H) { PORTC |= ROT; } else { PORTC &= ~ROT;}
50 holgerb 256
 
51 holgerb 257
}
58 hbuss 258
*/
51 holgerb 259
#define HIGH_A_EIN PORTB |= 0x08
260
#define HIGH_B_EIN PORTB |= 0x04
261
#define HIGH_C_EIN PORTB |= 0x02
262
#define LOW_A_EIN  PORTD |= 0x08
263
#define LOW_B_EIN  PORTD |= 0x10
264
#define LOW_C_EIN  PORTD |= 0x20
265
 
50 holgerb 266
void MotorTon(void)
1 ingob 267
//############################################################################
50 holgerb 268
{
81 pangu 269
    unsigned char ADR_TAB[7] = {0,0,3,1,4,2,5};
50 holgerb 270
    unsigned int timer = 300,i;
51 holgerb 271
    unsigned int t = 0;
58 hbuss 272
    unsigned char anz = 0,MosfetOkay = 0, grenze = 50;
51 holgerb 273
 
274
    PORTC &= ~ROT;
275
    Delay_ms(300 * ADR_TAB[MotorAdresse]);    
50 holgerb 276
    DISABLE_SENSE_INT;
277
    cli();//Globale Interrupts Ausschalten
278
    uart_putchar('\n');
279
    STEUER_OFF;
58 hbuss 280
    Strom_max = 0;
281
    DelayM(50);
282
    RuheStrom = Strom_max;
283
//    uart_putchar(RuheStrom + 'A');
50 holgerb 284
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
285
//+ High-Mosfets auf Kurzschluss testen
286
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
287
    Strom = 0;
71 hbuss 288
/*
51 holgerb 289
    LOW_B_EIN;
290
    HIGH_A_EIN;
50 holgerb 291
    if(DelayM(3))
292
       {
293
        anz = 1;
51 holgerb 294
        uart_putchar('1');
50 holgerb 295
       }
51 holgerb 296
    FETS_OFF;
297
    Delay(1000);
50 holgerb 298
    Strom = 0;
51 holgerb 299
    LOW_A_EIN;
300
    HIGH_B_EIN;
50 holgerb 301
    if(DelayM(3))
302
       {
303
        anz = 2;
51 holgerb 304
        uart_putchar('2');
50 holgerb 305
       }
51 holgerb 306
    FETS_OFF;
307
    Delay(1000);
50 holgerb 308
    Strom = 0;
51 holgerb 309
    LOW_B_EIN; // Low C ein
310
    HIGH_C_EIN; // High B ein
50 holgerb 311
    if(DelayM(3))
312
       {
313
        anz = 3;
51 holgerb 314
        uart_putchar('3');
50 holgerb 315
       }
51 holgerb 316
    FETS_OFF;
317
    Delay(1000);
318
    LOW_A_EIN; // Low  A ein; und A gegen C
319
    HIGH_C_EIN; // High C ein
50 holgerb 320
    if(DelayM(3))
321
       {
322
        anz = 3;
51 holgerb 323
        uart_putchar('7');
50 holgerb 324
       }
51 holgerb 325
    FETS_OFF;
326
    DelayM(10000);
71 hbuss 327
 
51 holgerb 328
if(anz) while(1) RotBlink(anz);  // bei Kurzschluss nicht starten
71 hbuss 329
*/
50 holgerb 330
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
51 holgerb 331
//+ LOW-Mosfets auf Schalten und Kurzschluss testen
50 holgerb 332
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
58 hbuss 333
 if(UDR == ' ') {t = 65535; grenze = 40; uart_putchar('_');} else t = 1000; // Ausführlicher Test
51 holgerb 334
 Strom = 0;
335
 for(i=0;i<t;i++)
336
 {
337
  LOW_A_EIN;
338
  DelayM(1);
339
  FETS_OFF;
340
  Delay(5);
341
  HIGH_A_EIN;
342
  DelayM(1);
343
  FETS_OFF;
73 hbuss 344
  if(Strom > grenze + RuheStrom) {anz = 4; uart_putchar('4'); FETS_OFF; break;}
51 holgerb 345
  Delay(5);
346
 }
347
 Delay(10000);
50 holgerb 348
 
51 holgerb 349
 Strom = 0;
350
 for(i=0;i<t;i++)
351
 {
352
  LOW_B_EIN;
353
  DelayM(1);
354
  FETS_OFF;
355
  Delay(5);
356
  HIGH_B_EIN;
357
  DelayM(1);
358
  FETS_OFF;
73 hbuss 359
  if(Strom > grenze + RuheStrom) {anz = 5; uart_putchar('5'); FETS_OFF;break;}
51 holgerb 360
  Delay(5);
361
 }
50 holgerb 362
 
51 holgerb 363
 Strom = 0;
364
 Delay(10000);
365
 
366
 for(i=0;i<t;i++)
367
 {
368
  LOW_C_EIN;
369
  DelayM(1);
370
  FETS_OFF;
371
  Delay(5);
372
  HIGH_C_EIN;
373
  DelayM(1);
374
  FETS_OFF;
73 hbuss 375
  if(Strom > grenze + RuheStrom) {anz = 6; uart_putchar('6'); FETS_OFF; break;}
51 holgerb 376
  Delay(5);
377
 }
378
 
73 hbuss 379
 if(anz) while(1) RotBlink(anz);  // bei Kurzschluss nicht starten
380
 
50 holgerb 381
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
382
//+ High-Mosfets auf Schalten testen
383
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
384
    SENSE_A;
51 holgerb 385
    FETS_OFF;
386
    LOW_B_EIN; // Low B ein
387
    LOW_C_EIN; // Low C ein
50 holgerb 388
    Strom = 0;
51 holgerb 389
#define TONDAUER  40000    
71 hbuss 390
#define SOUND_E 1  // 1
51 holgerb 391
#define SOUND1_A 300
392
#define SOUND2_A 330
393
#define SOUND3_A 360
394
 
50 holgerb 395
    for(i=0; i< (TONDAUER / SOUND2_A) ; i++)
396
     {
51 holgerb 397
      HIGH_A_EIN; // Test A
50 holgerb 398
      Delay(SOUND_E);
51 holgerb 399
      if(MessAD(0) > 50) { MosfetOkay |= 0x01; } else { MosfetOkay &= ~0x01;};
50 holgerb 400
      PORTB = 0;
51 holgerb 401
      Delay(SOUND1_A);
50 holgerb 402
     }
51 holgerb 403
    FETS_OFF;
404
 
405
    LOW_A_EIN; // Low A ein
406
    LOW_C_EIN; // Low C ein
50 holgerb 407
    for(i=0; i<(TONDAUER / SOUND1_A); i++)
408
     {
51 holgerb 409
      HIGH_B_EIN; // Test B
50 holgerb 410
      Delay(SOUND_E);
51 holgerb 411
      if(MessAD(1) > 50) { MosfetOkay |= 0x02; } else { MosfetOkay &= ~0x02;};
50 holgerb 412
      PORTB = 0;
413
      Delay(SOUND1_A);
414
     }
415
 
51 holgerb 416
    FETS_OFF;
417
    LOW_A_EIN; // Low A ein
418
    LOW_B_EIN; // Low B ein
50 holgerb 419
    for(i=0; i<(TONDAUER / SOUND3_A); i++)
420
     {
51 holgerb 421
      HIGH_C_EIN; // Test C
50 holgerb 422
      Delay(SOUND_E);
51 holgerb 423
      if(MessAD(2) > 50) { MosfetOkay |= 0x04; } else { MosfetOkay &= ~0x04;};
50 holgerb 424
      PORTB = 0;
51 holgerb 425
      Delay(SOUND2_A);
50 holgerb 426
     }
51 holgerb 427
    FETS_OFF;
50 holgerb 428
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
429
//+ Low-Mosfets auf Schalten testen
430
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
76 hbuss 431
//    SENSE_B;
51 holgerb 432
    LOW_A_EIN; // Low A ein
50 holgerb 433
    for(i=0; i< (TONDAUER / SOUND2_A) ; i++)
434
     {
51 holgerb 435
      HIGH_B_EIN; // Test B
50 holgerb 436
      Delay(SOUND_E);
76 hbuss 437
      if(MessAD(0) > 128) { MosfetOkay &= ~0x08;} else { MosfetOkay |= 0x08;};
50 holgerb 438
      PORTB = 0;
439
      Delay(SOUND2_A);
440
     }
51 holgerb 441
 
442
//++++++++++++++++++++++++++++++++++++
443
    LOW_C_EIN; // Low C ein
444
    for(i=0; i<(TONDAUER / SOUND1_A); i++)
50 holgerb 445
     {
51 holgerb 446
      HIGH_B_EIN; // Test B
50 holgerb 447
      Delay(SOUND_E);
76 hbuss 448
      if(MessAD(2) > 128) { MosfetOkay &= ~0x20;} else { MosfetOkay |= 0x20;};
50 holgerb 449
      PORTB = 0;
450
      Delay(SOUND3_A);
451
     }
51 holgerb 452
    FETS_OFF;
453
//++++++++++++++++++++++++++++++++++++
454
    FETS_OFF;
455
    LOW_B_EIN; // Low B ein
456
    for(i=0; i<(TONDAUER / SOUND3_A); i++)
50 holgerb 457
     {
51 holgerb 458
      HIGH_C_EIN; // Test C
50 holgerb 459
      Delay(SOUND_E);
76 hbuss 460
      if(MessAD(1) > 128) { MosfetOkay &= ~0x10;} else { MosfetOkay |= 0x10;};
50 holgerb 461
      PORTB = 0;
51 holgerb 462
      Delay(SOUND3_A);
50 holgerb 463
     }
51 holgerb 464
    FETS_OFF;
465
//++++++++++++++++++++++++++++++++++++
50 holgerb 466
 
51 holgerb 467
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
50 holgerb 468
    sei();//Globale Interrupts Einschalten
51 holgerb 469
//    Delay_ms(250 * MotorAdresse);    
470
/*
471
    LOW_A_EIN; // Low B ein
50 holgerb 472
#define SOUND8_A 650
473
    for(i=0; i<(TONDAUER / SOUND8_A); i++)
474
     {
51 holgerb 475
      HIGH_B_EIN; // Test B
476
      Delay(SOUND_E);
477
      PORTB = 0;
478
      Delay(SOUND8_A);
50 holgerb 479
     }
51 holgerb 480
*/
81 pangu 481
 Delay_ms(300 * (6-ADR_TAB[MotorAdresse]));    
58 hbuss 482
 if(!(MosfetOkay & 0x01))  { anz = 1; UDR='A'; } else
483
 if(!(MosfetOkay & 0x02))  { anz = 2; UDR='B'; } else
484
 if(!(MosfetOkay & 0x04))  { anz = 3; UDR='C'; } else
485
 if(!(MosfetOkay & 0x08))  { anz = 4; UDR='a'; } else
486
 if(!(MosfetOkay & 0x10))  { anz = 5; UDR='b'; } else
487
 if(!(MosfetOkay & 0x20))  { anz = 6; UDR='c'; }  
50 holgerb 488
 
73 hbuss 489
// if(anz) Delay_ms(1000); 
490
 if(anz) while(1) RotBlink(anz);  // bei Kurzschluss nicht starten
51 holgerb 491
 RotBlink(anz);
492
 uart_putchar('.');
50 holgerb 493
}
494
 
495
//############################################################################
1 ingob 496
//
497
unsigned char SollwertErmittlung(void)
498
//############################################################################
499
{
500
    static unsigned int sollwert = 0;
501
    unsigned int ppm;
502
    if(!I2C_Timeout)   // bei Erreichen von 0 ist der Wert ungültig
503
        {
504
        if(SIO_Timeout)  // es gibt gültige SIO-Daten
505
            {
50 holgerb 506
             sollwert =  (MAX_PWM * (unsigned int) SIO_Sollwert) / 200;  // skalieren auf 0-200 = 0-255
507
             PPM_Betrieb = 0;
508
             ICP_INT_DISABLE;
509
             PORTC &= ~ROT;
1 ingob 510
            }
511
        else
50 holgerb 512
            if(anz_ppm_werte > 20)  // es gibt gültige PPM-Daten
1 ingob 513
                {
50 holgerb 514
                PPM_Betrieb = 1;
1 ingob 515
                ppm = PPM_Signal;
516
                if(ppm > 300) ppm =   0;  // ungültiges Signal
517
                if(ppm > 200) ppm = 200;
518
                if(ppm <= MIN_PPM) sollwert = 0;
519
                else
520
                    {
521
                    sollwert = (int) MIN_PWM + ((MAX_PWM - MIN_PWM) * (ppm - MIN_PPM)) / (190 - MIN_PPM);
522
                    }
523
                PORTC &= ~ROT;
524
                }
525
            else   // Kein gültiger Sollwert
526
                {
50 holgerb 527
                 if(!TEST_SCHUB) { if(sollwert) sollwert--; }  
528
                 PORTC |= ROT;
1 ingob 529
                }
530
        }
531
    else // I2C-Daten sind gültig
532
        {
533
        sollwert = I2C_RXBuffer;
50 holgerb 534
        PPM_Betrieb = 0;
1 ingob 535
        PORTC &= ~ROT;
50 holgerb 536
        ICP_INT_DISABLE;
1 ingob 537
        }
538
    if(sollwert > MAX_PWM) sollwert = MAX_PWM;
539
    return(sollwert);
540
}
541
 
542
 
543
 
544
//############################################################################
545
//Hauptprogramm
546
int main (void)
547
//############################################################################
548
{
549
    char altPhase = 0;
550
    int test = 0;
73 hbuss 551
    unsigned int Blink,TestschubTimer;
1 ingob 552
    unsigned int Blink2,MittelstromTimer,DrehzahlMessTimer,MotorGestopptTimer;
553
 
554
    DDRC  = 0x08;
555
    PORTC = 0x08;
73 hbuss 556
    DDRD  = 0x3A;
26 ingob 557
    PORTD = 0x00;
1 ingob 558
    DDRB  = 0x0E;
559
    PORTB = 0x31;
24 ingob 560
 
561
#if (MOTORADRESSE == 0)
562
    PORTB |= (ADR1 + ADR2);   // Pullups für Adresswahl
43 holgerb 563
    for(test=0;test<500;test++);
73 hbuss 564
    if(PINB & ADR1)
24 ingob 565
         {
566
           if (PINB & ADR2) MotorAdresse = 1;
567
            else MotorAdresse = 2;
568
         }
569
         else
570
         {
571
           if (PINB & ADR2) MotorAdresse = 3;
572
            else MotorAdresse = 4;
573
         }
73 hbuss 574
    HwVersion = 11;
24 ingob 575
#else
576
    MotorAdresse  = MOTORADRESSE;
73 hbuss 577
    HwVersion = 10;
24 ingob 578
#endif
73 hbuss 579
    if(PIND & 0x80) {HwVersion = 12; IntRef = 0xc0;}    
580
    DDRD  = 0xBA;
1 ingob 581
    UART_Init();
582
    Timer0_Init();
50 holgerb 583
    sei();//Globale Interrupts Einschalten
1 ingob 584
 
585
    // Am Blinken erkennt man die richtige Motoradresse
50 holgerb 586
/*
1 ingob 587
    for(test=0;test<5;test++)
588
        {
24 ingob 589
        if(test == MotorAdresse) PORTD |= GRUEN;
1 ingob 590
        Delay_ms(150);
591
        PORTD &= ~GRUEN;
592
        Delay_ms(250);
593
        }      
594
 
595
    Delay_ms(500);
50 holgerb 596
*/  
18 ingob 597
   // UART_Init();  // war doppelt
1 ingob 598
    PWM_Init();
599
 
600
    InitIC2_Slave(0x50);                           
601
    InitPPM();
602
 
603
    Blink             = SetDelay(101);    
604
    Blink2            = SetDelay(102);
605
    MinUpmPulse       = SetDelay(103);
606
    MittelstromTimer  = SetDelay(254);
607
    DrehzahlMessTimer = SetDelay(1005);
608
    TestschubTimer    = SetDelay(1006);
50 holgerb 609
    while(!CheckDelay(MinUpmPulse))
610
    {
611
     if(SollwertErmittlung()) break;
612
    }
73 hbuss 613
 
614
    GRN_ON;
1 ingob 615
    PWM = 0;
616
 
617
    SetPWM();
618
 
619
    SFIOR = 0x08;  // Analog Comperator ein
620
    ADMUX = 1;
621
 
622
    MinUpmPulse = SetDelay(10);
623
    DebugOut.Analog[1] = 1;
624
    PPM_Signal = 0;
625
 
50 holgerb 626
    if(!SollwertErmittlung()) MotorTon();
58 hbuss 627
//MotorTon();    
628
    PORTB = 0x31; // Pullups wieder einschalten
50 holgerb 629
 
1 ingob 630
    // zum Test der Hardware; Motor dreht mit konstanter Drehzahl ohne Regelung
631
    if(TEST_MANUELL)    Anwerfen(TEST_MANUELL);  // kommt von dort nicht wieder
632
 
633
    while (1)
634
        {
51 holgerb 635
//ShowSense();
636
 
1 ingob 637
        if(!TEST_SCHUB)   PWM = SollwertErmittlung();
638
        //I2C_TXBuffer = PWM; // Antwort über I2C-Bus
639
        if(MANUELL_PWM)   PWM = MANUELL_PWM;
640
 
641
        // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
642
        if(Phase != altPhase)   // es gab eine Kommutierung im Interrupt
643
            {
644
            MotorGestoppt = 0;
645
            ZeitFuerBerechnungen = 0;    // direkt nach einer Kommutierung ist Zeit 
58 hbuss 646
            MinUpmPulse = SetDelay(250);  // Timeout, falls ein Motor stehen bleibt
1 ingob 647
            altPhase = Phase;
648
            }
649
        // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
650
        if(!PWM)    // Sollwert == 0
651
            {
652
            MotorAnwerfen = 0;      // kein Startversuch
653
            ZeitFuerBerechnungen = 0;
654
            // nach 1,5 Sekunden den Motor als gestoppt betrachten 
18 ingob 655
            if(CheckDelay(MotorGestopptTimer))
1 ingob 656
                {
657
                DISABLE_SENSE_INT;
658
                MotorGestoppt = 1;  
659
                STEUER_OFF;
660
                }
661
            }
662
        else
663
            {
664
            if(MotorGestoppt) MotorAnwerfen = 1;        // Startversuch
665
            MotorGestopptTimer = SetDelay(1500);
666
            }
667
 
668
        if(MotorGestoppt && !TEST_SCHUB) PWM = 0;
669
        SetPWM();
670
        // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
671
        if(!ZeitFuerBerechnungen++)
672
            {
73 hbuss 673
            if(MotorGestoppt)
674
             {
675
              GRN_ON;
676
              FastADConvert();
677
             }
1 ingob 678
            if(SIO_DEBUG)
679
                {
680
                DebugAusgaben();  // welche Werte sollen angezeigt werden?
681
                if(!UebertragungAbgeschlossen)  SendUart();
682
                else DatenUebertragung();
683
                }
684
            // Berechnen des Mittleren Stroms zur (langsamen) Strombegrenzung
685
            if(CheckDelay(MittelstromTimer))  
686
                {
687
                MittelstromTimer = SetDelay(50); // alle 50ms
688
                if(Mittelstrom <  Strom) Mittelstrom++;// Mittelwert des Stroms bilden
689
                else if(Mittelstrom >  Strom) Mittelstrom--;
73 hbuss 690
                if(Strom > MAX_STROM) MaxPWM -= MaxPWM / 32;              
58 hbuss 691
                if((Mittelstrom > LIMIT_STROM))// Strom am Limit?
1 ingob 692
                    {
58 hbuss 693
                    if(MaxPWM) MaxPWM--;// dann die Maximale PWM herunterfahren
1 ingob 694
                    PORTC |= ROT;
695
                    }
696
                else
697
                    {
698
                    if(MaxPWM < MAX_PWM) MaxPWM++;
699
                    }
700
                }
701
 
702
            if(CheckDelay(DrehzahlMessTimer))   // Ist-Drehzahl bestimmen
703
                {
704
                DrehzahlMessTimer = SetDelay(10);
58 hbuss 705
                SIO_Drehzahl = CntKommutierungen;//(6 * CntKommutierungen) / (POLANZAHL / 2);
1 ingob 706
                CntKommutierungen = 0;
71 hbuss 707
               // if(PPM_Timeout == 0) // keine PPM-Signale
1 ingob 708
                ZeitZumAdWandeln = 1;
709
                }
73 hbuss 710
 
711
#if TEST_SCHUB == 1
58 hbuss 712
           {
1 ingob 713
            if(CheckDelay(TestschubTimer))  
714
                {
715
                TestschubTimer = SetDelay(1500);
716
                    switch(test)
717
                        {
718
                        case 0: PWM = 50; test++; break;
719
                        case 1: PWM = 130; test++; break;
720
                        case 2: PWM = 60;  test++; break;
721
                        case 3: PWM = 140; test++; break;
722
                        case 4: PWM = 150; test = 0; break;
723
                        default: test = 0;
724
                        }
725
                }
58 hbuss 726
            }  
73 hbuss 727
#endif
58 hbuss 728
          // Motor Stehen geblieben
1 ingob 729
            if((CheckDelay(MinUpmPulse) && SIO_Drehzahl == 0) || MotorAnwerfen)
730
                {
731
                MotorGestoppt = 1;    
732
                DISABLE_SENSE_INT;
733
                MinUpmPulse = SetDelay(100);        
734
                if(MotorAnwerfen)
73 hbuss 735
                  {
736
                   PORTC &= ~ROT;
737
                   Strom_max = 0;
738
                   MotorAnwerfen = 0;
739
                   if(Anwerfen(10))
740
                   {  
741
                    GRN_ON;
1 ingob 742
                    MotorGestoppt = 0;    
743
                    Phase--;
744
                    PWM = 1;
745
                    SetPWM();
746
                    SENSE_TOGGLE_INT;
747
                    ENABLE_SENSE_INT;
73 hbuss 748
                    MinUpmPulse = SetDelay(20);
1 ingob 749
                    while(!CheckDelay(MinUpmPulse)); // kurz Synchronisieren
43 holgerb 750
                    PWM = 15;
1 ingob 751
                    SetPWM();
43 holgerb 752
                    MinUpmPulse = SetDelay(300);
73 hbuss 753
                    while(!CheckDelay(MinUpmPulse)) // kurz Durchstarten
754
                    {
755
                      if(Strom > LIMIT_STROM/2)
756
                      {
757
                        STEUER_OFF; // Abschalten wegen Kurzschluss
758
                        RotBlink(10);
759
                        MotorAnwerfen = 1;
760
                      }  
761
                    }
18 ingob 762
                                    // Drehzahlmessung wieder aufsetzen
763
                    DrehzahlMessTimer = SetDelay(50);
1 ingob 764
                    altPhase = 7;
73 hbuss 765
                   }
766
                   else if(SollwertErmittlung()) MotorAnwerfen = 1;
767
                  }
1 ingob 768
                }
769
            } // ZeitFuerBerechnungen
770
        } // while(1) - Hauptschleife
771
}
772