Subversion Repositories FlightCtrl

Rev

Rev 2158 | Rev 2160 | 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         ",
2095 - 104
                "gyroActivity    ",
105
                "ca              ",
2089 - 106
                "GActivityDivider", //15
2074 - 107
                "NaviMode        ",
108
                "NaviStatus      ",
109
        "NaviStickP      ",
110
                "NaviStickR      ",
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        ",
2092 - 119
                "tolerance       ",
2089 - 120
                "Gyro Act Cont.  ",
2088 - 121
                "GPS altitude    ", //30
2079 - 122
                "GPS vert accura "
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
 
2158 - 480
                case 'n':// Read motor mixer
481
            tempchar[0] = EEMIXER_REVISION;
482
            tempchar[1] = sizeof(MotorMixer_t);
483
            while (!txd_complete)
484
                ; // wait for previous frame to be sent
2159 - 485
                        sendOutData('N', FC_ADDRESS, 2, &tempchar, 2, (uint8_t*)&motorMixer, sizeof(motorMixer));
1821 - 486
                        break;
1612 dongfang 487
 
1821 - 488
                case 'm':// "Set Mixer Table
2158 - 489
                        if (pRxData[0] == EEMIXER_REVISION && (pRxData[1] == sizeof(MotorMixer_t))) {
2159 - 490
                                memcpy(&motorMixer, (uint8_t*)&pRxData[2], sizeof(motorMixer));
2158 - 491
                                motorMixer_writeToEEProm();
1821 - 492
                                while (!txd_complete)
493
                                        ; // wait for previous frame to be sent
1980 - 494
                                tempchar[0] = 1;
1821 - 495
                        } else {
1980 - 496
                                tempchar[0] = 0;
1821 - 497
                        }
2018 - 498
                        sendOutData('M', FC_ADDRESS, 1, &tempchar, 1);
1821 - 499
                        break;
1612 dongfang 500
 
1821 - 501
                case 'p': // get PPM channels
502
                        request_PPMChannels = TRUE;
503
                        break;
1612 dongfang 504
 
2097 - 505
        case 'i':// Read IMU configuration
2092 - 506
            tempchar[0] = IMUCONFIG_REVISION;
507
            tempchar[1] = sizeof(IMUConfig);
508
            while (!txd_complete)
509
                ; // wait for previous frame to be sent
510
            sendOutData('I', FC_ADDRESS, 2, &tempchar, 2, (uint8_t *) &IMUConfig, sizeof(IMUConfig));
511
            break;
512
 
2097 - 513
        case 'j':// Save IMU configuration
514
          if (!(MKFlags & MKFLAG_MOTOR_RUN)) // save settings only if motors are off
515
          {
516
              if ((pRxData[0] == IMUCONFIG_REVISION) && (pRxData[1] == sizeof(IMUConfig))) {
517
                  memcpy(&IMUConfig, (uint8_t*) &pRxData[2], sizeof(IMUConfig));
518
                  IMUConfig_writeToEEprom();
519
                  tempchar[0] = 1; //indicate ok data
520
              } else {
521
                  tempchar[0] = 0; //indicate bad data
522
              }
523
              while (!txd_complete)
524
                  ; // wait for previous frame to be sent
525
              sendOutData('J', FC_ADDRESS, 1, &tempchar, 1);
526
          }
527
          break;
528
 
1821 - 529
                case 'q':// request settings
530
                        if (pRxData[0] == 0xFF) {
1960 - 531
                                pRxData[0] = getParamByte(PID_ACTIVE_SET);
1821 - 532
                        }
533
                        // limit settings range
534
                        if (pRxData[0] < 1)
535
                                pRxData[0] = 1; // limit to 1
536
                        else if (pRxData[0] > 5)
537
                                pRxData[0] = 5; // limit to 5
538
                        // load requested parameter set
2059 - 539
 
1960 - 540
                        paramSet_readFromEEProm(pRxData[0]);
