Rev 1805 | Go to most recent revision | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 1805 | Rev 1821 | ||
|---|---|---|---|
| Line 124... | Line 124... | ||
| 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 | SPI_SYNC1, |
128 | typedef enum { |
| 130 | SPI_SYNC2, |
- | |
| 131 | SPI_DATA |
129 | SPI_SYNC1, SPI_SYNC2, SPI_DATA |
| Line 132... | Line 130... | ||
| 132 | } SPI_RXState_t; |
130 | } SPI_RXState_t; |
| 133 | 131 | ||
| 134 | // data exchange packets to and From NaviCtrl |
132 | // data exchange packets to and From NaviCtrl |
| 135 | ToNaviCtrl_t toNaviCtrl; |
133 | ToNaviCtrl_t toNaviCtrl; |
| Line 136... | Line -... | ||
| 136 | FromNaviCtrl_t fromNaviCtrl; |
- | |
| 137 | SPI_VersionInfo_t SPI_VersionInfo; |
134 | FromNaviCtrl_t fromNaviCtrl; |
| 138 | 135 | SPI_VersionInfo_t SPI_VersionInfo; |
|
| 139 | 136 | ||
| 140 | // rx packet buffer |
137 | // rx packet buffer |
| 141 | #define SPI_RXBUFFER_LEN sizeof(fromNaviCtrl) |
138 | #define SPI_RXBUFFER_LEN sizeof(fromNaviCtrl) |
| Line 151... | Line 148... | ||
| 151 | uint8_t SPITransferCompleted, SPI_ChkSum; |
148 | uint8_t SPITransferCompleted, SPI_ChkSum; |
| 152 | uint8_t SPI_RxDataValid = 0; |
149 | uint8_t SPI_RxDataValid = 0; |
| 153 | uint8_t NCDataOkay = 0; |
150 | uint8_t NCDataOkay = 0; |
| 154 | uint8_t NCSerialDataOkay = 0; |
151 | uint8_t NCSerialDataOkay = 0; |
| Line -... | Line 152... | ||
| - | 152 | ||
| 155 | 153 | uint8_t SPI_CommandSequence[] = { SPI_CMD_USER, SPI_CMD_STICK, |
|
| 156 | uint8_t SPI_CommandSequence[] = { SPI_CMD_USER, SPI_CMD_STICK, SPI_CMD_PARAMETER1, SPI_CMD_STICK, SPI_CMD_MISC, SPI_CMD_VERSION }; |
154 | SPI_CMD_PARAMETER1, SPI_CMD_STICK, SPI_CMD_MISC, SPI_CMD_VERSION }; |
| Line 157... | Line 155... | ||
| 157 | uint8_t SPI_CommandCounter = 0; |
155 | uint8_t SPI_CommandCounter = 0; |
| 158 | 156 | ||
| 159 | /*********************************************/ |
157 | /*********************************************/ |
| Line 192... | Line 190... | ||
| 192 | /* Update Data transferd by the SPI from/to NaviCtrl */ |
190 | /* Update Data transferd by the SPI from/to NaviCtrl */ |
| 193 | /**********************************************************/ |
191 | /**********************************************************/ |
| 194 | void UpdateSPI_Buffer(void) { |
192 | void UpdateSPI_Buffer(void) { |
| 195 | uint8_t i; |
193 | uint8_t i; |
| 196 | int16_t tmp; |
194 | int16_t tmp; |
| - | 195 | cli(); |
|
| 197 | cli(); // stop all interrupts to avoid writing of new data during update of that packet. |
196 | // stop all interrupts to avoid writing of new data during update of that packet. |
| Line 198... | Line 197... | ||
| 198 | 197 | ||
| - | 198 | // update content of packet to NaviCtrl |
|
| 199 | // update content of packet to NaviCtrl |
199 | toNaviCtrl.IntegralPitch = (int16_t) ((10 * angle[PITCH]) |
| - | 200 | / 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° |
201 | toNaviCtrl.IntegralRoll = (int16_t) ((10 * angle[ROLL]) |
| - | 202 | / 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° |
203 | toNaviCtrl.GyroHeading = (int16_t) ((10 * yawGyroHeading) |
| 202 | toNaviCtrl.GyroHeading = (int16_t)((10 * yawGyroHeading) / GYRO_DEG_FACTOR_YAW); // convert to multiple of 0.1° |
204 | / GYRO_DEG_FACTOR_YAW); // convert to multiple of 0.1° |
| 203 | toNaviCtrl.GyroPitch = rate_ATT[PITCH]; |
205 | toNaviCtrl.GyroPitch = rate_ATT[PITCH]; |
| 204 | toNaviCtrl.GyroRoll = rate_ATT[ROLL]; |
206 | toNaviCtrl.GyroRoll = rate_ATT[ROLL]; |
| - | 207 | toNaviCtrl.GyroYaw = yawRate; |
|
| 205 | toNaviCtrl.GyroYaw = yawRate; |
208 | toNaviCtrl.AccPitch = (10 * getAngleEstimateFromAcc(PITCH)) |
| - | 209 | / GYRO_DEG_FACTOR_PITCHROLL; // convert to multiple of 0.1° |
|
| 206 | toNaviCtrl.AccPitch = (10 * getAngleEstimateFromAcc(PITCH)) / GYRO_DEG_FACTOR_PITCHROLL; // convert to multiple of 0.1° |
210 | toNaviCtrl.AccRoll = (10 * getAngleEstimateFromAcc(ROLL)) |
| Line 207... | Line 211... | ||
| 207 | toNaviCtrl.AccRoll = (10 * getAngleEstimateFromAcc(ROLL)) / GYRO_DEG_FACTOR_PITCHROLL; // convert to multiple of 0.1° |
211 | / GYRO_DEG_FACTOR_PITCHROLL; // convert to multiple of 0.1° |
| Line 208... | Line 212... | ||
| 208 | 212 | ||
| Line 237... | Line 241... | ||
| 237 | toNaviCtrl.Param.Byte[10] = staticParams.NaviSpeedCompensation; |
241 | toNaviCtrl.Param.Byte[10] = staticParams.NaviSpeedCompensation; |
| 238 | toNaviCtrl.Param.Byte[11] = staticParams.NaviAngleLimitation; |
242 | toNaviCtrl.Param.Byte[11] = staticParams.NaviAngleLimitation; |
| 239 | break; |
243 | break; |
| Line 240... | Line 244... | ||
| 240 | 244 | ||
| 241 | case SPI_CMD_STICK: |
245 | case SPI_CMD_STICK: |
| - | 246 | tmp = PPM_in[staticParams.ChannelAssignment[CH_THROTTLE]]; |
|
| - | 247 | if (tmp > 127) |
|
| - | 248 | tmp = 127; |
|
| - | 249 | else if (tmp < -128) |
|
| 242 | tmp = PPM_in[staticParams.ChannelAssignment[CH_THROTTLE]]; if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128; |
250 | tmp = -128; |
| 243 | toNaviCtrl.Param.Byte[0] = (int8_t) tmp; |
251 | toNaviCtrl.Param.Byte[0] = (int8_t) tmp; |
| - | 252 | tmp = PPM_in[staticParams.ChannelAssignment[CH_YAW]]; |
|
| - | 253 | if (tmp > 127) |
|
| - | 254 | tmp = 127; |
|
| - | 255 | else if (tmp < -128) |
|
| 244 | tmp = PPM_in[staticParams.ChannelAssignment[CH_YAW]]; if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128; |
256 | tmp = -128; |
| 245 | toNaviCtrl.Param.Byte[1] = (int8_t) tmp; |
257 | toNaviCtrl.Param.Byte[1] = (int8_t) tmp; |
| - | 258 | tmp = PPM_in[staticParams.ChannelAssignment[CH_ROLL]]; |
|
| - | 259 | if (tmp > 127) |
|
| - | 260 | tmp = 127; |
|
| - | 261 | else if (tmp < -128) |
|
| 246 | tmp = PPM_in[staticParams.ChannelAssignment[CH_ROLL]]; if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128; |
262 | tmp = -128; |
| 247 | toNaviCtrl.Param.Byte[2] = (int8_t) tmp; |
263 | toNaviCtrl.Param.Byte[2] = (int8_t) tmp; |
| - | 264 | tmp = PPM_in[staticParams.ChannelAssignment[CH_PITCH]]; |
|
| - | 265 | if (tmp > 127) |
|
| - | 266 | tmp = 127; |
|
| - | 267 | else if (tmp < -128) |
|
| 248 | tmp = PPM_in[staticParams.ChannelAssignment[CH_PITCH]]; if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128; |
268 | tmp = -128; |
| 249 | toNaviCtrl.Param.Byte[3] = (int8_t) tmp; |
269 | toNaviCtrl.Param.Byte[3] = (int8_t) tmp; |
| 250 | toNaviCtrl.Param.Byte[4] = (uint8_t) variables[0]; |
270 | toNaviCtrl.Param.Byte[4] = (uint8_t) variables[0]; |
| 251 | toNaviCtrl.Param.Byte[5] = (uint8_t) variables[1]; |
271 | toNaviCtrl.Param.Byte[5] = (uint8_t) variables[1]; |
| 252 | toNaviCtrl.Param.Byte[6] = (uint8_t) variables[2]; |
272 | toNaviCtrl.Param.Byte[6] = (uint8_t) variables[2]; |
| Line 276... | Line 296... | ||
| 276 | break; |
296 | break; |
| 277 | default: |
297 | default: |
| 278 | break; |
298 | break; |
| 279 | } |
299 | } |
| Line -... | Line 300... | ||
| - | 300 | ||
| 280 | 301 | sei(); |
|
| Line 281... | Line 302... | ||
| 281 | sei(); // enable all interrupts |
302 | // enable all interrupts |
| 282 | 303 | ||
| 283 | // analyze content of packet from NaviCtrl if valid |
304 | // analyze content of packet from NaviCtrl if valid |
| 284 | if (SPI_RxDataValid) { |
305 | if (SPI_RxDataValid) { |
| - | 306 | // update gps controls |
|
| 285 | // update gps controls |
307 | if (abs(fromNaviCtrl.GPSStickPitch) < 512 && abs(fromNaviCtrl.GPSStickRoll) |
| 286 | if(abs(fromNaviCtrl.GPSStickPitch) < 512 && abs(fromNaviCtrl.GPSStickRoll) < 512 && (staticParams.GlobalConfig & CFG_GPS_ACTIVE)) { |
308 | < 512 && (staticParams.GlobalConfig & CFG_GPS_ACTIVE)) { |
| 287 | GPSStickPitch = fromNaviCtrl.GPSStickPitch; |
309 | GPSStickPitch = fromNaviCtrl.GPSStickPitch; |
| 288 | GPSStickRoll = fromNaviCtrl.GPSStickRoll; |
310 | GPSStickRoll = fromNaviCtrl.GPSStickRoll; |
| 289 | NCDataOkay = 250; |
311 | NCDataOkay = 250; |
| Line 293... | Line 315... | ||
| 293 | compassHeading = fromNaviCtrl.CompassHeading; |
315 | compassHeading = fromNaviCtrl.CompassHeading; |
| 294 | } |
316 | } |
| 295 | //if(compassHeading < 0) compassOffCourse = 0; |
317 | //if(compassHeading < 0) compassOffCourse = 0; |
| 296 | //else compassOffCourse = ((540 + compassHeading - compassCourse) % 360) - 180; |
318 | //else compassOffCourse = ((540 + compassHeading - compassCourse) % 360) - 180; |
| 297 | // NaviCtrl wants to beep? |
319 | // NaviCtrl wants to beep? |
| 298 | if (fromNaviCtrl.BeepTime > BeepTime && !compassCalState) BeepTime = fromNaviCtrl.BeepTime; |
320 | if (fromNaviCtrl.BeepTime > BeepTime && !compassCalState) |
| - | 321 | BeepTime = fromNaviCtrl.BeepTime; |
|
| Line 299... | Line 322... | ||
| 299 | 322 | ||
| 300 | switch (fromNaviCtrl.Command) { |
323 | switch (fromNaviCtrl.Command) { |
| 301 | case SPI_KALMAN: |
324 | case SPI_KALMAN: |
| 302 | dynamicParams.KalmanK = fromNaviCtrl.Param.Byte[0]; |
325 | dynamicParams.KalmanK = fromNaviCtrl.Param.Byte[0]; |
| Line 317... | Line 340... | ||
| 317 | 340 | ||
| 318 | /*********************************************/ |
341 | /*********************************************/ |
| 319 | /* Start Transmission of packet to NaviCtrl */ |
342 | /* Start Transmission of packet to NaviCtrl */ |
| 320 | /*********************************************/ |
343 | /*********************************************/ |
| - | 344 | void SPI_StartTransmitPacket(void) { |
|
| 321 | void SPI_StartTransmitPacket(void){ |
345 | if (!SPITransferCompleted) |
| 322 | if (!SPITransferCompleted) return; // return immediately if transfer is in progress |
346 | return; // return immediately if transfer is in progress |
| 323 | else // transmission was completed |
347 | else // transmission was completed |
| 324 | { |
348 | { |
| Line 325... | Line 349... | ||
| 325 | SLAVE_SELECT_PORT &= ~(1 << SPI_SLAVE_SELECT); // Select slave |
349 | SLAVE_SELECT_PORT &= ~(1 << SPI_SLAVE_SELECT); // Select slave |
| 326 | 350 | ||
| 327 | // cyclic commands |
351 | // cyclic commands |
| - | 352 | toNaviCtrl.Command = SPI_CommandSequence[SPI_CommandCounter++]; |
|
| Line 328... | Line 353... | ||
| 328 | toNaviCtrl.Command = SPI_CommandSequence[SPI_CommandCounter++]; |
353 | if (SPI_CommandCounter >= sizeof(SPI_CommandSequence)) |
| 329 | if (SPI_CommandCounter >= sizeof(SPI_CommandSequence)) SPI_CommandCounter = 0; |
354 | SPI_CommandCounter = 0; |
| Line 330... | Line 355... | ||
| 330 | 355 | ||
| Line 331... | Line 356... | ||
| 331 | SPITransferCompleted = 0; // transfer is in progress |
356 | SPITransferCompleted = 0; // transfer is in progress |
| 332 | UpdateSPI_Buffer(); // update data in toNaviCtrl |
357 | UpdateSPI_Buffer(); // update data in toNaviCtrl |
| - | 358 | ||
| - | 359 | SPI_TxBufferIndex = 1; //proceed with 2nd byte |
|
| - | 360 | ||
| - | 361 | // -- Debug-Output --- |
|
| - | 362 | //---- |
|
| - | 363 | asm volatile ("nop"); |
|
| - | 364 | asm volatile ("nop"); |
|
| - | 365 | asm volatile ("nop"); |
|
| - | 366 | asm volatile ("nop"); |
|
| 333 | 367 | asm volatile ("nop"); |
|
| - | 368 | asm volatile ("nop"); |
|
| - | 369 | asm volatile ("nop"); |
|
| - | 370 | asm volatile ("nop"); |
|
| - | 371 | asm volatile ("nop"); |
|
| - | 372 | asm volatile ("nop"); |
|
| - | 373 | asm volatile ("nop"); |
|
| - | 374 | asm volatile ("nop"); |
|
| - | 375 | asm volatile ("nop"); |
|
| - | 376 | asm volatile ("nop"); |
|
| - | 377 | asm volatile ("nop"); |
|
| - | 378 | asm volatile ("nop"); |
|
| 334 | SPI_TxBufferIndex = 1; //proceed with 2nd byte |
379 | asm volatile ("nop"); |
| - | 380 | asm volatile ("nop"); |
|
| 335 | 381 | asm volatile ("nop"); |
|
| 336 | // -- Debug-Output --- |
382 | asm volatile ("nop"); |
| 337 | //---- |
383 | 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"); |
384 | 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"); |
385 | asm volatile ("nop"); |
| Line 351... | Line 397... | ||
| 351 | void SPI_TransmitByte(void) { |
397 | void SPI_TransmitByte(void) { |
| 352 | static SPI_RXState_t SPI_RXState = SPI_SYNC1; |
398 | static SPI_RXState_t SPI_RXState = SPI_SYNC1; |
| 353 | uint8_t rxdata; |
399 | uint8_t rxdata; |
| 354 | static uint8_t rxchksum; |
400 | static uint8_t rxchksum; |
| Line -... | Line 401... | ||
| - | 401 | ||
| 355 | 402 | if (SPITransferCompleted) |
|
| - | 403 | return; // return immediatly if transfer was completed |
|
| 356 | if (SPITransferCompleted) return; // return immediatly if transfer was completed |
404 | if (!(SPSR & (1 << SPIF))) |
| 357 | if (!(SPSR & (1 << SPIF))) return; // return if no SPI-IRQ pending |
405 | return; // return if no SPI-IRQ pending |
| Line 358... | Line 406... | ||
| 358 | SendSPI = 4; // mait 4 * 0.102 ms for the next call of SPI_TransmitByte() in the main loop |
406 | SendSPI = 4; // mait 4 * 0.102 ms for the next call of SPI_TransmitByte() in the main loop |
| Line 359... | Line 407... | ||
| 359 | 407 | ||
| Line 360... | Line 408... | ||
| 360 | SLAVE_SELECT_PORT |= (1 << SPI_SLAVE_SELECT); // DeselectSlave |
408 | SLAVE_SELECT_PORT |= (1 << SPI_SLAVE_SELECT); // DeselectSlave |
| 361 | 409 | ||
| 362 | rxdata = SPDR; // save spi data register |
410 | rxdata = SPDR; // save spi data register |
| 363 | 411 | ||
| 364 | switch (SPI_RXState) { |
412 | switch (SPI_RXState) { |
| 365 | case SPI_SYNC1: // first sync byte |
- | |
| 366 | SPI_RxBufferIndex = 0; // set pointer to start of rx buffer |
413 | case SPI_SYNC1: // first sync byte |
| 367 | rxchksum = rxdata; // initialize checksum |
414 | SPI_RxBufferIndex = 0; // set pointer to start of rx buffer |
| 368 | if (rxdata == SPI_RXSYNCBYTE1 ) |
415 | rxchksum = rxdata; // initialize checksum |
| Line 369... | Line 416... | ||
| 369 | { // 1st Syncbyte found |
416 | if (rxdata == SPI_RXSYNCBYTE1) { // 1st Syncbyte found |
| 370 | SPI_RXState = SPI_SYNC2; // trigger to state for second sync byte |
417 | SPI_RXState = SPI_SYNC2; // trigger to state for second sync byte |
| 371 | } |
- | |
| 372 | break; |
418 | } |
| 373 | 419 | break; |
|
| 374 | case SPI_SYNC2: // second sync byte |
- | |
| 375 | if (rxdata == SPI_RXSYNCBYTE2) |
420 | |
| 376 | { // 2nd Syncbyte found |
421 | case SPI_SYNC2: // second sync byte |
| 377 | rxchksum += rxdata; // update checksum |
422 | if (rxdata == SPI_RXSYNCBYTE2) { // 2nd Syncbyte found |
| 378 | SPI_RXState = SPI_DATA; // trigger to state for second sync byte |
423 | rxchksum += rxdata; // update checksum |
| 379 | } |
424 | SPI_RXState = SPI_DATA; // trigger to state for second sync byte |
| Line 405... | Line 450... | ||
| 405 | }// eof switch(SPI_RXState) |
450 | }// eof switch(SPI_RXState) |
| Line 406... | Line 451... | ||
| 406 | 451 | ||
| 407 | // if still some bytes left for transmission to NaviCtrl |
452 | // if still some bytes left for transmission to NaviCtrl |
| 408 | if (SPI_TxBufferIndex < SPI_TXBUFFER_LEN) { |
453 | if (SPI_TxBufferIndex < SPI_TXBUFFER_LEN) { |
| - | 454 | SLAVE_SELECT_PORT &= ~(1 << SPI_SLAVE_SELECT); // SelectSlave |
|
| - | 455 | asm volatile ("nop"); |
|
| - | 456 | asm volatile ("nop"); |
|
| - | 457 | asm volatile ("nop"); |
|
| - | 458 | asm volatile ("nop"); |
|
| - | 459 | asm volatile ("nop"); |
|
| - | 460 | asm volatile ("nop"); |
|
| 409 | SLAVE_SELECT_PORT &= ~(1 << SPI_SLAVE_SELECT); // SelectSlave |
461 | asm volatile ("nop"); |
| - | 462 | asm volatile ("nop"); |
|
| - | 463 | asm volatile ("nop"); |
|
| - | 464 | asm volatile ("nop"); |
|
| - | 465 | asm volatile ("nop"); |
|
| - | 466 | asm volatile ("nop"); |
|
| - | 467 | asm volatile ("nop"); |
|
| - | 468 | asm volatile ("nop"); |
|
| - | 469 | asm volatile ("nop"); |
|
| - | 470 | asm volatile ("nop"); |
|
| - | 471 | asm volatile ("nop"); |
|
| - | 472 | asm volatile ("nop"); |
|
| - | 473 | asm volatile ("nop"); |
|
| - | 474 | 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"); |
475 | 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"); |
476 | asm volatile ("nop"); |
| - | 477 | asm volatile ("nop"); |
|
| Line 412... | Line 478... | ||
| 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"); |
478 | asm volatile ("nop"); |
| 413 | 479 | ||
| 414 | SPDR = SPI_TxBuffer[SPI_TxBufferIndex]; // transmit byte |
480 | SPDR = SPI_TxBuffer[SPI_TxBufferIndex]; // transmit byte |
| 415 | toNaviCtrl.Chksum += SPI_TxBuffer[SPI_TxBufferIndex]; // update checksum for everey byte that was sent |
481 | toNaviCtrl.Chksum += SPI_TxBuffer[SPI_TxBufferIndex]; // update checksum for everey byte that was sent |