130 lines
4.8 KiB
C
130 lines
4.8 KiB
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"
|
|
|
|
//! Èíèöèàëèçàöèÿ
|
|
|
|
#define SSI_NT_SPI SPI0 //Íàñòðîéêà íîìåðà SPI
|
|
|
|
//! \memberof TSSI_Encoder
|
|
void SSI_Encoder_init(TSSI_Encoder *p) {
|
|
volatile long delay;
|
|
volatile Uint32 tempREG;
|
|
|
|
// Íàñòðîéêà ñàìîãî ìîäóëÿ SPI
|
|
// Ïðîäîëæåíèå íàñòðîéêè òàêòèðîâàíèÿ.
|
|
// Ïîëó÷åííàÿ ðàíåå ÷àñòîòà f_SSP_IN ïðîõîäèò åù¸ ÷åðåç äâà äåëèòåëÿ.
|
|
// SSPCPSR - ïåðâûé äåëèòåëü, â äèàïàçîíå 2 ... 254, ìîæåò áûòü òîëüêî ÷¸òíûì (ìëàäøèé áèò âñåãäà õàðäâàðíî ðàâåí 0)
|
|
// SSPCR0.bit.SCR - âòîðîé äåëèòåëü îò 0 äî 255.
|
|
// Áèòðåéò â èòîãå BitRate = f_SSP_IN / ( SSPCPSR * (SCR + 1) )
|
|
// Òàêòèðîâàíèå 25 ÌÃö çàäàíî â system_K1921VK028.c
|
|
|
|
SSI_NT_SPI->CR1 = 0;// Ðåæèì - ìàñòåð, LoopBack îòêëþ÷¸í, ñàì ìîäóëü SSP òîæå îòêëþ÷¸í
|
|
SSI_NT_SPI->IMSC = 0x0; // Çàïðåòèòü âñå ïðåðûâàíèÿ
|
|
SSI_NT_SPI->DMACR = 0; // Çàïðåòèòü DMA
|
|
SSI_NT_SPI->ICR = 0x3; // Î÷èñòêà ïðåðûâàíèé ("ïåðåïîëíåíèå FIFO ïðè¸ìà" è "íåîáñëóæåííîå FIFO ïðè¸ìà")
|
|
|
|
SSI_NT_SPI->CPSR_bit.CPSDVSR = 4; // Äåëåíèå âõîäíîé ÷àñòîòû íà 4 -> 6,25 MHz
|
|
SSI_NT_SPI->CR0_bit.SCR = 0x3F; // Âòîðîé äåëèòåëü
|
|
SSI_NT_SPI->CR0_bit.DSS = 12; // Ðàçìåð äàííûõ - 12 áèò
|
|
SSI_NT_SPI->CR0_bit.FRF = 0x0; // Âûáîð ïðîòîêîëà: 0 = SPI
|
|
SSI_NT_SPI->CR0_bit.SPO = 0x0; // Ïðè íåàêòèâíîé ëèíèè äåðæàòü êëîê â "0"
|
|
SSI_NT_SPI->CR0_bit.SPH = 0x0; // ×èòàòü äàííûå ïî çàäíåìó ôðîíòó êëîêà
|
|
SSI_NT_SPI->CR1_bit.SSE = 1; // Ðàçðåøèòü ðàáîòó
|
|
|
|
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 (SSI_NT_SPI->SR_bit.BSY == 0){
|
|
//SPI ñâîáîäåí
|
|
Data_read = SSI_NT_SPI->DR; //êîä ñ äàò÷èêà (÷èñëî îò 0 äî resol)
|
|
SSI_NT_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;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*@}*/
|
|
|