Subversion Repositories NaviCtrl

Rev

Rev 81 | Details | Compare with Previous | Last modification | View Log | RSS feed

Rev Author Line No. Line
41 ingob 1
/*#######################################################################################*/
2
/* !!! THIS IS NOT FREE SOFTWARE !!!                                                     */
3
/*#######################################################################################*/
4
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
5
// + Copyright (c) 2008 Ingo Busker, Holger Buss
171 ingob 6
// + Nur für den privaten Gebrauch / NON-COMMERCIAL USE ONLY
41 ingob 7
// + FOR NON COMMERCIAL USE ONLY
8
// + www.MikroKopter.com
9
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
10
// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),
11
// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist.
12
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
13
// + bzgl. der Nutzungsbedingungen aufzunehmen.
14
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
15
// + Verkauf von Luftbildaufnahmen, usw.
16
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
17
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,
18
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen
19
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
20
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
21
// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
22
// + eindeutig als Ursprung verlinkt werden
23
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
24
// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
25
// + Benutzung auf eigene Gefahr
26
// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
27
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
171 ingob 28
// + Die Portierung oder Nutzung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
41 ingob 29
// + mit unserer Zustimmung zulässig
30
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
31
// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
32
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
33
// + Redistributions of source code (with or without modifications) must retain the above copyright notice,
34
// + this list of conditions and the following disclaimer.
35
// +   * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
36
// +     from this software without specific prior written permission.
37
// +   * The use of this project (hardware, software, binary files, sources and documentation) is only permitted
38
// +     for non-commercial use (directly or indirectly)
39
// +     Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
40
// +     with our written permission
41
// +   * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
42
// +     clearly linked as origin
171 ingob 43
// +   * porting the sources to other systems or using the software on other systems (except hardware from www.mikrokopter.de) is not allowed
41 ingob 44
//
45
// +  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
46
// +  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
47
// +  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
48
// +  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
49
// +  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
50
// +  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
51
// +  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
52
// +  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
53
// +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
54
// +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
55
// +  POSSIBILITY OF SUCH DAMAGE.
56
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
57
#include <stdio.h>
58
#include <stdarg.h>
59
#include <string.h>
60
 
61
#include "91x_lib.h"
62
#include "ramfunc.h"
63
#include "menu.h"
64
#include "printf_P.h"
65
#include "GPS.h"
66
#include "i2c.h"
67
#include "uart0.h"
68
#include "uart1.h"
69
#include "uart2.h"
70
#include "timer.h"
71
#include "usb.h"
72
#include "main.h"
73
#include "waypoints.h"
74
#include "GPS.h"
75
 
76
// slave addresses
77
#define FC_ADDRESS 1
78
#define NC_ADDRESS 2
79
#define MK3MAG_ADDRESS 3
80
 
81
#define FALSE   0
82
#define TRUE    1
83
 
84
u8 Request_SendFollowMe         = FALSE;
85
u8 Request_VerInfo                      = FALSE;
86
u8 Request_ExternalControl      = FALSE;
87
u8 Request_Display                      = FALSE;
88
u8 Request_Display1             = FALSE;
89
u8 Request_DebugData            = FALSE;
90
u8 Request_DebugLabel           = 255;
91
u8 Request_ChannelOnly          = FALSE;
92
u8 Request_NaviData                     = FALSE;
93
u8 Request_ErrorMessage     = FALSE;
94
u8 Request_NewWaypoint          = FALSE;
62 killagreg 95
u8 Request_Data3D                    = FALSE;
41 ingob 96
 
97
u8 DisplayLine = 0;
98
 
99
UART_TypeDef *DebugUART = UART1;
100
 
101
volatile u8 txd_buffer[TXD_BUFFER_LEN];
102
volatile u8 rxd_buffer_locked = FALSE;
103
volatile u8 rxd_buffer[RXD_BUFFER_LEN];
104
volatile u8 txd_complete = TRUE;
105
volatile u8 ReceivedBytes = 0;
106
volatile u8 CntCrcError = 0;
107
volatile u8 *pRxData = NULL;
108
volatile u8  RxDataLen = 0;
109
 
110
u8 text[100];
111
 
112
u8 PcAccess = 100;
113
u8 MotorTest[4] = {0,0,0,0};
114
u8 ConfirmFrame = 0;
115
 
