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

278 lines
7.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 SMProtect.c
\brief Ìîäóëü çàùèò. (ñì. TSM_Protect)
\author ÎÎÎ "ÍÏÔ Âåêòîð". http://motorcontrol.ru
\version v 2.0 25/03/2016
\addtogroup SMProtect
@{ */
#include "DSP.h"
#include "main.h"
#include "stdlib.h"
int16 WriteCounter = 0;
//!Èíèöèàëèçàöèÿ
//!Ïðèñâàèâàíèå âñÿêèõ ïåðåìåííûõ
//! \memberof TSM_Protect
void SM_Protect_Init(TSM_Protect *p) {
p->state_prev = 0xff;
p->state = 0x00;
#if defined (HW_MOTORCONTROLBOARD)
/*
* Èíèöèàëèçàöèÿ êíîïêè ÑÁÐÎÑ ÀÂÀÐÈÉ íå òðåáóåòñÿ, òàê êàê ìû èñïîëüçóåì äëÿ ýòîãî äèñêðåòíûé âõîä
*/
#endif
}
//! Áûñòðûé ðàñ÷åò.
//!Îáðàáàòûâàåò âñå àâàðèè è ïðè èõ âîçíèêíîâåíèè îñòàíàâëèâàåò ïðèâîä.
//! \memberof TSM_Protect
void SM_Protect_Fast_Calc(TSM_Protect *p) {
//àïïàðàòíàÿ àâàðèÿ îò èíâåðòîðà
if (pwm.PDP_Fault) {
if ((sm_ctrl.state != CTRL_STOP) && (sm_ctrl.state != CTRL_RUN))
sm_prot.bit_fault1 |= F_PDPINT;
}
if (!DRV_FAULT) {
//èìååòñÿ êàêàÿ-òî àïïàðàòíàÿ àâàðèÿ
sm_prot.bit_fault1 |= F_PDPINT;
}
#if defined (HW_MOTORCONTROLBOARD)
if (!DRV_FAULT1) {
//èìååòñÿ êàêàÿ-òî àïïàðàòíàÿ àâàðèÿ
sm_prot.bit_fault2 |= F_PDP_SWITCH_A0;
}
if (!DRV_FAULT2) {
//èìååòñÿ êàêàÿ-òî àïïàðàòíàÿ àâàðèÿ
sm_prot.bit_fault2 |= F_PDP_SWITCH_A1;
}
if (!DRV_FAULT3) {
//èìååòñÿ êàêàÿ-òî àïïàðàòíàÿ àâàðèÿ
sm_prot.bit_fault2 |= F_PDP_SWITCH_A2;
}
if (!DRV_FAULT4) {
//èìååòñÿ êàêàÿ-òî àïïàðàòíàÿ àâàðèÿ
sm_prot.bit_fault2 |= F_PDP_SWITCH_LOWER;
}
#endif
/*Çàùèòà îò ïóñêà ïðè íåçàøóíòèðîâàííîì ðåëå ïëàâíîãî çàðÿäà ÇÏÒ*/
if (udControl.Enabled == 1) { //Åñëè åñòü öåïü øóíòèðîâàíèÿ ÇÏÒ, òî âûñòàâëÿåì àâàðèè ïî âûïîëíåíèþ óñëîâèé
if (udControl.fault_start == 1)
sm_prot.bit_fault1 |= F_RELAY_START;
}
/*Àâàðèÿ, åñëè ïðîãðàììà çàïèñàíà â çàãðóçî÷íóþ îáëàñòü ïàìÿòè (BFLASH)*/
if (programMemType == 0x4) {
sm_prot.bit_fault1 |= F_PROGRAM_IS_ON_BFLASH;
}
/*Çàùèòà ïî ìàêñèìàëüíîìó òîêó*/
if (adc.Imeas_a > sm_prot.Imax_protect)
sm_prot.bit_fault2 |= F_CTRL_MAX_I_PH_A;
if (adc.Imeas_a < -sm_prot.Imax_protect)
sm_prot.bit_fault2 |= F_CTRL_MAX_I_PH_A;
if (adc.Imeas_b > sm_prot.Imax_protect)
sm_prot.bit_fault2 |= F_CTRL_MAX_I_PH_B;
if (adc.Imeas_b < -sm_prot.Imax_protect)
sm_prot.bit_fault2 |= F_CTRL_MAX_I_PH_B;
if (adc.Imeas_c > sm_prot.Imax_protect)
sm_prot.bit_fault2 |= F_CTRL_MAX_I_PH_C;
if (adc.Imeas_c < -sm_prot.Imax_protect)
sm_prot.bit_fault2 |= F_CTRL_MAX_I_PH_C;
//ïðè ïðåâûøåíèè ñêîðîñòè âûøå àâàðèéíîé íîðìû
if (labs(cur_par.speed) > sm_prot.speed_max) {
sm_prot.bit_fault2 |= F_CTRL_SPEED_MAX; // ìàêñèììàëüíîå Ud
}
//ïðè ïðåâûøåíèè íàïðÿæåíèÿ âûøå àâàðèéíîé íîðìû
if (adc.Udc_meas > sm_prot.Umax_protect) {
sm_prot.bit_fault1 |= F_CTRL_HI_UDC; // ìàêñèììàëüíîå Ud
}
//ñíèæåíèå íàïðÿæåíèÿ
if (adc.Udc_meas < sm_prot.Umin_protect) { //åñëè ïðèâîä ðàáîòàë òî ëîâèì àâàðèþ
if (sm_ctrl.state != CTRL_STOP) {
sm_prot.bit_fault1 |= F_CTRL_LOW_UDC;
}
}
//ïðè ïðåâûøåíèè òåìïåðàòóðû âûøå àâàðèéíîé íîðìû
if (adc.T_meas > sm_prot.T_max) {
sm_prot.bit_fault1 |= F_CTRL_HI_TEMP; // ìàêñèìàëüíàÿ òåìïåðàòóðà
}
#if defined(HW_VECTORCARD_SIMULATOR) || defined(HW_NIIET_BOARD_SIMULATOR)
if(model.fault){
sm_prot.bit_fault1 |= F_MODEL_FAULT;
}
#endif
DINT; //Çàïðåùåíèå ïðåðûâàíèé
//Àâàðèè, òðåáóþùèå ïîëíîãî îñòàíîâà
p->masked_bit_fault1 = p->bit_fault1 & p->mask_fault1;//ìàñêèðîâàíèå ôëàãîâ àâàðèé
p->masked_bit_fault2 = p->bit_fault2 & p->mask_fault2;
EINT; //Ðàçðåøåíèå ïðåðûâàíèé
//ñ÷èòàåì äèñêðåòíûé àâòîìàò çàùèò
if (p->state_prev != p->state) //ñìåíà ñîñòîÿíèÿ?
p->E = 1;//Âîçâîäèì ôëàã ïåðâîãî âõîæäåíèÿ "entry"
else
p->E = 0;//èíà÷å ñáðàñûâàåì
p->state_prev = p->state;
switch (p->state) {//â çàâèñèìîñòè îò òåêóùåãî ñîñòîÿíèÿ
case PROT_OFF: { //Çàùèòà âûêëþ÷åíà
if (p->E == 1) { //Ïåðâîå âõîæäåíèå
}
//Îáíóëÿåì âñå àâàðèè
p->bit_fault1 = 0;
p->bit_fault2 = 0;
//Ïðîïóñêàåì íåêîòîðûé òàéìàóò ïîñëå âêëþ÷åíèÿ êîíòðîëëåðà
//÷òîáû íå ëîâèòü ëîæíûå ñðàáàòûâàíèÿ àâàðèé ÀÖÏ
if (p->powered_okCounter++ > 30000) {
p->state = PROT_ON_OK;
}
break;
}
case PROT_ON_OK: { //Íîðìà
if (p->E == 1) { //Ïåðâîå âõîæäåíèå
drv_status.bit.fault = 0;
}
//åñòü àâàðèè?
if ((p->masked_bit_fault1 | p->masked_bit_fault2) != 0) {
p->state = PROT_FAIL; //ïåðåõîäèì â ñîñòîÿíèå àâàðèè
}
break;
}
case PROT_FAIL: { //ñîñòîÿíèå àâàðèè (ñðàáîòàëà çàùèòà)
if (p->E == 1) {
cmd.all = 0;
}
drv_status.bit.fault = 1;
//Âûêëþ÷åíèå ØÈÌ. Ïîêà òàì âñå îñòàëüíûå äèñêðåòíûå àâòîìàòû ïðîáóðëÿòñÿ, ÷òîáû èõ íå æäàòü
pwm.Off(&pwm);
//ëîãèêà ñáîðîñà àâàðèè
if ((cmd.bit.trip_reset == 1) || (RESET_FAULT_BUTTON == 1)) {//êîìàíäà íà ñáðîñ àâàðèè
p->state = PROT_ON_OK;//èäåì â "íîðìà"
p->clearDrvFault = 1; //îòïðàâèòü äðàéâåðó êëþ÷åé êîìàíäó ñáðîñà
//îáíóëÿåì âñå ôëàãè àâàðèé
p->bit_fault1 = 0;
p->bit_fault2 = 0;
cmd.all = 0;//êîìàíäíîå ñëîâî
if (udControl.Enabled == 1) { //Åñëè åñòü öåïü øóíòèðîâàíèÿ ÇÏÒ, òî ñáðàñûâàåì âîçìîæíûå ñîñòîÿíèÿ àâàðèé
udControl.fault_start = 0;
}
//â ñàìîì íèçó!!! ïûòàåìñÿ ñáðîñèòü ôëàã pdp àïïàðàòíûé
//åñëè àâàðèÿ âñå åùå èìååòñÿ, òî ñðàçó ïðîèçîéäåò ïðåðûâàíèå
//è âîçâåäåòñÿ ôëàã àâàðèè
if (PWM0->TZFLG_bit.OST == 1) { //èìååòñÿ ôëàã àïïàðàòíîé àâàðèè
//ôëàã ìîæíî ñáðàñûâàòü
PWM0->TZCLR = 0x7;
PWM1->TZCLR = 0x7;
PWM2->TZCLR = 0x7;
}
#if defined(HW_VECTORCARD_SIMULATOR) || defined(HW_NIIET_BOARD_SIMULATOR)
model.fault=0;
#endif
}
break;
}
}
}
//! \memberof TSM_Protect
void SM_Protect_ms_Calc(TSM_Protect *p) {
if (WriteCounter <= 15) //åñëè ïåðâîå ñëîâî
{
if (((sm_prot.masked_bit_fault1 >> WriteCounter) & 0x1) != 0) //ñòîèò ii-é ôëàã àâàðèè
{
if (((sm_prot.bit_fault_written1 >> WriteCounter) & 0x1) == 0) //è îíà íå çàïèñàíà
{
FaultLog.write(&FaultLog, WriteCounter + 1);
sm_prot.bit_fault_written1 |= (1 << WriteCounter);
}
}
else
sm_prot.bit_fault_written1 &= ~(1 << WriteCounter);
}
else if (WriteCounter <= 31) //âòîðîå ñëîâî
{
if (((sm_prot.masked_bit_fault2 >> (WriteCounter - 16)) & 0x1) != 0) //ñòîèò ii-é ôëàã àâàðèè
{
if (((sm_prot.bit_fault_written2 >> (WriteCounter - 16)) & 0x1) == 0) //è îíà òîëüêî ÷òî ïîßâèëàñü
{
FaultLog.write(&FaultLog, WriteCounter + 1);
sm_prot.bit_fault_written2 |= (1 << (WriteCounter - 16));
}
}
else
sm_prot.bit_fault_written2 &= ~(1 << (WriteCounter - 16));
}
WriteCounter++;
if (WriteCounter >= (33 - 1))
WriteCounter = 0;
}
//! Ìåäëåííûé ðàñ÷åò.
//! \memberof TSM_Protect
void SM_Protect_Slow_Calc(TSM_Protect *p) {
/* Ïðîâåðêà àïïàðàòíûõ çàùèò äðàéâåðà. Îí àâòîìàòè÷åñêè âûðóáàåò êëþ÷è ïðè àâàðèÿõ,
* è îïóñêàåò ñèãíàëû /FAULT è /OCTW, íî ÷òîáû ïîíÿòü, ÷òî èìåííî ñëó÷èëîñü, íàäî ïî SPI ïðî÷èòàòü ñòàòóñû.
* Ïîýòîìó, âèäèìî, ìîæíî â ôîíå.
*/
if (p->readDrvSts == 1) {
p->readDrvSts = 0;
drv8301.read(&drv8301);
}
/* ×òîáû ñáðîñèòü àâàðèþ, íàäî ïðîïèñàòü åìó áèòèê GATE_RESET */
if (p->clearDrvFault == 1) {
p->clearDrvFault = 0;
drv8301.CTRL1.bit.GATE_RESET = 1;
drv8301.write(&drv8301, CNTRL_REG_1_ADDR);
drv8301.write(&drv8301, CNTRL_REG_2_ADDR);
}
}
/*@}*/