Publish for the JRO Radar Controller.
Fork of jro by
JroIpdata.cpp
- Committer:
- miguelcordero191
- Date:
- 2015-02-05
- Revision:
- 2:3d8d52e9751c
- Child:
- 4:de495ce256b8
File content as of revision 2:3d8d52e9751c:
#include "JroIpdata.h" void printCadena(char* cad, int lenc){ //printf("\r\n"); for (int i=0; i<lenc; i++){ //printf("cad[%d] = %d\r\n", i, cad[i]); } } IpData::IpData(char* tx_buffer){ this->id = ID_DEV; this->isValidData = false; this->len_tx_data = 0; this->tx_buff = tx_buffer; } char* IpData::__findHeader(char* buffer){ /* Find header and get ip data (without header) Input: Return: valid_ipdata: ip data without header Example: buffer = [ B0 B1 $ J R O $ B7 B8 B9 B10 ..... ] return valid_ip_data = [ B7 B8 B9 B10 ..... ] */ char* valid_ip_data; //printf("\r\nSearching header in %s\r\n", buffer); //Finding header valid_ip_data = strstr(buffer, HEADER); if (valid_ip_data == NULL){ //printf("\r\nHeader was not found\r\n"); return NULL; } //printf("\r\nHeader was found %s\r\n", valid_ip_data); //printf("\r\nskipping header ...\r\n"); valid_ip_data += 5; //skip header return valid_ip_data; } char IpData::__getXor(char* data, unsigned long len_data){ char new_xor = 0; for(unsigned long i = 0; i < len_data; i++){ new_xor = new_xor xor data[i]; } return new_xor; } int IpData::__verifyData(char* ip_data, unsigned long len_data){ char new_xor = 0; new_xor = this->__getXor(ip_data, len_data); if (new_xor != 0){ //printf("\r\nInvalid XOR: %d\r\n", new_xor); return 0; } //printf("\r\nXOR verified successfully\r\n"); return 1; } int IpData::__getParameters(){ return 1; } char* IpData::encode(unsigned short cmd, char* payload, unsigned long len_payload){ int head_size= strlen(HEADER); char xor_wr; //Copy header strcpy(tx_buff, HEADER); len_tx_data = len_payload + 5; //Incluye la longitud de la data + 1B (id_class) + 1B (id_dev) + 2B (cmd) + 1B (xor) len_tx_buffer = len_tx_data + 9; //Incluye 5 bytes del Header + 4 de la longitud tx_buff[0+head_size] = (len_tx_data >> 24) & 0xff; tx_buff[1+head_size] = (len_tx_data >> 16) & 0xff; tx_buff[2+head_size] = (len_tx_data >> 8) & 0xff; tx_buff[3+head_size] = len_tx_data & 0xff; tx_buff[4+head_size] = ID_CLASS; tx_buff[5+head_size] = ID_DEV; tx_buff[6+head_size] = (cmd >> 8) & 0xff; tx_buff[7+head_size] = cmd & 0xff; for (unsigned int i=0; i<len_payload; i++){ tx_buff[8+head_size+i] = payload[i]; } xor_wr = this->__getXor(&tx_buff[0+head_size], len_tx_data+4-1); //Incluir todos los bytes de datos mas los 4B de la longitud menos 1B del xor tx_buff[8+head_size+len_payload] = xor_wr; //printf("\r\nTx Buffer = "); for(unsigned long i=0; i< len_tx_buffer; i++){ //printf("0x%x ", tx_buff[i]); } return tx_buff; } int IpData::decode(char* buffer, unsigned long len_buffer){ char* ip_data; unsigned long len_data; this->isValidData = false; //printf("\r\nRx Buffer = "); for(unsigned long i=0; i< len_buffer; i++){ //printf("0x%x ", buffer[i]); } if (len_buffer < 13){ //printf("\r\nLongitud de la data insuficiente\r\n"); return 0; } //Finding header and get ip data (without header) ip_data = this->__findHeader(buffer); if (ip_data == NULL) return 0; len_data = ip_data[0]*65536*256 + ip_data[1]*65536 + ip_data[2]*256 + ip_data[3]; //printf("\r\nLen data = %d\r\n", len_data); this->rx_buff = ip_data; //len_data + 4 = longitud de toda la data (incluyendo los 4 bytes del campo len) if (not this->__verifyData(ip_data, len_data+4)) return 0; //head = ip_data[0:5]; this->id_class = ip_data[4]; this->id_dev = ip_data[5]; this->cmd = ip_data[6]*256 + ip_data[7]; this->payload = ip_data + 8; this->len_payload = len_data - 5; //1B id_class, 1B id_dev, 2B cmd y 1B xor this->xor_rd = ip_data[4+len_data-1]; //printf("\r\nID_CLASS = 0x%x, ID_DEV = 0x%x, CMD = 0x%x, XOR = 0x%x", id_class, id_dev, cmd, xor_rd); //printf("\r\nPAYLOAD[0] = 0x%x, PYLD_LEN = 0x%x\r\n", payload[0], len_payload); if (this->id_class == ID_CLASS){ this->isValidData = true; return 1; } return 0; } char IpData::getIdClass(){ if (not this->isValidData) return 0; return this->id_class; } char IpData::getIdDevice(){ if (not this->isValidData) return 0; return this->id_dev; } unsigned short IpData::getCmd(){ if (not this->isValidData) return 0; return this->cmd; } unsigned long IpData::getPayloadLen(){ if (not this->isValidData) return 0; return this->len_payload; } char* IpData::getPayload(){ if (not this->isValidData) return NULL; return this->payload; } char* IpData::getTxData(){ return this->tx_buff; } unsigned long IpData::getTxDataLen(){ return this->len_tx_buffer; }