Gitee

https://gitee.com/WangXi_Chn/RttOs_ModuleLib

开发环境

CubeMX
MDK5 IDE
STM32F407IGH6TR 芯片
大疆开发板 C型
大疆M3508直流无刷电机
大疆C620无刷电机调速器
RT-Thread操作系统
面向对象模块接口设计

硬件条件

确保Can线供电电平达到5V（不可仅使用JTAG接口供电3.3V）
Can线接口连接正确

添加驱动

CubuMX
使能Can1

配置引脚（使能的自动引脚配置可能错误，需要在右侧引脚单独点击使能）

Kconfig

    menuconfig BSP_USING_CAN
        bool "Enable CAN"
        default n
        select RT_USING_CAN
        if BSP_USING_CAN
            config BSP_USING_CAN1
                bool "using CAN1"
                default n
            config BSP_USING_CAN2
                bool "using CAN2"
                default n
        endif

Env

使能CAN1驱动
SCON命令生成MDK5工程

修改时钟
STM32F407的APB1总线一般是42MHz，但是RT-Thread波特率选项默认为45MHz

需要修改 drv_can.c 文件为如下内容

#elif defined (SOC_SERIES_STM32F4)/* APB1 42MHz(max) */
static const struct stm32_baud_rate_tab can_baud_rate_tab[] =
{
  {CAN1MBaud, (CAN_SJW_2TQ | CAN_BS1_9TQ  | CAN_BS2_4TQ | 3)},
  {CAN800kBaud, (CAN_SJW_2TQ | CAN_BS1_8TQ  | CAN_BS2_4TQ | 4)},
  {CAN500kBaud, (CAN_SJW_2TQ | CAN_BS1_9TQ  | CAN_BS2_4TQ | 6)},
  {CAN250kBaud, (CAN_SJW_2TQ | CAN_BS1_9TQ  | CAN_BS2_4TQ | 12)},
  {CAN125kBaud, (CAN_SJW_2TQ | CAN_BS1_9TQ  | CAN_BS2_4TQ | 24)},
  {CAN100kBaud, (CAN_SJW_2TQ | CAN_BS1_9TQ  | CAN_BS2_4TQ | 30)},
  {CAN50kBaud, (CAN_SJW_2TQ | CAN_BS1_9TQ  | CAN_BS2_4TQ | 60)},
  {CAN20kBaud, (CAN_SJW_2TQ | CAN_BS1_9TQ  | CAN_BS2_4TQ | 150)},
  {CAN10kBaud, (CAN_SJW_2TQ | CAN_BS1_9TQ  | CAN_BS2_4TQ | 300)}
};

注意这个位置

接口函数

查找设备

打开设备

控制设备

发送数据

设置接收回调

接收数据

关闭设备

官方例程

