void allign(BowlerPacket * Packet,BYTE_FIFO_STORAGE * fifo){
int first = 0;
do
{
FifoReadByteStream(Packet->stream,1,fifo);// peak but dont read yet
if((Packet->use.head.ProtocolRevision != BOWLER_VERSION)){
if(first==0){
b_println("bad first byte. Fifo=",INFO_PRINT); // SPI ISR shits out messages when 0xAA fails to match. making this info.
p_int(calcByteCount(fifo),INFO_PRINT);
print_nnl(" [",INFO_PRINT);
}
first++;
print_nnl(" 0x",INFO_PRINT);
prHEX8(Packet->use.head.ProtocolRevision,INFO_PRINT);
uint8_t b;
if(getNumBytes(fifo)==0)
return;
//StartCritical();
FifoGetByte(fifo,& b);// dropping bad byte
//EndCritical();
}
}while(getNumBytes(fifo)>0 && (Packet->use.head.ProtocolRevision != BOWLER_VERSION));
if(first>0){
b_println("] ##Junked total:",INFO_PRINT);p_int(first,INFO_PRINT);
}
}
void Font::update() {
if(!needs_update) {
return ;
}
// check if we need to update something for the current font entry.
for(std::vector<FontEntry>::iterator it = entries.begin(); it != entries.end(); ++it) {
FontEntry& e = *it;
if(!e.needs_update) {
continue;
}
if(e.align == FEA_RIGHT) {
e.pos[0] = e.right_edge - e.w;
}
e.needs_update = false;
}
// do we need to realloacate our buffer?
glBindVertexArray(vao);
glBindBuffer(GL_ARRAY_BUFFER, vbo);
size_t needed = getNumBytes();
if(needed > allocated_bytes) {
while(allocated_bytes < needed) {
allocated_bytes = std::max<size_t>(allocated_bytes * 2, 256 * 10);
}
glBufferData(GL_ARRAY_BUFFER, allocated_bytes, NULL, GL_DYNAMIC_DRAW); // discard buffer, prevent syncing delay
glVertexAttribPointer(0, 2, GL_FLOAT, GL_FALSE, sizeof(CharVertex), (GLvoid*)offsetof(CharVertex, pos));
glVertexAttribPointer(1, 2, GL_FLOAT, GL_FALSE, sizeof(CharVertex), (GLvoid*)offsetof(CharVertex, tex));
glVertexAttribPointer(2, 4, GL_FLOAT, GL_FALSE, sizeof(CharVertex), (GLvoid*)offsetof(CharVertex, col));
glVertexAttribPointer(3, 4, GL_FLOAT, GL_FALSE, sizeof(CharVertex), (GLvoid*)offsetof(CharVertex, bg));
}
glBufferSubData(GL_ARRAY_BUFFER, 0, needed, getPtr());
needs_update = false;
}
/**
* Return a string with the object information (only the header)
*/
QString UAVObject::toStringBrief()
{
QString sout;
sout.append( QString("%1 (ID: %2, InstID: %3, NumBytes: %4, SInst: %5)\n")
.arg(getName())
.arg(getObjID())
.arg(getInstID())
.arg(getNumBytes())
.arg(isSingleInstance()) );
return sout;
}
// moves over the data into pixels, sets teh plane pointers into this pixels vector and sets the strides for the planes
void YUV420PGrabber::assignFrame(size_t id,
std::vector<uint8_t>& pixels,
uint8_t* planes[],
uint32_t* strides)
{
pixels.assign(getPtr(), getPtr() + getNumBytes());
YUV420PSize size = getSize(id);
planes[0] = &pixels[size.y_offset];
planes[1] = &pixels[size.u_offset];
planes[2] = &pixels[size.v_offset];
strides[0] = size.y_stride;
strides[1] = size.u_stride;
strides[2] = size.v_stride;
}
void dumpVar(VarType *var)
{
unsigned char bytes[MAX_BYTES];
int nBytes;
printf(" ---- name: %s\n", var->name);
convertToBytes(var->value, bytes);
printf(" ---- value: ");
nBytes = getNumBytes(var->dType);
dumpBytes(bytes, nBytes);
if (var->dType == C_INT_PTR) {
printf(" ---- ptee: ");
convertToBytes((int) *((int*)var->value), bytes);
dumpBytes(bytes, MAX_BYTES);
}
}
static void DMA_Thread(void *user)
{
for (;;)
{
if (diagnosticLevel >= 2)
bhmMessage("I", "KinetisDMA", "DMA_Thread waiting...");
bhmWaitEvent(chState.start);
if (diagnosticLevel >= 2)
bhmMessage("I", "KinetisDMA", "DMA_Thread triggered...");
// A channel has requested service.
Uns32 chNum = 0;
dma_tcdT xferTcd;
Uns32 numBytes;
while (! isEmptyQue())
{
// Check to see if we have recieved transfer cancel request.
if (chState.cancel > 0)
{
initRequestQue();
// ECX transfer with error flags.
if (chState.cancel == 1)
{
chReg->ES.bits.ERRCHN = chNum;
chReg->ES.bits.ECX = 1;
chReg->ES.bits.VLD = 1;
}
chState.cancel = 0;
continue;
}
if (popRequest(&chNum) != 0)
{
bhmMessage("W", periphName, "Error: No DMA Channel available on request queue");
break;
}
if (diagnosticLevel >= 3)
bhmMessage("I", periphName, "Servicing channel: %d", chNum);
// Verify Channel TCD Data.
if (! verifyChannel(chNum))
{
if (diagnosticLevel >= 2)
bhmMessage("W", periphName, "Channel configuration error: ES=0x%x", chReg->ES.value);
sendErrorInterrupt(chNum);
setBits(&(chReg->ERR.value), chNum, 1, "INT");
tcdReg[chNum].TCD_CSR.bits.ACTIVE = 0;
tcdReg[chNum].TCD_CSR.bits.DONE = 1;
continue;
}
// Set the apprioriate csr Bits.
tcdReg[chNum].TCD_CSR.bits.DONE = 0;
tcdReg[chNum].TCD_CSR.bits.START = 0;
tcdReg[chNum].TCD_CSR.bits.ACTIVE = 1;
numBytes = getNumBytes(chNum);
memcpy(&xferTcd, &(tcdReg[chNum]), sizeof(dma_tcdT));
switch (tcdReg[chNum].TCD_ATTR.bits.SSIZE)
{
case 0x0: // 8 - bit src.
doTransfer(chNum, &xferTcd, 1, numBytes);
break;
case 0x1: // 16 - bit src.
case 0x4: // 16 - bit burst:
doTransfer(chNum, &xferTcd, 2, numBytes);
break;
case 0x2: // 32 - bit src.
doTransfer(chNum, &xferTcd, 4, numBytes);
break;
default:
break; // should be caught in verify sections.
}
// Set the next address value.
tcdReg[chNum].TCD_SADDR.value = xferTcd.TCD_SADDR.value + xferTcd.TCD_SLAST.value;
tcdReg[chNum].TCD_DADDR.value = xferTcd.TCD_DADDR.value + xferTcd.TCD_DLASTSGA.value;
// TODO: Update CITER/BITER.
tcdReg[chNum].TCD_CSR.bits.ACTIVE = 0;
tcdReg[chNum].TCD_CSR.bits.DONE = 1;
sendDMADoneInterrupt(chNum);
}
}
}
//////////////////////////////////////////////
// Verify that the address for src and destination make sense for the transfer sizes.
// set error bits if errors found.
//////////////////////////////////////////////
static int verifyChannel(Uns32 chNum)
{
int retVal = 1; // Assume no errors.
Uns32 sMod = 0;
Uns32 dMod = 0;
switch (tcdReg[chNum].TCD_ATTR.bits.SSIZE)
{
case 0x0:
sMod = 1;
break;
case 0x1:
case 0x2:
sMod = 2;
break;
case 0x4:
sMod = 4;
break;
default:
chReg->ES.bits.ERRCHN = chNum;
chReg->ES.bits.SAE = 1;
chReg->ES.bits.VLD = 1;
return 0; // Exit on error, all others will show errors too.
}
switch (tcdReg[chNum].TCD_ATTR.bits.DSIZE)
{
case 0x0:
dMod = 1;
break;
case 0x1:
case 0x2:
dMod = 2;
break;
case 0x4:
dMod = 4;
break;
default:
chReg->ES.bits.ERRCHN = chNum;
chReg->ES.bits.DAE = 1;
chReg->ES.bits.VLD = 1;
return 0; // Exit on error, all others will show errors too.
}
if ((tcdReg[chNum].TCD_SADDR.value % sMod) != 0)
{
chReg->ES.bits.SAE = 1;
retVal = 0;
}
if ((tcdReg[chNum].TCD_SOFF.value % sMod) != 0)
{
chReg->ES.bits.SOE = 1;
retVal = 0;
}
if ((tcdReg[chNum].TCD_DADDR.value % dMod) != 0)
{
chReg->ES.bits.DAE = 1;
retVal = 0;
}
if ((tcdReg[chNum].TCD_DOFF.value % dMod) != 0)
{
chReg->ES.bits.DOE = 1;
retVal = 0;
}
Uns32 numBytes = getNumBytes(chNum);
if (((numBytes % sMod) != 0) && ((numBytes % dMod) != 0))
{
chReg->ES.bits.NCE = 1;
retVal = 0;
}
// Set the channel number of the last error if one occurred.
if (retVal == 0)
{
chReg->ES.bits.ERRCHN = chNum;
chReg->ES.bits.VLD = 1;
}
return retVal;
}
boolean _getBowlerPacket(BowlerPacket * Packet,BYTE_FIFO_STORAGE * fifo, boolean debug){
boolean PacketCheck=false;
//uint16_t PacketLegnth=0;
Packet->stream[0]=0;
if (getNumBytes(fifo) == 0 ) {
return false; //Not enough bytes to even be a header, try back later
}
allign(Packet,fifo);
if (getNumBytes(fifo) < ((_BowlerHeaderSize)+4)) {
if(debug){
//b_println("Current num bytes: ",ERROR_PRINT);p_int(getNumBytes(fifo),ERROR_PRINT);
}
return false; //Not enough bytes to even be a header, try back later
}
FifoReadByteStream(Packet->stream,_BowlerHeaderSize,fifo);
PacketCheck=false;
while(PacketCheck==false) {
if( (Packet->use.head.ProtocolRevision != BOWLER_VERSION)
|| (CheckCRC(Packet)==false)
){
if(Packet->use.head.ProtocolRevision != BOWLER_VERSION){
b_println("first",ERROR_PRINT);
}else if(CheckCRC(Packet)==false) {
b_println("crc",ERROR_PRINT);
}
//prHEX8(Packet->use.head.ProtocolRevision,ERROR_PRINT);print_nnl(" Fifo Size=",ERROR_PRINT);p_int(calcByteCount(fifo),ERROR_PRINT);
uint8_t b;
if(getNumBytes(fifo)==0)
return false;
//StartCritical();
getStream(& b,1,fifo);//junk out one
//EndCritical();
FifoReadByteStream(Packet->stream,_BowlerHeaderSize,fifo);
}else{
if(debug){
//b_println("Got header");
}
PacketCheck=true;
}
if (getNumBytes(fifo) < minSize) {
b_println("allign packet",ERROR_PRINT);
allign(Packet,fifo);
return false; //Not enough bytes to even be a header, try back later
}
}
//PacketLegnth = Packet->use.head.DataLegnth;
uint16_t totalLen = GetPacketLegnth(Packet);
// See if all the data has arived for this packet
int32_t num = getNumBytes(fifo);
if (num >=(totalLen) ){
if(debug){
//b_println("**Found packet, ");p_int(totalLen);//print_nnl(" Bytes, pulling out of buffer");
}
//StartCritical();
getStream(Packet->stream,totalLen,fifo);
//EndCritical();
if(CheckDataCRC(Packet)){
return true;
}else{
println_E("Data CRC Failed ");printBowlerPacketDEBUG(Packet,ERROR_PRINT);
}
}
if(debug){
//b_println("Header ready, but data is not. Need: ",INFO_PRINT);p_int(totalLen,INFO_PRINT);print_nnl(" have: ",INFO_PRINT);p_int(num ,INFO_PRINT);
}
return false;
}
void YUV420PGrabber::assignPixels(std::vector<uint8_t>& pixels) {
pixels.assign(getPtr(), getPtr() + getNumBytes());
}
void advancedBowlerExample() {
/**
* User code must implement a few functions needed by the stack internally
* This Platform specific code can be stored in the Platform directory if it is universal for all
* implementations on the given Platform.
* float getMs(void) Returns the current milliseconds since the application started
* StartCritical() Starts a critical protected section of code
* EndCritical() Ends the critical section and returns to normal operation
*/
setPrintLevelInfoPrint();// enable the stack specific printer. If you wish to use this feature putCharDebug(char c) needs to be defined
printf("\r\nStarting Sample Program\r\n");
int pngtmp = GetRPCValue("_png");
if(pngtmp != _PNG) {
printf("\r\nFAIL Expected: ");
prHEX32(_PNG,INFO_PRINT);
printf(" got: ");
prHEX32(pngtmp,INFO_PRINT);
return;
}
/**
* First we are going to put together dummy array of packet data. These are examples of a _png receive and a custom response.
* These would not exist in your implementation but would come in from the physical layer
*/
BowlerPacket myPngPacket;
myPngPacket.use.head.RPC = GetRPCValue("apid");
myPngPacket.use.head.MessageID = 2;// Specify namespace 2 for the collision detect
myPngPacket.use.head.Method = BOWLER_GET;
myPngPacket.use.head.DataLegnth=4;
FixPacket(&myPngPacket);// Set up the stack content
BowlerPacket myNamespacTestPacket;
myNamespacTestPacket.use.head.RPC = GetRPCValue("rtst");
myNamespacTestPacket.use.head.Method = BOWLER_GET;
myNamespacTestPacket.use.data[0] = 37;
myNamespacTestPacket.use.head.DataLegnth=4+1;
FixPacket(&myNamespacTestPacket);// Set up the stack content
/**
* Now we begin to set up the stack features. The first step is to set up the FIFO to receive the data coming in asynchronously
*
*/
BYTE privateRXCom[sizeof(BowlerPacket)];//make the buffer at least big enough to hold one full sized packet
BYTE_FIFO_STORAGE store;//this is the FIFO data storage struct. All interactions with the circular buffer will go through this.
/**
* Next we initialize the buffer
*/
InitByteFifo(&store,// the pointer to the storage struct
privateRXCom,//pointer the the buffer
sizeof(privateRXCom));//the size of the buffer
Bowler_Init();// Start the Bowler stack
/**
* Now we are going to regester what namespaces we implement with the framework
*/
NAMESPACE_LIST * tmp =getBcsPidNamespace();
addNamespaceToList(tmp);
tmp = getBcsTestNamespace();
addNamespaceToList(tmp);
printf("\r\n# of namespaces declared= %i",getNumberOfNamespaces());
int i=0;
for(i=0; i<getNumberOfNamespaces(); i++) {
NAMESPACE_LIST * nsPtr = getNamespaceAtIndex(i);
printf("\r\nNamespace %s at index %i",nsPtr->namespaceString,i);
}
/**
* Now we load the buffer with the the packet that we "Received"
* This step would come in from the physical layer, usually on
* an interrupt on a mcu.
*/
for (i=0; i<GetPacketLegnth(&myPngPacket); i++) {
BYTE err;// this is a stack error storage byte. See Bowler/FIFO.h for response codes
BYTE b= myPngPacket.stream[i];// This would be a new byte from the physical layer
FifoAddByte(&store, b, &err);// This helper function adds the byte to the storage buffer and manages the read write pointers.
}
/**
* Next we load the new namespace packet
*/
i=0;
for (i=0; i<GetPacketLegnth(&myNamespacTestPacket); i++) {
BYTE err;// this is a stack error storage byte. See Bowler/FIFO.h for response codes
BYTE b= myNamespacTestPacket.stream[i];// This would be a new byte from the physical layer
FifoAddByte(&store, b, &err);// This helper function adds the byte to the storage buffer and manages the read write pointers.
}
printf("\r\nData loaded into packet\r\n");
/**
* We have now loaded a packet into the storage struct 'store'
* All the while we can be polling the storage struct for a new packet
*/
BowlerPacket myLocalPacket; // Declare a packet struct to catch the parsed packet from the asynchronous storage buffer
while(getNumBytes(&store)>0) {
while(Bowler_Server_Static(&myLocalPacket,// pointer to the local packet into which to store the parsed packet
&store// storage struct from which the packet will be checked and parsed.
) == FALSE) { // Returns true when a packet is found and pulled out of the buffer
// wait because there is no packet yet
}
/**
* At this point the packet has been parsed and pulled out of the buffer
* The Static server will have also called the call backs to pars the packet, so
* the response should be loaded up to send back
*/
int packetLength = GetPacketLegnth(&myLocalPacket); // helper function to get packet length
printf("\r\nPreparing to send:\r\n");
printPacket(&myLocalPacket, INFO_PRINT);
printf("\r\nSending Packet Data back out: [ ");
for(i=0; i< packetLength; i++) {
//This would be sending to the physical layer. For this example we are just printing out the data
printf(" %i ",myLocalPacket.stream[i]);
}
printf(" ] \r\n");
}
}
qint32 UAVObjectField::unpack(const quint8* dataIn)
{
QMutexLocker locker(obj->getMutex());
// Unpack each element from input buffer
switch (type)
{
case INT8:
memcpy(&data[offset], dataIn, numElements);
break;
case INT16:
for (quint32 index = 0; index < numElements; ++index)
{
qint16 value;
value = qFromLittleEndian<qint16>(&dataIn[numBytesPerElement*index]);
memcpy(&data[offset + numBytesPerElement*index], &value, numBytesPerElement);
}
break;
case INT32:
for (quint32 index = 0; index < numElements; ++index)
{
qint32 value;
value = qFromLittleEndian<qint32>(&dataIn[numBytesPerElement*index]);
memcpy(&data[offset + numBytesPerElement*index], &value, numBytesPerElement);
}
break;
case UINT8:
for (quint32 index = 0; index < numElements; ++index)
{
data[offset + numBytesPerElement*index] = dataIn[numBytesPerElement*index];
}
break;
case UINT16:
for (quint32 index = 0; index < numElements; ++index)
{
quint16 value;
value = qFromLittleEndian<quint16>(&dataIn[numBytesPerElement*index]);
memcpy(&data[offset + numBytesPerElement*index], &value, numBytesPerElement);
}
break;
case UINT32:
for (quint32 index = 0; index < numElements; ++index)
{
quint32 value;
value = qFromLittleEndian<quint32>(&dataIn[numBytesPerElement*index]);
memcpy(&data[offset + numBytesPerElement*index], &value, numBytesPerElement);
}
break;
case FLOAT32:
for (quint32 index = 0; index < numElements; ++index)
{
quint32 value;
value = qFromLittleEndian<quint32>(&dataIn[numBytesPerElement*index]);
memcpy(&data[offset + numBytesPerElement*index], &value, numBytesPerElement);
}
break;
case ENUM:
for (quint32 index = 0; index < numElements; ++index)
{
data[offset + numBytesPerElement*index] = dataIn[numBytesPerElement*index];
}
break;
case STRING:
memcpy(&data[offset], dataIn, numElements);
break;
}
// Done
return getNumBytes();
}
qint32 UAVObjectField::pack(quint8* dataOut)
{
QMutexLocker locker(obj->getMutex());
// Pack each element in output buffer
switch (type)
{
case INT8:
memcpy(dataOut, &data[offset], numElements);
break;
case INT16:
for (quint32 index = 0; index < numElements; ++index)
{
qint16 value;
memcpy(&value, &data[offset + numBytesPerElement*index], numBytesPerElement);
qToLittleEndian<qint16>(value, &dataOut[numBytesPerElement*index]);
}
break;
case INT32:
for (quint32 index = 0; index < numElements; ++index)
{
qint32 value;
memcpy(&value, &data[offset + numBytesPerElement*index], numBytesPerElement);
qToLittleEndian<qint32>(value, &dataOut[numBytesPerElement*index]);
}
break;
case UINT8:
for (quint32 index = 0; index < numElements; ++index)
{
dataOut[numBytesPerElement*index] = data[offset + numBytesPerElement*index];
}
break;
case UINT16:
for (quint32 index = 0; index < numElements; ++index)
{
quint16 value;
memcpy(&value, &data[offset + numBytesPerElement*index], numBytesPerElement);
qToLittleEndian<quint16>(value, &dataOut[numBytesPerElement*index]);
}
break;
case UINT32:
for (quint32 index = 0; index < numElements; ++index)
{
quint32 value;
memcpy(&value, &data[offset + numBytesPerElement*index], numBytesPerElement);
qToLittleEndian<quint32>(value, &dataOut[numBytesPerElement*index]);
}
break;
case FLOAT32:
for (quint32 index = 0; index < numElements; ++index)
{
quint32 value;
memcpy(&value, &data[offset + numBytesPerElement*index], numBytesPerElement);
qToLittleEndian<quint32>(value, &dataOut[numBytesPerElement*index]);
}
break;
case ENUM:
for (quint32 index = 0; index < numElements; ++index)
{
dataOut[numBytesPerElement*index] = data[offset + numBytesPerElement*index];
}
break;
case STRING:
memcpy(dataOut, &data[offset], numElements);
break;
}
// Done
return getNumBytes();
}
