Subversion Repositories FlightCtrl

Rev

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

Rev Author Line No. Line
886 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
 
886 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"
886 killagreg 14
#include "menu.h"
15
#include "timer0.h"
1 ingob 16
#include "uart.h"
886 killagreg 17
#include "fc.h"
18
#include "_Settings.h"
19
#include "rc.h"
908 killagreg 20
#if defined (USE_KILLAGREG) || defined (USE_MK3MAG)
886 killagreg 21
#include "ubx.h"
22
#endif
908 killagreg 23
#ifdef USE_MK3MAG
886 killagreg 24
#include "mk3mag.h"
25
#endif
1 ingob 26
 
27
 
693 hbuss 28
 
886 killagreg 29
#define FALSE   0
30
#define TRUE    1
31
 
32
//int8_t test __attribute__ ((section (".noinit")));
33
uint8_t RequestVerInfo                  = FALSE;
34
uint8_t RequestExternalControl  = FALSE;
35
uint8_t RequestDisplay                  = FALSE;
36
uint8_t RequestDebugData                = FALSE;
37
uint8_t RequestDebugLabel               = 255;
38
uint8_t RequestChannelOnly              = FALSE;
39
uint8_t RemotePollDisplayLine   = 0;
40
 
41
volatile uint8_t txd_buffer[TXD_BUFFER_LEN];
42
volatile uint8_t rxd_buffer_locked = FALSE;
43
volatile uint8_t rxd_buffer[RXD_BUFFER_LEN];
44
volatile uint8_t txd_complete = TRUE;
45
volatile uint8_t ReceivedBytes = 0;
46
 
47
 
48
uint8_t PcAccess = 100;
49
uint8_t MotorTest[4] = {0,0,0,0};
50
uint8_t DubWiseKeys[4] = {0,0,0,0};
51
uint8_t MySlaveAddr = 0;
52
uint8_t ConfirmFrame;
53
 
54
DebugOut_t              DebugOut;
55
ExternControl_t ExternControl;
56
VersionInfo_t   VersionInfo;
57
 
58
int16_t  Debug_Timer;
59
 
908 killagreg 60
#ifdef USE_MK3MAG
886 killagreg 61
int16_t Compass_Timer;
62
#endif
63
 
64
 
65
const uint8_t ANALOG_LABEL[32][16] =
499 hbuss 66
{
886 killagreg 67
   //1234567890123456
911 killagreg 68
    "IntegralNick    ", //0
499 hbuss 69
    "IntegralRoll    ",
911 killagreg 70
    "AccNick         ",
499 hbuss 71
    "AccRoll         ",
886 killagreg 72
    "GyroYaw         ",
73
    "ReadingHeight   ", //5
499 hbuss 74
    "AccZ            ",
911 killagreg 75
    "Gas             ",
886 killagreg 76
    "CompassHeading  ",
77
    "Voltage         ",
78
    "Receiver Level  ", //10
79
    "YawGyroHeading  ",
80
    "Motor_Front     ",
81
    "Motor_Rear      ",
82
    "Motor_Right     ",
83
    "Motor_Left      ", //15
936 killagreg 84
    "                ",
886 killagreg 85
    "SPI Error       ",
86
    "SPI Ok          ",
87
    "                ",
854 hbuss 88
    "Servo           ", //20
911 killagreg 89
    "Nick            ",
854 hbuss 90
    "Roll            ",
720 ingob 91
    "                ",
92
    "                ",
93
    "                ", //25
94
    "                ",
95
    "                ",
96
    "                ",
97
    "                ",
911 killagreg 98
    "GPS_Nick        ", //30
720 ingob 99
    "GPS_Roll        "
499 hbuss 100
};
101
 
102
 
103
 