116
DebugOut_t DebugOut;
117
ExternControl_t ExternControl;
118
UART_VersionInfo_t UART_VersionInfo;
119
NaviData_t NaviData;
120
Waypoint_t FollowMe;
61 holgerb 121
Data3D_t Data3D;
41 ingob 122
 
123
u32 DebugData_Timer;
75 holgerb 124
u32 DebugData_Interval = 5000;  // in ms
41 ingob 125
u32 NaviData_Timer;
75 holgerb 126
u32 NaviData_Interval = 5000;   // in ms
62 killagreg 127
u32 Data3D_Timer = 0;                   // in ms
128
u32 Data3D_Interval = 0;
41 ingob 129
 
130
static u16 ptr_txd_buffer = 0;
131
 
132
const u8 ANALOG_LABEL[32][16] =
133
{
134
        //1234567890123456
135
        "AngleNick       ", //0
136
        "AngleRoll       ",
137
        "AccNick         ",
138
        "AccRoll         ",
139
        "                ",
75 holgerb 140
        "MK-Flags        ", //5
41 ingob 141
        "                ",
142
        "                ",
143
        "                ",
144
        "GPS Data        ",
61 holgerb 145
        "CompassHeading  ", //10
146
        "GyroHeading     ",
41 ingob 147
        "SPI Error       ",
148
        "SPI Okay        ",
149
        "I2C Error       ",
150
        "I2C Okay        ", //15
151
        "                ",//    "FC_Kalman_K     ",
152
        "ACC_Speed_N     ",
153
        "ACC_Speed_E     ",
154
        "                ",//    "GPS ACC         ",
155
        "                ",//    "MAXDrift        ", //20
156
        "N_Speed         ",
157
        "E_Speed         ",
158
        "                ",//    "KalmDist_N      ",
159
        "                ",//    "KalmDist_E      ",
61 holgerb 160
        "                ",//25
77 holgerb 161
        "SD-Logs         ",
41 ingob 162
        "Distance N      ",
163
        "Distance E      ",
164
        "GPS_Nick        ",
165
        "GPS_Roll        ", //30
166
        "Used_Sats       "
167
};
168
 
169
 
