Subversion Repositories FlightCtrl

Rev

Rev 1645 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 1645 Rev 1775
Line 109... Line 109...
109
#define SPIF   SPIF0
109
#define SPIF   SPIF0
110
#endif
110
#endif
111
#ifndef WCOL
111
#ifndef WCOL
112
#define WCOL   WCOL0
112
#define WCOL   WCOL0
113
#endif
113
#endif
114
//#ifndef SPI2X
114
#ifndef SPI2X
115
#define SPI2X  SPI2X0
115
#define SPI2X  SPI2X0
116
//#endif
116
#endif
117
// -------------------------
117
// -------------------------
Line 118... Line 118...
118
 
118
 
119
#define SLAVE_SELECT_DDR_PORT   DDRC
119
#define SLAVE_SELECT_DDR_PORT   DDRC
120
#define SLAVE_SELECT_PORT       PORTC
120
#define SLAVE_SELECT_PORT       PORTC
Line 123... Line 123...
123
#define SPI_TXSYNCBYTE1 0xAA
123
#define SPI_TXSYNCBYTE1 0xAA
124
#define SPI_TXSYNCBYTE2 0x83
124
#define SPI_TXSYNCBYTE2 0x83
125
#define SPI_RXSYNCBYTE1 0x81
125
#define SPI_RXSYNCBYTE1 0x81
126
#define SPI_RXSYNCBYTE2 0x55
126
#define SPI_RXSYNCBYTE2 0x55
Line 127... Line 127...
127
 
127
 
128
typedef enum
-
 
129
  {
128
typedef enum {
130
    SPI_SYNC1,
129
  SPI_SYNC1,
131
    SPI_SYNC2,
130
  SPI_SYNC2,
132
    SPI_DATA
131
  SPI_DATA
133
  } SPI_RXState_t;
-
 
Line 134... Line 132...
134
 
132
} SPI_RXState_t;
135
 
133
 
136
// data exchange packets to and From NaviCtrl
134
// data exchange packets to and From NaviCtrl
-
 
135
ToNaviCtrl_t                    toNaviCtrl;
Line 137... Line -...
137
ToNaviCtrl_t                    ToNaviCtrl;
-
 
Line 138... Line 136...
138
FromNaviCtrl_t                  FromNaviCtrl;
136
FromNaviCtrl_t                  fromNaviCtrl;
139
 
137
SPI_VersionInfo_t               SPI_VersionInfo;
140
SPI_VersionInfo_t SPI_VersionInfo;
138
 
141
 
139
 
142
// rx packet buffer
140
// rx packet buffer
Line 143... Line 141...
143
#define SPI_RXBUFFER_LEN sizeof(FromNaviCtrl)
141
#define SPI_RXBUFFER_LEN sizeof(fromNaviCtrl)
144
uint8_t SPI_RxBuffer[SPI_RXBUFFER_LEN];
142
uint8_t SPI_RxBuffer[SPI_RXBUFFER_LEN];
145
uint8_t SPI_RxBufferIndex = 0;
143
uint8_t SPI_RxBufferIndex = 0;
146
uint8_t SPI_RxBuffer_Request = 0;
144
uint8_t SPI_RxBuffer_Request = 0;
Line 147... Line 145...
147
 
145
 
148
// tx packet buffer
146
// tx packet buffer
Line 168... Line 166...
168
  SPCR = (1<<SPE)|(1<<MSTR)|(1<<SPR1)|(0<<SPR0)|(0<<SPIE);   // Enable SPI, Master, set clock rate fck/64
166
  SPCR = (1<<SPE)|(1<<MSTR)|(1<<SPR1)|(0<<SPR0)|(0<<SPIE);   // Enable SPI, Master, set clock rate fck/64
169
  SPSR = 0;//(1<<SPI2X);
167
  SPSR = 0;//(1<<SPI2X);
Line 170... Line 168...
170
 
168
 
Line 171... Line 169...
171
  SLAVE_SELECT_PORT |=  (1 << SPI_SLAVE_SELECT); // Deselect Slave