/*
 * 程序清单：这是一个 CAN 设备使用例程
 * 例程导出了 can_sample 命令到控制终端
 * 命令调用格式：can_sample can1
 * 命令解释：命令第二个参数是要使用的 CAN 设备名称，为空则使用默认的 CAN 设备
 * 程序功能：通过 CAN 设备发送一帧，并创建一个线程接收数据然后打印输出。
*/
#include <rtthread.h>
#include "rtdevice.h"
#define CAN_DEV_NAME       "can1"      /* CAN 设备名称 */
static struct rt_semaphore rx_sem;     /* 用于接收消息的信号量 */
static rt_device_t can_dev;            /* CAN 设备句柄 */
/* 接收数据回调函数 */
static rt_err_t can_rx_call(rt_device_t dev, rt_size_t size)
{
    /* CAN 接收到数据后产生中断，调用此回调函数，然后发送接收信号量 */
    rt_sem_release(&rx_sem);
    return RT_EOK;
}
static void can_rx_thread(void *parameter)
{
    int i;
    rt_err_t res;
    struct rt_can_msg rxmsg = {0};
    /* 设置接收回调函数 */
    rt_device_set_rx_indicate(can_dev, can_rx_call);
#ifdef RT_CAN_USING_HDR
    struct rt_can_filter_item items[5] =
    {
        RT_CAN_FILTER_ITEM_INIT(0x100, 0, 0, 1, 0x700, RT_NULL, RT_NULL), /* std,match ID:0x100~0x1ff，hdr 为 - 1，设置默认过滤表 */
        RT_CAN_FILTER_ITEM_INIT(0x300, 0, 0, 1, 0x700, RT_NULL, RT_NULL), /* std,match ID:0x300~0x3ff，hdr 为 - 1 */
        RT_CAN_FILTER_ITEM_INIT(0x211, 0, 0, 1, 0x7ff, RT_NULL, RT_NULL), /* std,match ID:0x211，hdr 为 - 1 */
        RT_CAN_FILTER_STD_INIT(0x486, RT_NULL, RT_NULL),                  /* std,match ID:0x486，hdr 为 - 1 */
        {0x555, 0, 0, 1, 0x7ff, 7,}                                       /* std,match ID:0x555，hdr 为 7，指定设置 7 号过滤表 */
    };
    struct rt_can_filter_config cfg = {5, 1, items}; /* 一共有 5 个过滤表 */
    /* 设置硬件过滤表 */
    res = rt_device_control(can_dev, RT_CAN_CMD_SET_FILTER, &cfg);
    RT_ASSERT(res == RT_EOK);
#endif
    while (1)
    {
        /* hdr 值为 - 1，表示直接从 uselist 链表读取数据 */
        rxmsg.hdr = -1;
        /* 阻塞等待接收信号量 */
        rt_sem_take(&rx_sem, RT_WAITING_FOREVER);
        /* 从 CAN 读取一帧数据 */
        rt_device_read(can_dev, 0, &rxmsg, sizeof(rxmsg));
        /* 打印数据 ID 及内容 */
        rt_kprintf("ID:%x", rxmsg.id);
        for (i = 0; i < 8; i++)
        {
            rt_kprintf("%2x", rxmsg.data[i]);
        }
        rt_kprintf("\n");
    }
}
int can_sample(int argc, char *argv[])
{
    struct rt_can_msg msg = {0};
    rt_err_t res;
    rt_size_t  size;
    rt_thread_t thread;
    char can_name[RT_NAME_MAX];
    if (argc == 2)
    {
        rt_strncpy(can_name, argv[1], RT_NAME_MAX);
    }
    else
    {
        rt_strncpy(can_name, CAN_DEV_NAME, RT_NAME_MAX);
    }
    /* 查找 CAN 设备 */
    can_dev = rt_device_find(can_name);
    if (!can_dev)
    {
        rt_kprintf("find %s failed!\n", can_name);
        return RT_ERROR;
    }
    /* 初始化 CAN 接收信号量 */
    rt_sem_init(&rx_sem, "rx_sem", 0, RT_IPC_FLAG_FIFO);
    /* 以中断接收及发送方式打开 CAN 设备 */
    res = rt_device_open(can_dev, RT_DEVICE_FLAG_INT_TX | RT_DEVICE_FLAG_INT_RX);
    RT_ASSERT(res == RT_EOK);
    /* 创建数据接收线程 */
    thread = rt_thread_create("can_rx", can_rx_thread, RT_NULL, 1024, 25, 10);
    if (thread != RT_NULL)
    {
        rt_thread_startup(thread);
    }
    else
    {
        rt_kprintf("create can_rx thread failed!\n");
    }
    msg.id = 0x78;              /* ID 为 0x78 */
    msg.ide = RT_CAN_STDID;     /* 标准格式 */
    msg.rtr = RT_CAN_DTR;       /* 数据帧 */
    msg.len = 8;                /* 数据长度为 8 */
    /* 待发送的 8 字节数据 */
    msg.data[0] = 0x00;
    msg.data[1] = 0x11;
    msg.data[2] = 0x22;
    msg.data[3] = 0x33;
    msg.data[4] = 0x44;
    msg.data[5] = 0x55;
    msg.data[6] = 0x66;
    msg.data[7] = 0x77;
    /* 发送一帧 CAN 数据 */
    size = rt_device_write(can_dev, 0, &msg, sizeof(msg));
    if (size == 0)
    {
        rt_kprintf("can dev write data failed!\n");
    }
    return res;
}
/* 导出到 msh 命令列表中 */
MSH_CMD_EXPORT(can_sample, can device sample);

