From 89484a8f645590caea0c127dddbcfdbaa7848db9 Mon Sep 17 00:00:00 2001
From: murata <ma2maru@gmail.com>
Date: Fri, 19 Apr 2019 19:47:11 +0900
Subject: [PATCH] AP_Beacon: Common modbus crc method

---
 libraries/AP_Beacon/AP_Beacon_Marvelmind.cpp | 26 +-------------------
 libraries/AP_Beacon/AP_Beacon_Marvelmind.h   |  1 -
 2 files changed, 1 insertion(+), 26 deletions(-)

diff --git a/libraries/AP_Beacon/AP_Beacon_Marvelmind.cpp b/libraries/AP_Beacon/AP_Beacon_Marvelmind.cpp
index e043bdbeb9..029a5ae7a5 100644
--- a/libraries/AP_Beacon/AP_Beacon_Marvelmind.cpp
+++ b/libraries/AP_Beacon/AP_Beacon_Marvelmind.cpp
@@ -19,7 +19,7 @@
  */
 
 #include <AP_HAL/AP_HAL.h>
-
+#include <AP_Math/crc.h>
 #include "AP_Beacon_Marvelmind.h"
 
 #define AP_BEACON_MARVELMIND_POSITION_DATAGRAM_ID 0x0001
@@ -56,30 +56,6 @@ AP_Beacon_Marvelmind::AP_Beacon_Marvelmind(AP_Beacon &frontend, AP_SerialManager
     }
 }
 
-//////////////////////////////////////////////////////////////////////////////
-// Calculate Modbus CRC16 for array of bytes
-// buf: input buffer
-// len: size of buffer
-// returncode: CRC value
-//////////////////////////////////////////////////////////////////////////////
-uint16_t AP_Beacon_Marvelmind::calc_crc_modbus(uint8_t *buf, uint16_t len)
-{
-    uint16_t crc = 0xFFFF;
-    for (uint16_t pos = 0; pos < len; pos++) {
-        crc ^= (uint16_t) buf[pos]; // XOR byte into least sig. byte of crc
-        for (uint8_t i = 8; i != 0; i--) { // Loop over each bit
-            if ((crc & 0x0001) != 0) { // If the LSB is set
-                crc >>= 1; // Shift right and XOR 0xA001
-                crc ^= 0xA001;
-            } else {
-                // Else LSB is not set
-                crc >>= 1; // Just shift right
-            }
-        }
-    }
-    return crc;
-}
-
 void AP_Beacon_Marvelmind::process_position_datagram()
 {
     hedge.cur_position.address = input_buffer[16];
diff --git a/libraries/AP_Beacon/AP_Beacon_Marvelmind.h b/libraries/AP_Beacon/AP_Beacon_Marvelmind.h
index fe34e7b9da..de8e0cfb0e 100644
--- a/libraries/AP_Beacon/AP_Beacon_Marvelmind.h
+++ b/libraries/AP_Beacon/AP_Beacon_Marvelmind.h
@@ -79,7 +79,6 @@ private:
     uint16_t num_bytes_in_block_received;
     uint16_t data_id;
 
-    uint16_t calc_crc_modbus(uint8_t *buf, uint16_t len);
     StationaryBeaconPosition* get_or_alloc_beacon(uint8_t address);
     void process_beacons_positions_datagram();
     void process_beacons_positions_highres_datagram();