From 4ad0e730dad417f03f52f537d98171f2cc56f74e Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 8 Dec 2013 12:23:16 +0100 Subject: [PATCH] added status uavobject for easier debugging --- flight/modules/AltitudeHold/altitudehold.c | 14 +++++++++++--- .../boards/revolution/firmware/UAVObjects.inc | 1 + .../boards/revoproto/firmware/UAVObjects.inc | 1 + .../boards/simposix/firmware/UAVObjects.inc | 1 + .../src/plugins/uavobjects/uavobjects.pro | 2 ++ shared/uavobjectdefinition/altitudeholddesired.xml | 2 +- 6 files changed, 17 insertions(+), 4 deletions(-) diff --git a/flight/modules/AltitudeHold/altitudehold.c b/flight/modules/AltitudeHold/altitudehold.c index ffe5b5cc3..78ec27332 100644 --- a/flight/modules/AltitudeHold/altitudehold.c +++ b/flight/modules/AltitudeHold/altitudehold.c @@ -51,6 +51,7 @@ #include #include #include // object that will be updated by the module +#include #include #include #include @@ -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); diff --git a/flight/targets/boards/revolution/firmware/UAVObjects.inc b/flight/targets/boards/revolution/firmware/UAVObjects.inc index 8d7c97181..33a47e757 100644 --- a/flight/targets/boards/revolution/firmware/UAVObjects.inc +++ b/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 diff --git a/flight/targets/boards/revoproto/firmware/UAVObjects.inc b/flight/targets/boards/revoproto/firmware/UAVObjects.inc index 56eff01b1..ee6c02bbd 100644 --- a/flight/targets/boards/revoproto/firmware/UAVObjects.inc +++ b/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 diff --git a/flight/targets/boards/simposix/firmware/UAVObjects.inc b/flight/targets/boards/simposix/firmware/UAVObjects.inc index 0f8adf8f8..e9fc43178 100644 --- a/flight/targets/boards/simposix/firmware/UAVObjects.inc +++ b/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 diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro index 0ca81220d..8fb4d384e 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro +++ b/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 \ diff --git a/shared/uavobjectdefinition/altitudeholddesired.xml b/shared/uavobjectdefinition/altitudeholddesired.xml index 116e82306..7a69227fd 100644 --- a/shared/uavobjectdefinition/altitudeholddesired.xml +++ b/shared/uavobjectdefinition/altitudeholddesired.xml @@ -8,7 +8,7 @@ - +