Subversion Repositories FlightCtrl

Rev

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