mirror of
https://github.com/rene-dev/stmbl.git
synced 2026-09-23 07:43:59 +08:00
simple iit
This commit is contained in:
crinq
@@ -10,6 +10,7 @@ load fault
|
||||
load hv
|
||||
load sim
|
||||
load io
|
||||
load iit
|
||||
link conf
|
||||
hv0.rt_prio = 0.9
|
||||
hv0.frt_prio = 1
|
||||
@@ -23,6 +24,7 @@ vel2.rt_prio = 6
|
||||
io0.rt_prio = 7
|
||||
pid0.rt_prio = 8
|
||||
fault0.rt_prio = 10
|
||||
iit0.rt_prio = 13
|
||||
sim0.rt_prio = 13
|
||||
term0.rt_prio = 16
|
||||
rev0.rev = conf0.cmd_rev
|
||||
@@ -116,4 +118,9 @@ pe0.torque = pid0.torque_cor_cmd
|
||||
pe0.vel = vel1.vel
|
||||
pe0.r = conf0.r
|
||||
pe0.j = conf0.j
|
||||
pe0.cap = 0.00054
|
||||
pe0.cap = 0.00054
|
||||
iit0.max_temp = conf0.max_mot_temp
|
||||
iit0.high_temp = conf0.high_mot_temp
|
||||
iit0.max_cur = conf0.max_ac_cur
|
||||
fault0.mot_temp = iit0.temp
|
||||
iit0.cur = hv0.abs_cur
|
||||
+37
-63
@@ -2,90 +2,64 @@
|
||||
#include "hal.h"
|
||||
#include "math.h"
|
||||
#include "defines.h"
|
||||
#include "angle.h"
|
||||
|
||||
HAL_COMP(iit);
|
||||
|
||||
HAL_PIN(brake_r);
|
||||
HAL_PIN(brake);
|
||||
HAL_PIN(air_temp);
|
||||
HAL_PIN(mot_air_res);
|
||||
HAL_PIN(mot_cap);
|
||||
HAL_PIN(mot_tau);
|
||||
HAL_PIN(mot_temp);
|
||||
HAL_PIN(mot_max_temp);
|
||||
HAL_PIN(mot_res);
|
||||
HAL_PIN(mot_psi);
|
||||
HAL_PIN(mot_real_res);
|
||||
HAL_PIN(mot_real_psi);
|
||||
HAL_PIN(mot_cont_cur);
|
||||
HAL_PIN(id);
|
||||
HAL_PIN(iq);
|
||||
HAL_PIN(flow);
|
||||
// conf
|
||||
HAL_PIN(amb_temp);
|
||||
HAL_PIN(high_temp);
|
||||
HAL_PIN(max_temp);
|
||||
HAL_PIN(max_cur);
|
||||
HAL_PIN(cur_boost);
|
||||
HAL_PIN(max_time);
|
||||
|
||||
// out
|
||||
HAL_PIN(temp);
|
||||
// in
|
||||
HAL_PIN(cur);
|
||||
|
||||
struct iit_ctx_t {
|
||||
float mot_temp;
|
||||
float e;
|
||||
};
|
||||
|
||||
static void nrt_init(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
|
||||
struct iit_ctx_t *ctx = (struct iit_ctx_t *)ctx_ptr;
|
||||
struct iit_pin_ctx_t *pins = (struct iit_pin_ctx_t *)pin_ptr;
|
||||
ctx->mot_temp = 25.0;
|
||||
PIN(brake_r) = 50.0;
|
||||
PIN(brake) = 2.0;
|
||||
PIN(air_temp) = 20.0;
|
||||
PIN(mot_air_res) = 0.0;
|
||||
PIN(mot_cap) = 0.0;
|
||||
PIN(mot_tau) = 300.0;
|
||||
PIN(mot_temp) = 25.0;
|
||||
PIN(mot_max_temp) = 130.0;
|
||||
PIN(mot_res) = 1.0;
|
||||
PIN(mot_psi) = 1.0;
|
||||
PIN(mot_real_res) = 1.0;
|
||||
PIN(mot_real_psi) = 1.0;
|
||||
PIN(mot_cont_cur) = 2.15;
|
||||
PIN(id) = 0.0;
|
||||
PIN(iq) = 0.0;
|
||||
PIN(flow) = 0.0;
|
||||
PIN(amb_temp) = 30.0;
|
||||
PIN(high_temp) = 80.0;
|
||||
PIN(max_temp) = 100.0;
|
||||
PIN(max_time) = 10.0;
|
||||
PIN(cur_boost) = 3.0;
|
||||
ctx->e = 0.0;
|
||||
}
|
||||
|
||||
static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
|
||||
struct iit_ctx_t *ctx = (struct iit_ctx_t *)ctx_ptr;
|
||||
struct iit_pin_ctx_t *pins = (struct iit_pin_ctx_t *)pin_ptr;
|
||||
|
||||
float mot_real_res = PIN(mot_res) * (1 + 0.004 * (ctx->mot_temp - PIN(air_temp)));
|
||||
float cur_n = PIN(max_cur) / MAX(PIN(cur_boost), 1.0);
|
||||
float max_e = cur_n * cur_n * MAX(PIN(max_time), 0.1);
|
||||
|
||||
float mot_max_res = PIN(mot_res) * (1 + 0.004 * (PIN(mot_max_temp) - PIN(air_temp)));
|
||||
float mot_air_res = (PIN(mot_max_temp) - PIN(air_temp)) / (PIN(mot_cont_cur) * PIN(mot_cont_cur) * mot_max_res);
|
||||
;
|
||||
float mot_cap = PIN(mot_tau) / mot_air_res;
|
||||
float temp = ctx->e / max_e * PIN(max_temp) + PIN(amb_temp);
|
||||
|
||||
float brake_flow = 24.0 * 24.0 / PIN(brake_r) * PIN(brake);
|
||||
float flow = (PIN(id) * PIN(id) + PIN(iq) * PIN(iq)) * 3.0 / 2.0 * mot_real_res - (ctx->mot_temp - PIN(air_temp)) / mot_air_res;
|
||||
float pin = PIN(cur) * PIN(cur);
|
||||
float pout = (temp - PIN(amb_temp)) * cur_n * cur_n / (PIN(high_temp) - PIN(amb_temp));
|
||||
|
||||
flow += brake_flow;
|
||||
ctx->e += (pin - pout) * period;
|
||||
|
||||
ctx->mot_temp += flow / mot_cap * period;
|
||||
|
||||
PIN(flow) = flow;
|
||||
PIN(mot_air_res) = mot_air_res;
|
||||
PIN(mot_cap) = mot_cap;
|
||||
PIN(mot_temp) = ctx->mot_temp;
|
||||
PIN(mot_real_res) = mot_real_res;
|
||||
PIN(mot_real_psi) = PIN(mot_psi) * (1 - 0.0013 * (ctx->mot_temp - PIN(air_temp)));
|
||||
;
|
||||
PIN(temp) = temp;
|
||||
}
|
||||
|
||||
hal_comp_t iit_comp_struct = {
|
||||
.name = "iit",
|
||||
.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 = sizeof(struct iit_ctx_t),
|
||||
.pin_count = sizeof(struct iit_pin_ctx_t) / sizeof(struct hal_pin_inst_t),
|
||||
.name = "iit",
|
||||
.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 = sizeof(struct iit_ctx_t),
|
||||
.pin_count = sizeof(struct iit_pin_ctx_t) / sizeof(struct hal_pin_inst_t),
|
||||
};
|
||||
|
||||
@@ -0,0 +1,91 @@
|
||||
#include "commands.h"
|
||||
#include "hal.h"
|
||||
#include "math.h"
|
||||
#include "defines.h"
|
||||
#include "angle.h"
|
||||
|
||||
HAL_COMP(iit);
|
||||
|
||||
HAL_PIN(brake_r);
|
||||
HAL_PIN(brake);
|
||||
HAL_PIN(air_temp);
|
||||
HAL_PIN(mot_air_res);
|
||||
HAL_PIN(mot_cap);
|
||||
HAL_PIN(mot_tau);
|
||||
HAL_PIN(mot_temp);
|
||||
HAL_PIN(mot_max_temp);
|
||||
HAL_PIN(mot_res);
|
||||
HAL_PIN(mot_psi);
|
||||
HAL_PIN(mot_real_res);
|
||||
HAL_PIN(mot_real_psi);
|
||||
HAL_PIN(mot_cont_cur);
|
||||
HAL_PIN(id);
|
||||
HAL_PIN(iq);
|
||||
HAL_PIN(flow);
|
||||
|
||||
struct iit_ctx_t {
|
||||
float mot_temp;
|
||||
};
|
||||
|
||||
static void nrt_init(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
|
||||
struct iit_ctx_t *ctx = (struct iit_ctx_t *)ctx_ptr;
|
||||
struct iit_pin_ctx_t *pins = (struct iit_pin_ctx_t *)pin_ptr;
|
||||
ctx->mot_temp = 25.0;
|
||||
PIN(brake_r) = 50.0;
|
||||
PIN(brake) = 2.0;
|
||||
PIN(air_temp) = 20.0;
|
||||
PIN(mot_air_res) = 0.0;
|
||||
PIN(mot_cap) = 0.0;
|
||||
PIN(mot_tau) = 300.0;
|
||||
PIN(mot_temp) = 25.0;
|
||||
PIN(mot_max_temp) = 130.0;
|
||||
PIN(mot_res) = 1.0;
|
||||
PIN(mot_psi) = 1.0;
|
||||
PIN(mot_real_res) = 1.0;
|
||||
PIN(mot_real_psi) = 1.0;
|
||||
PIN(mot_cont_cur) = 2.15;
|
||||
PIN(id) = 0.0;
|
||||
PIN(iq) = 0.0;
|
||||
PIN(flow) = 0.0;
|
||||
}
|
||||
|
||||
static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
|
||||
struct iit_ctx_t *ctx = (struct iit_ctx_t *)ctx_ptr;
|
||||
struct iit_pin_ctx_t *pins = (struct iit_pin_ctx_t *)pin_ptr;
|
||||
|
||||
float mot_real_res = PIN(mot_res) * (1 + 0.004 * (ctx->mot_temp - PIN(air_temp)));
|
||||
|
||||
float mot_max_res = PIN(mot_res) * (1 + 0.004 * (PIN(mot_max_temp) - PIN(air_temp)));
|
||||
float mot_air_res = (PIN(mot_max_temp) - PIN(air_temp)) / (PIN(mot_cont_cur) * PIN(mot_cont_cur) * mot_max_res);
|
||||
;
|
||||
float mot_cap = PIN(mot_tau) / mot_air_res;
|
||||
|
||||
float brake_flow = 24.0 * 24.0 / PIN(brake_r) * PIN(brake);
|
||||
float flow = (PIN(id) * PIN(id) + PIN(iq) * PIN(iq)) * 3.0 / 2.0 * mot_real_res - (ctx->mot_temp - PIN(air_temp)) / mot_air_res;
|
||||
|
||||
flow += brake_flow;
|
||||
|
||||
ctx->mot_temp += flow / mot_cap * period;
|
||||
|
||||
PIN(flow) = flow;
|
||||
PIN(mot_air_res) = mot_air_res;
|
||||
PIN(mot_cap) = mot_cap;
|
||||
PIN(mot_temp) = ctx->mot_temp;
|
||||
PIN(mot_real_res) = mot_real_res;
|
||||
PIN(mot_real_psi) = PIN(mot_psi) * (1 - 0.0013 * (ctx->mot_temp - PIN(air_temp)));
|
||||
;
|
||||
}
|
||||
|
||||
hal_comp_t iit_comp_struct = {
|
||||
.name = "iit",
|
||||
.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 = sizeof(struct iit_ctx_t),
|
||||
.pin_count = sizeof(struct iit_pin_ctx_t) / sizeof(struct hal_pin_inst_t),
|
||||
};
|
||||
Reference in New Issue
Block a user