Subversion Repositories NaviCtrl

Rev

Rev 494 | Rev 496 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 494 Rev 495
1
/*#######################################################################################*/
1
/*#######################################################################################*/
2
/* !!! THIS IS NOT FREE SOFTWARE !!!                                                     */
2
/* !!! THIS IS NOT FREE SOFTWARE !!!                                                     */
3
/*#######################################################################################*/
3
/*#######################################################################################*/
4
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
4
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
5
// + www.MikroKopter.com
5
// + www.MikroKopter.com
6
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
6
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
7
// + Software Nutzungsbedingungen (english version: see below)
7
// + Software Nutzungsbedingungen (english version: see below)
8
// + der Fa. HiSystems GmbH, Flachsmeerstrasse 2, 26802 Moormerland - nachfolgend Lizenzgeber genannt -
8
// + der Fa. HiSystems GmbH, Flachsmeerstrasse 2, 26802 Moormerland - nachfolgend Lizenzgeber genannt -
9
// + Der Lizenzgeber räumt dem Kunden ein nicht-ausschließliches, zeitlich und räumlich* unbeschränktes Recht ein, die im den
9
// + Der Lizenzgeber räumt dem Kunden ein nicht-ausschließliches, zeitlich und räumlich* unbeschränktes Recht ein, die im den
10
// + Mikrocontroller verwendete Firmware für die Hardware Flight-Ctrl, Navi-Ctrl, BL-Ctrl, MK3Mag & PC-Programm MikroKopter-Tool
10
// + Mikrocontroller verwendete Firmware für die Hardware Flight-Ctrl, Navi-Ctrl, BL-Ctrl, MK3Mag & PC-Programm MikroKopter-Tool
11
// + - nachfolgend Software genannt - nur für private Zwecke zu nutzen.
11
// + - nachfolgend Software genannt - nur für private Zwecke zu nutzen.
12
// + Der Einsatz dieser Software ist nur auf oder mit Produkten des Lizenzgebers zulässig.
12
// + Der Einsatz dieser Software ist nur auf oder mit Produkten des Lizenzgebers zulässig.
13
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
13
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
14
// + Die vom Lizenzgeber gelieferte Software ist urheberrechtlich geschützt. Alle Rechte an der Software sowie an sonstigen im
14
// + Die vom Lizenzgeber gelieferte Software ist urheberrechtlich geschützt. Alle Rechte an der Software sowie an sonstigen im
15
// + Rahmen der Vertragsanbahnung und Vertragsdurchführung überlassenen Unterlagen stehen im Verhältnis der Vertragspartner ausschließlich dem Lizenzgeber zu.
15
// + Rahmen der Vertragsanbahnung und Vertragsdurchführung überlassenen Unterlagen stehen im Verhältnis der Vertragspartner ausschließlich dem Lizenzgeber zu.
16
// + Die in der Software enthaltenen Copyright-Vermerke, Markenzeichen, andere Rechtsvorbehalte, Seriennummern sowie
16
// + Die in der Software enthaltenen Copyright-Vermerke, Markenzeichen, andere Rechtsvorbehalte, Seriennummern sowie
17
// + sonstige der Programmidentifikation dienenden Merkmale dürfen vom Kunden nicht verändert oder unkenntlich gemacht werden.
17
// + sonstige der Programmidentifikation dienenden Merkmale dürfen vom Kunden nicht verändert oder unkenntlich gemacht werden.
18
// + Der Kunde trifft angemessene Vorkehrungen für den sicheren Einsatz der Software. Er wird die Software gründlich auf deren
18
// + Der Kunde trifft angemessene Vorkehrungen für den sicheren Einsatz der Software. Er wird die Software gründlich auf deren
19
// + Verwendbarkeit zu dem von ihm beabsichtigten Zweck testen, bevor er diese operativ einsetzt.
19
// + Verwendbarkeit zu dem von ihm beabsichtigten Zweck testen, bevor er diese operativ einsetzt.
20
// + Die Haftung des Lizenzgebers wird - soweit gesetzlich zulässig - begrenzt in Höhe des typischen und vorhersehbaren
20
// + Die Haftung des Lizenzgebers wird - soweit gesetzlich zulässig - begrenzt in Höhe des typischen und vorhersehbaren
21
// + Schadens. Die gesetzliche Haftung bei Personenschäden und nach dem Produkthaftungsgesetz bleibt unberührt. Dem Lizenzgeber steht jedoch der Einwand
21
// + Schadens. Die gesetzliche Haftung bei Personenschäden und nach dem Produkthaftungsgesetz bleibt unberührt. Dem Lizenzgeber steht jedoch der Einwand
22
// + des Mitverschuldens offen.
22
// + des Mitverschuldens offen.
23
// + Der Kunde trifft angemessene Vorkehrungen für den Fall, dass die Software ganz oder teilweise nicht ordnungsgemäß arbeitet.
23
// + Der Kunde trifft angemessene Vorkehrungen für den Fall, dass die Software ganz oder teilweise nicht ordnungsgemäß arbeitet.
24
// + Er wird die Software gründlich auf deren Verwendbarkeit zu dem von ihm beabsichtigten Zweck testen, bevor er diese operativ einsetzt.
24
// + Er wird die Software gründlich auf deren Verwendbarkeit zu dem von ihm beabsichtigten Zweck testen, bevor er diese operativ einsetzt.
25
// + Der Kunde wird er seine Daten vor Einsatz der Software nach dem Stand der Technik sichern.
25
// + Der Kunde wird er seine Daten vor Einsatz der Software nach dem Stand der Technik sichern.
26
// + Der Kunde ist darüber unterrichtet, dass der Lizenzgeber seine Daten im zur Vertragsdurchführung erforderlichen Umfang
26
// + Der Kunde ist darüber unterrichtet, dass der Lizenzgeber seine Daten im zur Vertragsdurchführung erforderlichen Umfang
27
// + und auf Grundlage der Datenschutzvorschriften erhebt, speichert, verarbeitet und, sofern notwendig, an Dritte übermittelt.
27
// + und auf Grundlage der Datenschutzvorschriften erhebt, speichert, verarbeitet und, sofern notwendig, an Dritte übermittelt.
28
// + *) Die räumliche Nutzung bezieht sich nur auf den Einsatzort, nicht auf die Reichweite der programmierten Software.
28
// + *) Die räumliche Nutzung bezieht sich nur auf den Einsatzort, nicht auf die Reichweite der programmierten Software.
29
// + #### ENDE DER NUTZUNGSBEDINGUNGEN ####'
29
// + #### ENDE DER NUTZUNGSBEDINGUNGEN ####'
30
// +  Hinweis: Informationen über erweiterte Nutzungsrechte (wie z.B. Nutzung für nicht-private Zwecke) sind auf Anfrage per Email an info(@)hisystems.de verfügbar.
30
// +  Hinweis: Informationen über erweiterte Nutzungsrechte (wie z.B. Nutzung für nicht-private Zwecke) sind auf Anfrage per Email an info(@)hisystems.de verfügbar.
31
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
31
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
32
// + Software LICENSING TERMS
32
// + Software LICENSING TERMS
33
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
33
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
34
// + of HiSystems GmbH, Flachsmeerstrasse 2, 26802 Moormerland, Germany - the Licensor -
34
// + of HiSystems GmbH, Flachsmeerstrasse 2, 26802 Moormerland, Germany - the Licensor -
35
// + The Licensor grants the customer a non-exclusive license to use the microcontroller firmware of the Flight-Ctrl, Navi-Ctrl, BL-Ctrl, and MK3Mag hardware
35
// + The Licensor grants the customer a non-exclusive license to use the microcontroller firmware of the Flight-Ctrl, Navi-Ctrl, BL-Ctrl, and MK3Mag hardware
36
// + (the Software) exclusively for private purposes. The License is unrestricted with respect to time and territory*.
36
// + (the Software) exclusively for private purposes. The License is unrestricted with respect to time and territory*.
37
// + The Software may only be used with the Licensor's products.
37
// + The Software may only be used with the Licensor's products.
38
// + The Software provided by the Licensor is protected by copyright. With respect to the relationship between the parties to this
38
// + The Software provided by the Licensor is protected by copyright. With respect to the relationship between the parties to this
39
// + agreement, all rights pertaining to the Software and other documents provided during the preparation and execution of this
39
// + agreement, all rights pertaining to the Software and other documents provided during the preparation and execution of this
40
// + agreement shall be the property of the Licensor.
40
// + agreement shall be the property of the Licensor.
41
// + The information contained in the Software copyright notices, trademarks, other legal reservations, serial numbers and other
41
// + The information contained in the Software copyright notices, trademarks, other legal reservations, serial numbers and other
42
// + features that can be used to identify the program may not be altered or defaced by the customer.
42
// + features that can be used to identify the program may not be altered or defaced by the customer.
43
// + The customer shall be responsible for taking reasonable precautions
43
// + The customer shall be responsible for taking reasonable precautions
44
// + for the safe use of the Software. The customer shall test the Software thoroughly regarding its suitability for the
44
// + for the safe use of the Software. The customer shall test the Software thoroughly regarding its suitability for the
45
// + intended purpose before implementing it for actual operation. The Licensor's liability shall be limited to the extent of typical and
45
// + intended purpose before implementing it for actual operation. The Licensor's liability shall be limited to the extent of typical and
46
// + foreseeable damage to the extent permitted by law, notwithstanding statutory liability for bodily injury and product
46
// + foreseeable damage to the extent permitted by law, notwithstanding statutory liability for bodily injury and product
47
// + liability. However, the Licensor shall be entitled to the defense of contributory negligence.
47
// + liability. However, the Licensor shall be entitled to the defense of contributory negligence.
48
// + The customer will take adequate precautions in the case, that the software is not working properly. The customer will test
48
// + The customer will take adequate precautions in the case, that the software is not working properly. The customer will test
49
// + the software for his purpose before any operational usage. The customer will backup his data before using the software.
49
// + the software for his purpose before any operational usage. The customer will backup his data before using the software.
50
// + The customer understands that the Licensor collects, stores and processes, and, where required, forwards, customer data
50
// + The customer understands that the Licensor collects, stores and processes, and, where required, forwards, customer data
51
// + to third parties to the extent necessary for executing the agreement, subject to applicable data protection and privacy regulations.
51
// + to third parties to the extent necessary for executing the agreement, subject to applicable data protection and privacy regulations.
52
// + *) The territory aspect only refers to the place where the Software is used, not its programmed range.
52
// + *) The territory aspect only refers to the place where the Software is used, not its programmed range.
53
// + #### END OF LICENSING TERMS ####
53
// + #### END OF LICENSING TERMS ####
54
// + Note: For information on license extensions (e.g. commercial use), please contact us at info(@)hisystems.de.
54
// + Note: For information on license extensions (e.g. commercial use), please contact us at info(@)hisystems.de.
55
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
55
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
56
#include <stdio.h>
56
#include <stdio.h>
57
#include <stdlib.h>
57
#include <stdlib.h>
58
#include <stdarg.h>
58
#include <stdarg.h>
59
#include <string.h>
59
#include <string.h>
60
 
60
 
61
#include "91x_lib.h"
61
#include "91x_lib.h"
62
#include "main.h"
62
#include "main.h"
63
#include "config.h"
63
#include "config.h"
64
#include "menu.h"
64
#include "menu.h"
65
#include "GPS.h"
65
#include "GPS.h"
66
#include "i2c.h"
66
#include "i2c.h"
67
#include "uart0.h"
67
#include "uart0.h"
68
#include "uart1.h"
68
#include "uart1.h"
69
#include "uart2.h"
69
#include "uart2.h"
70
#include "timer1.h"
70
#include "timer1.h"
71
#include "timer2.h"
71
#include "timer2.h"
72
#include "analog.h"
72
#include "analog.h"
73
#include "compass.h"
73
#include "compass.h"
74
#include "waypoints.h"
74
#include "waypoints.h"
75
#include "mkprotocol.h"
75
#include "mkprotocol.h"
76
#include "params.h"
76
#include "params.h"
77
#include "fifo.h"
77
#include "fifo.h"
78
#include "debug.h"
78
#include "debug.h"
79
#include "spi_slave.h"
79
#include "spi_slave.h"
80
#include "ftphelper.h"
80
#include "ftphelper.h"
81
#include "led.h"
81
#include "led.h"
82
#include "fat16.h"
82
#include "fat16.h"
83
 
83
 
84
 
84
 
85
#define FALSE   0
85
#define FALSE   0
86
#define TRUE    1
86
#define TRUE    1
87
 
87
 
88
#define ABO_TIMEOUT 8000 // disable abo after 8 seconds
88
#define ABO_TIMEOUT 8000 // disable abo after 8 seconds
89
u32 UART1_AboTimeOut = 0;
89
u32 UART1_AboTimeOut = 0;
90
 
90
 
91
u8 UART1_Request_VersionInfo    = FALSE;
91
u8 UART1_Request_VersionInfo    = FALSE;
92
u8 UART1_Request_ExternalControl= FALSE;
92
u8 UART1_Request_ExternalControl= FALSE;
93
u8 UART1_Request_Display                = FALSE;
93
u8 UART1_Request_Display                = FALSE;
94
u8 UART1_Request_Display1               = FALSE;
94
u8 UART1_Request_Display1               = FALSE;
95
u8 UART1_Request_DebugData              = FALSE;
95
u8 UART1_Request_DebugData              = FALSE;
96
u8 UART1_Request_DebugLabel             = 255;
96
u8 UART1_Request_DebugLabel             = 255;
97
u8 UART1_Request_NaviData               = FALSE;
97
u8 UART1_Request_NaviData               = FALSE;
98
u8 UART1_Request_ErrorMessage   = FALSE;
98
u8 UART1_Request_ErrorMessage   = FALSE;
99
u8 UART1_Request_WritePoint             = 0xFF;
99
u8 UART1_Request_WritePoint             = 0xFF;
100
u8 UART1_Request_ReadPoint              = 0;
100
u8 UART1_Request_ReadPoint              = 0;
101
u8 UART1_Request_Data3D             = FALSE;
101
u8 UART1_Request_Data3D             = FALSE;
102
u8 UART1_Request_MotorData          = FALSE;
102
u8 UART1_Request_MotorData          = FALSE;
103
u8 UART1_Request_Echo               = FALSE;
103
u8 UART1_Request_Echo               = FALSE;
104
u8 UART1_Request_ParameterId    = 0;
104
u8 UART1_Request_ParameterId    = 0;
105
u8 UART1_Request_WPLStore               = FALSE;
105
u8 UART1_Request_WPLStore               = FALSE;
106
u8 UART1_Request_Parameter              = FALSE;
106
u8 UART1_Request_Parameter              = FALSE;
107
u8 UART1_Request_SystemTime             = FALSE;
107
u8 UART1_Request_SystemTime             = FALSE;
108
u8 UART1_DisplayKeys                    = 0;
108
u8 UART1_DisplayKeys                    = 0;
109
u8 UART1_DisplayLine                    = 0;
109
u8 UART1_DisplayLine                    = 0;
110
u8 UART1_ConfirmFrame                   = 0;
110
u8 UART1_ConfirmFrame                   = 0;
111
u8 UART1_Request_FTP                    = FALSE;
111
u8 UART1_Request_FTP                    = FALSE;
112
u8 LastTransmittedFCStatusFlags2 = 0;
112
u8 LastTransmittedFCStatusFlags2 = 0;
113
u8 UART1_ExternalControlConfirmFrame = FALSE;
113
u8 UART1_ExternalControlConfirmFrame = FALSE;
114
u8 Send_NMEA_RMC = FALSE;
114
u8 Send_NMEA_RMC = FALSE;
115
 
