Subversion Repositories FlightCtrl

Rev

Rev 1539 | Go to most recent revision | Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
1538 killagreg 1
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2
// + Copyright (c) Holger Buss, Ingo Busker
3
// + Nur für den privaten Gebrauch
4
// + www.MikroKopter.com
5
// + porting the sources to other systems or using the software on other systems (except hardware from www.mikrokopter.de) is not allowed
6
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
7
// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),
8
// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist.
9
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
10
// + bzgl. der Nutzungsbedingungen aufzunehmen.
11
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
12
// + Verkauf von Luftbildaufnahmen, usw.
13
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
14
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,
15
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen
16
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
17
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
18
// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
19
// + eindeutig als Ursprung verlinkt werden
20
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
21
// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
22
// + Benutzung auf eigene Gefahr
23
// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
24
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
25
// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
26
// + mit unserer Zustimmung zulässig
27
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
28
// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
29
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
30
// + Redistributions of source code (with or without modifications) must retain the above copyright notice,
31
// + this list of conditions and the following disclaimer.
32
// +   * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
33
// +     from this software without specific prior written permission.
34
// +   * The use of this project (hardware, software, binary files, sources and documentation) is only permittet
35
// +     for non-commercial use (directly or indirectly)
36
// +     Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
37
// +     with our written permission
38
// +   * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
39
// +     clearly linked as origin
40
// +   * porting to systems other than hardware from www.mikrokopter.de is not allowed
41
// +  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
42
// +  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
43
// +  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
44
// +  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
45
// +  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
46
// +  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
47
// +  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
48
// +  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
49
// +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
50
// +  POSSIBILITY OF SUCH DAMAGE.
51
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
52
#include <avr/io.h>
53
#include <avr/interrupt.h>
54
#include <avr/wdt.h>
55
#include <avr/pgmspace.h>
56
#include <stdarg.h>
57
#include <string.h>
58
 
59
#include "eeprom.h"
60
#include "main.h"
61
#include "menu.h"
62
#include "timer0.h"
63
#include "timer2.h"
64
#include "uart0.h"
65
#include "fc.h"
66
#include "rc.h"
67
#if defined (USE_KILLAGREG) || defined (USE_MK3MAG)
68
#include "ubx.h"
69
#endif
70
#ifdef USE_MK3MAG
71
#include "mk3mag.h"
72
#endif
73
 
74
 
75
#define FC_ADDRESS 1
76
#define NC_ADDRESS 2
77
#define MK3MAG_ADDRESS 3
78
 
79
#define FALSE   0
80
#define TRUE    1
81
 
82
//int8_t test __attribute__ ((section (".noinit")));
83
uint8_t Request_VerInfo                 = FALSE;
84
uint8_t Request_ExternalControl = FALSE;
85
uint8_t Request_Display                 = FALSE;
86
uint8_t Request_Display1                = FALSE;
87
uint8_t Request_DebugData               = FALSE;
88
uint8_t Request_Data3D                  = FALSE;
89
uint8_t Request_DebugLabel              = 255;
90
uint8_t Request_PPMChannels     = FALSE;
91
uint8_t Request_MotorTest               = FALSE;
92
uint8_t DisplayLine = 0;
93
 
94
volatile uint8_t txd_buffer[TXD_BUFFER_LEN];
95
volatile uint8_t rxd_buffer_locked = FALSE;
96
volatile uint8_t rxd_buffer[RXD_BUFFER_LEN];
97
volatile uint8_t txd_complete = TRUE;
98
volatile uint8_t ReceivedBytes = 0;
99
volatile uint8_t *pRxData = 0;
100
volatile uint8_t RxDataLen = 0;
101
 
102
uint8_t PcAccess = 100;
103
 
104
uint8_t MotorTest_Active  = 0;
105
uint8_t MotorTest[16] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
106
uint8_t ConfirmFrame;
107
 
108
typedef struct
109
{
110
        int16_t Heading;
111
} __attribute__((packed)) Heading_t;
112
 
113
DebugOut_t              DebugOut;
114
Data3D_t                Data3D;
115
ExternControl_t ExternControl;
116
UART_VersionInfo_t      UART_VersionInfo;
117
 