886 killagreg 104
/****************************************************************/
105
/*              Initialization of the USART0                    */
106
/****************************************************************/
107
void USART0_Init (void)
1 ingob 108
{
886 killagreg 109
        uint8_t sreg = SREG;
110
        uint16_t ubrr = (uint16_t) ((uint32_t) SYSCLK/(8 * USART0_BAUD) - 1);
111
 
112
        // disable all interrupts before configuration
113
        cli();
114
 
115
        // disable RX-Interrupt
116
        UCSR0B &= ~(1 << RXCIE0);
117
        // disable TX-Interrupt
118
        UCSR0B &= ~(1 << TXCIE0);
119
 
120
        // set direction of RXD0 and TXD0 pins
121
        // set RXD0 (PD0) as an input pin
122
        PORTD |= (1 << PORTD0);
123
        DDRD &= ~(1 << DDD0);
124
        // set TXD0 (PD1) as an output pin
125
        PORTD |= (1 << PORTD1);
126
        DDRD |=  (1 << DDD1);
127
 
128
        // USART0 Baud Rate Register
129
        // set clock divider
130
        UBRR0H = (uint8_t)(ubrr >> 8);
131
        UBRR0L = (uint8_t)ubrr;
132
 
133
        // USART0 Control and Status Register A, B, C
134
 
135
        // enable double speed operation in
136
        UCSR0A |= (1 << U2X0);
137
        // enable receiver and transmitter in
138
        UCSR0B = (1 << TXEN0) | (1 << RXEN0);
139
        // set asynchronous mode
140
        UCSR0C &= ~(1 << UMSEL01);
141
        UCSR0C &= ~(1 << UMSEL00);
142
        // no parity
143
        UCSR0C &= ~(1 << UPM01);
144
        UCSR0C &= ~(1 << UPM00);
145
        // 1 stop bit
146
        UCSR0C &= ~(1 << USBS0);
147
        // 8-bit
148
        UCSR0B &= ~(1 << UCSZ02);
149
        UCSR0C |=  (1 << UCSZ01);
150
        UCSR0C |=  (1 << UCSZ00);
151
 
152
                // flush receive buffer
153
        while ( UCSR0A & (1<<RXC0) ) UDR0;
154
 
155
        // enable interrupts at the end
156
        // enable RX-Interrupt
157
        UCSR0B |= (1 << RXCIE0);
158
        // enable TX-Interrupt
159
        UCSR0B |= (1 << TXCIE0);
160
 
161
        rxd_buffer_locked = FALSE;
162
        txd_complete = TRUE;
163
 
164
        Debug_Timer = SetDelay(200);
165
 
908 killagreg 166
        #ifdef USE_MK3MAG
886 killagreg 167
        Compass_Timer = SetDelay(220);
168
        #endif
169
 
170
        // restore global interrupt flags
171
    SREG = sreg;
1 ingob 172
}
173
 
886 killagreg 174
/****************************************************************/
175
/*               USART0 transmitter ISR                         */
176
/****************************************************************/
177
ISR(USART0_TX_vect)
1 ingob 178
{
886 killagreg 179
        static uint16_t ptr_txd_buffer = 0;
180
        uint8_t tmp_tx;
181
        if(!txd_complete) // transmission not completed
182
        {
183
                ptr_txd_buffer++;                    // die [0] wurde schon gesendet
184
                tmp_tx = txd_buffer[ptr_txd_buffer];
185
                // if terminating character or end of txd buffer was reached
186
                if((tmp_tx == '\r') || (ptr_txd_buffer == TXD_BUFFER_LEN))
187
                {
188
                        ptr_txd_buffer = 0; // reset txd pointer
189
                        txd_complete = 1; // stop transmission
190
                }
191
                UDR0 = tmp_tx; // send current byte will trigger this ISR again
192
        }
193
        // transmission completed
194
        else ptr_txd_buffer = 0;
195
}
1 ingob 196
 