115
 
116
UART_TypeDef *DebugUART = UART1;
116
UART_TypeDef *DebugUART = UART1;
117
 
117
 
118
#ifdef FOLLOW_ME
118
#ifdef FOLLOW_ME
119
#define FOLLOW_ME_INTERVAL 200 // 5 Hz
119
#define FOLLOW_ME_INTERVAL 200 // 5 Hz
120
u32 UART1_FollowMe_Timer        = 0;
120
u32 UART1_FollowMe_Timer        = 0;
121
Point_t FollowMe;
121
Point_t FollowMe;
122
#endif
122
#endif
123
 
123
 
124
// the primary rx fifo
124
// the primary rx fifo
125
#define UART1_RX_FIFO_LEN 1024
125
#define UART1_RX_FIFO_LEN 1024
126
u8 UART1_rxfifobuffer[UART1_RX_FIFO_LEN];
126
u8 UART1_rxfifobuffer[UART1_RX_FIFO_LEN];
127
fifo_t UART1_rx_fifo;
127
fifo_t UART1_rx_fifo;
128
 
128
 
129
// the rx buffer
129
// the rx buffer
130
#define UART1_RX_BUFFER_LEN  1024
130
#define UART1_RX_BUFFER_LEN  1024
131
u8 UART1_rbuffer[UART1_RX_BUFFER_LEN];
131
u8 UART1_rbuffer[UART1_RX_BUFFER_LEN];
132
Buffer_t UART1_rx_buffer;
132
Buffer_t UART1_rx_buffer;
133
 
133
 
134
// the tx buffer
134
// the tx buffer
135
#define UART1_TX_BUFFER_LEN  1024
135
#define UART1_TX_BUFFER_LEN  1024
136
u8 UART1_tbuffer[UART1_TX_BUFFER_LEN];
136
u8 UART1_tbuffer[UART1_TX_BUFFER_LEN];
137
Buffer_t UART1_tx_buffer;
137
Buffer_t UART1_tx_buffer;
138
 
138
 
139
volatile u8 SerialLinkOkay = 0;
139
volatile u8 SerialLinkOkay = 0;
140
 
140
 
141
u8 text[200];
141
u8 text[200];
142
 
142
 
143
const u8 ANALOG_LABEL[32][16] =
143
const u8 ANALOG_LABEL[32][16] =
144
{
144
{
145
   //1234567890123456
145
   //1234567890123456
146
        "AngleNick       ", //0
146
        "AngleNick       ", //0
147
        "AngleRoll       ",
147
        "AngleRoll       ",
148
        "AccNick         ",
148
        "AccNick         ",
149
        "AccRoll         ",
149
        "AccRoll         ",
150
        "OperatingRadius ",
150
        "OperatingRadius ",
151
        "FC-Flags        ", //5
151
        "FC-Flags        ", //5
152
        "NC-Flags        ",
152
        "NC-Flags        ",
153
        "Voltage  [0,1V] ",
153
        "Voltage  [0,1V] ",
154
        "Current  [0,1A] ",
154
        "Current  [0,1A] ",
155
        "GPS Data        ",
155
        "GPS Data        ",
156
        "CompassHeading  ", //10
156
        "CompassHeading  ", //10
157
        "GyroHeading     ",
157
        "GyroHeading     ",
158
        "SPI Error       ", // achtung: muss auf 12 bleiben
158
        "SPI Error       ", // achtung: muss auf 12 bleiben
159
        "SPI Okay        ",
159
        "SPI Okay        ",
160
        "I2C Error       ",
160
        "I2C Error       ",
161
        "I2C Okay        ", //15
161
        "I2C Okay        ", //15
162
        "16              ",
162
        "16              ",
163
        "17              ",
163
        "17              ",
164
        "18              ",
164
        "18              ",
165
        "19              ", // SD-Card-time
165
        "19              ", // SD-Card-time
166
        "EarthMagnet [%] ", //20
166
        "EarthMagnet [%] ", //20
167
        "Ground Speed    ", //  "Z_Speed         ",
167
        "Ground Speed    ", //  "Z_Speed         ",
168
        "N_Speed         ",
168
        "N_Speed         ",
169
        "E_Speed         ",
169
        "E_Speed         ",
170
        "Magnet X        ",
170
        "Magnet X        ",
171
        "Magnet Y        ", //25
171
        "Magnet Y        ", //25
172
        "Magnet Z        ",
172
        "Magnet Z        ",
173
        "Distance N      ",
173
        "Distance N      ",
174
        "Distance E      ",
174
        "Distance E      ",
175
        "GPS_Nick        ",
175
        "GPS_Nick        ",
176
        "GPS_Roll        ", //30
176
        "GPS_Roll        ", //30
177
        "Used_Sats       "
177
        "Used_Sats       "
178
};
178
};
-
 
179
 
-
 
180
typedef struct
-
 
181
{
-
 
182
        u8 Index;
-
 
183
        u8 Status;
-
 
184
} __attribute__((packed)) WPL_Answer_t;
-
 
185
WPL_Answer_t WPL_Answer;
179
 
186
 
180
DebugOut_t DebugOut;
187
DebugOut_t DebugOut;
181
ExternControl_t ExternControl;
188
ExternControl_t ExternControl;
182
UART_VersionInfo_t UART_VersionInfo;
189
UART_VersionInfo_t UART_VersionInfo;
183
NaviData_t NaviData;
190
NaviData_t NaviData;
184
Data3D_t Data3D;
191
Data3D_t Data3D;
185
WPL_Store_t WPL_Store;
192
WPL_Store_t WPL_Store;
186
u16 Echo; // 2 bytes recieved will be sent back as echo
193
u16 Echo; // 2 bytes recieved will be sent back as echo
187
 
194
 
188
u32 UART1_DebugData_Timer = 0;
195
u32 UART1_DebugData_Timer = 0;
189
u32 UART1_DebugData_Interval = 0;       // in ms
196
u32 UART1_DebugData_Interval = 0;       // in ms
190
u32 UART1_NaviData_Timer = 0;
197
u32 UART1_NaviData_Timer = 0;
191
u32 UART1_NaviData_Interval = 0;        // in ms
198
u32 UART1_NaviData_Interval = 0;        // in ms
192
u32 UART1_Data3D_Timer = 0;
199
u32 UART1_Data3D_Timer = 0;
193
u32 UART1_Data3D_Interval = 0;          // in ms
200
u32 UART1_Data3D_Interval = 0;          // in ms
194
u32 UART1_MotorData_Timer = 0;
201
u32 UART1_MotorData_Timer = 0;
195
u32 UART1_MotorData_Interval = 0;               // in ms
202
u32 UART1_MotorData_Interval = 0;               // in ms
196
u32 UART1_Display_Timer = 0;
203
u32 UART1_Display_Timer = 0;
197
u32 UART1_Display_Interval = 0;         // in ms
204
u32 UART1_Display_Interval = 0;         // in ms
198
u32 NMEA_Timer = 0;
205
u32 NMEA_Timer = 0;
199
u32 NMEA_Interval = 500;// in ms
206
u32 NMEA_Interval = 500;// in ms
200
 
207
 
201
/********************************************************/
208
/********************************************************/
202
/*            Initialization the UART1                  */
209
/*            Initialization the UART1                  */
203
/********************************************************/
210
/********************************************************/
204
void UART1_Init (void)
211
void UART1_Init (void)
205
{
212
{
206
        GPIO_InitTypeDef GPIO_InitStructure;
213
        GPIO_InitTypeDef GPIO_InitStructure;
207
        UART_InitTypeDef UART_InitStructure;
214
        UART_InitTypeDef UART_InitStructure;
208
 
215
 
209
        // initialize txd buffer
216
        // initialize txd buffer
210
        Buffer_Init(&UART1_tx_buffer, UART1_tbuffer, UART1_TX_BUFFER_LEN);
217
        Buffer_Init(&UART1_tx_buffer, UART1_tbuffer, UART1_TX_BUFFER_LEN);
211
 
218
 
212
        // initialize rxd buffer
219
        // initialize rxd buffer
213
        Buffer_Init(&UART1_rx_buffer, UART1_rbuffer, UART1_RX_BUFFER_LEN);
220
        Buffer_Init(&UART1_rx_buffer, UART1_rbuffer, UART1_RX_BUFFER_LEN);
214
 
221
 
215
        // initialize the rx fifo, block UART IRQ geting a byte from fifo
222
        // initialize the rx fifo, block UART IRQ geting a byte from fifo
216
        fifo_init(&UART1_rx_fifo, UART1_rxfifobuffer, UART1_RX_FIFO_LEN, NO_ITLine, UART1_ITLine);
223
        fifo_init(&UART1_rx_fifo, UART1_rxfifobuffer, UART1_RX_FIFO_LEN, NO_ITLine, UART1_ITLine);
217
 
224
 
218
        SCU_APBPeriphClockConfig(__UART1, ENABLE);  // Enable the UART1 Clock
225
        SCU_APBPeriphClockConfig(__UART1, ENABLE);  // Enable the UART1 Clock
219
        SCU_APBPeriphClockConfig(__GPIO3, ENABLE);  // Enable the GPIO3 Clock
226
        SCU_APBPeriphClockConfig(__GPIO3, ENABLE);  // Enable the GPIO3 Clock
220
 
227
 
221
        /*Configure UART1_Rx pin GPIO3.2*/
228
        /*Configure UART1_Rx pin GPIO3.2*/
222
        GPIO_StructInit(&GPIO_InitStructure);
229
        GPIO_StructInit(&GPIO_InitStructure);
223
        GPIO_InitStructure.GPIO_Direction =     GPIO_PinInput;
230
        GPIO_InitStructure.GPIO_Direction =     GPIO_PinInput;
224
        GPIO_InitStructure.GPIO_Pin =                   GPIO_Pin_2;
231
        GPIO_InitStructure.GPIO_Pin =                   GPIO_Pin_2;
225
        GPIO_InitStructure.GPIO_Type =                  GPIO_Type_PushPull;
232
        GPIO_InitStructure.GPIO_Type =                  GPIO_Type_PushPull;
226
        GPIO_InitStructure.GPIO_IPInputConnected =      GPIO_IPInputConnected_Enable;
233
        GPIO_InitStructure.GPIO_IPInputConnected =      GPIO_IPInputConnected_Enable;
227
        GPIO_InitStructure.GPIO_Alternate =     GPIO_InputAlt1; // UART1_RxD
234
        GPIO_InitStructure.GPIO_Alternate =     GPIO_InputAlt1; // UART1_RxD
228
        GPIO_Init(GPIO3, &GPIO_InitStructure);
235
        GPIO_Init(GPIO3, &GPIO_InitStructure);
229
 
236
 
230
        /*Configure UART1_Tx pin GPIO3.3*/
237
        /*Configure UART1_Tx pin GPIO3.3*/
231
        GPIO_StructInit(&GPIO_InitStructure);
238
        GPIO_StructInit(&GPIO_InitStructure);
232
        GPIO_InitStructure.GPIO_Direction =     GPIO_PinOutput;
239
        GPIO_InitStructure.GPIO_Direction =     GPIO_PinOutput;
233
        GPIO_InitStructure.GPIO_Pin =                   GPIO_Pin_3;
240
        GPIO_InitStructure.GPIO_Pin =                   GPIO_Pin_3;
234
        GPIO_InitStructure.GPIO_Type =                  GPIO_Type_PushPull;
241
        GPIO_InitStructure.GPIO_Type =                  GPIO_Type_PushPull;
235
        GPIO_InitStructure.GPIO_Alternate =     GPIO_OutputAlt2; // UART1_TX
242
        GPIO_InitStructure.GPIO_Alternate =     GPIO_OutputAlt2; // UART1_TX
236
        GPIO_Init(GPIO3, &GPIO_InitStructure);
243
        GPIO_Init(GPIO3, &GPIO_InitStructure);
237
 
244
 
238
        /* UART1 configured as follow:
245
        /* UART1 configured as follow:
239
        - Word Length = 8 Bits
246
        - Word Length = 8 Bits
240
        - One Stop Bit
247
        - One Stop Bit
241
        - No parity
248
        - No parity
242
        - BaudRate = 57600 baud
249
        - BaudRate = 57600 baud
243
        - Hardware flow control Disabled
250
        - Hardware flow control Disabled
244
        - Receive and transmit enabled
251
        - Receive and transmit enabled
245
        - Receive and transmit FIFOs are Disabled
252
        - Receive and transmit FIFOs are Disabled
246
        */
253
        */
247
        UART_StructInit(&UART_InitStructure);
254
        UART_StructInit(&UART_InitStructure);
248
        UART_InitStructure.UART_WordLength =                    UART_WordLength_8D;
255
        UART_InitStructure.UART_WordLength =                    UART_WordLength_8D;
249
        UART_InitStructure.UART_StopBits =                              UART_StopBits_1;
256
        UART_InitStructure.UART_StopBits =                              UART_StopBits_1;
250
        UART_InitStructure.UART_Parity =                                UART_Parity_No ;
257
        UART_InitStructure.UART_Parity =                                UART_Parity_No ;
251
        UART_InitStructure.UART_BaudRate =                              UART1_BAUD_RATE;
258
        UART_InitStructure.UART_BaudRate =                              UART1_BAUD_RATE;
252
        UART_InitStructure. UART_HardwareFlowControl =  UART_HardwareFlowControl_None;
259
        UART_InitStructure. UART_HardwareFlowControl =  UART_HardwareFlowControl_None;
253
        UART_InitStructure.UART_Mode =                                  UART_Mode_Tx_Rx;
260
        UART_InitStructure.UART_Mode =                                  UART_Mode_Tx_Rx;
254
        UART_InitStructure.UART_FIFO =                                  UART_FIFO_Enable;
261
        UART_InitStructure.UART_FIFO =                                  UART_FIFO_Enable;
255
        UART_InitStructure.UART_TxFIFOLevel =                   UART_FIFOLevel_1_2;
262
        UART_InitStructure.UART_TxFIFOLevel =                   UART_FIFOLevel_1_2;
256
        UART_InitStructure.UART_RxFIFOLevel =                   UART_FIFOLevel_1_2;      // FIFO size 16 bytes, FIFO level 8 bytes
263
        UART_InitStructure.UART_RxFIFOLevel =                   UART_FIFOLevel_1_2;      // FIFO size 16 bytes, FIFO level 8 bytes
257
 
264
 
258
        UART_DeInit(UART1); // reset uart 1     to default
265
        UART_DeInit(UART1); // reset uart 1     to default
259
        UART_Init(UART1, &UART_InitStructure); // initialize uart 1
266
        UART_Init(UART1, &UART_InitStructure); // initialize uart 1
260
        // enable uart 1 interrupts selective
267
        // enable uart 1 interrupts selective
261
        UART_ITConfig(UART1, UART_IT_Receive | UART_IT_ReceiveTimeOut, ENABLE);
268
        UART_ITConfig(UART1, UART_IT_Receive | UART_IT_ReceiveTimeOut, ENABLE);
262
        UART_Cmd(UART1, ENABLE); // enable uart 1
269
        UART_Cmd(UART1, ENABLE); // enable uart 1
263
        // configure the uart 1 interupt line
270
        // configure the uart 1 interupt line
264
        VIC_Config(UART1_ITLine, VIC_IRQ, PRIORITY_UART1);
271
        VIC_Config(UART1_ITLine, VIC_IRQ, PRIORITY_UART1);
265
        // enable the uart 1 IRQ
272
        // enable the uart 1 IRQ
266
        VIC_ITCmd(UART1_ITLine, ENABLE);
273
        VIC_ITCmd(UART1_ITLine, ENABLE);
267
 
274
 
268
        // initialize the debug timer
275
        // initialize the debug timer
269
        UART1_DebugData_Timer = SetDelay(UART1_DebugData_Interval);
276
        UART1_DebugData_Timer = SetDelay(UART1_DebugData_Interval);
270
        UART1_NaviData_Timer = SetDelay(UART1_NaviData_Interval)+500;
277
        UART1_NaviData_Timer = SetDelay(UART1_NaviData_Interval)+500;
271
        NMEA_Timer = SetDelay(14000);
278
        NMEA_Timer = SetDelay(14000);
272
 
279
 
273
        // Fill Version Info Structure
280
        // Fill Version Info Structure
274
        UART_VersionInfo.SWMajor = VERSION_MAJOR;
281
        UART_VersionInfo.SWMajor = VERSION_MAJOR;
275
        UART_VersionInfo.SWMinor = VERSION_MINOR;
282
        UART_VersionInfo.SWMinor = VERSION_MINOR;
276
        UART_VersionInfo.SWPatch = VERSION_PATCH;
283
        UART_VersionInfo.SWPatch = VERSION_PATCH;
277
        UART_VersionInfo.ProtoMajor = VERSION_SERIAL_MAJOR;
284
        UART_VersionInfo.ProtoMajor = VERSION_SERIAL_MAJOR;
278
        UART_VersionInfo.ProtoMinor = VERSION_SERIAL_MINOR;
285
        UART_VersionInfo.ProtoMinor = VERSION_SERIAL_MINOR;
279
    UART_VersionInfo.HWMajor = Version_HW & 0x7F;
286
    UART_VersionInfo.HWMajor = Version_HW & 0x7F;
280
    UART_VersionInfo.reserved2 = 0;
287
    UART_VersionInfo.reserved2 = 0;
281
        UART_VersionInfo.Flags = 0;
288
        UART_VersionInfo.Flags = 0;
282
        NaviData.Version = NAVIDATA_VERSION;
289
        NaviData.Version = NAVIDATA_VERSION;
283
 
290
 
284
        UART1_PutString("\r\n UART1 init...ok");
291
        UART1_PutString("\r\n UART1 init...ok");
285
}
292
}
286
 
