mirror of
https://github.com/rene-dev/stmbl.git
synced 2024-12-20 15:42:18 +00:00
142 lines
3.3 KiB
C
142 lines
3.3 KiB
C
#include "sensorless_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 shou l 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(sensorless);
|
|
|
|
//in
|
|
HAL_PIN(r);
|
|
HAL_PIN(l);
|
|
|
|
HAL_PIN(ki);
|
|
HAL_PIN(kb);
|
|
HAL_PIN(kl);
|
|
HAL_PIN(min_vel);
|
|
HAL_PIN(vel_boost);
|
|
HAL_PIN(max_vel);
|
|
|
|
HAL_PIN(id);
|
|
HAL_PIN(iq);
|
|
HAL_PIN(ud);
|
|
HAL_PIN(uq);
|
|
HAL_PIN(ud1);
|
|
HAL_PIN(uq1);
|
|
HAL_PIN(ud2);
|
|
HAL_PIN(uq2);
|
|
|
|
|
|
//out
|
|
HAL_PIN(vel);
|
|
HAL_PIN(pos);
|
|
|
|
HAL_PIN(ed);
|
|
HAL_PIN(eq);
|
|
|
|
HAL_PIN(old_id);
|
|
HAL_PIN(old_iq);
|
|
|
|
HAL_PIN(delta_id);
|
|
HAL_PIN(delta_iq);
|
|
|
|
static void nrt_init(void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
|
|
// struct sensorless_ctx_t *ctx = (struct sensorless_ctx_t *)ctx_ptr;
|
|
struct sensorless_pin_ctx_t *pins = (struct sensorless_pin_ctx_t *)pin_ptr;
|
|
PIN(ki) = 1500.0;
|
|
PIN(kb) = 1;
|
|
PIN(kl) = 0.75;
|
|
PIN(min_vel) = 3.0 * 2.0 * M_PI;
|
|
PIN(vel_boost) = 0.2;
|
|
PIN(max_vel) = 500.0 * 2.0 * M_PI * 1.1;
|
|
}
|
|
|
|
static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
|
|
// struct sensorless_ctx_t *ctx = (struct sensorless_ctx_t *)ctx_ptr;
|
|
struct sensorless_pin_ctx_t *pins = (struct sensorless_pin_ctx_t *)pin_ptr;
|
|
|
|
float l = MAX(PIN(l), 0.00001);
|
|
float r = MAX(PIN(r), 0.01);
|
|
|
|
float ud = PIN(ud2);
|
|
float uq = PIN(uq2);
|
|
PIN(ud2) = PIN(ud1);
|
|
PIN(uq2) = PIN(uq1);
|
|
PIN(ud1) = PIN(ud);
|
|
PIN(uq1) = PIN(uq);
|
|
|
|
float id = PIN(id);
|
|
float iq = PIN(iq);
|
|
|
|
float kb = CLAMP(PIN(kb), 0, 1);
|
|
float ki = CLAMP(PIN(ki), 0, 1.0 / period);
|
|
float kl = CLAMP(PIN(kl), 0, 1);
|
|
|
|
float vel = PIN(vel);
|
|
float pos = PIN(pos);
|
|
|
|
// lowpass current change
|
|
PIN(delta_id) = PIN(delta_id) * kl + (id - PIN(old_id)) * (1 - kl);
|
|
PIN(delta_iq) = PIN(delta_iq) * kl + (iq - PIN(old_iq)) * (1 - kl);
|
|
|
|
PIN(old_id) = id;
|
|
PIN(old_iq) = iq;
|
|
|
|
// calc bemf
|
|
float ed = ud - r * id - PIN(delta_id) * l / period + vel * l * iq * kb;
|
|
float eq = uq - r * iq - PIN(delta_iq) * l / period - vel * l * id * kb;
|
|
|
|
// velocity compensation, bemf_d -> 0
|
|
vel -= SIGN2(eq, 1.0) * ed * ki * period;
|
|
|
|
// startup boost
|
|
if(ABS(vel) < PIN(min_vel)){
|
|
vel += SIGN2(id * iq, 0.1) * PIN(vel_boost);
|
|
}
|
|
|
|
vel = LIMIT(vel, PIN(max_vel));
|
|
|
|
pos += vel * period;
|
|
|
|
PIN(ed) = ed;
|
|
PIN(eq) = eq;
|
|
|
|
PIN(vel) = vel;
|
|
PIN(pos) = mod(pos);
|
|
}
|
|
|
|
hal_comp_t sensorless_comp_struct = {
|
|
.name = "sensorless",
|
|
.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 sensorless_pin_ctx_t) / sizeof(struct hal_pin_inst_t),
|
|
}; |