170
/********************************************************/
171
/*            Initialization the UART1                  */
172
/********************************************************/
173
void UART1_Init (void)
174
{
175
        GPIO_InitTypeDef GPIO_InitStructure;
176
        UART_InitTypeDef UART_InitStructure;
177
 
178
        SCU_APBPeriphClockConfig(__UART1, ENABLE);  // Enable the UART1 Clock
179
        SCU_APBPeriphClockConfig(__GPIO3, ENABLE);  // Enable the GPIO3 Clock
180
 
181
        /*Configure UART1_Rx pin GPIO3.2*/
182
        GPIO_StructInit(&GPIO_InitStructure);
183
        GPIO_InitStructure.GPIO_Direction =     GPIO_PinInput;
184
        GPIO_InitStructure.GPIO_Pin =                   GPIO_Pin_2;
185
        GPIO_InitStructure.GPIO_Type =                  GPIO_Type_PushPull;
186
        GPIO_InitStructure.GPIO_IPConnected =   GPIO_IPConnected_Enable;
187
        GPIO_InitStructure.GPIO_Alternate =     GPIO_InputAlt1; // UART1_RxD
188
        GPIO_Init(GPIO3, &GPIO_InitStructure);
189
 
190
        /*Configure UART1_Tx pin GPIO3.3*/
191
        GPIO_StructInit(&GPIO_InitStructure);
192
        GPIO_InitStructure.GPIO_Direction =     GPIO_PinOutput;
193
        GPIO_InitStructure.GPIO_Pin =                   GPIO_Pin_3;
194
        GPIO_InitStructure.GPIO_Type =                  GPIO_Type_PushPull;
195
        GPIO_InitStructure.GPIO_Alternate =     GPIO_OutputAlt2; // UART1_TX
196
        GPIO_Init(GPIO3, &GPIO_InitStructure);
197
 
198
        /* UART1 configured as follow:
199
        - Word Length = 8 Bits
200
        - One Stop Bit
201
        - No parity
202
        - BaudRate = 57600 baud
203
        - Hardware flow control Disabled
204
        - Receive and transmit enabled
205
        - Receive and transmit FIFOs are Disabled
206
        */
207
        UART_StructInit(&UART_InitStructure);
208
        UART_InitStructure.UART_WordLength =                    UART_WordLength_8D;
209
        UART_InitStructure.UART_StopBits =                              UART_StopBits_1;
210
        UART_InitStructure.UART_Parity =                                UART_Parity_No ;
211
        UART_InitStructure.UART_BaudRate =                              BAUD_RATE;
212
        UART_InitStructure. UART_HardwareFlowControl =  UART_HardwareFlowControl_None;
213
        UART_InitStructure.UART_Mode =                                  UART_Mode_Tx_Rx;
214
        UART_InitStructure.UART_FIFO =                                  UART_FIFO_Enable;
215
        UART_InitStructure.UART_TxFIFOLevel =                   UART_FIFOLevel_1_2;
216
        UART_InitStructure.UART_RxFIFOLevel =                   UART_FIFOLevel_1_2;
217
 
218
        UART_DeInit(UART1); // reset uart 1     to default
219
        UART_Init(UART1, &UART_InitStructure); // initialize uart 1
220
        // enable uart 1 interrupts selective
221
        UART_ITConfig(UART1, UART_IT_Receive | UART_IT_ReceiveTimeOut, ENABLE);
222
        UART_Cmd(UART1, ENABLE); // enable uart 1
223
        // configure the uart 1 interupt line as an IRQ with priority 4 (0 is highest)
224
        VIC_Config(UART1_ITLine, VIC_IRQ, 4);
225
        // enable the uart 1 IRQ
226
        VIC_ITCmd(UART1_ITLine, ENABLE);
227
        // initialize the debug timer
228
        DebugData_Timer = SetDelay(DebugData_Interval);
54 killagreg 229
        NaviData_Timer = SetDelay(NaviData_Interval)+500;
41 ingob 230
        // unlock rxd_buffer
231
        rxd_buffer_locked = FALSE;
232
        pRxData = NULL;
233
        RxDataLen = 0;
234
        // no bytes to send
235
        txd_complete = TRUE;
236
        // Fill Version Info Structure
237
        UART_VersionInfo.SWMajor = VERSION_MAJOR;
238
        UART_VersionInfo.SWMinor = VERSION_MINOR;
239
        UART_VersionInfo.SWPatch = VERSION_PATCH;
240
        UART_VersionInfo.ProtoMajor = VERSION_SERIAL_MAJOR;
241
        UART_VersionInfo.ProtoMinor = VERSION_SERIAL_MINOR;
242
 
243
        SerialPutString("\r\nUART1 init...ok");
244
}
245
 
246
 
