diff --git a/aerial_robot_nerve/spinal/CMakeLists.txt b/aerial_robot_nerve/spinal/CMakeLists.txt
index ed8b22e0b..6891fc88a 100644
--- a/aerial_robot_nerve/spinal/CMakeLists.txt
+++ b/aerial_robot_nerve/spinal/CMakeLists.txt
@@ -23,7 +23,12 @@ add_message_files(
   BoardInfo.msg
   ServoState.msg
   ServoStates.msg
+  ServoExtendedState.msg
+  ServoExtendedStates.msg
+  ServoExtendedCmd.msg
+  ServoExtendedCmds.msg
   ServoControlCmd.msg
+  ServoPIDGain.msg
   ServoTorqueCmd.msg
   ServoTorqueStates.msg
   Gyro.msg
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..c0740eafa 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
@@ -41,7 +41,7 @@
 #include "sensors/baro/baro_ms5611.h"
 #include "sensors/gps/gps_ublox.h"
 #include "sensors/encoder/mag_encoder.h"
-
+#include "actuators/DJI_M2006/servo.h"
 #include "battery_status/battery_status.h"
 
 #include "state_estimate/state_estimate.h"
@@ -110,6 +110,10 @@ Baro baro_;
 GPS gps_;
 BatteryStatus battery_status_;
 
+/* actuators */
+#if DJI_CAN_SERVO
+DJI_M2006::Interface dji_servo_;
+#endif
 
 StateEstimate estimator_;
 FlightControl controller_;
@@ -239,6 +243,10 @@ 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_);
+#if DJI_CAN_SERVO
+  dji_servo_.init(&hfdcan1, &canMsgMailHandle, &nh_, LED1_GPIO_Port, LED1_Pin);
+#endif
+
   estimator_.init(&imu_, &baro_, &gps_, &nh_);  // imu + baro + gps => att + alt + pos(xy)
   controller_.init(&htim1, &htim4, &estimator_, &battery_status_, &nh_, &flightControlMutexHandle);
 
@@ -1072,6 +1080,9 @@ void coreTaskFunc(void const * argument)
       gps_.update();
       estimator_.update();
       controller_.update();
+#if DJI_CAN_SERVO
+      dji_servo_.update();
+#endif
 
 #if NERVE_COMM      
       Spine::update();
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..cca26b349 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
@@ -118,7 +118,14 @@ 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)
+  {
+    tx_header_.Identifier = identifier;
 
     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..a022edd18 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);
   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..4f30b7723 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){CAN::sendMessage(identifier, dlc, data, timeout);}
