a99491f9b8
- проект переведён на VectorIDE v1.3 В целях экономии памяти удалены: - модуль управления светодиодами - модуль ШИМ для двигателей SRD - модуль часов реального времени - режим привода для измерения задержки меджу сигналами ШИМ и измерениями токов Добавлены следующие модули: - проект переведён на VectorIDE v1.3 - модуль SPI для абсолютного ДПР - модуль управление реле для заряда ЗПТ - модуль дискретных вводов-выводов - модуль управления вентилятором Одноплатного Инвертора - модуль тормозного резистора Одноплатного Инвертора Прочие изменения: - оптимизирована инициализация регистров периферии - удалено множество неиспользуемых переменных - разрешение работы всех GPIO перенесено в функцию "PeripheralClockEnable" - добавлен счётчик индексной метки энкодера - исправлен сброс прерываний модуля захвата CAP - переработан режим задания постоянного тока статора - исправлены прочие мелкие ошибки в разных модулях
321 lines
12 KiB
C
321 lines
12 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) {
|
|
|
|
QEP->QCAPCTL_bit.UPPS = LOWEST_UPSS;
|
|
QEP->QCAPCTL_bit.CCPS = 4;
|
|
QEP->QDECCTL_bit.QSRC = 0; //Âêë. ðåæèì êâàäðàòóðíîãî ñ÷¸òà
|
|
QEP->QEPCTL_bit.FREESOFT = 0; //Ðåæèì îòëàä÷èêà
|
|
QEP->QPOSINIT = 0; //Çíà÷åíèå ñ÷¸ò÷èêà ïîëîæåíèÿ ïîñëå èíèöèàëèçàöèè èëè ïîëîæèòåëüíîãî ïåðåïîëíåíèÿ
|
|
QEP->QEPCTL_bit.SWI = 1; //Ñáðîñ ñ÷¸ò÷èêà
|
|
QEP->QPOSMAX = (p->resol << 2) - 1; //Ìàêñèìàëüíîå çíà÷åíèå ñ÷¸ò÷èêà, ïîñëå ÷åãî îáíóëåíèå
|
|
if (p->Posspeed_CTL.bit.dir == 1) { //Ïðîâåðêà íàïðàâëåíèÿ äâèæåíèÿ
|
|
QEP->QDECCTL_bit.QAP = 1;
|
|
}
|
|
|
|
QEP->QEPCTL_bit.QPEN = 1; //Âêë. êâàäðàòóðíîãî ñ÷¸ò÷èêà
|
|
QEP->QCLR = 0xFFFF; //Îò÷èñòêà âñåõ ôëàãîâ
|
|
QEP->QEPCTL_bit.QCLM = 0; //Ðåæèì çàõâàòà - ïî óìîë÷àíèþ, ïî ñîáûòèþ îòðàáîòêè çàäàííîãî ÷èñëà èìïóëüñîâ
|
|
|
|
if (p->Posspeed_CTL.bit.index_en == 1) { //åñëè èíäåêñíàÿ ìåòêà åñòü
|
|
QEP->QEPCTL_bit.PCRM = 0;
|
|
QEP->QEINT_bit.IEL = 1;
|
|
} else {
|
|
QEP->QEPCTL_bit.PCRM = 1; //Ðåæèì îáíóëåíèÿ ñ÷¸ò÷èêà - ïî ìàêñèìàëüíîìó çíà÷åíèþ
|
|
}
|
|
QEP->QWDPRD = 200000 / (1 << 2); //Ââîä ïåðèîäà ñòîðîæåâîãî òàéìåðà
|
|
QEP->QEPCTL_bit.WDE = 1; //Âêë. ñòîðîæåâîãî òàéìåðà
|
|
|
|
/*Ðàñ÷¸ò âñïîìîãàòåëüíîãî êîýôôèöèåíòà äëÿ ðàñ÷¸òà ñêîðîñòè,
|
|
ãäå 15=60/4, 60 - äëÿ ïåðåâîäà â îá/ìèí; 4 - äëÿ ïîëó÷åíèÿ èíòåðïîëÿöèè ðàçðåøåíèÿ äàò÷èêà (1 ïåðèîä äàåò 4 ôðîíòà);
|
|
SystemCoreClock - ÷àñòîòà ïðîöåññîðà*/
|
|
p->k_low = SystemCoreClock / ((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);
|
|
|
|
p->Index_eventCounter = 0;
|
|
|
|
//Èíèöèàëèçàöèÿ ïîðòîâ ââîäà âûâîäà
|
|
GPIOB->ALTFUNCSET = (1 << 11) + (1 << 12) + (1 << 13);
|
|
SIU->REMAPAF_bit.QEPEN = 1;
|
|
}
|
|
|
|
//! Ôóíêöèÿ ðàñ÷¸òà ñêîðîñòè è ïîëîæåíèÿ, âûçûâàåòñÿ ñ íåîáõîäèìîé äèñêðåòíîñòüþ
|
|
|
|
//! \memberof TposspeedEqep
|
|
void TposspeedEqep_Calc(TposspeedEqep *p) {
|
|
long temp;
|
|
//Òåêóùåå çíà÷åíèå GPIO äëÿ îòëàäêè. Ìîæíî ñìîòðåòü â îñöàõ
|
|
p->GPIOsValue = (GPIOB->DATA >> 12) & 3;
|
|
|
|
//åñëè ñêîðîñòü âûñîêàÿ, à äåëèòåëü UPEVNT íå âûñîêèé
|
|
if ((labs(p->speed_elec) > _IQ(2)) && (QEP->QCAPCTL_bit.UPPS != 5)) {
|
|
QEP->QCAPCTL_bit.UPPS = 5; //äåëàåì äåëèòåëü UPEVNT âûñîêèé
|
|
p->skip_counter = 2; //äâà òàêòà ïðîïóñòèì ðàñ÷åò ñêîðîñòè
|
|
}
|
|
|
|
//åñëè ñêîðîñòü íèçêàÿ, à äåëèòåëü íå íèçêèé
|
|
if ((labs(p->speed_elec) < _IQ(1.5)) && (QEP->QCAPCTL_bit.UPPS != LOWEST_UPSS)) {
|
|
QEP->QCAPCTL_bit.UPPS = LOWEST_UPSS; //äåëàåì äåëèòåëü UPEVNT íèçêèé
|
|
p->skip_counter = 2; //äâà òàêòà ïðîïóñòèì ðàñ÷åò ñêîðîñòè
|
|
}
|
|
|
|
p->UPPS_forWatch = QEP->QCAPCTL_bit.UPPS; //äëÿ îòîáðàæåíèÿ UPPS íàðóæó
|
|
|
|
if (p->Posspeed_CTL.bit.CmdInit == 1) { //Êîìàíäà ïåðåèíèöèàëèçàöèè - âñå îáíóëÿåì
|
|
QEP->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;
|
|
p->Poscnt_res = (unsigned long) QEP->QPOSCNT; //çàõâàò ïîëîæåíèÿ
|
|
|
|
|
|
p->Poscnt_res16=p->Poscnt_res;//Äëÿ íàáëþäåíèÿ íà îñöèëëîãðàôå
|
|
p->Posspeed_FLG1.bit.PCO_tmp = QEP->QFLG_bit.PCO; //êîïèÿ ôëàãà ïîëîæèòåëüíîãî ïåðåïîëíåíèÿ ñ÷¸ò÷èêà
|
|
p->Posspeed_FLG1.bit.PCU_tmp = QEP->QFLG_bit.PCU; //êîïèÿ ôëàãà îòðèöàòåëüíîãî ïåðåïîëíåíèÿ ñ÷¸ò÷èêà
|
|
QEP->QCLR_bit.PCO = 1; //îò÷èñòêà ôëàãà ïîëîæèòåëüíîãî ïåðåïîëíåíèÿ ñ÷¸ò÷èêà
|
|
QEP->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+((QEP->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 (QEP->QFLG_bit.WTO == 1) {
|
|
p->Posspeed_FLG1.bit.first_launch = FIRST_LAUNCH_UP; //óêàçàòåëü ïåðâîãî çàïóñêà àëãîðèòìà ïîñëå ñðàáàòûâàíèÿ òàéìåðà
|
|
p->speed_elec = 0; //îáíóëåíèå ñêîðîñòè
|
|
QEP->QCLR_bit.WTO = 1; //îò÷èñòêà ôëàãà
|
|
QEP->QWDTMR = 0; //îáíóëåíèå òàéìåðà
|
|
} else {
|
|
/*ïåðåêëþ÷åíèå ïî ïåðâîìó çàïóñêó äëÿ èíèöèàëèçàöèè*/
|
|
if (p->Posspeed_FLG1.bit.first_launch == FIRST_LAUNCH_UP) {
|
|
//Îòêë. áëîêà çàõâàòà
|
|
QEP->QCAPCTL_bit.CEN = 0;
|
|
QEP->QCAPCTL_bit.CEN = 1;
|
|
p->Posspeed_FLG1.bit.first_launch = NOT_FIRST_LAUNCH;
|
|
}
|
|
|
|
p->QEPSTS=QEP->QEPSTS;//êîïèÿ ðåãèñòðà ñòàòóñà
|
|
|
|
if (p->QEPSTS_bit.UPEVNT == 1) { //åñòü ñîáûòèå UPEVNT, çíà÷èò â òàéìåðå çàõâà÷åíî âðåìÿ
|
|
p->Qcprdlat_tmp = QEP->QCPRD; //êîïèÿ ñ÷åò÷èêà âðåìåíè
|
|
p->speed_calc_skip = 0; //ïðîïóñê ðàñ÷åòà ñêîðîñòè - íå ïðîïóñêàòü
|
|
|
|
if (p->QEPSTS_bit.COEF == 1) { //ïðîâåðêà ôëàãà ïåðåïîëíåíèÿ òàéìåðà
|
|
p->speed_elec = 0;
|
|
p->speed_calc_skip = 1; //ïðîïóñòèòü ðàñ÷åò ñêîðîñòè
|
|
QEP->QEPSTS = 1 << 3;
|
|
}
|
|
|
|
if (p->QEPSTS_bit.CDEF == 1) { //ïðîâåðêà íà èçìåíåíèÿ íàïðàâëåíèÿ äâèæåíèÿ âî âðåìÿ çàõâàòà
|
|
p->speed_elec = 0;
|
|
p->speed_calc_skip = 1; //ïðîïóñòèòü ðàñ÷åò ñêîðîñòè
|
|
QEP->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 = QEP->QCAPCTL_bit.UPPS
|
|
- QEP->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
|
|
}
|
|
|
|
QEP->QEPSTS = 1 << 7;
|
|
}
|
|
else if (QEP->QCTMR > QEP->QCPRD)
|
|
{ //íåò ñîáûòèÿ UPEVNT, ñ÷èòàåì ïî òàéìåðó QCTMR, à íå ïî ïåðèîäó QCPRD
|
|
p->Qcprdlat_tmp = QEP->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 = QEP->QCAPCTL_bit.UPPS
|
|
- QEP->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;
|
|
}
|
|
}
|
|
|
|
//! Ôóíêöèÿ îáðàáîòêè ðåïåðà (ñîáûòèå èíäåêñà)
|
|
|
|
//! \memberof TposspeedEqep
|
|
void TposspeedEqep_IndexEvent(TposspeedEqep *p) {
|
|
p->Posspeed_FLG2.bit.pos_ident = 1;
|
|
p->Index_eventCounter++;
|
|
}
|
|
|
|
//! \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);
|
|
}
|
|
/*@}*/
|
|
|