feat(track-core): add portable training runtimes

Move scheme and PID training runtime behavior into the pure track_core layer and expose render sinks for injected strip application.

Add ESP compatibility adapters, Python bindings/test scaffolding, and in-memory render support so app_track_bt can consume core render and runtime logic without duplicating it.

Cover circular/linear rendering boundaries, all scheme runtime types, scheme render_to parity, PID sample de-duplication, speed suppression, and live tuning in track-core tests.
This commit is contained in:
2026-05-18 16:15:45 +08:00
parent 84598cad20
commit 1005e50be0
24 changed files with 4169 additions and 15 deletions
+4
View File
@@ -1,3 +1,7 @@
build/
.venv/
*.egg-info/
__pycache__/
.pytest_cache/
.DS_Store
compile_commands.json
+28 -1
View File
@@ -2,14 +2,27 @@ set(TRACK_CORE_SOURCES
src/memory_strip.cpp
src/model.cpp
src/pid_program.cpp
src/pid_runtime.cpp
src/render.cpp
src/scheme_decoder.cpp
src/scheme_runtime.cpp
)
if(DEFINED IDF_TARGET)
set(TRACK_CORE_IDF_SOURCES
src/esp/app_track_decoder.cpp
src/esp/app_track_drawer.cpp
src/esp/app_track_model.cpp
)
idf_component_register(
SRCS ${TRACK_CORE_SOURCES}
SRCS ${TRACK_CORE_SOURCES} ${TRACK_CORE_IDF_SOURCES}
INCLUDE_DIRS include
REQUIRES
app_proto
app_strip_if
app_utils
app_utils_clock
)
else()
cmake_minimum_required(VERSION 3.20)
@@ -25,7 +38,21 @@ else()
)
target_compile_features(track_core PUBLIC cxx_std_23)
option(TRACK_CORE_BUILD_PYTHON "Build Python bindings" OFF)
option(TRACK_CORE_BUILD_TESTS "Build track-core tests" ON)
if(TRACK_CORE_BUILD_PYTHON)
find_package(Python 3.9 COMPONENTS Interpreter Development.Module REQUIRED)
find_package(nanobind CONFIG REQUIRED)
nanobind_add_module(_core
src/python/bindings.cpp
)
target_link_libraries(_core PRIVATE track_core::track_core)
target_compile_features(_core PRIVATE cxx_std_23)
install(TARGETS _core LIBRARY DESTINATION track_core)
endif()
if(TRACK_CORE_BUILD_TESTS)
enable_testing()
add_executable(track_core_tests tests/track_core_tests.cpp)
+17
View File
@@ -15,3 +15,20 @@ ctest --test-dir build --output-on-failure
When used inside an ESP-IDF project under `components/track-core`, the same
`CMakeLists.txt` registers an IDF component.
## Python bindings
The Python package builds a nanobind extension for the emulator-facing API:
```bash
python3 -m venv .venv
. .venv/bin/activate
python -m pip install -e ".[emulator,test]"
python -m pytest python/tests
python -m track_core.emulator
```
The DearPyGui emulator can either render a manual static report for boundary
checks or load an SM, MT, ST, or RSMT scheme and tick the portable training
runtime with explicit frame deltas. Both paths render pixels through the same
C++ planner used by firmware adapters.
+59
View File
@@ -0,0 +1,59 @@
#pragma once
#include <variant>
#include <vector>
#include "app_track_model.hpp"
#include "track_core/scheme_decoder.hpp"
namespace app::track {
struct TrackSchemeDecoder {
using proto_type = track_app_TrackScheme;
static inline const pb_msgdesc_t *pb_fields = &track_app_TrackScheme_msg;
TrackSchemeDecoder() = default;
[[nodiscard]]
static TrackSchemeDecoder from_proto(const proto_type &proto);
[[nodiscard]]
expected<track_core::DecodedScheme, error_t> decode_core() const;
uint8_t id = 0;
std::vector<uint8_t> binary;
Color color;
};
struct TrackSchemeMgr {
using proto_type = track_app_TrackSchemeMgr;
static inline const pb_msgdesc_t *pb_fields = &track_app_TrackSchemeMgr_msg;
struct Add {
using proto_type = track_app_TrackSchemeMgrAdd;
static inline const pb_msgdesc_t *pb_fields = &track_app_TrackSchemeMgrAdd_msg;
TrackSchemeDecoder scheme_decoder;
error_t err{ESP_OK};
static Add from_proto(const proto_type &proto);
};
struct Clear {};
using Unknown = std::monostate;
enum class MessageType : uint8_t {
NONE = 0,
ADD = 1,
CLEAR = 2
};
explicit TrackSchemeMgr() = default;
static TrackSchemeMgr from_proto(const proto_type &proto);
std::variant<Unknown, Add, Clear> choice{Unknown{}};
};
} // namespace app::track
+40
View File
@@ -0,0 +1,40 @@
#ifndef D2AF35D4_5EEF_406F_A3F6_F7E6F2AA24C4
#define D2AF35D4_5EEF_406F_A3F6_F7E6F2AA24C4
#include "app_strip_if.hpp"
#include "app_track_model.hpp"
#include "track_core/render.hpp"
#include <array>
#include <cstdint>
namespace app::track {
struct TrackRenderSpan {
uint16_t start_led{};
uint16_t led_count{};
Color color;
};
struct TrackRenderPlan {
static constexpr size_t MAX_SPANS = 4;
void add_fill(uint16_t start_led, uint16_t led_count, Color color);
[[nodiscard]]
bool empty() const {
return span_count == 0;
}
std::array<TrackRenderSpan, MAX_SPANS> spans{};
size_t span_count{};
};
[[nodiscard]]
TrackRenderPlan make_track_render_plan(const TrackConfig &config, const TrackInfo &info, const report &rep);
void apply_render_plan(app::strip::StripView strip, const TrackRenderPlan &plan);
[[nodiscard]]
track_core::TrackRenderSink make_track_render_sink(app::strip::StripView &strip);
}
#endif /* D2AF35D4_5EEF_406F_A3F6_F7E6F2AA24C4 */
File diff suppressed because it is too large Load Diff
+3
View File
@@ -46,4 +46,7 @@ private:
[[nodiscard]]
TrackError apply_render_plan(MemoryStrip &strip, const TrackRenderPlan &plan);
[[nodiscard]]
TrackRenderSink make_memory_strip_sink(MemoryStrip &strip);
} // namespace track_core
+73
View File
@@ -0,0 +1,73 @@
#pragma once
#include <cstdint>
#include <memory>
#include <optional>
#include "track_core/model.hpp"
#include "track_core/pid_program.hpp"
namespace track_core {
struct TrackPidBandSnapshot {
std::uint8_t band_id{};
std::uint8_t heart_rate{};
std::uint32_t heart_rate_sample_seq{};
bool has_heart_rate{};
bool hr_is_fresh{};
std::uint16_t step_count{};
bool has_step_count{};
bool band_is_active{};
};
class PidHrRuntime {
public:
using time_point = clock::time_point;
PidHrRuntime();
explicit PidHrRuntime(TrackPidConfig config);
void set_pid_config(TrackPidConfig config);
void update_target_hr_bpm(std::uint8_t target_hr_bpm);
[[nodiscard]]
expected<unit, TrackError> apply_pid_runtime_command(
const TrackPidRuntimeCommand &command,
const TrackConfig *track_config);
[[nodiscard]]
const TrackPidConfig &pid_config() const;
void start(time_point now);
void stop();
void tick(const TrackConfig *track_config, const TrackPidBandSnapshot &band, time_point now);
[[nodiscard]]
TrackReport state_report(time_point now) const;
[[nodiscard]]
TrackInfo info() const;
[[nodiscard]]
TrackPidStatus pid_status(const TrackPidBandSnapshot &band, time_point now) const;
private:
static constexpr std::uint8_t magic_pid_track_id = 0;
[[nodiscard]]
float effective_speed_m_s(float base_speed_m_s, const TrackConfig *track_config) const;
TrackPidConfig config_{TrackPidConfig::default_config()};
bool running_{false};
Color color_{Color::white()};
time_point start_timestamp_{};
time_point last_tick_timestamp_{};
std::optional<std::uint32_t> last_consumed_hr_sample_seq_;
time_point last_consumed_hr_sample_time_{};
float mileage_m_{0.0F};
float base_speed_m_s_{0.0F};
float effective_speed_m_s_{0.0F};
std::unique_ptr<TrackPidProgramState> program_state_;
};
} // namespace track_core
+25
View File
@@ -1,6 +1,7 @@
#pragma once
#include <array>
#include <cstddef>
#include <cstdint>
#include "track_core/model.hpp"
@@ -27,10 +28,34 @@ struct TrackRenderPlan {
std::size_t span_count{};
};
struct TrackRenderSink {
using ClearFn = TrackError (*)(void *context);
using FillFn = TrackError (*)(
void *context,
std::uint16_t start_led,
std::uint16_t led_count,
Color color);
using ShowFn = TrackError (*)(void *context);
void *context{};
ClearFn clear{};
FillFn fill{};
ShowFn show{};
};
[[nodiscard]]
TrackRenderPlan make_track_render_plan(
const TrackConfig &config,
const TrackInfo &info,
const TrackReport &report);
[[nodiscard]]
TrackError clear_render_sink(TrackRenderSink sink);
[[nodiscard]]
TrackError apply_render_plan(TrackRenderSink sink, const TrackRenderPlan &plan);
[[nodiscard]]
TrackError show_render_sink(TrackRenderSink sink);
} // namespace track_core
+108
View File
@@ -0,0 +1,108 @@
#pragma once
#include <cstddef>
#include <vector>
#include "track_core/model.hpp"
#include "track_core/render.hpp"
#include "track_core/scheme_decoder.hpp"
namespace track_core {
struct SchemeTrackState {
bool is_running{};
std::size_t primary_segment_index{};
std::size_t sub_segment_index{};
float mileage_m{};
float loop_mileage_m{};
float speed_m_s{};
float elapsed_s{};
};
struct SchemeTrackRuntime {
DecodedScheme scheme;
SchemeTrackState state;
};
[[nodiscard]]
DecodedScheme make_speed_mileage_scheme(
std::uint8_t id,
Color color,
AccelerationProfile acceleration_profile,
std::vector<SMSegment> segments);
[[nodiscard]]
DecodedScheme make_mileage_time_scheme(
std::uint8_t id,
Color color,
AccelerationProfile acceleration_profile,
std::vector<MTSegment> segments);
[[nodiscard]]
DecodedScheme make_speed_time_scheme(
std::uint8_t id,
Color color,
AccelerationProfile acceleration_profile,
std::vector<STSegment> segments);
[[nodiscard]]
DecodedScheme make_repeated_speed_mileage_time_scheme(
std::uint8_t id,
Color color,
AccelerationProfile acceleration_profile,
std::vector<RepeatedSMSegment> segments);
[[nodiscard]]
expected<SchemeTrackRuntime, TrackError> make_scheme_track_runtime(DecodedScheme scheme);
[[nodiscard]]
SchemeTrackRuntime start_scheme_track(SchemeTrackRuntime runtime);
[[nodiscard]]
SchemeTrackRuntime stop_scheme_track(SchemeTrackRuntime runtime);
[[nodiscard]]
SchemeTrackRuntime tick_scheme_track(
const TrackConfig &config,
SchemeTrackRuntime runtime,
float delta_s);
[[nodiscard]]
TrackInfo scheme_track_info(const SchemeTrackRuntime &runtime);
[[nodiscard]]
TrackReport scheme_track_report(const SchemeTrackRuntime &runtime);
class SchemeTrainingRuntime {
public:
[[nodiscard]]
bool has_program() const noexcept;
[[nodiscard]]
bool all_stopped() const noexcept;
[[nodiscard]]
expected<unit, TrackError> add_scheme(DecodedScheme scheme);
void clear();
void start();
void stop();
void tick(const TrackConfig &config, float delta_s);
[[nodiscard]]
TrackStateReportCollection state_collection() const;
[[nodiscard]]
TrackSchemeMgrRead scheme_status() const;
[[nodiscard]]
expected<unit, TrackError> render_to(const TrackConfig &config, TrackRenderSink sink) const;
[[nodiscard]]
expected<std::vector<Color>, TrackError> render_pixels(const TrackConfig &config) const;
private:
std::vector<SchemeTrackRuntime> tracks_;
};
} // namespace track_core
+23
View File
@@ -0,0 +1,23 @@
[build-system]
requires = ["scikit-build-core>=0.10", "nanobind>=2.4"]
build-backend = "scikit_build_core.build"
[project]
name = "track-core"
version = "0.1.0"
description = "Platform-neutral TrackBackFwd track simulation core with Python bindings"
readme = "README.md"
requires-python = ">=3.9"
dependencies = []
[project.optional-dependencies]
emulator = ["dearpygui>=1.10,<2"]
test = ["pytest>=8.0"]
[tool.scikit-build]
minimum-version = "build-system.requires"
wheel.packages = ["python/track_core"]
[tool.scikit-build.cmake.define]
TRACK_CORE_BUILD_PYTHON = "ON"
TRACK_CORE_BUILD_TESTS = "OFF"
+211
View File
@@ -0,0 +1,211 @@
import pytest
import track_core as tc
def running_info(color=None):
info = tc.TrackInfo()
info.color = color or tc.Color.green()
info.id = 1
info.is_running = True
info.num_segments = 1
return info
def report(mileage):
value = tc.TrackReport()
value.id = 1
value.state = tc.TrackState.run
value.mileage_m = mileage
value.speed_m_s = 1.0
return value
def config(draw_kind):
value = tc.TrackConfig()
value.draw_kind = draw_kind
value.line_length_m = 10.0
value.active_line_length_m = 4.0
value.head_offset_m = 0.0
value.line_leds_num = 20
return value
def test_circular_render_wraps():
plan = tc.make_render_plan(
config(tc.TrackDrawKind.circular),
running_info(tc.Color.green()),
report(8.5),
)
assert plan.span_count == 2
assert [(span.start_led, span.led_count, span.color) for span in plan.spans] == [
(17, 3, tc.Color.green()),
(0, 5, tc.Color.green()),
]
def test_linear_render_forward_pixels():
pixels = tc.render_pixels(
config(tc.TrackDrawKind.linear),
running_info(tc.Color.red()),
report(5.0),
)
assert len(pixels) == 20
assert pixels[6] == tc.Color.cyan()
assert pixels[8] == tc.Color.red()
assert pixels[10] == tc.Color.red()
assert pixels[12] == tc.Color.blue()
assert pixels[0] == tc.Color.black()
def test_linear_render_reverse_spans():
cfg = config(tc.TrackDrawKind.linear)
cfg.active_line_length_m = 5.0
plan = tc.make_render_plan(cfg, running_info(tc.Color.red()), report(15.0))
assert [(span.start_led, span.led_count, span.color) for span in plan.spans] == [
(5, 2, tc.Color.blue()),
(7, 3, tc.Color.red()),
(10, 3, tc.Color.red()),
(13, 2, tc.Color.cyan()),
]
def test_linear_render_boundary_sweep_does_not_raise():
cfg = config(tc.TrackDrawKind.linear)
for mileage in (
-20.0,
-10.1,
-10.0,
-9.9,
-1.0,
-0.1,
0.0,
0.1,
1.0,
9.9,
10.0,
10.1,
19.9,
20.0,
20.1,
29.9,
30.0,
30.1,
40.0,
):
pixels = tc.render_pixels(cfg, running_info(tc.Color.red()), report(mileage))
assert len(pixels) == cfg.line_leds_num
def test_not_running_renders_black_pixels():
info = running_info()
info.is_running = False
pixels = tc.render_pixels(config(tc.TrackDrawKind.circular), info, report(8.5))
assert len(pixels) == 20
assert all(pixel == tc.Color.black() for pixel in pixels)
def test_invalid_config_raises_value_error():
cfg = config(tc.TrackDrawKind.circular)
cfg.line_leds_num = 0
with pytest.raises(ValueError):
tc.render_pixels(cfg, running_info(), report(0.0))
def st_segment(speed, time_s):
segment = tc.STSegment()
segment.speed_m_s = speed
segment.time_since_start_s = time_s
return segment
def sm_segment(speed, mileage):
segment = tc.SMSegment()
segment.speed_m_s = speed
segment.mileage_from_start_m = mileage
return segment
def mt_segment(mileage, time_s):
segment = tc.MTSegment()
segment.mileage_to_travel_this_segment_m = mileage
segment.time_since_start_s = time_s
return segment
def test_pure_speed_time_runtime_ticks():
scheme = tc.make_speed_time_scheme(
7,
tc.Color.green(),
tc.AccelerationProfile.instant,
[st_segment(1.0, 0), st_segment(3.0, 5), st_segment(1.0, 10)],
)
runtime = tc.start_scheme_track(tc.make_scheme_track_runtime(scheme))
runtime = tc.tick_scheme_track(config(tc.TrackDrawKind.circular), runtime, 6.0)
state = runtime.state
report = runtime.report()
assert state.primary_segment_index == 1
assert report.state == tc.TrackState.run
assert report.speed_m_s == pytest.approx(3.0)
assert report.mileage_m == pytest.approx(6.0)
def test_training_runtime_accepts_all_scheme_kinds_and_renders():
runtime = tc.SchemeTrainingRuntime()
runtime.add_scheme(
tc.make_speed_mileage_scheme(
1,
tc.Color.red(),
tc.AccelerationProfile.instant,
[sm_segment(1.0, 0), sm_segment(2.0, 5), sm_segment(1.0, 10)],
)
)
runtime.add_scheme(
tc.make_mileage_time_scheme(
2,
tc.Color.green(),
tc.AccelerationProfile.instant,
[mt_segment(10, 0), mt_segment(20, 5), mt_segment(1, 15)],
)
)
runtime.add_scheme(
tc.make_speed_time_scheme(
3,
tc.Color.blue(),
tc.AccelerationProfile.instant,
[st_segment(1.0, 0), st_segment(1.0, 10)],
)
)
repeated = tc.RepeatedSMSegment()
repeated.time_since_start_s = 0
repeated.speed_mileage_segments = [sm_segment(1.0, 0), sm_segment(2.0, 5)]
repeated_end = tc.RepeatedSMSegment()
repeated_end.time_since_start_s = 20
repeated_end.speed_mileage_segments = [sm_segment(1.0, 0), sm_segment(1.0, 5)]
runtime.add_scheme(
tc.make_repeated_speed_mileage_time_scheme(
4,
tc.Color.white(),
tc.AccelerationProfile.instant,
[repeated, repeated_end],
)
)
runtime.start()
runtime.tick(config(tc.TrackDrawKind.circular), 1.0)
pixels = runtime.render_pixels(config(tc.TrackDrawKind.circular))
assert runtime.has_program()
assert not runtime.all_stopped()
assert len(runtime.state_collection()) == 4
assert len(runtime.scheme_status()) == 4
assert len(pixels) == 20
+71
View File
@@ -0,0 +1,71 @@
from ._core import (
AccelerationProfile,
Color,
DecodedScheme,
MemoryStrip,
MTSegment,
RepeatedSMSegment,
SMSegment,
SchemeKind,
SchemeTrackRuntime,
SchemeTrackState,
SchemeTrainingRuntime,
STSegment,
TrackConfig,
TrackDrawKind,
TrackError,
TrackInfo,
TrackRenderPlan,
TrackRenderSpan,
TrackReport,
TrackSchemeStatus,
TrackState,
make_mileage_time_scheme,
make_render_plan,
make_repeated_speed_mileage_time_scheme,
make_scheme_track_runtime,
make_speed_mileage_scheme,
make_speed_time_scheme,
render_pixels,
scheme_track_info,
scheme_track_report,
start_scheme_track,
stop_scheme_track,
tick_scheme_track,
)
__all__ = [
"AccelerationProfile",
"Color",
"DecodedScheme",
"MemoryStrip",
"MTSegment",
"RepeatedSMSegment",
"SMSegment",
"SchemeKind",
"SchemeTrackRuntime",
"SchemeTrackState",
"SchemeTrainingRuntime",
"STSegment",
"TrackConfig",
"TrackDrawKind",
"TrackError",
"TrackInfo",
"TrackRenderPlan",
"TrackRenderSpan",
"TrackReport",
"TrackSchemeStatus",
"TrackState",
"make_mileage_time_scheme",
"make_render_plan",
"make_repeated_speed_mileage_time_scheme",
"make_scheme_track_runtime",
"make_speed_mileage_scheme",
"make_speed_time_scheme",
"render_pixels",
"scheme_track_info",
"scheme_track_report",
"start_scheme_track",
"stop_scheme_track",
"tick_scheme_track",
]
+304
View File
@@ -0,0 +1,304 @@
from __future__ import annotations
import math
import time
from . import (
AccelerationProfile,
Color,
MTSegment,
RepeatedSMSegment,
SMSegment,
STSegment,
SchemeTrainingRuntime,
TrackConfig,
TrackDrawKind,
TrackInfo,
TrackReport,
TrackState,
make_mileage_time_scheme,
make_repeated_speed_mileage_time_scheme,
make_speed_mileage_scheme,
make_speed_time_scheme,
render_pixels,
)
DRAWLIST_TAG = "track_core_drawlist"
def _load_dearpygui():
try:
import dearpygui.dearpygui as dpg
except ImportError as exc:
raise SystemExit(
"DearPyGui is not installed. Install the emulator extra with "
'`python -m pip install -e ".[emulator]"`.'
) from exc
return dpg
def _rgba(color: Color) -> tuple[int, int, int, int]:
return (color.r, color.g, color.b, 255)
def _sm(speed: float, mileage_m: int) -> SMSegment:
segment = SMSegment()
segment.speed_m_s = speed
segment.mileage_from_start_m = mileage_m
return segment
def _mt(mileage_m: int, time_s: int) -> MTSegment:
segment = MTSegment()
segment.mileage_to_travel_this_segment_m = mileage_m
segment.time_since_start_s = time_s
return segment
def _st(speed: float, time_s: int) -> STSegment:
segment = STSegment()
segment.speed_m_s = speed
segment.time_since_start_s = time_s
return segment
def _rsmt(time_s: int, segments: list[SMSegment]) -> RepeatedSMSegment:
segment = RepeatedSMSegment()
segment.time_since_start_s = time_s
segment.speed_mileage_segments = segments
return segment
def _profile_from_ui(dpg) -> AccelerationProfile:
return (
AccelerationProfile.smooth
if dpg.get_value("accel_profile") == "smooth"
else AccelerationProfile.instant
)
def _color_from_ui(dpg) -> Color:
rgb = dpg.get_value("runner_color")
return Color(int(rgb[0]), int(rgb[1]), int(rgb[2]))
def _config_from_ui(dpg) -> TrackConfig:
line_length = float(dpg.get_value("line_length"))
active_length = min(float(dpg.get_value("active_length")), max(0.1, line_length - 0.001))
config = TrackConfig.default_config()
config.draw_kind = (
TrackDrawKind.circular
if dpg.get_value("draw_kind") == "circular"
else TrackDrawKind.linear
)
config.line_length_m = line_length
config.active_line_length_m = active_length
config.head_offset_m = float(dpg.get_value("head_offset"))
config.line_leds_num = int(dpg.get_value("led_count"))
return config
def _manual_state_to_core(dpg) -> tuple[TrackConfig, TrackInfo, TrackReport]:
config = _config_from_ui(dpg)
color = _color_from_ui(dpg)
info = TrackInfo()
info.color = color
info.id = 1
info.is_running = dpg.get_value("manual_running")
info.num_segments = 1
report = TrackReport()
report.id = 1
report.state = TrackState.run if info.is_running else TrackState.stop
report.mileage_m = dpg.get_value("manual_mileage")
report.speed_m_s = dpg.get_value("manual_speed")
report.time_elapsed_ms = 0
return config, info, report
def _draw_linear(dpg, pixels: list[Color]) -> None:
width = 840
x0 = 30
y0 = 180
gap = 1 if len(pixels) <= 240 else 0
cell_w = max(1, int((width - gap * max(0, len(pixels) - 1)) / max(1, len(pixels))))
cell_h = 34
for index, color in enumerate(pixels):
x = x0 + index * (cell_w + gap)
dpg.draw_rectangle(
(x, y0),
(x + cell_w, y0 + cell_h),
color=(55, 58, 64, 255),
fill=_rgba(color),
parent=DRAWLIST_TAG,
)
dpg.draw_rectangle(
(x0 - 1, y0 - 1),
(x0 + width + 1, y0 + cell_h + 1),
color=(90, 96, 108, 255),
parent=DRAWLIST_TAG,
)
dpg.draw_text((x0, y0 + 52), f"{len(pixels)} LEDs", color=(190, 195, 205, 255), parent=DRAWLIST_TAG)
def _draw_circular(dpg, pixels: list[Color]) -> None:
center = (440, 158)
radius = 122
led_radius = 5 if len(pixels) <= 160 else 3
dpg.draw_circle(center, radius, color=(80, 86, 98, 255), thickness=1, parent=DRAWLIST_TAG)
for index, color in enumerate(pixels):
angle = (2.0 * math.pi * index / max(1, len(pixels))) - (math.pi / 2.0)
pos = (
center[0] + math.cos(angle) * radius,
center[1] + math.sin(angle) * radius,
)
dpg.draw_circle(
pos,
led_radius,
color=(42, 45, 52, 255),
fill=_rgba(color),
parent=DRAWLIST_TAG,
)
dpg.draw_text((30, 300), f"{len(pixels)} LEDs", color=(190, 195, 205, 255), parent=DRAWLIST_TAG)
class TrackRuntimeApp:
def __init__(self) -> None:
self.runtime = SchemeTrainingRuntime()
self.last_frame_s = time.perf_counter()
def build_scheme(self, dpg):
kind = dpg.get_value("scheme_kind")
color = _color_from_ui(dpg)
profile = _profile_from_ui(dpg)
if kind == "SM":
return make_speed_mileage_scheme(
1,
color,
profile,
[_sm(1.0, 0), _sm(2.8, 10), _sm(1.2, 24), _sm(2.0, 40)],
)
if kind == "MT":
return make_mileage_time_scheme(
1,
color,
profile,
[_mt(12, 0), _mt(20, 6), _mt(10, 18), _mt(1, 28)],
)
if kind == "RSMT":
return make_repeated_speed_mileage_time_scheme(
1,
color,
profile,
[
_rsmt(0, [_sm(1.0, 0), _sm(3.0, 10), _sm(1.2, 24)]),
_rsmt(18, [_sm(2.2, 0), _sm(4.0, 8), _sm(1.0, 20)]),
_rsmt(42, [_sm(1.4, 0), _sm(2.4, 12), _sm(1.4, 24)]),
],
)
return make_speed_time_scheme(
1,
color,
profile,
[_st(1.0, 0), _st(3.2, 8), _st(1.4, 18), _st(2.4, 30), _st(0.8, 42)],
)
def load_scheme(self, dpg) -> None:
self.runtime.clear()
self.runtime.add_scheme(self.build_scheme(dpg))
self.runtime.start()
self.last_frame_s = time.perf_counter()
def start(self) -> None:
self.runtime.start()
self.last_frame_s = time.perf_counter()
def stop(self) -> None:
self.runtime.stop()
def _runtime_pixels(self, dpg, config: TrackConfig) -> list[Color]:
now_s = time.perf_counter()
delta_s = min(0.1, now_s - self.last_frame_s)
self.last_frame_s = now_s
if dpg.get_value("source_mode") == "runtime":
self.runtime.tick(config, delta_s * float(dpg.get_value("time_scale")))
return self.runtime.render_pixels(config)
manual_config, info, report = _manual_state_to_core(dpg)
return render_pixels(manual_config, info, report)
def render(self, dpg) -> None:
dpg.delete_item(DRAWLIST_TAG, children_only=True)
config = _config_from_ui(dpg)
pixels = self._runtime_pixels(dpg, config)
if config.draw_kind == TrackDrawKind.circular:
_draw_circular(dpg, pixels)
else:
_draw_linear(dpg, pixels)
states = self.runtime.state_collection()
if states:
state = states[0]
dpg.draw_text(
(30, 18),
f"id {state.id} {state.time_elapsed_ms / 1000.0:5.1f}s "
f"{state.mileage_m:6.2f}m {state.speed_m_s:4.2f}m/s",
color=(220, 225, 235, 255),
parent=DRAWLIST_TAG,
)
def main() -> None:
dpg = _load_dearpygui()
app = TrackRuntimeApp()
dpg.create_context()
dpg.create_viewport(title="track-core emulator", width=920, height=560)
with dpg.window(label="track-core emulator", tag="main_window", width=900, height=540):
with dpg.group(horizontal=True):
dpg.add_combo(("runtime", "manual"), default_value="runtime", label="source", tag="source_mode")
dpg.add_combo(("ST", "SM", "MT", "RSMT"), default_value="ST", label="scheme", tag="scheme_kind", callback=lambda *_: app.load_scheme(dpg))
dpg.add_combo(("instant", "smooth"), default_value="smooth", label="accel", tag="accel_profile", callback=lambda *_: app.load_scheme(dpg))
dpg.add_combo(("circular", "linear"), default_value="circular", label="draw", tag="draw_kind")
with dpg.group(horizontal=True):
dpg.add_button(label="load", callback=lambda *_: app.load_scheme(dpg))
dpg.add_button(label="start", callback=lambda *_: app.start())
dpg.add_button(label="stop", callback=lambda *_: app.stop())
dpg.add_color_edit(default_value=(0, 255, 0, 255), label="color", tag="runner_color", no_alpha=True, callback=lambda *_: app.load_scheme(dpg))
dpg.add_slider_float(label="time scale", tag="time_scale", default_value=1.0, min_value=0.0, max_value=8.0)
dpg.add_slider_float(label="line length m", tag="line_length", default_value=40.0, min_value=5.0, max_value=80.0)
dpg.add_slider_float(label="active length m", tag="active_length", default_value=10.0, min_value=0.1, max_value=40.0)
dpg.add_slider_float(label="head offset m", tag="head_offset", default_value=0.0, min_value=-40.0, max_value=40.0)
dpg.add_slider_int(label="LEDs", tag="led_count", default_value=160, min_value=8, max_value=400)
dpg.add_checkbox(label="manual running", default_value=True, tag="manual_running")
dpg.add_slider_float(label="manual mileage m", tag="manual_mileage", default_value=8.5, min_value=-80.0, max_value=120.0)
dpg.add_slider_float(label="manual speed m/s", tag="manual_speed", default_value=1.0, min_value=0.0, max_value=10.0)
dpg.add_drawlist(width=880, height=330, tag=DRAWLIST_TAG)
app.load_scheme(dpg)
dpg.setup_dearpygui()
dpg.set_primary_window("main_window", True)
dpg.show_viewport()
while dpg.is_dearpygui_running():
app.render(dpg)
dpg.render_dearpygui_frame()
dpg.destroy_context()
if __name__ == "__main__":
main()
+85
View File
@@ -0,0 +1,85 @@
#include "app_track_decoder.hpp"
#include <algorithm>
#include <cstdint>
namespace app::track {
namespace {
track_core::Color to_core(const Color &color) {
return {
color.inner.r,
color.inner.g,
color.inner.b,
};
}
error_t from_core(track_core::TrackError error) {
switch (error) {
case track_core::TrackError::ok:
return ESP_OK;
case track_core::TrackError::invalid_arg:
return ESP_ERR_INVALID_ARG;
case track_core::TrackError::invalid_size:
return ESP_ERR_INVALID_SIZE;
case track_core::TrackError::invalid_state:
return ESP_ERR_INVALID_STATE;
case track_core::TrackError::not_supported:
return ESP_ERR_NOT_SUPPORTED;
case track_core::TrackError::range:
return ESP_ERR_INVALID_SIZE;
}
return ESP_FAIL;
}
} // namespace
TrackSchemeDecoder TrackSchemeDecoder::from_proto(const proto_type &proto) {
TrackSchemeDecoder scheme;
scheme.id = proto.id;
if (proto.has_color) {
scheme.color = Color::from_proto(proto.color);
} else {
scheme.color = Color::white();
}
auto data = std::span<const uint8_t>(proto.data.bytes, proto.data.size);
std::ranges::copy(data, std::back_inserter(scheme.binary));
return scheme;
}
expected<track_core::DecodedScheme, error_t> TrackSchemeDecoder::decode_core() const {
using ue = unexpected<error_t>;
if (binary.empty()) {
return ue{ESP_ERR_INVALID_ARG};
}
const auto decoded = track_core::decode_scheme(id, to_core(color), binary);
if (!decoded) {
return ue{from_core(decoded.error())};
}
return *decoded;
}
TrackSchemeMgr::Add TrackSchemeMgr::Add::from_proto(const proto_type &proto) {
Add add;
assert(proto.has_scheme);
add.scheme_decoder = TrackSchemeDecoder::from_proto(proto.scheme);
return add;
}
TrackSchemeMgr TrackSchemeMgr::from_proto(const proto_type &proto) {
TrackSchemeMgr mgmt;
switch (proto.which_msg) {
case track_app_TrackSchemeMgr_add_tag:
mgmt.choice = Add::from_proto(proto.msg.add);
break;
case track_app_TrackSchemeMgr_clear_tag:
mgmt.choice = Clear{};
break;
default:
break;
}
return mgmt;
}
} // namespace app::track
+179
View File
@@ -0,0 +1,179 @@
#include "app_track_drawer.hpp"
#include <cassert>
#include <cstddef>
#include "track_core/render.hpp"
namespace {
track_core::Color to_core(const app::track::Color &color) {
return {
color.inner.r,
color.inner.g,
color.inner.b,
};
}
app::track::Color from_core(const track_core::Color &color) {
return {
color.r,
color.g,
color.b,
};
}
track_core::TrackDrawKind to_core(track_app_TrackDrawKind draw_kind) {
switch (draw_kind) {
case track_app_TrackDrawKind_CIRCULAR:
return track_core::TrackDrawKind::circular;
case track_app_TrackDrawKind_LINEAR:
return track_core::TrackDrawKind::linear;
}
return track_core::TrackDrawKind::circular;
}
track_core::SchemeKind to_core(app::track::SchemeKind kind) {
switch (kind) {
case track_app_TrackSchemeKind_SPEED_INPUT_MILEAGE_SEGMENTED_TIME_FREE:
return track_core::SchemeKind::speed_input_mileage_segmented_time_free;
case track_app_TrackSchemeKind_MILEAGE_INPUT_TIME_SEGMENTED_SPEED_FREE:
return track_core::SchemeKind::mileage_input_time_segmented_speed_free;
case track_app_TrackSchemeKind_SPEED_INPUT_TIME_SEGMENTED_MILEAGE_FREE:
return track_core::SchemeKind::speed_input_time_segmented_mileage_free;
case track_app_TrackSchemeKind_REPEATED_SPEED_INPUT_MILEAGE_SEGMENTATION_INPUT_TIME_SEGMENTED:
return track_core::SchemeKind::repeated_speed_input_mileage_segmentation_input_time_segmented;
}
return track_core::SchemeKind::speed_input_time_segmented_mileage_free;
}
track_core::TrackState to_core(app::track::TrackState state) {
switch (state) {
case track_app_TrackState_STOP:
return track_core::TrackState::stop;
case track_app_TrackState_RUN:
return track_core::TrackState::run;
case track_app_TrackState_TEST_RAINBOW:
return track_core::TrackState::test_rainbow;
case track_app_TrackState_TEST_BLINK:
return track_core::TrackState::test_blink;
}
return track_core::TrackState::stop;
}
track_core::TrackConfig to_core(const app::track::TrackConfig &config) {
return {
.draw_kind = to_core(config.draw_kind),
.line_length_m = config.line_length_m,
.active_line_length_m = config.active_line_length_m,
.head_offset_m = config.head_offset_m,
.line_leds_num = config.line_leds_num,
};
}
track_core::TrackInfo to_core(const app::track::TrackInfo &info) {
return {
.kind = to_core(info.kind),
.color = to_core(info.color),
.id = info.id,
.is_running = info.is_running,
.num_segments = info.num_segments,
};
}
track_core::TrackReport to_core(const app::track::report &report) {
return {
.id = report.id,
.state = to_core(report.state),
.mileage_m = report.mileage_m,
.speed_m_s = report.speed_m_s,
.time_elapsed_ms = report.time_elapsed_ms,
};
}
track_core::TrackError to_core(error_t error) {
if (error == ESP_OK) {
return track_core::TrackError::ok;
}
return track_core::TrackError::invalid_state;
}
app::strip::StripView *strip_from_context(void *context) {
return static_cast<app::strip::StripView *>(context);
}
track_core::TrackError strip_clear(void *context) {
auto *strip = strip_from_context(context);
if (strip == nullptr) {
return track_core::TrackError::invalid_arg;
}
return to_core((*strip)->clear());
}
track_core::TrackError strip_fill(
void *context,
std::uint16_t start_led,
std::uint16_t led_count,
track_core::Color color) {
auto *strip = strip_from_context(context);
if (strip == nullptr) {
return track_core::TrackError::invalid_arg;
}
return to_core((*strip)->fill(start_led, led_count, static_cast<std::uint32_t>(from_core(color))));
}
track_core::TrackError strip_show(void *context) {
auto *strip = strip_from_context(context);
if (strip == nullptr) {
return track_core::TrackError::invalid_arg;
}
return to_core((*strip)->show());
}
} // namespace
namespace app::track {
void TrackRenderPlan::add_fill(uint16_t start_led, uint16_t led_count, Color color) {
if (led_count == 0) {
return;
}
assert(span_count < spans.size() && "TrackRenderPlan capacity exceeded");
spans[span_count++] = TrackRenderSpan{
.start_led = start_led,
.led_count = led_count,
.color = color,
};
}
TrackRenderPlan make_track_render_plan(const TrackConfig &config, const TrackInfo &info, const report &rep) {
const auto core_plan = track_core::make_track_render_plan(
to_core(config),
to_core(info),
to_core(rep));
TrackRenderPlan plan{};
for (size_t i = 0; i < core_plan.span_count; ++i) {
const auto &span = core_plan.spans[i];
plan.add_fill(span.start_led, span.led_count, from_core(span.color));
}
return plan;
}
void apply_render_plan(app::strip::StripView strip, const TrackRenderPlan &plan) {
for (size_t i = 0; i < plan.span_count; ++i) {
const auto &span = plan.spans[i];
strip->fill(span.start_led, span.led_count, span.color);
}
}
track_core::TrackRenderSink make_track_render_sink(app::strip::StripView &strip) {
return track_core::TrackRenderSink{
.context = &strip,
.clear = strip_clear,
.fill = strip_fill,
.show = strip_show,
};
}
} // namespace app::track
+109
View File
@@ -0,0 +1,109 @@
#include "app_track_model.hpp"
#include "app_track.pb.h"
#include "esp_log.h"
namespace {
app::track::config_getter global_config_getter{nullptr};
constexpr auto TAG = "app::track";
}
namespace app::track {
const char *to_str(TrackState status) {
using enum TrackState;
switch (status) {
case track_app_TrackState_STOP:
return "STOP";
case track_app_TrackState_RUN:
return "RUN";
case track_app_TrackState_TEST_RAINBOW:
return "TEST_RAINBOW";
case track_app_TrackState_TEST_BLINK:
return "TEST_BLINK";
}
return "UNKNOWN";
}
const char *to_str(TrackControllerMode mode) {
switch (mode) {
case track_app_TrackControllerMode_SCHEME:
return "SCHEME";
case track_app_TrackControllerMode_PID_HR:
return "PID_HR";
}
return "UNKNOWN";
}
const char *to_str(TrackPidStageKind kind) {
switch (kind) {
case track_app_TrackPidStageKind_NONE:
return "NONE";
case track_app_TrackPidStageKind_CONSTANT:
return "CONSTANT";
case track_app_TrackPidStageKind_PID:
return "PID";
}
return "UNKNOWN";
}
const char *to_str(track_app_TrackDrawKind draw_kind) {
switch (draw_kind) {
case track_app_TrackDrawKind_CIRCULAR:
return "CIRCULAR";
case track_app_TrackDrawKind_LINEAR:
return "LINEAR";
}
return "UNKNOWN";
}
const char *to_str(AccelerationProfile profile) {
switch (profile) {
case track_app_TrackAccelerationProfile_SMOOTH:
return "SMOOTH";
case track_app_TrackAccelerationProfile_INSTANT:
return "INSTANT";
}
return "UNKNOWN";
}
void set_global_config_getter(config_getter getter) {
global_config_getter = std::move(getter);
}
const TrackConfig &global_config() {
if (not global_config_getter) {
static const auto DEFAULT_CONFIG = TrackConfig::Default();
ESP_LOGW(TAG, "unset global config getter");
return DEFAULT_CONFIG;
}
return global_config_getter();
}
void TrackConfig::log(const char *tag, esp_log_level_t level) const {
ESP_LOG_LEVEL(level, tag,
"TrackConfig{.draw_kind=%s, "
".line_length_m=%.2f, "
".active_line_length_m=%.2f, "
".head_offset_m=%.2f, "
".line_leds_num=%" PRIu16
"}",
to_str(draw_kind),
line_length_m,
active_line_length_m,
head_offset_m,
line_leds_num);
}
void TrackStateReportCollection::log(const char *tag, esp_log_level_t level) const {
for (size_t i = 0; i < states.size(); ++i) {
const auto &report = states[i];
ESP_LOG_LEVEL(level, tag,
"[%zu] Report{.id=%" PRIu8 ", .state=%s, .mileage_m=%.2f, .speed_m_s=%.2f, .time_elapsed_ms=%" PRIu32 "}",
i,
report.id,
to_str(report.state),
report.mileage_m,
report.speed_m_s,
report.time_elapsed_ms);
}
}
}
+45 -7
View File
@@ -3,6 +3,41 @@
#include <algorithm>
namespace track_core {
namespace {
MemoryStrip *memory_strip_from_context(void *context) {
return static_cast<MemoryStrip *>(context);
}
TrackError memory_strip_clear(void *context) {
auto *strip = memory_strip_from_context(context);
if (strip == nullptr) {
return TrackError::invalid_arg;
}
return strip->clear();
}
TrackError memory_strip_fill(
void *context,
std::uint16_t start_led,
std::uint16_t led_count,
Color color) {
auto *strip = memory_strip_from_context(context);
if (strip == nullptr) {
return TrackError::invalid_arg;
}
return strip->fill(start_led, led_count, color);
}
TrackError memory_strip_show(void *context) {
auto *strip = memory_strip_from_context(context);
if (strip == nullptr) {
return TrackError::invalid_arg;
}
return strip->show();
}
} // namespace
MemoryStrip::MemoryStrip(std::size_t led_count)
: pixels_(led_count, Color::black()) {}
@@ -60,13 +95,16 @@ std::span<const Color> MemoryStrip::pixels() const {
}
TrackError apply_render_plan(MemoryStrip &strip, const TrackRenderPlan &plan) {
for (std::size_t i = 0; i < plan.span_count; ++i) {
const auto &span = plan.spans[i];
if (const auto err = strip.fill(span.start_led, span.led_count, span.color); err != TrackError::ok) {
return err;
}
}
return TrackError::ok;
return apply_render_plan(make_memory_strip_sink(strip), plan);
}
TrackRenderSink make_memory_strip_sink(MemoryStrip &strip) {
return TrackRenderSink{
.context = &strip,
.clear = memory_strip_clear,
.fill = memory_strip_fill,
.show = memory_strip_show,
};
}
} // namespace track_core
+207
View File
@@ -0,0 +1,207 @@
#include "track_core/pid_runtime.hpp"
#include <algorithm>
#include <chrono>
#include <cmath>
#include <limits>
namespace track_core {
namespace {
constexpr float default_pid_nominal_sample_interval_s = 2.0F;
template <class... Ts>
struct overloads : Ts... {
using Ts::operator()...;
};
bool supports_live_pid_tuning(const TrackPidConfig &config) {
return config.schemas.size() == 1 &&
std::holds_alternative<TrackPidSegment>(config.schemas.front().segment);
}
} // namespace
PidHrRuntime::PidHrRuntime() = default;
PidHrRuntime::PidHrRuntime(TrackPidConfig config)
: config_(std::move(config)) {}
void PidHrRuntime::set_pid_config(TrackPidConfig config) {
config_ = std::move(config);
}
void PidHrRuntime::update_target_hr_bpm(std::uint8_t target_hr_bpm) {
config_.target_hr_bpm = target_hr_bpm;
if (program_state_) {
program_state_->update_target_hr_bpm(target_hr_bpm);
}
}
expected<unit, TrackError> PidHrRuntime::apply_pid_runtime_command(
const TrackPidRuntimeCommand &command,
const TrackConfig *track_config) {
if (!running_ || !program_state_) {
return unexpected<TrackError>{TrackError::invalid_state};
}
return std::visit(overloads{
[](unit) -> expected<unit, TrackError> {
return unexpected<TrackError>{TrackError::invalid_arg};
},
[&](const TrackPidSetTargetHr &target_hr) -> expected<unit, TrackError> {
update_target_hr_bpm(target_hr.target_hr_bpm);
return {};
},
[&](const TrackPidSetTuning &tuning) -> expected<unit, TrackError> {
if (!supports_live_pid_tuning(config_)) {
return unexpected<TrackError>{TrackError::invalid_state};
}
auto updated = program_state_->update_tuning(tuning);
if (!updated) {
return unexpected<TrackError>{updated.error()};
}
auto &segment = std::get<TrackPidSegment>(config_.schemas.front().segment);
tuning.apply_to(segment);
base_speed_m_s_ = program_state_->commanded_speed_m_s();
effective_speed_m_s_ = effective_speed_m_s(base_speed_m_s_, track_config);
return {};
}},
command.command);
}
const TrackPidConfig &PidHrRuntime::pid_config() const {
return config_;
}
void PidHrRuntime::start(time_point now) {
running_ = true;
mileage_m_ = 0.0F;
base_speed_m_s_ = 0.0F;
effective_speed_m_s_ = 0.0F;
start_timestamp_ = now;
last_tick_timestamp_ = now;
last_consumed_hr_sample_seq_.reset();
last_consumed_hr_sample_time_ = {};
program_state_ = std::make_unique<TrackPidProgramState>(now, config_);
}
void PidHrRuntime::stop() {
running_ = false;
base_speed_m_s_ = 0.0F;
effective_speed_m_s_ = 0.0F;
last_consumed_hr_sample_seq_.reset();
last_consumed_hr_sample_time_ = {};
program_state_.reset();
}
void PidHrRuntime::tick(
const TrackConfig *track_config,
const TrackPidBandSnapshot &band,
time_point now) {
if (!running_ || !program_state_) {
return;
}
const auto dt = std::chrono::duration_cast<std::chrono::duration<float>>(now - last_tick_timestamp_);
last_tick_timestamp_ = now;
if (dt.count() <= 0.0F) {
return;
}
std::optional<TrackPidProgramState::HrSample> fresh_hr;
if (band.hr_is_fresh && band.has_heart_rate &&
(!last_consumed_hr_sample_seq_.has_value() ||
band.heart_rate_sample_seq != *last_consumed_hr_sample_seq_)) {
auto sample_interval_s = default_pid_nominal_sample_interval_s;
if (last_consumed_hr_sample_seq_.has_value()) {
sample_interval_s = std::chrono::duration_cast<std::chrono::duration<float>>(
now - last_consumed_hr_sample_time_)
.count();
}
fresh_hr = TrackPidProgramState::HrSample{
.heart_rate_bpm = static_cast<float>(band.heart_rate),
.sample_interval_s = sample_interval_s,
};
last_consumed_hr_sample_seq_ = band.heart_rate_sample_seq;
last_consumed_hr_sample_time_ = now;
}
base_speed_m_s_ = program_state_->next(now, fresh_hr);
effective_speed_m_s_ = effective_speed_m_s(base_speed_m_s_, track_config);
if (program_state_->is_finished(now)) {
base_speed_m_s_ = 0.0F;
effective_speed_m_s_ = 0.0F;
running_ = false;
}
mileage_m_ += effective_speed_m_s_ * dt.count();
}
float PidHrRuntime::effective_speed_m_s(float base_speed_m_s, const TrackConfig *track_config) const {
auto effective_speed = std::max(0.0F, base_speed_m_s);
if (!config_.speed_suppression || effective_speed <= 0.0F || track_config == nullptr) {
return effective_speed;
}
if (!std::isfinite(track_config->line_length_m) || track_config->line_length_m <= 0.0F) {
return effective_speed;
}
const auto &suppression = *config_.speed_suppression;
if (!std::isfinite(suppression.sigma_m) || suppression.sigma_m <= 0.0F) {
return effective_speed;
}
const auto phase_raw_m = std::fmod(mileage_m_, track_config->line_length_m);
const auto phase_m = phase_raw_m < 0.0F ? (phase_raw_m + track_config->line_length_m) : phase_raw_m;
const auto seam_distance_m = std::min(phase_m, track_config->line_length_m - phase_m);
const auto sigma_ratio = seam_distance_m / suppression.sigma_m;
const auto gaussian = std::exp(-0.5F * sigma_ratio * sigma_ratio);
const auto speed_ratio = 1.0F - ((1.0F - suppression.ratio_min) * gaussian);
return effective_speed * speed_ratio;
}
TrackReport PidHrRuntime::state_report(time_point now) const {
return {
.id = magic_pid_track_id,
.state = running_ ? TrackState::run : TrackState::stop,
.mileage_m = mileage_m_,
.speed_m_s = effective_speed_m_s_,
.time_elapsed_ms = static_cast<std::uint32_t>(std::chrono::duration_cast<std::chrono::milliseconds>(
now - start_timestamp_)
.count()),
};
}
TrackInfo PidHrRuntime::info() const {
return {
.kind = SchemeKind::speed_input_time_segmented_mileage_free,
.color = color_,
.id = magic_pid_track_id,
.is_running = running_,
.num_segments = static_cast<std::uint8_t>(
std::min(config_.schemas.size(), static_cast<std::size_t>(std::numeric_limits<std::uint8_t>::max()))),
};
}
TrackPidStatus PidHrRuntime::pid_status(const TrackPidBandSnapshot &band, time_point now) const {
TrackPidStatus status;
status.band_id = config_.band_id;
status.band_is_active = band.band_is_active;
status.is_heart_rate_valid = band.hr_is_fresh;
status.heart_rate_bpm = band.has_heart_rate ? band.heart_rate : static_cast<std::uint8_t>(0);
status.step_count = band.has_step_count ? band.step_count : static_cast<std::uint16_t>(0);
status.effective_speed_m_s = effective_speed_m_s_;
status.base_speed_m_s = base_speed_m_s_;
if (program_state_) {
status.active_segment_index = program_state_->active_stage_index();
status.active_segment_kind = program_state_->active_stage_kind();
status.remaining_program_ms = program_state_->remaining_program_ms(now);
} else {
status.active_segment_kind = TrackPidStageKind::none;
}
return status;
}
} // namespace track_core
+310
View File
@@ -0,0 +1,310 @@
#include <cstdint>
#include <stdexcept>
#include <string>
#include <nanobind/nanobind.h>
#include <nanobind/stl/string.h>
#include <nanobind/stl/vector.h>
#include "track_core/memory_strip.hpp"
#include "track_core/render.hpp"
#include "track_core/scheme_runtime.hpp"
namespace nb = nanobind;
using namespace nb::literals;
namespace {
void validate_render_config(const track_core::TrackConfig &config) {
if (!config.verify() || config.line_leds_num == 0) {
throw nb::value_error("invalid TrackConfig");
}
if (config.draw_kind == track_core::TrackDrawKind::circular &&
config.active_line_length_m >= config.line_length_m) {
throw nb::value_error("circular tracks require active_line_length_m < line_length_m");
}
}
nb::list colors_to_list(std::span<const track_core::Color> colors) {
nb::list result;
for (const auto &color : colors) {
result.append(color);
}
return result;
}
nb::list spans_to_list(const track_core::TrackRenderPlan &plan) {
nb::list result;
for (std::size_t i = 0; i < plan.span_count; ++i) {
result.append(plan.spans[i]);
}
return result;
}
track_core::TrackRenderPlan make_render_plan_checked(
const track_core::TrackConfig &config,
const track_core::TrackInfo &info,
const track_core::TrackReport &report) {
validate_render_config(config);
return track_core::make_track_render_plan(config, info, report);
}
track_core::SchemeTrackRuntime make_scheme_track_runtime_checked(track_core::DecodedScheme scheme) {
auto result = track_core::make_scheme_track_runtime(std::move(scheme));
if (!result) {
throw std::runtime_error("invalid scheme");
}
return std::move(*result);
}
void add_scheme_checked(track_core::SchemeTrainingRuntime &runtime, track_core::DecodedScheme scheme) {
const auto result = runtime.add_scheme(std::move(scheme));
if (!result) {
throw std::runtime_error("invalid scheme");
}
}
nb::list runtime_state_collection(const track_core::SchemeTrainingRuntime &runtime) {
nb::list result;
for (const auto &report : runtime.state_collection().states) {
result.append(report);
}
return result;
}
nb::list runtime_scheme_status(const track_core::SchemeTrainingRuntime &runtime) {
nb::list result;
for (const auto &status : runtime.scheme_status().scheme_status) {
result.append(status);
}
return result;
}
nb::list runtime_render_pixels(
const track_core::SchemeTrainingRuntime &runtime,
const track_core::TrackConfig &config) {
const auto pixels = runtime.render_pixels(config);
if (!pixels) {
throw std::runtime_error("failed to render runtime pixels");
}
return colors_to_list(*pixels);
}
nb::list render_pixels(
const track_core::TrackConfig &config,
const track_core::TrackInfo &info,
const track_core::TrackReport &report) {
validate_render_config(config);
track_core::MemoryStrip strip(config.line_leds_num);
const auto plan = track_core::make_track_render_plan(config, info, report);
const auto err = track_core::apply_render_plan(strip, plan);
if (err != track_core::TrackError::ok) {
throw std::runtime_error("failed to apply render plan");
}
return colors_to_list(strip.pixels());
}
std::string color_repr(const track_core::Color &color) {
return "Color(" + std::to_string(color.r) + ", " +
std::to_string(color.g) + ", " +
std::to_string(color.b) + ")";
}
} // namespace
NB_MODULE(_core, m) {
m.doc() = "Python bindings for the platform-neutral track-core library";
nb::enum_<track_core::TrackError>(m, "TrackError")
.value("ok", track_core::TrackError::ok)
.value("invalid_arg", track_core::TrackError::invalid_arg)
.value("invalid_size", track_core::TrackError::invalid_size)
.value("invalid_state", track_core::TrackError::invalid_state)
.value("not_supported", track_core::TrackError::not_supported)
.value("range", track_core::TrackError::range);
nb::enum_<track_core::TrackDrawKind>(m, "TrackDrawKind")
.value("circular", track_core::TrackDrawKind::circular)
.value("linear", track_core::TrackDrawKind::linear);
nb::enum_<track_core::SchemeKind>(m, "SchemeKind")
.value("speed_input_mileage_segmented_time_free",
track_core::SchemeKind::speed_input_mileage_segmented_time_free)
.value("mileage_input_time_segmented_speed_free",
track_core::SchemeKind::mileage_input_time_segmented_speed_free)
.value("speed_input_time_segmented_mileage_free",
track_core::SchemeKind::speed_input_time_segmented_mileage_free)
.value("repeated_speed_input_mileage_segmentation_input_time_segmented",
track_core::SchemeKind::repeated_speed_input_mileage_segmentation_input_time_segmented);
nb::enum_<track_core::AccelerationProfile>(m, "AccelerationProfile")
.value("instant", track_core::AccelerationProfile::instant)
.value("smooth", track_core::AccelerationProfile::smooth);
nb::enum_<track_core::TrackState>(m, "TrackState")
.value("stop", track_core::TrackState::stop)
.value("run", track_core::TrackState::run)
.value("test_rainbow", track_core::TrackState::test_rainbow)
.value("test_blink", track_core::TrackState::test_blink);
nb::class_<track_core::Color>(m, "Color")
.def(nb::init<>())
.def(nb::init<std::uint8_t, std::uint8_t, std::uint8_t>(), "r"_a, "g"_a, "b"_a)
.def(nb::init<std::uint32_t>(), "value"_a)
.def_rw("r", &track_core::Color::r)
.def_rw("g", &track_core::Color::g)
.def_rw("b", &track_core::Color::b)
.def("hex", &track_core::Color::hex)
.def_prop_ro("value", [](const track_core::Color &color) {
return static_cast<std::uint32_t>(color);
})
.def_static("black", &track_core::Color::black)
.def_static("red", &track_core::Color::red)
.def_static("orange", &track_core::Color::orange)
.def_static("yellow", &track_core::Color::yellow)
.def_static("green", &track_core::Color::green)
.def_static("cyan", &track_core::Color::cyan)
.def_static("blue", &track_core::Color::blue)
.def_static("indigo", &track_core::Color::indigo)
.def_static("violet", &track_core::Color::violet)
.def_static("white", &track_core::Color::white)
.def("__eq__", [](const track_core::Color &lhs, const track_core::Color &rhs) {
return lhs == rhs;
})
.def("__repr__", &color_repr);
nb::class_<track_core::TrackConfig>(m, "TrackConfig")
.def(nb::init<>())
.def_rw("draw_kind", &track_core::TrackConfig::draw_kind)
.def_rw("line_length_m", &track_core::TrackConfig::line_length_m)
.def_rw("active_line_length_m", &track_core::TrackConfig::active_line_length_m)
.def_rw("head_offset_m", &track_core::TrackConfig::head_offset_m)
.def_rw("line_leds_num", &track_core::TrackConfig::line_leds_num)
.def("led_distance", &track_core::TrackConfig::led_distance)
.def("verify", &track_core::TrackConfig::verify)
.def_static("default_config", &track_core::TrackConfig::default_config);
nb::class_<track_core::TrackInfo>(m, "TrackInfo")
.def(nb::init<>())
.def_rw("kind", &track_core::TrackInfo::kind)
.def_rw("color", &track_core::TrackInfo::color)
.def_rw("id", &track_core::TrackInfo::id)
.def_rw("is_running", &track_core::TrackInfo::is_running)
.def_rw("num_segments", &track_core::TrackInfo::num_segments);
nb::class_<track_core::TrackSchemeMgrRead::Status>(m, "TrackSchemeStatus")
.def(nb::init<>())
.def_rw("id", &track_core::TrackSchemeMgrRead::Status::id)
.def_rw("segment_count", &track_core::TrackSchemeMgrRead::Status::segment_count)
.def_rw("kind", &track_core::TrackSchemeMgrRead::Status::kind);
nb::class_<track_core::TrackReport>(m, "TrackReport")
.def(nb::init<>())
.def_rw("id", &track_core::TrackReport::id)
.def_rw("state", &track_core::TrackReport::state)
.def_rw("mileage_m", &track_core::TrackReport::mileage_m)
.def_rw("speed_m_s", &track_core::TrackReport::speed_m_s)
.def_rw("time_elapsed_ms", &track_core::TrackReport::time_elapsed_ms);
nb::class_<track_core::TrackRenderSpan>(m, "TrackRenderSpan")
.def(nb::init<>())
.def_rw("start_led", &track_core::TrackRenderSpan::start_led)
.def_rw("led_count", &track_core::TrackRenderSpan::led_count)
.def_rw("color", &track_core::TrackRenderSpan::color);
nb::class_<track_core::TrackRenderPlan>(m, "TrackRenderPlan")
.def(nb::init<>())
.def("empty", &track_core::TrackRenderPlan::empty)
.def_prop_ro("span_count", [](const track_core::TrackRenderPlan &plan) {
return plan.span_count;
})
.def_prop_ro("spans", &spans_to_list);
nb::class_<track_core::MemoryStrip>(m, "MemoryStrip")
.def(nb::init<std::size_t>(), "led_count"_a = 0)
.def("begin", &track_core::MemoryStrip::begin)
.def("clear", &track_core::MemoryStrip::clear)
.def("fill", &track_core::MemoryStrip::fill, "start"_a, "count"_a, "color"_a)
.def("show", &track_core::MemoryStrip::show)
.def("set_leds_count", &track_core::MemoryStrip::set_leds_count, "count"_a)
.def_prop_ro("leds_count", &track_core::MemoryStrip::leds_count)
.def_prop_ro("frame_sequence", &track_core::MemoryStrip::frame_sequence)
.def_prop_ro("pixels", [](const track_core::MemoryStrip &strip) {
return colors_to_list(strip.pixels());
});
nb::class_<track_core::SMSegment>(m, "SMSegment")
.def(nb::init<>())
.def_rw("speed_m_s", &track_core::SMSegment::speed_m_s)
.def_rw("mileage_from_start_m", &track_core::SMSegment::mileage_from_start_m);
nb::class_<track_core::MTSegment>(m, "MTSegment")
.def(nb::init<>())
.def_rw("mileage_to_travel_this_segment_m", &track_core::MTSegment::mileage_to_travel_this_segment_m)
.def_rw("time_since_start_s", &track_core::MTSegment::time_since_start_s);
nb::class_<track_core::STSegment>(m, "STSegment")
.def(nb::init<>())
.def_rw("speed_m_s", &track_core::STSegment::speed_m_s)
.def_rw("time_since_start_s", &track_core::STSegment::time_since_start_s);
nb::class_<track_core::RepeatedSMSegment>(m, "RepeatedSMSegment")
.def(nb::init<>())
.def_rw("speed_mileage_segments", &track_core::RepeatedSMSegment::speed_mileage_segments)
.def_rw("time_since_start_s", &track_core::RepeatedSMSegment::time_since_start_s);
nb::class_<track_core::DecodedScheme>(m, "DecodedScheme")
.def_prop_ro("id", [](const track_core::DecodedScheme &scheme) { return scheme.id; })
.def_prop_ro("color", [](const track_core::DecodedScheme &scheme) { return scheme.color; })
.def_prop_ro("kind", [](const track_core::DecodedScheme &scheme) { return scheme.kind; })
.def_prop_ro("acceleration_profile", [](const track_core::DecodedScheme &scheme) {
return scheme.acceleration_profile;
});
nb::class_<track_core::SchemeTrackState>(m, "SchemeTrackState")
.def(nb::init<>())
.def_rw("is_running", &track_core::SchemeTrackState::is_running)
.def_rw("primary_segment_index", &track_core::SchemeTrackState::primary_segment_index)
.def_rw("sub_segment_index", &track_core::SchemeTrackState::sub_segment_index)
.def_rw("mileage_m", &track_core::SchemeTrackState::mileage_m)
.def_rw("loop_mileage_m", &track_core::SchemeTrackState::loop_mileage_m)
.def_rw("speed_m_s", &track_core::SchemeTrackState::speed_m_s)
.def_rw("elapsed_s", &track_core::SchemeTrackState::elapsed_s);
nb::class_<track_core::SchemeTrackRuntime>(m, "SchemeTrackRuntime")
.def_prop_ro("state", [](const track_core::SchemeTrackRuntime &runtime) {
return runtime.state;
})
.def("info", &track_core::scheme_track_info)
.def("report", &track_core::scheme_track_report);
nb::class_<track_core::SchemeTrainingRuntime>(m, "SchemeTrainingRuntime")
.def(nb::init<>())
.def("has_program", &track_core::SchemeTrainingRuntime::has_program)
.def("all_stopped", &track_core::SchemeTrainingRuntime::all_stopped)
.def("add_scheme", &add_scheme_checked, "scheme"_a)
.def("clear", &track_core::SchemeTrainingRuntime::clear)
.def("start", &track_core::SchemeTrainingRuntime::start)
.def("stop", &track_core::SchemeTrainingRuntime::stop)
.def("tick", &track_core::SchemeTrainingRuntime::tick, "config"_a, "delta_s"_a)
.def("state_collection", &runtime_state_collection)
.def("scheme_status", &runtime_scheme_status)
.def("render_pixels", &runtime_render_pixels, "config"_a);
m.def("make_render_plan", &make_render_plan_checked, "config"_a, "info"_a, "report"_a);
m.def("render_pixels", &render_pixels, "config"_a, "info"_a, "report"_a);
m.def("make_speed_mileage_scheme", &track_core::make_speed_mileage_scheme,
"id"_a, "color"_a, "acceleration_profile"_a, "segments"_a);
m.def("make_mileage_time_scheme", &track_core::make_mileage_time_scheme,
"id"_a, "color"_a, "acceleration_profile"_a, "segments"_a);
m.def("make_speed_time_scheme", &track_core::make_speed_time_scheme,
"id"_a, "color"_a, "acceleration_profile"_a, "segments"_a);
m.def("make_repeated_speed_mileage_time_scheme", &track_core::make_repeated_speed_mileage_time_scheme,
"id"_a, "color"_a, "acceleration_profile"_a, "segments"_a);
m.def("make_scheme_track_runtime", &make_scheme_track_runtime_checked, "scheme"_a);
m.def("start_scheme_track", &track_core::start_scheme_track, "runtime"_a);
m.def("stop_scheme_track", &track_core::stop_scheme_track, "runtime"_a);
m.def("tick_scheme_track", &track_core::tick_scheme_track, "config"_a, "runtime"_a, "delta_s"_a);
m.def("scheme_track_info", &track_core::scheme_track_info, "runtime"_a);
m.def("scheme_track_report", &track_core::scheme_track_report, "runtime"_a);
}
+66 -7
View File
@@ -173,6 +173,64 @@ void TrackRenderPlan::add_fill(std::uint16_t start_led, std::uint16_t led_count,
};
}
TrackError clear_render_sink(TrackRenderSink sink) {
if (sink.clear == nullptr) {
return TrackError::invalid_arg;
}
return sink.clear(sink.context);
}
TrackError apply_render_plan(TrackRenderSink sink, const TrackRenderPlan &plan) {
if (plan.empty()) {
return TrackError::ok;
}
if (sink.fill == nullptr) {
return TrackError::invalid_arg;
}
for (std::size_t i = 0; i < plan.span_count; ++i) {
const auto &span = plan.spans[i];
if (const auto err = sink.fill(sink.context, span.start_led, span.led_count, span.color);
err != TrackError::ok) {
return err;
}
}
return TrackError::ok;
}
TrackError show_render_sink(TrackRenderSink sink) {
if (sink.show == nullptr) {
return TrackError::invalid_arg;
}
return sink.show(sink.context);
}
namespace {
void add_clipped_fill(
TrackRenderPlan &plan,
int start_led,
int led_count,
int line_leds_num,
Color color) {
if (led_count <= 0 || line_leds_num <= 0) {
return;
}
const auto clipped_start = std::max(start_led, 0);
const auto clipped_end = std::min(start_led + led_count, line_leds_num);
if (clipped_end <= clipped_start) {
return;
}
plan.add_fill(
static_cast<std::uint16_t>(clipped_start),
static_cast<std::uint16_t>(clipped_end - clipped_start),
color);
}
} // namespace
TrackRenderPlan make_track_render_plan(
const TrackConfig &config,
const TrackInfo &info,
@@ -193,32 +251,33 @@ TrackRenderPlan make_track_render_plan(
const auto drawer = LinearLineDrawer::from_report(config, report);
const auto magic_color_ahead = Color::blue();
const auto magic_color_behind = Color::cyan();
const auto center_offset = drawer.center_offset_leds_num();
const auto center_offset = static_cast<int>(drawer.center_offset_leds_num());
const auto line_leds_num = static_cast<int>(config.line_leds_num);
const auto fill_positive_side = [&](std::uint16_t count, Color near_center, Color far_end) {
if (count == 0) {
return;
}
if (count == 1) {
plan.add_fill(center_offset, 1, near_center);
add_clipped_fill(plan, center_offset, 1, line_leds_num, near_center);
return;
}
const auto distal_count = static_cast<std::uint16_t>(count / 2);
const auto proximal_count = static_cast<std::uint16_t>(count - distal_count);
plan.add_fill(center_offset, proximal_count, near_center);
plan.add_fill(static_cast<std::uint16_t>(center_offset + proximal_count), distal_count, far_end);
add_clipped_fill(plan, center_offset, proximal_count, line_leds_num, near_center);
add_clipped_fill(plan, center_offset + proximal_count, distal_count, line_leds_num, far_end);
};
const auto fill_negative_side = [&](std::uint16_t count, Color near_center, Color far_end) {
if (count == 0) {
return;
}
if (count == 1) {
plan.add_fill(static_cast<std::uint16_t>(center_offset - 1), 1, near_center);
add_clipped_fill(plan, center_offset - 1, 1, line_leds_num, near_center);
return;
}
const auto distal_count = static_cast<std::uint16_t>(count / 2);
const auto proximal_count = static_cast<std::uint16_t>(count - distal_count);
plan.add_fill(static_cast<std::uint16_t>(center_offset - count), distal_count, far_end);
plan.add_fill(static_cast<std::uint16_t>(center_offset - proximal_count), proximal_count, near_center);
add_clipped_fill(plan, center_offset - count, distal_count, line_leds_num, far_end);
add_clipped_fill(plan, center_offset - proximal_count, proximal_count, line_leds_num, near_center);
};
const auto ahead = drawer.center_ahead_leds_num();
const auto behind = drawer.center_behind_leds_num();
+1
View File
@@ -30,6 +30,7 @@ expected<DecodedScheme, TrackError> decode_scheme(
.color = color,
.kind = kind,
.acceleration_profile = acceleration_profile,
.segments = std::vector<SMSegment>{},
};
switch (kind) {
+763
View File
@@ -0,0 +1,763 @@
#include "track_core/scheme_runtime.hpp"
#include <algorithm>
#include <cmath>
#include <cstdint>
#include <limits>
#include <numeric>
#include <type_traits>
#include "track_core/memory_strip.hpp"
#include "track_core/render.hpp"
namespace track_core {
namespace {
struct accel_calc_result {
float target_speed_m_s{};
float acceleration_m_s_2{};
};
constexpr float max_acceleration_m_s_2 = 5.0F;
[[nodiscard]]
bool valid_acceleration_profile(AccelerationProfile profile) {
return profile == AccelerationProfile::instant ||
profile == AccelerationProfile::smooth;
}
[[nodiscard]]
bool finite_nonnegative(float value) {
return std::isfinite(value) && value >= 0.0F;
}
[[nodiscard]]
std::uint8_t clamp_u8(std::size_t value) {
return static_cast<std::uint8_t>(
std::min<std::size_t>(value, std::numeric_limits<std::uint8_t>::max()));
}
[[nodiscard]]
std::uint32_t elapsed_ms(float elapsed_s) {
if (!std::isfinite(elapsed_s) || elapsed_s <= 0.0F) {
return 0;
}
const auto ms = elapsed_s * 1000.0F;
if (ms >= static_cast<float>(std::numeric_limits<std::uint32_t>::max())) {
return std::numeric_limits<std::uint32_t>::max();
}
return static_cast<std::uint32_t>(ms);
}
[[nodiscard]]
accel_calc_result speed_mileage_accel(
const SMSegment &current,
const SMSegment &next,
float current_mileage_m,
float current_speed_m_s) {
const float target_speed = next.speed_m_s;
float remaining_distance = static_cast<float>(next.mileage_from_start_m) - current_mileage_m;
if (remaining_distance <= 0.0F) {
return {target_speed, 0.0F};
}
float accel =
(target_speed * target_speed - current_speed_m_s * current_speed_m_s) /
(2.0F * remaining_distance);
accel = std::clamp(accel, -max_acceleration_m_s_2, max_acceleration_m_s_2);
return {target_speed, accel};
}
[[nodiscard]]
accel_calc_result speed_time_accel(const STSegment &current, const STSegment &next) {
const float target_speed = next.speed_m_s;
const float duration_s =
static_cast<float>(next.time_since_start_s) -
static_cast<float>(current.time_since_start_s);
if (duration_s <= 0.0F) {
return {target_speed, 0.0F};
}
float accel = (target_speed - current.speed_m_s) / duration_s;
accel = std::clamp(accel, -max_acceleration_m_s_2, max_acceleration_m_s_2);
return {target_speed, accel};
}
[[nodiscard]]
accel_calc_result mileage_time_accel(
float elapsed_s,
float next_segment_time_s,
float current_speed_m_s,
float target_speed_m_s) {
const float remaining_time_s = next_segment_time_s - elapsed_s;
if (remaining_time_s <= 0.0F) {
return {target_speed_m_s, 0.0F};
}
float accel = (target_speed_m_s - current_speed_m_s) / remaining_time_s;
accel = std::clamp(accel, -max_acceleration_m_s_2, max_acceleration_m_s_2);
return {target_speed_m_s, accel};
}
void apply_acceleration(SchemeTrackState &state, accel_calc_result profile, float delta_s) {
state.speed_m_s += profile.acceleration_m_s_2 * delta_s;
if (profile.acceleration_m_s_2 > 0.0F) {
state.speed_m_s = std::min(state.speed_m_s, profile.target_speed_m_s);
} else if (profile.acceleration_m_s_2 < 0.0F) {
state.speed_m_s = std::max(state.speed_m_s, profile.target_speed_m_s);
}
}
[[nodiscard]]
std::optional<float> calc_mts_target_speed(
std::size_t segment_index,
const std::vector<MTSegment> &segments) {
if (segments.size() < 2 || segment_index > segments.size() - 2) {
return std::nullopt;
}
const auto &current = segments[segment_index];
const auto &next = segments[segment_index + 1];
if (next.time_since_start_s < current.time_since_start_s) {
return std::nullopt;
}
const auto duration_s = next.time_since_start_s - current.time_since_start_s;
if (duration_s == 0 || current.mileage_to_travel_this_segment_m == 0) {
return std::nullopt;
}
return static_cast<float>(current.mileage_to_travel_this_segment_m) /
static_cast<float>(duration_s);
}
[[nodiscard]]
std::optional<float> calc_mts_target_speed(const MTSegment &current, const MTSegment &next) {
if (next.time_since_start_s < current.time_since_start_s) {
return std::nullopt;
}
const auto duration_s = next.time_since_start_s - current.time_since_start_s;
if (duration_s == 0 || current.mileage_to_travel_this_segment_m == 0) {
return std::nullopt;
}
return static_cast<float>(current.mileage_to_travel_this_segment_m) /
static_cast<float>(duration_s);
}
template <typename T>
[[nodiscard]]
bool strictly_sorted_by_time(const std::vector<T> &segments) {
for (std::size_t i = 1; i < segments.size(); ++i) {
if (segments[i].time_since_start_s <= segments[i - 1].time_since_start_s) {
return false;
}
}
return true;
}
[[nodiscard]]
expected<DecodedScheme, TrackError> canonicalize_scheme(DecodedScheme scheme) {
if (!valid_acceleration_profile(scheme.acceleration_profile)) {
return unexpected<TrackError>{TrackError::invalid_arg};
}
switch (scheme.kind) {
case SchemeKind::speed_input_mileage_segmented_time_free: {
auto *segments = std::get_if<std::vector<SMSegment>>(&scheme.segments);
if (segments == nullptr || segments->empty()) {
return unexpected<TrackError>{TrackError::invalid_arg};
}
std::ranges::sort(*segments, {}, &SMSegment::mileage_from_start_m);
if (segments->front().mileage_from_start_m != 0) {
return unexpected<TrackError>{TrackError::invalid_arg};
}
for (const auto &segment : *segments) {
if (!finite_nonnegative(segment.speed_m_s)) {
return unexpected<TrackError>{TrackError::invalid_arg};
}
}
return scheme;
}
case SchemeKind::mileage_input_time_segmented_speed_free: {
auto *segments = std::get_if<std::vector<MTSegment>>(&scheme.segments);
if (segments == nullptr || segments->size() < 2) {
return unexpected<TrackError>{TrackError::invalid_arg};
}
std::ranges::sort(*segments, {}, &MTSegment::time_since_start_s);
if (segments->front().time_since_start_s != 0 || !strictly_sorted_by_time(*segments)) {
return unexpected<TrackError>{TrackError::invalid_arg};
}
for (std::size_t i = 0; i + 1 < segments->size(); ++i) {
if (!calc_mts_target_speed(i, *segments)) {
return unexpected<TrackError>{TrackError::invalid_arg};
}
}
return scheme;
}
case SchemeKind::speed_input_time_segmented_mileage_free: {
auto *segments = std::get_if<std::vector<STSegment>>(&scheme.segments);
if (segments == nullptr || segments->empty()) {
return unexpected<TrackError>{TrackError::invalid_arg};
}
std::ranges::sort(*segments, {}, &STSegment::time_since_start_s);
if (segments->front().time_since_start_s != 0) {
return unexpected<TrackError>{TrackError::invalid_arg};
}
for (const auto &segment : *segments) {
if (!finite_nonnegative(segment.speed_m_s)) {
return unexpected<TrackError>{TrackError::invalid_arg};
}
}
return scheme;
}
case SchemeKind::repeated_speed_input_mileage_segmentation_input_time_segmented: {
auto *segments = std::get_if<std::vector<RepeatedSMSegment>>(&scheme.segments);
if (segments == nullptr || segments->empty()) {
return unexpected<TrackError>{TrackError::invalid_arg};
}
std::ranges::sort(*segments, {}, &RepeatedSMSegment::time_since_start_s);
if (segments->front().time_since_start_s != 0) {
return unexpected<TrackError>{TrackError::invalid_arg};
}
for (auto &time_segment : *segments) {
auto &sub_segments = time_segment.speed_mileage_segments;
if (sub_segments.empty()) {
return unexpected<TrackError>{TrackError::invalid_arg};
}
std::ranges::sort(sub_segments, {}, &SMSegment::mileage_from_start_m);
if (sub_segments.front().mileage_from_start_m != 0) {
return unexpected<TrackError>{TrackError::invalid_arg};
}
for (const auto &sub_segment : sub_segments) {
if (!finite_nonnegative(sub_segment.speed_m_s)) {
return unexpected<TrackError>{TrackError::invalid_arg};
}
}
}
return scheme;
}
}
return unexpected<TrackError>{TrackError::not_supported};
}
void tick_speed_mileage(SchemeTrackRuntime &runtime, float delta_s) {
auto &state = runtime.state;
const auto &segments = std::get<std::vector<SMSegment>>(runtime.scheme.segments);
state.elapsed_s += delta_s;
const auto &current = segments[state.primary_segment_index];
if (runtime.scheme.acceleration_profile == AccelerationProfile::smooth) {
accel_calc_result profile{current.speed_m_s, 0.0F};
if (state.primary_segment_index + 1 < segments.size()) {
profile = speed_mileage_accel(
current,
segments[state.primary_segment_index + 1],
state.mileage_m,
state.speed_m_s);
}
apply_acceleration(state, profile, delta_s);
} else {
state.speed_m_s = current.speed_m_s;
}
state.mileage_m += state.speed_m_s * delta_s;
if (static_cast<std::uint16_t>(state.mileage_m) >= segments.back().mileage_from_start_m) {
state.is_running = false;
return;
}
while (state.primary_segment_index + 1 < segments.size()) {
const auto next_index = state.primary_segment_index + 1;
const auto &next = segments[next_index];
if (static_cast<std::uint16_t>(state.mileage_m) < next.mileage_from_start_m) {
break;
}
state.primary_segment_index = next_index;
state.speed_m_s = next.speed_m_s;
}
}
void tick_mileage_time(SchemeTrackRuntime &runtime, float delta_s) {
auto &state = runtime.state;
const auto &segments = std::get<std::vector<MTSegment>>(runtime.scheme.segments);
const float next_elapsed_s = state.elapsed_s + delta_s;
if (next_elapsed_s >= static_cast<float>(segments.back().time_since_start_s)) {
state.elapsed_s = next_elapsed_s;
state.is_running = false;
return;
}
state.elapsed_s = next_elapsed_s;
const auto next_target_speed = calc_mts_target_speed(state.primary_segment_index + 1, segments);
if (runtime.scheme.acceleration_profile == AccelerationProfile::smooth && next_target_speed &&
state.primary_segment_index + 1 < segments.size()) {
const float next_segment_time_s =
static_cast<float>(segments[state.primary_segment_index + 1].time_since_start_s);
const auto profile = mileage_time_accel(
state.elapsed_s,
next_segment_time_s,
state.speed_m_s,
*next_target_speed);
apply_acceleration(state, profile, delta_s);
}
state.mileage_m += state.speed_m_s * delta_s;
while (state.primary_segment_index + 1 < segments.size()) {
const auto next_index = state.primary_segment_index + 1;
const auto &next = segments[next_index];
if (state.elapsed_s < static_cast<float>(next.time_since_start_s)) {
break;
}
const auto &current = segments[state.primary_segment_index];
const float last_speed_m_s = state.speed_m_s;
state.primary_segment_index = next_index;
state.speed_m_s = calc_mts_target_speed(current, next).value_or(last_speed_m_s);
}
}
void tick_speed_time(SchemeTrackRuntime &runtime, float delta_s) {
auto &state = runtime.state;
const auto &segments = std::get<std::vector<STSegment>>(runtime.scheme.segments);
const float next_elapsed_s = state.elapsed_s + delta_s;
if (next_elapsed_s >= static_cast<float>(segments.back().time_since_start_s)) {
state.elapsed_s = next_elapsed_s;
state.is_running = false;
return;
}
state.elapsed_s = next_elapsed_s;
const auto &current = segments[state.primary_segment_index];
if (runtime.scheme.acceleration_profile == AccelerationProfile::smooth) {
accel_calc_result profile{current.speed_m_s, 0.0F};
if (state.primary_segment_index + 1 < segments.size()) {
profile = speed_time_accel(current, segments[state.primary_segment_index + 1]);
}
apply_acceleration(state, profile, delta_s);
} else {
state.speed_m_s = current.speed_m_s;
}
state.mileage_m += state.speed_m_s * delta_s;
while (state.primary_segment_index + 1 < segments.size()) {
const auto next_index = state.primary_segment_index + 1;
const auto &next = segments[next_index];
if (state.elapsed_s < static_cast<float>(next.time_since_start_s)) {
break;
}
state.primary_segment_index = next_index;
state.speed_m_s = next.speed_m_s;
}
}
[[nodiscard]]
float current_rsmt_loop_length(const SchemeTrackRuntime &runtime) {
const auto &segments = std::get<std::vector<RepeatedSMSegment>>(runtime.scheme.segments);
const auto &sub_segments = segments[runtime.state.primary_segment_index].speed_mileage_segments;
return sub_segments.empty()
? 0.0F
: static_cast<float>(sub_segments.back().mileage_from_start_m);
}
[[nodiscard]]
std::optional<std::pair<std::size_t, const SMSegment *>>
next_rsmt_sub_segment(const SchemeTrackRuntime &runtime) {
const auto &segments = std::get<std::vector<RepeatedSMSegment>>(runtime.scheme.segments);
const auto &sub_segments = segments[runtime.state.primary_segment_index].speed_mileage_segments;
const auto next_index = runtime.state.sub_segment_index + 1;
if (next_index < sub_segments.size()) {
return std::pair{next_index, &sub_segments[next_index]};
}
if (!sub_segments.empty()) {
return std::pair{std::size_t{0}, &sub_segments[0]};
}
return std::nullopt;
}
[[nodiscard]]
accel_calc_result rsmt_accel(
const SchemeTrackRuntime &runtime,
const SMSegment &next) {
const auto &state = runtime.state;
const float target_speed = next.speed_m_s;
float remaining_distance =
static_cast<float>(next.mileage_from_start_m) - state.loop_mileage_m;
if (next.mileage_from_start_m == 0 && state.sub_segment_index > 0) {
remaining_distance = current_rsmt_loop_length(runtime) - state.loop_mileage_m;
}
if (remaining_distance <= 0.0F) {
return {target_speed, 0.0F};
}
float accel =
(target_speed * target_speed - state.speed_m_s * state.speed_m_s) /
(2.0F * remaining_distance);
accel = std::clamp(accel, -max_acceleration_m_s_2, max_acceleration_m_s_2);
return {target_speed, accel};
}
void tick_repeated_speed_mileage_time(const TrackConfig &config, SchemeTrackRuntime &runtime, float delta_s) {
auto &state = runtime.state;
const auto &segments = std::get<std::vector<RepeatedSMSegment>>(runtime.scheme.segments);
state.elapsed_s += delta_s;
if (state.primary_segment_index + 1 < segments.size()) {
const auto &next_time_segment = segments[state.primary_segment_index + 1];
if (state.elapsed_s >= static_cast<float>(next_time_segment.time_since_start_s)) {
const float line_length_m = config.line_length_m;
const float err = line_length_m > 0.0F
? std::fmod(state.mileage_m, line_length_m)
: 0.0F;
const float epsilon = line_length_m * 0.01F;
if (std::abs(err) <= epsilon) {
++state.primary_segment_index;
state.sub_segment_index = 0;
state.loop_mileage_m = 0.0F;
state.speed_m_s =
segments[state.primary_segment_index].speed_mileage_segments[0].speed_m_s;
}
}
} else if (state.elapsed_s >= static_cast<float>(segments.back().time_since_start_s)) {
state.is_running = false;
return;
}
const auto &sub_segments = segments[state.primary_segment_index].speed_mileage_segments;
const auto &current = sub_segments[state.sub_segment_index];
if (runtime.scheme.acceleration_profile == AccelerationProfile::smooth) {
accel_calc_result profile{current.speed_m_s, 0.0F};
if (auto next = next_rsmt_sub_segment(runtime)) {
profile = rsmt_accel(runtime, *next->second);
}
apply_acceleration(state, profile, delta_s);
} else {
state.speed_m_s = current.speed_m_s;
}
const float distance_traveled = state.speed_m_s * delta_s;
state.mileage_m += distance_traveled;
state.loop_mileage_m += distance_traveled;
const float loop_length = current_rsmt_loop_length(runtime);
if (state.loop_mileage_m >= loop_length && loop_length > 0.0F) {
state.loop_mileage_m -= loop_length;
state.sub_segment_index = 0;
return;
}
while (auto next = next_rsmt_sub_segment(runtime)) {
const auto [next_index, next_segment] = *next;
if (next_index == 0) {
break;
}
if (state.loop_mileage_m < static_cast<float>(next_segment->mileage_from_start_m)) {
break;
}
state.sub_segment_index = next_index;
if (runtime.scheme.acceleration_profile == AccelerationProfile::instant) {
state.speed_m_s = next_segment->speed_m_s;
}
}
}
} // namespace
DecodedScheme make_speed_mileage_scheme(
std::uint8_t id,
Color color,
AccelerationProfile acceleration_profile,
std::vector<SMSegment> segments) {
return {
.id = id,
.color = color,
.kind = SchemeKind::speed_input_mileage_segmented_time_free,
.acceleration_profile = acceleration_profile,
.segments = std::move(segments),
};
}
DecodedScheme make_mileage_time_scheme(
std::uint8_t id,
Color color,
AccelerationProfile acceleration_profile,
std::vector<MTSegment> segments) {
return {
.id = id,
.color = color,
.kind = SchemeKind::mileage_input_time_segmented_speed_free,
.acceleration_profile = acceleration_profile,
.segments = std::move(segments),
};
}
DecodedScheme make_speed_time_scheme(
std::uint8_t id,
Color color,
AccelerationProfile acceleration_profile,
std::vector<STSegment> segments) {
return {
.id = id,
.color = color,
.kind = SchemeKind::speed_input_time_segmented_mileage_free,
.acceleration_profile = acceleration_profile,
.segments = std::move(segments),
};
}
DecodedScheme make_repeated_speed_mileage_time_scheme(
std::uint8_t id,
Color color,
AccelerationProfile acceleration_profile,
std::vector<RepeatedSMSegment> segments) {
return {
.id = id,
.color = color,
.kind = SchemeKind::repeated_speed_input_mileage_segmentation_input_time_segmented,
.acceleration_profile = acceleration_profile,
.segments = std::move(segments),
};
}
expected<SchemeTrackRuntime, TrackError> make_scheme_track_runtime(DecodedScheme scheme) {
auto normalized = canonicalize_scheme(std::move(scheme));
if (!normalized) {
return unexpected<TrackError>{normalized.error()};
}
return SchemeTrackRuntime{
.scheme = std::move(*normalized),
.state = {},
};
}
SchemeTrackRuntime start_scheme_track(SchemeTrackRuntime runtime) {
runtime.state = {};
runtime.state.is_running = true;
switch (runtime.scheme.kind) {
case SchemeKind::speed_input_mileage_segmented_time_free: {
const auto &segments = std::get<std::vector<SMSegment>>(runtime.scheme.segments);
runtime.state.speed_m_s = segments[0].speed_m_s;
break;
}
case SchemeKind::mileage_input_time_segmented_speed_free: {
const auto &segments = std::get<std::vector<MTSegment>>(runtime.scheme.segments);
runtime.state.speed_m_s = calc_mts_target_speed(std::size_t{0}, segments).value_or(0.0F);
break;
}
case SchemeKind::speed_input_time_segmented_mileage_free: {
const auto &segments = std::get<std::vector<STSegment>>(runtime.scheme.segments);
runtime.state.speed_m_s = segments[0].speed_m_s;
break;
}
case SchemeKind::repeated_speed_input_mileage_segmentation_input_time_segmented: {
const auto &segments = std::get<std::vector<RepeatedSMSegment>>(runtime.scheme.segments);
runtime.state.speed_m_s = segments[0].speed_mileage_segments[0].speed_m_s;
break;
}
}
return runtime;
}
SchemeTrackRuntime stop_scheme_track(SchemeTrackRuntime runtime) {
runtime.state.is_running = false;
return runtime;
}
SchemeTrackRuntime tick_scheme_track(
const TrackConfig &config,
SchemeTrackRuntime runtime,
float delta_s) {
if (!runtime.state.is_running || !std::isfinite(delta_s) || delta_s <= 0.0F) {
return runtime;
}
switch (runtime.scheme.kind) {
case SchemeKind::speed_input_mileage_segmented_time_free:
tick_speed_mileage(runtime, delta_s);
break;
case SchemeKind::mileage_input_time_segmented_speed_free:
tick_mileage_time(runtime, delta_s);
break;
case SchemeKind::speed_input_time_segmented_mileage_free:
tick_speed_time(runtime, delta_s);
break;
case SchemeKind::repeated_speed_input_mileage_segmentation_input_time_segmented:
tick_repeated_speed_mileage_time(config, runtime, delta_s);
break;
}
return runtime;
}
TrackInfo scheme_track_info(const SchemeTrackRuntime &runtime) {
std::size_t segment_count = 0;
switch (runtime.scheme.kind) {
case SchemeKind::speed_input_mileage_segmented_time_free:
segment_count = std::get<std::vector<SMSegment>>(runtime.scheme.segments).size();
break;
case SchemeKind::mileage_input_time_segmented_speed_free:
segment_count = std::get<std::vector<MTSegment>>(runtime.scheme.segments).size();
break;
case SchemeKind::speed_input_time_segmented_mileage_free:
segment_count = std::get<std::vector<STSegment>>(runtime.scheme.segments).size();
break;
case SchemeKind::repeated_speed_input_mileage_segmentation_input_time_segmented: {
const auto &segments = std::get<std::vector<RepeatedSMSegment>>(runtime.scheme.segments);
segment_count = std::accumulate(
segments.begin(),
segments.end(),
std::size_t{0},
[](std::size_t sum, const RepeatedSMSegment &segment) {
return sum + segment.speed_mileage_segments.size();
});
break;
}
}
return {
.kind = runtime.scheme.kind,
.color = runtime.scheme.color,
.id = runtime.scheme.id,
.is_running = runtime.state.is_running,
.num_segments = clamp_u8(segment_count),
};
}
TrackReport scheme_track_report(const SchemeTrackRuntime &runtime) {
return {
.id = runtime.scheme.id,
.state = runtime.state.is_running ? TrackState::run : TrackState::stop,
.mileage_m = runtime.state.mileage_m,
.speed_m_s = runtime.state.speed_m_s,
.time_elapsed_ms = elapsed_ms(runtime.state.elapsed_s),
};
}
bool SchemeTrainingRuntime::has_program() const noexcept {
return !tracks_.empty();
}
bool SchemeTrainingRuntime::all_stopped() const noexcept {
return std::ranges::all_of(tracks_, [](const SchemeTrackRuntime &track) {
return !track.state.is_running;
});
}
expected<unit, TrackError> SchemeTrainingRuntime::add_scheme(DecodedScheme scheme) {
auto runtime = make_scheme_track_runtime(std::move(scheme));
if (!runtime) {
return unexpected<TrackError>{runtime.error()};
}
const auto id = runtime->scheme.id;
const auto existing = std::ranges::find_if(tracks_, [id](const SchemeTrackRuntime &track) {
return track.scheme.id == id;
});
if (existing != tracks_.end()) {
tracks_.erase(existing);
}
tracks_.push_back(std::move(*runtime));
return {};
}
void SchemeTrainingRuntime::clear() {
tracks_.clear();
}
void SchemeTrainingRuntime::start() {
for (auto &track : tracks_) {
track = start_scheme_track(std::move(track));
}
}
void SchemeTrainingRuntime::stop() {
for (auto &track : tracks_) {
track = stop_scheme_track(std::move(track));
}
}
void SchemeTrainingRuntime::tick(const TrackConfig &config, float delta_s) {
for (auto &track : tracks_) {
track = tick_scheme_track(config, std::move(track), delta_s);
}
}
TrackStateReportCollection SchemeTrainingRuntime::state_collection() const {
TrackStateReportCollection collection;
collection.states.reserve(tracks_.size());
for (const auto &track : tracks_) {
collection.states.push_back(scheme_track_report(track));
}
return collection;
}
TrackSchemeMgrRead SchemeTrainingRuntime::scheme_status() const {
TrackSchemeMgrRead status;
status.scheme_status.reserve(tracks_.size());
for (const auto &track : tracks_) {
const auto info = scheme_track_info(track);
status.scheme_status.push_back(TrackSchemeMgrRead::Status{
.id = info.id,
.segment_count = info.num_segments,
.kind = info.kind,
});
}
return status;
}
expected<unit, TrackError>
SchemeTrainingRuntime::render_to(const TrackConfig &config, TrackRenderSink sink) const {
if (!config.verify() || config.line_leds_num == 0) {
return unexpected<TrackError>{TrackError::invalid_arg};
}
for (const auto &track : tracks_) {
const auto plan = make_track_render_plan(
config,
scheme_track_info(track),
scheme_track_report(track));
const auto err = apply_render_plan(sink, plan);
if (err != TrackError::ok) {
return unexpected<TrackError>{err};
}
}
return {};
}
expected<std::vector<Color>, TrackError>
SchemeTrainingRuntime::render_pixels(const TrackConfig &config) const {
MemoryStrip strip(config.line_leds_num);
const auto rendered = render_to(config, make_memory_strip_sink(strip));
if (!rendered) {
return unexpected<TrackError>{rendered.error()};
}
return std::vector<Color>{strip.pixels().begin(), strip.pixels().end()};
}
} // namespace track_core
+332
View File
@@ -5,7 +5,9 @@
#include "track_core/memory_strip.hpp"
#include "track_core/pid_program.hpp"
#include "track_core/pid_runtime.hpp"
#include "track_core/scheme_decoder.hpp"
#include "track_core/scheme_runtime.hpp"
namespace {
@@ -129,6 +131,43 @@ void test_linear_render_reverse() {
require(plan.spans[3].color == track_core::Color::cyan(), "reverse span 3 color");
}
void test_linear_render_boundaries_apply() {
track_core::TrackConfig config{
.draw_kind = track_core::TrackDrawKind::linear,
.line_length_m = 10.0F,
.active_line_length_m = 4.0F,
.head_offset_m = 0.0F,
.line_leds_num = 20,
};
const auto info = running_info(track_core::Color::red());
for (const float mileage : {
-20.0F, -10.1F, -10.0F, -9.9F, -1.0F, -0.1F,
0.0F, 0.1F, 1.0F, 9.9F, 10.0F, 10.1F,
19.9F, 20.0F, 20.1F, 29.9F, 30.0F, 30.1F,
40.0F,
}) {
const track_core::TrackReport report{
.id = 4,
.state = track_core::TrackState::run,
.mileage_m = mileage,
.speed_m_s = 2.0F,
};
track_core::MemoryStrip strip(config.line_leds_num);
const auto plan = track_core::make_track_render_plan(config, info, report);
require(plan.span_count <= track_core::TrackRenderPlan::max_spans, "linear boundary span capacity");
for (std::size_t i = 0; i < plan.span_count; ++i) {
const auto &span = plan.spans[i];
require(span.start_led < config.line_leds_num, "linear boundary span start in range");
require(span.start_led + span.led_count <= config.line_leds_num, "linear boundary span end in range");
}
require(
track_core::apply_render_plan(strip, plan) == track_core::TrackError::ok,
"linear boundary plan applies");
}
}
void test_memory_strip_bounds() {
track_core::MemoryStrip strip(4);
@@ -176,15 +215,308 @@ void test_pid_program_constant() {
require(state.is_finished(start + std::chrono::seconds(2)), "constant PID finished");
}
track_core::TrackPidConfig pid_runtime_config() {
track_core::TrackPidConfig config;
config.band_id = 5;
config.target_hr_bpm = 148;
config.deadzone_bpm = 3;
config.schemas = {
track_core::TrackPidSchema{track_core::TrackPidSegment{
.duration_s = 45,
.min_speed_m_s = 0.0F,
.max_speed_m_s = 4.0F,
.kp = 1.0F,
.ki = 2.0F,
.kd = 0.0F,
.slew_rate_limit = 0.25F,
.fine_tune = std::nullopt,
}},
};
return config;
}
track_core::TrackPidConfig constant_pid_runtime_config(float speed_m_s) {
track_core::TrackPidConfig config;
config.band_id = 5;
config.target_hr_bpm = 148;
config.deadzone_bpm = 3;
config.schemas = {
track_core::TrackPidSchema{track_core::TrackConstantSegment{
.duration_s = 2,
.speed_m_s = speed_m_s,
}},
};
return config;
}
track_core::TrackPidBandSnapshot band_sample(std::uint32_t seq, std::uint8_t heart_rate = 140) {
return {
.band_id = 5,
.heart_rate = heart_rate,
.heart_rate_sample_seq = seq,
.has_heart_rate = true,
.hr_is_fresh = true,
.step_count = 0,
.has_step_count = false,
.band_is_active = true,
};
}
void test_pid_runtime_consumes_each_hr_sample_once() {
track_core::PidHrRuntime runtime(pid_runtime_config());
const auto start = track_core::clock::time_point(std::chrono::seconds(0));
runtime.start(start);
runtime.tick(nullptr, band_sample(1), start + std::chrono::milliseconds(1));
const auto first_status = runtime.pid_status(band_sample(1), start + std::chrono::milliseconds(1));
require_near(first_status.effective_speed_m_s, 0.25F, 0.0001F, "pid runtime first effective speed");
require_near(first_status.base_speed_m_s, 0.25F, 0.0001F, "pid runtime first base speed");
runtime.tick(nullptr, band_sample(1), start + std::chrono::milliseconds(2));
const auto repeated_status = runtime.pid_status(band_sample(1), start + std::chrono::milliseconds(2));
require_near(
repeated_status.effective_speed_m_s,
first_status.effective_speed_m_s,
0.0001F,
"pid runtime ignores repeated sample effective speed");
require_near(
repeated_status.base_speed_m_s,
first_status.base_speed_m_s,
0.0001F,
"pid runtime ignores repeated sample base speed");
runtime.tick(nullptr, band_sample(2), start + std::chrono::milliseconds(3));
const auto second_status = runtime.pid_status(band_sample(2), start + std::chrono::milliseconds(3));
require(second_status.effective_speed_m_s > repeated_status.effective_speed_m_s, "pid runtime consumes new sample");
require(second_status.base_speed_m_s > repeated_status.base_speed_m_s, "pid runtime updates base speed on new sample");
}
void test_pid_runtime_suppression_is_explicitly_configured() {
auto config = constant_pid_runtime_config(1.0F);
config.speed_suppression = track_core::TrackPidSpeedSuppression{
.ratio_min = 0.25F,
.sigma_m = 0.01F,
};
const track_core::TrackConfig track_config{
.draw_kind = track_core::TrackDrawKind::circular,
.line_length_m = 0.1F,
.active_line_length_m = 0.025F,
.head_offset_m = 0.0F,
.line_leds_num = 100,
};
const auto start = track_core::clock::time_point(std::chrono::seconds(0));
track_core::PidHrRuntime without_config(config);
without_config.start(start);
without_config.tick(nullptr, {}, start + std::chrono::milliseconds(1));
auto status = without_config.pid_status({}, start + std::chrono::milliseconds(1));
require_near(status.effective_speed_m_s, 1.0F, 0.0001F, "pid runtime skips suppression without track config");
require_near(status.base_speed_m_s, 1.0F, 0.0001F, "pid runtime base speed without track config");
track_core::PidHrRuntime with_config(config);
with_config.start(start);
with_config.tick(&track_config, {}, start + std::chrono::milliseconds(1));
status = with_config.pid_status({}, start + std::chrono::milliseconds(1));
require(status.active_segment_kind == track_core::TrackPidStageKind::constant, "pid runtime constant stage kind");
require_near(status.effective_speed_m_s, 0.25F, 0.0001F, "pid runtime suppresses at seam");
require_near(status.base_speed_m_s, 1.0F, 0.0001F, "pid runtime base speed at seam");
}
void test_pid_runtime_applies_live_tuning() {
track_core::PidHrRuntime runtime(pid_runtime_config());
const auto start = track_core::clock::time_point(std::chrono::seconds(0));
runtime.start(start);
runtime.tick(nullptr, band_sample(1), start + std::chrono::milliseconds(1));
const track_core::TrackPidRuntimeCommand command{
.command = track_core::TrackPidSetTuning{
.min_speed_m_s = 1.2F,
.max_speed_m_s = 3.5F,
.kp = 0.5F,
.ki = 0.25F,
.kd = 0.0F,
.slew_rate_limit = 0.25F,
.fine_tune = std::nullopt,
},
};
const auto applied = runtime.apply_pid_runtime_command(command, nullptr);
require(applied.has_value(), "pid runtime applies tuning");
require_near(
std::get<track_core::TrackPidSegment>(runtime.pid_config().schemas.front().segment).min_speed_m_s,
1.2F,
0.0001F,
"pid runtime config tracks tuning");
const auto status = runtime.pid_status(band_sample(1), start + std::chrono::milliseconds(1));
require_near(status.effective_speed_m_s, 1.2F, 0.0001F, "pid runtime tuned effective speed");
require_near(status.base_speed_m_s, 1.2F, 0.0001F, "pid runtime tuned base speed");
}
track_core::TrackConfig runtime_config() {
return {
.draw_kind = track_core::TrackDrawKind::circular,
.line_length_m = 5.0F,
.active_line_length_m = 1.0F,
.head_offset_m = 0.0F,
.line_leds_num = 50,
};
}
void test_speed_mileage_runtime_ticks() {
auto runtime = track_core::make_scheme_track_runtime(track_core::make_speed_mileage_scheme(
11,
track_core::Color::red(),
track_core::AccelerationProfile::instant,
{
track_core::SMSegment{.speed_m_s = 1.0F, .mileage_from_start_m = 0},
track_core::SMSegment{.speed_m_s = 2.0F, .mileage_from_start_m = 5},
track_core::SMSegment{.speed_m_s = 1.0F, .mileage_from_start_m = 10},
}));
require(runtime.has_value(), "SM runtime builds");
auto track = track_core::start_scheme_track(std::move(*runtime));
track = track_core::tick_scheme_track(runtime_config(), std::move(track), 6.0F);
const auto report = track_core::scheme_track_report(track);
require(report.state == track_core::TrackState::run, "SM still running before end");
require_near(report.mileage_m, 6.0F, 0.001F, "SM mileage after tick");
require_near(report.speed_m_s, 2.0F, 0.001F, "SM speed after boundary");
}
void test_mileage_time_runtime_ticks() {
auto runtime = track_core::make_scheme_track_runtime(track_core::make_mileage_time_scheme(
12,
track_core::Color::green(),
track_core::AccelerationProfile::instant,
{
track_core::MTSegment{.mileage_to_travel_this_segment_m = 10, .time_since_start_s = 0},
track_core::MTSegment{.mileage_to_travel_this_segment_m = 20, .time_since_start_s = 5},
track_core::MTSegment{.mileage_to_travel_this_segment_m = 1, .time_since_start_s = 15},
}));
require(runtime.has_value(), "MT runtime builds");
auto track = track_core::start_scheme_track(std::move(*runtime));
track = track_core::tick_scheme_track(runtime_config(), std::move(track), 2.0F);
auto report = track_core::scheme_track_report(track);
require_near(report.speed_m_s, 2.0F, 0.001F, "MT initial derived speed");
require_near(report.mileage_m, 4.0F, 0.001F, "MT mileage after first tick");
track = track_core::tick_scheme_track(runtime_config(), std::move(track), 4.0F);
report = track_core::scheme_track_report(track);
require_near(report.speed_m_s, 2.0F, 0.001F, "MT speed after first time boundary");
require_near(report.mileage_m, 12.0F, 0.001F, "MT mileage after boundary tick");
}
void test_speed_time_runtime_ticks() {
auto runtime = track_core::make_scheme_track_runtime(track_core::make_speed_time_scheme(
13,
track_core::Color::blue(),
track_core::AccelerationProfile::instant,
{
track_core::STSegment{.speed_m_s = 1.0F, .time_since_start_s = 0},
track_core::STSegment{.speed_m_s = 3.0F, .time_since_start_s = 5},
track_core::STSegment{.speed_m_s = 1.0F, .time_since_start_s = 10},
}));
require(runtime.has_value(), "ST runtime builds");
auto track = track_core::start_scheme_track(std::move(*runtime));
track = track_core::tick_scheme_track(runtime_config(), std::move(track), 6.0F);
const auto report = track_core::scheme_track_report(track);
require(report.state == track_core::TrackState::run, "ST still running before end");
require_near(report.mileage_m, 6.0F, 0.001F, "ST mileage after tick");
require_near(report.speed_m_s, 3.0F, 0.001F, "ST speed after time boundary");
}
void test_repeated_speed_mileage_time_runtime_ticks() {
auto runtime = track_core::make_scheme_track_runtime(track_core::make_repeated_speed_mileage_time_scheme(
14,
track_core::Color::white(),
track_core::AccelerationProfile::instant,
{
track_core::RepeatedSMSegment{
.speed_mileage_segments = {
track_core::SMSegment{.speed_m_s = 1.0F, .mileage_from_start_m = 0},
track_core::SMSegment{.speed_m_s = 2.0F, .mileage_from_start_m = 5},
},
.time_since_start_s = 0,
},
track_core::RepeatedSMSegment{
.speed_mileage_segments = {
track_core::SMSegment{.speed_m_s = 3.0F, .mileage_from_start_m = 0},
track_core::SMSegment{.speed_m_s = 1.0F, .mileage_from_start_m = 5},
},
.time_since_start_s = 4,
},
track_core::RepeatedSMSegment{
.speed_mileage_segments = {
track_core::SMSegment{.speed_m_s = 1.0F, .mileage_from_start_m = 0},
track_core::SMSegment{.speed_m_s = 1.0F, .mileage_from_start_m = 5},
},
.time_since_start_s = 20,
},
}));
require(runtime.has_value(), "RSMT runtime builds");
auto track = track_core::start_scheme_track(std::move(*runtime));
track = track_core::tick_scheme_track(runtime_config(), std::move(track), 3.0F);
track = track_core::tick_scheme_track(runtime_config(), std::move(track), 2.0F);
require_eq_u32(static_cast<std::uint32_t>(track.state.primary_segment_index), 0, "RSMT waits for line alignment");
track = track_core::tick_scheme_track(runtime_config(), std::move(track), 1.0F);
require_eq_u32(static_cast<std::uint32_t>(track.state.primary_segment_index), 1, "RSMT switches on alignment");
require_near(track.state.speed_m_s, 3.0F, 0.001F, "RSMT switched speed");
}
void test_scheme_training_runtime_renders() {
track_core::SchemeTrainingRuntime runtime;
const auto added = runtime.add_scheme(track_core::make_speed_time_scheme(
15,
track_core::Color::green(),
track_core::AccelerationProfile::instant,
{
track_core::STSegment{.speed_m_s = 1.0F, .time_since_start_s = 0},
track_core::STSegment{.speed_m_s = 1.0F, .time_since_start_s = 10},
}));
require(added.has_value(), "training runtime accepts scheme");
runtime.start();
runtime.tick(runtime_config(), 1.0F);
const auto pixels = runtime.render_pixels(runtime_config());
require(pixels.has_value(), "training runtime renders pixels");
require_eq_u32(static_cast<std::uint32_t>(pixels->size()), runtime_config().line_leds_num, "training runtime pixel count");
track_core::MemoryStrip strip(runtime_config().line_leds_num);
const auto rendered = runtime.render_to(runtime_config(), track_core::make_memory_strip_sink(strip));
require(rendered.has_value(), "training runtime renders to sink");
for (std::size_t i = 0; i < pixels->size(); ++i) {
require((*pixels)[i] == strip.pixels()[i], "render_to matches render_pixels");
}
require(!runtime.all_stopped(), "training runtime running");
require_eq_u32(runtime.state_collection().states[0].id, 15, "training runtime report id");
}
} // namespace
int main() {
test_circular_render_wraps();
test_linear_render_forward_and_memory_strip();
test_linear_render_reverse();
test_linear_render_boundaries_apply();
test_memory_strip_bounds();
test_scheme_decoder();
test_pid_program_constant();
test_pid_runtime_consumes_each_hr_sample_once();
test_pid_runtime_suppression_is_explicitly_configured();
test_pid_runtime_applies_live_tuning();
test_speed_mileage_runtime_ticks();
test_mileage_time_runtime_ticks();
test_speed_time_runtime_ticks();
test_repeated_speed_mileage_time_runtime_ticks();
test_scheme_training_runtime_renders();
std::cout << "track-core tests passed\n";
return 0;
}