Subversion Repositories FlightCtrl

Rev

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