适配模块接口

Module_3508Motor.h

/*
 * Copyright (c) 2020 - ~, HIT_HERO Team
 *
 * 2508MOTOR_CAN MODULE HEAD FILE
 * Used in RT-Thread Operate System
 *
 * Change Logs:
 * Date           Author       Notes                Mail
 * 2020-08-02     WangXi       first version        WangXi_chn@foxmail.com
 */
#ifndef _MODULE_3508MOTOR_H_
#define _MODULE_3508MOTOR_H_
#include <rtthread.h>
#include <rtdevice.h>
#include <board.h>
#include "Math_PID.h"
struct _MODULE_MOTOR3508
{
    /* Property */
    char * CanDevName; 
    rt_uint32_t MOTOR3508ID_HEAD;
    rt_uint32_t MOTOR3508ID_NUM;
    rt_uint32_t MOTOR3508ID_FEED;
    MATH_PID Motor3508_PID;
    /* Value */
    rt_device_t can_dev;
    struct rt_can_msg can_msg;
    struct rt_can_msg can_mrg;
    rt_int32_t motor_AimCurrent;
    rt_int32_t motor_AimAngSpeed;
    rt_int32_t motor_AimAngle;
    rt_int32_t motor_RealCurrent;
    rt_int32_t motor_RealAngSpeed;
    rt_int32_t motor_RealAngle;
    rt_uint8_t motor_Temperature;
    /* Method */
    void (*Init)(struct _MODULE_MOTOR3508 *module);
    void (*Send)(struct _MODULE_MOTOR3508 *module);
    void (*Feed)(struct _MODULE_MOTOR3508 *module);
    rt_err_t (*Handle)(rt_device_t dev,rt_size_t size);
};
typedef struct _MODULE_MOTOR3508 MODULE_MOTOR3508;
/* Glodal Method */
rt_err_t Module_Motor3508_Config(MODULE_MOTOR3508 *Dev_Motor3508);
#endif
/************************ (C) COPYRIGHT 2020 WANGXI **************END OF FILE****/

Module_3508Motor.c

/*
 * Copyright (c) 2020 - ~, HIT_HERO Team
 *
 * 2508MOTOR_CAN MODULE SOUCE FILE
 * Used in RT-Thread Operate System
 *
 * Change Logs:
 * Date           Author       Notes                Mail
 * 2020-08-02     WangXi       first version        WangXi_chn@foxmail.com
 */
