wip

tutrobo · Oct 29, 2024 · 34b58c5 · 34b58c5
1 parent 852cafe
commit 34b58c5
Show file tree

Hide file tree

Showing 5 changed files with 17 additions and 12 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -14,17 +14,16 @@ 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);
   tx_sem_ = osSemaphoreNew(1, 1, nullptr);
   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) {