293
 
287
 
294
 
288
/****************************************************************/
295
/****************************************************************/
289
/*               USART1 receiver ISR                            */
296
/*               USART1 receiver ISR                            */
290
/****************************************************************/
297
/****************************************************************/
291
void UART1_IRQHandler(void)
298
void UART1_IRQHandler(void)
292
{
299
{
293
        static u8 abortState = 0;
300
        static u8 abortState = 0;
294
        u8 c;
301
        u8 c;
295
 
302
 
296
        if((UART_GetITStatus(UART1, UART_IT_Receive) != RESET) || (UART_GetITStatus(UART1, UART_IT_ReceiveTimeOut) != RESET) )
303
        if((UART_GetITStatus(UART1, UART_IT_Receive) != RESET) || (UART_GetITStatus(UART1, UART_IT_ReceiveTimeOut) != RESET) )
297
        {
304
        {
298
                // clear the pending bits!
305
                // clear the pending bits!
299
                UART_ClearITPendingBit(UART1, UART_IT_Receive);
306
                UART_ClearITPendingBit(UART1, UART_IT_Receive);
300
                UART_ClearITPendingBit(UART1, UART_IT_ReceiveTimeOut);
307
                UART_ClearITPendingBit(UART1, UART_IT_ReceiveTimeOut);
301
                // if debug UART is not UART1
308
                // if debug UART is not UART1
302
                if (DebugUART != UART1)
309
                if (DebugUART != UART1)
303
                {       // forward received data to the debug UART tx buffer
310
                {       // forward received data to the debug UART tx buffer
304
                        while(UART_GetFlagStatus(UART1, UART_FLAG_RxFIFOEmpty) != SET)
311
                        while(UART_GetFlagStatus(UART1, UART_FLAG_RxFIFOEmpty) != SET)
305
                        {
312
                        {
306
                                // move the byte from the rx buffer of UART1 to the tx buffer of DebugUART
313
                                // move the byte from the rx buffer of UART1 to the tx buffer of DebugUART
307
                                c = UART_ReceiveData(UART1);
314
                                c = UART_ReceiveData(UART1);
308
 
315
 
309
                                // check for abort condition (ESC ESC 0x55 0xAA 0x00)
316
                                // check for abort condition (ESC ESC 0x55 0xAA 0x00)
310
                                switch (abortState)
317
                                switch (abortState)
311
                                {
318
                                {
312
                                        case 0:
319
                                        case 0:
313
                                                if (c == 27) abortState++;
320
                                                if (c == 27) abortState++;
314
                                                break;
321
                                                break;
315
                                        case 1:
322
                                        case 1:
316
                                                if (c == 27) abortState++;
323
                                                if (c == 27) abortState++;
317
                                                else abortState = 0;
324
                                                else abortState = 0;
318
                                        break;
325
                                        break;
319
                                        case 2:
326
                                        case 2:
320
                                                if (c == 0x55) abortState++;
327
                                                if (c == 0x55) abortState++;
321
                                                else abortState = 0;
328
                                                else abortState = 0;
322
                                                break;
329
                                                break;
323
                                        case 3:
330
                                        case 3:
324
                                                if (c == 0xAA) abortState++;
331
                                                if (c == 0xAA) abortState++;
325
                                                else abortState = 0;
332
                                                else abortState = 0;
326
                                                break;
333
                                                break;
327
                                        case 4:
334
                                        case 4:
328
                                                if (c == 0x00)
335
                                                if (c == 0x00)
329
                                                {
336
                                                {
330
                                                        if(DebugUART == UART0)
337
                                                        if(DebugUART == UART0)
331
                                                        {
338
                                                        {
332
                                                                UART0_Connect_to_MKGPS(UART0_BAUD_RATE);
339
                                                                UART0_Connect_to_MKGPS(UART0_BAUD_RATE);
333
                                                                TIMER2_Init(); // enbable servo outputs
340
                                                                TIMER2_Init(); // enbable servo outputs
334
                                                                fifo_purge(&UART1_rx_fifo); // flush the whole fifo init buffer
341
                                                                fifo_purge(&UART1_rx_fifo); // flush the whole fifo init buffer
335
                                                        }
342
                                                        }
336
                                                        DebugUART = UART1;
343
                                                        DebugUART = UART1;
337
                                                }
344
                                                }
338
                                                abortState = 0;
345
                                                abortState = 0;
339
                                                break;
346
                                                break;
340
                                } // end switch abort state
347
                                } // end switch abort state
341
                                // if the Debug uart is not UART1, redirect input to the Debug UART
348
                                // if the Debug uart is not UART1, redirect input to the Debug UART
342
                                if (DebugUART != UART1)
349
                                if (DebugUART != UART1)
343
                                {
350
                                {
344
                                        // wait for space in the tx buffer of the DebugUART
351
                                        // wait for space in the tx buffer of the DebugUART
345
                                        while(UART_GetFlagStatus(DebugUART, UART_FLAG_TxFIFOFull) == SET) {};
352
                                        while(UART_GetFlagStatus(DebugUART, UART_FLAG_TxFIFOFull) == SET) {};
346
                                        // move byte to the tx fifo of the debug uart
353
                                        // move byte to the tx fifo of the debug uart
347
                                        UART_SendData(DebugUART, c);
354
                                        UART_SendData(DebugUART, c);
348
                                }
355
                                }
349
                        }
356
                        }
350
                }
357
                }
351
                else  // DebugUART == UART1 (normal operation)
358
                else  // DebugUART == UART1 (normal operation)
352
                {
359
                {
353
                        while(UART_GetFlagStatus(UART1, UART_FLAG_RxFIFOEmpty) != SET)
360
                        while(UART_GetFlagStatus(UART1, UART_FLAG_RxFIFOEmpty) != SET)
354
                        { // some byes in the hardware fifo
361
                        { // some byes in the hardware fifo
355
                            // get byte from hardware fifo
362
                            // get byte from hardware fifo
356
                        c = UART_ReceiveData(UART1);
363
                        c = UART_ReceiveData(UART1);
357
                                // put into the software fifo
364
                                // put into the software fifo
358
                                if(!fifo_put(&UART1_rx_fifo, c))
365
                                if(!fifo_put(&UART1_rx_fifo, c))
359
                                {       // fifo overflow
366
                                {       // fifo overflow
360
                                        //fifo_purge(&UART1_rx_fifo); // flush the whole buffer
367
                                        //fifo_purge(&UART1_rx_fifo); // flush the whole buffer
361
                                }
368
                                }
362
                        } // EOF while some byes in the hardware fifo
369
                        } // EOF while some byes in the hardware fifo
363
                } // eof DebugUart = UART1
370
                } // eof DebugUart = UART1
364
        }
371
        }
365
 
372
 
366
 
373
 
367
 
374
 
368
        VIC1->VAR = 0xFF; // write any value to VIC1 Vector address register
375
        VIC1->VAR = 0xFF; // write any value to VIC1 Vector address register
369
}
376
}
370
 
377
 
