294 lines
12 KiB
C
294 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 SMSys.c
|
|
\brief Ìîäóëü-îáåðòêà äëÿ ðàñ÷åòà îñòàëüíûõ ìîäóëåé. (ñì. TSM_Sys)
|
|
\author ÎÎÎ "ÍÏÔ Âåêòîð". http://motorcontrol.ru
|
|
\version v 2.0 25/03/2016
|
|
|
|
*/
|
|
|
|
/** \addtogroup SMSys */
|
|
/*@{*/
|
|
#include "main.h"
|
|
|
|
/* Îáúÿâëåíèå àäðåñà ðàçìåùåíèÿ òàáëèöû âåêòîðîâ ïðåðûâàíèé */
|
|
#if defined ( __CMCPPARM__ )
|
|
extern const Uint32 __Vectors;
|
|
#elif defined (__GNUC__)
|
|
|
|
#endif
|
|
|
|
//! Èíèöèàëèçàöèÿ ñèñòåìû óïðàâëåíèÿ ïîñëå âêëþ÷åíèÿ
|
|
|
|
//!Èíèöèàëèçàöèÿ íåêîòîðûõ ìîäóëåé è íàñòðîéêà ïðåðûâàíèé ñèñòåìû.
|
|
//!Çäåñü íàñòðàèâàåòñÿ áîëüøèíñòâî ïðåðûâàíèé
|
|
//! \memberof TSM_Sys
|
|
void SM_Sys_Init(TSM_Sys *p) {
|
|
//TODO: äîáàâèòü òåêñòû ïåðå÷èñëåíèé äëÿ êîíôèãóðàöèé â CODEEDIT
|
|
#if defined (HW_MCB3)
|
|
sw.HardwareType = 4;
|
|
#elif defined (HW_MCB3_SIMULATOR)
|
|
sw.HardwareType = 5;
|
|
#endif
|
|
cmd.all = 0;
|
|
drv_status.all = 0;
|
|
adc.init(&adc); //Èíèöèàëèçàöèÿ ÀÖÏ
|
|
sm_net.init(&sm_net); //Ñåòåâàÿ ÷àñòü
|
|
sm_prot.init(&sm_prot); //Çàùèòû
|
|
sm_cmd_logic.init(&sm_cmd_logic); //Ëîãèêà âêëþ÷åíèÿ/âûêëþ÷åíèÿ
|
|
sm_ctrl.init(&sm_ctrl); //Ñòðóêòóðà ñèñòåìû óïðàâëåíèÿ
|
|
pwm.init(&pwm); //Ìîäóëü ØÈÌ
|
|
cur_par.init(&cur_par); //Ðàñ÷åò òåêóùèõ ïîêàçàòåëåé ïðèâîäà
|
|
leds.init(&leds);//ñâåòîäèîäû
|
|
udControl.init(&udControl); //ïëàâíàÿ çàðÿäêà ÇÏÒ ÷åðåç òåðìèñòîðû
|
|
DIO_Init(); //äèñêðåòíûå âõîäû/âûõîäû
|
|
FanControl.init(&FanControl); //âåíòèëÿòîð
|
|
//ãëîáàëüíîå âðåìß (âíåøíèå èëè âíóòð. ÷àñû)
|
|
global_time.init(&global_time);
|
|
if (drv_params.sens_type == POS_SENSOR_TYPE_HALL){ // òèïå äàò÷èêà âûáðàí ÄÏÐ íà ýëåìåíòàõ Õîëëà
|
|
DPReCAP.Init(&DPReCAP); //ÄÏÐ èíèö.
|
|
}
|
|
if (drv_params.sens_type == POS_SENSOR_TYPE_ENCODER){ // òèïå äàò÷èêà âûáðàí ýíêîäåð
|
|
//íàñòðîèì êâàäðàòóðíûé äåêîäåð
|
|
posspeedEqep.speed_nom = drv_params.speed_nom; //áàçîâàÿ ñêîðîñòü
|
|
posspeedEqep.pole_pairs = drv_params.p; //êîë-âî ïàð ïîëþñîâ äðèãàòåëÿ
|
|
posspeedEqep.Posspeed_CTL.bit.dir = 1; //íàïðàâëåíèå äâèæåíèÿ
|
|
posspeedEqep.RevolutionCounter=0;
|
|
posspeedEqep.init(&posspeedEqep);//èíèöèàëèçàöèÿ ìîäóëÿ ýíêîäåðà
|
|
}
|
|
if (drv_params.sens_type == POS_SENSOR_TYPE_SSI){ // òèïå äàò÷èêà âûáðàí äàò÷èê ñ SSI èíòåðôåéñîì. ×èòàéòå çàãîëîâî÷íèê ìîäóëÿ ïåðåä èñïîëüçîâàíèåì!
|
|
//íàñòðîèì ìîäóëü äàò÷èêà ïîëîæåíèÿ SSI
|
|
SSI_Encoder.speed_nom = drv_params.speed_nom; //áàçîâàÿ ñêîðîñòü
|
|
SSI_Encoder.pole_pairs = drv_params.p; //êîë-âî ïàð ïîëþñîâ äðèãàòåëÿ
|
|
SSI_Encoder.rotation_dir = 0; //íàïðàâëåíèå äâèæåíèÿ
|
|
SSI_Encoder.RevolutionCounter=0;
|
|
SSI_Encoder.init(&SSI_Encoder);//Äàò÷èê ïîëîæåíèÿ SSI
|
|
}
|
|
if (drv_params.sens_type == POS_SENSOR_TYPE_ENC_HALL){
|
|
//íàñòðîèì êâàäðàòóðíûé äåêîäåð
|
|
DPReCAP.Init(&DPReCAP); //ÄÏÐ èíèö.
|
|
posspeedEqep.speed_nom = drv_params.speed_nom; //áàçîâàÿ ñêîðîñòü
|
|
posspeedEqep.pole_pairs = drv_params.p; //êîë-âî ïàð ïîëþñîâ äðèãàòåëÿ
|
|
posspeedEqep.Posspeed_CTL.bit.dir = 1; //íàïðàâëåíèå äâèæåíèÿ
|
|
posspeedEqep.RevolutionCounter=0;
|
|
posspeedEqep.init(&posspeedEqep);//èíèöèàëèçàöèÿ ìîäóëÿ ýíêîäåðà
|
|
}
|
|
|
|
//ëèñòàëêà àâàðèé äëÿ îòîáðàæåíèÿ íà ïóëüòå óïðàâëåíèÿ (UniCON)
|
|
pult_faults_lister.num_of_words = 2;
|
|
pult_faults_lister.out_refresh_devisor = 1000; //ïðè âûçîâå â ìñ òàéìåðå ïåðèîä îáíîâëåíèÿ áóäåò ðàâåí 1ñåê
|
|
pult_faults_lister.w_ptrs[0] = (Uint16*) &sm_prot.masked_bit_fault1;//ïåðåáèðàþòñÿ ýòè ôëàãè àâàðèé ìîäóëÿ çàùèò
|
|
pult_faults_lister.w_ptrs[1] = (Uint16*) &sm_prot.masked_bit_fault2;
|
|
|
|
AutoOffset.FilterK = _IQ(0.00001);//ïîñòîÿííàÿ âðåìåíè ôèëüòðà äëÿ àâòîñìåùåíèÿ ÀÖÏ
|
|
AutoOffset.init(&AutoOffset); //àâòî ñìåùåíèå íåêîòîðûõ êàíàëîâ ÀÖÏ (òîêè)
|
|
|
|
#if defined (HW_MCB3_SIMULATOR)
|
|
//model.motorInternals.udc = 540; //çàäàåòñÿ ÷åðåç ñëîâàðü îáúåêòîâ
|
|
model.tpr = _IQ10div(_IQ10(APB0BusClock/1000.0), pwm.Frequency << 1) >> 10; //ïåðèîä ÷àñòîòû ØÈÌ
|
|
model.dt = _IQ4mpy(_IQ4(150 / 4), pwm.DeadBand >> 20) >> 4; //âåëè÷èíà ìåðòâîãî âðåìåíè
|
|
model.Init(&model); //Ìîäåëü äâèãàòåëÿ
|
|
#endif
|
|
|
|
// Íàñòðîéêà òàéìåðà 0 íà 10 êÃö
|
|
TMR0->INTSTATUS_bit.INT = 1;
|
|
TMR0->LOAD_bit.VAL = (APB0BusClock / 10000L) - 1; //9999 íàäî ÷òîáû íå ïëûëî îòíîñòèëüíî ïðåðûâàíèÿ ØÈÌà, êîãäà ó òîãî ÷àñòîòà 10êÃö
|
|
TMR0->CTRL = ((1 << 0) | (1 << 3)); // Çàïóñê è ðàçðåøåíèå ïðåðûâàíèé
|
|
|
|
// Íàñòðîéêà òàéìåðà 1 íà 1 êÃö
|
|
TMR1->INTSTATUS_bit.INT = 1;
|
|
TMR1->LOAD_bit.VAL = (APB0BusClock / 1000L) - 1;
|
|
TMR1->CTRL = ((1 << 0) | (1 << 3)); // Çàïóñê è ðàçðåøåíèå ïðåðûâàíèé
|
|
|
|
//Äëÿ ïîäñ÷åòà òàêòîâ
|
|
TMR2->LOAD_bit.VAL = 0xFFFFFFFF;
|
|
TMR2->CTRL = (1 << 0); // Çàïóñê
|
|
|
|
extern int *g_pfnVectors; //òàì, ãäå-òî â ñòàðòàï ôàéëå åñòü òàêîé ñèìâîë
|
|
//Íàäî ïîêàçàòü â ýòîì ðåãèñòðå, ãäå ëåæèò òàáëèöà ïðåðûâàíèé.
|
|
//À ëåæèò îíà òàì, êóäà çàïèñàë å¸ ëèíêåð, â çàâèñèìîñòè îò ôàéëà êîìïîíîâêè.
|
|
//Ïîýòîìó áåðåì àäðåñ îò ìàññèâà g_pfnVectors è êëàäåì òóäà
|
|
#if defined (__CMCPPARM__)
|
|
SCB->VTOR = (uint32_t) (&__Vectors);
|
|
#elif defined (__GNUC__)
|
|
extern int *g_pfnVectors;
|
|
SCB->VTOR = (uint32_t) (&g_pfnVectors);
|
|
#endif
|
|
NVIC_SetPriorityGrouping(3); // 4 bit preemption, 0 bit of subprio
|
|
|
|
NVIC_DisableIRQ(TMR0_IRQn);
|
|
NVIC_DisableIRQ(TMR1_IRQn);
|
|
NVIC_DisableIRQ(ECAP0_IRQn);
|
|
NVIC_DisableIRQ(ECAP1_IRQn);
|
|
NVIC_DisableIRQ(ECAP2_IRQn);
|
|
NVIC_DisableIRQ(PWM0_TZ_IRQn);
|
|
NVIC_DisableIRQ(QEP0_IRQn);
|
|
|
|
NVIC_ClearPendingIRQ(TMR0_IRQn);
|
|
NVIC_ClearPendingIRQ(TMR1_IRQn);
|
|
NVIC_ClearPendingIRQ(ECAP0_IRQn);
|
|
NVIC_ClearPendingIRQ(ECAP1_IRQn);
|
|
NVIC_ClearPendingIRQ(ECAP2_IRQn);
|
|
NVIC_ClearPendingIRQ(PWM0_TZ_IRQn);
|
|
NVIC_ClearPendingIRQ(QEP0_IRQn);
|
|
|
|
/* Ïðåðûâàíèå 10 êÃö */
|
|
NVIC_EnableIRQ(TMR0_IRQn);
|
|
NVIC_SetPriority(TMR0_IRQn, IRQ_PRIORITY_10K);
|
|
|
|
/* Ïðåðûâàíèå 1 êÃö */
|
|
NVIC_EnableIRQ(TMR1_IRQn);
|
|
NVIC_SetPriority(TMR1_IRQn, IRQ_PRIORITY_1K);
|
|
|
|
NVIC_EnableIRQ(ECAP0_IRQn); //CAP0
|
|
NVIC_SetPriority(ECAP0_IRQn, IRQ_PRIORITY_CAP);
|
|
|
|
NVIC_EnableIRQ(ECAP1_IRQn); //CAP1
|
|
NVIC_SetPriority(ECAP1_IRQn, IRQ_PRIORITY_CAP);
|
|
|
|
NVIC_EnableIRQ(ECAP2_IRQn); //CAP2
|
|
NVIC_SetPriority(ECAP2_IRQn, IRQ_PRIORITY_CAP);
|
|
|
|
NVIC_EnableIRQ(PWM0_TZ_IRQn); //PDP (â èíâåðòîðå òåêñàñ íåòó òàêîãî ïèíà)
|
|
NVIC_SetPriority(PWM0_TZ_IRQn, IRQ_PRIORITY_TZ);
|
|
|
|
NVIC_EnableIRQ(QEP0_IRQn); //QEP
|
|
NVIC_SetPriority(QEP0_IRQn, IRQ_PRIORITY_EQEP);
|
|
|
|
sm_sys.state = SYS_READY;
|
|
|
|
gpioPeripheralInit();
|
|
#ifdef WATCHDOG_ON
|
|
WatchDog.init(WATCHDOG_PERIOD); //åñëè ñòîðîæåâîé òàéìåð èñïîëüçóåòñÿ, èíèöèàëèçèðóåì
|
|
#endif //WATCHDOG_ON
|
|
}
|
|
|
|
|
|
//!Áûñòðûé ðàñ÷åò (îáû÷íî 10êÃö).
|
|
|
|
//!Âûçîâ áûñòðûõ ðàñ÷åòîâ ìîäóëåé ñèñòåìû óïðàâëåíèÿ
|
|
//! \memberof TSM_Sys
|
|
void SM_Sys_Fast_Calc(TSM_Sys *p) {
|
|
sm_net.fast_calc(&sm_net);//ðàñ÷åò êîììóíèêàöèîííûõ äðàéâåðîâ
|
|
if (drv_params.sens_type == POS_SENSOR_TYPE_ENCODER) // òèïå äàò÷èêà âûáðàí ýíêîäåð
|
|
posspeedEqep.calc(&posspeedEqep); //ÄÏÐ ýíêîäåð
|
|
if (drv_params.sens_type == POS_SENSOR_TYPE_HALL){ // òèïå äàò÷èêà âûáðàí ÄÏÐ Õîëëà
|
|
DPReCAP.AngleCalc(&DPReCAP); //ÄÏÐ Õîëë, èíòåðïîëÿöèÿ óãëà ïîëîæåíèÿ (ðåçóëüòàò â DPReCAP.Angle)
|
|
DPReCAP.Angle6Calc(&DPReCAP); //ÄÏÐ Õîëë, ïîëó÷åíèå óãëà ñ òî÷íãñòüþ 60 ãðàäóñîâ (ðåçóëüòàò â DPReCAP.Angle6)
|
|
DPReCAP.SpeedCalc(&DPReCAP); //ÄÏÐ Õîëë, ðàñ÷åò ñêîðîñòè âðàùåíèÿ (DPReCAP.speed)
|
|
DPReCAP.calc_10k(&DPReCAP); //ÄÏÐ Õîëë, ñëóæåáíûå ôóíêöèè
|
|
}
|
|
if (drv_params.sens_type == POS_SENSOR_TYPE_SSI){ //Äàò÷èê ñ èíòåðôåéñîì SSI. Ïåðåä óïîòðåáëåíèåì ÷èòàéòå çàãîëîâî÷íûé ôàéë!
|
|
SSI_Encoder.calc(&SSI_Encoder);//Äàò÷èê ïîëîæåíèÿ SSI
|
|
}
|
|
|
|
dlog.update(&dlog); //Îñöèëëîãðàôèðîâàíèå äàííûõ
|
|
sm_prot.fast_calc(&sm_prot); //Çàùèòû
|
|
sm_ctrl.fast_calc(&sm_ctrl); //Ãëàâíûé äèñêðåòíûé àâòîìàò ñèñòåìû óïðàâëåíèÿ
|
|
udControl.calc(&udControl);//Ïëàâíàÿ çàðÿäêà ÇÏÒ ÷åðåç òåðìèñòîðû
|
|
|
|
#if defined (HW_MCB3_SIMULATOR)
|
|
//Ïåðåäà÷à òåêóùèõ ñêâàæíîñòåé òàéìåðîâ ØÈÌ â ìîäåëü
|
|
model.cmpr0 = PWM0->CMPA_bit.CMPA;
|
|
model.cmpr1 = PWM1->CMPA_bit.CMPA;
|
|
model.cmpr2 = PWM2->CMPA_bit.CMPA;
|
|
model.InvertorEna=pwm.Enabled;//Ôëàã ðàçðåøåíèÿ ðàáîòû èíâåðòîðà
|
|
model.Execute(&model);
|
|
#endif
|
|
|
|
global_time.calc(&global_time);
|
|
|
|
cur_par.calc(&cur_par); //Ðàñ÷åò òåêóùèõ ïîêàçàòåëåé ïðèâîäà
|
|
drv_params.sens_type = drv_params.sens_type & 7; //Îòñåêàåì âåðõíþþ ÷àñòü ïåðåìåííîé, òàì ìóñîð
|
|
|
|
#ifdef WATCHDOG_ON
|
|
WatchDog.calc();//åñëè ñòîðîæåâîé òàéìåð èñïîëüçóåòñÿ, ñáðàñûâàåì åãî çäåñü
|
|
#endif //WATCHDOG_ON
|
|
}
|
|
|
|
//!Ìèëëèñåêóíäíûé ðàñ÷åò 1êÃö.
|
|
|
|
//!Âûçîâ ðàñ÷åòîâ ìîäóëåé ñèñòåìû óïðàâëåíèÿ, òðåáóþùèõ ìèëëèñåêóíäíîé äèñêðåòèçàöèè
|
|
//! \memberof TSM_Sys
|
|
void SM_Sys_ms_Calc(TSM_Sys* p) {
|
|
sm_cmd_logic.ms_calc(&sm_cmd_logic); //Îáðàáîòêà êîìàíä óïðàâëåíèÿ
|
|
sm_net.ms_calc(&sm_net); //îáåðòêà äëÿ âûçîâà êîììóíèêàöèîííûõ äðàéâåðîâ
|
|
adc.ms_calc(&adc); //ÀÖÏ
|
|
sm_prot.ms_calc(&sm_prot); //Çàùèòû
|
|
AutoOffset.ms_calc(&AutoOffset); //àâòî ñìåùåíèå íåêîòîðûõ êàíàëîâ ÀÖÏ
|
|
//áëî÷íàÿ ïåðåäà÷à äðàéâåðà CANopen. ×åðåç íå¸, â ÷àñòíîñòè, ãðóçÿòñÿ îñöèëëîãðàììû dlog
|
|
Can1BTInterface.ms_calc(&Can1BTInterface, TMR2->VALUE, &co1_vars);
|
|
Can2BTInterface.ms_calc(&Can2BTInterface, TMR2->VALUE, &co2_vars);
|
|
leds.msCalc(&leds);//ñâåòîäèîäû
|
|
RTCclock.msCalc(&RTCclock);//÷àñû
|
|
UserMem.ms_calc(&UserMem);//ïîëüçîâàòåëüñêàÿ ïàìÿòü â ÌÊ
|
|
if ((drv_params.sens_type == 2) || (drv_params.sens_type == 3)){ // òèïå äàò÷èêà âûáðàí ÄÏÐ Õîëëà
|
|
DPReCAP.ms_calc(&DPReCAP);
|
|
}
|
|
global_time.ms_calc(&global_time);
|
|
|
|
DIO_fast_calc(); //äèñêðåòíûå âõîäû/âûõîäû
|
|
}
|
|
|
|
//!Ìåäëåííûé ðàñ÷åò (ôîíîâûé).
|
|
|
|
//!Âûçîâ ìåäëåííûõ ðàñ÷åòîâ îñòàëüíûõ ìîäóëåé
|
|
//! \memberof TSM_Sys
|
|
void SM_Sys_Slow_Calc(TSM_Sys *p) {
|
|
Can1BTInterface.slow_calc(&Can1BTInterface);//èíòåðôåéñ áëî÷íîé ïåðåäà÷è CANopen, ìåäëåííûé ðàñ÷åò
|
|
Can2BTInterface.slow_calc(&Can2BTInterface);//èíòåðôåéñ áëî÷íîé ïåðåäà÷è CANopen, ìåäëåííûé ðàñ÷åò
|
|
UserMem.slow_calc(&UserMem);//ïîëüçîâàòåëüñêàÿ ïàìÿòü â ÌÊ
|
|
RTCclock.slowCalc(&RTCclock);//÷àñû ðåàëüíîãî âðåìåíè
|
|
sm_prot.slow_calc(&sm_prot);//ìîäóëü çàùèò
|
|
sm_net.slow_calc(&sm_net); //îáåðòêà äëÿ âûçîâà êîììóíèêàöèîííûõ äðàéâåðîâ
|
|
dlog.background_analizer(&dlog); //ôîíîâûé îáðàáîò÷èê ìîäóëÿ îñöèëëîãðàôèðîâàíèÿ
|
|
global_time.slow_calc(&global_time);
|
|
rmp.slow_calc(&rmp); //ïåðåñ÷åò èíòåíñèâíîñòè ðàçãîíà èç ïîëüçîâòåëüñêîé âî âíóòðåííåå ïðåäñòàâëåíèå
|
|
pwm.slow_calc(&pwm); //ôîíîâûé îáðàáîò÷èê ìîäóëÿ ØÈÌ (ïåðåñ÷åò ïîëüçîâàòåëüñêèõ çàäàíèé)
|
|
adc.slow_calc(&adc); //Ôîíîâûé îáðàáîò÷èê ÀÖÏ
|
|
cur_par.slow_calc(&cur_par); //Ðàñ÷åò òåêóùèõ ïîêàçàòåëåé ïðèâîäà
|
|
AutoOffset.slow_calc(&AutoOffset);
|
|
posspeedEqep.slow_calc(&posspeedEqep);//èíèöèàëèçàöèÿ ìîäóëÿ ýíêîäåðà
|
|
RotorObserver.slow_calc(&RotorObserver);//íàáëþäàòåëü ïîòîêà ðîòîðà ÀÄ
|
|
DIO_slow_calc(); //äèñêðåòíûå âõîäû/âûõîäû
|
|
FanControl.slow_calc(&FanControl);//âåíòèëÿòîð
|
|
|
|
|
|
//Ïåðåñ÷åò êîýôôèöèåíòîâ äëÿ ìàñøòàáèðîâàíèÿ òîêîâ è íàïðÿæåíèé
|
|
drv_params._1_Udc_nom = _IQdiv(_IQ16(1), _IQ16(drv_params.Udc_nom));
|
|
drv_params._1_I_nom = _IQdiv(_IQ16(1), _IQ16(drv_params.I_nom));
|
|
drv_params._1_U_nom = _IQdiv(_IQ16(1), _IQ16(drv_params.U_nom));
|
|
|
|
if (sw.Reboot & 1) { //êîìàíäà ïåðåçàãðóçêè
|
|
sw.Reboot = 0; //ñáðàñûâàåì å¸
|
|
if (sm_ctrl.state == CTRL_STOP) { //ïåðåçàãðóæàåìñÿ òîëüêî â îñòàíîâå
|
|
//âûïîëíÿåì ñáðîñ ïðîöà
|
|
//Ïåðåçàãðóæàåò âî ôëåø!
|
|
Watchdog.resetCPU();
|
|
}
|
|
}
|
|
|
|
//âûçîâ ðàñ÷åòà ñìåùåíèÿ ÀÖÏ äëÿ òîêîâ ôàç
|
|
//Äåëàåì ðàñ÷åò òîëüêî â îñòàíîâå è îòñóòñòâèè àâàðèè
|
|
if ((sw.AutoOffset & 1) && (sm_ctrl.state == CTRL_STOP))
|
|
AutoOffset.Enabled=1;
|
|
else
|
|
AutoOffset.Enabled=0;
|
|
|
|
}
|
|
/*@}*/
|
|
|