From: Keith Packard <keithp@keithp.com>
Date: Tue, 22 Mar 2011 07:51:04 +0000 (+0900)
Subject: altos/kalman: Kalman terms can be > 1, use 32-bit fixed point
X-Git-Tag: 0.9.3~115
X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=commitdiff_plain;h=6864e06d88a5b908cffa7c4cd2be8969ff46ce4d

altos/kalman: Kalman terms can be > 1, use 32-bit fixed point

Because speed and acceleration are scaled by 16, it's fairly common
for the kalman terms to end up larger than 1. Instead of trying to
fuss with 16-bit values and shifts, just use 32-bit values.

Signed-off-by: Keith Packard <keithp@keithp.com>
---

diff --git a/src/kalman/kalman.5c b/src/kalman/kalman.5c
index f7347184..cfb7abea 100755
--- a/src/kalman/kalman.5c
+++ b/src/kalman/kalman.5c
@@ -460,19 +460,17 @@ void main() {
 		mat_t k = converge(param);
 		int[] d = dims(k);
 		int time_inc = floor(1/time_step + 0.5);
-		if (d[1] == 2) {
-			for (int i = 0; i < d[0]; i++)
-				for (int j = 0; j < d[1]; j++)
-					printf ("#define %s_K%d%d_%d to_fix16(%12.10f)\n",
-						prefix, i, j, time_inc, k[i,j]);
-		} else {
-			for (int i = 0; i < d[0]; i++) {
-				printf ("#define %s_K%d_%d to_fix16(%12.10f)\n",
-					prefix, i, time_inc, k[i,0]);
+		for (int i = 0; i < d[0]; i++)
+			for (int j = 0; j < d[1]; j++) {
+				string name;
+				if (d[1] == 1)
+					name = sprintf("%s_K%d_%d", prefix, i, time_inc);
+				else
+					name = sprintf("%s_K%d%d_%d", prefix, i, j, time_inc);
+				printf ("#define %s to_fix32(%12.10f)\n", name, k[i,j]);
 			}
-		}
 		printf ("\n");
-		return;
+		exit(0);
 	}
 	string[dim(argv) - user_argind] rest = { [i] = argv[i+user_argind] };