Added octave source of Kalman filter calculation.

bench/bench.c

@@ -426,8 +426,6 @@ void bench_script()
 	pm.s_setpoint_speed = 2000.f;
 	sim_runtime(2.0);
 
-
-
 	sim_runtime(3.0);
 
 	tlm_PWM_grab();

bench/tsfunc.h

@@ -11,6 +11,9 @@ int ts_wait_IDLE();
 int ts_wait_motion();
 int ts_wait_spinup();
 
+void ts_adjust_sensor_hall();
+void ts_adjust_sensor_eabi();
+
 void ts_script_default();
 void ts_script_base();
 void ts_script_test();

src/app/autostart.c

@@ -14,7 +14,7 @@ LD_TASK void app_AUTOSTART(void *pData)
 {
 	volatile int		*lknob = (volatile int *) pData;
 
-	if (xTaskGetTickCount() >= (TickType_t) 5000) {
+	if (xTaskGetTickCount() >= (TickType_t) 1000) {
 
 		/* If the application task was started much later than powerup
 		 * we will pause to give you time for flash programing.

src/phobia/pm.c

@@ -127,7 +127,7 @@ pm_auto_config_default(pmc_t *pm)
 	pm->config_DTC_VOLTAGE = PM_ENABLED;
 	pm->config_LU_FORCED = PM_ENABLED;
 	pm->config_LU_FREEWHEEL = PM_ENABLED;
-	pm->config_LU_ESTIMATE = PM_FLUX_KALMAN;
+	pm->config_LU_ESTIMATE = PM_FLUX_ORTEGA;
 	pm->config_LU_SENSOR = PM_SENSOR_NONE;
 	pm->config_LU_LOCATION = PM_LOCATION_NONE;
 	pm->config_LU_DRIVE = PM_DRIVE_SPEED;
@@ -993,7 +993,15 @@ pm_kalman_forecast(pmc_t *pm)
 	const float	*A = pm->kalman_A;
 	const float	*Q = pm->kalman_gain_Q;
 
-	float		iX, iY, uX, uY, fC, fS, wS, bQ;
+	const float	iX = A[0];
+	const float	iY = A[1];
+	const float	uX = A[2];
+	const float	uY = A[3];
+	const float	fC = A[4];
+	const float	fS = A[5];
+	const float	wS = A[6];
+	const float	bQ = A[7];
+
 	float		u[17], F[10], R1, E1;
 
 	/*
@@ -1015,15 +1023,6 @@ pm_kalman_forecast(pmc_t *pm)
 	 *
 	 * */
 
-	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;

util/kalman.m

@@ -0,0 +1,68 @@
+#!/usr/bin/octave -q
+
+pkg load symbolic
+
+syms Ix Iy th w Rs Ld Lq lam T Ux Uy bias real
+
+% We derive Jacobian matrix \F from PMSM equations in stationary frame.
+%
+Ath = [cos(th) sin(th); -sin(th) cos(th)];		% XY -> DQ
+Awt = [-sin(2*th) cos(2*th); cos(2*th) sin(2*th)];
+
+L = simplify(Ath' * diag([1/Ld; 1/Lq]) * Ath);
+U = simplify([Ux; Uy] - Rs * [Ix; Iy] ...
+		- w * (Ld - Lq) * Awt * [Ix; Iy] ...
+		+ Ath' * [0; - w * lam + bias]);
+
+f = [Ix; Iy; th; w; bias] + [L * U; w; 0; 0] * T;
+h = [Ix; Iy];
+
+F = simplify([diff(f, Ix) diff(f, Iy) diff(f, th) diff(f, w) diff(f, bias)]);
+H = simplify([diff(h, Ix) diff(h, Iy) diff(h, th) diff(h, w) diff(h, bias)]);
+
+F = simplify(eval(F));
+H = eval(H);
+
+% Try to construct the same matrix \F from scratch.
+%
+Fb = sym(eye(5,5));
+Fw = sym(zeros(5,5));
+
+u(1) = (1/Ld - 1/Lq) * (Ux - Rs * Ix);
+u(2) = (1/Ld - 1/Lq) * (Uy - Rs * Iy);
+u(3) = (w*lam - bias) / Lq;
+
+u(4) = Ix * cos(2*th) + Iy * sin(2*th);
+u(5) = Iy * cos(2*th) - Ix * sin(2*th);
+
+Fb(3,4) = T;
+
+Fb(1,1) = 1 - T * Rs * (1/Ld - sin(th)^2 * (1/Ld - 1/Lq));
+Fb(1,2) =   - T * Rs * sin(th) * cos(th) * (1/Ld - 1/Lq);
+Fb(2,1) =   - T * Rs * sin(th) * cos(th) * (1/Ld - 1/Lq);
+Fb(2,2) = 1 - T * Rs * (1/Lq + sin(th)^2 * (1/Ld - 1/Lq));
+
+Fb(1,3) = T * (u(2) * cos(th*2) - u(1) * sin(th*2) + u(3) * cos(th));
+Fb(2,3) = T * (u(1) * cos(th*2) + u(2) * sin(th*2) + u(3) * sin(th));
+
+Fb(1,4) =   T * lam * sin(th) / Lq;
+Fb(2,4) = - T * lam * cos(th) / Lq;
+
+Fb(1,5) = - T * sin(th) / Lq;
+Fb(2,5) =   T * cos(th) / Lq;
+
+Fw(1,1) =   T * w * sin(th) * cos(th)        * (Ld/Lq - Lq/Ld);
+Fw(1,2) =   T * w * ((Ld/Lq - 1) - cos(th)^2 * (Ld/Lq - Lq/Ld));
+Fw(2,1) =   T * w * ((1 - Lq/Ld) - cos(th)^2 * (Ld/Lq - Lq/Ld));
+Fw(2,2) = - T * w * sin(th) * cos(th)        * (Ld/Lq - Lq/Ld);
+
+Fw(1,3) =   T * w * u(4) * (Ld/Lq - Lq/Ld);
+Fw(2,3) = - T * w * u(5) * (Ld/Lq - Lq/Ld);
+
+Fw(1,4) = - T / 2 * (Iy * (Ld - Lq) - u(5) * (Ld + Lq)) * (1/Ld - 1/Lq);
+Fw(2,4) =   T / 2 * (Ix * (Ld - Lq) + u(4) * (Ld + Lq)) * (1/Ld - 1/Lq);
+
+% Check for equality.
+%
+simplify(F - (Fb + Fw))
+

util/kty83.txt

@@ -0,0 +1,28 @@
+t@C t@F tk rmin@Ohm rtyp@Ohm rmax@Ohm
+-55 -67 0.97 485 500 515
+-50 -58 0.96 510 525 540
+-40 -40 0.93 562 577 592
+-30 -22 0.91 617 632 647
+-20 -4 0.88 677 691 706
+-10 14 0.85 740 754 768
+0 32 0.83 807 820 833
+10 50 0.80 877 889 902
+20 68 0.78 951 962 973
+25 77 0.76 990 1000 1010
+30 86 0.75 1027 1039 1050
+40 104 0.73 1105 1118 1132
+50 122 0.71 1185 1202 1219
+60 140 0.69 1268 1288 1309
+70 158 0.67 1355 1379 1402
+80 176 0.65 1445 1472 1500
+90 194 0.63 1537 1569 1601
+100 212 0.61 1633 1670 1707
+110 230 0.60 1732 1774 1816
+120 248 0.58 1834 1882 1929
+125 257 0.57 1886 1937 1987
+130 266 0.57 1939 1993 2046
+140 284 0.55 2047 2107 2167
+150 302 0.54 2158 2225 2292
+160 320 0.52 2272 2346 2420
+170 338 0.51 2389 2471 2553
+175 347 0.51 2449 2535 2621

util/kty84.txt

@@ -0,0 +1,37 @@
+t@C t@F tk rmin@Ohm rtyp@Ohm rmax@Ohm
+-40 -40 0.84 340 359 379
+-30 -22 0.83 370 391 411
+-20 -4 0.82 403 424 446
+-10 14 0.80 437 460 483
+0 32 0.79 474 498 522
+10 50 0.77 514 538 563
+20 68 0.75 555 581 607
+25 77 0.74 577 603 629
+30 86 0.73 599 626 652
+40 104 0.71 645 672 700
+50 122 0.70 694 722 750
+60 140 0.68 744 773 801
+70 158 0.66 797 826 855
+80 176 0.64 852 882 912
+90 194 0.63 910 940 970
+100 212 0.61 970 1000 1030
+110 230 0.60 1029 1062 1096
+120 248 0.58 1089 1127 1164
+130 266 0.57 1152 1194 1235
+140 284 0.55 1216 1262 1309
+150 302 0.54 1282 1334 1385
+160 320 0.53 1350 1407 1463
+170 338 0.52 1420 1482 1544
+180 356 0.51 1492 1560 1628
+190 374 0.49 1566 1640 1714
+200 392 0.48 1641 1722 1803
+210 410 0.47 1719 1807 1894
+220 428 0.46 1798 1893 1988
+230 446 0.45 1879 1982 2085
+240 464 0.44 1962 2073 2184
+250 482 0.44 2046 2166 2286
+260 500 0.42 2132 2261 2390
+270 518 0.41 2219 2357 2496
+280 536 0.38 2304 2452 2600
+290 554 0.34 2384 2542 2700
+300 572 0.29 2456 2624 2791

util/lmt87.txt

@@ -0,0 +1,23 @@
+temp@C vout@mV
+-50 3277
+-45 3221
+-40 3160
+-30 3030
+-20 2899
+-10 2767
+0 2633
+10 2500
+20 2363
+30 2231
+40 2095
+50 1958
+60 1819
+70 1679
+80 1539
+90 1399
+100 1257
+110 1115
+120 973
+130 829
+140 684
+150 538