118
uint16_t DebugData_Timer;
119
uint16_t Data3D_Timer;
120
uint16_t DebugData_Interval = 500; // in 1ms
121
uint16_t Data3D_Interval = 0; // in 1ms
122
 
123
#ifdef USE_MK3MAG
124
int16_t Compass_Timer;
125
#endif
126
 
127
// keep lables in flash to save 512 bytes of sram space
128
const prog_uint8_t ANALOG_LABEL[32][16] =
129
{
130
   //1234567890123456
131
    "AngleNick       ", //0
132
    "AngleRoll       ",
133
    "AccNick         ",
134
    "AccRoll         ",
135
    "YawGyro         ",
136
    "Height Value    ", //5
137
    "AccZ            ",
138
    "Gas             ",
139
    "Compass Heading ",
140
    "Voltage         ",
141
    "Receiver Level  ", //10
142
    "YawGyro Heading ",
143
    "Motor Front     ",
144
    "Motor Rear      ",
145
    "Motor Left      ",
146
    "Motor Right     ", //15
147
    "                ",
148
    "                ",
149
    "VarioMeter      ",
150
    "MK3MAG CalState ",
151
    "NickServo       ", //20
152
    "Hoovergas       ",
153
    "                ",
154
    "                ",
155
    "                ",
156
    "                ", //25
157
    "                ",
158
    "                ",
159
    "I2C-Error       ",
160
    "                ",
161
    "GPS Nick        ", //30
162
    "GPS Roll        "
163
};
164
 
165
 
166
 
167
/****************************************************************/
168
/*              Initialization of the USART0                    */
169
/****************************************************************/
170
void USART0_Init (void)
171
{
172
        uint8_t sreg = SREG;
173
        uint16_t ubrr = (uint16_t) ((uint32_t) SYSCLK/(8 * USART0_BAUD) - 1);
174
 
175
        // disable all interrupts before configuration
176
        cli();
177
 
178
        // disable RX-Interrupt
179
        UCSR0B &= ~(1 << RXCIE0);
180
        // disable TX-Interrupt
181
        UCSR0B &= ~(1 << TXCIE0);
182
 
183
        // set direction of RXD0 and TXD0 pins
184
        // set RXD0 (PD0) as an input pin
185
        PORTD |= (1 << PORTD0);
186
        DDRD &= ~(1 << DDD0);
187
        // set TXD0 (PD1) as an output pin
188
        PORTD |= (1 << PORTD1);
189
        DDRD |=  (1 << DDD1);
190
 
191
        // USART0 Baud Rate Register
192
        // set clock divider
193
        UBRR0H = (uint8_t)(ubrr >> 8);
194
        UBRR0L = (uint8_t)ubrr;
195
 
196
        // USART0 Control and Status Register A, B, C
197
 
198
        // enable double speed operation in
199
        UCSR0A |= (1 << U2X0);
200
        // enable receiver and transmitter in
201
        UCSR0B = (1 << TXEN0) | (1 << RXEN0);
202
        // set asynchronous mode
203
        UCSR0C &= ~(1 << UMSEL01);
204
        UCSR0C &= ~(1 << UMSEL00);
205
        // no parity
206
        UCSR0C &= ~(1 << UPM01);
207
        UCSR0C &= ~(1 << UPM00);
208
        // 1 stop bit
209
        UCSR0C &= ~(1 << USBS0);
210
        // 8-bit
211
        UCSR0B &= ~(1 << UCSZ02);
212
        UCSR0C |=  (1 << UCSZ01);
213
        UCSR0C |=  (1 << UCSZ00);
214
 
215
                // flush receive buffer
216
        while ( UCSR0A & (1<<RXC0) ) UDR0;
217
 
218
        // enable interrupts at the end
219
        // enable RX-Interrupt
220
        UCSR0B |= (1 << RXCIE0);
221
        // enable TX-Interrupt
222
        UCSR0B |= (1 << TXCIE0);
223
 
224
        // initialize the debug timer
225
        DebugData_Timer = SetDelay(DebugData_Interval);
226
 
227
        // unlock rxd_buffer
228
        rxd_buffer_locked = FALSE;
229
        pRxData = 0;
230
        RxDataLen = 0;
231
 
232
        // no bytes to send
233
        txd_complete = TRUE;
234
 
235
        #ifdef USE_MK3MAG
236
        Compass_Timer = SetDelay(220);
237
        #endif
238
 
239
        UART_VersionInfo.SWMajor = VERSION_MAJOR;
240
        UART_VersionInfo.SWMinor = VERSION_MINOR;
241
        UART_VersionInfo.SWPatch = VERSION_PATCH;
242
        UART_VersionInfo.ProtoMajor = VERSION_SERIAL_MAJOR;
243
        UART_VersionInfo.ProtoMinor = VERSION_SERIAL_MINOR;
244
 
245
        // restore global interrupt flags
246
    SREG = sreg;
247
}
248
 
