Disabled the OTG_HS interrupts when handling the ring-buffer for the …

…USB; reset the dirty-flag straight away before sending the first write-instruction so that new data is not ignored; changed the timeout-timer to single bytes rather than whole packets; made the polling of the IMU timeout so that there is no blocking that may stop NUSense; changed the memcpy's in the USB callback to for-loops to keep the volatile quailifier.

NUSense/Core/Src/dynamixel/PacketHandler.hpp

@@ -43,20 +43,20 @@ namespace dynamixel {
         template <uint16_t N>
         const Result check_sts(const platform::NUSense::NUgus::ID id) {
 
-            // If the packet has timed out, then return early.
-            // May be better to move this to where read_result == NO_BYTE_READ, but the handler may
-            // get stuck indefinitely if phantom bytes are constantly coming in. Any ideas are
-            // welcome.
-            if (timeout_timer.has_timed_out()) {
-                return TIMEOUT;
-            }
-
             if (!packetiser.is_packet_ready()) {
                 // Peek to see if there is a byte on the buffer yet.
                 uint16_t read_result = port.read();
                 if (read_result == uart::NO_BYTE_READ) {
+                    if (timeout_timer.has_timed_out()) {
+                        // If the packet has timed out, then return early.
+                        return TIMEOUT;
+                    }
                     return NONE;
                 }
+                else {
+                    // If at least one byte has been received, then restart the timer from now on.
+                    timeout_timer.restart(1000);
+                }
 
                 // If so, then decode it.
                 packetiser.decode(read_result);
@@ -121,7 +121,7 @@ namespace dynamixel {
          */
         void begin() {
             // For now, wait for at most 500 microseconds.
-            timeout_timer.begin(3000);
+            timeout_timer.begin(1000);
         }
 
         /**

NUSense/Core/Src/imu.cpp

@@ -136,6 +136,7 @@ void NUSense::IMU::readReg(Address addr, uint8_t* data) {
 void NUSense::IMU::readBurst(Address addr, uint8_t* data, uint16_t length) {
     uint8_t packet[length + 1];
     uint8_t rx_data[length + 1];
+    HAL_StatusTypeDef status;
 
     for (int i = 0; i < length + 1; i++) {
         rx_data[i] = 0xAA;
@@ -146,11 +147,13 @@ void NUSense::IMU::readBurst(Address addr, uint8_t* data, uint16_t length) {
     }
 
     HAL_GPIO_WritePin(MPU_NSS_GPIO_Port, MPU_NSS_Pin, GPIO_PIN_RESET);
-    HAL_SPI_TransmitReceive(&hspi4, packet, rx_data, length + 1, HAL_MAX_DELAY);
+    status = HAL_SPI_TransmitReceive(&hspi4, packet, rx_data, length + 1, 1);
     HAL_GPIO_WritePin(MPU_NSS_GPIO_Port, MPU_NSS_Pin, GPIO_PIN_SET);
 
-    for (int i = 0; i < length; i++)
-        data[i] = rx_data[i + 1];
+    if (status == HAL_OK) {
+        for (int i = 0; i < length; i++)
+            data[i] = rx_data[i + 1];
+    }
 }
 
 

NUSense/Core/Src/platform/NUSense/NUSenseIO/loop.cpp

@@ -47,10 +47,6 @@ namespace platform::NUSense {
                     // for the read bank of registers.
                     case StatusState::WRITE_2_RESPONSE:
 
-                        // Reset the flag now that the two write-instructions were properly
-                        // received.
-                        servo_states[(uint8_t) current_id - 1].dirty = false;
-
                         send_servo_read_request(current_id, i);
                         status_states[(uint8_t) current_id - 1] = READ_RESPONSE;
 
@@ -70,6 +66,10 @@ namespace platform::NUSense {
 
                         // If the servo-state is dirty, then send a write-instruction.
                         if (servo_states[(uint8_t) current_id - 1].dirty) {
+
+                            // Reset the flag now that the two write-instructions have begun.
+                            servo_states[(uint8_t) current_id - 1].dirty = false;
+
                             send_servo_write_1_request(current_id, i);
                             status_states[(uint8_t) current_id - 1] = WRITE_1_RESPONSE;
                         }
@@ -91,6 +91,10 @@ namespace platform::NUSense {
 
                 // If the servo-state is dirty, then send a write-instruction.
                 if (servo_states[(uint8_t) current_id - 1].dirty) {
+
+                    // Reset the flag now that the two write-instructions have begun.
+                    servo_states[(uint8_t) current_id - 1].dirty = false;
+
                     send_servo_write_1_request(current_id, i);
                     status_states[(uint8_t) current_id - 1] = WRITE_1_RESPONSE;
                 }

NUSense/Core/Src/usb/PacketHandler.hpp

@@ -27,6 +27,7 @@ namespace usb {
         bool handle_incoming() {
 
             if (rx_buffer.size != 0) {
+                HAL_NVIC_DisableIRQ(OTG_HS_IRQn);
                 // Check if we have a header and if we do extract our lengths and pb bytes
                 if ((rx_buffer.data[rx_buffer.front] == (char) 0xE2)
                     && (rx_buffer.data[(rx_buffer.front + 1) % RX_BUF_SIZE] == (char) 0x98)
@@ -63,6 +64,7 @@ namespace usb {
                     rx_buffer.front = (rx_buffer.front + 1) % RX_BUF_SIZE;
                     rx_buffer.size--;
                 }
+                HAL_NVIC_EnableIRQ(OTG_HS_IRQn);
             }
 
             if (is_packet_ready) {

NUSense/Core/Src/utility/support/MicrosecondTimer.hpp

@@ -55,6 +55,15 @@ namespace utility::support {
             is_counting = false;
         }
 
+        /**
+         * @brief   Restarts the timer.
+         * @param   timeout the timeout in microseconds, at most 65535.
+         */
+        void restart(uint16_t timeout) {
+            stop();
+            begin(timeout);
+        }
+
         /**
          * @brief   Sees whether the timer has timed out.
          * @note    This should be kept very short.
@@ -68,7 +77,7 @@ namespace utility::support {
                 // saw-tooth wave.
                 if (!((count - threshold) & 0x8000)) {
                     // Since the timer has timed out, it is no longer counting.
-                    is_counting = false;
+                    stop();
                     return true;
                 }
                 else

NUSense/USB_DEVICE/App/usbd_cdc_if.c

@@ -266,14 +266,24 @@ static int8_t CDC_Receive_HS(uint8_t* Buf, uint32_t *Len)
 
   // Move the back backwards (higher) in the array unless there is no more room left.
   if (rx_buffer.size < RX_BUF_SIZE) {
-      // If the max buffer size is exceeded, wrap around using 2 memcpy calls
+      // If the max buffer size is exceeded, wrap around using two copies.
       if (rx_buffer.back + *Len > RX_BUF_SIZE) {
-          memcpy(&rx_buffer.data[rx_buffer.back], &Buf[0], RX_BUF_SIZE - rx_buffer.back);
-          memcpy(&rx_buffer.data[0], &Buf[RX_BUF_SIZE - rx_buffer.back], rx_buffer.back + *Len - RX_BUF_SIZE);
+          // Use a dumb for-loop since memcpy throws the volatile qualifier away which apparently 
+          // can lead to undefined behaviour.
+          for (int i = 0; i < RX_BUF_SIZE - rx_buffer.back; i++) {
+            rx_buffer.data[rx_buffer.back + i] = Buf[i];
+          }
+          for (int i = 0; i < rx_buffer.back + *Len - RX_BUF_SIZE; i++) {
+            rx_buffer.data[i] = Buf[RX_BUF_SIZE - rx_buffer.back + i];
+          }
       }
-      // If not then 1 memcpy call should suffice
+      // If not then, one copy should be enough.
       else {
-          memcpy(&rx_buffer.data[rx_buffer.back], &Buf[0], *Len);
+          // Use a dumb for-loop since memcpy throws the volatile qualifier away which apparently 
+          // can lead to undefined behaviour.
+          for (int i = 0; i < *Len; i++) {
+            rx_buffer.data[rx_buffer.back + i] = Buf[i];
+          }
       }
 
       rx_buffer.back = (rx_buffer.back + *Len) % RX_BUF_SIZE;