Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 1820 → Rev 1821

/branches/dongfang_FC_rewrite/spi.c
126,27 → 126,24
#define SPI_RXSYNCBYTE2 0x55
 
typedef enum {
SPI_SYNC1,
SPI_SYNC2,
SPI_DATA
SPI_SYNC1, SPI_SYNC2, SPI_DATA
} SPI_RXState_t;
 
// data exchange packets to and From NaviCtrl
ToNaviCtrl_t toNaviCtrl;
FromNaviCtrl_t fromNaviCtrl;
SPI_VersionInfo_t SPI_VersionInfo;
ToNaviCtrl_t toNaviCtrl;
FromNaviCtrl_t fromNaviCtrl;
SPI_VersionInfo_t SPI_VersionInfo;
 
 
// rx packet buffer
#define SPI_RXBUFFER_LEN sizeof(fromNaviCtrl)
uint8_t SPI_RxBuffer[SPI_RXBUFFER_LEN];
uint8_t SPI_RxBufferIndex = 0;
uint8_t SPI_RxBufferIndex = 0;
uint8_t SPI_RxBuffer_Request = 0;
 
// tx packet buffer
#define SPI_TXBUFFER_LEN sizeof(toNaviCtrl)
uint8_t *SPI_TxBuffer;
uint8_t SPI_TxBufferIndex = 0;
uint8_t SPI_TxBufferIndex = 0;
 
uint8_t SPITransferCompleted, SPI_ChkSum;
uint8_t SPI_RxDataValid = 0;
153,7 → 150,8
uint8_t NCDataOkay = 0;
uint8_t NCSerialDataOkay = 0;
 
uint8_t SPI_CommandSequence[] = { SPI_CMD_USER, SPI_CMD_STICK, SPI_CMD_PARAMETER1, SPI_CMD_STICK, SPI_CMD_MISC, SPI_CMD_VERSION };
uint8_t SPI_CommandSequence[] = { SPI_CMD_USER, SPI_CMD_STICK,
SPI_CMD_PARAMETER1, SPI_CMD_STICK, SPI_CMD_MISC, SPI_CMD_VERSION };
uint8_t SPI_CommandCounter = 0;
 
/*********************************************/
160,32 → 158,32
/* Initialize SPI interface to NaviCtrl */
/*********************************************/
void SPI_MasterInit(void) {
DDR_SPI |= (1<<DD_MOSI)|(1<<DD_SCK); // Set MOSI and SCK output, all others input
SLAVE_SELECT_DDR_PORT |= (1 << SPI_SLAVE_SELECT); // set Slave select port as output port
SPCR = (1<<SPE)|(1<<MSTR)|(1<<SPR1)|(0<<SPR0)|(0<<SPIE); // Enable SPI, Master, set clock rate fck/64
SPSR = 0;//(1<<SPI2X);
SLAVE_SELECT_PORT |= (1 << SPI_SLAVE_SELECT); // Deselect Slave
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.Command = SPI_CMD_USER;
toNaviCtrl.IntegralPitch = 0;
toNaviCtrl.IntegralRoll = 0;
NCSerialDataOkay = 0;
NCDataOkay = 0;
SPI_RxDataValid = 0;
SPI_VersionInfo.Major = VERSION_MAJOR;
SPI_VersionInfo.Minor = VERSION_MINOR;
SPI_VersionInfo.Patch = VERSION_PATCH;
SPI_VersionInfo.Compatible = NC_SPI_COMPATIBLE;
DDR_SPI |= (1 << DD_MOSI) | (1 << DD_SCK); // Set MOSI and SCK output, all others input
SLAVE_SELECT_DDR_PORT |= (1 << SPI_SLAVE_SELECT); // set Slave select port as output port
 
SPCR = (1 << SPE) | (1 << MSTR) | (1 << SPR1) | (0 << SPR0) | (0 << SPIE); // Enable SPI, Master, set clock rate fck/64
SPSR = 0;//(1<<SPI2X);
 
SLAVE_SELECT_PORT |= (1 << SPI_SLAVE_SELECT); // Deselect Slave
 
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.Command = SPI_CMD_USER;
toNaviCtrl.IntegralPitch = 0;
toNaviCtrl.IntegralRoll = 0;
NCSerialDataOkay = 0;
NCDataOkay = 0;
 
SPI_RxDataValid = 0;
 
SPI_VersionInfo.Major = VERSION_MAJOR;
SPI_VersionInfo.Minor = VERSION_MINOR;
SPI_VersionInfo.Patch = VERSION_PATCH;
SPI_VersionInfo.Compatible = NC_SPI_COMPATIBLE;
}
 
