Andrey/templates
1
0
Fork 0
Code Issues Pull Requests 1 Actions Packages Projects Releases Wiki Activity

Добавить общие графики, декодер KONOR и порт STM32 bxCAN

Browse Source
This commit is contained in:
Andrey
2026-09-04 12:22:17 +03:00
parent af82687e42
commit e163d9ba55
51 changed files with 5040 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files

71
c/set-protocol/src/set_plot.c Normal file
View File

@@ -0,0 +1,71 @@
#include "set_plot.h"
#include <math.h>
static double clamp(double v, double lo, double hi) { return fmin(hi, fmax(lo, v)); }
static int finite_values(const double *v, size_t n) {
size_t i;
for (i = 0; i < n; ++i) if (!isfinite(v[i])) return 0;
return 1;
}
uint32_t set_plot_abi_version(void) { return 1U; }
size_t set_plot_eval(uint32_t op, const double *v, size_t n, double *out, size_t cap) {
static const size_t sizes[] = {10, 3, 4, 4, 6, 2, 3};
double span, fraction;
if (op > SET_PLOT_DELTA || !v || !out || n != sizes[op] ||
cap < (op == SET_PLOT_TRANSFORM ? 4U : 1U)) return 0;
if (op == SET_PLOT_TRANSFORM) {
double w, h, fx, fy;
if (!finite_values(v, 4) || v[2] < 1.0/128 || v[2] > 1 ||
v[3] < 1.0/128 || v[3] > 1 || v[0] < 0 || v[1] < 0 ||
v[0] + v[2] > 1.000000000001 || v[1] + v[3] > 1.000000000001) return 0;
out[0] = v[0]; out[1] = v[1]; out[2] = v[2]; out[3] = v[3];
if (!finite_values(v + 4, 6) || v[4] <= 0 || v[5] <= 0) return 4;
w = clamp(v[2] / v[4], 1.0/128, 1);
h = clamp(v[3] / v[5], 1.0/128, 1);
fx = clamp(v[8], 0, 1); fy = clamp(v[9], 0, 1);
out[0] = clamp(v[0] + (v[2] - w) * fx - v[6] * w, 0, 1 - w);
out[1] = clamp(v[1] + (v[3] - h) * fy - v[7] * h, 0, 1 - h);
out[2] = w; out[3] = h;
return 4;
}
if (!finite_values(v, n)) return 0;
switch (op) {
case SET_PLOT_AXIS:
if (v[2] < 0) return 0;
out[0] = fmax(fabs(v[0]), fabs(v[1])) <= v[2] ? 0 :
(fabs(v[1]) > fabs(v[0]) ? 2 : 1);
break;
case SET_PLOT_FRACTION:
case SET_PLOT_VALUE:
span = v[2] - v[1];
if (!isfinite(span) || span <= 0 || (v[3] != 0 && v[3] != 1)) return 0;
if (op == SET_PLOT_FRACTION) {
fraction = (v[0] - v[1]) / span;
out[0] = v[3] ? 1 - fraction : fraction;
} else {
fraction = v[3] ? 1 - v[0] : v[0];
out[0] = v[1] + fraction * span;
}
break;
case SET_PLOT_DRAG:
span = v[4] - v[3];
if (!isfinite(span) || span <= 0 || v[2] <= 0 || (v[5] != 0 && v[5] != 1)) return 0;
out[0] = clamp(v[0] + (v[5] ? -1 : 1) * (v[1] / v[2]) * span, v[3], v[4]);
break;
case SET_PLOT_TICK_STEP: {
double raw, magnitude, scaled;
if (v[0] <= 0 || v[1] <= 0) return 0;
raw = v[0] / fmax(2, floor(v[1] / 100));
magnitude = pow(10, floor(log10(raw)));
if (magnitude <= 0 || !isfinite(magnitude)) return 0;
scaled = raw / magnitude;
out[0] = (scaled <= 1 ? 1 : scaled <= 2 ? 2 : scaled <= 5 ? 5 : 10) * magnitude;
break;
}
case SET_PLOT_DELTA: out[0] = (v[1] - v[0]) * v[2]; break;
default: return 0;
}
return isfinite(out[0]) ? 1 : 0;
}

126
c/set-protocol/src/set_spectrum.c Normal file
View File

