Subversion Repositories FlightCtrl

Rev

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

Rev Author Line No. Line
1227 killagreg 1
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2
// + Copyright (c) 04.2007 Holger Buss
3
// + Nur für den privaten Gebrauch
4
// + www.MikroKopter.com
5
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
6
// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),
7
// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist.
8
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
9
// + bzgl. der Nutzungsbedingungen aufzunehmen.
10
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
11
// + Verkauf von Luftbildaufnahmen, usw.
12
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
13
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,
14
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen
15
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
16
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
17
// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
18
// + eindeutig als Ursprung verlinkt werden
19
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
20
// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
21
// + Benutzung auf eigene Gefahr
22
// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
23
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
24
// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
25
// + mit unserer Zustimmung zulässig
26
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
27
// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
28
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
29
// + Redistributions of source code (with or without modifications) must retain the above copyright notice,
30
// + this list of conditions and the following disclaimer.
31
// +   * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
32
// +     from this software without specific prior written permission.
33
// +   * The use of this project (hardware, software, binary files, sources and documentation) is only permittet
34
// +     for non-commercial use (directly or indirectly)
35
// +     Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
36
// +     with our written permission
37
// +   * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
38
// +     clearly linked as origin
39
// +   * porting to systems other than hardware from www.mikrokopter.de is not allowed
40
// +  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
41
// +  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
42
// +  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
43
// +  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
44
// +  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
45
// +  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
46
// +  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
47
// +  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
48
// +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
49
// +  POSSIBILITY OF SUCH DAMAGE.
50
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
51
#include <avr/io.h>
52
#include <avr/interrupt.h>
53
#include <string.h>
54
#include <stdlib.h>
55
#include "main.h"
56
#include "spi.h"
57
#include "fc.h"
58
#include "rc.h"
59
#include "eeprom.h"
60
#include "uart0.h"
61
#include "timer0.h"
62
#include "analog.h"
63
 
64
 
65
//-----------------------------------------
66
#define DDR_SPI DDRB
67
#define DD_SS   PB4
68
#define DD_SCK  PB7
69
#define DD_MOSI PB5
70
#define DD_MISO PB6
71
 
72
// for compatibility reasons gcc3.x <-> gcc4.x
73
#ifndef SPCR
74
#define SPCR   SPCR0
75
#endif
76
#ifndef SPIE
77
#define SPIE   SPIE0
78
#endif
79
#ifndef SPE
80
#define SPE    SPE0
81
#endif
82
#ifndef DORD
83
#define DORD   DORD0
84
#endif
85
#ifndef MSTR
86
#define MSTR   MSTR0
87
#endif
88
#ifndef CPOL
89
#define CPOL   CPOL0
90
#endif
91
#ifndef CPHA
92
#define CPHA   CPHA0
93
#endif
94
#ifndef SPR1
95
#define SPR1   SPR01
96
#endif
97
#ifndef SPR0
98
#define SPR0   SPR00
99
#endif
100
 
101
#ifndef SPDR
102
#define SPDR   SPDR0
103
#endif
104
 
105
#ifndef SPSR
106
#define SPSR   SPSR0
107
#endif
108
#ifndef SPIF
109
#define SPIF   SPIF0
110
#endif
111
#ifndef WCOL
112
#define WCOL   WCOL0
113
#endif
114
#ifndef SPI2X
115
#define SPI2X  SPI2X0
116
#endif
117
// -------------------------
118
 
119
#define SLAVE_SELECT_DDR_PORT   DDRC
120
#define SLAVE_SELECT_PORT       PORTC
121
#define SPI_SLAVE_SELECT        PC5
122
 
123
 
124
#define SPI_TXSYNCBYTE1 0xAA
125
#define SPI_TXSYNCBYTE2 0x83
126
#define SPI_RXSYNCBYTE1 0x81
127
#define SPI_RXSYNCBYTE2 0x55
128
 
129
typedef enum
130
{
131
        SPI_SYNC1,
132
        SPI_SYNC2,
133
        SPI_DATA
134
} SPI_RXState_t;
135
 
136
 
137
// data exchange packets to and From NaviCtrl
138
ToNaviCtrl_t                    ToNaviCtrl;
139
FromNaviCtrl_t                  FromNaviCtrl;
140
 
141
SPI_VersionInfo_t SPI_VersionInfo;
142
 
