запущен проект motor identification c терминалкой
This commit is contained in:
581
Inu_im_1wnd_3lvl/Inu/estimate.c
Normal file
581
Inu_im_1wnd_3lvl/Inu/estimate.c
Normal file
@@ -0,0 +1,581 @@
|
||||
/*
|
||||
* Файл: estimate.c
|
||||
* Модуль идентификации параметров асинхронного двигателя
|
||||
*/
|
||||
|
||||
#include "estimate.h"
|
||||
#include "def.h"
|
||||
#include <math.h>
|
||||
#include <string.h>
|
||||
|
||||
#ifndef M_PI
|
||||
#define M_PI 3.14159265358979323846f
|
||||
#endif
|
||||
|
||||
/*
|
||||
* Настройки эксперимента по определению активного сопротивления статора Rs
|
||||
* Метод: разностный метод с двумя уровнями постоянного тока
|
||||
*/
|
||||
float RS_IREF1 = 0.1f; /* первый уровень тока, А */
|
||||
float RS_IREF2 = 0.2f; /* второй уровень тока, А */
|
||||
float RS_SETTLE_TIME = 0.5f; /* время установления тока, с */
|
||||
float RS_THRESHOLD = 0.98f; /* относительный порог достижения тока */
|
||||
|
||||
/*
|
||||
* Настройки эксперимента по определению сопротивления ротора Rr
|
||||
* и индуктивностей рассеяния Llr, Lls
|
||||
* Метод: подача переменного тока по оси q, RMS измерения
|
||||
*/
|
||||
float RRL_IQ_AMP = 0.2f; /* амплитуда тока, А */
|
||||
float RRL_RAMP_TIME = 2.0f; /* время нарастания амплитуды, с */
|
||||
float RRL_FREQ = 1.0f; /* частота тестового сигнала, Гц (дефолт) */
|
||||
int RRL_SAMPLES = 200; /* количество выборок для RMS */
|
||||
int RRL_AVG = 10; /* количество усреднений на частоте */
|
||||
|
||||
/* Частоты для многоточечной идентификации (Гц) */
|
||||
#define RRL_FREQ_COUNT 5
|
||||
static const float RRL_FREQS[RRL_FREQ_COUNT] = { 1.0f, 2.0f, 3.0f, 5.0f, 10.0f };
|
||||
|
||||
/*
|
||||
* Настройки эксперимента по определению взаимной индуктивности Lm
|
||||
* Метод: подача постоянного тока по оси d, интегрирование ЭДС на фронте
|
||||
*/
|
||||
float LM_IDREF = 0.3f; /* заданный постоянный ток, о.е. */
|
||||
float LM_SETTLE_THRESHOLD = 0.95f; /* порог окончания переходного процесса */
|
||||
float LM_SETTLE_TIME = 0.5f; /* выдержка для усреднения Id, с */
|
||||
float LM_DEMAG_TIME = 0.3f; /* размагничивание между циклами, с */
|
||||
int LM_AVG = 5; /* количество измерений для усреднения */
|
||||
|
||||
/*
|
||||
* Настройки переключения между экспериментами
|
||||
*/
|
||||
float STEP_PAUSE_TIME = 0.5f;
|
||||
|
||||
/* Текущий шаг идентификации */
|
||||
static Estimate_Test_t test_step = ESTIMATE_TEST_IDLE;
|
||||
|
||||
/* Структура с вычисленными параметрами двигателя */
|
||||
static Params_t params = { 0 };
|
||||
|
||||
/* ============================================
|
||||
*
|
||||
* ============================================ */
|
||||
static RsState_t rs_state = {0};
|
||||
static RrlState_t rrl_state = {0};
|
||||
static LmState_t lm_state = {0};
|
||||
static int in_pause;
|
||||
static float pause_timer;
|
||||
|
||||
/* ============================================
|
||||
*
|
||||
* ============================================ */
|
||||
|
||||
/*
|
||||
* Vd/Vq
|
||||
* : v - ( )
|
||||
* :
|
||||
*/
|
||||
static inline float vdq_to_volts(float vq) {
|
||||
float vq_oe = vq / T1_PRD;
|
||||
float dt_oe = DT / T_PWM;
|
||||
if (vq_oe > dt_oe) vq_oe -= dt_oe;
|
||||
else if (vq_oe < -dt_oe) vq_oe += dt_oe;
|
||||
return vq_oe * U_BAZ / SQRT3;
|
||||
}
|
||||
|
||||
/*
|
||||
* Iq
|
||||
* : iq -
|
||||
* :
|
||||
*/
|
||||
static inline float idq_to_amps(float iq) {
|
||||
return iq * I_BAZ;
|
||||
}
|
||||
|
||||
/* ============================================
|
||||
*
|
||||
* ============================================ */
|
||||
|
||||
/*
|
||||
*
|
||||
*
|
||||
*/
|
||||
void estimate_init(void) {
|
||||
params.Rs = 0.0192f;
|
||||
params.Rr = 0.0085f;
|
||||
params.Lls = 0.0019364f;
|
||||
params.Llr = 0.0019364f;
|
||||
params.Lk = params.Llr + params.Lls;
|
||||
params.Zk = 0;
|
||||
params.Rk = params.Rs+ params.Rr;
|
||||
params.Xk = 0;
|
||||
params.Lm = 0;
|
||||
|
||||
memset(&rs_state, 0, sizeof(RsState_t));
|
||||
memset(&rrl_state, 0, sizeof(RrlState_t));
|
||||
memset(&lm_state, 0, sizeof(LmState_t));
|
||||
in_pause = 0;
|
||||
pause_timer = 0;
|
||||
}
|
||||
|
||||
void estimate_reset(void) {
|
||||
memset(&rs_state, 0, sizeof(RsState_t));
|
||||
memset(&rrl_state, 0, sizeof(RrlState_t));
|
||||
memset(&lm_state, 0, sizeof(LmState_t));
|
||||
test_step = ESTIMATE_TEST_IDLE;
|
||||
}
|
||||
|
||||
void estimate_start(Estimate_Test_t start_test) {
|
||||
estimate_reset();
|
||||
test_step = start_test;
|
||||
}
|
||||
|
||||
/*
|
||||
*
|
||||
*
|
||||
*/
|
||||
Estimate_Test_t estimate_get_step(void) {
|
||||
return test_step;
|
||||
}
|
||||
|
||||
/*
|
||||
*
|
||||
* params
|
||||
*/
|
||||
Params_t* estimate_get_params(void) {
|
||||
return ¶ms;
|
||||
}
|
||||
|
||||
/* ============================================
|
||||
*
|
||||
* ============================================ */
|
||||
|
||||
/*
|
||||
* : Rs
|
||||
*
|
||||
* :
|
||||
* step0: IREF1,
|
||||
* step1: , U1 I1
|
||||
* step2: IREF2,
|
||||
* step3: , U2 I2
|
||||
* step4: Rs = (U2-U1)/(I2-I1),
|
||||
*
|
||||
* :
|
||||
* vq, iq - (. .)
|
||||
* dt -
|
||||
* iq_ref -
|
||||
* freq_ref - (0 )
|
||||
*/
|
||||
static void process_Rs(float vq, float iq, float dt, float* iq_ref, float* freq_ref) {
|
||||
if (freq_ref)
|
||||
*freq_ref = 0;
|
||||
|
||||
switch (rs_state.step) {
|
||||
case 0:
|
||||
*iq_ref = RS_IREF1;
|
||||
rs_state.sum_v = 0;
|
||||
rs_state.sum_i = 0;
|
||||
rs_state.sample_cnt = 0;
|
||||
if (fabsf(iq) >= RS_IREF1 * RS_THRESHOLD) {
|
||||
rs_state.step = 1;
|
||||
rs_state.timer = 0;
|
||||
}
|
||||
break;
|
||||
|
||||
case 1:
|
||||
*iq_ref = RS_IREF1;
|
||||
rs_state.timer += dt;
|
||||
rs_state.sum_v += vdq_to_volts(vq);
|
||||
rs_state.sum_i += idq_to_amps(iq);
|
||||
rs_state.sample_cnt++;
|
||||
if (rs_state.timer >= RS_SETTLE_TIME) {
|
||||
if (rs_state.sample_cnt > 0) {
|
||||
rs_state.meas1 = rs_state.sum_v / rs_state.sample_cnt;
|
||||
rs_state.val1 = rs_state.sum_i / rs_state.sample_cnt;
|
||||
}
|
||||
rs_state.step = 2;
|
||||
}
|
||||
break;
|
||||
|
||||
case 2:
|
||||
*iq_ref = RS_IREF2;
|
||||
rs_state.sum_v = 0;
|
||||
rs_state.sum_i = 0;
|
||||
rs_state.sample_cnt = 0;
|
||||
if (fabsf(iq) >= RS_IREF2 * RS_THRESHOLD) {
|
||||
rs_state.step = 3;
|
||||
rs_state.timer = 0;
|
||||
}
|
||||
break;
|
||||
|
||||
case 3:
|
||||
*iq_ref = RS_IREF2;
|
||||
rs_state.timer += dt;
|
||||
rs_state.sum_v += vdq_to_volts(vq);
|
||||
rs_state.sum_i += idq_to_amps(iq);
|
||||
rs_state.sample_cnt++;
|
||||
if (rs_state.timer >= RS_SETTLE_TIME) {
|
||||
if (rs_state.sample_cnt > 0) {
|
||||
rs_state.meas2 = rs_state.sum_v / rs_state.sample_cnt;
|
||||
rs_state.val2 = rs_state.sum_i / rs_state.sample_cnt;
|
||||
}
|
||||
rs_state.step = 4;
|
||||
}
|
||||
break;
|
||||
|
||||
case 4:
|
||||
*iq_ref = 0;
|
||||
if (fabsf(rs_state.val2 - rs_state.val1) > 0.001f) {
|
||||
params.Rs = (rs_state.meas2 - rs_state.meas1) / (rs_state.val2 - rs_state.val1);
|
||||
if (params.Rs < 0) params.Rs = 0;
|
||||
}
|
||||
rs_state.step = 5;
|
||||
rs_state.done = 1; /* */
|
||||
break;
|
||||
|
||||
default:
|
||||
*iq_ref = 0;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
/*
|
||||
* : Rr
|
||||
*
|
||||
* :
|
||||
* - 0 RRL_IQ_AMP RRL_RAMP_TIME
|
||||
* -
|
||||
* - RRL_SAMPLES , Vq^2, Iq^2
|
||||
* - RMS , Zk Rk
|
||||
* - Rr = Rk - Rs, Xk, Lk
|
||||
* - RRL_AVG
|
||||
*
|
||||
* :
|
||||
* vq, iq - (. .)
|
||||
* dt -
|
||||
* iq_ref -
|
||||
* freq_ref -
|
||||
*/
|
||||
static void process_RrL(float vq, float iq, float dt, float* iq_ref, float* freq_ref) {
|
||||
|
||||
float f = RRL_FREQS[(rrl_state.freq_idx < RRL_FREQ_COUNT) ? rrl_state.freq_idx : (RRL_FREQ_COUNT - 1)];
|
||||
|
||||
/* (0...1) */
|
||||
float amp_k = rrl_state.ramp_timer / RRL_RAMP_TIME;
|
||||
if (amp_k > 1.0f) amp_k = 1.0f;
|
||||
|
||||
/* */
|
||||
float current_amp = RRL_IQ_AMP * amp_k;
|
||||
|
||||
/* */
|
||||
*iq_ref = current_amp * sinf(2.0f * M_PI * f * rrl_state.timer);
|
||||
if (freq_ref) *freq_ref = f;
|
||||
rrl_state.timer += dt;
|
||||
rrl_state.ramp_timer += dt;
|
||||
|
||||
/* - */
|
||||
if (amp_k < 1.0f) {
|
||||
return;
|
||||
}
|
||||
|
||||
/* , */
|
||||
float vq_oe = vq / T1_PRD;
|
||||
float dt_oe = DT / T_PWM;
|
||||
if (vq_oe > dt_oe) vq_oe -= dt_oe;
|
||||
else if (vq_oe < -dt_oe) vq_oe += dt_oe;
|
||||
|
||||
float vq_V = vdq_to_volts(vq);
|
||||
float iq_A = idq_to_amps(iq);
|
||||
|
||||
rrl_state.sum_p += vq_V * iq_A;
|
||||
rrl_state.sum_vq2 += vq_V * vq_V;
|
||||
rrl_state.sum_iq2 += iq_A * iq_A;
|
||||
rrl_state.sample_count++;
|
||||
|
||||
if (rrl_state.sample_count >= RRL_SAMPLES) {
|
||||
float P = rrl_state.sum_p / (float)RRL_SAMPLES;
|
||||
float Vq_rms = sqrtf(rrl_state.sum_vq2 / (float)RRL_SAMPLES);
|
||||
float Iq_rms = sqrtf(rrl_state.sum_iq2 / (float)RRL_SAMPLES);
|
||||
|
||||
if (Iq_rms > 0.001f) {
|
||||
float Zk = Vq_rms / Iq_rms;
|
||||
float Rk = P / (Iq_rms * Iq_rms);
|
||||
float Rr = Rk - params.Rs;
|
||||
if (Rr < 0) Rr = 0;
|
||||
float Xk = (Zk > Rk) ? sqrtf(Zk * Zk - Rk * Rk) / 2 : 0;
|
||||
float Lk = (f > 1e-6f) ? (Xk / (2.0f * M_PI * f)) : 0.0f;
|
||||
|
||||
(void)Rr;
|
||||
rrl_state.avg_Zk += Zk;
|
||||
rrl_state.avg_Xk += Xk;
|
||||
rrl_state.avg_Lk += Lk;
|
||||
rrl_state.avg_Rk += Rk;
|
||||
rrl_state.avg_count++;
|
||||
}
|
||||
|
||||
rrl_state.sum_p = 0;
|
||||
rrl_state.sum_vq2 = 0;
|
||||
rrl_state.sum_iq2 = 0;
|
||||
rrl_state.sample_count = 0;
|
||||
}
|
||||
|
||||
if (rrl_state.avg_count >= RRL_AVG) {
|
||||
/* */
|
||||
int k = rrl_state.freq_idx;
|
||||
if (k < RRL_FREQ_COUNT) {
|
||||
rrl_state.freq_Zk[k] = rrl_state.avg_Zk / (float)RRL_AVG;
|
||||
rrl_state.freq_Xk[k] = rrl_state.avg_Xk / (float)RRL_AVG;
|
||||
rrl_state.freq_Lk[k] = rrl_state.avg_Lk / (float)RRL_AVG;
|
||||
rrl_state.freq_Rk[k] = rrl_state.avg_Rk / (float)RRL_AVG;
|
||||
rrl_state.freq_ready = k + 1;
|
||||
}
|
||||
|
||||
/* */
|
||||
rrl_state.freq_idx++;
|
||||
rrl_state.timer = 0.0f;
|
||||
rrl_state.ramp_timer = 0.0f;
|
||||
rrl_state.sum_p = 0.0f;
|
||||
rrl_state.sum_vq2 = 0.0f;
|
||||
rrl_state.sum_iq2 = 0.0f;
|
||||
rrl_state.sample_count = 0;
|
||||
rrl_state.avg_Zk = 0.0f;
|
||||
rrl_state.avg_Xk = 0.0f;
|
||||
rrl_state.avg_Lk = 0.0f;
|
||||
rrl_state.avg_Rk = 0.0f;
|
||||
rrl_state.avg_count = 0;
|
||||
|
||||
/* 0 */
|
||||
if (rrl_state.freq_idx >= RRL_FREQ_COUNT) {
|
||||
float s1 = 0.0f, sf = 0.0f, sf2 = 0.0f;
|
||||
float sR = 0.0f, sfR = 0.0f;
|
||||
float sfX = 0.0f;
|
||||
int n = rrl_state.freq_ready;
|
||||
for (int i = 0; i < n; i++) {
|
||||
float fi = RRL_FREQS[i];
|
||||
float Ri = rrl_state.freq_Rk[i];
|
||||
float Xi = rrl_state.freq_Xk[i];
|
||||
s1 += 1.0f;
|
||||
sf += fi;
|
||||
sf2 += fi * fi;
|
||||
sR += Ri;
|
||||
sfR += fi * Ri;
|
||||
sfX += fi * Xi;
|
||||
}
|
||||
|
||||
/* Rk(f) = a + b f */
|
||||
float den = (s1 * sf2 - sf * sf);
|
||||
float aR = (fabsf(den) > 1e-9f) ? ((sR * sf2 - sf * sfR) / den) : (n > 0 ? (sR / (float)n) : 0.0f);
|
||||
|
||||
/* Xk(f) = c f ( 0) */
|
||||
float cX = (sf2 > 1e-9f) ? (sfX / sf2) : 0.0f;
|
||||
|
||||
params.Rk = aR;
|
||||
params.Xk = 0.0f; /* f=0 -> 0 */
|
||||
params.Zk = params.Rk;
|
||||
params.Lk = cX / (2.0f * M_PI);
|
||||
|
||||
params.Rr = params.Rk - params.Rs;
|
||||
if (params.Rr < 0.0f) params.Rr = 0.0f;
|
||||
params.Lls = params.Lk / 2.0f;
|
||||
params.Llr = params.Lk / 2.0f;
|
||||
|
||||
rrl_state.done = 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
/*
|
||||
* : Lm
|
||||
*
|
||||
* :
|
||||
* - Idref
|
||||
* - Ed = Vd - Rs*Id
|
||||
* - LM_SETTLE_THRESHOLD -
|
||||
* - Ls = integral(Ed*dt) / Id_steady
|
||||
* - Lm = Ls - Lls
|
||||
*
|
||||
* :
|
||||
* vd, id - (. .)
|
||||
* dt -
|
||||
* id_ref -
|
||||
* freq_ref -
|
||||
*/
|
||||
static void process_Lm(float vd, float id, float dt, float* id_ref, float* freq_ref) {
|
||||
float vd_abs = vdq_to_volts(vd);
|
||||
float id_abs = idq_to_amps(id);
|
||||
float ed = vd_abs - params.Rs * id_abs;
|
||||
float id_pu = fabsf(id);
|
||||
float id_thresh = LM_IDREF * LM_SETTLE_THRESHOLD;
|
||||
|
||||
if (freq_ref)
|
||||
*freq_ref = 0.0f;
|
||||
|
||||
switch (lm_state.step) {
|
||||
case 0:
|
||||
*id_ref = 0.0f;
|
||||
lm_state.timer += dt;
|
||||
if (lm_state.timer >= LM_DEMAG_TIME) {
|
||||
lm_state.timer = 0.0f;
|
||||
lm_state.integral_psi = 0.0f;
|
||||
lm_state.first_sample = 0;
|
||||
lm_state.prev_ed = ed;
|
||||
lm_state.step = 1;
|
||||
}
|
||||
break;
|
||||
|
||||
case 1:
|
||||
*id_ref = LM_IDREF;
|
||||
if (!lm_state.first_sample) {
|
||||
lm_state.prev_ed = ed;
|
||||
lm_state.first_sample = 1;
|
||||
}
|
||||
else {
|
||||
lm_state.integral_psi += (ed + lm_state.prev_ed) * 0.5f * dt;
|
||||
lm_state.prev_ed = ed;
|
||||
}
|
||||
|
||||
if (id_pu >= id_thresh) {
|
||||
lm_state.step = 2;
|
||||
lm_state.timer = 0.0f;
|
||||
lm_state.sum_id = 0.0f;
|
||||
lm_state.sample_cnt = 0;
|
||||
}
|
||||
break;
|
||||
|
||||
case 2:
|
||||
*id_ref = LM_IDREF;
|
||||
lm_state.timer += dt;
|
||||
lm_state.sum_id += id_abs;
|
||||
lm_state.sample_cnt++;
|
||||
if (lm_state.timer >= LM_SETTLE_TIME) {
|
||||
lm_state.step = 3;
|
||||
}
|
||||
break;
|
||||
|
||||
case 3: {
|
||||
*id_ref = LM_IDREF;
|
||||
int measure_ok = 0;
|
||||
if (lm_state.sample_cnt > 0 && lm_state.integral_psi > 0.0f) {
|
||||
float id_avg = lm_state.sum_id / (float)lm_state.sample_cnt;
|
||||
if (id_avg > 0.001f) {
|
||||
float Ls = lm_state.integral_psi / id_avg;
|
||||
float Lm = Ls - params.Lls;
|
||||
if (Lm > 0.0f) {
|
||||
lm_state.avg_Lm += Lm;
|
||||
lm_state.avg_count++;
|
||||
measure_ok = 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (lm_state.avg_count >= LM_AVG) {
|
||||
params.Lm = lm_state.avg_Lm / (float)LM_AVG;
|
||||
lm_state.done = 1;
|
||||
lm_state.step = 4;
|
||||
}
|
||||
else if (measure_ok) {
|
||||
lm_state.timer = 0.0f;
|
||||
lm_state.step = 0;
|
||||
}
|
||||
else {
|
||||
lm_state.timer = 0.0f;
|
||||
lm_state.step = 0;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
case 4:
|
||||
*id_ref = 0.0f;
|
||||
break;
|
||||
|
||||
default:
|
||||
*id_ref = 0.0f;
|
||||
lm_state.step = 0;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/* ============================================
|
||||
*
|
||||
* ============================================ */
|
||||
|
||||
/*
|
||||
* estimate_process -
|
||||
*
|
||||
* : ,
|
||||
*
|
||||
*
|
||||
* :
|
||||
* vd, vq - d,q ( )
|
||||
* id, iq - d,q ( )
|
||||
* dt - ()
|
||||
*
|
||||
* :
|
||||
* vd_ref - d (. .)
|
||||
* vq_ref - q (. .)
|
||||
* freq_ref - ()
|
||||
*/
|
||||
void estimate_process(float vd, float vq, float id, float iq, float dt,
|
||||
float* vd_ref, float* vq_ref, float* freq_ref) {
|
||||
|
||||
*vd_ref = 0;
|
||||
*vq_ref = 0;
|
||||
if (freq_ref) *freq_ref = 0;
|
||||
|
||||
/* IDLE - */
|
||||
if (test_step == ESTIMATE_TEST_IDLE) {
|
||||
return;
|
||||
}
|
||||
|
||||
/* */
|
||||
if (in_pause) {
|
||||
pause_timer += dt;
|
||||
if (pause_timer >= STEP_PAUSE_TIME) {
|
||||
in_pause = 0;
|
||||
/* */
|
||||
if (test_step == ESTIMATE_TEST_RS) test_step = ESTIMATE_TEST_RR_L;
|
||||
else if (test_step == ESTIMATE_TEST_RR_L) test_step = ESTIMATE_TEST_LM;
|
||||
else if (test_step == ESTIMATE_TEST_LM) test_step = ESTIMATE_TEST_DONE;
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
switch (test_step) {
|
||||
case ESTIMATE_TEST_RS:
|
||||
process_Rs(vq, iq, dt, vq_ref, freq_ref);
|
||||
if (rs_state.done) {
|
||||
in_pause = 1;
|
||||
pause_timer = 0;
|
||||
}
|
||||
break;
|
||||
|
||||
case ESTIMATE_TEST_RR_L:
|
||||
process_RrL(vq, iq, dt, vq_ref, freq_ref);
|
||||
if (rrl_state.done) {
|
||||
in_pause = 1;
|
||||
pause_timer = 0;
|
||||
}
|
||||
break;
|
||||
|
||||
case ESTIMATE_TEST_LM:
|
||||
if (vq_ref)
|
||||
*vq_ref = 0.0f;
|
||||
process_Lm(vd, id, dt, vd_ref, freq_ref);
|
||||
if (lm_state.done) {
|
||||
in_pause = 1;
|
||||
pause_timer = 0;
|
||||
}
|
||||
break;
|
||||
|
||||
case ESTIMATE_TEST_DONE:
|
||||
/* */
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user