added status uavobject for easier debugging

flight/modules/AltitudeHold/altitudehold.c

@@ -51,6 +51,7 @@
 #include <attitudestate.h>
 #include <altitudeholdsettings.h>
 #include <altitudeholddesired.h> // object that will be updated by the module
+#include <altitudeholdstatus.h>
 #include <flightstatus.h>
 #include <stabilizationdesired.h>
 #include <accelstate.h>
@@ -99,6 +100,7 @@ int32_t AltitudeHoldInitialize()
 {
     AltitudeHoldSettingsInitialize();
     AltitudeHoldDesiredInitialize();
+    AltitudeHoldStatusInitialize();
 
     // Create object queue
 
@@ -135,6 +137,9 @@ static void altitudeHoldTask(void)
         break;
     }
 
+    AltitudeHoldStatusData altitudeHoldStatus;
+    AltitudeHoldStatusGet(&altitudeHoldStatus);
+
     // do the actual control loop(s)
     AltitudeHoldDesiredData altitudeHoldDesired;
     AltitudeHoldDesiredGet(&altitudeHoldDesired);
@@ -144,11 +149,12 @@ static void altitudeHoldTask(void)
     VelocityStateDownGet(&velocityStateDown);
 
     // altitude control loop
-    float velocityDesiredDown = altitudeHoldSettings.AltitudeP * (positionStateDown - altitudeHoldDesired.Altitude) + altitudeHoldDesired.Velocity;
+    altitudeHoldStatus.VelocityDesired     = altitudeHoldSettings.AltitudeP * (positionStateDown - altitudeHoldDesired.Altitude) + altitudeHoldDesired.Velocity;
 
     // velocity control loop
-    float realAccelDesired    = altitudeHoldSettings.VelocityP * (velocityStateDown - velocityDesiredDown) - 9.81f;
+    altitudeHoldStatus.AccelerationDesired = altitudeHoldSettings.VelocityP * (velocityStateDown - altitudeHoldStatus.VelocityDesired) - 9.81f;
 
+    AltitudeHoldStatusSet(&altitudeHoldStatus);
 
     // compensate acceleration by rotation
     // explanation: Rbe[2][2] is the Down component of a 0,0,1 vector rotated by Attitude.Q
@@ -163,12 +169,14 @@ static void altitudeHoldTask(void)
     float Rbe[3][3];
     Quaternion2R(&attitudeState.q1, Rbe);
 
-    float rotatedAccelDesired = realAccelDesired;
+    float rotatedAccelDesired = altitudeHoldStatus.AccelerationDesired;
+#if 0
     if (fabsf(Rbe[2][2]) > 1e-3f) {
         rotatedAccelDesired /= Rbe[2][2];
     } else {
         rotatedAccelDesired = accelStateDown;
     }
+#endif
 
     // acceleration control loop
     float throttle = startThrottle - pid_apply_setpoint(&accelpid, 1.0f, rotatedAccelDesired, accelStateDown, 1000 / DESIRED_UPDATE_RATE_MS);

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

@@ -98,6 +98,7 @@ UAVOBJSRCFILENAMES += oplinksettings
 UAVOBJSRCFILENAMES += oplinkstatus
 UAVOBJSRCFILENAMES += altitudefiltersettings
 UAVOBJSRCFILENAMES += altitudeholddesired
+UAVOBJSRCFILENAMES += altitudeholdstatus
 UAVOBJSRCFILENAMES += waypoint
 UAVOBJSRCFILENAMES += waypointactive
 UAVOBJSRCFILENAMES += poilocation

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

@@ -96,6 +96,7 @@ UAVOBJSRCFILENAMES += camerastabsettings
 UAVOBJSRCFILENAMES += altitudeholdsettings
 UAVOBJSRCFILENAMES += altitudefiltersettings
 UAVOBJSRCFILENAMES += altitudeholddesired
+UAVOBJSRCFILENAMES += altitudeholdstatus
 UAVOBJSRCFILENAMES += waypoint
 UAVOBJSRCFILENAMES += waypointactive
 UAVOBJSRCFILENAMES += poilocation

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

@@ -101,6 +101,7 @@ UAVOBJSRCFILENAMES += altitudeholdsettings
 UAVOBJSRCFILENAMES += altitudefiltersettings 
 UAVOBJSRCFILENAMES += revosettings
 UAVOBJSRCFILENAMES += altitudeholddesired
+UAVOBJSRCFILENAMES += altitudeholdstatus
 UAVOBJSRCFILENAMES += ekfconfiguration
 UAVOBJSRCFILENAMES += ekfstatevariance
 

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

@@ -34,6 +34,7 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \
     $$UAVOBJECT_SYNTHETICS/attitudesimulated.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 \
@@ -124,6 +125,7 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \
     $$UAVOBJECT_SYNTHETICS/attitudesimulated.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 \

shared/uavobjectdefinition/altitudeholddesired.xml

@@ -8,7 +8,7 @@
         <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>