之前是完成了BSP的移植和導(dǎo)入，接下來就要嘗試移植FOC算法了，開源的FOC算法也比較多，我這里打算利用SimpleFOC進(jìn)行移植。本身的SimpleFOC是基于C++的，這里要移植成C代碼。另外，SimpleFOC的SDK其中已經(jīng)適配了很多種類的傳感器，驅(qū)動(dòng)器以及無刷電機(jī)。如果硬件使用的是它已經(jīng)適配的方案，則只需要簡單配置一下就可以驅(qū)動(dòng)了。

而我這里是要在RTThread下移植FOC，更傾向于使用RTThread的框架，所以各種傳感器和驅(qū)動(dòng)器的適配計(jì)劃加到RTThread的驅(qū)動(dòng)這邊來做。FOC那邊只移植SimpleFOC的核心算法即可。所以在正式移植FOC算法之前，還需要先搭建用到的底層驅(qū)動(dòng)。

今天就先整理一下讀取磁編碼器PWM信號(hào)的輸入捕獲驅(qū)動(dòng)的移植記錄。其實(shí)某些適配更好的BSP內(nèi)的RTThread驅(qū)動(dòng)庫里面已經(jīng)有了輸入捕獲驅(qū)動(dòng)，但只是捕獲了輸入脈寬的時(shí)間，而我這里需要的是捕獲PWM信號(hào)的占空比，也就對應(yīng)了磁編碼器探測到的電機(jī)位置。但大體功能類似，所以隨便找一個(gè)類似的底層驅(qū)動(dòng)進(jìn)行一下修改和移植即可。

磁編碼器簡介

我這里用的是賽卓電子的國產(chǎn)磁編碼器芯片SC60228，詳情請看其數(shù)據(jù)手冊，主要特性如下：

移植RTT驅(qū)動(dòng)

這個(gè)比較簡單，因?yàn)镽TT驅(qū)動(dòng)庫內(nèi)已經(jīng)有了“rt_inputcapture.c”的驅(qū)動(dòng)文件，在SDK的“rt-thread/components/drivers/misc”目錄下。只不過大多數(shù)的BSP沒有做對應(yīng)的適配而已。那先不管BSP那邊的適配問題，先把這個(gè)C文件和對應(yīng)的頭文件拷貝一份，比如我重命名為“PWM_input_capture.c”和“PWM_input_capture.h”。

然后代碼內(nèi)容改動(dòng)不大，主要改的是返回的數(shù)據(jù)除了脈寬時(shí)間還有一個(gè)周期時(shí)間，這樣就可以計(jì)算輸入PWM信號(hào)的占空比了。另外，原有的驅(qū)動(dòng)上使用的是ringbuffer做了一個(gè)數(shù)據(jù)緩存，這樣數(shù)據(jù)處理可以異步話，什么時(shí)候需要什么時(shí)候把緩存內(nèi)的數(shù)據(jù)全部讀走即可。但各人考慮，我應(yīng)用的場合是用這個(gè)信號(hào)來驅(qū)動(dòng)無刷電機(jī)，這個(gè)PWM信號(hào)的輸入頻率也才1Khz，市面上大多數(shù)的無刷電機(jī)驅(qū)動(dòng)，底層控制頻率基本都達(dá)到了10Khz以上。

所以我這里肯定不需要異步處理的，會(huì)直接用這個(gè)信號(hào)觸發(fā)底層控制。而且控制效果還需要測試，如果轉(zhuǎn)速上不去或者抖動(dòng)厲害的話，可能還需要想辦法插值細(xì)化或者改用SPI讀取編碼器數(shù)據(jù)（這也是為什么硬件上做了兩種接口的原因，就是想去測試探索一些好玩的東西）。所以我這里是直接去掉了ringbuffer，加入了信號(hào)量。到時(shí)候上層開一個(gè)線程去等待這個(gè)信號(hào)量去跑FOC算法。頭文件修改如下：

struct pwm_inputcapture_data
{
rt_uint32_t pulsewidth_us; //脈寬
rt_uint32_t pulsecycle_us; //周期
};
struct pwm_inputcapture_device
{
struct rt_device parent;
const struct pwm_inputcapture_ops ops;
rt_sem_t sem;
struct pwm_inputcapture_data pulse_param;
};
/