/**********************************************************/
192,155 → 190,203
/* Update Data transferd by the SPI from/to NaviCtrl */
/**********************************************************/
void UpdateSPI_Buffer(void) {
uint8_t i;
int16_t tmp;
cli(); // stop all interrupts to avoid writing of new data during update of that packet.
// update content of packet to NaviCtrl
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°
// TODO: What are these little bastards?
uint8_t i;
int16_t tmp;
cli();
// stop all interrupts to avoid writing of new data during update of that packet.
 
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[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();
break;
// update content of packet to NaviCtrl
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°
 
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;
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;
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;
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;
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;
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;
// 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;
break;
default:
break;
}
sei(); // enable all interrupts
// analyze content of packet from NaviCtrl if valid
if (SPI_RxDataValid) {
// update gps controls
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(compassHeading < 0) compassOffCourse = 0;
//else compassOffCourse = ((540 + compassHeading - compassCourse) % 360) - 180;
// NaviCtrl wants to beep?
if (fromNaviCtrl.BeepTime > BeepTime && !compassCalState) BeepTime = fromNaviCtrl.BeepTime;
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];
break;
default:
break;
}
} else { // no valid data from NaviCtrl
// disable GPS control
GPSStickPitch = 0;
GPSStickRoll = 0;
}
// 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[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();
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;
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;
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;
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;
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;
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;
// 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;
break;
default:
break;
}
 
sei();
// enable all interrupts
 
// analyze content of packet from NaviCtrl if valid
if (SPI_RxDataValid) {
// update gps controls
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(compassHeading < 0) compassOffCourse = 0;
//else compassOffCourse = ((540 + compassHeading - compassCourse) % 360) - 180;
// NaviCtrl wants to beep?
if (fromNaviCtrl.BeepTime > BeepTime && !compassCalState)
BeepTime = fromNaviCtrl.BeepTime;
 
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];
break;
 
default:
break;
}
} else { // no valid data from NaviCtrl
// disable GPS control
GPSStickPitch = 0;
GPSStickRoll = 0;
}
}
 
