mirs_esp_pio
読み取り中…
検索中…
一致する文字列を見つけられません
SystemContext.hpp
[詳解]
1#pragma once
2#include <Arduino.h>
3#include <freertos/FreeRTOS.h>
4#include <freertos/queue.h>
5#include <freertos/semphr.h>
6
7// タスク間共有の置き場所。tasksとlibの結合点。
8// ROSの型(Twist等)は持ち込まない。
9// Motion系はcontrol task、Sensor系はsensor taskが書込み、ros taskが読む。
10// (1構造体に2 writerを置かない。get-modify-setの消失更新を避けるため)
11
12// ROS速度指令(ros_task → control)。深さ1のmailboxで最新値のみ受渡す
15 float linear_x = 0.0f;
16 float angular_z = 0.0f;
17 uint32_t stamp_ms = 0;
18};
19
20// 車体・PIDパラメータ(/params受信→各タスク参照)。mutex保護
21// 既定値は config.yaml と同一に保つこと(単一真実はyaml。起動時はyaml値が
22// /params経由で上書きする。ROS不在時のみ本既定値で動作する)。
25 double wheel_radius = 0.0391;
26 double wheel_base = 0.39;
27 double rkp = 40.0, rki = 150.0, rkd = 0.4;
28 double lkp = 40.0, lki = 150.0, lkd = 0.4;
29
34 bool operator==(const SharedParams &other) const {
35 return wheel_radius == other.wheel_radius && wheel_base == other.wheel_base &&
36 rkp == other.rkp && rki == other.rki && rkd == other.rkd &&
37 lkp == other.lkp && lki == other.lki && lkd == other.lkd;
38 }
39};
40
41// 運動系テレメトリ(control task書込→ros task発行)。mutex保護
44 int32_t count_l = 0;
45 int32_t count_r = 0;
46 float vel_l = 0.0f;
47 float vel_r = 0.0f;
48 float vel_cmd_l = 0.0f;
49 float vel_cmd_r = 0.0f;
50 uint16_t rc_pulse[3] = {0, 0, 0};
51 uint8_t ctrl_mode = 0; // 0:MANUAL 1:ROS2
52 float odom_x = 0.0f; // オドメトリ推定位置・姿勢(/odomで配信)
53 float odom_y = 0.0f;
54 float odom_theta = 0.0f;
55 float lin_vel = 0.0f; // 推定直進速度 [m/s](制御時パラメータで確定済み)
56 float ang_vel = 0.0f; // 推定角速度 [rad/s](同上)
57};
58
59// センサ系テレメトリ(sensor task書込→ros task発行)。mutex保護
62 float voltage_1 = 0.0f;
63 float voltage_2 = 0.0f;
64 bool imu_ok = false;
65 bool mag_ok = false;
66 float ax = 0.0f, ay = 0.0f, az = 0.0f;
67 float gx = 0.0f, gy = 0.0f, gz = 0.0f;
68 float mx = 0.0f, my = 0.0f, mz = 0.0f;
69};
70
72template <typename T>
74public:
76 void create() {
77 if (mutex_ == nullptr) {
78 mutex_ = xSemaphoreCreateMutex();
79 }
80 }
81
85 void set(const T &value) {
86 if (mutex_ == nullptr) {
87 value_ = value;
88 return;
89 }
90 if (xSemaphoreTake(mutex_, pdMS_TO_TICKS(5)) == pdTRUE) {
91 value_ = value;
92 xSemaphoreGive(mutex_);
93 }
94 }
95
99 T get() const {
100 if (mutex_ == nullptr) {
101 return value_;
102 }
103 if (xSemaphoreTake(mutex_, pdMS_TO_TICKS(5)) == pdTRUE) {
104 cache_ = value_;
105 xSemaphoreGive(mutex_);
106 }
107 return cache_;
108 }
109
110private:
111 T value_{};
112 mutable T cache_{};
113 mutable SemaphoreHandle_t mutex_ = nullptr;
114};
115
118public:
120 void begin();
121
122 // ROS指令キュー(非ブロッキング)
123 bool pushRosCmd(const RosVelocityCmd &cmd);
124 bool popRosCmd(RosVelocityCmd &cmd);
125
126 // パラメータ・テレメトリ(mutex保護コピー)
127 void setParams(const SharedParams &p) { params_.set(p); }
128 SharedParams getParams() { return params_.get(); }
129 void setMotion(const SharedMotion &m) { motion_.set(m); }
130 SharedMotion getMotion() { return motion_.get(); }
131 void setSensor(const SharedSensor &s) { sensor_.set(s); }
132 SharedSensor getSensor() { return sensor_.get(); }
133
134private:
135 QueueHandle_t ros_cmd_queue_ = nullptr;
139};
140
141extern SystemContext g_sys;
SystemContext g_sys
mutex保護の共有値。set/getの定型を集約する。
T get() const
読み出す。mutex取得失敗時は前回成功値を返す(破損値を読まない)。
void set(const T &value)
書き込む。mutex取得失敗時は捨てる。
void create()
mutex生成。
タスク間共有ストア。
void setParams(const SharedParams &p)
void begin()
キュー・mutex生成。
void setSensor(const SharedSensor &s)
bool pushRosCmd(const RosVelocityCmd &cmd)
SharedSensor getSensor()
bool popRosCmd(RosVelocityCmd &cmd)
SharedMotion getMotion()
void setMotion(const SharedMotion &m)
SharedParams getParams()
ROS速度指令メールボックス要素。
運動系テレメトリ。
車体・PID共有パラメータ。
bool operator==(const SharedParams &other) const
全フィールド一致判定(パディングの影響を受けない)。
センサ系テレメトリ。