Began implementing a combined DMP and complementary filter angle calculation system

The DMP lags a bit, but is far more accurate than a complementary
filter, so I’m trying to combine them to be fast and accurate
LOTS of cleaning house....