+	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/DJI_M2006/servo.cpp b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/actuators/DJI_M2006/servo.cpp
new file mode 100644
index 000000000..649176d34
--- /dev/null
+++ b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/actuators/DJI_M2006/servo.cpp
@@ -0,0 +1,307 @@
+/**
+******************************************************************************
+* File Name          : servo.cpp
+* Description        : interface for DJI M2006 with C610
+ ------------------------------------------------------------------*/
+
+#include "servo.h"
+
+using namespace DJI_M2006;
+
+float clamp(float val, float limit) { return std::min(std::max(val, -limit), limit); }
+
+Servo::Servo()
+{
+  rotations_ = 0;
+  last_pos_measurement_ = kCountsPerRev;
+
+  counts_ = 0;
+  rpm_ = 0;
+  m_curr_ = 0;
+
+  output_pos_ = 0;
+  output_vel_ = 0;
+  curr_ = 0;
+
+  filter_vel_ = 0;
+  filter_vel_p_ = 0;
+
+  control_mode_ = 2;
+
+  goal_curr_ = 0;
+  goal_vel_ = 0;
+
+  v_k_p_ = 5.0; // 5.0 is good; 10.0 is oscillated for raw vel
+  v_k_i_ = 0.2; // 0.2 is good; 0.5 is too strong.
+  v_i_term_ = 0;
+
+
+  p_k_p_ = 20.0;
+  p_k_i_ = 0.0;
+  p_k_d_ = 2.0;
+  p_i_term_ = 0;
+}
+
+void Servo::update(uint16_t counts, int16_t rpm, int16_t m_curr)
+{
+  rpm_ = rpm;
+  m_curr_ = m_curr_;
+
+  if (last_pos_measurement_ == 8192) {
+    last_pos_measurement_ = counts;
+  }
+
+  int32_t delta = counts - last_pos_measurement_;
+  if (delta > kCountsPerRev / 2) {
+    // Crossed from >= 0 counts to <= 8191 counts. Could
+    // also trigger if spinning super fast (>2000rps)
+    rotations_ -= 1;
+  } else if (delta < -kCountsPerRev / 2) {
+    // Crossed from <= 8191 counts to >= 0 counts. Could
+    // also trigger if spinning super fast (>2000rps)
+    rotations_ += 1;
+  }
+
+  counts_ = rotations_ * kCountsPerRev + counts;
+  last_pos_measurement_ = counts;
+
+  output_pos_ = counts_ / kCountsPerRad; // TODO: rectify the offset;
+  output_vel_ = rpm_ / kRPMPerRadS;
+  curr_ = m_curr_ / 1000.0f;
+
+  // low pass filer
+  filter_vel_p_ -= (filter_vel_p_/GYRO_LPF_FACTOR);
+  filter_vel_p_ += output_vel_;
+  filter_vel_ = (filter_vel_p_/GYRO_LPF_FACTOR);
+}
+
+void Servo::control()
+{
+  switch (control_mode_)
+    {
+    case POS_MODE:
+      calcPosPid();
+      break;
+    case VEL_MODE:
+      calcVelPid();
+      break;
+    case CUR_MODE:
+      goal_curr_ = goal_value_;
+      break;
+    default:
+      goal_curr_ = 0;
+      break;
+    }
+
+  goal_curr_ = clamp(goal_curr_, MAX_CURRENT);
+}
+
+void Servo::calcVelPid(void)
+{
+  goal_vel_ = goal_value_;
+
+  // filterd velocity will cause the oscillation, even GYRO_LPF_FACTOR = 2 is invliad
+  // so use the raw value from C610 (the raw one might be already filtered in C610)
+  float err = goal_vel_ - output_vel_;
+  float p_term = v_k_p_ * err;
+  p_term = clamp(p_term, MAX_CURRENT);
+  v_i_term_ += v_k_i_ * err * SERVO_CTRL_INTERVAL;
+  v_i_term_ = clamp(v_i_term_, MAX_CURRENT);
+
+  goal_curr_ = p_term + v_i_term_;
+}
+
+void Servo::calcPosPid(void)
+{
+  goal_pos_ = goal_value_;
+
+  float err = goal_pos_ - output_pos_;
+  float p_term = p_k_p_ * err;
+  p_term = clamp(p_term, MAX_CURRENT);
+  p_i_term_ += p_k_i_ * err * SERVO_CTRL_INTERVAL;
+  p_i_term_ = clamp(p_i_term_, MAX_CURRENT);
+  float d_term = p_k_d_ * (-output_vel_);
+  d_term = clamp(d_term, MAX_CURRENT);
+
+  goal_curr_ = p_term + p_i_term_ + d_term;
+}
+
+void Servo::setPidGain(uint8_t mode, float p_gain, float i_gain, float d_gain)
+{
+  if (mode == POS_MODE)
+    {
+      p_k_p_ = p_gain;
+      p_k_i_ = i_gain;
+      p_k_d_ = d_gain;
+    }
+
+  if (mode == VEL_MODE)
+    {
+      v_k_p_ = p_gain;
+      v_k_i_ = i_gain;
+    }
+}
+
+Interface::Interface(): servo_state_pub_("servo/extended_states", &servo_states_msg_),
+                        servo_cmd_sub_("servo/extended_cmds", &Interface::servoControlCallback, this),
+                        servo_pid_gain_sub_("servo/set_pid_gain", &Interface::servoPIDGainCallback, this)
+{
+  /* variables */
+  init_cnt_ = 100; // for catch the CAN messages from servo, 100 messages.
+  servo_states_msg_.servos_length = 0;
+
+  /* timer */
+  last_connected_time_ = 0;
+  servo_last_pub_time_ = 0;
+  servo_last_ctrl_time_ = 0;
+}
+
+void Interface::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_);
+  nh_->subscribe(servo_pid_gain_sub_);
+
+  CANDeviceManager::CAN_START();
+}
+
+void Interface::sendData()
+{
+  bool cmd1_flag = false;
+  bool cmd2_flag = false;
+  uint8_t cmd1[8]; // 1~4
+  uint8_t cmd2[8]; // 5~8
+
+  for (std::map<int,Servo>::iterator it = servo_list_.begin(); it != servo_list_.end(); it++)
+    {
+      int id = it->first;
+      float goal_curr = it->second.getGoalCurrent();
+      int16_t goal_m_curr = goal_curr * 1000;
+
+      if (id < 5)
+        {
+          cmd1_flag = true;
+
+          cmd1[(id-1) * 2] = (goal_m_curr >> 8) & 0xFF;
+          cmd1[(id-1) * 2 + 1] = goal_m_curr & 0xFF;
+
+        }
+      else
+        {
+          cmd2_flag = true;
+
+          cmd2[(id-5) * 2] = (goal_m_curr >> 8) & 0xFF;
+          cmd2[(id-5) * 2 + 1] = goal_m_curr & 0xFF;
+        }
+    }
+
+  if (cmd1_flag) sendMessage(canTxId1, 8, cmd1, 0);
+  if (cmd2_flag) sendMessage(canTxId2, 8, cmd2, 0);
+}
+
+void Interface::update(void)
+{
+  CANDeviceManager::tick(1);
+  uint32_t now_time = HAL_GetTick();
+
+  /* control */
+  if(now_time - servo_last_ctrl_time_ >= SERVO_CTRL_INTERVAL)
+    {
+      for (std::map<int,Servo>::iterator it = servo_list_.begin(); it != servo_list_.end(); it++)
+        {
+          it->second.control();
+        }
+
+      sendData();
+      servo_last_ctrl_time_ = now_time;
+    }
+
+  /* ros publish */
+  if(now_time - servo_last_pub_time_ >= SERVO_PUB_INTERVAL)
+    {
+      if (servo_states_msg_.servos_length > 0)
+        {
+          servo_states_msg_.stamp = nh_->now();
+
+          int i = 0;
+          for (std::map<int,Servo>::iterator it = servo_list_.begin(); it != servo_list_.end(); it++)
+          {
+            spinal::ServoExtendedState servo;
+
+            servo.index = it->first;
+            servo.angle = it->second.getAngle();
+            servo.velocity = it->second.getFilteredVelocity();
+            servo.current = it->second.getCurrent();
+
+            servo_states_msg_.servos[i] = servo;
+            i++;
+          }
+
+          servo_state_pub_.publish(&servo_states_msg_);
+        }
+      servo_last_pub_time_ = now_time;
+    }
+}
+
+void Interface::receiveDataCallback(uint32_t identifier, uint32_t dlc, uint8_t* data)
+{
+  int id = identifier - 0x200;
+
+  // update map
+  if (init_cnt_ > 0)
+    {
+      std::map<int,Servo>::iterator it = servo_list_.find(id);
+      if (it == servo_list_.end())
+        {
+          servo_list_[id] = Servo();
+        }
+      init_cnt_ --;
+
+      if (init_cnt_ == 0)
+        {
+          servo_states_msg_.servos_length = servo_list_.size();
+          servo_states_msg_.servos = new spinal::ServoExtendedState[servo_list_.size()];
+        }
+    }
+
+  uint16_t counts = uint16_t((data[0] << 8) | data[1]);
+  int16_t rpm = int16_t((data[2] << 8) | data[3]);
+  int16_t m_curr = int16_t((data[4] << 8) | data[5]);
+  servo_list_.at(id).update(counts, rpm, m_curr);
+}
+
+
+void Interface::servoControlCallback(const spinal::ServoExtendedCmds& msg)
+{
+  for (uint32_t i = 0; i < msg.servos_length; i++)
+    {
+      int id = msg.servos[i].index;
+      uint8_t mode = msg.servos[i].mode;
+      float cmd = msg.servos[i].cmd;
+
+      std::map<int,Servo>::iterator it = servo_list_.find(id);
+
+      if (it != servo_list_.end())
+        {
+          it->second.setControlMode(mode);
+          it->second.setGoalValue(cmd);
+        }
+    }
+}
+
+void Interface::servoPIDGainCallback(const spinal::ServoPIDGain& msg)
+{
+  std::map<int,Servo>::iterator it = servo_list_.find(msg.index);
+  if (it != servo_list_.end())
+    {
+      it->second.setPidGain(msg.mode, msg.p_gain, msg.i_gain, msg.d_gain);
+    }
+
+}
diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/actuators/DJI_M2006/servo.h b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/actuators/DJI_M2006/servo.h
new file mode 100644
index 000000000..671715774
--- /dev/null
+++ b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/actuators/DJI_M2006/servo.h
@@ -0,0 +1,154 @@
+/**
+  ******************************************************************************
+  * File Name          : servo.h
+  * Description        : interface for DJI M2006 with C610
+ Includes ------------------------------------------------------------------*/
+
+#ifndef __DJI_M2006_H
+#define __DJI_M2006_H
+
+#include <config.h>
+#include <map>
+#include <ros.h>
+#include <CAN/can_device_manager.h>
+#include <spinal/ServoExtendedStates.h>
+#include <spinal/ServoExtendedCmds.h>
+#include <spinal/ServoPIDGain.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
+
+#define SERVO_CTRL_INTERVAL 2 // [ms]
+
+namespace DJI_M2006
+{
+  class Servo
+  {
+  public:
+    Servo();
+    ~Servo() {}
+
+    uint8_t getControlMode() {return control_mode_; }
+    float getGoalValue() {return goal_value_; }
+    float getGoalCurrent() {return goal_curr_; }
+
+    float getAngle() {return output_pos_; }
+    float getVelocity() {return output_vel_; }
+    float getFilteredVelocity() {return filter_vel_; }
+    float getCurrent() {return curr_; }
+
+
+    void setControlMode(int mode) { control_mode_ = mode; }
+    void setGoalValue(float cmd) { goal_value_ = cmd; }
+    void setPidGain(uint8_t mode, float p_gian, float i_gain, float d_gain);
+
+    void update(uint16_t counts, int16_t rpm, int16_t m_curr);
+    void control(void);
+
+    static constexpr uint8_t GYRO_LPF_FACTOR = 20;
+    static const int32_t kCountsPerRev = 8192;
+    static constexpr float kReduction = 36.0F;
+    static constexpr float kCountsPerRad = kCountsPerRev * kReduction / (2 * M_PI);
+    static constexpr float kRPMPerRadS = kReduction * 60 / (2.0F * M_PI);
+    static constexpr float kMilliAmpPerAmp = 1000.0F;
+
+    static constexpr float kResistance = 0.100;
+    static constexpr float kVoltageConstant = 100.0;
+
+    static const uint8_t POS_MODE = 0;
+    static const uint8_t VEL_MODE = 1;
+    static const uint8_t CUR_MODE = 2;
+
+    static constexpr float MAX_CURRENT = 10.0; // [A]
+
+  private:
+
+    uint8_t initialized_mechanical_angle_;
+    int32_t rotations_;
+    int32_t last_pos_measurement_;
+    int32_t counts_;
+    int32_t rpm_;
+    int32_t m_curr_;
+
+    float output_pos_;
+    float output_vel_;
+    float curr_;
+
+    float filter_vel_;
+    float filter_vel_p_;
+
+    int8_t control_mode_;
+    float goal_value_;
+    float goal_pos_;
+    float goal_vel_;
+    float goal_curr_;
+
+    // PID velocity control
+    float v_k_p_;
+    float v_k_i_;
+    float v_i_term_;
+
+    // PID position control
+    float p_k_p_;
+    float p_k_i_;
+    float p_k_d_;
+    float p_i_term_;
+
+    void calcPosPid();
+    void calcVelPid();
+  };
+
+  class Interface : public CANDirectDevice
+  {
+  public:
+    Interface();
+    ~Interface(){}
+
+    void init(CAN_GeranlHandleTypeDef* hcan, osMailQId* handle, ros::NodeHandle* nh, GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin);
+
+    void servoControlCallback(const spinal::ServoExtendedCmds& msg);
+    void servoPIDGainCallback(const spinal::ServoPIDGain& msg);
+
+    void update();
+
+    void sendData() override;
+    void receiveDataCallback(uint32_t identifier, uint32_t dlc, uint8_t* data) override;
+
+    Servo& getServo(int id) {return servo_list_.at(id); }
+
+
+  private:
+    ros::NodeHandle* nh_;
+    ros::Publisher servo_state_pub_;
+    ros::Subscriber<spinal::ServoExtendedCmds, Interface> servo_cmd_sub_;
+    ros::Subscriber<spinal::ServoPIDGain, Interface> servo_pid_gain_sub_;
+    spinal::ServoExtendedStates servo_states_msg_;
+
+
+    uint32_t last_connected_time_;
+    uint32_t servo_last_pub_time_;
+    uint32_t servo_last_ctrl_time_;
+
+    // Constants specific to the C610 + M2006 setup.
+    static const uint32_t canTxId1 = 0x200;
+    static const uint32_t canTxId2 = 0x1FF;
+
+    // Constants specific to ros
+    static constexpr int32_t SERVO_PUB_INTERVAL = 10; // [ms]
+
+    std::map<int, Servo> servo_list_;
+
+    int init_cnt_;
+  };
+}
+#endif
diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/config.h b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/config.h
index a7d1e5007..c8a8e88cb 100644
--- a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/config.h
+++ b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/config.h
@@ -28,7 +28,7 @@
 #define GPIO_L(port, pin) HAL_GPIO_WritePin(port, pin, GPIO_PIN_RESET)
 
 //0. Comm Type
