Subversion Repositories FlightCtrl

Rev

Details | 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"
20
#ifdef USE_KILLAGREG
21
#include "ubx.h"
22
#endif
23
#if !defined(USE_KILLAGREG) && !defined (USE_NAVICTRL)
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
 
60
#if  !defined (USE_KILLAGREG)  && !defined (USE_NAVICTRL)
61
int16_t Compass_Timer;
62
#endif
63
 
64
 
65
const uint8_t ANALOG_LABEL[32][16] =
499 hbuss 66
{
886 killagreg 67
   //1234567890123456
68
    "IntegralPitch   ", //0
499 hbuss 69
    "IntegralRoll    ",
886 killagreg 70
    "AccPitch        ",
499 hbuss 71
    "AccRoll         ",
886 killagreg 72
    "GyroYaw         ",
73
    "ReadingHeight   ", //5
499 hbuss 74
    "AccZ            ",
886 killagreg 75
    "Thrust          ",
76
    "CompassHeading  ",
77
    "Voltage         ",
78
    "Receiver Level  ", //10
79
    "YawGyroHeading  ",
80
    "Motor_Front     ",
81
    "Motor_Rear      ",
82
    "Motor_Right     ",
83
    "Motor_Left      ", //15
513 hbuss 84
    "Acc_Z           ",
886 killagreg 85
    "SPI Error       ",
86
    "SPI Ok          ",
87
    "                ",
854 hbuss 88
    "Servo           ", //20
886 killagreg 89
    "Pitch           ",
854 hbuss 90
    "Roll            ",
720 ingob 91
    "                ",
92
    "                ",
93
    "                ", //25
94
    "                ",
95
    "                ",
96
    "                ",
97
    "                ",
886 killagreg 98
    "GPS_Pitch       ", //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
 
166
        #if  !defined (USE_KILLAGREG)  && !defined (USE_NAVICTRL)
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
 
209
        #ifdef USE_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
217
        #endif // USE_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
        {
358
                #if  !defined (USE_KILLAGREG)  && !defined (USE_NAVICTRL)
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;
1 ingob 374
                        break;
886 killagreg 375
                case 'c': // extern control with debug request
376
                        Decode64((uint8_t *) &ExternControl,sizeof(ExternControl),3,ReceivedBytes);
377
                        RemoteButtons |= ExternControl.RemoteButtons;
378
                        ConfirmFrame = ExternControl.Frame;
379
                        RequestDebugData = TRUE;
380
                        PcAccess = 255;
1 ingob 381
                        break;
886 killagreg 382
                case 'h':// x-1 display columns
383
                        Decode64((uint8_t *) &tmp_char_arr2[0],sizeof(tmp_char_arr2),3,ReceivedBytes);
384
                        RemoteButtons |= tmp_char_arr2[0];
385
                        if(tmp_char_arr2[1] == 255) RequestChannelOnly = TRUE;
386
                        else RequestChannelOnly = FALSE; // keine Displaydaten
387
                        RequestDisplay = TRUE;
388
                        break;
389
                case 't':// motor test
390
                        Decode64((uint8_t *) &MotorTest[0],sizeof(MotorTest),3,ReceivedBytes);
391
                        PcAccess = 255;
392
                        break;
393
                case 'k':// keys from DubWise
394
                        Decode64((uint8_t *) &DubWiseKeys[0],sizeof(DubWiseKeys),3,ReceivedBytes);
595 hbuss 395
                        ConfirmFrame = DubWiseKeys[3];
886 killagreg 396
                        PcAccess = 255;
492 hbuss 397
                        break;
886 killagreg 398
                case 'v': // get version and board release
399
                        RequestVerInfo = TRUE;
400
                        break;
401
                case 'g':// get external control data
402
                        RequestExternalControl = TRUE;
403
                        break;
404
                case 'q':// get settings
405
                        Decode64((uint8_t *) &tmp_char_arr2[0],sizeof(tmp_char_arr2),3,ReceivedBytes);
406
                        while(!txd_complete);
407
                        if(tmp_char_arr2[0] != 0xff)
408
                        {
409
                                if(tmp_char_arr2[0] > 5) tmp_char_arr2[0] = 5; // limit to 5
410
                                // load requested parameter set
411
                                ParamSet_ReadFromEEProm(tmp_char_arr2[0]);
412
                                SendOutData('L' + tmp_char_arr2[0] -1,MySlaveAddr,(uint8_t *) &ParamSet.ChannelAssignment[0],PARAMSET_STRUCT_LEN);
413
                        }
414
                        else // send active parameter set
415
                        SendOutData('L' + GetParamByte(PID_ACTIVE_SET)-1,MySlaveAddr,(uint8_t *) &ParamSet.ChannelAssignment[0],PARAMSET_STRUCT_LEN);
416
 
417
                        break;
418
 
419
                case 'l':
420
                case 'm':
421
                case 'n':
422
                case 'o':
423
                case 'p': // save parameterset
424
                        Decode64((uint8_t *) &ParamSet.ChannelAssignment[0],PARAMSET_STRUCT_LEN,3,ReceivedBytes);
425
                        ParamSet_WriteToEEProm(rxd_buffer[2] - 'l' + 1);
426
                        TurnOver180Pitch = (int32_t) ParamSet.AngleTurnOverPitch * 2500L;
427
                        TurnOver180Roll = (int32_t) ParamSet.AngleTurnOverRoll * 2500L;
428
                        Beep(GetActiveParamSet());
429
                        break;
430
 
431
 
432
        }
433
        // unlock the rxd buffer after processing
434
        rxd_buffer_locked = FALSE;
1 ingob 435
}
436
 
