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

LP-353 - Implement a specific Nav only option in ekf filter, add a debug NavYaw field for testing pourpose(it may be used for integrity check as well)

Browse Source
This commit is contained in:
Alessio Morale 2016-09-20 02:15:17 +02:00
parent 08b7153624
commit 8df3758104
5 changed files with 96 additions and 68 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

124
flight/modules/StateEstimation/filterekf.c
View File

@ -71,87 +71,68 @@ struct data {
bool inited;
PiOSDeltatimeConfig dtconfig;
bool navOnly;
};
// Private variables
static bool initialized = 0;
// Private functions
static int32_t init13i(stateFilter *self);
static int32_t init13(stateFilter *self);
static int32_t maininit(stateFilter *self);
static int32_t init(stateFilter *self);
static filterResult filter(stateFilter *self, stateEstimation *state);
static inline bool invalid_var(float data);
static void globalInit(void);
static int32_t globalInit(stateFilter *handle, bool usePos, bool navOnly);
static void globalInit(void)
static int32_t globalInit(stateFilter *handle, bool usePos, bool navOnly)
{
if (!initialized) {
initialized = 1;
EKFConfigurationInitialize();
EKFStateVarianceInitialize();
HomeLocationInitialize();
}
handle->init = &init;
handle->filter = &filter;
handle->localdata = pios_malloc(sizeof(struct data));
struct data *this = (struct data *)handle->localdata;
this->usePos = usePos;
this->navOnly = navOnly;
EKFConfigurationInitialize();
EKFStateVarianceInitialize();
HomeLocationInitialize();
return STACK_REQUIRED;
}
int32_t filterEKF13iInitialize(stateFilter *handle)
{
globalInit();
handle->init = &init13i;
handle->filter = &filter;
handle->localdata = pios_malloc(sizeof(struct data));
return STACK_REQUIRED;
return globalInit(handle, false, false);
}
int32_t filterEKF13Initialize(stateFilter *handle)
{
globalInit();
handle->init = &init13;
handle->filter = &filter;
handle->localdata = pios_malloc(sizeof(struct data));
return STACK_REQUIRED;
return globalInit(handle, true, false);
}
int32_t filterEKF13iNavOnlyInitialize(stateFilter *handle)
{
return globalInit(handle, false, true);
}
int32_t filterEKF13NavOnlyInitialize(stateFilter *handle)
{
return globalInit(handle, true, true);
}
#ifdef FALSE
// XXX
// TODO: Until the 16 state EKF is implemented, run 13 state, so compilation runs through
// XXX
int32_t filterEKF16iInitialize(stateFilter *handle)
{
globalInit();
handle->init = &init13i;
handle->filter = &filter;
handle->localdata = pios_malloc(sizeof(struct data));
return STACK_REQUIRED;
return filterEKFi13Initialize(handle);
}
int32_t filterEKF16Initialize(stateFilter *handle)
{
globalInit();
handle->init = &init13;
handle->filter = &filter;
handle->localdata = pios_malloc(sizeof(struct data));
return STACK_REQUIRED;
return filterEKF13Initialize(handle);
}
#endif
static int32_t init13i(stateFilter *self)
{
struct data *this = (struct data *)self->localdata;
this->usePos = 0;
return maininit(self);
}
static int32_t init13(stateFilter *self)
{
struct data *this = (struct data *)self->localdata;
this->usePos = 1;
return maininit(self);
}
static int32_t maininit(stateFilter *self)
static int32_t init(stateFilter *self)
{
struct data *this = (struct data *)self->localdata;
@ -303,13 +284,16 @@ static filterResult filter(stateFilter *self, stateEstimation *state)
// Copy the attitude into the state
// NOTE: updating gyr correctly is valid, because this code is reached only when SENSORUPDATES_gyro is already true
state->attitude[0] = Nav.q[0];
state->attitude[1] = Nav.q[1];
state->attitude[2] = Nav.q[2];
state->attitude[3] = Nav.q[3];
state->gyro[0] -= RAD2DEG(Nav.gyro_bias[0]);
state->gyro[1] -= RAD2DEG(Nav.gyro_bias[1]);
state->gyro[2] -= RAD2DEG(Nav.gyro_bias[2]);
if(!this->navOnly){
state->attitude[0] = Nav.q[0];
state->attitude[1] = Nav.q[1];
state->attitude[2] = Nav.q[2];
state->attitude[3] = Nav.q[3];
state->gyro[0] -= RAD2DEG(Nav.gyro_bias[0]);
state->gyro[1] -= RAD2DEG(Nav.gyro_bias[1]);
state->gyro[2] -= RAD2DEG(Nav.gyro_bias[2]);
}
state->pos[0] = Nav.Pos[0];
state->pos[1] = Nav.Pos[1];
state->pos[2] = Nav.Pos[2];
@ -338,13 +322,21 @@ static filterResult filter(stateFilter *self, stateEstimation *state)
// Copy the attitude into the state
// NOTE: updating gyr correctly is valid, because this code is reached only when SENSORUPDATES_gyro is already true
state->attitude[0] = Nav.q[0];
state->attitude[1] = Nav.q[1];
state->attitude[2] = Nav.q[2];
state->attitude[3] = Nav.q[3];
state->gyro[0] -= RAD2DEG(Nav.gyro_bias[0]);
state->gyro[1] -= RAD2DEG(Nav.gyro_bias[1]);
state->gyro[2] -= RAD2DEG(Nav.gyro_bias[2]);
if(!this->navOnly){
state->attitude[0] = Nav.q[0];
state->attitude[1] = Nav.q[1];
state->attitude[2] = Nav.q[2];
state->attitude[3] = Nav.q[3];
state->gyro[0] -= RAD2DEG(Nav.gyro_bias[0]);
state->gyro[1] -= RAD2DEG(Nav.gyro_bias[1]);
state->gyro[2] -= RAD2DEG(Nav.gyro_bias[2]);
}
{
float tmp[3];
Quaternion2RPY(Nav.q, tmp);
state->debugNavYaw = tmp[2];
}
state->pos[0] = Nav.Pos[0];
state->pos[1] = Nav.Pos[1];
state->pos[2] = Nav.Pos[2];

3
flight/modules/StateEstimation/inc/stateestimation.h
View File

@ -71,6 +71,7 @@ typedef struct {
float baro[1];
float auxMag[3];
float boardMag[3];
float debugNavYaw;
uint8_t magStatus;
bool armed;
sensorUpdates updated;
@ -95,6 +96,8 @@ int32_t filterCFInitialize(stateFilter *handle);
int32_t filterCFMInitialize(stateFilter *handle);
int32_t filterEKF13iInitialize(stateFilter *handle);
int32_t filterEKF13Initialize(stateFilter *handle);
int32_t filterEKF13NavOnlyInitialize(stateFilter *handle);
int32_t filterEKF13iNavOnlyInitialize(stateFilter *handle);
int32_t filterEKF16iInitialize(stateFilter *handle);
int32_t filterEKF16Initialize(stateFilter *handle);

34
flight/modules/StateEstimation/stateestimation.c
View File

@ -157,6 +157,8 @@ static stateFilter cfFilter;
static stateFilter cfmFilter;
static stateFilter ekf13iFilter;
static stateFilter ekf13Filter;
static stateFilter ekf13iNavFilter;
static stateFilter ekf13NavFilter;
// this is a hack to provide a computational shortcut for faster gyro state progression
static float gyroRaw[3];
@ -260,7 +262,30 @@ static const filterPipeline *ekf13NavCFAttQueue = &(filterPipeline) {
.next = &(filterPipeline) {
.filter = &baroFilter,
.next = &(filterPipeline) {
.filter = &ekf13Filter,
.filter = &ekf13NavFilter,
.next = &(filterPipeline) {
.filter = &velocityFilter,
.next = &(filterPipeline) {
.filter = &cfmFilter,
.next = NULL,
}
}
}
}
}
}
};
static const filterPipeline *ekf13iNavCFAttQueue = &(filterPipeline) {
.filter = &magFilter,
.next = &(filterPipeline) {
.filter = &airFilter,
.next = &(filterPipeline) {
.filter = &baroiFilter,
.next = &(filterPipeline) {
.filter = &stationaryFilter,
.next = &(filterPipeline) {
.filter = &ekf13iNavFilter,
.next = &(filterPipeline) {
.filter = &velocityFilter,
.next = &(filterPipeline) {
@ -343,6 +368,8 @@ int32_t StateEstimationInitialize(void)
stack_required = maxint32_t(stack_required, filterCFMInitialize(&cfmFilter));
stack_required = maxint32_t(stack_required, filterEKF13iInitialize(&ekf13iFilter));
stack_required = maxint32_t(stack_required, filterEKF13Initialize(&ekf13Filter));
stack_required = maxint32_t(stack_required, filterEKF13NavOnlyInitialize(&ekf13NavFilter));
stack_required = maxint32_t(stack_required, filterEKF13iNavOnlyInitialize(&ekf13iNavFilter));
stateEstimationCallback = PIOS_CALLBACKSCHEDULER_Create(&StateEstimationCb, CALLBACK_PRIORITY, TASK_PRIORITY, CALLBACKINFO_RUNNING_STATEESTIMATION, stack_required);
@ -424,12 +451,16 @@ static void StateEstimationCb(void)
case REVOSETTINGS_FUSIONALGORITHM_GPSNAVIGATIONINS13CF:
newFilterChain = ekf13NavCFAttQueue;
break;
case REVOSETTINGS_FUSIONALGORITHM_TESTINGINSINDOORCF:
newFilterChain = ekf13iNavCFAttQueue;
break;
default:
newFilterChain = NULL;
}
// initialize filters in chain
current = newFilterChain;
bool error = 0;
states.debugNavYaw = 0;
while (current != NULL) {
int32_t result = current->filter->init((stateFilter *)current->filter);
if (result != 0) {
@ -524,6 +555,7 @@ static void StateEstimationCb(void)
s.q3 = states.attitude[2];
s.q4 = states.attitude[3];
Quaternion2RPY(&s.q1, &s.Roll);
s.NavYaw = states.debugNavYaw;
AttitudeStateSet(&s);
}
// throttle alarms, raise alarm flags immediately

1
shared/uavobjectdefinition/attitudestate.xml
View File

@ -8,6 +8,7 @@
<field name="Roll" units="degrees" type="float" elements="1"/>
<field name="Pitch" units="degrees" type="float" elements="1"/>
<field name="Yaw" units="degrees" type="float" elements="1"/>
<field name="NavYaw" units="degrees" type="float" elements="1"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="130"/>

2
shared/uavobjectdefinition/revosettings.xml
View File

@ -2,7 +2,7 @@
<object name="RevoSettings" singleinstance="true" settings="true" category="State">
<description>Settings for the revo to control the algorithm and what is updated</description>
<field name="FusionAlgorithm" units="" type="enum" elements="1"
options="None,Basic (Complementary),Complementary+Mag,Complementary+Mag+GPSOutdoor,INS13Indoor,GPS Navigation (INS13),GPS Navigation (INS13+CF)"
options="None,Basic (Complementary),Complementary+Mag,Complementary+Mag+GPSOutdoor,INS13Indoor,GPS Navigation (INS13),GPS Navigation (INS13+CF),Testing (INS Indoor+CF)"
defaultvalue="Basic (Complementary)"/>
<!-- Low pass filter configuration to calculate offset of barometric altitude sensor.