motorcontroldemo_028/Vsrc/V_MotorModel.c

/*!
     Copyright 2017 	ÀÎ "ÍÈÈÝÒ" è ÎÎÎ "ÍÏÔ ÂÅÊÒÎÐ"

   Licensed under the Apache License, Version 2.0 (the "License");
   you may not use this file except in compliance with the License.
   You may obtain a copy of the License at

       http://www.apache.org/licenses/LICENSE-2.0

   Unless required by applicable law or agreed to in writing, software
   distributed under the License is distributed on an "AS IS" BASIS,
   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   See the License for the specific language governing permissions and
   limitations under the License.

 \file      V_MotorModel.ñ
 \brief     Ìîäóëü ìîäåëåé ýëåêòðîäâèãàòåëåé (ñì. TMotorModel)
 \author    ÎÎÎ "ÍÏÔ Âåêòîð". http://motorcontrol.ru
 \version   v 2.0 25/03/2017

 \addtogroup V_MotorModel
 @{ */


#include "V_MotorModel.h"
#include "math.h"
#include "V_MotorParams.h"
#include <V_IQmath.h>
#include "main.h"

#define ADC_MAX_VALUE 4095
#define ADC_HALF_VALUE 2047

//ôóíêöèÿ ñëó÷àéíîãî "øóìà" äëÿ ÀÖÏ
int MotorModel_fastrand() {
	static unsigned long g_seed;
	g_seed = (214013 * g_seed + 2531011);
	return ((g_seed >> 16) & 0x3) -2;
}

