3.5 其实,这个该放开头——PWM电调

3.5 其实,这个该放开头——PWM电调

上小节讲过接收机的三种输入信号之一就是PWM,可以用于控制舵机与无刷电调,这小节的目的就是在飞控上编写四路这样的PWM信号,用于控制无人机的四人电机。

本节目标输出这样的PWM信号:

周期为2.5ms,即频率400Hz,高电平宽度在1ms到2ms之间变化用于控制电机从静止到满油门转动。

3.5.1 AT32 PWM相关的寄存器与库函数

(1)PWM相关的寄存器
  • 14.2.4.7 TMR2到TMR5通道模式寄存器1(TMRx_CM1)
  • 14.2.4.8 TMR2到TMR5通道模式寄存器2(TMRx_CM2)
    两个寄存器只使用了16个位,而每8个位用于设置一个定时器通道,共能设置4个通道,这里以1通道举例:
    • 位 1:0 C1C
      通道 1 配置 (Channel 1 configure)
      当 C1EN=’0’时,这些位用于选择通道 1 为输出或输入,
      以及输入时的映射选择:
      00:输出;
      01:输入,C1IN 映射在 C1IFP1 上;
      10:输入,C1IN 映射在 C2IFP1 上;
      11:输入,C1IN 映射在 STCI 上,只有在 STIS 选择内部
      触发输入时才工作。
      这里我们使用其PWM功能,即00输出功能
    • 位 3 C1OBEN
      通道 1 输出缓存使能(Channel 1 output buffer enable)
      0:关闭 TMRx_C1DT 的缓存功能,写入 TMRx_C1DT 的
      内容会立即生效。
      1:启用 TMRx_C1DT 的缓存功能,写入 TMRx_C1DT 的
      内容将保存到缓存寄存器中,当发生溢出事件时再更新到
      TMRx_C1DT 中。
      这个位是与PWM的比较值有关的使能。
    • 位 6:4 C1OCTRL
      通道 1 输出控制(Channel 1 output control)
      这些位用于设置原始信号 C1ORAW 的工作状态。
      ……
      110:PWM 模式 A
      -OWCDIR=0,若 TMRx_C1DT>TMRx_CVAL 时设置
      C1ORAW 为高,否则为低;
      -OWCDIR=1,若 TMRx_ C1DT <TMRx_CVAL 时设置
      C1ORAW 为低,否则为高。
      111:PWM 模式 B
      -OWCDIR=0,若 TMRx_ C1DT >TMRx_CVAL 时设置
      C1ORAW 为低,否则为高;
      -OWCDIR=1,若 TMRx_ C1DT <TMRx_CVAL 时设置
      C1ORAW 为高,否则为低。

注:除’000’外,其余配置下 C1OUT 将连接到
C1ORAW,C1OUT 的输出电平除了会根据 C1ORAW 变
化外,还与 CCTRL 所配置的输出极性有关。

可以看出PWM输出的具体形式由该C1OCTRL位、OWCDIR、CCTRL共同决定,其中OWCDIR是TMR2到TMR5控制寄存器1(TMRx_CTRL1)中的一个位,用于控制计数是向上还是向下计数,程序设置OWCDIR=0,即向上计数(从0开始递增)。

  • 14.2.4.9 TMR2到TMR5通道控制寄存器(TMRx_CCTRL)
    这个寄存器只用到了16位,第4个位设置1个通道,以通道1为例:

  • 位 0 C1EN
    通道 1 使能 (Channel 1 enable)
    0:禁止输入或输出;
    1:使能输入或输出。

  • 位 1 C1P
    通道 1 极性 (Channel 1 polarity)
    通道 1 配置为输出:
    0:C1OUT 的有效电平为高
    1:C1OUT 的有效电平为低

  • 14.2.4.13 TMR2到TMR5通道1数据寄存器(TMRx_C1DT)

  • 14.2.4.14 TMR2到TMR5通道2数据寄存器(TMRx_C2DT)

  • 14.2.4.15 TMR2到TMR5通道3数据寄存器(TMRx_C3DT)

  • 14.2.4.16 TMR2到TMR5通道4数据寄存器(TMRx_C4DT)
    4个寄存器对应4个定时器通道,这里以通道1为例:
    C1DT 是将要和 CVAL(也就是定时器的计数值) 进行比较的值,写入的值是否会立即生效取决于输出缓存使能位(C1OBEN),并根据设置在 通道1对应引脚 上产生相应的输出(即PWM)。

