第一次提交
This commit is contained in:
148
Bsp/cal_creep/lib_cal_creep_resistance.c
Normal file
148
Bsp/cal_creep/lib_cal_creep_resistance.c
Normal file
@@ -0,0 +1,148 @@
|
||||
#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);
|
||||
|
||||
}
|
||||
|
||||
/* <20>Ĺ<EFBFBD><C4B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڣ<EFBFBD><DAA3><EFBFBD><EFBFBD><EFBFBD>ԭ cal_creep_resistance_t <20><><EFBFBD><EFBFBD> */
|
||||
void cal_creep_resistance_4tile(const cal_creep_resistance_t *cfg)
|
||||
{
|
||||
/* ԭ<><D4AD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> */
|
||||
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;
|
||||
|
||||
/* <20><>̬<EFBFBD><CCAC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ŀ鹲<C4BF><E9B9B2>ͬһ֡<D2BB>ţ<EFBFBD><C5A3>̰߳<DFB3>ȫֻ<C8AB><D6BB><EFBFBD><EFBFBD> */
|
||||
static uint64_t frame_count = 0;
|
||||
#pragma omp atomic update
|
||||
frame_count++; /* ԭ<><D4AD>+1<><31><EFBFBD><EFBFBD>ֹ<EFBFBD><D6B9>̬ */
|
||||
|
||||
/* <20><><EFBFBD><EFBFBD><EFBFBD>Ŀ<EFBFBD> */
|
||||
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)
|
||||
{
|
||||
/* <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><DFBC><EFBFBD><EFBFBD><EFBFBD> */
|
||||
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;
|
||||
|
||||
/* <20><>ָ<EFBFBD>루<EFBFBD><EBA3A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> */
|
||||
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;
|
||||
|
||||
/* <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ԭ<EFBFBD>㷨 */
|
||||
for (uint8_t yy = 0; yy < bh; ++yy)
|
||||
{
|
||||
for (uint8_t xx = 0; xx < bw; ++xx)
|
||||
{
|
||||
uint16_t idx = yy * x_max + xx; /* ȫ<>ֿ<EFBFBD><D6BF><EFBFBD> */
|
||||
|
||||
uint8_t r = b_real[idx];
|
||||
if (r < 2)
|
||||
{
|
||||
b_disp[idx] = 0;
|
||||
b_creep[idx] = 0;
|
||||
continue;
|
||||
}
|
||||
|
||||
/* UPDATE <20>Σ<D7B6>ֻ<EFBFBD>ڼ<EFBFBD><DABC><EFBFBD>֡<EFBFBD><D6A1> */
|
||||
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 <20><EFBFBD> */
|
||||
uint8_t c = b_creep[idx];
|
||||
b_disp[idx] = (r > c) ? r - c : 0;
|
||||
if (r < c) b_creep[idx] = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user