886 killagreg 197
/****************************************************************/
198
/*               USART0 receiver ISR                            */
199
/****************************************************************/
200
ISR(USART0_RX_vect)
201
{
202
        static uint16_t crc;
203
        static uint8_t ptr_rxd_buffer = 0;
204
        uint8_t crc1, crc2;
205
        uint8_t c;
206
 
207
        c = UDR0;  // catch the received byte
208
 
908 killagreg 209
        #if defined (USE_KILLAGREG) || defined (USE_MK3MAG)
886 killagreg 210
        // If the FC 1.0 cpu is used the ublox module should be conneced to rxd of the 1st uart.
211
        // The FC 1.1 /1.2 has the ATMEGA644p cpu with a 2nd uart to which the ublox should be connected.
212
        #if defined (__AVR_ATmega644P__)
213
        if(BoardRelease == 10) ubx_parser(c);
214
        #else
215
        ubx_parser(c);
216
        #endif
908 killagreg 217
        #endif
886 killagreg 218
 
219
        if(rxd_buffer_locked) return; // if rxd buffer is locked immediately return
220
 
221
        // the rxd buffer is unlocked
222
        if((ptr_rxd_buffer == 0) && (c == '#')) // if rxd buffer is empty and syncronisation character is received
223
        {
224
                rxd_buffer[ptr_rxd_buffer++] = c; // copy 1st byte to buffer
225
                crc = c; // init crc
226
        }
227
        #if 0
228
        else if (ptr_rxd_buffer == 1) // handle address
229
        {
230
                rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer
231
                crc += c; // update crc
232
        }
233
        #endif
234
        else if (ptr_rxd_buffer < RXD_BUFFER_LEN) // collect incomming bytes
235
        {
236
                if(c != '\r') // no termination character
237
                {
238
                        rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer
239
                        crc += c; // update crc
240
                }
241
                else // termination character was received
242
                {
243
                        // the last 2 bytes are no subject for checksum calculation
244
                        // they are the checksum itself
245
                        crc -= rxd_buffer[ptr_rxd_buffer-2];
246
                        crc -= rxd_buffer[ptr_rxd_buffer-1];
247
                        // calculate checksum from transmitted data
248
                        crc %= 4096;
249
                        crc1 = '=' + crc / 64;
250
                        crc2 = '=' + crc % 64;
251
                        // compare checksum to transmitted checksum bytes
252
                        if((crc1 == rxd_buffer[ptr_rxd_buffer-2]) && (crc2 == rxd_buffer[ptr_rxd_buffer-1]))
253
                        {   // checksum valid
254
                                rxd_buffer_locked = TRUE;          // lock the rxd buffer
255
                                ReceivedBytes = ptr_rxd_buffer;    // store number of received bytes
256
                                rxd_buffer[ptr_rxd_buffer] = '\r'; // set termination character
257
                                // if 2nd byte is an 'R' enable watchdog that will result in an reset
258
                                if(rxd_buffer[2] == 'R') {wdt_enable(WDTO_250MS);} // Reset-Commando
259
                        }
260
                        else
261
                        {       // checksum invalid
262
                                rxd_buffer_locked = FALSE; // unlock rxd buffer
263
                        }
264
                        ptr_rxd_buffer = 0; // reset rxd buffer pointer
265
                }
266
        }
267
        else // rxd buffer overrun
268
        {
269
                ptr_rxd_buffer = 0; // reset rxd buffer
270
                rxd_buffer_locked = FALSE; // unlock rxd buffer
271
        }
272
 
1 ingob 273
}
274
 
275
 
276
// --------------------------------------------------------------------------
886 killagreg 277
void AddCRC(uint16_t datalen)
1 ingob 278
{
886 killagreg 279
        uint16_t tmpCRC = 0, i;
280
        for(i = 0; i < datalen; i++)
281
        {
282
                tmpCRC += txd_buffer[i];
283
        }
284
        tmpCRC %= 4096;
285
        txd_buffer[i++] = '=' + tmpCRC / 64;
286
        txd_buffer[i++] = '=' + tmpCRC % 64;
287
        txd_buffer[i++] = '\r';
288
        txd_complete = FALSE;
289
        UDR0 = txd_buffer[0]; // initiates the transmittion (continued in the TXD ISR)
1 ingob 290
}
291
 
292
 
293
 