capture operators
*/
struct pwm_inputcapture_ops
{
rt_err_t (*init)(struct pwm_inputcapture_device *inputcapture);
rt_err_t (*open)(struct pwm_inputcapture_device *inputcapture);
rt_err_t (*close)(struct pwm_inputcapture_device *inputcapture);
};
void pwm_hw_inputcapture_isr(struct pwm_inputcapture_device *inputcapture);
rt_err_t rt_device_pwm_inputcapture_register(struct pwm_inputcapture_device *inputcapture,
const char *name,
void *data);

C文件主要修改的是回調(diào)函數(shù)，把之前的數(shù)據(jù)加入ringbuffer的操作改成了釋放信號(hào)量，其它地方的修改都是一些簡單的適配，由于C代碼較多，我這里就不都貼出來了，相信大家肯定會(huì)自己完成適配，甚至比我的還要適配的好。而我的代碼，等我第一期的功能開發(fā)完了，會(huì)整體開源出來。C代碼主要修改的回調(diào)函數(shù)如下：

void rt_hw_pwm_inputcapture_isr(struct pwm_inputcapture_device *inputcapture)
{
rt_sem_release(inputcapture->sem);
if (inputcapture->parent.rx_indicate != RT_NULL)
inputcapture->parent.rx_indicate(&inputcapture->parent, 1);
}

適配BSP驅(qū)動(dòng)

BSP驅(qū)動(dòng)的適配稍微麻煩一點(diǎn)，如果大家能找到其它類似BSP內(nèi)的相似驅(qū)動(dòng)可以進(jìn)行移植，那我這里簡單找了一下并沒有找到，所以仿照RTT的驅(qū)動(dòng)適配方式，自己適配了一下。主要實(shí)現(xiàn)要點(diǎn)就是開啟每個(gè)Timer的CH0和CH1雙通道對CI0或者CI1輸入的PWM信號(hào)進(jìn)行采樣，一個(gè)捕獲脈寬，一個(gè)捕獲周期，從而得到占空比。剩下的就是一些向下調(diào)用GD32的驅(qū)動(dòng)庫API，向上適配RTT的驅(qū)動(dòng)接口。同樣，下面只給出主要的初始化代碼和中斷處理代碼，其它的可自行實(shí)現(xiàn)或者關(guān)注我后續(xù)開源的代碼。

rt_err_t pwm_inputcap_init(struct pwm_inputcapture_device *pwm_incap)
{
uint32_t sys_clk_freq;
uint32_t timer_prescaler = 1;
uint32_t trigger_ch;
timer_parameter_struct TimerConfig;
timer_ic_parameter_struct TimerICConfig;
struct gd32_pwm_inputcapture_device pwm_incap_device;
pwm_incap_device = (struct gd32_pwm_inputcapture_device )pwm_incap;
rcu_periph_clock_enable(pwm_incap_device->timer_rcu);
rcu_periph_clock_enable(pwm_incap_device->GPIO_rcu);
gpio_init(pwm_incap_device->GPIOx, GPIO_MODE_IN_FLOATING, GPIO_OSPEED_50MHZ, pwm_incap_device->PINx);
sys_clk_freq = rcu_clock_freq_get(CK_SYS);
LOG_I("system clock frequency:%d", sys_clk_freq);
TimerConfig.alignedmode = TIMER_COUNTER_EDGE;
TimerConfig.clockdivision = TIMER_CKDIV_DIV1;
TimerConfig.counterdirection = TIMER_COUNTER_UP;
TimerConfig.period = 65535U;
do{
if(sys_clk_freq / timer_prescaler / TimerConfig.period < pwm_incap_device->input_freq_min)
break;
if(timer_prescaler == 65536)
{
rt_kprintf("can not configure the prescaler for input signal frequency:%dhzn", pwm_incap_device->input_freq_min);
return RT_ERROR;
}
timer_prescaler++;
}while(1);
TimerConfig.prescaler = timer_prescaler-1;
TimerConfig.repetitioncounter = 0;
timer_init(pwm_incap_device->timerx, &TimerConfig);
LOG_I("%s timer prescaler:%d", pwm_incap_device->name, timer_prescaler);
TimerICConfig.icfilter = 10;
TimerICConfig.icpolarity = TIMER_IC_POLARITY_RISING;
TimerICConfig.icprescaler = TIMER_IC_PSC_DIV1;
TimerICConfig.icselection = TIMER_IC_SELECTION_DIRECTTI;
timer_input_pwm_capture_config(pwm_incap_device->timerx, pwm_incap_device->input_ch, &TimerICConfig);
timer_interrupt_flag_clear(pwm_incap_device->timerx, TIMER_INT_FLAG_UP);
timer_interrupt_enable(pwm_incap_device->timerx,TIMER_INT_UP);
trigger_ch = ((pwm_incap_device->input_ch == TIMER_CH_0) ? TIMER_SMCFG_TRGSEL_CI0FE0 : TIMER_SMCFG_TRGSEL_CI1FE1);
timer_input_trigger_source_select(pwm_incap_device->timerx, trigger_ch);
timer_slave_mode_select(pwm_incap_device->timerx, TIMER_SLAVE_MODE_RESTART);
timer_external_trigger_config(pwm_incap_device->timerx,TIMER_EXT_TRI_PSC_OFF,TIMER_ETP_RISING,10);
NVIC_SetPriority(pwm_incap_device->timerx_irqn, 3);
NVIC_EnableIRQ(pwm_incap_device->timerx_irqn);
timer_enable(pwm_incap_device->timerx);
return RT_EOK;
}
void pwm_inputcapture_update_isr(struct gd32_pwm_inputcapture_device device)
{
uint32_t width_ch;
/ TIM Update event */
if (timer_interrupt_flag_get(device->timerx, TIMER_INT_FLAG_UP) != RESET)
{
timer_interrupt_flag_clear(device->timerx, TIMER_INT_FLAG_UP);
device->pwm_inputcap.pulse_param.pulsecycle_us = timer_channel_capture_value_register_read(device->timerx, device->input_ch);
width_ch = ((device->input_ch == TIMER_CH_0) ? (TIMER_CH_1) : (TIMER_CH_0));
device->pwm_inputcap.pulse_param.pulsewidth_us = timer_channel_capture_value_register_read(device->timerx, width_ch);
rt_hw_pwm_inputcapture_isr(device);
}
}

