TauLabs/TauLabs — pull request #1063 by peabody124 (2013-12-30 23:06:45+00:00)
Title: Filter architecture

This is a follow up on #687. It introduces a new filter architecture that is designed to make it easy for to develop new filters and focus on the math while abstracting as much of our underlying architecture away as possible.

I used this to implement two filters:
- the existing complementary filter
- a complementary attitude filter with the ardupilot complementary inertial filter for position and velocity

The second I have tested with navigation and it seems to work quite nicely. It also has the benefit that it is much simpler and more intuitive to tune and get flying.

It is disabled by default as the existing implementation is more mature but It currently compiles for all platforms:
make V=1 FILTER_ARCHITECTURE=YES -j8 all_flight

will bring it in. The complementary filter has been ported to this new API already and has been flight tested (including position hold) on Sparky.

flight/Libraries/filters/template_se3_interface.c provides a minimal example for getting started with a new filter. flight/Libraries/filters/cf_interface.c provides a working example of a complementary filter (new position updates).

The filters can be extremely general and provide no infrastructure - just a series of hooks to write a get data / update / set data method. However, a core infrastructure is provided for SE(3) filters (position, velocity, attitude as well as gyro bias) which is probably the most prevalent for our needs. For this type of filter the main thing required to write a new filter is simply to fill out this method:

```
/**
* get_sensors Update the filter one time step
* @param[in] id the running filter handle
* @param[in] gyros new gyro data [rad/s] or NULL
* @param[in] accels new accel data [m/s^2] or NULL
* @param[in] mag new mag data [mGau] or NULL
* @param[in] pos new position measurement in NED [m] or NULL
* @param[in] vel new velocity meansurement in NED [m/s] or NULL
* @param[in] baro new baro data [m] or NULL
* @param[in] airspeed estimate of the airspeed
* @param[in] dt time step [s]
* @returns 0 if sufficient data to run update
*/
static int32_t template_interface_update(uintptr_t id, float gyros[3], float accels[3],
float mag[3], float pos[3], float vel[3], float baro[1],
float airspeed[1], float dt)
{
struct template_interface_data *h = (struct template_interface_data *) id;
if (!template_interface_validate(h))
return -1;

// This architecture currently always provides both but sanity check this
if (accels == NULL || gyros == NULL)
return -1;

/********************************************/
/* TODO: perform a filter update here */
/* The inputs that are not null indicate */
/* new data as documented above */
/* */
/* local data should be stored in the h */
/* structure which is defined above */
/********************************************/

if (true)
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL);
else
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);

return 0;
}
```