Subversion Repositories FlightCtrl

Rev

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

Rev Author Line No. Line
1612 dongfang 1
#include <avr/io.h>
2
#include <avr/interrupt.h>
3
#include <avr/wdt.h>
4
#include <avr/pgmspace.h>
5
#include <stdarg.h>
6
#include <string.h>
7
 
8
#include "eeprom.h"
9
#include "menu.h"
10
#include "timer0.h"
11
#include "uart0.h"
12
#include "rc.h"
13
#include "externalControl.h"
1775 - 14
#include "output.h"
1864 - 15
#include "attitude.h"
2048 - 16
#include "commands.h"
1612 dongfang 17
 
2039 - 18
#ifdef USE_DIRECT_GPS
1612 dongfang 19
#include "mk3mag.h"
20
#endif
21
 
22
#define FC_ADDRESS 1
23
#define NC_ADDRESS 2
24
#define MK3MAG_ADDRESS 3
25
 
26
#define FALSE   0
27
#define TRUE    1
2055 - 28
 
29
uint8_t requestedDebugLabel = 255;
30
 
2018 - 31
uint8_t request_verInfo = FALSE;
32
uint8_t request_externalControl = FALSE;
33
uint8_t request_display = FALSE;
34
uint8_t request_display1 = FALSE;
35
uint8_t request_debugData = FALSE;
36
uint8_t request_data3D = FALSE;
1821 - 37
uint8_t request_PPMChannels = FALSE;
2018 - 38
uint8_t request_motorTest = FALSE;
1821 - 39
uint8_t request_variables = FALSE;
2055 - 40
uint8_t request_OSD = FALSE;
1775 - 41
 
2055 - 42
/*
43
#define request_verInfo         (1<<0)
44
#define request_externalControl (1<<1)
45
#define request_display         (1<<3)
46
#define request_display1        (1<<4)
47
#define request_debugData       (1<<5)
48
#define request_data3D          (1<<6)
49
#define request_PPMChannels     (1<<7)
50
#define request_motorTest       (1<<8)
51
#define request_variables       (1<<9)
52
#define request_OSD             (1<<10)
53
*/
54
 
55
//uint16_t request = 0;
56
 
2018 - 57
uint8_t displayLine = 0;
1612 dongfang 58
 
59
volatile uint8_t txd_buffer[TXD_BUFFER_LEN];
60
volatile uint8_t rxd_buffer_locked = FALSE;
61
volatile uint8_t rxd_buffer[RXD_BUFFER_LEN];
62
volatile uint8_t txd_complete = TRUE;
2018 - 63
volatile uint8_t receivedBytes = 0;
1612 dongfang 64
volatile uint8_t *pRxData = 0;
2018 - 65
volatile uint8_t rxDataLen = 0;
1612 dongfang 66
 
1821 - 67
uint8_t motorTestActive = 0;
68
uint8_t motorTest[16] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
2018 - 69
uint8_t confirmFrame;
1612 dongfang 70
 
71
typedef struct {
2039 - 72
        int16_t heading;
1821 - 73
}__attribute__((packed)) Heading_t;
1612 dongfang 74
 
2018 - 75
Data3D_t data3D;
1612 dongfang 76
 
2018 - 77
uint16_t debugData_timer;
78
uint16_t data3D_timer;
2055 - 79
uint16_t OSD_timer;
2018 - 80
uint16_t debugData_interval = 0; // in 1ms
81
uint16_t data3D_interval = 0; // in 1ms
2055 - 82
uint16_t OSD_interval = 0;
1612 dongfang 83
 
2039 - 84
#ifdef USE_DIRECT_GPS
85
int16_t toMk3MagTimer;
1612 dongfang 86
#endif
87
 
88
// keep lables in flash to save 512 bytes of sram space
89
const prog_uint8_t ANALOG_LABEL[32][16] = {
1821 - 90
                //1234567890123456
91
                "AnglePitch      ", //0
92
                "AngleRoll       ",
93
                "AngleYaw        ",
1974 - 94
                "GyroPitch       ",
95
                "GyroRoll        ",
96
                "GyroYaw         ", //5
2055 - 97
                "PitchTerm       ",
98
                "RollTerm        ",
99
                "ThrottleTerm    ",
100
                "YawTerm         ",
2073 - 101
                "heightP         ", //10
102
                "heightI         ",
103
        "heightD         ",
104
                "heightErr/10    ",
2059 - 105
                "controlActivity ",
106
                "accVector       ", //15
2073 - 107
                "ppm 0           ",
108
                "ppm 1           ",
109
        "ppm 2           ",
110
                "ppm 3           ",
2069 - 111
                "control act wghd", //20
112
                "acc vector wghd ",
2058 - 113
                "Height[dm]      ",
114
                "dHeight         ",
2069 - 115
                "acc vector      ",
2059 - 116
                "EFT             ", //25
2058 - 117
                "naviPitch       ",
118
                "naviRoll        ",
119
                "i part contrib  ",
2069 - 120
                "Acc Z           ",
2073 - 121
                "Height D        ", //30
122
                "RCCommand       "
2045 - 123
  };
