From a5ddebc10206eb0298b350f0a774598bb8c21927 Mon Sep 17 00:00:00 2001 From: Roman Belov Date: Sun, 25 Feb 2024 21:50:03 +0300 Subject: [PATCH] Added freewheeling option in forced control. --- bench/tsfunc.c | 34 ++++- doc/HardwareVESC.md | 2 +- doc/InputAnalogKnob.md | 8 +- phobia/link.c | 9 +- phobia/phobia.c | 63 ++++++--- src/app/autostart.c | 3 +- src/app/mpu6050.c | 9 +- src/epcan.c | 3 +- src/hal/hw/{FSESC67PRO.h => FSESC67MINI.h} | 0 src/hal/mk/{FSESC67PRO.d => FSESC67MINI.d} | 0 src/main.c | 144 +++++++++++---------- src/main.h | 31 +++-- src/phobia/lse.c | 2 +- src/phobia/pm.c | 105 +++++++++------ src/phobia/pm.h | 4 +- src/phobia/pm_fsm.c | 41 ++---- src/regdefs.h | 10 +- src/regfile.c | 11 +- src/shdefs.h | 1 - src/shell.c | 12 +- 20 files changed, 285 insertions(+), 207 deletions(-) rename src/hal/hw/{FSESC67PRO.h => FSESC67MINI.h} (100%) rename src/hal/mk/{FSESC67PRO.d => FSESC67MINI.d} (100%) diff --git a/bench/tsfunc.c b/bench/tsfunc.c index 498073a..d543f19 100644 --- a/bench/tsfunc.c +++ b/bench/tsfunc.c @@ -684,7 +684,7 @@ void ts_script_test() m.Udc = 48.; m.Rdc = 0.1; m.Zp = 5; - m.lambda = blm_Kv_lambda(&m, 525.); + m.lambda = blm_Kv_lambda(&m, 525.); m.Jm = 2.E-4; ts_script_default(); @@ -695,7 +695,7 @@ void ts_script_test() blm_restart(&m); /*ts_script_hfi(); - blm_restart(&m);*/ + blm_restart(&m);*/ printf("\n---- Turnigy RotoMax 1.20 ----\n"); @@ -707,7 +707,7 @@ void ts_script_test() m.Udc = 22.; m.Rdc = 0.1; m.Zp = 14; - m.lambda = blm_Kv_lambda(&m, 270.); + m.lambda = blm_Kv_lambda(&m, 270.); m.Jm = 3.E-4; ts_script_default(); @@ -736,7 +736,7 @@ void ts_script_test() m.Udc = 48.; m.Rdc = 0.5; m.Zp = 15; - m.lambda = blm_Kv_lambda(&m, 15.7); + m.lambda = blm_Kv_lambda(&m, 15.); m.Jm = 6.E-3; ts_script_default(); @@ -751,5 +751,31 @@ void ts_script_test() ts_script_hall(); blm_restart(&m); + + printf("\n---- QS 138 (3000W) ----\n"); + + tlm_restart(); + + m.Rs = 4.E-3; + m.Ld = 31.E-6; + m.Lq = 44.E-6; + m.Udc = 48.; + m.Rdc = 0.1; + m.Zp = 5; + m.lambda = blm_Kv_lambda(&m, 58.); + m.Jm = 4.E-3; + + ts_script_default(); + ts_script_base(); + blm_restart(&m); + + ts_script_speed(); + blm_restart(&m); + + /*ts_script_weakening(); + blm_restart(&m);*/ + + ts_script_hall(); + blm_restart(&m); } diff --git a/doc/HardwareVESC.md b/doc/HardwareVESC.md index 0f81302..928f7ad 100644 --- a/doc/HardwareVESC.md +++ b/doc/HardwareVESC.md @@ -7,7 +7,7 @@ This page gives you an overview of VESC hardware supported by PMC. | Item | HWREV | Notes | |:--------------------------------------|:-----------:|:------------------| | FLIPSKY FSESC 6.7 | FSESC67 | Bad PCB design | -| FLIPSKY MINI FSESC 6.7 PRO | FSESC67PRO | Low side shunts | +| FLIPSKY MINI FSESC 6.7 PRO | FSESC67MINI | Low side shunts | | FLIPSKY 75100 V202 ESC | FSESC75100 | Low side shunts | | Makerbase VESC 75200 V2 84V 200A | FSESC75100 | Low side shunts | | Holybro Mini FOC ESC Based on VESC6 | HBESC6FOC | | diff --git a/doc/InputAnalogKnob.md b/doc/InputAnalogKnob.md index e724d63..a7f4243 100644 --- a/doc/InputAnalogKnob.md +++ b/doc/InputAnalogKnob.md @@ -90,14 +90,14 @@ Now you are ready to enable the analog knob interface. To stop the control we check if machine is run or setpoint is high. If setpoint is out of input range and machine does not make full turns for -`ap.idle_timeout` seconds the shutdown is requested. +`ap.timeout_IDLE` seconds the shutdown is requested. - (pmc) reg ap.idle_timeout + (pmc) reg ap.timeout_IDLE # Disarm timeout To ensure a safe startup it is required to hold low `ANG` signal for -`ap.disarm_timeout` seconds until disarmed state was reset. +`ap.timeout_DISARM` seconds until disarmed state was reset. - (pmc) reg ap.disarm_timeout + (pmc) reg ap.timeout_DISARM diff --git a/phobia/link.c b/phobia/link.c index 69f2b47..4bc49b7 100644 --- a/phobia/link.c +++ b/phobia/link.c @@ -192,7 +192,7 @@ link_fetch_network(struct link_pmc *lp) if (strstr(lbuf, "(pmc)") == lbuf) { - sprintf(lp->network, "local"); + sprintf(lp->network, "SERIAL"); rc = 1; } @@ -201,7 +201,7 @@ link_fetch_network(struct link_pmc *lp) if (lk_stoi(&n, lbuf + 5) != NULL) { - sprintf(lp->network, "remote/%i", n); + sprintf(lp->network, "REMOTE/%i", n); } else { lp->network[0] = 0; @@ -370,10 +370,7 @@ link_fetch_hwinfo(struct link_pmc *lp) tok = lk_token(&sp); - if (strcmp(tok, "OK") != 0) { - - sprintf(priv->hw_crc32 + strlen(priv->hw_crc32), " (%.16s)", tok); - } + sprintf(priv->hw_crc32 + strlen(priv->hw_crc32), " (%.16s)", tok); sprintf(lp->hwinfo, "%.16s / %.16s / %.36s", priv->hw_revision, priv->hw_build, priv->hw_crc32); diff --git a/phobia/phobia.c b/phobia/phobia.c index 963485c..4964504 100644 --- a/phobia/phobia.c +++ b/phobia/phobia.c @@ -185,6 +185,8 @@ pub_primal_reg(struct public *pub, const struct link_reg *reg) { const char *primal_map[] = { + "pm.scale_uS0", + "pm.scale_uS1", "pm.probe_current_hold", "pm.probe_current_weak", "pm.probe_current_sine", @@ -2812,6 +2814,12 @@ page_diagnose(struct public *pub) nk_layout_row_dynamic(ctx, 0, 1); nk_spacer(ctx); + reg_float(pub, "pm.scale_uS0", "DC link voltage offset"); + reg_float(pub, "pm.scale_uS1", "DC link voltage scale"); + + nk_layout_row_dynamic(ctx, 0, 1); + nk_spacer(ctx); + reg_enum_toggle(pub, "pm.tvm_ACTIVE", "TVM is active"); reg_float(pub, "pm.tvm_FIR_A_tau", "A voltage FIR tau"); reg_float(pub, "pm.tvm_FIR_B_tau", "B voltage FIR tau"); @@ -3649,8 +3657,8 @@ page_in_pwm(struct public *pub) nk_layout_row_dynamic(ctx, 0, 1); nk_spacer(ctx); - reg_float(pub, "ap.idle_timeout", "IDLE timeout"); - reg_float(pub, "ap.disarm_timeout", "DISARM timeout"); + reg_float(pub, "ap.timeout_DISARM", "Timeout DISARM "); + reg_float(pub, "ap.timeout_IDLE", "Timeout IDLE"); nk_layout_row_dynamic(ctx, 0, 1); nk_spacer(ctx); @@ -3727,8 +3735,8 @@ page_in_knob(struct public *pub) nk_layout_row_dynamic(ctx, 0, 1); nk_spacer(ctx); - reg_float(pub, "ap.idle_timeout", "IDLE timeout"); - reg_float(pub, "ap.disarm_timeout", "DISARM timeout"); + reg_float(pub, "ap.timeout_DISARM", "Timeout DISARM"); + reg_float(pub, "ap.timeout_IDLE", "Timeout IDLE"); nk_layout_row_dynamic(ctx, 0, 1); nk_spacer(ctx); @@ -3739,27 +3747,49 @@ static void page_application(struct public *pub) { struct nk_sdl *nk = pub->nk; + struct link_pmc *lp = pub->lp; struct nk_context *ctx = &nk->ctx; + struct link_reg *reg; nk_layout_row_dynamic(ctx, 0, 1); nk_spacer(ctx); reg_enum_toggle(pub, "ap.task_AUTOSTART", "Startup automatic"); + + reg = link_reg_lookup(lp, "ap.task_AUTOSTART"); + + if ( reg != NULL + && reg->lval != 0) { + + nk_layout_row_dynamic(ctx, 0, 1); + nk_spacer(ctx); + + reg_float(pub, "ap.auto_reg_DATA", "Auto register DATA"); + reg_linked(pub, "ap.auto_reg_ID", "Auto register ID"); + + nk_layout_row_dynamic(ctx, 0, 1); + nk_spacer(ctx); + } + reg_enum_toggle(pub, "ap.task_BUTTON", "Button control"); reg_enum_toggle(pub, "ap.task_AS5047", "AS5047 magnetic encoder"); reg_enum_toggle(pub, "ap.task_HX711", "HX711 load cell ADC"); - reg_enum_toggle(pub, "ap.task_MPU6050", "MPU6050 inertial unit"); - nk_layout_row_dynamic(ctx, 0, 1); - nk_spacer(ctx); + reg = link_reg_lookup(lp, "ap.task_HX711"); - reg_float(pub, "ap.auto_reg_DATA", "Auto register DATA"); - reg_linked(pub, "ap.auto_reg_ID", "Auto register ID"); + if ( reg != NULL + && reg->lval != 0) { - nk_layout_row_dynamic(ctx, 0, 1); - nk_spacer(ctx); + nk_layout_row_dynamic(ctx, 0, 1); + nk_spacer(ctx); + + reg_float(pub, "ap.load_HX711", "HX711 measurement"); + + nk_layout_row_dynamic(ctx, 0, 1); + nk_spacer(ctx); + } - reg_float(pub, "ap.load_HX711", "HX711 measurement"); + reg_enum_toggle(pub, "ap.task_MPU6050", "MPU6050 inertial unit"); nk_layout_row_dynamic(ctx, 0, 1); nk_spacer(ctx); @@ -3827,8 +3857,8 @@ page_thermal(struct public *pub) reg_float(pub, "ap.heat_PCB_temp_derate", "PCB derate threshold"); reg_float(pub, "ap.heat_PCB_temp_FAN", "PCB fan ON threshold"); reg_float(pub, "ap.heat_EXT_temp_derate", "EXT derate threshold"); - reg_float(pub, "ap.heat_derated_PCB", "PCB derated current"); - reg_float(pub, "ap.heat_derated_EXT", "EXT derated current"); + reg_float(pub, "ap.heat_maximal_PCB", "PCB maximal current"); + reg_float(pub, "ap.heat_maximal_EXT", "EXT maximal current"); reg_float(pub, "ap.heat_temp_recovery", "Recovery hysteresis"); nk_layout_row_dynamic(ctx, 0, 1); @@ -3901,6 +3931,7 @@ page_config(struct public *pub) nk_spacer(ctx); reg_enum_toggle(pub, "pm.config_LU_FORCED", "Forced control"); + reg_enum_toggle(pub, "pm.config_LU_FREEWHEEL", "Allow freewheeling"); reg_enum_combo(pub, "pm.config_LU_ESTIMATE", "Sensorless estimate", 1); reg_enum_combo(pub, "pm.config_LU_SENSOR", "Position sensor type", 1); reg_enum_combo(pub, "pm.config_LU_LOCATION", "Servo location source", 1); @@ -3914,8 +3945,8 @@ page_config(struct public *pub) reg_enum_toggle(pub, "pm.config_RELUCTANCE", "Reluctance MTPA control"); reg_enum_toggle(pub, "pm.config_WEAKENING", "Flux weakening control"); - reg_enum_toggle(pub, "pm.config_REVERSE_BRAKE", "Reverse brake"); - reg_enum_toggle(pub, "pm.config_SPEED_MAXIMAL", "Speed constraints"); + reg_enum_toggle(pub, "pm.config_REVERSE_BRAKE", "Reverse brake function"); + reg_enum_toggle(pub, "pm.config_SPEED_MAXIMAL", "Speed maximal function"); reg_enum_combo(pub, "pm.config_EABI_FRONTEND", "EABI frontend", 0); reg_enum_combo(pub, "pm.config_SINCOS_FRONTEND", "SINCOS frontend", 0); diff --git a/src/app/autostart.c b/src/app/autostart.c index 8b2a92b..867896c 100644 --- a/src/app/autostart.c +++ b/src/app/autostart.c @@ -25,7 +25,8 @@ LD_TASK void app_AUTOSTART(void *pData) do { vTaskDelay((TickType_t) 100); - if (pm.lu_MODE == PM_LU_DISABLED) { + if ( pm.lu_MODE == PM_LU_DISABLED + && pm.const_fb_U > pm.watt_uDC_minimal) { pm.fsm_req = PM_STATE_LU_STARTUP; diff --git a/src/app/mpu6050.c b/src/app/mpu6050.c index b66265c..7bf4ab5 100644 --- a/src/app/mpu6050.c +++ b/src/app/mpu6050.c @@ -11,6 +11,13 @@ LD_TASK void app_MPU6050(void *pData) { - /* TODO */ + volatile int *knob = (volatile int *) pData; + + do { + /* TODO */ + } + while (*knob != 0); + + vTaskDelete(NULL); } diff --git a/src/epcan.c b/src/epcan.c index b40ebc4..223b5a1 100644 --- a/src/epcan.c +++ b/src/epcan.c @@ -155,7 +155,7 @@ EPCAN_pipe_message_IN(const CAN_msg_t *msg) ep->tx_N = 0; if ( ep->ACTIVE == PM_ENABLED - && pm.fsm_errno != PM_OK) { + && pm.lu_MODE == PM_LU_DISABLED) { ep->ACTIVE = PM_DISABLED; } @@ -164,7 +164,6 @@ EPCAN_pipe_message_IN(const CAN_msg_t *msg) && ep->ACTIVE != PM_ENABLED && pm.lu_MODE == PM_LU_DISABLED) { - pm.fsm_errno = PM_OK; pm.fsm_req = PM_STATE_LU_STARTUP; ep->ACTIVE = PM_ENABLED; diff --git a/src/hal/hw/FSESC67PRO.h b/src/hal/hw/FSESC67MINI.h similarity index 100% rename from src/hal/hw/FSESC67PRO.h rename to src/hal/hw/FSESC67MINI.h diff --git a/src/hal/mk/FSESC67PRO.d b/src/hal/mk/FSESC67MINI.d similarity index 100% rename from src/hal/mk/FSESC67PRO.d rename to src/hal/mk/FSESC67MINI.d diff --git a/src/main.c b/src/main.c index fc4cda2..d59d844 100644 --- a/src/main.c +++ b/src/main.c @@ -119,64 +119,61 @@ GPIO_set_state_FAN(int knob) #endif /* HW_HAVE_FAN_CONTROL */ static int -elapsed_IDLE() +timeout_DISARM() { - TickType_t xIDLE, xNOW; + TickType_t xOUT, xNOW; - xIDLE = (TickType_t) (ap.idle_timeout * (float) configTICK_RATE_HZ); + xOUT = (TickType_t) (ap.timeout_DISARM * ((float) configTICK_RATE_HZ / 1000.f)); - if (xIDLE > 0) { + if (xOUT != 0) { xNOW = xTaskGetTickCount(); - if (xNOW - (TickType_t) ap.idle_INVOKE > (TickType_t) 50) { + if (xNOW - (TickType_t) ap.disarm_INK > (TickType_t) 40) { - ap.idle_RESET = xNOW; + ap.disarm_RST = xNOW; } - ap.idle_INVOKE = xNOW; + ap.disarm_INK = xNOW; - if (pm.lu_total_revol != ap.idle_revol_cached) { + if (xNOW - (TickType_t) ap.disarm_RST < xOUT) { - ap.idle_RESET = xNOW; - ap.idle_revol_cached = pm.lu_total_revol; - } - - if (xNOW - (TickType_t) ap.idle_RESET > xIDLE) { - - return PM_ENABLED; + return PM_DISABLED; } } - return PM_DISABLED; + return PM_ENABLED; } static int -elapsed_DISARM() +timeout_IDLE() { - TickType_t xDISARM, xNOW; + TickType_t xOUT, xNOW; - xDISARM = (TickType_t) (ap.disarm_timeout * (float) configTICK_RATE_HZ); + xOUT = (TickType_t) (ap.timeout_IDLE * ((float) configTICK_RATE_HZ / 1000.f)); - if (xDISARM > 0) { + if (xOUT != 0) { xNOW = xTaskGetTickCount(); - if (xNOW - (TickType_t) ap.disarm_INVOKE > (TickType_t) 50) { + if (xNOW - (TickType_t) ap.idle_INK > (TickType_t) 40) { - ap.disarm_RESET = xNOW; + ap.idle_RST = xNOW; } - ap.disarm_INVOKE = xNOW; + ap.idle_INK = xNOW; + + if (pm.lu_total_revol != ap.idle_revol) { - if (xNOW - (TickType_t) ap.disarm_RESET > xDISARM) { + ap.idle_RST = xNOW; + ap.idle_revol = pm.lu_total_revol; + } + + if (xNOW - (TickType_t) ap.idle_RST > xOUT) { return PM_ENABLED; } } - else { - return PM_ENABLED; - } return PM_DISABLED; } @@ -184,7 +181,9 @@ elapsed_DISARM() LD_TASK void task_TEMP(void *pData) { TickType_t xWake; - float x_PCB, x_EXT, lock_PCB; + + float maximal_PCB, maximal_EXT, lock_PCB; + int fsm_errno_last; if (ap.ntc_PCB.type != NTC_NONE) { @@ -198,11 +197,13 @@ LD_TASK void task_TEMP(void *pData) xWake = xTaskGetTickCount(); - x_PCB = PM_MAX_F; - x_EXT = PM_MAX_F; + maximal_PCB = PM_MAX_F; + maximal_EXT = PM_MAX_F; lock_PCB = 0.f; + fsm_errno_last = PM_OK; + do { /* 10 Hz. * */ @@ -228,7 +229,7 @@ LD_TASK void task_TEMP(void *pData) if (ap.temp_PCB > ap.heat_PCB_temp_halt - lock_PCB) { - x_PCB = 0.f; + maximal_PCB = 0.f; if (pm.lu_MODE != PM_LU_DISABLED) { @@ -243,11 +244,11 @@ LD_TASK void task_TEMP(void *pData) /* Derate current in case of PCB thermal overload. * */ - x_PCB = ap.heat_derated_PCB; + maximal_PCB = ap.heat_maximal_PCB; } else if (ap.temp_PCB < ap.heat_PCB_temp_derate - ap.heat_temp_recovery) { - x_PCB = PM_MAX_F; + maximal_PCB = PM_MAX_F; } lock_PCB = 0.f; @@ -273,15 +274,15 @@ LD_TASK void task_TEMP(void *pData) /* Derate current in case of external thermal * overload (machine overheat protection). * */ - x_EXT = ap.heat_derated_EXT; + maximal_EXT = ap.heat_maximal_EXT; } else if (ap.temp_EXT < ap.heat_EXT_temp_derate - ap.heat_temp_recovery) { - x_EXT = PM_MAX_F; + maximal_EXT = PM_MAX_F; } } - pm.i_maximal_on_PCB = (x_PCB < x_EXT) ? x_PCB : x_EXT; + pm.i_maximal_on_PCB = (maximal_PCB < maximal_EXT) ? maximal_PCB : maximal_EXT; #ifdef HW_HAVE_DRV_ON_PCB if ( hal.DRV.auto_RESTART == PM_ENABLED @@ -307,8 +308,12 @@ LD_TASK void task_TEMP(void *pData) } #endif /* GPIO_LED_MODE */ - if ( pm.fsm_errno != PM_OK - || log_status() != HAL_OK) { + if (pm.fsm_errno != PM_OK) { + + if (pm.fsm_errno != fsm_errno_last) { + + log_TRACE("FSM errno %s" EOL, pm_strerror(pm.fsm_errno)); + } if ((xWake & (TickType_t) 0x3FFU) < (TickType_t) 205) { @@ -318,6 +323,8 @@ LD_TASK void task_TEMP(void *pData) GPIO_set_LOW(GPIO_LED_ALERT); } } + + fsm_errno_last = pm.fsm_errno; } while (1); } @@ -328,13 +335,15 @@ conv_KNOB() { float control, range, scaled; + int hold_FLAG = PM_DISABLED; + if ( ap.knob_ACTIVE == PM_ENABLED && pm.lu_MODE == PM_LU_DISABLED) { ap.knob_ACTIVE = PM_DISABLED; ap.knob_DISARM = PM_ENABLED; - ap.knob_FAULT = 0; + ap.knob_NFAULT = 0; } if ( ap.knob_in_ANG < ap.knob_range_LOS[0] @@ -349,9 +358,9 @@ conv_KNOB() if (pm.lu_MODE != PM_LU_DISABLED) { - ap.knob_FAULT++; + ap.knob_NFAULT++; - if (unlikely(ap.knob_FAULT >= 10)) { + if (unlikely(ap.knob_NFAULT >= 10)) { pm.fsm_errno = PM_ERROR_KNOB_CONTROL_FAULT; pm.fsm_req = PM_STATE_LU_SHUTDOWN; @@ -380,7 +389,7 @@ conv_KNOB() if (ap.knob_ACTIVE == PM_ENABLED) { - if (elapsed_IDLE() == PM_ENABLED) { + if (timeout_IDLE() == PM_ENABLED) { if (pm.lu_MODE != PM_LU_DISABLED) { @@ -392,13 +401,15 @@ conv_KNOB() } else if (ap.knob_DISARM == PM_ENABLED) { - if (elapsed_DISARM() == PM_ENABLED) { + if (timeout_DISARM() == PM_ENABLED) { ap.knob_DISARM = PM_DISABLED; } } } else { + hold_FLAG = PM_ENABLED; + if (scaled > 1.f) { scaled = 1.f; @@ -419,7 +430,7 @@ conv_KNOB() } } - ap.knob_FAULT = 0; + ap.knob_NFAULT = 0; } if (scaled < 0.f) { @@ -450,7 +461,8 @@ conv_KNOB() } #endif /* HW_HAVE_BRAKE_KNOB */ - if (ap.knob_reg_DATA != control) { + if ( ap.knob_reg_DATA != control + || hold_FLAG == PM_ENABLED) { ap.knob_reg_DATA = control; @@ -568,14 +580,6 @@ default_flash_load() net.ep[2].rate = net.ep[0].rate; net.ep[3].ID = 40; net.ep[3].rate = net.ep[0].rate; - net.ep[4].ID = 50; - net.ep[4].rate = net.ep[0].rate; - net.ep[5].ID = 60; - net.ep[5].rate = net.ep[0].rate; - net.ep[6].ID = 70; - net.ep[6].rate = net.ep[0].rate; - net.ep[7].ID = 80; - net.ep[7].rate = net.ep[0].rate; #endif /* HW_HAVE_NETWORK_EPCAN */ ap.ppm_reg_ID = ID_PM_S_SETPOINT_SPEED_KNOB; @@ -617,11 +621,8 @@ default_flash_load() ap.knob_control_BRK = - 100.f; #endif /* HW_HAVE_ANALOG_KNOB */ - ap.idle_timeout = 2.f; /* (s) */ - ap.disarm_timeout = 1.f; /* (s) */ - - ap.auto_reg_DATA = 0.f; - ap.auto_reg_ID = ID_PM_S_SETPOINT_SPEED_KNOB; + ap.timeout_DISARM = 1000.f; /* (ms) */ + ap.timeout_IDLE = 5000.f; /* (ms) */ #ifdef HW_HAVE_NTC_ON_PCB ap.ntc_PCB.type = HW_NTC_PCB_TYPE; @@ -645,10 +646,13 @@ default_flash_load() ap.heat_PCB_temp_derate = 90.f; /* (C) */ ap.heat_PCB_temp_FAN = 60.f; /* (C) */ ap.heat_EXT_temp_derate = 0.f; /* (C) */ - ap.heat_derated_PCB = 20.f; /* (A) */ - ap.heat_derated_EXT = 20.f; /* (A) */ + ap.heat_maximal_PCB = 10.f; /* (A) */ + ap.heat_maximal_EXT = 10.f; /* (A) */ ap.heat_temp_recovery = 5.f; /* (C) */ + ap.auto_reg_DATA = 0.f; + ap.auto_reg_ID = ID_PM_S_SETPOINT_SPEED_KNOB; + pm.m_freq = hal.PWM_frequency; pm.m_dT = 1.f / pm.m_freq; pm.dc_resolution = hal.PWM_resolution; @@ -839,6 +843,8 @@ conv_PULSE_WIDTH() { float control, range, scaled; + int hold_FLAG = PM_DISABLED; + if ( ap.ppm_ACTIVE == PM_ENABLED && pm.lu_MODE == PM_LU_DISABLED) { @@ -862,7 +868,7 @@ conv_PULSE_WIDTH() if (ap.ppm_ACTIVE == PM_ENABLED) { - if (elapsed_IDLE() == PM_ENABLED) { + if (timeout_IDLE() == PM_ENABLED) { if (pm.lu_MODE != PM_LU_DISABLED) { @@ -874,13 +880,15 @@ conv_PULSE_WIDTH() } else if (ap.ppm_DISARM == PM_ENABLED) { - if (elapsed_DISARM() == PM_ENABLED) { + if (timeout_DISARM() == PM_ENABLED) { ap.ppm_DISARM = PM_DISABLED; } } } else { + hold_FLAG = PM_ENABLED; + if (scaled > 1.f) { scaled = 1.f; @@ -911,7 +919,8 @@ conv_PULSE_WIDTH() control = ap.ppm_control[1] + range * scaled; } - if (ap.ppm_reg_DATA != control) { + if ( ap.ppm_reg_DATA != control + || hold_FLAG == PM_ENABLED) { ap.ppm_reg_DATA = control; @@ -932,16 +941,16 @@ in_PULSE_WIDTH() conv_PULSE_WIDTH(); - ap.ppm_FAULT = 0; + ap.ppm_NFAULT = 0; } else { if (ap.ppm_ACTIVE == PM_ENABLED) { if (pm.lu_MODE != PM_LU_DISABLED) { - ap.ppm_FAULT++; + ap.ppm_NFAULT++; - if (unlikely(ap.ppm_FAULT >= 10)) { + if (unlikely(ap.ppm_NFAULT >= 10)) { pm.fsm_errno = PM_ERROR_KNOB_CONTROL_FAULT; pm.fsm_req = PM_STATE_LU_SHUTDOWN; @@ -988,8 +997,7 @@ in_STEP_DIR() if (ap.step_STARTUP == PM_ENABLED) { - if ( pm.lu_MODE == PM_LU_DISABLED - && pm.fsm_errno == PM_OK) { + if (pm.lu_MODE == PM_LU_DISABLED) { pm.fsm_req = PM_STATE_LU_STARTUP; } diff --git a/src/main.h b/src/main.h index 9f0cc23..dc4960f 100644 --- a/src/main.h +++ b/src/main.h @@ -18,7 +18,7 @@ typedef struct { int ppm_STARTUP; int ppm_ACTIVE; int ppm_DISARM; - int ppm_FAULT; + int ppm_NFAULT; float ppm_range[3]; float ppm_control[3]; @@ -47,7 +47,7 @@ typedef struct { int knob_STARTUP; int knob_ACTIVE; int knob_DISARM; - int knob_FAULT; + int knob_NFAULT; float knob_range_ANG[3]; #ifdef HW_HAVE_BRAKE_KNOB float knob_range_BRK[2]; @@ -57,25 +57,24 @@ typedef struct { float knob_control_BRK; #endif /* HW_HAVE_ANALOG_KNOB */ - /* IDLE function. + /* Timeout functions. * */ - float idle_timeout; - int idle_RESET; - int idle_INVOKE; - int idle_revol_cached; + float timeout_DISARM; + float timeout_IDLE; - /* Disarm function. - * */ - float disarm_timeout; - int disarm_RESET; - int disarm_INVOKE; + uint32_t disarm_RST; + uint32_t disarm_INK; + + uint32_t idle_RST; + uint32_t idle_INK; + int idle_revol; /* NTC constants. * */ ntc_t ntc_PCB; ntc_t ntc_EXT; - /* Thermal info. + /* Thermal information. * */ float temp_PCB; float temp_EXT; @@ -87,11 +86,11 @@ typedef struct { float heat_PCB_temp_derate; float heat_PCB_temp_FAN; float heat_EXT_temp_derate; - float heat_derated_PCB; - float heat_derated_EXT; + float heat_maximal_PCB; + float heat_maximal_EXT; float heat_temp_recovery; - /* App enable knobs. + /* App enable/disable knobs. * */ int task_AUTOSTART; int task_BUTTON; diff --git a/src/phobia/lse.c b/src/phobia/lse.c index 50d5627..717c70f 100644 --- a/src/phobia/lse.c +++ b/src/phobia/lse.c @@ -155,7 +155,7 @@ lse_qrupdate(lse_t *ls, lse_upper_t *rm, lse_float_t *xz, int nz) } #else /* LSE_FAST_TRANSFORM */ - /* WARNING: We use NAIVE hypot implementation as it is + /* WARNING: We use naive hypot implementation as it is * the fastest one and quite ulp-accurate. */ alpa = lse_sqrtf(x0 * x0 + xi * xi); diff --git a/src/phobia/pm.c b/src/phobia/pm.c index 12816e1..f9d0723 100644 --- a/src/phobia/pm.c +++ b/src/phobia/pm.c @@ -105,6 +105,7 @@ pm_auto_config_default(pmc_t *pm) pm->config_VSI_ZERO = PM_VSI_GND; pm->config_VSI_CLAMP = PM_DISABLED; pm->config_LU_FORCED = PM_ENABLED; + pm->config_LU_FREEWHEEL = PM_ENABLED; pm->config_LU_ESTIMATE = PM_FLUX_ORTEGA; pm->config_LU_SENSOR = PM_SENSOR_NONE; pm->config_LU_LOCATION = PM_LOCATION_NONE; @@ -119,7 +120,6 @@ pm_auto_config_default(pmc_t *pm) pm->config_SPEED_MAXIMAL = PM_ENABLED; pm->config_EABI_FRONTEND = PM_EABI_INCREMENTAL; pm->config_SINCOS_FRONTEND = PM_SINCOS_ANALOG; - pm->config_BOOST_CHARGE = PM_DISABLED; pm->tm_transient_slow = 40.f; /* (ms) */ pm->tm_transient_fast = 2.f; /* (ms) */ @@ -714,16 +714,7 @@ pm_forced(pmc_t *pm) { float wSP, dSA, xRF; - /* Get the SETPOINT of forced speed. - * */ - if (pm->config_LU_DRIVE == PM_DRIVE_CURRENT) { - - wSP = (pm->i_setpoint_current < - M_EPSILON) ? - PM_MAX_F - : (pm->i_setpoint_current > M_EPSILON) ? PM_MAX_F : 0.f; - } - else { - wSP = pm->s_setpoint_speed; - } + wSP = pm->s_setpoint_speed; /* Maximal forced speed constraint. * */ @@ -1736,7 +1727,6 @@ static void pm_lu_FSM(pmc_t *pm) { float lu_F[2], hS, A, B; - int lu_EABI = PM_DISABLED; /* Get the current on DQ-axes. @@ -1779,15 +1769,46 @@ pm_lu_FSM(pmc_t *pm) pm->lu_mq_produce = 0.f; } + if ( pm->config_LU_FORCED == PM_ENABLED + && pm->config_LU_DRIVE == PM_DRIVE_CURRENT) { + + float wSP; + + /* Derive the speed SETPOINT in case of current control. + * */ + wSP = (pm->i_setpoint_current < - M_EPSILON) ? - pm->forced_reverse + : (pm->i_setpoint_current > M_EPSILON) ? pm->forced_maximal : 0.f; + + pm->s_setpoint_speed = wSP; + } + if (pm->lu_MODE == PM_LU_DETACHED) { - if (pm->base_TIM >= 0) { + if (pm->flux_DETACH != PM_ENABLED) { - pm_flux_detached(pm); + pm->base_TIM = - PM_TSMS(pm, pm->tm_transient_fast); + pm->detach_TIM = 0; + + pm->watt_DC_MAX = PM_DISABLED; + pm->watt_DC_MIN = PM_DISABLED; + + pm->watt_lpf_D = 0.f; + pm->watt_lpf_Q = 0.f; + pm->watt_drain_wP = 0.f; + pm->watt_drain_wA = 0.f; + pm->watt_integral = 0.f; + + pm->i_track_D = 0.f; + pm->i_track_Q = 0.f; + pm->i_integral_D = 0.f; + pm->i_integral_Q = 0.f; + + pm->flux_DETACH = PM_ENABLED; } - if (pm->config_LU_ESTIMATE != PM_FLUX_NONE) { + if (pm->base_TIM >= 0) { + pm_flux_detached(pm); pm_flux_zone(pm); } @@ -1836,7 +1857,9 @@ pm_lu_FSM(pmc_t *pm) pm->proc_set_Z(PM_Z_NONE); } - else if (pm->config_LU_FORCED == PM_ENABLED) { + else if ( pm->config_LU_FORCED == PM_ENABLED + && ( pm->config_LU_FREEWHEEL != PM_ENABLED + || m_fabsf(pm->s_setpoint_speed) > M_EPSILON)) { pm->lu_MODE = PM_LU_FORCED; @@ -1892,6 +1915,17 @@ pm_lu_FSM(pmc_t *pm) pm->lu_MODE = PM_LU_ON_HFI; } + else if ( PM_CONFIG_TVM(pm) == PM_ENABLED + && pm->config_LU_FREEWHEEL == PM_ENABLED + && pm->forced_track_D < M_EPSILON) { + + pm->lu_MODE = PM_LU_DETACHED; + + pm->flux_DETACH = PM_DISABLED; + pm->flux_TYPE = PM_FLUX_NONE; + + pm->proc_set_Z(PM_Z_ABC); + } } } else if (pm->lu_MODE == PM_LU_ESTIMATE) { @@ -1934,7 +1968,9 @@ pm_lu_FSM(pmc_t *pm) pm->hold_TIM = 0; } - else if (pm->config_LU_FORCED == PM_ENABLED) { + else if ( pm->config_LU_FORCED == PM_ENABLED + && ( pm->config_LU_FREEWHEEL != PM_ENABLED + || m_fabsf(pm->s_setpoint_speed) > M_EPSILON)) { pm->lu_MODE = PM_LU_FORCED; @@ -1948,26 +1984,9 @@ pm_lu_FSM(pmc_t *pm) pm->lu_MODE = PM_LU_DETACHED; - pm->base_TIM = - PM_TSMS(pm, pm->tm_transient_fast); - pm->detach_TIM = 0; - + pm->flux_DETACH = PM_DISABLED; pm->flux_TYPE = PM_FLUX_NONE; - pm->lu_uD = 0.f; - pm->lu_uQ = 0.f; - - pm->watt_lpf_D = 0.f; - pm->watt_lpf_Q = 0.f; - pm->watt_drain_wP = 0.f; - pm->watt_drain_wA = 0.f; - - pm->i_track_D = 0.f; - pm->i_track_Q = 0.f; - pm->i_integral_D = 0.f; - pm->i_integral_Q = 0.f; - - pm->watt_integral = 0.f; - pm->proc_set_Z(PM_Z_ABC); } } @@ -2130,6 +2149,9 @@ pm_lu_FSM(pmc_t *pm) if (pm->config_EABI_FRONTEND == PM_EABI_INCREMENTAL) { + /* We need to adjust the position again + * after loss of tracking. + * */ pm->eabi_ADJUST = PM_DISABLED; } } @@ -2684,7 +2706,18 @@ pm_loop_current(pmc_t *pm) if (pm->lu_MODE == PM_LU_FORCED) { - track_D = pm->forced_hold_D; + if (pm->config_LU_FREEWHEEL != PM_ENABLED) { + + track_D = pm->forced_hold_D; + } + else if ( m_fabsf(pm->s_setpoint_speed) > M_EPSILON + || m_fabsf(pm->forced_wS) > M_EPSILON) { + + track_D = pm->forced_hold_D; + } + else { + track_D = 0.f; + } } else { track_D = 0.f; diff --git a/src/phobia/pm.h b/src/phobia/pm.h index 97b41af..5af0524 100644 --- a/src/phobia/pm.h +++ b/src/phobia/pm.h @@ -250,6 +250,7 @@ typedef struct { int config_VSI_ZERO; int config_VSI_CLAMP; int config_LU_FORCED; + int config_LU_FREEWHEEL; int config_LU_ESTIMATE; int config_LU_SENSOR; int config_LU_LOCATION; @@ -264,7 +265,6 @@ typedef struct { int config_SPEED_MAXIMAL; int config_EABI_FRONTEND; int config_SINCOS_FRONTEND; - int config_BOOST_CHARGE; /* TODO */ int fsm_req; int fsm_state; @@ -407,6 +407,8 @@ typedef struct { float forced_track_D; float forced_stop_DC; + int flux_DETACH; + int detach_TIM; float detach_threshold; float detach_trip_AP; diff --git a/src/phobia/pm_fsm.c b/src/phobia/pm_fsm.c index c2d6727..908d9ca 100644 --- a/src/phobia/pm_fsm.c +++ b/src/phobia/pm_fsm.c @@ -5,28 +5,6 @@ static void pm_fsm_state_idle(pmc_t *pm) { /* TODO */ - - /*if ( pm->lu_MODE == PM_LU_DISABLED - && pm->config_BOOST_CHARGE == PM_ENABLED) { - - switch (pm->fsm_phase) { - - case 0: - pm->proc_set_DC(0, 0, 0); - pm->proc_set_Z(5); - - pm->fsm_phase = 1; - break; - - case 1: - if (m_fabsf(pm->fb_iB) > pm->fault_current_tol) { - - pm->fsm_state = PM_STATE_LOOP_BOOST; - pm->fsm_phase = 0; - } - break; - } - }*/ } static void @@ -1605,20 +1583,24 @@ pm_fsm_state_lu_startup(pmc_t *pm, int in_ZONE) pm->forced_wS = 0.f; pm->forced_track_D = 0.f; + pm->flux_DETACH = PM_DISABLED; pm->detach_TIM = 0; - if ( pm->config_LU_ESTIMATE != PM_FLUX_NONE - && pm->config_EXCITATION == PM_EXCITATION_CONST - && pm->const_lambda < M_EPSILON) { + if (pm->config_LU_ESTIMATE == PM_FLUX_NONE) { + + pm->flux_LINKAGE = PM_ENABLED; + } + else if (pm->config_EXCITATION == PM_EXCITATION_NONE) { + + pm->flux_LINKAGE = PM_ENABLED; + } + else if (pm->const_lambda < M_EPSILON) { /* Here we indicate that flux linkage * is to be estimated further. * */ pm->flux_LINKAGE = PM_DISABLED; } - else { - pm->flux_LINKAGE = PM_ENABLED; - } pm->flux_TYPE = PM_FLUX_NONE; pm->flux_ZONE = in_ZONE; @@ -1647,6 +1629,9 @@ pm_fsm_state_lu_startup(pmc_t *pm, int in_ZONE) if (pm->config_EABI_FRONTEND == PM_EABI_INCREMENTAL) { + /* We need to adjust the position again + * after loss of tracking. + * */ pm->eabi_ADJUST = PM_DISABLED; } diff --git a/src/regdefs.h b/src/regdefs.h index 933a1b0..8b0aa80 100644 --- a/src/regdefs.h +++ b/src/regdefs.h @@ -131,8 +131,8 @@ ID_AP_KNOB_CONTROL_ANG2, ID_AP_KNOB_CONTROL_BRK, #endif /* HW_HAVE_BRAKE_KNOB */ #endif /* HW_HAVE_ANALOG_KNOB */ -ID_AP_IDLE_TIMEOUT, -ID_AP_DISARM_TIMEOUT, +ID_AP_TIMEOUT_DISARM, +ID_AP_TIMEOUT_IDLE, #ifdef HW_HAVE_NTC_ON_PCB ID_AP_NTC_PCB_TYPE, ID_AP_NTC_PCB_BALANCE, @@ -156,8 +156,8 @@ ID_AP_HEAT_PCB_TEMP_HALT, ID_AP_HEAT_PCB_TEMP_DERATE, ID_AP_HEAT_PCB_TEMP_FAN, ID_AP_HEAT_EXT_TEMP_DERATE, -ID_AP_HEAT_DERATED_PCB, -ID_AP_HEAT_DERATED_EXT, +ID_AP_HEAT_MAXIMAL_PCB, +ID_AP_HEAT_MAXIMAL_EXT, ID_AP_HEAT_TEMP_RECOVERY, ID_AP_TASK_AUTOSTART, ID_AP_TASK_BUTTON, @@ -184,6 +184,7 @@ ID_PM_CONFIG_DBG, ID_PM_CONFIG_VSI_ZERO, ID_PM_CONFIG_VSI_CLAMP, ID_PM_CONFIG_LU_FORCED, +ID_PM_CONFIG_LU_FREEWHEEL, ID_PM_CONFIG_LU_ESTIMATE, ID_PM_CONFIG_LU_SENSOR, ID_PM_CONFIG_LU_LOCATION, @@ -198,7 +199,6 @@ ID_PM_CONFIG_REVERSE_BRAKE, ID_PM_CONFIG_SPEED_MAXIMAL, ID_PM_CONFIG_EABI_FRONTEND, ID_PM_CONFIG_SINCOS_FRONTEND, -ID_PM_CONFIG_BOOST_CHARGE, ID_PM_FSM_REQ, ID_PM_FSM_STATE, ID_PM_FSM_ERRNO, diff --git a/src/regfile.c b/src/regfile.c index 6b76288..603a89a 100644 --- a/src/regfile.c +++ b/src/regfile.c @@ -1255,6 +1255,7 @@ reg_format_enum(const reg_t *reg) case ID_PM_CONFIG_DBG: case ID_PM_CONFIG_VSI_CLAMP: case ID_PM_CONFIG_LU_FORCED: + case ID_PM_CONFIG_LU_FREEWHEEL: case ID_PM_CONFIG_HFI_PERMANENT: case ID_PM_CONFIG_RELUCTANCE: case ID_PM_CONFIG_WEAKENING: @@ -1642,8 +1643,8 @@ const reg_t regfile[] = { #endif /* HW_HAVE_BRAKE_KNOB */ #endif /* HW_HAVE_ANALOG_KNOB */ - REG_DEF(ap.idle_timeout,,, "s", "%1f", REG_CONFIG, NULL, NULL), - REG_DEF(ap.disarm_timeout,,, "s", "%1f", REG_CONFIG, NULL, NULL), + REG_DEF(ap.timeout_DISARM,,, "s", "%1f", REG_CONFIG, NULL, NULL), + REG_DEF(ap.timeout_IDLE,,, "s", "%1f", REG_CONFIG, NULL, NULL), #ifdef HW_HAVE_NTC_ON_PCB REG_DEF(ap.ntc_PCB.type,,, "", "%0i", REG_CONFIG, NULL, ®_format_enum), @@ -1671,8 +1672,8 @@ const reg_t regfile[] = { REG_DEF(ap.heat_PCB_temp_derate,,, "C", "%1f", REG_CONFIG, NULL, NULL), REG_DEF(ap.heat_PCB_temp_FAN,,, "C", "%1f", REG_CONFIG, NULL, NULL), REG_DEF(ap.heat_EXT_temp_derate,,, "C", "%1f", REG_CONFIG, NULL, NULL), - REG_DEF(ap.heat_derated_PCB,,, "A", "%3f", REG_CONFIG, NULL, NULL), - REG_DEF(ap.heat_derated_EXT,,, "A", "%3f", REG_CONFIG, NULL, NULL), + REG_DEF(ap.heat_maximal_PCB,,, "A", "%3f", REG_CONFIG, NULL, NULL), + REG_DEF(ap.heat_maximal_EXT,,, "A", "%3f", REG_CONFIG, NULL, NULL), REG_DEF(ap.heat_temp_recovery,,, "C", "%1f", REG_CONFIG, NULL, NULL), REG_DEF(ap.task_AUTOSTART,,, "", "%0i", REG_CONFIG, ®_proc_task, ®_format_enum), @@ -1706,6 +1707,7 @@ const reg_t regfile[] = { REG_DEF(pm.config_VSI_ZERO,,, "", "%0i", REG_CONFIG, NULL, ®_format_enum), REG_DEF(pm.config_VSI_CLAMP,,, "", "%0i", REG_CONFIG, NULL, ®_format_enum), REG_DEF(pm.config_LU_FORCED,,, "", "%0i", REG_CONFIG, NULL, ®_format_enum), + REG_DEF(pm.config_LU_FREEWHEEL,,, "", "%0i", REG_CONFIG, NULL, ®_format_enum), REG_DEF(pm.config_LU_ESTIMATE,,, "", "%0i", REG_CONFIG, NULL, ®_format_enum), REG_DEF(pm.config_LU_SENSOR,,, "", "%0i", REG_CONFIG, NULL, ®_format_enum), REG_DEF(pm.config_LU_LOCATION,,, "", "%0i", REG_CONFIG, NULL, ®_format_enum), @@ -1720,7 +1722,6 @@ const reg_t regfile[] = { REG_DEF(pm.config_SPEED_MAXIMAL,,, "", "%0i", REG_CONFIG, NULL, ®_format_enum), REG_DEF(pm.config_EABI_FRONTEND,,, "", "%0i", REG_CONFIG, NULL, ®_format_enum), REG_DEF(pm.config_SINCOS_FRONTEND,,, "", "%0i", REG_CONFIG, NULL, ®_format_enum), - REG_DEF(pm.config_BOOST_CHARGE,,, "", "%0i", REG_CONFIG, NULL, ®_format_enum), REG_DEF(pm.fsm_req,,, "", "%0i", 0, NULL, NULL), REG_DEF(pm.fsm_state,,, "", "%0i", REG_READ_ONLY, NULL, NULL), diff --git a/src/shdefs.h b/src/shdefs.h index 32dcc66..d59efc7 100644 --- a/src/shdefs.h +++ b/src/shdefs.h @@ -34,7 +34,6 @@ SH_DEF(tlm_watch) SH_DEF(tlm_stop) SH_DEF(tlm_flush_sync) SH_DEF(tlm_live_sync) -SH_DEF(sh_keycodes) #ifdef HW_HAVE_NETWORK_EPCAN SH_DEF(net_survey) SH_DEF(net_assign) diff --git a/src/shell.c b/src/shell.c index 49b82d1..656a6f3 100644 --- a/src/shell.c +++ b/src/shell.c @@ -670,15 +670,5 @@ LD_TASK void task_CMDSH(void *pData) #undef SH_DEF #define SH_DEF(name) void name(const char *s) -SH_DEF(sh_keycodes) -{ - int c; - - do { - c = getc(); - - printf("%2x" EOL, c); - } - while (c != K_EOT && c != 'D'); -} +/* TODO */