Subversion Repositories FlightCtrl

Rev

Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
745 killagreg 1
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1 ingob 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
 
735 killagreg 21
#define FALSE   0
22
#define TRUE    1
685 killagreg 23
 
745 killagreg 24
//int8_t test __attribute__ ((section (".noinit")));
683 killagreg 25
 
745 killagreg 26
uint8_t DebugGetRequest = 0, DebugDisplayRequest = 0, DebugDataRequest = 0, GetVersionRequest = 0;
27
 
735 killagreg 28
volatile uint8_t txd_buffer[TXD_BUFFER_LEN];
737 killagreg 29
volatile uint8_t rxd_buffer_locked = FALSE;
735 killagreg 30
volatile uint8_t rxd_buffer[RXD_BUFFER_LEN];
737 killagreg 31
volatile uint8_t txd_complete = TRUE;
735 killagreg 32
volatile uint8_t ReceivedBytes = 0;
33
 
34
uint8_t RemotePollDisplayLine = 0;
35
uint8_t NurKanalAnforderung = 0;
36
uint8_t DebugTextAnforderung = 255;
37
uint8_t PcAccess = 100;
38
uint8_t MotorTest[4] = {0,0,0,0};
39
uint8_t DubWiseKeys[4] = {0,0,0,0};
745 killagreg 40
uint8_t MySlaveAddr = 0;
735 killagreg 41
uint8_t ConfirmFrame;
42
 
43
DebugOut_t              DebugOut;
44
ExternControl_t ExternControl;
45
VersionInfo_t   VersionInfo;
46
 
47
const uint8_t ANALOG_TEXT[32][16] =
499 hbuss 48
{
683 killagreg 49
   //1234567890123456
701 killagreg 50
    "IntegralPitch   ", //0
499 hbuss 51
    "IntegralRoll    ",
701 killagreg 52
    "AccPitch        ",
499 hbuss 53
    "AccRoll         ",
701 killagreg 54
    "GyroYaw         ",
55
    "ReadingHight    ", //5
499 hbuss 56
    "AccZ            ",
707 killagreg 57
    "Thrust          ",
701 killagreg 58
    "CompassHeading  ",
59
    "Voltage         ",
60
    "Receiver Level  ", //10
513 hbuss 61
    "Analog11        ",
741 killagreg 62
    "GPSDevNorth     ",
63
    "GPSDevEast      ",
64
    "GPS_Pitch       ",
65
    "GPS_Roll        ", //15
513 hbuss 66
    "Acc_Z           ",
701 killagreg 67
    "MeanAccPitch    ",
68
    "MeanAccRoll     ",
69
    "IntegralErrPitch",
499 hbuss 70
    "IntegralErrRoll ", //20
701 killagreg 71
    "MeanIntPitch    ",
711 killagreg 72
    "MeanIntRoll         ",
701 killagreg 73
    "NeutralPitch    ",
513 hbuss 74
    "RollOffset      ",
701 killagreg 75
    "IntRoll*Factor  ", //25
712 killagreg 76
    "ReadingGyroPitch",
701 killagreg 77
    "DirectCorrRoll  ",
712 killagreg 78
    "ReadingGyroRoll ",
701 killagreg 79
    "CorrectionRoll  ",
80
    "I-AttRoll       ", //30
513 hbuss 81
    "StickRoll       "
499 hbuss 82
};
83
 
84
 
85
 
