Subversion Repositories NaviCtrl

Rev

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

Rev Author Line No. Line
110 killagreg 1
/*#######################################################################################*/
2
/* !!! THIS IS NOT FREE SOFTWARE !!!                                                     */
3
/*#######################################################################################*/
4
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
5
// + www.MikroKopter.com
6
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
360 holgerb 7
// + Software Nutzungsbedingungen (english version: see below)
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
430 holgerb 10
// + Mikrocontroller verwendete Firmware für die Hardware Flight-Ctrl, Navi-Ctrl, BL-Ctrl, MK3Mag & PC-Programm MikroKopter-Tool 
360 holgerb 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.
110 killagreg 13
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
360 holgerb 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.
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.
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.
20
// + Die Haftung des Lizenzgebers wird - soweit gesetzlich zulässig - begrenzt in Höhe des typischen und vorhersehbaren
430 holgerb 21
// + Schadens. Die gesetzliche Haftung bei Personenschäden und nach dem Produkthaftungsgesetz bleibt unberührt. Dem Lizenzgeber steht jedoch der Einwand 
360 holgerb 22
// + des Mitverschuldens offen.
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.
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
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.
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.
110 killagreg 31
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
360 holgerb 32
// + Software LICENSING TERMS
110 killagreg 33
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
360 holgerb 34
// + of HiSystems GmbH, Flachsmeerstrasse 2, 26802 Moormerland, Germany - the Licensor -
430 holgerb 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 
360 holgerb 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.
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
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
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
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
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.
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.
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.
52
// + *) The territory aspect only refers to the place where the Software is used, not its programmed range.
53
// + #### END OF LICENSING TERMS ####
54
// + Note: For information on license extensions (e.g. commercial use), please contact us at info(@)hisystems.de.
110 killagreg 55
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
56
#include <stdarg.h>
489 killagreg 57
#include <stdio.h>
110 killagreg 58
#include "91x_lib.h"
430 holgerb 59
#include "mkprotocol.h" 
110 killagreg 60
#include "ramfunc.h"
378 holgerb 61
#include "uart1.h"
432 holgerb 62
#include "main.h"
558 holgerb 63
#include "GPS.h"
110 killagreg 64
 
65
/**************************************************************/
66
/* Create serial output frame                                 */
67
/**************************************************************/
460 holgerb 68
u8 AddSerialData(Buffer_t* pTxBuff, u8 *buffer, u16 size) //u8 *data, u8 len, ....
69
{
70
  unsigned int i;
71
        if(pTxBuff->Locked == TRUE) return(0);
72
        // tx-buffer is not in use
73
        // lock the buffer
74
        pTxBuff->Locked = TRUE;
75
        pTxBuff->Position = 0;
76
        for(i=0; i<size; i++) pTxBuff->pData[pTxBuff->Position++] = buffer[i];                     
77
        pTxBuff->DataBytes = pTxBuff->Position;
78
        pTxBuff->Position = 0;  // reset buffer position for transmision
79
        return(pTxBuff->Locked);
80
}
81
 
82
/**************************************************************/
83
/* Create serial output frame                                 */
84
/**************************************************************/
596 holgerb 85
u16 MKProtocol_CreateSerialFrame(Buffer_t* pTxBuff, u8 CmdID, u8 Address, u8 numofbuffers , ...) //u8 *data, u8 len, ....
110 killagreg 86
{
87
        va_list ap;
88
 
89
        u8 a,b,c;
349 ingob 90
        u16 ptr = 0;
110 killagreg 91
        u16 tmpCRC = 0, i;
92
 
93
        u8* pdata = NULL;
94
        int len = 0;
430 holgerb 95
 
245 killagreg 96
        if(pTxBuff->Locked == TRUE) return(0);
97
 
98
        // tx-buffer is not in use
99
        // lock the buffer
430 holgerb 100
        pTxBuff->Locked = TRUE;
245 killagreg 101
        pTxBuff->Position = 0;
102
        pTxBuff->pData[pTxBuff->Position++] = '#';                          // Start character
103
        pTxBuff->pData[pTxBuff->Position++] = 'a' + Address;    // Address (a=0; b=1,...)
104
        pTxBuff->pData[pTxBuff->Position++] = CmdID;                    // Command
105
 
106
        va_start(ap, numofbuffers);
107
        if(numofbuffers)
108
        {
109
                pdata = va_arg(ap, u8*);
110
                len = va_arg(ap, int);
111
                ptr = 0;
112
                numofbuffers--;
113
        }
114
        while(len)
115
        {
116
                if(len)
110 killagreg 117
                {
245 killagreg 118
                        a = pdata[ptr++];
119
                        len--;
120
                        if((!len) && numofbuffers) // try to jump to next buffer
121
                        {
122
                                pdata = va_arg(ap, u8*);
123
                                len = va_arg(ap, int);
124
                                ptr = 0;
125
                                numofbuffers--;
126
                        }
110 killagreg 127
                }
245 killagreg 128
                else a = 0;
129
                if(len)
110 killagreg 130
                {
245 killagreg 131
                        b = pdata[ptr++];
132
                        len--;
133
                        if((!len) && numofbuffers) // try to jump to next buffer
110 killagreg 134
                        {
245 killagreg 135
                                pdata = va_arg(ap, u8*);
136
                                len = va_arg(ap, int);
137
                                ptr = 0;
138
                                numofbuffers--;
110 killagreg 139
                        }
245 killagreg 140
                }
141
                else b = 0;
142
                if(len)
143
                {
144
                        c = pdata[ptr++];
145
                        len--;
146
                        if((!len) && numofbuffers) // try to jump to next buffer
110 killagreg 147
                        {
245 killagreg 148
                                pdata = va_arg(ap, u8*);
149
                                len = va_arg(ap, int);
150
                                ptr = 0;
151
                                numofbuffers--;
110 killagreg 152
                        }
153
                }
245 killagreg 154
                else c = 0;
155
                pTxBuff->pData[pTxBuff->Position++] = '=' + (a >> 2);
156
                pTxBuff->pData[pTxBuff->Position++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4));
157
                pTxBuff->pData[pTxBuff->Position++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6));
