Subversion Repositories BL-Ctrl

Rev

Rev 89 | 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
        }
89 hbuss 113
//GRN_ON;
1 ingob 114
}
115
 
73 hbuss 116
void DebugAusgaben(void)
117
{
118
    DebugOut.Analog[0] = Strom;
119
    DebugOut.Analog[1] = Mittelstrom;
120
    DebugOut.Analog[2] = SIO_Drehzahl;
121
    DebugOut.Analog[3] = PPM_Signal;
122
    DebugOut.Analog[4] = OCR2;
123
//    DebugOut.Analog[5] = PWM;
124
}
125
 
1 ingob 126
//############################################################################
127
//
128
void PWM_Init(void)
129
//############################################################################
130
{
131
    PWM_OFF;
50 holgerb 132
    TCCR1B = (1 << CS10) | (0 << CS11) | (0 << CS12) | (0 << WGM12) |
1 ingob 133
             (0 << WGM13) | (0<< ICES1) | (0 << ICNC1);
50 holgerb 134
/*    TCCR1B = (1 << CS10) | (0 << CS11) | (0 << CS12) | (1 << WGM12) |
135
             (0 << WGM13) | (0<< ICES1) | (0 << ICNC1);
136
*/
1 ingob 137
}
138
 
139
//############################################################################
140
//
141
void Wait(unsigned char dauer)
142
//############################################################################
143
{
144
    dauer = (unsigned char)TCNT0 + dauer;
145
    while((TCNT0 - dauer) & 0x80);
146
}
147
 
73 hbuss 148
void RotBlink(unsigned char anz)
149
{
150
sei(); // Interrupts ein
151
 while(anz--)
152
  {
153
   PORTC |= ROT;
154
   Delay_ms(300);    
155
   PORTC &= ~ROT;
156
   Delay_ms(300);    
157
  }
158
   Delay_ms(1000);    
159
}
160
 
1 ingob 161
//############################################################################
162
//
73 hbuss 163
char Anwerfen(unsigned char pwm)
1 ingob 164
//############################################################################
165
{
166
    unsigned long timer = 300,i;
167
    DISABLE_SENSE_INT;
168
    PWM = 5;
169
    SetPWM();
170
    Manuell();
73 hbuss 171
//    Delay_ms(200);
172
                    MinUpmPulse = SetDelay(300);
173
                    while(!CheckDelay(MinUpmPulse))
174
                    {
175
                     FastADConvert();
176
                      if(Strom > 120)
177
                      {
178
                        STEUER_OFF; // Abschalten wegen Kurzschluss
179
                        RotBlink(10);
180
                        return(0);
181
                      }  
182
                    }
1 ingob 183
    PWM = pwm;
184
    while(1)
185
        {
186
        for(i=0;i<timer; i++)
187
            {
188
            if(!UebertragungAbgeschlossen)  SendUart();
189
            else DatenUebertragung();
190
            Wait(100);  // warten
191
            }
73 hbuss 192
        DebugAusgaben();
193
        FastADConvert();
194
        if(Strom > 60)
195
          {
196
            STEUER_OFF; // Abschalten wegen Kurzschluss
197
            RotBlink(10);
198
            return(0);
199
          }  
200
 
1 ingob 201
        timer-= timer/15+1;
73 hbuss 202
        if(timer < 25) { if(TEST_MANUELL) timer = 25; else return(1); }
1 ingob 203
        Manuell();
204
        Phase++;
205
        Phase %= 6;
206
        AdConvert();
207
        PWM = pwm;
208
        SetPWM();
209
        if(SENSE)
210
            {
211
            PORTD ^= GRUEN;
212
            }
213
        }
214
}
215
 
50 holgerb 216
/*
217
#define SENSE_A ADMUX = 0;
218
#define SENSE_B ADMUX = 1;
219
#define SENSE_C ADMUX = 2;
220
 
221
#define ClrSENSE            ACSR |= 0x10
222
#define SENSE               ((ACSR & 0x10))
223
#define SENSE_L             (!(ACSR & 0x20))
224
#define SENSE_H             ((ACSR & 0x20))
225
*/
226
 
227
 
58 hbuss 228
#define TEST_STROMGRENZE 120
50 holgerb 229
unsigned char DelayM(unsigned int timer)
230
{
231
 while(timer--)
232
  {
233
   FastADConvert();
58 hbuss 234
   if(Strom > (TEST_STROMGRENZE + RuheStrom))
50 holgerb 235
       {
51 holgerb 236
        FETS_OFF;
50 holgerb 237
        return(1);
238
       }
239
  }
240
 return(0);  
241
}
242
 
