diff --git a/flight/libraries/math/mathmisc.h b/flight/libraries/math/mathmisc.h
index ba9062a0d..68026f682 100644
--- a/flight/libraries/math/mathmisc.h
+++ b/flight/libraries/math/mathmisc.h
@@ -119,6 +119,30 @@ static inline float y_on_curve(float x, const pointf points[], int num_points)
     // Find the y value on the selected line.
     return y_on_line(x, &points[end_point - 1], &points[end_point]);
 }
+// Fast inverse square root implementation from "quake3-1.32b/code/game/q_math.c"
+// http://en.wikipedia.org/wiki/Fast_inverse_square_root
+
+static inline float fast_invsqrtf(float number)
+{
+    float x2, y;
+    const float threehalfs = 1.5F;
+
+    union {
+        float    f;
+        uint32_t u;
+    } i;
+
+    x2  = number * 0.5F;
+    y   = number;
+
+    i.f = y; // evil floating point bit level hacking
+    i.u = 0x5f3759df - (i.u >> 1); // what the fxck?
+    y   = i.f;
+    y   = y * (threehalfs - (x2 * y * y));   // 1st iteration
+// y  = y * ( threehalfs - ( x2 * y * y ) );   // 2nd iteration, this can be removed
+
+    return y;
+}
 
 /**
  * Ultrafast pow() aproximation needed for expo
diff --git a/flight/modules/Altitude/revolution/altitude.c b/flight/modules/Altitude/revolution/altitude.c
index 4322c90d3..6cd1e5750 100644
--- a/flight/modules/Altitude/revolution/altitude.c
+++ b/flight/modules/Altitude/revolution/altitude.c
@@ -41,14 +41,23 @@
 #include "altitude.h"
 #include "barosensor.h" // object that will be updated by the module
 #include "revosettings.h"
+#include <mathmisc.h>
 #if defined(PIOS_INCLUDE_HCSR04)
 #include "sonaraltitude.h" // object that will be updated by the module
 #endif
 #include "taskinfo.h"
 
 // Private constants
-#define STACK_SIZE_BYTES 550
-#define TASK_PRIORITY    (tskIDLE_PRIORITY + 1)
+#define STACK_SIZE_BYTES    550
+#define TASK_PRIORITY       (tskIDLE_PRIORITY + 1)
+
+// Interval in number of sample to recalculate temp bias
+#define TEMP_CALIB_INTERVAL 10
+
+// LPF
+#define TEMP_DT             (1.0f / 120.0f)
+#define TEMP_LPF_FC         5.0f
+static const float temp_alpha = TEMP_DT / (TEMP_DT + 1.0f / (2.0f * M_PI_F * TEMP_LPF_FC));
 
 // Private types
 
@@ -57,6 +66,11 @@ static xTaskHandle taskHandle;
 static RevoSettingsBaroTempCorrectionPolynomialData baroCorrection;
 static RevoSettingsBaroTempCorrectionExtentData baroCorrectionExtent;
 static volatile bool tempCorrectionEnabled;
+
+static float baro_temp_bias   = 0;
+static float baro_temperature = NAN;
+static uint8_t temp_calibration_count = 0;
+
 // Private functions
 static void altitudeTask(void *parameters);
 static void SettingsUpdatedCb(UAVObjEvent *ev);
@@ -166,13 +180,22 @@ static void altitudeTask(__attribute__((unused)) void *parameters)
         temp  = PIOS_MS5611_GetTemperature();
         press = PIOS_MS5611_GetPressure();
 
-        if (tempCorrectionEnabled) {
+        if (isnan(baro_temperature)) {
+            baro_temperature = temp;
+        }
+
+        baro_temperature = temp_alpha * (temp - baro_temperature) + baro_temperature;
+
+        if (tempCorrectionEnabled && !temp_calibration_count) {
+            temp_calibration_count = TEMP_CALIB_INTERVAL;
             // pressure bias = A + B*t + C*t^2 + D * t^3
             // in case the temperature is outside of the calibrated range, uses the nearest extremes
-            float ctemp = temp > baroCorrectionExtent.max ? baroCorrectionExtent.max :
-                          (temp < baroCorrectionExtent.min ? baroCorrectionExtent.min : temp);
-            press -= baroCorrection.a + ((baroCorrection.d * ctemp + baroCorrection.c) * ctemp + baroCorrection.b) * ctemp;
+            float ctemp = boundf(baro_temperature, baroCorrectionExtent.max, baroCorrectionExtent.min);
+            baro_temp_bias = baroCorrection.a + ((baroCorrection.d * ctemp + baroCorrection.c) * ctemp + baroCorrection.b) * ctemp;
         }
+
+        press -= baro_temp_bias;
+
         float altitude = 44330.0f * (1.0f - powf((press) / MS5611_P0, (1.0f / 5.255f)));
 
         if (!isnan(altitude)) {
diff --git a/flight/modules/Attitude/attitude.c b/flight/modules/Attitude/attitude.c
index 705c713df..84a09b397 100644
--- a/flight/modules/Attitude/attitude.c
+++ b/flight/modules/Attitude/attitude.c
@@ -64,8 +64,8 @@
 
 #include "CoordinateConversions.h"
 #include <pios_notify.h>
-
-
+#include <mathmisc.h>
+#include <pios_constants.h>
 #include <pios_instrumentation_helper.h>
 
 PERF_DEFINE_COUNTER(counterUpd);
@@ -79,16 +79,25 @@ PERF_DEFINE_COUNTER(counterAtt);
 // - 0xA7710004 number of accel samples read for each loop (cc only).
 
 // Private constants
-#define STACK_SIZE_BYTES 540
-#define TASK_PRIORITY    (tskIDLE_PRIORITY + 3)
+#define STACK_SIZE_BYTES    540
+#define TASK_PRIORITY       (tskIDLE_PRIORITY + 3)
 
-#define SENSOR_PERIOD    4
-#define UPDATE_RATE      25.0f
+// Attitude module loop interval (defined by sensor rate in pios_config.h)
+static const uint32_t sensor_period_ms = ((uint32_t)1000.0f / PIOS_SENSOR_RATE);
 
-#define UPDATE_EXPECTED  (1.0f / 500.0f)
-#define UPDATE_MIN       1.0e-6f
-#define UPDATE_MAX       1.0f
-#define UPDATE_ALPHA     1.0e-2f
+#define UPDATE_RATE         25.0f
+
+// Interval in number of sample to recalculate temp bias
+#define TEMP_CALIB_INTERVAL 30
+// LPF
+#define TEMP_DT             (1.0f / PIOS_SENSOR_RATE)
+#define TEMP_LPF_FC         5.0f
+static const float temp_alpha = TEMP_DT / (TEMP_DT + 1.0f / (2.0f * M_PI_F * TEMP_LPF_FC));
+
+#define UPDATE_EXPECTED     (1.0f / PIOS_SENSOR_RATE)
+#define UPDATE_MIN          1.0e-6f
+#define UPDATE_MAX          1.0f
+#define UPDATE_ALPHA        1.0e-2f
 
 // Private types
 
@@ -130,6 +139,10 @@ static bool apply_accel_temp = false;
 static AccelGyroSettingsgyro_temp_coeffData gyro_temp_coeff;;
 static AccelGyroSettingsaccel_temp_coeffData accel_temp_coeff;
 static AccelGyroSettingstemp_calibrated_extentData temp_calibrated_extent;
+static float temperature = NAN;
+static float accel_temp_bias[3] = { 0 };
+static float gyro_temp_bias[3] = { 0 };
+static uint8_t temp_calibration_count = 0;
 
 // Accel and Gyro scaling (this is the product of sensor scale and adjustement in AccelGyroSettings
 static AccelGyroSettingsgyro_scaleData gyro_scale;
@@ -142,8 +155,7 @@ static volatile int32_t trim_accels[3];
 static volatile int32_t trim_samples;
 int32_t const MAX_TRIM_FLIGHT_SAMPLES = 65535;
 
-#define GRAV                       9.81f
-#define STD_CC_ACCEL_SCALE         (GRAV * 0.004f)
+#define STD_CC_ACCEL_SCALE         (PIOS_CONST_MKS_GRAV_ACCEL_F * 0.004f)
 /* 0.004f is gravity / LSB */
 #define STD_CC_ANALOG_GYRO_NEUTRAL 1665
 #define STD_CC_ANALOG_GYRO_GAIN    0.42f
@@ -222,12 +234,6 @@ static void AttitudeTask(__attribute__((unused)) void *parameters)
 
     AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
 
-    // Set critical error and wait until the accel is producing data
-    while (PIOS_ADXL345_FifoElements() == 0) {
-        AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL);
-        PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE);
-    }
-
     bool cc3d = BOARDISCC3D;
 
     if (cc3d) {
@@ -236,6 +242,11 @@ static void AttitudeTask(__attribute__((unused)) void *parameters)
 #endif
     } else {
 #if defined(PIOS_INCLUDE_ADXL345)
+        // Set critical error and wait until the accel is producing data
+        while (PIOS_ADXL345_FifoElements() == 0) {
+            AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL);
+            PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE);
+        }
         accel_test = PIOS_ADXL345_Test();
 #endif
 
@@ -257,7 +268,7 @@ static void AttitudeTask(__attribute__((unused)) void *parameters)
     settingsUpdatedCb(AttitudeSettingsHandle());
 
     PIOS_DELTATIME_Init(&dtconfig, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA);
-
+    portTickType lastSysTime = xTaskGetTickCount();
     // Main task loop
     while (1) {
         FlightStatusData flightStatus;
@@ -317,6 +328,7 @@ static void AttitudeTask(__attribute__((unused)) void *parameters)
             PERF_MEASURE_PERIOD(counterPeriod);
             AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
         }
+        vTaskDelayUntil(&lastSysTime, sensor_period_ms / portTICK_PERIOD_MS);
     }
 }
 
