A much simpler solution might be to call the i2c routines from a ticker object. See below code.
I had an I2C interface that would stop if my fingers got close to the wires ( adding capacitance? ) and I pulled my hair out trying to do a non-blocking read or implementing a timeout read. I was calling the I2c routines from a loop in main(). When the I2C bus stopped, it froze, locked up, or otherwise blocked the processor. (search keywords: i2c freeze lockup hang )
Using a ticker is much easier, and it recovers from a locked state well. It also does not block the processing of the main loop, so your robot can take action (like stopping and putting the hazard lights on...)
This technique also works for serial devices, spi, etc. - basically anything that can stop your processor should be done in a ticker. You won't need the complications of trying to run a real-time os or deal with watchdog timers.
moving read i2c sensors to a ticker
Timer ticktock;
int gGlobalSonarReading =0;
bool new_sonar_reading=false;
int last_sonar_reading_time = 0;
void my_read_sonar()
{
// send the commands to write then read the sonar here.....
// aka sonar.write(addr,cmd,2);
// aka sonar.read(read(readCmd, buffer, 6 ););
last_read_sonar_time = ticktock.read_us();
new_sonar_reading = true;
}
//////////////////////////////////////////////////////
////////////////////// old main()
///////////////////////////////////////////////////////
main()
{
ticktock.start(); //for getting the current time...
...
while(1) {
l1=!l1;
my_read_sonar();
wait(0.2);
}
...
}
//////////////////////////////////////////////////////
////////////////////// change main to....
///////////////////////////////////////////////////////
Ticker read_i2c_sonar;
int desired_hertz = 10; // 10 hz, ten times a second
int main()
{
//attach the read function to the ticker, will automatically be called at the desired interval
read_i2c_sonar.attach(&read_i2c_sonar, (1.0/desired_hertz));
// ...
while(1) {
if ( new_sonar_reading ) {
do_something_with_sonar_reading();
new_sonar_reading = false;
// you can also check the timestamp the sonar reading when you read it, and check the timestamp against the current time
// if the sonar timestamp is much smaller than the current time, perhaps there's a more serious problem
// if ( (ticktock.read_us() - last_sonar_reading_time ) > 100000) { /* handle bad sensor... */ }
}
wait(0.2);
}
// ...
}
I also had a regular I2C freeze while using an 24LC64 E2Prom and a FM24LC64 FRAM device on the same bus.
My solution which appears to have completely eliminated it is as follows.