//èíèöèàëèçàöèÿ ïåðåìåííûõ ìîäåëè
void MotorModel_Init(TMotorModel *p) {
	Uint16 MotorParametersValid=0;
	int i=0;

	p->hallSensor = 1;		//èíèöèàëèçàöèÿ äàò÷èêà Õîëëà ïðè íóëåâîì óãëå
	p->motorInternals.dt = p->dt;			//äëèòåëüíîñòü ìåðòâîãî âðåìåíè
	p->motorInternals.tpr = p->tpr;		//ïåðèîä ØÈÌ
	p->motorInternals.cmpr0 = p->motorInternals.tpr-p->cmpr0;	//óñòàâêè ñðàâíåíèÿ ØÈÌ. Ñ èíâåðñèåé, ïîòîìó ÷òî òàê íàñòðîåí ìîäóëü ØÈÌ
	p->motorInternals.cmpr1 = p->motorInternals.tpr-p->cmpr1;
	p->motorInternals.cmpr2 = p->motorInternals.tpr-p->cmpr2;
	p->motorInternals.cmpr3 = p->motorInternals.tpr-p->cmpr3;
	//øàã äèñêðåòèçàöèè ìîäåëè (ñ÷èòàåòñÿ â ïðåðûâàíèè 10êÃö)
	p->motorInternals.t = 1.0 / 10000;

	p->motorInternals.cos30 = cos(30.0/360*MOTOR_MODEL_PI*2);
	p->fault = 0;				//êîä îøèáêè ìîäåëè
	p->motorInternals.udSenseK = ADC_HALF_VALUE/540.0;
	p->motorInternals.type=p->MotorType&0x7;//ïåðåäàåì òèï äâèãàòåëÿ âíèç â ñòðóêòóðó ïàðàìåòðîâ äâèãàòåëÿ

	// Ïîèñê ìàêcèìàëüíîãî íîìåðà íàáîðà ïàðàìåòðîâ äâèãàòåëÿ èç ìàññèâà
	// Íóæíî äëÿ çàùèòû îò ââåäåíèÿ íîìåðà íåñóùåñòâóþùåãî ýëåìåíòà ìàññèâà
	// ÍÀ×ÀËÎ //
	if (p->motorInternals.type==MODEL_INDUCTION_MOTOR){
			// Åñëè r_s, l_s, j îäíîâðåìåííî ðàâíû íóëþ, çíà÷èò íàòêíóëèñü íà íóëåâîé íàáîð ïàðàìåòðîâ.
			// Çíà÷èò ïðåäûäóùèé íàáîð - ïîñëåäíèé ðàáî÷èé â ìàññèâå, è íîìåð áîëüøå, ÷åì ó íåãî âûáèðàòü íåëüçÿ.
			while ((IMDATA[i].r_s != 0) && (IMDATA[i].l_s != 0) && (IMDATA[i].j != 0) && (i < 50)) i++;}
	else if (p->motorInternals.type==MODEL_SYNC_MOTOR){
			// Åñëè l_sd, l_sq, j îäíîâðåìåííî ðàâíû íóëþ, çíà÷èò íàòêíóëèñü íà íóëåâîé íàáîð ïàðàìåòðîâ.
			// Çíà÷èò ïðåäûäóùèé íàáîð - ïîñëåäíèé ðàáî÷èé â ìàññèâå, è íîìåð áîëüøå, ÷åì ó íåãî âûáèðàòü íåëüçÿ.
			while ((SMDATA[i].l_sd != 0) && (SMDATA[i].l_sq != 0) && (SMDATA[i].j != 0) && (i < 50)) i++;}
	else if (p->motorInternals.type==MODEL_DC_MOTOR){
			// Åñëè r_a, l_a, j îäíîâðåìåííî ðàâíû íóëþ, çíà÷èò íàòêíóëèñü íà íóëåâîé íàáîð ïàðàìåòðîâ.
			// Çíà÷èò ïðåäûäóùèé íàáîð - ïîñëåäíèé ðàáî÷èé â ìàññèâå, è íîìåð áîëüøå, ÷åì ó íåãî âûáèðàòü íåëüçÿ.
			while ((DCMDATA[i].r_a != 0) && (DCMDATA[i].l_a != 0) && (DCMDATA[i].j != 0) && (i < 50)) i++;}
	else if (p->motorInternals.type==MODEL_SRD_MOTOR){
			// Åñëè r_s, pp, j îäíîâðåìåííî ðàâíû íóëþ, çíà÷èò íàòêíóëèñü íà íóëåâîé íàáîð ïàðàìåòðîâ.
			// Çíà÷èò ïðåäûäóùèé íàáîð - ïîñëåäíèé ðàáî÷èé â ìàññèâå, è íîìåð áîëüøå, ÷åì ó íåãî âûáèðàòü íåëüçÿ.
			while ((SRMDATA[i].r_s != 0) && (SRMDATA[i].pp != 0) && (SRMDATA[i].j != 0) && (i < 50)) i++;}
	p->MotorParametersNumMax = i;
	// ÊÎÍÅÖ //

	// ïðîâåðêà âàëèäíîñòè ââåäåííîãî íîìåðà íàáîðà ïàðàìåòðîâ
	// ÍÀ×ÀËÎ //
	if ((p->MotorParametersNum > 0) && (p->MotorParametersNum <= p->MotorParametersNumMax)) MotorParametersValid = 1;
	else
	{
		if (p->MotorParametersNum > p->MotorParametersNumMax){
			p->MotorParametersNum = p->MotorParametersNumMax;
			MotorParametersValid = 1;}
		else if (p->MotorParametersNum == 0) MotorParametersValid = 0;
	}
	// ÊÎÍÅÖ //

	// Èíèöèàëèçàöèÿ ïàðàìåòðîâ äâèãàòåëåé
	if (p->motorInternals.type==MODEL_INDUCTION_MOTOR){//òèï äâèãàòåëÿ - àñèíõðîííûé
		if (MotorParametersValid){
			p->motorInternals.ls = IMDATA[p->MotorParametersNum-1].l_s;		//èíäóêòèâíîñòü ñòàòîðà
			p->motorInternals.lr = IMDATA[p->MotorParametersNum-1].l_r;		//èíäóêòèâíîñòü ðîòîðà
			p->motorInternals.lm = IMDATA[p->MotorParametersNum-1].l_m;		//âçàèìíàÿ èíäóêòèâíîñòü
			//Âíèìàíèå: â ïàðàìåòðàõ ls è lr äîëæíà áûòü ïîëíàÿ èíäóêòèâíîñòü ñòàòîðà è ðîòîðà. Èíîãäà â ïàðàìåòðàõ äâèãàòåëÿ ïðèâîäÿòñÿ èíäóêòèâíîñòè ðàññåÿíèÿ
			//ñòàòîðà è ðîòîðà, êîòîðûå ìåíüøå, ÷åì l_m. ×òîáû ïîëó÷èòü ïîëíóþ èíäóêòèâíîñòü ñòàòîðà èëè ðîòîðà, íóæíî ïðèáàâèòü l_m ê l_s èëè l_r ñîîòâåòñòâåííî.
			//Ïðîâåðèì ïðàâèëüíîñòü ââîäà èíäóêòèâíîñòåé:
			if ((p->motorInternals.ls < p->motorInternals.lm) || (p->motorInternals.lr < p->motorInternals.lm))
				p->fault = MODEL_WRONG_PARAMETERS;
			p->motorInternals.rs = IMDATA[p->MotorParametersNum-1].r_s;				//ñîïðîòèâëåíèå ñòàòîðà
			p->motorInternals.rr = IMDATA[p->MotorParametersNum-1].r_r;				//ñîïðîòèâëåíèå ðîòîðà
			p->motorInternals.pp = IMDATA[p->MotorParametersNum-1].pp;				//÷èñëî ïàð ïîëþñîâ
			p->motorInternals.j = IMDATA[p->MotorParametersNum-1].j;				//ìîìåíò èíåðöèè
			p->motorInternals.QEPResolution = IMDATA[p->MotorParametersNum-1].qep;	//ðàçðåøåíèå ýíêîäåðà
			p->motorInternals.RatedPower = IMDATA[p->MotorParametersNum-1].RatedPower; //íîìèíàëüíàÿ ìîùíîñòü (ñïðàâî÷íàÿ âåëè÷èíà, íå èñïîëüçóåòñÿ â ðàñ÷åòàõ)
			p->motorInternals.RatedSpeed = IMDATA[p->MotorParametersNum-1].RatedSpeed; //íîìèíàëüíàÿ ñêîðîñòü (ñïðàâî÷íàÿ âåëè÷èíà, íå èñïîëüçóåòñÿ â ðàñ÷åòàõ)
			p->motorInternals.RatedCurrent = IMDATA[p->MotorParametersNum-1].RatedCurrent; //íîìèíàëüíûé äåéñòâóþùèé òîê
			p->motorInternals.MechLoss = p->motorInternals.j * 10; 		//ìåõàíè÷åñêèå ïîòåðè
		}

		// Èíèöèàëèçàöèÿ êîýôôèöèåíòîâ è ðàñ÷åòíûõ âåëè÷èí, çàâÿçàííûõ íà ïàðàìåòðû äâèãàòåëÿ

		p->motorInternals.speedK = 60/(4*MOTOR_MODEL_PI); //êîýôôèöèåíò äàò÷èêà ñêîðîñòè

		p->motorInternals.iADCk = p->motorInternals.RatedCurrent*1.414*3; 			// Ðàñ÷åòíûé êîýôôèöèåíò ÀÖÏ ðàâåí òðîåêðàòíîé àìïëèòóäå íîìèíàëüíîãî òîêà
		p->motorInternals.iSenseK = (ADC_HALF_VALUE*1.0)/p->motorInternals.iADCk;	//êîýôôèöèåíò äàò÷èêà òîêà äëÿ ïðåîáðàçîâàíèÿ òîêà â àìïåðàõ â êîä ÀÖÏ

		p->motorInternals.ks = -(p->motorInternals.lr/1000.0) / (p->motorInternals.lm/1000.0 * p->motorInternals.lm/1000.0 - p->motorInternals.ls/1000.0 * p->motorInternals.lr/1000.0);	//êîýôôèöèåíòû îáðàòíîé ìàòðèöû èíäóêòèâíîñòåé
		p->motorInternals.kr = -(p->motorInternals.ls/1000.0) / (p->motorInternals.lm/1000.0 * p->motorInternals.lm/1000.0 - p->motorInternals.ls/1000.0 * p->motorInternals.lr/1000.0);
		p->motorInternals.km = (p->motorInternals.lm/1000.0) / (p->motorInternals.lm/1000.0 * p->motorInternals.lm/1000.0 - p->motorInternals.ls/1000.0 * p->motorInternals.lr/1000.0);

		p->motorInternals.psa = 0;				//ïîòîêîñöåïëåíèÿ ñòàòîðà â îñÿõ àëüôà,áåòà
		p->motorInternals.psb = 0;
	}

	if (p->motorInternals.type==MODEL_SYNC_MOTOR){//òèï äâèãàòåëÿ - ñèíõðîííûé
		if (MotorParametersValid){
			p->motorInternals.r_f = SMDATA[p->MotorParametersNum-1].r_f;			//ñîïðîòèâëåíèå ÎÂ
			p->motorInternals.l_f = SMDATA[p->MotorParametersNum-1].l_f;			//èíäóêòèâíîñòü ÎÂ
			p->motorInternals.l_m = SMDATA[p->MotorParametersNum-1].l_m;			//êîýôôèöèåíò ìàãíèòíîãî ïîòîêà (âçàèìíàÿ èíäóêòèâíîñòü)
			p->motorInternals.rs = SMDATA[p->MotorParametersNum-1].r_s;				//ñîïðîòèâëåíèå ñòàòîðà
			p->motorInternals.lsd = SMDATA[p->MotorParametersNum-1].l_sd;			//èíäóêòèâíîñòü ñòàòîðà
			p->motorInternals.lsq = SMDATA[p->MotorParametersNum-1].l_sq;			//èíäóêòèâíîñòü ñòàòîðà
			p->motorInternals.pp = SMDATA[p->MotorParametersNum-1].pp;				//÷èñëî ïàð ïîëþñîâ
			p->motorInternals.j = SMDATA[p->MotorParametersNum-1].j;				//ìîìåíò èíåðöèè
			p->motorInternals.QEPResolution = SMDATA[p->MotorParametersNum-1].qep;	//ðàçðåøåíèå ýíêîäåðà
			p->motorInternals.m = SMDATA[p->MotorParametersNum-1].psi_pm;			//ïîòîêîñöåïëåíèå ðîòîðà ðàâíî ïîòîêó ïîñòîÿííûõ ìàãíèòîâ
			p->motorInternals.RatedPower = SMDATA[p->MotorParametersNum-1].RatedPower; //íîìèíàëüíàÿ ìîùíîñòü (ñïðàâî÷íàÿ âåëè÷èíà, íå èñïîëüçóåòñÿ â ðàñ÷åòàõ)
			p->motorInternals.RatedSpeed = SMDATA[p->MotorParametersNum-1].RatedSpeed; //íîìèíàëüíàÿ ñêîðîñòü (ñïðàâî÷íàÿ âåëè÷èíà, íå èñïîëüçóåòñÿ â ðàñ÷åòàõ)
			p->motorInternals.RatedCurrent = SMDATA[p->MotorParametersNum-1].RatedCurrent; //íîìèíàëüíûé äåéñòâóþùèé òîê
			p->motorInternals.RatedFluxCurrent = SMDATA[p->MotorParametersNum-1].RatedFluxCurrent; //íîìèíàëüíûé òîê ÎÂ
			p->motorInternals.MechLoss = p->motorInternals.j * 10; 		//ìåõàíè÷åñêèå ïîòåðè
		}
		// Èíèöèàëèçàöèÿ êîýôôèöèåíòîâ è ðàñ÷åòíûõ âåëè÷èí, çàâÿçàííûõ íà ïàðàìåòðû äâèãàòåëÿ

		p->motorInternals.speedK = 60/(4*MOTOR_MODEL_PI); //êîýôôèöèåíò äàò÷èêà ñêîðîñòè

		p->motorInternals.iADCk = p->motorInternals.RatedCurrent*1.414*3; 			// Ðàñ÷åòíûé êîýôôèöèåíò ÀÖÏ ðàâåí òðîåêðàòíîé àìïëèòóäå íîìèíàëüíîãî òîêà
		p->motorInternals.iSenseK = (ADC_HALF_VALUE*1.0)/p->motorInternals.iADCk;	//êîýôôèöèåíò äàò÷èêà òîêà äëÿ ïðåîáðàçîâàíèÿ òîêà â àìïåðàõ â êîä ÀÖÏ

		p->motorInternals.ifADCk = p->motorInternals.RatedFluxCurrent*3; 			// Ðàñ÷åòíûé êîýôôèöèåíò ÀÖÏ ðàâåí òðîåêðàòíîìó íîìèíàëüíîìó òîêó ÎÂ
		p->motorInternals.ifSenseK = (ADC_HALF_VALUE*1.0)/p->motorInternals.ifADCk;	//êîýôôèöèåíò äàò÷èêà òîêà äëÿ ïðåîáðàçîâàíèÿ òîêà â àìïåðàõ â êîä ÀÖÏ

		p->motorInternals._1_lsd = 1.0/(p->motorInternals.lsd/1000);			//îáðàòíàÿ âåëè÷èíà
		p->motorInternals._1_lsq = 1.0/(p->motorInternals.lsq/1000);			//îáðàòíàÿ âåëè÷èíà

		if (p->motorInternals.l_f != 0)	//Ñ îáìîòêîé âîçáóæäåíèÿ
		{
			p->motorInternals._1_l_f = 1.0/(p->motorInternals.l_f/1000); // Îáðàòíàÿ âåëè÷èíà (ïåðåâåäåííàÿ èç ìÃí â Ãí)
			p->motorInternals.syncm_pm = 0;
			p->motorInternals.m = 0;			//ïîòîêîñöåïëåíèå ðîòîðà ðàâíî íóëþ
		}
		else							//Ñ ïîñòîÿííûìè ìàãíèòàìè
		{
			p->motorInternals.syncm_pm = 1;
			p->motorInternals.ifADCk = 0; // Ðàñ÷åòíûé êîýôôèöèåíò ÀÖÏ òîêà ÎÂ
		}

		p->motorInternals.psd = p->motorInternals.m;//ïîòîêîñöåïëåíèÿ ñòàòîðà â îñÿõ d,q
		p->motorInternals.psq = 0;
		p->motorInternals.i_f = 0;
	}

	if (p->motorInternals.type==MODEL_DC_MOTOR){//òèï äâèãàòåëÿ - êîëëåêòîðíûé äâèãàòåëü ïîñòîÿííîãî òîêà
		if (MotorParametersValid){
			p->motorInternals.pp = 2;				//÷èñëî ïàð ïîëþñîâ (ïàðàìåòð íå íóæåí, ïðîñòî ÷òîáû íå ìóñîð)
			p->motorInternals.r_f = DCMDATA[p->MotorParametersNum-1].r_f;			//ñîïðîòèâëåíèå ÎÂ
			p->motorInternals.l_f = DCMDATA[p->MotorParametersNum-1].l_f;			//èíäóêòèâíîñòü ÎÂ
			p->motorInternals.l_m = DCMDATA[p->MotorParametersNum-1].l_m;			//êîýôôèöèåíò ìàãíèòíîãî ïîòîêà (âçàèìíàÿ èíäóêòèâíîñòü)
			p->motorInternals.r_a = DCMDATA[p->MotorParametersNum-1].r_a;			//ñîïðîòèâëåíèå ÿêîðÿ
			p->motorInternals.l_a = DCMDATA[p->MotorParametersNum-1].l_a;			//èíäóêòèâíîñòü ÿêîðÿ
			p->motorInternals.RatedFluxCurrent = DCMDATA[p->MotorParametersNum-1].RatedFluxCurrent;			//Íîìèíàëüíûé òîê ÎÂ
			p->motorInternals.RatedCurrent = DCMDATA[p->MotorParametersNum-1].RatedCurrent;			//Íîìèíàëüíûé òîê ÿêîðÿ
			p->motorInternals.j = DCMDATA[p->MotorParametersNum-1].j;				//ìîìåíò èíåðöèè
			p->motorInternals.QEPResolution = DCMDATA[p->MotorParametersNum-1].qep;	//ðàçðåøåíèå ýíêîäåðà
			p->motorInternals.RatedPower = DCMDATA[p->MotorParametersNum-1].RatedPower; //íîìèíàëüíàÿ ìîùíîñòü (ñïðàâî÷íàÿ âåëè÷èíà, íå èñïîëüçóåòñÿ â ðàñ÷åòàõ)
			p->motorInternals.RatedSpeed = DCMDATA[p->MotorParametersNum-1].RatedSpeed; //íîìèíàëüíàÿ ñêîðîñòü (ñïðàâî÷íàÿ âåëè÷èíà, íå èñïîëüçóåòñÿ â ðàñ÷åòàõ)
			p->motorInternals.MechLoss = p->motorInternals.j * 10; 					//ìåõàíè÷åñêèå ïîòåðè
		}
		// Èíèöèàëèçàöèÿ êîýôôèöèåíòîâ è ðàñ÷åòíûõ âåëè÷èí, çàâÿçàííûõ íà ïàðàìåòðû äâèãàòåëÿ

		p->motorInternals.speedK = 60/(4*MOTOR_MODEL_PI); //êîýôôèöèåíò äàò÷èêà ñêîðîñòè
		p->motorInternals.iADCk = (ADC_HALF_VALUE*1.0)/p->motorInternals.iSenseK; // Ðàñ÷åòíûé êîýôôèöèåíò ÀÖÏ òîêà Îÿ

		p->motorInternals.iADCk = p->motorInternals.RatedCurrent*1.414*3; 			// Ðàñ÷åòíûé êîýôôèöèåíò ÀÖÏ ðàâåí òðîåêðàòíîé àìïëèòóäå íîìèíàëüíîãî òîêà
		p->motorInternals.iSenseK = (ADC_HALF_VALUE*1.0)/p->motorInternals.iADCk;	//êîýôôèöèåíò äàò÷èêà òîêà äëÿ ïðåîáðàçîâàíèÿ òîêà â àìïåðàõ â êîä ÀÖÏ

		p->motorInternals.ifADCk = p->motorInternals.RatedFluxCurrent*3; 			// Ðàñ÷åòíûé êîýôôèöèåíò ÀÖÏ ðàâåí òðîåêðàòíîìó íîìèíàëüíîìó òîêó ÎÂ
		p->motorInternals.ifSenseK = (ADC_MAX_VALUE*1.0)/p->motorInternals.ifADCk;	//êîýôôèöèåíò äàò÷èêà òîêà äëÿ ïðåîáðàçîâàíèÿ òîêà â àìïåðàõ â êîä ÀÖÏ

		p->motorInternals.r_ad = 0;

		p->motorInternals._1_l_a = 1.0/(p->motorInternals.l_a/1000); // Îáðàòíàÿ âåëè÷èíà (ïåðåâåäåííàÿ èç ìÃí â Ãí)
		if (p->motorInternals.l_f != 0)	// åñëè ÄÏÒ ñ îáìîòêîé âîçáóæäåíèÿ
		{
			// Ðàñ÷èòûâàåì íåîáõîäèìûå êîýôôèöèåíòû äëÿ ðàáîòû ÎÂ
			p->motorInternals._1_l_f = 1.0/(p->motorInternals.l_f/1000); // Îáðàòíàÿ âåëè÷èíà (ïåðåâåäåííàÿ èç ìÃí â Ãí)
			p->motorInternals.ifADCk = (ADC_HALF_VALUE*1.0)/p->motorInternals.ifSenseK; // Ðàñ÷åòíûé êîýôôèöèåíò ÀÖÏ òîêà ÎÂ
			p->motorInternals.dcm_pm = 0;	// Ñáðàñûâàåì ôëàã âîçáóæäåíèÿ îò ÏÌ
		}
		else	// ÄÏÒ ñ ïîñòîÿííûìè ìàãíèòàìè
		{
			//  ýòîì ñëó÷àå îáìîòêó âîçáóæäåíèÿ íå ñ÷èòàåì
			p->motorInternals.dcm_pm = 1; // Óñòàíàâëèâàåì ôëàã âîçáóæäåíèÿ îò ÏÌ
			p->motorInternals.ifADCk = 0; // Ðàñ÷åòíûé êîýôôèöèåíò ÀÖÏ òîêà Πóñòàíàâëèâàåì íóëþ
		}
		p->motorInternals.i_a = 0;					//òîê ÎÂ
		p->motorInternals.i_f = 0;					//ïîòîê ÿêîðÿ
	}

	if (p->motorInternals.type==MODEL_SRD_MOTOR){//òèï äâèãàòåëÿ - ÂÈÄ (Switched Reluctance Drive, SRD)
		if (MotorParametersValid){
			p->motorInternals.rs = SRMDATA[p->MotorParametersNum-1].r_s;				//ñîïðîòèâëåíèå ôàçíîé îáìîòêè
			p->motorInternals.pp = SRMDATA[p->MotorParametersNum-1].pp;				//÷èñëî ïàð ïîëþñîâ
			p->motorInternals.j = SRMDATA[p->MotorParametersNum-1].j;					//ìîìåíò èíåðöèè
			p->motorInternals.QEPResolution = SRMDATA[p->MotorParametersNum-1].qep;	//ðàçðåøåíèå ýíêîäåðà
			p->motorInternals.speedK = SRMDATA[p->MotorParametersNum-1].sk;			//êîýôôèöèåíò äàò÷èêà ñêîðîñòè
			p->motorInternals.RatedPower = SRMDATA[p->MotorParametersNum-1].RatedPower; //íîìèíàëüíàÿ ìîùíîñòü (ñïðàâî÷íàÿ âåëè÷èíà, íå èñïîëüçóåòñÿ â ðàñ÷åòàõ)
			p->motorInternals.RatedSpeed = SRMDATA[p->MotorParametersNum-1].RatedSpeed; //íîìèíàëüíàÿ ñêîðîñòü (ñïðàâî÷íàÿ âåëè÷èíà, íå èñïîëüçóåòñÿ â ðàñ÷åòàõ)
			p->motorInternals.RatedCurrent = SRMDATA[p->MotorParametersNum-1].RatedCurrent; //íîìèíàëüíûé äåéñòâóþùèé òîê (ñïðàâî÷íàÿ âåëè÷èíà, íå èñïîëüçóåòñÿ â ðàñ÷åòàõ)
			p->motorInternals.MechLoss = p->motorInternals.j * 10; 				//ìåõàíè÷åñêèå ïîòåðè
		}
		// Èíèöèàëèçàöèÿ êîýôôèöèåíòîâ è ðàñ÷åòíûõ âåëè÷èí, çàâÿçàííûõ íà ïàðàìåòðû äâèãàòåëÿ

		p->motorInternals.inv_pp=1.0/p->motorInternals.pp;

		p->motorInternals.iADCk = p->motorInternals.RatedCurrent*1.414*3; 			// Ðàñ÷åòíûé êîýôôèöèåíò ÀÖÏ ðàâåí òðîåêðàòíîé àìïëèòóäå íîìèíàëüíîãî òîêà
		p->motorInternals.iSenseK = (ADC_MAX_VALUE*1.0)/p->motorInternals.iADCk;	//êîýôôèöèåíò äàò÷èêà òîêà äëÿ ïðåîáðàçîâàíèÿ òîêà â àìïåðàõ â êîä ÀÖÏ

		p->motorInternals.ifADCk = p->motorInternals.RatedFluxCurrent*3; 			// Ðàñ÷åòíûé êîýôôèöèåíò ÀÖÏ ðàâåí òðîåêðàòíîìó íîìèíàëüíîìó òîêó ÎÂ
		p->motorInternals.ifSenseK = (ADC_MAX_VALUE*1.0)/p->motorInternals.ifADCk;	//êîýôôèöèåíò äàò÷èêà òîêà äëÿ ïðåîáðàçîâàíèÿ òîêà â àìïåðàõ â êîä ÀÖÏ


		p->motorInternals.isPhaseA=0;	//ôàçíûå òîêè
		p->motorInternals.isPhaseB=0;
		p->motorInternals.isPhaseC=0;
		p->motorInternals.psa=0;	//ïîòîêîñöåïëåíèÿ ôàç
		p->motorInternals.psb=0;
		p->motorInternals.psc=0;

		//Îïðåäåëèì êîëè÷åñòâî îòðåçêîâ (íîìåð ïîñëåäíåé òî÷êè) êðèâûõ íàìàãíè÷èâàíèÿ
		volatile int Psi_I_s_resolution = 0;
		volatile int Psi_I_r_resolution = 0;
		//â ïîñëåäíåé òî÷êå äîëæíû áûòü îòðèöàòåëüíûå çíà÷åíèÿ, êàê ìàðêåð îêîí÷àíèÿ ìàññèâà
		while ((Psi_I_s[Psi_I_s_resolution].i >= 0) && (Psi_I_s_resolution < 50)) Psi_I_s_resolution++;
		while ((Psi_I_r[Psi_I_r_resolution].i >= 0) && (Psi_I_r_resolution < 50)) Psi_I_r_resolution++;
		Psi_I_s_resolution--;	//óìåíüøàåì íà 1, ÷òîáû ïîëó÷èòü êîëè÷åñòâî îòðåçêîâ
		Psi_I_r_resolution--;	//óìåíüøàåì íà 1, ÷òîáû ïîëó÷èòü êîëè÷åñòâî îòðåçêîâ

		//ðàñ÷åò ïðåäåëîâ èçìåíåíèÿ ïåðåìåííûõ (óãîë, òîê, ïîòîêîñöåïëåíèå) è øàãà äèñêðåòèçàöèè ïî íèì
		p->motorInternals.ThetaMax = 2*MOTOR_MODEL_PI;										//äèàïàçîí óãëà - 0..2Ïè
		p->motorInternals.IMax = Psi_I_s[Psi_I_s_resolution].i;				//Ñ÷èòàåì, ÷òî ìàêñèìàëüíûé òîê ëåæèò â ïîñëåäíåé òî÷êå êðèâîé äëÿ ñîãëàñîâàííîãî ïîëîæåíèÿ
		p->motorInternals.PsiMax = Psi_I_s[Psi_I_s_resolution].psi;			//Ìàêñèìàëüíûé ïîòîê ëåæèò â ïîñëåäíåé òî÷êå êðèâîé äëÿ ñîãëàñîâàííîãî ïîëîæåíèÿ
		p->motorInternals.ThetaStep = p->motorInternals.ThetaMax/(SRD_SURFACE_RESOLUTION+1);	//ïðèðàùåíèå óãëà. Çäåñü, â îòëè÷èå îò òîêà è ïîòîêà, äåëèòñÿ íà SRD_SURFACE_RESOLUTION+1,
																	//ò.ê. êðèâàÿ çàöèêëåíà, è íå äîëæíî áûòü äâóõ îäèíàêîâûõ ñîñåäíèõ òî÷åê äëÿ óãëîâ 0 è 2ïè. Ò.å. òî÷êó 2ïè â êîíöå âûêèíóëè
		p->motorInternals.ThetaStep_inv = 1.0/p->motorInternals.ThetaStep;
		p->motorInternals.IStep = p->motorInternals.IMax/SRD_SURFACE_RESOLUTION;				//ïðèðàùåíèå òîêà
		p->motorInternals.IStep_inv = 1.0/p->motorInternals.IStep;
		p->motorInternals.PsiStep = p->motorInternals.PsiMax/SRD_SURFACE_RESOLUTION;			//ïðèðàùåíèå ïîòîêà
		p->motorInternals.PsiStep_inv = 1.0/p->motorInternals.PsiStep;
//		p->motorInternals.j = srm_j[p->MotorParametersNum-1];		//óæå ïðèñâîåí âûøå (Ñàâêèí)
//		p->motorInternals.MechLoss = p->motorInternals.j * 10; 		//ìåõàíè÷åñêèå ïîòåðè

		//Óòî÷íåíèå êðèâûõ íàìàãíè÷èâàíèÿ ïðè ïîìîùè êóáè÷åñêîé èíòåðïîëÿöèè, èíèöèàëèçàöèÿ ïîâåðõíîñòè psi=f(i,theta)
		for (int i=0; i<SRD_SURFACE_RESOLUTION+1; i++)
		{
			float TmpI = i*p->motorInternals.IStep;
			Psi_I_s_acc[i] = MotorModel_FluxCubicInterpolation (TmpI, Psi_I_s, Psi_I_s_resolution);	//â ïðèíöèïå, ýòè äâå óòî÷íåííûå êðèâûå ìîæíî óáðàòü, ò.ê. èç íèõ òóò æå ãåíåðèòñÿ ïîâåðõíîñòü, à
			Psi_I_r_acc[i] = MotorModel_FluxCubicInterpolation (TmpI, Psi_I_r, Psi_I_r_resolution);	//ñàìè êðèâûå íèãäå íå èñïîëüçóþòñÿ. Ìîæíî çäåñü ïðîñòî îñòàâèòü ëîêàëüíûå ïåðåìåííûå è ñýêîíîìèòü ïàìÿòü
			for (int t=0; t<SRD_SURFACE_RESOLUTION+1; t++)
			{
				float TmpTheta = t*p->motorInternals.ThetaStep;
				Psi_ITheta[i][t] = (Psi_I_s_acc[i] + Psi_I_r_acc[i])*0.5 - (Psi_I_s_acc[i] - Psi_I_r_acc[i])*0.5*_IQtoF(_IQcos(_IQ(TmpTheta)));	//Ýòó ïîâåðõíîñòü òîæå ìîæíî ñäåëàòü ëîêàëüíîé, îíà â ðàñ÷åòå ìîäåëè íå ó÷àñòâóåò
			}
		}

		//Ãåíåðàöèÿ ïîâåðõíîñòè i=f(psi,theta) èç psi=f(i,theta)
		for (int t=0; t<SRD_SURFACE_RESOLUTION+1; t++)
		{
			//òî÷êè ïî óãëó áóäóò òå æå, ïåðåâîðà÷èâàòü ïîâåðõíîñòü áóäåì äëÿ êàêæäîãî äèñêðåòíîãî óãëà psi=f(i) -> i=f(psi)
			for (int n=0; n<SRD_SURFACE_RESOLUTION+1; n++)
			{
				float TmpPsi = n*p->motorInternals.PsiStep;
				if (TmpPsi <= Psi_ITheta[0][t])	//îãðàíè÷èâàåì ìèíèìóì (íà âñÿêèé ñëó÷àé)
				{
					I_PsiTheta[n][t] = 0;
				}
				else if (TmpPsi >= Psi_ITheta[SRD_SURFACE_RESOLUTION][t])	//îãðàíè÷èâàåì ìàêñèìóì
				{
					I_PsiTheta[n][t] = p->motorInternals.IMax;	//íà êðèâîé íàìàãíè÷èâàíèÿ äëÿ äàííîãî óãëà ìîæåò âîîáùå íå áûòü ìàêñèìàëüíîãî ïîòîêà (áóäåò îí òîëüêî â ñîãëàñîâàííîì ïîëîæåíèè). Ïîýòîìó, áóäåò íàñûùåíèå
				}
				else	//â îãðàíè÷åíèÿ íå ïîïàëè, èíòåðïîëèðóåì ëèíåéíî.
				{
					for (int i = 1; i < SRD_SURFACE_RESOLUTION+1; i++)	//Èùåì îòðåçîê â èñõîäíîé êðèâîé psi=f(i), êóäà ïîïàë äèñêðåòíûé ïîòîê p
					{
						if (Psi_ITheta[i][t] >= TmpPsi)	//ïîòîê â ñëåäóþùåé òî÷êå áîëüøå çàäàííîãî, çíà÷èò ïîïàëè â íóæíûé îòðåçîê
						{
							I_PsiTheta[n][t] = p->motorInternals.IStep*i - (Psi_ITheta[i][t] - TmpPsi)*p->motorInternals.IStep/(Psi_ITheta[i][t] - Psi_ITheta[i-1][t]);
							break;
						}
					}
				}
			}
		}

		//Ãåíåðàöèÿ ïîâåðõíîñòè ìîìåíòà M=f(i,theta)
		for (int t=0; t<SRD_SURFACE_RESOLUTION+1; t++)
		{
			float WcoPlus = 0;								//Êîýíåðãèÿ ïðè óãëå Theta+
			float WcoMinus = 0;								//Êîýíåðãèÿ ïðè óãëå Theta-
			M_ITheta[0][t] = 0;								//Ìîìåíò ïðè íóëåâîì òîêå è ëþáîì óãëå - íóëåâîé
			float PsiPlus_prev = 0;
			float PsiMinus_prev = 0;

			//Èùåì ñîñåäíèå òî÷êè ïî ïîëîæåíèþ, ïðè ýòîì çàöèêëèâàåì äëÿ íåïðåðûâíîñòè ëèíåéíîé èíòåðïîëÿöèè ïî óãëó â ðàéîíå theta=0 è theta=2Pi
			int tPlus = t+1;
			if (tPlus > SRD_SURFACE_RESOLUTION)
				tPlus -= (SRD_SURFACE_RESOLUTION+1);
			int tMinus = t-1;
			if (tMinus < 0)
				tMinus += (SRD_SURFACE_RESOLUTION+1);
			for (int i=1; i<SRD_SURFACE_RESOLUTION+1; i++)
			{
				//Èùåì ïîòîêîñöåïëåíèå ïðè çàäàííîì òîêå è óãëå ÷óòü áîëüøå è ÷óòü ìåíüøå (íà SRD_DELTA_THETA) çàäàííîãî ïðè ïîìîùè ëèíåéíîé èíòåðïîëÿöèè
				float PsiPlus = Psi_ITheta[i][t] + (Psi_ITheta[i][tPlus] - Psi_ITheta[i][t])*SRD_DELTA_THETA*p->motorInternals.ThetaStep_inv;
				float PsiMinus = Psi_ITheta[i][t] - (Psi_ITheta[i][t] - Psi_ITheta[i][tMinus])*SRD_DELTA_THETA*p->motorInternals.ThetaStep_inv;
				//Ñ÷èòàåì êîýíåðãèþ - èíòåãðàë ïîòîêîñöåïëåíèÿ ïî òîêó (ìåòîäîì òðàïåöèé)
				WcoPlus += (PsiPlus + PsiPlus_prev)*0.5*p->motorInternals.IStep;
				WcoMinus += (PsiMinus + PsiMinus_prev)*0.5*p->motorInternals.IStep;
				PsiPlus_prev = PsiPlus;
				PsiMinus_prev = PsiMinus;
				//PhaseTorque = p->motorInternals.pp*(p->motorInternals.wm-p->motorInternals.wp)*50;	// 50 = inv_0.02 dTeta				òàê áûëî â èñõîäíîì âàðèàíòå
				M_ITheta[i][t] = p->motorInternals.pp*(WcoPlus - WcoMinus)*SRD_DELTA_THETA_INV*0.5;		//ñ÷èòàåì ìîìåíò, êàê ïðîèçâîäíóþ êîýíåðãèè
			}
		}
	}

	// Ðàñ÷åò êîýôôèöèåíòîâ ìîäåëè, íåèçìåííûõ âî âðåìåíè
	p->motorInternals.inv_tpr=1.0/p->motorInternals.tpr;
	p->motorInternals.inv_j=1.0/p->motorInternals.j;

	p->motorInternals.t2 = p->motorInternals.t / 2;				//ïîëîâèíà øàãà äèñêðåòèçàöèè

	// Îáíóëåíèå ïåðåìåííûõ ñîñòîÿíèÿ
	p->motorInternals.tetaR = 0;			//óãîë ïîëîæåíèÿ ðîòîðà ýëåêòðè÷åñêèé
	p->motorInternals.tetaRM = 0;				//óãîë ïîëîæåíèÿ ðîòîðà ìåõàíè÷åñêèé
	p->motorInternals.omega = 0;						//ñêîðîñòü, ðàä/ñ
	p->motorInternals.prd = 0;					//ïîòîêîñöåïëåíèÿ ðîòîðà â îñÿõ d,q
	p->motorInternals.prq = 0;
	p->motorInternals.isa = 0;						//òîêè ñòàòîðà â îñÿõ àëüôà,áåòà
	p->motorInternals.isb = 0;
	p->motorInternals.ira = 0;						//òîêè ðîòîðà â îñÿõ àëüôà,áåòà
	p->motorInternals.irb = 0;
	p->motorInternals.ird = 0;						//òîêè ðîòîðà â îñÿõ d,q
	p->motorInternals.irq = 0;


}