@@ -454,7 +466,7 @@ static int32_t updateSensorsCC3D(AccelStateData *accelStateData, GyroStateData *
 #if defined(PIOS_INCLUDE_MPU6000)
 
     xQueueHandle queue = PIOS_MPU6000_GetQueue();
-    BaseType_t ret     = xQueueReceive(queue, (void *)&mpu6000_data, SENSOR_PERIOD);
+    BaseType_t ret     = xQueueReceive(queue, (void *)&mpu6000_data, sensor_period_ms);
     while (ret == pdTRUE) {
         gyros[0]  += mpu6000_data.gyro_x;
         gyros[1]  += mpu6000_data.gyro_y;
@@ -470,6 +482,7 @@ static int32_t updateSensorsCC3D(AccelStateData *accelStateData, GyroStateData *
         // check if further samples are already in queue
         ret = xQueueReceive(queue, (void *)&mpu6000_data, 0);
     }
+    PERF_TRACK_VALUE(counterAccelSamples, count);
 
     if (!count) {
         return -1; // Error, no data
@@ -488,21 +501,39 @@ static int32_t updateSensorsCC3D(AccelStateData *accelStateData, GyroStateData *
     accels[1] *= accel_scale.Y * invcount;
     accels[2] *= accel_scale.Z * invcount;
     temp *= invcount;
-    float ctemp = temp > temp_calibrated_extent.max ? temp_calibrated_extent.max :
-                  (temp < temp_calibrated_extent.min ? temp_calibrated_extent.min
-                   : temp);
 
+    if (isnan(temperature)) {
+        temperature = temp;
+    }
+    temperature = temp_alpha * (temp - temperature) + temperature;
+
+    if ((apply_gyro_temp || apply_accel_temp) && !temp_calibration_count) {
+        temp_calibration_count = TEMP_CALIB_INTERVAL;
+        float ctemp = boundf(temperature, temp_calibrated_extent.max, temp_calibrated_extent.min);
+        if (apply_gyro_temp) {
+            gyro_temp_bias[0] = (gyro_temp_coeff.X + gyro_temp_coeff.X2 * ctemp) * ctemp;
+            gyro_temp_bias[1] = (gyro_temp_coeff.Y + gyro_temp_coeff.Y2 * ctemp) * ctemp;
+            gyro_temp_bias[2] = (gyro_temp_coeff.Z + gyro_temp_coeff.Z2 * ctemp) * ctemp;
+        }
+
+        if (apply_accel_temp) {
+            accel_temp_bias[0] = accel_temp_coeff.X * ctemp;
+            accel_temp_bias[1] = accel_temp_coeff.Y * ctemp;
+            accel_temp_bias[2] = accel_temp_coeff.Z * ctemp;
+        }
+    }
+    temp_calibration_count--;
 
     if (apply_gyro_temp) {
-        gyros[0] -= (gyro_temp_coeff.X + gyro_temp_coeff.X2 * ctemp) * ctemp;
-        gyros[1] -= (gyro_temp_coeff.Y + gyro_temp_coeff.Y2 * ctemp) * ctemp;
-        gyros[2] -= (gyro_temp_coeff.Z + gyro_temp_coeff.Z2 * ctemp) * ctemp;
+        gyros[0] -= gyro_temp_bias[0];
+        gyros[1] -= gyro_temp_bias[1];
+        gyros[2] -= gyro_temp_bias[2];
     }
 
     if (apply_accel_temp) {
-        accels[0] -= accel_temp_coeff.X * ctemp;
-        accels[1] -= accel_temp_coeff.Y * ctemp;
-        accels[2] -= accel_temp_coeff.Z * ctemp;
+        accels[0] -= accel_temp_bias[0];
+        accels[1] -= accel_temp_bias[1];
+        accels[2] -= accel_temp_bias[2];
     }
     // gyrosData->temperature  = 35.0f + ((float)mpu6000_data.temperature + 512.0f) / 340.0f;
     // accelsData->temperature = 35.0f + ((float)mpu6000_data.temperature + 512.0f) / 340.0f;
@@ -587,24 +618,24 @@ __attribute__((optimize("O3"))) static void updateAttitude(AccelStateData *accel
     CrossProduct((const float *)accels_filtered, (const float *)grot_filtered, accel_err);
 
     // Account for accel magnitude
-    float accel_mag = sqrtf(accels_filtered[0] * accels_filtered[0] + accels_filtered[1] * accels_filtered[1] + accels_filtered[2] * accels_filtered[2]);
-    if (accel_mag < 1.0e-3f) {
+    float inv_accel_mag = fast_invsqrtf(accels_filtered[0] * accels_filtered[0] + accels_filtered[1] * accels_filtered[1] + accels_filtered[2] * accels_filtered[2]);
+    if (inv_accel_mag > 1e3f) {
         return;
     }
 
     // Account for filtered gravity vector magnitude
-    float grot_mag;
+    float inv_grot_mag;
 
     if (accel_filter_enabled) {
-        grot_mag = sqrtf(grot_filtered[0] * grot_filtered[0] + grot_filtered[1] * grot_filtered[1] + grot_filtered[2] * grot_filtered[2]);
+        inv_grot_mag = fast_invsqrtf(grot_filtered[0] * grot_filtered[0] + grot_filtered[1] * grot_filtered[1] + grot_filtered[2] * grot_filtered[2]);
     } else {
-        grot_mag = 1.0f;
+        inv_grot_mag = 1.0f;
     }
 
-    if (grot_mag < 1.0e-3f) {
+    if (inv_grot_mag > 1e3f) {
         return;
     }
-    const float invMag = 1.0f / (accel_mag * grot_mag);
+    const float invMag = (inv_accel_mag * inv_grot_mag);
     accel_err[0] *= invMag;
     accel_err[1] *= invMag;
     accel_err[2] *= invMag;
@@ -645,21 +676,20 @@ __attribute__((optimize("O3"))) static void updateAttitude(AccelStateData *accel
     }
 
     // Renomalize
-    float qmag = sqrtf(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
+    float inv_qmag = fast_invsqrtf(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
 
     // If quaternion has become inappropriately short or is nan reinit.
     // THIS SHOULD NEVER ACTUALLY HAPPEN
-    if ((fabsf(qmag) < 1e-3f) || isnan(qmag)) {
+    if ((fabsf(inv_qmag) > 1e3f) || isnan(inv_qmag)) {
         q[0] = 1;
         q[1] = 0;
         q[2] = 0;
         q[3] = 0;
     } else {
-        const float invQmag = 1.0f / qmag;
-        q[0] = q[0] * invQmag;
-        q[1] = q[1] * invQmag;
-        q[2] = q[2] * invQmag;
-        q[3] = q[3] * invQmag;
+        q[0] = q[0] * inv_qmag;
+        q[1] = q[1] * inv_qmag;
+        q[2] = q[2] * inv_qmag;
+        q[3] = q[3] * inv_qmag;
     }
 
     AttitudeStateData attitudeState;
@@ -777,7 +807,7 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
         accelGyroSettings.accel_scale.X = trim_accels[0] / trim_samples;
         accelGyroSettings.accel_scale.Y = trim_accels[1] / trim_samples;
         // Z should average -grav
-        accelGyroSettings.accel_scale.Z = trim_accels[2] / trim_samples + GRAV;
+        accelGyroSettings.accel_scale.Z = trim_accels[2] / trim_samples + PIOS_CONST_MKS_GRAV_ACCEL_F;
         attitudeSettings.TrimFlight     = ATTITUDESETTINGS_TRIMFLIGHT_NORMAL;
         AttitudeSettingsSet(&attitudeSettings);
     } else {
diff --git a/flight/modules/Sensors/sensors.c b/flight/modules/Sensors/sensors.c
index fb51af90e..764999928 100644
--- a/flight/modules/Sensors/sensors.c
+++ b/flight/modules/Sensors/sensors.c
@@ -58,19 +58,36 @@
 #include <accelgyrosettings.h>
 #include <flightstatus.h>
 #include <taskinfo.h>
-
+#include <pios_math.h>
 #include <CoordinateConversions.h>
 
 #include <pios_board_info.h>
 
 // Private constants
-#define STACK_SIZE_BYTES 1000
-#define TASK_PRIORITY    (tskIDLE_PRIORITY + 3)
-#define SENSOR_PERIOD    2
+#define STACK_SIZE_BYTES    1000
+#define TASK_PRIORITY       (tskIDLE_PRIORITY + 3)
 
-#define ZERO_ROT_ANGLE   0.00001f
+static const uint32_t sensor_period_ms = ((uint32_t)1000.0f / PIOS_SENSOR_RATE);
+
+// Interval in number of sample to recalculate temp bias
+#define TEMP_CALIB_INTERVAL 30
+
+// LPF
+#define TEMP_DT             (1.0f / PIOS_SENSOR_RATE)
+#define TEMP_LPF_FC         5.0f
+static const float temp_alpha = TEMP_DT / (TEMP_DT + 1.0f / (2.0f * M_PI_F * TEMP_LPF_FC));
+
+#define ZERO_ROT_ANGLE      0.00001f
 // Private types
+#define PIOS_INSTRUMENT_MODULE
+#include <pios_instrumentation_helper.h>
 
+PERF_DEFINE_COUNTER(counterGyroSamples);
+PERF_DEFINE_COUNTER(counterSensorPeriod);
+
+// Counters:
+// - 0x53000001 Sensor fetch rate(period)
+// - 0x53000002 number of gyro samples read for each loop
 
 // Private functions
 static void SensorsTask(void *parameters);
@@ -96,6 +113,13 @@ static float mag_transform[3][3] = {
 static volatile bool gyro_temp_calibrated  = false;
 static volatile bool accel_temp_calibrated = false;
 
+static float accel_temperature  = NAN;
+static float gyro_temperature   = NAN;
+static float accel_temp_bias[3] = { 0 };
+static float gyro_temp_bias[3] = { 0 };
+static uint8_t temp_calibration_count = 0;
+
+
 static float R[3][3] = {
     { 0 }
 };
@@ -219,7 +243,8 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
             vTaskDelay(10);
         }
     }
-
+    PERF_INIT_COUNTER(counterGyroSamples, 0x53000001);
+    PERF_INIT_COUNTER(counterSensorPeriod, 0x53000002);
     // Main task loop
     lastSysTime = xTaskGetTickCount();
     bool error = false;
@@ -234,7 +259,7 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
             PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS);
 #endif
             lastSysTime = xTaskGetTickCount();
-            vTaskDelayUntil(&lastSysTime, SENSOR_PERIOD / portTICK_RATE_MS);
+            vTaskDelayUntil(&lastSysTime, sensor_period_ms / portTICK_RATE_MS);
             AlarmsSet(SYSTEMALARMS_ALARM_SENSORS, SYSTEMALARMS_ALARM_CRITICAL);
             error = false;
         } else {
@@ -263,7 +288,7 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
 
                 count = 0;
                 while ((read_good = PIOS_BMA180_ReadFifo(&accel)) != 0 && !error) {
-                    error = ((xTaskGetTickCount() - lastSysTime) > SENSOR_PERIOD) ? true : error;
+                    error = ((xTaskGetTickCount() - lastSysTime) > sensor_period_ms) ? true : error;
                 }
                 if (error) {
                     // Unfortunately if the BMA180 ever misses getting read, then it will not
@@ -332,6 +357,9 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
                     accel_samples++;
                 }
 
+                PERF_MEASURE_PERIOD(counterSensorPeriod);
+                PERF_TRACK_VALUE(counterGyroSamples, gyro_samples);
+
                 if (gyro_samples == 0) {
                     PIOS_MPU6000_ReadGyros(&mpu6000_data);
                     error = true;
@@ -350,24 +378,43 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
             PIOS_DEBUG_Assert(0);
         }
 
+        if (isnan(accel_temperature)) {
+            accel_temperature = accelSensorData.temperature;
+            gyro_temperature  = gyroSensorData.temperature;
+        }
+
+        accel_temperature = temp_alpha * (accelSensorData.temperature - accel_temperature) + accel_temperature;
+        gyro_temperature  = temp_alpha * (gyroSensorData.temperature - gyro_temperature) + gyro_temperature;
+
+        if ((accel_temp_calibrated || gyro_temp_calibrated) && !temp_calibration_count) {
+            temp_calibration_count = TEMP_CALIB_INTERVAL;
+            if (accel_temp_calibrated) {
+                float ctemp = boundf(accel_temperature, agcal.temp_calibrated_extent.max, agcal.temp_calibrated_extent.min);
+                accel_temp_bias[0] = agcal.accel_temp_coeff.X * ctemp;
+                accel_temp_bias[1] = agcal.accel_temp_coeff.Y * ctemp;
+                accel_temp_bias[2] = agcal.accel_temp_coeff.Z * ctemp;
+            }
+
+            if (gyro_temp_calibrated) {
+                float ctemp = boundf(gyro_temperature, agcal.temp_calibrated_extent.max, agcal.temp_calibrated_extent.min);
+
+                gyro_temp_bias[0] = (agcal.gyro_temp_coeff.X + agcal.gyro_temp_coeff.X2 * ctemp) * ctemp;
+                gyro_temp_bias[1] = (agcal.gyro_temp_coeff.Y + agcal.gyro_temp_coeff.Y2 * ctemp) * ctemp;
+                gyro_temp_bias[2] = (agcal.gyro_temp_coeff.Z + agcal.gyro_temp_coeff.Z2 * ctemp) * ctemp;
+            }
+        }
+        temp_calibration_count--;
         // Scale the accels
         float accels[3] = { (float)accel_accum[0] / accel_samples,
                             (float)accel_accum[1] / accel_samples,
                             (float)accel_accum[2] / accel_samples };
 
 
-        float accels_out[3] = { accels[0] * accel_scaling * agcal.accel_scale.X - agcal.accel_bias.X,
-                                accels[1] * accel_scaling * agcal.accel_scale.Y - agcal.accel_bias.Y,
-                                accels[2] * accel_scaling * agcal.accel_scale.Z - agcal.accel_bias.Z };
+        float accels_out[3] = { accels[0] * accel_scaling * agcal.accel_scale.X - agcal.accel_bias.X - accel_temp_bias[0],
+                                accels[1] * accel_scaling * agcal.accel_scale.Y - agcal.accel_bias.Y - accel_temp_bias[1],
+                                accels[2] * accel_scaling * agcal.accel_scale.Z - agcal.accel_bias.Z - accel_temp_bias[2] };
+
 
-        if (accel_temp_calibrated) {
-            float ctemp = accelSensorData.temperature > agcal.temp_calibrated_extent.max ? agcal.temp_calibrated_extent.max :
-                          (accelSensorData.temperature < agcal.temp_calibrated_extent.min ? agcal.temp_calibrated_extent.min
-                           : accelSensorData.temperature);
-            accels_out[0] -= agcal.accel_temp_coeff.X * ctemp;
-            accels_out[1] -= agcal.accel_temp_coeff.Y * ctemp;
-            accels_out[2] -= agcal.accel_temp_coeff.Z * ctemp;
-        }
         if (rotate) {
             rot_mult(R, accels_out, accels);
             accelSensorData.x = accels[0];
@@ -385,18 +432,10 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
                            (float)gyro_accum[1] / gyro_samples,
                            (float)gyro_accum[2] / gyro_samples };
 
-        float gyros_out[3] = { gyros[0] * gyro_scaling * agcal.gyro_scale.X - agcal.gyro_bias.X,
-                               gyros[1] * gyro_scaling * agcal.gyro_scale.Y - agcal.gyro_bias.Y,
-                               gyros[2] * gyro_scaling * agcal.gyro_scale.Z - agcal.gyro_bias.Z };
+        float gyros_out[3] = { gyros[0] * gyro_scaling * agcal.gyro_scale.X - agcal.gyro_bias.X - gyro_temp_bias[0],
+                               gyros[1] * gyro_scaling * agcal.gyro_scale.Y - agcal.gyro_bias.Y - gyro_temp_bias[1],
+                               gyros[2] * gyro_scaling * agcal.gyro_scale.Z - agcal.gyro_bias.Z - gyro_temp_bias[2] };
 
-        if (gyro_temp_calibrated) {
-            float ctemp = gyroSensorData.temperature > agcal.temp_calibrated_extent.max ? agcal.temp_calibrated_extent.max :
-                          (gyroSensorData.temperature < agcal.temp_calibrated_extent.min ? agcal.temp_calibrated_extent.min
-                           : gyroSensorData.temperature);
-            gyros_out[0] -= (agcal.gyro_temp_coeff.X + agcal.gyro_temp_coeff.X2 * ctemp) * ctemp;
-            gyros_out[1] -= (agcal.gyro_temp_coeff.Y + agcal.gyro_temp_coeff.Y2 * ctemp) * ctemp;
-            gyros_out[2] -= (agcal.gyro_temp_coeff.Z + agcal.gyro_temp_coeff.Z2 * ctemp) * ctemp;
-        }
         if (rotate) {
             rot_mult(R, gyros_out, gyros);
             gyroSensorData.x = gyros[0];
@@ -437,8 +476,7 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
 #ifdef PIOS_INCLUDE_WDG
         PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS);
 #endif
-
-        lastSysTime = xTaskGetTickCount();
+        vTaskDelayUntil(&lastSysTime, sensor_period_ms / portTICK_RATE_MS);
     }
 }
 
diff --git a/flight/modules/Stabilization/altitudeloop.c b/flight/modules/Stabilization/altitudeloop.c
index f54ed5740..489e6e0d1 100644
--- a/flight/modules/Stabilization/altitudeloop.c
+++ b/flight/modules/Stabilization/altitudeloop.c
@@ -42,7 +42,7 @@
 
 #ifdef REVOLUTION
 
-#define UPDATE_EXPECTED   (1.0f / 666.0f)
+#define UPDATE_EXPECTED   (1.0f / PIOS_SENSOR_RATE)
 #define UPDATE_MIN        1.0e-6f
 #define UPDATE_MAX        1.0f
 #define UPDATE_ALPHA      1.0e-2f
diff --git a/flight/modules/Stabilization/innerloop.c b/flight/modules/Stabilization/innerloop.c
index 0ce96edc8..ca83bf247 100644
--- a/flight/modules/Stabilization/innerloop.c
+++ b/flight/modules/Stabilization/innerloop.c
@@ -55,7 +55,7 @@
 
 #define CALLBACK_PRIORITY CALLBACK_PRIORITY_CRITICAL
 
-#define UPDATE_EXPECTED   (1.0f / 666.0f)
+#define UPDATE_EXPECTED   (1.0f / PIOS_SENSOR_RATE)
 #define UPDATE_MIN        1.0e-6f
 #define UPDATE_MAX        1.0f
 #define UPDATE_ALPHA      1.0e-2f
diff --git a/flight/modules/Stabilization/outerloop.c b/flight/modules/Stabilization/outerloop.c
index a1f8d37c1..995003208 100644
--- a/flight/modules/Stabilization/outerloop.c
+++ b/flight/modules/Stabilization/outerloop.c
@@ -52,7 +52,7 @@
 
 #define CALLBACK_PRIORITY CALLBACK_PRIORITY_REGULAR
 
-#define UPDATE_EXPECTED   (1.0f / 666.0f)
+#define UPDATE_EXPECTED   (1.0f / PIOS_SENSOR_RATE)
 #define UPDATE_MIN        1.0e-6f
 #define UPDATE_MAX        1.0f
 #define UPDATE_ALPHA      1.0e-2f
diff --git a/flight/modules/StateEstimation/filterekf.c b/flight/modules/StateEstimation/filterekf.c
index 3931226e4..1fd036afd 100644
--- a/flight/modules/StateEstimation/filterekf.c
+++ b/flight/modules/StateEstimation/filterekf.c
@@ -47,7 +47,7 @@
 #define DT_ALPHA       1e-3f
 #define DT_MIN         1e-6f
 #define DT_MAX         1.0f
-#define DT_INIT        (1.0f / 666.0f) // initialize with 666 Hz (default sensor update rate on revo)
+#define DT_INIT        (1.0f / PIOS_SENSOR_RATE) // initialize with board sensor rate
 
 #define IMPORT_SENSOR_IF_UPDATED(shortname, num) \
     if (IS_SET(state->updated, SENSORUPDATES_##shortname)) { \
diff --git a/flight/modules/StateEstimation/filtervelocity.c b/flight/modules/StateEstimation/filtervelocity.c
index 493f0f3d7..ec6e29867 100644
--- a/flight/modules/StateEstimation/filtervelocity.c
+++ b/flight/modules/StateEstimation/filtervelocity.c
@@ -40,7 +40,7 @@
 #define DT_ALPHA       1e-3f
 #define DT_MIN         1e-6f
 #define DT_MAX         1.0f
-#define DT_INIT        (1.0f / 666.0f) // initialize with 666 Hz (default sensor update rate on revo)
+#define DT_INIT        (1.0f / PIOS_SENSOR_RATE) // initialize with board sensor rate
 
 // Private types
 struct data {
diff --git a/flight/modules/gpsp/gpsdsysmod.c b/flight/modules/gpsp/gpsdsysmod.c
index 41987db43..92cf4be50 100644
--- a/flight/modules/gpsp/gpsdsysmod.c
+++ b/flight/modules/gpsp/gpsdsysmod.c
@@ -216,7 +216,7 @@ static void readFirmwareInfo()
     struct fw_version_info *fwinfo = (struct fw_version_info *)(bdinfo->fw_base + bdinfo->fw_size);
 
     memcpy(&sysPkt.fragments.data.commit_tag_name, &fwinfo->commit_tag_name, sizeof(sysPkt.fragments.data.commit_tag_name));
-    memcpy(&sysPkt.fragments.data.sha1sum, &fwinfo->commit_tag_name, sizeof(sysPkt.fragments.data.sha1sum));
+    memcpy(&sysPkt.fragments.data.sha1sum, &fwinfo->sha1sum, sizeof(sysPkt.fragments.data.sha1sum));
 }
 
 /**
diff --git a/flight/pios/common/pios_mpu6000.c b/flight/pios/common/pios_mpu6000.c
index 2585c346c..bf00cfa18 100644
--- a/flight/pios/common/pios_mpu6000.c
+++ b/flight/pios/common/pios_mpu6000.c
@@ -33,15 +33,13 @@
 
 #ifdef PIOS_INCLUDE_MPU6000
 
-#include "fifo_buffer.h"
-
+#include <pios_constants.h>
 /* Global Variables */
 
 enum pios_mpu6000_dev_magic {
     PIOS_MPU6000_DEV_MAGIC = 0x9da9b3ed,
 };
 
-#define PIOS_MPU6000_MAX_DOWNSAMPLE 2
 struct mpu6000_dev {
     uint32_t     spi_id;
     uint32_t     slave_num;
@@ -53,29 +51,58 @@ struct mpu6000_dev {
     enum pios_mpu6000_dev_magic   magic;
 };
 
+#ifdef PIOS_MPU6000_ACCEL
+#define PIOS_MPU6000_SAMPLES_BYTES    14
+#define PIOS_MPU6000_SENSOR_FIRST_REG PIOS_MPU6000_ACCEL_X_OUT_MSB
+#else
+#define PIOS_MPU6000_SENSOR_FIRST_REG PIOS_MPU6000_TEMP_OUT_MSB
+#define PIOS_MPU6000_SAMPLES_BYTES    8
+#endif
+
+typedef union {
+    uint8_t buffer[1 + PIOS_MPU6000_SAMPLES_BYTES];
+    struct {
+        uint8_t dummy;
+#ifdef PIOS_MPU6000_ACCEL
+        uint8_t Accel_X_h;
+        uint8_t Accel_X_l;
+        uint8_t Accel_Y_h;
+        uint8_t Accel_Y_l;
+        uint8_t Accel_Z_h;
+        uint8_t Accel_Z_l;
+#endif
+        uint8_t Temperature_h;
+        uint8_t Temperature_l;
+        uint8_t Gyro_X_h;
+        uint8_t Gyro_X_l;
+        uint8_t Gyro_Y_h;
+        uint8_t Gyro_Y_l;
+        uint8_t Gyro_Z_h;
+        uint8_t Gyro_Z_l;
+    } data;
+} mpu6000_data_t;
+
+#define GET_SENSOR_DATA(mpudataptr, sensor) (mpudataptr.data.sensor##_h << 8 | mpudataptr.data.sensor##_l)
+
 // ! Global structure for this device device
 static struct mpu6000_dev *dev;
 volatile bool mpu6000_configured = false;
+static mpu6000_data_t mpu6000_data;
 
 // ! Private functions
-static struct mpu6000_dev *PIOS_MPU6000_alloc(void);
+static struct mpu6000_dev *PIOS_MPU6000_alloc(const struct pios_mpu6000_cfg *cfg);
 static int32_t PIOS_MPU6000_Validate(struct mpu6000_dev *dev);
 static void PIOS_MPU6000_Config(struct pios_mpu6000_cfg const *cfg);
 static int32_t PIOS_MPU6000_SetReg(uint8_t address, uint8_t buffer);
 static int32_t PIOS_MPU6000_GetReg(uint8_t address);
-
-#define GRAV                       9.81f
-
-#ifdef PIOS_MPU6000_ACCEL
-#define PIOS_MPU6000_SAMPLES_BYTES 14
-#else
-#define PIOS_MPU6000_SAMPLES_BYTES 8
-#endif
-
+static void PIOS_MPU6000_SetSpeed(const bool fast);
+static bool PIOS_MPU6000_HandleData();
+static bool PIOS_MPU6000_ReadFifo(bool *woken);
+static bool PIOS_MPU6000_ReadSensor(bool *woken);
 /**
  * @brief Allocate a new device
  */
-static struct mpu6000_dev *PIOS_MPU6000_alloc(void)
+static struct mpu6000_dev *PIOS_MPU6000_alloc(const struct pios_mpu6000_cfg *cfg)
 {
     struct mpu6000_dev *mpu6000_dev;
 
@@ -86,7 +113,7 @@ static struct mpu6000_dev *PIOS_MPU6000_alloc(void)
 
     mpu6000_dev->magic = PIOS_MPU6000_DEV_MAGIC;
 
-    mpu6000_dev->queue = xQueueCreate(PIOS_MPU6000_MAX_DOWNSAMPLE, sizeof(struct pios_mpu6000_data));
+    mpu6000_dev->queue = xQueueCreate(cfg->max_downsample + 1, sizeof(struct pios_mpu6000_data));
     if (mpu6000_dev->queue == NULL) {
         vPortFree(mpu6000_dev);
         return NULL;
@@ -119,7 +146,7 @@ static int32_t PIOS_MPU6000_Validate(struct mpu6000_dev *vdev)
  */
 int32_t PIOS_MPU6000_Init(uint32_t spi_id, uint32_t slave_num, const struct pios_mpu6000_cfg *cfg)
 {
-    dev = PIOS_MPU6000_alloc();
+    dev = PIOS_MPU6000_alloc(cfg);
     if (dev == NULL) {
         return -1;
     }
@@ -129,9 +156,7 @@ int32_t PIOS_MPU6000_Init(uint32_t spi_id, uint32_t slave_num, const struct pios
     dev->cfg = cfg;
 
     /* Configure the MPU6000 Sensor */
-    PIOS_SPI_SetClockSpeed(dev->spi_id, PIOS_SPI_PRESCALER_256);
     PIOS_MPU6000_Config(cfg);
-    PIOS_SPI_SetClockSpeed(dev->spi_id, PIOS_SPI_PRESCALER_16);
 
     /* Set up EXTI line */
     PIOS_EXTI_Init(cfg->exti_cfg);
@@ -234,11 +259,6 @@ int32_t PIOS_MPU6000_ConfigureRanges(
         return -1;
     }
 
-    // TODO: check that changing the SPI clock speed is safe
-    // to do here given that interrupts are enabled and the bus has
-    // not been claimed/is not claimed during this call
-    PIOS_SPI_SetClockSpeed(dev->spi_id, PIOS_SPI_PRESCALER_256);
-
     // update filter settings
     while (PIOS_MPU6000_SetReg(PIOS_MPU6000_DLPF_CFG_REG, filterSetting) != 0) {
         ;
@@ -267,7 +287,6 @@ int32_t PIOS_MPU6000_ConfigureRanges(
 
     dev->accel_range = accelRange;
 #endif
-    PIOS_SPI_SetClockSpeed(dev->spi_id, PIOS_SPI_PRESCALER_16);
     return 0;
 }
 
@@ -275,7 +294,7 @@ int32_t PIOS_MPU6000_ConfigureRanges(
  * @brief Claim the SPI bus for the accel communications and select this chip
  * @return 0 if successful, -1 for invalid device, -2 if unable to claim bus
  */
-static int32_t PIOS_MPU6000_ClaimBus()
+static int32_t PIOS_MPU6000_ClaimBus(bool fast_spi)
 {
     if (PIOS_MPU6000_Validate(dev) != 0) {
         return -1;
@@ -283,17 +302,28 @@ static int32_t PIOS_MPU6000_ClaimBus()
     if (PIOS_SPI_ClaimBus(dev->spi_id) != 0) {
         return -2;
     }
+    PIOS_MPU6000_SetSpeed(fast_spi);
     PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 0);
     return 0;
 }
 
+
+static void PIOS_MPU6000_SetSpeed(const bool fast)
+{
+    if (fast) {
+        PIOS_SPI_SetClockSpeed(dev->spi_id, dev->cfg->fast_prescaler);
+    } else {
+        PIOS_SPI_SetClockSpeed(dev->spi_id, dev->cfg->std_prescaler);
+    }
+}
+
 /**
  * @brief Claim the SPI bus for the accel communications and select this chip
  * @return 0 if successful, -1 for invalid device, -2 if unable to claim bus
  * @param woken[in,out] If non-NULL, will be set to true if woken was false and a higher priority
  *                      task has is now eligible to run, else unchanged
  */
-static int32_t PIOS_MPU6000_ClaimBusISR(bool *woken)
+static int32_t PIOS_MPU6000_ClaimBusISR(bool *woken, bool fast_spi)
 {
     if (PIOS_MPU6000_Validate(dev) != 0) {
         return -1;
@@ -301,6 +331,7 @@ static int32_t PIOS_MPU6000_ClaimBusISR(bool *woken)
     if (PIOS_SPI_ClaimBusISR(dev->spi_id, woken) != 0) {
         return -2;
     }
+    PIOS_MPU6000_SetSpeed(fast_spi);
     PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 0);
     return 0;
 }
@@ -342,7 +373,7 @@ static int32_t PIOS_MPU6000_GetReg(uint8_t reg)
 {
     uint8_t data;
 
-    if (PIOS_MPU6000_ClaimBus() != 0) {
+    if (PIOS_MPU6000_ClaimBus(false) != 0) {
         return -1;
     }
 
@@ -363,7 +394,7 @@ static int32_t PIOS_MPU6000_GetReg(uint8_t reg)
  */
 static int32_t PIOS_MPU6000_SetReg(uint8_t reg, uint8_t data)
 {
-    if (PIOS_MPU6000_ClaimBus() != 0) {
+    if (PIOS_MPU6000_ClaimBus(false) != 0) {
         return -1;
     }
 
@@ -393,7 +424,7 @@ int32_t PIOS_MPU6000_ReadGyros(struct pios_mpu6000_data *data)
     uint8_t buf[7] = { PIOS_MPU6000_GYRO_X_OUT_MSB | 0x80, 0, 0, 0, 0, 0, 0 };
     uint8_t rec[7];
 
-    if (PIOS_MPU6000_ClaimBus() != 0) {
+    if (PIOS_MPU6000_ClaimBus(true) != 0) {
         return -1;
     }
 
@@ -460,16 +491,16 @@ float PIOS_MPU6000_GetAccelScale()
 {
     switch (dev->accel_range) {
     case PIOS_MPU6000_ACCEL_2G:
-        return GRAV / 16384.0f;
+        return PIOS_CONST_MKS_GRAV_ACCEL_F / 16384.0f;
 
     case PIOS_MPU6000_ACCEL_4G:
-        return GRAV / 8192.0f;
+        return PIOS_CONST_MKS_GRAV_ACCEL_F / 8192.0f;
 
     case PIOS_MPU6000_ACCEL_8G:
-        return GRAV / 4096.0f;
+        return PIOS_CONST_MKS_GRAV_ACCEL_F / 4096.0f;
 
     case PIOS_MPU6000_ACCEL_16G:
-        return GRAV / 2048.0f;
+        return PIOS_CONST_MKS_GRAV_ACCEL_F / 2048.0f;
     }
     return 0;
 }
@@ -504,7 +535,7 @@ static int32_t PIOS_MPU6000_GetInterruptStatusRegISR(bool *woken)
     /* Interrupt Status register can be read at high SPI clock speed */
     uint8_t data;
 
-    if (PIOS_MPU6000_ClaimBusISR(woken) != 0) {
+    if (PIOS_MPU6000_ClaimBusISR(woken, false) != 0) {
         return -1;
     }
     PIOS_SPI_TransferByte(dev->spi_id, (0x80 | PIOS_MPU6000_INT_STATUS_REG));
@@ -525,29 +556,16 @@ static int32_t PIOS_MPU6000_ResetFifoISR(bool *woken)
 {
     int32_t result = 0;
 
-    /* Register writes must be at < 1MHz SPI clock.
-     * Speed can only be changed when SPI bus semaphore is held, but
-     * device chip select must not be enabled, so we use the direct
-     * SPI bus claim call here */
-    if (PIOS_SPI_ClaimBusISR(dev->spi_id, woken) != 0) {
+    if (PIOS_MPU6000_ClaimBusISR(woken, false) != 0) {
         return -1;
     }
-    /* Reduce SPI clock speed. */
-    PIOS_SPI_SetClockSpeed(dev->spi_id, PIOS_SPI_PRESCALER_256);
-    /* Enable chip select */
-    PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 0);
     /* Reset FIFO. */
     if (PIOS_SPI_TransferByte(dev->spi_id, 0x7f & PIOS_MPU6000_USER_CTRL_REG) != 0) {
         result = -2;
     } else if (PIOS_SPI_TransferByte(dev->spi_id, (dev->cfg->User_ctl | PIOS_MPU6000_USERCTL_FIFO_RST)) != 0) {
         result = -2;
     }
-    /* Disable chip select. */
-    PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 1);
-    /* Increase SPI clock speed. */
-    PIOS_SPI_SetClockSpeed(dev->spi_id, PIOS_SPI_PRESCALER_16);
-    /* Release the SPI bus semaphore. */
-    PIOS_SPI_ReleaseBusISR(dev->spi_id, woken);
+    PIOS_MPU6000_ReleaseBusISR(woken);
     return result;
 }
 
@@ -562,7 +580,7 @@ static int32_t PIOS_MPU6000_FifoDepthISR(bool *woken)
     uint8_t mpu6000_send_buf[3] = { PIOS_MPU6000_FIFO_CNT_MSB | 0x80, 0, 0 };
     uint8_t mpu6000_rec_buf[3];
 
-    if (PIOS_MPU6000_ClaimBusISR(woken) != 0) {
+    if (PIOS_MPU6000_ClaimBusISR(woken, false) != 0) {
         return -1;
     }
 
@@ -604,6 +622,97 @@ bool PIOS_MPU6000_IRQHandler(void)
         return false;
     }
 
+    bool read_ok = false;
+    if (dev->cfg->User_ctl & PIOS_MPU6000_USERCTL_FIFO_EN) {
+        read_ok = PIOS_MPU6000_ReadFifo(&woken);
+    } else {
+        read_ok = PIOS_MPU6000_ReadSensor(&woken);
+    }
+    if (read_ok) {
+        bool woken2 = PIOS_MPU6000_HandleData();
+        woken |= woken2;
+    }
+
+    mpu6000_irq++;
+
+    mpu6000_time_us = PIOS_DELAY_DiffuS(timeval);
+
+    return woken;
+}
+
+static bool PIOS_MPU6000_HandleData()
+{
+    // Rotate the sensor to OP convention.  The datasheet defines X as towards the right
+    // and Y as forward.  OP convention transposes this.  Also the Z is defined negatively
+    // to our convention
+
+    static struct pios_mpu6000_data data;
+
+    // Currently we only support rotations on top so switch X/Y accordingly
+    switch (dev->cfg->orientation) {
+    case PIOS_MPU6000_TOP_0DEG:
+#ifdef PIOS_MPU6000_ACCEL
+        data.accel_y = GET_SENSOR_DATA(mpu6000_data, Accel_X); // chip X
+        data.accel_x = GET_SENSOR_DATA(mpu6000_data, Accel_Y); // chip Y
+#endif
+        data.gyro_y  = GET_SENSOR_DATA(mpu6000_data, Gyro_X); // chip X
+        data.gyro_x  = GET_SENSOR_DATA(mpu6000_data, Gyro_Y); // chip Y
+        break;
+    case PIOS_MPU6000_TOP_90DEG:
+        // -1 to bring it back to -32768 +32767 range
+#ifdef PIOS_MPU6000_ACCEL
+        data.accel_y = -1 - (GET_SENSOR_DATA(mpu6000_data, Accel_Y)); // chip Y
+        data.accel_x = GET_SENSOR_DATA(mpu6000_data, Accel_X); // chip X
+#endif
+        data.gyro_y  = -1 - (GET_SENSOR_DATA(mpu6000_data, Gyro_Y)); // chip Y
+        data.gyro_x  = GET_SENSOR_DATA(mpu6000_data, Gyro_X); // chip X
+        break;
+    case PIOS_MPU6000_TOP_180DEG:
+#ifdef PIOS_MPU6000_ACCEL
+        data.accel_y = -1 - (GET_SENSOR_DATA(mpu6000_data, Accel_X)); // chip X
+        data.accel_x = -1 - (GET_SENSOR_DATA(mpu6000_data, Accel_Y)); // chip Y
+#endif
+        data.gyro_y  = -1 - (GET_SENSOR_DATA(mpu6000_data, Gyro_X)); // chip X
+        data.gyro_x  = -1 - (GET_SENSOR_DATA(mpu6000_data, Gyro_Y)); // chip Y
+        break;
+    case PIOS_MPU6000_TOP_270DEG:
+#ifdef PIOS_MPU6000_ACCEL
+        data.accel_y = GET_SENSOR_DATA(mpu6000_data, Accel_Y); // chip Y
+        data.accel_x = -1 - (GET_SENSOR_DATA(mpu6000_data, Accel_X)); // chip X
+#endif
+        data.gyro_y  = GET_SENSOR_DATA(mpu6000_data, Gyro_Y); // chip Y
+        data.gyro_x  = -1 - (GET_SENSOR_DATA(mpu6000_data, Gyro_X)); // chip X
+        break;
+    }
+#ifdef PIOS_MPU6000_ACCEL
+    data.accel_z     = -1 - (GET_SENSOR_DATA(mpu6000_data, Accel_Z));
+#endif
+    data.gyro_z      = -1 - (GET_SENSOR_DATA(mpu6000_data, Gyro_Z));
+    data.temperature = GET_SENSOR_DATA(mpu6000_data, Temperature);
+
+    BaseType_t higherPriorityTaskWoken;
+    xQueueSendToBackFromISR(dev->queue, (void *)&data, &higherPriorityTaskWoken);
+    return higherPriorityTaskWoken == pdTRUE;
+}
+
+static bool PIOS_MPU6000_ReadSensor(bool *woken)
+{
+    const uint8_t mpu6000_send_buf[1 + PIOS_MPU6000_SAMPLES_BYTES] = { PIOS_MPU6000_SENSOR_FIRST_REG | 0x80 };
+
+    if (PIOS_MPU6000_ClaimBusISR(woken, true) != 0) {
+        return false;
+    }
+    if (PIOS_SPI_TransferBlock(dev->spi_id, &mpu6000_send_buf[0], &mpu6000_data.buffer[0], sizeof(mpu6000_data_t), NULL) < 0) {
+        PIOS_MPU6000_ReleaseBusISR(woken);
+        mpu6000_fails++;
+        return false;
+    }
+    PIOS_MPU6000_ReleaseBusISR(woken);
+    return true;
+}
+
+static bool PIOS_MPU6000_ReadFifo(bool *woken)
+{
     /* Temporary fix for OP-1049. Expected to be superceded for next major release
      * by code changes for OP-1039.
      * Read interrupt status register to check for FIFO overflow. Must be the
@@ -611,128 +720,57 @@ bool PIOS_MPU6000_IRQHandler(void)
      * any read clears in the status register (PIOS_MPU6000_INT_CLR_ANYRD set in
      * interrupt config register) */
     int32_t result;
-    if ((result = PIOS_MPU6000_GetInterruptStatusRegISR(&woken)) < 0) {
-        return woken;
+
+    if ((result = PIOS_MPU6000_GetInterruptStatusRegISR(woken)) < 0) {
+        return false;
     }
     if (result & PIOS_MPU6000_INT_STATUS_FIFO_OVERFLOW) {
         /* The FIFO has overflowed, so reset it,
          * to enable sample sync to be recovered.
          * If the reset fails, we are in trouble, but
          * we keep trying on subsequent interrupts. */
-        PIOS_MPU6000_ResetFifoISR(&woken);
+        PIOS_MPU6000_ResetFifoISR(woken);
         /* Return and wait for the next new sample. */
-        return woken;
+        return false;
     }
 
     /* Usual case - FIFO has not overflowed. */
-    mpu6000_count = PIOS_MPU6000_FifoDepthISR(&woken);
+    mpu6000_count = PIOS_MPU6000_FifoDepthISR(woken);
     if (mpu6000_count < PIOS_MPU6000_SAMPLES_BYTES) {
-        return woken;
+        return false;
     }
 
-    if (PIOS_MPU6000_ClaimBusISR(&woken) != 0) {
-        return woken;
+    if (PIOS_MPU6000_ClaimBusISR(woken, true) != 0) {
+        return false;
     }
 
-    static uint8_t mpu6000_send_buf[1 + PIOS_MPU6000_SAMPLES_BYTES] = { PIOS_MPU6000_FIFO_REG | 0x80 };
-    static uint8_t mpu6000_rec_buf[1 + PIOS_MPU6000_SAMPLES_BYTES];
+    const uint8_t mpu6000_send_buf[1 + PIOS_MPU6000_SAMPLES_BYTES] = { PIOS_MPU6000_FIFO_REG | 0x80 };
 
-    if (PIOS_SPI_TransferBlock(dev->spi_id, &mpu6000_send_buf[0], &mpu6000_rec_buf[0], sizeof(mpu6000_send_buf), NULL) < 0) {
-        PIOS_MPU6000_ReleaseBusISR(&woken);
+    if (PIOS_SPI_TransferBlock(dev->spi_id, &mpu6000_send_buf[0], &mpu6000_data.buffer[0], sizeof(mpu6000_data_t), NULL) < 0) {
+        PIOS_MPU6000_ReleaseBusISR(woken);
         mpu6000_fails++;
-        return woken;
+        return false;
     }
 
-    PIOS_MPU6000_ReleaseBusISR(&woken);
-
-    static struct pios_mpu6000_data data;
+    PIOS_MPU6000_ReleaseBusISR(woken);
 
     // In the case where extras samples backed up grabbed an extra
     if (mpu6000_count >= PIOS_MPU6000_SAMPLES_BYTES * 2) {
         mpu6000_fifo_backup++;
-        if (PIOS_MPU6000_ClaimBusISR(&woken) != 0) {
-            return woken;
+        if (PIOS_MPU6000_ClaimBusISR(woken, true) != 0) {
+            return false;
         }
 
-        if (PIOS_SPI_TransferBlock(dev->spi_id, &mpu6000_send_buf[0], &mpu6000_rec_buf[0], sizeof(mpu6000_send_buf), NULL) < 0) {
-            PIOS_MPU6000_ReleaseBusISR(&woken);
+        if (PIOS_SPI_TransferBlock(dev->spi_id, &mpu6000_send_buf[0], &mpu6000_data.buffer[0], sizeof(mpu6000_data_t), NULL) < 0) {
+            PIOS_MPU6000_ReleaseBusISR(woken);
             mpu6000_fails++;
-            return woken;
+            return false;
         }
 
-        PIOS_MPU6000_ReleaseBusISR(&woken);
+        PIOS_MPU6000_ReleaseBusISR(woken);
     }
-
-    // Rotate the sensor to OP convention.  The datasheet defines X as towards the right
-    // and Y as forward.  OP convention transposes this.  Also the Z is defined negatively
-    // to our convention
-#if defined(PIOS_MPU6000_ACCEL)
-    // Currently we only support rotations on top so switch X/Y accordingly
-    switch (dev->cfg->orientation) {
-    case PIOS_MPU6000_TOP_0DEG:
-        data.accel_y = mpu6000_rec_buf[1] << 8 | mpu6000_rec_buf[2]; // chip X
-        data.accel_x = mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]; // chip Y
-        data.gyro_y  = mpu6000_rec_buf[9] << 8 | mpu6000_rec_buf[10]; // chip X
-        data.gyro_x  = mpu6000_rec_buf[11] << 8 | mpu6000_rec_buf[12]; // chip Y
-        break;
-    case PIOS_MPU6000_TOP_90DEG:
-        // -1 to bring it back to -32768 +32767 range
-        data.accel_y = -1 - (mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]); // chip Y
-        data.accel_x = mpu6000_rec_buf[1] << 8 | mpu6000_rec_buf[2]; // chip X
-        data.gyro_y  = -1 - (mpu6000_rec_buf[11] << 8 | mpu6000_rec_buf[12]); // chip Y
-        data.gyro_x  = mpu6000_rec_buf[9] << 8 | mpu6000_rec_buf[10]; // chip X
-        break;
-    case PIOS_MPU6000_TOP_180DEG:
-        data.accel_y = -1 - (mpu6000_rec_buf[1] << 8 | mpu6000_rec_buf[2]); // chip X
-        data.accel_x = -1 - (mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]); // chip Y
-        data.gyro_y  = -1 - (mpu6000_rec_buf[9] << 8 | mpu6000_rec_buf[10]); // chip X
-        data.gyro_x  = -1 - (mpu6000_rec_buf[11] << 8 | mpu6000_rec_buf[12]); // chip Y
-        break;
-    case PIOS_MPU6000_TOP_270DEG:
-        data.accel_y = mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]; // chip Y
-        data.accel_x = -1 - (mpu6000_rec_buf[1] << 8 | mpu6000_rec_buf[2]); // chip X
-        data.gyro_y  = mpu6000_rec_buf[11] << 8 | mpu6000_rec_buf[12]; // chip Y
-        data.gyro_x  = -1 - (mpu6000_rec_buf[9] << 8 | mpu6000_rec_buf[10]); // chip X
-        break;
-    }
-    data.gyro_z      = -1 - (mpu6000_rec_buf[13] << 8 | mpu6000_rec_buf[14]);
-    data.accel_z     = -1 - (mpu6000_rec_buf[5] << 8 | mpu6000_rec_buf[6]);
-    data.temperature = mpu6000_rec_buf[7] << 8 | mpu6000_rec_buf[8];
-#else /* if defined(PIOS_MPU6000_ACCEL) */
-    data.gyro_x      = mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4];
-    data.gyro_y      = mpu6000_rec_buf[5] << 8 | mpu6000_rec_buf[6];
-    switch (dev->cfg->orientation) {
-    case PIOS_MPU6000_TOP_0DEG:
-        data.gyro_y = mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4];
-        data.gyro_x = mpu6000_rec_buf[5] << 8 | mpu6000_rec_buf[6];
-        break;
-    case PIOS_MPU6000_TOP_90DEG:
-        data.gyro_y = -1 - (mpu6000_rec_buf[5] << 8 | mpu6000_rec_buf[6]); // chip Y
-        data.gyro_x = mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]; // chip X
-        break;
-    case PIOS_MPU6000_TOP_180DEG:
-        data.gyro_y = -1 - (mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]);
-        data.gyro_x = -1 - (mpu6000_rec_buf[5] << 8 | mpu6000_rec_buf[6]);
-        break;
-    case PIOS_MPU6000_TOP_270DEG:
-        data.gyro_y = mpu6000_rec_buf[5] << 8 | mpu6000_rec_buf[6]; // chip Y
-        data.gyro_x = -1 - (mpu6000_rec_buf[3] << 8 | mpu6000_rec_buf[4]); // chip X
-        break;
-    }
-    data.gyro_z = -1 - (mpu6000_rec_buf[7] << 8 | mpu6000_rec_buf[8]);
-    data.temperature = mpu6000_rec_buf[1] << 8 | mpu6000_rec_buf[2];
-#endif /* if defined(PIOS_MPU6000_ACCEL) */
-
-    signed portBASE_TYPE higherPriorityTaskWoken;
-    xQueueSendToBackFromISR(dev->queue, (void *)&data, &higherPriorityTaskWoken);
-
-    mpu6000_irq++;
-
-    mpu6000_time_us = PIOS_DELAY_DiffuS(timeval);
-
-    return woken || higherPriorityTaskWoken == pdTRUE;
+    return true;
 }
-
 #endif /* PIOS_INCLUDE_MPU6000 */
 
 /**
diff --git a/flight/pios/common/pios_rfm22b.c b/flight/pios/common/pios_rfm22b.c
index 2783e09da..eb2f2bb8b 100644
--- a/flight/pios/common/pios_rfm22b.c
+++ b/flight/pios/common/pios_rfm22b.c
@@ -116,8 +116,6 @@
 #define USB_LED_OFF
 #endif
 
-#define CONNECTED_LIVE                   8
-
 /* Local type definitions */
 
 struct pios_rfm22b_transition {
@@ -360,7 +358,7 @@ static const uint8_t reg_72[] = { 0x30, 0x48, 0x48, 0x48, 0x48, 0x60, 0x90, 0xCD
 
 static const uint8_t packet_time[] = { 80, 40, 25, 15, 13, 10, 8, 6, 5 };
 static const uint8_t packet_time_ppm[] = { 26, 25, 25, 15, 13, 10, 8, 6, 5 };
-static const uint8_t num_channels[] = { 32, 32, 32, 32, 32, 32, 32, 32, 32 };
+static const uint8_t num_channels[] = { 4, 4, 4, 6, 8, 8, 10, 12, 16 };
 
 static struct pios_rfm22b_dev *g_rfm22b_dev = NULL;
 
@@ -608,8 +606,6 @@ void PIOS_RFM22B_SetChannelConfig(uint32_t rfm22b_id, enum rfm22b_datarate datar
         }
     }
 
-    rfm22b_dev->num_channels = num_found;
-
     // Calculate the maximum packet length from the datarate.
     float bytes_per_period = (float)data_rate[datarate] * (float)(rfm22b_dev->packet_time - 2) / 9000;
 
@@ -1359,7 +1355,7 @@ static enum pios_radio_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev)
     rfm22b_dev->packet_start_ticks = 0;
     rfm22b_dev->tx_complete_ticks  = 0;
     rfm22b_dev->rfm22b_state       = RFM22B_STATE_INITIALIZING;
-    rfm22b_dev->connected_timeout  = 0;
+    rfm22b_dev->on_sync_channel    = false;
 
     // software reset the RF chip .. following procedure according to Si4x3x Errata (rev. B)
     rfm22_write_claim(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_swres);
@@ -1803,8 +1799,8 @@ static enum pios_radio_event radio_txStart(struct pios_rfm22b_dev *radio_dev)
         len += (radio_dev->tx_out_cb)(radio_dev->tx_out_context, p + len, max_data_len - len, NULL, &need_yield);
     }
 
-    // Always send a packet if this modem is a coordinator.
-    if ((len == 0) && !rfm22_isCoordinator(radio_dev)) {
+    // Always send a packet on the sync channel if this modem is a coordinator.
+    if ((len == 0) && ((radio_dev->channel_index != 0) || !rfm22_isCoordinator(radio_dev))) {
         return RADIO_EVENT_RX_MODE;
     }
 
@@ -1964,16 +1960,12 @@ static enum pios_radio_event radio_receivePacket(struct pios_rfm22b_dev *radio_d
             (radio_dev->rx_in_cb)(radio_dev->rx_in_context, p, data_len, NULL, &rx_need_yield);
         }
 
-        /*
-         * If the packet is valid and destined for us we synchronize the clock.
-         */
-        if (!rfm22_isCoordinator(radio_dev) &&
-                radio_dev->rx_destination_id == rfm22_destinationID(radio_dev)) {
+        // We only synchronize the clock on packets from our coordinator on the sync channel.
+        if (!rfm22_isCoordinator(radio_dev) && (radio_dev->rx_destination_id == rfm22_destinationID(radio_dev)) && (radio_dev->channel_index == 0)) {
             rfm22_synchronizeClock(radio_dev);
             radio_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_CONNECTED;
+            radio_dev->on_sync_channel  = false;
         }
-
-        radio_dev->connected_timeout = CONNECTED_LIVE;
     } else {
         ret_event = RADIO_EVENT_RX_COMPLETE;
     }
@@ -2169,14 +2161,14 @@ static void rfm22_synchronizeClock(struct pios_rfm22b_dev *rfm22b_dev)
     portTickType start_time = rfm22b_dev->packet_start_ticks;
 
     // This packet was transmitted on channel 0, calculate the time delta that will force us to transmit on channel 0 at the time this packet started.
-    uint16_t frequency_hop_cycle_time = rfm22b_dev->packet_time * rfm22b_dev->num_channels;
+    uint8_t num_chan    = num_channels[rfm22b_dev->datarate];
+    uint16_t frequency_hop_cycle_time = rfm22b_dev->packet_time * num_chan;
     uint16_t time_delta = start_time % frequency_hop_cycle_time;
 
     // Calculate the adjustment for the preamble
     uint8_t offset = (uint8_t)ceil(35000.0F / data_rate[rfm22b_dev->datarate]);
 
-    rfm22b_dev->time_delta = frequency_hop_cycle_time - time_delta + offset +
-        rfm22b_dev->packet_time * rfm22b_dev->channel_index;
+    rfm22b_dev->time_delta = frequency_hop_cycle_time - time_delta + offset;
 }
 
 /**
@@ -2231,11 +2223,14 @@ static bool rfm22_timeToSend(struct pios_rfm22b_dev *rfm22b_dev)
 static uint8_t rfm22_calcChannel(struct pios_rfm22b_dev *rfm22b_dev, uint8_t index)
 {
     // Make sure we don't index outside of the range.
-    uint8_t idx = index % rfm22b_dev->num_channels;
+    uint8_t num_chan = num_channels[rfm22b_dev->datarate];
+    uint8_t idx = index % num_chan;
 
     // Are we switching to a new channel?
     if (idx != rfm22b_dev->channel_index) {
-        if (!rfm22_isCoordinator(rfm22b_dev) && rfm22b_dev->connected_timeout == 0) {
+        // If the on_sync_channel flag is set, it means that we were on the sync channel, but no packet was received, so transition to a non-connected state.
+        if (!rfm22_isCoordinator(rfm22b_dev) && (rfm22b_dev->channel_index == 0) && rfm22b_dev->on_sync_channel) {
+            rfm22b_dev->on_sync_channel = false;
 
             // Set the link state to disconnected.
             if (rfm22b_dev->stats.link_state == OPLINKSTATUS_LINKSTATE_CONNECTED) {
@@ -2246,14 +2241,14 @@ static uint8_t rfm22_calcChannel(struct pios_rfm22b_dev *rfm22b_dev, uint8_t ind
                 }
             }
 
-            // Stay on first channel.
+            // Stay on the sync channel.
             idx = 0;
+        } else if (idx == 0) {
+            // If we're switching to the sync channel, set a flag that can be used to detect if a packet was received.
+            rfm22b_dev->on_sync_channel = true;
         }
 
         rfm22b_dev->channel_index = idx;
-        if (rfm22b_dev->connected_timeout > 0)
-            rfm22b_dev->connected_timeout--;
-
     }
 
     return rfm22b_dev->channels[idx];
@@ -2269,7 +2264,8 @@ static uint8_t rfm22_calcChannelFromClock(struct pios_rfm22b_dev *rfm22b_dev)
     portTickType time = rfm22_coordinatorTime(rfm22b_dev, xTaskGetTickCount());
     // Divide time into 8ms blocks.  Coordinator sends in first 2 ms, and remote send in 5th and 6th ms.
     // Channel changes occur in the last 2 ms.
-    uint8_t n = (time / rfm22b_dev->packet_time) % rfm22b_dev->num_channels;
+    uint8_t num_chan  = num_channels[rfm22b_dev->datarate];
+    uint8_t n = (time / rfm22b_dev->packet_time) % num_chan;
 
     return rfm22_calcChannel(rfm22b_dev, n);
 }
diff --git a/flight/pios/inc/pios_mpu6000.h b/flight/pios/inc/pios_mpu6000.h
index cda019edf..e8c40a5c8 100644
--- a/flight/pios/inc/pios_mpu6000.h
+++ b/flight/pios/inc/pios_mpu6000.h
@@ -159,6 +159,9 @@ struct pios_mpu6000_cfg {
     enum pios_mpu6000_range gyro_range;
     enum pios_mpu6000_filter filter;
     enum pios_mpu6000_orientation orientation;
+    SPIPrescalerTypeDef fast_prescaler;
+    SPIPrescalerTypeDef std_prescaler;
+    uint8_t max_downsample;
 };
 
 /* Public Functions */
diff --git a/flight/pios/inc/pios_rfm22b_priv.h b/flight/pios/inc/pios_rfm22b_priv.h
index 7efdd449e..bacc2dcda 100644
--- a/flight/pios/inc/pios_rfm22b_priv.h
+++ b/flight/pios/inc/pios_rfm22b_priv.h
@@ -780,7 +780,7 @@ struct pios_rfm22b_dev {
     portTickType packet_start_ticks;
     portTickType tx_complete_ticks;
     portTickType time_delta;
-    uint8_t      connected_timeout;
+    bool         on_sync_channel;
 };
 
 
diff --git a/flight/pios/stm32f10x/pios_spi.c b/flight/pios/stm32f10x/pios_spi.c
index 21db6bd5e..01b7722aa 100644
--- a/flight/pios/stm32f10x/pios_spi.c
+++ b/flight/pios/stm32f10x/pios_spi.c
@@ -44,6 +44,7 @@ static bool PIOS_SPI_validate(__attribute__((unused)) struct pios_spi_dev *com_d
     /* Should check device magic here */
     return true;
 }
+#define SPI_MAX_BLOCK_PIO 128
 
 #if defined(PIOS_INCLUDE_FREERTOS)
 static struct pios_spi_dev *PIOS_SPI_alloc(void)
@@ -432,23 +433,7 @@ int32_t PIOS_SPI_TransferByte(uint32_t spi_id, uint8_t b)
     return rx_byte;
 }
 
-/**
- * Transfers a block of bytes via DMA.
- * \param[in] spi SPI number (0 or 1)
- * \param[in] send_buffer pointer to buffer which should be sent.<BR>
- * If NULL, 0xff (all-one) will be sent.
- * \param[in] receive_buffer pointer to buffer which should get the received values.<BR>
- * If NULL, received bytes will be discarded.
- * \param[in] len number of bytes which should be transfered
- * \param[in] callback pointer to callback function which will be executed
- * from DMA channel interrupt once the transfer is finished.
- * If NULL, no callback function will be used, and PIOS_SPI_TransferBlock() will
- * block until the transfer is finished.
- * \return >= 0 if no error during transfer
- * \return -1 if disabled SPI port selected
- * \return -3 if function has been called during an ongoing DMA transfer
- */
-int32_t PIOS_SPI_TransferBlock(uint32_t spi_id, const uint8_t *send_buffer, uint8_t *receive_buffer, uint16_t len, void *callback)
+static int32_t SPI_DMA_TransferBlock(uint32_t spi_id, const uint8_t *send_buffer, uint8_t *receive_buffer, uint16_t len, void *callback)
 {
     struct pios_spi_dev *spi_dev = (struct pios_spi_dev *)spi_id;
 
@@ -580,6 +565,95 @@ int32_t PIOS_SPI_TransferBlock(uint32_t spi_id, const uint8_t *send_buffer, uint
     return 0;
 }
 
+/**
+ * Transfers a block of bytes via PIO.
+ *
+ * \param[in] spi_id SPI device handle
+ * \param[in] send_buffer pointer to buffer which should be sent.<BR>
+ * If NULL, 0xff (all-one) will be sent.
+ * \param[in] receive_buffer pointer to buffer which should get the received values.<BR>
+ * If NULL, received bytes will be discarded.
+ * \param[in] len number of bytes which should be transfered
+ * \return >= 0 if no error during transfer
+ * \return -1 if disabled SPI port selected
+ * \return -3 if function has been called during an ongoing DMA transfer
+ */
+static int32_t SPI_PIO_TransferBlock(uint32_t spi_id, const uint8_t *send_buffer, uint8_t *receive_buffer, uint16_t len)
+{
+    struct pios_spi_dev *spi_dev = (struct pios_spi_dev *)spi_id;
+    uint8_t b;
+
+    bool valid = PIOS_SPI_validate(spi_dev);
+
+    PIOS_Assert(valid)
+
+    /* Exit if ongoing transfer */
+    if (DMA_GetCurrDataCounter(spi_dev->cfg->dma.rx.channel)) {
+        return -3;
+    }
+
+    /* Make sure the RXNE flag is cleared by reading the DR register */
+    b = spi_dev->cfg->regs->DR;
+
+    while (len--) {
+        /* get the byte to send */
+        b = send_buffer ? *(send_buffer++) : 0xff;
+
+        /* Start the transfer */
+        spi_dev->cfg->regs->DR = b;
+
+        /* Wait until there is a byte to read */
+        while (!(spi_dev->cfg->regs->SR & SPI_I2S_FLAG_RXNE)) {
+            ;
+        }
+
+        /* Read the rx'd byte */
+        b = spi_dev->cfg->regs->DR;
+
+        /* save the received byte */
+        if (receive_buffer) {
+            *(receive_buffer++) = b;
+        }
+
+        /* Wait until the TXE goes high */
+        while (!(spi_dev->cfg->regs->SR & SPI_I2S_FLAG_TXE)) {
+            ;
+        }
+    }
+
+    /* Wait for SPI transfer to have fully completed */
+    while (spi_dev->cfg->regs->SR & SPI_I2S_FLAG_BSY) {
+        ;
+    }
+
+    return 0;
+}
+
+
+/**
+ * Transfers a block of bytes via PIO or DMA.
+ * \param[in] spi_id SPI device handle
+ * \param[in] send_buffer pointer to buffer which should be sent.<BR>
+ * If NULL, 0xff (all-one) will be sent.
+ * \param[in] receive_buffer pointer to buffer which should get the received values.<BR>
+ * If NULL, received bytes will be discarded.
+ * \param[in] len number of bytes which should be transfered
+ * \param[in] callback pointer to callback function which will be executed
+ * from DMA channel interrupt once the transfer is finished.
+ * If NULL, no callback function will be used, and PIOS_SPI_TransferBlock() will
+ * block until the transfer is finished.
+ * \return >= 0 if no error during transfer
+ * \return -1 if disabled SPI port selected
+ * \return -3 if function has been called during an ongoing DMA transfer
+ */
+int32_t PIOS_SPI_TransferBlock(uint32_t spi_id, const uint8_t *send_buffer, uint8_t *receive_buffer, uint16_t len, void *callback)
+{
+    if (callback || len > SPI_MAX_BLOCK_PIO) {
+        return SPI_DMA_TransferBlock(spi_id, send_buffer, receive_buffer, len, callback);
+    }
+    return SPI_PIO_TransferBlock(spi_id, send_buffer, receive_buffer, len);
+}
+
 /**
  * Check if a transfer is in progress
  * \param[in] spi SPI number (0 or 1)
diff --git a/flight/targets/boards/coptercontrol/board_hw_defs.c b/flight/targets/boards/coptercontrol/board_hw_defs.c
index c2cb8b48d..6463451b3 100644
--- a/flight/targets/boards/coptercontrol/board_hw_defs.c
+++ b/flight/targets/boards/coptercontrol/board_hw_defs.c
@@ -260,7 +260,7 @@ static const struct pios_spi_cfg pios_spi_flash_accel_cfg_cc3d = {
         .gpio = GPIOB,
         .init = {
             .GPIO_Pin   = GPIO_Pin_13,
-            .GPIO_Speed = GPIO_Speed_10MHz,
+            .GPIO_Speed = GPIO_Speed_50MHz,
             .GPIO_Mode  = GPIO_Mode_AF_PP,
         },
     },
@@ -268,7 +268,7 @@ static const struct pios_spi_cfg pios_spi_flash_accel_cfg_cc3d = {
         .gpio = GPIOB,
         .init = {
             .GPIO_Pin   = GPIO_Pin_14,
-            .GPIO_Speed = GPIO_Speed_10MHz,
+            .GPIO_Speed = GPIO_Speed_50MHz,
             .GPIO_Mode  = GPIO_Mode_IN_FLOATING,
         },
     },
@@ -276,7 +276,7 @@ static const struct pios_spi_cfg pios_spi_flash_accel_cfg_cc3d = {
         .gpio = GPIOB,
         .init = {
             .GPIO_Pin   = GPIO_Pin_15,
-            .GPIO_Speed = GPIO_Speed_10MHz,
+            .GPIO_Speed = GPIO_Speed_50MHz,
             .GPIO_Mode  = GPIO_Mode_AF_PP,
         },
     },
@@ -360,7 +360,7 @@ static const struct pios_spi_cfg pios_spi_flash_accel_cfg_cc = {
         .gpio = GPIOB,
         .init = {
             .GPIO_Pin   = GPIO_Pin_13,
-            .GPIO_Speed = GPIO_Speed_10MHz,
+            .GPIO_Speed = GPIO_Speed_50MHz,
             .GPIO_Mode  = GPIO_Mode_AF_PP,
         },
     },
@@ -368,7 +368,7 @@ static const struct pios_spi_cfg pios_spi_flash_accel_cfg_cc = {
         .gpio = GPIOB,
         .init = {
             .GPIO_Pin   = GPIO_Pin_14,
-            .GPIO_Speed = GPIO_Speed_10MHz,
+            .GPIO_Speed = GPIO_Speed_50MHz,
             .GPIO_Mode  = GPIO_Mode_IN_FLOATING,
         },
     },
@@ -376,7 +376,7 @@ static const struct pios_spi_cfg pios_spi_flash_accel_cfg_cc = {
         .gpio = GPIOB,
         .init = {
             .GPIO_Pin   = GPIO_Pin_15,
-            .GPIO_Speed = GPIO_Speed_10MHz,
+            .GPIO_Speed = GPIO_Speed_50MHz,
             .GPIO_Mode  = GPIO_Mode_AF_PP,
         },
     },
diff --git a/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h b/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h
index 95332df45..9450a6781 100644
--- a/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h
+++ b/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h
@@ -92,6 +92,8 @@
 /* #define PIOS_INCLUDE_ETASV3 */
 /* #define PIOS_INCLUDE_HCSR04 */
 
+#define PIOS_SENSOR_RATE 500.0f
+
 /* PIOS receiver drivers */
 #define PIOS_INCLUDE_PWM
 #define PIOS_INCLUDE_PPM
diff --git a/flight/targets/boards/coptercontrol/firmware/pios_board.c b/flight/targets/boards/coptercontrol/firmware/pios_board.c
index 2cb1734ff..fdbdde5a0 100644
--- a/flight/targets/boards/coptercontrol/firmware/pios_board.c
+++ b/flight/targets/boards/coptercontrol/firmware/pios_board.c
@@ -119,18 +119,21 @@ static const struct pios_exti_cfg pios_exti_mpu6000_cfg __exti_config = {
 static const struct pios_mpu6000_cfg pios_mpu6000_cfg = {
     .exti_cfg   = &pios_exti_mpu6000_cfg,
     .Fifo_store = PIOS_MPU6000_FIFO_TEMP_OUT | PIOS_MPU6000_FIFO_GYRO_X_OUT | PIOS_MPU6000_FIFO_GYRO_Y_OUT | PIOS_MPU6000_FIFO_GYRO_Z_OUT,
-    // Clock at 8 khz, downsampled by 16 for 500 Hz
-    .Smpl_rate_div_no_dlp = 15,
-    // Clock at 1 khz, downsampled by 2 for 500 Hz
-    .Smpl_rate_div_dlp    = 1,
-    .interrupt_cfg = PIOS_MPU6000_INT_CLR_ANYRD,
-    .interrupt_en  = PIOS_MPU6000_INTEN_DATA_RDY,
-    .User_ctl             = PIOS_MPU6000_USERCTL_FIFO_EN | PIOS_MPU6000_USERCTL_DIS_I2C,
-    .Pwr_mgmt_clk  = PIOS_MPU6000_PWRMGMT_PLL_X_CLK,
-    .accel_range   = PIOS_MPU6000_ACCEL_8G,
-    .gyro_range    = PIOS_MPU6000_SCALE_2000_DEG,
+    // Clock at 8 khz, downsampled by 8 for 1000 Hz
+    .Smpl_rate_div_no_dlp = 7,
+    // Clock at 1 khz, downsampled by 1 for 1000 Hz
+    .Smpl_rate_div_dlp    = 0,
+    .interrupt_cfg  = PIOS_MPU6000_INT_CLR_ANYRD,
+    .interrupt_en   = PIOS_MPU6000_INTEN_DATA_RDY,
+    .User_ctl             = PIOS_MPU6000_USERCTL_DIS_I2C,
+    .Pwr_mgmt_clk   = PIOS_MPU6000_PWRMGMT_PLL_X_CLK,
+    .accel_range    = PIOS_MPU6000_ACCEL_8G,
+    .gyro_range     = PIOS_MPU6000_SCALE_2000_DEG,
     .filter               = PIOS_MPU6000_LOWPASS_256_HZ,
-    .orientation   = PIOS_MPU6000_TOP_180DEG
+    .orientation    = PIOS_MPU6000_TOP_180DEG,
+    .fast_prescaler = PIOS_SPI_PRESCALER_4,
+    .std_prescaler  = PIOS_SPI_PRESCALER_64,
+    .max_downsample = 2
 };
 #endif /* PIOS_INCLUDE_MPU6000 */
 
diff --git a/flight/targets/boards/discoveryf4bare/firmware/inc/pios_config.h b/flight/targets/boards/discoveryf4bare/firmware/inc/pios_config.h
index 44065be33..11ec1415a 100644
--- a/flight/targets/boards/discoveryf4bare/firmware/inc/pios_config.h
+++ b/flight/targets/boards/discoveryf4bare/firmware/inc/pios_config.h
@@ -91,6 +91,9 @@
 // #define PIOS_INCLUDE_MPXV
 // #define PIOS_INCLUDE_ETASV3
 /* #define PIOS_INCLUDE_HCSR04 */
+
+#define PIOS_SENSOR_RATE 500.0f
+
 #define PIOS_INCLUDE_WS2811
 
 /* PIOS receiver drivers */
diff --git a/flight/targets/boards/revolution/firmware/inc/pios_config.h b/flight/targets/boards/revolution/firmware/inc/pios_config.h
index 5ee827783..15281a88b 100644
--- a/flight/targets/boards/revolution/firmware/inc/pios_config.h
+++ b/flight/targets/boards/revolution/firmware/inc/pios_config.h
@@ -91,11 +91,12 @@
 #define PIOS_INCLUDE_MPXV
 #define PIOS_INCLUDE_ETASV3
 #define PIOS_INCLUDE_MS4525DO
+/* #define PIOS_INCLUDE_HCSR04 */
+
+#define PIOS_SENSOR_RATE 500.0f
 
 #define PIOS_INCLUDE_WS2811
 
-/* #define PIOS_INCLUDE_HCSR04 */
-
 /* PIOS receiver drivers */
 #define PIOS_INCLUDE_PWM
 #define PIOS_INCLUDE_PPM
diff --git a/flight/targets/boards/revolution/firmware/pios_board.c b/flight/targets/boards/revolution/firmware/pios_board.c
index b1c418dae..1f8029992 100644
--- a/flight/targets/boards/revolution/firmware/pios_board.c
+++ b/flight/targets/boards/revolution/firmware/pios_board.c
@@ -192,18 +192,21 @@ static const struct pios_exti_cfg pios_exti_mpu6000_cfg __exti_config = {
 static const struct pios_mpu6000_cfg pios_mpu6000_cfg = {
     .exti_cfg   = &pios_exti_mpu6000_cfg,
     .Fifo_store = PIOS_MPU6000_FIFO_TEMP_OUT | PIOS_MPU6000_FIFO_GYRO_X_OUT | PIOS_MPU6000_FIFO_GYRO_Y_OUT | PIOS_MPU6000_FIFO_GYRO_Z_OUT,
-    // Clock at 8 khz, downsampled by 12 for 666Hz
-    .Smpl_rate_div_no_dlp = 11,
-    // with dlp on output rate is 500Hz
-    .Smpl_rate_div_dlp    = 1,
-    .interrupt_cfg = PIOS_MPU6000_INT_CLR_ANYRD,
-    .interrupt_en  = PIOS_MPU6000_INTEN_DATA_RDY,
-    .User_ctl             = PIOS_MPU6000_USERCTL_FIFO_EN | PIOS_MPU6000_USERCTL_DIS_I2C,
-    .Pwr_mgmt_clk  = PIOS_MPU6000_PWRMGMT_PLL_X_CLK,
-    .accel_range   = PIOS_MPU6000_ACCEL_8G,
-    .gyro_range    = PIOS_MPU6000_SCALE_2000_DEG,
+    // Clock at 8 khz
+    .Smpl_rate_div_no_dlp = 0,
+    // with dlp on output rate is 1000Hz
+    .Smpl_rate_div_dlp    = 0,
+    .interrupt_cfg  = PIOS_MPU6000_INT_CLR_ANYRD,
+    .interrupt_en   = PIOS_MPU6000_INTEN_DATA_RDY,
+    .User_ctl             = PIOS_MPU6000_USERCTL_DIS_I2C,
+    .Pwr_mgmt_clk   = PIOS_MPU6000_PWRMGMT_PLL_X_CLK,
+    .accel_range    = PIOS_MPU6000_ACCEL_8G,
+    .gyro_range     = PIOS_MPU6000_SCALE_2000_DEG,
     .filter               = PIOS_MPU6000_LOWPASS_256_HZ,
-    .orientation   = PIOS_MPU6000_TOP_180DEG
+    .orientation    = PIOS_MPU6000_TOP_180DEG,
+    .fast_prescaler = PIOS_SPI_PRESCALER_4,
+    .std_prescaler  = PIOS_SPI_PRESCALER_64,
+    .max_downsample = 16,
 };
 #endif /* PIOS_INCLUDE_MPU6000 */
 
diff --git a/flight/targets/boards/revoproto/firmware/inc/pios_config.h b/flight/targets/boards/revoproto/firmware/inc/pios_config.h
index 897f9b360..b7aed8375 100644
--- a/flight/targets/boards/revoproto/firmware/inc/pios_config.h
+++ b/flight/targets/boards/revoproto/firmware/inc/pios_config.h
@@ -89,6 +89,8 @@
 #define PIOS_INCLUDE_ETASV3
 /* #define PIOS_INCLUDE_HCSR04 */
 
+#define PIOS_SENSOR_RATE 500.0f
+
 /* PIOS receiver drivers */
 #define PIOS_INCLUDE_PWM
 #define PIOS_INCLUDE_PPM
diff --git a/flight/targets/boards/revoproto/firmware/pios_board.c b/flight/targets/boards/revoproto/firmware/pios_board.c
index 8a414ddbd..c7e887d28 100644
--- a/flight/targets/boards/revoproto/firmware/pios_board.c
+++ b/flight/targets/boards/revoproto/firmware/pios_board.c
@@ -222,18 +222,21 @@ static const struct pios_exti_cfg pios_exti_mpu6000_cfg __exti_config = {
 static const struct pios_mpu6000_cfg pios_mpu6000_cfg = {
     .exti_cfg   = &pios_exti_mpu6000_cfg,
     .Fifo_store = PIOS_MPU6000_FIFO_TEMP_OUT | PIOS_MPU6000_FIFO_GYRO_X_OUT | PIOS_MPU6000_FIFO_GYRO_Y_OUT | PIOS_MPU6000_FIFO_GYRO_Z_OUT,
-    // Clock at 8 khz, downsampled by 12 for 666Hz
-    .Smpl_rate_div_no_dlp = 11,
-    // with dlp on output rate is 500Hz
-    .Smpl_rate_div_dlp    = 1,
-    .interrupt_cfg = PIOS_MPU6000_INT_CLR_ANYRD,
-    .interrupt_en  = PIOS_MPU6000_INTEN_DATA_RDY,
-    .User_ctl             = PIOS_MPU6000_USERCTL_FIFO_EN | PIOS_MPU6000_USERCTL_DIS_I2C,
-    .Pwr_mgmt_clk  = PIOS_MPU6000_PWRMGMT_PLL_X_CLK,
-    .accel_range   = PIOS_MPU6000_ACCEL_8G,
-    .gyro_range    = PIOS_MPU6000_SCALE_2000_DEG,
+    // Clock at 8 khz
+    .Smpl_rate_div_no_dlp = 0,
+    // with dlp on output rate is 1000Hz
+    .Smpl_rate_div_dlp    = 0,
+    .interrupt_cfg  = PIOS_MPU6000_INT_CLR_ANYRD,
+    .interrupt_en   = PIOS_MPU6000_INTEN_DATA_RDY,
+    .User_ctl             = PIOS_MPU6000_USERCTL_DIS_I2C,
+    .Pwr_mgmt_clk   = PIOS_MPU6000_PWRMGMT_PLL_X_CLK,
+    .accel_range    = PIOS_MPU6000_ACCEL_8G,
+    .gyro_range     = PIOS_MPU6000_SCALE_2000_DEG,
     .filter               = PIOS_MPU6000_LOWPASS_256_HZ,
-    .orientation   = PIOS_MPU6000_TOP_0DEG
+    .orientation    = PIOS_MPU6000_TOP_0DEG,
+    .fast_prescaler = PIOS_SPI_PRESCALER_4,
+    .std_prescaler  = PIOS_SPI_PRESCALER_64,
+    .max_downsample = 16,
 };
 #endif /* PIOS_INCLUDE_MPU6000 */
 
diff --git a/flight/targets/boards/simposix/firmware/inc/pios_config.h b/flight/targets/boards/simposix/firmware/inc/pios_config.h
index 0dc6b7aaa..a592b487c 100644
--- a/flight/targets/boards/simposix/firmware/inc/pios_config.h
+++ b/flight/targets/boards/simposix/firmware/inc/pios_config.h
@@ -70,6 +70,9 @@
 // #define PIOS_INCLUDE_MS5611
 // #define PIOS_INCLUDE_HCSR04
 #define PIOS_FLASH_ON_ACCEL /* true for second revo */
+
+#define PIOS_SENSOR_RATE    500.0f
+
 #define FLASH_FREERTOS
 /* Com systems to include */
 #define PIOS_INCLUDE_COM
diff --git a/ground/openpilotgcs/src/libs/utils/mustache.cpp b/ground/openpilotgcs/src/libs/utils/mustache.cpp
new file mode 100644
index 000000000..8cb33b734
--- /dev/null
+++ b/ground/openpilotgcs/src/libs/utils/mustache.cpp
@@ -0,0 +1,559 @@
+/*
+   Copyright 2012, Robert Knight
+
+   Redistribution and use in source and binary forms, with or without modification,
+   are permitted provided that the following conditions are met:
+
+    Redistributions of source code must retain the above copyright notice,
+    this list of conditions and the following disclaimer.
+
+    Redistributions in binary form must reproduce the above copyright notice,
+    this list of conditions and the following disclaimer in the documentation
+    and/or other materials provided with the distribution.
+ */
+
+#include "mustache.h"
+
+#include <QtCore/QDebug>
+#include <QtCore/QFile>
+#include <QtCore/QStringList>
+#include <QtCore/QTextStream>
+
+using namespace Mustache;
+
+QString Mustache::renderTemplate(const QString & templateString, const QVariantHash & args)
+{
+    Mustache::QtVariantContext context(args);
+    Mustache::Renderer renderer;
+
+    return renderer.render(templateString, &context);
+}
+
+QString escapeHtml(const QString & input)
+{
+    QString escaped(input);
+
+    for (int i = 0; i < escaped.count();) {
+        const char *replacement = 0;
+        ushort ch = escaped.at(i).unicode();
+        if (ch == '&') {
+            replacement = "&amp;";
+        } else if (ch == '<') {
+            replacement = "&lt;";
+        } else if (ch == '>') {
+            replacement = "&gt;";
+        } else if (ch == '"') {
+            replacement = "&quot;";
+        }
+        if (replacement) {
+            escaped.replace(i, 1, QLatin1String(replacement));
+            i += strlen(replacement);
+        } else {
+            ++i;
+        }
+    }
+    return escaped;
+}
+
+QString unescapeHtml(const QString & escaped)
+{
+    QString unescaped(escaped);
+
+    unescaped.replace(QLatin1String("&lt;"), QLatin1String("<"));
+    unescaped.replace(QLatin1String("&gt;"), QLatin1String(">"));
+    unescaped.replace(QLatin1String("&amp;"), QLatin1String("&"));
+    unescaped.replace(QLatin1String("&quot;"), QLatin1String("\""));
+    return unescaped;
+}
+
+Context::Context(PartialResolver *resolver)
+    : m_partialResolver(resolver)
+{}
+
+PartialResolver *Context::partialResolver() const
+{
+    return m_partialResolver;
+}
+
+QString Context::partialValue(const QString & key) const
+{
+    if (!m_partialResolver) {
+        return QString();
+    }
+    return m_partialResolver->getPartial(key);
+}
+
+bool Context::canEval(const QString &) const
+{
+    return false;
+}
+
+QString Context::eval(const QString & key, const QString & _template, Renderer *renderer)
+{
+    Q_UNUSED(key);
+    Q_UNUSED(_template);
+    Q_UNUSED(renderer);
+
+    return QString();
+}
+
+QtVariantContext::QtVariantContext(const QVariant & root, PartialResolver *resolver)
+    : Context(resolver)
+{
+    m_contextStack << root;
+}
+
+QVariant variantMapValue(const QVariant & value, const QString & key)
+{
+    if (value.userType() == QVariant::Map) {
+        return value.toMap().value(key);
+    } else {
+        return value.toHash().value(key);
+    }
+}
+
+QVariant variantMapValueForKeyPath(const QVariant & value, const QStringList keyPath)
+{
+    if (keyPath.count() > 1) {
+        QVariant firstValue = variantMapValue(value, keyPath.first());
+        return firstValue.isNull() ? QVariant() : variantMapValueForKeyPath(firstValue, keyPath.mid(1));
+    } else if (!keyPath.isEmpty()) {
+        return variantMapValue(value, keyPath.first());
+    }
+    return QVariant();
+}
+
+QVariant QtVariantContext::value(const QString & key) const
+{
+    if (key == "." && !m_contextStack.isEmpty()) {
+        return m_contextStack.last();
+    }
+    QStringList keyPath = key.split(".");
+    for (int i = m_contextStack.count() - 1; i >= 0; i--) {
+        QVariant value = variantMapValueForKeyPath(m_contextStack.at(i), keyPath);
+        if (!value.isNull()) {
+            return value;
+        }
+    }
+    return QVariant();
+}
+
+bool QtVariantContext::isFalse(const QString & key) const
+{
+    QVariant value = this->value(key);
+
+    switch (value.userType()) {
+    case QVariant::Bool:
+        return !value.toBool();
+
+    case QVariant::List:
+        return value.toList().isEmpty();
+
+    case QVariant::Hash:
+        return value.toHash().isEmpty();
+
+    case QVariant::Map:
+        return value.toMap().isEmpty();
+
+    default:
+        return value.toString().isEmpty();
+    }
+}
+
+QString QtVariantContext::stringValue(const QString & key) const
+{
+    if (isFalse(key)) {
+        return QString();
+    }
+    return value(key).toString();
+}
+
+void QtVariantContext::push(const QString & key, int index)
+{
+    QVariant mapItem = value(key);
+
+    if (index == -1) {
+        m_contextStack << mapItem;
+    } else {
+        QVariantList list = mapItem.toList();
+        m_contextStack << list.value(index, QVariant());
+    }
+}
+
+void QtVariantContext::pop()
+{
+    m_contextStack.pop();
+}
+
+int QtVariantContext::listCount(const QString & key) const
+{
+    if (value(key).userType() == QVariant::List) {
+        return value(key).toList().count();
+    }
+    return 0;
+}
+
+bool QtVariantContext::canEval(const QString & key) const
+{
+    return value(key).canConvert<fn_t>();
+}
+
+QString QtVariantContext::eval(const QString & key, const QString & _template, Renderer *renderer)
+{
+    QVariant fn = value(key);
+
+    if (fn.isNull()) {
+        return QString();
+    }
+    return fn.value<fn_t>() (_template, renderer, this);
+}
+
+PartialMap::PartialMap(const QHash<QString, QString> & partials)
+    : m_partials(partials)
+{}
+
+QString PartialMap::getPartial(const QString & name)
+{
+    return m_partials.value(name);
+}
+
+PartialFileLoader::PartialFileLoader(const QString & basePath)
+    : m_basePath(basePath)
+{}
+
+QString PartialFileLoader::getPartial(const QString & name)
+{
+    if (!m_cache.contains(name)) {
+        QString path = m_basePath + '/' + name + ".mustache";
+        QFile file(path);
+        if (file.open(QIODevice::ReadOnly)) {
+            QTextStream stream(&file);
+            m_cache.insert(name, stream.readAll());
+        }
+    }
+    return m_cache.value(name);
+}
+
+Renderer::Renderer()
+    : m_errorPos(-1)
+    , m_defaultTagStartMarker("{{")
+    , m_defaultTagEndMarker("}}")
+{}
+
+QString Renderer::error() const
+{
+    return m_error;
+}
+
+int Renderer::errorPos() const
+{
+    return m_errorPos;
+}
+
+QString Renderer::errorPartial() const
+{
+    return m_errorPartial;
+}
+
+QString Renderer::render(const QString & _template, Context *context)
+{
+    m_error.clear();
+    m_errorPos = -1;
+    m_errorPartial.clear();
+
+    m_tagStartMarker = m_defaultTagStartMarker;
+    m_tagEndMarker   = m_defaultTagEndMarker;
+
+    return render(_template, 0, _template.length(), context);
+}
+
+QString Renderer::render(const QString & _template, int startPos, int endPos, Context *context)
+{
+    QString output;
+    int lastTagEnd = startPos;
+
+    while (m_errorPos == -1) {
+        Tag tag = findTag(_template, lastTagEnd, endPos);
+        if (tag.type == Tag::Null) {
+            output += _template.midRef(lastTagEnd, endPos - lastTagEnd);
+            break;
+        }
+        output += _template.midRef(lastTagEnd, tag.start - lastTagEnd);
+        switch (tag.type) {
+        case Tag::Value:
+        {
+            QString value = context->stringValue(tag.key);
+            if (tag.escapeMode == Tag::Escape) {
+                value = escapeHtml(value);
+            } else if (tag.escapeMode == Tag::Unescape) {
+                value = unescapeHtml(value);
+            }
+            output    += value;
+            lastTagEnd = tag.end;
+        }
+        break;
+        case Tag::SectionStart:
+        {
+            Tag endTag = findEndTag(_template, tag, endPos);
+            if (endTag.type == Tag::Null) {
+                if (m_errorPos == -1) {
+                    setError("No matching end tag found for section", tag.start);
+                }
+            } else {
+                int listCount = context->listCount(tag.key);
+                if (listCount > 0) {
+                    for (int i = 0; i < listCount; i++) {
+                        context->push(tag.key, i);
+                        output += render(_template, tag.end, endTag.start, context);
+                        context->pop();
+                    }
+                } else if (context->canEval(tag.key)) {
+                    output += context->eval(tag.key, _template.mid(tag.end, endTag.start - tag.end), this);
+                } else if (!context->isFalse(tag.key)) {
+                    context->push(tag.key);
+                    output += render(_template, tag.end, endTag.start, context);
+                    context->pop();
+                }
+                lastTagEnd = endTag.end;
+            }
+        }
+        break;
+        case Tag::InvertedSectionStart:
+        {
+            Tag endTag = findEndTag(_template, tag, endPos);
+            if (endTag.type == Tag::Null) {
+                if (m_errorPos == -1) {
+                    setError("No matching end tag found for inverted section", tag.start);
+                }
+            } else {
+                if (context->isFalse(tag.key)) {
+                    output += render(_template, tag.end, endTag.start, context);
+                }
+                lastTagEnd = endTag.end;
+            }
+        }
+        break;
+        case Tag::SectionEnd:
+            setError("Unexpected end tag", tag.start);
+            lastTagEnd = tag.end;
+            break;
+        case Tag::Partial:
+        {
+            QString tagStartMarker = m_tagStartMarker;
+            QString tagEndMarker   = m_tagEndMarker;
+
+            m_tagStartMarker = m_defaultTagStartMarker;
+            m_tagEndMarker   = m_defaultTagEndMarker;
+
+            m_partialStack.push(tag.key);
+
+            QString partial = context->partialValue(tag.key);
+            output    += render(partial, 0, partial.length(), context);
+            lastTagEnd = tag.end;
+
+            m_partialStack.pop();
+
+            m_tagStartMarker = tagStartMarker;
+            m_tagEndMarker   = tagEndMarker;
+        }
+        break;
+        case Tag::SetDelimiter:
+            lastTagEnd = tag.end;
+            break;
+        case Tag::Comment:
+            lastTagEnd = tag.end;
+            break;
+        case Tag::Null:
+            break;
+        }
+    }
+
+    return output;
+}
+
+void Renderer::setError(const QString & error, int pos)
+{
+    Q_ASSERT(!error.isEmpty());
+    Q_ASSERT(pos >= 0);
+
+    m_error    = error;
+    m_errorPos = pos;
+
+    if (!m_partialStack.isEmpty()) {
+        m_errorPartial = m_partialStack.top();
+    }
+}
+
+Tag Renderer::findTag(const QString & content, int pos, int endPos)
+{
+    int tagStartPos = content.indexOf(m_tagStartMarker, pos);
+
+    if (tagStartPos == -1 || tagStartPos >= endPos) {
+        return Tag();
+    }
+
+    int tagEndPos = content.indexOf(m_tagEndMarker, tagStartPos + m_tagStartMarker.length());
+    if (tagEndPos == -1) {
+        return Tag();
+    }
+    tagEndPos += m_tagEndMarker.length();
+
+    Tag tag;
+    tag.type   = Tag::Value;
+    tag.start  = tagStartPos;
+    tag.end    = tagEndPos;
+
+    pos = tagStartPos + m_tagStartMarker.length();
+    endPos     = tagEndPos - m_tagEndMarker.length();
+
+    QChar typeChar = content.at(pos);
+
+    if (typeChar == '#') {
+        tag.type = Tag::SectionStart;
+        tag.key  = readTagName(content, pos + 1, endPos);
+    } else if (typeChar == '^') {
+        tag.type = Tag::InvertedSectionStart;
+        tag.key  = readTagName(content, pos + 1, endPos);
+    } else if (typeChar == '/') {
+        tag.type = Tag::SectionEnd;
+        tag.key  = readTagName(content, pos + 1, endPos);
+    } else if (typeChar == '!') {
+        tag.type = Tag::Comment;
+    } else if (typeChar == '>') {
+        tag.type = Tag::Partial;
+        tag.key  = readTagName(content, pos + 1, endPos);
+    } else if (typeChar == '=') {
+        tag.type = Tag::SetDelimiter;
+        readSetDelimiter(content, pos + 1, tagEndPos - m_tagEndMarker.length());
+    } else {
+        if (typeChar == '&') {
+            tag.escapeMode = Tag::Unescape;
+            ++pos;
+        } else if (typeChar == '{') {
+            tag.escapeMode = Tag::Raw;
+            ++pos;
+            int endTache = content.indexOf('}', pos);
+            if (endTache == tag.end - m_tagEndMarker.length()) {
+                ++tag.end;
+            } else {
+                endPos = endTache;
+            }
+        }
+        tag.type = Tag::Value;
+        tag.key  = readTagName(content, pos, endPos);
+    }
+
+    if (tag.type != Tag::Value) {
+        expandTag(tag, content);
+    }
+
+    return tag;
+}
+
+QString Renderer::readTagName(const QString & content, int pos, int endPos)
+{
+    QString name;
+
+    name.reserve(endPos - pos);
+    while (content.at(pos).isSpace()) {
+        ++pos;
+    }
+    while (!content.at(pos).isSpace() && pos < endPos) {
+        name += content.at(pos);
+        ++pos;
+    }
+    return name;
+}
+
+void Renderer::readSetDelimiter(const QString & content, int pos, int endPos)
+{
+    QString startMarker;
+    QString endMarker;
+
+    while (content.at(pos).isSpace() && pos < endPos) {
+        ++pos;
+    }
+
+    while (!content.at(pos).isSpace() && pos < endPos) {
+        if (content.at(pos) == '=') {
+            setError("Custom delimiters may not contain '='.", pos);
+            return;
+        }
+        startMarker += content.at(pos);
+        ++pos;
+    }
+
+    while (content.at(pos).isSpace() && pos < endPos) {
+        ++pos;
+    }
+
+    while (!content.at(pos).isSpace() && pos < endPos - 1) {
+        if (content.at(pos) == '=') {
+            setError("Custom delimiters may not contain '='.", pos);
+            return;
+        }
+        endMarker += content.at(pos);
+        ++pos;
+    }
+
+    m_tagStartMarker = startMarker;
+    m_tagEndMarker   = endMarker;
+}
+
+Tag Renderer::findEndTag(const QString & content, const Tag & startTag, int endPos)
+{
+    int tagDepth = 1;
+    int pos = startTag.end;
+
+    while (true) {
+        Tag nextTag = findTag(content, pos, endPos);
+        if (nextTag.type == Tag::Null) {
+            return nextTag;
+        } else if (nextTag.type == Tag::SectionStart || nextTag.type == Tag::InvertedSectionStart) {
+            ++tagDepth;
+        } else if (nextTag.type == Tag::SectionEnd) {
+            --tagDepth;
+            if (tagDepth == 0) {
+                if (nextTag.key != startTag.key) {
+                    setError("Tag start/end key mismatch", nextTag.start);
+                    return Tag();
+                }
+                return nextTag;
+            }
+        }
+        pos = nextTag.end;
+    }
+
+    return Tag();
+}
+
+void Renderer::setTagMarkers(const QString & startMarker, const QString & endMarker)
+{
+    m_defaultTagStartMarker = startMarker;
+    m_defaultTagEndMarker   = endMarker;
+}
+
+void Renderer::expandTag(Tag & tag, const QString & content)
+{
+    int start = tag.start;
+    int end   = tag.end;
+
+    // Move start to beginning of line.
+    while (start > 0 && content.at(start - 1) != QLatin1Char('\n')) {
+        --start;
+        if (!content.at(start).isSpace()) {
+            return; // Not standalone.
+        }
+    }
+
+    // Move end to one past end of line.
+    while (end <= content.size() && content.at(end - 1) != QLatin1Char('\n')) {
+        if (end < content.size() && !content.at(end).isSpace()) {
+            return; // Not standalone.
+        }
+        ++end;
+    }
+
+    tag.start = start;
+    tag.end   = end;
+}
diff --git a/ground/openpilotgcs/src/libs/utils/mustache.h b/ground/openpilotgcs/src/libs/utils/mustache.h
new file mode 100644
index 000000000..2d76e3564
--- /dev/null
+++ b/ground/openpilotgcs/src/libs/utils/mustache.h
@@ -0,0 +1,266 @@
+/*
+   Copyright 2012, Robert Knight
+
+   Redistribution and use in source and binary forms, with or without modification,
+   are permitted provided that the following conditions are met:
+
+    Redistributions of source code must retain the above copyright notice,
+    this list of conditions and the following disclaimer.
+
+    Redistributions in binary form must reproduce the above copyright notice,
+    this list of conditions and the following disclaimer in the documentation
+    and/or other materials provided with the distribution.
+ */
+
+#pragma once
+
+#include <QtCore/QStack>
+#include <QtCore/QString>
+#include <QtCore/QVariant>
+
+#include "utils_global.h"
+
+#if __cplusplus >= 201103L
+#include <functional> /* for std::function */
+#endif
+
+namespace Mustache {
+class PartialResolver;
+class Renderer;
+
+/** Context is an interface that Mustache::Renderer::render() uses to
+ * fetch substitutions for template tags.
+ */
+class QTCREATOR_UTILS_EXPORT Context {
+public:
+    /** Create a context.  @p resolver is used to fetch the expansions for any {{>partial}} tags
+     * which appear in a template.
+     */
+    explicit Context(PartialResolver *resolver = 0);
+    virtual ~Context() {}
+
+    /** Returns a string representation of the value for @p key in the current context.
+     * This is used to replace a Mustache value tag.
+     */
+    virtual QString stringValue(const QString & key) const = 0;
+
+    /** Returns true if the value for @p key is 'false' or an empty list.
+     * 'False' values typically include empty strings, the boolean value false etc.
+     *
+     * When processing a section Mustache tag, the section is not rendered if the key
+     * is false, or for an inverted section tag, the section is only rendered if the key
+     * is false.
+     */
+    virtual bool isFalse(const QString & key) const  = 0;
+
+    /** Returns the number of items in the list value for @p key or 0 if
+     * the value for @p key is not a list.
+     */
+    virtual int listCount(const QString & key) const = 0;
+
+    /** Set the current context to the value for @p key.
+     * If index is >= 0, set the current context to the @p index'th value
+     * in the list value for @p key.
+     */
+    virtual void push(const QString & key, int index = -1) = 0;
+
+    /** Exit the current context. */
+    virtual void pop() = 0;
+
+    /** Returns the partial template for a given @p key. */
+    QString partialValue(const QString & key) const;
+
+    /** Returns the partial resolver passed to the constructor. */
+    PartialResolver *partialResolver() const;
+
+    /** Returns true if eval() should be used to render section tags using @p key.
+     * If canEval() returns true for a key, the renderer will pass the literal, unrendered
+     * block of text for the section to eval() and replace the section with the result.
+     *
+     * canEval() and eval() are equivalents for callable objects (eg. lambdas) in other
+     * Mustache implementations.
+     *
+     * The default implementation always returns false.
+     */
+    virtual bool canEval(const QString & key) const;
+
+    /** Callback used to render a template section with the given @p key.
+     * @p renderer will substitute the original section tag with the result of eval().
+     *
+     * The default implementation returns an empty string.
+     */
+    virtual QString eval(const QString & key, const QString & _template, Renderer *renderer);
+
+private:
+    PartialResolver *m_partialResolver;
+};
+
+/** A context implementation which wraps a QVariantHash or QVariantMap. */
+class QTCREATOR_UTILS_EXPORT QtVariantContext : public Context {
+public:
+    /** Construct a QtVariantContext which wraps a dictionary in a QVariantHash
+     * or a QVariantMap.
+     */
+#if __cplusplus >= 201103L
+    typedef std::function<QString(const QString &, Mustache::Renderer *, Mustache::Context *)> fn_t;
+#else
+    typedef QString (*fn_t)(const QString &, Mustache::Renderer *, Mustache::Context *);
+#endif
+    explicit QtVariantContext(const QVariant & root, PartialResolver *resolver = 0);
+
+    virtual QString stringValue(const QString & key) const;
+    virtual bool isFalse(const QString & key) const;
+    virtual int listCount(const QString & key) const;
+    virtual void push(const QString & key, int index = -1);
+    virtual void pop();
+    virtual bool canEval(const QString & key) const;
+    virtual QString eval(const QString & key, const QString & _template, Mustache::Renderer *renderer);
+
+private:
+    QVariant value(const QString & key) const;
+
+    QStack<QVariant> m_contextStack;
+};
+
+/** Interface for fetching template partials. */
+class QTCREATOR_UTILS_EXPORT PartialResolver {
+public:
+    virtual ~PartialResolver() {}
+
+    /** Returns the partial template with a given @p name. */
+    virtual QString getPartial(const QString & name) = 0;
+};
+
+/** A simple partial fetcher which returns templates from a map of (partial name -> template)
+ */
+class QTCREATOR_UTILS_EXPORT PartialMap : public PartialResolver {
+public:
+    explicit PartialMap(const QHash<QString, QString> & partials);
+
+    virtual QString getPartial(const QString & name);
+
+private:
+    QHash<QString, QString> m_partials;
+};
+
+/** A partial fetcher when loads templates from '<name>.mustache' files
+ * in a given directory.
+ *
+ * Once a partial has been loaded, it is cached for future use.
+ */
+class QTCREATOR_UTILS_EXPORT PartialFileLoader : public PartialResolver {
+public:
+    explicit PartialFileLoader(const QString & basePath);
+
+    virtual QString getPartial(const QString & name);
+
+private:
+    QString m_basePath;
+    QHash<QString, QString> m_cache;
+};
+
+/** Holds properties of a tag in a mustache template. */
+struct Tag {
+    enum Type {
+        Null,
+        Value, /// A {{key}} or {{{key}}} tag
+        SectionStart, /// A {{#section}} tag
+        InvertedSectionStart, /// An {{^inverted-section}} tag
+        SectionEnd, /// A {{/section}} tag
+        Partial, /// A {{^partial}} tag
+        Comment, /// A {{! comment }} tag
+        SetDelimiter /// A {{=<% %>=}} tag
+    };
+
+    enum EscapeMode {
+        Escape,
+        Unescape,
+        Raw
+    };
+
+    Tag()
+        : type(Null)
+        , start(0)
+        , end(0)
+        , escapeMode(Escape)
+    {}
+
+    Type       type;
+    QString    key;
+    int        start;
+    int        end;
+    EscapeMode escapeMode;
+};
+
+/** Renders Mustache templates, replacing mustache tags with
+ * values from a provided context.
+ */
+class QTCREATOR_UTILS_EXPORT Renderer {
+public:
+    Renderer();
+
+    /** Render a Mustache template, using @p context to fetch
+     * the values used to replace Mustache tags.
+     */
+    QString render(const QString & _template, Context *context);
+
+    /** Returns a message describing the last error encountered by the previous
+     * render() call.
+     */
+    QString error() const;
+
+    /** Returns the position in the template where the last error occurred
+     * when rendering the template or -1 if no error occurred.
+     *
+     * If the error occurred in a partial template, the returned position is the offset
+     * in the partial template.
+     */
+    int errorPos() const;
+
+    /** Returns the name of the partial where the error occurred, or an empty string
+     * if the error occurred in the main template.
+     */
+    QString errorPartial() const;
+
+    /** Sets the default tag start and end markers.
+     * This can be overridden within a template.
+     */
+    void setTagMarkers(const QString & startMarker, const QString & endMarker);
+
+private:
+    QString render(const QString & _template, int startPos, int endPos, Context *context);
+
+    Tag findTag(const QString & content, int pos, int endPos);
+    Tag findEndTag(const QString & content, const Tag & startTag, int endPos);
+    void setError(const QString & error, int pos);
+
+    void readSetDelimiter(const QString & content, int pos, int endPos);
+    static QString readTagName(const QString & content, int pos, int endPos);
+
+    /** Expands @p tag to fill the line, but only if it is standalone.
+     *
+     * The start position is moved to the beginning of the line. The end position is
+     * moved to one past the end of the line. If @p tag is not standalone, it is
+     * left unmodified.
+     *
+     * A tag is standalone if it is the only non-whitespace token on the the line.
+     */
+    static void expandTag(Tag & tag, const QString & content);
+
+    QStack<QString> m_partialStack;
+    QString m_error;
+    int m_errorPos;
+    QString m_errorPartial;
+
+    QString m_tagStartMarker;
+    QString m_tagEndMarker;
+
+    QString m_defaultTagStartMarker;
+    QString m_defaultTagEndMarker;
+};
+
+/** A convenience function which renders a template using the given data. */
+QString renderTemplate(const QString & templateString, const QVariantHash & args);
+};
+
+Q_DECLARE_METATYPE(Mustache::QtVariantContext::fn_t)
diff --git a/ground/openpilotgcs/src/libs/utils/utils.pro b/ground/openpilotgcs/src/libs/utils/utils.pro
index 231ff64f0..be86d2abe 100644
--- a/ground/openpilotgcs/src/libs/utils/utils.pro
+++ b/ground/openpilotgcs/src/libs/utils/utils.pro
@@ -56,7 +56,8 @@ SOURCES += reloadpromptutils.cpp \
     svgimageprovider.cpp \
     hostosinfo.cpp \
     logfile.cpp \
-    crc.cpp
+    crc.cpp \
+    mustache.cpp
 
 SOURCES += xmlconfig.cpp
 
@@ -115,7 +116,8 @@ HEADERS += utils_global.h \
     svgimageprovider.h \
     hostosinfo.h \
     logfile.h \
-    crc.h
+    crc.h \
+    mustache.h
 
 
 HEADERS += xmlconfig.h
diff --git a/ground/openpilotgcs/src/plugins/config/airframe.ui b/ground/openpilotgcs/src/plugins/config/airframe.ui
index 100a9f50a..9609b357a 100644
--- a/ground/openpilotgcs/src/plugins/config/airframe.ui
+++ b/ground/openpilotgcs/src/plugins/config/airframe.ui
@@ -17,6 +17,89 @@
    <property name="spacing">
     <number>0</number>
    </property>
+   <item>
+    <layout class="QHBoxLayout" name="horizontalLayout">
+     <property name="leftMargin">
+      <number>0</number>
+     </property>
+     <property name="topMargin">
+      <number>0</number>
+     </property>
+     <property name="rightMargin">
+      <number>0</number>
+     </property>
+     <property name="bottomMargin">
+      <number>9</number>
+     </property>
+     <item>
+      <widget class="QLabel" name="label">
+       <property name="sizePolicy">
+        <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
+         <horstretch>0</horstretch>
+         <verstretch>0</verstretch>
+        </sizepolicy>
+       </property>
+       <property name="text">
+        <string>Vehicle name</string>
+       </property>
+      </widget>
+     </item>
+     <item>
+      <spacer name="horizontalSpacer_2">
+       <property name="orientation">
+        <enum>Qt::Horizontal</enum>
+       </property>
+       <property name="sizeType">
+        <enum>QSizePolicy::Fixed</enum>
+       </property>
+       <property name="sizeHint" stdset="0">
+        <size>
+         <width>6</width>
+         <height>20</height>
+        </size>
+       </property>
+      </spacer>
+     </item>
+     <item alignment="Qt::AlignLeft">
+      <widget class="QLineEdit" name="nameEdit">
+       <property name="sizePolicy">
+        <sizepolicy hsizetype="Preferred" vsizetype="Fixed">
+         <horstretch>0</horstretch>
+         <verstretch>0</verstretch>
+        </sizepolicy>
+       </property>
+       <property name="minimumSize">
+        <size>
+         <width>300</width>
+         <height>0</height>
+        </size>
+       </property>
+       <property name="inputMask">
+        <string/>
+       </property>
+       <property name="maxLength">
+        <number>20</number>
+       </property>
+       <property name="placeholderText">
+        <string>Enter name of vehicle. Max 20 characters.</string>
+       </property>
+      </widget>
+     </item>
+     <item>
+      <spacer name="horizontalSpacer">
+       <property name="orientation">
+        <enum>Qt::Horizontal</enum>
+       </property>
+       <property name="sizeHint" stdset="0">
+        <size>
+         <width>40</width>
+         <height>20</height>
+        </size>
+       </property>
+      </spacer>
+     </item>
+    </layout>
+   </item>
    <item>
     <widget class="QTabBar" name="aircraftType" native="true"/>
    </item>
@@ -26,7 +109,7 @@
       <enum>Qt::LeftToRight</enum>
      </property>
      <property name="autoFillBackground">
-      <bool>true</bool>
+      <bool>false</bool>
      </property>
      <property name="styleSheet">
       <string notr="true">#vehicleTypeFrame{
@@ -159,7 +242,7 @@
                <x>0</x>
                <y>0</y>
                <width>820</width>
-               <height>478</height>
+               <height>435</height>
               </rect>
              </property>
              <layout class="QVBoxLayout" name="verticalLayout">
@@ -306,8 +389,8 @@
               <rect>
                <x>0</x>
                <y>0</y>
-               <width>271</width>
-               <height>307</height>
+               <width>838</width>
+               <height>445</height>
               </rect>
              </property>
              <layout class="QVBoxLayout" name="verticalLayout_18">
@@ -717,14 +800,14 @@ p, li { white-space: pre-wrap; }
                  <string>&lt;!DOCTYPE HTML PUBLIC &quot;-//W3C//DTD HTML 4.0//EN&quot; &quot;http://www.w3.org/TR/REC-html40/strict.dtd&quot;&gt;
 &lt;html&gt;&lt;head&gt;&lt;meta name=&quot;qrichtext&quot; content=&quot;1&quot; /&gt;&lt;style type=&quot;text/css&quot;&gt;
 p, li { white-space: pre-wrap; }
-&lt;/style&gt;&lt;/head&gt;&lt;body style=&quot; font-family:'Sans Serif'; font-size:9pt; font-weight:400; font-style:normal;&quot;&gt;
+&lt;/style&gt;&lt;/head&gt;&lt;body style=&quot; font-family:'Ubuntu'; font-size:11pt; font-weight:400; font-style:normal;&quot;&gt;
 &lt;table border=&quot;0&quot; style=&quot;-qt-table-type: root; margin-top:4px; margin-bottom:4px; margin-left:4px; margin-right:4px;&quot;&gt;
 &lt;tr&gt;
 &lt;td style=&quot;border: none;&quot;&gt;
-&lt;p align=&quot;center&quot; style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-family:'Ubuntu'; font-size:14pt; font-weight:600; color:#ff0000;&quot;&gt;SETTING UP FEED FORWARD REQUIRES CAUTION&lt;/span&gt;&lt;/p&gt;
-&lt;p style=&quot;-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-family:'Ubuntu'; font-size:11pt;&quot;&gt;&lt;br /&gt;&lt;/p&gt;
-&lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-family:'Ubuntu'; font-size:11pt;&quot;&gt;&lt;br /&gt;&lt;/span&gt;&lt;/p&gt;
-&lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-family:'Ubuntu'; font-size:11pt;&quot;&gt;&lt;br /&gt;&lt;/span&gt;&lt;/p&gt;
+&lt;p align=&quot;center&quot; style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-size:14pt; font-weight:600; color:#ff0000;&quot;&gt;SETTING UP FEED FORWARD REQUIRES CAUTION&lt;/span&gt;&lt;/p&gt;
+&lt;p style=&quot;-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;br /&gt;&lt;/p&gt;
+&lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;br /&gt;&lt;/p&gt;
+&lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;br /&gt;&lt;/p&gt;
 &lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-family:'Lucida Grande'; font-size:13pt;&quot;&gt;Beware: Feed Forward Tuning will launch all engines around mid-throttle, you have been warned!&lt;/span&gt;&lt;/p&gt;
 &lt;p style=&quot; margin-top:12px; margin-bottom:12px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-family:'Lucida Grande'; font-size:13pt;&quot;&gt;Remove your props initially, and for fine-tuning, make sure your airframe is safely held in place. Wear glasses and protect your face and body.&lt;/span&gt;&lt;/p&gt;&lt;/td&gt;&lt;/tr&gt;&lt;/table&gt;&lt;/body&gt;&lt;/html&gt;</string>
                 </property>
diff --git a/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.cpp b/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.cpp
index 0593600e0..276362643 100644
--- a/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.cpp
+++ b/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.cpp
@@ -473,8 +473,8 @@ void SixPointCalibrationModel::compute()
     revoCalibrationData.MagBiasNullingRate = memento.revoCalibrationData.MagBiasNullingRate;;
 
     // Check the mag calibration is good
-    bool good_calibration     = true;
-    bool good_aux_calibration = true;
+    bool good_calibration = true;
+    // bool good_aux_calibration = true;
     if (calibratingMag) {
         good_calibration &= !IS_NAN(revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C0]);
         good_calibration &= !IS_NAN(revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C1]);
@@ -581,7 +581,7 @@ void SixPointCalibrationModel::save()
     if (calibratingMag) {
         RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData();
 
-        for (int i = 0; i < RevoCalibration::MAG_TRANSFORM_NUMELEM; i++) {
+        for (uint i = 0; i < RevoCalibration::MAG_TRANSFORM_NUMELEM; i++) {
             revoCalibrationData.mag_transform[i] = result.revoCalibrationData.mag_transform[i];
         }
         for (int i = 0; i < 3; i++) {
@@ -592,7 +592,7 @@ void SixPointCalibrationModel::save()
         if (calibratingAuxMag) {
             AuxMagSettings::DataFields auxCalibrationData = auxMagSettings->getData();
             // Note that Revo/AuxMag MAG_TRANSFORM_RxCx are interchangeable, an assertion at initialization enforces the structs are equal
-            for (int i = 0; i < RevoCalibration::MAG_TRANSFORM_NUMELEM; i++) {
+            for (uint i = 0; i < RevoCalibration::MAG_TRANSFORM_NUMELEM; i++) {
                 auxCalibrationData.mag_transform[i] = result.auxMagSettingsData.mag_transform[i];
             }
             for (int i = 0; i < 3; i++) {
diff --git a/ground/openpilotgcs/src/plugins/config/configstabilizationwidget.cpp b/ground/openpilotgcs/src/plugins/config/configstabilizationwidget.cpp
index 7c9fb0eae..f7786ec1a 100644
--- a/ground/openpilotgcs/src/plugins/config/configstabilizationwidget.cpp
+++ b/ground/openpilotgcs/src/plugins/config/configstabilizationwidget.cpp
@@ -187,14 +187,12 @@ void ConfigStabilizationWidget::updateThrottleCurveFromObject()
     UAVObject *stabBank = getObjectManager()->getObject(QString(m_pidTabBars.at(0)->tabData(m_currentPIDBank).toString()));
 
     Q_ASSERT(stabBank);
-    qDebug() << "updatingCurveFromObject" << stabBank->getName();
 
     UAVObjectField *field = stabBank->getField("ThrustPIDScaleCurve");
     Q_ASSERT(field);
 
     QList<double> curve;
     for (quint32 i = 0; i < field->getNumElements(); i++) {
-        qDebug() << field->getName() << field->getElementNames().at(i) << "=>" << field->getValue(i);
         curve.append(field->getValue(i).toDouble());
     }
 
@@ -214,7 +212,6 @@ void ConfigStabilizationWidget::updateObjectFromThrottleCurve()
     UAVObject *stabBank = getObjectManager()->getObject(QString(m_pidTabBars.at(0)->tabData(m_currentPIDBank).toString()));
 
     Q_ASSERT(stabBank);
-    qDebug() << "updatingObjectFromCurve" << stabBank->getName();
 
     UAVObjectField *field = stabBank->getField("ThrustPIDScaleCurve");
     Q_ASSERT(field);
@@ -222,7 +219,6 @@ void ConfigStabilizationWidget::updateObjectFromThrottleCurve()
     QList<double> curve   = ui->thrustPIDScalingCurve->getCurve();
     for (quint32 i = 0; i < field->getNumElements(); i++) {
         field->setValue(curve.at(i), i);
-        qDebug() << field->getName() << field->getElementNames().at(i) << "<=" << curve.at(i);
     }
 
     field = stabBank->getField("EnableThrustPIDScaling");
@@ -351,10 +347,8 @@ void ConfigStabilizationWidget::realtimeUpdatesSlot(bool value)
 
     if (value && !realtimeUpdates->isActive()) {
         realtimeUpdates->start(AUTOMATIC_UPDATE_RATE);
-        qDebug() << "Instant Update timer started.";
     } else if (!value && realtimeUpdates->isActive()) {
         realtimeUpdates->stop();
-        qDebug() << "Instant Update timer stopped.";
     }
 }
 
@@ -451,7 +445,6 @@ void ConfigStabilizationWidget::pidBankChanged(int index)
     setWidgetBindingObjectEnabled(m_pidTabBars.at(0)->tabData(index).toString(), true);
 
     m_currentPIDBank = index;
-    qDebug() << "current bank:" << m_currentPIDBank;
     updateThrottleCurveFromObject();
     setDirty(dirty);
 }
diff --git a/ground/openpilotgcs/src/plugins/config/configstabilizationwidget.h b/ground/openpilotgcs/src/plugins/config/configstabilizationwidget.h
index 180700112..e8ed225a0 100644
--- a/ground/openpilotgcs/src/plugins/config/configstabilizationwidget.h
+++ b/ground/openpilotgcs/src/plugins/config/configstabilizationwidget.h
@@ -56,7 +56,7 @@ private:
     static const int AUTOMATIC_UPDATE_RATE   = 500;
 
     static const int EXPO_CURVE_POINTS_COUNT = 100;
-    static const double EXPO_CURVE_CONSTANT = 1.00695;
+    static const double EXPO_CURVE_CONSTANT  = 1.00695;
 
     int boardModel;
     int m_pidBankCount;
diff --git a/ground/openpilotgcs/src/plugins/config/configvehicletypewidget.cpp b/ground/openpilotgcs/src/plugins/config/configvehicletypewidget.cpp
index d012f3ecf..3925431eb 100644
--- a/ground/openpilotgcs/src/plugins/config/configvehicletypewidget.cpp
+++ b/ground/openpilotgcs/src/plugins/config/configvehicletypewidget.cpp
@@ -121,6 +121,10 @@ ConfigVehicleTypeWidget::ConfigVehicleTypeWidget(QWidget *parent) : ConfigTaskWi
         m_aircraft->saveAircraftToRAM->setVisible(false);
     }
 
+    SystemSettings *syssettings = SystemSettings::GetInstance(getObjectManager());
+    Q_ASSERT(syssettings);
+    m_aircraft->nameEdit->setMaxLength(syssettings->VEHICLENAME_NUMELEM);
+
     addApplySaveButtons(m_aircraft->saveAircraftToRAM, m_aircraft->saveAircraftToSD);
 
     addUAVObject("SystemSettings");
@@ -158,6 +162,7 @@ ConfigVehicleTypeWidget::ConfigVehicleTypeWidget(QWidget *parent) : ConfigTaskWi
     addWidget(m_aircraft->ffTestBox1);
     addWidget(m_aircraft->ffTestBox2);
     addWidget(m_aircraft->ffTestBox3);
+    addWidget(m_aircraft->nameEdit);
 
     disableMouseWheelEvents();
     updateEnableControls();
@@ -225,6 +230,19 @@ void ConfigVehicleTypeWidget::refreshWidgetsValues(UAVObject *o)
         }
     }
 
+    field = system->getField(QString("VehicleName"));
+    Q_ASSERT(field);
+    QString name;
+    for (uint i = 0; i < field->getNumElements(); ++i) {
+        QChar chr = field->getValue(i).toChar();
+        if (chr != 0) {
+            name.append(chr);
+        } else {
+            break;
+        }
+    }
+    m_aircraft->nameEdit->setText(name);
+
     updateFeedForwardUI();
 
     setDirty(dirty);
@@ -271,6 +289,17 @@ void ConfigVehicleTypeWidget::updateObjectsFromWidgets()
     vconfig->setMixerValue(mixer, "DecelTime", m_aircraft->decelTime->value());
     vconfig->setMixerValue(mixer, "MaxAccel", m_aircraft->maxAccelSlider->value());
 
+    field = system->getField(QString("VehicleName"));
+    Q_ASSERT(field);
+    QString name = m_aircraft->nameEdit->text();
+    for (uint i = 0; i < field->getNumElements(); ++i) {
+        if (i < (uint)name.length()) {
+            field->setValue(name.at(i).toLatin1(), i);
+        } else {
+            field->setValue(0, i);
+        }
+    }
+
     // call refreshWidgetsValues() to reflect actual saved values
     refreshWidgetsValues();
     updateFeedForwardUI();
diff --git a/ground/openpilotgcs/src/plugins/config/stabilization.ui b/ground/openpilotgcs/src/plugins/config/stabilization.ui
index 14dc71ae9..0d6ab931b 100644
--- a/ground/openpilotgcs/src/plugins/config/stabilization.ui
+++ b/ground/openpilotgcs/src/plugins/config/stabilization.ui
@@ -136,8 +136,8 @@
            <rect>
             <x>0</x>
             <y>0</y>
-            <width>952</width>
-            <height>771</height>
+            <width>950</width>
+            <height>780</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_3">
@@ -3435,7 +3435,7 @@ border-radius: 5;</string>
                        <enum>Qt::WheelFocus</enum>
                       </property>
                       <property name="toolTip">
-                       <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;This adjusts how much stability your vehicle will have when flying tilted (ie forward flight) in Rate mode. A good starting point for Integral is the same as Proportional&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+                       <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;This adjusts how much stability your vehicle will have when flying tilted (ie forward flight) in Rate mode. A good starting point for Integral is double the  Proportional value&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
                       </property>
                       <property name="maximum">
                        <number>100</number>
@@ -5339,7 +5339,7 @@ border-radius: 5;</string>
                        </size>
                       </property>
                       <property name="toolTip">
-                       <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;This adjusts how much stability your vehicle will have when flying tilted (ie forward flight) in Rate mode. A good starting point for Integral is the same as Proportional&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+                       <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;This adjusts how much stability your vehicle will have when flying tilted (ie forward flight) in Rate mode. A good starting point for Integral is double the Proportional value&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
                       </property>
                       <property name="maximum">
                        <number>100</number>
@@ -5455,7 +5455,7 @@ Then lower the value by 5 or so.</string>
                        </size>
                       </property>
                       <property name="toolTip">
-                       <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;This adjusts how much stability your vehicle will have when flying tilted (ie forward flight) in Rate mode. A good starting point for Integral is the same as Proportional&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+                       <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;&lt;span style=&quot; font-family:'Lucida Grande,arial,sans-serif'; font-size:12px; color:#333333; background-color:#ffffff;&quot;&gt;This adjusts how much yaw stability your vehicle will have in Rate mode. A good starting point for Integral is double the Proportional value&lt;/span&gt;&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
                       </property>
                       <property name="maximum">
                        <number>100</number>
@@ -8244,8 +8244,8 @@ border-radius: 5;</string>
            <rect>
             <x>0</x>
             <y>0</y>
-            <width>952</width>
-            <height>771</height>
+            <width>950</width>
+            <height>780</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_29">
@@ -11024,7 +11024,7 @@ border-radius: 5;</string>
                   <property name="checkable">
                    <bool>false</bool>
                   </property>
-                  <layout class="QGridLayout" name="gridLayout_27" rowstretch="0,0,0" columnminimumwidth="0,0">
+                  <layout class="QGridLayout" name="gridLayout_277" rowstretch="0,0,0" columnminimumwidth="0,0">
                    <property name="leftMargin">
                     <number>9</number>
                    </property>
@@ -13371,7 +13371,7 @@ border-radius: 5;</string>
                          <enum>Qt::StrongFocus</enum>
                         </property>
                         <property name="toolTip">
-                         <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;This adjusts how much stability your vehicle will have when flying tilted (ie forward flight) in Rate mode. A good starting point for Integral is the same as Proportional.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+                         <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;This adjusts how much stability your vehicle will have when flying tilted (ie forward flight) in Rate mode. A good starting point for Integral is double the Proportional value&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
                         </property>
                         <property name="styleSheet">
                          <string notr="true"/>
@@ -13971,7 +13971,7 @@ border-radius: 5;</string>
                          <enum>Qt::StrongFocus</enum>
                         </property>
                         <property name="toolTip">
-                         <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;This adjusts how much stability your vehicle will have when flying tilted (ie forward flight) in Rate mode. A good starting point for Integral is the same as Proportional.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+                         <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;&lt;span style=&quot; font-family:'Lucida Grande,arial,sans-serif'; font-size:12px; color:#333333; background-color:#ffffff;&quot;&gt;This adjusts how much yaw stability your vehicle will have in Rate mode. A good starting point for Integral is double the Proportional value&lt;/span&gt;&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
                         </property>
                         <property name="styleSheet">
                          <string notr="true"/>
@@ -14621,7 +14621,7 @@ border-radius: 5;</string>
                          <enum>Qt::StrongFocus</enum>
                         </property>
                         <property name="toolTip">
-                         <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;This adjusts how much stability your vehicle will have when flying tilted (ie forward flight) in Rate mode. A good starting point for Integral is the same as Proportional.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+                         <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;This adjusts how much stability your vehicle will have when flying tilted (ie forward flight) in Rate mode. A good starting point for Integral is double the Proportional value&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
                         </property>
                         <property name="styleSheet">
                          <string notr="true"/>
@@ -18236,8 +18236,8 @@ border-radius: 5;</string>
            <rect>
             <x>0</x>
             <y>0</y>
-            <width>952</width>
-            <height>771</height>
+            <width>950</width>
+            <height>780</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_8" stretch="0,0,0,0,0,0">
@@ -24082,8 +24082,8 @@ font:bold;</string>
            <rect>
             <x>0</x>
             <y>0</y>
-            <width>952</width>
-            <height>771</height>
+            <width>950</width>
+            <height>780</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_18">
diff --git a/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadgetwidget.cpp
index 37231eaa5..981aeb5b7 100644
--- a/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadgetwidget.cpp
+++ b/ground/openpilotgcs/src/plugins/magicwaypoint/magicwaypointgadgetwidget.cpp
@@ -47,8 +47,8 @@ MagicWaypointGadgetWidget::MagicWaypointGadgetWidget(QWidget *parent) : QLabel(p
     m_magicwaypoint->setupUi(this);
 
     // Connect object updated event from UAVObject to also update check boxes
-    connect(getPathDesired(), SIGNAL(objectUpdated(UAVObject *)), this, SLOT(positionObjectChanged(UAVObject *)));
-    connect(getPositionState(), SIGNAL(objectUpdated(UAVObject *)), this, SLOT(positionObjectChanged(UAVObject *)));
+    connect(getPathDesired(), SIGNAL(objectUpdated(UAVObject *)), this, SLOT(positionStateChanged(UAVObject *)));
+    connect(getPositionState(), SIGNAL(objectUpdated(UAVObject *)), this, SLOT(positionStateChanged(UAVObject *)));
 
     // Connect updates from the position widget to this widget
     connect(m_magicwaypoint->widgetPosition, SIGNAL(positionClicked(double, double)), this, SLOT(positionSelected(double, double)));
diff --git a/ground/openpilotgcs/src/plugins/uavobjectbrowser/resources/uavodescription.mustache b/ground/openpilotgcs/src/plugins/uavobjectbrowser/resources/uavodescription.mustache
new file mode 100644
index 000000000..1bb68c0f7
--- /dev/null
+++ b/ground/openpilotgcs/src/plugins/uavobjectbrowser/resources/uavodescription.mustache
@@ -0,0 +1,62 @@
+<html>
+    <head></head>
+    <body style="font-family:'Ubuntu'; font-size:11pt; font-weight:400; font-style:normal;">
+        <table width='100%' cellpadding="5">
+            <tr bgcolor='#308BC6'>
+                <td rowspan='1' colspan='4'><b>{{OBJECT_NAME_TITLE}}:&nbsp;</b>{{OBJECT_NAME}}</td>
+            </tr>
+            <tr>
+                <td colspan='1'><b>{{CATEGORY_TITLE}}:&nbsp;</b>{{CATEGORY}}</td>
+                <td colspan='1'><b>{{TYPE_TITLE}}:&nbsp;</b>{{TYPE}}</td>
+                <td colspan='1'><b>{{SIZE_TITLE}}:&nbsp;</b>{{SIZE}}&nbsp;bytes</td>
+                <td colspan='1'><b>{{MULTI_INSTANCE_TITLE}}:&nbsp;</b>{{MULTI_INSTANCE}}</td>
+            </tr>
+            <tr>
+                <td colspan='4' rowspan='1' valign='top'><b>{{DESCRIPTION_TITLE}}:&nbsp;</b>{{DESCRIPTION}}</td>
+            <tr bgcolor='#51A0D3'>
+                <td rowspan='1' colspan='4'><b>{{FIELDS_NAME_TITLE}}</td>
+            </tr>
+            {{#FIELDS}}
+            <tr>
+                <td colspan='4'>
+                    <table width='100%' cellpadding="5">
+                        <tr bgcolor='#7DBAE2'>
+                            <td rowspan='1' colspan='4'><b>{{FIELD_NAME_TITLE}}:&nbsp;</b>{{FIELD_NAME}}</td>
+                        </tr>
+                        <tr>
+                            <td rowspan='1' colspan='1'><b>{{FIELD_TYPE_TITLE}}:&nbsp;</b>{{FIELD_TYPE}}</td>
+                            {{#FIELD_UNIT_TITLE}}
+                            <td rowspan='1' colspan='2'><b>{{FIELD_UNIT_TITLE}}:&nbsp;</b>{{FIELD_UNIT}}</td>
+                            {{/FIELD_UNIT_TITLE}}
+                        </tr>
+                        {{#FIELD_OPTIONS_TITLE}}
+                        <tr>
+                            <td rowspan='1' colspan='2'><b>{{FIELD_OPTIONS_TITLE}}:&nbsp;
+                            </b>{{#FIELD_OPTIONS}}{{FIELD_OPTION_DELIM}}{{FIELD_OPTION}}{{/FIELD_OPTIONS}}</td>
+                        </tr>
+                        {{/FIELD_OPTIONS_TITLE}}
+                        {{#FIELD_ELEMENTS_TITLE}}
+                        <tr>
+                            <td rowspan='1' colspan='2'><b>{{FIELD_ELEMENTS_TITLE}}:&nbsp;
+                            </b>{{#FIELD_ELEMENTS}}{{FIELD_ELEMENT_DELIM}}{{FIELD_ELEMENT}}{{FIELD_ELEMENT_LIMIT}}{{/FIELD_ELEMENTS}}</td>
+                        </tr>
+                        {{/FIELD_ELEMENTS_TITLE}}
+                        {{^FIELD_ELEMENTS_TITLE}}
+                        {{#FIELD_LIMIT_TITLE}}
+                        <tr>
+                            <td rowspan='1' colspan='2'><b>{{FIELD_LIMIT_TITLE}}:&nbsp;</b>{{FIELD_LIMIT}}</td>
+                        </tr>
+                        {{/FIELD_LIMIT_TITLE}}
+                        {{/FIELD_ELEMENTS_TITLE}}
+                        {{#FIELD_DESCRIPTION_TITLE}}
+                        <tr>
+                            <td rowspan='1' colspan='2'><b>{{FIELD_DESCRIPTION_TITLE}}:&nbsp;</b>{{FIELD_DESCRIPTION}}</td>
+                        </tr>
+                        {{/FIELD_DESCRIPTION_TITLE}}
+                    </table>
+                </td>
+            </tr>
+            {{/FIELDS}}
+        </table>
+    </body>
+</html>
diff --git a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowser.cpp b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowser.cpp
index 6c5342e8d..b3dfacdea 100644
--- a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowser.cpp
+++ b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowser.cpp
@@ -32,7 +32,8 @@ UAVObjectBrowser::UAVObjectBrowser(QString classId, UAVObjectBrowserWidget *widg
     m_widget(widget),
     m_config(NULL)
 {
-    connect(m_widget, SIGNAL(viewOptionsChanged(bool, bool, bool)), this, SLOT(viewOptionsChangedSlot(bool, bool, bool)));
+    connect(m_widget, SIGNAL(viewOptionsChanged(bool, bool, bool, bool)), this, SLOT(viewOptionsChangedSlot(bool, bool, bool, bool)));
+    connect(m_widget, SIGNAL(splitterChanged(QByteArray)), this, SLOT(splitterChanged(QByteArray)));
 }
 
 UAVObjectBrowser::~UAVObjectBrowser()
@@ -49,14 +50,23 @@ void UAVObjectBrowser::loadConfiguration(IUAVGadgetConfiguration *config)
     m_widget->setManuallyChangedColor(m->manuallyChangedColor());
     m_widget->setRecentlyUpdatedTimeout(m->recentlyUpdatedTimeout());
     m_widget->setOnlyHilightChangedValues(m->onlyHighlightChangedValues());
-    m_widget->setViewOptions(m->categorizedView(), m->scientificView(), m->showMetaData());
+    m_widget->setViewOptions(m->categorizedView(), m->scientificView(), m->showMetaData(), m->showDescription());
+    m_widget->setSplitterState(m->splitterState());
 }
 
-void UAVObjectBrowser::viewOptionsChangedSlot(bool categorized, bool scientific, bool metadata)
+void UAVObjectBrowser::viewOptionsChangedSlot(bool categorized, bool scientific, bool metadata, bool description)
 {
     if (m_config) {
         m_config->setCategorizedView(categorized);
         m_config->setScientificView(scientific);
         m_config->setShowMetaData(metadata);
+        m_config->setShowDescription(description);
+    }
+}
+
+void UAVObjectBrowser::splitterChanged(QByteArray state)
+{
+    if (m_config) {
+        m_config->setSplitterState(state);
     }
 }
diff --git a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowser.h b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowser.h
index 261583d06..d84891a95 100644
--- a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowser.h
+++ b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowser.h
@@ -50,8 +50,11 @@ public:
         return m_widget;
     }
     void loadConfiguration(IUAVGadgetConfiguration *config);
+
 private slots:
-    void viewOptionsChangedSlot(bool categorized, bool scientific, bool metadata);
+    void viewOptionsChangedSlot(bool categorized, bool scientific, bool metadata, bool description);
+    void splitterChanged(QByteArray state);
+
 private:
     UAVObjectBrowserWidget *m_widget;
     UAVObjectBrowserConfiguration *m_config;
diff --git a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowser.qrc b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowser.qrc
index e5ac47658..7d3b82bb9 100644
--- a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowser.qrc
+++ b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowser.qrc
@@ -6,5 +6,6 @@
         <file>images/up_alt.png</file>
         <file>images/trash.png</file>
         <file>images/1343241276_eye.png</file>
+        <file>resources/uavodescription.mustache</file>
     </qresource>
 </RCC>
diff --git a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowser.ui b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowser.ui
index 959df59cc..492ab119f 100644
--- a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowser.ui
+++ b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowser.ui
@@ -7,7 +7,7 @@
     <x>0</x>
     <y>0</y>
     <width>786</width>
-    <height>300</height>
+    <height>462</height>
    </rect>
   </property>
   <property name="windowTitle">
@@ -220,7 +220,63 @@
     </layout>
    </item>
    <item>
-    <widget class="QTreeView" name="treeView"/>
+    <widget class="QWidget" name="widget" native="true">
+     <layout class="QVBoxLayout" name="verticalLayout_3">
+      <property name="leftMargin">
+       <number>0</number>
+      </property>
+      <property name="topMargin">
+       <number>0</number>
+      </property>
+      <property name="rightMargin">
+       <number>0</number>
+      </property>
+      <property name="bottomMargin">
+       <number>0</number>
+      </property>
+      <item>
+       <widget class="QSplitter" name="splitter">
+        <property name="lineWidth">
+         <number>0</number>
+        </property>
+        <property name="orientation">
+         <enum>Qt::Vertical</enum>
+        </property>
+        <widget class="QTreeView" name="treeView">
+         <property name="sizePolicy">
+          <sizepolicy hsizetype="Expanding" vsizetype="Expanding">
+           <horstretch>0</horstretch>
+           <verstretch>0</verstretch>
+          </sizepolicy>
+         </property>
+        </widget>
+        <widget class="QTextEdit" name="descriptionText">
+         <property name="sizePolicy">
+          <sizepolicy hsizetype="Expanding" vsizetype="Expanding">
+           <horstretch>0</horstretch>
+           <verstretch>0</verstretch>
+          </sizepolicy>
+         </property>
+         <property name="minimumSize">
+          <size>
+           <width>0</width>
+           <height>150</height>
+          </size>
+         </property>
+         <property name="horizontalScrollBarPolicy">
+          <enum>Qt::ScrollBarAlwaysOff</enum>
+         </property>
+         <property name="textInteractionFlags">
+          <set>Qt::TextSelectableByKeyboard|Qt::TextSelectableByMouse</set>
+         </property>
+         <property name="placeholderText">
+          <string>This space shows a description of the selected UAVObject.</string>
+         </property>
+        </widget>
+       </widget>
+      </item>
+     </layout>
+    </widget>
    </item>
   </layout>
  </widget>
diff --git a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserconfiguration.cpp b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserconfiguration.cpp
index f249d496a..54a756322 100644
--- a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserconfiguration.cpp
+++ b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserconfiguration.cpp
@@ -35,22 +35,20 @@ UAVObjectBrowserConfiguration::UAVObjectBrowserConfiguration(QString classId, QS
     m_recentlyUpdatedTimeout(500),
     m_useCategorizedView(false),
     m_useScientificView(false),
-    m_showMetaData(false)
+    m_showMetaData(false),
+    m_showDescription(false)
 {
     // if a saved configuration exists load it
     if (qSettings != 0) {
-        QColor recent = qSettings->value("recentlyUpdatedColor").value<QColor>();
-        QColor manual = qSettings->value("manuallyChangedColor").value<QColor>();
-        int timeout   = qSettings->value("recentlyUpdatedTimeout").toInt();
-        bool hilight  = qSettings->value("onlyHilightChangedValues").toBool();
-
         m_useCategorizedView       = qSettings->value("CategorizedView").toBool();
         m_useScientificView        = qSettings->value("ScientificView").toBool();
         m_showMetaData = qSettings->value("showMetaData").toBool();
-        m_recentlyUpdatedColor     = recent;
-        m_manuallyChangedColor     = manual;
-        m_recentlyUpdatedTimeout   = timeout;
-        m_onlyHilightChangedValues = hilight;
+        m_showDescription          = qSettings->value("showDescription").toBool();
+        m_splitterState = qSettings->value("splitterState").toByteArray();
+        m_recentlyUpdatedColor     = qSettings->value("recentlyUpdatedColor").value<QColor>();
+        m_manuallyChangedColor     = qSettings->value("manuallyChangedColor").value<QColor>();
+        m_recentlyUpdatedTimeout   = qSettings->value("recentlyUpdatedTimeout").toInt();
+        m_onlyHilightChangedValues = qSettings->value("onlyHilightChangedValues").toBool();
     }
 }
 
@@ -64,7 +62,9 @@ IUAVGadgetConfiguration *UAVObjectBrowserConfiguration::clone()
     m->m_onlyHilightChangedValues = m_onlyHilightChangedValues;
     m->m_useCategorizedView = m_useCategorizedView;
     m->m_useScientificView  = m_useScientificView;
+    m->m_splitterState = m_splitterState;
     m->m_showMetaData = m_showMetaData;
+    m->m_showDescription    = m_showDescription;
     return m;
 }
 
@@ -81,4 +81,6 @@ void UAVObjectBrowserConfiguration::saveConfig(QSettings *qSettings) const
     qSettings->setValue("CategorizedView", m_useCategorizedView);
     qSettings->setValue("ScientificView", m_useScientificView);
     qSettings->setValue("showMetaData", m_showMetaData);
+    qSettings->setValue("showDescription", m_showDescription);
+    qSettings->setValue("splitterState", m_splitterState);
 }
diff --git a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserconfiguration.h b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserconfiguration.h
index ff6237e7c..825e6aa06 100644
--- a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserconfiguration.h
+++ b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserconfiguration.h
@@ -40,7 +40,9 @@ class UAVObjectBrowserConfiguration : public IUAVGadgetConfiguration {
     Q_PROPERTY(bool m_onlyHilightChangedValues READ onlyHighlightChangedValues WRITE setOnlyHighlightChangedValues)
     Q_PROPERTY(bool m_useCategorizedView READ categorizedView WRITE setCategorizedView)
     Q_PROPERTY(bool m_useScientificView READ scientificView WRITE setScientificView)
+    Q_PROPERTY(bool m_showDescription READ showDescription WRITE setShowDescription)
     Q_PROPERTY(bool m_showMetaData READ showMetaData WRITE setShowMetaData)
+    Q_PROPERTY(QByteArray m_splitterState READ splitterState WRITE setSplitterState)
 public:
     explicit UAVObjectBrowserConfiguration(QString classId, QSettings *qSettings = 0, QObject *parent = 0);
 
@@ -75,6 +77,15 @@ public:
     {
         return m_showMetaData;
     }
+    bool showDescription() const
+    {
+        return m_showDescription;
+    }
+
+    QByteArray splitterState() const
+    {
+        return m_splitterState;
+    }
 
 signals:
 
@@ -107,6 +118,15 @@ public slots:
     {
         m_showMetaData = value;
     }
+    void setShowDescription(bool value)
+    {
+        m_showDescription = value;
+    }
+
+    void setSplitterState(QByteArray arg)
+    {
+        m_splitterState = arg;
+    }
 
 private:
     QColor m_recentlyUpdatedColor;
@@ -116,6 +136,8 @@ private:
     bool m_useCategorizedView;
     bool m_useScientificView;
     bool m_showMetaData;
+    bool m_showDescription;
+    QByteArray m_splitterState;
 };
 
 #endif // UAVOBJECTBROWSERCONFIGURATION_H
diff --git a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserwidget.cpp b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserwidget.cpp
index fd900591c..2c5456cc1 100644
--- a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserwidget.cpp
+++ b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserwidget.cpp
@@ -39,6 +39,7 @@
 #include <QtCore/QDebug>
 #include <QItemEditorFactory>
 #include "extensionsystem/pluginmanager.h"
+#include "utils/mustache.h"
 
 UAVObjectBrowserWidget::UAVObjectBrowserWidget(QWidget *parent) : QWidget(parent)
 {
@@ -50,15 +51,19 @@ UAVObjectBrowserWidget::UAVObjectBrowserWidget(QWidget *parent) : QWidget(parent
     m_model = new UAVObjectTreeModel();
     m_browser->treeView->setModel(m_model);
     m_browser->treeView->setColumnWidth(0, 300);
-    // m_browser->treeView->expandAll();
     BrowserItemDelegate *m_delegate = new BrowserItemDelegate();
     m_browser->treeView->setItemDelegate(m_delegate);
     m_browser->treeView->setEditTriggers(QAbstractItemView::AllEditTriggers);
     m_browser->treeView->setSelectionBehavior(QAbstractItemView::SelectItems);
+    m_browser->splitter->setChildrenCollapsible(false);
+    m_mustacheTemplate = loadFileIntoString(QString(":/uavobjectbrowser/resources/uavodescription.mustache"));
     showMetaData(m_viewoptions->cbMetaData->isChecked());
-    connect(m_browser->treeView->selectionModel(), SIGNAL(currentChanged(QModelIndex, QModelIndex)), this, SLOT(currentChanged(QModelIndex, QModelIndex)), Qt::UniqueConnection);
+    showDescription(m_viewoptions->cbDescription->isChecked());
+    connect(m_browser->treeView->selectionModel(), SIGNAL(currentChanged(QModelIndex, QModelIndex)),
+            this, SLOT(currentChanged(QModelIndex, QModelIndex)), Qt::UniqueConnection);
     connect(m_viewoptions->cbMetaData, SIGNAL(toggled(bool)), this, SLOT(showMetaData(bool)));
     connect(m_viewoptions->cbCategorized, SIGNAL(toggled(bool)), this, SLOT(categorize(bool)));
+    connect(m_viewoptions->cbDescription, SIGNAL(toggled(bool)), this, SLOT(showDescription(bool)));
     connect(m_browser->saveSDButton, SIGNAL(clicked()), this, SLOT(saveObject()));
     connect(m_browser->readSDButton, SIGNAL(clicked()), this, SLOT(loadObject()));
     connect(m_browser->eraseSDButton, SIGNAL(clicked()), this, SLOT(eraseObject()));
@@ -69,6 +74,8 @@ UAVObjectBrowserWidget::UAVObjectBrowserWidget(QWidget *parent) : QWidget(parent
     connect(m_viewoptions->cbScientific, SIGNAL(toggled(bool)), this, SLOT(viewOptionsChangedSlot()));
     connect(m_viewoptions->cbMetaData, SIGNAL(toggled(bool)), this, SLOT(viewOptionsChangedSlot()));
     connect(m_viewoptions->cbCategorized, SIGNAL(toggled(bool)), this, SLOT(viewOptionsChangedSlot()));
+    connect(m_viewoptions->cbDescription, SIGNAL(toggled(bool)), this, SLOT(viewOptionsChangedSlot()));
+    connect(m_browser->splitter, SIGNAL(splitterMoved(int, int)), this, SLOT(splitterMoved()));
     enableSendRequest(false);
 }
 
@@ -77,11 +84,17 @@ UAVObjectBrowserWidget::~UAVObjectBrowserWidget()
     delete m_browser;
 }
 
-void UAVObjectBrowserWidget::setViewOptions(bool categorized, bool scientific, bool metadata)
+void UAVObjectBrowserWidget::setViewOptions(bool categorized, bool scientific, bool metadata, bool description)
 {
     m_viewoptions->cbCategorized->setChecked(categorized);
     m_viewoptions->cbMetaData->setChecked(metadata);
     m_viewoptions->cbScientific->setChecked(scientific);
+    m_viewoptions->cbDescription->setChecked(description);
+}
+
+void UAVObjectBrowserWidget::setSplitterState(QByteArray state)
+{
+    m_browser->splitter->restoreState(state);
 }
 
 void UAVObjectBrowserWidget::showMetaData(bool show)
@@ -92,6 +105,11 @@ void UAVObjectBrowserWidget::showMetaData(bool show)
     }
 }
 
+void UAVObjectBrowserWidget::showDescription(bool show)
+{
+    m_browser->descriptionText->setVisible(show);
+}
+
 void UAVObjectBrowserWidget::categorize(bool categorize)
 {
     UAVObjectTreeModel *tmpModel = m_model;
@@ -160,6 +178,17 @@ ObjectTreeItem *UAVObjectBrowserWidget::findCurrentObjectTreeItem()
     return objItem;
 }
 
+QString UAVObjectBrowserWidget::loadFileIntoString(QString fileName)
+{
+    QFile file(fileName);
+
+    file.open(QIODevice::ReadOnly);
+    QTextStream stream(&file);
+    QString line = stream.readAll();
+    file.close();
+    return line;
+}
+
 void UAVObjectBrowserWidget::saveObject()
 {
     this->setFocus();
@@ -228,6 +257,7 @@ void UAVObjectBrowserWidget::currentChanged(const QModelIndex &current, const QM
         enable = false;
     }
     enableSendRequest(enable);
+    updateDescription();
 }
 
 void UAVObjectBrowserWidget::viewSlot()
@@ -244,7 +274,94 @@ void UAVObjectBrowserWidget::viewSlot()
 
 void UAVObjectBrowserWidget::viewOptionsChangedSlot()
 {
-    emit viewOptionsChanged(m_viewoptions->cbCategorized->isChecked(), m_viewoptions->cbScientific->isChecked(), m_viewoptions->cbMetaData->isChecked());
+    emit viewOptionsChanged(m_viewoptions->cbCategorized->isChecked(), m_viewoptions->cbScientific->isChecked(),
+                            m_viewoptions->cbMetaData->isChecked(), m_viewoptions->cbDescription->isChecked());
+}
+
+void UAVObjectBrowserWidget::splitterMoved()
+{
+    emit splitterChanged(m_browser->splitter->saveState());
+}
+
+QString UAVObjectBrowserWidget::createObjectDescription(UAVObject *object)
+{
+    QString mustache(m_mustacheTemplate);
+
+    QVariantHash uavoHash;
+
+    uavoHash["OBJECT_NAME_TITLE"] = tr("Name");
+    uavoHash["OBJECT_NAME"] = object->getName();
+    uavoHash["CATEGORY_TITLE"]    = tr("Category");
+    uavoHash["CATEGORY"]          = object->getCategory();
+    uavoHash["TYPE_TITLE"]        = tr("Type");
+    uavoHash["TYPE"] = object->isMetaDataObject() ? tr("Metadata") : object->isSettingsObject() ? tr("Setting") : tr("Data");
+    uavoHash["SIZE_TITLE"]        = tr("Size");
+    uavoHash["SIZE"] = object->getNumBytes();
+    uavoHash["DESCRIPTION_TITLE"] = tr("Description");
+    uavoHash["DESCRIPTION"]       = object->getDescription().replace("@ref", "");
+    uavoHash["MULTI_INSTANCE_TITLE"] = tr("Multi");
+    uavoHash["MULTI_INSTANCE"]    = object->isSingleInstance() ? tr("No") : tr("Yes");
+    uavoHash["FIELDS_NAME_TITLE"] = tr("Fields");
+    QVariantList fields;
+    foreach(UAVObjectField * field, object->getFields()) {
+        QVariantHash fieldHash;
+
+        fieldHash["FIELD_NAME_TITLE"] = tr("Name");
+        fieldHash["FIELD_NAME"] = field->getName();
+        fieldHash["FIELD_TYPE_TITLE"] = tr("Type");
+        fieldHash["FIELD_TYPE"] = QString("%1%2").arg(field->getTypeAsString(),
+                                                      (field->getNumElements() > 1 ? QString("[%1]").arg(field->getNumElements()) : QString()));
+        if (!field->getUnits().isEmpty()) {
+            fieldHash["FIELD_UNIT_TITLE"] = tr("Unit");
+            fieldHash["FIELD_UNIT"] = field->getUnits();
+        }
+        if (!field->getOptions().isEmpty()) {
+            fieldHash["FIELD_OPTIONS_TITLE"] = tr("Options");
+            QVariantList options;
+            foreach(QString option, field->getOptions()) {
+                QVariantHash optionHash;
+
+                optionHash["FIELD_OPTION"] = option;
+                if (!options.isEmpty()) {
+                    optionHash["FIELD_OPTION_DELIM"] = ", ";
+                }
+                options.append(optionHash);
+            }
+            fieldHash["FIELD_OPTIONS"] = options;
+        }
+        if (field->getElementNames().count() > 1) {
+            fieldHash["FIELD_ELEMENTS_TITLE"] = tr("Elements");
+            QVariantList elements;
+            for (int i = 0; i < field->getElementNames().count(); i++) {
+                QString element = field->getElementNames().at(i);
+                QVariantHash elementHash;
+                elementHash["FIELD_ELEMENT"] = element;
+                QString limitsString = field->getLimitsAsString(i);
+                if (!limitsString.isEmpty()) {
+                    elementHash["FIELD_ELEMENT_LIMIT"] = limitsString.prepend(" (").append(")");
+                }
+                if (!elements.isEmpty()) {
+                    elementHash["FIELD_ELEMENT_DELIM"] = ", ";
+                }
+                elements.append(elementHash);
+            }
+            fieldHash["FIELD_ELEMENTS"] = elements;
+        } else if (!field->getLimitsAsString(0).isEmpty()) {
+            fieldHash["FIELD_LIMIT_TITLE"] = tr("Limits");
+            fieldHash["FIELD_LIMIT"] = field->getLimitsAsString(0);
+        }
+
+        if (!field->getDescription().isEmpty()) {
+            fieldHash["FIELD_DESCRIPTION_TITLE"] = tr("Description");
+            fieldHash["FIELD_DESCRIPTION"] = field->getDescription();
+        }
+
+        fields.append(fieldHash);
+    }
+    uavoHash["FIELDS"] = fields;
+    Mustache::QtVariantContext context(uavoHash);
+    Mustache::Renderer renderer;
+    return renderer.render(mustache, &context);
 }
 
 void UAVObjectBrowserWidget::enableSendRequest(bool enable)
@@ -255,3 +372,17 @@ void UAVObjectBrowserWidget::enableSendRequest(bool enable)
     m_browser->readSDButton->setEnabled(enable);
     m_browser->eraseSDButton->setEnabled(enable);
 }
+
+void UAVObjectBrowserWidget::updateDescription()
+{
+    ObjectTreeItem *objItem = findCurrentObjectTreeItem();
+
+    if (objItem) {
+        UAVObject *obj = objItem->object();
+        if (obj) {
+            m_browser->descriptionText->setText(createObjectDescription(obj));
+            return;
+        }
+    }
+    m_browser->descriptionText->setText("");
+}
diff --git a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserwidget.h b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserwidget.h
index dfc3bb394..8843d0c29 100644
--- a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserwidget.h
+++ b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserwidget.h
@@ -60,9 +60,11 @@ public:
     {
         m_onlyHilightChangedValues = hilight; m_model->setOnlyHilightChangedValues(hilight);
     }
-    void setViewOptions(bool categorized, bool scientific, bool metadata);
+    void setViewOptions(bool categorized, bool scientific, bool metadata, bool description);
+    void setSplitterState(QByteArray state);
 public slots:
     void showMetaData(bool show);
+    void showDescription(bool show);
     void categorize(bool categorize);
     void useScientificNotation(bool scientific);
 
@@ -75,8 +77,11 @@ private slots:
     void currentChanged(const QModelIndex &current, const QModelIndex &previous);
     void viewSlot();
     void viewOptionsChangedSlot();
+    void splitterMoved();
+    QString createObjectDescription(UAVObject *object);
 signals:
-    void viewOptionsChanged(bool categorized, bool scientific, bool metadata);
+    void viewOptionsChanged(bool categorized, bool scientific, bool metadata, bool description);
+    void splitterChanged(QByteArray state);
 private:
     QPushButton *m_requestUpdate;
     QPushButton *m_sendUpdate;
@@ -89,10 +94,13 @@ private:
     QColor m_recentlyUpdatedColor;
     QColor m_manuallyChangedColor;
     bool m_onlyHilightChangedValues;
+    QString m_mustacheTemplate;
 
     void updateObjectPersistance(ObjectPersistence::OperationOptions op, UAVObject *obj);
     void enableSendRequest(bool enable);
+    void updateDescription();
     ObjectTreeItem *findCurrentObjectTreeItem();
+    QString loadFileIntoString(QString fileName);
 };
 
 #endif /* UAVOBJECTBROWSERWIDGET_H_ */
diff --git a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjecttreemodel.cpp b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjecttreemodel.cpp
index ca57295a8..bd456d145 100644
--- a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjecttreemodel.cpp
+++ b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjecttreemodel.cpp
@@ -245,6 +245,7 @@ void UAVObjectTreeModel::addSingleField(int index, UAVObjectField *field, TreeIt
         Q_ASSERT(false);
     }
     item->setHighlightManager(m_highlightManager);
+    item->setDescription(field->getDescription());
     connect(item, SIGNAL(updateHighlight(TreeItem *)), this, SLOT(updateHighlight(TreeItem *)));
     parent->appendChild(item);
 }
diff --git a/ground/openpilotgcs/src/plugins/uavobjectbrowser/viewoptions.ui b/ground/openpilotgcs/src/plugins/uavobjectbrowser/viewoptions.ui
index bc2c1d815..3fcf887fd 100644
--- a/ground/openpilotgcs/src/plugins/uavobjectbrowser/viewoptions.ui
+++ b/ground/openpilotgcs/src/plugins/uavobjectbrowser/viewoptions.ui
@@ -6,8 +6,8 @@
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>169</width>
-    <height>96</height>
+    <width>172</width>
+    <height>124</height>
    </rect>
   </property>
   <property name="windowTitle">
@@ -35,6 +35,13 @@
      </property>
     </widget>
    </item>
+   <item>
+    <widget class="QCheckBox" name="cbDescription">
+     <property name="text">
+      <string>Show description</string>
+     </property>
+    </widget>
+   </item>
   </layout>
  </widget>
  <resources/>
diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavmetaobject.cpp b/ground/openpilotgcs/src/plugins/uavobjects/uavmetaobject.cpp
index 5c7d2755d..985eaffe2 100644
--- a/ground/openpilotgcs/src/plugins/uavobjects/uavmetaobject.cpp
+++ b/ground/openpilotgcs/src/plugins/uavobjects/uavmetaobject.cpp
@@ -41,10 +41,10 @@ UAVMetaObject::UAVMetaObject(quint32 objID, const QString & name, UAVObject *par
     QStringList modesBitField;
     modesBitField << tr("FlightReadOnly") << tr("GCSReadOnly") << tr("FlightTelemetryAcked") << tr("GCSTelemetryAcked") << tr("FlightUpdatePeriodic") << tr("FlightUpdateOnChange") << tr("GCSUpdatePeriodic") << tr("GCSUpdateOnChange") << tr("LoggingUpdatePeriodic") << tr("LoggingUpdateOnChange");
     QList<UAVObjectField *> fields;
-    fields.append(new UAVObjectField(tr("Modes"), tr("boolean"), UAVObjectField::BITFIELD, modesBitField, QStringList()));
-    fields.append(new UAVObjectField(tr("Flight Telemetry Update Period"), tr("ms"), UAVObjectField::UINT16, 1, QStringList()));
-    fields.append(new UAVObjectField(tr("GCS Telemetry Update Period"), tr("ms"), UAVObjectField::UINT16, 1, QStringList()));
-    fields.append(new UAVObjectField(tr("Logging Update Period"), tr("ms"), UAVObjectField::UINT16, 1, QStringList()));
+    fields.append(new UAVObjectField(tr("Modes"), tr("Metadata modes"), tr("boolean"), UAVObjectField::BITFIELD, modesBitField, QStringList()));
+    fields.append(new UAVObjectField(tr("Flight Telemetry Update Period"), tr("This is how often flight side will update telemetry data"), tr("ms"), UAVObjectField::UINT16, 1, QStringList()));
+    fields.append(new UAVObjectField(tr("GCS Telemetry Update Period"), tr("This is how often GCS will update telemetry data"), tr("ms"), UAVObjectField::UINT16, 1, QStringList()));
+    fields.append(new UAVObjectField(tr("Logging Update Period"), tr("This is how often logging will be updated."), tr("ms"), UAVObjectField::UINT16, 1, QStringList()));
     // Initialize parent
     UAVObject::initialize(0);
     UAVObject::initializeFields(fields, (quint8 *)&parentMetadata, sizeof(Metadata));
diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjectfield.cpp b/ground/openpilotgcs/src/plugins/uavobjects/uavobjectfield.cpp
index 76ab6296c..14309f8e6 100644
--- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjectfield.cpp
+++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjectfield.cpp
@@ -30,7 +30,7 @@
 #include <QDebug>
 #include <QtWidgets>
 
-UAVObjectField::UAVObjectField(const QString & name, const QString & units, FieldType type, quint32 numElements, const QStringList & options, const QString &limits)
+UAVObjectField::UAVObjectField(const QString & name, const QString & description, const QString & units, FieldType type, quint32 numElements, const QStringList & options, const QString &limits)
 {
     QStringList elementNames;
 
@@ -39,18 +39,19 @@ UAVObjectField::UAVObjectField(const QString & name, const QString & units, Fiel
         elementNames.append(QString("%1").arg(n));
     }
     // Initialize
-    constructorInitialize(name, units, type, elementNames, options, limits);
+    constructorInitialize(name, description, units, type, elementNames, options, limits);
 }
 
-UAVObjectField::UAVObjectField(const QString & name, const QString & units, FieldType type, const QStringList & elementNames, const QStringList & options, const QString &limits)
+UAVObjectField::UAVObjectField(const QString & name, const QString & description, const QString & units, FieldType type, const QStringList & elementNames, const QStringList & options, const QString &limits)
 {
-    constructorInitialize(name, units, type, elementNames, options, limits);
+    constructorInitialize(name, description, units, type, elementNames, options, limits);
 }
 
-void UAVObjectField::constructorInitialize(const QString & name, const QString & units, FieldType type, const QStringList & elementNames, const QStringList & options, const QString &limits)
+void UAVObjectField::constructorInitialize(const QString & name, const QString & description, const QString & units, FieldType type, const QStringList & elementNames, const QStringList & options, const QString &limits)
 {
     // Copy params
     this->name         = name;
+    this->description  = description;
     this->units        = units;
     this->type         = type;
     this->options      = options;
@@ -458,11 +459,65 @@ bool UAVObjectField::isWithinLimits(QVariant var, quint32 index, int board)
             default:
                 return true;
             }
+        default:
+            return true;
         }
     }
     return true;
 }
 
+QString UAVObjectField::getLimitsAsString(quint32 index, int board)
+{
+    QString limitString;
+
+    if (elementLimits.keys().contains(index)) {
+        foreach(LimitStruct struc, elementLimits.value(index)) {
+            if ((struc.board != board) && board != 0 && struc.board != 0) {
+                continue;
+            }
+            switch (struc.type) {
+            case EQUAL:
+            {
+                limitString.append(tr("one of")).append(" [");
+                bool first = true;
+                foreach(QVariant var, struc.values) {
+                    if (!first) {
+                        limitString.append(", ");
+                    }
+                    limitString.append(var.toString());
+                    first = false;
+                }
+                return limitString.append("]");
+            }
+            case NOT_EQUAL:
+            {
+                limitString.append(tr("none of")).append(" [");
+                bool first = true;
+                foreach(QVariant var, struc.values) {
+                    if (!first) {
+                        limitString.append(", ");
+                    }
+                    limitString.append(var.toString());
+                    first = false;
+                }
+                return limitString.append("]");
+            }
+            case BIGGER: return limitString.append(QString("%1 %2").arg(tr("more than"), struc.values.at(0).toString()));
+
+            case BETWEEN: return limitString.append(QString("%1 %2 %3 %4")
+                                                    .arg(tr("between"), struc.values.at(0).toString(),
+                                                         tr(" and "), struc.values.at(1).toString()));
+
+            case SMALLER: return limitString.append(QString("%1 %2").arg(tr("less than"), struc.values.at(0).toString()));
+
+            default:
+                break;
+            }
+        }
+    }
+    return limitString;
+}
+
 QVariant UAVObjectField::getMaxLimit(quint32 index, int board)
 {
     if (!elementLimits.keys().contains(index)) {
@@ -478,20 +533,14 @@ QVariant UAVObjectField::getMaxLimit(quint32 index, int board)
         case BIGGER:
             return QVariant();
 
-            break;
-            break;
         case BETWEEN:
             return struc.values.at(1);
 
-            break;
         case SMALLER:
             return struc.values.at(0);
 
-            break;
         default:
             return QVariant();
-
-            break;
         }
     }
     return QVariant();
@@ -511,20 +560,14 @@ QVariant UAVObjectField::getMinLimit(quint32 index, int board)
         case SMALLER:
             return QVariant();
 
-            break;
-            break;
         case BETWEEN:
             return struc.values.at(0);
 
-            break;
         case BIGGER:
             return struc.values.at(0);
 
-            break;
         default:
             return QVariant();
-
-            break;
         }
     }
     return QVariant();
@@ -609,6 +652,11 @@ QString UAVObjectField::getName()
     return name;
 }
 
+QString UAVObjectField::getDescription()
+{
+    return description;
+}
+
 QString UAVObjectField::getUnits()
 {
     return units;
diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjectfield.h b/ground/openpilotgcs/src/plugins/uavobjects/uavobjectfield.h
index 6142bc923..6794c7308 100644
--- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjectfield.h
+++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjectfield.h
@@ -45,20 +45,21 @@ class UAVOBJECTS_EXPORT UAVObjectField : public QObject {
 
 public:
     typedef enum { INT8 = 0, INT16, INT32, UINT8, UINT16, UINT32, FLOAT32, ENUM, BITFIELD, STRING } FieldType;
-    typedef enum { EQUAL, NOT_EQUAL, BETWEEN, BIGGER, SMALLER } LimitType;
+    typedef enum { EQUAL, NOT_EQUAL, BETWEEN, BIGGER, SMALLER, UNDEFINED } LimitType;
     typedef struct {
         LimitType type;
         QList<QVariant> values;
         int board;
     } LimitStruct;
 
-    UAVObjectField(const QString & name, const QString & units, FieldType type, quint32 numElements, const QStringList & options, const QString & limits = QString());
-    UAVObjectField(const QString & name, const QString & units, FieldType type, const QStringList & elementNames, const QStringList & options, const QString & limits = QString());
+    UAVObjectField(const QString & name, const QString & description, const QString & units, FieldType type, quint32 numElements, const QStringList & options, const QString & limits = QString());
+    UAVObjectField(const QString & name, const QString & description, const QString & units, FieldType type, const QStringList & elementNames, const QStringList & options, const QString & limits = QString());
     void initialize(quint8 *data, quint32 dataOffset, UAVObject *obj);
     UAVObject *getObject();
     FieldType getType();
     QString getTypeAsString();
     QString getName();
+    QString getDescription();
     QString getUnits();
     quint32 getNumElements();
     QStringList getElementNames();
@@ -84,6 +85,7 @@ public:
     void fromJson(const QJsonObject &jsonObject);
 
     bool isWithinLimits(QVariant var, quint32 index, int board = 0);
+    QString getLimitsAsString(quint32 index, int board = 0);
     QVariant getMaxLimit(quint32 index, int board = 0);
     QVariant getMinLimit(quint32 index, int board = 0);
 signals:
@@ -91,6 +93,7 @@ signals:
 
 protected:
     QString name;
+    QString description;
     QString units;
     FieldType type;
     QStringList elementNames;
@@ -102,7 +105,7 @@ protected:
     UAVObject *obj;
     QMap<quint32, QList<LimitStruct> > elementLimits;
     void clear();
-    void constructorInitialize(const QString & name, const QString & units, FieldType type, const QStringList & elementNames, const QStringList & options, const QString &limits);
+    void constructorInitialize(const QString & name, const QString & description, const QString & units, FieldType type, const QStringList & elementNames, const QStringList & options, const QString &limits);
     void limitsInitialize(const QString &limits);
 };
 
diff --git a/ground/openpilotgcs/src/plugins/welcome/qml/ScrollDecorator.qml b/ground/openpilotgcs/src/plugins/welcome/qml/ScrollDecorator.qml
index 5e4e7e8d9..b3bdb0313 100644
--- a/ground/openpilotgcs/src/plugins/welcome/qml/ScrollDecorator.qml
+++ b/ground/openpilotgcs/src/plugins/welcome/qml/ScrollDecorator.qml
@@ -10,8 +10,6 @@ Rectangle {
         sourceComponent: scrollDecorator.flickableItem ? scrollBar : undefined
     }
 
-    Component.onDestruction: scrollLoader.sourceComponent = undefined
-
     Component {
         id: scrollBar
         Rectangle {
diff --git a/ground/uavobjgenerator/generators/gcs/uavobjectgeneratorgcs.cpp b/ground/uavobjgenerator/generators/gcs/uavobjectgeneratorgcs.cpp
index 7265d411a..1279e8e4f 100644
--- a/ground/uavobjgenerator/generators/gcs/uavobjectgeneratorgcs.cpp
+++ b/ground/uavobjgenerator/generators/gcs/uavobjectgeneratorgcs.cpp
@@ -259,8 +259,9 @@ bool UAVObjectGeneratorGCS::process_object(ObjectInfo *info)
                              .arg(varOptionName)
                              .arg(options[m]));
             }
-            finit.append(QString("    fields.append( new UAVObjectField(QString(\"%1\"), QString(\"%2\"), UAVObjectField::ENUM, %3, %4, QString(\"%5\")));\n")
+            finit.append(QString("    fields.append( new UAVObjectField(QString(\"%1\"), tr(\"%2\"), QString(\"%3\"), UAVObjectField::ENUM, %4, %5, QString(\"%6\")));\n")
                          .arg(info->fields[n]->name)
+                         .arg(info->fields[n]->description)
                          .arg(info->fields[n]->units)
                          .arg(varElemName)
                          .arg(varOptionName)
@@ -268,8 +269,9 @@ bool UAVObjectGeneratorGCS::process_object(ObjectInfo *info)
         }
         // For all other types
         else {
-            finit.append(QString("    fields.append( new UAVObjectField(QString(\"%1\"), QString(\"%2\"), UAVObjectField::%3, %4, QStringList(), QString(\"%5\")));\n")
+            finit.append(QString("    fields.append( new UAVObjectField(QString(\"%1\"), tr(\"%2\"), QString(\"%3\"), UAVObjectField::%4, %5, QStringList(), QString(\"%6\")));\n")
                          .arg(info->fields[n]->name)
+                         .arg(info->fields[n]->description)
                          .arg(info->fields[n]->units)
                          .arg(fieldTypeStrCPPClass[info->fields[n]->type])
                          .arg(varElemName)
diff --git a/ground/uavobjgenerator/uavobjectparser.cpp b/ground/uavobjgenerator/uavobjectparser.cpp
index a8534088c..dde870df4 100644
--- a/ground/uavobjgenerator/uavobjectparser.cpp
+++ b/ground/uavobjgenerator/uavobjectparser.cpp
@@ -25,7 +25,7 @@
  */
 
 #include "uavobjectparser.h"
-
+#include <QDebug>
 /**
  * Constructor
  */
@@ -441,6 +441,21 @@ QString UAVObjectParser::processObjectFields(QDomNode & childNode, ObjectInfo *i
         field->name = name;
     }
 
+    // Get description attribute if any
+    elemAttr = elemAttributes.namedItem("description");
+    if (!elemAttr.isNull()) {
+        field->description = elemAttr.nodeValue();
+    } else {
+        // Look for a child description node
+        QDomNode node = childNode.firstChildElement("description");
+        if (!node.isNull()) {
+            QDomNode description = node.firstChild();
+            if (!description.isNull()) {
+                field->description = description.nodeValue();
+            }
+        }
+    }
+
     // Get units attribute
     elemAttr = elemAttributes.namedItem("units");
     if (elemAttr.isNull()) {
diff --git a/ground/uavobjgenerator/uavobjectparser.h b/ground/uavobjgenerator/uavobjectparser.h
index 9028af325..ccc537cdf 100644
--- a/ground/uavobjgenerator/uavobjectparser.h
+++ b/ground/uavobjgenerator/uavobjectparser.h
@@ -49,6 +49,7 @@ typedef enum {
 
 typedef struct {
     QString     name;
+    QString     description;
     QString     units;
     FieldType   type;
     int numElements;
diff --git a/shared/uavobjectdefinition/camerastabsettings.xml b/shared/uavobjectdefinition/camerastabsettings.xml
index 0a017dff8..c0b27c647 100644
--- a/shared/uavobjectdefinition/camerastabsettings.xml
+++ b/shared/uavobjectdefinition/camerastabsettings.xml
@@ -1,6 +1,6 @@
 <xml>
     <object name="CameraStabSettings" singleinstance="true" settings="true" category="Control">
-        <description>Settings for the @ref CameraStab mmodule</description>
+        <description>Settings for the @ref CameraStab module</description>
         <field name="Input" units="channel" type="enum" elementnames="Roll,Pitch,Yaw" options="Accessory0,Accessory1,Accessory2,Accessory3,Accessory4,Accessory5,None" defaultvalue="None"/>
         <field name="InputRange" units="deg" type="uint8" elementnames="Roll,Pitch,Yaw" defaultvalue="20"/>
         <field name="InputRate" units="deg/s" type="uint8" elementnames="Roll,Pitch,Yaw" defaultvalue="50"/>
diff --git a/shared/uavobjectdefinition/debuglogsettings.xml b/shared/uavobjectdefinition/debuglogsettings.xml
index cb5fef6ed..2a97b04cb 100644
--- a/shared/uavobjectdefinition/debuglogsettings.xml
+++ b/shared/uavobjectdefinition/debuglogsettings.xml
@@ -1,7 +1,10 @@
 <xml>
     <object name="DebugLogSettings" singleinstance="true" settings="true" category="System">
         <description>Configure On Board Logging Facilities</description>
-        <field name="LoggingEnabled" units="" type="enum" elements="1" options="Disabled,OnlyWhenArmed,Always" defaultvalue="Disabled" />
+
+        <field name="LoggingEnabled" units="" type="enum" elements="1" options="Disabled,OnlyWhenArmed,Always" defaultvalue="Disabled">
+            <description>If set to OnlyWhenArmed logs will only be saved when craft is armed. Disabled turns logging off, and Always will always log.</description>
+        </field>
 
         <access gcs="readwrite" flight="readwrite"/>
         <telemetrygcs acked="true" updatemode="onchange" period="0"/>
diff --git a/shared/uavobjectdefinition/debuglogstatus.xml b/shared/uavobjectdefinition/debuglogstatus.xml
index 0d510a332..b1183d684 100644
--- a/shared/uavobjectdefinition/debuglogstatus.xml
+++ b/shared/uavobjectdefinition/debuglogstatus.xml
@@ -1,10 +1,10 @@
 <xml>
     <object name="DebugLogStatus" singleinstance="true" settings="false" category="System">
         <description>Log Status Object, contains log partition status information</description>
-	<field name="Flight" units="" type="uint16" elements="1" />
-	<field name="Entry" units="" type="uint16" elements="1" />
-	<field name="UsedSlots" units="" type="uint16" elements="1" />
-	<field name="FreeSlots" units="" type="uint16" elements="1" />
+        <field name="Flight" units="" type="uint16" elements="1" description="The current flight number (logging session)"/>
+        <field name="Entry" units="" type="uint16" elements="1" description="The current log entry id"/>
+        <field name="UsedSlots" units="" type="uint16" elements="1" description="Holds the total log entries saved"/>
+        <field name="FreeSlots" units="" type="uint16" elements="1" description="The number of free log slots available"/>
         <access gcs="readwrite" flight="readwrite"/>
         <telemetrygcs acked="false" updatemode="manual" period="0"/>
         <telemetryflight acked="false" updatemode="periodic" period="1000"/>
diff --git a/shared/uavobjectdefinition/homelocation.xml b/shared/uavobjectdefinition/homelocation.xml
index 3f99eb24b..64527c7e8 100644
--- a/shared/uavobjectdefinition/homelocation.xml
+++ b/shared/uavobjectdefinition/homelocation.xml
@@ -1,6 +1,6 @@
 <xml>
     <object name="HomeLocation" singleinstance="true" settings="true" category="Navigation">
-        <description>HomeLocation setting which contains the constants to tranlate from longitutde and latitude to NED reference frame.  Automatically set by @ref GPSModule after acquiring a 3D lock.  Used by @ref AHRSCommsModule.</description>
+        <description>HomeLocation setting which contains the constants to translate from longitude and latitude to NED reference frame.  Automatically set by @ref GPSModule after acquiring a 3D lock.  Used by @ref AHRSCommsModule.</description>
         <field name="Set" units="" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="FALSE"/>
         <field name="Latitude" units="deg * 10e6" type="int32" elements="1" defaultvalue="0"/>
         <field name="Longitude" units="deg * 10e6" type="int32" elements="1" defaultvalue="0"/>
diff --git a/shared/uavobjectdefinition/manualcontrolcommand.xml b/shared/uavobjectdefinition/manualcontrolcommand.xml
index 067b4821e..139a0dc0a 100644
--- a/shared/uavobjectdefinition/manualcontrolcommand.xml
+++ b/shared/uavobjectdefinition/manualcontrolcommand.xml
@@ -1,6 +1,6 @@
 <xml>
     <object name="ManualControlCommand" singleinstance="true" settings="false" category="Control">
-        <description>The output from the @ref ManualControlModule which descodes the receiver inputs.  Overriden by GCS for fly-by-wire control.</description>
+        <description>The output from the @ref ManualControlModule which decodes the receiver inputs. Overriden by GCS for fly-by-wire control.</description>
         <field name="Connected" units="" type="enum" elements="1" options="False,True"/>
 	<field name="Throttle" units="%" type="float" elements="1"/>
 	<field name="Roll" units="%" type="float" elements="1"/>
diff --git a/shared/uavobjectdefinition/systemsettings.xml b/shared/uavobjectdefinition/systemsettings.xml
index 0b9780597..8f86537d7 100644
--- a/shared/uavobjectdefinition/systemsettings.xml
+++ b/shared/uavobjectdefinition/systemsettings.xml
@@ -2,6 +2,7 @@
     <object name="SystemSettings" singleinstance="true" settings="true" category="System">
         <description>Select airframe type.  Currently used by @ref ActuatorModule to choose mixing from @ref ActuatorDesired to @ref ActuatorCommand</description>
         <field name="AirframeType" units="" type="enum" elements="1" options="FixedWing,FixedWingElevon,FixedWingVtail,VTOL,HeliCP,QuadX,QuadP,QuadH,Hexa,Octo,Custom,HexaX,HexaH,OctoV,OctoCoaxP,OctoCoaxX,OctoX,HexaCoax,Tri,GroundVehicleCar,GroundVehicleDifferential,GroundVehicleMotorcycle" defaultvalue="QuadX"/>
+        <field name="VehicleName" units="char" type="uint8" elements="20" defaultvalue="0"/>
 	<field name="ThrustControl" units="" type="enum" elements="1" options="Throttle,Collective,None" defaultvalue="Throttle" />
 		<!-- Which way the vehicle controls its thrust. Can be through
 			"Throttle" (quadcopter, simple brushless planes,