From 10dec58a1095a285de1fd4699dc913558ea406e5 Mon Sep 17 00:00:00 2001 From: Bertrand Oresve Date: Sun, 23 Mar 2014 12:56:01 +0100 Subject: [PATCH 1/2] OP-1267 Fix UAV position after a HomeLocation Update --- .../modules/StateEstimation/stateestimation.c | 19 ++++++++++++++++++- 1 file changed, 18 insertions(+), 1 deletion(-) diff --git a/flight/modules/StateEstimation/stateestimation.c b/flight/modules/StateEstimation/stateestimation.c index bff326c87..db05edc59 100644 --- a/flight/modules/StateEstimation/stateestimation.c +++ b/flight/modules/StateEstimation/stateestimation.c @@ -40,6 +40,7 @@ #include #include #include +#include #include #include @@ -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 From 199193aa3f4375e3b5ce98bd2cfee0ade6ec0a3b Mon Sep 17 00:00:00 2001 From: Bertrand Oresve Date: Tue, 25 Mar 2014 21:00:10 +0100 Subject: [PATCH 2/2] OP-1267 Fix UAV position after a HomeLocation Update (adding #define to be clearer) --- .../modules/StateEstimation/stateestimation.c | 20 +++++++++++-------- 1 file changed, 12 insertions(+), 8 deletions(-) diff --git a/flight/modules/StateEstimation/stateestimation.c b/flight/modules/StateEstimation/stateestimation.c index db05edc59..1eca88c4c 100644 --- a/flight/modules/StateEstimation/stateestimation.c +++ b/flight/modules/StateEstimation/stateestimation.c @@ -56,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) \ @@ -335,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: @@ -488,9 +492,9 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) */ 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; + // Ask for a filter init (necessary for LLA filter) + // Only possible if disarmed + fusionAlgorithm = FILTER_INIT_IF_POSSIBLE; }