//ðàñ÷åò íàïðÿæåíèé äëÿ äâóõôàçíîãî äâèãàòåëÿ
void MotorModel_calcU42Phase(TMotorModel *p) {
	if (p->InvertorEna) { //èíâåðòîð âêëþ÷åí
		// ðàñ÷åò íàïðÿæåíèé ïèòàíèÿ
		// ïåðåðàñ÷åò âëèÿíèÿ ìåðòâîãî âðåìåíè íà ñêâàæíîñòü ôàçû â çàâèñèìîñòè îò çíàêà òîêà
		if (p->motorInternals.i_a > 0) {
			if (p->motorInternals.cmpr0 > p->motorInternals.dt)
				p->motorInternals.dta = -p->motorInternals.dt;
			else
				p->motorInternals.dta = -p->motorInternals.cmpr0;
		} else {
			if ((p->motorInternals.cmpr0 + p->motorInternals.dt) < p->motorInternals.tpr)
				p->motorInternals.dta = +p->motorInternals.dt;
			else
				p->motorInternals.dta = p->motorInternals.tpr - p->motorInternals.cmpr0;
		}
		if (p->motorInternals.i_a < 0) {
			if (p->motorInternals.cmpr1 > p->motorInternals.dt)
				p->motorInternals.dtb = -p->motorInternals.dt;
			else
				p->motorInternals.dtb = -p->motorInternals.cmpr1;
		} else {
			if ((p->motorInternals.cmpr1 + p->motorInternals.dt) < p->motorInternals.tpr)
				p->motorInternals.dtb = +p->motorInternals.dt;
			else
				p->motorInternals.dtb = p->motorInternals.tpr - p->motorInternals.cmpr1;
		}
		//ðàñ÷åò ïîòåíöèàëîâ ñ ó÷åòîì âëèÿíèÿ ìåðòâîãî âðåìåíè
		p->motorInternals.fia = p->motorInternals.udc * (p->motorInternals.cmpr0 + (p->motorInternals.dta >> 1)) / p->motorInternals.tpr;
		p->motorInternals.fib = p->motorInternals.udc * (p->motorInternals.cmpr1 + (p->motorInternals.dtb >> 1)) / p->motorInternals.tpr;
		p->motorInternals.u_a = p->motorInternals.fia - p->motorInternals.fib;//Íàïðÿæåíèå íà ÿêîðå

		if (p->motorInternals.i_f > 0) {
			if (p->motorInternals.cmpr2 > p->motorInternals.dt)
				p->motorInternals.dtc = -p->motorInternals.dt;
			else
				p->motorInternals.dtc = -p->motorInternals.cmpr2;
		} else {
			if ((p->motorInternals.cmpr2 + p->motorInternals.dt) < p->motorInternals.tpr)
				p->motorInternals.dtc = +p->motorInternals.dt;
			else
				p->motorInternals.dtc = p->motorInternals.tpr - p->motorInternals.cmpr2;
		}
		//ðàñ÷åò ïîòåíöèàëîâ ñ ó÷åòîì âëèÿíèÿ ìåðòâîãî âðåìåíè
		p->motorInternals.fic = p->motorInternals.udc * (p->motorInternals.cmpr2 + (p->motorInternals.dtc >> 1)) / p->motorInternals.tpr;
		p->motorInternals.u_f = p->motorInternals.fic;//Íàïðÿæåíèå íà ÎÂ


		p->motorInternals.isPhaseA_prev = p->motorInternals.isPhaseA;
		p->motorInternals.CurrentInvertedFlag = 0;
	} else
	{//Èíâåðòîð âûêëþ÷åí
/*
		if (p->motorInternals.isPhaseA > 0)
			p->motorInternals.u_a = -p->motorInternals.udc;
		else
			p->motorInternals.u_a = p->motorInternals.udc;
*/
		p->motorInternals.u_a = 0;

		p->motorInternals.fia = 0;
		p->motorInternals.fib = 0;
		p->motorInternals.fic = 0;

		if (p->motorInternals.isPhaseA * p->motorInternals.isPhaseA_prev < 0)		//òîê ñìåíèë ñâîé çíàê ñ ìîìåíòà âûêë÷þåíèÿ èíâåðòîðà
			p->motorInternals.CurrentInvertedFlag = 7;//ôëàã, ÷òî âñå èíòåðåñóþùèå òîêè ïåðåñåêëè íîëü

		if (p->motorInternals.CurrentInvertedFlag == 7) {//ïðèêëàäûâàåì íóëåâîå íàïðÿæåíèå, à â ìîäåëè ÄÏÒ îáíóëèòñÿ ôîðìóëà òîêà
			p->motorInternals.u_a=0;
		}
		p->motorInternals.u_f = 0;//Íàïðÿæåíèå ÎÂ ðàâíî íóëþ
	}


}

