char mb_data [ 128 ] = { 0 } ;
int mb_len = 0 ;
bool mb_state = 1;
void setup() {
Serial . begin ( 115200 , SERIAL_8N1);
}
void loop() {
if ( Serial . available ( ) > 0 ) {
mb_state = 0;
mb_data [ mb_len ] = char ( Serial . read ( ) ) ;
mb_len++ ;
} else {
if (!mb_state) {
while (Serial.read() >= 0);
uint16_t crc_hy = crc_16 ( mb_data , 4 );
Serial . println(crc_hy );
for (int i = 0 ; i < mb_len ; i++ ) {
Serial . print( mb_data[ i ] );
mb_data[ i ] = 0;
}
mb_len = 0 ;
mb_state = 1;
}
}
}
uint16_t crc_16 ( char data [ ] , int len )
{
unsigned int i , crc ;
unsigned int table [ 256 ] = { 0 } ;
for ( i = 0 ; i <= 255 ; i++ ) {
table [ i + 1 ] = 0 ;
crc = i ;
for ( int n = 1 ; n <= 8 ; n++ ) {
if ( ( crc & 1 ) != 0 ) {
crc = crc >> 1 ^ 40961 ;
} else {
crc = crc >> 1 ;
}
}
table [ i + 1 ] = crc ;
}
crc = 65535 ;
for ( i = 0 ; i < len ; i++ ) {
crc = ( crc >> 8 ) ^ table [ ( byte ( data [ i ] ) ^ ( crc & 255 ) ) + 1 ] ;
}
return (uint16_t(crc)) ;
}