117 lines
3.0 KiB
C++
117 lines
3.0 KiB
C++
#pragma once
|
|
|
|
#include "track_core/model.hpp"
|
|
|
|
namespace track_core {
|
|
|
|
class TrackPidProgramState {
|
|
public:
|
|
using time_point = clock::time_point;
|
|
|
|
struct HrSample {
|
|
float heart_rate_bpm{0.0F};
|
|
float sample_interval_s{0.0F};
|
|
};
|
|
|
|
TrackPidProgramState() = delete;
|
|
TrackPidProgramState(time_point now, TrackPidConfig config);
|
|
|
|
[[nodiscard]]
|
|
float next(time_point now, std::optional<HrSample> fresh_hr_sample);
|
|
|
|
[[nodiscard]]
|
|
bool is_finished(time_point now) const;
|
|
|
|
[[nodiscard]]
|
|
TrackPidStageKind active_stage_kind() const;
|
|
|
|
[[nodiscard]]
|
|
std::uint8_t active_stage_index() const;
|
|
|
|
[[nodiscard]]
|
|
float commanded_speed_m_s() const;
|
|
|
|
[[nodiscard]]
|
|
std::uint32_t remaining_program_ms(time_point now) const;
|
|
|
|
void update_target_hr_bpm(std::uint8_t target_hr_bpm);
|
|
|
|
[[nodiscard]]
|
|
expected<unit, TrackError> update_tuning(const TrackPidSetTuning &tuning);
|
|
|
|
private:
|
|
struct pid_state {
|
|
struct fine_tune_state {
|
|
std::uint8_t band_plus{};
|
|
std::uint8_t band_minus{};
|
|
float gain_scale{1.0F};
|
|
};
|
|
|
|
float kp{0.0F};
|
|
float ki{1.0F};
|
|
float kd{0.0F};
|
|
std::optional<fine_tune_state> fine_tune;
|
|
float slew_rate_limit{0.0F};
|
|
float e_t_1{0.0F};
|
|
float e_t_2{0.0F};
|
|
float u_t_1{0.0F};
|
|
float min_speed_m_s{0.0F};
|
|
float max_speed_m_s{0.0F};
|
|
bool should_use_nominal_sample_interval{true};
|
|
|
|
void from_segment(const TrackPidSegment &segment);
|
|
void reset_with(float speed_m_s);
|
|
|
|
[[nodiscard]]
|
|
float effective_gain_scale(float target_hr, float current_hr) const;
|
|
|
|
[[nodiscard]]
|
|
float clamp_commanded_speed(float speed_m_s) const;
|
|
|
|
void apply_tuning(const TrackPidSetTuning &tuning);
|
|
};
|
|
|
|
[[nodiscard]]
|
|
const TrackPidSchema &schema() const;
|
|
|
|
[[nodiscard]]
|
|
static std::uint16_t schema_duration_s(const TrackPidSchema &schema);
|
|
|
|
[[nodiscard]]
|
|
bool sync_schema_for_time(time_point now);
|
|
|
|
void advance_to_schema_index(std::size_t target_index);
|
|
|
|
[[nodiscard]]
|
|
std::size_t resolve_schema_index(time_point now) const;
|
|
|
|
[[nodiscard]]
|
|
bool maybe_jump_to_final_pid(time_point now, std::optional<HrSample> fresh_hr_sample);
|
|
|
|
[[nodiscard]]
|
|
bool is_in_deadzone(float heart_rate) const;
|
|
|
|
[[nodiscard]]
|
|
float do_pid(float current_hr, float sample_interval_s);
|
|
|
|
void apply_schema(const TrackPidSchema &schema);
|
|
void finish();
|
|
|
|
[[nodiscard]]
|
|
static bool validate_preemptive_schema(const std::vector<TrackPidSchema> &schemas);
|
|
|
|
[[nodiscard]]
|
|
static time_point safe_add_seconds(time_point now, std::uint32_t duration_s);
|
|
|
|
TrackPidConfig config_;
|
|
time_point program_start_timestamp_{};
|
|
time_point program_end_timestamp_{};
|
|
std::size_t schema_index_{};
|
|
pid_state pid_{};
|
|
bool preemptive_enabled_{};
|
|
bool forced_final_pid_{};
|
|
bool finished_{};
|
|
};
|
|
|
|
} // namespace track_core
|