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
513 hbuss 65
    "Analog11        ",
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.
189
        if(BoardRelease == 10) ubx_parser(c);
737 killagreg 190
 
191
        if(rxd_buffer_locked) return; // if txd buffer is locked immediately return
192
 
193
        // the rxd buffer is unlocked
194
        if((ptr_rxd_buffer == 0) && (c == '#')) // if rxd buffer is empty and syncronisation character is received
735 killagreg 195
        {
737 killagreg 196
                rxd_buffer[ptr_rxd_buffer++] = c; // copy 1st byte to buffer
197
                crc = c; // init crc
198
        }
199
        #if 0
200
        else if (ptr_rxd_buffer == 1) // handle address
201
        {
202
                rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer
203
                crc += c; // update crc
204
        }
205
        #endif
206
        else if (ptr_rxd_buffer < RXD_BUFFER_LEN) // collect incomming bytes
207
        {
208
                if(c != '\r') // no termination character
735 killagreg 209
                {
737 killagreg 210
                        rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer
211
                        crc += c; // update crc
735 killagreg 212
                }
737 killagreg 213
                else // termination character was received
214
                {
215
                        // the last 2 bytes are no subject for checksum calculation
216
                        // they are the checksum itself
217
                        crc -= rxd_buffer[ptr_rxd_buffer-2];
218
                        crc -= rxd_buffer[ptr_rxd_buffer-1];
219
                        // calculate checksum from transmitted data
220
                        crc %= 4096;
221
                        crc1 = '=' + crc / 64;
222
                        crc2 = '=' + crc % 64;
223
                        // compare checksum to transmitted checksum bytes
224
                        if((crc1 == rxd_buffer[ptr_rxd_buffer-2]) && (crc2 == rxd_buffer[ptr_rxd_buffer-1]))
225
                        {   // checksum valid
226
                                rxd_buffer_locked = TRUE;          // lock the rxd buffer
227
                                ReceivedBytes = ptr_rxd_buffer;    // store number of received bytes
228
                                rxd_buffer[ptr_rxd_buffer] = '\r'; // set termination character
229
                                // if 2nd byte is an 'R' enable watchdog that will result in an reset
745 killagreg 230
                                if(rxd_buffer[2] == 'R') {wdt_enable(WDTO_250MS);} // Reset-Commando
737 killagreg 231
                        }
232
                        else
233
                        {       // checksum invalid
234
                                rxd_buffer_locked = FALSE; // unlock rxd buffer
235
                        }
236
                        ptr_rxd_buffer = 0; // reset txd buffer
237
                }
238
        } // buffer overrun
735 killagreg 239
        else
240
        {
737 killagreg 241
                ptr_rxd_buffer = 0; // reset rxd buffer
242
                rxd_buffer_locked = FALSE; // unlock rxd buffer
735 killagreg 243
        }
737 killagreg 244
 
1 ingob 245
}
246
 
247
 
248
// --------------------------------------------------------------------------
735 killagreg 249
void AddCRC(uint16_t datalen)
1 ingob 250
{
683 killagreg 251
 uint16_t tmpCRC = 0,i;
735 killagreg 252
 for(i = 0; i < datalen;i++)
1 ingob 253
  {
683 killagreg 254
   tmpCRC += txd_buffer[i];
1 ingob 255
  }
256
   tmpCRC %= 4096;
683 killagreg 257
   txd_buffer[i++] = '=' + tmpCRC / 64;
258
   txd_buffer[i++] = '=' + tmpCRC % 64;
259
   txd_buffer[i++] = '\r';
735 killagreg 260
  txd_complete = 0;
261
  UDR0 = txd_buffer[0]; // initiates the transmittion
1 ingob 262
}
263
 
264
 
265
 
266
// --------------------------------------------------------------------------
745 killagreg 267
void SendOutData(uint8_t cmd,uint8_t module, uint8_t *snd, uint8_t len)
1 ingob 268
{
735 killagreg 269
        uint16_t pt = 0;
270
        uint8_t a,b,c;
271
        uint8_t ptr = 0;
1 ingob 272
 
735 killagreg 273
        txd_buffer[pt++] = '#';         // Start character
745 killagreg 274
        txd_buffer[pt++] = module;      // Address (a=0; b=1,...)
735 killagreg 275
        txd_buffer[pt++] = cmd;         // Command
1 ingob 276
 
735 killagreg 277
        while(len)
278
        {
279
                if(len) { a = snd[ptr++]; len--;} else a = 0;
280
                if(len) { b = snd[ptr++]; len--;} else b = 0;
281
                if(len) { c = snd[ptr++]; len--;} else c = 0;
282
                txd_buffer[pt++] = '=' + (a >> 2);
283
                txd_buffer[pt++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4));
284
                txd_buffer[pt++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6));
285
                txd_buffer[pt++] = '=' + ( c & 0x3f);
286
        }
287
        AddCRC(pt); // add checksum after data block and initates the transmission
