Initial standalone track core

This commit is contained in:
2026-05-15 16:02:44 +08:00
commit 84598cad20
15 changed files with 1733 additions and 0 deletions
+3
View File
@@ -0,0 +1,3 @@
build/
.DS_Store
compile_commands.json
+40
View File
@@ -0,0 +1,40 @@
set(TRACK_CORE_SOURCES
src/memory_strip.cpp
src/model.cpp
src/pid_program.cpp
src/render.cpp
src/scheme_decoder.cpp
)
if(DEFINED IDF_TARGET)
idf_component_register(
SRCS ${TRACK_CORE_SOURCES}
INCLUDE_DIRS include
)
else()
cmake_minimum_required(VERSION 3.20)
project(track_core VERSION 0.1.0 LANGUAGES CXX)
add_library(track_core ${TRACK_CORE_SOURCES})
add_library(track_core::track_core ALIAS track_core)
target_include_directories(track_core
PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
)
target_compile_features(track_core PUBLIC cxx_std_23)
option(TRACK_CORE_BUILD_TESTS "Build track-core tests" ON)
if(TRACK_CORE_BUILD_TESTS)
enable_testing()
add_executable(track_core_tests tests/track_core_tests.cpp)
target_link_libraries(track_core_tests PRIVATE track_core::track_core)
target_compile_features(track_core_tests PRIVATE cxx_std_23)
add_test(NAME track_core_tests COMMAND track_core_tests)
endif()
add_executable(track_core_render_demo examples/render_demo.cpp)
target_link_libraries(track_core_render_demo PRIVATE track_core::track_core)
target_compile_features(track_core_render_demo PRIVATE cxx_std_23)
endif()
+17
View File
@@ -0,0 +1,17 @@
# track-core
Standalone C++ core for TrackBackFwd track simulation and strip rendering.
This library is intentionally platform-neutral. It does not depend on ESP-IDF,
FreeRTOS, protobuf/nanopb, BLE, or hardware LED drivers.
## Build
```bash
cmake -S . -B build
cmake --build build
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.
+40
View File
@@ -0,0 +1,40 @@
#include <iostream>
#include "track_core/memory_strip.hpp"
int main() {
track_core::TrackConfig config{
.draw_kind = track_core::TrackDrawKind::circular,
.line_length_m = 10.0F,
.active_line_length_m = 4.0F,
.head_offset_m = 0.0F,
.line_leds_num = 20,
};
track_core::TrackInfo info{
.kind = track_core::SchemeKind::speed_input_time_segmented_mileage_free,
.color = track_core::Color::green(),
.id = 1,
.is_running = true,
.num_segments = 1,
};
track_core::TrackReport report{
.id = 1,
.state = track_core::TrackState::run,
.mileage_m = 8.5F,
.speed_m_s = 1.0F,
.time_elapsed_ms = 0,
};
track_core::MemoryStrip strip(config.line_leds_num);
const auto plan = track_core::make_track_render_plan(config, info, report);
static_cast<void>(track_core::apply_render_plan(strip, plan));
static_cast<void>(strip.show());
std::cout << "frame=" << strip.frame_sequence() << " leds=" << strip.leds_count() << "\n";
for (std::size_t i = 0; i < plan.span_count; ++i) {
const auto &span = plan.spans[i];
std::cout << "span start=" << span.start_led
<< " count=" << span.led_count
<< " color=" << span.color.hex() << "\n";
}
}
+49
View File
@@ -0,0 +1,49 @@
#pragma once
#include <cstddef>
#include <cstdint>
#include <vector>
#include "track_core/model.hpp"
#include "track_core/render.hpp"
namespace track_core {
class MemoryStrip {
public:
explicit MemoryStrip(std::size_t led_count = 0);
[[nodiscard]]
TrackError begin();
[[nodiscard]]
TrackError clear();
[[nodiscard]]
TrackError fill(std::size_t start, std::size_t count, Color color);
[[nodiscard]]
TrackError show();
[[nodiscard]]
TrackError set_leds_count(std::size_t count);
[[nodiscard]]
std::size_t leds_count() const;
[[nodiscard]]
std::uint64_t frame_sequence() const;
[[nodiscard]]
std::span<const Color> pixels() const;
private:
std::vector<Color> pixels_;
std::uint64_t frame_sequence_{};
bool begun_{};
};
[[nodiscard]]
TrackError apply_render_plan(MemoryStrip &strip, const TrackRenderPlan &plan);
} // namespace track_core
+330
View File
@@ -0,0 +1,330 @@
#pragma once
#include <algorithm>
#include <chrono>
#include <cstdint>
#include <expected>
#include <limits>
#include <optional>
#include <span>
#include <string>
#include <variant>
#include <vector>
namespace track_core {
template <typename T, typename E>
using expected = std::expected<T, E>;
template <typename E>
using unexpected = std::unexpected<E>;
using unit = std::monostate;
enum class TrackError : int {
ok = 0,
invalid_arg,
invalid_size,
invalid_state,
not_supported,
range,
};
enum class TrackDrawKind : std::uint8_t {
circular = 0,
linear = 1,
};
enum class SchemeKind : std::uint8_t {
speed_input_mileage_segmented_time_free = 0,
mileage_input_time_segmented_speed_free = 1,
speed_input_time_segmented_mileage_free = 2,
repeated_speed_input_mileage_segmentation_input_time_segmented = 3,
};
enum class AccelerationProfile : std::uint8_t {
instant = 0,
smooth = 1,
};
enum class TrackState : std::uint8_t {
stop = 0,
run = 1,
test_rainbow = 2,
test_blink = 3,
};
enum class TrackControllerMode : std::uint8_t {
scheme = 0,
pid_hr = 1,
};
enum class TrackPidStageKind : std::uint8_t {
none = 0,
constant = 1,
pid = 2,
};
struct Color {
std::uint8_t r{};
std::uint8_t g{};
std::uint8_t b{};
constexpr Color() = default;
constexpr Color(std::uint8_t red, std::uint8_t green, std::uint8_t blue)
: r(red), g(green), b(blue) {}
constexpr explicit Color(std::uint32_t value)
: r(static_cast<std::uint8_t>((value >> 16U) & 0xFFU)),
g(static_cast<std::uint8_t>((value >> 8U) & 0xFFU)),
b(static_cast<std::uint8_t>(value & 0xFFU)) {}
[[nodiscard]]
constexpr bool operator==(const Color &) const = default;
[[nodiscard]]
constexpr explicit operator std::uint32_t() const {
return (static_cast<std::uint32_t>(r) << 16U) |
(static_cast<std::uint32_t>(g) << 8U) |
static_cast<std::uint32_t>(b);
}
[[nodiscard]]
std::string hex() const;
[[nodiscard]]
static constexpr Color black() { return {0, 0, 0}; }
[[nodiscard]]
static constexpr Color red() { return {255, 0, 0}; }
[[nodiscard]]
static constexpr Color orange() { return {255, 165, 0}; }
[[nodiscard]]
static constexpr Color yellow() { return {255, 255, 0}; }
[[nodiscard]]
static constexpr Color green() { return {0, 255, 0}; }
[[nodiscard]]
static constexpr Color cyan() { return {0, 255, 255}; }
[[nodiscard]]
static constexpr Color blue() { return {0, 0, 255}; }
[[nodiscard]]
static constexpr Color indigo() { return {75, 0, 130}; }
[[nodiscard]]
static constexpr Color violet() { return {238, 130, 238}; }
[[nodiscard]]
static constexpr Color white() { return {255, 255, 255}; }
};
struct TrackConfig {
TrackDrawKind draw_kind{TrackDrawKind::circular};
float line_length_m{0.0F};
float active_line_length_m{0.0F};
float head_offset_m{0.0F};
std::uint16_t line_leds_num{0};
[[nodiscard]]
float led_distance() const {
return line_leds_num > 0 ? line_length_m / static_cast<float>(line_leds_num) : 0.0F;
}
[[nodiscard]]
bool verify() const {
if (line_length_m <= 0.0F || active_line_length_m <= 0.0F) {
return false;
}
if (active_line_length_m > line_length_m) {
return false;
}
if (head_offset_m >= line_length_m) {
return false;
}
return true;
}
[[nodiscard]]
static TrackConfig default_config() {
return {
.draw_kind = TrackDrawKind::circular,
.line_length_m = 400.0F,
.active_line_length_m = 10.0F,
.head_offset_m = 0.0F,
.line_leds_num = 400,
};
}
};
struct TrackInfo {
SchemeKind kind{SchemeKind::speed_input_time_segmented_mileage_free};
Color color{Color::white()};
std::uint8_t id{};
bool is_running{};
std::uint8_t num_segments{};
};
struct TrackReport {
std::uint8_t id{};
TrackState state{TrackState::stop};
float mileage_m{};
float speed_m_s{};
std::uint32_t time_elapsed_ms{};
};
struct TrackStateReportCollection {
std::vector<TrackReport> states;
};
struct TrackSchemeMgrRead {
struct Status {
std::uint8_t id{};
std::uint8_t segment_count{};
SchemeKind kind{SchemeKind::speed_input_time_segmented_mileage_free};
};
std::vector<Status> scheme_status;
};
struct SMSegment {
static constexpr float speed_lsb = 10.0F / 255.0F;
static constexpr std::size_t raw_size = 3;
float speed_m_s{};
std::uint16_t mileage_from_start_m{};
};
struct MTSegment {
std::uint16_t mileage_to_travel_this_segment_m{};
std::uint16_t time_since_start_s{};
};
struct STSegment {
static constexpr float speed_lsb = 10.0F / 255.0F;
static constexpr std::size_t raw_size = 3;
float speed_m_s{};
std::uint16_t time_since_start_s{};
};
struct RepeatedSMSegment {
std::vector<SMSegment> speed_mileage_segments;
std::uint16_t time_since_start_s{};
};
struct TrackPidFineTune {
std::uint8_t band_plus{};
std::uint8_t band_minus{};
float gain_scale{0.0F};
};
struct TrackPidSpeedSuppression {
float ratio_min{0.1F};
float sigma_m{1.0F};
};
struct TrackPidSegment {
static constexpr float default_kp = 0.0457F;
static constexpr float default_ki = 0.001464F;
static constexpr float default_kd = 0.0F;
static constexpr float default_slew_rate_limit_m_s = 0.225F;
std::uint16_t duration_s{0};
float min_speed_m_s{0.0F};
float max_speed_m_s{0.0F};
float kp{0.0F};
float ki{1.0F};
float kd{0.0F};
float slew_rate_limit{0.0F};
std::optional<TrackPidFineTune> fine_tune;
[[nodiscard]]
static TrackPidSegment default_segment() {
return {
.duration_s = 300,
.min_speed_m_s = 0.0F,
.max_speed_m_s = 4.0F,
.kp = default_kp,
.ki = default_ki,
.kd = default_kd,
.slew_rate_limit = default_slew_rate_limit_m_s,
.fine_tune = std::nullopt,
};
}
};
struct TrackConstantSegment {
std::uint16_t duration_s{0};
float speed_m_s{0.0F};
};
struct TrackPidSchema {
using variant_type = std::variant<TrackPidSegment, TrackConstantSegment>;
variant_type segment{TrackConstantSegment{}};
[[nodiscard]]
TrackPidStageKind kind() const {
return std::holds_alternative<TrackPidSegment>(segment)
? TrackPidStageKind::pid
: TrackPidStageKind::constant;
}
};
struct TrackPidConfig {
std::uint8_t band_id{0};
std::uint8_t target_hr_bpm{120};
std::uint8_t deadzone_bpm{3};
bool preemptive_pid_activation{false};
std::optional<TrackPidSpeedSuppression> speed_suppression;
std::vector<TrackPidSchema> schemas;
[[nodiscard]]
bool empty() const {
return schemas.empty();
}
[[nodiscard]]
static TrackPidConfig default_config() {
TrackPidConfig config;
config.schemas.push_back(TrackPidSchema{TrackPidSegment::default_segment()});
return config;
}
[[nodiscard]]
expected<unit, TrackError> validate(bool allow_empty = false) const;
};
struct TrackPidSetTargetHr {
std::uint8_t target_hr_bpm{120};
};
struct TrackPidSetTuning {
float min_speed_m_s{0.0F};
float max_speed_m_s{0.0F};
float kp{0.0F};
float ki{1.0F};
float kd{0.0F};
float slew_rate_limit{0.0F};
std::optional<TrackPidFineTune> fine_tune;
[[nodiscard]]
expected<unit, TrackError> validate() const;
void apply_to(TrackPidSegment &segment) const;
};
struct TrackPidRuntimeCommand {
std::variant<unit, TrackPidSetTargetHr, TrackPidSetTuning> command{unit{}};
};
struct TrackPidStatus {
std::uint8_t band_id{};
bool band_is_active{};
bool is_heart_rate_valid{};
std::uint8_t heart_rate_bpm{};
std::uint16_t step_count{};
std::uint8_t active_segment_index{};
TrackPidStageKind active_segment_kind{TrackPidStageKind::none};
float effective_speed_m_s{};
std::uint32_t remaining_program_ms{};
float base_speed_m_s{};
};
using clock = std::chrono::steady_clock;
} // namespace track_core
+116
View File
@@ -0,0 +1,116 @@
#pragma once
#include "track_core/model.hpp"
namespace track_core {
class TrackPidProgramState {
public:
using time_point = clock::time_point;
struct HrSample {
float heart_rate_bpm{0.0F};
float sample_interval_s{0.0F};
};
TrackPidProgramState() = delete;
TrackPidProgramState(time_point now, TrackPidConfig config);
[[nodiscard]]
float next(time_point now, std::optional<HrSample> fresh_hr_sample);
[[nodiscard]]
bool is_finished(time_point now) const;
[[nodiscard]]
TrackPidStageKind active_stage_kind() const;
[[nodiscard]]
std::uint8_t active_stage_index() const;
[[nodiscard]]
float commanded_speed_m_s() const;
[[nodiscard]]
std::uint32_t remaining_program_ms(time_point now) const;
void update_target_hr_bpm(std::uint8_t target_hr_bpm);
[[nodiscard]]
expected<unit, TrackError> update_tuning(const TrackPidSetTuning &tuning);
private:
struct pid_state {
struct fine_tune_state {
std::uint8_t band_plus{};
std::uint8_t band_minus{};
float gain_scale{1.0F};
};
float kp{0.0F};
float ki{1.0F};
float kd{0.0F};
std::optional<fine_tune_state> fine_tune;
float slew_rate_limit{0.0F};
float e_t_1{0.0F};
float e_t_2{0.0F};
float u_t_1{0.0F};
float min_speed_m_s{0.0F};
float max_speed_m_s{0.0F};
bool should_use_nominal_sample_interval{true};
void from_segment(const TrackPidSegment &segment);
void reset_with(float speed_m_s);
[[nodiscard]]
float effective_gain_scale(float target_hr, float current_hr) const;
[[nodiscard]]
float clamp_commanded_speed(float speed_m_s) const;
void apply_tuning(const TrackPidSetTuning &tuning);
};
[[nodiscard]]
const TrackPidSchema &schema() const;
[[nodiscard]]
static std::uint16_t schema_duration_s(const TrackPidSchema &schema);
[[nodiscard]]
bool sync_schema_for_time(time_point now);
void advance_to_schema_index(std::size_t target_index);
[[nodiscard]]
std::size_t resolve_schema_index(time_point now) const;
[[nodiscard]]
bool maybe_jump_to_final_pid(time_point now, std::optional<HrSample> fresh_hr_sample);
[[nodiscard]]
bool is_in_deadzone(float heart_rate) const;
[[nodiscard]]
float do_pid(float current_hr, float sample_interval_s);
void apply_schema(const TrackPidSchema &schema);
void finish();
[[nodiscard]]
static bool validate_preemptive_schema(const std::vector<TrackPidSchema> &schemas);
[[nodiscard]]
static time_point safe_add_seconds(time_point now, std::uint32_t duration_s);
TrackPidConfig config_;
time_point program_start_timestamp_{};
time_point program_end_timestamp_{};
std::size_t schema_index_{};
pid_state pid_{};
bool preemptive_enabled_{};
bool forced_final_pid_{};
bool finished_{};
};
} // namespace track_core
+36
View File
@@ -0,0 +1,36 @@
#pragma once
#include <array>
#include <cstdint>
#include "track_core/model.hpp"
namespace track_core {
struct TrackRenderSpan {
std::uint16_t start_led{};
std::uint16_t led_count{};
Color color{};
};
struct TrackRenderPlan {
static constexpr std::size_t max_spans = 4;
void add_fill(std::uint16_t start_led, std::uint16_t led_count, Color color);
[[nodiscard]]
bool empty() const {
return span_count == 0;
}
std::array<TrackRenderSpan, max_spans> spans{};
std::size_t span_count{};
};
[[nodiscard]]
TrackRenderPlan make_track_render_plan(
const TrackConfig &config,
const TrackInfo &info,
const TrackReport &report);
} // namespace track_core
+31
View File
@@ -0,0 +1,31 @@
#pragma once
#include <cstdint>
#include <span>
#include <variant>
#include <vector>
#include "track_core/model.hpp"
namespace track_core {
struct DecodedScheme {
std::uint8_t id{};
Color color{Color::white()};
SchemeKind kind{SchemeKind::speed_input_time_segmented_mileage_free};
AccelerationProfile acceleration_profile{AccelerationProfile::smooth};
std::variant<
std::vector<SMSegment>,
std::vector<MTSegment>,
std::vector<STSegment>,
std::vector<RepeatedSMSegment>>
segments;
};
[[nodiscard]]
expected<DecodedScheme, TrackError> decode_scheme(
std::uint8_t id,
Color color,
std::span<const std::uint8_t> binary);
} // namespace track_core
+72
View File
@@ -0,0 +1,72 @@
#include "track_core/memory_strip.hpp"
#include <algorithm>
namespace track_core {
MemoryStrip::MemoryStrip(std::size_t led_count)
: pixels_(led_count, Color::black()) {}
TrackError MemoryStrip::begin() {
if (pixels_.empty()) {
return TrackError::invalid_size;
}
begun_ = true;
return TrackError::ok;
}
TrackError MemoryStrip::clear() {
std::ranges::fill(pixels_, Color::black());
return TrackError::ok;
}
TrackError MemoryStrip::fill(std::size_t start, std::size_t count, Color color) {
if (count == 0) {
return TrackError::ok;
}
if (start >= pixels_.size()) {
return TrackError::range;
}
const auto available = pixels_.size() - start;
const auto length = std::min(count, available);
std::fill_n(pixels_.begin() + static_cast<std::ptrdiff_t>(start), length, color);
return TrackError::ok;
}
TrackError MemoryStrip::show() {
++frame_sequence_;
return TrackError::ok;
}
TrackError MemoryStrip::set_leds_count(std::size_t count) {
if (count == 0) {
return TrackError::invalid_arg;
}
pixels_.assign(count, Color::black());
return TrackError::ok;
}
std::size_t MemoryStrip::leds_count() const {
return pixels_.size();
}
std::uint64_t MemoryStrip::frame_sequence() const {
return frame_sequence_;
}
std::span<const Color> MemoryStrip::pixels() const {
return pixels_;
}
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;
}
} // namespace track_core
+104
View File
@@ -0,0 +1,104 @@
#include "track_core/model.hpp"
#include <cmath>
#include <cstdio>
namespace track_core {
namespace {
template <class... Ts>
struct overloads : Ts... {
using Ts::operator()...;
};
} // namespace
std::string Color::hex() const {
char buffer[8]{};
static_cast<void>(std::snprintf(buffer, sizeof(buffer), "#%02X%02X%02X", r, g, b));
return buffer;
}
expected<unit, TrackError> TrackPidConfig::validate(bool allow_empty) const {
const auto speed_suppression_ok =
!speed_suppression.has_value() ||
(std::isfinite(speed_suppression->ratio_min) &&
std::isfinite(speed_suppression->sigma_m) &&
speed_suppression->ratio_min > 0.0F &&
speed_suppression->ratio_min < 1.0F &&
speed_suppression->sigma_m > 0.0F);
if (!speed_suppression_ok) {
return unexpected<TrackError>{TrackError::invalid_arg};
}
if (schemas.empty()) {
if (allow_empty) {
return {};
}
return unexpected<TrackError>{TrackError::invalid_arg};
}
for (const auto &schema : schemas) {
const auto ok = std::visit(overloads{
[](const TrackPidSegment &pid) {
return pid.duration_s > 0 &&
std::isfinite(pid.min_speed_m_s) &&
std::isfinite(pid.max_speed_m_s) &&
std::isfinite(pid.kp) &&
std::isfinite(pid.ki) &&
std::isfinite(pid.kd) &&
std::isfinite(pid.slew_rate_limit) &&
(!pid.fine_tune.has_value() ||
(std::isfinite(pid.fine_tune->gain_scale) &&
pid.fine_tune->gain_scale >= 0.0F)) &&
pid.min_speed_m_s >= 0.0F &&
pid.max_speed_m_s >= 0.0F &&
pid.min_speed_m_s <= pid.max_speed_m_s &&
pid.kp >= 0.0F &&
pid.ki >= 0.0F &&
pid.kd >= 0.0F &&
pid.slew_rate_limit >= 0.0F;
},
[](const TrackConstantSegment &constant) {
return constant.duration_s > 0 &&
std::isfinite(constant.speed_m_s) &&
constant.speed_m_s >= 0.0F;
}},
schema.segment);
if (!ok) {
return unexpected<TrackError>{TrackError::invalid_arg};
}
}
return {};
}
expected<unit, TrackError> TrackPidSetTuning::validate() const {
const auto fine_tune_ok =
!fine_tune.has_value() ||
(std::isfinite(fine_tune->gain_scale) && fine_tune->gain_scale >= 0.0F);
if (!std::isfinite(min_speed_m_s) ||
!std::isfinite(max_speed_m_s) ||
!std::isfinite(kp) ||
!std::isfinite(ki) ||
!std::isfinite(kd) ||
!std::isfinite(slew_rate_limit) ||
!fine_tune_ok ||
min_speed_m_s < 0.0F ||
max_speed_m_s < 0.0F ||
min_speed_m_s > max_speed_m_s ||
kp < 0.0F ||
ki < 0.0F ||
kd < 0.0F ||
slew_rate_limit < 0.0F) {
return unexpected<TrackError>{TrackError::invalid_arg};
}
return {};
}
void TrackPidSetTuning::apply_to(TrackPidSegment &segment) const {
segment.min_speed_m_s = min_speed_m_s;
segment.max_speed_m_s = max_speed_m_s;
segment.kp = kp;
segment.ki = ki;
segment.kd = kd;
segment.slew_rate_limit = slew_rate_limit;
segment.fine_tune = fine_tune;
}
} // namespace track_core
+326
View File
@@ -0,0 +1,326 @@
#include "track_core/pid_program.hpp"
#include <algorithm>
#include <cassert>
#include <cmath>
namespace track_core {
namespace {
constexpr float default_pid_nominal_sample_interval_s = 2.0F;
constexpr float default_pid_min_sample_interval_s = 1.0F;
constexpr float default_pid_max_sample_interval_s = 3.0F;
template <class... Ts>
struct overloads : Ts... {
using Ts::operator()...;
};
float clamp_hr_sample_interval_s(float sample_interval_s) {
if (!std::isfinite(sample_interval_s) || sample_interval_s <= 0.0F) {
return default_pid_nominal_sample_interval_s;
}
return std::clamp(sample_interval_s,
default_pid_min_sample_interval_s,
default_pid_max_sample_interval_s);
}
} // namespace
void TrackPidProgramState::pid_state::from_segment(const TrackPidSegment &segment) {
kp = segment.kp;
ki = segment.ki;
kd = segment.kd;
slew_rate_limit = segment.slew_rate_limit;
min_speed_m_s = segment.min_speed_m_s;
max_speed_m_s = segment.max_speed_m_s;
u_t_1 = clamp_commanded_speed(u_t_1);
should_use_nominal_sample_interval = true;
if (segment.fine_tune) {
fine_tune = fine_tune_state{
.band_plus = segment.fine_tune->band_plus,
.band_minus = segment.fine_tune->band_minus,
.gain_scale = segment.fine_tune->gain_scale,
};
} else {
fine_tune.reset();
}
}
void TrackPidProgramState::pid_state::reset_with(float speed_m_s) {
kp = 0.0F;
ki = 1.0F;
kd = 0.0F;
fine_tune.reset();
slew_rate_limit = 0.0F;
e_t_1 = 0.0F;
e_t_2 = 0.0F;
u_t_1 = std::max(0.0F, speed_m_s);
min_speed_m_s = 0.0F;
max_speed_m_s = u_t_1;
should_use_nominal_sample_interval = true;
}
float TrackPidProgramState::pid_state::effective_gain_scale(float target_hr, float current_hr) const {
if (!fine_tune) {
return 1.0F;
}
const auto &ft = *fine_tune;
if (current_hr > (target_hr + static_cast<float>(ft.band_plus)) ||
current_hr < (target_hr - static_cast<float>(ft.band_minus))) {
return 1.0F;
}
return ft.gain_scale;
}
float TrackPidProgramState::pid_state::clamp_commanded_speed(float speed_m_s) const {
const auto upper_speed_m_s = std::max(0.0F, max_speed_m_s);
const auto lower_speed_m_s = std::clamp(min_speed_m_s, 0.0F, upper_speed_m_s);
return std::clamp(speed_m_s, lower_speed_m_s, upper_speed_m_s);
}
void TrackPidProgramState::pid_state::apply_tuning(const TrackPidSetTuning &tuning) {
kp = tuning.kp;
ki = tuning.ki;
kd = tuning.kd;
slew_rate_limit = tuning.slew_rate_limit;
min_speed_m_s = tuning.min_speed_m_s;
max_speed_m_s = tuning.max_speed_m_s;
u_t_1 = clamp_commanded_speed(u_t_1);
if (tuning.fine_tune) {
fine_tune = fine_tune_state{
.band_plus = tuning.fine_tune->band_plus,
.band_minus = tuning.fine_tune->band_minus,
.gain_scale = tuning.fine_tune->gain_scale,
};
} else {
fine_tune.reset();
}
}
TrackPidProgramState::TrackPidProgramState(time_point now, TrackPidConfig config)
: config_(std::move(config)) {
program_start_timestamp_ = now;
pid_.reset_with(0.0F);
preemptive_enabled_ = config_.preemptive_pid_activation && validate_preemptive_schema(config_.schemas);
if (config_.schemas.empty()) {
finished_ = true;
return;
}
std::uint32_t total_duration_s = 0;
for (const auto &item : config_.schemas) {
total_duration_s += schema_duration_s(item);
}
program_end_timestamp_ = safe_add_seconds(now, total_duration_s);
apply_schema(schema());
}
float TrackPidProgramState::next(time_point now, std::optional<HrSample> fresh_hr_sample) {
if (is_finished(now)) {
finish();
return 0.0F;
}
static_cast<void>(sync_schema_for_time(now));
static_cast<void>(maybe_jump_to_final_pid(now, fresh_hr_sample));
if (is_finished(now)) {
finish();
return 0.0F;
}
return std::visit(overloads{
[&](const TrackPidSegment &) -> float {
if (!fresh_hr_sample) {
return pid_.u_t_1;
}
return do_pid(fresh_hr_sample->heart_rate_bpm, fresh_hr_sample->sample_interval_s);
},
[&](const TrackConstantSegment &constant) -> float {
pid_.u_t_1 = std::max(0.0F, constant.speed_m_s);
return pid_.u_t_1;
}},
schema().segment);
}
bool TrackPidProgramState::is_finished(time_point now) const {
return finished_ || config_.schemas.empty() || now >= program_end_timestamp_;
}
TrackPidStageKind TrackPidProgramState::active_stage_kind() const {
if (finished_ || config_.schemas.empty()) {
return TrackPidStageKind::none;
}
return schema().kind();
}
std::uint8_t TrackPidProgramState::active_stage_index() const {
if (finished_ || config_.schemas.empty()) {
return 0;
}
return static_cast<std::uint8_t>(std::min(schema_index_, static_cast<std::size_t>(std::numeric_limits<std::uint8_t>::max())));
}
float TrackPidProgramState::commanded_speed_m_s() const {
return finished_ ? 0.0F : pid_.u_t_1;
}
std::uint32_t TrackPidProgramState::remaining_program_ms(time_point now) const {
if (is_finished(now)) {
return 0;
}
const auto remaining = std::chrono::duration_cast<std::chrono::milliseconds>(program_end_timestamp_ - now);
return static_cast<std::uint32_t>(std::max<std::int64_t>(0, remaining.count()));
}
void TrackPidProgramState::update_target_hr_bpm(std::uint8_t target_hr_bpm) {
config_.target_hr_bpm = target_hr_bpm;
}
expected<unit, TrackError> TrackPidProgramState::update_tuning(const TrackPidSetTuning &tuning) {
auto valid = tuning.validate();
if (!valid) {
return unexpected<TrackError>{valid.error()};
}
if (config_.schemas.size() != 1 || !std::holds_alternative<TrackPidSegment>(config_.schemas.front().segment)) {
return unexpected<TrackError>{TrackError::invalid_state};
}
auto &segment = std::get<TrackPidSegment>(config_.schemas.front().segment);
tuning.apply_to(segment);
pid_.apply_tuning(tuning);
return {};
}
const TrackPidSchema &TrackPidProgramState::schema() const {
return config_.schemas[schema_index_];
}
std::uint16_t TrackPidProgramState::schema_duration_s(const TrackPidSchema &schema) {
return std::visit([](const auto &segment) {
return segment.duration_s;
}, schema.segment);
}
bool TrackPidProgramState::sync_schema_for_time(time_point now) {
if (forced_final_pid_ || config_.schemas.empty()) {
return false;
}
const auto target_index = resolve_schema_index(now);
if (target_index == schema_index_) {
return false;
}
advance_to_schema_index(target_index);
return true;
}
void TrackPidProgramState::advance_to_schema_index(std::size_t target_index) {
assert(target_index < config_.schemas.size() && "target schema index out of range");
while (schema_index_ < target_index) {
++schema_index_;
apply_schema(schema());
}
}
std::size_t TrackPidProgramState::resolve_schema_index(time_point now) const {
assert(!config_.schemas.empty() && "resolve_schema_index requires a non-empty program");
auto boundary = program_start_timestamp_;
for (std::size_t i = 0; i < config_.schemas.size(); ++i) {
boundary = safe_add_seconds(boundary, schema_duration_s(config_.schemas[i]));
if (now < boundary) {
return i;
}
}
return config_.schemas.size() - 1;
}
bool TrackPidProgramState::maybe_jump_to_final_pid(time_point, std::optional<HrSample> fresh_hr_sample) {
if (!preemptive_enabled_ || !fresh_hr_sample) {
return false;
}
if (active_stage_kind() == TrackPidStageKind::pid) {
return false;
}
if (!is_in_deadzone(fresh_hr_sample->heart_rate_bpm)) {
return false;
}
schema_index_ = config_.schemas.size() - 1;
apply_schema(schema());
preemptive_enabled_ = false;
forced_final_pid_ = true;
return true;
}
bool TrackPidProgramState::is_in_deadzone(float heart_rate) const {
if (config_.deadzone_bpm == 0) {
return heart_rate == static_cast<float>(config_.target_hr_bpm);
}
return heart_rate <= (config_.target_hr_bpm + config_.deadzone_bpm) &&
heart_rate >= (config_.target_hr_bpm - config_.deadzone_bpm);
}
float TrackPidProgramState::do_pid(float current_hr, float sample_interval_s) {
const auto dt_s = pid_.should_use_nominal_sample_interval
? default_pid_nominal_sample_interval_s
: clamp_hr_sample_interval_s(sample_interval_s);
pid_.should_use_nominal_sample_interval = false;
if (is_in_deadzone(current_hr)) {
return pid_.u_t_1;
}
const auto ek = static_cast<float>(config_.target_hr_bpm) - current_hr;
const auto gain_scale = pid_.effective_gain_scale(static_cast<float>(config_.target_hr_bpm), current_hr);
const auto kp = pid_.kp * gain_scale;
const auto ki = pid_.ki * gain_scale;
const auto kd = pid_.kd * gain_scale;
auto delta_u = (kp * (ek - pid_.e_t_1)) +
(ki * dt_s * ek) +
((kd / dt_s) * (ek - (2.0F * pid_.e_t_1) + pid_.e_t_2));
if (pid_.slew_rate_limit > 0.0F && std::isfinite(pid_.slew_rate_limit)) {
delta_u = std::clamp(delta_u, -pid_.slew_rate_limit, pid_.slew_rate_limit);
}
pid_.e_t_2 = pid_.e_t_1;
pid_.e_t_1 = ek;
pid_.u_t_1 = pid_.clamp_commanded_speed(pid_.u_t_1 + delta_u);
return pid_.u_t_1;
}
void TrackPidProgramState::apply_schema(const TrackPidSchema &item) {
std::visit(overloads{
[&](const TrackPidSegment &pid) {
pid_.from_segment(pid);
},
[&](const TrackConstantSegment &constant) {
pid_.reset_with(constant.speed_m_s);
}},
item.segment);
}
void TrackPidProgramState::finish() {
finished_ = true;
pid_.reset_with(0.0F);
}
bool TrackPidProgramState::validate_preemptive_schema(const std::vector<TrackPidSchema> &schemas) {
if (schemas.size() < 2) {
return false;
}
if (!std::holds_alternative<TrackConstantSegment>(schemas.front().segment)) {
return false;
}
if (!std::holds_alternative<TrackPidSegment>(schemas.back().segment)) {
return false;
}
for (std::size_t i = 0; i < schemas.size(); ++i) {
if (std::holds_alternative<TrackPidSegment>(schemas[i].segment) && i != schemas.size() - 1) {
return false;
}
}
return true;
}
TrackPidProgramState::time_point TrackPidProgramState::safe_add_seconds(time_point now, std::uint32_t duration_s) {
const auto delta = std::chrono::seconds(duration_s);
const auto candidate = now + delta;
if (candidate < now) {
return time_point::max();
}
return candidate;
}
} // namespace track_core
+242
View File
@@ -0,0 +1,242 @@
#include "track_core/render.hpp"
#include <algorithm>
#include <cassert>
#include <cmath>
namespace {
struct CircularLineDrawer {
static CircularLineDrawer from_report(const track_core::TrackConfig &config, const track_core::TrackReport &report);
[[nodiscard]]
std::uint16_t tail_offset_leds_num() const {
return static_cast<std::uint16_t>(std::round(tail_m / led_distance_m));
}
[[nodiscard]]
std::uint16_t trail_tail_to_head_leds_num() const {
return static_cast<std::uint16_t>(std::round(trail_length_m / led_distance_m));
}
[[nodiscard]]
std::uint16_t wrapped_trail_start_to_head_leds_num() const {
return static_cast<std::uint16_t>(std::round(wrapped_trail_start_to_head_length_m / led_distance_m));
}
float tail_m{};
float trail_length_m{};
float wrapped_trail_start_to_head_length_m{};
float led_distance_m{};
};
struct LinearLineDrawer {
enum Direction {
head_to_tail,
tail_to_head,
};
static LinearLineDrawer from_report(const track_core::TrackConfig &config, const track_core::TrackReport &report);
[[nodiscard]]
std::uint16_t center_offset_leds_num() const {
return static_cast<std::uint16_t>(std::round(regulated_center_m / led_distance_m));
}
[[nodiscard]]
std::uint16_t center_ahead_leds_num() const {
return static_cast<std::uint16_t>(std::round(center_ahead_m / led_distance_m));
}
[[nodiscard]]
std::uint16_t center_behind_leds_num() const {
return static_cast<std::uint16_t>(std::round(center_behind_m / led_distance_m));
}
float head_m{};
float tail_m{};
float regulated_center_m{};
float center_ahead_m{};
float center_behind_m{};
float led_distance_m{};
Direction direction{head_to_tail};
};
CircularLineDrawer CircularLineDrawer::from_report(
const track_core::TrackConfig &config,
const track_core::TrackReport &report) {
assert(config.draw_kind == track_core::TrackDrawKind::circular && "unmatched draw kind; expected circular");
float trail_length_m = 0.0F;
float wrapped_length_m = 0.0F;
constexpr auto calc_tail_m = [](float mileage_m, float line_length_m, float offset_m) -> float {
return std::fmod(mileage_m + offset_m, line_length_m);
};
const auto tail_m = calc_tail_m(report.mileage_m, config.line_length_m, config.head_offset_m);
if (tail_m < 0.0F) {
const auto start_part = config.active_line_length_m + tail_m;
if (start_part <= 0.0F) {
return {
.tail_m = 0.0F,
.trail_length_m = 0.0F,
.wrapped_trail_start_to_head_length_m = 0.0F,
.led_distance_m = config.led_distance(),
};
}
return {
.tail_m = 0.0F,
.trail_length_m = 0.0F,
.wrapped_trail_start_to_head_length_m = start_part,
.led_distance_m = config.led_distance(),
};
}
assert(config.line_length_m > config.active_line_length_m &&
"line length must be greater than active line length");
const auto wrap_head = config.line_length_m - config.active_line_length_m;
if (config.active_line_length_m < config.led_distance() || tail_m <= wrap_head) {
trail_length_m = config.active_line_length_m;
wrapped_length_m = 0.0F;
} else {
wrapped_length_m = tail_m - wrap_head;
trail_length_m = std::max(0.0F, config.active_line_length_m - wrapped_length_m);
}
return {
.tail_m = tail_m,
.trail_length_m = trail_length_m,
.wrapped_trail_start_to_head_length_m = wrapped_length_m,
.led_distance_m = config.led_distance(),
};
}
float pingpong(float x, float length) {
const float period = 2.0F * length;
float phase = std::fmod(x, period);
if (phase < 0.0F) {
phase += period;
}
return (phase <= length) ? phase : (period - phase);
}
LinearLineDrawer LinearLineDrawer::from_report(
const track_core::TrackConfig &config,
const track_core::TrackReport &report) {
assert(config.draw_kind == track_core::TrackDrawKind::linear && "unmatched draw kind; expected linear");
const float length = config.line_length_m;
const float active_length = config.active_line_length_m;
assert(length > 0.0F && "line length must be greater than 0");
assert(active_length <= length && "active line length must be less than or equal to line length");
const float center_raw = pingpong(report.mileage_m, length);
const float period = 2.0F * length;
float phase = std::fmod(report.mileage_m, period);
if (phase < 0.0F) {
phase += period;
}
const auto dir = (phase <= length) ? head_to_tail : tail_to_head;
const float head_m = center_raw + 0.5F * active_length;
const float tail_m = center_raw - 0.5F * active_length;
const float center_ahead_m = std::min(0.5F * active_length, length - center_raw);
const float center_behind_m = std::min(0.5F * active_length, center_raw);
return {
.head_m = head_m,
.tail_m = tail_m,
.regulated_center_m = center_raw,
.center_ahead_m = center_ahead_m,
.center_behind_m = center_behind_m,
.led_distance_m = config.led_distance(),
.direction = dir,
};
}
} // namespace
namespace track_core {
void TrackRenderPlan::add_fill(std::uint16_t start_led, std::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 TrackReport &report) {
TrackRenderPlan plan{};
if (!info.is_running) {
return plan;
}
switch (config.draw_kind) {
case TrackDrawKind::circular: {
const auto drawer = CircularLineDrawer::from_report(config, report);
plan.add_fill(drawer.tail_offset_leds_num(), drawer.trail_tail_to_head_leds_num(), info.color);
plan.add_fill(0, drawer.wrapped_trail_start_to_head_leds_num(), info.color);
break;
}
case TrackDrawKind::linear: {
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 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);
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);
};
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);
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);
};
const auto ahead = drawer.center_ahead_leds_num();
const auto behind = drawer.center_behind_leds_num();
switch (drawer.direction) {
case LinearLineDrawer::head_to_tail:
fill_positive_side(ahead, info.color, magic_color_ahead);
fill_negative_side(behind, info.color, magic_color_behind);
break;
case LinearLineDrawer::tail_to_head:
fill_negative_side(ahead, info.color, magic_color_ahead);
fill_positive_side(behind, info.color, magic_color_behind);
break;
}
break;
}
}
return plan;
}
} // namespace track_core
+137
View File
@@ -0,0 +1,137 @@
#include "track_core/scheme_decoder.hpp"
#include <cstring>
namespace track_core {
namespace {
constexpr std::size_t data_header_size = 3;
std::uint16_t read_u16_le(const std::uint8_t *data) {
return static_cast<std::uint16_t>(data[0]) |
static_cast<std::uint16_t>(static_cast<std::uint16_t>(data[1]) << 8U);
}
} // namespace
expected<DecodedScheme, TrackError> decode_scheme(
std::uint8_t id,
Color color,
std::span<const std::uint8_t> binary) {
if (binary.empty() || binary.size() < data_header_size) {
return unexpected<TrackError>{TrackError::invalid_size};
}
const auto kind = static_cast<SchemeKind>(binary[0]);
const auto acceleration_profile = static_cast<AccelerationProfile>(binary[1]);
const std::uint8_t segment_count = binary[2];
const auto segment_data = binary.subspan(data_header_size);
DecodedScheme scheme{
.id = id,
.color = color,
.kind = kind,
.acceleration_profile = acceleration_profile,
};
switch (kind) {
case SchemeKind::speed_input_mileage_segmented_time_free: {
constexpr std::size_t segment_size = SMSegment::raw_size;
const std::size_t expected_size = segment_count * segment_size;
if (segment_data.size() < expected_size) {
return unexpected<TrackError>{TrackError::invalid_size};
}
std::vector<SMSegment> segments;
segments.reserve(segment_count);
for (std::uint8_t i = 0; i < segment_count; ++i) {
const auto *raw = segment_data.data() + (i * segment_size);
segments.push_back(SMSegment{
.speed_m_s = static_cast<float>(raw[0]) * SMSegment::speed_lsb,
.mileage_from_start_m = read_u16_le(raw + 1),
});
}
scheme.segments = std::move(segments);
return scheme;
}
case SchemeKind::mileage_input_time_segmented_speed_free: {
constexpr std::size_t segment_size = 4;
const std::size_t expected_size = segment_count * segment_size;
if (segment_data.size() < expected_size) {
return unexpected<TrackError>{TrackError::invalid_size};
}
std::vector<MTSegment> segments;
segments.reserve(segment_count);
for (std::uint8_t i = 0; i < segment_count; ++i) {
const auto *raw = segment_data.data() + (i * segment_size);
segments.push_back(MTSegment{
.mileage_to_travel_this_segment_m = read_u16_le(raw),
.time_since_start_s = read_u16_le(raw + 2),
});
}
scheme.segments = std::move(segments);
return scheme;
}
case SchemeKind::speed_input_time_segmented_mileage_free: {
constexpr std::size_t segment_size = STSegment::raw_size;
const std::size_t expected_size = segment_count * segment_size;
if (segment_data.size() < expected_size) {
return unexpected<TrackError>{TrackError::invalid_size};
}
std::vector<STSegment> segments;
segments.reserve(segment_count);
for (std::uint8_t i = 0; i < segment_count; ++i) {
const auto *raw = segment_data.data() + (i * segment_size);
segments.push_back(STSegment{
.speed_m_s = static_cast<float>(raw[0]) * STSegment::speed_lsb,
.time_since_start_s = read_u16_le(raw + 1),
});
}
scheme.segments = std::move(segments);
return scheme;
}
case SchemeKind::repeated_speed_input_mileage_segmentation_input_time_segmented: {
std::vector<RepeatedSMSegment> time_segments;
time_segments.reserve(segment_count);
std::size_t offset = 0;
for (std::uint8_t i = 0; i < segment_count; ++i) {
if (offset + 3 > segment_data.size()) {
return unexpected<TrackError>{TrackError::invalid_size};
}
const auto time_since_start_s = read_u16_le(segment_data.data() + offset);
offset += sizeof(std::uint16_t);
const auto sub_segment_count = segment_data[offset++];
const std::size_t sub_segments_size = sub_segment_count * SMSegment::raw_size;
if (offset + sub_segments_size > segment_data.size()) {
return unexpected<TrackError>{TrackError::invalid_size};
}
std::vector<SMSegment> sub_segments;
sub_segments.reserve(sub_segment_count);
for (std::uint8_t j = 0; j < sub_segment_count; ++j) {
const auto *raw = segment_data.data() + offset;
sub_segments.push_back(SMSegment{
.speed_m_s = static_cast<float>(raw[0]) * SMSegment::speed_lsb,
.mileage_from_start_m = read_u16_le(raw + 1),
});
offset += SMSegment::raw_size;
}
time_segments.push_back(RepeatedSMSegment{
.speed_mileage_segments = std::move(sub_segments),
.time_since_start_s = time_since_start_s,
});
}
scheme.segments = std::move(time_segments);
return scheme;
}
default:
return unexpected<TrackError>{TrackError::not_supported};
}
}
} // namespace track_core
+190
View File
@@ -0,0 +1,190 @@
#include <cmath>
#include <cstdlib>
#include <iostream>
#include <vector>
#include "track_core/memory_strip.hpp"
#include "track_core/pid_program.hpp"
#include "track_core/scheme_decoder.hpp"
namespace {
void require(bool condition, const char *message) {
if (!condition) {
std::cerr << "FAIL: " << message << '\n';
std::exit(1);
}
}
void require_eq_u32(std::uint32_t actual, std::uint32_t expected, const char *message) {
if (actual != expected) {
std::cerr << "FAIL: " << message << ": actual=" << actual << " expected=" << expected << '\n';
std::exit(1);
}
}
void require_near(float actual, float expected, float tolerance, const char *message) {
if (std::fabs(actual - expected) > tolerance) {
std::cerr << "FAIL: " << message << ": actual=" << actual << " expected=" << expected << '\n';
std::exit(1);
}
}
track_core::TrackInfo running_info(track_core::Color color = track_core::Color::green()) {
return {
.kind = track_core::SchemeKind::speed_input_time_segmented_mileage_free,
.color = color,
.id = 1,
.is_running = true,
.num_segments = 1,
};
}
void test_circular_render_wraps() {
track_core::TrackConfig config{
.draw_kind = track_core::TrackDrawKind::circular,
.line_length_m = 10.0F,
.active_line_length_m = 4.0F,
.head_offset_m = 0.0F,
.line_leds_num = 20,
};
const track_core::TrackReport report{
.id = 1,
.state = track_core::TrackState::run,
.mileage_m = 8.5F,
.speed_m_s = 1.0F,
};
const auto plan = track_core::make_track_render_plan(config, running_info(), report);
require_eq_u32(static_cast<std::uint32_t>(plan.span_count), 2, "circular span count");
require_eq_u32(plan.spans[0].start_led, 17, "circular span 0 start");
require_eq_u32(plan.spans[0].led_count, 3, "circular span 0 count");
require(plan.spans[0].color == track_core::Color::green(), "circular span 0 color");
require_eq_u32(plan.spans[1].start_led, 0, "circular span 1 start");
require_eq_u32(plan.spans[1].led_count, 5, "circular span 1 count");
}
void test_linear_render_forward_and_memory_strip() {
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 track_core::TrackReport report{
.id = 2,
.state = track_core::TrackState::run,
.mileage_m = 5.0F,
.speed_m_s = 2.0F,
};
track_core::MemoryStrip strip(config.line_leds_num);
const auto plan = track_core::make_track_render_plan(config, running_info(track_core::Color::red()), report);
const auto applied = track_core::apply_render_plan(strip, plan);
require(applied == track_core::TrackError::ok, "linear plan applies");
require_eq_u32(static_cast<std::uint32_t>(plan.span_count), 4, "linear span count");
require_eq_u32(plan.spans[0].start_led, 10, "linear span 0 start");
require_eq_u32(plan.spans[0].led_count, 2, "linear span 0 count");
require(plan.spans[0].color == track_core::Color::red(), "linear span 0 color");
require_eq_u32(plan.spans[1].start_led, 12, "linear span 1 start");
require(plan.spans[1].color == track_core::Color::blue(), "linear span 1 color");
require_eq_u32(plan.spans[2].start_led, 6, "linear span 2 start");
require(plan.spans[2].color == track_core::Color::cyan(), "linear span 2 color");
require_eq_u32(plan.spans[3].start_led, 8, "linear span 3 start");
require(plan.spans[3].color == track_core::Color::red(), "linear span 3 color");
}
void test_linear_render_reverse() {
track_core::TrackConfig config{
.draw_kind = track_core::TrackDrawKind::linear,
.line_length_m = 10.0F,
.active_line_length_m = 5.0F,
.head_offset_m = 0.0F,
.line_leds_num = 20,
};
const track_core::TrackReport report{
.id = 3,
.state = track_core::TrackState::run,
.mileage_m = 15.0F,
.speed_m_s = 2.0F,
};
const auto plan = track_core::make_track_render_plan(config, running_info(track_core::Color::red()), report);
require_eq_u32(static_cast<std::uint32_t>(plan.span_count), 4, "reverse span count");
require_eq_u32(plan.spans[0].start_led, 5, "reverse span 0 start");
require_eq_u32(plan.spans[0].led_count, 2, "reverse span 0 count");
require(plan.spans[0].color == track_core::Color::blue(), "reverse span 0 color");
require_eq_u32(plan.spans[1].start_led, 7, "reverse span 1 start");
require_eq_u32(plan.spans[1].led_count, 3, "reverse span 1 count");
require(plan.spans[1].color == track_core::Color::red(), "reverse span 1 color");
require_eq_u32(plan.spans[2].start_led, 10, "reverse span 2 start");
require_eq_u32(plan.spans[2].led_count, 3, "reverse span 2 count");
require(plan.spans[2].color == track_core::Color::red(), "reverse span 2 color");
require_eq_u32(plan.spans[3].start_led, 13, "reverse span 3 start");
require_eq_u32(plan.spans[3].led_count, 2, "reverse span 3 count");
require(plan.spans[3].color == track_core::Color::cyan(), "reverse span 3 color");
}
void test_memory_strip_bounds() {
track_core::MemoryStrip strip(4);
require(strip.fill(0, 0, track_core::Color::red()) == track_core::TrackError::ok, "zero fill");
require(strip.fill(2, 8, track_core::Color::blue()) == track_core::TrackError::ok, "clamped fill");
require(strip.pixels()[2] == track_core::Color::blue(), "clamped fill pixel 2");
require(strip.pixels()[3] == track_core::Color::blue(), "clamped fill pixel 3");
require(strip.fill(4, 1, track_core::Color::red()) == track_core::TrackError::range, "out-of-range fill");
}
void test_scheme_decoder() {
const std::vector<std::uint8_t> data{
static_cast<std::uint8_t>(track_core::SchemeKind::speed_input_time_segmented_mileage_free),
static_cast<std::uint8_t>(track_core::AccelerationProfile::instant),
2,
0, 0, 0,
51, 10, 0,
};
const auto decoded = track_core::decode_scheme(7, track_core::Color::white(), data);
require(decoded.has_value(), "decode ST scheme");
require(decoded->id == 7, "decoded id");
require(decoded->kind == track_core::SchemeKind::speed_input_time_segmented_mileage_free, "decoded kind");
const auto *segments = std::get_if<std::vector<track_core::STSegment>>(&decoded->segments);
require(segments != nullptr, "decoded ST segment vector");
require_eq_u32(static_cast<std::uint32_t>(segments->size()), 2, "decoded ST segment count");
require_near((*segments)[1].speed_m_s, 2.0F, 0.01F, "decoded ST speed");
require_eq_u32((*segments)[1].time_since_start_s, 10, "decoded ST time");
}
void test_pid_program_constant() {
track_core::TrackPidConfig config;
config.schemas = {
track_core::TrackPidSchema{track_core::TrackConstantSegment{
.duration_s = 2,
.speed_m_s = 1.5F,
}},
};
const auto start = track_core::clock::time_point(std::chrono::seconds(0));
track_core::TrackPidProgramState state(start, config);
require_near(state.next(start + std::chrono::seconds(1), std::nullopt), 1.5F, 0.0001F, "constant PID speed");
require(!state.is_finished(start + std::chrono::seconds(1)), "constant PID not finished");
require(state.is_finished(start + std::chrono::seconds(2)), "constant PID finished");
}
} // namespace
int main() {
test_circular_render_wraps();
test_linear_render_forward_and_memory_strip();
test_linear_render_reverse();
test_memory_strip_bounds();
test_scheme_decoder();
test_pid_program_constant();
std::cout << "track-core tests passed\n";
return 0;
}