1612 dongfang 124
 
125
/****************************************************************/
126
/*              Initialization of the USART0                    */
127
/****************************************************************/
2018 - 128
void usart0_init(void) {
1821 - 129
        uint8_t sreg = SREG;
130
        uint16_t ubrr = (uint16_t) ((uint32_t) SYSCLK / (8 * USART0_BAUD) - 1);
131
 
132
        // disable all interrupts before configuration
133
        cli();
134
 
135
        // disable RX-Interrupt
136
        UCSR0B &= ~(1 << RXCIE0);
137
        // disable TX-Interrupt
138
        UCSR0B &= ~(1 << TXCIE0);
139
 
140
        // set direction of RXD0 and TXD0 pins
141
        // set RXD0 (PD0) as an input pin
142
        PORTD |= (1 << PORTD0);
143
        DDRD &= ~(1 << DDD0);
144
        // set TXD0 (PD1) as an output pin
145
        PORTD |= (1 << PORTD1);
146
        DDRD |= (1 << DDD1);
147
 
148
        // USART0 Baud Rate Register
149
        // set clock divider
150
        UBRR0H = (uint8_t) (ubrr >> 8);
151
        UBRR0L = (uint8_t) ubrr;
152
 
153
        // USART0 Control and Status Register A, B, C
154
 
155
        // enable double speed operation in
156
        UCSR0A |= (1 << U2X0);
157
        // enable receiver and transmitter in
158
        UCSR0B = (1 << TXEN0) | (1 << RXEN0);
159
        // set asynchronous mode
160
        UCSR0C &= ~(1 << UMSEL01);
161
        UCSR0C &= ~(1 << UMSEL00);
162
        // no parity
163
        UCSR0C &= ~(1 << UPM01);
164
        UCSR0C &= ~(1 << UPM00);
165
        // 1 stop bit
166
        UCSR0C &= ~(1 << USBS0);
167
        // 8-bit
168
        UCSR0B &= ~(1 << UCSZ02);
169
        UCSR0C |= (1 << UCSZ01);
170
        UCSR0C |= (1 << UCSZ00);
171
 
172
        // flush receive buffer
173
        while (UCSR0A & (1 << RXC0))
174
                UDR0;
175
 
176
        // enable interrupts at the end
177
        // enable RX-Interrupt
178
        UCSR0B |= (1 << RXCIE0);
179
        // enable TX-Interrupt
180
        UCSR0B |= (1 << TXCIE0);
181
 
182
        // initialize the debug timer
2018 - 183
        debugData_timer = setDelay(debugData_interval);
1821 - 184
 
185
        // unlock rxd_buffer
186
        rxd_buffer_locked = FALSE;
187
        pRxData = 0;
2018 - 188
        rxDataLen = 0;
1821 - 189
 
190
        // no bytes to send
191
        txd_complete = TRUE;
192
 
2039 - 193
#ifdef USE_DIRECT_GPS
194
        toMk3MagTimer = setDelay(220);
1612 dongfang 195
#endif
1821 - 196
 
2018 - 197
        versionInfo.SWMajor = VERSION_MAJOR;
198
        versionInfo.SWMinor = VERSION_MINOR;
199
        versionInfo.SWPatch = VERSION_PATCH;
200
        versionInfo.protoMajor = VERSION_SERIAL_MAJOR;
201
        versionInfo.protoMinor = VERSION_SERIAL_MINOR;
1821 - 202
 
203
        // restore global interrupt flags
204
        SREG = sreg;
1612 dongfang 205
}
206
 
