ultra96+petalinux2019.2+ros2 dashingでpwm_controlをしてみた

これの続き。

subscriberを改造し、uio経由でPWM制御します。uioはここを参考にしています。

ultra96v2_oobでは以下の通りの準備が必要です。

ros_controlの作法とかc++の作法は知らないのであしからず。

1.configでCONFIG_SUBSYSTEM_REMOVE_PL_DTBのオプションを無効にし、BOOTARGSでuioを有効にするオプションを追加する。

diff --git a/project-spec/configs/config b/project-spec/configs/config
index 8663a10..806558c 100644
--- a/project-spec/configs/config
+++ b/project-spec/configs/config
@@ -154,7 +154,7 @@ CONFIG_SUBSYSTEM_BOOTARGS_EARLYPRINTK=y

 CONFIG_SUBSYSTEM_DEVICETREE_FLAGS=""
 # CONFIG_SUBSYSTEM_DTB_OVERLAY is not set
-CONFIG_SUBSYSTEM_REMOVE_PL_DTB=y
+# CONFIG_SUBSYSTEM_REMOVE_PL_DTB is not set

 #
 # ARM Trusted Firmware Compilation Configuration
@@ -241,4 +241,4 @@ CONFIG_YOCTO_ENABLE_DEBUG_TWEAKS=y
 # User Layers
 #
 CONFIG_USER_LAYER_0=""
-CONFIG_SUBSYSTEM_BOOTARGS_GENERATED="earlycon console=ttyPS0,115200 clk_ignore_unused root=/dev/mmcblk0p2 rw rootwait"
+CONFIG_SUBSYSTEM_BOOTARGS_GENERATED="earlycon console=ttyPS0,115200 clk_ignore_unused root=/dev/mmcblk0p2 rw rootwait uio_pdrv_genirq.of_id=generic-uio"
diff --git a/project-spec/meta-plnx-generated/recipes-bsp/device-tree/device-tree.bbappend b/project-spec/meta-plnx-generated/recipes-bsp/device-tree/device-tree.bbappend
index 2bce90c..b5f8609 100644
--- a/project-spec/meta-plnx-generated/recipes-bsp/device-tree/device-tree.bbappend
+++ b/project-spec/meta-plnx-generated/recipes-bsp/device-tree/device-tree.bbappend
@@ -3,7 +3,6 @@ YAML_MAIN_MEMORY_CONFIG_forcevariable = "PSU_DDR_0"
 XSCTH_WS = "${TOPDIR}/../components/plnx_workspace/device-tree"
 FILESEXTRAPATHS_append := ":${sysconf}"
 YAML_CONSOLE_DEVICE_CONFIG_forcevariable = "psu_uart_1"
-YAML_REMOVE_PL_DT = "1"
 export PETALINUX
 SRC_URI_append ="\
     file://config\

device-treeに自動生成されたPL設定を反映するためにCONFIG_SUBSYSTEM_REMOVE_PL_DTBを無効にし、uio使用するためのパラメータを追加しました。

2.設定が衝突するdevice-treeの記述の修正とPWM_w_Int_0をuioで使用する設定を行う

diff --git a/project-spec/meta-user/recipes-bsp/device-tree/files/system-user.dtsi b/project-spec/meta-user/recipes-bsp/device-tree/files/system-user.dtsi
index d7be975..cb4ce5e 100755
--- a/project-spec/meta-user/recipes-bsp/device-tree/files/system-user.dtsi
+++ b/project-spec/meta-user/recipes-bsp/device-tree/files/system-user.dtsi
@@ -194,18 +194,22 @@
                 reg = <0>;
         };
 };
