123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140 |
- #include "DataHelper.hpp"
- void DataHelper::check_sum(uint8_t buf[], const int start, const int len, uint8_t checksum[])
- {
- // 所得结果模65536余数取反加1
- int sum = 0;
- for (int i = start; i < start + len; i++) {
- sum += buf[i];
- }
- int ts = (~(sum % 65536) + 1);
- checksum[0] = static_cast<uint8_t>(ts >> 8);
- checksum[1] = static_cast<uint8_t>(ts & 0xFF);
- }
- int DataHelper::get_len(uint8_t buf[])
- {
- // uint8_t dest[4];
- // buffer_from_ascii_uint8(dest, buf, 4);
- // 计算长度
- int len1 = buf[0] & 0x0F << 8;
- int len2 = buf[1];
- const int len = len1 + len2;
- // 校验长度 校验码的计算:D11D10D9D8+D7D6D5D4+D3D2D1D0,求和后模16余数取反加1。
- uint8_t b1 = (buf[0] & 0x0F);
- uint8_t b2 = (buf[1] & 0xF0) >> 4;
- uint8_t b3 = (buf[1] & 0x0F);
- const int t = b1 + b2 + b3;
- uint8_t check = (~(t % 16) + 1) & 0x0F;
- uint8_t check1 = (buf[0] & 0xF0) >> 4;
- if (check!= check1) {
- return -1;
- }
- return len;
- }
- void DataHelper::generate_len(const int len, uint8_t after_len[])
- {
- const uint8_t hi = static_cast<uint8_t>(len >> 8);
- const uint8_t lo = static_cast<uint8_t>(len & 0xFF);
- const int t = hi & 0x0F + (lo & 0xF0 >> 4) + lo & 0x0F;
- const uint8_t check = static_cast<uint8_t>(~(t % 16) + 1);
- after_len[0] = hi + (check << 4);
- after_len[1] = lo;
- }
- int DataHelper::get_unsigned_int(uint8_t buf[], int len)
- {
- // 高位在前,低位在后,支持多个字节,用for循环实现
- int val = 0;
- for (int i = 0; i < len; i++) {
- val += buf[i] << (8 * (len-i-1));
- }
- return val;
- }
- int DataHelper::get_signed_int(uint8_t buf[], int len)
- {
- // 高位在前,低位在后,支持多个字节,用for循环实现
- int val = 0;
- for (int i = 0; i < len; i++) {
- uint8_t b = buf[i];
- if(i == 0) {
- b = b & 0x7F;
- }
- val += b << (8 * (len-i-1));
- }
- return buf[0] >> 7 == 0 ? val: -val;
-
- }
- double DataHelper::get_unsigned_double(uint8_t buf[], const int len, const int factor)
- {
- const int val = get_unsigned_int(buf, len);
- return val / (factor * 1.0);
- }
- double DataHelper::get_signed_double(uint8_t buf[], const int len, const int factor)
- {
- const int val = get_signed_int(buf, len);
- return val / (factor * 1.0);
- }
- void DataHelper::buffer_copy_uint8_ascii(uint8_t* dest, uint8_t b, uint8_t& index)
- {
- char hexString[3];
- int len = snprintf(hexString, 3, "%02X", b);
- if (len > 0) {
- dest[index++] = hexString[0];
- dest[index++] = hexString[1];
- }
- }
- void DataHelper::buffer_copy_uint16_ascii(uint8_t* dest, uint16_t b, uint8_t& index)
- {
- char hexString[5];
- int len = snprintf(hexString, 5,"%04X", b);
- if (len > 0) {
- dest[index++] = hexString[0];
- dest[index++] = hexString[1];
- dest[index++] = hexString[2];
- dest[index++] = hexString[3];
- }
- }
- int DataHelper::buffer_from_ascii_uint8(uint8_t* dest, uint8_t* src, int len)
- {
- const int byteLen = len/2;
- for (int i = 0; i < byteLen; i++) {
- char hex[3];
- hex[0] = src[i*2];
- hex[1] = src[i*2+1];
- hex[2] = '\0';
- int byteValue = 0;
- sscanf_s(hex, "%x", &byteValue);
- dest[i] = byteValue;
- }
- return byteLen;
- }
- uint8_t DataHelper::read_uint8_ascii(uint8_t* src, uint8_t& index)
- {
- uint8_t data[1];
- buffer_from_ascii_uint8(data, src+ index, 2);
- index += 2;
- return data[0];
- }
- uint16_t DataHelper::read_uint16_ascii(uint8_t* src, uint8_t& index)
- {
- uint8_t data[2];
- buffer_from_ascii_uint8(data , src + index, 4);
- index += 4;
- return (data[0] << 8) + data[1];
- }
|