@@ -0,0 +1,126 @@
#include "set_spectrum.h"
#include <math.h>
#include <stdlib.h>
#define PI 3.14159265358979323846
static double window_value(int window, size_t index, size_t n)
{
double phase = 2.0 * PI * (double)index / (double)n;
switch (window) {
case SET_WINDOW_HANN: return 0.5 - 0.5 * cos(phase);
case SET_WINDOW_HAMMING: return 0.54 - 0.46 * cos(phase);
case SET_WINDOW_BLACKMAN: return 0.42 - 0.5 * cos(phase) + 0.08 * cos(2.0 * phase);
case SET_WINDOW_FLATTOP: return 0.21557895 - 0.41663158 * cos(phase) +
0.277263158 * cos(2.0 * phase) - 0.083578947 * cos(3.0 * phase) + 0.006947368 * cos(4.0 * phase);
default: return 1.0;
}
}
/* RBJ biquads, https://www.w3.org/TR/audio-eq-cookbook/ (LP/HP: Q=1/sqrt(2)). */
static void filter_block(double *data, size_t n, double fs, double cutoff, int kind)
{
double w = 2.0 * PI * cutoff / fs, c = cos(w);
double alpha = sin(w) / (2.0 * (kind == SET_FILTER_NOTCH ? 30.0 : sqrt(0.5)));
double a0 = 1.0 + alpha, a1 = -2.0 * c / a0, a2 = (1.0 - alpha) / a0;
double b0, b1, b2;
if (kind == SET_FILTER_LOW_PASS) {
b0 = (1.0 - c) / (2.0 * a0); b1 = 2.0 * b0; b2 = b0;
} else if (kind == SET_FILTER_HIGH_PASS) {
b0 = (1.0 + c) / (2.0 * a0); b1 = -2.0 * b0; b2 = b0;
} else { b0 = 1.0 / a0; b1 = -2.0 * c / a0; b2 = b0; }
double x1 = data[0], x2 = x1;
double y1 = kind == SET_FILTER_HIGH_PASS ? 0.0 : x1, y2 = y1;
for (size_t i = 0; i < n; ++i) {
double x = data[i], y = b0 * x + b1 * x1 + b2 * x2 - a1 * y1 - a2 * y2;
data[i] = y; x2 = x1; x1 = x; y2 = y1; y1 = y;
}
}
static void fft(double *re, double *im, size_t n)
{
for (size_t i = 1, j = 0; i < n; ++i) {
size_t bit = n >> 1;
for (; j & bit; bit >>= 1) j ^= bit;
j ^= bit;
if (i < j) { double tmp = re[i]; re[i] = re[j]; re[j] = tmp; }
}
for (size_t length = 2; length <= n; length <<= 1) {
double angle = -2.0 * PI / (double)length, wr = cos(angle), wi = sin(angle);
for (size_t start = 0; start < n; start += length) {
double tr = 1.0, ti = 0.0;
for (size_t k = 0; k < length / 2; ++k) {
size_t a = start + k, b = a + length / 2;
double br = re[b] * tr - im[b] * ti, bi = re[b] * ti + im[b] * tr;
re[b] = re[a] - br; im[b] = im[a] - bi; re[a] += br; im[a] += bi;
double next = tr * wr - ti * wi;
ti = tr * wi + ti * wr; tr = next;
}
}
}
}
int set_spectrum_analyze(const double *times, const double *values, size_t count,
size_t max_size, int window, int filter, double low_hz, double high_hz, int remove_mean,
double *amplitudes, size_t capacity, double *meta)
{
if (meta == NULL) return SET_SPECTRUM_INVALID;
meta[0] = meta[1] = meta[2] = 0.0;
if (times == NULL || values == NULL || amplitudes == NULL || max_size < 16 ||
max_size > SET_SPECTRUM_MAX || (max_size & (max_size - 1)) != 0 ||
window < SET_WINDOW_RECT || window > SET_WINDOW_FLATTOP ||
filter < SET_FILTER_NONE || filter > SET_FILTER_NOTCH) return SET_SPECTRUM_INVALID;
if (count < 16) return SET_SPECTRUM_SHORT;
size_t n = 16;
while (n * 2 <= count && n * 2 <= max_size) n *= 2;
if (capacity < n / 2 + 1) return SET_SPECTRUM_INVALID;
times += count - n; values += count - n;
double dt = (times[n - 1] - times[0]) / (double)(n - 1);
if (!isfinite(dt) || dt <= 0.0) return SET_SPECTRUM_TIMING;
double fs = 1.0 / dt, jitter = 0.0;
if (!isfinite(fs)) return SET_SPECTRUM_TIMING;
for (size_t i = 0; i < n; ++i) {
if (!isfinite(values[i]) || !isfinite(times[i])) return SET_SPECTRUM_INVALID;
if (i > 0) {
double step = times[i] - times[i - 1];
if (step <= 0.0) return SET_SPECTRUM_TIMING;
double relative = fabs(step / dt - 1.0);
if (relative > jitter) jitter = relative;
}
}
meta[0] = (double)n; meta[1] = fs; meta[2] = jitter;
if (jitter > 0.5) return SET_SPECTRUM_TIMING;
int need_low = filter == SET_FILTER_HIGH_PASS || filter == SET_FILTER_BAND_PASS || filter == SET_FILTER_NOTCH;
int need_high = filter == SET_FILTER_LOW_PASS || filter == SET_FILTER_BAND_PASS;
if ((need_low && (!isfinite(low_hz) || low_hz <= 0.0 || low_hz >= fs / 2.0)) ||
(need_high && (!isfinite(high_hz) || high_hz <= 0.0 || high_hz >= fs / 2.0)) ||
(filter == SET_FILTER_BAND_PASS && low_hz >= high_hz)) return SET_SPECTRUM_CUTOFF;
double *scratch = (double *)calloc(n * 2, sizeof(double));
if (scratch == NULL) return SET_SPECTRUM_MEMORY;
double *re = scratch, *im = scratch + n, mean = 0.0;
size_t right = 1;
for (size_t i = 0; i < n; ++i) {
double time = times[0] + (double)i * dt;
while (right < n - 1 && times[right] < time) ++right;
double fraction = (time - times[right - 1]) / (times[right] - times[right - 1]);
fraction = fmax(0.0, fmin(1.0, fraction));
re[i] = values[right - 1] * (1.0 - fraction) + values[right] * fraction;
mean += re[i] / (double)n;
}
if (remove_mean) for (size_t i = 0; i < n; ++i) re[i] -= mean;
if (filter == SET_FILTER_HIGH_PASS || filter == SET_FILTER_BAND_PASS)
filter_block(re, n, fs, low_hz, SET_FILTER_HIGH_PASS);
if (filter == SET_FILTER_LOW_PASS || filter == SET_FILTER_BAND_PASS)
filter_block(re, n, fs, high_hz, SET_FILTER_LOW_PASS);
if (filter == SET_FILTER_NOTCH) filter_block(re, n, fs, low_hz, SET_FILTER_NOTCH);
double gain = 0.0;
for (size_t i = 0; i < n; ++i) { double w = window_value(window, i, n); gain += w; re[i] *= w; }
fft(re, im, n);
int result = SET_SPECTRUM_OK;
for (size_t i = 0; i <= n / 2; ++i) {
amplitudes[i] = hypot(re[i], im[i]) / gain * ((i == 0 || i == n / 2) ? 1.0 : 2.0);
if (!isfinite(amplitudes[i])) result = SET_SPECTRUM_INVALID;
}
free(scratch);
return result;
}

