altos: Make sure pa to altitude conversion is done with 32 bits

[fw/altos] / src / core / ao_freq.c

diff --git a/src/core/ao_freq.c b/src/core/ao_freq.c
index 13bcb383e8914d04e38aa8a02fa6a3be2d7cbdeb..12496f6fdf785a2d94f5fac84c239092ede0e004 100644 (file)
--- a/src/core/ao_freq.c
+++ b/src/core/ao_freq.c
@@ -25,25 +25,28 @@
  * frequency, and updating the radio setting along the way
  */
 
-int32_t ao_freq_to_set(int32_t freq, int32_t cal) {
-       __pdata int32_t set = 0;
-       uint8_t neg = 0;
-       __pdata int32_t error = -434550 / 2;
+int32_t ao_freq_to_set(int32_t freq, int32_t cal) __reentrant
+{
+       static __pdata int32_t  set;
+       static __pdata uint8_t  neg;
+       static __pdata int32_t  error;
 
-       freq -= 434550;
-       if (freq < 0) {
+       set = 0;
+       neg = 0;
+       error = -434550 / 2;
+
+       if ((freq -= 434550) < 0) {
                neg = 1;
                freq = -freq;
        }
        for (;;) {
-               if (freq == 0 && error <= 0)
-                       break;
                if (error > 0) {
                        error -= 434550;
                        set++;
                } else {
                        error += cal;
-                       freq--;
+                       if (--freq < 0)
+                               break;
                }
        }
        if (neg)