Raspberry Pi Picoを使ってラジコン受信機からの信号を読み取りモータを回す

アイキャッチ

はじめに

マルチコプタを制御するためにRaspberry Pi Pico（以下PIco）でラジコン受信機からの信号（S.BUS信号）を読み取りモータドライバ（ESC）にPWM信号を送りDutyを可変しブラシレスモータの回転数を制御してみました。

はじめに
UARTで受信機の信号を読み取る
PWM信号をESCに送る
二つのプログラムを合体
- 主な特徴
- Dutyの計算
おわりに

UARTで受信機の信号を読み取る

これは以前にも当ブログで取り上げていました。

Picoで受信機の信号を読み取るには受信機の信号をインバータで反転して UARTで読み取ります。

方法については以前の記事をご参照ください。

rikei-tawamure.com

PWM信号をESCに送る

ブラシレスモータのモータドライバであるESCはPWM信号を受け取りそのDuty（信号がHighの状態の時間幅）でモータのスピードを可変します。

この方法は今となっては古い方法で、最近のESCはシリアル通信でデジタルで回転速度指令を受け付けることもできます。

PicoでPWM信号を作る方法についても当ブログで以前お話ししていますので詳しくはそちらをご覧ください。

rikei-tawamure.com

二つのプログラムを合体

主な特徴

今回は、受信機信号を読み取るプログラムとPWMを作り出すプログラムを合体させました。

その際

受信機からの各CHの信号はグローバル変数で受ける
受信機からの信号は以前と同様、信号を受信した際の割り込みで処理している
UARTやPWMのための初期化関数を改めて設ける
Dutyの可変は読み込んだ信号をメインの無限ループの中で行う
PicoとUSB接続で各CHからの受信信号をシリアル端末で確認できる

といった様なプログラムを作成しました。

Dutyの可変部分は今後、インターバルタイマー割り込みで周期的に行いたいと思います

Dutyの計算

今回は詳しい解説は割愛するのですが、Dutyの計算だけ少し触れると

UARTから読み取られるスロットル（通常チャンネル３）の値の最大値をCH3MAX、最小値をCH3MINとします。これらは、事前にレバーを一番下に下げた時と上にあげた時を計測しておいてマクロとしてプログラムでは設定します。

同様に、ESCに送るDutyを決める値の最大値と最小値をそれぞれDUTYMAX、DUTYMINとしてマクロに定義します。

そうすると以下の様な、式でdutyの値が計算できます。

(float)でキャストしているのは上記の値が整数でdutyという変数も整数なので、整数計算で割り算が０になるのを防ぐためです。

duty=(float)(DUTYMAX-DUTYMIN)*(float)(Chdata[2]-CH3MIN)/(float)(CH3MAX-CH3MIN)+DUTYMIN;

以下が、今回製作した「にこいち」プログラムです。

#include <stdio.h>
#include "pico/stdlib.h"
#include "hardware/pwm.h"
#include "hardware/uart.h"
#include "hardware/irq.h"

#define UART_ID uart0
#define BAUD_RATE 100000
#define DATA_BITS 8
#define STOP_BITS 2
#define PARITY    UART_PARITY_EVEN
#define DUTYMIN 1330
#define DUTYMAX 2315
#define CH3MAX 1707
#define CH3MIN 321


//０番と1番ピンに接続
#define UART_TX_PIN 0
#define UART_RX_PIN 1

//グルーバル変数の宣言
static int chars_rxed = 0;
static int data_num=0;
uint8_t sbus_data[25];
uint8_t ch;
uint slice_num;
uint16_t Chdata[6];

//関数の宣言
uint8_t init_serial(void);
uint8_t init_pwm();
void on_uart_rx(); 

