mirs_esp_pio
Toggle main menu visibility
読み取り中…
検索中…
一致する文字列を見つけられません
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で最新値のみ受渡す
14
struct
RosVelocityCmd
{
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不在時のみ本既定値で動作する)。
24
struct
SharedParams
{
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保護
43
struct
SharedMotion
{
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保護
61
struct
SharedSensor
{
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
72
template
<
typename
T>
73
class
SharedValue
{
74
public
:
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
110
private
:
111
T value_{};
112
mutable
T cache_{};
113
mutable
SemaphoreHandle_t mutex_ =
nullptr
;
114
};
115
117
class
SystemContext
{
118
public
:
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
134
private
:
135
QueueHandle_t ros_cmd_queue_ =
nullptr
;
136
SharedValue<SharedParams>
params_;
137
SharedValue<SharedMotion>
motion_;
138
SharedValue<SharedSensor>
sensor_;
139
};
140
141
extern
SystemContext
g_sys
;
g_sys
SystemContext g_sys
定義
SystemContext.cpp:3
SharedValue
mutex保護の共有値。set/getの定型を集約する。
定義
SystemContext.hpp:73
SharedValue::get
T get() const
読み出す。mutex取得失敗時は前回成功値を返す(破損値を読まない)。
定義
SystemContext.hpp:99
SharedValue::set
void set(const T &value)
書き込む。mutex取得失敗時は捨てる。
定義
SystemContext.hpp:85
SharedValue::create
void create()
mutex生成。
定義
SystemContext.hpp:76
SystemContext
タスク間共有ストア。
定義
SystemContext.hpp:117
SystemContext::setParams
void setParams(const SharedParams &p)
定義
SystemContext.hpp:127
SystemContext::begin
void begin()
キュー・mutex生成。
定義
SystemContext.cpp:5
SystemContext::setSensor
void setSensor(const SharedSensor &s)
定義
SystemContext.hpp:131
SystemContext::pushRosCmd
bool pushRosCmd(const RosVelocityCmd &cmd)
定義
SystemContext.cpp:15
SystemContext::getSensor
SharedSensor getSensor()
定義
SystemContext.hpp:132
SystemContext::popRosCmd
bool popRosCmd(RosVelocityCmd &cmd)
定義
SystemContext.cpp:23
SystemContext::getMotion
SharedMotion getMotion()
定義
SystemContext.hpp:130
SystemContext::setMotion
void setMotion(const SharedMotion &m)
定義
SystemContext.hpp:129
SystemContext::getParams
SharedParams getParams()
定義
SystemContext.hpp:128
RosVelocityCmd
ROS速度指令メールボックス要素。
定義
SystemContext.hpp:14
RosVelocityCmd::stamp_ms
uint32_t stamp_ms
定義
SystemContext.hpp:17
RosVelocityCmd::linear_x
float linear_x
定義
SystemContext.hpp:15
RosVelocityCmd::angular_z
float angular_z
定義
SystemContext.hpp:16
SharedMotion
運動系テレメトリ。
定義
SystemContext.hpp:43
SharedMotion::vel_l
float vel_l
定義
SystemContext.hpp:46
SharedMotion::vel_cmd_l
float vel_cmd_l
定義
SystemContext.hpp:48
SharedMotion::rc_pulse
uint16_t rc_pulse[3]
定義
SystemContext.hpp:50
SharedMotion::vel_cmd_r
float vel_cmd_r
定義
SystemContext.hpp:49
SharedMotion::odom_y
float odom_y
定義
SystemContext.hpp:53
SharedMotion::ctrl_mode
uint8_t ctrl_mode
定義
SystemContext.hpp:51
SharedMotion::count_r
int32_t count_r
定義
SystemContext.hpp:45
SharedMotion::odom_x
float odom_x
定義
SystemContext.hpp:52
SharedMotion::ang_vel
float ang_vel
定義
SystemContext.hpp:56
SharedMotion::vel_r
float vel_r
定義
SystemContext.hpp:47
SharedMotion::count_l
int32_t count_l
定義
SystemContext.hpp:44
SharedMotion::odom_theta
float odom_theta
定義
SystemContext.hpp:54
SharedMotion::lin_vel
float lin_vel
定義
SystemContext.hpp:55
SharedParams
車体・PID共有パラメータ。
定義
SystemContext.hpp:24
SharedParams::lkd
double lkd
定義
SystemContext.hpp:28
SharedParams::wheel_radius
double wheel_radius
定義
SystemContext.hpp:25
SharedParams::rkd
double rkd
定義
SystemContext.hpp:27
SharedParams::wheel_base
double wheel_base
定義
SystemContext.hpp:26
SharedParams::operator==
bool operator==(const SharedParams &other) const
全フィールド一致判定(パディングの影響を受けない)。
定義
SystemContext.hpp:34
SharedParams::rkp
double rkp
定義
SystemContext.hpp:27
SharedParams::lki
double lki
定義
SystemContext.hpp:28
SharedParams::lkp
double lkp
定義
SystemContext.hpp:28
SharedParams::rki
double rki
定義
SystemContext.hpp:27
SharedSensor
センサ系テレメトリ。
定義
SystemContext.hpp:61
SharedSensor::gz
float gz
定義
SystemContext.hpp:67
SharedSensor::imu_ok
bool imu_ok
定義
SystemContext.hpp:64
SharedSensor::ax
float ax
定義
SystemContext.hpp:66
SharedSensor::az
float az
定義
SystemContext.hpp:66
SharedSensor::my
float my
定義
SystemContext.hpp:68
SharedSensor::mx
float mx
定義
SystemContext.hpp:68
SharedSensor::mag_ok
bool mag_ok
定義
SystemContext.hpp:65
SharedSensor::voltage_1
float voltage_1
定義
SystemContext.hpp:62
SharedSensor::mz
float mz
定義
SystemContext.hpp:68
SharedSensor::gx
float gx
定義
SystemContext.hpp:67
SharedSensor::gy
float gy
定義
SystemContext.hpp:67
SharedSensor::voltage_2
float voltage_2
定義
SystemContext.hpp:63
SharedSensor::ay
float ay
定義
SystemContext.hpp:66
lib
SystemContext
SystemContext.hpp
構築:
1.18.0