修改工程構(gòu)建文件
修改相關(guān)SConscript文件

在“l(fā)ibraries/gd32_drivers/SConscript”文件內(nèi)的適當(dāng)位置加入如下代碼：

if GetDepend(['RT_USING_PWM_INPUT_CAPTURE']):
src += ['drv_pwm_inputcapture.c']
在“rt-thread/components/drivers/misc/SConscript”文件內(nèi)的適當(dāng)位置加入如下代碼：

if GetDepend(['RT_USING_INPUT_CAPTURE']):
src = src + ['rt_inputcapture.c']

意思很簡單，就是當(dāng)rtconfig.h內(nèi)定義了”RT_USING_PWM_INPUT_CAPTURE”宏，則把“drv_pwm_inputcapture.c”和“rt_inputcapture.c”驅(qū)動(dòng)文件加入工程，更準(zhǔn)確的是加入編譯。

修改相關(guān)Kconfig文件
在“rt-thread/components/drivers/Kconfig”文件內(nèi)的適當(dāng)位置加入如下代碼：

config RT_USING_INPUT_CAPTURE
bool "Using INPUT CAPTURE device drivers"
default n

管理BSP驅(qū)動(dòng)代碼的Kconfig文件不再librares目錄下，而是在board目錄下。于是在“board/Kconfig”文件內(nèi)的適當(dāng)位置，仿照其它驅(qū)動(dòng)加入如下代碼：

menuconfig BSP_USING_PWM_INPUTCAPTURE
bool "Enable pwm input capture"
default n
select RT_USING_PWM_INPUT_CAPTURE
if BSP_USING_PWM_INPUTCAPTURE
config BSP_USING_PWM_INPUTCAPTURE1
bool "Enable pwm input capture 1"
default n
config BSP_USING_PWM_INPUTCAPTURE2
bool "Enable pwm input capture 2"
default n
config BSP_USING_PWM_INPUTCAPTURE3
bool "Enable pwm input capture 3"
default n
config BSP_USING_PWM_INPUTCAPTURE4
bool "Enable pwm input capture 4"
default n
config BSP_USING_PWM_INPUTCAPTURE5
bool "Enable pwm input capture 5"
default n
config BSP_USING_PWM_INPUTCAPTURE6
bool "Enable pwm input capture 6"
default n
endif

