Razvalyaev
7e0063eee0
Все основные файлы подтянуты без изменений Изменены (только папка main_matlab): - заглушки для ненужных функций (main_matlab.c) - iq библиотека (IQmathLib_matlab.c) - библиотеки DSP281x
113 lines
3.3 KiB
C
113 lines
3.3 KiB
C
/*
|
|
* detect_phase_break.c
|
|
*
|
|
* Created on: 10 äåê. 2020 ã.
|
|
* Author: star
|
|
*/
|
|
|
|
#include "DSP281x_Examples.h" // DSP281x Examples Include File
|
|
#include "DSP281x_Device.h" // DSP281x Headerfile Include File
|
|
#include "IQmathLib.h"
|
|
|
|
#include <detect_phase_break.h>
|
|
|
|
|
|
#define CONST_IQ_2PI 105414357 // 360 ãðàäóñîâ
|
|
#define CONST_IQ_PI2 26353589 // 90 ãðàäóñîâ
|
|
|
|
|
|
static void clear_alg_vars(BREAK_PHASE_I *v);
|
|
static int calc_direction(BREAK_PHASE_I *v);
|
|
static int calc_error_if_break(BREAK_PHASE_I *v, int num, int field_direction);
|
|
|
|
//Ôóíêöèÿ âîçâðàùàåò íîìåð êàíàäà, ïî îïðåäåëèëñÿ îáðûâ ôàçû
|
|
// 0 - íåò îáðûâà
|
|
// 1- ôàçà U
|
|
// 2- ôàçà V
|
|
// 3- ôàçà W
|
|
int calc_break_I_phase(BREAK_PHASE_I *v) {
|
|
|
|
int field_direction = 1; //1 - forward, 0 - reverse
|
|
int err = 0;
|
|
|
|
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 clear_alg_vars(BREAK_PHASE_I *v) {
|
|
int i = 0;
|
|
for (i = 0; i < 3; i++) {
|
|
v->latch_break_start[i] = 0;
|
|
v->latched_teta[i] = 0;
|
|
}
|
|
}
|
|
|
|
int calc_direction(BREAK_PHASE_I *v) {
|
|
int direction = 1;
|
|
if (v->teta > v->prev_teta) {
|
|
if (v->teta - v->prev_teta < CONST_IQ_PI2) { direction = 1;}
|
|
else { direction = 0;}
|
|
} else {
|
|
if (v->prev_teta - v->teta < CONST_IQ_2PI) { direction = 0;}
|
|
else { direction = 1;}
|
|
}
|
|
v->prev_teta = v->teta;
|
|
return direction;
|
|
}
|
|
|
|
int calc_error_if_break(BREAK_PHASE_I *v, int num, int field_direction) {
|
|
int err = 0;
|
|
if (v->latch_break_start[num] == 0) {
|
|
v->latch_break_start[num] = 1;
|
|
v->latched_teta[num] = v->teta;
|
|
} else {
|
|
if (field_direction == 0) {
|
|
if (v->latched_teta[num] > v->teta) {
|
|
err = v->latched_teta[num] - v->teta > CONST_IQ_PI2 ? num + 1 : 0;
|
|
} else {
|
|
err = v->teta - v->latched_teta[num] < CONST_IQ_PI2 - CONST_IQ_2PI ? num + 1 : 0;
|
|
}
|
|
} else {
|
|
if (v->latched_teta[num] < v->teta) {
|
|
err = v->teta - v->latched_teta[num] > CONST_IQ_PI2 ? num + 1 : 0;
|
|
} else {
|
|
err = v->latched_teta[num] - v->teta < CONST_IQ_PI2 - CONST_IQ_2PI ? num + 1 : 0;
|
|
}
|
|
}
|
|
}
|
|
return err;
|
|
}
|