2017-07-01 01:18:08 +00:00
|
|
|
#include "commands.h"
|
|
|
|
|
#include "hal.h"
|
|
|
|
|
#include "math.h"
|
|
|
|
|
#include "defines.h"
|
|
|
|
|
#include "angle.h"
|
|
|
|
|
|
2017-07-01 03:21:03 +00:00
|
|
|
HAL_COMP(iit);
|
2017-07-01 01:18:08 +00:00
|
|
|
|
|
|
|
|
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);
|
|
|
|
|
|
2017-09-06 02:20:06 +00:00
|
|
|
struct iit_ctx_t {
|
|
|
|
|
float mot_temp;
|
2017-07-01 01:18:08 +00:00
|
|
|
};
|
|
|
|
|
|
2017-09-06 02:20:06 +00:00
|
|
|
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;
|
2017-07-01 01:18:08 +00:00
|
|
|
}
|
|
|
|
|
|
2017-09-06 02:20:06 +00:00
|
|
|
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)));
|
|
|
|
|
;
|
2017-07-01 01:18:08 +00:00
|
|
|
}
|
|
|
|
|
|
2017-07-01 03:21:03 +00:00
|
|
|
hal_comp_t iit_comp_struct = {
|
2017-09-06 02:20:06 +00:00
|
|
|
.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),
|
2017-07-01 01:18:08 +00:00
|
|
|
};
|