683 killagreg 86
/****************************************************************/
87
/*              Initialization of the USART0                    */
88
/****************************************************************/
89
void USART0_Init (void)
1 ingob 90
{
683 killagreg 91
        uint8_t sreg = SREG;
92
        uint16_t ubrr = (uint16_t) ((uint32_t) SYSCLK/(8 * USART0_BAUD) - 1);
93
 
94
        // disable all interrupts before configuration
95
        cli();
96
 
97
        // disable RX-Interrupt
98
        UCSR0B &= ~(1 << RXCIE0);
99
        // disable TX-Interrupt
100
        UCSR0B &= ~(1 << TXCIE0);
101
 
102
        // set direction of RXD0 and TXD0 pins
103
        // set RXD0 (PD0) as an input pin
104
        PORTD |= (1 << PORTD0);
105
        DDRD &= ~(1 << DDD0);
106
        // set TXD0 (PD1) as an output pin
107
        PORTD |= (1 << PORTD1);
108
        DDRD |=  (1 << DDD1);
109
 
110
        // USART0 Baud Rate Register
111
        // set clock divider
112
        UBRR0H = (uint8_t)(ubrr >> 8);
113
        UBRR0L = (uint8_t)ubrr;
114
 
115
        // USART0 Control and Status Register A, B, C
116
 
117
        // enable double speed operation in
118
        UCSR0A |= (1 << U2X0);
119
        // enable receiver and transmitter in
120
        UCSR0B = (1 << TXEN0) | (1 << RXEN0);
121
        // set asynchronous mode
122
        UCSR0C &= ~(1 << UMSEL01);
123
        UCSR0C &= ~(1 << UMSEL00);
124
        // no parity
125
        UCSR0C &= ~(1 << UPM01);
126
        UCSR0C &= ~(1 << UPM00);
127
        // 1 stop bit
128
        UCSR0C &= ~(1 << USBS0);
129
        // 8-bit
130
        UCSR0B &= ~(1 << UCSZ02);
131
        UCSR0C |=  (1 << UCSZ01);
132
        UCSR0C |=  (1 << UCSZ00);
133
 
134
                // flush receive buffer
135
        while ( UCSR0A & (1<<RXC0) ) UDR0;
136
 
137
        // enable interrupts at the end
138
        // enable RX-Interrupt
139
        UCSR0B |= (1 << RXCIE0);
140
        // enable TX-Interrupt
141
        UCSR0B |= (1 << TXCIE0);
142
 
737 killagreg 143
        rxd_buffer_locked = FALSE;
683 killagreg 144
        // restore global interrupt flags
145
    SREG = sreg;
146
}
147
 
148
/****************************************************************/
149
/*               USART0 transmitter ISR                         */
150
/****************************************************************/
151
ISR(USART0_TX_vect)
152
{
735 killagreg 153
        static uint16_t ptr_txd_buffer = 0;
154
        uint8_t tmp_tx;
155
        if(!txd_complete) // transmission not completed
156
        {
157
                ptr_txd_buffer++;                    // die [0] wurde schon gesendet
158
                tmp_tx = txd_buffer[ptr_txd_buffer];
159
                // if terminating character or end of txd buffer was reached
160
                if((tmp_tx == '\r') || (ptr_txd_buffer == TXD_BUFFER_LEN))
161
                {
162
                        ptr_txd_buffer = 0; // reset txd pointer
163
                        txd_complete = 1; // stop transmission
164
                }
165
                UDR0 = tmp_tx; // send current byte will trigger this ISR again
166
        }
167
        // transmission completed
168
        else ptr_txd_buffer = 0;
1 ingob 169
}
170
 
683 killagreg 171
/****************************************************************/
172
/*               USART0 receiver ISR                            */
173
/****************************************************************/
174
ISR(USART0_RX_vect)
1 ingob 175
{
735 killagreg 176
        static uint16_t crc;
737 killagreg 177
        static uint8_t ptr_rxd_buffer = 0;
178
        uint8_t crc1, crc2;
735 killagreg 179
        uint8_t c;
1 ingob 180
 
737 killagreg 181
        c = UDR0;  // catch the received byte
182
 
183
 
184
        if(rxd_buffer_locked) return; // if txd buffer is locked immediately return
185
 
186
        // the rxd buffer is unlocked
187
        if((ptr_rxd_buffer == 0) && (c == '#')) // if rxd buffer is empty and syncronisation character is received
735 killagreg 188
        {
737 killagreg 189
                rxd_buffer[ptr_rxd_buffer++] = c; // copy 1st byte to buffer
190
                crc = c; // init crc
191
        }
192
        #if 0
193
        else if (ptr_rxd_buffer == 1) // handle address
194
        {
195
                rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer
196
                crc += c; // update crc
197
        }
198
        #endif
199
        else if (ptr_rxd_buffer < RXD_BUFFER_LEN) // collect incomming bytes
200
        {
201
                if(c != '\r') // no termination character
735 killagreg 202
                {
737 killagreg 203
                        rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer
204
                        crc += c; // update crc
735 killagreg 205
                }
737 killagreg 206
                else // termination character was received
207
                {
208
                        // the last 2 bytes are no subject for checksum calculation
209
                        // they are the checksum itself
210
                        crc -= rxd_buffer[ptr_rxd_buffer-2];
211
                        crc -= rxd_buffer[ptr_rxd_buffer-1];
212
                        // calculate checksum from transmitted data
213
                        crc %= 4096;
214
                        crc1 = '=' + crc / 64;
215
                        crc2 = '=' + crc % 64;
216
                        // compare checksum to transmitted checksum bytes
217
                        if((crc1 == rxd_buffer[ptr_rxd_buffer-2]) && (crc2 == rxd_buffer[ptr_rxd_buffer-1]))
218
                        {   // checksum valid
219
                                rxd_buffer_locked = TRUE;          // lock the rxd buffer
220
                                ReceivedBytes = ptr_rxd_buffer;    // store number of received bytes
221
                                rxd_buffer[ptr_rxd_buffer] = '\r'; // set termination character
222
                                // if 2nd byte is an 'R' enable watchdog that will result in an reset
745 killagreg 223
                                if(rxd_buffer[2] == 'R') {wdt_enable(WDTO_250MS);} // Reset-Commando
737 killagreg 224
                        }
225
                        else
226
                        {       // checksum invalid
227
                                rxd_buffer_locked = FALSE; // unlock rxd buffer
228
                        }
229
                        ptr_rxd_buffer = 0; // reset txd buffer
230
                }
231
        } // buffer overrun
735 killagreg 232
        else
233
        {
737 killagreg 234
                ptr_rxd_buffer = 0; // reset rxd buffer
235
                rxd_buffer_locked = FALSE; // unlock rxd buffer
735 killagreg 236
        }
737 killagreg 237
 
1 ingob 238
}
239
 
