unsigned char checksum(char * packet)
{
unsigned char checksum, packet_len, i;
struct Packet * mypacket = (struct Packet *)packet;
packet_len = getTypeLength(mypacket->data_type) + 3;
for (i = 0; i < packet_len; i++)
checksum += packet[i];
checksum = !checksum + 1;
return checksum;
}
int DeviceInfo(struct Device * dev)
{
int i;
printf("Device information.\n");
printf("Device Type: %02X.\n", dev->type);
printf("Device Number: %02X.\n", dev->number);
printf("Device Data Type: %02X.\n", dev->data_type);
printf("Device Data: ");
for (i = 0; i < getTypeLength(dev->data_type); i++)
{
printf("%02X ", dev->data[i]);
}
printf("\n");
return i;
}
SpGistLeafTuple
spgFormLeafTuple(SpGistState *state, ItemPointer heapPtr, Datum datum)
{
SpGistLeafTuple tup;
unsigned int size = SGLTHDRSZ + getTypeLength(&state->attType, datum);
Assert(size < 0xffff);
tup = palloc0(size);
tup->heapPtr = *heapPtr;
tup->nextOffset = InvalidOffsetNumber;
tup->size = size;
memcpyDatum(SGLTDATAPTR(tup), &state->attType, datum);
return tup;
}
int sendControl(struct Device dev)
{
int data_len = getTypeLength(dev.data_type);
int packet_len = 4 + data_len;
struct Packet * packet = malloc(packet_len);
packet->id = dev.number | dev.type;
packet->cmd = CMD_TYPE_CONTROL;
#if __BYTE_ORDER == __BIG_ENDIAN
packet->data_type = (dev->data_type & 0x0f) | BIG_ENDIAN_BYTE_ORDER;
#else
packet->data_type = DEV_TYPE_MASK(dev.data_type) | LITTLE_ENDIAN_BYTE_ORDER;
#endif
memset(packet->data, 0, data_len);
packet->data[0] = dev.type | dev.number;
// add checksum byte
*(((char *)packet) + packet_len - 1) = checksum((char *)packet);
Serial_SendMultiBytes((unsigned char *) packet, packet_len);
free(packet);
return 0;
}
SpGistInnerTuple
spgFormInnerTuple(SpGistState *state, bool hasPrefix, Datum prefix, int nNodes, IndexTuple *nodes)
{
SpGistInnerTuple tup;
unsigned int size;
int i;
char *ptr;
size = SGITHDRSZ;
if (hasPrefix)
size += getTypeLength(&state->attPrefixType, prefix);
for(i=0; i<nNodes; i++)
size += IndexTupleSize(nodes[i]);
Assert(size < 0xffff);
tup = palloc0(size);
tup->hasPrefix = !!hasPrefix;
tup->nNodes = nNodes;
tup->size = size;
if (tup->hasPrefix)
memcpyDatum(SGITDATAPTR(tup), &state->attPrefixType, prefix);
ptr= (char*)SGITNODEPTR(tup, state);
for(i=0; i<nNodes; i++)
{
IndexTuple node = nodes[i];
memcpy(ptr, node, IndexTupleSize(node));
ptr += IndexTupleSize(node);
}
return tup;
}
void * DevicePolling(void * host_number) // thread
{
unsigned char poll_en = 0;
unsigned int time_poll = 30000;
unsigned char destroy = 0;
int host = (int) host_number;
unsigned char trying_time = 0, dev_disconnect_try_time = 3;
float_struct_t my_float;
if (host < 0 || host >= DEV_HOST_NUMBER)
{
printf("Host number not valid.\r\nIt should be greater or equal zero and lester than %d.\r\n", DEV_HOST_NUMBER);
printf("Thread exiting.\r\n");
pthread_exit(NULL);
}
printf("Thread: %d start with host: %d.\n",
(int)polling_thread[host], host);
while(1)
{
if (pthread_mutex_trylock(&device_control_access) == 0)
{
poll_en = dev_host[host].polling_control.enable;
time_poll = dev_host[host].polling_control.time_poll_ms * 1000;
destroy = dev_host[host].polling_control.destroy;
pthread_mutex_unlock(&device_control_access);
}
else
{
printf("Thread: %d. host: %d. Fail to access device control.\n",
(int)polling_thread[host], host);
usleep(1000);
}
if (destroy)
{
printf("Thread: %d. host: %d. Destroying.\n",
(int)polling_thread[host], host);
pthread_exit(NULL);
}
if (poll_en)
{
//while (pthread_mutex_trylock(&device_control_access) != 0)
//usleep(1000);
if (dev_host[host].type != DEV_UNKNOWN) // already known device type
{
trying_time = 0;
while (pthread_mutex_trylock(&serial_access) != 0)
{
usleep(1000);
trying_time ++;
if (trying_time > 10)
break;
}
if (trying_time > 10)
{
#if DEVICE_DEBUG
printf("Thread: %d. host: %d. Fail to access serial port.\n",
(int)polling_thread[host], host);
#endif
//pthread_mutex_unlock(&device_control_access);
usleep(time_poll);
continue;
}
else
{
RaspiExt_Pin_Hostx_Active(host + 1);
if (queryData(&dev_host[host]))
{
#if DEVICE_DEBUG
printf("Thread: %d. host: %d. Got data from device.\n",
(int)polling_thread[host], host);
DeviceInfo(&dev_host[host]);
#endif
dev_disconnect_try_time = 3;
}
else
{
#if DEVICE_DEBUG
printf("Thread: %d. host: %d. No device here.\n",
(int)polling_thread[host], host);
#endif
if (dev_disconnect_try_time == 0)
{
// TODO: unregister this device
unsigned char reg_id = dev_host[host].number | dev_host[host].type;
printf("Thread: %d. host: %d. Unregister device %X.\n",
(int)polling_thread[host],
host,
reg_id);
UnRegisterID(®_id);
if (IS_MY_THESIS(DEV_TYPE_MASK(dev_host[host].type)))
{
if (dev_host[host].data != NULL)
memset(dev_host[host].data, 0, sizeof(struct ThesisData));
}
else
{
if (dev_host[host].data != NULL)
memset(dev_host[host].data, 0, getTypeLength(dev_host[host].type));
}
dev_host[host].type = DEV_UNKNOWN;
dev_host[host].number = DEV_NUMBER_UNKNOWN;
}
else
{
dev_disconnect_try_time--;
}
}
RaspiExt_Pin_Hostx_Inactive(host + 1);
}
pthread_mutex_unlock(&serial_access);
switch (dev_host[host].type)
{
case DEV_SENSOR_TEMPERATURE:
RaspiExt_LED_Hostx_Config(LED_MODE_TOGGLE, 1000, host + 1);
if (IS_BIG_ENDIAN_BYTE_ORDER(dev_host[host].data_type))
{
my_float.f_byte[0] = dev_host[host].data[3];
my_float.f_byte[1] = dev_host[host].data[2];
my_float.f_byte[2] = dev_host[host].data[1];
my_float.f_byte[3] = dev_host[host].data[0];
}
else
{
my_float.f_byte[0] = dev_host[host].data[0];
my_float.f_byte[1] = dev_host[host].data[1];
my_float.f_byte[2] = dev_host[host].data[2];
my_float.f_byte[3] = dev_host[host].data[3];
}
printf("Thread: %d. host: %d. Temperature: %0.3f.\n",
(int)polling_thread[host],
host,
my_float.f);
// adjust time polling
if (pthread_mutex_trylock(&device_control_access) == 0)
{
dev_host[host].polling_control.time_poll_ms = 50;
}
else
{
printf("Thread: %d. host: %d. Fail to access device control.\n",
(int)polling_thread[host],
host);
}
// save to shared memory
break;
case DEV_SENSOR_ULTRA_SONIC:
RaspiExt_LED_Hostx_Config(LED_MODE_TOGGLE, 100, host + 1);
my_float.f_byte[0] = dev_host[host].data[3];
my_float.f_byte[1] = dev_host[host].data[2];
my_float.f_byte[2] = dev_host[host].data[1];
my_float.f_byte[3] = dev_host[host].data[0];
printf("Thread: %d. host: %d. Distance: %0.3f.\n",
(int)polling_thread[host],
host,
my_float.f);
// put to db
#if DATABASE
//// put this device into database
//if (DB_IsExist_sensors(dev_host[host].number, dev_host[host].type, UltraSonic_name,
//UltraSonic_code, UltraSonic_symbol, 30, 10, UltraSonic_unit) == 0) // not exist
//{
//DB_Record_sensors(dev_host[host].number, dev_host[host].type, UltraSonic_name,
//UltraSonic_code, UltraSonic_symbol, 30, 10, UltraSonic_unit);
//}
//if (DB_IsExist_sensor_types(dev_host[host].type, UltraSonic_name, UltraSonic_description) == 0) // not exist
//{
//DB_Record_sensor_types(dev_host[host].type, UltraSonic_name, UltraSonic_description);
//}
////DB_Record_sensor_values(dev_host[host].type, UltraSonic_name, UltraSonic_description);
DB_Record_sensor_values_short(my_float.f, millis()/1000);
#endif
// adjust time polling
// save to shared memory
break;
case DEV_SENSOR_LIGTH:
RaspiExt_LED_Hostx_Config(LED_MODE_TOGGLE, 50, host + 1);
break;
case DEV_RF:
break;
case DEV_BLUETOOTH:
break;
case DEV_BUZZER:
break;
case DEV_SENSOR_GAS:
RaspiExt_LED_Hostx_Config(LED_MODE_TOGGLE, 50, host + 1);
break;
case DEV_SIM900:
break;
case DEV_MY_THESIS:
RaspiExt_LED_Hostx_Config(LED_MODE_TOGGLE, 1000, host + 1);
break;
default:
#if DEVICE_DEBUG
printf("Thread: %d. host: %d. Unknown device type.\n",
(int)polling_thread[host], host);
#endif
dev_host[host].type = DEV_UNKNOWN;
dev_host[host].number = DEV_NUMBER_UNKNOWN;
break;
}
}
else // unknown device type
{
RaspiExt_LED_Hostx_Config(LED_MODE_OFF, 50, host + 1);
#if DEVICE_DEBUG
printf("Thread: %d. host: %d. Unknown device, identifying.\n",
(int)polling_thread[host], host);
#endif
// query broadcast id to identify what it is
// adjust time polling to 500 ms
time_poll = 500000;
trying_time = 0;
while (pthread_mutex_trylock(&serial_access) != 0)
{
usleep(1000);
trying_time ++;
if (trying_time > 10)
{
break;
}
}
if (trying_time > 10)
{
#if DEVICE_DEBUG
printf("Thread: %d. host: %d. Fail to access serial port.\n",
(int)polling_thread[host], host);
#endif
usleep(time_poll);
continue;
}
else
{
#if DEVICE_DEBUG
//dev_host[host].type = DEV_SENSOR_ULTRA_SONIC;
//dev_host[host].number = 0x01;
#endif
RaspiExt_Pin_Hostx_Active(host + 1);
if (queryData(&dev_host[host]))
{
#if DEVICE_DEBUG
printf("Thread: %d. host: %d. Got data from device.\n",
(int)polling_thread[host], host);
#endif
// TODO: register new id
unsigned char reg_id = dev_host[host].type | dev_host[host].number;
if (RegisterID(®_id) != 0)
{
printf("Thread: %d. host: %d. Fail to register new device.\n",
(int)polling_thread[host],
host);
}
else
{
printf("Thread: %d. host: %d. Registered new device %X.\n",
(int)polling_thread[host],
host,
reg_id);
}
dev_host[host].type = DEV_TYPE_MASK(reg_id);
dev_host[host].number = DEV_NUMBER_MASK(reg_id);
sendControl(dev_host[host]);
}
else
{
#if DEVICE_DEBUG
printf("Thread: %d. host: %d. No device here.\n",
(int)polling_thread[host], host);
#endif
}
RaspiExt_Pin_Hostx_Inactive(host + 1);
}
pthread_mutex_unlock(&serial_access);
}
usleep(time_poll);
//pthread_mutex_unlock(&device_control_access);
}// query device
}
}
int queryData(struct Device * dev)
{
int packet_len, data_len;
struct Packet * packet = malloc(128);
if (dev->data == NULL)
{
if (IS_MY_THESIS(dev->type))
{
dev->data = malloc(sizeof (struct ThesisData));
memset(dev->data, 0, sizeof (struct ThesisData));
}
else
{
dev->data = malloc(getTypeLength(dev->data_type));
memset(dev->data, 0, getTypeLength(dev->data_type));
}
}
#if DEVICE_DEBUG
printf("Query Device: \n");
DeviceInfo(dev);
#endif
if (IS_MY_THESIS(dev->type))
{
data_len = sizeof(struct ThesisData);
packet_len = 4 + data_len;
//packet = malloc(packet_len);
}
else
{
data_len = getTypeLength(dev->data_type);
packet_len = 4 + data_len;
//packet = malloc(packet_len);
}
packet->id = dev->number | dev->type;
packet->cmd = CMD_TYPE_QUERY;
#if __BYTE_ORDER == __BIG_ENDIAN
packet->data_type = DEV_TYPE_MASK(dev->data_type) | BIG_ENDIAN_BYTE_ORDER;
#else
packet->data_type = DEV_TYPE_MASK(dev->data_type) | LITTLE_ENDIAN_BYTE_ORDER;
#endif
memcpy(packet->data, dev->data, data_len);
// add checksum byte
packet->data[data_len] = checksum((char *)packet);
#if DEVICE_DEBUG
printf("Query Packet: ");
int i;
for (i = 0; i < packet_len + 1; i++)
{
printf("%02X ", *((unsigned char *) packet + i));
}
printf("Checksum: %02X.\n", *(((char *)packet) + packet_len));
printf("\n");
#endif
Serial_SendMultiBytes((unsigned char *) packet, packet_len);
// sleep 5ms for timeout
usleep(5000);
packet_len = Serial_Available();
if (packet_len)
{
Serial_GetData((char *) packet, packet_len);
// checksum check
if (checksum((char *)packet) != packet->data[getTypeLength(packet->data_type)])
{
#if DEVICE_DEBUG
printf("Wrong checksum.\n");
#endif
// what should to do now?
if (packet != NULL)
{
free(packet);
packet = NULL;
}
return 0;
}
#if DEVICE_DEBUG
printf("Received Packet: ");
int i;
for (i = 0; i < packet_len + 1; i++)
{
printf("%02X ", *((unsigned char *) packet + i));
}
printf("Checksum: %02X.\n", *(((char *)packet) + packet_len));
printf("\n");
#endif
// reallocating memory if not old data type
if (dev->data_type != DATA_TYPE_MASK(packet->data_type))
{
if (dev->data != NULL)
free(dev->data);
if (IS_MY_THESIS(DEV_TYPE_MASK(packet->id)))
{
dev->data = malloc(sizeof(struct ThesisData));
}
else
{
dev->data = malloc(getTypeLength(DATA_TYPE_MASK(packet->data_type)));
}
}
dev->data_type = DATA_TYPE_MASK(packet->data_type);
dev->type = DEV_TYPE_MASK(packet->id);
dev->number = DEV_NUMBER_MASK(packet->id);
if (IS_MY_THESIS(DEV_TYPE_MASK(packet->id)))
{
dev->polling_control.time_poll_ms = 500;
memcpy(dev->data, packet->data, sizeof (struct ThesisData));
}
else
{
memcpy(dev->data, packet->data, getTypeLength(packet->data_type));
}
if (packet != NULL)
{
free(packet);
packet = NULL;
}
return packet_len;
}
else
{
if (packet != NULL)
{
free(packet);
packet = NULL;
}
return 0;
}
}
uint8_t getPacketLength(char * packet)
{
struct Packet * mypacket = (struct Packet *)packet;
return sizeof(struct Packet) + getTypeLength(mypacket->data_type) + 1; // add 1 for checksum
}
/*! \void XCLProcessor::performSetLength(XCLSyntaxExpression* expr, QVector<XCLProcessParameter> param,XCLParsingItem* item)
* \brief Sets the length of the XCLSyntaxExpression \a expr to the value given by the parameters of the method.
* \param expr A pointer to the XCLSyntaxExpression, the processing method will be performed on.
* \param param A list of parameters for this method.
* \param item A pointer to the parent XCLParsingItem.
* \exception XCLException
**/
void XCLProcessor::performSetLength(XCLSyntaxExpression* expr, QVector<XCLProcessParameter> param,FileParserState& item)
{
_UINT32 count=0;
QString count2; // referenced or calculated value as QString
BOOL isBigEndian;
QString interpretation;
try
{
//Value and DataType is given
if(param.size()==2)
{
XCLProcessParameter p1=param.at(0); //the value
XCLProcessParameter p2=param.at(1); //the data type
QString type = p2.getValue(item.index);
//value has to be calculated
if (p1.getValueType() == XCLProcessParameter::MATHEX)
{
XCLCalculator calc;
count2 = calc.parseExpression(p1.getValue(), item.index);
// count=XCLStringConverter::string2Number<_UINT32>(count2,"uint64",expr->getIsBigEndian()); //something funny returns here ???
count=count2.toInt();
}
else
{
XCLInputNormalizer normalizer;
isBigEndian = (item.index.get(p1.getValueReference()))->at(0)->getIsBigEndian();
interpretation = (item.index.get(p1.getValueReference()))->at(0)->getInterpretation();
if (isBigEndian)
{
QByteArray ba = p1.getValueAsBA(&item.index);
count = (normalizer.normalizeValue((UCHAR*)ba.data(),ba.size(),interpretation, isBigEndian)->toInt());
//count = p1.getValue(item.index).toLong();
}
else
{
QByteArray ba = p1.getValueAsBA(&item.index);
count = (normalizer.normalizeValue((UCHAR*)ba.data(),ba.size(),interpretation, isBigEndian)->toInt());
}
}
_UINT8 typeLength = getTypeLength(type);
expr->setLength(count*typeLength);
}
else if (param.size()==1)
{
XCLProcessParameter p=param.at(0);
//value has to be calculated
if (p.getValueType() == XCLProcessParameter::MATHEX)
{
XCLCalculator calc;
count = calc.parseExpression(p.getValue(), item.index);
expr->setLength(count);
}
else
{
_UINT32 num1=p.getValue(item.index).toLong();
expr->setLength(num1);
}
}
else
{
throw XCLException("Possible candidates for setLength are: setLength( length ) or setLength( count , type )\n");
}
}
catch(XCLException exception)
{
exception.message();
throw XCLException("XCLProcessor couln´t execute setLength()\n");
}
}
void main(void)
{
CLK_HSIPrescalerConfig(CLK_PRESCALER_HSIDIV1);
Delay_Init();
RS485_Init(115200);
#if DEBUG
// UART_Init(115200);
#endif
SRF05_Init();
SRF05_AutoPoolEnable();
//flash_read_buffer((char *)&my_data, sizeof (struct flash_data));
my_data.id = 0x21;
// UART_SendByte(0x21, HEX);
// RS485_SendStr("Hello world.\n");
// LED run
GPIO_Init(LED_RUN_PORT, LED_RUN_PIN, GPIO_MODE_OUT_PP_HIGH_FAST);
GPIO_WriteHigh(LED_RUN_PORT, LED_RUN_PIN);
while (1)
{
if (Millis() - tmp_time > 500)
{
LED_RUN_TOGGLE;
tmp_time = Millis();
#if DEBUG
RS485_DIR_OUTPUT;
RS485_SendStr("\n");
RS485_SendFloat(SRF05_GetDistance());
RS485_DIR_INPUT;
#endif
}
if (GPIO_ReadInputPin(RS485_SEL_PORT, RS485_SEL_PIN) == RESET)
{
if (RS485_Available() >= 8)
{
// memset(packet_buff, 0, PACKET_BUFFER_SIZE);
packet_len = RS485_GetData(packet_buff);
packet = (struct Packet *)packet_buff;
if (packet_len < 4 + getTypeLength(packet->data_type))
{
// not enough length
}
else
{
switch (packet->cmd)
{
case CMD_QUERY:
if (packet->id == my_data.id)
{
LED_RUN_TOGGLE;
tmp_distance = SRF05_GetDistance();
packet->data_type = TYPE_FLOAT | BIG_ENDIAN_BYTE_ORDER;
memcpy(packet->data, &tmp_distance, getTypeLength(packet->data_type));
packet->data[getTypeLength(packet->data_type)] = checksum((char *)packet);
RS485_DIR_OUTPUT;
RS485_SendData(packet_buff, 4 + getTypeLength(packet->data_type));
RS485_DIR_INPUT;
}
else if (IS_BROADCAST_ID(packet->id))
{
// if (GPIO_ReadInputPin(RS485_SEL_PORT, RS485_SEL_PIN) == RESET)
// {
LED_RUN_TOGGLE;
packet->id = my_data.id;
tmp_distance = SRF05_GetDistance();
packet->data_type = TYPE_FLOAT | BIG_ENDIAN_BYTE_ORDER;
memcpy(packet->data, &tmp_distance, getTypeLength(packet->data_type));
packet->data[getTypeLength(packet->data_type)] = checksum((char *)packet);
RS485_DIR_OUTPUT;
RS485_SendData(packet_buff, 4 + getTypeLength(packet->data_type));
RS485_DIR_INPUT;
// }
}
else
{
// not own id
}
break;
case CMD_CONTROL:
// by default, this mode used only for setting id
// the id stored on the first bytes of data bytes
if (IS_BROADCAST_ID(packet->id))
{
LED_RUN_TOGGLE;
if (IS_SENSOR_ULTRA_SONIC(packet->data[0]))
{
my_data.id = packet->data[0];
}
}
break;
default:
break;
}
RS485_Flush();
}
}
}
else
{
RS485_Flush();
}
SRF05_ProcessTrigger();
}
}
