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 |