forked from EmbeddedTeam/app_photomagnetic
1e6d6b9b30
Extend the UART protocol to support writing and reading the heating state, and reading individual pole NTC sensors and the heating pad NTC. Cache NTC readings for query on demand. Move PID updates to a workqueue to avoid blocking the timer IRQ context.
129 lines
3.4 KiB
C++
129 lines
3.4 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 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
|