204 lines
4.9 KiB
C
204 lines
4.9 KiB
C
/*
|
|
* detect_phase_break.c
|
|
*
|
|
* Created on: 10 äĺę. 2020 ă.
|
|
* Author: star
|
|
*/
|
|
|
|
#include "IQmathLib.h"
|
|
|
|
//#include "DSP281x_Examples.h" // DSP281x Examples Include File
|
|
//#include "DSP281x_Device.h" // DSP281x Headerfile Include File
|
|
|
|
#include "detect_phase_break2.h"
|
|
|
|
|
|
#define CONST_IQ_2PI 105414357 // 2*pi 360 ăđŕäóńîâ
|
|
#define CONST_IQ_3_2PI 79060767 // 4/3 pi 270 ăđŕäóńîâ
|
|
#define CONST_IQ_PI2 26353589 // 90 ăđŕäóńîâ
|
|
|
|
|
|
|
|
void check_brocken_phase(BREAK2_PHASE *v)
|
|
{
|
|
int i;
|
|
int ph_a=0, ph_b=0, ph_c=0;
|
|
_iq plus_a, max_a;
|
|
|
|
if ( (v->iqCh[0] >= v->iqCh[1] && v->iqCh[0] < v->iqCh[2])
|
|
|| (v->iqCh[0] >= v->iqCh[2] && v->iqCh[0] < v->iqCh[1]) )
|
|
ph_a = 1;
|
|
else
|
|
ph_a = -1;
|
|
|
|
if ( (v->iqCh[1] >= v->iqCh[0] && v->iqCh[1] < v->iqCh[2])
|
|
|| (v->iqCh[1] >= v->iqCh[2] && v->iqCh[1] < v->iqCh[0]) )
|
|
ph_b = 1;
|
|
else
|
|
ph_b = -1;
|
|
|
|
if ( (v->iqCh[2] >= v->iqCh[0] && v->iqCh[2] < v->iqCh[1])
|
|
|| (v->iqCh[2] >= v->iqCh[1] && v->iqCh[2] < v->iqCh[0]) )
|
|
ph_c = 1;
|
|
else
|
|
ph_c = -1;
|
|
|
|
|
|
// plus_a = _IQ(360.0/v->config.freq_pwm * v->freq_signal);
|
|
plus_a = _IQmpy(v->config.calc_const, v->freq_signal);
|
|
|
|
|
|
v->sum_brocken_out[0] += plus_a*ph_a;
|
|
v->sum_brocken_out[1] += plus_a*ph_b;
|
|
v->sum_brocken_out[2] += plus_a*ph_c;
|
|
|
|
v->plus_a = plus_a;
|
|
|
|
for (i=0;i<3;i++)
|
|
{
|
|
if (v->sum_brocken_out[i]>=CONST_IQ_2PI) v->sum_brocken_out[i] = CONST_IQ_2PI;
|
|
if (v->sum_brocken_out[i]<=0) v->sum_brocken_out[i] = 0;
|
|
|
|
if (v->sum_brocken_out[i]>CONST_IQ_3_2PI)
|
|
v->return_brocken_code |= (1<<i);
|
|
}
|
|
|
|
max_a = 0;
|
|
if (v->sum_brocken_out[0]>=max_a) max_a = v->sum_brocken_out[0];
|
|
if (v->sum_brocken_out[1]>=max_a) max_a = v->sum_brocken_out[1];
|
|
if (v->sum_brocken_out[2]>=max_a) max_a = v->sum_brocken_out[2];
|
|
v->sum_brocken_out[3] = max_a; // íŕřëč ěŕęńčěóě
|
|
|
|
}
|
|
|
|
|
|
|
|
void check_i_out_brocken(float freq)
|
|
{
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
//Ôóíęöčß âîçâđŕůŕĺň íîěĺđ ęŕíŕäŕ, ďî îďđĺäĺëčëńß îáđűâ ôŕçű
|
|
// 0 - íĺň îáđűâŕ
|
|
// 1- ôŕçŕ U
|
|
// 2- ôŕçŕ V
|
|
// 3- ôŕçŕ W
|
|
int calc_break2_phase(BREAK2_PHASE *v) {
|
|
|
|
// int field_direction = 1; //1 - forward, 0 - reverse
|
|
int err = 0;
|
|
|
|
if (v->freq_signal==0)
|
|
{
|
|
v->sum_brocken_out[0] = 0;
|
|
v->sum_brocken_out[1] = 0;
|
|
v->sum_brocken_out[2] = 0;
|
|
v->sum_brocken_out[3] = 0;
|
|
v->brocken_i_out = 0;
|
|
}
|
|
else
|
|
{
|
|
if (_IQabs(v->iqCh[0])>v->config.minimal_level
|
|
|| _IQabs(v->iqCh[1])>v->config.minimal_level
|
|
|| _IQabs(v->iqCh[2])>v->config.minimal_level )
|
|
{
|
|
check_brocken_phase(v);
|
|
}
|
|
else
|
|
{
|
|
|
|
v->iqCh[0] = 0;
|
|
v->iqCh[1] = 0;
|
|
v->iqCh[2] = 0;
|
|
|
|
check_brocken_phase(v);
|
|
|
|
}
|
|
}
|
|
|
|
// if (brocken_i_out & 0x1)
|
|
// error.power_errors.bit.phase_a_brocken |= 1;
|
|
// if (brocken_i_out & 0x2)
|
|
// error.power_errors.bit.phase_b_brocken |= 1;
|
|
// if (brocken_i_out & 0x4)
|
|
// error.power_errors.bit.phase_c_brocken |= 1;
|
|
//
|
|
// if(is_errors()) set_err_state();
|
|
|
|
|
|
//
|
|
// if (v->teta > CONST_IQ_2PI) {
|
|
// v->teta = CONST_IQ_2PI;
|
|
// }
|
|
// if(v->teta < 0) {
|
|
// v->teta = 0;
|
|
// }
|
|
// field_direction = calc_direction(v);
|
|
// if (v->iqImod < v->config.iqLevelZero) {
|
|
// clear_alg_vars(v);
|
|
// return 0;
|
|
// }
|
|
//
|
|
// if (_IQabs(v->iqIu) < v->config.iqLevelZero &&
|
|
// _IQabs(v->iqIv + v->iqIw) < v->config.iqLevelZero &&
|
|
// _IQabs(v->iqIv) > v->config.iqLevelZero && _IQabs(v->iqIw) > v->config.iqLevelZero) {
|
|
// err = calc_error_if_break(v, 0, field_direction);
|
|
// } else {
|
|
// v->latch_break_start[0] = 0;
|
|
// }
|
|
// if (_IQabs(v->iqIv) < v->config.iqLevelZero &&
|
|
// _IQabs(v->iqIu + v->iqIw) < v->config.iqLevelZero &&
|
|
// _IQabs(v->iqIu) > v->config.iqLevelZero && _IQabs(v->iqIw) > v->config.iqLevelZero) {
|
|
// err = calc_error_if_break(v, 1, field_direction);
|
|
// } else {
|
|
// v->latch_break_start[1] = 0;
|
|
// }
|
|
// if (_IQabs(v->iqIw) < v->config.iqLevelZero &&
|
|
// _IQabs(v->iqIv + v->iqIu) < v->config.iqLevelZero &&
|
|
// _IQabs(v->iqIv) > v->config.iqLevelZero && _IQabs(v->iqIu) > v->config.iqLevelZero) {
|
|
// err = calc_error_if_break(v, 2, field_direction);
|
|
// } else {
|
|
// v->latch_break_start[2] = 0;
|
|
// }
|
|
|
|
return err;
|
|
}
|
|
|
|
|
|
|
|
|
|
void init_break2_phase(BREAK2_PHASE *v)
|
|
{
|
|
v->config.iq_freq = _IQ(v->config.freq_pwm / v->config.norma_freq);
|
|
v->config.calc_const = _IQdiv(CONST_IQ_2PI, v->config.iq_freq);
|
|
v->return_brocken_code = 0;
|
|
|
|
|
|
}
|
|
|
|
void clear_break2_phase(BREAK2_PHASE *v)
|
|
{
|
|
|
|
v->iqCh[0] = 0;
|
|
v->iqCh[1] = 0;
|
|
v->iqCh[2] = 0;
|
|
v->sum_brocken_out[0] = 0;
|
|
v->sum_brocken_out[1] = 0;
|
|
v->sum_brocken_out[2] = 0;
|
|
v->sum_brocken_out[3] = 0;
|
|
v->brocken_i_out = 0;
|
|
|
|
v->return_brocken_code = 0;
|
|
}
|
|
|
|
|
|
|
|
|
|
|