-&amba {
-       axi_intc_0: axi-interrupt-ctrl {
-               #interrupt-cells = <2>;
-               compatible = "xlnx,xps-intc-1.00.a";
-               interrupt-controller;
-               reg = <0x0 0x80020000 0x0 0x1000>;
-               xlnx,kind-of-intr = <0x0>;
-               xlnx,num-intr-inputs = <0x20>;
-               interrupt-parent = <&gic>;
-               interrupts = <0 89 4>;
-       };

+&axi_intc_0 {
+       #interrupt-cells = <2>;
+       compatible = "xlnx,xps-intc-1.00.a";
+       interrupt-controller;
+       reg = <0x0 0x80020000 0x0 0x1000>;
+       xlnx,kind-of-intr = <0x0>;
+       xlnx,num-intr-inputs = <0x20>;
+       interrupt-parent = <&gic>;
+       interrupts = <0 89 4>;
+};
+&PWM_w_Int_0 {
+       compatible = "generic-uio";
+};
+
+&amba {
        zyxclmm_drm {
                compatible = "xlnx,zocl";
                status = "okay";
                                

axi_intc_0の設定が衝突するので、system-user.dtsiのaxi_intc_0を参照して設定を上書きするように変更ました。(もしかしたらaxi_intc_0の設定は消すだけでもいいかもしれないけど調べていない)

ultra96v2_oobではPWM用のIPがPWM_w_Int_0として用意してあり、0x80070000にアサインされています。

この信号の出力はLow Speed Expansion Connectorの29ピンに接続されているので、テスターで設定に従って動作しているか調べることが可能です。

3.ros2 dashingからアクセスできるようにpwm_controlノードを準備する。

rclcppとstd_msgsに依存したament_cmakeでbuildするpwm_controlパッケージを作成します。pwm_controlのソースは以下の通りとしました。ホストのcolconでbuild可能なことを確認し、git commit=>hashを指定しての.bbファイルを作成して、petalinux-user-image.bbのインストール対象にpwm_controlを含めました。

//pwm_control.cpp
#include <cstdio>
#include <memory>
#include <fcntl.h>
#include <unistd.h>
#include <string>
#include <sys/mman.h>
#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/int32.hpp>

class PwmControl : public rclcpp::Node
{
public:
  explicit PwmControl(const std::string & topic_name)
  : Node("pwm_control")
  {
    /* メモリアクセス用デバイスファイルを開く */
    int fd;
    const char* file_name = "/dev/uio4";
    const int size = 0x1000;
    if ((fd = open(file_name, O_RDWR | O_SYNC)) < 0) {
      throw std::runtime_error("Failed to init Pwm_W_Int");
    }

    /* ARM(CPU)から見た物理アドレス → 仮想アドレスへのマッピング */
    uio_ptr = static_cast<int*>(mmap(NULL, size, PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0));
    if (uio_ptr == MAP_FAILED) {
      close(fd);
      throw std::runtime_error("Failed to init Pwm_W_Int mmap");
    }
    
    // pwm_valueトピックのコールバック関数
    auto callback =
      [this](const std_msgs::msg::Int32::UniquePtr msg) -> void
      {
	*(volatile unsigned int*)(uio_ptr) = msg->data;
	RCLCPP_INFO(this->get_logger(), "pwm_value:%d", msg->data);
      };

    // pwm_valueトピックの受信設定
    rclcpp::QoS qos(rclcpp::KeepLast(10));
    sub_ = create_subscription<std_msgs::msg::Int32>(
      topic_name, qos, callback);
  }

  ~PwmControl(){
    delete(uio_ptr);
  }

private:
  rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr sub_;
  int* uio_ptr;
};

int main(int argc, char * argv[])
{
  setvbuf(stdout, NULL, _IONBF, BUFSIZ);
  rclcpp::init(argc, argv);

  auto node = std::make_shared<PwmControl>("pwm_value");
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}

4.bitbakeからの動作確認。

sdに書き込みultra96を起動後以下のコマンドを実行。

AvnetのPWM_w_Intは先頭レジスタでDuty比を指定する仕組みのようなので、topicでDuty比が指定できることを確認しました。

ちなみにDuty比の上限は99%(990000)となっているようです。

#source ros2_setup.sh
#ros2 run pwm_control pwm_control &
#ros2 topic pub /pwm_value std_msgs/Int32 "{data: 800000}"

テスターでLow Speed Expansion Connectorの29ピンをチェックし、指定した値に応じた電圧値が出ていることを確認しました。

ros2からは普通にuio経由でアクセスすればPL部の機能にアクセス可能ということがわかりました。

次はデバイスファイルとトピック名をパラメーター指定するのと、Lチカさせてみるのが目標かな。

参考ページ

https://qiita.com/iwatake2222/items/da91ce4dc2a8a8df3c0a