From d9923e2219f9bc6b621041dc849f4fdd7a1f6c13 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?=E8=B6=99=E3=80=80=E6=BC=A0=E5=B1=85=28Zhao=2C=20Moju=29?=
 <chou@jsk.imi.i.u-tokyo.ac.jp>
Date: Wed, 27 Nov 2024 14:34:05 +0900
Subject: [PATCH] [Neuron][Servo][Inialize] introduce a new variable to check
 the connnection between spinal and neuron (#635)

---
 .../neuron/neuronlib/Initializer/initializer.cpp              | 1 +
 aerial_robot_nerve/neuron/neuronlib/Servo/servo.cpp           | 3 +++
 aerial_robot_nerve/neuron/neuronlib/Servo/servo.h             | 4 ++++
 3 files changed, 8 insertions(+)

diff --git a/aerial_robot_nerve/neuron/neuronlib/Initializer/initializer.cpp b/aerial_robot_nerve/neuron/neuronlib/Initializer/initializer.cpp
index e5425f915..47d7549c5 100644
--- a/aerial_robot_nerve/neuron/neuronlib/Initializer/initializer.cpp
+++ b/aerial_robot_nerve/neuron/neuronlib/Initializer/initializer.cpp
@@ -62,6 +62,7 @@ void Initializer::receiveDataCallback(uint8_t message_id, uint32_t DLC, uint8_t*
 	case CAN::MESSAGEID_RECEIVE_INITIAL_CONFIG_REQUEST:
 	{
 		sendBoardConfig();
+		servo_.setConnect(true);
 	}
 	break;
 	case CAN::MESSAGEID_RECEIVE_BOARD_CONFIG_REQUEST:
diff --git a/aerial_robot_nerve/neuron/neuronlib/Servo/servo.cpp b/aerial_robot_nerve/neuron/neuronlib/Servo/servo.cpp
index 63fa3b5cd..5477e84e1 100644
--- a/aerial_robot_nerve/neuron/neuronlib/Servo/servo.cpp
+++ b/aerial_robot_nerve/neuron/neuronlib/Servo/servo.cpp
@@ -11,6 +11,7 @@
 void Servo::init(UART_HandleTypeDef* huart, I2C_HandleTypeDef* hi2c, osMutexId* mutex = NULL)
 {
   servo_handler_.init(huart, hi2c, mutex);
+  connect_ = false;
 }
 
 void Servo::update()
@@ -35,6 +36,8 @@ void Servo::sendData()
 
 void Servo::receiveDataCallback(uint8_t message_id, uint32_t DLC, uint8_t* data)
 {
+	if (!connect_) return;
+
 	switch (message_id) {
 		case CAN::MESSAGEID_RECEIVE_SERVO_ANGLE:
 		{
diff --git a/aerial_robot_nerve/neuron/neuronlib/Servo/servo.h b/aerial_robot_nerve/neuron/neuronlib/Servo/servo.h
index eabd39e46..9d8162bd4 100644
--- a/aerial_robot_nerve/neuron/neuronlib/Servo/servo.h
+++ b/aerial_robot_nerve/neuron/neuronlib/Servo/servo.h
@@ -24,6 +24,9 @@ class Servo : public CANDevice
 	void sendData() override;
 	void receiveDataCallback(uint8_t message_id, uint32_t DLC, uint8_t* data) override;
 
+	bool getConnect() {return connect_;}
+	void setConnect(bool connect) {connect_ = connect;}
+
 private:
 	struct CANServoData{
 		int16_t angle;
@@ -35,6 +38,7 @@ class Servo : public CANDevice
 		:angle(angle), temperature(temperature), moving(moving), current(current), error(error){}
 	};
 
+	bool connect_;
 	DynamixelSerial servo_handler_;
 	friend class Initializer;
 };