uint8_t init_serial(void){

    /// シリアル通信の設定

    // UARTを基本の通信速度で設定
    uart_init(UART_ID, 2400);

    // 指定のGPIOをUARTのTX、RXピンに設定する
    gpio_set_function(UART_TX_PIN, GPIO_FUNC_UART);
    gpio_set_function(UART_RX_PIN, GPIO_FUNC_UART);

    //指定のUARTを指定の通信速度に設定する
    int actual = uart_set_baudrate(UART_ID, BAUD_RATE);

    //UART flow control CTS/RTSを使用しない設定
    uart_set_hw_flow(UART_ID, false, false);

    //通信フォーマットの設定
    uart_set_format(UART_ID, DATA_BITS, STOP_BITS, PARITY);

    // Turn off FIFO's - we want to do this character by character
    uart_set_fifo_enabled(UART_ID, false);

    // Set up a RX interrupt
    // We need to set up the handler first
    // Select correct interrupt for the UART we are using
    int UART_IRQ = UART_ID == uart0 ? UART0_IRQ : UART1_IRQ;

    // And set up and enable the interrupt handlers
    irq_set_exclusive_handler(UART_IRQ, on_uart_rx);
    irq_set_enabled(UART_IRQ, true);

    // Now enable the UART to send interrupts - RX only
    uart_set_irq_enables(UART_ID, true, false);
}

uint8_t init_pwm(){

   // PWMの設定
   // Tell GPIO 0 and 1 they are allocated to the PWM
    gpio_set_function(2, GPIO_FUNC_PWM);
    gpio_set_function(3, GPIO_FUNC_PWM);

    // Find out which PWM slice is connected to GPIO 0 (it's slice 0)
    slice_num = pwm_gpio_to_slice_num(3);

    // Set period
    pwm_set_wrap(slice_num, 24999);
    pwm_set_clkdiv(slice_num, 100.0);
    // Set channel A output high for one cycle before dropping
    pwm_set_chan_level(slice_num, PWM_CHAN_A, 2315);
    // Set initial B output high for three cycles before dropping
    pwm_set_chan_level(slice_num, PWM_CHAN_B, 1330);
    // Set the PWM running
    pwm_set_enabled(slice_num, true);
    /// \end::setup_pwm[]
    sleep_ms(2000);
    pwm_set_chan_level(slice_num, PWM_CHAN_A, 1330);
}

// RX interrupt handler
void on_uart_rx() {
    short data;
    while (uart_is_readable(UART_ID)) {
        ch = uart_getc(UART_ID);
        if(ch==0x0f&&chars_rxed==00){
            sbus_data[chars_rxed]=ch;
            //printf("%02X ",ch);
            chars_rxed++;
        }
        else if(chars_rxed>0){
            sbus_data[chars_rxed]=ch;
            //printf("%02X ",ch);
            chars_rxed++;            
        }
        
        switch(chars_rxed){
            case 3:
                Chdata[0]=(sbus_data[1]|(sbus_data[2]<<8)&0x07ff);
                //printf("%04d ",data);
                break;
            case 4:
                Chdata[1]=(sbus_data[3]<<5|sbus_data[2]>>3)&0x07ff;
                //printf("%04d ",data);
                break;
            case 6:
                Chdata[2]=(sbus_data[3]>>6|sbus_data[4]<<2|sbus_data[5]<<10)&0x07ff;
                //printf("%04d ",data);
                break;
            case 7:
                Chdata[3]=(sbus_data[6]<<7|sbus_data[5]>>1)&0x07ff;
                //printf("%04d ",data);
                break;
            case 8:
                Chdata[4]=(sbus_data[7]<<4|sbus_data[6]>>4)&0x07ff;
                //printf("%04d ",data);
                break;
            case 10:
                Chdata[5]=(sbus_data[7]>>7|sbus_data[8]<<1|sbus_data[9]<<9)&0x07ff;
                //printf("%04d ",data);
                break;
        }

        if(chars_rxed==25){
            //printf("\n");
            chars_rxed=0;
        }
    }
}

int main() {
    stdio_init_all();
    init_serial();
    init_pwm();

    uint16_t duty;

    sleep_ms(5000);
    while(true){
      tight_loop_contents();
      duty=(float)(DUTYMAX-DUTYMIN)*(float)(Chdata[2]-CH3MIN)/(float)(CH3MAX-CH3MIN)+DUTYMIN;
      if (duty>DUTYMAX)duty=DUTYMAX;
      if (duty<DUTYMIN)duty=DUTYMIN;
      pwm_set_chan_level(slice_num, PWM_CHAN_A, duty);
      printf("%04d %04d %04d %04d %04d %04d \n",Chdata[0], Chdata[1], duty, Chdata[3], Chdata[4], Chdata[5]);
      sleep_ms(10);
    }
}