<commit_before>/*******************************************************************************
* discrete_siso_filter.c
* <NAME> 2016
*
* This is a collection of functions for generating and implementing discrete 
* SISO filters for arbitrary transfer functions. 
*******************************************************************************/

#include "../roboticscape.h"
#include <stdio.h>
#include <math.h>
#include <string.h> // for memset
#include <stdlib.h>

/*******************************************************************************
* d_filter_t create_filter(vector_t num, vector_t den, double dt)
*
* Allocate memory for a filter of specified order & fill with transfer
* function constants. Use enable_saturation immediately after this if you want
* to enable automatic saturation. Memory allocated by the num and den vectors
* is maintained so there is no need to destroy the vectors after passing to 
* create_fitler. 
*
* returns an empty uninitialized filter if something went wrong. Otherwise
* it returns a ready-to-use filter.
*******************************************************************************/
d_filter_t create_filter(vector_t num, vector_t den, double dt){
	d_filter_t filter;

	if(dt <= 0.0){
		printf("ERROR: dt must be >0\n");
		return filter;
	}
	if(num.initialized == 0){
		printf("ERROR: numerator vector not initialized\n");
		return filter;
	}
	if(den.initialized == 0){
		printf("ERROR: numerator vector not initialized\n");
		return filter;
	}
	if(num.len > den.len){
		printf("ERROR: num and den represent an improper transfer function\n");
		return filter;
	}
	if(den.data[0]==0.0){
		printf("ERROR: first coefficient in denominator is 0\n");
	}

	filter.order = den.len-1;
	filter.gain = 1;
	filter.newest_input = 0;
	filter.newest_output = 0;
	filter.saturation_en = 0;
	filter.saturation_min = 0;
	filter.saturation_max = 0;
	filter.saturation_flag = 0;
	filter.soft_start_en = 0;
	filter.soft_start_steps = 0;
	filter.numerator   = num;
	filter.denominator = den;
	filter.in_buf 	   = create_ring_buf(den.len);
	filter.out_buf     = create_ring_buf(den.len);
	filter.initialized = 1;
	filter.step = 0;
	return filter;
}


/*******************************************************************************
* d_filter_t create_filter_from_arrays(int order, double dt, double* num, double* den)
*
* like create_filter(), but constructs the vectors itself. Numerator and
* denominator arrays must be the same length like a semi-proper transfer
* function. Proper transfer functions with relative degree >=1 can still be
* used but the numerator must be filled with leading zeros. This function
* will throw a segmentation fault if your arrays are not both of length
* order+1. It is safer to use the standard create_filter and construct the
* vectors yourself.
*******************************************************************************/
d_filter_t create_filter_from_arrays(int order, double dt, double* num, double* den){
	d_filter_t filter;

	if(order<1){
		printf("ERROR: order must be >1\n");
		return filter;
	}

	vector_t num_vec = create_vector_from_array(num, order+1);
	vector_t den_vec = create_vector_from_array(den, order+1);

	return create_filter(num_vec, den_vec, dt);
}



/*******************************************************************************
* int destroy_filter(d_filter_t* filter)
*
* free the memory allocated by the filter's buffers and coefficient vectors.
*******************************************************************************/
int destroy_filter(d_filter_t* filter){
	if(filter->initialized == 0) return -1;
	destroy_ring_buf(&(filter->in_buf));
	destroy_ring_buf(&(filter->out_buf));
	destroy_vector(&(filter->numerator));
	destroy_vector(&(filter->denominator));
	filter->initialized = 0;
	return 0;
}

/*******************************************************************************
* d_filter_t create_empty_filter(int order)
*
* Returns a working initialized d_filter_t with enough memory allocated for
* the given order. However, all TF constants are set to zero ready to be
* set by the user.
*******************************************************************************/
d_filter_t create_empty_filter(int order){
	d_filter_t filter;
	if(order<1){
		printf("ERROR: order must be >=1\n");
		return filter;
	}
	filter.order = order;
	filter.gain = 1;
	filter.newest_input = 0;
	filter.newest_output = 0;
	filter.saturation_en = 0;
	filter.saturation_min = 0;
	filter.saturation_max = 0;
	filter.saturation_flag = 0;
	filter.soft_start_en = 0;
	filter.soft_start_steps = 0;
	filter.numerator   = create_vector(order+1);
	filter.denominator = create_vector(order+1);
	filter.in_buf 	   = create_ring_buf(order+1);
	filter.out_buf     = create_ring_buf(order+1);
	filter.initialized = 1;
	filter.step = 0;
	return filter;
	
}

