diff --git a/libraries/AP_IOMCU/iofirmware/iofirmware.cpp b/libraries/AP_IOMCU/iofirmware/iofirmware.cpp
index 105ff245dc..1f354808c4 100644
--- a/libraries/AP_IOMCU/iofirmware/iofirmware.cpp
+++ b/libraries/AP_IOMCU/iofirmware/iofirmware.cpp
@@ -342,8 +342,6 @@ bool AP_IOMCU_FW::handle_code_read()
     default:
         return false;
     }
-    last_page = rx_io_packet.page;
-    last_offset = rx_io_packet.offset;
 
     /* if the offset is at or beyond the end of the page, we have no data */
     if (rx_io_packet.offset >= tx_io_packet.count) {
@@ -353,6 +351,7 @@ bool AP_IOMCU_FW::handle_code_read()
     /* correct the data pointer and count for the offset */
     values += rx_io_packet.offset;
     tx_io_packet.count -= rx_io_packet.offset;
+    tx_io_packet.count = MIN(tx_io_packet.count, rx_io_packet.count);
     memcpy(tx_io_packet.regs, values, sizeof(uint16_t)*tx_io_packet.count);
     tx_io_packet.crc = 0;
     tx_io_packet.crc =  crc_crc8((const uint8_t *)&tx_io_packet, tx_io_packet.get_size());
diff --git a/libraries/AP_IOMCU/iofirmware/iofirmware.h b/libraries/AP_IOMCU/iofirmware/iofirmware.h
index 87913a294e..57e62080ac 100644
--- a/libraries/AP_IOMCU/iofirmware/iofirmware.h
+++ b/libraries/AP_IOMCU/iofirmware/iofirmware.h
@@ -84,8 +84,6 @@ private:
     uint32_t sbus_last_ms;
     uint32_t sbus_interval_ms;
 
-    uint8_t last_page;
-    uint8_t last_offset;
     uint32_t fmu_data_received_time;
     uint32_t last_heater_ms;
     uint32_t reboot_time;