74
c/set-protocol/src/set_trends.c Normal file
View File

@@ -0,0 +1,74 @@
#include "set_trends.h"
#include "pcan_id.h"
static uint16_t get16(const uint8_t *p)
{
return (uint16_t)((uint16_t)p[0] | ((uint16_t)p[1] << 8));
}
static void put16(uint8_t *p, uint16_t value)
{
p[0] = (uint8_t)value;
p[1] = (uint8_t)(value >> 8);
}
int32_t set_trend_word_value(uint16_t word, uint8_t is_signed)
{
return (is_signed != 0U && word >= 0x8000U) ? (int32_t)word - 65536 : (int32_t)word;
}
int32_t set_trend_can_value(
uint8_t source, uint32_t address, uint8_t device_type, uint8_t device,
uint8_t byte_offset, uint8_t extended, uint8_t is_signed,
uint32_t can_id, uint8_t flags, const uint8_t *data, size_t size)
{
size_t offset;
const uint8_t ide = (uint8_t)(flags & 1U);
if (data == NULL || size < 2U || size > 8U || (flags & 0x0EU) != 0U ||
can_id > (ide != 0U ? 0x1FFFFFFFUL : 0x7FFUL)) return SET_TREND_NO_VALUE;
if (source == SET_TREND_CAN_GAS) {
pcan_id_t id;
if (ide == 0U || (size & 1U) != 0U || address > 0xFFFFUL ||
device_type > 7U || device > 15U) return SET_TREND_NO_VALUE;
pcan_id_unpack(can_id, &id);
if (id.msg_type != PCAN_MSG_GAS || id.route != PCAN_ROUTE_FROM_DEVICE ||
id.device_type != device_type || id.device_id != device ||
address < id.msg_body) return SET_TREND_NO_VALUE;
offset = (size_t)(address - id.msg_body) * 2U;
} else if (source == SET_TREND_CAN_RAW) {
if (ide != (extended != 0U ? 1U : 0U) || can_id != address || byte_offset > 6U)
return SET_TREND_NO_VALUE;
offset = byte_offset;
} else return SET_TREND_NO_VALUE;
if (offset + 2U > size) return SET_TREND_NO_VALUE;
return set_trend_word_value(get16(data + offset), is_signed);
}
size_t set_trend_watch_request(uint16_t period_ms, const uint16_t *addresses,
size_t count, uint8_t *output, size_t capacity)
{
size_t i;
if (count > SET_TREND_WATCH_MAX || output == NULL || capacity < 4U + count * 2U ||
(count > 0U && addresses == NULL)) return 0U;
put16(output, period_ms);
put16(output + 2U, (uint16_t)count);
for (i = 0U; i < count; ++i) put16(output + 4U + i * 2U, addresses[i]);
return 4U + count * 2U;
}
int set_trend_watch_ack(const uint8_t *payload, size_t size, uint16_t period_ms, size_t count)
{
return payload != NULL && size == 4U && count <= SET_TREND_WATCH_MAX &&
get16(payload) == period_ms && get16(payload + 2U) == count;
}
int set_trend_watch_values(const uint8_t *payload, size_t size, uint16_t *words, size_t capacity)
{
size_t i, count;
if (payload == NULL || size < 6U) return -1;
count = get16(payload + 4U);
if (count > SET_TREND_WATCH_MAX || size != 6U + count * 2U || count > capacity ||
(count > 0U && words == NULL)) return -1;
for (i = 0U; i < count; ++i) words[i] = get16(payload + 6U + i * 2U);
return (int)count;
}