inline sf::Packet buildPacket(TArgs&&... mArgs) { sf::Packet result; appendToPacket(result, TP, FWD(mArgs)...); return result; }
/* This function is usually called by the worker subsystem */ void logRunBlock(void * arg) { struct log_block *blk = arg; struct log_ops *ops = blk->ops; static CRTPPacket pk; unsigned int timestamp; xSemaphoreTake(logLock, portMAX_DELAY); timestamp = ((long long)xTaskGetTickCount())/portTICK_RATE_MS; pk.header = CRTP_HEADER(CRTP_PORT_LOG, LOG_CH); pk.size = 4; pk.data[0] = blk->id; pk.data[1] = timestamp&0x0ff; pk.data[2] = (timestamp>>8)&0x0ff; pk.data[3] = (timestamp>>16)&0x0ff; while (ops) { float variable; int valuei = 0; float valuef = 0; // FPU instructions must run on aligned data. Make sure it is. variable = *(float *)ops->variable; switch(ops->storageType) { case LOG_UINT8: valuei = *(uint8_t *)&variable; break; case LOG_INT8: valuei = *(int8_t *)&variable; break; case LOG_UINT16: valuei = *(uint16_t *)&variable; break; case LOG_INT16: valuei = *(int16_t *)&variable; break; case LOG_UINT32: valuei = *(uint32_t *)&variable; break; case LOG_INT32: valuei = *(int32_t *)&variable; break; case LOG_FLOAT: valuei = *(float *)&variable; break; } if (ops->logType == LOG_FLOAT || ops->logType == LOG_FP16) { if (ops->storageType == LOG_FLOAT) valuef = *(float *)&variable; else valuef = valuei; // Try to append the next item to the packet. If we run out of space, // drop this and subsequent items. if (ops->logType == LOG_FLOAT) { if (!appendToPacket(&pk, &valuef, 4)) break; } else { valuei = single2half(valuef); if (!appendToPacket(&pk, &valuei, 2)) break; } } else //logType is an integer { if (!appendToPacket(&pk, &valuei, typeLength[ops->logType])) break; } ops = ops->next; } xSemaphoreGive(logLock); // Check if the connection is still up, oherwise disable // all the logging and flush all the CRTP queues. if (!crtpIsConnected()) { logReset(); crtpReset(); } else { crtpSendPacket(&pk); } }
inline void appendToPacket(sf::Packet& mPacket, T&& mArg, TArgs&&... mArgs) { appendToPacket(mPacket, FWD(mArg)); appendToPacket(mPacket, FWD(mArgs)...); }