#include "Module_3508Motor.h"
/* User Code Begin*/
/* User Code End */
/* Static Method */
static void Module_Motor3508Init(MODULE_MOTOR3508 *module);
static void Module_Motor3508Send(MODULE_MOTOR3508 *module);
static void Module_Motor3508Feed(MODULE_MOTOR3508 *module);
static rt_err_t Module_Motor3508Handle(rt_device_t dev, rt_size_t size);
static struct rt_semaphore can_sem;
/* Global Method */
rt_err_t Module_Motor3508_Config(MODULE_MOTOR3508 *Dev_Motor3508)
{
    if( Dev_Motor3508->Init        ==NULL &&
        Dev_Motor3508->Handle    ==NULL &&
        Dev_Motor3508->Send        ==NULL &&
        Dev_Motor3508->Feed        ==NULL 
    ){  
        /* Link the Method */
        Dev_Motor3508->Init     = Module_Motor3508Init;
        Dev_Motor3508->Handle     = Module_Motor3508Handle;
        Dev_Motor3508->Send        = Module_Motor3508Send;
        Dev_Motor3508->Feed        = Module_Motor3508Feed;
    }
    else{
        rt_kprintf("Warning: Module Motor 3508 is Configed twice\n");
        return RT_ERROR;
    }
    /* Device Init */
    Dev_Motor3508->Init(Dev_Motor3508);
    /* Module PID controler Init */
    Module_PID_Config(&(Dev_Motor3508->Motor3508_PID));
    return RT_EOK;
}
/* Static Method */
static void Module_Motor3508Init(MODULE_MOTOR3508 *module)
{    
    rt_err_t res;
    /* link can devices */
    module->can_dev = rt_device_find(module->CanDevName);
    if (!module->can_dev)
        rt_kprintf("find %s failed!\n", module->CanDevName);
    /* init semaphone */
    rt_sem_init(&can_sem, "can_sem", 0, RT_IPC_FLAG_FIFO);
    res = rt_device_open(module->can_dev, RT_DEVICE_FLAG_INT_TX | RT_DEVICE_FLAG_INT_RX);
    RT_ASSERT(res == RT_EOK);
    /* set up hardware filter */
    struct rt_can_filter_item items[1] =
    {
        //    id                            ide        rtr        mode        mask        hdr    
        {    module->MOTOR3508ID_FEED,    0,        0,        1,            0xFFFF,        1        }
    };
    struct rt_can_filter_config cfg = {1, 1, items};
    res = rt_device_control(module->can_dev, RT_CAN_CMD_SET_FILTER, &cfg);
    RT_ASSERT(res == RT_EOK);
    /* set up can baudrate */
    rt_device_control(module->can_dev, RT_CAN_CMD_SET_BAUD, (void *)CAN1MBaud);
    /* send frame preprocess */
    module->can_msg.id = module->MOTOR3508ID_HEAD;
    module->can_msg.ide = RT_CAN_STDID;
    module->can_msg.rtr = RT_CAN_DTR;
    module->can_msg.len = 8;
    /* feed frame preprocess */
    module->can_mrg.hdr = 1;
    res = rt_device_control(module->can_dev, RT_CAN_CMD_SET_MODE, (void *)RT_CAN_MODE_NORMAL);
    RT_ASSERT(res == RT_EOK);
    /* link call back function */
    rt_device_set_rx_indicate(module->can_dev, module->Handle);
}
static void Module_Motor3508Send(MODULE_MOTOR3508 *module)
{
    rt_size_t  size;
    module->motor_AimCurrent = module->Motor3508_PID.Value_out;
    /* limit output */
    module->motor_AimCurrent = module->motor_AimCurrent > 16384? 16384 : module->motor_AimCurrent;
    module->motor_AimCurrent = module->motor_AimCurrent < -16384? -16384 : module->motor_AimCurrent;
    if(module->MOTOR3508ID_HEAD == 0x200)
    {
        module->can_msg.data[module->MOTOR3508ID_NUM*2-1]=module->motor_AimCurrent&0XFF;
        module->can_msg.data[module->MOTOR3508ID_NUM*2-2]=module->motor_AimCurrent>>8;
    }
    else if(module->MOTOR3508ID_HEAD == 0x1FF)
    {
        module->can_msg.data[module->MOTOR3508ID_NUM*2-9]=module->motor_AimCurrent&0XFF;
        module->can_msg.data[module->MOTOR3508ID_NUM*2-10]=module->motor_AimCurrent>>8;
    }
    else
    {
        rt_kprintf("MOTOR3508ID_HEAD is a wrong number!\n");
    }
    size = rt_device_write(module->can_dev, 0, &(module->can_msg), sizeof(module->can_msg));
    if (size == 0)
        rt_kprintf("can dev write data failed!\n");
}
static rt_err_t Module_Motor3508Handle(rt_device_t dev, rt_size_t size)
{
    rt_sem_release(&can_sem);
    return RT_EOK;
}
static void Module_Motor3508Feed(MODULE_MOTOR3508 *module)
{
    rt_sem_take(&can_sem, RT_WAITING_FOREVER);
    rt_device_read(module->can_dev, 0, &(module->can_mrg), sizeof(module->can_mrg));
    module->motor_RealAngle = ((module->can_mrg.data[0]<<8) + module->can_mrg.data[1])*360/8189;      //   °
    module->motor_RealAngSpeed = ((module->can_mrg.data[2]<<8) + module->can_mrg.data[3])*360/3600;   //   °/s
    module->motor_RealCurrent = ((module->can_mrg.data[4]<<8) + module->can_mrg.data[5]);
    module->motor_Temperature = module->can_mrg.data[6];
    module->Motor3508_PID.Method_Update(    &(module->Motor3508_PID),
                                            module->motor_AimAngSpeed,
                                            module->motor_RealAngSpeed,
                                            0.5);
}
/************************ (C) COPYRIGHT 2020 WANGXI **************END OF FILE****/

