Subversion Repositories FlightCtrl

Rev

Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
1612 dongfang 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 "spi.h"
56
#include "rc.h"
57
#include "eeprom.h"
58
#include "uart0.h"
59
#include "timer0.h"
60
#include "analog.h"
61
#include "attitude.h"
1645 - 62
#include "GPSControl.h"
1612 dongfang 63
#include "flight.h"
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
1775 - 114
#ifndef SPI2X
1612 dongfang 115
#define SPI2X  SPI2X0
1775 - 116
#endif
1612 dongfang 117
// -------------------------
118
 
119
#define SLAVE_SELECT_DDR_PORT   DDRC
120
#define SLAVE_SELECT_PORT       PORTC
121
#define SPI_SLAVE_SELECT        PC5
122
 
123
#define SPI_TXSYNCBYTE1 0xAA
124
#define SPI_TXSYNCBYTE2 0x83
125
#define SPI_RXSYNCBYTE1 0x81
126
#define SPI_RXSYNCBYTE2 0x55
127
 
1775 - 128
typedef enum {
129
  SPI_SYNC1,
130
  SPI_SYNC2,
131
  SPI_DATA
132
} SPI_RXState_t;
1612 dongfang 133
 
134
// data exchange packets to and From NaviCtrl
1775 - 135
ToNaviCtrl_t                    toNaviCtrl;
136
FromNaviCtrl_t                  fromNaviCtrl;
137
SPI_VersionInfo_t               SPI_VersionInfo;
1612 dongfang 138
 
139
 
140
// rx packet buffer
1775 - 141
#define SPI_RXBUFFER_LEN sizeof(fromNaviCtrl)
1612 dongfang 142
uint8_t SPI_RxBuffer[SPI_RXBUFFER_LEN];
143
uint8_t SPI_RxBufferIndex = 0;
144
uint8_t SPI_RxBuffer_Request = 0;
145
 
146
// tx packet buffer
1775 - 147
#define SPI_TXBUFFER_LEN sizeof(toNaviCtrl)
1612 dongfang 148
uint8_t *SPI_TxBuffer;
149
uint8_t SPI_TxBufferIndex = 0;
150
 
151
uint8_t SPITransferCompleted, SPI_ChkSum;
152
uint8_t SPI_RxDataValid = 0;
153
uint8_t NCDataOkay = 0;
154
uint8_t NCSerialDataOkay = 0;
155
 
156
uint8_t SPI_CommandSequence[] = { SPI_CMD_USER, SPI_CMD_STICK, SPI_CMD_PARAMETER1, SPI_CMD_STICK, SPI_CMD_MISC, SPI_CMD_VERSION };
157
uint8_t SPI_CommandCounter = 0;
158
 
159
/*********************************************/
160
/*  Initialize SPI interface to NaviCtrl     */
161
/*********************************************/
162
void SPI_MasterInit(void) {
163
  DDR_SPI |= (1<<DD_MOSI)|(1<<DD_SCK);    // Set MOSI and SCK output, all others input
164
  SLAVE_SELECT_DDR_PORT |= (1 << SPI_SLAVE_SELECT); // set Slave select port as output port
165
 
166
  SPCR = (1<<SPE)|(1<<MSTR)|(1<<SPR1)|(0<<SPR0)|(0<<SPIE);   // Enable SPI, Master, set clock rate fck/64
167
  SPSR = 0;//(1<<SPI2X);
168
 
169
  SLAVE_SELECT_PORT |=  (1 << SPI_SLAVE_SELECT); // Deselect Slave
170
 
1775 - 171
  SPI_TxBuffer = (uint8_t *) &toNaviCtrl; // set pointer to tx-buffer
1612 dongfang 172
  SPITransferCompleted = 1;
173
  // initialize data packet to NaviControl
1775 - 174
  toNaviCtrl.Sync1 = SPI_TXSYNCBYTE1;
175
  toNaviCtrl.Sync2 = SPI_TXSYNCBYTE2;
1612 dongfang 176
 
1775 - 177
  toNaviCtrl.Command = SPI_CMD_USER;
178
  toNaviCtrl.IntegralPitch = 0;
179
  toNaviCtrl.IntegralRoll = 0;
1612 dongfang 180
  NCSerialDataOkay = 0;
181
  NCDataOkay = 0;
182
 
183
  SPI_RxDataValid = 0;
184
 
185
  SPI_VersionInfo.Major = VERSION_MAJOR;
186
  SPI_VersionInfo.Minor = VERSION_MINOR;
187
  SPI_VersionInfo.Patch = VERSION_PATCH;
188
  SPI_VersionInfo.Compatible = NC_SPI_COMPATIBLE;
189
}
190
 