207
/****************************************************************/
208
/* USART0 transmitter ISR                                       */
209
/****************************************************************/
2018 - 210
ISR(USART0_TX_vect) {
1821 - 211
        static uint16_t ptr_txd_buffer = 0;
212
        uint8_t tmp_tx;
213
        if (!txd_complete) { // transmission not completed
214
                ptr_txd_buffer++; // die [0] wurde schon gesendet
215
                tmp_tx = txd_buffer[ptr_txd_buffer];
216
                // if terminating character or end of txd buffer was reached
217
                if ((tmp_tx == '\r') || (ptr_txd_buffer == TXD_BUFFER_LEN)) {
218
                        ptr_txd_buffer = 0; // reset txd pointer
219
                        txd_complete = 1; // stop transmission
220
                }
221
                UDR0 = tmp_tx; // send current byte will trigger this ISR again
222
        }
223
        // transmission completed
224
        else
225
                ptr_txd_buffer = 0;
1612 dongfang 226
}
227
 
228
/****************************************************************/
229
/* USART0 receiver               ISR                            */
230
/****************************************************************/
2018 - 231
ISR(USART0_RX_vect) {
1969 - 232
        static uint16_t checksum;
1821 - 233
        static uint8_t ptr_rxd_buffer = 0;
1969 - 234
        uint8_t checksum1, checksum2;
1821 - 235
        uint8_t c;
1612 dongfang 236
 
1821 - 237
        c = UDR0; // catch the received byte
1612 dongfang 238
 
1821 - 239
        if (rxd_buffer_locked)
240
                return; // if rxd buffer is locked immediately return
1612 dongfang 241
 
1821 - 242
        // the rxd buffer is unlocked
243
        if ((ptr_rxd_buffer == 0) && (c == '#')) { // if rxd buffer is empty and syncronisation character is received
244
                rxd_buffer[ptr_rxd_buffer++] = c; // copy 1st byte to buffer
1969 - 245
                checksum = c; // init checksum
1821 - 246
        }
247
        else if (ptr_rxd_buffer < RXD_BUFFER_LEN) { // collect incomming bytes
248
                if (c != '\r') { // no termination character
249
                        rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer
1969 - 250
                        checksum += c; // update checksum
1821 - 251
                } else { // termination character was received
252
                        // the last 2 bytes are no subject for checksum calculation
253
                        // they are the checksum itself
1969 - 254
                        checksum -= rxd_buffer[ptr_rxd_buffer - 2];
255
                        checksum -= rxd_buffer[ptr_rxd_buffer - 1];
1821 - 256
                        // calculate checksum from transmitted data
1969 - 257
                        checksum %= 4096;
258
                        checksum1 = '=' + checksum / 64;
259
                        checksum2 = '=' + checksum % 64;
1821 - 260
                        // compare checksum to transmitted checksum bytes
1969 - 261
                        if ((checksum1 == rxd_buffer[ptr_rxd_buffer - 2]) && (checksum2
1821 - 262
                                        == rxd_buffer[ptr_rxd_buffer - 1])) {
263
                                // checksum valid
264
                                rxd_buffer[ptr_rxd_buffer] = '\r'; // set termination character
2018 - 265
                                receivedBytes = ptr_rxd_buffer + 1;// store number of received bytes
1821 - 266
                                rxd_buffer_locked = TRUE; // lock the rxd buffer
267
                                // if 2nd byte is an 'R' enable watchdog that will result in an reset
268
                                if (rxd_buffer[2] == 'R') {
269
                                        wdt_enable(WDTO_250MS);
270
                                } // Reset-Commando
271
                        } else { // checksum invalid
272
                                rxd_buffer_locked = FALSE; // unlock rxd buffer
273
                        }
274
                        ptr_rxd_buffer = 0; // reset rxd buffer pointer
275
                }
276
        } else { // rxd buffer overrun
277
                ptr_rxd_buffer = 0; // reset rxd buffer
278
                rxd_buffer_locked = FALSE; // unlock rxd buffer
279
        }
1612 dongfang 280
}
281
 
282
// --------------------------------------------------------------------------
2055 - 283
void addChecksum(uint16_t datalen) {
1969 - 284
        uint16_t tmpchecksum = 0, i;
1821 - 285
        for (i = 0; i < datalen; i++) {
1969 - 286
                tmpchecksum += txd_buffer[i];
1821 - 287
        }
1969 - 288
        tmpchecksum %= 4096;
2059 - 289
        txd_buffer[i++] = '=' + (tmpchecksum >> 6);
290
        txd_buffer[i++] = '=' + (tmpchecksum & 0x3F);
1821 - 291
        txd_buffer[i++] = '\r';
292
        txd_complete = FALSE;
293
        UDR0 = txd_buffer[0]; // initiates the transmittion (continued in the TXD ISR)
1612 dongfang 294
}
295
 
