From 6864e06d88a5b908cffa7c4cd2be8969ff46ce4d Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Tue, 22 Mar 2011 16:51:04 +0900 Subject: [PATCH] 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 --- src/kalman/kalman.5c | 20 +++++++++----------- 1 file changed, 9 insertions(+), 11 deletions(-) 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] }; -- 2.30.2