Subversion Repositories FlightCtrl

Rev

Rev 711 | Rev 735 | Go to most recent revision | Details | Compare with Previous | 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
 
687 killagreg 8
#include <avr/io.h>
9
#include <avr/interrupt.h>
10
#include <avr/wdt.h>
11
 
12
#include "eeprom.h"
1 ingob 13
#include "main.h"
685 killagreg 14
#include "menu.h"
15
#include "timer0.h"
1 ingob 16
#include "uart.h"
685 killagreg 17
#include "fc.h"
18
#include "_Settings.h"
19
#include "rc.h"
1 ingob 20
 
685 killagreg 21
 
1 ingob 22
unsigned char DebugGetAnforderung = 0,DebugDisplayAnforderung = 0,DebugDataAnforderung = 0,GetVersionAnforderung = 0;
23
unsigned volatile char SioTmp = 0;
683 killagreg 24
unsigned volatile char txd_buffer[TXD_BUFFER_LEN];
25
unsigned volatile char rxd_buffer[RXD_BUFFER_LEN];
26
 
1 ingob 27
unsigned volatile char NeuerDatensatzEmpfangen = 0;
28
unsigned volatile char NeueKoordinateEmpfangen = 0;
29
unsigned volatile char UebertragungAbgeschlossen = 1;
30
unsigned volatile char CntCrcError = 0;
31
unsigned volatile char AnzahlEmpfangsBytes = 0;
32
unsigned volatile char PC_DebugTimeout = 0;
499 hbuss 33
unsigned char RemotePollDisplayLine = 0;
395 hbuss 34
unsigned char NurKanalAnforderung = 0;
499 hbuss 35
unsigned char DebugTextAnforderung = 255;
706 killagreg 36
unsigned char PcAccess = 100;
1 ingob 37
unsigned char MotorTest[4] = {0,0,0,0};
683 killagreg 38
unsigned char DubWiseKeys[4] = {0,0,0,0};
1 ingob 39
unsigned char MeineSlaveAdresse;
595 hbuss 40
unsigned char ConfirmFrame;
1 ingob 41
struct str_DebugOut    DebugOut;
595 hbuss 42
struct str_ExternControl  ExternControl;
1 ingob 43
struct str_VersionInfo VersionInfo;
44
int Debug_Timer;
45
 
499 hbuss 46
const unsigned char ANALOG_TEXT[32][16] =
47
{
683 killagreg 48
   //1234567890123456
701 killagreg 49
    "IntegralPitch   ", //0
499 hbuss 50
    "IntegralRoll    ",
701 killagreg 51
    "AccPitch        ",
499 hbuss 52
    "AccRoll         ",
701 killagreg 53
    "GyroYaw         ",
54
    "ReadingHight    ", //5
499 hbuss 55
    "AccZ            ",
707 killagreg 56
    "Thrust          ",
701 killagreg 57
    "CompassHeading  ",
58
    "Voltage         ",
59
    "Receiver Level  ", //10
513 hbuss 60
    "Analog11        ",
701 killagreg 61
    "Motor_Front     ",
62
    "Motor_Rear      ",
63
    "Motor_Left      ",
64
    "Motor_Right     ", //15
513 hbuss 65
    "Acc_Z           ",
701 killagreg 66
    "MeanAccPitch    ",
67
    "MeanAccRoll     ",
68
    "IntegralErrPitch",
499 hbuss 69
    "IntegralErrRoll ", //20
701 killagreg 70
    "MeanIntPitch    ",
711 killagreg 71
    "MeanIntRoll         ",
701 killagreg 72
    "NeutralPitch    ",
513 hbuss 73
    "RollOffset      ",
701 killagreg 74
    "IntRoll*Factor  ", //25
712 killagreg 75
    "ReadingGyroPitch",
701 killagreg 76
    "DirectCorrRoll  ",
712 killagreg 77
    "ReadingGyroRoll ",
701 killagreg 78
    "CorrectionRoll  ",
79
    "I-AttRoll       ", //30
513 hbuss 80
    "StickRoll       "
499 hbuss 81
};
82
 
83
 
84
 
