Subversion Repositories FlightCtrl

Rev

Go to most recent revision | Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
1 ingob 1
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2
// + Copyright (c) 04.2007 Holger Buss
3
// + only for non-profit use
4
// + www.MikroKopter.com
5
// + see the File "License.txt" for further Informations
6
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
7
 
8
#include "main.h"
9
#include "uart.h"
10
 
11
unsigned char DebugGetAnforderung = 0,DebugDisplayAnforderung = 0,DebugDataAnforderung = 0,GetVersionAnforderung = 0;
12
unsigned volatile char SioTmp = 0;
13
unsigned volatile char SendeBuffer[MAX_SENDE_BUFF];
14
unsigned volatile char RxdBuffer[MAX_EMPFANGS_BUFF];
15
unsigned volatile char NMEABuffer[MAX_EMPFANGS_BUFF];
16
unsigned volatile char NeuerDatensatzEmpfangen = 0;
17
unsigned volatile char NeueKoordinateEmpfangen = 0;
18
unsigned volatile char UebertragungAbgeschlossen = 1;
19
unsigned volatile char CntCrcError = 0;
20
unsigned volatile char AnzahlEmpfangsBytes = 0;
21
unsigned volatile char PC_DebugTimeout = 0;
499 hbuss 22
unsigned char RemotePollDisplayLine = 0;
395 hbuss 23
unsigned char NurKanalAnforderung = 0;
499 hbuss 24
unsigned char DebugTextAnforderung = 255;
1 ingob 25
unsigned char PcZugriff = 100;
26
unsigned char MotorTest[4] = {0,0,0,0};
595 hbuss 27
unsigned char DubWiseKeys[4] = {0,0,0,0};
1 ingob 28
unsigned char MeineSlaveAdresse;
595 hbuss 29
unsigned char ConfirmFrame;
1 ingob 30
struct str_DebugOut    DebugOut;
595 hbuss 31
struct str_ExternControl  ExternControl;
1 ingob 32
struct str_VersionInfo VersionInfo;
693 hbuss 33
struct str_WinkelOut WinkelOut;
1 ingob 34
 
693 hbuss 35
int Debug_Timer,Kompass_Timer;
36
 
499 hbuss 37
const unsigned char ANALOG_TEXT[32][16] =
38
{
39
   //1234567890123456 
40
    "IntegralNick    ", //0
41
    "IntegralRoll    ",
42
    "AccNick         ",
43
    "AccRoll         ",
44
    "GyroGier        ",
45
    "HoehenWert      ", //5
46
    "AccZ            ",
47
    "Gas             ",
48
    "KompassValue    ",
513 hbuss 49
    "Spannung        ",
50
    "Empfang         ", //10
693 hbuss 51
    "Ersatzkompass   ",
499 hbuss 52
    "Motor_Vorne     ",
53
    "Motor_Hinten    ",
54
    "Motor_Links     ",
55
    "Motor_Rechts    ", //15
929 hbuss 56
    "                ",
744 hbuss 57
    "Distance        ",
58
    "OsdBar          ",
819 hbuss 59
    "MK3Mag CalState ",
854 hbuss 60
    "Servo           ", //20
61
    "Nick            ",
62
    "Roll            ",
720 ingob 63
    "                ",
64
    "                ",
65
    "                ", //25
66
    "                ",
992 hbuss 67
    "Kalman_MaxDrift ",
720 ingob 68
    "                ",
992 hbuss 69
    "Kalman K        ",
720 ingob 70
    "GPS_Nick        ", //30
71
    "GPS_Roll        "
499 hbuss 72
};
73
 
74
 
75
 
1 ingob 76
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
77
//++ Sende-Part der Datenübertragung
78
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
79
SIGNAL(INT_VEC_TX)
80
{
81
 static unsigned int ptr = 0;
82
 unsigned char tmp_tx;
83
 if(!UebertragungAbgeschlossen)  
84
  {
85
   ptr++;                    // die [0] wurde schon gesendet
86
   tmp_tx = SendeBuffer[ptr];  
87
   if((tmp_tx == '\r') || (ptr == MAX_SENDE_BUFF))
88
    {
89
     ptr = 0;
90
     UebertragungAbgeschlossen = 1;
91
    }
92
   UDR = tmp_tx;
93
  }
94
  else ptr = 0;
95
}
96
 