//ðàñ÷åò íàïðÿæåíèé äëÿ òðåõôàçíîãî äâèãàòåëÿ
void MotorModel_calcU43Phase(TMotorModel *p) {
	if (p->InvertorEna) { //èíâåðòîð âêëþ÷åí
		// ðàñ÷åò íàïðÿæåíèé ïèòàíèÿ
		// ïåðåðàñ÷åò âëèÿíèÿ ìåðòâîãî âðåìåíè íà ñêâàæíîñòü ôàçû â çàâèñèìîñòè îò çíàêà òîêà
		if (p->motorInternals.isPhaseA > 0) {
			if (p->motorInternals.cmpr0 > p->motorInternals.dt)
				p->motorInternals.dta = -p->motorInternals.dt;
			else
				p->motorInternals.dta = -p->motorInternals.cmpr0;
		} else {
			if ((p->motorInternals.cmpr0 + p->motorInternals.dt) < p->motorInternals.tpr)
				p->motorInternals.dta = +p->motorInternals.dt;
			else
				p->motorInternals.dta = p->motorInternals.tpr - p->motorInternals.cmpr0;
		}
		if (p->motorInternals.isPhaseB > 0) {
			if (p->motorInternals.cmpr1 > p->motorInternals.dt)
				p->motorInternals.dtb = -p->motorInternals.dt;
			else
				p->motorInternals.dtb = -p->motorInternals.cmpr1;
		} else {
			if ((p->motorInternals.cmpr1 + p->motorInternals.dt) < p->motorInternals.tpr)
				p->motorInternals.dtb = +p->motorInternals.dt;
			else
				p->motorInternals.dtb = p->motorInternals.tpr - p->motorInternals.cmpr1;
		}
		if (p->motorInternals.isPhaseC > 0) {
			if (p->motorInternals.cmpr2 > p->motorInternals.dt)
				p->motorInternals.dtc = -p->motorInternals.dt;
			else
				p->motorInternals.dtc = -p->motorInternals.cmpr2;
		} else {
			if ((p->motorInternals.cmpr2 + p->motorInternals.dt) < p->motorInternals.tpr)
				p->motorInternals.dtc = +p->motorInternals.dt;
			else
				p->motorInternals.dtc = p->motorInternals.tpr - p->motorInternals.cmpr2;
		}

		//ðàñ÷åò ïîòåíöèàëîâ ñ ó÷åòîì âëèÿíèÿ ìåðòâîãî âðåìåíè
		p->motorInternals.fia = p->motorInternals.udc * (p->motorInternals.cmpr0 + (p->motorInternals.dta >> 1)) / p->motorInternals.tpr;
		p->motorInternals.fib = p->motorInternals.udc * (p->motorInternals.cmpr1 + (p->motorInternals.dtb >> 1)) / p->motorInternals.tpr;
		p->motorInternals.fic = p->motorInternals.udc * (p->motorInternals.cmpr2 + (p->motorInternals.dtc >> 1)) / p->motorInternals.tpr;
		p->motorInternals.fiav = (p->motorInternals.fia + p->motorInternals.fib + p->motorInternals.fic) / 3;		//ïîòåíöèàë ñðåæíåé òî÷êè
		p->motorInternals.ua = p->motorInternals.fia - p->motorInternals.fiav;					//íàïðÿæåíèÿ
		p->motorInternals.ub = p->motorInternals.fib - p->motorInternals.fiav;
		p->motorInternals.uc = p->motorInternals.fic - p->motorInternals.fiav;

		//íàïðÿæåíèÿ â îñÿõ àëüôà,áåòà
		p->motorInternals.usa = p->motorInternals.ua;
		p->motorInternals.usb = 0.577350269 * p->motorInternals.ua + 1.154700538 * p->motorInternals.ub;

		p->motorInternals.isPhaseA_prev = p->motorInternals.isPhaseA;
		p->motorInternals.isPhaseB_prev = p->motorInternals.isPhaseB;
		p->motorInternals.isPhaseC_prev = p->motorInternals.isPhaseC;
		p->motorInternals.CurrentInvertedFlag = 0;

	} else { //èíâåðòîð âûêëþ÷åí
		//ñëîæíî ðàññ÷èòàòü íàïðÿæåíèå, êîòîðîå ïðèëîæåíî ê äâèãàòåëþ, êîãäà èíâåðòîð âûêëþ÷åí.
		//ïîýòîìó òóò èíâåðòîð êàê áû â ðåëåéíîì ðåæèìå ñòðåìèòñÿ ïîääåðæèâàòü òîêè íóëåâûìè, ïîêà ñêîðîñòü íå ñíèçèòñÿ äî íóëÿ,
		//à ïîòîì "çàìûêàåò" ôàçû äâèãàòåëÿ. Ýòî îòëè÷àåòñÿ îò ðåàëüíîñòè, êîíå÷íî, íî ëó÷øå, ÷åì íå äåëàòü íè÷åãî.

		if (p->motorInternals.isPhaseA > 0)
			p->motorInternals.fia = 0;
		else
			p->motorInternals.fia = p->motorInternals.udc;

		if (p->motorInternals.isPhaseB > 0)
			p->motorInternals.fib = 0;
		else
			p->motorInternals.fib = p->motorInternals.udc;

		if (p->motorInternals.isPhaseC > 0)
			p->motorInternals.fic = 0;
		else
			p->motorInternals.fic = p->motorInternals.udc;

		if (p->motorInternals.isPhaseA * p->motorInternals.isPhaseA_prev < 0)		//òîê ñìåíèë ñâîé çíàê ñ ìîìåíòà âûêë÷þåíèÿ èíâåðòîðà
			p->motorInternals.CurrentInvertedFlag |= 1;

		if (p->motorInternals.isPhaseB * p->motorInternals.isPhaseB_prev < 0)		//òîê ñìåíèë ñâîé çíàê ñ ìîìåíòà âûêë÷þåíèÿ èíâåðòîðà
			p->motorInternals.CurrentInvertedFlag |= 2;

		if (p->motorInternals.isPhaseC * p->motorInternals.isPhaseC_prev < 0)		//òîê ñìåíèë ñâîé çíàê ñ ìîìåíòà âûêë÷þåíèÿ èíâåðòîðà
			p->motorInternals.CurrentInvertedFlag |= 4;

		if (p->motorInternals.CurrentInvertedFlag == 7) {
			p->motorInternals.fia = 0;
			p->motorInternals.fib = 0;
			p->motorInternals.fic = 0;
		}

		p->motorInternals.fiav = (p->motorInternals.fia + p->motorInternals.fib + p->motorInternals.fic) * (1.0/3);		//ïîòåíöèàë ñðåæíåé òî÷êè
		p->motorInternals.ua = p->motorInternals.fia - p->motorInternals.fiav;					//íàïðÿæåíèÿ
		p->motorInternals.ub = p->motorInternals.fib - p->motorInternals.fiav;
		p->motorInternals.uc = p->motorInternals.fic - p->motorInternals.fiav;

		//íàïðÿæåíèÿ â îñÿõ àëüôà,áåòà
		p->motorInternals.usa = p->motorInternals.ua;
		p->motorInternals.usb = 0.577350269 * p->motorInternals.ua + 1.154700538 * p->motorInternals.ub;
	}
}