371
/**************************************************************/
378
/**************************************************************/
372
/* Process incomming data from debug uart                     */
379
/* Process incomming data from debug uart                     */
373
/**************************************************************/
380
/**************************************************************/
374
void UART1_ProcessRxData(void)
381
void UART1_ProcessRxData(void)
375
{
382
{
376
        // return on forwarding uart  or unlocked rx buffer
383
        // return on forwarding uart  or unlocked rx buffer
377
        u8 c;
384
        u8 c;
378
        if(DebugUART != UART1) return;
385
        if(DebugUART != UART1) return;
379
        // if rx buffer is not locked
386
        // if rx buffer is not locked
380
        if(UART1_rx_buffer.Locked == FALSE)
387
        if(UART1_rx_buffer.Locked == FALSE)
381
        {
388
        {
382
                //collect data from primary rx fifo
389
                //collect data from primary rx fifo
383
                while(fifo_get(&UART1_rx_fifo, &c))
390
                while(fifo_get(&UART1_rx_fifo, &c))
384
                {
391
                {
385
                        // break if complete frame is collected
392
                        // break if complete frame is collected
386
                        if(MKProtocol_CollectSerialFrame(&UART1_rx_buffer, c)) break;
393
                        if(MKProtocol_CollectSerialFrame(&UART1_rx_buffer, c)) break;
387
                }
394
                }
388
        }
395
        }
389
        if(UART1_rx_buffer.Locked == FALSE) return;
396
        if(UART1_rx_buffer.Locked == FALSE) return;
390
 
397
 
391
        Point_t * pPoint = NULL;
398
        Point_t * pPoint = NULL;
392
        SerialMsg_t SerialMsg;
399
        SerialMsg_t SerialMsg;
393
 
400
 
394
        // analyze header first
401
        // analyze header first
395
        MKProtocol_DecodeSerialFrameHeader(&UART1_rx_buffer, &SerialMsg);
402
        MKProtocol_DecodeSerialFrameHeader(&UART1_rx_buffer, &SerialMsg);
396
        if( SerialMsg.Address == FC_ADDRESS )
403
        if( SerialMsg.Address == FC_ADDRESS )
397
        {
404
        {
398
                switch(SerialMsg.CmdID)
405
                switch(SerialMsg.CmdID)
399
                {
406
                {
400
//                      case 'v': // version
407
//                      case 'v': // version
401
                        case 'b': // extern control
408
                        case 'b': // extern control
402
                                          UART1_ExternalControlConfirmFrame = 1;
409
                                          UART1_ExternalControlConfirmFrame = 1;
403
                        case 'y': // serial poti values
410
                        case 'y': // serial poti values
404
                                Buffer_Copy(&UART1_rx_buffer, &UART2_tx_buffer); //forward to FC
411
                                Buffer_Copy(&UART1_rx_buffer, &UART2_tx_buffer); //forward to FC
405
                                Buffer_Clear(&UART1_rx_buffer); // free rc buffer for next frame
412
                                Buffer_Clear(&UART1_rx_buffer); // free rc buffer for next frame
406
                                return; //end process rx data
413
                                return; //end process rx data
407
                        break;
414
                        break;
408
                }
415
                }
409
        }
416
        }
410
 
417
 
411
        MKProtocol_DecodeSerialFrameData(&UART1_rx_buffer, &SerialMsg); // decode serial frame in rxd buffer
418
        MKProtocol_DecodeSerialFrameData(&UART1_rx_buffer, &SerialMsg); // decode serial frame in rxd buffer
412
    if(SerialMsg.CmdID != 'z') SerialLinkOkay = 250;      // reset SerialTimeout, but not in case of the "ping"
419
    if(SerialMsg.CmdID != 'z') SerialLinkOkay = 250;      // reset SerialTimeout, but not in case of the "ping"
413
        switch(SerialMsg.Address) // check for Slave Address
420
        switch(SerialMsg.Address) // check for Slave Address
414
        {
421
        {
415
                case NC_ADDRESS:  // own Slave Address
422
                case NC_ADDRESS:  // own Slave Address
416
                switch(SerialMsg.CmdID)
423
                switch(SerialMsg.CmdID)
417
                {
424
                {
418
                        case 't': // request for the GPS time
425
                        case 't': // request for the GPS time
419
                                UART1_Request_SystemTime  = TRUE;
426
                                UART1_Request_SystemTime  = TRUE;
420
                                break;
427
                                break;
421
 
428
 
422
                        case 'f': // ftp command
429
                        case 'f': // ftp command
423
                                UART1_Request_FTP = SerialMsg.pData[0];
430
                                UART1_Request_FTP = SerialMsg.pData[0];
424
                                //if (UART1_Request_FTP == FTP_CMD_SET_CWD || UART1_Request_FTP == FTP_CMD_GET_FILE)
431
                                //if (UART1_Request_FTP == FTP_CMD_SET_CWD || UART1_Request_FTP == FTP_CMD_GET_FILE)
425
                                memcpy(&FTP_data, &SerialMsg.pData[1], sizeof(FTP_data)); // copy ftp parameter
432
                                memcpy(&FTP_data, &SerialMsg.pData[1], sizeof(FTP_data)); // copy ftp parameter
426
                        break;
433
                        break;
427
 
434
 
428
                        case 'z': // connection checker
435
                        case 'z': // connection checker
429
                                memcpy(&Echo, SerialMsg.pData, sizeof(Echo)); // copy echo pattern
436
                                memcpy(&Echo, SerialMsg.pData, sizeof(Echo)); // copy echo pattern
430
                                UART1_Request_Echo = TRUE;
437
                                UART1_Request_Echo = TRUE;
431
                                break;
438
                                break;
432
 
439
 
433
                        case 'e': // request for the text of the error status
440
                        case 'e': // request for the text of the error status
434
                                UART1_Request_ErrorMessage = TRUE;
441
                                UART1_Request_ErrorMessage = TRUE;
435
                                break;
442
                                break;
436
 
443
 
437
                        case 's'://  new target position
444
                        case 's'://  new target position
438
                                pPoint = (Point_t*)SerialMsg.pData;
445
                                pPoint = (Point_t*)SerialMsg.pData;
439
                                if(pPoint->Position.Status == NEWDATA)
446
                                if(pPoint->Position.Status == NEWDATA)
440
                                {
447
                                {
441
                                        //if(!(FC.StatusFlags & FC_STATUS_FLY)) PointList_Clear(); // flush the list
448
                                        //if(!(FC.StatusFlags & FC_STATUS_FLY)) PointList_Clear(); // flush the list
442
                                        //pPoint->Index = 1; // must be one after empty list
449
                                        //pPoint->Index = 1; // must be one after empty list
443
                                        PointList_SetAt(pPoint);
450
                                        PointList_SetAt(pPoint);
444
                                        if(FC.StatusFlags & FC_STATUS_FLY) PointList_WPActive(TRUE);
451
                                        if(FC.StatusFlags & FC_STATUS_FLY) PointList_WPActive(TRUE);
445
                                        GPS_pWaypoint = PointList_WPBegin(); // updates POI index
452
                                        GPS_pWaypoint = PointList_WPBegin(); // updates POI index
446
                                        if(GPS_pWaypoint != NULL) // if new WP exist
453
                                        if(GPS_pWaypoint != NULL) // if new WP exist
447
                                        {   // update WP hold time stamp immediately!
454
                                        {   // update WP hold time stamp immediately!
448
                                                EnableNewWpHeading();
455
                                                EnableNewWpHeading();
449
/*                                              if(GPS_pWaypoint->Heading > 0 && GPS_pWaypoint->Heading <= 360)
456
/*                                              if(GPS_pWaypoint->Heading > 0 && GPS_pWaypoint->Heading <= 360)
450
                                                {
457
                                                {
451
                                                 CAM_Orientation.Azimuth = GPS_pWaypoint->Heading;
458
                                                 CAM_Orientation.Azimuth = GPS_pWaypoint->Heading;
452
                                                 CAM_Orientation.UpdateMask |= CAM_UPDATE_AZIMUTH;
459
                                                 CAM_Orientation.UpdateMask |= CAM_UPDATE_AZIMUTH;
453
                                                }
460
                                                }
454
*/
461
*/
455
                                        }
462
                                        }
456
                                        BeepTime = 50;
463
                                        BeepTime = 50;
457
                                }
464
                                }
458
                                break;
465
                                break;
459
 
466
 
460
                        case 'u': // redirect debug uart
467
                        case 'u': // redirect debug uart
461
                                switch(SerialMsg.pData[0])
468
                                switch(SerialMsg.pData[0])
462
                                {
469
                                {
463
                                        case UART_FLIGHTCTRL:
470
                                        case UART_FLIGHTCTRL:
464
                                                UART2_Init();                           // initialize UART2 to FC pins
471
                                                UART2_Init();                           // initialize UART2 to FC pins
465
                                                fifo_purge(&UART1_rx_fifo);
472
                                                fifo_purge(&UART1_rx_fifo);
466
                                                TIMER2_Deinit();                        // reduce irq load
473
                                                TIMER2_Deinit();                        // reduce irq load
467
                                                DebugUART = UART2;
474
                                                DebugUART = UART2;
468
                                                break;
475
                                                break;
469
                                        case UART_MK3MAG:
476
                                        case UART_MK3MAG:
470
                                                if(FC.StatusFlags & FC_STATUS_MOTOR_RUN) break; // not if the motors are running
477
                                                if(FC.StatusFlags & FC_STATUS_MOTOR_RUN) break; // not if the motors are running
471
                                                UART0_Connect_to_MK3MAG();      // mux UART0 to MK3MAG pins
478
                                                UART0_Connect_to_MK3MAG();      // mux UART0 to MK3MAG pins
472
                                                GPSData.Status = INVALID;
479
                                                GPSData.Status = INVALID;
473
                                                fifo_purge(&UART1_rx_fifo);
480
                                                fifo_purge(&UART1_rx_fifo);
474
                                                DebugUART = UART0;
481
                                                DebugUART = UART0;
475
                                                break;
482
                                                break;
476
                                        case UART_MKGPS:
483
                                        case UART_MKGPS:
477
                                                if(FC.StatusFlags & FC_STATUS_MOTOR_RUN) break; // not if the motors are running
484
                                                if(FC.StatusFlags & FC_STATUS_MOTOR_RUN) break; // not if the motors are running
478
                                                TIMER2_Deinit();                        // disable servo outputs to reduce irq load
485
                                                TIMER2_Deinit();                        // disable servo outputs to reduce irq load
479
                                                UART0_Connect_to_MKGPS(UART0_BAUD_RATE);        // connect UART0 to MKGPS pins
486
                                                UART0_Connect_to_MKGPS(UART0_BAUD_RATE);        // connect UART0 to MKGPS pins
480
                                                GPSData.Status = INVALID;
487
                                                GPSData.Status = INVALID;
481
                                                fifo_purge(&UART1_rx_fifo);
488
                                                fifo_purge(&UART1_rx_fifo);
482
                                                DebugUART = UART0;
489
                                                DebugUART = UART0;
483
                                                break;
490
                                                break;
484
                                        default:
491
                                        default:
485
                                                break;
492
                                                break;
486
                                }
493
                                }
487
                                break;
494
                                break;
488
 
495
 
489
                        case 'w'://  Set point in list at index
496
                        case 'w'://  Set point in list at index
490
                                {
497
                                {
491
                                        pPoint = (Point_t*)SerialMsg.pData;
498
                                        pPoint = (Point_t*)SerialMsg.pData;
492
 
499
 
493
                                        if((pPoint->Position.Status == INVALID) && (pPoint->Index == 0))
500
                                        if((pPoint->Position.Status == INVALID) && (pPoint->Index == 0))
494
                                        {
501
                                        {
495
                                                PointList_Clear();
502
                                                PointList_Clear();
496
                                                GPS_pWaypoint = PointList_WPBegin();
503
                                                GPS_pWaypoint = PointList_WPBegin();
497
                                                UART1_Request_WritePoint = 0; // return new point count
504
                                                UART1_Request_WritePoint = 0; // return new point count
498
                                        }
505
                                        }
499
                                        else
506
                                        else
500
                                        {  // update WP in list at index
507
                                        {  // update WP in list at index
501
                                                if(pPoint->Index > MaxNumberOfWaypoints)
508
                                                if(pPoint->Index > MaxNumberOfWaypoints)
502
                                                {
509
                                                {
503
                                                        UART1_Request_WritePoint = 254;
510
                                                        UART1_Request_WritePoint = 254;
504
                                                        pPoint->Index = MaxNumberOfWaypoints;
511
                                                        pPoint->Index = MaxNumberOfWaypoints;
505
                                                }
512
                                                }
506
                                                else UART1_Request_WritePoint = PointList_SetAt(pPoint);
513
                                                else UART1_Request_WritePoint = PointList_SetAt(pPoint);
507
                                                if(FC.StatusFlags & FC_STATUS_FLY) PointList_WPActive(TRUE);
514
                                                if(FC.StatusFlags & FC_STATUS_FLY) PointList_WPActive(TRUE);
508
                                                if(UART1_Request_WritePoint == pPoint->Index)
515
                                                if(UART1_Request_WritePoint == pPoint->Index)
509
                                                {
516
                                                {
510
                                                        BeepTime = 500;
517
                                                        BeepTime = 500;
511
                                                }
518
                                                }
512
                                        }
519
                                        }
513
                                }
520
                                }
514
                                break;
521
                                break;
515
 
522
 
516
                        case 'x'://  Read Waypoint from List
523
                        case 'x'://  Read Waypoint from List
517
                                UART1_Request_ReadPoint = SerialMsg.pData[0];
524
                                UART1_Request_ReadPoint = SerialMsg.pData[0];
518
                                break;
525
                                break;
519
 
526
 
520
                        case 'i':// Store WP List to file
527
                        case 'i':// Store WP List to file
-
 
528
 
-
 
529
                                memcpy((u8*)&WPL_Store, SerialMsg.pData, sizeof(WPL_Store));
-
 
530
                                WPL_Store.Name[11] = 0; // make sure the string is terminated
-
 
531
 
-
 
532
                                WPL_Answer.Index = WPL_Store.Index; // echo Index
-
 
533
                                WPL_Answer.Status = WPL_ERROR; // set bad state by default
-
 
534
 
-
 
535
                                if(WPL_Store.Index != 0) // valid index
521
                                {
536
                                {
522
                                        memcpy((u8*)&WPL_Store, SerialMsg.pData, sizeof(WPL_Store));
-
 
523
                                        WPL_Store.Name[11] = 0; // make sure the string is terminated
-
 
524
                                        WPL_Store.reserved[0] = 0;
-
 
525
                                        if(WPL_Store.Index != 0)
537
                                        if(WPL_Store.Type == WPL_STORE_TYPE_REL)
526
                                        {
538
                                        {
527
                                                if(WPL_Store.Type == WPL_STORE_TYPE_REL)
-
 
528
                                                {
-
 
529
                                                        if(PointList_Move(1, &(GPSData.Position)))
539
                                                if(PointList_Move(1, &(NaviData.HomePosition)))
530
                                                        {
540
                                                {      
531
                                                                WPL_Store.reserved[0] = PointList_SaveToFile(WPL_Store.Index, WPL_Store.Name);
541
                                                        WPL_Answer.Status = PointList_SaveToFile(&WPL_Store);
532
                                                        }
-
 
533
                                                }
542
                                                }
534
                                                else
-
 
535
                                                {
-
 
536
                                                        WPL_Store.reserved[0] = PointList_SaveToFile(WPL_Store.Index, WPL_Store.Name);
-
 
537
                                                }      
-
 
538
                                        }
543
                                        }
-
 
544
                                        else
-
 
545
                                        {
539
                                        UART1_Request_WPLStore = TRUE;
546
                                                WPL_Answer.Status = PointList_SaveToFile(&WPL_Store);
-
 
547
                                        }      
540
                                }
548
                                }
-
 
549
                                UART1_Request_WPLStore = TRUE;
-
 
550
 
541
                                break;
551
                                break;
542
 
552
 
543
 
553
 
544
                        case 'j':// Set/Get NC-Parameter
554
                        case 'j':// Set/Get NC-Parameter
545
                                switch(SerialMsg.pData[0])
555
                                switch(SerialMsg.pData[0])
546
                                {
556
                                {
547
                                        case 0: // get
557
                                        case 0: // get
548
                                        break;
558
                                        break;
549
 
559
 
550
                                        case 1: // set
560
                                        case 1: // set
551
                                        {
561
                                        {
552
                                                s16 value;
562
                                                s16 value;
553
                                                value = SerialMsg.pData[2] + (s16)SerialMsg.pData[3] * 0x0100;
563
                                                value = SerialMsg.pData[2] + (s16)SerialMsg.pData[3] * 0x0100;
554
                                                NCParams_SetValue(SerialMsg.pData[1], &value);
564
                                                NCParams_SetValue(SerialMsg.pData[1], &value);
555
                                        }
565
                                        }
556
                                        break;
566
                                        break;
557
                                       
567
                                       
558
                                        default:
568
                                        default:
559
                                        break;
569
                                        break;
560
                                }
570
                                }
561
                                UART1_Request_ParameterId = SerialMsg.pData[1];
571
                                UART1_Request_ParameterId = SerialMsg.pData[1];
562
                                UART1_Request_Parameter = TRUE;
572
                                UART1_Request_Parameter = TRUE;
563
                                break;
573
                                break;
564
                        default:
574
                        default:
565
                                // unsupported command recieved
575
                                // unsupported command recieved
566
                                break;
576
                                break;
567
                } // case NC_ADDRESS
577
                } // case NC_ADDRESS
568
                // "break;" is missing here to fall thru to the common commands
578
                // "break;" is missing here to fall thru to the common commands
569
 
579
 
570
                default:  // and any other Slave Address
580
                default:  // and any other Slave Address
571
 
581
 
572
                switch(SerialMsg.CmdID) // check CmdID
582
                switch(SerialMsg.CmdID) // check CmdID
573
                {
583
                {
574
                        case 'a':// request for the labels of the analog debug outputs
584
                        case 'a':// request for the labels of the analog debug outputs
575
                                UART1_Request_DebugLabel = SerialMsg.pData[0];
585
                                UART1_Request_DebugLabel = SerialMsg.pData[0];
576
                                if(UART1_Request_DebugLabel > 31) UART1_Request_DebugLabel = 31;
586
                                if(UART1_Request_DebugLabel > 31) UART1_Request_DebugLabel = 31;
577
                                break;
587
                                break;
578
                        /*
588
                        /*
579
                        case 'b': // submit extern control
589
                        case 'b': // submit extern control
580
                                memcpy(&ExternControl, SerialMsg.pData, sizeof(ExternControl));
590
                                memcpy(&ExternControl, SerialMsg.pData, sizeof(ExternControl));
581
                                UART1_ConfirmFrame = ExternControl.Frame;
591
                                UART1_ConfirmFrame = ExternControl.Frame;
582
                                break;
592
                                break;
583
                        */
593
                        */
584
                        case 'd': // request for debug data;
594
                        case 'd': // request for debug data;
585
                                UART1_DebugData_Interval = (u32) SerialMsg.pData[0] * 10;
595
                                UART1_DebugData_Interval = (u32) SerialMsg.pData[0] * 10;
586
                                if(UART1_DebugData_Interval > 0) UART1_Request_DebugData = TRUE;
596
                                if(UART1_DebugData_Interval > 0) UART1_Request_DebugData = TRUE;
587
                                UART1_AboTimeOut = SetDelay(ABO_TIMEOUT);
597
                                UART1_AboTimeOut = SetDelay(ABO_TIMEOUT);
588
                                break;
598
                                break;
589
 
599
 
590
                        case 'c': // request for 3D data;
600
                        case 'c': // request for 3D data;
591
                                UART1_Data3D_Interval = (u32) SerialMsg.pData[0] * 10;
601
                                UART1_Data3D_Interval = (u32) SerialMsg.pData[0] * 10;
592
                                if(UART1_Data3D_Interval > 0) UART1_Request_Data3D = TRUE;
602
                                if(UART1_Data3D_Interval > 0) UART1_Request_Data3D = TRUE;
593
                                UART1_AboTimeOut = SetDelay(ABO_TIMEOUT);
603
                                UART1_AboTimeOut = SetDelay(ABO_TIMEOUT);
594
                                break;
604
                                break;
595
 
605
 
596
                        case 'k': // request for Motor data;
606
                        case 'k': // request for Motor data;
597
                                UART1_MotorData_Interval = (u32) SerialMsg.pData[0] * 10;
607
                                UART1_MotorData_Interval = (u32) SerialMsg.pData[0] * 10;
598
                                if(UART1_MotorData_Interval > 0) UART1_Request_MotorData = TRUE;
608
                                if(UART1_MotorData_Interval > 0) UART1_Request_MotorData = TRUE;
599
                                UART1_AboTimeOut = SetDelay(ABO_TIMEOUT);
609
                                UART1_AboTimeOut = SetDelay(ABO_TIMEOUT);
600
                                break;
610
                                break;
601
 
611
 
602
                        case 'h':// reqest for display line
612
                        case 'h':// reqest for display line
603
                                if((SerialMsg.pData[0]& 0x80) == 0x00)// old format
613
                                if((SerialMsg.pData[0]& 0x80) == 0x00)// old format
604
                                {
614
                                {
605
                                        UART1_DisplayLine = 2;
615
                                        UART1_DisplayLine = 2;
606
                                        UART1_Display_Interval = 0;
616
                                        UART1_Display_Interval = 0;
607
                                }
617
                                }
608
                                else
618
                                else
609
                                {
619
                                {
610
                                        UART1_DisplayKeys |= ~SerialMsg.pData[0];
620
                                        UART1_DisplayKeys |= ~SerialMsg.pData[0];
611
                                        UART1_Display_Interval = (u32) SerialMsg.pData[1] * 10;
621
                                        UART1_Display_Interval = (u32) SerialMsg.pData[1] * 10;
612
                                        UART1_DisplayLine = 4;
622
                                        UART1_DisplayLine = 4;
613
                                        UART1_AboTimeOut = SetDelay(ABO_TIMEOUT);
623
                                        UART1_AboTimeOut = SetDelay(ABO_TIMEOUT);
614
                                }
624
                                }
615
                                UART1_Request_Display = TRUE;
625
                                UART1_Request_Display = TRUE;
616
                                break;
626
                                break;
617
 
627
 
618
                        case 'l':// reqest for display columns
628
                        case 'l':// reqest for display columns
619
                                MenuItem = SerialMsg.pData[0];
629
                                MenuItem = SerialMsg.pData[0];
620
                                UART1_Request_Display1 = TRUE;
630
                                UART1_Request_Display1 = TRUE;
621
                                break;
631
                                break;
622
 
632
 
623
                        case 'o': // request for navigation information
633
                        case 'o': // request for navigation information
624
                                UART1_NaviData_Interval = (u32) SerialMsg.pData[0] * 10;
634
                                UART1_NaviData_Interval = (u32) SerialMsg.pData[0] * 10;
625
                                if(UART1_NaviData_Interval > 0) UART1_Request_NaviData = TRUE;
635
                                if(UART1_NaviData_Interval > 0) UART1_Request_NaviData = TRUE;
626
                                UART1_AboTimeOut = SetDelay(ABO_TIMEOUT);
636
                                UART1_AboTimeOut = SetDelay(ABO_TIMEOUT);
627
                                break;
637
                                break;
628
 
638
 
629
                        case 'v': // request for version info
639
                        case 'v': // request for version info
630
                                UART1_Request_VersionInfo = TRUE;
640
                                UART1_Request_VersionInfo = TRUE;
631
                                break;
641
                                break;
632
                        default:
642
                        default:
633
                                // unsupported command recieved
643
                                // unsupported command recieved
634
                                break;
644
                                break;
635
                }
645
                }
636
                break; // default:
646
                break; // default:
637
        }
647
        }
638
        Buffer_Clear(&UART1_rx_buffer); // free rc buffer for next frame
648
        Buffer_Clear(&UART1_rx_buffer); // free rc buffer for next frame
639
}
649
}
640
 
