1005e50be0
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.
1107 lines
32 KiB
C++
1107 lines
32 KiB
C++
#ifndef E32F663C_9C5C_4317_882A_E6457E88B576
|
|
#define E32F663C_9C5C_4317_882A_E6457E88B576
|
|
/**
|
|
* @brief ESP-IDF/protobuf compatibility model for the Track BLE service
|
|
*/
|
|
|
|
#include <span>
|
|
#include <algorithm>
|
|
#include <cstdint>
|
|
#include <sys/types.h>
|
|
#include <vector>
|
|
#include <variant>
|
|
#include <format>
|
|
#include <cmath>
|
|
#include <functional>
|
|
#include <cstring>
|
|
#include <cassert>
|
|
#include <expected>
|
|
#include <optional>
|
|
#include <string>
|
|
#include <esp_log.h>
|
|
#include "app_track.pb.h"
|
|
#include "app_const.hpp"
|
|
#include "app_color.hpp"
|
|
#include "app_utils.hpp"
|
|
#include "esp_err.h"
|
|
#include "app_clock.hpp"
|
|
#include "esp_log_level.h"
|
|
|
|
#ifndef APP_TRACK_C_ARRAY_SIZE
|
|
/**
|
|
* @brief A macro to calculate the size of a C-style array
|
|
* @param arr The array to calculate the size of
|
|
*/
|
|
#define APP_TRACK_C_ARRAY_SIZE(arr) (sizeof(arr) / sizeof((arr)[0]))
|
|
#endif
|
|
|
|
#ifndef __packed
|
|
#if defined(__GNUC__) || defined(__clang__)
|
|
#define __packed __attribute__((__packed__))
|
|
#elif defined(_MSC_VER)
|
|
#define __packed __declspec(align(1))
|
|
#else
|
|
#error "Unsupported compiler for __packed attribute"
|
|
#endif // __GNUC__ || __clang__
|
|
#endif // __packed
|
|
|
|
namespace app::track {
|
|
template <typename T, typename E>
|
|
using expected = std::expected<T, E>;
|
|
|
|
template <typename E>
|
|
using unexpected = std::unexpected<E>;
|
|
|
|
template <typename T>
|
|
using optional = std::optional<T>;
|
|
|
|
using unit = std::monostate;
|
|
|
|
using error_t = esp_err_t;
|
|
|
|
using SchemeKind = track_app_TrackSchemeKind;
|
|
using AccelerationProfile = track_app_TrackAccelerationProfile;
|
|
|
|
|
|
struct Color {
|
|
using proto_type = track_app_Color;
|
|
static inline const pb_msgdesc_t *pb_fields = &track_app_Color_msg;
|
|
|
|
|
|
union {
|
|
struct {
|
|
uint8_t r;
|
|
uint8_t g;
|
|
uint8_t b;
|
|
};
|
|
uint8_t bytes[3];
|
|
} inner{};
|
|
|
|
Color() = default;
|
|
constexpr Color(uint8_t r, uint8_t g, uint8_t b) : inner{.r = r, .g = g, .b = b} {}
|
|
constexpr Color(uint32_t value) {
|
|
inner.r = (value >> 16) & 0xFF;
|
|
inner.g = (value >> 8) & 0xFF;
|
|
inner.b = value & 0xFF;
|
|
}
|
|
constexpr Color(std::span<const uint8_t, 3> bytes) {
|
|
inner.r = bytes[0];
|
|
inner.g = bytes[1];
|
|
inner.b = bytes[2];
|
|
}
|
|
|
|
constexpr bool operator==(const Color &other) const {
|
|
return inner.r == other.inner.r &&
|
|
inner.g == other.inner.g &&
|
|
inner.b == other.inner.b;
|
|
}
|
|
|
|
constexpr bool operator!=(const Color &other) const {
|
|
return not(*this == other);
|
|
}
|
|
|
|
operator uint32_t() const {
|
|
return ((uint32_t)inner.r << 16) | ((uint32_t)inner.g << 8) | inner.b;
|
|
}
|
|
|
|
explicit operator std::string() const {
|
|
return std::format("#{:02X}{:02X}{:02X}",
|
|
static_cast<uint8_t>(inner.r),
|
|
static_cast<uint8_t>(inner.g),
|
|
static_cast<uint8_t>(inner.b));
|
|
}
|
|
|
|
[[nodiscard]]
|
|
std::string string() const {
|
|
return static_cast<std::string>(*this);
|
|
}
|
|
|
|
static Color from_proto(const proto_type &proto) {
|
|
return {proto.r, proto.g, proto.b};
|
|
}
|
|
|
|
static Color black() {
|
|
return {0, 0, 0};
|
|
}
|
|
|
|
static Color green() {
|
|
return {0, 255, 0};
|
|
}
|
|
|
|
static Color red() {
|
|
return {255, 0, 0};
|
|
}
|
|
|
|
static Color blue() {
|
|
return {0, 0, 255};
|
|
}
|
|
|
|
static Color white() {
|
|
return {255, 255, 255};
|
|
}
|
|
|
|
[[nodiscard]]
|
|
proto_type to_proto() const {
|
|
proto_type proto{};
|
|
proto.r = inner.r;
|
|
proto.g = inner.g;
|
|
proto.b = inner.b;
|
|
return proto;
|
|
}
|
|
|
|
[[nodiscard]]
|
|
app::utils::Color to_app_color() const {
|
|
return app::utils::Color{inner.r, inner.g, inner.b};
|
|
}
|
|
};
|
|
|
|
struct TrackInfo {
|
|
SchemeKind kind{track_app_TrackSchemeKind_SPEED_INPUT_TIME_SEGMENTED_MILEAGE_FREE};
|
|
Color color{Color::white()};
|
|
uint8_t id{};
|
|
bool is_running{};
|
|
uint8_t num_segments{};
|
|
};
|
|
|
|
struct TrackConfig {
|
|
using proto_type = track_app_TrackConfig;
|
|
static inline const pb_msgdesc_t *pb_fields = &track_app_TrackConfig_msg;
|
|
using TrackDrawKind = track_app_TrackDrawKind;
|
|
|
|
/** properties */
|
|
TrackDrawKind draw_kind{track_app_TrackDrawKind_CIRCULAR};
|
|
float line_length_m{0.0F};
|
|
float active_line_length_m{0.0F};
|
|
float head_offset_m{0.0F};
|
|
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;
|
|
}
|
|
|
|
static TrackConfig from_proto(const proto_type &proto) {
|
|
TrackConfig config;
|
|
config.draw_kind = static_cast<TrackDrawKind>(proto.draw_kind);
|
|
config.line_length_m = proto.line_length_m;
|
|
config.active_line_length_m = proto.active_line_length_m;
|
|
config.head_offset_m = proto.head_offset_m;
|
|
config.line_leds_num = proto.line_leds_num;
|
|
return config;
|
|
}
|
|
|
|
/**
|
|
* @brief a reasonable default configuration
|
|
*/
|
|
static TrackConfig Default() {
|
|
return {track_app_TrackDrawKind_CIRCULAR,
|
|
400.0F,
|
|
10.0F,
|
|
0.0F,
|
|
400};
|
|
}
|
|
|
|
[[nodiscard]]
|
|
bool verify() const {
|
|
bool length_non_zero_ok = line_length_m > 0.0F && active_line_length_m > 0.0F;
|
|
if (not length_non_zero_ok) {
|
|
return false;
|
|
}
|
|
// active line length cannot exceed total line length
|
|
if (active_line_length_m > line_length_m) {
|
|
return false;
|
|
}
|
|
// head offset must be within the line length
|
|
if (head_offset_m >= line_length_m) {
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
[[nodiscard]]
|
|
proto_type to_proto() const {
|
|
proto_type proto{};
|
|
proto.draw_kind = static_cast<track_app_TrackDrawKind>(draw_kind);
|
|
proto.line_length_m = line_length_m;
|
|
proto.active_line_length_m = active_line_length_m;
|
|
proto.head_offset_m = head_offset_m;
|
|
proto.line_leds_num = line_leds_num;
|
|
return proto;
|
|
}
|
|
|
|
void log(const char *tag, esp_log_level_t level) const;
|
|
};
|
|
|
|
|
|
struct TrackSchemeMgrRead {
|
|
using proto_type = track_app_TrackSchemeMgrRead;
|
|
static inline const pb_msgdesc_t *pb_fields = &track_app_TrackSchemeMgrRead_msg;
|
|
static constexpr size_t MAX_SCHEMES = APP_TRACK_C_ARRAY_SIZE(
|
|
track_app_TrackSchemeMgrRead::scheme_status);
|
|
|
|
struct Status {
|
|
using proto_type = track_app_TrackSchemeMgrStatus;
|
|
static inline const pb_msgdesc_t *pb_fields = &track_app_TrackSchemeMgrStatus_msg;
|
|
|
|
uint8_t id = 0;
|
|
uint8_t segment_count = 0;
|
|
|
|
static Status from_proto(const proto_type &proto) {
|
|
return {proto.id, proto.segment_count};
|
|
}
|
|
|
|
[[nodiscard]]
|
|
proto_type to_proto() const {
|
|
proto_type proto{};
|
|
proto.id = id;
|
|
proto.segment_count = segment_count;
|
|
return proto;
|
|
}
|
|
};
|
|
|
|
static TrackSchemeMgrRead from_proto(const proto_type &proto) {
|
|
TrackSchemeMgrRead collection;
|
|
collection.scheme_status.reserve(proto.scheme_status_count);
|
|
for (pb_size_t i = 0; i < proto.scheme_status_count; ++i) {
|
|
collection.scheme_status.push_back(Status::from_proto(proto.scheme_status[i]));
|
|
}
|
|
return collection;
|
|
}
|
|
|
|
[[nodiscard]]
|
|
proto_type to_proto() const {
|
|
proto_type proto = track_app_TrackSchemeMgrRead_init_default;
|
|
proto.scheme_status_count = std::min(scheme_status.size(), MAX_SCHEMES);
|
|
for (size_t i = 0; i < proto.scheme_status_count; ++i) {
|
|
proto.scheme_status[i] = scheme_status[i].to_proto();
|
|
}
|
|
return proto;
|
|
}
|
|
|
|
/** properties */
|
|
std::vector<Status> scheme_status;
|
|
};
|
|
|
|
using TrackState = track_app_TrackState;
|
|
using TrackControllerMode = track_app_TrackControllerMode;
|
|
using TrackPidStageKind = track_app_TrackPidStageKind;
|
|
|
|
constexpr float PID_COMPAT_OUTPUT_SCALE_MAX_M_S = 10.0F;
|
|
|
|
struct TrackPidFineTune {
|
|
using proto_type = track_app_TrackPidFineTune;
|
|
static inline const pb_msgdesc_t *pb_fields = &track_app_TrackPidFineTune_msg;
|
|
|
|
uint8_t band_plus{0};
|
|
uint8_t band_minus{0};
|
|
float gain_scale{0.0F};
|
|
|
|
static TrackPidFineTune from_proto(const proto_type &proto) {
|
|
return {
|
|
static_cast<uint8_t>(proto.band_plus),
|
|
static_cast<uint8_t>(proto.band_minus),
|
|
proto.gain_scale,
|
|
};
|
|
}
|
|
|
|
[[nodiscard]]
|
|
proto_type to_proto() const {
|
|
proto_type proto = track_app_TrackPidFineTune_init_default;
|
|
proto.band_plus = band_plus;
|
|
proto.band_minus = band_minus;
|
|
proto.gain_scale = gain_scale;
|
|
return proto;
|
|
}
|
|
};
|
|
|
|
struct TrackPidSpeedSuppression {
|
|
using proto_type = track_app_TrackPidSpeedSuppression;
|
|
static inline const pb_msgdesc_t *pb_fields = &track_app_TrackPidSpeedSuppression_msg;
|
|
|
|
float ratio_min{0.1F};
|
|
float sigma_m{1.0F};
|
|
|
|
static TrackPidSpeedSuppression from_proto(const proto_type &proto) {
|
|
return {
|
|
.ratio_min = proto.ratio_min,
|
|
.sigma_m = proto.sigma_m,
|
|
};
|
|
}
|
|
|
|
[[nodiscard]]
|
|
proto_type to_proto() const {
|
|
proto_type proto = track_app_TrackPidSpeedSuppression_init_default;
|
|
proto.ratio_min = ratio_min;
|
|
proto.sigma_m = sigma_m;
|
|
return proto;
|
|
}
|
|
};
|
|
|
|
struct TrackPidSegment {
|
|
using proto_type = track_app_TrackPidSegment;
|
|
static inline const pb_msgdesc_t *pb_fields = &track_app_TrackPidSegment_msg;
|
|
|
|
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};
|
|
optional<TrackPidFineTune> fine_tune;
|
|
|
|
static TrackPidSegment Default() {
|
|
return {
|
|
.duration_s = 300,
|
|
.min_speed_m_s = 0.0F,
|
|
.max_speed_m_s = 4.0F,
|
|
.kp = app::global::constants::DEFAULT_PID_Kp,
|
|
.ki = app::global::constants::DEFAULT_PID_Ki,
|
|
.kd = app::global::constants::DEFAULT_PID_Kd,
|
|
.slew_rate_limit = app::global::constants::DEFAULT_PID_SLEW_RATE_LIMIT_M_S,
|
|
.fine_tune = std::nullopt,
|
|
};
|
|
}
|
|
|
|
static TrackPidSegment from_proto(const proto_type &proto) {
|
|
TrackPidSegment segment;
|
|
segment.duration_s = static_cast<uint16_t>(proto.duration_s);
|
|
segment.min_speed_m_s = proto.min_speed_m_s;
|
|
segment.max_speed_m_s = proto.max_speed_m_s;
|
|
segment.kp = proto.kp;
|
|
segment.ki = proto.ki;
|
|
segment.kd = proto.kd;
|
|
segment.slew_rate_limit = proto.slew_rate_limit;
|
|
if (proto.has_fine_tune) {
|
|
segment.fine_tune = TrackPidFineTune::from_proto(proto.fine_tune);
|
|
}
|
|
return segment;
|
|
}
|
|
|
|
[[nodiscard]]
|
|
proto_type to_proto() const {
|
|
proto_type proto = track_app_TrackPidSegment_init_default;
|
|
proto.duration_s = duration_s;
|
|
proto.min_speed_m_s = min_speed_m_s;
|
|
proto.max_speed_m_s = max_speed_m_s;
|
|
proto.kp = kp;
|
|
proto.ki = ki;
|
|
proto.kd = kd;
|
|
proto.slew_rate_limit = slew_rate_limit;
|
|
if (fine_tune) {
|
|
proto.has_fine_tune = true;
|
|
proto.fine_tune = fine_tune->to_proto();
|
|
}
|
|
return proto;
|
|
}
|
|
};
|
|
|
|
struct TrackConstantSegment {
|
|
using proto_type = track_app_TrackConstantSegment;
|
|
static inline const pb_msgdesc_t *pb_fields = &track_app_TrackConstantSegment_msg;
|
|
|
|
uint16_t duration_s{0};
|
|
float speed_m_s{0.0F};
|
|
|
|
static TrackConstantSegment from_proto(const proto_type &proto) {
|
|
return {
|
|
.duration_s = static_cast<uint16_t>(proto.duration_s),
|
|
.speed_m_s = proto.speed_m_s,
|
|
};
|
|
}
|
|
|
|
[[nodiscard]]
|
|
proto_type to_proto() const {
|
|
proto_type proto = track_app_TrackConstantSegment_init_default;
|
|
proto.duration_s = duration_s;
|
|
proto.speed_m_s = speed_m_s;
|
|
return proto;
|
|
}
|
|
};
|
|
|
|
struct TrackPidSchema {
|
|
using proto_type = track_app_TrackPidSchema;
|
|
static inline const pb_msgdesc_t *pb_fields = &track_app_TrackPidSchema_msg;
|
|
using variant_type = std::variant<TrackPidSegment, TrackConstantSegment>;
|
|
|
|
variant_type segment{TrackConstantSegment{}};
|
|
|
|
static expected<TrackPidSchema, error_t> try_from_proto(const proto_type &proto) {
|
|
switch (proto.which_segment) {
|
|
case track_app_TrackPidSchema_pid_tag:
|
|
return TrackPidSchema{TrackPidSegment::from_proto(proto.segment.pid)};
|
|
case track_app_TrackPidSchema_constant_tag:
|
|
return TrackPidSchema{TrackConstantSegment::from_proto(proto.segment.constant)};
|
|
default:
|
|
return unexpected<error_t>{ESP_ERR_INVALID_ARG};
|
|
}
|
|
}
|
|
|
|
[[nodiscard]]
|
|
proto_type to_proto() const {
|
|
proto_type proto = track_app_TrackPidSchema_init_default;
|
|
std::visit(app::utils::overloads{
|
|
[&](const TrackPidSegment &pid) {
|
|
proto.which_segment = track_app_TrackPidSchema_pid_tag;
|
|
proto.segment.pid = pid.to_proto();
|
|
},
|
|
[&](const TrackConstantSegment &constant) {
|
|
proto.which_segment = track_app_TrackPidSchema_constant_tag;
|
|
proto.segment.constant = constant.to_proto();
|
|
}},
|
|
segment);
|
|
return proto;
|
|
}
|
|
|
|
[[nodiscard]]
|
|
TrackPidStageKind kind() const {
|
|
return std::holds_alternative<TrackPidSegment>(segment)
|
|
? track_app_TrackPidStageKind_PID
|
|
: track_app_TrackPidStageKind_CONSTANT;
|
|
}
|
|
};
|
|
|
|
struct TrackPidConfig {
|
|
using proto_type = track_app_TrackPidConfig;
|
|
static inline const pb_msgdesc_t *pb_fields = &track_app_TrackPidConfig_msg;
|
|
|
|
uint8_t band_id{0};
|
|
uint8_t target_hr_bpm{120};
|
|
uint8_t deadzone_bpm{3};
|
|
bool preemptive_pid_activation{false};
|
|
optional<TrackPidSpeedSuppression> speed_suppression;
|
|
std::vector<TrackPidSchema> schemas{};
|
|
|
|
static TrackPidConfig Default() {
|
|
TrackPidConfig config;
|
|
config.schemas.push_back(TrackPidSchema{TrackPidSegment::Default()});
|
|
return config;
|
|
}
|
|
|
|
static TrackPidConfig from_proto(const proto_type &proto) {
|
|
auto config = try_from_proto(proto);
|
|
if (config) {
|
|
return *config;
|
|
}
|
|
return Default();
|
|
}
|
|
|
|
[[nodiscard]]
|
|
bool empty() const {
|
|
return schemas.empty();
|
|
}
|
|
|
|
[[nodiscard]]
|
|
static expected<TrackPidConfig, error_t> try_from_proto(const proto_type &proto) {
|
|
return try_from_proto_impl(proto, false);
|
|
}
|
|
|
|
[[nodiscard]]
|
|
static expected<TrackPidConfig, error_t> try_from_proto_allow_empty(const proto_type &proto) {
|
|
return try_from_proto_impl(proto, true);
|
|
}
|
|
|
|
private:
|
|
[[nodiscard]]
|
|
static expected<TrackPidConfig, error_t> try_from_proto_impl(const proto_type &proto, bool allow_empty) {
|
|
TrackPidConfig config;
|
|
config.band_id = static_cast<uint8_t>(proto.band_id);
|
|
config.target_hr_bpm = static_cast<uint8_t>(proto.target_hr_bpm);
|
|
config.deadzone_bpm = static_cast<uint8_t>(proto.deadzone_bpm);
|
|
config.preemptive_pid_activation = proto.preemptive_pid_activation;
|
|
if (proto.has_speed_suppression) {
|
|
config.speed_suppression = TrackPidSpeedSuppression::from_proto(proto.speed_suppression);
|
|
}
|
|
config.schemas.reserve(proto.schemas_count);
|
|
for (pb_size_t i = 0; i < proto.schemas_count; ++i) {
|
|
auto schema = TrackPidSchema::try_from_proto(proto.schemas[i]);
|
|
if (!schema) {
|
|
return unexpected<error_t>{schema.error()};
|
|
}
|
|
config.schemas.push_back(*schema);
|
|
}
|
|
auto validation = config.validate(allow_empty);
|
|
if (!validation) {
|
|
return unexpected<error_t>{validation.error()};
|
|
}
|
|
return {std::move(config)};
|
|
}
|
|
|
|
public:
|
|
[[nodiscard]]
|
|
proto_type to_proto() const {
|
|
proto_type proto = track_app_TrackPidConfig_init_default;
|
|
proto.band_id = band_id;
|
|
proto.target_hr_bpm = target_hr_bpm;
|
|
proto.deadzone_bpm = deadzone_bpm;
|
|
proto.preemptive_pid_activation = preemptive_pid_activation;
|
|
if (speed_suppression) {
|
|
proto.has_speed_suppression = true;
|
|
proto.speed_suppression = speed_suppression->to_proto();
|
|
}
|
|
proto.schemas_count = std::min(schemas.size(), APP_TRACK_C_ARRAY_SIZE(proto.schemas));
|
|
for (size_t i = 0; i < proto.schemas_count; ++i) {
|
|
proto.schemas[i] = schemas[i].to_proto();
|
|
}
|
|
return proto;
|
|
}
|
|
|
|
[[nodiscard]]
|
|
expected<unit, error_t> validate(bool allow_empty = false) 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<error_t>{ESP_ERR_INVALID_ARG};
|
|
}
|
|
if (schemas.empty()) {
|
|
if (allow_empty) {
|
|
return {};
|
|
}
|
|
return unexpected<error_t>{ESP_ERR_INVALID_ARG};
|
|
}
|
|
for (const auto &schema : schemas) {
|
|
auto ok = std::visit(app::utils::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<error_t>{ESP_ERR_INVALID_ARG};
|
|
}
|
|
}
|
|
return {};
|
|
}
|
|
};
|
|
|
|
struct TrackPidSetTargetHr {
|
|
using proto_type = track_app_TrackPidSetTargetHr;
|
|
static inline const pb_msgdesc_t *pb_fields = &track_app_TrackPidSetTargetHr_msg;
|
|
|
|
uint8_t target_hr_bpm{120};
|
|
|
|
static TrackPidSetTargetHr from_proto(const proto_type &proto) {
|
|
return {
|
|
.target_hr_bpm = static_cast<uint8_t>(proto.target_hr_bpm),
|
|
};
|
|
}
|
|
|
|
[[nodiscard]]
|
|
proto_type to_proto() const {
|
|
proto_type proto = track_app_TrackPidSetTargetHr_init_default;
|
|
proto.target_hr_bpm = target_hr_bpm;
|
|
return proto;
|
|
}
|
|
};
|
|
|
|
struct TrackPidSetTuning {
|
|
using proto_type = track_app_TrackPidSetTuning;
|
|
static inline const pb_msgdesc_t *pb_fields = &track_app_TrackPidSetTuning_msg;
|
|
|
|
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};
|
|
optional<TrackPidFineTune> fine_tune;
|
|
|
|
static TrackPidSetTuning from_proto(const proto_type &proto) {
|
|
TrackPidSetTuning tuning;
|
|
tuning.min_speed_m_s = proto.min_speed_m_s;
|
|
tuning.max_speed_m_s = proto.max_speed_m_s;
|
|
tuning.kp = proto.kp;
|
|
tuning.ki = proto.ki;
|
|
tuning.kd = proto.kd;
|
|
tuning.slew_rate_limit = proto.slew_rate_limit;
|
|
if (proto.has_fine_tune) {
|
|
tuning.fine_tune = TrackPidFineTune::from_proto(proto.fine_tune);
|
|
}
|
|
return tuning;
|
|
}
|
|
|
|
[[nodiscard]]
|
|
proto_type to_proto() const {
|
|
proto_type proto = track_app_TrackPidSetTuning_init_default;
|
|
proto.min_speed_m_s = min_speed_m_s;
|
|
proto.max_speed_m_s = max_speed_m_s;
|
|
proto.kp = kp;
|
|
proto.ki = ki;
|
|
proto.kd = kd;
|
|
proto.slew_rate_limit = slew_rate_limit;
|
|
if (fine_tune) {
|
|
proto.has_fine_tune = true;
|
|
proto.fine_tune = fine_tune->to_proto();
|
|
}
|
|
return proto;
|
|
}
|
|
|
|
[[nodiscard]]
|
|
expected<unit, error_t> 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<error_t>{ESP_ERR_INVALID_ARG};
|
|
}
|
|
return {};
|
|
}
|
|
|
|
void 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;
|
|
}
|
|
};
|
|
|
|
struct TrackPidRuntimeCommand {
|
|
using proto_type = track_app_TrackPidRuntimeCommand;
|
|
static inline const pb_msgdesc_t *pb_fields = &track_app_TrackPidRuntimeCommand_msg;
|
|
|
|
std::variant<unit, TrackPidSetTargetHr, TrackPidSetTuning> command{unit{}};
|
|
|
|
static TrackPidRuntimeCommand from_proto(const proto_type &proto) {
|
|
switch (proto.which_command) {
|
|
case track_app_TrackPidRuntimeCommand_set_target_hr_tag:
|
|
return TrackPidRuntimeCommand{
|
|
.command = TrackPidSetTargetHr::from_proto(proto.command.set_target_hr),
|
|
};
|
|
case track_app_TrackPidRuntimeCommand_set_tuning_tag:
|
|
return TrackPidRuntimeCommand{
|
|
.command = TrackPidSetTuning::from_proto(proto.command.set_tuning),
|
|
};
|
|
default:
|
|
return TrackPidRuntimeCommand{};
|
|
}
|
|
}
|
|
|
|
[[nodiscard]]
|
|
proto_type to_proto() const {
|
|
proto_type proto = track_app_TrackPidRuntimeCommand_init_default;
|
|
std::visit(app::utils::overloads{
|
|
[](unit) {
|
|
// do nothing
|
|
},
|
|
[&](const TrackPidSetTargetHr &target_hr) {
|
|
proto.which_command = track_app_TrackPidRuntimeCommand_set_target_hr_tag;
|
|
proto.command.set_target_hr = target_hr.to_proto();
|
|
},
|
|
[&](const TrackPidSetTuning &tuning) {
|
|
proto.which_command = track_app_TrackPidRuntimeCommand_set_tuning_tag;
|
|
proto.command.set_tuning = tuning.to_proto();
|
|
}},
|
|
command);
|
|
return proto;
|
|
}
|
|
};
|
|
|
|
struct TrackPidStatus {
|
|
using proto_type = track_app_TrackPidStatus;
|
|
static inline const pb_msgdesc_t *pb_fields = &track_app_TrackPidStatus_msg;
|
|
|
|
uint8_t band_id{0};
|
|
bool band_is_active{false};
|
|
bool is_heart_rate_valid{false};
|
|
uint8_t heart_rate_bpm{0};
|
|
uint16_t step_count{0};
|
|
uint8_t active_segment_index{0};
|
|
TrackPidStageKind active_segment_kind{track_app_TrackPidStageKind_NONE};
|
|
float effective_speed_m_s{0.0F};
|
|
uint32_t remaining_program_ms{0};
|
|
float base_speed_m_s{0.0F};
|
|
|
|
static TrackPidStatus Default() {
|
|
return {
|
|
.band_id = 0,
|
|
.band_is_active = false,
|
|
.is_heart_rate_valid = false,
|
|
.heart_rate_bpm = 0,
|
|
.step_count = 0,
|
|
.active_segment_index = 0,
|
|
.active_segment_kind = track_app_TrackPidStageKind_NONE,
|
|
.effective_speed_m_s = 0.0F,
|
|
.remaining_program_ms = 0,
|
|
.base_speed_m_s = 0.0F,
|
|
};
|
|
}
|
|
|
|
static TrackPidStatus from_proto(const proto_type &proto) {
|
|
return {
|
|
.band_id = static_cast<uint8_t>(proto.band_id),
|
|
.band_is_active = proto.band_is_active,
|
|
.is_heart_rate_valid = proto.is_heart_rate_valid,
|
|
.heart_rate_bpm = static_cast<uint8_t>(proto.heart_rate_bpm),
|
|
.step_count = static_cast<uint16_t>(proto.step_count),
|
|
.active_segment_index = static_cast<uint8_t>(proto.active_segment_index),
|
|
.active_segment_kind = proto.active_segment_kind,
|
|
.effective_speed_m_s = proto.effective_speed_m_s,
|
|
.remaining_program_ms = proto.remaining_program_ms,
|
|
.base_speed_m_s = proto.base_speed_m_s,
|
|
};
|
|
}
|
|
|
|
[[nodiscard]]
|
|
proto_type to_proto() const {
|
|
proto_type proto = track_app_TrackPidStatus_init_default;
|
|
proto.band_id = band_id;
|
|
proto.band_is_active = band_is_active;
|
|
proto.is_heart_rate_valid = is_heart_rate_valid;
|
|
proto.heart_rate_bpm = heart_rate_bpm;
|
|
proto.step_count = step_count;
|
|
proto.active_segment_index = active_segment_index;
|
|
proto.active_segment_kind = active_segment_kind;
|
|
proto.effective_speed_m_s = effective_speed_m_s;
|
|
proto.remaining_program_ms = remaining_program_ms;
|
|
proto.base_speed_m_s = base_speed_m_s;
|
|
return proto;
|
|
}
|
|
};
|
|
|
|
struct TrackTestParameters {
|
|
using proto_type = track_app_TrackTestParameters;
|
|
static inline const pb_msgdesc_t *pb_fields = &track_app_TrackTestParameters_msg;
|
|
|
|
/** properties */
|
|
Color blink_color{Color::green()};
|
|
uint8_t blink_duty_cycle_percent{128};
|
|
uint32_t blink_period_ms{1'000};
|
|
uint32_t rainbow_move_speed_leds_per_sec{0};
|
|
uint16_t segment_led_start{0};
|
|
uint16_t segment_led_count{0};
|
|
uint16_t highlight_last_n_leds{0};
|
|
|
|
static TrackTestParameters from_proto(const proto_type &proto) {
|
|
TrackTestParameters params;
|
|
if (proto.has_blink_color) {
|
|
params.blink_color = Color::from_proto(proto.blink_color);
|
|
}
|
|
params.blink_duty_cycle_percent = proto.blink_duty_cycle_percent;
|
|
params.blink_period_ms = proto.blink_period_ms;
|
|
params.rainbow_move_speed_leds_per_sec = proto.rainbow_move_speed_leds_per_sec;
|
|
params.segment_led_start = proto.segment_led_start;
|
|
params.segment_led_count = proto.segment_led_count;
|
|
params.highlight_last_n_leds = proto.highlight_last_n_leds;
|
|
return params;
|
|
}
|
|
|
|
[[nodiscard]]
|
|
proto_type to_proto() const {
|
|
proto_type proto{};
|
|
proto.has_blink_color = true;
|
|
proto.blink_color = blink_color.to_proto();
|
|
proto.blink_duty_cycle_percent = blink_duty_cycle_percent;
|
|
proto.blink_period_ms = blink_period_ms;
|
|
proto.rainbow_move_speed_leds_per_sec = rainbow_move_speed_leds_per_sec;
|
|
proto.segment_led_start = segment_led_start;
|
|
proto.segment_led_count = segment_led_count;
|
|
proto.highlight_last_n_leds = highlight_last_n_leds;
|
|
return proto;
|
|
}
|
|
|
|
|
|
void log(const char *tag, esp_log_level_t level) const {
|
|
ESP_LOG_LEVEL(level, tag,
|
|
"TrackTestParameters{"
|
|
".blink_color=%s, "
|
|
".blink_duty_cycle_percent=%" PRIu8 ", "
|
|
".blink_period_ms=%" PRIu32 ", "
|
|
".rainbow_move_speed_leds_per_sec=%" PRIu32 ", "
|
|
".segment_led_start=%" PRIu16 ", "
|
|
".segment_led_count=%" PRIu16 "}",
|
|
blink_color.string().c_str(),
|
|
blink_duty_cycle_percent,
|
|
blink_period_ms,
|
|
rainbow_move_speed_leds_per_sec,
|
|
segment_led_start,
|
|
segment_led_count);
|
|
}
|
|
};
|
|
|
|
struct TrackControlMsg {
|
|
using proto_type = track_app_TrackControlMsg;
|
|
static inline const pb_msgdesc_t *pb_fields = &track_app_TrackControlMsg_msg;
|
|
|
|
/** properties */
|
|
std::variant<unit, TrackState, TrackTestParameters, TrackControllerMode> state{unit{}};
|
|
|
|
static TrackControlMsg from_proto(const proto_type &proto) {
|
|
switch (proto.which_msg) {
|
|
case track_app_TrackControlMsg_set_state_tag:
|
|
return TrackControlMsg{.state = static_cast<TrackState>(proto.msg.set_state)};
|
|
case track_app_TrackControlMsg_set_test_parameters_tag:
|
|
return TrackControlMsg{.state = TrackTestParameters::from_proto(proto.msg.set_test_parameters)};
|
|
case track_app_TrackControlMsg_set_mode_tag:
|
|
return TrackControlMsg{.state = static_cast<TrackControllerMode>(proto.msg.set_mode)};
|
|
default:
|
|
return TrackControlMsg{.state = unit{}};
|
|
}
|
|
}
|
|
|
|
[[nodiscard]]
|
|
proto_type to_proto() const {
|
|
proto_type proto = track_app_TrackControlMsg_init_default;
|
|
std::visit(app::utils::overloads{
|
|
[](unit) {
|
|
// do nothing
|
|
},
|
|
[&](TrackState track_state) {
|
|
proto.which_msg = track_app_TrackControlMsg_set_state_tag;
|
|
proto.msg.set_state = static_cast<track_app_TrackState>(track_state);
|
|
},
|
|
[&](const TrackTestParameters &test_params) {
|
|
proto.which_msg = track_app_TrackControlMsg_set_test_parameters_tag;
|
|
proto.msg.set_test_parameters = test_params.to_proto();
|
|
},
|
|
[&](TrackControllerMode mode) {
|
|
proto.which_msg = track_app_TrackControlMsg_set_mode_tag;
|
|
proto.msg.set_mode = static_cast<track_app_TrackControllerMode>(mode);
|
|
}},
|
|
state);
|
|
return proto;
|
|
}
|
|
};
|
|
|
|
|
|
struct TrackStateReportCollection {
|
|
using proto_type = track_app_TrackStateReportCollection;
|
|
static inline const pb_msgdesc_t *pb_fields = &track_app_TrackStateReportCollection_msg;
|
|
constexpr static size_t MAX_STATES = APP_TRACK_C_ARRAY_SIZE(proto_type::states);
|
|
|
|
struct Report {
|
|
using proto_type = track_app_TrackStateReport;
|
|
static inline const pb_msgdesc_t *pb_fields = &track_app_TrackStateReport_msg;
|
|
|
|
uint8_t id{0};
|
|
TrackState state{TrackState::track_app_TrackState_STOP};
|
|
float mileage_m{0.0F};
|
|
float speed_m_s{0.0F};
|
|
uint32_t time_elapsed_ms{0};
|
|
|
|
static Report from_proto(const proto_type &proto) {
|
|
return {proto.id,
|
|
static_cast<TrackState>(proto.state),
|
|
proto.mileage_m,
|
|
proto.speed_m_s,
|
|
proto.time_elapsed_ms};
|
|
}
|
|
|
|
[[nodiscard]]
|
|
proto_type to_proto() const {
|
|
proto_type proto{};
|
|
proto.id = id;
|
|
proto.state = static_cast<track_app_TrackState>(state);
|
|
proto.mileage_m = mileage_m;
|
|
proto.speed_m_s = speed_m_s;
|
|
proto.time_elapsed_ms = time_elapsed_ms;
|
|
return proto;
|
|
}
|
|
};
|
|
|
|
/** properties */
|
|
std::vector<Report> states;
|
|
|
|
static TrackStateReportCollection from_proto(const proto_type &proto) {
|
|
TrackStateReportCollection collection;
|
|
collection.states.reserve(proto.states_count);
|
|
for (pb_size_t i = 0; i < proto.states_count; ++i) {
|
|
collection.states.push_back(Report::from_proto(proto.states[i]));
|
|
}
|
|
return collection;
|
|
}
|
|
|
|
[[nodiscard]]
|
|
proto_type to_proto() const {
|
|
proto_type proto = track_app_TrackStateReportCollection_init_default;
|
|
proto.states_count = std::min(states.size(), MAX_STATES);
|
|
for (size_t i = 0; i < proto.states_count; ++i) {
|
|
proto.states[i] = states[i].to_proto();
|
|
}
|
|
return proto;
|
|
}
|
|
|
|
void log(const char *tag, esp_log_level_t level) const;
|
|
};
|
|
|
|
/**
|
|
* @brief Speed-Mileage training segment
|
|
*/
|
|
struct SMSegment {
|
|
struct __attribute__((packed)) raw {
|
|
static constexpr float SPEED_LSB = 10 / 255.0F;
|
|
/**
|
|
* @brief Speed in m/s
|
|
* @note LSB: ((10m/s) / 255) = 0.039m/s
|
|
*/
|
|
uint8_t speed_m_s;
|
|
uint16_t mileage_from_start_m;
|
|
};
|
|
static constexpr size_t RAW_SIZE = sizeof(raw);
|
|
|
|
static SMSegment from_raw(const raw &segment) {
|
|
return {.speed_m_s = static_cast<float>(segment.speed_m_s) * raw::SPEED_LSB,
|
|
.mileage_from_start_m = segment.mileage_from_start_m};
|
|
}
|
|
|
|
[[nodiscard]]
|
|
raw to_raw() const {
|
|
raw segment{
|
|
.speed_m_s = static_cast<uint8_t>(std::round(speed_m_s / raw::SPEED_LSB)),
|
|
.mileage_from_start_m = mileage_from_start_m};
|
|
return segment;
|
|
}
|
|
|
|
|
|
/** properties */
|
|
|
|
/**
|
|
* @brief speed in m/s
|
|
*/
|
|
float speed_m_s;
|
|
/**
|
|
* @brief absolute start mileage since the training (not difference nor the mileage the segment should maintain)
|
|
*/
|
|
uint16_t mileage_from_start_m;
|
|
};
|
|
|
|
/**
|
|
* @brief Time-Mileage training segment
|
|
*/
|
|
struct MTSegment {
|
|
/**
|
|
* @brief not the absolute mileage from start, but the mileage to travel in this segment
|
|
*/
|
|
uint16_t mileage_to_travel_this_segment_m;
|
|
/**
|
|
* @brief absolute time since the start of training (not time difference since last segment)
|
|
*/
|
|
uint16_t time_since_start_s;
|
|
};
|
|
|
|
/**
|
|
* @brief Speed-Time training segment
|
|
*/
|
|
struct STSegment {
|
|
struct __attribute__((packed)) raw {
|
|
static constexpr float SPEED_LSB = 10 / 255.0F;
|
|
/**
|
|
* @brief Speed in m/s
|
|
* @note LSB: ((10m/s) / 255) = 0.039m/s
|
|
*/
|
|
uint8_t speed_m_s;
|
|
uint16_t time_since_start_s;
|
|
};
|
|
static constexpr size_t RAW_SIZE = sizeof(raw);
|
|
|
|
static STSegment from_raw(const raw &segment) {
|
|
return {.speed_m_s = static_cast<float>(segment.speed_m_s) * raw::SPEED_LSB,
|
|
.time_since_start_s = segment.time_since_start_s};
|
|
}
|
|
|
|
[[nodiscard]]
|
|
raw to_raw() const {
|
|
raw segment{
|
|
.speed_m_s = static_cast<uint8_t>(std::round(speed_m_s / raw::SPEED_LSB)),
|
|
.time_since_start_s = time_since_start_s};
|
|
return segment;
|
|
}
|
|
|
|
|
|
/** properties */
|
|
|
|
/**
|
|
* @brief speed to maintain in m/s
|
|
*/
|
|
float speed_m_s;
|
|
/**
|
|
* @brief
|
|
*/
|
|
uint16_t time_since_start_s;
|
|
};
|
|
|
|
struct RepeatedSMSegment {
|
|
/** properties */
|
|
std::vector<SMSegment> speed_mileage_segments;
|
|
uint16_t time_since_start_s;
|
|
};
|
|
|
|
|
|
using config_getter = std::function<const TrackConfig &()>;
|
|
using report = TrackStateReportCollection::Report;
|
|
/**
|
|
* @brief provide `clock::now()` as the method to get the current timestamp
|
|
*/
|
|
using clock = app::utils::clock_t;
|
|
|
|
const char *to_str(AccelerationProfile profile);
|
|
|
|
/**
|
|
* @brief describes an acceleration process
|
|
*/
|
|
struct accel_calc_result_t {
|
|
float target_speed_m_s;
|
|
/**
|
|
* @brief acceleration in m/s^2; when 0, no acceleration is applied
|
|
* @note calculated at switching stage; note that it's an absolute value
|
|
* if the `target_speed_m_s` is higher than the current speed,
|
|
* the acceleration is positive, otherwise negative
|
|
*
|
|
* basically, depending the distance to the next segment,
|
|
* an acceleration rule is applied to the track
|
|
* (i.e. when to accelerate, and how fast to accelerate)
|
|
* it should be linear acceleration though
|
|
*
|
|
* if this value changed dynamically, it could be used as lerp
|
|
*/
|
|
float acceleration_m_s_2;
|
|
};
|
|
|
|
void set_global_config_getter(config_getter getter);
|
|
const TrackConfig &global_config();
|
|
|
|
const char *to_str(TrackState status);
|
|
const char *to_str(TrackControllerMode mode);
|
|
const char *to_str(TrackPidStageKind kind);
|
|
}
|
|
|
|
#endif /* E32F663C_9C5C_4317_882A_E6457E88B576 */
|