mirror of https://github.com/rene-dev/stmbl.git
120 lines
3.1 KiB
C
120 lines
3.1 KiB
C
#include "comps/pmsm_comp.h"
|
|
/*
|
|
* This file is part of the stmbl project.
|
|
*
|
|
* Copyright (C) 2013-2016 Rene Hopf <renehopf@mac.com>
|
|
* Copyright (C) 2013-2016 Nico Stute <crinq@crinq.de>
|
|
*
|
|
* This program is free software: you can redistribute it and/or modify
|
|
* it under the terms of the GNU General Public License as published by
|
|
* the Free Software Foundation, either version 3 of the License, or
|
|
* (at your option) any later version.
|
|
*
|
|
* This program is distributed in the hope that it will be useful,
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
* GNU General Public License for more details.
|
|
*
|
|
* You should have received a copy of the GNU General Public License
|
|
* along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
#include "commands.h"
|
|
#include "hal.h"
|
|
#include "math.h"
|
|
#include "defines.h"
|
|
#include "angle.h"
|
|
|
|
|
|
HAL_COMP(pmsm);
|
|
|
|
//in
|
|
HAL_PIN(psi);
|
|
HAL_PIN(r);
|
|
HAL_PIN(ld);
|
|
HAL_PIN(lq);
|
|
HAL_PIN(polecount);
|
|
|
|
HAL_PIN(vel);
|
|
HAL_PIN(ud);
|
|
HAL_PIN(uq);
|
|
|
|
HAL_PIN(indd);
|
|
HAL_PIN(indq);
|
|
|
|
//out
|
|
HAL_PIN(id);
|
|
HAL_PIN(iq);
|
|
HAL_PIN(psi_d);
|
|
HAL_PIN(psi_q);
|
|
HAL_PIN(torque);
|
|
|
|
static void nrt_init(void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
|
|
// struct pmsm_ctx_t *ctx = (struct pmsm_ctx_t *)ctx_ptr;
|
|
struct pmsm_pin_ctx_t *pins = (struct pmsm_pin_ctx_t *)pin_ptr;
|
|
PIN(psi) = 0.01;
|
|
PIN(r) = 1.0;
|
|
PIN(ld) = 0.001;
|
|
PIN(lq) = 0.001;
|
|
PIN(polecount) = 1.0;
|
|
}
|
|
|
|
static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
|
|
// struct pmsm_ctx_t *ctx = (struct pmsm_ctx_t *)ctx_ptr;
|
|
struct pmsm_pin_ctx_t *pins = (struct pmsm_pin_ctx_t *)pin_ptr;
|
|
|
|
|
|
float p = (int)MAX(PIN(polecount), 1.0);
|
|
float vel_e = PIN(vel) * p;
|
|
float ld = MAX(PIN(ld), 0.0001);
|
|
float lq = MAX(PIN(lq), 0.0001);
|
|
float ud = PIN(ud);
|
|
float uq = PIN(uq);
|
|
float psi_m = MAX(PIN(psi), 0.01);
|
|
float r = MAX(PIN(r), 0.01);
|
|
float id = PIN(id);
|
|
float iq = PIN(iq);
|
|
|
|
float psi_d = ld * id + psi_m;
|
|
float psi_q = lq * iq;
|
|
|
|
float indd = vel_e * psi_q; // todo redundant calculation
|
|
float indq = vel_e * psi_d;
|
|
|
|
id += (ud - r * id + indd) / ld * period / 4.0;
|
|
iq += (uq - r * iq - indq) / lq * period / 4.0;
|
|
|
|
id += (ud - r * id + indd) / ld * period / 4.0;
|
|
iq += (uq - r * iq - indq) / lq * period / 4.0;
|
|
|
|
id += (ud - r * id + indd) / ld * period / 4.0;
|
|
iq += (uq - r * iq - indq) / lq * period / 4.0;
|
|
|
|
id += (ud - r * id + indd) / ld * period / 4.0;
|
|
iq += (uq - r * iq - indq) / lq * period / 4.0;
|
|
|
|
float t = 3.0 / 2.0 * p * (psi_m * iq + (ld - lq) * id * iq);
|
|
|
|
PIN(id) = id;
|
|
PIN(iq) = iq;
|
|
PIN(indd) = indd;
|
|
PIN(indq) = indq;
|
|
PIN(psi_d) = psi_d;
|
|
PIN(psi_q) = psi_q;
|
|
PIN(torque) = t;
|
|
}
|
|
|
|
hal_comp_t pmsm_comp_struct = {
|
|
.name = "pmsm",
|
|
.nrt = 0,
|
|
.rt = rt_func,
|
|
.frt = 0,
|
|
.nrt_init = nrt_init,
|
|
.rt_start = 0,
|
|
.frt_start = 0,
|
|
.rt_stop = 0,
|
|
.frt_stop = 0,
|
|
.ctx_size = 0,
|
|
.pin_count = sizeof(struct pmsm_pin_ctx_t) / sizeof(struct hal_pin_inst_t),
|
|
};
|