169
  SLAVE_SELECT_PORT |=  (1 << SPI_SLAVE_SELECT); // Deselect Slave
172
 
170
 
173
  SPI_TxBuffer = (uint8_t *) &ToNaviCtrl; // set pointer to tx-buffer
171
  SPI_TxBuffer = (uint8_t *) &toNaviCtrl; // set pointer to tx-buffer
174
  SPITransferCompleted = 1;
172
  SPITransferCompleted = 1;
175
  // initialize data packet to NaviControl
173
  // initialize data packet to NaviControl
Line 176... Line 174...
176
  ToNaviCtrl.Sync1 = SPI_TXSYNCBYTE1;
174
  toNaviCtrl.Sync1 = SPI_TXSYNCBYTE1;
177
  ToNaviCtrl.Sync2 = SPI_TXSYNCBYTE2;
175
  toNaviCtrl.Sync2 = SPI_TXSYNCBYTE2;
178
 
176
 
179
  ToNaviCtrl.Command = SPI_CMD_USER;
177
  toNaviCtrl.Command = SPI_CMD_USER;
180
  ToNaviCtrl.IntegralNick = 0;
178
  toNaviCtrl.IntegralPitch = 0;
Line 181... Line 179...
181
  ToNaviCtrl.IntegralRoll = 0;
179
  toNaviCtrl.IntegralRoll = 0;
Line 188... Line 186...
188
  SPI_VersionInfo.Minor = VERSION_MINOR;
186
  SPI_VersionInfo.Minor = VERSION_MINOR;
189
  SPI_VersionInfo.Patch = VERSION_PATCH;
187
  SPI_VersionInfo.Patch = VERSION_PATCH;
190
  SPI_VersionInfo.Compatible = NC_SPI_COMPATIBLE;
188
  SPI_VersionInfo.Compatible = NC_SPI_COMPATIBLE;
191
}
189
}
Line 192... Line -...
192
 
-
 
193
 
190
 