247
/****************************************************************/
248
/*               USART1 receiver ISR                            */
249
/****************************************************************/
250
void UART1_IRQHandler(void)
251
{
252
        static u16 crc;
253
        static u8 ptr_rxd_buffer = 0;
254
        static u8 crc1, crc2;
255
        static u8 abortState = 0;
256
        u8 c;
257
 
258
        if((UART_GetITStatus(UART1, UART_IT_Receive) != RESET) || (UART_GetITStatus(UART1, UART_IT_ReceiveTimeOut) != RESET) )
259
        {
260
                // clear the pending bits
261
                UART_ClearITPendingBit(UART1, UART_IT_Receive);
262
                UART_ClearITPendingBit(UART1, UART_IT_ReceiveTimeOut);
263
                // if debug UART is not UART1
264
                if (DebugUART != UART1)
265
                {       // forward received data to the debug UART tx buffer
266
                        while(UART_GetFlagStatus(UART1, UART_FLAG_RxFIFOEmpty) != SET)
267
                        {
268
                                // wait for space in the tx buffer of the DebugUART
269
                                while(UART_GetFlagStatus(DebugUART, UART_FLAG_TxFIFOFull) == SET) {};
270
                                // move the byte from the rx buffer of UART1 to the tx buffer of DebugUART
271
                                c = UART_ReceiveData(UART1);
272
 
273
                                // check for abort condition (ESC ESC 0x55 0xAA 0x00)
274
                                switch (abortState)
275
                                {
276
                                  case 0: if (c == 27) abortState++;
277
                                                break;
278
                                  case 1: if (c == 27) abortState++; else abortState = 0;
279
                                        break;
280
                                  case 2: if (c == 0x55) abortState++; else abortState = 0;
281
                                                break;
282
                                  case 3: if (c == 0xAA) abortState++; else abortState = 0;
283
                                                break;
284
                                  case 4: if (c == 0x00)
285
                                           {
286
                                                    DebugUART = UART1;
287
                                                        UART0_Connect_to_MKGPS();
288
                                                   }
289
                                          abortState = 0;
290
                                                break;
291
                                }
292
 
293
                                if (DebugUART != UART1) UART_SendData(DebugUART, c);
294
                        }
295
                }
296
                else  // DebugUART == UART1 (normal operation)
297
                {
298
                        while ((UART_GetFlagStatus(UART1, UART_FLAG_RxFIFOEmpty) != SET) && (!rxd_buffer_locked))
299
                        { // some byes in the fifo and rxd buffer not locked
300
                            // get byte from fifo
301
                        c = UART_ReceiveData(UART1);
302
                                if((ptr_rxd_buffer == 0) && (c == '#')) // if rxd buffer is empty and syncronisation character is received
303
                                {
304
                                        rxd_buffer[ptr_rxd_buffer++] = c; // copy 1st byte to buffer
305
                                        crc = c; // init crc
306
                                }
307
                                #if 0
308
                                else if (ptr_rxd_buffer == 1) // handle address
309
                                {
310
                                        rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer
311
                                        crc += c; // update crc
312
                                }
313
                                #endif
314
                                else if (ptr_rxd_buffer < RXD_BUFFER_LEN) // rxd buffer not full
315
                                {
316
                                        if (c != '\r') // no termination character received
317
                                        {
318
                                                rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer
319
                                                crc += c; // update crc
320
                                        }
321
                                        else // termination character received
322
                                        {
323
                                                // the last 2 bytes are no subject for checksum calculation
324
                                                // they are the checksum itself
325
                                                crc -= rxd_buffer[ptr_rxd_buffer-2];
326
                                                crc -= rxd_buffer[ptr_rxd_buffer-1];
327
                                                // calculate checksum from transmitted data
328
                                                crc %= 4096;
329
                                                crc1 = '=' + crc / 64;
330
                                                crc2 = '=' + crc % 64;
331
                                                // compare checksum to transmitted checksum bytes
332
                                                if((crc1 == rxd_buffer[ptr_rxd_buffer-2]) && (crc2 == rxd_buffer[ptr_rxd_buffer-1]))
333
                                                {   // checksum valid
334
                                                        rxd_buffer_locked = TRUE;               // lock the rxd buffer
335
                                                        ReceivedBytes = ptr_rxd_buffer + 1;     // store number of received bytes
336
                                                        rxd_buffer[ptr_rxd_buffer] = '\r';      // set termination character
337
                                                        // if 2nd byte is an 'R' start bootloader
338
                                                        if(rxd_buffer[2] == 'R')
339
                                                        {
340
                                                                PowerOff();
341
                                                                VIC_DeInit();
342
                                                                Execute_Bootloader(); // Reset-Commando - Bootloader starten
343
                                                        }
344
                                                } // eof checksum valid
345
                                                else
346
                                                {       // checksum invalid
347
                                                        rxd_buffer_locked = FALSE; // unlock rxd buffer
348
                                                }  // eof checksum invalid
349
                                                ptr_rxd_buffer = 0; // reset rxd buffer pointer
350
                                        } // eof termination character received
351
                                } // rxd buffer not full
352
                                else // rxd buffer overrun
353
                                {
354
                                        ptr_rxd_buffer = 0; // reset rxd buffer pointer
355
                                        rxd_buffer_locked = FALSE; // unlock rxd buffer
356
                                } // eof rxd buffer overrrun
357
                        } // some byes in the fifo and rxd buffer not locked
358
                } // eof DebugUart = UART1
359
        }
360
}
361
 
