Initial standalone track core
This commit is contained in:
@@ -0,0 +1,330 @@
|
||||
#pragma once
|
||||
|
||||
#include <algorithm>
|
||||
#include <chrono>
|
||||
#include <cstdint>
|
||||
#include <expected>
|
||||
#include <limits>
|
||||
#include <optional>
|
||||
#include <span>
|
||||
#include <string>
|
||||
#include <variant>
|
||||
#include <vector>
|
||||
|
||||
namespace track_core {
|
||||
|
||||
template <typename T, typename E>
|
||||
using expected = std::expected<T, E>;
|
||||
|
||||
template <typename E>
|
||||
using unexpected = std::unexpected<E>;
|
||||
|
||||
using unit = std::monostate;
|
||||
|
||||
enum class TrackError : int {
|
||||
ok = 0,
|
||||
invalid_arg,
|
||||
invalid_size,
|
||||
invalid_state,
|
||||
not_supported,
|
||||
range,
|
||||
};
|
||||
|
||||
enum class TrackDrawKind : std::uint8_t {
|
||||
circular = 0,
|
||||
linear = 1,
|
||||
};
|
||||
|
||||
enum class SchemeKind : std::uint8_t {
|
||||
speed_input_mileage_segmented_time_free = 0,
|
||||
mileage_input_time_segmented_speed_free = 1,
|
||||
speed_input_time_segmented_mileage_free = 2,
|
||||
repeated_speed_input_mileage_segmentation_input_time_segmented = 3,
|
||||
};
|
||||
|
||||
enum class AccelerationProfile : std::uint8_t {
|
||||
instant = 0,
|
||||
smooth = 1,
|
||||
};
|
||||
|
||||
enum class TrackState : std::uint8_t {
|
||||
stop = 0,
|
||||
run = 1,
|
||||
test_rainbow = 2,
|
||||
test_blink = 3,
|
||||
};
|
||||
|
||||
enum class TrackControllerMode : std::uint8_t {
|
||||
scheme = 0,
|
||||
pid_hr = 1,
|
||||
};
|
||||
|
||||
enum class TrackPidStageKind : std::uint8_t {
|
||||
none = 0,
|
||||
constant = 1,
|
||||
pid = 2,
|
||||
};
|
||||
|
||||
struct Color {
|
||||
std::uint8_t r{};
|
||||
std::uint8_t g{};
|
||||
std::uint8_t b{};
|
||||
|
||||
constexpr Color() = default;
|
||||
constexpr Color(std::uint8_t red, std::uint8_t green, std::uint8_t blue)
|
||||
: r(red), g(green), b(blue) {}
|
||||
constexpr explicit Color(std::uint32_t value)
|
||||
: r(static_cast<std::uint8_t>((value >> 16U) & 0xFFU)),
|
||||
g(static_cast<std::uint8_t>((value >> 8U) & 0xFFU)),
|
||||
b(static_cast<std::uint8_t>(value & 0xFFU)) {}
|
||||
|
||||
[[nodiscard]]
|
||||
constexpr bool operator==(const Color &) const = default;
|
||||
|
||||
[[nodiscard]]
|
||||
constexpr explicit operator std::uint32_t() const {
|
||||
return (static_cast<std::uint32_t>(r) << 16U) |
|
||||
(static_cast<std::uint32_t>(g) << 8U) |
|
||||
static_cast<std::uint32_t>(b);
|
||||
}
|
||||
|
||||
[[nodiscard]]
|
||||
std::string hex() const;
|
||||
|
||||
[[nodiscard]]
|
||||
static constexpr Color black() { return {0, 0, 0}; }
|
||||
[[nodiscard]]
|
||||
static constexpr Color red() { return {255, 0, 0}; }
|
||||
[[nodiscard]]
|
||||
static constexpr Color orange() { return {255, 165, 0}; }
|
||||
[[nodiscard]]
|
||||
static constexpr Color yellow() { return {255, 255, 0}; }
|
||||
[[nodiscard]]
|
||||
static constexpr Color green() { return {0, 255, 0}; }
|
||||
[[nodiscard]]
|
||||
static constexpr Color cyan() { return {0, 255, 255}; }
|
||||
[[nodiscard]]
|
||||
static constexpr Color blue() { return {0, 0, 255}; }
|
||||
[[nodiscard]]
|
||||
static constexpr Color indigo() { return {75, 0, 130}; }
|
||||
[[nodiscard]]
|
||||
static constexpr Color violet() { return {238, 130, 238}; }
|
||||
[[nodiscard]]
|
||||
static constexpr Color white() { return {255, 255, 255}; }
|
||||
};
|
||||
|
||||
struct TrackConfig {
|
||||
TrackDrawKind draw_kind{TrackDrawKind::circular};
|
||||
float line_length_m{0.0F};
|
||||
float active_line_length_m{0.0F};
|
||||
float head_offset_m{0.0F};
|
||||
std::uint16_t line_leds_num{0};
|
||||
|
||||
[[nodiscard]]
|
||||
float led_distance() const {
|
||||
return line_leds_num > 0 ? line_length_m / static_cast<float>(line_leds_num) : 0.0F;
|
||||
}
|
||||
|
||||
[[nodiscard]]
|
||||
bool verify() const {
|
||||
if (line_length_m <= 0.0F || active_line_length_m <= 0.0F) {
|
||||
return false;
|
||||
}
|
||||
if (active_line_length_m > line_length_m) {
|
||||
return false;
|
||||
}
|
||||
if (head_offset_m >= line_length_m) {
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
[[nodiscard]]
|
||||
static TrackConfig default_config() {
|
||||
return {
|
||||
.draw_kind = TrackDrawKind::circular,
|
||||
.line_length_m = 400.0F,
|
||||
.active_line_length_m = 10.0F,
|
||||
.head_offset_m = 0.0F,
|
||||
.line_leds_num = 400,
|
||||
};
|
||||
}
|
||||
};
|
||||
|
||||
struct TrackInfo {
|
||||
SchemeKind kind{SchemeKind::speed_input_time_segmented_mileage_free};
|
||||
Color color{Color::white()};
|
||||
std::uint8_t id{};
|
||||
bool is_running{};
|
||||
std::uint8_t num_segments{};
|
||||
};
|
||||
|
||||
struct TrackReport {
|
||||
std::uint8_t id{};
|
||||
TrackState state{TrackState::stop};
|
||||
float mileage_m{};
|
||||
float speed_m_s{};
|
||||
std::uint32_t time_elapsed_ms{};
|
||||
};
|
||||
|
||||
struct TrackStateReportCollection {
|
||||
std::vector<TrackReport> states;
|
||||
};
|
||||
|
||||
struct TrackSchemeMgrRead {
|
||||
struct Status {
|
||||
std::uint8_t id{};
|
||||
std::uint8_t segment_count{};
|
||||
SchemeKind kind{SchemeKind::speed_input_time_segmented_mileage_free};
|
||||
};
|
||||
|
||||
std::vector<Status> scheme_status;
|
||||
};
|
||||
|
||||
struct SMSegment {
|
||||
static constexpr float speed_lsb = 10.0F / 255.0F;
|
||||
static constexpr std::size_t raw_size = 3;
|
||||
|
||||
float speed_m_s{};
|
||||
std::uint16_t mileage_from_start_m{};
|
||||
};
|
||||
|
||||
struct MTSegment {
|
||||
std::uint16_t mileage_to_travel_this_segment_m{};
|
||||
std::uint16_t time_since_start_s{};
|
||||
};
|
||||
|
||||
struct STSegment {
|
||||
static constexpr float speed_lsb = 10.0F / 255.0F;
|
||||
static constexpr std::size_t raw_size = 3;
|
||||
|
||||
float speed_m_s{};
|
||||
std::uint16_t time_since_start_s{};
|
||||
};
|
||||
|
||||
struct RepeatedSMSegment {
|
||||
std::vector<SMSegment> speed_mileage_segments;
|
||||
std::uint16_t time_since_start_s{};
|
||||
};
|
||||
|
||||
struct TrackPidFineTune {
|
||||
std::uint8_t band_plus{};
|
||||
std::uint8_t band_minus{};
|
||||
float gain_scale{0.0F};
|
||||
};
|
||||
|
||||
struct TrackPidSpeedSuppression {
|
||||
float ratio_min{0.1F};
|
||||
float sigma_m{1.0F};
|
||||
};
|
||||
|
||||
struct TrackPidSegment {
|
||||
static constexpr float default_kp = 0.0457F;
|
||||
static constexpr float default_ki = 0.001464F;
|
||||
static constexpr float default_kd = 0.0F;
|
||||
static constexpr float default_slew_rate_limit_m_s = 0.225F;
|
||||
|
||||
std::uint16_t duration_s{0};
|
||||
float min_speed_m_s{0.0F};
|
||||
float max_speed_m_s{0.0F};
|
||||
float kp{0.0F};
|
||||
float ki{1.0F};
|
||||
float kd{0.0F};
|
||||
float slew_rate_limit{0.0F};
|
||||
std::optional<TrackPidFineTune> fine_tune;
|
||||
|
||||
[[nodiscard]]
|
||||
static TrackPidSegment default_segment() {
|
||||
return {
|
||||
.duration_s = 300,
|
||||
.min_speed_m_s = 0.0F,
|
||||
.max_speed_m_s = 4.0F,
|
||||
.kp = default_kp,
|
||||
.ki = default_ki,
|
||||
.kd = default_kd,
|
||||
.slew_rate_limit = default_slew_rate_limit_m_s,
|
||||
.fine_tune = std::nullopt,
|
||||
};
|
||||
}
|
||||
};
|
||||
|
||||
struct TrackConstantSegment {
|
||||
std::uint16_t duration_s{0};
|
||||
float speed_m_s{0.0F};
|
||||
};
|
||||
|
||||
struct TrackPidSchema {
|
||||
using variant_type = std::variant<TrackPidSegment, TrackConstantSegment>;
|
||||
|
||||
variant_type segment{TrackConstantSegment{}};
|
||||
|
||||
[[nodiscard]]
|
||||
TrackPidStageKind kind() const {
|
||||
return std::holds_alternative<TrackPidSegment>(segment)
|
||||
? TrackPidStageKind::pid
|
||||
: TrackPidStageKind::constant;
|
||||
}
|
||||
};
|
||||
|
||||
struct TrackPidConfig {
|
||||
std::uint8_t band_id{0};
|
||||
std::uint8_t target_hr_bpm{120};
|
||||
std::uint8_t deadzone_bpm{3};
|
||||
bool preemptive_pid_activation{false};
|
||||
std::optional<TrackPidSpeedSuppression> speed_suppression;
|
||||
std::vector<TrackPidSchema> schemas;
|
||||
|
||||
[[nodiscard]]
|
||||
bool empty() const {
|
||||
return schemas.empty();
|
||||
}
|
||||
|
||||
[[nodiscard]]
|
||||
static TrackPidConfig default_config() {
|
||||
TrackPidConfig config;
|
||||
config.schemas.push_back(TrackPidSchema{TrackPidSegment::default_segment()});
|
||||
return config;
|
||||
}
|
||||
|
||||
[[nodiscard]]
|
||||
expected<unit, TrackError> validate(bool allow_empty = false) const;
|
||||
};
|
||||
|
||||
struct TrackPidSetTargetHr {
|
||||
std::uint8_t target_hr_bpm{120};
|
||||
};
|
||||
|
||||
struct TrackPidSetTuning {
|
||||
float min_speed_m_s{0.0F};
|
||||
float max_speed_m_s{0.0F};
|
||||
float kp{0.0F};
|
||||
float ki{1.0F};
|
||||
float kd{0.0F};
|
||||
float slew_rate_limit{0.0F};
|
||||
std::optional<TrackPidFineTune> fine_tune;
|
||||
|
||||
[[nodiscard]]
|
||||
expected<unit, TrackError> validate() const;
|
||||
void apply_to(TrackPidSegment &segment) const;
|
||||
};
|
||||
|
||||
struct TrackPidRuntimeCommand {
|
||||
std::variant<unit, TrackPidSetTargetHr, TrackPidSetTuning> command{unit{}};
|
||||
};
|
||||
|
||||
struct TrackPidStatus {
|
||||
std::uint8_t band_id{};
|
||||
bool band_is_active{};
|
||||
bool is_heart_rate_valid{};
|
||||
std::uint8_t heart_rate_bpm{};
|
||||
std::uint16_t step_count{};
|
||||
std::uint8_t active_segment_index{};
|
||||
TrackPidStageKind active_segment_kind{TrackPidStageKind::none};
|
||||
float effective_speed_m_s{};
|
||||
std::uint32_t remaining_program_ms{};
|
||||
float base_speed_m_s{};
|
||||
};
|
||||
|
||||
using clock = std::chrono::steady_clock;
|
||||
|
||||
} // namespace track_core
|
||||
Reference in New Issue
Block a user