1
0
mirror of https://github.com/rene-dev/stmbl.git synced 2025-01-01 21:42:14 +00:00
stmbl/shared/comps/svm.c

111 lines
2.2 KiB
C
Raw Permalink Normal View History

2017-04-15 18:54:21 +00:00
#include "commands.h"
#include "hal.h"
#include "math.h"
#include "defines.h"
#include "angle.h"
2017-01-14 02:30:26 +00:00
HAL_COMP(svm);
2017-01-23 08:11:36 +00:00
//U V W inputs
2017-04-15 18:54:21 +00:00
HAL_PIN(u);
HAL_PIN(v);
HAL_PIN(w);
2017-01-23 08:11:36 +00:00
//dclink input
2017-04-15 18:54:21 +00:00
HAL_PIN(udc);
2017-01-14 02:30:26 +00:00
2017-01-23 08:11:36 +00:00
//U V W outputs
2017-01-14 02:30:26 +00:00
HAL_PIN(su);
HAL_PIN(sv);
HAL_PIN(sw);
2017-01-23 08:11:36 +00:00
//commutation mode
2017-04-15 18:54:21 +00:00
HAL_PIN(cmode);
2017-01-23 08:11:36 +00:00
//modulation mode
2017-04-15 18:54:21 +00:00
HAL_PIN(mode);
2017-01-14 02:30:26 +00:00
2017-01-23 08:11:36 +00:00
//half bridge enable out
2017-04-15 18:54:21 +00:00
HAL_PIN(enu);
HAL_PIN(env);
HAL_PIN(enw);
static void nrt_init(volatile void * ctx_ptr, volatile hal_pin_inst_t * pin_ptr){
// struct svm_ctx_t * ctx = (struct svm_ctx_t *)ctx_ptr;
struct svm_pin_ctx_t * pins = (struct svm_pin_ctx_t *)pin_ptr;
PIN(mode) = 2.0;
PIN(enu) = 1.0;
PIN(env) = 1.0;
PIN(enw) = 1.0;
}
static void rt_func(float period, volatile void * ctx_ptr, volatile hal_pin_inst_t * pin_ptr){
// struct svm_ctx_t * ctx = (struct svm_ctx_t *)ctx_ptr;
struct svm_pin_ctx_t * pins = (struct svm_pin_ctx_t *)pin_ptr;
2017-01-14 02:30:26 +00:00
float offset = 0;
float udc = PIN(udc);
float u = PIN(u);
float v = PIN(v);
float w = PIN(w);
switch((int)PIN(mode)){
2017-01-16 04:54:32 +00:00
default:
2017-01-23 08:11:36 +00:00
case 0: // sine modulation
2017-01-14 02:30:26 +00:00
offset = (u + v + w) / 3.0 - udc / 2.0;
break;
2017-01-23 08:11:36 +00:00
case 1: // space vector modulation
2017-01-14 02:30:26 +00:00
offset = (MIN3(u, v, w) + MAX3(u, v, w)) / 2.0 - udc / 2.0;
break;
2017-01-23 08:11:36 +00:00
case 2: // flat bottom space vector modulation
2017-01-14 02:30:26 +00:00
offset = MIN3(u, v, w);
break;
2017-01-14 03:36:21 +00:00
2017-01-23 08:11:36 +00:00
case 3: // flat top space vector modulation
2017-01-14 03:36:21 +00:00
offset = MAX3(u, v, w) - udc;
break;
2017-01-16 04:54:32 +00:00
}
PIN(enu) = 1.0;
PIN(env) = 1.0;
PIN(enw) = 1.0;
switch((int)PIN(cmode)){
case 1: // block
if(u > v && u < w){
PIN(enu) = 0.0;
}
if(v > u && v < w){
PIN(env) = 0.0;
}
if(w > u && w < v){
PIN(enw) = 0.0;
}
break;
default: // sine
break;
2017-01-14 02:30:26 +00:00
}
PIN(su) = u - offset;
PIN(sv) = v - offset;
PIN(sw) = w - offset;
2017-04-15 18:54:21 +00:00
}
2017-01-14 02:30:26 +00:00
2017-04-15 18:54:21 +00:00
hal_comp_t svm_comp_struct = {
.name = "svm",
.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 svm_pin_ctx_t) / sizeof(struct hal_pin_inst_t),
};