feat(track-core): add portable training runtimes
Move scheme and PID training runtime behavior into the pure track_core layer and expose render sinks for injected strip application. Add ESP compatibility adapters, Python bindings/test scaffolding, and in-memory render support so app_track_bt can consume core render and runtime logic without duplicating it. Cover circular/linear rendering boundaries, all scheme runtime types, scheme render_to parity, PID sample de-duplication, speed suppression, and live tuning in track-core tests.
This commit is contained in:
@@ -0,0 +1,85 @@
|
||||
#include "app_track_decoder.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <cstdint>
|
||||
|
||||
namespace app::track {
|
||||
namespace {
|
||||
|
||||
track_core::Color to_core(const Color &color) {
|
||||
return {
|
||||
color.inner.r,
|
||||
color.inner.g,
|
||||
color.inner.b,
|
||||
};
|
||||
}
|
||||
|
||||
error_t from_core(track_core::TrackError error) {
|
||||
switch (error) {
|
||||
case track_core::TrackError::ok:
|
||||
return ESP_OK;
|
||||
case track_core::TrackError::invalid_arg:
|
||||
return ESP_ERR_INVALID_ARG;
|
||||
case track_core::TrackError::invalid_size:
|
||||
return ESP_ERR_INVALID_SIZE;
|
||||
case track_core::TrackError::invalid_state:
|
||||
return ESP_ERR_INVALID_STATE;
|
||||
case track_core::TrackError::not_supported:
|
||||
return ESP_ERR_NOT_SUPPORTED;
|
||||
case track_core::TrackError::range:
|
||||
return ESP_ERR_INVALID_SIZE;
|
||||
}
|
||||
return ESP_FAIL;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
TrackSchemeDecoder TrackSchemeDecoder::from_proto(const proto_type &proto) {
|
||||
TrackSchemeDecoder scheme;
|
||||
scheme.id = proto.id;
|
||||
if (proto.has_color) {
|
||||
scheme.color = Color::from_proto(proto.color);
|
||||
} else {
|
||||
scheme.color = Color::white();
|
||||
}
|
||||
auto data = std::span<const uint8_t>(proto.data.bytes, proto.data.size);
|
||||
std::ranges::copy(data, std::back_inserter(scheme.binary));
|
||||
return scheme;
|
||||
}
|
||||
|
||||
expected<track_core::DecodedScheme, error_t> TrackSchemeDecoder::decode_core() const {
|
||||
using ue = unexpected<error_t>;
|
||||
if (binary.empty()) {
|
||||
return ue{ESP_ERR_INVALID_ARG};
|
||||
}
|
||||
|
||||
const auto decoded = track_core::decode_scheme(id, to_core(color), binary);
|
||||
if (!decoded) {
|
||||
return ue{from_core(decoded.error())};
|
||||
}
|
||||
return *decoded;
|
||||
}
|
||||
|
||||
TrackSchemeMgr::Add TrackSchemeMgr::Add::from_proto(const proto_type &proto) {
|
||||
Add add;
|
||||
assert(proto.has_scheme);
|
||||
add.scheme_decoder = TrackSchemeDecoder::from_proto(proto.scheme);
|
||||
return add;
|
||||
}
|
||||
|
||||
TrackSchemeMgr TrackSchemeMgr::from_proto(const proto_type &proto) {
|
||||
TrackSchemeMgr mgmt;
|
||||
switch (proto.which_msg) {
|
||||
case track_app_TrackSchemeMgr_add_tag:
|
||||
mgmt.choice = Add::from_proto(proto.msg.add);
|
||||
break;
|
||||
case track_app_TrackSchemeMgr_clear_tag:
|
||||
mgmt.choice = Clear{};
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
return mgmt;
|
||||
}
|
||||
|
||||
} // namespace app::track
|
||||
@@ -0,0 +1,179 @@
|
||||
#include "app_track_drawer.hpp"
|
||||
|
||||
#include <cassert>
|
||||
#include <cstddef>
|
||||
|
||||
#include "track_core/render.hpp"
|
||||
|
||||
namespace {
|
||||
|
||||
track_core::Color to_core(const app::track::Color &color) {
|
||||
return {
|
||||
color.inner.r,
|
||||
color.inner.g,
|
||||
color.inner.b,
|
||||
};
|
||||
}
|
||||
|
||||
app::track::Color from_core(const track_core::Color &color) {
|
||||
return {
|
||||
color.r,
|
||||
color.g,
|
||||
color.b,
|
||||
};
|
||||
}
|
||||
|
||||
track_core::TrackDrawKind to_core(track_app_TrackDrawKind draw_kind) {
|
||||
switch (draw_kind) {
|
||||
case track_app_TrackDrawKind_CIRCULAR:
|
||||
return track_core::TrackDrawKind::circular;
|
||||
case track_app_TrackDrawKind_LINEAR:
|
||||
return track_core::TrackDrawKind::linear;
|
||||
}
|
||||
return track_core::TrackDrawKind::circular;
|
||||
}
|
||||
|
||||
track_core::SchemeKind to_core(app::track::SchemeKind kind) {
|
||||
switch (kind) {
|
||||
case track_app_TrackSchemeKind_SPEED_INPUT_MILEAGE_SEGMENTED_TIME_FREE:
|
||||
return track_core::SchemeKind::speed_input_mileage_segmented_time_free;
|
||||
case track_app_TrackSchemeKind_MILEAGE_INPUT_TIME_SEGMENTED_SPEED_FREE:
|
||||
return track_core::SchemeKind::mileage_input_time_segmented_speed_free;
|
||||
case track_app_TrackSchemeKind_SPEED_INPUT_TIME_SEGMENTED_MILEAGE_FREE:
|
||||
return track_core::SchemeKind::speed_input_time_segmented_mileage_free;
|
||||
case track_app_TrackSchemeKind_REPEATED_SPEED_INPUT_MILEAGE_SEGMENTATION_INPUT_TIME_SEGMENTED:
|
||||
return track_core::SchemeKind::repeated_speed_input_mileage_segmentation_input_time_segmented;
|
||||
}
|
||||
return track_core::SchemeKind::speed_input_time_segmented_mileage_free;
|
||||
}
|
||||
|
||||
track_core::TrackState to_core(app::track::TrackState state) {
|
||||
switch (state) {
|
||||
case track_app_TrackState_STOP:
|
||||
return track_core::TrackState::stop;
|
||||
case track_app_TrackState_RUN:
|
||||
return track_core::TrackState::run;
|
||||
case track_app_TrackState_TEST_RAINBOW:
|
||||
return track_core::TrackState::test_rainbow;
|
||||
case track_app_TrackState_TEST_BLINK:
|
||||
return track_core::TrackState::test_blink;
|
||||
}
|
||||
return track_core::TrackState::stop;
|
||||
}
|
||||
|
||||
track_core::TrackConfig to_core(const app::track::TrackConfig &config) {
|
||||
return {
|
||||
.draw_kind = to_core(config.draw_kind),
|
||||
.line_length_m = config.line_length_m,
|
||||
.active_line_length_m = config.active_line_length_m,
|
||||
.head_offset_m = config.head_offset_m,
|
||||
.line_leds_num = config.line_leds_num,
|
||||
};
|
||||
}
|
||||
|
||||
track_core::TrackInfo to_core(const app::track::TrackInfo &info) {
|
||||
return {
|
||||
.kind = to_core(info.kind),
|
||||
.color = to_core(info.color),
|
||||
.id = info.id,
|
||||
.is_running = info.is_running,
|
||||
.num_segments = info.num_segments,
|
||||
};
|
||||
}
|
||||
|
||||
track_core::TrackReport to_core(const app::track::report &report) {
|
||||
return {
|
||||
.id = report.id,
|
||||
.state = to_core(report.state),
|
||||
.mileage_m = report.mileage_m,
|
||||
.speed_m_s = report.speed_m_s,
|
||||
.time_elapsed_ms = report.time_elapsed_ms,
|
||||
};
|
||||
}
|
||||
|
||||
track_core::TrackError to_core(error_t error) {
|
||||
if (error == ESP_OK) {
|
||||
return track_core::TrackError::ok;
|
||||
}
|
||||
return track_core::TrackError::invalid_state;
|
||||
}
|
||||
|
||||
app::strip::StripView *strip_from_context(void *context) {
|
||||
return static_cast<app::strip::StripView *>(context);
|
||||
}
|
||||
|
||||
track_core::TrackError strip_clear(void *context) {
|
||||
auto *strip = strip_from_context(context);
|
||||
if (strip == nullptr) {
|
||||
return track_core::TrackError::invalid_arg;
|
||||
}
|
||||
return to_core((*strip)->clear());
|
||||
}
|
||||
|
||||
track_core::TrackError strip_fill(
|
||||
void *context,
|
||||
std::uint16_t start_led,
|
||||
std::uint16_t led_count,
|
||||
track_core::Color color) {
|
||||
auto *strip = strip_from_context(context);
|
||||
if (strip == nullptr) {
|
||||
return track_core::TrackError::invalid_arg;
|
||||
}
|
||||
return to_core((*strip)->fill(start_led, led_count, static_cast<std::uint32_t>(from_core(color))));
|
||||
}
|
||||
|
||||
track_core::TrackError strip_show(void *context) {
|
||||
auto *strip = strip_from_context(context);
|
||||
if (strip == nullptr) {
|
||||
return track_core::TrackError::invalid_arg;
|
||||
}
|
||||
return to_core((*strip)->show());
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
namespace app::track {
|
||||
|
||||
void TrackRenderPlan::add_fill(uint16_t start_led, uint16_t led_count, Color color) {
|
||||
if (led_count == 0) {
|
||||
return;
|
||||
}
|
||||
assert(span_count < spans.size() && "TrackRenderPlan capacity exceeded");
|
||||
spans[span_count++] = TrackRenderSpan{
|
||||
.start_led = start_led,
|
||||
.led_count = led_count,
|
||||
.color = color,
|
||||
};
|
||||
}
|
||||
|
||||
TrackRenderPlan make_track_render_plan(const TrackConfig &config, const TrackInfo &info, const report &rep) {
|
||||
const auto core_plan = track_core::make_track_render_plan(
|
||||
to_core(config),
|
||||
to_core(info),
|
||||
to_core(rep));
|
||||
|
||||
TrackRenderPlan plan{};
|
||||
for (size_t i = 0; i < core_plan.span_count; ++i) {
|
||||
const auto &span = core_plan.spans[i];
|
||||
plan.add_fill(span.start_led, span.led_count, from_core(span.color));
|
||||
}
|
||||
return plan;
|
||||
}
|
||||
|
||||
void apply_render_plan(app::strip::StripView strip, const TrackRenderPlan &plan) {
|
||||
for (size_t i = 0; i < plan.span_count; ++i) {
|
||||
const auto &span = plan.spans[i];
|
||||
strip->fill(span.start_led, span.led_count, span.color);
|
||||
}
|
||||
}
|
||||
|
||||
track_core::TrackRenderSink make_track_render_sink(app::strip::StripView &strip) {
|
||||
return track_core::TrackRenderSink{
|
||||
.context = &strip,
|
||||
.clear = strip_clear,
|
||||
.fill = strip_fill,
|
||||
.show = strip_show,
|
||||
};
|
||||
}
|
||||
|
||||
} // namespace app::track
|
||||
@@ -0,0 +1,109 @@
|
||||
#include "app_track_model.hpp"
|
||||
#include "app_track.pb.h"
|
||||
#include "esp_log.h"
|
||||
|
||||
namespace {
|
||||
app::track::config_getter global_config_getter{nullptr};
|
||||
constexpr auto TAG = "app::track";
|
||||
}
|
||||
|
||||
namespace app::track {
|
||||
const char *to_str(TrackState status) {
|
||||
using enum TrackState;
|
||||
switch (status) {
|
||||
case track_app_TrackState_STOP:
|
||||
return "STOP";
|
||||
case track_app_TrackState_RUN:
|
||||
return "RUN";
|
||||
case track_app_TrackState_TEST_RAINBOW:
|
||||
return "TEST_RAINBOW";
|
||||
case track_app_TrackState_TEST_BLINK:
|
||||
return "TEST_BLINK";
|
||||
}
|
||||
return "UNKNOWN";
|
||||
}
|
||||
|
||||
const char *to_str(TrackControllerMode mode) {
|
||||
switch (mode) {
|
||||
case track_app_TrackControllerMode_SCHEME:
|
||||
return "SCHEME";
|
||||
case track_app_TrackControllerMode_PID_HR:
|
||||
return "PID_HR";
|
||||
}
|
||||
return "UNKNOWN";
|
||||
}
|
||||
|
||||
const char *to_str(TrackPidStageKind kind) {
|
||||
switch (kind) {
|
||||
case track_app_TrackPidStageKind_NONE:
|
||||
return "NONE";
|
||||
case track_app_TrackPidStageKind_CONSTANT:
|
||||
return "CONSTANT";
|
||||
case track_app_TrackPidStageKind_PID:
|
||||
return "PID";
|
||||
}
|
||||
return "UNKNOWN";
|
||||
}
|
||||
|
||||
const char *to_str(track_app_TrackDrawKind draw_kind) {
|
||||
switch (draw_kind) {
|
||||
case track_app_TrackDrawKind_CIRCULAR:
|
||||
return "CIRCULAR";
|
||||
case track_app_TrackDrawKind_LINEAR:
|
||||
return "LINEAR";
|
||||
}
|
||||
return "UNKNOWN";
|
||||
}
|
||||
|
||||
const char *to_str(AccelerationProfile profile) {
|
||||
switch (profile) {
|
||||
case track_app_TrackAccelerationProfile_SMOOTH:
|
||||
return "SMOOTH";
|
||||
case track_app_TrackAccelerationProfile_INSTANT:
|
||||
return "INSTANT";
|
||||
}
|
||||
return "UNKNOWN";
|
||||
}
|
||||
|
||||
void set_global_config_getter(config_getter getter) {
|
||||
global_config_getter = std::move(getter);
|
||||
}
|
||||
|
||||
const TrackConfig &global_config() {
|
||||
if (not global_config_getter) {
|
||||
static const auto DEFAULT_CONFIG = TrackConfig::Default();
|
||||
ESP_LOGW(TAG, "unset global config getter");
|
||||
return DEFAULT_CONFIG;
|
||||
}
|
||||
return global_config_getter();
|
||||
}
|
||||
|
||||
void TrackConfig::log(const char *tag, esp_log_level_t level) const {
|
||||
ESP_LOG_LEVEL(level, tag,
|
||||
"TrackConfig{.draw_kind=%s, "
|
||||
".line_length_m=%.2f, "
|
||||
".active_line_length_m=%.2f, "
|
||||
".head_offset_m=%.2f, "
|
||||
".line_leds_num=%" PRIu16
|
||||
"}",
|
||||
to_str(draw_kind),
|
||||
line_length_m,
|
||||
active_line_length_m,
|
||||
head_offset_m,
|
||||
line_leds_num);
|
||||
}
|
||||
|
||||
void TrackStateReportCollection::log(const char *tag, esp_log_level_t level) const {
|
||||
for (size_t i = 0; i < states.size(); ++i) {
|
||||
const auto &report = states[i];
|
||||
ESP_LOG_LEVEL(level, tag,
|
||||
"[%zu] Report{.id=%" PRIu8 ", .state=%s, .mileage_m=%.2f, .speed_m_s=%.2f, .time_elapsed_ms=%" PRIu32 "}",
|
||||
i,
|
||||
report.id,
|
||||
to_str(report.state),
|
||||
report.mileage_m,
|
||||
report.speed_m_s,
|
||||
report.time_elapsed_ms);
|
||||
}
|
||||
}
|
||||
}
|
||||
+45
-7
@@ -3,6 +3,41 @@
|
||||
#include <algorithm>
|
||||
|
||||
namespace track_core {
|
||||
namespace {
|
||||
|
||||
MemoryStrip *memory_strip_from_context(void *context) {
|
||||
return static_cast<MemoryStrip *>(context);
|
||||
}
|
||||
|
||||
TrackError memory_strip_clear(void *context) {
|
||||
auto *strip = memory_strip_from_context(context);
|
||||
if (strip == nullptr) {
|
||||
return TrackError::invalid_arg;
|
||||
}
|
||||
return strip->clear();
|
||||
}
|
||||
|
||||
TrackError memory_strip_fill(
|
||||
void *context,
|
||||
std::uint16_t start_led,
|
||||
std::uint16_t led_count,
|
||||
Color color) {
|
||||
auto *strip = memory_strip_from_context(context);
|
||||
if (strip == nullptr) {
|
||||
return TrackError::invalid_arg;
|
||||
}
|
||||
return strip->fill(start_led, led_count, color);
|
||||
}
|
||||
|
||||
TrackError memory_strip_show(void *context) {
|
||||
auto *strip = memory_strip_from_context(context);
|
||||
if (strip == nullptr) {
|
||||
return TrackError::invalid_arg;
|
||||
}
|
||||
return strip->show();
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
MemoryStrip::MemoryStrip(std::size_t led_count)
|
||||
: pixels_(led_count, Color::black()) {}
|
||||
@@ -60,13 +95,16 @@ std::span<const Color> MemoryStrip::pixels() const {
|
||||
}
|
||||
|
||||
TrackError apply_render_plan(MemoryStrip &strip, const TrackRenderPlan &plan) {
|
||||
for (std::size_t i = 0; i < plan.span_count; ++i) {
|
||||
const auto &span = plan.spans[i];
|
||||
if (const auto err = strip.fill(span.start_led, span.led_count, span.color); err != TrackError::ok) {
|
||||
return err;
|
||||
}
|
||||
}
|
||||
return TrackError::ok;
|
||||
return apply_render_plan(make_memory_strip_sink(strip), plan);
|
||||
}
|
||||
|
||||
TrackRenderSink make_memory_strip_sink(MemoryStrip &strip) {
|
||||
return TrackRenderSink{
|
||||
.context = &strip,
|
||||
.clear = memory_strip_clear,
|
||||
.fill = memory_strip_fill,
|
||||
.show = memory_strip_show,
|
||||
};
|
||||
}
|
||||
|
||||
} // namespace track_core
|
||||
|
||||
@@ -0,0 +1,207 @@
|
||||
#include "track_core/pid_runtime.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <chrono>
|
||||
#include <cmath>
|
||||
#include <limits>
|
||||
|
||||
namespace track_core {
|
||||
namespace {
|
||||
|
||||
constexpr float default_pid_nominal_sample_interval_s = 2.0F;
|
||||
|
||||
template <class... Ts>
|
||||
struct overloads : Ts... {
|
||||
using Ts::operator()...;
|
||||
};
|
||||
|
||||
bool supports_live_pid_tuning(const TrackPidConfig &config) {
|
||||
return config.schemas.size() == 1 &&
|
||||
std::holds_alternative<TrackPidSegment>(config.schemas.front().segment);
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
PidHrRuntime::PidHrRuntime() = default;
|
||||
|
||||
PidHrRuntime::PidHrRuntime(TrackPidConfig config)
|
||||
: config_(std::move(config)) {}
|
||||
|
||||
void PidHrRuntime::set_pid_config(TrackPidConfig config) {
|
||||
config_ = std::move(config);
|
||||
}
|
||||
|
||||
void PidHrRuntime::update_target_hr_bpm(std::uint8_t target_hr_bpm) {
|
||||
config_.target_hr_bpm = target_hr_bpm;
|
||||
if (program_state_) {
|
||||
program_state_->update_target_hr_bpm(target_hr_bpm);
|
||||
}
|
||||
}
|
||||
|
||||
expected<unit, TrackError> PidHrRuntime::apply_pid_runtime_command(
|
||||
const TrackPidRuntimeCommand &command,
|
||||
const TrackConfig *track_config) {
|
||||
if (!running_ || !program_state_) {
|
||||
return unexpected<TrackError>{TrackError::invalid_state};
|
||||
}
|
||||
|
||||
return std::visit(overloads{
|
||||
[](unit) -> expected<unit, TrackError> {
|
||||
return unexpected<TrackError>{TrackError::invalid_arg};
|
||||
},
|
||||
[&](const TrackPidSetTargetHr &target_hr) -> expected<unit, TrackError> {
|
||||
update_target_hr_bpm(target_hr.target_hr_bpm);
|
||||
return {};
|
||||
},
|
||||
[&](const TrackPidSetTuning &tuning) -> expected<unit, TrackError> {
|
||||
if (!supports_live_pid_tuning(config_)) {
|
||||
return unexpected<TrackError>{TrackError::invalid_state};
|
||||
}
|
||||
auto updated = program_state_->update_tuning(tuning);
|
||||
if (!updated) {
|
||||
return unexpected<TrackError>{updated.error()};
|
||||
}
|
||||
auto &segment = std::get<TrackPidSegment>(config_.schemas.front().segment);
|
||||
tuning.apply_to(segment);
|
||||
base_speed_m_s_ = program_state_->commanded_speed_m_s();
|
||||
effective_speed_m_s_ = effective_speed_m_s(base_speed_m_s_, track_config);
|
||||
return {};
|
||||
}},
|
||||
command.command);
|
||||
}
|
||||
|
||||
const TrackPidConfig &PidHrRuntime::pid_config() const {
|
||||
return config_;
|
||||
}
|
||||
|
||||
void PidHrRuntime::start(time_point now) {
|
||||
running_ = true;
|
||||
mileage_m_ = 0.0F;
|
||||
base_speed_m_s_ = 0.0F;
|
||||
effective_speed_m_s_ = 0.0F;
|
||||
start_timestamp_ = now;
|
||||
last_tick_timestamp_ = now;
|
||||
last_consumed_hr_sample_seq_.reset();
|
||||
last_consumed_hr_sample_time_ = {};
|
||||
program_state_ = std::make_unique<TrackPidProgramState>(now, config_);
|
||||
}
|
||||
|
||||
void PidHrRuntime::stop() {
|
||||
running_ = false;
|
||||
base_speed_m_s_ = 0.0F;
|
||||
effective_speed_m_s_ = 0.0F;
|
||||
last_consumed_hr_sample_seq_.reset();
|
||||
last_consumed_hr_sample_time_ = {};
|
||||
program_state_.reset();
|
||||
}
|
||||
|
||||
void PidHrRuntime::tick(
|
||||
const TrackConfig *track_config,
|
||||
const TrackPidBandSnapshot &band,
|
||||
time_point now) {
|
||||
if (!running_ || !program_state_) {
|
||||
return;
|
||||
}
|
||||
|
||||
const auto dt = std::chrono::duration_cast<std::chrono::duration<float>>(now - last_tick_timestamp_);
|
||||
last_tick_timestamp_ = now;
|
||||
if (dt.count() <= 0.0F) {
|
||||
return;
|
||||
}
|
||||
|
||||
std::optional<TrackPidProgramState::HrSample> fresh_hr;
|
||||
if (band.hr_is_fresh && band.has_heart_rate &&
|
||||
(!last_consumed_hr_sample_seq_.has_value() ||
|
||||
band.heart_rate_sample_seq != *last_consumed_hr_sample_seq_)) {
|
||||
auto sample_interval_s = default_pid_nominal_sample_interval_s;
|
||||
if (last_consumed_hr_sample_seq_.has_value()) {
|
||||
sample_interval_s = std::chrono::duration_cast<std::chrono::duration<float>>(
|
||||
now - last_consumed_hr_sample_time_)
|
||||
.count();
|
||||
}
|
||||
fresh_hr = TrackPidProgramState::HrSample{
|
||||
.heart_rate_bpm = static_cast<float>(band.heart_rate),
|
||||
.sample_interval_s = sample_interval_s,
|
||||
};
|
||||
last_consumed_hr_sample_seq_ = band.heart_rate_sample_seq;
|
||||
last_consumed_hr_sample_time_ = now;
|
||||
}
|
||||
|
||||
base_speed_m_s_ = program_state_->next(now, fresh_hr);
|
||||
effective_speed_m_s_ = effective_speed_m_s(base_speed_m_s_, track_config);
|
||||
if (program_state_->is_finished(now)) {
|
||||
base_speed_m_s_ = 0.0F;
|
||||
effective_speed_m_s_ = 0.0F;
|
||||
running_ = false;
|
||||
}
|
||||
|
||||
mileage_m_ += effective_speed_m_s_ * dt.count();
|
||||
}
|
||||
|
||||
float PidHrRuntime::effective_speed_m_s(float base_speed_m_s, const TrackConfig *track_config) const {
|
||||
auto effective_speed = std::max(0.0F, base_speed_m_s);
|
||||
if (!config_.speed_suppression || effective_speed <= 0.0F || track_config == nullptr) {
|
||||
return effective_speed;
|
||||
}
|
||||
|
||||
if (!std::isfinite(track_config->line_length_m) || track_config->line_length_m <= 0.0F) {
|
||||
return effective_speed;
|
||||
}
|
||||
|
||||
const auto &suppression = *config_.speed_suppression;
|
||||
if (!std::isfinite(suppression.sigma_m) || suppression.sigma_m <= 0.0F) {
|
||||
return effective_speed;
|
||||
}
|
||||
|
||||
const auto phase_raw_m = std::fmod(mileage_m_, track_config->line_length_m);
|
||||
const auto phase_m = phase_raw_m < 0.0F ? (phase_raw_m + track_config->line_length_m) : phase_raw_m;
|
||||
const auto seam_distance_m = std::min(phase_m, track_config->line_length_m - phase_m);
|
||||
const auto sigma_ratio = seam_distance_m / suppression.sigma_m;
|
||||
const auto gaussian = std::exp(-0.5F * sigma_ratio * sigma_ratio);
|
||||
const auto speed_ratio = 1.0F - ((1.0F - suppression.ratio_min) * gaussian);
|
||||
return effective_speed * speed_ratio;
|
||||
}
|
||||
|
||||
TrackReport PidHrRuntime::state_report(time_point now) const {
|
||||
return {
|
||||
.id = magic_pid_track_id,
|
||||
.state = running_ ? TrackState::run : TrackState::stop,
|
||||
.mileage_m = mileage_m_,
|
||||
.speed_m_s = effective_speed_m_s_,
|
||||
.time_elapsed_ms = static_cast<std::uint32_t>(std::chrono::duration_cast<std::chrono::milliseconds>(
|
||||
now - start_timestamp_)
|
||||
.count()),
|
||||
};
|
||||
}
|
||||
|
||||
TrackInfo PidHrRuntime::info() const {
|
||||
return {
|
||||
.kind = SchemeKind::speed_input_time_segmented_mileage_free,
|
||||
.color = color_,
|
||||
.id = magic_pid_track_id,
|
||||
.is_running = running_,
|
||||
.num_segments = static_cast<std::uint8_t>(
|
||||
std::min(config_.schemas.size(), static_cast<std::size_t>(std::numeric_limits<std::uint8_t>::max()))),
|
||||
};
|
||||
}
|
||||
|
||||
TrackPidStatus PidHrRuntime::pid_status(const TrackPidBandSnapshot &band, time_point now) const {
|
||||
TrackPidStatus status;
|
||||
status.band_id = config_.band_id;
|
||||
status.band_is_active = band.band_is_active;
|
||||
status.is_heart_rate_valid = band.hr_is_fresh;
|
||||
status.heart_rate_bpm = band.has_heart_rate ? band.heart_rate : static_cast<std::uint8_t>(0);
|
||||
status.step_count = band.has_step_count ? band.step_count : static_cast<std::uint16_t>(0);
|
||||
status.effective_speed_m_s = effective_speed_m_s_;
|
||||
status.base_speed_m_s = base_speed_m_s_;
|
||||
if (program_state_) {
|
||||
status.active_segment_index = program_state_->active_stage_index();
|
||||
status.active_segment_kind = program_state_->active_stage_kind();
|
||||
status.remaining_program_ms = program_state_->remaining_program_ms(now);
|
||||
} else {
|
||||
status.active_segment_kind = TrackPidStageKind::none;
|
||||
}
|
||||
return status;
|
||||
}
|
||||
|
||||
} // namespace track_core
|
||||
@@ -0,0 +1,310 @@
|
||||
#include <cstdint>
|
||||
#include <stdexcept>
|
||||
#include <string>
|
||||
|
||||
#include <nanobind/nanobind.h>
|
||||
#include <nanobind/stl/string.h>
|
||||
#include <nanobind/stl/vector.h>
|
||||
|
||||
#include "track_core/memory_strip.hpp"
|
||||
#include "track_core/render.hpp"
|
||||
#include "track_core/scheme_runtime.hpp"
|
||||
|
||||
namespace nb = nanobind;
|
||||
using namespace nb::literals;
|
||||
|
||||
namespace {
|
||||
|
||||
void validate_render_config(const track_core::TrackConfig &config) {
|
||||
if (!config.verify() || config.line_leds_num == 0) {
|
||||
throw nb::value_error("invalid TrackConfig");
|
||||
}
|
||||
if (config.draw_kind == track_core::TrackDrawKind::circular &&
|
||||
config.active_line_length_m >= config.line_length_m) {
|
||||
throw nb::value_error("circular tracks require active_line_length_m < line_length_m");
|
||||
}
|
||||
}
|
||||
|
||||
nb::list colors_to_list(std::span<const track_core::Color> colors) {
|
||||
nb::list result;
|
||||
for (const auto &color : colors) {
|
||||
result.append(color);
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
nb::list spans_to_list(const track_core::TrackRenderPlan &plan) {
|
||||
nb::list result;
|
||||
for (std::size_t i = 0; i < plan.span_count; ++i) {
|
||||
result.append(plan.spans[i]);
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
track_core::TrackRenderPlan make_render_plan_checked(
|
||||
const track_core::TrackConfig &config,
|
||||
const track_core::TrackInfo &info,
|
||||
const track_core::TrackReport &report) {
|
||||
validate_render_config(config);
|
||||
return track_core::make_track_render_plan(config, info, report);
|
||||
}
|
||||
|
||||
track_core::SchemeTrackRuntime make_scheme_track_runtime_checked(track_core::DecodedScheme scheme) {
|
||||
auto result = track_core::make_scheme_track_runtime(std::move(scheme));
|
||||
if (!result) {
|
||||
throw std::runtime_error("invalid scheme");
|
||||
}
|
||||
return std::move(*result);
|
||||
}
|
||||
|
||||
void add_scheme_checked(track_core::SchemeTrainingRuntime &runtime, track_core::DecodedScheme scheme) {
|
||||
const auto result = runtime.add_scheme(std::move(scheme));
|
||||
if (!result) {
|
||||
throw std::runtime_error("invalid scheme");
|
||||
}
|
||||
}
|
||||
|
||||
nb::list runtime_state_collection(const track_core::SchemeTrainingRuntime &runtime) {
|
||||
nb::list result;
|
||||
for (const auto &report : runtime.state_collection().states) {
|
||||
result.append(report);
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
nb::list runtime_scheme_status(const track_core::SchemeTrainingRuntime &runtime) {
|
||||
nb::list result;
|
||||
for (const auto &status : runtime.scheme_status().scheme_status) {
|
||||
result.append(status);
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
nb::list runtime_render_pixels(
|
||||
const track_core::SchemeTrainingRuntime &runtime,
|
||||
const track_core::TrackConfig &config) {
|
||||
const auto pixels = runtime.render_pixels(config);
|
||||
if (!pixels) {
|
||||
throw std::runtime_error("failed to render runtime pixels");
|
||||
}
|
||||
return colors_to_list(*pixels);
|
||||
}
|
||||
|
||||
nb::list render_pixels(
|
||||
const track_core::TrackConfig &config,
|
||||
const track_core::TrackInfo &info,
|
||||
const track_core::TrackReport &report) {
|
||||
validate_render_config(config);
|
||||
track_core::MemoryStrip strip(config.line_leds_num);
|
||||
const auto plan = track_core::make_track_render_plan(config, info, report);
|
||||
const auto err = track_core::apply_render_plan(strip, plan);
|
||||
if (err != track_core::TrackError::ok) {
|
||||
throw std::runtime_error("failed to apply render plan");
|
||||
}
|
||||
return colors_to_list(strip.pixels());
|
||||
}
|
||||
|
||||
std::string color_repr(const track_core::Color &color) {
|
||||
return "Color(" + std::to_string(color.r) + ", " +
|
||||
std::to_string(color.g) + ", " +
|
||||
std::to_string(color.b) + ")";
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
NB_MODULE(_core, m) {
|
||||
m.doc() = "Python bindings for the platform-neutral track-core library";
|
||||
|
||||
nb::enum_<track_core::TrackError>(m, "TrackError")
|
||||
.value("ok", track_core::TrackError::ok)
|
||||
.value("invalid_arg", track_core::TrackError::invalid_arg)
|
||||
.value("invalid_size", track_core::TrackError::invalid_size)
|
||||
.value("invalid_state", track_core::TrackError::invalid_state)
|
||||
.value("not_supported", track_core::TrackError::not_supported)
|
||||
.value("range", track_core::TrackError::range);
|
||||
|
||||
nb::enum_<track_core::TrackDrawKind>(m, "TrackDrawKind")
|
||||
.value("circular", track_core::TrackDrawKind::circular)
|
||||
.value("linear", track_core::TrackDrawKind::linear);
|
||||
|
||||
nb::enum_<track_core::SchemeKind>(m, "SchemeKind")
|
||||
.value("speed_input_mileage_segmented_time_free",
|
||||
track_core::SchemeKind::speed_input_mileage_segmented_time_free)
|
||||
.value("mileage_input_time_segmented_speed_free",
|
||||
track_core::SchemeKind::mileage_input_time_segmented_speed_free)
|
||||
.value("speed_input_time_segmented_mileage_free",
|
||||
track_core::SchemeKind::speed_input_time_segmented_mileage_free)
|
||||
.value("repeated_speed_input_mileage_segmentation_input_time_segmented",
|
||||
track_core::SchemeKind::repeated_speed_input_mileage_segmentation_input_time_segmented);
|
||||
|
||||
nb::enum_<track_core::AccelerationProfile>(m, "AccelerationProfile")
|
||||
.value("instant", track_core::AccelerationProfile::instant)
|
||||
.value("smooth", track_core::AccelerationProfile::smooth);
|
||||
|
||||
nb::enum_<track_core::TrackState>(m, "TrackState")
|
||||
.value("stop", track_core::TrackState::stop)
|
||||
.value("run", track_core::TrackState::run)
|
||||
.value("test_rainbow", track_core::TrackState::test_rainbow)
|
||||
.value("test_blink", track_core::TrackState::test_blink);
|
||||
|
||||
nb::class_<track_core::Color>(m, "Color")
|
||||
.def(nb::init<>())
|
||||
.def(nb::init<std::uint8_t, std::uint8_t, std::uint8_t>(), "r"_a, "g"_a, "b"_a)
|
||||
.def(nb::init<std::uint32_t>(), "value"_a)
|
||||
.def_rw("r", &track_core::Color::r)
|
||||
.def_rw("g", &track_core::Color::g)
|
||||
.def_rw("b", &track_core::Color::b)
|
||||
.def("hex", &track_core::Color::hex)
|
||||
.def_prop_ro("value", [](const track_core::Color &color) {
|
||||
return static_cast<std::uint32_t>(color);
|
||||
})
|
||||
.def_static("black", &track_core::Color::black)
|
||||
.def_static("red", &track_core::Color::red)
|
||||
.def_static("orange", &track_core::Color::orange)
|
||||
.def_static("yellow", &track_core::Color::yellow)
|
||||
.def_static("green", &track_core::Color::green)
|
||||
.def_static("cyan", &track_core::Color::cyan)
|
||||
.def_static("blue", &track_core::Color::blue)
|
||||
.def_static("indigo", &track_core::Color::indigo)
|
||||
.def_static("violet", &track_core::Color::violet)
|
||||
.def_static("white", &track_core::Color::white)
|
||||
.def("__eq__", [](const track_core::Color &lhs, const track_core::Color &rhs) {
|
||||
return lhs == rhs;
|
||||
})
|
||||
.def("__repr__", &color_repr);
|
||||
|
||||
nb::class_<track_core::TrackConfig>(m, "TrackConfig")
|
||||
.def(nb::init<>())
|
||||
.def_rw("draw_kind", &track_core::TrackConfig::draw_kind)
|
||||
.def_rw("line_length_m", &track_core::TrackConfig::line_length_m)
|
||||
.def_rw("active_line_length_m", &track_core::TrackConfig::active_line_length_m)
|
||||
.def_rw("head_offset_m", &track_core::TrackConfig::head_offset_m)
|
||||
.def_rw("line_leds_num", &track_core::TrackConfig::line_leds_num)
|
||||
.def("led_distance", &track_core::TrackConfig::led_distance)
|
||||
.def("verify", &track_core::TrackConfig::verify)
|
||||
.def_static("default_config", &track_core::TrackConfig::default_config);
|
||||
|
||||
nb::class_<track_core::TrackInfo>(m, "TrackInfo")
|
||||
.def(nb::init<>())
|
||||
.def_rw("kind", &track_core::TrackInfo::kind)
|
||||
.def_rw("color", &track_core::TrackInfo::color)
|
||||
.def_rw("id", &track_core::TrackInfo::id)
|
||||
.def_rw("is_running", &track_core::TrackInfo::is_running)
|
||||
.def_rw("num_segments", &track_core::TrackInfo::num_segments);
|
||||
|
||||
nb::class_<track_core::TrackSchemeMgrRead::Status>(m, "TrackSchemeStatus")
|
||||
.def(nb::init<>())
|
||||
.def_rw("id", &track_core::TrackSchemeMgrRead::Status::id)
|
||||
.def_rw("segment_count", &track_core::TrackSchemeMgrRead::Status::segment_count)
|
||||
.def_rw("kind", &track_core::TrackSchemeMgrRead::Status::kind);
|
||||
|
||||
nb::class_<track_core::TrackReport>(m, "TrackReport")
|
||||
.def(nb::init<>())
|
||||
.def_rw("id", &track_core::TrackReport::id)
|
||||
.def_rw("state", &track_core::TrackReport::state)
|
||||
.def_rw("mileage_m", &track_core::TrackReport::mileage_m)
|
||||
.def_rw("speed_m_s", &track_core::TrackReport::speed_m_s)
|
||||
.def_rw("time_elapsed_ms", &track_core::TrackReport::time_elapsed_ms);
|
||||
|
||||
nb::class_<track_core::TrackRenderSpan>(m, "TrackRenderSpan")
|
||||
.def(nb::init<>())
|
||||
.def_rw("start_led", &track_core::TrackRenderSpan::start_led)
|
||||
.def_rw("led_count", &track_core::TrackRenderSpan::led_count)
|
||||
.def_rw("color", &track_core::TrackRenderSpan::color);
|
||||
|
||||
nb::class_<track_core::TrackRenderPlan>(m, "TrackRenderPlan")
|
||||
.def(nb::init<>())
|
||||
.def("empty", &track_core::TrackRenderPlan::empty)
|
||||
.def_prop_ro("span_count", [](const track_core::TrackRenderPlan &plan) {
|
||||
return plan.span_count;
|
||||
})
|
||||
.def_prop_ro("spans", &spans_to_list);
|
||||
|
||||
nb::class_<track_core::MemoryStrip>(m, "MemoryStrip")
|
||||
.def(nb::init<std::size_t>(), "led_count"_a = 0)
|
||||
.def("begin", &track_core::MemoryStrip::begin)
|
||||
.def("clear", &track_core::MemoryStrip::clear)
|
||||
.def("fill", &track_core::MemoryStrip::fill, "start"_a, "count"_a, "color"_a)
|
||||
.def("show", &track_core::MemoryStrip::show)
|
||||
.def("set_leds_count", &track_core::MemoryStrip::set_leds_count, "count"_a)
|
||||
.def_prop_ro("leds_count", &track_core::MemoryStrip::leds_count)
|
||||
.def_prop_ro("frame_sequence", &track_core::MemoryStrip::frame_sequence)
|
||||
.def_prop_ro("pixels", [](const track_core::MemoryStrip &strip) {
|
||||
return colors_to_list(strip.pixels());
|
||||
});
|
||||
|
||||
nb::class_<track_core::SMSegment>(m, "SMSegment")
|
||||
.def(nb::init<>())
|
||||
.def_rw("speed_m_s", &track_core::SMSegment::speed_m_s)
|
||||
.def_rw("mileage_from_start_m", &track_core::SMSegment::mileage_from_start_m);
|
||||
|
||||
nb::class_<track_core::MTSegment>(m, "MTSegment")
|
||||
.def(nb::init<>())
|
||||
.def_rw("mileage_to_travel_this_segment_m", &track_core::MTSegment::mileage_to_travel_this_segment_m)
|
||||
.def_rw("time_since_start_s", &track_core::MTSegment::time_since_start_s);
|
||||
|
||||
nb::class_<track_core::STSegment>(m, "STSegment")
|
||||
.def(nb::init<>())
|
||||
.def_rw("speed_m_s", &track_core::STSegment::speed_m_s)
|
||||
.def_rw("time_since_start_s", &track_core::STSegment::time_since_start_s);
|
||||
|
||||
nb::class_<track_core::RepeatedSMSegment>(m, "RepeatedSMSegment")
|
||||
.def(nb::init<>())
|
||||
.def_rw("speed_mileage_segments", &track_core::RepeatedSMSegment::speed_mileage_segments)
|
||||
.def_rw("time_since_start_s", &track_core::RepeatedSMSegment::time_since_start_s);
|
||||
|
||||
nb::class_<track_core::DecodedScheme>(m, "DecodedScheme")
|
||||
.def_prop_ro("id", [](const track_core::DecodedScheme &scheme) { return scheme.id; })
|
||||
.def_prop_ro("color", [](const track_core::DecodedScheme &scheme) { return scheme.color; })
|
||||
.def_prop_ro("kind", [](const track_core::DecodedScheme &scheme) { return scheme.kind; })
|
||||
.def_prop_ro("acceleration_profile", [](const track_core::DecodedScheme &scheme) {
|
||||
return scheme.acceleration_profile;
|
||||
});
|
||||
|
||||
nb::class_<track_core::SchemeTrackState>(m, "SchemeTrackState")
|
||||
.def(nb::init<>())
|
||||
.def_rw("is_running", &track_core::SchemeTrackState::is_running)
|
||||
.def_rw("primary_segment_index", &track_core::SchemeTrackState::primary_segment_index)
|
||||
.def_rw("sub_segment_index", &track_core::SchemeTrackState::sub_segment_index)
|
||||
.def_rw("mileage_m", &track_core::SchemeTrackState::mileage_m)
|
||||
.def_rw("loop_mileage_m", &track_core::SchemeTrackState::loop_mileage_m)
|
||||
.def_rw("speed_m_s", &track_core::SchemeTrackState::speed_m_s)
|
||||
.def_rw("elapsed_s", &track_core::SchemeTrackState::elapsed_s);
|
||||
|
||||
nb::class_<track_core::SchemeTrackRuntime>(m, "SchemeTrackRuntime")
|
||||
.def_prop_ro("state", [](const track_core::SchemeTrackRuntime &runtime) {
|
||||
return runtime.state;
|
||||
})
|
||||
.def("info", &track_core::scheme_track_info)
|
||||
.def("report", &track_core::scheme_track_report);
|
||||
|
||||
nb::class_<track_core::SchemeTrainingRuntime>(m, "SchemeTrainingRuntime")
|
||||
.def(nb::init<>())
|
||||
.def("has_program", &track_core::SchemeTrainingRuntime::has_program)
|
||||
.def("all_stopped", &track_core::SchemeTrainingRuntime::all_stopped)
|
||||
.def("add_scheme", &add_scheme_checked, "scheme"_a)
|
||||
.def("clear", &track_core::SchemeTrainingRuntime::clear)
|
||||
.def("start", &track_core::SchemeTrainingRuntime::start)
|
||||
.def("stop", &track_core::SchemeTrainingRuntime::stop)
|
||||
.def("tick", &track_core::SchemeTrainingRuntime::tick, "config"_a, "delta_s"_a)
|
||||
.def("state_collection", &runtime_state_collection)
|
||||
.def("scheme_status", &runtime_scheme_status)
|
||||
.def("render_pixels", &runtime_render_pixels, "config"_a);
|
||||
|
||||
m.def("make_render_plan", &make_render_plan_checked, "config"_a, "info"_a, "report"_a);
|
||||
m.def("render_pixels", &render_pixels, "config"_a, "info"_a, "report"_a);
|
||||
m.def("make_speed_mileage_scheme", &track_core::make_speed_mileage_scheme,
|
||||
"id"_a, "color"_a, "acceleration_profile"_a, "segments"_a);
|
||||
m.def("make_mileage_time_scheme", &track_core::make_mileage_time_scheme,
|
||||
"id"_a, "color"_a, "acceleration_profile"_a, "segments"_a);
|
||||
m.def("make_speed_time_scheme", &track_core::make_speed_time_scheme,
|
||||
"id"_a, "color"_a, "acceleration_profile"_a, "segments"_a);
|
||||
m.def("make_repeated_speed_mileage_time_scheme", &track_core::make_repeated_speed_mileage_time_scheme,
|
||||
"id"_a, "color"_a, "acceleration_profile"_a, "segments"_a);
|
||||
m.def("make_scheme_track_runtime", &make_scheme_track_runtime_checked, "scheme"_a);
|
||||
m.def("start_scheme_track", &track_core::start_scheme_track, "runtime"_a);
|
||||
m.def("stop_scheme_track", &track_core::stop_scheme_track, "runtime"_a);
|
||||
m.def("tick_scheme_track", &track_core::tick_scheme_track, "config"_a, "runtime"_a, "delta_s"_a);
|
||||
m.def("scheme_track_info", &track_core::scheme_track_info, "runtime"_a);
|
||||
m.def("scheme_track_report", &track_core::scheme_track_report, "runtime"_a);
|
||||
}
|
||||
+66
-7
@@ -173,6 +173,64 @@ void TrackRenderPlan::add_fill(std::uint16_t start_led, std::uint16_t led_count,
|
||||
};
|
||||
}
|
||||
|
||||
TrackError clear_render_sink(TrackRenderSink sink) {
|
||||
if (sink.clear == nullptr) {
|
||||
return TrackError::invalid_arg;
|
||||
}
|
||||
return sink.clear(sink.context);
|
||||
}
|
||||
|
||||
TrackError apply_render_plan(TrackRenderSink sink, const TrackRenderPlan &plan) {
|
||||
if (plan.empty()) {
|
||||
return TrackError::ok;
|
||||
}
|
||||
if (sink.fill == nullptr) {
|
||||
return TrackError::invalid_arg;
|
||||
}
|
||||
|
||||
for (std::size_t i = 0; i < plan.span_count; ++i) {
|
||||
const auto &span = plan.spans[i];
|
||||
if (const auto err = sink.fill(sink.context, span.start_led, span.led_count, span.color);
|
||||
err != TrackError::ok) {
|
||||
return err;
|
||||
}
|
||||
}
|
||||
return TrackError::ok;
|
||||
}
|
||||
|
||||
TrackError show_render_sink(TrackRenderSink sink) {
|
||||
if (sink.show == nullptr) {
|
||||
return TrackError::invalid_arg;
|
||||
}
|
||||
return sink.show(sink.context);
|
||||
}
|
||||
|
||||
namespace {
|
||||
|
||||
void add_clipped_fill(
|
||||
TrackRenderPlan &plan,
|
||||
int start_led,
|
||||
int led_count,
|
||||
int line_leds_num,
|
||||
Color color) {
|
||||
if (led_count <= 0 || line_leds_num <= 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
const auto clipped_start = std::max(start_led, 0);
|
||||
const auto clipped_end = std::min(start_led + led_count, line_leds_num);
|
||||
if (clipped_end <= clipped_start) {
|
||||
return;
|
||||
}
|
||||
|
||||
plan.add_fill(
|
||||
static_cast<std::uint16_t>(clipped_start),
|
||||
static_cast<std::uint16_t>(clipped_end - clipped_start),
|
||||
color);
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
TrackRenderPlan make_track_render_plan(
|
||||
const TrackConfig &config,
|
||||
const TrackInfo &info,
|
||||
@@ -193,32 +251,33 @@ TrackRenderPlan make_track_render_plan(
|
||||
const auto drawer = LinearLineDrawer::from_report(config, report);
|
||||
const auto magic_color_ahead = Color::blue();
|
||||
const auto magic_color_behind = Color::cyan();
|
||||
const auto center_offset = drawer.center_offset_leds_num();
|
||||
const auto center_offset = static_cast<int>(drawer.center_offset_leds_num());
|
||||
const auto line_leds_num = static_cast<int>(config.line_leds_num);
|
||||
const auto fill_positive_side = [&](std::uint16_t count, Color near_center, Color far_end) {
|
||||
if (count == 0) {
|
||||
return;
|
||||
}
|
||||
if (count == 1) {
|
||||
plan.add_fill(center_offset, 1, near_center);
|
||||
add_clipped_fill(plan, center_offset, 1, line_leds_num, near_center);
|
||||
return;
|
||||
}
|
||||
const auto distal_count = static_cast<std::uint16_t>(count / 2);
|
||||
const auto proximal_count = static_cast<std::uint16_t>(count - distal_count);
|
||||
plan.add_fill(center_offset, proximal_count, near_center);
|
||||
plan.add_fill(static_cast<std::uint16_t>(center_offset + proximal_count), distal_count, far_end);
|
||||
add_clipped_fill(plan, center_offset, proximal_count, line_leds_num, near_center);
|
||||
add_clipped_fill(plan, center_offset + proximal_count, distal_count, line_leds_num, far_end);
|
||||
};
|
||||
const auto fill_negative_side = [&](std::uint16_t count, Color near_center, Color far_end) {
|
||||
if (count == 0) {
|
||||
return;
|
||||
}
|
||||
if (count == 1) {
|
||||
plan.add_fill(static_cast<std::uint16_t>(center_offset - 1), 1, near_center);
|
||||
add_clipped_fill(plan, center_offset - 1, 1, line_leds_num, near_center);
|
||||
return;
|
||||
}
|
||||
const auto distal_count = static_cast<std::uint16_t>(count / 2);
|
||||
const auto proximal_count = static_cast<std::uint16_t>(count - distal_count);
|
||||
plan.add_fill(static_cast<std::uint16_t>(center_offset - count), distal_count, far_end);
|
||||
plan.add_fill(static_cast<std::uint16_t>(center_offset - proximal_count), proximal_count, near_center);
|
||||
add_clipped_fill(plan, center_offset - count, distal_count, line_leds_num, far_end);
|
||||
add_clipped_fill(plan, center_offset - proximal_count, proximal_count, line_leds_num, near_center);
|
||||
};
|
||||
const auto ahead = drawer.center_ahead_leds_num();
|
||||
const auto behind = drawer.center_behind_leds_num();
|
||||
|
||||
@@ -30,6 +30,7 @@ expected<DecodedScheme, TrackError> decode_scheme(
|
||||
.color = color,
|
||||
.kind = kind,
|
||||
.acceleration_profile = acceleration_profile,
|
||||
.segments = std::vector<SMSegment>{},
|
||||
};
|
||||
|
||||
switch (kind) {
|
||||
|
||||
@@ -0,0 +1,763 @@
|
||||
#include "track_core/scheme_runtime.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <cstdint>
|
||||
#include <limits>
|
||||
#include <numeric>
|
||||
#include <type_traits>
|
||||
|
||||
#include "track_core/memory_strip.hpp"
|
||||
#include "track_core/render.hpp"
|
||||
|
||||
namespace track_core {
|
||||
namespace {
|
||||
|
||||
struct accel_calc_result {
|
||||
float target_speed_m_s{};
|
||||
float acceleration_m_s_2{};
|
||||
};
|
||||
|
||||
constexpr float max_acceleration_m_s_2 = 5.0F;
|
||||
|
||||
[[nodiscard]]
|
||||
bool valid_acceleration_profile(AccelerationProfile profile) {
|
||||
return profile == AccelerationProfile::instant ||
|
||||
profile == AccelerationProfile::smooth;
|
||||
}
|
||||
|
||||
[[nodiscard]]
|
||||
bool finite_nonnegative(float value) {
|
||||
return std::isfinite(value) && value >= 0.0F;
|
||||
}
|
||||
|
||||
[[nodiscard]]
|
||||
std::uint8_t clamp_u8(std::size_t value) {
|
||||
return static_cast<std::uint8_t>(
|
||||
std::min<std::size_t>(value, std::numeric_limits<std::uint8_t>::max()));
|
||||
}
|
||||
|
||||
[[nodiscard]]
|
||||
std::uint32_t elapsed_ms(float elapsed_s) {
|
||||
if (!std::isfinite(elapsed_s) || elapsed_s <= 0.0F) {
|
||||
return 0;
|
||||
}
|
||||
const auto ms = elapsed_s * 1000.0F;
|
||||
if (ms >= static_cast<float>(std::numeric_limits<std::uint32_t>::max())) {
|
||||
return std::numeric_limits<std::uint32_t>::max();
|
||||
}
|
||||
return static_cast<std::uint32_t>(ms);
|
||||
}
|
||||
|
||||
[[nodiscard]]
|
||||
accel_calc_result speed_mileage_accel(
|
||||
const SMSegment ¤t,
|
||||
const SMSegment &next,
|
||||
float current_mileage_m,
|
||||
float current_speed_m_s) {
|
||||
const float target_speed = next.speed_m_s;
|
||||
float remaining_distance = static_cast<float>(next.mileage_from_start_m) - current_mileage_m;
|
||||
|
||||
if (remaining_distance <= 0.0F) {
|
||||
return {target_speed, 0.0F};
|
||||
}
|
||||
|
||||
float accel =
|
||||
(target_speed * target_speed - current_speed_m_s * current_speed_m_s) /
|
||||
(2.0F * remaining_distance);
|
||||
accel = std::clamp(accel, -max_acceleration_m_s_2, max_acceleration_m_s_2);
|
||||
return {target_speed, accel};
|
||||
}
|
||||
|
||||
[[nodiscard]]
|
||||
accel_calc_result speed_time_accel(const STSegment ¤t, const STSegment &next) {
|
||||
const float target_speed = next.speed_m_s;
|
||||
const float duration_s =
|
||||
static_cast<float>(next.time_since_start_s) -
|
||||
static_cast<float>(current.time_since_start_s);
|
||||
|
||||
if (duration_s <= 0.0F) {
|
||||
return {target_speed, 0.0F};
|
||||
}
|
||||
|
||||
float accel = (target_speed - current.speed_m_s) / duration_s;
|
||||
accel = std::clamp(accel, -max_acceleration_m_s_2, max_acceleration_m_s_2);
|
||||
return {target_speed, accel};
|
||||
}
|
||||
|
||||
[[nodiscard]]
|
||||
accel_calc_result mileage_time_accel(
|
||||
float elapsed_s,
|
||||
float next_segment_time_s,
|
||||
float current_speed_m_s,
|
||||
float target_speed_m_s) {
|
||||
const float remaining_time_s = next_segment_time_s - elapsed_s;
|
||||
if (remaining_time_s <= 0.0F) {
|
||||
return {target_speed_m_s, 0.0F};
|
||||
}
|
||||
|
||||
float accel = (target_speed_m_s - current_speed_m_s) / remaining_time_s;
|
||||
accel = std::clamp(accel, -max_acceleration_m_s_2, max_acceleration_m_s_2);
|
||||
return {target_speed_m_s, accel};
|
||||
}
|
||||
|
||||
void apply_acceleration(SchemeTrackState &state, accel_calc_result profile, float delta_s) {
|
||||
state.speed_m_s += profile.acceleration_m_s_2 * delta_s;
|
||||
|
||||
if (profile.acceleration_m_s_2 > 0.0F) {
|
||||
state.speed_m_s = std::min(state.speed_m_s, profile.target_speed_m_s);
|
||||
} else if (profile.acceleration_m_s_2 < 0.0F) {
|
||||
state.speed_m_s = std::max(state.speed_m_s, profile.target_speed_m_s);
|
||||
}
|
||||
}
|
||||
|
||||
[[nodiscard]]
|
||||
std::optional<float> calc_mts_target_speed(
|
||||
std::size_t segment_index,
|
||||
const std::vector<MTSegment> &segments) {
|
||||
if (segments.size() < 2 || segment_index > segments.size() - 2) {
|
||||
return std::nullopt;
|
||||
}
|
||||
|
||||
const auto ¤t = segments[segment_index];
|
||||
const auto &next = segments[segment_index + 1];
|
||||
if (next.time_since_start_s < current.time_since_start_s) {
|
||||
return std::nullopt;
|
||||
}
|
||||
|
||||
const auto duration_s = next.time_since_start_s - current.time_since_start_s;
|
||||
if (duration_s == 0 || current.mileage_to_travel_this_segment_m == 0) {
|
||||
return std::nullopt;
|
||||
}
|
||||
|
||||
return static_cast<float>(current.mileage_to_travel_this_segment_m) /
|
||||
static_cast<float>(duration_s);
|
||||
}
|
||||
|
||||
[[nodiscard]]
|
||||
std::optional<float> calc_mts_target_speed(const MTSegment ¤t, const MTSegment &next) {
|
||||
if (next.time_since_start_s < current.time_since_start_s) {
|
||||
return std::nullopt;
|
||||
}
|
||||
|
||||
const auto duration_s = next.time_since_start_s - current.time_since_start_s;
|
||||
if (duration_s == 0 || current.mileage_to_travel_this_segment_m == 0) {
|
||||
return std::nullopt;
|
||||
}
|
||||
|
||||
return static_cast<float>(current.mileage_to_travel_this_segment_m) /
|
||||
static_cast<float>(duration_s);
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
[[nodiscard]]
|
||||
bool strictly_sorted_by_time(const std::vector<T> &segments) {
|
||||
for (std::size_t i = 1; i < segments.size(); ++i) {
|
||||
if (segments[i].time_since_start_s <= segments[i - 1].time_since_start_s) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
[[nodiscard]]
|
||||
expected<DecodedScheme, TrackError> canonicalize_scheme(DecodedScheme scheme) {
|
||||
if (!valid_acceleration_profile(scheme.acceleration_profile)) {
|
||||
return unexpected<TrackError>{TrackError::invalid_arg};
|
||||
}
|
||||
|
||||
switch (scheme.kind) {
|
||||
case SchemeKind::speed_input_mileage_segmented_time_free: {
|
||||
auto *segments = std::get_if<std::vector<SMSegment>>(&scheme.segments);
|
||||
if (segments == nullptr || segments->empty()) {
|
||||
return unexpected<TrackError>{TrackError::invalid_arg};
|
||||
}
|
||||
std::ranges::sort(*segments, {}, &SMSegment::mileage_from_start_m);
|
||||
if (segments->front().mileage_from_start_m != 0) {
|
||||
return unexpected<TrackError>{TrackError::invalid_arg};
|
||||
}
|
||||
for (const auto &segment : *segments) {
|
||||
if (!finite_nonnegative(segment.speed_m_s)) {
|
||||
return unexpected<TrackError>{TrackError::invalid_arg};
|
||||
}
|
||||
}
|
||||
return scheme;
|
||||
}
|
||||
|
||||
case SchemeKind::mileage_input_time_segmented_speed_free: {
|
||||
auto *segments = std::get_if<std::vector<MTSegment>>(&scheme.segments);
|
||||
if (segments == nullptr || segments->size() < 2) {
|
||||
return unexpected<TrackError>{TrackError::invalid_arg};
|
||||
}
|
||||
std::ranges::sort(*segments, {}, &MTSegment::time_since_start_s);
|
||||
if (segments->front().time_since_start_s != 0 || !strictly_sorted_by_time(*segments)) {
|
||||
return unexpected<TrackError>{TrackError::invalid_arg};
|
||||
}
|
||||
for (std::size_t i = 0; i + 1 < segments->size(); ++i) {
|
||||
if (!calc_mts_target_speed(i, *segments)) {
|
||||
return unexpected<TrackError>{TrackError::invalid_arg};
|
||||
}
|
||||
}
|
||||
return scheme;
|
||||
}
|
||||
|
||||
case SchemeKind::speed_input_time_segmented_mileage_free: {
|
||||
auto *segments = std::get_if<std::vector<STSegment>>(&scheme.segments);
|
||||
if (segments == nullptr || segments->empty()) {
|
||||
return unexpected<TrackError>{TrackError::invalid_arg};
|
||||
}
|
||||
std::ranges::sort(*segments, {}, &STSegment::time_since_start_s);
|
||||
if (segments->front().time_since_start_s != 0) {
|
||||
return unexpected<TrackError>{TrackError::invalid_arg};
|
||||
}
|
||||
for (const auto &segment : *segments) {
|
||||
if (!finite_nonnegative(segment.speed_m_s)) {
|
||||
return unexpected<TrackError>{TrackError::invalid_arg};
|
||||
}
|
||||
}
|
||||
return scheme;
|
||||
}
|
||||
|
||||
case SchemeKind::repeated_speed_input_mileage_segmentation_input_time_segmented: {
|
||||
auto *segments = std::get_if<std::vector<RepeatedSMSegment>>(&scheme.segments);
|
||||
if (segments == nullptr || segments->empty()) {
|
||||
return unexpected<TrackError>{TrackError::invalid_arg};
|
||||
}
|
||||
std::ranges::sort(*segments, {}, &RepeatedSMSegment::time_since_start_s);
|
||||
if (segments->front().time_since_start_s != 0) {
|
||||
return unexpected<TrackError>{TrackError::invalid_arg};
|
||||
}
|
||||
for (auto &time_segment : *segments) {
|
||||
auto &sub_segments = time_segment.speed_mileage_segments;
|
||||
if (sub_segments.empty()) {
|
||||
return unexpected<TrackError>{TrackError::invalid_arg};
|
||||
}
|
||||
std::ranges::sort(sub_segments, {}, &SMSegment::mileage_from_start_m);
|
||||
if (sub_segments.front().mileage_from_start_m != 0) {
|
||||
return unexpected<TrackError>{TrackError::invalid_arg};
|
||||
}
|
||||
for (const auto &sub_segment : sub_segments) {
|
||||
if (!finite_nonnegative(sub_segment.speed_m_s)) {
|
||||
return unexpected<TrackError>{TrackError::invalid_arg};
|
||||
}
|
||||
}
|
||||
}
|
||||
return scheme;
|
||||
}
|
||||
}
|
||||
|
||||
return unexpected<TrackError>{TrackError::not_supported};
|
||||
}
|
||||
|
||||
void tick_speed_mileage(SchemeTrackRuntime &runtime, float delta_s) {
|
||||
auto &state = runtime.state;
|
||||
const auto &segments = std::get<std::vector<SMSegment>>(runtime.scheme.segments);
|
||||
|
||||
state.elapsed_s += delta_s;
|
||||
const auto ¤t = segments[state.primary_segment_index];
|
||||
|
||||
if (runtime.scheme.acceleration_profile == AccelerationProfile::smooth) {
|
||||
accel_calc_result profile{current.speed_m_s, 0.0F};
|
||||
if (state.primary_segment_index + 1 < segments.size()) {
|
||||
profile = speed_mileage_accel(
|
||||
current,
|
||||
segments[state.primary_segment_index + 1],
|
||||
state.mileage_m,
|
||||
state.speed_m_s);
|
||||
}
|
||||
apply_acceleration(state, profile, delta_s);
|
||||
} else {
|
||||
state.speed_m_s = current.speed_m_s;
|
||||
}
|
||||
|
||||
state.mileage_m += state.speed_m_s * delta_s;
|
||||
|
||||
if (static_cast<std::uint16_t>(state.mileage_m) >= segments.back().mileage_from_start_m) {
|
||||
state.is_running = false;
|
||||
return;
|
||||
}
|
||||
|
||||
while (state.primary_segment_index + 1 < segments.size()) {
|
||||
const auto next_index = state.primary_segment_index + 1;
|
||||
const auto &next = segments[next_index];
|
||||
if (static_cast<std::uint16_t>(state.mileage_m) < next.mileage_from_start_m) {
|
||||
break;
|
||||
}
|
||||
state.primary_segment_index = next_index;
|
||||
state.speed_m_s = next.speed_m_s;
|
||||
}
|
||||
}
|
||||
|
||||
void tick_mileage_time(SchemeTrackRuntime &runtime, float delta_s) {
|
||||
auto &state = runtime.state;
|
||||
const auto &segments = std::get<std::vector<MTSegment>>(runtime.scheme.segments);
|
||||
const float next_elapsed_s = state.elapsed_s + delta_s;
|
||||
|
||||
if (next_elapsed_s >= static_cast<float>(segments.back().time_since_start_s)) {
|
||||
state.elapsed_s = next_elapsed_s;
|
||||
state.is_running = false;
|
||||
return;
|
||||
}
|
||||
|
||||
state.elapsed_s = next_elapsed_s;
|
||||
|
||||
const auto next_target_speed = calc_mts_target_speed(state.primary_segment_index + 1, segments);
|
||||
if (runtime.scheme.acceleration_profile == AccelerationProfile::smooth && next_target_speed &&
|
||||
state.primary_segment_index + 1 < segments.size()) {
|
||||
const float next_segment_time_s =
|
||||
static_cast<float>(segments[state.primary_segment_index + 1].time_since_start_s);
|
||||
const auto profile = mileage_time_accel(
|
||||
state.elapsed_s,
|
||||
next_segment_time_s,
|
||||
state.speed_m_s,
|
||||
*next_target_speed);
|
||||
apply_acceleration(state, profile, delta_s);
|
||||
}
|
||||
|
||||
state.mileage_m += state.speed_m_s * delta_s;
|
||||
|
||||
while (state.primary_segment_index + 1 < segments.size()) {
|
||||
const auto next_index = state.primary_segment_index + 1;
|
||||
const auto &next = segments[next_index];
|
||||
if (state.elapsed_s < static_cast<float>(next.time_since_start_s)) {
|
||||
break;
|
||||
}
|
||||
|
||||
const auto ¤t = segments[state.primary_segment_index];
|
||||
const float last_speed_m_s = state.speed_m_s;
|
||||
state.primary_segment_index = next_index;
|
||||
state.speed_m_s = calc_mts_target_speed(current, next).value_or(last_speed_m_s);
|
||||
}
|
||||
}
|
||||
|
||||
void tick_speed_time(SchemeTrackRuntime &runtime, float delta_s) {
|
||||
auto &state = runtime.state;
|
||||
const auto &segments = std::get<std::vector<STSegment>>(runtime.scheme.segments);
|
||||
const float next_elapsed_s = state.elapsed_s + delta_s;
|
||||
|
||||
if (next_elapsed_s >= static_cast<float>(segments.back().time_since_start_s)) {
|
||||
state.elapsed_s = next_elapsed_s;
|
||||
state.is_running = false;
|
||||
return;
|
||||
}
|
||||
|
||||
state.elapsed_s = next_elapsed_s;
|
||||
const auto ¤t = segments[state.primary_segment_index];
|
||||
|
||||
if (runtime.scheme.acceleration_profile == AccelerationProfile::smooth) {
|
||||
accel_calc_result profile{current.speed_m_s, 0.0F};
|
||||
if (state.primary_segment_index + 1 < segments.size()) {
|
||||
profile = speed_time_accel(current, segments[state.primary_segment_index + 1]);
|
||||
}
|
||||
apply_acceleration(state, profile, delta_s);
|
||||
} else {
|
||||
state.speed_m_s = current.speed_m_s;
|
||||
}
|
||||
|
||||
state.mileage_m += state.speed_m_s * delta_s;
|
||||
|
||||
while (state.primary_segment_index + 1 < segments.size()) {
|
||||
const auto next_index = state.primary_segment_index + 1;
|
||||
const auto &next = segments[next_index];
|
||||
if (state.elapsed_s < static_cast<float>(next.time_since_start_s)) {
|
||||
break;
|
||||
}
|
||||
state.primary_segment_index = next_index;
|
||||
state.speed_m_s = next.speed_m_s;
|
||||
}
|
||||
}
|
||||
|
||||
[[nodiscard]]
|
||||
float current_rsmt_loop_length(const SchemeTrackRuntime &runtime) {
|
||||
const auto &segments = std::get<std::vector<RepeatedSMSegment>>(runtime.scheme.segments);
|
||||
const auto &sub_segments = segments[runtime.state.primary_segment_index].speed_mileage_segments;
|
||||
return sub_segments.empty()
|
||||
? 0.0F
|
||||
: static_cast<float>(sub_segments.back().mileage_from_start_m);
|
||||
}
|
||||
|
||||
[[nodiscard]]
|
||||
std::optional<std::pair<std::size_t, const SMSegment *>>
|
||||
next_rsmt_sub_segment(const SchemeTrackRuntime &runtime) {
|
||||
const auto &segments = std::get<std::vector<RepeatedSMSegment>>(runtime.scheme.segments);
|
||||
const auto &sub_segments = segments[runtime.state.primary_segment_index].speed_mileage_segments;
|
||||
const auto next_index = runtime.state.sub_segment_index + 1;
|
||||
|
||||
if (next_index < sub_segments.size()) {
|
||||
return std::pair{next_index, &sub_segments[next_index]};
|
||||
}
|
||||
if (!sub_segments.empty()) {
|
||||
return std::pair{std::size_t{0}, &sub_segments[0]};
|
||||
}
|
||||
return std::nullopt;
|
||||
}
|
||||
|
||||
[[nodiscard]]
|
||||
accel_calc_result rsmt_accel(
|
||||
const SchemeTrackRuntime &runtime,
|
||||
const SMSegment &next) {
|
||||
const auto &state = runtime.state;
|
||||
const float target_speed = next.speed_m_s;
|
||||
float remaining_distance =
|
||||
static_cast<float>(next.mileage_from_start_m) - state.loop_mileage_m;
|
||||
|
||||
if (next.mileage_from_start_m == 0 && state.sub_segment_index > 0) {
|
||||
remaining_distance = current_rsmt_loop_length(runtime) - state.loop_mileage_m;
|
||||
}
|
||||
|
||||
if (remaining_distance <= 0.0F) {
|
||||
return {target_speed, 0.0F};
|
||||
}
|
||||
|
||||
float accel =
|
||||
(target_speed * target_speed - state.speed_m_s * state.speed_m_s) /
|
||||
(2.0F * remaining_distance);
|
||||
accel = std::clamp(accel, -max_acceleration_m_s_2, max_acceleration_m_s_2);
|
||||
return {target_speed, accel};
|
||||
}
|
||||
|
||||
void tick_repeated_speed_mileage_time(const TrackConfig &config, SchemeTrackRuntime &runtime, float delta_s) {
|
||||
auto &state = runtime.state;
|
||||
const auto &segments = std::get<std::vector<RepeatedSMSegment>>(runtime.scheme.segments);
|
||||
state.elapsed_s += delta_s;
|
||||
|
||||
if (state.primary_segment_index + 1 < segments.size()) {
|
||||
const auto &next_time_segment = segments[state.primary_segment_index + 1];
|
||||
if (state.elapsed_s >= static_cast<float>(next_time_segment.time_since_start_s)) {
|
||||
const float line_length_m = config.line_length_m;
|
||||
const float err = line_length_m > 0.0F
|
||||
? std::fmod(state.mileage_m, line_length_m)
|
||||
: 0.0F;
|
||||
const float epsilon = line_length_m * 0.01F;
|
||||
if (std::abs(err) <= epsilon) {
|
||||
++state.primary_segment_index;
|
||||
state.sub_segment_index = 0;
|
||||
state.loop_mileage_m = 0.0F;
|
||||
state.speed_m_s =
|
||||
segments[state.primary_segment_index].speed_mileage_segments[0].speed_m_s;
|
||||
}
|
||||
}
|
||||
} else if (state.elapsed_s >= static_cast<float>(segments.back().time_since_start_s)) {
|
||||
state.is_running = false;
|
||||
return;
|
||||
}
|
||||
|
||||
const auto &sub_segments = segments[state.primary_segment_index].speed_mileage_segments;
|
||||
const auto ¤t = sub_segments[state.sub_segment_index];
|
||||
|
||||
if (runtime.scheme.acceleration_profile == AccelerationProfile::smooth) {
|
||||
accel_calc_result profile{current.speed_m_s, 0.0F};
|
||||
if (auto next = next_rsmt_sub_segment(runtime)) {
|
||||
profile = rsmt_accel(runtime, *next->second);
|
||||
}
|
||||
apply_acceleration(state, profile, delta_s);
|
||||
} else {
|
||||
state.speed_m_s = current.speed_m_s;
|
||||
}
|
||||
|
||||
const float distance_traveled = state.speed_m_s * delta_s;
|
||||
state.mileage_m += distance_traveled;
|
||||
state.loop_mileage_m += distance_traveled;
|
||||
|
||||
const float loop_length = current_rsmt_loop_length(runtime);
|
||||
if (state.loop_mileage_m >= loop_length && loop_length > 0.0F) {
|
||||
state.loop_mileage_m -= loop_length;
|
||||
state.sub_segment_index = 0;
|
||||
return;
|
||||
}
|
||||
|
||||
while (auto next = next_rsmt_sub_segment(runtime)) {
|
||||
const auto [next_index, next_segment] = *next;
|
||||
if (next_index == 0) {
|
||||
break;
|
||||
}
|
||||
if (state.loop_mileage_m < static_cast<float>(next_segment->mileage_from_start_m)) {
|
||||
break;
|
||||
}
|
||||
state.sub_segment_index = next_index;
|
||||
if (runtime.scheme.acceleration_profile == AccelerationProfile::instant) {
|
||||
state.speed_m_s = next_segment->speed_m_s;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
DecodedScheme make_speed_mileage_scheme(
|
||||
std::uint8_t id,
|
||||
Color color,
|
||||
AccelerationProfile acceleration_profile,
|
||||
std::vector<SMSegment> segments) {
|
||||
return {
|
||||
.id = id,
|
||||
.color = color,
|
||||
.kind = SchemeKind::speed_input_mileage_segmented_time_free,
|
||||
.acceleration_profile = acceleration_profile,
|
||||
.segments = std::move(segments),
|
||||
};
|
||||
}
|
||||
|
||||
DecodedScheme make_mileage_time_scheme(
|
||||
std::uint8_t id,
|
||||
Color color,
|
||||
AccelerationProfile acceleration_profile,
|
||||
std::vector<MTSegment> segments) {
|
||||
return {
|
||||
.id = id,
|
||||
.color = color,
|
||||
.kind = SchemeKind::mileage_input_time_segmented_speed_free,
|
||||
.acceleration_profile = acceleration_profile,
|
||||
.segments = std::move(segments),
|
||||
};
|
||||
}
|
||||
|
||||
DecodedScheme make_speed_time_scheme(
|
||||
std::uint8_t id,
|
||||
Color color,
|
||||
AccelerationProfile acceleration_profile,
|
||||
std::vector<STSegment> segments) {
|
||||
return {
|
||||
.id = id,
|
||||
.color = color,
|
||||
.kind = SchemeKind::speed_input_time_segmented_mileage_free,
|
||||
.acceleration_profile = acceleration_profile,
|
||||
.segments = std::move(segments),
|
||||
};
|
||||
}
|
||||
|
||||
DecodedScheme make_repeated_speed_mileage_time_scheme(
|
||||
std::uint8_t id,
|
||||
Color color,
|
||||
AccelerationProfile acceleration_profile,
|
||||
std::vector<RepeatedSMSegment> segments) {
|
||||
return {
|
||||
.id = id,
|
||||
.color = color,
|
||||
.kind = SchemeKind::repeated_speed_input_mileage_segmentation_input_time_segmented,
|
||||
.acceleration_profile = acceleration_profile,
|
||||
.segments = std::move(segments),
|
||||
};
|
||||
}
|
||||
|
||||
expected<SchemeTrackRuntime, TrackError> make_scheme_track_runtime(DecodedScheme scheme) {
|
||||
auto normalized = canonicalize_scheme(std::move(scheme));
|
||||
if (!normalized) {
|
||||
return unexpected<TrackError>{normalized.error()};
|
||||
}
|
||||
|
||||
return SchemeTrackRuntime{
|
||||
.scheme = std::move(*normalized),
|
||||
.state = {},
|
||||
};
|
||||
}
|
||||
|
||||
SchemeTrackRuntime start_scheme_track(SchemeTrackRuntime runtime) {
|
||||
runtime.state = {};
|
||||
runtime.state.is_running = true;
|
||||
|
||||
switch (runtime.scheme.kind) {
|
||||
case SchemeKind::speed_input_mileage_segmented_time_free: {
|
||||
const auto &segments = std::get<std::vector<SMSegment>>(runtime.scheme.segments);
|
||||
runtime.state.speed_m_s = segments[0].speed_m_s;
|
||||
break;
|
||||
}
|
||||
case SchemeKind::mileage_input_time_segmented_speed_free: {
|
||||
const auto &segments = std::get<std::vector<MTSegment>>(runtime.scheme.segments);
|
||||
runtime.state.speed_m_s = calc_mts_target_speed(std::size_t{0}, segments).value_or(0.0F);
|
||||
break;
|
||||
}
|
||||
case SchemeKind::speed_input_time_segmented_mileage_free: {
|
||||
const auto &segments = std::get<std::vector<STSegment>>(runtime.scheme.segments);
|
||||
runtime.state.speed_m_s = segments[0].speed_m_s;
|
||||
break;
|
||||
}
|
||||
case SchemeKind::repeated_speed_input_mileage_segmentation_input_time_segmented: {
|
||||
const auto &segments = std::get<std::vector<RepeatedSMSegment>>(runtime.scheme.segments);
|
||||
runtime.state.speed_m_s = segments[0].speed_mileage_segments[0].speed_m_s;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
return runtime;
|
||||
}
|
||||
|
||||
SchemeTrackRuntime stop_scheme_track(SchemeTrackRuntime runtime) {
|
||||
runtime.state.is_running = false;
|
||||
return runtime;
|
||||
}
|
||||
|
||||
SchemeTrackRuntime tick_scheme_track(
|
||||
const TrackConfig &config,
|
||||
SchemeTrackRuntime runtime,
|
||||
float delta_s) {
|
||||
if (!runtime.state.is_running || !std::isfinite(delta_s) || delta_s <= 0.0F) {
|
||||
return runtime;
|
||||
}
|
||||
|
||||
switch (runtime.scheme.kind) {
|
||||
case SchemeKind::speed_input_mileage_segmented_time_free:
|
||||
tick_speed_mileage(runtime, delta_s);
|
||||
break;
|
||||
case SchemeKind::mileage_input_time_segmented_speed_free:
|
||||
tick_mileage_time(runtime, delta_s);
|
||||
break;
|
||||
case SchemeKind::speed_input_time_segmented_mileage_free:
|
||||
tick_speed_time(runtime, delta_s);
|
||||
break;
|
||||
case SchemeKind::repeated_speed_input_mileage_segmentation_input_time_segmented:
|
||||
tick_repeated_speed_mileage_time(config, runtime, delta_s);
|
||||
break;
|
||||
}
|
||||
|
||||
return runtime;
|
||||
}
|
||||
|
||||
TrackInfo scheme_track_info(const SchemeTrackRuntime &runtime) {
|
||||
std::size_t segment_count = 0;
|
||||
switch (runtime.scheme.kind) {
|
||||
case SchemeKind::speed_input_mileage_segmented_time_free:
|
||||
segment_count = std::get<std::vector<SMSegment>>(runtime.scheme.segments).size();
|
||||
break;
|
||||
case SchemeKind::mileage_input_time_segmented_speed_free:
|
||||
segment_count = std::get<std::vector<MTSegment>>(runtime.scheme.segments).size();
|
||||
break;
|
||||
case SchemeKind::speed_input_time_segmented_mileage_free:
|
||||
segment_count = std::get<std::vector<STSegment>>(runtime.scheme.segments).size();
|
||||
break;
|
||||
case SchemeKind::repeated_speed_input_mileage_segmentation_input_time_segmented: {
|
||||
const auto &segments = std::get<std::vector<RepeatedSMSegment>>(runtime.scheme.segments);
|
||||
segment_count = std::accumulate(
|
||||
segments.begin(),
|
||||
segments.end(),
|
||||
std::size_t{0},
|
||||
[](std::size_t sum, const RepeatedSMSegment &segment) {
|
||||
return sum + segment.speed_mileage_segments.size();
|
||||
});
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
return {
|
||||
.kind = runtime.scheme.kind,
|
||||
.color = runtime.scheme.color,
|
||||
.id = runtime.scheme.id,
|
||||
.is_running = runtime.state.is_running,
|
||||
.num_segments = clamp_u8(segment_count),
|
||||
};
|
||||
}
|
||||
|
||||
TrackReport scheme_track_report(const SchemeTrackRuntime &runtime) {
|
||||
return {
|
||||
.id = runtime.scheme.id,
|
||||
.state = runtime.state.is_running ? TrackState::run : TrackState::stop,
|
||||
.mileage_m = runtime.state.mileage_m,
|
||||
.speed_m_s = runtime.state.speed_m_s,
|
||||
.time_elapsed_ms = elapsed_ms(runtime.state.elapsed_s),
|
||||
};
|
||||
}
|
||||
|
||||
bool SchemeTrainingRuntime::has_program() const noexcept {
|
||||
return !tracks_.empty();
|
||||
}
|
||||
|
||||
bool SchemeTrainingRuntime::all_stopped() const noexcept {
|
||||
return std::ranges::all_of(tracks_, [](const SchemeTrackRuntime &track) {
|
||||
return !track.state.is_running;
|
||||
});
|
||||
}
|
||||
|
||||
expected<unit, TrackError> SchemeTrainingRuntime::add_scheme(DecodedScheme scheme) {
|
||||
auto runtime = make_scheme_track_runtime(std::move(scheme));
|
||||
if (!runtime) {
|
||||
return unexpected<TrackError>{runtime.error()};
|
||||
}
|
||||
|
||||
const auto id = runtime->scheme.id;
|
||||
const auto existing = std::ranges::find_if(tracks_, [id](const SchemeTrackRuntime &track) {
|
||||
return track.scheme.id == id;
|
||||
});
|
||||
if (existing != tracks_.end()) {
|
||||
tracks_.erase(existing);
|
||||
}
|
||||
|
||||
tracks_.push_back(std::move(*runtime));
|
||||
return {};
|
||||
}
|
||||
|
||||
void SchemeTrainingRuntime::clear() {
|
||||
tracks_.clear();
|
||||
}
|
||||
|
||||
void SchemeTrainingRuntime::start() {
|
||||
for (auto &track : tracks_) {
|
||||
track = start_scheme_track(std::move(track));
|
||||
}
|
||||
}
|
||||
|
||||
void SchemeTrainingRuntime::stop() {
|
||||
for (auto &track : tracks_) {
|
||||
track = stop_scheme_track(std::move(track));
|
||||
}
|
||||
}
|
||||
|
||||
void SchemeTrainingRuntime::tick(const TrackConfig &config, float delta_s) {
|
||||
for (auto &track : tracks_) {
|
||||
track = tick_scheme_track(config, std::move(track), delta_s);
|
||||
}
|
||||
}
|
||||
|
||||
TrackStateReportCollection SchemeTrainingRuntime::state_collection() const {
|
||||
TrackStateReportCollection collection;
|
||||
collection.states.reserve(tracks_.size());
|
||||
for (const auto &track : tracks_) {
|
||||
collection.states.push_back(scheme_track_report(track));
|
||||
}
|
||||
return collection;
|
||||
}
|
||||
|
||||
TrackSchemeMgrRead SchemeTrainingRuntime::scheme_status() const {
|
||||
TrackSchemeMgrRead status;
|
||||
status.scheme_status.reserve(tracks_.size());
|
||||
for (const auto &track : tracks_) {
|
||||
const auto info = scheme_track_info(track);
|
||||
status.scheme_status.push_back(TrackSchemeMgrRead::Status{
|
||||
.id = info.id,
|
||||
.segment_count = info.num_segments,
|
||||
.kind = info.kind,
|
||||
});
|
||||
}
|
||||
return status;
|
||||
}
|
||||
|
||||
expected<unit, TrackError>
|
||||
SchemeTrainingRuntime::render_to(const TrackConfig &config, TrackRenderSink sink) const {
|
||||
if (!config.verify() || config.line_leds_num == 0) {
|
||||
return unexpected<TrackError>{TrackError::invalid_arg};
|
||||
}
|
||||
|
||||
for (const auto &track : tracks_) {
|
||||
const auto plan = make_track_render_plan(
|
||||
config,
|
||||
scheme_track_info(track),
|
||||
scheme_track_report(track));
|
||||
const auto err = apply_render_plan(sink, plan);
|
||||
if (err != TrackError::ok) {
|
||||
return unexpected<TrackError>{err};
|
||||
}
|
||||
}
|
||||
|
||||
return {};
|
||||
}
|
||||
|
||||
expected<std::vector<Color>, TrackError>
|
||||
SchemeTrainingRuntime::render_pixels(const TrackConfig &config) const {
|
||||
MemoryStrip strip(config.line_leds_num);
|
||||
const auto rendered = render_to(config, make_memory_strip_sink(strip));
|
||||
if (!rendered) {
|
||||
return unexpected<TrackError>{rendered.error()};
|
||||
}
|
||||
|
||||
return std::vector<Color>{strip.pixels().begin(), strip.pixels().end()};
|
||||
}
|
||||
|
||||
} // namespace track_core
|
||||
Reference in New Issue
Block a user