362
/**************************************************************/
363
/*         Transmit tx buffer via debug uart                  */
364
/**************************************************************/
365
void UART1_Transmit(void)
366
{
367
        u8 tmp_tx;
368
        // if something has to be send and the txd fifo is not full
369
        if((!txd_complete) && (UART_GetFlagStatus(UART1, UART_FLAG_TxFIFOFull) == RESET))
370
        {
371
                tmp_tx = txd_buffer[ptr_txd_buffer]; // read byte from txd buffer
372
                // if terminating character or end of txd buffer reached
373
                if((tmp_tx == '\r') || (ptr_txd_buffer == TXD_BUFFER_LEN))
374
                {
375
                        ptr_txd_buffer = 0;     // reset txd buffer pointer
376
                        txd_complete = TRUE;// set complete flag
377
                }
378
                UART_SendData(UART1, tmp_tx); // put character to txd fifo
379
                // set pointer to next byte
380
                ptr_txd_buffer++;
381
        }
382
}
383
 
384
/**************************************************************/
385
/* Add CRC and initiate transmission via debug uart           */
386
/**************************************************************/
387
void AddCRC(u16 datalen)
388
{
389
        u16 tmpCRC = 0, i;
390
        for(i = 0; i < datalen; i++)
391
        {
392
                tmpCRC += txd_buffer[i];
393
        }
394
        tmpCRC %= 4096;
395
        txd_buffer[i++] = '=' + tmpCRC / 64;
396
        txd_buffer[i++] = '=' + tmpCRC % 64;
397
        txd_buffer[i++] = '\r';
398
 
399
        ptr_txd_buffer = 0;
400
        txd_complete = FALSE;
49 ingob 401
        UART_SendData(UART1, txd_buffer[ptr_txd_buffer++]);     // send first byte, to be continued in the txd irq
41 ingob 402
}
403
 
404
 
405
 
406
/**************************************************************/
407
/* Code output data                                           */
408
/**************************************************************/
409
void SendOutData(u8 cmd, u8 Address, u8 numofbuffers , ...) //u8 *data, u8 len, ....
410
{
411
        va_list ap;
412
 
413
        u16 pt = 0;
414
        u8 a,b,c;
415
        u8 ptr = 0;
416
 
417
        u8* pdata = NULL;
418
        int len = 0;
419
 
420
        txd_buffer[pt++] = '#';                         // Start character
421
        txd_buffer[pt++] = 'a' + Address;       // Address (a=0; b=1,...)
422
        txd_buffer[pt++] = cmd;                         // Command
423
 
424
        va_start(ap, numofbuffers);
425
        if(numofbuffers)
426
        {
427
                pdata = va_arg(ap, u8*);
428
                len = va_arg(ap, int);
429
                ptr = 0;
430
                numofbuffers--;
431
        }
432
        while(len)
433
        {
434
                if(len)
435
                {
436
                        a = pdata[ptr++];
437
                        len--;
438
                        if((!len) && numofbuffers) // try to jump to next buffer
439
                        {
440
                                pdata = va_arg(ap, u8*);
441
                                len = va_arg(ap, int);
442
                                ptr = 0;
443
                                numofbuffers--;
444
                        }
445
                }
446
                else a = 0;
447
                if(len)
448
                {
449
                        b = pdata[ptr++];
450
                        len--;
451
                        if((!len) && numofbuffers) // try to jump to next buffer
452
                        {
453
                                pdata = va_arg(ap, u8*);
454
                                len = va_arg(ap, int);
455
                                ptr = 0;
456
                                numofbuffers--;
457
                        }
458
                }
459
                else b = 0;
460
                if(len)
461
                {
462
                        c = pdata[ptr++];
463
                        len--;
464
                        if((!len) && numofbuffers) // try to jump to next buffer
465
                        {
466
                                pdata = va_arg(ap, u8*);
467
                                len = va_arg(ap, int);
468
                                ptr = 0;
469
                                numofbuffers--;
470
                        }
471
                }
472
                else c = 0;
473
                txd_buffer[pt++] = '=' + (a >> 2);
474
                txd_buffer[pt++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4));
475
                txd_buffer[pt++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6));
476
                txd_buffer[pt++] = '=' + ( c & 0x3f);
477
        }
478
        va_end(ap);
479
        AddCRC(pt);     // add checksum after data block and initates the transmission
480
}
481
 
482
 