2059 - 541
 
1980 - 542
                        tempchar[0] = pRxData[0];
543
                        tempchar[1] = EEPARAM_REVISION;
544
                        tempchar[2] = sizeof(staticParams);
1821 - 545
                        while (!txd_complete)
546
                                ; // wait for previous frame to be sent
2018 - 547
                        sendOutData('Q', FC_ADDRESS, 2, &tempchar, 3, (uint8_t *) &staticParams, sizeof(staticParams));
1821 - 548
                        break;
1612 dongfang 549
 
1821 - 550
                case 's': // save settings
551
                        if (!(MKFlags & MKFLAG_MOTOR_RUN)) // save settings only if motors are off
552
                        {
2051 - 553
                                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 - 554
                                {
2051 - 555
                                        memcpy(&staticParams, (uint8_t*) &pRxData[3], sizeof(staticParams));
1960 - 556
                                        paramSet_writeToEEProm(pRxData[0]);
2059 - 557
                                        setActiveParamSet(pRxData[0]);
558
                                        configuration_paramSetDidChange();
1980 - 559
                                        tempchar[0] = getActiveParamSet();
560
                                        beepNumber(tempchar[0]);
1821 - 561
                                } else {
1980 - 562
                                        tempchar[0] = 0; //indicate bad data
1821 - 563
                                }
564
                                while (!txd_complete)
565
                                        ; // wait for previous frame to be sent
2018 - 566
                                sendOutData('S', FC_ADDRESS, 1, &tempchar, 1);
1821 - 567
                        }
568
                        break;
1612 dongfang 569
 
1821 - 570
                default:
571
                        //unsupported command received
572
                        break;
573
                } // case FC_ADDRESS:
1612 dongfang 574
 
1821 - 575
        default: // any Slave Address
576
                switch (rxd_buffer[2]) {
577
                case 'a':// request for labels of the analog debug outputs
2055 - 578
                        requestedDebugLabel = pRxData[0];
579
                        if (requestedDebugLabel > 31)
580
                                requestedDebugLabel = 31;
1821 - 581
                        break;
1612 dongfang 582
 
1821 - 583
                case 'b': // submit extern control
584
                        memcpy(&externalControl, (uint8_t*) pRxData, sizeof(externalControl));
2018 - 585
                        confirmFrame = externalControl.frame;
1821 - 586
                        externalControlActive = 255;
587
                        break;
1612 dongfang 588
 
1821 - 589
                case 'h':// request for display columns
2018 - 590
                        remoteKeys |= pRxData[0];
591
                        if (remoteKeys)
592
                                displayLine = 0;
593
                        request_display = TRUE;
1821 - 594
                        break;
1612 dongfang 595
 
1821 - 596
                case 'l':// request for display columns
2018 - 597
                        menuItem = pRxData[0];
598
                        request_display1 = TRUE;
1821 - 599
                        break;
1612 dongfang 600
 
2055 - 601
                case 'o':// request for OSD data (FC style)
602
                  OSD_interval = (uint16_t) pRxData[0] * 10;
603
                  if (OSD_interval > 0)
604
                    request_OSD = TRUE;
605
                  break;
606
 
1821 - 607
                case 'v': // request for version and board release
2018 - 608
                        request_verInfo = TRUE;
1821 - 609
                        break;
1775 - 610
 
1821 - 611
                case 'x':
612
                        request_variables = TRUE;
613
                        break;
1612 dongfang 614
 
1821 - 615
                case 'g':// get external control data
2018 - 616
                        request_externalControl = TRUE;
1821 - 617
                        break;
1612 dongfang 618
 
1821 - 619
                case 'd': // request for the debug data
2018 - 620
                        debugData_interval = (uint16_t) pRxData[0] * 10;
621
                        if (debugData_interval > 0)
622
                                request_debugData = TRUE;
1821 - 623
                        break;
1612 dongfang 624
 
1821 - 625
                case 'c': // request for the 3D data
2018 - 626
                        data3D_interval = (uint16_t) pRxData[0] * 10;
627
                        if (data3D_interval > 0)
628
                                request_data3D = TRUE;
1821 - 629
                        break;
630
 
631
                default:
632
                        //unsupported command received
633
                        break;
634
                }