296
// --------------------------------------------------------------------------
1775 - 297
// application example:
2018 - 298
// sendOutData('A', FC_ADDRESS, 2, (uint8_t *)&request_DebugLabel, sizeof(request_DebugLabel), label, 16);
1775 - 299
/*
2018 - 300
 void sendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) { // uint8_t *pdata, uint8_t len, ...
1821 - 301
 va_list ap;
302
 uint16_t txd_bufferIndex = 0;
303
 uint8_t *currentBuffer;
304
 uint8_t currentBufferIndex;
305
 uint16_t lengthOfCurrentBuffer;
306
 uint8_t shift = 0;
1775 - 307
 
1821 - 308
 txd_buffer[txd_bufferIndex++] = '#';                   // Start character
309
 txd_buffer[txd_bufferIndex++] = 'a' + addr;            // Address (a=0; b=1,...)
310
 txd_buffer[txd_bufferIndex++] = cmd;                   // Command
1775 - 311
 
1821 - 312
 va_start(ap, numofbuffers);
313
 
314
 while(numofbuffers) {
315
 currentBuffer = va_arg(ap, uint8_t*);
316
 lengthOfCurrentBuffer = va_arg(ap, int);
317
 currentBufferIndex = 0;
318
 // Encode data: 3 bytes of data are encoded into 4 bytes,
319
 // where the 2 most significant bits are both 0.
320
 while(currentBufferIndex != lengthOfCurrentBuffer) {
321
 if (!shift) txd_buffer[txd_bufferIndex] = 0;
322
 txd_buffer[txd_bufferIndex]  |= currentBuffer[currentBufferIndex] >> (shift + 2);
323
 txd_buffer[++txd_bufferIndex] = (currentBuffer[currentBufferIndex] << (4 - shift)) & 0b00111111;
324
 shift += 2;
325
 if (shift == 6) { shift=0; txd_bufferIndex++; }
326
 currentBufferIndex++;
327
 }
328
 }
329
 // If the number of data bytes was not divisible by 3, stuff
330
 //  with 0 pseudodata  until length is again divisible by 3.
331
 if (shift == 2) {
332
 // We need to stuff with zero bytes at the end.
333
 txd_buffer[txd_bufferIndex]  &= 0b00110000;
334
 txd_buffer[++txd_bufferIndex] = 0;
335
 shift = 4;
336
 }
337
 if (shift == 4) {
338
 // We need to stuff with zero bytes at the end.
339
 txd_buffer[txd_bufferIndex++] &= 0b00111100;
340
 txd_buffer[txd_bufferIndex]    = 0;
341
 }
342
 va_end(ap);
1969 - 343
 Addchecksum(pt); // add checksum after data block and initates the transmission
1821 - 344
 }
345
 */
346
 
2018 - 347
void sendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) { // uint8_t *pdata, uint8_t len, ...
1821 - 348
        va_list ap;
349
        uint16_t pt = 0;
350
        uint8_t a, b, c;
351
        uint8_t ptr = 0;
1612 dongfang 352
 
1821 - 353
        uint8_t *pdata = 0;
354
        int len = 0;
1612 dongfang 355
 
1821 - 356
        txd_buffer[pt++] = '#'; // Start character
357
        txd_buffer[pt++] = 'a' + addr; // Address (a=0; b=1,...)
358
        txd_buffer[pt++] = cmd; // Command
359
 
360
        va_start(ap, numofbuffers);
361
 
362
        if (numofbuffers) {
363
                pdata = va_arg(ap, uint8_t*);
364
                len = va_arg(ap, int);
365
                ptr = 0;
366
                numofbuffers--;
367
        }
368
 
369
        while (len) {
370
                if (len) {
371
                        a = pdata[ptr++];
372
                        len--;
373
                        if ((!len) && numofbuffers) {
374
                                pdata = va_arg(ap, uint8_t*);
375
                                len = va_arg(ap, int);
376
                                ptr = 0;
377
                                numofbuffers--;
378
                        }
379
                } else
380
                        a = 0;
381
                if (len) {
382
                        b = pdata[ptr++];
383
                        len--;
384
                        if ((!len) && numofbuffers) {
385
                                pdata = va_arg(ap, uint8_t*);
386
                                len = va_arg(ap, int);
387
                                ptr = 0;
388
                                numofbuffers--;
389
                        }
390
                } else
391
                        b = 0;
392
                if (len) {
393
                        c = pdata[ptr++];
394
                        len--;
395
                        if ((!len) && numofbuffers) {
396
                                pdata = va_arg(ap, uint8_t*);
397
                                len = va_arg(ap, int);
398
                                ptr = 0;
399
                                numofbuffers--;
400
                        }
401
                } else
402
                        c = 0;
403
                txd_buffer[pt++] = '=' + (a >> 2);
404
                txd_buffer[pt++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4));
405
                txd_buffer[pt++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6));