249
/****************************************************************/
250
/*               USART0 transmitter ISR                         */
251
/****************************************************************/
252
ISR(USART0_TX_vect)
253
{
254
        static uint16_t ptr_txd_buffer = 0;
255
        uint8_t tmp_tx;
256
        if(!txd_complete) // transmission not completed
257
        {
258
                ptr_txd_buffer++;                    // die [0] wurde schon gesendet
259
                tmp_tx = txd_buffer[ptr_txd_buffer];
260
                // if terminating character or end of txd buffer was reached
261
                if((tmp_tx == '\r') || (ptr_txd_buffer == TXD_BUFFER_LEN))
262
                {
263
                        ptr_txd_buffer = 0; // reset txd pointer
264
                        txd_complete = 1; // stop transmission
265
                }
266
                UDR0 = tmp_tx; // send current byte will trigger this ISR again
267
        }
268
        // transmission completed
269
        else ptr_txd_buffer = 0;
270
}
271
 
272
/****************************************************************/
273
/*               USART0 receiver ISR                            */
274
/****************************************************************/
275
ISR(USART0_RX_vect)
276
{
277
        static uint16_t crc;
278
        static uint8_t ptr_rxd_buffer = 0;
279
        uint8_t crc1, crc2;
280
        uint8_t c;
281
 
282
        c = UDR0;  // catch the received byte
283
 
284
        #if (defined (USE_KILLAGREG) || defined (USE_MK3MAG))
285
        // If the cpu is not an Atmega644P the ublox module should be conneced to rxd of the 1st uart.
286
        if(CPUType != ATMEGA644P) ubx_parser(c);
287
        #endif
288
 
289
        if(rxd_buffer_locked) return; // if rxd buffer is locked immediately return
290
 
291
        // the rxd buffer is unlocked
292
        if((ptr_rxd_buffer == 0) && (c == '#')) // if rxd buffer is empty and syncronisation character is received
293
        {
294
                rxd_buffer[ptr_rxd_buffer++] = c; // copy 1st byte to buffer
295
                crc = c; // init crc
296
        }
297
        #if 0
298
        else if (ptr_rxd_buffer == 1) // handle address
299
        {
300
                rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer
301
                crc += c; // update crc
302
        }
303
        #endif
304
        else if (ptr_rxd_buffer < RXD_BUFFER_LEN) // collect incomming bytes
305
        {
306
                if(c != '\r') // no termination character
307
                {
308
                        rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer
309
                        crc += c; // update crc
310
                }
311
                else // termination character was received
312
                {
313
                        // the last 2 bytes are no subject for checksum calculation
314
                        // they are the checksum itself
315
                        crc -= rxd_buffer[ptr_rxd_buffer-2];
316
                        crc -= rxd_buffer[ptr_rxd_buffer-1];
317
                        // calculate checksum from transmitted data
318
                        crc %= 4096;
319
                        crc1 = '=' + crc / 64;
320
                        crc2 = '=' + crc % 64;
321
                        // compare checksum to transmitted checksum bytes
322
                        if((crc1 == rxd_buffer[ptr_rxd_buffer-2]) && (crc2 == rxd_buffer[ptr_rxd_buffer-1]))
323
                        {   // checksum valid
324
                                rxd_buffer[ptr_rxd_buffer] = '\r'; // set termination character
325
                                ReceivedBytes = ptr_rxd_buffer + 1;// store number of received bytes
326
                                rxd_buffer_locked = TRUE;          // lock the rxd buffer
327
                                // if 2nd byte is an 'R' enable watchdog that will result in an reset
328
                                if(rxd_buffer[2] == 'R') // Reset-Commando
329
                                {
330
                                        wdt_enable(WDTO_250MS);
331
                                        Servo_Off();
332
                                }
333
                        }
334
                        else
335
                        {       // checksum invalid
336
                                rxd_buffer_locked = FALSE; // unlock rxd buffer
337
                        }
338
                        ptr_rxd_buffer = 0; // reset rxd buffer pointer
339
                }
340
        }
341
        else // rxd buffer overrun
342
        {
343
                ptr_rxd_buffer = 0; // reset rxd buffer
344
                rxd_buffer_locked = FALSE; // unlock rxd buffer
345
        }
346
 
347
}
348
 
