105 lines
4.1 KiB
C++
105 lines
4.1 KiB
C++
#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
|