diff --git a/aerial_robot_nerve/spinal/CMakeLists.txt b/aerial_robot_nerve/spinal/CMakeLists.txt
index cea6ea2a9..17ae712e0 100644
--- a/aerial_robot_nerve/spinal/CMakeLists.txt
+++ b/aerial_robot_nerve/spinal/CMakeLists.txt
@@ -24,6 +24,7 @@ add_message_files(
   ServoState.msg
   ServoStates.msg
   ServoExtendedState.msg
+  ServoExtendedCmd.msg
   ServoControlCmd.msg
   ServoTorqueCmd.msg
   ServoTorqueStates.msg
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
index f7b5b0d41..62a999ee8 100644
--- 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
@@ -8,7 +8,9 @@
 
 using namespace DJI_M2006;
 
-Servo::Servo(): servo_state_pub_("servo/states", &servo_state_msg_), servo_cmd_sub_("servo/target_states", &Servo::servoControlCallback, this)
+float clamp(float val, float limit) { return std::min(std::max(val, -limit), limit); }
+
+Servo::Servo(): servo_state_pub_("servo/extended_states", &servo_state_msg_), servo_cmd_sub_("servo/extended_cmd", &Servo::servoControlCallback, this)
 {
   last_connected_time_ = 0;
   servo_last_pub_time_ = 0;
@@ -19,13 +21,29 @@ Servo::Servo(): servo_state_pub_("servo/states", &servo_state_msg_), servo_cmd_s
 
   counts_ = 0;
   rpm_ = 0;
-  m_current_ = 0;
+  m_curr_ = 0;
 
-  output_ang_ = 0;
+  output_pos_ = 0;
   output_vel_ = 0;
-  current_ = 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;
 
-  goal_current_ = 0;
+
+  p_k_p_ = 20.0;
+  p_k_i_ = 0.0;
+  p_k_d_ = 2.0;
+  p_i_term_ = 0;
 }
 
 void Servo::init(CAN_GeranlHandleTypeDef* hcan, osMailQId* handle, ros::NodeHandle* nh, GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin)
@@ -45,9 +63,10 @@ void Servo::init(CAN_GeranlHandleTypeDef* hcan, osMailQId* handle, ros::NodeHand
 
 void Servo::sendData()
 {
+  int16_t goal_m_curr = goal_curr_ * 1000;
   uint8_t cmd[8];
-  cmd[0] = (goal_current_ >> 8) & 0xFF;
-  cmd[1] = goal_current_ & 0xFF;
+  cmd[0] = (goal_m_curr >> 8) & 0xFF;
+  cmd[1] = goal_m_curr & 0xFF;
 
   sendMessage(canTxId, 8, cmd, 0);
 }
@@ -56,7 +75,7 @@ void Servo::receiveDataCallback(uint32_t identifier, uint32_t dlc, uint8_t* data
 {
   uint16_t counts = uint16_t((data[0] << 8) | data[1]);
   rpm_ = int16_t((data[2] << 8) | data[3]);
-  m_current_ = int16_t((data[4] << 8) | data[5]);
+  m_curr_ = int16_t((data[4] << 8) | data[5]);
 
   if (last_pos_measurement_ == 8192) {
     last_pos_measurement_ = counts;
@@ -76,9 +95,15 @@ void Servo::receiveDataCallback(uint32_t identifier, uint32_t dlc, uint8_t* data
   counts_ = rotations_ * kCountsPerRev + counts;
   last_pos_measurement_ = counts;
 
-  output_ang_ = counts_ / kCountsPerRad; // TODO: rectify the offset;
+  output_pos_ = counts_ / kCountsPerRad; // TODO: rectify the offset;
   output_vel_ = rpm_ / kRPMPerRadS;
-  current_ = m_current_ / 1000.0f;
+  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::update(void)
@@ -89,6 +114,24 @@ void Servo::update(void)
   /* control */
   if( now_time - servo_last_ctrl_time_ >= SERVO_CTRL_INTERVAL)
     {
+      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);
+
       sendData();
       servo_last_ctrl_time_ = now_time;
     }
@@ -99,18 +142,50 @@ void Servo::update(void)
     {
       /* send servo */
       servo_state_msg_.index = 0;
-      servo_state_msg_.angle = output_ang_;
+      servo_state_msg_.angle = output_pos_;
       servo_state_msg_.velocity = output_vel_;
-      servo_state_msg_.current = current_;
+      servo_state_msg_.current = curr_;
+
+      servo_state_msg_.current = filter_vel_;
 
       servo_state_pub_.publish(&servo_state_msg_);
       servo_last_pub_time_ = now_time;
     }
 }
 
-void Servo::servoControlCallback(const spinal::ServoControlCmd& control_msg)
+void Servo::servoControlCallback(const spinal::ServoExtendedCmd& msg)
 {
-  goal_current_ = control_msg.angles[0];
+  control_mode_ = msg.mode;
+  goal_value_ = msg.cmd;
+}
+
+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;
+}
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
index 1656737d1..d1927c73a 100644
--- 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
@@ -11,7 +11,7 @@
 #include <ros.h>
 #include <CAN/can_device_manager.h>
 #include <spinal/ServoExtendedState.h>
-#include <spinal/ServoControlCmd.h>
+#include <spinal/ServoExtendedCmd.h>
 
 #include "math/AP_Math.h"
 
@@ -37,7 +37,7 @@ namespace DJI_M2006
     void init(CAN_GeranlHandleTypeDef* hcan, osMailQId* handle, ros::NodeHandle* nh, GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin);
 
     void update(void);
-    void servoControlCallback(const spinal::ServoControlCmd& control_msg);
+    void servoControlCallback(const spinal::ServoExtendedCmd& control_msg);
 
     void sendData() override;
     void receiveDataCallback(uint32_t identifier, uint32_t dlc, uint8_t* data) override;
@@ -45,7 +45,7 @@ namespace DJI_M2006
   private:
     ros::NodeHandle* nh_;
     ros::Publisher servo_state_pub_;
-    ros::Subscriber<spinal::ServoControlCmd, Servo> servo_cmd_sub_;
+    ros::Subscriber<spinal::ServoExtendedCmd, Servo> servo_cmd_sub_;
     spinal::ServoExtendedState servo_state_msg_;
 
     uint32_t last_connected_time_;
@@ -57,13 +57,33 @@ namespace DJI_M2006
     int32_t last_pos_measurement_;
     int32_t counts_;
     int32_t rpm_;
-    int32_t m_current_;
+    int32_t m_curr_;
 
-    float output_ang_;
+    float output_pos_;
     float output_vel_;
-    float current_;
+    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_;
 
-    int16_t goal_current_;
 
     // Constants specific to the C610 + M2006 setup.
     static const uint32_t canTxId = 0x200;
@@ -76,8 +96,18 @@ namespace DJI_M2006
     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]
     static constexpr int32_t SERVO_PUB_INTERVAL = 10; // [ms]
     static constexpr int32_t SERVO_CTRL_INTERVAL = 2; // [ms]
+
+    static constexpr uint8_t GYRO_LPF_FACTOR = 20;
+
+    void calcPosPid();
+    void calcVelPid();
   };
 }
 #endif
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
+