为了便于直观设置,使TMRx_ C1DT所设置的值代表高电平的宽度,将TMR2到TMR5通道控制寄存器(TMRx_CCTRL)中的通道极性设置为高电平有效,将PWM设置为模式A(即若 TMRx_C1DT>TMRx_CVAL 时设置
C1ORAW 为高,否则为低;)

(2)PWM相关的库函数

PWM本质上是定时器功能的一部分,PWM的周期就是定时器完成一次溢出计数的周期,所以会用到之前讲的定时器相关的基本的库函数,不再缀述。

  • 函数tmr_output_default_para_init

    • 函数原型
      void tmr_output_default_para_init(tmr_output_config_type *tmr_output_struct);
    • 功能描述
      初始化 tmr 输出默认参数
    • 输入参数
      tmr_output_struct:指向结构体 tmr_output_config_type 的待初始化指针
  • 函数tmr_output_channel_config

    • 函数原型
      void tmr_output_channel_config(tmr_type *tmr_x, tmr_channel_select_type
      tmr_channel, tmr_output_config_type *tmr_output_struct);
    • 功能描述
      配置 TMR 输出通道
    • 输入参数 1
      tmr_x:所选择的 TMR 外设,该参数可以选取自其中之一:
      TMR1, TMR2, TMR3, TMR4, TMR5, TMR8, TMR9, TMR10, TMR11, TMR12,
      TMR13, TMR14, TMR20
    • 输入参数 2
      tmr_channel:定时器通道
    • 输入参数 3
      tmr_output_struct:指向结构体 tmr_output_config_type 的指针
  • 函数tmr_channel_value_set

    • 函数原型
      void tmr_channel_value_set(tmr_type *tmr_x, tmr_channel_select_type
      tmr_channel, uint32_t tmr_channel_value);
    • 功能描述
      设置 TMR 通道值
    • 输入参数 1
      tmr_x:所选择的 TMR 外设,该参数可以选取自其中之一:
      TMR1, TMR2, TMR3, TMR4, TMR5, TMR8, TMR9, TMR10, TMR11, TMR12,
      TMR13, TMR14, TMR20
    • 输入参数 2
      tmr_channel:定时器通道
    • 输入参数 3
      tmr_channel_value:定时器通道值,16 位定时器可取 0x0000~0xFFFF,32 位定
      时器可取 0x0000_0000~0xFFFF_FFFF
  • 函数tmr_output_channel_buffer_enable
    具体作用还不太清楚

    • 函数原型
      void tmr_output_channel_buffer_enable(tmr_type *tmr_x,
      tmr_channel_select_type tmr_channel, confirm_state new_state);
    • 功能描述
      启用或禁用 TMR 输出通道缓冲区
    • 输入参数 1
      tmr_x:所选择的 TMR 外设,该参数可以选取自其中之一:
      TMR1, TMR2, TMR3, TMR4, TMR5, TMR8, TMR9, TMR10, TMR11, TMR12,
      TMR13, TMR14, TMR20
    • 输入参数 2
      tmr_channel:定时器通道
    • 输入参数 3
      new_state:将要配置的输出通道缓冲区状态,可选择启用(TRUE)或禁用
      (FALSE)

3.5.2 硬件设计

小节例程功能:使用遥控器油门操纵杆控制四路电机转动
!!!注意去掉桨叶
!!!注意去掉桨叶
!!!注意去掉桨叶

用到的硬件资源除了上一小节的蓝牙与遥控器外还有:
(1)TMR4定时器的4路PWM,即
T4_CH1_PB6
T4_CH2_PB7
T4_CH3_PB8
T4_CH4_PB9

(2)四合一电调,该电调与飞控使用8P SH1.0接线进行连接,要注意接线顺序。
(3)无刷电机
在这里插入图片描述