143
// rx packet buffer
144
#define SPI_RXBUFFER_LEN sizeof(FromNaviCtrl)
145
uint8_t SPI_RxBuffer[SPI_RXBUFFER_LEN];
146
uint8_t SPI_RxBufferIndex = 0;
147
uint8_t SPI_RxBuffer_Request = 0;
148
 
149
// tx packet buffer
150
#define SPI_TXBUFFER_LEN sizeof(ToNaviCtrl)
151
uint8_t *SPI_TxBuffer;
152
uint8_t SPI_TxBufferIndex = 0;
153
 
154
uint8_t SPITransferCompleted, SPI_ChkSum;
155
uint8_t SPI_RxDataValid = 0;
156
uint8_t NCDataOkay = 0;
157
uint8_t NCSerialDataOkay = 0;
158
 
159
uint8_t SPI_CommandSequence[] = { SPI_CMD_USER, SPI_CMD_STICK, SPI_CMD_PARAMETER1, SPI_CMD_STICK, SPI_CMD_MISC, SPI_CMD_VERSION };
160
uint8_t SPI_CommandCounter = 0;
161
 
162
/*********************************************/
163
/*  Initialize SPI interface to NaviCtrl     */
164
/*********************************************/
165
void SPI_MasterInit(void)
166
{
167
        DDR_SPI |= (1<<DD_MOSI)|(1<<DD_SCK);    // Set MOSI and SCK output, all others input
168
        SLAVE_SELECT_DDR_PORT |= (1 << SPI_SLAVE_SELECT); // set Slave select port as output port
169
 
170
        SPCR = (1<<SPE)|(1<<MSTR)|(1<<SPR1)|(0<<SPR0)|(0<<SPIE);   // Enable SPI, Master, set clock rate fck/64
171
        SPSR = 0;//(1<<SPI2X);
172
 
173
        SLAVE_SELECT_PORT |=  (1 << SPI_SLAVE_SELECT); // Deselect Slave
174
 
175
        SPI_TxBuffer = (uint8_t *) &ToNaviCtrl; // set pointer to tx-buffer
176
        SPITransferCompleted = 1;
177
        // initialize data packet to NaviControl
178
        ToNaviCtrl.Sync1 = SPI_TXSYNCBYTE1;
179
        ToNaviCtrl.Sync2 = SPI_TXSYNCBYTE2;
180
 
181
        ToNaviCtrl.Command = SPI_CMD_USER;
182
        ToNaviCtrl.IntegralNick = 0;
183
        ToNaviCtrl.IntegralRoll = 0;
184
        NCSerialDataOkay = 0;
185
        NCDataOkay = 0;
186
 
187
        SPI_RxDataValid = 0;
188
 
189
        SPI_VersionInfo.Major = VERSION_MAJOR;
190
        SPI_VersionInfo.Minor = VERSION_MINOR;
191
        SPI_VersionInfo.Patch = VERSION_PATCH;
192
        SPI_VersionInfo.Compatible = NC_SPI_COMPATIBLE;
193
}
194
 
195
 