Math_PID.h

/*
 * Copyright (c) 2020 - ~, HIT_HERO Team
 *
 * PID MATH MODULE HEAD FILE
 * Used in RT-Thread Operate System
 *
 * Change Logs:
 * Date           Author       Notes                Mail
 * 2020-08-05     WangXi       first version        WangXi_chn@foxmail.com
 */
#include <rtthread.h>
#include <rtdevice.h>
#include <board.h>
#ifndef _MATH_PID_H_
#define _MATH_PID_H_
struct _MATH_PID
{
    /* Property */
    float Property_kP;
    float Property_kI;
    float Property_kD;
    float Property_Imax;
    float Property_outmax;
    float Property_IerrorMax;        //允许积分的最大偏差值
    /* Value */
    float Value_dt;
    float Value_P;
    float Value_I;
    float Value_D;
    float Value_AntiIFactor;    //快速退积分系数
    float Value_set;
    float Value_feedback;
    float Value_Lasterror;
    float Value_error;
    float Value_derror;
    float Value_Lastderror;        //滤波用
    float Value_out;
    /* Method */
    void (*Method_Init)(struct _MATH_PID *module);
    void (*Method_Update)(struct _MATH_PID *module,float Aim, float Feedback,double Coafficient);
};
typedef struct _MATH_PID MATH_PID;
rt_err_t Module_PID_Config(MATH_PID *module);
#endif
/************************ (C) COPYRIGHT 2020 WANGXI **************END OF FILE****/

Math_PID.c

/*
 * Copyright (c) 2020 - ~, HIT_HERO Team
 *
 * PID MATH MODULE SOUCE FILE
 * Used in RT-Thread Operate System
 *
 * Change Logs:
 * Date           Author       Notes                Mail
 * 2020-08-05     WangXi       first version        WangXi_chn@foxmail.com
 */
#include "Math_PID.h"
#include <math.h>
static void Module_PIDInit(MATH_PID *module);
static void Module_PIDUpdate(MATH_PID *module,float Aim, float Feedback,double Coafficient);
static float constrain_float(float amt, float low, float high);
/* Global Method */
rt_err_t Module_PID_Config(MATH_PID *module)
{
    if( module->Method_Init        ==NULL &&
        module->Method_Update        ==NULL 
    ){  
        /* Link the Method */
        module->Method_Init         = Module_PIDInit;
        module->Method_Update     = Module_PIDUpdate;
    }
    else{
        rt_kprintf("Warning: Module Motor 3508 is Configed twice\n");
        return RT_ERROR;
    }
    /* Device Init */
    module->Method_Init(module);
    return RT_EOK;
}
static void Module_PIDInit(MATH_PID *module)
{
    module->Value_AntiIFactor         = 1;
    module->Value_Lastderror         = 0;
    module->Value_P                 = 0;
    module->Value_I                 = 0;
    module->Value_D                 = 0;
    module->Value_error             = 0;
    module->Value_Lasterror         = 0;
    module->Value_derror             = 0;
    module->Value_out                 = 0;
}
static void Module_PIDUpdate(MATH_PID *module,float Aim, float Feedback,double Coafficient)
{
    module->Value_set = Aim;
    module->Value_feedback = module->Value_feedback * Coafficient + Feedback * (1 - Coafficient);    
    module->Value_error= module->Value_set - module->Value_feedback;
    module->Value_derror = ((1.0f - Coafficient) * (module->Value_error - module->Value_Lasterror) + Coafficient * (module->Value_Lastderror));
    module->Value_Lastderror = module->Value_derror;
    module->Value_Lasterror = module->Value_error;
    module->Value_P = module->Property_kP * module->Value_error;
    if(fabs(module->Value_error) < module->Property_IerrorMax)    //如果偏差大于这个，不进行积分
    {
        if(fabs((float)(module->Value_I + module->Property_kI * module->Value_error + module->Value_P)) < module->Property_outmax)
        {
            module->Value_I += (float)(module->Property_kI * module->Value_error * module->Value_AntiIFactor);
            module->Value_I = constrain_float(module->Value_I, -module->Property_Imax, +module->Property_Imax);
        }
    }
    module->Value_D = module->Property_kD * module->Value_derror;
    module->Value_out = module->Value_P + module->Value_I + module->Value_D;
    module->Value_out = constrain_float(module->Value_out, -module->Property_outmax, +module->Property_outmax)/100.0f;
}
 /**
  * @brief  输出限幅
  * @param  int32_t amt：待输出量
    *                    int32_t low：输出最小值
    *                    int32_t high：输出最大值
  * @retval int32_t X：限幅后的输出量
  */