/*********************************************/
/* Start Transmission of packet to NaviCtrl */
/*********************************************/
void SPI_StartTransmitPacket(void){
if (!SPITransferCompleted) return; // return immediately if transfer is in progress
else // transmission was completed
{
SLAVE_SELECT_PORT &= ~(1 << SPI_SLAVE_SELECT); // Select slave
// cyclic commands
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
SPI_TxBufferIndex = 1; //proceed with 2nd byte
// -- Debug-Output ---
//----
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
}
void SPI_StartTransmitPacket(void) {
if (!SPITransferCompleted)
return; // return immediately if transfer is in progress
else // transmission was completed
{
SLAVE_SELECT_PORT &= ~(1 << SPI_SLAVE_SELECT); // Select slave
 
// cyclic commands
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
 
SPI_TxBufferIndex = 1; //proceed with 2nd byte
 
// -- Debug-Output ---
//----
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
}
}
 
//------------------------------------------------------
349,73 → 395,93
// the NaviCtrl and one byte of the packet from the NaviCtrl is possible transfered
 
void SPI_TransmitByte(void) {
static SPI_RXState_t SPI_RXState = SPI_SYNC1;
uint8_t rxdata;
static uint8_t rxchksum;
if (SPITransferCompleted) return; // return immediatly if transfer was completed
if (!(SPSR & (1 << SPIF))) return; // return if no SPI-IRQ pending
SendSPI = 4; // mait 4 * 0.102 ms for the next call of SPI_TransmitByte() in the main loop
SLAVE_SELECT_PORT |= (1 << SPI_SLAVE_SELECT); // DeselectSlave
rxdata = SPDR; // save spi data register
switch (SPI_RXState) {
case SPI_SYNC1: // first sync byte
SPI_RxBufferIndex = 0; // set pointer to start of rx buffer
rxchksum = rxdata; // initialize checksum
if (rxdata == SPI_RXSYNCBYTE1 )
{ // 1st Syncbyte found
SPI_RXState = SPI_SYNC2; // trigger to state for second sync byte
}
break;
case SPI_SYNC2: // second sync byte
if (rxdata == SPI_RXSYNCBYTE2)
{ // 2nd Syncbyte found
rxchksum += rxdata; // update checksum
SPI_RXState = SPI_DATA; // trigger to state for second sync byte
}
else // 2nd Syncbyte not found
{
SPI_RXState = SPI_SYNC1; // jump back to 1st sync byte
}
break;
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?
// copy SPI_RxBuffer -> FromFlightCtrl
uint8_t *ptr = (uint8_t *)&fromNaviCtrl;
cli();
memcpy(ptr, (uint8_t *) SPI_RxBuffer, sizeof(fromNaviCtrl));
sei();
SPI_RxDataValid = 1;
} 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
}
break;
}// eof switch(SPI_RXState)
// if still some bytes left for transmission to NaviCtrl
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");
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
SPI_TxBufferIndex++;
} else {
//Transfer of all bytes of the packet to NaviCtrl completed
SPITransferCompleted = 1;
}
static SPI_RXState_t SPI_RXState = SPI_SYNC1;
uint8_t rxdata;
static uint8_t rxchksum;
 
if (SPITransferCompleted)
return; // return immediatly if transfer was completed
if (!(SPSR & (1 << SPIF)))
return; // return if no SPI-IRQ pending
SendSPI = 4; // mait 4 * 0.102 ms for the next call of SPI_TransmitByte() in the main loop
 
SLAVE_SELECT_PORT |= (1 << SPI_SLAVE_SELECT); // DeselectSlave
 
rxdata = SPDR; // save spi data register
 
switch (SPI_RXState) {
case SPI_SYNC1: // first sync byte
SPI_RxBufferIndex = 0; // set pointer to start of rx buffer
rxchksum = rxdata; // initialize checksum
if (rxdata == SPI_RXSYNCBYTE1) { // 1st Syncbyte found
SPI_RXState = SPI_SYNC2; // trigger to state for second sync byte
}
break;
 
case SPI_SYNC2: // second sync byte
if (rxdata == SPI_RXSYNCBYTE2) { // 2nd Syncbyte found
rxchksum += rxdata; // update checksum
SPI_RXState = SPI_DATA; // trigger to state for second sync byte
} else // 2nd Syncbyte not found
{
SPI_RXState = SPI_SYNC1; // jump back to 1st sync byte
}
break;
 
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?
// copy SPI_RxBuffer -> FromFlightCtrl
uint8_t *ptr = (uint8_t *) &fromNaviCtrl;
cli();
memcpy(ptr, (uint8_t *) SPI_RxBuffer, sizeof(fromNaviCtrl));
sei();
SPI_RxDataValid = 1;
} 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
}
break;
}// eof switch(SPI_RXState)
 
// if still some bytes left for transmission to NaviCtrl
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");
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
SPI_TxBufferIndex++;
} else {
//Transfer of all bytes of the packet to NaviCtrl completed
SPITransferCompleted = 1;
}
}