Initial standalone track core
This commit is contained in:
@@ -0,0 +1,3 @@
|
||||
build/
|
||||
.DS_Store
|
||||
compile_commands.json
|
||||
@@ -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()
|
||||
@@ -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.
|
||||
@@ -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";
|
||||
}
|
||||
}
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
@@ -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
|
||||
@@ -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
@@ -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
|
||||
@@ -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
|
||||
@@ -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;
|
||||
}
|
||||
Reference in New Issue
Block a user