uint32_t RoboClaw::Read4(uint8_t address, uint8_t cmd, bool *valid){ uint8_t crc; if(valid) *valid = false; uint32_t value=0; uint8_t trys=MAXRETRY; int16_t data; do{ flush(); crc_clear(); write(address); crc_update(address); write(cmd); crc_update(cmd); data = read(timeout); crc_update(data); value=(uint32_t)data<<24; if(data!=-1){ data = read(timeout); crc_update(data); value|=(uint32_t)data<<16; } if(data!=-1){ data = read(timeout); crc_update(data); value|=(uint32_t)data<<8; } if(data!=-1){ data = read(timeout); crc_update(data); value|=(uint32_t)data; } if(data!=-1){ uint16_t ccrc; data = read(timeout); if(data!=-1){ ccrc = data << 8; data = read(timeout); if(data!=-1){ ccrc |= data; if(crc_get()==ccrc){ *valid = true; return value; } } } } }while(trys--); return false; }
void crc_init(crc_t *crc, int order, uint32_t polynom, uint32_t initial_value, uint32_t final_xor) { crc->order = order; crc->polynom = polynom; crc->initial_value = initial_value; crc->final_xor = final_xor; crc->mask = (1L<<order)-1; crc_clear(crc); }
//credits to iceman uint32_t CRC8Maxim(uint8_t *buff, size_t size) { crc_t crc; crc_init(&crc, 9, 0x8c, 0x00, 0x00); crc_clear(&crc); for (size_t i=0; i < size; ++i) crc_update(&crc, buff[i], 8); return crc_finish(&crc); }
//credits to iceman uint32_t CRC8Legic(uint8_t *buff, size_t size) { // Poly 0x63, reversed poly 0xC6, Init 0x55, Final 0x00 crc_t crc; crc_init(&crc, 8, 0xC6, 0x55, 0); crc_clear(&crc); for ( int i = 0; i < size; ++i) crc_update(&crc, buff[i], 8); return SwapBits(crc_finish(&crc), 8); }
bool RoboClaw::GetPinFunctions(uint8_t address, uint8_t &S3mode, uint8_t &S4mode, uint8_t &S5mode){ uint8_t crc; bool valid = false; uint8_t val1,val2,val3; uint8_t trys=MAXRETRY; int16_t data; do{ flush(); crc_clear(); write(address); crc_update(address); write(GETPINFUNCTIONS); crc_update(GETPINFUNCTIONS); data = read(timeout); crc_update(data); val1=data; if(data!=-1){ data = read(timeout); crc_update(data); val2=data; } if(data!=-1){ data = read(timeout); crc_update(data); val3=data; } if(data!=-1){ uint16_t ccrc; data = read(timeout); if(data!=-1){ ccrc = data << 8; data = read(timeout); if(data!=-1){ ccrc |= data; if(crc_get()==ccrc){ S3mode = val1; S4mode = val2; S5mode = val3; return true; } } } } }while(trys--); return false; }
bool RoboClaw::ReadVersion(uint8_t address,char *version){ uint8_t data; uint8_t trys=MAXRETRY; do{ flush(); data = 0; crc_clear(); write(address); crc_update(address); write(GETVERSION); crc_update(GETVERSION); uint8_t i; for(i=0;i<48;i++){ if(data!=-1){ data=read(timeout); version[i] = data; crc_update(version[i]); if(version[i]==0){ uint16_t ccrc; data = read(timeout); if(data!=-1){ ccrc = data << 8; data = read(timeout); if(data!=-1){ ccrc |= data; return crc_get()==ccrc; } } break; } } else{ break; } } }while(trys--); return false; }
bool RoboClaw::write_n(uint8_t cnt, ... ) { uint8_t trys=MAXRETRY; do{ crc_clear(); //send data with crc va_list marker; va_start( marker, cnt ); /* Initialize variable arguments. */ for(uint8_t index=0;index<cnt;index++){ uint8_t data = va_arg(marker, uint16_t); crc_update(data); write(data); } va_end( marker ); /* Reset variable arguments. */ uint16_t crc = crc_get(); write(crc>>8); write(crc); if(read(timeout)==0xFF) return true; }while(trys--); return false; }
bool RoboClaw::read_n(uint8_t cnt,uint8_t address,uint8_t cmd,...) { uint32_t value=0; uint8_t trys=MAXRETRY; int16_t data; do{ flush(); data=0; crc_clear(); write(address); crc_update(address); write(cmd); crc_update(cmd); //send data with crc va_list marker; va_start( marker, cnt ); /* Initialize variable arguments. */ for(uint8_t index=0;index<cnt;index++){ uint32_t *ptr = (uint32_t *)va_arg(marker, uint16_t); if(data!=-1){ data = read(timeout); crc_update(data); value=(uint32_t)data<<24; } else{ break; } if(data!=-1){ data = read(timeout); crc_update(data); value|=(uint32_t)data<<16; } else{ break; } if(data!=-1){ data = read(timeout); crc_update(data); value|=(uint32_t)data<<8; } else{ break; } if(data!=-1){ data = read(timeout); crc_update(data); value|=(uint32_t)data; } else{ break; } *ptr = value; } va_end( marker ); /* Reset variable arguments. */ if(data!=-1){ uint16_t ccrc; data = read(timeout); if(data!=-1){ ccrc = data << 8; data = read(timeout); if(data!=-1){ ccrc |= data; return crc_get()==ccrc; } } } }while(trys--); return false; }
static uint8_t calc_crc4(uint16_t cmd, uint8_t cmd_sz, uint8_t value) { crc_clear(&legic_crc); crc_update(&legic_crc, (value << cmd_sz) | cmd, 8 + cmd_sz); return crc_finish(&legic_crc); }