diff --git a/libraries/AP_Math/crc.cpp b/libraries/AP_Math/crc.cpp
index 2ec7441758..a53540de61 100644
--- a/libraries/AP_Math/crc.cpp
+++ b/libraries/AP_Math/crc.cpp
@@ -225,7 +225,33 @@ static const uint16_t crc16tab[256] = {
 
 uint16_t crc16_ccitt(const uint8_t *buf, uint32_t len, uint16_t crc)
 {
-    for (uint32_t i = 0; i < len; i++)
+    for (uint32_t i = 0; i < len; i++) {
         crc = (crc << 8) ^ crc16tab[((crc >> 8) ^ *buf++) & 0x00FF];
+    }
+    return crc;
+}
+
+/**
+ * Calculate Modbus CRC16 for array of bytes
+ * 
+ * @param [in] buf input buffer
+ * @param [in] len size of buffer
+ * @return CRC value
+ */
+uint16_t 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;
 }
diff --git a/libraries/AP_Math/crc.h b/libraries/AP_Math/crc.h
index c820f71668..cd81977785 100644
--- a/libraries/AP_Math/crc.h
+++ b/libraries/AP_Math/crc.h
@@ -25,4 +25,7 @@ uint32_t crc_crc32(uint32_t crc, const uint8_t *buf, uint32_t size);
 
 // Copyright (C) 2010 Swift Navigation Inc.
 // Contact: Fergus Noble <fergus@swift-nav.com>
-uint16_t crc16_ccitt(const uint8_t *buf, uint32_t len, uint16_t crc);
\ No newline at end of file
+uint16_t crc16_ccitt(const uint8_t *buf, uint32_t len, uint16_t crc);
+
+uint16_t calc_crc_modbus(uint8_t *buf, uint16_t len);
+