240
 
241
// --------------------------------------------------------------------------
735 killagreg 242
void AddCRC(uint16_t datalen)
1 ingob 243
{
683 killagreg 244
 uint16_t tmpCRC = 0,i;
735 killagreg 245
 for(i = 0; i < datalen;i++)
1 ingob 246
  {
683 killagreg 247
   tmpCRC += txd_buffer[i];
1 ingob 248
  }
249
   tmpCRC %= 4096;
683 killagreg 250
   txd_buffer[i++] = '=' + tmpCRC / 64;
251
   txd_buffer[i++] = '=' + tmpCRC % 64;
252
   txd_buffer[i++] = '\r';
735 killagreg 253
  txd_complete = 0;
254
  UDR0 = txd_buffer[0]; // initiates the transmittion
1 ingob 255
}
256
 
257
 
258
 
259
// --------------------------------------------------------------------------
745 killagreg 260
void SendOutData(uint8_t cmd,uint8_t module, uint8_t *snd, uint8_t len)
1 ingob 261
{
735 killagreg 262
        uint16_t pt = 0;
263
        uint8_t a,b,c;
264
        uint8_t ptr = 0;
1 ingob 265
 
735 killagreg 266
        txd_buffer[pt++] = '#';         // Start character
745 killagreg 267
        txd_buffer[pt++] = module;      // Address (a=0; b=1,...)
735 killagreg 268
        txd_buffer[pt++] = cmd;         // Command
1 ingob 269
 
735 killagreg 270
        while(len)
271
        {
272
                if(len) { a = snd[ptr++]; len--;} else a = 0;
273
                if(len) { b = snd[ptr++]; len--;} else b = 0;
274
                if(len) { c = snd[ptr++]; len--;} else c = 0;
275
                txd_buffer[pt++] = '=' + (a >> 2);
276
                txd_buffer[pt++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4));
277
                txd_buffer[pt++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6));
278
                txd_buffer[pt++] = '=' + ( c & 0x3f);
279
        }
280
        AddCRC(pt); // add checksum after data block and initates the transmission
1 ingob 281
}
282
 
283
 
284
// --------------------------------------------------------------------------
745 killagreg 285
void Decode64(uint8_t *ptrOut, uint8_t len, uint8_t ptrIn, uint8_t max)
1 ingob 286
{
735 killagreg 287
        uint8_t a,b,c,d;
288
        uint8_t ptr = 0;
289
        uint8_t x,y,z;
290
        while(len)
291
        {
292
                a = rxd_buffer[ptrIn++] - '=';
293
                b = rxd_buffer[ptrIn++] - '=';
294
                c = rxd_buffer[ptrIn++] - '=';
295
                d = rxd_buffer[ptrIn++] - '=';
745 killagreg 296
                if(ptrIn > max - 2) break;
1 ingob 297
 
735 killagreg 298
                x = (a << 2) | (b >> 4);
299
                y = ((b & 0x0f) << 4) | (c >> 2);
300
                z = ((c & 0x03) << 6) | d;
1 ingob 301
 
735 killagreg 302
                if(len--) ptrOut[ptr++] = x; else break;
303
                if(len--) ptrOut[ptr++] = y; else break;
304
                if(len--) ptrOut[ptr++] = z; else break;
305
        }
1 ingob 306
}
307
 
