diff --git a/aerial_robot_nerve/spinal/CMakeLists.txt b/aerial_robot_nerve/spinal/CMakeLists.txt
index ed8b22e0b..e1393a972 100644
--- a/aerial_robot_nerve/spinal/CMakeLists.txt
+++ b/aerial_robot_nerve/spinal/CMakeLists.txt
@@ -44,6 +44,8 @@ add_message_files(
   FlightConfigCmd.msg
   Vector3Int16.msg
   TorqueAllocationMatrixInv.msg
+  XiaomiCybergearCmd.msg
+  XiaomiCybergearState.msg
   )
 
 add_service_files(
diff --git a/aerial_robot_nerve/spinal/mcu_project/boards/stm32H7/Src/main.c b/aerial_robot_nerve/spinal/mcu_project/boards/stm32H7/Src/main.c
index 5821159b6..7fae7ce15 100644
--- a/aerial_robot_nerve/spinal/mcu_project/boards/stm32H7/Src/main.c
+++ b/aerial_robot_nerve/spinal/mcu_project/boards/stm32H7/Src/main.c
@@ -42,6 +42,8 @@
 #include "sensors/gps/gps_ublox.h"
 #include "sensors/encoder/mag_encoder.h"
 
+#include "actuators/xiaomi_cybergear/servo.h"
+
 #include "battery_status/battery_status.h"
 
 #include "state_estimate/state_estimate.h"
@@ -110,6 +112,8 @@ Baro baro_;
 GPS gps_;
 BatteryStatus battery_status_;
 
+/* actuators */
+Xiaomi_Cybergear::Servo xiaomi_servo_;
 
 StateEstimate estimator_;
 FlightControl controller_;
@@ -239,6 +243,7 @@ int main(void)
   baro_.init(&hi2c1, &nh_, BAROCS_GPIO_Port, BAROCS_Pin);
   gps_.init(&huart3, &nh_, LED2_GPIO_Port, LED2_Pin);
   battery_status_.init(&hadc1, &nh_);
+  xiaomi_servo_.init(&hfdcan1, &canMsgMailHandle, &nh_, LED1_GPIO_Port, LED1_Pin);
   estimator_.init(&imu_, &baro_, &gps_, &nh_);  // imu + baro + gps => att + alt + pos(xy)
   controller_.init(&htim1, &htim4, &estimator_, &battery_status_, &nh_, &flightControlMutexHandle);
 
@@ -375,6 +380,7 @@ void SystemClock_Config(void)
   RCC_OscInitStruct.PLL.PLLM = 1;
   RCC_OscInitStruct.PLL.PLLN = 100;
   RCC_OscInitStruct.PLL.PLLP = 2;
+
   RCC_OscInitStruct.PLL.PLLQ = 4;
   RCC_OscInitStruct.PLL.PLLR = 2;
   RCC_OscInitStruct.PLL.PLLRGE = RCC_PLL1VCIRANGE_3;
@@ -520,8 +526,8 @@ static void MX_FDCAN1_Init(void)
   hfdcan1.Init.DataTimeSeg1 = 7;
   hfdcan1.Init.DataTimeSeg2 = 2;
   hfdcan1.Init.MessageRAMOffset = 0;
-  hfdcan1.Init.StdFiltersNbr = 1;
-  hfdcan1.Init.ExtFiltersNbr = 0;
+  hfdcan1.Init.StdFiltersNbr = 0;
+  hfdcan1.Init.ExtFiltersNbr = 1;
   hfdcan1.Init.RxFifo0ElmtsNbr = 1;
   hfdcan1.Init.RxFifo0ElmtSize = FDCAN_DATA_BYTES_64;
   hfdcan1.Init.RxFifo1ElmtsNbr = 0;
@@ -1073,6 +1079,8 @@ void coreTaskFunc(void const * argument)
       estimator_.update();
       controller_.update();
 
+      xiaomi_servo_.update();
+
 #if NERVE_COMM      
       Spine::update();
 #endif
diff --git a/aerial_robot_nerve/spinal/mcu_project/boards/stm32H7/spinal.ioc b/aerial_robot_nerve/spinal/mcu_project/boards/stm32H7/spinal.ioc
index 00842a327..9dde47018 100644
--- a/aerial_robot_nerve/spinal/mcu_project/boards/stm32H7/spinal.ioc
+++ b/aerial_robot_nerve/spinal/mcu_project/boards/stm32H7/spinal.ioc
@@ -107,8 +107,9 @@ FDCAN1.AutoRetransmission=ENABLE
 FDCAN1.DataSyncJumpWidth=2
 FDCAN1.DataTimeSeg1=7
 FDCAN1.DataTimeSeg2=2
+FDCAN1.ExtFiltersNbr=1
 FDCAN1.FrameFormat=FDCAN_FRAME_FD_BRS
-FDCAN1.IPParameters=FrameFormat,Mode,AutoRetransmission,NominalSyncJumpWidth,NominalTimeSeg1,NominalTimeSeg2,DataSyncJumpWidth,DataTimeSeg1,DataTimeSeg2,MessageRAMOffset,StdFiltersNbr,RxFifo0ElmtsNbr,RxFifo0ElmtSize,TxElmtSize,TxFifoQueueElmtsNbr
+FDCAN1.IPParameters=FrameFormat,Mode,AutoRetransmission,NominalSyncJumpWidth,NominalTimeSeg1,NominalTimeSeg2,DataSyncJumpWidth,DataTimeSeg1,DataTimeSeg2,MessageRAMOffset,StdFiltersNbr,RxFifo0ElmtsNbr,RxFifo0ElmtSize,TxElmtSize,TxFifoQueueElmtsNbr,ExtFiltersNbr
 FDCAN1.MessageRAMOffset=0
 FDCAN1.Mode=FDCAN_MODE_NORMAL
 FDCAN1.NominalSyncJumpWidth=16
diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/CAN/can_core.cpp b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/CAN/can_core.cpp
index 59bf0b735..c96709124 100644
--- a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/CAN/can_core.cpp
+++ b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/CAN/can_core.cpp
@@ -78,14 +78,26 @@ namespace CAN {
   {
     hfdcan_ = hfdcan;
 
-    FDCAN_FilterTypeDef sFilterConfig;
-    sFilterConfig.IdType = FDCAN_STANDARD_ID;
-    sFilterConfig.FilterIndex = 0;
-    sFilterConfig.FilterType = FDCAN_FILTER_MASK;
-    sFilterConfig.FilterConfig = FDCAN_FILTER_TO_RXFIFO0;
-    sFilterConfig.FilterID1 = 0x000;
-    sFilterConfig.FilterID2 = 0x000;
-    HAL_FDCAN_ConfigFilter(hfdcan_, &sFilterConfig);
+    // /* message filter for message with standard id */
+    // FDCAN_FilterTypeDef sFilterConfig;
+    // sFilterConfig.IdType = FDCAN_STANDARD_ID;
+    // sFilterConfig.FilterIndex = 0;
+    // sFilterConfig.FilterType = FDCAN_FILTER_MASK;
+    // sFilterConfig.FilterConfig = FDCAN_FILTER_TO_RXFIFO0;
+    // sFilterConfig.FilterID1 = 0x000;
+    // sFilterConfig.FilterID2 = 0x000;
+    // HAL_FDCAN_ConfigFilter(hfdcan_, &sFilterConfig);
+
+    /* message filter for message with extended id */
+    FDCAN_FilterTypeDef eFilterConfig;
+    eFilterConfig.IdType = FDCAN_EXTENDED_ID;
+    eFilterConfig.FilterIndex = 0;
+    eFilterConfig.FilterType = FDCAN_FILTER_MASK;
+    eFilterConfig.FilterConfig = FDCAN_FILTER_TO_RXFIFO0;
+    eFilterConfig.FilterID1 = 0x000;
+    eFilterConfig.FilterID2 = 0x000;
+    HAL_FDCAN_ConfigFilter(hfdcan_, &eFilterConfig);
+
     /*
        Configure global filter:
        - Reject all remote frames with STD and EXT ID
@@ -118,7 +130,19 @@ namespace CAN {
 
   void sendMessage(uint8_t device_id, uint8_t message_id, uint8_t slave_id, uint32_t dlc, uint8_t* data, uint32_t timeout)
   {
-    tx_header_.Identifier = (((device_id & ((1 << DEVICE_ID_LEN) - 1))  << (MESSAGE_ID_LEN + SLAVE_ID_LEN))) | ((message_id & ((1 << MESSAGE_ID_LEN) - 1)) << SLAVE_ID_LEN) | (slave_id & ((1 << SLAVE_ID_LEN) - 1));
+    uint32_t identifier = (((device_id & ((1 << DEVICE_ID_LEN) - 1))  << (MESSAGE_ID_LEN + SLAVE_ID_LEN))) | ((message_id & ((1 << MESSAGE_ID_LEN) - 1)) << SLAVE_ID_LEN) | (slave_id & ((1 << SLAVE_ID_LEN) - 1));
+
+    sendMessage(identifier, dlc, data, timeout);
+  }
+
+  void sendMessage(uint32_t identifier, uint32_t dlc, uint8_t* data, uint32_t timeout, bool is_extended_id)
+  {
+    tx_header_.Identifier = identifier;
+
+    if(is_extended_id) // id length
+      tx_header_.IdType = FDCAN_EXTENDED_ID;
+    else
+      tx_header_.IdType = FDCAN_STANDARD_ID;
 
     if (dlc <= 8) { // calssic  model
       tx_header_.FDFormat = FDCAN_CLASSIC_CAN;
diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/CAN/can_core.h b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/CAN/can_core.h
index 0323af063..6631d8a9c 100644
--- a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/CAN/can_core.h
+++ b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/CAN/can_core.h
@@ -31,6 +31,10 @@ namespace CAN {
     HAL_CAN_ActivateNotification(getHcanInstance(), CAN_IT_RX_FIFO1_MSG_PENDING);
   }
 
+  inline uint32_t getIdentifier(CAN_RxHeaderTypeDef rx_header) {
+    return rx_header.StdId;
+  }
+
   inline uint8_t getDeviceId(CAN_RxHeaderTypeDef rx_header) {
     return static_cast<uint8_t>(((rx_header.StdId) >> (MESSAGE_ID_LEN + SLAVE_ID_LEN)) & ((1 << DEVICE_ID_LEN) - 1));
   }
@@ -59,6 +63,7 @@ namespace CAN {
 namespace CAN {
   void init(FDCAN_HandleTypeDef* hfdcan);
   FDCAN_HandleTypeDef* getHcanInstance();
+  void sendMessage(uint32_t identifier, uint32_t DLC, uint8_t* data, uint32_t timeout, bool is_extended_id=false);
   void sendMessage(uint8_t device_id, uint8_t message_id, uint8_t slave_id, uint32_t DLC, uint8_t* data, uint32_t timeout);
 
   inline void CAN_START()
@@ -67,6 +72,10 @@ namespace CAN {
     HAL_FDCAN_ActivateNotification(getHcanInstance(), FDCAN_IT_RX_FIFO0_NEW_MESSAGE, 0);
   }
 
+  inline uint32_t getIdentifier(FDCAN_RxHeaderTypeDef rx_header) {
+    return rx_header.Identifier;
+  }
+
   inline uint8_t getDeviceId(FDCAN_RxHeaderTypeDef rx_header) {
     return static_cast<uint8_t>(((rx_header.Identifier) >> (MESSAGE_ID_LEN + SLAVE_ID_LEN)) & ((1 << DEVICE_ID_LEN) - 1));
   }
diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/CAN/can_device.h b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/CAN/can_device.h
index d941d652d..6e8eae2c3 100644
--- a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/CAN/can_device.h
+++ b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/CAN/can_device.h
@@ -31,4 +31,18 @@ class CANDevice
 };
 
 
+
+class CANDirectDevice
+{
+protected:
+	uint32_t m_identifier;
+public:
+	CANDirectDevice(){}
+	CANDirectDevice(uint32_t identifier):m_identifier(identifier){}
+	void sendMessage(uint32_t identifier, uint32_t dlc, uint8_t* data, uint32_t timeout, bool is_extended_id=false){CAN::sendMessage(identifier, dlc, data, timeout, is_extended_id);}
+	virtual void sendData() = 0;
+	virtual void receiveDataCallback(uint32_t identifier, uint32_t dlc, uint8_t* data) = 0;
+};
+
+
 #endif /* APPLICATION_CAN_CAN_DEVICE_H_ */
diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/CAN/can_device_manager.cpp b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/CAN/can_device_manager.cpp
index c5206a775..d7fc1ad78 100644
--- a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/CAN/can_device_manager.cpp
+++ b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/CAN/can_device_manager.cpp
@@ -25,6 +25,8 @@ namespace CANDeviceManager
     GPIO_TypeDef* m_GPIOx;
     uint16_t m_GPIO_Pin;
     osMailQId* canMsgMailHandle = NULL;
+
+    CANDirectDevice* can_direct_device = NULL;
   }
 
   void init(CAN_GeranlHandleTypeDef* hcan, GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin)
@@ -49,6 +51,11 @@ namespace CANDeviceManager
     can_device_list.insert(std::pair<int, CANDevice& >(makeCommunicationId(device.getDeviceId(), device.getSlaveId()), device));
   }
 
+  void addDirectDevice(CANDirectDevice* device)
+  {
+    can_direct_device = device;
+  }
+
   void tick(int cycle /* ms */)
   {
     can_timeout_count++;
@@ -77,10 +84,17 @@ namespace CANDeviceManager
 
   void receiveMessage(can_msg msg)
   {
+    uint32_t identifier = CAN::getIdentifier(msg.rx_header);
+    uint32_t dlc = CAN::getDlc(msg.rx_header);
+
+    if (can_direct_device != NULL) {
+      can_direct_device->receiveDataCallback(identifier, dlc, msg.rx_data);
+      return;
+    }
+
     uint8_t slave_id = CAN::getSlaveId(msg.rx_header);
     uint8_t device_id = CAN::getDeviceId(msg.rx_header);
     uint8_t message_id = CAN::getMessageId(msg.rx_header);
-    uint32_t dlc = CAN::getDlc(msg.rx_header);
 
     int communication_id = CANDeviceManager::makeCommunicationId(device_id, slave_id);
     if (device_id == CAN::DEVICEID_INITIALIZER) { //special
diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/CAN/can_device_manager.h b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/CAN/can_device_manager.h
index 90228a3b9..d2fd61c5f 100644
--- a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/CAN/can_device_manager.h
+++ b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/CAN/can_device_manager.h
@@ -37,6 +37,7 @@ namespace CANDeviceManager
   void init(FDCAN_HandleTypeDef* hcan, GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin);
 #endif
   void addDevice(CANDevice& device);
+  void addDirectDevice(CANDirectDevice* device);
   void useRTOS(osMailQId* handle);
   void tick(int cycle /* ms */);
   bool connected();
diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/actuators/xiaomi_cybergear/cybergear_driver.cpp b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/actuators/xiaomi_cybergear/cybergear_driver.cpp
new file mode 100644
index 000000000..f5ae8d1c3
--- /dev/null
+++ b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/actuators/xiaomi_cybergear/cybergear_driver.cpp
@@ -0,0 +1,114 @@
+#include "servo.h"
+#include "cybergear_driver_defs.hh"
+
+using namespace Xiaomi_Cybergear;
+
+void Servo::init_motor(uint8_t run_mode)
+{
+  reset_motor();
+  set_run_mode(run_mode);
+}
+
+void Servo::enable_motor()
+{
+  uint8_t data[8] = {0x00};
+  send_command(motor_can_id_, CMD_ENABLE, master_can_id_, 8, data);
+}
+
+void Servo::reset_motor()
+{
+  uint8_t data[8] = {0x00};
+  send_command(motor_can_id_, CMD_RESET, master_can_id_, 8, data);
+}
+
+void Servo::set_run_mode(uint8_t run_mode)
+{
+  // set class variable
+  uint8_t data[8] = {0x00};
+  data[0] = ADDR_RUN_MODE & 0x00FF;
+  data[1] = ADDR_RUN_MODE >> 8;
+  data[4] = run_mode;
+  send_command(motor_can_id_, CMD_RAM_WRITE, master_can_id_, 8, data);
+}
+
+void Servo::motor_control(float position, float speed, float torque, float kp, float kd)
+{
+  uint8_t data[8] = {0x00};
+  data[0] = float_to_uint(position, P_MIN, P_MAX, 16) >> 8;
+  data[1] = float_to_uint(position, P_MIN, P_MAX, 16);
+  data[2] = float_to_uint(speed, V_MIN, V_MAX, 16) >> 8;
+  data[3] = float_to_uint(speed, V_MIN, V_MAX, 16);
+  data[4] = float_to_uint(kp, KP_MIN, KP_MAX, 16) >> 8;
+  data[5] = float_to_uint(kp, KP_MIN, KP_MAX, 16);
+  data[6] = float_to_uint(kd, KD_MIN, KD_MAX, 16) >> 8;
+  data[7] = float_to_uint(kd, KD_MIN, KD_MAX, 16);
+
+  uint16_t data_torque = float_to_uint(torque, T_MIN, T_MAX, 16);
+  send_command(motor_can_id_, CMD_POSITION, data_torque, 8, data);
+}
+
+void Servo::set_mech_position_to_zero()
+{
+  uint8_t data[8] = {0x00};
+  data[0] = 0x01;
+  send_command(motor_can_id_, CMD_SET_MECH_POSITION_TO_ZERO, master_can_id_, 8, data);
+}
+
+void Servo::read_ram_data(uint16_t index)
+{
+  uint8_t data[8] = {0x00};
+  memcpy(&data[0], &index, 2);
+  send_command(motor_can_id_, CMD_RAM_READ, master_can_id_, 8, data);
+}
+
+void Servo::write_float_data(uint8_t can_id, uint16_t addr, float value, float min, float max)
+{
+  uint8_t data[8] = {0x00};
+  data[0] = addr & 0x00FF;
+  data[1] = addr >> 8;
+
+  float val = (max < value) ? max : value;
+  val = (min > value) ? min : value;
+  memcpy(&data[4], &value, 4);
+  send_command(can_id, CMD_RAM_WRITE, master_can_id_, 8, data);
+}
+
+int Servo::float_to_uint(float x, float x_min, float x_max, int bits)
+{
+  float span = x_max - x_min;
+  float offset = x_min;
+  if (x > x_max)
+    x = x_max;
+  else if (x < x_min)
+    x = x_min;
+  return (int)((x - offset) * ((float)((1 << bits) - 1)) / span);
+}
+
+float Servo::uint_to_float(uint16_t x, float x_min, float x_max)
+{
+  uint16_t type_max = 0xFFFF;
+  float span = x_max - x_min;
+  return (float)x / type_max * span + x_min;
+}
+
+uint32_t Servo::makeIdentifier(uint8_t can_id, uint8_t cmd_id, uint16_t option)
+{
+  uint32_t id = cmd_id << 24 | option << 8 | can_id;
+  return id;
+}
+
+void Servo::process_motor_packet(const uint8_t * data)
+{
+  motor_status_.raw_position = data[1] | data[0] << 8;
+  motor_status_.raw_velocity = data[3] | data[2] << 8;
+  motor_status_.raw_effort = data[5] | data[4] << 8;
+  motor_status_.raw_temperature = data[7] | data[6] << 8;
+
+  // convert motor data
+  motor_status_.motor_id = motor_can_id_;
+  motor_status_.position = uint_to_float(motor_status_.raw_position, P_MIN, P_MAX);
+  motor_status_.velocity = uint_to_float(motor_status_.raw_velocity, V_MIN, V_MAX);
+  motor_status_.effort = uint_to_float(motor_status_.raw_effort, T_MIN, T_MAX);
+  motor_status_.temperature = motor_status_.raw_temperature / 10.0;
+}
+
diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/actuators/xiaomi_cybergear/cybergear_driver_defs.hh b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/actuators/xiaomi_cybergear/cybergear_driver_defs.hh
new file mode 100644
index 000000000..a5f1faa69
--- /dev/null
+++ b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/actuators/xiaomi_cybergear/cybergear_driver_defs.hh
@@ -0,0 +1,93 @@
+#ifndef CYBER_GEAR_DRIVER_DEFS_H
+#define CYBER_GEAR_DRIVER_DEFS_H
+
+#define CMD_CONTROL 0
+#define CMD_POSITION 1
+#define CMD_RESPONSE 2
+#define CMD_ENABLE 3
+#define CMD_RESET 4
+#define CMD_SET_MECH_POSITION_TO_ZERO 6
+#define CMD_CHANGE_CAN_ID 7
+#define CMD_RAM_READ 17
+#define CMD_RAM_WRITE 18
+#define CMD_GET_MOTOR_FAIL 21
+
+#define ADDR_RUN_MODE 0x7005
+#define ADDR_IQ_REF 0x7006
+#define ADDR_SPEED_REF 0x700A
+#define ADDR_LIMIT_TORQUE 0x700B
+#define ADDR_CURRENT_KP 0x7010
+#define ADDR_CURRENT_KI 0x7011
+#define ADDR_CURRENT_FILTER_GAIN 0x7014
+#define ADDR_LOC_REF 0x7016
+#define ADDR_LIMIT_SPEED 0x7017
+#define ADDR_LIMIT_CURRENT 0x7018
+#define ADDR_MECH_POS 0x7019
+#define ADDR_IQF 0x701A
+#define ADDR_MECH_VEL 0x701B
+#define ADDR_VBUS 0x701C
+#define ADDR_ROTATION 0x701D
+#define ADDR_LOC_KP 0x701E
+#define ADDR_SPD_KP 0x701F
+#define ADDR_SPD_KI 0x7020
+
+#define MODE_MOTION 0x00
+#define MODE_POSITION 0x01
+#define MODE_SPEED 0x02
+#define MODE_CURRENT 0x03
+
+#define P_MIN -12.5f
+#define P_MAX 12.5f
+#define V_MIN -30.0f
+#define V_MAX 30.0f
+#define KP_MIN 0.0f
+#define KP_MAX 500.0f
+#define KD_MIN 0.0f
+#define KD_MAX 5.0f
+#define T_MIN -12.0f
+#define T_MAX 12.0f
+#define IQ_MIN -27.0f
+#define IQ_MAX 27.0f
+#define CURRENT_FILTER_GAIN_MIN 0.0f
+#define CURRENT_FILTER_GAIN_MAX 1.0f
+
+#define IQ_REF_MAX 23.0f
+#define IQ_REF_MIN -23.0f
+#define SPD_REF_MAX 30.0f
+#define SPD_REF_MIN -30.0f
+#define LIMIT_TORQUE_MAX 12.0f
+#define LIMIT_TORQUE_MIN 0.0f
+#define CUR_KP_MAX 200.0f
+#define CUR_KP_MIN 0.0f
+#define CUR_KI_MAX 200.0f
+#define CUR_KI_MIN 0.0f
+#define LOC_KP_MAX 200.0f
+#define LOC_KP_MIN 0.0f
+#define SPD_KP_MAX 200.0f
+#define SPD_KP_MIN 0.0f
+#define LIMIT_SPD_MAX 30.0f
+#define LIMIT_SPD_MIN 0.0f
+#define LIMIT_CURRENT_MAX 27.0f
+#define LIMIT_CURRENT_MIN 0.0f
+
+#define DEFAULT_CURRENT_KP 0.125f
+#define DEFAULT_CURRENT_KI 0.0158f
+#define DEFAULT_CURRENT_FINTER_GAIN 0.1f
+#define DEFAULT_POSITION_KP 30.0f
+#define DEFAULT_VELOCITY_KP 2.0f
+#define DEFAULT_VELOCITY_KI 0.002f
+#define DEFAULT_VELOCITY_LIMIT 2.0f
+#define DEFAULT_CURRENT_LIMIT 27.0f
+#define DEFAULT_TORQUE_LIMIT 12.0f
+
+#define RET_CYBERGEAR_OK 0x00
+#define RET_CYBERGEAR_MSG_NOT_AVAIL 0x01
+#define RET_CYBERGEAR_INVALID_CAN_ID 0x02
+#define RET_CYBERGEAR_INVALID_PACKET 0x03
+
+#define CYBERGEAR_RESPONSE_TIME_USEC 250
+
+#define CW 1
+#define CCW -1
+
+#endif  // !CYBER_GEAR_DRIVER_DEFS_H
diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/actuators/xiaomi_cybergear/servo.cpp b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/actuators/xiaomi_cybergear/servo.cpp
new file mode 100644
index 000000000..84f2b019e
--- /dev/null
+++ b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/actuators/xiaomi_cybergear/servo.cpp
@@ -0,0 +1,101 @@
+#include "servo.h"
+#include "cybergear_driver_defs.hh"
+
+using namespace Xiaomi_Cybergear;
+
+Servo::Servo():
+  servo_state_pub_("servo/extended_states", &servo_state_msg_),
+  servo_cmd_sub_("servo/extended_cmd", &Servo::servoControlCallback, this)
+{
+  servo_last_pub_time_ = 0;
+  servo_last_ctrl_time_ = 0;
+
+  motor_command_.target_position = 0.0;
+  motor_command_.target_speed = 0.0;
+  motor_command_.target_torque = 0.0;
+  motor_command_.target_kp = 0.0;
+  motor_command_.target_kd = 0.0;
+
+  activated_ = false;
+}
+
+void Servo::init(CAN_GeranlHandleTypeDef* hcan, osMailQId* handle, ros::NodeHandle* nh, GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin)
+{
+  /* CAN */
+  CANDeviceManager::init(hcan, GPIOx, GPIO_Pin);
+  CANDeviceManager::useRTOS(handle);
+  CANDeviceManager::addDirectDevice(this);
+
+  /* ros */
+  nh_ = nh;
+  nh_->advertise(servo_state_pub_);
+  nh_->subscribe(servo_cmd_sub_);
+
+  CANDeviceManager::CAN_START();
+}
+
+void Servo::sendData()
+{
+  if(!activated_)
+    {
+      reset_motor();
+      set_mech_position_to_zero();
+      enable_motor();
+      set_run_mode(CMD_CONTROL);
+      activated_ = true;
+    }
+  else
+    motor_control(motor_command_.target_position,
+                  motor_command_.target_speed,
+                  motor_command_.target_torque,
+                  motor_command_.target_kp,
+                  motor_command_.target_kd);
+}
+
+void Servo::send_command(uint8_t can_id, uint8_t cmd_id, uint16_t option, uint8_t len, uint8_t * data)
+{
+  uint32_t id = makeIdentifier(can_id, cmd_id, option);
+  sendMessage(id, 8, data, 0, true); // send data with extended id (last parameter)
+}
+
+void Servo::receiveDataCallback(uint32_t identifier, uint32_t dlc, uint8_t* data)
+{
+  process_motor_packet(data);
+}
+
+void Servo::update(void)
+{
+  CANDeviceManager::tick(1);
+  uint32_t now_time = HAL_GetTick();
+
+  /* control */
+  if( now_time - servo_last_ctrl_time_ >= SERVO_CTRL_INTERVAL)
+    {
+      sendData();
+
+      servo_last_ctrl_time_ = now_time;
+    }
+
+  /* ros publish */
+  if( now_time - servo_last_pub_time_ >= SERVO_PUB_INTERVAL)
+    {
+      servo_state_msg_.position = motor_status_.position;
+      servo_state_msg_.velocity = motor_status_.velocity;
+      servo_state_msg_.effort = motor_status_.effort;
+      servo_state_msg_.temperature = motor_status_.temperature;
+
+      servo_state_pub_.publish(&servo_state_msg_);
+
+      servo_last_pub_time_ = now_time;
+    }
+}
+
+void Servo::servoControlCallback(const spinal::XiaomiCybergearCmd& control_msg)
+{
+  motor_command_.target_position = control_msg.position;
+  motor_command_.target_speed = control_msg.speed;
+  motor_command_.target_torque = control_msg.torque;
+  motor_command_.target_kp = control_msg.kp;
+  motor_command_.target_kd = control_msg.kd;
+}
+
diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/actuators/xiaomi_cybergear/servo.h b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/actuators/xiaomi_cybergear/servo.h
new file mode 100644
index 000000000..c0fc5a8bb
--- /dev/null
+++ b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/actuators/xiaomi_cybergear/servo.h
@@ -0,0 +1,125 @@
+#ifndef __XIAOMI_CYBERGEAR_H
+#define __XIAOMI_CYBERGEAR_H
+
+#include <config.h>
+#include <ros.h>
+#include <CAN/can_device_manager.h>
+#include <spinal/XiaomiCybergearCmd.h>
+#include <spinal/XiaomiCybergearState.h>
+
+#include "math/AP_Math.h"
+
+/* RTOS */
+#include "cmsis_os.h"
+
+#ifdef STM32F7
+using CAN_GeranlHandleTypeDef = CAN_HandleTypeDef;
+#endif
+
+#ifdef STM32H7
+using CAN_GeranlHandleTypeDef = FDCAN_HandleTypeDef;
+#endif
+
+namespace Xiaomi_Cybergear
+{
+  struct MotorComand
+  {
+    float target_position;
+    float target_speed;
+    float target_torque;
+    float target_kp;
+    float target_kd;
+  };
+
+  struct MotorStatus
+  {
+    uint8_t motor_id;          //!< motor id
+    float position;            //!< encoder position (-4pi to 4pi)
+    float velocity;            //!< motor velocity (-30rad/s to 30rad/s)
+    float effort;              //!< motor effort (-12Nm - 12Nm)
+    float temperature;         //!< temperature
+    uint16_t raw_position;     //!< raw position (for sync data)
+    uint16_t raw_velocity;     //!< raw velocity (for sync data)
+    uint16_t raw_effort;       //!< raw effort (for sync data)
+    uint16_t raw_temperature;  //!< raw temperature (for sync data)
+  };
+
+  class Servo : public CANDirectDevice
+  {
+  public:
+    Servo();
+    ~Servo(){}
+
+    void init(CAN_GeranlHandleTypeDef* hcan, osMailQId* handle, ros::NodeHandle* nh, GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin);
+
+    void update(void);
+    void servoControlCallback(const spinal::XiaomiCybergearCmd& control_msg);
+
+    void sendData() override;
+    void receiveDataCallback(uint32_t identifier, uint32_t dlc, uint8_t* data) override;
+
+  private:
+    ros::NodeHandle* nh_;
+    ros::Publisher servo_state_pub_;
+    ros::Subscriber<spinal::XiaomiCybergearCmd, Servo> servo_cmd_sub_;
+    spinal::XiaomiCybergearState servo_state_msg_;
+
+    bool activated_;
+    uint32_t servo_last_pub_time_;
+    uint32_t servo_last_ctrl_time_;
+
+    uint8_t master_can_id_ = 0x00;
+    uint8_t motor_can_id_ = 0x7F;
+
+    MotorComand motor_command_;
+    MotorStatus motor_status_;
+
+    static constexpr int32_t SERVO_PUB_INTERVAL = 10; // [ms]
+    static constexpr int32_t SERVO_CTRL_INTERVAL = 2; // [ms]
+
+    // cybergear driver
+    uint32_t makeIdentifier(uint8_t can_id, uint8_t cmd_id, uint16_t option);
+    void init_motor(uint8_t run_mode);
+    void enable_motor();
+    void reset_motor();
+    void set_run_mode(uint8_t run_mode);
+    void motor_control(float position, float speed, float torque, float kp, float kd);
+    void set_limit_speed(float speed);
+    void set_limit_current(float current);
+    void set_current_kp(float kp);
+    void set_current_ki(float ki);
+    void set_current_filter_gain(float gain);
+    void set_limit_torque(float torque);
+    void set_position_kp(float kp);
+    void set_velocity_kp(float kp);
+    void set_velocity_ki(float ki);
+    void get_mech_position();
+    void get_mech_velocity();
+    void get_vbus();
+    void get_rotation();
+    void dump_motor_param();
+    void set_position_ref(float position);
+    void set_speed_ref(float speed);
+    void set_current_ref(float current);
+    void set_mech_position_to_zero();
+    void change_motor_can_id(uint8_t can_id);
+    void read_ram_data(uint16_t index);
+    uint8_t get_run_mode() const;
+    uint8_t get_motor_id() const;
+    bool process_packet();
+    bool update_motor_status(unsigned long id, const uint8_t * data, unsigned long len);
+    MotorStatus get_motor_status() const { return motor_status_; }
+    // MotorParameter get_motor_param() const { return motor_param_; }
+    // unsigned long send_count() const { return send_count_; }
+    void write_float_data(uint8_t can_id, uint16_t addr, float value, float min, float max);
+    int float_to_uint(float x, float x_min, float x_max, int bits);
+    float uint_to_float(uint16_t x, float x_min, float x_max);
+    void send_command(uint8_t can_id, uint8_t cmd_id, uint16_t option, uint8_t len, uint8_t * data);
+    bool receive_motor_data(MotorStatus & mot);
+    void process_motor_packet(const uint8_t * data);
+    void process_read_parameter_packet(const uint8_t * data, unsigned long len);
+    void print_can_packet(uint32_t id, const uint8_t * data, uint8_t len);
+  };
+}
+
+#endif
diff --git a/aerial_robot_nerve/spinal/msg/XiaomiCybergearCmd.msg b/aerial_robot_nerve/spinal/msg/XiaomiCybergearCmd.msg
new file mode 100644
index 000000000..6060cfa63
--- /dev/null
+++ b/aerial_robot_nerve/spinal/msg/XiaomiCybergearCmd.msg
@@ -0,0 +1,5 @@
+float32 position
+float32 speed
+float32 torque
+float32 kp
+float32 kd
diff --git a/aerial_robot_nerve/spinal/msg/XiaomiCybergearState.msg b/aerial_robot_nerve/spinal/msg/XiaomiCybergearState.msg
new file mode 100644
index 000000000..35750829a
--- /dev/null
+++ b/aerial_robot_nerve/spinal/msg/XiaomiCybergearState.msg
@@ -0,0 +1,4 @@
+float32 position
+float32 velocity
+float32 effort
+float32 temperature