MotorControlModuleSDFM_TMS320F28388D/Projects/EFC_Application/UMLibrary/technological/commissioning/ThetaNullEstimate.cpp

/*
 * ThetaNullEstimate.cpp
 *
 *  Created on: 23 íîÿá. 2016 ã.
 *      Author: titov
 */

#include "ThetaNullEstimate.hh"

//#include "../system/DebugConsole.hh"

using technological::commissioning::ThetaNullEstimate;
using algorithm::LinearRegressionCirc;

ThetaNullEstimate::ThetaNullEstimate() : null_calculator(nullptr), buff_resource(nullptr) {

	enable = false;
	sector_setpoint = 0;
	Np = 0;
	speed = 0;
	Ts = 0;
	count = 0;

	data_collection = false;
	data_analysis = false;
	direction = forward;
	count_rise = 0;
	count_next = 0;
	sector = 0;
	errorEstimate = 0;

	previous_feedback = NAN;
	delta_previous = 0;
}

bool ThetaNullEstimate::configure(const TSetting & setting) {
	bool result = false;

	if(setting.isValid()) {
		Ts = setting.Ts;
		Np = static_cast<unsigned int>(setting.Np);

        //Äîïóñòèìîå îòêëîíåíèå.
        errorEstimate = setting.valid_mismatch_theta_1;
        totalError = setting.valid_error;

		result = true;
	}

	return result;
}

bool ThetaNullEstimate::start(float average_speed, unsigned int number_sector, float rise_to_hold) {
	//Ðåçóëüòàò èíèöèàëèçàöèè.
	bool result = false;

	//Ïðîâåðêà äîïóñòèìîñòè èñõîäíûõ äàííûõ.
	if(		!enable and buff_resource
			&& rise_to_hold > 0.0f
			&& rise_to_hold < 1.0f
			&& number_sector > 5
			&& average_speed > 0.0f
			) {
		//Ðàñ÷åò äåëüòà
		delta_previous = math::constants::pi_3 / (Np * 2);

		//Èñõîäíûå äàííûå.
		float k = 1 / rise_to_hold;
		speed = average_speed * k;
		sector_setpoint = number_sector;
		//Ðàñ÷åò êîëè÷åñòâà îòñ÷åòîâ äëÿ ñåêòîðà.
		count = 0;
		sector = 0;
		count_rise = math::constants::pi_3 / (Ts * speed * Np);
		count_next = count_rise * k;
		direction = forward;
		//Êîëè÷åñòâî äàííûõ.
		unsigned int row = number_sector * 2;		//Êîëè÷åñòâî ñòðîê ðàâíî êîëè÷åñòâó èçìåðÿåìûõ òî÷åê.
		unsigned int col = 2;						//Êîëè÷åñòâî ñòîëáöîâ ðàâíî êîëè÷åñòâó ïðèçíàêîâ.
		//
		//
		null_calculator = nullptr;
		buff_resource->release();
		std::pmr::polymorphic_allocator<LinearRegressionCirc> alloca = std::pmr::polymorphic_allocator<LinearRegressionCirc>(buff_resource);

        LinearRegressionCirc * temp = alloca.allocate(1);
		alloca.construct<LinearRegressionCirc>( temp, row, col, buff_resource );
        null_calculator = temp;

		//Ïàðàìåòðû îáó÷åíèÿ.
		float tempTheta[2] = {0.0f, math::constants::pi_3};
		float alfa = 0.1f;
		unsigned int num_iteration = 2000;
		//Åñëè ñîçäàëîñü, èíèöèàëèçèðóåì.
		if( null_calculator && null_calculator->init(alfa, tempTheta, num_iteration) ) {
			result = true;
			data_collection = true;
			enable = true;
		}
	}

	return result;
}

void ThetaNullEstimate::stop() {
	data_collection = false;
	data_analysis = false;
	if(null_calculator) (void) null_calculator->stopCalc();
	enable = false;
}

void ThetaNullEstimate::add_data(float theta_feedback) {
//	//Ïðîâåðêà.
//	if(std::fabs(previous_feedback - theta_feedback) < delta_previous) {
//		//Generate Error!
//		stop();
//	}
	previous_feedback = theta_feedback;
	//Äàííûå.
	float data[2];
	float y;
	//Íîìåð ñåêòîðà è ïîñëåäíÿÿ ïîçèöèÿ.
	data[0] = 1.0f;
	data[1] = sector % 6;
	y = std::fmod(theta_feedback * Np, math::constants::pi2);
	//Äîáàâëåíèå äàííûõ
	null_calculator->addData(data, y);

}

void ThetaNullEstimate::execute(float theta_feedback, float & iota) {
	float angle;

	if(data_collection) {

		if(count < count_rise) {
			angle = speed * Np * count * Ts;
		} else {
			angle = speed * Np * count_rise * Ts;
		}

		if(direction == forward) {
			if(count == count_next && sector != sector_setpoint) {
				//Äîáàâëåíèå äàííûõ.
				add_data(theta_feedback);
			}
			if(count >= count_next) {
				//Îáíîâëåíèå ñåêòîðà.
				sector++;
				count = 0;
				angle = 0.0f;
			}
			count++;
		}

		if(direction == backward) {
			if(count == count_rise && sector != sector_setpoint) {
				//Äîáàâëåíèå äàííûõ.
				add_data(theta_feedback);
			}
			if(count == 0) {
				//Îáíîâëåíèå ñåêòîðà.
				sector--;
				count = count_next;
				angle = math::constants::pi_3;
			}
			count--;
		}

		angle = angle + (sector % 6) * math::constants::pi_3;

		if(direction == forward && sector > sector_setpoint) {

			direction = backward;
			count = count_next;
			sector = sector_setpoint;
		}

		if(direction == backward && sector < 0) {

			data_collection = false;
			(void) null_calculator->startCalc();
			sector = 0;
			count = 0;
		}

		iota = angle;

	} else { //if(data_collection)

		iota = 0;
	}
}

void ThetaNullEstimate::process()	{

	data_analysis = null_calculator && null_calculator->calculation();

}

bool technological::commissioning::ThetaNullEstimate::setBuffer(
        std::pmr::monotonic_buffer_resource * buffer ) {

    if( enable )
        return false;

    stop();

    if( buff_resource ) {
        buff_resource->release();
        null_calculator = nullptr;
    }

    return buff_resource = buffer;
}

void technological::commissioning::ThetaNullEstimate::resetBuffer() {

    stop();

    if( buff_resource ) {
        buff_resource->release();
        null_calculator = nullptr;
    }

    buff_resource = nullptr;

}