483
/**************************************************************/
484
/* Decode data                                                */
485
/**************************************************************/
486
void Decode64(void)
487
{
488
        u8 a,b,c,d;
489
        u8 x,y,z;
490
        u8 ptrIn = 3; // start with first data byte in rx buffer
491
        u8 ptrOut = 3;
492
        u8 len = ReceivedBytes - 6;      // must be a multiple of 4 (3 bytes at begin and 3 bytes at end are no payload )
493
        while(len)
494
        {
495
                a = rxd_buffer[ptrIn++] - '=';
496
                b = rxd_buffer[ptrIn++] - '=';
497
                c = rxd_buffer[ptrIn++] - '=';
498
                d = rxd_buffer[ptrIn++] - '=';
499
                //if(ptrIn > ReceivedBytes - 3) break;
500
 
501
                x = (a << 2) | (b >> 4);
502
                y = ((b & 0x0f) << 4) | (c >> 2);
503
                z = ((c & 0x03) << 6) | d;
504
 
505
                if(len--) rxd_buffer[ptrOut++] = x; else break;
506
                if(len--) rxd_buffer[ptrOut++] = y; else break;
507
                if(len--) rxd_buffer[ptrOut++] = z; else break;
508
        }
509
        pRxData = &rxd_buffer[3];
510
        RxDataLen = ptrOut - 3;
511
}
512
 
513
/**************************************************************/
514
/* Process incomming data from debug uart                     */
515
/**************************************************************/
516
void UART1_ProcessRxData(void)
517
{
518
        // if data in the rxd buffer are not locked immediately return
519
        if((!rxd_buffer_locked) || (DebugUART != UART1) ) return;
520
        Waypoint_t * pWaypoint = NULL;
521
 
522
 
523
 
524
        PcAccess = 255;
525
        Decode64(); // decode data block in rxd buffer
526
        switch(rxd_buffer[1] - 'a') // check for Slave Address
527
        {
528
                case NC_ADDRESS:  // own Slave Address
529
 
530
                switch(rxd_buffer[2])
531
                {
532
                        case 'e': // request for the text of the error status
533
                                Request_ErrorMessage = TRUE;
534
                                break;
535
 
536
                        case 's'://  new target position
537
                                pWaypoint = (Waypoint_t*)&pRxData[0];
538
                                BeepTime = 300;
539
                                if(pWaypoint->Position.Status == NEWDATA)
540
                                {
541
                                        WPList_Clear(); // empty WPList
542
                                        WPList_Append(pWaypoint);
543
                                        GPS_pWaypoint = WPList_Begin();
544
                                }
545
                                break;
546
 
547
                        case 'u': // redirect debug uart
548
                                switch(pRxData[0])
549
                                {
550
                                        case UART_FLIGHTCTRL:
551
                                                UART2_Init();                           // initialize UART2 to FC pins
552
                                                DebugUART = UART2;
553
                                                break;
554
                                        case UART_MK3MAG:
555
                                                if(FC.MKFlags & MKFLAG_MOTOR_RUN) break; // not if the motors are running
556
                                                UART0_Connect_to_MK3MAG();      // mux UART0 to MK3MAG pins
557
                                                GPSData.Status = INVALID;
558
                                                DebugUART = UART0;
559
                                                break;
560
                                        case UART_MKGPS:
561
                                                if(FC.MKFlags & MKFLAG_MOTOR_RUN) break; // not if the motors are running
562
                                                UART0_Connect_to_MKGPS();       // connect UART0 to MKGPS pins
563
                                                GPSData.Status = INVALID;
564
                                                DebugUART = UART0;
565
                                                break;
566
                                }
567
                                break;
568
 
569
                        case 'w'://  new PCPosition for GPSTargetList
570
                                pWaypoint = (Waypoint_t*)&pRxData[0];
571
                                if(pWaypoint->Position.Status == INVALID)
572
                                {  // clear WP List
573
                                        WPList_Clear();
574
                                        GPS_pWaypoint = WPList_Begin();
575
                                        //SerialPutString("\r\nClear WP List\r\n");
576
                                }
577
                                else if (pWaypoint->Position.Status == NEWDATA)
578
                                {  // app current WP to the list
579
                                        WPList_Append(pWaypoint);
580
                                        BeepTime = 500;
581
                                        //SerialPutString("\r\nAdd WP to List\r\n");
582
                                }
583
                                Request_NewWaypoint = TRUE;
584
                                break;
585
 
586
                        default:
587
                                // unsupported command recieved
588
                                break;
589
                } // case NC_ADDRESS
65 killagreg 590
                // "break;" is missing here to fall thru to the common commands
41 ingob 591
 
592
                default:  // and any other Slave Address
593
 
594
                switch(rxd_buffer[2]) // check CmdID
595
                {
596
                        case 'a':// request for the labels of the analog debug outputs
597
                                Request_DebugLabel = pRxData[0];
598
                                if(Request_DebugLabel > 31) Request_DebugLabel = 31;
599
                                break;
600
 
601
                        case 'b': // submit extern control
602
                                memcpy(&ExternControl, (u8*)&pRxData[0], sizeof(ExternControl));
603
                                ConfirmFrame = ExternControl.Frame;
604
                                break;
605
 
606
                        case 'd': // request for debug data;
607
                                DebugData_Interval = (u32) pRxData[0] * 10;
608
                                if(DebugData_Interval > 0) Request_DebugData = TRUE;
609
                                break;
610
 
63 killagreg 611
                        case 'c': // request for 3D data;
612
                                Data3D_Interval = (u32) pRxData[0] * 10;
613
                                if(Data3D_Interval > 0) Request_Data3D = TRUE;
614
                                break;
615
 
41 ingob 616
                        case 'g':// request for external control data
617
                                Request_ExternalControl = TRUE;
618
                                break;
619
 
620
                        case 'h':// reqest for display line
621
                                RemoteKeys |= pRxData[0];
622
                                if(RemoteKeys != 0) DisplayLine = 0;
623
                                Request_Display = TRUE;
624
                                break;
625
 
626
                        case 'l':// reqest for display columns
627
                                MenuItem = pRxData[0];
628
                                Request_Display1 = TRUE;
629
                                break;
64 holgerb 630
 
631
                        case 'o': // request for navigation information
632
                                NaviData_Interval = (u32) pRxData[0] * 10;
633
                                if(NaviData_Interval > 0) Request_NaviData = TRUE;
634
                                break;
635
 
41 ingob 636
                        case 'v': // request for version info
637
                                Request_VerInfo = TRUE;
638
                                break;
639
                        default:
640
                                // unsupported command recieved
641
                                break;
642
                }
643
                break; // default:
644
        }
645
        // unlock the rxd buffer after processing
646
        pRxData = NULL;
647
        RxDataLen = 0;
648
        rxd_buffer_locked = FALSE;
649
}
650
 
