236 lines
6.6 KiB
C
236 lines
6.6 KiB
C
#include "kit_table.h"
|
||
|
||
|
||
//根据数据类型(1字节 2字节 4字节)来查表
|
||
KitResult kit_table_get_data(TabItem* item, uint8_t idx, int32_t * value)
|
||
{
|
||
KitResult res = kKitResult_ParamErr;
|
||
|
||
if ((item != NULL) && (item->tab != NULL) && (idx < item->len))
|
||
{
|
||
if (item->type == kTabDataType_8Bit)
|
||
{
|
||
*value = ((int8_t *)item->tab)[idx];
|
||
}
|
||
else if (item->type == kTabDataType_16Bit)
|
||
{
|
||
*value = ((int16_t*)item->tab)[idx];
|
||
}
|
||
else
|
||
{
|
||
*value = ((int32_t*)item->tab)[idx];
|
||
}
|
||
res = kKitResult_Ok;
|
||
}
|
||
return res;
|
||
}
|
||
|
||
//根据数据类型(1字节 2字节 4字节)来写数据
|
||
KitResult kit_table_set_data(TabItem* item, uint8_t idx, int32_t value)
|
||
{
|
||
KitResult res = kKitResult_ParamErr;
|
||
|
||
if ((item != NULL) && (item->tab != NULL) && (idx < item->len))
|
||
{
|
||
if (item->type == kTabDataType_8Bit)
|
||
{
|
||
((int8_t*)item->tab)[idx] = value;
|
||
}
|
||
else if (item->type == kTabDataType_16Bit)
|
||
{
|
||
((int16_t*)item->tab)[idx] = value;
|
||
}
|
||
else
|
||
{
|
||
((int32_t*)item->tab)[idx] = value;
|
||
}
|
||
res = kKitResult_Ok;
|
||
}
|
||
|
||
return res;
|
||
}
|
||
|
||
//返回kKitResult_OutRange 数据超出上下限
|
||
KitResult kit_table_search_index(TabItem *item, int32_t input_value, uint8_t * left_idx, uint8_t * right_idx)
|
||
{
|
||
KitResult res = kKitResult_ParamErr;
|
||
uint8_t end;
|
||
int32_t i, v1, v2;
|
||
if ((item != NULL) && (item->len > 1))
|
||
{
|
||
end = item->len - 1;
|
||
//查找两端数据
|
||
if ((kit_table_get_data(item, 0, &v1) == kKitResult_Ok)
|
||
&& (kit_table_get_data(item, end, &v2) == kKitResult_Ok))
|
||
{
|
||
if (((v1 < v2) && (input_value < v1)) || ((v1 > v2) && (input_value > v1)))
|
||
{
|
||
*left_idx = *right_idx = 0;
|
||
res = kKitResult_OutRange;
|
||
}
|
||
else if (((v1 < v2) && (input_value > v2)) || ((v1 > v2) && (input_value < v2)))
|
||
{
|
||
*left_idx = *right_idx = end;
|
||
res = kKitResult_OutRange;
|
||
}
|
||
else
|
||
{
|
||
*left_idx = 0;
|
||
*right_idx = 1;
|
||
for (i = 0; i < end; i++)
|
||
{
|
||
kit_table_get_data(item, i, &v1);
|
||
kit_table_get_data(item, i + 1, &v2);
|
||
if ((input_value >= v1 && input_value <= v2) || (input_value <= v1 && input_value >= v2))
|
||
{
|
||
*left_idx = i;
|
||
*right_idx = i + 1;
|
||
break;
|
||
}
|
||
}
|
||
res = kKitResult_Ok;
|
||
}
|
||
}
|
||
}
|
||
return res;
|
||
}
|
||
|
||
|
||
|
||
KitResult kit_table_linear_search(TwoDTabItem *item, int32_t input_value, uint32_t factor, int32_t *output_value)
|
||
{
|
||
KitResult res = kKitResult_ParamErr;
|
||
uint8_t left_idx, right_idx;
|
||
int32_t dat_x, dat_y, x_left_value, x_right_value, y_left_value, y_right_value;
|
||
|
||
*output_value = 0;
|
||
if ((item != NULL) && (item->x.tab != NULL) && (item->y.tab != NULL))
|
||
{
|
||
res = kit_table_search_index(&item->x, input_value, &left_idx, &right_idx);
|
||
|
||
kit_table_get_data(&item->y, left_idx, &y_left_value);
|
||
kit_table_get_data(&item->y, right_idx, &y_right_value);
|
||
|
||
if((res == kKitResult_OutRange) || (y_left_value == y_right_value))
|
||
{
|
||
*output_value = y_left_value * factor;
|
||
}
|
||
else
|
||
{
|
||
kit_table_get_data(&item->y, left_idx, &y_left_value);
|
||
kit_table_get_data(&item->y, right_idx, &y_right_value);
|
||
|
||
kit_table_get_data(&item->x, left_idx, &x_left_value);
|
||
kit_table_get_data(&item->x, right_idx, &x_right_value);
|
||
|
||
dat_x = x_right_value - x_left_value;
|
||
dat_y = y_right_value - y_left_value;
|
||
|
||
*output_value = ((int64_t)dat_y * factor * ((int32_t)input_value - x_left_value) / dat_x) + y_left_value * factor;
|
||
|
||
res = kKitResult_Ok;
|
||
}
|
||
}
|
||
return res;
|
||
}
|
||
|
||
|
||
KitResult kit_table_bilinear_search(ThreeDTabItem *item, int32_t x_input, int32_t y_input, int32_t *z_output)
|
||
{
|
||
KitResult res = kKitResult_Ok;
|
||
uint8_t x_left_idx, x_right_idx, y_left_idx, y_right_idx;
|
||
int32_t data_x1_y1, data_x1_y2, data_x2_y1, data_x2_y2;
|
||
int32_t x1, x2, y1, y2, dat_x;
|
||
|
||
|
||
res &= kit_table_search_index(&item->x, x_input, &x_left_idx, &x_right_idx);
|
||
res &= kit_table_search_index(&item->y, y_input, &y_left_idx, &y_right_idx);
|
||
|
||
if (res == kKitResult_Ok)
|
||
{
|
||
kit_table_get_data(&item->z, item->x.len * y_left_idx + x_left_idx, &data_x1_y1);
|
||
kit_table_get_data(&item->z, item->x.len * y_right_idx + x_left_idx, &data_x1_y2);
|
||
kit_table_get_data(&item->z, item->x.len * y_left_idx + x_right_idx, &data_x2_y1);
|
||
kit_table_get_data(&item->z, item->x.len * y_right_idx + x_right_idx, &data_x2_y2);
|
||
|
||
kit_table_get_data(&item->x, x_left_idx, &x1);
|
||
kit_table_get_data(&item->x, x_right_idx, &x2);
|
||
kit_table_get_data(&item->y, y_left_idx, &y1);
|
||
kit_table_get_data(&item->y, y_right_idx, &y2);
|
||
|
||
dat_x = (x2 - x1) * (y2 - y1);
|
||
|
||
*z_output = data_x1_y1 * (x2 - (int32_t)x_input) * (y2 - (int32_t)y_input) + data_x2_y1 * ((int32_t)x_input - x1) * (y2 - (int32_t)y_input)
|
||
+ data_x1_y2 * (x2 - (int32_t)x_input) * ((int32_t)y_input - y1) + data_x2_y2 * ((int32_t)x_input - x1) * ((int32_t)y_input - y1);
|
||
|
||
*z_output = (*z_output + (dat_x >> 1)) / dat_x;
|
||
}
|
||
|
||
return res;
|
||
}
|
||
|
||
KitResult kit_table_bilinear_search_by_yz(ThreeDTabItem* item, int32_t y_input, int32_t z_input, uint32_t factor, int32_t* x_output)
|
||
{
|
||
KitResult res = kKitResult_ParamErr;
|
||
uint8_t y_left_idx, y_right_idx, value[200];
|
||
int32_t i, tmp, dat_x, dat_y;
|
||
int32_t x_left_value, x_right_value, y_left_value, y_right_value;
|
||
TwoDTabItem table;
|
||
|
||
if (item != NULL)
|
||
{
|
||
res = kit_table_search_index(&item->y, y_input, &y_left_idx, &y_right_idx);
|
||
|
||
if ((res == kKitResult_Ok) || (res == kKitResult_OutRange))
|
||
{
|
||
//创建二维表，x轴数据为拟合出来新数据，y轴为原来三维表的x轴数据
|
||
table.x.len = item->x.len;
|
||
table.x.tab = (uint8_t*)value;
|
||
table.x.type = item->z.type;
|
||
|
||
table.y.len = item->x.len;
|
||
table.y.tab = item->x.tab;
|
||
table.y.type = item->x.type;
|
||
|
||
kit_table_get_data(&item->y, y_left_idx, &y_left_value);
|
||
kit_table_get_data(&item->y, y_right_idx, &y_right_value);
|
||
|
||
dat_x = y_input - y_left_value;
|
||
//数据不用插值，直接取
|
||
if ((dat_x == 0) || (res == kKitResult_OutRange))
|
||
{
|
||
for (i = 0; i < item->x.len; i++)
|
||
{
|
||
kit_table_get_data(&item->z, item->x.len * y_left_idx + i, &x_left_value);
|
||
kit_table_set_data(&table.x, i, x_left_value);
|
||
}
|
||
}
|
||
//数据不用插值，直接取
|
||
else if (y_input == y_right_value)
|
||
{
|
||
for (i = 0; i < item->x.len; i++)
|
||
{
|
||
kit_table_get_data(&item->z, item->x.len * y_right_idx + i, &x_left_value);
|
||
kit_table_set_data(&table.x, i, x_left_value);
|
||
}
|
||
}
|
||
else
|
||
{
|
||
dat_y = y_right_value - y_left_value;
|
||
for (i = 0; i < item->x.len; i++)
|
||
{
|
||
kit_table_get_data(&item->z, item->x.len * y_left_idx + i, &x_left_value);
|
||
kit_table_get_data(&item->z, item->x.len * y_right_idx + i, &x_right_value);
|
||
|
||
tmp = (x_right_value - x_left_value) * dat_x / dat_y + x_left_value;
|
||
kit_table_set_data(&table.x, i, tmp);
|
||
}
|
||
}
|
||
|
||
res = kit_table_linear_search(&table, z_input, factor, x_output);
|
||
}
|
||
}
|
||
|
||
return res;
|
||
}
|