349
 
350
// --------------------------------------------------------------------------
351
void AddCRC(uint16_t datalen)
352
{
353
        uint16_t tmpCRC = 0, i;
354
        for(i = 0; i < datalen; i++)
355
        {
356
                tmpCRC += txd_buffer[i];
357
        }
358
        tmpCRC %= 4096;
359
        txd_buffer[i++] = '=' + tmpCRC / 64;
360
        txd_buffer[i++] = '=' + tmpCRC % 64;
361
        txd_buffer[i++] = '\r';
362
        txd_complete = FALSE;
363
        UDR0 = txd_buffer[0]; // initiates the transmittion (continued in the TXD ISR)
364
}
365
 
366
 
367
 
368
// --------------------------------------------------------------------------
369
void SendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) // uint8_t *pdata, uint8_t len, ...
370
{
371
        va_list ap;
372
        uint16_t pt = 0;
373
        uint8_t a,b,c;
374
        uint8_t ptr = 0;
375
 
376
        uint8_t *pdata = 0;
377
        int len = 0;
378
 
379
        txd_buffer[pt++] = '#';                 // Start character
380
        txd_buffer[pt++] = 'a' + addr;  // Address (a=0; b=1,...)
381
        txd_buffer[pt++] = cmd;                 // Command
382
 
383
        va_start(ap, numofbuffers);
384
        if(numofbuffers)
385
        {
386
                pdata = va_arg(ap, uint8_t*);
387
                len = va_arg(ap, int);
388
                ptr = 0;
389
                numofbuffers--;
390
        }
391
 
392
        while(len)
393
        {
394
                if(len)
395
                {
396
                        a = pdata[ptr++];
397
                        len--;
398
                        if((!len) && numofbuffers)
399
                        {
400
                                pdata = va_arg(ap, uint8_t*);
401
                                len = va_arg(ap, int);
402
                                ptr = 0;
403
                                numofbuffers--;
404
                        }
405
                }
406
                else a = 0;
407
                if(len)
408
                {
409
                        b = pdata[ptr++];
410
                        len--;
411
                        if((!len) && numofbuffers)
412
                        {
413
                                pdata = va_arg(ap, uint8_t*);
414
                                len = va_arg(ap, int);
415
                                ptr = 0;
416
                                numofbuffers--;
417
                        }
418
                }
419
                else b = 0;
420
                if(len)
421
                {
422
                        c = pdata[ptr++];
423
                        len--;
424
                        if((!len) && numofbuffers)
425
                        {
426
                                pdata = va_arg(ap, uint8_t*);
427
                                len = va_arg(ap, int);
428
                                ptr = 0;
429
                                numofbuffers--;
430
                        }
431
                }
432
                else c = 0;
433
                txd_buffer[pt++] = '=' + (a >> 2);
434
                txd_buffer[pt++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4));
435
                txd_buffer[pt++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6));
436
                txd_buffer[pt++] = '=' + ( c & 0x3f);
437
        }
438
        va_end(ap);
439
        AddCRC(pt); // add checksum after data block and initates the transmission
440
}
441
 
442
 
443
// --------------------------------------------------------------------------
444
void Decode64(void)
445
{
446
        uint8_t a,b,c,d;
447
        uint8_t x,y,z;
448
        uint8_t ptrIn = 3;
449
        uint8_t ptrOut = 3;
450
        uint8_t len = ReceivedBytes - 6;
451
 
452
        while(len)
453
        {
454
                a = rxd_buffer[ptrIn++] - '=';
455
                b = rxd_buffer[ptrIn++] - '=';
456
                c = rxd_buffer[ptrIn++] - '=';
457
                d = rxd_buffer[ptrIn++] - '=';
458
                //if(ptrIn > ReceivedBytes - 3) break;
459
 
460
                x = (a << 2) | (b >> 4);
461
                y = ((b & 0x0f) << 4) | (c >> 2);
462
                z = ((c & 0x03) << 6) | d;
463
 
464
                if(len--) rxd_buffer[ptrOut++] = x; else break;
465
                if(len--) rxd_buffer[ptrOut++] = y; else break;
466
                if(len--) rxd_buffer[ptrOut++] = z; else break;
467
        }
468
        pRxData = &rxd_buffer[3];
469
        RxDataLen = ptrOut - 3;
470
}
471
 
