rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-27 16: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 <gpspositionsensor.h>
#include <gpsvelocitysensor.h>
#include <homelocation.h>
#include <gyrostate.h>
#include <accelstate.h>
@ -55,10 +56,14 @@
#include "CoordinateConversions.h"
// Private constants
#define STACK_SIZE_BYTES 256
#define CALLBACK_PRIORITY CALLBACK_PRIORITY_REGULAR
#define TASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL
#define TIMEOUT_MS 10
#define STACK_SIZE_BYTES 256
#define CALLBACK_PRIORITY CALLBACK_PRIORITY_REGULAR
#define TASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL
#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!
#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 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 +218,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 +244,8 @@ int32_t StateEstimationInitialize(void)
GPSVelocitySensorInitialize();
GPSPositionSensorInitialize();
HomeLocationInitialize();
GyroStateInitialize();
AccelStateInitialize();
MagStateInitialize();
@ -247,6 +255,8 @@ int32_t StateEstimationInitialize(void)
RevoSettingsConnectCallback(&settingsUpdatedCb);
HomeLocationConnectCallback(&homeLocationUpdatedCb);
GyroSensorConnectCallback(&sensorUpdatedCb);
AccelSensorConnectCallback(&sensorUpdatedCb);
MagSensorConnectCallback(&sensorUpdatedCb);
@ -329,7 +339,7 @@ static void StateEstimationCb(void)
if (fusionAlgorithm != revoSettings.FusionAlgorithm) {
FlightStatusData fs;
FlightStatusGet(&fs);
if (fs.Armed == FLIGHTSTATUS_ARMED_DISARMED || fusionAlgorithm == -1) {
if (fs.Armed == FLIGHTSTATUS_ARMED_DISARMED || fusionAlgorithm == FILTER_INIT_FORCE) {
const filterPipeline *newFilterChain;
switch (revoSettings.FusionAlgorithm) {
case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARY:
@ -477,6 +487,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)
{
// 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
* updates the list of "recently updated UAVObjects" and dispatches the state estimator callback