From 34b58c5acd6035ab5a8de520ccf506905a301dcb Mon Sep 17 00:00:00 2001
From: Rin Iwai <rin@eyrin.jp>
Date: Tue, 29 Oct 2024 17:44:52 +0900
Subject: [PATCH] wip

---
 include/tutrc_harurobo_lib/bno055.hpp | 8 +++++---
 include/tutrc_harurobo_lib/c610.hpp   | 5 ++++-
 include/tutrc_harurobo_lib/uart.hpp   | 2 +-
 src/fdcan.cpp                         | 7 ++++---
 src/uart.cpp                          | 7 +++----
 5 files changed, 17 insertions(+), 12 deletions(-)

diff --git a/include/tutrc_harurobo_lib/bno055.hpp b/include/tutrc_harurobo_lib/bno055.hpp
index afdcb95..5669907 100644
--- a/include/tutrc_harurobo_lib/bno055.hpp
+++ b/include/tutrc_harurobo_lib/bno055.hpp
@@ -11,7 +11,8 @@ namespace tutrc_harurobo_lib {
 
 class BNO055 {
 public:
-  BNO055(UART *uart) : uart_(uart) {
+  bool init(UART *uart) {
+    uart_ = uart;
     uint32_t start = osKernelGetTickCount();
     while (osKernelGetTickCount() - start < 500) {
       uint8_t data = 0x00;
@@ -26,12 +27,13 @@ class BNO055 {
       if (!write_reg(0x3D, &data, 1)) {
         continue;
       }
-      break;
+      return true;
     }
+    return false;
   }
 
   void update() {
-    static std::array<int16_t, 3> data;
+    std::array<int16_t, 3> data;
     if (read_reg(0x1A, reinterpret_cast<uint8_t *>(data.data()), 6)) {
       euler_x_ = data[0] / 900.0f;
       euler_y_ = data[1] / 900.0f;
diff --git a/include/tutrc_harurobo_lib/c610.hpp b/include/tutrc_harurobo_lib/c610.hpp
index 57c0dbd..dfd67aa 100644
--- a/include/tutrc_harurobo_lib/c610.hpp
+++ b/include/tutrc_harurobo_lib/c610.hpp
@@ -56,7 +56,10 @@ class C610 {
     ID8,
   };
 
-  bool init(CANBase *can) { can_ = can; }
+  bool init(CANBase *can) {
+    can_ = can;
+    return true;
+  }
 
   void update() {
     CANMessage msg;
diff --git a/include/tutrc_harurobo_lib/uart.hpp b/include/tutrc_harurobo_lib/uart.hpp
index 72f2362..6e8758b 100644
--- a/include/tutrc_harurobo_lib/uart.hpp
+++ b/include/tutrc_harurobo_lib/uart.hpp
@@ -60,7 +60,7 @@ class UART {
    * @param huart UART_HandleTypeDefへのポインタ
    * @param rx_queue_size 受信バッファサイズ
    */
-  UART(UART_HandleTypeDef *huart, size_t rx_queue_size = 64);
+  bool init(UART_HandleTypeDef *huart, size_t rx_queue_size = 64);
 
   /**
    * UART送信
diff --git a/src/fdcan.cpp b/src/fdcan.cpp
index 896e1fb..0e29f2b 100644
--- a/src/fdcan.cpp
+++ b/src/fdcan.cpp
@@ -17,17 +17,18 @@ bool FDCAN::init(FDCAN_HandleTypeDef *hfdcan, size_t rx_queue_size) {
   rx_queue_ = osMessageQueueNew(rx_queue_size, sizeof(CANMessage), nullptr);
 
   if (hfdcan_->State != HAL_FDCAN_STATE_READY) {
-    Error_Handler();
+    return false;
   }
 
   if (HAL_FDCAN_ActivateNotification(hfdcan_, FDCAN_IT_RX_FIFO0_NEW_MESSAGE,
                                      0) != HAL_OK) {
-    Error_Handler();
+    return false;
   }
 
   if (HAL_FDCAN_Start(hfdcan_) != HAL_OK) {
-    Error_Handler();
+    return false;
   }
+  return true;
 }
 
 bool FDCAN::transmit(const CANMessage *msg) {
diff --git a/src/uart.cpp b/src/uart.cpp
index c7caa34..4ca5aa1 100644
--- a/src/uart.cpp
+++ b/src/uart.cpp
@@ -14,7 +14,8 @@ namespace tutrc_harurobo_lib {
 std::unordered_map<UART_HandleTypeDef *, UART *> UART::instances_;
 UART *UART::uart_printf_ = nullptr;
 
-UART::UART(UART_HandleTypeDef *huart, size_t rx_queue_size) : huart_(huart) {
+bool UART::init(UART_HandleTypeDef *huart, size_t rx_queue_size) {
+  huart_ = huart;
   instances_[huart_] = this;
   tx_mutex_ = osMutexNew(nullptr);
   rx_mutex_ = osMutexNew(nullptr);
@@ -22,9 +23,7 @@ UART::UART(UART_HandleTypeDef *huart, size_t rx_queue_size) : huart_(huart) {
   rx_sem_ = osSemaphoreNew(1, 1, nullptr);
   rx_queue_ = osMessageQueueNew(rx_queue_size, sizeof(uint8_t), nullptr);
 
-  if (HAL_UART_Receive_IT(huart_, &rx_buf_, 1) != HAL_OK) {
-    Error_Handler();
-  }
+  return HAL_UART_Receive_IT(huart_, &rx_buf_, 1) == HAL_OK;
 }
 
 bool UART::transmit(const uint8_t *data, size_t size) {