From 716cc26082b17db98b283e60faf440aed804906c Mon Sep 17 00:00:00 2001
From: murata <ma2maru@gmail.com>
Date: Wed, 24 Apr 2019 15:51:54 +0900
Subject: [PATCH] AP_Baro: Commonize the CRC4 method

---
 libraries/AP_Baro/AP_Baro_MS5611.cpp | 33 +++-------------------------
 libraries/AP_Baro/AP_Baro_MS5611.h   |  2 --
 2 files changed, 3 insertions(+), 32 deletions(-)

diff --git a/libraries/AP_Baro/AP_Baro_MS5611.cpp b/libraries/AP_Baro/AP_Baro_MS5611.cpp
index 9fa08554c2..86a531deb5 100644
--- a/libraries/AP_Baro/AP_Baro_MS5611.cpp
+++ b/libraries/AP_Baro/AP_Baro_MS5611.cpp
@@ -18,6 +18,7 @@
 #include <stdio.h>
 
 #include <AP_Math/AP_Math.h>
+#include <AP_Math/crc.h>
 
 extern const AP_HAL::HAL &hal;
 
@@ -150,34 +151,6 @@ bool AP_Baro_MS56XX::_init()
     return true;
 }
 
-/**
- * MS56XX crc4 method from datasheet for 16 bytes (8 short values)
- */
-uint16_t AP_Baro_MS56XX::crc4(uint16_t *data)
-{
-    uint16_t n_rem = 0;
-    uint8_t n_bit;
-
-    for (uint8_t cnt = 0; cnt < 16; cnt++) {
-        /* uneven bytes */
-        if (cnt & 1) {
-            n_rem ^= (uint8_t)((data[cnt >> 1]) & 0x00FF);
-        } else {
-            n_rem ^= (uint8_t)(data[cnt >> 1] >> 8);
-        }
-
-        for (n_bit = 8; n_bit > 0; n_bit--) {
-            if (n_rem & 0x8000) {
-                n_rem = (n_rem << 1) ^ 0x3000;
-            } else {
-                n_rem = (n_rem << 1);
-            }
-        }
-    }
-
-    return (n_rem >> 12) & 0xF;
-}
-
 uint16_t AP_Baro_MS56XX::_read_prom_word(uint8_t word)
 {
     const uint8_t reg = CMD_MS56XX_PROM + (word << 1);
@@ -224,7 +197,7 @@ bool AP_Baro_MS56XX::_read_prom_5611(uint16_t prom[8])
     /* remove CRC byte */
     prom[7] &= 0xff00;
 
-    return crc_read == crc4(prom);
+    return crc_read == crc_crc4(prom);
 }
 
 bool AP_Baro_MS56XX::_read_prom_5637(uint16_t prom[8])
@@ -257,7 +230,7 @@ bool AP_Baro_MS56XX::_read_prom_5637(uint16_t prom[8])
     /* remove CRC byte */
     prom[0] &= ~0xf000;
 
-    return crc_read == crc4(prom);
+    return crc_read == crc_crc4(prom);
 }
 
 /*
diff --git a/libraries/AP_Baro/AP_Baro_MS5611.h b/libraries/AP_Baro/AP_Baro_MS5611.h
index 9af6d0c58c..146ceff37f 100644
--- a/libraries/AP_Baro/AP_Baro_MS5611.h
+++ b/libraries/AP_Baro/AP_Baro_MS5611.h
@@ -40,8 +40,6 @@ public:
 
     static AP_Baro_Backend *probe(AP_Baro &baro, AP_HAL::OwnPtr<AP_HAL::Device> dev, enum MS56XX_TYPE ms56xx_type = BARO_MS5611);
 
-    static uint16_t crc4(uint16_t *data);
-    
 private:
     /*
      * Update @accum and @count with the new sample in @val, taking into