mirs_esp_pio
読み取り中…
検索中…
一致する文字列を見つけられません
main.cpp
[詳解]
1/*
2 * mirs_esp_pio - micro-ROS ESP32 メインプログラム
3 *
4 * タスク構成(詳細は README.md の仕様章):
5 * control task (Core0): RC読取→調停→PID/直結→モータ出力。ROS非依存で完結
6 * ros task (Core1): micro-ROS通信・テレメトリ発行・パラメータ反映
7 * sensor task (Core1): IMU・電圧ポーリング
8 *
9 * トピック:
10 * Subscribe: /cmd_vel (geometry_msgs/Twist)
11 * /params (mirs_msgs/BasicParam)
12 * /reset_odometry (std_msgs/Empty)
13 * Publish: /encoder (std_msgs/Int32MultiArray) - エンコーダーカウント
14 * /vel (std_msgs/Float64MultiArray) - 現在速度 [m/s]
15 * /vlt (std_msgs/Float64MultiArray) - バッテリー電圧 [V]
16 * /rc_debug (std_msgs/Float64MultiArray) - RC脈幅・指令・モード
17 * /imu/data_raw (sensor_msgs/Imu) - IMU接続時のみ
18 * /imu/mag (sensor_msgs/MagneticField) - IMU接続時のみ
19 * /odom (nav_msgs/Odometry) - ENABLE_EKF時はEKF値
20 */
21
22#include <Arduino.h>
23#include <freertos/FreeRTOS.h>
24#include <freertos/task.h>
25
26#include "SystemContext.hpp"
28#include "tasks/ros_task.hpp"
29#include "tasks/sensor_task.hpp"
30
31void setup() {
32 g_sys.begin();
33
34 // 各タスクは自身のハード初期化を持つ。mainは生成のみ行う
35 xTaskCreatePinnedToCore(controlTask, "control", 4096, nullptr, 10, nullptr, 0);
36 xTaskCreatePinnedToCore(rosTask, "ros", 12288, nullptr, 5, nullptr, 1);
37 xTaskCreatePinnedToCore(sensorTask, "sensor", 4096, nullptr, 3, nullptr, 1);
38}
39
40void loop() {
41 // 全処理はタスク側。Arduinoタスクはウォッチドッグ用に待機のみ
42 vTaskDelay(pdMS_TO_TICKS(1000));
43}
SystemContext g_sys
制御・通信・センサのタスク間で、指令と計測値を排他制御して受け渡す。
void begin()
キュー・mutex生成。
void controlTask(void *arg)
指令の受付、操作元の切替、速度推定、モータ出力を周期的に実行する。
void setup()
void loop()
void rosTask(void *arg)
micro-ROSの接続と再接続、指令の受信、計測値の配信を処理する。
void sensorTask(void *arg)
IMUと電圧を周期的に読み取り、制御・通信タスクへ渡す。