683 killagreg 85
/****************************************************************/
86
/*              Initialization of the USART0                    */
87
/****************************************************************/
88
void USART0_Init (void)
1 ingob 89
{
683 killagreg 90
        uint8_t sreg = SREG;
91
        uint16_t ubrr = (uint16_t) ((uint32_t) SYSCLK/(8 * USART0_BAUD) - 1);
92
 
93
        // disable all interrupts before configuration
94
        cli();
95
 
96
        // disable RX-Interrupt
97
        UCSR0B &= ~(1 << RXCIE0);
98
        // disable TX-Interrupt
99
        UCSR0B &= ~(1 << TXCIE0);
100
 
101
        // set direction of RXD0 and TXD0 pins
102
        // set RXD0 (PD0) as an input pin
103
        PORTD |= (1 << PORTD0);
104
        DDRD &= ~(1 << DDD0);
105
        // set TXD0 (PD1) as an output pin
106
        PORTD |= (1 << PORTD1);
107
        DDRD |=  (1 << DDD1);
108
 
109
        // USART0 Baud Rate Register
110
        // set clock divider
111
        UBRR0H = (uint8_t)(ubrr >> 8);
112
        UBRR0L = (uint8_t)ubrr;
113
 
114
        // USART0 Control and Status Register A, B, C
115
 
116
        // enable double speed operation in
117
        UCSR0A |= (1 << U2X0);
118
        // enable receiver and transmitter in
119
        UCSR0B = (1 << TXEN0) | (1 << RXEN0);
120
        // set asynchronous mode
121
        UCSR0C &= ~(1 << UMSEL01);
122
        UCSR0C &= ~(1 << UMSEL00);
123
        // no parity
124
        UCSR0C &= ~(1 << UPM01);
125
        UCSR0C &= ~(1 << UPM00);
126
        // 1 stop bit
127
        UCSR0C &= ~(1 << USBS0);
128
        // 8-bit
129
        UCSR0B &= ~(1 << UCSZ02);
130
        UCSR0C |=  (1 << UCSZ01);
131
        UCSR0C |=  (1 << UCSZ00);
132
 
133
                // flush receive buffer
134
        while ( UCSR0A & (1<<RXC0) ) UDR0;
135
 
136
        // enable interrupts at the end
137
        // enable RX-Interrupt
138
        UCSR0B |= (1 << RXCIE0);
139
        // enable TX-Interrupt
140
        UCSR0B |= (1 << TXCIE0);
141
 
142
        // restore global interrupt flags
143
    SREG = sreg;
144
}
145
 
146
/****************************************************************/
147
/*               USART0 transmitter ISR                         */
148
/****************************************************************/
149
ISR(USART0_TX_vect)
150
{
151
 static uint16_t ptr = 0;
152
 uint8_t tmp_tx;
153
 if(!UebertragungAbgeschlossen)
1 ingob 154
  {
155
   ptr++;                    // die [0] wurde schon gesendet
683 killagreg 156
   tmp_tx = txd_buffer[ptr];
157
   if((tmp_tx == '\r') || (ptr == TXD_BUFFER_LEN))
1 ingob 158
    {
159
     ptr = 0;
160
     UebertragungAbgeschlossen = 1;
161
    }
683 killagreg 162
   UDR0 = tmp_tx; // send byte will trigger this ISR again
163
  }
1 ingob 164
  else ptr = 0;
165
}
166
 
683 killagreg 167
/****************************************************************/
168
/*               USART0 receiver ISR                            */
169
/****************************************************************/
170
ISR(USART0_RX_vect)
1 ingob 171
{
683 killagreg 172
 static uint16_t crc;
173
 static uint8_t crc1,crc2,buf_ptr;
174
 static uint8_t UartState = 0;
175
 uint8_t CrcOkay = 0;
1 ingob 176
 
683 killagreg 177
 SioTmp = UDR0;
178
 if(buf_ptr >= RXD_BUFFER_LEN)    UartState = 0;
179
 if(SioTmp == '\r' && UartState == 2)
1 ingob 180
  {
181
   UartState = 0;
683 killagreg 182
   crc -= rxd_buffer[buf_ptr-2];
183
   crc -= rxd_buffer[buf_ptr-1];
1 ingob 184
   crc %= 4096;
185
   crc1 = '=' + crc / 64;
186
   crc2 = '=' + crc % 64;
187
   CrcOkay = 0;
683 killagreg 188
   if((crc1 == rxd_buffer[buf_ptr-2]) && (crc2 == rxd_buffer[buf_ptr-1])) CrcOkay = 1; else { CrcOkay = 0; CntCrcError++;};
1 ingob 189
   if(!NeuerDatensatzEmpfangen && CrcOkay) // Datensatz schon verarbeitet
190
    {
683 killagreg 191
     NeuerDatensatzEmpfangen = 1;
1 ingob 192
         AnzahlEmpfangsBytes = buf_ptr;
683 killagreg 193
     rxd_buffer[buf_ptr] = '\r';
194
         if(rxd_buffer[2] == 'R') wdt_enable(WDTO_250MS); // Reset-Commando
195
        }
1 ingob 196
  }
197
  else
198
  switch(UartState)
199
  {
200
   case 0:
201
          if(SioTmp == '#' && !NeuerDatensatzEmpfangen) UartState = 1;  // Startzeichen und Daten schon verarbeitet
202
                  buf_ptr = 0;
683 killagreg 203
                  rxd_buffer[buf_ptr++] = SioTmp;
1 ingob 204
                  crc = SioTmp;
205
          break;
206
   case 1: // Adresse auswerten
207
                  UartState++;
683 killagreg 208
                  rxd_buffer[buf_ptr++] = SioTmp;
1 ingob 209
                  crc += SioTmp;
210
                  break;
211
   case 2: //  Eingangsdaten sammeln
683 killagreg 212
                  rxd_buffer[buf_ptr] = SioTmp;
213
                  if(buf_ptr < RXD_BUFFER_LEN) buf_ptr++;
1 ingob 214
                  else UartState = 0;
215
                  crc += SioTmp;
216
                  break;
683 killagreg 217
   default:
218
          UartState = 0;
1 ingob 219
          break;
220
  }
221
}
222
 