635
                break; // default:
636
        }
637
        // unlock the rxd buffer after processing
638
        pRxData = 0;
2018 - 639
        rxDataLen = 0;
1821 - 640
        rxd_buffer_locked = FALSE;
1612 dongfang 641
}
642
 
1645 - 643
/************************************************************************/
2035 - 644
/* Routine f�r die Serielle Ausgabe                                     */
1645 - 645
/************************************************************************/
1821 - 646
int16_t uart_putchar(int8_t c) {
647
        if (c == '\n')
648
                uart_putchar('\r');
649
        // wait until previous character was send
650
        loop_until_bit_is_set(UCSR0A, UDRE0);
651
        // send character
652
        UDR0 = c;
653
        return (0);
1612 dongfang 654
}
655
 
656
//---------------------------------------------------------------------------------------------
2018 - 657
void usart0_transmitTxData(void) {
1821 - 658
        if (!txd_complete)
659
                return;
1612 dongfang 660
 
2018 - 661
        if (request_verInfo && txd_complete) {
662
                sendOutData('V', FC_ADDRESS, 1, (uint8_t *) &versionInfo, sizeof(versionInfo));
663
                request_verInfo = FALSE;
1821 - 664
        }
1612 dongfang 665
 
2018 - 666
        if (request_display && txd_complete) {
667
                LCD_printMenu();
668
                sendOutData('H', FC_ADDRESS, 2, &displayLine, sizeof(displayLine),
669
                                &displayBuff[displayLine * 20], 20);
670
                displayLine++;
671
                if (displayLine >= 4)
672
                        displayLine = 0;
673
                request_display = FALSE;
1821 - 674
        }
1612 dongfang 675
 
2018 - 676
        if (request_display1 && txd_complete) {
677
                LCD_printMenu();
678
                sendOutData('L', FC_ADDRESS, 3, &menuItem, sizeof(menuItem), &maxMenuItem,
679
                                sizeof(maxMenuItem), displayBuff, sizeof(displayBuff));
680
                request_display1 = FALSE;
1821 - 681
        }
682
 
2055 - 683
        if (requestedDebugLabel != 0xFF && txd_complete) { // Texte f�r die Analogdaten
1821 - 684
                uint8_t label[16]; // local sram buffer
2055 - 685
                memcpy_P(label, ANALOG_LABEL[requestedDebugLabel], 16); // read lable from flash to sram buffer
686
                sendOutData('A', FC_ADDRESS, 2, (uint8_t *) &requestedDebugLabel,
687
                                sizeof(requestedDebugLabel), label, 16);
688
                requestedDebugLabel = 0xFF;
1821 - 689
        }
690
 
2035 - 691
        if (confirmFrame && txd_complete) { // Datensatz ohne checksum best�tigen
2018 - 692
                sendOutData('B', FC_ADDRESS, 1, (uint8_t*) &confirmFrame, sizeof(confirmFrame));
693
                confirmFrame = 0;
1821 - 694
        }
695
 
2018 - 696
        if (((debugData_interval && checkDelay(debugData_timer)) || request_debugData)
1821 - 697
                        && txd_complete) {
2018 - 698
                sendOutData('D', FC_ADDRESS, 1, (uint8_t *) &debugOut, sizeof(debugOut));
699
                debugData_timer = setDelay(debugData_interval);
700
                request_debugData = FALSE;
1821 - 701
        }
702
 
2055 - 703
        if (((data3D_interval && checkDelay(data3D_timer)) || request_data3D) && txd_complete) {
2018 - 704
                sendOutData('C', FC_ADDRESS, 1, (uint8_t *) &data3D, sizeof(data3D));
2055 - 705
                data3D.anglePitch = (int16_t) (attitude[PITCH] / (GYRO_DEG_FACTOR_PITCHROLL/10)); // convert to multiple of 0.1 deg
706
                data3D.angleRoll = (int16_t) (attitude[ROLL] / (GYRO_DEG_FACTOR_PITCHROLL/10)); // convert to multiple of 0.1 deg
707
                data3D.heading = (int16_t) (heading / (GYRO_DEG_FACTOR_YAW/10)); // convert to multiple of 0.1 deg
2018 - 708
                data3D_timer = setDelay(data3D_interval);
709
                request_data3D = FALSE;
1821 - 710
        }
711
 
2018 - 712
        if (request_externalControl && txd_complete) {
713
                sendOutData('G', FC_ADDRESS, 1, (uint8_t *) &externalControl,
1821 - 714
                                sizeof(externalControl));
2018 - 715
                request_externalControl = FALSE;
1821 - 716
        }
717
 
2039 - 718
#ifdef USE_DIRECT_GPS
719
        if((checkDelay(toMk3MagTimer)) && txd_complete) {
2048 - 720
                toMk3Mag.attitude[0] = (int16_t)(attitude[PITCH] / (GYRO_DEG_FACTOR_PITCHROLL/10)); // approx. 0.1 deg
721
                toMk3Mag.attitude[1] = (int16_t)(attitude[ROLL] / (GYRO_DEG_FACTOR_PITCHROLL/10)); // approx. 0.1 deg
2039 - 722
                toMk3Mag.userParam[0] = dynamicParams.userParams[0];
723
                toMk3Mag.userParam[1] = dynamicParams.userParams[1];
724
                toMk3Mag.calState = compassCalState;
2018 - 725
                sendOutData('w', MK3MAG_ADDRESS, 1,(uint8_t *) &toMk3Mag,sizeof(toMk3Mag));
1821 - 726
                // the last state is 5 and should be send only once to avoid multiple flash writing
727
                if(compassCalState > 4) compassCalState = 0;
2039 - 728
                toMk3MagTimer = setDelay(99);
1821 - 729
        }
1612 dongfang 730
#endif
731
 
2018 - 732
        if (request_motorTest && txd_complete) {
733
                sendOutData('T', FC_ADDRESS, 0);
734
                request_motorTest = FALSE;
1821 - 735
        }
1775 - 736
 
1821 - 737
        if (request_PPMChannels && txd_complete) {
2018 - 738
                sendOutData('P', FC_ADDRESS, 1, (uint8_t *) &PPM_in, sizeof(PPM_in));
1821 - 739
                request_PPMChannels = FALSE;
740
        }
741
 
742
        if (request_variables && txd_complete) {
2018 - 743
                sendOutData('X', FC_ADDRESS, 1, (uint8_t *) &variables, sizeof(variables));
1821 - 744
                request_variables = FALSE;
745
        }
2055 - 746
 
747
        if (((OSD_interval && checkDelay(OSD_timer)) || request_OSD) && txd_complete) {
748
          int32_t height = analog_getHeight();
749
          data3D.anglePitch = (int16_t) (attitude[PITCH] / (GYRO_DEG_FACTOR_PITCHROLL/10)); // convert to multiple of 0.1 deg
750
          data3D.angleRoll = (int16_t) (attitude[ROLL] / (GYRO_DEG_FACTOR_PITCHROLL/10)); // convert to multiple of 0.1 deg
751
          data3D.heading = (int16_t) (heading / (GYRO_DEG_FACTOR_YAW/10)); // convert to multiple of 0.1 deg
752
          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));
753
          OSD_timer = setDelay(OSD_interval);
754
          request_OSD = FALSE;
755
        }
1612 dongfang 756
}