ms/dist_tester
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

dodany kod c od Krzyska po wielu modyfikacjach przez Gemini zeby usunac zalezność od TI-RTOS

Browse Source
This commit is contained in:
Mirek
2026-02-19 11:59:33 +01:00
parent bf9c4596b8
commit d086ef28de
336 changed files with 52294 additions and 1006 deletions
Download Patch File Download Diff File Expand all files Collapse all files

365
src/e_mem.c Normal file
View File

@@ -0,0 +1,365 @@
/*
* 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;
}