223
 
683 killagreg 224
 
1 ingob 225
// --------------------------------------------------------------------------
226
void AddCRC(unsigned int wieviele)
227
{
683 killagreg 228
 uint16_t tmpCRC = 0,i;
1 ingob 229
 for(i = 0; i < wieviele;i++)
230
  {
683 killagreg 231
   tmpCRC += txd_buffer[i];
1 ingob 232
  }
233
   tmpCRC %= 4096;
683 killagreg 234
   txd_buffer[i++] = '=' + tmpCRC / 64;
235
   txd_buffer[i++] = '=' + tmpCRC % 64;
236
   txd_buffer[i++] = '\r';
1 ingob 237
  UebertragungAbgeschlossen = 0;
683 killagreg 238
  UDR0 = txd_buffer[0];
1 ingob 239
}
240
 
241
 
242
 
243
// --------------------------------------------------------------------------
683 killagreg 244
void SendOutData(uint8_t cmd,uint8_t modul, uint8_t *snd, uint8_t len)
1 ingob 245
{
683 killagreg 246
 uint16_t pt = 0;
247
 uint8_t a,b,c;
248
 uint8_t ptr = 0;
1 ingob 249
 
683 killagreg 250
 txd_buffer[pt++] = '#';               // Startzeichen
251
 txd_buffer[pt++] = modul;             // Adresse (a=0; b=1,...)
252
 txd_buffer[pt++] = cmd;                        // Commando
1 ingob 253
 
254
 while(len)
255
  {
256
   if(len) { a = snd[ptr++]; len--;} else a = 0;
257
   if(len) { b = snd[ptr++]; len--;} else b = 0;
258
   if(len) { c = snd[ptr++]; len--;} else c = 0;
683 killagreg 259
   txd_buffer[pt++] = '=' + (a >> 2);
260
   txd_buffer[pt++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4));
261
   txd_buffer[pt++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6));
262
   txd_buffer[pt++] = '=' + ( c & 0x3f);
1 ingob 263
  }
264
 AddCRC(pt);
265
}
266
 
267
 
268
// --------------------------------------------------------------------------
683 killagreg 269
void Decode64(uint8_t *ptrOut, uint8_t len, uint8_t ptrIn, uint8_t max)  // Wohin mit den Daten; Wie lang; Wo im rxd_buffer
1 ingob 270
{
683 killagreg 271
 uint8_t a,b,c,d;
272
 uint8_t ptr = 0;
273
 uint8_t x,y,z;
1 ingob 274
 while(len)
275
  {
683 killagreg 276
   a = rxd_buffer[ptrIn++] - '=';
277
   b = rxd_buffer[ptrIn++] - '=';
278
   c = rxd_buffer[ptrIn++] - '=';
279
   d = rxd_buffer[ptrIn++] - '=';
1 ingob 280
   if(ptrIn > max - 2) break;     // nicht mehr Daten verarbeiten, als empfangen wurden
281
 
282
   x = (a << 2) | (b >> 4);
283
   y = ((b & 0x0f) << 4) | (c >> 2);
284
   z = ((c & 0x03) << 6) | d;
285
 
286
   if(len--) ptrOut[ptr++] = x; else break;
287
   if(len--) ptrOut[ptr++] = y; else break;
288
   if(len--) ptrOut[ptr++] = z; else break;
289
  }
290
 
291
}
292
 