97
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
98
//++ Empfangs-Part der Datenübertragung, incl. CRC-Auswertung
99
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
100
SIGNAL(INT_VEC_RX)
101
{
102
 static unsigned int crc;
103
 static unsigned char crc1,crc2,buf_ptr;
104
 static unsigned char UartState = 0;
105
 unsigned char CrcOkay = 0;
106
 
107
 SioTmp = UDR;
108
 if(buf_ptr >= MAX_EMPFANGS_BUFF)    UartState = 0;
109
 if(SioTmp == '\r' && UartState == 2)
110
  {
111
   UartState = 0;
112
   crc -= RxdBuffer[buf_ptr-2];
113
   crc -= RxdBuffer[buf_ptr-1];
114
   crc %= 4096;
115
   crc1 = '=' + crc / 64;
116
   crc2 = '=' + crc % 64;
117
   CrcOkay = 0;
118
   if((crc1 == RxdBuffer[buf_ptr-2]) && (crc2 == RxdBuffer[buf_ptr-1])) CrcOkay = 1; else { CrcOkay = 0; CntCrcError++;};
119
   if(!NeuerDatensatzEmpfangen && CrcOkay) // Datensatz schon verarbeitet
120
    {
121
     NeuerDatensatzEmpfangen = 1;
122
         AnzahlEmpfangsBytes = buf_ptr;
123
     RxdBuffer[buf_ptr] = '\r';
173 holgerb 124
         if(RxdBuffer[2] == 'R') wdt_enable(WDTO_250MS); // Reset-Commando
1 ingob 125
        }                                
126
  }
127
  else
128
  switch(UartState)
129
  {
130
   case 0:
131
          if(SioTmp == '#' && !NeuerDatensatzEmpfangen) UartState = 1;  // Startzeichen und Daten schon verarbeitet
132
                  buf_ptr = 0;
133
                  RxdBuffer[buf_ptr++] = SioTmp;
134
                  crc = SioTmp;
135
          break;
136
   case 1: // Adresse auswerten
137
                  UartState++;
138
                  RxdBuffer[buf_ptr++] = SioTmp;
139
                  crc += SioTmp;
140
                  break;
141
   case 2: //  Eingangsdaten sammeln
142
                  RxdBuffer[buf_ptr] = SioTmp;
143
                  if(buf_ptr < MAX_EMPFANGS_BUFF) buf_ptr++;
144
                  else UartState = 0;
145
                  crc += SioTmp;
146
                  break;
147
   default:
148
          UartState = 0;
149
          break;
150
  }
151
}
152
 
153
 
154
// --------------------------------------------------------------------------
155
void AddCRC(unsigned int wieviele)
156
{
157
 unsigned int tmpCRC = 0,i;
158
 for(i = 0; i < wieviele;i++)
159
  {
160
   tmpCRC += SendeBuffer[i];
161
  }
162
   tmpCRC %= 4096;
163
   SendeBuffer[i++] = '=' + tmpCRC / 64;
164
   SendeBuffer[i++] = '=' + tmpCRC % 64;
165
   SendeBuffer[i++] = '\r';
166
  UebertragungAbgeschlossen = 0;
167
  UDR = SendeBuffer[0];
168
}
169
 
170
 
171
 
172
// --------------------------------------------------------------------------
173
void SendOutData(unsigned char cmd,unsigned char modul, unsigned char *snd, unsigned char len)
174
{
175
 unsigned int pt = 0;
176
 unsigned char a,b,c;
177
 unsigned char ptr = 0;
178
 
179
 SendeBuffer[pt++] = '#';               // Startzeichen
180
 SendeBuffer[pt++] = modul;             // Adresse (a=0; b=1,...)
181
 SendeBuffer[pt++] = cmd;                       // Commando
182
 
183
 while(len)
184
  {
185
   if(len) { a = snd[ptr++]; len--;} else a = 0;
186
   if(len) { b = snd[ptr++]; len--;} else b = 0;
187
   if(len) { c = snd[ptr++]; len--;} else c = 0;
188
   SendeBuffer[pt++] = '=' + (a >> 2);
189
   SendeBuffer[pt++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4));
190
   SendeBuffer[pt++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6));
