[Spinal][Neuron] refactor the initialization/reinitialization process (…

…#640) * [Neuron][Servo] forbid spinal to set servo torque if connection between spinal and neuron is not established * [Spinal][Neuron][Reboot] recall config reqeust to the reboot neuron to reconect between spinal and neuron
jsk-ros-pkg · Dec 1, 2024 · ab2ae0d · ab2ae0d
1 parent 6e3219b
commit ab2ae0d
Show file tree

Hide file tree

Showing 4 changed files with 27 additions and 4 deletions.
diff --git a/aerial_robot_nerve/neuron/neuronlib/Servo/servo.cpp b/aerial_robot_nerve/neuron/neuronlib/Servo/servo.cpp
@@ -27,7 +27,7 @@ void Servo::sendData()
 			CANServoData data(static_cast<int16_t>(s.getPresentPosition()),
 							  s.present_temp_,
 							  s.moving_,
-							  s.force_servo_off_,
+							  !connect_ || s.force_servo_off_,
 							  s.present_current_,
 							  s.hardware_error_status_);
 			sendMessage(CAN::MESSAGEID_SEND_SERVO_LIST[i], m_slave_id, 8, reinterpret_cast<uint8_t*>(&data), 1);

diff --git a/...l_robot_nerve/spinal/mcu_project/lib/Hydrus_Lib/CANDevice/initializer/can_initializer.cpp b/...l_robot_nerve/spinal/mcu_project/lib/Hydrus_Lib/CANDevice/initializer/can_initializer.cpp
@@ -9,7 +9,16 @@
 
 void CANInitializer::sendData()
 {
-	return;
+  // reconnection with reboot neuron
+  if (reboot_id_ > 0) {
+
+    // wait for the neuron completely reboot, 5000ms
+    if (HAL_GetTick() - reboot_time_ > 5000) {
+      requestConfig(reboot_id_);
+      reboot_time_ = 0;
+      reboot_id_ = 0;
+    }
+  }
 }
 
 void CANInitializer::initDevices()
@@ -18,11 +27,17 @@ void CANInitializer::initDevices()
 	HAL_Delay(200);
 	std::sort(neuron_.begin(), neuron_.end());
 	for (unsigned int i = 0; i < neuron_.size(); i++) {
-		sendMessage(CAN::MESSAGEID_RECEIVE_INITIAL_CONFIG_REQUEST, neuron_.at(i).getSlaveId(), 0, nullptr, 1);
+		uint8_t slave_id = neuron_.at(i).getSlaveId();
+		requestConfig(slave_id);
 		HAL_Delay(200);
 	}
 }
 
+void CANInitializer::requestConfig(uint8_t slave_id)
+{
+	sendMessage(CAN::MESSAGEID_RECEIVE_INITIAL_CONFIG_REQUEST, slave_id, 0, nullptr, 1);
+}
+
 void CANInitializer::configDevice(const spinal::SetBoardConfig::Request& req)
 {
 	uint8_t slave_id = static_cast<uint8_t>(req.data[0]);
@@ -144,6 +159,8 @@ void CANInitializer::configDevice(const spinal::SetBoardConfig::Request& req)
 			uint8_t send_data[1];
 			send_data[0] = CAN::BOARD_CONFIG_REBOOT;
 			sendMessage(CAN::MESSAGEID_RECEIVE_BOARD_CONFIG_REQUEST, slave_id, 1, send_data, 1);
+			reboot_id_ = slave_id;
+			reboot_time_ = HAL_GetTick();
 			break;
 		}
 		case spinal::SetBoardConfig::Request::SET_DYNAMIXEL_TTL_RS485_MIXED:

diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Hydrus_Lib/CANDevice/initializer/can_initializer.h b/aerial_robot_nerve/spinal/mcu_project/lib/Hydrus_Lib/CANDevice/initializer/can_initializer.h
@@ -18,9 +18,13 @@ class CANInitializer : public CANDevice
 {
 private:
 	std::vector<Neuron>& neuron_;
+	uint8_t reboot_id_;
+	uint32_t reboot_time_;
+
 public:
-	CANInitializer(std::vector<Neuron>& neuron):CANDevice(CAN::DEVICEID_INITIALIZER, CAN::MASTER_ID), neuron_(neuron){}
+	CANInitializer(std::vector<Neuron>& neuron):CANDevice(CAN::DEVICEID_INITIALIZER, CAN::MASTER_ID), neuron_(neuron), reboot_id_(0), reboot_time_(0){}
 	void initDevices();
+	void requestConfig(uint8_t slave_id);
 	void configDevice(const spinal::SetBoardConfig::Request& req);
 	void sendData() override;
 	void receiveDataCallback(uint8_t slave_id, uint8_t message_id, uint32_t DLC, uint8_t* data) override;

diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Hydrus_Lib/Spine/spine.cpp b/aerial_robot_nerve/spinal/mcu_project/lib/Hydrus_Lib/Spine/spine.cpp
@@ -228,6 +228,8 @@ namespace Spine
         if (send_board_index == slave_num_) send_board_index = 0;
       }
     }
+
+    can_initializer_.sendData(); // if necessary
   }
 
   void update(void)
-Original file line number
+Diff line change
@@ Expand Up / @@ -228,6 +228,8 @@ namespace Spine @@
             if (send_board_index == slave_num_) send_board_index = 0;
           }
         }
+        can_initializer_.sendData(); // if necessary
       }
       void update(void)
@@ Expand Down @@