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:
@@ -1,3 +1,7 @@
|
||||
build/
|
||||
.venv/
|
||||
*.egg-info/
|
||||
__pycache__/
|
||||
.pytest_cache/
|
||||
.DS_Store
|
||||
compile_commands.json
|
||||
|
||||
+28
-1
@@ -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)
|
||||
|
||||
@@ -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.
|
||||
|
||||
@@ -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
|
||||
@@ -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
@@ -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
|
||||
|
||||
@@ -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
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
@@ -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"
|
||||
@@ -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
|
||||
@@ -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",
|
||||
]
|
||||
@@ -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()
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
@@ -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
|
||||
|
||||
@@ -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
|
||||
@@ -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
@@ -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();
|
||||
|
||||
@@ -30,6 +30,7 @@ expected<DecodedScheme, TrackError> decode_scheme(
|
||||
.color = color,
|
||||
.kind = kind,
|
||||
.acceleration_profile = acceleration_profile,
|
||||
.segments = std::vector<SMSegment>{},
|
||||
};
|
||||
|
||||
switch (kind) {
|
||||
|
||||
@@ -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 ¤t,
|
||||
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 ¤t, 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 ¤t = 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 ¤t, 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 ¤t = 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 ¤t = 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 ¤t = 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 ¤t = 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
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user