图3.5.2.1 四路PWM
在这里插入图片描述

图3.5.2.2 四路PWM 2

图3.5.2.3 飞控与电调接线

3.5.3 软件设计

(1)电机PWM初始化

打开“2、飞控例程\5_Moters”下的工程,在main.c的开始任务中有如下代码:

void START_task_function(void *pvParameters)		//开始任务
{
	
	/****************************************** 进入临界区,原子操作 ***************************************/
	taskENTER_CRITICAL();
	
	/****************************************** 地面站串口初始化 *****************************************/
	printfSerialInit();
	ANO_init_usart(230400);
	
	/****************************************** LED初始化 ************************************************/
	My_LED_init();
	
	/****************************************** 遥控器初始化 ************************************************/
	Remoter_init();
	
	/****************************************** 航模遥控器初始化 ************************************/
	Motor_init();
	
	
#ifdef QY_BY_CORE_BOARD			//AT32核心板
	
#else							//AT32飞控
	
#endif
	
	
	/* 创建其他任务 */
	qy_tasks_creat();	

	
	vTaskDelete(START_handler);		//删除开始任务
	/* 退出临界区 */
	taskEXIT_CRITICAL();
}

行18:
为电机初始化,调用了Motor_init()函数。

Motor_init()函数定义在“qy_drivers\pwm.c”中,原型为:

void Motor_init(void)
{	
	/* system peripheral configuration */
	crm_configuration();

	/* get system clock */
	crm_clocks_freq_get(&crm_clocks_freq_struct);

	/* gpio configuration */
	gpio_configuration();

	/* compute the prescaler value */
	prescalervalue = (uint16_t) ((crm_clocks_freq_struct.apb1_freq * 2) / 1000000) - 1;		//计数频率1M

//Motor PWM初始化
	/* tmr4 time base configuration */
	tmr_base_init(TMR4, 2500, prescalervalue);		//PWM周期:2.5ms
	tmr_cnt_dir_set(TMR4, TMR_COUNT_UP);
	tmr_clock_source_div_set(TMR4, TMR_CLOCK_DIV1);

	tmr_output_default_para_init(&tmr_oc_init_structure);
	tmr_oc_init_structure.oc_mode = TMR_OUTPUT_CONTROL_PWM_MODE_A;
	tmr_oc_init_structure.oc_idle_state = FALSE;
	tmr_oc_init_structure.oc_polarity = TMR_OUTPUT_ACTIVE_HIGH;
	tmr_oc_init_structure.oc_output_state = TRUE;
	
	tmr_output_channel_config(TMR4, TMR_SELECT_CHANNEL_1, &tmr_oc_init_structure);
	tmr_channel_value_set(TMR4, TMR_SELECT_CHANNEL_1, 1000);
	tmr_output_channel_buffer_enable(TMR4, TMR_SELECT_CHANNEL_1, TRUE);

	tmr_output_channel_config(TMR4, TMR_SELECT_CHANNEL_2, &tmr_oc_init_structure);
	tmr_channel_value_set(TMR4, TMR_SELECT_CHANNEL_2, 1000);
	tmr_output_channel_buffer_enable(TMR4, TMR_SELECT_CHANNEL_2, TRUE);

	tmr_output_channel_config(TMR4, TMR_SELECT_CHANNEL_3, &tmr_oc_init_structure);
	tmr_channel_value_set(TMR4, TMR_SELECT_CHANNEL_3, 1000);
	tmr_output_channel_buffer_enable(TMR4, TMR_SELECT_CHANNEL_3, TRUE);

	tmr_output_channel_config(TMR4, TMR_SELECT_CHANNEL_4, &tmr_oc_init_structure);
	tmr_channel_value_set(TMR4, TMR_SELECT_CHANNEL_4, 1000);
	tmr_output_channel_buffer_enable(TMR4, TMR_SELECT_CHANNEL_4, TRUE);

	tmr_period_buffer_enable(TMR4, TRUE);

	/* tmr enable counter */
	tmr_counter_enable(TMR4, TRUE);
	
//Servo PWM初始化
	/* tmr3 time base configuration */
	tmr_base_init(TMR3,  2500, prescalervalue);		//PWM周期:2.5ms
	tmr_cnt_dir_set(TMR3, TMR_COUNT_UP);
	tmr_clock_source_div_set(TMR3, TMR_CLOCK_DIV1);

	tmr_output_default_para_init(&tmr_oc_init_structure);
	tmr_oc_init_structure.oc_mode = TMR_OUTPUT_CONTROL_PWM_MODE_A;
	tmr_oc_init_structure.oc_idle_state = FALSE;
	tmr_oc_init_structure.oc_polarity = TMR_OUTPUT_ACTIVE_HIGH;
	tmr_oc_init_structure.oc_output_state = TRUE;
	
	tmr_output_channel_config(TMR3, TMR_SELECT_CHANNEL_1, &tmr_oc_init_structure);
	tmr_channel_value_set(TMR3, TMR_SELECT_CHANNEL_1, 1500);
	tmr_output_channel_buffer_enable(TMR3, TMR_SELECT_CHANNEL_1, TRUE);

	tmr_output_channel_config(TMR3, TMR_SELECT_CHANNEL_2, &tmr_oc_init_structure);
	tmr_channel_value_set(TMR3, TMR_SELECT_CHANNEL_2, 1500);
	tmr_output_channel_buffer_enable(TMR3, TMR_SELECT_CHANNEL_2, TRUE);

	tmr_output_channel_config(TMR3, TMR_SELECT_CHANNEL_3, &tmr_oc_init_structure);
	tmr_channel_value_set(TMR3, TMR_SELECT_CHANNEL_3, 1500);
	tmr_output_channel_buffer_enable(TMR3, TMR_SELECT_CHANNEL_3, TRUE);

	tmr_output_channel_config(TMR3, TMR_SELECT_CHANNEL_4, &tmr_oc_init_structure);
	tmr_channel_value_set(TMR3, TMR_SELECT_CHANNEL_4, 1500);
	tmr_output_channel_buffer_enable(TMR3, TMR_SELECT_CHANNEL_4, TRUE);

	tmr_period_buffer_enable(TMR3, TRUE);

	/* tmr enable counter */
	tmr_counter_enable(TMR3, TRUE);
	

}