191
   SendeBuffer[pt++] = '=' + ( c & 0x3f);
192
  }
193
 AddCRC(pt);
194
}
195
 
196
 
197
// --------------------------------------------------------------------------
198
void Decode64(unsigned char *ptrOut, unsigned char len, unsigned char ptrIn,unsigned char max)  // Wohin mit den Daten; Wie lang; Wo im RxdBuffer
199
{
200
 unsigned char a,b,c,d;
201
 unsigned char ptr = 0;
202
 unsigned char x,y,z;
203
 while(len)
204
  {
205
   a = RxdBuffer[ptrIn++] - '=';
206
   b = RxdBuffer[ptrIn++] - '=';
207
   c = RxdBuffer[ptrIn++] - '=';
208
   d = RxdBuffer[ptrIn++] - '=';
209
   if(ptrIn > max - 2) break;     // nicht mehr Daten verarbeiten, als empfangen wurden
210
 
211
   x = (a << 2) | (b >> 4);
212
   y = ((b & 0x0f) << 4) | (c >> 2);
213
   z = ((c & 0x03) << 6) | d;
214
 
215
   if(len--) ptrOut[ptr++] = x; else break;
216
   if(len--) ptrOut[ptr++] = y; else break;
217
   if(len--) ptrOut[ptr++] = z; else break;
218
  }
219
 
220
}
221
 