/*******************************************************************************
* double march_filter(d_filter_t* filter, double new_input)
*
* March the filter forward in time one step with new input data.
* Returns new output which could also be accessed with filter.current_output
* If saturation is enabled then the output will automatically be bound by the
* min and max values given to enable_saturation. The enable_saturation entry
* in the filter struct will also be set to 1 if saturation occurred. 
*******************************************************************************/
double march_filter(d_filter_t* filter, double new_input){
	int i = 0;
	double new_output = 0;
	double input_i, output_i;
	int relative_degree;
	
	if(filter->initialized != 1){
		printf("ERROR: filter not initialized yet\n");
		return -1;
	}
	
	insert_new_ring_buf_value(&filter->in_buf, new_input);
	filter->newest_input = new_input;

	relative_degree = filter->numerator.len - filter->denominator.len;
	if(relative_degree<0){
		printf("ERROR: can't march filter: improper transfer function\n");
		return -1;
	}

	// evaluate the difference equation
	for(i=0; i<(filter->numerator.len); i++){
		input_i = get_ring_buf_value(&filter->in_buf, i+relative_degree);
		new_output += filter->numerator.data[i] * input_i;
	}
	for(i=0; i<(filter->order); i++){
		output_i = get_ring_buf_value(&filter->out_buf, i);
		new_output -= filter->denominator.data[i+1] * output_i; <commit_msg>fix relative degree<commit_after>/*******************************************************************************
* discrete_siso_filter.c
* <NAME> 2016
*
* This is a collection of functions for generating and implementing discrete 
* SISO filters for arbitrary transfer functions. 
*******************************************************************************/

#include "../roboticscape.h"
#include <stdio.h>
#include <math.h>
#include <string.h> // for memset
#include <stdlib.h>

/*******************************************************************************
* d_filter_t create_filter(vector_t num, vector_t den, double dt)
*
* Allocate memory for a filter of specified order & fill with transfer
* function constants. Use enable_saturation immediately after this if you want
* to enable automatic saturation. Memory allocated by the num and den vectors
* is maintained so there is no need to destroy the vectors after passing to 
* create_fitler. 
*
* returns an empty uninitialized filter if something went wrong. Otherwise
* it returns a ready-to-use filter.
*******************************************************************************/
d_filter_t create_filter(vector_t num, vector_t den, double dt){
	d_filter_t filter;

	if(dt <= 0.0){
		printf("ERROR: dt must be >0\n");
		return filter;
	}
	if(num.initialized == 0){
		printf("ERROR: numerator vector not initialized\n");
		return filter;
	}
	if(den.initialized == 0){
		printf("ERROR: numerator vector not initialized\n");
		return filter;
	}
	if(num.len > den.len){
		printf("ERROR: num and den represent an improper transfer function\n");
		return filter;
	}
	if(den.data[0]==0.0){
		printf("ERROR: first coefficient in denominator is 0\n");
	}

	filter.order = den.len-1;
	filter.gain = 1;
	filter.newest_input = 0;
	filter.newest_output = 0;
	filter.saturation_en = 0;
	filter.saturation_min = 0;
	filter.saturation_max = 0;
	filter.saturation_flag = 0;
	filter.soft_start_en = 0;
	filter.soft_start_steps = 0;
	filter.numerator   = num;
	filter.denominator = den;
	filter.in_buf 	   = create_ring_buf(den.len);
	filter.out_buf     = create_ring_buf(den.len);
	filter.initialized = 1;
	filter.step = 0;
	return filter;
}


/*******************************************************************************
* d_filter_t create_filter_from_arrays(int order, double dt, double* num, double* den)
*
* like create_filter(), but constructs the vectors itself. Numerator and
* denominator arrays must be the same length like a semi-proper transfer
* function. Proper transfer functions with relative degree >=1 can still be
* used but the numerator must be filled with leading zeros. This function
* will throw a segmentation fault if your arrays are not both of length
* order+1. It is safer to use the standard create_filter and construct the
* vectors yourself.
*******************************************************************************/
d_filter_t create_filter_from_arrays(int order, double dt, double* num, double* den){
	d_filter_t filter;

	if(order<1){
		printf("ERROR: order must be >1\n");
		return filter;
	}

	vector_t num_vec = create_vector_from_array(num, order+1);
	vector_t den_vec = create_vector_from_array(den, order+1);

	return create_filter(num_vec, den_vec, dt);
}



/*******************************************************************************
* int destroy_filter(d_filter_t* filter)
*
* free the memory allocated by the filter's buffers and coefficient vectors.
*******************************************************************************/
int destroy_filter(d_filter_t* filter){
	if(filter->initialized == 0) return -1;
	destroy_ring_buf(&(filter->in_buf));
	destroy_ring_buf(&(filter->out_buf));
	destroy_vector(&(filter->numerator));
	destroy_vector(&(filter->denominator));
	filter->initialized = 0;
	return 0;
}

/*******************************************************************************
* d_filter_t create_empty_filter(int order)
*
* Returns a working initialized d_filter_t with enough memory allocated for
* the given order. However, all TF constants are set to zero ready to be
* set by the user.
*******************************************************************************/
d_filter_t create_empty_filter(int order){
	d_filter_t filter;
	if(order<1){
		printf("ERROR: order must be >=1\n");
		return filter;
	}
	filter.order = order;
	filter.gain = 1;
	filter.newest_input = 0;
	filter.newest_output = 0;
	filter.saturation_en = 0;
	filter.saturation_min = 0;
	filter.saturation_max = 0;
	filter.saturation_flag = 0;
	filter.soft_start_en = 0;
	filter.soft_start_steps = 0;
	filter.numerator   = create_vector(order+1);
	filter.denominator = create_vector(order+1);
	filter.in_buf 	   = create_ring_buf(order+1);
	filter.out_buf     = create_ring_buf(order+1);
	filter.initialized = 1;
	filter.step = 0;
	return filter;
	
}

/*******************************************************************************
* double march_filter(d_filter_t* filter, double new_input)
*
* March the filter forward in time one step with new input data.
* Returns new output which could also be accessed with filter.current_output
* If saturation is enabled then the output will automatically be bound by the
* min and max values given to enable_saturation. The enable_saturation entry
* in the filter struct will also be set to 1 if saturation occurred. 
*******************************************************************************/
double march_filter(d_filter_t* filter, double new_input){
	int i = 0;
	double new_output = 0;
	double input_i, output_i;
	int relative_degree;
	
	if(filter->initialized != 1){
		printf("ERROR: filter not initialized yet\n");
		return -1;
	}
	
	insert_new_ring_buf_value(&filter->in_buf, new_input);
	filter->newest_input = new_input;

	relative_degree = filter->denominator.len - filter->numerator.len;
	if(relative_degree<0){
		printf("ERROR: can't march filter: improper transfer function\n");
		return -1;
	}

	// evaluate the difference equation
	for(i=0; i<(filter->numerator.len); i++){
		input_i = get_ring_buf_value(&filter->in_buf, i+relative_degree);
		new_output += filter->numerator.data[i] * input_i;
	}
	for(i=0; i<(filter->order); i++){
		output_i = get_ring_buf_value(&filter->out_buf, i);
		new_output -= filter->denominator.data[i+1] * output_i; 
{'fraction_non_alphanumeric': 0.24190989847715735, 'fraction_numerical': 0.00983502538071066, 'mean_word_length': 5.646810753821824, 'pattern_counts': {'":': 0, '<': 25, '<?xml version=': 0, '>': 57, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '4424549', 'n_tokens_mistral': 3858, 'n_tokens_neox': 3607, 'n_words': 1078}