Portanto... tens este código todo e não tens sensores?
Não seria melhor primeiro ver o que dão os sensores e depois comecar a juntar as coisas? Algo que se calhar pode limitar um pouco o sistema é a escrita para o cartão. Isso talvez demore um pouco mais e limite a frequência de amostragem. Também reparei que usas timers... e uma interrupcão algo complexa. Isso pode tornar dificil fazer as medicões sempre no mesmo intervalo...
Algo que também notei é a ausência de filtros... Mas isso pode ser feito à posteriori já que não vais controlar nada.
Depois mostra o resultado final.