472
 
473
// --------------------------------------------------------------------------
474
void USART0_ProcessRxData(void)
475
{
476
        // if data in the rxd buffer are not locked immediately return
477
        if(!rxd_buffer_locked) return;
478
 
479
        uint8_t tempchar1, tempchar2;
480
 
481
        Decode64(); // decode data block in rxd_buffer
482
 
483
        switch(rxd_buffer[1] - 'a')
484
        {
485
                case FC_ADDRESS:
486
 
487
                switch(rxd_buffer[2])
488
                {
489
                        #ifdef USE_MK3MAG
490
                        case 'K':// compass value
491
                                CompassHeading = ((Heading_t *)pRxData)->Heading;
492
                                CompassOffCourse = ((540 + CompassHeading - CompassCourse) % 360) - 180;
493
                                break;
494
                        #endif
495
 
496
                        case 't':// motor test
497
                                if(RxDataLen > 20) //
498
                                {
499
                                        memcpy(&MotorTest[0], (uint8_t*)pRxData, sizeof(MotorTest));
500
                                }
501
                                else
502
                                {
503
                                        memcpy(&MotorTest[0], (uint8_t*)pRxData, 4);
504
                                }
505
                                //Request_MotorTest = TRUE;
506
                                MotorTest_Active = 255;
507
                                PcAccess = 255;
508
                                break;
509
 
510
                        case 'n':// "Get Mixer Table
511
                                while(!txd_complete); // wait for previous frame to be sent
512
                                SendOutData('N', FC_ADDRESS, 1, (uint8_t *) &Mixer, sizeof(Mixer));
513
                                break;
514
 
515
                        case 'm':// "Set Mixer Table
516
                                if(pRxData[0] == EEMIXER_REVISION)
517
                                {
518
                                        memcpy(&Mixer, (uint8_t*)pRxData, sizeof(Mixer));
519
                                        MixerTable_WriteToEEProm();
520
                                        while(!txd_complete); // wait for previous frame to be sent
521
                                        tempchar1 = 1;
522
                                }
523
                                else
524
                                {
525
                                        tempchar1 = 0;
526
                                }
527
                                SendOutData('M', FC_ADDRESS,  1, &tempchar1, 1);
528
                                break;
529
 
530
                        case 'p': // get PPM channels
531
                                Request_PPMChannels = TRUE;
532
                                break;
533
 
534
                        case 'q':// request settings
535
                                if(pRxData[0] == 0xFF)
536
                                {
537
                                        pRxData[0] = GetParamByte(PID_ACTIVE_SET);
538
                                }
539
                                // limit settings range
540
                                if(pRxData[0] < 1) pRxData[0] = 1; // limit to 1
541
                                else if(pRxData[0] > 5) pRxData[0] = 5; // limit to 5
542
                                // load requested parameter set
543
                                ParamSet_ReadFromEEProm(pRxData[0]);
544
                                tempchar1 = pRxData[0];
545
                                tempchar2 = EEPARAM_REVISION;
546
                                while(!txd_complete); // wait for previous frame to be sent
547
                                SendOutData('Q', FC_ADDRESS,3, &tempchar1, sizeof(tempchar1), &tempchar2, sizeof(tempchar2), (uint8_t *) &ParamSet, sizeof(ParamSet));
548
                                break;
549
 
550
                        case 's': // save settings
551
                                if(!(MKFlags & MKFLAG_MOTOR_RUN)) // save settings only if motors are off
552
                                {
553
                                        if((1 <= pRxData[0]) && (pRxData[0] <= 5) && (pRxData[1] == EEPARAM_REVISION)) // check for setting to be in range and version of settings
554
                                        {
555
                                                memcpy(&ParamSet, (uint8_t*)&pRxData[2], sizeof(ParamSet));
556
                                                ParamSet_WriteToEEProm(pRxData[0]);
557
                                                TurnOver180Nick = (int32_t) ParamSet.AngleTurnOverNick * 2500L;
558
                                                TurnOver180Roll = (int32_t) ParamSet.AngleTurnOverRoll * 2500L;
559
                                                tempchar1 = GetActiveParamSet();
560
                                                LipoDetection(0); // low voltage warning
561
                                                Beep(tempchar1, 110);
562
                                        }
563
                                        else
564
                                        {
565
                                                tempchar1 = 0;  //indicate bad data
566
                                        }
567
                                        while(!txd_complete); // wait for previous frame to be sent
568
                                        SendOutData('S', FC_ADDRESS,1, &tempchar1, sizeof(tempchar1));
569
                                }
570
                                break;
571
 
572
                        default:
573
                                //unsupported command received
574
                                break;
575
                } // case FC_ADDRESS:
576
 
577
                default: // any Slave Address
578
 
579
                switch(rxd_buffer[2])
580
                {
581
                        case 'a':// request for labels of the analog debug outputs
582
                                Request_DebugLabel = pRxData[0];
583
                                if(Request_DebugLabel > 31) Request_DebugLabel = 31;
584
                                PcAccess = 255;
585
                                break;
586
 
587
                        case 'b': // submit extern control
588
                                memcpy(&ExternControl, (uint8_t*)pRxData, sizeof(ExternControl));
589
                                ConfirmFrame = ExternControl.Frame;
590
                                PcAccess = 255;
591
                                break;
592
 
593
                        case 'h':// request for display columns
594
                                PcAccess = 255;
595
                                RemoteKeys |= pRxData[0];
596
                                if(RemoteKeys) DisplayLine = 0;
597
                                Request_Display = TRUE;
598
                                break;
599
 
600
                        case 'l':// request for display columns
601
                                PcAccess = 255;
602
                                MenuItem = pRxData[0];
603
                                Request_Display1 = TRUE;
604
                                break;
605
 
606
                        case 'v': // request for version and board release
607
                                Request_VerInfo = TRUE;
608
                                break;
609
 
610
                        case 'g':// get external control data
611
                                Request_ExternalControl = TRUE;
612
                                break;
613
 
614
                        case 'd': // request for the debug data
615
                                DebugData_Interval = (uint16_t) pRxData[0] * 10;
616
                                if(DebugData_Interval > 0) Request_DebugData = TRUE;
617
                                break;
618
 
619
                        case 'c': // request for the 3D data
620
                                Data3D_Interval = (uint16_t) pRxData[0] * 10;
621
                                if(Data3D_Interval > 0) Request_Data3D = TRUE;
622
                                break;
623
 
624
                        default:
625
                                //unsupported command received
626
                                break;
627
                }
628
                break; // default:
629
        }
630
        // unlock the rxd buffer after processing
631
        pRxData = 0;
632
        RxDataLen = 0;
633
        rxd_buffer_locked = FALSE;
634
}
635
 
