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

New EKFStateVariance UAVObject with EKF state variance information

Browse Source
This commit is contained in:
Corvus Corax 2013-04-28 19:38:37 +02:00
parent 14f72459c6
commit 6e2c9b3c46
7 changed files with 34 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
flight/libraries/inc/insgps.h
View File

@ -59,6 +59,7 @@ void INSCovariancePrediction(float dT);
void INSCorrection(float mag_data[3], float Pos[3], float Vel[3], float BaroAlt, uint16_t SensorsUsed);
void INSResetP(float PDiag[13]);
void INSGetP(float PDiag[13]);
void INSSetState(float pos[3], float vel[3], float q[4], float gyro_bias[3], float accel_bias[3]);
void INSSetPosVelVar(float PosVar, float VelVar);
void INSSetGyroBias(float gyro_bias[3]);

12
flight/libraries/insgps13state.c
View File

@ -141,6 +141,18 @@ void INSResetP(float PDiag[NUMX])
}
}
void INSGetP(float PDiag[NUMX])
{
uint8_t i;
// retrieve diagonal elements (aka state variance)
for (i=0;i<NUMX;i++){
if (PDiag != 0){
PDiag[i] = P[i][i];
}
}
}
void INSSetState(float pos[3], float vel[3], float q[4], float gyro_bias[3], float accel_bias[3])
{
/* Note: accel_bias not used in 13 state INS */

7
flight/modules/Attitude/revolution/attitude.c
View File

@ -66,6 +66,7 @@
#include "homelocation.h"
#include "magnetometer.h"
#include "positionactual.h"
#include "ekfstatevariance.h"
#include "revocalibration.h"
#include "revosettings.h"
#include "velocityactual.h"
@ -137,6 +138,7 @@ int32_t AttitudeInitialize(void)
VelocityActualInitialize();
RevoSettingsInitialize();
RevoCalibrationInitialize();
EKFStateVarianceInitialize();
// Initialize this here while we aren't setting the homelocation in GPS
HomeLocationInitialize();
@ -885,6 +887,11 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
gyrosBias.z = Nav.gyro_bias[2] * 180.0f / M_PI_F;
GyrosBiasSet(&gyrosBias);
EKFStateVarianceData vardata;
EKFStateVarianceGet(&vardata);
INSGetP(vardata.P);
EKFStateVarianceSet(&vardata);
return 0;
}

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

@ -68,6 +68,7 @@ UAVOBJSRCFILENAMES += positionactual
UAVOBJSRCFILENAMES += ratedesired
UAVOBJSRCFILENAMES += relaytuning
UAVOBJSRCFILENAMES += relaytuningsettings
UAVOBJSRCFILENAMES += ekfstatevariance
UAVOBJSRCFILENAMES += revocalibration
UAVOBJSRCFILENAMES += revosettings
UAVOBJSRCFILENAMES += sonaraltitude

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

@ -73,6 +73,7 @@ UAVOBJSRCFILENAMES += positionactual
UAVOBJSRCFILENAMES += ratedesired
UAVOBJSRCFILENAMES += relaytuning
UAVOBJSRCFILENAMES += relaytuningsettings
UAVOBJSRCFILENAMES += ekfstatevariance
UAVOBJSRCFILENAMES += revocalibration
UAVOBJSRCFILENAMES += revosettings
UAVOBJSRCFILENAMES += sonaraltitude

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

@ -33,6 +33,7 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \
$$UAVOBJECT_SYNTHETICS/altholdsmoothed.h \
$$UAVOBJECT_SYNTHETICS/altitudeholddesired.h \
$$UAVOBJECT_SYNTHETICS/altitudeholdsettings.h \
$$UAVOBJECT_SYNTHETICS/ekfstatevariance.h \
$$UAVOBJECT_SYNTHETICS/revocalibration.h \
$$UAVOBJECT_SYNTHETICS/revosettings.h \
$$UAVOBJECT_SYNTHETICS/gcstelemetrystats.h \
@ -112,6 +113,7 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \
$$UAVOBJECT_SYNTHETICS/altholdsmoothed.cpp \
$$UAVOBJECT_SYNTHETICS/altitudeholddesired.cpp \
$$UAVOBJECT_SYNTHETICS/altitudeholdsettings.cpp \
$$UAVOBJECT_SYNTHETICS/ekfstatevariance.cpp \
$$UAVOBJECT_SYNTHETICS/revocalibration.cpp \
$$UAVOBJECT_SYNTHETICS/revosettings.cpp \
$$UAVOBJECT_SYNTHETICS/gcstelemetrystats.cpp \

10
shared/uavobjectdefinition/ekfstatevariance.xml Normal file
View File

@ -0,0 +1,10 @@
<xml>
<object name="EKFStateVariance" singleinstance="true" settings="false">
<description>Extended Kalman Filter state covariance</description>
<field name="P" units="1^2" type="float" elements="13"/>
<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>