293
// --------------------------------------------------------------------------
294
void BearbeiteRxDaten(void)
295
{
296
 if(!NeuerDatensatzEmpfangen) return;
297
 
683 killagreg 298
 uint8_t tmp_char_arr2[2];
299
 
706 killagreg 300
 PcAccess = 255;
683 killagreg 301
  switch(rxd_buffer[2])
1 ingob 302
  {
499 hbuss 303
   case 'a':// Texte der Analogwerte
683 killagreg 304
            Decode64((uint8_t *) &tmp_char_arr2[0],sizeof(tmp_char_arr2),3,AnzahlEmpfangsBytes);
499 hbuss 305
            DebugTextAnforderung = tmp_char_arr2[0];
306
                        break;
595 hbuss 307
   case 'b':
683 killagreg 308
                        Decode64((uint8_t *) &ExternControl,sizeof(ExternControl),3,AnzahlEmpfangsBytes);
685 killagreg 309
                        RemoteButtons |= ExternControl.RemoteButtons;
595 hbuss 310
            ConfirmFrame = ExternControl.Frame;
311
            break;
312
   case 'c':
683 killagreg 313
                        Decode64((uint8_t *) &ExternControl,sizeof(ExternControl),3,AnzahlEmpfangsBytes);
685 killagreg 314
                        RemoteButtons |= ExternControl.RemoteButtons;
595 hbuss 315
            ConfirmFrame = ExternControl.Frame;
1 ingob 316
            DebugDataAnforderung = 1;
317
            break;
318
   case 'h':// x-1 Displayzeilen
683 killagreg 319
            Decode64((uint8_t *) &tmp_char_arr2[0],sizeof(tmp_char_arr2),3,AnzahlEmpfangsBytes);
685 killagreg 320
            RemoteButtons |= tmp_char_arr2[0];
395 hbuss 321
                        if(tmp_char_arr2[1] == 255) NurKanalAnforderung = 1; else NurKanalAnforderung = 0; // keine Displaydaten
1 ingob 322
                        DebugDisplayAnforderung = 1;
323
                        break;
324
   case 't':// Motortest
683 killagreg 325
            Decode64((uint8_t *) &MotorTest[0],sizeof(MotorTest),3,AnzahlEmpfangsBytes);
1 ingob 326
                        break;
492 hbuss 327
   case 'k':// Keys von DubWise
683 killagreg 328
            Decode64((uint8_t *) &DubWiseKeys[0],sizeof(DubWiseKeys),3,AnzahlEmpfangsBytes);
595 hbuss 329
                        ConfirmFrame = DubWiseKeys[3];
492 hbuss 330
                        break;
1 ingob 331
   case 'v': // Version-Anforderung     und Ausbaustufe
332
            GetVersionAnforderung = 1;
683 killagreg 333
            break;
334
   case 'g':// "Get"-Anforderung für Debug-Daten
1 ingob 335
            // Bei Get werden die vom PC einstellbaren Werte vom PC zurückgelesen
336
            DebugGetAnforderung = 1;
337
            break;
338
   case 'q':// "Get"-Anforderung für Settings
339
            // Bei Get werden die vom PC einstellbaren Werte vom PC zurückgelesen
683 killagreg 340
            Decode64((uint8_t *) &tmp_char_arr2[0],sizeof(tmp_char_arr2),3,AnzahlEmpfangsBytes);
1 ingob 341
            if(tmp_char_arr2[0] != 0xff)
342
             {
343
                          if(tmp_char_arr2[0] > 5) tmp_char_arr2[0] = 5;
687 killagreg 344
                  ParamSet_ReadFromEEProm(tmp_char_arr2[0]);
690 killagreg 345
                  SendOutData('L' + tmp_char_arr2[0] -1,MeineSlaveAdresse,(uint8_t *) &ParamSet.ChannelAssignment[0],PARAMSET_STRUCT_LEN);
683 killagreg 346
             }
347
             else
690 killagreg 348
                  SendOutData('L' + GetParamByte(PID_ACTIVE_SET)-1,MeineSlaveAdresse,(uint8_t *) &ParamSet.ChannelAssignment[0],PARAMSET_STRUCT_LEN);
683 killagreg 349
 
1 ingob 350
            break;
683 killagreg 351
 
1 ingob 352
   case 'l':
353
   case 'm':
354
   case 'n':
355
   case 'o':
356
   case 'p': // Parametersatz speichern
690 killagreg 357
            Decode64((uint8_t *) &ParamSet.ChannelAssignment[0],PARAMSET_STRUCT_LEN,3,AnzahlEmpfangsBytes);
687 killagreg 358
                        ParamSet_WriteToEEProm(rxd_buffer[2] - 'l' + 1);
359
            //SetActiveParamSet(rxd_buffer[2] - 'l' + 1);  // is alredy done in ParamSet_WriteToEEProm()
701 killagreg 360
            TurnOver180Pitch = (int32_t) ParamSet.AngleTurnOverPitch * 2500L;
361
            TurnOver180Roll = (int32_t) ParamSet.AngleTurnOverRoll * 2500L;
703 killagreg 362
            Beep(GetActiveParamSet());
1 ingob 363
         break;
683 killagreg 364
 
365
 
1 ingob 366
  }
367
// DebugOut.AnzahlZyklen =  Debug_Timer_Intervall;
368
 NeuerDatensatzEmpfangen = 0;
369
}
370
 