//ðàñ÷åò íàïðÿæåíèÿ äëÿ Πñèíõðîííîãî äâèãàòåëÿ
void MotorModel_calcU4SyncMFieldWinding(TMotorModel *p) {
	if (p->InvertorEna) { //èíâåðòîð âêëþ÷åí
		// ðàñ÷åò íàïðÿæåíèé ïèòàíèÿ
		// ïåðåðàñ÷åò âëèÿíèÿ ìåðòâîãî âðåìåíè íà ñêâàæíîñòü ôàçû â çàâèñèìîñòè îò çíàêà òîêà
		if (p->motorInternals.i_f > 0) {
			if (p->motorInternals.cmpr3 > p->motorInternals.dt)
				p->motorInternals.dtf = -p->motorInternals.dt;
			else
				p->motorInternals.dtf = -p->motorInternals.cmpr3;
		} else {
			if ((p->motorInternals.cmpr3 + p->motorInternals.dt) < p->motorInternals.tpr)
				p->motorInternals.dtf = +p->motorInternals.dt;
			else
				p->motorInternals.dtf = p->motorInternals.tpr - p->motorInternals.cmpr3;
		}
		//ðàñ÷åò ïîòåíöèàëîâ ñ ó÷åòîì âëèÿíèÿ ìåðòâîãî âðåìåíè
		p->motorInternals.u_f = p->motorInternals.udc * (p->motorInternals.cmpr3 + (p->motorInternals.dtf >> 1)) / p->motorInternals.tpr;//Íàïðÿæåíèå íà ÎÂ
	} else
	{//Èíâåðòîð âûêëþ÷åí
		p->motorInternals.u_f = 0;//Íàïðÿæåíèå ÎÂ ðàâíî íóëþ
	}


}

void MotorModel_MechModelExecute(TMotorModel *p) {
	float d_omega;
	p->loadTmp = 0;
	if (p->motorInternals.omega > 0)
		p->loadTmp = p->load + p->motorInternals.MechLoss;
	if (p->motorInternals.omega < 0)
		p->loadTmp = -p->load - p->motorInternals.MechLoss;

	d_omega = p->motorInternals.t / p->motorInternals.j * (p->motorInternals.torque - p->loadTmp);					//ïðèðàùåíèå ñêîðîñòè

	if ((fabs(d_omega) > fabs(p->motorInternals.omega)) && (fabs(p->motorInternals.torque) < p->load)) {
		p->motorInternals.omega = 0;
		d_omega = 0;
	}

	p->motorInternals.omega = p->motorInternals.omega + d_omega;	//ñêîðîñòü


}

//êóáè÷åñêàÿ èíòåðïîëÿöèÿ äëÿ êðèâûõ íàìàãíè÷èâàíèÿ SRD
float MotorModel_FluxCubicInterpolation (volatile float Isource, volatile TFluxCurvePoint SourceArray[], volatile int SourceResolution)
{
	volatile float PsiResult=0;
	if (Isource <= SourceArray[0].i)	//òîê ìåíüøå èëè ðàâåí ìèíèìóìó
	{
		PsiResult = SourceArray[0].psi;
		return PsiResult;
	}
	if (Isource >= SourceArray[SourceResolution].i)	//òîê áîëüøå èëè ðàâåí ìàêñèìóìó
	{
		PsiResult = SourceArray[SourceResolution].psi;
		return PsiResult;
	}
	for (int count = 1; count < SourceResolution+1; count++)	//ïîïàëè âíóòðü ãðàíèö, èùåì íóæíûé îòðåçîê
	{
		if (SourceArray[count].i >= Isource)	//òîê â ñëåäóþùåé òî÷êå áîëüøå çàäàííîãî, çíà÷èò ïîïàëè â íóæíûé îòðåçîê
		{
			volatile float a, b, c, d;	//êîýôôèöèåíòû êóáè÷åñêîãî ïîëèíîìà
			volatile float f_d0, f_d1;	//ïðîèçâîäíûå â ëåâîé è ïðàâîé òî÷êå îòðåçêà
			volatile float x_m;			//ìàñøòàáèðîâàííûé èêñ: òàêîé, ÷òîáû ëåâàÿ òî÷êà îòðåçêà îêàçàëàñü â íóëå, à ïðàâàÿ - â åäèíèöå
			//Ôîðìóëà êóáè÷åñêîé èíòåðïîëÿöèè f(x)=a*x^3 + b*x^2 + c*x + d, ïðîèçâîäíàÿ: f(x)=3*a*x^2 + 2*b*x + c
			//×òîáû íàéòè êîýôôèöèåíòû êóáè÷åñêîãî ïîëèíîìà, íóæíî íàéòè çíà÷åíèÿ ïîëèíîìà íà ãðàíèöàõ îòðåçêà ñïðàâà è ñëåâà,
			//à òàêæå ïðîèçâîäíóþ ïîëèíîìà â ýòèõ òî÷êàõ. Äëÿ âû÷èñëåíèÿ ïðîèçâîäíûõ íóæíû çíà÷åíèÿ èñõîäíîé äèñêðåòíîé ôóíêöèè
			//â 2 ñîñåäíèõ òî÷êàõ (ëåâåå è ïðàâåå ãðàíèö îòðåçêà). Ñ÷èòàåì òîê=0 - ëåâàÿ ãðàíèöà îòðåçêà, òîê=1 - ïðàâàÿ ãðàíèöà
			//a = 2*f(0) - 2*f(1) + f'(0) + f'(1)
			//b = -3*f(0) + 3*f(1) - 2*f'(0) - f'(1)
			//c = f'(0)
			//d = f(0)
			//Ò.ê. øàã ïî òîêó íåðàâíîìåðíûé, à x[1]-x[0]!=1, ïðîèçâîäíûå èùþòñÿ ïî ôîðìóëàì:
			//f'(0) = (f[1] - f[-1]) / {(x[1]-x[-1]) / (x[1]-x[0])}, ò.å., èñõîäíàÿ êðèâàÿ ðàñòÿãèâàåòñÿ ïî îñè x, ÷òîáû âûïîëíÿëîñü x[1]-x[0]=1
			//f'(1) = (f[2] - f[0]) / {(x[2]-x[0]) / (x[1]-x[0])}
			//Óïðîñòèì: f'(0) = (f[1] - f[-1]) *(x[1]-x[0]) / (x[1]-x[-1]), f'(1) = (f[2] - f[0]) *(x[1]-x[0]) / (x[2]-x[0])
			//Ïîñ÷èòàâ, êîýôôèöèåíòû, íóæíî âûäàòü ðåçóëüòàò ïî ôîðìóëå:
			//f(x)= a*xm^3 + b*xm^2 + c*xm + d,
			//ãäå xm=(x-x[0])/(x[1]-x[0]), ò.ê. x[0] íå ðàâåí 0, à x[1] íå ðàâåí 1. Ò.å., íàäî ñäâèíóòü (âû÷èòàåì x[0]) è ñæàòü îáðàòíî (äåëèì íà x[1]-x[0])
			//òî÷êà -1: count-2
			//òî÷êà 0: count-1
			//òî÷êà 1: count
			//òî÷êà 2: count+1

			//ñ÷èòàåì ïðîèçâîäíûå â òî÷êàõ 0 è 1 (íà êðàÿõ îòðåçêà)
			if (count == 1)	//êðàéíèé ëåâûé îòðåçîê
			{
				//ïðîèçâîäíóþ ñëåâà ïî-îáû÷íîìó ïîñ÷èòàòü íåëüçÿ, ò.ê. êðàéíÿÿ òî÷êà, ïîñ÷èòàåì ïðîèçâîäíóþ ïî ñàìîìó îòðåçêó
				f_d0 = SourceArray[count].psi - SourceArray[count-1].psi;	//íà ðàçíîñòü òîêîâ íà êðàÿõ îòðåçêà íå äåëèì, ò.ê. ïðèíÿëè åå çà åäèíèöó
			}
			else	//ñ÷èòàåì ëåâóþ ïðîèçâîäíóþ êàê îáû÷íî
			{
				f_d0 = (SourceArray[count].psi - SourceArray[count-2].psi)*(SourceArray[count].i - SourceArray[count-1].i)/(SourceArray[count].i - SourceArray[count-2].i);
			}
			if (count == SourceResolution)	//êðàéíèé ïðàâûé îòðåçîê
			{
				//ïðîèçâîäíóþ ñïðàâà ïî-îáû÷íîìó ïîñ÷èòàòü íåëüçÿ, ò.ê. êðàéíÿÿ òî÷êà, ïîñ÷èòàåì ïðîèçâîäíóþ ïî ñàìîìó îòðåçêó
				f_d1 = SourceArray[count].psi - SourceArray[count-1].psi;	//íà ðàçíîñòü òîêîâ íà êðàÿõ îòðåçêà íå äåëèì, ò.ê. ïðèíÿëè åå çà åäèíèöó
			}
			else	//ñ÷èòàåì ïðàâóþ ïðîèçâîäíóþ êàê îáû÷íî
			{
				f_d1 = (SourceArray[count+1].psi - SourceArray[count-1].psi)*(SourceArray[count].i - SourceArray[count-1].i)/(SourceArray[count+1].i - SourceArray[count-1].i);
			}

			//ñ÷èòàåì êîýôôèöèåíòû ïîëèíîìà
			a = 2*SourceArray[count-1].psi - 2*SourceArray[count].psi + f_d0 + f_d1;
			b = -3*SourceArray[count-1].psi + 3*SourceArray[count].psi - 2*f_d0 - f_d1;
			c = f_d0;
			d = SourceArray[count-1].psi;

			//ñ÷èòàåì òîê ñ ó÷åòîì ñäâèíóòîñòè è ðàñòÿíóòîñòè êóáè÷åñêîé ïàðàáîëû îòíîñèòåëüíî èñõîäíîãî ãðàôèêà ïî îñè èêñ
			x_m = (Isource - SourceArray[count-1].i)/(SourceArray[count].i - SourceArray[count-1].i);

			PsiResult = a*x_m*x_m*x_m + b*x_m*x_m + c*x_m + d;	//ñ÷èòàåì èñêîìûé ïîòîê
			return PsiResult;
		}
	}
	return PsiResult;//ñþäà íå äîéäåì íà ñàìîì äåëå, íî ÷òîáû êîìïèëÿòîð íå ðóãàëñÿ
}

//äâóõìåðíàÿ èíòåðïîëÿöèÿ ïî ïîâåðõíîñòè äëÿ ïîèñêà òîêà â ìîäåëè SRD
float MotorModel_PhaseCurrentInterpolation (TMotorModel *p, volatile float Theta, volatile float PsiS)
{
	volatile float Current = 0;
	volatile float Current1, Current2;
	volatile int cntThetaMinus, cntThetaPlus, cntPsiMinus, cntPsiPlus;

	//îãðàíè÷èâàåì âõîäíûå ïåðåìåííûå
	//ïî óãëó íàäî çàöèêëèòü
	if (Theta < 0)
		Theta += p->motorInternals.ThetaMax;	//ïðèáàâëÿåì 2ïè
	if (Theta > p->motorInternals.ThetaMax)
		Theta -= p->motorInternals.ThetaMax;	//âû÷èòàåì 2ïè
	cntThetaMinus = Theta*p->motorInternals.ThetaStep_inv;	//èíäåêñ ìàññèâà ïî óãëó (ìåíüøèé èç äâóõ)
	cntThetaPlus = cntThetaMinus + 1;			//áîëüøèé èç äâóõ
	if (cntThetaPlus > SRD_SURFACE_RESOLUTION)		//åñëè âûøåë çà ãðàíèöó ìàññèâà, çàöèêëèâàåì
	{
		cntThetaPlus -= (SRD_SURFACE_RESOLUTION+1);
	}
	//ïî ïîòîêó íàäî ïðîñòî îãðàíè÷èòü ñâåðõó è ñíèçó
	if (PsiS < 0)
		PsiS = 0;
	if (PsiS > p->motorInternals.PsiMax)
		PsiS = p->motorInternals.PsiMax;
	cntPsiMinus = PsiS*p->motorInternals.PsiStep_inv;		//èíäåêñ ìàññèâà ïî ïîòîêó (ìåíüøèé èç äâóõ)
	cntPsiPlus = cntPsiMinus + 1;				//áîëüøèé èç äâóõ

	//ïåðâàÿ èíòåðïîëÿöèÿ ïî óãëó
	Current1 = I_PsiTheta[cntPsiMinus][cntThetaMinus] + (I_PsiTheta[cntPsiMinus][cntThetaPlus] - I_PsiTheta[cntPsiMinus][cntThetaMinus])*(Theta - (float)cntThetaMinus*p->motorInternals.ThetaStep)*p->motorInternals.ThetaStep_inv;
	//åñëè ïîïàëè íà âåðõíþþ ãðàíèöó ïî ïîòîêó, íàäî èíòåðïîëèðîâàòü ïî ñîêðàùåííûì ôîðìóëàì, ÷òîáû íå âûïàñòü çà ãðàíèöû ìàññèâà
	if (cntPsiMinus >= SRD_SURFACE_RESOLUTION)	//ïî ïîòîêó îêàçàëèñü íà âåðõíåé ãðàíèöå
	{
		//èíòåðïîëÿöèÿ ïî ïîòîêó
		Current = Current1;	//èíòåðïîëèðîâàòü íà ãðàíèöå ïî ïîòîêó íå íàäî, ìîìåíò áóäåò ðàâåí ìîìåíòó îò ïåðâîé èíòåðïîëÿöèè ïî óãëó
		return Current;
	}
	//âòîðàÿ èíòåðïîëÿöèÿ ïî óãëó
	Current2 = I_PsiTheta[cntPsiPlus][cntThetaMinus] + (I_PsiTheta[cntPsiPlus][cntThetaPlus] - I_PsiTheta[cntPsiPlus][cntThetaMinus])*(Theta - (float)cntThetaMinus*p->motorInternals.ThetaStep)*p->motorInternals.ThetaStep_inv;
	//èíòåðïîëÿöèÿ ïî ïîòîêó
	Current = Current1 + (Current2 - Current1)*(PsiS - (float)cntPsiMinus*p->motorInternals.PsiStep)*p->motorInternals.PsiStep_inv;
	return Current;
}