196
/**********************************************************/
197
/*  Update Data transferd by the SPI from/to NaviCtrl     */
198
/**********************************************************/
199
void UpdateSPI_Buffer(void)
200
{
201
        int16_t tmp;
202
        cli(); // stop all interrupts to avoid writing of new data during update of that packet.
203
 
204
        // update content of packet to NaviCtrl
205
        ToNaviCtrl.IntegralNick = (int16_t)((10 * IntegralGyroNick) / GYRO_DEG_FACTOR); // convert to multiple of 0.1°
206
        ToNaviCtrl.IntegralRoll = (int16_t)((10 * IntegralGyroRoll) / GYRO_DEG_FACTOR); // convert to multiple of 0.1°
207
        ToNaviCtrl.GyroHeading  = (int16_t)((10 * YawGyroHeading)   / GYRO_DEG_FACTOR); // convert to multiple of 0.1°
208
        ToNaviCtrl.GyroNick = GyroNick;
209
        ToNaviCtrl.GyroRoll = GyroRoll;
210
        ToNaviCtrl.GyroYaw =  GyroYaw;
211
        ToNaviCtrl.AccNick =  ((int16_t) 10 * ACC_AMPLIFY * (NaviAccNick / NaviCntAcc)) / ACC_DEG_FACTOR; // convert to multiple of 0.1°
212
        ToNaviCtrl.AccRoll =  ((int16_t) 10 * ACC_AMPLIFY * (NaviAccRoll / NaviCntAcc)) / ACC_DEG_FACTOR; // convert to multiple of 0.1°
213
        NaviCntAcc = 0; NaviAccNick = 0; NaviAccRoll = 0;
214
 
215
        switch(ToNaviCtrl.Command)
216
        {
217
                case SPI_CMD_USER:
218
                        ToNaviCtrl.Param.Byte[0] = FCParam.UserParam1;
219
                        ToNaviCtrl.Param.Byte[1] = FCParam.UserParam2;
220
                        ToNaviCtrl.Param.Byte[2] = FCParam.UserParam3;
221
                        ToNaviCtrl.Param.Byte[3] = FCParam.UserParam4;
222
                        ToNaviCtrl.Param.Byte[4] = FCParam.UserParam5;
223
                        ToNaviCtrl.Param.Byte[5] = FCParam.UserParam6;
224
                        ToNaviCtrl.Param.Byte[6] = FCParam.UserParam7;
225
                        ToNaviCtrl.Param.Byte[7] = FCParam.UserParam8;
226
                        ToNaviCtrl.Param.Byte[8] = MKFlags;
227
                        MKFlags &= ~(MKFLAG_CALIBRATE | MKFLAG_START); // calibrate and start are temporal states that are cleared immediately after transmitting
228
                        ToNaviCtrl.Param.Byte[9] = (uint8_t)UBat;
229
                        ToNaviCtrl.Param.Byte[10] = ParamSet.LowVoltageWarning;
230
                        ToNaviCtrl.Param.Byte[11] = GetActiveParamSet();
231
                        break;
232
 
233
                case SPI_CMD_PARAMETER1:
234
                        ToNaviCtrl.Param.Byte[0] = ParamSet.NaviGpsModeControl;     // Parameters for the Naviboard
235
                        ToNaviCtrl.Param.Byte[1] = ParamSet.NaviGpsGain;
236
                        ToNaviCtrl.Param.Byte[2] = ParamSet.NaviGpsP;
237
                        ToNaviCtrl.Param.Byte[3] = ParamSet.NaviGpsI;
238
                        ToNaviCtrl.Param.Byte[4] = ParamSet.NaviGpsD;
239
                        ToNaviCtrl.Param.Byte[5] = ParamSet.NaviGpsACC;
240
                        ToNaviCtrl.Param.Byte[6] = ParamSet.NaviGpsMinSat;
241
                        ToNaviCtrl.Param.Byte[7] = ParamSet.NaviStickThreshold;
242
                        ToNaviCtrl.Param.Byte[8] = ParamSet.NaviOperatingRadius;
243
                        ToNaviCtrl.Param.Byte[9] = ParamSet.NaviWindCorrection;
244
            ToNaviCtrl.Param.Byte[10] = ParamSet.NaviSpeedCompensation;
245
            ToNaviCtrl.Param.Byte[11] = ParamSet.NaviAngleLimitation;
246
                        break;
247
 
248
 
249
                case SPI_CMD_STICK:
250
                        tmp = PPM_in[ParamSet.ChannelAssignment[CH_GAS]];  if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128;
251
                        ToNaviCtrl.Param.Byte[0] = (int8_t) tmp;
252
                        tmp = PPM_in[ParamSet.ChannelAssignment[CH_YAW]]; if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128;
253
                        ToNaviCtrl.Param.Byte[1] = (int8_t) tmp;
254
                        tmp = PPM_in[ParamSet.ChannelAssignment[CH_ROLL]]; if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128;
255
                        ToNaviCtrl.Param.Byte[2] = (int8_t) tmp;
256
                        tmp = PPM_in[ParamSet.ChannelAssignment[CH_NICK]]; if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128;
257
                        ToNaviCtrl.Param.Byte[3] = (int8_t) tmp;
258
                        ToNaviCtrl.Param.Byte[4] = (uint8_t) Poti1;
259
                        ToNaviCtrl.Param.Byte[5] = (uint8_t) Poti2;
260
                        ToNaviCtrl.Param.Byte[6] = (uint8_t) Poti3;
261
                        ToNaviCtrl.Param.Byte[7] = (uint8_t) Poti4;
262
                        ToNaviCtrl.Param.Byte[8] = (uint8_t) RC_Quality;
263
                        break;
264
 
265
                case SPI_CMD_MISC:
266
                        ToNaviCtrl.Param.Byte[0] = CompassCalState;
267
                        if(CompassCalState > 4)
268
                        { // jump from 5 to 0
269
                                CompassCalState  = 0;
270
                        }
271
                        ToNaviCtrl.Param.Byte[1] = ParamSet.NaviPHLoginTime;
272
                        ToNaviCtrl.Param.Int[1]  = ReadingHeight; // at address of Byte 2 and 3
273
                        ToNaviCtrl.Param.Byte[4] = ParamSet.NaviGpsPLimit;
274
                        ToNaviCtrl.Param.Byte[5] = ParamSet.NaviGpsILimit;
275
                        ToNaviCtrl.Param.Byte[6] = ParamSet.NaviGpsDLimit;
276
                        break;
277
 
278
                case SPI_CMD_VERSION:
279
                        ToNaviCtrl.Param.Byte[0] = SPI_VersionInfo.Major;
280
                        ToNaviCtrl.Param.Byte[1] = SPI_VersionInfo.Minor;
281
                        ToNaviCtrl.Param.Byte[2] = SPI_VersionInfo.Patch;
282
                        ToNaviCtrl.Param.Byte[3] = SPI_VersionInfo.Compatible;
283
                        ToNaviCtrl.Param.Byte[4] = BoardRelease;
284
                        break;
285
 
286
                default:
287
                        break;
288
        }
289
 
290
 
291
        sei(); // enable all interrupts
292
 
293
        // analyze content of packet from NaviCtrl if valid
294
        if (SPI_RxDataValid)
295
        {
296
                // update gps controls
297
                if(abs(FromNaviCtrl.GPSStickNick) < 512 && abs(FromNaviCtrl.GPSStickRoll) < 512 && (ParamSet.GlobalConfig & CFG_GPS_ACTIVE))
298
                {
299
                        GPSStickNick    = FromNaviCtrl.GPSStickNick;
300
                        GPSStickRoll    = FromNaviCtrl.GPSStickRoll;
301
                        NCDataOkay = 250;
302
                }
303
                // update compass readings
304
                if(FromNaviCtrl.CompassHeading <= 360)
305
                {
306
                        CompassHeading = FromNaviCtrl.CompassHeading;
307
                }
308
                if(CompassHeading < 0) CompassOffCourse = 0;
309
                else CompassOffCourse = ((540 + CompassHeading - CompassCourse) % 360) - 180;
310
        // NaviCtrl wants to beep?
311
                if (FromNaviCtrl.BeepTime > BeepTime && !CompassCalState) BeepTime = FromNaviCtrl.BeepTime;
312
 
313
                switch (FromNaviCtrl.Command)
314
                {
315
                        case SPI_KALMAN:
316
                                FCParam.KalmanK = FromNaviCtrl.Param.Byte[0];
317
                                FCParam.KalmanMaxFusion = FromNaviCtrl.Param.Byte[1];
318
                                FCParam.KalmanMaxDrift = FromNaviCtrl.Param.Byte[2];
319
                                NCSerialDataOkay = FromNaviCtrl.Param.Byte[3];
320
                                DebugOut.Analog[29] = NCSerialDataOkay;
321
                                break;
322
 
323
                        default:
324
                                break;
325
                }
326
        }
327
        else // no valid data from NaviCtrl
328
        {
329
                // disable GPS control
330
                GPSStickNick = 0;
331
                GPSStickRoll = 0;
332
        }
333
}
334
 
