165 lines
6.9 KiB
C
165 lines
6.9 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"
|
|
|
|
//! Èíèöèàëèçàöèÿ
|
|
|
|
//! \memberof TSSI_Encoder
|
|
void SSI_Encoder_init(TSSI_Encoder *p) {
|
|
// volatile long delay;
|
|
// volatile Uint32 tempREG;
|
|
//
|
|
// //SPI-SOMI - íà VectorCard íîãà 88, íà ïðîöå B15, SPI_RXD2
|
|
// //SPI-SIMO - íà VectorCard íîãà 38, íà ïðîöå C6, SPI_TXD2
|
|
// //SPI-CLK - íà VectorCard íîãà 39, íà ïðîöå B14, SPI_CLK2
|
|
// //SPI-STE - íà VectorCard íîãà 89, íà ïðîöå B13, SPI_FSS2
|
|
// //Ïëàòà texas DRV8301-HC-EVM ðàçâåäåíà òàê, ÷òî ìèêðîêîíòðîëëåð - ýòî âåäîìîå óñòðîéñòâî.
|
|
// //Íóæíî ñäåëàòü åãî ìàñòåðîì. Ïîäðîáíåå ÷èòàéòå êîììåíòàðèé â çàãîëîâî÷íîì ôàéëå.
|
|
//
|
|
// // Íàñòðîéêà íîã SPI
|
|
// GPIOB->ALTFUNCSET = (1 << 13);//÷èï-ñåëåêò
|
|
// GPIOB->ALTFUNCSET = (1 << 14) | (1 << 15);
|
|
// NT_GPIOC->ALTFUNCSET = (1 << 6);//SIMO
|
|
// NT_COMMON_REG->GPIOPCTLB_bit.PIN13 = 2;//÷èï-ñåëåêò
|
|
// NT_COMMON_REG->GPIOPCTLB_bit.PIN14 = 2;
|
|
// NT_COMMON_REG->GPIOPCTLB_bit.PIN15 = 2;
|
|
// NT_COMMON_REG->GPIOPCTLC_bit.PIN6 = 2;//SIMO
|
|
//
|
|
//
|
|
// // Íàñòðîéêà òàêòèðîâàíèÿ SSP ìîäóëÿ
|
|
// // Âñåãî 4 ìîäóëÿ, íà êàæäûé ìîäóëü ïî 8 áèò èç ðåãèñòðà SSP_CLK_CTRL è ïî äâà áèòà èç UART_SSP_CLK_SEL
|
|
//
|
|
// // UART_SSP_CLK_SEL
|
|
// // Âî âòîðîì áàéòå ñëîâà íà êàæäûé èç 4-¸õ ìîäóëåé SSP îòâîäèòñÿ ïî äâà áèòà äëÿ âûáîðà èñòî÷íèêà òàêòèðîâàíèÿ 0x0000XX00
|
|
// // "00" - â êà÷åñòâå èòî÷íèêà òàêòèðîâàíèÿ ìîäóëÿ SSP âûáèðàåòñÿ ñèñòåìíàÿ ÷àñòîòà 100 ÌÃö, òàêèì îáðàçîì f_SSP_IN = SysClk = 100 MHz
|
|
//
|
|
// // SSP_CLK_CTRL
|
|
// // Ìëàäøèé áèò ðàçðåøàåò òàêòèðîâàíèå (1 - ðàçðåøèòü)
|
|
// // Âòîðîé áèò ðàçðåøàåò äåëåíèå ÷àñòîòû f_SSP_IN èñòî÷íèêà òàêòîâîãî ñèãíàëà (0 - íå äåëèòü, 1 - äåëèòü)
|
|
// // Ñòàðøèå øåñòü âûáèðàþò äåëèòåëü ÷àñòîòû ýòîãî èñîò÷íèêà:
|
|
// // X - äåëåíèå ïî ôîðìóëå SSPclk = f_SSP_IN /( 2 * (X +1) )
|
|
// // Èç äîêóìåíòàöèè íà áëîê SSP - ìèíèìàëüíàÿ ÷àñòîòà äëÿ ðàáîòû ìîäóëÿ â ðåæèìàõ è ìàñòåð è ñëåéâ,
|
|
// // f_SSP_IN > 22.12 MHz, ïîýòîìó 25 ÌÃö ñäåëàåì
|
|
// /* çàêîììåí÷åíû, òàê êàê òîæå ñàìîå äåëàåòñÿ â DRV8301_SPI
|
|
// tempREG = NT_COMMON_REG->UART_SPI_CLK_SEL;// ×åðåç tempREG, ÷òîáû íå çàäåòü äðóãèå áèòû, îòâå÷àþùèå çà òàêòèðîâàíèå UART'a
|
|
// tempREG &= 0xFFFF00FF;
|
|
// NT_COMMON_REG->UART_SPI_CLK_SEL = tempREG;
|
|
// NT_COMMON_REG->SPI_CLK = 0x07070707;// Ðàçðåøèòü òàêòèðîâàíèå, ðàçðåøèòü äåëåíèå ÷àñòîòû, äåëèòü íà 4 - 25 ÌÃö
|
|
// */
|
|
//
|
|
//
|
|
// // Íàñòðîéêà ñàìîãî ìîäóëÿ SPI
|
|
// // Ïðîäîëæåíèå íàñòðîéêè òàêòèðîâàíèÿ.
|
|
// // Ïîëó÷åííàÿ ðàíåå ÷àñòîòà f_SSP_IN ïðîõîäèò åù¸ ÷åðåç äâà äåëèòåëÿ.
|
|
// // SSPCPSR - ïåðâûé äåëèòåëü, â äèàïàçîíå 2 ... 254, ìîæåò áûòü òîëüêî ÷¸òíûì (ìëàäøèé áèò âñåãäà õàðäâàðíî ðàâåí 0)
|
|
// // SSPCR0.bit.SCR - âòîðîé äåëèòåëü îò 0 äî 255.
|
|
// // Áèòðåéò â èòîãå BitRate = f_SSP_IN / ( SSPCPSR * (SCR + 1) )
|
|
//
|
|
// SPI2->SPI_CR1 = 0;// Ðåæèì - ìàñòåð, LoopBack îòêëþ÷¸í, ñàì ìîäóëü SSP òîæå îòêëþ÷¸í
|
|
// SPI2->SPI_IMSC = 0x0; // Çàïðåòèòü âñå ïðåðûâàíèÿ
|
|
// SPI2->SPI_DMACR = 0; // Çàïðåòèòü DMA
|
|
// SPI2->SPI_ICR = 0x3; // Î÷èñòêà ïðåðûâàíèé ("ïåðåïîëíåíèå FIFO ïðè¸ìà" è "íåîáñëóæåííîå FIFO ïðè¸ìà")
|
|
//
|
|
// SPI2->SPI_CPSR = 4; // Äåëåíèå âõîäíîé ÷àñòîòû íà 4 -> 6,25 MHz
|
|
// SPI2->SPI_CR0_bit.DSS = 12; // Ðàçìåð äàííûõ - 12 áèò
|
|
// SPI2->SPI_CR0_bit.SCR = 0x3F; // Âòîðîé äåëèòåëü
|
|
// SPI2->SPI_CR0_bit.FRF = 0x0; // Êàêàÿ-òî "ôàçà" äëÿ ïðîòîêîëà Motorola SPI
|
|
// SPI2->SPI_CR0_bit.SPH = 0x1; // Êàêàÿ-òî "ïîëÿðíîñòü" äëÿ ïðîòîêîëà Motorola SPI
|
|
// SPI2->SPI_CR0_bit.SPO = 0x0; // Âûáîð ôîðìàòà êàäðà Motorola/TI/Microwire. "0" - ïî ïðîòîêîëó Motorola SPI
|
|
// SPI2->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 (SPI2->SPI_SR_bit.BSY == 0){//SPI ñâîáîäåí
|
|
// Data_read = SPI2->SPI_DR;//êîä ñ äàò÷èêà (÷èñëî îò 0 äî resol)
|
|
// SPI2->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;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*@}*/
|
|
|