//äâóõìåðíàÿ èíòåðïîëÿöèÿ ïî ïîâåðõíîñòè äëÿ ïîèñêà ìîìåíòà â ìîäåëè SRD
float MotorModel_PhaseTorqueInterpolation (TMotorModel *p, volatile float Theta, volatile float IPh)
{

	volatile float Torque = 0;
	volatile float Torque1, Torque2;
	volatile int cntThetaMinus, cntThetaPlus, cntIMinus, cntIPlus;

	//îãðàíè÷èâàåì âõîäíûå ïåðåìåííûå
	//ïî óãëó íàäî çàöèêëèòü
	if (Theta < 0)
		Theta += p->motorInternals.ThetaMax;	//ïðèáàâëÿåì 2ïè
	if (Theta > p->motorInternals.ThetaMax)
		Theta -= p->motorInternals.ThetaMax;	//âû÷èòàåì 2ïè
	cntThetaMinus = Theta*p->motorInternals.ThetaStep_inv;	//èíäåêñ ìàññèâà ïî óãëó (ìåíüøèé èç äâóõ)
	cntThetaPlus = cntThetaMinus + 1;			//áîëüøèé èç äâóõ
	if (cntThetaPlus > SRD_SURFACE_RESOLUTION)		//åñëè âûøåë çà ãðàíèöó ìàññèâà, çàöèêëèâàåì
	{
		cntThetaPlus -= (SRD_SURFACE_RESOLUTION+1);
	}
	//ïî òîêó íàäî ïðîñòî îãðàíè÷èòü ñâåðõó è ñíèçó
	if (IPh < 0)
		IPh = 0;
	if (IPh > p->motorInternals.IMax)
		IPh = p->motorInternals.IMax;
	cntIMinus = IPh*p->motorInternals.IStep_inv;		//èíäåêñ ìàññèâà ïî òîêó (ìåíüøèé èç äâóõ)
	cntIPlus = cntIMinus + 1;				//áîëüøèé èç äâóõ

	//ïåðâàÿ èíòåðïîëÿöèÿ ïî óãëó
	Torque1 = M_ITheta[cntIMinus][cntThetaMinus] + (M_ITheta[cntIMinus][cntThetaPlus] - M_ITheta[cntIMinus][cntThetaMinus])*(Theta - (float)cntThetaMinus*p->motorInternals.ThetaStep)*p->motorInternals.ThetaStep_inv;
	//åñëè ïîïàëè íà âåðõíþþ ãðàíèöó ïî òîêó, íàäî èíòåðïîëèðîâàòü ïî ñîêðàùåííûì ôîðìóëàì, ÷òîáû íå âûïàñòü çà ãðàíèöû ìàññèâà
	if (cntIMinus >= SRD_SURFACE_RESOLUTION)	//ïî òîêó îêàçàëèñü íà âåðõíåé ãðàíèöå
	{
		//èíòåðïîëÿöèÿ ïî òîêó
		Torque = Torque1;	//èíòåðïîëèðîâàòü íà ãðàíèöå ïî òîêó íå íàäî, ìîìåíò áóäåò ðàâåí ìîìåíòó îò ïåðâîé èíòåðïîëÿöèè ïî óãëó
		return Torque;
	}
	//âòîðàÿ èíòåðïîëÿöèÿ ïî óãëó
	Torque2 = M_ITheta[cntIPlus][cntThetaMinus] + (M_ITheta[cntIPlus][cntThetaPlus] - M_ITheta[cntIPlus][cntThetaMinus])*(Theta - (float)cntThetaMinus*p->motorInternals.ThetaStep)*p->motorInternals.ThetaStep_inv;
	//èíòåðïîëÿöèÿ ïî òîêó
	Torque = Torque1 + (Torque2 - Torque1)*(IPh - (float)cntIMinus*p->motorInternals.IStep)*p->motorInternals.IStep_inv;
	return Torque;
}

void Model_Execute_InductionMotor(TMotorModel *p) {
	MotorModel_calcU43Phase(p);	//ðàñ÷åò íàïðÿæåíèé

	// Ðàñ÷åò ñèíóñà è êîñèíóñà óãëà ðîòîðà
	p->motorInternals.cosTetaR = _IQtoF(_IQcos(_IQ(p->motorInternals.tetaR)));//ñèíóñ è êîñèíóñ ñ÷èòàþòñÿ ñ ôèêñèðîâàííîé òî÷êîé, ò.ê. ôëîàòîâñêèå çàíèìàþò òûñÿ÷è òàêòîâ (âèäèìî, êàêàÿ-òî ìåäëåííàÿ ðåàëèçàöèÿ, íå èñïîëüçóþùàÿ àïïàðàòíóþ ïîääåðæêó).
	p->motorInternals.sinTetaR = _IQtoF(_IQsin(_IQ(p->motorInternals.tetaR)));//åñëè óäàñòñÿ íàéòè áûñòðûé ïëàâàþùèé ñèíóñ è êîñèíóñ, òî èõ ìîæíî ñ÷èòàòü âî ôëîàòå

	// Ðàñ÷åò èçìåíåíèé ïîòîêîñöåïëåíèé (ïðåäèêòîðû)
	p->motorInternals.dpsa = (p->motorInternals.usa - p->motorInternals.isa * p->motorInternals.rs);
	p->motorInternals.dpsb = (p->motorInternals.usb - p->motorInternals.isb * p->motorInternals.rs);
	p->motorInternals.dprd = -(p->motorInternals.ird * p->motorInternals.rr);
	p->motorInternals.dprq = -(p->motorInternals.irq * p->motorInternals.rr);

	// Ðàñ÷åò ïðåäèêòîðîâ ïîòîêîñöåïëåíèé
	p->motorInternals.ppsa = p->motorInternals.psa + p->motorInternals.dpsa * p->motorInternals.t;
	p->motorInternals.ppsb = p->motorInternals.psb + p->motorInternals.dpsb * p->motorInternals.t;
	p->motorInternals.pprd = p->motorInternals.prd + p->motorInternals.dprd * p->motorInternals.t;
	p->motorInternals.pprq = p->motorInternals.prq + p->motorInternals.dprq * p->motorInternals.t;

	// Ïîâîðîò ïîòîêîñöåïëåíèé ðîòîðà â êîîðäèíàòû alpha,beta
	p->motorInternals.ppra = p->motorInternals.pprd * p->motorInternals.cosTetaR - p->motorInternals.pprq * p->motorInternals.sinTetaR;
	p->motorInternals.pprb = +p->motorInternals.pprd * p->motorInternals.sinTetaR + p->motorInternals.pprq * p->motorInternals.cosTetaR;

	// Ðàñ÷åò òîêîâ äëÿ ïðåäèêòîðíîãî óðàâíåíèÿ
	p->motorInternals.isa = p->motorInternals.ks * p->motorInternals.ppsa + p->motorInternals.km * p->motorInternals.ppra;
	p->motorInternals.isb = p->motorInternals.ks * p->motorInternals.ppsb + p->motorInternals.km * p->motorInternals.pprb;
	p->motorInternals.ira = p->motorInternals.km * p->motorInternals.ppsa + p->motorInternals.kr * p->motorInternals.ppra;
	p->motorInternals.irb = p->motorInternals.km * p->motorInternals.ppsb + p->motorInternals.kr * p->motorInternals.pprb;

	// Ïîâîðîò ðîòîðíûõ òîêîâ â d,q êîîðäèíàòû
	p->motorInternals.ird = p->motorInternals.ira * p->motorInternals.cosTetaR + p->motorInternals.irb * p->motorInternals.sinTetaR;
	p->motorInternals.irq = -p->motorInternals.ira * p->motorInternals.sinTetaR + p->motorInternals.irb * p->motorInternals.cosTetaR;

	// Ðàñ÷åò èçìåíåíèé ïîòîêîñöåïëåíèé ïî Ðóíãå-Êóòòà âòîðîãî ïîðÿäêà
	p->motorInternals.psa = p->motorInternals.psa + p->motorInternals.t2 * (p->motorInternals.dpsa + (p->motorInternals.usa - p->motorInternals.isa * p->motorInternals.rs));
	p->motorInternals.psb = p->motorInternals.psb + p->motorInternals.t2 * (p->motorInternals.dpsb + (p->motorInternals.usb - p->motorInternals.isb * p->motorInternals.rs));
	p->motorInternals.prd = p->motorInternals.prd + p->motorInternals.t2 * (p->motorInternals.dprd + (-p->motorInternals.ird * p->motorInternals.rr));
	p->motorInternals.prq = p->motorInternals.prq + p->motorInternals.t2 * (p->motorInternals.dprq + (-p->motorInternals.irq * p->motorInternals.rr));

	// Ïîâîðîò ïîòîêîñöåïëåíèé ðîòîðà â êîîðäèíàòû alpha,beta
	p->motorInternals.pra = p->motorInternals.prd * p->motorInternals.cosTetaR - p->motorInternals.prq * p->motorInternals.sinTetaR;
	p->motorInternals.prb = p->motorInternals.prd * p->motorInternals.sinTetaR + p->motorInternals.prq * p->motorInternals.cosTetaR;

	//åñëè èíâåðòîð âûêëþ÷åí è âñå òîêè ïåðåñåêëè íîëü (êîëåáëþòñÿ îêîëî íóëÿ, ðàñôîðñèðîâêà ïðîøëà)
	//òî îáíóëÿåì âñå ïîòîêè â äâèãàòåëå (÷èòåðñòâî, èíà÷å ñëîæíûé ðàñ÷åò áóäåò)
	if ((p->InvertorEna == 0) && (p->motorInternals.CurrentInvertedFlag == 7)) {
		p->motorInternals.psa = 0;
		p->motorInternals.psb = 0;
		p->motorInternals.prd = 0;
		p->motorInternals.prq = 0;
		p->motorInternals.pra = 0;
		p->motorInternals.prb = 0;
	}

	// Ðàñ÷åò òîêîâ ïîñëå óòî÷íåíèÿ èçìåíåíèÿ ïîòîêîñöåïëåíèé
	p->motorInternals.isa = p->motorInternals.ks * p->motorInternals.psa + p->motorInternals.km * p->motorInternals.pra;
	p->motorInternals.isb = p->motorInternals.ks * p->motorInternals.psb + p->motorInternals.km * p->motorInternals.prb;
	p->motorInternals.ira = p->motorInternals.km * p->motorInternals.psa + p->motorInternals.kr * p->motorInternals.pra;
	p->motorInternals.irb = p->motorInternals.km * p->motorInternals.psb + p->motorInternals.kr * p->motorInternals.prb;

	// Ïîâîðîò ðîòîðíûõ òîêîâ â d,q êîîðäèíàòû
	p->motorInternals.ird = p->motorInternals.ira * p->motorInternals.cosTetaR + p->motorInternals.irb * p->motorInternals.sinTetaR;
	p->motorInternals.irq = -p->motorInternals.ira * p->motorInternals.sinTetaR + p->motorInternals.irb * p->motorInternals.cosTetaR;

	// Ðàñ÷åò ìîìåíòà
	p->motorInternals.torque = 3.0 / 2.0 * p->motorInternals.pp * (p->motorInternals.psa * p->motorInternals.isb - p->motorInternals.psb * p->motorInternals.isa);

	// Ðàñ÷åò ìåõàíèêè
	MotorModel_MechModelExecute(p);

	//òîêè èç äâóõôàçíîé ñèñòåìû â òðåõôàçíóþ
	p->motorInternals.isPhaseA = p->motorInternals.isa;
	p->motorInternals.isPhaseB = -0.5 * p->motorInternals.isa + p->motorInternals.cos30 * p->motorInternals.isb;
	p->motorInternals.isPhaseC = -p->motorInternals.isa - p->motorInternals.isPhaseB;

	//ðàñ÷åò èçìåðÿåìûõ âåëè÷èí
	p->motorInternals.adcSpeedtemp = p->motorInternals.omega * p->motorInternals.speedK + ADC_HALF_VALUE;						//ñêîðîñòü (òàõîãåíåðàòîð)
	p->motorInternals.iAtemp = p->motorInternals.isa * p->motorInternals.iSenseK + ADC_HALF_VALUE;				//òîê ôàçû A
	p->motorInternals.iBtemp = p->motorInternals.isPhaseB * p->motorInternals.iSenseK + ADC_HALF_VALUE;			//òîê ôàçû B
	p->motorInternals.tetaRM = p->motorInternals.tetaRM + p->motorInternals.t * p->motorInternals.omega;	//ìåõàíè÷åñêîå ïîëîæåíèå ðîòîðà
	if (p->motorInternals.tetaRM > 2 * MOTOR_MODEL_PI)					//îãðàíè÷èâàåì 0..2Ïè
		p->motorInternals.tetaRM -= 2 * MOTOR_MODEL_PI;
	if (p->motorInternals.tetaRM < 0)
		p->motorInternals.tetaRM += 2 * MOTOR_MODEL_PI;
	p->motorInternals.tetaR=p->motorInternals.tetaRM*p->motorInternals.pp;
}