335
 
336
 
337
/*********************************************/
338
/*  Start Transmission of packet to NaviCtrl */
339
/*********************************************/
340
void SPI_StartTransmitPacket(void)
341
{
342
 
343
        if (!SPITransferCompleted) return; // return immediately if transfer is in progress
344
        else // transmission was completed
345
        {
346
                SLAVE_SELECT_PORT &=  ~(1 << SPI_SLAVE_SELECT);  // Select slave
347
 
348
                // cyclic commands
349
                ToNaviCtrl.Command = SPI_CommandSequence[SPI_CommandCounter++];
350
                if (SPI_CommandCounter >= sizeof(SPI_CommandSequence)) SPI_CommandCounter = 0;
351
 
352
                SPITransferCompleted = 0; // transfer is in progress
353
                UpdateSPI_Buffer();    // update data in ToNaviCtrl
354
 
355
                SPI_TxBufferIndex = 1; //proceed with 2nd byte
356
 
357
                // -- Debug-Output ---
358
                //----
359
                asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop");            asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop");
360
                asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop");            asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop");
361
                asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop");            asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop");
362
                ToNaviCtrl.Chksum = ToNaviCtrl.Sync1; // init checksum
363
                SPDR = ToNaviCtrl.Sync1; // send first byte
364
        }
