Stabilize mt100eocg temperature readings4.4.6

1 files changed, 34 insertions, 11 deletions

diff --git a/io-module/spi.c b/io-module/spi.c
index 1244cfc..873caaf 100644
--- a/io-module/spi.c
+++ b/io-module/spi.c
@@ -348,22 +348,45 @@ static ssize_t mts_attr_show_board_temperature(struct device *dev,
 			char *buf)
 {
 	int tmp;
-	u16 temp_raw;
+	u16 temp_raw, save[3];
+	int i; int count, diff;
 
 	if (!spi_board_temp_dev) {
-		log_notice("spi_board_temp device not present");
-		return -ENODEV;
+            log_notice("spi_board_temp device not present");
+            return -ENODEV;
 	}
 
-	tmp = spi_readn(spi_board_temp_dev, (u8 *) buf, 2);
-	if (tmp) {
-		log_error("spi_readn failed %d", tmp);
-		return tmp;
+	for(count = 3; count > 0; count--) {
+            for (i=0;i<3;i++) {
+                tmp = spi_readn(spi_board_temp_dev, (u8 *) buf, 2);
+                if (tmp) {
+                        log_error("spi_readn failed %d", tmp);
+                        return tmp;
+                }
+                temp_raw = ((u8 *) buf)[0] << 8 | ((u8 *) buf)[1];
+                save[i] = temp_raw;
+            }
+            log_debug("temp: 0x%04X", temp_raw);
+
+            /* If temp reading is very small, use the last
+             * reading, because it could bounce around more.
+             */
+            if (save[2] < 50)
+                break;
+            diff = save[2] - save[1];
+            if (diff < 0)
+                diff = -diff;
+            if (diff == 0)
+                break;
+            /* If temperature difference of the last two readings is less
+             * than 10%, use the last reading */
+            if ((save[2] / diff) > 10)
+                break;
+            /* If we have tried 3 times and we still have more than a 10%
+             * difference, we give up */
 	}
-	temp_raw = ((u8 *) buf)[0] << 8 | ((u8 *) buf)[1];
-
-	log_debug("temp: 0x%04X", temp_raw);
-
+	if (count == 0)
+            log_info("temp=%d, count=%d, diff=%d, save[0]=%d, save[1]=%d\n", temp_raw,count,diff,save[0],save[1]);
 	return sprintf(buf, "%d\n", ADT7302_to_celsius(temp_raw));
 }