forked from EmbeddedTeam/app_photomagnetic
Add support for heating state and NTC commands
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.
This commit is contained in:
zhangyisong
parent
2c42b67eca
commit
1e6d6b9b30
@ -1,7 +1,9 @@
|
||||
#ifndef __THER_COM_HPP__
|
||||
#define __THER_COM_HPP__
|
||||
|
||||
#include "heating.hpp"
|
||||
#include "led.hpp"
|
||||
#include "ntc.hpp"
|
||||
#include <led_strip_indicator/led_strip_indicator.hpp>
|
||||
#include <uart_com/simple_protocal.hpp>
|
||||
#include <zephyr/drivers/hwinfo.h>
|
||||
@ -23,7 +25,17 @@ public:
|
||||
|
||||
private:
|
||||
enum HostStatus : uint8_t { STANDBY, RUNNING, PAUSE, ERROR };
|
||||
enum Addr : uint8_t { TEMP = 0, GET_ID, RUNNING_STATE };
|
||||
enum Addr : uint8_t {
|
||||
TEMP = 0,
|
||||
GET_ID,
|
||||
RUNNING_STATE,
|
||||
W_HEATING_STATE,
|
||||
R_HEATING_STATE
|
||||
};
|
||||
enum DevAddr : uint8_t {
|
||||
R_TEMP_POLE_NTC = 100,
|
||||
R_TEMP_HETING_PAD_NTC = 101,
|
||||
};
|
||||
using CbTableValueType =
|
||||
std::pair<const uint8_t, void (*)(uart_com::DataType)>;
|
||||
struct Cb {
|
||||
@ -42,17 +54,65 @@ private:
|
||||
} else {
|
||||
InfLed::Off();
|
||||
}
|
||||
s_led_strip_indicator->Status(data[0]);
|
||||
printk("set ledsrtip to %d", data[0]);
|
||||
const uint8_t id = data[0];
|
||||
s_led_strip_indicator->Status(id).on_error([](zpp::error_code err) {
|
||||
printk("err code: %s", zpp::error_str(err));
|
||||
});
|
||||
}
|
||||
static auto WHeatingState(uart_com::DataType data) -> void {
|
||||
if (data.size() != 1) {
|
||||
return;
|
||||
}
|
||||
const bool state = (data[0] > 0);
|
||||
if (state) {
|
||||
HeatingPad::Start(sensor_value{data[0], 0});
|
||||
} else {
|
||||
HeatingPad::Stop();
|
||||
}
|
||||
}
|
||||
static auto RHeatingState(uart_com::DataType data) -> void {
|
||||
if (data.size() != 0) {
|
||||
return;
|
||||
}
|
||||
const sensor_value temp = HeatingPad::CurrentTemp();
|
||||
const uint8_t state[] = {s_heating_state, (uint8_t)temp.val1,
|
||||
(uint8_t)temp.val2};
|
||||
s_proto->Send(R_HEATING_STATE, state);
|
||||
}
|
||||
/// Read one pole NTC by index (0~7).
|
||||
static auto ReadPoleNtcTemperature(uart_com::DataType data) -> void {
|
||||
if (data.size() != 1) {
|
||||
return;
|
||||
}
|
||||
const sensor_value temp = NtcGroup::GetSensorValue(data[0]);
|
||||
const uint8_t payload[] = {(uint8_t)temp.val1, (uint8_t)temp.val2};
|
||||
s_proto->Send(R_TEMP_POLE_NTC, payload);
|
||||
}
|
||||
/// Read heating pad NTC (onboard ADC).
|
||||
static auto ReadHeatingPadNtcTemperature(uart_com::DataType data) -> void {
|
||||
const sensor_value temp = HeatingPad::CurrentTemp();
|
||||
const uint8_t payload[] = {(uint8_t)temp.val1, (uint8_t)temp.val2};
|
||||
s_proto->Send(R_TEMP_HETING_PAD_NTC, payload);
|
||||
}
|
||||
};
|
||||
constexpr static std::pair<const uint8_t,
|
||||
uart_com::SimpleProtocal::CallbackType>
|
||||
kRxCallbackTable[] = {{GET_ID, Cb::GetId},
|
||||
{RUNNING_STATE, Cb::RunningState}};
|
||||
kRxCallbackTable[] = {
|
||||
{GET_ID, Cb::GetId},
|
||||
{RUNNING_STATE, Cb::RunningState},
|
||||
{W_HEATING_STATE, Cb::WHeatingState},
|
||||
{R_HEATING_STATE, Cb::RHeatingState},
|
||||
// Pole NTC (MCP3208)
|
||||
{R_TEMP_POLE_NTC, Cb::ReadPoleNtcTemperature},
|
||||
// Heating pad NTC (ADC1)
|
||||
{R_TEMP_HETING_PAD_NTC, Cb::ReadHeatingPadNtcTemperature},
|
||||
};
|
||||
inline static auto s_led_strip_indicator =
|
||||
ZPP_DRV_GET_P(ledstrip::Indicator, DT_NODELABEL(indicator));
|
||||
inline static auto s_proto =
|
||||
(uart_com::SimpleProtocal *)(DEVICE_DT_GET(DT_NODELABEL(pm_protocal)));
|
||||
inline static auto s_heating_state = false;
|
||||
using InfLed = ther::Led<LED_DT_SPEC_GET(DT_NODELABEL(inf_led))>;
|
||||
};
|
||||
} // namespace ther
|
||||
|
||||
@ -1,6 +1,7 @@
|
||||
#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>
|
||||
@ -11,69 +12,41 @@ namespace ther {
|
||||
|
||||
class HeatingPad {
|
||||
public:
|
||||
/// Start PID temperature control toward the given target.
|
||||
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);
|
||||
printk("[heat] start target=%.1f C\n", s_target);
|
||||
|
||||
// Check PWM device
|
||||
if (!device_is_ready(s_pwm_spec.dev)) {
|
||||
printk("[heat] ERR: PWM device not ready\n");
|
||||
printk("[heat] ERR: PWM not ready\n");
|
||||
return;
|
||||
}
|
||||
printk("[heat] PWM dev=%s ch=%d period=%u ns\n", s_pwm_spec.dev->name,
|
||||
s_pwm_spec.channel, s_pwm_spec.period);
|
||||
printk("[heat] PWM OK\n");
|
||||
|
||||
// Check sensor
|
||||
if (!device_is_ready(s_temp_sensor)) {
|
||||
printk("[heat] ERR: temp sensor not ready\n");
|
||||
return;
|
||||
}
|
||||
printk("[heat] temp sensor OK\n");
|
||||
|
||||
// Test: set 50% duty immediately to verify PWM works
|
||||
const uint32_t pulse_50 = s_pwm_spec.period / 2;
|
||||
int ret = pwm_set_pulse_dt(&s_pwm_spec, pulse_50);
|
||||
printk("[heat] test 50%% duty pulse=%u ret=%d\n", pulse_50, ret);
|
||||
|
||||
k_timer_init(&s_timer, Tick, nullptr);
|
||||
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 Start() {
|
||||
|
||||
const uint32_t pulse_50 = s_pwm_spec.period / 2;
|
||||
int ret = pwm_set_pulse_dt(&s_pwm_spec, pulse_50);
|
||||
return ret;
|
||||
}
|
||||
/// Stop PID control and turn off heating.
|
||||
static auto Stop() -> void {
|
||||
printk("[heat] stop\n");
|
||||
s_active = false;
|
||||
k_timer_stop(&s_timer);
|
||||
pwm_set_pulse_dt(&s_pwm_spec, 0);
|
||||
}
|
||||
|
||||
/// Change the target temperature while keeping PID running.
|
||||
static auto SetTarget(sensor_value target_temp) -> void {
|
||||
s_target = sensor_value_to_double(&target_temp);
|
||||
printk("[heat] new target=%.1f°C\n", s_target);
|
||||
}
|
||||
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;
|
||||
|
||||
// ── PI gains (tune these) ─────────────────────────
|
||||
static constexpr double kKp = 2.0; // proportional
|
||||
static constexpr double kKi = 0.02; // integral
|
||||
static constexpr double kKd = 0.0; // derivative (not needed for heating-only)
|
||||
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};
|
||||
@ -92,45 +65,44 @@ private:
|
||||
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};
|
||||
|
||||
/// Called periodically by the timer.
|
||||
static auto Tick(k_timer * /*timer*/) -> void {
|
||||
/// 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) {
|
||||
printk("[heat] ERR: read temp failed\n");
|
||||
s_work_pending = false;
|
||||
return;
|
||||
}
|
||||
|
||||
printk("[heat] tick: cur=%.1f°C target=%.1f°C\n", current_temp, s_target);
|
||||
printk("[heat] cur=%.1f C target=%.1f C\n", current_temp, s_target);
|
||||
|
||||
// PI computation
|
||||
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;
|
||||
|
||||
// Proportional
|
||||
const double p = kKp * error;
|
||||
|
||||
// Integral with anti-windup
|
||||
s_integral += kKi * error * dt;
|
||||
s_integral = std::clamp(s_integral, kOutMin, kOutMax);
|
||||
|
||||
s_last_error = error;
|
||||
|
||||
// Compute output, clamp to [0, 1]
|
||||
double output = p + s_integral;
|
||||
double output = kKp * error + s_integral;
|
||||
output = std::clamp(output, kOutMin, kOutMax);
|
||||
|
||||
// Apply PWM duty cycle
|
||||
const uint32_t pulse = static_cast<uint32_t>(output * kPeriodNs);
|
||||
printk("[heat] PID out=%.2f%% pulse=%u / %u\n", output * 100.0, pulse,
|
||||
s_pwm_spec.period);
|
||||
pwm_set_pulse_dt(&s_pwm_spec, pulse);
|
||||
s_work_pending = false;
|
||||
}
|
||||
|
||||
/// Read temperature from the NTC sensor into `out_temp` (in °C).
|
||||
static auto ReadTemperature(double &out_temp) -> int {
|
||||
if (0 != sensor_sample_fetch(s_temp_sensor)) {
|
||||
printk("[heat] ERR: sensor fetch failed\n");
|
||||
@ -145,8 +117,10 @@ private:
|
||||
}
|
||||
|
||||
out_temp = sensor_value_to_double(&val);
|
||||
s_current_temp = val;
|
||||
return 0;
|
||||
}
|
||||
inline static sensor_value s_current_temp{};
|
||||
};
|
||||
|
||||
} // namespace ther
|
||||
|
||||
@ -1,5 +1,6 @@
|
||||
#ifndef THER_NTC_HPP
|
||||
#define THER_NTC_HPP
|
||||
#include <array>
|
||||
#include <cmath>
|
||||
#include <zephyr/device.h>
|
||||
#include <zephyr/drivers/sensor.h>
|
||||
@ -30,7 +31,8 @@ public:
|
||||
continue;
|
||||
}
|
||||
|
||||
printk("[ntc]#%zu = %d.%d°C\n", i + 1, val.val1, val.val2);
|
||||
// Always cache the latest reading for individual query
|
||||
s_current_temp[i] = val;
|
||||
|
||||
if (!found || val.val1 > max_val.val1 ||
|
||||
(val.val1 == max_val.val1 && val.val2 > max_val.val2)) {
|
||||
@ -46,9 +48,17 @@ public:
|
||||
return zpp::error_code::k_io;
|
||||
}
|
||||
|
||||
/// Return the cached value for a single NTC sensor.
|
||||
static auto GetSensorValue(uint8_t id) -> sensor_value {
|
||||
if (id >= NTC_COUNT) {
|
||||
return sensor_value{};
|
||||
}
|
||||
return s_current_temp[id];
|
||||
}
|
||||
|
||||
private:
|
||||
static constexpr size_t NTC_COUNT = 8;
|
||||
|
||||
inline static std::array<sensor_value, NTC_COUNT> s_current_temp{};
|
||||
inline static const device *s_devices[] = {
|
||||
DEVICE_DT_GET(DT_NODELABEL(pole_ntc1)),
|
||||
DEVICE_DT_GET(DT_NODELABEL(pole_ntc2)),
|
||||
|
||||
294
scripts/test.py
294
scripts/test.py
@ -6,15 +6,23 @@ Frame format (SimpleProtocal, per DTS config):
|
||||
H0(0x7E) H1(0xE7) CMD(1B) LEN(1B) DATA[0..N] CRC_LO CRC_HI
|
||||
CRC = CRC-16 Modbus over CMD+LEN+DATA, LSB-MSB order
|
||||
|
||||
Commands:
|
||||
0x00 TEMP D→H (2 bytes: val1 val2)
|
||||
0x01 GET_ID H→D / D→H (20 bytes HWID)
|
||||
0x02 RUNNING_STATE H→D (1 byte: 0=Standby 1=Running)
|
||||
0x03 W_HEATING_STATE H→D (1 byte: 0=stop, >0=target °C)
|
||||
0x04 R_HEATING_STATE H→D / D→H (3 bytes: state val1 val2)
|
||||
0x64 R_TEMP_POLE_NTC H→D (1 byte index) / D→H (2 bytes: val1 val2)
|
||||
0x65 R_HEATING_PAD_NTC H→D (0 bytes) / D→H (2 bytes: val1 val2)
|
||||
|
||||
Usage:
|
||||
python test.py /dev/ttyUSB0 --get-id (request device ID)
|
||||
python test.py /dev/ttyUSB0 --state 1 (set running state)
|
||||
python test.py /dev/ttyUSB0 --monitor (listen for temp data)
|
||||
python test.py /dev/ttyUSB0 --baud 115200 --loop (poll ID in a loop)
|
||||
python test.py /dev/ttyUSB0 --monitor
|
||||
python test.py /dev/ttyUSB0 --heating 42
|
||||
python test.py /dev/ttyUSB0 --read-pole-ntc 3
|
||||
python test.py /dev/ttyUSB0 --read-heating-ntc
|
||||
"""
|
||||
|
||||
import argparse
|
||||
import struct
|
||||
import sys
|
||||
import time
|
||||
|
||||
@ -25,27 +33,22 @@ except ImportError:
|
||||
sys.exit(1)
|
||||
|
||||
# ── Protocol constants ────────────────────────────────────────
|
||||
HEADER = b"\x7e\xe7" # 2-byte header (DTS: header = <0x7EE7>)
|
||||
HEADER = b"\x7e\xe7"
|
||||
HEADER_SIZE = len(HEADER)
|
||||
|
||||
CMD_TEMP = 0x00
|
||||
CMD_GET_ID = 0x01
|
||||
CMD_RUNNING_STATE = 0x02
|
||||
CMD_W_HEATING = 0x03
|
||||
CMD_R_HEATING = 0x04
|
||||
CMD_READ_POLE_NTC = 0x64
|
||||
CMD_READ_HEATING_NTC = 0x65
|
||||
|
||||
STATE_NAMES = {
|
||||
0x00: "Standby",
|
||||
0x01: "Running",
|
||||
0x02: "Pause",
|
||||
0x03: "Fault",
|
||||
0x04: "Upgrade",
|
||||
0x05: "EStop",
|
||||
0x06: "Exception",
|
||||
}
|
||||
HEATING_STATE_NAMES = {0: "OFF", 1: "ON"}
|
||||
|
||||
|
||||
# ── CRC-16 Modbus ─────────────────────────────────────────────
|
||||
def crc16_modbus(data: bytes) -> int:
|
||||
"""CRC-16 Modbus (polynomial 0xA001, init 0xFFFF)."""
|
||||
crc = 0xFFFF
|
||||
for byte in data:
|
||||
crc ^= byte
|
||||
@ -59,128 +62,165 @@ def crc16_modbus(data: bytes) -> int:
|
||||
|
||||
# ── Frame helpers ─────────────────────────────────────────────
|
||||
def build_frame(cmd: int, payload: bytes = b"") -> bytes:
|
||||
"""Build a SimpleProtocal frame with LSB-MSB CRC.
|
||||
Frame: H0 H1 CMD LEN DATA[0..N] CRC_LO CRC_HI"""
|
||||
body = bytes([cmd, len(payload)]) + payload
|
||||
crc = crc16_modbus(body)
|
||||
crc_lo = crc & 0xFF
|
||||
crc_hi = (crc >> 8) & 0xFF
|
||||
return HEADER + body + bytes([crc_lo, crc_hi])
|
||||
return HEADER + body + bytes([crc & 0xFF, (crc >> 8) & 0xFF])
|
||||
|
||||
|
||||
def parse_frame(frame: bytes) -> tuple[int, bytes] | None:
|
||||
"""Parse a SimpleProtocal frame with 2-byte header + LSB-MSB CRC.
|
||||
Returns (cmd, data) or None on error."""
|
||||
min_len = HEADER_SIZE + 2 + 2 # HDR(2) + CMD(1) + LEN(1) + CRC(2) = 6
|
||||
if len(frame) < min_len:
|
||||
if len(frame) < HEADER_SIZE + 2 + 2:
|
||||
return None
|
||||
if frame[:HEADER_SIZE] != HEADER:
|
||||
return None
|
||||
|
||||
cmd = frame[HEADER_SIZE]
|
||||
length = frame[HEADER_SIZE + 1]
|
||||
expected = HEADER_SIZE + 2 + length + 2 # HDR + CMD+LEN + DATA + CRC
|
||||
expected = HEADER_SIZE + 2 + length + 2
|
||||
if len(frame) != expected:
|
||||
return None
|
||||
|
||||
data = frame[HEADER_SIZE + 2 : HEADER_SIZE + 2 + length]
|
||||
|
||||
# CRC covers CMD + LEN + DATA (LSB-MSB order on wire)
|
||||
crc_body = frame[HEADER_SIZE : HEADER_SIZE + 2 + length]
|
||||
actual_crc = crc16_modbus(crc_body)
|
||||
crc_lo = frame[HEADER_SIZE + 2 + length]
|
||||
crc_hi = frame[HEADER_SIZE + 2 + length + 1]
|
||||
wire_crc = (crc_hi << 8) | crc_lo
|
||||
|
||||
wire_crc = (frame[expected - 1] << 8) | frame[expected - 2]
|
||||
if actual_crc != wire_crc:
|
||||
return None
|
||||
|
||||
return cmd, data
|
||||
|
||||
|
||||
# ── Serial reader ─────────────────────────────────────────────
|
||||
class ProtocolReader:
|
||||
"""State-machine frame reader for 2-byte header protocol."""
|
||||
|
||||
def __init__(self, ser: serial.Serial):
|
||||
self._ser = ser
|
||||
self._buf = bytearray()
|
||||
|
||||
def read_frame(self, timeout: float = 1.0) -> bytes | None:
|
||||
"""Read one complete frame. Returns raw frame bytes or None on timeout."""
|
||||
min_frame = HEADER_SIZE + 2 + 2 # HDR + CMD + LEN + CRC = 6
|
||||
|
||||
deadline = time.time() + timeout
|
||||
while time.time() < deadline:
|
||||
# Look for header
|
||||
if len(self._buf) >= 2:
|
||||
idx = self._buf.find(HEADER)
|
||||
if idx > 0:
|
||||
del self._buf[:idx] # discard bytes before header
|
||||
elif idx < 0 and len(self._buf) > 0:
|
||||
del self._buf[:idx]
|
||||
elif idx < 0:
|
||||
self._buf.clear()
|
||||
|
||||
# Have a header → try to parse frame length
|
||||
if len(self._buf) >= HEADER_SIZE:
|
||||
if self._buf[:HEADER_SIZE] == HEADER:
|
||||
if len(self._buf) >= min_frame:
|
||||
length = self._buf[HEADER_SIZE + 1]
|
||||
total = HEADER_SIZE + 2 + length + 2
|
||||
if len(self._buf) >= total:
|
||||
frame = bytes(self._buf[:total])
|
||||
del self._buf[:total]
|
||||
return frame
|
||||
else:
|
||||
# Wrong header, discard first byte and retry
|
||||
del self._buf[0]
|
||||
continue
|
||||
if len(self._buf) >= HEADER_SIZE and self._buf[:HEADER_SIZE] == HEADER:
|
||||
if len(self._buf) >= HEADER_SIZE + 2 + 2:
|
||||
length = self._buf[HEADER_SIZE + 1]
|
||||
total = HEADER_SIZE + 2 + length + 2
|
||||
if len(self._buf) >= total:
|
||||
frame = bytes(self._buf[:total])
|
||||
del self._buf[:total]
|
||||
return frame
|
||||
elif len(self._buf) >= HEADER_SIZE:
|
||||
del self._buf[0]
|
||||
continue
|
||||
|
||||
# Read more bytes
|
||||
try:
|
||||
chunk = self._ser.read(self._ser.in_waiting or 1)
|
||||
if chunk:
|
||||
self._buf.extend(chunk)
|
||||
except (serial.SerialTimeoutException, serial.SerialException):
|
||||
pass
|
||||
|
||||
return None
|
||||
|
||||
|
||||
# ── Command handlers ──────────────────────────────────────────
|
||||
def cmd_get_id(ser: serial.Serial, timeout: float = 2.0):
|
||||
"""Send GET_ID request and print response."""
|
||||
print("→ Sending GET_ID...")
|
||||
ser.write(build_frame(CMD_GET_ID))
|
||||
|
||||
def _read_reply(ser: serial.Serial, timeout: float = 2.0) -> tuple[int, bytes] | None:
|
||||
reader = ProtocolReader(ser)
|
||||
frame = reader.read_frame(timeout)
|
||||
if frame is None:
|
||||
print("✗ No response (timeout)")
|
||||
return
|
||||
|
||||
return None
|
||||
result = parse_frame(frame)
|
||||
if result is None:
|
||||
print(f"✗ Invalid frame: {frame.hex()}")
|
||||
return
|
||||
return None
|
||||
return result
|
||||
|
||||
cmd, data = result
|
||||
|
||||
def _temp_from_data(data: bytes) -> float:
|
||||
return data[0] + data[1] / 100.0 if len(data) >= 2 else 0.0
|
||||
|
||||
|
||||
# ── Command handlers ──────────────────────────────────────────
|
||||
def cmd_get_id(ser: serial.Serial, timeout: float = 2.0):
|
||||
print("→ Sending GET_ID...")
|
||||
ser.write(build_frame(CMD_GET_ID))
|
||||
r = _read_reply(ser, timeout)
|
||||
if r is None:
|
||||
return
|
||||
cmd, data = r
|
||||
if cmd != CMD_GET_ID:
|
||||
print(f"✗ Unexpected cmd=0x{cmd:02X} (expected 0x{CMD_GET_ID:02X})")
|
||||
print(f"✗ Unexpected cmd=0x{cmd:02X}")
|
||||
return
|
||||
|
||||
print(f"✓ Device ID ({len(data)} bytes): {data.hex(' ')}")
|
||||
|
||||
|
||||
def cmd_set_state(ser: serial.Serial, state: int):
|
||||
"""Send RUNNING_STATE command."""
|
||||
name = STATE_NAMES.get(state, "Unknown")
|
||||
names = {0: "Standby", 1: "Running", 2: "Pause", 3: "Error"}
|
||||
name = names.get(state, "Unknown")
|
||||
print(f"→ Setting state: {name} (0x{state:02X})")
|
||||
ser.write(build_frame(CMD_RUNNING_STATE, bytes([state])))
|
||||
print(f"✓ Sent")
|
||||
print("✓ Sent")
|
||||
|
||||
|
||||
def cmd_write_heating(ser: serial.Serial, target: int):
|
||||
if target == 0:
|
||||
print("→ Stopping heating")
|
||||
else:
|
||||
print(f"→ Starting heating to {target}°C")
|
||||
ser.write(build_frame(CMD_W_HEATING, bytes([target])))
|
||||
print("✓ Sent")
|
||||
|
||||
|
||||
def cmd_read_heating(ser: serial.Serial, timeout: float = 2.0):
|
||||
print("→ Querying heating state...")
|
||||
ser.write(build_frame(CMD_R_HEATING))
|
||||
r = _read_reply(ser, timeout)
|
||||
if r is None:
|
||||
return
|
||||
cmd, data = r
|
||||
if cmd != CMD_R_HEATING:
|
||||
print(f"✗ Unexpected cmd=0x{cmd:02X}")
|
||||
return
|
||||
if len(data) < 3:
|
||||
print(f"✗ Short data: {data.hex()}")
|
||||
return
|
||||
state = HEATING_STATE_NAMES.get(data[0], f"Unknown({data[0]})")
|
||||
print(f"✓ Heating: {state}, temp={_temp_from_data(data[1:3]):.2f}°C")
|
||||
|
||||
|
||||
def cmd_read_pole_ntc(ser: serial.Serial, index: int, timeout: float = 2.0):
|
||||
print(f"→ Reading pole NTC #{index}...")
|
||||
ser.write(build_frame(CMD_READ_POLE_NTC, bytes([index])))
|
||||
r = _read_reply(ser, timeout)
|
||||
if r is None:
|
||||
return
|
||||
cmd, data = r
|
||||
if cmd != CMD_READ_POLE_NTC:
|
||||
print(f"✗ Unexpected cmd=0x{cmd:02X}")
|
||||
return
|
||||
if len(data) < 2:
|
||||
print(f"✗ Short data: {data.hex()}")
|
||||
return
|
||||
print(f"✓ Pole NTC #{index}: {_temp_from_data(data):.2f}°C")
|
||||
|
||||
|
||||
def cmd_read_heating_ntc(ser: serial.Serial, timeout: float = 2.0):
|
||||
print("→ Reading heating pad NTC...")
|
||||
ser.write(build_frame(CMD_READ_HEATING_NTC))
|
||||
r = _read_reply(ser, timeout)
|
||||
if r is None:
|
||||
return
|
||||
cmd, data = r
|
||||
if cmd != CMD_READ_HEATING_NTC:
|
||||
print(f"✗ Unexpected cmd=0x{cmd:02X}")
|
||||
return
|
||||
if len(data) < 2:
|
||||
print(f"✗ Short data: {data.hex()}")
|
||||
return
|
||||
print(f"✓ Heating pad NTC: {_temp_from_data(data):.2f}°C")
|
||||
|
||||
|
||||
def cmd_monitor(ser: serial.Serial):
|
||||
"""Listen for incoming frames (TEMP data) and print them."""
|
||||
print("Monitoring... (Ctrl+C to stop)")
|
||||
reader = ProtocolReader(ser)
|
||||
try:
|
||||
@ -188,7 +228,6 @@ def cmd_monitor(ser: serial.Serial):
|
||||
frame = reader.read_frame(timeout=0.5)
|
||||
if frame is None:
|
||||
continue
|
||||
|
||||
result = parse_frame(frame)
|
||||
if result is None:
|
||||
print(f"⚠ Bad frame: {frame.hex()}")
|
||||
@ -196,28 +235,34 @@ def cmd_monitor(ser: serial.Serial):
|
||||
|
||||
cmd, data = result
|
||||
if cmd == CMD_TEMP:
|
||||
if len(data) >= 2:
|
||||
# sensor_value: val1 (int8), val2 (int8)
|
||||
t = data[0]
|
||||
frac = data[1] / 100.0
|
||||
temp = t + frac
|
||||
print(f"🌡 Temp: {temp:.2f} °C (raw: {data.hex(' ')})")
|
||||
else:
|
||||
print(f"⚠ TEMP with invalid data: {data.hex()}")
|
||||
t = _temp_from_data(data)
|
||||
print(f"🌡 Temp: {t:.2f}°C (raw: {data.hex(' ')})")
|
||||
elif cmd == CMD_GET_ID:
|
||||
print(f"🆔 Device ID response: {data.hex(' ')}")
|
||||
print(f"🆔 Device ID: {data.hex(' ')}")
|
||||
elif cmd == CMD_RUNNING_STATE:
|
||||
print(f"🔁 Running state echo: 0x{data.hex()}")
|
||||
print(f"🔁 Running state: 0x{data.hex()}")
|
||||
elif cmd == CMD_R_HEATING:
|
||||
if len(data) >= 3:
|
||||
state = HEATING_STATE_NAMES.get(data[0], f"?{data[0]}")
|
||||
t = _temp_from_data(data[1:3])
|
||||
print(f"🔥 Heating: {state}, temp={t:.2f}°C")
|
||||
else:
|
||||
print(f"🔥 Heating: {data.hex()}")
|
||||
elif cmd == CMD_READ_POLE_NTC:
|
||||
t = _temp_from_data(data)
|
||||
print(f"📡 Pole NTC reply: {t:.2f}°C")
|
||||
elif cmd == CMD_READ_HEATING_NTC:
|
||||
t = _temp_from_data(data)
|
||||
print(f"🔥 Heating pad NTC reply: {t:.2f}°C")
|
||||
else:
|
||||
print(f"📦 Unknown cmd=0x{cmd:02X} data={data.hex()}")
|
||||
print(f"📦 cmd=0x{cmd:02X} data={data.hex()}")
|
||||
except KeyboardInterrupt:
|
||||
print("\nDone.")
|
||||
|
||||
|
||||
def cmd_loop(ser: serial.Serial, count: int = 0):
|
||||
"""Send GET_ID in a loop."""
|
||||
i = 0
|
||||
print(f"Looping GET_ID... (Ctrl+C to stop)")
|
||||
print("Looping GET_ID... (Ctrl+C to stop)")
|
||||
try:
|
||||
while count == 0 or i < count:
|
||||
cmd_get_id(ser, timeout=1.0)
|
||||
@ -230,46 +275,37 @@ def cmd_loop(ser: serial.Serial, count: int = 0):
|
||||
# ── Main ──────────────────────────────────────────────────────
|
||||
def main():
|
||||
parser = argparse.ArgumentParser(
|
||||
description="Test script for app_photomagnetic communication protocol"
|
||||
description="Test script for app_photomagnetic comm protocol"
|
||||
)
|
||||
parser.add_argument("port", help="Serial port (e.g. /dev/ttyUSB0, COM3)")
|
||||
parser.add_argument("port", help="Serial port")
|
||||
parser.add_argument("--baud", type=int, default=115200)
|
||||
parser.add_argument("--get-id", action="store_true")
|
||||
parser.add_argument(
|
||||
"--baud", type=int, default=115200, help="Baud rate (default: 115200)"
|
||||
)
|
||||
"--state", type=lambda x: int(x, 0), metavar="N",
|
||||
help="Set running state")
|
||||
parser.add_argument(
|
||||
"--timeout", type=float, default=0.1, help="Serial timeout in seconds"
|
||||
)
|
||||
"--heating", type=int, metavar="TEMP",
|
||||
help="Write heating target (0=stop)")
|
||||
parser.add_argument(
|
||||
"--get-id", action="store_true", help="Send GET_ID and print response"
|
||||
)
|
||||
"--read-heating", action="store_true",
|
||||
help="Read heating state + temp")
|
||||
parser.add_argument(
|
||||
"--state",
|
||||
type=lambda x: int(x, 0),
|
||||
metavar="N",
|
||||
help="Set running state (0=Standby, 1=Running, 2=Pause, ...)",
|
||||
)
|
||||
"--read-pole-ntc", type=int, metavar="IDX",
|
||||
help="Read pole NTC by index (0..7)")
|
||||
parser.add_argument(
|
||||
"--monitor", action="store_true", help="Listen for incoming frames"
|
||||
)
|
||||
"--read-heating-ntc", action="store_true",
|
||||
help="Read heating pad NTC temperature")
|
||||
parser.add_argument("--monitor", action="store_true")
|
||||
parser.add_argument(
|
||||
"--loop",
|
||||
type=int,
|
||||
nargs="?",
|
||||
const=0,
|
||||
metavar="N",
|
||||
help="Send GET_ID in a loop (N times, or infinite if omitted)",
|
||||
)
|
||||
"--loop", type=int, nargs="?", const=0, metavar="N",
|
||||
help="Loop GET_ID N times")
|
||||
parser.add_argument(
|
||||
"--raw",
|
||||
type=lambda x: bytes.fromhex(x),
|
||||
metavar="HEX",
|
||||
help="Send raw payload (CMD DATA..., CRC auto-appended)",
|
||||
)
|
||||
parser.add_argument("--debug", action="store_true", help="Show raw frame hex")
|
||||
"--raw", type=lambda x: bytes.fromhex(x), metavar="HEX",
|
||||
help="Send raw CMD+DATA")
|
||||
|
||||
args = parser.parse_args()
|
||||
|
||||
ser = serial.Serial(args.port, args.baud, timeout=args.timeout)
|
||||
ser = serial.Serial(args.port, args.baud, timeout=0.1)
|
||||
print(f"Connected to {args.port} @ {args.baud} baud")
|
||||
|
||||
try:
|
||||
@ -277,20 +313,28 @@ def main():
|
||||
cmd_get_id(ser)
|
||||
elif args.state is not None:
|
||||
cmd_set_state(ser, args.state)
|
||||
elif args.heating is not None:
|
||||
cmd_write_heating(ser, args.heating)
|
||||
elif args.read_heating:
|
||||
cmd_read_heating(ser)
|
||||
elif args.read_pole_ntc is not None:
|
||||
cmd_read_pole_ntc(ser, args.read_pole_ntc)
|
||||
elif args.read_heating_ntc:
|
||||
cmd_read_heating_ntc(ser)
|
||||
elif args.monitor:
|
||||
cmd_monitor(ser)
|
||||
elif args.loop is not None:
|
||||
cmd_loop(ser, args.loop)
|
||||
elif args.raw is not None:
|
||||
payload = args.raw
|
||||
cmd, data = payload[0], payload[1:] if len(payload) > 1 else b""
|
||||
print(f"→ Sending CMD=0x{cmd:02X} data={data.hex()}")
|
||||
frame = build_frame(cmd, data)
|
||||
print(f" Frame: {frame.hex()}")
|
||||
ser.write(frame)
|
||||
print(f"✓ Sent")
|
||||
c = payload[0]
|
||||
d = payload[1:] if len(payload) > 1 else b""
|
||||
print(f"→ Sending CMD=0x{c:02X} data={d.hex()}")
|
||||
ser.write(build_frame(c, d))
|
||||
print("✓ Sent")
|
||||
else:
|
||||
print("No action. Use --get-id, --state N, --monitor, --loop, or --raw")
|
||||
print("Use --get-id, --state, --heating, --read-heating, "
|
||||
"--read-pole-ntc, --read-heating-ntc, --monitor, --loop, --raw")
|
||||
finally:
|
||||
ser.close()
|
||||
|
||||
|
||||
12
src/main.cpp
12
src/main.cpp
@ -20,17 +20,20 @@ auto GetmaxTemp() -> std::optional<sensor_value> {
|
||||
max_val = *ntc_result;
|
||||
found = true;
|
||||
}
|
||||
|
||||
// 红外数据就绪时参与比较
|
||||
sensor_value ir_val{};
|
||||
if (0 == sensor_sample_fetch_chan(infrared, SENSOR_CHAN_AMBIENT_TEMP)) {
|
||||
if (0 ==
|
||||
sensor_channel_get(infrared, SENSOR_CHAN_AMBIENT_TEMP, &ir_val)) {
|
||||
printk("[ir] = %d.%d°C\n", ir_val.val1, ir_val.val2);
|
||||
double ir_t = sensor_value_to_double(&ir_val);
|
||||
// printk("[ir] = %.2f°C\n", ir_t);
|
||||
if (!found || ir_val.val1 > max_val.val1 ||
|
||||
(ir_val.val1 == max_val.val1 && ir_val.val2 > max_val.val2)) {
|
||||
max_val = ir_val;
|
||||
}
|
||||
max_val = ir_val;
|
||||
|
||||
// printk("[out] sent = %.2f°C\n", sensor_value_to_double(&max_val));
|
||||
is_infrared_ready = false;
|
||||
}
|
||||
}
|
||||
@ -45,7 +48,7 @@ auto main(void) -> int {
|
||||
// auto wdt = app::WatchDogConfig{};
|
||||
StatusLed::Flash(1s);
|
||||
ther::Com::Init();
|
||||
ther::HeatingPad::Start(sensor_value{42, 0});
|
||||
// ther::HeatingPad::Start(sensor_value{42, 0});
|
||||
sensor_trigger tri{.type = SENSOR_TRIG_DATA_READY,
|
||||
.chan = SENSOR_CHAN_AMBIENT_TEMP};
|
||||
sensor_trigger_set(infrared, &tri,
|
||||
@ -56,7 +59,8 @@ auto main(void) -> int {
|
||||
if (auto v = GetmaxTemp(); v.has_value()) {
|
||||
ther::Com::Send(v.value());
|
||||
}
|
||||
k_sleep(K_MSEC(50));
|
||||
|
||||
k_sleep(K_MSEC(20));
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user