意思也比較簡單，我這里適配了6個(gè)PWM的輸入捕獲驅(qū)動(dòng)，并且利用“select”語句，在BSP的驅(qū)動(dòng)管理里面自動(dòng)開啟了RTT驅(qū)動(dòng)里面的“RT_USING_PWM_INPUT_CAPTURE”選項(xiàng)。修改完上述代碼后，就可以用menuconfig命令或者RTThreadIDE的RT-Thread Settings圖形配置界面內(nèi)進(jìn)行配置了：

測試

驅(qū)動(dòng)有了，再在頂層邏輯內(nèi)創(chuàng)建并實(shí)現(xiàn)兩個(gè)測試線程：

int main(void)
{
...
rt_thread_t MotorL_encoder_thread;
MotorL_encoder_thread = rt_thread_create("MotorLEncoder", MotorLEncoder_thread_entry, RT_NULL, 1024, 4, 20);
rt_thread_startup(MotorL_encoder_thread);
rt_thread_t MotorR_encoder_thread;
MotorR_encoder_thread = rt_thread_create("MotorREncoder", MotorREncoder_thread_entry, RT_NULL, 1024, 4, 20);
rt_thread_startup(MotorR_encoder_thread);
...
}
void MotorLEncoder_thread_entry(void *parameter)
{
rt_device_t Lpwm_input_dev;
rt_uint16_t wait_i;
struct pwm_inputcapture_device *inputcap_dev;
Lpwm_input_dev = rt_device_find("pwm_inputcap1");
if(Lpwm_input_dev == RT_NULL)
return;
inputcap_dev = (struct pwm_inputcapture_device *)Lpwm_input_dev;
rt_device_open(Lpwm_input_dev, RT_DEVICE_OFLAG_RDONLY);
while(1)
{
if(inputcap_dev->sem != RT_NULL)
{
rt_sem_take(inputcap_dev->sem, RT_WAITING_FOREVER);
if(wait_i++ >= 1000)
{
rt_kprintf("MotorL encoder:tt%dn",inputcap_dev->pulse_param.pulsewidth_us * 10000 / inputcap_dev->pulse_param.pulsecycle_us);
wait_i = 0;
}
}
}
}
void MotorREncoder_thread_entry(void *parameter)
{
rt_device_t Rpwm_input_dev;
rt_uint16_t wait_i;
struct pwm_inputcapture_device *inputcap_dev;
Rpwm_input_dev = rt_device_find("pwm_inputcap3");
if(Rpwm_input_dev == RT_NULL)
return;
inputcap_dev = (struct pwm_inputcapture_device *)Rpwm_input_dev;
rt_device_open(Rpwm_input_dev, RT_DEVICE_OFLAG_RDONLY);
while(1)
{
if(inputcap_dev->sem != RT_NULL)
{
rt_sem_take(inputcap_dev->sem, RT_WAITING_FOREVER);
if(wait_i++ >= 1000)
{
rt_kprintf("MotorR encoder:%dn",inputcap_dev->pulse_param.pulsewidth_us * 10000 / inputcap_dev->pulse_param.pulsecycle_us);
wait_i = 0;
}
}
}
}

目前只是實(shí)現(xiàn)了大概1S鐘打印一次編碼器位置，一圈的機(jī)械角度范圍擴(kuò)大到了0~10000（我用的是12位磁編碼器，分辨率是4096，我這里統(tǒng)一歸一化到了10000），終端輸出如下：

可以看到，慢慢向一個(gè)方向推動(dòng)小車，兩個(gè)編碼器的變化規(guī)律是相反的，和實(shí)際的兩個(gè)電機(jī)對向安裝相匹配，實(shí)際使用的時(shí)候按照其中一個(gè)為基準(zhǔn)，把另外一個(gè)編碼器數(shù)據(jù)反向即可。

只看其中一個(gè)輪子，輸出頻率改為原有的1Khz，輸出值轉(zhuǎn)換為浮點(diǎn)的角度值，可得到如下的測試曲線：

靜止不動(dòng)，暫時(shí)也沒有驅(qū)動(dòng)電機(jī)，也就沒有電機(jī)的電磁干擾，在次條件下測得的靜態(tài)數(shù)據(jù)如下，靜態(tài)穩(wěn)定度在0.2度左右，比12位的最小測量精度0.088度大了二倍多一點(diǎn)：