行3~10:
使能时钟、得到总线时钟频率、初始化GPIO,其中GPIO相关的初始化都放在了gpio_configuration();函数中。

行13:
计算定时器的分频系数,这里目标定时器计数的频率为1M Hz,具体计数方法与上小节定时器部分提到的相同。

行16~19:
初始化定时器,设置为向上计数,设置重装载值为2500,结合1M Hz的计数频率,PWM的周期(也即定时器溢出的周期)就是2.5ms

行21~41:
初始化定时器的4个通道为PWM输出,并设置PWM初始高电平的宽度为1ms,用于控制电机的话表示电机停转。可以看出,行22、24处将PWM的控制方式设置为PWM模式A、高电平有效。

行43~46:
定时器使能的部分。

行49~79:
AT32飞控有8路PWM输出接口,这里只用到了其中4路用于控制电机。而另外4路PWM常用于控制舵机,这里没用到但写下初始化预备着,原理与4路电机PWM类似。

(2)遥控器控制电机

在qy_Remoter_task.c的遥控器任务中我们加入如下内容:

/* Remoter任务函数 */
void Remoter_task_function(void *pvParameters)
{
	
	while(1)
	{	
		……
		Channel_Action();
		
		vTaskDelay(10);
	}
}


行8:
可以看到这里调用了Channel_Action()函数,用于将遥控器油门操纵杆的通道值与电机的功率联系在一起,该函数定义在remoter.c中:

//通道数据功能
void Channel_Action(void)
{
    Motor_set_pwm(MOT1, PWM_DUTY_MAX*Remoter_post.THR_unit);
    Motor_set_pwm(MOT2, PWM_DUTY_MAX*Remoter_post.THR_unit);
    Motor_set_pwm(MOT3, PWM_DUTY_MAX*Remoter_post.THR_unit);
    Motor_set_pwm(MOT4, PWM_DUTY_MAX*Remoter_post.THR_unit);
}