650
 
641
 
651
 
642
/*****************************************************/
652
/*****************************************************/
643
/*                   Send a character                */
653
/*                   Send a character                */
644
/*****************************************************/
654
/*****************************************************/
645
s16 UART1_Putchar(char c)
655
s16 UART1_Putchar(char c)
646
{
656
{
647
        u32 timeout = 10000;
657
        u32 timeout = 10000;
648
        if (c == '\n') UART1_Putchar('\r');
658
        if (c == '\n') UART1_Putchar('\r');
649
        // wait until txd fifo is not full
659
        // wait until txd fifo is not full
650
        while(UART_GetFlagStatus(UART1, UART_FLAG_TxFIFOFull) != RESET) if(--timeout == 0) return(0);
660
        while(UART_GetFlagStatus(UART1, UART_FLAG_TxFIFOFull) != RESET) if(--timeout == 0) return(0);
651
        // transmit byte
661
        // transmit byte
652
        UART_SendData(UART1, c);
662
        UART_SendData(UART1, c);
653
        #ifdef FOLLOW_ME
663
        #ifdef FOLLOW_ME
654
        if(TransmitAlsoToFC) UART_SendData(UART2, c);
664
        if(TransmitAlsoToFC) UART_SendData(UART2, c);
655
        #endif
665
        #endif
656
        return (0);
666
        return (0);
657
}
667
}
658
 
668
 
659
/*****************************************************/
669
/*****************************************************/
660
/*       Send a string to the debug uart              */
670
/*       Send a string to the debug uart              */
661
/*****************************************************/
671
/*****************************************************/
662
void UART1_PutString(u8 *s)
672
void UART1_PutString(u8 *s)
663
{
673
{
664
        if(s == NULL) return;
674
        if(s == NULL) return;
665
        while (*s != '\0' && DebugUART == UART1)
675
        while (*s != '\0' && DebugUART == UART1)
666
        {
676
        {
667
                UART1_Putchar(*s);
677
                UART1_Putchar(*s);
668
                s ++;
678
                s ++;
669
        }
679
        }
670
}
680
}
671
 
681
 
672
 
682
 
673
/**************************************************************/
683
/**************************************************************/
674
/*         Transmit tx buffer via debug uart                  */
684
/*         Transmit tx buffer via debug uart                  */
675
/**************************************************************/
685
/**************************************************************/
676
void UART1_Transmit(void)
686
void UART1_Transmit(void)
677
{
687
{
678
        u8 tmp_tx;
688
        u8 tmp_tx;
679
        if(DebugUART != UART1) return;
689
        if(DebugUART != UART1) return;
680
        // if something has to be send and the txd fifo is not full
690
        // if something has to be send and the txd fifo is not full
681
        if(UART1_tx_buffer.Locked == TRUE)
691
        if(UART1_tx_buffer.Locked == TRUE)
682
        {
692
        {
683
                // while there is some space in the tx fifo
693
                // while there is some space in the tx fifo
684
                while(UART_GetFlagStatus(UART1, UART_FLAG_TxFIFOFull) != SET)
694
                while(UART_GetFlagStatus(UART1, UART_FLAG_TxFIFOFull) != SET)
685
                {
695
                {
686
                        tmp_tx = UART1_tx_buffer.pData[UART1_tx_buffer.Position++]; // read next byte from txd buffer
696
                        tmp_tx = UART1_tx_buffer.pData[UART1_tx_buffer.Position++]; // read next byte from txd buffer
687
                        UART_SendData(UART1, tmp_tx); // put character to txd fifo
697
                        UART_SendData(UART1, tmp_tx); // put character to txd fifo
688
                        #ifdef FOLLOW_ME
698
                        #ifdef FOLLOW_ME
689
                        if(TransmitAlsoToFC)
699
                        if(TransmitAlsoToFC)
690
                        {
700
                        {
691
                                UART_SendData(UART2, tmp_tx); // put character to txd fifo
701
                                UART_SendData(UART2, tmp_tx); // put character to txd fifo
692
                        }
702
                        }
693
                        #endif
703
                        #endif
694
                        // if terminating character or end of txd buffer reached
704
                        // if terminating character or end of txd buffer reached
695
                        if((tmp_tx == '\0') || (UART1_tx_buffer.Position == UART1_tx_buffer.DataBytes))
705
                        if((tmp_tx == '\0') || (UART1_tx_buffer.Position == UART1_tx_buffer.DataBytes))
696
                        {
706
                        {
697
                                Buffer_Clear(&UART1_tx_buffer); // clear txd buffer
707
                                Buffer_Clear(&UART1_tx_buffer); // clear txd buffer
698
                                #ifdef FOLLOW_ME
708
                                #ifdef FOLLOW_ME
699
                                TransmitAlsoToFC = 0;
709
                                TransmitAlsoToFC = 0;
700
                                #endif
710
                                #endif
701
                                break; // end while loop
711
                                break; // end while loop
702
                        }
712
                        }
703
                }
713
                }
704
        }
714
        }
705
}
715
}
706
 
716
 
707
//$GPGGA,HHMMSS.ss,BBBB.BBBB,b,LLLLL.LLLL,l,Q,NN,D.D,H.H,h,G.G,g,A.A,RRRR*PP
717
//$GPGGA,HHMMSS.ss,BBBB.BBBB,b,LLLLL.LLLL,l,Q,NN,D.D,H.H,h,G.G,g,A.A,RRRR*PP
708
//$GPGGA,191410,4735.5634,N,00739.3538,E,1,04,4.4,351.5,M,48.0,M,,*45
718
//$GPGGA,191410,4735.5634,N,00739.3538,E,1,04,4.4,351.5,M,48.0,M,,*45
709
//$GPGGA,092120.20,,,,,0,00,99.99,,,,,,*6C
719
//$GPGGA,092120.20,,,,,0,00,99.99,,,,,,*6C
710
//http://www.kowoma.de/gps/zusatzerklaerungen/NMEA.htm
720
//http://www.kowoma.de/gps/zusatzerklaerungen/NMEA.htm
711
void CreateNmeaGGA(void)
721
void CreateNmeaGGA(void)
712
{
722
{
713
        u8 array[200], i = 0, crc = 0, x;
723
        u8 array[200], i = 0, crc = 0, x;
714
        s32 tmp1, tmp2;
724
        s32 tmp1, tmp2;
715
 
725
 
716
        i += sprintf(array, "$GPGGA,");
726
        i += sprintf(array, "$GPGGA,");
717
        // +++++++++++++++++++++++++++++++++++++++++++
727
        // +++++++++++++++++++++++++++++++++++++++++++
718
        if(SystemTime.Valid)
728
        if(SystemTime.Valid)
719
        {
729
        {
720
                i += sprintf(&array[i], "%02d%02d%02d.%02d,", SystemTime.Hour, SystemTime.Min, SystemTime.Sec, SystemTime.mSec/10);
730
                i += sprintf(&array[i], "%02d%02d%02d.%02d,", SystemTime.Hour, SystemTime.Min, SystemTime.Sec, SystemTime.mSec/10);
721
        }
731
        }
722
        else
732
        else
723
        {
733
        {
724
                i += sprintf(&array[i], ",");
734
                i += sprintf(&array[i], ",");
725
        }
735
        }
726
        // +++++++++++++++++++++++++++++++++++++++++++
736
        // +++++++++++++++++++++++++++++++++++++++++++
727
        if(GPSData.Flags & FLAG_GPSFIXOK)
737
        if(GPSData.Flags & FLAG_GPSFIXOK)
728
        {
738
        {
729
                tmp1 = abs(GPSData.Position.Latitude)/10000000L;
739
                tmp1 = abs(GPSData.Position.Latitude)/10000000L;
730
                i += sprintf(&array[i],"%02d",(s16)tmp1);
740
                i += sprintf(&array[i],"%02d",(s16)tmp1);
731
 
741
 
732
                tmp1 = abs(GPSData.Position.Latitude)%10000000L;
742
                tmp1 = abs(GPSData.Position.Latitude)%10000000L;
733
                tmp1 *= 6; // in Minuten
743
                tmp1 *= 6; // in Minuten
734
                tmp2 = tmp1 / 1000000L;
744
                tmp2 = tmp1 / 1000000L;
735
                i += sprintf(&array[i],"%02d", (s16)tmp2);
745
                i += sprintf(&array[i],"%02d", (s16)tmp2);
736
                tmp2 = tmp1 % 1000000L;
746
                tmp2 = tmp1 % 1000000L;
737
                tmp2 /= 10; // eine Stelle zu viel
747
                tmp2 /= 10; // eine Stelle zu viel
738
                i += sprintf(&array[i],".%05d,", (s16)tmp2);
748
                i += sprintf(&array[i],".%05d,", (s16)tmp2);
739
 
749
 
740
                if(GPSData.Position.Latitude >= 0) i += sprintf(&array[i],"N,");
750
                if(GPSData.Position.Latitude >= 0) i += sprintf(&array[i],"N,");
741
                else i += sprintf(&array[i],"S,");
751
                else i += sprintf(&array[i],"S,");
742
                // +++++++++++++++++++++++++++++++++++++++++++
752
                // +++++++++++++++++++++++++++++++++++++++++++
743
 
753
 
744
                tmp1 = abs(GPSData.Position.Longitude)/10000000L;
754
                tmp1 = abs(GPSData.Position.Longitude)/10000000L;
745
                i += sprintf(&array[i],"%03d", (s16)tmp1);
755
                i += sprintf(&array[i],"%03d", (s16)tmp1);
746
 
756
 
747
                tmp1 = abs(GPSData.Position.Longitude)%10000000L;
757
                tmp1 = abs(GPSData.Position.Longitude)%10000000L;
748
                tmp1 *= 6; // in Minuten
758
                tmp1 *= 6; // in Minuten
749
                tmp2 = tmp1 / 1000000L;
759
                tmp2 = tmp1 / 1000000L;
750
                i += sprintf(&array[i],"%02d", (s16)tmp2);
760
                i += sprintf(&array[i],"%02d", (s16)tmp2);
751
                tmp2 = tmp1 % 1000000L;
761
                tmp2 = tmp1 % 1000000L;
752
                tmp2 /= 10; // eine Stelle zu viel
762
                tmp2 /= 10; // eine Stelle zu viel
753
                i += sprintf(&array[i],".%05d,", (s16)tmp2);
763
                i += sprintf(&array[i],".%05d,", (s16)tmp2);
754
 
764
 
755
 
765
 
756
                if(GPSData.Position.Longitude >= 0) i += sprintf(&array[i],"E,");
766
                if(GPSData.Position.Longitude >= 0) i += sprintf(&array[i],"E,");
757
                else i += sprintf(&array[i],"W,");
767
                else i += sprintf(&array[i],"W,");
758
                i += sprintf(&array[i],"%d,",GPSData.SatFix);
768
                i += sprintf(&array[i],"%d,",GPSData.SatFix);
759
                i += sprintf(&array[i],"%d,",GPSData.NumOfSats);
769
                i += sprintf(&array[i],"%d,",GPSData.NumOfSats);
760
                i += sprintf(&array[i],"%d.%d,",(s16)(GPSData.Position_Accuracy/100),abs(GPSData.Position_Accuracy%100));
770
                i += sprintf(&array[i],"%d.%d,",(s16)(GPSData.Position_Accuracy/100),abs(GPSData.Position_Accuracy%100));
761
                //  i += sprintf(&array[i],"%d.%d,M,",GPSData.Position.Altitude/1000,abs(GPSData.Position.Altitude%1000)/100);
771
                //  i += sprintf(&array[i],"%d.%d,M,",GPSData.Position.Altitude/1000,abs(GPSData.Position.Altitude%1000)/100);
762
                tmp1 = NaviData.Altimeter / 2; // in dm
772
                tmp1 = NaviData.Altimeter / 2; // in dm
763
                i += sprintf(&array[i],"%d.%d,M,",(s16)tmp1 / 10,abs((s16)tmp1 % 10));
773
                i += sprintf(&array[i],"%d.%d,M,",(s16)tmp1 / 10,abs((s16)tmp1 % 10));
764
                i += sprintf(&array[i],",,,*");
774
                i += sprintf(&array[i],",,,*");
765
        }
775
        }
766
        else
776
        else
767
        {
777
        {
768
                i += sprintf(&array[i], ",,,,%d,00,99.99,,,,,,*",GPSData.NumOfSats);
778
                i += sprintf(&array[i], ",,,,%d,00,99.99,,,,,,*",GPSData.NumOfSats);
769
        }
779
        }
770
        for(x = 1; x < i-1; x++)
780
        for(x = 1; x < i-1; x++)
771
        {
781
        {
772
                crc ^= array[x];
782
                crc ^= array[x];
773
        }
783
        }
774
        i += sprintf(&array[i], "%02x%c%c",crc,0x0d,0x0a);
784
        i += sprintf(&array[i], "%02x%c%c",crc,0x0d,0x0a);
775
        AddSerialData(&UART1_tx_buffer,array,i);
785
        AddSerialData(&UART1_tx_buffer,array,i);
776
 
786
 
777
        // +++++++++++++++++++++++++++++++++++++++++++
787
        // +++++++++++++++++++++++++++++++++++++++++++
778
}
788
}
779
 
