void paramTOCProcess(int command)
{
int ptr = 0;
char * group = "";
int n=0;
switch (command)
{
case CMD_GET_INFO: //Get info packet about the param implementation
ptr = 0;
group = "";
p.header=CRTP_HEADER(CRTP_PORT_PARAM, TOC_CH);
p.size=6;
p.data[0]=CMD_GET_INFO;
p.data[1]=paramsCount;
memcpy(&p.data[2], ¶msCrc, 4);
crtpSendPacket(&p);
break;
case CMD_GET_ITEM: //Get param variable
for (ptr=0; ptr<paramsLen; ptr++) //Ptr points a group
{
if (params[ptr].type & PARAM_GROUP)
{
if (params[ptr].type & PARAM_START)
group = params[ptr].name;
else
group = "";
}
else //Ptr points a variable
{
if (n==p.data[1])
break;
n++;
}
}
if (ptr<paramsLen)
{
p.header=CRTP_HEADER(CRTP_PORT_PARAM, TOC_CH);
p.data[0]=CMD_GET_ITEM;
p.data[1]=n;
p.data[2]=params[ptr].type;
memcpy(p.data+3, group, strlen(group)+1);
memcpy(p.data+3+strlen(group)+1, params[ptr].name, strlen(params[ptr].name)+1);
p.size=3+2+strlen(group)+strlen(params[ptr].name);
crtpSendPacket(&p);
} else {
p.header=CRTP_HEADER(CRTP_PORT_PARAM, TOC_CH);
p.data[0]=CMD_GET_ITEM;
p.size=1;
crtpSendPacket(&p);
}
break;
}
}
void memSettingsProcess(int command)
{
uint8_t memId;
switch (command)
{
case CMD_GET_NBR:
p.header = CRTP_HEADER(CRTP_PORT_MEM, SETTINGS_CH);
p.size = 2;
p.data[0] = CMD_GET_NBR;
p.data[1] = nbrOwMems + NBR_EEPROM;
crtpSendPacket(&p);
break;
case CMD_GET_INFO:
memId = p.data[1];
p.header = CRTP_HEADER(CRTP_PORT_MEM, SETTINGS_CH);
p.size = 2;
p.data[0] = CMD_GET_INFO;
p.data[1] = memId;
// No error code if we fail, just send an empty packet back
if (memId == EEPROM_ID)
{
// Memory type (eeprom)
p.data[2] = MEM_TYPE_EEPROM;
p.size += 1;
// Size of the memory
memSize = EEPROM_SIZE;
memcpy(&p.data[3], &memSize, 4);
p.size += 4;
memcpy(&p.data[7], eepromSerialNum.data, 8);
p.size += 8;
}
else
{
if (owGetinfo(memId - NBR_EEPROM, &serialNbr))
{
// Memory type (1-wire)
p.data[2] = MEM_TYPE_OW;
p.size += 1;
// Size of the memory TODO: Define length type
memSize = OW_MAX_SIZE;
memcpy(&p.data[3], &memSize, 4);
p.size += 4;
memcpy(&p.data[7], serialNbr.data, 8);
p.size += 8;
}
}
crtpSendPacket(&p);
break;
}
}
void createNbrResponse(CRTPPacket* p)
{
p->header = CRTP_HEADER(CRTP_PORT_MEM, MEM_SETTINGS_CH);
p->size = 2;
p->data[0] = MEM_CMD_GET_NBR;
p->data[1] = 0;
}
void memWriteProcess()
{
uint8_t memId = p.data[0];
uint8_t writeLen;
uint32_t memAddr;
uint8_t status = 0;
memcpy(&memAddr, &p.data[1], 4);
writeLen = p.size - 5;
MEM_DEBUG("Packet is MEM WRITE\n");
p.header = CRTP_HEADER(CRTP_PORT_MEM, WRITE_CH);
// Dont' touch the first 5 bytes, they will be the same.
if (memId == EEPROM_ID)
{
if (memAddr + writeLen <= EEPROM_SIZE &&
eepromWriteBuffer(&p.data[5], memAddr, writeLen))
status = 0;
else
status = EIO;
}
else if(memId == LEDMEM_ID)
{
if ((memAddr + writeLen) <= sizeof(ledringmem))
{
memcpy(&(ledringmem[memAddr]), &p.data[5], writeLen);
MEM_DEBUG("LED write addr:%i, led:%i\n", memAddr, writeLen);
}
else
{
MEM_DEBUG("\LED write failed! addr:%i, led:%i\n", memAddr, writeLen);
}
}
else
{
void memReadProcess()
{
uint8_t memId = p.data[0];
uint8_t readLen = p.data[5];
uint32_t memAddr;
uint8_t status = 0;
memcpy(&memAddr, &p.data[1], 4);
MEM_DEBUG("Packet is MEM READ\n");
p.header = CRTP_HEADER(CRTP_PORT_MEM, READ_CH);
// Dont' touch the first 5 bytes, they will be the same.
if (memId == EEPROM_ID)
{
if (memAddr + readLen <= EEPROM_SIZE &&
eepromReadBuffer(&p.data[6], memAddr, readLen))
status = 0;
else
status = EIO;
}
else if (memId == LEDMEM_ID)
{
if (memAddr + readLen <= sizeof(ledringmem) &&
memcpy(&p.data[6], &(ledringmem[memAddr]), readLen))
status = 0;
else
status = EIO;
}
else
{
memId = memId - NBR_STATIC_MEM;
if (memAddr + readLen <= OW_MAX_SIZE &&
owRead(memId, memAddr, readLen, &p.data[6]))
status = 0;
else
status = EIO;
}
#if 0
{
int i;
for (i = 0; i < readLen; i++)
consolePrintf("%X ", p.data[i+6]);
consolePrintf("\nStatus %i\n", status);
}
#endif
p.data[5] = status;
if (status == 0)
p.size = 6 + readLen;
else
p.size = 6;
crtpSendPacket(&p);
}
void sendSensorData(uint64_t* ts, float* roll, float *pitch, float* yaw, float* acc_x, float* acc_y, float* acc_z, float* gyro_x, float* gyro_y, float* gyro_z) {
pk1.header = CRTP_HEADER(CRTP_PORT_SENSORDATA, PACKET1_CH);
pk1.size=28;
pk1.data[0] = *ts;
pk1.data[8] = *roll;
pk1.data[12] = *pitch;
pk1.data[16] = *yaw;
pk1.data[20] = *acc_x;
pk1.data[24] = *acc_y;
crtpSendPacket(&pk1);
pk2.header = CRTP_HEADER(CRTP_PORT_SENSORDATA, PACKET2_CH);
pk2.size=24;
pk2.data[0] = *ts;
pk2.data[8] = *acc_z;
pk2.data[12] = *gyro_x;
pk2.data[16] = *gyro_y;
pk2.data[20] = *gyro_z;
crtpSendPacket(&pk2);
}
void consoleInit()
{
if (isInit)
return;
messageToPrint.size = 0;
messageToPrint.header = CRTP_HEADER(CRTP_PORT_CONSOLE, 0);
vSemaphoreCreateBinary(synch);
isInit = true;
}
void memWriteProcess()
{
uint8_t memId = p.data[0];
uint8_t writeLen;
uint32_t memAddr;
uint8_t status;
memcpy(&memAddr, &p.data[1], 4);
writeLen = p.size - 5;
DEBUG("Packet is MEM WRITE\n");
p.header = CRTP_HEADER(CRTP_PORT_MEM, WRITE_CH);
// Dont' touch the first 5 bytes, they will be the same.
if (memId == EEPROM_ID)
{
if (memAddr + writeLen <= EEPROM_SIZE &&
eepromWriteBuffer(&p.data[5], memAddr, writeLen))
status = 0;
else
status = EIO;
}
else
{
memId = memId - NBR_EEPROM;
if (memAddr + writeLen <= OW_MAX_SIZE &&
owWrite(memId, memAddr, writeLen, &p.data[5]))
status = 0;
else
status = EIO;
}
p.data[5] = status;
p.size = 6;
crtpSendPacket(&p);
}
/* 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.crtp_header = CRTP_HEADER(CRTP_PORT_LOG, LOG_CH);
pk.size = 4;
pk.crtp_data[0] = blk->id;
pk.crtp_data[1] = timestamp&0x0ff;
pk.crtp_data[2] = (timestamp>>8)&0x0ff;
pk.crtp_data[3] = (timestamp>>16)&0x0ff;
while (ops)
{
int valuei = 0;
float valuef = 0;
switch(ops->storageType)
{
case LOG_UINT8:
valuei = *(uint8_t *)ops->variable;
break;
case LOG_INT8:
valuei = *(int8_t *)ops->variable;
break;
case LOG_UINT16:
valuei = *(uint16_t *)ops->variable;
break;
case LOG_INT16:
valuei = *(int16_t *)ops->variable;
break;
case LOG_UINT32:
valuei = *(uint32_t *)ops->variable;
break;
case LOG_INT32:
valuei = *(int32_t *)ops->variable;
break;
case LOG_FLOAT:
valuei = *(float *)ops->variable;
break;
}
if (ops->logType == LOG_FLOAT || ops->logType == LOG_FP16)
{
if (ops->storageType == LOG_FLOAT)
valuef = *(float *)ops->variable;
else
valuef = valuei;
if (ops->logType == LOG_FLOAT)
{
memcpy(&pk.crtp_data[pk.size], &valuef, 4);
pk.size += 4;
}
else
{
valuei = single2half(valuef);
memcpy(&pk.crtp_data[pk.size], &valuei, 2);
pk.size += 2;
}
}
else //logType is an integer
{
memcpy(&pk.crtp_data[pk.size], &valuei, typeLength[ops->logType]);
pk.size += typeLength[ops->logType];
}
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);
}
}
void logTOCProcess(int command)
{
int ptr = 0;
char * group = "plop";
int n=0;
switch (command)
{
case CMD_GET_INFO: //Get info packet about the log implementation
DEBUG("Packet is TOC_GET_INFO\n");
ptr = 0;
group = "";
p.crtp_header=CRTP_HEADER(CRTP_PORT_LOG, TOC_CH);
p.size=8;
p.crtp_data[0]=CMD_GET_INFO;
p.crtp_data[1]=logsCount;
memcpy(&p.crtp_data[2], &logsCrc, 4);
p.crtp_data[6]=LOG_MAX_BLOCKS;
p.crtp_data[7]=LOG_MAX_OPS;
crtpSendPacket(&p);
break;
case CMD_GET_ITEM: //Get log variable
DEBUG("Packet is TOC_GET_ITEM Id: %d\n", p.data[1]);
for (ptr=0; ptr<logsLen; ptr++) //Ptr points a group
{
if (logs[ptr].type & LOG_GROUP)
{
if (logs[ptr].type & LOG_START)
group = logs[ptr].name;
else
group = "";
}
else //Ptr points a variable
{
if (n==p.crtp_data[1])
break;
n++;
}
}
if (ptr<logsLen)
{
DEBUG(" Item is \"%s\":\"%s\"\n", group, logs[ptr].name);
p.crtp_header=CRTP_HEADER(CRTP_PORT_LOG, TOC_CH);
p.crtp_data[0]=CMD_GET_ITEM;
p.crtp_data[1]=n;
p.crtp_data[2]=logs[ptr].type;
memcpy(p.crtp_data+3, group, strlen(group)+1);
memcpy(p.crtp_data+3+strlen(group)+1, logs[ptr].name, strlen(logs[ptr].name)+1);
p.size=3+2+strlen(group)+strlen(logs[ptr].name);
crtpSendPacket(&p);
} else {
DEBUG(" Index out of range!");
p.crtp_header=CRTP_HEADER(CRTP_PORT_LOG, TOC_CH);
p.crtp_data[0]=CMD_GET_ITEM;
p.size=1;
crtpSendPacket(&p);
}
break;
}
}
void paramTOCProcess(int command)
{
int ptr = 0;
char * group = "";
uint16_t n=0;
uint16_t paramId=0;
switch (command)
{
case CMD_GET_INFO: //Get info packet about the param implementation
DEBUG_PRINT("Client uses old param API!\n");
ptr = 0;
group = "";
p.header=CRTP_HEADER(CRTP_PORT_PARAM, TOC_CH);
p.size=6;
p.data[0]=CMD_GET_INFO;
if (paramsCount < 255) {
p.data[1]=paramsCount;
} else {
p.data[1]=255;
}
memcpy(&p.data[2], ¶msCrc, 4);
crtpSendPacket(&p);
break;
case CMD_GET_ITEM: //Get param variable
for (ptr=0; ptr<paramsLen; ptr++) //Ptr points a group
{
if (params[ptr].type & PARAM_GROUP)
{
if (params[ptr].type & PARAM_START)
group = params[ptr].name;
else
group = "";
}
else //Ptr points a variable
{
if (n==p.data[1])
break;
n++;
}
}
if (ptr<paramsLen)
{
p.header=CRTP_HEADER(CRTP_PORT_PARAM, TOC_CH);
p.data[0]=CMD_GET_ITEM;
p.data[1]=n;
p.data[2]=params[ptr].type;
p.size=3+2+strlen(group)+strlen(params[ptr].name);
ASSERT(p.size <= CRTP_MAX_DATA_SIZE); // Too long! The name of the group or the parameter may be too long.
memcpy(p.data+3, group, strlen(group)+1);
memcpy(p.data+3+strlen(group)+1, params[ptr].name, strlen(params[ptr].name)+1);
crtpSendPacket(&p);
} else {
p.header=CRTP_HEADER(CRTP_PORT_PARAM, TOC_CH);
p.data[0]=CMD_GET_ITEM;
p.size=1;
crtpSendPacket(&p);
}
break;
case CMD_GET_INFO_V2: //Get info packet about the param implementation
ptr = 0;
group = "";
p.header=CRTP_HEADER(CRTP_PORT_PARAM, TOC_CH);
p.size=7;
p.data[0]=CMD_GET_INFO_V2;
memcpy(&p.data[1], ¶msCount, 2);
memcpy(&p.data[3], ¶msCrc, 4);
crtpSendPacket(&p);
useV2 = true;
break;
case CMD_GET_ITEM_V2: //Get param variable
memcpy(¶mId, &p.data[1], 2);
for (ptr=0; ptr<paramsLen; ptr++) //Ptr points a group
{
if (params[ptr].type & PARAM_GROUP)
{
if (params[ptr].type & PARAM_START)
group = params[ptr].name;
else
group = "";
}
else //Ptr points a variable
{
if (n==paramId)
break;
n++;
}
}
if (ptr<paramsLen)
{
p.header=CRTP_HEADER(CRTP_PORT_PARAM, TOC_CH);
p.data[0]=CMD_GET_ITEM_V2;
memcpy(&p.data[1], ¶mId, 2);
p.data[3]=params[ptr].type;
p.size=4+2+strlen(group)+strlen(params[ptr].name);
ASSERT(p.size <= CRTP_MAX_DATA_SIZE); // Too long! The name of the group or the parameter may be too long.
memcpy(p.data+4, group, strlen(group)+1);
memcpy(p.data+4+strlen(group)+1, params[ptr].name, strlen(params[ptr].name)+1);
crtpSendPacket(&p);
} else {
p.header=CRTP_HEADER(CRTP_PORT_PARAM, TOC_CH);
p.data[0]=CMD_GET_ITEM_V2;
p.size=1;
crtpSendPacket(&p);
}
break;
}
}
/* 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);
}
}
