Frame-rate-optimization/Lib/cali_process.c

#include "cali_process.h"

//ʵ<><CAB5>ԭ<EFBFBD><D4AD><EFBFBD><EFBFBD>ʹ<EFBFBD><CAB9><EFBFBD><EFBFBD><EFBFBD>Բ<EFBFBD>ֵ<EFBFBD><D6B5>ʽ y = y1 + (x-x1)*(y2-y1)/(x2-x1)
uint16_t linear_fuction_double_byte(uint16_t x, uint16_t x1, uint16_t x2, uint16_t y1, uint16_t y2)
{
	uint16_t y = 0;
	uint32_t cal_y = 0;
	if(x2==x1) return y1;
	
	if(x >= x1)
	{
		cal_y = (uint32_t)(x - x1) * (uint32_t)(y2 - y1) / (uint32_t)(x2 - x1) + y1;
		if(cal_y > UINT16_MAX)
		{
			cal_y = UINT16_MAX;
		}
		y = (uint16_t)cal_y;
		return y;
	}
	else
		return 0;
}

uint8_t linear_fuction_single_byte(uint16_t x, uint16_t x1, uint16_t x2, uint8_t y1, uint8_t y2)
{
	uint8_t y = 0;
	uint32_t cal_y = 0;
	if(x2==x1) return y1;
	
	if(x >= x1)
	{
		cal_y = (uint32_t)(x - x1) * (uint32_t)(y2 - y1) / (uint32_t)(x2 - x1) + y1;
		if(cal_y > UINT8_MAX)
		{
			cal_y = UINT8_MAX;
		}
		y = (uint8_t)cal_y;
		return y;
	}
	else
		return 0;
}


void math_resi_cali(uint16_t *raw_value, CALI_INFO_T *cali_info, void *processed_value, uint16_t math_cali_buffer_size)
{
	uint8_t 	*processed_value_single_pointer = NULL;
	uint16_t 	*processed_value_double_pointer = NULL;
	
	if(cali_info->cali_pressure_num < 2 || raw_value == NULL || cali_info == NULL || processed_value ==NULL || math_cali_buffer_size == 0) 
	{
		return;
	}
	
	switch(cali_info->cali_data_out_bit_size) 
	{
		case UINT8_SINGLE:
			processed_value_single_pointer = (uint8_t*)processed_value;
			for(uint16_t count = 0; count < math_cali_buffer_size; ++count)
			{
				uint16_t min_press_idx = count;
				uint16_t max_press_idx = count + (cali_info->cali_pressure_num - 1) * math_cali_buffer_size;
			
				if(raw_value[count] > cali_info->cali_params[max_press_idx])
						raw_value[count] = cali_info->cali_params[max_press_idx];
				if(raw_value[count] < cali_info->cali_params[min_press_idx])
						raw_value[count] = cali_info->cali_params[min_press_idx];
			
				uint8_t found_flag = 0;
				for(uint8_t press_idx = 0; press_idx < cali_info->cali_pressure_num-1; ++press_idx) 
				{
					uint16_t current_press_idx = count + press_idx * math_cali_buffer_size;
					uint16_t next_press_idx = count + (press_idx + 1) * math_cali_buffer_size;
				
					if(raw_value[count] >= cali_info->cali_params[current_press_idx] && raw_value[count] <= cali_info->cali_params[next_press_idx])
					{
						if(raw_value[count] == cali_info->cali_params[current_press_idx]) {
								processed_value_single_pointer[count] = cali_info->real_pressure_value[press_idx];
						}
						else if(raw_value[count] == cali_info->cali_params[next_press_idx]) {
								processed_value_single_pointer[count] = cali_info->real_pressure_value[press_idx + 1];
						}
						else {	
								processed_value_single_pointer[count] = linear_fuction_single_byte(
										raw_value[count], 
										cali_info->cali_params[current_press_idx], 
										cali_info->cali_params[next_press_idx], 
										cali_info->real_pressure_value[press_idx], 
										cali_info->real_pressure_value[press_idx + 1]);
						}
						found_flag = 1;
						break;
					} 
				}	

				if(!found_flag)
				{
					processed_value_single_pointer[count] = cali_info->real_pressure_value[0];
				}
			}
			break;	
			
		case UINT16_DOUBLE:
			processed_value_double_pointer = (uint16_t*)processed_value;
			for(uint16_t count = 0; count < math_cali_buffer_size; ++count)
			{
				uint16_t min_press_idx = count;
				uint16_t max_press_idx = count + (cali_info->cali_pressure_num - 1) * math_cali_buffer_size;
			
				if(raw_value[count] > cali_info->cali_params[max_press_idx])
						raw_value[count] = cali_info->cali_params[max_press_idx];
				if(raw_value[count] < cali_info->cali_params[min_press_idx])
						raw_value[count] = cali_info->cali_params[min_press_idx];
			
				uint8_t found_flag = 0;
				for(uint8_t press_idx = 0; press_idx < cali_info->cali_pressure_num-1; ++press_idx) 
				{
					uint16_t current_press_idx = count + press_idx * math_cali_buffer_size;
					uint16_t next_press_idx = count + (press_idx + 1) * math_cali_buffer_size;
				
					if(raw_value[count] >= cali_info->cali_params[current_press_idx] && raw_value[count] <= cali_info->cali_params[next_press_idx])
					{
						if(raw_value[count] == cali_info->cali_params[current_press_idx]) {
								processed_value_double_pointer[count] = cali_info->real_pressure_value[press_idx];
						}
						else if(raw_value[count] == cali_info->cali_params[next_press_idx]) {
								processed_value_double_pointer[count] = cali_info->real_pressure_value[press_idx + 1];
						}
						else 
						{	
							processed_value_double_pointer[count] = linear_fuction_double_byte(
									raw_value[count], 
									cali_info->cali_params[current_press_idx], 
									cali_info->cali_params[next_press_idx], 
									cali_info->real_pressure_value[press_idx], 
									cali_info->real_pressure_value[press_idx + 1]);
						}
						found_flag = 1;
						break;
					} 
				}	

				if(!found_flag)
				{
					processed_value_double_pointer[count] = cali_info->real_pressure_value[0];
				}
			}
			break;
			
		default: break;
	}
}