191
/**********************************************************/
192
/*  Update Data transferd by the SPI from/to NaviCtrl     */
193
/**********************************************************/
194
void UpdateSPI_Buffer(void) {
195
  uint8_t i;
196
  int16_t tmp;
197
  cli(); // stop all interrupts to avoid writing of new data during update of that packet.
198
 
199
  // update content of packet to NaviCtrl
1775 - 200
  toNaviCtrl.IntegralPitch = (int16_t)((10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1°
201
  toNaviCtrl.IntegralRoll  = (int16_t)((10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL);  // convert to multiple of 0.1°
202
  toNaviCtrl.GyroHeading   = (int16_t)((10 * yawGyroHeading)   / GYRO_DEG_FACTOR_YAW);   // convert to multiple of 0.1°
203
  toNaviCtrl.GyroPitch = rate_ATT[PITCH];
204
  toNaviCtrl.GyroRoll = rate_ATT[ROLL];
205
  toNaviCtrl.GyroYaw  = yawRate;
206
  toNaviCtrl.AccPitch = (10 * getAngleEstimateFromAcc(PITCH)) / GYRO_DEG_FACTOR_PITCHROLL; // convert to multiple of 0.1°
207
  toNaviCtrl.AccRoll  = (10 * getAngleEstimateFromAcc(ROLL))  / GYRO_DEG_FACTOR_PITCHROLL; // convert to multiple of 0.1°
1612 dongfang 208
 
1775 - 209
 
210
  // TODO: What are these little bastards?
211
 
212
  averageAcc[PITCH] = averageAcc[ROLL] = averageAccCount = 0;
213
 
214
  switch(toNaviCtrl.Command) {
1612 dongfang 215
  case SPI_CMD_USER:
216
    for (i=0; i<sizeof(dynamicParams.UserParams); i++) {
1775 - 217
      toNaviCtrl.Param.Byte[i] = dynamicParams.UserParams[i];
1612 dongfang 218
    }
1775 - 219
    toNaviCtrl.Param.Byte[8] = MKFlags;
1612 dongfang 220
    MKFlags &= ~(MKFLAG_CALIBRATE | MKFLAG_START); // calibrate and start are temporal states that are cleared immediately after transmitting
1775 - 221
    toNaviCtrl.Param.Byte[9] = (uint8_t)UBat;
222
    toNaviCtrl.Param.Byte[10] = staticParams.LowVoltageWarning;
223
    toNaviCtrl.Param.Byte[11] = getActiveParamSet();
1612 dongfang 224
    break;
225
 
226
  case SPI_CMD_PARAMETER1:
1775 - 227
    toNaviCtrl.Param.Byte[0] = staticParams.NaviGpsModeControl;     // Parameters for the Naviboard
228
    toNaviCtrl.Param.Byte[1] = staticParams.NaviGpsGain;
229
    toNaviCtrl.Param.Byte[2] = staticParams.NaviGpsP;
230
    toNaviCtrl.Param.Byte[3] = staticParams.NaviGpsI;
231
    toNaviCtrl.Param.Byte[4] = staticParams.NaviGpsD;
232
    toNaviCtrl.Param.Byte[5] = staticParams.NaviGpsACC;
233
    toNaviCtrl.Param.Byte[6] = staticParams.NaviGpsMinSat;
234
    toNaviCtrl.Param.Byte[7] = staticParams.NaviStickThreshold;
235
    toNaviCtrl.Param.Byte[8] = staticParams.NaviOperatingRadius;
236
    toNaviCtrl.Param.Byte[9] = staticParams.NaviWindCorrection;
237
    toNaviCtrl.Param.Byte[10] = staticParams.NaviSpeedCompensation;
238
    toNaviCtrl.Param.Byte[11] = staticParams.NaviAngleLimitation;
1612 dongfang 239
    break;
240
 
241
  case SPI_CMD_STICK:
242
    tmp = PPM_in[staticParams.ChannelAssignment[CH_THROTTLE]];  if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128;
1775 - 243
    toNaviCtrl.Param.Byte[0] = (int8_t) tmp;
1612 dongfang 244
    tmp = PPM_in[staticParams.ChannelAssignment[CH_YAW]]; if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128;
1775 - 245
    toNaviCtrl.Param.Byte[1] = (int8_t) tmp;
1612 dongfang 246
    tmp = PPM_in[staticParams.ChannelAssignment[CH_ROLL]]; if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128;
1775 - 247
    toNaviCtrl.Param.Byte[2] = (int8_t) tmp;
1612 dongfang 248
    tmp = PPM_in[staticParams.ChannelAssignment[CH_PITCH]]; if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128;
1775 - 249
    toNaviCtrl.Param.Byte[3] = (int8_t) tmp;
250
    toNaviCtrl.Param.Byte[4] = (uint8_t) variables[0];
251
    toNaviCtrl.Param.Byte[5] = (uint8_t) variables[1];
252
    toNaviCtrl.Param.Byte[6] = (uint8_t) variables[2];
253
    toNaviCtrl.Param.Byte[7] = (uint8_t) variables[3];
254
    toNaviCtrl.Param.Byte[8] = (uint8_t) RC_Quality;
1612 dongfang 255
    break;
256
 
257
  case SPI_CMD_MISC:
1775 - 258
    toNaviCtrl.Param.Byte[0] = compassCalState;
259
    if(compassCalState > 4) { // jump from 5 to 0
260
          compassCalState  = 0;
261
    }
262
    toNaviCtrl.Param.Byte[1] = staticParams.NaviPHLoginTime;
263
    // TODO: Height and in the correct scaling...
264
    toNaviCtrl.Param.Int[1]  = 0; //readingHeight; // at address of Byte 2 and 3
265
    toNaviCtrl.Param.Byte[4] = staticParams.NaviGpsPLimit;
266
    toNaviCtrl.Param.Byte[5] = staticParams.NaviGpsILimit;
267
    toNaviCtrl.Param.Byte[6] = staticParams.NaviGpsDLimit;
1612 dongfang 268
    break;
269
 
270
  case SPI_CMD_VERSION:
1775 - 271
    toNaviCtrl.Param.Byte[0] = SPI_VersionInfo.Major;
272
    toNaviCtrl.Param.Byte[1] = SPI_VersionInfo.Minor;
273
    toNaviCtrl.Param.Byte[2] = SPI_VersionInfo.Patch;
274
    toNaviCtrl.Param.Byte[3] = SPI_VersionInfo.Compatible;
275
    toNaviCtrl.Param.Byte[4] = BoardRelease;
1612 dongfang 276
    break;
277
  default:
278
    break;
279
  }
280
 
281
  sei(); // enable all interrupts
282
 
283
  // analyze content of packet from NaviCtrl if valid
284
  if (SPI_RxDataValid) {
285
    // update gps controls
1775 - 286
    if(abs(fromNaviCtrl.GPSStickPitch) < 512 && abs(fromNaviCtrl.GPSStickRoll) < 512 && (staticParams.GlobalConfig & CFG_GPS_ACTIVE)) {
287
      GPSStickPitch     = fromNaviCtrl.GPSStickPitch;
288
      GPSStickRoll      = fromNaviCtrl.GPSStickRoll;
1612 dongfang 289
      NCDataOkay = 250;
290
    }
291
    // update compass readings
1775 - 292
    if(fromNaviCtrl.CompassHeading <= 360) {
293
      compassHeading = fromNaviCtrl.CompassHeading;
1612 dongfang 294
    }
295
    if(compassHeading < 0) compassOffCourse = 0;
296
    else compassOffCourse = ((540 + compassHeading - compassCourse) % 360) - 180;
297
    // NaviCtrl wants to beep?
1775 - 298
    if (fromNaviCtrl.BeepTime > BeepTime && !compassCalState) BeepTime = fromNaviCtrl.BeepTime;
1612 dongfang 299
 
1775 - 300
    switch (fromNaviCtrl.Command) {
1612 dongfang 301
    case SPI_KALMAN:
1775 - 302
      dynamicParams.KalmanK             = fromNaviCtrl.Param.Byte[0];
303
      dynamicParams.KalmanMaxFusion = fromNaviCtrl.Param.Byte[1];
304
      dynamicParams.KalmanMaxDrift      = fromNaviCtrl.Param.Byte[2];
305
      NCSerialDataOkay                          = fromNaviCtrl.Param.Byte[3];
1612 dongfang 306
      break;
307
 
308
    default:
309
      break;
310
    }
1775 - 311
  } else { // no valid data from NaviCtrl
1612 dongfang 312
      // disable GPS control
313
      GPSStickPitch = 0;
314
      GPSStickRoll = 0;
315
    }
316
}
317
 
318
/*********************************************/
319
/*  Start Transmission of packet to NaviCtrl */
320
/*********************************************/
321
void SPI_StartTransmitPacket(void){
322
  if (!SPITransferCompleted) return; // return immediately if transfer is in progress
323
  else // transmission was completed
324
    {
325
      SLAVE_SELECT_PORT &=  ~(1 << SPI_SLAVE_SELECT);  // Select slave
326
 
327
      // cyclic commands
1775 - 328
      toNaviCtrl.Command = SPI_CommandSequence[SPI_CommandCounter++];
1612 dongfang 329
      if (SPI_CommandCounter >= sizeof(SPI_CommandSequence)) SPI_CommandCounter = 0;
330
 
331
      SPITransferCompleted = 0; // transfer is in progress
1775 - 332
      UpdateSPI_Buffer();    // update data in toNaviCtrl
1612 dongfang 333
 
334
      SPI_TxBufferIndex = 1; //proceed with 2nd byte
335
 
336
      // -- Debug-Output ---
337
      //----
338
      asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop");      asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop");
339
      asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop");      asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop");
340
      asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop");      asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop");
1775 - 341
      toNaviCtrl.Chksum = toNaviCtrl.Sync1; // init checksum
342
      SPDR = toNaviCtrl.Sync1; // send first byte
1612 dongfang 343
    }
344
}
345
 