158
                pTxBuff->pData[pTxBuff->Position++] = '=' + ( c & 0x3f);
159
        }
160
        va_end(ap);
161
        // add crc
162
        for(i = 0; i < pTxBuff->Position; i++)
163
        {
164
                tmpCRC += pTxBuff->pData[i];
165
        }
166
        tmpCRC %= 4096;
167
        pTxBuff->pData[pTxBuff->Position++] = '=' + tmpCRC / 64;
168
        pTxBuff->pData[pTxBuff->Position++] = '=' + tmpCRC % 64;
169
        pTxBuff->pData[pTxBuff->Position++] = '\r';
170
        pTxBuff->DataBytes = pTxBuff->Position;
171
        pTxBuff->Position = 0;  // reset buffer position for transmision
596 holgerb 172
//      return(pTxBuff->Locked);
173
        return(pTxBuff->DataBytes);
110 killagreg 174
}
175
 
176
// typical called in an UART Rx ISR
177
/**************************************************************/
178
/* Collect serial frame                                       */
179
/**************************************************************/
153 killagreg 180
u8 MKProtocol_CollectSerialFrame(Buffer_t* pRxBuff, u8 c)
110 killagreg 181
{
182
        if(pRxBuff->Locked == FALSE)
183
        { // rx buffer not locked
113 killagreg 184
                if(c == '#') // if syncronisation character is received
185
                {
186
                        pRxBuff->Position = 0;                                          // reset buffer
187
                        pRxBuff->pData[pRxBuff->Position++] = c;        // copy 1st byte to buffer
188
                        pRxBuff->DataBytes = 1;
189
                }
190
                else if (pRxBuff->Position < pRxBuff->Size) // rx buffer not full
191
                {
190 killagreg 192
                        pRxBuff->pData[pRxBuff->Position++] = c; // copy byte to rxd buffer
193
                        pRxBuff->DataBytes++;
244 killagreg 194
                        // termination character received and sync has been established
195
                        if ((c == '\r') && (pRxBuff->pData[0]== '#'))
113 killagreg 196
                        {
197
                                // calculate checksum from transmitted data
198
                                u16 crc = 0, i;
199
                                u8 crc1, crc2;
190 killagreg 200
                                for(i = 0; i < (pRxBuff->Position-3); i++)
110 killagreg 201
                                {
430 holgerb 202
                                        crc +=  pRxBuff->pData[i];  
203
                                }              
113 killagreg 204
                                crc %= 4096;
205
                                crc1 = '=' + crc / 64;
206
                                crc2 = '=' + crc % 64;
207
                                // compare checksum to transmitted checksum bytes
190 killagreg 208
                                if((crc1 == pRxBuff->pData[pRxBuff->Position-3]) && (crc2 == pRxBuff->pData[pRxBuff->Position-2]))
244 killagreg 209
                                {
210
                                    // checksum is valid
430 holgerb 211
                                        pRxBuff->Position = 0;                                 
212
                                        pRxBuff->Locked = TRUE;                                     // lock the rxd buffer 
113 killagreg 213
                                        // if 2nd byte is an 'R' start bootloader
558 holgerb 214
                                        if(pRxBuff->pData[2] == 'R' && !(FC.RealStatusFlags & FC_STATUS_MOTOR_RUN)) // not if the motors are running)
113 killagreg 215
                                        {
216
                                                VIC_DeInit();
217
                                                Execute_Bootloader(); // Reset-Commando - Bootloader starten
218
                                        }
219
                                } // eof checksum valid
220
                                else
221
                                {       // checksum is invalid
222
                                        Buffer_Clear(pRxBuff);
223
                                }  // eof checksum invalid
224
                        } // eof termination character received
225
                } // rxd buffer not full
226
                else // rxd buffer overrun
227
                {
228
                        Buffer_Clear(pRxBuff);
275 killagreg 229
                } // eof rxd buffer overrun
110 killagreg 230
        }
153 killagreg 231
        return(pRxBuff->Locked);
110 killagreg 232
}
233
 
