| 111,9 → 111,9 |
|---|
| #ifndef WCOL |
| #define WCOL WCOL0 |
| #endif |
| //#ifndef SPI2X |
| #ifndef SPI2X |
| #define SPI2X SPI2X0 |
| //#endif |
| #endif |
| // ------------------------- |
| |
| #define SLAVE_SELECT_DDR_PORT DDRC |
| 125,28 → 125,26 |
|---|
| #define SPI_RXSYNCBYTE1 0x81 |
| #define SPI_RXSYNCBYTE2 0x55 |
| |
| typedef enum |
| { |
| SPI_SYNC1, |
| SPI_SYNC2, |
| SPI_DATA |
| } SPI_RXState_t; |
| typedef enum { |
| SPI_SYNC1, |
| SPI_SYNC2, |
| SPI_DATA |
| } SPI_RXState_t; |
| |
| |
| // data exchange packets to and From NaviCtrl |
| ToNaviCtrl_t ToNaviCtrl; |
| FromNaviCtrl_t FromNaviCtrl; |
| ToNaviCtrl_t toNaviCtrl; |
| FromNaviCtrl_t fromNaviCtrl; |
| SPI_VersionInfo_t SPI_VersionInfo; |
| |
| SPI_VersionInfo_t SPI_VersionInfo; |
| |
| // rx packet buffer |
| #define SPI_RXBUFFER_LEN sizeof(FromNaviCtrl) |
| #define SPI_RXBUFFER_LEN sizeof(fromNaviCtrl) |
| uint8_t SPI_RxBuffer[SPI_RXBUFFER_LEN]; |
| uint8_t SPI_RxBufferIndex = 0; |
| uint8_t SPI_RxBuffer_Request = 0; |
| |
| // tx packet buffer |
| #define SPI_TXBUFFER_LEN sizeof(ToNaviCtrl) |
| #define SPI_TXBUFFER_LEN sizeof(toNaviCtrl) |
| uint8_t *SPI_TxBuffer; |
| uint8_t SPI_TxBufferIndex = 0; |
| |
| 170,15 → 168,15 |
|---|
| |
| SLAVE_SELECT_PORT |= (1 << SPI_SLAVE_SELECT); // Deselect Slave |
| |
| SPI_TxBuffer = (uint8_t *) &ToNaviCtrl; // set pointer to tx-buffer |
| SPI_TxBuffer = (uint8_t *) &toNaviCtrl; // set pointer to tx-buffer |
| SPITransferCompleted = 1; |
| // initialize data packet to NaviControl |
| ToNaviCtrl.Sync1 = SPI_TXSYNCBYTE1; |
| ToNaviCtrl.Sync2 = SPI_TXSYNCBYTE2; |
| toNaviCtrl.Sync1 = SPI_TXSYNCBYTE1; |
| toNaviCtrl.Sync2 = SPI_TXSYNCBYTE2; |
| |
| ToNaviCtrl.Command = SPI_CMD_USER; |
| ToNaviCtrl.IntegralNick = 0; |
| ToNaviCtrl.IntegralRoll = 0; |
| toNaviCtrl.Command = SPI_CMD_USER; |
| toNaviCtrl.IntegralPitch = 0; |
| toNaviCtrl.IntegralRoll = 0; |
| NCSerialDataOkay = 0; |
| NCDataOkay = 0; |
| |
| 190,7 → 188,6 |
|---|
| SPI_VersionInfo.Compatible = NC_SPI_COMPATIBLE; |
| } |
| |
| |
| /**********************************************************/ |
| /* Update Data transferd by the SPI from/to NaviCtrl */ |
| /**********************************************************/ |
| 200,80 → 197,83 |
|---|
| cli(); // stop all interrupts to avoid writing of new data during update of that packet. |
| |
| // update content of packet to NaviCtrl |
| ToNaviCtrl.IntegralNick = (int16_t)((10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1° |
| ToNaviCtrl.IntegralRoll = (int16_t)((10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1° |
| ToNaviCtrl.GyroHeading = (int16_t)((10 * yawGyroHeading) / GYRO_DEG_FACTOR_YAW); // convert to multiple of 0.1° |
| ToNaviCtrl.GyroNick = rate_PID[PITCH]; // TODO: Which one should it be?? |
| ToNaviCtrl.GyroRoll = rate_PID[ROLL]; |
| ToNaviCtrl.GyroYaw = yawRate; |
| ToNaviCtrl.AccNick = 0; // ((int16_t) 10 * ACC_AMPLIFY * (NaviAccNick / NaviCntAcc)) / ACC_DEG_FACTOR; // convert to multiple of 0.1° |
| ToNaviCtrl.AccRoll = 0; // ((int16_t) 10 * ACC_AMPLIFY * (NaviAccRoll / NaviCntAcc)) / ACC_DEG_FACTOR; // convert to multiple of 0.1° |
| // naviCntAcc = 0; naviAccPitch = 0; naviAccRoll = 0; |
| toNaviCtrl.IntegralPitch = (int16_t)((10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1° |
| toNaviCtrl.IntegralRoll = (int16_t)((10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1° |
| toNaviCtrl.GyroHeading = (int16_t)((10 * yawGyroHeading) / GYRO_DEG_FACTOR_YAW); // convert to multiple of 0.1° |
| toNaviCtrl.GyroPitch = rate_ATT[PITCH]; |
| toNaviCtrl.GyroRoll = rate_ATT[ROLL]; |
| toNaviCtrl.GyroYaw = yawRate; |
| toNaviCtrl.AccPitch = (10 * getAngleEstimateFromAcc(PITCH)) / GYRO_DEG_FACTOR_PITCHROLL; // convert to multiple of 0.1° |
| toNaviCtrl.AccRoll = (10 * getAngleEstimateFromAcc(ROLL)) / GYRO_DEG_FACTOR_PITCHROLL; // convert to multiple of 0.1° |
| |
| switch(ToNaviCtrl.Command) { |
| |
| // TODO: What are these little bastards? |
| |
| averageAcc[PITCH] = averageAcc[ROLL] = averageAccCount = 0; |
| |
| switch(toNaviCtrl.Command) { |
| case SPI_CMD_USER: |
| for (i=0; i<sizeof(dynamicParams.UserParams); i++) { |
| ToNaviCtrl.Param.Byte[i] = dynamicParams.UserParams[i]; |
| toNaviCtrl.Param.Byte[i] = dynamicParams.UserParams[i]; |
| } |
| ToNaviCtrl.Param.Byte[8] = MKFlags; |
| toNaviCtrl.Param.Byte[8] = MKFlags; |
| MKFlags &= ~(MKFLAG_CALIBRATE | MKFLAG_START); // calibrate and start are temporal states that are cleared immediately after transmitting |
| ToNaviCtrl.Param.Byte[9] = (uint8_t)UBat; |
| ToNaviCtrl.Param.Byte[10] = staticParams.LowVoltageWarning; |
| ToNaviCtrl.Param.Byte[11] = getActiveParamSet(); |
| toNaviCtrl.Param.Byte[9] = (uint8_t)UBat; |
| toNaviCtrl.Param.Byte[10] = staticParams.LowVoltageWarning; |
| toNaviCtrl.Param.Byte[11] = getActiveParamSet(); |
| break; |
| |
| case SPI_CMD_PARAMETER1: |
| ToNaviCtrl.Param.Byte[0] = staticParams.NaviGpsModeControl; // Parameters for the Naviboard |
| ToNaviCtrl.Param.Byte[1] = staticParams.NaviGpsGain; |
| ToNaviCtrl.Param.Byte[2] = staticParams.NaviGpsP; |
| ToNaviCtrl.Param.Byte[3] = staticParams.NaviGpsI; |
| ToNaviCtrl.Param.Byte[4] = staticParams.NaviGpsD; |
| ToNaviCtrl.Param.Byte[5] = staticParams.NaviGpsACC; |
| ToNaviCtrl.Param.Byte[6] = staticParams.NaviGpsMinSat; |
| ToNaviCtrl.Param.Byte[7] = staticParams.NaviStickThreshold; |
| ToNaviCtrl.Param.Byte[8] = staticParams.NaviOperatingRadius; |
| ToNaviCtrl.Param.Byte[9] = staticParams.NaviWindCorrection; |
| ToNaviCtrl.Param.Byte[10] = staticParams.NaviSpeedCompensation; |
| ToNaviCtrl.Param.Byte[11] = staticParams.NaviAngleLimitation; |
| toNaviCtrl.Param.Byte[0] = staticParams.NaviGpsModeControl; // Parameters for the Naviboard |
| toNaviCtrl.Param.Byte[1] = staticParams.NaviGpsGain; |
| toNaviCtrl.Param.Byte[2] = staticParams.NaviGpsP; |
| toNaviCtrl.Param.Byte[3] = staticParams.NaviGpsI; |
| toNaviCtrl.Param.Byte[4] = staticParams.NaviGpsD; |
| toNaviCtrl.Param.Byte[5] = staticParams.NaviGpsACC; |
| toNaviCtrl.Param.Byte[6] = staticParams.NaviGpsMinSat; |
| toNaviCtrl.Param.Byte[7] = staticParams.NaviStickThreshold; |
| toNaviCtrl.Param.Byte[8] = staticParams.NaviOperatingRadius; |
| toNaviCtrl.Param.Byte[9] = staticParams.NaviWindCorrection; |
| toNaviCtrl.Param.Byte[10] = staticParams.NaviSpeedCompensation; |
| toNaviCtrl.Param.Byte[11] = staticParams.NaviAngleLimitation; |
| break; |
| |
| case SPI_CMD_STICK: |
| tmp = PPM_in[staticParams.ChannelAssignment[CH_THROTTLE]]; if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128; |
| ToNaviCtrl.Param.Byte[0] = (int8_t) tmp; |
| toNaviCtrl.Param.Byte[0] = (int8_t) tmp; |
| tmp = PPM_in[staticParams.ChannelAssignment[CH_YAW]]; if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128; |
| ToNaviCtrl.Param.Byte[1] = (int8_t) tmp; |
| toNaviCtrl.Param.Byte[1] = (int8_t) tmp; |
| tmp = PPM_in[staticParams.ChannelAssignment[CH_ROLL]]; if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128; |
| ToNaviCtrl.Param.Byte[2] = (int8_t) tmp; |
| toNaviCtrl.Param.Byte[2] = (int8_t) tmp; |
| tmp = PPM_in[staticParams.ChannelAssignment[CH_PITCH]]; if(tmp > 127) tmp = 127; else if(tmp < -128) tmp = -128; |
| ToNaviCtrl.Param.Byte[3] = (int8_t) tmp; |
| ToNaviCtrl.Param.Byte[4] = (uint8_t) variables[0]; |
| ToNaviCtrl.Param.Byte[5] = (uint8_t) variables[1]; |
| ToNaviCtrl.Param.Byte[6] = (uint8_t) variables[2]; |
| ToNaviCtrl.Param.Byte[7] = (uint8_t) variables[3]; |
| ToNaviCtrl.Param.Byte[8] = (uint8_t) RC_Quality; |
| toNaviCtrl.Param.Byte[3] = (int8_t) tmp; |
| toNaviCtrl.Param.Byte[4] = (uint8_t) variables[0]; |
| toNaviCtrl.Param.Byte[5] = (uint8_t) variables[1]; |
| toNaviCtrl.Param.Byte[6] = (uint8_t) variables[2]; |
| toNaviCtrl.Param.Byte[7] = (uint8_t) variables[3]; |
| toNaviCtrl.Param.Byte[8] = (uint8_t) RC_Quality; |
| break; |
| |
| case SPI_CMD_MISC: |
| ToNaviCtrl.Param.Byte[0] = compassCalState; |
| if(compassCalState > 4) |
| { // jump from 5 to 0 |
| compassCalState = 0; |
| } |
| ToNaviCtrl.Param.Byte[1] = staticParams.NaviPHLoginTime; |
| ToNaviCtrl.Param.Int[1] = 0; //readingHeight; // at address of Byte 2 and 3 |
| ToNaviCtrl.Param.Byte[4] = staticParams.NaviGpsPLimit; |
| ToNaviCtrl.Param.Byte[5] = staticParams.NaviGpsILimit; |
| ToNaviCtrl.Param.Byte[6] = staticParams.NaviGpsDLimit; |
| toNaviCtrl.Param.Byte[0] = compassCalState; |
| if(compassCalState > 4) { // jump from 5 to 0 |
| compassCalState = 0; |
| } |
| toNaviCtrl.Param.Byte[1] = staticParams.NaviPHLoginTime; |
| // TODO: Height and in the correct scaling... |
| toNaviCtrl.Param.Int[1] = 0; //readingHeight; // at address of Byte 2 and 3 |
| toNaviCtrl.Param.Byte[4] = staticParams.NaviGpsPLimit; |
| toNaviCtrl.Param.Byte[5] = staticParams.NaviGpsILimit; |
| toNaviCtrl.Param.Byte[6] = staticParams.NaviGpsDLimit; |
| break; |
| |
| case SPI_CMD_VERSION: |
| ToNaviCtrl.Param.Byte[0] = SPI_VersionInfo.Major; |
| ToNaviCtrl.Param.Byte[1] = SPI_VersionInfo.Minor; |
| ToNaviCtrl.Param.Byte[2] = SPI_VersionInfo.Patch; |
| ToNaviCtrl.Param.Byte[3] = SPI_VersionInfo.Compatible; |
| ToNaviCtrl.Param.Byte[4] = BoardRelease; |
| toNaviCtrl.Param.Byte[0] = SPI_VersionInfo.Major; |
| toNaviCtrl.Param.Byte[1] = SPI_VersionInfo.Minor; |
| toNaviCtrl.Param.Byte[2] = SPI_VersionInfo.Patch; |
| toNaviCtrl.Param.Byte[3] = SPI_VersionInfo.Compatible; |
| toNaviCtrl.Param.Byte[4] = BoardRelease; |
| break; |
| |
| default: |
| break; |
| } |
| 283,35 → 283,32 |
|---|
| // analyze content of packet from NaviCtrl if valid |
| if (SPI_RxDataValid) { |
| // update gps controls |
| if(abs(FromNaviCtrl.GPSStickNick) < 512 && abs(FromNaviCtrl.GPSStickRoll) < 512 && (staticParams.GlobalConfig & CFG_GPS_ACTIVE)) { |
| GPSStickPitch = FromNaviCtrl.GPSStickNick; |
| GPSStickRoll = FromNaviCtrl.GPSStickRoll; |
| if(abs(fromNaviCtrl.GPSStickPitch) < 512 && abs(fromNaviCtrl.GPSStickRoll) < 512 && (staticParams.GlobalConfig & CFG_GPS_ACTIVE)) { |
| GPSStickPitch = fromNaviCtrl.GPSStickPitch; |
| GPSStickRoll = fromNaviCtrl.GPSStickRoll; |
| NCDataOkay = 250; |
| } |
| // update compass readings |
| if(FromNaviCtrl.CompassHeading <= 360) { |
| compassHeading = FromNaviCtrl.CompassHeading; |
| if(fromNaviCtrl.CompassHeading <= 360) { |
| compassHeading = fromNaviCtrl.CompassHeading; |
| } |
| if(compassHeading < 0) compassOffCourse = 0; |
| else compassOffCourse = ((540 + compassHeading - compassCourse) % 360) - 180; |
| // NaviCtrl wants to beep? |
| if (FromNaviCtrl.BeepTime > BeepTime && !compassCalState) BeepTime = FromNaviCtrl.BeepTime; |
| if (fromNaviCtrl.BeepTime > BeepTime && !compassCalState) BeepTime = fromNaviCtrl.BeepTime; |
| |
| switch (FromNaviCtrl.Command) { |
| switch (fromNaviCtrl.Command) { |
| case SPI_KALMAN: |
| dynamicParams.KalmanK = FromNaviCtrl.Param.Byte[0]; |
| dynamicParams.KalmanMaxFusion = FromNaviCtrl.Param.Byte[1]; |
| dynamicParams.KalmanMaxDrift = FromNaviCtrl.Param.Byte[2]; |
| NCSerialDataOkay = FromNaviCtrl.Param.Byte[3]; |
| //DebugOut.Analog[29] = NCSerialDataOkay; |
| dynamicParams.KalmanK = fromNaviCtrl.Param.Byte[0]; |
| dynamicParams.KalmanMaxFusion = fromNaviCtrl.Param.Byte[1]; |
| dynamicParams.KalmanMaxDrift = fromNaviCtrl.Param.Byte[2]; |
| NCSerialDataOkay = fromNaviCtrl.Param.Byte[3]; |
| break; |
| |
| default: |
| break; |
| } |
| } |
| else // no valid data from NaviCtrl |
| { |
| } else { // no valid data from NaviCtrl |
| // disable GPS control |
| GPSStickPitch = 0; |
| GPSStickRoll = 0; |
| 328,11 → 325,11 |
|---|
| SLAVE_SELECT_PORT &= ~(1 << SPI_SLAVE_SELECT); // Select slave |
| |
| // cyclic commands |
| ToNaviCtrl.Command = SPI_CommandSequence[SPI_CommandCounter++]; |
| toNaviCtrl.Command = SPI_CommandSequence[SPI_CommandCounter++]; |
| if (SPI_CommandCounter >= sizeof(SPI_CommandSequence)) SPI_CommandCounter = 0; |
| |
| SPITransferCompleted = 0; // transfer is in progress |
| UpdateSPI_Buffer(); // update data in ToNaviCtrl |
| UpdateSPI_Buffer(); // update data in toNaviCtrl |
| |
| SPI_TxBufferIndex = 1; //proceed with 2nd byte |
| |
| 341,8 → 338,8 |
|---|
| asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); |
| asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); |
| asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); |
| ToNaviCtrl.Chksum = ToNaviCtrl.Sync1; // init checksum |
| SPDR = ToNaviCtrl.Sync1; // send first byte |
| toNaviCtrl.Chksum = toNaviCtrl.Sync1; // init checksum |
| SPDR = toNaviCtrl.Sync1; // send first byte |
| } |
| } |
| |
| 389,35 → 386,26 |
|---|
| case SPI_DATA: // data bytes |
| SPI_RxBuffer[SPI_RxBufferIndex++] = rxdata; // copy data byte to spi buffer |
| // if all bytes are received of a packet from the NaviCtrl |
| if (SPI_RxBufferIndex >= SPI_RXBUFFER_LEN) |
| { // last byte transfered is the checksum of the packet |
| if (rxdata == rxchksum) // checksum matching? |
| { |
| if (SPI_RxBufferIndex >= SPI_RXBUFFER_LEN) { // last byte transfered is the checksum of the packet |
| if (rxdata == rxchksum) { // checksum matching? |
| // copy SPI_RxBuffer -> FromFlightCtrl |
| uint8_t *ptr = (uint8_t *)&FromNaviCtrl; |
| uint8_t *ptr = (uint8_t *)&fromNaviCtrl; |
| cli(); |
| memcpy(ptr, (uint8_t *) SPI_RxBuffer, sizeof(FromNaviCtrl)); |
| memcpy(ptr, (uint8_t *) SPI_RxBuffer, sizeof(fromNaviCtrl)); |
| sei(); |
| SPI_RxDataValid = 1; |
| //DebugOut.Analog[18]++; |
| } |
| else |
| { // checksum does not match |
| //DebugOut.Analog[17]++; |
| } else { // checksum does not match |
| SPI_RxDataValid = 0; // reset valid flag |
| } |
| SPI_RXState = SPI_SYNC1; // reset state sync |
| } |
| else // not all bytes transfered |
| { |
| rxchksum += rxdata; // update checksum |
| } |
| SPI_RXState = SPI_SYNC1; // reset state sync |
| } else { // not all bytes transfered |
| rxchksum += rxdata; // update checksum |
| } |
| break; |
| }// eof switch(SPI_RXState) |
| |
| // if still some bytes left for transmission to NaviCtrl |
| if (SPI_TxBufferIndex < SPI_TXBUFFER_LEN) |
| { |
| if (SPI_TxBufferIndex < SPI_TXBUFFER_LEN) { |
| SLAVE_SELECT_PORT &= ~(1 << SPI_SLAVE_SELECT); // SelectSlave |
| asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); |
| 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,7 → 412,7 |
|---|
| asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); asm volatile ("nop"); |
| |
| SPDR = SPI_TxBuffer[SPI_TxBufferIndex]; // transmit byte |
| ToNaviCtrl.Chksum += SPI_TxBuffer[SPI_TxBufferIndex]; // update checksum for everey byte that was sent |
| toNaviCtrl.Chksum += SPI_TxBuffer[SPI_TxBufferIndex]; // update checksum for everey byte that was sent |
| SPI_TxBufferIndex++; |
| } else { |
| //Transfer of all bytes of the packet to NaviCtrl completed |