346
//------------------------------------------------------
347
// This is the spi data transfer between FlightCtrl and NaviCtrl
348
// Every time this routine is called within the mainloop one byte of the packet to
349
// the NaviCtrl and one byte of the packet from the NaviCtrl is possible transfered
350
 
351
void SPI_TransmitByte(void) {
352
  static SPI_RXState_t SPI_RXState = SPI_SYNC1;
353
  uint8_t rxdata;
354
  static uint8_t rxchksum;
355
 
356
  if (SPITransferCompleted) return;  // return immediatly if transfer was completed
357
  if (!(SPSR & (1 << SPIF))) return; // return if no SPI-IRQ pending
358
  SendSPI = 4; // mait 4 * 0.102 ms for the next call of SPI_TransmitByte() in the main loop
359
 
360
  SLAVE_SELECT_PORT |=  (1 << SPI_SLAVE_SELECT);   // DeselectSlave
361
 
362
  rxdata = SPDR; // save spi data register
363
 
364
  switch (SPI_RXState) {
365
  case SPI_SYNC1: // first sync byte
366
    SPI_RxBufferIndex = 0; // set pointer to start of rx buffer
367
    rxchksum = rxdata; // initialize checksum
368
    if (rxdata == SPI_RXSYNCBYTE1 )
369
      { // 1st Syncbyte found
370
        SPI_RXState  = SPI_SYNC2; // trigger to state for second sync byte
371
      }
372
    break;
373
 
374
  case SPI_SYNC2: // second sync byte
375
    if (rxdata == SPI_RXSYNCBYTE2)
376
      { // 2nd Syncbyte found
377
        rxchksum += rxdata; // update checksum
378
        SPI_RXState  = SPI_DATA;   // trigger to state for second sync byte
379
      }
380
    else // 2nd Syncbyte not found
381
      {
382
        SPI_RXState  = SPI_SYNC1; // jump back to 1st sync byte
383
      }
384
    break;
385
 
386
  case SPI_DATA: // data bytes
387
    SPI_RxBuffer[SPI_RxBufferIndex++] = rxdata;  // copy data byte to spi buffer
388
    // if all bytes are received of a packet from the NaviCtrl
1775 - 389
    if (SPI_RxBufferIndex >= SPI_RXBUFFER_LEN) {   // last byte transfered is the checksum of the packet
390
        if (rxdata == rxchksum) { // checksum matching?
1612 dongfang 391
            // copy SPI_RxBuffer -> FromFlightCtrl
1775 - 392
            uint8_t *ptr = (uint8_t *)&fromNaviCtrl;
1612 dongfang 393
            cli();
1775 - 394
            memcpy(ptr, (uint8_t *) SPI_RxBuffer, sizeof(fromNaviCtrl));
1612 dongfang 395
            sei();
396
            SPI_RxDataValid = 1;
1775 - 397
          } else {   // checksum does not match
1612 dongfang 398
            SPI_RxDataValid = 0; // reset valid flag
399
          }
1775 - 400
          SPI_RXState  = SPI_SYNC1; // reset state sync
401
    } else { // not all bytes transfered
402
          rxchksum += rxdata; // update checksum
403
    }
1612 dongfang 404
    break;
405
  }// eof switch(SPI_RXState)
406
 
407
  // if still some bytes left for transmission to NaviCtrl
1775 - 408
  if (SPI_TxBufferIndex < SPI_TXBUFFER_LEN) {
1612 dongfang 409
      SLAVE_SELECT_PORT &=  ~(1 << SPI_SLAVE_SELECT);  // SelectSlave
410
      asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop");
411
      asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop");
412
      asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop");   asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop");
413
 
414
      SPDR = SPI_TxBuffer[SPI_TxBufferIndex]; // transmit byte
1775 - 415
      toNaviCtrl.Chksum += SPI_TxBuffer[SPI_TxBufferIndex]; // update checksum for everey byte that was sent
1612 dongfang 416
      SPI_TxBufferIndex++;
417
    } else {
418
      //Transfer of all bytes of the packet to NaviCtrl completed
419
      SPITransferCompleted = 1;
420
    }
421
}