其中Remoter_post.THR_unit为油门操纵杆归一化的值。PWM_DUTY_MAX是表示电机最大功率值的一个宏。Motor_set_pwm()函数会根据输入参数计算控制各电机PWM的装载值,并最终控制电机。
Motor_set_pwm()函数定义在pwm.c中,原型为:

/*********************************************************
*@brief  设置电机功率、舵机角度
*@param  M_pwm    电机对应pwm索引(0~7)
*@param  Duty     电机速度(0 ~ 1000)
*@return 无
*********************************************************/
void Motor_set_pwm(Moters_index_e M_index, int16 Duty)
{
    Duty = limit(Duty, 0, PWM_DUTY_MAX);
	Duty = Duty+1000;     //控制脉宽占部脉宽2/5
	
    switch(M_index)
    {
        case MOT1:
        {
           tmr_channel_value_set(TMR4, TMR_SELECT_CHANNEL_1, Duty);
        }break;

		case MOT2:
        {
           tmr_channel_value_set(TMR4, TMR_SELECT_CHANNEL_2, Duty);
        }break;
		
		case MOT3:
        {
           tmr_channel_value_set(TMR4, TMR_SELECT_CHANNEL_3, Duty);
        }break;
		
		case MOT4:
        {
           tmr_channel_value_set(TMR4, TMR_SELECT_CHANNEL_4, Duty);
        }break;
		
		
		
		case SER1:
        {
           tmr_channel_value_set(TMR3, TMR_SELECT_CHANNEL_1, Duty);
        }break;
		
		case SER2:
        {
           tmr_channel_value_set(TMR3, TMR_SELECT_CHANNEL_2, Duty);
        }break;
		
		case SER3:
        {
           tmr_channel_value_set(TMR3, TMR_SELECT_CHANNEL_3, Duty);
        }break;
		
		case SER4:
        {
           tmr_channel_value_set(TMR3, TMR_SELECT_CHANNEL_4, Duty);
        }break;
		
		
        default:
        break;
    }
}

可以看到,开关部分计算了PWM装载值,并进行了限幅。

3.5.4 下载验证

再次提醒,安全起见,请:
!!!注意去掉桨叶
!!!注意去掉桨叶
!!!注意去掉桨叶

进入“2、飞控例程\5_Moters”,打开工程并编译。

按住BOOT按键,给飞控上电,进入Bootloader(同时进入USB的DFU模式)。编译例程,双击“Download_tool_ISP”文件夹下的“1_Download_by_usb.bat”,开始下载程序。

该例程的功能为:使用遥控器油门操纵杆控制四路电机转动。

下载成功后,接上电池,等待电机发出一阵提示音后就可以使用遥控器控制电机转动了。注意缓慢向上推动油门操纵杆,电机就会随其提高转速。

在这里插入图片描述

图3.5.4.1

确保四个电机都能正常转动,否则的话可以检查一下飞控与电调之间、电调与电机之间的接线是否正常,程序是否正常运行。

,并进行了限幅。

3.5.4 下载验证

再次提醒,安全起见,请:
!!!注意去掉桨叶
!!!注意去掉桨叶
!!!注意去掉桨叶

进入“2、飞控例程\5_Moters”,打开工程并编译。

按住BOOT按键,给飞控上电,进入Bootloader(同时进入USB的DFU模式)。编译例程,双击“Download_tool_ISP”文件夹下的“1_Download_by_usb.bat”,开始下载程序。

该例程的功能为:使用遥控器油门操纵杆控制四路电机转动。

下载成功后,接上电池,等待电机发出一阵提示音后就可以使用遥控器控制电机转动了。注意缓慢向上推动油门操纵杆,电机就会随其提高转速。

[外链图片转存中…(img-bJbKSZgU-1702797404036)]
图3.5.4.1

确保四个电机都能正常转动,否则的话可以检查一下飞控与电调之间、电调与电机之间的接线是否正常,程序是否正常运行。

  • 0
    点赞
  • 2
    收藏
    觉得还不错? 一键收藏
  • 0
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值