State Estimator Sensor Push Redesign

[whoenig]

The current design of the state estimators has multiple issues:

1. IMU sensor data is not pushed, but pulled by each state estimator. This has weird side effects, for example, the barometer logging variables are not valid if the state estimator does not need it. It also makes it harder to log the data that is put into the filter.
2. Each new state estimator needs to provide functions to enqueue sensor data, as well as keep track of the enqueued data separately. Most (advanced) state estimators will have similar implementations of such functionality.
3. The current way of enqueuing data in the Kalman filter loses the order of the sensor data, since separate queues for sensors are used. In general, the order might be important, especially for slower filters.

A possible solution to address these issues is:

• In estimator.c add a new unified queue:

struct measurement_s {
measurement_type_e type;
union {
  distanceMeasurement_t distance;
  positionMeasurement_t position;
  // ...
} data;
};
static xQueueHandle measurementsQueue;
STATIC_MEM_QUEUE_ALLOC(measurementsQueue, 50, sizeof(struct measurement_s));

• In estimator.c, we can now fill definitions for all the enqueue functions to just push data in the queue. In all state estimators, we simply extract items from the queue and ignore measurements that we do not want to handle. There is no need anymore to define additional enqueue functions in each state estimator. This solves both problems 2) and 3).
• In the various sensor implementations, we push the measurements to the stateEstimator, similar to the LH deck. Note that we need to do that in the sensor implementation directly, because the barometer is updated at a different frequency. This fixes issue 1).
• In stabilizer.c, we always call sensorsAcquire(&sensorData, tick); to ensure that current logging variables are updated. In the future, we should eliminate the sensorData struct entirely, but this would be the scope of a separate issue.

[whoenig]

@ataffanel @krichardsson It would be nice if you can comment on this design, before I work on an actual implementation of it.

[krichardsson]

Looking good! Pushing the IMU data the same was as other sensors would be nice and we could get rid of some nasty dependencies as well.

One property that is lost in this design is "prioritisation" between sensor types. Even though it is not utilised in a structured way today, it is possible to set the queue length for different sensors. If one sensor pushes lots of data (too much) into the queue today, it will be discarded while other sensor data still will be used as it is pushed to other queues. In the suggested design we might discard the "wrong" sensor data instead. One may argue that we should have a different mechanism to throttle sensor data anyway.

[whoenig]

There was a in-person discussion about the issue of dropping data: We will add code to detect if the queue overflows and warn the user (only once!) using a DEBUG_PRINT. Then, the queue size in the firmware should be increased accordingly to find a good length. For the current implementation, we do not have such warnings, so we actually don't know if our queues are too large or too small.