AP_Beacon: Common modbus crc method

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];

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();