[Servo][Spinal][DJI M2006] implement the interface to connect with DJ…

…I M2006 (RM C610).

aerial_robot_nerve/spinal/CMakeLists.txt

@@ -23,6 +23,7 @@ add_message_files(
   BoardInfo.msg
   ServoState.msg
   ServoStates.msg
+  ServoExtendedState.msg
   ServoControlCmd.msg
   ServoTorqueCmd.msg
   ServoTorqueStates.msg

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,8 @@ Baro baro_;
 GPS gps_;
 BatteryStatus battery_status_;
 
+/* actuators */
+DJI_M2006::Servo dji_servo_;
 
 StateEstimate estimator_;
 FlightControl controller_;
@@ -239,6 +241,8 @@ 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_);
+  dji_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);
 
@@ -1073,6 +1077,9 @@ void coreTaskFunc(void const * argument)
       estimator_.update();
       controller_.update();
 
+      dji_servo_.update();
+
+
 #if NERVE_COMM      
       Spine::update();
 #endif

aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/actuators/DJI_M2006/servo.cpp

@@ -0,0 +1,108 @@
+/**
+******************************************************************************
+* File Name          : servo.cpp
+* Description        : interface for DJI M2006 with C610
+ ------------------------------------------------------------------*/
+
+#include "servo.h"
+
+using namespace DJI_M2006;
+
+Servo::Servo(): servo_state_pub_("servo/states", &servo_state_msg_), servo_cmd_sub_("servo/target_states", &Servo::servoControlCallback, this)
+{
+  last_connected_time_ = 0;
+  servo_last_pub_time_ = 0;
+  servo_last_ctrl_time_ = 0;
+
+  rotations_ = 0;
+  last_pos_measurement_ = kCountsPerRev;
+  counts_ = 0;
+  rpm_ = 0;
+  current_ = 0;
+
+  goal_current_ = 0;
+}
+
+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()
+{
+  uint8_t cmd[8];
+  cmd[0] = (goal_current_ >> 8) & 0xFF;
+  cmd[1] = goal_current_ & 0xFF;
+
+  sendMessage(canTxId, 8, cmd, 0);
+}
+
+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]);
+  current_ = int16_t((data[4] << 8) | data[5]);
+
+  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;
+  }
+
+  rotations_ = 0;
+  counts_ = rotations_ * kCountsPerRev + counts;
+  last_pos_measurement_ = counts;
+}
+
+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)
+    {
+      /* send servo */
+      servo_state_msg_.index = 0;
+      servo_state_msg_.angle = counts_;
+      servo_state_msg_.rpm = rpm_;
+      servo_state_msg_.current = current_;
+
+      servo_state_pub_.publish(&servo_state_msg_);
+      servo_last_pub_time_ = now_time;
+    }
+}
+
+void Servo::servoControlCallback(const spinal::ServoControlCmd& control_msg)
+{
+  goal_current_ = control_msg.angles[0];
+}
+
+

aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/actuators/DJI_M2006/servo.h

@@ -0,0 +1,79 @@
+/**
+  ******************************************************************************
+  * File Name          : servo.h
+  * Description        : interface for DJI M2006 with C610
+ Includes ------------------------------------------------------------------*/
+
+#ifndef __DJI_M2006_H
+#define __DJI_M2006_H
+
+#include <config.h>
+#include <ros.h>
+#include <CAN/can_device_manager.h>
+#include <spinal/ServoExtendedState.h>
+#include <spinal/ServoControlCmd.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 DJI_M2006
+{
+  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::ServoControlCmd& 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::ServoControlCmd, Servo> servo_cmd_sub_;
+    spinal::ServoExtendedState servo_state_msg_;
+
+    uint32_t last_connected_time_;
+    uint32_t servo_last_pub_time_;
+    uint32_t servo_last_ctrl_time_;
+
+    uint8_t initialized_mechanical_angle_;
+    int32_t rotations_;
+    int32_t last_pos_measurement_;
+    int32_t counts_;
+    int32_t rpm_;
+    int32_t current_;
+
+    int16_t goal_current_;
+
+    // Constants specific to the C610 + M2006 setup.
+    static const uint32_t canTxId = 0x200;
+    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 constexpr int32_t SERVO_PUB_INTERVAL = 10; // [ms]
+    static constexpr int32_t SERVO_CTRL_INTERVAL = 2; // [ms]
+  };
+}
+#endif

aerial_robot_nerve/spinal/msg/ServoExtendedState.msg

@@ -0,0 +1,5 @@
+uint8 index # from 0
+int32 angle
+int32 rpm
+int32 current # load
+