taskI2C: use Fastwire library for I2C communication instead of Wire

In certain cases, Wire can get stuck in an infinite loop.

Example of such a case: connect the SDA bus to ground (with the other
MPUs connected as well). Wire.endTransmission() will enter an infinite
loop with no way to stop it (except resetting the Arduino).

Now, this is a very bad thing because taskI2C has the highest task
priority -- if it enters an infinite loop, all other tasks cannot run
and it will seem like the Arduino crashed.

To fix this, use the Fastwire library, which does not seem to have this
problem.

Author: pyokagan

arduino/Makefile

@@ -2,5 +2,6 @@ BOARD_TAG = mega
 BOARD_SUB = atmega2560
 MONITOR_BAUDRATE=115200
 ARDUINO_SKETCHBOOK = ../
-ARDUINO_LIBS = Arduino_FreeRTOS_Library ucomm Wire I2Cdev MPU6050 TimerOne
+ARDUINO_LIBS = Arduino_FreeRTOS_Library ucomm I2Cdev MPU6050 TimerOne
+CPPFLAGS=-DI2CDEV_IMPLEMENTATION=3
 include ../.Arduino-Makefile/Arduino.mk

arduino/taskI2C.cpp

@@ -1,6 +1,5 @@
 #include "taskI2C.h"
 #include "taskComm.h"
-#include <Wire.h>
 #include <I2Cdev.h>
 #include <MPU6050.h>
 #include <stdbool.h>
@@ -51,7 +50,7 @@ isMpu2Zeroes(const ucomm_Sample *sample)
 void
 taskI2C_setup()
 {
-    Wire.begin();
+    Fastwire::setup(100, true);
 
     mpu1Active = setupMpu(&mpu1);
     mpu2Active = setupMpu(&mpu2);