1 ingob 288
}
289
 
290
 
291
// --------------------------------------------------------------------------
745 killagreg 292
void Decode64(uint8_t *ptrOut, uint8_t len, uint8_t ptrIn, uint8_t max)
1 ingob 293
{
735 killagreg 294
        uint8_t a,b,c,d;
295
        uint8_t ptr = 0;
296
        uint8_t x,y,z;
297
        while(len)
298
        {
299
                a = rxd_buffer[ptrIn++] - '=';
300
                b = rxd_buffer[ptrIn++] - '=';
301
                c = rxd_buffer[ptrIn++] - '=';
302
                d = rxd_buffer[ptrIn++] - '=';
745 killagreg 303
                if(ptrIn > max - 2) break;
1 ingob 304
 
735 killagreg 305
                x = (a << 2) | (b >> 4);
306
                y = ((b & 0x0f) << 4) | (c >> 2);
307
                z = ((c & 0x03) << 6) | d;
1 ingob 308
 
735 killagreg 309
                if(len--) ptrOut[ptr++] = x; else break;
310
                if(len--) ptrOut[ptr++] = y; else break;
311
                if(len--) ptrOut[ptr++] = z; else break;
312
        }
1 ingob 313
}
314
 
735 killagreg 315
 
1 ingob 316
// --------------------------------------------------------------------------
735 killagreg 317
void ProcessRxData(void)
1 ingob 318
{
737 killagreg 319
        // if data in the rxd buffer are not locked immediately return
320
        if(!rxd_buffer_locked) return;
1 ingob 321
 
735 killagreg 322
        uint8_t tmp_char_arr2[2]; // local buffer
683 killagreg 323
 
735 killagreg 324
        PcAccess = 255;
325
        switch(rxd_buffer[2])
326
        {
327
                case 'a':// Labels of the Analog Deboug outputs
328
                        Decode64((uint8_t *) &tmp_char_arr2[0], sizeof(tmp_char_arr2), 3, ReceivedBytes);
329
                        DebugTextAnforderung = tmp_char_arr2[0];
499 hbuss 330
                        break;
735 killagreg 331
                case 'b': // extern control
332
                        Decode64((uint8_t *) &ExternControl,sizeof(ExternControl), 3, ReceivedBytes);
685 killagreg 333
                        RemoteButtons |= ExternControl.RemoteButtons;
735 killagreg 334
                        ConfirmFrame = ExternControl.Frame;
335
                        break;
336
                case 'c': // extern control with debug request
337
                        Decode64((uint8_t *) &ExternControl,sizeof(ExternControl),3,ReceivedBytes);
685 killagreg 338
                        RemoteButtons |= ExternControl.RemoteButtons;
735 killagreg 339
                        ConfirmFrame = ExternControl.Frame;
745 killagreg 340
                        DebugDataRequest = 1;
735 killagreg 341
                        break;
342
                case 'h':// x-1 display columns
343
                        Decode64((uint8_t *) &tmp_char_arr2[0],sizeof(tmp_char_arr2),3,ReceivedBytes);
344
                        RemoteButtons |= tmp_char_arr2[0];
345
                        if(tmp_char_arr2[1] == 255) NurKanalAnforderung = 1;
346
                        else NurKanalAnforderung = 0; // keine Displaydaten
745 killagreg 347
                        DebugDisplayRequest = 1;
1 ingob 348
                        break;
735 killagreg 349
                case 't':// motor test
350
                        Decode64((uint8_t *) &MotorTest[0],sizeof(MotorTest),3,ReceivedBytes);
1 ingob 351
                        break;
735 killagreg 352
                case 'k':// keys from DubWise
353
                        Decode64((uint8_t *) &DubWiseKeys[0],sizeof(DubWiseKeys),3,ReceivedBytes);
595 hbuss 354
                        ConfirmFrame = DubWiseKeys[3];
492 hbuss 355
                        break;
735 killagreg 356
                case 'v': // get version and board release
745 killagreg 357
                        GetVersionRequest = 1;
735 killagreg 358
                        break;
359
                case 'g':// get debug data
745 killagreg 360
                        DebugGetRequest = 1;
735 killagreg 361
                        break;
362
                case 'q':// get settings
363
                        Decode64((uint8_t *) &tmp_char_arr2[0],sizeof(tmp_char_arr2),3,ReceivedBytes);
364
                        if(tmp_char_arr2[0] != 0xff)
365
                        {
366
                                if(tmp_char_arr2[0] > 5) tmp_char_arr2[0] = 5; // limit to 5
367
                                // load requested parameter set
368
                                ParamSet_ReadFromEEProm(tmp_char_arr2[0]);
369
                                SendOutData('L' + tmp_char_arr2[0] -1,MySlaveAddr,(uint8_t *) &ParamSet.ChannelAssignment[0],PARAMSET_STRUCT_LEN);
370
                        }
371
                        else // send active parameter set
372
                        SendOutData('L' + GetParamByte(PID_ACTIVE_SET)-1,MySlaveAddr,(uint8_t *) &ParamSet.ChannelAssignment[0],PARAMSET_STRUCT_LEN);
683 killagreg 373
 
735 killagreg 374
                        break;
683 killagreg 375
 
735 killagreg 376
                case 'l':
377
                case 'm':
378
                case 'n':
379
                case 'o':
380
                case 'p': // save parameterset
381
                        Decode64((uint8_t *) &ParamSet.ChannelAssignment[0],PARAMSET_STRUCT_LEN,3,ReceivedBytes);
687 killagreg 382
                        ParamSet_WriteToEEProm(rxd_buffer[2] - 'l' + 1);
735 killagreg 383
                        TurnOver180Pitch = (int32_t) ParamSet.AngleTurnOverPitch * 2500L;
384
                        TurnOver180Roll = (int32_t) ParamSet.AngleTurnOverRoll * 2500L;
385
                        Beep(GetActiveParamSet());
386
                        break;
683 killagreg 387
 
388
 
735 killagreg 389
        }
737 killagreg 390
        // unlock the rxd buffer after processing
391
        rxd_buffer_locked = FALSE;
1 ingob 392
}
393
 