406
                txd_buffer[pt++] = '=' + (c & 0x3f);
407
        }
408
        va_end(ap);
2055 - 409
        addChecksum(pt); // add checksum after data block and initates the transmission
1612 dongfang 410
}
411
 
412
// --------------------------------------------------------------------------
2055 - 413
void decode64(void) {
1821 - 414
        uint8_t a, b, c, d;
415
        uint8_t x, y, z;
416
        uint8_t ptrIn = 3;
417
        uint8_t ptrOut = 3;
2018 - 418
        uint8_t len = receivedBytes - 6;
1821 - 419
 
420
        while (len) {
421
                a = rxd_buffer[ptrIn++] - '=';
422
                b = rxd_buffer[ptrIn++] - '=';
423
                c = rxd_buffer[ptrIn++] - '=';
424
                d = rxd_buffer[ptrIn++] - '=';
425
                //if(ptrIn > ReceivedBytes - 3) break;
426
 
427
                x = (a << 2) | (b >> 4);
428
                y = ((b & 0x0f) << 4) | (c >> 2);
429
                z = ((c & 0x03) << 6) | d;
430
 
431
                if (len--)
432
                        rxd_buffer[ptrOut++] = x;
433
                else
434
                        break;
435
                if (len--)
436
                        rxd_buffer[ptrOut++] = y;
437
                else
438
                        break;
439
                if (len--)
440
                        rxd_buffer[ptrOut++] = z;
441
                else
442
                        break;
443
        }
444
        pRxData = &rxd_buffer[3];
2018 - 445
        rxDataLen = ptrOut - 3;
1612 dongfang 446
}
447
 
