diff --git a/README.md b/README.md
index f1ca017..aa87abc 100644
--- a/README.md
+++ b/README.md
@@ -20,7 +20,7 @@ There are a few videos about PMC on [youtube](https://www.youtube.com/@romblv).
 
 - Sensorless vector control of PMSM by measurement of currents and voltages.
 - Robust ORTEGA observer with gain scheduling against speed.
-- Accurate KALMAN observer having convergence at HF injection.
+- Accurate KALMAN observer having convergence at HF injection (**EXPERIMENTAL**).
 - Flux weakening and MTPA control (**EXPERIMENTAL**).
 - Three and two phases machine connection.
 - Hardware abstraction layer (HAL) over STM32F4 and STM32F7.
@@ -41,6 +41,7 @@ There are a few videos about PMC on [youtube](https://www.youtube.com/@romblv).
   with inline or low-side placement.
 - Self-adjustment of all onboard measurements (current and voltage) along
   symmetrical channels.
+- Dead-Time Compensation (DTC) based on currents polatity (**EXPERIMENTAL**).
 
 - Advanced SVPWM scheme provides:
 	- Reduced switching losses and fully utilised DC link voltage.
@@ -49,14 +50,13 @@ There are a few videos about PMC on [youtube](https://www.youtube.com/@romblv).
 	- Prevent bootstrap circuit undervoltage condition.
 
 - Terminal voltage measurements (TVM):
-	- Compensation of the voltage distortion caused by dead-time insertion.
 	- Back EMF voltage tracking to catch an already running machine.
 	- Self-test of the power stages integrity and machine wiring.
 	- Self-test of bootstrap retention time.
 
-- Automated machine parameters identification (with no external tools):
+- Automated machine parameters identification:
 	- Stator DC resistance (Rs).
-	- Stator AC impedance in DQ frame (L1, L2, R).
+	- Stator AC impedance in DQ frame (Ld, Lq, R).
 	- Rotor flux linkage constant (lambda).
 	- Mechanical moment of inertia (Ja).
 
@@ -82,7 +82,7 @@ There are a few videos about PMC on [youtube](https://www.youtube.com/@romblv).
 
 - Adjustable constraints:
 	- Phase current (forward and reverse, on HFI current, weakening D current).
-	- Hardware overtemperature protection (decrease phase current or halt).
+	- Hardware overtemperature protection (derate phase current or halt).
 	- Machine voltage applied from VSI.
 	- DC link current consumption and regeneration.
 	- DC link overvoltage and undervoltage.
@@ -90,7 +90,7 @@ There are a few videos about PMC on [youtube](https://www.youtube.com/@romblv).
 	- Absolute location limits in servo operation.
 
 - Input control interfaces:
-	- Analog input knob with brake signal.
+	- Analog input knob and brake signal.
 	- RC servo pulse width modulation.
 	- CAN bus flexible configurable data transfers.
 	- STEP/DIR (or CW/CCW) interface (**EXPERIMENTAL**).
@@ -105,7 +105,7 @@ There are a few videos about PMC on [youtube](https://www.youtube.com/@romblv).
 	- Flexible configurable data transfers.
 
 - Available information:
-	- Machine state (electrical position, speed, load torque, etc.)
+	- Machine state (angle, speed, torque, current, etc.)
 	- DC link voltage and current consumption.
 	- Information from temperature sensors.
 	- Total distance traveled.
diff --git a/bench/bench.c b/bench/bench.c
index 0b82dea..45e5c63 100644
--- a/bench/bench.c
+++ b/bench/bench.c
@@ -144,15 +144,10 @@ tlm_plot_grab()
 	fmt_GP(pm.vsi_BF, 0);
 	fmt_GP(pm.vsi_CF, 0);
 	fmt_GP(pm.vsi_IF, 0);
-	fmt_GP(pm.vsi_SF, 0);
 	fmt_GP(pm.vsi_UF, 0);
 
-	fmt_GP(pm.tvm_A, "V");
-	fmt_GP(pm.tvm_B, "V");
-	fmt_GP(pm.tvm_C, "V");
-
-	fmt_GP(pm.tvm_X0, "V");
-	fmt_GP(pm.tvm_Y0, "V");
+	fmt_GP(pm.dtc_uX, "V");
+	fmt_GP(pm.dtc_uY, "V");
 
 	fmt_GP(pm.lu_MODE, 0);
 	fmk_GP(pm.lu_mq_produce, pm.const_Zp, "Nm");
@@ -177,8 +172,8 @@ tlm_plot_grab()
 	sym_GP(atan2(pm.flux_F[1], pm.flux_F[0]) * kDEG, "pm.flux_F", "deg");
 	fmk_GP(pm.flux_wS, kRPM, "rpm");
 
-	fmt_GP(pm.kalman_residual_D, "A");
-	fmt_GP(pm.kalman_residual_Q, "A");
+	fmt_GP(pm.kalman_E[0], "A");
+	fmt_GP(pm.kalman_E[1], "A");
 	fmt_GP(pm.kalman_bias_Q, "V");
 	fmk_GP(pm.kalman_lpf_wS, kRPM, "rpm");
 
@@ -379,60 +374,61 @@ void bench_script()
 
 	tlm_restart();
 
-	/*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, 87.);
-	m.Jm = 0.82E-3;
-
-	/*m.Rs = 257.E-3;
-	m.Ld = 1.1E-3;
-	m.Lq = 1.5E-3;
-	m.Udc = 48.;
+	m.Rs = 7.E-3;
+	m.Ld = 37.E-6;
+	m.Lq = 57.E-6;
+	m.Udc = 49.;
 	m.Rdc = 0.1;
-	m.Zp = 1;
-	m.lambda = blm_Kv_lambda(&m, 55.);
-	m.Jm = 5.39E-3;*/
+	m.Zp = 5;
+	m.lambda = blm_Kv_lambda(&m, 58.);
+	m.Jm = 17.E-3;
 
 	ts_script_default();
 	ts_script_base();
 	blm_restart(&m);
 
+	ts_adjust_sensor_hall();
+	blm_restart(&m);
+
 	pm.config_LU_ESTIMATE = PM_FLUX_KALMAN;
+	pm.config_LU_SENSOR = PM_SENSOR_HALL;
+	pm.config_HFI_WAVETYPE = PM_HFI_SINE;
+	//pm.config_DTC_VOLTAGE = PM_DISABLED;
+
+	pm.watt_wA_maximal = 80.f;
+	pm.watt_wA_reverse = 80.f;
+
+	pm.zone_threshold *= 2.f;
+
+	pm.hall_gain_IF = 0.1f;
+	pm.flux_gain_IF = 0.1f;
+
+	m.Jm = 17.E-3 * 48. / 10.;
+
+	/*pm.const_im_Ld /= 1.2f;
+	pm.const_im_Lq /= 1.2f;*/
 
 	pm.fsm_req = PM_STATE_LU_STARTUP;
 	ts_wait_IDLE();
 
-	pm.s_setpoint_speed = 4000.f;
+	pm.s_setpoint_speed = 10.f;
 	sim_runtime(1.0);
 
-	//m.Udc = 30.;
-	//m.Rdc = 50.;
+	pm.s_setpoint_speed = 100.f;
+	sim_runtime(2.0);
+
+	pm.s_setpoint_speed = 2000.f;
+	sim_runtime(1.0);
 
-	pm.watt_wP_reverse = 1000.f;
-	pm.watt_uDC_maximal = 48.f;
-	//pm.watt_uDC_tol = 10.f;
+	pm.s_setpoint_speed = - 2000.f;
+	sim_runtime(2.0);
 
-	sim_runtime(0.2);
+	pm.s_setpoint_speed = 2000.f;
+	sim_runtime(2.0);
 
-	pm.s_setpoint_speed = 0.f;
-	//pm.s_track = 0.f;
-	sim_runtime(0.3);
+	
 
-	//m.Rdc = 1.;
-	sim_runtime(0.5);
+	sim_runtime(3.0);
 
 	tlm_PWM_grab();
 }
diff --git a/bench/blm.c b/bench/blm.c
index 3dab8f7..b562ecb 100644
--- a/bench/blm.c
+++ b/bench/blm.c
@@ -51,7 +51,7 @@ void blm_enable(blm_t *m)
 	m->sol_dT = 5.E-6;	/* ODE solver step (Second) */
 
 	m->pwm_dT = 35.E-6;		/* PWM cycle (Second)    */
-	m->pwm_deadtime = 170.E-9;	/* PWM deadtime (Second) */
+	m->pwm_deadtime = 90.E-9;	/* PWM deadtime (Second) */
 	m->pwm_minimal = 50.E-9;	/* PWM minimal (Second)  */
 	m->pwm_resolution = 2940;	/* PWM resolution        */
 
@@ -61,7 +61,7 @@ void blm_enable(blm_t *m)
 
 	/* Winding resistance (Ohm).
          * */
-	m->Rs = 0.007;
+	m->Rs = 0.011;
 
 	/* Winding inductance (Henry).
          * */
@@ -100,14 +100,14 @@ void blm_enable(blm_t *m)
 	 * */
 	m->Cdc = 940.E-6;
 
-	/* Moment of inertia.
+	/* Moment of inertia (Kilogram Metre squared).
 	 * */
 	m->Jm = 5.E-3;
 
-	/* Load torque constants.
+	/* Load torque polynom.
 	 * */
 	m->Mq[0] = 0.E-3;
-	m->Mq[1] = 5.E-5;
+	m->Mq[1] = 5.E-4;
 	m->Mq[2] = 5.E-7;
 	m->Mq[3] = 5.E-2;
 
@@ -125,7 +125,7 @@ void blm_enable(blm_t *m)
 	m->range_A = 165.;	/* (Ampere) */
 	m->range_B = 60.;	/* (Volt)   */
 
-	/* Hall sensor installation angles.
+	/* Hall Sensor installation angles (Degree).
 	 * */
 	m->hall[0] = 30.7;
 	m->hall[1] = 150.1;
@@ -228,7 +228,7 @@ blm_equation(const blm_t *m, const double state[7], double y[7])
 	 * */
 	mS = state[2] / m->Zp;
 	mQ = m->Mq[0] - mS * (m->Mq[1] + fabs(mS) * m->Mq[2]);
-	mQ += (mS < 0.) ? m->Mq[3] : - m->Mq[3];
+	mQ += - mS / (1.f + fabs(mS)) * m->Mq[3];
 
 	/* Mechanical equations.
 	 * */
@@ -244,47 +244,49 @@ blm_equation(const blm_t *m, const double state[7], double y[7])
 static void
 blm_ode_step(blm_t *m, double dT)
 {
-	double		x2[7], y1[7], y2[7];
-	double		iA, iB, iC, uA, uB, uC, kA, kB, uMIN;
+	double		x0[7], y0[7], y1[7];
+
+	double		iA, iB, iC, uA, uB, uC;
+	double		kA, kB, uMIN;
 
 	/* Second-order ODE solver.
 	 * */
 
-	blm_equation(m, m->state, y1);
+	blm_equation(m, m->state, y0);
 
 	if (m->pwm_Z != BLM_Z_DETACHED) {
 
-		x2[0] = m->state[0] + y1[0] * dT;
-		x2[1] = m->state[1] + y1[1] * dT;
+		x0[0] = m->state[0] + y0[0] * dT;
+		x0[1] = m->state[1] + y0[1] * dT;
 	}
 	else {
-		x2[0] = 0.;
-		x2[1] = 0.;
+		x0[0] = 0.;
+		x0[1] = 0.;
 	}
 
-	x2[2] = m->state[2] + y1[2] * dT;
-	x2[3] = m->state[3] + y1[3] * dT;
-	x2[4] = m->state[4] + y1[4] * dT;
-	x2[5] = m->state[5] + y1[5] * dT;
-	x2[6] = m->state[6] + y1[6] * dT;
+	x0[2] = m->state[2] + y0[2] * dT;
+	x0[3] = m->state[3] + y0[3] * dT;
+	x0[4] = m->state[4] + y0[4] * dT;
+	x0[5] = m->state[5] + y0[5] * dT;
+	x0[6] = m->state[6] + y0[6] * dT;
 
-	blm_equation(m, x2, y2);
+	blm_equation(m, x0, y1);
 
 	if (m->pwm_Z != BLM_Z_DETACHED) {
 
-		m->state[0] += (y1[0] + y2[0]) * dT / 2.;
-		m->state[1] += (y1[1] + y2[1]) * dT / 2.;
+		m->state[0] += (y0[0] + y1[0]) * dT / 2.;
+		m->state[1] += (y0[1] + y1[1]) * dT / 2.;
 	}
 	else {
 		m->state[0] = 0.;
 		m->state[1] = 0.;
 	}
 
-	m->state[2] += (y1[2] + y2[2]) * dT / 2.;
-	m->state[3] += (y1[3] + y2[3]) * dT / 2.;
-	m->state[4] += (y1[4] + y2[4]) * dT / 2.;
-	m->state[5] += (y1[5] + y2[5]) * dT / 2.;
-	m->state[6] += (y1[6] + y2[6]) * dT / 2.;
+	m->state[2] += (y0[2] + y1[2]) * dT / 2.;
+	m->state[3] += (y0[3] + y1[3]) * dT / 2.;
+	m->state[4] += (y0[4] + y1[4]) * dT / 2.;
+	m->state[5] += (y0[5] + y1[5]) * dT / 2.;
+	m->state[6] += (y0[6] + y1[6]) * dT / 2.;
 
 	/* Sensor transient (FAST).
 	 * */
diff --git a/bench/tsfunc.c b/bench/tsfunc.c
index 5df14fe..b3819cd 100644
--- a/bench/tsfunc.c
+++ b/bench/tsfunc.c
@@ -100,7 +100,7 @@ int ts_wait_spinup()
 
 void ts_self_adjust()
 {
-	double		tau_A, tau_B, tau_C;
+	double		usual_Mq;
 
 	do {
 		pm.fsm_req = PM_STATE_ZERO_DRIFT;
@@ -108,7 +108,7 @@ void ts_self_adjust()
 
 		printf("const_fb_U = %.3f (V)\n", pm.const_fb_U);
 
-		printf("scale_iABC0 = %.3f %.3f %.3f (A)\n", pm.scale_iA[0],
+		printf("scale_i_0 = %.3f %.3f %.3f (A)\n", pm.scale_iA[0],
 				pm.scale_iB[0], pm.scale_iC[0]);
 
 		printf("self_STDi = %.3f %.3f %.3f (A)\n", pm.self_STDi[0],
@@ -119,46 +119,55 @@ void ts_self_adjust()
 
 		if (PM_CONFIG_TVM(&pm) == PM_ENABLED) {
 
-			pm.fsm_req = PM_STATE_ADJUST_VOLTAGE;
+			pm.fsm_req = PM_STATE_ADJUST_ON_PCB_VOLTAGE;
 			ts_wait_IDLE();
 
 			printf("scale_uA = %.4E %.4f (V)\n", pm.scale_uA[1], pm.scale_uA[0]);
 			printf("scale_uB = %.4E %.4f (V)\n", pm.scale_uB[1], pm.scale_uB[0]);
 			printf("scale_uC = %.4E %.4f (V)\n", pm.scale_uC[1], pm.scale_uC[0]);
 
-			tau_A = pm.m_dT / log(pm.tvm_FIR_A[0] / - pm.tvm_FIR_A[1]);
-			tau_B = pm.m_dT / log(pm.tvm_FIR_B[0] / - pm.tvm_FIR_B[1]);
-			tau_C = pm.m_dT / log(pm.tvm_FIR_C[0] / - pm.tvm_FIR_C[1]);
-
-			printf("tau_A = %.2f (us)\n", tau_A * 1000000.);
-			printf("tau_B = %.2f (us)\n", tau_B * 1000000.);
-			printf("tau_C = %.2f (us)\n", tau_C * 1000000.);
-
-			printf("self_RMSu = %.4f %.4f %.4f (V)\n", pm.self_RMSu[1],
-					pm.self_RMSu[2], pm.self_RMSu[3]);
-
-			TS_assert_relative(tau_A, m.tau_B);
-			TS_assert_relative(tau_B, m.tau_B);
-			TS_assert_relative(tau_C, m.tau_B);
+			printf("self_RMSu = %.4f (V)\n", pm.self_RMSu);
+			printf("self_RMSt = %.4f %.4f %.4f (V)\n", pm.self_RMSt[0],
+					pm.self_RMSt[1], pm.self_RMSt[2]);
 
 			if (pm.fsm_errno != PM_OK)
 				break;
 		}
+
+		usual_Mq = m.Mq[3];
+		m.Mq[3] = 5.E-1;
+
+		pm.fsm_req = PM_STATE_ADJUST_DTC_VOLTAGE;
+		ts_wait_IDLE();
+
+		m.Mq[3] = usual_Mq;
+
+		printf("const_im_Rz = %.4E (Ohm)\n", pm.const_im_Rz);
+		printf("dtc_deadband = %.1f (ns)\n", pm.dtc_deadband);
+		printf("self_DTu = %.4f (V)\n", pm.self_DTu);
 	}
 	while (0);
 }
 
 void ts_probe_impedance()
 {
+	double		usual_Mq;
+
 	do {
+		usual_Mq = m.Mq[3];
+		m.Mq[3] = 5.E-1;
+
 		pm.fsm_req = PM_STATE_PROBE_CONST_RESISTANCE;
 
 		if (ts_wait_IDLE() != PM_OK)
 			break;
 
-		pm.const_Rs = pm.const_im_R;
+		m.Mq[3] = usual_Mq;
+
+		pm.const_Rs = pm.const_im_Rz;
 
 		printf("const_Rs = %.4E (Ohm)\n", pm.const_Rs);
+		printf("self_DTu = %.4f (V)\n", pm.self_DTu);
 
 		TS_assert_relative(pm.const_Rs, m.Rs);
 
@@ -167,13 +176,13 @@ void ts_probe_impedance()
 		if (ts_wait_IDLE() != PM_OK)
 			break;
 
-		printf("const_im_L1 = %.4E (H)\n", pm.const_im_L1);
-		printf("const_im_L2 = %.4E (H)\n", pm.const_im_L2);
-		printf("const_im_B = %.2f (deg)\n", pm.const_im_B);
-		printf("const_im_R = %.4E (Ohm)\n", pm.const_im_R);
+		printf("const_im_Ld = %.4E (H)\n", pm.const_im_Ld);
+		printf("const_im_Lq = %.4E (H)\n", pm.const_im_Lq);
+		printf("const_im_A = %.2f (deg)\n", pm.const_im_A);
+		printf("const_im_Rz = %.4E (Ohm)\n", pm.const_im_Rz);
 
-		TS_assert_relative(pm.const_im_L1, m.Ld);
-		TS_assert_relative(pm.const_im_L2, m.Lq);
+		TS_assert_relative(pm.const_im_Ld, m.Ld);
+		TS_assert_relative(pm.const_im_Lq, m.Lq);
 
 		pm_auto(&pm, PM_AUTO_MAXIMAL_CURRENT);
 		pm_auto(&pm, PM_AUTO_LOOP_CURRENT);
@@ -287,11 +296,11 @@ void ts_probe_spinup()
 		pm.s_setpoint_speed = 110.f * pm.k_EMAX / 100.f
 				* pm.const_fb_U / pm.const_lambda;
 
-		sim_runtime(300 / (double) TS_TICK_RATE);
+		sim_runtime(400 / (double) TS_TICK_RATE);
 
 		pm.s_setpoint_speed = pm.probe_speed_hold;
 
-		sim_runtime(300 / (double) TS_TICK_RATE);
+		sim_runtime(400 / (double) TS_TICK_RATE);
 
 		if (ts_wait_IDLE() != PM_OK)
 			break;
@@ -688,7 +697,7 @@ void ts_script_test()
 	tlm_restart();
 
 	m.Rs = 7.E-3;
-	m.Ld = 2.E-6;
+	m.Ld = 3.E-6;
 	m.Lq = 5.E-6;
 	m.Udc = 48.;
 	m.Rdc = 0.1;
diff --git a/doc/HighFrequencyInjection.md b/doc/HighFrequencyInjection.md
index 5488153..c1f1b4a 100644
--- a/doc/HighFrequencyInjection.md
+++ b/doc/HighFrequencyInjection.md
@@ -16,7 +16,7 @@ Note that non-salient machine implies `Ld = Lq`, negative saliency (BLDC)
 implies `Ld < Lq`, positive saliency (IPM, SynRM) implies `Ld > Lq`.
 
 You should check the inductance difference along the DQ axes after the machine
-probing procedures. Inductances L1 and L2 should differ by more than ~10% to
+probing procedures. Inductances Ld and Lq should differ by more than ~10% to
 get reliable HFI operation.
 
 	(pmc) pm_probe_impedance
diff --git a/doc/MachineProbe.md b/doc/MachineProbe.md
index c2c33c2..7cc9d0b 100644
--- a/doc/MachineProbe.md
+++ b/doc/MachineProbe.md
@@ -76,7 +76,7 @@ values of holding current. For more accuracy you need to increase the probing
 current or reduce DC link voltage.
 
 Then we inject a high frequency sinusoidal signal to measure the full impedance
-tensor and calculate DQ inductances `pm.const_im_L1` and `pm.const_im_L2`.
+tensor and calculate DQ inductances `pm.const_im_Ld` and `pm.const_im_Lq`.
 
 	(pmc) pm_probe_impedance
 
diff --git a/doc/TroubleShooting.md b/doc/TroubleShooting.md
index 174119b..7c3e625 100644
--- a/doc/TroubleShooting.md
+++ b/doc/TroubleShooting.md
@@ -25,9 +25,8 @@ completely inoperable.
 the output terminals. Power stages or terminal voltage sensors are definitely
 broken.
 
-`PM_ERROR_INSUFFICIENT_ACCURACY` - Result of adjustment shows the parameter
-deviation is above the fault threshold. Check the voltage and current
-measurement circuit.
+`PM_ERROR_LOW_ACCURACY` - Result of adjustment shows the parameter deviation is
+above the fault threshold. Check the voltage and current measurement circuit.
 
 `PM_ERROR_CURRENT_LOOP_FAULT` - This means that we apply the full voltage but
 actual current is still near zero. If current sensing circuit is ok this may be
@@ -46,8 +45,8 @@ for the machine speed does not rises uncontrolled.
 ill-conditioned. The observability conditions may not have been met. For
 example there was no speed maneuver during the moment of inertia probing.
 
-`PM_ERROR_INVALID_OPERATION` - Numerical instability inside PMC control code or
-deliberately invalid operation was requested.
+`PM_ERROR_NAN_OPERATION` - Numerical instability inside PMC control code or
+deliberately invalid operation was requested that could result in NaN.
 
 `PM_ERROR_SENSOR_HALL_FAULT` - Forbidden value of Hall code was detected or
 result of Hall self-adjustment shows an inadequacy of sensor measurements.
diff --git a/pgui/config.c b/pgui/config.c
index 7e54141..22f2cdc 100644
--- a/pgui/config.c
+++ b/pgui/config.c
@@ -239,6 +239,6 @@ void config_default(struct config_phobia *fe)
 
 	strcpy(fe->fuzzy, "setpoint");
 
-	fe->regfile = 500;
+	fe->regfile = 490;
 }
 
diff --git a/pgui/phobia.c b/pgui/phobia.c
index ef13890..e62c292 100644
--- a/pgui/phobia.c
+++ b/pgui/phobia.c
@@ -193,7 +193,7 @@ pub_primal_reg(struct public *pub, struct link_reg *reg)
 		"pm.forced_hold_D",
 		"pm.eabi_const_Zq",
 		"pm.const_Zp",
-		"pm.const_ld_S",
+		"pm.const_ld_Sm",
 		"pm.watt_wP_maximal",
 		"pm.watt_wP_reverse",
 		"pm.watt_wA_maximal",
@@ -2698,11 +2698,17 @@ page_diagnose(struct public *pub)
 	reg_float(pub, "pm.scale_iA0", "A sensor drift");
 	reg_float(pub, "pm.scale_iB0", "B sensor drift");
 	reg_float(pub, "pm.scale_iC0", "C sensor drift");
+
+	nk_layout_row_dynamic(ctx, 0, 1);
+	nk_spacer(ctx);
+
 	reg_text_large(pub, "pm.self_BST", "Bootstrap retention");
 	reg_text_large(pub, "pm.self_IST", "Self test result");
 	reg_text_large(pub, "pm.self_STDi", "Current noise STD");
-	reg_text_large(pub, "pm.self_RMSi", "Current sensor RMS");
-	reg_text_large(pub, "pm.self_RMSu", "Voltage sensor RMS");
+	reg_text_large(pub, "pm.self_RMSi", "Current transient RMS");
+	reg_text_large(pub, "pm.self_RMSu", "DC link voltage RMS");
+	reg_text_large(pub, "pm.self_RMSt", "Terminal voltage RMS");
+	reg_text_large(pub, "pm.self_DTu", "DTC voltage RMS");
 
 	nk_layout_row_dynamic(ctx, 0, 1);
 	nk_spacer(ctx);
@@ -2726,10 +2732,8 @@ page_diagnose(struct public *pub)
 	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");
-	reg_float(pub, "pm.tvm_FIR_C_tau", "C voltage FIR tau");
+	reg_float(pub, "pm.dtc_deadband", "DTC deadband time");
+	reg_float(pub, "pm.dtc_tol", "DTC tolerance");
 
 	nk_layout_row_dynamic(ctx, 0, 1);
 	nk_spacer(ctx);
@@ -2883,9 +2887,9 @@ page_probe(struct public *pub)
 
 		nk_spacer(ctx);
 
-		if (nk_menu_item_label(ctx, "DC resistance probing", NK_TEXT_LEFT)) {
+		if (nk_menu_item_label(ctx, "DTC voltage adjusting", NK_TEXT_LEFT)) {
 
-			link_command(lp, "pm_probe_const_resistance");
+			link_command(lp, "pm_adjust_dtc_voltage");
 		}
 
 		if (nk_menu_item_label(ctx, "KV flux linkage", NK_TEXT_LEFT)) {
@@ -2972,11 +2976,11 @@ page_probe(struct public *pub)
 	reg_float(pub, "pm.const_Rs", "Winding resistance");
 	reg_float(pub, "pm.const_Zp", "Rotor pole pairs number");
 	reg_float_um(pub, "pm.const_Ja", "Moment of inertia", 1);
-	reg_float(pub, "pm.const_im_L1", "Inductance D");
-	reg_float(pub, "pm.const_im_L2", "Inductance Q");
-	reg_float(pub, "pm.const_im_B", "Principal angle");
-	reg_float(pub, "pm.const_im_R", "Active impedance");
-	reg_float(pub, "pm.const_ld_S", "Wheel circumference");
+	reg_float(pub, "pm.const_im_Ld", "Inductance D");
+	reg_float(pub, "pm.const_im_Lq", "Inductance Q");
+	reg_float(pub, "pm.const_im_A", "Principal angle");
+	reg_float(pub, "pm.const_im_Rz", "Active impedance");
+	reg_float(pub, "pm.const_ld_Sm", "Wheel circumference");
 
 	reg = link_reg_lookup(lp, "pm.const_Zp");
 
@@ -3165,7 +3169,6 @@ page_hal(struct public *pub)
 	reg_float(pub, "hal.ADC_amplifier_gain", "Current amplifier gain");
 	reg_float(pub, "hal.ADC_voltage_ratio", "DC link voltage ratio");
 	reg_float(pub, "hal.ADC_terminal_ratio", "Terminal voltage ratio");
-	reg_float(pub, "hal.ADC_terminal_bias", "Terminal voltage bias");
 	reg_float(pub, "hal.ADC_knob_ratio", "Knob voltage ratio");
 
 	nk_layout_row_dynamic(ctx, 0, 1);
@@ -3913,6 +3916,7 @@ page_config(struct public *pub)
 
 	reg_enum_combo(pub, "pm.config_VSI_ZERO", "ZERO sequence modulation", 1);
 	reg_enum_toggle(pub, "pm.config_VSI_CLAMP", "Circular voltage clamping");
+	reg_enum_toggle(pub, "pm.config_DTC_VOLTAGE", "DTC voltage compensation");
 
 	nk_layout_row_dynamic(ctx, 0, 1);
 	nk_spacer(ctx);
@@ -3984,13 +3988,12 @@ page_config(struct public *pub)
 	nk_spacer(ctx);
 
 	reg_float(pub, "pm.vsi_gain_LP", "VSI gain LP");
-	reg_enum_combo(pub, "pm.vsi_mask_XF", "VSI channel mask", 1);
 
 	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_clean_zone", "TVM clean zone");
+	reg_float(pub, "pm.dtc_deadband", "DTC deadband time");
+	reg_float(pub, "pm.dtc_tol", "DTC tolerance");
 
 	nk_layout_row_dynamic(ctx, 0, 1);
 	nk_spacer(ctx);
@@ -4606,7 +4609,8 @@ page_wattage(struct public *pub)
 	reg_float(pub, "pm.watt_uDC_tol", "DC regulation tolerance");
 	reg_float(pub, "pm.watt_gain_P", "DC proportional gain");
 	reg_float(pub, "pm.watt_gain_I", "DC integral gain");
-	reg_float(pub, "pm.watt_gain_LP", "Wattage gain LP");
+	reg_float(pub, "pm.watt_gain_LP", "Voltage gain LP");
+	reg_float(pub, "pm.watt_gain_WF", "Wattage gain WF");
 
 	nk_layout_row_dynamic(ctx, 0, 1);
 	nk_spacer(ctx);
@@ -4834,7 +4838,7 @@ page_lp_location(struct public *pub)
 	nk_layout_row_dynamic(ctx, 0, 1);
 	nk_spacer(ctx);
 
-	reg = link_reg_lookup(lp, "pm.const_ld_S");
+	reg = link_reg_lookup(lp, "pm.const_ld_Sm");
 	if (reg != NULL && reg->fval > 0.f) { um_def = 2; }
 
 	reg_float_um(pub, "pm.x_maximal", "Maximal location", um_def);
diff --git a/src/hal/adc.c b/src/hal/adc.c
index a2b330f..575b23b 100644
--- a/src/hal/adc.c
+++ b/src/hal/adc.c
@@ -29,18 +29,18 @@ void irq_ADC()
 		hal.ADC_current_A = (float) ((int) ADC1->JDR1 - 2047) * hal.const_ADC.GA;
 		hal.ADC_current_B = (float) ((int) ADC2->JDR1 - 2047) * hal.const_ADC.GA;
 		hal.ADC_voltage_U = (float) ((int) ADC3->JDR1) * hal.const_ADC.GU;
-		hal.ADC_voltage_A = (float) ((int) ADC1->JDR2) * hal.const_ADC.GT[1] + hal.const_ADC.GT[0];
-		hal.ADC_voltage_B = (float) ((int) ADC2->JDR2) * hal.const_ADC.GT[1] + hal.const_ADC.GT[0];
-		hal.ADC_voltage_C = (float) ((int) ADC3->JDR2) * hal.const_ADC.GT[1] + hal.const_ADC.GT[0];
+		hal.ADC_voltage_A = (float) ((int) ADC1->JDR2) * hal.const_ADC.GT;
+		hal.ADC_voltage_B = (float) ((int) ADC2->JDR2) * hal.const_ADC.GT;
+		hal.ADC_voltage_C = (float) ((int) ADC3->JDR2) * hal.const_ADC.GT;
 #elif (HW_ADC_SAMPLING_SEQUENCE == ADC_SEQUENCE__ABC_UXX)
 #elif (HW_ADC_SAMPLING_SEQUENCE == ADC_SEQUENCE__ABC_UTT_TXX)
 		hal.ADC_current_A = (float) ((int) ADC1->JDR1 - 2047) * hal.const_ADC.GA;
 		hal.ADC_current_B = (float) ((int) ADC2->JDR1 - 2047) * hal.const_ADC.GA;
 		hal.ADC_current_C = (float) ((int) ADC3->JDR1 - 2047) * hal.const_ADC.GA;
 		hal.ADC_voltage_U = (float) ((int) ADC1->JDR2) * hal.const_ADC.GU;
-		hal.ADC_voltage_A = (float) ((int) ADC2->JDR2) * hal.const_ADC.GT[1] + hal.const_ADC.GT[0];
-		hal.ADC_voltage_B = (float) ((int) ADC3->JDR2) * hal.const_ADC.GT[1] + hal.const_ADC.GT[0];
-		hal.ADC_voltage_C = (float) ((int) ADC1->JDR3) * hal.const_ADC.GT[1] + hal.const_ADC.GT[0];
+		hal.ADC_voltage_A = (float) ((int) ADC2->JDR2) * hal.const_ADC.GT;
+		hal.ADC_voltage_B = (float) ((int) ADC3->JDR2) * hal.const_ADC.GT;
+		hal.ADC_voltage_C = (float) ((int) ADC1->JDR3) * hal.const_ADC.GT;
 #elif (HW_ADC_SAMPLING_SEQUENCE == ADC_SEQUENCE__ABC_UTT_TSC)
 #endif /* HW_ADC_SAMPLING_SEQUENCE */
 
@@ -98,8 +98,7 @@ void ADC_const_build()
 
 	hal.const_ADC.GA = U_reference / R_equivalent;
 	hal.const_ADC.GU = U_reference / hal.ADC_voltage_ratio;
-	hal.const_ADC.GT[1] = U_reference / hal.ADC_terminal_ratio;
-	hal.const_ADC.GT[0] = - hal.ADC_terminal_bias / hal.ADC_terminal_ratio;
+	hal.const_ADC.GT = U_reference / hal.ADC_terminal_ratio;
 	hal.const_ADC.GS = hal.ADC_reference_voltage / (float) ADC_RESOLUTION;
 	hal.const_ADC.TS[1] = 80.f / (float) (*TS_110 - *TS_30);
 	hal.const_ADC.TS[0] = 110.f - hal.const_ADC.TS[1] * (float) (*TS_110);
diff --git a/src/hal/entry.c b/src/hal/entry.c
index e184771..273eb2b 100644
--- a/src/hal/entry.c
+++ b/src/hal/entry.c
@@ -40,6 +40,8 @@ void irq_CAN1_RX1() LD_IRQ_WEAK;
 void irq_CAN1_SCE() LD_IRQ_WEAK;
 void irq_TIM1_UP_TIM10() LD_IRQ_WEAK;
 void irq_TIM4() LD_IRQ_WEAK;
+void irq_USART1() LD_IRQ_WEAK;
+void irq_USART2() LD_IRQ_WEAK;
 void irq_USART3() LD_IRQ_WEAK;
 void irq_TIM7() LD_IRQ_WEAK;
 void irq_OTG_FS() LD_IRQ_WEAK;
@@ -109,8 +111,8 @@ LD_VECTORS void *vtab[] = {
 	irq_Default,
 	irq_Default,
 	irq_Default,
-	irq_Default,
-	irq_Default,
+	irq_USART1,
+	irq_USART2,
 	irq_USART3,
 	irq_Default,
 	irq_Default,
diff --git a/src/hal/hal.h b/src/hal/hal.h
index 22b5840..19f4a16 100644
--- a/src/hal/hal.h
+++ b/src/hal/hal.h
@@ -123,7 +123,6 @@ typedef struct {
 	float		ADC_amplifier_gain;
 	float		ADC_voltage_ratio;
 	float		ADC_terminal_ratio;
-	float		ADC_terminal_bias;
 
 	int		ADC_sample_time;
 	int		ADC_sample_advance;
@@ -171,9 +170,9 @@ typedef struct {
 
 		float		GA;
 		float		GU;
-		float		GT[2];
-		float		TS[2];
+		float		GT;
 		float		GS;
+		float		TS[2];
 #ifdef HW_HAVE_ANALOG_KNOB
 		float		GK;
 #endif /* HW_HAVE_ANALOG_KNOB */
diff --git a/src/hal/hw/FSESC_67.h b/src/hal/hw/FSESC_67.h
index c23b437..0685725 100644
--- a/src/hal/hw/FSESC_67.h
+++ b/src/hal/hw/FSESC_67.h
@@ -36,7 +36,6 @@
 
 #define HW_ADC_VOLTAGE_R1		39000.f
 #define HW_ADC_VOLTAGE_R2		2200.f
-#define HW_ADC_VOLTAGE_R3		1000000000000.f		/* have no bias */
 
 #define HW_ADC_KNOB_R1			0.f			/* have no */
 #define HW_ADC_KNOB_R2			10000.f
diff --git a/src/hal/hw/FSESC_67_mini.h b/src/hal/hw/FSESC_67_mini.h
index 05fe26f..79e3aab 100644
--- a/src/hal/hw/FSESC_67_mini.h
+++ b/src/hal/hw/FSESC_67_mini.h
@@ -37,7 +37,6 @@
 
 #define HW_ADC_VOLTAGE_R1		39000.f
 #define HW_ADC_VOLTAGE_R2		2200.f
-#define HW_ADC_VOLTAGE_R3		1000000000000.f		/* have no bias */
 
 #define HW_ADC_KNOB_R1			0.f			/* have no */
 #define HW_ADC_KNOB_R2			10000.f
diff --git a/src/hal/hw/FSESC_75100_v2.h b/src/hal/hw/FSESC_75100_v2.h
index ba77654..9df3c8f 100644
--- a/src/hal/hw/FSESC_75100_v2.h
+++ b/src/hal/hw/FSESC_75100_v2.h
@@ -30,7 +30,6 @@
 
 #define HW_ADC_VOLTAGE_R1		56000.f
 #define HW_ADC_VOLTAGE_R2		2200.f
-#define HW_ADC_VOLTAGE_R3		1000000000000.f		/* have no bias */
 
 #define HW_ADC_KNOB_R1			0.f			/* have no */
 #define HW_ADC_KNOB_R2			10000.f
diff --git a/src/hal/hw/HBRO_foc_mini.h b/src/hal/hw/HBRO_foc_mini.h
index 11afdc7..7da50d8 100644
--- a/src/hal/hw/HBRO_foc_mini.h
+++ b/src/hal/hw/HBRO_foc_mini.h
@@ -28,7 +28,6 @@
 
 #define HW_ADC_VOLTAGE_R1		39000.f
 #define HW_ADC_VOLTAGE_R2		2200.f
-#define HW_ADC_VOLTAGE_R3		1000000000000.f		/* have no bias */
 
 #define HW_ADC_KNOB_R1			0.f			/* have no */
 #define HW_ADC_KNOB_R2			10000.f
diff --git a/src/hal/hw/KLEN_r1.h b/src/hal/hw/KLEN_r1.h
index 1c44026..d1ddfa6 100644
--- a/src/hal/hw/KLEN_r1.h
+++ b/src/hal/hw/KLEN_r1.h
@@ -22,7 +22,6 @@
 
 #define HW_ADC_VOLTAGE_R1		47000.f
 #define HW_ADC_VOLTAGE_R2		2200.f
-#define HW_ADC_VOLTAGE_R3		1000000000000.f		/* have no bias */
 
 #define HW_NTC_PCB_TYPE			NTC_GND
 #define HW_NTC_PCB_BALANCE		10000.f
diff --git a/src/hal/hw/MKESC_60100_v2.h b/src/hal/hw/MKESC_60100_v2.h
index cfc5fa3..a95478b 100644
--- a/src/hal/hw/MKESC_60100_v2.h
+++ b/src/hal/hw/MKESC_60100_v2.h
@@ -36,7 +36,6 @@
 
 #define HW_ADC_VOLTAGE_R1		39000.f
 #define HW_ADC_VOLTAGE_R2		2200.f
-#define HW_ADC_VOLTAGE_R3		1000000000000.f		/* have no bias */
 
 #define HW_ADC_KNOB_R1			0.f			/* have no */
 #define HW_ADC_KNOB_R2			10000.f
diff --git a/src/hal/hw/MKESC_84200.h b/src/hal/hw/MKESC_84200.h
index 00c06f4..774af8c 100644
--- a/src/hal/hw/MKESC_84200.h
+++ b/src/hal/hw/MKESC_84200.h
@@ -30,7 +30,6 @@
 
 #define HW_ADC_VOLTAGE_R1		560000.f
 #define HW_ADC_VOLTAGE_R2		21500.f
-#define HW_ADC_VOLTAGE_R3		1000000000000.f		/* have no bias */
 
 #define HW_ADC_KNOB_R1			0.f			/* have no */
 #define HW_ADC_KNOB_R2			10000.f
diff --git a/src/hal/hw/PESC_r1.h b/src/hal/hw/PESC_r1.h
index 3a5bea5..9481b73 100644
--- a/src/hal/hw/PESC_r1.h
+++ b/src/hal/hw/PESC_r1.h
@@ -27,7 +27,6 @@
 
 #define HW_ADC_VOLTAGE_R1		800000.f
 #define HW_ADC_VOLTAGE_R2		27000.f
-#define HW_ADC_VOLTAGE_R3		470000.f
 
 #define HW_ADC_KNOB_R1			5000.f
 #define HW_ADC_KNOB_R2			10000.f
diff --git a/src/hal/hw/PHOBIA_rev2.h b/src/hal/hw/PHOBIA_rev2.h
index 72a62b9..a47a2fe 100644
--- a/src/hal/hw/PHOBIA_rev2.h
+++ b/src/hal/hw/PHOBIA_rev2.h
@@ -22,7 +22,6 @@
 
 #define HW_ADC_VOLTAGE_R1		470000.f
 #define HW_ADC_VOLTAGE_R2		27000.f
-#define HW_ADC_VOLTAGE_R3		1000000000000.f		/* have no bias */
 
 #define HW_NTC_PCB_TYPE			NTC_GND
 #define HW_NTC_PCB_BALANCE		10000.f
diff --git a/src/hal/hw/PHOBIA_rev3.h b/src/hal/hw/PHOBIA_rev3.h
index 64c5395..fca67d2 100644
--- a/src/hal/hw/PHOBIA_rev3.h
+++ b/src/hal/hw/PHOBIA_rev3.h
@@ -23,7 +23,6 @@
 
 #define HW_ADC_VOLTAGE_R1		470000.f
 #define HW_ADC_VOLTAGE_R2		27000.f
-#define HW_ADC_VOLTAGE_R3		1000000000000.f		/* have no bias */
 
 #define HW_ADC_KNOB_R1			10000.f
 #define HW_ADC_KNOB_R2			10000.f
diff --git a/src/hal/hw/PHOBIA_rev4b.h b/src/hal/hw/PHOBIA_rev4b.h
index b971286..9885fc3 100644
--- a/src/hal/hw/PHOBIA_rev4b.h
+++ b/src/hal/hw/PHOBIA_rev4b.h
@@ -23,7 +23,6 @@
 
 #define HW_ADC_VOLTAGE_R1		470000.f
 #define HW_ADC_VOLTAGE_R2		27000.f
-#define HW_ADC_VOLTAGE_R3		1000000000000.f		/* have no bias */
 
 #define HW_NTC_PCB_TYPE			NTC_GND
 #define HW_NTC_PCB_BALANCE		10000.f
diff --git a/src/hal/hw/PHOBIA_rev4c.h b/src/hal/hw/PHOBIA_rev4c.h
index 3280982..8028c5a 100644
--- a/src/hal/hw/PHOBIA_rev4c.h
+++ b/src/hal/hw/PHOBIA_rev4c.h
@@ -26,7 +26,6 @@
 
 #define HW_ADC_VOLTAGE_R1		470000.f
 #define HW_ADC_VOLTAGE_R2		27000.f
-#define HW_ADC_VOLTAGE_R3		470000.f
 
 #define HW_ADC_KNOB_R1			10000.f
 #define HW_ADC_KNOB_R2			10000.f
diff --git a/src/hal/hw/PHOBIA_rev5.h b/src/hal/hw/PHOBIA_rev5.h
index 25c82b6..2488660 100644
--- a/src/hal/hw/PHOBIA_rev5.h
+++ b/src/hal/hw/PHOBIA_rev5.h
@@ -26,7 +26,6 @@
 
 #define HW_ADC_VOLTAGE_R1		470000.f
 #define HW_ADC_VOLTAGE_R2		27000.f
-#define HW_ADC_VOLTAGE_R3		470000.f
 
 #define HW_ADC_KNOB_R1			10000.f
 #define HW_ADC_KNOB_R2			10000.f
diff --git a/src/hal/hw/RIO_r1.h b/src/hal/hw/RIO_r1.h
new file mode 100644
index 0000000..a527429
--- /dev/null
+++ b/src/hal/hw/RIO_r1.h
@@ -0,0 +1,54 @@
+#define HW_MCU_STM32F405
+
+#define HW_HAVE_LOW_SIDE_SHUNT
+#define HW_HAVE_PWM_THREE_WIRE
+#define HW_HAVE_PWM_GATE_FAULT
+#define HW_HAVE_NTC_ON_PCB
+#define HW_HAVE_NETWORK_EPCAN
+
+#define HW_CLOCK_CRYSTAL_HZ		8000000U
+
+#define HW_PWM_FREQUENCY_HZ		28571.f
+#define HW_PWM_DEADTIME_NS		50.f		/* HL20N06D  */
+
+#define HW_PWM_MINIMAL_PULSE		0.2f
+#define HW_PWM_CLEARANCE_ZONE		5.0f
+#define HW_PWM_SKIP_ZONE		2.0f
+#define HW_PWM_BOOTSTRAP_RETENTION	100.f		/* MP6530 */
+
+#define HW_ADC_SAMPLING_SEQUENCE	ADC_SEQUENCE__ABC_UTT_TXX
+
+#define HW_ADC_REFERENCE_VOLTAGE	3.3f
+#define HW_ADC_SHUNT_RESISTANCE		0.05f
+#define HW_ADC_AMPLIFIER_GAIN		5.0f		/* TSV521A */
+
+#define HW_ADC_VOLTAGE_R1		47000.f
+#define HW_ADC_VOLTAGE_R2		4700.f
+
+#define HW_NTC_PCB_TYPE			NTC_VCC
+#define HW_NTC_PCB_BALANCE		10000.f
+#define HW_NTC_PCB_NTC0			10000.f
+#define HW_NTC_PCB_TA0			25.f
+#define HW_NTC_PCB_BETTA		3434.f		/* NCP15XH103F03RC */
+
+#define GPIO_ADC_CURRENT_A		XGPIO_DEF3('A', 0, 0)
+#define GPIO_ADC_CURRENT_B		XGPIO_DEF3('A', 1, 1)
+#define GPIO_ADC_CURRENT_C		XGPIO_DEF3('A', 2, 2)
+#define GPIO_ADC_VOLTAGE_U		XGPIO_DEF3('B', 1, 9)
+#define GPIO_ADC_VOLTAGE_A		XGPIO_DEF3('C', 1, 11)
+#define GPIO_ADC_VOLTAGE_B		XGPIO_DEF3('C', 3, 13)
+#define GPIO_ADC_VOLTAGE_C		XGPIO_DEF3('C', 2, 12)
+#define GPIO_ADC_NTC_PCB		XGPIO_DEF3('B', 0, 8)
+
+#define GPIO_USART_TX			XGPIO_DEF4('B', 6, 0, 7)
+#define GPIO_USART_RX			XGPIO_DEF4('B', 7, 0, 7)
+
+#define GPIO_CAN_RX			XGPIO_DEF4('B', 8, 0, 9)
+#define GPIO_CAN_TX			XGPIO_DEF4('B', 9, 0, 9)
+
+#define GPIO_GATE_EN			XGPIO_DEF2('A', 11)
+#define GPIO_GATE_FAULT			XGPIO_DEF2('B', 7)
+#define GPIO_LED_ALERT			XGPIO_DEF2('C', 5)	/* have no LED */
+#define GPIO_485_DE			XGPIO_DEF2('A', 3)
+#define GPIO_DCDC_EN			XGPIO_DEF2('C', 15)
+
diff --git a/src/hal/hw/VESC_60_mk6.h b/src/hal/hw/VESC_60_mk6.h
index b9d9a08..1f089d3 100644
--- a/src/hal/hw/VESC_60_mk6.h
+++ b/src/hal/hw/VESC_60_mk6.h
@@ -37,7 +37,6 @@
 
 #define HW_ADC_VOLTAGE_R1		39000.f
 #define HW_ADC_VOLTAGE_R2		2200.f
-#define HW_ADC_VOLTAGE_R3		1000000000000.f		/* have no bias */
 
 #define HW_ADC_KNOB_R1			0.f			/* have no */
 #define HW_ADC_KNOB_R2			10000.f
diff --git a/src/hal/hw/VESC_75_300_r3.h b/src/hal/hw/VESC_75_300_r3.h
index a2c5dc0..0416bd7 100644
--- a/src/hal/hw/VESC_75_300_r3.h
+++ b/src/hal/hw/VESC_75_300_r3.h
@@ -30,7 +30,6 @@
 
 #define HW_ADC_VOLTAGE_R1		56000.f
 #define HW_ADC_VOLTAGE_R2		2200.f
-#define HW_ADC_VOLTAGE_R3		1000000000000.f		/* have no bias */
 
 #define HW_ADC_KNOB_R1			0.f			/* have no */
 #define HW_ADC_KNOB_R2			10000.f
diff --git a/src/hal/hw/ZAEV_r3.h b/src/hal/hw/ZAEV_r3.h
index 99057e9..757842d 100644
--- a/src/hal/hw/ZAEV_r3.h
+++ b/src/hal/hw/ZAEV_r3.h
@@ -24,7 +24,6 @@
 
 #define HW_ADC_VOLTAGE_R1		475000.f
 #define HW_ADC_VOLTAGE_R2		27400.f
-#define HW_ADC_VOLTAGE_R3		475000.f
 
 #define HW_ADC_KNOB_R1			10000.f
 #define HW_ADC_KNOB_R2			10000.f
diff --git a/src/hal/mk/RIO_r1.d b/src/hal/mk/RIO_r1.d
new file mode 100644
index 0000000..de7cd0d
--- /dev/null
+++ b/src/hal/mk/RIO_r1.d
@@ -0,0 +1,3 @@
+HWMCU = STM32F405
+OBJ_NET_EPCAN = 1
+OBJ_HAL_CAN = 1
diff --git a/src/hal/pwm.c b/src/hal/pwm.c
index 7de1015..a93ca7c 100644
--- a/src/hal/pwm.c
+++ b/src/hal/pwm.c
@@ -1,6 +1,10 @@
 #include "hal.h"
 #include "cmsis/stm32xx.h"
 
+#ifdef HW_HAVE_PWM_GATE_FAULT
+static int priv_PWM_fault_CNT;
+#endif /* HW_HAVE_PWM_GATE_FAULT */
+
 void irq_TIM1_UP_TIM10() { }
 
 static int
@@ -62,12 +66,23 @@ void PWM_startup()
 
 	/* Enable TIM1 pins.
 	 * */
+#ifdef HW_HAVE_PWM_THREE_WIRE
+	GPIO_set_mode_OUTPUT(GPIO_TIM1_CH1N);
+	GPIO_set_mode_OUTPUT(GPIO_TIM1_CH2N);
+	GPIO_set_mode_OUTPUT(GPIO_TIM1_CH3N);
+
+	GPIO_set_mode_FUNCTION(GPIO_TIM1_CH1);
+	GPIO_set_mode_FUNCTION(GPIO_TIM1_CH2);
+	GPIO_set_mode_FUNCTION(GPIO_TIM1_CH3);
+#else /* HW_HAVE_PWM_THREE_WIRE */
+
 	GPIO_set_mode_FUNCTION(GPIO_TIM1_CH1N);
 	GPIO_set_mode_FUNCTION(GPIO_TIM1_CH2N);
 	GPIO_set_mode_FUNCTION(GPIO_TIM1_CH3N);
 	GPIO_set_mode_FUNCTION(GPIO_TIM1_CH1);
 	GPIO_set_mode_FUNCTION(GPIO_TIM1_CH2);
 	GPIO_set_mode_FUNCTION(GPIO_TIM1_CH3);
+#endif
 
 	GPIO_set_mode_SPEED_HIGH(GPIO_TIM1_CH1N);
 	GPIO_set_mode_SPEED_HIGH(GPIO_TIM1_CH2N);
@@ -116,17 +131,35 @@ void PWM_set_Z(int Z)
 	if (Z & LEG_A) {
 #endif
 
+#ifdef HW_HAVE_PWM_THREE_WIRE
+		GPIO_set_LOW(GPIO_TIM1_CH1N);
+#endif /* HW_HAVE_PWM_THREE_WIRE */
+
 		TIM1->CCER &= ~(TIM_CCER_CC1NE | TIM_CCER_CC1E);
 	}
 	else {
+
+#ifdef HW_HAVE_PWM_THREE_WIRE
+		GPIO_set_HIGH(GPIO_TIM1_CH1N);
+#endif /* HW_HAVE_PWM_THREE_WIRE */
+
 		TIM1->CCER |= (TIM_CCER_CC1NE | TIM_CCER_CC1E);
 	}
 
 	if (Z & LEG_B) {
 
+#ifdef HW_HAVE_PWM_THREE_WIRE
+		GPIO_set_LOW(GPIO_TIM1_CH2N);
+#endif /* HW_HAVE_PWM_THREE_WIRE */
+
 		TIM1->CCER &= ~(TIM_CCER_CC2NE | TIM_CCER_CC2E);
 	}
 	else {
+
+#ifdef HW_HAVE_PWM_THREE_WIRE
+		GPIO_set_HIGH(GPIO_TIM1_CH2N);
+#endif /* HW_HAVE_PWM_THREE_WIRE */
+
 		TIM1->CCER |= (TIM_CCER_CC2NE | TIM_CCER_CC2E);
 	}
 
@@ -135,9 +168,19 @@ void PWM_set_Z(int Z)
 #else /* HW_HAVE_PWM_REVERSED */
 	if (Z & LEG_C) {
 #endif
+
+#ifdef HW_HAVE_PWM_THREE_WIRE
+		GPIO_set_LOW(GPIO_TIM1_CH3N);
+#endif /* HW_HAVE_PWM_THREE_WIRE */
+
 		TIM1->CCER &= ~(TIM_CCER_CC3NE | TIM_CCER_CC3E);
 	}
 	else {
+
+#ifdef HW_HAVE_PWM_THREE_WIRE
+		GPIO_set_HIGH(GPIO_TIM1_CH3N);
+#endif /* HW_HAVE_PWM_THREE_WIRE */
+
 		TIM1->CCER |= (TIM_CCER_CC3NE | TIM_CCER_CC3E);
 	}
 
@@ -146,12 +189,30 @@ void PWM_set_Z(int Z)
 
 int PWM_fault()
 {
+#ifdef HW_HAVE_PWM_GATE_FAULT
+	if (unlikely(priv_PWM_fault_CNT >= 20)) {
+
+		return HAL_FAULT;
+	}
+	else {
+		if (likely(GPIO_get_STATE(GPIO_GATE_FAULT) != 0)) {
+
+			priv_PWM_fault_CNT = 0;
+		}
+		else {
+			priv_PWM_fault_CNT += 1;
+		}
+	}
+#endif /* HW_HAVE_PWM_GATE_FAULT */
+
+#ifdef HW_HAVE_PWM_BREAK
 	if (TIM1->SR & TIM_SR_BIF) {
 
 		TIM1->SR &= ~TIM_SR_BIF;
 
 		return HAL_FAULT;
 	}
+#endif /* HW_HAVE_PWM_BREAK */
 
 	return HAL_OK;
 }
diff --git a/src/hal/pwm.h b/src/hal/pwm.h
index 7b8e1b9..b82861c 100644
--- a/src/hal/pwm.h
+++ b/src/hal/pwm.h
@@ -4,9 +4,11 @@
 #define GPIO_TIM1_CH1N			XGPIO_DEF4('B', 13, 0, 1)
 #define GPIO_TIM1_CH2N			XGPIO_DEF4('B', 14, 0, 1)
 #define GPIO_TIM1_CH3N			XGPIO_DEF4('B', 15, 0, 1)
+
 #define GPIO_TIM1_CH1			XGPIO_DEF4('A', 8, 0, 1)
 #define GPIO_TIM1_CH2			XGPIO_DEF4('A', 9, 0, 1)
 #define GPIO_TIM1_CH3			XGPIO_DEF4('A', 10, 0, 1)
+
 #define GPIO_TIM1_BKIN			XGPIO_DEF4('B', 12, 0, 1)
 
 void PWM_startup();
diff --git a/src/hal/usart.c b/src/hal/usart.c
index da9efa4..5925e92 100644
--- a/src/hal/usart.c
+++ b/src/hal/usart.c
@@ -3,8 +3,19 @@
 #include "freertos/FreeRTOS.h"
 #include "cmsis/stm32xx.h"
 
+#define _HW_HAVE_USART1		  ((GPIO_USART_TX & 0x7FU) == XGPIO_DEF2('A', 9)  \
+				|| (GPIO_USART_TX & 0x7FU) == XGPIO_DEF2('B', 6))
+
+#define _HW_HAVE_USART2		  ((GPIO_USART_TX & 0x7FU) == XGPIO_DEF2('A', 2)  \
+				|| (GPIO_USART_TX & 0x7FU) == XGPIO_DEF2('D', 5))
+
+#define _HW_HAVE_USART3		  ((GPIO_USART_TX & 0x7FU) == XGPIO_DEF2('B', 10)  \
+				|| (GPIO_USART_TX & 0x7FU) == XGPIO_DEF2('C', 10))
+
 typedef struct {
 
+	USART_TypeDef		*BASE;
+
 	QueueHandle_t		rx_queue;
 	QueueHandle_t		tx_queue;
 }
@@ -12,16 +23,17 @@ priv_USART_t;
 
 static priv_USART_t		priv_USART;
 
-void irq_USART3()
+static void
+irq_USART(USART_TypeDef *USART)
 {
 	BaseType_t		xWoken = pdFALSE;
 	uint32_t		SR;
 	char			xbyte;
 
 #if defined(STM32F4)
-	SR = USART3->SR;
+	SR = USART->SR;
 #elif defined(STM32F7)
-	SR = USART3->ISR;
+	SR = USART->ISR;
 #endif /* STM32Fx */
 
 #if defined(STM32F4)
@@ -31,9 +43,9 @@ void irq_USART3()
 #endif /* STM32Fx */
 
 #if defined(STM32F4)
-		xbyte = USART3->DR;
+		xbyte = USART->DR;
 #elif defined(STM32F7)
-		xbyte = USART3->RDR;
+		xbyte = USART->RDR;
 #endif /* STM32Fx */
 
 		xQueueSendToBackFromISR(priv_USART.rx_queue, &xbyte, &xWoken);
@@ -50,30 +62,59 @@ void irq_USART3()
 		if (xQueueReceiveFromISR(priv_USART.tx_queue, &xbyte, &xWoken) == pdTRUE) {
 
 #if defined(STM32F4)
-			USART3->DR = xbyte;
+			USART->DR = xbyte;
 #elif defined(STM32F7)
-			USART3->TDR = xbyte;
+			USART->TDR = xbyte;
 #endif /* STM32Fx */
 
 		}
 		else {
-			USART3->CR1 &= ~USART_CR1_TXEIE;
+			USART->CR1 &= ~USART_CR1_TXEIE;
 		}
 	}
 
 	portYIELD_FROM_ISR(xWoken);
 }
 
+void irq_USART1() { irq_USART(USART1); }
+void irq_USART2() { irq_USART(USART2); }
+void irq_USART3() { irq_USART(USART3); }
+
 void USART_startup()
 {
-	/* Enable USART3 clock.
+	if (_HW_HAVE_USART1) {
+
+		priv_USART.BASE = USART1;
+	}
+	else if (_HW_HAVE_USART2) {
+
+		priv_USART.BASE = USART2;
+	}
+	else if (_HW_HAVE_USART3) {
+
+		priv_USART.BASE = USART3;
+	}
+
+	/* Enable USART clock.
 	 * */
-	RCC->APB1ENR |= RCC_APB1ENR_USART3EN;
+	if (_HW_HAVE_USART1) {
 
-	/* Enable USART3 pins.
+		RCC->APB2ENR |= RCC_APB2ENR_USART1EN;
+	}
+	else if (_HW_HAVE_USART2) {
+
+		RCC->APB1ENR |= RCC_APB1ENR_USART2EN;
+	}
+	else if (_HW_HAVE_USART3) {
+
+		RCC->APB1ENR |= RCC_APB1ENR_USART3EN;
+	}
+
+	/* Enable USART pins.
 	 * */
 	GPIO_set_mode_FUNCTION(GPIO_USART_TX);
 	GPIO_set_mode_FUNCTION(GPIO_USART_RX);
+
 	GPIO_set_mode_PULL_UP(GPIO_USART_RX);
 
 	/* Alloc queues.
@@ -83,48 +124,68 @@ void USART_startup()
 
 	/* Configure USART.
 	 * */
-	USART3->BRR = CLOCK_APB1_HZ / hal.USART_baudrate;
+	if (_HW_HAVE_USART1) {
+
+		priv_USART.BASE->BRR = CLOCK_APB2_HZ / hal.USART_baudrate;
+	}
+	else if (_HW_HAVE_USART2 || _HW_HAVE_USART3) {
+
+		priv_USART.BASE->BRR = CLOCK_APB1_HZ / hal.USART_baudrate;
+	}
 
 #if defined(STM32F4)
 	if (hal.USART_parity == PARITY_EVEN) {
 
-		USART3->CR1 = USART_CR1_UE | USART_CR1_M | USART_CR1_PCE
+		priv_USART.BASE->CR1 = USART_CR1_UE | USART_CR1_M | USART_CR1_PCE
 			| USART_CR1_RXNEIE | USART_CR1_TE | USART_CR1_RE;
 	}
 	else if (hal.USART_parity == PARITY_ODD) {
 
-		USART3->CR1 = USART_CR1_UE | USART_CR1_M | USART_CR1_PCE
+		priv_USART.BASE->CR1 = USART_CR1_UE | USART_CR1_M | USART_CR1_PCE
 			| USART_CR1_PS | USART_CR1_RXNEIE | USART_CR1_TE | USART_CR1_RE;
 	}
 	else {
-		USART3->CR1 = USART_CR1_UE | USART_CR1_RXNEIE
+		priv_USART.BASE->CR1 = USART_CR1_UE | USART_CR1_RXNEIE
 			| USART_CR1_TE | USART_CR1_RE;
 	}
 
 #elif defined(STM32F7)
 	if (hal.USART_parity == PARITY_EVEN) {
 
-		USART3->CR1 = USART_CR1_UE | USART_CR1_M0 | USART_CR1_PCE
+		priv_USART.BASE->CR1 = USART_CR1_UE | USART_CR1_M0 | USART_CR1_PCE
 			| USART_CR1_RXNEIE | USART_CR1_TE | USART_CR1_RE;
 	}
 	else if (hal.USART_parity == PARITY_ODD) {
 
-		USART3->CR1 = USART_CR1_UE | USART_CR1_M0 | USART_CR1_PCE
+		priv_USART.BASE->CR1 = USART_CR1_UE | USART_CR1_M0 | USART_CR1_PCE
 			| USART_CR1_PS | USART_CR1_RXNEIE | USART_CR1_TE | USART_CR1_RE;
 	}
 	else {
-		USART3->CR1 = USART_CR1_UE | USART_CR1_RXNEIE
+		priv_USART.BASE->CR1 = USART_CR1_UE | USART_CR1_RXNEIE
 			| USART_CR1_TE | USART_CR1_RE;
 	}
 #endif /* STM32Fx */
 
-	USART3->CR2 = 0;
-	USART3->CR3 = 0;
+	priv_USART.BASE->CR2 = 0;
+	priv_USART.BASE->CR3 = 0;
 
 	/* Enable IRQ.
 	 * */
-	NVIC_SetPriority(USART3_IRQn, 11);
-	NVIC_EnableIRQ(USART3_IRQn);
+	if (_HW_HAVE_USART1) {
+
+		NVIC_SetPriority(USART1_IRQn, 11);
+		NVIC_EnableIRQ(USART1_IRQn);
+	}
+	else if (_HW_HAVE_USART2) {
+
+		NVIC_SetPriority(USART2_IRQn, 11);
+		NVIC_EnableIRQ(USART2_IRQn);
+	}
+	else if (_HW_HAVE_USART3) {
+
+		NVIC_SetPriority(USART3_IRQn, 11);
+		NVIC_EnableIRQ(USART3_IRQn);
+	}
 }
 
 int USART_getc()
@@ -149,7 +210,7 @@ void USART_putc(int c)
 
 	xQueueSendToBack(priv_USART.tx_queue, &xbyte, portMAX_DELAY);
 
-	USART3->CR1 |= USART_CR1_TXEIE;
+	priv_USART.BASE->CR1 |= USART_CR1_TXEIE;
 
 	GPIO_set_LOW(GPIO_LED_ALERT);
 }
diff --git a/src/main.c b/src/main.c
index 4e4e24b..0e057a0 100644
--- a/src/main.c
+++ b/src/main.c
@@ -535,7 +535,7 @@ LD_TASK void task_KNOB(void *pData)
 static void
 default_flash_load()
 {
-	float			volt_D, halt_I, halt_U;
+	float			halt_I, halt_U;
 
 	hal.USART_baudrate = 57600;
 	hal.USART_parity = PARITY_EVEN;
@@ -547,13 +547,15 @@ default_flash_load()
 	hal.ADC_amplifier_gain = HW_ADC_AMPLIFIER_GAIN;
 	hal.ADC_voltage_ratio = HW_ADC_VOLTAGE_R2 / (HW_ADC_VOLTAGE_R1 + HW_ADC_VOLTAGE_R2);
 
-	volt_D =  HW_ADC_VOLTAGE_R1 * HW_ADC_VOLTAGE_R2
-		+ HW_ADC_VOLTAGE_R2 * HW_ADC_VOLTAGE_R3
-		+ HW_ADC_VOLTAGE_R1 * HW_ADC_VOLTAGE_R3;
+#ifndef HW_ADC_TERMINAL_R1
+#define HW_ADC_TERMINAL_R1	HW_ADC_VOLTAGE_R1
+#endif /* HW_ADC_TERMINAL_R1 */
 
-	hal.ADC_terminal_ratio = HW_ADC_VOLTAGE_R2 * HW_ADC_VOLTAGE_R3 / volt_D;
-	hal.ADC_terminal_bias = HW_ADC_VOLTAGE_R1 * HW_ADC_VOLTAGE_R2
-				* hal.ADC_reference_voltage / volt_D;
+#ifndef HW_ADC_TERMINAL_R2
+#define HW_ADC_TERMINAL_R2	HW_ADC_VOLTAGE_R2
+#endif /* HW_ADC_TERMINAL_R2 */
+
+	hal.ADC_terminal_ratio = HW_ADC_TERMINAL_R2 / (HW_ADC_TERMINAL_R1 + HW_ADC_TERMINAL_R2);
 
 #ifdef HW_HAVE_ANALOG_KNOB
 	hal.ADC_knob_ratio = HW_ADC_KNOB_R2 / (HW_ADC_KNOB_R1 + HW_ADC_KNOB_R2);
@@ -726,6 +728,8 @@ default_flash_load()
 	pm.fault_current_halt = (float) (int) (.95f * halt_I);
 	pm.fault_voltage_halt = (float) (int) (.95f * halt_U);
 
+	pm.dtc_deadband = HW_PWM_DEADTIME_NS;
+
 	pm_auto(&pm, PM_AUTO_MAXIMAL_CURRENT);
 
 	pm.watt_wA_maximal = (float) (int) (0.66666667f * pm.i_maximal);
@@ -1086,13 +1090,11 @@ void ADC_IRQ()
 			pm.fsm_req = PM_STATE_HALT;
 		}
 
-#ifdef HW_HAVE_PWM_BREAK
 		if (unlikely(PWM_fault() != HAL_OK)) {
 
 			pm.fsm_errno = PM_ERROR_HW_EMERGENCY_STOP;
 			pm.fsm_req = PM_STATE_HALT;
 		}
-#endif /* HW_HAVE_PWM_BREAK */
 
 #ifdef HW_HAVE_DRV_ON_PCB
 		if (unlikely(DRV_fault() != HAL_OK)) {
diff --git a/src/phobia/libm.c b/src/phobia/libm.c
index 1a45fb0..2da81fa 100644
--- a/src/phobia/libm.c
+++ b/src/phobia/libm.c
@@ -59,7 +59,7 @@ void m_normalizef(float x[2])
 {
 	float		l;
 
-	/* Approximate normalize the vector \x.
+	/* Approximate normalization of the vector \x.
 	 * */
 
 	l = x[0] * x[0] + x[1] * x[1];
@@ -345,7 +345,7 @@ m_lf_lcgu(uint32_t rseed)
 	return rseed * 17317U + 1U;
 }
 
-void m_lf_randseed(m_seed_t *lf, int seed)
+void m_lf_randseed(lfseed_t *lf, int seed)
 {
 	uint32_t	lcgu;
 
@@ -359,7 +359,7 @@ void m_lf_randseed(m_seed_t *lf, int seed)
 	lf->nb = 0;
 }
 
-float m_lf_urandf(m_seed_t *lf)
+float m_lf_urandf(lfseed_t *lf)
 {
 	float		y, a, b;
 	int		n, k;
@@ -381,7 +381,7 @@ float m_lf_urandf(m_seed_t *lf)
 	return y;
 }
 
-float m_lf_gaussf(m_seed_t *lf)
+float m_lf_gaussf(lfseed_t *lf)
 {
 	float		x;
 
diff --git a/src/phobia/libm.h b/src/phobia/libm.h
index 6395881..3504a5b 100644
--- a/src/phobia/libm.h
+++ b/src/phobia/libm.h
@@ -45,11 +45,11 @@ typedef struct {
 	float		seed[4];
 	int		nb;
 }
-m_seed_t;
+lfseed_t;
 
-void m_lf_randseed(m_seed_t *lf, int seed);
-float m_lf_urandf(m_seed_t *lf);
-float m_lf_gaussf(m_seed_t *lf);
+void m_lf_randseed(lfseed_t *lf, int seed);
+float m_lf_urandf(lfseed_t *lf);
+float m_lf_gaussf(lfseed_t *lf);
 
 #endif /* _H_LIB_M_ */
 
diff --git a/src/phobia/lse.h b/src/phobia/lse.h
index d96bf2b..a05a1b8 100644
--- a/src/phobia/lse.h
+++ b/src/phobia/lse.h
@@ -4,7 +4,7 @@
 /* Define the maximal full size to be allocated. This is the sum of \x and \z
  * row-vector sizes.
  * */
-#define LSE_FULL_MAX			10
+#define LSE_FULL_MAX			8
 
 /* Define the maximal number of cascades. A large value gives greater precision
  * on large datasets but consumes more memory. Reasonable values are from 2 to 4.
diff --git a/src/phobia/pm.c b/src/phobia/pm.c
index 60fa7dc..7c653cd 100644
--- a/src/phobia/pm.c
+++ b/src/phobia/pm.c
@@ -32,16 +32,33 @@ void pm_quick_build(pmc_t *pm)
 		pm->flux_LINKAGE = PM_ENABLED;
 	}
 
-	if (pm->const_im_L1 > M_EPSILON) {
+	if (		   pm->const_im_Ld > M_EPSILON
+			&& pm->const_im_Lq > M_EPSILON) {
 
-		pm->quick_iL1 = 1.f / pm->const_im_L1;
-		pm->quick_TiL1 = pm->m_dT * pm->quick_iL1;
-	}
+		float		Ld = pm->const_im_Ld;
+		float		Lq = pm->const_im_Lq;
+
+		float		iLd = 1.f / Ld;
+		float		iLq = 1.f / Lq;
+
+		pm->quick_iLd = iLd;
+		pm->quick_iLq = iLq;
 
-	if (pm->const_im_L2 > M_EPSILON) {
+		pm->quick_TiLd = pm->m_dT * iLd;
+		pm->quick_TiLq = pm->m_dT * iLq;
 
-		pm->quick_iL2 = 1.f / pm->const_im_L2;
-		pm->quick_TiL2 = pm->m_dT * pm->quick_iL2;
+		pm->quick_TiLu[0] = pm->m_dT * (iLd - iLq);
+		pm->quick_TiLu[1] = pm->m_dT * (Ld * iLq - Lq * iLd);
+		pm->quick_TiLu[2] = pm->m_dT * (Ld * iLq - 1.f);
+		pm->quick_TiLu[3] = pm->m_dT * (1.f - Lq * iLd);
+
+		if (m_fabsf(Ld - Lq) > M_EPSILON) {
+
+			pm->quick_WiL4 = pm->const_lambda / ((Ld - Lq) * 4.f);
+		}
+		else {
+			pm->quick_WiL4 = PM_MAX_F;
+		}
 	}
 
 	pm->quick_HFwS = M_2_PI_F * pm->hfi_freq;
@@ -62,17 +79,6 @@ void pm_quick_build(pmc_t *pm)
 
 		pm->quick_ZiSQ = Zf / Zq;
 	}
-
-	if (		   pm->const_im_L1 > M_EPSILON
-			&& pm->const_im_L2 > M_EPSILON) {
-
-		float		rel = (pm->const_im_L1 - pm->const_im_L2) * 4.f;
-
-		if (m_fabsf(rel) > M_EPSILON) {
-
-			pm->quick_WiL4 = pm->const_lambda / rel;
-		}
-	}
 }
 
 static void
@@ -88,17 +94,16 @@ pm_auto_basic_default(pmc_t *pm)
 	pm->config_TVM = PM_ENABLED;
 	pm->config_DBG = PM_DISABLED;
 
-	pm->fault_voltage_tol = 4.f;		/* (V) */
-	pm->fault_current_tol = 4.f;		/* (A) */
+	pm->fault_voltage_tol = 5.f;		/* (V) */
+	pm->fault_current_tol = 5.f;		/* (A) */
 	pm->fault_accuracy_tol = 0.10f;		/*     */
-	pm->fault_terminal_tol = 0.090f;	/* (V) */
+	pm->fault_terminal_tol = 0.10f;		/* (V) */
 	pm->fault_current_halt = 156.f;		/* (A) */
 	pm->fault_voltage_halt = 57.f;		/* (V) */
 
 	pm->vsi_AF = 1;				/* NOTE: Disable all flags until  */
 	pm->vsi_BF = 1;				/* clearance zone is calculated.  */
 	pm->vsi_CF = 1;
-	pm->vsi_SF = 1;
 	pm->vsi_UF = 1;
 
 	pm->watt_wP_maximal = 4000.f;		/* (Watt) */
@@ -111,7 +116,7 @@ pm_auto_basic_default(pmc_t *pm)
 	pm->i_maximal = 120.f;			/* (A) */
 	pm->i_reverse = pm->i_maximal;
 
-	m_lf_randseed(&pm->hfi_seed, 75);	/* NOTE: lfg initial random seed. */
+	m_lf_randseed(&pm->lfseed, 80);		/* NOTE: lfg initial random seed. */
 }
 
 static void
@@ -119,9 +124,10 @@ pm_auto_config_default(pmc_t *pm)
 {
 	pm->config_VSI_ZERO = PM_VSI_GND;
 	pm->config_VSI_CLAMP = PM_DISABLED;
+	pm->config_DTC_VOLTAGE = PM_ENABLED;
 	pm->config_LU_FORCED = PM_ENABLED;
 	pm->config_LU_FREEWHEEL = PM_ENABLED;
-	pm->config_LU_ESTIMATE = PM_FLUX_ORTEGA;
+	pm->config_LU_ESTIMATE = PM_FLUX_KALMAN;
 	pm->config_LU_SENSOR = PM_SENSOR_NONE;
 	pm->config_LU_LOCATION = PM_LOCATION_NONE;
 	pm->config_LU_DRIVE = PM_DRIVE_SPEED;
@@ -131,20 +137,20 @@ pm_auto_config_default(pmc_t *pm)
 	pm->config_SALIENCY = PM_SALIENCY_NEGATIVE;
 	pm->config_RELUCTANCE = PM_DISABLED;
 	pm->config_WEAKENING = PM_DISABLED;
-	pm->config_REVERSE_BRAKE = PM_DISABLED;
+	pm->config_REVERSE_BRAKE = PM_ENABLED;
 	pm->config_SPEED_MAXIMAL = PM_ENABLED;
 	pm->config_EABI_FRONTEND = PM_EABI_INCREMENTAL;
 	pm->config_SINCOS_FRONTEND = PM_SINCOS_ANALOG;
 
 	pm->tm_transient_slow = 40.f;		/* (ms) */
-	pm->tm_transient_fast = 2.f;		/* (ms) */
+	pm->tm_transient_fast = 4.f;		/* (ms) */
 	pm->tm_voltage_hold = 100.f;		/* (ms) */
-	pm->tm_current_hold = 300.f;		/* (ms) */
-	pm->tm_current_ramp = 400.f;		/* (ms) */
-	pm->tm_instant_probe = 2.f;		/* (ms) */
+	pm->tm_current_hold = 200.f;		/* (ms) */
+	pm->tm_current_ramp = 900.f;		/* (ms) */
+	pm->tm_instant_probe = 4.f;		/* (ms) */
 	pm->tm_average_probe = 200.f;		/* (ms) */
 	pm->tm_average_drift = 100.f;		/* (ms) */
-	pm->tm_average_inertia = 700.f;		/* (ms) */
+	pm->tm_average_inertia = 900.f;		/* (ms) */
 	pm->tm_pause_startup = 100.f;		/* (ms) */
 	pm->tm_pause_forced = 1000.f;		/* (ms) */
 	pm->tm_pause_halt = 2000.f;		/* (ms) */
@@ -167,30 +173,20 @@ pm_auto_config_default(pmc_t *pm)
 	pm->probe_current_hold = 20.f;		/* (A) */
 	pm->probe_weak_level = 0.5f;
 	pm->probe_hold_angle = 0.f;
-	pm->probe_current_sine = 5.f;		/* (A) */
+	pm->probe_current_sine = 10.f;		/* (A) */
 	pm->probe_current_bias = 0.f;		/* (A) */
 	pm->probe_freq_sine = 1100.f;		/* (Hz) */
 	pm->probe_speed_hold = 900.f;		/* (rad/s) */
 	pm->probe_speed_tol = 50.f;		/* (rad/s) */
 	pm->probe_location_tol = 0.10f;		/* (rad) */
 	pm->probe_loss_maximal = 400.f;		/* (Watt) */
-	pm->probe_gain_P = 1.E-2f;
-	pm->probe_gain_I = 1.E-3f;
+	pm->probe_gain_P = 5.E-2f;
+	pm->probe_gain_I = 5.E-4f;
 
 	pm->vsi_gain_LP = 5.E-3f;
-	pm->vsi_mask_XF = PM_MASK_NONE;
-
-	pm->tvm_ACTIVE = PM_DISABLED;
-	pm->tvm_clean_zone = 0.10f;
-	pm->tvm_FIR_A[0] = 0.f;
-	pm->tvm_FIR_A[1] = 0.f;
-	pm->tvm_FIR_A[2] = 0.f;
-	pm->tvm_FIR_B[0] = 0.f;
-	pm->tvm_FIR_B[1] = 0.f;
-	pm->tvm_FIR_B[2] = 0.f;
-	pm->tvm_FIR_C[0] = 0.f;
-	pm->tvm_FIR_C[1] = 0.f;
-	pm->tvm_FIR_C[2] = 0.f;
+
+	pm->dtc_deadband = 100.f;		/* (ns) */
+	pm->dtc_tol = 0.5f;			/* (A) */
 
 	pm->lu_transient = 400.f;		/* (rad/s) */
 	pm->lu_gain_mq_LP = 5.E-4f;
@@ -215,11 +211,10 @@ pm_auto_config_default(pmc_t *pm)
 	pm->flux_gain_SF = 5.E-2f;
 	pm->flux_gain_IF = 0.1f;
 
-	pm->kalman_gain_Q[0] = 5.E-2f;
-	pm->kalman_gain_Q[1] = 5.E-2f;
-	pm->kalman_gain_Q[2] = 5.E-4f;
-	pm->kalman_gain_Q[3] = 7.E+1f;
-	pm->kalman_gain_Q[4] = 5.E-5f;
+	pm->kalman_gain_Q[0] = 5.E-1f;
+	pm->kalman_gain_Q[1] = 2.E+0f;
+	pm->kalman_gain_Q[2] = 2.E+6f;
+	pm->kalman_gain_Q[3] = 2.E-1f;
 	pm->kalman_gain_R = 5.E-1f;
 
 	pm->zone_noise = 50.f;			/* (rad/s) */
@@ -233,7 +228,7 @@ pm_auto_config_default(pmc_t *pm)
 	pm->hall_trip_tol = 200.f;		/* (rad/s) */
 	pm->hall_gain_LO = 5.E-4f;
 	pm->hall_gain_SF = 7.E-3f;
-	pm->hall_gain_IF = 0.5f;
+	pm->hall_gain_IF = 0.9f;
 
 	pm->eabi_const_EP = 2400;
 	pm->eabi_const_Zs = 1;
@@ -250,15 +245,16 @@ pm_auto_config_default(pmc_t *pm)
 	pm->const_Rs = 0.f;			/* (Ohm) */
 	pm->const_Zp = 1;
 	pm->const_Ja = 0.f;
-	pm->const_im_L1 = 0.f;			/* (H) */
-	pm->const_im_L2 = 0.f;			/* (H) */
-	pm->const_im_B = 0.f;
-	pm->const_im_R = 0.f;
+	pm->const_im_Ld = 0.f;			/* (H) */
+	pm->const_im_Lq = 0.f;			/* (H) */
+	pm->const_im_A = 0.f;
+	pm->const_im_Rz = 0.f;
 
 	pm->watt_uDC_tol = 4.f;			/* (V) */
 	pm->watt_gain_P = 5.E+1f;
 	pm->watt_gain_I = 5.E-1f;
-	pm->watt_gain_LP = 5.E-2f;
+	pm->watt_gain_LP = 5.E-3f;
+	pm->watt_gain_WF = 5.E-2f;
 
 	pm->i_maximal_on_HFI = 10.f;		/* (A) */
 	pm->i_slew_rate = 10000.f;		/* (A/s) */
@@ -311,10 +307,10 @@ pm_auto_machine_default(pmc_t *pm)
 	pm->const_lambda = 0.f;
 	pm->const_Rs = 0.f;
 	pm->const_Ja = 0.f;
-	pm->const_im_L1 = 0.f;
-	pm->const_im_L2 = 0.f;
-	pm->const_im_B = 0.f;
-	pm->const_im_R = 0.f;
+	pm->const_im_Ld = 0.f;
+	pm->const_im_Lq = 0.f;
+	pm->const_im_A = 0.f;
+	pm->const_im_Rz = 0.f;
 
 	pm->i_slew_rate = 10000.f;
 	pm->i_gain_P = 2.E-1f;
@@ -342,17 +338,6 @@ pm_auto_scale_default(pmc_t *pm)
 	pm->scale_uB[1] = 1.f;
 	pm->scale_uC[0] = 0.f;
 	pm->scale_uC[1] = 1.f;
-
-	pm->tvm_ACTIVE = PM_DISABLED;
-	pm->tvm_FIR_A[0] = 0.f;
-	pm->tvm_FIR_A[1] = 0.f;
-	pm->tvm_FIR_A[2] = 0.f;
-	pm->tvm_FIR_B[0] = 0.f;
-	pm->tvm_FIR_B[1] = 0.f;
-	pm->tvm_FIR_B[2] = 0.f;
-	pm->tvm_FIR_C[0] = 0.f;
-	pm->tvm_FIR_C[1] = 0.f;
-	pm->tvm_FIR_C[2] = 0.f;
 }
 
 static void
@@ -492,19 +477,18 @@ pm_auto_zone_threshold(pmc_t *pm)
 
 		/* Based on uncertainty due to resistance thermal drift.
 		 * */
-		thld_IRU = 0.2f * pm->i_maximal * pm->const_Rs;
+		thld_IRU = 0.2f * pm->const_Rs * pm->i_maximal;
 
-		if (		PM_CONFIG_TVM(pm) == PM_ENABLED
-				&& pm->tvm_ACTIVE == PM_ENABLED) {
+		if (pm->config_DTC_VOLTAGE == PM_ENABLED) {
 
-			/* Based on TVM terminal accuracy.
+			/* Based on DT compensation accuracy.
 			 * */
 			thld_IRU += pm->fault_terminal_tol;
 		}
 		else {
-			/* Based on voltage uncertainty.
+			/* Based on voltage uncertainty on DT.
 			 * */
-			thld_IRU += pm->dc_minimal * (1.f / 1000000.f)
+			thld_IRU += pm->dtc_deadband * 1.E-9f
 				* pm->m_freq * pm->const_fb_U;
 		}
 
@@ -533,11 +517,11 @@ pm_auto_loop_current(pmc_t *pm)
 {
 	float		Lmin, Df, Kp, Ki;
 
-	if (		   pm->const_im_L1 > M_EPSILON
-			&& pm->const_im_L1 > M_EPSILON) {
+	if (		   pm->const_im_Ld > M_EPSILON
+			&& pm->const_im_Lq > M_EPSILON) {
 
-		Lmin = (pm->const_im_L1 < pm->const_im_L2)
-			? pm->const_im_L1 : pm->const_im_L2;
+		Lmin = (pm->const_im_Ld < pm->const_im_Lq)
+			? pm->const_im_Ld : pm->const_im_Lq;
 
 		Df = pm->i_damping;
 
@@ -570,7 +554,7 @@ pm_auto_loop_speed(pmc_t *pm)
 
 		if (pm->config_RELUCTANCE == PM_ENABLED) {
 
-			rel = (pm->const_im_L1 - pm->const_im_L2) * pm->i_maximal;
+			rel = (pm->const_im_Ld - pm->const_im_Lq) * pm->i_maximal;
 
 			pm->lu_gain_mq_LP = 4.f * Df * (pm->const_lambda + rel)
 				* pm->m_dT * m_fast_recipf(pm->const_Ja);
@@ -645,7 +629,7 @@ float pm_torque_equation(pmc_t *pm, float iD, float iQ)
 
 	if (pm->config_RELUCTANCE == PM_ENABLED) {
 
-		rel = (pm->const_im_L1 - pm->const_im_L2) * iD;
+		rel = (pm->const_im_Ld - pm->const_im_Lq) * iD;
 	}
 	else {
 		rel = 0.f;
@@ -709,7 +693,7 @@ pm_torque_get_current(pmc_t *pm, float mQ)
 			pm->mtpa_approx_D = 1.f;
 		}
 
-		rel = (pm->const_im_L1 - pm->const_im_L2) * pm->mtpa_approx_D;
+		rel = (pm->const_im_Ld - pm->const_im_Lq) * pm->mtpa_approx_D;
 		iQ = mQ / (pm->k_KWAT * (pm->const_lambda + rel));
 
 		pm->mtpa_approx_D = pm_torque_approx_MTPA(pm, pm->mtpa_approx_D, iQ);
@@ -859,34 +843,27 @@ pm_flux_detached(pmc_t *pm)
 static void
 pm_flux_ortega(pmc_t *pm)
 {
-	float		uX, uY, lX, lY, EX, EY, E, A, B, blend;
+	float		uX, uY, lX, lY, fX, fY, E, A, B, blend;
 
 	/* Get the actual voltage.
 	 * */
-	uX = pm->vsi_X - pm->const_Rs * pm->lu_iX;
-	uY = pm->vsi_Y - pm->const_Rs * pm->lu_iY;
-
-	if (		PM_CONFIG_TVM(pm) == PM_ENABLED
-			&& pm->tvm_ACTIVE == PM_ENABLED) {
-
-		uX += pm->tvm_X0 - pm->vsi_X0;
-		uY += pm->tvm_Y0 - pm->vsi_Y0;
-	}
+	uX = pm->dtc_X - pm->const_Rs * pm->lu_iX;
+	uY = pm->dtc_Y - pm->const_Rs * pm->lu_iY;
 
 	/* Stator FLUX.
 	 * */
-	lX = pm->const_im_L2 * pm->lu_iX;
-	lY = pm->const_im_L2 * pm->lu_iY;
+	lX = pm->const_im_Lq * pm->lu_iX;
+	lY = pm->const_im_Lq * pm->lu_iY;
 
 	if (pm->flux_LINKAGE == PM_ENABLED) {
 
-		/* FLUX equations.
+		/* Total FLUX equations.
 		 * */
 		pm->flux_X[0] += uX * pm->m_dT;
 		pm->flux_X[1] += uY * pm->m_dT;
 
-		EX = pm->flux_X[0] - lX;
-		EY = pm->flux_X[1] - lY;
+		fX = pm->flux_X[0] - lX;
+		fY = pm->flux_X[1] - lY;
 
 		blend = m_fabsf(pm->flux_wS * pm->const_lambda)
 			* m_fast_recipf(pm->flux_trip_tol);
@@ -894,15 +871,15 @@ pm_flux_ortega(pmc_t *pm)
 
 		/* Get the flux RESIDUE.
 		 * */
-		E = 1.f - (EX * EX + EY * EY) * pm->quick_iWb2;
+		E = 1.f - (fX * fX + fY * fY) * pm->quick_iWb2;
 
 		/* Adaptive GAIN.
 		 * */
 		E *=	  pm->flux_gain_HI * blend
 			+ pm->flux_gain_LO * (1.f - blend);
 
-		pm->flux_X[0] += EX * E * pm->quick_iWb;
-		pm->flux_X[1] += EY * E * pm->quick_iWb;
+		pm->flux_X[0] += fX * E * pm->quick_iWb;
+		pm->flux_X[1] += fY * E * pm->quick_iWb;
 	}
 	else {
 		/* Startup estimation.
@@ -910,21 +887,21 @@ pm_flux_ortega(pmc_t *pm)
 		pm->flux_X[0] += uX * pm->m_dT;
 		pm->flux_X[1] += uY * pm->m_dT;
 
-		EX = pm->flux_X[0] - lX;
-		EY = pm->flux_X[1] - lY;
+		fX = pm->flux_X[0] - lX;
+		fY = pm->flux_X[1] - lY;
 
 		E = - pm->flux_gain_IN;
 
-		pm->flux_X[0] += EX * E;
-		pm->flux_X[1] += EY * E;
+		pm->flux_X[0] += fX * E;
+		pm->flux_X[1] += fY * E;
 	}
 
 	/* Extract the rotor flux linkage.
 	 * */
-	EX = pm->flux_X[0] - lX;
-	EY = pm->flux_X[1] - lY;
+	fX = pm->flux_X[0] - lX;
+	fY = pm->flux_X[1] - lY;
 
-	E = m_sqrtf(EX * EX + EY * EY);
+	E = m_sqrtf(fX * fX + fY * fY);
 
 	pm->flux_lambda = E;
 
@@ -932,8 +909,8 @@ pm_flux_ortega(pmc_t *pm)
 
 		A = 1.f / E;
 
-		EX *= A;
-		EY *= A;
+		fX *= A;
+		fY *= A;
 
 		if (pm->flux_LINKAGE == PM_ENABLED) {
 
@@ -942,8 +919,8 @@ pm_flux_ortega(pmc_t *pm)
 			m_rotatef(pm->flux_F, pm->flux_wS * pm->m_dT);
 			m_normalizef(pm->flux_F);
 
-			A = EX * pm->flux_F[0] + EY * pm->flux_F[1];
-			B = EY * pm->flux_F[0] - EX * pm->flux_F[1];
+			A = fX * pm->flux_F[0] + fY * pm->flux_F[1];
+			B = fY * pm->flux_F[0] - fX * pm->flux_F[1];
 
 			if (A > M_EPSILON) {
 
@@ -952,8 +929,8 @@ pm_flux_ortega(pmc_t *pm)
 
 			if (pm->flux_gain_IF > M_EPSILON) {
 
-				A = pm_torque_get_accel(pm);
-				pm->flux_wS += A * pm->m_dT * pm->flux_gain_IF;
+				A = pm_torque_get_accel(pm) * pm->m_dT;
+				pm->flux_wS += A * pm->flux_gain_IF;
 			}
 		}
 		else {
@@ -962,96 +939,51 @@ pm_flux_ortega(pmc_t *pm)
 			pm->flux_wS = pm->lu_wS;
 		}
 
-		pm->flux_F[0] = EX;
-		pm->flux_F[1] = EY;
+		pm->flux_F[0] = fX;
+		pm->flux_F[1] = fY;
 	}
 }
 
 static void
-pm_kalman_equation(pmc_t *pm, float Y[2], const float X[2], const float F[2])
+pm_kalman_equation(pmc_t *pm, float D[2])
 {
-	float		uD, uQ, R1, E1, flux_D, flux_Q;
+	float		uD, uQ, R1, E1, fD, fQ;
 
-	uD = F[0] * pm->vsi_X + F[1] * pm->vsi_Y;
-	uQ = F[0] * pm->vsi_Y - F[1] * pm->vsi_X;
+	uD = pm->flux_F[0] * pm->dtc_X + pm->flux_F[1] * pm->dtc_Y;
+	uQ = pm->flux_F[0] * pm->dtc_Y - pm->flux_F[1] * pm->dtc_X;
 
 	R1 = pm->const_Rs;
 	E1 = pm->const_lambda;
 
 	uQ += pm->kalman_bias_Q;
 
-	flux_D = pm->const_im_L1 * X[0] + E1;
-	flux_Q = pm->const_im_L2 * X[1];
+	fD = pm->const_im_Ld * pm->flux_X[0] + E1;
+	fQ = pm->const_im_Lq * pm->flux_X[1];
 
-	Y[0] = (uD - R1 * X[0] + flux_Q * pm->flux_wS) * pm->quick_iL1;
-	Y[1] = (uQ - R1 * X[1] - flux_D * pm->flux_wS) * pm->quick_iL2;
+	D[0] = (uD - R1 * pm->flux_X[0] + fQ * pm->flux_wS) * pm->quick_iLd;
+	D[1] = (uQ - R1 * pm->flux_X[1] - fD * pm->flux_wS) * pm->quick_iLq;
 }
 
 static void
-pm_kalman_solve(pmc_t *pm, float X[2], float F[2], float wS)
+pm_kalman_solve(pmc_t *pm)
 {
-	float		Y1[2], Y2[2];
+	float		D0[2], D1[2];
 
 	/* Second-order ODE solver.
 	 * */
 
-	pm_kalman_equation(pm, Y1, X, F);
+	pm_kalman_equation(pm, D0);
 
-	X[0] += Y1[0] * pm->m_dT;
-	X[1] += Y1[1] * pm->m_dT;
+	pm->flux_X[0] += D0[0] * pm->m_dT;
+	pm->flux_X[1] += D0[1] * pm->m_dT;
 
-	m_rotatef(F, wS * pm->m_dT);
-	m_normalizef(F);
+	m_rotatef(pm->flux_F, pm->flux_wS * pm->m_dT);
+	m_normalizef(pm->flux_F);
 
-	pm_kalman_equation(pm, Y2, X, F);
+	pm_kalman_equation(pm, D1);
 
-	X[0] += (Y2[0] - Y1[0]) * pm->m_dT * 0.5f;
-	X[1] += (Y2[1] - Y1[1]) * pm->m_dT * 0.5f;
-}
-
-static void
-pm_kalman_solve_tvm(pmc_t *pm, float X[2], float F[2])
-{
-	float		uX, uY, uD, uQ;
-
-	/* First-order ODE solver.
-	 * */
-
-	uX = pm->tvm_X0 - pm->vsi_X0;
-	uY = pm->tvm_Y0 - pm->vsi_Y0;
-
-	uD = F[0] * uX + F[1] * uY;
-	uQ = F[0] * uY - F[1] * uX;
-
-	X[0] += uD * pm->m_dT * pm->quick_iL1;
-	X[1] += uQ * pm->m_dT * pm->quick_iL2;
-}
-
-static void
-pm_kalman_jacobian(pmc_t *pm, const float X[2], const float F[2], float wS)
-{
-	float		*A = pm->kalman_A;
-
-	/*
-	 *     [ A(0) A(1) A(2) A(3) 0    ]
-	 *     [ A(4) A(5) A(6) A(7) A(8) ]
-	 * A = [ 0    0    1    A(9) 0    ]
-	 *     [ 0    0    0    1    0    ]
-	 *     [ 0    0    0    0    1    ]
-	 * */
-
-	A[0] = 1.f - pm->const_Rs * pm->quick_TiL1;
-	A[1] = wS * pm->const_im_L2 * pm->quick_TiL1;
-	A[2] = (F[0] * pm->vsi_Y - F[1] * pm->vsi_X) * pm->quick_TiL1;
-	A[3] = X[1] * pm->const_im_L2 * pm->quick_TiL1;
-
-	A[4] = - wS * pm->const_im_L1 * pm->quick_TiL2;
-	A[5] = 1.f - pm->const_Rs * pm->quick_TiL2;
-	A[6] = (- F[0] * pm->vsi_X - F[1] * pm->vsi_Y) * pm->quick_TiL2;
-	A[7] = (- pm->const_lambda - X[0] * pm->const_im_L1) * pm->quick_TiL2;
-	A[8] = pm->quick_TiL2;
-
-	A[9] = pm->m_dT;
+	pm->flux_X[0] += (D1[0] - D0[0]) * pm->m_dT * 0.5f;
+	pm->flux_X[1] += (D1[1] - D0[1]) * pm->m_dT * 0.5f;
 }
 
 static void
@@ -1061,12 +993,19 @@ pm_kalman_forecast(pmc_t *pm)
 	const float	*A = pm->kalman_A;
 	const float	*Q = pm->kalman_gain_Q;
 
-	float		AP[16];
+	float		iX, iY, uX, uY, fC, fS, wS, bQ;
+	float		u[17], F[10], R1, E1;
 
 	/*
 	 * Calculate predicted (a priori) covariance to the next cycle.
 	 *
-	 * P = A * P * A' + Q.
+	 * P = F * P * F' + Q.
+	 *
+	 *     [ F(0) F(1) F(2) F(3) F(4) ]
+	 *     [ F(5) F(6) F(7) F(8) F(9) ]
+	 * F = [ 0    0    1    dT   0    ]
+	 *     [ 0    0    0    1    0    ]
+	 *     [ 0    0    0    0    1    ]
 	 *
 	 *     [ P(0)  P(1)  P(3)  P(6)  P(10) ]
 	 *     [ P(1)  P(2)  P(4)  P(7)  P(11) ]
@@ -1076,163 +1015,208 @@ pm_kalman_forecast(pmc_t *pm)
 	 *
 	 * */
 
-	AP[0] = A[0] * P[0] + A[1] * P[1] + A[2] * P[3] + A[3] * P[6];
-	AP[1] = A[0] * P[1] + A[1] * P[2] + A[2] * P[4] + A[3] * P[7];
-	AP[2] = A[0] * P[3] + A[1] * P[4] + A[2] * P[5] + A[3] * P[8];
-	AP[3] = A[0] * P[6] + A[1] * P[7] + A[2] * P[8] + A[3] * P[9];
-
-	AP[4] = A[4] * P[0] + A[5] * P[1] + A[6] * P[3] + A[7] * P[6] + A[8] * P[10];
-	AP[5] = A[4] * P[1] + A[5] * P[2] + A[6] * P[4] + A[7] * P[7] + A[8] * P[11];
-	AP[6] = A[4] * P[3] + A[5] * P[4] + A[6] * P[5] + A[7] * P[8] + A[8] * P[12];
-	AP[7] = A[4] * P[6] + A[5] * P[7] + A[6] * P[8] + A[7] * P[9] + A[8] * P[13];
-	AP[8] = A[4] * P[10] + A[5] * P[11] + A[6] * P[12] + A[7] * P[13] + A[8] * P[14];
-
-	AP[9] = P[3] + A[9] * P[6];
-	AP[10] = P[4] + A[9] * P[7];
-	AP[11] = P[5] + A[9] * P[8];
-	AP[12] = P[8] + A[9] * P[9];
-	AP[13] = P[12] + A[9] * P[13];
-
-	AP[14] = P[6];
-	AP[15] = P[10];
-
-	P[0] = AP[0] * A[0] + AP[1] * A[1] + AP[2] * A[2] + AP[3] * A[3];
-	P[1] = AP[4] * A[0] + AP[5] * A[1] + AP[6] * A[2] + AP[7] * A[3];
-	P[2] = AP[4] * A[4] + AP[5] * A[5] + AP[6] * A[6] + AP[7] * A[7] + AP[8] * A[8];
-	P[3] = AP[9] * A[0] + AP[10] * A[1] + AP[11] * A[2] + AP[12] * A[3];
-	P[4] = AP[9] * A[4] + AP[10] * A[5] + AP[11] * A[6] + AP[12] * A[7] + AP[13] * A[8];
-	P[5] = AP[11] + AP[12] * A[9];
-	P[6] = AP[14] * A[0] + P[7] * A[1] + P[8] * A[2] + P[9] * A[3];
-	P[7] = AP[14] * A[4] + P[7] * A[5] + P[8] * A[6] + P[9] * A[7] + P[13] * A[8];
-	P[8] = P[8] + P[9] * A[9];
-	P[10] = AP[15] * A[0] + P[11] * A[1] + P[12] * A[2] + P[13] * A[3];
-	P[11] = AP[15] * A[4] + P[11] * A[5] + P[12] * A[6] + P[13] * A[7] + P[14] * A[8];
-	P[12] = P[12] + P[13] * A[9];
-
-	P[0] += Q[0];
-	P[2] += Q[1];
-	P[5] += Q[2];
-	P[9] += Q[3];
-	P[14] += Q[4];
+	iX = A[0];
+	iY = A[1];
+	uX = A[2];
+	uY = A[3];
+	fC = A[4];
+	fS = A[5];
+	wS = A[6];
+	bQ = A[7];
+
+	u[0] = fC * fC;
+	u[1] = fS * fS;
+	u[2] = fC * fS;
+	u[3] = 2.0f * u[2];	/* sin(2*th) */
+	u[4] = u[0] - u[1];	/* cos(2*th) */
+
+	R1 = pm->const_Rs;
+	E1 = pm->const_lambda;
+
+	u[5] = (uX - R1 * iX) * pm->quick_TiLu[0];
+	u[6] = (uY - R1 * iY) * pm->quick_TiLu[0];
+	u[7] = (wS * E1 - bQ) * pm->quick_TiLq;
+
+	u[8] = iX * u[4] + iY * u[3];
+	u[9] = iY * u[4] - iX * u[3];
+
+	F[0] = 1.f - R1 * (pm->quick_TiLd - u[1] * pm->quick_TiLu[0]);
+	F[6] = 1.f - R1 * (pm->quick_TiLq + u[1] * pm->quick_TiLu[0]);
+
+	F[1] = - R1 * u[2] * pm->quick_TiLu[0];
+	F[5] = F[1];
+
+	F[2] = u[6] * u[4] - u[5] * u[3] + u[7] * fC;
+	F[7] = u[5] * u[4] + u[6] * u[3] + u[7] * fS;
+
+	F[3] =   E1 * fS * pm->quick_TiLq;
+	F[8] = - E1 * fC * pm->quick_TiLq;
+
+	F[4] = - fS * pm->quick_TiLq;
+	F[9] =   fC * pm->quick_TiLq;
+
+	F[0] +=   wS * u[2] * pm->quick_TiLu[1];
+	F[6] += - wS * u[2] * pm->quick_TiLu[1];
+
+	F[1] += wS * (pm->quick_TiLu[2] - u[0] * pm->quick_TiLu[1]);
+	F[5] += wS * (pm->quick_TiLu[3] - u[0] * pm->quick_TiLu[1]);
+
+	F[2] +=   wS * u[8] * pm->quick_TiLu[1];
+	F[7] += - wS * u[9] * pm->quick_TiLu[1];
+
+	u[5] = pm->const_im_Ld - pm->const_im_Lq;
+	u[6] = pm->const_im_Ld + pm->const_im_Lq;
+
+	F[3] += - 0.5f * (iY * u[5] + u[9] * u[6]) * pm->quick_TiLu[0];
+	F[8] +=   0.5f * (iX * u[5] + u[8] * u[6]) * pm->quick_TiLu[0];
+
+	u[0] = F[0] * P[0]  + F[1] * P[1]  + F[2] * P[3]  + F[3] * P[6]  + F[4] * P[10];
+	u[1] = F[0] * P[1]  + F[1] * P[2]  + F[2] * P[4]  + F[3] * P[7]  + F[4] * P[11];
+	u[2] = F[0] * P[3]  + F[1] * P[4]  + F[2] * P[5]  + F[3] * P[8]  + F[4] * P[12];
+	u[3] = F[0] * P[6]  + F[1] * P[7]  + F[2] * P[8]  + F[3] * P[9]  + F[4] * P[13];
+	u[4] = F[0] * P[10] + F[1] * P[11] + F[2] * P[12] + F[3] * P[13] + F[4] * P[14];
+
+	u[5] = F[5] * P[0]  + F[6] * P[1]  + F[7] * P[3]  + F[8] * P[6]  + F[9] * P[10];
+	u[6] = F[5] * P[1]  + F[6] * P[2]  + F[7] * P[4]  + F[8] * P[7]  + F[9] * P[11];
+	u[7] = F[5] * P[3]  + F[6] * P[4]  + F[7] * P[5]  + F[8] * P[8]  + F[9] * P[12];
+	u[8] = F[5] * P[6]  + F[6] * P[7]  + F[7] * P[8]  + F[8] * P[9]  + F[9] * P[13];
+	u[9] = F[5] * P[10] + F[6] * P[11] + F[7] * P[12] + F[8] * P[13] + F[9] * P[14];
+
+	u[10] = P[3]  + pm->m_dT * P[6];
+	u[11] = P[4]  + pm->m_dT * P[7];
+	u[12] = P[5]  + pm->m_dT * P[8];
+	u[13] = P[8]  + pm->m_dT * P[9];
+	u[14] = P[12] + pm->m_dT * P[13];
+
+	u[15] = P[6];
+	u[16] = P[10];
+
+	P[0]  = u[0] * F[0] + u[1] * F[1] + u[2] * F[2] + u[3] * F[3] + u[4] * F[4];
+	P[1]  = u[5] * F[0] + u[6] * F[1] + u[7] * F[2] + u[8] * F[3] + u[9] * F[4];
+	P[2]  = u[5] * F[5] + u[6] * F[6] + u[7] * F[7] + u[8] * F[8] + u[9] * F[9];
+	P[3]  = u[10] * F[0] + u[11] * F[1] + u[12] * F[2] + u[13] * F[3] + u[14] * F[4];
+	P[4]  = u[10] * F[5] + u[11] * F[6] + u[12] * F[7] + u[13] * F[8] + u[14] * F[9];
+	P[5]  = u[12] + u[13] * pm->m_dT;
+	P[6]  = u[15] * F[0] + P[7] * F[1] + P[8] * F[2] + P[9] * F[3] + P[13] * F[4];
+	P[7]  = u[15] * F[5] + P[7] * F[6] + P[8] * F[7] + P[9] * F[8] + P[13] * F[9];
+	P[10] = u[16] * F[0] + P[11] * F[1] + P[12] * F[2] + P[13] * F[3] + P[14] * F[4];
+	P[11] = u[16] * F[5] + P[11] * F[6] + P[12] * F[7] + P[13] * F[8] + P[14] * F[9];
+
+	P[8]  += P[9]  * pm->m_dT;
+	P[12] += P[13] * pm->m_dT;
+
+	P[0]  += Q[0] * pm->quick_TiLq;
+	P[2]  += Q[0] * pm->quick_TiLq;
+	P[5]  += Q[1] * pm->m_dT;
+	P[9]  += Q[2] * pm->m_dT;
+	P[14] += Q[3] * pm->m_dT;
 }
 
 static void
-pm_kalman_update(pmc_t *pm, const float X[2])
+pm_kalman_update(pmc_t *pm)
 {
 	float		*P = pm->kalman_P;
 	float		*K = pm->kalman_K;
+	float		iR = pm->kalman_gain_R;
 
-	float		CP[5], S, u;
+	float		HP[5], u;
 
 	/*
 	 * Calculate updated (a posteriori) covariance and Kalman gain.
 	 *
-	 * S = C * P * C' + R.
-	 * K = P * C' / S.
-	 * P = P - K * C * P.
+	 * S = H * P * H' + R.
+	 * K = P * H' / S.
+	 * P = P - K * H * P.
 	 *
-	 * C(1) = [ 1  0  -X[1]  0  0 ]
-	 * C(2) = [ 0  1   X[0]  0  0 ]
+	 * H(1) = [ 1  0  0  0  0 ]
+	 * H(2) = [ 0  1  0  0  0 ]
 	 *
 	 * */
 
-	CP[0] = P[0] - X[1] * P[3];
-	CP[1] = P[1] - X[1] * P[4];
-	CP[2] = P[3] - X[1] * P[5];
-	CP[3] = P[6] - X[1] * P[8];
-	CP[4] = P[10] - X[1] * P[12];
-
-	S = CP[0] - CP[2] * X[1] + pm->kalman_gain_R;
-	u = m_fast_recipf(S);
-
-	K[0] = CP[0] * u;
-	K[2] = CP[1] * u;
-	K[4] = CP[2] * u;
-	K[6] = CP[3] * u;
-	K[8] = CP[4] * u;
-
-	P[0] += - K[0] * CP[0];
-	P[1] += - K[2] * CP[0];
-	P[2] += - K[2] * CP[1];
-	P[3] += - K[4] * CP[0];
-	P[4] += - K[4] * CP[1];
-	P[5] += - K[4] * CP[2];
-	P[6] += - K[6] * CP[0];
-	P[7] += - K[6] * CP[1];
-	P[8] += - K[6] * CP[2];
-	P[9] += - K[6] * CP[3];
-	P[10] += - K[8] * CP[0];
-	P[11] += - K[8] * CP[1];
-	P[12] += - K[8] * CP[2];
-	P[13] += - K[8] * CP[3];
-	P[14] += - K[8] * CP[4];
-
-	CP[0] = P[1] + X[0] * P[3];
-	CP[1] = P[2] + X[0] * P[4];
-	CP[2] = P[4] + X[0] * P[5];
-	CP[3] = P[7] + X[0] * P[8];
-	CP[4] = P[11] + X[0] * P[12];
-
-	S = CP[1] + CP[2] * X[0] + pm->kalman_gain_R;
-	u = m_fast_recipf(S);
-
-	K[1] = CP[0] * u;
-	K[3] = CP[1] * u;
-	K[5] = CP[2] * u;
-	K[7] = CP[3] * u;
-	K[9] = CP[4] * u;
-
-	P[0] += - K[1] * CP[0];
-	P[1] += - K[3] * CP[0];
-	P[2] += - K[3] * CP[1];
-	P[3] += - K[5] * CP[0];
-	P[4] += - K[5] * CP[1];
-	P[5] += - K[5] * CP[2];
-	P[6] += - K[7] * CP[0];
-	P[7] += - K[7] * CP[1];
-	P[8] += - K[7] * CP[2];
-	P[9] += - K[7] * CP[3];
-	P[10] += - K[9] * CP[0];
-	P[11] += - K[9] * CP[1];
-	P[12] += - K[9] * CP[2];
-	P[13] += - K[9] * CP[3];
-	P[14] += - K[9] * CP[4];
+	HP[0] = P[0];
+	HP[1] = P[1];
+	HP[2] = P[3];
+	HP[3] = P[6];
+	HP[4] = P[10];
+
+	u = 1.f / (HP[0] + iR);
+
+	K[0] = HP[0] * u;
+	K[2] = HP[1] * u;
+	K[4] = HP[2] * u;
+	K[6] = HP[3] * u;
+	K[8] = HP[4] * u;
+
+	P[0]  += - K[0] * HP[0];
+	P[1]  += - K[2] * HP[0];
+	P[2]  += - K[2] * HP[1];
+	P[3]  += - K[4] * HP[0];
+	P[4]  += - K[4] * HP[1];
+	P[5]  += - K[4] * HP[2];
+	P[6]  += - K[6] * HP[0];
+	P[7]  += - K[6] * HP[1];
+	P[8]  += - K[6] * HP[2];
+	P[9]  += - K[6] * HP[3];
+	P[10] += - K[8] * HP[0];
+	P[11] += - K[8] * HP[1];
+	P[12] += - K[8] * HP[2];
+	P[13] += - K[8] * HP[3];
+	P[14] += - K[8] * HP[4];
+
+	HP[0] = P[1];
+	HP[1] = P[2];
+	HP[2] = P[4];
+	HP[3] = P[7];
+	HP[4] = P[11];
+
+	u = 1.f / (HP[1] + iR);
+
+	K[1] = HP[0] * u;
+	K[3] = HP[1] * u;
+	K[5] = HP[2] * u;
+	K[7] = HP[3] * u;
+	K[9] = HP[4] * u;
+
+	P[0]  += - K[1] * HP[0];
+	P[1]  += - K[3] * HP[0];
+	P[2]  += - K[3] * HP[1];
+	P[3]  += - K[5] * HP[0];
+	P[4]  += - K[5] * HP[1];
+	P[5]  += - K[5] * HP[2];
+	P[6]  += - K[7] * HP[0];
+	P[7]  += - K[7] * HP[1];
+	P[8]  += - K[7] * HP[2];
+	P[9]  += - K[7] * HP[3];
+	P[10] += - K[9] * HP[0];
+	P[11] += - K[9] * HP[1];
+	P[12] += - K[9] * HP[2];
+	P[13] += - K[9] * HP[3];
+	P[14] += - K[9] * HP[4];
+
+	u = - K[2];
+
+	K[0] += K[1] * u;
+	K[2] += K[3] * u;
+	K[4] += K[5] * u;
+	K[6] += K[7] * u;
+	K[8] += K[9] * u;
 }
 
 static void
-pm_kalman_lockout_guard(pmc_t *pm, float A)
+pm_kalman_lockout_guard(pmc_t *pm, float dA)
 {
-	float		thld_wS, bCOS;
-	int		flux_RESET = PM_DISABLED;
+	float		thld_wS;
 
-	/* Bare speed estiamte LPF.
+	/* Get speed LPF of actual DQ-axes.
 	 * */
-	pm->kalman_lpf_wS += (A * pm->m_freq - pm->kalman_lpf_wS) * pm->zone_gain_LP;
+	pm->kalman_lpf_wS += (dA * pm->m_freq - pm->kalman_lpf_wS) * pm->zone_gain_LP;
 
 	if (		   pm->flux_ZONE == PM_ZONE_NONE
 			|| pm->flux_ZONE == PM_ZONE_UNCERTAIN) {
 
 		thld_wS = pm->zone_threshold * pm->zone_gain_TH;
 
-		if (		pm->kalman_lpf_wS < - thld_wS
-				&& pm->flux_wS > thld_wS) {
-
-			flux_RESET = PM_ENABLED;
-		}
-		else if (	pm->kalman_lpf_wS > thld_wS
-				&& pm->flux_wS < - thld_wS) {
-
-			flux_RESET = PM_ENABLED;
-		}
-
-		bCOS = pm->lu_F[0] * pm->flux_F[0] + pm->lu_F[1] * pm->flux_F[1];
-
-		if (bCOS < 0.f) {
-
-			flux_RESET = PM_ENABLED;
-		}
-
-		if (flux_RESET == PM_ENABLED) {
+		if (m_fabsf(pm->kalman_lpf_wS) > thld_wS) {
 
 			/* Restart Kalman and flip DQ-axes.
 			 * */
@@ -1240,10 +1224,10 @@ pm_kalman_lockout_guard(pmc_t *pm, float A)
 
 			pm->flux_F[0] = - pm->flux_F[0];
 			pm->flux_F[1] = - pm->flux_F[1];
-			pm->flux_wS = pm->kalman_lpf_wS;
+			pm->flux_wS += pm->kalman_lpf_wS;
 
 			pm->kalman_POSTPONED = PM_DISABLED;
-			pm->kalman_bias_Q = 0.f;
+			pm->kalman_lpf_wS = 0.f;
 		}
 	}
 }
@@ -1252,50 +1236,43 @@ static void
 pm_flux_kalman(pmc_t *pm)
 {
 	const float		*K = pm->kalman_K;
-	float			eD, eQ, A, bF[2];
-
-	if (		PM_CONFIG_TVM(pm) == PM_ENABLED
-			&& pm->tvm_ACTIVE == PM_ENABLED) {
+	float			*A = pm->kalman_A;
+	float			*E = pm->kalman_E;
 
-		pm_kalman_solve_tvm(pm, pm->flux_X, pm->flux_F);
-	}
+	float			tA, dA = 0.f;
 
-	/* Get DQ-axes frame.
+	/* Get the current estimate in XY-axes.
 	 * */
-	bF[0] = pm->flux_F[0];
-	bF[1] = pm->flux_F[1];
+	A[0] = pm->flux_F[0] * pm->flux_X[0] - pm->flux_F[1] * pm->flux_X[1];
+	A[1] = pm->flux_F[1] * pm->flux_X[0] + pm->flux_F[0] * pm->flux_X[1];
 
-	/* Get the current residuals in DQ-axes.
+	/* Get the current residuals.
 	 * */
-	eD = bF[0] * pm->lu_iX + bF[1] * pm->lu_iY - pm->flux_X[0];
-	eQ = bF[0] * pm->lu_iY - bF[1] * pm->lu_iX - pm->flux_X[1];
-
-	pm->kalman_residual_D = eD;
-	pm->kalman_residual_Q = eQ;
+	E[0] = pm->lu_iX - A[0];
+	E[1] = pm->lu_iY - A[1];
 
 	if (likely(pm->vsi_IF == 0)) {
 
-		pm->flux_X[0] += K[0] * eD + K[1] * eQ;
-		pm->flux_X[1] += K[2] * eD + K[3] * eQ;
+		A[0] += K[0] * E[0] + K[1] * E[1];
+		A[1] += K[2] * E[0] + K[3] * E[1];
 
-		A = K[4] * eD + K[5] * eQ;
-		A = (A < - 1.f) ? - 1.f : (A > 1.f) ? 1.f : A;
+		dA = K[4] * E[0] + K[5] * E[1];
+		dA = (dA < - 1.f) ? - 1.f : (dA > 1.f) ? 1.f : dA;
 
-		m_rotatef(pm->flux_F, A);
+		m_rotatef(pm->flux_F, dA);
 
-		if (pm->flux_LINKAGE == PM_ENABLED) {
+		pm->flux_X[0] = pm->flux_F[0] * A[0] + pm->flux_F[1] * A[1];
+		pm->flux_X[1] = pm->flux_F[0] * A[1] - pm->flux_F[1] * A[0];
 
-			pm->flux_wS += K[6] * eD + K[7] * eQ;
+		if (pm->flux_LINKAGE == PM_ENABLED) {
 
-			if (pm->flux_gain_IF > M_EPSILON) {
-
-				A = pm_torque_get_accel(pm);
-				pm->flux_wS += A * pm->m_dT * pm->flux_gain_IF;
-			}
+			pm->flux_wS += K[6] * E[0] + K[7] * E[1];
+			pm->flux_wS = (pm->flux_wS < - pm->m_freq) ? - pm->m_freq
+				: (pm->flux_wS > pm->m_freq) ? pm->m_freq : pm->flux_wS;
 
 			if (pm->flux_ZONE == PM_ZONE_HIGH) {
 
-				pm->kalman_bias_Q += K[8] * eD + K[9] * eQ;
+				pm->kalman_bias_Q += K[8] * E[0] + K[9] * E[1];
 			}
 			else {
 				pm->kalman_bias_Q = 0.f;
@@ -1306,23 +1283,46 @@ pm_flux_kalman(pmc_t *pm)
 			 * */
 			pm->flux_wS = pm->lu_wS;
 
-			pm->kalman_bias_Q += K[8] * eD + K[9] * eQ;
+			pm->kalman_bias_Q += K[8] * E[0] + K[9] * E[1];
+
+			if (		pm->kalman_bias_Q < 0.f
+					&& pm->flux_wS < 0.f) {
+
+				pm->kalman_bias_Q = 0.f;
+			}
+			else if (	pm->kalman_bias_Q > 0.f
+					&& pm->flux_wS > 0.f) {
+
+				pm->kalman_bias_Q = 0.f;
+			}
 		}
 	}
 
-	pm->kalman_POSTPONED = PM_ENABLED;
+	if (		pm->flux_LINKAGE == PM_ENABLED
+			&& pm->flux_gain_IF > M_EPSILON) {
+
+		tA = pm_torque_get_accel(pm) * pm->m_dT;
+		pm->flux_wS += tA * pm->flux_gain_IF;
+	}
 
-	/* Build sparse Jacobian and postpone the rest of work.
+	/* Set aside the variables to calculate covariance at the end of cycle.
 	 * */
-	pm_kalman_jacobian(pm, pm->flux_X, pm->flux_F, pm->flux_wS);
+	A[2] = pm->vsi_X;
+	A[3] = pm->vsi_Y;
+	A[4] = pm->flux_F[0];
+	A[5] = pm->flux_F[1];
+	A[6] = pm->flux_wS;
+	A[7] = pm->kalman_bias_Q;
 
-	/* Time update to the next cycle.
+	pm->kalman_POSTPONED = PM_ENABLED;
+
+	/* We propagate the state estimates to the next cycle.
 	 * */
-	pm_kalman_solve(pm, pm->flux_X, pm->flux_F, pm->flux_wS);
+	pm_kalman_solve(pm);
 
-	/* Guard against lockout of DQ-axes in reversed position.
+	/* Guard against lockout of DQ-axes in reverse position.
 	 * */
-	pm_kalman_lockout_guard(pm, bF[0] * pm->flux_F[1] - bF[1] * pm->flux_F[0]);
+	pm_kalman_lockout_guard(pm, dA);
 }
 
 static void
@@ -1390,13 +1390,13 @@ pm_estimate(pmc_t *pm)
 
 		if (pm->flux_TYPE != PM_FLUX_ORTEGA) {
 
-			float			E1, L2;
+			float			E1, Lq;
 
 			E1 = pm->const_lambda;
-			L2 = pm->const_im_L2;
+			Lq = pm->const_im_Lq;
 
-			pm->flux_X[0] = E1 * pm->flux_F[0] + L2 * pm->lu_iX;
-			pm->flux_X[1] = E1 * pm->flux_F[1] + L2 * pm->lu_iY;
+			pm->flux_X[0] = E1 * pm->flux_F[0] + Lq * pm->lu_iX;
+			pm->flux_X[1] = E1 * pm->flux_F[1] + Lq * pm->lu_iY;
 
 			pm->flux_TYPE = PM_FLUX_ORTEGA;
 		}
@@ -1411,16 +1411,19 @@ pm_estimate(pmc_t *pm)
 			pm->flux_X[0] = pm->lu_iD;
 			pm->flux_X[1] = pm->lu_iQ;
 
+			pm->flux_F[0] = pm->lu_F[0];
+			pm->flux_F[1] = pm->lu_F[1];
+
 			pm->kalman_P[0] = 0.f;
 			pm->kalman_P[1] = 0.f;
 			pm->kalman_P[2] = 0.f;
 			pm->kalman_P[3] = 0.f;
 			pm->kalman_P[4] = 0.f;
-			pm->kalman_P[5] = 0.f;
+			pm->kalman_P[5] = 1.f;
 			pm->kalman_P[6] = 0.f;
 			pm->kalman_P[7] = 0.f;
 			pm->kalman_P[8] = 0.f;
-			pm->kalman_P[9] = 0.f;
+			pm->kalman_P[9] = 1.f;
 			pm->kalman_P[10] = 0.f;
 			pm->kalman_P[11] = 0.f;
 			pm->kalman_P[12] = 0.f;
@@ -1473,7 +1476,7 @@ pm_hfi_wave(pmc_t *pm)
 		 * */
 		if (pm->hfi_wave[1] > M_PI_F) {
 
-			pm->hfi_wave[0] = m_lf_gaussf(&pm->hfi_seed) * 0.33f;
+			pm->hfi_wave[0] = m_lf_gaussf(&pm->lfseed) * 0.33f;
 			pm->hfi_wave[1] += - M_PI_F;
 		}
 
@@ -1527,8 +1530,8 @@ pm_sensor_hall(pmc_t *pm)
 
 		if (pm->hall_gain_IF > M_EPSILON) {
 
-			A = pm_torque_get_accel(pm);
-			pm->hall_wS += A * pm->m_dT * pm->hall_gain_IF;
+			A = pm_torque_get_accel(pm) * pm->m_dT;
+			pm->hall_wS += A * pm->hall_gain_IF;
 		}
 
 		m_rotatef(pm->hall_F, pm->hall_wS * pm->m_dT);
@@ -1639,8 +1642,8 @@ pm_sensor_eabi(pmc_t *pm)
 
 	if (pm->eabi_gain_IF > M_EPSILON) {
 
-		A = pm_torque_get_accel(pm);
-		pm->eabi_wS += A * pm->m_dT * pm->eabi_gain_IF;
+		A = pm_torque_get_accel(pm) * pm->m_dT;
+		pm->eabi_wS += A * pm->eabi_gain_IF;
 	}
 
 	pm->eabi_interp += pm->eabi_wS * pm->m_dT;
@@ -1776,6 +1779,7 @@ 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.
@@ -1783,25 +1787,22 @@ pm_lu_FSM(pmc_t *pm)
 	pm->lu_iD = pm->lu_F[0] * pm->lu_iX + pm->lu_F[1] * pm->lu_iY;
 	pm->lu_iQ = pm->lu_F[0] * pm->lu_iY - pm->lu_F[1] * pm->lu_iX;
 
-	/* Get TVM voltage on DQ-axes to the middle of past cycle.
+	/* Get voltage on DQ-axes on the middle of cycle.
 	 * */
-	lu_F[0] = pm->lu_F[0];
-	lu_F[1] = pm->lu_F[1];
-
-	m_rotatef(lu_F, - pm->lu_wS * pm->m_dT * 0.5f);
+	m_rotatef(pm->lu_F, pm->lu_wS * pm->m_dT * 0.5f);
 
-	pm->lu_uD = lu_F[0] * pm->tvm_X0 + lu_F[1] * pm->tvm_Y0;
-	pm->lu_uQ = lu_F[0] * pm->tvm_Y0 - lu_F[1] * pm->tvm_X0;
+	pm->lu_uD = pm->lu_F[0] * pm->dtc_X + pm->lu_F[1] * pm->dtc_Y;
+	pm->lu_uQ = pm->lu_F[0] * pm->dtc_Y - pm->lu_F[1] * pm->dtc_X;
 
-	/* Transfer to the next apriori position.
+	/* Transform DQ-axes to the future cycle position.
 	 * */
-	m_rotatef(pm->lu_F, pm->lu_wS * pm->m_dT);
+	m_rotatef(pm->lu_F, pm->lu_wS * pm->m_dT * 0.5f);
 	m_normalizef(pm->lu_F);
 
 	if (unlikely(pm->vsi_IF != 0)) {
 
-		/* We transform DQ-axes current back to XY-axes throught
-		 * apriori DQ-axes if there are no clean measurements available.
+		/* We transform DQ-axes current back to XY-axes throught future
+		 * DQ-axes if there are no clean measurements available.
 		 * */
 		pm->lu_iX = pm->lu_F[0] * pm->lu_iD - pm->lu_F[1] * pm->lu_iQ;
 		pm->lu_iY = pm->lu_F[1] * pm->lu_iD + pm->lu_F[0] * pm->lu_iQ;
@@ -2252,7 +2253,7 @@ pm_lu_FSM(pmc_t *pm)
 
 void pm_clearance(pmc_t *pm, int xA, int xB, int xC)
 {
-	int		xZONE, xSKIP, xMIN, xTOP;
+	int		xZONE, xSKIP, xTOP;
 
 	xZONE = pm->dc_resolution - pm->ts_clearance;
 	xSKIP = pm->dc_resolution - pm->ts_skip;
@@ -2271,81 +2272,47 @@ void pm_clearance(pmc_t *pm, int xA, int xB, int xC)
 	 * */
 	if (PM_CONFIG_IFB(pm) == PM_IFB_AB_INLINE) {
 
-		pm->vsi_AF = (pm->vsi_AQ < xZONE || pm->vsi_AQ == xTOP) ? 0 : 1;
-		pm->vsi_BF = (pm->vsi_BQ < xZONE || pm->vsi_BQ == xTOP) ? 0 : 1;
+		pm->vsi_AF = (pm->vsi_A0 < xZONE || pm->vsi_A0 == xTOP) ? 0 : 1;
+		pm->vsi_BF = (pm->vsi_B0 < xZONE || pm->vsi_B0 == xTOP) ? 0 : 1;
 		pm->vsi_CF = 1;
 	}
 	else if (PM_CONFIG_IFB(pm) == PM_IFB_AB_GND) {
 
-		pm->vsi_AF = (pm->vsi_AQ < xZONE) ? 0 : 1;
-		pm->vsi_BF = (pm->vsi_BQ < xZONE) ? 0 : 1;
+		pm->vsi_AF = (pm->vsi_A0 < xZONE) ? 0 : 1;
+		pm->vsi_BF = (pm->vsi_B0 < xZONE) ? 0 : 1;
 		pm->vsi_CF = 1;
 	}
 	else if (PM_CONFIG_IFB(pm) == PM_IFB_ABC_INLINE) {
 
-		pm->vsi_AF = (pm->vsi_AQ < xZONE || pm->vsi_AQ == xTOP) ? 0 : 1;
-		pm->vsi_BF = (pm->vsi_BQ < xZONE || pm->vsi_BQ == xTOP) ? 0 : 1;
-		pm->vsi_CF = (pm->vsi_CQ < xZONE || pm->vsi_CQ == xTOP) ? 0 : 1;
+		pm->vsi_AF = (pm->vsi_A0 < xZONE || pm->vsi_A0 == xTOP) ? 0 : 1;
+		pm->vsi_BF = (pm->vsi_B0 < xZONE || pm->vsi_B0 == xTOP) ? 0 : 1;
+		pm->vsi_CF = (pm->vsi_C0 < xZONE || pm->vsi_C0 == xTOP) ? 0 : 1;
 	}
 	else if (PM_CONFIG_IFB(pm) == PM_IFB_ABC_GND) {
 
-		pm->vsi_AF = (pm->vsi_AQ < xZONE) ? 0 : 1;
-		pm->vsi_BF = (pm->vsi_BQ < xZONE) ? 0 : 1;
-		pm->vsi_CF = (pm->vsi_CQ < xZONE) ? 0 : 1;
+		pm->vsi_AF = (pm->vsi_A0 < xZONE) ? 0 : 1;
+		pm->vsi_BF = (pm->vsi_B0 < xZONE) ? 0 : 1;
+		pm->vsi_CF = (pm->vsi_C0 < xZONE) ? 0 : 1;
 	}
 
-	/* NOTE: You can mask a specific current measurement channel on your
-	 * own considerations.
-	 * */
-	pm->vsi_AF = unlikely(pm->vsi_mask_XF == PM_MASK_A) ? 1 : pm->vsi_AF;
-	pm->vsi_BF = unlikely(pm->vsi_mask_XF == PM_MASK_B) ? 1 : pm->vsi_BF;
-	pm->vsi_CF = unlikely(pm->vsi_mask_XF == PM_MASK_C) ? 1 : pm->vsi_CF;
-
-	/* Chech if at least TWO samples are clean so they can be used in
-	 * control loops.
+	/* Chech if at least TWO samples are clean so the current can be used
+	 * in control loops.
 	 * */
 	pm->vsi_IF = likely(pm->vsi_AF + pm->vsi_BF + pm->vsi_CF < 2) ? 0 : 1;
 
 	/* Check if there are PWM edges within clearance zone. The DC link
 	 * voltage measurement will be used or rejected based on this flag.
 	 * */
-	pm->vsi_SF = (	   ((pm->vsi_AQ < xSKIP && xA < xSKIP)
-				|| (pm->vsi_AQ == xTOP && xA == xTOP))
-			&& ((pm->vsi_BQ < xSKIP && xB < xSKIP)
-				|| (pm->vsi_BQ == xTOP && xB == xTOP))
-			&& ((pm->vsi_CQ < xSKIP && xC < xSKIP)
-				|| (pm->vsi_CQ == xTOP && xC == xTOP))) ? 0 : 1;
-
-	if (		PM_CONFIG_TVM(pm) == PM_ENABLED
-			&& pm->tvm_ACTIVE == PM_ENABLED) {
-
-		xMIN = (int) (pm->dc_resolution * pm->tvm_clean_zone);
-
-		/* Check if terminal voltages were sampled within acceptable
-		 * zone. The VOLTAGE measurement will be used or rejected based
-		 * on these flags.
-		 * */
-		pm->vsi_UF = (	   pm->vsi_AQ < xMIN
-				&& pm->vsi_BQ < xMIN
-				&& pm->vsi_CQ < xMIN
-				&& xA < xMIN
-				&& xB < xMIN
-				&& xC < xMIN) ? 0 : 1;
-
-		/* Check if terminal voltages are exactly ZERO to get more
-		 * accuracy.
-		 * */
-		pm->vsi_AZ = (pm->vsi_AQ == 0) ? 0 : 1;
-		pm->vsi_BZ = (pm->vsi_BQ == 0) ? 0 : 1;
-		pm->vsi_CZ = (pm->vsi_CQ == 0) ? 0 : 1;
-	}
-	else {
-		pm->vsi_UF = 1;
-	}
+	pm->vsi_UF = (	   ((pm->vsi_A0 < xSKIP && xA < xSKIP)
+				|| (pm->vsi_A0 == xTOP && xA == xTOP))
+			&& ((pm->vsi_B0 < xSKIP && xB < xSKIP)
+				|| (pm->vsi_B0 == xTOP && xB == xTOP))
+			&& ((pm->vsi_C0 < xSKIP && xC < xSKIP)
+				|| (pm->vsi_C0 == xTOP && xC == xTOP))) ? 0 : 1;
 
-	pm->vsi_AQ = xA;
-	pm->vsi_BQ = xB;
-	pm->vsi_CQ = xC;
+	pm->vsi_A0 = xA;
+	pm->vsi_B0 = xB;
+	pm->vsi_C0 = xC;
 }
 
 void pm_voltage(pmc_t *pm, float uX, float uY)
@@ -2353,8 +2320,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_iU;
+	uY *= pm->quick_iU;
 
 	uDC = m_sqrtf(uX * uX + uY * uY);
 
@@ -2425,13 +2392,13 @@ void pm_voltage(pmc_t *pm, float uX, float uY)
 		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);
-			bC = m_fabsf(0.5f * pm->lu_iX + 0.8660254f * pm->lu_iY);
+			bB = m_fabsf(- 0.5f * pm->lu_iX + 0.8660254f * pm->lu_iY);
+			bC = m_fabsf(- 0.5f * pm->lu_iX - 0.8660254f * pm->lu_iY);
 		}
 		else {
 			bA = m_fabsf(pm->lu_iX);
 			bB = m_fabsf(pm->lu_iY);
-			bC = 0.f;
+			bC = m_fabsf(- pm->lu_iX - pm->lu_iY);
 		}
 
 		if (uA < uB) {
@@ -2446,9 +2413,9 @@ void pm_voltage(pmc_t *pm, float uX, float uY)
 
 		bA = pm->fault_current_tol;
 
-		bA = (		   pm->vsi_AQ < pm->dc_resolution
-				&& pm->vsi_BQ < pm->dc_resolution
-				&& pm->vsi_CQ < pm->dc_resolution) ? bA : 0.f;
+		bA = (		   pm->vsi_A0 < pm->dc_resolution
+				&& pm->vsi_B0 < pm->dc_resolution
+				&& pm->vsi_C0 < pm->dc_resolution) ? bA : 0.f;
 
 		uDC = (bMIN + bA < bMAX) ? 1.f - uMAX : 0.f - uMIN;
 	}
@@ -2628,30 +2595,17 @@ void pm_voltage(pmc_t *pm, float uX, float uY)
 		}
 	}
 	else {
-		if (		PM_CONFIG_TVM(pm) == PM_ENABLED
-				&& pm->tvm_ACTIVE == PM_ENABLED) {
-
-			xMIN = (xA < xB) ? (xC < xA) ? xC : xA : (xC < xB) ? xC : xB;
-
-			/* Clamp to minimal.
-			 * */
-			xA += pm->ts_minimal - xMIN;
-			xB += pm->ts_minimal - xMIN;
-			xC += pm->ts_minimal - xMIN;
-		}
-		else {
-			xMIN = (xA < xB) ? (xC < xA) ? xC : xA : (xC < xB) ? xC : xB;
-			xMAX = (xA > xB) ? (xC > xA) ? xC : xA : (xC > xB) ? xC : xB;
+		xMIN = (xA < xB) ? (xC < xA) ? xC : xA : (xC < xB) ? xC : xB;
+		xMAX = (xA > xB) ? (xC > xA) ? xC : xA : (xC > xB) ? xC : xB;
 
-			xMAX = (pm->dc_resolution - (pm->ts_clearance + 1)
-					- (xMAX + xMIN) + pm->ts_minimal) / 2;
+		xMAX = (pm->dc_resolution - (pm->ts_clearance + 1)
+				- (xMAX + xMIN) + pm->ts_minimal) / 2;
 
-			/* Clamp to middle.
-			 * */
-			xA += xMAX;
-			xB += xMAX;
-			xC += xMAX;
-		}
+		/* Clamp to middle.
+		 * */
+		xA += xMAX;
+		xB += xMAX;
+		xC += xMAX;
 
 		xMAX = pm->dc_resolution - (pm->ts_clearance + 1);
 		xMIN = pm->ts_minimal;
@@ -2680,9 +2634,6 @@ void pm_voltage(pmc_t *pm, float uX, float uY)
 	 * */
 	pm->proc_set_DC(xA, xB, xC);
 
-	pm->vsi_X0 = pm->vsi_X;
-	pm->vsi_Y0 = pm->vsi_Y;
-
 	if (PM_CONFIG_NOP(pm) == PM_NOP_THREE_PHASE) {
 
 		uDC = 0.33333333f * (xA + xB + xC);
@@ -2701,7 +2652,7 @@ void pm_voltage(pmc_t *pm, float uX, float uY)
 		pm->vsi_Y = uB;
 	}
 
-	/* Update the measurement clearance flags according to the DC values.
+	/* Update the clearance flags according to the new DC values.
 	 * */
 	pm_clearance(pm, xA, xB, xC);
 }
@@ -2746,29 +2697,36 @@ 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_wP += (wP - pm->watt_drain_wP) * pm->watt_gain_WF;
+	pm->watt_drain_wA = pm->watt_drain_wP * pm->quick_iU;
 
 	/* Traveled distance.
 	 * */
 	pm->watt_traveled = (float) pm->lu_total_revol
-		* pm->const_ld_S / (float) pm->const_Zp;
+		* pm->const_ld_Sm / (float) pm->const_Zp;
 
-	TiH = pm->m_dT * 0.00027777778f;
+	if (likely(m_isfinitef(pm->watt_drain_wA) != 0)) {
 
-	/* Get WATT per HOUR.
-	 * */
-	Wh = pm->watt_drain_wP * TiH;
-	Ah = pm->watt_drain_wA * TiH;
+		TiH = pm->m_dT * 0.00027777778f;
+
+		/* Get WATT per HOUR.
+		 * */
+		Wh = pm->watt_drain_wP * TiH;
+		Ah = pm->watt_drain_wA * TiH;
 
-	if (likely(Wh > 0.f)) {
+		if (likely(Wh > 0.f)) {
 
-		m_rsumf(&pm->watt_consumed_Wh, &pm->watt_rem[0], Wh);
-		m_rsumf(&pm->watt_consumed_Ah, &pm->watt_rem[1], Ah);
+			m_rsumf(&pm->watt_consumed_Wh, &pm->watt_rem[0], Wh);
+			m_rsumf(&pm->watt_consumed_Ah, &pm->watt_rem[1], Ah);
+		}
+		else {
+			m_rsumf(&pm->watt_reverted_Wh, &pm->watt_rem[2], - Wh);
+			m_rsumf(&pm->watt_reverted_Ah, &pm->watt_rem[3], - Ah);
+		}
 	}
 	else {
-		m_rsumf(&pm->watt_reverted_Wh, &pm->watt_rem[2], - Wh);
-		m_rsumf(&pm->watt_reverted_Ah, &pm->watt_rem[3], - Ah);
+		pm->fsm_errno = PM_ERROR_NAN_OPERATION;
+		pm->fsm_state = PM_STATE_HALT;
 	}
 
 	/* Fuel gauge.
@@ -2911,7 +2869,7 @@ pm_loop_current(pmc_t *pm)
 			if (pm->weak_D < - M_EPSILON) {
 
 				eDC = pm->k_EMAX * pm->const_fb_U;
-				wLS = pm->lu_wS * pm->const_im_L2;
+				wLS = pm->lu_wS * pm->const_im_Lq;
 
 				iMAX = eDC * m_fast_recipf(m_fabsf(wLS));
 
@@ -3127,8 +3085,8 @@ pm_loop_current(pmc_t *pm)
 
 	/* Feed forward compensation (L).
 	 * */
-	uD += - pm->lu_wS * pm->const_im_L2 * pm->i_track_Q;
-	uQ += pm->lu_wS * (pm->const_im_L1 * pm->i_track_D + pm->const_lambda);
+	uD += - pm->lu_wS * pm->const_im_Lq * pm->i_track_Q;
+	uQ += pm->lu_wS * (pm->const_im_Ld * pm->i_track_D + pm->const_lambda);
 
 	uMAX = pm->k_UMAX * pm->const_fb_U;
 
@@ -3165,7 +3123,7 @@ pm_loop_current(pmc_t *pm)
 		 * */
 		pm_hfi_wave(pm);
 
-		uHF = pm->hfi_sine * pm->quick_HFwS * pm->const_im_L1;
+		uHF = pm->hfi_sine * pm->quick_HFwS * pm->const_im_Ld;
 
 		/* HF voltage injection.
 		 * */
@@ -3280,9 +3238,74 @@ pm_loop_location(pmc_t *pm)
 	pm->s_setpoint_speed = wSP;
 }
 
+static void
+pm_dtc_voltage(pmc_t *pm)
+{
+	float		iA, iB, iC, uA, uB, uC, DTu;
+
+	if (PM_CONFIG_NOP(pm) == PM_NOP_THREE_PHASE) {
+
+		iA = pm->lu_iX;
+		iB = - 0.5f * pm->lu_iX + 0.8660254f * pm->lu_iY;
+		iC = - 0.5f * pm->lu_iX - 0.8660254f * pm->lu_iY;
+	}
+	else {
+		iA = pm->lu_iX;
+		iB = pm->lu_iY;
+		iC = - pm->lu_iX - pm->lu_iY;
+	}
+
+	DTu = pm->dtc_deadband * 1.E-9f * pm->m_freq * pm->const_fb_U;
+
+	if (likely(		pm->vsi_A0 != pm->dc_resolution
+				&& pm->vsi_A0 != 0)) {
+
+		uA = (iA > pm->dtc_tol) ? DTu : (iA < - pm->dtc_tol) ? - DTu : 0.f;
+	}
+	else {
+		uA = 0.f;
+	}
+
+	if (likely(		pm->vsi_B0 != pm->dc_resolution
+				&& pm->vsi_B0 != 0)) {
+
+		uB = (iB > pm->dtc_tol) ? DTu : (iB < - pm->dtc_tol) ? - DTu : 0.f;
+	}
+	else {
+		uB = 0.f;
+	}
+
+	if (likely(		pm->vsi_C0 != pm->dc_resolution
+				&& pm->vsi_C0 != 0)) {
+
+		uC = (iC > pm->dtc_tol) ? DTu : (iC < - pm->dtc_tol) ? - DTu : 0.f;
+	}
+	else {
+		uC = 0.f;
+	}
+
+	if (PM_CONFIG_NOP(pm) == PM_NOP_THREE_PHASE) {
+
+		uC = 0.33333333f * (uA + uB + uC);
+
+		uA = uA - uC;
+		uB = uB - uC;
+
+		pm->dtc_uX = uA;
+		pm->dtc_uY = 0.57735027f * uA + 1.1547005f * uB;
+	}
+	else {
+		uA = uA - uC;
+		uB = uB - uC;
+
+		pm->dtc_uX = uA;
+		pm->dtc_uY = uB;
+	}
+}
+
 void pm_feedback(pmc_t *pm, pmfb_t *fb)
 {
-	float		vA, vB, vC, Q;
+	float		iA, iB, Q;
 
 	if (likely(pm->vsi_AF == 0)) {
 
@@ -3331,11 +3354,11 @@ void pm_feedback(pmc_t *pm, pmfb_t *fb)
 
 			Q = 0.33333333f * (pm->fb_iA + pm->fb_iB + pm->fb_iC);
 
-			vA = pm->fb_iA - Q;
-			vB = pm->fb_iB - Q;
+			iA = pm->fb_iA - Q;
+			iB = pm->fb_iB - Q;
 
-			pm->lu_iX = vA;
-			pm->lu_iY = 0.57735027f * vA + 1.1547005f * vB;
+			pm->lu_iX = iA;
+			pm->lu_iY = 0.57735027f * iA + 1.1547005f * iB;
 		}
 		else if (	   pm->vsi_AF == 0
 				&& pm->vsi_BF == 0) {
@@ -3363,11 +3386,11 @@ void pm_feedback(pmc_t *pm, pmfb_t *fb)
 
 			Q = 0.33333333f * (pm->fb_iA + pm->fb_iB + pm->fb_iC);
 
-			vA = pm->fb_iA - Q;
-			vB = pm->fb_iB - Q;
+			iA = pm->fb_iA - Q;
+			iB = pm->fb_iB - Q;
 
-			pm->lu_iX = vA;
-			pm->lu_iY = vB;
+			pm->lu_iX = iA;
+			pm->lu_iY = iB;
 		}
 		else if (	   pm->vsi_AF == 0
 				&& pm->vsi_BF == 0) {
@@ -3389,12 +3412,12 @@ void pm_feedback(pmc_t *pm, pmfb_t *fb)
 		}
 	}
 
-	if (likely(pm->vsi_SF == 0)) {
+	if (likely(pm->vsi_UF == 0)) {
 
 		/* 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_iU = 1.f / pm->const_fb_U;
 
 		if (unlikely(		pm->const_fb_U > pm->fault_voltage_halt
 					&& pm->weak_D > - M_EPSILON)) {
@@ -3404,58 +3427,13 @@ void pm_feedback(pmc_t *pm, pmfb_t *fb)
 		}
 	}
 
-	pm->tvm_X0 = pm->vsi_X0;
-	pm->tvm_Y0 = pm->vsi_Y0;
-
 	if (PM_CONFIG_TVM(pm) == PM_ENABLED) {
 
-		vA = pm->fb_uA;
-		vB = pm->fb_uB;
-		vC = pm->fb_uC;
-
 		/* Get instant terminal voltages.
 		 * */
 		pm->fb_uA = pm->scale_uA[1] * fb->voltage_A + pm->scale_uA[0];
 		pm->fb_uB = pm->scale_uB[1] * fb->voltage_B + pm->scale_uB[0];
 		pm->fb_uC = pm->scale_uC[1] * fb->voltage_C + pm->scale_uC[0];
-
-		if (		pm->lu_MODE != PM_LU_DETACHED
-				&& pm->vsi_UF == 0) {
-
-			/* Extract the average terminal voltages using FIR.
-			 * */
-
-			vA = (pm->vsi_AZ != 0) ? pm->tvm_FIR_A[0] * pm->fb_uA
-				+ pm->tvm_FIR_A[1] * vA + pm->tvm_FIR_A[2] : 0.f;
-
-			vB = (pm->vsi_BZ != 0) ? pm->tvm_FIR_B[0] * pm->fb_uB
-				+ pm->tvm_FIR_B[1] * vB + pm->tvm_FIR_B[2] : 0.f;
-
-			vC = (pm->vsi_CZ != 0) ? pm->tvm_FIR_C[0] * pm->fb_uC
-				+ pm->tvm_FIR_C[1] * vC + pm->tvm_FIR_C[2] : 0.f;
-
-			pm->tvm_A = vA;
-			pm->tvm_B = vB;
-			pm->tvm_C = vC;
-
-			if (PM_CONFIG_NOP(pm) == PM_NOP_THREE_PHASE) {
-
-				Q = .33333333f * (vA + vB + vC);
-
-				vA = vA - Q;
-				vB = vB - Q;
-
-				pm->tvm_X0 = vA;
-				pm->tvm_Y0 = 0.57735027f * vA + 1.1547005f * vB;
-			}
-			else {
-				vA = vA - vC;
-				vB = vB - vC;
-
-				pm->tvm_X0 = vA;
-				pm->tvm_Y0 = vB;
-			}
-		}
 	}
 
 	pm->fb_SIN = fb->analog_SIN;
@@ -3463,6 +3441,19 @@ void pm_feedback(pmc_t *pm, pmfb_t *fb)
 	pm->fb_HS = fb->pulse_HS;
 	pm->fb_EP = fb->pulse_EP;
 
+	if (		pm->config_DTC_VOLTAGE == PM_ENABLED
+			&& pm->lu_MODE != PM_LU_DETACHED) {
+
+		pm_dtc_voltage(pm);
+	}
+	else {
+		pm->dtc_uX = 0.f;
+		pm->dtc_uY = 0.f;
+	}
+
+	pm->dtc_X = pm->vsi_X - pm->dtc_uX;
+	pm->dtc_Y = pm->vsi_Y - pm->dtc_uY;
+
 	if (pm->lu_MODE != PM_LU_DISABLED) {
 
 		/* The observer FSM.
@@ -3498,7 +3489,7 @@ void pm_feedback(pmc_t *pm, pmfb_t *fb)
 
 				if (likely(pm->vsi_IF == 0)) {
 
-					pm_kalman_update(pm, pm->flux_X);
+					pm_kalman_update(pm);
 				}
 
 				pm->kalman_POSTPONED = PM_DISABLED;
@@ -3518,12 +3509,6 @@ void pm_feedback(pmc_t *pm, pmfb_t *fb)
 
 			pm->dbg_flux_rsu = m_atan2f(B, A) * (180.f / M_PI_F);
 		}
-
-		if (unlikely(m_isfinitef(pm->lu_F[0]) == 0)) {
-
-			pm->fsm_errno = PM_ERROR_INVALID_OPERATION;
-			pm->fsm_state = PM_STATE_HALT;
-		}
 	}
 
 	/* The FSM is used to execute assistive routines.
diff --git a/src/phobia/pm.h b/src/phobia/pm.h
index f285d3d..1fa6f2c 100644
--- a/src/phobia/pm.h
+++ b/src/phobia/pm.h
@@ -25,13 +25,6 @@ enum {
 	PM_Z_ABC
 };
 
-enum {
-	PM_MASK_NONE				= 0,
-	PM_MASK_A,
-	PM_MASK_B,
-	PM_MASK_C,
-};
-
 enum {
 	PM_NOP_THREE_PHASE			= 0,
 	PM_NOP_TWO_PHASE
@@ -132,8 +125,9 @@ enum {
 	PM_STATE_SELF_TEST_BOOTSTRAP,
 	PM_STATE_SELF_TEST_POWER_STAGE,
 	PM_STATE_SELF_TEST_CLEARANCE,
-	PM_STATE_ADJUST_VOLTAGE,
-	PM_STATE_ADJUST_CURRENT,
+	PM_STATE_ADJUST_ON_PCB_VOLTAGE,
+	PM_STATE_ADJUST_ON_PCB_CURRENT,
+	PM_STATE_ADJUST_DTC_VOLTAGE,
 	PM_STATE_PROBE_CONST_RESISTANCE,
 	PM_STATE_PROBE_CONST_INDUCTANCE,
 	PM_STATE_LU_DETACHED,
@@ -171,12 +165,12 @@ enum {
 	PM_ERROR_NO_MOTOR_CONNECTED,
 	PM_ERROR_BOOTSTRAP_FAULT,
 	PM_ERROR_POWER_STAGE_BROKEN,
-	PM_ERROR_INSUFFICIENT_ACCURACY,
+	PM_ERROR_LOW_ACCURACY,
 	PM_ERROR_CURRENT_LOOP_FAULT,
 	PM_ERROR_INSTANT_OVERCURRENT,
 	PM_ERROR_DC_LINK_OVERVOLTAGE,
 	PM_ERROR_UNCERTAIN_RESULT,
-	PM_ERROR_INVALID_OPERATION,
+	PM_ERROR_NAN_OPERATION,
 	PM_ERROR_SENSOR_HALL_FAULT,
 	PM_ERROR_SENSOR_EABI_FAULT,
 
@@ -239,7 +233,9 @@ typedef struct {
 	int		self_IST[8];
 	float		self_STDi[3];
 	float		self_RMSi[3];
-	float		self_RMSu[4];
+	float		self_RMSu;
+	float		self_RMSt[3];
+	float		self_DTu;
 
 	int		config_NOP;
 	int		config_IFB;
@@ -248,6 +244,7 @@ typedef struct {
 
 	int		config_VSI_ZERO;
 	int		config_VSI_CLAMP;
+	int		config_DTC_VOLTAGE;
 	int		config_LU_FORCED;
 	int		config_LU_FREEWHEEL;
 	int		config_LU_ESTIMATE;
@@ -321,13 +318,10 @@ typedef struct {
 
 	float		probe_DFT[8];
 	float		probe_REM[8];
-	float		probe_SC[2];
-	float		probe_HF_residual;
-	float		probe_HF_integral;
+	float		probe_WS[2];
+	float		probe_HF[2];
 	float		probe_gain_LP;
-	float		probe_TEMP[2];
-
-	lse_t		probe_lse[3];
+	float		probe_HOLD[2];
 
 	float		fault_voltage_tol;
 	float		fault_current_tol;
@@ -340,40 +334,28 @@ typedef struct {
 	float		vsi_lpf_DC;
 	float		vsi_X;
 	float		vsi_Y;
-	float		vsi_X0;
-	float		vsi_Y0;
 	float		vsi_gain_LP;
-	int		vsi_mask_XF;
 
 	int		vsi_AF;
 	int		vsi_BF;
 	int		vsi_CF;
 	int		vsi_IF;
-	int		vsi_SF;
 	int		vsi_UF;
-	int		vsi_AZ;
-	int		vsi_BZ;
-	int		vsi_CZ;
 	int		vsi_AT;
 	int		vsi_BT;
 	int		vsi_CT;
-	int		vsi_AQ;
-	int		vsi_BQ;
-	int		vsi_CQ;
-
-	int		tvm_ACTIVE;
-	float		tvm_clean_zone;
-	float		tvm_A;
-	float		tvm_B;
-	float		tvm_C;
-	float		tvm_FIR_A[3];
-	float		tvm_FIR_B[3];
-	float		tvm_FIR_C[3];
-	float		tvm_X0;
-	float		tvm_Y0;
+	int		vsi_A0;
+	int		vsi_B0;
+	int		vsi_C0;
 
-	int		lu_MODE;
+	float		dtc_deadband;
+	float		dtc_tol;
+	float		dtc_uX;
+	float		dtc_uY;
+	float		dtc_X;
+	float		dtc_Y;
 
+	int		lu_MODE;
 	float		lu_iX;
 	float		lu_iY;
 	float		lu_iD;
@@ -419,9 +401,9 @@ typedef struct {
 	int		flux_ZONE;
 
 	float		flux_X[2];
-	float		flux_lambda;
 	float		flux_F[2];
 	float		flux_wS;
+	float		flux_lambda;
 	float		flux_trip_tol;
 	float		flux_gain_IN;
 	float		flux_gain_LO;
@@ -434,11 +416,10 @@ typedef struct {
 	float		kalman_P[15];
 	float		kalman_A[10];
 	float		kalman_K[10];
-	float		kalman_residual_D;
-	float		kalman_residual_Q;
+	float		kalman_E[2];
 	float		kalman_bias_Q;
 	float		kalman_lpf_wS;
-	float		kalman_gain_Q[5];
+	float		kalman_gain_Q[4];
 	float		kalman_gain_R;
 
 	float		zone_noise;
@@ -451,8 +432,6 @@ typedef struct {
 	float		hfi_sine;
 	float		hfi_wave[2];
 
-	m_seed_t	hfi_seed;
-
 	struct {
 
 		float	X;
@@ -504,23 +483,24 @@ typedef struct {
 	float		const_Rs;
 	int		const_Zp;
 	float		const_Ja;
-	float		const_im_L1;
-	float		const_im_L2;
-	float		const_im_B;
-	float		const_im_R;
-	float		const_ld_S;
+	float		const_im_Ld;
+	float		const_im_Lq;
+	float		const_im_A;
+	float		const_im_Rz;
+	float		const_ld_Sm;
 
-	float		quick_iUDC;
+	float		quick_iU;
 	float		quick_iWb;
 	float		quick_iWb2;
-	float		quick_iL1;
-	float		quick_iL2;
-	float		quick_TiL1;
-	float		quick_TiL2;
+	float		quick_iLd;
+	float		quick_iLq;
+	float		quick_TiLd;
+	float		quick_TiLq;
+	float		quick_TiLu[4];
+	float		quick_WiL4;
 	float		quick_HFwS;
 	float		quick_ZiEP;
 	float		quick_ZiSQ;
-	float		quick_WiL4;
 
 	int		watt_DC_MAX;
 	int		watt_DC_MIN;
@@ -548,6 +528,7 @@ typedef struct {
 	float		watt_gain_P;
 	float		watt_gain_I;
 	float		watt_gain_LP;
+	float		watt_gain_WF;
 
 	float		i_setpoint_current;
 	float		i_maximal;
@@ -604,6 +585,9 @@ typedef struct {
 
 	void 		(* proc_set_DC) (int, int, int);
 	void 		(* proc_set_Z) (int);
+
+	lfseed_t	lfseed;
+	lse_t		lse[2];
 }
 pmc_t;
 
diff --git a/src/phobia/pm_fsm.c b/src/phobia/pm_fsm.c
index cc4609b..762856f 100644
--- a/src/phobia/pm_fsm.c
+++ b/src/phobia/pm_fsm.c
@@ -10,7 +10,7 @@ pm_fsm_state_idle(pmc_t *pm)
 static void
 pm_fsm_state_zero_drift(pmc_t *pm)
 {
-	lse_t			*ls = &pm->probe_lse[0];
+	lse_t			*ls = &pm->lse[0];
 	lse_float_t		v[4];
 
 	switch (pm->fsm_phase) {
@@ -79,7 +79,7 @@ pm_fsm_state_zero_drift(pmc_t *pm)
 						|| pm->self_STDi[1] > pm->fault_current_tol
 						|| pm->self_STDi[2] > pm->fault_current_tol) {
 
-					pm->fsm_errno = PM_ERROR_INSUFFICIENT_ACCURACY;
+					pm->fsm_errno = PM_ERROR_LOW_ACCURACY;
 				}
 			}
 			else {
@@ -98,10 +98,9 @@ pm_fsm_state_self_test_bootstrap(pmc_t *pm)
 	switch (pm->fsm_phase) {
 
 		case 0:
-			pm->fsm_errno = PM_OK;
-
 			if (PM_CONFIG_TVM(pm) == PM_ENABLED) {
 
+				pm->fsm_errno = PM_OK;
 				pm->fsm_phase = 1;
 			}
 			else {
@@ -277,7 +276,7 @@ pm_fsm_state_self_test_bootstrap(pmc_t *pm)
 static void
 pm_fsm_state_self_test_power_stage(pmc_t *pm)
 {
-	lse_t			*ls = &pm->probe_lse[0];
+	lse_t			*ls = &pm->lse[0];
 	lse_float_t		v[4];
 
 	float			uA, uB, uC, tol;
@@ -286,12 +285,12 @@ pm_fsm_state_self_test_power_stage(pmc_t *pm)
 	switch (pm->fsm_phase) {
 
 		case 0:
-			pm->fsm_errno = PM_OK;
-
 			if (PM_CONFIG_TVM(pm) == PM_ENABLED) {
 
-				pm->fsm_phase = 1;
 				pm->fsm_subi = 0;
+
+				pm->fsm_errno = PM_OK;
+				pm->fsm_phase = 1;
 			}
 			else {
 				pm->fsm_state = PM_STATE_HALT;
@@ -434,7 +433,7 @@ pm_fsm_state_self_test_power_stage(pmc_t *pm)
 static void
 pm_fsm_state_self_test_clearance(pmc_t *pm)
 {
-	lse_t			*ls = &pm->probe_lse[0];
+	lse_t			*ls = &pm->lse[0];
 	lse_float_t		v[8];
 
 	switch (pm->fsm_phase) {
@@ -505,31 +504,41 @@ pm_fsm_state_self_test_clearance(pmc_t *pm)
 			pm->self_RMSi[0] = m_sqrtf(ls->sol.m[0] * ls->sol.m[0] + ls->std.m[0] * ls->std.m[0]);
 			pm->self_RMSi[1] = m_sqrtf(ls->sol.m[1] * ls->sol.m[1] + ls->std.m[1] * ls->std.m[1]);
 			pm->self_RMSi[2] = m_sqrtf(ls->sol.m[2] * ls->sol.m[2] + ls->std.m[2] * ls->std.m[2]);
-			pm->self_RMSu[0] = ls->std.m[3];
-			pm->self_RMSu[1] = ls->std.m[4];
-			pm->self_RMSu[2] = ls->std.m[5];
-			pm->self_RMSu[3] = ls->std.m[6];
+
+			pm->self_RMSu = ls->std.m[3];
+
+			if (PM_CONFIG_TVM(pm) == PM_ENABLED) {
+
+				pm->self_RMSt[0] = ls->std.m[4];
+				pm->self_RMSt[1] = ls->std.m[5];
+				pm->self_RMSt[2] = ls->std.m[6];
+			}
+			else {
+				pm->self_RMSt[0] = 0.f;
+				pm->self_RMSt[1] = 0.f;
+				pm->self_RMSt[2] = 0.f;
+			}
 
 			if (		   m_isfinitef(pm->self_RMSi[0]) != 0
 					&& m_isfinitef(pm->self_RMSi[1]) != 0
 					&& m_isfinitef(pm->self_RMSi[2]) != 0
-					&& m_isfinitef(pm->self_RMSu[0]) != 0
-					&& m_isfinitef(pm->self_RMSu[1]) != 0
-					&& m_isfinitef(pm->self_RMSu[2]) != 0
-					&& m_isfinitef(pm->self_RMSu[3]) != 0) {
+					&& m_isfinitef(pm->self_RMSu) != 0
+					&& m_isfinitef(pm->self_RMSt[0]) != 0
+					&& m_isfinitef(pm->self_RMSt[1]) != 0
+					&& m_isfinitef(pm->self_RMSt[2]) != 0) {
 
 				if (		   pm->self_RMSi[0] > pm->fault_current_tol
 						|| pm->self_RMSi[1] > pm->fault_current_tol
 						|| pm->self_RMSi[2] > pm->fault_current_tol) {
 
-					pm->fsm_errno = PM_ERROR_INSUFFICIENT_ACCURACY;
+					pm->fsm_errno = PM_ERROR_LOW_ACCURACY;
 				}
-				else if (	   pm->self_RMSu[0] > pm->fault_voltage_tol
-						|| pm->self_RMSu[1] > pm->fault_voltage_tol
-						|| pm->self_RMSu[2] > pm->fault_voltage_tol
-						|| pm->self_RMSu[3] > pm->fault_voltage_tol) {
+				else if (	   pm->self_RMSu > pm->fault_voltage_tol
+						|| pm->self_RMSt[0] > pm->fault_voltage_tol
+						|| pm->self_RMSt[1] > pm->fault_voltage_tol
+						|| pm->self_RMSt[2] > pm->fault_voltage_tol) {
 
-					pm->fsm_errno = PM_ERROR_INSUFFICIENT_ACCURACY;
+					pm->fsm_errno = PM_ERROR_LOW_ACCURACY;
 				}
 			}
 			else {
@@ -543,26 +552,24 @@ pm_fsm_state_self_test_clearance(pmc_t *pm)
 }
 
 static void
-pm_fsm_state_adjust_voltage(pmc_t *pm)
+pm_fsm_state_adjust_on_pcb_voltage(pmc_t *pm)
 {
-	float			*rem = pm->probe_REM;
-	lse_t			*ls = &pm->probe_lse[0];
+	lse_t			*ls = &pm->lse[0];
 	lse_float_t		v[5];
 
-	int			xDC, xMIN, xMAX;
-	float			REF;
+	int			xDC;
 
 	switch (pm->fsm_phase) {
 
 		case 0:
-			pm->fsm_errno = PM_OK;
-
 			if (PM_CONFIG_TVM(pm) == PM_ENABLED) {
 
 				lse_construct(ls, LSE_CASCADE_MAX, 2, 3);
 
-				pm->fsm_phase = 1;
 				pm->fsm_subi = 0;
+
+				pm->fsm_errno = PM_OK;
+				pm->fsm_phase = 1;
 			}
 			else {
 				pm->fsm_state = PM_STATE_HALT;
@@ -638,8 +645,6 @@ pm_fsm_state_adjust_voltage(pmc_t *pm)
 				ls->sol.m[5] = pm->scale_uC[1] / ls->sol.m[5];
 			}
 			else {
-				pm->tvm_ACTIVE = PM_DISABLED;
-
 				pm->fsm_errno = PM_ERROR_UNCERTAIN_RESULT;
 				pm->fsm_state = PM_STATE_HALT;
 				pm->fsm_phase = 0;
@@ -650,9 +655,7 @@ pm_fsm_state_adjust_voltage(pmc_t *pm)
 					|| m_fabsf(ls->sol.m[2]) > pm->fault_voltage_tol
 					|| m_fabsf(ls->sol.m[4]) > pm->fault_voltage_tol) {
 
-				pm->tvm_ACTIVE = PM_DISABLED;
-
-				pm->fsm_errno = PM_ERROR_INSUFFICIENT_ACCURACY;
+				pm->fsm_errno = PM_ERROR_LOW_ACCURACY;
 				pm->fsm_state = PM_STATE_HALT;
 				pm->fsm_phase = 0;
 				break;
@@ -662,9 +665,7 @@ pm_fsm_state_adjust_voltage(pmc_t *pm)
 					|| m_fabsf(ls->sol.m[3] - 1.f) > pm->fault_accuracy_tol
 					|| m_fabsf(ls->sol.m[5] - 1.f) > pm->fault_accuracy_tol) {
 
-				pm->tvm_ACTIVE = PM_DISABLED;
-
-				pm->fsm_errno = PM_ERROR_INSUFFICIENT_ACCURACY;
+				pm->fsm_errno = PM_ERROR_LOW_ACCURACY;
 				pm->fsm_state = PM_STATE_HALT;
 				pm->fsm_phase = 0;
 				break;
@@ -677,119 +678,11 @@ pm_fsm_state_adjust_voltage(pmc_t *pm)
 			pm->scale_uC[0] = ls->sol.m[4];
 			pm->scale_uC[1] = ls->sol.m[5];
 
-			lse_construct(&pm->probe_lse[0], LSE_CASCADE_MAX, 3, 1);
-			lse_construct(&pm->probe_lse[1], LSE_CASCADE_MAX, 3, 1);
-			lse_construct(&pm->probe_lse[2], LSE_CASCADE_MAX, 3, 1);
-
-			pm->tm_value = 0;
-			pm->tm_end = PM_TSMS(pm, pm->tm_average_probe);
-
-			pm->fsm_phase = 5;
-			break;
-
-		case 5:
-			xMIN = pm->ts_minimal;
-			xMAX = (int) (pm->dc_resolution * pm->tvm_clean_zone);
-
-			REF = m_lf_urandf(&pm->hfi_seed) * 0.5f + 0.5f;
-			xDC = xMIN + (int) (REF * (float) (xMAX - xMIN));
-
-			pm->proc_set_DC(xDC, xDC, xDC);
-
-			if (pm->tm_value >= 2) {
-
-				REF = rem[0] * pm->const_fb_U * pm->ts_inverted;
-
-				v[0] = pm->fb_uA;
-				v[1] = rem[2];
-				v[2] = 1.f;
-				v[3] = REF;
-
-				lse_insert(&pm->probe_lse[0], v);
-
-				v[0] = pm->fb_uB;
-				v[1] = rem[3];
-				v[2] = 1.f;
-				v[3] = REF;
-
-				lse_insert(&pm->probe_lse[1], v);
-
-				v[0] = pm->fb_uC;
-				v[1] = rem[4];
-				v[2] = 1.f;
-				v[3] = REF;
-
-				lse_insert(&pm->probe_lse[2], v);
-			}
-
-			rem[0] = rem[1];
-			rem[1] = (float) xDC;
-			rem[2] = pm->fb_uA;
-			rem[3] = pm->fb_uB;
-			rem[4] = pm->fb_uC;
-
-			pm->tm_value++;
-
-			if (pm->tm_value >= pm->tm_end) {
-
-				pm->fsm_phase += 1;
-			}
-			break;
-
-		case 6:
-			lse_solve(&pm->probe_lse[0]);
-			lse_solve(&pm->probe_lse[1]);
-			lse_solve(&pm->probe_lse[2]);
-
-			pm->tvm_FIR_A[0] = pm->probe_lse[0].sol.m[0];
-			pm->tvm_FIR_A[1] = pm->probe_lse[0].sol.m[1];
-			pm->tvm_FIR_A[2] = pm->probe_lse[0].sol.m[2];
-
-			pm->tvm_FIR_B[0] = pm->probe_lse[1].sol.m[0];
-			pm->tvm_FIR_B[1] = pm->probe_lse[1].sol.m[1];
-			pm->tvm_FIR_B[2] = pm->probe_lse[1].sol.m[2];
-
-			pm->tvm_FIR_C[0] = pm->probe_lse[2].sol.m[0];
-			pm->tvm_FIR_C[1] = pm->probe_lse[2].sol.m[1];
-			pm->tvm_FIR_C[2] = pm->probe_lse[2].sol.m[2];
-
-			pm->fsm_phase += 1;
-			break;
-
-		case 7:
-			lse_std(&pm->probe_lse[0]);
-			lse_std(&pm->probe_lse[1]);
-			lse_std(&pm->probe_lse[2]);
-
-			pm->self_RMSu[1] = pm->probe_lse[0].std.m[0];
-			pm->self_RMSu[2] = pm->probe_lse[1].std.m[0];
-			pm->self_RMSu[3] = pm->probe_lse[2].std.m[0];
-
-			if (		   m_isfinitef(pm->tvm_FIR_A[0]) != 0
-					&& m_isfinitef(pm->tvm_FIR_A[1]) != 0
-					&& m_isfinitef(pm->tvm_FIR_A[2]) != 0
-					&& m_isfinitef(pm->tvm_FIR_B[0]) != 0
-					&& m_isfinitef(pm->tvm_FIR_B[1]) != 0
-					&& m_isfinitef(pm->tvm_FIR_B[2]) != 0
-					&& m_isfinitef(pm->tvm_FIR_C[0]) != 0
-					&& m_isfinitef(pm->tvm_FIR_C[1]) != 0
-					&& m_isfinitef(pm->tvm_FIR_C[2]) != 0) {
-
-				if (		   pm->self_RMSu[1] < pm->fault_terminal_tol
-						&& pm->self_RMSu[2] < pm->fault_terminal_tol
-						&& pm->self_RMSu[3] < pm->fault_terminal_tol) {
-
-					pm->tvm_ACTIVE = PM_ENABLED;
-				}
-				else {
-					pm->tvm_ACTIVE = PM_DISABLED;
-				}
-			}
-			else {
-				pm->tvm_ACTIVE = PM_DISABLED;
+			lse_std(ls);
 
-				pm->fsm_errno = PM_ERROR_UNCERTAIN_RESULT;
-			}
+			pm->self_RMSt[0] = ls->std.m[0];
+			pm->self_RMSt[1] = ls->std.m[1];
+			pm->self_RMSt[2] = ls->std.m[2];
 
 			pm->fsm_state = PM_STATE_HALT;
 			pm->fsm_phase = 0;
@@ -798,10 +691,10 @@ pm_fsm_state_adjust_voltage(pmc_t *pm)
 }
 
 static void
-pm_fsm_state_adjust_current(pmc_t *pm)
+pm_fsm_state_adjust_on_pcb_current(pmc_t *pm)
 {
-	lse_t			*ls = &pm->probe_lse[0];
-	lse_t			*lb = &pm->probe_lse[1];
+	lse_t			*ls = &pm->lse[0];
+	lse_t			*lb = &pm->lse[1];
 	lse_float_t		v[4];
 
 	float			eA, uA, uMAX, iA, REF;
@@ -909,7 +802,7 @@ pm_fsm_state_adjust_current(pmc_t *pm)
 
 		case 2:
 		case 3:
-			if (pm->fsm_phase == 2) {
+			if (pm->fsm_phase < 3) {
 
 				pm->i_track_D = (float) (pm->tm_value + 1)
 					* pm->probe_current_hold / (float) pm->tm_end;
@@ -943,7 +836,7 @@ pm_fsm_state_adjust_current(pmc_t *pm)
 
 			if (pm->tm_value >= pm->tm_end) {
 
-				if (pm->fsm_phase == 2) {
+				if (pm->fsm_phase < 3) {
 
 					pm->tm_value = 0;
 					pm->tm_end = PM_TSMS(pm, pm->tm_current_hold);
@@ -1069,7 +962,7 @@ pm_fsm_state_adjust_current(pmc_t *pm)
 					|| m_fabsf(pm->scale_iB[1] - 1.f) > pm->fault_accuracy_tol
 					|| m_fabsf(pm->scale_iC[1] - 1.f) > pm->fault_accuracy_tol) {
 
-				pm->fsm_errno = PM_ERROR_INSUFFICIENT_ACCURACY;
+				pm->fsm_errno = PM_ERROR_LOW_ACCURACY;
 			}
 
 			pm->fsm_state = PM_STATE_HALT;
@@ -1081,7 +974,7 @@ pm_fsm_state_adjust_current(pmc_t *pm)
 static void
 pm_fsm_probe_impedance_DFT(pmc_t *pm, float la[5])
 {
-	lse_t		*ls = &pm->probe_lse[0];
+	lse_t		*ls = &pm->lse[0];
 	lse_float_t	v[5];
 
 	float		*DFT = pm->probe_DFT;
@@ -1177,13 +1070,13 @@ pm_fsm_probe_loop_current(pmc_t *pm, float track_HF)
 
 		eHF = track_HF - m_sqrtf(eD * eD + eQ * eQ);
 
-		pm->probe_HF_residual += (eHF - pm->probe_HF_residual) * pm->probe_gain_LP;
+		pm->probe_HF[0] += (eHF - pm->probe_HF[0]) * pm->probe_gain_LP;
 
-		pm->probe_HF_integral += pm->probe_gain_I * pm->probe_HF_residual;
-		pm->probe_HF_integral = (pm->probe_HF_integral > uMAX) ? uMAX
-			: (pm->probe_HF_integral < - uMAX) ? - uMAX : pm->probe_HF_integral;
+		pm->probe_HF[1] += pm->probe_gain_I * pm->probe_HF[0];
+		pm->probe_HF[1] = (pm->probe_HF[1] > uMAX) ? uMAX
+			: (pm->probe_HF[1] < - uMAX) ? - uMAX : pm->probe_HF[1];
 
-		uHF = pm->probe_gain_P * pm->probe_HF_residual + pm->probe_HF_integral;
+		uHF = pm->probe_gain_P * pm->probe_HF[0] + pm->probe_HF[1];
 	}
 
 	pm->i_integral_D += pm->probe_gain_I * eD;
@@ -1222,10 +1115,227 @@ pm_fsm_probe_loop_current(pmc_t *pm, float track_HF)
 	pm_voltage(pm, uD, uQ);
 }
 
+static void
+pm_fsm_state_adjust_dtc_voltage(pmc_t *pm)
+{
+	lse_t			*ls = &pm->lse[0];
+	lse_float_t		v[5];
+
+	float			hold_A, ramp_A, Rz;
+
+	switch (pm->fsm_phase) {
+
+		case 0:
+			if (pm->config_DTC_VOLTAGE == PM_ENABLED) {
+
+				pm->proc_set_DC(0, 0, 0);
+				pm->proc_set_Z(PM_Z_NONE);
+
+				lse_construct(ls, LSE_CASCADE_MAX, 4, 1);
+
+				pm->probe_HF[0] = 0.f;
+				pm->probe_HF[1] = 0.f;
+
+				pm->probe_WS[0] = 1.f / pm->dtc_deadband;
+
+				pm->fsm_subi = 0;
+
+				pm->fsm_errno = PM_OK;
+				pm->fsm_phase = 1;
+			}
+			else {
+				pm->fsm_state = PM_STATE_HALT;
+				pm->fsm_phase = 0;
+			}
+			break;
+
+		case 1:
+			switch (pm->fsm_subi) {
+
+				case 0:
+					hold_A = 0.f;
+					break;
+
+				case 1:
+					hold_A = 120.f * M_PI_F / 180.f;
+					break;
+
+				case 2:
+					hold_A = - 120.f * M_PI_F / 180.f;
+					break;
+			}
+
+			pm->probe_HOLD[0] = m_cosf(hold_A);
+			pm->probe_HOLD[1] = m_sinf(hold_A);
+
+			pm->i_integral_D = 0.f;
+			pm->i_integral_Q = 0.f;
+
+			pm->tm_value = 0;
+			pm->tm_end = PM_TSMS(pm, pm->tm_current_ramp);
+
+			pm->fsm_phase += 1;
+			break;
+
+		case 2:
+			ramp_A = (float) (pm->tm_value + 1)
+				* pm->probe_current_hold / (float) pm->tm_end;
+
+			pm->i_track_D = pm->probe_HOLD[0] * ramp_A;
+			pm->i_track_Q = pm->probe_HOLD[1] * ramp_A;
+
+		case 3:
+			pm_fsm_probe_loop_current(pm, 0.f);
+
+			if (pm->fsm_errno != PM_OK)
+				break;
+
+			pm->tm_value++;
+
+			if (pm->tm_value >= pm->tm_end) {
+
+				if (pm->fsm_phase < 3) {
+
+					pm->tm_value = 0;
+					pm->tm_end = PM_TSMS(pm, pm->tm_current_hold);
+				}
+				else {
+					pm->tm_value = 0;
+					pm->tm_end = PM_TSMS(pm, pm->tm_average_probe);
+				}
+
+				pm->fsm_phase += 1;
+			}
+			break;
+
+		case 4:
+		case 6:
+			v[0] = (pm->fsm_subi == 0) ? pm->lu_iX : 0.f;
+			v[1] = (pm->fsm_subi == 1) ? pm->lu_iX : 0.f;
+			v[2] = (pm->fsm_subi == 2) ? pm->lu_iX : 0.f;
+			v[3] = pm->dtc_uX * pm->probe_WS[0];
+			v[4] = pm->vsi_X;
+
+			lse_insert(ls, v);
+
+			v[0] = (pm->fsm_subi == 0) ? pm->lu_iY : 0.f;
+			v[1] = (pm->fsm_subi == 1) ? pm->lu_iY : 0.f;
+			v[2] = (pm->fsm_subi == 2) ? pm->lu_iY : 0.f;
+			v[3] = pm->dtc_uY * pm->probe_WS[0];
+			v[4] = pm->vsi_Y;
+
+			lse_insert(ls, v);
+
+		case 5:
+			pm_fsm_probe_loop_current(pm, 0.f);
+
+			if (pm->fsm_errno != PM_OK)
+				break;
+
+			pm->tm_value++;
+
+			if (pm->tm_value >= pm->tm_end) {
+
+				if (pm->fsm_phase < 5) {
+
+					pm->tm_value = 0;
+					pm->tm_end = PM_TSMS(pm, pm->tm_transient_slow);
+				}
+				else if (pm->fsm_phase < 6) {
+
+					pm->tm_value = 0;
+					pm->tm_end = PM_TSMS(pm, pm->tm_average_probe);
+				}
+				else {
+					pm->tm_value = 0;
+					pm->tm_end = PM_TSMS(pm, pm->tm_transient_fast);
+				}
+
+				hold_A = pm->probe_current_hold * pm->probe_weak_level;
+
+				pm->i_track_D = pm->probe_HOLD[0] * hold_A;
+				pm->i_track_Q = pm->probe_HOLD[1] * hold_A;
+
+				pm->fsm_phase += 1;
+			}
+			break;
+
+		case 7:
+			pm_voltage(pm, 0.f, 0.f);
+
+			pm->tm_value++;
+
+			if (pm->tm_value >= pm->tm_end) {
+
+				if (pm->fsm_subi < 2) {
+
+					pm->fsm_phase = 1;
+					pm->fsm_subi += 1;
+				}
+				else {
+					pm->fsm_phase += 1;
+				}
+			}
+			break;
+
+		case 8:
+			lse_solve(ls);
+
+			if (		   m_isfinitef(ls->sol.m[0]) != 0
+					&& m_isfinitef(ls->sol.m[1]) != 0
+					&& m_isfinitef(ls->sol.m[2]) != 0
+					&& ls->sol.m[0] > M_EPSILON
+					&& ls->sol.m[1] > M_EPSILON
+					&& ls->sol.m[2] > M_EPSILON) {
+
+				Rz = (ls->sol.m[0] + ls->sol.m[1] + ls->sol.m[2]) / 3.f;
+
+				pm->const_im_Rz = Rz;
+			}
+			else {
+				pm->fsm_errno = PM_ERROR_UNCERTAIN_RESULT;
+				pm->fsm_state = PM_STATE_HALT;
+				pm->fsm_phase = 0;
+				break;
+			}
+
+			if (		m_isfinitef(ls->sol.m[3]) != 0
+					&& m_fabsf(ls->sol.m[3]) > M_EPSILON) {
+
+				pm->dtc_deadband = ls->sol.m[3];
+			}
+			else {
+				pm->fsm_errno = PM_ERROR_UNCERTAIN_RESULT;
+				pm->fsm_state = PM_STATE_HALT;
+				pm->fsm_phase = 0;
+				break;
+			}
+
+			lse_std(ls);
+
+			pm->self_DTu = ls->std.m[0];
+
+			if (m_isfinitef(pm->self_DTu) != 0) {
+
+				if (pm->self_DTu > pm->fault_terminal_tol) {
+
+					pm->fsm_errno = PM_ERROR_LOW_ACCURACY;
+				}
+			}
+			else {
+				pm->fsm_errno = PM_ERROR_UNCERTAIN_RESULT;
+			}
+
+			pm->fsm_state = PM_STATE_HALT;
+			pm->fsm_phase = 0;
+			break;
+	}
+}
+
 static void
 pm_fsm_state_probe_const_resistance(pmc_t *pm)
 {
-	lse_t			*ls = &pm->probe_lse[0];
+	lse_t			*ls = &pm->lse[0];
 	lse_float_t		v[3];
 
 	float			hold_A, ramp_A;
@@ -1238,13 +1348,13 @@ pm_fsm_state_probe_const_resistance(pmc_t *pm)
 
 			lse_construct(ls, LSE_CASCADE_MAX, 2, 1);
 
-			pm->probe_HF_residual = 0.f;
-			pm->probe_HF_integral = 0.f;
+			pm->probe_HF[0] = 0.f;
+			pm->probe_HF[1] = 0.f;
 
 			hold_A = pm->probe_hold_angle * (M_PI_F / 180.f);
 
-			pm->probe_TEMP[0] = m_cosf(hold_A);
-			pm->probe_TEMP[1] = m_sinf(hold_A);
+			pm->probe_HOLD[0] = m_cosf(hold_A);
+			pm->probe_HOLD[1] = m_sinf(hold_A);
 
 			pm->i_integral_D = 0.f;
 			pm->i_integral_Q = 0.f;
@@ -1260,8 +1370,8 @@ pm_fsm_state_probe_const_resistance(pmc_t *pm)
 			ramp_A = (float) (pm->tm_value + 1)
 				* pm->probe_current_hold / (float) pm->tm_end;
 
-			pm->i_track_D = pm->probe_TEMP[0] * ramp_A;
-			pm->i_track_Q = pm->probe_TEMP[1] * ramp_A;
+			pm->i_track_D = pm->probe_HOLD[0] * ramp_A;
+			pm->i_track_Q = pm->probe_HOLD[1] * ramp_A;
 
 		case 2:
 			pm_fsm_probe_loop_current(pm, 0.f);
@@ -1273,7 +1383,7 @@ pm_fsm_state_probe_const_resistance(pmc_t *pm)
 
 			if (pm->tm_value >= pm->tm_end) {
 
-				if (pm->fsm_phase == 1) {
+				if (pm->fsm_phase < 2) {
 
 					pm->tm_value = 0;
 					pm->tm_end = PM_TSMS(pm, pm->tm_current_hold);
@@ -1288,16 +1398,20 @@ pm_fsm_state_probe_const_resistance(pmc_t *pm)
 			break;
 
 		case 3:
-		case 4:
-			v[0] = pm->probe_TEMP[0] * pm->lu_iX
-				+ pm->probe_TEMP[1] * pm->lu_iY;
+		case 5:
+			v[0] = pm->lu_iX;
+			v[1] = 1.f;
+			v[2] = pm->dtc_X;
 
+			lse_insert(ls, v);
+
+			v[0] = pm->lu_iY;
 			v[1] = 1.f;
-			v[2] = pm->probe_TEMP[0] * pm->tvm_X0
-				+ pm->probe_TEMP[1] * pm->tvm_Y0;
+			v[2] = pm->dtc_Y;
 
 			lse_insert(ls, v);
 
+		case 4:
 			pm_fsm_probe_loop_current(pm, 0.f);
 
 			if (pm->fsm_errno != PM_OK)
@@ -1307,7 +1421,12 @@ pm_fsm_state_probe_const_resistance(pmc_t *pm)
 
 			if (pm->tm_value >= pm->tm_end) {
 
-				if (pm->fsm_phase == 3) {
+				if (pm->fsm_phase < 4) {
+
+					pm->tm_value = 0;
+					pm->tm_end = PM_TSMS(pm, pm->tm_transient_slow);
+				}
+				else if (pm->fsm_phase < 5) {
 
 					pm->tm_value = 0;
 					pm->tm_end = PM_TSMS(pm, pm->tm_average_probe);
@@ -1319,14 +1438,14 @@ pm_fsm_state_probe_const_resistance(pmc_t *pm)
 
 				hold_A = pm->probe_current_hold * pm->probe_weak_level;
 
-				pm->i_track_D = pm->probe_TEMP[0] * hold_A;
-				pm->i_track_Q = pm->probe_TEMP[1] * hold_A;
+				pm->i_track_D = pm->probe_HOLD[0] * hold_A;
+				pm->i_track_Q = pm->probe_HOLD[1] * hold_A;
 
 				pm->fsm_phase += 1;
 			}
 			break;
 
-		case 5:
+		case 6:
 			pm_voltage(pm, 0.f, 0.f);
 
 			pm->tm_value++;
@@ -1337,18 +1456,20 @@ pm_fsm_state_probe_const_resistance(pmc_t *pm)
 			}
 			break;
 
-		case 6:
+		case 7:
 			lse_solve(ls);
 
 			if (		m_isfinitef(ls->sol.m[0]) != 0
 					&& ls->sol.m[0] > M_EPSILON) {
 
-				pm->const_im_R = ls->sol.m[0];
+				pm->const_im_Rz = ls->sol.m[0];
 			}
 			else {
 				pm->fsm_errno = PM_ERROR_UNCERTAIN_RESULT;
 			}
 
+			pm->self_DTu = ls->sol.m[1];
+
 			pm->fsm_state = PM_STATE_HALT;
 			pm->fsm_phase = 0;
 			break;
@@ -1387,11 +1508,11 @@ pm_fsm_state_probe_const_inductance(pmc_t *pm)
 
 			pm->quick_HFwS = M_2_PI_F * pm->probe_freq_sine;
 
-			pm->probe_SC[0] = m_cosf(pm->quick_HFwS * pm->m_dT * 0.5f);
-			pm->probe_SC[1] = m_sinf(pm->quick_HFwS * pm->m_dT * 0.5f);
+			pm->probe_WS[0] = m_cosf(pm->quick_HFwS * pm->m_dT * 0.5f);
+			pm->probe_WS[1] = m_sinf(pm->quick_HFwS * pm->m_dT * 0.5f);
 
-			pm->probe_HF_residual = 0.f;
-			pm->probe_HF_integral = 0.f;
+			pm->probe_HF[0] = 0.f;
+			pm->probe_HF[1] = 0.f;
 			pm->probe_gain_LP = pm->quick_HFwS * pm->m_dT / 4.f;
 
 			pm->hfi_wave[0] = 1.f;
@@ -1413,11 +1534,11 @@ pm_fsm_state_probe_const_inductance(pmc_t *pm)
 			break;
 
 		case 2:
-			iX = pm->probe_TEMP[0];
-			iY = pm->probe_TEMP[1];
+			iX = pm->lu_iX;
+			iY = pm->lu_iY;
 
-			uX = pm->tvm_X0 * pm->probe_SC[0] + pm->tvm_Y0 * pm->probe_SC[1];
-			uY = pm->tvm_Y0 * pm->probe_SC[0] - pm->tvm_X0 * pm->probe_SC[1];
+			uX = pm->dtc_X * pm->probe_WS[0] + pm->dtc_Y * pm->probe_WS[1];
+			uY = pm->dtc_Y * pm->probe_WS[0] - pm->dtc_X * pm->probe_WS[1];
 
 			m_rsumf(&pm->probe_DFT[0], &pm->probe_REM[0], iX * pm->hfi_wave[0]);
 			m_rsumf(&pm->probe_DFT[1], &pm->probe_REM[1], iX * pm->hfi_wave[1]);
@@ -1429,9 +1550,6 @@ pm_fsm_state_probe_const_inductance(pmc_t *pm)
 			m_rsumf(&pm->probe_DFT[7], &pm->probe_REM[7], uY * pm->hfi_wave[1]);
 
 		case 1:
-			pm->probe_TEMP[0] = pm->lu_iX;
-			pm->probe_TEMP[1] = pm->lu_iY;
-
 			pm_fsm_probe_loop_current(pm, pm->probe_current_sine);
 
 			if (pm->fsm_errno != PM_OK)
@@ -1441,7 +1559,7 @@ pm_fsm_state_probe_const_inductance(pmc_t *pm)
 
 			if (pm->tm_value >= pm->tm_end) {
 
-				if (pm->fsm_phase == 1) {
+				if (pm->fsm_phase < 2) {
 
 					pm->tm_value = 0;
 					pm->tm_end = PM_TSMS(pm, pm->tm_average_probe);
@@ -1472,10 +1590,10 @@ pm_fsm_state_probe_const_inductance(pmc_t *pm)
 			if (		   m_isfinitef(la[2]) != 0 && la[2] > M_EPSILON
 					&& m_isfinitef(la[3]) != 0 && la[3] > M_EPSILON) {
 
-				pm->const_im_L1 = la[2];
-				pm->const_im_L2 = la[3];
-				pm->const_im_B = m_atan2f(la[1], la[0]) * (180.f / M_PI_F);
-				pm->const_im_R = la[4];
+				pm->const_im_Ld = la[2];
+				pm->const_im_Lq = la[3];
+				pm->const_im_A = m_atan2f(la[1], la[0]) * (180.f / M_PI_F);
+				pm->const_im_Rz = la[4];
 			}
 			else {
 				pm->fsm_errno = PM_ERROR_UNCERTAIN_RESULT;
@@ -1493,17 +1611,15 @@ pm_fsm_state_lu_startup(pmc_t *pm, int in_ZONE)
 	switch (pm->fsm_phase) {
 
 		case 0:
-			if (		   m_isfinitef(pm->const_im_L1) != 0
-					&& m_isfinitef(pm->const_im_L2) != 0
-					&& pm->const_im_L1 > M_EPSILON
-					&& pm->const_im_L2 > M_EPSILON) {
+			if (		   m_isfinitef(pm->const_im_Ld) != 0
+					&& m_isfinitef(pm->const_im_Lq) != 0
+					&& pm->const_im_Ld > M_EPSILON
+					&& pm->const_im_Lq > M_EPSILON) {
 
 				pm->vsi_DC = 0.f;
 				pm->vsi_lpf_DC = 0.f;
 				pm->vsi_X = 0.f;
 				pm->vsi_Y = 0.f;
-				pm->vsi_X0 = 0.f;
-				pm->vsi_Y0 = 0.f;
 
 				pm_clearance(pm, 0, 0, 0);
 				pm_clearance(pm, 0, 0, 0);
@@ -1512,6 +1628,11 @@ pm_fsm_state_lu_startup(pmc_t *pm, int in_ZONE)
 				pm->vsi_BT = 0;
 				pm->vsi_CT = 0;
 
+				pm->dtc_uX = 0.f;
+				pm->dtc_uY = 0.f;
+				pm->dtc_X = 0.f;
+				pm->dtc_Y = 0.f;
+
 				pm->lu_iX = 0.f;
 				pm->lu_iY = 0.f;
 				pm->lu_iD = 0.f;
@@ -1571,6 +1692,10 @@ pm_fsm_state_lu_startup(pmc_t *pm, int in_ZONE)
 				pm->flux_wS = 0.f;
 
 				pm->kalman_POSTPONED = PM_DISABLED;
+
+				pm->kalman_E[0] = 0.f;
+				pm->kalman_E[1] = 0.f;
+				pm->kalman_bias_Q = 0.f;
 				pm->kalman_lpf_wS = 0.f;
 
 				pm->zone_lpf_wS = 0.f;
@@ -1662,7 +1787,7 @@ pm_fsm_state_lu_startup(pmc_t *pm, int in_ZONE)
 						pm->lu_MODE = PM_LU_FORCED;
 					}
 					else {
-						pm->fsm_errno = PM_ERROR_INVALID_OPERATION;
+						pm->fsm_errno = PM_ERROR_NAN_OPERATION;
 						pm->fsm_state = PM_STATE_HALT;
 						pm->fsm_phase = 0;
 					}
@@ -1676,7 +1801,7 @@ pm_fsm_state_lu_startup(pmc_t *pm, int in_ZONE)
 				pm->fsm_phase = 0;
 			}
 			else {
-				pm->fsm_errno = PM_ERROR_INVALID_OPERATION;
+				pm->fsm_errno = PM_ERROR_NAN_OPERATION;
 				pm->fsm_state = PM_STATE_HALT;
 				pm->fsm_phase = 0;
 			}
@@ -1718,7 +1843,7 @@ pm_fsm_state_lu_shutdown(pmc_t *pm)
 static void
 pm_fsm_state_probe_const_flux_linkage(pmc_t *pm)
 {
-	lse_t			*ls = &pm->probe_lse[0];
+	lse_t			*ls = &pm->lse[0];
 	lse_float_t		v[2];
 
 	switch (pm->fsm_phase) {
@@ -1796,7 +1921,7 @@ pm_fsm_state_probe_const_flux_linkage(pmc_t *pm)
 				}
 			}
 			else {
-				pm->fsm_errno = PM_ERROR_INVALID_OPERATION;
+				pm->fsm_errno = PM_ERROR_NAN_OPERATION;
 			}
 
 			pm->fsm_state = PM_STATE_IDLE;
@@ -1808,10 +1933,10 @@ pm_fsm_state_probe_const_flux_linkage(pmc_t *pm)
 static void
 pm_fsm_state_probe_const_inertia(pmc_t *pm)
 {
-	lse_t			*ls = &pm->probe_lse[0];
+	lse_t			*ls = &pm->lse[0];
 	lse_float_t		v[4];
 
-	float			*m = pm->probe_lse[1].vm;
+	float			*m = pm->lse[1].vm;
 	float			mQ, temp_Ja;
 
 	switch (pm->fsm_phase) {
@@ -1880,7 +2005,7 @@ pm_fsm_state_probe_const_inertia(pmc_t *pm)
 static void
 pm_fsm_state_probe_noise_threshold(pmc_t *pm)
 {
-	lse_t			*ls = &pm->probe_lse[0];
+	lse_t			*ls = &pm->lse[0];
 	lse_float_t		v[3];
 
 	switch (pm->fsm_phase) {
@@ -1931,9 +2056,9 @@ pm_fsm_state_probe_noise_threshold(pmc_t *pm)
 static void
 pm_fsm_state_adjust_sensor_hall(pmc_t *pm)
 {
-	int			*dnum = (int *) pm->probe_lse[1].vm;
-	float			*rem0 = pm->probe_lse[1].vm + 10;
-	float			*rem1 = pm->probe_lse[1].vm + 18;
+	int			*dnum = (int *) pm->lse[1].vm;
+	float			*rem0 = pm->lse[1].vm + 10;
+	float			*rem1 = pm->lse[1].vm + 18;
 
 	int			HS, N, thld;
 
@@ -2038,10 +2163,10 @@ pm_fsm_state_adjust_sensor_hall(pmc_t *pm)
 static void
 pm_fsm_state_adjust_sensor_eabi(pmc_t *pm)
 {
-	lse_t			*ls = &pm->probe_lse[0];
+	lse_t			*ls = &pm->lse[0];
 	lse_float_t		v[4];
 
-	int			*range_bEP = (int *) pm->probe_lse[1].vm;
+	int			*range_bEP = (int *) pm->lse[1].vm;
 
 	int			relEP, WRAP, N;
 
@@ -2270,8 +2395,9 @@ void pm_FSM(pmc_t *pm)
 		case PM_STATE_SELF_TEST_BOOTSTRAP:
 		case PM_STATE_SELF_TEST_POWER_STAGE:
 		case PM_STATE_SELF_TEST_CLEARANCE:
-		case PM_STATE_ADJUST_VOLTAGE:
-		case PM_STATE_ADJUST_CURRENT:
+		case PM_STATE_ADJUST_ON_PCB_VOLTAGE:
+		case PM_STATE_ADJUST_ON_PCB_CURRENT:
+		case PM_STATE_ADJUST_DTC_VOLTAGE:
 		case PM_STATE_PROBE_CONST_RESISTANCE:
 		case PM_STATE_PROBE_CONST_INDUCTANCE:
 		case PM_STATE_LU_DETACHED:
@@ -2344,12 +2470,16 @@ void pm_FSM(pmc_t *pm)
 			pm_fsm_state_self_test_clearance(pm);
 			break;
 
-		case PM_STATE_ADJUST_VOLTAGE:
-			pm_fsm_state_adjust_voltage(pm);
+		case PM_STATE_ADJUST_ON_PCB_VOLTAGE:
+			pm_fsm_state_adjust_on_pcb_voltage(pm);
+			break;
+
+		case PM_STATE_ADJUST_ON_PCB_CURRENT:
+			pm_fsm_state_adjust_on_pcb_current(pm);
 			break;
 
-		case PM_STATE_ADJUST_CURRENT:
-			pm_fsm_state_adjust_current(pm);
+		case PM_STATE_ADJUST_DTC_VOLTAGE:
+			pm_fsm_state_adjust_dtc_voltage(pm);
 			break;
 
 		case PM_STATE_PROBE_CONST_RESISTANCE:
@@ -2412,12 +2542,12 @@ const char *pm_strerror(int fsm_errno)
 		PM_SFI_CASE(PM_ERROR_NO_MOTOR_CONNECTED);
 		PM_SFI_CASE(PM_ERROR_BOOTSTRAP_FAULT);
 		PM_SFI_CASE(PM_ERROR_POWER_STAGE_BROKEN);
-		PM_SFI_CASE(PM_ERROR_INSUFFICIENT_ACCURACY);
+		PM_SFI_CASE(PM_ERROR_LOW_ACCURACY);
 		PM_SFI_CASE(PM_ERROR_CURRENT_LOOP_FAULT);
 		PM_SFI_CASE(PM_ERROR_INSTANT_OVERCURRENT);
 		PM_SFI_CASE(PM_ERROR_DC_LINK_OVERVOLTAGE);
 		PM_SFI_CASE(PM_ERROR_UNCERTAIN_RESULT);
-		PM_SFI_CASE(PM_ERROR_INVALID_OPERATION);
+		PM_SFI_CASE(PM_ERROR_NAN_OPERATION);
 		PM_SFI_CASE(PM_ERROR_SENSOR_HALL_FAULT);
 		PM_SFI_CASE(PM_ERROR_SENSOR_EABI_FAULT);
 
diff --git a/src/pmfunc.c b/src/pmfunc.c
index 627dd2c..a577c68 100644
--- a/src/pmfunc.c
+++ b/src/pmfunc.c
@@ -119,6 +119,47 @@ int pm_wait_settle()
 	return pm.fsm_errno;
 }
 
+SH_DEF(pm_adjust_dtc_voltage)
+{
+	if (pm.lu_MODE != PM_LU_DISABLED) {
+
+		printf("Unable when PM is running" EOL);
+		return;
+	}
+
+	do {
+		pm.fsm_req = PM_STATE_ZERO_DRIFT;
+		pm_wait_IDLE();
+
+		reg_OUTP(ID_PM_CONST_FB_U);
+		reg_OUTP(ID_PM_SCALE_IA0);
+		reg_OUTP(ID_PM_SCALE_IB0);
+		reg_OUTP(ID_PM_SCALE_IC0);
+		reg_OUTP(ID_PM_SELF_STDI);
+
+		if (pm.fsm_errno != PM_OK)
+			break;
+
+		if (PM_CONFIG_TVM(&pm) == PM_ENABLED) {
+
+			pm.fsm_req = PM_STATE_SELF_TEST_POWER_STAGE;
+
+			if (pm_wait_IDLE() != PM_OK)
+				break;
+		}
+
+		pm.fsm_req = PM_STATE_ADJUST_DTC_VOLTAGE;
+		pm_wait_IDLE();
+
+		reg_OUTP(ID_PM_CONST_IM_RZ);
+		reg_OUTP(ID_PM_DTC_DEADBAND);
+		reg_OUTP(ID_PM_SELF_DTU);
+	}
+	while (0);
+
+	reg_OUTP(ID_PM_FSM_ERRNO);
+}
+
 SH_DEF(pm_probe_impedance)
 {
 	if (pm.lu_MODE != PM_LU_DISABLED) {
@@ -155,19 +196,20 @@ SH_DEF(pm_probe_impedance)
 		if (pm_wait_IDLE() != PM_OK)
 			break;
 
-		pm.const_Rs = pm.const_im_R;
+		pm.const_Rs = pm.const_im_Rz;
 
 		reg_OUTP(ID_PM_CONST_RS);
+		reg_OUTP(ID_PM_SELF_DTU);
 
 		pm.fsm_req = PM_STATE_PROBE_CONST_INDUCTANCE;
 
 		if (pm_wait_IDLE() != PM_OK)
 			break;
 
-		reg_OUTP(ID_PM_CONST_IM_L1);
-		reg_OUTP(ID_PM_CONST_IM_L2);
-		reg_OUTP(ID_PM_CONST_IM_B);
-		reg_OUTP(ID_PM_CONST_IM_R);
+		reg_OUTP(ID_PM_CONST_IM_LD);
+		reg_OUTP(ID_PM_CONST_IM_LQ);
+		reg_OUTP(ID_PM_CONST_IM_A);
+		reg_OUTP(ID_PM_CONST_IM_RZ);
 
 		pm_auto(&pm, PM_AUTO_MAXIMAL_CURRENT);
 		pm_auto(&pm, PM_AUTO_LOOP_CURRENT);
@@ -278,11 +320,11 @@ SH_DEF(pm_probe_spinup)
 
 		reg_SET_F(ID_PM_S_SETPOINT_SPEED_PC, 110.f);
 
-		vTaskDelay((TickType_t) 300);
+		vTaskDelay((TickType_t) 400);
 
 		reg_SET_F(ID_PM_S_SETPOINT_SPEED, pm.probe_speed_hold);
 
-		vTaskDelay((TickType_t) 300);
+		vTaskDelay((TickType_t) 400);
 
 		if (pm_wait_IDLE() != PM_OK)
 			break;
@@ -360,81 +402,6 @@ SH_DEF(pm_probe_detached)
 	tlm_halt(&tlm);
 }
 
-SH_DEF(pm_probe_const_resistance)
-{
-	float		R[3];
-
-	if (pm.lu_MODE != PM_LU_DISABLED) {
-
-		printf("Unable when PM is running" EOL);
-		return;
-	}
-
-	do {
-		pm.fsm_req = PM_STATE_ZERO_DRIFT;
-		pm_wait_IDLE();
-
-		reg_OUTP(ID_PM_CONST_FB_U);
-		reg_OUTP(ID_PM_SCALE_IA0);
-		reg_OUTP(ID_PM_SCALE_IB0);
-		reg_OUTP(ID_PM_SCALE_IC0);
-		reg_OUTP(ID_PM_SELF_STDI);
-
-		if (pm.fsm_errno != PM_OK)
-			break;
-
-		if (PM_CONFIG_TVM(&pm) == PM_ENABLED) {
-
-			pm.fsm_req = PM_STATE_SELF_TEST_POWER_STAGE;
-
-			if (pm_wait_IDLE() != PM_OK)
-				break;
-		}
-
-		pm.probe_hold_angle = 0.f;
-
-		pm.fsm_req = PM_STATE_PROBE_CONST_RESISTANCE;
-
-		if (pm_wait_IDLE() != PM_OK)
-			break;
-
-		R[0] = pm.const_im_R;
-
-		reg_OUTP(ID_PM_CONST_IM_R);
-
-		pm.probe_hold_angle = 120.f;
-
-		pm.fsm_req = PM_STATE_PROBE_CONST_RESISTANCE;
-
-		if (pm_wait_IDLE() != PM_OK)
-			break;
-
-		R[1] = pm.const_im_R;
-
-		reg_OUTP(ID_PM_CONST_IM_R);
-
-		pm.probe_hold_angle = - 120.f;
-
-		pm.fsm_req = PM_STATE_PROBE_CONST_RESISTANCE;
-
-		if (pm_wait_IDLE() != PM_OK)
-			break;
-
-		R[2] = pm.const_im_R;
-
-		reg_OUTP(ID_PM_CONST_IM_R);
-
-		pm.const_Rs = (R[0] + R[1] + R[2]) / 3.f;
-
-		reg_OUTP(ID_PM_CONST_RS);
-	}
-	while (0);
-
-	pm.probe_hold_angle = 0.f;
-
-	reg_OUTP(ID_PM_FSM_ERRNO);
-}
-
 SH_DEF(pm_probe_const_flux_linkage)
 {
 	if (pm.lu_MODE == PM_LU_DISABLED) {
@@ -498,11 +465,11 @@ SH_DEF(pm_probe_const_inertia)
 		wSP = reg_GET_F(ID_PM_S_SETPOINT_SPEED);
 		reg_SET_F(ID_PM_S_SETPOINT_SPEED_PC, 110.f);
 
-		vTaskDelay((TickType_t) 300);
+		vTaskDelay((TickType_t) 400);
 
 		reg_SET_F(ID_PM_S_SETPOINT_SPEED, wSP);
 
-		vTaskDelay((TickType_t) 300);
+		vTaskDelay((TickType_t) 400);
 
 		if (pm_wait_IDLE() != PM_OK)
 			break;
@@ -702,11 +669,11 @@ SH_DEF(ld_probe_const_inertia)
 
 		reg_SET_F(ID_PM_X_SETPOINT_LOCATION, pm.x_maximal);
 
-		vTaskDelay((TickType_t) 300);
+		vTaskDelay((TickType_t) 400);
 
 		reg_SET_F(ID_PM_X_SETPOINT_LOCATION, pm.x_minimal);
 
-		vTaskDelay((TickType_t) 300);
+		vTaskDelay((TickType_t) 400);
 
 		if (pm_wait_IDLE() != PM_OK)
 			break;
diff --git a/src/pmtest.c b/src/pmtest.c
index 2f1ed57..618ebc8 100644
--- a/src/pmtest.c
+++ b/src/pmtest.c
@@ -77,6 +77,7 @@ SH_DEF(pm_self_test)
 			if (PM_CONFIG_TVM(&pm) == PM_ENABLED) {
 
 				reg_OUTP(ID_PM_SELF_RMSU);
+				reg_OUTP(ID_PM_SELF_RMST);
 			}
 
 			reg_OUTP(ID_PM_FSM_ERRNO);
@@ -112,7 +113,7 @@ SH_DEF(pm_self_adjust)
 
 		if (PM_CONFIG_TVM(&pm) == PM_ENABLED) {
 
-			pm.fsm_req = PM_STATE_ADJUST_VOLTAGE;
+			pm.fsm_req = PM_STATE_ADJUST_ON_PCB_VOLTAGE;
 			pm_wait_IDLE();
 
 			reg_OUTP(ID_PM_SCALE_UA0);
@@ -122,104 +123,37 @@ SH_DEF(pm_self_adjust)
 			reg_OUTP(ID_PM_SCALE_UC0);
 			reg_OUTP(ID_PM_SCALE_UC1);
 
-			reg_OUTP(ID_PM_TVM_ACTIVE);
-			reg_OUTP(ID_PM_TVM_FIR_A_TAU);
-			reg_OUTP(ID_PM_TVM_FIR_B_TAU);
-			reg_OUTP(ID_PM_TVM_FIR_C_TAU);
-
 			reg_OUTP(ID_PM_SELF_RMSU);
+			reg_OUTP(ID_PM_SELF_RMST);
 
 			if (pm.fsm_errno != PM_OK)
 				break;
 		}
 
-		pm.fsm_req = PM_STATE_ADJUST_CURRENT;
+		pm.fsm_req = PM_STATE_ADJUST_ON_PCB_CURRENT;
 		pm_wait_IDLE();
 
 		reg_OUTP(ID_PM_SCALE_IA1);
 		reg_OUTP(ID_PM_SCALE_IB1);
 		reg_OUTP(ID_PM_SCALE_IC1);
 		reg_OUTP(ID_PM_SELF_RMSI);
-	}
-	while (0);
-
-	reg_OUTP(ID_PM_FSM_ERRNO);
-
-	tlm_halt(&tlm);
-}
-
-SH_DEF(pm_analysis_TVM)
-{
-	TickType_t		xWake, xTim0;
-	int			xDC, xMIN, xMAX;
-
-	if (pm.lu_MODE != PM_LU_DISABLED) {
-
-		printf("Unable when PM is running" EOL);
-		return;
-	}
-
-	if (		PM_CONFIG_TVM(&pm) != PM_ENABLED
-			|| pm.tvm_ACTIVE != PM_ENABLED) {
-
-		printf("Unable with TVM disabled" EOL);
-		return;
-	}
-
-	xMIN = pm.ts_minimal;
-	xMAX = (int) (pm.dc_resolution * pm.tvm_clean_zone);
-
-	xDC = xMIN;
-
-	PWM_set_DC(0, 0, 0);
-	PWM_set_Z(0);
-
-	pm_clearance(&pm, 0, 0, 0);
-	pm_clearance(&pm, 0, 0, 0);
-
-	xWake = xTaskGetTickCount();
-	xTim0 = xWake;
-
-	/*
-	tlm.reg_ID[0] = ID_PM_VSI_X;
-	tlm.reg_ID[1] = ID_PM_TVM_A;
-	tlm.reg_ID[2] = ID_PM_TVM_B;
-	tlm.reg_ID[3] = ID_PM_TVM_C;
-	*/
-
-	tlm_startup(&tlm, tlm.rate_grab, TLM_MODE_GRAB);
-
-	do {
-		/* 1000 Hz.
-		 * */
-		vTaskDelayUntil(&xWake, (TickType_t) 1);
 
 		if (pm.fsm_errno != PM_OK)
 			break;
 
-		xDC = (xDC < xMAX) ? xDC + 1 : xMIN;
-
-		PWM_set_DC(xDC, xDC, xDC);
-
-		pm_clearance(&pm, xDC, xDC, xDC);
-
-		/* Get reference VOLTAGE.
-		 * */
-		pm.vsi_X = xDC * pm.const_fb_U * pm.ts_inverted;
-
-		if (tlm.mode == TLM_MODE_DISABLED)
-			break;
+		pm.fsm_req = PM_STATE_ADJUST_DTC_VOLTAGE;
+		pm_wait_IDLE();
 
-		if ((xWake - xTim0) > (TickType_t) 10000) {
+		reg_OUTP(ID_PM_CONST_IM_RZ);
+		reg_OUTP(ID_PM_DTC_DEADBAND);
+		reg_OUTP(ID_PM_SELF_DTU);
 
-			pm.fsm_errno = PM_ERROR_TIMEOUT;
+		if (pm.fsm_errno != PM_OK)
 			break;
-		}
 	}
-	while (1);
+	while (0);
 
-	PWM_set_DC(0, 0, 0);
-	PWM_set_Z(PM_Z_ABC);
+	reg_OUTP(ID_PM_FSM_ERRNO);
 
 	tlm_halt(&tlm);
 }
@@ -261,10 +195,10 @@ SH_DEF(pm_analysis_impedance)
 
 	/*
 	tlm.reg_ID[0] = ID_PM_PROBE_FREQ_SINE;
-	tlm.reg_ID[1] = ID_PM_CONST_IM_L1;
-	tlm.reg_ID[2] = ID_PM_CONST_IM_L2;
-	tlm.reg_ID[3] = ID_PM_CONST_IM_B;
-	tlm.reg_ID[4] = ID_PM_CONST_IM_R;
+	tlm.reg_ID[1] = ID_PM_CONST_IM_LD;
+	tlm.reg_ID[2] = ID_PM_CONST_IM_LQ;
+	tlm.reg_ID[3] = ID_PM_CONST_IM_A;
+	tlm.reg_ID[4] = ID_PM_CONST_IM_RZ;
 	*/
 
 	tlm_startup(&tlm, tlm.rate_live, TLM_MODE_WATCH);
@@ -275,7 +209,7 @@ SH_DEF(pm_analysis_impedance)
 	stop_freq = 400.f;
 	walk_freq = (float) (int) ((pm.probe_freq_sine - stop_freq) / 90.f);
 
-	printf("Fq@Hz     L1@H   L2@H   Rz@Ohm" EOL);
+	printf("Fq@Hz     Ld@H   Lq@H   Rz@Ohm" EOL);
 
 	do {
 		if (pm.fsm_errno != PM_OK)
@@ -285,7 +219,7 @@ SH_DEF(pm_analysis_impedance)
 		pm_wait_IDLE();
 
 		printf("%4g    %4g %4g %4g" EOL, &pm.probe_freq_sine,
-				&pm.const_im_L1, &pm.const_im_L2, &pm.const_im_R);
+				&pm.const_im_Ld, &pm.const_im_Lq, &pm.const_im_Rz);
 
 		pm.probe_freq_sine += - walk_freq;
 
diff --git a/src/regdefs.h b/src/regdefs.h
index 89303f6..e21102c 100644
--- a/src/regdefs.h
+++ b/src/regdefs.h
@@ -9,7 +9,6 @@ ID_HAL_ADC_SHUNT_RESISTANCE,
 ID_HAL_ADC_AMPLIFIER_GAIN,
 ID_HAL_ADC_VOLTAGE_RATIO,
 ID_HAL_ADC_TERMINAL_RATIO,
-ID_HAL_ADC_TERMINAL_BIAS,
 #ifdef HW_HAVE_ANALOG_KNOB
 ID_HAL_ADC_KNOB_RATIO,
 #endif /* HW_HAVE_ANALOG_KNOB */
@@ -181,12 +180,15 @@ ID_PM_SELF_IST,
 ID_PM_SELF_STDI,
 ID_PM_SELF_RMSI,
 ID_PM_SELF_RMSU,
+ID_PM_SELF_RMST,
+ID_PM_SELF_DTU,
 ID_PM_CONFIG_NOP,
 ID_PM_CONFIG_IFB,
 ID_PM_CONFIG_TVM,
 ID_PM_CONFIG_DBG,
 ID_PM_CONFIG_VSI_ZERO,
 ID_PM_CONFIG_VSI_CLAMP,
+ID_PM_CONFIG_DTC_VOLTAGE,
 ID_PM_CONFIG_LU_FORCED,
 ID_PM_CONFIG_LU_FREEWHEEL,
 ID_PM_CONFIG_LU_ESTIMATE,
@@ -268,35 +270,20 @@ ID_PM_VSI_LPF_DC,
 ID_PM_VSI_X,
 ID_PM_VSI_Y,
 ID_PM_VSI_GAIN_LP,
-ID_PM_VSI_MASK_XF,
 ID_PM_VSI_AF,
 ID_PM_VSI_BF,
 ID_PM_VSI_CF,
 ID_PM_VSI_IF,
-ID_PM_VSI_SF,
 ID_PM_VSI_UF,
-ID_PM_VSI_AQ,
-ID_PM_VSI_BQ,
-ID_PM_VSI_CQ,
-ID_PM_TVM_ACTIVE,
-ID_PM_TVM_CLEAN_ZONE,
-ID_PM_TVM_A,
-ID_PM_TVM_B,
-ID_PM_TVM_C,
-ID_PM_TVM_FIR_A0,
-ID_PM_TVM_FIR_A1,
-ID_PM_TVM_FIR_A2,
-ID_PM_TVM_FIR_A_TAU,
-ID_PM_TVM_FIR_B0,
-ID_PM_TVM_FIR_B1,
-ID_PM_TVM_FIR_B2,
-ID_PM_TVM_FIR_B_TAU,
-ID_PM_TVM_FIR_C0,
-ID_PM_TVM_FIR_C1,
-ID_PM_TVM_FIR_C2,
-ID_PM_TVM_FIR_C_TAU,
-ID_PM_TVM_X0,
-ID_PM_TVM_Y0,
+ID_PM_VSI_A0,
+ID_PM_VSI_B0,
+ID_PM_VSI_C0,
+ID_PM_DTC_DEADBAND,
+ID_PM_DTC_TOL,
+ID_PM_DTC_UX,
+ID_PM_DTC_UY,
+ID_PM_DTC_X,
+ID_PM_DTC_Y,
 ID_PM_LU_MODE,
 ID_PM_LU_IX,
 ID_PM_LU_IY,
@@ -345,9 +332,10 @@ ID_PM_FLUX_GAIN_LO,
 ID_PM_FLUX_GAIN_HI,
 ID_PM_FLUX_GAIN_SF,
 ID_PM_FLUX_GAIN_IF,
-ID_PM_KALMAN_RESIDUAL_D,
-ID_PM_KALMAN_RESIDUAL_Q,
+ID_PM_KALMAN_E0,
+ID_PM_KALMAN_E1,
 ID_PM_KALMAN_BIAS_Q,
+ID_PM_KALMAN_LPF_WS,
 ID_PM_KALMAN_GAIN_Q0,
 ID_PM_KALMAN_GAIN_Q1,
 ID_PM_KALMAN_GAIN_Q2,
@@ -414,11 +402,11 @@ ID_PM_CONST_ZP,
 ID_PM_CONST_JA,
 ID_PM_CONST_JA_KGM2,
 ID_PM_CONST_JA_KG,
-ID_PM_CONST_IM_L1,
-ID_PM_CONST_IM_L2,
-ID_PM_CONST_IM_B,
-ID_PM_CONST_IM_R,
-ID_PM_CONST_LD_S,
+ID_PM_CONST_IM_LD,
+ID_PM_CONST_IM_LQ,
+ID_PM_CONST_IM_A,
+ID_PM_CONST_IM_RZ,
+ID_PM_CONST_LD_SM,
 ID_PM_WATT_DC_MAX,
 ID_PM_WATT_DC_MIN,
 ID_PM_WATT_WP_MAXIMAL,
@@ -443,6 +431,7 @@ ID_PM_WATT_FUEL_GAUGE,
 ID_PM_WATT_GAIN_P,
 ID_PM_WATT_GAIN_I,
 ID_PM_WATT_GAIN_LP,
+ID_PM_WATT_GAIN_WF,
 ID_PM_I_SETPOINT_CURRENT,
 ID_PM_I_SETPOINT_CURRENT_PC,
 ID_PM_I_MAXIMAL,
diff --git a/src/regfile.c b/src/regfile.c
index 85bd0f1..16be9d7 100644
--- a/src/regfile.c
+++ b/src/regfile.c
@@ -339,13 +339,13 @@ reg_proc_mmps(const reg_t *reg, rval_t *lval, const rval_t *rval)
 
 		reg_proc_rpm(reg, &rpm, NULL);
 
-		lval->f = rpm.f * pm.const_ld_S * (1000.f / 60.f);
+		lval->f = rpm.f * pm.const_ld_Sm * (1000.f / 60.f);
 	}
 	else if (rval != NULL) {
 
-		if (pm.const_ld_S > M_EPSILON) {
+		if (pm.const_ld_Sm > M_EPSILON) {
 
-			rpm.f = rval->f / pm.const_ld_S * (60.f / 1000.f);
+			rpm.f = rval->f / pm.const_ld_Sm * (60.f / 1000.f);
 
 			reg_proc_rpm(reg, NULL, &rpm);
 		}
@@ -361,13 +361,13 @@ reg_proc_kmh(const reg_t *reg, rval_t *lval, const rval_t *rval)
 
 		reg_proc_rpm(reg, &rpm, NULL);
 
-		lval->f = rpm.f * pm.const_ld_S * (3.6f / 60.f);
+		lval->f = rpm.f * pm.const_ld_Sm * (3.6f / 60.f);
 	}
 	else if (rval != NULL) {
 
-		if (pm.const_ld_S > M_EPSILON) {
+		if (pm.const_ld_Sm > M_EPSILON) {
 
-			rpm.f = rval->f / pm.const_ld_S * (60.f / 3.6f);
+			rpm.f = rval->f / pm.const_ld_Sm * (60.f / 3.6f);
 
 			reg_proc_rpm(reg, NULL, &rpm);
 		}
@@ -493,7 +493,7 @@ reg_proc_kg(const reg_t *reg, rval_t *lval, const rval_t *rval)
 {
 	const float             const_Zp2 = (float) (pm.const_Zp * pm.const_Zp);
 
-	const float		lR = pm.const_ld_S / M_2_PI_F;
+	const float		lR = pm.const_ld_Sm / M_2_PI_F;
 	const float		lQ = lR * lR;
 
 	if (lval != NULL) {
@@ -666,7 +666,7 @@ reg_proc_weakening(const reg_t *reg, rval_t *lval, const rval_t *rval)
 
 	if (lval != NULL) {
 
-		weak = pm.const_lambda - reg->link->f * pm.const_im_L1;
+		weak = pm.const_lambda - reg->link->f * pm.const_im_Ld;
 
 		if (weak > M_EPSILON) {
 
@@ -684,12 +684,12 @@ reg_proc_weakening(const reg_t *reg, rval_t *lval, const rval_t *rval)
 	}
 	else if (rval != NULL) {
 
-		if (		pm.const_im_L1 > M_EPSILON
+		if (		pm.const_im_Ld > M_EPSILON
 				&& rval->f > 100.f) {
 
 			weak = pm.const_lambda * 100.f / rval->f;
 
-			reg->link->f = (pm.const_lambda - weak) / pm.const_im_L1;
+			reg->link->f = (pm.const_lambda - weak) / pm.const_im_Ld;
 		}
 		else {
 			reg->link->f = 0.f;
@@ -773,15 +773,15 @@ reg_proc_location_mm(const reg_t *reg, rval_t *lval, const rval_t *rval)
 {
 	if (lval != NULL) {
 
-		lval->f = reg->link->f * (pm.const_ld_S * 1000.f)
+		lval->f = reg->link->f * (pm.const_ld_Sm * 1000.f)
 			/ (M_2_PI_F * (float) pm.const_Zp);
 	}
 	else if (rval != NULL) {
 
-		if (pm.const_ld_S > M_EPSILON) {
+		if (pm.const_ld_Sm > M_EPSILON) {
 
 			reg->link->f = rval->f * (M_2_PI_F * (float) pm.const_Zp)
-				/ (pm.const_ld_S * 1000.f);
+				/ (pm.const_ld_Sm * 1000.f);
 		}
 	}
 }
@@ -834,35 +834,21 @@ reg_proc_x_accel_mm(const reg_t *reg, rval_t *lval, const rval_t *rval)
 
 		reg_proc_x_accel(reg, &rad, NULL);
 
-		lval->f = rad.f * (pm.const_ld_S * 1000.f)
+		lval->f = rad.f * (pm.const_ld_Sm * 1000.f)
 			/ (M_2_PI_F * (float) pm.const_Zp);
 	}
 	else if (rval != NULL) {
 
-		if (pm.const_ld_S > M_EPSILON) {
+		if (pm.const_ld_Sm > M_EPSILON) {
 
 			rad.f = rval->f * (M_2_PI_F * (float) pm.const_Zp)
-				/ (pm.const_ld_S * 1000.f);
+				/ (pm.const_ld_Sm * 1000.f);
 
 			reg_proc_x_accel(reg, NULL, &rad);
 		}
 	}
 }
 
-static void
-reg_proc_tvm_FIR_tau(const reg_t *reg, rval_t *lval, const rval_t *rval)
-{
-	float		tau, *FIR = (void *) reg->link;
-
-	if (lval != NULL) {
-
-		tau = FIR[0] / - FIR[1];
-		tau = (tau > M_EPSILON) ? pm.m_dT * 1000000.f / m_logf(tau) : 0.f;
-
-		lval->f = tau;
-	}
-}
-
 static void
 reg_proc_watt_fuel(const reg_t *reg, rval_t *lval, const rval_t *rval)
 {
@@ -934,7 +920,7 @@ reg_format_DRV_ocp_level(const reg_t *reg)
 static void
 reg_format_self_BST(const reg_t *reg)
 {
-	int		*BST = (void *) reg->link;
+	const int	*BST = (const void *) reg->link;
 
 	printf("%1f %1f %1f (ms)", &BST[0], &BST[1], &BST[2]);
 }
@@ -942,7 +928,7 @@ reg_format_self_BST(const reg_t *reg)
 static void
 reg_format_self_IST(const reg_t *reg)
 {
-	int		*IST = (void *) reg->link;
+	const int	*IST = (const void *) reg->link;
 
 	printf("%2x %2x %2x %2x %2x %2x %2x", IST[0], IST[1],
 			IST[2], IST[3], IST[4], IST[5], IST[6]);
@@ -957,11 +943,11 @@ reg_format_self_RMSi(const reg_t *reg)
 }
 
 static void
-reg_format_self_RMSu(const reg_t *reg)
+reg_format_self_RMSt(const reg_t *reg)
 {
 	float		*RMS = (void *) reg->link;
 
-	printf("%4f %4f %4f %4f (V)", &RMS[0], &RMS[1], &RMS[2], &RMS[3]);
+	printf("%4f %4f %4f (V)", &RMS[0], &RMS[1], &RMS[2]);
 }
 
 static void
@@ -1320,6 +1306,7 @@ reg_format_enum(const reg_t *reg)
 		case ID_PM_CONFIG_TVM:
 		case ID_PM_CONFIG_DBG:
 		case ID_PM_CONFIG_VSI_CLAMP:
+		case ID_PM_CONFIG_DTC_VOLTAGE:
 		case ID_PM_CONFIG_LU_FORCED:
 		case ID_PM_CONFIG_LU_FREEWHEEL:
 		case ID_PM_CONFIG_HFI_PERMANENT:
@@ -1462,30 +1449,6 @@ reg_format_enum(const reg_t *reg)
 			printf("(%s)", pm_strerror(val));
 			break;
 
-		case ID_PM_VSI_MASK_XF:
-
-			switch (val) {
-
-				PM_SFI_CASE(PM_MASK_NONE);
-				PM_SFI_CASE(PM_MASK_A);
-				PM_SFI_CASE(PM_MASK_B);
-				PM_SFI_CASE(PM_MASK_C);
-
-				default: break;
-			}
-			break;
-
-		case ID_PM_TVM_ACTIVE:
-
-			switch (val) {
-
-				PM_SFI_CASE(PM_DISABLED);
-				PM_SFI_CASE(PM_ENABLED);
-
-				default: break;
-			}
-			break;
-
 		case ID_PM_LU_MODE:
 
 			switch (val) {
@@ -1572,7 +1535,6 @@ const reg_t		regfile[] = {
 	REG_DEF(hal.ADC_amplifier_gain,,,	"",	"%1f",	REG_CONFIG, &reg_proc_ADC, NULL),
 	REG_DEF(hal.ADC_voltage_ratio,,,	"",	"%4e",	REG_CONFIG, &reg_proc_ADC, NULL),
 	REG_DEF(hal.ADC_terminal_ratio,,,	"",	"%4e",	REG_CONFIG, &reg_proc_ADC, NULL),
-	REG_DEF(hal.ADC_terminal_bias,,,	"",	"%4e",	REG_CONFIG, &reg_proc_ADC, NULL),
 
 #ifdef HW_HAVE_ANALOG_KNOB
 	REG_DEF(hal.ADC_knob_ratio,,,		"",	"%4e",	REG_CONFIG, &reg_proc_ADC, NULL),
@@ -1770,7 +1732,9 @@ const reg_t		regfile[] = {
 	REG_DEF(pm.self_IST,,,			"",	"%0i",	REG_READ_ONLY, NULL, &reg_format_self_IST),
 	REG_DEF(pm.self_STDi,,,			"",	"%0i",	REG_READ_ONLY, NULL, &reg_format_self_RMSi),
 	REG_DEF(pm.self_RMSi,,,			"",	"%0i",	REG_READ_ONLY, NULL, &reg_format_self_RMSi),
-	REG_DEF(pm.self_RMSu,,,			"",	"%0i",	REG_READ_ONLY, NULL, &reg_format_self_RMSu),
+	REG_DEF(pm.self_RMSu,,,			"V",	"%4f",	REG_READ_ONLY, NULL, NULL),
+	REG_DEF(pm.self_RMSt,,,			"",	"%0i",	REG_READ_ONLY, NULL, &reg_format_self_RMSt),
+	REG_DEF(pm.self_DTu,,,			"V",	"%4f",	REG_READ_ONLY, NULL, NULL),
 
 	REG_DEF(pm.config_NOP,,,		"",	"%0i",	REG_CONFIG, NULL, &reg_format_enum),
 	REG_DEF(pm.config_IFB,,,		"",	"%0i",	REG_CONFIG, NULL, &reg_format_enum),
@@ -1779,6 +1743,7 @@ const reg_t		regfile[] = {
 
 	REG_DEF(pm.config_VSI_ZERO,,,		"",	"%0i",	REG_CONFIG, NULL, &reg_format_enum),
 	REG_DEF(pm.config_VSI_CLAMP,,,		"",	"%0i",	REG_CONFIG, NULL, &reg_format_enum),
+	REG_DEF(pm.config_DTC_VOLTAGE,,,	"",	"%0i",	REG_CONFIG, NULL, &reg_format_enum),
 	REG_DEF(pm.config_LU_FORCED,,,		"",	"%0i",	REG_CONFIG, NULL, &reg_format_enum),
 	REG_DEF(pm.config_LU_FREEWHEEL,,,	"",	"%0i",	REG_CONFIG, NULL, &reg_format_enum),
 	REG_DEF(pm.config_LU_ESTIMATE,,,	"",	"%0i",	REG_CONFIG, NULL, &reg_format_enum),
@@ -1867,37 +1832,22 @@ const reg_t		regfile[] = {
 	REG_DEF(pm.vsi_X,,,			"V",	"%3f",	REG_READ_ONLY, NULL, NULL),
 	REG_DEF(pm.vsi_Y,,,			"V",	"%3f",	REG_READ_ONLY, NULL, NULL),
 	REG_DEF(pm.vsi_gain_LP,,,		"",	"%2e",	REG_CONFIG, NULL, NULL),
-	REG_DEF(pm.vsi_mask_XF,,,		"",	"%0i",	REG_CONFIG, NULL, &reg_format_enum),
 
 	REG_DEF(pm.vsi_AF,,,			"",	"%0i",	REG_READ_ONLY, NULL, NULL),
 	REG_DEF(pm.vsi_BF,,,			"",	"%0i",	REG_READ_ONLY, NULL, NULL),
 	REG_DEF(pm.vsi_CF,,,			"",	"%0i",	REG_READ_ONLY, NULL, NULL),
 	REG_DEF(pm.vsi_IF,,,			"",	"%0i",	REG_READ_ONLY, NULL, NULL),
-	REG_DEF(pm.vsi_SF,,,			"",	"%0i",	REG_READ_ONLY, NULL, NULL),
 	REG_DEF(pm.vsi_UF,,,			"",	"%0i",	REG_READ_ONLY, NULL, NULL),
-	REG_DEF(pm.vsi_AQ,,,			"",	"%0i",	REG_READ_ONLY, NULL, NULL),
-	REG_DEF(pm.vsi_BQ,,,			"",	"%0i",	REG_READ_ONLY, NULL, NULL),
-	REG_DEF(pm.vsi_CQ,,,			"",	"%0i",	REG_READ_ONLY, NULL, NULL),
-
-	REG_DEF(pm.tvm_ACTIVE,,,		"",	"%0i",	REG_CONFIG, NULL, &reg_format_enum),
-	REG_DEF(pm.tvm_clean_zone,,,		"%",	"%1f",	REG_CONFIG, &reg_proc_percent, NULL),
-	REG_DEF(pm.tvm_A,,,			"V",	"%3f",	REG_READ_ONLY, NULL, NULL),
-	REG_DEF(pm.tvm_B,,,			"V",	"%3f",	REG_READ_ONLY, NULL, NULL),
-	REG_DEF(pm.tvm_C,,,			"V",	"%3f",	REG_READ_ONLY, NULL, NULL),
-	REG_DEF(pm.tvm_FIR_A, 0, [0],		"",	"%4e",	REG_CONFIG | REG_READ_ONLY | REG_HIDDEN, NULL, NULL),
-	REG_DEF(pm.tvm_FIR_A, 1, [1],		"",	"%4e",	REG_CONFIG | REG_READ_ONLY | REG_HIDDEN, NULL, NULL),
-	REG_DEF(pm.tvm_FIR_A, 2, [2],		"",	"%4e",	REG_CONFIG | REG_READ_ONLY | REG_HIDDEN, NULL, NULL),
-	REG_DEF(pm.tvm_FIR_A, _tau,,		"us",	"%3f",	REG_READ_ONLY, &reg_proc_tvm_FIR_tau, NULL),
-	REG_DEF(pm.tvm_FIR_B, 0, [0],		"",	"%4e",	REG_CONFIG | REG_READ_ONLY | REG_HIDDEN, NULL, NULL),
-	REG_DEF(pm.tvm_FIR_B, 1, [1],		"",	"%4e",	REG_CONFIG | REG_READ_ONLY | REG_HIDDEN, NULL, NULL),
-	REG_DEF(pm.tvm_FIR_B, 2, [2],		"",	"%4e",	REG_CONFIG | REG_READ_ONLY | REG_HIDDEN, NULL, NULL),
-	REG_DEF(pm.tvm_FIR_B, _tau,,		"us",	"%3f",	REG_READ_ONLY, &reg_proc_tvm_FIR_tau, NULL),
-	REG_DEF(pm.tvm_FIR_C, 0, [0],		"",	"%4e",	REG_CONFIG | REG_READ_ONLY | REG_HIDDEN, NULL, NULL),
-	REG_DEF(pm.tvm_FIR_C, 1, [1],		"",	"%4e",	REG_CONFIG | REG_READ_ONLY | REG_HIDDEN, NULL, NULL),
-	REG_DEF(pm.tvm_FIR_C, 2, [2],		"",	"%4e",	REG_CONFIG | REG_READ_ONLY | REG_HIDDEN, NULL, NULL),
-	REG_DEF(pm.tvm_FIR_C, _tau,,		"us",	"%3f",	REG_READ_ONLY, &reg_proc_tvm_FIR_tau, NULL),
-	REG_DEF(pm.tvm_X0,,,			"V",	"%3f",	REG_READ_ONLY, NULL, NULL),
-	REG_DEF(pm.tvm_Y0,,,			"V",	"%3f",	REG_READ_ONLY, NULL, NULL),
+	REG_DEF(pm.vsi_A0,,,			"",	"%0i",	REG_READ_ONLY, NULL, NULL),
+	REG_DEF(pm.vsi_B0,,,			"",	"%0i",	REG_READ_ONLY, NULL, NULL),
+	REG_DEF(pm.vsi_C0,,,			"",	"%0i",	REG_READ_ONLY, NULL, NULL),
+
+	REG_DEF(pm.dtc_deadband,,,		"ns",	"%1f",	REG_CONFIG, NULL, NULL),
+	REG_DEF(pm.dtc_tol,,,			"A",	"%3f",	REG_CONFIG, NULL, NULL),
+	REG_DEF(pm.dtc_uX,,,			"V",	"%3f",	REG_READ_ONLY, NULL, NULL),
+	REG_DEF(pm.dtc_uY,,,			"V",	"%3f",	REG_READ_ONLY, NULL, NULL),
+	REG_DEF(pm.dtc_X,,,			"V",	"%3f",	REG_READ_ONLY, NULL, NULL),
+	REG_DEF(pm.dtc_Y,,,			"V",	"%3f",	REG_READ_ONLY, NULL, NULL),
 
 	REG_DEF(pm.lu_MODE,,,			"",	"%0i",	REG_READ_ONLY, NULL, &reg_format_enum),
 	REG_DEF(pm.lu_iX,,,			"A",	"%3f",	REG_READ_ONLY, NULL, NULL),
@@ -1951,9 +1901,10 @@ const reg_t		regfile[] = {
 	REG_DEF(pm.flux_gain_SF,,,		"",	"%2e",	REG_CONFIG, NULL, NULL),
 	REG_DEF(pm.flux_gain_IF,,,		"%",	"%1f",	REG_CONFIG, &reg_proc_percent, NULL),
 
-	REG_DEF(pm.kalman_residual_D,,,		"A",	"%3f",	REG_READ_ONLY, NULL, NULL),
-	REG_DEF(pm.kalman_residual_Q,,,		"A",	"%3f",	REG_READ_ONLY, NULL, NULL),
+	REG_DEF(pm.kalman_E, 0, [0],		"A",	"%3f",	REG_READ_ONLY, NULL, NULL),
+	REG_DEF(pm.kalman_E, 1, [1],		"A",	"%3f",	REG_READ_ONLY, NULL, NULL),
 	REG_DEF(pm.kalman_bias_Q,,,		"V",	"%3f",	REG_READ_ONLY, NULL, NULL),
+	REG_DEF(pm.kalman_lpf_wS,,,	"rad/s",	"%2f",	REG_READ_ONLY, NULL, NULL),
 	REG_DEF(pm.kalman_gain_Q, 0, [0],	"",	"%2e",	REG_CONFIG, NULL, NULL),
 	REG_DEF(pm.kalman_gain_Q, 1, [1],	"",	"%2e",	REG_CONFIG, NULL, NULL),
 	REG_DEF(pm.kalman_gain_Q, 2, [2],	"",	"%2e",	REG_CONFIG, NULL, NULL),
@@ -2026,11 +1977,11 @@ const reg_t		regfile[] = {
 	REG_DEF(pm.const_Ja,,,		"ekgm2",	"%4g",	REG_CONFIG, NULL, NULL),
 	REG_DEF(pm.const_Ja, _kgm2,,		"kgm2",	"%4g",	0, &reg_proc_kgm2, NULL),
 	REG_DEF(pm.const_Ja, _kg,,		"kg",	"%4g",	0, &reg_proc_kg, NULL),
-	REG_DEF(pm.const_im_L1,,,		"H",	"%4g",	REG_CONFIG, NULL, NULL),
-	REG_DEF(pm.const_im_L2,,,		"H",	"%4g",	REG_CONFIG, NULL, NULL),
-	REG_DEF(pm.const_im_B,,,		"deg",	"%1f",	REG_CONFIG, NULL, NULL),
-	REG_DEF(pm.const_im_R,,,		"Ohm",	"%4g",	REG_CONFIG, NULL, NULL),
-	REG_DEF(pm.const_ld_S,,,		"mm",	"%3f",	REG_CONFIG, &reg_proc_mm, NULL),
+	REG_DEF(pm.const_im_Ld,,,		"H",	"%4g",	REG_CONFIG, NULL, NULL),
+	REG_DEF(pm.const_im_Lq,,,		"H",	"%4g",	REG_CONFIG, NULL, NULL),
+	REG_DEF(pm.const_im_A,,,		"deg",	"%1f",	REG_CONFIG, NULL, NULL),
+	REG_DEF(pm.const_im_Rz,,,		"Ohm",	"%4g",	REG_CONFIG, NULL, NULL),
+	REG_DEF(pm.const_ld_Sm,,,		"mm",	"%3f",	REG_CONFIG, &reg_proc_mm, NULL),
 
 	REG_DEF(pm.watt_DC_MAX,,,		"",	"%0i",	REG_READ_ONLY, NULL, &reg_format_enum),
 	REG_DEF(pm.watt_DC_MIN,,,		"",	"%0i",	REG_READ_ONLY, NULL, &reg_format_enum),
@@ -2057,6 +2008,7 @@ const reg_t		regfile[] = {
 	REG_DEF(pm.watt_gain_P,,,		"",	"%2e",	REG_CONFIG, NULL, NULL),
 	REG_DEF(pm.watt_gain_I,,,		"",	"%2e",	REG_CONFIG, NULL, NULL),
 	REG_DEF(pm.watt_gain_LP,,,		"",	"%2e",	REG_CONFIG, NULL, NULL),
+	REG_DEF(pm.watt_gain_WF,,,		"",	"%2e",	REG_CONFIG, NULL, NULL),
 
 	REG_DEF(pm.i_setpoint_current,,,	"A",	"%3f",	0, NULL, NULL),
 	REG_DEF(pm.i_setpoint_current, _pc,,	"%",	"%2f",	0, &reg_proc_current_pc, NULL),
diff --git a/src/shdefs.h b/src/shdefs.h
index 1a97a3c..97b2c6b 100644
--- a/src/shdefs.h
+++ b/src/shdefs.h
@@ -10,10 +10,10 @@ SH_DEF(ap_log_flush)
 SH_DEF(ap_log_clean)
 SH_DEF(ap_reboot)
 SH_DEF(ap_bootload)
+SH_DEF(pm_adjust_dtc_voltage)
 SH_DEF(pm_probe_impedance)
 SH_DEF(pm_probe_spinup)
 SH_DEF(pm_probe_detached)
-SH_DEF(pm_probe_const_resistance)
 SH_DEF(pm_probe_const_flux_linkage)
 SH_DEF(pm_probe_const_inertia)
 SH_DEF(pm_probe_noise_threshold)
@@ -34,6 +34,7 @@ SH_DEF(tlm_watch)
 SH_DEF(tlm_stop)
 SH_DEF(tlm_flush_sync)
 SH_DEF(tlm_live_sync)
+SH_DEF(help)
 #ifdef HW_HAVE_NETWORK_EPCAN
 SH_DEF(net_survey)
 #endif /* HW_HAVE_NETWORK_EPCAN */
@@ -48,7 +49,6 @@ SH_DEF(net_node_remote)
 #endif /* HW_HAVE_NETWORK_EPCAN */
 SH_DEF(pm_self_test)
 SH_DEF(pm_self_adjust)
-SH_DEF(pm_analysis_TVM)
 SH_DEF(pm_analysis_impedance)
 SH_DEF(hal_ADC_scan)
 SH_DEF(hal_PWM_set_DC)
diff --git a/src/shell.c b/src/shell.c
index 95f6b72..e427692 100644
--- a/src/shell.c
+++ b/src/shell.c
@@ -670,5 +670,23 @@ LD_TASK void task_CMDSH(void *pData)
 #undef SH_DEF
 #define SH_DEF(name)		void name(const char *s)
 
-/* TODO */
+SH_DEF(help)
+{
+	const sh_cmd_t		*cmd;
+
+	cmd = cmLIST;
+
+	do {
+		if (cmd->sym == NULL)
+			break;
+
+		if (strstr(cmd->sym, s) != NULL) {
+
+			printf("%s\n", cmd->sym);
+		}
+
+		++cmd;
+	}
+	while (1);
+}
 
diff --git a/src/tlm.c b/src/tlm.c
index 5a55836..1c658ac 100644
--- a/src/tlm.c
+++ b/src/tlm.c
@@ -20,22 +20,22 @@ void tlm_reg_default(tlm_t *tlm)
 	tlm->reg_ID[2] = ID_AP_TEMP_EXT;
 #endif /* HW_HAVE_NTC_MACHINE */
 	tlm->reg_ID[3] = ID_PM_VSI_DC;
-	tlm->reg_ID[4] = ID_PM_LU_ID;
-	tlm->reg_ID[5] = ID_PM_LU_IQ;
-	tlm->reg_ID[6] = ID_PM_LU_WS_RPM;
-	tlm->reg_ID[7] = ID_PM_LU_MQ_LOAD;
-	tlm->reg_ID[8] = ID_PM_CONST_FB_U;
-	tlm->reg_ID[9] = ID_PM_WATT_DRAIN_WA;
-
-	tlm->reg_ID[10] = ID_PM_WATT_CONSUMED_WH;
-	tlm->reg_ID[11] = ID_PM_WATT_REVERTED_WH;
-	tlm->reg_ID[12] = ID_PM_FB_IA;
-	tlm->reg_ID[13] = ID_PM_FB_IB;
-	tlm->reg_ID[14] = ID_PM_FB_IC;
-	tlm->reg_ID[15] = ID_PM_FB_UA;
-	tlm->reg_ID[16] = ID_PM_FB_UB;
-	tlm->reg_ID[17] = ID_PM_FB_UC;
-	tlm->reg_ID[18] = ID_NULL;
+	tlm->reg_ID[4] = ID_PM_LU_MODE;
+	tlm->reg_ID[5] = ID_PM_LU_ID;
+	tlm->reg_ID[6] = ID_PM_LU_IQ;
+	tlm->reg_ID[7] = ID_PM_LU_WS_RPM;
+	tlm->reg_ID[8] = ID_PM_LU_MQ_LOAD;
+	tlm->reg_ID[9] = ID_PM_CONST_FB_U;
+
+	tlm->reg_ID[10] = ID_PM_WATT_DRAIN_WA;
+	tlm->reg_ID[11] = ID_PM_WATT_CONSUMED_WH;
+	tlm->reg_ID[12] = ID_PM_WATT_REVERTED_WH;
+	tlm->reg_ID[13] = ID_PM_FB_IA;
+	tlm->reg_ID[14] = ID_PM_FB_IB;
+	tlm->reg_ID[15] = ID_PM_FB_IC;
+	tlm->reg_ID[16] = ID_PM_FB_UA;
+	tlm->reg_ID[17] = ID_PM_FB_UB;
+	tlm->reg_ID[18] = ID_PM_FB_UC;
 	tlm->reg_ID[19] = ID_NULL;
 }