[Spinal][cybergear][WIP] implement interface for xiaomi cybergear

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(

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

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

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;

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

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

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

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