Merge branch 'thread/OP-1154_Cruise_Control_automatically_increase_copter_throttle_per' of ssh://git.openpilot.org/OpenPilot into thread/OP-1154_Cruise_Control_automatically_increase_copter_throttle_per

flight/libraries/sanitycheck.c

@@ -35,6 +35,7 @@
 // UAVOs
 #include <manualcontrolsettings.h>
 #include <systemsettings.h>
+#include <systemalarms.h>
 #include <taskinfo.h>
 
 /****************************
@@ -113,9 +114,9 @@ int32_t configuration_check()
         case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_ALTITUDEVARIO:
             if (coptercontrol) {
                 severity = SYSTEMALARMS_ALARM_ERROR;
-            } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_ALTITUDEHOLD)) { // Revo supports altitude hold
-                severity = SYSTEMALARMS_ALARM_ERROR;
             }
+            // TODO: put check equivalent to TASK_MONITOR_IsRunning
+            // here as soon as available for delayed callbacks
             break;
         case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL:
             if (coptercontrol) {
@@ -151,9 +152,15 @@ int32_t configuration_check()
         case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_PATHPLANNER:
             if (coptercontrol) {
                 severity = SYSTEMALARMS_ALARM_ERROR;
-            } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) {
+            } else {
-                // Revo supports PathPlanner
+                // Revo supports PathPlanner and that must be OK or we are not sane
-                severity = SYSTEMALARMS_ALARM_ERROR;
+                // PathPlan alarm is uninitialized if not running
+                // PathPlan alarm is warning or error if the flightplan is invalid
+                SystemAlarmsAlarmData alarms;
+                SystemAlarmsAlarmGet(&alarms);
+                if (alarms.PathPlan != SYSTEMALARMS_ALARM_OK) {
+                    severity = SYSTEMALARMS_ALARM_ERROR;
+                }
             }
             break;
         case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE:

flight/modules/Altitude/revolution/altitude.c

@@ -40,22 +40,24 @@
 
 #include "altitude.h"
 #include "barosensor.h" // object that will be updated by the module
+#include "revosettings.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 500
+#define STACK_SIZE_BYTES 550
 #define TASK_PRIORITY    (tskIDLE_PRIORITY + 1)
 
 // Private types
 
 // Private variables
 static xTaskHandle taskHandle;
-
+static RevoSettingsBaroTempCorrectionPolynomialData baroCorrection;
 // Private functions
 static void altitudeTask(void *parameters);
+static void SettingsUpdatedCb(UAVObjEvent *ev);
 
 /**
  * Initialise the module, called on startup
@@ -77,6 +79,9 @@ int32_t AltitudeStart()
 int32_t AltitudeInitialize()
 {
     BaroSensorInitialize();
+    RevoSettingsInitialize();
+    RevoSettingsConnectCallback(&SettingsUpdatedCb);
+
 #if defined(PIOS_INCLUDE_HCSR04)
     SonarAltitudeInitialize();
 #endif
@@ -103,6 +108,8 @@ static void altitudeTask(__attribute__((unused)) void *parameters)
 // Undef for normal operation
 // #define PIOS_MS5611_SLOW_TEMP_RATE 20
 
+    RevoSettingsBaroTempCorrectionPolynomialGet(&baroCorrection);
+
 #ifdef PIOS_MS5611_SLOW_TEMP_RATE
     uint8_t temp_press_interleave_count = 1;
 #endif
@@ -154,12 +161,12 @@ static void altitudeTask(__attribute__((unused)) void *parameters)
         vTaskDelay(PIOS_MS5611_GetDelay());
         PIOS_MS5611_ReadADC();
 
-
         temp  = PIOS_MS5611_GetTemperature();
         press = PIOS_MS5611_GetPressure();
+        float temp2    = temp * temp;
+        press = press - baroCorrection.a + temp * baroCorrection.b + temp2 * baroCorrection.c + temp * temp2 * baroCorrection.d;
 
-
+        float altitude = 44330.0f * (1.0f - powf((press) / MS5611_P0, (1.0f / 5.255f)));
-        float altitude = 44330.0f * (1.0f - powf(press / MS5611_P0, (1.0f / 5.255f)));
 
         if (!isnan(altitude)) {
             data.Altitude    = altitude;
@@ -171,6 +178,10 @@ static void altitudeTask(__attribute__((unused)) void *parameters)
     }
 }
 
+static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
+{
+    RevoSettingsBaroTempCorrectionPolynomialGet(&baroCorrection);
+}
 /**
  * @}
  * @}

flight/modules/AltitudeHold/altitudehold.c

@@ -46,33 +46,35 @@
 #include <openpilot.h>
 
 #include <math.h>
+#include <pid.h>
 #include <CoordinateConversions.h>
-#include <altholdsmoothed.h>
 #include <attitudestate.h>
 #include <altitudeholdsettings.h>
 #include <altitudeholddesired.h> // object that will be updated by the module
-#include <barosensor.h>
+#include <altitudeholdstatus.h>
-#include <positionstate.h>
 #include <flightstatus.h>
 #include <stabilizationdesired.h>
 #include <accelstate.h>
-#include <taskinfo.h>
 #include <pios_constants.h>
+#include <velocitystate.h>
+#include <positionstate.h>
 // Private constants
-#define MAX_QUEUE_SIZE   2
+
-#define STACK_SIZE_BYTES 1024
+#define CALLBACK_PRIORITY      CALLBACK_PRIORITY_LOW
-#define TASK_PRIORITY    (tskIDLE_PRIORITY + 1)
+#define CBTASK_PRIORITY        CALLBACK_TASK_FLIGHTCONTROL
-#define ACCEL_DOWNSAMPLE 4
+
-#define TIMEOUT_TRESHOLD 200000
+#define STACK_SIZE_BYTES       1024
+#define DESIRED_UPDATE_RATE_MS 100 // milliseconds
 // Private types
 
 // Private variables
-static xTaskHandle altitudeHoldTaskHandle;
+static DelayedCallbackInfo *altitudeHoldCBInfo;
-static xQueueHandle queue;
 static AltitudeHoldSettingsData altitudeHoldSettings;
+static struct pid pid0, pid1;
+
 
 // Private functions
-static void altitudeHoldTask(void *parameters);
+static void altitudeHoldTask(void);
 static void SettingsUpdatedCb(UAVObjEvent *ev);
 
 /**
@@ -82,8 +84,8 @@ static void SettingsUpdatedCb(UAVObjEvent *ev);
 int32_t AltitudeHoldStart()
 {
     // Start main task
-    xTaskCreate(altitudeHoldTask, (signed char *)"AltitudeHold", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &altitudeHoldTaskHandle);
+    SettingsUpdatedCb(NULL);
-    PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_ALTITUDEHOLD, altitudeHoldTaskHandle);
+    DelayedCallbackDispatch(altitudeHoldCBInfo);
 
     return 0;
 }
@@ -96,319 +98,122 @@ int32_t AltitudeHoldInitialize()
 {
     AltitudeHoldSettingsInitialize();
     AltitudeHoldDesiredInitialize();
-    AltHoldSmoothedInitialize();
+    AltitudeHoldStatusInitialize();
 
     // Create object queue
-    queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
 
+    altitudeHoldCBInfo = DelayedCallbackCreate(&altitudeHoldTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, STACK_SIZE_BYTES);
     AltitudeHoldSettingsConnectCallback(&SettingsUpdatedCb);
 
     return 0;
 }
 MODULE_INITCALL(AltitudeHoldInitialize, AltitudeHoldStart);
 
-float tau;
-float throttleIntegral;
-float velocity;
-float decay;
-float velocity_decay;
-bool running = false;
-float error;
-float switchThrottle;
-float smoothed_altitude;
-float starting_altitude;
 
 /**
  * Module thread, should not return.
  */
-static void altitudeHoldTask(__attribute__((unused)) void *parameters)
+static void altitudeHoldTask(void)
 {
-    AltitudeHoldDesiredData altitudeHoldDesired;
+    static float startThrottle = 0.5f;
-    StabilizationDesiredData stabilizationDesired;
 
-    portTickType thisTime, lastUpdateTime;
+    // make sure we run only when we are supposed to run
-    UAVObjEvent ev;
+    FlightStatusData flightStatus;
 
-    // Force update of the settings
+    FlightStatusGet(&flightStatus);
-    SettingsUpdatedCb(&ev);
+    switch (flightStatus.FlightMode) {
-    // Failsafe handling
+    case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD:
-    uint32_t lastAltitudeHoldDesiredUpdate = 0;
+    case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO:
-    bool enterFailSafe = false;
+        break;
-    // Listen for updates.
+    default:
-    AltitudeHoldDesiredConnectQueue(queue);
+        pid_zero(&pid0);
-    BaroSensorConnectQueue(queue);
+        pid_zero(&pid1);
-    FlightStatusConnectQueue(queue);
+        StabilizationDesiredThrottleGet(&startThrottle);
-    AccelStateConnectQueue(queue);
+        DelayedCallbackSchedule(altitudeHoldCBInfo, DESIRED_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER);
-    bool altitudeHoldFlightMode = false;
+        return;
-    BaroSensorAltitudeGet(&smoothed_altitude);
-    running = false;
-    enum init_state { WAITING_BARO, WAITIING_INIT, INITED } init = WAITING_BARO;
 
-    uint8_t flightMode;
+        break;
-    FlightStatusFlightModeGet(&flightMode);
-    // initialize enable flag
-    altitudeHoldFlightMode = flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD || flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO;
-    // Main task loop
-    bool baro_updated = false;
-    while (1) {
-        enterFailSafe = PIOS_DELAY_DiffuS(lastAltitudeHoldDesiredUpdate) > TIMEOUT_TRESHOLD;
-        // Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe
-        if (xQueueReceive(queue, &ev, 100 / portTICK_RATE_MS) != pdTRUE) {
-            if (!running) {
-                throttleIntegral = 0;
-            }
-
-            // Todo: Add alarm if it should be running
-            continue;
-        } else if (ev.obj == BaroSensorHandle()) {
-            baro_updated = true;
-
-            init = (init == WAITING_BARO) ? WAITIING_INIT : init;
-        } else if (ev.obj == FlightStatusHandle()) {
-            FlightStatusFlightModeGet(&flightMode);
-            altitudeHoldFlightMode = flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD || flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO;
-            if (altitudeHoldFlightMode && !running) {
-                // Copy the current throttle as a starting point for integral
-                StabilizationDesiredThrottleGet(&throttleIntegral);
-                switchThrottle = throttleIntegral;
-                error    = 0;
-                velocity = 0;
-                running  = true;
-
-                AltHoldSmoothedData altHold;
-                AltHoldSmoothedGet(&altHold);
-                starting_altitude = altHold.Altitude;
-            } else if (!altitudeHoldFlightMode) {
-                running = false;
-                lastAltitudeHoldDesiredUpdate = PIOS_DELAY_GetRaw();
-            }
-        } else if (ev.obj == AccelStateHandle()) {
-            static uint32_t timeval;
-
-            static float z[4] = { 0, 0, 0, 0 };
-            float z_new[4];
-            float P[4][4], K[4][2], x[2];
-            float G[4] = { 1.0e-15f, 1.0e-15f, 1.0e-3f, 1.0e-7f };
-            static float V[4][4] = {
-                { 10.0f, 0.0f,   0.0f,   0.0f    },
-                { 0.0f,  100.0f, 0.0f,   0.0f    },
-                { 0.0f,  0.0f,   100.0f, 0.0f    },
-                { 0.0f,  0.0f,   0.0f,   1000.0f }
-            };
-            static uint32_t accel_downsample_count = 0;
-            static float accels_accum[3] = { 0.0f, 0.0f, 0.0f };
-            float dT;
-            static float S[2] = { 1.0f, 10.0f };
-
-            /* Somehow this always assigns to zero.  Compiler bug? Race condition? */
-            S[0] = altitudeHoldSettings.PressureNoise;
-            S[1] = altitudeHoldSettings.AccelNoise;
-            G[2] = altitudeHoldSettings.AccelDrift;
-
-            AccelStateData accelState;
-            AccelStateGet(&accelState);
-            AttitudeStateData attitudeState;
-            AttitudeStateGet(&attitudeState);
-            BaroSensorData baro;
-            BaroSensorGet(&baro);
-
-            /* Downsample accels to stop this calculation consuming too much CPU */
-            accels_accum[0] += accelState.x;
-            accels_accum[1] += accelState.y;
-            accels_accum[2] += accelState.z;
-            accel_downsample_count++;
-
-            if (accel_downsample_count < ACCEL_DOWNSAMPLE) {
-                continue;
-            }
-
-            accel_downsample_count = 0;
-            accelState.x    = accels_accum[0] / ACCEL_DOWNSAMPLE;
-            accelState.y    = accels_accum[1] / ACCEL_DOWNSAMPLE;
-            accelState.z    = accels_accum[2] / ACCEL_DOWNSAMPLE;
-            accels_accum[0] = accels_accum[1] = accels_accum[2] = 0;
-
-            thisTime = xTaskGetTickCount();
-
-            if (init == WAITIING_INIT) {
-                z[0] = baro.Altitude;
-                z[1] = 0;
-                z[2] = accelState.z;
-                z[3] = 0;
-                init = INITED;
-            } else if (init == WAITING_BARO) {
-                continue;
-            }
-
-            x[0] = baro.Altitude;
-            // rotate avg accels into earth frame and store it
-            if (1) {
-                float q[4], Rbe[3][3];
-                q[0] = attitudeState.q1;
-                q[1] = attitudeState.q2;
-                q[2] = attitudeState.q3;
-                q[3] = attitudeState.q4;
-                Quaternion2R(q, Rbe);
-                x[1] = -(Rbe[0][2] * accelState.x + Rbe[1][2] * accelState.y + Rbe[2][2] * accelState.z + 9.81f);
-            } else {
-                x[1] = -accelState.z + 9.81f;
-            }
-
-            dT = PIOS_DELAY_DiffuS(timeval) / 1.0e6f;
-            timeval = PIOS_DELAY_GetRaw();
-
-            P[0][0] = dT * (V[0][1] + dT * V[1][1]) + V[0][0] + G[0] + dT * V[1][0];
-            P[0][1] = dT * (V[0][2] + dT * V[1][2]) + V[0][1] + dT * V[1][1];
-            P[0][2] = V[0][2] + dT * V[1][2];
-            P[0][3] = V[0][3] + dT * V[1][3];
-            P[1][0] = dT * (V[1][1] + dT * V[2][1]) + V[1][0] + dT * V[2][0];
-            P[1][1] = dT * (V[1][2] + dT * V[2][2]) + V[1][1] + G[1] + dT * V[2][1];
-            P[1][2] = V[1][2] + dT * V[2][2];
-            P[1][3] = V[1][3] + dT * V[2][3];
-            P[2][0] = V[2][0] + dT * V[2][1];
-            P[2][1] = V[2][1] + dT * V[2][2];
-            P[2][2] = V[2][2] + G[2];
-            P[2][3] = V[2][3];
-            P[3][0] = V[3][0] + dT * V[3][1];
-            P[3][1] = V[3][1] + dT * V[3][2];
-            P[3][2] = V[3][2];
-            P[3][3] = V[3][3] + G[3];
-
-            if (baro_updated) {
-                K[0][0]  = -(V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1]) / (V[0][0] * G[2] + V[0][0] * G[3] + V[2][2] * G[0] + V[2][3] * G[0] + V[3][2] * G[0] + V[3][3] * G[0] + V[0][0] * S[1] + V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] + V[0][0] * V[3][2] - V[0][2] * V[3][0] - V[2][0] * V[0][3] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[2] + G[0] * G[3] + G[0] * S[1] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] - (dT * dT) * V[2][1] * V[1][3] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + dT * V[0][1] * S[1] + dT * V[1][0] * S[1] + (dT * dT) * V[1][1] * G[2] + (dT * dT) * V[1][1] * G[3] + (dT * dT) * V[1][1] * S[1] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[2][3] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]) + 1.0f;
-                K[0][1]  = ((V[0][2] + V[0][3]) * S[0] + dT * (V[1][2] + V[1][3]) * S[0]) / (V[0][0] * G[2] + V[0][0] * G[3] + V[2][2] * G[0] + V[2][3] * G[0] + V[3][2] * G[0] + V[3][3] * G[0] + V[0][0] * S[1] + V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] + V[0][0] * V[3][2] - V[0][2] * V[3][0] - V[2][0] * V[0][3] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[2] + G[0] * G[3] + G[0] * S[1] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] - (dT * dT) * V[2][1] * V[1][3] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + dT * V[0][1] * S[1] + dT * V[1][0] * S[1] + (dT * dT) * V[1][1] * G[2] + (dT * dT) * V[1][1] * G[3] + (dT * dT) * V[1][1] * S[1] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[2][3] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]);
-                K[1][0]  = (V[1][0] * G[2] + V[1][0] * G[3] + V[1][0] * S[1] + V[1][0] * V[2][2] - V[2][0] * V[1][2] + V[1][0] * V[2][3] + V[1][0] * V[3][2] - V[2][0] * V[1][3] - V[1][2] * V[3][0] + V[1][0] * V[3][3] - V[3][0] * V[1][3] + (dT * dT) * V[2][1] * V[3][2] - (dT * dT) * V[2][2] * V[3][1] + (dT * dT) * V[2][1] * V[3][3] - (dT * dT) * V[3][1] * V[2][3] + dT * V[1][1] * G[2] + dT * V[2][0] * G[2] + dT * V[1][1] * G[3] + dT * V[2][0] * G[3] + dT * V[1][1] * S[1] + dT * V[2][0] * S[1] + (dT * dT) * V[2][1] * G[2] + (dT * dT) * V[2][1] * G[3] + (dT * dT) * V[2][1] * S[1] + dT * V[1][1] * V[2][2] - dT * V[1][2] * V[2][1] + dT * V[1][1] * V[2][3] + dT * V[1][1] * V[3][2] + dT * V[2][0] * V[3][2] - dT * V[1][2] * V[3][1] - dT * V[2][1] * V[1][3] - dT * V[3][0] * V[2][2] + dT * V[1][1] * V[3][3] + dT * V[2][0] * V[3][3] - dT * V[3][0] * V[2][3] - dT * V[1][3] * V[3][1]) / (V[0][0] * G[2] + V[0][0] * G[3] + V[2][2] * G[0] + V[2][3] * G[0] + V[3][2] * G[0] + V[3][3] * G[0] + V[0][0] * S[1] + V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] + V[0][0] * V[3][2] - V[0][2] * V[3][0] - V[2][0] * V[0][3] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[2] + G[0] * G[3] + G[0] * S[1] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] - (dT * dT) * V[2][1] * V[1][3] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + dT * V[0][1] * S[1] + dT * V[1][0] * S[1] + (dT * dT) * V[1][1] * G[2] + (dT * dT) * V[1][1] * G[3] + (dT * dT) * V[1][1] * S[1] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[2][3] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]);
-                K[1][1]  = (V[1][2] * G[0] + V[1][3] * G[0] + V[1][2] * S[0] + V[1][3] * S[0] + V[0][0] * V[1][2] - V[1][0] * V[0][2] + V[0][0] * V[1][3] - V[1][0] * V[0][3] + (dT * dT) * V[0][1] * V[2][2] + (dT * dT) * V[1][0] * V[2][2] - (dT * dT) * V[0][2] * V[2][1] - (dT * dT) * V[2][0] * V[1][2] + (dT * dT) * V[0][1] * V[2][3] + (dT * dT) * V[1][0] * V[2][3] - (dT * dT) * V[2][0] * V[1][3] - (dT * dT) * V[0][3] * V[2][1] + (dT * dT * dT) * V[1][1] * V[2][2] - (dT * dT * dT) * V[1][2] * V[2][1] + (dT * dT * dT) * V[1][1] * V[2][3] - (dT * dT * dT) * V[2][1] * V[1][3] + dT * V[2][2] * G[0] + dT * V[2][3] * G[0] + dT * V[2][2] * S[0] + dT * V[2][3] * S[0] + dT * V[0][0] * V[2][2] + dT * V[0][1] * V[1][2] - dT * V[0][2] * V[1][1] - dT * V[0][2] * V[2][0] + dT * V[0][0] * V[2][3] + dT * V[0][1] * V[1][3] - dT * V[1][1] * V[0][3] - dT * V[2][0] * V[0][3]) / (V[0][0] * G[2] + V[0][0] * G[3] + V[2][2] * G[0] + V[2][3] * G[0] + V[3][2] * G[0] + V[3][3] * G[0] + V[0][0] * S[1] + V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] + V[0][0] * V[3][2] - V[0][2] * V[3][0] - V[2][0] * V[0][3] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[2] + G[0] * G[3] + G[0] * S[1] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] - (dT * dT) * V[2][1] * V[1][3] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + dT * V[0][1] * S[1] + dT * V[1][0] * S[1] + (dT * dT) * V[1][1] * G[2] + (dT * dT) * V[1][1] * G[3] + (dT * dT) * V[1][1] * S[1] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[2][3] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]);
-                K[2][0]  = (V[2][0] * G[3] - V[3][0] * G[2] + V[2][0] * S[1] + V[2][0] * V[3][2] - V[3][0] * V[2][2] + V[2][0] * V[3][3] - V[3][0] * V[2][3] + dT * V[2][1] * G[3] - dT * V[3][1] * G[2] + dT * V[2][1] * S[1] + dT * V[2][1] * V[3][2] - dT * V[2][2] * V[3][1] + dT * V[2][1] * V[3][3] - dT * V[3][1] * V[2][3]) / (V[0][0] * G[2] + V[0][0] * G[3] + V[2][2] * G[0] + V[2][3] * G[0] + V[3][2] * G[0] + V[3][3] * G[0] + V[0][0] * S[1] + V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] + V[0][0] * V[3][2] - V[0][2] * V[3][0] - V[2][0] * V[0][3] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[2] + G[0] * G[3] + G[0] * S[1] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] - (dT * dT) * V[2][1] * V[1][3] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + dT * V[0][1] * S[1] + dT * V[1][0] * S[1] + (dT * dT) * V[1][1] * G[2] + (dT * dT) * V[1][1] * G[3] + (dT * dT) * V[1][1] * S[1] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[2][3] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]);
-                K[2][1]  = (V[0][0] * G[2] + V[2][2] * G[0] + V[2][3] * G[0] + V[2][2] * S[0] + V[2][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] - V[2][0] * V[0][3] + G[0] * G[2] + G[2] * S[0] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] - (dT * dT) * V[2][1] * V[1][3] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + (dT * dT) * V[1][1] * G[2] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[1][0] * V[2][3] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1]) / (V[0][0] * G[2] + V[0][0] * G[3] + V[2][2] * G[0] + V[2][3] * G[0] + V[3][2] * G[0] + V[3][3] * G[0] + V[0][0] * S[1] + V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] + V[0][0] * V[3][2] - V[0][2] * V[3][0] - V[2][0] * V[0][3] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[2] + G[0] * G[3] + G[0] * S[1] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] - (dT * dT) * V[2][1] * V[1][3] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + dT * V[0][1] * S[1] + dT * V[1][0] * S[1] + (dT * dT) * V[1][1] * G[2] + (dT * dT) * V[1][1] * G[3] + (dT * dT) * V[1][1] * S[1] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[2][3] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]);
-                K[3][0]  = (-V[2][0] * G[3] + V[3][0] * G[2] + V[3][0] * S[1] - V[2][0] * V[3][2] + V[3][0] * V[2][2] - V[2][0] * V[3][3] + V[3][0] * V[2][3] - dT * V[2][1] * G[3] + dT * V[3][1] * G[2] + dT * V[3][1] * S[1] - dT * V[2][1] * V[3][2] + dT * V[2][2] * V[3][1] - dT * V[2][1] * V[3][3] + dT * V[3][1] * V[2][3]) / (V[0][0] * G[2] + V[0][0] * G[3] + V[2][2] * G[0] + V[2][3] * G[0] + V[3][2] * G[0] + V[3][3] * G[0] + V[0][0] * S[1] + V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] + V[0][0] * V[3][2] - V[0][2] * V[3][0] - V[2][0] * V[0][3] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[2] + G[0] * G[3] + G[0] * S[1] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] - (dT * dT) * V[2][1] * V[1][3] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + dT * V[0][1] * S[1] + dT * V[1][0] * S[1] + (dT * dT) * V[1][1] * G[2] + (dT * dT) * V[1][1] * G[3] + (dT * dT) * V[1][1] * S[1] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[2][3] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]);
-                K[3][1]  = (V[0][0] * G[3] + V[3][2] * G[0] + V[3][3] * G[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[3][2] - V[0][2] * V[3][0] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[3] + G[3] * S[0] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + (dT * dT) * V[1][1] * G[3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]) / (V[0][0] * G[2] + V[0][0] * G[3] + V[2][2] * G[0] + V[2][3] * G[0] + V[3][2] * G[0] + V[3][3] * G[0] + V[0][0] * S[1] + V[2][2] * S[0] + V[2][3] * S[0] + V[3][2] * S[0] + V[3][3] * S[0] + V[0][0] * V[2][2] - V[0][2] * V[2][0] + V[0][0] * V[2][3] + V[0][0] * V[3][2] - V[0][2] * V[3][0] - V[2][0] * V[0][3] + V[0][0] * V[3][3] - V[0][3] * V[3][0] + G[0] * G[2] + G[0] * G[3] + G[0] * S[1] + G[2] * S[0] + G[3] * S[0] + S[0] * S[1] + (dT * dT) * V[1][1] * V[2][2] - (dT * dT) * V[1][2] * V[2][1] + (dT * dT) * V[1][1] * V[2][3] + (dT * dT) * V[1][1] * V[3][2] - (dT * dT) * V[1][2] * V[3][1] - (dT * dT) * V[2][1] * V[1][3] + (dT * dT) * V[1][1] * V[3][3] - (dT * dT) * V[1][3] * V[3][1] + dT * V[0][1] * G[2] + dT * V[1][0] * G[2] + dT * V[0][1] * G[3] + dT * V[1][0] * G[3] + dT * V[0][1] * S[1] + dT * V[1][0] * S[1] + (dT * dT) * V[1][1] * G[2] + (dT * dT) * V[1][1] * G[3] + (dT * dT) * V[1][1] * S[1] + dT * V[0][1] * V[2][2] + dT * V[1][0] * V[2][2] - dT * V[0][2] * V[2][1] - dT * V[2][0] * V[1][2] + dT * V[0][1] * V[2][3] + dT * V[0][1] * V[3][2] + dT * V[1][0] * V[2][3] + dT * V[1][0] * V[3][2] - dT * V[0][2] * V[3][1] - dT * V[2][0] * V[1][3] - dT * V[0][3] * V[2][1] - dT * V[1][2] * V[3][0] + dT * V[0][1] * V[3][3] + dT * V[1][0] * V[3][3] - dT * V[0][3] * V[3][1] - dT * V[3][0] * V[1][3]);
-
-                z_new[0] = -K[0][0] * (dT * z[1] - x[0] + z[0]) + dT * z[1] - K[0][1] * (-x[1] + z[2] + z[3]) + z[0];
-                z_new[1] = -K[1][0] * (dT * z[1] - x[0] + z[0]) + dT * z[2] - K[1][1] * (-x[1] + z[2] + z[3]) + z[1];
-                z_new[2] = -K[2][0] * (dT * z[1] - x[0] + z[0]) - K[2][1] * (-x[1] + z[2] + z[3]) + z[2];
-                z_new[3] = -K[3][0] * (dT * z[1] - x[0] + z[0]) - K[3][1] * (-x[1] + z[2] + z[3]) + z[3];
-
-                memcpy(z, z_new, sizeof(z_new));
-
-                V[0][0] = -K[0][1] * P[2][0] - K[0][1] * P[3][0] - P[0][0] * (K[0][0] - 1.0f);
-                V[0][1] = -K[0][1] * P[2][1] - K[0][1] * P[3][2] - P[0][1] * (K[0][0] - 1.0f);
-                V[0][2] = -K[0][1] * P[2][2] - K[0][1] * P[3][2] - P[0][2] * (K[0][0] - 1.0f);
-                V[0][3] = -K[0][1] * P[2][3] - K[0][1] * P[3][3] - P[0][3] * (K[0][0] - 1.0f);
-                V[1][0] = P[1][0] - K[1][0] * P[0][0] - K[1][1] * P[2][0] - K[1][1] * P[3][0];
-                V[1][1] = P[1][1] - K[1][0] * P[0][1] - K[1][1] * P[2][1] - K[1][1] * P[3][2];
-                V[1][2] = P[1][2] - K[1][0] * P[0][2] - K[1][1] * P[2][2] - K[1][1] * P[3][2];
-                V[1][3] = P[1][3] - K[1][0] * P[0][3] - K[1][1] * P[2][3] - K[1][1] * P[3][3];
-                V[2][0] = -K[2][0] * P[0][0] - K[2][1] * P[3][0] - P[2][0] * (K[2][1] - 1.0f);
-                V[2][1] = -K[2][0] * P[0][1] - K[2][1] * P[3][2] - P[2][1] * (K[2][1] - 1.0f);
-                V[2][2] = -K[2][0] * P[0][2] - K[2][1] * P[3][2] - P[2][2] * (K[2][1] - 1.0f);
-                V[2][3] = -K[2][0] * P[0][3] - K[2][1] * P[3][3] - P[2][3] * (K[2][1] - 1.0f);
-                V[3][0] = -K[3][0] * P[0][0] - K[3][1] * P[2][0] - P[3][0] * (K[3][1] - 1.0f);
-                V[3][1] = -K[3][0] * P[0][1] - K[3][1] * P[2][1] - P[3][2] * (K[3][1] - 1.0f);
-                V[3][2] = -K[3][0] * P[0][2] - K[3][1] * P[2][2] - P[3][2] * (K[3][1] - 1.0f);
-                V[3][3] = -K[3][0] * P[0][3] - K[3][1] * P[2][3] - P[3][3] * (K[3][1] - 1.0f);
-
-
-                baro_updated = false;
-            } else {
-                K[0][0]  = (V[0][2] + V[0][3] + dT * V[1][2] + dT * V[1][3]) / (V[2][2] + V[2][3] + V[3][2] + V[3][3] + G[2] + G[3] + S[1]);
-                K[1][0]  = (V[1][2] + V[1][3] + dT * V[2][2] + dT * V[2][3]) / (V[2][2] + V[2][3] + V[3][2] + V[3][3] + G[2] + G[3] + S[1]);
-                K[2][0]  = (V[2][2] + V[2][3] + G[2]) / (V[2][2] + V[2][3] + V[3][2] + V[3][3] + G[2] + G[3] + S[1]);
-                K[3][0]  = (V[3][2] + V[3][3] + G[3]) / (V[2][2] + V[2][3] + V[3][2] + V[3][3] + G[2] + G[3] + S[1]);
-
-
-                z_new[0] = dT * z[1] - K[0][0] * (-x[1] + z[2] + z[3]) + z[0];
-                z_new[1] = dT * z[2] - K[1][0] * (-x[1] + z[2] + z[3]) + z[1];
-                z_new[2] = -K[2][0] * (-x[1] + z[2] + z[3]) + z[2];
-                z_new[3] = -K[3][0] * (-x[1] + z[2] + z[3]) + z[3];
-
-                memcpy(z, z_new, sizeof(z_new));
-
-                V[0][0] = P[0][0] - K[0][0] * P[2][0] - K[0][0] * P[3][0];
-                V[0][1] = P[0][1] - K[0][0] * P[2][1] - K[0][0] * P[3][2];
-                V[0][2] = P[0][2] - K[0][0] * P[2][2] - K[0][0] * P[3][2];
-                V[0][3] = P[0][3] - K[0][0] * P[2][3] - K[0][0] * P[3][3];
-                V[1][0] = P[1][0] - K[1][0] * P[2][0] - K[1][0] * P[3][0];
-                V[1][1] = P[1][1] - K[1][0] * P[2][1] - K[1][0] * P[3][2];
-                V[1][2] = P[1][2] - K[1][0] * P[2][2] - K[1][0] * P[3][2];
-                V[1][3] = P[1][3] - K[1][0] * P[2][3] - K[1][0] * P[3][3];
-                V[2][0] = -K[2][0] * P[3][0] - P[2][0] * (K[2][0] - 1.0f);
-                V[2][1] = -K[2][0] * P[3][2] - P[2][1] * (K[2][0] - 1.0f);
-                V[2][2] = -K[2][0] * P[3][2] - P[2][2] * (K[2][0] - 1.0f);
-                V[2][3] = -K[2][0] * P[3][3] - P[2][3] * (K[2][0] - 1.0f);
-                V[3][0] = -K[3][0] * P[2][0] - P[3][0] * (K[3][0] - 1.0f);
-                V[3][1] = -K[3][0] * P[2][1] - P[3][2] * (K[3][0] - 1.0f);
-                V[3][2] = -K[3][0] * P[2][2] - P[3][2] * (K[3][0] - 1.0f);
-                V[3][3] = -K[3][0] * P[2][3] - P[3][3] * (K[3][0] - 1.0f);
-            }
-
-            AltHoldSmoothedData altHold;
-            AltHoldSmoothedGet(&altHold);
-            altHold.Altitude = z[0];
-            altHold.Velocity = z[1];
-            altHold.Accel    = z[2];
-            AltHoldSmoothedSet(&altHold);
-
-            AltHoldSmoothedGet(&altHold);
-
-            // Verify that we are in altitude hold mode
-            uint8_t armed;
-            FlightStatusArmedGet(&armed);
-            if (!altitudeHoldFlightMode || armed != FLIGHTSTATUS_ARMED_ARMED) {
-                running = false;
-            }
-
-            if (!running) {
-                lastAltitudeHoldDesiredUpdate = PIOS_DELAY_GetRaw();
-                continue;
-            }
-
-            // Compute the altitude error
-            error = (starting_altitude + altitudeHoldDesired.Altitude) - altHold.Altitude;
-
-            // Compute integral off altitude error
-            throttleIntegral += error * altitudeHoldSettings.Ki * dT;
-
-            // Only update stabilizationDesired less frequently
-            if ((thisTime - lastUpdateTime) < 20) {
-                continue;
-            }
-
-            lastUpdateTime = thisTime;
-
-            // Instead of explicit limit on integral you output limit feedback
-            StabilizationDesiredGet(&stabilizationDesired);
-            if (!enterFailSafe) {
-                stabilizationDesired.Throttle = error * altitudeHoldSettings.Kp + throttleIntegral -
-                                                altHold.Velocity * altitudeHoldSettings.Kd - altHold.Accel * altitudeHoldSettings.Ka;
-                if (stabilizationDesired.Throttle > 1) {
-                    throttleIntegral -= (stabilizationDesired.Throttle - 1);
-                    stabilizationDesired.Throttle = 1;
-                } else if (stabilizationDesired.Throttle < 0) {
-                    throttleIntegral -= stabilizationDesired.Throttle;
-                    stabilizationDesired.Throttle = 0;
-                }
-            } else {
-                // shutdown motors
-                stabilizationDesired.Throttle = -1;
-            }
-            stabilizationDesired.StabilizationMode.Roll  = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
-            stabilizationDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
-            stabilizationDesired.StabilizationMode.Yaw   = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
-            stabilizationDesired.Roll  = altitudeHoldDesired.Roll;
-            stabilizationDesired.Pitch = altitudeHoldDesired.Pitch;
-            stabilizationDesired.Yaw   = altitudeHoldDesired.Yaw;
-
-            StabilizationDesiredSet(&stabilizationDesired);
-        } else if (ev.obj == AltitudeHoldDesiredHandle()) {
-            // reset the failsafe timer
-            lastAltitudeHoldDesiredUpdate = PIOS_DELAY_GetRaw();
-            AltitudeHoldDesiredGet(&altitudeHoldDesired);
-        }
     }
+
+    AltitudeHoldStatusData altitudeHoldStatus;
+    AltitudeHoldStatusGet(&altitudeHoldStatus);
+
+    // do the actual control loop(s)
+    AltitudeHoldDesiredData altitudeHoldDesired;
+    AltitudeHoldDesiredGet(&altitudeHoldDesired);
+    float positionStateDown;
+    PositionStateDownGet(&positionStateDown);
+    float velocityStateDown;
+    VelocityStateDownGet(&velocityStateDown);
+
+    switch (altitudeHoldDesired.ControlMode) {
+    case ALTITUDEHOLDDESIRED_CONTROLMODE_ALTITUDE:
+        // altitude control loop
+        altitudeHoldStatus.VelocityDesired = pid_apply_setpoint(&pid0, 1.0f, altitudeHoldDesired.SetPoint, positionStateDown, 1000.0f / DESIRED_UPDATE_RATE_MS);
+        break;
+    case ALTITUDEHOLDDESIRED_CONTROLMODE_VELOCITY:
+        altitudeHoldStatus.VelocityDesired = altitudeHoldDesired.SetPoint;
+        break;
+    default:
+        altitudeHoldStatus.VelocityDesired = 0;
+        break;
+    }
+
+    AltitudeHoldStatusSet(&altitudeHoldStatus);
+
+    float throttle;
+    switch (altitudeHoldDesired.ControlMode) {
+    case ALTITUDEHOLDDESIRED_CONTROLMODE_THROTTLE:
+        throttle = altitudeHoldDesired.SetPoint;
+        break;
+    default:
+        // velocity control loop
+        throttle = startThrottle - pid_apply_setpoint(&pid1, 1.0f, altitudeHoldStatus.VelocityDesired, velocityStateDown, 1000.0f / DESIRED_UPDATE_RATE_MS);
+
+        // compensate throttle for current attitude
+        // Rbe[2][2] in the rotation matrix is the rotated down component of a
+        // 0,0,1 vector
+        // it is used to scale the throttle, but only up to 4 times the regular
+        // throttle
+        AttitudeStateData attitudeState;
+        AttitudeStateGet(&attitudeState);
+        float Rbe[3][3];
+        Quaternion2R(&attitudeState.q1, Rbe);
+        if (fabsf(Rbe[2][2]) > 0.25f) {
+            throttle /= Rbe[2][2];
+        } else {
+            throttle /= 0.25f;
+        }
+
+        if (throttle >= 1.0f) {
+            throttle = 1.0f;
+        }
+        if (throttle <= 0.0f) {
+            throttle = 0.0f;
+        }
+        break;
+    }
+
+    StabilizationDesiredData stab;
+    StabilizationDesiredGet(&stab);
+    stab.Roll     = altitudeHoldDesired.Roll;
+    stab.Pitch    = altitudeHoldDesired.Pitch;
+    stab.Yaw      = altitudeHoldDesired.Yaw;
+    stab.Throttle = throttle;
+    stab.StabilizationMode.Roll  = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+    stab.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
+    stab.StabilizationMode.Yaw   = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
+
+    StabilizationDesiredSet(&stab);
+
+    DelayedCallbackSchedule(altitudeHoldCBInfo, DESIRED_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER);
 }
 
 static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 {
     AltitudeHoldSettingsGet(&altitudeHoldSettings);
+    pid_configure(&pid0, altitudeHoldSettings.AltitudePI.Kp, altitudeHoldSettings.AltitudePI.Ki, 0, altitudeHoldSettings.AltitudePI.Ilimit);
+    pid_zero(&pid0);
+    pid_configure(&pid1, altitudeHoldSettings.VelocityPI.Kp, altitudeHoldSettings.VelocityPI.Ki, 0, altitudeHoldSettings.VelocityPI.Ilimit);
+    pid_zero(&pid1);
 }

flight/modules/FlightPlan/flightplan.c

@@ -31,7 +31,6 @@
 
 #include <openpilot.h>
 
-#include "flightplan.h"
 #include "flightplanstatus.h"
 #include "flightplancontrol.h"
 #include "flightplansettings.h"

flight/modules/FlightPlan/inc/flightplan.h

@@ -1,36 +0,0 @@
-/**
- ******************************************************************************
- * @addtogroup OpenPilotModules OpenPilot Modules
- * @{
- * @addtogroup FlightPlan Flight Plan Module
- * @brief Executes flight plan scripts in Python
- * @{
- *
- * @file       flightplan.c
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
- * @brief      Executes flight plan scripts in Python
- *
- * @see        The GNU Public License (GPL) Version 3
- *
- *****************************************************************************/
-/*
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 3 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
- * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
- * for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
- */
-#ifndef FLIGHTPLAN_H
-#define FLIGHTPLAN_H
-
-int32_t FlightPlanInitialize();
-
-#endif // FLIGHTPLAN_H

flight/modules/ManualControl/manualcontrol.c

@@ -846,65 +846,60 @@ static void updateLandDesired(__attribute__((unused)) ManualControlCommandData *
  */
 static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed)
 {
-    const float DEADBAND      = 0.25f;
+    const float DEADBAND      = 0.20f;
     const float DEADBAND_HIGH = 1.0f / 2 + DEADBAND / 2;
     const float DEADBAND_LOW  = 1.0f / 2 - DEADBAND / 2;
 
-    // Stop updating AltitudeHoldDesired triggering a failsafe condition.
-    if (cmd->Throttle < 0) {
-        return;
-    }
-
     // this is the max speed in m/s at the extents of throttle
-    uint8_t throttleRate;
+    float throttleRate;
     uint8_t throttleExp;
 
     static uint8_t flightMode;
-    static portTickType lastSysTimeAH;
+    static bool newaltitude = true;
-    static bool zeroed = false;
-    portTickType thisSysTime;
-    float dT;
 
     FlightStatusFlightModeGet(&flightMode);
 
     AltitudeHoldDesiredData altitudeHoldDesiredData;
     AltitudeHoldDesiredGet(&altitudeHoldDesiredData);
 
-    if (flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD) {
+    AltitudeHoldSettingsThrottleExpGet(&throttleExp);
-        throttleExp  = 128;
+    AltitudeHoldSettingsThrottleRateGet(&throttleRate);
-        throttleRate = 0;
-    } else {
-        AltitudeHoldSettingsThrottleExpGet(&throttleExp);
-        AltitudeHoldSettingsThrottleRateGet(&throttleRate);
-    }
 
     StabilizationBankData stabSettings;
     StabilizationBankGet(&stabSettings);
 
-    thisSysTime   = xTaskGetTickCount();
+    PositionStateData posState;
-    dT = ((thisSysTime == lastSysTimeAH) ? 0.001f : (thisSysTime - lastSysTimeAH) * portTICK_RATE_MS * 0.001f);
+    PositionStateGet(&posState);
-    lastSysTimeAH = thisSysTime;
 
     altitudeHoldDesiredData.Roll  = cmd->Roll * stabSettings.RollMax;
     altitudeHoldDesiredData.Pitch = cmd->Pitch * stabSettings.PitchMax;
     altitudeHoldDesiredData.Yaw   = cmd->Yaw * stabSettings.ManualRate.Yaw;
 
-    float currentDown;
-    PositionStateDownGet(&currentDown);
     if (changed) {
-        // After not being in this mode for a while init at current height
+        newaltitude = true;
-        altitudeHoldDesiredData.Altitude = 0;
+    }
-        zeroed = false;
+
-    } else if (cmd->Throttle > DEADBAND_HIGH && zeroed) {
+    uint8_t cutOff;
+    AltitudeHoldSettingsCutThrottleWhenZeroGet(&cutOff);
+    if (cutOff && cmd->Throttle < 0) {
+        // Cut throttle if desired
+        altitudeHoldDesiredData.SetPoint    = cmd->Throttle;
+        altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_THROTTLE;
+        newaltitude = true;
+    } else if (flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO && cmd->Throttle > DEADBAND_HIGH) {
         // being the two band symmetrical I can divide by DEADBAND_LOW to scale it to a value betweeon 0 and 1
-        // then apply an "exp" f(x,k) = (k*x*x*x + x*(256−k)) / 256
+        // then apply an "exp" f(x,k) = (k*x*x*x + (255-k)*x) / 255
-        altitudeHoldDesiredData.Altitude += (throttleExp * powf((cmd->Throttle - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (256 - throttleExp)) / 256 * throttleRate * dT;
+        altitudeHoldDesiredData.SetPoint    = -((throttleExp * powf((cmd->Throttle - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (255 - throttleExp) * (cmd->Throttle - DEADBAND_HIGH) / DEADBAND_LOW) / 255 * throttleRate);
-    } else if (cmd->Throttle < DEADBAND_LOW && zeroed) {
+        altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_VELOCITY;
-        altitudeHoldDesiredData.Altitude -= (throttleExp * powf((DEADBAND_LOW - (cmd->Throttle < 0 ? 0 : cmd->Throttle)) / DEADBAND_LOW, 3) + (256 - throttleExp)) / 256 * throttleRate * dT;
+        newaltitude = true;
-    } else if (cmd->Throttle >= DEADBAND_LOW && cmd->Throttle <= DEADBAND_HIGH && (throttleRate != 0)) {
+    } else if (flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO && cmd->Throttle < DEADBAND_LOW) {
-        // Require the stick to enter the dead band before they can move height
+        altitudeHoldDesiredData.SetPoint    = -(-(throttleExp * powf((DEADBAND_LOW - (cmd->Throttle < 0 ? 0 : cmd->Throttle)) / DEADBAND_LOW, 3) + (255 - throttleExp) * (DEADBAND_LOW - cmd->Throttle) / DEADBAND_LOW) / 255 * throttleRate);
-        // Vario is not "engaged" when throttleRate == 0
+        altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_VELOCITY;
-        zeroed = true;
+        newaltitude = true;
+    } else if (newaltitude == true) {
+        altitudeHoldDesiredData.SetPoint    = posState.Down;
+        altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_ALTITUDE;
+        newaltitude = false;
     }
 
     AltitudeHoldDesiredSet(&altitudeHoldDesiredData);

flight/modules/PathPlanner/pathplanner.c

@@ -31,6 +31,7 @@
 
 #include "openpilot.h"
 
+#include "pathplan.h"
 #include "flightstatus.h"
 #include "airspeedstate.h"
 #include "pathaction.h"
@@ -40,23 +41,26 @@
 #include "velocitystate.h"
 #include "waypoint.h"
 #include "waypointactive.h"
-#include "taskinfo.h"
+#include "manualcontrolsettings.h"
 #include <pios_struct_helper.h>
 #include "paths.h"
 
 // Private constants
 #define STACK_SIZE_BYTES            1024
-#define TASK_PRIORITY               (tskIDLE_PRIORITY + 1)
+#define TASK_PRIORITY               CALLBACK_TASK_NAVIGATION
 #define MAX_QUEUE_SIZE              2
-#define PATH_PLANNER_UPDATE_RATE_MS 20
+#define PATH_PLANNER_UPDATE_RATE_MS 100 // can be slow, since we listen to status updates as well
 
 // Private types
 
 // Private functions
-static void pathPlannerTask(void *parameters);
+static void pathPlannerTask();
-static void updatePathDesired(UAVObjEvent *ev);
+static void commandUpdated(UAVObjEvent *ev);
+static void statusUpdated(UAVObjEvent *ev);
+static void updatePathDesired();
 static void setWaypoint(uint16_t num);
 
+static uint8_t checkPathPlan();
 static uint8_t pathConditionCheck();
 static uint8_t conditionNone();
 static uint8_t conditionTimeOut();
@@ -71,7 +75,8 @@ static uint8_t conditionImmediate();
 
 
 // Private variables
-static xTaskHandle taskHandle;
+static DelayedCallbackInfo *pathPlannerHandle;
+static DelayedCallbackInfo *pathDesiredUpdaterHandle;
 static WaypointActiveData waypointActive;
 static WaypointData waypoint;
 static PathActionData pathAction;
@@ -83,11 +88,14 @@ static bool pathplanner_active = false;
  */
 int32_t PathPlannerStart()
 {
-    taskHandle = NULL;
+    // when the active waypoint changes, update pathDesired
+    WaypointConnectCallback(commandUpdated);
+    WaypointActiveConnectCallback(commandUpdated);
+    PathActionConnectCallback(commandUpdated);
+    PathStatusConnectCallback(statusUpdated);
 
-    // Start VM thread
+    // Start main task callback
-    xTaskCreate(pathPlannerTask, (signed char *)"PathPlanner", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle);
+    DelayedCallbackDispatch(pathPlannerHandle);
-    PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_PATHPLANNER, taskHandle);
 
     return 0;
 }
@@ -97,8 +105,7 @@ int32_t PathPlannerStart()
  */
 int32_t PathPlannerInitialize()
 {
-    taskHandle = NULL;
+    PathPlanInitialize();
-
     PathActionInitialize();
     PathStatusInitialize();
     PathDesiredInitialize();
@@ -108,6 +115,9 @@ int32_t PathPlannerInitialize()
     WaypointInitialize();
     WaypointActiveInitialize();
 
+    pathPlannerHandle = DelayedCallbackCreate(&pathPlannerTask, CALLBACK_PRIORITY_REGULAR, TASK_PRIORITY, STACK_SIZE_BYTES);
+    pathDesiredUpdaterHandle = DelayedCallbackCreate(&updatePathDesired, CALLBACK_PRIORITY_CRITICAL, TASK_PRIORITY, STACK_SIZE_BYTES);
+
     return 0;
 }
 
@@ -116,129 +126,250 @@ MODULE_INITCALL(PathPlannerInitialize, PathPlannerStart);
 /**
  * Module task
  */
-static void pathPlannerTask(__attribute__((unused)) void *parameters)
+static void pathPlannerTask()
 {
-    // when the active waypoint changes, update pathDesired
+    DelayedCallbackSchedule(pathPlannerHandle, PATH_PLANNER_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER);
-    WaypointConnectCallback(updatePathDesired);
+
-    WaypointActiveConnectCallback(updatePathDesired);
+    bool endCondition = false;
-    PathActionConnectCallback(updatePathDesired);
+
+    // check path plan validity early to raise alarm
+    // even if not in guided mode
+    uint8_t validPathPlan = checkPathPlan();
 
     FlightStatusData flightStatus;
+    FlightStatusGet(&flightStatus);
+    if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) {
+        pathplanner_active = false;
+        if (!validPathPlan) {
+            // unverified path plans are only a warning while we are not in pathplanner mode
+            // so it does not prevent arming. However manualcontrols safety check
+            // shall test for this warning when pathplan is on the flight mode selector
+            // thus a valid flight plan is a prerequirement for arming
+            AlarmsSet(SYSTEMALARMS_ALARM_PATHPLAN, SYSTEMALARMS_ALARM_WARNING);
+        } else {
+            AlarmsClear(SYSTEMALARMS_ALARM_PATHPLAN);
+        }
+
+        return;
+    }
+
     PathDesiredData pathDesired;
+    PathDesiredGet(&pathDesired);
+
+    static uint8_t failsafeRTHset = 0;
+    if (!validPathPlan) {
+        // At this point the craft is in PathPlanner mode, so pilot has no manual control capability.
+        // Failsafe: behave as if in return to base mode
+        // what to do in this case is debatable, it might be better to just
+        // make a forced disarming but rth has a higher chance of survival when
+        // in flight.
+        pathplanner_active = false;
+
+        if (!failsafeRTHset) {
+            failsafeRTHset = 1;
+            // copy pasta: same calculation as in manualcontrol, set return to home coordinates
+            PositionStateData positionState;
+            PositionStateGet(&positionState);
+            ManualControlSettingsData settings;
+            ManualControlSettingsGet(&settings);
+
+            pathDesired.Start.North      = 0;
+            pathDesired.Start.East       = 0;
+            pathDesired.Start.Down       = positionState.Down - settings.ReturnToHomeAltitudeOffset;
+            pathDesired.End.North        = 0;
+            pathDesired.End.East         = 0;
+            pathDesired.End.Down         = positionState.Down - settings.ReturnToHomeAltitudeOffset;
+            pathDesired.StartingVelocity = 1;
+            pathDesired.EndingVelocity   = 0;
+            pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
+            PathDesiredSet(&pathDesired);
+        }
+        AlarmsSet(SYSTEMALARMS_ALARM_PATHPLAN, SYSTEMALARMS_ALARM_ERROR);
+
+        return;
+    }
+    failsafeRTHset = 0;
+    AlarmsClear(SYSTEMALARMS_ALARM_PATHPLAN);
+
+    WaypointActiveGet(&waypointActive);
+
+    if (pathplanner_active == false) {
+        pathplanner_active   = true;
+
+        // This triggers callback to update variable
+        waypointActive.Index = 0;
+        WaypointActiveSet(&waypointActive);
+
+        return;
+    }
+
+    WaypointInstGet(waypointActive.Index, &waypoint);
+    PathActionInstGet(waypoint.Action, &pathAction);
     PathStatusData pathStatus;
+    PathStatusGet(&pathStatus);
 
-    // Main thread loop
+    // delay next step until path follower has acknowledged the path mode
-    bool endCondition = false;
+    if (pathStatus.UID != pathDesired.UID) {
-    while (1) {
+        return;
-        vTaskDelay(PATH_PLANNER_UPDATE_RATE_MS);
+    }
 
-        FlightStatusGet(&flightStatus);
+    // negative destinations DISABLE this feature
-        if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) {
+    if (pathStatus.Status == PATHSTATUS_STATUS_CRITICAL && waypointActive.Index != pathAction.ErrorDestination && pathAction.ErrorDestination >= 0) {
-            pathplanner_active = false;
+        setWaypoint(pathAction.ErrorDestination);
-            continue;
+        return;
+    }
+
+    // check if condition has been met
+    endCondition = pathConditionCheck();
+    // decide what to do
+    switch (pathAction.Command) {
+    case PATHACTION_COMMAND_ONNOTCONDITIONNEXTWAYPOINT:
+        endCondition = !endCondition;
+    case PATHACTION_COMMAND_ONCONDITIONNEXTWAYPOINT:
+        if (endCondition) {
+            setWaypoint(waypointActive.Index + 1);
         }
-
+        break;
-        WaypointActiveGet(&waypointActive);
+    case PATHACTION_COMMAND_ONNOTCONDITIONJUMPWAYPOINT:
-
+        endCondition = !endCondition;
-        if (pathplanner_active == false) {
+    case PATHACTION_COMMAND_ONCONDITIONJUMPWAYPOINT:
-            pathplanner_active   = true;
+        if (endCondition) {
-
+            if (pathAction.JumpDestination < 0) {
-            // This triggers callback to update variable
+                // waypoint ids <0 code relative jumps
-            waypointActive.Index = 0;
+                setWaypoint(waypointActive.Index - pathAction.JumpDestination);
-            WaypointActiveSet(&waypointActive);
-
-            continue;
-        }
-
-        WaypointInstGet(waypointActive.Index, &waypoint);
-        PathActionInstGet(waypoint.Action, &pathAction);
-        PathStatusGet(&pathStatus);
-        PathDesiredGet(&pathDesired);
-
-        // delay next step until path follower has acknowledged the path mode
-        if (pathStatus.UID != pathDesired.UID) {
-            continue;
-        }
-
-        // negative destinations DISABLE this feature
-        if (pathStatus.Status == PATHSTATUS_STATUS_CRITICAL && waypointActive.Index != pathAction.ErrorDestination && pathAction.ErrorDestination >= 0) {
-            setWaypoint(pathAction.ErrorDestination);
-            continue;
-        }
-
-        // check if condition has been met
-        endCondition = pathConditionCheck();
-
-        // decide what to do
-        switch (pathAction.Command) {
-        case PATHACTION_COMMAND_ONNOTCONDITIONNEXTWAYPOINT:
-            endCondition = !endCondition;
-        case PATHACTION_COMMAND_ONCONDITIONNEXTWAYPOINT:
-            if (endCondition) {
-                setWaypoint(waypointActive.Index + 1);
-            }
-            break;
-        case PATHACTION_COMMAND_ONNOTCONDITIONJUMPWAYPOINT:
-            endCondition = !endCondition;
-        case PATHACTION_COMMAND_ONCONDITIONJUMPWAYPOINT:
-            if (endCondition) {
-                if (pathAction.JumpDestination < 0) {
-                    // waypoint ids <0 code relative jumps
-                    setWaypoint(waypointActive.Index - pathAction.JumpDestination);
-                } else {
-                    setWaypoint(pathAction.JumpDestination);
-                }
-            }
-            break;
-        case PATHACTION_COMMAND_IFCONDITIONJUMPWAYPOINTELSENEXTWAYPOINT:
-            if (endCondition) {
-                if (pathAction.JumpDestination < 0) {
-                    // waypoint ids <0 code relative jumps
-                    setWaypoint(waypointActive.Index - pathAction.JumpDestination);
-                } else {
-                    setWaypoint(pathAction.JumpDestination);
-                }
             } else {
-                setWaypoint(waypointActive.Index + 1);
+                setWaypoint(pathAction.JumpDestination);
             }
-            break;
         }
+        break;
+    case PATHACTION_COMMAND_IFCONDITIONJUMPWAYPOINTELSENEXTWAYPOINT:
+        if (endCondition) {
+            if (pathAction.JumpDestination < 0) {
+                // waypoint ids <0 code relative jumps
+                setWaypoint(waypointActive.Index - pathAction.JumpDestination);
+            } else {
+                setWaypoint(pathAction.JumpDestination);
+            }
+        } else {
+            setWaypoint(waypointActive.Index + 1);
+        }
+        break;
     }
 }
 
+// safety checks for path plan integrity
+static uint8_t checkPathPlan()
+{
+    uint16_t i;
+    uint16_t waypointCount;
+    uint16_t actionCount;
+    uint8_t pathCrc;
+    PathPlanData pathPlan;
+
+    // WaypointData waypoint; // using global instead (?)
+    // PathActionData action; // using global instead (?)
+
+    PathPlanGet(&pathPlan);
+
+    waypointCount = pathPlan.WaypointCount;
+    if (waypointCount == 0) {
+        // an empty path plan is invalid
+        return false;
+    }
+    actionCount = pathPlan.PathActionCount;
+
+    // check count consistency
+    if (waypointCount > UAVObjGetNumInstances(WaypointHandle())) {
+        // PIOS_DEBUGLOG_Printf("PathPlan : waypoint count error!");
+        return false;
+    }
+    if (actionCount > UAVObjGetNumInstances(PathActionHandle())) {
+        // PIOS_DEBUGLOG_Printf("PathPlan : path action count error!");
+        return false;
+    }
+
+    // check CRC
+    pathCrc = 0;
+    for (i = 0; i < waypointCount; i++) {
+        pathCrc = UAVObjUpdateCRC(WaypointHandle(), i, pathCrc);
+    }
+    for (i = 0; i < actionCount; i++) {
+        pathCrc = UAVObjUpdateCRC(PathActionHandle(), i, pathCrc);
+    }
+    if (pathCrc != pathPlan.Crc) {
+        // failed crc check
+        // PIOS_DEBUGLOG_Printf("PathPlan : bad CRC (%d / %d)!", pathCrc, pathPlan.Crc);
+        return false;
+    }
+
+    // waypoint consistency
+    for (i = 0; i < waypointCount; i++) {
+        WaypointInstGet(i, &waypoint);
+        if (waypoint.Action >= actionCount) {
+            // path action id is out of range
+            return false;
+        }
+    }
+
+    // path action consistency
+    for (i = 0; i < actionCount; i++) {
+        PathActionInstGet(i, &pathAction);
+        if (pathAction.ErrorDestination >= waypointCount) {
+            // waypoint id is out of range
+            return false;
+        }
+        if (pathAction.JumpDestination >= waypointCount) {
+            // waypoint id is out of range
+            return false;
+        }
+    }
+
+    // path plan passed checks
+
+    return true;
+}
+
+// callback function when status changed, issue execution of state machine
+void commandUpdated(__attribute__((unused)) UAVObjEvent *ev)
+{
+    DelayedCallbackDispatch(pathDesiredUpdaterHandle);
+}
+
 // callback function when waypoints changed in any way, update pathDesired
-void updatePathDesired(__attribute__((unused)) UAVObjEvent *ev)
+void statusUpdated(__attribute__((unused)) UAVObjEvent *ev)
+{
+    DelayedCallbackDispatch(pathPlannerHandle);
+}
+
+
+// callback function when waypoints changed in any way, update pathDesired
+void updatePathDesired()
 {
     // only ever touch pathDesired if pathplanner is enabled
     if (!pathplanner_active) {
         return;
     }
 
-    // use local variables, dont use stack since this is huge and a callback,
+    PathDesiredData pathDesired;
-    // dont use the globals because we cant use mutexes here
-    static WaypointActiveData waypointActiveData;
-    static PathActionData pathActionData;
-    static WaypointData waypointData;
-    static PathDesiredData pathDesired;
 
     // find out current waypoint
-    WaypointActiveGet(&waypointActiveData);
+    WaypointActiveGet(&waypointActive);
 
-    WaypointInstGet(waypointActiveData.Index, &waypointData);
+    WaypointInstGet(waypointActive.Index, &waypoint);
-    PathActionInstGet(waypointData.Action, &pathActionData);
+    PathActionInstGet(waypoint.Action, &pathAction);
 
-    pathDesired.End.North = waypointData.Position.North;
+    pathDesired.End.North = waypoint.Position.North;
-    pathDesired.End.East  = waypointData.Position.East;
+    pathDesired.End.East  = waypoint.Position.East;
-    pathDesired.End.Down  = waypointData.Position.Down;
+    pathDesired.End.Down  = waypoint.Position.Down;
-    pathDesired.EndingVelocity    = waypointData.Velocity;
+    pathDesired.EndingVelocity    = waypoint.Velocity;
-    pathDesired.Mode = pathActionData.Mode;
+    pathDesired.Mode = pathAction.Mode;
-    pathDesired.ModeParameters[0] = pathActionData.ModeParameters[0];
+    pathDesired.ModeParameters[0] = pathAction.ModeParameters[0];
-    pathDesired.ModeParameters[1] = pathActionData.ModeParameters[1];
+    pathDesired.ModeParameters[1] = pathAction.ModeParameters[1];
-    pathDesired.ModeParameters[2] = pathActionData.ModeParameters[2];
+    pathDesired.ModeParameters[2] = pathAction.ModeParameters[2];
-    pathDesired.ModeParameters[3] = pathActionData.ModeParameters[3];
+    pathDesired.ModeParameters[3] = pathAction.ModeParameters[3];
-    pathDesired.UID = waypointActiveData.Index;
+    pathDesired.UID = waypointActive.Index;
 
-    if (waypointActiveData.Index == 0) {
+    if (waypointActive.Index == 0) {
         PositionStateData positionState;
         PositionStateGet(&positionState);
         // First waypoint has itself as start point (used to be home position but that proved dangerous when looping)
@@ -266,8 +397,13 @@ void updatePathDesired(__attribute__((unused)) UAVObjEvent *ev)
 // helper function to go to a specific waypoint
 static void setWaypoint(uint16_t num)
 {
-    // path plans wrap around
+    PathPlanData pathPlan;
-    if (num >= UAVObjGetNumInstances(WaypointHandle())) {
+
+    PathPlanGet(&pathPlan);
+
+    // here it is assumed that the path plan has been validated (waypoint count is consistent)
+    if (num >= pathPlan.WaypointCount) {
+        // path plans wrap around
         num = 0;
     }
 
@@ -275,10 +411,10 @@ static void setWaypoint(uint16_t num)
     WaypointActiveSet(&waypointActive);
 }
 
-// execute the apropriate condition and report result
+// execute the appropriate condition and report result
 static uint8_t pathConditionCheck()
 {
-    // i thought about a lookup table, but a switch is saver considering there could be invalid EndCondition ID's
+    // i thought about a lookup table, but a switch is safer considering there could be invalid EndCondition ID's
     switch (pathAction.EndCondition) {
     case PATHACTION_ENDCONDITION_NONE:
         return conditionNone();

flight/modules/RadioComBridge/RadioComBridge.c

@@ -1,5 +1,6 @@
 /**
  ******************************************************************************
+
  * @addtogroup OpenPilotModules OpenPilot Modules
  * @{
  * @addtogroup RadioComBridgeModule Com Port to Radio Bridge Module
@@ -36,6 +37,7 @@
 #include <objectpersistence.h>
 #include <oplinksettings.h>
 #include <oplinkreceiver.h>
+#include <radiocombridgestats.h>
 #include <uavtalk_priv.h>
 #include <pios_rfm22b.h>
 #include <ecc.h>
@@ -57,6 +59,7 @@
 #define SERIAL_RX_BUF_LEN 100
 #define PPM_INPUT_TIMEOUT 100
 
+
 // ****************
 // Private types
 
@@ -81,10 +84,11 @@ typedef struct {
     uint8_t  serialRxBuf[SERIAL_RX_BUF_LEN];
 
     // Error statistics.
-    uint32_t comTxErrors;
+    uint32_t telemetryTxRetries;
-    uint32_t comTxRetries;
+    uint32_t radioTxRetries;
-    uint32_t UAVTalkErrors;
+
-    uint32_t droppedPackets;
+    // Is this modem the coordinator
+    bool     isCoordinator;
 
     // Should we parse UAVTalk?
     bool     parseUAVTalk;
@@ -107,6 +111,7 @@ static int32_t RadioSendHandler(uint8_t *buf, int32_t length);
 static void ProcessTelemetryStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t rxbyte);
 static void ProcessRadioStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t rxbyte);
 static void objectPersistenceUpdatedCb(UAVObjEvent *objEv);
+static void registerObject(UAVObjHandle obj);
 
 // ****************
 // Private variables
@@ -124,12 +129,14 @@ static int32_t RadioComBridgeStart(void)
         // Get the settings.
         OPLinkSettingsData oplinkSettings;
         OPLinkSettingsGet(&oplinkSettings);
-        bool is_coordinator = (oplinkSettings.Coordinator == OPLINKSETTINGS_COORDINATOR_TRUE);
+
+        // Check if this is the coordinator modem
+        data->isCoordinator = (oplinkSettings.Coordinator == OPLINKSETTINGS_COORDINATOR_TRUE);
 
         // We will not parse/send UAVTalk if any ports are configured as Serial (except for over the USB HID port).
-        data->parseUAVTalk = ((oplinkSettings.MainPort != OPLINKSETTINGS_MAINPORT_SERIAL) &&
+        data->parseUAVTalk  = ((oplinkSettings.MainPort != OPLINKSETTINGS_MAINPORT_SERIAL) &&
-                              (oplinkSettings.FlexiPort != OPLINKSETTINGS_FLEXIPORT_SERIAL) &&
+                               (oplinkSettings.FlexiPort != OPLINKSETTINGS_FLEXIPORT_SERIAL) &&
-                              (oplinkSettings.VCPPort != OPLINKSETTINGS_VCPPORT_SERIAL));
+                               (oplinkSettings.VCPPort != OPLINKSETTINGS_VCPPORT_SERIAL));
 
         // Set the maximum radio RF power.
         switch (oplinkSettings.MaxRFPower) {
@@ -165,12 +172,16 @@ static int32_t RadioComBridgeStart(void)
         // Configure our UAVObjects for updates.
         UAVObjConnectQueue(UAVObjGetByID(OPLINKSTATUS_OBJID), data->uavtalkEventQueue, EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ);
         UAVObjConnectQueue(UAVObjGetByID(OBJECTPERSISTENCE_OBJID), data->uavtalkEventQueue, EV_UPDATED | EV_UPDATED_MANUAL);
-        if (is_coordinator) {
+        if (data->isCoordinator) {
             UAVObjConnectQueue(UAVObjGetByID(OPLINKRECEIVER_OBJID), data->radioEventQueue, EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ);
         } else {
             UAVObjConnectQueue(UAVObjGetByID(OPLINKRECEIVER_OBJID), data->uavtalkEventQueue, EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ);
         }
 
+        if (data->isCoordinator) {
+            registerObject(RadioComBridgeStatsHandle());
+        }
+
         // Configure the UAVObject callbacks
         ObjectPersistenceConnectCallback(&objectPersistenceUpdatedCb);
 
@@ -223,27 +234,94 @@ static int32_t RadioComBridgeInitialize(void)
     OPLinkStatusInitialize();
     ObjectPersistenceInitialize();
     OPLinkReceiverInitialize();
+    RadioComBridgeStatsInitialize();
 
     // Initialise UAVTalk
-    data->telemUAVTalkCon   = UAVTalkInitialize(&UAVTalkSendHandler);
+    data->telemUAVTalkCon    = UAVTalkInitialize(&UAVTalkSendHandler);
-    data->radioUAVTalkCon   = UAVTalkInitialize(&RadioSendHandler);
+    data->radioUAVTalkCon    = UAVTalkInitialize(&RadioSendHandler);
 
     // Initialize the queues.
-    data->uavtalkEventQueue = xQueueCreate(EVENT_QUEUE_SIZE, sizeof(UAVObjEvent));
+    data->uavtalkEventQueue  = xQueueCreate(EVENT_QUEUE_SIZE, sizeof(UAVObjEvent));
-    data->radioEventQueue   = xQueueCreate(EVENT_QUEUE_SIZE, sizeof(UAVObjEvent));
+    data->radioEventQueue    = xQueueCreate(EVENT_QUEUE_SIZE, sizeof(UAVObjEvent));
 
     // Initialize the statistics.
-    data->comTxErrors   = 0;
+    data->telemetryTxRetries = 0;
-    data->comTxRetries  = 0;
+    data->radioTxRetries     = 0;
-    data->UAVTalkErrors = 0;
+
-    data->parseUAVTalk  = true;
+    data->parseUAVTalk = true;
-    data->comSpeed = OPLINKSETTINGS_COMSPEED_9600;
+    data->comSpeed     = OPLINKSETTINGS_COMSPEED_9600;
-    PIOS_COM_RADIO = PIOS_COM_RFM22B;
+    PIOS_COM_RADIO     = PIOS_COM_RFM22B;
 
     return 0;
 }
 MODULE_INITCALL(RadioComBridgeInitialize, RadioComBridgeStart);
 
+
+// TODO this code (badly) duplicates code from telemetry.c
+// This method should be used only for periodically updated objects.
+// The register method defined in telemetry.c should be factored out in a shared location so it can be
+// used from here...
+static void registerObject(UAVObjHandle obj)
+{
+    // Setup object for periodic updates
+    UAVObjEvent ev = {
+        .obj    = obj,
+        .instId = UAVOBJ_ALL_INSTANCES,
+        .event  = EV_UPDATED_PERIODIC,
+    };
+
+    // Get metadata
+    UAVObjMetadata metadata;
+
+    UAVObjGetMetadata(obj, &metadata);
+
+    EventPeriodicQueueCreate(&ev, data->uavtalkEventQueue, metadata.telemetryUpdatePeriod);
+    UAVObjConnectQueue(obj, data->uavtalkEventQueue, EV_UPDATED_PERIODIC | EV_UPDATED_MANUAL | EV_UPDATE_REQ);
+}
+
+/**
+ * Update telemetry statistics
+ */
+static void updateRadioComBridgeStats()
+{
+    UAVTalkStats telemetryUAVTalkStats;
+    UAVTalkStats radioUAVTalkStats;
+    RadioComBridgeStatsData radioComBridgeStats;
+
+    // Get telemetry stats
+    UAVTalkGetStats(data->telemUAVTalkCon, &telemetryUAVTalkStats);
+    UAVTalkResetStats(data->telemUAVTalkCon);
+
+    // Get radio stats
+    UAVTalkGetStats(data->radioUAVTalkCon, &radioUAVTalkStats);
+    UAVTalkResetStats(data->radioUAVTalkCon);
+
+    // Get stats object data
+    RadioComBridgeStatsGet(&radioComBridgeStats);
+
+    // Update stats object
+    radioComBridgeStats.TelemetryTxBytes      += telemetryUAVTalkStats.txBytes;
+    radioComBridgeStats.TelemetryTxFailures   += telemetryUAVTalkStats.txErrors;
+    radioComBridgeStats.TelemetryTxRetries    += data->telemetryTxRetries;
+
+    radioComBridgeStats.TelemetryRxBytes      += telemetryUAVTalkStats.rxBytes;
+    radioComBridgeStats.TelemetryRxFailures   += telemetryUAVTalkStats.rxErrors;
+    radioComBridgeStats.TelemetryRxSyncErrors += telemetryUAVTalkStats.rxSyncErrors;
+    radioComBridgeStats.TelemetryRxCrcErrors  += telemetryUAVTalkStats.rxCrcErrors;
+
+    radioComBridgeStats.RadioTxBytes      += radioUAVTalkStats.txBytes;
+    radioComBridgeStats.RadioTxFailures   += radioUAVTalkStats.txErrors;
+    radioComBridgeStats.RadioTxRetries    += data->radioTxRetries;
+
+    radioComBridgeStats.RadioRxBytes      += radioUAVTalkStats.rxBytes;
+    radioComBridgeStats.RadioRxFailures   += radioUAVTalkStats.rxErrors;
+    radioComBridgeStats.RadioRxSyncErrors += radioUAVTalkStats.rxSyncErrors;
+    radioComBridgeStats.RadioRxCrcErrors  += radioUAVTalkStats.rxCrcErrors;
+
+    // Update stats object data
+    RadioComBridgeStatsSet(&radioComBridgeStats);
+}
+
 /**
  * @brief Telemetry transmit task, regular priority
  *
@@ -260,18 +338,20 @@ static void telemetryTxTask(__attribute__((unused)) void *parameters)
 #endif
         // Wait for queue message
         if (xQueueReceive(data->uavtalkEventQueue, &ev, MAX_PORT_DELAY) == pdTRUE) {
-            if ((ev.event == EV_UPDATED) || (ev.event == EV_UPDATE_REQ)) {
+            if (ev.obj == RadioComBridgeStatsHandle()) {
-                // Send update (with retries)
+                updateRadioComBridgeStats();
-                uint32_t retries = 0;
-                int32_t success  = -1;
-                while (retries < MAX_RETRIES && success == -1) {
-                    success = UAVTalkSendObject(data->telemUAVTalkCon, ev.obj, 0, 0, RETRY_TIMEOUT_MS) == 0;
-                    if (!success) {
-                        ++retries;
-                    }
-                }
-                data->comTxRetries += retries;
             }
+            // Send update (with retries)
+            uint32_t retries = 0;
+            int32_t success  = -1;
+            while (retries < MAX_RETRIES && success == -1) {
+                success = UAVTalkSendObject(data->telemUAVTalkCon, ev.obj, ev.instId, 0, RETRY_TIMEOUT_MS) == 0;
+                if (success == -1) {
+                    ++retries;
+                }
+            }
+            // Update stats
+            data->telemetryTxRetries += retries;
         }
     }
 }
@@ -299,12 +379,12 @@ static void radioTxTask(__attribute__((unused)) void *parameters)
                 uint32_t retries = 0;
                 int32_t success  = -1;
                 while (retries < MAX_RETRIES && success == -1) {
-                    success = UAVTalkSendObject(data->radioUAVTalkCon, ev.obj, 0, 0, RETRY_TIMEOUT_MS) == 0;
+                    success = UAVTalkSendObject(data->radioUAVTalkCon, ev.obj, ev.instId, 0, RETRY_TIMEOUT_MS) == 0;
-                    if (!success) {
+                    if (success == -1) {
                         ++retries;
                     }
                 }
-                data->comTxRetries += retries;
+                data->radioTxRetries += retries;
             }
         }
     }
@@ -333,6 +413,8 @@ static void radioRxTask(__attribute__((unused)) void *parameters)
                     }
                 } else if (PIOS_COM_TELEMETRY) {
                     // Send the data straight to the telemetry port.
+                    // FIXME following call can fail (with -2 error code) if buffer is full
+                    // it is the caller responsibility to retry in such cases...
                     PIOS_COM_SendBufferNonBlocking(PIOS_COM_TELEMETRY, serial_data, bytes_to_process);
                 }
             }
@@ -425,8 +507,10 @@ static void serialRxTask(__attribute__((unused)) void *parameters)
             // Receive some data.
             uint16_t bytes_to_process = PIOS_COM_ReceiveBuffer(inputPort, data->serialRxBuf, sizeof(data->serialRxBuf), MAX_PORT_DELAY);
 
-            // Send the data over the radio link.
             if (bytes_to_process > 0) {
+                // Send the data over the radio link.
+                // FIXME following call can fail (with -2 error code) if buffer is full
+                // it is the caller responsibility to retry in such cases...
                 PIOS_COM_SendBufferNonBlocking(PIOS_COM_RADIO, data->serialRxBuf, bytes_to_process);
             }
         } else {
@@ -445,6 +529,7 @@ static void serialRxTask(__attribute__((unused)) void *parameters)
  */
 static int32_t UAVTalkSendHandler(uint8_t *buf, int32_t length)
 {
+    int32_t ret;
     uint32_t outputPort = data->parseUAVTalk ? PIOS_COM_TELEMETRY : 0;
 
 #if defined(PIOS_INCLUDE_USB)
@@ -454,10 +539,13 @@ static int32_t UAVTalkSendHandler(uint8_t *buf, int32_t length)
     }
 #endif /* PIOS_INCLUDE_USB */
     if (outputPort) {
-        return PIOS_COM_SendBufferNonBlocking(outputPort, buf, length);
+        // FIXME following call can fail (with -2 error code) if buffer is full
+        // it is the caller responsibility to retry in such cases...
+        ret = PIOS_COM_SendBufferNonBlocking(outputPort, buf, length);
     } else {
-        return -1;
+        ret = -1;
     }
+    return ret;
 }
 
 /**
@@ -477,10 +565,11 @@ static int32_t RadioSendHandler(uint8_t *buf, int32_t length)
 
     // Don't send any data unless the radio port is available.
     if (outputPort && PIOS_COM_Available(outputPort)) {
+        // FIXME following call can fail (with -2 error code) if buffer is full
+        // it is the caller responsibility to retry in such cases...
         return PIOS_COM_SendBufferNonBlocking(outputPort, buf, length);
     } else {
-        // For some reason, if this function returns failure, it prevents saving settings.
+        return -1;
-        return length;
     }
 }
 
@@ -494,12 +583,40 @@ static int32_t RadioSendHandler(uint8_t *buf, int32_t length)
 static void ProcessTelemetryStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t rxbyte)
 {
     // Keep reading until we receive a completed packet.
-    UAVTalkRxState state = UAVTalkProcessInputStream(inConnectionHandle, rxbyte);
+    UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(inConnectionHandle, rxbyte);
 
-    if (state == UAVTALK_STATE_ERROR) {
+    if (state == UAVTALK_STATE_COMPLETE) {
-        data->UAVTalkErrors++;
+        // We only want to unpack certain telemetry objects
-    } else if (state == UAVTALK_STATE_COMPLETE) {
+        uint32_t objId = UAVTalkGetPacketObjId(inConnectionHandle);
-        UAVTalkRelayPacket(inConnectionHandle, outConnectionHandle);
+        switch (objId) {
+        case OPLINKSTATUS_OBJID:
+        case OPLINKSETTINGS_OBJID:
+        case OPLINKRECEIVER_OBJID:
+        case MetaObjectId(OPLINKSTATUS_OBJID):
+        case MetaObjectId(OPLINKSETTINGS_OBJID):
+        case MetaObjectId(OPLINKRECEIVER_OBJID):
+            UAVTalkReceiveObject(inConnectionHandle);
+            break;
+        case OBJECTPERSISTENCE_OBJID:
+        case MetaObjectId(OBJECTPERSISTENCE_OBJID):
+            // receive object locally
+            // some objects will send back a response to telemetry
+            // FIXME:
+            // OPLM will ack or nack all objects requests and acked object sends
+            // Receiver will probably also ack / nack the same messages
+            // This has some consequences like :
+            // Second ack/nack will not match an open transaction or will apply to wrong transaction
+            // Question : how does GCS handle receiving the same object twice
+            // The OBJECTPERSISTENCE logic can be broken too if for example OPLM nacks and then REVO acks...
+            UAVTalkReceiveObject(inConnectionHandle);
+            // relay packet to remote modem
+            UAVTalkRelayPacket(inConnectionHandle, outConnectionHandle);
+            break;
+        default:
+            // all other packets are relayed to the remote modem
+            UAVTalkRelayPacket(inConnectionHandle, outConnectionHandle);
+            break;
+        }
     }
 }
 
@@ -515,19 +632,30 @@ static void ProcessRadioStream(UAVTalkConnection inConnectionHandle, UAVTalkConn
     // Keep reading until we receive a completed packet.
     UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(inConnectionHandle, rxbyte);
 
-    if (state == UAVTALK_STATE_ERROR) {
+    if (state == UAVTALK_STATE_COMPLETE) {
-        data->UAVTalkErrors++;
+        // We only want to unpack certain objects from the remote modem
-    } else if (state == UAVTALK_STATE_COMPLETE) {
+        // Similarly we only want to relay certain objects to the telemetry port
-        // We only want to unpack certain objects from the remote modem.
         uint32_t objId = UAVTalkGetPacketObjId(inConnectionHandle);
         switch (objId) {
         case OPLINKSTATUS_OBJID:
         case OPLINKSETTINGS_OBJID:
+        case MetaObjectId(OPLINKSTATUS_OBJID):
+        case MetaObjectId(OPLINKSETTINGS_OBJID):
+            // Ignore object...
+            // These objects are shadowed by the modem and are not transmitted to the telemetry port
+            // - OPLINKSTATUS_OBJID : ground station will receive the OPLM link status instead
+            // - OPLINKSETTINGS_OBJID : ground station will read and write the OPLM settings instead
             break;
         case OPLINKRECEIVER_OBJID:
+        case MetaObjectId(OPLINKRECEIVER_OBJID):
+            // Receive object locally
+            // These objects are received by the modem and are not transmitted to the telemetry port
+            // - OPLINKRECEIVER_OBJID : not sure why
+            // some objects will send back a response to the remote modem
             UAVTalkReceiveObject(inConnectionHandle);
             break;
         default:
+            // all other packets are relayed to the telemetry port
             UAVTalkRelayPacket(inConnectionHandle, outConnectionHandle);
             break;
         }
@@ -545,7 +673,7 @@ static void objectPersistenceUpdatedCb(__attribute__((unused)) UAVObjEvent *objE
 
     ObjectPersistenceGet(&obj_per);
 
-    // Is this concerning or setting object?
+    // Is this concerning our setting object?
     if (obj_per.ObjectID == OPLINKSETTINGS_OBJID) {
         // Is this a save, load, or delete?
         bool success = false;

flight/modules/StateEstimation/filterbaro.c

@@ -37,6 +37,7 @@
 // Private constants
 
 #define STACK_REQUIRED 64
+#define INIT_CYCLES    100
 
 // Private types
 struct data {
@@ -67,7 +68,7 @@ static int32_t init(stateFilter *self)
     struct data *this = (struct data *)self->localdata;
 
     this->baroOffset = 0.0f;
-    this->first_run  = 100;
+    this->first_run  = INIT_CYCLES;
 
     RevoSettingsInitialize();
     RevoSettingsBaroGPSOffsetCorrectionAlphaGet(&this->baroGPSOffsetCorrectionAlpha);
@@ -82,8 +83,8 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
     if (this->first_run) {
         // Initialize to current altitude reading at initial location
         if (IS_SET(state->updated, SENSORUPDATES_baro)) {
-            this->baroOffset = (100.f - this->first_run) / 100.f * this->baroOffset + (this->first_run / 100.f) * state->baro[0];
+            this->baroOffset = ((float)(INIT_CYCLES)-this->first_run) / (float)(INIT_CYCLES)*this->baroOffset + (this->first_run / (float)(INIT_CYCLES)) * state->baro[0];
-            this->baroAlt    = this->baroOffset;
+            this->baroAlt    = 0;
             this->first_run--;
             UNSET_MASK(state->updated, SENSORUPDATES_baro);
         }

flight/modules/Telemetry/telemetry.c

@@ -158,6 +158,7 @@ int32_t TelemetryInitialize(void)
 #endif
 
     // Create periodic event that will be used to update the telemetry stats
+    // FIXME STATS_UPDATE_PERIOD_MS is 4000ms while FlighTelemetryStats update period is 5000ms...
     txErrors  = 0;
     txRetries = 0;
     UAVObjEvent ev;
@@ -177,22 +178,10 @@ MODULE_INITCALL(TelemetryInitialize, TelemetryStart);
 static void registerObject(UAVObjHandle obj)
 {
     if (UAVObjIsMetaobject(obj)) {
-        /* Only connect change notifications for meta objects.  No periodic updates */
+        // Only connect change notifications for meta objects.  No periodic updates
         UAVObjConnectQueue(obj, priorityQueue, EV_MASK_ALL_UPDATES);
-        return;
     } else {
         // Setup object for periodic updates
-        UAVObjEvent ev = {
-            .obj    = obj,
-            .instId = UAVOBJ_ALL_INSTANCES,
-            .event  = EV_UPDATED_PERIODIC,
-        };
-        EventPeriodicQueueCreate(&ev, queue, 0);
-        ev.event = EV_LOGGING_PERIODIC;
-        EventPeriodicQueueCreate(&ev, queue, 0);
-
-
-        // Setup object for telemetry updates
         updateObject(obj, EV_NONE);
     }
 }
@@ -208,9 +197,8 @@ static void updateObject(UAVObjHandle obj, int32_t eventType)
     int32_t eventMask;
 
     if (UAVObjIsMetaobject(obj)) {
-        /* This function updates the periodic updates for the object.
+        // This function updates the periodic updates for the object.
-         * Meta Objects cannot have periodic updates.
+        // Meta Objects cannot have periodic updates.
-         */
         PIOS_Assert(false);
         return;
     }
@@ -298,7 +286,6 @@ static void processObjEvent(UAVObjEvent *ev)
 {
     UAVObjMetadata metadata;
     UAVObjUpdateMode updateMode;
-    FlightTelemetryStatsData flightStats;
     int32_t retries;
     int32_t success;
 
@@ -307,7 +294,6 @@ static void processObjEvent(UAVObjEvent *ev)
     } else if (ev->obj == GCSTelemetryStatsHandle()) {
         gcsTelemetryStatsUpdated();
     } else {
-        FlightTelemetryStatsGet(&flightStats);
         // Get object metadata
         UAVObjGetMetadata(ev->obj, &metadata);
         updateMode = UAVObjGetTelemetryUpdateMode(&metadata);
@@ -315,32 +301,41 @@ static void processObjEvent(UAVObjEvent *ev)
         // Act on event
         retries    = 0;
         success    = -1;
-        if ((ev->event == EV_UPDATED && (updateMode == UPDATEMODE_ONCHANGE || updateMode == UPDATEMODE_THROTTLED)) || ev->event == EV_UPDATED_MANUAL || ((ev->event == EV_UPDATED_PERIODIC) && (updateMode != UPDATEMODE_THROTTLED))) {
+        if ((ev->event == EV_UPDATED && (updateMode == UPDATEMODE_ONCHANGE || updateMode == UPDATEMODE_THROTTLED))
+            || ev->event == EV_UPDATED_MANUAL
+            || (ev->event == EV_UPDATED_PERIODIC && updateMode != UPDATEMODE_THROTTLED)) {
             // Send update to GCS (with retries)
             while (retries < MAX_RETRIES && success == -1) {
-                success = UAVTalkSendObject(uavTalkCon, ev->obj, ev->instId, UAVObjGetTelemetryAcked(&metadata), REQ_TIMEOUT_MS); // call blocks until ack is received or timeout
+                // call blocks until ack is received or timeout
-                ++retries;
+                success = UAVTalkSendObject(uavTalkCon, ev->obj, ev->instId, UAVObjGetTelemetryAcked(&metadata), REQ_TIMEOUT_MS);
+                if (success == -1) {
+                    ++retries;
+                }
             }
             // Update stats
-            txRetries += (retries - 1);
+            txRetries += retries;
             if (success == -1) {
                 ++txErrors;
             }
         } else if (ev->event == EV_UPDATE_REQ) {
             // Request object update from GCS (with retries)
             while (retries < MAX_RETRIES && success == -1) {
-                success = UAVTalkSendObjectRequest(uavTalkCon, ev->obj, ev->instId, REQ_TIMEOUT_MS); // call blocks until update is received or timeout
+                // call blocks until update is received or timeout
-                ++retries;
+                success = UAVTalkSendObjectRequest(uavTalkCon, ev->obj, ev->instId, REQ_TIMEOUT_MS);
+                if (success == -1) {
+                    ++retries;
+                }
             }
             // Update stats
-            txRetries += (retries - 1);
+            txRetries += retries;
             if (success == -1) {
                 ++txErrors;
             }
         }
         // If this is a metaobject then make necessary telemetry updates
         if (UAVObjIsMetaobject(ev->obj)) {
-            updateObject(UAVObjGetLinkedObj(ev->obj), EV_NONE); // linked object will be the actual object the metadata are for
+            // linked object will be the actual object the metadata are for
+            updateObject(UAVObjGetLinkedObj(ev->obj), EV_NONE);
         } else {
             if (updateMode == UPDATEMODE_THROTTLED) {
                 // If this is UPDATEMODE_THROTTLED, the event mask changes on every event.
@@ -351,7 +346,9 @@ static void processObjEvent(UAVObjEvent *ev)
     // Log UAVObject if necessary
     if (ev->obj) {
         updateMode = UAVObjGetLoggingUpdateMode(&metadata);
-        if ((ev->event == EV_UPDATED && (updateMode == UPDATEMODE_ONCHANGE || updateMode == UPDATEMODE_THROTTLED)) || ev->event == EV_LOGGING_MANUAL || ((ev->event == EV_LOGGING_PERIODIC) && (updateMode != UPDATEMODE_THROTTLED))) {
+        if ((ev->event == EV_UPDATED && (updateMode == UPDATEMODE_ONCHANGE || updateMode == UPDATEMODE_THROTTLED))
+            || ev->event == EV_LOGGING_MANUAL
+            || (ev->event == EV_LOGGING_PERIODIC && updateMode != UPDATEMODE_THROTTLED)) {
             if (ev->instId == UAVOBJ_ALL_INSTANCES) {
                 success = UAVObjGetNumInstances(ev->obj);
                 for (retries = 0; retries < success; retries++) {
@@ -484,12 +481,18 @@ static int32_t transmitData(uint8_t *data, int32_t length)
 static int32_t setUpdatePeriod(UAVObjHandle obj, int32_t updatePeriodMs)
 {
     UAVObjEvent ev;
+    int32_t ret;
 
-    // Add object for periodic updates
+    // Add or update object for periodic updates
     ev.obj    = obj;
     ev.instId = UAVOBJ_ALL_INSTANCES;
     ev.event  = EV_UPDATED_PERIODIC;
-    return EventPeriodicQueueUpdate(&ev, queue, updatePeriodMs);
+
+    ret = EventPeriodicQueueUpdate(&ev, queue, updatePeriodMs);
+    if (ret == -1) {
+        ret = EventPeriodicQueueCreate(&ev, queue, updatePeriodMs);
+    }
+    return ret;
 }
 
 /**
@@ -502,12 +505,18 @@ static int32_t setUpdatePeriod(UAVObjHandle obj, int32_t updatePeriodMs)
 static int32_t setLoggingPeriod(UAVObjHandle obj, int32_t updatePeriodMs)
 {
     UAVObjEvent ev;
+    int32_t ret;
 
-    // Add object for periodic updates
+    // Add or update object for periodic updates
     ev.obj    = obj;
     ev.instId = UAVOBJ_ALL_INSTANCES;
     ev.event  = EV_LOGGING_PERIODIC;
-    return EventPeriodicQueueUpdate(&ev, queue, updatePeriodMs);
+
+    ret = EventPeriodicQueueUpdate(&ev, queue, updatePeriodMs);
+    if (ret == -1) {
+        ret = EventPeriodicQueueCreate(&ev, queue, updatePeriodMs);
+    }
+    return ret;
 }
 
 /**
@@ -553,25 +562,33 @@ static void updateTelemetryStats()
 
     // Update stats object
     if (flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) {
-        flightStats.RxDataRate  = (float)utalkStats.rxBytes / ((float)STATS_UPDATE_PERIOD_MS / 1000.0f);
+        flightStats.TxDataRate    = (float)utalkStats.txBytes / ((float)STATS_UPDATE_PERIOD_MS / 1000.0f);
-        flightStats.TxDataRate  = (float)utalkStats.txBytes / ((float)STATS_UPDATE_PERIOD_MS / 1000.0f);
+        flightStats.TxBytes      += utalkStats.txBytes;
-        flightStats.RxFailures += utalkStats.rxErrors;
+        flightStats.TxFailures   += txErrors;
-        flightStats.TxFailures += txErrors;
+        flightStats.TxRetries    += txRetries;
-        flightStats.TxRetries  += txRetries;
+
-        txErrors = 0;
+        flightStats.RxDataRate    = (float)utalkStats.rxBytes / ((float)STATS_UPDATE_PERIOD_MS / 1000.0f);
-        txRetries = 0;
+        flightStats.RxBytes      += utalkStats.rxBytes;
+        flightStats.RxFailures   += utalkStats.rxErrors;
+        flightStats.RxSyncErrors += utalkStats.rxSyncErrors;
+        flightStats.RxCrcErrors  += utalkStats.rxCrcErrors;
     } else {
-        flightStats.RxDataRate = 0;
+        flightStats.TxDataRate   = 0;
-        flightStats.TxDataRate = 0;
+        flightStats.TxBytes      = 0;
-        flightStats.RxFailures = 0;
+        flightStats.TxFailures   = 0;
-        flightStats.TxFailures = 0;
+        flightStats.TxRetries    = 0;
-        flightStats.TxRetries  = 0;
+
-        txErrors = 0;
+        flightStats.RxDataRate   = 0;
-        txRetries = 0;
+        flightStats.RxBytes      = 0;
+        flightStats.RxFailures   = 0;
+        flightStats.RxSyncErrors = 0;
+        flightStats.RxCrcErrors  = 0;
     }
+    txErrors  = 0;
+    txRetries = 0;
 
     // Check for connection timeout
-    timeNow = xTaskGetTickCount() * portTICK_RATE_MS;
+    timeNow   = xTaskGetTickCount() * portTICK_RATE_MS;
     if (utalkStats.rxObjects > 0) {
         timeOfLastObjectUpdate = timeNow;
     }

flight/pios/common/pios_crc.c

@@ -64,6 +64,17 @@ static const uint16_t CRC_Table16[] = { // HDLC polynomial
     0x7bc7, 0x6a4e, 0x58d5, 0x495c, 0x3de3, 0x2c6a, 0x1ef1, 0x0f78
 };
 
+/**
+ * Generated by pycrc v0.7.5, http://www.tty1.net/pycrc/
+ * using the configuration:
+ *    Width        = 8
+ *    Poly         = 0x07
+ *    XorIn        = 0x00
+ *    ReflectIn    = False
+ *    XorOut       = 0x00
+ *    ReflectOut   = False
+ *    Algorithm    = table-driven
+ */
 static const uint32_t CRC_Table32[] = {
     0x00000000, 0x04c11db7, 0x09823b6e, 0x0d4326d9, 0x130476dc, 0x17c56b6b, 0x1a864db2, 0x1e475005,
     0x2608edb8, 0x22c9f00f, 0x2f8ad6d6, 0x2b4bcb61, 0x350c9b64, 0x31cd86d3, 0x3c8ea00a, 0x384fbdbd,
@@ -101,17 +112,6 @@ static const uint32_t CRC_Table32[] = {
 
 /**
  * Update the crc value with new data.
- *
- * Generated by pycrc v0.7.5, http://www.tty1.net/pycrc/
- * using the configuration:
- *    Width        = 8
- *    Poly         = 0x07
- *    XorIn        = 0x00
- *    ReflectIn    = False
- *    XorOut       = 0x00
- *    ReflectOut   = False
- *    Algorithm    = table-driven
- *
  * \param crc      The current crc value.
  * \param data     Pointer to a buffer of \a data_len bytes.
  * \param length   Number of bytes in the \a data buffer.

flight/pios/common/pios_ms5611.c

@@ -52,6 +52,9 @@ static int32_t lastConversionStart;
 static int32_t PIOS_MS5611_Read(uint8_t address, uint8_t *buffer, uint8_t len);
 static int32_t PIOS_MS5611_WriteCommand(uint8_t command);
 
+// Second order temperature compensation. Temperature offset
+static int64_t compensation_t2;
+
 // Move into proper driver structure with cfg stored
 static uint32_t oversampling;
 static const struct pios_ms5611_cfg *dev_cfg;
@@ -74,6 +77,9 @@ void PIOS_MS5611_Init(const struct pios_ms5611_cfg *cfg, int32_t i2c_device)
 
     uint8_t data[2];
 
+    // reset temperature compensation values
+    compensation_t2 = 0;
+
     /* Calibration parameters */
     for (int i = 0; i < 6; i++) {
         PIOS_MS5611_Read(MS5611_CALIB_ADDR + i * 2, data, 2);
@@ -190,17 +196,34 @@ int32_t PIOS_MS5611_ReadADC(void)
     } else {
         int64_t Offset;
         int64_t Sens;
+        // used for second order temperature compensation
+        int64_t Offset2 = 0;
+        int64_t Sens2   = 0;
 
         /* Read the pressure conversion */
         if (PIOS_MS5611_Read(MS5611_ADC_READ, Data, 3) != 0) {
             return -1;
         }
+        // check if temperature is less than 20°C
+        if (Temperature < 2000) {
+            Offset2 = 5 * (Temperature - 2000) >> 1;
+            Sens2   = Offset2 >> 1;
+            compensation_t2 = (deltaTemp * deltaTemp) >> 31;
+            // Apply the "Very low temperature compensation" when temp is less than -15°C
+            if (Temperature < -1500) {
+                int64_t tcorr = (Temperature + 1500) * (Temperature + 1500);
+                Offset2 += 7 * tcorr;
+                Sens2   += (11 * tcorr) >> 1;
+            }
+        } else {
+            compensation_t2 = 0;
+            Offset2 = 0;
+            Sens2 = 0;
+        }
         RawPressure = ((Data[0] << 16) | (Data[1] << 8) | Data[2]);
-
+        Offset   = (((int64_t)CalibData.C[1]) << 16) + ((((int64_t)CalibData.C[3]) * deltaTemp) >> 7) - Offset2;
-        Offset   = (((int64_t)CalibData.C[1]) << 16) + ((((int64_t)CalibData.C[3]) * deltaTemp) >> 7);
         Sens     = ((int64_t)CalibData.C[0]) << 15;
-        Sens     = Sens + ((((int64_t)CalibData.C[2]) * deltaTemp) >> 8);
+        Sens     = Sens + ((((int64_t)CalibData.C[2]) * deltaTemp) >> 8) - Sens2;
-
         Pressure = (((((int64_t)RawPressure) * Sens) >> 21) - Offset) >> 15;
     }
     return 0;
@@ -211,7 +234,8 @@ int32_t PIOS_MS5611_ReadADC(void)
  */
 float PIOS_MS5611_GetTemperature(void)
 {
-    return ((float)Temperature) / 100.0f;
+    // Apply the second order low and very low temperature compensation offset
+    return ((float)(Temperature - compensation_t2)) / 100.0f;
 }
 
 /**

flight/pios/common/pios_rfm22b.c

@@ -1221,7 +1221,7 @@ static void pios_rfm22_inject_event(struct pios_rfm22b_dev *rfm22b_dev, enum pio
             // Something went fairly seriously wrong
             rfm22b_dev->errors++;
         }
-        portEND_SWITCHING_ISR((pxHigherPriorityTaskWoken2 == pdTRUE) || (pxHigherPriorityTaskWoken2 == pdTRUE));
+        portEND_SWITCHING_ISR((pxHigherPriorityTaskWoken1 == pdTRUE) || (pxHigherPriorityTaskWoken2 == pdTRUE));
     } else {
         // Store the event.
         if (xQueueSend(rfm22b_dev->eventQueue, &event, portMAX_DELAY) != pdTRUE) {

flight/targets/boards/oplinkmini/firmware/Makefile

@@ -53,6 +53,7 @@ ifndef TESTAPP
     SRC += $(OPUAVSYNTHDIR)/oplinksettings.c
     SRC += $(OPUAVSYNTHDIR)/objectpersistence.c
     SRC += $(OPUAVSYNTHDIR)/oplinkreceiver.c
+    SRC += $(OPUAVSYNTHDIR)/radiocombridgestats.c
 else
     ## Test Code
     SRC += $(OPTESTS)/test_common.c

flight/targets/boards/revolution/firmware/UAVObjects.inc

@@ -23,7 +23,6 @@ UAVOBJSRCFILENAMES += accessorydesired
 UAVOBJSRCFILENAMES += actuatorcommand
 UAVOBJSRCFILENAMES += actuatordesired
 UAVOBJSRCFILENAMES += actuatorsettings
-UAVOBJSRCFILENAMES += altholdsmoothed
 UAVOBJSRCFILENAMES += attitudesettings
 UAVOBJSRCFILENAMES += attitudestate
 UAVOBJSRCFILENAMES += gyrostate
@@ -70,6 +69,7 @@ UAVOBJSRCFILENAMES += overosyncstats
 UAVOBJSRCFILENAMES += overosyncsettings
 UAVOBJSRCFILENAMES += pathaction
 UAVOBJSRCFILENAMES += pathdesired
+UAVOBJSRCFILENAMES += pathplan
 UAVOBJSRCFILENAMES += pathstatus
 UAVOBJSRCFILENAMES += positionstate
 UAVOBJSRCFILENAMES += ratedesired
@@ -103,6 +103,7 @@ UAVOBJSRCFILENAMES += oplinksettings
 UAVOBJSRCFILENAMES += oplinkstatus
 UAVOBJSRCFILENAMES += altitudefiltersettings
 UAVOBJSRCFILENAMES += altitudeholddesired
+UAVOBJSRCFILENAMES += altitudeholdstatus
 UAVOBJSRCFILENAMES += waypoint
 UAVOBJSRCFILENAMES += waypointactive
 UAVOBJSRCFILENAMES += poilocation

flight/targets/boards/revolution/firmware/pios_board.c

@@ -133,7 +133,7 @@ static const struct pios_hmc5883_cfg pios_hmc5883_cfg = {
 #if defined(PIOS_INCLUDE_MS5611)
 #include "pios_ms5611.h"
 static const struct pios_ms5611_cfg pios_ms5611_cfg = {
-    .oversampling = MS5611_OSR_512,
+    .oversampling = MS5611_OSR_4096,
 };
 #endif /* PIOS_INCLUDE_MS5611 */
 

flight/targets/boards/revoproto/firmware/UAVObjects.inc

@@ -28,7 +28,6 @@ UAVOBJSRCFILENAMES += accessorydesired
 UAVOBJSRCFILENAMES += actuatorcommand
 UAVOBJSRCFILENAMES += actuatordesired
 UAVOBJSRCFILENAMES += actuatorsettings
-UAVOBJSRCFILENAMES += altholdsmoothed
 UAVOBJSRCFILENAMES += attitudesettings
 UAVOBJSRCFILENAMES += attitudestate
 UAVOBJSRCFILENAMES += gyrostate
@@ -70,6 +69,7 @@ UAVOBJSRCFILENAMES += overosyncstats
 UAVOBJSRCFILENAMES += overosyncsettings
 UAVOBJSRCFILENAMES += pathaction
 UAVOBJSRCFILENAMES += pathdesired
+UAVOBJSRCFILENAMES += pathplan
 UAVOBJSRCFILENAMES += pathstatus
 UAVOBJSRCFILENAMES += positionstate
 UAVOBJSRCFILENAMES += ratedesired
@@ -101,6 +101,7 @@ UAVOBJSRCFILENAMES += camerastabsettings
 UAVOBJSRCFILENAMES += altitudeholdsettings
 UAVOBJSRCFILENAMES += altitudefiltersettings
 UAVOBJSRCFILENAMES += altitudeholddesired
+UAVOBJSRCFILENAMES += altitudeholdstatus
 UAVOBJSRCFILENAMES += waypoint
 UAVOBJSRCFILENAMES += waypointactive
 UAVOBJSRCFILENAMES += poilocation

flight/targets/boards/revoproto/firmware/pios_board.c

@@ -129,7 +129,7 @@ static const struct pios_hmc5883_cfg pios_hmc5883_cfg = {
 #if defined(PIOS_INCLUDE_MS5611)
 #include "pios_ms5611.h"
 static const struct pios_ms5611_cfg pios_ms5611_cfg = {
-    .oversampling = MS5611_OSR_512,
+    .oversampling = MS5611_OSR_4096,
 };
 #endif /* PIOS_INCLUDE_MS5611 */
 

flight/targets/boards/simposix/firmware/Makefile

@@ -42,6 +42,7 @@ MODULES += FirmwareIAP
 MODULES += StateEstimation
 #MODULES += Sensors/simulated/Sensors
 MODULES += Airspeed
+MODULES += AltitudeHold
 #MODULES += OveroSync
 
 # Paths
@@ -68,7 +69,7 @@ UAVOBJSYNTHDIR = $(OUTDIR)/../uavobject-synthetics/flight
 
 # optional component libraries
 include $(PIOS)/common/libraries/FreeRTOS/library.mk
-include $(FLIGHTLIB)/PyMite/pymite.mk
+#include $(FLIGHTLIB)/PyMite/pymite.mk
 
 # List C source files here. (C dependencies are automatically generated.)
 # use file-extension c for "c-only"-files

flight/targets/boards/simposix/firmware/UAVObjects.inc

@@ -28,7 +28,6 @@ UAVOBJSRCFILENAMES += accessorydesired
 UAVOBJSRCFILENAMES += actuatorcommand
 UAVOBJSRCFILENAMES += actuatordesired
 UAVOBJSRCFILENAMES += actuatorsettings
-UAVOBJSRCFILENAMES += altholdsmoothed
 UAVOBJSRCFILENAMES += attitudesettings
 UAVOBJSRCFILENAMES += attitudestate
 UAVOBJSRCFILENAMES += attitudesimulated
@@ -73,6 +72,7 @@ UAVOBJSRCFILENAMES += objectpersistence
 UAVOBJSRCFILENAMES += overosyncstats
 UAVOBJSRCFILENAMES += pathaction
 UAVOBJSRCFILENAMES += pathdesired
+UAVOBJSRCFILENAMES += pathplan
 UAVOBJSRCFILENAMES += pathstatus
 UAVOBJSRCFILENAMES += positionstate
 UAVOBJSRCFILENAMES += ratedesired
@@ -106,6 +106,7 @@ UAVOBJSRCFILENAMES += altitudeholdsettings
 UAVOBJSRCFILENAMES += altitudefiltersettings
 UAVOBJSRCFILENAMES += revosettings
 UAVOBJSRCFILENAMES += altitudeholddesired
+UAVOBJSRCFILENAMES += altitudeholdstatus
 UAVOBJSRCFILENAMES += ekfconfiguration
 UAVOBJSRCFILENAMES += ekfstatevariance
 

flight/uavobjects/inc/uavobjectmanager.h

@@ -49,6 +49,8 @@
 
 typedef void *UAVObjHandle;
 
+#define MetaObjectId(id) ((id) + 1)
+
 /**
  * Object update mode, used by multiple modules (e.g. telemetry and logger)
  */
@@ -158,6 +160,7 @@ bool UAVObjIsMetaobject(UAVObjHandle obj);
 bool UAVObjIsSettings(UAVObjHandle obj);
 int32_t UAVObjUnpack(UAVObjHandle obj_handle, uint16_t instId, const uint8_t *dataIn);
 int32_t UAVObjPack(UAVObjHandle obj_handle, uint16_t instId, uint8_t *dataOut);
+uint8_t UAVObjUpdateCRC(UAVObjHandle obj_handle, uint16_t instId, uint8_t crc);
 int32_t UAVObjSave(UAVObjHandle obj_handle, uint16_t instId);
 int32_t UAVObjLoad(UAVObjHandle obj_handle, uint16_t instId);
 int32_t UAVObjDelete(UAVObjHandle obj_handle, uint16_t instId);

flight/uavobjects/uavobjectmanager.c

@@ -167,22 +167,19 @@ struct UAVOMulti {
 #define MetaObjectPtr(obj)               ((struct UAVODataMeta *)&((obj)->metaObj))
 #define MetaDataPtr(obj)                 ((UAVObjMetadata *)&((obj)->instance0))
 #define LinkedMetaDataPtr(obj)           ((UAVObjMetadata *)&((obj)->metaObj.instance0))
-#define MetaObjectId(id)                 ((id) + 1)
 
 /** all information about instances are dependant on object type **/
 #define ObjSingleInstanceDataOffset(obj) ((void *)(&(((struct UAVOSingle *)obj)->instance0)))
 #define InstanceDataOffset(inst)         ((void *)&(((struct UAVOMultiInst *)inst)->instance))
-#define InstanceData(instance)           (void *)instance
+#define InstanceData(instance)           ((void *)instance)
 
 // Private functions
-static int32_t sendEvent(struct UAVOBase *obj, uint16_t instId,
+static int32_t sendEvent(struct UAVOBase *obj, uint16_t instId, UAVObjEventType event);
-                         UAVObjEventType event);
 static InstanceHandle createInstance(struct UAVOData *obj, uint16_t instId);
 static InstanceHandle getInstance(struct UAVOData *obj, uint16_t instId);
-static int32_t connectObj(UAVObjHandle obj_handle, xQueueHandle queue,
+static int32_t connectObj(UAVObjHandle obj_handle, xQueueHandle queue, UAVObjEventCallback cb, uint8_t eventMask);
-                          UAVObjEventCallback cb, uint8_t eventMask);
+static int32_t disconnectObj(UAVObjHandle obj_handle, xQueueHandle queue, UAVObjEventCallback cb);
-static int32_t disconnectObj(UAVObjHandle obj_handle, xQueueHandle queue,
+static void instanceAutoUpdated(UAVObjHandle obj_handle, uint16_t instId);
-                             UAVObjEventCallback cb);
 
 // Private variables
 static xSemaphoreHandle mutex;
@@ -390,8 +387,8 @@ UAVObjHandle UAVObjRegister(uint32_t id,
     }
 
     // fire events for outer object and its embedded meta object
-    UAVObjInstanceUpdated((UAVObjHandle)uavo_data, 0);
+    instanceAutoUpdated((UAVObjHandle)uavo_data, 0);
-    UAVObjInstanceUpdated((UAVObjHandle) & (uavo_data->metaObj), 0);
+    instanceAutoUpdated((UAVObjHandle) & (uavo_data->metaObj), 0);
 
 unlock_exit:
     xSemaphoreGiveRecursive(mutex);
@@ -615,8 +612,7 @@ bool UAVObjIsSettings(UAVObjHandle obj_handle)
  * \param[in] dataIn The byte array
  * \return 0 if success or -1 if failure
  */
-int32_t UAVObjUnpack(UAVObjHandle obj_handle, uint16_t instId,
+int32_t UAVObjUnpack(UAVObjHandle obj_handle, uint16_t instId, const uint8_t *dataIn)
-                     const uint8_t *dataIn)
 {
     PIOS_Assert(obj_handle);
 
@@ -704,6 +700,45 @@ unlock_exit:
     return rc;
 }
 
+/**
+ * Update a CRC with an object data
+ * \param[in] obj The object handle
+ * \param[in] instId The instance ID
+ * \param[in] crc The crc to update
+ * \return the updated crc
+ */
+uint8_t UAVObjUpdateCRC(UAVObjHandle obj_handle, uint16_t instId, uint8_t crc)
+{
+    PIOS_Assert(obj_handle);
+
+    // Lock
+    xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
+
+    if (UAVObjIsMetaobject(obj_handle)) {
+        if (instId != 0) {
+            goto unlock_exit;
+        }
+        // TODO
+    } else {
+        struct UAVOData *obj;
+        InstanceHandle instEntry;
+
+        // Cast handle to object
+        obj = (struct UAVOData *)obj_handle;
+
+        // Get the instance
+        instEntry = getInstance(obj, instId);
+        if (instEntry == NULL) {
+            goto unlock_exit;
+        }
+        // Update crc
+        crc = PIOS_CRC_updateCRC(crc, (uint8_t *)InstanceData(instEntry), (int32_t)obj->instance_size);
+    }
+
+unlock_exit:
+    xSemaphoreGiveRecursive(mutex);
+    return crc;
+}
 
 /**
  * Actually write the object's data to the logfile
@@ -1582,8 +1617,8 @@ void UAVObjRequestUpdate(UAVObjHandle obj_handle)
 }
 
 /**
- * Request an update of the object's data from the GCS. The call will not wait for the response, a EV_UPDATED event
+ * Request an update of the object's data from the GCS.
- * will be generated as soon as the object is updated.
+ * The call will not wait for the response, a EV_UPDATED event will be generated as soon as the object is updated.
  * \param[in] obj The object handle
  * \param[in] instId Object instance ID to update
  */
@@ -1596,7 +1631,7 @@ void UAVObjRequestInstanceUpdate(UAVObjHandle obj_handle, uint16_t instId)
 }
 
 /**
- * Send the object's data to the GCS (triggers a EV_UPDATED_MANUAL event on this object).
+ * Trigger a EV_UPDATED_MANUAL event for an object.
  * \param[in] obj The object handle
  */
 void UAVObjUpdated(UAVObjHandle obj_handle)
@@ -1605,7 +1640,7 @@ void UAVObjUpdated(UAVObjHandle obj_handle)
 }
 
 /**
- * Send the object's data to the GCS (triggers a EV_UPDATED_MANUAL event on this object).
+ * Trigger a EV_UPDATED_MANUAL event for an object instance.
  * \param[in] obj The object handle
  * \param[in] instId The object instance ID
  */
@@ -1618,6 +1653,19 @@ void UAVObjInstanceUpdated(UAVObjHandle obj_handle, uint16_t instId)
 }
 
 /**
+ * Trigger a EV_UPDATED event for an object instance.
+ * \param[in] obj The object handle
+ * \param[in] instId The object instance ID
+ */
+static void instanceAutoUpdated(UAVObjHandle obj_handle, uint16_t instId)
+{
+    PIOS_Assert(obj_handle);
+    xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
+    sendEvent((struct UAVOBase *)obj_handle, instId, EV_UPDATED);
+    xSemaphoreGiveRecursive(mutex);
+}
+
+/*
  * Log the object's data (triggers a EV_LOGGING_MANUAL event on this object).
  * \param[in] obj The object handle
  */
@@ -1664,8 +1712,7 @@ xSemaphoreGiveRecursive(mutex);
 /**
  * Send a triggered event to all event queues registered on the object.
  */
-static int32_t sendEvent(struct UAVOBase *obj, uint16_t instId,
+static int32_t sendEvent(struct UAVOBase *obj, uint16_t instId, UAVObjEventType triggered_event)
-                         UAVObjEventType triggered_event)
 {
     /* Set up the message that will be sent to all registered listeners */
     UAVObjEvent msg = {
@@ -1678,14 +1725,13 @@ static int32_t sendEvent(struct UAVOBase *obj, uint16_t instId,
     struct ObjectEventEntry *event;
 
     LL_FOREACH(obj->next_event, event) {
-        if (event->eventMask == 0
+        if (event->eventMask == 0 || (event->eventMask & triggered_event) != 0) {
-            || (event->eventMask & triggered_event) != 0) {
             // Send to queue if a valid queue is registered
             if (event->queue) {
                 // will not block
                 if (xQueueSend(event->queue, &msg, 0) != pdTRUE) {
-                    stats.lastQueueErrorID = UAVObjGetID(obj);
                     ++stats.eventQueueErrors;
+                    stats.lastQueueErrorID = UAVObjGetID(obj);
                 }
             }
 
@@ -1745,7 +1791,7 @@ static InstanceHandle createInstance(struct UAVOData *obj, uint16_t instId)
     ((struct UAVOMulti *)obj)->num_instances++;
 
     // Fire event
-    UAVObjInstanceUpdated((UAVObjHandle)obj, instId);
+    instanceAutoUpdated((UAVObjHandle)obj, instId);
 
     // Done
     return InstanceDataOffset(instEntry);

flight/uavtalk/inc/uavtalk.h

@@ -34,13 +34,16 @@ typedef int32_t (*UAVTalkOutputStream)(uint8_t *data, int32_t length);
 
 typedef struct {
     uint32_t txBytes;
-    uint32_t rxBytes;
     uint32_t txObjectBytes;
-    uint32_t rxObjectBytes;
-    uint32_t rxObjects;
     uint32_t txObjects;
     uint32_t txErrors;
+
+    uint32_t rxBytes;
+    uint32_t rxObjectBytes;
+    uint32_t rxObjects;
     uint32_t rxErrors;
+    uint32_t rxSyncErrors;
+    uint32_t rxCrcErrors;
 } UAVTalkStats;
 
 typedef void *UAVTalkConnection;
@@ -54,9 +57,6 @@ UAVTalkOutputStream UAVTalkGetOutputStream(UAVTalkConnection connection);
 int32_t UAVTalkSendObject(UAVTalkConnection connection, UAVObjHandle obj, uint16_t instId, uint8_t acked, int32_t timeoutMs);
 int32_t UAVTalkSendObjectTimestamped(UAVTalkConnection connectionHandle, UAVObjHandle obj, uint16_t instId, uint8_t acked, int32_t timeoutMs);
 int32_t UAVTalkSendObjectRequest(UAVTalkConnection connection, UAVObjHandle obj, uint16_t instId, int32_t timeoutMs);
-int32_t UAVTalkSendAck(UAVTalkConnection connectionHandle, UAVObjHandle obj, uint16_t instId);
-int32_t UAVTalkSendNack(UAVTalkConnection connectionHandle, uint32_t objId);
-int32_t UAVTalkSendBuf(UAVTalkConnection connectionHandle, uint8_t *buf, uint16_t len);
 UAVTalkRxState UAVTalkProcessInputStream(UAVTalkConnection connection, uint8_t rxbyte);
 UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connection, uint8_t rxbyte);
 UAVTalkRxState UAVTalkRelayPacket(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle);

flight/uavtalk/inc/uavtalk_priv.h

@@ -32,38 +32,26 @@
 #include "uavobjectsinit.h"
 
 // Private types and constants
-typedef struct {
-    uint8_t  sync;
-    uint8_t  type;
-    uint16_t size;
-    uint32_t objId;
-} uavtalk_min_header;
-#define UAVTALK_MIN_HEADER_LENGTH sizeof(uavtalk_min_header)
 
-typedef struct {
+// min header : sync(1), type (1), size(2), object ID(4), instance ID(2)
-    uint8_t  sync;
+#define UAVTALK_MIN_HEADER_LENGTH  10
-    uint8_t  type;
+
-    uint16_t size;
+// max header : sync(1), type (1), size(2), object ID(4), instance ID(2), timestamp(2)
-    uint32_t objId;
+#define UAVTALK_MAX_HEADER_LENGTH  12
-    uint16_t instId;
+
-    uint16_t timestamp;
+#define UAVTALK_CHECKSUM_LENGTH    1
-} uavtalk_max_header;
-#define UAVTALK_MAX_HEADER_LENGTH  sizeof(uavtalk_max_header)
 
-typedef uint8_t uavtalk_checksum;
-#define UAVTALK_CHECKSUM_LENGTH    sizeof(uavtalk_checksum)
 #define UAVTALK_MAX_PAYLOAD_LENGTH (UAVOBJECTS_LARGEST + 1)
+
 #define UAVTALK_MIN_PACKET_LENGTH  UAVTALK_MAX_HEADER_LENGTH + UAVTALK_CHECKSUM_LENGTH
 #define UAVTALK_MAX_PACKET_LENGTH  UAVTALK_MIN_PACKET_LENGTH + UAVTALK_MAX_PAYLOAD_LENGTH
 
 typedef struct {
-    UAVObjHandle obj;
+    uint8_t  type;
-    uint8_t type;
+    uint16_t packet_size;
-    uint16_t     packet_size;
+    uint32_t objId;
-    uint32_t     objId;
+    uint16_t instId;
-    uint16_t     instId;
+    uint32_t length;
-    uint32_t     length;
-    uint8_t  instanceLength;
     uint8_t  timestampLength;
     uint8_t  cs;
     uint16_t timestamp;
@@ -78,19 +66,18 @@ typedef struct {
     xSemaphoreHandle    lock;
     xSemaphoreHandle    transLock;
     xSemaphoreHandle    respSema;
-    UAVObjHandle respObj;
+    uint8_t      respType;
+    uint32_t     respObjId;
     uint16_t     respInstId;
     UAVTalkStats stats;
     UAVTalkInputProcessor iproc;
     uint8_t      *rxBuffer;
-    uint32_t     txSize;
     uint8_t      *txBuffer;
 } UAVTalkConnectionData;
 
 #define UAVTALK_CANARI          0xCA
-#define UAVTALK_WAITFOREVER     -1
-#define UAVTALK_NOWAIT          0
 #define UAVTALK_SYNC_VAL        0x3C
+
 #define UAVTALK_TYPE_MASK       0x78
 #define UAVTALK_TYPE_VER        0x20
 #define UAVTALK_TIMESTAMPED     0x80

flight/uavtalk/uavtalk.c

@@ -32,14 +32,27 @@
 #include "openpilot.h"
 #include "uavtalk_priv.h"
 
+// #define UAV_DEBUGLOG 1
+
+#if defined UAV_DEBUGLOG && defined FLASH_FREERTOS
+#define UAVT_DEBUGLOG_PRINTF(...) PIOS_DEBUGLOG_Printf(__VA_ARGS__)
+// uncomment and adapt the following lines to filter verbose logging to include specific object(s) only
+// #include "flighttelemetrystats.h"
+// #define UAVT_DEBUGLOG_CPRINTF(objId, ...) if (objId == FLIGHTTELEMETRYSTATS_OBJID) { UAVT_DEBUGLOG_PRINTF(__VA_ARGS__); }
+#endif
+#ifndef UAVT_DEBUGLOG_PRINTF
+#define UAVT_DEBUGLOG_PRINTF(...)
+#endif
+#ifndef UAVT_DEBUGLOG_CPRINTF
+#define UAVT_DEBUGLOG_CPRINTF(objId, ...)
+#endif
 
 // Private functions
-static int32_t objectTransaction(UAVTalkConnectionData *connection, UAVObjHandle objectId, uint16_t instId, uint8_t type, int32_t timeout);
+static int32_t objectTransaction(UAVTalkConnectionData *connection, uint8_t type, UAVObjHandle obj, uint16_t instId, int32_t timeout);
-static int32_t sendObject(UAVTalkConnectionData *connection, UAVObjHandle obj, uint16_t instId, uint8_t type);
+static int32_t sendObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, UAVObjHandle obj);
-static int32_t sendSingleObject(UAVTalkConnectionData *connection, UAVObjHandle obj, uint16_t instId, uint8_t type);
+static int32_t sendSingleObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, UAVObjHandle obj);
-static int32_t sendNack(UAVTalkConnectionData *connection, uint32_t objId);
+static int32_t receiveObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, uint8_t *data);
-static int32_t receiveObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, uint8_t *data, int32_t length);
+static void updateAck(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId);
-static void updateAck(UAVTalkConnectionData *connection, UAVObjHandle obj, uint16_t instId);
 
 /**
  * Initialize the UAVTalk library
@@ -152,13 +165,15 @@ void UAVTalkAddStats(UAVTalkConnection connectionHandle, UAVTalkStats *statsOut)
 
     // Copy stats
     statsOut->txBytes       += connection->stats.txBytes;
-    statsOut->rxBytes       += connection->stats.rxBytes;
     statsOut->txObjectBytes += connection->stats.txObjectBytes;
-    statsOut->rxObjectBytes += connection->stats.rxObjectBytes;
     statsOut->txObjects     += connection->stats.txObjects;
+    statsOut->txErrors      += connection->stats.txErrors;
+    statsOut->rxBytes       += connection->stats.rxBytes;
+    statsOut->rxObjectBytes += connection->stats.rxObjectBytes;
     statsOut->rxObjects     += connection->stats.rxObjects;
-    statsOut->txErrors      += connection->stats.txErrors;
+    statsOut->rxErrors      += connection->stats.rxErrors;
-    statsOut->txErrors      += connection->stats.txErrors;
+    statsOut->rxSyncErrors  += connection->stats.rxSyncErrors;
+    statsOut->rxCrcErrors   += connection->stats.rxCrcErrors;
 
     // Release lock
     xSemaphoreGiveRecursive(connection->lock);
@@ -197,7 +212,6 @@ void UAVTalkGetLastTimestamp(UAVTalkConnection connectionHandle, uint16_t *times
     *timestamp = iproc->timestamp;
 }
 
-
 /**
  * Request an update for the specified object, on success the object data would have been
  * updated by the GCS.
@@ -213,7 +227,8 @@ int32_t UAVTalkSendObjectRequest(UAVTalkConnection connectionHandle, UAVObjHandl
     UAVTalkConnectionData *connection;
 
     CHECKCONHANDLE(connectionHandle, connection, return -1);
-    return objectTransaction(connection, obj, instId, UAVTALK_TYPE_OBJ_REQ, timeout);
+
+    return objectTransaction(connection, UAVTALK_TYPE_OBJ_REQ, obj, instId, timeout);
 }
 
 /**
@@ -231,11 +246,12 @@ int32_t UAVTalkSendObject(UAVTalkConnection connectionHandle, UAVObjHandle obj,
     UAVTalkConnectionData *connection;
 
     CHECKCONHANDLE(connectionHandle, connection, return -1);
+
     // Send object
     if (acked == 1) {
-        return objectTransaction(connection, obj, instId, UAVTALK_TYPE_OBJ_ACK, timeoutMs);
+        return objectTransaction(connection, UAVTALK_TYPE_OBJ_ACK, obj, instId, timeoutMs);
     } else {
-        return objectTransaction(connection, obj, instId, UAVTALK_TYPE_OBJ, timeoutMs);
+        return objectTransaction(connection, UAVTALK_TYPE_OBJ, obj, instId, timeoutMs);
     }
 }
 
@@ -254,29 +270,32 @@ int32_t UAVTalkSendObjectTimestamped(UAVTalkConnection connectionHandle, UAVObjH
     UAVTalkConnectionData *connection;
 
     CHECKCONHANDLE(connectionHandle, connection, return -1);
+
     // Send object
     if (acked == 1) {
-        return objectTransaction(connection, obj, instId, UAVTALK_TYPE_OBJ_ACK_TS, timeoutMs);
+        return objectTransaction(connection, UAVTALK_TYPE_OBJ_ACK_TS, obj, instId, timeoutMs);
     } else {
-        return objectTransaction(connection, obj, instId, UAVTALK_TYPE_OBJ_TS, timeoutMs);
+        return objectTransaction(connection, UAVTALK_TYPE_OBJ_TS, obj, instId, timeoutMs);
     }
 }
 
 /**
  * Execute the requested transaction on an object.
  * \param[in] connection UAVTalkConnection to be used
- * \param[in] obj Object
- * \param[in] instId The instance ID of UAVOBJ_ALL_INSTANCES for all instances.
  * \param[in] type Transaction type
  *                        UAVTALK_TYPE_OBJ: send object,
  *                        UAVTALK_TYPE_OBJ_REQ: request object update
  *                        UAVTALK_TYPE_OBJ_ACK: send object with an ack
+ * \param[in] obj Object
+ * \param[in] instId The instance ID of UAVOBJ_ALL_INSTANCES for all instances.
+ * \param[in] timeoutMs Time to wait for the ack, when zero it will return immediately
  * \return 0 Success
  * \return -1 Failure
  */
-static int32_t objectTransaction(UAVTalkConnectionData *connection, UAVObjHandle obj, uint16_t instId, uint8_t type, int32_t timeoutMs)
+static int32_t objectTransaction(UAVTalkConnectionData *connection, uint8_t type, UAVObjHandle obj, uint16_t instId, int32_t timeoutMs)
 {
     int32_t respReceived;
+    int32_t ret = -1;
 
     // Send object depending on if a response is needed
     if (type == UAVTALK_TYPE_OBJ_ACK || type == UAVTALK_TYPE_OBJ_ACK_TS || type == UAVTALK_TYPE_OBJ_REQ) {
@@ -284,33 +303,38 @@ static int32_t objectTransaction(UAVTalkConnectionData *connection, UAVObjHandle
         xSemaphoreTakeRecursive(connection->transLock, portMAX_DELAY);
         // Send object
         xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY);
-        connection->respObj    = obj;
+        // expected response type
+        connection->respType   = (type == UAVTALK_TYPE_OBJ_REQ) ? UAVTALK_TYPE_OBJ : UAVTALK_TYPE_ACK;
+        connection->respObjId  = UAVObjGetID(obj);
         connection->respInstId = instId;
-        sendObject(connection, obj, instId, type);
+        ret = sendObject(connection, type, UAVObjGetID(obj), instId, obj);
         xSemaphoreGiveRecursive(connection->lock);
-        // Wait for response (or timeout)
+        // Wait for response (or timeout) if sending the object succeeded
-        respReceived = xSemaphoreTake(connection->respSema, timeoutMs / portTICK_RATE_MS);
+        respReceived = pdFALSE;
+        if (ret == 0) {
+            respReceived = xSemaphoreTake(connection->respSema, timeoutMs / portTICK_RATE_MS);
+        }
         // Check if a response was received
-        if (respReceived == pdFALSE) {
+        if (respReceived == pdTRUE) {
+            // We are done successfully
+            xSemaphoreGiveRecursive(connection->transLock);
+            ret = 0;
+        } else {
             // Cancel transaction
             xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY);
-            xSemaphoreTake(connection->respSema, 0); // non blocking call to make sure the value is reset to zero (binary sema)
+            // non blocking call to make sure the value is reset to zero (binary sema)
-            connection->respObj = 0;
+            xSemaphoreTake(connection->respSema, 0);
+            connection->respObjId = 0;
             xSemaphoreGiveRecursive(connection->lock);
             xSemaphoreGiveRecursive(connection->transLock);
             return -1;
-        } else {
-            xSemaphoreGiveRecursive(connection->transLock);
-            return 0;
         }
     } else if (type == UAVTALK_TYPE_OBJ || type == UAVTALK_TYPE_OBJ_TS) {
         xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY);
-        sendObject(connection, obj, instId, type);
+        ret = sendObject(connection, type, UAVObjGetID(obj), instId, obj);
         xSemaphoreGiveRecursive(connection->lock);
-        return 0;
-    } else {
-        return -1;
     }
+    return ret;
 }
 
 /**
@@ -326,6 +350,7 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle
     CHECKCONHANDLE(connectionHandle, connection, return -1);
 
     UAVTalkInputProcessor *iproc = &connection->iproc;
+
     ++connection->stats.rxBytes;
 
     if (iproc->state == UAVTALK_STATE_ERROR || iproc->state == UAVTALK_STATE_COMPLETE) {
@@ -333,12 +358,16 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle
     }
 
     if (iproc->rxPacketLength < 0xffff) {
-        iproc->rxPacketLength++; // update packet byte count
+        // update packet byte count
+        iproc->rxPacketLength++;
     }
+
     // Receive state machine
     switch (iproc->state) {
     case UAVTALK_STATE_SYNC:
+
         if (rxbyte != UAVTALK_SYNC_VAL) {
+            connection->stats.rxSyncErrors++;
             break;
         }
 
@@ -346,26 +375,27 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle
         iproc->cs = PIOS_CRC_updateByte(0, rxbyte);
 
         iproc->rxPacketLength = 1;
+        iproc->rxCount = 0;
 
-        iproc->state = UAVTALK_STATE_TYPE;
+        iproc->type    = 0;
+        iproc->state   = UAVTALK_STATE_TYPE;
         break;
 
     case UAVTALK_STATE_TYPE:
 
-        // update the CRC
-        iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
-
         if ((rxbyte & UAVTALK_TYPE_MASK) != UAVTALK_TYPE_VER) {
-            iproc->state = UAVTALK_STATE_ERROR;
+            connection->stats.rxErrors++;
+            iproc->state = UAVTALK_STATE_SYNC;
             break;
         }
 
-        iproc->type = rxbyte;
+        // update the CRC
+        iproc->cs    = PIOS_CRC_updateByte(iproc->cs, rxbyte);
+
+        iproc->type  = rxbyte;
 
         iproc->packet_size = 0;
-
         iproc->state = UAVTALK_STATE_SIZE;
-        iproc->rxCount     = 0;
         break;
 
     case UAVTALK_STATE_SIZE:
@@ -378,17 +408,18 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle
             iproc->rxCount++;
             break;
         }
-
         iproc->packet_size += rxbyte << 8;
+        iproc->rxCount      = 0;
 
-        if (iproc->packet_size < UAVTALK_MIN_HEADER_LENGTH || iproc->packet_size > UAVTALK_MAX_HEADER_LENGTH + UAVTALK_MAX_PAYLOAD_LENGTH) { // incorrect packet size
+        if (iproc->packet_size < UAVTALK_MIN_HEADER_LENGTH || iproc->packet_size > UAVTALK_MAX_HEADER_LENGTH + UAVTALK_MAX_PAYLOAD_LENGTH) {
+            // incorrect packet size
+            connection->stats.rxErrors++;
             iproc->state = UAVTALK_STATE_ERROR;
             break;
         }
 
-        iproc->rxCount = 0;
+        iproc->objId = 0;
-        iproc->objId   = 0;
+        iproc->state = UAVTALK_STATE_OBJID;
-        iproc->state   = UAVTALK_STATE_OBJID;
         break;
 
     case UAVTALK_STATE_OBJID:
@@ -397,55 +428,60 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle
         iproc->cs     = PIOS_CRC_updateByte(iproc->cs, rxbyte);
 
         iproc->objId += rxbyte << (8 * (iproc->rxCount++));
-
         if (iproc->rxCount < 4) {
             break;
         }
+        iproc->rxCount = 0;
 
-        // Search for object.
+        iproc->instId  = 0;
-        iproc->obj = UAVObjGetByID(iproc->objId);
+        iproc->state   = UAVTALK_STATE_INSTID;
+        break;
+
+    case UAVTALK_STATE_INSTID:
+
+        // update the CRC
+        iproc->cs      = PIOS_CRC_updateByte(iproc->cs, rxbyte);
+
+        iproc->instId += rxbyte << (8 * (iproc->rxCount++));
+        if (iproc->rxCount < 2) {
+            break;
+        }
+        iproc->rxCount = 0;
+
+        UAVObjHandle obj = UAVObjGetByID(iproc->objId);
 
         // Determine data length
         if (iproc->type == UAVTALK_TYPE_OBJ_REQ || iproc->type == UAVTALK_TYPE_ACK || iproc->type == UAVTALK_TYPE_NACK) {
             iproc->length = 0;
-            iproc->instanceLength = 0;
+            iproc->timestampLength = 0;
         } else {
-            if (iproc->obj) {
-                iproc->length = UAVObjGetNumBytes(iproc->obj);
-                iproc->instanceLength = (UAVObjIsSingleInstance(iproc->obj) ? 0 : 2);
-            } else {
-                // We don't know if it's a multi-instance object, so just assume it's 0.
-                iproc->instanceLength = 0;
-                iproc->length = iproc->packet_size - iproc->rxPacketLength;
-            }
             iproc->timestampLength = (iproc->type & UAVTALK_TIMESTAMPED) ? 2 : 0;
+            if (obj) {
+                iproc->length = UAVObjGetNumBytes(obj);
+            } else {
+                iproc->length = iproc->packet_size - iproc->rxPacketLength - iproc->timestampLength;
+            }
         }
 
-        // Check length and determine next state
+        // Check length
         if (iproc->length >= UAVTALK_MAX_PAYLOAD_LENGTH) {
+            // packet error - exceeded payload max length
             connection->stats.rxErrors++;
             iproc->state = UAVTALK_STATE_ERROR;
             break;
         }
 
         // Check the lengths match
-        if ((iproc->rxPacketLength + iproc->instanceLength + iproc->timestampLength + iproc->length) != iproc->packet_size) { // packet error - mismatched packet size
+        if ((iproc->rxPacketLength + iproc->timestampLength + iproc->length) != iproc->packet_size) {
+            // packet error - mismatched packet size
             connection->stats.rxErrors++;
             iproc->state = UAVTALK_STATE_ERROR;
             break;
         }
 
-        iproc->instId = 0;
+        // Determine next state
-        if (iproc->type == UAVTALK_TYPE_NACK) {
+        if (iproc->type & UAVTALK_TIMESTAMPED) {
-            // If this is a NACK, we skip to Checksum
+            // If there is a timestamp get it
-            iproc->state = UAVTALK_STATE_CS;
-        }
-        // Check if this is a single instance object (i.e. if the instance ID field is coming next)
-        else if ((iproc->obj != 0) && !UAVObjIsSingleInstance(iproc->obj)) {
-            iproc->state = UAVTALK_STATE_INSTID;
-        }
-        // Check if this is a single instance and has a timestamp in it
-        else if ((iproc->obj != 0) && (iproc->type & UAVTALK_TIMESTAMPED)) {
             iproc->timestamp = 0;
             iproc->state     = UAVTALK_STATE_TIMESTAMP;
         } else {
@@ -456,47 +492,17 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle
                 iproc->state = UAVTALK_STATE_CS;
             }
         }
-        iproc->rxCount = 0;
-
-        break;
-
-    case UAVTALK_STATE_INSTID:
-
-        // update the CRC
-        iproc->cs      = PIOS_CRC_updateByte(iproc->cs, rxbyte);
-
-        iproc->instId += rxbyte << (8 * (iproc->rxCount++));
-
-        if (iproc->rxCount < 2) {
-            break;
-        }
-
-        iproc->rxCount = 0;
-
-        // If there is a timestamp, get it
-        if ((iproc->length > 0) && (iproc->type & UAVTALK_TIMESTAMPED)) {
-            iproc->timestamp = 0;
-            iproc->state     = UAVTALK_STATE_TIMESTAMP;
-        }
-        // If there is a payload get it, otherwise receive checksum
-        else if (iproc->length > 0) {
-            iproc->state = UAVTALK_STATE_DATA;
-        } else {
-            iproc->state = UAVTALK_STATE_CS;
-        }
-
         break;
 
     case UAVTALK_STATE_TIMESTAMP:
+
         // update the CRC
         iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
 
         iproc->timestamp += rxbyte << (8 * (iproc->rxCount++));
-
         if (iproc->rxCount < 2) {
             break;
         }
-
         iproc->rxCount = 0;
 
         // If there is a payload get it, otherwise receive checksum
@@ -516,35 +522,40 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle
         if (iproc->rxCount < iproc->length) {
             break;
         }
+        iproc->rxCount = 0;
 
         iproc->state   = UAVTALK_STATE_CS;
-        iproc->rxCount = 0;
         break;
 
     case UAVTALK_STATE_CS:
 
-        // the CRC byte
+        // Check the CRC byte
-        if (rxbyte != iproc->cs) { // packet error - faulty CRC
+        if (rxbyte != iproc->cs) {
+            // packet error - faulty CRC
+            UAVT_DEBUGLOG_PRINTF("BAD CRC");
+            connection->stats.rxCrcErrors++;
             connection->stats.rxErrors++;
             iproc->state = UAVTALK_STATE_ERROR;
             break;
         }
 
-        if (iproc->rxPacketLength != (iproc->packet_size + 1)) { // packet error - mismatched packet size
+        if (iproc->rxPacketLength != (iproc->packet_size + UAVTALK_CHECKSUM_LENGTH)) {
+            // packet error - mismatched packet size
             connection->stats.rxErrors++;
             iproc->state = UAVTALK_STATE_ERROR;
             break;
         }
 
-        connection->stats.rxObjectBytes += iproc->length;
         connection->stats.rxObjects++;
+        connection->stats.rxObjectBytes += iproc->length;
 
         iproc->state = UAVTALK_STATE_COMPLETE;
         break;
 
     default:
-        connection->stats.rxErrors++;
+
         iproc->state = UAVTALK_STATE_ERROR;
+        break;
     }
 
     // Done
@@ -587,6 +598,8 @@ UAVTalkRxState UAVTalkRelayPacket(UAVTalkConnection inConnectionHandle, UAVTalkC
 
     // The input packet must be completely parsed.
     if (inIproc->state != UAVTALK_STATE_COMPLETE) {
+        inConnection->stats.rxErrors++;
+
         return -1;
     }
 
@@ -594,66 +607,66 @@ UAVTalkRxState UAVTalkRelayPacket(UAVTalkConnection inConnectionHandle, UAVTalkC
     CHECKCONHANDLE(outConnectionHandle, outConnection, return -1);
 
     if (!outConnection->outStream) {
+        outConnection->stats.txErrors++;
+
         return -1;
     }
 
     // Lock
     xSemaphoreTakeRecursive(outConnection->lock, portMAX_DELAY);
 
-    // Setup type and object id fields
+    outConnection->txBuffer[0] = UAVTALK_SYNC_VAL;
-    outConnection->txBuffer[0] = UAVTALK_SYNC_VAL; // sync byte
+    // Setup type
     outConnection->txBuffer[1] = inIproc->type;
-    // data length inserted here below
+    // next 2 bytes are reserved for data length (inserted here later)
-    int32_t dataOffset = 8;
+    // Setup object ID
-    if (inIproc->objId != 0) {
+    outConnection->txBuffer[4] = (uint8_t)(inIproc->objId & 0xFF);
-        outConnection->txBuffer[4] = (uint8_t)(inIproc->objId & 0xFF);
+    outConnection->txBuffer[5] = (uint8_t)((inIproc->objId >> 8) & 0xFF);
-        outConnection->txBuffer[5] = (uint8_t)((inIproc->objId >> 8) & 0xFF);
+    outConnection->txBuffer[6] = (uint8_t)((inIproc->objId >> 16) & 0xFF);
-        outConnection->txBuffer[6] = (uint8_t)((inIproc->objId >> 16) & 0xFF);
+    outConnection->txBuffer[7] = (uint8_t)((inIproc->objId >> 24) & 0xFF);
-        outConnection->txBuffer[7] = (uint8_t)((inIproc->objId >> 24) & 0xFF);
+    // Setup instance ID
-
+    outConnection->txBuffer[8] = (uint8_t)(inIproc->instId & 0xFF);
-        // Setup instance ID if one is required
+    outConnection->txBuffer[9] = (uint8_t)((inIproc->instId >> 8) & 0xFF);
-        if (inIproc->instanceLength > 0) {
+    int32_t headerLength = 10;
-            outConnection->txBuffer[8] = (uint8_t)(inIproc->instId & 0xFF);
-            outConnection->txBuffer[9] = (uint8_t)((inIproc->instId >> 8) & 0xFF);
-            dataOffset = 10;
-        }
-    } else {
-        dataOffset = 4;
-    }
 
     // Add timestamp when the transaction type is appropriate
     if (inIproc->type & UAVTALK_TIMESTAMPED) {
         portTickType time = xTaskGetTickCount();
-        outConnection->txBuffer[dataOffset]     = (uint8_t)(time & 0xFF);
+        outConnection->txBuffer[10] = (uint8_t)(time & 0xFF);
-        outConnection->txBuffer[dataOffset + 1] = (uint8_t)((time >> 8) & 0xFF);
+        outConnection->txBuffer[11] = (uint8_t)((time >> 8) & 0xFF);
-        dataOffset += 2;
+        headerLength += 2;
     }
 
     // Copy data (if any)
-    memcpy(&outConnection->txBuffer[dataOffset], inConnection->rxBuffer, inIproc->length);
+    if (inIproc->length > 0) {
+        memcpy(&outConnection->txBuffer[headerLength], inConnection->rxBuffer, inIproc->length);
+    }
 
     // Store the packet length
-    outConnection->txBuffer[2] = (uint8_t)((dataOffset + inIproc->length) & 0xFF);
+    outConnection->txBuffer[2] = (uint8_t)((headerLength + inIproc->length) & 0xFF);
-    outConnection->txBuffer[3] = (uint8_t)(((dataOffset + inIproc->length) >> 8) & 0xFF);
+    outConnection->txBuffer[3] = (uint8_t)(((headerLength + inIproc->length) >> 8) & 0xFF);
 
     // Copy the checksum
-    outConnection->txBuffer[dataOffset + inIproc->length] = inIproc->cs;
+    outConnection->txBuffer[headerLength + inIproc->length] = inIproc->cs;
 
     // Send the buffer.
-    int32_t rc = (*outConnection->outStream)(outConnection->txBuffer, inIproc->rxPacketLength);
+    int32_t rc = (*outConnection->outStream)(outConnection->txBuffer, headerLength + inIproc->length + UAVTALK_CHECKSUM_LENGTH);
 
     // Update stats
-    outConnection->stats.txBytes += rc;
+    outConnection->stats.txBytes += (rc > 0) ? rc : 0;
+
+    // evaluate return value before releasing the lock
+    UAVTalkRxState rxState = 0;
+    if (rc != (int32_t)(headerLength + inIproc->length + UAVTALK_CHECKSUM_LENGTH)) {
+        outConnection->stats.txErrors++;
+        rxState = -1;
+    }
 
     // Release lock
     xSemaphoreGiveRecursive(outConnection->lock);
 
     // Done
-    if (rc != inIproc->rxPacketLength) {
+    return rxState;
-        return -1;
-    }
-
-    return 0;
 }
 
 /**
@@ -667,66 +680,18 @@ int32_t UAVTalkReceiveObject(UAVTalkConnection connectionHandle)
     UAVTalkConnectionData *connection;
 
     CHECKCONHANDLE(connectionHandle, connection, return -1);
+
     UAVTalkInputProcessor *iproc = &connection->iproc;
     if (iproc->state != UAVTALK_STATE_COMPLETE) {
         return -1;
     }
-    receiveObject(connection, iproc->type, iproc->objId, iproc->instId, connection->rxBuffer, iproc->length);
-    return 0;
-}
 
-/**
+    return receiveObject(connection, iproc->type, iproc->objId, iproc->instId, connection->rxBuffer);
- * Send a ACK through the telemetry link.
- * \param[in] connectionHandle UAVTalkConnection to be used
- * \param[in] objId Object ID to send a NACK for
- * \return 0 Success
- * \return -1 Failure
- */
-int32_t UAVTalkSendAck(UAVTalkConnection connectionHandle, UAVObjHandle obj, uint16_t instId)
-{
-    UAVTalkConnectionData *connection;
-
-    CHECKCONHANDLE(connectionHandle, connection, return -1);
-
-    // Lock
-    xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY);
-
-    int32_t ret = sendObject(connection, obj, instId, UAVTALK_TYPE_ACK);
-
-    // Release lock
-    xSemaphoreGiveRecursive(connection->lock);
-
-    return ret;
-}
-
-/**
- * Send a NACK through the telemetry link.
- * \param[in] connectionHandle UAVTalkConnection to be used
- * \param[in] objId Object ID to send a NACK for
- * \return 0 Success
- * \return -1 Failure
- */
-int32_t UAVTalkSendNack(UAVTalkConnection connectionHandle, uint32_t objId)
-{
-    UAVTalkConnectionData *connection;
-
-    CHECKCONHANDLE(connectionHandle, connection, return -1);
-
-    // Lock
-    xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY);
-
-    int32_t ret = sendNack(connection, objId);
-
-    // Release lock
-    xSemaphoreGiveRecursive(connection->lock);
-
-    return ret;
 }
 
 /**
  * Get the object ID of the current packet.
  * \param[in] connectionHandle UAVTalkConnection to be used
- * \param[in] objId Object ID to send a NACK for
  * \return The object ID, or 0 on error.
  */
 uint32_t UAVTalkGetPacketObjId(UAVTalkConnection connectionHandle)
@@ -734,11 +699,19 @@ uint32_t UAVTalkGetPacketObjId(UAVTalkConnection connectionHandle)
     UAVTalkConnectionData *connection;
 
     CHECKCONHANDLE(connectionHandle, connection, return 0);
+
     return connection->iproc.objId;
 }
 
 /**
  * Receive an object. This function process objects received through the telemetry stream.
+ *
+ * Parser errors are considered as transmission errors and are not NACKed.
+ * Some senders (GCS) can timeout and retry if the message is not answered by an ack or nack.
+ *
+ * Object handling errors are considered as application errors and are NACked.
+ * In that case we want to nack as there is no point in the sender retrying to send invalid objects.
+ *
  * \param[in] connection UAVTalkConnection to be used
  * \param[in] type Type of received message (UAVTALK_TYPE_OBJ, UAVTALK_TYPE_OBJ_REQ, UAVTALK_TYPE_OBJ_ACK, UAVTALK_TYPE_ACK, UAVTALK_TYPE_NACK)
  * \param[in] objId ID of the object to work on
@@ -748,12 +721,7 @@ uint32_t UAVTalkGetPacketObjId(UAVTalkConnection connectionHandle)
  * \return 0 Success
  * \return -1 Failure
  */
-static int32_t receiveObject(UAVTalkConnectionData *connection,
+static int32_t receiveObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, uint8_t *data)
-                             uint8_t type,
-                             uint32_t objId,
-                             uint16_t instId,
-                             uint8_t *data,
-                             __attribute__((unused)) int32_t length)
 {
     UAVObjHandle obj;
     int32_t ret = 0;
@@ -761,33 +729,25 @@ static int32_t receiveObject(UAVTalkConnectionData *connection,
     // Lock
     xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY);
 
-    // Get the handle to the Object. Will be zero
+    // Get the handle to the object. Will be null if object does not exist.
-    // if object does not exist.
+    // Warning :
+    // Here we ask for instance ID 0 without taking into account the provided instId
+    // The provided instId will be used later when packing, unpacking, etc...
+    // TODO the above should be fixed as it is cumbersome and error prone
     obj = UAVObjGetByID(objId);
 
     // Process message type
     switch (type) {
     case UAVTALK_TYPE_OBJ:
     case UAVTALK_TYPE_OBJ_TS:
-        // All instances, not allowed for OBJ messages
+        // All instances not allowed for OBJ messages
-        if (obj && (instId != UAVOBJ_ALL_INSTANCES)) {
-            // Unpack object, if the instance does not exist it will be created!
-            UAVObjUnpack(obj, instId, data);
-            // Check if an ack is pending
-            updateAck(connection, obj, instId);
-        } else {
-            ret = -1;
-        }
-        break;
-
-    case UAVTALK_TYPE_OBJ_ACK:
-    case UAVTALK_TYPE_OBJ_ACK_TS:
-        // All instances, not allowed for OBJ_ACK messages
         if (obj && (instId != UAVOBJ_ALL_INSTANCES)) {
             // Unpack object, if the instance does not exist it will be created!
             if (UAVObjUnpack(obj, instId, data) == 0) {
-                // Transmit ACK
+                // Check if this object acks a pending OBJ_REQ message
-                sendObject(connection, obj, instId, UAVTALK_TYPE_ACK);
+                // any OBJ message can ack a pending OBJ_REQ message
+                // even one that was not sent in response to the OBJ_REQ message
+                updateAck(connection, type, objId, instId);
             } else {
                 ret = -1;
             }
@@ -795,26 +755,68 @@ static int32_t receiveObject(UAVTalkConnectionData *connection,
             ret = -1;
         }
         break;
-    case UAVTALK_TYPE_OBJ_REQ:
+
-        // Send requested object if message is of type OBJ_REQ
+    case UAVTALK_TYPE_OBJ_ACK:
-        if (obj == 0) {
+    case UAVTALK_TYPE_OBJ_ACK_TS:
-            sendNack(connection, objId);
+        UAVT_DEBUGLOG_CPRINTF(objId, "OBJ_ACK %X %d", objId, instId);
+        // All instances not allowed for OBJ_ACK messages
+        if (obj && (instId != UAVOBJ_ALL_INSTANCES)) {
+            // Unpack object, if the instance does not exist it will be created!
+            if (UAVObjUnpack(obj, instId, data) == 0) {
+                UAVT_DEBUGLOG_CPRINTF(objId, "OBJ ACK %X %d", objId, instId);
+                // Object updated or created, transmit ACK
+                sendObject(connection, UAVTALK_TYPE_ACK, objId, instId, NULL);
+            } else {
+                ret = -1;
+            }
         } else {
-            sendObject(connection, obj, instId, UAVTALK_TYPE_OBJ);
+            ret = -1;
+        }
+        if (ret == -1) {
+            // failed to update object, transmit NACK
+            UAVT_DEBUGLOG_PRINTF("OBJ NACK %X %d", objId, instId);
+            sendObject(connection, UAVTALK_TYPE_NACK, objId, instId, NULL);
         }
         break;
+
+    case UAVTALK_TYPE_OBJ_REQ:
+        // Check if requested object exists
+        UAVT_DEBUGLOG_CPRINTF(objId, "REQ %X %d", objId, instId);
+        if (obj) {
+            // Object found, transmit it
+            // The sent object will ack the object request on the receiver side
+            ret = sendObject(connection, UAVTALK_TYPE_OBJ, objId, instId, obj);
+        } else {
+            ret = -1;
+        }
+        if (ret == -1) {
+            // failed to send object, transmit NACK
+            UAVT_DEBUGLOG_PRINTF("REQ NACK %X %d", objId, instId);
+            sendObject(connection, UAVTALK_TYPE_NACK, objId, instId, NULL);
+        }
+        break;
+
     case UAVTALK_TYPE_NACK:
         // Do nothing on flight side, let it time out.
+        // TODO:
+        // The transaction takes the result code of the "semaphore taking operation" into account to determine success.
+        // If we give that semaphore in time, its "success" (ack received)
+        // If we do not give that semaphore before the timeout it will return failure.
+        // What would have to be done here is give the semaphore, but set a flag (for example connection->respFail=true)
+        // that indicates failure and then above where it checks for the result code, have it behave as if it failed
+        // if the explicit failure is set.
         break;
+
     case UAVTALK_TYPE_ACK:
-        // All instances, not allowed for ACK messages
+        // All instances not allowed for ACK messages
         if (obj && (instId != UAVOBJ_ALL_INSTANCES)) {
-            // Check if an ack is pending
+            // Check if an ACK is pending
-            updateAck(connection, obj, instId);
+            updateAck(connection, type, objId, instId);
         } else {
             ret = -1;
         }
         break;
+
     default:
         ret = -1;
     }
@@ -832,30 +834,40 @@ static int32_t receiveObject(UAVTalkConnectionData *connection,
  * \param[in] obj Object
  * \param[in] instId The instance ID of UAVOBJ_ALL_INSTANCES for all instances.
  */
-static void updateAck(UAVTalkConnectionData *connection, UAVObjHandle obj, uint16_t instId)
+static void updateAck(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId)
 {
-    if (connection->respObj == obj && (connection->respInstId == instId || connection->respInstId == UAVOBJ_ALL_INSTANCES)) {
+    if ((connection->respObjId == objId) && (connection->respType == type)) {
-        xSemaphoreGive(connection->respSema);
+        if ((connection->respInstId == UAVOBJ_ALL_INSTANCES) && (instId == 0)) {
-        connection->respObj = 0;
+            // last instance received, complete transaction
+            xSemaphoreGive(connection->respSema);
+            connection->respObjId = 0;
+        } else if (connection->respInstId == instId) {
+            xSemaphoreGive(connection->respSema);
+            connection->respObjId = 0;
+        }
     }
 }
 
 /**
  * Send an object through the telemetry link.
  * \param[in] connection UAVTalkConnection to be used
- * \param[in] obj Object handle to send
- * \param[in] instId The instance ID or UAVOBJ_ALL_INSTANCES for all instances
  * \param[in] type Transaction type
+ * \param[in] objId The object ID
+ * \param[in] instId The instance ID or UAVOBJ_ALL_INSTANCES for all instances
+ * \param[in] obj Object handle to send (null when type is NACK)
  * \return 0 Success
  * \return -1 Failure
  */
-static int32_t sendObject(UAVTalkConnectionData *connection, UAVObjHandle obj, uint16_t instId, uint8_t type)
+static int32_t sendObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, UAVObjHandle obj)
 {
     uint32_t numInst;
     uint32_t n;
+    uint32_t ret = -1;
+
+    // Important note : obj can be null (when type is NACK for example) so protect all obj dereferences.
 
     // If all instances are requested and this is a single instance object, force instance ID to zero
-    if (instId == UAVOBJ_ALL_INSTANCES && UAVObjIsSingleInstance(obj)) {
+    if ((obj != NULL) && (instId == UAVOBJ_ALL_INSTANCES) && UAVObjIsSingleInstance(obj)) {
         instId = 0;
     }
 
@@ -864,153 +876,117 @@ static int32_t sendObject(UAVTalkConnectionData *connection, UAVObjHandle obj, u
         if (instId == UAVOBJ_ALL_INSTANCES) {
             // Get number of instances
             numInst = UAVObjGetNumInstances(obj);
-            // Send all instances
+            // Send all instances in reverse order
+            // This allows the receiver to detect when the last object has been received (i.e. when instance 0 is received)
+            ret     = 0;
             for (n = 0; n < numInst; ++n) {
-                sendSingleObject(connection, obj, n, type);
+                if (sendSingleObject(connection, type, objId, numInst - n - 1, obj) == -1) {
+                    ret = -1;
+                    break;
+                }
             }
-            return 0;
         } else {
-            return sendSingleObject(connection, obj, instId, type);
+            ret = sendSingleObject(connection, type, objId, instId, obj);
         }
     } else if (type == UAVTALK_TYPE_OBJ_REQ) {
-        return sendSingleObject(connection, obj, instId, UAVTALK_TYPE_OBJ_REQ);
+        ret = sendSingleObject(connection, type, objId, instId, obj);
-    } else if (type == UAVTALK_TYPE_ACK) {
+    } else if (type == UAVTALK_TYPE_ACK || type == UAVTALK_TYPE_NACK) {
         if (instId != UAVOBJ_ALL_INSTANCES) {
-            return sendSingleObject(connection, obj, instId, UAVTALK_TYPE_ACK);
+            ret = sendSingleObject(connection, type, objId, instId, obj);
-        } else {
-            return -1;
         }
-    } else {
-        return -1;
     }
+
+    return ret;
 }
 
 /**
  * Send an object through the telemetry link.
  * \param[in] connection UAVTalkConnection to be used
- * \param[in] obj Object handle to send
- * \param[in] instId The instance ID (can NOT be UAVOBJ_ALL_INSTANCES, use sendObject() instead)
  * \param[in] type Transaction type
+ * \param[in] objId The object ID
+ * \param[in] instId The instance ID (can NOT be UAVOBJ_ALL_INSTANCES, use
+   () instead)
+ * \param[in] obj Object handle to send (null when type is NACK)
  * \return 0 Success
  * \return -1 Failure
  */
-static int32_t sendSingleObject(UAVTalkConnectionData *connection, UAVObjHandle obj, uint16_t instId, uint8_t type)
+static int32_t sendSingleObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, UAVObjHandle obj)
 {
-    int32_t length;
+    // IMPORTANT : obj can be null (when type is NACK for example)
-    int32_t dataOffset;
-    uint32_t objId;
 
     if (!connection->outStream) {
+        connection->stats.txErrors++;
         return -1;
     }
 
-    // Setup type and object id fields
+    // Setup sync byte
-    objId = UAVObjGetID(obj);
+    connection->txBuffer[0] = UAVTALK_SYNC_VAL;
-    connection->txBuffer[0] = UAVTALK_SYNC_VAL; // sync byte
+    // Setup type
     connection->txBuffer[1] = type;
-    // data length inserted here below
+    // next 2 bytes are reserved for data length (inserted here later)
+    // Setup object ID
     connection->txBuffer[4] = (uint8_t)(objId & 0xFF);
     connection->txBuffer[5] = (uint8_t)((objId >> 8) & 0xFF);
     connection->txBuffer[6] = (uint8_t)((objId >> 16) & 0xFF);
     connection->txBuffer[7] = (uint8_t)((objId >> 24) & 0xFF);
-
+    // Setup instance ID
-    // Setup instance ID if one is required
+    connection->txBuffer[8] = (uint8_t)(instId & 0xFF);
-    if (UAVObjIsSingleInstance(obj)) {
+    connection->txBuffer[9] = (uint8_t)((instId >> 8) & 0xFF);
-        dataOffset = 8;
+    int32_t headerLength = 10;
-    } else {
-        connection->txBuffer[8] = (uint8_t)(instId & 0xFF);
-        connection->txBuffer[9] = (uint8_t)((instId >> 8) & 0xFF);
-        dataOffset = 10;
-    }
 
     // Add timestamp when the transaction type is appropriate
     if (type & UAVTALK_TIMESTAMPED) {
         portTickType time = xTaskGetTickCount();
-        connection->txBuffer[dataOffset]     = (uint8_t)(time & 0xFF);
+        connection->txBuffer[10] = (uint8_t)(time & 0xFF);
-        connection->txBuffer[dataOffset + 1] = (uint8_t)((time >> 8) & 0xFF);
+        connection->txBuffer[11] = (uint8_t)((time >> 8) & 0xFF);
-        dataOffset += 2;
+        headerLength += 2;
     }
 
     // Determine data length
-    if (type == UAVTALK_TYPE_OBJ_REQ || type == UAVTALK_TYPE_ACK) {
+    int32_t length;
+    if (type == UAVTALK_TYPE_OBJ_REQ || type == UAVTALK_TYPE_ACK || type == UAVTALK_TYPE_NACK) {
         length = 0;
     } else {
         length = UAVObjGetNumBytes(obj);
     }
 
     // Check length
-    if (length >= UAVTALK_MAX_PAYLOAD_LENGTH) {
+    if (length > UAVOBJECTS_LARGEST) {
+        connection->stats.txErrors++;
         return -1;
     }
 
     // Copy data (if any)
     if (length > 0) {
-        if (UAVObjPack(obj, instId, &connection->txBuffer[dataOffset]) < 0) {
+        if (UAVObjPack(obj, instId, &connection->txBuffer[headerLength]) == -1) {
+            connection->stats.txErrors++;
             return -1;
         }
     }
 
     // Store the packet length
-    connection->txBuffer[2] = (uint8_t)((dataOffset + length) & 0xFF);
+    connection->txBuffer[2] = (uint8_t)((headerLength + length) & 0xFF);
-    connection->txBuffer[3] = (uint8_t)(((dataOffset + length) >> 8) & 0xFF);
+    connection->txBuffer[3] = (uint8_t)(((headerLength + length) >> 8) & 0xFF);
 
-    // Calculate checksum
+    // Calculate and store checksum
-    connection->txBuffer[dataOffset + length] = PIOS_CRC_updateCRC(0, connection->txBuffer, dataOffset + length);
+    connection->txBuffer[headerLength + length] = PIOS_CRC_updateCRC(0, connection->txBuffer, headerLength + length);
 
-    uint16_t tx_msg_len = dataOffset + length + UAVTALK_CHECKSUM_LENGTH;
+    // Send object
+    uint16_t tx_msg_len = headerLength + length + UAVTALK_CHECKSUM_LENGTH;
     int32_t rc = (*connection->outStream)(connection->txBuffer, tx_msg_len);
 
+    // Update stats
     if (rc == tx_msg_len) {
-        // Update stats
         ++connection->stats.txObjects;
-        connection->stats.txBytes += tx_msg_len;
         connection->stats.txObjectBytes += length;
-    }
+        connection->stats.txBytes += tx_msg_len;
-
+    } else {
-    // Done
+        connection->stats.txErrors++;
-    return 0;
+        // TDOD rc == -1 connection not open, -2 buffer full should retry
-}
+        connection->stats.txBytes += (rc > 0) ? rc : 0;
-
-/**
- * Send a NACK through the telemetry link.
- * \param[in] connection UAVTalkConnection to be used
- * \param[in] objId Object ID to send a NACK for
- * \return 0 Success
- * \return -1 Failure
- */
-static int32_t sendNack(UAVTalkConnectionData *connection, uint32_t objId)
-{
-    int32_t dataOffset;
-
-    if (!connection->outStream) {
         return -1;
     }
 
-    connection->txBuffer[0] = UAVTALK_SYNC_VAL; // sync byte
-    connection->txBuffer[1] = UAVTALK_TYPE_NACK;
-    // data length inserted here below
-    connection->txBuffer[4] = (uint8_t)(objId & 0xFF);
-    connection->txBuffer[5] = (uint8_t)((objId >> 8) & 0xFF);
-    connection->txBuffer[6] = (uint8_t)((objId >> 16) & 0xFF);
-    connection->txBuffer[7] = (uint8_t)((objId >> 24) & 0xFF);
-
-    dataOffset = 8;
-
-    // Store the packet length
-    connection->txBuffer[2] = (uint8_t)((dataOffset) & 0xFF);
-    connection->txBuffer[3] = (uint8_t)(((dataOffset) >> 8) & 0xFF);
-
-    // Calculate checksum
-    connection->txBuffer[dataOffset] = PIOS_CRC_updateCRC(0, connection->txBuffer, dataOffset);
-
-    uint16_t tx_msg_len = dataOffset + UAVTALK_CHECKSUM_LENGTH;
-    int32_t rc = (*connection->outStream)(connection->txBuffer, tx_msg_len);
-
-    if (rc == tx_msg_len) {
-        // Update stats
-        connection->stats.txBytes += tx_msg_len;
-    }
-
     // Done
     return 0;
 }

ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointitem.cpp

@@ -292,26 +292,21 @@ void WayPointItem::setRelativeCoord(distBearingAltitude value)
 
 void WayPointItem::SetCoord(const internals::PointLatLng &value)
 {
-    qDebug() << "1 SetCoord(" << value.Lat() << "," << value.Lng() << ")" << "OLD:" << Coord().Lat() << "," << Coord().Lng();
     if (qAbs(Coord().Lat() - value.Lat()) < 0.0001 && qAbs(Coord().Lng() - value.Lng()) < 0.0001) {
-        qDebug() << "2 SetCoord nothing changed returning";
         return;
     }
-    qDebug() << "3 setCoord there were changes";
     coord = value;
     distBearingAltitude back = relativeCoord;
     if (myHome) {
         map->Projection()->offSetFromLatLngs(myHome->Coord(), Coord(), back.distance, back.bearing);
     }
     if (qAbs(back.bearing - relativeCoord.bearing) > 0.01 || qAbs(back.distance - relativeCoord.distance) > 0.1) {
-        qDebug() << "4 setCoord-relative coordinates where also updated";
         relativeCoord = back;
     }
     emit WPValuesChanged(this);
     RefreshPos();
     RefreshToolTip();
     this->update();
-    qDebug() << "5 setCoord EXIT";
 }
 void WayPointItem::SetDescription(const QString &value)
 {

ground/openpilotgcs/src/libs/utils/crc.cpp

@@ -0,0 +1,74 @@
+/**
+ ******************************************************************************
+ *
+ * @file       crc.cpp
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013.
+ *             Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009.
+ * @addtogroup GCSPlugins GCS Plugins
+ * @{
+ * @addtogroup CorePlugin Core Plugin
+ * @{
+ * @brief The Core GCS plugin
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#include "crc.h"
+
+using namespace Utils;
+
+/*
+ * Generated by pycrc v0.7.5, http://www.tty1.net/pycrc/
+ * using the configuration:
+ *    Width        = 8
+ *    Poly         = 0x07
+ *    XorIn        = 0x00
+ *    ReflectIn    = False
+ *    XorOut       = 0x00
+ *    ReflectOut   = False
+ *    Algorithm    = table-driven
+ */
+const quint8 crc_table[256] = {
+    0x00, 0x07, 0x0e, 0x09, 0x1c, 0x1b, 0x12, 0x15, 0x38, 0x3f, 0x36, 0x31, 0x24, 0x23, 0x2a, 0x2d,
+    0x70, 0x77, 0x7e, 0x79, 0x6c, 0x6b, 0x62, 0x65, 0x48, 0x4f, 0x46, 0x41, 0x54, 0x53, 0x5a, 0x5d,
+    0xe0, 0xe7, 0xee, 0xe9, 0xfc, 0xfb, 0xf2, 0xf5, 0xd8, 0xdf, 0xd6, 0xd1, 0xc4, 0xc3, 0xca, 0xcd,
+    0x90, 0x97, 0x9e, 0x99, 0x8c, 0x8b, 0x82, 0x85, 0xa8, 0xaf, 0xa6, 0xa1, 0xb4, 0xb3, 0xba, 0xbd,
+    0xc7, 0xc0, 0xc9, 0xce, 0xdb, 0xdc, 0xd5, 0xd2, 0xff, 0xf8, 0xf1, 0xf6, 0xe3, 0xe4, 0xed, 0xea,
+    0xb7, 0xb0, 0xb9, 0xbe, 0xab, 0xac, 0xa5, 0xa2, 0x8f, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9d, 0x9a,
+    0x27, 0x20, 0x29, 0x2e, 0x3b, 0x3c, 0x35, 0x32, 0x1f, 0x18, 0x11, 0x16, 0x03, 0x04, 0x0d, 0x0a,
+    0x57, 0x50, 0x59, 0x5e, 0x4b, 0x4c, 0x45, 0x42, 0x6f, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7d, 0x7a,
+    0x89, 0x8e, 0x87, 0x80, 0x95, 0x92, 0x9b, 0x9c, 0xb1, 0xb6, 0xbf, 0xb8, 0xad, 0xaa, 0xa3, 0xa4,
+    0xf9, 0xfe, 0xf7, 0xf0, 0xe5, 0xe2, 0xeb, 0xec, 0xc1, 0xc6, 0xcf, 0xc8, 0xdd, 0xda, 0xd3, 0xd4,
+    0x69, 0x6e, 0x67, 0x60, 0x75, 0x72, 0x7b, 0x7c, 0x51, 0x56, 0x5f, 0x58, 0x4d, 0x4a, 0x43, 0x44,
+    0x19, 0x1e, 0x17, 0x10, 0x05, 0x02, 0x0b, 0x0c, 0x21, 0x26, 0x2f, 0x28, 0x3d, 0x3a, 0x33, 0x34,
+    0x4e, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5c, 0x5b, 0x76, 0x71, 0x78, 0x7f, 0x6a, 0x6d, 0x64, 0x63,
+    0x3e, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2c, 0x2b, 0x06, 0x01, 0x08, 0x0f, 0x1a, 0x1d, 0x14, 0x13,
+    0xae, 0xa9, 0xa0, 0xa7, 0xb2, 0xb5, 0xbc, 0xbb, 0x96, 0x91, 0x98, 0x9f, 0x8a, 0x8d, 0x84, 0x83,
+    0xde, 0xd9, 0xd0, 0xd7, 0xc2, 0xc5, 0xcc, 0xcb, 0xe6, 0xe1, 0xe8, 0xef, 0xfa, 0xfd, 0xf4, 0xf3
+};
+
+quint8 Crc::updateCRC(quint8 crc, const quint8 data)
+{
+    return crc_table[crc ^ data];
+}
+
+quint8 Crc::updateCRC(quint8 crc, const quint8 *data, qint32 length)
+{
+    while (length--) {
+        crc = crc_table[crc ^ *data++];
+    }
+    return crc;
+}

ground/openpilotgcs/src/libs/utils/crc.h

@@ -0,0 +1,58 @@
+/**
+ ******************************************************************************
+ *
+ * @file       crc.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013.
+ *             Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009.
+ * @addtogroup GCSPlugins GCS Plugins
+ * @{
+ * @addtogroup CorePlugin Core Plugin
+ * @{
+ * @brief The Core GCS plugin
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#ifndef CRC_H
+#define CRC_H
+
+#include "utils_global.h"
+
+namespace Utils {
+class QTCREATOR_UTILS_EXPORT Crc {
+public:
+    /**
+     * Update the crc value with new data.
+     *
+     * \param crc      The current crc value.
+     * \param data     data to update the crc with.
+     * \return         The updated crc value.
+     */
+    static quint8 updateCRC(quint8 crc, const quint8 data);
+
+    /**
+     * Update the crc value with new data.
+     *
+     * \param crc      The current crc value.
+     * \param data     Pointer to a buffer of \a data_len bytes.
+     * \param length   Number of bytes in the \a data buffer.
+     * \return         The updated crc value.
+     */
+    static quint8 updateCRC(quint8 crc, const quint8 *data, qint32 length);
+};
+} // namespace Utils
+
+#endif // CRC_H

ground/openpilotgcs/src/libs/utils/mytabbedstackwidget.cpp

@@ -137,6 +137,7 @@ void MyTabbedStackWidget::showWidget(int index)
 void MyTabbedStackWidget::resizeEvent(QResizeEvent *event)
 {
     QWidget::resizeEvent(event);
+
     m_listWidget->setFixedWidth(m_listWidget->verticalScrollBar()->isVisible() ? LIST_VIEW_WIDTH + 20 : LIST_VIEW_WIDTH);
 }
 

ground/openpilotgcs/src/libs/utils/mytabbedstackwidget.h

@@ -79,7 +79,7 @@ private:
     static const int LIST_VIEW_WIDTH = 80;
 
 protected:
-    void resizeEvent(QResizeEvent * event);
+    void resizeEvent(QResizeEvent *event);
 };
 
 #endif // MYTABBEDSTACKWIDGET_H

ground/openpilotgcs/src/libs/utils/utils.pro

@@ -55,7 +55,8 @@ SOURCES += reloadpromptutils.cpp \
     cachedsvgitem.cpp \
     svgimageprovider.cpp \
     hostosinfo.cpp \
-    logfile.cpp
+    logfile.cpp \
+    crc.cpp
 
 SOURCES += xmlconfig.cpp
 
@@ -113,7 +114,8 @@ HEADERS += utils_global.h \
     cachedsvgitem.h \
     svgimageprovider.h \
     hostosinfo.h \
-    logfile.h
+    logfile.h \
+    crc.h
 
 
 HEADERS += xmlconfig.h

ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configcustomwidget.h

@@ -33,7 +33,6 @@
 #include "extensionsystem/pluginmanager.h"
 #include "uavobjectmanager.h"
 #include "uavobject.h"
-#include "uavtalk/telemetrymanager.h"
 
 #include <QWidget>
 #include <QList>

ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.h

@@ -33,7 +33,7 @@
 #include "extensionsystem/pluginmanager.h"
 #include "uavobjectmanager.h"
 #include "uavobject.h"
-#include "uavtalk/telemetrymanager.h"
+
 #include <QWidget>
 #include <QList>
 #include <QItemDelegate>

ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configgroundvehiclewidget.h

@@ -33,7 +33,7 @@
 #include "extensionsystem/pluginmanager.h"
 #include "uavobjectmanager.h"
 #include "uavobject.h"
-#include "uavtalk/telemetrymanager.h"
+
 #include <QWidget>
 #include <QList>
 #include <QItemDelegate>

ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configmultirotorwidget.h

@@ -33,7 +33,6 @@
 #include "extensionsystem/pluginmanager.h"
 #include "uavobjectmanager.h"
 #include "uavobject.h"
-#include "uavtalk/telemetrymanager.h"
 
 #include <QtCore/QList>
 #include <QWidget>

ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp

@@ -42,6 +42,8 @@
 #include "defaulthwsettingswidget.h"
 #include "uavobjectutilmanager.h"
 
+#include <uavtalk/telemetrymanager.h>
+
 #include <QDebug>
 #include <QStringList>
 #include <QWidget>

ground/openpilotgcs/src/plugins/config/configgadgetwidget.h

@@ -27,18 +27,18 @@
 #ifndef CONFIGGADGETWIDGET_H
 #define CONFIGGADGETWIDGET_H
 
-#include "uavtalk/telemetrymanager.h"
 #include "extensionsystem/pluginmanager.h"
 #include "uavobjectmanager.h"
 #include "uavobject.h"
 #include "objectpersistence.h"
+#include "utils/pathutils.h"
+#include "utils/mytabbedstackwidget.h"
+#include "../uavobjectwidgetutils/configtaskwidget.h"
+
 #include <QWidget>
 #include <QList>
 #include <QTextBrowser>
-#include "utils/pathutils.h"
 #include <QMessageBox>
-#include "utils/mytabbedstackwidget.h"
-#include "../uavobjectwidgetutils/configtaskwidget.h"
 
 class ConfigGadgetWidget : public QWidget {
     Q_OBJECT

ground/openpilotgcs/src/plugins/config/configinputwidget.cpp

@@ -27,7 +27,8 @@
 
 #include "configinputwidget.h"
 
-#include "uavtalk/telemetrymanager.h"
+#include <extensionsystem/pluginmanager.h>
+#include <coreplugin/generalsettings.h>
 
 #include <QDebug>
 #include <QStringList>
@@ -41,9 +42,6 @@
 #include <utils/stylehelper.h>
 #include <QMessageBox>
 
-#include <extensionsystem/pluginmanager.h>
-#include <coreplugin/generalsettings.h>
-
 #define ACCESS_MIN_MOVE -3
 #define ACCESS_MAX_MOVE 3
 #define STICK_MIN_MOVE  -8

ground/openpilotgcs/src/plugins/config/configoutputwidget.cpp

@@ -29,7 +29,14 @@
 #include "outputchannelform.h"
 #include "configvehicletypewidget.h"
 
-#include "uavtalk/telemetrymanager.h"
+#include "mixersettings.h"
+#include "actuatorcommand.h"
+#include "actuatorsettings.h"
+#include "systemalarms.h"
+#include "systemsettings.h"
+#include "uavsettingsimportexport/uavsettingsimportexportfactory.h"
+#include <extensionsystem/pluginmanager.h>
+#include <coreplugin/generalsettings.h>
 
 #include <QDebug>
 #include <QStringList>
@@ -40,14 +47,6 @@
 #include <QMessageBox>
 #include <QDesktopServices>
 #include <QUrl>
-#include "mixersettings.h"
-#include "actuatorcommand.h"
-#include "actuatorsettings.h"
-#include "systemalarms.h"
-#include "systemsettings.h"
-#include "uavsettingsimportexport/uavsettingsimportexportfactory.h"
-#include <extensionsystem/pluginmanager.h>
-#include <coreplugin/generalsettings.h>
 
 ConfigOutputWidget::ConfigOutputWidget(QWidget *parent) : ConfigTaskWidget(parent), wasItMe(false)
 {

ground/openpilotgcs/src/plugins/config/configplugin.h

@@ -31,7 +31,6 @@
 #include <coreplugin/icore.h>
 #include <coreplugin/coreconstants.h>
 #include <coreplugin/actionmanager/actionmanager.h>
-#include "uavtalk/telemetrymanager.h"
 #include "objectpersistence.h"
 
 #include <QMessageBox>

ground/openpilotgcs/src/plugins/config/input.ui

@@ -128,8 +128,8 @@
            <rect>
             <x>0</x>
             <y>0</y>
-            <width>766</width>
+            <width>768</width>
-            <height>745</height>
+            <height>742</height>
            </rect>
           </property>
           <layout class="QGridLayout" name="gridLayout">
@@ -559,8 +559,8 @@
            <rect>
             <x>0</x>
             <y>0</y>
-            <width>766</width>
+            <width>768</width>
-            <height>745</height>
+            <height>742</height>
            </rect>
           </property>
           <layout class="QGridLayout" name="gridLayout_7" rowstretch="1,0,0,0">
@@ -746,18 +746,18 @@ margin:1px;</string>
                <widget class="QFrame" name="frame_3">
                 <property name="minimumSize">
                  <size>
-                  <width>75</width>
+                  <width>106</width>
                   <height>0</height>
                  </size>
                 </property>
                 <property name="maximumSize">
                  <size>
-                  <width>75</width>
+                  <width>105</width>
                   <height>16777215</height>
                  </size>
                 </property>
                 <property name="frameShape">
-                 <enum>QFrame::StyledPanel</enum>
+                 <enum>QFrame::NoFrame</enum>
                 </property>
                 <property name="frameShadow">
                  <enum>QFrame::Raised</enum>
@@ -799,7 +799,7 @@ margin:1px;</string>
               <item row="1" column="3">
                <widget class="QFrame" name="frame_2">
                 <property name="frameShape">
-                 <enum>QFrame::StyledPanel</enum>
+                 <enum>QFrame::NoFrame</enum>
                 </property>
                 <property name="frameShadow">
                  <enum>QFrame::Raised</enum>
@@ -850,7 +850,7 @@ margin:1px;</string>
               <item row="1" column="7">
                <widget class="QFrame" name="frame_5">
                 <property name="frameShape">
-                 <enum>QFrame::StyledPanel</enum>
+                 <enum>QFrame::NoFrame</enum>
                 </property>
                 <property name="frameShadow">
                  <enum>QFrame::Raised</enum>
@@ -901,7 +901,7 @@ margin:1px;</string>
               <item row="1" column="9">
                <widget class="QFrame" name="frame_6">
                 <property name="frameShape">
-                 <enum>QFrame::StyledPanel</enum>
+                 <enum>QFrame::NoFrame</enum>
                 </property>
                 <property name="frameShadow">
                  <enum>QFrame::Raised</enum>
@@ -1005,7 +1005,7 @@ margin:1px;</string>
                <widget class="QLabel" name="label_11">
                 <property name="minimumSize">
                  <size>
-                  <width>120</width>
+                  <width>80</width>
                   <height>20</height>
                  </size>
                 </property>
@@ -1034,7 +1034,7 @@ margin:1px;</string>
                <widget class="QLabel" name="label_16">
                 <property name="minimumSize">
                  <size>
-                  <width>120</width>
+                  <width>132</width>
                   <height>20</height>
                  </size>
                 </property>
@@ -1181,7 +1181,7 @@ channel value for each flight mode.</string>
                <widget class="QLabel" name="label_12">
                 <property name="minimumSize">
                  <size>
-                  <width>120</width>
+                  <width>105</width>
                   <height>20</height>
                  </size>
                 </property>
@@ -1217,7 +1217,7 @@ margin:1px;</string>
                <widget class="QLabel" name="label_13">
                 <property name="minimumSize">
                  <size>
-                  <width>120</width>
+                  <width>138</width>
                   <height>20</height>
                  </size>
                 </property>
@@ -1246,7 +1246,7 @@ margin:1px;</string>
                <widget class="QFrame" name="frame_4">
                 <property name="minimumSize">
                  <size>
-                  <width>120</width>
+                  <width>30</width>
                   <height>0</height>
                  </size>
                 </property>
@@ -1299,7 +1299,7 @@ margin:1px;</string>
                     </font>
                    </property>
                    <property name="toolTip">
-                    <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Enabling this check this box will enable Cruise Control for Flight Mode Switch position #1.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+                    <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Enabling this checkbox will enable Cruise Control for Flight Mode Switch position #1.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
                    </property>
                    <property name="layoutDirection">
                     <enum>Qt::RightToLeft</enum>
@@ -1340,7 +1340,7 @@ margin:1px;</string>
                     </font>
                    </property>
                    <property name="toolTip">
-                    <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Enabling this check this box will enable Cruise Control for Flight Mode Switch position #2.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+                    <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Enabling this checkbox will enable Cruise Control for Flight Mode Switch position #2.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
                    </property>
                    <property name="layoutDirection">
                     <enum>Qt::RightToLeft</enum>
@@ -1381,7 +1381,7 @@ margin:1px;</string>
                     </font>
                    </property>
                    <property name="toolTip">
-                    <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Enabling this check this box will enable Cruise Control for Flight Mode Switch position #3.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+                    <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Enabling this checkbox will enable Cruise Control for Flight Mode Switch position #3.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
                    </property>
                    <property name="layoutDirection">
                     <enum>Qt::RightToLeft</enum>
@@ -1425,7 +1425,7 @@ margin:1px;</string>
                     </font>
                    </property>
                    <property name="toolTip">
-                    <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Enabling this check this box will enable Cruise Control for Flight Mode Switch position #4.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+                    <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Enabling this checkbox will enable Cruise Control for Flight Mode Switch position #4.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
                    </property>
                    <property name="layoutDirection">
                     <enum>Qt::RightToLeft</enum>
@@ -1469,7 +1469,7 @@ margin:1px;</string>
                     </font>
                    </property>
                    <property name="toolTip">
-                    <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Enabling this check this box will enable Cruise Control for Flight Mode Switch position #5.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+                    <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Enabling this checkbox will enable Cruise Control for Flight Mode Switch position #5.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
                    </property>
                    <property name="layoutDirection">
                     <enum>Qt::RightToLeft</enum>
@@ -1513,7 +1513,7 @@ margin:1px;</string>
                     </font>
                    </property>
                    <property name="toolTip">
-                    <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Enabling this check this box will enable Cruise Control for Flight Mode Switch position #6.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+                    <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Enabling this checkbox will enable Cruise Control for Flight Mode Switch position #6.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
                    </property>
                    <property name="layoutDirection">
                     <enum>Qt::RightToLeft</enum>
@@ -1540,18 +1540,18 @@ margin:1px;</string>
                <widget class="QFrame" name="frame">
                 <property name="minimumSize">
                  <size>
-                  <width>75</width>
+                  <width>90</width>
                   <height>0</height>
                  </size>
                 </property>
                 <property name="maximumSize">
                  <size>
-                  <width>75</width>
+                  <width>90</width>
                   <height>16777215</height>
                  </size>
                 </property>
                 <property name="frameShape">
-                 <enum>QFrame::StyledPanel</enum>
+                 <enum>QFrame::NoFrame</enum>
                 </property>
                 <property name="frameShadow">
                  <enum>QFrame::Raised</enum>
@@ -1809,6 +1809,9 @@ Setup the flight mode channel on the RC Input tab if you have not done so alread
                    <property name="focusPolicy">
                     <enum>Qt::StrongFocus</enum>
                    </property>
+                   <property name="toolTip">
+                    <string>Select the stabilization mode on this position (manual/stabilized/auto)</string>
+                   </property>
                   </widget>
                  </item>
                  <item row="0" column="1">
@@ -1841,6 +1844,9 @@ Setup the flight mode channel on the RC Input tab if you have not done so alread
                    <property name="focusPolicy">
                     <enum>Qt::StrongFocus</enum>
                    </property>
+                   <property name="toolTip">
+                    <string>Select the stabilization mode on this position (manual/stabilized/auto)</string>
+                   </property>
                   </widget>
                  </item>
                  <item row="2" column="1">
@@ -1848,6 +1854,9 @@ Setup the flight mode channel on the RC Input tab if you have not done so alread
                    <property name="focusPolicy">
                     <enum>Qt::StrongFocus</enum>
                    </property>
+                   <property name="toolTip">
+                    <string>Select the stabilization mode on this position (manual/stabilized/auto)</string>
+                   </property>
                   </widget>
                  </item>
                  <item row="3" column="1">
@@ -1868,6 +1877,9 @@ Setup the flight mode channel on the RC Input tab if you have not done so alread
                    <property name="focusPolicy">
                     <enum>Qt::StrongFocus</enum>
                    </property>
+                   <property name="toolTip">
+                    <string>Select the stabilization mode on this position (manual/stabilized/auto)</string>
+                   </property>
                   </widget>
                  </item>
                 </layout>
@@ -1877,7 +1889,7 @@ Setup the flight mode channel on the RC Input tab if you have not done so alread
                <widget class="QFrame" name="frame_7">
                 <property name="minimumSize">
                  <size>
-                  <width>120</width>
+                  <width>75</width>
                   <height>0</height>
                  </size>
                 </property>
@@ -1904,13 +1916,16 @@ Setup the flight mode channel on the RC Input tab if you have not done so alread
                   <widget class="QComboBox" name="pidBankSs1_0">
                    <property name="minimumSize">
                     <size>
-                     <width>102</width>
+                     <width>75</width>
                      <height>0</height>
                     </size>
                    </property>
                    <property name="focusPolicy">
                     <enum>Qt::StrongFocus</enum>
                    </property>
+                   <property name="toolTip">
+                    <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Select which set of roll rates / max bank angles / PIDs you want active on this switch position.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+                   </property>
                    <property name="objrelation" stdset="0">
                     <stringlist>
                      <string>objname:StabilizationSettings</string>
@@ -1927,13 +1942,16 @@ Setup the flight mode channel on the RC Input tab if you have not done so alread
                   <widget class="QComboBox" name="pidBankSs1_1">
                    <property name="minimumSize">
                     <size>
-                     <width>102</width>
+                     <width>75</width>
                      <height>0</height>
                     </size>
                    </property>
                    <property name="focusPolicy">
                     <enum>Qt::StrongFocus</enum>
                    </property>
+                   <property name="toolTip">
+                    <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Select which set of roll rates / max bank angles / PIDs you want active on this switch position.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+                   </property>
                    <property name="objrelation" stdset="0">
                     <stringlist>
                      <string>objname:StabilizationSettings</string>
@@ -1950,13 +1968,16 @@ Setup the flight mode channel on the RC Input tab if you have not done so alread
                   <widget class="QComboBox" name="pidBankSs1_2">
                    <property name="minimumSize">
                     <size>
-                     <width>102</width>
+                     <width>75</width>
                      <height>0</height>
                     </size>
                    </property>
                    <property name="focusPolicy">
                     <enum>Qt::StrongFocus</enum>
                    </property>
+                   <property name="toolTip">
+                    <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Select which set of roll rates / max bank angles / PIDs you want active on this switch position.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+                   </property>
                    <property name="objrelation" stdset="0">
                     <stringlist>
                      <string>objname:StabilizationSettings</string>
@@ -1976,13 +1997,16 @@ Setup the flight mode channel on the RC Input tab if you have not done so alread
                    </property>
                    <property name="minimumSize">
                     <size>
-                     <width>102</width>
+                     <width>75</width>
                      <height>0</height>
                     </size>
                    </property>
                    <property name="focusPolicy">
                     <enum>Qt::StrongFocus</enum>
                    </property>
+                   <property name="toolTip">
+                    <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Select which set of roll rates / max bank angles / PIDs you want active on this switch position.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+                   </property>
                    <property name="objrelation" stdset="0">
                     <stringlist>
                      <string>objname:StabilizationSettings</string>
@@ -2002,13 +2026,16 @@ Setup the flight mode channel on the RC Input tab if you have not done so alread
                    </property>
                    <property name="minimumSize">
                     <size>
-                     <width>102</width>
+                     <width>75</width>
                      <height>0</height>
                     </size>
                    </property>
                    <property name="focusPolicy">
                     <enum>Qt::StrongFocus</enum>
                    </property>
+                   <property name="toolTip">
+                    <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Select which set of roll rates / max bank angles / PIDs you want active on this switch position.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+                   </property>
                    <property name="objrelation" stdset="0">
                     <stringlist>
                      <string>objname:StabilizationSettings</string>
@@ -2028,13 +2055,16 @@ Setup the flight mode channel on the RC Input tab if you have not done so alread
                    </property>
                    <property name="minimumSize">
                     <size>
-                     <width>102</width>
+                     <width>75</width>
                      <height>0</height>
                     </size>
                    </property>
                    <property name="focusPolicy">
                     <enum>Qt::StrongFocus</enum>
                    </property>
+                   <property name="toolTip">
+                    <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Select which set of roll rates / max bank angles / PIDs you want active on this switch position.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+                   </property>
                    <property name="objrelation" stdset="0">
                     <stringlist>
                      <string>objname:StabilizationSettings</string>
@@ -2153,8 +2183,8 @@ Setup the flight mode channel on the RC Input tab if you have not done so alread
            <rect>
             <x>0</x>
             <y>0</y>
-            <width>766</width>
+            <width>504</width>
-            <height>745</height>
+            <height>156</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_2">

ground/openpilotgcs/src/plugins/config/stabilization.ui

@@ -559,9 +559,6 @@
            <property name="spacing">
             <number>0</number>
            </property>
-           <property name="topMargin">
-            <number>9</number>
-           </property>
            <item>
             <widget class="QTabBar" name="basicPIDBankTabBar" native="true"/>
            </item>
@@ -573,7 +570,7 @@
              <property name="styleSheet">
               <string notr="true">#basicPidBankFrame{
 	color: rgb(180, 180, 180);
-margin-top: -1px
+margin-top: -1px;
 }</string>
              </property>
              <property name="frameShape">
@@ -8617,16 +8614,16 @@ border-radius: 5;</string>
                </property>
                <layout class="QVBoxLayout" name="verticalLayout_11">
                 <property name="leftMargin">
-                 <number>9</number>
+                 <number>0</number>
                 </property>
                 <property name="topMargin">
-                 <number>9</number>
+                 <number>0</number>
                 </property>
                 <property name="rightMargin">
-                 <number>9</number>
+                 <number>0</number>
                 </property>
                 <property name="bottomMargin">
-                 <number>9</number>
+                 <number>0</number>
                 </property>
                 <item>
                  <widget class="QCheckBox" name="lowThrottleZeroIntegral_8">
@@ -8718,7 +8715,7 @@ border-radius: 5;</string>
              <property name="sizeHint" stdset="0">
               <size>
                <width>20</width>
-               <height>40</height>
+               <height>20</height>
               </size>
              </property>
             </spacer>
@@ -8766,9 +8763,6 @@ border-radius: 5;</string>
             <height>708</height>
            </rect>
           </property>
-          <property name="autoFillBackground">
-           <bool>true</bool>
-          </property>
           <layout class="QVBoxLayout" name="verticalLayout_29">
            <property name="spacing">
             <number>0</number>
@@ -8791,9 +8785,6 @@ border-radius: 5;</string>
               <enum>QFrame::Plain</enum>
              </property>
              <layout class="QVBoxLayout" name="verticalLayout_7">
-              <property name="topMargin">
-               <number>9</number>
-              </property>
               <item>
                <widget class="QGroupBox" name="advancedResponsivenessGroupBox">
                 <property name="sizePolicy">
@@ -18664,7 +18655,7 @@ border-radius: 5;</string>
            <item>
             <widget class="QGroupBox" name="groupBox_24">
              <property name="sizePolicy">
-              <sizepolicy hsizetype="Preferred" vsizetype="Minimum">
+              <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
                <horstretch>0</horstretch>
                <verstretch>0</verstretch>
               </sizepolicy>
@@ -18672,16 +18663,13 @@ border-radius: 5;</string>
              <property name="minimumSize">
               <size>
                <width>0</width>
-               <height>60</height>
+               <height>0</height>
               </size>
              </property>
              <property name="title">
               <string>Instant Update</string>
              </property>
              <layout class="QVBoxLayout" name="verticalLayout_16">
-              <property name="topMargin">
-               <number>9</number>
-              </property>
               <item>
                <widget class="QCheckBox" name="realTimeUpdates_12">
                 <property name="sizePolicy">
@@ -18710,19 +18698,6 @@ border-radius: 5;</string>
              </layout>
             </widget>
            </item>
-           <item>
-            <spacer name="verticalSpacer_5">
-             <property name="orientation">
-              <enum>Qt::Vertical</enum>
-             </property>
-             <property name="sizeHint" stdset="0">
-              <size>
-               <width>20</width>
-               <height>40</height>
-              </size>
-             </property>
-            </spacer>
-           </item>
            <item>
             <spacer name="verticalSpacer_2">
              <property name="orientation">
@@ -18731,7 +18706,7 @@ border-radius: 5;</string>
              <property name="sizeHint" stdset="0">
               <size>
                <width>20</width>
-               <height>200</height>
+               <height>20</height>
               </size>
              </property>
             </spacer>
@@ -18775,26 +18750,14 @@ border-radius: 5;</string>
            <rect>
             <x>0</x>
             <y>0</y>
-            <width>796</width>
+            <width>752</width>
-            <height>708</height>
+            <height>526</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_9">
            <property name="spacing">
             <number>0</number>
            </property>
-           <property name="leftMargin">
-            <number>9</number>
-           </property>
-           <property name="topMargin">
-            <number>9</number>
-           </property>
-           <property name="rightMargin">
-            <number>9</number>
-           </property>
-           <property name="bottomMargin">
-            <number>9</number>
-           </property>
            <item>
             <widget class="QTabBar" name="expertPIDBankTabBar" native="true"/>
            </item>
@@ -21272,18 +21235,6 @@ border-radius: 5;</string>
            </item>
            <item>
             <widget class="QGroupBox" name="groupBox_4">
-             <property name="sizePolicy">
-              <sizepolicy hsizetype="Preferred" vsizetype="Fixed">
-               <horstretch>0</horstretch>
-               <verstretch>0</verstretch>
-              </sizepolicy>
-             </property>
-             <property name="minimumSize">
-              <size>
-               <width>0</width>
-               <height>0</height>
-              </size>
-             </property>
              <property name="autoFillBackground">
               <bool>false</bool>
              </property>
@@ -21355,7 +21306,7 @@ border-radius: 5;</string>
                </widget>
               </item>
               <item row="1" column="0" colspan="2">
-               <widget class="QGroupBox" name="groupBox">
+               <widget class="QGroupBox" name="groupBox2">
                 <property name="styleSheet">
                  <string>QGroupBox{border: 0px;}</string>
                 </property>
@@ -23914,24 +23865,6 @@ border-radius: 5;</string>
            </item>
            <item>
             <widget class="QGroupBox" name="RateStabilizationGroup_23">
-             <property name="sizePolicy">
-              <sizepolicy hsizetype="Preferred" vsizetype="Fixed">
-               <horstretch>0</horstretch>
-               <verstretch>0</verstretch>
-              </sizepolicy>
-             </property>
-             <property name="minimumSize">
-              <size>
-               <width>0</width>
-               <height>0</height>
-              </size>
-             </property>
-             <property name="maximumSize">
-              <size>
-               <width>16777215</width>
-               <height>16777215</height>
-              </size>
-             </property>
              <property name="palette">
               <palette>
                <active>
@@ -24452,9 +24385,6 @@ border-radius: 5;</string>
              <property name="title">
               <string>Sensor Tuning</string>
              </property>
-             <property name="alignment">
-              <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
-             </property>
              <layout class="QGridLayout" name="gridLayout">
               <property name="leftMargin">
                <number>9</number>
@@ -26859,7 +26789,7 @@ border-radius: 5;</string>
              </property>
              <property name="sizeHint" stdset="0">
               <size>
-               <width>40</width>
+               <width>20</width>
                <height>9</height>
               </size>
              </property>
@@ -26867,18 +26797,6 @@ border-radius: 5;</string>
            </item>
            <item>
             <widget class="QGroupBox" name="groupBox_3">
-             <property name="sizePolicy">
-              <sizepolicy hsizetype="Preferred" vsizetype="Minimum">
-               <horstretch>0</horstretch>
-               <verstretch>0</verstretch>
-              </sizepolicy>
-             </property>
-             <property name="minimumSize">
-              <size>
-               <width>0</width>
-               <height>60</height>
-              </size>
-             </property>
              <property name="title">
               <string>Instant Update</string>
              </property>
@@ -26919,7 +26837,7 @@ border-radius: 5;</string>
              <property name="sizeHint" stdset="0">
               <size>
                <width>20</width>
-               <height>149</height>
+               <height>20</height>
               </size>
              </property>
             </spacer>
@@ -26963,8 +26881,8 @@ border-radius: 5;</string>
            <rect>
             <x>0</x>
             <y>0</y>
-            <width>796</width>
+            <width>284</width>
-            <height>708</height>
+            <height>133</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_17">
@@ -27778,14 +27696,11 @@ border-radius: 5;</string>
            <rect>
             <x>0</x>
             <y>0</y>
-            <width>796</width>
+            <width>347</width>
-            <height>708</height>
+            <height>500</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_18">
-           <property name="spacing">
-            <number>9</number>
-           </property>
            <item>
             <widget class="QGroupBox" name="RateStabilizationGroup_11">
              <property name="sizePolicy">
@@ -28859,7 +28774,7 @@ border-radius: 5;</string>
                     <string notr="true"/>
                    </property>
                    <property name="text">
-                    <string>Integral</string>
+                    <string>Velocity Proportional</string>
                    </property>
                    <property name="alignment">
                     <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
@@ -28884,7 +28799,7 @@ border-radius: 5;</string>
                     <string notr="true"/>
                    </property>
                    <property name="text">
-                    <string>Proportional</string>
+                    <string>Altitude Proportional</string>
                    </property>
                    <property name="alignment">
                     <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
@@ -28915,7 +28830,7 @@ border-radius: 5;</string>
                     <string notr="true"/>
                    </property>
                    <property name="text">
-                    <string>Derivative</string>
+                    <string>Velocity Integral</string>
                    </property>
                    <property name="alignment">
                     <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
@@ -28939,8 +28854,9 @@ border-radius: 5;</string>
                    <property name="objrelation" stdset="0">
                     <stringlist>
                      <string>objname:AltitudeHoldSettings</string>
-                     <string>fieldname:Kp</string>
+                     <string>fieldname:AltitudePI</string>
-                     <string>scale:0.001</string>
+                     <string>element:Kp</string>
+                     <string>scale:0.01</string>
                      <string>haslimits:yes</string>
                      <string>buttongroup:98</string>
                     </stringlist>
@@ -28964,8 +28880,9 @@ border-radius: 5;</string>
                    <property name="objrelation" stdset="0">
                     <stringlist>
                      <string>objname:AltitudeHoldSettings</string>
-                     <string>fieldname:Ki</string>
+                     <string>fieldname:VelocityPI</string>
-                     <string>scale:0.001</string>
+                     <string>element:Kp</string>
+                     <string>scale:0.01</string>
                      <string>haslimits:yes</string>
                      <string>buttongroup:98</string>
                     </stringlist>
@@ -28975,7 +28892,7 @@ border-radius: 5;</string>
                  <item row="3" column="2">
                   <widget class="QSlider" name="AltKdSlider">
                    <property name="maximum">
-                    <number>100</number>
+                    <number>1000</number>
                    </property>
                    <property name="value">
                     <number>50</number>
@@ -28989,8 +28906,9 @@ border-radius: 5;</string>
                    <property name="objrelation" stdset="0">
                     <stringlist>
                      <string>objname:AltitudeHoldSettings</string>
-                     <string>fieldname:Kd</string>
+                     <string>fieldname:VelocityPI</string>
-                     <string>scale:0.001</string>
+                     <string>element:Ki</string>
+                     <string>scale:0.00001</string>
                      <string>haslimits:yes</string>
                      <string>buttongroup:98</string>
                     </stringlist>
@@ -29556,7 +29474,7 @@ color: rgb(255, 255, 255);
 border-radius: 5;</string>
                    </property>
                    <property name="text">
-                    <string>Altitude</string>
+                    <string>Control Coefficients</string>
                    </property>
                    <property name="alignment">
                     <set>Qt::AlignCenter</set>
@@ -29601,8 +29519,9 @@ border-radius: 5;</string>
                    <property name="objrelation" stdset="0">
                     <stringlist>
                      <string>objname:AltitudeHoldSettings</string>
-                     <string>fieldname:Kp</string>
+                     <string>fieldname:AltitudePI</string>
-                     <string>scale:0.001</string>
+                     <string>element:Kp</string>
+                     <string>scale:0.01</string>
                      <string>haslimits:yes</string>
                      <string>buttongroup:98</string>
                     </stringlist>
@@ -29647,8 +29566,9 @@ border-radius: 5;</string>
                    <property name="objrelation" stdset="0">
                     <stringlist>
                      <string>objname:AltitudeHoldSettings</string>
-                     <string>fieldname:Ki</string>
+                     <string>fieldname:VelocityPI</string>
-                     <string>scale:0.001</string>
+                     <string>element:Kp</string>
+                     <string>scale:0.01</string>
                      <string>haslimits:yes</string>
                      <string>buttongroup:98</string>
                     </stringlist>
@@ -29685,7 +29605,7 @@ border-radius: 5;</string>
                     <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
                    </property>
                    <property name="maximum">
-                    <number>100</number>
+                    <number>1000</number>
                    </property>
                    <property name="value">
                     <number>51</number>
@@ -29693,8 +29613,9 @@ border-radius: 5;</string>
                    <property name="objrelation" stdset="0">
                     <stringlist>
                      <string>objname:AltitudeHoldSettings</string>
-                     <string>fieldname:Kd</string>
+                     <string>fieldname:VelocityPI</string>
-                     <string>scale:0.001</string>
+                     <string>element:Ki</string>
+                     <string>scale:0.00001</string>
                      <string>haslimits:yes</string>
                      <string>buttongroup:98</string>
                     </stringlist>

ground/openpilotgcs/src/plugins/coreplugin/mainwindow.cpp

@@ -188,7 +188,8 @@ MainWindow::MainWindow() :
 
 MainWindow::~MainWindow()
 {
-    if (m_connectionManager) { // Pip
+    if (m_connectionManager) {
+        // Pip
         m_connectionManager->disconnectDevice();
         m_connectionManager->suspendPolling();
     }
@@ -351,6 +352,9 @@ void MainWindow::closeEvent(QCloseEvent *event)
         saveSettings(m_settings);
         m_uavGadgetInstanceManager->saveSettings(m_settings);
     }
+
+    qApp->closeAllWindows();
+
     event->accept();
 }
 

ground/openpilotgcs/src/plugins/hitl/isimulator.h

@@ -1,12 +1,6 @@
 #ifndef ISIMULATOR_H
 #define ISIMULATOR_H
 
-
-#include <QObject>
-#include <QUdpSocket>
-#include <QTimer>
-#include <math.h>
-#include "uavtalk/telemetrymanager.h"
 #include "uavobjectmanager.h"
 #include "actuatordesired.h"
 #include "altitudestate.h"
@@ -15,6 +9,11 @@
 #include "positionstate.h"
 #include "gcstelemetrystats.h"
 
+#include <QObject>
+#include <QUdpSocket>
+#include <QTimer>
+#include <math.h>
+
 class Simulator : public QObject {
     Q_OBJECT
 public:

ground/openpilotgcs/src/plugins/hitl/simulator.cpp

@@ -27,11 +27,13 @@
 
 
 #include "simulator.h"
-#include "extensionsystem/pluginmanager.h"
-#include "coreplugin/icore.h"
-#include "coreplugin/threadmanager.h"
 #include "hitlnoisegeneration.h"
 
+#include <extensionsystem/pluginmanager.h>
+#include <coreplugin/icore.h>
+#include <coreplugin/threadmanager.h>
+#include <uavtalk/telemetrymanager.h>
+
 volatile bool Simulator::isStarted = false;
 
 const float Simulator::GEE      = 9.81;

ground/openpilotgcs/src/plugins/hitl/simulator.h

@@ -28,14 +28,7 @@
 #ifndef ISIMULATOR_H
 #define ISIMULATOR_H
 
-#include <QObject>
-#include <QUdpSocket>
-#include <QTimer>
-#include <QProcess>
-#include <qmath.h>
-
 #include "qscopedpointer.h"
-#include "uavtalk/telemetrymanager.h"
 #include "uavobjectmanager.h"
 
 #include "accelstate.h"
@@ -61,6 +54,13 @@
 
 #include "utils/coordinateconversions.h"
 
+#include <QObject>
+#include <QUdpSocket>
+#include <QTime>
+#include <QTimer>
+#include <QProcess>
+#include <qmath.h>
+
 /**
  * just imagine this was a class without methods and all public properties
  */

ground/openpilotgcs/src/plugins/hitl/xplanesimulator.cpp

@@ -318,10 +318,10 @@ void XplaneSimulator::processUpdate(const QByteArray & dataBuf)
         out.velEast   = velX;
         out.velDown   = -velZ;
 
-        // Update gyroscope sensor data
+        // Update gyroscope sensor data - convert from rad/s to deg/s
-        out.rollRate  = rollRate_rad;
+        out.rollRate  = rollRate_rad * (180.0 / M_PI);
-        out.pitchRate = pitchRate_rad;
+        out.pitchRate = pitchRate_rad * (180.0 / M_PI);
-        out.yawRate   = yawRate_rad;
+        out.yawRate   = yawRate_rad * (180.0 / M_PI);
 
         // Update accelerometer sensor data
         out.accX      = accX;

ground/openpilotgcs/src/plugins/notify/notifyplugin.cpp

@@ -29,15 +29,17 @@
 #include "notificationitem.h"
 #include "notifypluginoptionspage.h"
 #include "notifylogging.h"
+
+#include <extensionsystem/pluginmanager.h>
 #include <coreplugin/icore.h>
+#include <uavtalk/telemetrymanager.h>
+
+#include <iostream>
+
 #include <QDebug>
 #include <QtPlugin>
 #include <QStringList>
 
-#include <extensionsystem/pluginmanager.h>
-
-#include <iostream>
-
 static const QString VERSION = "1.0.0";
 
 // #define DEBUG_NOTIFIES
@@ -125,7 +127,8 @@ void SoundNotifyPlugin::readConfig(QSettings *settings, UAVConfigInfo * /* confi
 
 void SoundNotifyPlugin::onTelemetryManagerAdded(QObject *obj)
 {
-    telMngr = qobject_cast<TelemetryManager *>(obj);
+    TelemetryManager *telMngr = qobject_cast<TelemetryManager *>(obj);
+
     if (telMngr) {
         connect(telMngr, SIGNAL(disconnected()), this, SLOT(onAutopilotDisconnect()));
     }

ground/openpilotgcs/src/plugins/notify/notifyplugin.h

@@ -29,7 +29,6 @@
 
 #include <extensionsystem/iplugin.h>
 #include <coreplugin/iconfigurableplugin.h>
-#include "uavtalk/telemetrymanager.h"
 #include "uavobjectmanager.h"
 #include "uavobject.h"
 #include "notificationitem.h"
@@ -111,7 +110,6 @@ private:
 
     PhononObject phonon;
     NotifyPluginOptionsPage *mop;
-    TelemetryManager *telMngr;
     QMediaPlaylist *playlist;
 };
 

ground/openpilotgcs/src/plugins/opmap/flightdatamodel.cpp

@@ -28,6 +28,7 @@
 #include "flightdatamodel.h"
 #include <QMessageBox>
 #include <QDomDocument>
+
 flightDataModel::flightDataModel(QObject *parent) : QAbstractTableModel(parent)
 {}
 
@@ -72,329 +73,275 @@ QVariant flightDataModel::data(const QModelIndex &index, int role) const
 
 bool flightDataModel::setColumnByIndex(pathPlanData *row, const int index, const QVariant value)
 {
+    bool b;
+
     switch (index) {
     case WPDESCRITPTION:
         row->wpDescritption = value.toString();
-        return true;
+        b = true;
-
         break;
     case LATPOSITION:
         row->latPosition = value.toDouble();
-        return true;
+        b = true;
-
         break;
     case LNGPOSITION:
         row->lngPosition = value.toDouble();
-        return true;
+        b = true;
-
         break;
     case DISRELATIVE:
         row->disRelative = value.toDouble();
-        return true;
+        b = true;
-
         break;
     case BEARELATIVE:
         row->beaRelative = value.toDouble();
-        return true;
+        b = true;
-
         break;
     case ALTITUDERELATIVE:
         row->altitudeRelative = value.toFloat();
-        return true;
+        b = true;
-
         break;
     case ISRELATIVE:
         row->isRelative = value.toBool();
-        return true;
+        b = true;
-
         break;
     case ALTITUDE:
         row->altitude = value.toDouble();
-        return true;
+        b = true;
-
         break;
     case VELOCITY:
         row->velocity = value.toFloat();
-        return true;
+        b = true;
-
         break;
     case MODE:
         row->mode = value.toInt();
-        return true;
+        b = true;
-
         break;
     case MODE_PARAMS0:
         row->mode_params[0] = value.toFloat();
-        return true;
+        b = true;
-
         break;
     case MODE_PARAMS1:
         row->mode_params[1] = value.toFloat();
-        return true;
+        b = true;
-
         break;
     case MODE_PARAMS2:
         row->mode_params[2] = value.toFloat();
-        return true;
+        b = true;
-
         break;
     case MODE_PARAMS3:
         row->mode_params[3] = value.toFloat();
-        return true;
+        b = true;
-
         break;
     case CONDITION:
         row->condition = value.toInt();
-        return true;
+        b = true;
-
         break;
     case CONDITION_PARAMS0:
         row->condition_params[0] = value.toFloat();
-        return true;
+        b = true;
-
         break;
     case CONDITION_PARAMS1:
         row->condition_params[1] = value.toFloat();
-        return true;
+        b = true;
-
         break;
     case CONDITION_PARAMS2:
         row->condition_params[2] = value.toFloat();
-        return true;
+        b = true;
-
         break;
     case CONDITION_PARAMS3:
         row->condition_params[3] = value.toFloat();
-        return true;
+        b = true;
-
         break;
     case COMMAND:
         row->command = value.toInt();
+        b = true;
         break;
     case JUMPDESTINATION:
         row->jumpdestination = value.toInt();
-        return true;
+        b = true;
-
         break;
     case ERRORDESTINATION:
         row->errordestination = value.toInt();
-        return true;
+        b = true;
-
         break;
     case LOCKED:
         row->locked = value.toBool();
-        return true;
+        b = true;
-
         break;
     default:
-        return false;
+        b = false;
+        break;
     }
-    return false;
+    return b;
 }
+
 QVariant flightDataModel::getColumnByIndex(const pathPlanData *row, const int index) const
 {
+    QVariant value;
+
     switch (index) {
     case WPDESCRITPTION:
-        return row->wpDescritption;
+        value = row->wpDescritption;
-
         break;
     case LATPOSITION:
-        return row->latPosition;
+        value = row->latPosition;
-
         break;
     case LNGPOSITION:
-        return row->lngPosition;
+        value = row->lngPosition;
-
         break;
     case DISRELATIVE:
-        return row->disRelative;
+        value = row->disRelative;
-
         break;
     case BEARELATIVE:
-        return row->beaRelative;
+        value = row->beaRelative;
-
         break;
     case ALTITUDERELATIVE:
-        return row->altitudeRelative;
+        value = row->altitudeRelative;
-
         break;
     case ISRELATIVE:
-        return row->isRelative;
+        value = row->isRelative;
-
         break;
     case ALTITUDE:
-        return row->altitude;
+        value = row->altitude;
-
         break;
     case VELOCITY:
-        return row->velocity;
+        value = row->velocity;
-
         break;
     case MODE:
-        return row->mode;
+        value = row->mode;
-
         break;
     case MODE_PARAMS0:
-        return row->mode_params[0];
+        value = row->mode_params[0];
-
         break;
     case MODE_PARAMS1:
-        return row->mode_params[1];
+        value = row->mode_params[1];
-
         break;
     case MODE_PARAMS2:
-        return row->mode_params[2];
+        value = row->mode_params[2];
-
         break;
     case MODE_PARAMS3:
-        return row->mode_params[3];
+        value = row->mode_params[3];
-
         break;
     case CONDITION:
-        return row->condition;
+        value = row->condition;
-
         break;
     case CONDITION_PARAMS0:
-        return row->condition_params[0];
+        value = row->condition_params[0];
-
         break;
     case CONDITION_PARAMS1:
-        return row->condition_params[1];
+        value = row->condition_params[1];
-
         break;
     case CONDITION_PARAMS2:
-        return row->condition_params[2];
+        value = row->condition_params[2];
-
         break;
     case CONDITION_PARAMS3:
-        return row->condition_params[3];
+        value = row->condition_params[3];
-
         break;
     case COMMAND:
-        return row->command;
+        value = row->command;
-
         break;
     case JUMPDESTINATION:
-        return row->jumpdestination;
+        value = row->jumpdestination;
-
         break;
     case ERRORDESTINATION:
-        return row->errordestination;
+        value = row->errordestination;
-
         break;
     case LOCKED:
-        return row->locked;
+        value = row->locked;
+        break;
     }
+    return value;
 }
+
 QVariant flightDataModel::headerData(int section, Qt::Orientation orientation, int role) const
 {
+    QVariant value;
+
     if (role == Qt::DisplayRole) {
         if (orientation == Qt::Vertical) {
-            return QString::number(section + 1);
+            value = QString::number(section + 1);
         } else if (orientation == Qt::Horizontal) {
             switch (section) {
             case WPDESCRITPTION:
-                return QString("Description");
+                value = QString("Description");
-
                 break;
             case LATPOSITION:
-                return QString("Latitude");
+                value = QString("Latitude");
-
                 break;
             case LNGPOSITION:
-                return QString("Longitude");
+                value = QString("Longitude");
-
                 break;
             case DISRELATIVE:
-                return QString("Distance to home");
+                value = QString("Distance to home");
-
                 break;
             case BEARELATIVE:
-                return QString("Bearing from home");
+                value = QString("Bearing from home");
-
                 break;
             case ALTITUDERELATIVE:
-                return QString("Altitude above home");
+                value = QString("Altitude above home");
-
                 break;
             case ISRELATIVE:
-                return QString("Relative to home");
+                value = QString("Relative to home");
-
                 break;
             case ALTITUDE:
-                return QString("Altitude");
+                value = QString("Altitude");
-
                 break;
             case VELOCITY:
-                return QString("Velocity");
+                value = QString("Velocity");
-
                 break;
             case MODE:
-                return QString("Mode");
+                value = QString("Mode");
-
                 break;
             case MODE_PARAMS0:
-                return QString("Mode parameter 0");
+                value = QString("Mode parameter 0");
-
                 break;
             case MODE_PARAMS1:
-                return QString("Mode parameter 1");
+                value = QString("Mode parameter 1");
-
                 break;
             case MODE_PARAMS2:
-                return QString("Mode parameter 2");
+                value = QString("Mode parameter 2");
-
                 break;
             case MODE_PARAMS3:
-                return QString("Mode parameter 3");
+                value = QString("Mode parameter 3");
-
                 break;
             case CONDITION:
-                return QString("Condition");
+                value = QString("Condition");
-
                 break;
             case CONDITION_PARAMS0:
-                return QString("Condition parameter 0");
+                value = QString("Condition parameter 0");
-
                 break;
             case CONDITION_PARAMS1:
-                return QString("Condition parameter 1");
+                value = QString("Condition parameter 1");
-
                 break;
             case CONDITION_PARAMS2:
-                return QString("Condition parameter 2");
+                value = QString("Condition parameter 2");
-
                 break;
             case CONDITION_PARAMS3:
-                return QString("Condition parameter 3");
+                value = QString("Condition parameter 3");
-
                 break;
             case COMMAND:
-                return QString("Command");
+                value = QString("Command");
-
                 break;
             case JUMPDESTINATION:
-                return QString("Jump Destination");
+                value = QString("Jump Destination");
-
                 break;
             case ERRORDESTINATION:
-                return QString("Error Destination");
+                value = QString("Error Destination");
-
                 break;
             case LOCKED:
-                return QString("Locked");
+                value = QString("Locked");
-
                 break;
             default:
-                return QString();
+                value = QString();
-
                 break;
             }
         }
     } else {
-        return QAbstractTableModel::headerData(section, orientation, role);
+        value = QAbstractTableModel::headerData(section, orientation, role);
     }
+    return value;
 }
+
 bool flightDataModel::setData(const QModelIndex &index, const QVariant &value, int role)
 {
     if (role == Qt::EditRole) {
@@ -465,11 +412,12 @@ bool flightDataModel::insertRows(int row, int count, const QModelIndex & /*paren
         dataStorage.insert(row, data);
     }
     endInsertRows();
+    return true;
 }
 
 bool flightDataModel::removeRows(int row, int count, const QModelIndex & /*parent*/)
 {
-    if (row < 0) {
+    if (row < 0 || count <= 0) {
         return false;
     }
     beginRemoveRows(QModelIndex(), row, row + count - 1);
@@ -478,6 +426,7 @@ bool flightDataModel::removeRows(int row, int count, const QModelIndex & /*paren
         dataStorage.removeAt(row);
     }
     endRemoveRows();
+    return true;
 }
 
 bool flightDataModel::writeToFile(QString fileName)
@@ -617,6 +566,7 @@ bool flightDataModel::writeToFile(QString fileName)
     file.close();
     return true;
 }
+
 void flightDataModel::readFromFile(QString fileName)
 {
     // TODO warning message
@@ -657,54 +607,56 @@ void flightDataModel::readFromFile(QString fileName)
             while (!fieldNode.isNull()) {
                 QDomElement field = fieldNode.toElement();
                 if (field.tagName() == "field") {
-                    if (field.attribute("name") == "altitude") {
+                    QString name  = field.attribute("name");
-                        data->altitude = field.attribute("value").toDouble();
+                    QString value = field.attribute("value");
-                    } else if (field.attribute("name") == "description") {
+                    if (name == "altitude") {
-                        data->wpDescritption = field.attribute("value");
+                        data->altitude = value.toDouble();
-                    } else if (field.attribute("name") == "latitude") {
+                    } else if (name == "description") {
-                        data->latPosition = field.attribute("value").toDouble();
+                        data->wpDescritption = value;
-                    } else if (field.attribute("name") == "longitude") {
+                    } else if (name == "latitude") {
-                        data->lngPosition = field.attribute("value").toDouble();
+                        data->latPosition = value.toDouble();
-                    } else if (field.attribute("name") == "distance_to_home") {
+                    } else if (name == "longitude") {
-                        data->disRelative = field.attribute("value").toDouble();
+                        data->lngPosition = value.toDouble();
-                    } else if (field.attribute("name") == "bearing_from_home") {
+                    } else if (name == "distance_to_home") {
-                        data->beaRelative = field.attribute("value").toDouble();
+                        data->disRelative = value.toDouble();
-                    } else if (field.attribute("name") == "altitude_above_home") {
+                    } else if (name == "bearing_from_home") {
-                        data->altitudeRelative = field.attribute("value").toFloat();
+                        data->beaRelative = value.toDouble();
-                    } else if (field.attribute("name") == "is_relative_to_home") {
+                    } else if (name == "altitude_above_home") {
-                        data->isRelative = field.attribute("value").toInt();
+                        data->altitudeRelative = value.toFloat();
-                    } else if (field.attribute("name") == "altitude") {
+                    } else if (name == "is_relative_to_home") {
-                        data->altitude = field.attribute("value").toDouble();
+                        data->isRelative = value.toInt();
-                    } else if (field.attribute("name") == "velocity") {
+                    } else if (name == "altitude") {
-                        data->velocity = field.attribute("value").toFloat();
+                        data->altitude = value.toDouble();
-                    } else if (field.attribute("name") == "mode") {
+                    } else if (name == "velocity") {
-                        data->mode = field.attribute("value").toInt();
+                        data->velocity = value.toFloat();
-                    } else if (field.attribute("name") == "mode_param0") {
+                    } else if (name == "mode") {
-                        data->mode_params[0] = field.attribute("value").toFloat();
+                        data->mode = value.toInt();
-                    } else if (field.attribute("name") == "mode_param1") {
+                    } else if (name == "mode_param0") {
-                        data->mode_params[1] = field.attribute("value").toFloat();
+                        data->mode_params[0] = value.toFloat();
-                    } else if (field.attribute("name") == "mode_param2") {
+                    } else if (name == "mode_param1") {
-                        data->mode_params[2] = field.attribute("value").toFloat();
+                        data->mode_params[1] = value.toFloat();
-                    } else if (field.attribute("name") == "mode_param3") {
+                    } else if (name == "mode_param2") {
-                        data->mode_params[3] = field.attribute("value").toFloat();
+                        data->mode_params[2] = value.toFloat();
-                    } else if (field.attribute("name") == "condition") {
+                    } else if (name == "mode_param3") {
-                        data->condition = field.attribute("value").toDouble();
+                        data->mode_params[3] = value.toFloat();
-                    } else if (field.attribute("name") == "condition_param0") {
+                    } else if (name == "condition") {
-                        data->condition_params[0] = field.attribute("value").toFloat();
+                        data->condition = value.toDouble();
-                    } else if (field.attribute("name") == "condition_param1") {
+                    } else if (name == "condition_param0") {
-                        data->condition_params[1] = field.attribute("value").toFloat();
+                        data->condition_params[0] = value.toFloat();
-                    } else if (field.attribute("name") == "condition_param2") {
+                    } else if (name == "condition_param1") {
-                        data->condition_params[2] = field.attribute("value").toFloat();
+                        data->condition_params[1] = value.toFloat();
-                    } else if (field.attribute("name") == "condition_param3") {
+                    } else if (name == "condition_param2") {
-                        data->condition_params[3] = field.attribute("value").toFloat();
+                        data->condition_params[2] = value.toFloat();
-                    } else if (field.attribute("name") == "command") {
+                    } else if (name == "condition_param3") {
-                        data->command = field.attribute("value").toInt();
+                        data->condition_params[3] = value.toFloat();
-                    } else if (field.attribute("name") == "jumpdestination") {
+                    } else if (name == "command") {
-                        data->jumpdestination = field.attribute("value").toInt();
+                        data->command = value.toInt();
-                    } else if (field.attribute("name") == "errordestination") {
+                    } else if (name == "jumpdestination") {
-                        data->errordestination = field.attribute("value").toInt();
+                        data->jumpdestination = value.toInt();
-                    } else if (field.attribute("name") == "is_locked") {
+                    } else if (name == "errordestination") {
-                        data->locked = field.attribute("value").toInt();
+                        data->errordestination = value.toInt();
+                    } else if (name == "is_locked") {
+                        data->locked = value.toInt();
                     }
                 }
                 fieldNode = fieldNode.nextSibling();

ground/openpilotgcs/src/plugins/opmap/modelmapproxy.cpp

@@ -345,7 +345,10 @@ void modelMapProxy::createWayPoint(internals::PointLatLng coord)
     index = model->index(model->rowCount() - 1, flightDataModel::ERRORDESTINATION, QModelIndex());
     model->setData(index, 1, Qt::EditRole);
 }
+
 void modelMapProxy::deleteAll()
 {
-    model->removeRows(0, model->rowCount(), QModelIndex());
+    if (model->rowCount() > 0) {
+        model->removeRows(0, model->rowCount(), QModelIndex());
+    }
 }

ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp

@@ -26,222 +26,455 @@
  */
 #include "modeluavoproxy.h"
 #include "extensionsystem/pluginmanager.h"
+
 #include <math.h>
-modelUavoProxy::modelUavoProxy(QObject *parent, flightDataModel *model) : QObject(parent), myModel(model)
+
+ModelUavoProxy::ModelUavoProxy(QObject *parent, flightDataModel *model) : QObject(parent), myModel(model)
 {
     ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
 
     Q_ASSERT(pm != NULL);
-    objManager    = pm->getObject<UAVObjectManager>();
-    Q_ASSERT(objManager != NULL);
-    waypointObj   = Waypoint::GetInstance(objManager);
-    Q_ASSERT(waypointObj != NULL);
-    pathactionObj = PathAction::GetInstance(objManager);
-    Q_ASSERT(pathactionObj != NULL);
-}
-void modelUavoProxy::modelToObjects()
-{
-    PathAction *act = NULL;
-    Waypoint *wp    = NULL;
-    QModelIndex index;
-    double distance;
-    double bearing;
-    double altitude;
-    int lastaction = -1;
 
-    for (int x = 0; x < myModel->rowCount(); ++x) {
+    objMngr = pm->getObject<UAVObjectManager>();
-        int instances = objManager->getNumInstances(waypointObj->getObjID());
+    Q_ASSERT(objMngr != NULL);
-        if (x > instances - 1) {
+
-            wp = new Waypoint;
+    completionCountdown = 0;
-            wp->initialize(x, wp->getMetaObject());
+    successCountdown    = 0;
-            objManager->registerObject(wp);
+}
+
+void ModelUavoProxy::sendPathPlan()
+{
+    modelToObjects();
+
+    PathPlan *pathPlan = PathPlan::GetInstance(objMngr, 0);
+    connect(pathPlan, SIGNAL(transactionCompleted(UAVObject *, bool)),
+            this, SLOT(pathPlanElementSent(UAVObject *, bool)), Qt::UniqueConnection);
+
+    Waypoint *waypoint = Waypoint::GetInstance(objMngr, 0);
+    connect(waypoint, SIGNAL(transactionCompleted(UAVObject *, bool)),
+            this, SLOT(pathPlanElementSent(UAVObject *, bool)), Qt::UniqueConnection);
+
+    PathAction *action = PathAction::GetInstance(objMngr, 0);
+    connect(action, SIGNAL(transactionCompleted(UAVObject *, bool)),
+            this, SLOT(pathPlanElementSent(UAVObject *, bool)), Qt::UniqueConnection);
+
+    // we will start 3 update all
+    completionCountdown = 3;
+    successCountdown    = completionCountdown;
+
+    pathPlan->updated();
+    waypoint->updatedAll();
+    action->updatedAll();
+}
+
+void ModelUavoProxy::pathPlanElementSent(UAVObject *obj, bool success)
+{
+    obj->disconnect(this);
+
+    completionCountdown--;
+    successCountdown -= success ? 1 : 0;
+
+    if (completionCountdown == 0) {
+        qDebug() << "ModelUavoProxy::pathPlanSent - completed" << (successCountdown == 0);
+        if (successCountdown == 0) {
+            QMessageBox::information(NULL, tr("Path Plan Upload Successful"), tr("Path plan upload was successful."));
         } else {
-            wp = Waypoint::GetInstance(objManager, x);
+            QMessageBox::critical(NULL, tr("Path Plan Upload Failed"), tr("Failed to upload the path plan !"));
         }
-        act = new PathAction;
-        Q_ASSERT(act);
-        Q_ASSERT(wp);
-        Waypoint::DataFields waypoint = wp->getData();
-        PathAction::DataFields action = act->getData();
-
-        ///Waypoint object data
-        index    = myModel->index(x, flightDataModel::DISRELATIVE);
-        distance = myModel->data(index).toDouble();
-        index    = myModel->index(x, flightDataModel::BEARELATIVE);
-        bearing  = myModel->data(index).toDouble();
-        index    = myModel->index(x, flightDataModel::ALTITUDERELATIVE);
-        altitude = myModel->data(index).toFloat();
-        index    = myModel->index(x, flightDataModel::VELOCITY);
-        waypoint.Velocity = myModel->data(index).toFloat();
-
-        waypoint.Position[Waypoint::POSITION_NORTH] = distance * cos(bearing / 180 * M_PI);
-        waypoint.Position[Waypoint::POSITION_EAST]  = distance * sin(bearing / 180 * M_PI);
-        waypoint.Position[Waypoint::POSITION_DOWN]  = (-1.0f) * altitude;
-
-        ///PathAction object data
-        index = myModel->index(x, flightDataModel::MODE);
-        action.Mode = myModel->data(index).toInt();
-        index = myModel->index(x, flightDataModel::MODE_PARAMS0);
-        action.ModeParameters[0] = myModel->data(index).toFloat();
-        index = myModel->index(x, flightDataModel::MODE_PARAMS1);
-        action.ModeParameters[1] = myModel->data(index).toFloat();
-        index = myModel->index(x, flightDataModel::MODE_PARAMS2);
-        action.ModeParameters[2] = myModel->data(index).toFloat();
-        index = myModel->index(x, flightDataModel::MODE_PARAMS3);
-        action.ModeParameters[3] = myModel->data(index).toFloat();
-
-        index = myModel->index(x, flightDataModel::CONDITION);
-        action.EndCondition = myModel->data(index).toInt();
-        index = myModel->index(x, flightDataModel::CONDITION_PARAMS0);
-        action.ConditionParameters[0] = myModel->data(index).toFloat();
-        index = myModel->index(x, flightDataModel::CONDITION_PARAMS1);
-        action.ConditionParameters[1] = myModel->data(index).toFloat();
-        index = myModel->index(x, flightDataModel::CONDITION_PARAMS2);
-        action.ConditionParameters[2] = myModel->data(index).toFloat();
-        index = myModel->index(x, flightDataModel::CONDITION_PARAMS3);
-        action.ConditionParameters[3] = myModel->data(index).toFloat();
-
-        index = myModel->index(x, flightDataModel::COMMAND);
-        action.Command = myModel->data(index).toInt();
-        index = myModel->index(x, flightDataModel::JUMPDESTINATION);
-        action.JumpDestination = myModel->data(index).toInt() - 1;
-        index = myModel->index(x, flightDataModel::ERRORDESTINATION);
-        action.ErrorDestination = myModel->data(index).toInt() - 1;
-
-        int actionNumber = addAction(act, action, lastaction);
-        if (actionNumber > lastaction) {
-            lastaction = actionNumber;
-        }
-        waypoint.Action = actionNumber;
-        wp->setData(waypoint);
-        wp->updated();
     }
 }
 
-void modelUavoProxy::objectsToModel()
+void ModelUavoProxy::receivePathPlan()
 {
-    Waypoint *wp;
+    PathPlan *pathPlan = PathPlan::GetInstance(objMngr, 0);
-    Waypoint::DataFields wpfields;
-    PathAction *action;
-    QModelIndex index;
-    double distance;
-    double bearing;
 
-    PathAction::DataFields actionfields;
+    connect(pathPlan, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(pathPlanElementReceived(UAVObject *, bool)));
 
-    myModel->removeRows(0, myModel->rowCount());
+    Waypoint *waypoint = Waypoint::GetInstance(objMngr, 0);
-    for (int x = 0; x < objManager->getNumInstances(waypointObj->getObjID()); ++x) {
+    connect(waypoint, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(pathPlanElementReceived(UAVObject *, bool)));
-        wp = Waypoint::GetInstance(objManager, x);
+
-        Q_ASSERT(wp);
+    PathAction *action = PathAction::GetInstance(objMngr, 0);
-        if (!wp) {
+    connect(action, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(pathPlanElementReceived(UAVObject *, bool)));
-            continue;
+
+    // we will start 3 update requests
+    completionCountdown = 3;
+    successCountdown    = completionCountdown;
+
+    pathPlan->requestUpdate();
+    waypoint->requestUpdateAll();
+    action->requestUpdateAll();
+}
+
+void ModelUavoProxy::pathPlanElementReceived(UAVObject *obj, bool success)
+{
+    obj->disconnect(this);
+
+    completionCountdown--;
+    successCountdown -= success ? 1 : 0;
+
+    if (completionCountdown == 0) {
+        qDebug() << "ModelUavoProxy::pathPlanReceived - completed" << (successCountdown == 0);
+        if (successCountdown == 0) {
+            if (objectsToModel()) {
+                QMessageBox::information(NULL, tr("Path Plan Download Successful"), tr("Path plan download was successful."));
+            }
+        } else {
+            QMessageBox::critical(NULL, tr("Path Plan Download Failed"), tr("Failed to download the path plan !"));
         }
-        wpfields = wp->getData();
-        myModel->insertRow(x);
-        index    = myModel->index(x, flightDataModel::VELOCITY);
-        myModel->setData(index, wpfields.Velocity);
-        distance = sqrt(wpfields.Position[Waypoint::POSITION_NORTH] * wpfields.Position[Waypoint::POSITION_NORTH] +
-                        wpfields.Position[Waypoint::POSITION_EAST] * wpfields.Position[Waypoint::POSITION_EAST]);
-        bearing  = atan2(wpfields.Position[Waypoint::POSITION_EAST], wpfields.Position[Waypoint::POSITION_NORTH]) * 180 / M_PI;
-
-        if (bearing != bearing) {
-            bearing = 0;
-        }
-        index  = myModel->index(x, flightDataModel::DISRELATIVE);
-        myModel->setData(index, distance);
-        index  = myModel->index(x, flightDataModel::BEARELATIVE);
-        myModel->setData(index, bearing);
-        index  = myModel->index(x, flightDataModel::ALTITUDERELATIVE);
-        myModel->setData(index, (-1.0f) * wpfields.Position[Waypoint::POSITION_DOWN]);
-
-        action = PathAction::GetInstance(objManager, wpfields.Action);
-        Q_ASSERT(action);
-        if (!action) {
-            continue;
-        }
-        actionfields = action->getData();
-
-        index = myModel->index(x, flightDataModel::ISRELATIVE);
-        myModel->setData(index, true);
-
-        index = myModel->index(x, flightDataModel::COMMAND);
-        myModel->setData(index, actionfields.Command);
-
-        index = myModel->index(x, flightDataModel::CONDITION_PARAMS0);
-        myModel->setData(index, actionfields.ConditionParameters[0]);
-        index = myModel->index(x, flightDataModel::CONDITION_PARAMS1);
-        myModel->setData(index, actionfields.ConditionParameters[1]);
-        index = myModel->index(x, flightDataModel::CONDITION_PARAMS2);
-        myModel->setData(index, actionfields.ConditionParameters[2]);
-        index = myModel->index(x, flightDataModel::CONDITION_PARAMS3);
-        myModel->setData(index, actionfields.ConditionParameters[3]);
-
-        index = myModel->index(x, flightDataModel::CONDITION);
-        myModel->setData(index, actionfields.EndCondition);
-
-        index = myModel->index(x, flightDataModel::ERRORDESTINATION);
-        myModel->setData(index, actionfields.ErrorDestination + 1);
-
-        index = myModel->index(x, flightDataModel::JUMPDESTINATION);
-        myModel->setData(index, actionfields.JumpDestination + 1);
-
-        index = myModel->index(x, flightDataModel::MODE);
-        myModel->setData(index, actionfields.Mode);
-
-        index = myModel->index(x, flightDataModel::MODE_PARAMS0);
-        myModel->setData(index, actionfields.ModeParameters[0]);
-        index = myModel->index(x, flightDataModel::MODE_PARAMS1);
-        myModel->setData(index, actionfields.ModeParameters[1]);
-        index = myModel->index(x, flightDataModel::MODE_PARAMS2);
-        myModel->setData(index, actionfields.ModeParameters[2]);
-        index = myModel->index(x, flightDataModel::MODE_PARAMS3);
-        myModel->setData(index, actionfields.ModeParameters[3]);
     }
 }
-int modelUavoProxy::addAction(PathAction *actionObj, PathAction::DataFields actionFields, int lastaction)
-{
-    // check if a similar action already exhists
-    int instances = objManager->getNumInstances(pathactionObj->getObjID());
 
-    for (int x = 0; x < lastaction + 1; ++x) {
+// update waypoint and path actions UAV objects
-        PathAction *action = PathAction::GetInstance(objManager, x);
+//
+// waypoints are unique and each waypoint has an entry in the UAV waypoint list
+//
+// a path action can be referenced by multiple waypoints
+// waypoints reference path action by their index in the UAV path action list
+// the compression of path actions happens here.
+// (compression consists in keeping only one instance of similar path actions)
+//
+// the UAV waypoint list and path action list are probably not empty, so we try to reuse existing instances
+bool ModelUavoProxy::modelToObjects()
+{
+    qDebug() << "ModelUAVProxy::modelToObjects";
+
+    // track number of path actions
+    int actionCount   = 0;
+
+    // iterate over waypoints
+    int waypointCount = myModel->rowCount();
+    for (int i = 0; i < waypointCount; ++i) {
+        // Path Actions
+
+        // create action to use as a search criteria
+        // this object does not need to be deleted as it will either be added to the managed list or deleted later
+        PathAction *action = new PathAction;
+
+        // get model data
+        PathAction::DataFields actionData = action->getData();
+        modelToPathAction(i, actionData);
+
+        // see if that path action has already been added in this run
+        PathAction *foundAction = findPathAction(actionData, actionCount);
+        // TODO this test needs a consistency check as it is unsafe.
+        // if the find method is buggy and returns false positives then the flight plan sent to the uav is broken!
+        // the find method should do a "binary" compare instead of a field by field compare
+        // if a field is added everywhere and not in the compare method then you can start having false positives
+        if (!foundAction) {
+            // create or reuse an action instance
+            action = createPathAction(actionCount, action);
+            actionCount++;
+
+            // update UAVObject
+            action->setData(actionData);
+        } else {
+            action->deleteLater();
+            action = foundAction;
+            qDebug() << "ModelUAVProxy::modelToObjects - found action instance :" << action->getInstID();
+        }
+
+        // Waypoints
+
+        // create or reuse a waypoint instance
+        Waypoint *waypoint = createWaypoint(i, NULL);
+        Q_ASSERT(waypoint);
+
+        // get model data
+        Waypoint::DataFields waypointData = waypoint->getData();
+        modelToWaypoint(i, waypointData);
+
+        // connect waypoint to path action
+        waypointData.Action = action->getInstID();
+
+        // update UAVObject
+        waypoint->setData(waypointData);
+    }
+
+    // Put "safe" values in unused waypoint and path action objects
+    if (waypointCount < objMngr->getNumInstances(Waypoint::OBJID)) {
+        for (int i = waypointCount; i < objMngr->getNumInstances(Waypoint::OBJID); ++i) {
+            // TODO
+        }
+    }
+    if (actionCount < objMngr->getNumInstances(PathAction::OBJID)) {
+        for (int i = actionCount; i < objMngr->getNumInstances(PathAction::OBJID); ++i) {
+            // TODO
+        }
+    }
+
+    // Update PathPlan
+    PathPlan *pathPlan = PathPlan::GetInstance(objMngr);
+    PathPlan::DataFields pathPlanData = pathPlan->getData();
+
+    pathPlanData.WaypointCount   = waypointCount;
+    pathPlanData.PathActionCount = actionCount;
+    pathPlanData.Crc = computePathPlanCrc(waypointCount, actionCount);
+
+    pathPlan->setData(pathPlanData);
+
+    return true;
+}
+
+Waypoint *ModelUavoProxy::createWaypoint(int index, Waypoint *newWaypoint)
+{
+    Waypoint *waypoint = NULL;
+    int count = objMngr->getNumInstances(Waypoint::OBJID);
+
+    if (index < count) {
+        // reuse object
+        qDebug() << "ModelUAVProxy::createWaypoint - reused waypoint instance :" << index << "/" << count;
+        waypoint = Waypoint::GetInstance(objMngr, index);
+        if (newWaypoint) {
+            newWaypoint->deleteLater();
+        }
+    } else if (index < count + 1) {
+        // create "next" object
+        qDebug() << "ModelUAVProxy::createWaypoint - created waypoint instance :" << index;
+        // TODO is there a limit to the number of wp?
+        waypoint = newWaypoint ? newWaypoint : new Waypoint;
+        waypoint->initialize(index, waypoint->getMetaObject());
+        objMngr->registerObject(waypoint);
+    } else {
+        // we can only create the "next" object
+        // TODO fail in a clean way :(
+    }
+    return waypoint;
+}
+
+PathAction *ModelUavoProxy::createPathAction(int index, PathAction *newAction)
+{
+    PathAction *action = NULL;
+    int count = objMngr->getNumInstances(PathAction::OBJID);
+
+    if (index < count) {
+        // reuse object
+        qDebug() << "ModelUAVProxy::createPathAction - reused action instance :" << index << "/" << count;
+        action = PathAction::GetInstance(objMngr, index);
+        if (newAction) {
+            newAction->deleteLater();
+        }
+    } else if (index < count + 1) {
+        // create "next" object
+        qDebug() << "ModelUAVProxy::createPathAction - created action instance :" << index;
+        // TODO is there a limit to the number of path actions?
+        action = newAction ? newAction : new PathAction;
+        action->initialize(index, action->getMetaObject());
+        objMngr->registerObject(action);
+    } else {
+        // we can only create the "next" object
+        // TODO fail in a clean way :(
+    }
+    return action;
+}
+
+PathAction *ModelUavoProxy::findPathAction(const PathAction::DataFields &actionData, int actionCount)
+{
+    int instancesCount = objMngr->getNumInstances(PathAction::OBJID);
+    int count = actionCount <= instancesCount ? actionCount : instancesCount;
+
+    for (int i = 0; i < count; ++i) {
+        PathAction *action = PathAction::GetInstance(objMngr, i);
         Q_ASSERT(action);
         if (!action) {
             continue;
         }
         PathAction::DataFields fields = action->getData();
-        if (fields.Command == actionFields.Command
+        if (fields.Command == actionData.Command
-            && fields.ConditionParameters[0] == actionFields.ConditionParameters[0]
+            && fields.ConditionParameters[0] == actionData.ConditionParameters[0]
-            && fields.ConditionParameters[1] == actionFields.ConditionParameters[1]
+            && fields.ConditionParameters[1] == actionData.ConditionParameters[1]
-            && fields.ConditionParameters[2] == actionFields.ConditionParameters[2]
+            && fields.ConditionParameters[2] == actionData.ConditionParameters[2]
-            && fields.EndCondition == actionFields.EndCondition
+            && fields.EndCondition == actionData.EndCondition
-            && fields.ErrorDestination == actionFields.ErrorDestination
+            && fields.ErrorDestination == actionData.ErrorDestination
-            && fields.JumpDestination == actionFields.JumpDestination
+            && fields.JumpDestination == actionData.JumpDestination && fields.Mode == actionData.Mode
-            && fields.Mode == actionFields.Mode
+            && fields.ModeParameters[0] == actionData.ModeParameters[0]
-            && fields.ModeParameters[0] == actionFields.ModeParameters[0]
+            && fields.ModeParameters[1] == actionData.ModeParameters[1]
-            && fields.ModeParameters[1] == actionFields.ModeParameters[1]
+            && fields.ModeParameters[2] == actionData.ModeParameters[2]) {
-            && fields.ModeParameters[2] == actionFields.ModeParameters[2]) {
+            return action;
-            qDebug() << "ModelUAVProxy:" << "found similar action instance:" << x;
-            actionObj->deleteLater();
-            return x;
         }
     }
-    // if we get here it means no similar action was found, we have to create it
+    return NULL;
-    if (instances < lastaction + 2) {
+}
-        actionObj->initialize(instances, actionObj->getMetaObject());
+
-        objManager->registerObject(actionObj);
+bool ModelUavoProxy::objectsToModel()
-        actionObj->setData(actionFields);
+{
-        actionObj->updated();
+    // build model from uav objects
-        qDebug() << "ModelUAVProxy:" << "created new action instance:" << instances;
+    // the list of objects can end with "garbage" instances due to previous flightpath
-        return lastaction + 1;
+    // they need to be ignored
-    } else {
+
-        PathAction *action = PathAction::GetInstance(objManager, lastaction + 1);
+    PathPlan *pathPlan = PathPlan::GetInstance(objMngr);
-        Q_ASSERT(action);
+    PathPlan::DataFields pathPlanData = pathPlan->getData();
-        action->setData(actionFields);
+
-        action->updated();
+    int waypointCount  = pathPlanData.WaypointCount;
-        actionObj->deleteLater();
+    int actionCount    = pathPlanData.PathActionCount;
-        qDebug() << "ModelUAVProxy:" << "reused action instance:" << lastaction + 1;
+
-        return lastaction + 1;
+    // consistency check
-    }
+    if (waypointCount > objMngr->getNumInstances(Waypoint::OBJID)) {
-    return -1; // error we should never get here
+        QMessageBox::critical(NULL, tr("Path Plan Download Failed"), tr("Path plan way point count error !"));
+        return false;
+    }
+    if (actionCount > objMngr->getNumInstances(PathAction::OBJID)) {
+        QMessageBox::critical(NULL, tr("Path Plan Download Failed"), tr("Path plan path action count error !"));
+        return false;
+    }
+    if (pathPlanData.Crc != computePathPlanCrc(waypointCount, actionCount)) {
+        QMessageBox::critical(NULL, tr("Path Plan Upload Failed"), tr("Path plan CRC error !"));
+        return false;
+    }
+
+    int rowCount = myModel->rowCount();
+    if (waypointCount < rowCount) {
+        myModel->removeRows(waypointCount, rowCount - waypointCount);
+    } else if (waypointCount > rowCount) {
+        myModel->insertRows(rowCount, waypointCount - rowCount);
+    }
+
+    for (int i = 0; i < waypointCount; ++i) {
+        Waypoint *waypoint = Waypoint::GetInstance(objMngr, i);
+        Q_ASSERT(waypoint);
+        if (!waypoint) {
+            continue;
+        }
+
+        Waypoint::DataFields waypointData = waypoint->getData();
+        waypointToModel(i, waypointData);
+
+        PathAction *action = PathAction::GetInstance(objMngr, waypoint->getAction());
+        Q_ASSERT(action);
+        if (!action) {
+            continue;
+        }
+
+        PathAction::DataFields actionData = action->getData();
+        pathActionToModel(i, actionData);
+    }
+    return true;
+}
+
+void ModelUavoProxy::modelToWaypoint(int i, Waypoint::DataFields &data)
+{
+    double distance, bearing, altitude, velocity;
+
+    QModelIndex index = myModel->index(i, flightDataModel::DISRELATIVE);
+
+    distance = myModel->data(index).toDouble();
+    index    = myModel->index(i, flightDataModel::BEARELATIVE);
+    bearing  = myModel->data(index).toDouble();
+    index    = myModel->index(i, flightDataModel::ALTITUDERELATIVE);
+    altitude = myModel->data(index).toFloat();
+    index    = myModel->index(i, flightDataModel::VELOCITY);
+    velocity = myModel->data(index).toFloat();
+
+    data.Position[Waypoint::POSITION_NORTH] = distance * cos(bearing / 180 * M_PI);
+    data.Position[Waypoint::POSITION_EAST]  = distance * sin(bearing / 180 * M_PI);
+    data.Position[Waypoint::POSITION_DOWN]  = -altitude;
+    data.Velocity = velocity;
+}
+
+void ModelUavoProxy::waypointToModel(int i, Waypoint::DataFields &data)
+{
+    double distance = sqrt(data.Position[Waypoint::POSITION_NORTH] * data.Position[Waypoint::POSITION_NORTH] +
+                           data.Position[Waypoint::POSITION_EAST] * data.Position[Waypoint::POSITION_EAST]);
+
+    double bearing  = atan2(data.Position[Waypoint::POSITION_EAST], data.Position[Waypoint::POSITION_NORTH]) * 180 / M_PI;
+
+    if (bearing != bearing) {
+        bearing = 0;
+    }
+
+    double altitude   = -data.Position[Waypoint::POSITION_DOWN];
+
+    QModelIndex index = myModel->index(i, flightDataModel::VELOCITY);
+    myModel->setData(index, data.Velocity);
+    index = myModel->index(i, flightDataModel::DISRELATIVE);
+    myModel->setData(index, distance);
+    index = myModel->index(i, flightDataModel::BEARELATIVE);
+    myModel->setData(index, bearing);
+    index = myModel->index(i, flightDataModel::ALTITUDERELATIVE);
+    myModel->setData(index, altitude);
+}
+
+void ModelUavoProxy::modelToPathAction(int i, PathAction::DataFields &data)
+{
+    QModelIndex index = myModel->index(i, flightDataModel::MODE);
+
+    data.Mode    = myModel->data(index).toInt();
+    index        = myModel->index(i, flightDataModel::MODE_PARAMS0);
+    data.ModeParameters[0] = myModel->data(index).toFloat();
+    index        = myModel->index(i, flightDataModel::MODE_PARAMS1);
+    data.ModeParameters[1] = myModel->data(index).toFloat();
+    index        = myModel->index(i, flightDataModel::MODE_PARAMS2);
+    data.ModeParameters[2] = myModel->data(index).toFloat();
+    index        = myModel->index(i, flightDataModel::MODE_PARAMS3);
+    data.ModeParameters[3] = myModel->data(index).toFloat();
+    index        = myModel->index(i, flightDataModel::CONDITION);
+    data.EndCondition = myModel->data(index).toInt();
+    index        = myModel->index(i, flightDataModel::CONDITION_PARAMS0);
+    data.ConditionParameters[0] = myModel->data(index).toFloat();
+    index        = myModel->index(i, flightDataModel::CONDITION_PARAMS1);
+    data.ConditionParameters[1] = myModel->data(index).toFloat();
+    index        = myModel->index(i, flightDataModel::CONDITION_PARAMS2);
+    data.ConditionParameters[2] = myModel->data(index).toFloat();
+    index        = myModel->index(i, flightDataModel::CONDITION_PARAMS3);
+    data.ConditionParameters[3] = myModel->data(index).toFloat();
+    index        = myModel->index(i, flightDataModel::COMMAND);
+    data.Command = myModel->data(index).toInt();
+    index        = myModel->index(i, flightDataModel::JUMPDESTINATION);
+    data.JumpDestination = myModel->data(index).toInt() - 1;
+    index        = myModel->index(i, flightDataModel::ERRORDESTINATION);
+    data.ErrorDestination = myModel->data(index).toInt() - 1;
+}
+
+void ModelUavoProxy::pathActionToModel(int i, PathAction::DataFields &data)
+{
+    QModelIndex index = myModel->index(i, flightDataModel::ISRELATIVE);
+
+    myModel->setData(index, true);
+
+    index = myModel->index(i, flightDataModel::COMMAND);
+    myModel->setData(index, data.Command);
+
+    index = myModel->index(i, flightDataModel::CONDITION_PARAMS0);
+    myModel->setData(index, data.ConditionParameters[0]);
+    index = myModel->index(i, flightDataModel::CONDITION_PARAMS1);
+    myModel->setData(index, data.ConditionParameters[1]);
+    index = myModel->index(i, flightDataModel::CONDITION_PARAMS2);
+    myModel->setData(index, data.ConditionParameters[2]);
+    index = myModel->index(i, flightDataModel::CONDITION_PARAMS3);
+    myModel->setData(index, data.ConditionParameters[3]);
+
+    index = myModel->index(i, flightDataModel::CONDITION);
+    myModel->setData(index, data.EndCondition);
+
+    index = myModel->index(i, flightDataModel::ERRORDESTINATION);
+    myModel->setData(index, data.ErrorDestination + 1);
+
+    index = myModel->index(i, flightDataModel::JUMPDESTINATION);
+    myModel->setData(index, data.JumpDestination + 1);
+
+    index = myModel->index(i, flightDataModel::MODE);
+    myModel->setData(index, data.Mode);
+
+    index = myModel->index(i, flightDataModel::MODE_PARAMS0);
+    myModel->setData(index, data.ModeParameters[0]);
+    index = myModel->index(i, flightDataModel::MODE_PARAMS1);
+    myModel->setData(index, data.ModeParameters[1]);
+    index = myModel->index(i, flightDataModel::MODE_PARAMS2);
+    myModel->setData(index, data.ModeParameters[2]);
+    index = myModel->index(i, flightDataModel::MODE_PARAMS3);
+    myModel->setData(index, data.ModeParameters[3]);
+}
+
+quint8 ModelUavoProxy::computePathPlanCrc(int waypointCount, int actionCount)
+{
+    quint8 crc = 0;
+
+    for (int i = 0; i < waypointCount; ++i) {
+        Waypoint *waypoint = Waypoint::GetInstance(objMngr, i);
+        crc = waypoint->updateCRC(crc);
+    }
+    for (int i = 0; i < actionCount; ++i) {
+        PathAction *action = PathAction::GetInstance(objMngr, i);
+        crc = action->updateCRC(crc);
+    }
+    return crc;
 }

ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.h

@@ -27,24 +27,50 @@
 #ifndef MODELUAVOPROXY_H
 #define MODELUAVOPROXY_H
 
-#include <QObject>
 #include "flightdatamodel.h"
+
+#include "pathplan.h"
 #include "pathaction.h"
 #include "waypoint.h"
 
-class modelUavoProxy : public QObject {
+#include <QObject>
+
+class ModelUavoProxy : public QObject {
     Q_OBJECT
+
 public:
-    explicit modelUavoProxy(QObject *parent, flightDataModel *model);
+    explicit ModelUavoProxy(QObject *parent, flightDataModel *model);
-    int addAction(PathAction *actionObj, PathAction::DataFields actionFields, int lastaction);
+
 public slots:
-    void modelToObjects();
+    void sendPathPlan();
-    void objectsToModel();
+    void receivePathPlan();
+
 private:
-    UAVObjectManager *objManager;
+    UAVObjectManager *objMngr;
-    Waypoint *waypointObj;
-    PathAction *pathactionObj;
     flightDataModel *myModel;
+
+    uint completionCountdown;
+    uint successCountdown;
+
+    bool modelToObjects();
+    bool objectsToModel();
+
+    Waypoint *createWaypoint(int index, Waypoint *newWaypoint);
+    PathAction *createPathAction(int index, PathAction *newAction);
+
+    PathAction *findPathAction(const PathAction::DataFields & actionFields, int actionCount);
+
+    void modelToWaypoint(int i, Waypoint::DataFields &data);
+    void modelToPathAction(int i, PathAction::DataFields &data);
+
+    void waypointToModel(int i, Waypoint::DataFields &data);
+    void pathActionToModel(int i, PathAction::DataFields &data);
+
+    quint8 computePathPlanCrc(int waypointCount, int actionCount);
+
+private slots:
+    void pathPlanElementSent(UAVObject *, bool success);
+    void pathPlanElementReceived(UAVObject *, bool success);
 };
 
 #endif // MODELUAVOPROXY_H

ground/openpilotgcs/src/plugins/opmap/opmap_edit_waypoint_dialog.cpp

@@ -176,7 +176,7 @@ void opmap_edit_waypoint_dialog::setupConditionWidgets()
         ui->dsb_condParam2->setVisible(false);
         ui->dsb_condParam3->setVisible(false);
         ui->dsb_condParam4->setVisible(false);
-        ui->condParam1->setText("Timeout(ms)");
+        ui->condParam1->setText("Timeout(s)");
         break;
     case MapDataDelegate::ENDCONDITION_DISTANCETOTARGET:
         ui->condParam1->setVisible(true);

ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp

@@ -225,9 +225,11 @@ OPMapGadgetWidget::OPMapGadgetWidget(QWidget *parent) : QWidget(parent)
     mapProxy = new modelMapProxy(this, m_map, model, selectionModel);
     table->setModel(model, selectionModel);
     waypoint_edit_dialog = new opmap_edit_waypoint_dialog(this, model, selectionModel);
-    UAVProxy = new modelUavoProxy(this, model);
+    UAVProxy = new ModelUavoProxy(this, model);
-    connect(table, SIGNAL(sendPathPlanToUAV()), UAVProxy, SLOT(modelToObjects()));
+    // sending and receiving is asynchronous
-    connect(table, SIGNAL(receivePathPlanFromUAV()), UAVProxy, SLOT(objectsToModel()));
+    // TODO : buttons should be disabled while a send or receive is in progress
+    connect(table, SIGNAL(sendPathPlanToUAV()), UAVProxy, SLOT(sendPathPlan()));
+    connect(table, SIGNAL(receivePathPlanFromUAV()), UAVProxy, SLOT(receivePathPlan()));
 #endif
     magicWayPoint = m_map->magicWPCreate();
     magicWayPoint->setVisible(false);
@@ -500,7 +502,8 @@ void OPMapGadgetWidget::contextMenuEvent(QContextMenuEvent *event)
         contextMenu.addAction(wayPointEditorAct);
         contextMenu.addAction(addWayPointActFromContextMenu);
 
-        if (m_mouse_waypoint) { // we have a waypoint under the mouse
+        if (m_mouse_waypoint) {
+            // we have a waypoint under the mouse
             contextMenu.addAction(editWayPointAct);
 
             lockWayPointAct->setChecked(waypoint_locked);
@@ -1868,8 +1871,12 @@ void OPMapGadgetWidget::onUAVTrailDistanceActGroup_triggered(QAction *action)
 
 void OPMapGadgetWidget::onOpenWayPointEditorAct_triggered()
 {
+    // open dialog
     table->show();
+    // bring dialog to the front in case it was already open and hidden away
+    table->raise();
 }
+
 void OPMapGadgetWidget::onAddWayPointAct_triggeredFromContextMenu()
 {
     onAddWayPointAct_triggered(m_context_menu_lat_lon);

ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.h

@@ -233,7 +233,7 @@ private:
     QPointer<opmap_edit_waypoint_dialog> waypoint_edit_dialog;
     QStandardItemModel wayPoint_treeView_model;
     mapcontrol::WayPointItem *m_mouse_waypoint;
-    QPointer<modelUavoProxy> UAVProxy;
+    QPointer<ModelUavoProxy> UAVProxy;
     QMutex m_map_mutex;
     bool m_telemetry_connected;
     QAction *closeAct1;

ground/openpilotgcs/src/plugins/opmap/widgetdelegates.cpp

@@ -76,7 +76,6 @@ void MapDataDelegate::setEditorData(QWidget *editor,
         int value = index.model()->data(index, Qt::EditRole).toInt();
         QComboBox *comboBox = static_cast<QComboBox *>(editor);
         int x     = comboBox->findData(value);
-        qDebug() << "VALUE=" << x;
         comboBox->setCurrentIndex(x);
     } else {
         QItemDelegate::setEditorData(editor, index);

ground/openpilotgcs/src/plugins/scope/scopegadgetwidget.cpp

@@ -30,7 +30,6 @@
 #include "scopegadgetwidget.h"
 #include "utils/stylehelper.h"
 
-#include "uavtalk/telemetrymanager.h"
 #include "extensionsystem/pluginmanager.h"
 #include "uavobjectmanager.h"
 #include "uavobject.h"

ground/openpilotgcs/src/plugins/setupwizard/pages/autoupdatepage.h

@@ -31,7 +31,6 @@
 #include <coreplugin/icore.h>
 #include <coreplugin/connectionmanager.h>
 #include "setupwizard.h"
-#include "uavtalk/telemetrymanager.h"
 #include "abstractwizardpage.h"
 #include "uploader/enums.h"
 

ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetwidget.cpp

@@ -25,11 +25,13 @@
  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  */
 
+#include "systemalarms.h"
 #include "systemhealthgadgetwidget.h"
+
 #include "utils/stylehelper.h"
 #include "extensionsystem/pluginmanager.h"
 #include "uavobjectmanager.h"
-#include "systemalarms.h"
+#include <uavtalk/telemetrymanager.h>
 
 #include <QDebug>
 #include <QWhatsThis>

ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetwidget.h

@@ -30,7 +30,7 @@
 
 #include "systemhealthgadgetconfiguration.h"
 #include "uavobject.h"
-#include "uavtalk/telemetrymanager.h"
+
 #include <QGraphicsView>
 #include <QtSvg/QSvgRenderer>
 #include <QtSvg/QGraphicsSvgItem>

ground/openpilotgcs/src/plugins/telemetry/monitorgadgetfactory.cpp

@@ -25,14 +25,15 @@
  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  */
 #include "monitorgadgetfactory.h"
-#include "uavtalk/telemetrymanager.h"
+
-#include "extensionsystem/pluginmanager.h"
 #include "monitorgadgetconfiguration.h"
 #include "monitorgadget.h"
 #include "monitorgadgetoptionspage.h"
 
+#include <extensionsystem/pluginmanager.h>
 #include <coreplugin/connectionmanager.h>
 #include <coreplugin/icore.h>
+#include <uavtalk/telemetrymanager.h>
 
 MonitorGadgetFactory::MonitorGadgetFactory(QObject *parent) :
     IUAVGadgetFactory(QString("TelemetryMonitorGadget"), tr("Telemetry Monitor"), parent)

ground/openpilotgcs/src/plugins/uavobjects/uavobject.cpp

@@ -26,9 +26,14 @@
  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  */
 #include "uavobject.h"
+
+#include <utils/crc.h>
+
 #include <QtEndian>
 #include <QDebug>
 
+using namespace Utils;
+
 // Constants
 #define UAVOBJ_ACCESS_SHIFT                    0
 #define UAVOBJ_GCS_ACCESS_SHIFT                1
@@ -50,11 +55,13 @@
  */
 UAVObject::UAVObject(quint32 objID, bool isSingleInst, const QString & name)
 {
-    this->objID  = objID;
+    this->objID        = objID;
-    this->instID = 0;
+    this->instID       = 0;
     this->isSingleInst = isSingleInst;
-    this->name   = name;
+    this->name         = name;
-    this->mutex  = new QMutex(QMutex::Recursive);
+    this->data         = 0;
+    this->numBytes     = 0;
+    this->mutex        = new QMutex(QMutex::Recursive);
 }
 
 /**
@@ -163,7 +170,6 @@ void UAVObject::setCategory(const QString & category)
     this->category = category;
 }
 
-
 /**
  * Get the total number of bytes of the object's data
  */
@@ -180,6 +186,17 @@ void UAVObject::requestUpdate()
     emit updateRequested(this);
 }
 
+/**
+ * Request that all instances of this object are updated with the latest values from the autopilot
+ * Must be called on instance zero
+ */
+void UAVObject::requestUpdateAll()
+{
+    if (instID == 0) {
+        emit updateRequested(this, true);
+    }
+}
+
 /**
  * Signal that the object has been updated
  */
@@ -189,6 +206,19 @@ void UAVObject::updated()
     emit objectUpdated(this);
 }
 
+/**
+ * Signal that all instance of the object have been updated
+ * Must be called on instance zero
+ */
+void UAVObject::updatedAll()
+{
+    if (instID == 0) {
+        emit objectUpdatedManual(this, true);
+        // TODO call objectUpdated() for all instances?
+        // emit objectUpdated(this);
+    }
+}
+
 /**
  * Lock mutex of this object
  */
@@ -256,7 +286,8 @@ UAVObjectField *UAVObject::getField(const QString & name)
         }
     }
     // If this point is reached then the field was not found
-    qWarning() << "UAVObject::getField Non existant field " << name << " requested.  This indicates a bug.  Make sure you also have null checking for non-debug code.";
+    qWarning() << "UAVObject::getField Non existant field" << name << "requested."
+               << "This indicates a bug. Make sure you also have null checking for non-debug code.";
     return NULL;
 }
 
@@ -295,6 +326,21 @@ qint32 UAVObject::unpack(const quint8 *dataIn)
     return numBytes;
 }
 
+/**
+ * Update a CRC with the object data
+ * @returns The updated CRC
+ */
+quint8 UAVObject::updateCRC(quint8 crc)
+{
+    QMutexLocker locker(mutex);
+
+    // crc = Crc::updateCRC(crc, (quint8 *) &objID, sizeof(objID));
+    // crc = Crc::updateCRC(crc, (quint8 *) &instID, sizeof(instID));
+    crc = Crc::updateCRC(crc, data, numBytes);
+
+    return crc;
+}
+
 /**
  * Save the object data to the file.
  * The file will be created in the current directory
@@ -437,6 +483,7 @@ QString UAVObject::toString()
     QString sout;
 
     sout.append(toStringBrief());
+    sout.append('\n');
     sout.append(toStringData());
     return sout;
 }
@@ -448,12 +495,13 @@ QString UAVObject::toStringBrief()
 {
     QString sout;
 
-    sout.append(QString("%1 (ID: %2, InstID: %3, NumBytes: %4, SInst: %5)\n")
+    // object Id is converted to uppercase hexadecimal
+    sout.append(QString("%1 (ID: %2-%3, %4 bytes, %5)")
                 .arg(getName())
-                .arg(getObjID())
+                .arg(getObjID(), 1, 16).toUpper()
                 .arg(getInstID())
                 .arg(getNumBytes())
-                .arg(isSingleInstance()));
+                .arg(isSingleInstance() ? "single" : "multiple"));
     return sout;
 }
 

ground/openpilotgcs/src/plugins/uavobjects/uavobject.h

@@ -109,6 +109,7 @@ public:
     quint32 getNumBytes();
     qint32 pack(quint8 *dataOut);
     qint32 unpack(const quint8 *dataIn);
+    quint8 updateCRC(quint8 crc = 0);
     bool save();
     bool save(QFile & file);
     bool load();
@@ -146,15 +147,17 @@ public:
 
 public slots:
     void requestUpdate();
+    void requestUpdateAll();
     void updated();
+    void updatedAll();
 
 signals:
     void objectUpdated(UAVObject *obj);
     void objectUpdatedAuto(UAVObject *obj);
-    void objectUpdatedManual(UAVObject *obj);
+    void objectUpdatedManual(UAVObject *obj, bool all = false);
     void objectUpdatedPeriodic(UAVObject *obj);
     void objectUnpacked(UAVObject *obj);
-    void updateRequested(UAVObject *obj);
+    void updateRequested(UAVObject *obj, bool all = false);
     void transactionCompleted(UAVObject *obj, bool success);
     void newInstance(UAVObject *obj);
 

ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro

@@ -1,10 +1,13 @@
 TEMPLATE = lib
 TARGET = UAVObjects
+
 DEFINES += UAVOBJECTS_LIBRARY
+
 include(../../openpilotgcsplugin.pri)
 include(uavobjects_dependencies.pri)
 
-HEADERS += uavobjects_global.h \
+HEADERS += \
+    uavobjects_global.h \
     uavobject.h \
     uavmetaobject.h \
     uavobjectmanager.h \
@@ -14,7 +17,8 @@ HEADERS += uavobjects_global.h \
     uavobjectsplugin.h \
     uavobjecthelper.h
 
-SOURCES += uavobject.cpp \
+SOURCES += \
+    uavobject.cpp \
     uavmetaobject.cpp \
     uavobjectmanager.cpp \
     uavdataobject.cpp \
@@ -25,16 +29,17 @@ SOURCES += uavobject.cpp \
 OTHER_FILES += UAVObjects.pluginspec
 
 # Add in all of the synthetic/generated uavobject files
-HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \
+HEADERS += \
+    $$UAVOBJECT_SYNTHETICS/accessorydesired.h \
     $$UAVOBJECT_SYNTHETICS/barosensor.h \
     $$UAVOBJECT_SYNTHETICS/airspeedsensor.h \
     $$UAVOBJECT_SYNTHETICS/airspeedsettings.h \
     $$UAVOBJECT_SYNTHETICS/airspeedstate.h \
     $$UAVOBJECT_SYNTHETICS/attitudestate.h \
     $$UAVOBJECT_SYNTHETICS/attitudesimulated.h \
-    $$UAVOBJECT_SYNTHETICS/altholdsmoothed.h \
     $$UAVOBJECT_SYNTHETICS/altitudeholddesired.h \
     $$UAVOBJECT_SYNTHETICS/altitudeholdsettings.h \
+    $$UAVOBJECT_SYNTHETICS/altitudeholdstatus.h \
     $$UAVOBJECT_SYNTHETICS/altitudefiltersettings.h \
     $$UAVOBJECT_SYNTHETICS/debuglogsettings.h \
     $$UAVOBJECT_SYNTHETICS/debuglogcontrol.h \
@@ -76,6 +81,7 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \
     $$UAVOBJECT_SYNTHETICS/gpssettings.h \
     $$UAVOBJECT_SYNTHETICS/pathaction.h \
     $$UAVOBJECT_SYNTHETICS/pathdesired.h \
+    $$UAVOBJECT_SYNTHETICS/pathplan.h \
     $$UAVOBJECT_SYNTHETICS/pathstatus.h \
     $$UAVOBJECT_SYNTHETICS/gpsvelocitysensor.h \
     $$UAVOBJECT_SYNTHETICS/positionstate.h \
@@ -115,21 +121,23 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \
     $$UAVOBJECT_SYNTHETICS/oplinksettings.h \
     $$UAVOBJECT_SYNTHETICS/oplinkstatus.h \
     $$UAVOBJECT_SYNTHETICS/oplinkreceiver.h \
+    $$UAVOBJECT_SYNTHETICS/radiocombridgestats.h \
     $$UAVOBJECT_SYNTHETICS/osdsettings.h \
     $$UAVOBJECT_SYNTHETICS/waypoint.h \
     $$UAVOBJECT_SYNTHETICS/waypointactive.h \
     $$UAVOBJECT_SYNTHETICS/mpu6000settings.h
 
-SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \
+SOURCES += \
+    $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \
     $$UAVOBJECT_SYNTHETICS/barosensor.cpp \
     $$UAVOBJECT_SYNTHETICS/airspeedsensor.cpp \
     $$UAVOBJECT_SYNTHETICS/airspeedsettings.cpp \
     $$UAVOBJECT_SYNTHETICS/airspeedstate.cpp \
     $$UAVOBJECT_SYNTHETICS/attitudestate.cpp \
     $$UAVOBJECT_SYNTHETICS/attitudesimulated.cpp \
-    $$UAVOBJECT_SYNTHETICS/altholdsmoothed.cpp \
     $$UAVOBJECT_SYNTHETICS/altitudeholddesired.cpp \
     $$UAVOBJECT_SYNTHETICS/altitudeholdsettings.cpp \
+    $$UAVOBJECT_SYNTHETICS/altitudeholdstatus.cpp \
     $$UAVOBJECT_SYNTHETICS/debuglogsettings.cpp \
     $$UAVOBJECT_SYNTHETICS/debuglogcontrol.cpp \
     $$UAVOBJECT_SYNTHETICS/debuglogstatus.cpp \
@@ -171,6 +179,7 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \
     $$UAVOBJECT_SYNTHETICS/gpssettings.cpp \
     $$UAVOBJECT_SYNTHETICS/pathaction.cpp \
     $$UAVOBJECT_SYNTHETICS/pathdesired.cpp \
+    $$UAVOBJECT_SYNTHETICS/pathplan.cpp \
     $$UAVOBJECT_SYNTHETICS/pathstatus.cpp \
     $$UAVOBJECT_SYNTHETICS/gpsvelocitysensor.cpp \
     $$UAVOBJECT_SYNTHETICS/positionstate.cpp \
@@ -211,6 +220,7 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \
     $$UAVOBJECT_SYNTHETICS/oplinksettings.cpp \
     $$UAVOBJECT_SYNTHETICS/oplinkstatus.cpp \
     $$UAVOBJECT_SYNTHETICS/oplinkreceiver.cpp \
+    $$UAVOBJECT_SYNTHETICS/radiocombridgestats.cpp \
     $$UAVOBJECT_SYNTHETICS/osdsettings.cpp \
     $$UAVOBJECT_SYNTHETICS/waypoint.cpp \
     $$UAVOBJECT_SYNTHETICS/waypointactive.cpp \

ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp

@@ -256,11 +256,10 @@ int UAVObjectUtilManager::getBoardModel()
 {
     FirmwareIAPObj::DataFields firmwareIapData = getFirmwareIap();
 
-    qDebug() << "Board type=" << firmwareIapData.BoardType;
-    qDebug() << "Board revision=" << firmwareIapData.BoardRevision;
     int ret = firmwareIapData.BoardType << 8;
+
     ret = ret + firmwareIapData.BoardRevision;
-    qDebug() << "Board info=" << ret;
+
     return ret;
 }
 

ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.cpp

@@ -25,9 +25,12 @@
  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  */
 #include "configtaskwidget.h"
+
+#include <uavtalk/telemetrymanager.h>
+#include "uavsettingsimportexport/uavsettingsimportexportfactory.h"
+
 #include <QWidget>
 #include <QLineEdit>
-#include "uavsettingsimportexport/uavsettingsimportexportfactory.h"
 
 ConfigTaskWidget::ConfigTaskWidget(QWidget *parent) : QWidget(parent), m_isConnected(false), m_isWidgetUpdatesAllowed(true),
     m_saveButton(NULL), m_isDirty(false), m_outOfLimitsStyle("background-color: rgb(255, 0, 0);"), m_realtimeUpdateTimer(NULL)
@@ -269,16 +272,10 @@ void ConfigTaskWidget::forceConnectedState() // dynamic widgets don't recieve th
 void ConfigTaskWidget::onAutopilotConnect()
 {
     if (m_objectUtilManager) {
-        m_currentBoardId = m_objectUtilManager->getBoardModel(); // TODO REMEMBER TO ADD THIS TO FORCE CONNECTED FUNC ON CC3D_RELEASE
+        m_currentBoardId = m_objectUtilManager->getBoardModel();
     }
     invalidateObjects();
     m_isConnected = true;
-    foreach(WidgetBinding * binding, m_widgetBindingsPerObject) {
-        if (!binding->isEnabled()) {
-            continue;
-        }
-        loadWidgetLimits(binding->widget(), binding->field(), binding->index(), binding->isLimited(), binding->scale());
-    }
     setDirty(false);
     enableControls(true);
     refreshWidgetsValues();
@@ -1046,6 +1043,7 @@ void ConfigTaskWidget::updateEnableControls()
     TelemetryManager *telMngr = m_pluginManager->getObject<TelemetryManager>();
 
     Q_ASSERT(telMngr);
+
     enableControls(telMngr->isConnected());
 }
 

ground/openpilotgcs/src/plugins/uavobjectwidgetutils/smartsavebutton.h

@@ -27,7 +27,6 @@
 #ifndef SMARTSAVEBUTTON_H
 #define SMARTSAVEBUTTON_H
 
-#include "uavtalk/telemetrymanager.h"
 #include "extensionsystem/pluginmanager.h"
 #include "uavobjectmanager.h"
 #include "uavobject.h"

ground/openpilotgcs/src/plugins/uavtalk/telemetry.cpp

@@ -36,28 +36,34 @@
 /**
  * Constructor
  */
-Telemetry::Telemetry(UAVTalk *utalk, UAVObjectManager *objMngr)
+Telemetry::Telemetry(UAVTalk *utalk, UAVObjectManager *objMngr) : objMngr(objMngr), utalk(utalk)
 {
-    this->utalk   = utalk;
-    this->objMngr = objMngr;
     mutex = new QMutex(QMutex::Recursive);
-    // Process all objects in the list
+
+    // Register all objects in the list
     QList< QList<UAVObject *> > objs = objMngr->getObjects();
     for (int objidx = 0; objidx < objs.length(); ++objidx) {
-        registerObject(objs[objidx][0]); // we only need to register one instance per object type
+        // we only need to register one instance per object type
+        registerObject(objs[objidx][0]);
     }
+
     // Listen to new object creations
     connect(objMngr, SIGNAL(newObject(UAVObject *)), this, SLOT(newObject(UAVObject *)));
     connect(objMngr, SIGNAL(newInstance(UAVObject *)), this, SLOT(newInstance(UAVObject *)));
+
     // Listen to transaction completions
+    // TODO should send a status (SUCCESS, FAILED, TIMEOUT)
     connect(utalk, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(transactionCompleted(UAVObject *, bool)));
+
     // Get GCS stats object
     gcsStatsObj = GCSTelemetryStats::GetInstance(objMngr);
+
     // Setup and start the periodic timer
     timeToNextUpdateMs = 0;
     updateTimer = new QTimer(this);
     connect(updateTimer, SIGNAL(timeout()), this, SLOT(processPeriodicUpdates()));
     updateTimer->start(1000);
+
     // Setup and start the stats timer
     txErrors  = 0;
     txRetries = 0;
@@ -65,9 +71,7 @@ Telemetry::Telemetry(UAVTalk *utalk, UAVObjectManager *objMngr)
 
 Telemetry::~Telemetry()
 {
-    for (QMap<quint32, ObjectTransactionInfo *>::iterator itr = transMap.begin(); itr != transMap.end(); ++itr) {
+    closeAllTransactions();
-        delete itr.value();
-    }
 }
 
 /**
@@ -122,26 +126,36 @@ void Telemetry::setUpdatePeriod(UAVObject *obj, qint32 periodMs)
  */
 void Telemetry::connectToObjectInstances(UAVObject *obj, quint32 eventMask)
 {
+    // TODO why connect systematically to all instances?
+    // It is probably not needed to connect to always connect to all instances.
     QList<UAVObject *> objs = objMngr->getObjectInstances(obj->getObjID());
     for (int n = 0; n < objs.length(); ++n) {
-        // Disconnect all
+        connectToObject(objs[n], eventMask);
-        objs[n]->disconnect(this);
+    }
-        // Connect only the selected events
+}
-        if ((eventMask & EV_UNPACKED) != 0) {
+
-            connect(objs[n], SIGNAL(objectUnpacked(UAVObject *)), this, SLOT(objectUnpacked(UAVObject *)));
+/**
-        }
+ * Connect to all instances of an object depending on the event mask specified
-        if ((eventMask & EV_UPDATED) != 0) {
+ */
-            connect(objs[n], SIGNAL(objectUpdatedAuto(UAVObject *)), this, SLOT(objectUpdatedAuto(UAVObject *)));
+void Telemetry::connectToObject(UAVObject *obj, quint32 eventMask)
-        }
+{
-        if ((eventMask & EV_UPDATED_MANUAL) != 0) {
+    // Disconnect all
-            connect(objs[n], SIGNAL(objectUpdatedManual(UAVObject *)), this, SLOT(objectUpdatedManual(UAVObject *)));
+    obj->disconnect(this);
-        }
+    // Connect only the selected events
-        if ((eventMask & EV_UPDATED_PERIODIC) != 0) {
+    if ((eventMask & EV_UNPACKED) != 0) {
-            connect(objs[n], SIGNAL(objectUpdatedPeriodic(UAVObject *)), this, SLOT(objectUpdatedPeriodic(UAVObject *)));
+        connect(obj, SIGNAL(objectUnpacked(UAVObject *)), this, SLOT(objectUnpacked(UAVObject *)));
-        }
+    }
-        if ((eventMask & EV_UPDATE_REQ) != 0) {
+    if ((eventMask & EV_UPDATED) != 0) {
-            connect(objs[n], SIGNAL(updateRequested(UAVObject *)), this, SLOT(updateRequested(UAVObject *)));
+        connect(obj, SIGNAL(objectUpdatedAuto(UAVObject *)), this, SLOT(objectUpdatedAuto(UAVObject *)));
-        }
+    }
+    if ((eventMask & EV_UPDATED_MANUAL) != 0) {
+        connect(obj, SIGNAL(objectUpdatedManual(UAVObject *, bool)), this, SLOT(objectUpdatedManual(UAVObject *, bool)));
+    }
+    if ((eventMask & EV_UPDATED_PERIODIC) != 0) {
+        connect(obj, SIGNAL(objectUpdatedPeriodic(UAVObject *)), this, SLOT(objectUpdatedPeriodic(UAVObject *)));
+    }
+    if ((eventMask & EV_UPDATE_REQ) != 0) {
+        connect(obj, SIGNAL(updateRequested(UAVObject *, bool)), this, SLOT(updateRequested(UAVObject *, bool)));
     }
 }
 
@@ -160,19 +174,21 @@ void Telemetry::updateObject(UAVObject *obj, quint32 eventType)
     if (updateMode == UAVObject::UPDATEMODE_PERIODIC) {
         // Set update period
         setUpdatePeriod(obj, metadata.gcsTelemetryUpdatePeriod);
-        // Connect signals for all instances
+        // Connect signals
         eventMask = EV_UPDATED_MANUAL | EV_UPDATE_REQ | EV_UPDATED_PERIODIC;
         if (dynamic_cast<UAVMetaObject *>(obj) != NULL) {
-            eventMask |= EV_UNPACKED; // we also need to act on remote updates (unpack events)
+            // we also need to act on remote updates (unpack events)
+            eventMask |= EV_UNPACKED;
         }
         connectToObjectInstances(obj, eventMask);
     } else if (updateMode == UAVObject::UPDATEMODE_ONCHANGE) {
         // Set update period
         setUpdatePeriod(obj, 0);
-        // Connect signals for all instances
+        // Connect signals
         eventMask = EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ;
         if (dynamic_cast<UAVMetaObject *>(obj) != NULL) {
-            eventMask |= EV_UNPACKED; // we also need to act on remote updates (unpack events)
+            // we also need to act on remote updates (unpack events)
+            eventMask |= EV_UNPACKED;
         }
         connectToObjectInstances(obj, eventMask);
     } else if (updateMode == UAVObject::UPDATEMODE_THROTTLED) {
@@ -182,24 +198,26 @@ void Telemetry::updateObject(UAVObject *obj, quint32 eventType)
             if (eventType == EV_NONE) {
                 setUpdatePeriod(obj, metadata.gcsTelemetryUpdatePeriod);
             }
-            // Connect signals for all instances
+            // Connect signals
             eventMask = EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ | EV_UPDATED_PERIODIC;
         } else {
             // Otherwise, we just received an object update, so switch to periodic for the timeout period to prevent more updates
-            // Connect signals for all instances
+            // Connect signals
             eventMask = EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ;
         }
         if (dynamic_cast<UAVMetaObject *>(obj) != NULL) {
-            eventMask |= EV_UNPACKED; // we also need to act on remote updates (unpack events)
+            // we also need to act on remote updates (unpack events)
+            eventMask |= EV_UNPACKED;
         }
         connectToObjectInstances(obj, eventMask);
     } else if (updateMode == UAVObject::UPDATEMODE_MANUAL) {
         // Set update period
         setUpdatePeriod(obj, 0);
-        // Connect signals for all instances
+        // Connect signals
         eventMask = EV_UPDATED_MANUAL | EV_UPDATE_REQ;
         if (dynamic_cast<UAVMetaObject *>(obj) != NULL) {
-            eventMask |= EV_UNPACKED; // we also need to act on remote updates (unpack events)
+            // we also need to act on remote updates (unpack events)
+            eventMask |= EV_UNPACKED;
         }
         connectToObjectInstances(obj, eventMask);
     }
@@ -211,21 +229,27 @@ void Telemetry::updateObject(UAVObject *obj, quint32 eventType)
 void Telemetry::transactionCompleted(UAVObject *obj, bool success)
 {
     // Lookup the transaction in the transaction map.
-    quint32 objId = obj->getObjID();
+    ObjectTransactionInfo *transInfo = findTransaction(obj);
+
+    if (transInfo) {
+        if (success) {
+#ifdef VERBOSE_TELEMETRY
+            qDebug() << "Telemetry - transaction successful for object" << obj->toStringBrief();
+#endif
+        } else {
+            qWarning() << "Telemetry - !!! transaction failed for object" << obj->toStringBrief();
+        }
 
-    QMap<quint32, ObjectTransactionInfo *>::iterator itr = transMap.find(objId);
-    if (itr != transMap.end()) {
-        ObjectTransactionInfo *transInfo = itr.value();
         // Remove this transaction as it's complete.
-        transInfo->timer->stop();
+        closeTransaction(transInfo);
-        transMap.remove(objId);
+
-        delete transInfo;
         // Send signal
         obj->emitTransactionCompleted(success);
+
         // Process new object updates from queue
         processObjectQueue();
     } else {
-        qDebug() << "Error: received a transaction completed when did not expect it.";
+        qWarning() << "Telemetry - Error: received a transaction completed when did not expect it for" << obj->toStringBrief();
     }
 }
 
@@ -234,25 +258,33 @@ void Telemetry::transactionCompleted(UAVObject *obj, bool success)
  */
 void Telemetry::transactionTimeout(ObjectTransactionInfo *transInfo)
 {
-    transInfo->timer->stop();
     // Check if more retries are pending
     if (transInfo->retriesRemaining > 0) {
-        --transInfo->retriesRemaining;
+#ifdef VERBOSE_TELEMETRY
-        processObjectTransaction(transInfo);
+        qDebug().nospace() << "Telemetry - transaction timed out for object " << transInfo->obj->toStringBrief() << ", retrying...";
+#endif
         ++txRetries;
+        --transInfo->retriesRemaining;
+
+        // Retry the transaction
+        processObjectTransaction(transInfo);
     } else {
-        // Stop the timer.
+        qWarning().nospace() << "Telemetry - !!! transaction timed out for object " << transInfo->obj->toStringBrief();
-        transInfo->timer->stop();
+
+        ++txErrors;
+
         // Terminate transaction
         utalk->cancelTransaction(transInfo->obj);
-        // Send signal
+
-        transInfo->obj->emitTransactionCompleted(false);
         // Remove this transaction as it's complete.
-        transMap.remove(transInfo->obj->getObjID());
+        UAVObject *obj = transInfo->obj;
-        delete transInfo;
+        closeTransaction(transInfo);
+
+        // Send signal
+        obj->emitTransactionCompleted(false);
+
         // Process new object updates from queue
         processObjectQueue();
-        ++txErrors;
     }
 }
 
@@ -262,18 +294,32 @@ void Telemetry::transactionTimeout(ObjectTransactionInfo *transInfo)
 void Telemetry::processObjectTransaction(ObjectTransactionInfo *transInfo)
 {
     // Initiate transaction
+    bool sent = false;
+
     if (transInfo->objRequest) {
-        utalk->sendObjectRequest(transInfo->obj, transInfo->allInstances);
+#ifdef VERBOSE_TELEMETRY
+        qDebug().nospace() << "Telemetry - sending request for object " << transInfo->obj->toStringBrief() << ", " << (transInfo->allInstances ? "all" : "single") << " " << (transInfo->acked ? "acked" : "");
+#endif
+        sent = utalk->sendObjectRequest(transInfo->obj, transInfo->allInstances);
     } else {
-        utalk->sendObject(transInfo->obj, transInfo->acked, transInfo->allInstances);
+#ifdef VERBOSE_TELEMETRY
+        qDebug().nospace() << "Telemetry - sending object " << transInfo->obj->toStringBrief() << ", " << (transInfo->allInstances ? "all" : "single") << " " << (transInfo->acked ? "acked" : "");
+#endif
+        sent = utalk->sendObject(transInfo->obj, transInfo->acked, transInfo->allInstances);
     }
-    // Start timer if a response is expected
+    // Check if a response is needed now or will arrive asynchronously
     if (transInfo->objRequest || transInfo->acked) {
-        transInfo->timer->start(REQ_TIMEOUT_MS);
+        if (sent) {
+            // Start timer if a response is expected
+            transInfo->timer->start(REQ_TIMEOUT_MS);
+        } else {
+            // message was not sent, the transaction will not complete and will timeout
+            // there is no need to wait to close the transaction and notify of completion failure
+            // transactionCompleted(transInfo->obj, false);
+        }
     } else {
-        // Otherwise, remove this transaction as it's complete.
+        // not transacted, so just close the transaction with no notification of completion
-        transMap.remove(transInfo->obj->getObjID());
+        closeTransaction(transInfo);
-        delete transInfo;
     }
 }
 
@@ -293,14 +339,15 @@ void Telemetry::processObjectUpdates(UAVObject *obj, EventMask event, bool allIn
             objPriorityQueue.enqueue(objInfo);
         } else {
             ++txErrors;
+            qWarning().nospace() << "Telemetry - !!! priority event queue is full, event lost " << obj->toStringBrief();
             obj->emitTransactionCompleted(false);
-            qDebug() << tr("Telemetry: priority event queue is full, event lost (%1)").arg(obj->getName());
         }
     } else {
         if (objQueue.length() < MAX_QUEUE_SIZE) {
             objQueue.enqueue(objInfo);
         } else {
             ++txErrors;
+            qWarning().nospace() << "Telemetry - !!! event queue is full, event lost " << obj->toStringBrief();
             obj->emitTransactionCompleted(false);
         }
     }
@@ -330,7 +377,9 @@ void Telemetry::processObjectQueue()
     GCSTelemetryStats::DataFields gcsStats = gcsStatsObj->getData();
     if (gcsStats.Status != GCSTelemetryStats::STATUS_CONNECTED) {
         objQueue.clear();
-        if (objInfo.obj->getObjID() != GCSTelemetryStats::OBJID && objInfo.obj->getObjID() != OPLinkSettings::OBJID && objInfo.obj->getObjID() != ObjectPersistence::OBJID) {
+        if ((objInfo.obj->getObjID() != GCSTelemetryStats::OBJID) &&
+            (objInfo.obj->getObjID() != OPLinkSettings::OBJID) &&
+            (objInfo.obj->getObjID() != ObjectPersistence::OBJID)) {
             objInfo.obj->emitTransactionCompleted(false);
             return;
         }
@@ -340,9 +389,15 @@ void Telemetry::processObjectQueue()
     UAVObject::Metadata metadata     = objInfo.obj->getMetadata();
     UAVObject::UpdateMode updateMode = UAVObject::GetGcsTelemetryUpdateMode(metadata);
     if ((objInfo.event != EV_UNPACKED) && ((objInfo.event != EV_UPDATED_PERIODIC) || (updateMode != UAVObject::UPDATEMODE_THROTTLED))) {
-        QMap<quint32, ObjectTransactionInfo *>::iterator itr = transMap.find(objInfo.obj->getObjID());
+        // Check if a transaction for that object already exists
-        if (itr != transMap.end()) {
+        // It is allowed to have multiple transaction on the same object ID provided that the instance IDs are different
-            qDebug() << "!!!!!! Making request for an object: " << objInfo.obj->getName() << " for which a request is already in progress!!!!!!";
+        // If an "all instances" transaction is running, then it is not allowed to start another transaction with same object ID
+        // If a single instance transaction is running, then starting an "all instance" transaction is not allowed
+        // TODO make the above logic a reality...
+        if (findTransaction(objInfo.obj)) {
+            qWarning().nospace() << "Telemetry - !!! Making request for an object " << objInfo.obj->toStringBrief() << " for which a request is already in progress";
+            // objInfo.obj->emitTransactionCompleted(false);
+            return;
         }
         UAVObject::Metadata metadata     = objInfo.obj->getMetadata();
         ObjectTransactionInfo *transInfo = new ObjectTransactionInfo(this);
@@ -357,7 +412,7 @@ void Telemetry::processObjectQueue()
         }
         transInfo->telem = this;
         // Insert the transaction into the transaction map.
-        transMap.insert(objInfo.obj->getObjID(), transInfo);
+        openTransaction(transInfo);
         processObjectTransaction(transInfo);
     }
 
@@ -396,6 +451,7 @@ void Telemetry::processPeriodicUpdates()
     qint32 elapsedMs = 0;
     QTime time;
     qint32 offset;
+    bool allInstances;
     for (int n = 0; n < objList.length(); ++n) {
         objinfo = &objList[n];
         // If object is configured for periodic updates
@@ -408,8 +464,9 @@ void Telemetry::processPeriodicUpdates()
                 objinfo->timeToNextUpdateMs = objinfo->updatePeriodMs - offset;
                 // Send object
                 time.start();
-                processObjectUpdates(objinfo->obj, EV_UPDATED_PERIODIC, true, false);
+                allInstances = !objinfo->obj->isSingleInstance();
-                elapsedMs = time.elapsed();
+                processObjectUpdates(objinfo->obj, EV_UPDATED_PERIODIC, allInstances, false);
+                elapsedMs    = time.elapsed();
                 // Update timeToNextUpdateMs with the elapsed delay of sending the object;
                 timeToNextUpdateMs += elapsedMs;
             }
@@ -443,15 +500,18 @@ Telemetry::TelemetryStats Telemetry::getStats()
     TelemetryStats stats;
 
     stats.txBytes       = utalkStats.txBytes;
-    stats.rxBytes       = utalkStats.rxBytes;
     stats.txObjectBytes = utalkStats.txObjectBytes;
-    stats.rxObjectBytes = utalkStats.rxObjectBytes;
-    stats.rxObjects     = utalkStats.rxObjects;
     stats.txObjects     = utalkStats.txObjects;
     stats.txErrors      = utalkStats.txErrors + txErrors;
-    stats.rxErrors      = utalkStats.rxErrors;
     stats.txRetries     = txRetries;
 
+    stats.rxBytes       = utalkStats.rxBytes;
+    stats.rxObjectBytes = utalkStats.rxObjectBytes;
+    stats.rxObjects     = utalkStats.rxObjects;
+    stats.rxErrors      = utalkStats.rxErrors;
+    stats.rxSyncErrors  = utalkStats.rxSyncErrors;
+    stats.rxCrcErrors   = utalkStats.rxCrcErrors;
+
     // Done
     return stats;
 }
@@ -472,11 +532,13 @@ void Telemetry::objectUpdatedAuto(UAVObject *obj)
     processObjectUpdates(obj, EV_UPDATED, false, true);
 }
 
-void Telemetry::objectUpdatedManual(UAVObject *obj)
+void Telemetry::objectUpdatedManual(UAVObject *obj, bool all)
 {
     QMutexLocker locker(mutex);
 
-    processObjectUpdates(obj, EV_UPDATED_MANUAL, false, true);
+    bool allInstances = obj->isSingleInstance() ? false : all;
+
+    processObjectUpdates(obj, EV_UPDATED_MANUAL, allInstances, true);
 }
 
 void Telemetry::objectUpdatedPeriodic(UAVObject *obj)
@@ -493,11 +555,13 @@ void Telemetry::objectUnpacked(UAVObject *obj)
     processObjectUpdates(obj, EV_UNPACKED, false, true);
 }
 
-void Telemetry::updateRequested(UAVObject *obj)
+void Telemetry::updateRequested(UAVObject *obj, bool all)
 {
     QMutexLocker locker(mutex);
 
-    processObjectUpdates(obj, EV_UPDATE_REQ, false, true);
+    bool allInstances = obj->isSingleInstance() ? false : all;
+
+    processObjectUpdates(obj, EV_UPDATE_REQ, allInstances, true);
 }
 
 void Telemetry::newObject(UAVObject *obj)
@@ -514,6 +578,67 @@ void Telemetry::newInstance(UAVObject *obj)
     registerObject(obj);
 }
 
+ObjectTransactionInfo *Telemetry::findTransaction(UAVObject *obj)
+{
+    quint32 objId  = obj->getObjID();
+    quint16 instId = obj->getInstID();
+
+    // Lookup the transaction in the transaction map
+    QMap<quint32, ObjectTransactionInfo *> *objTransactions = transMap.value(objId);
+    if (objTransactions != NULL) {
+        ObjectTransactionInfo *trans = objTransactions->value(instId);
+        if (trans == NULL) {
+            // see if there is an ALL_INSTANCES transaction
+            trans = objTransactions->value(UAVTalk::ALL_INSTANCES);
+        }
+        return trans;
+    }
+    return NULL;
+}
+
+void Telemetry::openTransaction(ObjectTransactionInfo *trans)
+{
+    quint32 objId  = trans->obj->getObjID();
+    quint16 instId = trans->allInstances ? UAVTalk::ALL_INSTANCES : trans->obj->getInstID();
+
+    QMap<quint32, ObjectTransactionInfo *> *objTransactions = transMap.value(objId);
+    if (objTransactions == NULL) {
+        objTransactions = new QMap<quint32, ObjectTransactionInfo *>();
+        transMap.insert(objId, objTransactions);
+    }
+    objTransactions->insert(instId, trans);
+}
+
+void Telemetry::closeTransaction(ObjectTransactionInfo *trans)
+{
+    quint32 objId  = trans->obj->getObjID();
+    quint16 instId = trans->allInstances ? UAVTalk::ALL_INSTANCES : trans->obj->getInstID();
+
+    QMap<quint32, ObjectTransactionInfo *> *objTransactions = transMap.value(objId);
+    if (objTransactions != NULL) {
+        objTransactions->remove(instId);
+        // Keep the map even if it is empty
+        // There are at most 100 different object IDs...
+    }
+    delete trans;
+}
+
+void Telemetry::closeAllTransactions()
+{
+    foreach(quint32 objId, transMap.keys()) {
+        QMap<quint32, ObjectTransactionInfo *> *objTransactions = transMap.value(objId);
+        foreach(quint32 instId, objTransactions->keys()) {
+            ObjectTransactionInfo *trans = objTransactions->value(instId);
+
+            qWarning() << "Telemetry - closing active transaction for object" << trans->obj->toStringBrief();
+            objTransactions->remove(instId);
+            delete trans;
+        }
+        transMap.remove(objId);
+        delete objTransactions;
+    }
+}
+
 ObjectTransactionInfo::ObjectTransactionInfo(QObject *parent) : QObject(parent)
 {
     obj = 0;
@@ -524,7 +649,7 @@ ObjectTransactionInfo::ObjectTransactionInfo(QObject *parent) : QObject(parent)
     telem = 0;
     // Setup transaction timer
     timer = new QTimer(this);
-    timer->stop();
+    timer->setSingleShot(true);
     connect(timer, SIGNAL(timeout()), this, SLOT(timeout()));
 }
 

ground/openpilotgcs/src/plugins/uavtalk/telemetry.h

@@ -60,14 +60,17 @@ class Telemetry : public QObject {
 public:
     typedef struct {
         quint32 txBytes;
-        quint32 rxBytes;
         quint32 txObjectBytes;
-        quint32 rxObjectBytes;
-        quint32 rxObjects;
         quint32 txObjects;
         quint32 txErrors;
-        quint32 rxErrors;
         quint32 txRetries;
+
+        quint32 rxBytes;
+        quint32 rxObjectBytes;
+        quint32 rxObjects;
+        quint32 rxErrors;
+        quint32 rxSyncErrors;
+        quint32 rxCrcErrors;
     } TelemetryStats;
 
     Telemetry(UAVTalk *utalk, UAVObjectManager *objMngr);
@@ -95,8 +98,8 @@ private:
         EV_UNPACKED         = 0x01, /** Object data updated by unpacking */
         EV_UPDATED          = 0x02, /** Object data updated by changing the data structure */
         EV_UPDATED_MANUAL   = 0x04, /** Object update event manually generated */
-        EV_UPDATED_PERIODIC = 0x8, /** Object update event generated by timer */
+        EV_UPDATED_PERIODIC = 0x08, /** Object update event generated by timer */
-        EV_UPDATE_REQ       = 0x010 /** Request to update object data */
+        EV_UPDATE_REQ       = 0x10 /** Request to update object data */
     } EventMask;
 
     typedef struct {
@@ -118,7 +121,7 @@ private:
     QList<ObjectTimeInfo> objList;
     QQueue<ObjectQueueInfo> objQueue;
     QQueue<ObjectQueueInfo> objPriorityQueue;
-    QMap<quint32, ObjectTransactionInfo *>transMap;
+    QMap<quint32, QMap<quint32, ObjectTransactionInfo *> *> transMap;
     QMutex *mutex;
     QTimer *updateTimer;
     QTimer *statsTimer;
@@ -131,17 +134,23 @@ private:
     void addObject(UAVObject *obj);
     void setUpdatePeriod(UAVObject *obj, qint32 periodMs);
     void connectToObjectInstances(UAVObject *obj, quint32 eventMask);
+    void connectToObject(UAVObject *obj, quint32 eventMask);
     void updateObject(UAVObject *obj, quint32 eventMask);
     void processObjectUpdates(UAVObject *obj, EventMask event, bool allInstances, bool priority);
     void processObjectTransaction(ObjectTransactionInfo *transInfo);
     void processObjectQueue();
 
+    ObjectTransactionInfo *findTransaction(UAVObject *obj);
+    void openTransaction(ObjectTransactionInfo *trans);
+    void closeTransaction(ObjectTransactionInfo *trans);
+    void closeAllTransactions();
+
 private slots:
     void objectUpdatedAuto(UAVObject *obj);
-    void objectUpdatedManual(UAVObject *obj);
+    void objectUpdatedManual(UAVObject *obj, bool all = false);
     void objectUpdatedPeriodic(UAVObject *obj);
     void objectUnpacked(UAVObject *obj);
-    void updateRequested(UAVObject *obj);
+    void updateRequested(UAVObject *obj, bool all = false);
     void newObject(UAVObject *obj);
     void newInstance(UAVObject *obj);
     void processPeriodicUpdates();

ground/openpilotgcs/src/plugins/uavtalk/telemetrymanager.cpp

@@ -30,8 +30,7 @@
 #include <coreplugin/icore.h>
 #include <coreplugin/threadmanager.h>
 
-TelemetryManager::TelemetryManager() :
+TelemetryManager::TelemetryManager() : autopilotConnected(false)
-    autopilotConnected(false)
 {
     moveToThread(Core::ICore::instance()->threadManager()->getRealTimeThread());
     // Get UAVObjectManager instance
@@ -59,9 +58,31 @@ void TelemetryManager::start(QIODevice *dev)
 
 void TelemetryManager::onStart()
 {
-    utalk        = new UAVTalk(device, objMngr);
+    utalk = new UAVTalk(device, objMngr);
+    if (false) {
+        // UAVTalk must be thread safe and for that:
+        // 1- all public methods must lock a mutex
+        // 2- the reader thread must lock that mutex too
+        // The reader thread locks the mutex once a packet is read and decoded.
+        // It is assumed that the UAVObjectManager is thread safe
+
+        // Create the reader and move it to the reader thread
+        IODeviceReader *reader = new IODeviceReader(utalk);
+        reader->moveToThread(&readerThread);
+        // The reader will be deleted (later) when the thread finishes
+        connect(&readerThread, &QThread::finished, reader, &QObject::deleteLater);
+        // Connect IO device to reader
+        connect(device, SIGNAL(readyRead()), reader, SLOT(read()));
+        // start the reader thread
+        readerThread.start();
+    } else {
+        // Connect IO device to reader
+        connect(device, SIGNAL(readyRead()), utalk, SLOT(processInputStream()));
+    }
+
     telemetry    = new Telemetry(utalk, objMngr);
     telemetryMon = new TelemetryMonitor(objMngr, telemetry);
+
     connect(telemetryMon, SIGNAL(connected()), this, SLOT(onConnect()));
     connect(telemetryMon, SIGNAL(disconnected()), this, SLOT(onDisconnect()));
     connect(telemetryMon, SIGNAL(telemetryUpdated(double, double)), this, SLOT(onTelemetryUpdate(double, double)));
@@ -70,6 +91,11 @@ void TelemetryManager::onStart()
 void TelemetryManager::stop()
 {
     emit myStop();
+
+    if (false) {
+        readerThread.quit();
+        readerThread.wait();
+    }
 }
 
 void TelemetryManager::onStop()
@@ -97,3 +123,11 @@ void TelemetryManager::onTelemetryUpdate(double txRate, double rxRate)
 {
     emit telemetryUpdated(txRate, rxRate);
 }
+
+IODeviceReader::IODeviceReader(UAVTalk *uavTalk) : uavTalk(uavTalk)
+{}
+
+void IODeviceReader::read()
+{
+    uavTalk->processInputStream();
+}

ground/openpilotgcs/src/plugins/uavtalk/telemetrymanager.h

@@ -68,6 +68,19 @@ private:
     TelemetryMonitor *telemetryMon;
     QIODevice *device;
     bool autopilotConnected;
+    QThread readerThread;
+};
+
+
+class IODeviceReader : public QObject {
+    Q_OBJECT
+public:
+    IODeviceReader(UAVTalk *uavTalk);
+
+    UAVTalk *uavTalk;
+
+public slots:
+    void read();
 };
 
 #endif // TELEMETRYMANAGER_H

ground/openpilotgcs/src/plugins/uavtalk/telemetrymonitor.cpp

@@ -179,6 +179,7 @@ void TelemetryMonitor::firmwareIAPUpdated(UAVObject *obj)
     QMutexLocker locker(mutex);
 
     if (firmwareIAPObj->getBoardType() != 0) {
+        disconnect(firmwareIAPObj);
         emit connected();
     }
 }
@@ -198,11 +199,16 @@ void TelemetryMonitor::processStatsUpdates()
     tel->resetStats();
 
     // Update stats object
-    gcsStats.RxDataRate  = (float)telStats.rxBytes / ((float)statsTimer->interval() / 1000.0);
+    gcsStats.TxDataRate    = (float)telStats.txBytes / ((float)statsTimer->interval() / 1000.0);
-    gcsStats.TxDataRate  = (float)telStats.txBytes / ((float)statsTimer->interval() / 1000.0);
+    gcsStats.TxBytes      += telStats.txBytes;
-    gcsStats.RxFailures += telStats.rxErrors;
+    gcsStats.TxFailures   += telStats.txErrors;
-    gcsStats.TxFailures += telStats.txErrors;
+    gcsStats.TxRetries    += telStats.txRetries;
-    gcsStats.TxRetries  += telStats.txRetries;
+
+    gcsStats.RxDataRate    = (float)telStats.rxBytes / ((float)statsTimer->interval() / 1000.0);
+    gcsStats.RxBytes      += telStats.rxBytes;
+    gcsStats.RxFailures   += telStats.rxErrors;
+    gcsStats.RxSyncErrors += telStats.rxSyncErrors;
+    gcsStats.RxCrcErrors  += telStats.rxCrcErrors;
 
     // Check for a connection timeout
     bool connectionTimeout;

ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp

@@ -25,56 +25,37 @@
  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  */
 #include "uavtalk.h"
-#include <QtEndian>
-#include <QDebug>
 #include <extensionsystem/pluginmanager.h>
 #include <coreplugin/generalsettings.h>
-// #define UAVTALK_DEBUG
+#include <utils/crc.h>
-#ifdef UAVTALK_DEBUG
+
-  #define UAVTALK_QXTLOG_DEBUG(args ...)
+#include <QtEndian>
-#else // UAVTALK_DEBUG
+#include <QDebug>
-  #define UAVTALK_QXTLOG_DEBUG(args ...)
+#include <QEventLoop>
-#endif // UAVTALK_DEBUG
+
+#ifdef VERBOSE_UAVTALK
+// uncomment and adapt the following lines to filter verbose logging to include specific object(s) only
+// #include "flighttelemetrystats.h"
+// #define VERBOSE_FILTER(objId) if (objId == FlightTelemetryStats::OBJID)
+#endif
+#ifndef VERBOSE_FILTER
+#define VERBOSE_FILTER(objId)
+#endif
 
 #define SYNC_VAL 0x3C
 
-const quint8 UAVTalk::crc_table[256] = {
+using namespace Utils;
-    0x00, 0x07, 0x0e, 0x09, 0x1c, 0x1b, 0x12, 0x15, 0x38, 0x3f, 0x36, 0x31, 0x24, 0x23, 0x2a, 0x2d,
-    0x70, 0x77, 0x7e, 0x79, 0x6c, 0x6b, 0x62, 0x65, 0x48, 0x4f, 0x46, 0x41, 0x54, 0x53, 0x5a, 0x5d,
-    0xe0, 0xe7, 0xee, 0xe9, 0xfc, 0xfb, 0xf2, 0xf5, 0xd8, 0xdf, 0xd6, 0xd1, 0xc4, 0xc3, 0xca, 0xcd,
-    0x90, 0x97, 0x9e, 0x99, 0x8c, 0x8b, 0x82, 0x85, 0xa8, 0xaf, 0xa6, 0xa1, 0xb4, 0xb3, 0xba, 0xbd,
-    0xc7, 0xc0, 0xc9, 0xce, 0xdb, 0xdc, 0xd5, 0xd2, 0xff, 0xf8, 0xf1, 0xf6, 0xe3, 0xe4, 0xed, 0xea,
-    0xb7, 0xb0, 0xb9, 0xbe, 0xab, 0xac, 0xa5, 0xa2, 0x8f, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9d, 0x9a,
-    0x27, 0x20, 0x29, 0x2e, 0x3b, 0x3c, 0x35, 0x32, 0x1f, 0x18, 0x11, 0x16, 0x03, 0x04, 0x0d, 0x0a,
-    0x57, 0x50, 0x59, 0x5e, 0x4b, 0x4c, 0x45, 0x42, 0x6f, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7d, 0x7a,
-    0x89, 0x8e, 0x87, 0x80, 0x95, 0x92, 0x9b, 0x9c, 0xb1, 0xb6, 0xbf, 0xb8, 0xad, 0xaa, 0xa3, 0xa4,
-    0xf9, 0xfe, 0xf7, 0xf0, 0xe5, 0xe2, 0xeb, 0xec, 0xc1, 0xc6, 0xcf, 0xc8, 0xdd, 0xda, 0xd3, 0xd4,
-    0x69, 0x6e, 0x67, 0x60, 0x75, 0x72, 0x7b, 0x7c, 0x51, 0x56, 0x5f, 0x58, 0x4d, 0x4a, 0x43, 0x44,
-    0x19, 0x1e, 0x17, 0x10, 0x05, 0x02, 0x0b, 0x0c, 0x21, 0x26, 0x2f, 0x28, 0x3d, 0x3a, 0x33, 0x34,
-    0x4e, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5c, 0x5b, 0x76, 0x71, 0x78, 0x7f, 0x6a, 0x6d, 0x64, 0x63,
-    0x3e, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2c, 0x2b, 0x06, 0x01, 0x08, 0x0f, 0x1a, 0x1d, 0x14, 0x13,
-    0xae, 0xa9, 0xa0, 0xa7, 0xb2, 0xb5, 0xbc, 0xbb, 0x96, 0x91, 0x98, 0x9f, 0x8a, 0x8d, 0x84, 0x83,
-    0xde, 0xd9, 0xd0, 0xd7, 0xc2, 0xc5, 0xcc, 0xcb, 0xe6, 0xe1, 0xe8, 0xef, 0xfa, 0xfd, 0xf4, 0xf3
-};
-
 
 /**
  * Constructor
  */
-UAVTalk::UAVTalk(QIODevice *iodev, UAVObjectManager *objMngr)
+UAVTalk::UAVTalk(QIODevice *iodev, UAVObjectManager *objMngr) : io(iodev), objMngr(objMngr), mutex(QMutex::Recursive)
 {
-    io = iodev;
-
-    this->objMngr  = objMngr;
-
     rxState = STATE_SYNC;
     rxPacketLength = 0;
 
-    mutex = new QMutex(QMutex::Recursive);
-
     memset(&stats, 0, sizeof(ComStats));
 
-    connect(io, SIGNAL(readyRead()), this, SLOT(processInputStream()));
     ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
     Core::Internal::GeneralSettings *settings = pm->getObject<Core::Internal::GeneralSettings>();
     useUDPMirror = settings->useUDPMirror();
@@ -91,18 +72,18 @@ UAVTalk::UAVTalk(QIODevice *iodev, UAVObjectManager *objMngr)
 
 UAVTalk::~UAVTalk()
 {
-    // According to Qt, it is not necessary to disconnect upon
+    // According to Qt, it is not necessary to disconnect upon object deletion.
-    // object deletion.
+    // disconnect(io, SIGNAL(readyRead()), worker, SLOT(processInputStream()));
-    // disconnect(io, SIGNAL(readyRead()), this, SLOT(processInputStream()));
-}
 
+    closeAllTransactions();
+}
 
 /**
  * Reset the statistics counters
  */
 void UAVTalk::resetStats()
 {
-    QMutexLocker locker(mutex);
+    QMutexLocker locker(&mutex);
 
     memset(&stats, 0, sizeof(ComStats));
 }
@@ -112,26 +93,11 @@ void UAVTalk::resetStats()
  */
 UAVTalk::ComStats UAVTalk::getStats()
 {
-    QMutexLocker locker(mutex);
+    QMutexLocker locker(&mutex);
 
     return stats;
 }
 
-/**
- * Called each time there are data in the input buffer
- */
-void UAVTalk::processInputStream()
-{
-    quint8 tmp;
-
-    if (io && io->isReadable()) {
-        while (io->bytesAvailable() > 0) {
-            io->read((char *)&tmp, 1);
-            processInputByte(tmp);
-        }
-    }
-}
-
 void UAVTalk::dummyUDPRead()
 {
     QUdpSocket *socket = qobject_cast<QUdpSocket *>(sender());
@@ -143,20 +109,6 @@ void UAVTalk::dummyUDPRead()
     }
 }
 
-/**
- * Request an update for the specified object, on success the object data would have been
- * updated by the GCS.
- * \param[in] obj Object to update
- * \param[in] allInstances If set true then all instances will be updated
- * \return Success (true), Failure (false)
- */
-bool UAVTalk::sendObjectRequest(UAVObject *obj, bool allInstances)
-{
-    QMutexLocker locker(mutex);
-
-    return objectTransaction(obj, TYPE_OBJ_REQ, allInstances);
-}
-
 /**
  * Send the specified object through the telemetry link.
  * \param[in] obj Object to send
@@ -166,13 +118,44 @@ bool UAVTalk::sendObjectRequest(UAVObject *obj, bool allInstances)
  */
 bool UAVTalk::sendObject(UAVObject *obj, bool acked, bool allInstances)
 {
-    QMutexLocker locker(mutex);
+    QMutexLocker locker(&mutex);
 
-    if (acked) {
+    quint16 instId = 0;
-        return objectTransaction(obj, TYPE_OBJ_ACK, allInstances);
+
-    } else {
+    if (allInstances) {
-        return objectTransaction(obj, TYPE_OBJ, allInstances);
+        instId = ALL_INSTANCES;
+    } else if (obj) {
+        instId = obj->getInstID();
     }
+    bool success = false;
+    if (acked) {
+        success = objectTransaction(TYPE_OBJ_ACK, obj->getObjID(), instId, obj);
+    } else {
+        success = objectTransaction(TYPE_OBJ, obj->getObjID(), instId, obj);
+    }
+
+    return success;
+}
+
+/**
+ * Request an update for the specified object, on success the object data would have been
+ * updated by the GCS.
+ * \param[in] obj Object to update
+ * \param[in] allInstances If set true then all instances will be updated
+ * \return Success (true), Failure (false)
+ */
+bool UAVTalk::sendObjectRequest(UAVObject *obj, bool allInstances)
+{
+    QMutexLocker locker(&mutex);
+
+    quint16 instId = 0;
+
+    if (allInstances) {
+        instId = ALL_INSTANCES;
+    } else if (obj) {
+        instId = obj->getInstID();
+    }
+    return objectTransaction(TYPE_OBJ_REQ, obj->getObjID(), instId, obj);
 }
 
 /**
@@ -180,16 +163,14 @@ bool UAVTalk::sendObject(UAVObject *obj, bool acked, bool allInstances)
  */
 void UAVTalk::cancelTransaction(UAVObject *obj)
 {
-    QMutexLocker locker(mutex);
+    QMutexLocker locker(&mutex);
-    quint32 objId = obj->getObjID();
 
     if (io.isNull()) {
         return;
     }
-    QMap<quint32, Transaction *>::iterator itr = transMap.find(objId);
+    Transaction *trans = findTransaction(obj->getObjID(), obj->getInstID());
-    if (itr != transMap.end()) {
+    if (trans != NULL) {
-        transMap.remove(objId);
+        closeTransaction(trans);
-        delete itr.value();
     }
 }
 
@@ -203,26 +184,60 @@ void UAVTalk::cancelTransaction(UAVObject *obj)
  * \param[in] allInstances If set true then all instances will be updated
  * \return Success (true), Failure (false)
  */
-bool UAVTalk::objectTransaction(UAVObject *obj, quint8 type, bool allInstances)
+bool UAVTalk::objectTransaction(quint8 type, quint32 objId, quint16 instId, UAVObject *obj)
 {
+    Q_ASSERT(obj);
     // Send object depending on if a response is needed
+    // transactions of TYPE_OBJ_REQ are acked by the response
     if (type == TYPE_OBJ_ACK || type == TYPE_OBJ_REQ) {
-        if (transmitObject(obj, type, allInstances)) {
+        if (transmitObject(type, objId, instId, obj)) {
-            Transaction *trans = new Transaction();
+            openTransaction(type, objId, instId);
-            trans->obj = obj;
-            trans->allInstances = allInstances;
-            transMap.insert(obj->getObjID(), trans);
             return true;
         } else {
             return false;
         }
     } else if (type == TYPE_OBJ) {
-        return transmitObject(obj, TYPE_OBJ, allInstances);
+        return transmitObject(type, objId, instId, obj);
     } else {
         return false;
     }
 }
 
+/**
+ * Called each time there are data in the input buffer
+ */
+void UAVTalk::processInputStream()
+{
+    quint8 tmp;
+
+    if (io && io->isReadable()) {
+        while (io->bytesAvailable() > 0) {
+            int ret = io->read((char *)&tmp, 1);
+            if (ret != -1) {
+                processInputByte(tmp);
+            } else {
+                // TODOD
+            }
+            if (rxState == STATE_COMPLETE) {
+                mutex.lock();
+                if (receiveObject(rxType, rxObjId, rxInstId, rxBuffer, rxLength)) {
+                    stats.rxObjectBytes += rxLength;
+                    stats.rxObjects++;
+                } else {
+                    // TODO...
+                }
+                mutex.unlock();
+
+                if (useUDPMirror) {
+                    // it is safe to do this outside of the above critical section as the rxDataArray is
+                    // accessed from this thread only
+                    udpSocketTx->writeDatagram(rxDataArray, QHostAddress::LocalHost, udpSocketRx->localPort());
+                }
+            }
+        }
+    }
+}
+
 /**
  * Process an byte from the telemetry stream.
  * \param[in] rxbyte Received byte
@@ -230,10 +245,19 @@ bool UAVTalk::objectTransaction(UAVObject *obj, quint8 type, bool allInstances)
  */
 bool UAVTalk::processInputByte(quint8 rxbyte)
 {
+    if (rxState == STATE_COMPLETE || rxState == STATE_ERROR) {
+        rxState = STATE_SYNC;
+
+        if (useUDPMirror) {
+            rxDataArray.clear();
+        }
+    }
+
     // Update stats
     stats.rxBytes++;
 
-    rxPacketLength++; // update packet byte count
+    // update packet byte count
+    rxPacketLength++;
 
     if (useUDPMirror) {
         rxDataArray.append(rxbyte);
@@ -244,32 +268,31 @@ bool UAVTalk::processInputByte(quint8 rxbyte)
     case STATE_SYNC:
 
         if (rxbyte != SYNC_VAL) {
-            UAVTALK_QXTLOG_DEBUG("UAVTalk: Sync->Sync (" + QString::number(rxbyte) + " " + QString("0x%1").arg(rxbyte, 2, 16) + ")");
+            // continue until sync byte is matched
+            stats.rxSyncErrors++;
             break;
         }
 
         // Initialize and update CRC
-        rxCS = updateCRC(0, rxbyte);
+        rxCS = Crc::updateCRC(0, rxbyte);
 
         rxPacketLength = 1;
 
-        if (useUDPMirror) {
+        // case local byte counter, don't forget to zero it after use.
-            rxDataArray.clear();
+        rxCount = 0;
-            rxDataArray.append(rxbyte);
-        }
 
         rxState = STATE_TYPE;
-        UAVTALK_QXTLOG_DEBUG("UAVTalk: Sync->Type");
         break;
 
     case STATE_TYPE:
 
         // Update CRC
-        rxCS = updateCRC(rxCS, rxbyte);
+        rxCS = Crc::updateCRC(rxCS, rxbyte);
 
         if ((rxbyte & TYPE_MASK) != TYPE_VER) {
-            rxState = STATE_SYNC;
+            qWarning() << "UAVTalk - error : bad type";
-            UAVTALK_QXTLOG_DEBUG("UAVTalk: Type->Sync");
+            stats.rxErrors++;
+            rxState = STATE_ERROR;
             break;
         }
 
@@ -278,149 +301,124 @@ bool UAVTalk::processInputByte(quint8 rxbyte)
         packetSize = 0;
 
         rxState    = STATE_SIZE;
-        UAVTALK_QXTLOG_DEBUG("UAVTalk: Type->Size");
-        rxCount    = 0;
         break;
 
     case STATE_SIZE:
 
         // Update CRC
-        rxCS = updateCRC(rxCS, rxbyte);
+        rxCS = Crc::updateCRC(rxCS, rxbyte);
 
         if (rxCount == 0) {
             packetSize += rxbyte;
             rxCount++;
-            UAVTALK_QXTLOG_DEBUG("UAVTalk: Size->Size");
             break;
         }
-
         packetSize += (quint32)rxbyte << 8;
+        rxCount     = 0;
 
-        if (packetSize < MIN_HEADER_LENGTH || packetSize > MAX_HEADER_LENGTH + MAX_PAYLOAD_LENGTH) { // incorrect packet size
+
-            rxState = STATE_SYNC;
+        if (packetSize < HEADER_LENGTH || packetSize > HEADER_LENGTH + MAX_PAYLOAD_LENGTH) {
-            UAVTALK_QXTLOG_DEBUG("UAVTalk: Size->Sync");
+            // incorrect packet size
+            qWarning() << "UAVTalk - error : incorrect packet size";
+            stats.rxErrors++;
+            rxState = STATE_ERROR;
             break;
         }
 
-        rxCount = 0;
         rxState = STATE_OBJID;
-        UAVTALK_QXTLOG_DEBUG("UAVTalk: Size->ObjID");
         break;
 
     case STATE_OBJID:
 
         // Update CRC
-        rxCS = updateCRC(rxCS, rxbyte);
+        rxCS = Crc::updateCRC(rxCS, rxbyte);
 
         rxTmpBuffer[rxCount++] = rxbyte;
         if (rxCount < 4) {
-            UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->ObjID");
             break;
         }
+        rxCount  = 0;
+
+        rxObjId  = (qint32)qFromLittleEndian<quint32>(rxTmpBuffer);
+
+        // Message always contain an instance ID
+        rxInstId = 0;
+        rxState  = STATE_INSTID;
+        break;
+
+    case STATE_INSTID:
+
+        // Update CRC
+        rxCS = Crc::updateCRC(rxCS, rxbyte);
+
+        rxTmpBuffer[rxCount++] = rxbyte;
+        if (rxCount < 2) {
+            break;
+        }
+        rxCount  = 0;
+
+        rxInstId = (qint16)qFromLittleEndian<quint16>(rxTmpBuffer);
 
         // Search for object, if not found reset state machine
-        rxObjId = (qint32)qFromLittleEndian<quint32>(rxTmpBuffer);
         {
             UAVObject *rxObj = objMngr->getObject(rxObjId);
             if (rxObj == NULL && rxType != TYPE_OBJ_REQ) {
+                qWarning() << "UAVTalk - error : unknown object" << rxObjId;
                 stats.rxErrors++;
-                rxState = STATE_SYNC;
+                rxState = STATE_ERROR;
-                UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->Sync (badtype)");
                 break;
             }
 
             // Determine data length
             if (rxType == TYPE_OBJ_REQ || rxType == TYPE_ACK || rxType == TYPE_NACK) {
                 rxLength = 0;
-                rxInstanceLength = 0;
             } else {
-                rxLength = rxObj->getNumBytes();
+                if (rxObj) {
-                rxInstanceLength = (rxObj->isSingleInstance() ? 0 : 2);
+                    rxLength = rxObj->getNumBytes();
+                } else {
+                    rxLength = packetSize - rxPacketLength;
+                }
             }
 
             // Check length and determine next state
             if (rxLength >= MAX_PAYLOAD_LENGTH) {
+                // packet error - exceeded payload max length
+                qWarning() << "UAVTalk - error : exceeded payload max length" << rxObjId;
                 stats.rxErrors++;
-                rxState = STATE_SYNC;
+                rxState = STATE_ERROR;
-                UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->Sync (oversize)");
                 break;
             }
 
             // Check the lengths match
-            if ((rxPacketLength + rxInstanceLength + rxLength) != packetSize) { // packet error - mismatched packet size
+            if ((rxPacketLength + rxLength) != packetSize) {
+                // packet error - mismatched packet size
+                qWarning() << "UAVTalk - error : mismatched packet size" << rxObjId;
                 stats.rxErrors++;
-                rxState = STATE_SYNC;
+                rxState = STATE_ERROR;
-                UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->Sync (length mismatch)");
                 break;
             }
-
-            // Check if this is a single instance object (i.e. if the instance ID field is coming next)
-            if (rxObj == NULL) {
-                // This is a non-existing object, just skip to checksum
-                // and we'll send a NACK next.
-                rxState  = STATE_CS;
-                UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->CSum (no obj)");
-                rxInstId = 0;
-                rxCount  = 0;
-            } else if (rxObj->isSingleInstance()) {
-                // If there is a payload get it, otherwise receive checksum
-                if (rxLength > 0) {
-                    rxState = STATE_DATA;
-                    UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->Data (needs data)");
-                } else {
-                    rxState = STATE_CS;
-                    UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->Checksum");
-                }
-                rxInstId = 0;
-                rxCount  = 0;
-            } else {
-                rxState = STATE_INSTID;
-                UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->InstID");
-                rxCount = 0;
-            }
         }
 
-        break;
-
-    case STATE_INSTID:
-
-        // Update CRC
-        rxCS = updateCRC(rxCS, rxbyte);
-
-        rxTmpBuffer[rxCount++] = rxbyte;
-        if (rxCount < 2) {
-            UAVTALK_QXTLOG_DEBUG("UAVTalk: InstID->InstID");
-            break;
-        }
-
-        rxInstId = (qint16)qFromLittleEndian<quint16>(rxTmpBuffer);
-
-        rxCount  = 0;
-
         // If there is a payload get it, otherwise receive checksum
         if (rxLength > 0) {
             rxState = STATE_DATA;
-            UAVTALK_QXTLOG_DEBUG("UAVTalk: InstID->Data");
         } else {
             rxState = STATE_CS;
-            UAVTALK_QXTLOG_DEBUG("UAVTalk: InstID->CSum");
         }
         break;
 
     case STATE_DATA:
 
         // Update CRC
-        rxCS = updateCRC(rxCS, rxbyte);
+        rxCS = Crc::updateCRC(rxCS, rxbyte);
 
         rxBuffer[rxCount++] = rxbyte;
         if (rxCount < rxLength) {
-            UAVTALK_QXTLOG_DEBUG("UAVTalk: Data->Data");
             break;
         }
+        rxCount = 0;
 
         rxState = STATE_CS;
-        UAVTALK_QXTLOG_DEBUG("UAVTalk: Data->CSum");
-        rxCount = 0;
         break;
 
     case STATE_CS:
@@ -428,37 +426,30 @@ bool UAVTalk::processInputByte(quint8 rxbyte)
         // The CRC byte
         rxCSPacket = rxbyte;
 
-        if (rxCS != rxCSPacket) { // packet error - faulty CRC
+        if (rxCS != rxCSPacket) {
-            stats.rxErrors++;
+            // packet error - faulty CRC
-            rxState = STATE_SYNC;
+            qWarning() << "UAVTalk - error : failed CRC check" << rxObjId;
-            UAVTALK_QXTLOG_DEBUG("UAVTalk: CSum->Sync (badcrc)");
+            stats.rxCrcErrors++;
+            rxState = STATE_ERROR;
             break;
         }
 
-        if (rxPacketLength != packetSize + 1) { // packet error - mismatched packet size
+        if (rxPacketLength != packetSize + CHECKSUM_LENGTH) {
+            // packet error - mismatched packet size
+            qWarning() << "UAVTalk - error : mismatched packet size" << rxObjId;
             stats.rxErrors++;
-            rxState = STATE_SYNC;
+            rxState = STATE_ERROR;
-            UAVTALK_QXTLOG_DEBUG("UAVTalk: CSum->Sync (length mismatch)");
             break;
         }
 
-        mutex->lock();
+        rxState = STATE_COMPLETE;
-        receiveObject(rxType, rxObjId, rxInstId, rxBuffer, rxLength);
-        if (useUDPMirror) {
-            udpSocketTx->writeDatagram(rxDataArray, QHostAddress::LocalHost, udpSocketRx->localPort());
-        }
-        stats.rxObjectBytes += rxLength;
-        stats.rxObjects++;
-        mutex->unlock();
-
-        rxState = STATE_SYNC;
-        UAVTALK_QXTLOG_DEBUG("UAVTalk: CSum->Sync (OK)");
         break;
 
     default:
-        rxState = STATE_SYNC;
+
-        stats.rxErrors++;
+        qWarning() << "UAVTalk - error : bad state";
-        UAVTALK_QXTLOG_DEBUG("UAVTalk: \?\?\?->Sync"); // Use the escape character for '?' so that the tripgraph isn't triggered.
+        rxState = STATE_ERROR;
+        break;
     }
 
     // Done
@@ -467,6 +458,13 @@ bool UAVTalk::processInputByte(quint8 rxbyte)
 
 /**
  * Receive an object. This function process objects received through the telemetry stream.
+ *
+ * Parser errors are considered as transmission errors and are not NACKed.
+ * Some senders (GCS) can timeout and retry if the message is not answered by an ack or nack.
+ *
+ * Object handling errors are considered as application errors and are NACked.
+ * In that case we want to nack as there is no point in the sender retrying to send invalid objects.
+ *
  * \param[in] type Type of received message (TYPE_OBJ, TYPE_OBJ_REQ, TYPE_OBJ_ACK, TYPE_ACK, TYPE_NACK)
  * \param[in] obj Handle of the received object
  * \param[in] instId The instance ID of UAVOBJ_ALL_INSTANCES for all instances.
@@ -477,6 +475,7 @@ bool UAVTalk::processInputByte(quint8 rxbyte)
 bool UAVTalk::receiveObject(quint8 type, quint32 objId, quint16 instId, quint8 *data, qint32 length)
 {
     Q_UNUSED(length);
+
     UAVObject *obj    = NULL;
     bool error        = false;
     bool allInstances = (instId == ALL_INSTANCES);
@@ -488,9 +487,14 @@ bool UAVTalk::receiveObject(quint8 type, quint32 objId, quint16 instId, quint8 *
         if (!allInstances) {
             // Get object and update its data
             obj = updateObject(objId, instId, data);
-            // Check if an ack is pending
+#ifdef VERBOSE_UAVTALK
+            VERBOSE_FILTER(objId) qDebug() << "UAVTalk - received object" << objId << instId << (obj != NULL ? obj->toStringBrief() : "<null object>");
+#endif
             if (obj != NULL) {
-                updateAck(obj);
+                // Check if this object acks a pending OBJ_REQ message
+                // any OBJ message can ack a pending OBJ_REQ message
+                // even one that was not sent in response to the OBJ_REQ message
+                updateAck(type, objId, instId, obj);
             } else {
                 error = true;
             }
@@ -498,67 +502,94 @@ bool UAVTalk::receiveObject(quint8 type, quint32 objId, quint16 instId, quint8 *
             error = true;
         }
         break;
+
     case TYPE_OBJ_ACK:
         // All instances, not allowed for OBJ_ACK messages
         if (!allInstances) {
             // Get object and update its data
             obj = updateObject(objId, instId, data);
-            // Transmit ACK
+#ifdef VERBOSE_UAVTALK
+            VERBOSE_FILTER(objId) qDebug() << "UAVTalk - received object (acked)" << objId << instId << (obj != NULL ? obj->toStringBrief() : "<null object>");
+#endif
             if (obj != NULL) {
-                transmitObject(obj, TYPE_ACK, false);
+                // Object updated or created, transmit ACK
+                error = !transmitObject(TYPE_ACK, objId, instId, obj);
             } else {
                 error = true;
             }
         } else {
             error = true;
         }
+        if (error) {
+            // failed to update object, transmit NACK
+            transmitObject(TYPE_NACK, objId, instId, NULL);
+        }
         break;
+
     case TYPE_OBJ_REQ:
-        // Get object, if all instances are requested get instance 0 of the object
+        // Check if requested object exists
         if (allInstances) {
+            // All instances, so get instance zero
             obj = objMngr->getObject(objId);
         } else {
             obj = objMngr->getObject(objId, instId);
         }
-        // If object was found transmit it
+#ifdef VERBOSE_UAVTALK
+        VERBOSE_FILTER(objId) qDebug() << "UAVTalk - received object request" << objId << instId << (obj != NULL ? obj->toStringBrief() : "<null object>");
+#endif
         if (obj != NULL) {
-            transmitObject(obj, TYPE_OBJ, allInstances);
+            // Object found, transmit it
+            // The sent object will ack the object request on the receiver side
+            error = !transmitObject(TYPE_OBJ, objId, instId, obj);
         } else {
-            // Object was not found, transmit a NACK with the
-            // objId which was not found.
-            transmitNack(objId);
             error = true;
         }
-        break;
+        if (error) {
-    case TYPE_NACK:
+            // failed to send object, transmit NACK
-        // All instances, not allowed for NACK messages
+            transmitObject(TYPE_NACK, objId, instId, NULL);
-        if (!allInstances) {
-            // Get object
-            obj = objMngr->getObject(objId, instId);
-            // Check if object exists:
-            if (obj != NULL) {
-                updateNack(obj);
-            } else {
-                error = true;
-            }
         }
         break;
+
     case TYPE_ACK:
         // All instances, not allowed for ACK messages
         if (!allInstances) {
             // Get object
             obj = objMngr->getObject(objId, instId);
-            // Check if an ack is pending
+#ifdef VERBOSE_UAVTALK
+            VERBOSE_FILTER(objId) qDebug() << "UAVTalk - received ack" << objId << instId << (obj != NULL ? obj->toStringBrief() : "<null object>");
+#endif
             if (obj != NULL) {
-                updateAck(obj);
+                // Check if an ACK is pending
+                updateAck(type, objId, instId, obj);
             } else {
                 error = true;
             }
         }
         break;
+
+    case TYPE_NACK:
+        // All instances, not allowed for NACK messages
+        if (!allInstances) {
+            // Get object
+            obj = objMngr->getObject(objId, instId);
+#ifdef VERBOSE_UAVTALK
+            VERBOSE_FILTER(objId) qDebug() << "UAVTalk - received nack" << objId << instId << (obj != NULL ? obj->toStringBrief() : "<null object>");
+#endif
+            if (obj != NULL) {
+                // Check if a NACK is pending
+                updateNack(objId, instId, obj);
+            } else {
+                error = true;
+            }
+        }
+        break;
+
     default:
         error = true;
     }
+    if (error) {
+        qWarning() << "UAVTalk - !!! error receiving" << typeToString(type) << objId << instId << (obj != NULL ? obj->toStringBrief() : "<null object>");
+    }
     // Done
     return !error;
 }
@@ -576,22 +607,24 @@ UAVObject *UAVTalk::updateObject(quint32 objId, quint16 instId, quint8 *data)
     // If the instance does not exist create it
     if (obj == NULL) {
         // Get the object type
-        UAVObject *tobj = objMngr->getObject(objId);
+        UAVObject *typeObj = objMngr->getObject(objId);
-        if (tobj == NULL) {
+        if (typeObj == NULL) {
+            qWarning() << "UAVTalk - failed to get object, object ID :" << objId;
             return NULL;
         }
         // Make sure this is a data object
-        UAVDataObject *dobj = dynamic_cast<UAVDataObject *>(tobj);
+        UAVDataObject *dataObj = dynamic_cast<UAVDataObject *>(typeObj);
-        if (dobj == NULL) {
+        if (dataObj == NULL) {
             return NULL;
         }
         // Create a new instance, unpack and register
-        UAVDataObject *instobj = dobj->clone(instId);
+        UAVDataObject *instObj = dataObj->clone(instId);
-        if (!objMngr->registerObject(instobj)) {
+        if (!objMngr->registerObject(instObj)) {
+            qWarning() << "UAVTalk - failed to register object " << instObj->toStringBrief();
             return NULL;
         }
-        instobj->unpack(data);
+        instObj->unpack(data);
-        return instobj;
+        return instObj;
     } else {
         // Unpack data into object instance
         obj->unpack(data);
@@ -602,151 +635,129 @@ UAVObject *UAVTalk::updateObject(quint32 objId, quint16 instId, quint8 *data)
 /**
  * Check if a transaction is pending and if yes complete it.
  */
-void UAVTalk::updateNack(UAVObject *obj)
+void UAVTalk::updateAck(quint8 type, quint32 objId, quint16 instId, UAVObject *obj)
 {
     Q_ASSERT(obj);
     if (!obj) {
         return;
     }
-    quint32 objId = obj->getObjID();
+    Transaction *trans = findTransaction(objId, instId);
-    QMap<quint32, Transaction *>::iterator itr = transMap.find(objId);
+    if (trans && trans->respType == type) {
-    if (itr != transMap.end() && (itr.value()->obj->getInstID() == obj->getInstID() || itr.value()->allInstances)) {
+        if (trans->respInstId == ALL_INSTANCES) {
-        transMap.remove(objId);
+            if (instId == 0) {
-        delete itr.value();
+                // last instance received, complete transaction
-        emit transactionCompleted(obj, false);
+                closeTransaction(trans);
+                emit transactionCompleted(obj, true);
+            } else {
+                // TODO extend timeout?
+            }
+        } else {
+            closeTransaction(trans);
+            emit transactionCompleted(obj, true);
+        }
     }
 }
 
-
 /**
  * Check if a transaction is pending and if yes complete it.
  */
-void UAVTalk::updateAck(UAVObject *obj)
+void UAVTalk::updateNack(quint32 objId, quint16 instId, UAVObject *obj)
 {
-    quint32 objId = obj->getObjID();
+    Q_ASSERT(obj);
-
+    if (!obj) {
-    QMap<quint32, Transaction *>::iterator itr = transMap.find(objId);
+        return;
-    if (itr != transMap.end() && (itr.value()->obj->getInstID() == obj->getInstID() || itr.value()->allInstances)) {
+    }
-        transMap.remove(objId);
+    Transaction *trans = findTransaction(objId, instId);
-        delete itr.value();
+    if (trans) {
-        emit transactionCompleted(obj, true);
+        closeTransaction(trans);
+        emit transactionCompleted(obj, false);
     }
 }
 
-
 /**
  * Send an object through the telemetry link.
- * \param[in] obj Object to send
  * \param[in] type Transaction type
- * \param[in] allInstances True is all instances of the object are to be sent
+ * \param[in] objId Object ID to send
+ * \param[in] instId Instance ID to send
+ * \param[in] obj Object to send (null when type is NACK)
  * \return Success (true), Failure (false)
  */
-bool UAVTalk::transmitObject(UAVObject *obj, quint8 type, bool allInstances)
+bool UAVTalk::transmitObject(quint8 type, quint32 objId, quint16 instId, UAVObject *obj)
 {
+    // Important note : obj can be null (when type is NACK for example) so protect all obj dereferences.
+
     // If all instances are requested on a single instance object it is an error
-    if (allInstances && obj->isSingleInstance()) {
+    if ((obj != NULL) && (instId == ALL_INSTANCES) && obj->isSingleInstance()) {
-        allInstances = false;
+        instId = 0;
     }
+    bool allInstances = (instId == ALL_INSTANCES);
+
+#ifdef VERBOSE_UAVTALK
+    VERBOSE_FILTER(objId) qDebug() << "UAVTalk - transmitting" << typeToString(type) << objId << instId << (obj != NULL ? obj->toStringBrief() : "<null object>");
+#endif
 
     // Process message type
+    bool ret = false;
     if (type == TYPE_OBJ || type == TYPE_OBJ_ACK) {
         if (allInstances) {
-            // Get number of instances
+            // Send all instances in reverse order
-            quint32 numInst = objMngr->getNumInstances(obj->getObjID());
+            // This allows the receiver to detect when the last object has been received (i.e. when instance 0 is received)
-            // Send all instances
+            ret = true;
-            for (quint32 instId = 0; instId < numInst; ++instId) {
+            quint32 numInst = objMngr->getNumInstances(objId);
-                UAVObject *inst = objMngr->getObject(obj->getObjID(), instId);
+            for (quint32 n = 0; n < numInst; ++n) {
-                transmitSingleObject(inst, type, false);
+                quint32 i    = numInst - n - 1;
+                UAVObject *o = objMngr->getObject(objId, i);
+                if (!transmitSingleObject(type, objId, i, o)) {
+                    ret = false;
+                    break;
+                }
             }
-            return true;
         } else {
-            return transmitSingleObject(obj, type, false);
+            ret = transmitSingleObject(type, objId, instId, obj);
         }
     } else if (type == TYPE_OBJ_REQ) {
-        return transmitSingleObject(obj, TYPE_OBJ_REQ, allInstances);
+        ret = transmitSingleObject(TYPE_OBJ_REQ, objId, instId, NULL);
-    } else if (type == TYPE_ACK) {
+    } else if (type == TYPE_ACK || type == TYPE_NACK) {
         if (!allInstances) {
-            return transmitSingleObject(obj, TYPE_ACK, false);
+            ret = transmitSingleObject(type, objId, instId, NULL);
-        } else {
-            return false;
         }
-    } else {
-        return false;
-    }
-}
-
-/**
- * Transmit a NACK through the telemetry link.
- * \param[in] objId the ObjectID we rejected
- */
-bool UAVTalk::transmitNack(quint32 objId)
-{
-    int dataOffset = 8;
-
-    txBuffer[0] = SYNC_VAL;
-    txBuffer[1] = TYPE_NACK;
-    qToLittleEndian<quint32>(objId, &txBuffer[4]);
-
-    // Calculate checksum
-    txBuffer[dataOffset] = updateCRC(0, txBuffer, dataOffset);
-
-    qToLittleEndian<quint16>(dataOffset, &txBuffer[2]);
-
-    // Send buffer, check that the transmit backlog does not grow above limit
-    if (io && io->isWritable() && io->bytesToWrite() < TX_BUFFER_SIZE) {
-        io->write((const char *)txBuffer, dataOffset + CHECKSUM_LENGTH);
-        if (useUDPMirror) {
-            udpSocketRx->writeDatagram((const char *)txBuffer, dataOffset + CHECKSUM_LENGTH, QHostAddress::LocalHost, udpSocketTx->localPort());
-        }
-    } else {
-        ++stats.txErrors;
-        return false;
     }
 
-    // Update stats
+#ifdef VERBOSE_UAVTALK
-    stats.txBytes += 8 + CHECKSUM_LENGTH;
+    if (!ret) {
+        VERBOSE_FILTER(objId) qDebug() << "UAVTalk - failed to transmit" << typeToString(type) << objId << instId << (obj != NULL ? obj->toStringBrief() : "<null object>");
+    }
+#endif
 
-    // Done
+    return ret;
-    return true;
 }
 
-
 /**
  * Send an object through the telemetry link.
- * \param[in] obj Object handle to send
  * \param[in] type Transaction type
+ * \param[in] objId Object ID to send
+ * \param[in] instId Instance ID to send
+ * \param[in] obj Object to send (null when type is NACK)
  * \return Success (true), Failure (false)
  */
-bool UAVTalk::transmitSingleObject(UAVObject *obj, quint8 type, bool allInstances)
+bool UAVTalk::transmitSingleObject(quint8 type, quint32 objId, quint16 instId, UAVObject *obj)
 {
     qint32 length;
-    qint32 dataOffset;
-    quint32 objId;
-    quint16 instId;
-    quint16 allInstId = ALL_INSTANCES;
 
-    // Setup type and object id fields
+    // IMPORTANT : obj can be null (when type is NACK for example)
-    objId = obj->getObjID();
+
+    // Setup sync byte
     txBuffer[0] = SYNC_VAL;
+    // Setup type
     txBuffer[1] = type;
+    // next 2 bytes are reserved for data length (inserted here later)
+    // Setup object ID
     qToLittleEndian<quint32>(objId, &txBuffer[4]);
-
+    // Setup instance ID
-    // Setup instance ID if one is required
+    qToLittleEndian<quint16>(instId, &txBuffer[8]);
-    if (obj->isSingleInstance()) {
-        dataOffset = 8;
-    } else {
-        // Check if all instances are requested
-        if (allInstances) {
-            qToLittleEndian<quint16>(allInstId, &txBuffer[8]);
-        } else {
-            instId = obj->getInstID();
-            qToLittleEndian<quint16>(instId, &txBuffer[8]);
-        }
-        dataOffset = 10;
-    }
 
     // Determine data length
-    if (type == TYPE_OBJ_REQ || type == TYPE_ACK) {
+    if (type == TYPE_OBJ_REQ || type == TYPE_ACK || type == TYPE_NACK) {
         length = 0;
     } else {
         length = obj->getNumBytes();
@@ -754,67 +765,138 @@ bool UAVTalk::transmitSingleObject(UAVObject *obj, quint8 type, bool allInstance
 
     // Check length
     if (length >= MAX_PAYLOAD_LENGTH) {
+        qWarning() << "UAVTalk - error transmitting : object exceeds max payload length" << obj->toStringBrief();
+        ++stats.txErrors;
         return false;
     }
 
     // Copy data (if any)
     if (length > 0) {
-        if (!obj->pack(&txBuffer[dataOffset])) {
+        if (!obj->pack(&txBuffer[HEADER_LENGTH])) {
+            qWarning() << "UAVTalk - error transmitting : failed to pack object" << obj->toStringBrief();
+            ++stats.txErrors;
             return false;
         }
     }
 
-    qToLittleEndian<quint16>(dataOffset + length, &txBuffer[2]);
+    // Store the packet length
+    qToLittleEndian<quint16>(HEADER_LENGTH + length, &txBuffer[2]);
 
     // Calculate checksum
-    txBuffer[dataOffset + length] = updateCRC(0, txBuffer, dataOffset + length);
+    txBuffer[HEADER_LENGTH + length] = Crc::updateCRC(0, txBuffer, HEADER_LENGTH + length);
 
     // Send buffer, check that the transmit backlog does not grow above limit
-    if (!io.isNull() && io->isWritable() && io->bytesToWrite() < TX_BUFFER_SIZE) {
+    if (!io.isNull() && io->isWritable()) {
-        io->write((const char *)txBuffer, dataOffset + length + CHECKSUM_LENGTH);
+        if (io->bytesToWrite() < TX_BUFFER_SIZE) {
-        if (useUDPMirror) {
+            io->write((const char *)txBuffer, HEADER_LENGTH + length + CHECKSUM_LENGTH);
-            udpSocketRx->writeDatagram((const char *)txBuffer, dataOffset + length + CHECKSUM_LENGTH, QHostAddress::LocalHost, udpSocketTx->localPort());
+            if (useUDPMirror) {
+                udpSocketRx->writeDatagram((const char *)txBuffer, HEADER_LENGTH + length + CHECKSUM_LENGTH, QHostAddress::LocalHost, udpSocketTx->localPort());
+            }
+        } else {
+            qWarning() << "UAVTalk - error transmitting : io device full";
+            ++stats.txErrors;
+            return false;
         }
     } else {
+        qWarning() << "UAVTalk - error transmitting : io device not writable";
         ++stats.txErrors;
         return false;
     }
 
     // Update stats
     ++stats.txObjects;
-    stats.txBytes += dataOffset + length + CHECKSUM_LENGTH;
     stats.txObjectBytes += length;
+    stats.txBytes += HEADER_LENGTH + length + CHECKSUM_LENGTH;
 
     // Done
     return true;
 }
 
-/**
+UAVTalk::Transaction *UAVTalk::findTransaction(quint32 objId, quint16 instId)
- * Update the crc value with new data.
- *
- * Generated by pycrc v0.7.5, http://www.tty1.net/pycrc/
- * using the configuration:
- *    Width        = 8
- *    Poly         = 0x07
- *    XorIn        = 0x00
- *    ReflectIn    = False
- *    XorOut       = 0x00
- *    ReflectOut   = False
- *    Algorithm    = table-driven
- *
- * \param crc      The current crc value.
- * \param data     Pointer to a buffer of \a data_len bytes.
- * \param length   Number of bytes in the \a data buffer.
- * \return         The updated crc value.
- */
-quint8 UAVTalk::updateCRC(quint8 crc, const quint8 data)
 {
-    return crc_table[crc ^ data];
+    // Lookup the transaction in the transaction map
-}
+    QMap<quint32, Transaction *> *objTransactions = transMap.value(objId);
-quint8 UAVTalk::updateCRC(quint8 crc, const quint8 *data, qint32 length)
+    if (objTransactions != NULL) {
-{
+        Transaction *trans = objTransactions->value(instId);
-    while (length--) {
+        if (trans == NULL) {
-        crc = crc_table[crc ^ *data++];
+            // see if there is an ALL_INSTANCES transaction
+            trans = objTransactions->value(ALL_INSTANCES);
+        }
+        return trans;
     }
-    return crc;
+    return NULL;
+}
+
+void UAVTalk::openTransaction(quint8 type, quint32 objId, quint16 instId)
+{
+    Transaction *trans = new Transaction();
+
+    trans->respType   = (type == TYPE_OBJ_REQ) ? TYPE_OBJ : TYPE_ACK;
+    trans->respObjId  = objId;
+    trans->respInstId = instId;
+
+    QMap<quint32, Transaction *> *objTransactions = transMap.value(trans->respObjId);
+    if (objTransactions == NULL) {
+        objTransactions = new QMap<quint32, Transaction *>();
+        transMap.insert(trans->respObjId, objTransactions);
+    }
+    objTransactions->insert(trans->respInstId, trans);
+}
+
+void UAVTalk::closeTransaction(Transaction *trans)
+{
+    QMap<quint32, Transaction *> *objTransactions = transMap.value(trans->respObjId);
+    if (objTransactions != NULL) {
+        objTransactions->remove(trans->respInstId);
+        // Keep the map even if it is empty
+        // There are at most 100 different object IDs...
+        delete trans;
+    }
+}
+
+void UAVTalk::closeAllTransactions()
+{
+    foreach(quint32 objId, transMap.keys()) {
+        QMap<quint32, Transaction *> *objTransactions = transMap.value(objId);
+        foreach(quint32 instId, objTransactions->keys()) {
+            Transaction *trans = objTransactions->value(instId);
+
+            qWarning() << "UAVTalk - closing active transaction for object" << trans->respObjId;
+            objTransactions->remove(instId);
+            delete trans;
+        }
+        transMap.remove(objId);
+        delete objTransactions;
+    }
+}
+
+const char *UAVTalk::typeToString(quint8 type)
+{
+    switch (type) {
+    case TYPE_OBJ:
+        return "object";
+
+        break;
+
+    case TYPE_OBJ_ACK:
+        return "object (acked)";
+
+        break;
+
+    case TYPE_OBJ_REQ:
+        return "object request";
+
+        break;
+
+    case TYPE_ACK:
+        return "ack";
+
+        break;
+
+    case TYPE_NACK:
+        return "nack";
+
+        break;
+    }
+    return "<error>";
 }

ground/openpilotgcs/src/plugins/uavtalk/uavtalk.h

@@ -27,101 +27,120 @@
 #ifndef UAVTALK_H
 #define UAVTALK_H
 
+#include "uavobjectmanager.h"
+#include "uavtalk_global.h"
+
 #include <QtCore>
 #include <QIODevice>
 #include <QMutex>
 #include <QMutexLocker>
 #include <QMap>
-#include <QSemaphore>
+#include <QThread>
-#include "uavobjectmanager.h"
-#include "uavtalk_global.h"
 #include <QtNetwork/QUdpSocket>
 
 class UAVTALK_EXPORT UAVTalk : public QObject {
     Q_OBJECT
 
+    friend class IODeviceReader;
+
 public:
+    static const quint16 ALL_INSTANCES = 0xFFFF;
+
     typedef struct {
         quint32 txBytes;
-        quint32 rxBytes;
         quint32 txObjectBytes;
-        quint32 rxObjectBytes;
-        quint32 rxObjects;
         quint32 txObjects;
         quint32 txErrors;
+
+        quint32 rxBytes;
+        quint32 rxObjectBytes;
+        quint32 rxObjects;
         quint32 rxErrors;
+        quint32 rxSyncErrors;
+        quint32 rxCrcErrors;
     } ComStats;
 
     UAVTalk(QIODevice *iodev, UAVObjectManager *objMngr);
     ~UAVTalk();
+
+    ComStats getStats();
+    void resetStats();
+
     bool sendObject(UAVObject *obj, bool acked, bool allInstances);
     bool sendObjectRequest(UAVObject *obj, bool allInstances);
     void cancelTransaction(UAVObject *obj);
-    ComStats getStats();
-    void resetStats();
 
 signals:
     void transactionCompleted(UAVObject *obj, bool success);
 
 private slots:
-    void processInputStream(void);
+    void processInputStream();
     void dummyUDPRead();
 
 private:
 
     typedef struct {
-        UAVObject *obj;
+        quint8  respType;
-        bool allInstances;
+        quint32 respObjId;
+        quint16 respInstId;
     } Transaction;
 
     // Constants
-    static const int TYPE_MASK    = 0xF8;
+    static const int TYPE_MASK     = 0xF8;
-    static const int TYPE_VER     = 0x20;
+    static const int TYPE_VER      = 0x20;
-    static const int TYPE_OBJ     = (TYPE_VER | 0x00);
+    static const int TYPE_OBJ      = (TYPE_VER | 0x00);
-    static const int TYPE_OBJ_REQ = (TYPE_VER | 0x01);
+    static const int TYPE_OBJ_REQ  = (TYPE_VER | 0x01);
-    static const int TYPE_OBJ_ACK = (TYPE_VER | 0x02);
+    static const int TYPE_OBJ_ACK  = (TYPE_VER | 0x02);
-    static const int TYPE_ACK     = (TYPE_VER | 0x03);
+    static const int TYPE_ACK      = (TYPE_VER | 0x03);
-    static const int TYPE_NACK    = (TYPE_VER | 0x04);
+    static const int TYPE_NACK     = (TYPE_VER | 0x04);
 
-    static const int MIN_HEADER_LENGTH  = 8; // sync(1), type (1), size(2), object ID(4)
+    // header : sync(1), type (1), size(2), object ID(4), instance ID(2)
-    static const int MAX_HEADER_LENGTH  = 10; // sync(1), type (1), size(2), object ID (4), instance ID(2, not used in single objects)
+    static const int HEADER_LENGTH = 10;
-
-    static const int CHECKSUM_LENGTH    = 1;
 
     static const int MAX_PAYLOAD_LENGTH = 256;
 
-    static const int MAX_PACKET_LENGTH  = (MAX_HEADER_LENGTH + MAX_PAYLOAD_LENGTH + CHECKSUM_LENGTH);
+    static const int CHECKSUM_LENGTH    = 1;
 
-    static const quint16 ALL_INSTANCES  = 0xFFFF;
+    static const int MAX_PACKET_LENGTH  = (HEADER_LENGTH + MAX_PAYLOAD_LENGTH + CHECKSUM_LENGTH);
-    static const quint16 OBJID_NOTFOUND = 0x0000;
 
     static const int TX_BUFFER_SIZE     = 2 * 1024;
+
     static const quint8 crc_table[256];
 
     // Types
-    typedef enum { STATE_SYNC, STATE_TYPE, STATE_SIZE, STATE_OBJID, STATE_INSTID, STATE_DATA, STATE_CS } RxStateType;
+    typedef enum {
+        STATE_SYNC, STATE_TYPE, STATE_SIZE, STATE_OBJID, STATE_INSTID, STATE_DATA, STATE_CS, STATE_COMPLETE, STATE_ERROR
+    } RxStateType;
 
     // Variables
     QPointer<QIODevice> io;
+
     UAVObjectManager *objMngr;
-    QMutex *mutex;
+
-    QMap<quint32, Transaction *> transMap;
+    ComStats stats;
+
+    QMutex mutex;
+
+    QMap<quint32, QMap<quint32, Transaction *> *> transMap;
+
     quint8 rxBuffer[MAX_PACKET_LENGTH];
+
     quint8 txBuffer[MAX_PACKET_LENGTH];
+
     // Variables used by the receive state machine
+    // state machine variables
+    qint32 rxCount;
+    qint32 packetSize;
+    RxStateType rxState;
+    // data variables
     quint8 rxTmpBuffer[4];
     quint8 rxType;
     quint32 rxObjId;
     quint16 rxInstId;
     quint16 rxLength;
     quint16 rxPacketLength;
-    quint8 rxInstanceLength;
+    quint8 rxCSPacket;
-
+    quint8 rxCS;
-    quint8 rxCSPacket, rxCS;
-    qint32 rxCount;
-    qint32 packetSize;
-    RxStateType rxState;
-    ComStats stats;
 
     bool useUDPMirror;
     QUdpSocket *udpSocketTx;
@@ -129,17 +148,21 @@ private:
     QByteArray rxDataArray;
 
     // Methods
-    bool objectTransaction(UAVObject *obj, quint8 type, bool allInstances);
+    bool objectTransaction(quint8 type, quint32 objId, quint16 instId, UAVObject *obj);
     bool processInputByte(quint8 rxbyte);
     bool receiveObject(quint8 type, quint32 objId, quint16 instId, quint8 *data, qint32 length);
     UAVObject *updateObject(quint32 objId, quint16 instId, quint8 *data);
-    void updateAck(UAVObject *obj);
+    void updateAck(quint8 type, quint32 objId, quint16 instId, UAVObject *obj);
-    void updateNack(UAVObject *obj);
+    void updateNack(quint32 objId, quint16 instId, UAVObject *obj);
-    bool transmitNack(quint32 objId);
+    bool transmitObject(quint8 type, quint32 objId, quint16 instId, UAVObject *obj);
-    bool transmitObject(UAVObject *obj, quint8 type, bool allInstances);
+    bool transmitSingleObject(quint8 type, quint32 objId, quint16 instId, UAVObject *obj);
-    bool transmitSingleObject(UAVObject *obj, quint8 type, bool allInstances);
+
-    quint8 updateCRC(quint8 crc, const quint8 data);
+    Transaction *findTransaction(quint32 objId, quint16 instId);
-    quint8 updateCRC(quint8 crc, const quint8 *data, qint32 length);
+    void openTransaction(quint8 type, quint32 objId, quint16 instId);
+    void closeTransaction(Transaction *trans);
+    void closeAllTransactions();
+
+    const char *typeToString(quint8 type);
 };
 
 #endif // UAVTALK_H

ground/openpilotgcs/src/plugins/uavtalk/uavtalk.pro

@@ -3,22 +3,27 @@ TARGET = UAVTalk
 
 QT += network
 
+DEFINES += UAVTALK_LIBRARY
+
+#DEFINES += VERBOSE_TELEMETRY
+#DEFINES += VERBOSE_UAVTALK
+
 include(../../openpilotgcsplugin.pri)
 include(uavtalk_dependencies.pri)
 
-HEADERS += uavtalk.h \
+HEADERS += \
+    uavtalk.h \
     uavtalkplugin.h \
     telemetrymonitor.h \
     telemetrymanager.h \
     uavtalk_global.h \
     telemetry.h
 
-SOURCES += uavtalk.cpp \
+SOURCES += \
+    uavtalk.cpp \
     uavtalkplugin.cpp \
     telemetrymonitor.cpp \
     telemetrymanager.cpp \
     telemetry.cpp
 
-DEFINES += UAVTALK_LIBRARY
-
 OTHER_FILES += UAVTalk.pluginspec

ground/openpilotgcs/src/plugins/uploader/runningdevicewidget.h

@@ -30,17 +30,17 @@
 
 #include "ui_runningdevicewidget.h"
 
-#include <QWidget>
-#include <QErrorMessage>
-#include <QtSvg/QGraphicsSvgItem>
-#include <QtSvg/QSvgRenderer>
-#include "uavtalk/telemetrymanager.h"
 #include "extensionsystem/pluginmanager.h"
 #include "uavobjectmanager.h"
 #include "uavobject.h"
 #include "uavobjectutilmanager.h"
 #include "uploader_global.h"
 
+#include <QWidget>
+#include <QErrorMessage>
+#include <QtSvg/QGraphicsSvgItem>
+#include <QtSvg/QSvgRenderer>
+
 class UPLOADER_EXPORT RunningDeviceWidget : public QWidget {
     Q_OBJECT
 public:

ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp

@@ -26,10 +26,13 @@
  */
 #include "uploadergadgetwidget.h"
 #include "version_info/version_info.h"
-#include <coreplugin/coreconstants.h>
-#include <QDebug>
 #include "flightstatus.h"
 
+#include <coreplugin/coreconstants.h>
+#include <uavtalk/telemetrymanager.h>
+
+#include <QDebug>
+
 #define DFU_DEBUG true
 
 const int UploaderGadgetWidget::AUTOUPDATE_CLOSE_TIMEOUT = 7000;

ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.h

@@ -36,7 +36,6 @@
 #include <QtSerialPort/QSerialPort>
 #include <QtSerialPort/QSerialPortInfo>
 
-#include "uavtalk/telemetrymanager.h"
 #include "extensionsystem/pluginmanager.h"
 #include "uavobjectmanager.h"
 #include "uavobject.h"

shared/uavobjectdefinition/altholdsmoothed.xml

@@ -1,12 +0,0 @@
-<xml>
-    <object name="AltHoldSmoothed" singleinstance="true" settings="false" category="State">
-        <description>The output on the kalman filter on altitude.</description>
-        <field name="Altitude" units="m" type="float" elements="1"/>
-	<field name="Velocity" units="m/s" type="float" elements="1"/>
-	<field name="Accel" units="m/s^2" type="float" elements="1"/>
-        <access gcs="readwrite" flight="readwrite"/>
-        <telemetrygcs acked="false" updatemode="manual" period="0"/>
-        <telemetryflight acked="false" updatemode="periodic" period="1000"/>
-        <logging updatemode="manual" period="0"/>
-    </object>
-</xml>

shared/uavobjectdefinition/altitudefiltersettings.xml

@@ -1,9 +1,9 @@
 <xml>
     <object name="AltitudeFilterSettings" singleinstance="true" settings="true" category="State">
         <description>Settings for the @ref State Estimator module plugin altitudeFilter</description>
-        <field name="AccelLowPassKp" units="m/s^2" type="float" elements="1" defaultvalue="0.07"/>
+        <field name="AccelLowPassKp" units="m/s^2" type="float" elements="1" defaultvalue="0.04"/>
         <field name="AccelDriftKi" units="m/s^2" type="float" elements="1" defaultvalue="0.0005"/>
-        <field name="BaroKp" units="m" type="float" elements="1" defaultvalue="0.02"/>
+        <field name="BaroKp" units="m" type="float" elements="1" defaultvalue="0.04"/>
         <access gcs="readwrite" flight="readwrite"/>
         <telemetrygcs acked="true" updatemode="onchange" period="0"/>
         <telemetryflight acked="true" updatemode="onchange" period="0"/>

shared/uavobjectdefinition/altitudeholddesired.xml

@@ -1,13 +1,14 @@
 <xml>
     <object name="AltitudeHoldDesired" singleinstance="true" settings="false" category="Control">
         <description>Holds the desired altitude (from manual control) as well as the desired attitude to pass through</description>
-        <field name="Altitude" units="m" type="float" elements="1"/>
+        <field name="SetPoint" units="" type="float" elements="1"/>
-        <field name="Roll" units="deg" type="float" elements="1"/>
+        <field name="ControlMode" units="" type="enum" elements="1" options="Altitude,Velocity,Throttle" />
+	<field name="Roll" units="deg" type="float" elements="1"/>
         <field name="Pitch" units="deg" type="float" elements="1"/>
         <field name="Yaw" units="deg/s" type="float" elements="1"/>
         <access gcs="readwrite" flight="readwrite"/>
         <telemetrygcs acked="false" updatemode="manual" period="0"/>
-        <telemetryflight acked="false" updatemode="onchange" period="0"/>
+        <telemetryflight acked="false" updatemode="periodic" period="1000"/>
         <logging updatemode="manual" period="0"/>
     </object>
 </xml>

shared/uavobjectdefinition/altitudeholdsettings.xml

@@ -1,15 +1,11 @@
 <xml>
     <object name="AltitudeHoldSettings" singleinstance="true" settings="true" category="Control">
         <description>Settings for the @ref AltitudeHold module</description>
-	<field name="Kp" units="throttle/m" type="float" elements="1" defaultvalue="0.0001"/>
+	<field name="AltitudePI" units="(m/s)/m" type="float" elementnames="Kp,Ki,Ilimit" defaultvalue="0.8,0,0" />
-	<field name="Ki" units="throttle/m" type="float" elements="1" defaultvalue="0"/>
+	<field name="VelocityPI" units="(m/s^2)/(m/s)" type="float" elementnames="Kp,Ki,Ilimit" defaultvalue="0.2,0.0002,2.0" />
-	<field name="Kd" units="throttle/m" type="float" elements="1" defaultvalue="0.05"/>
+	<field name="CutThrottleWhenZero" units="bool" type="enum" elements="1" options="False,True" defaultvalue="True" />
-	<field name="Ka" units="throttle/(m/s^2)" type="float" elements="1" defaultvalue="0.005"/>
+        <field name="ThrottleExp" units="" type="uint8" elements="1" defaultvalue="128" />
-	<field name="PressureNoise" units="m" type="float" elements="1" defaultvalue="0.4"/>
+        <field name="ThrottleRate" units="m/s" type="float" elements="1" defaultvalue="5" />
-	<field name="AccelNoise" units="m/s^2" type="float" elements="1" defaultvalue="5"/>
-	<field name="AccelDrift" units="m/s^2" type="float" elements="1" defaultvalue="0.001"/>
-	<field name="ThrottleExp" units="" type="uint8" elements="1" defaultvalue="128"/>
-	<field name="ThrottleRate" units="m/s" type="uint8" elements="1" defaultvalue="5"/>
         <access gcs="readwrite" flight="readwrite"/>
         <telemetrygcs acked="true" updatemode="onchange" period="0"/>
         <telemetryflight acked="true" updatemode="onchange" period="0"/>

shared/uavobjectdefinition/altitudeholdstatus.xml

@@ -0,0 +1,10 @@
+<xml>
+    <object name="AltitudeHoldStatus" singleinstance="true" settings="false" category="Control">
+        <description>Status Data from Altitude Hold Control Loops</description>
+        <field name="VelocityDesired" units="m/s" type="float" elements="1"/>
+        <access gcs="readwrite" flight="readwrite"/>
+        <telemetrygcs acked="false" updatemode="manual" period="0"/>
+        <telemetryflight acked="false" updatemode="periodic" period="1000"/>
+        <logging updatemode="manual" period="0"/>
+    </object>
+</xml>

shared/uavobjectdefinition/flighttelemetrystats.xml

@@ -1,12 +1,20 @@
 <xml>
     <object name="FlightTelemetryStats" singleinstance="true" settings="false" category="System">
         <description>Maintains the telemetry statistics from the OpenPilot flight computer.</description>
+        
         <field name="Status" units="" type="enum" elements="1" options="Disconnected,HandshakeReq,HandshakeAck,Connected"/>
+        
         <field name="TxDataRate" units="bytes/sec" type="float" elements="1"/>
-        <field name="RxDataRate" units="bytes/sec" type="float" elements="1"/>
+        <field name="TxBytes" units="bytes" type="uint32" elements="1"/>
         <field name="TxFailures" units="count" type="uint32" elements="1"/>
-        <field name="RxFailures" units="count" type="uint32" elements="1"/>
         <field name="TxRetries" units="count" type="uint32" elements="1"/>
+        
+        <field name="RxDataRate" units="bytes/sec" type="float" elements="1"/>
+        <field name="RxBytes" units="bytes" type="uint32" elements="1"/>
+        <field name="RxFailures" units="count" type="uint32" elements="1"/>
+        <field name="RxSyncErrors" units="count" type="uint32" elements="1"/>
+        <field name="RxCrcErrors" units="count" type="uint32" elements="1"/>
+        
         <access gcs="readwrite" flight="readwrite"/>
         <telemetrygcs acked="false" updatemode="manual" period="0"/>
         <telemetryflight acked="false" updatemode="periodic" period="5000"/>

shared/uavobjectdefinition/gcstelemetrystats.xml

@@ -1,12 +1,18 @@
 <xml>
     <object name="GCSTelemetryStats" singleinstance="true" settings="false" category="System">
         <description>The telemetry statistics from the ground computer</description>
+        
         <field name="Status" units="" type="enum" elements="1" options="Disconnected,HandshakeReq,HandshakeAck,Connected"/>
         <field name="TxDataRate" units="bytes/sec" type="float" elements="1"/>
-        <field name="RxDataRate" units="bytes/sec" type="float" elements="1"/>
+        <field name="TxBytes" units="bytes" type="uint32" elements="1"/>
         <field name="TxFailures" units="count" type="uint32" elements="1"/>
-        <field name="RxFailures" units="count" type="uint32" elements="1"/>
         <field name="TxRetries" units="count" type="uint32" elements="1"/>
+        <field name="RxDataRate" units="bytes/sec" type="float" elements="1"/>
+        <field name="RxBytes" units="bytes" type="uint32" elements="1"/>
+        <field name="RxFailures" units="count" type="uint32" elements="1"/>
+        <field name="RxSyncErrors" units="count" type="uint32" elements="1"/>
+        <field name="RxCrcErrors" units="count" type="uint32" elements="1"/>
+        
         <access gcs="readwrite" flight="readwrite"/>
         <telemetrygcs acked="false" updatemode="periodic" period="5000"/>
         <telemetryflight acked="false" updatemode="manual" period="0"/>

shared/uavobjectdefinition/oplinkreceiver.xml

@@ -1,5 +1,5 @@
 <xml>
-    <object name="OPLinkReceiver" singleinstance="true" settings="false">
+    <object name="OPLinkReceiver" singleinstance="true" settings="false" category="System">
         <description>A receiver channel group carried over the OPLink radio.</description>
         <field name="Channel" units="us" type="int16" elements="8"/>
         <access gcs="readonly" flight="readwrite"/>

shared/uavobjectdefinition/pathaction.xml

@@ -1,30 +1,24 @@
 <xml>
     <object name="PathAction" singleinstance="false" settings="false" category="Navigation">
         <description>A waypoint command the pathplanner is to use at a certain waypoint</description>
-	<field name="Mode" units="" type="enum" elements="1" options="FlyEndpoint,FlyVector,FlyCircleRight,FlyCircleLeft,
-		DriveEndpoint,DriveVector,DriveCircleLeft,DriveCircleRight,
-		FixedAttitude,
-		SetAccessory,
-		DisarmAlarm" default="Endpoint" />
-	<field name="ModeParameters" units="" type="float" elements="4" default="0"/>
 
-	<field name="EndCondition" units="" type="enum" elements="1" options="None,TimeOut,
+        <field name="Mode" units="" type="enum" elements="1" options="FlyEndpoint,FlyVector,FlyCircleRight,FlyCircleLeft,
-		DistanceToTarget,LegRemaining,BelowError,
+            DriveEndpoint,DriveVector,DriveCircleLeft,DriveCircleRight,
-		AboveAltitude,AboveSpeed,
+            FixedAttitude, SetAccessory, DisarmAlarm" default="Endpoint" />
-		PointingTowardsNext,
+        <field name="ModeParameters" units="" type="float" elements="4" default="0"/>
-		PythonScript,
+        <field name="EndCondition" units="" type="enum" elements="1" options="None,TimeOut,
-		Immediate" default="None" />
+            DistanceToTarget,LegRemaining,BelowError,AboveAltitude,AboveSpeed,PointingTowardsNext,
-	<field name="ConditionParameters" units="" type="float" elements="4" default="0"/>
+            PythonScript,Immediate" default="None" />
-
+        <field name="ConditionParameters" units="" type="float" elements="4" default="0"/>
-	<field name="Command" units="" type="enum" elements="1" options="OnConditionNextWaypoint,OnNotConditionNextWaypoint,
+        <field name="Command" units="" type="enum" elements="1" options="OnConditionNextWaypoint,OnNotConditionNextWaypoint,
-	OnConditionJumpWaypoint,OnNotConditionJumpWaypoint,
+            OnConditionJumpWaypoint,OnNotConditionJumpWaypoint,
-	IfConditionJumpWaypointElseNextWaypoint" default="OnConditionNextWaypoint" />
+            IfConditionJumpWaypointElseNextWaypoint" default="OnConditionNextWaypoint" />
-	<field name="JumpDestination" units="waypoint" type="int16" elements="1" default="0"/>
+        <field name="JumpDestination" units="waypoint" type="int16" elements="1" default="0"/>
-	<field name="ErrorDestination" units="waypoint" type="int16" elements="1" default="0"/>
+        <field name="ErrorDestination" units="waypoint" type="int16" elements="1" default="0"/>
 
         <access gcs="readwrite" flight="readwrite"/>
         <telemetrygcs acked="true" updatemode="manual" period="0"/>
-        <telemetryflight acked="false" updatemode="periodic" period="4000"/>
+        <telemetryflight acked="true" updatemode="manual" period="0"/>
-	<logging updatemode="manual" period="0"/>
+        <logging updatemode="manual" period="0"/>
     </object>
 </xml>

shared/uavobjectdefinition/pathplan.xml

@@ -0,0 +1,14 @@
+<xml>
+    <object name="PathPlan" singleinstance="true" settings="false" category="Navigation">
+        <description>Flight plan informations</description>
+
+        <field name="WaypointCount" units="" type="uint16" elements="1" default="0" />
+        <field name="PathActionCount" units="" type="uint16" elements="1" default="0" />
+        <field name="Crc" units="" type="uint8" elements="1" default="0" />
+
+        <access gcs="readwrite" flight="readonly"/>
+        <telemetrygcs acked="true" updatemode="manual" period="0"/>
+        <telemetryflight acked="true" updatemode="manual" period="0"/>
+        <logging updatemode="manual" period="0"/>
+    </object>
+</xml>

shared/uavobjectdefinition/radiocombridgestats.xml

@@ -0,0 +1,26 @@
+<xml>
+    <object name="RadioComBridgeStats" singleinstance="true" settings="false" category="System">
+        <description>Maintains the telemetry statistics from the OPLM RadioComBridge.</description>
+
+        <field name="TelemetryTxBytes" units="bytes" type="uint32" elements="1"/>
+        <field name="TelemetryTxFailures" units="count" type="uint32" elements="1"/>
+        <field name="TelemetryTxRetries" units="count" type="uint32" elements="1"/>
+        <field name="TelemetryRxBytes" units="bytes" type="uint32" elements="1"/>
+        <field name="TelemetryRxFailures" units="count" type="uint32" elements="1"/>
+        <field name="TelemetryRxSyncErrors" units="count" type="uint32" elements="1"/>
+        <field name="TelemetryRxCrcErrors" units="count" type="uint32" elements="1"/>
+        
+        <field name="RadioTxBytes" units="bytes" type="uint32" elements="1"/>
+        <field name="RadioTxFailures" units="count" type="uint32" elements="1"/>
+        <field name="RadioTxRetries" units="count" type="uint32" elements="1"/>
+        <field name="RadioRxBytes" units="bytes" type="uint32" elements="1"/>
+        <field name="RadioRxFailures" units="count" type="uint32" elements="1"/>
+        <field name="RadioRxSyncErrors" units="count" type="uint32" elements="1"/>
+        <field name="RadioRxCrcErrors" units="count" type="uint32" elements="1"/>
+
+        <access gcs="readonly" flight="readwrite"/>
+        <telemetrygcs acked="false" updatemode="manual" period="0"/>
+        <telemetryflight acked="false" updatemode="periodic" period="5000"/>
+        <logging updatemode="manual" period="0"/>
+    </object>
+</xml>

shared/uavobjectdefinition/revosettings.xml

@@ -7,7 +7,9 @@
         Defaults: updates at 5 Hz, tau = 300s settle time, exp(-(1/f)/tau) ~= 0.9993335555062
         Set BaroGPSOffsetCorrectionAlpha = 1.0 to completely disable baro offset updates. -->
         <field name="BaroGPSOffsetCorrectionAlpha" units="" type="float" elements="1" defaultvalue="0.9993335555062"/>
-
+        <!--  Cooefficients for the polynomial that models the barometer pressure bias as a function of temperature 
+              bias = a + b * temp + c * temp^2 + d * temp^3 --> 
+        <field name="BaroTempCorrectionPolynomial" units="" type="float" elements="3" elementnames="a,b,c,d" defaultvalue="0,0,0,0"/>
         <access gcs="readwrite" flight="readwrite"/>
         <telemetrygcs acked="true" updatemode="onchange" period="0"/>
         <telemetryflight acked="true" updatemode="onchange" period="0"/>

shared/uavobjectdefinition/systemalarms.xml

@@ -16,6 +16,7 @@
 			<elementname>Sensors</elementname>
 			<elementname>Stabilization</elementname>
 			<elementname>Guidance</elementname>
+			<elementname>PathPlan</elementname>
 			<elementname>Battery</elementname>
 			<elementname>FlightTime</elementname>
 			<elementname>I2C</elementname>

shared/uavobjectdefinition/waypoint.xml

@@ -1,12 +1,14 @@
 <xml>
     <object name="Waypoint" singleinstance="false" settings="false" category="Navigation">
         <description>A waypoint the aircraft can try and hit.  Used by the @ref PathPlanner module</description>
+
         <field name="Position" units="m" type="float" elementnames="North, East, Down"/>
-	<field name="Velocity" units="m/s" type="float" elements="1"/>
+        <field name="Velocity" units="m/s" type="float" elements="1"/>
-	<field name="Action" units="" type="uint8" elements="1" />
+        <field name="Action" units="" type="uint8" elements="1" />
+
         <access gcs="readwrite" flight="readwrite"/>
         <telemetrygcs acked="true" updatemode="manual" period="0"/>
-        <telemetryflight acked="false" updatemode="periodic" period="4000"/>
+        <telemetryflight acked="true" updatemode="manual" period="0"/>
-	<logging updatemode="manual" period="0"/>
+        <logging updatemode="manual" period="0"/>
     </object>
 </xml>