636
//############################################################################
637
//Routine für die Serielle Ausgabe
638
int16_t uart_putchar (int8_t c)
639
//############################################################################
640
{
641
        if (c == '\n')
642
                uart_putchar('\r');
643
        // wait until previous character was send
644
        loop_until_bit_is_set(UCSR0A, UDRE0);
645
        // send character
646
        UDR0 = c;
647
        return (0);
648
}
649
 
650
 
651
//---------------------------------------------------------------------------------------------
652
void USART0_TransmitTxData(void)
653
{
654
        if(!txd_complete) return;
655
 
656
        if(Request_VerInfo && txd_complete)
657
        {
658
                SendOutData('V', FC_ADDRESS, 1, (uint8_t *) &UART_VersionInfo, sizeof(UART_VersionInfo));
659
                Request_VerInfo = FALSE;
660
        }
661
        else if(Request_Display && txd_complete)
662
        {
663
                LCD_PrintMenu();
664
                SendOutData('H', FC_ADDRESS, 2, &DisplayLine, sizeof(DisplayLine), &DisplayBuff[DisplayLine * 20], 20);
665
                DisplayLine++;
666
                if(DisplayLine >= 4) DisplayLine = 0;
667
                Request_Display = FALSE;
668
        }
669
        else if(Request_Display1 && txd_complete)
670
        {
671
                LCD_PrintMenu();
672
                SendOutData('L', FC_ADDRESS, 3, &MenuItem, sizeof(MenuItem), &MaxMenuItem, sizeof(MaxMenuItem), DisplayBuff, sizeof(DisplayBuff));
673
                Request_Display1 = FALSE;
674
        }
675
        else if(Request_DebugLabel != 0xFF) // Texte für die Analogdaten
676
        {
677
                uint8_t label[16]; // local sram buffer
678
                memcpy_P(label, ANALOG_LABEL[Request_DebugLabel], 16); // read lable from flash to sram buffer
679
                SendOutData('A', FC_ADDRESS, 2, (uint8_t *) &Request_DebugLabel, sizeof(Request_DebugLabel), label, 16);
680
                Request_DebugLabel = 0xFF;
681
        }
682
        else if(ConfirmFrame && txd_complete)   // Datensatz ohne CRC bestätigen
683
        {
684
                SendOutData('B', FC_ADDRESS, 1, (uint8_t*)&ConfirmFrame, sizeof(ConfirmFrame));
685
                ConfirmFrame = 0;
686
        }
687
        else if( (((DebugData_Interval > 0) && CheckDelay(DebugData_Timer)) || Request_DebugData) && txd_complete)
688
        {
689
                SendOutData('D', FC_ADDRESS, 1,(uint8_t *) &DebugOut, sizeof(DebugOut));
690
                DebugData_Timer = SetDelay(DebugData_Interval);
691
                Request_DebugData = FALSE;
692
    }
693
    else if( (((Data3D_Interval > 0) && CheckDelay(Data3D_Timer)) || Request_Data3D) && txd_complete)
694
        {
695
                SendOutData('C', FC_ADDRESS, 1,(uint8_t *) &Data3D, sizeof(Data3D));
696
                Data3D.AngleNick = (int16_t)((10 * IntegralGyroNick) / GYRO_DEG_FACTOR); // convert to multiple of 0.1°
697
                Data3D.AngleRoll = (int16_t)((10 * IntegralGyroRoll) / GYRO_DEG_FACTOR); // convert to multiple of 0.1°
698
                Data3D.Heading   = (int16_t)((10 * YawGyroHeading)   / GYRO_DEG_FACTOR); // convert to multiple of 0.1°
699
                Data3D_Timer = SetDelay(Data3D_Interval);
700
                Request_Data3D = FALSE;
701
    }
702
        else if(Request_ExternalControl && txd_complete)
703
        {
704
                SendOutData('G', FC_ADDRESS, 1,(uint8_t *) &ExternControl, sizeof(ExternControl));
705
                Request_ExternalControl = FALSE;
706
        }
707
 
708
        #ifdef USE_MK3MAG
709
    else if((CheckDelay(Compass_Timer)) && txd_complete)
710
        {
711
                ToMk3Mag.Attitude[0] = (int16_t)((10 * IntegralGyroNick) / GYRO_DEG_FACTOR);  // approx. 0.1 deg
712
                ToMk3Mag.Attitude[1] = (int16_t)((10 * IntegralGyroRoll) / GYRO_DEG_FACTOR);  // approx. 0.1 deg
713
                ToMk3Mag.UserParam[0] = FCParam.UserParam1;
714
                ToMk3Mag.UserParam[1] = FCParam.UserParam2;
715
                ToMk3Mag.CalState = CompassCalState;
716
                SendOutData('w', MK3MAG_ADDRESS, 1,(uint8_t *) &ToMk3Mag,sizeof(ToMk3Mag));
717
                // the last state is 5 and should be send only once to avoid multiple flash writing
718
                if(CompassCalState > 4)  CompassCalState = 0;
719
                Compass_Timer = SetDelay(99);
720
        }
721
        #endif
722
 
723
        else if(Request_MotorTest && txd_complete)
724
        {
725
                SendOutData('T', FC_ADDRESS, 0);
726
                Request_MotorTest = FALSE;
727
        }
728
        else if(Request_PPMChannels && txd_complete)
729
        {
730
                SendOutData('P', FC_ADDRESS, 1, (uint8_t *)&PPM_in, sizeof(PPM_in));
731
                Request_PPMChannels = FALSE;
732
        }
733
}
734