diff --git a/js/data_storage.js b/js/data_storage.js
index dffb6a7b..cd8d28e1 100755
--- a/js/data_storage.js
+++ b/js/data_storage.js
@@ -93,6 +93,7 @@ var SENSOR_DATA = {
     accelerometer: [0, 0, 0],
     magnetometer:  [0, 0, 0],
     altitude:      0,
+    sonar:         0,
     kinematics:    [0.0, 0.0, 0.0],
     debug:         [0, 0, 0, 0]
 };
@@ -143,4 +144,4 @@ var MISC = {
     vbatmincellvoltage:     0,
     vbatmaxcellvoltage:     0,
     vbatwarningcellvoltage: 0
-};
\ No newline at end of file
+};
diff --git a/js/msp.js b/js/msp.js
index 09bb13e9..e7fa7555 100644
--- a/js/msp.js
+++ b/js/msp.js
@@ -42,6 +42,7 @@ var MSP_codes = {
     MSP_WP:                 118,
     MSP_BOXIDS:             119,
     MSP_SERVO_CONF:         120,
+    MSP_SONAR:              123,
 
     MSP_SET_RAW_RC:         200,
     MSP_SET_RAW_GPS:        201,
@@ -260,6 +261,9 @@ var MSP = {
             case MSP_codes.MSP_ALTITUDE:
                 SENSOR_DATA.altitude = parseFloat((data.getInt32(0, 1) / 100.0).toFixed(2)); // correct scale factor
                 break;
+            case MSP_codes.MSP_SONAR:
+                SENSOR_DATA.sonar = data.getInt32(0, 1);
+                break;
             case MSP_codes.MSP_ANALOG:
                 ANALOG.voltage = data.getUint8(0) / 10.0;
                 ANALOG.mAhdrawn = data.getUint16(1, 1);
diff --git a/tabs/sensors.html b/tabs/sensors.html
index aef419c4..905a5ca3 100644
--- a/tabs/sensors.html
+++ b/tabs/sensors.html
@@ -10,6 +10,7 @@
             <label><input type="checkbox" name="accel_on" />Accelerometer</label>
             <label><input type="checkbox" name="mag_on" />Magnetometer</label>
             <label><input type="checkbox" name="baro_on" />Barometer</label>
+            <label><input type="checkbox" name="sonar_on" />Sonar</label>
             <label><input type="checkbox" name="debug_on" />Debug</label>
         </div>
     </div>
@@ -163,6 +164,36 @@
         </svg>
         <div class="clear-both"></div>
     </div>
+    <div class="wrapper sonar">
+        <div class="plot_control">
+            <div class="title">Sonar - cm</div>
+            <dl>
+                <dt i18n="sensorsRefresh"></dt>
+                <dd class="rate">
+                    <select name="sonar_refresh_rate">
+                        <option value="10">10 ms</option>
+                        <option value="20">20 ms</option>
+                        <option value="30">30 ms</option>
+                        <option value="40">40 ms</option>
+                        <option value="50">50 ms</option>
+                        <option value="100" selected="selected">100 ms</option>
+                        <option value="250">250 ms</option>
+                        <option value="500">500 ms</option>
+                        <option value="1000">1000 ms</option>
+                    </select>
+                </dd>
+                <dt>X:</dt><dd class="x">0</dd>
+            </dl>
+        </div>
+        <svg id="sonar">
+            <g class="grid x" transform="translate(40, 120)"></g>
+            <g class="grid y" transform="translate(40, 10)"></g>
+            <g class="data" transform="translate(41, 10)"></g>
+            <g class="axis x" transform="translate(40, 120)"></g>
+            <g class="axis y" transform="translate(40, 10)"></g>
+        </svg>
+        <div class="clear-both"></div>
+    </div>
     <div class="wrapper debug">
         <div class="plot_control">
             <div class="title">Debug 1</div>
diff --git a/tabs/sensors.js b/tabs/sensors.js
index 65540894..08423115 100644
--- a/tabs/sensors.js
+++ b/tabs/sensors.js
@@ -14,6 +14,7 @@ TABS.sensors.initialize = function (callback) {
             SENSOR_DATA.accelerometer[i] = 0;
             SENSOR_DATA.gyroscope[i] = 0;
             SENSOR_DATA.magnetometer[i] = 0;
+            SENSOR_DATA.sonar[i] = 0;
             SENSOR_DATA.debug[i] = 0;
         }
     }
@@ -162,6 +163,14 @@ TABS.sensors.initialize = function (callback) {
         }
     }
 
