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

Merge remote-tracking branch 'origin/Bertrand/OP-1267_Update_HomeLocation_In_LLA_Filter' into next

Browse Source
This commit is contained in:
Corvus Corax 2014-03-27 21:17:42 +01:00
commit 7bd7c20fd9
1 changed files with 27 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -40,6 +40,7 @@
#include <airspeedsensor.h> #include <airspeedsensor.h>
#include <gpspositionsensor.h> #include <gpspositionsensor.h>
#include <gpsvelocitysensor.h> #include <gpsvelocitysensor.h>
#include <homelocation.h>
#include <gyrostate.h> #include <gyrostate.h>
#include <accelstate.h> #include <accelstate.h>
@ -55,10 +56,14 @@
#include "CoordinateConversions.h" #include "CoordinateConversions.h"
// Private constants // Private constants
#define STACK_SIZE_BYTES 256 #define STACK_SIZE_BYTES 256
#define CALLBACK_PRIORITY CALLBACK_PRIORITY_REGULAR #define CALLBACK_PRIORITY CALLBACK_PRIORITY_REGULAR
#define TASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL #define TASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL
#define TIMEOUT_MS 10 #define TIMEOUT_MS 10
// Private filter init const
#define FILTER_INIT_FORCE -1
#define FILTER_INIT_IF_POSSIBLE -2
// local macros, ONLY to be used in the middle of StateEstimationCb in section RUNSTATE_LOAD after the update of states updated! // local macros, ONLY to be used in the middle of StateEstimationCb in section RUNSTATE_LOAD after the update of states updated!
#define FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(sensorname, shortname, a1, a2, a3) \ #define FETCH_SENSOR_FROM_UAVOBJECT_CHECK_AND_LOAD_TO_STATE_3_DIMENSIONS(sensorname, shortname, a1, a2, a3) \
@ -119,7 +124,7 @@ static DelayedCallbackInfo *stateEstimationCallback;
static volatile RevoSettingsData revoSettings; static volatile RevoSettingsData revoSettings;
static volatile sensorUpdates updatedSensors; static volatile sensorUpdates updatedSensors;
static int32_t fusionAlgorithm = -1; static volatile int32_t fusionAlgorithm = -1;
static filterPipeline *filterChain = NULL; static filterPipeline *filterChain = NULL;
// different filters available to state estimation // different filters available to state estimation
@ -213,6 +218,7 @@ static const filterPipeline *ekf13Queue = &(filterPipeline) {
static void settingsUpdatedCb(UAVObjEvent *objEv); static void settingsUpdatedCb(UAVObjEvent *objEv);
static void sensorUpdatedCb(UAVObjEvent *objEv); static void sensorUpdatedCb(UAVObjEvent *objEv);
static void homeLocationUpdatedCb(UAVObjEvent *objEv);
static void StateEstimationCb(void); static void StateEstimationCb(void);
static inline int32_t maxint32_t(int32_t a, int32_t b) static inline int32_t maxint32_t(int32_t a, int32_t b)
@ -238,6 +244,8 @@ int32_t StateEstimationInitialize(void)
GPSVelocitySensorInitialize(); GPSVelocitySensorInitialize();
GPSPositionSensorInitialize(); GPSPositionSensorInitialize();
HomeLocationInitialize();
GyroStateInitialize(); GyroStateInitialize();
AccelStateInitialize(); AccelStateInitialize();
MagStateInitialize(); MagStateInitialize();
@ -247,6 +255,8 @@ int32_t StateEstimationInitialize(void)
RevoSettingsConnectCallback(&settingsUpdatedCb); RevoSettingsConnectCallback(&settingsUpdatedCb);
HomeLocationConnectCallback(&homeLocationUpdatedCb);
GyroSensorConnectCallback(&sensorUpdatedCb); GyroSensorConnectCallback(&sensorUpdatedCb);
AccelSensorConnectCallback(&sensorUpdatedCb); AccelSensorConnectCallback(&sensorUpdatedCb);
MagSensorConnectCallback(&sensorUpdatedCb); MagSensorConnectCallback(&sensorUpdatedCb);
@ -329,7 +339,7 @@ static void StateEstimationCb(void)
if (fusionAlgorithm != revoSettings.FusionAlgorithm) { if (fusionAlgorithm != revoSettings.FusionAlgorithm) {
FlightStatusData fs; FlightStatusData fs;
FlightStatusGet(&fs); FlightStatusGet(&fs);
if (fs.Armed == FLIGHTSTATUS_ARMED_DISARMED || fusionAlgorithm == -1) { if (fs.Armed == FLIGHTSTATUS_ARMED_DISARMED || fusionAlgorithm == FILTER_INIT_FORCE) {
const filterPipeline *newFilterChain; const filterPipeline *newFilterChain;
switch (revoSettings.FusionAlgorithm) { switch (revoSettings.FusionAlgorithm) {
case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARY: case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARY:
@ -477,6 +487,17 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
RevoSettingsGet((RevoSettingsData *)&revoSettings); RevoSettingsGet((RevoSettingsData *)&revoSettings);
} }
/**
* Callback for eventdispatcher when HomeLocation has been updated
*/
static void homeLocationUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
// Ask for a filter init (necessary for LLA filter)
// Only possible if disarmed
fusionAlgorithm = FILTER_INIT_IF_POSSIBLE;
}
/** /**
* Callback for eventdispatcher when any sensor UAVObject has been updated * Callback for eventdispatcher when any sensor UAVObject has been updated
* updates the list of "recently updated UAVObjects" and dispatches the state estimator callback * updates the list of "recently updated UAVObjects" and dispatches the state estimator callback