[Servo Motor][DJI M2006][WIP] implement the interface to control DJI M2006 via spinal

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

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();

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;

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));
   }

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_ */

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

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();