437
//############################################################################
438
//Routine für die Serielle Ausgabe
886 killagreg 439
int16_t uart_putchar (int8_t c)
1 ingob 440
//############################################################################
441
{
442
        if (c == '\n')
443
                uart_putchar('\r');
886 killagreg 444
        // wait until previous character was send
445
        loop_until_bit_is_set(UCSR0A, UDRE0);
1 ingob 446
        //Ausgabe des Zeichens
886 killagreg 447
        UDR0 = c;
1 ingob 448
        return (0);
449
}
450
 
451
 
886 killagreg 452
//---------------------------------------------------------------------------------------------
453
void USART0_TransmitTxData(void)
1 ingob 454
{
886 killagreg 455
        if(!txd_complete) return;
1 ingob 456
 
886 killagreg 457
        if(RequestExternalControl && txd_complete)            // Bei Get werden die vom PC einstellbaren Werte vom PC zurückgelesen
458
        {
459
                SendOutData('G',MySlaveAddr,(uint8_t *) &ExternControl,sizeof(ExternControl));
460
                RequestExternalControl = FALSE;
461
        }
1 ingob 462
 
886 killagreg 463
        #if  !defined (USE_KILLAGREG)  && !defined (USE_NAVICTRL)
464
    if((CheckDelay(Compass_Timer)) && txd_complete)
465
        {
466
                ToMk3Mag.Attitude[0] = (int16_t) (IntegralPitch / 108);  // approx. 0,1 Deg
467
                ToMk3Mag.Attitude[1] = (int16_t) (IntegralRoll / 108);   // approx. 0,1 Deg
468
                ToMk3Mag.UserParam[0] = FCParam.UserParam1;
469
                ToMk3Mag.UserParam[1] = FCParam.UserParam2;
470
                ToMk3Mag.CalState = CompassCalState;
471
                SendOutData('w',MySlaveAddr,(uint8_t *) &ToMk3Mag,sizeof(ToMk3Mag));
472
                // the last state is 5 and should be send only once to avoid multiple flash writing
473
                if(CompassCalState > 4)  CompassCalState = 0;
474
                Compass_Timer = SetDelay(99);
475
        }
476
        #endif
1 ingob 477
 
886 killagreg 478
    if((CheckDelay(Debug_Timer) || RequestDebugData) && txd_complete)
479
    {
480
          SendOutData('D',MySlaveAddr,(uint8_t *) &DebugOut,sizeof(DebugOut));
481
          RequestDebugData = FALSE;
482
          Debug_Timer = SetDelay(MIN_DEBUG_INTERVALL);
483
    }
1 ingob 484
 
886 killagreg 485
    if(RequestDebugLabel != 255) // Texte für die Analogdaten
499 hbuss 486
     {
886 killagreg 487
      SendOutData('A',RequestDebugLabel + '0',(uint8_t *) ANALOG_LABEL[RequestDebugLabel],16);
488
      RequestDebugLabel = 255;
499 hbuss 489
         }
886 killagreg 490
     if(ConfirmFrame && txd_complete)   // Datensatz ohne CRC bestätigen
595 hbuss 491
         {
886 killagreg 492
      txd_buffer[0] = '#';
493
      txd_buffer[1] = ConfirmFrame;
494
      txd_buffer[2] = '\r';
495
      txd_complete = 0;
595 hbuss 496
      ConfirmFrame = 0;
886 killagreg 497
      UDR0 = txd_buffer[0];
595 hbuss 498
     }
886 killagreg 499
     if(RequestDisplay && txd_complete)
1 ingob 500
         {
886 killagreg 501
      LCD_PrintMenu();
502
          RequestDisplay = FALSE;
503
      if(++RemotePollDisplayLine == 4 || RequestChannelOnly)
173 holgerb 504
      {
886 killagreg 505
       SendOutData('4',0,(uint8_t *)&PPM_in,sizeof(PPM_in));   // DisplayZeile übertragen
499 hbuss 506
       RemotePollDisplayLine = -1;
886 killagreg 507
      }
508
      else  SendOutData('0' + RemotePollDisplayLine,0,(uint8_t *)&DisplayBuff[20 * RemotePollDisplayLine],20);   // DisplayZeile übertragen
509
         }
510
    if(RequestVerInfo && txd_complete)
511
     {
512
      SendOutData('V',MySlaveAddr,(uint8_t *) &VersionInfo,sizeof(VersionInfo));
513
          RequestVerInfo = FALSE;
1 ingob 514
     }
515
 
516
}
517