222
// --------------------------------------------------------------------------
223
void BearbeiteRxDaten(void)
224
{
225
 if(!NeuerDatensatzEmpfangen) return;
226
 
693 hbuss 227
 unsigned int tmp_int_arr1[1];
304 ingob 228
// unsigned int tmp_int_arr2[2];
229
// unsigned int tmp_int_arr3[3];
1 ingob 230
 unsigned char tmp_char_arr2[2];
304 ingob 231
// unsigned char tmp_char_arr3[3];
232
// unsigned char tmp_char_arr4[4];
1 ingob 233
 //if(!MotorenEin) 
234
  switch(RxdBuffer[2])
235
  {
693 hbuss 236
   case 'K':// Kompasswert
237
            Decode64((unsigned char *) &tmp_int_arr1[0],sizeof(tmp_int_arr1),3,AnzahlEmpfangsBytes);
238
            KompassValue = tmp_int_arr1[0];
239
            KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180;
240
                        break;
499 hbuss 241
   case 'a':// Texte der Analogwerte
242
            Decode64((unsigned char *) &tmp_char_arr2[0],sizeof(tmp_char_arr2),3,AnzahlEmpfangsBytes);
243
            DebugTextAnforderung = tmp_char_arr2[0];
693 hbuss 244
            PcZugriff = 255;
499 hbuss 245
                        break;
595 hbuss 246
   case 'b':
247
                        Decode64((unsigned char *) &ExternControl,sizeof(ExternControl),3,AnzahlEmpfangsBytes);
248
                        RemoteTasten |= ExternControl.RemoteTasten;
249
            ConfirmFrame = ExternControl.Frame;
918 hbuss 250
            PcZugriff = 255;
595 hbuss 251
            break;
252
   case 'c':
253
                        Decode64((unsigned char *) &ExternControl,sizeof(ExternControl),3,AnzahlEmpfangsBytes);
254
                        RemoteTasten |= ExternControl.RemoteTasten;
255
            ConfirmFrame = ExternControl.Frame;
1 ingob 256
            DebugDataAnforderung = 1;
693 hbuss 257
            PcZugriff = 255;
1 ingob 258
            break;
259
   case 'h':// x-1 Displayzeilen
260
            Decode64((unsigned char *) &tmp_char_arr2[0],sizeof(tmp_char_arr2),3,AnzahlEmpfangsBytes);
261
            RemoteTasten |= tmp_char_arr2[0];
395 hbuss 262
                        if(tmp_char_arr2[1] == 255) NurKanalAnforderung = 1; else NurKanalAnforderung = 0; // keine Displaydaten
1 ingob 263
                        DebugDisplayAnforderung = 1;
264
                        break;
265
   case 't':// Motortest
266
            Decode64((unsigned char *) &MotorTest[0],sizeof(MotorTest),3,AnzahlEmpfangsBytes);
693 hbuss 267
            PcZugriff = 255;
1 ingob 268
                        break;
492 hbuss 269
   case 'k':// Keys von DubWise
270
            Decode64((unsigned char *) &DubWiseKeys[0],sizeof(DubWiseKeys),3,AnzahlEmpfangsBytes);
595 hbuss 271
                        ConfirmFrame = DubWiseKeys[3];
693 hbuss 272
            PcZugriff = 255;
492 hbuss 273
                        break;
1 ingob 274
   case 'v': // Version-Anforderung     und Ausbaustufe
275
            GetVersionAnforderung = 1;
276
            break;                                                               
277
   case 'g':// "Get"-Anforderung für Debug-Daten 
278
            // Bei Get werden die vom PC einstellbaren Werte vom PC zurückgelesen
279
            DebugGetAnforderung = 1;
280
            break;
281
   case 'q':// "Get"-Anforderung für Settings
282
            // Bei Get werden die vom PC einstellbaren Werte vom PC zurückgelesen
283
            Decode64((unsigned char *) &tmp_char_arr2[0],sizeof(tmp_char_arr2),3,AnzahlEmpfangsBytes);
846 hbuss 284
            while(!UebertragungAbgeschlossen);
1 ingob 285
            if(tmp_char_arr2[0] != 0xff)
286
             {
287
                          if(tmp_char_arr2[0] > 5) tmp_char_arr2[0] = 5;
288
                  ReadParameterSet(tmp_char_arr2[0], (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);                   
289
                  SendOutData('L' + tmp_char_arr2[0] -1,MeineSlaveAdresse,(unsigned char *) &EE_Parameter.Kanalbelegung[0],STRUCT_PARAM_LAENGE);
290
             }
291
             else
292
                  SendOutData('L' + GetActiveParamSetNumber()-1,MeineSlaveAdresse,(unsigned char *) &EE_Parameter.Kanalbelegung[0],STRUCT_PARAM_LAENGE);
293
 
294
            break;
295
 
296
   case 'l':
297
   case 'm':
298
   case 'n':
299
   case 'o':
300
   case 'p': // Parametersatz speichern
301
            Decode64((unsigned char *) &EE_Parameter.Kanalbelegung[0],STRUCT_PARAM_LAENGE,3,AnzahlEmpfangsBytes);
302
                        WriteParameterSet(RxdBuffer[2] - 'l' + 1, (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
303
            eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], RxdBuffer[2] - 'l' + 1);  // aktiven Datensatz merken
395 hbuss 304
            Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L;
305
            Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L;
1 ingob 306
            Piep(GetActiveParamSetNumber());
307
         break;
308
 
309
 
310
  }
311
// DebugOut.AnzahlZyklen =  Debug_Timer_Intervall;
312
 NeuerDatensatzEmpfangen = 0;
313
}
314
 
315
//############################################################################
316
//Routine für die Serielle Ausgabe
317
int uart_putchar (char c)
318
//############################################################################
319
{
320
        if (c == '\n')
321
                uart_putchar('\r');
322
        //Warten solange bis Zeichen gesendet wurde
323
        loop_until_bit_is_set(USR, UDRE);
324
        //Ausgabe des Zeichens
325
        UDR = c;
326
 
327
        return (0);
328
}
329
 
330
// --------------------------------------------------------------------------
331
void WriteProgramData(unsigned int pos, unsigned char wert)
332
{
333
  //if (ProgramLocation == IN_RAM) Buffer[pos] = wert;
334
  // else eeprom_write_byte(&EE_Buffer[pos], wert);
335
  // Buffer[pos] = wert;
336
}
337
 
338
//############################################################################
339
//INstallation der Seriellen Schnittstelle
340
void UART_Init (void)
341
//############################################################################
342
{
343
        //Enable TXEN im Register UCR TX-Data Enable & RX Enable
344
 
345
        UCR=(1 << TXEN) | (1 << RXEN);
346
    // UART Double Speed (U2X)
347
        USR   |= (1<<U2X);          
348
        // RX-Interrupt Freigabe
349
        UCSRB |= (1<<RXCIE);          
350
        // TX-Interrupt Freigabe
351
        UCSRB |= (1<<TXCIE);          
352
 
353
        //Teiler wird gesetzt 
354
        UBRR=(SYSCLK / (BAUD_RATE * 8L) - 1);
355
        //UBRR = 33;
356
        //öffnet einen Kanal für printf (STDOUT)
357
        //fdevopen (uart_putchar, 0);
358
        //sbi(PORTD,4);
359
  Debug_Timer = SetDelay(200);  
693 hbuss 360
  Kompass_Timer = SetDelay(220);  
1 ingob 361
}
362
 
363
//---------------------------------------------------------------------------------------------
364
void DatenUebertragung(void)  
365
{
366
 if(!UebertragungAbgeschlossen) return;
367
 
368
   if(DebugGetAnforderung && UebertragungAbgeschlossen)               // Bei Get werden die vom PC einstellbaren Werte vom PC zurückgelesen
369
   {
595 hbuss 370
      SendOutData('G',MeineSlaveAdresse,(unsigned char *) &ExternControl,sizeof(ExternControl));
1 ingob 371
          DebugGetAnforderung = 0;
372
   }
373
 
693 hbuss 374
    if((CheckDelay(Kompass_Timer)) && UebertragungAbgeschlossen)        
375
         {
846 hbuss 376
                  WinkelOut.Winkel[0] = (int) (IntegralNick / 108);  // etwa in 0,1 Grad
377
                  WinkelOut.Winkel[1] = (int) (IntegralRoll / 108);  // etwa in 0,1 Grad
717 hbuss 378
                  WinkelOut.UserParameter[0] = Parameter_UserParam1;
379
                  WinkelOut.UserParameter[1] = Parameter_UserParam2;
693 hbuss 380
          SendOutData('w',MeineSlaveAdresse,(unsigned char *) &WinkelOut,sizeof(WinkelOut));
855 hbuss 381
          if(WinkelOut.CalcState > 4)  WinkelOut.CalcState = 6; // wird dann in SPI auf Null gesetzt
693 hbuss 382
          Kompass_Timer = SetDelay(99);  
383
         }
384
 
1 ingob 385
    if((CheckDelay(Debug_Timer) || DebugDataAnforderung) && UebertragungAbgeschlossen)  
386
         {
387
          SendOutData('D',MeineSlaveAdresse,(unsigned char *) &DebugOut,sizeof(DebugOut));
388
          DebugDataAnforderung = 0;
389
          Debug_Timer = SetDelay(MIN_DEBUG_INTERVALL);  
390
         }
499 hbuss 391
    if(DebugTextAnforderung != 255) // Texte für die Analogdaten
392
     {
393
      SendOutData('A',DebugTextAnforderung + '0',(unsigned char *) ANALOG_TEXT[DebugTextAnforderung],16);
394
      DebugTextAnforderung = 255;
395
         }
595 hbuss 396
     if(ConfirmFrame && UebertragungAbgeschlossen)   // Datensatz ohne CRC bestätigen 
397
         {
398
      SendeBuffer[0] = '#';
399
      SendeBuffer[1] = ConfirmFrame;
400
      SendeBuffer[2] = '\r';
401
      UebertragungAbgeschlossen = 0;
402
      ConfirmFrame = 0;
403
      UDR = SendeBuffer[0];
404
     }
1 ingob 405
     if(DebugDisplayAnforderung && UebertragungAbgeschlossen)
406
         {
407
      Menu();
408
          DebugDisplayAnforderung = 0;
499 hbuss 409
      if(++RemotePollDisplayLine == 4 || NurKanalAnforderung)
173 holgerb 410
      {
304 ingob 411
       SendOutData('4',0,(unsigned char *)&PPM_in,sizeof(PPM_in));   // DisplayZeile übertragen
499 hbuss 412
       RemotePollDisplayLine = -1;
173 holgerb 413
      }
499 hbuss 414
      else  SendOutData('0' + RemotePollDisplayLine,0,(unsigned char *)&DisplayBuff[20 * RemotePollDisplayLine],20);   // DisplayZeile übertragen
1 ingob 415
         }
416
    if(GetVersionAnforderung && UebertragungAbgeschlossen)
417
     {
418
      SendOutData('V',MeineSlaveAdresse,(unsigned char *) &VersionInfo,sizeof(VersionInfo));
419
          GetVersionAnforderung = 0;
420
     }
421
 
422
}
423