rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-29 07:24:13 +01:00
Code Issues Releases Activity

OP-1267 Fix UAV position after a HomeLocation Update

Browse Source
This commit is contained in:
Bertrand Oresve 2014-03-23 12:56:01 +01:00
parent 2190ccdf77
commit 10dec58a10
1 changed files with 18 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -40,6 +40,7 @@
#include <airspeedsensor.h>
#include <gpspositionsensor.h>
#include <gpsvelocitysensor.h>
#include <homelocation.h>
#include <gyrostate.h>
#include <accelstate.h>
@ -119,7 +120,7 @@ static DelayedCallbackInfo *stateEstimationCallback;
static volatile RevoSettingsData revoSettings;
static volatile sensorUpdates updatedSensors;
static int32_t fusionAlgorithm = -1;
static volatile int32_t fusionAlgorithm = -1;
static filterPipeline *filterChain = NULL;
// different filters available to state estimation
@ -213,6 +214,7 @@ static const filterPipeline *ekf13Queue = &(filterPipeline) {
static void settingsUpdatedCb(UAVObjEvent *objEv);
static void sensorUpdatedCb(UAVObjEvent *objEv);
static void homeLocationUpdatedCb(UAVObjEvent *objEv);
static void StateEstimationCb(void);
static inline int32_t maxint32_t(int32_t a, int32_t b)
@ -238,6 +240,8 @@ int32_t StateEstimationInitialize(void)
GPSVelocitySensorInitialize();
GPSPositionSensorInitialize();
HomeLocationInitialize();
GyroStateInitialize();
AccelStateInitialize();
MagStateInitialize();
@ -247,6 +251,8 @@ int32_t StateEstimationInitialize(void)
RevoSettingsConnectCallback(&settingsUpdatedCb);
HomeLocationConnectCallback(&homeLocationUpdatedCb);
GyroSensorConnectCallback(&sensorUpdatedCb);
AccelSensorConnectCallback(&sensorUpdatedCb);
MagSensorConnectCallback(&sensorUpdatedCb);
@ -477,6 +483,17 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
RevoSettingsGet((RevoSettingsData *)&revoSettings);
}
/**
* Callback for eventdispatcher when HomeLocation has been updated
*/
static void homeLocationUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
// Set fusionAlgorithm to -2 to force a filter init (necessary for LLA filter)
// This value force a filter init only when disarmed
fusionAlgorithm = -2;
}
/**
* Callback for eventdispatcher when any sensor UAVObject has been updated
* updates the list of "recently updated UAVObjects" and dispatches the state estimator callback