394
//############################################################################
395
//Routine für die Serielle Ausgabe
683 killagreg 396
int16_t uart_putchar (int8_t c)
1 ingob 397
//############################################################################
398
{
399
        if (c == '\n')
400
                uart_putchar('\r');
735 killagreg 401
        // wait until previous character was send
683 killagreg 402
        loop_until_bit_is_set(UCSR0A, UDRE0);
1 ingob 403
        //Ausgabe des Zeichens
683 killagreg 404
        UDR0 = c;
405
 
1 ingob 406
        return (0);
407
}
408
 
409
 
410
//---------------------------------------------------------------------------------------------
735 killagreg 411
void TransmitTxData(void)
1 ingob 412
{
735 killagreg 413
 static int16_t Debug_Timer = 0;
414
 if(!txd_complete) return;
1 ingob 415
 
745 killagreg 416
   if(DebugGetRequest && txd_complete)        // Bei Get werden die vom PC einstellbaren Werte vom PC zurückgelesen
683 killagreg 417
   {
735 killagreg 418
      SendOutData('G',MySlaveAddr,(uint8_t *) &ExternControl,sizeof(ExternControl));
745 killagreg 419
          DebugGetRequest = 0;
1 ingob 420
   }
421
 
745 killagreg 422
    if((CheckDelay(Debug_Timer) || DebugDataRequest) && txd_complete)
1 ingob 423
         {
735 killagreg 424
          SendOutData('D',MySlaveAddr,(uint8_t *) &DebugOut,sizeof(DebugOut));
745 killagreg 425
          DebugDataRequest = 0;
683 killagreg 426
          Debug_Timer = SetDelay(MIN_DEBUG_INTERVALL);
427
         }
499 hbuss 428
    if(DebugTextAnforderung != 255) // Texte für die Analogdaten
429
     {
683 killagreg 430
      SendOutData('A',DebugTextAnforderung + '0',(uint8_t *) ANALOG_TEXT[DebugTextAnforderung],16);
499 hbuss 431
      DebugTextAnforderung = 255;
432
         }
735 killagreg 433
     if(ConfirmFrame && txd_complete)   // Datensatz ohne CRC bestätigen
595 hbuss 434
         {
683 killagreg 435
      txd_buffer[0] = '#';
436
      txd_buffer[1] = ConfirmFrame;
437
      txd_buffer[2] = '\r';
735 killagreg 438
      txd_complete = 0;
595 hbuss 439
      ConfirmFrame = 0;
683 killagreg 440
      UDR0 = txd_buffer[0];
595 hbuss 441
     }
745 killagreg 442
     if(DebugDisplayRequest && txd_complete)
1 ingob 443
         {
685 killagreg 444
      LCD_PrintMenu();
745 killagreg 445
          DebugDisplayRequest = 0;
683 killagreg 446
      if(++RemotePollDisplayLine == 4 || NurKanalAnforderung)
173 holgerb 447
      {
683 killagreg 448
       SendOutData('4',0,(uint8_t *)&PPM_in,sizeof(PPM_in));   // DisplayZeile übertragen
499 hbuss 449
       RemotePollDisplayLine = -1;
683 killagreg 450
      }
451
      else  SendOutData('0' + RemotePollDisplayLine,0,(uint8_t *)&DisplayBuff[20 * RemotePollDisplayLine],20);   // DisplayZeile übertragen
452
         }
745 killagreg 453
    if(GetVersionRequest && txd_complete)
683 killagreg 454
     {
735 killagreg 455
      SendOutData('V',MySlaveAddr,(uint8_t *) &VersionInfo,sizeof(VersionInfo));
745 killagreg 456
          GetVersionRequest = 0;
1 ingob 457
     }
458
 
459
}
460