252 lines
10 KiB
C
252 lines
10 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.h
|
|
\brief Ìîäóëü îöåíêè ñêîðîñòè è ïîëîæåíèÿ ïðè ïîìîùè eQEP (ñì. TposspeedEqep)
|
|
\author ÎÎÎ "ÍÏÔ Âåêòîð". Âñå ïðàâà çàùèùåíû. http://motorcontrol.ru
|
|
\version v 2.0 25/03/2016
|
|
|
|
\defgroup V_QEP Ìîäóëü îöåíêè ñêîðîñòè è ïîëîæåíèÿ ïðè ïîìîùè eQEP (ñì. TposspeedEqep)
|
|
|
|
\addtogroup V_QEP
|
|
@{*/
|
|
|
|
|
|
#include "V_IQmath.h"
|
|
#include "filter.h"
|
|
#ifndef V_POSSPEED_EQEP_H
|
|
#define V_POSSPEED_EQEP_H
|
|
|
|
#ifdef __cplusplus
|
|
extern "C" {
|
|
#endif
|
|
|
|
|
|
struct SPOSSPEEDCTL_BITS { // bits
|
|
Uint16 InitType:
|
|
2; // 0:1 INPUT: ðåæèì èíèöèàëèçàöèè ïîëîæåíèÿ ïðè ïóñêå:
|
|
// 0 - âñ¸ âûñòàâëÿåòñÿ âðó÷íóþ
|
|
// 1 - âûñòàâëÿåì âåêòîð òîêà, äàëåå ïóñê â âåêòîðíîì ðåæèìå(èñõîäíîå ïîëîæåíèå - 0 ýëåêòðè÷åñêèõ ãðàäóñîâ);
|
|
// 2 - ïóñê â âåêòîðíîì ðåæèìå(èñõîäíîå ïîëîæåíèå - ïîñëåäíåå ñîõðàí¸ííîå)
|
|
Uint16 index_en:
|
|
1; // 2 INPUT: íàëè÷èå ðåïåðà
|
|
// 0 - ðåïåð îòñóòñòâóåò
|
|
// 1 - ðåïåð ïîäêëþ÷¸í;
|
|
Uint16 dir:
|
|
1; // 3 INPUT: íàïðàâëåíèå äâèæåíèÿ(â ñëó÷àå íåïðàâèëüíîãî íàïðàâëåíèÿ);
|
|
// 0 - (ïî óìîë÷àíèþ)ïðÿìîå âêëþ÷åíèå;
|
|
// 1 - èíâåðñèÿ;
|
|
Uint16 CmdInit:
|
|
1; // 4 INPUT: äëÿ ðåæèìà èíèöèàëèçàöèè
|
|
Uint16 Min_clk:
|
|
4; // 5:8 INPUT: ìèíèìàëüíîå ÷èñëî ìåòîê äëÿ çàõâàòà íà íèçêîé ñêîðîñòè - ÍÅ ÌÅÍÿÒÜ!!!!!
|
|
// Min_clk=2 - 4 ìåòêè
|
|
// 3 - 8 ìåòîê - ïî óìîë÷àíèþ
|
|
// 4 - 16 ìåòîê
|
|
// 5 - 32 ìåòêè
|
|
// 6 - 64 ìåòêè
|
|
// 7 - 128 ìåòîê
|
|
// 8 - 256 ìåòîê
|
|
// 9 - 512 ìåòîê
|
|
// 10 - 1024 ìåòêè
|
|
Uint16 rsvd:
|
|
7; // 9:15 reserved;
|
|
};
|
|
|
|
union SPOSSPEED_CTL {
|
|
Uint16 all;
|
|
struct SPOSSPEEDCTL_BITS bit;
|
|
};
|
|
|
|
struct SPOSSPEEDFLG1_BITS { // bits
|
|
Uint16 UTO_tmp:
|
|
1; // 0 TEMPORARY FLAG: ôëàã time-out;
|
|
Uint16 PCO_tmp:
|
|
1; // 1 TEMPORARY FLAG: ôëàã ïîëîæèòåëüíîãî ïåðåïîëíåíèÿ ñ÷¸ò÷èêà;
|
|
Uint16 PCU_tmp:
|
|
1; // 2 TEMPORARY FLAG: ôëàã îòðèöàòåëüíîãî ïåðåïîëíåíèÿ ñ÷¸ò÷èêà;
|
|
Uint16 UPPS_tmp:
|
|
4; // 3:6 TEMPORARY: çíà÷åíèå òåêóùåãî êâàíòà ïåðåìåùåíèÿ äëÿ çàõâàòà;
|
|
Uint16 CCPS_tmp:
|
|
3; // 7:9 TEMPORARY: çíà÷åíèå òåêóùåãî êîýôôèöèåíòà äåëåíèÿ;
|
|
Uint16 first_launch:
|
|
2; // 10:11 FLAG: èíèöèàëèçàöèÿ ïåðâîãî çàïóñêà;
|
|
Uint16 accuracy:
|
|
2; // 12:13 FLAG: ôëàã ïåðåêëþ÷åíèÿ àëãîðèòìîâ;
|
|
Uint16 first_theta:
|
|
1; // 14 FLAG: ôëàã ïåðâîãî ðàñ÷¸òà óãëîâîãî ïðåðàùåíèÿ;
|
|
Uint16 transit:
|
|
1; // 15 FLAG: ôëàã ïåðåõîäà íà àëãîðèòì âûñîêèõ ñêîðîñòåé;
|
|
|
|
};
|
|
|
|
union SPOSSPEED_FLG1 {
|
|
Uint16 all;
|
|
struct SPOSSPEEDFLG1_BITS bit;
|
|
};
|
|
|
|
struct SPOSSPEEDFLG2_BITS { // bits
|
|
Uint16 pos_ident:
|
|
1; // 0 FLAG: ôëàã òî÷íîé èäåíòèôèêàöèè àáñîëþòíîãî ïîëîæåíèÿ ðîòîðà;
|
|
Uint16 UTO_tmp2:
|
|
1; // 1 TEMPORARY FLAG: ôëàã time-out;
|
|
Uint16 PCO_tmp2:
|
|
1; // 2 TEMPORARY FLAG: ôëàã ïîëîæèòåëüíîãî ïåðåïîëíåíèÿ ñ÷¸ò÷èêà;
|
|
Uint16 PCU_tmp2:
|
|
1; // 3
|
|
Uint16 lost_UTO:
|
|
1; // 4
|
|
Uint16 Dir_prev:
|
|
1; // 5
|
|
Uint16 Cdef:
|
|
1; // 6
|
|
Uint16 Delay_flux:
|
|
1; // 7
|
|
Uint16 first_index:
|
|
1; // 8
|
|
Uint16 rsvd:
|
|
8; // 9:15 reserved;
|
|
};
|
|
|
|
union SPOSSPEED_FLG2 {
|
|
Uint16 all;
|
|
struct SPOSSPEEDFLG2_BITS bit;
|
|
};
|
|
|
|
|
|
|
|
/*! \class TposspeedEqep
|
|
\brief Ìîäóëü îáðàáîòêè êâàäðàòóðíîãî äåêîäåðà (ÄÏÐ òèïà "ýíêîäåð")
|
|
|
|
Êëàññ \a TposspeedEqep, îñíîâàííûé íà ñòðóêòóðå SposspeedEqep,
|
|
ÿâëÿåòñÿ ìîäóëåì äëÿ îïðåäåëåíèÿ óãëîâîãî ïîëîæåíèÿ è ñêîðîñòè ðîòîðà
|
|
äâèãàòåëÿ ïî äàííûì îò êâàäðàòóðíîãî ýíêîäåðà. Äëÿ ðàáîòû ñ äàò÷èêîì
|
|
èñïîëüçóåòñÿ ïåðèôåðèéíûé ìîäóëü ïðîöåññîðà EQEP. Ïîëîæåíèå âàëà theta_elec îïðåäåëÿåòñÿ
|
|
ïî àïïàðàòíîìó ñ÷åò÷èêó QPOSCNT, êîòîðûé óâåëè÷èâàåò è óìåíüøàåò
|
|
ñâîå çíà÷åíèå àâòîìàòè÷åñêè ïðè âðàùåíèè âàëà. Ïîñëå âêëþ÷åíèÿ ïðîöåññîðà
|
|
àáñîëþòíîå óãëîâîå ïîëîæåíèå âàëà íå ìîæåò áûòü îïåðåëåíî, ïîêà íå ïðîèçîéäåò
|
|
ñîáûòèå èíäåêñàöèè (ñðàáîòàåò ðåïåðíàÿ ìåòêà). Ìîäóëü âîçâîäèò ôëàã Posspeed_FLG2.bit.pos_ident,
|
|
êàê òîëüêî ýòî ïðîèñõîäèò. Äî ýòîãî ìîìåíòà çíà÷åíèå óãëîâîãî ïîëîæåíèÿ íå âàëèäíîå è íå äîëæíî èñïîëüçîâàòüñÿ
|
|
ñèñòåìîé óïðàâëåíèÿ. Äëÿ êàëèáðîâêè äàò÷èêà ïîëîæåíèÿ (ñìåùåíèÿ íóëåâîãî ïîëîæåíèÿ)
|
|
èñïîëüçóåòñÿ ïåðåìåííàÿ AngleOffset.
|
|
Äëÿ âû÷èñëåíèÿ ñêîðîñòè speed_elec èñïîëüçóåòñÿ àïïàðàòíûé ñ÷åò÷èê QCPRD,
|
|
êîòîðûé èçìåðÿåò âðåìÿ ìåæäó çàäàííûì êîëè÷åñòâîì ìåòîê äàò÷èêà. Äëÿ îáåñïå÷åíèÿ ïðèåìëåìîé
|
|
òî÷íîñòè íà âûñîêèõ è íèçêèõ ñêîðîñòÿõ "çàäàííîå" êîëè÷åñòâî ìåòîê ïåðåêëþ÷àåòñÿ íà õîäó ïðè ïîìîùè
|
|
äåëèòåëÿ UPPS. Òàê êàê ñêîðîñòü íà ìàëûõ ñêîðîñòÿõ âðàùåíèÿ ïóëüñèðóåò,
|
|
èìååòñÿ èíåðöèîííûé ôèëüòð ñêîðîñòè ïåðâîãî ïîðÿäêà, çíà÷åíèå êîòîðîãî,
|
|
speed.output, ìîæåò áûòü âûâåäåíî ïîëüçîâàòåëþ.
|
|
*/
|
|
|
|
//! ñì. TposspeedEqep
|
|
struct SposspeedEqep {
|
|
Uint32 resol; //!<INPUT: ðàçðåøåíèå äàò÷èêà â ìåòêàõ/îáîðîò;
|
|
float resol_inv; //!<îáðàòíàÿ âåëè÷èíà ê ðàçðåøåíèþ äàò÷èêà resol
|
|
Uint16 pole_pairs; //!<INPUT: ÷èñëî ïàð ïîëþñîâ;
|
|
Uint16 SpeedCalcType; //!< Òèï ðàñ÷åòà ñêîðîñòè âðàùåíèÿ: 0 - àïïàðàòíî, 1 - ïðîãðàììíî ïî óãëó.
|
|
float speed_nom; //!<INPUT: íîìèíàëüíàÿ ñêîðîñòü â îá/ìèí;
|
|
_iq speed_elec; //!<OUTPUT: ýëåêòðè÷åñêàÿ ñêîðîñòü â Q24(îòíîñèòåëüíûå åäèíèöû)
|
|
_iq theta_elec; //!<OUTPUT: ýëåêòðè÷åñêèé óãîë â Q24(îòíîñèòåëüíûå åäèíèöû)
|
|
float k_low; //!<PARAMETER;
|
|
Uint32 Poscnt_res; //!<Ìåõàíè÷åñê.óãîë â ìåòêàõ;
|
|
Uint16 Poscnt_res16; //!<Ìåõàíè÷åñê.óãîë â ìåòêàõ 16òè ðàçðÿäíûé äëÿ îñöà;
|
|
Uint32 Poscnt_resPrev; //!<Ìåõàíè÷åñê.óãîë â ìåòêàõ ïðåäûäóùåå çíà÷åíèå;
|
|
int16 MoveK; //!<TEMPORARY;
|
|
_iq theta_mech; //!<Ìåõàíè÷åñêèé óãîë;
|
|
_iq prevThetaMech; //!<Ìåõàíè÷åñêèé óãîë ïðåäûäóùåå çíà÷åíèå;
|
|
_iq theta_el_tmp; //!<TEMPORARY: ýëåêòðè÷åñêèé óãîë;
|
|
Uint16 speed_calc_skip; //!<ïðîïóñê ðàñ÷åòà ñêîðîñòè, ôëàã
|
|
_iq speedK; //!<TEMPORARY: äåëèòåëü ÷àñòîòû, òàêòèðóþùåé òàéìåð çàõâàòà;
|
|
union { /*!< Status interrupt */
|
|
Uint16 Qcprdlat16_tmp; //!<TEMPORARY: êîïèÿ çàù¸ëêè âðåìåíè;
|
|
Uint32 Qcprdlat_tmp; //!<TEMPORARY: êîïèÿ çàù¸ëêè âðåìåíè;
|
|
}; //!<Äëÿ íàáëþäåíèÿ â þíèêîíå
|
|
union { /*!< Status interrupt */
|
|
uint16_t QEPSTS; /*!< QEPSTS : type used for word access */
|
|
_QEP_QEPSTS_bits QEPSTS_bit; /*!< QEPSTS_bit: structure used for bit access */
|
|
}; //!<Äëÿ íàáëþäåíèÿ â þíèêîíå
|
|
Uint16 DirPrev;
|
|
_iq speed_tmpIQ; //!<TEMPORARY;
|
|
TFilter speed_filter; //!<Ôèëüòð ñêîðîñòè
|
|
Uint16 skip_counter;//!< Ñ÷åò÷èê ïðîïóñêà ðàñ÷åòà ñêîðîñòè ïîñëå ñìåíû UPPS
|
|
union SPOSSPEED_CTL Posspeed_CTL;
|
|
union SPOSSPEED_FLG1 Posspeed_FLG1;
|
|
union SPOSSPEED_FLG2 Posspeed_FLG2;
|
|
Uint16 GPIOsValue;//!< Òåêóùåå çíà÷åíèå íîæåê GPIO ìîäóëÿ QEP
|
|
Uint16 Index_eventFlag;
|
|
Uint16 UPPS_forWatch;//!< Òåêóùåå çíà÷åíèå UPPS
|
|
_iq AngleOffset;//!< Ñìåùåíèå ýëåêòðè÷åñêîãî óãëà â ãðàäóñàõ
|
|
int16 RevolutionCounter; // Êîëè÷åñòâî ìåõàíè÷åñêèõ îáîðîòîâ;
|
|
int16 Poscnt_resContinouosInt8;//!<Ìåõàíè÷åñê.óãîë â ìåòêàõ c ó÷åòîì êîë-âà îáîðîòîâ (ïåðåìåííàÿ 8 ðàçðÿäîâ îòñåêàåòñÿ)
|
|
int16 Poscnt_resContinouosInt;//!<Ìåõàíè÷åñê.óãîë â ìåòêàõ c ó÷åòîì êîë-âà îáîðîòîâ (ïåðåìåííàÿ 16 ðàçðÿäîâ)
|
|
long Poscnt_resContinouosLong;//!<Ìåõàíè÷åñê.óãîë â ìåòêàõ c ó÷åòîì êîë-âà îáîðîòîâ (ïåðåìåííàÿ 32 ðàçðÿäîâ)
|
|
_iq theta_mechContinouos; //!<Ìåõàíè÷åñêèé óãîë â ãðàäóñàõ àáñîëþòíûé;
|
|
_iq theta_elecContinouos; //!<Ýëåêòðè÷åñêèé óãîë â ãðàäóñàõ àáñîëþòíûé;
|
|
|
|
long speed_elec_temp; //Ñêîðîñòü íåôèëüòðîâàííàÿ
|
|
long theta_start; //âíóòðåííÿÿ ïåððåìåííàÿ: Íà÷àëüíûé óãîë
|
|
long theta_finish;//âíóòðåííÿÿ ïåððåìåííàÿ: Êîíå÷íûé óãîë
|
|
long d_fi;//âíóòðåííÿÿ ïåððåìåííàÿ: ðàçíèöà â óãëå
|
|
long KThetaToSpeed; //âíóòðåííÿÿ ïåððåìåííàÿ: êîýô. ïåðåñ÷åòà ïðîèçâ. óãëà â ñêîðîñòü
|
|
void (*init)(volatile struct SposspeedEqep*);
|
|
void (*calc)(volatile struct SposspeedEqep*);
|
|
void (*slow_calc)(volatile struct SposspeedEqep*);
|
|
void (*index)(volatile struct SposspeedEqep*);
|
|
};
|
|
|
|
|
|
|
|
|
|
typedef volatile struct SposspeedEqep TposspeedEqep;
|
|
|
|
//! Èíèöèàëèçàòîð ïî-óìîë÷àíèþ.
|
|
#define POSSPEED_DEFAULTS {0,0,0,0,0, 0,0,0,0,0,0,0, 0,0,0,0, 0,{0},{0},0,0,FILTER_DEFAULTS,0,\
|
|
0,0x0790,0,0,0,0,0,0,0,0,0,0,0,\
|
|
0,0,0,0,0,\
|
|
TposspeedEqep_init,\
|
|
TposspeedEqep_Calc,\
|
|
TposspeedEqep_SlowCalc,\
|
|
TposspeedEqep_IndexEvent,\
|
|
}
|
|
|
|
//! \memberof TposspeedEqep
|
|
void TposspeedEqep_init(TposspeedEqep*);
|
|
//! \memberof TposspeedEqep
|
|
void TposspeedEqep_Calc(TposspeedEqep*);
|
|
//! \memberof TposspeedEqep
|
|
void TposspeedEqep_SlowCalc(TposspeedEqep*);
|
|
//! \memberof TposspeedEqep
|
|
void TposspeedEqep_IndexEvent(TposspeedEqep*);
|
|
#ifdef __cplusplus
|
|
}
|
|
#endif
|
|
|
|
#endif
|
|
/*@}*/
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|