rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-19 09:54:15 +01:00
Code Issues Releases Activity

UAVObjects/AttitudeSettinsg: Object for CC to tune the attitude estimation

Browse Source 
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2666 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124 2011-02-01 02:18:26 +00:00 committed by peabody124
parent 15f3ada700
commit 1c6b51b704
5 changed files with 31 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
flight/CopterControl/Makefile
View File

@ -178,6 +178,7 @@ SRC += $(OPUAVSYNTHDIR)/mixersettings.c
SRC += $(OPUAVSYNTHDIR)/mixerstatus.c
SRC += $(OPUAVSYNTHDIR)/firmwareiapobj.c
SRC += $(OPUAVSYNTHDIR)/uavobjectsinit.c
SRC += $(OPUAVSYNTHDIR)/attitudesettings.c
#${wildcard ${OBJ}/$(shell echo $(VAR) | tr A-Z a-z)/*.c}
#SRC += ${foreach OBJ, ${UAVOBJECTS}, $(UAVOBJECTS)/$(OBJ).c}
# Cant use until i can automatically generate list of UAVObjects

17
flight/Modules/Attitude/attitude.c
View File

@ -53,6 +53,7 @@
#include "attituderaw.h"
#include "attitudeactual.h"
#include "attitudedesired.h"
#include "attitudesettings.h"
#include "manualcontrolcommand.h"
#include "CoordinateConversions.h"
#include "pios_flash_w25x.h"
@ -128,10 +129,14 @@ void updateSensors()
{
AttitudeRawData attitudeRaw;
AttitudeRawGet(&attitudeRaw);
AttitudeSettingsData settings;
AttitudeSettingsGet(&settings);
struct pios_adxl345_data accel_data;
static float gyro_bias[3] = {0,0,0};
static const float tau = 0.9999f;
float tau = (1-settings.GyroBiasTau);
attitudeRaw.gyros_filtered[ATTITUDERAW_GYROS_FILTERED_X] = -(gyro[0] - GYRO_NEUTRAL) * GYRO_SCALE;
attitudeRaw.gyros_filtered[ATTITUDERAW_GYROS_FILTERED_Y] = (gyro[1] - GYRO_NEUTRAL) * GYRO_SCALE;
@ -172,9 +177,12 @@ void updateSensors()
AttitudeRawSet(&attitudeRaw);
}
#define UPDATE_FRAC 0.999f
#define UPDATE_FRAC 0.99999f
void updateAttitude()
{
AttitudeSettingsData settings;
AttitudeSettingsGet(&settings);
AttitudeActualData attitudeActual;
AttitudeActualGet(&attitudeActual);
@ -186,6 +194,7 @@ void updateAttitude()
float accel_pitch, accel_roll;
static float dT = 0;
float tau = 1-settings.AccelTau;
thisSysTime = xTaskGetTickCount();
if(thisSysTime > lastSysTime) // reuse dt in case of wraparound
@ -212,8 +221,8 @@ void updateAttitude()
RPY2Quaternion(&attitudeActual.Roll, &attitudeActual.q1);
// Weighted average and back into degrees
attitudeActual.Roll = (UPDATE_FRAC * attitudeActual.Roll + (1-UPDATE_FRAC) * accel_roll) * 180 / M_PI;
attitudeActual.Pitch = (UPDATE_FRAC * attitudeActual.Pitch + (1-UPDATE_FRAC) * accel_pitch) * 180 / M_PI;
attitudeActual.Roll = (tau * attitudeActual.Roll + (1-tau) * accel_roll) * 180 / M_PI;
attitudeActual.Pitch = (tau * attitudeActual.Pitch + (1-tau) * accel_pitch) * 180 / M_PI;
attitudeActual.Yaw = fmod(attitudeActual.Yaw * 180 / M_PI, 360);
AttitudeActualSet(&attitudeActual);
}

2
flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj
View File

@ -2682,6 +2682,7 @@
65C35F6812F0DC2D004811C2 /* attitude.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = attitude.h; sourceTree = "<group>"; };
65D2CA841248F9A400B1E7D6 /* mixersettings.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = mixersettings.xml; sourceTree = "<group>"; };
65D2CA851248F9A400B1E7D6 /* mixerstatus.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = mixerstatus.xml; sourceTree = "<group>"; };
65E410AE12F65AEA00725888 /* attitudesettings.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = attitudesettings.xml; sourceTree = "<group>"; };
65E6DF7112E02E8E00058553 /* Makefile */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.make; path = Makefile; sourceTree = "<group>"; };
65E6DF7312E02E8E00058553 /* alarms.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = alarms.c; sourceTree = "<group>"; };
65E6DF7412E02E8E00058553 /* coptercontrol.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = coptercontrol.c; sourceTree = "<group>"; };
@ -7399,6 +7400,7 @@
65C35E7712EFB2F3004811C2 /* velocityactual.xml */,
65C35E7812EFB2F3004811C2 /* velocitydesired.xml */,
65C35E7912EFB2F3004811C2 /* watchdogstatus.xml */,
65E410AE12F65AEA00725888 /* attitudesettings.xml */,
);
path = uavobjectdefinition;
sourceTree = "<group>";

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

@ -65,7 +65,8 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/ahrsstatus.h \
$$UAVOBJECT_SYNTHETICS/flightplancontrol.h \
$$UAVOBJECT_SYNTHETICS/watchdogstatus.h \
$$UAVOBJECT_SYNTHETICS/nedaccel.h \
$$UAVOBJECT_SYNTHETICS/sonaraltitude.h
$$UAVOBJECT_SYNTHETICS/sonaraltitude.h \
$$UAVOBJECT_SYNTHETICS/attitudesettings.h
SOURCES += $$UAVOBJECT_SYNTHETICS/ahrsstatus.cpp \
$$UAVOBJECT_SYNTHETICS/ahrscalibration.cpp \
@ -110,5 +111,5 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/ahrsstatus.cpp \
$$UAVOBJECT_SYNTHETICS/watchdogstatus.cpp \
$$UAVOBJECT_SYNTHETICS/nedaccel.cpp \
$$UAVOBJECT_SYNTHETICS/sonaraltitude.cpp \
$$UAVOBJECT_SYNTHETICS/uavobjectsinit.cpp
$$UAVOBJECT_SYNTHETICS/uavobjectsinit.cpp \
$$UAVOBJECT_SYNTHETICS/attitudesettings.cpp

11
shared/uavobjectdefinition/attitudesettings.xml Normal file
View File

@ -0,0 +1,11 @@
<xml>
<object name="AttitudeSettings" singleinstance="true" settings="true">
<description>Settings for the @ref Attitude module used on CopterControl</description>
<field name="GyroBiasTau" units="channel" type="float" elements="1" defaultvalue="0.00001"/>
<field name="AccelTau" units="channel" type="float" elements="1" defaultvalue="0.00001"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="onchange" period="0"/>
<logging updatemode="never" period="0"/>
</object>
</xml>