194
/**********************************************************/
191
/**********************************************************/
195
/*  Update Data transferd by the SPI from/to NaviCtrl     */
192
/*  Update Data transferd by the SPI from/to NaviCtrl     */
196
/**********************************************************/
193
/**********************************************************/
197
void UpdateSPI_Buffer(void) {
194
void UpdateSPI_Buffer(void) {
198
  uint8_t i;
195
  uint8_t i;
199
  int16_t tmp;
196
  int16_t tmp;
Line 200... Line 197...
200
  cli(); // stop all interrupts to avoid writing of new data during update of that packet.
197
  cli(); // stop all interrupts to avoid writing of new data during update of that packet.
201
 
198
 
202
  // update content of packet to NaviCtrl
199
  // update content of packet to NaviCtrl
203
  ToNaviCtrl.IntegralNick = (int16_t)((10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1°
200
  toNaviCtrl.IntegralPitch = (int16_t)((10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1°
204
  ToNaviCtrl.IntegralRoll = (int16_t)((10 * angle[ROLL]) / 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°
205
  ToNaviCtrl.GyroHeading  = (int16_t)((10 * yawGyroHeading)   / GYRO_DEG_FACTOR_YAW); // convert to multiple of 0.1°
202
  toNaviCtrl.GyroHeading   = (int16_t)((10 * yawGyroHeading)   / GYRO_DEG_FACTOR_YAW);   // convert to multiple of 0.1°
206
  ToNaviCtrl.GyroNick = rate_PID[PITCH]; // TODO: Which one should it be??
203
  toNaviCtrl.GyroPitch = rate_ATT[PITCH];
207
  ToNaviCtrl.GyroRoll = rate_PID[ROLL];
204
  toNaviCtrl.GyroRoll = rate_ATT[ROLL];
208
  ToNaviCtrl.GyroYaw =  yawRate;
205
  toNaviCtrl.GyroYaw  = yawRate;
209
  ToNaviCtrl.AccNick =  0; // ((int16_t) 10 * ACC_AMPLIFY * (NaviAccNick / NaviCntAcc)) / ACC_DEG_FACTOR; // convert to multiple of 0.1°
-
 
Line -... Line 206...
-
 
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°
-
 
208
 
-
 
209
 
-
 
210
  // TODO: What are these little bastards?
210
  ToNaviCtrl.AccRoll =  0; // ((int16_t) 10 * ACC_AMPLIFY * (NaviAccRoll / NaviCntAcc)) / ACC_DEG_FACTOR; // convert to multiple of 0.1°
211
 
211
  // naviCntAcc = 0; naviAccPitch = 0; naviAccRoll = 0;
212
  averageAcc[PITCH] = averageAcc[ROLL] = averageAccCount = 0;
212
 
213
 
213
  switch(ToNaviCtrl.Command) {
214
  switch(toNaviCtrl.Command) {
214
  case SPI_CMD_USER:
215
  case SPI_CMD_USER:
215
    for (i=0; i<sizeof(dynamicParams.UserParams); i++) {
216
    for (i=0; i<sizeof(dynamicParams.UserParams); i++) {
216
      ToNaviCtrl.Param.Byte[i] = dynamicParams.UserParams[i];
217
      toNaviCtrl.Param.Byte[i] = dynamicParams.UserParams[i];
217
    }
218
    }
218
    ToNaviCtrl.Param.Byte[8] = MKFlags;
219
    toNaviCtrl.Param.Byte[8] = MKFlags;
219
    MKFlags &= ~(MKFLAG_CALIBRATE | MKFLAG_START); // calibrate and start are temporal states that are cleared immediately after transmitting
220
    MKFlags &= ~(MKFLAG_CALIBRATE | MKFLAG_START); // calibrate and start are temporal states that are cleared immediately after transmitting
220
    ToNaviCtrl.Param.Byte[9] = (uint8_t)UBat;
221
    toNaviCtrl.Param.Byte[9] = (uint8_t)UBat;
Line 221... Line 222...
221
    ToNaviCtrl.Param.Byte[10] = staticParams.LowVoltageWarning;
222
    toNaviCtrl.Param.Byte[10] = staticParams.LowVoltageWarning;
222
    ToNaviCtrl.Param.Byte[11] = getActiveParamSet();
223
    toNaviCtrl.Param.Byte[11] = getActiveParamSet();
223
    break;
224
    break;
224
 
225
 
225
  case SPI_CMD_PARAMETER1:
226
  case SPI_CMD_PARAMETER1:
226
    ToNaviCtrl.Param.Byte[0] = staticParams.NaviGpsModeControl;     // Parameters for the Naviboard
227
    toNaviCtrl.Param.Byte[0] = staticParams.NaviGpsModeControl;     // Parameters for the Naviboard
227
    ToNaviCtrl.Param.Byte[1] = staticParams.NaviGpsGain;
228
    toNaviCtrl.Param.Byte[1] = staticParams.NaviGpsGain;
228
    ToNaviCtrl.Param.Byte[2] = staticParams.NaviGpsP;
229
    toNaviCtrl.Param.Byte[2] = staticParams.NaviGpsP;
229
    ToNaviCtrl.Param.Byte[3] = staticParams.NaviGpsI;
230
    toNaviCtrl.Param.Byte[3] = staticParams.NaviGpsI;
230
    ToNaviCtrl.Param.Byte[4] = staticParams.NaviGpsD;
231
    toNaviCtrl.Param.Byte[4] = staticParams.NaviGpsD;
231
    ToNaviCtrl.Param.Byte[5] = staticParams.NaviGpsACC;
232
    toNaviCtrl.Param.Byte[5] = staticParams.NaviGpsACC;
232
    ToNaviCtrl.Param.Byte[6] = staticParams.NaviGpsMinSat;
233
    toNaviCtrl.Param.Byte[6] = staticParams.NaviGpsMinSat;
233
    ToNaviCtrl.Param.Byte[7] = staticParams.NaviStickThreshold;
234
    toNaviCtrl.Param.Byte[7] = staticParams.NaviStickThreshold;
234
    ToNaviCtrl.Param.Byte[8] = staticParams.NaviOperatingRadius;
235
    toNaviCtrl.Param.Byte[8] = staticParams.NaviOperatingRadius;
Line 235... Line 236...
235
    ToNaviCtrl.Param.Byte[9] = staticParams.NaviWindCorrection;
236
    toNaviCtrl.Param.Byte[9] = staticParams.NaviWindCorrection;
236
    ToNaviCtrl.Param.Byte[10] = staticParams.NaviSpeedCompensation;
237
    toNaviCtrl.Param.Byte[10] = staticParams.NaviSpeedCompensation;
237
    ToNaviCtrl.Param.Byte[11] = staticParams.NaviAngleLimitation;
238
    toNaviCtrl.Param.Byte[11] = staticParams.NaviAngleLimitation;
238
    break;
239
    break;
239
   
240
   
240
  case SPI_CMD_STICK:
241
  case SPI_CMD_STICK:
241
    tmp = PPM_in[staticParams.ChannelAssignment[CH_THROTTLE]];  if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128;
242
    tmp = PPM_in[staticParams.ChannelAssignment[CH_THROTTLE]];  if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128;
242
    ToNaviCtrl.Param.Byte[0] = (int8_t) tmp;
243
    toNaviCtrl.Param.Byte[0] = (int8_t) tmp;
243
    tmp = PPM_in[staticParams.ChannelAssignment[CH_YAW]]; if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128;
244
    tmp = PPM_in[staticParams.ChannelAssignment[CH_YAW]]; if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128;
244
    ToNaviCtrl.Param.Byte[1] = (int8_t) tmp;
245
    toNaviCtrl.Param.Byte[1] = (int8_t) tmp;
245
    tmp = PPM_in[staticParams.ChannelAssignment[CH_ROLL]]; if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128;
246
    tmp = PPM_in[staticParams.ChannelAssignment[CH_ROLL]]; if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128;
246
    ToNaviCtrl.Param.Byte[2] = (int8_t) tmp;
247
    toNaviCtrl.Param.Byte[2] = (int8_t) tmp;
247
    tmp = PPM_in[staticParams.ChannelAssignment[CH_PITCH]]; if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128;
248
    tmp = PPM_in[staticParams.ChannelAssignment[CH_PITCH]]; if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128;
248
    ToNaviCtrl.Param.Byte[3] = (int8_t) tmp;
249
    toNaviCtrl.Param.Byte[3] = (int8_t) tmp;
249
    ToNaviCtrl.Param.Byte[4] = (uint8_t) variables[0];
250
    toNaviCtrl.Param.Byte[4] = (uint8_t) variables[0];
Line 250... Line 251...
250
    ToNaviCtrl.Param.Byte[5] = (uint8_t) variables[1];
251
    toNaviCtrl.Param.Byte[5] = (uint8_t) variables[1];
251
    ToNaviCtrl.Param.Byte[6] = (uint8_t) variables[2];
252
    toNaviCtrl.Param.Byte[6] = (uint8_t) variables[2];
252
    ToNaviCtrl.Param.Byte[7] = (uint8_t) variables[3];
253
    toNaviCtrl.Param.Byte[7] = (uint8_t) variables[3];
253
    ToNaviCtrl.Param.Byte[8] = (uint8_t) RC_Quality;
-
 
254
    break;
254
    toNaviCtrl.Param.Byte[8] = (uint8_t) RC_Quality;
255
 
255
    break;
256
  case SPI_CMD_MISC:
256
 
-
 
257
  case SPI_CMD_MISC:
257
    ToNaviCtrl.Param.Byte[0] = compassCalState;
258
    toNaviCtrl.Param.Byte[0] = compassCalState;
258
    if(compassCalState > 4)
259
    if(compassCalState > 4) { // jump from 5 to 0
259
      { // jump from 5 to 0
260
          compassCalState  = 0;
260
        compassCalState  = 0;
261
    }
261
      }
262
    toNaviCtrl.Param.Byte[1] = staticParams.NaviPHLoginTime;
Line 262... Line 263...
262
    ToNaviCtrl.Param.Byte[1] = staticParams.NaviPHLoginTime;
263
    // TODO: Height and in the correct scaling...
263
    ToNaviCtrl.Param.Int[1]  = 0; //readingHeight; // at address of Byte 2 and 3
264
    toNaviCtrl.Param.Int[1]  = 0; //readingHeight; // at address of Byte 2 and 3
264
    ToNaviCtrl.Param.Byte[4] = staticParams.NaviGpsPLimit;
265
    toNaviCtrl.Param.Byte[4] = staticParams.NaviGpsPLimit;
265
    ToNaviCtrl.Param.Byte[5] = staticParams.NaviGpsILimit;
266
    toNaviCtrl.Param.Byte[5] = staticParams.NaviGpsILimit;
266
    ToNaviCtrl.Param.Byte[6] = staticParams.NaviGpsDLimit;
267
    toNaviCtrl.Param.Byte[6] = staticParams.NaviGpsDLimit;
267
    break;
268
    break;
268
   
269
   
269
  case SPI_CMD_VERSION:
-
 
270
    ToNaviCtrl.Param.Byte[0] = SPI_VersionInfo.Major;
270
  case SPI_CMD_VERSION:
271
    ToNaviCtrl.Param.Byte[1] = SPI_VersionInfo.Minor;
271
    toNaviCtrl.Param.Byte[0] = SPI_VersionInfo.Major;
272
    ToNaviCtrl.Param.Byte[2] = SPI_VersionInfo.Patch;
272
    toNaviCtrl.Param.Byte[1] = SPI_VersionInfo.Minor;
Line 273... Line 273...
273
    ToNaviCtrl.Param.Byte[3] = SPI_VersionInfo.Compatible;
273
    toNaviCtrl.Param.Byte[2] = SPI_VersionInfo.Patch;
Line 274... Line 274...
274
    ToNaviCtrl.Param.Byte[4] = BoardRelease;
274
    toNaviCtrl.Param.Byte[3] = SPI_VersionInfo.Compatible;
275
    break;
275
    toNaviCtrl.Param.Byte[4] = BoardRelease;
276
   
276
    break;
277
  default:
277
  default:
278
    break;
278
    break;
279
  }
279
  }
280
 
280
 
281
  sei(); // enable all interrupts
281
  sei(); // enable all interrupts
282
 
282
 
283
  // analyze content of packet from NaviCtrl if valid
283
  // analyze content of packet from NaviCtrl if valid
284
  if (SPI_RxDataValid) {
284
  if (SPI_RxDataValid) {
285
    // update gps controls
285
    // update gps controls
286
    if(abs(FromNaviCtrl.GPSStickNick) < 512 && abs(FromNaviCtrl.GPSStickRoll) < 512 && (staticParams.GlobalConfig & CFG_GPS_ACTIVE)) {
286
    if(abs(fromNaviCtrl.GPSStickPitch) < 512 && abs(fromNaviCtrl.GPSStickRoll) < 512 && (staticParams.GlobalConfig & CFG_GPS_ACTIVE)) {
287
      GPSStickPitch     = FromNaviCtrl.GPSStickNick;
287
      GPSStickPitch     = fromNaviCtrl.GPSStickPitch;
288
      GPSStickRoll      = FromNaviCtrl.GPSStickRoll;
288
      GPSStickRoll      = fromNaviCtrl.GPSStickRoll;
289
      NCDataOkay = 250;
289
      NCDataOkay = 250;
Line 290... Line 290...
290
    }
290
    }
291
    // update compass readings
291
    // update compass readings
292
    if(FromNaviCtrl.CompassHeading <= 360) {
292
    if(fromNaviCtrl.CompassHeading <= 360) {
293
      compassHeading = FromNaviCtrl.CompassHeading;
293
      compassHeading = fromNaviCtrl.CompassHeading;
294
    }
294
    }
295
    if(compassHeading < 0) compassOffCourse = 0;
295
    if(compassHeading < 0) compassOffCourse = 0;
296
    else compassOffCourse = ((540 + compassHeading - compassCourse) % 360) - 180;
-
 
297
    // NaviCtrl wants to beep?
296
    else compassOffCourse = ((540 + compassHeading - compassCourse) % 360) - 180;
Line 298... Line 297...
298
    if (FromNaviCtrl.BeepTime > BeepTime && !compassCalState) BeepTime = FromNaviCtrl.BeepTime;
297
    // NaviCtrl wants to beep?
299
   
298
    if (fromNaviCtrl.BeepTime > BeepTime && !compassCalState) BeepTime = fromNaviCtrl.BeepTime;
300
    switch (FromNaviCtrl.Command) {
299
   
301
    case SPI_KALMAN:
-
 
302
      dynamicParams.KalmanK = FromNaviCtrl.Param.Byte[0];
300
    switch (fromNaviCtrl.Command) {
303
      dynamicParams.KalmanMaxFusion = FromNaviCtrl.Param.Byte[1];
-
 
304
      dynamicParams.KalmanMaxDrift = FromNaviCtrl.Param.Byte[2];
301
    case SPI_KALMAN:
305
      NCSerialDataOkay = FromNaviCtrl.Param.Byte[3];
302
      dynamicParams.KalmanK             = fromNaviCtrl.Param.Byte[0];
306
      //DebugOut.Analog[29] = NCSerialDataOkay;
303
      dynamicParams.KalmanMaxFusion = fromNaviCtrl.Param.Byte[1];
307
      break;
304
      dynamicParams.KalmanMaxDrift      = fromNaviCtrl.Param.Byte[2];
308
     
305
      NCSerialDataOkay                          = fromNaviCtrl.Param.Byte[3];
Line 326... Line 323...
326
  else // transmission was completed
323
  else // transmission was completed
327
    {
324
    {
328
      SLAVE_SELECT_PORT &=  ~(1 << SPI_SLAVE_SELECT);  // Select slave
325
      SLAVE_SELECT_PORT &=  ~(1 << SPI_SLAVE_SELECT);  // Select slave
Line 329... Line 326...
329
     
326
     
330
      // cyclic commands
327
      // cyclic commands
331
      ToNaviCtrl.Command = SPI_CommandSequence[SPI_CommandCounter++];
328
      toNaviCtrl.Command = SPI_CommandSequence[SPI_CommandCounter++];
Line 332... Line 329...
332
      if (SPI_CommandCounter >= sizeof(SPI_CommandSequence)) SPI_CommandCounter = 0;
329
      if (SPI_CommandCounter >= sizeof(SPI_CommandSequence)) SPI_CommandCounter = 0;
333
     
330
     
Line 334... Line 331...
334
      SPITransferCompleted = 0; // transfer is in progress
331
      SPITransferCompleted = 0; // transfer is in progress
Line 335... Line 332...
335
      UpdateSPI_Buffer();    // update data in ToNaviCtrl
332
      UpdateSPI_Buffer();    // update data in toNaviCtrl
336
     
333
     
337
      SPI_TxBufferIndex = 1; //proceed with 2nd byte
334
      SPI_TxBufferIndex = 1; //proceed with 2nd byte
338
     
335
     
339
      // -- Debug-Output ---
336
      // -- Debug-Output ---
340
      //----
337
      //----
341
      asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop");      asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop");
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");
342
      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");
343
      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");
Line 344... Line 341...
344
      ToNaviCtrl.Chksum = ToNaviCtrl.Sync1; // init checksum
341
      toNaviCtrl.Chksum = toNaviCtrl.Sync1; // init checksum
345
      SPDR = ToNaviCtrl.Sync1; // send first byte
342
      SPDR = toNaviCtrl.Sync1; // send first byte
Line 387... Line 384...
387
    break;
384
    break;
Line 388... Line 385...
388
   
385
   
389
  case SPI_DATA: // data bytes
386
  case SPI_DATA: // data bytes
390
    SPI_RxBuffer[SPI_RxBufferIndex++] = rxdata;  // copy data byte to spi buffer
387
    SPI_RxBuffer[SPI_RxBufferIndex++] = rxdata;  // copy data byte to spi buffer
391
    // if all bytes are received of a packet from the NaviCtrl
-
 
392
    if (SPI_RxBufferIndex >= SPI_RXBUFFER_LEN)
388
    // if all bytes are received of a packet from the NaviCtrl
393
      {   // last byte transfered is the checksum of the packet
389
    if (SPI_RxBufferIndex >= SPI_RXBUFFER_LEN) {   // last byte transfered is the checksum of the packet
394
        if (rxdata == rxchksum) // checksum matching?
-
 
395
          {
390
        if (rxdata == rxchksum) { // checksum matching?
396
            // copy SPI_RxBuffer -> FromFlightCtrl
391
            // copy SPI_RxBuffer -> FromFlightCtrl
397
            uint8_t *ptr = (uint8_t *)&FromNaviCtrl;
392
            uint8_t *ptr = (uint8_t *)&fromNaviCtrl;
398
            cli();
393
            cli();
399
            memcpy(ptr, (uint8_t *) SPI_RxBuffer, sizeof(FromNaviCtrl));
394
            memcpy(ptr, (uint8_t *) SPI_RxBuffer, sizeof(fromNaviCtrl));
400
            sei();
395
            sei();
401
            SPI_RxDataValid = 1;
-
 
402
            //DebugOut.Analog[18]++;
-
 
403
          }
-
 
404
        else
396
            SPI_RxDataValid = 1;
405
          {   // checksum does not match
-
 
406
            //DebugOut.Analog[17]++;
397
          } else {   // checksum does not match
407
            SPI_RxDataValid = 0; // reset valid flag
398
            SPI_RxDataValid = 0; // reset valid flag
408
          }
399
          }
409
        SPI_RXState  = SPI_SYNC1; // reset state sync
-
 
410
      }
400
          SPI_RXState  = SPI_SYNC1; // reset state sync
411
    else // not all bytes transfered
-
 
412
      {
401
    } else { // not all bytes transfered
413
        rxchksum += rxdata; // update checksum
402
          rxchksum += rxdata; // update checksum
414
      }
403
    }
415
    break;
404
    break;
Line 416... Line 405...
416
  }// eof switch(SPI_RXState)
405
  }// eof switch(SPI_RXState)
417
 
406
 
418
  // if still some bytes left for transmission to NaviCtrl
-
 
419
  if (SPI_TxBufferIndex < SPI_TXBUFFER_LEN)
407
  // if still some bytes left for transmission to NaviCtrl
420
    {
408
  if (SPI_TxBufferIndex < SPI_TXBUFFER_LEN) {
421
      SLAVE_SELECT_PORT &=  ~(1 << SPI_SLAVE_SELECT);  // SelectSlave
409
      SLAVE_SELECT_PORT &=  ~(1 << SPI_SLAVE_SELECT);  // SelectSlave
422
      asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop");
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");
Line 423... Line 411...
423
      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");
424
      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");
425
     
413
     
426
      SPDR = SPI_TxBuffer[SPI_TxBufferIndex]; // transmit byte
414
      SPDR = SPI_TxBuffer[SPI_TxBufferIndex]; // transmit byte
427
      ToNaviCtrl.Chksum += SPI_TxBuffer[SPI_TxBufferIndex]; // update checksum for everey byte that was sent
415
      toNaviCtrl.Chksum += SPI_TxBuffer[SPI_TxBufferIndex]; // update checksum for everey byte that was sent
428
      SPI_TxBufferIndex++;
416
      SPI_TxBufferIndex++;
429
    } else {
417
    } else {