Skip to content

Commit

Permalink
#495 Initialize kalman estimator correctly
Browse files Browse the repository at this point in the history
  • Loading branch information
krichardsson committed Nov 3, 2019
1 parent 173ca50 commit 821e249
Show file tree
Hide file tree
Showing 3 changed files with 41 additions and 32 deletions.
3 changes: 3 additions & 0 deletions src/modules/interface/estimator_kalman.h
Original file line number Diff line number Diff line change
Expand Up @@ -62,6 +62,9 @@ bool estimatorKalmanTest(void);
void estimatorKalman(state_t *state, sensorData_t *sensors, control_t *control, const uint32_t tick);


void estimatorKalmanTaskInit();
bool estimatorKalmanTaskTest();

/**
* The filter supports the incorporation of additional sensors into the state estimate via the following functions:
*/
Expand Down
67 changes: 35 additions & 32 deletions src/modules/src/estimator_kalman.c
Original file line number Diff line number Diff line change
Expand Up @@ -161,7 +161,7 @@ static inline bool stateEstimatorHasYawErrorPacket(float *error) {
static SemaphoreHandle_t measurementQueueSemaphore;

// Mutex to protect data that is shared between the task and
// function called byt he stabilizer loop
// functions called by the stabilizer loop
static SemaphoreHandle_t dataMutex;


Expand Down Expand Up @@ -254,13 +254,38 @@ static inline void mat_mult(const arm_matrix_instance_f32 * pSrcA, const arm_mat
static inline float arm_sqrt(float32_t in)
{ float pOut = 0; arm_status result = arm_sqrt_f32(in, &pOut); configASSERT(ARM_MATH_SUCCESS == result); return pOut; }

static void kalmanTask(void* parameters);
static bool predictStateForward(uint32_t osTick, float dt);
static bool updateQueuedMeasurments(const Axis3f *gyro);


// --------------------------------------------------

void kalmanTask(void* parameters) {
void estimatorKalmanTaskInit() {
distDataQueue = xQueueCreate(DIST_QUEUE_LENGTH, sizeof(distanceMeasurement_t));
posDataQueue = xQueueCreate(POS_QUEUE_LENGTH, sizeof(positionMeasurement_t));
poseDataQueue = xQueueCreate(POSE_QUEUE_LENGTH, sizeof(poseMeasurement_t));
tdoaDataQueue = xQueueCreate(UWB_QUEUE_LENGTH, sizeof(tdoaMeasurement_t));
flowDataQueue = xQueueCreate(FLOW_QUEUE_LENGTH, sizeof(flowMeasurement_t));
tofDataQueue = xQueueCreate(TOF_QUEUE_LENGTH, sizeof(tofMeasurement_t));
heightDataQueue = xQueueCreate(HEIGHT_QUEUE_LENGTH, sizeof(heightMeasurement_t));
yawErrorDataQueue = xQueueCreate(YAW_ERROR_QUEUE_LENGTH, sizeof(float));

vSemaphoreCreateBinary(measurementQueueSemaphore);
xSemaphoreGive(measurementQueueSemaphore);

dataMutex = xSemaphoreCreateMutex();

xTaskCreate(kalmanTask, KALMAN_TASK_NAME, 3 * configMINIMAL_STACK_SIZE, NULL, KALMAN_TASK_PRI, NULL);

isInit = true;
}

bool estimatorKalmanTaskTest() {
return isInit;
}

static void kalmanTask(void* parameters) {
systemWaitStart();

uint32_t lastPrediction = xTaskGetTickCount();
Expand Down Expand Up @@ -539,27 +564,14 @@ static bool updateQueuedMeasurments(const Axis3f *gyro) {
}

void estimatorKalmanInit(void) {
if (!isInit)
{
distDataQueue = xQueueCreate(DIST_QUEUE_LENGTH, sizeof(distanceMeasurement_t));
posDataQueue = xQueueCreate(POS_QUEUE_LENGTH, sizeof(positionMeasurement_t));
poseDataQueue = xQueueCreate(POSE_QUEUE_LENGTH, sizeof(poseMeasurement_t));
tdoaDataQueue = xQueueCreate(UWB_QUEUE_LENGTH, sizeof(tdoaMeasurement_t));
flowDataQueue = xQueueCreate(FLOW_QUEUE_LENGTH, sizeof(flowMeasurement_t));
tofDataQueue = xQueueCreate(TOF_QUEUE_LENGTH, sizeof(tofMeasurement_t));
heightDataQueue = xQueueCreate(HEIGHT_QUEUE_LENGTH, sizeof(heightMeasurement_t));
yawErrorDataQueue = xQueueCreate(YAW_ERROR_QUEUE_LENGTH, sizeof(float));
}
else
{
xQueueReset(distDataQueue);
xQueueReset(posDataQueue);
xQueueReset(poseDataQueue);
xQueueReset(tdoaDataQueue);
xQueueReset(flowDataQueue);
xQueueReset(tofDataQueue);
}
xQueueReset(distDataQueue);
xQueueReset(posDataQueue);
xQueueReset(poseDataQueue);
xQueueReset(tdoaDataQueue);
xQueueReset(flowDataQueue);
xQueueReset(tofDataQueue);

xSemaphoreTake(dataMutex, portMAX_DELAY);
accAccumulator = (Axis3f){.axis={0}};
gyroAccumulator = (Axis3f){.axis={0}};
thrustAccumulator = 0;
Expand All @@ -569,26 +581,17 @@ void estimatorKalmanInit(void) {
gyroAccumulatorCount = 0;
thrustAccumulatorCount = 0;
baroAccumulatorCount = 0;
xSemaphoreGive(dataMutex);

kalmanCoreInit(&coreData);

vSemaphoreCreateBinary(measurementQueueSemaphore);
xSemaphoreGive(measurementQueueSemaphore);

dataMutex = xSemaphoreCreateMutex();

const uint32_t now_ms = T2M(xTaskGetTickCount());
statsCntReset(&statsUpdates, now_ms);
statsCntReset(&statsPredictions, now_ms);
statsCntReset(&statsBaroUpdates, now_ms);
statsCntReset(&statsFinalize, now_ms);
statsCntReset(&statsUMeasurementAppended, now_ms);
statsCntReset(&statsUMeasurementNotAppended, now_ms);

xTaskCreate(kalmanTask, KALMAN_TASK_NAME, 3 * configMINIMAL_STACK_SIZE, NULL,
KALMAN_TASK_PRI, NULL);

isInit = true;
}

static bool appendMeasurement(xQueueHandle queue, void *measurement)
Expand Down
3 changes: 3 additions & 0 deletions src/modules/src/system.c
Original file line number Diff line number Diff line change
Expand Up @@ -61,6 +61,7 @@
#include "buzzer.h"
#include "sound.h"
#include "sysload.h"
#include "estimator_kalman.h"
#include "deck.h"
#include "extrx.h"
#include "app.h"
Expand Down Expand Up @@ -166,6 +167,7 @@ void systemTask(void *arg)
commanderInit();

StateEstimatorType estimator = anyEstimator;
estimatorKalmanTaskInit();
deckInit();
estimator = deckGetRequiredEstimator();
stabilizerInit(estimator);
Expand All @@ -186,6 +188,7 @@ void systemTask(void *arg)
pass &= commTest();
pass &= commanderTest();
pass &= stabilizerTest();
pass &= estimatorKalmanTaskTest();
pass &= deckTest();
pass &= soundTest();
pass &= memTest();
Expand Down

0 comments on commit 821e249

Please sign in to comment.