448
// --------------------------------------------------------------------------
2018 - 449
void usart0_processRxData(void) {
1821 - 450
        // We control the motorTestActive var from here: Count it down.
451
        if (motorTestActive)
452
                motorTestActive--;
453
        // if data in the rxd buffer are not locked immediately return
454
        if (!rxd_buffer_locked)
455
                return;
1980 - 456
        uint8_t tempchar[3];
2055 - 457
        decode64(); // decode data block in rxd_buffer
1775 - 458
 
1821 - 459
        switch (rxd_buffer[1] - 'a') {
1775 - 460
 
1821 - 461
        case FC_ADDRESS:
462
                switch (rxd_buffer[2]) {
2039 - 463
#ifdef USE_DIRECT_GPS
1821 - 464
                case 'K':// compass value
2041 - 465
                  // What is the point of this - the compass will overwrite this soon?
466
                magneticHeading = ((Heading_t *)pRxData)->heading;
1821 - 467
                // compassOffCourse = ((540 + compassHeading - compassCourse) % 360) - 180;
468
                break;
1612 dongfang 469
#endif
1821 - 470
                case 't': // motor test
2018 - 471
                        if (rxDataLen > 20) {
1821 - 472
                                memcpy(&motorTest[0], (uint8_t*) pRxData, sizeof(motorTest));
473
                        } else {
474
                                memcpy(&motorTest[0], (uint8_t*) pRxData, 4);
475
                        }
476
                        motorTestActive = 255;
477
                        externalControlActive = 255;
478
                        break;
1612 dongfang 479
 
1821 - 480
                case 'n':// "Get Mixer Table
481
                        while (!txd_complete)
482
                                ; // wait for previous frame to be sent
2018 - 483
                        sendOutData('N', FC_ADDRESS, 1, (uint8_t *) &mixerMatrix, sizeof(mixerMatrix));
1821 - 484
                        break;
1612 dongfang 485
 
1821 - 486
                case 'm':// "Set Mixer Table
487
                        if (pRxData[0] == EEMIXER_REVISION) {
1960 - 488
                                memcpy(&mixerMatrix, (uint8_t*) pRxData, sizeof(mixerMatrix));
489
                                mixerMatrix_writeToEEProm();
1821 - 490
                                while (!txd_complete)
491
                                        ; // wait for previous frame to be sent
1980 - 492
                                tempchar[0] = 1;
1821 - 493
                        } else {
1980 - 494
                                tempchar[0] = 0;
1821 - 495
                        }
2018 - 496
                        sendOutData('M', FC_ADDRESS, 1, &tempchar, 1);
1821 - 497
                        break;
1612 dongfang 498
 
1821 - 499
                case 'p': // get PPM channels
500
                        request_PPMChannels = TRUE;
501
                        break;
1612 dongfang 502
 
1821 - 503
                case 'q':// request settings
504
                        if (pRxData[0] == 0xFF) {
1960 - 505
                                pRxData[0] = getParamByte(PID_ACTIVE_SET);
1821 - 506
                        }
507
                        // limit settings range
508
                        if (pRxData[0] < 1)
509
                                pRxData[0] = 1; // limit to 1
510
                        else if (pRxData[0] > 5)
511
                                pRxData[0] = 5; // limit to 5
512
                        // load requested parameter set
2059 - 513
 
1960 - 514
                        paramSet_readFromEEProm(pRxData[0]);
2059 - 515
 
1980 - 516
                        tempchar[0] = pRxData[0];
517
                        tempchar[1] = EEPARAM_REVISION;
518
                        tempchar[2] = sizeof(staticParams);
1821 - 519
                        while (!txd_complete)
520
                                ; // wait for previous frame to be sent
2018 - 521
                        sendOutData('Q', FC_ADDRESS, 2, &tempchar, 3, (uint8_t *) &staticParams, sizeof(staticParams));
1821 - 522
                        break;
1612 dongfang 523
 
1821 - 524
                case 's': // save settings
525
                        if (!(MKFlags & MKFLAG_MOTOR_RUN)) // save settings only if motors are off
526
                        {
2051 - 527
                                if ((1 <= pRxData[0]) && (pRxData[0] <= 5) && (pRxData[1] == EEPARAM_REVISION) && (pRxData[2] == sizeof(staticParams))) // check for setting to be in range and version of settings
1821 - 528
                                {
2051 - 529
                                        memcpy(&staticParams, (uint8_t*) &pRxData[3], sizeof(staticParams));
1960 - 530
                                        paramSet_writeToEEProm(pRxData[0]);
2059 - 531
                                        setActiveParamSet(pRxData[0]);
532
                                        configuration_paramSetDidChange();
1980 - 533
                                        tempchar[0] = getActiveParamSet();
534
                                        beepNumber(tempchar[0]);
1821 - 535
                                } else {
1980 - 536
                                        tempchar[0] = 0; //indicate bad data
1821 - 537
                                }
538
                                while (!txd_complete)
539
                                        ; // wait for previous frame to be sent
2018 - 540
                                sendOutData('S', FC_ADDRESS, 1, &tempchar, 1);
1821 - 541
                        }
542
                        break;
1612 dongfang 543
 
1821 - 544
                default:
545
                        //unsupported command received
546
                        break;
547
                } // case FC_ADDRESS:
1612 dongfang 548
 
1821 - 549
        default: // any Slave Address
550
                switch (rxd_buffer[2]) {
551
                case 'a':// request for labels of the analog debug outputs
2055 - 552
                        requestedDebugLabel = pRxData[0];
553
                        if (requestedDebugLabel > 31)
554
                                requestedDebugLabel = 31;
1821 - 555
                        break;
1612 dongfang 556
 
1821 - 557
                case 'b': // submit extern control
558
                        memcpy(&externalControl, (uint8_t*) pRxData, sizeof(externalControl));
2018 - 559
                        confirmFrame = externalControl.frame;
1821 - 560
                        externalControlActive = 255;
561
                        break;
1612 dongfang 562
 
1821 - 563
                case 'h':// request for display columns
2018 - 564
                        remoteKeys |= pRxData[0];
565
                        if (remoteKeys)
566
                                displayLine = 0;
567
                        request_display = TRUE;
1821 - 568
                        break;
1612 dongfang 569
 
1821 - 570
                case 'l':// request for display columns
2018 - 571
                        menuItem = pRxData[0];
572
                        request_display1 = TRUE;
1821 - 573
                        break;
1612 dongfang 574
 
2055 - 575
                case 'o':// request for OSD data (FC style)
576
                  OSD_interval = (uint16_t) pRxData[0] * 10;
577
                  if (OSD_interval > 0)
578
                    request_OSD = TRUE;
579
                  break;
580
 
1821 - 581
                case 'v': // request for version and board release
2018 - 582
                        request_verInfo = TRUE;
1821 - 583
                        break;
1775 - 584
 
1821 - 585
                case 'x':
586
                        request_variables = TRUE;
587
                        break;
1612 dongfang 588
 
1821 - 589
                case 'g':// get external control data
2018 - 590
                        request_externalControl = TRUE;
1821 - 591
                        break;
1612 dongfang 592
 
1821 - 593
                case 'd': // request for the debug data
2018 - 594
                        debugData_interval = (uint16_t) pRxData[0] * 10;
595
                        if (debugData_interval > 0)
596
                                request_debugData = TRUE;
1821 - 597
                        break;
1612 dongfang 598
 
1821 - 599
                case 'c': // request for the 3D data
2018 - 600
                        data3D_interval = (uint16_t) pRxData[0] * 10;
601
                        if (data3D_interval > 0)
602
                                request_data3D = TRUE;
1821 - 603
                        break;
604
 
605
                default:
606
                        //unsupported command received
607
                        break;
608
                }
609
                break; // default:
610
        }
611
        // unlock the rxd buffer after processing
612
        pRxData = 0;
2018 - 613
        rxDataLen = 0;
1821 - 614
        rxd_buffer_locked = FALSE;
1612 dongfang 615
}
616
 