789
 
780
//$GPRMC,162614.123,A,5230.5900,N,01322.3900,E,10.0,90.0,131006,1.2,E,A*13
790
//$GPRMC,162614.123,A,5230.5900,N,01322.3900,E,10.0,90.0,131006,1.2,E,A*13
781
//$GPRMC,HHMMSS.sss,A,BBBB.BBBB,b,LLLLL.LLLL,l,GG.G,RR.R,DDMMYY,M.M,m,F*PP
791
//$GPRMC,HHMMSS.sss,A,BBBB.BBBB,b,LLLLL.LLLL,l,GG.G,RR.R,DDMMYY,M.M,m,F*PP
782
 
792
 
783
void CreateNmeaRMC(void)
793
void CreateNmeaRMC(void)
784
{
794
{
785
        u8 array[200], i = 0, crc = 0, x;
795
        u8 array[200], i = 0, crc = 0, x;
786
        s16 tmp_int;
796
        s16 tmp_int;
787
        s32 tmp1, tmp2;
797
        s32 tmp1, tmp2;
788
        // +++++++++++++++++++++++++++++++++++++++++++
798
        // +++++++++++++++++++++++++++++++++++++++++++
789
        i += sprintf(array, "$GPRMC,");
799
        i += sprintf(array, "$GPRMC,");
790
        // +++++++++++++++++++++++++++++++++++++++++++
800
        // +++++++++++++++++++++++++++++++++++++++++++
791
        if(SystemTime.Valid)
801
        if(SystemTime.Valid)
792
        {
802
        {
793
                i += sprintf(&array[i], "%02d%02d%02d.%03d,", SystemTime.Hour, SystemTime.Min, SystemTime.Sec, SystemTime.mSec);
803
                i += sprintf(&array[i], "%02d%02d%02d.%03d,", SystemTime.Hour, SystemTime.Min, SystemTime.Sec, SystemTime.mSec);
794
        }
804
        }
795
        else
805
        else
796
        {
806
        {
797
                i += sprintf(&array[i], ",");
807
                i += sprintf(&array[i], ",");
798
        }
808
        }
799
        if(GPSData.Flags & FLAG_GPSFIXOK)
809
        if(GPSData.Flags & FLAG_GPSFIXOK)
800
        {
810
        {
801
                // +++++++++++++++++++++++++++++++++++++++++++
811
                // +++++++++++++++++++++++++++++++++++++++++++
802
                tmp1 = abs(GPSData.Position.Latitude)/10000000L;
812
                tmp1 = abs(GPSData.Position.Latitude)/10000000L;
803
                i += sprintf(&array[i],"A,%02d", (s16)tmp1); // Status: A = Okay  V = Warnung
813
                i += sprintf(&array[i],"A,%02d", (s16)tmp1); // Status: A = Okay  V = Warnung
804
 
814
 
805
                tmp1 = abs(GPSData.Position.Latitude)%10000000L;
815
                tmp1 = abs(GPSData.Position.Latitude)%10000000L;
806
                tmp1 *= 6; // in Minuten
816
                tmp1 *= 6; // in Minuten
807
                tmp2 = tmp1 / 1000000L;
817
                tmp2 = tmp1 / 1000000L;
808
                i += sprintf(&array[i],"%02d", (s16)tmp2);
818
                i += sprintf(&array[i],"%02d", (s16)tmp2);
809
                tmp2 = tmp1 % 1000000L;
819
                tmp2 = tmp1 % 1000000L;
810
                tmp2 /= 10; // eine Stelle zu viel
820
                tmp2 /= 10; // eine Stelle zu viel
811
                i += sprintf(&array[i],".%05d,", (s16)tmp2);
821
                i += sprintf(&array[i],".%05d,", (s16)tmp2);
812
                if(GPSData.Position.Latitude >= 0) i += sprintf(&array[i],"N,");
822
                if(GPSData.Position.Latitude >= 0) i += sprintf(&array[i],"N,");
813
                else i += sprintf(&array[i],"S,");
823
                else i += sprintf(&array[i],"S,");
814
                // +++++++++++++++++++++++++++++++++++++++++++
824
                // +++++++++++++++++++++++++++++++++++++++++++
815
                tmp1 = abs(GPSData.Position.Longitude)/10000000L;
825
                tmp1 = abs(GPSData.Position.Longitude)/10000000L;
816
                i += sprintf(&array[i],"%03d", (s16)tmp1);
826
                i += sprintf(&array[i],"%03d", (s16)tmp1);
817
 
827
 
818
                tmp1 = abs(GPSData.Position.Longitude)%10000000L;
828
                tmp1 = abs(GPSData.Position.Longitude)%10000000L;
819
                tmp1 *= 6; // in Minuten
829
                tmp1 *= 6; // in Minuten
820
                tmp2 = tmp1 / 1000000L;
830
                tmp2 = tmp1 / 1000000L;
821
                i += sprintf(&array[i],"%02d", (s16)tmp2);
831
                i += sprintf(&array[i],"%02d", (s16)tmp2);
822
                tmp2 = tmp1 % 1000000L;
832
                tmp2 = tmp1 % 1000000L;
823
                tmp2 /= 10; // eine Stelle zu viel
833
                tmp2 /= 10; // eine Stelle zu viel
824
                i += sprintf(&array[i],".%05d,", (s16)tmp2);
834
                i += sprintf(&array[i],".%05d,", (s16)tmp2);
825
                if(GPSData.Position.Longitude >= 0) i += sprintf(&array[i],"E,");
835
                if(GPSData.Position.Longitude >= 0) i += sprintf(&array[i],"E,");
826
                else i += sprintf(&array[i],"W,");
836
                else i += sprintf(&array[i],"W,");
827
                // +++++++++++++++++++++++++++++++++++++++++++
837
                // +++++++++++++++++++++++++++++++++++++++++++
828
                tmp_int = GPSData.Speed_Ground; // in cm/sek
838
                tmp_int = GPSData.Speed_Ground; // in cm/sek
829
                tmp_int *= 90;
839
                tmp_int *= 90;
830
                tmp_int /= 463;
840
                tmp_int /= 463;
831
                i += sprintf(&array[i],"%02d.%d,",tmp_int/10,tmp_int%10); // in Knoten
841
                i += sprintf(&array[i],"%02d.%d,",tmp_int/10,tmp_int%10); // in Knoten
832
                // +++++++++++++++++++++++++++++++++++++++++++
842
                // +++++++++++++++++++++++++++++++++++++++++++
833
                i += sprintf(&array[i],"%03d.%d,",GyroCompassCorrected/10,GyroCompassCorrected%10);
843
                i += sprintf(&array[i],"%03d.%d,",GyroCompassCorrected/10,GyroCompassCorrected%10);
834
                // +++++++++++++++++++++++++++++++++++++++++++
844
                // +++++++++++++++++++++++++++++++++++++++++++
835
                if(SystemTime.Valid)
845
                if(SystemTime.Valid)
836
                {
846
                {
837
                        i += sprintf(&array[i], "%02d%02d%02d,",SystemTime.Day,SystemTime.Month,SystemTime.Year);
847
                        i += sprintf(&array[i], "%02d%02d%02d,",SystemTime.Day,SystemTime.Month,SystemTime.Year);
838
                }
848
                }
839
                else
849
                else
840
                {
850
                {
841
                        i += sprintf(&array[i], ",");
851
                        i += sprintf(&array[i], ",");
842
                }
852
                }
843
                // +++++++++++++++++++++++++++++++++++++++++++
853
                // +++++++++++++++++++++++++++++++++++++++++++
844
                i += sprintf(&array[i],"%d.%1d,", abs(GeoMagDec)/10,abs(GeoMagDec)%10);
854
                i += sprintf(&array[i],"%d.%1d,", abs(GeoMagDec)/10,abs(GeoMagDec)%10);
845
                if(GeoMagDec < 0) i += sprintf(&array[i], "W,"); else i += sprintf(&array[i], "E,");
855
                if(GeoMagDec < 0) i += sprintf(&array[i], "W,"); else i += sprintf(&array[i], "E,");
846
                // +++++++++++++++++++++++++++++++++++++++++++
856
                // +++++++++++++++++++++++++++++++++++++++++++
847
                if(GPSData.Flags & FLAG_DIFFSOLN) i += sprintf(&array[i], "D,");
857
                if(GPSData.Flags & FLAG_DIFFSOLN) i += sprintf(&array[i], "D,");
848
                else i += sprintf(&array[i], "A,");
858
                else i += sprintf(&array[i], "A,");
849
        }
859
        }
850
        else // kein Satfix
860
        else // kein Satfix
851
        {
861
        {
852
                i += sprintf(&array[i], "V,,,,,,,,,,N*");
862
                i += sprintf(&array[i], "V,,,,,,,,,,N*");
853
        }
863
        }
854
        // +++++++++++++++++++++++++++++++++++++++++++
864
        // +++++++++++++++++++++++++++++++++++++++++++
855
        //  CRC
865
        //  CRC
856
        // +++++++++++++++++++++++++++++++++++++++++++
866
        // +++++++++++++++++++++++++++++++++++++++++++
857
        for(x=1; x<i-1; x++)
867
        for(x=1; x<i-1; x++)
858
        {
868
        {
859
                crc ^= array[x];
869
                crc ^= array[x];
860
        }
870
        }
861
        i += sprintf(&array[i], "%02x%c%c",crc,0x0d,0x0a);
871
        i += sprintf(&array[i], "%02x%c%c",crc,0x0d,0x0a);
862
        // +++++++++++++++++++++++++++++++++++++++++++
872
        // +++++++++++++++++++++++++++++++++++++++++++
863
        AddSerialData(&UART1_tx_buffer,array,i);
873
        AddSerialData(&UART1_tx_buffer,array,i);
864
        // +++++++++++++++++++++++++++++++++++++++++++
874
        // +++++++++++++++++++++++++++++++++++++++++++
865
/*
875
/*
866
 
876
 
867
 
877
 
868
 
878
 
869
                GPSData.Flags =         (GPSData.Flags & 0xf0) | (UbxSol.Flags & 0x0f); // we take only the lower bits
879
                GPSData.Flags =         (GPSData.Flags & 0xf0) | (UbxSol.Flags & 0x0f); // we take only the lower bits
870
                GPSData.NumOfSats =                     UbxSol.numSV;
880
                GPSData.NumOfSats =                     UbxSol.numSV;
871
                GPSData.SatFix =                                UbxSol.GPSfix;
881
                GPSData.SatFix =                                UbxSol.GPSfix;
872
                GPSData.Position_Accuracy =             UbxSol.PAcc;
882
                GPSData.Position_Accuracy =             UbxSol.PAcc;
873
                GPSData.Speed_Accuracy =                UbxSol.SAcc;
883
                GPSData.Speed_Accuracy =                UbxSol.SAcc;
874
                SetGPSTime(&SystemTime); // update system time
884
                SetGPSTime(&SystemTime); // update system time
875
                // NAV POSLLH
885
                // NAV POSLLH
876
                GPSData.Position.Status =               INVALID;
886
                GPSData.Position.Status =               INVALID;
877
                GPSData.Position.Longitude =    UbxPosLlh.LON;
887
                GPSData.Position.Longitude =    UbxPosLlh.LON;
878
                GPSData.Position.Latitude =     UbxPosLlh.LAT;
888
                GPSData.Position.Latitude =     UbxPosLlh.LAT;
879
                GPSData.Position.Altitude =     UbxPosLlh.HMSL;
889
                GPSData.Position.Altitude =     UbxPosLlh.HMSL;
880
                GPSData.Position.Status =               NEWDATA;
890
                GPSData.Position.Status =               NEWDATA;
881
                // NAV VELNED
891
                // NAV VELNED
882
                GPSData.Speed_East =                    UbxVelNed.VEL_E;
892
                GPSData.Speed_East =                    UbxVelNed.VEL_E;
883
                GPSData.Speed_North =                   UbxVelNed.VEL_N;
893
                GPSData.Speed_North =                   UbxVelNed.VEL_N;
884
                GPSData.Speed_Top       =                       -UbxVelNed.VEL_D;
894
                GPSData.Speed_Top       =                       -UbxVelNed.VEL_D;
885
                GPSData.Speed_Ground =                  UbxVelNed.GSpeed;
895
                GPSData.Speed_Ground =                  UbxVelNed.GSpeed;
886
                GPSData.Heading =                               UbxVelNed.Heading;
896
                GPSData.Heading =                               UbxVelNed.Heading;
887
                SystemTime.Year = 0;
897
                SystemTime.Year = 0;
888
                SystemTime.Month = 0;
898
                SystemTime.Month = 0;
889
                SystemTime.Day = 0;
899
                SystemTime.Day = 0;
890
                SystemTime.Hour = 0;
900
                SystemTime.Hour = 0;
891
                SystemTime.Min = 0;
901
                SystemTime.Min = 0;
892
                SystemTime.Sec = 0;
902
                SystemTime.Sec = 0;
893
                SystemTime.mSec = 0;
903
                SystemTime.mSec = 0;
894
                SystemTime.Valid = 0;
904
                SystemTime.Valid = 0;
895
 
905
 
896
                FromFlightCtrl.GyroHeading / 10;//NaviData.HomePositionDeviation.Bearing / 2;
906
                FromFlightCtrl.GyroHeading / 10;//NaviData.HomePositionDeviation.Bearing / 2;
897
                        if(GPSData.Position.Latitude < 0) ToFlightCtrl.Param.Byte[5]  = 1; // 1 = S
907
                        if(GPSData.Position.Latitude < 0) ToFlightCtrl.Param.Byte[5]  = 1; // 1 = S
898
                        else ToFlightCtrl.Param.Byte[5]  = 0; // 1 = S
908
                        else ToFlightCtrl.Param.Byte[5]  = 0; // 1 = S
899
                        i1 = abs(GPSData.Position.Latitude)/10000000L;
909
                        i1 = abs(GPSData.Position.Latitude)/10000000L;
900
                        i2 = abs(GPSData.Position.Latitude)%10000000L;
910
                        i2 = abs(GPSData.Position.Latitude)%10000000L;
901
 
911
 
902
 
912
 
903
 
913
 
904
                        if(!(NCFlags & NC_FLAG_GPS_OK)) {i1 = 0; i2 = 0;}
914
                        if(!(NCFlags & NC_FLAG_GPS_OK)) {i1 = 0; i2 = 0;}
905
                        i1 *= 100;
915
                        i1 *= 100;
906
                        i1 += i2 / 100000;
916
                        i1 += i2 / 100000;
907
                        i2  = i2 % 100000;
917
                        i2  = i2 % 100000;
908
                        i2 /= 10;
918
                        i2 /= 10;
909
                        ToFlightCtrl.Param.Byte[6]  = i1 % 256;
919
                        ToFlightCtrl.Param.Byte[6]  = i1 % 256;
910
                        ToFlightCtrl.Param.Byte[7]  = i1 / 256;
920
                        ToFlightCtrl.Param.Byte[7]  = i1 / 256;
911
                        ToFlightCtrl.Param.Byte[8]  = i2 % 256;
921
                        ToFlightCtrl.Param.Byte[8]  = i2 % 256;
912
                        ToFlightCtrl.Param.Byte[9]  = i2 / 256;
922
                        ToFlightCtrl.Param.Byte[9]  = i2 / 256;
913
                        break;
923
                        break;
914
                case 1:
924
                case 1:
915
                        ToFlightCtrl.Param.Byte[11] = HOTT_GPS_PACKET_ID;
925
                        ToFlightCtrl.Param.Byte[11] = HOTT_GPS_PACKET_ID;
916
                        ToFlightCtrl.Param.Byte[0] = 11+3;      // index          +3, weil bei HoTT V4 3 Bytes eingeschoben wurden
926
                        ToFlightCtrl.Param.Byte[0] = 11+3;      // index          +3, weil bei HoTT V4 3 Bytes eingeschoben wurden
917
                        ToFlightCtrl.Param.Byte[1] = 8-1;       // how many
927
                        ToFlightCtrl.Param.Byte[1] = 8-1;       // how many
918
                        //-----------------------------
928
                        //-----------------------------
919
                        if(GPSData.Position.Longitude < 0) ToFlightCtrl.Param.Byte[2]  = 1; // 1 = E
929
                        if(GPSData.Position.Longitude < 0) ToFlightCtrl.Param.Byte[2]  = 1; // 1 = E
920
                        else ToFlightCtrl.Param.Byte[2]  = 0; // 1 = S
930
                        else ToFlightCtrl.Param.Byte[2]  = 0; // 1 = S
921
                        i1 = abs(GPSData.Position.Longitude)/10000000L;
931
                        i1 = abs(GPSData.Position.Longitude)/10000000L;
922
                        i2 = abs(GPSData.Position.Longitude)%10000000L;
932
                        i2 = abs(GPSData.Position.Longitude)%10000000L;
923
 
933
 
924
*/
934
*/
925
}
935
}
926
 
