diff --git a/controller/fw/embed/src/flash.cpp b/controller/fw/embed/src/flash.cpp
index de96e4a..b06814d 100644
--- a/controller/fw/embed/src/flash.cpp
+++ b/controller/fw/embed/src/flash.cpp
@@ -115,9 +115,9 @@ uint8_t flash_read_word(uint32_t address){
 
 }
     // Wait if flash
-bool validata_crc(FLASH_RECORD* crc){
-    return crc->crc == 0x6933? true : false;
-}
+// bool validata_crc(FLASH_RECORD* crc){
+//     return crc->crc == 0x6933? true : false;
+// }
 
 uint16_t validate_crc16(uint8_t *data, uint32_t length) {
     uint16_t crc = 0xFFFF; // start value for CRC MODBUS
diff --git a/controller/fw/embed/src/main.cpp b/controller/fw/embed/src/main.cpp
index 14e9ce4..6379863 100644
--- a/controller/fw/embed/src/main.cpp
+++ b/controller/fw/embed/src/main.cpp
@@ -117,25 +117,25 @@ void setup_foc(MagneticSensorAS5045 *encoder, BLDCMotor *motor,
   motor->initFOC();
 }
 
-void send_can_with_id_crc(uint8_t id, uint8_t message_type, const void* data, size_t data_length) {
+void send_can_with_id_crc(uint8_t id, uint8_t message_type, const void* data, size_t data_length = 7) {
   // Create CAN message
   CAN_message_t msg_l;
   msg_l.id = id;
   // msg_l.len = 8;  // Protocol-defined message length
-  msg_l.buf[0] = message_type;
+  memcpy(&msg_l.buf[0], &message_type, sizeof(uint8_t));
   memcpy(&msg_l.buf[1], data, 1);
 
   // Prepare CRC calculation buffer (ID + data)
-  size_t crc_data_size = sizeof(msg_l.id) + 1;
-  uint8_t crc_data[crc_data_size];
+  size_t crc_data_size = sizeof((uint8_t)msg_l.id) +  2 * sizeof(uint8_t);
+  uint8_t crc_data[7];
 
-  // Copy message ID
-  memcpy(crc_data, &msg_l.id, sizeof(msg_l.id));
+  // Copy message ID 
+  memcpy(crc_data, (uint8_t*)&msg_l.id, sizeof(uint8_t));
   // Copy all data bytes
-  memcpy(crc_data + sizeof(msg_l.id), data, 1);
-
+  for(int i = 1;i < 7;i++)
+    memcpy(crc_data + i,(uint8_t*)&msg_l.buf[i - 1],1);
   // Calculate CRC
-  uint16_t crc_value = validate_crc16(crc_data, crc_data_size);
+  uint16_t crc_value = validate_crc16(crc_data, 7);
  
   // Insert CRC into buffer
   msg_l.buf[6] = crc_value & 0xFF;
@@ -390,7 +390,7 @@ void foc_step(BLDCMotor *motor, Commander *commander) {
 
   flash_rec = load_params();
   /*  For test          */
-  // int value = 73;  //addr
+  // int value = 3;  //addr
   // write_param(addr_id,value); //for update address in firmware
   // // Load parameters from flash
   // flash_rec = load_params();