1645 - 617
/************************************************************************/
2035 - 618
/* Routine f�r die Serielle Ausgabe                                     */
1645 - 619
/************************************************************************/
1821 - 620
int16_t uart_putchar(int8_t c) {
621
        if (c == '\n')
622
                uart_putchar('\r');
623
        // wait until previous character was send
624
        loop_until_bit_is_set(UCSR0A, UDRE0);
625
        // send character
626
        UDR0 = c;
627
        return (0);
1612 dongfang 628
}
629
 
630
//---------------------------------------------------------------------------------------------
2018 - 631
void usart0_transmitTxData(void) {
1821 - 632
        if (!txd_complete)
633
                return;
1612 dongfang 634
 
2018 - 635
        if (request_verInfo && txd_complete) {
636
                sendOutData('V', FC_ADDRESS, 1, (uint8_t *) &versionInfo, sizeof(versionInfo));
637
                request_verInfo = FALSE;
1821 - 638
        }
1612 dongfang 639
 
2018 - 640
        if (request_display && txd_complete) {
641
                LCD_printMenu();
642
                sendOutData('H', FC_ADDRESS, 2, &displayLine, sizeof(displayLine),
643
                                &displayBuff[displayLine * 20], 20);
644
                displayLine++;
645
                if (displayLine >= 4)
646
                        displayLine = 0;
647
                request_display = FALSE;
1821 - 648
        }
1612 dongfang 649
 
2018 - 650
        if (request_display1 && txd_complete) {
651
                LCD_printMenu();
652
                sendOutData('L', FC_ADDRESS, 3, &menuItem, sizeof(menuItem), &maxMenuItem,
653
                                sizeof(maxMenuItem), displayBuff, sizeof(displayBuff));
654
                request_display1 = FALSE;
1821 - 655
        }
656
 
2055 - 657
        if (requestedDebugLabel != 0xFF && txd_complete) { // Texte f�r die Analogdaten
1821 - 658
                uint8_t label[16]; // local sram buffer
2055 - 659
                memcpy_P(label, ANALOG_LABEL[requestedDebugLabel], 16); // read lable from flash to sram buffer
660
                sendOutData('A', FC_ADDRESS, 2, (uint8_t *) &requestedDebugLabel,
661
                                sizeof(requestedDebugLabel), label, 16);
662
                requestedDebugLabel = 0xFF;
1821 - 663
        }
664
 
2035 - 665
        if (confirmFrame && txd_complete) { // Datensatz ohne checksum best�tigen
2018 - 666
                sendOutData('B', FC_ADDRESS, 1, (uint8_t*) &confirmFrame, sizeof(confirmFrame));
667
                confirmFrame = 0;
1821 - 668
        }
669
 
2018 - 670
        if (((debugData_interval && checkDelay(debugData_timer)) || request_debugData)
1821 - 671
                        && txd_complete) {
2018 - 672
                sendOutData('D', FC_ADDRESS, 1, (uint8_t *) &debugOut, sizeof(debugOut));
673
                debugData_timer = setDelay(debugData_interval);
674
                request_debugData = FALSE;
1821 - 675
        }
676
 
2055 - 677
        if (((data3D_interval && checkDelay(data3D_timer)) || request_data3D) && txd_complete) {
2018 - 678
                sendOutData('C', FC_ADDRESS, 1, (uint8_t *) &data3D, sizeof(data3D));
2055 - 679
                data3D.anglePitch = (int16_t) (attitude[PITCH] / (GYRO_DEG_FACTOR_PITCHROLL/10)); // convert to multiple of 0.1 deg
680
                data3D.angleRoll = (int16_t) (attitude[ROLL] / (GYRO_DEG_FACTOR_PITCHROLL/10)); // convert to multiple of 0.1 deg
681
                data3D.heading = (int16_t) (heading / (GYRO_DEG_FACTOR_YAW/10)); // convert to multiple of 0.1 deg
2018 - 682
                data3D_timer = setDelay(data3D_interval);
683
                request_data3D = FALSE;
1821 - 684
        }
685
 
2018 - 686
        if (request_externalControl && txd_complete) {
687
                sendOutData('G', FC_ADDRESS, 1, (uint8_t *) &externalControl,
1821 - 688
                                sizeof(externalControl));
2018 - 689
                request_externalControl = FALSE;
1821 - 690
        }
691
 
2039 - 692
#ifdef USE_DIRECT_GPS
693
        if((checkDelay(toMk3MagTimer)) && txd_complete) {
2048 - 694
                toMk3Mag.attitude[0] = (int16_t)(attitude[PITCH] / (GYRO_DEG_FACTOR_PITCHROLL/10)); // approx. 0.1 deg
695
                toMk3Mag.attitude[1] = (int16_t)(attitude[ROLL] / (GYRO_DEG_FACTOR_PITCHROLL/10)); // approx. 0.1 deg
2039 - 696
                toMk3Mag.userParam[0] = dynamicParams.userParams[0];
697
                toMk3Mag.userParam[1] = dynamicParams.userParams[1];
698
                toMk3Mag.calState = compassCalState;
2018 - 699
                sendOutData('w', MK3MAG_ADDRESS, 1,(uint8_t *) &toMk3Mag,sizeof(toMk3Mag));
1821 - 700
                // the last state is 5 and should be send only once to avoid multiple flash writing
701
                if(compassCalState > 4) compassCalState = 0;
2039 - 702
                toMk3MagTimer = setDelay(99);
1821 - 703
        }
1612 dongfang 704
#endif
705
 
2018 - 706
        if (request_motorTest && txd_complete) {
707
                sendOutData('T', FC_ADDRESS, 0);
708
                request_motorTest = FALSE;
1821 - 709
        }
1775 - 710
 
1821 - 711
        if (request_PPMChannels && txd_complete) {
2018 - 712
                sendOutData('P', FC_ADDRESS, 1, (uint8_t *) &PPM_in, sizeof(PPM_in));
1821 - 713
                request_PPMChannels = FALSE;
714
        }
715
 
716
        if (request_variables && txd_complete) {
2018 - 717
                sendOutData('X', FC_ADDRESS, 1, (uint8_t *) &variables, sizeof(variables));
1821 - 718
                request_variables = FALSE;
719
        }
2055 - 720
 
721
        if (((OSD_interval && checkDelay(OSD_timer)) || request_OSD) && txd_complete) {
722
          int32_t height = analog_getHeight();
723
          data3D.anglePitch = (int16_t) (attitude[PITCH] / (GYRO_DEG_FACTOR_PITCHROLL/10)); // convert to multiple of 0.1 deg
724
          data3D.angleRoll = (int16_t) (attitude[ROLL] / (GYRO_DEG_FACTOR_PITCHROLL/10)); // convert to multiple of 0.1 deg
725
          data3D.heading = (int16_t) (heading / (GYRO_DEG_FACTOR_YAW/10)); // convert to multiple of 0.1 deg
726
          sendOutData('O', FC_ADDRESS, 4, (uint8_t*)&data3D, sizeof(data3D), (uint8_t*)&GPSInfo, sizeof(GPSInfo), (uint8_t*)&height, sizeof(height), (uint8_t*)UBat, sizeof(UBat));
727
          OSD_timer = setDelay(OSD_interval);
728
          request_OSD = FALSE;
729
        }
1612 dongfang 730
}