Commit 8b89f47f authored by NiZenMeZhiDao's avatar NiZenMeZhiDao
Browse files

Fixed the Xiaomi motor drive.

parent ccf97b17

drivers/motor/mi/motor_mi.c

+103 −86
Original line number Diff line number Diff line
@@ -2,6 +2,9 @@ 
#include "zephyr/device.h"

#include "zephyr/drivers/can.h"

#include "zephyr/types.h"

#include <stdint.h>

#include <string.h>

#include <sys/_stdint.h>

#include <zephyr/drivers/pid.h>

#include <zephyr/kernel.h>

#include <zephyr/logging/log.h>
@@ -18,17 +21,15 @@ static float uint16_to_float(uint16_t x, float x_min, float x_max, int bits) { 
  float offset = x_max - x_min;

  return offset * x / span + x_min;

}



int 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;

  if(x > x_max) x=x_max;

  else if(x < x_min) x= x_min;

  return (int) ((x-offset)*((float)((1<<bits)-1))/span);

  }





static int get_can_id(const struct device *dev) {

  const struct mi_motor_cfg *cfg = dev->config;

  for (int i = 0; i < CAN_COUNT; i++) {
@@ -47,6 +48,7 @@ void can_tx_callback(const struct device *can_dev, int error, void *user_data) { 
}



int mi_init(const struct device *dev) {

  LOG_DBG("mi_init");

  const struct mi_motor_cfg *cfg = dev->config;

  int can_id = get_can_id(dev);

  k_sem_init(&tx_queue_sem[can_id], 3, 3); // 初始化信号量
@@ -64,6 +66,7 @@ int mi_init(const struct device *dev) { 
  return 0;

}

int mi_send_queued(struct tx_frame *frame, struct k_msgq *msgq) {

  // LOG_DBG("mi_send_queued");

  int err = k_sem_take(frame->sem, K_NO_WAIT);

  if (err == 0) {

    err = can_send(frame->can_dev, &frame->frame, K_NO_WAIT, can_tx_callback,
@@ -82,6 +85,7 @@ int mi_send_queued(struct tx_frame *frame, struct k_msgq *msgq) { 
}



int mi_queue_proceed(struct k_msgq *msgq) {

  // LOG_DBG("mi_queue_proceed");

  struct tx_frame frame;

  int err = 0;

  bool give_up = false;
@@ -108,6 +112,7 @@ int mi_queue_proceed(struct k_msgq *msgq) { 
}



void mi_motor_control(const struct device *dev, enum motor_cmd cmd) {

  // LOG_DBG("mi_motor_control");

  struct mi_motor_data *data = dev->data;

  const struct mi_motor_cfg *cfg = dev->config;
@@ -118,8 +123,8 @@ void mi_motor_control(const struct device *dev, enum motor_cmd cmd) { 
  // uint16_t master_id;



  int can_id = get_can_id(dev);

  struct mi_can_id *mi_can_id = (struct mi_can_id *)&frame.id;

  mi_can_id->id = can_id;

  struct mi_can_id *mi_can_id = (struct mi_can_id *)&(frame.id);

  mi_can_id->id = cfg->common.id;

  switch (cmd) {

  case ENABLE_MOTOR:

    data->online = true;
@@ -154,22 +159,24 @@ void mi_motor_control(const struct device *dev, enum motor_cmd cmd) { 
}



static void mi_motor_pack(const struct device *dev, struct can_frame *frame) {

  uint16_t pos_tmp, vel_tmp, kp_tmp, kd_tmp, tor_tmp, cur_tep;

  // LOG_DBG("mi_motor_pack");

  uint32_t pos_tmp, vel_tmp, kp_tmp, kd_tmp, tor_tmp, cur_tep;



  struct mi_motor_data *data = (struct mi_motor_data *)(dev->data);

  struct mi_motor_cfg *cfg = (struct mi_motor_cfg *)(dev->config);

  struct mi_can_id *mi_can_id = (struct mi_can_id *)&(frame->id);



  struct mi_can_id *mi_can_id = (struct mi_can_id *)(frame->id);



  mi_can_id->id = data->can_id;

  mi_can_id->id = cfg->common.id;



  frame->dlc = 8;

  frame->flags = 0;

  struct can_frame *frame_follow = frame + 1;

 

  struct can_frame *frame_follow =&frame[1];

  frame->flags = CAN_FRAME_IDE;

  frame_follow->flags = CAN_FRAME_IDE;

  struct mi_can_id *mi_can_id_fol = (struct mi_can_id *)(frame_follow->id);



  mi_can_id_fol->id = data->can_id;

  struct mi_can_id *mi_can_id_fol = (struct mi_can_id *)&(frame_follow->id);

  uint16_t index[2];

  mi_can_id_fol->id = cfg->common.id;

  switch (data->common.mode) {

  case MIT:

    mi_can_id->mi_msg_mode = Communication_Type_MotionControl_MIT;
@@ -190,56 +197,47 @@ static void mi_motor_pack(const struct device *dev, struct can_frame *frame) { 
    frame->data[7] = kd_tmp & 0xFF;

    break;

  case PV:

    mi_can_id->mi_msg_mode = Communication_Type_GetSingleParameter;

    mi_can_id->mi_msg_mode = Communication_Type_SetSingleParameter;

    index[0]=Limit_Spd;

    index[1]=Loc_Ref;

    memcpy(&frame->data[0], &index[0], 2);

    

    pos_tmp = float_to_uint(data->target_pos, P_MIN, P_MAX, 32);

    vel_tmp = float_to_uint(data->target_radps, V_MIN, V_MAX, 32);

    frame->data[0] = (Limit_Spd >> 8) & 0xFF;

    frame->data[1] = Limit_Spd & 0xFF;

    frame->data[2] = 0;

    frame->data[3] = 0;

    frame->data[4] = (vel_tmp >> 24) & 0xFF;

    frame->data[5] = (vel_tmp >> 16) & 0xFF;

    frame->data[6] = (vel_tmp >> 8) & 0xFF;

    frame->data[7] = vel_tmp & 0xFF;

    memcpy(&frame->data[4], &data->target_radps, 4);

    



    mi_can_id_fol->mi_msg_mode = Communication_Type_SetSingleParameter;

    frame_follow->dlc = 8;

    frame_follow->flags = 0;

    frame_follow->data[0] = (Loc_Ref >> 8) & 0xff;

    frame_follow->data[1] = Loc_Ref & 0xff;

   

    memcpy(&frame_follow->data[0], &index[1], 2);

    frame_follow->data[2] = 0;

    frame_follow->data[3] = 0;

    frame_follow->data[4] = (pos_tmp >> 24) & 0xff;

    frame_follow->data[5] = (pos_tmp >> 16) & 0xff;

    frame_follow->data[6] = (pos_tmp >> 8) & 0xff;

    frame_follow->data[7] = pos_tmp & 0xff;

    memcpy(&frame_follow->data[4], &data->target_pos, 4);

   

    break;

  case VO:

    mi_can_id->mi_msg_mode = Communication_Type_GetSingleParameter;

    mi_can_id->mi_msg_mode = Communication_Type_SetSingleParameter;

    vel_tmp = float_to_uint(data->target_radps, V_MIN, V_MAX, 32);

    cur_tep = float_to_uint(data->limit_cur, 0, CUR_MAX, 32);

    frame->data[0] = (Limit_Cur >> 8) & 0xFF;

    frame->data[1] = Limit_Cur & 0xFF;

    index[0]=Limit_Cur;

    index[1]=Spd_Ref;

    memcpy(&frame->data[0], &index[0], 2);

    

    frame->data[2] = 0;

    frame->data[3] = 0;

    frame->data[4] = (cur_tep >> 24) & 0xFF;

    frame->data[5] = (cur_tep >> 16) & 0xFF;

    frame->data[6] = (cur_tep >> 8) & 0xFF;

    frame->data[7] = cur_tep & 0xFF;

    memcpy(&frame->data[4], &data->limit_cur, 4);

    



    mi_can_id_fol->mi_msg_mode = Communication_Type_SetSingleParameter;



    frame_follow->dlc = 8;

    frame_follow->flags = 0;

    frame_follow->data[0] = (Spd_Ref >> 8) & 0xff;

    frame_follow->data[1] = Spd_Ref & 0xff;

    

    memcpy(&frame_follow->data[0], &index[1], 2);

    frame_follow->data[2] = 0;

    frame_follow->data[3] = 0;

    frame_follow->data[4] = (vel_tmp >> 24) & 0xff;

    frame_follow->data[5] = (vel_tmp >> 16) & 0xff;

    frame_follow->data[6] = (vel_tmp >> 8) & 0xff;

    frame_follow->data[7] = vel_tmp & 0xff;

    memcpy(&frame_follow->data[4], &data->target_radps, 4);

    

    break;

  default:

    break;
@@ -262,9 +260,10 @@ float mi_motor_get_torque(const struct device *dev) { 
}



int mi_motor_set_mode(const struct device *dev, enum motor_mode mode) {

  

  struct mi_motor_data *data = dev->data;

  const struct mi_motor_cfg *cfg = dev->config;

  char mode_str[10];

  char mode_str[10] = {0};



  data->common.mode = mode;
@@ -279,7 +278,7 @@ int mi_motor_set_mode(const struct device *dev, enum motor_mode mode) { 
    break;

  case VO:

    strcpy(mode_str, "vo");



data->limit_cur=23.0f;

    break;

  case MULTILOOP:

    data->online = false;
@@ -289,6 +288,25 @@ int mi_motor_set_mode(const struct device *dev, enum motor_mode mode) { 
    LOG_DBG("Unknown motor mode: %d", mode);

    break;

  }

struct can_frame frame={0};

struct mi_can_id *mi_can_id = (struct mi_can_id *)&(frame.id);

mi_can_id->mi_msg_mode = Communication_Type_SetSingleParameter;

mi_can_id->id = cfg->common.id;

    frame.flags = CAN_FRAME_IDE;

    frame.dlc = 8;

    uint16_t index=Run_mode;

    memcpy(&frame.data[0], &index, 2);



    frame.data[4] = (uint8_t)mode;

    int can_id = get_can_id(dev);

    int err = 0;

  if (k_sem_take(&tx_queue_sem[can_id], K_NO_WAIT) == 0) {

    err = can_send(cfg->common.phy, &frame, K_NO_WAIT, can_tx_callback,

                   &tx_queue_sem[can_id]);

  }

  if (err != 0) {

    LOG_ERR("Failed to send CAN frame: %d", err);

  }

  

  for (int i = 0; i < SIZE_OF_ARRAY(cfg->common.capabilities); i++) {

    if (cfg->common.pid_datas[i]->pid_dev == NULL) {
@@ -326,7 +344,7 @@ int mi_motor_set_speed(const struct device *dev, float speed_rpm) { 
int mi_motor_set_angle(const struct device *dev, float angle) {

  struct mi_motor_data *data = dev->data;



  data->target_pos = RAD2DEG * angle;

  data->target_pos = angle/RAD2DEG;



  return 0;

}
@@ -335,7 +353,7 @@ static int get_motor_id(struct can_frame *frame) { 
  for (int i = 0; i < MOTOR_COUNT; i++) {

    const struct device *dev = motor_devices[i];

    const struct mi_motor_cfg *cfg = (const struct mi_motor_cfg *)(dev->config);

    if ((cfg->common.rx_id & 0xFF) == (frame->id & 0xFF)) {

    if ((cfg->common.id & 0xFF) == (frame->id & 0xFF00) >> 8) {

      return i;

    }

  }
@@ -346,6 +364,7 @@ static struct can_filter filters[CAN_COUNT]; 


static void mi_can_rx_handler(const struct device *can_dev,

                              struct can_frame *frame, void *user_data) {

                                // LOG_DBG("mi_can_rx_handler");

  int id = get_motor_id(frame);

  if (id == -1) {

    LOG_ERR("Unknown motor ID: %d", frame->id);
@@ -362,7 +381,7 @@ static void mi_can_rx_handler(const struct device *can_dev, 
    }

    data->missed_times--;

  }

  struct mi_can_id *can_id = (struct mi_can_id *)(frame->id);

  struct mi_can_id *can_id = (struct mi_can_id *)&(frame->id);

  if (can_id->mi_msg_mode == Communication_Type_MotorFeedback) {

    data->err = ((can_id->data) >> 8) & 0x1f;

    data->update = true;
@@ -378,6 +397,7 @@ static void mi_can_rx_handler(const struct device *can_dev, 
}



void mi_rx_data_handler(struct k_work *work) {

  // LOG_DBG("mi_rx_data_handler");

  for (int i = 0; i < MOTOR_COUNT; i++) {

    struct mi_motor_data *data =

        (struct mi_motor_data *)(motor_devices[i]->data);
@@ -408,10 +428,12 @@ void mi_rx_data_handler(struct k_work *work) { 
}



void mi_tx_isr_handler(struct k_timer *dummy) {

  // LOG_DBG("mi_tx_isr_handler");

  k_work_submit_to_queue(&mi_work_queue, &mi_tx_data_handle);

}



void mi_isr_init_handler(struct k_timer *dummy) {

  LOG_DBG("mi_isr_init_handler");

  dummy->expiry_fn = mi_tx_isr_handler;

  k_work_queue_start(&mi_work_queue, mi_work_queue_stack, CAN_SEND_STACK_SIZE,

                     CAN_SEND_PRIORITY, NULL);
@@ -419,7 +441,10 @@ void mi_isr_init_handler(struct k_timer *dummy) { 
}



void mi_tx_data_handler(struct k_work *work) {

  // LOG_DBG("mi_tx_data_handler");

  struct can_frame tx_frame[2] = {0};





  tx_frame[0].flags = CAN_FRAME_IDE;

  tx_frame[1].flags = CAN_FRAME_IDE;

  for (int i = 0; i < MOTOR_COUNT; i++) {
@@ -428,23 +453,26 @@ tx_frame[1].flags = CAN_FRAME_IDE; 
    if (data->online && data->missed_times <= 5) {

      int can_id = get_can_id(motor_devices[i]);



      mi_motor_pack(motor_devices[i], &tx_frame[0]);

      mi_motor_pack(motor_devices[i], tx_frame);

      struct tx_frame queued_frame = {

          .can_dev = cfg->common.phy,

          .sem = &tx_queue_sem[can_id],

          .sem = &(tx_queue_sem[can_id]),

          .frame = tx_frame[0],

      };



      mi_send_queued(&queued_frame, &mi_can_tx_msgq);

      mi_queue_proceed(&mi_can_tx_msgq);



      if ((data->common.mode == PV) || (data->common.mode == VO)) {

        mi_motor_pack(motor_devices[i], &tx_frame[1]);

        struct tx_frame queued_frame = {

        

        struct tx_frame queued_frame_fol = {

            .can_dev = cfg->common.phy,

            .sem = &tx_queue_sem[can_id],

            .frame = tx_frame[1],

        };



        mi_send_queued(&queued_frame, &mi_can_tx_msgq);

        mi_send_queued(&queued_frame_fol, &mi_can_tx_msgq);

        mi_queue_proceed(&mi_can_tx_msgq);

      }

      data->missed_times++;

      if (data->missed_times > 5) {
@@ -454,20 +482,17 @@ tx_frame[1].flags = CAN_FRAME_IDE; 
                i, data->missed_times);

        continue;

      }

      if (data->missed_times > 3 && data->err > 1) {

        mi_motor_control(motor_devices[i], CLEAR_ERROR);

      // if (data->missed_times > 3 && data->err > 1) {

      //   mi_motor_control(motor_devices[i], CLEAR_ERROR);

      // }

    }

  }

}

  mi_queue_proceed(&mi_can_tx_msgq);

}



void mi_init_handler(struct k_work *work) {

  LOG_DBG("mi_init_handler");

  k_timer_stop(&mi_tx_timer);

  LOG_DBG("mi motor control thread started");

  

  uint32_t id_full1[CAN_COUNT] = {0};

  uint32_t id_full0[CAN_COUNT] = {0};

for (int i = 0; i < MOTOR_COUNT; i++) {

    int can_id = 0;

    const struct mi_motor_cfg *cfg =
@@ -479,15 +504,6 @@ void mi_init_handler(struct k_work *work) { 
        break;

      }

    }

    if (filters[can_id].id == 0) {

      filters[can_id].id = cfg->common.rx_id & 0xFF;

      id_full1[can_id] = cfg->common.rx_id & 0xFF;

      id_full0[can_id] = ~(cfg->common.rx_id & 0xFF);

    }

    filters[can_id].id &= cfg->common.rx_id & 0xFF;

    id_full1[can_id] &= cfg->common.rx_id & 0xFF;

    id_full0[can_id] &= ~(cfg->common.rx_id & 0xFF);



    int err = can_start(cfg->common.phy);

    if (err) {

      LOG_ERR("Failed to start CAN device: %d", err);
@@ -496,8 +512,9 @@ void mi_init_handler(struct k_work *work) { 


  for (int i = 0; i < CAN_COUNT; i++) {

    const struct device *can_dev = can_devices[i];



    filters[i].mask = (id_full1[i] | id_full0[i]) & 0x7FF;

    filters[i].flags = CAN_FILTER_IDE;

    filters[i].mask = 0x02000000;

    filters[i].id = 0x02000000;

    int err = can_add_rx_filter(can_dev, mi_can_rx_handler, 0, &filters[i]);

    if (err < 0) {

      LOG_ERR("Error adding CAN filter (err %d)", err);
@@ -513,7 +530,7 @@ void mi_init_handler(struct k_work *work) { 
    mi_motor_control(motor_devices[i], ENABLE_MOTOR);

  }



  k_timer_start(&mi_tx_timer, K_NO_WAIT, K_MSEC(2));

  k_timer_start(&mi_tx_timer, K_NO_WAIT, K_MSEC(1));

  k_timer_user_data_set(&mi_tx_timer, &mi_tx_data_handle);

}

drivers/motor/mi/motor_mi.h

+5 −5
Original line number Diff line number Diff line
@@ -48,7 +48,7 @@ 
#define PI 3.14159265f

#define SIZE_OF_ARRAY(x) (sizeof(x) / sizeof(x[0]))

#define RAD2ROUND 1.0f / (2 * PI)

#define RAD2DEG   180.0f / PI

#define RAD2DEG   (180.0f/PI)

//参数读取宏定义

#define Run_mode      0x7005

#define Iq_Ref        0x7006
@@ -72,7 +72,7 @@ 
#define Motor_Error 0x00

#define Motor_OK    0x01



#define CAN_SEND_STACK_SIZE 4096

#define CAN_SEND_STACK_SIZE 2048

#define CAN_SEND_PRIORITY   -1



enum CONTROL_MODE //控制模式定义
@@ -95,10 +95,10 @@ enum ERROR_TAG //错误回传对照 
};



struct mi_can_id {

    uint32_t mi_msg_mode   : 5;  // 通信类型

    uint32_t data   : 16; // 数据区

	uint32_t id     : 8;  // 目标ID

    // uint32_t res    : 3;  // 保留位src/yunqiu

    uint32_t data   : 16; // 数据区

    uint32_t mi_msg_mode   : 5;  // 通信类型

    uint32_t res    : 3;  // 保留位src/yunqiu

};

struct mi_motor_data {

    struct motor_driver_data common;

include/ares/board/init.c

+1 −0
Original line number Diff line number Diff line
@@ -165,6 +165,7 @@ void led_init(void) 


void board_init(void)

{

	k_sleep(K_MSEC(550));

	PWR_INIT

	LED_INIT

	printk("Board init done.\n");

CRA Git | Maintained and supported by SUSTech CRA and CCSE