void Model_Execute_SyncMotor(TMotorModel *p) {
	MotorModel_calcU43Phase(p);	//ðàñ÷åò íàïðÿæåíèé

	// Ðàñ÷åò ïîòîêîñöåïëåíèÿ ðîòîðà
	if (p->motorInternals.syncm_pm == 0) // Åñëè ñ íåçàâèñèìîé ÎÂ
	{
		MotorModel_calcU4SyncMFieldWinding(p);	//ðàñ÷åò íàïðÿæåíèÿ ÎÂ
		p->motorInternals.dif = (p->motorInternals.u_f - p->motorInternals.i_f * p->motorInternals.r_f)*p->motorInternals._1_l_f;
		p->motorInternals.i_f = p->motorInternals.i_f + p->motorInternals.dif * p->motorInternals.t;
		if (p->motorInternals.i_f <= p->motorInternals.RatedFluxCurrent)		// Ó÷åò íàñûùåíèÿ
			p->motorInternals.m = p->motorInternals.i_f * p->motorInternals.l_m;
		else
			p->motorInternals.m = p->motorInternals.RatedFluxCurrent * p->motorInternals.l_m; // òóò ìîæíî ÷óòü èçìåíèòü, ââåäÿ íåáîëüøóþ íàäáàâêó.
	}	// Åñëè ñ ïîñòîÿííûìè ìàãíèòàìè, òî ïîòîê ðîòîðà ïðèñâîèëñÿ åùå â èíèòå è íå ìåíÿåòñÿ

	// Ðàñ÷åò ñèíóñà è êîñèíóñà óãëà ðîòîðà
	p->motorInternals.cosTetaR = _IQtoF(_IQcos(_IQ(p->motorInternals.tetaR)));//ñèíóñ è êîñèíóñ ñ÷èòàþòñÿ ñ ôèêñèðîâàííîé òî÷êîé, ò.ê. ôëîàòîâñêèå çàíèìàþò òûñÿ÷è òàêòîâ (âèäèìî, êàêàÿ-òî ìåäëåííàÿ ðåàëèçàöèÿ, íå èñïîëüçóþùàÿ àïïàðàòíóþ ïîääåðæêó).
	p->motorInternals.sinTetaR = _IQtoF(_IQsin(_IQ(p->motorInternals.tetaR)));//åñëè óäàñòñÿ íàéòè áûñòðûé ïëàâàþùèé ñèíóñ è êîñèíóñ, òî èõ ìîæíî ñ÷èòàòü âî ôëîàòå

	// Ïîâîðîò íàïðÿæåíèé èç îñåé àëüôà.áåòà â îñè d,q
	p->motorInternals.usd = p->motorInternals.usa * p->motorInternals.cosTetaR + p->motorInternals.usb * p->motorInternals.sinTetaR;
	p->motorInternals.usq = -p->motorInternals.usa * p->motorInternals.sinTetaR + p->motorInternals.usb * p->motorInternals.cosTetaR;

	// Ðàñ÷åò èçìåíåíèé ïîòîêîñöåïëåíèé (ïðåäèêòîðû)
	p->motorInternals.dpsd = (p->motorInternals.usd - p->motorInternals.isd * p->motorInternals.rs + p->motorInternals.psq * p->motorInternals.omega);
	p->motorInternals.dpsq = (p->motorInternals.usq - p->motorInternals.isq * p->motorInternals.rs - p->motorInternals.psd * p->motorInternals.omega);

	// Ðàñ÷åò ïðåäèêòîðîâ ïîòîêîñöåïëåíèé
	p->motorInternals.ppsd = p->motorInternals.psd + p->motorInternals.dpsd * p->motorInternals.t;
	p->motorInternals.ppsq = p->motorInternals.psq + p->motorInternals.dpsq * p->motorInternals.t;

	// Ðàñ÷åò òîêîâ äëÿ ïðåäèêòîðíîãî óðàâíåíèÿ
	p->motorInternals.isd = (p->motorInternals.ppsd - p->motorInternals.m) * p->motorInternals._1_lsd;
	p->motorInternals.isq = p->motorInternals.ppsq * p->motorInternals._1_lsq;

	// Ðàñ÷åò èçìåíåíèé ïîòîêîñöåïëåíèé ïî Ðóíãå-Êóòòà âòîðîãî ïîðÿäêà
	p->motorInternals.psd = p->motorInternals.psd + p->motorInternals.t2 * (p->motorInternals.dpsd + (p->motorInternals.usd - p->motorInternals.isd * p->motorInternals.rs + p->motorInternals.psq * p->motorInternals.omega));
	p->motorInternals.psq = p->motorInternals.psq + p->motorInternals.t2 * (p->motorInternals.dpsq + (p->motorInternals.usq - p->motorInternals.isq * p->motorInternals.rs - p->motorInternals.psd * p->motorInternals.omega));

	//åñëè èíâåðòîð âûêëþ÷åí è âñå òîêè ïåðåñåêëè íîëü (êîëåáëþòñÿ îêîëî íóëÿ, ðàñôîðñèðîâêà ïðîøëà)
	//òî âûêëþ÷àåì ðàñ÷åò òîêîâ ñòàòîðà, ÷òîáû íå äåðãàëèñü (÷èòåðñòâî, èíà÷å ñëîæíûé ðàñ÷åò áóäåò)
	if ((p->InvertorEna == 0) && (p->motorInternals.CurrentInvertedFlag == 7)) {
		p->motorInternals.psd = p->motorInternals.m;
		p->motorInternals.psq = 0;
	}

	// Ðàñ÷åò òîêîâ ïîñëå óòî÷íåíèÿ èçìåíåíèÿ ïîòîêîñöåïëåíèé
	p->motorInternals.isd = (p->motorInternals.psd - p->motorInternals.m) * p->motorInternals._1_lsd;
	p->motorInternals.isq = p->motorInternals.psq * p->motorInternals._1_lsq;

	//ïîâîðîò òîêîâ â îñè àëüôà,áåòà (äëÿ âûâîäà íà ÀÖÏ)
	p->motorInternals.isa = p->motorInternals.isd * p->motorInternals.cosTetaR - p->motorInternals.isq * p->motorInternals.sinTetaR;
	p->motorInternals.isb = +p->motorInternals.isd * p->motorInternals.sinTetaR + p->motorInternals.isq * p->motorInternals.cosTetaR;

	// Ðàñ÷åò ìîìåíòà
	p->motorInternals.torque = 3.0 / 2.0 * p->motorInternals.pp * (p->motorInternals.psd * p->motorInternals.isq - p->motorInternals.psq * p->motorInternals.isd);

	// Ðàñ÷åò ìåõàíèêè
	MotorModel_MechModelExecute(p);

	//òîêè èç äâóõôàçíîé ñèñòåìû â òðåõôàçíóþ
	p->motorInternals.isPhaseA = p->motorInternals.isa;
	p->motorInternals.isPhaseB = -0.5 * p->motorInternals.isa + p->motorInternals.cos30 * p->motorInternals.isb;
	p->motorInternals.isPhaseC = -p->motorInternals.isa - p->motorInternals.isPhaseB;

	p->motorInternals.isPhaseD = p->motorInternals.i_f;

	//ðàñ÷åò èçìåðÿåìûõ âåëè÷èí
	p->adc_code_Speed = p->motorInternals.omega * p->motorInternals.speedK + ADC_HALF_VALUE;						//ñêîðîñòü (òàõîãåíåðàòîð)
	p->motorInternals.iAtemp = p->motorInternals.isa * p->motorInternals.iSenseK + ADC_HALF_VALUE;				//òîê ôàçû A
	p->motorInternals.iBtemp = p->motorInternals.isPhaseB * p->motorInternals.iSenseK + ADC_HALF_VALUE;			//òîê ôàçû B
	p->motorInternals.iDtemp = p->motorInternals.i_f * p->motorInternals.ifSenseK + ADC_HALF_VALUE;					//òîê ÎÂ
	p->motorInternals.tetaRM = p->motorInternals.tetaRM + p->motorInternals.t * p->motorInternals.omega;	//ìåõàíè÷åñêîå ïîëîæåíèå ðîòîðà
	if (p->motorInternals.tetaRM > 2 * MOTOR_MODEL_PI)					//îãðàíè÷èâàåì 0..2Ïè
		p->motorInternals.tetaRM -= 2 * MOTOR_MODEL_PI;
	if (p->motorInternals.tetaRM < 0)
		p->motorInternals.tetaRM += 2 * MOTOR_MODEL_PI;

	p->motorInternals.tetaR=p->motorInternals.tetaRM*p->motorInternals.pp;
}

void Model_Execute_DCMotor(TMotorModel *p) {
	// Ïðåîáðàçîâàíèå íàïðÿæåíèé
	MotorModel_calcU42Phase(p);

	// !!! Âíèìàíèå! Ïðè ñîîòíîøåíèè ïàðàìåòðîâ Πñ ìàëîé èíäóêòèâíîñòü è áîëüøèì ñîïðîòèâëåíèåì ðàñ÷åò Πìîæåò âûïîëíÿòüñÿ íåêîððåêòíî
	// !!! Êðèòè÷åñêîå ñîîòíîøåíèå ïàðàìåòðîâ Lf=6.3 ìÃí è Rf=125 Îì

	// ïðåäâàðèòåëüíûé ðàñ÷åò ïðèðàùåíèé ïî òîêó
	if (p->motorInternals.dcm_pm == 0) // Åñëè ñ íåçàâèñèìîé ÎÂ
	{
		p->motorInternals.dif = (p->motorInternals.u_f - p->motorInternals.i_f * p->motorInternals.r_f)*p->motorInternals._1_l_f;
		p->motorInternals.i_f = p->motorInternals.i_f + p->motorInternals.dif * p->motorInternals.t;
		if (p->motorInternals.i_f <= p->motorInternals.RatedFluxCurrent)		// Ó÷åò íàñûùåíèÿ
			p->motorInternals.kf = p->motorInternals.i_f * p->motorInternals.l_m;
		else
			p->motorInternals.kf = p->motorInternals.RatedFluxCurrent * p->motorInternals.l_m; // òóò ìîæíî ÷óòü èçìåíèòü, ââåäÿ íåáîëüøóþ íàäáàâêó.
	}
	else // Åñëè ñ ïîñòîÿííûìè ìàãíèòàìè
		p->motorInternals.kf = p->motorInternals.l_m * 1.0;

	p->motorInternals.dia = (p->motorInternals.u_a - p->motorInternals.kf*p->motorInternals.omega - p->motorInternals.i_a*(p->motorInternals.r_a + p->motorInternals.r_ad))*p->motorInternals._1_l_a;
	p->motorInternals.i_a = p->motorInternals.i_a + p->motorInternals.dia * p->motorInternals.t;

	//åñëè èíâåðòîð âûêëþ÷åí è âñå òîêè ïåðåñåêëè íîëü (êîëåáëþòñÿ îêîëî íóëÿ, ðàñôîðñèðîâêà ïðîøëà)
	//òî âûêëþ÷àåì ðàñ÷åò òîêîâ ñòàòîðà, ÷òîáû íå äåðãàëèñü (÷èòåðñòâî, èíà÷å ñëîæíûé ðàñ÷åò áóäåò)
	if ((p->InvertorEna == 0) && (p->motorInternals.CurrentInvertedFlag == 7)) {
		p->motorInternals.i_a=0;
	}

	// Ìîìåíò
	p->motorInternals.torque = p->motorInternals.kf * p->motorInternals.i_a;

	// âû÷èñëåíèå ìåõàíè÷åñêèõ âåëè÷èí
	MotorModel_MechModelExecute(p);

	//omega+=0.5;
	p->motorInternals.tetaRM = p->motorInternals.tetaRM + p->motorInternals.t * p->motorInternals.omega;		//ïîëîæåíèå
	if (p->motorInternals.tetaRM > 2 * MOTOR_MODEL_PI)						//îãðàíè÷åíèå 0..2Ïè
		p->motorInternals.tetaRM -= 2 * MOTOR_MODEL_PI;
	if (p->motorInternals.tetaRM < 0)
		p->motorInternals.tetaRM += 2 * MOTOR_MODEL_PI;

	p->motorInternals.isPhaseA = p->motorInternals.i_a;
	p->motorInternals.isPhaseB = p->motorInternals.isPhaseA;
	p->motorInternals.isPhaseC = p->motorInternals.i_f;

	//ðàñ÷åò èçìåðÿåìûõ âåëè÷èí
	p->motorInternals.adcSpeedtemp = p->motorInternals.omega * p->motorInternals.speedK + ADC_HALF_VALUE;			//ñêîðîñòü (òàõîãåíåðàòîð)
	p->motorInternals.iAtemp = p->motorInternals.i_a * p->motorInternals.iSenseK + ADC_HALF_VALUE;					//òîê ÿêîðÿ
	p->motorInternals.iCtemp = p->motorInternals.i_f * p->motorInternals.ifSenseK + ADC_HALF_VALUE;					//òîê ÎÂ
}