735 killagreg 308
 
1 ingob 309
// --------------------------------------------------------------------------
735 killagreg 310
void ProcessRxData(void)
1 ingob 311
{
737 killagreg 312
        // if data in the rxd buffer are not locked immediately return
313
        if(!rxd_buffer_locked) return;
1 ingob 314
 
735 killagreg 315
        uint8_t tmp_char_arr2[2]; // local buffer
683 killagreg 316
 
735 killagreg 317
        PcAccess = 255;
318
        switch(rxd_buffer[2])
319
        {
320
                case 'a':// Labels of the Analog Deboug outputs
321
                        Decode64((uint8_t *) &tmp_char_arr2[0], sizeof(tmp_char_arr2), 3, ReceivedBytes);
322
                        DebugTextAnforderung = tmp_char_arr2[0];
499 hbuss 323
                        break;
735 killagreg 324
                case 'b': // extern control
325
                        Decode64((uint8_t *) &ExternControl,sizeof(ExternControl), 3, ReceivedBytes);
685 killagreg 326
                        RemoteButtons |= ExternControl.RemoteButtons;
735 killagreg 327
                        ConfirmFrame = ExternControl.Frame;
328
                        break;
329
                case 'c': // extern control with debug request
330
                        Decode64((uint8_t *) &ExternControl,sizeof(ExternControl),3,ReceivedBytes);
685 killagreg 331
                        RemoteButtons |= ExternControl.RemoteButtons;
735 killagreg 332
                        ConfirmFrame = ExternControl.Frame;
745 killagreg 333
                        DebugDataRequest = 1;
735 killagreg 334
                        break;
335
                case 'h':// x-1 display columns
336
                        Decode64((uint8_t *) &tmp_char_arr2[0],sizeof(tmp_char_arr2),3,ReceivedBytes);
337
                        RemoteButtons |= tmp_char_arr2[0];
338
                        if(tmp_char_arr2[1] == 255) NurKanalAnforderung = 1;
339
                        else NurKanalAnforderung = 0; // keine Displaydaten
745 killagreg 340
                        DebugDisplayRequest = 1;
1 ingob 341
                        break;
735 killagreg 342
                case 't':// motor test
343
                        Decode64((uint8_t *) &MotorTest[0],sizeof(MotorTest),3,ReceivedBytes);
1 ingob 344
                        break;
735 killagreg 345
                case 'k':// keys from DubWise
346
                        Decode64((uint8_t *) &DubWiseKeys[0],sizeof(DubWiseKeys),3,ReceivedBytes);
595 hbuss 347
                        ConfirmFrame = DubWiseKeys[3];
492 hbuss 348
                        break;
735 killagreg 349
                case 'v': // get version and board release
745 killagreg 350
                        GetVersionRequest = 1;
735 killagreg 351
                        break;
352
                case 'g':// get debug data
745 killagreg 353
                        DebugGetRequest = 1;
735 killagreg 354
                        break;
355
                case 'q':// get settings
356
                        Decode64((uint8_t *) &tmp_char_arr2[0],sizeof(tmp_char_arr2),3,ReceivedBytes);
357
                        if(tmp_char_arr2[0] != 0xff)
358
                        {
359
                                if(tmp_char_arr2[0] > 5) tmp_char_arr2[0] = 5; // limit to 5
360
                                // load requested parameter set
361
                                ParamSet_ReadFromEEProm(tmp_char_arr2[0]);
362
                                SendOutData('L' + tmp_char_arr2[0] -1,MySlaveAddr,(uint8_t *) &ParamSet.ChannelAssignment[0],PARAMSET_STRUCT_LEN);
363
                        }
364
                        else // send active parameter set
365
                        SendOutData('L' + GetParamByte(PID_ACTIVE_SET)-1,MySlaveAddr,(uint8_t *) &ParamSet.ChannelAssignment[0],PARAMSET_STRUCT_LEN);
683 killagreg 366
 
735 killagreg 367
                        break;
683 killagreg 368
 
735 killagreg 369
                case 'l':
370
                case 'm':
371
                case 'n':
372
                case 'o':
373
                case 'p': // save parameterset
374
                        Decode64((uint8_t *) &ParamSet.ChannelAssignment[0],PARAMSET_STRUCT_LEN,3,ReceivedBytes);
687 killagreg 375
                        ParamSet_WriteToEEProm(rxd_buffer[2] - 'l' + 1);
735 killagreg 376
                        TurnOver180Pitch = (int32_t) ParamSet.AngleTurnOverPitch * 2500L;
377
                        TurnOver180Roll = (int32_t) ParamSet.AngleTurnOverRoll * 2500L;
378
                        Beep(GetActiveParamSet());
379
                        break;
683 killagreg 380
 
381
 
735 killagreg 382
        }
737 killagreg 383
        // unlock the rxd buffer after processing
384
        rxd_buffer_locked = FALSE;
1 ingob 385
}
386
 
