Initial standalone track core
This commit is contained in:
@@ -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
|
||||
Reference in New Issue
Block a user