149 lines
4.5 KiB
C
149 lines
4.5 KiB
C
#include "lib_cal_creep_resistance.h"
|
||
|
||
#define CREEP_VALUE 1
|
||
#define CREEP_INTERVAL_FRAME 5
|
||
|
||
uint8_t g_creep_value = 0;
|
||
uint8_t g_creep_interval_frame = 0;
|
||
|
||
|
||
static cal_creep_resistance_t creep_strength_value;
|
||
|
||
cal_creep_resistance_t * svc_creep_strength_value_get(void)
|
||
{
|
||
return &creep_strength_value;
|
||
}
|
||
|
||
|
||
void cal_creep_resistance_init(uint8_t creep_strength, uint8_t creep_level)
|
||
{
|
||
g_creep_value = creep_strength;
|
||
g_creep_interval_frame = creep_level;
|
||
}
|
||
|
||
void cal_creep_resistance(cal_creep_resistance_t *cfg)
|
||
{
|
||
static uint64_t frame_count = 0;
|
||
uint16_t count = cfg->x_max * cfg->y_max;
|
||
int16_t matrix_deviation = 0;
|
||
frame_count ++;
|
||
|
||
do{
|
||
count--;
|
||
|
||
if (cfg->matrix_real[count] < 2)
|
||
{
|
||
cfg->matrix_display[count] = 0;
|
||
cfg->matrix_creep[count] = 0;
|
||
continue;
|
||
}
|
||
|
||
if (!(frame_count % g_creep_interval_frame))
|
||
{
|
||
matrix_deviation = (cfg->matrix_real[count] > cfg->matrix_stab[count]) ?
|
||
(cfg->matrix_real[count] - cfg->matrix_stab[count]) : (cfg->matrix_stab[count] - cfg->matrix_real[count]);
|
||
|
||
if (matrix_deviation > g_creep_value)
|
||
{
|
||
|
||
}
|
||
else
|
||
{
|
||
matrix_deviation = (cfg->matrix_real[count] > cfg->matrix_stab[count]) ? matrix_deviation : -matrix_deviation;
|
||
cfg->matrix_creep[count] += matrix_deviation;
|
||
}
|
||
|
||
cfg->matrix_stab[count] = cfg->matrix_real[count];
|
||
}
|
||
|
||
cfg->matrix_display[count] = (cfg->matrix_real[count] > cfg->matrix_creep[count]) ?
|
||
(cfg->matrix_real[count] - cfg->matrix_creep[count]) : cfg->matrix_real[count];
|
||
|
||
if (cfg->matrix_real[count] < cfg->matrix_creep[count])
|
||
{
|
||
cfg->matrix_creep[count] = 0;
|
||
}
|
||
|
||
} while(count);
|
||
|
||
}
|
||
|
||
/* 四宫格并行入口，兼容原 cal_creep_resistance_t 配置 */
|
||
void cal_creep_resistance_4tile(const cal_creep_resistance_t *cfg)
|
||
{
|
||
/* 原矩阵参数 */
|
||
uint8_t x_max = cfg->x_max; /* 62 */
|
||
uint8_t y_max = cfg->y_max; /* 68 */
|
||
uint8_t *real = cfg->matrix_real;
|
||
uint8_t *stab = cfg->matrix_stab;
|
||
uint8_t *creep = cfg->matrix_creep;
|
||
uint8_t *disp = cfg->matrix_display;
|
||
|
||
/* 静态变量：四块共享同一帧号（线程安全只读） */
|
||
static uint64_t frame_count = 0;
|
||
#pragma omp atomic update
|
||
frame_count++; /* 原子+1，防止竞态 */
|
||
|
||
/* 均分四块 */
|
||
uint8_t w2 = x_max / 2; /* 31 */
|
||
uint8_t h2 = y_max / 2; /* 34 */
|
||
|
||
#pragma omp parallel for collapse(2) schedule(static) if(1)
|
||
for (int by = 0; by < 2; ++by)
|
||
{
|
||
for (int bx = 0; bx < 2; ++bx)
|
||
{
|
||
/* 本块逻辑区域 */
|
||
uint8_t x0 = bx * w2;
|
||
uint8_t y0 = by * h2;
|
||
uint8_t x1 = (bx == 1) ? x_max : x0 + w2;
|
||
uint8_t y1 = (by == 1) ? y_max : y0 + h2;
|
||
uint8_t bw = x1 - x0;
|
||
uint8_t bh = y1 - y0;
|
||
|
||
/* 基指针（按行主序） */
|
||
uint8_t *b_real = real + y0 * x_max + x0;
|
||
uint8_t *b_stab = stab + y0 * x_max + x0;
|
||
uint8_t *b_creep = creep + y0 * x_max + x0;
|
||
uint8_t *b_disp = disp + y0 * x_max + x0;
|
||
|
||
/* 对这块逐像素跑原算法 */
|
||
for (uint8_t yy = 0; yy < bh; ++yy)
|
||
{
|
||
for (uint8_t xx = 0; xx < bw; ++xx)
|
||
{
|
||
uint16_t idx = yy * x_max + xx; /* 全局跨度 */
|
||
|
||
uint8_t r = b_real[idx];
|
||
if (r < 2)
|
||
{
|
||
b_disp[idx] = 0;
|
||
b_creep[idx] = 0;
|
||
continue;
|
||
}
|
||
|
||
/* UPDATE 阶段：只在间隔帧做 */
|
||
if ((frame_count % g_creep_interval_frame) == 0)
|
||
{
|
||
int16_t s = b_stab[idx];
|
||
int16_t dev = (r > s) ? (r - s) : (s - r);
|
||
if (dev <= g_creep_value)
|
||
{
|
||
int16_t c = b_creep[idx];
|
||
c += (r > s) ? dev : -dev;
|
||
b_creep[idx] = (uint8_t)(c < 0 ? 0 : c);
|
||
}
|
||
b_stab[idx] = r;
|
||
}
|
||
|
||
/* DISPLAY 阶段 */
|
||
uint8_t c = b_creep[idx];
|
||
b_disp[idx] = (r > c) ? r - c : 0;
|
||
if (r < c) b_creep[idx] = 0;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|