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; |
} |
} |