rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-18 03:52:11 +01:00
Code Issues Releases Activity

added status uavobject for easier debugging

Browse Source
This commit is contained in:
Corvus Corax 2013-12-08 12:23:16 +01:00
parent 90f689cfa1
commit 4ad0e730da
6 changed files with 17 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
flight/modules/AltitudeHold/altitudehold.c
View File

@ -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);

1
flight/targets/boards/revolution/firmware/UAVObjects.inc
View File

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

1
flight/targets/boards/revoproto/firmware/UAVObjects.inc
View File

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

1
flight/targets/boards/simposix/firmware/UAVObjects.inc
View File

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

2
ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro
View File

@ -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 \

2
shared/uavobjectdefinition/altitudeholddesired.xml
View File

@ -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>