From 786817d9a3ca9dc080f746b4e31aafd9914ffe72 Mon Sep 17 00:00:00 2001 From: Roman Belov Date: Tue, 6 Feb 2024 20:57:08 +0300 Subject: [PATCH] Improved DC link voltage constraints, DRV updates. --- bench/Makefile | 4 +- bench/bench.c | 67 ++++++--- bench/blm.c | 2 +- bench/tsfunc.c | 8 +- bench/tsfunc.h | 2 +- doc/CommandLineInterface.md | 2 +- doc/HardwareDesign.md | 23 ++- doc/HardwareVESC.md | 2 +- doc/InputAnalogKnob.md | 4 +- doc/MachineProbe.md | 6 +- doc/MachineTuning.md | 10 +- phobia/config.c | 2 +- phobia/link.c | 3 +- phobia/nuklear.h | 6 +- phobia/phobia.c | 34 +++-- src/Makefile | 2 +- src/app/as5047.c | 2 +- src/app/autostart.c | 2 +- src/epcan.c | 38 ++--- src/epcan.h | 2 +- src/hal/adc.c | 2 +- src/hal/can.c | 99 +++++++----- src/hal/can.h | 6 +- src/hal/drv.c | 117 +++++++++++---- src/hal/drv.h | 11 +- src/hal/hal.c | 6 +- src/hal/hal.h | 7 +- src/hal/hw/VESC6A.h | 2 +- src/hal/hw/VESC6B.h | 2 +- src/hal/hw/VESC6C.h | 2 +- src/hal/mk/PESC1.d | 4 +- src/hal/mk/REV2.d | 2 +- src/hal/mk/REV3.d | 2 +- src/hal/mk/REV4B.d | 2 +- src/hal/mk/REV4C.d | 2 +- src/hal/mk/REV5A.d | 2 +- src/hal/mk/VESC6A.d | 4 +- src/hal/mk/VESC6B.d | 4 +- src/hal/mk/VESC6C.d | 4 +- src/hal/mk/VESC6H.d | 2 +- src/hal/mk/VESC75A.d | 4 +- src/hal/mk/VESC75B.d | 4 +- src/hal/mk/ZAEV3.d | 4 +- src/main.c | 60 +++++--- src/main.h | 4 +- src/mkconfig | 4 +- src/ntc.c | 11 +- src/phobia/pm.c | 290 +++++++++++++++++++++++------------- src/phobia/pm.h | 16 +- src/phobia/pm_fsm.c | 29 +++- src/regdefs.h | 22 ++- src/regfile.c | 209 +++++++++++++++++--------- src/tlm.h | 4 +- 53 files changed, 760 insertions(+), 404 deletions(-) diff --git a/bench/Makefile b/bench/Makefile index b5c7b71..6873ed1 100644 --- a/bench/Makefile +++ b/bench/Makefile @@ -34,9 +34,9 @@ $(TARGET): $(SIM_OBJS) @ echo " LD " $(notdir $@) @ $(LD) $(CFLAGS) -o $@ $^ $(LFLAGS) -verify: $(TARGET) +test: $(TARGET) @ echo " TEST " $(notdir $<) - @ $< verify + @ $< test run: $(TARGET) @ echo " RUN " $(notdir $<) diff --git a/bench/bench.c b/bench/bench.c index 8c72569..d3deabd 100644 --- a/bench/bench.c +++ b/bench/bench.c @@ -203,6 +203,9 @@ tlm_plot_grab() fmt_GP(pm.mtpa_D, "A"); fmt_GP(pm.weak_D, "A"); + fmt_GP(pm.v_DC_MAX, 0); + fmt_GP(pm.v_DC_MIN, 0); + fmk_GP(pm.s_setpoint_speed, kRPM, "rpm"); fmk_GP(pm.s_track, kRPM, "rpm"); fmt_GP(pm.s_integral, "A"); @@ -372,41 +375,61 @@ void bench_script() tlm_restart(); - m.Rs = 14.E-3; - m.Ld = 10.E-6; - m.Lq = 15.E-6; - m.Udc = 22.; + m.Rs = 8.1E-3; + m.Ld = 3.2E-6; + m.Lq = 4.8E-6; + m.Udc = 48.; + m.Rdc = 0.1; + m.Zp = 21; + m.lambda = blm_Kv_lambda(&m, 109.); + m.Jm = 5.39E-3; + + /*m.Rs = 28.E-3; + m.Ld = 14.E-6; + m.Lq = 22.E-6; + m.Udc = 48.; m.Rdc = 0.1; m.Zp = 14; - m.lambda = blm_Kv_lambda(&m, 270.); - m.Jm = 3.E-4; - - m.eabi_ERES = 16384; - m.eabi_WRAP = 16384; + m.lambda = blm_Kv_lambda(&m, 87.); + m.Jm = 0.82E-3;*/ + + /*m.Rs = 0.24; + m.Ld = 520.E-6; + m.Lq = 650.E-6; + m.Udc = 48.; + m.Rdc = 0.5; + m.Zp = 15; + m.lambda = blm_Kv_lambda(&m, 15.7); + m.Jm = 6.E-3;*/ ts_script_default(); ts_script_base(); blm_restart(&m); - pm.config_EABI_FRONTEND = PM_EABI_ABSOLUTE; + //pm.config_LU_FORCED = PM_DISABLED; + pm.watt_uDC_maximal = 49.f; + pm.watt_uDC_minimal = 47.f; + //pm.zone_threshold = 5.0 / pm.const_lambda; - ts_adjust_sensor_eabi(); - blm_restart(&m); + pm.s_accel = 500000.f; - //pm.config_LU_ESTIMATE = PM_FLUX_NONE; - pm.config_LU_SENSOR = PM_SENSOR_EABI; + //m.Udc = 10.; + m.Rdc = 0.5; pm.fsm_req = PM_STATE_LU_STARTUP; ts_wait_for_idle(); - pm.s_setpoint_speed = 40.f; - sim_runtime(1.); + pm.s_setpoint_speed = 2000.f; + sim_runtime(1.0); + + m.Mq[0] = - 1.5 * m.Zp * m.lambda * 10.f; + sim_runtime(1.0); - pm.s_setpoint_speed = 200.f; - sim_runtime(2.); + m.Mq[0] = 0.f; + sim_runtime(1.0); - pm.s_setpoint_speed = -50.f; - sim_runtime(2.); + pm.s_setpoint_speed = 100.f; + sim_runtime(2.0); tlm_PWM_grab(); } @@ -420,9 +443,9 @@ int main(int argc, char *argv[]) lfg_start((int) time(NULL)); - if (strcmp(argv[1], "verify") == 0) { + if (strcmp(argv[1], "test") == 0) { - ts_script_verify(); + ts_script_test(); } else if (strcmp(argv[1], "bench") == 0) { diff --git a/bench/blm.c b/bench/blm.c index a0c0a06..fe43741 100644 --- a/bench/blm.c +++ b/bench/blm.c @@ -125,7 +125,7 @@ void blm_enable(blm_t *m) m->range_A = 165.; /* (Ampere) */ m->range_B = 60.; /* (Volt) */ - /* Hall sensor mount angles. + /* Hall sensor installation angles. * */ m->hall[0] = 30.7; m->hall[1] = 150.1; diff --git a/bench/tsfunc.c b/bench/tsfunc.c index 5bd5a9e..0312dbd 100644 --- a/bench/tsfunc.c +++ b/bench/tsfunc.c @@ -671,7 +671,7 @@ ts_script_eabi(int knob_EABI) pm.config_LU_SENSOR = PM_SENSOR_NONE; } -void ts_script_verify() +void ts_script_test() { blm_enable(&m); blm_restart(&m); @@ -701,6 +701,8 @@ void ts_script_verify() printf("\n---- Turnigy RotoMax 1.20 ----\n"); + tlm_restart(); + m.Rs = 14.E-3; m.Ld = 10.E-6; m.Lq = 15.E-6; @@ -726,7 +728,9 @@ void ts_script_verify() ts_script_eabi(PM_EABI_ABSOLUTE); blm_restart(&m); - printf("\n---- 8-inch Hub Motor (350W) ----\n"); + printf("\n---- Hub Motor (250W) ----\n"); + + tlm_restart(); m.Rs = 0.24; m.Ld = 520.E-6; diff --git a/bench/tsfunc.h b/bench/tsfunc.h index da5cc20..1fab9e2 100644 --- a/bench/tsfunc.h +++ b/bench/tsfunc.h @@ -13,7 +13,7 @@ int ts_wait_for_spinup(); void ts_script_default(); void ts_script_base(); -void ts_script_verify(); +void ts_script_test(); #endif /* _H_TSFUNC_ */ diff --git a/doc/CommandLineInterface.md b/doc/CommandLineInterface.md index 8436867..a64db46 100644 --- a/doc/CommandLineInterface.md +++ b/doc/CommandLineInterface.md @@ -1,7 +1,7 @@ ## Overview This page introduces you to the Command Line Interface (CLI). We have a regular -CLI with autocompletion and command history. +CLI with autocompletion function and command history. ## Key mapping diff --git a/doc/HardwareDesign.md b/doc/HardwareDesign.md index a342d77..bf8d538 100644 --- a/doc/HardwareDesign.md +++ b/doc/HardwareDesign.md @@ -7,8 +7,8 @@ PMC internals. Also this page will be useful for understanding. We have a three-phase Voltage Source Inverter (VSI) which consists of at least six metal–oxide–semiconductor field-effect transistors (MOSFET). The voltage at -each of the output terminals is measured. Phase current A and B (optionally -C) is measured. The output terminals are connected to the machine. +each of the output terminals is measured. Phase current A and B (optionally C) +is measured. The output terminals are connected to the machine. ``` (VCC) >---+--------+---------+---------+ @@ -88,10 +88,11 @@ amount of uncertainty in the output voltage `dU` expressed as follows: ``` The voltage divider (R1, R2) and filter capacitor (C1) are used to measure the -terminal voltage (uA, uB, uC). This RC scheme forms an exponential integrator -that allows us to restore the pulse width by measured voltage. Additional -resistor R3 is used to bias the operating point into the linear region. You -can skip voltage sensing if you do not need related features. +terminal voltage (uA, uB, uC) and supply voltage (uS). This RC scheme forms an +exponential integrator that allows us to restore the pulse width by measured +voltage. Additional resistor R3 is used to bias the operating point into the +linear region. You can skip the terminal voltage sensing if you do not need +related features but supply voltage measuring is mandatory. To get acceptable accuracy you need to make sure that the RC scheme time constant is comparable to dT. Also make sure that your capacitors are stable @@ -143,9 +144,7 @@ measurement you will need to configure the software appropriately. +---< Terminal ``` -Also supply voltage (uS) is measured using a voltage divider. - -## Control loop +## Sampling scheme Currents are sampled strictly in the middle of PWM period simultaneously using three ADCs. Then the voltages and other signals are sampled depending on @@ -188,9 +187,9 @@ switching occur at this time then ADC result is discarded. | | | | | | ---*--*--*-------------------------------------------*--*--*-------- | | - -->| clearence |<-- - | | - -->| skip |<-- + -->| clearence |<-- + | | + -->| skip |<-- ``` The diagram above shows `clearance` and `skip` parameters that is used in diff --git a/doc/HardwareVESC.md b/doc/HardwareVESC.md index 2be6b27..83c9e14 100644 --- a/doc/HardwareVESC.md +++ b/doc/HardwareVESC.md @@ -12,7 +12,7 @@ This page gives you an overview of VESC hardware supported by PMC. | VESC 6 MkVI | VESC6C | Original | | Holybro Mini FOC ESC Based on VESC6 | VESC6H | | | FLIPSKY 75100 V202 ESC | VESC75A | Low side shunts | -| Makerbase VESC 75200 V2 84V 200A | VESC75A | Bad PCB design | +| Makerbase VESC 75200 V2 84V 200A | VESC75A | | | VESC 75/300 R3 | VESC75B | Original | Note that some of VESC clone have bad PCB design that causes distorted current diff --git a/doc/InputAnalogKnob.md b/doc/InputAnalogKnob.md index 1a0052c..e724d63 100644 --- a/doc/InputAnalogKnob.md +++ b/doc/InputAnalogKnob.md @@ -70,8 +70,8 @@ the control variable will be interpolated to this value. If you need you can change input lost range. If signal goes beyond this range it is considered lost and halt happens with `PM_ERROR_SENSOR_HALL_FAULT` reason. - (pmc) reg ap.knob_range_LST0 - (pmc) reg ap.knob_range_LST1 + (pmc) reg ap.knob_range_LOS0 + (pmc) reg ap.knob_range_LOS1 Enable machine startup control. Each time when `ANG` signal is in range the startup is requested. diff --git a/doc/MachineProbe.md b/doc/MachineProbe.md index e882ad7..4462cd2 100644 --- a/doc/MachineProbe.md +++ b/doc/MachineProbe.md @@ -23,14 +23,16 @@ you will need to decrease probe currents for a small machine. amplitude that is used to estimate stator impedance. * `pm.probe_speed_hold` - Speed setpoint for the initial spinup. At this speed flux linkage and noise threshold will be estimated. +* `pm.probe_loss_maximal` - Maximal heating losses on stator winding. This + allows us to assume maximal machine current. Also pay attention to the forced control parameters which are used to achieve initial spinup. (pmc) reg pm.forced -* `pm.forced_hold_D` - Current setpoint which should be enough to hold rotor in - aligned position and force it turn. +* `pm.forced_hold_D` - Forced current setpoint which should be enough to hold + rotor in aligned position and force it turn. * `pm.forced_accel` - Allowed acceleration of the forced control. If you use power supply that not tolerate reverse current then consider the diff --git a/doc/MachineTuning.md b/doc/MachineTuning.md index b69bb0c..610d15f 100644 --- a/doc/MachineTuning.md +++ b/doc/MachineTuning.md @@ -5,11 +5,11 @@ conditions. We will consider the most important of them. ## Forced control -If the machine is at low speed then FLUX observer is unable to provide reliable -estimates. You need to use some sort of position sensor. If this is not -possible the forced control is used. We apply a current vector without feedback -to force rotor turn. You can adapt the current value and acceleration to your -needs. +If the machine is at low speed then sensorless flux observer is unable to +provide reliable estimates. You need to use some sort of position sensor. If +this is not possible the forced control is used. We apply a current vector +without feedback to force rotor turn. You can adapt the current value and +acceleration to your needs. (pmc) reg pm.forced_hold_D (pmc) reg pm.forced_accel diff --git a/phobia/config.c b/phobia/config.c index 696c7eb..a30cfad 100644 --- a/phobia/config.c +++ b/phobia/config.c @@ -239,6 +239,6 @@ void config_default(struct config_phobia *fe) strcpy(fe->fuzzy, "setpoint"); - fe->regfile = 480; + fe->regfile = 485; } diff --git a/phobia/link.c b/phobia/link.c index 045e3e3..89cb4a8 100644 --- a/phobia/link.c +++ b/phobia/link.c @@ -584,7 +584,8 @@ void link_remote(struct link_pmc *lp) lp->reg_MAX_N = 0; - serial_fputs(priv->fd, LINK_EOL); + sprintf(priv->lbuf, "\x04\x04\x04\x04" LINK_EOL); + serial_fputs(priv->fd, priv->lbuf); sprintf(priv->lbuf, "ap_version" LINK_EOL); serial_fputs(priv->fd, priv->lbuf); diff --git a/phobia/nuklear.h b/phobia/nuklear.h index 4abcbc1..25a3e14 100644 --- a/phobia/nuklear.h +++ b/phobia/nuklear.h @@ -7662,9 +7662,9 @@ nk_rgb_factor(struct nk_color col, const float factor) { if (factor == 1.0f) return col; - col.r *= factor; - col.g *= factor; - col.b *= factor; + col.r = (nk_byte)(col.r * factor); + col.g = (nk_byte)(col.g * factor); + col.b = (nk_byte)(col.b * factor); return col; } NK_API struct nk_color diff --git a/phobia/phobia.c b/phobia/phobia.c index 454b662..a83fe62 100644 --- a/phobia/phobia.c +++ b/phobia/phobia.c @@ -988,7 +988,7 @@ pub_popup_telemetry_grab(struct public *pub, int popup) if (reg_tlm != NULL) { - reg_tlm->lval = 1; + reg_tlm->lval = 0; } } } @@ -3220,6 +3220,12 @@ page_hal(struct public *pub) nk_layout_row_dynamic(ctx, 0, 1); nk_spacer(ctx); + reg_float(pub, "hal.CAN_bitfreq", "CAN frequency"); + reg_float(pub, "hal.CAN_errate", "CAN bus errate"); + + nk_layout_row_dynamic(ctx, 0, 1); + nk_spacer(ctx); + reg_enum_combo(pub, "hal.DPS_mode", "DPS operation mode", 0); reg_enum_combo(pub, "hal.PPM_mode", "PPM operation mode", 0); reg_float(pub, "hal.PPM_frequency", "PPM frequency"); @@ -3234,7 +3240,6 @@ page_hal(struct public *pub) reg_float(pub, "hal.DRV.status_raw", "DRV status raw"); reg_float(pub, "hal.DRV.gate_current", "DRV gate current"); reg_float(pub, "hal.DRV.ocp_level", "DRV OCP level"); - reg_float(pub, "hal.DRV.fault_safety", "DRV fault safety"); nk_layout_row_dynamic(ctx, 0, 1); nk_spacer(ctx); @@ -3693,8 +3698,8 @@ page_in_knob(struct public *pub) reg_float(pub, "ap.knob_range_ANG2", "ANG range HIGH"); reg_float(pub, "ap.knob_range_BRK0", "BRK range LOW"); reg_float(pub, "ap.knob_range_BRK1", "BRK range HIGH"); - reg_float(pub, "ap.knob_range_LST0", "Lost range LOW"); - reg_float(pub, "ap.knob_range_LST1", "Lost range HIGH"); + reg_float(pub, "ap.knob_range_LOS0", "LOST range LOW"); + reg_float(pub, "ap.knob_range_LOS1", "LOST range HIGH"); reg_float(pub, "ap.knob_control_ANG0", "Control range LOW"); reg_float(pub, "ap.knob_control_ANG1", "Control range MID"); reg_float(pub, "ap.knob_control_ANG2", "Control range HIGH"); @@ -3886,7 +3891,7 @@ page_config(struct public *pub) reg_enum_combo(pub, "pm.config_NOP", "Number of machine phases", 0); reg_enum_combo(pub, "pm.config_IFB", "Current measurement scheme", 0); reg_enum_toggle(pub, "pm.config_TVM", "Terminal voltage measurement"); - reg_enum_toggle(pub, "pm.config_DBG", "Debug facilities"); + reg_enum_toggle(pub, "pm.config_DBG", "Debug information gather"); nk_layout_row_dynamic(ctx, 0, 1); nk_spacer(ctx); @@ -3952,10 +3957,10 @@ page_config(struct public *pub) nk_layout_row_dynamic(ctx, 0, 1); nk_spacer(ctx); - reg_float(pub, "pm.fault_voltage_tol", "Voltage tolerance"); - reg_float(pub, "pm.fault_current_tol", "Current tolerance"); - reg_float(pub, "pm.fault_accuracy_tol", "Accuracy tolerance"); - reg_float(pub, "pm.fault_terminal_tol", "Terminal tolerance"); + reg_float(pub, "pm.fault_voltage_tol", "Voltage fault tolerance"); + reg_float(pub, "pm.fault_current_tol", "Current fault tolerance"); + reg_float(pub, "pm.fault_accuracy_tol", "Accuracy fault tolerance"); + reg_float(pub, "pm.fault_terminal_tol", "Terminal fault tolerance"); reg_float(pub, "pm.fault_current_halt", "Current halt threshold"); reg_float(pub, "pm.fault_voltage_halt", "Voltage halt threshold"); @@ -4586,6 +4591,13 @@ page_wattage(struct public *pub) nk_layout_row_dynamic(ctx, 0, 1); nk_spacer(ctx); + reg_float(pub, "pm.v_uDC_tol", "DC regulation tolerance"); + reg_float(pub, "pm.v_gain_P", "DC link proportional gain"); + reg_float(pub, "pm.v_gain_I", "DC link integral gain"); + + nk_layout_row_dynamic(ctx, 0, 1); + nk_spacer(ctx); + reg = link_reg_lookup(lp, "pm.lu_MODE"); if (reg != NULL) { @@ -4821,8 +4833,8 @@ page_lp_location(struct public *pub) reg_float_um(pub, "pm.x_maximal", "Maximal location limit", um_def); reg_float_um(pub, "pm.x_minimal", "Minimal location limit", um_def); - reg_float_um(pub, "pm.x_damping", "Damping range", 0); - reg_float_um(pub, "pm.x_tolerance", "Tolerance", 0); + reg_float_um(pub, "pm.x_damping", "Damping distance", 0); + reg_float_um(pub, "pm.x_residual_tol", "Residual tolerance", 0); reg_float_um(pub, "pm.x_gain_P", "Proportional gain", 0); reg_float(pub, "pm.x_gain_D", "Damped gain"); diff --git a/src/Makefile b/src/Makefile index b64809f..631a933 100644 --- a/src/Makefile +++ b/src/Makefile @@ -71,7 +71,7 @@ OBJS = app/as5047.o \ app/hx711.o \ app/mpu6050.o -ifeq ($(OBJ_LIB_CHERRY), 1) +ifeq ($(OBJ_CHERRY), 1) OBJS += cherry/usb_dc_dwc2.o \ cherry/usbd_cdc.o \ cherry/usbd_core.o diff --git a/src/app/as5047.c b/src/app/as5047.c index 6c30bf9..a4e7b45 100644 --- a/src/app/as5047.c +++ b/src/app/as5047.c @@ -102,7 +102,7 @@ LD_TASK void app_AS5047(void *pData) } } - log_TRACE("AS5047 -- EF %i PA %i" EOL, + log_TRACE("AS5047 errate EF %i PA %i" EOL, priv_AS5047.EF_errcnt, priv_AS5047.PA_errcnt); diff --git a/src/app/autostart.c b/src/app/autostart.c index 74b43ad..8b2a92b 100644 --- a/src/app/autostart.c +++ b/src/app/autostart.c @@ -7,7 +7,7 @@ #include "main.h" #include "regfile.h" -/* The application allows you to startup PMC permanently. +/* The application allows you to startup PMC automatically. * */ LD_TASK void app_AUTOSTART(void *pData) diff --git a/src/epcan.c b/src/epcan.c index d0f6804..6b758ef 100644 --- a/src/epcan.c +++ b/src/epcan.c @@ -576,7 +576,7 @@ EPCAN_message_IN(const CAN_msg_t *msg) * */ net.node_ID = msg->payload[4]; - EPCAN_filter_ID(); + EPCAN_bind(); } else { local_node_insert(* (uint32_t *) &msg->payload[0], msg->payload[4]); @@ -734,47 +734,47 @@ void EPCAN_startup() CAN_startup(); - EPCAN_filter_ID(); + EPCAN_bind(); } -void EPCAN_filter_ID() +void EPCAN_bind() { int N; if (net.log_MODE == EPCAN_LOG_PROMISCUOUS) { - CAN_filter_ID(0, 0, 1, 0); + CAN_bind_ID(0, 0, 1, 0); } else { - CAN_filter_ID(0, 0, EPCAN_FILTER_NETWORK, EPCAN_FILTER_NETWORK); + CAN_bind_ID(0, 0, EPCAN_FILTER_NETWORK, EPCAN_FILTER_NETWORK); } if (net.node_ID != 0) { - CAN_filter_ID(1, 0, EPCAN_ID(net.node_ID, EPCAN_NODE_REQ), EPCAN_FILTER_MATCH); - CAN_filter_ID(2, 0, EPCAN_ID(0, EPCAN_NODE_ACK), EPCAN_ID(0, 31)); - CAN_filter_ID(3, 0, EPCAN_ID(net.node_ID, EPCAN_NODE_RX), EPCAN_FILTER_MATCH); - CAN_filter_ID(4, 0, 0, 0); + CAN_bind_ID(1, 0, EPCAN_ID(net.node_ID, EPCAN_NODE_REQ), EPCAN_FILTER_MATCH); + CAN_bind_ID(2, 0, EPCAN_ID(0, EPCAN_NODE_ACK), EPCAN_ID(0, 31)); + CAN_bind_ID(3, 0, EPCAN_ID(net.node_ID, EPCAN_NODE_RX), EPCAN_FILTER_MATCH); + CAN_bind_ID(4, 0, 0, 0); } else { - CAN_filter_ID(1, 0, 0, 0); - CAN_filter_ID(2, 0, EPCAN_ID(0, EPCAN_NODE_ACK), EPCAN_ID(0, 31)); - CAN_filter_ID(3, 0, 0, 0); - CAN_filter_ID(4, 0, 0, 0); + CAN_bind_ID(1, 0, 0, 0); + CAN_bind_ID(2, 0, EPCAN_ID(0, EPCAN_NODE_ACK), EPCAN_ID(0, 31)); + CAN_bind_ID(3, 0, 0, 0); + CAN_bind_ID(4, 0, 0, 0); } for (N = 0; N < EPCAN_PIPE_MAX; ++N) { if (net.ep[N].MODE == EPCAN_PIPE_INCOMING) { - CAN_filter_ID(20 + N, 1, net.ep[N].ID, EPCAN_FILTER_MATCH); + CAN_bind_ID(20 + N, 1, net.ep[N].ID, EPCAN_FILTER_MATCH); } else if (net.ep[N].MODE == EPCAN_PIPE_OUTGOING_INJECTED) { - CAN_filter_ID(20 + N, 1, net.ep[N].clock_ID, EPCAN_FILTER_MATCH); + CAN_bind_ID(20 + N, 1, net.ep[N].clock_ID, EPCAN_FILTER_MATCH); } else { - CAN_filter_ID(20 + N, 1, 0, 0); + CAN_bind_ID(20 + N, 1, 0, 0); } } } @@ -831,7 +831,7 @@ SH_DEF(net_assign) net.node_ID = local_node_assign_ID(); - EPCAN_filter_ID(); + EPCAN_bind(); printf("local assigned to %i" EOL, net.node_ID); } @@ -955,7 +955,7 @@ SH_DEF(net_node_remote) /* Do listen to incoming messages from remote node. * */ - CAN_filter_ID(4, 0, EPCAN_ID(local.remote_node_ID, EPCAN_NODE_TX), EPCAN_FILTER_MATCH); + CAN_bind_ID(4, 0, EPCAN_ID(local.remote_node_ID, EPCAN_NODE_TX), EPCAN_FILTER_MATCH); /* Create task to outgoing message packaging. * */ @@ -976,7 +976,7 @@ SH_DEF(net_node_remote) /* Drop incoming connection immediately. * */ - CAN_filter_ID(4, 0, 0, 0); + CAN_bind_ID(4, 0, 0, 0); local.remote_node_ID = 0; diff --git a/src/epcan.h b/src/epcan.h index f2dc35f..255af4c 100644 --- a/src/epcan.h +++ b/src/epcan.h @@ -108,7 +108,7 @@ void EPCAN_pipe_REGULAR(); void EPCAN_putc(int c); void EPCAN_startup(); -void EPCAN_filter_ID(); +void EPCAN_bind(); #endif /* _H_EPCAN_ */ diff --git a/src/hal/adc.c b/src/hal/adc.c index 75ec8e4..68fc8a5 100644 --- a/src/hal/adc.c +++ b/src/hal/adc.c @@ -167,7 +167,7 @@ void ADC_startup() GPIO_set_mode_ANALOG(GPIO_ADC_VOLTAGE_C); #endif /* GPIO_ADC_VOLTAGE_C */ - /* Common configuration (overclocked 42 MHz). + /* Common configuration (ADC clock = CLOCK_APB2_HZ / 2U). * */ ADC->CCR = ADC_CCR_TSVREFE; diff --git a/src/hal/can.c b/src/hal/can.c index 3f18033..64ada2f 100644 --- a/src/hal/can.c +++ b/src/hal/can.c @@ -6,22 +6,22 @@ void irq_CAN1_TX() { } static void irq_CAN1_RX(int mb) { - uint32_t xRDLR, xRDHR; - - hal.CAN_msg.ID = (uint16_t) (CAN1->sFIFOMailBox[mb].RIR >> 21); - hal.CAN_msg.len = (uint16_t) (CAN1->sFIFOMailBox[mb].RDTR & 0xFU); - - xRDLR = CAN1->sFIFOMailBox[mb].RDLR; - xRDHR = CAN1->sFIFOMailBox[mb].RDHR; - - hal.CAN_msg.payload[0] = (uint8_t) ((xRDLR >> 0) & 0xFFU); - hal.CAN_msg.payload[1] = (uint8_t) ((xRDLR >> 8) & 0xFFU); - hal.CAN_msg.payload[2] = (uint8_t) ((xRDLR >> 16) & 0xFFU); - hal.CAN_msg.payload[3] = (uint8_t) ((xRDLR >> 24) & 0xFFU); - hal.CAN_msg.payload[4] = (uint8_t) ((xRDHR >> 0) & 0xFFU); - hal.CAN_msg.payload[5] = (uint8_t) ((xRDHR >> 8) & 0xFFU); - hal.CAN_msg.payload[6] = (uint8_t) ((xRDHR >> 16) & 0xFFU); - hal.CAN_msg.payload[7] = (uint8_t) ((xRDHR >> 24) & 0xFFU); + uint32_t xLO, xHI; + + hal.CAN_msg.ID = (uint16_t) (CAN1->sFIFOMailBox[mb].RIR >> CAN_RI0R_STID_Pos); + hal.CAN_msg.len = (uint16_t) (CAN1->sFIFOMailBox[mb].RDTR & CAN_RDT0R_DLC_Msk); + + xLO = CAN1->sFIFOMailBox[mb].RDLR; + xHI = CAN1->sFIFOMailBox[mb].RDHR; + + hal.CAN_msg.payload[0] = (uint8_t) ((xLO >> 0) & 0xFFU); + hal.CAN_msg.payload[1] = (uint8_t) ((xLO >> 8) & 0xFFU); + hal.CAN_msg.payload[2] = (uint8_t) ((xLO >> 16) & 0xFFU); + hal.CAN_msg.payload[3] = (uint8_t) ((xLO >> 24) & 0xFFU); + hal.CAN_msg.payload[4] = (uint8_t) ((xHI >> 0) & 0xFFU); + hal.CAN_msg.payload[5] = (uint8_t) ((xHI >> 8) & 0xFFU); + hal.CAN_msg.payload[6] = (uint8_t) ((xHI >> 16) & 0xFFU); + hal.CAN_msg.payload[7] = (uint8_t) ((xHI >> 24) & 0xFFU); } void irq_CAN1_RX0() @@ -74,10 +74,10 @@ void CAN_startup() } static int -CAN_wait_for_MSR(uint32_t xBITS, uint32_t xSET) +CAN_wait_MSR(uint32_t xBITS, uint32_t xSET) { uint32_t xMSR; - int wait_N = 0; + int N = 0; do { xMSR = CAN1->MSR & xBITS; @@ -89,45 +89,65 @@ CAN_wait_for_MSR(uint32_t xBITS, uint32_t xSET) __NOP(); - wait_N++; + N++; } - while (wait_N < 70000U); + while (N < 70000U); return HAL_FAULT; } void CAN_configure() { - /* No mode SLEEP. + int BRP, TS1, TS2; + + /* Exit mode SLEEP. * */ CAN1->MCR &= ~CAN_MCR_SLEEP; - if (CAN_wait_for_MSR(CAN_MSR_SLAK, 0) != HAL_OK) { + if (CAN_wait_MSR(CAN_MSR_SLAK, 0) != HAL_OK) { - log_TRACE("CAN from SLEEP failed" EOL); + log_TRACE("CAN no SLEEP fault" EOL); } /* Go to mode INIT. * */ CAN1->MCR |= CAN_MCR_INRQ; - if (CAN_wait_for_MSR(CAN_MSR_INAK, CAN_MSR_INAK) != HAL_OK) { + if (CAN_wait_MSR(CAN_MSR_INAK, CAN_MSR_INAK) != HAL_OK) { - log_TRACE("CAN to INIT failed" EOL); + log_TRACE("CAN to INIT fault" EOL); } CAN1->MCR |= CAN_MCR_ABOM | CAN_MCR_AWUM | CAN_MCR_TXFP; + + /* Enable message pending IRQs. + * */ CAN1->IER = CAN_IER_FMPIE0 | CAN_IER_FMPIE1; - /* Bit timing (1 Mb/s). +#if defined(STM32F4) + TS1 = 7; + TS2 = 6; +#elif defined(STM32F7) + TS1 = 9; + TS2 = 8; +#endif /* STM32Fx */ + + BRP = (CLOCK_APB1_HZ / hal.CAN_bitfreq + 5U) / (1U + TS1 + TS2); + + /* Update bit timing. * */ - CAN1->BTR = (5U << 20) | (6U << 16) | (2U); + CAN1->BTR = ((TS2 - 1U) << CAN_BTR_TS2_Pos) + | ((TS1 - 1U) << CAN_BTR_TS1_Pos) | (BRP - 1U); + + hal.CAN_bitfreq = CLOCK_APB1_HZ / BRP / (1U + TS1 + TS2); +#ifdef STM32F4 CAN1->FMR |= CAN_FMR_FINIT; /* Enable all 28 filters to CAN1. * */ - MODIFY_REG(CAN1->FMR, 0x3F00U, 28U << 8); + MODIFY_REG(CAN1->FMR, CAN_FMR_CAN2SB_Msk, 28U << CAN_FMR_CAN2SB_Pos); +#endif /* STM32F4 */ CAN1->FMR &= ~CAN_FMR_FINIT; @@ -135,13 +155,13 @@ void CAN_configure() * */ CAN1->MCR &= ~CAN_MCR_INRQ; - if (CAN_wait_for_MSR(CAN_MSR_INAK, CAN_MSR_INAK) != HAL_OK) { + if (CAN_wait_MSR(CAN_MSR_INAK, CAN_MSR_INAK) != HAL_OK) { - log_TRACE("CAN to NORMAL failed" EOL); + log_TRACE("CAN to NORMAL fault" EOL); } } -void CAN_filter_ID(int fs, int mb, int ID, int mID) +void CAN_bind_ID(int fs, int mb, int ID, int mask_ID) { uint32_t BFS = (1U << fs); @@ -156,8 +176,8 @@ void CAN_filter_ID(int fs, int mb, int ID, int mID) CAN1->FFA1R &= ~BFS; CAN1->FFA1R |= (mb == 1) ? BFS : 0U; - CAN1->sFilterRegister[fs].FR1 = (ID << 21); - CAN1->sFilterRegister[fs].FR2 = (mID << 21) + 6U; + CAN1->sFilterRegister[fs].FR1 = (ID << CAN_F0R1_FB21_Pos); + CAN1->sFilterRegister[fs].FR2 = (mask_ID << CAN_F0R2_FB21_Pos) + 6U; CAN1->FA1R |= BFS; } @@ -184,7 +204,7 @@ int CAN_send_msg(const CAN_msg_t *msg) return HAL_FAULT; } - CAN1->sTxMailBox[mb].TIR = (uint32_t) msg->ID << 21; + CAN1->sTxMailBox[mb].TIR = (uint32_t) msg->ID << CAN_TI0R_STID_Pos; CAN1->sTxMailBox[mb].TDTR = (uint32_t) msg->len; CAN1->sTxMailBox[mb].TDLR = @@ -206,3 +226,14 @@ int CAN_send_msg(const CAN_msg_t *msg) return HAL_OK; } +int CAN_errate() +{ + int errate; + + errate = ((CAN1->ESR & CAN_ESR_REC_Msk) >> (CAN_ESR_REC_Pos - 8U)) + | ((CAN1->ESR & CAN_ESR_TEC_Msk) >> (CAN_ESR_TEC_Pos - 3U)) + | ((CAN1->ESR & CAN_ESR_LEC_Msk) >> CAN_ESR_LEC_Pos); + + return errate; +} + diff --git a/src/hal/can.h b/src/hal/can.h index 1111d9a..f4579ff 100644 --- a/src/hal/can.h +++ b/src/hal/can.h @@ -3,6 +3,8 @@ #include "libc.h" +#define CAN_BITFREQ_HZ 1000000U + typedef struct { uint16_t ID; @@ -14,9 +16,11 @@ CAN_msg_t; void CAN_startup(); void CAN_configure(); -void CAN_filter_ID(int fs, int mb, int ID, int mID); +void CAN_bind_ID(int fs, int mb, int ID, int mask_ID); int CAN_send_msg(const CAN_msg_t *msg); +int CAN_errate(); + extern void CAN_IRQ(); #endif /* _H_CAN_ */ diff --git a/src/hal/drv.c b/src/hal/drv.c index 3d4167d..30c910b 100644 --- a/src/hal/drv.c +++ b/src/hal/drv.c @@ -7,6 +7,10 @@ #define HW_DRV_FREQUENCY 2000000U /* (Hz) */ #endif /* HW_DRV_FREQUENCY */ +#ifndef HW_DRV_FAULT_SAFETY +#define HW_DRV_FAULT_SAFETY 20 +#endif /* HW_DRV_FAULT_SAFETY */ + static int DRV_read_reg(int addr) { @@ -50,7 +54,7 @@ DRV_write_reg(int addr, int data) static void DRV8301_configure() { - int config, control; + int config, check; config = (hal.DRV.gate_current & 0x3U); @@ -65,18 +69,18 @@ DRV8301_configure() DRV_write_reg(2, config); DRV_write_reg(3, 0); - control = DRV_read_reg(2); + check = DRV_read_reg(2); - if (control != config) { + if (check != config) { - log_TRACE("DRV8301 configuration fault %4x" EOL, control); + log_TRACE("DRV configuration fault %4x" EOL, check); } hal.DRV.status_raw = DRV_read_reg(0); if (hal.DRV.status_raw != 0) { - log_TRACE("DRV8301 status %4x" EOL, hal.DRV.status_raw); + log_TRACE("DRV status %4x" EOL, hal.DRV.status_raw); } } @@ -94,9 +98,15 @@ DRV8301_startup() SPI_startup(HW_DRV_ID_ON_PCB, HW_DRV_FREQUENCY, SPI_LOW_FALLING); DRV8301_configure(); +} - hal.DRV.gate_ON = 1; - hal.DRV.fault_CNT = 0; +static void +DRV8301_halt() +{ + SPI_halt(HW_DRV_ID_ON_PCB); + + GPIO_set_LOW(hal.DRV.gpio_GATE_EN); + vTaskDelay((TickType_t) 100); } void DRV_startup() @@ -109,32 +119,32 @@ void DRV_startup() /* TODO */ } + + hal.DRV.partno_ENABLED = hal.DRV.partno; } void DRV_halt() { - if (hal.DRV.gate_ON != 0) { - - hal.DRV.gate_ON = 0; - hal.DRV.fault_CNT = 0; + if (hal.DRV.partno_ENABLED == DRV_PART_DRV8301) { - SPI_halt(HW_DRV_ID_ON_PCB); + DRV8301_halt(); + } + else if (hal.DRV.partno_ENABLED == DRV_PART_DRV8305) { - GPIO_set_LOW(hal.DRV.gpio_GATE_EN); - vTaskDelay((TickType_t) 100); + /* TODO */ } + + hal.DRV.partno_ENABLED = DRV_NONE; + hal.DRV.fault_CNT = 0; } void DRV_configure() { - if (hal.DRV.partno == DRV_PART_DRV8301) { - - if (hal.DRV.gate_ON != 0) { + if (hal.DRV.partno_ENABLED == DRV_PART_DRV8301) { - DRV8301_configure(); - } + DRV8301_configure(); } - else if (hal.DRV.partno == DRV_PART_DRV8305) { + else if (hal.DRV.partno_ENABLED == DRV_PART_DRV8305) { /* TODO */ } @@ -142,14 +152,11 @@ void DRV_configure() void DRV_status() { - if (hal.DRV.partno == DRV_PART_DRV8301) { - - if (hal.DRV.gate_ON != 0) { + if (hal.DRV.partno_ENABLED == DRV_PART_DRV8301) { - hal.DRV.status_raw = DRV_read_reg(0); - } + hal.DRV.status_raw = DRV_read_reg(0); } - else if (hal.DRV.partno == DRV_PART_DRV8305) { + else if (hal.DRV.partno_ENABLED == DRV_PART_DRV8305) { /* TODO */ } @@ -157,9 +164,9 @@ void DRV_status() int DRV_fault() { - if (hal.DRV.gate_ON != 0) { + if (hal.DRV.partno_ENABLED != DRV_NONE) { - if (unlikely(hal.DRV.fault_CNT >= hal.DRV.fault_safety)) { + if (unlikely(hal.DRV.fault_CNT >= HW_DRV_FAULT_SAFETY)) { return HAL_FAULT; } @@ -177,3 +184,57 @@ int DRV_fault() return HAL_OK; } +float DRV_gate_current() +{ + float current = 0.f; + + if (hal.DRV.partno_ENABLED == DRV_PART_DRV8301) { + + switch (hal.DRV.gate_current) { + + case 0: + current = 1.7f; + break; + + case 1: + current = 0.7f; + break; + + case 2: + current = 0.25f; + break; + + default: + current = 0.f; + break; + } + } + else if (hal.DRV.partno_ENABLED == DRV_PART_DRV8305) { + + /* TODO */ + } + + return current; +} + +float DRV_ocp_level() +{ + extern float m_expf(float x); + + float level = 0.f; + + if (hal.DRV.partno_ENABLED == DRV_PART_DRV8301) { + + if (hal.DRV.ocp_level < 32) { + + level = 0.06f * m_expf(hal.DRV.ocp_level * 0.119f); + } + } + else if (hal.DRV.partno_ENABLED == DRV_PART_DRV8305) { + + /* TODO */ + } + + return level; +} + diff --git a/src/hal/drv.h b/src/hal/drv.h index bd1dd6a..ea2d0ee 100644 --- a/src/hal/drv.h +++ b/src/hal/drv.h @@ -1,10 +1,6 @@ #ifndef _H_DRV_ #define _H_DRV_ -#ifndef HW_DRV_FAULT_SAFETY -#define HW_DRV_FAULT_SAFETY 40 -#endif /* HW_DRV_FAULT_SAFETY */ - enum { DRV_NONE = 0, DRV_PART_DRV8301, @@ -22,9 +18,8 @@ typedef struct { int status_raw; int gate_current; int ocp_level; - int fault_safety; - int gate_ON; + int partno_ENABLED; int fault_CNT; } DRV_config_t; @@ -34,7 +29,11 @@ void DRV_halt(); void DRV_configure(); void DRV_status(); + int DRV_fault(); +float DRV_gate_current(); +float DRV_ocp_level(); + #endif /* _H_DRV_ */ diff --git a/src/hal/hal.c b/src/hal/hal.c index 03826ae..7f79270 100644 --- a/src/hal/hal.c +++ b/src/hal/hal.c @@ -21,7 +21,7 @@ void irq_NMI() RCC->CIR |= RCC_CIR_CSSC; - log_TRACE("HSE fault" EOL); + log_TRACE("HSE clock fault" EOL); } hal_system_reset(); @@ -188,8 +188,8 @@ core_startup() /* Configure Flash. * */ #if defined(STM32F4) - FLASH->ACR = FLASH_ACR_DCEN | FLASH_ACR_ICEN | FLASH_ACR_PRFTEN - | FLASH_ACR_LATENCY_5WS; + FLASH->ACR = FLASH_ACR_DCEN | FLASH_ACR_ICEN + | FLASH_ACR_PRFTEN | FLASH_ACR_LATENCY_5WS; #elif defined(STM32F7) FLASH->ACR = FLASH_ACR_PRFTEN | FLASH_ACR_LATENCY_5WS; #endif /* STM32Fx */ diff --git a/src/hal/hal.h b/src/hal/hal.h index cf140ef..ff90600 100644 --- a/src/hal/hal.h +++ b/src/hal/hal.h @@ -131,12 +131,15 @@ typedef struct { float ADC_voltage_B; float ADC_voltage_C; - int DPS_mode; - #ifdef HW_HAVE_NETWORK_EPCAN + int CAN_bitfreq; + int CAN_errate; + CAN_msg_t CAN_msg; #endif /* HW_HAVE_NETWORK_EPCAN */ + int DPS_mode; + int PPM_mode; int PPM_frequency; diff --git a/src/hal/hw/VESC6A.h b/src/hal/hw/VESC6A.h index 8dc65d2..4f66bd7 100644 --- a/src/hal/hw/VESC6A.h +++ b/src/hal/hw/VESC6A.h @@ -27,7 +27,7 @@ #define HW_DRV_PARTNO DRV_PART_DRV8301 #define HW_DRV_GATE_CURRENT 0 -#define HW_DRV_OCP_LEVEL 22 /* 0.82 (V) */ +#define HW_DRV_OCP_LEVEL 26 /* 1.324 (V) */ #define HW_ADC_SAMPLING_SEQUENCE ADC_SEQUENCE__ABC_UTT_TXX diff --git a/src/hal/hw/VESC6B.h b/src/hal/hw/VESC6B.h index cd8319b..d2b80a0 100644 --- a/src/hal/hw/VESC6B.h +++ b/src/hal/hw/VESC6B.h @@ -27,7 +27,7 @@ #define HW_DRV_PARTNO DRV_PART_DRV8301 #define HW_DRV_GATE_CURRENT 0 -#define HW_DRV_OCP_LEVEL 22 /* 0.82 (V) */ +#define HW_DRV_OCP_LEVEL 26 /* 1.324 (V) */ #define HW_ADC_SAMPLING_SEQUENCE ADC_SEQUENCE__ABC_UTT_TXX diff --git a/src/hal/hw/VESC6C.h b/src/hal/hw/VESC6C.h index 818ef96..7a39bed 100644 --- a/src/hal/hw/VESC6C.h +++ b/src/hal/hw/VESC6C.h @@ -26,7 +26,7 @@ #define HW_DRV_PARTNO DRV_PART_DRV8301 #define HW_DRV_GATE_CURRENT 0 -#define HW_DRV_OCP_LEVEL 22 /* 0.82 (V) */ +#define HW_DRV_OCP_LEVEL 26 /* 1.324 (V) */ #define HW_ADC_SAMPLING_SEQUENCE ADC_SEQUENCE__ABC_UTT_TXX diff --git a/src/hal/mk/PESC1.d b/src/hal/mk/PESC1.d index f7db17d..f87b9ac 100644 --- a/src/hal/mk/PESC1.d +++ b/src/hal/mk/PESC1.d @@ -1,6 +1,6 @@ HWMCU = STM32F722 OBJ_HAL_STEP = 1 +OBJ_CHERRY = 1 OBJ_HAL_USB = 1 -OBJ_LIB_CHERRY = 1 -OBJ_HAL_CAN = 1 OBJ_EPCAN = 1 +OBJ_HAL_CAN = 1 diff --git a/src/hal/mk/REV2.d b/src/hal/mk/REV2.d index 81411e9..85a58c2 100644 --- a/src/hal/mk/REV2.d +++ b/src/hal/mk/REV2.d @@ -1,4 +1,4 @@ HWMCU = STM32F405 OBJ_HAL_STEP = 1 -OBJ_HAL_CAN = 1 OBJ_EPCAN = 1 +OBJ_HAL_CAN = 1 diff --git a/src/hal/mk/REV3.d b/src/hal/mk/REV3.d index 81411e9..85a58c2 100644 --- a/src/hal/mk/REV3.d +++ b/src/hal/mk/REV3.d @@ -1,4 +1,4 @@ HWMCU = STM32F405 OBJ_HAL_STEP = 1 -OBJ_HAL_CAN = 1 OBJ_EPCAN = 1 +OBJ_HAL_CAN = 1 diff --git a/src/hal/mk/REV4B.d b/src/hal/mk/REV4B.d index 81411e9..85a58c2 100644 --- a/src/hal/mk/REV4B.d +++ b/src/hal/mk/REV4B.d @@ -1,4 +1,4 @@ HWMCU = STM32F405 OBJ_HAL_STEP = 1 -OBJ_HAL_CAN = 1 OBJ_EPCAN = 1 +OBJ_HAL_CAN = 1 diff --git a/src/hal/mk/REV4C.d b/src/hal/mk/REV4C.d index 81411e9..85a58c2 100644 --- a/src/hal/mk/REV4C.d +++ b/src/hal/mk/REV4C.d @@ -1,4 +1,4 @@ HWMCU = STM32F405 OBJ_HAL_STEP = 1 -OBJ_HAL_CAN = 1 OBJ_EPCAN = 1 +OBJ_HAL_CAN = 1 diff --git a/src/hal/mk/REV5A.d b/src/hal/mk/REV5A.d index 81411e9..85a58c2 100644 --- a/src/hal/mk/REV5A.d +++ b/src/hal/mk/REV5A.d @@ -1,4 +1,4 @@ HWMCU = STM32F405 OBJ_HAL_STEP = 1 -OBJ_HAL_CAN = 1 OBJ_EPCAN = 1 +OBJ_HAL_CAN = 1 diff --git a/src/hal/mk/VESC6A.d b/src/hal/mk/VESC6A.d index 595dfe8..103d7ee 100644 --- a/src/hal/mk/VESC6A.d +++ b/src/hal/mk/VESC6A.d @@ -1,7 +1,7 @@ HWMCU = STM32F405 OBJ_HAL_DRV = 1 OBJ_HAL_STEP = 1 +OBJ_CHERRY = 1 OBJ_HAL_USB = 1 -OBJ_LIB_CHERRY = 1 -OBJ_HAL_CAN = 1 OBJ_EPCAN = 1 +OBJ_HAL_CAN = 1 diff --git a/src/hal/mk/VESC6B.d b/src/hal/mk/VESC6B.d index 595dfe8..103d7ee 100644 --- a/src/hal/mk/VESC6B.d +++ b/src/hal/mk/VESC6B.d @@ -1,7 +1,7 @@ HWMCU = STM32F405 OBJ_HAL_DRV = 1 OBJ_HAL_STEP = 1 +OBJ_CHERRY = 1 OBJ_HAL_USB = 1 -OBJ_LIB_CHERRY = 1 -OBJ_HAL_CAN = 1 OBJ_EPCAN = 1 +OBJ_HAL_CAN = 1 diff --git a/src/hal/mk/VESC6C.d b/src/hal/mk/VESC6C.d index 595dfe8..103d7ee 100644 --- a/src/hal/mk/VESC6C.d +++ b/src/hal/mk/VESC6C.d @@ -1,7 +1,7 @@ HWMCU = STM32F405 OBJ_HAL_DRV = 1 OBJ_HAL_STEP = 1 +OBJ_CHERRY = 1 OBJ_HAL_USB = 1 -OBJ_LIB_CHERRY = 1 -OBJ_HAL_CAN = 1 OBJ_EPCAN = 1 +OBJ_HAL_CAN = 1 diff --git a/src/hal/mk/VESC6H.d b/src/hal/mk/VESC6H.d index 426aed9..bff68fd 100644 --- a/src/hal/mk/VESC6H.d +++ b/src/hal/mk/VESC6H.d @@ -1,4 +1,4 @@ HWMCU = STM32F405 OBJ_HAL_STEP = 1 +OBJ_CHERRY = 1 OBJ_HAL_USB = 1 -OBJ_LIB_CHERRY = 1 diff --git a/src/hal/mk/VESC75A.d b/src/hal/mk/VESC75A.d index ffcb71b..3d06f12 100644 --- a/src/hal/mk/VESC75A.d +++ b/src/hal/mk/VESC75A.d @@ -1,6 +1,6 @@ HWMCU = STM32F405 OBJ_HAL_STEP = 1 +OBJ_CHERRY = 1 OBJ_HAL_USB = 1 -OBJ_LIB_CHERRY = 1 -OBJ_HAL_CAN = 1 OBJ_EPCAN = 1 +OBJ_HAL_CAN = 1 diff --git a/src/hal/mk/VESC75B.d b/src/hal/mk/VESC75B.d index ffcb71b..3d06f12 100644 --- a/src/hal/mk/VESC75B.d +++ b/src/hal/mk/VESC75B.d @@ -1,6 +1,6 @@ HWMCU = STM32F405 OBJ_HAL_STEP = 1 +OBJ_CHERRY = 1 OBJ_HAL_USB = 1 -OBJ_LIB_CHERRY = 1 -OBJ_HAL_CAN = 1 OBJ_EPCAN = 1 +OBJ_HAL_CAN = 1 diff --git a/src/hal/mk/ZAEV3.d b/src/hal/mk/ZAEV3.d index 11061ff..ffada54 100644 --- a/src/hal/mk/ZAEV3.d +++ b/src/hal/mk/ZAEV3.d @@ -1,5 +1,5 @@ HWMCU = STM32F405 +OBJ_CHERRY = 1 OBJ_HAL_USB = 1 -OBJ_LIB_CHERRY = 1 -OBJ_HAL_CAN = 1 OBJ_EPCAN = 1 +OBJ_HAL_CAN = 1 diff --git a/src/main.c b/src/main.c index b6270d5..c22b052 100644 --- a/src/main.c +++ b/src/main.c @@ -47,7 +47,7 @@ void vAssertHook(const char *file, int line) void vApplicationMallocFailedHook() { taskDISABLE_INTERRUPTS(); - log_TRACE("FreeRTOS: Heap Allocation Failed" EOL); + log_TRACE("FreeRTOS: Heap allocation fault" EOL); hal_system_reset(); } @@ -55,7 +55,7 @@ void vApplicationMallocFailedHook() void vApplicationStackOverflowHook(TaskHandle_t xTask, char *pcTaskName) { taskDISABLE_INTERRUPTS(); - log_TRACE("FreeRTOS: Stack Overflow in %8x task" EOL, (uint32_t) xTask); + log_TRACE("FreeRTOS: Stack overflow in %8x task" EOL, (uint32_t) xTask); hal_system_reset(); } @@ -333,24 +333,32 @@ conv_KNOB() ap.knob_ACTIVE = PM_DISABLED; ap.knob_DISARM = PM_ENABLED; + + ap.knob_FAULT = 0; } - if ( ap.knob_in_ANG < ap.knob_range_LST[0] - || ap.knob_in_ANG > ap.knob_range_LST[1]) { + if ( ap.knob_in_ANG < ap.knob_range_LOS[0] + || ap.knob_in_ANG > ap.knob_range_LOS[1]) { + + scaled = - 1.f; /* Loss of KNOB signal. * */ - scaled = - 1.f; if (ap.knob_ACTIVE == PM_ENABLED) { if (pm.lu_MODE != PM_LU_DISABLED) { - pm.fsm_errno = PM_ERROR_KNOB_CONTROL_FAULT; - pm.fsm_req = PM_STATE_LU_SHUTDOWN; - } + ap.knob_FAULT++; + + if (unlikely(ap.knob_FAULT >= 10)) { - ap.knob_ACTIVE = PM_DISABLED; + pm.fsm_errno = PM_ERROR_KNOB_CONTROL_FAULT; + pm.fsm_req = PM_STATE_LU_SHUTDOWN; + + ap.knob_ACTIVE = PM_DISABLED; + } + } } ap.knob_DISARM = PM_ENABLED; @@ -410,6 +418,8 @@ conv_KNOB() } } } + + ap.knob_FAULT = 0; } if (scaled < 0.f) { @@ -423,8 +433,8 @@ conv_KNOB() } #ifdef HW_HAVE_BRAKE_KNOB - if ( ap.knob_in_BRK < ap.knob_range_LST[0] - || ap.knob_in_BRK > ap.knob_range_LST[1]) { + if ( ap.knob_in_BRK < ap.knob_range_LOS[0] + || ap.knob_in_BRK > ap.knob_range_LOS[1]) { /* Loss of BRAKE signal. * */ @@ -524,6 +534,10 @@ default_flash_load() hal.ADC_sample_time = ADC_SMP_28; hal.ADC_sample_advance = ADC_SAMPLE_ADVANCE; +#ifdef HW_HAVE_NETWORK_EPCAN + hal.CAN_bitfreq = CAN_BITFREQ_HZ; +#endif /* HW_HAVE_NETWORK_EPCAN */ + hal.DPS_mode = DPS_DISABLED; hal.PPM_mode = PPM_DISABLED; hal.PPM_frequency = 2000000; /* (Hz) */ @@ -540,7 +554,6 @@ default_flash_load() hal.DRV.gpio_FAULT = GPIO_DRV_FAULT; hal.DRV.gate_current = HW_DRV_GATE_CURRENT; hal.DRV.ocp_level = HW_DRV_OCP_LEVEL; - hal.DRV.fault_safety = HW_DRV_FAULT_SAFETY; #endif /* HW_HAVE_DRV_ON_PCB */ #ifdef HW_HAVE_NETWORK_EPCAN @@ -596,8 +609,8 @@ default_flash_load() ap.knob_range_BRK[0] = 2.0f; /* (V) */ ap.knob_range_BRK[1] = 4.0f; /* (V) */ #endif /* HW_HAVE_BRAKE_KNOB */ - ap.knob_range_LST[0] = 0.2f; /* (V) */ - ap.knob_range_LST[1] = 4.8f; /* (V) */ + ap.knob_range_LOS[0] = 0.2f; /* (V) */ + ap.knob_range_LOS[1] = 4.8f; /* (V) */ ap.knob_control_ANG[0] = 0.f; ap.knob_control_ANG[1] = 50.f; ap.knob_control_ANG[2] = 100.f; @@ -728,9 +741,9 @@ LD_TASK void task_INIT(void *pData) seed[1] = RNG_urand(); seed[2] = RNG_urand(); - if (seed[1] == seed[2]) { + if (unlikely(seed[1] == seed[2])) { - log_TRACE("RNG failed" EOL); + log_TRACE("RNG fault %8x" EOL, seed[2]); } /* Initial SEED. @@ -918,17 +931,24 @@ in_PULSE_WIDTH() if (ap.ppm_FREQ > M_EPSILON) { conv_PULSE_WIDTH(); + + ap.ppm_FAULT = 0; } else { if (ap.ppm_ACTIVE == PM_ENABLED) { if (pm.lu_MODE != PM_LU_DISABLED) { - pm.fsm_errno = PM_ERROR_KNOB_CONTROL_FAULT; - pm.fsm_req = PM_STATE_LU_SHUTDOWN; - } + ap.ppm_FAULT++; - ap.ppm_ACTIVE = PM_DISABLED; + if (unlikely(ap.ppm_FAULT >= 10)) { + + pm.fsm_errno = PM_ERROR_KNOB_CONTROL_FAULT; + pm.fsm_req = PM_STATE_LU_SHUTDOWN; + + ap.ppm_ACTIVE = PM_DISABLED; + } + } } ap.ppm_DISARM = PM_ENABLED; diff --git a/src/main.h b/src/main.h index e7414a3..0db90ef 100644 --- a/src/main.h +++ b/src/main.h @@ -18,6 +18,7 @@ typedef struct { int ppm_STARTUP; int ppm_ACTIVE; int ppm_DISARM; + int ppm_FAULT; float ppm_range[3]; float ppm_control[3]; @@ -46,11 +47,12 @@ typedef struct { int knob_STARTUP; int knob_ACTIVE; int knob_DISARM; + int knob_FAULT; float knob_range_ANG[3]; #ifdef HW_HAVE_BRAKE_KNOB float knob_range_BRK[2]; #endif /* HW_HAVE_BRAKE_KNOB */ - float knob_range_LST[2]; + float knob_range_LOS[2]; float knob_control_ANG[3]; float knob_control_BRK; #endif /* HW_HAVE_ANALOG_KNOB */ diff --git a/src/mkconfig b/src/mkconfig index 1970faa..5cb2480 100755 --- a/src/mkconfig +++ b/src/mkconfig @@ -20,12 +20,12 @@ def mkdefs(hw): g.write('OBJ_HAL_STEP = 1\n') elif 'HW_HAVE_NETWORK_EPCAN' in s: - g.write('OBJ_HAL_CAN = 1\n') g.write('OBJ_EPCAN = 1\n') + g.write('OBJ_HAL_CAN = 1\n') elif 'HW_HAVE_USB_CDC_ACM' in s: + g.write('OBJ_CHERRY = 1\n') g.write('OBJ_HAL_USB = 1\n') - g.write('OBJ_LIB_CHERRY = 1\n') f.close() g.close() diff --git a/src/ntc.c b/src/ntc.c index 9ad65f2..db8fc7b 100644 --- a/src/ntc.c +++ b/src/ntc.c @@ -7,14 +7,14 @@ float ntc_read_temperature(ntc_t *ntc) { - float um, ohm, log; - float temp = 0.f; + float um, ohm, log, temp; + + um = ADC_get_sample(ntc->gpio); switch (ntc->type) { case NTC_GND: - um = ADC_get_sample(ntc->gpio); ohm = um * ntc->balance / (1.f - um); log = m_logf(ohm / ntc->ntc0); temp = 1.f / (1.f / (ntc->ta0 + 273.f) @@ -23,7 +23,6 @@ float ntc_read_temperature(ntc_t *ntc) case NTC_VCC: - um = ADC_get_sample(ntc->gpio); ohm = (1.f - um) * ntc->balance / um; log = m_logf(ohm / ntc->ntc0); temp = 1.f / (1.f / (ntc->ta0 + 273.f) @@ -32,14 +31,16 @@ float ntc_read_temperature(ntc_t *ntc) case NTC_LMT87: - um = ADC_get_sample(ntc->gpio); temp = 194.1f - um * 242.7f; break; case NTC_KTY84: + + temp = 0.f; /* TODO */ break; default: + temp = 0.f; break; } diff --git a/src/phobia/pm.c b/src/phobia/pm.c index 454e116..fe8558b 100644 --- a/src/phobia/pm.c +++ b/src/phobia/pm.c @@ -3,7 +3,7 @@ void pm_quick_build(pmc_t *pm) { - if (likely(PM_CONFIG_NOP(pm) == PM_NOP_THREE_PHASE)) { + if (PM_CONFIG_NOP(pm) == PM_NOP_THREE_PHASE) { pm->k_UMAX = 0.66666667f; /* 2 / NOP */ pm->k_EMAX = 0.57735027f; /* 1 / sqrt(NOP) */ @@ -255,6 +255,10 @@ pm_auto_config_default(pmc_t *pm) pm->v_maximal = 90.f; pm->v_reverse = pm->v_maximal; + pm->v_uDC_tol = 4.f; + pm->v_gain_P = 5E+1f; + pm->v_gain_I = 5E-1f; + pm->s_maximal = 15000.f; pm->s_reverse = pm->s_maximal; pm->s_accel = 7000.f; @@ -270,7 +274,7 @@ pm_auto_config_default(pmc_t *pm) pm->x_maximal = 100.f; pm->x_minimal = - pm->x_maximal; pm->x_damping = 0.10f; - pm->x_tolerance = 0.f; + pm->x_residual_tol = 0.f; pm->x_gain_P = 35.f; pm->x_gain_D = 10.f; } @@ -784,7 +788,7 @@ pm_flux_detached(pmc_t *pm) uB = pm->fb_uB; uC = pm->fb_uC; - if (likely(PM_CONFIG_NOP(pm) == PM_NOP_THREE_PHASE)) { + if (PM_CONFIG_NOP(pm) == PM_NOP_THREE_PHASE) { U = 0.33333333f * (uA + uB + uC); @@ -1819,12 +1823,11 @@ pm_lu_FSM(pmc_t *pm) * */ pm->base_TIM++; } - else if (pm->flux_ZONE == PM_ZONE_HIGH) { + else if ( pm->config_LU_ESTIMATE != PM_FLUX_NONE + && pm->flux_ZONE == PM_ZONE_HIGH) { pm->lu_MODE = PM_LU_ESTIMATE; - pm->s_track = pm->lu_wS; - pm->proc_set_Z(PM_Z_NONE); } else if (pm->config_LU_SENSOR == PM_SENSOR_HALL) { @@ -1839,7 +1842,8 @@ pm_lu_FSM(pmc_t *pm) pm->proc_set_Z(PM_Z_NONE); } else if ( pm->config_LU_SENSOR == PM_SENSOR_EABI - && pm->eabi_ADJUST == PM_ENABLED) { + && ( pm->eabi_ADJUST == PM_ENABLED + || pm->flux_ZONE == PM_ZONE_HIGH)) { pm->lu_MODE = PM_LU_SENSOR_EABI; @@ -1931,12 +1935,8 @@ pm_lu_FSM(pmc_t *pm) pm->hall_wS = pm->lu_wS; } else if ( pm->config_LU_SENSOR == PM_SENSOR_EABI - && pm->eabi_ADJUST == PM_ENABLED) { - - pm->lu_MODE = PM_LU_SENSOR_EABI; - } - else if ( pm->config_LU_SENSOR == PM_SENSOR_EABI - && pm->flux_ZONE == PM_ZONE_UNCERTAIN) { + && ( pm->eabi_ADJUST == PM_ENABLED + || pm->flux_ZONE == PM_ZONE_UNCERTAIN)) { pm->lu_MODE = PM_LU_SENSOR_EABI; } @@ -1979,6 +1979,8 @@ pm_lu_FSM(pmc_t *pm) pm->i_integral_D = 0.f; pm->i_integral_Q = 0.f; + pm->v_integral = 0.f; + pm->proc_set_Z(PM_Z_ABC); } } @@ -2171,6 +2173,12 @@ pm_lu_FSM(pmc_t *pm) } pm->lu_wS0 = pm->lu_wS; + + if ( pm->config_LU_DRIVE == PM_DRIVE_CURRENT + && pm->config_SPEED_MAXIMAL == PM_ENABLED) { + + pm->l_track_TIM++; + } } void pm_clearance(pmc_t *pm, int xA, int xB, int xC) @@ -2276,8 +2284,8 @@ void pm_voltage(pmc_t *pm, float uX, float uY) float uA, uB, uC, uMIN, uMAX, uDC; int xA, xB, xC, xMIN, xMAX, nZONE; - uX *= pm->quick_iUdc; - uY *= pm->quick_iUdc; + uX *= pm->quick_iUDC; + uY *= pm->quick_iUDC; uDC = m_sqrtf(uX * uX + uY * uY); @@ -2295,7 +2303,7 @@ void pm_voltage(pmc_t *pm, float uX, float uY) uY *= uDC; } - if (likely(PM_CONFIG_NOP(pm) == PM_NOP_THREE_PHASE)) { + if (PM_CONFIG_NOP(pm) == PM_NOP_THREE_PHASE) { uA = uX; uB = - 0.5f * uX + 0.8660254f * uY; @@ -2345,7 +2353,7 @@ void pm_voltage(pmc_t *pm, float uX, float uY) float bA, bB, bC, bMIN, bMAX; - if (likely(PM_CONFIG_NOP(pm) == PM_NOP_THREE_PHASE)) { + if (PM_CONFIG_NOP(pm) == PM_NOP_THREE_PHASE) { bA = m_fabsf(pm->lu_iX); bB = m_fabsf(0.5f * pm->lu_iX - 0.8660254f * pm->lu_iY); @@ -2586,7 +2594,7 @@ void pm_voltage(pmc_t *pm, float uX, float uY) pm->vsi_X0 = pm->vsi_X; pm->vsi_Y0 = pm->vsi_Y; - if (likely(PM_CONFIG_NOP(pm) == PM_NOP_THREE_PHASE)) { + if (PM_CONFIG_NOP(pm) == PM_NOP_THREE_PHASE) { uDC = 0.33333333f * (xA + xB + xC); @@ -2610,13 +2618,13 @@ void pm_voltage(pmc_t *pm, float uX, float uY) } static float -pm_form_SP(pmc_t *pm, float eS) +pm_form_SP(pmc_t *pm, float eSP) { float iSP; /* Basic proportional-integral regulator. * */ - iSP = pm->s_gain_P * eS + pm->s_integral; + iSP = pm->s_gain_P * eSP + pm->s_integral; /* Add an load torque estimate as feed forward term. * */ @@ -2626,10 +2634,10 @@ pm_form_SP(pmc_t *pm, float eS) * */ iSP += - pm->s_gain_D * pm_torque_get_accel(pm); - if ( (iSP < pm->i_maximal || eS < 0.f) - && (iSP > - pm->i_reverse || eS > 0.f)) { + if ( (iSP < pm->i_maximal || eSP < 0.f) + && (iSP > - pm->i_reverse || eSP > 0.f)) { - pm->s_integral += pm->s_gain_I * eS; + pm->s_integral += pm->s_gain_I * eSP; } return iSP; @@ -2645,7 +2653,7 @@ pm_wattage(pmc_t *pm) wP = pm->k_KWAT * (pm->lu_iD * pm->lu_uD + pm->lu_iQ * pm->lu_uQ); pm->watt_drain_wP += (wP - pm->watt_drain_wP) * pm->watt_gain_LP; - pm->watt_drain_wA = pm->watt_drain_wP * pm->quick_iUdc; + pm->watt_drain_wA = pm->watt_drain_wP * pm->quick_iUDC; /* Traveled distance. * */ @@ -2683,7 +2691,7 @@ static void pm_loop_current(pmc_t *pm) { float track_D, track_Q, eD, eQ, uD, uQ, uX, uY, wP; - float iMAX, iREV, uMAX, uREV, wMAX, wREV, dSA; + float iMAX, iREV, uMAX, uREV, wMAX, wREV, wKEP, dSA; if (pm->lu_MODE == PM_LU_FORCED) { @@ -2707,24 +2715,11 @@ pm_loop_current(pmc_t *pm) track_D = pm->forced_track_D; track_Q = 0.f; - if (pm->lu_MODE == PM_LU_FORCED) { - - if ( pm->config_LU_DRIVE == PM_DRIVE_CURRENT - && pm->config_SPEED_MAXIMAL == PM_ENABLED) { + if ( pm->lu_MODE == PM_LU_FORCED + || ( pm->lu_MODE == PM_LU_ESTIMATE + && pm->flux_ZONE != PM_ZONE_HIGH)) { - pm->s_track = pm->lu_wS; - pm->l_blend = 0.f; - } - } - else if ( pm->lu_MODE == PM_LU_ESTIMATE - && pm->flux_ZONE != PM_ZONE_HIGH) { - - if ( pm->config_LU_DRIVE == PM_DRIVE_CURRENT - && pm->config_SPEED_MAXIMAL == PM_ENABLED) { - - pm->s_track = pm->lu_wS; - pm->l_blend = 0.f; - } + /* No current drive in this case. */ } else { track_Q = pm->i_setpoint_current; @@ -2748,21 +2743,30 @@ pm_loop_current(pmc_t *pm) float wSP, eSP, blend; + if (pm->l_track_TIM >= 10) { + + /* Update not relevant speed trackpoint. + * */ + pm->l_track = pm->lu_wS; + } + wSP = pm->lu_wS; wSP = (wSP > pm->s_maximal) ? pm->s_maximal : (wSP < - pm->s_reverse) ? - pm->s_reverse : wSP; dSA = pm->s_accel * pm->m_dT; - pm->s_track = (pm->s_track < wSP - dSA) ? pm->s_track + dSA - : (pm->s_track > wSP + dSA) ? pm->s_track - dSA : wSP; + pm->l_track = (pm->l_track < wSP - dSA) ? pm->l_track + dSA + : (pm->l_track > wSP + dSA) ? pm->l_track - dSA : wSP; - eSP = pm->s_track - pm->lu_wS; + eSP = pm->l_track - pm->lu_wS; blend = m_fabsf(eSP) / pm->l_track_tol; blend = (blend > 1.f) ? 1.f : blend; pm->l_blend += (blend - pm->l_blend) * pm->l_gain_LP; track_Q += (pm_form_SP(pm, eSP) - track_Q) * pm->l_blend; + + pm->l_track_TIM = 0; } } @@ -2780,25 +2784,20 @@ pm_loop_current(pmc_t *pm) if (pm->config_WEAKENING == PM_ENABLED) { - float DC_lack, LS_abs; + float eDC, wLS; - DC_lack = (1.f - pm->vsi_DC) * pm->const_fb_U; + eDC = (1.f - pm->vsi_DC) * pm->const_fb_U; - /* Prevent the lack of weakening in case of DC link overvoltage. - * */ - DC_lack = (pm->const_fb_U > pm->watt_uDC_maximal - && DC_lack > 0.f) ? 0.f : DC_lack; - - pm->weak_D += DC_lack * pm->weak_gain_EU; + pm->weak_D += eDC * pm->weak_gain_EU; pm->weak_D = (pm->weak_D < - pm->weak_maximal) ? - pm->weak_maximal : (pm->weak_D > 0.f) ? 0.f : pm->weak_D; if (pm->weak_D < - M_EPSILON) { - DC_lack = pm->k_EMAX * pm->const_fb_U; - LS_abs = m_fabsf(pm->lu_wS) * pm->const_im_L2; + eDC = pm->k_EMAX * pm->const_fb_U; + wLS = pm->lu_wS * pm->const_im_L2; - iMAX = DC_lack / LS_abs; + iMAX = eDC / m_fabsf(wLS); track_Q = (track_Q > iMAX) ? iMAX : (track_Q < - iMAX) ? - iMAX : track_Q; @@ -2810,7 +2809,7 @@ pm_loop_current(pmc_t *pm) } } - /* Get filtered voltage to use in POWER constraints. + /* Update filtered voltage to use in POWER constraints. * */ pm->watt_lpf_D += (pm->lu_uD - pm->watt_lpf_D) * pm->watt_gain_LP; pm->watt_lpf_Q += (pm->lu_uQ - pm->watt_lpf_Q) * pm->watt_gain_LP; @@ -2820,15 +2819,9 @@ pm_loop_current(pmc_t *pm) iMAX = pm->i_maximal; iREV = - pm->i_reverse; - if (pm->weak_D > - M_EPSILON) { - - iMAX = (iMAX < pm->i_derate_on_PCB) ? iMAX : pm->i_derate_on_PCB; - iREV = (iREV > - pm->i_derate_on_PCB) ? iREV : - pm->i_derate_on_PCB; - } - if (pm->lu_MODE == PM_LU_ON_HFI) { - /* Add CURRENT constraints on HFI. + /* Add CURRENT constraint from HFI. * */ iMAX = (iMAX < pm->i_derate_on_HFI) ? iMAX : pm->i_derate_on_HFI; iREV = (iREV > - pm->i_derate_on_HFI) ? iREV : - pm->i_derate_on_HFI; @@ -2837,6 +2830,19 @@ pm_loop_current(pmc_t *pm) track_D = (track_D > iMAX) ? iMAX : (track_D < - iMAX) ? - iMAX : track_D; track_Q = (track_Q > iMAX) ? iMAX : (track_Q < iREV) ? iREV : track_Q; + /* Add CURRENT constraint from PCB. + * */ + iMAX = pm->i_derate_on_PCB; + + track_Q = (track_Q > iMAX) ? iMAX : (track_Q < - iMAX) ? - iMAX : track_Q; + + if (pm->weak_D > - M_EPSILON) { + + /* In case of no flux weakening also constraint D-axis current. + * */ + track_D = (track_D > iMAX) ? iMAX : (track_D < - iMAX) ? - iMAX : track_D; + } + /* Maximal POWER constraint. * */ wMAX = pm->watt_wP_maximal; @@ -2849,63 +2855,135 @@ pm_loop_current(pmc_t *pm) wP = - pm->watt_wA_reverse * pm->const_fb_U; wREV = (wP > wREV) ? wP : wREV; - if (pm->weak_D < - M_EPSILON) { + /* Keep D-axis CURRENT in case of flux weakening. + * */ + wKEP = (pm->weak_D < - M_EPSILON) ? pm->k_KWAT * track_D * pm->watt_lpf_Q : 0.f; - /* Keep D-axis CURRENT in case of weakening. - * */ - wP = pm->k_KWAT * track_D * pm->watt_lpf_Q; + uMAX = (pm->v_DC_MAX != PM_ENABLED) + ? pm->watt_uDC_maximal + : pm->watt_uDC_maximal - pm->v_uDC_tol; + + /* Prevent DC link OVERVOLTAGE. + * */ + if (unlikely(pm->const_fb_U > uMAX)) { + + float eDC, bSP; + + eDC = pm->const_fb_U - pm->watt_uDC_maximal; + + if (pm->v_DC_MAX != PM_ENABLED) { + + pm->v_DC_MAX = PM_ENABLED; + pm->v_integral = pm->watt_drain_wP; + } + + bSP = pm->v_gain_P * eDC + pm->v_integral; + bSP = (bSP < 0.f) ? bSP : 0.f; + + if ( (bSP < 0.f || eDC < 0.f) + && (bSP > wREV || eDC > 0.f)) { + + pm->v_integral += pm->v_gain_I * eDC; + } + + wREV = bSP; } - else { - wP = 0.f; + else if (pm->v_DC_MAX == PM_ENABLED) { + + pm->v_DC_MAX = PM_DISABLED; } - /* Prevent DC link OVERVOLTAGE and UNDERVOLTAGE. - * */ - wREV = unlikely(pm->const_fb_U > pm->watt_uDC_maximal) ? wP : wREV; - wMAX = unlikely(pm->const_fb_U < pm->watt_uDC_minimal) ? wP : wMAX; + uREV = (pm->v_DC_MIN != PM_ENABLED) + ? pm->watt_uDC_minimal + : pm->watt_uDC_minimal + pm->v_uDC_tol; - /* Apply POWER constraints (with D-axis priority). + /* Prevent DC link UNDERVOLTAGE. * */ + if (unlikely(pm->const_fb_U < uREV)) { + + float eDC, bSP; + + eDC = pm->const_fb_U - pm->watt_uDC_minimal; + + if (pm->v_DC_MIN != PM_ENABLED) { + + pm->v_DC_MIN = PM_ENABLED; + pm->v_integral = pm->watt_drain_wP; + } + + bSP = pm->v_gain_P * eDC + pm->v_integral; + bSP = (bSP > wKEP) ? bSP : wKEP; + + wMAX = (wMAX < wKEP) ? wKEP : wMAX; + + if ( (bSP < wKEP || eDC < 0.f) + && (bSP > wMAX || eDC > 0.f)) { + + pm->v_integral += pm->v_gain_I * eDC; + } + + wMAX = bSP; + } + else if (pm->v_DC_MIN == PM_ENABLED) { + + pm->v_DC_MIN = PM_DISABLED; + } + wP = pm->k_KWAT * (track_D * pm->watt_lpf_D + track_Q * pm->watt_lpf_Q); - if (unlikely(wP > wMAX)) { + /* Apply POWER constraints of regeneration. + * */ + if (unlikely(wP < wREV)) { wP = pm->k_KWAT * track_D * pm->watt_lpf_D; - if (wP > wMAX) { + if (wP < wREV) { - track_D *= wMAX / wP; + track_D *= wREV / wP; track_Q = 0.f; } else { - wMAX -= wP; + wREV -= wP; wP = pm->k_KWAT * track_Q * pm->watt_lpf_Q; - if (wP > M_EPSILON) { + if (wP < - M_EPSILON) { - track_Q *= wMAX / wP; + track_Q *= wREV / wP; } } } - else if (unlikely(wP < wREV)) { + else if (pm->v_DC_MAX == PM_ENABLED) { + + pm->v_DC_MAX = PM_DISABLED; + } + + wMAX = (wMAX < wKEP) ? wKEP : wMAX; + + /* Apply POWER constraints of consumption (separately along DQ-axes). + * */ + if (unlikely(wP > wMAX)) { wP = pm->k_KWAT * track_D * pm->watt_lpf_D; - if (wP < wREV) { + if (wP > wMAX) { - track_D *= wREV / wP; + track_D *= wMAX / wP; track_Q = 0.f; } else { - wREV -= wP; + wMAX -= wP; wP = pm->k_KWAT * track_Q * pm->watt_lpf_Q; - if (wP < - M_EPSILON) { + if (wP > M_EPSILON) { - track_Q *= wREV / wP; + track_Q *= wMAX / wP; } } } + else if (pm->v_DC_MIN == PM_ENABLED) { + + pm->v_DC_MIN = PM_DISABLED; + } dSA = pm->i_slew_rate * pm->m_dT; @@ -2934,8 +3012,7 @@ pm_loop_current(pmc_t *pm) /* Feed forward compensation (L). * */ uD += - pm->i_gain_Q * pm->lu_wS * pm->const_im_L2 * pm->i_track_Q; - uQ += pm->i_gain_Q * pm->lu_wS - * (pm->const_im_L1 * pm->i_track_D + pm->const_lambda); + uQ += pm->i_gain_Q * pm->lu_wS * (pm->const_im_L1 * pm->i_track_D + pm->const_lambda); uMAX = pm->k_UMAX * pm->const_fb_U; @@ -2972,9 +3049,10 @@ pm_loop_current(pmc_t *pm) * */ pm_hfi_wave(pm); + uHF = pm->hfi_sine * pm->quick_HFwS * pm->const_im_L1; + /* HF voltage injection. * */ - uHF = pm->hfi_sine * pm->quick_HFwS * pm->const_im_L1; uD += pm->hfi_wave[0] * uHF; } @@ -2989,7 +3067,7 @@ pm_loop_current(pmc_t *pm) static void pm_loop_speed(pmc_t *pm) { - float wSP, eS, dSA; + float wSP, eSP, dSA; wSP = pm->s_setpoint_speed; @@ -3019,18 +3097,18 @@ pm_loop_speed(pmc_t *pm) /* Get speed discrepancy. * */ - eS = pm->s_track - pm->lu_wS; + eSP = pm->s_track - pm->lu_wS; /* Update current loop SETPOINT. * */ - pm->i_setpoint_current = pm_form_SP(pm, eS); + pm->i_setpoint_current = pm_form_SP(pm, eSP); } } static void pm_loop_location(pmc_t *pm) { - float xSP, xER, xEA, wSP, blend, gain; + float xSP, wSP, eLOC, eABS, blend, gain; xSP = pm->x_setpoint_location; wSP = pm->x_setpoint_speed; @@ -3056,23 +3134,23 @@ pm_loop_location(pmc_t *pm) /* Get location discrepancy. * */ - xER = pm->x_setpoint_location - pm->lu_location; + eLOC = pm->x_setpoint_location - pm->lu_location; /* Servo is based on constant acceleration formula. * */ - xEA = m_fabsf(xER); - xER = (xER < 0.f) ? - m_sqrtf(xEA) : m_sqrtf(xEA); + eABS= m_fabsf(eLOC); + eLOC = (eLOC < 0.f) ? - m_sqrtf(eABS) : m_sqrtf(eABS); - /* There is a tolerance. + /* There is a residual tolerance. * */ - xER = (xEA > pm->x_tolerance) ? xER : 0.f; + eLOC = (eABS > pm->x_residual_tol) ? eLOC : 0.f; /* Damping inside NEAR zone. * */ - blend = (xEA < pm->x_damping) ? xEA / pm->x_damping : 1.f; + blend = (eABS < pm->x_damping) ? eABS / pm->x_damping : 1.f; gain = pm->x_gain_P * blend + pm->x_gain_D * (1.f - blend); - wSP += gain * xER; + wSP += gain * eLOC; /* Update speed loop SETPOINT. * */ @@ -3122,7 +3200,7 @@ void pm_feedback(pmc_t *pm, pmfb_t *fb) } } - if (likely(PM_CONFIG_NOP(pm) == PM_NOP_THREE_PHASE)) { + if (PM_CONFIG_NOP(pm) == PM_NOP_THREE_PHASE) { if ( pm->vsi_AF == 0 && pm->vsi_BF == 0 @@ -3193,7 +3271,7 @@ void pm_feedback(pmc_t *pm, pmfb_t *fb) /* Get DC link voltage. * */ pm->const_fb_U = pm->scale_uS[1] * fb->voltage_U + pm->scale_uS[0]; - pm->quick_iUdc = 1.f / pm->const_fb_U; + pm->quick_iUDC = 1.f / pm->const_fb_U; if (unlikely( pm->const_fb_U > pm->fault_voltage_halt && pm->weak_D > - M_EPSILON)) { @@ -3237,7 +3315,7 @@ void pm_feedback(pmc_t *pm, pmfb_t *fb) pm->tvm_B = vB; pm->tvm_C = vC; - if (likely(PM_CONFIG_NOP(pm) == PM_NOP_THREE_PHASE)) { + if (PM_CONFIG_NOP(pm) == PM_NOP_THREE_PHASE) { Q = .33333333f * (vA + vB + vC); @@ -3308,14 +3386,14 @@ void pm_feedback(pmc_t *pm, pmfb_t *fb) pm_wattage(pm); } - if (unlikely(PM_CONFIG_DBG(pm) == PM_ENABLED)) { + if (PM_CONFIG_DBG(pm) == PM_ENABLED) { float A, B; A = pm->lu_F[0] * pm->flux_F[0] + pm->lu_F[1] * pm->flux_F[1]; B = pm->lu_F[1] * pm->flux_F[0] - pm->lu_F[0] * pm->flux_F[1]; - pm->dbg[0] = m_atan2f(B, A) * (180.f / M_PI_F); + pm->d_flux_rsu = m_atan2f(B, A) * (180.f / M_PI_F); } if (unlikely(m_isfinitef(pm->lu_F[0]) == 0)) { diff --git a/src/phobia/pm.h b/src/phobia/pm.h index ffca64c..3c5f3ba 100644 --- a/src/phobia/pm.h +++ b/src/phobia/pm.h @@ -506,7 +506,7 @@ typedef struct { float const_im_R; float const_ld_S; - float quick_iUdc; + float quick_iUDC; float quick_iWb; float quick_iWb2; float quick_iL1; @@ -565,6 +565,14 @@ typedef struct { float v_maximal; float v_reverse; + int v_DC_MAX; + int v_DC_MIN; + + float v_uDC_tol; + float v_integral; + float v_gain_P; + float v_gain_I; + float s_setpoint_speed; float s_maximal; float s_reverse; @@ -577,6 +585,8 @@ typedef struct { float s_gain_D; float s_gain_Q; + int l_track_TIM; + float l_track; float l_track_tol; float l_blend; float l_gain_LP; @@ -586,11 +596,11 @@ typedef struct { float x_maximal; float x_minimal; float x_damping; - float x_tolerance; + float x_residual_tol; float x_gain_P; float x_gain_D; - float dbg[2]; + float d_flux_rsu; void (* proc_set_DC) (int, int, int); void (* proc_set_Z) (int); diff --git a/src/phobia/pm_fsm.c b/src/phobia/pm_fsm.c index 1e47aca..b3fbd0f 100644 --- a/src/phobia/pm_fsm.c +++ b/src/phobia/pm_fsm.c @@ -1638,6 +1638,7 @@ pm_fsm_state_lu_startup(pmc_t *pm, int in_ZONE) pm->hfi_wave[0] = 0.f; pm->hfi_wave[1] = 1.f; + pm->hall_ERN = 0; pm->hall_F[0] = 1.f; pm->hall_F[1] = 0.f; pm->hall_wS = 0.f; @@ -1679,10 +1680,15 @@ pm_fsm_state_lu_startup(pmc_t *pm, int in_ZONE) pm->mtpa_D = 0.f; pm->weak_D = 0.f; + pm->v_DC_MAX = PM_DISABLED; + pm->v_DC_MIN = PM_DISABLED; + pm->v_integral = 0.f; + pm->s_setpoint_speed = 0.f; pm->s_track = 0.f; pm->s_integral = 0.f; + pm->l_track = 0.f; pm->l_blend = 0.f; pm->x_setpoint_location = 0.f; @@ -1696,7 +1702,28 @@ pm_fsm_state_lu_startup(pmc_t *pm, int in_ZONE) pm->proc_set_Z(PM_Z_ABC); } else { - pm->lu_MODE = PM_LU_ESTIMATE; + if (pm->config_LU_ESTIMATE != PM_FLUX_NONE) { + + pm->lu_MODE = PM_LU_ESTIMATE; + } + else if (pm->config_LU_SENSOR == PM_SENSOR_HALL) { + + pm->lu_MODE = PM_LU_SENSOR_HALL; + } + else if ( pm->config_LU_SENSOR == PM_SENSOR_EABI + && pm->eabi_ADJUST == PM_ENABLED) { + + pm->lu_MODE = PM_LU_SENSOR_EABI; + } + else if (pm->config_LU_FORCED == PM_ENABLED) { + + pm->lu_MODE = PM_LU_FORCED; + } + else { + pm->fsm_errno = PM_ERROR_INVALID_OPERATION; + pm->fsm_state = PM_STATE_HALT; + pm->fsm_phase = 0; + } pm->proc_set_DC(0, 0, 0); pm->proc_set_Z(PM_Z_NONE); diff --git a/src/regdefs.h b/src/regdefs.h index c6a16b8..40b41ea 100644 --- a/src/regdefs.h +++ b/src/regdefs.h @@ -14,6 +14,10 @@ ID_HAL_ADC_KNOB_RATIO, #endif /* HW_HAVE_ANALOG_KNOB */ ID_HAL_ADC_SAMPLE_TIME, ID_HAL_ADC_SAMPLE_ADVANCE, +#ifdef HW_HAVE_NETWORK_EPCAN +ID_HAL_CAN_BITFREQ, +ID_HAL_CAN_ERRATE, +#endif /* HW_HAVE_NETWORK_EPCAN */ ID_HAL_DPS_MODE, ID_HAL_PPM_MODE, ID_HAL_PPM_FREQUENCY, @@ -27,7 +31,6 @@ ID_HAL_DRV_AUTO_RESTART, ID_HAL_DRV_STATUS_RAW, ID_HAL_DRV_GATE_CURRENT, ID_HAL_DRV_OCP_LEVEL, -ID_HAL_DRV_FAULT_SAFETY, #endif /* HW_HAVE_DRV_ON_PCB */ ID_HAL_CNT_DIAG0, ID_HAL_CNT_DIAG0_PC, @@ -119,8 +122,8 @@ ID_AP_KNOB_RANGE_ANG2, ID_AP_KNOB_RANGE_BRK0, ID_AP_KNOB_RANGE_BRK1, #endif /* HW_HAVE_BRAKE_KNOB */ -ID_AP_KNOB_RANGE_LST0, -ID_AP_KNOB_RANGE_LST1, +ID_AP_KNOB_RANGE_LOS0, +ID_AP_KNOB_RANGE_LOS1, ID_AP_KNOB_CONTROL_ANG0, ID_AP_KNOB_CONTROL_ANG1, ID_AP_KNOB_CONTROL_ANG2, @@ -444,6 +447,11 @@ ID_PM_WEAK_D, ID_PM_WEAK_GAIN_EU, ID_PM_V_MAXIMAL, ID_PM_V_REVERSE, +ID_PM_V_DC_MAX, +ID_PM_V_DC_MIN, +ID_PM_V_UDC_TOL, +ID_PM_V_GAIN_P, +ID_PM_V_GAIN_I, ID_PM_S_SETPOINT_SPEED, ID_PM_S_SETPOINT_SPEED_RPM, ID_PM_S_SETPOINT_SPEED_MMPS, @@ -467,6 +475,7 @@ ID_PM_S_GAIN_P, ID_PM_S_GAIN_I, ID_PM_S_GAIN_D, ID_PM_S_GAIN_Q, +ID_PM_L_TRACK, ID_PM_L_TRACK_TOL, ID_PM_L_GAIN_LP, ID_PM_X_SETPOINT_LOCATION, @@ -483,14 +492,13 @@ ID_PM_X_MINIMAL_DEG, ID_PM_X_MINIMAL_MM, ID_PM_X_DAMPING, ID_PM_X_DAMPING_MM, -ID_PM_X_TOLERANCE, -ID_PM_X_TOLERANCE_MM, +ID_PM_X_RESIDUAL_TOL, +ID_PM_X_RESIDUAL_TOL_MM, ID_PM_X_GAIN_P, ID_PM_X_GAIN_P_RADPS, ID_PM_X_GAIN_P_MMPS, ID_PM_X_GAIN_D, -ID_PM_DBG0, -ID_PM_DBG1, +ID_PM_D_FLUX_RSU, ID_TLM_RATE_GRAB, ID_TLM_RATE_LIVE, ID_TLM_MODE, diff --git a/src/regfile.c b/src/regfile.c index 9cbd2d0..3d1b529 100644 --- a/src/regfile.c +++ b/src/regfile.c @@ -74,85 +74,81 @@ reg_proc_ADC(const reg_t *reg, rval_t *lval, const rval_t *rval) #ifdef HW_HAVE_NETWORK_EPCAN static void -reg_proc_net_rate(const reg_t *reg, rval_t *lval, const rval_t *rval) +reg_proc_CAN_bitfreq(const reg_t *reg, rval_t *lval, const rval_t *rval) { if (lval != NULL) { - lval->f = hal.PWM_frequency / (float) reg->link->i; + lval->i = reg->link->i; } else if (rval != NULL) { - reg->link->i = (int) (hal.PWM_frequency / rval->f + 0.5f); + reg->link->i = rval->i; + + hal_memory_fence(); + + CAN_configure(); } } static void -reg_proc_net_ID(const reg_t *reg, rval_t *lval, const rval_t *rval) +reg_proc_CAN_errate(const reg_t *reg, rval_t *lval, const rval_t *rval) { if (lval != NULL) { - lval->i = reg->link->i; - } - else if (rval != NULL) { - - reg->link->i = rval->i; + hal.CAN_errate = CAN_errate(); hal_memory_fence(); - EPCAN_filter_ID(); + lval->i = reg->link->i; } } static void -reg_proc_net_timeout(const reg_t *reg, rval_t *lval, const rval_t *rval) +reg_proc_CAN_epfreq(const reg_t *reg, rval_t *lval, const rval_t *rval) { if (lval != NULL) { - lval->f = 1000.f / hal.PWM_frequency * (float) reg->link->i; + lval->f = hal.PWM_frequency / (float) reg->link->i; } else if (rval != NULL) { - reg->link->i = (int) (rval->f * hal.PWM_frequency / 1000.f + 0.5f); + reg->link->i = (int) (hal.PWM_frequency / rval->f + 0.5f); } } -#endif /* HW_HAVE_NETWORK_EPCAN */ - -#undef APP_DEF -#define APP_DEF(name) extern void app_ ## name(void *); -#include "app/apdefs.h" - -void app_control(const reg_t *reg, void (* pvTask) (void *), const char *pcName); static void -reg_proc_task(const reg_t *reg, rval_t *lval, const rval_t *rval) +reg_proc_CAN_ID(const reg_t *reg, rval_t *lval, const rval_t *rval) { - int reg_ID; - if (lval != NULL) { lval->i = reg->link->i; } else if (rval != NULL) { - reg_ID = (int) (reg - regfile); - reg->link->i = rval->i; - switch (reg_ID) { + hal_memory_fence(); -#undef APP_DEF -#define APP_DEF(name) case ID_AP_TASK_ ## name: \ - app_control(reg, app_ ## name, PM_SFI(name)); \ - break; -#include "app/apdefs.h" + EPCAN_bind(); + } +} - default: break; - } +static void +reg_proc_CAN_timeout(const reg_t *reg, rval_t *lval, const rval_t *rval) +{ + if (lval != NULL) { + + lval->f = 1000.f / hal.PWM_frequency * (float) reg->link->i; + } + else if (rval != NULL) { + + reg->link->i = (int) (rval->f * hal.PWM_frequency / 1000.f + 0.5f); } } +#endif /* HW_HAVE_NETWORK_EPCAN */ static void -reg_proc_PPM(const reg_t *reg, rval_t *lval, const rval_t *rval) +reg_proc_DPS(const reg_t *reg, rval_t *lval, const rval_t *rval) { if (lval != NULL) { @@ -164,13 +160,12 @@ reg_proc_PPM(const reg_t *reg, rval_t *lval, const rval_t *rval) hal_memory_fence(); - PPM_configure(); + DPS_configure(); } } -#ifdef HW_HAVE_STEP_DIR_KNOB static void -reg_proc_STEP(const reg_t *reg, rval_t *lval, const rval_t *rval) +reg_proc_PPM(const reg_t *reg, rval_t *lval, const rval_t *rval) { if (lval != NULL) { @@ -182,13 +177,13 @@ reg_proc_STEP(const reg_t *reg, rval_t *lval, const rval_t *rval) hal_memory_fence(); - STEP_configure(); + PPM_configure(); } } -#endif /* HW_HAVE_STEP_DIR_KNOB */ +#ifdef HW_HAVE_STEP_DIR_KNOB static void -reg_proc_DPS(const reg_t *reg, rval_t *lval, const rval_t *rval) +reg_proc_STEP(const reg_t *reg, rval_t *lval, const rval_t *rval) { if (lval != NULL) { @@ -200,9 +195,10 @@ reg_proc_DPS(const reg_t *reg, rval_t *lval, const rval_t *rval) hal_memory_fence(); - DPS_configure(); + STEP_configure(); } } +#endif /* HW_HAVE_STEP_DIR_KNOB */ #ifdef HW_HAVE_DRV_ON_PCB static void @@ -278,6 +274,40 @@ reg_proc_CNT_diag_pc(const reg_t *reg, rval_t *lval, const rval_t *rval) } } +#undef APP_DEF +#define APP_DEF(name) extern void app_ ## name(void *); +#include "app/apdefs.h" + +void app_control(const reg_t *reg, void (* pvTask) (void *), const char *pcName); + +static void +reg_proc_task(const reg_t *reg, rval_t *lval, const rval_t *rval) +{ + int reg_ID; + + if (lval != NULL) { + + lval->i = reg->link->i; + } + else if (rval != NULL) { + + reg_ID = (int) (reg - regfile); + + reg->link->i = rval->i; + + switch (reg_ID) { + +#undef APP_DEF +#define APP_DEF(name) case ID_AP_TASK_ ## name: \ + app_control(reg, app_ ## name, PM_SFI(name)); \ + break; +#include "app/apdefs.h" + + default: break; + } + } +} + static void reg_proc_ppm_freq(const reg_t *reg, rval_t *lval, const rval_t *rval) { @@ -865,6 +895,24 @@ reg_proc_tlm_rate(const reg_t *reg, rval_t *lval, const rval_t *rval) } } +#ifdef HW_HAVE_DRV_ON_PCB +static void +reg_format_DRV_gate_current(const reg_t *reg) +{ + float current = DRV_gate_current(); + + printf("%0i (%3f A)", reg->link->i, ¤t); +} + +static void +reg_format_DRV_ocp_level(const reg_t *reg) +{ + float level = DRV_ocp_level(); + + printf("%0i (%3f V)", reg->link->i, &level); +} +#endif /* HW_HAVE_DRV_ON_PCB */ + static void reg_format_self_BST(const reg_t *reg) { @@ -1432,6 +1480,18 @@ reg_format_enum(const reg_t *reg) } break; + case ID_PM_V_DC_MAX: + case ID_PM_V_DC_MIN: + + switch (val) { + + PM_SFI_CASE(PM_DISABLED); + PM_SFI_CASE(PM_ENABLED); + + default: break; + } + break; + case ID_TLM_MODE: switch (val) { @@ -1472,6 +1532,11 @@ const reg_t regfile[] = { REG_DEF(hal.ADC_sample_time,,, "", "%0i", REG_CONFIG, ®_proc_ADC, ®_format_enum), REG_DEF(hal.ADC_sample_advance,,, "", "%0i", REG_CONFIG, ®_proc_PWM, NULL), +#ifdef HW_HAVE_NETWORK_EPCAN + REG_DEF(hal.CAN_bitfreq,,, "Hz", "%0i", REG_CONFIG, ®_proc_CAN_bitfreq, NULL), + REG_DEF(hal.CAN_errate,,, "", "%4x", REG_READ_ONLY, ®_proc_CAN_errate, NULL), +#endif /* HW_HAVE_NETWORK_EPCAN */ + REG_DEF(hal.DPS_mode,,, "", "%0i", REG_CONFIG, ®_proc_DPS, ®_format_enum), REG_DEF(hal.PPM_mode,,, "", "%0i", REG_CONFIG, ®_proc_PPM, ®_format_enum), REG_DEF(hal.PPM_frequency,,, "Hz", "%0i", REG_CONFIG, ®_proc_PPM, NULL), @@ -1485,9 +1550,8 @@ const reg_t regfile[] = { REG_DEF(hal.DRV.partno,,, "", "%0i", REG_CONFIG, ®_proc_DRV_partno, ®_format_enum), REG_DEF(hal.DRV.auto_RESTART,,, "", "%0i", REG_CONFIG, NULL, ®_format_enum), REG_DEF(hal.DRV.status_raw,,, "", "%4x", REG_READ_ONLY, ®_proc_DRV_status, NULL), - REG_DEF(hal.DRV.gate_current,,, "", "%0i", REG_CONFIG, ®_proc_DRV_configure, NULL), - REG_DEF(hal.DRV.ocp_level,,, "", "%0i", REG_CONFIG, ®_proc_DRV_configure, NULL), - REG_DEF(hal.DRV.fault_safety,,, "", "%0i", REG_CONFIG, NULL, NULL), + REG_DEF(hal.DRV.gate_current,,, "", "%0i", REG_CONFIG, ®_proc_DRV_configure, ®_format_DRV_gate_current), + REG_DEF(hal.DRV.ocp_level,,, "", "%0i", REG_CONFIG, ®_proc_DRV_configure, ®_format_DRV_ocp_level), #endif /* HW_HAVE_DRV_ON_PCB */ REG_DEF(hal.CNT_diag, 0, [0], "us", "%2f", REG_READ_ONLY, ®_proc_CNT_diag_us, NULL), @@ -1498,51 +1562,51 @@ const reg_t regfile[] = { REG_DEF(hal.CNT_diag, 2_pc, [2], "%", "%1f", REG_READ_ONLY, ®_proc_CNT_diag_pc, NULL), #ifdef HW_HAVE_NETWORK_EPCAN - REG_DEF(net.node_ID,,, "", "%0i", REG_CONFIG, ®_proc_net_ID, NULL), - REG_DEF(net.log_MODE,,, "", "%0i", REG_CONFIG, ®_proc_net_ID, ®_format_enum), - REG_DEF(net.timeout_EP,,, "ms", "%1f", REG_CONFIG, ®_proc_net_timeout, NULL), + REG_DEF(net.node_ID,,, "", "%0i", REG_CONFIG, ®_proc_CAN_ID, NULL), + REG_DEF(net.log_MODE,,, "", "%0i", REG_CONFIG, ®_proc_CAN_ID, ®_format_enum), + REG_DEF(net.timeout_EP,,, "ms", "%1f", REG_CONFIG, ®_proc_CAN_timeout, NULL), - REG_DEF(net.ep, 0_MODE, [0].MODE,"", "%0i", REG_CONFIG, ®_proc_net_ID, ®_format_enum), - REG_DEF(net.ep, 0_ID, [0].ID,"", "%0i", REG_CONFIG, ®_proc_net_ID, NULL), - REG_DEF(net.ep, 0_clock_ID, [0].clock_ID,"", "%0i", REG_CONFIG, ®_proc_net_ID, NULL), + REG_DEF(net.ep, 0_MODE, [0].MODE,"", "%0i", REG_CONFIG, ®_proc_CAN_ID, ®_format_enum), + REG_DEF(net.ep, 0_ID, [0].ID,"", "%0i", REG_CONFIG, ®_proc_CAN_ID, NULL), + REG_DEF(net.ep, 0_clock_ID, [0].clock_ID,"", "%0i", REG_CONFIG, ®_proc_CAN_ID, NULL), REG_DEF(net.ep, 0_reg_DATA, [0].reg_DATA,"", "%4f", 0, NULL, ®_format_referenced_net_ep0), REG_DEF(net.ep, 0_reg_ID, [0].reg_ID,"", "%0i", REG_CONFIG | REG_LINKED, NULL, NULL), REG_DEF(net.ep, 0_PAYLOAD, [0].PAYLOAD,"", "%0i", REG_CONFIG, NULL, ®_format_enum), REG_DEF(net.ep, 0_STARTUP, [0].STARTUP,"", "%0i", REG_CONFIG, NULL, ®_format_enum), - REG_DEF(net.ep, 0_rate, [0].rate, "Hz", "%1f", REG_CONFIG, ®_proc_net_rate, NULL), + REG_DEF(net.ep, 0_rate, [0].rate, "Hz", "%1f", REG_CONFIG, ®_proc_CAN_epfreq, NULL), REG_DEF(net.ep, 0_range0, [0].range[0],"", "%4f", REG_CONFIG, NULL, ®_format_referenced_net_ep0), REG_DEF(net.ep, 0_range1, [0].range[1],"", "%4f", REG_CONFIG, NULL, ®_format_referenced_net_ep0), - REG_DEF(net.ep, 1_MODE, [1].MODE,"", "%0i", REG_CONFIG, ®_proc_net_ID, ®_format_enum), - REG_DEF(net.ep, 1_ID, [1].ID,"", "%0i", REG_CONFIG, ®_proc_net_ID, NULL), - REG_DEF(net.ep, 1_clock_ID, [1].clock_ID,"", "%0i", REG_CONFIG, ®_proc_net_ID, NULL), + REG_DEF(net.ep, 1_MODE, [1].MODE,"", "%0i", REG_CONFIG, ®_proc_CAN_ID, ®_format_enum), + REG_DEF(net.ep, 1_ID, [1].ID,"", "%0i", REG_CONFIG, ®_proc_CAN_ID, NULL), + REG_DEF(net.ep, 1_clock_ID, [1].clock_ID,"", "%0i", REG_CONFIG, ®_proc_CAN_ID, NULL), REG_DEF(net.ep, 1_reg_DATA, [1].reg_DATA,"", "%4f", 0, NULL, ®_format_referenced_net_ep1), REG_DEF(net.ep, 1_reg_ID, [1].reg_ID,"", "%0i", REG_CONFIG | REG_LINKED, NULL, NULL), REG_DEF(net.ep, 1_PAYLOAD, [1].PAYLOAD,"", "%0i", REG_CONFIG, NULL, ®_format_enum), REG_DEF(net.ep, 1_STARTUP, [1].STARTUP,"", "%0i", REG_CONFIG, NULL, ®_format_enum), - REG_DEF(net.ep, 1_rate, [1].rate, "Hz", "%1f", REG_CONFIG, ®_proc_net_rate, NULL), + REG_DEF(net.ep, 1_rate, [1].rate, "Hz", "%1f", REG_CONFIG, ®_proc_CAN_epfreq, NULL), REG_DEF(net.ep, 1_range0, [1].range[0],"", "%4f", REG_CONFIG, NULL, ®_format_referenced_net_ep1), REG_DEF(net.ep, 1_range1, [1].range[1],"", "%4f", REG_CONFIG, NULL, ®_format_referenced_net_ep1), - REG_DEF(net.ep, 2_MODE, [2].MODE,"", "%0i", REG_CONFIG, ®_proc_net_ID, ®_format_enum), - REG_DEF(net.ep, 2_ID, [2].ID,"", "%0i", REG_CONFIG, ®_proc_net_ID, NULL), - REG_DEF(net.ep, 2_clock_ID, [2].clock_ID,"", "%0i", REG_CONFIG, ®_proc_net_ID, NULL), + REG_DEF(net.ep, 2_MODE, [2].MODE,"", "%0i", REG_CONFIG, ®_proc_CAN_ID, ®_format_enum), + REG_DEF(net.ep, 2_ID, [2].ID,"", "%0i", REG_CONFIG, ®_proc_CAN_ID, NULL), + REG_DEF(net.ep, 2_clock_ID, [2].clock_ID,"", "%0i", REG_CONFIG, ®_proc_CAN_ID, NULL), REG_DEF(net.ep, 2_reg_DATA, [2].reg_DATA,"", "%4f", 0, NULL, ®_format_referenced_net_ep2), REG_DEF(net.ep, 2_reg_ID, [2].reg_ID,"", "%0i", REG_CONFIG | REG_LINKED, NULL, NULL), REG_DEF(net.ep, 2_PAYLOAD, [2].PAYLOAD,"", "%0i", REG_CONFIG, NULL, ®_format_enum), REG_DEF(net.ep, 2_STARTUP, [2].STARTUP,"", "%0i", REG_CONFIG, NULL, ®_format_enum), - REG_DEF(net.ep, 2_rate, [2].rate, "Hz", "%1f", REG_CONFIG, ®_proc_net_rate, NULL), + REG_DEF(net.ep, 2_rate, [2].rate, "Hz", "%1f", REG_CONFIG, ®_proc_CAN_epfreq, NULL), REG_DEF(net.ep, 2_range0, [2].range[0],"", "%4f", REG_CONFIG, NULL, ®_format_referenced_net_ep2), REG_DEF(net.ep, 2_range1, [2].range[1],"", "%4f", REG_CONFIG, NULL, ®_format_referenced_net_ep2), - REG_DEF(net.ep, 3_MODE, [3].MODE,"", "%0i", REG_CONFIG, ®_proc_net_ID, ®_format_enum), - REG_DEF(net.ep, 3_ID, [3].ID,"", "%0i", REG_CONFIG, ®_proc_net_ID, NULL), - REG_DEF(net.ep, 3_clock_ID, [3].clock_ID,"", "%0i", REG_CONFIG, ®_proc_net_ID, NULL), + REG_DEF(net.ep, 3_MODE, [3].MODE,"", "%0i", REG_CONFIG, ®_proc_CAN_ID, ®_format_enum), + REG_DEF(net.ep, 3_ID, [3].ID,"", "%0i", REG_CONFIG, ®_proc_CAN_ID, NULL), + REG_DEF(net.ep, 3_clock_ID, [3].clock_ID,"", "%0i", REG_CONFIG, ®_proc_CAN_ID, NULL), REG_DEF(net.ep, 3_reg_DATA, [3].reg_DATA,"", "%4f", 0, NULL, ®_format_referenced_net_ep3), REG_DEF(net.ep, 3_reg_ID, [3].reg_ID,"", "%0i", REG_CONFIG | REG_LINKED, NULL, NULL), REG_DEF(net.ep, 3_PAYLOAD, [3].PAYLOAD,"", "%0i", REG_CONFIG, NULL, ®_format_enum), REG_DEF(net.ep, 3_STARTUP, [3].STARTUP,"", "%0i", REG_CONFIG, NULL, ®_format_enum), - REG_DEF(net.ep, 3_rate, [3].rate, "Hz", "%1f", REG_CONFIG, ®_proc_net_rate, NULL), + REG_DEF(net.ep, 3_rate, [3].rate, "Hz", "%1f", REG_CONFIG, ®_proc_CAN_epfreq, NULL), REG_DEF(net.ep, 3_range0, [3].range[0],"", "%4f", REG_CONFIG, NULL, ®_format_referenced_net_ep3), REG_DEF(net.ep, 3_range1, [3].range[1],"", "%4f", REG_CONFIG, NULL, ®_format_referenced_net_ep3), #endif /* HW_HAVE_NETWORK_EPCAN */ @@ -1588,8 +1652,8 @@ const reg_t regfile[] = { REG_DEF(ap.knob_range_BRK, 0, [0], "V", "%3f", REG_CONFIG, NULL, NULL), REG_DEF(ap.knob_range_BRK, 1, [1], "V", "%3f", REG_CONFIG, NULL, NULL), #endif /* HW_HAVE_BRAKE_KNOB */ - REG_DEF(ap.knob_range_LST, 0, [0], "V", "%3f", REG_CONFIG, NULL, NULL), - REG_DEF(ap.knob_range_LST, 1, [1], "V", "%3f", REG_CONFIG, NULL, NULL), + REG_DEF(ap.knob_range_LOS, 0, [0], "V", "%3f", REG_CONFIG, NULL, NULL), + REG_DEF(ap.knob_range_LOS, 1, [1], "V", "%3f", REG_CONFIG, NULL, NULL), REG_DEF(ap.knob_control_ANG, 0, [0], "", "%2f", REG_CONFIG, NULL, ®_format_referenced_knob), REG_DEF(ap.knob_control_ANG, 1, [1], "", "%2f", REG_CONFIG, NULL, ®_format_referenced_knob), REG_DEF(ap.knob_control_ANG, 2, [2], "", "%2f", REG_CONFIG, NULL, ®_format_referenced_knob), @@ -1952,6 +2016,13 @@ const reg_t regfile[] = { REG_DEF(pm.v_maximal,,, "V", "%3f", REG_CONFIG, NULL, NULL), REG_DEF(pm.v_reverse,,, "V", "%3f", REG_CONFIG, NULL, NULL), + REG_DEF(pm.v_DC_MAX,,, "", "%0i", REG_READ_ONLY, NULL, ®_format_enum), + REG_DEF(pm.v_DC_MIN,,, "", "%0i", REG_READ_ONLY, NULL, ®_format_enum), + + REG_DEF(pm.v_uDC_tol,,, "V", "%3f", REG_CONFIG, NULL, NULL), + REG_DEF(pm.v_gain_P,,, "", "%2e", REG_CONFIG, NULL, NULL), + REG_DEF(pm.v_gain_I,,, "", "%2e", REG_CONFIG, NULL, NULL), + REG_DEF(pm.s_setpoint_speed,,, "rad/s", "%2f", 0, NULL, NULL), REG_DEF(pm.s_setpoint_speed, _rpm,, "rpm", "%2f", 0, ®_proc_rpm, NULL), REG_DEF(pm.s_setpoint_speed, _mmps,, "mm/s", "%2f", 0, ®_proc_mmps, NULL), @@ -1976,6 +2047,7 @@ const reg_t regfile[] = { REG_DEF(pm.s_gain_D,,, "", "%2e", REG_CONFIG, NULL, NULL), REG_DEF(pm.s_gain_Q,,, "", "%2e", REG_CONFIG, NULL, NULL), + REG_DEF(pm.l_track,,, "rad/s", "%2f", REG_READ_ONLY, NULL, NULL), REG_DEF(pm.l_track_tol,,, "rad/s", "%2f", REG_CONFIG, NULL, NULL), REG_DEF(pm.l_gain_LP,,, "", "%2e", REG_CONFIG, NULL, NULL), @@ -1993,15 +2065,14 @@ const reg_t regfile[] = { REG_DEF(pm.x_minimal, _mm,, "mm", "%3f", 0, ®_proc_location_mm, NULL), REG_DEF(pm.x_damping,,, "rad", "%4f", REG_CONFIG, NULL, NULL), REG_DEF(pm.x_damping, _mm,, "mm", "%3f", 0, ®_proc_mmps, NULL), - REG_DEF(pm.x_tolerance,,, "rad", "%4f", REG_CONFIG, NULL, NULL), - REG_DEF(pm.x_tolerance, _mm,, "mm", "%3f", 0, ®_proc_mmps, NULL), + REG_DEF(pm.x_residual_tol,,, "rad", "%4f", REG_CONFIG, NULL, NULL), + REG_DEF(pm.x_residual_tol, _mm,, "mm", "%3f", 0, ®_proc_mmps, NULL), REG_DEF(pm.x_gain_P,,, "", "%1f", REG_CONFIG, NULL, NULL), REG_DEF(pm.x_gain_P, _radps,, "rad/s2", "%1f", 0, ®_proc_x_accel, NULL), REG_DEF(pm.x_gain_P, _mmps,, "mm/s2", "%1f", 0, ®_proc_x_accel_mm, NULL), REG_DEF(pm.x_gain_D,,, "", "%1f", REG_CONFIG, NULL, NULL), - REG_DEF(pm.dbg, 0, [0], "", "%4f", REG_READ_ONLY, NULL, NULL), - REG_DEF(pm.dbg, 1, [1], "", "%4f", REG_READ_ONLY, NULL, NULL), + REG_DEF(pm.d_flux_rsu,,, "", "%3f", REG_READ_ONLY, NULL, NULL), REG_DEF(tlm.rate_grab,,, "Hz", "%1f", REG_CONFIG, ®_proc_tlm_rate, NULL), REG_DEF(tlm.rate_live,,, "Hz", "%1f", REG_CONFIG, ®_proc_tlm_rate, NULL), diff --git a/src/tlm.h b/src/tlm.h index 40d2378..4ff6b78 100644 --- a/src/tlm.h +++ b/src/tlm.h @@ -5,7 +5,7 @@ #include "regfile.h" -#define TLM_DATA_MAX 20000 /* memory to keep telemetry data */ +#define TLM_DATA_MAX 20000 #define TLM_INPUT_MAX 10 enum { @@ -34,7 +34,7 @@ typedef struct { int rate; int line; - rval_t rdata[TLM_DATA_MAX]; + rval_t rdata[TLM_DATA_MAX]; /* memory to keep telemetry data */ } tlm_t;