Merge RoboSub 2023 Final Firmware

examples/template/src/main.c

@@ -173,6 +173,13 @@ int main() {
     }
     #endif
 
+    #ifdef MICRO_ROS_TRANSPORT_ETH
+    if (!transport_eth_init()) {
+        // No point in continuing onwards from here, if we can't initialize ETH hardware might as well panic and retry
+        panic("Failed to initialize Ethernet hardware!");
+    }
+    #endif
+
     #ifdef MICRO_ROS_TRANSPORT_USB
     transport_usb_init();
     #endif
@@ -228,4 +235,4 @@ int main() {
     }
 
     return 0;
-}
+}

lib/CMakeLists.txt

@@ -1,5 +1,6 @@
 add_subdirectory(driver_async_i2c/)
 add_subdirectory(driver_canbus/)
+add_subdirectory(driver_depth/)
 add_subdirectory(driver_dynamixel/)
 add_subdirectory(driver_led/)
 add_subdirectory(driver_mcp3426/)

lib/driver_canbus/include/driver/canbus.h

@@ -220,4 +220,12 @@ typedef void (*canbus_utility_chan_cb_t)(uint32_t channel, uint8_t *buf, size_t
  */
 void canbus_utility_frame_register_cb(uint32_t channel, canbus_utility_chan_cb_t cb);
 
-#endif
+
+// Debug Bindings
+// TODO: Remove me
+
+void canbus_reenable_intr(void);
+void canbus_fifo_clear(void);
+void canbus_reset(void);
+
+#endif

lib/driver_canbus/src/can_interface.c

@@ -109,7 +109,10 @@ bool canbus_msg_read_available(void) {
 size_t canbus_msg_read(uint8_t *buf, size_t len) {
     assert(canbus_initialized);
 
+    uint32_t prev_interrupt = save_and_disable_mcp251Xfd_irq();
+
     if (!canbus_msg_opened || !canmore_received_msg.waiting) {
+        restore_mcp251Xfd_irq(prev_interrupt);
         return 0;
     }
 
@@ -123,6 +126,8 @@ size_t canbus_msg_read(uint8_t *buf, size_t len) {
 
     can_mcp251xfd_report_msg_rx_fifo_ready();
 
+    restore_mcp251Xfd_irq(prev_interrupt);
+
     return copy_len;
 }
 
@@ -205,7 +210,10 @@ bool canbus_utility_frame_read_available(void) {
 size_t canbus_utility_frame_read(uint32_t *channel_out, uint8_t *buf, size_t len) {
     assert(canbus_initialized);
 
+    uint32_t prev_interrupt = save_and_disable_mcp251Xfd_irq();
+
     if (!utility_rx_buf.waiting) {
+        restore_mcp251Xfd_irq(prev_interrupt);
         return 0;
     }
 
@@ -219,6 +227,8 @@ size_t canbus_utility_frame_read(uint32_t *channel_out, uint8_t *buf, size_t len
 
     can_mcp251xfd_report_utility_rx_fifo_ready();
 
+    restore_mcp251Xfd_irq(prev_interrupt);
+
     return copy_len;
 }
 
@@ -231,10 +241,6 @@ bool canbus_utility_frame_write_available(void) {
 size_t canbus_utility_frame_write(uint32_t channel, uint8_t *buf, size_t len) {
     assert(canbus_initialized);
 
-    if (utility_tx_buf.waiting) {
-        return 0;
-    }
-
     // Check channel ID
     if (channel > (1<<CANMORE_NOC_LENGTH)) {
         return 0;
@@ -248,6 +254,13 @@ size_t canbus_utility_frame_write(uint32_t channel, uint8_t *buf, size_t len) {
         return 0;
     }
 
+    uint32_t prev_interrupt = save_and_disable_mcp251Xfd_irq();
+
+    if (utility_tx_buf.waiting) {
+        restore_mcp251Xfd_irq(prev_interrupt);
+        return 0;
+    }
+
     // Copy in frame to buffer
     memcpy(utility_tx_buf.data, buf, len);
     utility_tx_buf.length = len;
@@ -262,6 +275,8 @@ size_t canbus_utility_frame_write(uint32_t channel, uint8_t *buf, size_t len) {
 
     can_mcp251xfd_report_utility_tx_fifo_ready();
 
+    restore_mcp251Xfd_irq(prev_interrupt);
+
     return len;
 }
 

lib/driver_canbus/src/can_mcp251XFD.c

@@ -293,19 +293,17 @@ MCP251XFD_Config mcp251xfd_device_config =
 };
 
 // FIFO definitions
-#define mcp251xfd_msg_tx_fifo            MCP251XFD_FIFO1
+#define mcp251xfd_tx_fifo                MCP251XFD_FIFO1        // Utility and msg tx fifos joined to ensure that if the msg fifo stalls so do heartbeats, causing the device to reset
 #define mcp251xfd_msg_rx_fifo            MCP251XFD_FIFO2
-#define mcp251xfd_utility_tx_fifo        MCP251XFD_FIFO3
-#define mcp251xfd_utility_rx_fifo        MCP251XFD_FIFO4
+#define mcp251xfd_utility_rx_fifo        MCP251XFD_FIFO3
 
 #define mcp251xfd_msg_rx_filter_num      MCP251XFD_FILTER0
 #define mcp251xfd_msg_rx_crc_filter_num  MCP251XFD_FILTER1
 #define mcp251xfd_utility_rx_filter_num  MCP251XFD_FILTER2
 
 MCP251XFD_RAMInfos mcp251xfd_transmit_event_raminfo;
-MCP251XFD_RAMInfos mcp251xfd_msg_tx_raminfo;
+MCP251XFD_RAMInfos mcp251xfd_tx_raminfo;
 MCP251XFD_RAMInfos mcp251xfd_msg_rx_raminfo;
-MCP251XFD_RAMInfos mcp251xfd_utility_tx_raminfo;
 MCP251XFD_RAMInfos mcp251xfd_utility_rx_raminfo;
 
 MCP251XFD_FIFO mcp251xfd_transmit_event_fifo_config = {
@@ -314,12 +312,12 @@ MCP251XFD_FIFO mcp251xfd_transmit_event_fifo_config = {
     .RAMInfos = &mcp251xfd_transmit_event_raminfo,
 };
 
-MCP251XFD_FIFO mcp251xfd_msg_tx_fifo_config = {
-    .Name = mcp251xfd_msg_tx_fifo, .Size = MCP251XFD_FIFO_8_MESSAGE_DEEP, .Payload = MCP251XFD_PAYLOAD_8BYTE,
+MCP251XFD_FIFO mcp251xfd_tx_fifo_config = {
+    .Name = mcp251xfd_tx_fifo, .Size = MCP251XFD_FIFO_12_MESSAGE_DEEP, .Payload = MCP251XFD_PAYLOAD_8BYTE,
     .Direction = MCP251XFD_TRANSMIT_FIFO, .Attempts = MCP251XFD_THREE_ATTEMPTS,
-    .Priority = MCP251XFD_MESSAGE_TX_PRIORITY17, .ControlFlags = MCP251XFD_FIFO_NO_RTR_RESPONSE,
+    .Priority = MCP251XFD_MESSAGE_TX_PRIORITY16, .ControlFlags = MCP251XFD_FIFO_NO_RTR_RESPONSE,
     .InterruptFlags = MCP251XFD_FIFO_TX_ATTEMPTS_EXHAUSTED_INT + MCP251XFD_FIFO_TRANSMIT_FIFO_NOT_FULL_INT,
-    .RAMInfos = &mcp251xfd_msg_tx_raminfo,
+    .RAMInfos = &mcp251xfd_tx_raminfo,
 };
 
 MCP251XFD_FIFO mcp251xfd_msg_rx_fifo_config = {
@@ -329,15 +327,6 @@ MCP251XFD_FIFO mcp251xfd_msg_rx_fifo_config = {
     .RAMInfos = &mcp251xfd_msg_rx_raminfo,
 };
 
-MCP251XFD_FIFO mcp251xfd_utility_tx_fifo_config = {
-    // NOTE: Don't notify on TX failures
-    .Name = mcp251xfd_utility_tx_fifo, .Size = MCP251XFD_FIFO_4_MESSAGE_DEEP, .Payload = UTILITY_MSG_PAYLOAD_SIZE_ENUM,
-    .Direction = MCP251XFD_TRANSMIT_FIFO, .Attempts = MCP251XFD_THREE_ATTEMPTS,
-    .Priority = MCP251XFD_MESSAGE_TX_PRIORITY16, .ControlFlags = MCP251XFD_FIFO_NO_RTR_RESPONSE,
-    .InterruptFlags = MCP251XFD_FIFO_TRANSMIT_FIFO_NOT_FULL_INT,
-    .RAMInfos = &mcp251xfd_utility_tx_raminfo,
-};
-
 MCP251XFD_FIFO mcp251xfd_utility_rx_fifo_config = {
     .Name = mcp251xfd_utility_rx_fifo, .Size = MCP251XFD_FIFO_4_MESSAGE_DEEP, .Payload = UTILITY_MSG_PAYLOAD_SIZE_ENUM,
     .Direction = MCP251XFD_RECEIVE_FIFO, .ControlFlags = MCP251XFD_FIFO_ADD_TIMESTAMP_ON_RX,
@@ -387,9 +376,9 @@ bool can_mcp251x_get_term_state(bool *term_state_out) {
 
     if (err == ERR_OK) {
         if (pin_state & MCP251XFD_GPIO0_HIGH) {
-            *term_state_out = false;
-        } else {
             *term_state_out = true;
+        } else {
+            *term_state_out = false;
         }
 
         return true;
@@ -475,15 +464,15 @@ void can_mcp251xfd_interrupt_cb(uint gpio, uint32_t events) {
         if (error_code != ERR_OK) canbus_report_driver_error(error_code);
     }
     if (active_interrupts & MCP251XFD_INT_TX_ATTEMPTS_EVENT) {
-        if (check_and_clear_fifo_event(&mcp251xfd_device, mcp251xfd_msg_tx_fifo, MCP251XFD_TX_FIFO_ATTEMPTS_EXHAUSTED)){
+        if (check_and_clear_fifo_event(&mcp251xfd_device, mcp251xfd_tx_fifo, MCP251XFD_TX_FIFO_ATTEMPTS_EXHAUSTED)){
             canbus_call_receive_error_cb(CANBUS_RECVERR_MSG_TX_FAILURE);
             interrupt_cleared = true;
         }
     }
 
     // Handle FIFO Events
     if (active_interrupts & MCP251XFD_INT_TX_EVENT) {
-        if (check_fifo_event(&mcp251xfd_device, mcp251xfd_msg_tx_fifo, MCP251XFD_TX_FIFO_NOT_FULL)) {
+        if (check_fifo_event(&mcp251xfd_device, mcp251xfd_tx_fifo, MCP251XFD_TX_FIFO_NOT_FULL)) {
             if (!canmore_msg_encode_done(&encoding_buffer)) {
                 uint8_t buffer[MCP251XFD_PAYLOAD_MAX];
                 bool is_extended;
@@ -504,16 +493,9 @@ void can_mcp251xfd_interrupt_cb(uint gpio, uint32_t events) {
                 msg.PayloadData = buffer;
 
                 interrupt_cleared = true;
-                error_code = MCP251XFD_TransmitMessageToFIFO(&mcp251xfd_device, &msg, mcp251xfd_msg_tx_fifo, true);
+                error_code = MCP251XFD_TransmitMessageToFIFO(&mcp251xfd_device, &msg, mcp251xfd_tx_fifo, true);
                 if (error_code != ERR_OK) canbus_report_driver_error(error_code);
-            } else {
-                interrupt_cleared = true;
-                set_fifo_not_full_empty_interrupt(&mcp251xfd_device, mcp251xfd_msg_tx_fifo, false);
-            }
-        }
-
-        if (check_fifo_event(&mcp251xfd_device, mcp251xfd_utility_tx_fifo, MCP251XFD_TX_FIFO_NOT_FULL)) {
-            if (utility_tx_buf.waiting) {
+            } else if (utility_tx_buf.waiting) {
                 MCP251XFD_CANMessage msg;
 
                 // Note this only holds when max utility_tx_buf.length is 8
@@ -524,13 +506,13 @@ void can_mcp251xfd_interrupt_cb(uint gpio, uint32_t events) {
                 msg.PayloadData = utility_tx_buf.data;
 
                 interrupt_cleared = true;
-                error_code = MCP251XFD_TransmitMessageToFIFO(&mcp251xfd_device, &msg, mcp251xfd_utility_tx_fifo, true);
+                error_code = MCP251XFD_TransmitMessageToFIFO(&mcp251xfd_device, &msg, mcp251xfd_tx_fifo, true);
                 if (error_code != ERR_OK) canbus_report_driver_error(error_code);
 
                 utility_tx_buf.waiting = false;
             } else {
                 interrupt_cleared = true;
-                set_fifo_not_full_empty_interrupt(&mcp251xfd_device, mcp251xfd_utility_tx_fifo, false);
+                set_fifo_not_full_empty_interrupt(&mcp251xfd_device, mcp251xfd_tx_fifo, false);
             }
         }
     }
@@ -672,7 +654,7 @@ static bool can_mcp251xfd_configure_chip(void) {
         return false;
     }
 
-    error_code = MCP251XFD_ConfigureFIFO(&mcp251xfd_device, &mcp251xfd_msg_tx_fifo_config);
+    error_code = MCP251XFD_ConfigureFIFO(&mcp251xfd_device, &mcp251xfd_tx_fifo_config);
     if (error_code != ERR_OK) {
         canbus_report_driver_error(error_code);
         return false;
@@ -684,12 +666,6 @@ static bool can_mcp251xfd_configure_chip(void) {
         return false;
     }
 
-    error_code = MCP251XFD_ConfigureFIFO(&mcp251xfd_device, &mcp251xfd_utility_tx_fifo_config);
-    if (error_code != ERR_OK) {
-        canbus_report_driver_error(error_code);
-        return false;
-    }
-
     error_code = MCP251XFD_ConfigureFIFO(&mcp251xfd_device, &mcp251xfd_utility_rx_fifo_config);
     if (error_code != ERR_OK) {
         canbus_report_driver_error(error_code);
@@ -779,11 +755,14 @@ bool can_mcp251xfd_configure(unsigned int client_id)
     return can_mcp251xfd_configure_chip();
 }
 
+static bool reset_now = false;
+
 void can_mcp251xfd_check_offline_reset(void) {
     // Check if we need to reset the chip after being offline for too long
     // Handles edge cases where the chip will randomly stop transmitting in the event of certain edge cases
-    if (absolute_time_diff_us(heartbeat_transmit_timeout, get_absolute_time()) > (CAN_MCP251XFD_OFFLINE_RESET_TIMEOUT_MS*1000)) {
+    if (absolute_time_diff_us(heartbeat_transmit_timeout, get_absolute_time()) > (CAN_MCP251XFD_OFFLINE_RESET_TIMEOUT_MS*1000) || reset_now) {
         if (can_mcp251xfd_configure_chip()) {
+            reset_now = false;
             // Only clear the heartbeat transmit timeout (but not put it in the future, marking the bus online)
             // if the initialization was successful. If not, then next time the canbus ticks this function will be called
             // and this function should retry
@@ -795,7 +774,7 @@ void can_mcp251xfd_check_offline_reset(void) {
 void can_mcp251xfd_report_msg_tx_fifo_ready(void) {
     // Ensure that readback/writing to FIFO configuration isn't interrupted
     uint32_t prev_state = save_and_disable_mcp251Xfd_irq();
-    set_fifo_not_full_empty_interrupt(&mcp251xfd_device, mcp251xfd_msg_tx_fifo, true);
+    set_fifo_not_full_empty_interrupt(&mcp251xfd_device, mcp251xfd_tx_fifo, true);
     restore_mcp251Xfd_irq(prev_state);
 }
 
@@ -809,7 +788,7 @@ void can_mcp251xfd_report_msg_rx_fifo_ready(void) {
 void can_mcp251xfd_report_utility_tx_fifo_ready(void) {
     // Ensure that readback/writing to FIFO configuration isn't interrupted
     uint32_t prev_state = save_and_disable_mcp251Xfd_irq();
-    set_fifo_not_full_empty_interrupt(&mcp251xfd_device, mcp251xfd_utility_tx_fifo, true);
+    set_fifo_not_full_empty_interrupt(&mcp251xfd_device, mcp251xfd_tx_fifo, true);
     restore_mcp251Xfd_irq(prev_state);
 }
 
@@ -830,3 +809,21 @@ uint32_t save_and_disable_mcp251Xfd_irq(void) {
 void restore_mcp251Xfd_irq(uint32_t prev_interrupt_state) {
     gpio_set_irq_enabled(CAN_MCP251XFD_EXTERNAL_INTERRUPT_PIN, CAN_MCP251XFD_EXTERNAL_INTERRUPT_EVENTS, prev_interrupt_state != 0);
 }
+
+void canbus_reenable_intr(void) {
+    uint32_t prev_state = save_and_disable_mcp251Xfd_irq();
+    set_fifo_not_full_empty_interrupt(&mcp251xfd_device, mcp251xfd_tx_fifo, true);
+    set_fifo_not_full_empty_interrupt(&mcp251xfd_device, mcp251xfd_msg_rx_fifo, true);
+    set_fifo_not_full_empty_interrupt(&mcp251xfd_device, mcp251xfd_utility_rx_fifo, true);
+    restore_mcp251Xfd_irq(prev_state);
+}
+
+void canbus_fifo_clear(void) {
+    uint32_t prev_state = save_and_disable_mcp251Xfd_irq();
+    MCP251XFD_ResetFIFO(&mcp251xfd_device, mcp251xfd_msg_rx_fifo);
+    restore_mcp251Xfd_irq(prev_state);
+}
+
+void canbus_reset(void) {
+    reset_now = true;
+}

lib/driver_depth/CMakeLists.txt

@@ -0,0 +1,15 @@
+add_library(driver_depth INTERFACE)
+
+target_sources(driver_depth INTERFACE
+    ${CMAKE_CURRENT_LIST_DIR}/src/depth.c
+    ${CMAKE_CURRENT_LIST_DIR}/src/ms5837.c
+)
+
+target_include_directories(driver_depth INTERFACE
+    ${CMAKE_CURRENT_LIST_DIR}/include
+)
+
+target_link_libraries(driver_depth INTERFACE
+    driver_async_i2c
+    titan_logger
+)