936
 
927
 
937
 
928
/**************************************************************/
938
/**************************************************************/
929
/* Send the answers to incomming commands at the debug uart   */
939
/* Send the answers to incomming commands at the debug uart   */
930
/**************************************************************/
940
/**************************************************************/
931
void UART1_TransmitTxData(void)
941
void UART1_TransmitTxData(void)
932
{
942
{
933
static u8 motorindex1 = 255, motorindex2 = 0;
943
static u8 motorindex1 = 255, motorindex2 = 0;
934
        if(DebugUART != UART1) return;
944
        if(DebugUART != UART1) return;
935
 
945
 
936
        if(CheckDelay(UART1_AboTimeOut))
946
        if(CheckDelay(UART1_AboTimeOut))
937
        {
947
        {
938
                UART1_DebugData_Interval = 0;
948
                UART1_DebugData_Interval = 0;
939
                UART1_NaviData_Interval = 0;
949
                UART1_NaviData_Interval = 0;
940
                UART1_Data3D_Interval = 0;
950
                UART1_Data3D_Interval = 0;
941
                UART1_Display_Interval = 0;
951
                UART1_Display_Interval = 0;
942
                UART1_MotorData_Interval = 0;
952
                UART1_MotorData_Interval = 0;
943
        }
953
        }
944
 
954
 
945
        UART1_Transmit(); // output pending bytes in tx buffer
955
        UART1_Transmit(); // output pending bytes in tx buffer
946
        if((UART1_tx_buffer.Locked == TRUE)) return;
956
        if((UART1_tx_buffer.Locked == TRUE)) return;
947
 
957
 
948
        if(UART1_Request_Parameter && (UART1_tx_buffer.Locked == FALSE))
958
        if(UART1_Request_Parameter && (UART1_tx_buffer.Locked == FALSE))
949
        {
959
        {
950
                s16 ParamValue;
960
                s16 ParamValue;
951
                NCParams_GetValue(UART1_Request_ParameterId, &ParamValue);
961
                NCParams_GetValue(UART1_Request_ParameterId, &ParamValue);
952
                MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'J', NC_ADDRESS, 2, &UART1_Request_ParameterId, sizeof(UART1_Request_ParameterId), &ParamValue, sizeof(ParamValue)); // answer the param request
962
                MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'J', NC_ADDRESS, 2, &UART1_Request_ParameterId, sizeof(UART1_Request_ParameterId), &ParamValue, sizeof(ParamValue)); // answer the param request
953
                UART1_Request_Parameter = FALSE;
963
                UART1_Request_Parameter = FALSE;
954
        }
964
        }
955
        else if(UART1_Request_Echo && (UART1_tx_buffer.Locked == FALSE))
965
        else if(UART1_Request_Echo && (UART1_tx_buffer.Locked == FALSE))
956
        {
966
        {
957
                MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'Z', NC_ADDRESS, 1, &Echo, sizeof(Echo)); // answer the echo request
967
                MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'Z', NC_ADDRESS, 1, &Echo, sizeof(Echo)); // answer the echo request
958
                Echo = 0; // reset echo value
968
                Echo = 0; // reset echo value
959
                UART1_Request_Echo = FALSE;
969
                UART1_Request_Echo = FALSE;
960
        }
970
        }
961
        else if(UART1_Request_FTP && (UART1_tx_buffer.Locked == FALSE))
971
        else if(UART1_Request_FTP && (UART1_tx_buffer.Locked == FALSE))
962
        {
972
        {
963
                u8 errorcode = FTP_ERROR_NONE;
973
                u8 errorcode = FTP_ERROR_NONE;
964
        if(FC.StatusFlags & FC_STATUS_MOTOR_RUN) errorcode = FTP_ERROR_MOTOR_RUN;
974
        if(FC.StatusFlags & FC_STATUS_MOTOR_RUN) errorcode = FTP_ERROR_MOTOR_RUN;
965
                else if (!Partition.IsValid) errorcode = FTP_ERROR_NO_SDCARD;
975
                else if (!Partition.IsValid) errorcode = FTP_ERROR_NO_SDCARD;
966
 
976
 
967
                if (!errorcode) CheckFTPCommand(UART1_Request_FTP);
977
                if (!errorcode) CheckFTPCommand(UART1_Request_FTP);
968
                else
978
                else
969
                {
979
                {
970
                        u8 cmd = FTP_CMD_ERROR;
980
                        u8 cmd = FTP_CMD_ERROR;
971
                        MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'F', NC_ADDRESS, 2, &cmd, 1, &errorcode, 1);
981
                        MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'F', NC_ADDRESS, 2, &cmd, 1, &errorcode, 1);
972
                }
982
                }
973
 
983
 
974
                UART1_Request_FTP = FALSE;
984
                UART1_Request_FTP = FALSE;
975
        }
985
        }
976
        else if((UART1_Request_WritePoint!= 0xFF) && (UART1_tx_buffer.Locked == FALSE))
986
        else if((UART1_Request_WritePoint!= 0xFF) && (UART1_tx_buffer.Locked == FALSE))
977
        {
987
        {
978
                MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'W', NC_ADDRESS, 1, &UART1_Request_WritePoint, sizeof(UART1_Request_WritePoint));
988
                MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'W', NC_ADDRESS, 1, &UART1_Request_WritePoint, sizeof(UART1_Request_WritePoint));
979
                UART1_Request_WritePoint = 0xFF;
989
                UART1_Request_WritePoint = 0xFF;
980
        }
990
        }
981
        else if((UART1_Request_ReadPoint) && (UART1_tx_buffer.Locked == FALSE))
991
        else if((UART1_Request_ReadPoint) && (UART1_tx_buffer.Locked == FALSE))
982
        {
992
        {
983
                u8 PointCount = PointList_GetCount();
993
                u8 PointCount = PointList_GetCount();
984
                if (UART1_Request_ReadPoint <= PointCount)
994
                if (UART1_Request_ReadPoint <= PointCount)
985
                {
995
                {
986
                        MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'X', NC_ADDRESS, 3, &PointCount, 1, &UART1_Request_ReadPoint, 1, PointList_GetAt(UART1_Request_ReadPoint), sizeof(Point_t));
996
                        MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'X', NC_ADDRESS, 3, &PointCount, 1, &UART1_Request_ReadPoint, 1, PointList_GetAt(UART1_Request_ReadPoint), sizeof(Point_t));
987
                }
997
                }
988
                else
998
                else
989
                {
999
                {
990
                        MKProtocol_CreateSerialFrame(&UART1_tx_buffer,'X', NC_ADDRESS, 1, &PointCount, sizeof(PointCount));
1000
                        MKProtocol_CreateSerialFrame(&UART1_tx_buffer,'X', NC_ADDRESS, 1, &PointCount, sizeof(PointCount));
991
                }
1001
                }
992
                UART1_Request_ReadPoint = 0;
1002
                UART1_Request_ReadPoint = 0;
993
        }
1003
        }
994
        else if((UART1_Request_DebugLabel != 0xFF) && (UART1_tx_buffer.Locked == FALSE))
1004
        else if((UART1_Request_DebugLabel != 0xFF) && (UART1_tx_buffer.Locked == FALSE))
995
        {
1005
        {
996
                MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'A', NC_ADDRESS, 2, &UART1_Request_DebugLabel, sizeof(UART1_Request_DebugLabel), (u8 *) ANALOG_LABEL[UART1_Request_DebugLabel], 16);
1006
                MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'A', NC_ADDRESS, 2, &UART1_Request_DebugLabel, sizeof(UART1_Request_DebugLabel), (u8 *) ANALOG_LABEL[UART1_Request_DebugLabel], 16);
997
                UART1_Request_DebugLabel = 0xFF;
1007
                UART1_Request_DebugLabel = 0xFF;
998
        }
1008
        }
999
        else if(UART1_ExternalControlConfirmFrame && (UART1_tx_buffer.Locked == FALSE))
1009
        else if(UART1_ExternalControlConfirmFrame && (UART1_tx_buffer.Locked == FALSE))
1000
        {
1010
        {
1001
                MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'B', NC_ADDRESS, 1,(u8 *)&UART1_ExternalControlConfirmFrame, sizeof(UART1_ExternalControlConfirmFrame));
1011
                MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'B', NC_ADDRESS, 1,(u8 *)&UART1_ExternalControlConfirmFrame, sizeof(UART1_ExternalControlConfirmFrame));
1002
                UART1_ExternalControlConfirmFrame = 0;
1012
                UART1_ExternalControlConfirmFrame = 0;
1003
        }
1013
        }
1004
        else if(( ((UART1_NaviData_Interval > 0) && CheckDelay(UART1_NaviData_Timer) ) || UART1_Request_NaviData) && (UART1_tx_buffer.Locked == FALSE))
1014
        else if(( ((UART1_NaviData_Interval > 0) && CheckDelay(UART1_NaviData_Timer) ) || UART1_Request_NaviData) && (UART1_tx_buffer.Locked == FALSE))
1005
        {
1015
        {
1006
                NaviData.Errorcode = ErrorCode;
1016
                NaviData.Errorcode = ErrorCode;
1007
                MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'O', NC_ADDRESS, 1 , (u8 *)&NaviData, sizeof(NaviData));
1017
                MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'O', NC_ADDRESS, 1 , (u8 *)&NaviData, sizeof(NaviData));
1008
                UART1_NaviData_Timer = SetDelay(UART1_NaviData_Interval);
1018
                UART1_NaviData_Timer = SetDelay(UART1_NaviData_Interval);
1009
                UART1_Request_NaviData = FALSE;
1019
                UART1_Request_NaviData = FALSE;
1010
                LastTransmittedFCStatusFlags2 = NaviData.FCStatusFlags2;
1020
                LastTransmittedFCStatusFlags2 = NaviData.FCStatusFlags2;
1011
        }
1021
        }
1012
        else if( (( (UART1_DebugData_Interval > 0) && CheckDelay(UART1_DebugData_Timer)) || UART1_Request_DebugData) && (UART1_tx_buffer.Locked == FALSE))
1022
        else if( (( (UART1_DebugData_Interval > 0) && CheckDelay(UART1_DebugData_Timer)) || UART1_Request_DebugData) && (UART1_tx_buffer.Locked == FALSE))
1013
        {
1023
        {
1014
                MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'D', NC_ADDRESS, 1,(u8 *)&DebugOut, sizeof(DebugOut));
1024
                MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'D', NC_ADDRESS, 1,(u8 *)&DebugOut, sizeof(DebugOut));
1015
                UART1_DebugData_Timer = SetDelay(UART1_DebugData_Interval);
1025
                UART1_DebugData_Timer = SetDelay(UART1_DebugData_Interval);
1016
                UART1_Request_DebugData = FALSE;
1026
                UART1_Request_DebugData = FALSE;
1017
        }
1027
        }
1018
        else if((( (UART1_Data3D_Interval > 0) && CheckDelay(UART1_Data3D_Timer) ) || UART1_Request_Data3D) && (UART1_tx_buffer.Locked == FALSE))
1028
        else if((( (UART1_Data3D_Interval > 0) && CheckDelay(UART1_Data3D_Timer) ) || UART1_Request_Data3D) && (UART1_tx_buffer.Locked == FALSE))
1019
        {
1029
        {
1020
                Data3D.StickNick = FC.StickNick;
1030
                Data3D.StickNick = FC.StickNick;
1021
                Data3D.StickRoll = FC.StickRoll;
1031
                Data3D.StickRoll = FC.StickRoll;
1022
        Data3D.StickYaw = FC.StickYaw;
1032
        Data3D.StickYaw = FC.StickYaw;
1023
        Data3D.StickGas = FC.StickGas;
1033
        Data3D.StickGas = FC.StickGas;
1024
                MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'C', NC_ADDRESS, 1,(u8 *)&Data3D, sizeof(Data3D));
1034
                MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'C', NC_ADDRESS, 1,(u8 *)&Data3D, sizeof(Data3D));
1025
                UART1_Data3D_Timer = SetDelay(UART1_Data3D_Interval);
1035
                UART1_Data3D_Timer = SetDelay(UART1_Data3D_Interval);
1026
                UART1_Request_Data3D = FALSE;
1036
                UART1_Request_Data3D = FALSE;
1027
        }
1037
        }
1028
        else if((((UART1_MotorData_Interval > 0) && CheckDelay(UART1_MotorData_Timer) ) || UART1_Request_MotorData) && (UART1_tx_buffer.Locked == FALSE))
1038
        else if((((UART1_MotorData_Interval > 0) && CheckDelay(UART1_MotorData_Timer) ) || UART1_Request_MotorData) && (UART1_tx_buffer.Locked == FALSE))
