motorcontroldemo_035/Vsrc/V_SSI_Encoder.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_SSI_Encoder.c
 \brief     Ìîäóëü îöåíêè ñêîðîñòè è ïîëîæåíèÿ ïðè ïîìîùè öèôðîâîãî ýíêîäåðà, ðàáîòàþùåãî ïî èíòåðôåéñó SSI (ñì. TSSI_Encoder)
 \author    ÎÎÎ "ÍÏÔ Âåêòîð". http://motorcontrol.ru
 \version   v 1.0 25/04/2016

 \addtogroup V_QEP
 @{*/

#include "DSP.h"
#include "V_IQmath.h"
#include "V_SSI_Encoder.h"
#include "math.h"
#include "stdlib.h"
#include "main.h"

//! Èíèöèàëèçàöèÿ

//! \memberof TSSI_Encoder
void SSI_Encoder_init(TSSI_Encoder *p) {
	volatile long delay;
	volatile Uint32 tempREG;

	// Íàñòðîéêà íîæåê ïîä SPI
	GPIOB->ALTFUNCSET = (1 << 5) | (1 << 6); // Òàêòèðîâàíèå (SPI_SCK)

	// Íàñòðîéêà ìîäóëÿ SPI
	// Òàêòèðîâàíèå ìîäóëÿ SPI â ôàéëå "src/system_K1921VK035.c" íàñòðîåíî íà fIN = 25 ÌÃö
	// Ýòà ÷àñòîòà ïðîõîäèò åù¸ ÷åðåç äâà äåëèòåëÿ.
	// SSPCPSR - ïåðâûé äåëèòåëü, â äèàïàçîíå 2 ... 254, ìîæåò áûòü òîëüêî ÷¸òíûì (ìëàäøèé áèò âñåãäà àïïàðàòíî ðàâåí 0)
	// SSPCR0.bit.SCR - âòîðîé äåëèòåëü îò 0 äî 255.
	// Áèòðåéò â èòîãå BitRate = fIN / ( CPSR * (SCR + 1) )

	SPI->IMSC = 0x0;	// Çàïðåòèòü âñå ïðåðûâàíèÿ
	SPI->DMACR = 0;		// Çàïðåòèòü DMA
	SPI->ICR = 0x3;		// Ñáðîñ ïðåðûâàíèé íà âñÿêèé ñëó÷àé ("ïåðåïîëíåíèå FIFO ïðè¸ìà" è "íåîáñëóæåííîå FIFO ïðè¸ìà")

	SPI->CPSR = 4;				// Äåëåíèå âõîäíîé ÷àñòîòû íà 4



	//  CR0 íàñòðàèâàåòñÿ âòîðîé äåëèòåëü ÷àñòîòû, ðàçðåøåíèå è ðåæèì ðàáîòû ýíêîäåðà.
	//
	// Âòîðîé äåëèòåëü = 62, èòîãî f_ssp = 25 ÌÃö / (4 * (62 + 1)) = 99,2 êÃö
	//
	// Íàñòðîéêà ôàçû è ïîëÿðíîñòè äëÿ ðàáîòû â ðåæèìå SPI
	// Ïî îïèñàíèþ íà äàò÷èê äåëà òàêèå: ïî ïåðâîìó ïàäàþùåìó ôðîíòó îí çàãðóæàåò â ñâîé
	// âíóòðåííèé ðåãèñòð íîâûå äàííûå, à ñ êàæäûì ðàñòóùèì ôðîíòîâ âûâîäèò èõ íà ëèíèþ.
	// Ñòàëî áûòü äàííûå íà ëèíèè ìåíÿþòñÿ íà ðàñòóùåì ôðîíòå, à çíà÷èò ñ÷èòûâàòü èõ íàäî
	// íà ïàäàþùåì. Íî ïðè ýòîì ïî ñàìîìó ïåðâîìó ôðîíòó íè÷åãî âû÷èòûâàòü íå íàäî, à çíà÷èò
	// ñòàðøèé áèò ïðèíÿòûõ äàííûõ íàäî îáíóëÿòü.
	// Êðîìå òîãî íà ÎÈ êëîê èíâåðòèðóåòñÿ, è ýòî íàäî ó÷èòûâàòü â íàñòðîéêå.
	//
	// Ðàçìåð êàäðà íà 1 áîëüøå, ÷åì ÷èñëî â ðåãèòñðå, ñòàëî áûòü (12 + 1) = 13 áèò, èç êîòîðûõ ïåðâûé âûêèíåì
	//
	// Â öåëÿõ ýêîíîìèè ìåñòà, çàïèñü îäíîé ñòðîêîé
	//SPI->CR0_bit.FRF = 0x0;		// SSI ðàáîòàåò â ðåæèìå SPI, ÷òî ïîçâîëÿåò çàäàòü, ïî êàêîìó ôðîíòó ÷èòàòü äàííûå
	//SPI->CR0_bit.SPO = 0x0;		//  ðåæèìå îæèäàíèÿ CLK â íóëå (ïîñëå èíâåðñèè ïîëó÷èòñÿ â åäèíèöå)
	//SPI->CR0_bit.SPH = 0x0;		// Âûáîðêà äàííûõ ïî ðàñòóùåìó ôðîíòó ñèíõðîñèãíàëà (ïîñëå èíâåðñèè ïîëó÷èòñÿ ïî ïàäàþùåìó)
	//SPI->CR0_bit.SCR = 62;		// Âòîðîé äåëèòåëü = 62 + 1 = 63
	//SPI->CR0_bit.DSS = SSI_ENC_RESOLUTION; // Ðàçìåð êàäðà 12 + 1 = 13 áèò
	SPI->CR0 = (SSI_ENC_RESOLUTION << SPI_CR0_DSS_Pos) | (62 << SPI_CR0_SCR_Pos);

	// Â ýòèõ êîììåíòàðèÿõ ðàñïèñàíî ïî êàæäîìó ïîëþ, ÷òî êàê ïðèñâàèâàåòñÿ, íî
	// äëÿ ýêîíîìèè íà èíñòðóêöèÿõ çàïèñûâàåòñÿ îäíèì äåéñòâèåì
	//SPI->CR1_bit.MS = 0;		// Ðåæèì - ìàñòåð
	//SPI->CR1_bit.RXIFLSEL = 0;	// Ãëóáèíà FIFO íà ïðè¸ì = 0 (FIFO íå èñïîëüçóåòñÿ)
	//SPI->CR1_bit.TXIFLSEL = 0;	// Ãëóáèíà FIFO íà îòïðàâêó = 0 (FIFO íå èñïîëüçóåòñÿ)
	//SPI->CR1_bit.SSE = 1;		// Ðàçðåøèòü ðàáîòó SPI
	SPI->CR1 = (1 << SPI_CR1_SSE_Pos);
	p->resol_inv = 1.0 / ((float) p->resol);

	p->read(p);
}


//! Ôóíêöèÿ ðàñ÷¸òà ñêîðîñòè è ïîëîæåíèÿ, âûçûâàåòñÿ ñ íåîáõîäèìîé äèñêðåòíîñòüþ

//! \memberof TSSI_Encoder
void SSI_Encoder_Calc(TSSI_Encoder *p) {
	p->read(p);
}



void SSI_Encoder_Read(TSSI_Encoder*p) {
	_iq theta_elec_temp;
	Uint16 Data_read=0;
	_iq theta_mech_temp;

	if (SPI->SR_bit.BSY == 0){//SPI ñâîáîäåí
		Data_read = SPI->DR & SSI_ENC_DATA_MASK;//êîä ñ äàò÷èêà (÷èñëî îò 0 äî resol) ñ îáíóë¸ííûì ñòàðøèì ðàçðÿäîì
		SPI->DR = 0xff;//îòïðàâëÿåì ÷òî óãîäíî, ãëàâíîå, ÷òîáû òàêòèðîâàíèå øëî


		if (p->rotation_dir)//îáðàòíîå íàïðàâëåíèå âðàùåíèÿ
			Data_read=(p->resol-1)-Data_read;//ïåðèîä - òåêóùåå
		p->Poscnt_res=Data_read;
	}

	//ïåðåâîä óãëà â ìåòêàõ íà îáîðîòå â ìåõàíè÷åñêèé óãîë
	//Çäåñü ðàñ÷åò âî float - æåëàòåëüíî ïåðåäåëàòü â IQ
	p->theta_mech = _IQ((float )p->Poscnt_res * p->resol_inv); //ðàñ÷¸ò ìåõàíè÷åñêîãî óãëà
	p->theta_mech &= 0x00FFFFFF;
	//Ôèëüòð óãëà
	if (p->theta_mech_filterK!=0){
        p->theta_mech_filtered=p->theta_mech_filtered+_IQmpy(p->theta_mech_filterK,((p->theta_mech-p->theta_mech_filtered+_IQ(0.5))&0x00FFFFFF)-_IQ(0.5));
        p->theta_mech_filtered&=0x00FFFFFF;
	}else
		p->theta_mech_filtered=p->theta_mech;

    // Ïîäñ÷¸ò êîëè÷åñòâà ïîëíûõ îáîðîòîâ.
    if (p->prevThetaMech - p->theta_mech_filtered > _IQ(0.5))
		p->RevolutionCounter++;
	if (p->prevThetaMech - p->theta_mech_filtered < _IQ(-0.5))
		p->RevolutionCounter--;
	p->prevThetaMech=p->theta_mech_filtered;

	//óãîë â ìåòêàõ áåç îáíóëåíèÿ íà îáîðîòå, àáñîëþòíûé
	p->Poscnt_resContinouosLong=p->Poscnt_res+(p->resol*p->RevolutionCounter);
	p->Poscnt_resContinouosInt=p->Poscnt_resContinouosLong;//÷òîáû áûëî óäíîáíî ñìîòðåòü â 16òè ðàçðÿäíîì îñöèëëîãðàôå
	p->Poscnt_resContinouosInt8=p->Poscnt_resContinouosLong&0xF;//÷òîáû âèäåòü ìåòêè â êðóïíîì ìàñøòàáå

	//ïåðåâîä óãëà â ìåòêàõ àáñîëþòíûõ (íå îáíóëÿåìûõ íàîáîðîòå) â ìåõàíè÷åñêèé óãîë
	//íà 127 îáîðîòàõ âñ¸ ïåðåïîëíèòñÿ, íî äëÿ äåìî ñãîäèòñÿ
	p->theta_mechContinouos = p->theta_mech_filtered + _IQ(p->RevolutionCounter); //ðàñ÷¸ò ìåõàíè÷åñêîãî óãëà
	p->theta_elecContinouos = p->theta_mechContinouos*p->pole_pairs+ p->AngleOffset;//ýëåêòðè÷åñêèé óãîë àáñîëþòíûé (íå îáíóëÿåìûé)

	//Ðàñ÷¸ò ýëåêòðè÷åñêîãî ïîëîæåíèÿ îáíóëåìîãî ïî äîñòèæåíèþ 360 ãðàäóñîâ
	p->theta_el_tmp = p->theta_mech_filtered*p->pole_pairs + p->AngleOffset;
	p->theta_elec = p->theta_el_tmp & 0x00FFFFFF;

}



/*@}*/