243
unsigned char Delay(unsigned int timer)
244
{
245
 while(timer--)
246
  {
51 holgerb 247
//   if(SENSE_H) { PORTC |= ROT; } else { PORTC &= ~ROT;}
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
{
89 hbuss 269
    unsigned char ADR_TAB[9] = {0,0,2,1,3,4,5,6,7};
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
*/
481
 Delay_ms(300 * (4-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;
89 hbuss 560
 
561
#define ADRESSOFFSET 0
562
 
24 ingob 563
#if (MOTORADRESSE == 0)
564
    PORTB |= (ADR1 + ADR2);   // Pullups für Adresswahl
43 holgerb 565
    for(test=0;test<500;test++);
73 hbuss 566
    if(PINB & ADR1)
24 ingob 567
         {
89 hbuss 568
           if (PINB & ADR2) MotorAdresse = 1 + ADRESSOFFSET;
569
            else MotorAdresse = 2 + ADRESSOFFSET;
24 ingob 570
         }
571
         else
572
         {
89 hbuss 573
           if (PINB & ADR2) MotorAdresse = 3 + ADRESSOFFSET;
574
            else MotorAdresse = 4 + ADRESSOFFSET;
24 ingob 575
         }
73 hbuss 576
    HwVersion = 11;
24 ingob 577
#else
578
    MotorAdresse  = MOTORADRESSE;
73 hbuss 579
    HwVersion = 10;
24 ingob 580
#endif
73 hbuss 581
    if(PIND & 0x80) {HwVersion = 12; IntRef = 0xc0;}    
582
    DDRD  = 0xBA;
1 ingob 583
    UART_Init();
584
    Timer0_Init();
50 holgerb 585
    sei();//Globale Interrupts Einschalten
1 ingob 586
 
587
    // Am Blinken erkennt man die richtige Motoradresse
50 holgerb 588
/*
1 ingob 589
    for(test=0;test<5;test++)
590
        {
24 ingob 591
        if(test == MotorAdresse) PORTD |= GRUEN;
1 ingob 592
        Delay_ms(150);
593
        PORTD &= ~GRUEN;
594
        Delay_ms(250);
595
        }      
596
 
597
    Delay_ms(500);
50 holgerb 598
*/  
18 ingob 599
   // UART_Init();  // war doppelt
1 ingob 600
    PWM_Init();
601
 
602
    InitIC2_Slave(0x50);                           
603
    InitPPM();
604
 
605
    Blink             = SetDelay(101);    
606
    Blink2            = SetDelay(102);
607
    MinUpmPulse       = SetDelay(103);
608
    MittelstromTimer  = SetDelay(254);
609
    DrehzahlMessTimer = SetDelay(1005);
610
    TestschubTimer    = SetDelay(1006);
50 holgerb 611
    while(!CheckDelay(MinUpmPulse))
612
    {
613
     if(SollwertErmittlung()) break;
614
    }
73 hbuss 615
 
616
    GRN_ON;
1 ingob 617
    PWM = 0;
618
 
619
    SetPWM();
620
 
621
    SFIOR = 0x08;  // Analog Comperator ein
622
    ADMUX = 1;
623
 
624
    MinUpmPulse = SetDelay(10);
625
    DebugOut.Analog[1] = 1;
626
    PPM_Signal = 0;
627
 
50 holgerb 628
    if(!SollwertErmittlung()) MotorTon();
58 hbuss 629
//MotorTon();    
630
    PORTB = 0x31; // Pullups wieder einschalten
50 holgerb 631
 
1 ingob 632
    // zum Test der Hardware; Motor dreht mit konstanter Drehzahl ohne Regelung
633
    if(TEST_MANUELL)    Anwerfen(TEST_MANUELL);  // kommt von dort nicht wieder
634
 
635
    while (1)
636
        {
51 holgerb 637
//ShowSense();
638
 
1 ingob 639
        if(!TEST_SCHUB)   PWM = SollwertErmittlung();
640
        //I2C_TXBuffer = PWM; // Antwort über I2C-Bus
641
        if(MANUELL_PWM)   PWM = MANUELL_PWM;
642
 
643
        // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
644
        if(Phase != altPhase)   // es gab eine Kommutierung im Interrupt
645
            {
646
            MotorGestoppt = 0;
647
            ZeitFuerBerechnungen = 0;    // direkt nach einer Kommutierung ist Zeit 
58 hbuss 648
            MinUpmPulse = SetDelay(250);  // Timeout, falls ein Motor stehen bleibt
1 ingob 649
            altPhase = Phase;
650
            }
651
        // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
652
        if(!PWM)    // Sollwert == 0
653
            {
654
            MotorAnwerfen = 0;      // kein Startversuch
655
            ZeitFuerBerechnungen = 0;
656
            // nach 1,5 Sekunden den Motor als gestoppt betrachten 
18 ingob 657
            if(CheckDelay(MotorGestopptTimer))
1 ingob 658
                {
659
                DISABLE_SENSE_INT;
660
                MotorGestoppt = 1;  
661
                STEUER_OFF;
662
                }
663
            }
664
        else
665
            {
666
            if(MotorGestoppt) MotorAnwerfen = 1;        // Startversuch
667
            MotorGestopptTimer = SetDelay(1500);
668
            }
669
 
670
        if(MotorGestoppt && !TEST_SCHUB) PWM = 0;
671
        SetPWM();
672
        // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
673
        if(!ZeitFuerBerechnungen++)
674
            {
73 hbuss 675
            if(MotorGestoppt)
676
             {
89 hbuss 677
//              GRN_ON; 
73 hbuss 678
              FastADConvert();
679
             }
1 ingob 680
            if(SIO_DEBUG)
681
                {
682
                DebugAusgaben();  // welche Werte sollen angezeigt werden?
683
                if(!UebertragungAbgeschlossen)  SendUart();
684
                else DatenUebertragung();
685
                }
686
            // Berechnen des Mittleren Stroms zur (langsamen) Strombegrenzung
687
            if(CheckDelay(MittelstromTimer))  
688
                {
689
                MittelstromTimer = SetDelay(50); // alle 50ms
690
                if(Mittelstrom <  Strom) Mittelstrom++;// Mittelwert des Stroms bilden
691
                else if(Mittelstrom >  Strom) Mittelstrom--;
73 hbuss 692
                if(Strom > MAX_STROM) MaxPWM -= MaxPWM / 32;              
58 hbuss 693
                if((Mittelstrom > LIMIT_STROM))// Strom am Limit?
1 ingob 694
                    {
58 hbuss 695
                    if(MaxPWM) MaxPWM--;// dann die Maximale PWM herunterfahren
1 ingob 696
                    PORTC |= ROT;
697
                    }
698
                else
699
                    {
700
                    if(MaxPWM < MAX_PWM) MaxPWM++;
701
                    }
702
                }
703
 
704
            if(CheckDelay(DrehzahlMessTimer))   // Ist-Drehzahl bestimmen
705
                {
706
                DrehzahlMessTimer = SetDelay(10);
58 hbuss 707
                SIO_Drehzahl = CntKommutierungen;//(6 * CntKommutierungen) / (POLANZAHL / 2);
1 ingob 708
                CntKommutierungen = 0;
71 hbuss 709
               // if(PPM_Timeout == 0) // keine PPM-Signale
1 ingob 710
                ZeitZumAdWandeln = 1;
711
                }
73 hbuss 712
 
713
#if TEST_SCHUB == 1
58 hbuss 714
           {
1 ingob 715
            if(CheckDelay(TestschubTimer))  
716
                {
717
                TestschubTimer = SetDelay(1500);
718
                    switch(test)
719
                        {
720
                        case 0: PWM = 50; test++; break;
721
                        case 1: PWM = 130; test++; break;
722
                        case 2: PWM = 60;  test++; break;
723
                        case 3: PWM = 140; test++; break;
724
                        case 4: PWM = 150; test = 0; break;
725
                        default: test = 0;
726
                        }
727
                }
58 hbuss 728
            }  
73 hbuss 729
#endif
58 hbuss 730
          // Motor Stehen geblieben
1 ingob 731
            if((CheckDelay(MinUpmPulse) && SIO_Drehzahl == 0) || MotorAnwerfen)
732
                {
733
                MotorGestoppt = 1;    
734
                DISABLE_SENSE_INT;
735
                MinUpmPulse = SetDelay(100);        
736
                if(MotorAnwerfen)
73 hbuss 737
                  {
738
                   PORTC &= ~ROT;
739
                   Strom_max = 0;
740
                   MotorAnwerfen = 0;
741
                   if(Anwerfen(10))
742
                   {  
89 hbuss 743
//                    GRN_ON;
1 ingob 744
                    MotorGestoppt = 0;    
745
                    Phase--;
746
                    PWM = 1;
747
                    SetPWM();
748
                    SENSE_TOGGLE_INT;
749
                    ENABLE_SENSE_INT;
73 hbuss 750
                    MinUpmPulse = SetDelay(20);
1 ingob 751
                    while(!CheckDelay(MinUpmPulse)); // kurz Synchronisieren
43 holgerb 752
                    PWM = 15;
1 ingob 753
                    SetPWM();
43 holgerb 754
                    MinUpmPulse = SetDelay(300);
73 hbuss 755
                    while(!CheckDelay(MinUpmPulse)) // kurz Durchstarten
756
                    {
757
                      if(Strom > LIMIT_STROM/2)
758
                      {
759
                        STEUER_OFF; // Abschalten wegen Kurzschluss
760
                        RotBlink(10);
761
                        MotorAnwerfen = 1;
762
                      }  
763
                    }
18 ingob 764
                                    // Drehzahlmessung wieder aufsetzen
765
                    DrehzahlMessTimer = SetDelay(50);
1 ingob 766
                    altPhase = 7;
73 hbuss 767
                   }
768
                   else if(SollwertErmittlung()) MotorAnwerfen = 1;
769
                  }
1 ingob 770
                }
771
            } // ZeitFuerBerechnungen
772
        } // while(1) - Hauptschleife
773
}
774