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;ds=sidebyside

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] };