294
// --------------------------------------------------------------------------
886 killagreg 295
void SendOutData(uint8_t cmd,uint8_t module, uint8_t *snd, uint8_t len)
1 ingob 296
{
886 killagreg 297
        uint16_t pt = 0;
298
        uint8_t a,b,c;
299
        uint8_t ptr = 0;
1 ingob 300
 
886 killagreg 301
        txd_buffer[pt++] = '#';         // Start character
302
        txd_buffer[pt++] = module;      // Address (a=0; b=1,...)
303
        txd_buffer[pt++] = cmd;         // Command
1 ingob 304
 
886 killagreg 305
        while(len)
306
        {
307
                if(len) { a = snd[ptr++]; len--;} else a = 0;
308
                if(len) { b = snd[ptr++]; len--;} else b = 0;
309
                if(len) { c = snd[ptr++]; len--;} else c = 0;
310
                txd_buffer[pt++] = '=' + (a >> 2);
311
                txd_buffer[pt++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4));
312
                txd_buffer[pt++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6));
313
                txd_buffer[pt++] = '=' + ( c & 0x3f);
314
        }
315
        AddCRC(pt); // add checksum after data block and initates the transmission
1 ingob 316
}
317
 
318
 
319
// --------------------------------------------------------------------------
886 killagreg 320
void Decode64(uint8_t *ptrOut, uint8_t len, uint8_t ptrIn, uint8_t max)
1 ingob 321
{
886 killagreg 322
        uint8_t a,b,c,d;
323
        uint8_t ptr = 0;
324
        uint8_t x,y,z;
325
        while(len)
326
        {
327
                a = rxd_buffer[ptrIn++] - '=';
328
                b = rxd_buffer[ptrIn++] - '=';
329
                c = rxd_buffer[ptrIn++] - '=';
330
                d = rxd_buffer[ptrIn++] - '=';
331
                if(ptrIn > max - 2) break;
1 ingob 332
 
886 killagreg 333
                x = (a << 2) | (b >> 4);
334
                y = ((b & 0x0f) << 4) | (c >> 2);
335
                z = ((c & 0x03) << 6) | d;
1 ingob 336
 
886 killagreg 337
                if(len--) ptrOut[ptr++] = x; else break;
338
                if(len--) ptrOut[ptr++] = y; else break;
339
                if(len--) ptrOut[ptr++] = z; else break;
340
        }
1 ingob 341
}
342
 
886 killagreg 343
 
