rx_spi_received_e frSkyDHandlePacket(uint8_t * const packet, uint8_t * const protocolState)
{
static timeUs_t lastPacketReceivedTime = 0;
static timeUs_t telemetryTimeUs;
rx_spi_received_e ret = RX_SPI_RECEIVED_NONE;
const timeUs_t currentPacketReceivedTime = micros();
switch (*protocolState) {
case STATE_STARTING:
listLength = 47;
initialiseData(0);
*protocolState = STATE_UPDATE;
nextChannel(1);
cc2500Strobe(CC2500_SRX);
break;
case STATE_UPDATE:
lastPacketReceivedTime = currentPacketReceivedTime;
*protocolState = STATE_DATA;
if (rxSpiCheckBindRequested(false)) {
lastPacketReceivedTime = 0;
timeoutUs = 50;
missingPackets = 0;
*protocolState = STATE_INIT;
break;
}
FALLTHROUGH; //!!TODO -check this fall through is correct
// here FS code could be
case STATE_DATA:
if (cc2500getGdo()) {
uint8_t ccLen = cc2500ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
if (ccLen >= 20) {
cc2500ReadFifo(packet, 20);
if (packet[19] & 0x80) {
missingPackets = 0;
timeoutUs = 1;
if (packet[0] == 0x11) {
if ((packet[1] == rxFrSkySpiConfig()->bindTxId[0]) &&
(packet[2] == rxFrSkySpiConfig()->bindTxId[1])) {
rxSpiLedOn();
nextChannel(1);
cc2500setRssiDbm(packet[18]);
#if defined(USE_RX_FRSKY_SPI_TELEMETRY)
if ((packet[3] % 4) == 2) {
telemetryTimeUs = micros();
buildTelemetryFrame(packet);
*protocolState = STATE_TELEMETRY;
} else
#endif
{
cc2500Strobe(CC2500_SRX);
*protocolState = STATE_UPDATE;
}
ret = RX_SPI_RECEIVED_DATA;
lastPacketReceivedTime = currentPacketReceivedTime;
}
}
}
}
}
if (cmpTimeUs(currentPacketReceivedTime, lastPacketReceivedTime) > (timeoutUs * SYNC_DELAY_MAX)) {
#if defined(USE_RX_CC2500_SPI_PA_LNA)
cc2500TxDisable();
#endif
if (timeoutUs == 1) {
#if defined(USE_RX_CC2500_SPI_PA_LNA) && defined(USE_RX_CC2500_SPI_DIVERSITY) // SE4311 chip
if (missingPackets >= 2) {
cc2500switchAntennae();
}
#endif
if (missingPackets > MAX_MISSING_PKT) {
timeoutUs = 50;
setRssiDirect(0, RSSI_SOURCE_RX_PROTOCOL);
}
missingPackets++;
nextChannel(1);
} else {
rxSpiLedToggle();
setRssi(0, RSSI_SOURCE_RX_PROTOCOL);
nextChannel(13);
}
cc2500Strobe(CC2500_SRX);
*protocolState = STATE_UPDATE;
}
break;
#if defined(USE_RX_FRSKY_SPI_TELEMETRY)
case STATE_TELEMETRY:
if (cmpTimeUs(micros(), telemetryTimeUs) >= 1380) {
cc2500Strobe(CC2500_SIDLE);
cc2500SetPower(6);
cc2500Strobe(CC2500_SFRX);
#if defined(USE_RX_CC2500_SPI_PA_LNA)
cc2500TxEnable();
#endif
cc2500Strobe(CC2500_SIDLE);
cc2500WriteFifo(frame, frame[0] + 1);
*protocolState = STATE_DATA;
ret = RX_SPI_RECEIVED_DATA;
lastPacketReceivedTime = currentPacketReceivedTime;
}
break;
#endif
}
return ret;
}
void peano::applications::latticeboltzmann::blocklatticeboltzmann::tests::DensityInterpolationTest::run(){
// vertex data TODO
tarch::la::Vector<TWO_POWER_D, std::bitset<LB_BLOCK_NUMBER_OF_CELLS>* > inner;
tarch::la::Vector<TWO_POWER_D, std::bitset<LB_BLOCK_NUMBER_OF_CELLS>* > boundary;
tarch::la::Vector<TWO_POWER_D, std::bitset<TWO_POWER_D>* > nonLocalStreamingFinished;
tarch::la::Vector<TWO_POWER_D, tarch::la::Vector<LB_BLOCK_NUMBER_OF_CELLS*LB_CURRENT_DIR,double>* > pdf;
tarch::la::Vector<TWO_POWER_D, tarch::la::Vector<LB_BLOCK_NUMBER_OF_CELLS,double>* > density;
tarch::la::Vector<TWO_POWER_D, tarch::la::Vector<LB_BLOCK_NUMBER_OF_CELLS*DIMENSIONS,double>* > velocity;
// parameters for testing all cells
const tarch::la::Vector<2,int> start(LB_BLOCKSIZE/2, 0);
const tarch::la::Vector<2,int> end ( LB_BLOCKSIZE, (LB_BLOCKSIZE/2+(LB_BLOCKSIZE%2)));
int cellNumber = 0;
int vertexNumber = 0;
tarch::la::Vector<DIMENSIONS,int> vertexCoords(0);
tarch::la::Vector<DIMENSIONS,int> cellCoords(0);
// initialise pointers
for (int i = 0; i < TWO_POWER_D; i++){
inner(i) = new std::bitset<LB_BLOCK_NUMBER_OF_CELLS>();
boundary(i) = new std::bitset<LB_BLOCK_NUMBER_OF_CELLS>();
nonLocalStreamingFinished(i) = new std::bitset<TWO_POWER_D>();
pdf(i) = new tarch::la::Vector<LB_BLOCK_NUMBER_OF_CELLS*LB_CURRENT_DIR,double>(0.0);
density(i) = new tarch::la::Vector<LB_BLOCK_NUMBER_OF_CELLS,double>(0.0);
velocity(i) = new tarch::la::Vector<LB_BLOCK_NUMBER_OF_CELLS*DIMENSIONS,double>(0.0);
}
// loop over vertices
#if (DIMENSIONS==3)
for (vertexCoords(2) = 0; vertexCoords(2) < 2; vertexCoords(2)++){
#endif
for (vertexCoords(1) = 0; vertexCoords(1) < 2; vertexCoords(1)++){
for (vertexCoords(0) = 0; vertexCoords(0) < 2; vertexCoords(0)++){
#if (DIMENSIONS==3)
for (cellCoords(2) = start(vertexCoords(2)); cellCoords(2) < end(vertexCoords(2)); cellCoords(2)++){
#endif
for (cellCoords(1) = start(vertexCoords(1)); cellCoords(1) < end(vertexCoords(1)); cellCoords(1)++){
for (cellCoords(0) = start(vertexCoords(0)); cellCoords(0) < end(vertexCoords(0)); cellCoords(0)++){
std::vector<int> elements;
cellNumber = cellCoords(0) + cellCoords(1)*LB_BLOCKSIZE
#if (DIMENSIONS==3)
+ cellCoords(2)*LB_BLOCKSIZE*LB_BLOCKSIZE
#endif
;
// find all element (non-streaming-finished)-combinations and store them in the vector
determineRelevantElementCombinations(cellCoords,vertexCoords,elements);
for (int n = 0; n < tarch::la::aPowI(elements.size(),2); n++){
// initialise data
initialiseData(
cellCoords,cellNumber,vertexCoords,vertexNumber,elements,n,nonLocalStreamingFinished,inner,boundary,pdf,density
);
// carry out interpolation
// first vertexwise
for (int v = 0; v < TWO_POWER_D; v++){
_densityInterpolation.interpolateDensityLocal(*inner(v),*boundary(v),*pdf(v),*density(v),*velocity(v));
}
// second elementwise
_densityInterpolation.interpolateDensity(inner,boundary,nonLocalStreamingFinished,pdf,density,velocity);
// validate result
if (!tarch::la::equals((*density(vertexNumber))(cellNumber),((double) (THREE_POWER_D+1)/2),1e-10) ){
logInfo("run()", "Data for vertex/cell-number " << vertexNumber << "," << cellNumber << ":" << std::endl);
#if (DIMENSIONS==2)
logInfo("run()",
"nonLocalStreamingFinished: " << (*nonLocalStreamingFinished(0))[0] << (*nonLocalStreamingFinished(0))[1] << (*nonLocalStreamingFinished(0))[2] << (*nonLocalStreamingFinished(0))[3] << \
" " << (*nonLocalStreamingFinished(1))[0] << (*nonLocalStreamingFinished(1))[1] << (*nonLocalStreamingFinished(1))[2] << (*nonLocalStreamingFinished(1))[3] << \
" " << (*nonLocalStreamingFinished(2))[0] << (*nonLocalStreamingFinished(2))[1] << (*nonLocalStreamingFinished(2))[2] << (*nonLocalStreamingFinished(2))[3] << \
" " << (*nonLocalStreamingFinished(3))[0] << (*nonLocalStreamingFinished(3))[1] << (*nonLocalStreamingFinished(3))[2] << (*nonLocalStreamingFinished(3))[3]);
#elif (DIMENSIONS==3)
logInfo("run()",
"nonLocalStreamingFinished: " << (*nonLocalStreamingFinished(0))[0] << (*nonLocalStreamingFinished(0))[1] << (*nonLocalStreamingFinished(0))[2] << (*nonLocalStreamingFinished(0))[3] << (*nonLocalStreamingFinished(0))[4] << (*nonLocalStreamingFinished(0))[5] << (*nonLocalStreamingFinished(0))[6] << (*nonLocalStreamingFinished(0))[7] << \
" " << (*nonLocalStreamingFinished(1))[0] << (*nonLocalStreamingFinished(1))[1] << (*nonLocalStreamingFinished(1))[2] << (*nonLocalStreamingFinished(1))[3] << (*nonLocalStreamingFinished(1))[4] << (*nonLocalStreamingFinished(1))[5] << (*nonLocalStreamingFinished(1))[6] << (*nonLocalStreamingFinished(1))[7] << \
" " << (*nonLocalStreamingFinished(2))[0] << (*nonLocalStreamingFinished(2))[1] << (*nonLocalStreamingFinished(2))[2] << (*nonLocalStreamingFinished(2))[3] << (*nonLocalStreamingFinished(2))[4] << (*nonLocalStreamingFinished(2))[5] << (*nonLocalStreamingFinished(2))[6] << (*nonLocalStreamingFinished(2))[7] << \
" " << (*nonLocalStreamingFinished(3))[0] << (*nonLocalStreamingFinished(3))[1] << (*nonLocalStreamingFinished(3))[2] << (*nonLocalStreamingFinished(3))[3] << (*nonLocalStreamingFinished(3))[4] << (*nonLocalStreamingFinished(3))[5] << (*nonLocalStreamingFinished(3))[6] << (*nonLocalStreamingFinished(3))[7]);
#endif
logInfo("run()", "inner/boundary: ");
for (int v = 0; v < TWO_POWER_D; v++){
for (int c = 0; c < LB_BLOCK_NUMBER_OF_CELLS; c++){
tarch::la::Vector<LB_CURRENT_DIR,double> localPdf(0.0);
for (int i = 0; i < LB_CURRENT_DIR; i++){
localPdf(i) = (*pdf(v))(c*LB_CURRENT_DIR+i);
}
logInfo("run()", "Vertex " << v << ", cell " << c << ": " << (*inner(v))[c] << "," << (*boundary(v))[c] << "," << (*density(v))[c] << "," << localPdf);
}
}
} else {
logInfo("run()", "Data for vertex/cell-number " << vertexNumber << "," << cellNumber << ": OK");
}
validateNumericalEquals((*density(vertexNumber))(cellNumber),((double) (THREE_POWER_D+1)/2));
}
logInfo("run()", "Interpolation on Vertex/Cell " << vertexCoords << "/" << cellCoords << ": OK");
} // cellCoords(0)
} // cellCoords(1)
#if (DIMENSIONS==3)
} // cellCoords(2)
#endif
// increment vertex counter
vertexNumber++;
} // vertexCoords(0)
} // vertexCoords(1)
#if (DIMENSIONS==3)
} // vertexCoords(2)
#endif
// delete pointers
for (int i = 0; i < TWO_POWER_D; i++){
delete inner(i);
delete boundary(i);
delete nonLocalStreamingFinished(i);
delete pdf(i);
delete density(i);
delete velocity(i);
}
}
void Projector::initZeros(int current_size)
{
initialiseData(current_size);
data.initZeros();
}
rx_spi_received_e frSkySpiDataReceived(uint8_t *packet)
{
rx_spi_received_e ret = RX_SPI_RECEIVED_NONE;
switch (protocolState) {
case STATE_INIT:
if ((millis() - start_time) > 10) {
initialise();
protocolState = STATE_BIND;
}
break;
case STATE_BIND:
if (rxSpiCheckBindRequested(true) || rxCc2500SpiConfig()->autoBind) {
rxSpiLedOn();
initTuneRx();
protocolState = STATE_BIND_TUNING;
} else {
protocolState = STATE_STARTING;
}
break;
case STATE_BIND_TUNING:
if (tuneRx(packet)) {
initGetBind();
initialiseData(1);
protocolState = STATE_BIND_BINDING1;
}
break;
case STATE_BIND_BINDING1:
if (getBind1(packet)) {
protocolState = STATE_BIND_BINDING2;
}
break;
case STATE_BIND_BINDING2:
if (getBind2(packet)) {
cc2500Strobe(CC2500_SIDLE);
protocolState = STATE_BIND_COMPLETE;
}
break;
case STATE_BIND_COMPLETE:
if (!rxCc2500SpiConfig()->autoBind) {
writeEEPROM();
} else {
uint8_t ctr = 80;
while (ctr--) {
rxSpiLedToggle();
delay(50);
}
}
ret = RX_SPI_RECEIVED_BIND;
protocolState = STATE_STARTING;
break;
default:
ret = handlePacket(packet, &protocolState);
break;
}
return ret;
}
