@@ -29,7 +29,7 @@ Slave::Slave(DXLPortHandler &port, const uint16_t model_num, float protocol_ver)
2929 packet_buf_capacity_ = DEFAULT_DXL_BUF_LENGTH;
3030 is_buf_malloced_ = true ;
3131 }
32- info_tx_packet_.is_init = false ;
32+ info_tx_packet_.is_init = false ;
3333 info_rx_packet_.is_init = false ;
3434}
3535
@@ -493,95 +493,131 @@ Slave::processInstWrite()
493493 p_rx_info = &info_rx_packet_;
494494 p_rx_param = p_rx_info->p_param_buf ;
495495
496- if (p_rx_info->id != DXL_BROADCAST_ID){
497- if (p_rx_info->protocol_ver == 2 ){
498- if (p_rx_info->recv_param_len > 2 ){ // 2 = Address(2)+Data(n)
499- addr = ((uint16_t )p_rx_param[1 ]<<8 ) | (uint16_t )p_rx_param[0 ];
500- p_data = &p_rx_param[2 ];
501- data_length = p_rx_info->recv_param_len -2 ;
502- if (data_length+11 > packet_buf_capacity_){
503- err = DXL_LIB_ERROR_NOT_ENOUGH_BUFFER_SIZE;
504- }
505- }else {
506- err = DXL_LIB_ERROR_WRONG_PACKET;
507- }
508- }else if (p_rx_info->protocol_ver == 1 ){
509- if (p_rx_info->recv_param_len > 1 ){ // 1 = Address(1)+Data(n)
510- addr = p_rx_param[0 ];
511- p_data = &p_rx_param[1 ];
512- data_length = p_rx_info->recv_param_len -1 ;
513- if (data_length+6 > packet_buf_capacity_){
514- err = DXL_LIB_ERROR_NOT_ENOUGH_BUFFER_SIZE;
515- }
516- }else {
517- err = DXL_LIB_ERROR_WRONG_PACKET;
496+ if (p_rx_info->id == DXL_BROADCAST_ID){
497+ last_lib_err_ = DXL_LIB_ERROR_NOT_SUPPORT_BROADCAST;
498+ return false ;
499+ }
500+
501+ // extract start address and length from the instruction packet
502+ switch (p_rx_info->protocol_ver )
503+ {
504+ case 2 :
505+ if (p_rx_info->recv_param_len <= 2 ) { // 2 = Address(2)+Data(n)
506+ err = DXL_LIB_ERROR_WRONG_PACKET;
507+ }
508+ else {
509+ addr = ((uint16_t )p_rx_param[1 ]<<8 ) | (uint16_t )p_rx_param[0 ];
510+ p_data = &p_rx_param[2 ];
511+ data_length = p_rx_info->recv_param_len -2 ;
512+ if (data_length+11 > packet_buf_capacity_){
513+ err = DXL_LIB_ERROR_NOT_ENOUGH_BUFFER_SIZE;
518514 }
519- }else {
515+ }
516+ break ;
517+
518+ case 1 :
519+ if (p_rx_info->recv_param_len <= 1 ){ // 1 = Address(1)+Data(n)
520520 err = DXL_LIB_ERROR_WRONG_PACKET;
521521 }
522+ else {
523+ addr = p_rx_param[0 ];
524+ p_data = &p_rx_param[1 ];
525+ data_length = p_rx_info->recv_param_len -1 ;
526+ if (data_length+6 > packet_buf_capacity_){
527+ err = DXL_LIB_ERROR_NOT_ENOUGH_BUFFER_SIZE;
528+ }
529+ }
530+ break ;
522531
523- if (err == DXL_LIB_OK){
524- uint8_t i, j, backup_data = 0 ;
525- uint16_t item_start_addr, item_addr_length;
526- ControlItem_t *p_item;
527- for (i=0 ; i < registered_item_cnt_; i++){
528- p_item = &control_table_[i];
529- item_start_addr = p_item->start_addr ;
530- item_addr_length = p_item->length ;
531- if (item_start_addr != ADDR_ITEM_MODEL_NUMBER
532- && item_start_addr != ADDR_ITEM_FIRMWARE_VER){
533- if (item_addr_length != 0
534- && p_item->p_data != nullptr
535- && isAddrInRange (item_start_addr, item_addr_length, addr, data_length) == true ){
536- // Check data for ID, Protocol Version (Act as a system callback)
537- if (item_start_addr == ADDR_ITEM_ID){
538- backup_data = p_data[item_start_addr-addr];
539- if (protocol_ver_idx_ == 2 && backup_data >= 0xFD ){
532+ default :
533+ err = DXL_LIB_ERROR_WRONG_PACKET;
534+ break ;
535+ }
536+
537+ if (err == DXL_LIB_OK){
538+ uint8_t i, j;
539+ uint8_t backup_data[32 ]; // we max support registers of 32 bytes
540+ uint16_t item_start_addr, item_addr_length;
541+ ControlItem_t *p_item;
542+
543+ for (i=0 ; i < registered_item_cnt_; i++){
544+
545+ p_item = &control_table_[i];
546+ item_start_addr = p_item->start_addr ;
547+ item_addr_length = p_item->length ;
548+
549+ if (item_addr_length != 0 && p_item->p_data != nullptr
550+ && isAddrInRange (item_start_addr, item_addr_length, addr, data_length) == true ){
551+
552+ // backup data, copy new data
553+ for (j=0 ; j<item_addr_length; j++){
554+ // backup supported only for registers smaller than 32 bytes
555+ if (item_addr_length <= 32 ) {
556+ backup_data[j] = p_item->p_data [j];
557+ }
558+ p_item->p_data [j] = p_data[item_start_addr - addr + j];
559+ }
560+ // Check data for ID, Protocol Version (Act as a system callback)
561+ switch (item_start_addr) {
562+
563+ case ADDR_ITEM_ID: // validate ID
564+ if (protocol_ver_idx_ == 2 && id_ >= 0xFD ){
565+ packet_err = DXL2_0_ERR_DATA_RANGE;
566+ }
567+ if (protocol_ver_idx_ == 1 && id_ >= 0xFE ){
568+ packet_err |= 1 <<DXL1_0_ERR_RANGE_BIT;
569+ }
570+ break ;
571+
572+ case ADDR_ITEM_PROTOCOL_VER: // validate Protocol Version
573+ if (protocol_ver_idx_ != 1 && protocol_ver_idx_ != 2 ){
574+ if (backup_data[0 ] == 2 ){
540575 packet_err = DXL2_0_ERR_DATA_RANGE;
541- }else if (protocol_ver_idx_ == 1 && backup_data >= 0xFE ){
542- packet_err |= 1 <<DXL1_0_ERR_RANGE_BIT;
543576 }
544- backup_data = id_;
545- }else if (item_start_addr == ADDR_ITEM_PROTOCOL_VER){
546- backup_data = p_data[item_start_addr-addr];
547- if (backup_data != 1 && backup_data != 2 ){
548- if (protocol_ver_idx_ == 2 ){
549- packet_err = DXL2_0_ERR_DATA_RANGE;
550- }else if (protocol_ver_idx_ == 1 ){
551- packet_err |= 1 <<DXL1_0_ERR_RANGE_BIT;
552- }
577+ else if (backup_data[0 ] == 1 ){
578+ packet_err |= 1 <<DXL1_0_ERR_RANGE_BIT;
553579 }
554- backup_data = protocol_ver_idx_;
555580 }
556- if (packet_err != 0 ){
557- break ;
581+ break ;
582+
583+ case ADDR_ITEM_MODEL_NUMBER: // model number if Read Only
584+ case ADDR_ITEM_FIRMWARE_VER: // firmware is Read Only
585+ if (backup_data[0 ] == 2 ){
586+ packet_err = DXL2_0_ERR_DATA_RANGE;
558587 }
559- // Copy received data to each item
560- for (j=0 ; j<item_addr_length; j++){
561- p_item->p_data [j] = p_data[item_start_addr-addr+j];
588+ else if (backup_data[0 ] == 1 ){
589+ packet_err |= 1 <<DXL1_0_ERR_RANGE_BIT;
562590 }
563- // Run user callback for Write instruction,
564- // then check and handle the returned error.
565- if (user_write_callback_ != nullptr ){
566- user_write_callback_ (item_start_addr, packet_err, user_write_callbakc_arg_);
567- if (packet_err != 0 ){
568- // If an error occurs for the ID and Protocol Version,
569- // restore the previous data. (Act as a system callback)
570- if (item_start_addr == ADDR_ITEM_ID){
571- id_ = backup_data;
572- }else if (item_start_addr == ADDR_ITEM_PROTOCOL_VER){
573- protocol_ver_idx_ = backup_data;
574- }
575- break ;
576- }
591+ break ;
592+
593+ }
594+
595+ // Run user callback for Write instruction
596+ //
597+ // NOTE: Slave does not implement EEPROM persistence of data.
598+ // If your device needs to persist data to ROM you must implement
599+ // `user_write_callback_` and there save the registers to ROM
600+ // including the ones implemented in Slave (id, protocol,
601+ // model number, firmware version)
602+ if (packet_err == 0 && user_write_callback_ != nullptr ){
603+ user_write_callback_ (item_start_addr, packet_err, user_write_callbakc_arg_);
604+ }
605+
606+ if (packet_err != 0 ){
607+ // If an error occurs restore the previous data.
608+ // backup supported only for registers smaller than 32 bytes
609+ if (item_addr_length <= 32 ) {
610+ for (j=0 ; j<item_addr_length; j++){
611+ p_item->p_data [j] = backup_data[j];
577612 }
578613 }
579614 }
580- }
581- ret = txStatusPacket (id_, packet_err, nullptr , 0 );
582- }
583- }else {
584- err = DXL_LIB_ERROR_NOT_SUPPORT_BROADCAST;
615+
616+ } // if
617+
618+ } // for
619+
620+ ret = txStatusPacket (id_, packet_err, nullptr , 0 );
585621 }
586622
587623 last_lib_err_ = err;
@@ -728,4 +764,4 @@ static bool isAddrInOtherItem(uint16_t start_addr, uint16_t length,
728764 }
729765
730766 return ret;
731- }
767+ }
0 commit comments