234
/**************************************************************/
190 killagreg 235
/* Decode detination address                                  */
236
/**************************************************************/
237
void MKProtocol_DecodeSerialFrameHeader(Buffer_t* pRxBuff, SerialMsg_t* pSerialMsg)
238
{
239
        if(pRxBuff->Locked)
240
        {
241
                pSerialMsg->Address = pRxBuff->pData[1] - 'a';
242
                pSerialMsg->CmdID = pRxBuff->pData[2];
243
        }
244
        else
245
        {
246
                pSerialMsg->Address = 0;
247
                pSerialMsg->CmdID = ' ';
430 holgerb 248
        }      
190 killagreg 249
}
250
 
251
/**************************************************************/
110 killagreg 252
/* Decode data                                                */
253
/**************************************************************/
190 killagreg 254
void MKProtocol_DecodeSerialFrameData(Buffer_t* pRxBuff, SerialMsg_t* pSerialMsg)
110 killagreg 255
{
256
        u8 a,b,c,d;
378 holgerb 257
        u16 ptrIn = 3; // start with first data byte in rx buffer
258
        u16 ptrOut = 3;
259
        u16 len = pRxBuff->DataBytes - 6;        // must be a multiple of 4 (3 bytes at begin and 3 bytes at end are no payload )
418 killagreg 260
 
261
        len/=4;   // number of 4 byte blocks
430 holgerb 262
        while(len--)
110 killagreg 263
        {
264
                a = pRxBuff->pData[ptrIn++] - '=';
265
                b = pRxBuff->pData[ptrIn++] - '=';
266
                c = pRxBuff->pData[ptrIn++] - '=';
267
                d = pRxBuff->pData[ptrIn++] - '=';
268
 
418 killagreg 269
                pRxBuff->pData[ptrOut++] = (a << 2) | (b >> 4);
270
                pRxBuff->pData[ptrOut++] = ((b & 0x0f) << 4) | (c >> 2);
271
                pRxBuff->pData[ptrOut++] = ((c & 0x03) << 6) | d;
110 killagreg 272
        }
273
        pSerialMsg->pData = &(pRxBuff->pData[3]);
274
        pSerialMsg->DataLen = ptrOut - 3;       // return number of data in bytes
112 killagreg 275
        pRxBuff->Position = 0;
276
        pRxBuff->DataBytes = ptrOut;
110 killagreg 277
}
430 holgerb 278
 
279
/*
280
void MKProtocol_DecodeSerialFrameData(Buffer_t* pRxBuff, SerialMsg_t* pSerialMsg)
281
{
282
        u8 a,b,c,d;
283
        u8 x,y,z;
284
        u16 ptrIn = 3; // start with first data byte in rx buffer
285
        u16 ptrOut = 3;
286
        u16 len = pRxBuff->DataBytes - 6;        // must be a multiple of 4 (3 bytes at begin and 3 bytes at end are no payload )
287
        while(len)
288
        {
289
                a = pRxBuff->pData[ptrIn++] - '=';
290
                b = pRxBuff->pData[ptrIn++] - '=';
291
                c = pRxBuff->pData[ptrIn++] - '=';
292
                d = pRxBuff->pData[ptrIn++] - '=';
293
                //if(ptrIn > ReceivedBytes - 3) break;
294
 
295
                x = (a << 2) | (b >> 4);
296
                y = ((b & 0x0f) << 4) | (c >> 2);
297
                z = ((c & 0x03) << 6) | d;
298
 
299
                if(len--) pRxBuff->pData[ptrOut++] = x; else break;
300
                if(len--) pRxBuff->pData[ptrOut++] = y; else break;
301
                if(len--) pRxBuff->pData[ptrOut++] = z; else break;
302
        }
303
        pSerialMsg->pData = &(pRxBuff->pData[3]);
304
        pSerialMsg->DataLen = ptrOut - 3;       // return number of data in bytes
305
        pRxBuff->Position = 0;
306
        pRxBuff->DataBytes = ptrOut;
307
}
308
*/
309