1 ingob 344
// --------------------------------------------------------------------------
886 killagreg 345
void USART0_ProcessRxData(void)
1 ingob 346
{
886 killagreg 347
        // if data in the rxd buffer are not locked immediately return
348
        if(!rxd_buffer_locked) return;
1 ingob 349
 
886 killagreg 350
        #if  !defined (USE_KILLAGREG)  && !defined (USE_NAVICTRL)
351
        uint16_t tmp_int_arr1[1]; // local int buffer
352
        #endif
353
        uint8_t tmp_char_arr2[2]; // local char buffer
354
 
355
 
356
        switch(rxd_buffer[2])
357
        {
908 killagreg 358
                #ifdef USE_MK3MAG
886 killagreg 359
                case 'K':// Compass value
360
                        Decode64((uint8_t *) &tmp_int_arr1[0], sizeof(tmp_int_arr1), 3, ReceivedBytes);
361
                        CompassHeading = tmp_int_arr1[0];
362
                        CompassOffCourse = ((540 + CompassHeading - CompassCourse) % 360) - 180;
693 hbuss 363
                        break;
886 killagreg 364
                #endif
365
                case 'a':// Labels of the Analog Debug outputs
366
                        Decode64((uint8_t *) &tmp_char_arr2[0], sizeof(tmp_char_arr2), 3, ReceivedBytes);
367
                        RequestDebugLabel = tmp_char_arr2[0];
368
                        PcAccess = 255;
499 hbuss 369
                        break;
886 killagreg 370
                case 'b': // extern control
371
                        Decode64((uint8_t *) &ExternControl,sizeof(ExternControl), 3, ReceivedBytes);
372
                        RemoteButtons |= ExternControl.RemoteButtons;
373
                        ConfirmFrame = ExternControl.Frame;
936 killagreg 374
                        PcAccess = 255;
1 ingob 375
                        break;
886 killagreg 376
                case 'c': // extern control with debug request
377
                        Decode64((uint8_t *) &ExternControl,sizeof(ExternControl),3,ReceivedBytes);
378
                        RemoteButtons |= ExternControl.RemoteButtons;
379
                        ConfirmFrame = ExternControl.Frame;
380
                        RequestDebugData = TRUE;
381
                        PcAccess = 255;
1 ingob 382
                        break;
886 killagreg 383
                case 'h':// x-1 display columns
384
                        Decode64((uint8_t *) &tmp_char_arr2[0],sizeof(tmp_char_arr2),3,ReceivedBytes);
385
                        RemoteButtons |= tmp_char_arr2[0];
386
                        if(tmp_char_arr2[1] == 255) RequestChannelOnly = TRUE;
387
                        else RequestChannelOnly = FALSE; // keine Displaydaten
388
                        RequestDisplay = TRUE;
389
                        break;
390
                case 't':// motor test
391
                        Decode64((uint8_t *) &MotorTest[0],sizeof(MotorTest),3,ReceivedBytes);
392
                        PcAccess = 255;
393
                        break;
394
                case 'k':// keys from DubWise
395
                        Decode64((uint8_t *) &DubWiseKeys[0],sizeof(DubWiseKeys),3,ReceivedBytes);
595 hbuss 396
                        ConfirmFrame = DubWiseKeys[3];
886 killagreg 397
                        PcAccess = 255;
492 hbuss 398
                        break;
886 killagreg 399
                case 'v': // get version and board release
400
                        RequestVerInfo = TRUE;
401
                        break;
402
                case 'g':// get external control data
403
                        RequestExternalControl = TRUE;
404
                        break;
405
                case 'q':// get settings
406
                        Decode64((uint8_t *) &tmp_char_arr2[0],sizeof(tmp_char_arr2),3,ReceivedBytes);
407
                        while(!txd_complete);
408
                        if(tmp_char_arr2[0] != 0xff)
409
                        {
410
                                if(tmp_char_arr2[0] > 5) tmp_char_arr2[0] = 5; // limit to 5
411
                                // load requested parameter set
412
                                ParamSet_ReadFromEEProm(tmp_char_arr2[0]);
413
                                SendOutData('L' + tmp_char_arr2[0] -1,MySlaveAddr,(uint8_t *) &ParamSet.ChannelAssignment[0],PARAMSET_STRUCT_LEN);
414
                        }
415
                        else // send active parameter set
416
                        SendOutData('L' + GetParamByte(PID_ACTIVE_SET)-1,MySlaveAddr,(uint8_t *) &ParamSet.ChannelAssignment[0],PARAMSET_STRUCT_LEN);
417
 
418
                        break;
419
 
420
                case 'l':
421
                case 'm':
422
                case 'n':
423
                case 'o':
424
                case 'p': // save parameterset
425
                        Decode64((uint8_t *) &ParamSet.ChannelAssignment[0],PARAMSET_STRUCT_LEN,3,ReceivedBytes);
426
                        ParamSet_WriteToEEProm(rxd_buffer[2] - 'l' + 1);
911 killagreg 427
                        TurnOver180Nick = (int32_t) ParamSet.AngleTurnOverNick * 2500L;
886 killagreg 428
                        TurnOver180Roll = (int32_t) ParamSet.AngleTurnOverRoll * 2500L;
429
                        Beep(GetActiveParamSet());
430
                        break;
431
 
432
 
433
        }
434
        // unlock the rxd buffer after processing
435
        rxd_buffer_locked = FALSE;
1 ingob 436
}
437
 
438
//############################################################################
439
//Routine für die Serielle Ausgabe
886 killagreg 440
int16_t uart_putchar (int8_t c)
1 ingob 441
//############################################################################
442
{
443
        if (c == '\n')
444
                uart_putchar('\r');
886 killagreg 445
        // wait until previous character was send
446
        loop_until_bit_is_set(UCSR0A, UDRE0);
1 ingob 447
        //Ausgabe des Zeichens
886 killagreg 448
        UDR0 = c;
1 ingob 449
        return (0);
450
}
451
 