371
//############################################################################
372
//Routine für die Serielle Ausgabe
683 killagreg 373
int16_t uart_putchar (int8_t c)
1 ingob 374
//############################################################################
375
{
376
        if (c == '\n')
377
                uart_putchar('\r');
378
        //Warten solange bis Zeichen gesendet wurde
683 killagreg 379
        loop_until_bit_is_set(UCSR0A, UDRE0);
1 ingob 380
        //Ausgabe des Zeichens
683 killagreg 381
        UDR0 = c;
382
 
1 ingob 383
        return (0);
384
}
385
 
386
// --------------------------------------------------------------------------
387
void WriteProgramData(unsigned int pos, unsigned char wert)
388
{
389
  //if (ProgramLocation == IN_RAM) Buffer[pos] = wert;
390
  // else eeprom_write_byte(&EE_Buffer[pos], wert);
391
  // Buffer[pos] = wert;
392
}
393
 
394
 
395
 
396
//---------------------------------------------------------------------------------------------
683 killagreg 397
void DatenUebertragung(void)
1 ingob 398
{
399
 if(!UebertragungAbgeschlossen) return;
400
 
401
   if(DebugGetAnforderung && UebertragungAbgeschlossen)               // Bei Get werden die vom PC einstellbaren Werte vom PC zurückgelesen
683 killagreg 402
   {
403
      SendOutData('G',MeineSlaveAdresse,(uint8_t *) &ExternControl,sizeof(ExternControl));
1 ingob 404
          DebugGetAnforderung = 0;
405
   }
406
 
683 killagreg 407
    if((CheckDelay(Debug_Timer) || DebugDataAnforderung) && UebertragungAbgeschlossen)
1 ingob 408
         {
683 killagreg 409
          SendOutData('D',MeineSlaveAdresse,(uint8_t *) &DebugOut,sizeof(DebugOut));
1 ingob 410
          DebugDataAnforderung = 0;
683 killagreg 411
          Debug_Timer = SetDelay(MIN_DEBUG_INTERVALL);
412
         }
499 hbuss 413
    if(DebugTextAnforderung != 255) // Texte für die Analogdaten
414
     {
683 killagreg 415
      SendOutData('A',DebugTextAnforderung + '0',(uint8_t *) ANALOG_TEXT[DebugTextAnforderung],16);
499 hbuss 416
      DebugTextAnforderung = 255;
417
         }
683 killagreg 418
     if(ConfirmFrame && UebertragungAbgeschlossen)   // Datensatz ohne CRC bestätigen
595 hbuss 419
         {
683 killagreg 420
      txd_buffer[0] = '#';
421
      txd_buffer[1] = ConfirmFrame;
422
      txd_buffer[2] = '\r';
595 hbuss 423
      UebertragungAbgeschlossen = 0;
424
      ConfirmFrame = 0;
683 killagreg 425
      UDR0 = txd_buffer[0];
595 hbuss 426
     }
1 ingob 427
     if(DebugDisplayAnforderung && UebertragungAbgeschlossen)
428
         {
685 killagreg 429
      LCD_PrintMenu();
1 ingob 430
          DebugDisplayAnforderung = 0;
683 killagreg 431
      if(++RemotePollDisplayLine == 4 || NurKanalAnforderung)
173 holgerb 432
      {
683 killagreg 433
       SendOutData('4',0,(uint8_t *)&PPM_in,sizeof(PPM_in));   // DisplayZeile übertragen
499 hbuss 434
       RemotePollDisplayLine = -1;
683 killagreg 435
      }
436
      else  SendOutData('0' + RemotePollDisplayLine,0,(uint8_t *)&DisplayBuff[20 * RemotePollDisplayLine],20);   // DisplayZeile übertragen
437
         }
438
    if(GetVersionAnforderung && UebertragungAbgeschlossen)
439
     {
440
      SendOutData('V',MeineSlaveAdresse,(uint8_t *) &VersionInfo,sizeof(VersionInfo));
1 ingob 441
          GetVersionAnforderung = 0;
442
     }
443
 
444
}
445