387
//############################################################################
388
//Routine für die Serielle Ausgabe
683 killagreg 389
int16_t uart_putchar (int8_t c)
1 ingob 390
//############################################################################
391
{
392
        if (c == '\n')
393
                uart_putchar('\r');
735 killagreg 394
        // wait until previous character was send
683 killagreg 395
        loop_until_bit_is_set(UCSR0A, UDRE0);
1 ingob 396
        //Ausgabe des Zeichens
683 killagreg 397
        UDR0 = c;
398
 
1 ingob 399
        return (0);
400
}
401
 
402
 
403
//---------------------------------------------------------------------------------------------
735 killagreg 404
void TransmitTxData(void)
1 ingob 405
{
735 killagreg 406
 static int16_t Debug_Timer = 0;
407
 if(!txd_complete) return;
1 ingob 408
 
745 killagreg 409
   if(DebugGetRequest && txd_complete)        // Bei Get werden die vom PC einstellbaren Werte vom PC zurückgelesen
683 killagreg 410
   {
735 killagreg 411
      SendOutData('G',MySlaveAddr,(uint8_t *) &ExternControl,sizeof(ExternControl));
745 killagreg 412
          DebugGetRequest = 0;
1 ingob 413
   }
414
 
745 killagreg 415
    if((CheckDelay(Debug_Timer) || DebugDataRequest) && txd_complete)
1 ingob 416
         {
735 killagreg 417
          SendOutData('D',MySlaveAddr,(uint8_t *) &DebugOut,sizeof(DebugOut));
745 killagreg 418
          DebugDataRequest = 0;
683 killagreg 419
          Debug_Timer = SetDelay(MIN_DEBUG_INTERVALL);
420
         }
499 hbuss 421
    if(DebugTextAnforderung != 255) // Texte für die Analogdaten
422
     {
683 killagreg 423
      SendOutData('A',DebugTextAnforderung + '0',(uint8_t *) ANALOG_TEXT[DebugTextAnforderung],16);
499 hbuss 424
      DebugTextAnforderung = 255;
425
         }
735 killagreg 426
     if(ConfirmFrame && txd_complete)   // Datensatz ohne CRC bestätigen
595 hbuss 427
         {
683 killagreg 428
      txd_buffer[0] = '#';
429
      txd_buffer[1] = ConfirmFrame;
430
      txd_buffer[2] = '\r';
735 killagreg 431
      txd_complete = 0;
595 hbuss 432
      ConfirmFrame = 0;
683 killagreg 433
      UDR0 = txd_buffer[0];
595 hbuss 434
     }
745 killagreg 435
     if(DebugDisplayRequest && txd_complete)
1 ingob 436
         {
685 killagreg 437
      LCD_PrintMenu();
745 killagreg 438
          DebugDisplayRequest = 0;
683 killagreg 439
      if(++RemotePollDisplayLine == 4 || NurKanalAnforderung)
173 holgerb 440
      {
683 killagreg 441
       SendOutData('4',0,(uint8_t *)&PPM_in,sizeof(PPM_in));   // DisplayZeile übertragen
499 hbuss 442
       RemotePollDisplayLine = -1;
683 killagreg 443
      }
444
      else  SendOutData('0' + RemotePollDisplayLine,0,(uint8_t *)&DisplayBuff[20 * RemotePollDisplayLine],20);   // DisplayZeile übertragen
445
         }
745 killagreg 446
    if(GetVersionRequest && txd_complete)
683 killagreg 447
     {
735 killagreg 448
      SendOutData('V',MySlaveAddr,(uint8_t *) &VersionInfo,sizeof(VersionInfo));
745 killagreg 449
          GetVersionRequest = 0;
1 ingob 450
     }
451
 
452
}
453