void Model_Execute_SRDMotor(TMotorModel *p) {
	p->motorInternals.ua=p->motorInternals.udc*(p->motorInternals.cmpr0*p->motorInternals.inv_tpr*2-1);	//ôàçíîå íàïðÿæåíèå
	p->motorInternals.psa+=(p->motorInternals.ua - p->motorInternals.isPhaseA*p->motorInternals.rs)*p->motorInternals.t;					//èíòåãðèðóåì ïîòîêîñöåïëåíèå
	if (p->motorInternals.psa<0)
		p->motorInternals.psa=0;
	p->motorInternals.isPhaseA=MotorModel_PhaseCurrentInterpolation(p,p->motorInternals.tetaR, p->motorInternals.psa);			//èùåì òîê ôàçû
	p->motorInternals.torque_a = MotorModel_PhaseTorqueInterpolation(p,p->motorInternals.tetaR, p->motorInternals.isPhaseA);		//èùåì ìîìåíò ôàçû
	//DebugW2 = _IQ(p->motorInternals.torque_a);

	p->motorInternals.ub=p->motorInternals.udc*(p->motorInternals.cmpr1*p->motorInternals.inv_tpr*2-1);	//ôàçíîå íàïðÿæåíèå
	p->motorInternals.psb+=(p->motorInternals.ub - p->motorInternals.isPhaseB*p->motorInternals.rs)*p->motorInternals.t;					//èíòåãðèðóåì ïîòîêîñöåïëåíèå
	if (p->motorInternals.psb<0)
		p->motorInternals.psb=0;
	float TetaB = p->motorInternals.tetaR+MOTOR_MODEL_PI*1.33333;				//ôàçîâûé ñäâèã
	if (TetaB > 2*MOTOR_MODEL_PI)
		TetaB -= 2*MOTOR_MODEL_PI;
	p->motorInternals.isPhaseB=MotorModel_PhaseCurrentInterpolation(p,TetaB, p->motorInternals.psb);					//èùåì òîê ôàçû
	p->motorInternals.torque_b = MotorModel_PhaseTorqueInterpolation(p,TetaB, p->motorInternals.isPhaseB);			//èùåì ìîìåíò ôàçû
	//DebugW3 = _IQ(p->motorInternals.torque_b);

	p->motorInternals.uc=p->motorInternals.udc*(p->motorInternals.cmpr2*p->motorInternals.inv_tpr*2-1);	//ôàçíîå íàïðÿæåíèå
	p->motorInternals.psc+=(p->motorInternals.uc - p->motorInternals.isPhaseC*p->motorInternals.rs)*p->motorInternals.t;					//èíòåãðèðóåì ïîòîêîñöåïëåíèå
	if (p->motorInternals.psc<0)
		p->motorInternals.psc=0;
	float TetaC = p->motorInternals.tetaR+MOTOR_MODEL_PI*0.66667;				//ôàçîâûé ñäâèã
	if (TetaC > 2*MOTOR_MODEL_PI)
		TetaC -= 2*MOTOR_MODEL_PI;
	p->motorInternals.isPhaseC=MotorModel_PhaseCurrentInterpolation(p,TetaC, p->motorInternals.psc);					//èùåì òîê ôàçû
	p->motorInternals.torque_c = MotorModel_PhaseTorqueInterpolation(p,TetaC, p->motorInternals.isPhaseC);			//èùåì ìîìåíò ôàçû
	//DebugW4 = _IQ(p->motorInternals.torque_c);

	//Ñóììàðíûé ìîìåíò âñåõ ôàç (ôàçû - íåçàâèñèìûå)
	p->motorInternals.torque = p->motorInternals.torque_a + p->motorInternals.torque_b + p->motorInternals.torque_c;

	// Ðàñ÷åò ìåõàíèêè
	MotorModel_MechModelExecute(p);

	p->motorInternals.tetaRM+=p->motorInternals.t*p->motorInternals.omega;
	//p->motorInternals.tetaRM = 0.393;	//óãîë ñ ìàêñèìàëüíûì ìîìåíòîì äëÿ ôàçû A, äëÿ òåñòà
	if (p->motorInternals.tetaRM>2*MOTOR_MODEL_PI)
		p->motorInternals.tetaRM-=2*MOTOR_MODEL_PI;
	if (p->motorInternals.tetaRM<0)
		p->motorInternals.tetaRM+=2*MOTOR_MODEL_PI;

	p->motorInternals.tetaR=p->motorInternals.tetaRM*p->motorInternals.pp;
	while (p->motorInternals.tetaR>2*MOTOR_MODEL_PI)
		p->motorInternals.tetaR-=2*MOTOR_MODEL_PI;

	p->adc_code_Speed=p->motorInternals.omega*p->motorInternals.speedK+ADC_HALF_VALUE;
	p->motorInternals.iAtemp=p->motorInternals.isPhaseA*p->motorInternals.iSenseK;
	p->motorInternals.iBtemp=p->motorInternals.isPhaseB*p->motorInternals.iSenseK;
	p->motorInternals.iCtemp=p->motorInternals.isPhaseC*p->motorInternals.iSenseK;


}

//îñíîâíàÿ ôóíêöèÿ ðàñ÷åòà ìîäåëè, âûçûâàåòñÿ â ðïåðûâàíèè 10 êÃö
void MotorModel_Execute(TMotorModel *p) {

	p->motorInternals.cmpr0 = p->motorInternals.tpr-p->cmpr0;	//óñòàâêè ñðàâíåíèÿ ØÈÌ. Ñ èíâåðñèåé, ïîòîìó ÷òî òàê íàñòðîåí ìîäóëü ØÈÌ
	p->motorInternals.cmpr1 = p->motorInternals.tpr-p->cmpr1;
	p->motorInternals.cmpr2 = p->motorInternals.tpr-p->cmpr2;
	p->motorInternals.cmpr3 = p->motorInternals.tpr-p->cmpr3;

	if (p->fault == 0)		//åñëè íåò îøèáêè ìîäåëè, ñ÷èòàåì
	{
		if (p->motorInternals.type == MODEL_INDUCTION_MOTOR) {
			Model_Execute_InductionMotor(p);

		} else if (p->motorInternals.type == MODEL_SYNC_MOTOR) {
			Model_Execute_SyncMotor(p);

		} else if (p->motorInternals.type == MODEL_DC_MOTOR) {
			Model_Execute_DCMotor(p);

		}
		else if (p->motorInternals.type==MODEL_SRD_MOTOR){
			Model_Execute_SRDMotor(p);
		}
	} else {//ìîäåëü â àâàðèè
		p->motorInternals.iAtemp=ADC_HALF_VALUE;
		p->motorInternals.iBtemp=ADC_HALF_VALUE;
		p->motorInternals.iCtemp=ADC_HALF_VALUE;
		p->motorInternals.iDtemp=ADC_HALF_VALUE;

	}

	//Èìèòàöèÿ ñèãíàëà ñ äàò÷èêà Õîëëà (3 ýëåìåíòà) â çàâèñèìîñòè îò òåêóùåãî ýëåêòðè÷åñêîãî ïîëîæåíèÿ
	//ðàññ÷èòûâàåì íîìåð ýëåêòðè÷åñêîãî ñåêòîðà, ãäå ñåé÷àñ ðîòîð èç ýëåêòðè÷åñêîãî óãëà
	Uint16 TempSector=p->motorInternals.tetaR * (1/(2 * MOTOR_MODEL_PI)*6);
	TempSector=TempSector%6;//óãîë íå îãðàíè÷åí, ïîýòîìó èñïîëüçóåì îñòàòîê îò äåëåëåíèÿ íà êîë-âî ñåêòîðîâ

	switch (TempSector){//â çàâèñèìîñòè îò íîìåðà ñåêòîðà ôîðìèðóåì ïîáèòîâûé êîä íà ýëåìåíòàõ Õîëëà
		case 0:{
			p->motorInternals.hallSensor=5;//101
			break;
		}
		case 1:{
			p->motorInternals.hallSensor=4;//100
			break;
		}
		case 2:{
			p->motorInternals.hallSensor=6;//110
			break;
		}
		case 3:{
			p->motorInternals.hallSensor=2;//010
			break;
		}
		case 4:{
			p->motorInternals.hallSensor=3;//011
			break;
		}
		case 5:{
			p->motorInternals.hallSensor=1;//001
			break;
		}

	}

	if (p->motorInternals.hallSensor!=p->motorInternals.hallSensorPrev){//Ïðèøëà íîâàÿ ìåòêà ÄÏÐ Õîëëà
		p->hallSensorOmega=p->motorInternals.hallSensorOmegaPrev;//Íà âûõîä ìîäåëè - ñêîðîñòü, êîòîðàÿ áûëà ìåæäó ïðåäûäóùèìè ìåòêàìè ÄÏÐ Õîëëà
		p->motorInternals.hallSensorOmegaPrev=p->motorInternals.omega;//Çàïîìèíàåì òåêóùóþ ñêîðîñòü, ÷òîáû îòäàòü å¸ ïîòîì
		p->hallSensorInterpAdd=0;
		p->motorInternals.hallSensorPrev=p->motorInternals.hallSensor;
		p->hallSensor=p->motorInternals.hallSensor;
	}

	if (fabs(p->motorInternals.omega)<1){//åñëè äâèãàòåëü ïî÷òè ñòîèò
		p->hallSensorOmega=0;
		p->motorInternals.hallSensorOmegaPrev=0;
		p->hallSensorInterpAdd=0;
	}
	if (p->hallSensorInterpAdd < (MOTOR_MODEL_PI/3))	//îãðàíè÷èâàåì øåñòîé ÷àñòüþ óãëà
	p->hallSensorInterpAdd = p->hallSensorInterpAdd + p->motorInternals.t * p->hallSensorOmega *p->motorInternals.pp;	//ìåõàíè÷åñêîå ïîëîæåíèå ðîòîðà

	p->motorInternals.power=p->motorInternals.omega*p->motorInternals.torque*0.001;//Ìîùíîñòü íà âàëó â êÂò
	p->qepCounter=p->motorInternals.tetaRM*MODEL_INV_2PI*p->motorInternals.QEPResolution*4;//Èìèòàöèÿ ñ÷åò÷èêà èíêðåìåíòàëüíîãî ýíêîäåðà èç òåêóùåãî óãëà äâèãàòåëÿ
	p->motorInternals.omega_rpm = p->motorInternals.omega*MODEL_INV_2PI*60;//×àñòîòà âðàùåíèÿ âàëà èç ðàä/ñ â îá/ìèí
	p->motorInternals.Udc_temp=p->motorInternals.udc*p->motorInternals.udSenseK;//Èìèòàöèÿ èçìåðåíèÿ íàïðÿæåíèÿ ÇÏÒ


	//íàëîæåíèå øóìà ÀÖÏ
	p->motorInternals.iAtemp += MotorModel_fastrand();
	p->motorInternals.iBtemp += MotorModel_fastrand();
	p->motorInternals.iCtemp += MotorModel_fastrand();
	p->motorInternals.iDtemp += MotorModel_fastrand();
	p->motorInternals.adcSpeedtemp += MotorModel_fastrand();
	p->motorInternals.Udc_temp += MotorModel_fastrand();

	//íàñûùåíèå êàíàëîâ ÀÖÏ
	if (p->motorInternals.iAtemp > ADC_MAX_VALUE)
		p->motorInternals.iAtemp = ADC_MAX_VALUE;
	if (p->motorInternals.iAtemp < 0)
		p->motorInternals.iAtemp = 0;
	if (p->motorInternals.iBtemp > ADC_MAX_VALUE)
		p->motorInternals.iBtemp = ADC_MAX_VALUE;
	if (p->motorInternals.iBtemp < 0)
		p->motorInternals.iBtemp = 0;
	if (p->motorInternals.iCtemp > ADC_MAX_VALUE)
		p->motorInternals.iCtemp = ADC_MAX_VALUE;
	if (p->motorInternals.iCtemp < 0)
		p->motorInternals.iCtemp = 0;
	if (p->motorInternals.iDtemp > ADC_MAX_VALUE)
		p->motorInternals.iDtemp = ADC_MAX_VALUE;
	if (p->motorInternals.iDtemp < 0)
		p->motorInternals.iDtemp = 0;
	if (p->motorInternals.adcSpeedtemp > ADC_MAX_VALUE)
		p->motorInternals.adcSpeedtemp = ADC_MAX_VALUE;
	if (p->motorInternals.adcSpeedtemp < 0)
		p->motorInternals.adcSpeedtemp = 0;
	if (p->motorInternals.Udc_temp > ADC_MAX_VALUE)
		p->motorInternals.Udc_temp = ADC_MAX_VALUE;
	if (p->motorInternals.Udc_temp < 0)
		p->motorInternals.Udc_temp = 0;

	p->adc_code_iA = p->motorInternals.iAtemp;
	p->adc_code_iB = p->motorInternals.iBtemp;
	p->adc_code_iC = p->motorInternals.iCtemp;
	p->adc_code_iD = p->motorInternals.iDtemp;
	p->adc_code_Speed = p->motorInternals.adcSpeedtemp;
	p->adc_code_Udc =  p->motorInternals.Udc_temp;

}


/*@}*/