1029
        {
1039
        {
1030
                do
1040
                do
1031
                {
1041
                {
1032
                 motorindex1++;
1042
                 motorindex1++;
1033
                 motorindex1%=12;
1043
                 motorindex1%=12;
1034
         if(!motorindex1) {motorindex2++;  motorindex2 %= 12;};
1044
         if(!motorindex1) {motorindex2++;  motorindex2 %= 12;};
1035
                 if(motorindex1 == motorindex2) break;
1045
                 if(motorindex1 == motorindex2) break;
1036
                }
1046
                }
1037
                while((Motor[motorindex1].State & 0x80) != 0x80); // skip unused Motors
1047
                while((Motor[motorindex1].State & 0x80) != 0x80); // skip unused Motors
1038
 
1048
 
1039
                MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'K', NC_ADDRESS, 2, &motorindex1, sizeof(motorindex1),(u8 *)&Motor[motorindex1], sizeof(Motor_t));
1049
                MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'K', NC_ADDRESS, 2, &motorindex1, sizeof(motorindex1),(u8 *)&Motor[motorindex1], sizeof(Motor_t));
1040
            UART1_MotorData_Timer = SetDelay(UART1_MotorData_Interval);
1050
            UART1_MotorData_Timer = SetDelay(UART1_MotorData_Interval);
1041
                UART1_Request_MotorData = FALSE;
1051
                UART1_Request_MotorData = FALSE;
1042
        }
1052
        }
1043
        else if(UART1_Request_WPLStore)
1053
        else if(UART1_Request_WPLStore)
1044
        {
1054
        {
-
 
1055
                /*
-
 
1056
                s8 txt[50];
-
 
1057
                sprintf(txt, "\r\nWPL Overwride = %d, Type = %d, Index = %d, Status = %d\r\n",  WPL_Store.OverwriteFile,  WPL_Store.Type, WPL_Answer.Index, WPL_Answer.Status);
-
 
1058
                UART1_PutString(txt);
-
 
1059
                */
1045
                MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'I', NC_ADDRESS, 2,(u8 *)&(WPL_Store.Index), 1, (u8 *)&(WPL_Store.reserved[0]), 1 );
1060
                MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'I', NC_ADDRESS, 1,(u8 *)&(WPL_Answer), sizeof(WPL_Answer_t));
1046
                UART1_Request_WPLStore = FALSE;                
1061
                UART1_Request_WPLStore = FALSE;                
1047
        }
1062
        }
1048
        else if((((NMEA_Interval > 0) && CheckDelay(NMEA_Timer))) && (UART1_tx_buffer.Locked == FALSE))
1063
        else if((((NMEA_Interval > 0) && CheckDelay(NMEA_Timer))) && (UART1_tx_buffer.Locked == FALSE))
1049
        {
1064
        {
1050
                CreateNmeaGGA();
1065
                CreateNmeaGGA();
1051
                Send_NMEA_RMC = TRUE;   // das muss noch da hinter
1066
                Send_NMEA_RMC = TRUE;   // das muss noch da hinter
1052
                NMEA_Timer = SetDelay(NMEA_Interval);
1067
                NMEA_Timer = SetDelay(NMEA_Interval);
1053
        }
1068
        }
1054
        else if(Send_NMEA_RMC == TRUE && (UART1_tx_buffer.Locked == FALSE))
1069
        else if(Send_NMEA_RMC == TRUE && (UART1_tx_buffer.Locked == FALSE))
1055
        {
1070
        {
1056
          CreateNmeaRMC();
1071
          CreateNmeaRMC();
1057
          Send_NMEA_RMC = FALSE;
1072
          Send_NMEA_RMC = FALSE;
1058
        }
1073
        }
1059
 
1074
 
1060
        /*
1075
        /*
1061
        else if(UART1_ConfirmFrame && (UART1_tx_buffer.Locked == FALSE))
1076
        else if(UART1_ConfirmFrame && (UART1_tx_buffer.Locked == FALSE))
1062
        {
1077
        {
1063
                MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'B', NC_ADDRESS, 1, &UART1_ConfirmFrame, sizeof(UART1_ConfirmFrame));
1078
                MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'B', NC_ADDRESS, 1, &UART1_ConfirmFrame, sizeof(UART1_ConfirmFrame));
1064
                UART1_ConfirmFrame = 0;
1079
                UART1_ConfirmFrame = 0;
1065
        }
1080
        }
1066
        */
1081
        */
1067
        /*
1082
        /*
1068
        else if(UART1_Request_ExternalControl && (UART1_tx_buffer.Locked == FALSE))
1083
        else if(UART1_Request_ExternalControl && (UART1_tx_buffer.Locked == FALSE))
1069
        {
1084
        {
1070
                MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'G', NC_ADDRESS, 1, (u8 *)&ExternControl, sizeof(ExternControl));
1085
                MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'G', NC_ADDRESS, 1, (u8 *)&ExternControl, sizeof(ExternControl));
1071
                UART1_Request_ExternalControl = FALSE;
1086
                UART1_Request_ExternalControl = FALSE;
1072
        }
1087
        }
1073
        */
1088
        */
1074
        else if( (( (UART1_Display_Interval > 0) && CheckDelay(UART1_Display_Timer)) || UART1_Request_Display) && (UART1_tx_buffer.Locked == FALSE))
1089
        else if( (( (UART1_Display_Interval > 0) && CheckDelay(UART1_Display_Timer)) || UART1_Request_Display) && (UART1_tx_buffer.Locked == FALSE))
1075
        {
1090
        {
1076
                if(UART1_DisplayLine > 3)
1091
                if(UART1_DisplayLine > 3)
1077
                {
1092
                {
1078
                        Menu_Update(UART1_DisplayKeys);
1093
                        Menu_Update(UART1_DisplayKeys);
1079
                        UART1_DisplayKeys = 0;
1094
                        UART1_DisplayKeys = 0;
1080
                        MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'H', NC_ADDRESS, 1, (u8*)DisplayBuff, sizeof(DisplayBuff));
1095
                        MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'H', NC_ADDRESS, 1, (u8*)DisplayBuff, sizeof(DisplayBuff));
1081
                }
1096
                }
1082
                else
1097
                else
1083
                {
1098
                {
1084
                        UART1_DisplayLine = 2;
1099
                        UART1_DisplayLine = 2;
1085
                        sprintf(text,"!!! incompatible !!!");
1100
                        sprintf(text,"!!! incompatible !!!");
1086
                        MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'H', NC_ADDRESS, 2, &UART1_DisplayLine, sizeof(UART1_DisplayLine), (u8*)&text, 20);
1101
                        MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'H', NC_ADDRESS, 2, &UART1_DisplayLine, sizeof(UART1_DisplayLine), (u8*)&text, 20);
1087
                        if(UART1_DisplayLine++ > 3) UART1_DisplayLine = 0;
1102
                        if(UART1_DisplayLine++ > 3) UART1_DisplayLine = 0;
1088
                }
1103
                }
1089
                UART1_Display_Timer = SetDelay(UART1_Display_Interval);
1104
                UART1_Display_Timer = SetDelay(UART1_Display_Interval);
1090
                UART1_Request_Display = FALSE;
1105
                UART1_Request_Display = FALSE;
1091
        }
1106
        }
1092
        else if(UART1_Request_Display1 && (UART1_tx_buffer.Locked == FALSE))
1107
        else if(UART1_Request_Display1 && (UART1_tx_buffer.Locked == FALSE))
1093
        {
1108
        {
1094
                Menu_Update(0);
1109
                Menu_Update(0);
1095
                MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'L', NC_ADDRESS, 3, (u8*)&MenuItem, sizeof(MenuItem), (u8*)&MaxMenuItem, sizeof(MaxMenuItem),(u8*)DisplayBuff, sizeof(DisplayBuff));
1110
                MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'L', NC_ADDRESS, 3, (u8*)&MenuItem, sizeof(MenuItem), (u8*)&MaxMenuItem, sizeof(MaxMenuItem),(u8*)DisplayBuff, sizeof(DisplayBuff));
1096
                UART1_Request_Display1 = FALSE;
1111
                UART1_Request_Display1 = FALSE;
1097
        }
1112
        }
1098
        else if(UART1_Request_VersionInfo && (UART1_tx_buffer.Locked == FALSE))
1113
        else if(UART1_Request_VersionInfo && (UART1_tx_buffer.Locked == FALSE))
1099
        {
1114
        {
1100
                MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'V', NC_ADDRESS,1, (u8 *)&UART_VersionInfo, sizeof(UART_VersionInfo));
1115
                MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'V', NC_ADDRESS,1, (u8 *)&UART_VersionInfo, sizeof(UART_VersionInfo));
1101
                UART1_Request_VersionInfo = FALSE;
1116
                UART1_Request_VersionInfo = FALSE;
1102
        }
1117
        }
1103
        else if(UART1_Request_SystemTime && (UART1_tx_buffer.Locked == FALSE))
1118
        else if(UART1_Request_SystemTime && (UART1_tx_buffer.Locked == FALSE))
1104
        {
1119
        {
1105
                MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'T', NC_ADDRESS,1, (u8 *)&SystemTime, sizeof(SystemTime));
1120
                MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'T', NC_ADDRESS,1, (u8 *)&SystemTime, sizeof(SystemTime));
1106
                UART1_Request_SystemTime = FALSE;
1121
                UART1_Request_SystemTime = FALSE;
1107
        }
1122
        }
1108
        else if(UART1_Request_ErrorMessage && (UART1_tx_buffer.Locked == FALSE))
1123
        else if(UART1_Request_ErrorMessage && (UART1_tx_buffer.Locked == FALSE))
1109
        {
1124
        {
1110
                MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'E', NC_ADDRESS, 1, (u8 *)&ErrorMSG, sizeof(ErrorMSG));
1125
                MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 'E', NC_ADDRESS, 1, (u8 *)&ErrorMSG, sizeof(ErrorMSG));
1111
                UART1_Request_ErrorMessage = FALSE;
1126
                UART1_Request_ErrorMessage = FALSE;
1112
        }
1127
        }
1113
#ifdef FOLLOW_ME
1128
#ifdef FOLLOW_ME
1114
        else if(CheckDelay(UART1_FollowMe_Timer) && (UART1_tx_buffer.Locked == FALSE))
1129
        else if(CheckDelay(UART1_FollowMe_Timer) && (UART1_tx_buffer.Locked == FALSE))
1115
        {
1130
        {
1116
                if((GPSData.Status != INVALID) && (GPSData.SatFix == SATFIX_3D) && (GPSData.Flags & FLAG_GPSFIXOK) && (GPSData.NumOfSats >= 4))
1131
                if((GPSData.Status != INVALID) && (GPSData.SatFix == SATFIX_3D) && (GPSData.Flags & FLAG_GPSFIXOK) && (GPSData.NumOfSats >= 4))
1117
                {
1132
                {
1118
                        TransmitAlsoToFC = 1;
1133
                        TransmitAlsoToFC = 1;
1119
                        // update FollowMe content
1134
                        // update FollowMe content
1120
                        FollowMe.Position.Longitude     = GPSData.Position.Longitude;
1135
                        FollowMe.Position.Longitude     = GPSData.Position.Longitude;
1121
                        FollowMe.Position.Latitude      = GPSData.Position.Latitude;
1136
                        FollowMe.Position.Latitude      = GPSData.Position.Latitude;
1122
                        FollowMe.Position.Status = NEWDATA;
1137
                        FollowMe.Position.Status = NEWDATA;
1123
                        FollowMe.Position.Altitude = 1;
1138
                        FollowMe.Position.Altitude = 1;
1124
                        // 0  -> no Orientation
1139
                        // 0  -> no Orientation
1125
                        // 1-360 -> CompassCourse Setpoint
1140
                        // 1-360 -> CompassCourse Setpoint
1126
                        // -1 -> points to  WP1 -> itself
1141
                        // -1 -> points to  WP1 -> itself
1127
                FollowMe.Heading = -1;
1142
                FollowMe.Heading = -1;
1128
                        FollowMe.ToleranceRadius = 1;
1143
                        FollowMe.ToleranceRadius = 1;
1129
                        FollowMe.HoldTime = 60;
1144
                        FollowMe.HoldTime = 60;
1130
                        FollowMe.Event_Flag = 1;
1145
                        FollowMe.Event_Flag = 1;
1131
                        FollowMe.Index = 1; // 0 = Delete List, 1 place at first entry in the list
1146
                        FollowMe.Index = 1; // 0 = Delete List, 1 place at first entry in the list
1132
                        FollowMe.Type = POINT_TYPE_WP;
1147
                        FollowMe.Type = POINT_TYPE_WP;
1133
                        FollowMe.WP_EventChannelValue = 100;  // set servo value
1148
                        FollowMe.WP_EventChannelValue = 100;  // set servo value
1134
                        FollowMe.AltitudeRate = 0;                        // do not change height
1149
                        FollowMe.AltitudeRate = 0;                        // do not change height
1135
                        FollowMe.Speed = 0;                             // rate to change the Position (0 = max)
1150
                        FollowMe.Speed = 0;                             // rate to change the Position (0 = max)
1136
                        FollowMe.CamAngle = 255;                // Camera servo angle in degree (255 -> POI-Automatic)
1151
                        FollowMe.CamAngle = 255;                // Camera servo angle in degree (255 -> POI-Automatic)
1137
                        FollowMe.Name[0] = 'F';             // Name of that point (ASCII)
1152
                        FollowMe.Name[0] = 'F';             // Name of that point (ASCII)
1138
                        FollowMe.Name[1] = 'O';             // Name of that point (ASCII)
1153
                        FollowMe.Name[1] = 'O';             // Name of that point (ASCII)
1139
                        FollowMe.Name[2] = 'L';             // Name of that point (ASCII)
1154
                        FollowMe.Name[2] = 'L';             // Name of that point (ASCII)
1140
                        FollowMe.Name[3] = 'L';             // Name of that point (ASCII)
1155
                        FollowMe.Name[3] = 'L';             // Name of that point (ASCII)
1141
                        FollowMe.reserve[0] = 0;                // reserve
1156
                        FollowMe.reserve[0] = 0;                // reserve
1142
                        FollowMe.reserve[1] = 0;                // reserve
1157
                        FollowMe.reserve[1] = 0;                // reserve
1143
                        MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 's', NC_ADDRESS, 1, (u8 *)&FollowMe, sizeof(FollowMe));
1158
                        MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 's', NC_ADDRESS, 1, (u8 *)&FollowMe, sizeof(FollowMe));
1144
                }
1159
                }
1145
                UART1_FollowMe_Timer = SetDelay(FOLLOW_ME_INTERVAL); // set new update time
1160
                UART1_FollowMe_Timer = SetDelay(FOLLOW_ME_INTERVAL); // set new update time
1146
        }
1161
        }
1147
#endif
1162
#endif
1148
#ifdef DEBUG                                                                                                                    // only include functions if DEBUG is defined
1163
#ifdef DEBUG                                                                                                                    // only include functions if DEBUG is defined
1149
        else if(SendDebugOutput && (UART1_tx_buffer.Locked == FALSE))
1164
        else if(SendDebugOutput && (UART1_tx_buffer.Locked == FALSE))
1150
        {
1165
        {
1151
                MKProtocol_CreateSerialFrame(&UART1_tx_buffer,'0', NC_ADDRESS, 1, (u8 *) &tDebug, sizeof(tDebug));
1166
                MKProtocol_CreateSerialFrame(&UART1_tx_buffer,'0', NC_ADDRESS, 1, (u8 *) &tDebug, sizeof(tDebug));
1152
                SendDebugOutput = 0;
1167
                SendDebugOutput = 0;
1153
        }
1168
        }
1154
#endif
1169
#endif
1155
        UART1_Transmit(); // output pending bytes in tx buffer
1170
        UART1_Transmit(); // output pending bytes in tx buffer
1156
}
1171
}
1157
 
1172
 
1158
 
1173