452
 
886 killagreg 453
//---------------------------------------------------------------------------------------------
454
void USART0_TransmitTxData(void)
1 ingob 455
{
886 killagreg 456
        if(!txd_complete) return;
1 ingob 457
 
886 killagreg 458
        if(RequestExternalControl && txd_complete)            // Bei Get werden die vom PC einstellbaren Werte vom PC zurückgelesen
459
        {
460
                SendOutData('G',MySlaveAddr,(uint8_t *) &ExternControl,sizeof(ExternControl));
461
                RequestExternalControl = FALSE;
462
        }
1 ingob 463
 
908 killagreg 464
        #ifdef USE_MK3MAG
886 killagreg 465
    if((CheckDelay(Compass_Timer)) && txd_complete)
466
        {
911 killagreg 467
                ToMk3Mag.Attitude[0] = (int16_t) (IntegralNick / 108);  // approx. 0,1 Deg
886 killagreg 468
                ToMk3Mag.Attitude[1] = (int16_t) (IntegralRoll / 108);   // approx. 0,1 Deg
469
                ToMk3Mag.UserParam[0] = FCParam.UserParam1;
470
                ToMk3Mag.UserParam[1] = FCParam.UserParam2;
471
                ToMk3Mag.CalState = CompassCalState;
472
                SendOutData('w',MySlaveAddr,(uint8_t *) &ToMk3Mag,sizeof(ToMk3Mag));
473
                // the last state is 5 and should be send only once to avoid multiple flash writing
474
                if(CompassCalState > 4)  CompassCalState = 0;
475
                Compass_Timer = SetDelay(99);
476
        }
477
        #endif
1 ingob 478
 
886 killagreg 479
    if((CheckDelay(Debug_Timer) || RequestDebugData) && txd_complete)
480
    {
481
          SendOutData('D',MySlaveAddr,(uint8_t *) &DebugOut,sizeof(DebugOut));
482
          RequestDebugData = FALSE;
483
          Debug_Timer = SetDelay(MIN_DEBUG_INTERVALL);
484
    }
1 ingob 485
 
886 killagreg 486
    if(RequestDebugLabel != 255) // Texte für die Analogdaten
499 hbuss 487
     {
886 killagreg 488
      SendOutData('A',RequestDebugLabel + '0',(uint8_t *) ANALOG_LABEL[RequestDebugLabel],16);
489
      RequestDebugLabel = 255;
499 hbuss 490
         }
886 killagreg 491
     if(ConfirmFrame && txd_complete)   // Datensatz ohne CRC bestätigen
595 hbuss 492
         {
886 killagreg 493
      txd_buffer[0] = '#';
494
      txd_buffer[1] = ConfirmFrame;
495
      txd_buffer[2] = '\r';
496
      txd_complete = 0;
595 hbuss 497
      ConfirmFrame = 0;
886 killagreg 498
      UDR0 = txd_buffer[0];
595 hbuss 499
     }
886 killagreg 500
     if(RequestDisplay && txd_complete)
1 ingob 501
         {
886 killagreg 502
      LCD_PrintMenu();
503
          RequestDisplay = FALSE;
504
      if(++RemotePollDisplayLine == 4 || RequestChannelOnly)
173 holgerb 505
      {
886 killagreg 506
       SendOutData('4',0,(uint8_t *)&PPM_in,sizeof(PPM_in));   // DisplayZeile übertragen
499 hbuss 507
       RemotePollDisplayLine = -1;
886 killagreg 508
      }
509
      else  SendOutData('0' + RemotePollDisplayLine,0,(uint8_t *)&DisplayBuff[20 * RemotePollDisplayLine],20);   // DisplayZeile übertragen
510
         }
511
    if(RequestVerInfo && txd_complete)
512
     {
513
      SendOutData('V',MySlaveAddr,(uint8_t *) &VersionInfo,sizeof(VersionInfo));
514
          RequestVerInfo = FALSE;
1 ingob 515
     }
516
 
517
}
518