UsaPack for mbed OS 6
Embed:
(wiki syntax)
Show/hide line numbers
UsaPackOS.cpp
00001 #include "UsaPackOS.hpp" 00002 #include "mbed.h" 00003 00004 00005 void UsaPackOS::Receive() 00006 { 00007 while (serial.readable()) 00008 { 00009 char c; 00010 serial.read(&c, 1); 00011 receive_buffer[receive_index] = c; 00012 receive_index = (receive_index + 1) % receive_size; 00013 if (c == '\0') 00014 { 00015 // printf("DEC\r\n"); 00016 Decode(); 00017 receive_index = 0; 00018 } 00019 } 00020 } 00021 00022 void UsaPackOS::SendByte() 00023 { 00024 while (serial.writable()) 00025 { 00026 if (send_index != send_end_index) 00027 { 00028 serial.write(&send_buffer[send_index],1); 00029 send_index = (send_index + 1) % send_size; 00030 } 00031 else 00032 { 00033 serial.attach(NULL, UnbufferedSerial::TxIrq); 00034 break; 00035 } 00036 } 00037 } 00038 00039 void UsaPackOS::Decode() 00040 { 00041 char data[receive_size]; 00042 // for (int i = 0; i < 43; i++) { 00043 // printf("%02x ", receive_buffer[i]); 00044 // } 00045 // printf("\r\n"); 00046 // printf("%02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x \r\n",receive_buffer[0],receive_buffer[1],receive_buffer[2],receive_buffer[3],receive_buffer[4],receive_buffer[5],receive_buffer[6],receive_buffer[7],receive_buffer[8],receive_buffer[9],receive_buffer[10],receive_buffer[11]); 00047 CobsDecode(receive_buffer, receive_index-2, data); 00048 // printf("%02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x \r\n",data[0],data[1],data[2],data[3],data[4],data[5],data[6],data[7],data[8],data[9],data[10],data[11]); 00049 00050 // for (int i = 0; i < 43; i++) { 00051 // printf("%02x ", data[i]); 00052 // } 00053 // printf("\r\n"); 00054 union c4 00055 { 00056 char c[4]; 00057 int a; 00058 }; 00059 c4 _address; 00060 c4 _checksum; 00061 for (int i = 0; i < 4; i++) 00062 { 00063 _address.c[i] = data[i]; 00064 } 00065 // printf("%d\r\n",package_index); 00066 for (int i = 0; i < 4; i++) 00067 { 00068 _checksum.c[i] = data[i+4]; 00069 } 00070 for (int i = 0; i < package_index; i++) 00071 { 00072 if (package_address[i] == _address.a) 00073 { 00074 // printf("%\r\n",package_size[0]); 00075 int checksum = 0; 00076 for (int j = 0; j < package_size[i]; j++) 00077 { 00078 checksum += data[j+8]; 00079 } 00080 // printf("check %d %d\r\n",_checksum.a,checksum); 00081 if (_checksum.a != checksum) 00082 { 00083 // printf("not checksum\r\n"); 00084 return; 00085 } 00086 for (int j = 0; j < package_size[i]; j++) 00087 { 00088 *((char*)(package_object[i])+j) = data[j+8]; 00089 } 00090 // printf("Dec ok\r\n"); 00091 break; 00092 } 00093 } 00094 } 00095 00096 UsaPackOS::UsaPackOS(PinName tx, PinName rx, int baud) 00097 :serial(tx, rx, baud), receive_index(0), package_index(0), send_index(0), send_end_index(0) 00098 { 00099 serial.attach(callback(this, &UsaPackOS::Receive), UnbufferedSerial::RxIrq); 00100 for (int i = 0; i < receive_size; i++) 00101 { 00102 receive_buffer[i] = '\0'; 00103 } 00104 //receive_thread.start(callback(this, &UsaPackOS::ThreadReceive)); 00105 00106 } 00107 00108 void UsaPackOS::CobsEncode(char data[], int length, char send_data[]) 00109 { 00110 int last_zero = 0; 00111 for (int i = 0; i < length; i++) 00112 { 00113 if (data[i] == 0) 00114 { 00115 send_data[last_zero] = i - last_zero + 1; 00116 last_zero = i + 1; 00117 } 00118 else 00119 { 00120 send_data[i + 1] = data[i]; 00121 } 00122 } 00123 send_data[last_zero] = length - last_zero + 1; 00124 send_data[length + 1] = 0; 00125 } 00126 00127 void UsaPackOS::CobsDecode(char receive_data[], int length, char data[]) 00128 { 00129 int next_zero; 00130 next_zero = receive_data[0] - 1; 00131 for (int i = 0; i < length; i++) 00132 { 00133 data[i] = receive_data[i + 1]; 00134 } 00135 while (next_zero < length) 00136 { 00137 data[next_zero] = 0; 00138 next_zero += receive_data[next_zero + 1]; 00139 } 00140 } 00141
Generated on Thu Sep 1 2022 14:06:47 by
1.7.2