motorcontroldemo_028/Vsrc/V_QEP.c
2019-07-29 08:18:57 +03:00

341 lines
13 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_QEP.c
\brief Ìîäóëü îöåíêè ñêîðîñòè è ïîëîæåíèÿ ïðè ïîìîùè eQEP (ñì. TposspeedEqep)
\author ÎÎÎ "ÍÏÔ Âåêòîð". http://motorcontrol.ru
\version v 2.0 25/03/2016
\addtogroup V_QEP
@{*/
#include "DSP.h"
#include "V_IQmath.h"
#include "V_QEP.h"
#include "math.h"
#include "stdlib.h"
#include "main.h"
#define FIRST_LAUNCH_UP 1//!< ïåðâûé ïóñê ïðè ïåðåõîäå ñ íèçêîé ñêîðîñòè íà áîëåå âûñîêóþ
#define NOT_FIRST_LAUNCH 0//!< íå ïåðâûé ïóñê
#define SCALE_IQ_K 10
#define LOWEST_UPSS 0
//Êàê ðàññ÷èòûâàòü ñêîðîñòü -
//Òàéìåðîâ QEP ìåæäó ìåòêàìè àïïàðàòíî
#define SPEED_CALC_TYPE_BY_QEP 0
//Ïðîãðàììíî ïî ïðîèçâîäíîé óãëà
#define SPEED_CALC_TYPE_BY_SOFT 1
//! Èíèöèàëèçàöèÿ
//! \memberof TposspeedEqep
void TposspeedEqep_init(TposspeedEqep *p) {
QEP0->QCAPCTL_bit.UPPS = LOWEST_UPSS;
QEP0->QCAPCTL_bit.CCPS = 4;
//Âêë. ðåæèì êâàäðàòóðíîãî ñ÷¸òà
QEP0->QDECCTL_bit.QSRC = 0;
//Ðåæèì îòëàä÷èêà
QEP0->QEPCTL_bit.FREESOFT = 0;
//Çíà÷åíèå ñ÷¸ò÷èêà ïîëîæåíèÿ ïîñëå èíèöèàëèçàöèè èëè ïîëîæèòåëüíîãî ïåðåïîëíåíèÿ
QEP0->QPOSINIT = 0;
//Ñáðîñ ñ÷¸ò÷èêà
QEP0->QEPCTL_bit.SWI = 1;
//Ìàêñèìàëüíîå çíà÷åíèå ñ÷¸ò÷èêà, ïîñëå ÷åãî îáíóëåíèå
QEP0->QPOSMAX = (p->resol << 2) - 1;
//Ïðîâåðêà íàïðàâëåíèÿ äâèæåíèÿ
if (p->Posspeed_CTL.bit.dir == 1) {
QEP0->QDECCTL_bit.QAP = 1;
}
//Âêë. êâàäðàòóðíîãî ñ÷¸ò÷èêà
QEP0->QEPCTL_bit.QPEN = 1;
//Îò÷èñòêà âñåõ ôëàãîâ
QEP0->QCLR = 0xFFFF;
//Ðåæèì çàõâàòà - ïî óìîë÷àíèþ, ïî ñîáûòèþ îòðàáîòêè çàäàííîãî ÷èñëà èìïóëüñîâ
QEP0->QEPCTL_bit.QCLM = 0;
//Ðåæèì îáíóëåíèÿ ñ÷¸ò÷èêà - ïî ðåïåðíîìó ñîáûòèþ
if (p->Posspeed_CTL.bit.index_en == 1) { //åñëè èíäåêñíàÿ ìåòêà åñòü
QEP0->QEPCTL_bit.PCRM = 0;
QEP0->QEINT_bit.IEL = 1;
} else {
//Ðåæèì îáíóëåíèÿ ñ÷¸ò÷èêà - ïî ìàêñèìàëüíîìó çíà÷åíèþ
QEP0->QEPCTL_bit.PCRM = 1;
}
//Ââîä ïåðèîäà ñòîðîæåâîãî òàéìåðà
QEP0->QWDPRD = 200000 / (1 << 2);
//Âêë. ñòîðîæåâîãî òàéìåðà
QEP0->QEPCTL_bit.WDE = 1;
/*Ðàñ÷¸ò âñïîìîãàòåëüíîãî êîýôôèöèåíòà äëÿ ðàñ÷¸òà ñêîðîñòè,
ãäå 15=60/4, 60 - äëÿ ïåðåâîäà â îá/ìèí; 4 - äëÿ ïîëó÷åíèÿ èíòåðïîëÿöèè ðàçðåøåíèÿ äàò÷èêà (1 ïåðèîä äàåò 4 ôðîíòà);
CORE_CLK - ÷àñòîòà ïðîöåññîðà*/
p->k_low = CORE_CLK / ((float) p->resol * p->speed_nom) * 15.0;
// iq ôîðìàòå óìåíüøàåì ÷èñëî â 1<<(24-SCALE_IQ_K), ãäå 24 - äðîáíàÿ ÷àñòü iq, à SCALE_IQ_K - òî, íàñêîëüêî ñäâèãàåòñÿ âåëè÷èíà
//ñ êîòîðîé speedK äàëüøå ó÷àñòâóåò â ôîðìóëå
p->speedK = _IQ(p->k_low / (1 << (24 - 10)));
p->resol_inv = 1.0 / ((float) p->resol);
if (p->Posspeed_CTL.bit.index_en){//åñëè èíäåêñíàÿ ìåòêà åñòü
// TODO ïåðåäåëàòü NT_COMMON_REG->GPIOPCTLG_bit.PIN12 = 1; //QEP1 I
}
}
//! Ôóíêöèÿ ðàñ÷¸òà ñêîðîñòè è ïîëîæåíèÿ, âûçûâàåòñÿ ñ íåîáõîäèìîé äèñêðåòíîñòüþ
//! \memberof TposspeedEqep
void TposspeedEqep_Calc(TposspeedEqep *p) {
long temp;
//Òåêóùåå çíà÷åíèå GPIO äëÿ îòëàäêè. Ìîæíî ñìîòðåòü â îñöàõ
p->GPIOsValue = (GPIOG->DATA >> 10) & 3;
//åñëè ñêîðîñòü âûñîêàÿ, à äåëèòåëü UPEVNT íå âûñîêèé
if ((labs(p->speed_elec) > _IQ(2)) && (QEP0->QCAPCTL_bit.UPPS != 5)) {
QEP0->QCAPCTL_bit.UPPS = 5; //äåëàåì äåëèòåëü UPEVNT âûñîêèé
p->skip_counter = 2; //äâà òàêòà ïðîïóñòèì ðàñ÷åò ñêîðîñòè
}
//åñëè ñêîðîñòü íèçêàÿ, à äåëèòåëü íå íèçêèé
if ((labs(p->speed_elec) < _IQ(1.5)) && (QEP0->QCAPCTL_bit.UPPS != LOWEST_UPSS)) {
QEP0->QCAPCTL_bit.UPPS = LOWEST_UPSS; //äåëàåì äåëèòåëü UPEVNT íèçêèé
p->skip_counter = 2; //äâà òàêòà ïðîïóñòèì ðàñ÷åò ñêîðîñòè
}
p->UPPS_forWatch = QEP0->QCAPCTL_bit.UPPS; //äëÿ îòîáðàæåíèÿ UPPS íàðóæó
if (p->Posspeed_CTL.bit.CmdInit == 1) { //Êîìàíäà ïåðåèíèöèàëèçàöèè - âñå îáíóëÿåì
QEP0->QEPCTL_bit.SWI = 1;
p->Posspeed_FLG1.bit.first_theta = 1;
p->Posspeed_FLG2.bit.pos_ident = 0;
p->Posspeed_CTL.bit.CmdInit = 0;
p->theta_elec = 0;
p->Poscnt_res = 0;
}
p->Poscnt_resPrev=p->Poscnt_res;
#if defined(HW_VECTORCARD_SIMULATOR) || defined(HW_NIIET_BOARD_SIMULATOR)
p->Poscnt_res = (unsigned long) model.qepCounter; //çàõâàò ïîëîæåíèÿ
#else
p->Poscnt_res = (unsigned long) QEP0->QPOSCNT; //çàõâàò ïîëîæåíèÿ
#endif
p->Poscnt_res16=p->Poscnt_res;//Äëÿ íàáëþäåíèÿ íà îñöèëëîãðàôå
p->Posspeed_FLG1.bit.PCO_tmp = QEP0->QFLG_bit.PCO; //êîïèÿ ôëàãà ïîëîæèòåëüíîãî ïåðåïîëíåíèÿ ñ÷¸ò÷èêà
p->Posspeed_FLG1.bit.PCU_tmp = QEP0->QFLG_bit.PCU; //êîïèÿ ôëàãà îòðèöàòåëüíîãî ïåðåïîëíåíèÿ ñ÷¸ò÷èêà
QEP0->QCLR_bit.PCO = 1; //îò÷èñòêà ôëàãà ïîëîæèòåëüíîãî ïåðåïîëíåíèÿ ñ÷¸ò÷èêà
QEP0->QCLR_bit.PCU = 1; //îò÷èñòêà ôëàãà îòðèöàòåëüíîãî ïåðåïîëíåíèÿ ñ÷¸ò÷èêà
//ïåðåâîä óãëà â ìåòêàõ íà îáîðîòå â ìåõàíè÷åñêèé óãîë
//Çäåñü ðàñ÷åò âî float - æåëàòåëüíî ïåðåäåëàòü â IQ
p->theta_mech = _IQ((float )p->Poscnt_res * p->resol_inv * 0.25); //ðàñ÷¸ò ìåõàíè÷åñêîãî óãëà
p->theta_mech &= 0x00FFFFFF;
// Ïîäñ÷¸ò êîëè÷åñòâà ïîëíûõ îáîðîòîâ. ×åðåç PCO è PCU ïîëó÷àåòñÿ êàê-òî ãëþ÷íî
if (p->prevThetaMech - p->theta_mech > _IQ(0.5))
p->RevolutionCounter++;
if (p->prevThetaMech - p->theta_mech < _IQ(-0.5))
p->RevolutionCounter--;
p->prevThetaMech=p->theta_mech;
//óãîë â ìåòêàõ áåç îáíóëåíèÿ íà îáîðîòå, àáñîëþòíûé
p->Poscnt_resContinouosLong=p->Poscnt_res+((QEP0->QPOSMAX+1)*p->RevolutionCounter);
p->Poscnt_resContinouosInt=p->Poscnt_resContinouosLong;//÷òîáû áûëî óäíîáíî ñìîòðåòü â 16òè ðàçðÿäíîì îñöèëëîãðàôå
p->Poscnt_resContinouosInt8=p->Poscnt_resContinouosLong&0xF;//÷òîáû âèäåòü ìåòêè â êðóïíîì ìàñøòàáå
//ïåðåâîä óãëà â ìåòêàõ àáñîëþòíûõ (íå îáíóëÿåìûõ íàîáîðîòå) â ìåõàíè÷åñêèé óãîë
//íà 127 îáîðîòàõ âñ¸ ïåðåïîëíèòñÿ, íî äëÿ äåìî ñãîäèòñÿ
p->theta_mechContinouos = _IQ((float )p->Poscnt_resContinouosLong * p->resol_inv * 0.25); //ðàñ÷¸ò ìåõàíè÷åñêîãî óãëà
p->theta_elecContinouos = p->theta_mechContinouos*p->pole_pairs+ p->AngleOffset;//ýëåêòðè÷åñêèé óãîë àáñîëþòíûé (íå îáíóëÿåìûé)
//Ðàñ÷¸ò ýëåêòðè÷åñêîãî ïîëîæåíèÿ îáíóëåìîãî ïî äîñòèæåíèþ 360 ãðàäóñîâ
p->theta_el_tmp = p->theta_mech*p->pole_pairs + p->AngleOffset;
p->theta_elec = p->theta_el_tmp & 0x00FFFFFF;
//ðàññ÷èòûâàòü ñêîðîñòü àïïàðàòíî, çàñåêàÿ âðåìÿ ìåæäó ìåòêàìè ñðåäñòâàìè QEP
if (p->SpeedCalcType==SPEED_CALC_TYPE_BY_QEP){
/*Ïðîâåðêà íà ñðàáàòûâàíèå ñòîðîæåâîãî òàéìåðà - íå ïðèøëî íå åäèíîãî èìïóëüñà*/
if (QEP0->QFLG_bit.WTO == 1) {
p->Posspeed_FLG1.bit.first_launch = FIRST_LAUNCH_UP; //óêàçàòåëü ïåðâîãî çàïóñêà àëãîðèòìà ïîñëå ñðàáàòûâàíèÿ òàéìåðà
p->speed_elec = 0; //îáíóëåíèå ñêîðîñòè
QEP0->QCLR_bit.WTO = 1; //îò÷èñòêà ôëàãà
QEP0->QWDTMR = 0; //îáíóëåíèå òàéìåðà
} else {
/*ïåðåêëþ÷åíèå ïî ïåðâîìó çàïóñêó äëÿ èíèöèàëèçàöèè*/
if (p->Posspeed_FLG1.bit.first_launch == FIRST_LAUNCH_UP) {
//Îòêë. áëîêà çàõâàòà
QEP0->QCAPCTL_bit.CEN = 0;
QEP0->QCAPCTL_bit.CEN = 1;
p->Posspeed_FLG1.bit.first_launch = NOT_FIRST_LAUNCH;
}
p->QEPSTS=QEP0->QEPSTS;//êîïèÿ ðåãèñòðà ñòàòóñà
if (p->QEPSTS_bit.UPEVNT == 1) { //åñòü ñîáûòèå UPEVNT, çíà÷èò â òàéìåðå çàõâà÷åíî âðåìÿ
p->Qcprdlat_tmp = QEP0->QCPRD; //êîïèÿ ñ÷åò÷èêà âðåìåíè
p->speed_calc_skip = 0; //ïðîïóñê ðàñ÷åòà ñêîðîñòè - íå ïðîïóñêàòü
if (p->QEPSTS_bit.COEF == 1) { //ïðîâåðêà ôëàãà ïåðåïîëíåíèÿ òàéìåðà
p->speed_elec = 0;
p->speed_calc_skip = 1; //ïðîïóñòèòü ðàñ÷åò ñêîðîñòè
QEP0->QEPSTS = 1 << 3;
}
if (p->QEPSTS_bit.CDEF == 1) { //ïðîâåðêà íà èçìåíåíèÿ íàïðàâëåíèÿ äâèæåíèÿ âî âðåìÿ çàõâàòà
p->speed_elec = 0;
p->speed_calc_skip = 1; //ïðîïóñòèòü ðàñ÷åò ñêîðîñòè
QEP0->QEPSTS = 1 << 2;
}
if (p->Qcprdlat_tmp<7){//ëîæíûé ôðîíò
p->speed_calc_skip = 1; //ïðîïóñòèòü ðàñ÷åò ñêîðîñòè
}
if (p->QEPSTS_bit.QDF!=p->DirPrev){//ñìåíèëîñü íàïðàâëåíèå äâèæåíèÿ
p->speed_elec = 0;
p->speed_calc_skip = 1; //ïðîïóñòèòü ðàñ÷åò ñêîðîñòè
}
p->DirPrev=p->QEPSTS_bit.QDF;
if (p->skip_counter != 0) { //Ñìåíèëñÿ UPPS
p->skip_counter--;
p->speed_calc_skip = 1; //ïðîïóñòèòü ðàñ÷åò ñêîðîñòè
}
if (p->Qcprdlat_tmp > ((1<<(32-SCALE_IQ_K-1)))) { //Ñ÷åò÷èê 32 ðàçðÿäà, à äðàéâåð ïðåäíàçíà÷åí äëÿ ìåíüøåãî ÷èñëà, îïðåäåëÿåìîãî SCALE_IQ_K
p->speed_elec = 0;
p->speed_calc_skip = 1; //ïðîïóñòèòü ðàñ÷åò ñêîðîñòè
}
if (!p->speed_calc_skip) { //åñëè ðàñ÷åò ñêîðîñòè ïðîïóñêàòü íå íàäî
if (p->QEPSTS_bit.QDF == 1) //â çàâèñèìîñòè îò íàïðàâëåíèÿ äâèæåíèÿ
p->speed_tmpIQ = _IQdiv(p->speedK, p->Qcprdlat_tmp << SCALE_IQ_K); //êîýôôèöèåò ñêîðîñòè äåëåòñÿ íà 16òè ðàçðÿäíîå âðåìÿ, ïîäâèíóòîå ââåðõ íà 15
else
p->speed_tmpIQ = -_IQdiv(p->speedK, p->Qcprdlat_tmp << SCALE_IQ_K);
//Íà ñêîëüêî íóæíî ïîäâèíóòü ðåçóëüòàò â çàâèñèìîñòè îò
//äåëèòåëåé íà UPEVNT è òàêòèðîâàíèè ñ÷åò÷èêà âðåìåíè
p->MoveK = QEP0->QCAPCTL_bit.UPPS
- QEP0->QCAPCTL_bit.CCPS;
if (p->MoveK >= 0) //íàäî ïîäâèíóòü ââåðõ
p->speed_tmpIQ = p->speed_tmpIQ << p->MoveK;
else
//âíèç
p->speed_tmpIQ = p->speed_tmpIQ >> -p->MoveK;
p->speed_elec = p->speed_tmpIQ; //ãîòîâàÿ ñêîðîñòü â ôîðìàòå 8.24
}
QEP0->QEPSTS = 1 << 7;
}
else if (QEP0->QCTMR > QEP0->QCPRD)
{ //íåò ñîáûòèÿ UPEVNT, ñ÷èòàåì ïî òàéìåðó QCTMR, à íå ïî ïåðèîäó QCPRD
p->Qcprdlat_tmp = QEP0->QCTMR; //êîïèÿ ñ÷åò÷èêà
if (p->QEPSTS_bit.COEF == 1) { //ïðîâåðêà ôëàãà ïåðåïîëíåíèÿ òàéìåðà
p->speed_elec = 0;
p->speed_calc_skip = 1; //ïðîïóñòèòü ðàñ÷åò ñêîðîñòè
}
if (p->QEPSTS_bit.CDEF == 1) { //ïðîâåðêà íà èçìåíåíèÿ íàïðàâëåíèÿ äâèæåíèÿ âî âðåìÿ çàõâàòà
p->speed_elec = 0;
p->speed_calc_skip = 1; //ïðîïóñòèòü ðàñ÷åò ñêîðîñòè
}
if (p->Qcprdlat_tmp<7){//ëîæíûé ôðîíò
p->speed_calc_skip = 1; //ïðîïóñòèòü ðàñ÷åò ñêîðîñòè
}
if (p->Qcprdlat_tmp > ((1<<(32-SCALE_IQ_K-1)))) { //Ñ÷åò÷èê 32 ðàçðÿäà, à äðàéâåð ïðåäíàçíà÷åí äëÿ ìåíüøåãî ÷èñëà, îïðåäåëÿåìîãî SCALE_IQ_K
p->speed_elec = 0;
p->speed_calc_skip = 1; //ïðîïóñòèòü ðàñ÷åò ñêîðîñòè
}
if (!p->speed_calc_skip) { //åñëè ðàñ÷åò ñêîðîñòè ïðîïóñêàòü íå íàäî
if (p->QEPSTS_bit.QDF == 1) //â çàâèñèìîñòè îò íàïðàâëåíèÿ äâèæåíèÿ
p->speed_tmpIQ = _IQdiv(p->speedK, p->Qcprdlat_tmp << SCALE_IQ_K); //êîýôôèöèåò ñêîðîñòè äåëåòñÿ íà 16òè ðàçðÿäíîå âðåìÿ, ïîäâèíóòîå ââåðõ íà 15
else
p->speed_tmpIQ = -_IQdiv(p->speedK, p->Qcprdlat_tmp << SCALE_IQ_K);
//Íà ñêîëüêî íóæíî ïîäâèíóòü ðåçóëüòàò â çàâèñèìîñòè îò
//äåëèòåëåé íà UPEVNT è òàêòèðîâàíèè ñ÷åò÷èêà âðåìåíè
p->MoveK = QEP0->QCAPCTL_bit.UPPS
- QEP0->QCAPCTL_bit.CCPS;
if (p->MoveK >= 0) //íàäî ïîäâèíóòü ââåðõ
p->speed_tmpIQ = p->speed_tmpIQ << p->MoveK;
else
//âíèç
p->speed_tmpIQ = p->speed_tmpIQ >> -p->MoveK;
p->speed_elec = p->speed_tmpIQ; //ãîòîâàÿ ñêîðîñòü â ôîðìàòå 8.24
}
}
}
}
//ðàññ÷èòûâàòü ñêîðîñòü ïðîãðàììíî ÷åðåç ïðîèçâîäíóþ óãëà
if (p->SpeedCalcType==SPEED_CALC_TYPE_BY_SOFT){
//Ñíà÷àëà ëåãêèé ôèëüòð íà ñàì óãîë
//0.5 è ìàñêè - ýòî ìàãèÿ íå÷óâñòâèòåëüíîñòè ôèëüòðà ê ðàçíèöå óãëîâ áîëüøå 360
//×òîáû ïðè îáíóëåíèè óãëà ïðè ïåðåõîäå ÷åðåç 360 ãðàäóñîâ ïðè
//âû÷èòàíèè èç 5 ãðàäóñîâ 355 ïîëó÷èëîñü 10 (â IQ ôîðìàòå)
p->theta_finish=p->theta_finish+_IQmpy(_IQ(0.5),((p->theta_elec-p->theta_finish+_IQ(0.5))&0x00FFFFFF)-_IQ(0.5));
p->theta_finish&=0x00FFFFFF;
//ðàñ÷èòûâàåì ïðèðàùåíèå óãëà
temp = (p->theta_finish << 8) - (p->theta_start << 8);
p->d_fi = (temp >> 8);
p->theta_start = p->theta_finish;
//ñêîðîñòü íåôèëüòðîâàííàÿ, ðàññ÷èòàííàÿ èç ïðîèçâîäíîé óãëà çà îäèí ïåðèîä
p->speed_elec_temp=_IQmpy(p->d_fi,p->KThetaToSpeed)<<3;
//èíåðöèîííûé ôèëüòð äëÿ ñêîðîñòè
p->speed_filter.input = p->speed_elec_temp;
p->speed_filter.calc(&p->speed_filter);
p->speed_elec=p->speed_filter.output;
}
//Â ñèìóëÿòîðå äîñòóïåí òîëüêî îäèí ñïîñîá èçìåðåíèÿ ñêîðîñòè - ïî óãëó ïðîãðàììíî.
#if (defined(HW_VECTORCARD_SIMULATOR)) || (defined(HW_NIIET_BOARD_SIMULATOR))
p->SpeedCalcType=SPEED_CALC_TYPE_BY_SOFT;
#endif
}
//! Ôóíêöèÿ îáðàáîòêè ðåïåðà (ñîáûòèå èíäåêñà)
//! \memberof TposspeedEqep
void TposspeedEqep_IndexEvent(TposspeedEqep *p) {
p->Posspeed_FLG2.bit.pos_ident = 1;
}
//! \memberof TposspeedEqep
void TposspeedEqep_SlowCalc(TposspeedEqep *p) {
//êîýôôèöèåíò ïåðåâîäà ïðîèçâîäíîé îò óãëà (âû÷èñëÿåìóþ íåèçâåñòíî â ÷åì) â ñêîðîñòü
//ñäâèã íà 3 ÷òîáû ðåçóëüòàò äåëåíèÿ íå ïåðåïîëíèë 127, ïîòîì ïðè èñïîëüçîâàíèè êîýôà
//ñäâèíåòñÿ íàçàä íà 3
p->KThetaToSpeed=_IQdiv(_IQ(1.0),_IQmpyI32(drv_params.freq_nom,FAST_CALC_TS)<<3);
}
/*@}*/