forked from EmbeddedTeam/app_photomagnetic
133 lines
3.5 KiB
C++
133 lines
3.5 KiB
C++
#ifndef __THER_HEATING_HPP__
|
|
#define __THER_HEATING_HPP__
|
|
#include <algorithm>
|
|
#include <cmath>
|
|
#include <zephyr/device.h>
|
|
#include <zephyr/drivers/pwm.h>
|
|
#include <zephyr/drivers/sensor.h>
|
|
#include <zephyr/kernel.h>
|
|
#include <zephyr/sys/printk.h>
|
|
|
|
namespace ther {
|
|
|
|
class HeatingPad {
|
|
public:
|
|
static auto Start(sensor_value target_temp) -> void {
|
|
s_target = sensor_value_to_double(&target_temp);
|
|
s_integral = 0.0;
|
|
s_last_error = 0.0;
|
|
s_last_time = k_uptime_get();
|
|
|
|
printk("[heat] start target=%.1f C\n", s_target);
|
|
|
|
if (!device_is_ready(s_pwm_spec.dev)) {
|
|
printk("[heat] ERR: PWM not ready\n");
|
|
return;
|
|
}
|
|
printk("[heat] PWM OK\n");
|
|
|
|
k_work_init(&s_work, WorkHandler);
|
|
k_timer_init(&s_timer, TimerTick, nullptr);
|
|
k_timer_start(&s_timer, K_NO_WAIT, K_MSEC(kUpdatePeriodMs));
|
|
s_active = true;
|
|
}
|
|
|
|
static auto StartFullEnergy() -> void {
|
|
pwm_set_pulse_dt(&s_pwm_spec, s_pwm_spec.period);
|
|
}
|
|
|
|
static auto Stop() -> void {
|
|
s_active = false;
|
|
k_timer_stop(&s_timer);
|
|
pwm_set_pulse_dt(&s_pwm_spec, 0);
|
|
}
|
|
|
|
static auto CurrentTemp() -> sensor_value { return s_current_temp; }
|
|
|
|
private:
|
|
static constexpr uint32_t kPeriodNs = PWM_KHZ(5);
|
|
static constexpr uint32_t kUpdatePeriodMs = 100;
|
|
static constexpr double kOutMin = 0.0;
|
|
static constexpr double kOutMax = 1.0;
|
|
static constexpr double kKp = 2.0;
|
|
static constexpr double kKi = 0.02;
|
|
|
|
inline static bool s_active{false};
|
|
inline static double s_target{0.0};
|
|
inline static double s_integral{0.0};
|
|
inline static double s_last_error{0.0};
|
|
inline static int64_t s_last_time{0};
|
|
|
|
inline static pwm_dt_spec s_pwm_spec = {
|
|
.dev = DEVICE_DT_GET(DT_NODELABEL(pwm1)),
|
|
.channel = 1,
|
|
.period = PWM_KHZ(5),
|
|
.flags = PWM_POLARITY_NORMAL,
|
|
};
|
|
|
|
inline static const device *s_temp_sensor =
|
|
DEVICE_DT_GET(DT_NODELABEL(heating_pad_ntc));
|
|
|
|
inline static k_timer s_timer;
|
|
inline static k_work s_work;
|
|
inline static bool s_work_pending{false};
|
|
|
|
/// Timer fires → submit work. Skips if previous work hasn't finished.
|
|
static auto TimerTick(k_timer * /*timer*/) -> void {
|
|
if (s_work_pending) {
|
|
return;
|
|
}
|
|
s_work_pending = true;
|
|
k_work_submit(&s_work);
|
|
}
|
|
|
|
/// PID update running in system workqueue context.
|
|
static auto WorkHandler(k_work * /*work*/) -> void {
|
|
double current_temp;
|
|
if (ReadTemperature(current_temp) != 0) {
|
|
s_work_pending = false;
|
|
return;
|
|
}
|
|
|
|
printk("[heat] cur=%.1f C target=%.1f C\n", current_temp, s_target);
|
|
|
|
const int64_t now = k_uptime_get();
|
|
const double dt = static_cast<double>(now - s_last_time) / 1000.0;
|
|
s_last_time = now;
|
|
|
|
const double error = s_target - current_temp;
|
|
s_integral += kKi * error * dt;
|
|
s_integral = std::clamp(s_integral, kOutMin, kOutMax);
|
|
|
|
double output = kKp * error + s_integral;
|
|
output = std::clamp(output, kOutMin, kOutMax);
|
|
|
|
const uint32_t pulse = static_cast<uint32_t>(output * kPeriodNs);
|
|
pwm_set_pulse_dt(&s_pwm_spec, pulse);
|
|
s_work_pending = false;
|
|
}
|
|
|
|
static auto ReadTemperature(double &out_temp) -> int {
|
|
if (0 != sensor_sample_fetch(s_temp_sensor)) {
|
|
printk("[heat] ERR: sensor fetch failed\n");
|
|
return -EIO;
|
|
}
|
|
|
|
sensor_value val{};
|
|
if (0 !=
|
|
sensor_channel_get(s_temp_sensor, SENSOR_CHAN_AMBIENT_TEMP, &val)) {
|
|
printk("[heat] ERR: channel_get failed\n");
|
|
return -EIO;
|
|
}
|
|
|
|
out_temp = sensor_value_to_double(&val);
|
|
s_current_temp = val;
|
|
return 0;
|
|
}
|
|
inline static sensor_value s_current_temp{};
|
|
};
|
|
|
|
} // namespace ther
|
|
|
|
#endif
|