diff --git a/canopen/sdo/client.py b/canopen/sdo/client.py index 0ed083e4..7e0f58bf 100644 --- a/canopen/sdo/client.py +++ b/canopen/sdo/client.py @@ -628,6 +628,8 @@ def __init__(self, sdo_client, index, subindex=0, size=None, request_crc_support self._seqno = 0 self._crc = sdo_client.crc_cls() self._last_bytes_sent = 0 + self._current_block = [] + self._retransmitting = False command = REQUEST_BLOCK_DOWNLOAD | INITIATE_BLOCK_TRANSFER if request_crc_support: command |= CRC_SUPPORTED @@ -708,7 +710,10 @@ def send(self, b, end=False): request[1:len(b) + 1] = b self.sdo_client.send_request(request) self.pos += len(b) - if self.crc_supported: + # Add the sent data to the current block buffer + self._current_block.append(b) + # Don't calculate crc if retransmitting + if self.crc_supported and not self._retransmitting: # Calculate CRC self._crc.process(b) if self._seqno >= self._blksize: @@ -731,14 +736,37 @@ def _block_ack(self): raise SdoCommunicationError("Server did not respond with a " "block download response") if ackseq != self._blksize: - self.sdo_client.abort(0x05040003) - raise SdoCommunicationError( - ("%d of %d sequences were received. " - "Retransmission is not supported yet.") % (ackseq, self._blksize)) + # Sequence error, try to retransmit + self._retransmit(ackseq, blksize) + # We should be back in sync + return + # Clear the current block buffer + self._current_block = [] logger.debug("All %d sequences were received successfully", ackseq) logger.debug("Server requested a block size of %d", blksize) self._blksize = blksize self._seqno = 0 + + def _retransmit(self, ackseq, blksize): + """Retransmit the failed block""" + logger.info(("%d of %d sequences were received. " + "Will start retransmission") % (ackseq, self._blksize)) + # Sub blocks betwen ackseq and end of corrupted block need to be resent + # Get the part of the block to resend + block = self._current_block[ackseq:] + # Go back to correct position in stream + self.pos = self.pos - (len(block) * 7) + # Reset the _current_block before starting the retransmission + self._current_block = [] + # Reset _seqno and update blksize + self._seqno = 0 + self._blksize = blksize + # We are retransmitting + self._retransmitting = True + # Resend the block + for b in block: + self.write(b) + self._retransmitting = False def close(self): """Closes the stream."""