static float constrain_float(float amt, float low, float high) 
{
    return ((amt)<(low)?(low):((amt)>(high)?(high):(amt)));
}
/************************ (C) COPYRIGHT 2020 WANGXI **************END OF FILE****/

使用main.c

#include <rtthread.h>
#include <rtdevice.h>
#include <board.h>
#include "Module_3508Motor.h"
MODULE_MOTOR3508 dev_motor3508_1 = 
    {
        .CanDevName            = "can1",
        .MOTOR3508ID_HEAD    = 0x200,
        .MOTOR3508ID_NUM    = 1,
        .MOTOR3508ID_FEED    = 0x201,
        .Motor3508_PID = 
            {
                .Property_kP         = 15,
                .Property_kI        = 3,
                .Property_kD        = 50,
                .Property_Imax        = 500,
                .Property_outmax     = 1638400,
                .Property_IerrorMax = 5000
            }
    };
static void motor_entry(void *parameter)
{
    while (1)
    {
        dev_motor3508_1.Feed(&dev_motor3508_1);        
        rt_thread_mdelay(5);
        dev_motor3508_1.Send(&dev_motor3508_1);                
    }
}
int main(void)
{    
    Module_Motor3508_Config(&dev_motor3508_1);
    dev_motor3508_1.motor_AimAngSpeed = 5;
    /* system running motor control */
    rt_thread_t motor_thread = rt_thread_create("motor", motor_entry, RT_NULL,
                                  1024, RT_THREAD_PRIORITY_MAX - 3, 20);
    if (led_thread != RT_NULL){
        rt_thread_startup(motor_thread);
    }
}

部分解释

模块头文件中的Property注释下的变量为属性，是结构体声明时必须定义的数值，否则无法正常工作
模块头文件中的Value注释下的变量为中间变量，是计算过程中的必要数值，可能是输入记录或输出等，可用来调试修改和观察，其初始化赋值在模块的初始化函数中自动进行
模块头文件中的Method注释下的变量为函数指针，是该模块提供的函数方法，可通过结构体变量访问成员直接调用
模块在使用前必须通过提供的全局方法（Global Method）处理，主要作用是绑定方法函数，初始化变量，初始化驱动，使能外设等
目前电机刷入电流、读出数据功能可正常使用，但PID参数和算法还需要调整，慎重使用

优势
面向对象设计，大大减少全局变量，宏定义等量，近乎没有，移植性强
将PID模块嵌入到电机模块中
- PID模块暂未得到实际测试，算法和参数正确与否不确定，但代码结构固定
Can接收中断仅释放信号量，在线程中处理接收数据，比在中断中处理数据更安全
对模块内的函数static静态保护，实现切实的封装，仅可通过访问结构体变量调用函数
仿C#使用方法，减少使用时的工作量

存在问题
单电机驱动仅修改电机ID即可
但驱动多个电机时，Can外设会被多次开启，RTT操作系统报错
驱动多电机时，控制帧是一个电机一个电机发送，控制效率低，不符合M3508报文设计
接受多电机返回数据时会发生报文错乱
电机PID控制缺少验证，而且不满足速度闭环角度闭环的需求
以上问题均在下一节中的电机群设计解决

开发编程

【RT-Thread】DJI_C620电调Can驱动

Gitee

开发环境

硬件条件

添加驱动

CubuMX

Kconfig

Env

修改时钟

接口函数

查找设备

打开设备

控制设备

发送数据

设置接收回调

接收数据

关闭设备

官方例程

适配模块接口

Module_3508Motor.h

Module_3508Motor.c

Math_PID.h

Math_PID.c

使用main.c

部分解释

优势

存在问题