-#define NERVE_COMM 1
+#define NERVE_COMM 0
 
 //2. Enable Flags
 //* Please set/reset follwing flags according to your utility.
@@ -85,6 +85,8 @@
 #endif
 ///////////////////////////////////
 
+//2.4 CAN based Actuator
+#define DJI_CAN_SERVO 1
 
 
 #endif //__CONFIG_H
diff --git a/aerial_robot_nerve/spinal/msg/ServoExtendedCmd.msg b/aerial_robot_nerve/spinal/msg/ServoExtendedCmd.msg
new file mode 100644
index 000000000..0469a6cfa
--- /dev/null
+++ b/aerial_robot_nerve/spinal/msg/ServoExtendedCmd.msg
@@ -0,0 +1,4 @@
+uint8 index # from 0
+uint8 mode # 0: position model; 1: velocity mode; 2: torque(current) mode
+float32 cmd
+
diff --git a/aerial_robot_nerve/spinal/msg/ServoExtendedCmds.msg b/aerial_robot_nerve/spinal/msg/ServoExtendedCmds.msg
new file mode 100644
index 000000000..708115683
--- /dev/null
+++ b/aerial_robot_nerve/spinal/msg/ServoExtendedCmds.msg
@@ -0,0 +1,3 @@
+time stamp
+spinal/ServoExtendedCmd[] servos
+
diff --git a/aerial_robot_nerve/spinal/msg/ServoExtendedState.msg b/aerial_robot_nerve/spinal/msg/ServoExtendedState.msg
new file mode 100644
index 000000000..642c75f8a
--- /dev/null
+++ b/aerial_robot_nerve/spinal/msg/ServoExtendedState.msg
@@ -0,0 +1,5 @@
+uint8 index # from 0
+float32 angle # [rad]
+float32 velocity # [rad/s]
+float32 current # [A]
+
diff --git a/aerial_robot_nerve/spinal/msg/ServoExtendedStates.msg b/aerial_robot_nerve/spinal/msg/ServoExtendedStates.msg
new file mode 100644
index 000000000..181e17841
--- /dev/null
+++ b/aerial_robot_nerve/spinal/msg/ServoExtendedStates.msg
@@ -0,0 +1,2 @@
+time stamp
+spinal/ServoExtendedState[] servos
diff --git a/aerial_robot_nerve/spinal/msg/ServoPIDGain.msg b/aerial_robot_nerve/spinal/msg/ServoPIDGain.msg
new file mode 100644
index 000000000..f3b79762a
--- /dev/null
+++ b/aerial_robot_nerve/spinal/msg/ServoPIDGain.msg
@@ -0,0 +1,5 @@
+uint8 index
+uint8 mode
+float32 p_gain
+float32 i_gain
+float32 d_gain