+    function plot_sonar(enable) {
+        if (enable) {
+            $('.wrapper.sonar').show();
+        } else {
+            $('.wrapper.sonar').hide();
+        }
+    }
+
     function plot_debug(enable) {
         if (enable) {
             $('.wrapper.debug').show();
@@ -182,6 +191,9 @@ TABS.sensors.initialize = function (callback) {
         if (!bit_check(CONFIG.activeSensors, 2)) { // mag
             checkboxes.eq(2).prop('disabled', true);
         }
+        if (!bit_check(CONFIG.activeSensors, 4)) { // sonar
+            checkboxes.eq(4).prop('disabled', true);
+        }
 
         $('.tab-sensors .info .checkboxes input').change(function () {
             var enable = $(this).prop('checked');
@@ -201,6 +213,9 @@ TABS.sensors.initialize = function (callback) {
                     plot_baro(enable);
                     break;
                 case 4:
+                    plot_sonar(enable);
+                    break;
+                case 5:
                     plot_debug(enable);
                     break;
             }
@@ -234,11 +249,13 @@ TABS.sensors.initialize = function (callback) {
             samples_accel_i = 0,
             samples_mag_i = 0,
             samples_baro_i = 0,
+            samples_sonar_i = 0,
             samples_debug_i = 0,
             gyro_data = initDataArray(3),
             accel_data = initDataArray(3),
             mag_data = initDataArray(3),
             baro_data = initDataArray(1),
+            sonar_data = initDataArray(1),
             debug_data = [
             initDataArray(1),
             initDataArray(1),
@@ -250,6 +267,7 @@ TABS.sensors.initialize = function (callback) {
         var accelHelpers = initGraphHelpers('#accel', samples_accel_i, [-2, 2]);
         var magHelpers = initGraphHelpers('#mag', samples_mag_i, [-1, 1]);
         var baroHelpers = initGraphHelpers('#baro', samples_baro_i);
+        var sonarHelpers = initGraphHelpers('#sonar', samples_sonar_i);
         var debugHelpers = [
             initGraphHelpers('#debug1', samples_debug_i),
             initGraphHelpers('#debug2', samples_debug_i),
@@ -286,6 +304,8 @@ TABS.sensors.initialize = function (callback) {
                 $('.tab-sensors select[name="mag_scale"]').val(result.sensor_settings.scales.mag);
 
                 $('.tab-sensors select[name="baro_refresh_rate"]').val(result.sensor_settings.rates.baro);
+                $('.tab-sensors select[name="sonar_refresh_rate"]').val(result.sensor_settings.rates.sonar);
+
                 $('.tab-sensors select[name="debug_refresh_rate"]').val(result.sensor_settings.rates.debug);
 
                 // start polling data by triggering refresh rate change event
@@ -304,6 +324,7 @@ TABS.sensors.initialize = function (callback) {
                 'accel':  parseInt($('.tab-sensors select[name="accel_refresh_rate"]').val(), 10),
                 'mag':    parseInt($('.tab-sensors select[name="mag_refresh_rate"]').val(), 10),
                 'baro':   parseInt($('.tab-sensors select[name="baro_refresh_rate"]').val(), 10),
+                'sonar':  parseInt($('.tab-sensors select[name="sonar_refresh_rate"]').val(), 10),
                 'debug':  parseInt($('.tab-sensors select[name="debug_refresh_rate"]').val(), 10)
             };
 
@@ -350,6 +371,12 @@ TABS.sensors.initialize = function (callback) {
             }
 
             if (checkboxes[4]) {
+                GUI.interval_add('sonar_pull', function sonar_data_pull() {
+                    MSP.send_message(MSP_codes.MSP_SONAR, false, false, update_sonar_graphs);
+                }, rates.sonar, true);
+            }
+
+            if (checkboxes[5]) {
                 GUI.interval_add('debug_pull', function debug_data_pull() {
                     MSP.send_message(MSP_codes.MSP_DEBUG, false, false, update_debug_graphs);
                 }, rates.debug, true);
@@ -395,13 +422,21 @@ TABS.sensors.initialize = function (callback) {
                 raw_data_text_ements.x[3].text(SENSOR_DATA.altitude.toFixed(2));
             }
 
+            function update_sonar_graphs() {
+                updateGraphHelperSize(sonarHelpers);
+
+                samples_sonar_i = addSampleToData(sonar_data, samples_sonar_i, [SENSOR_DATA.sonar]);
+                drawGraph(sonarHelpers, sonar_data, samples_sonar_i);
+                raw_data_text_ements.x[4].text(SENSOR_DATA.sonar.toFixed(2));
+            }
+
             function update_debug_graphs() {
                 for (var i = 0; i < 4; i++) {
                     updateGraphHelperSize(debugHelpers[i]);
 
                     addSampleToData(debug_data[i], samples_debug_i, [SENSOR_DATA.debug[i]]);
                     drawGraph(debugHelpers[i], debug_data[i], samples_debug_i);
-                    raw_data_text_ements.x[4 + i].text(SENSOR_DATA.debug[i]);
+                    raw_data_text_ements.x[5 + i].text(SENSOR_DATA.debug[i]);
                 }
                 samples_debug_i++;
             }