kfkernelsketch

#include "BRAM-uio-driver/src/bram_uio.h"
#include "KalmanFilterKernel-driver/src/xkalmanfilterkernel.h"

#define INPUT_FILENAME "input.txt"
#define OUTPUT_FILENAME "output.txt"
#define DELIMITER ','

#define 	SAMPLE_FREQ 		10	// Hz
#define 	DT 			0.1
#define		BRAM_SIZE		2048
#define		N_SAMPLES		300
#define		N_STATE_VARS		6
#define		N_CTRL_VARS		3
#define		N_MEAS_VARS		3

#include <stdint.h>
#include <iostream>

#include <stdlib.h>
#include <fstream>
#include <sstream>
#include <string>
#include <vector>


/*
vector<vector<float>> parseFile(string filename) {
	vector <vector <float> > data;

	ifstream infile( INPUT_FILENAME );

	bool first_line = true;

	while(infile) {

		string s;
		if (!getline(infile, s)) break;

		if (first_line) {
			first_line = false;
			continue;
		}

		istringstream ss(s);
		vector<float> record;

		while(ss) {
			string s;
			if(!getline(ss,s,DELIMITER)) break;
			record.push_back(stof(s));
		}

		data.push_back(record);

	}

	if (!infile.eof()) {
		cerr << "Fooey!\n";
	}

	return data;
}
*/

void writeDataToFile(float dout[N_SAMPLES*N_STATE_VARS], string filename) {

	ofstream f;
	f.open(filename);

	f << "x_hat_0,x_hat_1,x_hat_2,x_hat_3,x_hat_4,x_hat_5\n";

	for (int i = 0; i < N_SAMPLES; i++) {
		for (int j = 0; j < N_STATE_VARS; j++) {

			f << dout[i*N_STATE_VARS+j];

			if (j < N_STATE_VARS-1) {

				f << ",";

			}
		}

		if (i < N_SAMPLES-1) {

			f << "\n";

		}
	}
}

int main(int argc, char *argv){
	
	// API of bram from LEC4 bram uio driver. Also i suppose the data is automatically written to and from the BRAMs as this api is taken from the IP file of Kalman kernel
	// initialization
	//BRAM bram0
	//BRAM bram1

	XKalmanfilterkernel kf_kernel;

	XKalmanfilterkernel_Initialize(&kf_kernel, "KalmanFilterKernel");

	#include "data.h"  //din is declared in here so later in the code we are supposed to read from BRAM (unclear part)

	
	vector<vector<float>> data_vec = parseFile(INPUT_FILENAME);
	
	//float din[N_SAMPLES*(N_MEAS_VARS+N_CTRL_VARS)];
	float dout[N_SAMPLES*N_STATE_VARS];

	
	// setting parameters
	XKalmanfilterkernel_Set_q(&kf_kernel, u32 Data); // maybe 0.05 in place of u32 Data
	XKalmanfilterkernel_Set_r(&kf_kernel, u32 Data); // maybe 0.95 ...
	
	
	
	if(XKalmanfilterkernel_IsIdle(&kf_kernel)){ //returns: return (Data >> 1) & 0x1; so not sure here
	 	
	 	XKalmanfilterkernel_Start(&kf_kernel);
	 } 
		
	while(XKalmanfilterkernel_IsDone(&kf_kernel)){ //again not sure as to how to wait until its finished
		
		sleep(10); //some time probably?
	}
	
	if(XKalmanfilterkernel_IsIdle(&kf_kernel)){ //returns: return (Data >> 1) & 0x1; so not sure here
	 	
	 	//Method to read the data from from BRAM1, maybe some function like ParseData, but im not sure 
	 	
		writeDataToFile(dout, OUTPUT_FILENAME); // BRAM outputs to file
	 } 


	XKalmanfilterkernel_Release(&kf_kernel);

	return 0;
}