366 lines
9.7 KiB
C
366 lines
9.7 KiB
C
/*
|
|
* e_mem.c
|
|
*
|
|
* Created on: 22-03-2017
|
|
* Author: Krzysztof Jakubczyk
|
|
*/
|
|
|
|
#include <math.h>
|
|
|
|
#include "tdefs.h"
|
|
#include "misc.h"
|
|
|
|
#include "helper.h"
|
|
#include "e_mem.h"
|
|
|
|
int e_mem_initlog(void *arguments, void *logic)
|
|
{
|
|
struct e_mem_args *args = (struct e_mem_args *)arguments;
|
|
struct e_mem_logic *log = (struct e_mem_logic *)logic;
|
|
|
|
if(set_float_ptr(args->io.orta_float_in,&log->orta))
|
|
return -1;
|
|
if(set_float_ptr(args->io.ortb_float_in,&log->ortb))
|
|
return -1;
|
|
if(set_float_ptr(args->io.esk_float_in,&log->esk))
|
|
return -1;
|
|
|
|
if(set_float_ptr(args->io.orta_float_out,&log->orta_out))
|
|
return -1;
|
|
if(set_float_ptr(args->io.ortb_float_out,&log->ortb_out))
|
|
return -1;
|
|
if(set_float_ptr(args->io.esk_float_out,&log->esk_out))
|
|
return -1;
|
|
|
|
if(set_bit_ptr_struct(args->io.memory_ok_out,&log->memory_OK))
|
|
return -1;
|
|
if(set_bit_ptr_struct(args->io.memory_on_out,&log->memory_ON))
|
|
return -1;
|
|
|
|
log->nast_.Ur_ = args->params.napiecie_ * args->params.napiecie_; // wyliczenie wartosci granicznej
|
|
log->nast_.Up_ = log->nast_.Ur_ * (float)0.81; // wyliczenia wartosci powrotu dla wsp 0.9
|
|
log->nast_.licz_t_ = (unsigned short)(args->params.czas_ * 100); // wyliczenie ilosci petli dla czasu pamieci
|
|
log->korekcja_orta = 1;
|
|
log->korekcja_ortb = 0;
|
|
log->U_OK = 0;
|
|
log->idx_buf = log->idx_buf1 = 0;
|
|
log->stan_pamieci = oczekiwanie_na_OK;
|
|
log->licz_pow = 0;
|
|
log->index_buf_T = 0;
|
|
|
|
return 0;
|
|
}
|
|
|
|
|
|
void oblicz_okres(struct e_mem_logic *log,struct dane_probki* we)
|
|
{
|
|
float T;
|
|
float local1;
|
|
u8 OK = 1;
|
|
float local3;
|
|
|
|
|
|
OK = log->U_OK;
|
|
if (we->flag)
|
|
{
|
|
|
|
we->flag = 0;
|
|
T = we->reszta; // pozostalosc z poprzedniego wyliczania
|
|
T += (we->il_p) * 2500 * LOOP_CYCLE_MS;//dodanie calych okresow probkowania
|
|
local1 = (float)((float)(2500*LOOP_CYCLE_MS) * we->minus1 / (we->minus1 - we->bie));//wyliczenie czasu dopelnienia
|
|
T += local1; // wyliczenie okresu
|
|
|
|
|
|
if (T > 52700) OK = 0; // jesli wynik mniejszy niz 47,5 Hz
|
|
if (T < 47300) OK = 0; // jesli wynik wiekszy niz 52,5 Hz
|
|
|
|
local3 = fabs((float)we->T_last - T);
|
|
if (local3>(30*LOOP_CYCLE_MS)) OK = 0; //jesli zmiana wieksza niz 30mHz to traktujemy wynik za niepewny
|
|
|
|
we->T_last = T;
|
|
we->reszta = (2500*LOOP_CYCLE_MS) - local1; // wyliczenie reszty do nastepnego okresu
|
|
}
|
|
else
|
|
{
|
|
|
|
OK = 0;
|
|
T = 0;
|
|
}
|
|
|
|
if (we->flag) OK = 0;
|
|
log->buf_T[log->index_buf_T].T = T;
|
|
log->buf_T[log->index_buf_T].OK = OK;
|
|
//
|
|
// log->buf_T[log->index_buf_T].OK=1;
|
|
// log->buf_T[log->index_buf_T].T = 50000;
|
|
//
|
|
|
|
log->index_buf_T++;
|
|
if (log->index_buf_T>19)
|
|
{
|
|
log->index_buf_T = 0;
|
|
}
|
|
}
|
|
|
|
void e_mem(void *arguments, void *logic)
|
|
{
|
|
struct e_mem_logic *log = (struct e_mem_logic *)logic;
|
|
|
|
short idx_temp;
|
|
|
|
log->w.kwampl_ = *log->esk;
|
|
log->w.orta_ = *log->orta;
|
|
log->w.ortb_ = *log->ortb;
|
|
|
|
//wykrywanie przejscia przez zero
|
|
|
|
if ((log->dane.minus1>0)&&(log->w.orta_<=0)) //przejscie z plusa do minusa
|
|
{
|
|
log->dane.plus.flag = 1;
|
|
log->dane.plus.il_p = log->dane.plus_il_p;
|
|
log->dane.plus_il_p = 0;
|
|
log->dane.plus.bie = log->w.orta_;
|
|
log->dane.plus.minus1 = log->dane.minus1;
|
|
|
|
} else {
|
|
log->dane.plus_il_p++;//=LOOP_CYCLE_MS;
|
|
}
|
|
|
|
if ((log->dane.minus1<0)&&(log->w.orta_>=0)) //przejscie z minusa do plusa
|
|
{
|
|
|
|
log->dane.minus.flag = 1;
|
|
log->dane.minus.il_p = log->dane.minus_il_p;
|
|
log->dane.minus_il_p = 0;
|
|
log->dane.minus.bie = log->w.orta_;
|
|
log->dane.minus.minus1 = log->dane.minus1;
|
|
} else {
|
|
log->dane.minus_il_p++;//=LOOP_CYCLE_MS;
|
|
}
|
|
log->dane.minus1 = log->w.orta_;
|
|
//--------------------------------------------------------------------
|
|
|
|
|
|
sprawdz_P(
|
|
&log->U_OK,
|
|
log->w.kwampl_>log->nast_.Ur_,
|
|
log->w.kwampl_<log->nast_.Up_,
|
|
&log->licz_pow,
|
|
72,2
|
|
);
|
|
|
|
|
|
|
|
log->stan_.memory_ON_ = 0;
|
|
log->stan_.memory_OK_ = 0;
|
|
|
|
switch(log->stan_pamieci)
|
|
{
|
|
case oczekiwanie_na_OK:
|
|
//aktualizacja buforow danych
|
|
log->buf_orta[log->idx_buf][0] = log->w.orta_;
|
|
log->buf_ortb[log->idx_buf][0] = log->w.ortb_;
|
|
if(log->f.OK) log->f_kor = log->f.T;
|
|
|
|
if(log->f.OK && log->U_OK)
|
|
{
|
|
log->stan_pamieci = napiecie_OK_0;
|
|
log->stan_.memory_OK_ = 1;
|
|
}
|
|
log->stan_.orta_ = log->w.orta_;
|
|
log->stan_.ortb_ = log->w.ortb_;
|
|
log->stan_.kwampl_ = log->w.kwampl_;
|
|
break;
|
|
|
|
case napiecie_OK_0:
|
|
//wystawienie wyjsc
|
|
idx_temp = log->idx_buf - (MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS);
|
|
if(idx_temp<0) idx_temp += MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS*2;
|
|
log->stan_.orta_ = log->buf_orta[idx_temp][0];
|
|
log->stan_.ortb_ = log->buf_ortb[idx_temp][0];
|
|
log->stan_.kwampl_ = (log->stan_.orta_ * log->stan_.orta_ +
|
|
log->stan_.ortb_ * log->stan_.ortb_) * 0.5f;
|
|
//
|
|
//aktualizacja buforow danych
|
|
if (log->U_OK)
|
|
{
|
|
log->buf_orta[log->idx_buf][0] = log->w.orta_;
|
|
log->buf_ortb[log->idx_buf][0] = log->w.ortb_;
|
|
if (log->f.OK)
|
|
{
|
|
log->f_kor = log->f.T;
|
|
}
|
|
} else {
|
|
log->stan_pamieci = napiecie_z_pamieci_0;
|
|
log->idx_buf_start = log->idx_buf;
|
|
log->idx_buf1 = 0;
|
|
}
|
|
log->stan_.memory_OK_ = 1;
|
|
break;
|
|
|
|
case napiecie_OK_1:
|
|
//wystawienie wyjsc
|
|
idx_temp = log->idx_buf - (MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS);
|
|
if(idx_temp<0) idx_temp +=(MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS)*2;
|
|
log->stan_.orta_ = log->buf_orta[idx_temp][1];
|
|
log->stan_.ortb_ = log->buf_ortb[idx_temp][1];
|
|
log->stan_.kwampl_ = (log->stan_.orta_ * log->stan_.orta_ +
|
|
log->stan_.ortb_ * log->stan_.ortb_) * 0.5f;
|
|
//
|
|
//aktualizacja buforow danych
|
|
if (log->U_OK)
|
|
{
|
|
log->buf_orta[log->idx_buf][1] = log->w.orta_;
|
|
log->buf_ortb[log->idx_buf][1] = log->w.ortb_;
|
|
if (log->f.OK)
|
|
{
|
|
log->f_kor = log->f.T;
|
|
}
|
|
} else {
|
|
log->stan_pamieci = napiecie_z_pamieci_1;
|
|
log->idx_buf_start = log->idx_buf;
|
|
log->idx_buf1 = 0;
|
|
}
|
|
log->stan_.memory_OK_ = 1;
|
|
break;
|
|
|
|
case napiecie_z_pamieci_0:
|
|
//aktualizacja buforow danych drugiego banku danych
|
|
log->buf_orta[log->idx_buf][1] = log->w.orta_;
|
|
log->buf_ortb[log->idx_buf][1] = log->w.ortb_;
|
|
if(log->f.OK) log->f_kor_new = log->f.T;
|
|
if(log->f.OK && log->U_OK)
|
|
{
|
|
log->stan_pamieci = napiecie_OK_1;
|
|
log->f_kor = log->f_kor_new;
|
|
log->stan_.memory_OK_ = 1;
|
|
}
|
|
//wystawienie wyjsc
|
|
|
|
log->idx_buf1++; // przesuniecie pozycji w buforze probek
|
|
if(log->idx_buf1>(MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS)-1)log->idx_buf1 = 0;
|
|
idx_temp = log->idx_buf_start + log->idx_buf1;
|
|
if(idx_temp>(MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS)*2-1) idx_temp -= (MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS)*2;
|
|
log->stan_.orta_ = log->buf_orta[idx_temp][0] *log->korekcja_orta
|
|
- log->buf_ortb[idx_temp][0] * log->korekcja_ortb
|
|
; // wyliczenie skladowej ortogonalnej a wyniku
|
|
log->stan_.ortb_ = log->buf_ortb[idx_temp][0] * log->korekcja_orta
|
|
+ log->buf_orta[idx_temp][0] * log->korekcja_ortb
|
|
; // wyliczenie skladowej ortogonalnej b wyniku
|
|
log->stan_.kwampl_ = (log->stan_.orta_ * log->stan_.orta_ +
|
|
log->stan_.ortb_ * log->stan_.ortb_) * 0.5f;
|
|
|
|
//
|
|
log->stan_.memory_ON_ = 1;
|
|
break;
|
|
|
|
case napiecie_z_pamieci_1:
|
|
//aktualizacja buforow danych drugiego banku danych
|
|
log->buf_orta[log->idx_buf][0] = log->w.orta_;
|
|
log->buf_ortb[log->idx_buf][0] = log->w.ortb_;
|
|
if(log->f.OK) log->f_kor_new = log->f.T;
|
|
if(log->f.OK && log->U_OK)
|
|
{
|
|
log->stan_pamieci = napiecie_OK_0;
|
|
log->f_kor = log->f_kor_new;
|
|
log->stan_.memory_OK_ = 1;
|
|
}
|
|
//wystawienie wyjsc
|
|
|
|
log->idx_buf1++; // przesuniecie pozycji w buforze probek
|
|
if(log->idx_buf1>(MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS)-1)log->idx_buf1 = 0;
|
|
idx_temp = log->idx_buf_start + log->idx_buf1;
|
|
if(idx_temp>(MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS)*2-1) idx_temp -= (MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS)*2;
|
|
log->stan_.orta_ = log->buf_orta[idx_temp][1] * log->korekcja_orta
|
|
- log->buf_ortb[idx_temp][1] * log->korekcja_ortb
|
|
; // wyliczenie skladowej ortogonalnej a wyniku
|
|
log->stan_.ortb_ = log->buf_ortb[idx_temp][1] * log->korekcja_orta
|
|
+ log->buf_orta[idx_temp][1] * log->korekcja_ortb
|
|
; // wyliczenie skladowej ortogonalnej b wyniku
|
|
log->stan_.kwampl_ = (log->stan_.orta_ * log->stan_.orta_ +
|
|
log->stan_.ortb_ * log->stan_.ortb_) * 0.5f;
|
|
|
|
//
|
|
log->stan_.memory_ON_ = 1;
|
|
break;
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
log->idx_buf++; // przesuniecie pozycjiw buforze probek
|
|
if(log->idx_buf>(MAIN_FREQ_PERIOD_MS/LOOP_CYCLE_MS)*2-1)log->idx_buf = 0;
|
|
|
|
check_and_set_struct(log->stan_.memory_OK_,&log->memory_OK);
|
|
check_and_set_struct(log->stan_.memory_ON_,&log->memory_ON);
|
|
*log->esk_out=log->stan_.kwampl_;
|
|
*log->orta_out=log->stan_.orta_;
|
|
*log->ortb_out=log->stan_.ortb_;
|
|
}
|
|
|
|
void e_mem_100hz(void *arguments, void *logic)
|
|
{
|
|
struct e_mem_logic *log = (struct e_mem_logic *)logic;
|
|
|
|
float kat_przesuniecia;
|
|
float a = 1;
|
|
float b = 0;
|
|
unsigned short idx = 0;
|
|
float suma = 0;
|
|
unsigned short i;
|
|
|
|
//jesli nie wykryto przejsc przez zero zbyt dlugo
|
|
if (log->dane.minus_il_p > 30/LOOP_CYCLE_MS)
|
|
{
|
|
log->dane.minus_il_p = 0;
|
|
}
|
|
if (log->dane.plus_il_p > 30/LOOP_CYCLE_MS)
|
|
{
|
|
log->dane.plus_il_p = 0;
|
|
}
|
|
//
|
|
|
|
oblicz_okres(log,&log->dane.plus);
|
|
oblicz_okres(log,&log->dane.minus);
|
|
|
|
//wyliczenie jako srenia waona
|
|
|
|
for (i=0;i<20;i++)
|
|
{
|
|
if (log->buf_T[i].OK)
|
|
{
|
|
idx++;
|
|
suma += (float)log->buf_T[i].T;
|
|
}
|
|
}
|
|
if (idx>8)
|
|
{
|
|
log->f.T = (float)idx * (2500000) / suma;
|
|
log->f.OK = 1;
|
|
}
|
|
else
|
|
{
|
|
log->f.OK = 0;
|
|
log->f.T = 0;
|
|
}
|
|
|
|
|
|
if (log->stan_.memory_ON_)
|
|
{
|
|
log->licznik_czasu++; // odliczanie czasu aktywnosci pamieci
|
|
if(log->licznik_czasu>log->nast_.licz_t_)
|
|
{
|
|
log->stan_pamieci = oczekiwanie_na_OK;// wylaczenie pamieci po czasie
|
|
log->licz_pow = 0;
|
|
}
|
|
kat_przesuniecia = ((float)3.1416 *log->licznik_czasu + (float)0.62832)*(1-log->f_kor*0.02f);
|
|
a = cos(kat_przesuniecia);
|
|
b = sin(kat_przesuniecia);
|
|
} else {
|
|
log->licznik_czasu = 0;
|
|
}
|
|
log->korekcja_orta = a;
|
|
log->korekcja_ortb = b;
|
|
}
|