365
}
366
 
367
//------------------------------------------------------
368
// This is the spi data transfer between FlightCtrl and NaviCtrl
369
// Every time this routine is called within the mainloop one byte of the packet to
370
// the NaviCtrl and one byte of the packet from the NaviCtrl is possible transfered
371
 
372
void SPI_TransmitByte(void)
373
{
374
        static SPI_RXState_t SPI_RXState = SPI_SYNC1;
375
        uint8_t rxdata;
376
        static uint8_t rxchksum;
377
 
378
        if (SPITransferCompleted) return;  // return immediatly if transfer was completed
379
        if (!(SPSR & (1 << SPIF))) return; // return if no SPI-IRQ pending
380
        SendSPI = 4; // mait 4 * 0.102 ms for the next call of SPI_TransmitByte() in the main loop
381
 
382
        SLAVE_SELECT_PORT |=  (1 << SPI_SLAVE_SELECT);   // DeselectSlave
383
 
384
        rxdata = SPDR; // save spi data register
385
 
386
        switch (SPI_RXState)
387
        {
388
                case SPI_SYNC1: // first sync byte
389
                        SPI_RxBufferIndex = 0; // set pointer to start of rx buffer
390
                        rxchksum = rxdata; // initialize checksum
391
                        if (rxdata == SPI_RXSYNCBYTE1 )
392
                        {       // 1st Syncbyte found
393
                                SPI_RXState  = SPI_SYNC2; // trigger to state for second sync byte
394
                        }
395
                        break;
396
 
397
                case SPI_SYNC2: // second sync byte
398
                        if (rxdata == SPI_RXSYNCBYTE2)
399
                        {       // 2nd Syncbyte found
400
                                rxchksum += rxdata; // update checksum
401
                                SPI_RXState  = SPI_DATA;   // trigger to state for second sync byte
402
                        }
403
                        else // 2nd Syncbyte not found
404
                        {
405
                                SPI_RXState  = SPI_SYNC1; // jump back to 1st sync byte
406
                        }
407
                        break;
408
 
409
                case SPI_DATA: // data bytes
410
                        SPI_RxBuffer[SPI_RxBufferIndex++] = rxdata;  // copy data byte to spi buffer
411
                        // if all bytes are received of a packet from the NaviCtrl
412
                        if (SPI_RxBufferIndex >= SPI_RXBUFFER_LEN)
413
                        {   // last byte transfered is the checksum of the packet
414
                                if (rxdata == rxchksum) // checksum matching?
415
                                {
416
                                        // copy SPI_RxBuffer -> FromFlightCtrl
417
                                        uint8_t *ptr = (uint8_t *)&FromNaviCtrl;
418
                                        cli();
419
                                        memcpy(ptr, (uint8_t *) SPI_RxBuffer, sizeof(FromNaviCtrl));
420
                                        sei();
421
                                        SPI_RxDataValid = 1;
422
                                        //DebugOut.Analog[18]++;
423
                                }
424
                                else
425
                                {   // checksum does not match
426
                                        //DebugOut.Analog[17]++;
427
                                        SPI_RxDataValid = 0; // reset valid flag
428
                                }
429
                                SPI_RXState  = SPI_SYNC1; // reset state sync
430
                        }
431
                        else // not all bytes transfered
432
                        {
433
                                rxchksum += rxdata; // update checksum
434
                        }
435
                        break;
436
        }// eof switch(SPI_RXState)
437
 
438
    // if still some bytes left for transmission to NaviCtrl
439
        if (SPI_TxBufferIndex < SPI_TXBUFFER_LEN)
440
        {
441
                SLAVE_SELECT_PORT &=  ~(1 << SPI_SLAVE_SELECT);  // SelectSlave
442
                asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop");
443
                asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop");
444
                asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop");
445
 
446
                SPDR = SPI_TxBuffer[SPI_TxBufferIndex]; // transmit byte
447
                ToNaviCtrl.Chksum += SPI_TxBuffer[SPI_TxBufferIndex]; // update checksum for everey byte that was sent
448
                SPI_TxBufferIndex++;
449
        }
450
        else
451
        {
452
                //Transfer of all bytes of the packet to NaviCtrl completed
453
                SPITransferCompleted = 1;
454
        }
455
}
456
 
457
 
458