From 3319c3ccdcc9e6d2979f45b809e59c02316a64fa Mon Sep 17 00:00:00 2001
From: Michael du Breuil <wicked.shell.scripts@gmail.com>
Date: Wed, 19 Apr 2017 12:15:24 -0700
Subject: [PATCH] AP_BattMonitor: Use a common PEC helper

---
 .../AP_BattMonitor/AP_BattMonitor_SMBus.cpp   | 43 +++++++++++++++++++
 .../AP_BattMonitor/AP_BattMonitor_SMBus.h     |  7 +++
 .../AP_BattMonitor_SMBus_Maxell.cpp           | 40 -----------------
 .../AP_BattMonitor_SMBus_Maxell.h             |  4 --
 .../AP_BattMonitor_SMBus_Solo.cpp             | 41 ------------------
 .../AP_BattMonitor_SMBus_Solo.h               |  4 --
 6 files changed, 50 insertions(+), 89 deletions(-)
 create mode 100644 libraries/AP_BattMonitor/AP_BattMonitor_SMBus.cpp

diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus.cpp
new file mode 100644
index 0000000000..7097be4658
--- /dev/null
+++ b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus.cpp
@@ -0,0 +1,43 @@
+#include "AP_BattMonitor_SMBus.h"
+
+#define AP_BATTMONITOR_SMBUS_PEC_POLYNOME 0x07 // Polynome for CRC generation
+
+/// get_PEC - calculate packet error correction code of buffer
+uint8_t AP_BattMonitor_SMBus::get_PEC(const uint8_t i2c_addr, uint8_t cmd, bool reading, const uint8_t buff[], uint8_t len) const
+{
+    // exit immediately if no data
+    if (len == 0) {
+        return 0;
+    }
+
+    // prepare temp buffer for calcing crc
+    uint8_t tmp_buff[len+3];
+    tmp_buff[0] = i2c_addr << 1;
+    tmp_buff[1] = cmd;
+    tmp_buff[2] = tmp_buff[0] | (uint8_t)reading;
+    memcpy(&tmp_buff[3],buff,len);
+
+    // initialise crc to zero
+    uint8_t crc = 0;
+    uint8_t shift_reg = 0;
+    bool do_invert;
+
+    // for each byte in the stream
+    for (uint8_t i=0; i<sizeof(tmp_buff); i++) {
+        // load next data byte into the shift register
+        shift_reg = tmp_buff[i];
+        // for each bit in the current byte
+        for (uint8_t j=0; j<8; j++) {
+            do_invert = (crc ^ shift_reg) & 0x80;
+            crc <<= 1;
+            shift_reg <<= 1;
+            if(do_invert) {
+                crc ^= AP_BATTMONITOR_SMBUS_PEC_POLYNOME;
+            }
+        }
+    }
+
+    // return result
+    return crc;
+}
+
diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus.h b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus.h
index 6a82791894..9a8d424796 100644
--- a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus.h
+++ b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus.h
@@ -22,6 +22,13 @@ public:
     // virtual destructor to reduce compiler warnings
     virtual ~AP_BattMonitor_SMBus() {}
 
+
+protected:
+
+    // get_PEC - calculate PEC for a read or write from the battery
+    // buff is the data that was read or will be written
+    uint8_t get_PEC(const uint8_t i2c_addr, uint8_t cmd, bool reading, const uint8_t buff[], uint8_t len) const;
+
 };
 
 // include specific implementations
diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Maxell.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Maxell.cpp
index ccc315e5e6..a3aed1f750 100755
--- a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Maxell.cpp
+++ b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Maxell.cpp
@@ -26,8 +26,6 @@ uint8_t maxell_cell_ids[] = { 0x3f,  // cell 1
 
 #define SMBUS_READ_BLOCK_MAXIMUM_TRANSFER    0x20   // A Block Read or Write is allowed to transfer a maximum of 32 data bytes.
 
-#define SMBUS_PEC_POLYNOME  0x07 // Polynome for CRC generation
-
 /*
  * Other potentially useful registers, listed here for future use
  * #define BATTMONITOR_SMBUS_MAXELL_CHARGE_STATUS         0x0d    // relative state of charge
@@ -218,41 +216,3 @@ bool AP_BattMonitor_SMBus_Maxell::check_pec_support()
 	return true;
 }
 
-/// get_PEC - calculate packet error correction code of buffer
-uint8_t AP_BattMonitor_SMBus_Maxell::get_PEC(const uint8_t i2c_addr, uint8_t cmd, bool reading, const uint8_t buff[], uint8_t len) const
-{
-    // exit immediately if no data
-    if (len == 0) {
-        return 0;
-    }
-
-    // prepare temp buffer for calcing crc
-    uint8_t tmp_buff[len+3];
-    tmp_buff[0] = i2c_addr << 1;
-    tmp_buff[1] = cmd;
-    tmp_buff[2] = tmp_buff[0] | (uint8_t)reading;
-    memcpy(&tmp_buff[3],buff,len);
-
-    // initialise crc to zero
-    uint8_t crc = 0;
-    uint8_t shift_reg = 0;
-    bool do_invert;
-
-    // for each byte in the stream
-    for (uint8_t i=0; i<sizeof(tmp_buff); i++) {
-        // load next data byte into the shift register
-        shift_reg = tmp_buff[i];
-        // for each bit in the current byte
-        for (uint8_t j=0; j<8; j++) {
-            do_invert = (crc ^ shift_reg) & 0x80;
-            crc <<= 1;
-            shift_reg <<= 1;
-            if(do_invert) {
-                crc ^= SMBUS_PEC_POLYNOME;
-            }
-        }
-    }
-
-    // return result
-    return crc;
-}
diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Maxell.h b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Maxell.h
index 7894be1550..c6c8f2b91d 100755
--- a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Maxell.h
+++ b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Maxell.h
@@ -33,10 +33,6 @@ private:
     // read_block - returns number of characters read if successful, zero if unsuccessful
     uint8_t read_block(uint8_t reg, uint8_t* data, bool append_zero) const;
 
-    // get_PEC - calculate PEC for a read or write from the battery
-    //  buff is the data that was read or will be written
-    uint8_t get_PEC(const uint8_t i2c_addr, uint8_t cmd, bool reading, const uint8_t buff[], uint8_t len) const;
-
     AP_HAL::OwnPtr<AP_HAL::I2CDevice> _dev;
     uint8_t _pec_confirmed; // count of the number of times PEC has been confirmed as working
     bool _pec_supported;    // true if pec is supported
diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Solo.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Solo.cpp
index 0b2dba6c2c..b2970d98a7 100644
--- a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Solo.cpp
+++ b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Solo.cpp
@@ -189,44 +189,3 @@ uint8_t AP_BattMonitor_SMBus_Solo::read_block(uint8_t reg, uint8_t* data, uint8_
     return bufflen;
 }
 
-#define SMBUS_PEC_POLYNOME  0x07 // Polynome for CRC generation
-
-/// get_PEC - calculate packet error correction code of buffer
-uint8_t AP_BattMonitor_SMBus_Solo::get_PEC(const uint8_t i2c_addr, uint8_t cmd, bool reading, const uint8_t buff[], uint8_t len) const
-{
-    // exit immediately if no data
-    if (len <= 0) {
-        return 0;
-    }
-
-    // prepare temp buffer for calcing crc
-    uint8_t tmp_buff[len+3];
-    tmp_buff[0] = i2c_addr << 1;
-    tmp_buff[1] = cmd;
-    tmp_buff[2] = tmp_buff[0] | (uint8_t)reading;
-    memcpy(&tmp_buff[3],buff,len);
-
-    // initialise crc to zero
-    uint8_t crc = 0;
-    uint8_t shift_reg = 0;
-    bool do_invert;
-
-    // for each byte in the stream
-    for (uint8_t i=0; i<sizeof(tmp_buff); i++) {
-        // load next data byte into the shift register
-        shift_reg = tmp_buff[i];
-        // for each bit in the current byte
-        for (uint8_t j=0; j<8; j++) {
-            do_invert = (crc ^ shift_reg) & 0x80;
-            crc <<= 1;
-            shift_reg <<= 1;
-            if(do_invert) {
-                crc ^= SMBUS_PEC_POLYNOME;
-            }
-        }
-    }
-
-    // return result
-    return crc;
-}
-
diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Solo.h b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Solo.h
index 0e3c8a40d9..a3485082b4 100644
--- a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Solo.h
+++ b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Solo.h
@@ -29,10 +29,6 @@ private:
     // read_block - returns number of characters read if successful, zero if unsuccessful
     uint8_t read_block(uint8_t reg, uint8_t* data, uint8_t max_len, bool append_zero) const;
 
-    // get_PEC - calculate PEC for a read or write from the battery
-    //  buff is the data that was read or will be written
-    uint8_t get_PEC(const uint8_t i2c_addr, uint8_t cmd, bool reading, const uint8_t buff[], uint8_t len) const;
-
     AP_HAL::OwnPtr<AP_HAL::I2CDevice> _dev;
     uint8_t _button_press_count;
 };