651
 
652
/*****************************************************/
653
/*                   Send a character                */
654
/*****************************************************/
655
s16 uart_putchar (char c)
656
{
657
        if (c == '\n') uart_putchar('\r');
658
        // wait until txd fifo is not full
659
        while (UART_GetFlagStatus(UART1, UART_FLAG_TxFIFOFull) != RESET);
660
        // transmit byte
661
        UART_SendData(UART1, c);
662
        return (0);
663
}
664
 
665
/*****************************************************/
666
/*       Send a string to the debug uart              */
667
/*****************************************************/
668
void SerialPutString(u8 *s)
669
{
670
        if(s == NULL) return;
671
        while (*s != '\0' && DebugUART == UART1)
672
        {
673
                uart_putchar(*s);
674
                s ++;
675
        }
676
}
677
 
678
 
679
 
680
/**************************************************************/
681
/* Send the answers to incomming commands at the debug uart   */
682
/**************************************************************/
683
void UART1_TransmitTxData(void)
684
{
685
        if(!txd_complete || (DebugUART != UART1) ) return;
686
 
687
        if(Request_DebugLabel != 0xFF)
688
        {
689
                SendOutData('A', NC_ADDRESS, 2, &Request_DebugLabel, sizeof(Request_DebugLabel), (u8 *) ANALOG_LABEL[Request_DebugLabel], 16);
690
                Request_DebugLabel = 0xFF;
691
        }
692
        if(ConfirmFrame && txd_complete)
693
        {
694
                SendOutData('B', NC_ADDRESS, 1, &ConfirmFrame, sizeof(ConfirmFrame));
695
                ConfirmFrame = 0;
696
        }
62 killagreg 697
        if( (( (DebugData_Interval > 0) && CheckDelay(DebugData_Timer)) || Request_DebugData) && txd_complete)
41 ingob 698
        {
699
                SendOutData('D', NC_ADDRESS, 1,(u8 *)&DebugOut, sizeof(DebugOut));
700
                DebugData_Timer = SetDelay(DebugData_Interval);
701
                Request_DebugData = FALSE;
702
        }
61 holgerb 703
 
62 killagreg 704
        if((( (Data3D_Interval > 0) && CheckDelay(Data3D_Timer) ) || Request_Data3D) && txd_complete)
61 holgerb 705
        {
706
                SendOutData('C', NC_ADDRESS, 1,(u8 *)&Data3D, sizeof(Data3D));
62 killagreg 707
                Data3D_Timer = SetDelay(Data3D_Interval);
708
                Request_Data3D = FALSE;
61 holgerb 709
        }
710
 
41 ingob 711
        if(Request_ExternalControl && txd_complete)
712
        {
713
                SendOutData('G', NC_ADDRESS, 1, (u8 *)&ExternControl, sizeof(ExternControl));
714
                Request_ExternalControl = FALSE;
715
        }
716
        if(Request_Display && txd_complete)
717
        {
718
                LCD_PrintMenu();
719
                SendOutData('H', NC_ADDRESS, 2, &DisplayLine, sizeof(DisplayLine), (u8*)&DisplayBuff[DisplayLine * 20], 20);
720
                DisplayLine++;
721
                if(DisplayLine >= 4) DisplayLine = 0;
722
                Request_Display = FALSE;
723
        }
724
        if(Request_Display1 && txd_complete)
725
        {
726
                LCD_PrintMenu();
727
                SendOutData('L', NC_ADDRESS, 3, (u8*)&MenuItem, sizeof(MenuItem), (u8*)&MaxMenuItem, sizeof(MaxMenuItem),(u8*)DisplayBuff, sizeof(DisplayBuff));
728
                Request_Display1 = FALSE;
729
        }
730
        if(Request_VerInfo && txd_complete)
731
        {
732
                SendOutData('V', NC_ADDRESS,1, (u8 *)&UART_VersionInfo, sizeof(UART_VersionInfo));
733
                Request_VerInfo = FALSE;
734
        }
735
        if(( (NaviData_Interval && CheckDelay(NaviData_Timer) ) || Request_NaviData) && txd_complete)
736
        {
737
                NaviData.Errorcode = ErrorCode;
65 killagreg 738
                SendOutData('O', NC_ADDRESS,1, (u8 *)&NaviData, sizeof(NaviData));
739
                if (DebugUART == UART1) SendOutData0('O', NC_ADDRESS,1, (u8 *)&NaviData, sizeof(NaviData));
41 ingob 740
                NaviData_Timer = SetDelay(NaviData_Interval);
741
                Request_NaviData = FALSE;
742
        }
743
        if(Request_ErrorMessage && txd_complete)
744
        {
745
                SendOutData('E', NC_ADDRESS, 1, (u8 *)&ErrorMSG, sizeof(ErrorMSG));
746
                Request_ErrorMessage = FALSE;
747
        }
748
        if(Request_SendFollowMe && txd_complete && (GPSData.NumOfSats >= 4))              // sending for "Follow me"
749
        {
750
                GPS_CopyPosition(&(GPSData.Position),&(FollowMe.Position));
751
                FollowMe.Position.Status = NEWDATA;
752
                FollowMe.Heading = -1;
753
                FollowMe.ToleranceRadius = 1;
754
                FollowMe.HoldTime = 60;
755
                FollowMe.Event_Flag = 0;
756
                FollowMe.reserve[0] = 0;                // reserve
757
                FollowMe.reserve[1] = 0;                // reserve
758
                FollowMe.reserve[2] = 0;                // reserve
759
                FollowMe.reserve[3] = 0;                // reserve
760
                SendOutData('s', NC_ADDRESS, 1, (u8 *)&FollowMe, sizeof(FollowMe));
761
                Request_SendFollowMe = FALSE;
762
        }
763
 
764
        if(Request_NewWaypoint && txd_complete)
765
        {
766
                u8 WPNumber = WPList_GetCount();
65 killagreg 767
                SendOutData('W', NC_ADDRESS, 1, &WPNumber, sizeof(WPNumber));
41 ingob 768
                Request_NewWaypoint = FALSE;
769
        }
770
 
771
}
772