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