Tag Archives: Teensy

I2C between an Arduino Mega and a Teensy 3.x

posted 18 March 2020

This post describes my efforts to troubleshoot an I2C communications problem between an Arduino Mega control board and a Teensy 3.2 IR beam demodulation module.

Background:

My Wall-E2 autonomous wall-following robot homes in on a modulated IR beacon to connect to a charging station. The IR beacon modulation is decoded by a dedicated Teensy 3.2 and provides left/right and combined steering values to an Arduino Mega main controller over an I2C link.

During my recent work to update the robot after some enhancements, I discovered that the main controller was no longer receiving steering information from the Teensy, even though both seemed to be operating properly. Initial efforts to troubleshoot the problem did not bear fruit, so I was forced to back up and start over from scratch.

Troubleshooting:

Initially I thought the problem was a loose connection, as the system was working before. However, I am now pretty sure that I have eliminated all the obvious culprits, so I am left with the non-obvious ones.

To start with, I resurrected an old example project to demonstrate I2C master/slave operation between two Teensy modules.

Teensy 3.2 Slave (left) and Teensy 3.5 Master (right)

Here’s the Teensy ‘Master’ code:

// -------------------------------------------------------------------------------------------
// Basic Master
// -------------------------------------------------------------------------------------------
//
// This creates a simple I2C Master device which when triggered will send/receive a text 
// string to/from a Slave device.  It is intended to pair with a Slave device running the 
// basic_slave sketch.
//
// Pull pin12 input low to send.
// Pull pin11 input low to receive.
//
// This example code is in the public domain.
//
// -------------------------------------------------------------------------------------------

#include <i2c_t3.h>
#include <T3_I2C_Anything.h>

// Memory
#define MEM_LEN 256
char databuf[MEM_LEN];
int count;
float xmtfloat = 1.2344;

void setup()
{
	pinMode(LED_BUILTIN, OUTPUT);    // LED
	digitalWrite(LED_BUILTIN, LOW);  // LED off
	pinMode(12, INPUT_PULLUP);       // Control for Send
	pinMode(11, INPUT_PULLUP);       // Control for Receive

									 // Setup for Master mode, pins 18/19, external pullups, 400kHz, 200ms default timeout
	//Wire.begin(I2C_MASTER, 0x00, I2C_PINS_18_19, I2C_PULLUP_EXT, 400000);
	Wire.begin(I2C_MASTER, 0x00, I2C_PINS_18_19, I2C_PULLUP_INT, 400000);
	Wire.setDefaultTimeout(200000); // 200ms

	// Data init
	memset(databuf, 0, sizeof(databuf));
	count = 0;

	//Serial.begin(115200);
	unsigned long now = millis();
	Serial.begin(1); //rate value ignored
	int idx = 0;
	while (!Serial && (millis() - now) < 3000)
	{
		delay(500);
		idx++;
	}
	Serial.printf("Serial available after %lu mSec\n", millis() - now);
	Serial.printf("loop idx = %d\n", idx);

	Serial.printf("I2C_Master Example\n");
	Serial.printf("\nGround Pin 12 to transmit a float value to the slave\n");
	Serial.printf("\nGround Pin 11 to request a float value from the slave\n\n");
	
}

void loop()
{
	count++;
	uint8_t target = 0x66; // target Slave address
	//size_t idx;

	// Send string to Slave
	//
	if (digitalRead(12) == LOW)
	{
		digitalWrite(LED_BUILTIN, HIGH);   // LED on

		// Transmit to Slave
		//Serial.printf("Sending to Slave: %3.3f\n", xmtfloat*count);
		float a = xmtfloat * count;
		Serial.printf("Sending to Slave: %3.6f (0x%x)\n", a , *(unsigned int*)&a);
		Wire.beginTransmission(target);   // Slave address
		I2C_writeAnything(xmtfloat*count);
		Wire.endTransmission();           // Transmit to Slave

		// Check if error occured
		if (Wire.getError())
			Serial.print("FAIL\n");
		else
			Serial.print("OK\n");

		digitalWrite(LED_BUILTIN, LOW);    // LED off
		delay(100);                       // Delay to space out tests
	}

	// Read string from Slave
	if (digitalRead(11) == LOW)
	{
		digitalWrite(LED_BUILTIN, HIGH);   // LED on
										   
		Serial.print("Reading from Slave: ");// Print message

		// Read from Slave
		Wire.requestFrom(target, (size_t)MEM_LEN); // Read from Slave (string len unknown, request full buffer)

		if (Wire.getError())
			Serial.print("FAIL\n");
		else
		{
			float slave_val = 0;
			I2C_readAnything(slave_val);
			Serial.printf("Received %f from Slave\n", slave_val);
		}

		digitalWrite(LED_BUILTIN, LOW);    // LED off
		delay(100);                       // Delay to space out tests
	}
}

100

101

102

103

104

105

106

107

108

109

110

111

// -------------------------------------------------------------------------------------------

// Basic Master

// -------------------------------------------------------------------------------------------

// This creates a simple I2C Master device which when triggered will send/receive a text

// string to/from a Slave device. It is intended to pair with a Slave device running the

// basic_slave sketch.

// Pull pin12 input low to send.

// Pull pin11 input low to receive.

// This example code is in the public domain.

// -------------------------------------------------------------------------------------------

#include <i2c_t3.h>

#include <T3_I2C_Anything.h>

// Memory

#define MEM_LEN 256

char databuf[MEM_LEN];

int count;

float xmtfloat = 1.2344;

void setup()

{

pinMode(LED_BUILTIN, OUTPUT); // LED

digitalWrite(LED_BUILTIN, LOW); // LED off

pinMode(12, INPUT_PULLUP); // Control for Send

pinMode(11, INPUT_PULLUP); // Control for Receive

// Setup for Master mode, pins 18/19, external pullups, 400kHz, 200ms default timeout

//Wire.begin(I2C_MASTER, 0x00, I2C_PINS_18_19, I2C_PULLUP_EXT, 400000);

Wire.begin(I2C_MASTER, 0x00, I2C_PINS_18_19, I2C_PULLUP_INT, 400000);

Wire.setDefaultTimeout(200000); // 200ms

// Data init

memset(databuf, 0, sizeof(databuf));

count = 0;

//Serial.begin(115200);

unsigned long now = millis();

Serial.begin(1); //rate value ignored

int idx = 0;

while (!Serial && (millis() - now) < 3000)

{

delay(500);

idx++;

}

Serial.printf("Serial available after %lu mSec\n", millis() - now);

Serial.printf("loop idx = %d\n", idx);

Serial.printf("I2C_Master Example\n");

Serial.printf("\nGround Pin 12 to transmit a float value to the slave\n");

Serial.printf("\nGround Pin 11 to request a float value from the slave\n\n");

}

void loop()

{

count++;

uint8_t target = 0x66; // target Slave address

//size_t idx;

// Send string to Slave

if (digitalRead(12) == LOW)

{

digitalWrite(LED_BUILTIN, HIGH); // LED on

// Transmit to Slave

//Serial.printf("Sending to Slave: %3.3f\n", xmtfloat*count);

float a = xmtfloat * count;

Serial.printf("Sending to Slave: %3.6f (0x%x)\n", a , *(unsigned int*)&a);

Wire.beginTransmission(target); // Slave address

I2C_writeAnything(xmtfloat*count);

Wire.endTransmission(); // Transmit to Slave

// Check if error occured

if (Wire.getError())

Serial.print("FAIL\n");

else

Serial.print("OK\n");

digitalWrite(LED_BUILTIN, LOW); // LED off

delay(100); // Delay to space out tests

}

// Read string from Slave

if (digitalRead(11) == LOW)

{

digitalWrite(LED_BUILTIN, HIGH); // LED on

Serial.print("Reading from Slave: ");// Print message

// Read from Slave

Wire.requestFrom(target, (size_t)MEM_LEN); // Read from Slave (string len unknown, request full buffer)

if (Wire.getError())

Serial.print("FAIL\n");

else

{

float slave_val = 0;

I2C_readAnything(slave_val);

Serial.printf("Received %f from Slave\n", slave_val);

}

digitalWrite(LED_BUILTIN, LOW); // LED off

delay(100); // Delay to space out tests

}

And the Teensy ‘Slave’ code:

	// -------------------------------------------------------------------------------------------
	// Basic Slave
	// -------------------------------------------------------------------------------------------
	//
	// This creates a simple I2C Slave device which will print whatever text string is sent to it.
	// It will retain the text string in memory and will send it back to a Master device if 
	// requested.  It is intended to pair with a Master device running the basic_master sketch.
	//
	// This example code is in the public domain.
	//
	// -------------------------------------------------------------------------------------------

/*
	03/07/20 Modified to be Teensy-specific, and to use 'I2C_Anything' for I2C communications
*/
#include <i2c_t3.h>
#include <T3_I2C_Anything.h>

// Function prototypes
void receiveEvent(size_t count);
void requestEvent(void);

// Memory
volatile uint8_t received;
volatile float rcvfloat = 0;

//
// Setup
//
void setup()
{
	pinMode(LED_BUILTIN, OUTPUT); // LED

	// Setup for Slave mode, address 0x66, pins 18/19, internal pullups, 400kHz
	//Wire.begin(I2C_SLAVE, 0x66, I2C_PINS_18_19, I2C_PULLUP_INT, 400000);
	//Wire.begin(I2C_SLAVE, 0x66, I2C_PINS_18_19, I2C_PULLUP_INT, 100000);
	Wire.begin(I2C_SLAVE, 0x66, I2C_PINS_18_19, I2C_PULLUP_EXT, 100000);

	// register events
	Wire.onReceive(receiveEvent);
	Wire.onRequest(requestEvent);

	//Serial.begin(115200);
	unsigned long now = millis();
	Serial.begin(1); //rate value ignored
	int idx = 0;
	while (!Serial && (millis() - now) < 3000)
	{
		delay(500);
		idx++;
	}

	Serial.printf("Serial available after %lu mSec\n", millis() - now);
	Serial.printf("I2C Slave Example\n");
}

void loop()
{
	// print received data - this is done in main loop to keep time spent in I2C ISR to minimum
	if (received)
	{
		digitalWrite(LED_BUILTIN, HIGH);
		Serial.printf("Slave received: %f\n", rcvfloat);
		received = 0;
		digitalWrite(LED_BUILTIN, LOW);
	}
}

//
// handle Rx Event (incoming I2C data)
//
void receiveEvent(size_t count)
{
	received = count; // set received flag to count, this triggers print in main loop
	I2C_readAnything(rcvfloat);
}

//
// handle Tx Event (outgoing I2C data)
//
void requestEvent(void)
{
	Serial.printf("Sending %f to Master\n", rcvfloat);

	const byte* p = (const byte*)&rcvfloat;
	unsigned int i;
	for (i = 0; i < sizeof rcvfloat; i++)
		Wire.write(*p++);
	delay(5); //experiment
	//I2C_writeAnything(rcvfloat);
}

// -------------------------------------------------------------------------------------------

// Basic Slave

// -------------------------------------------------------------------------------------------

// This creates a simple I2C Slave device which will print whatever text string is sent to it.

// It will retain the text string in memory and will send it back to a Master device if

// requested. It is intended to pair with a Master device running the basic_master sketch.

// This example code is in the public domain.

// -------------------------------------------------------------------------------------------

03/07/20 Modified to be Teensy-specific, and to use 'I2C_Anything' for I2C communications

#include <i2c_t3.h>

#include <T3_I2C_Anything.h>

// Function prototypes

void receiveEvent(size_t count);

void requestEvent(void);

// Memory

volatile uint8_t received;

volatile float rcvfloat = 0;

// Setup

void setup()

{

pinMode(LED_BUILTIN, OUTPUT); // LED

// Setup for Slave mode, address 0x66, pins 18/19, internal pullups, 400kHz

//Wire.begin(I2C_SLAVE, 0x66, I2C_PINS_18_19, I2C_PULLUP_INT, 400000);

//Wire.begin(I2C_SLAVE, 0x66, I2C_PINS_18_19, I2C_PULLUP_INT, 100000);

Wire.begin(I2C_SLAVE, 0x66, I2C_PINS_18_19, I2C_PULLUP_EXT, 100000);

// register events

Wire.onReceive(receiveEvent);

Wire.onRequest(requestEvent);

//Serial.begin(115200);

unsigned long now = millis();

Serial.begin(1); //rate value ignored

int idx = 0;

while (!Serial && (millis() - now) < 3000)

{

delay(500);

idx++;

}

Serial.printf("Serial available after %lu mSec\n", millis() - now);

Serial.printf("I2C Slave Example\n");

}

void loop()

{

// print received data - this is done in main loop to keep time spent in I2C ISR to minimum

if (received)

{

digitalWrite(LED_BUILTIN, HIGH);

Serial.printf("Slave received: %f\n", rcvfloat);

received = 0;

digitalWrite(LED_BUILTIN, LOW);

}

// handle Rx Event (incoming I2C data)

void receiveEvent(size_t count)

{

received = count; // set received flag to count, this triggers print in main loop

I2C_readAnything(rcvfloat);

}

// handle Tx Event (outgoing I2C data)

void requestEvent(void)

{

Serial.printf("Sending %f to Master\n", rcvfloat);

const byte* p = (const byte*)&rcvfloat;

unsigned int i;

for (i = 0; i < sizeof rcvfloat; i++)

Wire.write(*p++);

delay(5); //experiment

//I2C_writeAnything(rcvfloat);

}

With this configuration I got the following output:

Master:

Opening port
Port open
Sending to Slave: 812935680.000000 (0x4e41d1a8)
OK
Sending to Slave: 812935680.000000 (0x4e41d1a8)
OK
Sending to Slave: 812935680.000000 (0x4e41d1a8)
OK
Sending to Slave: 812935680.000000 (0x4e41d1a8)
OK
...
...
Reading from Slave: Received 812935744.000000 from Slave
Reading from Slave: Received 812935744.000000 from Slave
Reading from Slave: Received 812935744.000000 from Slave
Reading from Slave: Received 812935744.000000 from Slave
Reading from Slave: Received 812935744.000000 from Slave

Opening port

Port open

Sending to Slave: 812935680.000000 (0x4e41d1a8)

...

Reading from Slave: Received 812935744.000000 from Slave

Slave:

Opening port
Port open
Slave received: 812935680.000000
Slave received: 812935680.000000
Slave received: 812935680.000000
Slave received: 812935680.000000
...
...
Sending 812935744.000000 to Master
Sending 812935744.000000 to Master
Sending 812935744.000000 to Master
Sending 812935744.000000 to Master

Opening port

Port open

Slave received: 812935680.000000

...

Sending 812935744.000000 to Master

So this seems to be working OK.

Then I added in a third Teensy module (T3.5) running my newly developed I2C-Sniffer code, so that I could ‘sniff’ the I2C traffic between the two Teensy’s. This will give me a ‘known-good’ baseline for when I move back to the non-working Arduino Mega Master and Teensy Slave condition that is causing me problems.

Teensy 3.2 Slave (left), Teensy 3.5 I2C Sniffer (middle), Teensy 3.5 Master (right)

Here’s the Teensy ‘Sniffer’ code:

/*
    Name:       Teensy_I2C_Sniffer_V11.ino
    Created:	1/18/2020 10:55:55 AM
    Author:     FRANKNEWXPS15\Frank
*/
/* 'Notes:

    A typical I2C sentence when communicating with a MPU6050 IMU module goes something like:
        "I2C(68) wrote 1 byte to 75 - C0 Done."
        "I2C(68) wrote 3 bytes to 72 - C0 0C 10 Done."
        "I2C(68) read 5 bytes from 6A - C0 0C 10 14 03 Done."

    To form a sentence, we need:
        Device addr: 68 in the above examples
        Read/Write direction
        To/From register address:  75, 72 and 6A in the above examples
        Data:  C0, C0 0C 10, and C0 0C 10 14 03 in the above examples
        number of bytes written/read:  1,3 & 5 in the above examples

     Each I2C communication proceeds as follows (assuming a START from an IDLE condition):
         A START or RESTART condition, denoted by SDA & SCL HIGH, followed by SDA LOW, SCL HIGH
         A 7-bit device address, MSB first (0x8/0xC = 1, 0x0/0x4 = 0)
         A R/W bit (0x8/0xC = read, 0x0/0x4 = write)
         An ACK bit (0x8/0xC = NAK, 0x0/0x4 = ACK)
         If the bus direction is WRITE, then
             A register address for read/write
             zero or more additional data bytes
         Else (the bus direction is READ)
            One or more additional data bytes
         Endif

    This version uses a fixed-size (2048 bytes) array instead of tonton81's circular buffer library.

    To generalize for any I2C slave device rather than just the MPU6050 IMU, comment out the 
    "#define MPU6050_SPECIFIC line below. This will remove all MPU6050 specific code
*/


//#define MPU6050_SPECIFIC

#include <TimerOne.h> //needed for ISR
#ifdef MPU6050_SPECIFIC
#include "helper_3dmath.h" //Arduino\Libraries\i2cdevlib\Arduino\MPU6050\ needed to compute yaw from MPU6050 DMP packet
#endif


#define PARSE_LOOP_DEBUG

const uint16_t CAPTURE_ARRAY_SIZE = 2048;
const uint16_t VALID_DATA_ARRAY_SIZE = 2048;
const int WAITING_PRINT_INTERVAL_MSEC = 200;//interval timer for 'Waiting for data...' printout

#define MONITOR_OUT1 2 //so can monitor ISR activity with O'scope
#define MONITOR_OUT2 3 //so can monitor ISR activity with O'scope
#define MONITOR_OUT3 4 //so can monitor ISR activity with O'scope
#define SDA_PIN 18 
#define SCL_PIN 19

#pragma region PROCESSING_VARIABLES
uint8_t devAddr;
uint8_t regAddr;
uint8_t databytes[2048]; //holds multiple databytes for later output sentence construction
uint16_t numbytes = 0; //number of data bytes extracted from data stream
int ACKNAKFlag; //can be negative
uint16_t databyte_idx = 0; //index into databyte_array
uint8_t killbuff[2]; //used to consume start/stop bytes
elapsedMillis mSecSinceLastWaitingPrint;
uint8_t valid_data[2048];
uint16_t numvalidbytes = 0; //number of valid bytes in this burst
uint16_t read_idx = 0; //pointer to next byte pair to be processed

//added for bus direction labels
enum BUSDIR
{
    WRITE,
    READ,
    UNKNOWN = -1
} RWDir;
BUSDIR BusDir = BUSDIR::UNKNOWN;
#pragma endregion ProcVars


#pragma region ISR_SUPPORT
uint8_t raw_data[CAPTURE_ARRAY_SIZE]; //holds data captured from I2C bus
volatile uint16_t  write_idx = 0;
volatile uint8_t   current_portb = 0xFF;
volatile uint8_t   last_portb = 0xFF;
volatile uint16_t mult0xCCount = 0;
const uint16_t MAX_IDLE_COUNT = 2500;

volatile bool bDone = false;
volatile bool bWaitingForStart = true;
volatile bool bIsData = true;
volatile bool bIsStart = false;
volatile bool bIsStop = false;
volatile uint8_t last_current;
#pragma endregion ISR Support

#ifdef MPU6050_SPECIFIC
//01/21/20 these forward declarations are required to force the preprocessor 
//to handle #ifdef MPU6050_SPECIFIC properly
uint8_t dmpGetQuaternion(int16_t* data, const uint8_t* packet);
uint8_t dmpGetQuaternion(Quaternion* q, const uint8_t* packet);
uint8_t dmpGetYawPitchRoll(float* data, Quaternion* q, VectorFloat* gravity);
uint8_t dmpGetGravity(VectorFloat* v, Quaternion* q);

Quaternion q;           // [w, x, y, z]         quaternion container
VectorInt16 aa;         // [x, y, z]            accel sensor measurements
VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
VectorFloat gravity;    // [x, y, z]            gravity vector
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
float global_yawval = 0; //updated by GetIMUHeadingDeg()
#endif // MPU6050_SPECIFIC

void setup()
{
    unsigned long now = millis();
    Serial.begin(1); //rate value ignored
    int idx = 0;
    while (!Serial && (millis() - now) < 3000)
    {
        delay(500);
        idx++;
    }
    Serial.printf("Serial available after %lu mSec\n", millis() - now);

    pinMode(MONITOR_OUT1, OUTPUT);
    digitalWrite(MONITOR_OUT1, LOW);
    pinMode(MONITOR_OUT2, OUTPUT);
    digitalWrite(MONITOR_OUT2, LOW);
    pinMode(MONITOR_OUT3, OUTPUT);
    digitalWrite(MONITOR_OUT3, LOW);

    pinMode(SCL_PIN, INPUT);
    pinMode(SDA_PIN, INPUT);

    //reset port byte vars & start timer
    last_portb = current_portb = 0;
    write_idx = 0;
    memset(raw_data, 255, CAPTURE_ARRAY_SIZE);
    //PrintNextArrayBytes(raw_data, 255, 20);
    Timer1.initialize(1); // run every mico second
    Timer1.attachInterrupt(capture_data);


    mSecSinceLastWaitingPrint = 0;
}
//-------------------------------------------------------------------------------
//--------------------------------    ISR    ------------------------------------
//-------------------------------------------------------------------------------
FASTRUN void capture_data()
//void capture_data()
{
    last_portb = current_portb;
    current_portb = GPIOB_PDIR & 12; //reads state of SDA (18) & SCL (19) at same time

    if (!bDone && last_portb != current_portb)
    {
        mult0xCCount = 0; //reset IDLE counter
        digitalWriteFast(MONITOR_OUT1, HIGH);

        //01/17/20: joepasquariello suggestion
        last_current = (last_portb << 4) | (current_portb);
        bIsStart = (last_current == 0xC4);
        bIsStop = (last_current == 0x4C);
        bIsData = (last_current == 0x04) || (last_current == 0x8C);

        if (bIsStart) //START  
        {
            digitalWriteFast(MONITOR_OUT2, HIGH);
            if (bWaitingForStart)
            {
                digitalWriteFast(MONITOR_OUT3, HIGH); //start of entire capture
                bWaitingForStart = false;
            }
        }
        else if (bIsStop) //STOP
        {
            digitalWriteFast(MONITOR_OUT2, LOW);
        }

        if (!bWaitingForStart && (bIsData || bIsStart || bIsStop))
        {
            //digitalWriteFast(MONITOR_OUT3, HIGH);
            raw_data[write_idx] = last_portb;
            write_idx++;
            raw_data[write_idx] = current_portb;
            write_idx++;
            if (write_idx >= CAPTURE_ARRAY_SIZE)
            {
                bDone = true;
                digitalWriteFast(MONITOR_OUT3, LOW);
            }
        }
        digitalWriteFast(MONITOR_OUT1, LOW);
    }
    else if (!bDone && mult0xCCount < MAX_IDLE_COUNT && last_portb == 0xc && current_portb == 0xc)
    {
        mult0xCCount++;
        if (mult0xCCount >= MAX_IDLE_COUNT)
        {
            digitalWriteFast(MONITOR_OUT3, LOW);
            bDone = true;
        }
    }
}
//-------------------------------------------------------------------------------
//-------------------------------- END ISR    ---------------------------------
//-------------------------------------------------------------------------------

void loop()
{
    if (bDone)
    { 
        if (write_idx > 14)
        {
            //OK, we have some data to process. IDLE detection must have been EOM
            Timer1.stop();

            unsigned long startMsec = millis();

            //Serial.printf("%lu\t %d\t", millis(), write_idx);
            //PrintNextArrayBytes(raw_data, 0, 50);
            //Serial.printf(" - %lu\n", millis());
            uint16_t numprocessed = DecodeAndPrintValidData(raw_data); //decode and print everything captured so far
            unsigned long endMsec = millis();
            Serial.printf("%lu: processed = %d elements in %lu mSec\n\n", startMsec, numprocessed, endMsec-startMsec);

            Timer1.start();
        }

        read_idx = 0;
        bDone = false;
        mult0xCCount = 0;
        write_idx = 0;
        bWaitingForStart = true;
    }    
    else
    {
        //no data to process, but don't blow prints out every mSec...
        if (mSecSinceLastWaitingPrint > WAITING_PRINT_INTERVAL_MSEC)
        {
            mSecSinceLastWaitingPrint -= WAITING_PRINT_INTERVAL_MSEC;
            Serial.printf("%lu: Waiting for Data...\n", millis());
        }
    }
}

void PrintNextArrayBytes(uint8_t* data, uint16_t startidx, uint16_t numbytes)
{
    Serial.printf("%d bytes starting at %d: ", numbytes, startidx);
    for (uint16_t i = 0; i < numbytes; i++)
    {
        Serial.printf("%x ", data[i + startidx]);
    }
}


uint16_t DecodeAndPrintValidData(byte* data)
{
    //Purpose:  decode and print I2C conversation held in raw_data array
    //Inputs:  
    //  cb = 2048 element FIFO
    //Outputs:
    //  returns number of bytes processed, or -1 for failure
    //  outputs structured I2C sentence to serial monitor
    //Plan:
    //  Step1: Cull out invalid bytes
    //  Step2: Determine if there is anything to do (have to have more than one transition in FIFO)
    //  Step3: Parse transitions into I2C sentence structure
    //  Step4: Output sentence to serial monitor

    memset(valid_data, 0, VALID_DATA_ARRAY_SIZE);
#ifdef PARSE_LOOP_DEBUG
    PrintNextArrayBytes(valid_data, 0, 20); //print out first 20 bytes for verification
#endif
    numvalidbytes = RemoveInvalidBytes(raw_data, valid_data);
#ifdef PARSE_LOOP_DEBUG
    Serial.printf("Removed %d invalid bytes, leaving %d remaining\n", write_idx + 1 - numvalidbytes, numvalidbytes);
    PrintNextArrayBytes(valid_data, 0, numvalidbytes); //print out first 20 bytes of valid_data array
#endif


    if (numvalidbytes < 2)
    {
        return 0;
    }

    while (read_idx < numvalidbytes)
    {
#ifdef PARSE_LOOP_DEBUG
        Serial.printf("At top of while (read_idx < numvalidbytes): read_idx = %d\n", read_idx);
        Serial.printf("Next two bytes in valid_data are %x, %x\n", valid_data[read_idx], valid_data[read_idx + 1]);
#endif
        //Find a START sequence (0xC followed by 0x4)
        while (!IsStart(valid_data, read_idx) && read_idx < numvalidbytes)
        {
            //Serial.printf("looking for start...\n");
            read_idx++;
        }
        //at this point, read_idx should point to next valid byte pair

#ifdef PARSE_LOOP_DEBUG
        Serial.printf("Start sequence found at %d\n", read_idx - 2);
        //PrintNextFIFOBytes(valid_data, 20);
#endif

        if (numvalidbytes - read_idx > 14)//14 entries required for 7-bit address
        {
            //Get 7-bit device address
            devAddr = Get7BitDeviceAddr(valid_data, read_idx);
#ifdef PARSE_LOOP_DEBUG
            Serial.printf("devAddr = %x\n", devAddr);
#endif
        }
        else
        {

#ifdef PARSE_LOOP_DEBUG
            Serial.printf("ran out of data at readidx = %d - exiting!\n", read_idx);
#endif
            break;
        }

        //get read/write flag  1 = Read, 0 = Write, -1 = error
        BusDir = (BUSDIR)GetReadWriteFlag(valid_data, read_idx);

#ifdef PARSE_LOOP_DEBUG
        Serial.printf("BusDir = %s\n", ((BusDir == BUSDIR::WRITE) ? "WRITE" : "READ"));
        //PrintNextFIFOBytes(valid_data, 20);
#endif

    //get ACK/NAK flag
        ACKNAKFlag = GetACKNAKFlag(valid_data, read_idx);
        numbytes = GetDataBytes(valid_data, read_idx, databytes); //terminates on a START, but the start bytes are not consumed
#ifdef PARSE_LOOP_DEBUG
        Serial.printf("Got %d bytes from GetDataBytes() --> ", numbytes);
        for (size_t i = 0; i < numbytes; i++)
        {
            Serial.printf(" %x ", databytes[i]);
        }
        Serial.printf("\n");

        //PrintNextFIFOBytes(cb_trans, 20);
#endif
    //If the bus direction is WRITE, then extract
    //    A register address for read / write
    //    zero or more additional data bytes
        if (BusDir == BUSDIR::WRITE)
        {
            regAddr = databytes[0];
#ifdef PARSE_LOOP_DEBUG
            Serial.printf("regAddr = %x, read_idx = %d\n", regAddr, read_idx);
#endif

            //check for additional data
            if (numbytes > 1)
            {
#ifdef PARSE_LOOP_DEBUG
                Serial.printf("Additional data found!\n");
                for (size_t i = 0; i < numbytes; i++)
                {
                    Serial.printf("data[%d] = %x\n", i, databytes[i]);
                }
#endif
                //1st byte is register addr, subsequent bytes are data
                OutputFormattedSentence(BusDir, devAddr, regAddr, numbytes, databytes, 1);
            }
        }
        else  //all bytes are data
        {
#ifdef PARSE_LOOP_DEBUG
            Serial.printf("In data block:  got %d bytes of data\n", numbytes);
            for (size_t i = 0; i < numbytes; i++)
            {
                Serial.printf("data[%d] = %x\n", i, databytes[i]);
            }
#endif
            OutputFormattedSentence(BusDir, devAddr, regAddr, numbytes, databytes, 0);
        }
#ifdef PARSE_LOOP_DEBUG
        Serial.printf("At end of while (read_idx < numvalidbytes): read_idx = %d\n", read_idx);
#endif

    }//while (read_idx < numvalidbytes)
    return numvalidbytes;
}


#pragma region Support Functions
bool IsStart(byte* data, uint16_t& readidx)
{
    bool result = false;

    //Serial.printf("IsStart[%d] = %x, IsStart[%d] = %x\n",
    //    readidx, data[readidx], readidx + 1, data[readidx + 1]);

    if (data[readidx] == 0xC && data[readidx + 1] == 0x4)
    {
        result = true;
        readidx += 2; //bump to next byte pair
    }
    return result;
}

bool IsStop(byte* data, uint16_t& readidx)
{
    bool result = false;

    //Serial.printf("IsStop[%d] = %x, IsStop[%d] = %x\n",
        //readidx, data[readidx], readidx + 1, data[readidx + 1]);

    if (data[readidx] == 0x4 && data[readidx + 1] == 0xC)
    {
        result = true;
        readidx += 2; //bump to next byte pair
    }
    return result;
}

uint8_t Get7BitDeviceAddr(byte* data, uint16_t& readidx)
{
    //Purpose: Construct a 7-bit address starting from dataidx
    //Inputs:
    //  data = pointer to valid data array
    //  readidx = starting index of 7-bit address sequence (MSB first)
    //Outputs:
    //  returns the address as an 8-bit value with the MSB = 0, or 0x0 if unsuccessful
    //  dataidx = pointer to next data entry
    //Plan:
    //  Step1: Convert a pair of data entries into a 0 or 1
    //  Step2: Add the appropriate value to an ongoing sum
    //  Step3: return the total.
    //Notes:
    //  A '0' is coded as a 0x0 followed by a 0x4
    //  A '1' is coded as a 0x8 followed by a 0xC

    uint8_t devAddr = 0x0; //failure return value

    //Serial.printf("Get7BitDeviceAddr: readidx = %d\n",readidx);

    //devAddr is exactly 7 bits long, so 8 bits with MSB = 0
    for (size_t i = 0; i < 7; i++)
    {
        if (data[readidx] == 0x0 && data[readidx + 1] == 0x4)
        {
            readidx += 2; //advance the pointer, but don't add to sum
        }

        else if (data[readidx] == 0x8 && data[readidx + 1] == 0xC)
        {
            //Serial.printf("Get7BitDeviceAddr: '1' found at i = %d, adding %x to devAddr to get %x\n",
            //    i, 1 << (7 - i), devAddr + (1 << (7-i)));

            readidx += 2; //advance the pointer
            devAddr += (1 << (7 - i)); //add 2^(7-i) to sum
        }
    }

    devAddr = devAddr >> 1; //divide result by 2 to get 7-bit addr from 8 bits
    return devAddr;
}

int Get8BitDataByte(byte* data, uint16_t& readidx)
{
    //Purpose: Construct a 8-bit data byte starting from dataidx
    //Inputs:
    //  data = pointer to valid data array
    //  readidx = starting index of 8-bit data byte (MSB first)
    //Outputs:
    //  returns the address as an 8-bit value, or 0x0 if unsuccessful
    //  dataidx = pointer to next data entry
    //Plan:
    //  Step1: Convert a pair of data entries into a 0 or 1
    //  Step2: Add the appropriate value to an ongoing sum
    //  Step3: return the total.
    //Notes:
    //  A '0' is coded as a 0x0 followed by a 0x4
    //  A '1' is coded as a 0x8 followed by a 0xC
    //  12/29/19 - changed return val to int, so can return -1 when a 'short byte' is detected

    int dataval = 0x0; //failure return value

#ifdef GET_8BIT_DATABYTE_DEBUG
    Serial.printf("Get8BitDataByte: data[%d] = %x, data[%d] = %x\n",
        readidx, data[readidx], readidx + 1, data[readidx + 1]);
#endif

    //8 bits with MSB = 0
    int numbytes = 0;
    for (size_t i = 0; i < 8; i++)
    {
        if (data[readidx] == 0x0 && data[readidx + 1] == 0x4)
        {
            readidx += 2; //advance the pointer, but don't add to sum
            numbytes++;
        }

        else if (data[readidx] == 0x8 && data[readidx + 1] == 0xC)
        {
#ifdef GET_8BIT_DATABYTE_DEBUG
            Serial.printf("Get8BitDataByte: '1' found at i = %d, adding %x to devAddr to get %x\n",
                i, 1 << (7 - i), dataval + (1 << (7 - i)));
#endif
            readidx += 2; //advance the pointer
            dataval += (1 << (7 - i)); //add 2^(8-i) to sum
            numbytes++;
        }
    }

#ifdef GET_8BIT_DATABYTE_DEBUG
    Serial.printf("Get8BitDataByte: numbytes = %d\n", numbytes);
#endif
    if (numbytes != 8)
    {
        dataval = -1; //error return value
    }

    return dataval;
}

int GetReadWriteFlag(byte* data, uint16_t& readidx)
{
    //Purpose: decode R/W byte pair
    //Inputs:
    //  data = pointer to valid data array
    //  readidx = index into data to start of R/W byte pair
    //Outputs:
    //  readidx = if successful, points to next byte pair in data
    //  returns 1 for Read (0x8/0xC), 0 for Write (0x0/0x4), -1 for failure
    //Notes:
    //  

    //Serial.printf("GetReadWriteFlag: readidx = %d, data[readidx] = %x, data[readidx+1]= %x\n",
    //    readidx, data[readidx], data[readidx + 1]);
    int result = 0;
    if (data[readidx] == 0x8 && data[readidx + 1] == 0xC)
    {
        result = 1; //read detected
        readidx += 2; //point to next byte pair
    }

    else if (data[readidx] == 0x0 && data[readidx + 1] == 0x4)
    {
        result = 0; //write detected
        readidx += 2; //point to next byte pair
    }
    else
    {
        result = -1; //failed to detect read or write
    }

    return result;
}

int GetACKNAKFlag(byte* data, uint16_t& readidx)
{
    //Purpose: decode ACK/NAK byte pair
    //Inputs:
    //  data = pointer to valid data array
    //  readidx = index into data to start of ACK/NAK byte pair
    //Outputs:
    //  readidx = if successful, points to next byte pair in data
    //  returns 1 for NAK (0x8/0xC), 0 for ACK (0x0/0x4), -1 for failure
    //Notes:
    //  

    //Serial.printf("GetACKNAKFlag: readidx = %d, data[readidx] = %x, data[readidx+1]= %x\n",
    //    readidx, data[readidx], data[readidx + 1]);
    int result = 0;
    if (data[readidx] == 0x8 && data[readidx + 1] == 0xC)
    {
        result = 1; //NAK detected
        readidx += 2; //point to next byte pair
    }

    else if (data[readidx] == 0x0 && data[readidx + 1] == 0x4)
    {
        result = 0; //ACK detected
        readidx += 2; //point to next byte pair
    }
    else
    {
        result = -1; //failed to detect ACK or NAK
    }

    return result;
}

int GetDataBytes(uint8_t* data, uint16_t& readidx, uint8_t* databytes)
{
    //Notes:
    //  01/01/2020: removed databyteidx from sig - always starts at zero

    uint16_t numbytes = 0;
    uint16_t databyte_idx = 0;

    bool StartFlag = false;
    bool StopFlag = false;

    do
    {
        int dataval = Get8BitDataByte(data, readidx);

        //watch out for 'short byte' reads
        if (dataval >= 0)
        {
            uint8_t databyte = (uint8_t)dataval;
            databytes[databyte_idx] = databyte;
            databyte_idx++;
            numbytes++;
        }

        ACKNAKFlag = GetACKNAKFlag(data, readidx);
        StartFlag = IsStart(data, readidx);
        StopFlag = IsStop(data, readidx);

#ifdef PARSE_LOOP_DEBUG
        Serial.printf("IsStart returned %d, IsStop returned %d, dataidx = %d\n",
            StartFlag, StopFlag, readidx);
#endif

    } while (!StartFlag && !StopFlag && readidx < numvalidbytes);


    readidx -= 2;//back readidx up so loop top is positioned correctly.

    return numbytes;
}

void OutputFormattedSentence(int RW, uint8_t dev, uint8_t reg, uint8_t numbytes, uint8_t* bytearray, uint16_t startidx)
{
    Serial.printf("%lu I2C(%x) %s %d bytes %s %x... ",
        millis(), dev, (RW == 0 ? "writing" : "reading"),  numbytes - startidx, (RW == 0 ? "to" : "from"), reg);
    //for (size_t i = startidx; i < numbytes; i++)
    //{
    //    Serial.printf("%x ", bytearray[i]);
    //}
    //Serial.printf(". Done\n");

#ifdef MPU6050_SPECIFIC


    //01/18/20 experiment to decode 28-byte packet into yaw value
    if (numbytes == 28)
    {
        dmpGetQuaternion(&q, bytearray);
        dmpGetGravity(&gravity, &q);
        dmpGetYawPitchRoll(ypr, &q, &gravity);

        //compute the yaw value
        global_yawval = ypr[0] * 180 / M_PI;
        Serial.printf("yawval = %3.2f\n", global_yawval);
    }
    else
    {
        Serial.printf(". Done\n");
    }
#endif // MPU6050_SPECIFIC



}

uint16_t RemoveInvalidBytes(uint8_t* rawdata, uint8_t* validdata)
{
    uint16_t numvalid = 0;
    uint16_t valididx = 0;

    //Serial.printf("raw data array contains %d bytes\n", write_idx + 1);
    //PrintNextArrayBytes(raw_data, 0, 20);

    //OK, now go back through the array, excising invalid sequences
    for (uint16_t rawidx = 0; rawidx < write_idx;/*rawidx incremented internally*/)
    {
        uint8_t firstByte = raw_data[rawidx]; //get the first byte
        uint8_t secondByte = raw_data[rawidx + 1]; //get the next byte
        bool validpair =
            (
            (firstByte == 0xC && secondByte == 0x4) //START or RESTART
                || (firstByte == 0x4 && secondByte == 0xC) //STOP
                || (firstByte == 0x0 && secondByte == 0x4) //0 OR ACK
                || (firstByte == 0x8 && secondByte == 0xC) //1 or NAK
                );

        //Serial.printf("rawidx %d: Considering %x and %x: validity = %d\n",
            //rawidx, firstByte, secondByte, validpair);
        if (validpair)
        {
            //save valid bytes to valid_bytes array
            validdata[valididx] = firstByte;
            validdata[valididx + 1] = secondByte;
            numvalid += 2;
            //Serial.printf("Added %x & %x at idx = %d & %d\n", firstByte, secondByte, valididx, valididx + 1);
            //PrintNextArrayBytes(validdata,0,numvalid);
            rawidx += 2;
            valididx += 2;
        }
        else
        {
            rawidx++; //on invalid, just go to next byte
        }
    }

    return numvalid;
}
#pragma endregion Support Functions

#ifdef MPU6050_SPECIFIC

//#pragma region YAW_COMPUTATIONS
////01/18/2020: I copied these functions from MPU6050_6Axis_MotionApps_V6_12.h and
////  modified them to be called directly instead of from an 'mpu' object
//    
uint8_t dmpGetQuaternion(int16_t* data, const uint8_t* packet) 
{
    // TODO: accommodate different arrangements of sent data (ONLY default supported now)
    if (packet != 0) 
    {
        data[0] = ((packet[0] << 8) | packet[1]);
        data[1] = ((packet[4] << 8) | packet[5]);
        data[2] = ((packet[8] << 8) | packet[9]);
        data[3] = ((packet[12] << 8) | packet[13]);
    }
    return 0;
}

uint8_t dmpGetQuaternion(Quaternion* q, const uint8_t* packet) 
{
    // TODO: accommodate different arrangements of sent data (ONLY default supported now)
    int16_t qI[4]{ 0,0,0,0 };
    uint8_t status = dmpGetQuaternion(qI, packet);
    if (status == 0) {
        q->w = (float)qI[0] / 16384.0f;
        q->x = (float)qI[1] / 16384.0f;
        q->y = (float)qI[2] / 16384.0f;
        q->z = (float)qI[3] / 16384.0f;
        return 0;
    }
    return status; // int16 return value, indicates error if this line is reached
}

uint8_t dmpGetYawPitchRoll(float* data, Quaternion* q, VectorFloat* gravity) 
{
    // yaw: (about Z axis)
    data[0] = atan2(2 * q->x * q->y - 2 * q->w * q->z, 2 * q->w * q->w + 2 * q->x * q->x - 1);
    // pitch: (nose up/down, about Y axis)
    data[1] = atan2(gravity->x, sqrt(gravity->y * gravity->y + gravity->z * gravity->z));
    // roll: (tilt left/right, about X axis)
    data[2] = atan2(gravity->y, gravity->z);
    if (gravity->z < 0) {
        if (data[1] > 0) {
            data[1] = PI - data[1];
        }
        else {
            data[1] = -PI - data[1];
        }
    }
    return 0;
}


uint8_t dmpGetGravity(VectorFloat* v, Quaternion* q) 
{
    v->x = 2 * (q->x * q->z - q->w * q->y);
    v->y = 2 * (q->w * q->x + q->y * q->z);
    v->z = q->w * q->w - q->x * q->x - q->y * q->y + q->z * q->z;
    return 0;
}
//#pragma endregion YAW_COMPUTATIONS
#endif //MPU6050_SPECIFIC

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

189

190

191

192

193

194

195

196

197

198

199

200

201

202

203

204

205

206

207

208

209

210

211

212

213

214

215

216

217

218

219

220

221

222

223

224

225

226

227

228

229

230

231

232

233

234

235

236

237

238

239

240

241

242

243

244

245

246

247

248

249

250

251

252

253

254

255

256

257

258

259

260

261

262

263

264

265

266

267

268

269

270

271

272

273

274

275

276

277

278

279

280

281

282

283

284

285

286

287

288

289

290

291

292

293

294

295

296

297

298

299

300

301

302

303

304

305

306

307

308

309

310

311

312

313

314

315

316

317

318

319

320

321

322

323

324

325

326

327

328

329

330

331

332

333

334

335

336

337

338

339

340

341

342

343

344

345

346

347

348

349

350

351

352

353

354

355

356

357

358

359

360

361

362

363

364

365

366

367

368

369

370

371

372

373

374

375

376

377

378

379

380

381

382

383

384

385

386

387

388

389

390

391

392

393

394

395

396

397

398

399

400

401

402

403

404

405

406

407

408

409

410

411

412

413

414

415

416

417

418

419

420

421

422

423

424

425

426

427

428

429

430

431

432

433

434

435

436

437

438

439

440

441

442

443

444

445

446

447

448

449

450

451

452

453

454

455

456

457

458

459

460

461

462

463

464

465

466

467

468

469

470

471

472

473

474

475

476

477

478

479

480

481

482

483

484

485

486

487

488

489

490

491

492

493

494

495

496

497

498

499

500

501

502

503

504

505

506

507

508

509

510

511

512

513

514

515

516

517

518

519

520

521

522

523

524

525

526

527

528

529

530

531

532

533

534

535

536

537

538

539

540

541

542

543

544

545

546

547

548

549

550

551

552

553

554

555

556

557

558

559

560

561

562

563

564

565

566

567

568

569

570

571

572

573

574

575

576

577

578

579

580

581

582

583

584

585

586

587

588

589

590

591

592

593

594

595

596

597

598

599

600

601

602

603

604

605

606

607

608

609

610

611

612

613

614

615

616

617

618

619

620

621

622

623

624

625

626

627

628

629

630

631

632

633

634

635

636

637

638

639

640

641

642

643

644

645

646

647

648

649

650

651

652

653

654

655

656

657

658

659

660

661

662

663

664

665

666

667

668

669

670

671

672

673

674

675

676

677

678

679

680

681

682

683

684

685

686

687

688

689

690

691

692

693

694

695

696

697

698

699

700

701

702

703

704

705

706

707

708

709

710

711

712

713

714

715

716

717

718

719

720

721

722

723

724

725

726

727

728

729

730

731

732

733

734

735

736

737

738

739

740

741

742

743

744

745

746

747

748

749

750

751

752

753

754

755

756

757

758

759

760

761

762

763

764

765

766

767

768

769

770

771

772

Name: Teensy_I2C_Sniffer_V11.ino

Created: 1/18/2020 10:55:55 AM

Author: FRANKNEWXPS15\Frank

/* 'Notes:

A typical I2C sentence when communicating with a MPU6050 IMU module goes something like:

"I2C(68) wrote 1 byte to 75 - C0 Done."

"I2C(68) wrote 3 bytes to 72 - C0 0C 10 Done."

"I2C(68) read 5 bytes from 6A - C0 0C 10 14 03 Done."

To form a sentence, we need:

Device addr: 68 in the above examples

Read/Write direction

To/From register address: 75, 72 and 6A in the above examples

Data: C0, C0 0C 10, and C0 0C 10 14 03 in the above examples

number of bytes written/read: 1,3 & 5 in the above examples

Each I2C communication proceeds as follows (assuming a START from an IDLE condition):

A START or RESTART condition, denoted by SDA & SCL HIGH, followed by SDA LOW, SCL HIGH

A 7-bit device address, MSB first (0x8/0xC = 1, 0x0/0x4 = 0)

A R/W bit (0x8/0xC = read, 0x0/0x4 = write)

An ACK bit (0x8/0xC = NAK, 0x0/0x4 = ACK)

If the bus direction is WRITE, then

A register address for read/write

zero or more additional data bytes

Else (the bus direction is READ)

One or more additional data bytes

Endif

This version uses a fixed-size (2048 bytes) array instead of tonton81's circular buffer library.

To generalize for any I2C slave device rather than just the MPU6050 IMU, comment out the

"#define MPU6050_SPECIFIC line below. This will remove all MPU6050 specific code

//#define MPU6050_SPECIFIC

#include <TimerOne.h> //needed for ISR

#ifdef MPU6050_SPECIFIC

#include "helper_3dmath.h" //Arduino\Libraries\i2cdevlib\Arduino\MPU6050\ needed to compute yaw from MPU6050 DMP packet

#endif

#define PARSE_LOOP_DEBUG

const uint16_t CAPTURE_ARRAY_SIZE = 2048;

const uint16_t VALID_DATA_ARRAY_SIZE = 2048;

const int WAITING_PRINT_INTERVAL_MSEC = 200;//interval timer for 'Waiting for data...' printout

#define MONITOR_OUT1 2 //so can monitor ISR activity with O'scope

#define MONITOR_OUT2 3 //so can monitor ISR activity with O'scope

#define MONITOR_OUT3 4 //so can monitor ISR activity with O'scope

#define SDA_PIN 18

#define SCL_PIN 19

#pragma region PROCESSING_VARIABLES

uint8_t devAddr;

uint8_t regAddr;

uint8_t databytes[2048]; //holds multiple databytes for later output sentence construction

uint16_t numbytes = 0; //number of data bytes extracted from data stream

int ACKNAKFlag; //can be negative

uint16_t databyte_idx = 0; //index into databyte_array

uint8_t killbuff[2]; //used to consume start/stop bytes

elapsedMillis mSecSinceLastWaitingPrint;

uint8_t valid_data[2048];

uint16_t numvalidbytes = 0; //number of valid bytes in this burst

uint16_t read_idx = 0; //pointer to next byte pair to be processed

//added for bus direction labels

enum BUSDIR

{

WRITE,

READ,

UNKNOWN = -1

} RWDir;

BUSDIR BusDir = BUSDIR::UNKNOWN;

#pragma endregion ProcVars

#pragma region ISR_SUPPORT

uint8_t raw_data[CAPTURE_ARRAY_SIZE]; //holds data captured from I2C bus

volatile uint16_t write_idx = 0;

volatile uint8_t current_portb = 0xFF;

volatile uint8_t last_portb = 0xFF;

volatile uint16_t mult0xCCount = 0;

const uint16_t MAX_IDLE_COUNT = 2500;

volatile bool bDone = false;

volatile bool bWaitingForStart = true;

volatile bool bIsData = true;

volatile bool bIsStart = false;

volatile bool bIsStop = false;

volatile uint8_t last_current;

#pragma endregion ISR Support

#ifdef MPU6050_SPECIFIC

//01/21/20 these forward declarations are required to force the preprocessor

//to handle #ifdef MPU6050_SPECIFIC properly

uint8_t dmpGetQuaternion(int16_t* data, const uint8_t* packet);

uint8_t dmpGetQuaternion(Quaternion* q, const uint8_t* packet);

uint8_t dmpGetYawPitchRoll(float* data, Quaternion* q, VectorFloat* gravity);

uint8_t dmpGetGravity(VectorFloat* v, Quaternion* q);

Quaternion q; // [w, x, y, z] quaternion container

VectorInt16 aa; // [x, y, z] accel sensor measurements

VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements

VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements

VectorFloat gravity; // [x, y, z] gravity vector

float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector

float global_yawval = 0; //updated by GetIMUHeadingDeg()

#endif // MPU6050_SPECIFIC

void setup()

{

unsigned long now = millis();

Serial.begin(1); //rate value ignored

int idx = 0;

while (!Serial && (millis() - now) < 3000)

{

delay(500);

idx++;

}

Serial.printf("Serial available after %lu mSec\n", millis() - now);

pinMode(MONITOR_OUT1, OUTPUT);

digitalWrite(MONITOR_OUT1, LOW);

pinMode(MONITOR_OUT2, OUTPUT);

digitalWrite(MONITOR_OUT2, LOW);

pinMode(MONITOR_OUT3, OUTPUT);

digitalWrite(MONITOR_OUT3, LOW);

pinMode(SCL_PIN, INPUT);

pinMode(SDA_PIN, INPUT);

//reset port byte vars & start timer

last_portb = current_portb = 0;

write_idx = 0;

memset(raw_data, 255, CAPTURE_ARRAY_SIZE);

//PrintNextArrayBytes(raw_data, 255, 20);

Timer1.initialize(1); // run every mico second

Timer1.attachInterrupt(capture_data);

mSecSinceLastWaitingPrint = 0;

}

//-------------------------------------------------------------------------------

//-------------------------------- ISR ------------------------------------

//-------------------------------------------------------------------------------

FASTRUN void capture_data()

//void capture_data()

{

last_portb = current_portb;

current_portb = GPIOB_PDIR & 12; //reads state of SDA (18) & SCL (19) at same time

if (!bDone && last_portb != current_portb)

{

mult0xCCount = 0; //reset IDLE counter

digitalWriteFast(MONITOR_OUT1, HIGH);

//01/17/20: joepasquariello suggestion

last_current = (last_portb << 4) | (current_portb);

bIsStart = (last_current == 0xC4);

bIsStop = (last_current == 0x4C);

bIsData = (last_current == 0x04) || (last_current == 0x8C);

if (bIsStart) //START

{

digitalWriteFast(MONITOR_OUT2, HIGH);

if (bWaitingForStart)

{

digitalWriteFast(MONITOR_OUT3, HIGH); //start of entire capture

bWaitingForStart = false;

}

else if (bIsStop) //STOP

{

digitalWriteFast(MONITOR_OUT2, LOW);

}

if (!bWaitingForStart && (bIsData || bIsStart || bIsStop))

{

//digitalWriteFast(MONITOR_OUT3, HIGH);

raw_data[write_idx] = last_portb;

write_idx++;

raw_data[write_idx] = current_portb;

write_idx++;

if (write_idx >= CAPTURE_ARRAY_SIZE)

{

bDone = true;

digitalWriteFast(MONITOR_OUT3, LOW);

}

digitalWriteFast(MONITOR_OUT1, LOW);

}

else if (!bDone && mult0xCCount < MAX_IDLE_COUNT && last_portb == 0xc && current_portb == 0xc)

{

mult0xCCount++;

if (mult0xCCount >= MAX_IDLE_COUNT)

{

digitalWriteFast(MONITOR_OUT3, LOW);

bDone = true;

}

//-------------------------------------------------------------------------------

//-------------------------------- END ISR ---------------------------------

//-------------------------------------------------------------------------------

void loop()

{

if (bDone)

{

if (write_idx > 14)

{

//OK, we have some data to process. IDLE detection must have been EOM

Timer1.stop();

unsigned long startMsec = millis();

//Serial.printf("%lu\t %d\t", millis(), write_idx);

//PrintNextArrayBytes(raw_data, 0, 50);

//Serial.printf(" - %lu\n", millis());

uint16_t numprocessed = DecodeAndPrintValidData(raw_data); //decode and print everything captured so far

unsigned long endMsec = millis();

Serial.printf("%lu: processed = %d elements in %lu mSec\n\n", startMsec, numprocessed, endMsec-startMsec);

Timer1.start();

}

read_idx = 0;

bDone = false;

mult0xCCount = 0;

write_idx = 0;

bWaitingForStart = true;

}

else

{

//no data to process, but don't blow prints out every mSec...

if (mSecSinceLastWaitingPrint > WAITING_PRINT_INTERVAL_MSEC)

{

mSecSinceLastWaitingPrint -= WAITING_PRINT_INTERVAL_MSEC;

Serial.printf("%lu: Waiting for Data...\n", millis());

}

void PrintNextArrayBytes(uint8_t* data, uint16_t startidx, uint16_t numbytes)

{

Serial.printf("%d bytes starting at %d: ", numbytes, startidx);

for (uint16_t i = 0; i < numbytes; i++)

{

Serial.printf("%x ", data[i + startidx]);

}

uint16_t DecodeAndPrintValidData(byte* data)

{

//Purpose: decode and print I2C conversation held in raw_data array

//Inputs:

// cb = 2048 element FIFO

//Outputs:

// returns number of bytes processed, or -1 for failure

// outputs structured I2C sentence to serial monitor

//Plan:

// Step1: Cull out invalid bytes

// Step2: Determine if there is anything to do (have to have more than one transition in FIFO)

// Step3: Parse transitions into I2C sentence structure

// Step4: Output sentence to serial monitor

memset(valid_data, 0, VALID_DATA_ARRAY_SIZE);

#ifdef PARSE_LOOP_DEBUG

PrintNextArrayBytes(valid_data, 0, 20); //print out first 20 bytes for verification

#endif

numvalidbytes = RemoveInvalidBytes(raw_data, valid_data);

#ifdef PARSE_LOOP_DEBUG

Serial.printf("Removed %d invalid bytes, leaving %d remaining\n", write_idx + 1 - numvalidbytes, numvalidbytes);

PrintNextArrayBytes(valid_data, 0, numvalidbytes); //print out first 20 bytes of valid_data array

#endif

if (numvalidbytes < 2)

{

return 0;

}

while (read_idx < numvalidbytes)

{

#ifdef PARSE_LOOP_DEBUG

Serial.printf("At top of while (read_idx < numvalidbytes): read_idx = %d\n", read_idx);

Serial.printf("Next two bytes in valid_data are %x, %x\n", valid_data[read_idx], valid_data[read_idx + 1]);

#endif

//Find a START sequence (0xC followed by 0x4)

while (!IsStart(valid_data, read_idx) && read_idx < numvalidbytes)

{

//Serial.printf("looking for start...\n");

read_idx++;

}

//at this point, read_idx should point to next valid byte pair

#ifdef PARSE_LOOP_DEBUG

Serial.printf("Start sequence found at %d\n", read_idx - 2);

//PrintNextFIFOBytes(valid_data, 20);

#endif

if (numvalidbytes - read_idx > 14)//14 entries required for 7-bit address

{

//Get 7-bit device address

devAddr = Get7BitDeviceAddr(valid_data, read_idx);

#ifdef PARSE_LOOP_DEBUG

Serial.printf("devAddr = %x\n", devAddr);

#endif

}

else

{

#ifdef PARSE_LOOP_DEBUG

Serial.printf("ran out of data at readidx = %d - exiting!\n", read_idx);

#endif

break;

}

//get read/write flag 1 = Read, 0 = Write, -1 = error

BusDir = (BUSDIR)GetReadWriteFlag(valid_data, read_idx);

#ifdef PARSE_LOOP_DEBUG

Serial.printf("BusDir = %s\n", ((BusDir == BUSDIR::WRITE) ? "WRITE" : "READ"));

//PrintNextFIFOBytes(valid_data, 20);

#endif

//get ACK/NAK flag

ACKNAKFlag = GetACKNAKFlag(valid_data, read_idx);

numbytes = GetDataBytes(valid_data, read_idx, databytes); //terminates on a START, but the start bytes are not consumed

#ifdef PARSE_LOOP_DEBUG

Serial.printf("Got %d bytes from GetDataBytes() --> ", numbytes);

for (size_t i = 0; i < numbytes; i++)

{

Serial.printf(" %x ", databytes[i]);

}

Serial.printf("\n");

//PrintNextFIFOBytes(cb_trans, 20);

#endif

//If the bus direction is WRITE, then extract

// A register address for read / write

// zero or more additional data bytes

if (BusDir == BUSDIR::WRITE)

{

regAddr = databytes[0];

#ifdef PARSE_LOOP_DEBUG

Serial.printf("regAddr = %x, read_idx = %d\n", regAddr, read_idx);

#endif

//check for additional data

if (numbytes > 1)

{

#ifdef PARSE_LOOP_DEBUG

Serial.printf("Additional data found!\n");

for (size_t i = 0; i < numbytes; i++)

{

Serial.printf("data[%d] = %x\n", i, databytes[i]);

}

#endif

//1st byte is register addr, subsequent bytes are data

OutputFormattedSentence(BusDir, devAddr, regAddr, numbytes, databytes, 1);

}

else //all bytes are data

{

#ifdef PARSE_LOOP_DEBUG

Serial.printf("In data block: got %d bytes of data\n", numbytes);

for (size_t i = 0; i < numbytes; i++)

{

Serial.printf("data[%d] = %x\n", i, databytes[i]);

}

#endif

OutputFormattedSentence(BusDir, devAddr, regAddr, numbytes, databytes, 0);

}

#ifdef PARSE_LOOP_DEBUG

Serial.printf("At end of while (read_idx < numvalidbytes): read_idx = %d\n", read_idx);

#endif

}//while (read_idx < numvalidbytes)

return numvalidbytes;

}

#pragma region Support Functions

bool IsStart(byte* data, uint16_t& readidx)

{

bool result = false;

//Serial.printf("IsStart[%d] = %x, IsStart[%d] = %x\n",

// readidx, data[readidx], readidx + 1, data[readidx + 1]);

if (data[readidx] == 0xC && data[readidx + 1] == 0x4)

{

result = true;

readidx += 2; //bump to next byte pair

}

return result;

}

bool IsStop(byte* data, uint16_t& readidx)

{

bool result = false;

//Serial.printf("IsStop[%d] = %x, IsStop[%d] = %x\n",

//readidx, data[readidx], readidx + 1, data[readidx + 1]);

if (data[readidx] == 0x4 && data[readidx + 1] == 0xC)

{

result = true;

readidx += 2; //bump to next byte pair

}

return result;

}

uint8_t Get7BitDeviceAddr(byte* data, uint16_t& readidx)

{

//Purpose: Construct a 7-bit address starting from dataidx

//Inputs:

// data = pointer to valid data array

// readidx = starting index of 7-bit address sequence (MSB first)

//Outputs:

// returns the address as an 8-bit value with the MSB = 0, or 0x0 if unsuccessful

// dataidx = pointer to next data entry

//Plan:

// Step1: Convert a pair of data entries into a 0 or 1

// Step2: Add the appropriate value to an ongoing sum

// Step3: return the total.

//Notes:

// A '0' is coded as a 0x0 followed by a 0x4

// A '1' is coded as a 0x8 followed by a 0xC

uint8_t devAddr = 0x0; //failure return value

//Serial.printf("Get7BitDeviceAddr: readidx = %d\n",readidx);

//devAddr is exactly 7 bits long, so 8 bits with MSB = 0

for (size_t i = 0; i < 7; i++)

{

if (data[readidx] == 0x0 && data[readidx + 1] == 0x4)

{

readidx += 2; //advance the pointer, but don't add to sum

}

else if (data[readidx] == 0x8 && data[readidx + 1] == 0xC)

{

//Serial.printf("Get7BitDeviceAddr: '1' found at i = %d, adding %x to devAddr to get %x\n",

// i, 1 << (7 - i), devAddr + (1 << (7-i)));

readidx += 2; //advance the pointer

devAddr += (1 << (7 - i)); //add 2^(7-i) to sum

}

devAddr = devAddr >> 1; //divide result by 2 to get 7-bit addr from 8 bits

return devAddr;

}

int Get8BitDataByte(byte* data, uint16_t& readidx)

{

//Purpose: Construct a 8-bit data byte starting from dataidx

//Inputs:

// data = pointer to valid data array

// readidx = starting index of 8-bit data byte (MSB first)

//Outputs:

// returns the address as an 8-bit value, or 0x0 if unsuccessful

// dataidx = pointer to next data entry

//Plan:

// Step1: Convert a pair of data entries into a 0 or 1

// Step2: Add the appropriate value to an ongoing sum

// Step3: return the total.

//Notes:

// A '0' is coded as a 0x0 followed by a 0x4

// A '1' is coded as a 0x8 followed by a 0xC

// 12/29/19 - changed return val to int, so can return -1 when a 'short byte' is detected

int dataval = 0x0; //failure return value

#ifdef GET_8BIT_DATABYTE_DEBUG

Serial.printf("Get8BitDataByte: data[%d] = %x, data[%d] = %x\n",

readidx, data[readidx], readidx + 1, data[readidx + 1]);

#endif

//8 bits with MSB = 0

int numbytes = 0;

for (size_t i = 0; i < 8; i++)

{

if (data[readidx] == 0x0 && data[readidx + 1] == 0x4)

{

readidx += 2; //advance the pointer, but don't add to sum

numbytes++;

}

else if (data[readidx] == 0x8 && data[readidx + 1] == 0xC)

{

#ifdef GET_8BIT_DATABYTE_DEBUG

Serial.printf("Get8BitDataByte: '1' found at i = %d, adding %x to devAddr to get %x\n",

i, 1 << (7 - i), dataval + (1 << (7 - i)));

#endif

readidx += 2; //advance the pointer

dataval += (1 << (7 - i)); //add 2^(8-i) to sum

numbytes++;

}

#ifdef GET_8BIT_DATABYTE_DEBUG

Serial.printf("Get8BitDataByte: numbytes = %d\n", numbytes);

#endif

if (numbytes != 8)

{

dataval = -1; //error return value

}

return dataval;

}

int GetReadWriteFlag(byte* data, uint16_t& readidx)

{

//Purpose: decode R/W byte pair

//Inputs:

// data = pointer to valid data array

// readidx = index into data to start of R/W byte pair

//Outputs:

// readidx = if successful, points to next byte pair in data

// returns 1 for Read (0x8/0xC), 0 for Write (0x0/0x4), -1 for failure

//Notes:

//Serial.printf("GetReadWriteFlag: readidx = %d, data[readidx] = %x, data[readidx+1]= %x\n",

// readidx, data[readidx], data[readidx + 1]);

int result = 0;

if (data[readidx] == 0x8 && data[readidx + 1] == 0xC)

{

result = 1; //read detected

readidx += 2; //point to next byte pair

}

else if (data[readidx] == 0x0 && data[readidx + 1] == 0x4)

{

result = 0; //write detected

readidx += 2; //point to next byte pair

}

else

{

result = -1; //failed to detect read or write

}

return result;

}

int GetACKNAKFlag(byte* data, uint16_t& readidx)

{

//Purpose: decode ACK/NAK byte pair

//Inputs:

// data = pointer to valid data array

// readidx = index into data to start of ACK/NAK byte pair

//Outputs:

// readidx = if successful, points to next byte pair in data

// returns 1 for NAK (0x8/0xC), 0 for ACK (0x0/0x4), -1 for failure

//Notes:

//Serial.printf("GetACKNAKFlag: readidx = %d, data[readidx] = %x, data[readidx+1]= %x\n",

// readidx, data[readidx], data[readidx + 1]);

int result = 0;

if (data[readidx] == 0x8 && data[readidx + 1] == 0xC)

{

result = 1; //NAK detected

readidx += 2; //point to next byte pair

}

else if (data[readidx] == 0x0 && data[readidx + 1] == 0x4)

{

result = 0; //ACK detected

readidx += 2; //point to next byte pair

}

else

{

result = -1; //failed to detect ACK or NAK

}

return result;

}

int GetDataBytes(uint8_t* data, uint16_t& readidx, uint8_t* databytes)

{

//Notes:

// 01/01/2020: removed databyteidx from sig - always starts at zero

uint16_t numbytes = 0;

uint16_t databyte_idx = 0;

bool StartFlag = false;

bool StopFlag = false;

{

int dataval = Get8BitDataByte(data, readidx);

//watch out for 'short byte' reads

if (dataval >= 0)

{

uint8_t databyte = (uint8_t)dataval;

databytes[databyte_idx] = databyte;

databyte_idx++;

numbytes++;

}

ACKNAKFlag = GetACKNAKFlag(data, readidx);

StartFlag = IsStart(data, readidx);

StopFlag = IsStop(data, readidx);

#ifdef PARSE_LOOP_DEBUG

Serial.printf("IsStart returned %d, IsStop returned %d, dataidx = %d\n",

StartFlag, StopFlag, readidx);

#endif

} while (!StartFlag && !StopFlag && readidx < numvalidbytes);

readidx -= 2;//back readidx up so loop top is positioned correctly.

return numbytes;

}

void OutputFormattedSentence(int RW, uint8_t dev, uint8_t reg, uint8_t numbytes, uint8_t* bytearray, uint16_t startidx)

{

Serial.printf("%lu I2C(%x) %s %d bytes %s %x... ",

millis(), dev, (RW == 0 ? "writing" : "reading"), numbytes - startidx, (RW == 0 ? "to" : "from"), reg);

//for (size_t i = startidx; i < numbytes; i++)

//{

// Serial.printf("%x ", bytearray[i]);

//}

//Serial.printf(". Done\n");

#ifdef MPU6050_SPECIFIC

//01/18/20 experiment to decode 28-byte packet into yaw value

if (numbytes == 28)

{

dmpGetQuaternion(&q, bytearray);

dmpGetGravity(&gravity, &q);

dmpGetYawPitchRoll(ypr, &q, &gravity);

//compute the yaw value

global_yawval = ypr[0] * 180 / M_PI;

Serial.printf("yawval = %3.2f\n", global_yawval);

}

else

{

Serial.printf(". Done\n");

}

#endif // MPU6050_SPECIFIC

}

uint16_t RemoveInvalidBytes(uint8_t* rawdata, uint8_t* validdata)

{

uint16_t numvalid = 0;

uint16_t valididx = 0;

//Serial.printf("raw data array contains %d bytes\n", write_idx + 1);

//PrintNextArrayBytes(raw_data, 0, 20);

//OK, now go back through the array, excising invalid sequences

for (uint16_t rawidx = 0; rawidx < write_idx;/*rawidx incremented internally*/)

{

uint8_t firstByte = raw_data[rawidx]; //get the first byte

uint8_t secondByte = raw_data[rawidx + 1]; //get the next byte

bool validpair =

(

(firstByte == 0xC && secondByte == 0x4) //START or RESTART

|| (firstByte == 0x4 && secondByte == 0xC) //STOP

|| (firstByte == 0x0 && secondByte == 0x4) //0 OR ACK

|| (firstByte == 0x8 && secondByte == 0xC) //1 or NAK

);

//Serial.printf("rawidx %d: Considering %x and %x: validity = %d\n",

//rawidx, firstByte, secondByte, validpair);

if (validpair)

{

//save valid bytes to valid_bytes array

validdata[valididx] = firstByte;

validdata[valididx + 1] = secondByte;

numvalid += 2;

//Serial.printf("Added %x & %x at idx = %d & %d\n", firstByte, secondByte, valididx, valididx + 1);

//PrintNextArrayBytes(validdata,0,numvalid);

rawidx += 2;

valididx += 2;

}

else

{

rawidx++; //on invalid, just go to next byte

}

return numvalid;

}

#pragma endregion Support Functions

#ifdef MPU6050_SPECIFIC

//#pragma region YAW_COMPUTATIONS

////01/18/2020: I copied these functions from MPU6050_6Axis_MotionApps_V6_12.h and

//// modified them to be called directly instead of from an 'mpu' object

uint8_t dmpGetQuaternion(int16_t* data, const uint8_t* packet)

{

// TODO: accommodate different arrangements of sent data (ONLY default supported now)

if (packet != 0)

{

data[0] = ((packet[0] << 8) | packet[1]);

data[1] = ((packet[4] << 8) | packet[5]);

data[2] = ((packet[8] << 8) | packet[9]);

data[3] = ((packet[12] << 8) | packet[13]);

}

return 0;

}

uint8_t dmpGetQuaternion(Quaternion* q, const uint8_t* packet)

{

// TODO: accommodate different arrangements of sent data (ONLY default supported now)

int16_t qI[4]{ 0,0,0,0 };

uint8_t status = dmpGetQuaternion(qI, packet);

if (status == 0) {

q->w = (float)qI[0] / 16384.0f;

q->x = (float)qI[1] / 16384.0f;

q->y = (float)qI[2] / 16384.0f;

q->z = (float)qI[3] / 16384.0f;

return 0;

}

return status; // int16 return value, indicates error if this line is reached

}

uint8_t dmpGetYawPitchRoll(float* data, Quaternion* q, VectorFloat* gravity)

{

// yaw: (about Z axis)

data[0] = atan2(2 * q->x * q->y - 2 * q->w * q->z, 2 * q->w * q->w + 2 * q->x * q->x - 1);

// pitch: (nose up/down, about Y axis)

data[1] = atan2(gravity->x, sqrt(gravity->y * gravity->y + gravity->z * gravity->z));

// roll: (tilt left/right, about X axis)

data[2] = atan2(gravity->y, gravity->z);

if (gravity->z < 0) {

if (data[1] > 0) {

data[1] = PI - data[1];

}

else {

data[1] = -PI - data[1];

}

return 0;

}

uint8_t dmpGetGravity(VectorFloat* v, Quaternion* q)

{

v->x = 2 * (q->x * q->z - q->w * q->y);

v->y = 2 * (q->w * q->x + q->y * q->z);

v->z = q->w * q->w - q->x * q->x - q->y * q->y + q->z * q->z;

return 0;

}

//#pragma endregion YAW_COMPUTATIONS

#endif //MPU6050_SPECIFIC

With this setup with the master sending data to the slave, I got the following outputs:

Master

Sending to Slave: 1048.005615 (0x4483002e)
i = 0: I2C_WriteAnything() sending 2E
i = 1: I2C_WriteAnything() sending 0
i = 2: I2C_WriteAnything() sending 83
i = 3: I2C_WriteAnything() sending 44
OK
Sending to Slave: 1049.239990 (0x448327ae)
i = 0: I2C_WriteAnything() sending AE
i = 1: I2C_WriteAnything() sending 27
i = 2: I2C_WriteAnything() sending 83
i = 3: I2C_WriteAnything() sending 44
OK
Sending to Slave: 1050.474487 (0x44834f2f)
i = 0: I2C_WriteAnything() sending 2F
i = 1: I2C_WriteAnything() sending 4F
i = 2: I2C_WriteAnything() sending 83
i = 3: I2C_WriteAnything() sending 44
OK
Sending to Slave: 1051.708862 (0x448376af)
i = 0: I2C_WriteAnything() sending AF
i = 1: I2C_WriteAnything() sending 76
i = 2: I2C_WriteAnything() sending 83
i = 3: I2C_WriteAnything() sending 44
OK
Sending to Slave: 1052.943237 (0x44839e2f)
i = 0: I2C_WriteAnything() sending 2F
i = 1: I2C_WriteAnything() sending 9E
i = 2: I2C_WriteAnything() sending 83
i = 3: I2C_WriteAnything() sending 44
OK

Sending to Slave: 1048.005615 (0x4483002e)

i = 0: I2C_WriteAnything() sending 2E

i = 1: I2C_WriteAnything() sending 0

i = 2: I2C_WriteAnything() sending 83

i = 3: I2C_WriteAnything() sending 44

Sending to Slave: 1049.239990 (0x448327ae)

i = 0: I2C_WriteAnything() sending AE

i = 1: I2C_WriteAnything() sending 27

i = 2: I2C_WriteAnything() sending 83

i = 3: I2C_WriteAnything() sending 44

Sending to Slave: 1050.474487 (0x44834f2f)

i = 0: I2C_WriteAnything() sending 2F

i = 1: I2C_WriteAnything() sending 4F

i = 2: I2C_WriteAnything() sending 83

i = 3: I2C_WriteAnything() sending 44

Sending to Slave: 1051.708862 (0x448376af)

i = 0: I2C_WriteAnything() sending AF

i = 1: I2C_WriteAnything() sending 76

i = 2: I2C_WriteAnything() sending 83

i = 3: I2C_WriteAnything() sending 44

Sending to Slave: 1052.943237 (0x44839e2f)

i = 0: I2C_WriteAnything() sending 2F

i = 1: I2C_WriteAnything() sending 9E

i = 2: I2C_WriteAnything() sending 83

i = 3: I2C_WriteAnything() sending 44

Slave:

Slave received: 1048.005615
Slave received: 1049.239990
Slave received: 1050.474487
Slave received: 1051.708862
Slave received: 1052.943237

Slave received: 1048.005615

Slave received: 1049.239990

Slave received: 1050.474487

Slave received: 1051.708862

Slave received: 1052.943237

I2C Sniffer:

Start sequence found at 0
devAddr = 66
BusDir = WRITE
Got 4 bytes from GetDataBytes() -->  2e  0  83  44 
Start sequence found at 0
devAddr = 66
BusDir = WRITE
Got 4 bytes from GetDataBytes() -->  ae  27  83  44 
Start sequence found at 0
devAddr = 66
BusDir = WRITE
Got 4 bytes from GetDataBytes() -->  2f  4f  83  44 
Start sequence found at 0
devAddr = 66
BusDir = WRITE
Got 4 bytes from GetDataBytes() -->  af  76  83  44 
Start sequence found at 0
devAddr = 66
BusDir = WRITE
Got 4 bytes from GetDataBytes() -->  2f  9e  83  44

Start sequence found at 0

devAddr = 66

BusDir = WRITE

Got 4 bytes from GetDataBytes() --> 2e 0 83 44

Start sequence found at 0

devAddr = 66

BusDir = WRITE

Got 4 bytes from GetDataBytes() --> ae 27 83 44

Start sequence found at 0

devAddr = 66

BusDir = WRITE

Got 4 bytes from GetDataBytes() --> 2f 4f 83 44

Start sequence found at 0

devAddr = 66

BusDir = WRITE

Got 4 bytes from GetDataBytes() --> af 76 83 44

Start sequence found at 0

devAddr = 66

BusDir = WRITE

Got 4 bytes from GetDataBytes() --> 2f 9e 83 44

In the other direction, with the slave sending data to the master, I got the following:

Slave:

Sending 28398426.000000 to Master
Sending 28398426.000000 to Master
Sending 28398426.000000 to Master
Sending 28398426.000000 to Master

Sending 28398426.000000 to Master

Master:

Reading from Slave: Received 28398426.000000 from Slave
Reading from Slave: Received 28398426.000000 from Slave
Reading from Slave: Received 28398426.000000 from Slave

Reading from Slave: Received 28398426.000000 from Slave

I2C Sniffer:

In data block:  got 112 bytes of data
data[0] = ad
data[1] = a9
data[2] = d8
data[3] = 4b
data[4] = 0
data[5] = 0
....
In data block:  got 112 bytes of data
data[0] = ad
data[1] = a9
data[2] = d8
data[3] = 4b
data[4] = 0
data[5] = 0
.....
In data block:  got 112 bytes of data
data[0] = ad
data[1] = a9
data[2] = d8
data[3] = 4b
data[4] = 0
data[5] = 0

In data block: got 112 bytes of data

data[0] = ad

data[1] = a9

data[2] = d8

data[3] = 4b

data[4] = 0

data[5] = 0

....

In data block: got 112 bytes of data

data[0] = ad

data[1] = a9

data[2] = d8

data[3] = 4b

data[4] = 0

data[5] = 0

.....

In data block: got 112 bytes of data

data[0] = ad

data[1] = a9

data[2] = d8

data[3] = 4b

data[4] = 0

data[5] = 0

Where the HEX sequence 4B D8 A9 AD converted to a IEEE float = 2.83984e+07

So, the Teensy-to-Teensy I2C connection is clearly working in both directions, and the Teensy I2C Sniffer is successfully capturing and decoding the I2C traffic between the two modules – cool!

So, the next step is to replace the ‘Master’ Teensy with an Arduino Mega and repeat the process. Hopefully this will allow me to figure out why the slave-to-master data transfer isn’t working

21 March 2020 Update:

Based on the above results, I formed a hypothesis that the problem with sending I2C data from a Teensy slave to an Arduino Mega master might be due to the Mega not being able to properly interpret 0-3.3V transitions from the Teensy. So, I decided to construct a low-to-high level converter to make sure that SDA data from the Teensy was presented to the Mega as 0-5V transitions rather than the Teensy’s ‘raw’ 0-3.3V ones. To do this I used the circuit shown below, except with a 1K instead of 10K resistor for R2 to clean up the transitions at 100KHz.

Bidirectional 3.3-5V level shifter using 2N7000 MosFet

Here’s a scope photo of the 3.3V input to and the 5V output from the 2N7000-based level shifter. The top trace is the Teensy 0-3.3V output, and the bottom trace is the 0-5V level-shifted version. Both traces are 1V/cm, and the horizontal time scale is 20 uSec/cm.

Teensy slave, Mega master: Top: 0-3.3V input. Bottom: 0-5V output. Both traces are 1V/cm vertical, 20 uSec cm horizontally

To verify that the above circuit worked properly, I used it in the SDA line (the SCL line doesn’t require any level shifting as it is from the 5V Mega to the 3.3V Teensy) with a Teensy slave and a Teensy master. This is OK, as Teensy 3.x’s can handle 5V inputs, and it worked properly in both directions. Here’s a shot of the SDA line

Teensy slave, Teensy master: Top: 0-3.3V input. Bottom: 0-5V output. Both traces are 1V/cm vertical, 20 uSec cm horizontally

However, after replacing the Teensy master with the Mega one, I had the same problem as before – I can successfully transfer data from Mega master to Teensy slave, but not in the other direction. There is still a problem somewhere, and I no longer think it’s due to level-shifting problems. From the above scope photos, it is clear that only a few bytes are transmitted by the Teensy slave each time it services the OnRequest() interrupt, while that same function transmits MUCH more data when servicing the same request from the Teensy 3.5 master.

Stay tuned!

23 March 2020 Update:

OK, back to the basics: I downloaded the code for a very basic Arduino-Arduino I2C tutorial, and verified that it worked OK. I also took a scope shot of SDA/SCL activity during the two-way data transfers, as shown below:

Basic Arduino-Arduino tutorial layout

I2C line activity for the ‘Basic Arduino – Arduino I2C Tutorial’

So now that I have a working baseline for the Arduino-Arduino I2C case, I plan to incrementally modify it toward duplicating the non-working Arduino-Arduino I2C case until it breaks, and then I’ll know that the last incremental modification is the culprit. At the moment, I’m leaning toward the use of the I2CDev & SBWIRE libraries as they are the only significant difference between the working and non-working setups. We’ll see….

24 March 2020 Update:

I created a new Arduino project called ‘I2C_Master_Tut_Mod1’ initially identical to ‘I2C_Master_Tutorial’ and verified that it worked properly with the unmodified ‘I2C_Slave_Tutorial’ project, and then started modifying it.

Added #include <PrintEx.h> //allows printf-style printout syntax
StreamEx mySerial = Serial; //added 03/18/18 for printf-style printing, and changed all occurrences of ‘Serial’ to ‘mySerial’ and modified print statements to ‘printf’ format. All worked fine.
Added #include <I2C_Anything.h> and changed loop() to write a float value to the slave. However, this caused a compile error. When I started tracking it down I realized that I had previously modified ‘I2C_Anything.h’ to #include SBWIRE.h rather than the original #include Wire.h. So, this could be the culprit, assuming that there is something about SBWIRE that causes Arduino I2C slaves to misbehave.
Copied I2C_Anything.h/cpp from the Libraries folder to my new ‘I2C_Master_Tut_Mod1’ folder, and un-modified it to reference back Wire.h vs SBWIRE.h.

25 March 2020 Update:

As it turns out, I was never able to get the ‘I2C_Master_Tut_Mod1’ project working with #include <I2C_Anything.h> , no matter what I did, including copying the #include <I2C_Anything.h> to the local folder and including it as a resource int the VS2019 project, build cleans, removing the _vm folder created by Visual Micro, etc. To make it even stranger, I created a new Arduino project in VS2019 called ‘I2C_Master_Tut_Mod2’, copied the ‘I2C_Anything.h’ file to the local folder, and it compiles without error! So now I have two almost completely identical Arduino projects, one of which compiles fine and the other of which blows a bunch of errors about twi.c. I’m currently working with Tim Leek at VisualMicro to sort out what I did wrong.

27 March 2020 Update:

Ok, I now have the Arduino Mega master to Arduino Uno slave setup working, with I2C_Anything. I can transfer a float value in either direction with no problem. The master and slave code sets and the corresponding outputs are shown below.

Master Sketch:

/*
    Name:       I2C_Master_Tut_Mod2.ino
    Created:	3/25/2020 8:54:55 AM
    Author:     FRANKNEWXPS15\Frank
*/


/*
    Name:       I2C_Master_Tutorial.ino
    Created:	3/23/2020 1:20:08 PM
    Author:     FRANKNEWXPS15\Frank
*/
/*
  I2C Master Demo
  i2c-master-demo.ino
  Demonstrate use of I2C bus
  Master sends character and gets reply from Slave
  DroneBot Workshop 2019
  https://dronebotworkshop.com
*/

// Include Arduino Wire library for I2C
#include <Wire.h>
#include "PrintEx.h"
#include "I2C_Anything.h"
// Define Slave I2C Address
#define SLAVE_ADDR 9
StreamEx mySerial = Serial; //added 03/18/18 for printf-style printing

// Define Slave answer size
#define ANSWERSIZE 5
const float sendval = 3.14159f;
float rcvval = 0;

void setup() {

    // Initialize I2C communications as Master
    Wire.begin();

    // Setup serial monitor
    Serial.begin(115200);
    Serial.println("I2C Master Demonstration");
}

void loop() {
    delay(50);
    //Serial.println("Write data to slave");
    mySerial.printf("Sent %3.5f: ",sendval);

    // Write a charatre to the Slave
    Wire.beginTransmission(SLAVE_ADDR);
    //Wire.write(0);
    I2C_writeAnything(sendval);
    Wire.endTransmission();

    //Serial.println("Receive data");

    //// Read response from Slave
    //// Read back 5 characters
    //Wire.requestFrom(SLAVE_ADDR, ANSWERSIZE);

    //// Add characters to string
    //String response = "";
    //while (Wire.available()) {
    //    char b = Wire.read();
    //    response += b;
    //}

    //// Print to Serial Monitor
    //Serial.println(response);

    //03/26/2020 gfp rev to retrieve float value
    Wire.requestFrom(SLAVE_ADDR, sizeof(float));
    I2C_readAnything(rcvval);
    mySerial.printf("Received %4.1f\n",rcvval);


}

Name: I2C_Master_Tut_Mod2.ino

Created: 3/25/2020 8:54:55 AM

Author: FRANKNEWXPS15\Frank

Name: I2C_Master_Tutorial.ino

Created: 3/23/2020 1:20:08 PM

Author: FRANKNEWXPS15\Frank

I2C Master Demo

i2c-master-demo.ino

Demonstrate use of I2C bus

Master sends character and gets reply from Slave

DroneBot Workshop 2019

https://dronebotworkshop.com

// Include Arduino Wire library for I2C

#include <Wire.h>

#include "PrintEx.h"

#include "I2C_Anything.h"

// Define Slave I2C Address

#define SLAVE_ADDR 9

StreamEx mySerial = Serial; //added 03/18/18 for printf-style printing

// Define Slave answer size

#define ANSWERSIZE 5

const float sendval = 3.14159f;

float rcvval = 0;

void setup() {

// Initialize I2C communications as Master

Wire.begin();

// Setup serial monitor

Serial.begin(115200);

Serial.println("I2C Master Demonstration");

}

void loop() {

delay(50);

//Serial.println("Write data to slave");

mySerial.printf("Sent %3.5f: ",sendval);

// Write a charatre to the Slave

Wire.beginTransmission(SLAVE_ADDR);

//Wire.write(0);

I2C_writeAnything(sendval);

Wire.endTransmission();

//Serial.println("Receive data");

//// Read response from Slave

//// Read back 5 characters

//Wire.requestFrom(SLAVE_ADDR, ANSWERSIZE);

//// Add characters to string

//String response = "";

//while (Wire.available()) {

// char b = Wire.read();

// response += b;

//}

//// Print to Serial Monitor

//Serial.println(response);

//03/26/2020 gfp rev to retrieve float value

Wire.requestFrom(SLAVE_ADDR, sizeof(float));

I2C_readAnything(rcvval);

mySerial.printf("Received %4.1f\n",rcvval);

}

Slave Sketch:

/*
    Name:       I2C_Slave_Tutorial.ino
    Created:	3/23/2020 1:23:40 PM
    Author:     FRANKNEWXPS15\Frank
*/


/*
  I2C Slave Demo
  i2c-slave-demo.ino
  Demonstrate use of I2C bus
  Slave receives character from Master and responds
  DroneBot Workshop 2019
  https://dronebotworkshop.com
*/

// Include Arduino Wire library for I2C
#include <Wire.h>
#include "PrintEx.h"
StreamEx mySerial = Serial; //added 03/18/18 for printf-style printing

//03/26/2020 experiment with I2C_Anything
#include "I2C_Anything.h"
float rcvval = 0;
float sendval = 95141.3; //3.14159 written backwards


// Define Slave I2C Address
#define SLAVE_ADDR 9

// Define Slave answer size
#define ANSWERSIZE 5

// Define string with response to Master
String answer = "Hello";

void setup() {

    // Initialize I2C communications as Slave
    Wire.begin(SLAVE_ADDR);

    // Function to run when data requested from master
    Wire.onRequest(requestEvent);

    // Function to run when data received from master
    Wire.onReceive(receiveEvent);

    // Setup Serial Monitor 
    Serial.begin(115200);
    Serial.println("I2C Slave Demonstration");
}

void receiveEvent() {

    //Serial.println("Receive event");
    // Read while data received
    //while (0 < Wire.available()) {
    //    byte x = Wire.read();
    //}

    //03/26/2020 experiment with I2C_Anything
    I2C_readAnything(rcvval);

    // Print to Serial Monitor
    //Serial.print("Received "); Serial.print(rcvval, 6); Serial.println(" From Master");
    mySerial.printf("Received %3.6f: ", rcvval);
}

void requestEvent() {

    //// Setup byte variable in the correct size
    //byte response[ANSWERSIZE];

    //// Format answer as array
    //for (byte i = 0; i < ANSWERSIZE; i++) {
    //    response[i] = (byte)answer.charAt(i);
    //}

    //// Send response back to Master
    //Wire.write(response, sizeof(response));

    //// Print to Serial Monitor
    //Serial.println("Request event");

    //03/26/2020 experiment with I2C_Anything
    //Serial.print("Sending "); Serial.print(sendval, 6); Serial.println(" to Master");
    mySerial.printf("Sent %6.1f\n",sendval);
    I2C_writeAnything(sendval);
}

void loop() {

    // Time delay in loop
    delay(50);
}

Name: I2C_Slave_Tutorial.ino

Created: 3/23/2020 1:23:40 PM

Author: FRANKNEWXPS15\Frank

I2C Slave Demo

i2c-slave-demo.ino

Demonstrate use of I2C bus

Slave receives character from Master and responds

DroneBot Workshop 2019

https://dronebotworkshop.com

// Include Arduino Wire library for I2C

#include <Wire.h>

#include "PrintEx.h"

StreamEx mySerial = Serial; //added 03/18/18 for printf-style printing

//03/26/2020 experiment with I2C_Anything

#include "I2C_Anything.h"

float rcvval = 0;

float sendval = 95141.3; //3.14159 written backwards

// Define Slave I2C Address

#define SLAVE_ADDR 9

// Define Slave answer size

#define ANSWERSIZE 5

// Define string with response to Master

String answer = "Hello";

void setup() {

// Initialize I2C communications as Slave

Wire.begin(SLAVE_ADDR);

// Function to run when data requested from master

Wire.onRequest(requestEvent);

// Function to run when data received from master

Wire.onReceive(receiveEvent);

// Setup Serial Monitor

Serial.begin(115200);

Serial.println("I2C Slave Demonstration");

}

void receiveEvent() {

//Serial.println("Receive event");

// Read while data received

//while (0 < Wire.available()) {

// byte x = Wire.read();

//}

//03/26/2020 experiment with I2C_Anything

I2C_readAnything(rcvval);

// Print to Serial Monitor

//Serial.print("Received "); Serial.print(rcvval, 6); Serial.println(" From Master");

mySerial.printf("Received %3.6f: ", rcvval);

}

void requestEvent() {

//// Setup byte variable in the correct size

//byte response[ANSWERSIZE];

//// Format answer as array

//for (byte i = 0; i < ANSWERSIZE; i++) {

// response[i] = (byte)answer.charAt(i);

//}

//// Send response back to Master

//Wire.write(response, sizeof(response));

//// Print to Serial Monitor

//Serial.println("Request event");

//03/26/2020 experiment with I2C_Anything

//Serial.print("Sending "); Serial.print(sendval, 6); Serial.println(" to Master");

mySerial.printf("Sent %6.1f\n",sendval);

I2C_writeAnything(sendval);

}

void loop() {

// Time delay in loop

delay(50);

}

Master & Slave Output:

Opening port
Port open
I2C Master Demonstration
Sent 3.14159: Received 95141.3
Sent 3.14159: Received 95141.3
Sent 3.14159: Received 95141.3
Sent 3.14159: Received 95141.3
Sent 3.14159: Received 95141.3
Sent 3.14159: Received 95141.3


Opening port
Port open
I2C Slave Demonstration
Received 3.141590: Sent 95141.3
Received 3.141590: Sent 95141.3
Received 3.141590: Sent 95141.3
Received 3.141590: Sent 95141.3
Received 3.141590: Sent 95141.3
Received 3.141590: Sent 95141.3

Opening port

Port open

I2C Master Demonstration

Sent 3.14159: Received 95141.3

Opening port

Port open

I2C Slave Demonstration

Received 3.141590: Sent 95141.3

So now I have a working Arduino-Arduino I2C master/slave setup, using I2C_Anything.h to transfer float values back and forth, and a working Teensy-Teensy I2C master/slave setup, using I2C_Anything.h to transfer float values back and forth.

The next step was to replace the Arduino Uno I2C slave with the Teensy 3.2 I2C slave and see if I can get that combination working. This turned out to be surprisingly easy – basically plug-and-pray (oops, I meant ‘play’).

Now that I have a simple Arduiono Mega Master – Teensy 3.2 Slave setup working, I can start to explore why my 4WD robot setup doesn’t work. Some possibilities:

It could be that the SBWire version of the Wire library has some hidden bugs in the slave-related code. The SBWire library eliminates the well-known and well-hated ‘hang’ bug in the Arduino wire library.
It could be that some other library I am using, like the RTC library or the Adafruit FRAM library is interfering with the Arduino-Teensy interrupts needed for master-slave communication.
It could be that the IR demod program running on the slave Teensy is somehow interfering with master-slave communications (this is very unlikely as this same program had been working flawlessly for several months).
Something else…

To start with, I copied the current ‘I2C_Master_Tut2’ VS2019 project to a new one – ‘I2C_Master_Tut3’ before making any modifications. I plan to keep a ‘breadcrumb trail’ of incrementally modified projects so that I can go backwards in case I get lost at some point (and, in my fairly long 50+ years as a practicing engineer, I have learned that ‘getting lost’ is part and parcel of any non-trivial troubleshooting project).

Once I verified that ‘Tut3’ properly communicated with the Teensy slave, I started making modifications.

Replaced #include <Wire.h> with #include SBWire.h. This failed in compile with a linker error, but succeeded after I did a ‘Build -> Clean’. After uploading the new ‘Tut3’ version to the Mega, I found that master-slave communications still worked properly. This is actually a welcome development, as that now eliminates SBWire as the culprit for lost Mega-Teensy I2C capability on the robot.
Replaced ‘#include “SBWire” with #include “MPU6050_6Axis_MotionApps_V6_12.h” and include “I2Cdev.h” (this includes SBWire.h). These two libraries are required for interfacing to the MPU6050 IMU module.

29 March 2020 Update:

So I created a new copy of the I2C master, ‘I2C_Master_Tut4’ and started adding things from the original non-working robot program.

Added all the remaining #include’s. Still works fine with the Teensy 3.2 slave
Added all the #defines. Still works fine
Added all the IRHOMING parameters. OK
Added all ENUMs, Battery constants, distance measurement support constants & array declarations, motor parameters, wheel direction constants and variables, and heading based turn parameters. No problem
Added pin assignments. No problem.
Added all the other pre-setup stuff. No problem
Added ‘ Serial1.begin(9600); //03/04/16 bugfix’. This isn’t actually used anymore in the robot project, but it is there, so it could be the culprit. Nope – master/slave comms still OK.
Added RTC initialization and support function. Still no problem
Added FRAM initialization. No problem.
Added MPU6050 initialization. No problem
Added PID distance array and incremental variance initialization. No problem
Added I/O pin initialization. No problem
Added the rest of Setup(), and all the support functions required for the POST check to run. Compiles and runs fine. Of course, no peripherals are attached, so not much happens.

At this point I have all the pre-setup and setup code incorporated into the master/slave example, and everything is still happily plugging away. So obviously the culprit hasn’t yet been identified. However, before going any farther, I think I’ll drop ‘Tut4’ into the safe-deposit box and create a ‘Tut5’ to continue on into the loop() function.

OK, so I have a ‘Tut5’ project now, with everything up to loop() incorporated from the ‘FourWD_WallE2_V2’ robot project. Now the question is, ‘What next?’. I need to figure out what is causing I2C comms between the Mega master and the Teensy 3.2 slave to fail, so I need a way to faithfully replicate/simulate/emulate the actual robot code for this function.

The current robot algorithm as pertaining to the Teensy IR Demod module

Each time through the loop, the current operating mode is determined.
- If IsIRBeamAvail() returns TRUE, the IR HOMING mode is activated
  - IsIRBeamAvail() gets three float values from the Teensy 3.2, and returns TRUE if the total of the first two values (Fin1 & Fin2) is above a set threshold. It is this function that is failing to communicate with the Teensy. More specifically, it is this function that is not successfully acquiring the three float values, and subsequently always returns FALSE.

So, it may be that all I have to do is to call IsIRBeamAvail() by itself, and modify the slave code to send back three floats as expected. If this works, then I’ll have to start suspecting the robot’s Teensy or the I2C wiring, or something else entirely.

2 April 2020 Update:

After some additional fumbling around with I2C_Anything and the PrintEx library printf() formats, I now have a working ‘IsIRBeamAvail()’ function in ‘Tut5’, as follows:

bool IsIRBeamAvail()
{
	//Purpose: Determine whether or not an IR beam is available for homing
	//Inputs: call from loop()
	//Outputs: true if an IR beam is detected, false otherwise
	//Plan:
	//	Step1: Get analog levels from all 4 IR detectors
	//	Step2: return true if any of the 4 show detection level
	//Notes:
	//	Might need some hysteresis here to avoid toggling in and out of the TRACKING_IRBEAM mode
	//	02/21/17 read each det 3 times. If any sum is less than 3 X threshold, return true.  Otherwise return false
	//	10/15/17 rewritten to use info from IR Demod Module

	//get latest info from IR Demod Module
	long Fin1 = 0; //03/30/20 needs to be 'long int' (4 bytes) here to match Teensy int (4 bytes)
	long Fin2 = 23; //03/30/20 needs to be 'long int' (4 bytes) here to match Teensy int (4 bytes)
	float SteeringValue;

	mySerial.printf("\n<<<<<<<   In IsIRBeamAvail()  >>>>>>>>>>>>>>>>>>>>\n\n");

	Wire.requestFrom(SLAVE_ADDR, sizeof(Fin1) + sizeof(Fin2)+ sizeof(SteeringValue));
	
	I2C_readAnything(Fin1);
	I2C_readAnything(Fin2);
	I2C_readAnything(SteeringValue);

	mySerial.printf("Fin1 = %ld, Fin2 = %ld, SteeringValue = %3.2f\n", Fin1, Fin2, SteeringValue);

	return false;
}

bool IsIRBeamAvail()

{

//Purpose: Determine whether or not an IR beam is available for homing

//Inputs: call from loop()

//Outputs: true if an IR beam is detected, false otherwise

//Plan:

// Step1: Get analog levels from all 4 IR detectors

// Step2: return true if any of the 4 show detection level

//Notes:

// Might need some hysteresis here to avoid toggling in and out of the TRACKING_IRBEAM mode

// 02/21/17 read each det 3 times. If any sum is less than 3 X threshold, return true. Otherwise return false

// 10/15/17 rewritten to use info from IR Demod Module

//get latest info from IR Demod Module

long Fin1 = 0; //03/30/20 needs to be 'long int' (4 bytes) here to match Teensy int (4 bytes)

long Fin2 = 23; //03/30/20 needs to be 'long int' (4 bytes) here to match Teensy int (4 bytes)

float SteeringValue;

mySerial.printf("\n<<<<<<< In IsIRBeamAvail() >>>>>>>>>>>>>>>>>>>>\n\n");

Wire.requestFrom(SLAVE_ADDR, sizeof(Fin1) + sizeof(Fin2)+ sizeof(SteeringValue));

I2C_readAnything(Fin1);

I2C_readAnything(Fin2);

I2C_readAnything(SteeringValue);

mySerial.printf("Fin1 = %ld, Fin2 = %ld, SteeringValue = %3.2f\n", Fin1, Fin2, SteeringValue);

return false;

}

Which, when connected to my basic Teensy 3.2 I2C slave program produces the following (correct) output:

<<<<<<<   In IsIRBeamAvail()  >>>>>>>>>>>>>>>>>>>>

Fin1 = 469, Fin2 = 938, SteeringValue = 1473.41

<<<<<<<   In IsIRBeamAvail()  >>>>>>>>>>>>>>>>>>>>

Fin1 = 470, Fin2 = 940, SteeringValue = 1476.55

<<<<<<<   In IsIRBeamAvail()  >>>>>>>>>>>>>>>>>>>>

Fin1 = 471, Fin2 = 942, SteeringValue = 1479.69

<<<<<<< In IsIRBeamAvail() >>>>>>>>>>>>>>>>>>>>

Fin1 = 469, Fin2 = 938, SteeringValue = 1473.41

<<<<<<< In IsIRBeamAvail() >>>>>>>>>>>>>>>>>>>>

Fin1 = 470, Fin2 = 940, SteeringValue = 1476.55

<<<<<<< In IsIRBeamAvail() >>>>>>>>>>>>>>>>>>>>

Fin1 = 471, Fin2 = 942, SteeringValue = 1479.69

At this point we have a working robot code emulation that communicates successfully with a basic Teensy 3.2 slave. So, the available culprits have been reduced significantly to

Something about the Teensy IR demod code
The I2C Wiring
A hardware problem at either the robot’s Mega controller or the robot’s Teensy 3.2 controller
Something else.

For the next step, I modified the Teensy IR Demod code to emulate the IR detector response and loaded this code into the Teensy 3.2 slave connected to the Mega master. After some initial mis-steps, it started working nicely, with the following (correct) output from the Mega master:

<<<<<<<   In IsIRBeamAvail()  >>>>>>>>>>>>>>>>>>>>

Fin1 = 6400, Fin2 = 3200, SteeringValue = 0.33

<<<<<<<   In IsIRBeamAvail()  >>>>>>>>>>>>>>>>>>>>

Fin1 = 6400, Fin2 = 3200, SteeringValue = 0.33

<<<<<<<   In IsIRBeamAvail()  >>>>>>>>>>>>>>>>>>>>

Fin1 = 6400, Fin2 = 3200, SteeringValue = 0.33

Port closed

<<<<<<< In IsIRBeamAvail() >>>>>>>>>>>>>>>>>>>>

Fin1 = 6400, Fin2 = 3200, SteeringValue = 0.33

<<<<<<< In IsIRBeamAvail() >>>>>>>>>>>>>>>>>>>>

Fin1 = 6400, Fin2 = 3200, SteeringValue = 0.33

<<<<<<< In IsIRBeamAvail() >>>>>>>>>>>>>>>>>>>>

Fin1 = 6400, Fin2 = 3200, SteeringValue = 0.33

Port closed

So this eliminates the ‘Something about the Teensy IR demod code’ possibility from the above list.

Next, I replaced the direct SCL/SDA jumpers with the daisy-chain wiring from the robot, and miracle of miracles, I2C comms failed – YAY!!. Now hopefully I can figure out (and fix) the problem.

First, I un-replaced the daisy-chain wiring to confirm that I2C comms were still working, and they were.
Unplugged the daisy-chain from the FRAM, 6050IMU, and RTC modules – now working OK
Plugged back in to FRAM unit (now have FRAM and Teensy 3.2 in chain)- – still OK
Plugged back in to 6050 IMU (now have IMU, FRAM, and Teensy 3.2 in chain)- failed
Plugged back in to RTC (now have RTC, FRAM, and Teensy 3.2 in chain)- failed

3 April 2020 Update:

The next step in the saga was to load the ‘Tut5’ code onto the robot’s Mega 2560 controller, with the slave code still running on the original (off-robot) Teensy 3.2. Setting things up in this fashion allowed the sensors to receive power from the robot’s Mega controller as in normal operation.

In the photo above, the Teensy 3.2 slave is shown to the left of the robot. The I2C ‘daisy-chain’ cable starts at the Mega controller (just to the right of the front left wheel), and goes through three ‘hops’ to the Teensy. With this setup, the I2C comms code still ran fine, with direct I2C jumper wires or with the daisy-chain wiring setup (with no connections to the other I2C sensors) as shown above.

The next step was to connect the robot’s Mega controller to the robot’s Teensy 3.2 running the unmodified IR Demodulation code, with the I2C daisy-chain cable, but without anything else connected. This actually worked! So now I have a working robot system again, so something about the connections with the other sensors must be killing the I2C link to the Teensy. Here’s a scope photo of the SDA line showing the data activity. The vertical scale is 1V/cm, showing the HIGH value is about 3.8V, so the Mega must be comfortable with that value as a HIGH logic level

In the above photo, notice the business card for Probe Master, Inc (www.probemaster.com). These folks were kind enough to replace my ten year old scope probe with a new upgraded version at no cost – thank you Probe Master!

At this point I have the ‘Tut5’ program running on the Mega, and the unmodified IR Demod code running on the robot’s Teensy 3.2, and the Mega appears to be acquiring valid demod data from the Teensy. To further validate the data, I fired up my charging station with it’s square-wave modulated IR beam, and was pleased to see that the acquired data from the Teensy varied appropriately as I manually rotated the robot, as shown in the following Excel chart.

So, after all this work, the whole thing came down to a bad I2C ‘daisy-chain’ cable. This particular cable has been on the robot for a while, and was the soldering graduation exercise of my grandson Danny when he was here a couple of summers ago. It wasn’t the best job I’ve ever seen, but it was pretty good for a 15 year old ;-). In any case, I took the opportunity to build a new cable with smaller diameter wire so things would fit a little bit better into the Pololu pins, sockets, and 2-pin header sleeves as shown in the following photo.

New I2C ‘daisy-chain’ cable. Run starts from the Mega connector at bottom left, then to the RTC, IMU and FRAM modules in that order, then last to the Teensy 3.2 IR Demod module.

05 April 2020 Epilogue:

Well, there was one last ‘gotcha’ in all this. When I loaded the original ‘FourWD-WallE2_V2.ino’ program back onto the Mega, it still refused to acquire valid data from the Teensy IR Demod module. So, I compared the ‘Tut5’ code to the ‘_V2’ code, and noticed three significant differences:

In the ‘_V2’ code, the ‘Fin1/2’ variables had at some time been changed from ‘long’ to ‘int’. While this sounds reasonable, it isn’t – because a Teensy ‘int’ is 4 bytes – the same as a Mega ‘long’.
In the ‘_V2’ code, the Wire.requestFrom() call asks for 32 bytes, but the Teensy only sends 12.
In the ‘_V2’ code there is a loop that waits for either a timeout or receiving an entire 32-byte buffer from the Teensy. Apparently the Teensy (and/or the Mega) is slow enough so that Wire.Available() never reports a non-zero buffer size, so the loop times out.

Replacing the line

	uint8_t numbytes = Wire.requestFrom(slaveAddr, (size_t)32); //this works, with Wire.Available() reporting 32

1	uint8_t numbytes = Wire.requestFrom(slaveAddr, (size_t)32); //this works, with Wire.Available() reporting 32

with the line

	Wire.requestFrom(slaveAddr, sizeof(Fin1) + sizeof(Fin2) + sizeof(SteeringValue));

1	Wire.requestFrom(slaveAddr, sizeof(Fin1) + sizeof(Fin2) + sizeof(SteeringValue));

did the trick, but I have no idea why.

Replacing the debug printout lines

Serial.print("in IsIRBeamAvail(): got ");
Serial.print(Fin1); Serial.print(", ");
Serial.print(Fin2); Serial.print(", total =  ");
//Serial.print(SteeringValue); Serial.println(" from slave");
Serial.println(total);

Serial.print("in IsIRBeamAvail(): got ");

Serial.print(Fin1); Serial.print(", ");

Serial.print(Fin2); Serial.print(", total = ");

//Serial.print(SteeringValue); Serial.println(" from slave");

Serial.println(total);

with the line

mySerial.printf("Fin1 = %ld, Fin2 = %ld, SteeringValue = %3.2f\n", Fin1, Fin2, SteeringValue);

1	mySerial.printf("Fin1 = %ld, Fin2 = %ld, SteeringValue = %3.2f\n", Fin1, Fin2, SteeringValue);

Produced lines like this one from the ‘_V2’ program

Fin1 = 390556, Fin2 = 153386, SteeringValue = 0.44
IR Beam Detected with Battery Voltage = 7.32DateTime	Batt	FinVal1

1 2	Fin1 = 390556, Fin2 = 153386, SteeringValue = 0.44 IR Beam Detected with Battery Voltage = 7.32DateTime Batt FinVal1

Which appear to be the correct values for the given robot orientation with respect to the charging station.

With no other changed except removing power from the charging station, the output became

Fin1 = 14, Fin2 = 28, SteeringValue = -0.33
Fin1 = 66, Fin2 = 42, SteeringValue = 0.22
Fin1 = 26, Fin2 = 48, SteeringValue = -0.30
Fin1 = 54, Fin2 = 30, SteeringValue = 0.29

Fin1 = 14, Fin2 = 28, SteeringValue = -0.33

Fin1 = 66, Fin2 = 42, SteeringValue = 0.22

Fin1 = 26, Fin2 = 48, SteeringValue = -0.30

Fin1 = 54, Fin2 = 30, SteeringValue = 0.29

showing definitively that the ‘_V2’ program correctly identified and decoded the square-wave modulated IR beam.

So, after a two-week off-road trip to I2C comms hell and back, I think I finally have that particular issue put to bed. It wasn’t exactly what I wanted to do, but it was at least interesting, and more important – consumed a lot of coronavirus quarantine time ;-).

Stay tuned!

Frank

Arduino SPI Data Exchange Between Two Arduinos in a Master/Slave Configuration

2 Replies

posted 13 March 2020,

While updating my four wheel autonomous wall-following robot (aka Wall-E2), I ran into a roadblock when I couldn’t get my Teensy 3.2 IR demodulator/tracker module to communicate with the Mega 2560 main microcontroller over I2C. This worked fine when I last tested it, but now it seems to have taken a vacation. It’s been a while, and I have added some functionality since I originally installed and tested IR homing to the charging station, but it still should all work, right?

In any case, after trying (and failing) to get the I2C bus connection working, I thought I might try an alternate solution and just use SPI between the main controller (Mega 2560) and the IR homing controller (Teensy 3.2). This would have the advantage of making the Teensy independent of the I2C bus, and also give me the chance to play with a part of the Arduino ecosystem that I haven’t used before.

As usual, I started this process with a lot of Googling, and quickly ran across Australian Nick Gammon’s “SPI – Serial Peripheral Interface – for Arduino“ . This post was way more than I ever wanted to know about SPI, but it sure is complete!

Then I moved on to trying to get SPI working for myself. As noted above, my intended application is to transfer steering values from my IR detector/demodulator/homing module to the main Mega 2560 microcontroller. The Mega uses the steering data to adjust wheel speeds to home in on a charging station, so Wall-E2 can continue to roam our house autonomously.

It turns out that SPI between two Arduinos isn’t entirely straightforward, at least not at first. I went through a bunch of iterations, and even more passes through the available documentation. In the end though, I think I wrassled it into a reasonable facsimile of a working solution, as shown below. The program repeatedly transfers a fixed string (“Hello World!”) from the UNO (master) to the Mega 2560 (slave), and then transfers the string version of a float value (3.159) from the slave to the master.

The Circuit:

The circuit and layout is just about as basic as it gets. The UNO (master) connections are the default pinouts:

UNO (master) connected to Mega 2560 (slave) using default SPI pins on both ends

The Master:

/*
    Name:       SPI_EX3_Master.ino
    Created:	3/13/2020 4:19:35 PM
    Author:     FRANKNEWXPS15\Frank
*/


#include <SPI.h>
#include "pins_arduino.h"
#include <PrintEx.h> //allows printf-style printout syntax
StreamEx mySerial = Serial; //added 03/18/18 for printf-style printing

volatile byte rcvbyte = 0;
char rcvbuf[100];

void setup(void)
{
    Serial.begin(115200);

    // Put SCK, MOSI, SS pins into output mode
    // also put SCK, MOSI into LOW state, and SS into HIGH state.
    // Then put SPI hardware into Master mode and turn SPI on
    SPI.begin();

    Serial.println("SPI Example Master");

    memset(rcvbuf, 0, sizeof(rcvbuf));

}  // end of setup

void loop(void)
{

    char c;

    // send test string
    Serial.println("Sending Test String");

    //tell slave to get ready
    digitalWrite(SS, LOW);    // SS is pin 10
    delayMicroseconds(200); //give slave some time 

    // send test string
    for (const char* p = "Hello, world!\n"; c = *p; p++)
    {
        mySerial.printf("Sending %x to Slave\n", c);
        SPI.transfer(c);
        delayMicroseconds(20); //required for arduino-arduino xfrs
    }

    //receive data from slave
    delay(500);

    //get float value from slave
    Serial.println("Receiving Data String\n");
    int pos = 0;
    memset(rcvbuf, sizeof(rcvbuf), 0);

    c = 1;
    while (c != 0x0D)
    {
        c = SPI.transfer(0);
        mySerial.printf("got %x from Slave\n", c);
        rcvbuf[pos++] = c;
        delayMicroseconds(20);
    }

    //remove any non-numeric characters
    char buf2[20];
    int buf2pos = 0;
    for (size_t i = 0; i < sizeof(rcvbuf); i++)
    {
        char p = rcvbuf[i];  // search for space
        if (p > 0x20 )
        {
            buf2[buf2pos++] = p;
        }
        buf2[buf2pos] = 0;
    }

    rcvbuf[pos] = 0; //add NULL to terminate string

    if (pos > 0)
    {
        mySerial.printf("received %s from slave\n", buf2);
    }

    // tell slave to get ready for next data transfer
    digitalWrite(SS, HIGH);

    delay(1000);  // 1 seconds delay
}  // end of loop

Name: SPI_EX3_Master.ino

Created: 3/13/2020 4:19:35 PM

Author: FRANKNEWXPS15\Frank

#include <SPI.h>

#include "pins_arduino.h"

#include <PrintEx.h> //allows printf-style printout syntax

StreamEx mySerial = Serial; //added 03/18/18 for printf-style printing

volatile byte rcvbyte = 0;

char rcvbuf[100];

void setup(void)

{

Serial.begin(115200);

// Put SCK, MOSI, SS pins into output mode

// also put SCK, MOSI into LOW state, and SS into HIGH state.

// Then put SPI hardware into Master mode and turn SPI on

SPI.begin();

Serial.println("SPI Example Master");

memset(rcvbuf, 0, sizeof(rcvbuf));

} // end of setup

void loop(void)

{

char c;

// send test string

Serial.println("Sending Test String");

//tell slave to get ready

digitalWrite(SS, LOW); // SS is pin 10

delayMicroseconds(200); //give slave some time

// send test string

for (const char* p = "Hello, world!\n"; c = *p; p++)

{

mySerial.printf("Sending %x to Slave\n", c);

SPI.transfer(c);

delayMicroseconds(20); //required for arduino-arduino xfrs

}

//receive data from slave

delay(500);

//get float value from slave

Serial.println("Receiving Data String\n");

int pos = 0;

memset(rcvbuf, sizeof(rcvbuf), 0);

c = 1;

while (c != 0x0D)

{

c = SPI.transfer(0);

mySerial.printf("got %x from Slave\n", c);

rcvbuf[pos++] = c;

delayMicroseconds(20);

}

//remove any non-numeric characters

char buf2[20];

int buf2pos = 0;

for (size_t i = 0; i < sizeof(rcvbuf); i++)

{

char p = rcvbuf[i]; // search for space

if (p > 0x20 )

{

buf2[buf2pos++] = p;

}

buf2[buf2pos] = 0;

}

rcvbuf[pos] = 0; //add NULL to terminate string

if (pos > 0)

{

mySerial.printf("received %s from slave\n", buf2);

}

// tell slave to get ready for next data transfer

digitalWrite(SS, HIGH);

delay(1000); // 1 seconds delay

} // end of loop

The Slave:

/*
 Name:		SPI_EX3_Slave.ino
 Created:	3/13/2020 4:21:43 PM
 Author:	Frank
*/


#include "pins_arduino.h"
#include <SPI.h>
#include <PrintEx.h> //allows printf-style printout syntax
StreamEx mySerial = Serial; //added 03/18/18 for printf-style printing

char buf[100];
char val[20];
volatile byte rcv_pos;
volatile byte xmt_pos;
volatile boolean process_it;
volatile boolean bReceivingString;

void setup(void)
{
    Serial.begin(115200);   // debugging
    mySerial.printf("SPI Example Slave\n");

    // have to send on master in, *slave out*
    pinMode(MISO, OUTPUT);
    pinMode(PIN_SPI_SS, INPUT_PULLUP); //SS is INPUT for slave device

    // turn on SPI in slave mode
    SPCR |= _BV(SPE);

    // turn on interrupts
    SPCR |= _BV(SPIE);

    rcv_pos = 0;
    process_it = false;

    //use dstrtof to produce string version of float value
    float x = 3.14159;
    memset(val, 0x0D, sizeof(val));
    dtostrf(x, 7, 5, val);
    mySerial.printf("string representation of %2.5f is %s\n", x, val);

    for (size_t i = 0; i < sizeof(val) - 1; i++)
    {
        mySerial.printf("%x", val[i]);
    }
    mySerial.printf("%x\n", val[sizeof(val) - 1]);

    xmt_pos = 0;

}  // end of setup


// SPI interrupt routine
ISR(SPI_STC_vect)
{
    if (bReceivingString)
    {
        byte c = SPDR;

        // add to buffer if room
        if (rcv_pos < sizeof buf)
        {
            buf[rcv_pos++] = c;

            //newline means time to process buffer
            if (c == '\n')
                process_it = true;

        }  // end of room available
    }
    else
    {
        SPDR = val[xmt_pos];
        xmt_pos++;
    }
}

// main loop - wait for flag set in interrupt routine
void loop(void)
{
    if (process_it)
    {
        mySerial.printf("In process_it with rcv_pos = %d\n",
            rcv_pos);
        buf[rcv_pos] = 0;
        mySerial.printf("Received %s from Master\n", buf);
        rcv_pos = 0;
        process_it = false;
        bReceivingString = false;
    }  // end of flag set

    if (digitalRead(PIN_SPI_SS) == HIGH)
    {
        xmt_pos = 0; //get ready for next iteration
        SPDR = 0;
        bReceivingString = true;
        process_it = false;

    }

}  // end of loop

100

101

102

103

Name: SPI_EX3_Slave.ino

Created: 3/13/2020 4:21:43 PM

Author: Frank

#include "pins_arduino.h"

#include <SPI.h>

#include <PrintEx.h> //allows printf-style printout syntax

StreamEx mySerial = Serial; //added 03/18/18 for printf-style printing

char buf[100];

char val[20];

volatile byte rcv_pos;

volatile byte xmt_pos;

volatile boolean process_it;

volatile boolean bReceivingString;

void setup(void)

{

Serial.begin(115200); // debugging

mySerial.printf("SPI Example Slave\n");

// have to send on master in, *slave out*

pinMode(MISO, OUTPUT);

pinMode(PIN_SPI_SS, INPUT_PULLUP); //SS is INPUT for slave device

// turn on SPI in slave mode

SPCR |= _BV(SPE);

// turn on interrupts

SPCR |= _BV(SPIE);

rcv_pos = 0;

process_it = false;

//use dstrtof to produce string version of float value

float x = 3.14159;

memset(val, 0x0D, sizeof(val));

dtostrf(x, 7, 5, val);

mySerial.printf("string representation of %2.5f is %s\n", x, val);

for (size_t i = 0; i < sizeof(val) - 1; i++)

{

mySerial.printf("%x", val[i]);

}

mySerial.printf("%x\n", val[sizeof(val) - 1]);

xmt_pos = 0;

} // end of setup

// SPI interrupt routine

ISR(SPI_STC_vect)

{

if (bReceivingString)

{

byte c = SPDR;

// add to buffer if room

if (rcv_pos < sizeof buf)

{

buf[rcv_pos++] = c;

//newline means time to process buffer

if (c == '\n')

process_it = true;

} // end of room available

}

else

{

SPDR = val[xmt_pos];

xmt_pos++;

}

// main loop - wait for flag set in interrupt routine

void loop(void)

{

if (process_it)

{

mySerial.printf("In process_it with rcv_pos = %d\n",

rcv_pos);

buf[rcv_pos] = 0;

mySerial.printf("Received %s from Master\n", buf);

rcv_pos = 0;

process_it = false;

bReceivingString = false;

} // end of flag set

if (digitalRead(PIN_SPI_SS) == HIGH)

{

xmt_pos = 0; //get ready for next iteration

SPDR = 0;

bReceivingString = true;

process_it = false;

}

} // end of loop

The above configuration produced the following output:

Master:

Opening port
Port open
SPI Example Master
Sending Test String
Sending 48 to Slave
Sending 65 to Slave
Sending 6C to Slave
Sending 6C to Slave
Sending 6F to Slave
Sending 2C to Slave
Sending 20 to Slave
Sending 77 to Slave
Sending 6F to Slave
Sending 72 to Slave
Sending 6C to Slave
Sending 64 to Slave
Sending 21 to Slave
Sending A to Slave
Receiving Data String

got A from Slave
got 33 from Slave
got 2E from Slave
got 31 from Slave
got 34 from Slave
got 31 from Slave
got 35 from Slave
got 39 from Slave
got 0 from Slave
got D from Slave
received 3.14159 from slave

Opening port

Port open

SPI Example Master

Sending Test String

Sending 48 to Slave

Sending 65 to Slave

Sending 6C to Slave

Sending 6F to Slave

Sending 2C to Slave

Sending 20 to Slave

Sending 77 to Slave

Sending 6F to Slave

Sending 72 to Slave

Sending 6C to Slave

Sending 64 to Slave

Sending 21 to Slave

Sending A to Slave

Receiving Data String

got A from Slave

got 33 from Slave

got 2E from Slave

got 31 from Slave

got 34 from Slave

got 31 from Slave

got 35 from Slave

got 39 from Slave

got 0 from Slave

got D from Slave

received 3.14159 from slave

Slave:

Opening port
Port open
SPI Example Slave
string representation of 3.14159 is 3.14159
332E31343135390DDDDDDDDDDDD
In process_it with rcv_pos = 14
Received Hello, world!
 from Master
In process_it with rcv_pos = 14
Received Hello, world!
 from Master
In process_it with rcv_pos = 14
Received Hello, world!
 from Master
In process_it with rcv_pos = 14
Received Hello, world!
 from Master
In process_it with rcv_pos = 14
Received Hello, world!
 from Master
In process_it with rcv_pos = 14
Received Hello, world!
 from Master

Port closed

Opening port

Port open

SPI Example Slave

string representation of 3.14159 is 3.14159

332E31343135390DDDDDDDDDDDD

In process_it with rcv_pos = 14

Received Hello, world!

from Master

In process_it with rcv_pos = 14

Received Hello, world!

from Master

In process_it with rcv_pos = 14

Received Hello, world!

from Master

In process_it with rcv_pos = 14

Received Hello, world!

from Master

In process_it with rcv_pos = 14

Received Hello, world!

from Master

In process_it with rcv_pos = 14

Received Hello, world!

from Master

Port closed

Now that I’ve had my fun and games with SPI, I’m still not at all sure I want to use it to connect the Mega to the Teensy IR Demodulator/Homing module. While it is almost certain to work (eventually), it has some drawbacks

It requires 4 additional wires
It requires that I add an ISR and other code to the Teensy module, and companion code to the Mega
More things to go wrong.

So, I think I’ll try again to get the original I2C based code working, as it worked before, so it should work again. If I can’t get it working after another good try, I’ll go with door #2 (SPI)

Stay tuned!

Frank

Back to the future with Wall-E2. Wall-following Part VIII

1 Reply

Posted 25 January 2020

About 6 weeks ago I posted that I had finally killed the “intermittent MPU6050 failure” dragon, by belatedly following Pololu’s recommendations for installing bypass capacitors on their metal-geared motors. Unfortunately it turned out that my celebration was cut short by more annoying intermittent MPU6050 failures, so I was once again forced back to the drawing board.

This time I decided that the only way to figure out what was going on was to actively examine the I2C traffic in real time, to determine who exactly was doing what to whom. So, over the course of the six week period between my last declaration of victory to this one, I created a Teensy based I2C bus ‘sniffer’ and used it to figure out what was going on. I was able to determine that the ‘master’ micro-controller continued to operate normally through a failure, but the MPU6050 didn’t. I was also able to determine that just resetting the IMU would not allow the system to recover, but resetting the micro-controller often did. Moreover, I was able to definitively show that the problem was caused by ‘contact bounce’ on one or more of the four 6″ male-male jumper wires connecting the micro-controller to the IMU. Eliminating these jumpers also (I hope) eliminated the last piece of the “I2C Intermittent failure” puzzle.

Looking back over the entire I2C failure saga, I now realize that this was the classic case of multiple failure modes complicating the troubleshooting effort. The RFI/EMI problem caused by the Pololu metal-geared motors completely overshadowed the issue of non-secure jumper connections. Then, after finally coming to my senses and installing the recommended bypass capacitors on the motors, the ‘contact bounce’ problem was unmasked. I do love interesting problems, but this one went past ‘interesting’ and was well into ‘agonizing’ by the time I got it solved ;-).

After getting everything set up, I ran some wall tracking tests in my entry hall ‘test range’ with pretty good results, as shown in the short video clip below.

Stay tuned,

Frank

30 January 2020 Update:

Still having trouble with the initial approach to a wall from outside the target distance. The robot still has a tendency to dive into the wall, unable to cope with the problem of the measured distance increasing instead of decreasing when the robot turns into the wall. This inverse relationship makes it almost impossible to use a simple ‘turn toward the wall and wait for the distance to count down’ technique.

After thinking about this for while, I realized that this would all be so much simpler if I cheated and started with the robot placed parallel to the target wall. Then the robot could simply turn 45 deg toward the wall and proceed until the measured wall distance was appropriate (Dtgt / 0.707), and then turn parallel again. Then I realized that I could easily determine the parallel condition by turning the robot toward and/or away from the wall while continuously measuring the distance; when the measurement goes through a minimum, then the robot is parallel to the wall. Simple in concept, and not all that hard to program, either.

IMU Motor Noise Troubleshooting, Part III

1 Reply

Posted 19 January 2020

In Part II of this saga, I described my continuing efforts to track down and fix the problem of intermittent failures associated with the MPU6050 IMU on my robot. The MPU6050 IMU is required for the ability to make precise heading-based turns, which is in turn required to track walls at a designated stand-off distance.

This post summarizes the work to date and suggests new avenues of investigation for fully addressing the motor noise issue.

Summary of work to date:

July 2019: First started working with heading-based turns, and first noticed the motor noise problem. Basically the problem presented itself as frequent, abrupt, and wildly divergent heading readings when the motors were running, but perfectly stable readings when the motors are not running. See this post for the details.
October 2019: Successfully demonstrated polling-based (vs interrupt-based) MPU6050 IMU management. This development meant that I could acquire yaw (heading) values on an as-needed basis rather than at a 20 or 200Hz rate, throwing away 99% of the results. This was demonstrated in this post.
November 2019: Made another run at solving the motor noise problem using a home-brew optical isolator and a 2-stage power filter. After a LOT of work, I wound up discovering that most (but not all!) of the problem could be addressed with proper RF bypassing at the terminals of the metal-geared Pololu motors I was using. See this post for the details.
Early December 2019: Demonstrated heading-based wall offset tracking using my 2-motor robot, with RF bypassing installed on both Pololu metal gear motors. No IMU failures were noticed during these runs. See this post for details.
Early December 2019: Reprised some of the motor driver testing performed back in May of 2019 (see this post), and again noticed MPU6050 IMU communication failures when the motors were running, but none when the motors weren’t running. This test was performed on the 2-motor robot using the Pololu motors with the RF bypassing in place. So clearly just the bypassing was not of and by itself sufficient to solve the problem; something else had to be going on. See this post for the details.
Late December 2019 to mid-January 2020: I decided I needed a tool to monitor the I2C bus traffic between the robot’s controller and the MPU6050 IMU – an I2C ‘sniffer’. After some research, I found that the cheapest commercial sniffer cost about $330, and DIY sniffers were few and far between. I did, however, find a Teensy-based sniffer program by Kito, so I had a starting place. After three major development stages, I had a Teensy 3.2 program that would reliably monitor I2C communications between an Arduino (Mega or Uno) master and a MPU6050 slave, using the polling approach developed earlier. See this post, this post, and this post for the development details.

Current Effort:

With the above history in mind, I applied my new I2C sniffer tool to the Motor Noise Problem. As usual, I started this using the simplest possible setup; an Arduino Mega acting as the I2C master running my polling based ‘MPU6050_MotorNoiseTest1’ program, and a Teensy 3.2 and a MPU6050 IMU module both mounted on a small plugboard, as shown below.

Arduino Mega I2C master, with Teensy I2C sniffer and MPU6050 module on a separate plugboard

I played around with this setup for a while, and captured at least one IMU communications failure with the sniffer active. The failure occurred when I was moving the plugboard around a bit to verify that the MPU6050 IMU heading values changed appropriately. At some point I noticed the I2C monitor output had changed its character significantly, so I quickly stopped the sniffer program and opened the log file (see attached file below).

From the log I can see that things proceeded normally until 6012443 mSec ( 1.67 hours) and then changed to report that nothing was being received from register 0x72 (the FIFO count register). This continued until 6022224 mSec (9.8 seconds later) where it returned to what looks like normal operation.

So, my preliminary guess at what happened is the connection from the Mega to the Teensy/MPU6050 got dropped momentarily, and it took the Teensy a while to find another START sequence in the I2C data stream from the Mega, as the ‘2048’ number in “6017280: processed = 2048 elements in 3 mSec” means that the capture buffer overflowed before a START sequence was detected. “At 6022240: processed = 1224 elements in 2 mSec” means that a normal Mega ‘burst’ was captured and operation returned to normal.

Since the Teensy I2C monitor is on the MPU6050 end of the male-male jumpers, It begins to look like the Mega was still doing fine, but the jumper connection burped on one end or the other. More testing to follow.

Taking too long?

Reload document

Open in new tab

Download

Next, I moved the plugboard containing the Teensy I2C Sniffer and the MPU6050 module to my 2-motor robot, and used the existing Arduino Uno on the robot as the master, as shown below.

I loaded my MotorNoiseTest1 program on the Uno, and allowed it to run both motors at a steady rate, while monitoring the I2C traffic with the Teensy, and also monitoring the heading values being printed out by the Uno. I started the program just before 1PM, and it was still running fine with no IMU errors at 10pm, more than 9 hours later! The I2C sniffer log shows regular communication with the MPU6050, and the calculated yaw value based on the packet bytes received by the sniffer program matches the yaw value calculated in the Uno program. This is clear verification that the sniffer program will run ‘forever’, and that at least in this case, the two motor robot will also run ‘forever’ with no yaw errors.

Based on my earlier experience with the captured I2C communications failure, I’m more inclined now to believe that motor vibration or other mechanical perturbation is causing a momentary I2C bus or power/ground lead disconnect. More tests to follow:

21 January 2020 Update:

After the 10-hour run described above, I tried to induce some failures by fiddling with the I2C and power/ground jumper wires, and found that I could easily and reliably cause a failure by ‘flicking’ the wires with my finger or a pen. After each failure, the built-in recovery routine of clearing the FIFO and resetting the DMP failed to restore communications. However, manually resetting the UNO did allow the system to recover.

From the above, I believe it’s safe to say that the current male-male jumper connections between the UNO and the Teensy/IMU are unreliable, and are hopefully the only remaining failure mode. I haven’t quite figured out how to replace the connections with something more reliable, but I’m working on it. I moved the IMU module from the plugboard and plugged its I2C pins directly into the I2C sockets on the UNO. Then I replaced the power & ground leads with a permanent twisted pair connection to the Wixel shield, as shown in the following photo.

MPU6050 plugged directly into Uno board, with pwr/gnd jumpers replaced with permanent twisted pair

Then I fired up the system and ran it for a while but was unable to make it fail. This is encouraging news to say the least.

Stay tuned,

Frank

Teensy I2C Sniffer for MPU6050 Part II

1 Reply

Posted 13 January 2020,

In my last post on this subject, I described my efforts to build an I2C bus sniffer using a Teensy 3.2 micro-controller. This post describes my efforts to move from a fixed array containing a 928-byte snapshot of an I2C bus conversation between an Arduino Mega 2560 and a MPU6050 IMU to a live, repeated-burst setup.

As the source for I2C traffic for the MPU6050 IMU I am using my MPU6050_MotorNoiseTest1 Arduino project with no motors or sensors connected. All the code does is ask the MPU6050 for a yaw value every 200 mSec (the value of NAV_UPDATE_INTERVAL_MSEC), as shown below:

void loop()
{
	if (bFirstTime)
	{
		bFirstTime = false;
		mySerial.printf("Min\tYaw\tLDist\tRDist\n");
	}

	if (sinceLastNavUpdateMsec >=  NAV_UPDATE_INTERVAL_MSEC)
	{
		sinceLastNavUpdateMsec -= NAV_UPDATE_INTERVAL_MSEC;

		bool getYawResult = GetIMUHeadingDeg(); //updates global_yawval if successful

		if (getYawResult == 0)
		{
			if (mpu.testConnection())
			{
				mySerial.printf("resetting IMU\n");
				mpu.reset();
				delay(50);
			}
		}

		int leftdist = GetLeftDistCm();
		int rightdist = GetRightDistCm();

		mySerial.printf("%3.2f\t%3.2f\t%d\t%d\n", millis()/60000.0, global_yawval, leftdist, rightdist);
	}
}

void loop()

{

if (bFirstTime)

{

bFirstTime = false;

mySerial.printf("Min\tYaw\tLDist\tRDist\n");

}

if (sinceLastNavUpdateMsec >= NAV_UPDATE_INTERVAL_MSEC)

{

sinceLastNavUpdateMsec -= NAV_UPDATE_INTERVAL_MSEC;

bool getYawResult = GetIMUHeadingDeg(); //updates global_yawval if successful

if (getYawResult == 0)

{

if (mpu.testConnection())

{

mySerial.printf("resetting IMU\n");

mpu.reset();

delay(50);

}

int leftdist = GetLeftDistCm();

int rightdist = GetRightDistCm();

mySerial.printf("%3.2f\t%3.2f\t%d\t%d\n", millis()/60000.0, global_yawval, leftdist, rightdist);

}

The Teensy code to monitor the I2C bus traffic is shown below. When I first started working with this project, I copied Kito’s I2C sniffer code, which used Teensy’s Timer1 interval timer set to produce interrupts every 1 uSec, and an ISR to capture the data. This turned out to be hard to deal with, as I couldn’t add instrumentation code to the ISR without overrunning the 1 uSec interrupt period, leading to confusing results. So, for this part of the project I disabled the Timer1 interrupt, and called the ISR directly from the loop() function. As others have pointed out, the Arduino loop() function does a lot of housekeeping in the background, so for top performance it is best to never let loop() execute, by placing another infinite loop inside loop() or inside setup(). This is what I did with the code designed to investigate whether or not the Teensy could keep up with an I2C bus running at 100Kbs.

/*
    Name:       Teensy_I2C_Sniffer_V9.ino
    Created:	1/10/2020 4:15:25 PM
    Author:     FRANKNEWXPS15\Frank
*/


#include <TimerOne.h> //needed for ISR

const uint16_t CAPTURE_ARRAY_SIZE = 2048;
const uint16_t VALID_DATA_ARRAY_SIZE = 2048;

uint8_t raw_data[CAPTURE_ARRAY_SIZE]; //holds data captured from I2C bus
uint8_t valid_data[VALID_DATA_ARRAY_SIZE]; //invalid bytes removed
//const int WAITING_PRINT_INTERVAL_MSEC = 200;//interval timer for 'Waiting for data...' printout

#define MONITOR_OUT1 2 //so can monitor ISR activity with O'scope
#define MONITOR_OUT2 3 //so can monitor ISR activity with O'scope
#define MONITOR_OUT3 4 //so can monitor ISR activity with O'scope
#define SDA_PIN 18 
#define SCL_PIN 19


//#define PARSE_LOOP_DEBUG
//#define GET_8BIT_DATABYTE_DEBUG

#pragma region PROCESSING_VARIABLES
uint8_t devAddr;
uint8_t regAddr;
uint8_t databytes[2048]; //holds multiple databytes for later output sentence construction
uint16_t numbytes = 0; //number of data bytes extracted from data stream
int ACKNAKFlag; //can be negative
uint16_t databyte_idx = 0; //index into databyte_array
uint8_t killbuff[2]; //used to consume start/stop bytes
elapsedMillis mSecSinceLastWaitingPrint;
uint16_t numvalidbytes = 0; //number of valid bytes in this burst
uint16_t read_idx = 0; //pointer to next byte pair to be processed
unsigned long curMsec;
unsigned long lastMsec;

//added for bus direction labels
enum BUSDIR
{
    WRITE,
    READ,
    UNKNOWN = -1
} RWDir;
BUSDIR BusDir = BUSDIR::UNKNOWN;

//added for bus direction labels
enum BUS_STATES
{
    IDLE, //repeated 0xC/0xC
    ACTIVE,//0xC/0x4 transition
    COUNTING, //0x4/0xC transition, but 0XC count < MAC_0XC_COUNT
    PROCESSING //0XC count >= MAC_0XC_COUNT 
};

enum BUS_STATES last_State = IDLE;
enum BUS_STATES BusState = IDLE;


#pragma region ISR_SUPPORT
volatile uint16_t  write_idx = 0;
volatile uint16_t  BurstLength = 0;
uint16_t  read_index = 0;
uint8_t   current_portb = 0xFF;
uint8_t   last_portb = 0xFF;
bool bBufferFull = false;
uint16_t idlecount = 0; //used to detect EOM (end-of-message)
//const uint16_t MAX_IDLE_COUNT = 10000; //how many idle cycles before EOM detection
//const uint16_t MAX_IDLE_COUNT = 5000; //how many idle cycles before EOM detection
const uint16_t MAX_IDLE_COUNT = 2500; //how many idle cycles before EOM detection
//const uint16_t MAX_IDLE_COUNT = 1000; //how many idle cycles before EOM detection
//const uint16_t MAX_IDLE_COUNT = 20000; //how many idle cycles before EOM detection
//const uint16_t MAX_IDLE_COUNT = 30000; //how many idle cycles before EOM detection

volatile bool bStartCapture = false;
volatile uint16_t mult0xCCount = 0;
#pragma endregion ISR Support

volatile bool bDone = false;
unsigned long timevals[100];
uint8_t timevalidx = 0;
//-------------------------------------------------------------------------------
//--------------------------------    ISR    ------------------------------------
//-------------------------------------------------------------------------------
//01/06/2020 modified back to capture data from SDA/SCL lines
//01/09/20 modified to just capture between START & STOP
void capture_data(void)
{
    last_portb = current_portb;
    current_portb = GPIOB_PDIR & 12; //reads state of SDA (18) & SCL (19) at same time

    if (last_portb == 0xC && current_portb == 0x4) //START  
    {
        digitalWriteFast(MONITOR_OUT2, HIGH);
        digitalWriteFast(MONITOR_OUT3, HIGH);
        bDone = false;
        mult0xCCount = 0;
    }
    else if (last_portb == 0x4 && current_portb == 0xc) //STOP
    {
        digitalWriteFast(MONITOR_OUT2, LOW);
        mult0xCCount = 0;
    }
    else if (mult0xCCount < MAX_IDLE_COUNT && last_portb == 0xc && current_portb == 0xc)
    {
        mult0xCCount++;
        if (mult0xCCount >= MAX_IDLE_COUNT)
        {
            digitalWriteFast(MONITOR_OUT3, LOW);
            //mult0xCCount = 0;
            //Serial.printf("%lu: wrote %d bytes\n", millis(),write_idx);
            Serial.printf("%lu\t%d\n", millis(),write_idx);
            bDone = true;
        }
    }
    if (!bDone)
    {
        digitalWriteFast(MONITOR_OUT1, HIGH);
        raw_data[write_idx++] = last_portb;
        raw_data[write_idx++] = current_portb;
        if (write_idx >= CAPTURE_ARRAY_SIZE)
        {
            //BurstLength = write_idx;
            write_idx = 0;
        }
        digitalWriteFast(MONITOR_OUT1, LOW);
    }
}


//-------------------------------------------------------------------------------
//-------------------------------- END ISR    ---------------------------------
//-------------------------------------------------------------------------------


void setup()
{
    unsigned long now = millis();
    Serial.begin(1); //rate value ignored
    int idx = 0;
    while (!Serial && (millis() - now) < 3000)
    {
        delay(500);
        idx++;
    }
    Serial.printf("Serial available after %lu mSec\n", millis() - now);

    pinMode(MONITOR_OUT1, OUTPUT);
    pinMode(MONITOR_OUT2, OUTPUT);
    pinMode(MONITOR_OUT3, OUTPUT);
    pinMode(LED_BUILTIN, OUTPUT);

    digitalWrite(MONITOR_OUT1, LOW);
    digitalWrite(MONITOR_OUT2, LOW);
    digitalWrite(MONITOR_OUT3, LOW);

    pinMode(SDA_PIN, INPUT);
    pinMode(SCL_PIN, INPUT);

    //reset port byte vars & start timer
    last_portb = current_portb = 0;
    write_idx = 0;
    memset(raw_data, 0, CAPTURE_ARRAY_SIZE);
    PrintNextArrayBytes(raw_data, 0, 20);
    //Timer1.initialize(1); // run every mico second
    //Timer1.attachInterrupt(capture_data);

    Serial.printf("mSec\tlast\tnew\tmult\tburst\n");


    mSecSinceLastWaitingPrint = 0;
    last_State = BusState = BUS_STATES::IDLE;  
    curMsec = millis();
    lastMsec = curMsec;
}

void loop()
{
    while (true)
    {
        capture_data();
    }
}

void PrintNextArrayBytes(uint8_t* data, uint16_t startidx, uint16_t numbytes)
{
    Serial.printf("%d bytes starting at %d: ", numbytes, startidx);
    for (uint16_t i = 0; i < numbytes - 1; i++)
    {
        if (i % 40 != 0)
        {
            Serial.printf("0x%x,", data[i + startidx]);
        }
        else Serial.printf("\n");
    }
    Serial.printf("0x%x\n", data[numbytes + startidx - 1]);
}

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

189

190

191

192

193

194

195

196

197

198

199

200

Name: Teensy_I2C_Sniffer_V9.ino

Created: 1/10/2020 4:15:25 PM

Author: FRANKNEWXPS15\Frank

#include <TimerOne.h> //needed for ISR

const uint16_t CAPTURE_ARRAY_SIZE = 2048;

const uint16_t VALID_DATA_ARRAY_SIZE = 2048;

uint8_t raw_data[CAPTURE_ARRAY_SIZE]; //holds data captured from I2C bus

uint8_t valid_data[VALID_DATA_ARRAY_SIZE]; //invalid bytes removed

//const int WAITING_PRINT_INTERVAL_MSEC = 200;//interval timer for 'Waiting for data...' printout

#define MONITOR_OUT1 2 //so can monitor ISR activity with O'scope

#define MONITOR_OUT2 3 //so can monitor ISR activity with O'scope

#define MONITOR_OUT3 4 //so can monitor ISR activity with O'scope

#define SDA_PIN 18

#define SCL_PIN 19

//#define PARSE_LOOP_DEBUG

//#define GET_8BIT_DATABYTE_DEBUG

#pragma region PROCESSING_VARIABLES

uint8_t devAddr;

uint8_t regAddr;

uint8_t databytes[2048]; //holds multiple databytes for later output sentence construction

uint16_t numbytes = 0; //number of data bytes extracted from data stream

int ACKNAKFlag; //can be negative

uint16_t databyte_idx = 0; //index into databyte_array

uint8_t killbuff[2]; //used to consume start/stop bytes

elapsedMillis mSecSinceLastWaitingPrint;

uint16_t numvalidbytes = 0; //number of valid bytes in this burst

uint16_t read_idx = 0; //pointer to next byte pair to be processed

unsigned long curMsec;

unsigned long lastMsec;

//added for bus direction labels

enum BUSDIR

{

WRITE,

READ,

UNKNOWN = -1

} RWDir;

BUSDIR BusDir = BUSDIR::UNKNOWN;

//added for bus direction labels

enum BUS_STATES

{

IDLE, //repeated 0xC/0xC

ACTIVE,//0xC/0x4 transition

COUNTING, //0x4/0xC transition, but 0XC count < MAC_0XC_COUNT

PROCESSING //0XC count >= MAC_0XC_COUNT

};

enum BUS_STATES last_State = IDLE;

enum BUS_STATES BusState = IDLE;

#pragma region ISR_SUPPORT

volatile uint16_t write_idx = 0;

volatile uint16_t BurstLength = 0;

uint16_t read_index = 0;

uint8_t current_portb = 0xFF;

uint8_t last_portb = 0xFF;

bool bBufferFull = false;

uint16_t idlecount = 0; //used to detect EOM (end-of-message)

//const uint16_t MAX_IDLE_COUNT = 10000; //how many idle cycles before EOM detection

//const uint16_t MAX_IDLE_COUNT = 5000; //how many idle cycles before EOM detection

const uint16_t MAX_IDLE_COUNT = 2500; //how many idle cycles before EOM detection

//const uint16_t MAX_IDLE_COUNT = 1000; //how many idle cycles before EOM detection

//const uint16_t MAX_IDLE_COUNT = 20000; //how many idle cycles before EOM detection

//const uint16_t MAX_IDLE_COUNT = 30000; //how many idle cycles before EOM detection

volatile bool bStartCapture = false;

volatile uint16_t mult0xCCount = 0;

#pragma endregion ISR Support

volatile bool bDone = false;

unsigned long timevals[100];

uint8_t timevalidx = 0;

//-------------------------------------------------------------------------------

//-------------------------------- ISR ------------------------------------

//-------------------------------------------------------------------------------

//01/06/2020 modified back to capture data from SDA/SCL lines

//01/09/20 modified to just capture between START & STOP

void capture_data(void)

{

last_portb = current_portb;

current_portb = GPIOB_PDIR & 12; //reads state of SDA (18) & SCL (19) at same time

if (last_portb == 0xC && current_portb == 0x4) //START

{

digitalWriteFast(MONITOR_OUT2, HIGH);

digitalWriteFast(MONITOR_OUT3, HIGH);

bDone = false;

mult0xCCount = 0;

}

else if (last_portb == 0x4 && current_portb == 0xc) //STOP

{

digitalWriteFast(MONITOR_OUT2, LOW);

mult0xCCount = 0;

}

else if (mult0xCCount < MAX_IDLE_COUNT && last_portb == 0xc && current_portb == 0xc)

{

mult0xCCount++;

if (mult0xCCount >= MAX_IDLE_COUNT)

{

digitalWriteFast(MONITOR_OUT3, LOW);

//mult0xCCount = 0;

//Serial.printf("%lu: wrote %d bytes\n", millis(),write_idx);

Serial.printf("%lu\t%d\n", millis(),write_idx);

bDone = true;

}

if (!bDone)

{

digitalWriteFast(MONITOR_OUT1, HIGH);

raw_data[write_idx++] = last_portb;

raw_data[write_idx++] = current_portb;

if (write_idx >= CAPTURE_ARRAY_SIZE)

{

//BurstLength = write_idx;

write_idx = 0;

}

digitalWriteFast(MONITOR_OUT1, LOW);

}

//-------------------------------------------------------------------------------

//-------------------------------- END ISR ---------------------------------

//-------------------------------------------------------------------------------

void setup()

{

unsigned long now = millis();

Serial.begin(1); //rate value ignored

int idx = 0;

while (!Serial && (millis() - now) < 3000)

{

delay(500);

idx++;

}

Serial.printf("Serial available after %lu mSec\n", millis() - now);

pinMode(MONITOR_OUT1, OUTPUT);

pinMode(MONITOR_OUT2, OUTPUT);

pinMode(MONITOR_OUT3, OUTPUT);

pinMode(LED_BUILTIN, OUTPUT);

digitalWrite(MONITOR_OUT1, LOW);

digitalWrite(MONITOR_OUT2, LOW);

digitalWrite(MONITOR_OUT3, LOW);

pinMode(SDA_PIN, INPUT);

pinMode(SCL_PIN, INPUT);

//reset port byte vars & start timer

last_portb = current_portb = 0;

write_idx = 0;

memset(raw_data, 0, CAPTURE_ARRAY_SIZE);

PrintNextArrayBytes(raw_data, 0, 20);

//Timer1.initialize(1); // run every mico second

//Timer1.attachInterrupt(capture_data);

Serial.printf("mSec\tlast\tnew\tmult\tburst\n");

mSecSinceLastWaitingPrint = 0;

last_State = BusState = BUS_STATES::IDLE;

curMsec = millis();

lastMsec = curMsec;

}

void loop()

{

while (true)

{

capture_data();

}

void PrintNextArrayBytes(uint8_t* data, uint16_t startidx, uint16_t numbytes)

{

Serial.printf("%d bytes starting at %d: ", numbytes, startidx);

for (uint16_t i = 0; i < numbytes - 1; i++)

{

if (i % 40 != 0)

{

Serial.printf("0x%x,", data[i + startidx]);

}

else Serial.printf("\n");

}

Serial.printf("0x%x\n", data[numbytes + startidx - 1]);

}

The ‘capture_data()’ function (no longer used as an ISR) captures SCL & SDA states with a single port operation as shown

    current_portb = GPIOB_PDIR & 12; //reads state of SDA (18) & SCL (19) at same time

1	current_portb = GPIOB_PDIR & 12; //reads state of SDA (18) & SCL (19) at same time

and then everything from a START pair (0xC followed by 0x4) to a STOP pair (0X4 followed by 0xC) inclusive is captured in the raw_data array.

Any I2C Sniffer project like this one assumes that I2C activity occurs in short bursts with fairly long pauses in between. This is certainly the case with my robot project, as yaw data is only acquired every 200 mSec. However, there is still the problem of determining when a I2C ‘burst’ has finished so the sniffer program can decode and print the results from the last burst. In my investigation, it became clear that at the end of the burst both the SDA line goes HIGH and stays that way until the next START condition (a 0XC followed by a 0X4). So then the question becomes “how many 0XC/0XC pairs do I have to wait before determining that the last burst is over?”

In order to answer this question I decided to use my trusty Tektronix 2236 O’Scope and Teeny’s ‘digitalReadFast’ and ‘digitalWriteFast’ functions to implement a hardware-based timing capability using Teensy pins 0,1, and 2 (MONITOR_OUT1, 2 & 3 respectively). Among other things, this allowed me to definitively determine that a ‘idle’ (0XC/0XC) count of 1000 was too small, but an idle count of 2500 was plenty, without consuming too much of the available processing time. It also turned out that ‘idle’ counts all the way up to 30,000 work too, but leave less time for processing.

O’Scope shot showing I2C traffic on the bottom trace, and the point at which 2500 0xC/0xC (Idle) pairs is reached on the top trace (the high-to-low transition)

As can be seen in the above photo, the I2C ‘sentence’ lasts about 15 mSec, and the ‘idle’ condition is detected about 5 mSec later for a total of about 20 mSec out of the nominal 200 mSec cycle time for my robot application. This leaves about 190 mSec for I2C sentence processing and display.

18 January 2020 Update:

Success!! I now have a working Teensy 3.2 I2C Sniffer program that can continuously monitor the I2C traffic between my Arduino Mega test program acting as a I2C master and a MPU6050 IMU I2C slave. The Teensy code is available on my GitHub account here.

A major challenge in creating the sniffer program was the requirement to sample the I2C SCL & SDA lines quickly enough to accurately detect the line transitions denoting all the different I2C signals. With the I2C bus running at 100Kbs, SCL (clock) transitions occur every 5 uSec. Good sampling requires at least 2 and preferably more samples per SCL state. As noted above, I started off by copying the ISR routine from Kito’s I2C sniffer, but discovered I needed to add some logic to zero in on the desired I2C bus states (IDLE, START, DATA & STOP), and the additional code made the ISR take more than the desired 1 uSec window. After posting about this problem to Paul Stoffregen’s Teensy forum, I got some good pointers for speedup, incuding a post that mentioned the Teensy FASTRUN macro that runs functions from RAM rather than FLASH. As it turned out, adding this macro to the program allowed me to reduce the ISR cycle time from about 1.4 uSec to about .89 uSec – yay! The final ISR routine is shown below:

//-------------------------------------------------------------------------------
//--------------------------------    ISR    ------------------------------------
//-------------------------------------------------------------------------------
FASTRUN void capture_data()
//void capture_data()
{
    last_portb = current_portb;
    current_portb = GPIOB_PDIR & 12; //reads state of SDA (18) & SCL (19) at same time

    if (!bDone && last_portb != current_portb)
    {
        mult0xCCount = 0; //reset IDLE counter
        digitalWriteFast(MONITOR_OUT1, HIGH);

        //01/17/20: joepasquariello suggestion
        last_current = (last_portb << 4) | (current_portb);
        bIsStart = (last_current == 0xC4);
        bIsStop = (last_current == 0x4C);
        bIsData = (last_current == 0x04) || (last_current == 0x8C);

        if (bIsStart) //START  
        {
            digitalWriteFast(MONITOR_OUT2, HIGH);
            if (bWaitingForStart)
            {
                digitalWriteFast(MONITOR_OUT3, HIGH); //start of entire capture
                bWaitingForStart = false;
            }
        }
        else if (bIsStop) //STOP
        {
            digitalWriteFast(MONITOR_OUT2, LOW);
        }

        if (!bWaitingForStart && (bIsData || bIsStart || bIsStop))
        {
            //digitalWriteFast(MONITOR_OUT3, HIGH);
            raw_data[write_idx] = last_portb;
            write_idx++;
            raw_data[write_idx] = current_portb;
            write_idx++;
            if (write_idx >= CAPTURE_ARRAY_SIZE)
            {
                bDone = true;
                digitalWriteFast(MONITOR_OUT3, LOW);
            }
        }
        digitalWriteFast(MONITOR_OUT1, LOW);
    }
    else if (!bDone && mult0xCCount < MAX_IDLE_COUNT && last_portb == 0xc && current_portb == 0xc)
    {
        mult0xCCount++;
        if (mult0xCCount >= MAX_IDLE_COUNT)
        {
            digitalWriteFast(MONITOR_OUT3, LOW);
            bDone = true;
        }
    }
}
//-------------------------------------------------------------------------------
//-------------------------------- END ISR    ---------------------------------
//-------------------------------------------------------------------------------

//-------------------------------------------------------------------------------

//-------------------------------- ISR ------------------------------------

//-------------------------------------------------------------------------------

FASTRUN void capture_data()

//void capture_data()

{

last_portb = current_portb;

current_portb = GPIOB_PDIR & 12; //reads state of SDA (18) & SCL (19) at same time

if (!bDone && last_portb != current_portb)

{

mult0xCCount = 0; //reset IDLE counter

digitalWriteFast(MONITOR_OUT1, HIGH);

//01/17/20: joepasquariello suggestion

last_current = (last_portb << 4) | (current_portb);

bIsStart = (last_current == 0xC4);

bIsStop = (last_current == 0x4C);

bIsData = (last_current == 0x04) || (last_current == 0x8C);

if (bIsStart) //START

{

digitalWriteFast(MONITOR_OUT2, HIGH);

if (bWaitingForStart)

{

digitalWriteFast(MONITOR_OUT3, HIGH); //start of entire capture

bWaitingForStart = false;

}

else if (bIsStop) //STOP

{

digitalWriteFast(MONITOR_OUT2, LOW);

}

if (!bWaitingForStart && (bIsData || bIsStart || bIsStop))

{

//digitalWriteFast(MONITOR_OUT3, HIGH);

raw_data[write_idx] = last_portb;

write_idx++;

raw_data[write_idx] = current_portb;

write_idx++;

if (write_idx >= CAPTURE_ARRAY_SIZE)

{

bDone = true;

digitalWriteFast(MONITOR_OUT3, LOW);

}

digitalWriteFast(MONITOR_OUT1, LOW);

}

else if (!bDone && mult0xCCount < MAX_IDLE_COUNT && last_portb == 0xc && current_portb == 0xc)

{

mult0xCCount++;

if (mult0xCCount >= MAX_IDLE_COUNT)

{

digitalWriteFast(MONITOR_OUT3, LOW);

bDone = true;

}

//-------------------------------------------------------------------------------

//-------------------------------- END ISR ---------------------------------

//-------------------------------------------------------------------------------

Note the use of digitalWriteFast() calls to output timing pulses on Teensy hardware pins so I could use my trusty Tek 2236 100 MHz O’scope to verify proper timing.

Once I got the ISR running properly, then I focused on getting the data parsing algorithm integrated into the program. I had previously shown that I could correctly parse simulated I2C traffic, so all the current challenge was to integrate the algorithm in a way that allowed continuous capture-decode-print cycles at at rate that could keep up with the desired 5 measurements/sec rate. So, I instrumented the sniffer program to display the decoded IMU traffic, along with the calculated yaw value and the time required to perform the decode.

Here’s a short section of the printout from the test program, showing the time (in minutes), the yaw (relative heading ) value retrieved from the IMU, and left/right ping distances (unused in this application).

Min	Yaw	LDist	RDist
0.72	4.15	200	200
0.72	4.15	200	200
0.73	4.14	200	200
0.73	4.14	200	200
0.73	4.14	200	200
0.74	4.14	200	200
0.74	4.14	200	200
0.74	4.14	200	200
0.75	4.14	200	200
0.75	4.14	200	200
0.75	4.13	200	200
0.76	4.13	200	200
0.76	4.13	200	200
0.76	4.13	200	200
0.77	4.13	200	200
0.77	4.13	200	200
0.77	4.13	200	200
0.78	4.13	200	200
0.78	4.13	200	200

Min Yaw LDist RDist

0.72 4.15 200 200

0.73 4.14 200 200

0.74 4.14 200 200

0.75 4.14 200 200

0.75 4.13 200 200

0.76 4.13 200 200

0.77 4.13 200 200

0.78 4.13 200 200

And here is the corresponding output from the I2C sniffer program

Opening port
Port open
Serial available after 500 mSec
4922 I2C(69) reading 2 bytes from 72... 2 14 . Done
4922 I2C(69) reading 1 bytes from 6a... c0 . Done
4922 I2C(69) writing 1 bytes to 6a... c4 . Done
4922 I2C(69) reading 2 bytes from 72... 0 0 . Done
4923 I2C(69) reading 2 bytes from 72... 0 0 . Done
4923 I2C(69) reading 2 bytes from 72... 0 0 . Done
4923 I2C(69) reading 2 bytes from 72... 0 0 . Done
4923 I2C(69) reading 2 bytes from 72... 0 1c . Done
4923 I2C(69) reading 28 bytes from 74... 3f ce 5d a1 fb b4 7e 27 0 db 6e 91 fd a2 4a 16 fe e4 f7 76 3f f0 ff ff ff fe ff fd yawval = 4.13
4922: processed = 1326 elements in 2 mSec

5103: Waiting for Data...
5122 I2C(69) reading 2 bytes from 72... 2 14 . Done
5122 I2C(69) reading 1 bytes from 6a... c0 . Done
5122 I2C(69) writing 1 bytes to 6a... c4 . Done
5122 I2C(69) reading 2 bytes from 72... 0 0 . Done
5122 I2C(69) reading 2 bytes from 72... 0 0 . Done
5123 I2C(69) reading 2 bytes from 72... 0 0 . Done
5123 I2C(69) reading 2 bytes from 72... 0 0 . Done
5123 I2C(69) reading 2 bytes from 72... 0 1c . Done
5123 I2C(69) reading 28 bytes from 74... 3f ce 5b d2 fb b4 5c 1b 0 da e8 1b fd a2 26 70 fe 86 f7 20 3f 3e 0 0 0 0 0 0 yawval = 4.13
5122: processed = 1326 elements in 2 mSec

5303: Waiting for Data...
5322 I2C(69) reading 2 bytes from 72... 2 14 . Done
5322 I2C(69) reading 1 bytes from 6a... c0 . Done
5322 I2C(69) writing 1 bytes to 6a... c4 . Done
5322 I2C(69) reading 2 bytes from 72... 0 0 . Done
5322 I2C(69) reading 2 bytes from 72... 0 0 . Done
5322 I2C(69) reading 2 bytes from 72... 0 0 . Done
5323 I2C(69) reading 2 bytes from 72... 0 0 . Done
5323 I2C(69) reading 2 bytes from 72... 0 1c . Done
5323 I2C(69) reading 28 bytes from 74... 3f ce 54 aa fb b4 42 d4 0 db 4d a7 fd a1 b8 30 fe 7e f7 72 3f 74 0 0 0 0 ff fe yawval = 4.14
5322: processed = 1326 elements in 2 mSec

5504: Waiting for Data...
5520 I2C(69) reading 2 bytes from 72... 2 14 . Done
5520 I2C(69) reading 1 bytes from 6a... c0 . Done
5520 I2C(69) writing 1 bytes to 6a... c4 . Done
5520 I2C(69) reading 2 bytes from 72... 0 0 . Done
5521 I2C(69) reading 2 bytes from 72... 0 0 . Done
5521 I2C(69) reading 2 bytes from 72... 0 0 . Done
5521 I2C(69) reading 2 bytes from 72... 0 1c . Done
5521 I2C(69) reading 28 bytes from 74... 3f ce 4e 97 fb b3 f9 2c 0 db 4f 8f fd a1 9a d9 fe 90 f7 22 3f 3c ff ff ff ff ff fd yawval = 4.14
5520: processed = 1224 elements in 2 mSec

5704: Waiting for Data...
5720 I2C(69) reading 2 bytes from 72... 2 14 . Done
5720 I2C(69) reading 1 bytes from 6a... c0 . Done
5720 I2C(69) writing 1 bytes to 6a... c4 . Done
5720 I2C(69) reading 2 bytes from 72... 0 0 . Done
5720 I2C(69) reading 2 bytes from 72... 0 0 . Done
5720 I2C(69) reading 2 bytes from 72... 0 0 . Done
5721 I2C(69) reading 2 bytes from 72... 0 1c . Done
5721 I2C(69) reading 28 bytes from 74... 3f ce 3b 8f fb b2 cf a3 0 db bd 9a fd a1 de 18 fe 84 f7 3a 3f 6e 0 0 0 1 ff ff yawval = 4.14
5720: processed = 1224 elements in 1 mSec

5903: Waiting for Data...
5920 I2C(69) reading 2 bytes from 72... 2 14 . Done
5920 I2C(69) reading 1 bytes from 6a... c0 . Done
5920 I2C(69) writing 1 bytes to 6a... c4 . Done
5920 I2C(69) reading 2 bytes from 72... 0 0 . Done
5920 I2C(69) reading 2 bytes from 72... 0 0 . Done
5920 I2C(69) reading 2 bytes from 72... 0 0 . Done
5920 I2C(69) reading 2 bytes from 72... 0 1c . Done
5921 I2C(69) reading 28 bytes from 74... 3f ce 3f 14 fb b3 1f ee 0 dc 35 23 fd a1 d6 7e fe a6 f7 7c 3f 2c ff fe ff fd 0 0 yawval = 4.13
5919: processed = 1224 elements in 2 mSec

6104: Waiting for Data...
6120 I2C(69) reading 2 bytes from 72... 2 14 . Done
6121 I2C(69) reading 1 bytes from 6a... c0 . Done
6121 I2C(69) writing 1 bytes to 6a... c4 . Done
6121 I2C(69) reading 2 bytes from 72... 0 0 . Done
6121 I2C(69) reading 2 bytes from 72... 0 0 . Done
6121 I2C(69) reading 2 bytes from 72... 0 0 . Done
6121 I2C(69) reading 2 bytes from 72... 0 1c . Done
6121 I2C(69) reading 28 bytes from 74... 3f ce 36 df fb b2 93 7 0 db 6c 5b fd a1 b0 7a fe 72 f7 54 3f a2 0 0 0 0 0 0 yawval = 4.14
6120: processed = 1224 elements in 2 mSec

6304: Waiting for Data...
6320 I2C(69) reading 2 bytes from 72... 2 14 . Done
6320 I2C(69) reading 1 bytes from 6a... c0 . Done
6320 I2C(69) writing 1 bytes to 6a... c4 . Done
6321 I2C(69) reading 2 bytes from 72... 0 0 . Done
6321 I2C(69) reading 2 bytes from 72... 0 0 . Done
6321 I2C(69) reading 2 bytes from 72... 0 0 . Done
6321 I2C(69) reading 2 bytes from 72... 0 1c . Done
6321 I2C(69) reading 28 bytes from 74... 3f ce 40 75 fb b2 dc d1 0 db 56 f8 fd a2 24 ff fe be f7 64 3f 4c 0 1 0 1 0 1 yawval = 4.13
6320: processed = 1224 elements in 2 mSec

Opening port

Port open

Serial available after 500 mSec

4922 I2C(69) reading 2 bytes from 72... 2 14 . Done

4922 I2C(69) reading 1 bytes from 6a... c0 . Done

4922 I2C(69) writing 1 bytes to 6a... c4 . Done

4922 I2C(69) reading 2 bytes from 72... 0 0 . Done

4923 I2C(69) reading 2 bytes from 72... 0 0 . Done

4923 I2C(69) reading 2 bytes from 72... 0 1c . Done

4923 I2C(69) reading 28 bytes from 74... 3f ce 5d a1 fb b4 7e 27 0 db 6e 91 fd a2 4a 16 fe e4 f7 76 3f f0 ff ff ff fe ff fd yawval = 4.13

4922: processed = 1326 elements in 2 mSec

5103: Waiting for Data...

5122 I2C(69) reading 2 bytes from 72... 2 14 . Done

5122 I2C(69) reading 1 bytes from 6a... c0 . Done

5122 I2C(69) writing 1 bytes to 6a... c4 . Done

5122 I2C(69) reading 2 bytes from 72... 0 0 . Done

5123 I2C(69) reading 2 bytes from 72... 0 0 . Done

5123 I2C(69) reading 2 bytes from 72... 0 1c . Done

5123 I2C(69) reading 28 bytes from 74... 3f ce 5b d2 fb b4 5c 1b 0 da e8 1b fd a2 26 70 fe 86 f7 20 3f 3e 0 0 0 0 0 0 yawval = 4.13

5122: processed = 1326 elements in 2 mSec

5303: Waiting for Data...

5322 I2C(69) reading 2 bytes from 72... 2 14 . Done

5322 I2C(69) reading 1 bytes from 6a... c0 . Done

5322 I2C(69) writing 1 bytes to 6a... c4 . Done

5322 I2C(69) reading 2 bytes from 72... 0 0 . Done

5323 I2C(69) reading 2 bytes from 72... 0 0 . Done

5323 I2C(69) reading 2 bytes from 72... 0 1c . Done

5323 I2C(69) reading 28 bytes from 74... 3f ce 54 aa fb b4 42 d4 0 db 4d a7 fd a1 b8 30 fe 7e f7 72 3f 74 0 0 0 0 ff fe yawval = 4.14

5322: processed = 1326 elements in 2 mSec

5504: Waiting for Data...

5520 I2C(69) reading 2 bytes from 72... 2 14 . Done

5520 I2C(69) reading 1 bytes from 6a... c0 . Done

5520 I2C(69) writing 1 bytes to 6a... c4 . Done

5520 I2C(69) reading 2 bytes from 72... 0 0 . Done

5521 I2C(69) reading 2 bytes from 72... 0 0 . Done

5521 I2C(69) reading 2 bytes from 72... 0 1c . Done

5521 I2C(69) reading 28 bytes from 74... 3f ce 4e 97 fb b3 f9 2c 0 db 4f 8f fd a1 9a d9 fe 90 f7 22 3f 3c ff ff ff ff ff fd yawval = 4.14

5520: processed = 1224 elements in 2 mSec

5704: Waiting for Data...

5720 I2C(69) reading 2 bytes from 72... 2 14 . Done

5720 I2C(69) reading 1 bytes from 6a... c0 . Done

5720 I2C(69) writing 1 bytes to 6a... c4 . Done

5720 I2C(69) reading 2 bytes from 72... 0 0 . Done

5721 I2C(69) reading 2 bytes from 72... 0 1c . Done

5721 I2C(69) reading 28 bytes from 74... 3f ce 3b 8f fb b2 cf a3 0 db bd 9a fd a1 de 18 fe 84 f7 3a 3f 6e 0 0 0 1 ff ff yawval = 4.14

5720: processed = 1224 elements in 1 mSec

5903: Waiting for Data...

5920 I2C(69) reading 2 bytes from 72... 2 14 . Done

5920 I2C(69) reading 1 bytes from 6a... c0 . Done

5920 I2C(69) writing 1 bytes to 6a... c4 . Done

5920 I2C(69) reading 2 bytes from 72... 0 0 . Done

5920 I2C(69) reading 2 bytes from 72... 0 1c . Done

5921 I2C(69) reading 28 bytes from 74... 3f ce 3f 14 fb b3 1f ee 0 dc 35 23 fd a1 d6 7e fe a6 f7 7c 3f 2c ff fe ff fd 0 0 yawval = 4.13

5919: processed = 1224 elements in 2 mSec

6104: Waiting for Data...

6120 I2C(69) reading 2 bytes from 72... 2 14 . Done

6121 I2C(69) reading 1 bytes from 6a... c0 . Done

6121 I2C(69) writing 1 bytes to 6a... c4 . Done

6121 I2C(69) reading 2 bytes from 72... 0 0 . Done

6121 I2C(69) reading 2 bytes from 72... 0 1c . Done

6121 I2C(69) reading 28 bytes from 74... 3f ce 36 df fb b2 93 7 0 db 6c 5b fd a1 b0 7a fe 72 f7 54 3f a2 0 0 0 0 0 0 yawval = 4.14

6120: processed = 1224 elements in 2 mSec

6304: Waiting for Data...

6320 I2C(69) reading 2 bytes from 72... 2 14 . Done

6320 I2C(69) reading 1 bytes from 6a... c0 . Done

6320 I2C(69) writing 1 bytes to 6a... c4 . Done

6321 I2C(69) reading 2 bytes from 72... 0 0 . Done

6321 I2C(69) reading 2 bytes from 72... 0 1c . Done

6321 I2C(69) reading 28 bytes from 74... 3f ce 40 75 fb b2 dc d1 0 db 56 f8 fd a2 24 ff fe be f7 64 3f 4c 0 1 0 1 0 1 yawval = 4.13

6320: processed = 1224 elements in 2 mSec

In the above printout, each printout shows the individual transmit & receive ‘sentences’ to/from the IMU, and the 28-byte packet received from the IMU containing, among other things, the values required to calculate a yaw (relative heading value). As can be seen, the yaw value calculated from the received bytes, closely matches the yaw values retrieved using the test program. In addition the last line of each section of the readout shows the time tag for the start of the decode process, and the total time taken to decode all the bytes in that particular burst. From the data, it is clear that only 1-2 mSec is required to decode and display a full burst.

The complete I2C Sniffer program is available on my GitHub site here. The complete test program that obtains a yaw value from the IMU every 200 mSec is shown below:

/*
    Name:       MPU6050_MotorNoiseTest1.ino
    Created:	11/10/2019 9:17:51 AM
    Author:     FRANKWIN10\Frank
*/


#pragma region INCLUDES
//#include <Adafruit_INA219.h>
#include <elapsedMillis.h>
#include <PrintEx.h> //allows printf-style printout syntax
#include "MPU6050_6Axis_MotionApps_V6_12.h"  //changed to this version 10/05/19
#include "I2Cdev.h" //02/19/19: this includes SBWire.h
#include <NewPing.h> //added 01/15/15
#pragma endregion INCLUDES

StreamEx mySerial = Serial; //added 03/18/18 for printf-style printing
elapsedMillis 	sinceLastNavUpdateMsec; //added 10/15/18 to replace lastmillisec

MPU6050 mpu(0x69); //06/23/18 chg to AD0 high addr on 2-motor robot


#pragma region MOTOR_PARAMETERS
//drive wheel speed parameters
const int MOTOR_SPEED_FULL = 200; //range is 0-255
const int MOTOR_SPEED_MAX = 255; //range is 0-255
const int MOTOR_SPEED_HALF = 127; //range is 0-255
const int MOTOR_SPEED_LOW = 50; //added 01/22/15
const int MOTOR_SPEED_OFF = 0; //range is 0-255
const int MOTOR_SPEED_ADJ_FACTOR = 0; //chg to 40 at 5:55pm
const int LEFT_SPEED_COMP_VAL_FWD = 0; //left speed compensation value
const int RIGHT_SPEED_COMP_VAL_FWD = 5; //right speed compensation value
const int LEFT_SPEED_COMP_VAL_REV = 0; //left speed compensation value
const int RIGHT_SPEED_COMP_VAL_REV = 0; //right speed compensation value

//drive wheel direction constants
const boolean FWD_DIR = true;
const boolean REV_DIR = !FWD_DIR;

//Motor direction variables
boolean bLeftMotorDirIsFwd = true;
boolean bRightMotorDirIsFwd = true;
int leftspeednum = MOTOR_SPEED_HALF;
int rightspeednum = MOTOR_SPEED_HALF;
#pragma endregion Motor Parameters


#define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6)


#pragma region DIST_HDG_MEASUREMENT_SUPPORT
//misc LIDAR and Ping sensor parameters
const int MIN_OBS_DIST_CM = 20; //rev 04/28/17 for better obstacle handling
const int MAX_LR_DISTANCE_CM = 200;  //04/19/15 now using sep parameters for front and side sensors

//distance and heading running average
const int LR_PING_DIST_ARRAY_SIZE = 3; //04/28/19 added to reinstate l/r dist running avg
const int LR_PING_AVG_WINDOW_SIZE = 3; //added 04/28/19 so front & lr averages can differ
const int HDG_RUNNING_AVG_ARRAY_SIZE = 20; //1 second

byte aLeftDist[LR_PING_DIST_ARRAY_SIZE];
byte aRightDist[LR_PING_DIST_ARRAY_SIZE];
#pragma endregion Distance Measurement Support


#pragma region SENSOR_PINS
const int RightSensorTrigPin = 2;
const int LeftSensorTrigPin = 3;
const int LeftSensorEchoPin = LeftSensorTrigPin;
const int RightSensorEchoPin = RightSensorTrigPin;


//added 01/16/15 for NewPing support
NewPing LeftPing(LeftSensorTrigPin, LeftSensorEchoPin, MAX_LR_DISTANCE_CM / 2);
NewPing RightPing(RightSensorTrigPin, RightSensorEchoPin, MAX_LR_DISTANCE_CM / 2);

//const int TOT_CURR_PIN = A8;
#pragma endregion Sensor Pins

#pragma region MOTOR_PINS
//testing with a power-blocked UNO
const int EnA_Pin = 11;
const int In1_Pin = 10;
const int In2_Pin = 9;

//Right Motor
const int In3_Pin = 8;
const int In4_Pin = 7;
const int EnB_Pin = 6;

const int MOTOR_SPEED_STEP_VAL = 20;
const int MAX_MOTOR_SPEED = 255;
const int MIN_MOTOR_SPEED = 0;
bool bMotorIsFwd = true;
bool bMotorSpeedingUp = true;
int MotorSpeed = 0;

#pragma endregion Motor Pin Assignments


#pragma region MPU6050_SUPPORT
uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU. Used in Homer's Overflow routine
uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount;     // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer

						// orientation/motion vars
Quaternion q;           // [w, x, y, z]         quaternion container
VectorInt16 aa;         // [x, y, z]            accel sensor measurements
VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
VectorFloat gravity;    // [x, y, z]            gravity vector
float euler[3];         // [psi, theta, phi]    Euler angle container
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
int GetPacketLoopCount = 0;
int OuterGetPacketLoopCount = 0;

//RTC/FRAM/MPU6050 status flags
bool bMPU6050Ready = true;
bool dmpReady = false;  // set true if DMP init was successful
volatile float global_yawval = 0; //updated by GetIMUHeadingDeg()
const uint16_t MAX_GETPACKET_LOOPS = 100; //10/30/19 added for backup loop exit condition in GetCurrentFIFOPacket()
uint8_t GetCurrentFIFOPacket(uint8_t* data, uint8_t length, uint16_t max_loops = MAX_GETPACKET_LOOPS); //prototype here so can define a default param
bool bFirstTime = true;
#pragma endregion MPU6050 Support

const int NAV_UPDATE_INTERVAL_MSEC = 200;

void setup()
{
	Serial.begin(115200);

	Serial.println("Two Wheel Robot");

	// join I2C bus (I2Cdev library doesn't do this automatically)
	Wire.begin();
	Wire.setTwiMaxLoops(15000); //08/23 needed for SBWire library
	Wire.setClock(100000); //07/25/19 slow down I2C clock for better noise immunity

	//motor control PWM pins
	Serial.println("...Setting up Motor Control Pins...");
	pinMode(EnA_Pin, OUTPUT);//set up left motor pins as outputs
	pinMode(In1_Pin, OUTPUT);
	pinMode(In2_Pin, OUTPUT);


	pinMode(In3_Pin, OUTPUT);//set up right motor pins as outputs
	pinMode(In4_Pin, OUTPUT);
	pinMode(EnB_Pin, OUTPUT);
	pinMode(LED_PIN, OUTPUT);


	// initialize MPU6050 added 09/03/18
	Serial.println(F("Initializing MPU6050 ..."));
	mpu.initialize();

	// verify connection
	Serial.println(F("Testing device connections..."));
	Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

	Serial.println(F("Initializing DMP..."));
	devStatus = mpu.dmpInitialize();

	// supply your own gyro offsets here, scaled for min sensitivity
	//mpu.setXGyroOffset(51);
	//mpu.setYGyroOffset(8);
	//mpu.setZGyroOffset(21);
	//mpu.setXAccelOffset(1150);
	//mpu.setYAccelOffset(-50);
	//mpu.setZAccelOffset(1060);

	//12/03/19 settings from calibration routines
	mpu.setXGyroOffset(58);
	mpu.setYGyroOffset(20);
	mpu.setZGyroOffset(12);
	mpu.setXAccelOffset(1618);
	mpu.setYAccelOffset(3046);
	mpu.setZAccelOffset(4554);

	// make sure it worked (returns 0 if successful)
	if (devStatus == 0)
	{
		//12/06/19 no longer needed; cal vals already acquired
		//// Calibration Time: generate offsets and calibrate our MPU6050
		//mpu.CalibrateAccel(6); 
		//mpu.CalibrateGyro(6);
		//Serial.println();
		//mpu.PrintActiveOffsets();

		// turn on the DMP, now that it's ready
		Serial.println(F("Enabling DMP..."));
		mpu.setDMPEnabled(true);

		// set our DMP Ready flag so the main loop() function knows it's okay to use it
		Serial.println(F("DMP ready! Waiting for MPU6050 drift rate to settle..."));
		dmpReady = true;

		// get expected DMP packet size for later comparison
		packetSize = mpu.dmpGetFIFOPacketSize();
		bMPU6050Ready = true;
		mySerial.printf("\nMPU6050 Ready at %2.2f Sec\n", millis() / 1000.f);
	}
	else
	{
		// ERROR!
		// 1 = initial memory load failed
		// 2 = DMP configuration updates failed
		// (if it's going to break, usually the code will be 1)
		Serial.print(F("DMP Initialization failed (code "));
		Serial.print(devStatus);
		Serial.println(F(")"));

		bMPU6050Ready = false;
	}

	//mySerial.printf("Starting Motors...");
	//MoveAhead(MOTOR_SPEED_FULL, MOTOR_SPEED_FULL);

	Serial.println("Set motor speed full & FWD direction ...");
	analogWrite(EnA_Pin, 255);
	digitalWrite(In1_Pin, LOW);
	digitalWrite(In2_Pin, HIGH);
	analogWrite(EnB_Pin, 255);
	digitalWrite(In3_Pin, LOW);
	digitalWrite(In4_Pin, HIGH);

	sinceLastNavUpdateMsec = 0;
}

void loop()
{
	if (bFirstTime)
	{
		bFirstTime = false;
		mySerial.printf("Min\tYaw\tLDist\tRDist\n");
	}

	if (sinceLastNavUpdateMsec >=  NAV_UPDATE_INTERVAL_MSEC)
	{
		sinceLastNavUpdateMsec -= NAV_UPDATE_INTERVAL_MSEC;

		bool getYawResult = GetIMUHeadingDeg(); //updates global_yawval if successful

		if (getYawResult == 0)
		{
			if (mpu.testConnection())
			{
				mySerial.printf("resetting IMU\n");
				mpu.reset();
				delay(50);
			}
		}

		int leftdist = GetLeftDistCm();
		int rightdist = GetRightDistCm();

		//mySerial.printf("%3.2f\t%3.2f\t%d\t%d\t%d\t%d\n", millis()/60000.0, global_yawval, getYawResult, fifoCount, GetPacketLoopCount, OuterGetPacketLoopCount);
		mySerial.printf("%3.2f\t%3.2f\t%d\t%d\n", millis()/60000.0, global_yawval, leftdist, rightdist);

		//12/06/19 commented out for now - just want constant motor speed
		////change the motor speed (and maybe the direction)
		//if (bMotorSpeedingUp)
		//{
		//	MotorSpeed += MOTOR_SPEED_STEP_VAL;
		//}
		//else
		//{
		//	MotorSpeed -= MOTOR_SPEED_STEP_VAL;
		//}

		//if (MotorSpeed >= MAX_MOTOR_SPEED)
		//{
		//	bMotorSpeedingUp = false;
		//}
		//if (MotorSpeed <= MIN_MOTOR_SPEED)
		//{
		//	//Serial.println("Reversing Motor Direction...");
		//	bMotorSpeedingUp = true;
		//	bMotorIsFwd = !bMotorIsFwd;
		//}

		////now actually set the motor speed and direction
		//analogWrite(EnA_Pin, MotorSpeed);
		//digitalWrite(In1_Pin, bMotorIsFwd ? LOW : HIGH);
		//digitalWrite(In2_Pin, bMotorIsFwd ? HIGH : LOW);

		////now actually set the motor speed and direction
		//analogWrite(EnB_Pin, MotorSpeed);
		//digitalWrite(In3_Pin, bMotorIsFwd ? LOW : HIGH);
		//digitalWrite(In4_Pin, bMotorIsFwd ? HIGH : LOW);

	}

}

#pragma region MOTOR_SUPPORT_ROUTINES
void RunBothMotors(int leftspeednum, int rightspeednum)
{
	//Purpose: Run both Motors at left/rightspeednum speeds
	//Inputs:
	//    speednum = speed value (0 = OFF, 255 = full speed)
	//Outputs: Both Motors run for timesec seconds at speednum speed
	//Plan:
	//Notes:
	//    01/14/15 - added left/right speed offset for straightness compensation
	//    01/22/15 - added code to restrict fast/slow values
	//    01/24/15 - revised for continuous run - no timing
	//  01/26/15 - speednum modifications moved to UpdateWallFollowmyMotorspeeds()
	//	12/07/15 - chg args from &int to int

////DEBUG!!
//	mySerial.printf("In RunBothMotors(%d,%d\n", leftspeednum, rightspeednum);
////DEBUG!!
	SetLeftMotorSpeed(leftspeednum);
	SetRightMotorSpeed(rightspeednum);
}

//01/24/15 removed time from MoveAhead sig, added MoveAheadMsec
void MoveAhead(int leftspeednum, int rightspeednum)
{
	//Purpose:  Move ahead continuously
	//Provenance: G. Frank Paynter and Danny Frank 01/24/2014
	//Inputs:  
	//    leftspeednum = integer denoting speed (0=stop, 255 = full speed)
	//    rightspeednum = integer denoting speed (0=stop, 255 = full speed)
	//Outputs: both drive Motors energized with the specified speed
	//Plan:
	//    Step 1: Set forward direction for both wheels
	//    Step 2: Run both Motors at specified speeds

	//Step 1: Set forward direction for both wheels
	SetLeftMotorDir(true);
	SetRightMotorDir(true);

	//Step 2: Run both myMotors for timsec seconds
	RunBothMotors(leftspeednum, rightspeednum);
}
void SetLeftMotorSpeed(int speed)
{
	if (bLeftMotorDirIsFwd)
	{
		speed = speed + LEFT_SPEED_COMP_VAL_FWD;

		//check limits
		speed = (speed <= MOTOR_SPEED_MAX) ? speed : MOTOR_SPEED_MAX;
		speed = (speed >= MOTOR_SPEED_OFF) ? speed : MOTOR_SPEED_OFF;

	}
	else //must be REV
	{
		speed = speed + LEFT_SPEED_COMP_VAL_REV;

		//check limits
		speed = (speed <= MOTOR_SPEED_MAX) ? speed : MOTOR_SPEED_MAX;
		speed = (speed >= MOTOR_SPEED_OFF) ? speed : MOTOR_SPEED_OFF;
	}


#ifndef NO_MOTORS
	Serial.print("Left Speed = "); Serial.println(speed);
	analogWrite(EnA_Pin, speed);
#endif

}

//11/25/15 added to utilize new private member variable bRightMotorDirIsFwd
void SetRightMotorSpeed(int speed)
{
	if (bRightMotorDirIsFwd)
	{
		speed = speed + LEFT_SPEED_COMP_VAL_FWD;

		//check limits
		speed = (speed <= MOTOR_SPEED_MAX) ? speed : MOTOR_SPEED_MAX;
		speed = (speed >= MOTOR_SPEED_OFF) ? speed : MOTOR_SPEED_OFF;

	}
	else //must be REV
	{
		speed = speed + LEFT_SPEED_COMP_VAL_REV;

		//check limits
		speed = (speed <= MOTOR_SPEED_MAX) ? speed : MOTOR_SPEED_MAX;
		speed = (speed >= MOTOR_SPEED_OFF) ? speed : MOTOR_SPEED_OFF;

	}

#ifndef NO_MOTORS
	Serial.print("Right Speed = "); Serial.println(speed);
	analogWrite(EnB_Pin, speed);
#endif

}

void SetLeftMotorDir(boolean bIsFwdDir)
{
	if (bIsFwdDir)
	{
		digitalWrite(In1_Pin, LOW);
		digitalWrite(In2_Pin, HIGH);
	}
	else //must be REV
	{
		digitalWrite(In1_Pin, HIGH);
		digitalWrite(In2_Pin, LOW);
	}

	bLeftMotorDirIsFwd = bIsFwdDir;
}

void SetRightMotorDir(boolean bIsFwdDir)
{
	if (bIsFwdDir)
	{
		digitalWrite(In3_Pin, LOW);
		digitalWrite(In4_Pin, HIGH);
	}
	else //must be REV
	{
		digitalWrite(In3_Pin, HIGH);
		digitalWrite(In4_Pin, LOW);
	}

	bRightMotorDirIsFwd = bIsFwdDir;
}
#pragma endregion Motor Support Routines


bool GetIMUHeadingDeg()
{
	//Purpose: Get latest yaw (heading) value from IMU
	//Inputs: None.  This function should only be called after mpu.dmpPacketAvailable() returns TRUE
	//Outputs: 
	//	returns true if successful, otherwise false
	//	global_yawval updated on success
	//Plan:
	//Step1: check for overflow and reset the FIFO if it occurs. In this case, wait for new packet
	//Step2: read all available packets to get to latest data
	//Step3: update global_yawval with latest value
	//Notes:
	//	10/08/19 changed return type to boolean
	//	10/08/19 no longer need mpuIntStatus
	//	10/21/19 completely rewritten to use Homer's algorithm

	bool retval = false;

	int flag = GetCurrentFIFOPacket(fifoBuffer, packetSize, MAX_GETPACKET_LOOPS); //get the latest mpu packet

	if (flag != 0) //0 = error exit, 1 = normal exit, 2 = recovered from an overflow
	{
		// display Euler angles in degrees
		mpu.dmpGetQuaternion(&q, fifoBuffer);
		mpu.dmpGetGravity(&gravity, &q);
		mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

		//compute the yaw value
		global_yawval = ypr[0] * 180 / M_PI;
		retval = true;
	}

	return retval;
}

uint8_t GetCurrentFIFOPacket(uint8_t *data, uint8_t length, uint16_t max_loops)
{
	max_loops = max(10, max_loops); //added 12/04/19 to ensure we loop at least 10 times. This really IS a hard-coded number!
	fifoCount = mpu.getFIFOCount();
	GetPacketLoopCount = 0;

	//11/10/19 I have seen this fail with fifoC > 28 on occasion, so now I loop max three times
	OuterGetPacketLoopCount = 0;
	while (fifoCount != packetSize && OuterGetPacketLoopCount <= 3)
	{
		mpu.resetFIFO();
		delay(1);

		fifoCount = mpu.getFIFOCount();
		GetPacketLoopCount = 0;

		while (fifoCount < packetSize && GetPacketLoopCount < max_loops)
		{
			GetPacketLoopCount++;
			fifoCount = mpu.getFIFOCount();
			delay(2);
		}

		//if we get to here, there should be exactly one packet in the FIFO
		OuterGetPacketLoopCount++;
	}

	if (OuterGetPacketLoopCount < 3)
	{
		mpu.getFIFOBytes(data, packetSize);
		return 1;
	}

	return 0; //failed to get a good packet
}


int GetLeftDistCm()
{
	//Serial.print("LeftPing\t"); Serial.println(millis());
	//Notes:
	//	04/30/17 added limit detection/correction

	int timeUsec = LeftPing.ping();
	//Serial.print("Left Ping Usec = "); Serial.println(timeUsec);
	int distCm = LeftPing.convert_cm(timeUsec);

	//04/30/17 added limit detection/correction
	distCm = (distCm > 0) ? distCm : MAX_LR_DISTANCE_CM;

	return distCm;
}

//01/10/18 reverted to regular ping().  median distance function takes too long
int GetRightDistCm()
{
	//Serial.print("RightPing\t"); Serial.println(millis());
	//Notes:
	//	04/30/17 added limit detection/correction

	int timeUsec = RightPing.ping();
	//Serial.print("Right Ping Usec = "); Serial.println(timeUsec);
	int distCm = RightPing.convert_cm(timeUsec);

	//04/30/17 added limit detection/correction
	distCm = (distCm > 0) ? distCm : MAX_LR_DISTANCE_CM;

	return distCm;
}

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

189

190

191

192

193

194

195

196

197

198

199

200

201

202

203

204

205

206

207

208

209

210

211

212

213

214

215

216

217

218

219

220

221

222

223

224

225

226

227

228

229

230

231

232

233

234

235

236

237

238

239

240

241

242

243

244

245

246

247

248

249

250

251

252

253

254

255

256

257

258

259

260

261

262

263

264

265

266

267

268

269

270

271

272

273

274

275

276

277

278

279

280

281

282

283

284

285

286

287

288

289

290

291

292

293

294

295

296

297

298

299

300

301

302

303

304

305

306

307

308

309

310

311

312

313

314

315

316

317

318

319

320

321

322

323

324

325

326

327

328

329

330

331

332

333

334

335

336

337

338

339

340

341

342

343

344

345

346

347

348

349

350

351

352

353

354

355

356

357

358

359

360

361

362

363

364

365

366

367

368

369

370

371

372

373

374

375

376

377

378

379

380

381

382

383

384

385

386

387

388

389

390

391

392

393

394

395

396

397

398

399

400

401

402

403

404

405

406

407

408

409

410

411

412

413

414

415

416

417

418

419

420

421

422

423

424

425

426

427

428

429

430

431

432

433

434

435

436

437

438

439

440

441

442

443

444

445

446

447

448

449

450

451

452

453

454

455

456

457

458

459

460

461

462

463

464

465

466

467

468

469

470

471

472

473

474

475

476

477

478

479

480

481

482

483

484

485

486

487

488

489

490

491

492

493

494

495

496

497

498

499

500

501

502

503

504

505

506

507

508

509

510

511

512

513

514

515

516

517

518

519

520

521

522

523

524

525

526

527

528

529

530

531

532

533

Name: MPU6050_MotorNoiseTest1.ino

Created: 11/10/2019 9:17:51 AM

Author: FRANKWIN10\Frank

#pragma region INCLUDES

//#include <Adafruit_INA219.h>

#include <elapsedMillis.h>

#include <PrintEx.h> //allows printf-style printout syntax

#include "MPU6050_6Axis_MotionApps_V6_12.h" //changed to this version 10/05/19

#include "I2Cdev.h" //02/19/19: this includes SBWire.h

#include <NewPing.h> //added 01/15/15

#pragma endregion INCLUDES

StreamEx mySerial = Serial; //added 03/18/18 for printf-style printing

elapsedMillis sinceLastNavUpdateMsec; //added 10/15/18 to replace lastmillisec

MPU6050 mpu(0x69); //06/23/18 chg to AD0 high addr on 2-motor robot

#pragma region MOTOR_PARAMETERS

//drive wheel speed parameters

const int MOTOR_SPEED_FULL = 200; //range is 0-255

const int MOTOR_SPEED_MAX = 255; //range is 0-255

const int MOTOR_SPEED_HALF = 127; //range is 0-255

const int MOTOR_SPEED_LOW = 50; //added 01/22/15

const int MOTOR_SPEED_OFF = 0; //range is 0-255

const int MOTOR_SPEED_ADJ_FACTOR = 0; //chg to 40 at 5:55pm

const int LEFT_SPEED_COMP_VAL_FWD = 0; //left speed compensation value

const int RIGHT_SPEED_COMP_VAL_FWD = 5; //right speed compensation value

const int LEFT_SPEED_COMP_VAL_REV = 0; //left speed compensation value

const int RIGHT_SPEED_COMP_VAL_REV = 0; //right speed compensation value

//drive wheel direction constants

const boolean FWD_DIR = true;

const boolean REV_DIR = !FWD_DIR;

//Motor direction variables

boolean bLeftMotorDirIsFwd = true;

boolean bRightMotorDirIsFwd = true;

int leftspeednum = MOTOR_SPEED_HALF;

int rightspeednum = MOTOR_SPEED_HALF;

#pragma endregion Motor Parameters

#define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6)

#pragma region DIST_HDG_MEASUREMENT_SUPPORT

//misc LIDAR and Ping sensor parameters

const int MIN_OBS_DIST_CM = 20; //rev 04/28/17 for better obstacle handling

const int MAX_LR_DISTANCE_CM = 200; //04/19/15 now using sep parameters for front and side sensors

//distance and heading running average

const int LR_PING_DIST_ARRAY_SIZE = 3; //04/28/19 added to reinstate l/r dist running avg

const int LR_PING_AVG_WINDOW_SIZE = 3; //added 04/28/19 so front & lr averages can differ

const int HDG_RUNNING_AVG_ARRAY_SIZE = 20; //1 second

byte aLeftDist[LR_PING_DIST_ARRAY_SIZE];

byte aRightDist[LR_PING_DIST_ARRAY_SIZE];

#pragma endregion Distance Measurement Support

#pragma region SENSOR_PINS

const int RightSensorTrigPin = 2;

const int LeftSensorTrigPin = 3;

const int LeftSensorEchoPin = LeftSensorTrigPin;

const int RightSensorEchoPin = RightSensorTrigPin;

//added 01/16/15 for NewPing support

NewPing LeftPing(LeftSensorTrigPin, LeftSensorEchoPin, MAX_LR_DISTANCE_CM / 2);

NewPing RightPing(RightSensorTrigPin, RightSensorEchoPin, MAX_LR_DISTANCE_CM / 2);

//const int TOT_CURR_PIN = A8;

#pragma endregion Sensor Pins

#pragma region MOTOR_PINS

//testing with a power-blocked UNO

const int EnA_Pin = 11;

const int In1_Pin = 10;

const int In2_Pin = 9;

//Right Motor

const int In3_Pin = 8;

const int In4_Pin = 7;

const int EnB_Pin = 6;

const int MOTOR_SPEED_STEP_VAL = 20;

const int MAX_MOTOR_SPEED = 255;

const int MIN_MOTOR_SPEED = 0;

bool bMotorIsFwd = true;

bool bMotorSpeedingUp = true;

int MotorSpeed = 0;

#pragma endregion Motor Pin Assignments

#pragma region MPU6050_SUPPORT

uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU. Used in Homer's Overflow routine

uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)

uint16_t packetSize; // expected DMP packet size (default is 42 bytes)

uint16_t fifoCount; // count of all bytes currently in FIFO

uint8_t fifoBuffer[64]; // FIFO storage buffer

// orientation/motion vars

Quaternion q; // [w, x, y, z] quaternion container

VectorInt16 aa; // [x, y, z] accel sensor measurements

VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements

VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements

VectorFloat gravity; // [x, y, z] gravity vector

float euler[3]; // [psi, theta, phi] Euler angle container

float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector

int GetPacketLoopCount = 0;

int OuterGetPacketLoopCount = 0;

//RTC/FRAM/MPU6050 status flags

bool bMPU6050Ready = true;

bool dmpReady = false; // set true if DMP init was successful

volatile float global_yawval = 0; //updated by GetIMUHeadingDeg()

const uint16_t MAX_GETPACKET_LOOPS = 100; //10/30/19 added for backup loop exit condition in GetCurrentFIFOPacket()

uint8_t GetCurrentFIFOPacket(uint8_t* data, uint8_t length, uint16_t max_loops = MAX_GETPACKET_LOOPS); //prototype here so can define a default param

bool bFirstTime = true;

#pragma endregion MPU6050 Support

const int NAV_UPDATE_INTERVAL_MSEC = 200;

void setup()

{

Serial.begin(115200);

Serial.println("Two Wheel Robot");

// join I2C bus (I2Cdev library doesn't do this automatically)

Wire.begin();

Wire.setTwiMaxLoops(15000); //08/23 needed for SBWire library

Wire.setClock(100000); //07/25/19 slow down I2C clock for better noise immunity

//motor control PWM pins

Serial.println("...Setting up Motor Control Pins...");

pinMode(EnA_Pin, OUTPUT);//set up left motor pins as outputs

pinMode(In1_Pin, OUTPUT);

pinMode(In2_Pin, OUTPUT);

pinMode(In3_Pin, OUTPUT);//set up right motor pins as outputs

pinMode(In4_Pin, OUTPUT);

pinMode(EnB_Pin, OUTPUT);

pinMode(LED_PIN, OUTPUT);

// initialize MPU6050 added 09/03/18

Serial.println(F("Initializing MPU6050 ..."));

mpu.initialize();

// verify connection

Serial.println(F("Testing device connections..."));

Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

Serial.println(F("Initializing DMP..."));

devStatus = mpu.dmpInitialize();

// supply your own gyro offsets here, scaled for min sensitivity

//mpu.setXGyroOffset(51);

//mpu.setYGyroOffset(8);

//mpu.setZGyroOffset(21);

//mpu.setXAccelOffset(1150);

//mpu.setYAccelOffset(-50);

//mpu.setZAccelOffset(1060);

//12/03/19 settings from calibration routines

mpu.setXGyroOffset(58);

mpu.setYGyroOffset(20);

mpu.setZGyroOffset(12);

mpu.setXAccelOffset(1618);

mpu.setYAccelOffset(3046);

mpu.setZAccelOffset(4554);

// make sure it worked (returns 0 if successful)

if (devStatus == 0)

{

//12/06/19 no longer needed; cal vals already acquired

//// Calibration Time: generate offsets and calibrate our MPU6050

//mpu.CalibrateAccel(6);

//mpu.CalibrateGyro(6);

//Serial.println();

//mpu.PrintActiveOffsets();

// turn on the DMP, now that it's ready

Serial.println(F("Enabling DMP..."));

mpu.setDMPEnabled(true);

// set our DMP Ready flag so the main loop() function knows it's okay to use it

Serial.println(F("DMP ready! Waiting for MPU6050 drift rate to settle..."));

dmpReady = true;

// get expected DMP packet size for later comparison

packetSize = mpu.dmpGetFIFOPacketSize();

bMPU6050Ready = true;

mySerial.printf("\nMPU6050 Ready at %2.2f Sec\n", millis() / 1000.f);

}

else

{

// ERROR!

// 1 = initial memory load failed

// 2 = DMP configuration updates failed

// (if it's going to break, usually the code will be 1)

Serial.print(F("DMP Initialization failed (code "));

Serial.print(devStatus);

Serial.println(F(")"));

bMPU6050Ready = false;

}

//mySerial.printf("Starting Motors...");

//MoveAhead(MOTOR_SPEED_FULL, MOTOR_SPEED_FULL);

Serial.println("Set motor speed full & FWD direction ...");

analogWrite(EnA_Pin, 255);

digitalWrite(In1_Pin, LOW);

digitalWrite(In2_Pin, HIGH);

analogWrite(EnB_Pin, 255);

digitalWrite(In3_Pin, LOW);

digitalWrite(In4_Pin, HIGH);

sinceLastNavUpdateMsec = 0;

}

void loop()

{

if (bFirstTime)

{

bFirstTime = false;

mySerial.printf("Min\tYaw\tLDist\tRDist\n");

}

if (sinceLastNavUpdateMsec >= NAV_UPDATE_INTERVAL_MSEC)

{

sinceLastNavUpdateMsec -= NAV_UPDATE_INTERVAL_MSEC;

bool getYawResult = GetIMUHeadingDeg(); //updates global_yawval if successful

if (getYawResult == 0)

{

if (mpu.testConnection())

{

mySerial.printf("resetting IMU\n");

mpu.reset();

delay(50);

}

int leftdist = GetLeftDistCm();

int rightdist = GetRightDistCm();

//mySerial.printf("%3.2f\t%3.2f\t%d\t%d\t%d\t%d\n", millis()/60000.0, global_yawval, getYawResult, fifoCount, GetPacketLoopCount, OuterGetPacketLoopCount);

mySerial.printf("%3.2f\t%3.2f\t%d\t%d\n", millis()/60000.0, global_yawval, leftdist, rightdist);

//12/06/19 commented out for now - just want constant motor speed

////change the motor speed (and maybe the direction)

//if (bMotorSpeedingUp)

//{

// MotorSpeed += MOTOR_SPEED_STEP_VAL;

//}

//else

//{

// MotorSpeed -= MOTOR_SPEED_STEP_VAL;

//}

//if (MotorSpeed >= MAX_MOTOR_SPEED)

//{

// bMotorSpeedingUp = false;

//}

//if (MotorSpeed <= MIN_MOTOR_SPEED)

//{

// //Serial.println("Reversing Motor Direction...");

// bMotorSpeedingUp = true;

// bMotorIsFwd = !bMotorIsFwd;

//}

////now actually set the motor speed and direction

//analogWrite(EnA_Pin, MotorSpeed);

//digitalWrite(In1_Pin, bMotorIsFwd ? LOW : HIGH);

//digitalWrite(In2_Pin, bMotorIsFwd ? HIGH : LOW);

////now actually set the motor speed and direction

//analogWrite(EnB_Pin, MotorSpeed);

//digitalWrite(In3_Pin, bMotorIsFwd ? LOW : HIGH);

//digitalWrite(In4_Pin, bMotorIsFwd ? HIGH : LOW);

}

#pragma region MOTOR_SUPPORT_ROUTINES

void RunBothMotors(int leftspeednum, int rightspeednum)

{

//Purpose: Run both Motors at left/rightspeednum speeds

//Inputs:

// speednum = speed value (0 = OFF, 255 = full speed)

//Outputs: Both Motors run for timesec seconds at speednum speed

//Plan:

//Notes:

// 01/14/15 - added left/right speed offset for straightness compensation

// 01/22/15 - added code to restrict fast/slow values

// 01/24/15 - revised for continuous run - no timing

// 01/26/15 - speednum modifications moved to UpdateWallFollowmyMotorspeeds()

// 12/07/15 - chg args from &int to int

////DEBUG!!

// mySerial.printf("In RunBothMotors(%d,%d\n", leftspeednum, rightspeednum);

////DEBUG!!

SetLeftMotorSpeed(leftspeednum);

SetRightMotorSpeed(rightspeednum);

}

//01/24/15 removed time from MoveAhead sig, added MoveAheadMsec

void MoveAhead(int leftspeednum, int rightspeednum)

{

//Purpose: Move ahead continuously

//Provenance: G. Frank Paynter and Danny Frank 01/24/2014

//Inputs:

// leftspeednum = integer denoting speed (0=stop, 255 = full speed)

// rightspeednum = integer denoting speed (0=stop, 255 = full speed)

//Outputs: both drive Motors energized with the specified speed

//Plan:

// Step 1: Set forward direction for both wheels

// Step 2: Run both Motors at specified speeds

//Step 1: Set forward direction for both wheels

SetLeftMotorDir(true);

SetRightMotorDir(true);

//Step 2: Run both myMotors for timsec seconds

RunBothMotors(leftspeednum, rightspeednum);

}

void SetLeftMotorSpeed(int speed)

{

if (bLeftMotorDirIsFwd)

{

speed = speed + LEFT_SPEED_COMP_VAL_FWD;

//check limits

speed = (speed <= MOTOR_SPEED_MAX) ? speed : MOTOR_SPEED_MAX;

speed = (speed >= MOTOR_SPEED_OFF) ? speed : MOTOR_SPEED_OFF;

}

else //must be REV

{

speed = speed + LEFT_SPEED_COMP_VAL_REV;

//check limits

speed = (speed <= MOTOR_SPEED_MAX) ? speed : MOTOR_SPEED_MAX;

speed = (speed >= MOTOR_SPEED_OFF) ? speed : MOTOR_SPEED_OFF;

}

#ifndef NO_MOTORS

Serial.print("Left Speed = "); Serial.println(speed);

analogWrite(EnA_Pin, speed);

#endif

}

//11/25/15 added to utilize new private member variable bRightMotorDirIsFwd

void SetRightMotorSpeed(int speed)

{

if (bRightMotorDirIsFwd)

{

speed = speed + LEFT_SPEED_COMP_VAL_FWD;

//check limits

speed = (speed <= MOTOR_SPEED_MAX) ? speed : MOTOR_SPEED_MAX;

speed = (speed >= MOTOR_SPEED_OFF) ? speed : MOTOR_SPEED_OFF;

}

else //must be REV

{

speed = speed + LEFT_SPEED_COMP_VAL_REV;

//check limits

speed = (speed <= MOTOR_SPEED_MAX) ? speed : MOTOR_SPEED_MAX;

speed = (speed >= MOTOR_SPEED_OFF) ? speed : MOTOR_SPEED_OFF;

}

#ifndef NO_MOTORS

Serial.print("Right Speed = "); Serial.println(speed);

analogWrite(EnB_Pin, speed);

#endif

}

void SetLeftMotorDir(boolean bIsFwdDir)

{

if (bIsFwdDir)

{

digitalWrite(In1_Pin, LOW);

digitalWrite(In2_Pin, HIGH);

}

else //must be REV

{

digitalWrite(In1_Pin, HIGH);

digitalWrite(In2_Pin, LOW);

}

bLeftMotorDirIsFwd = bIsFwdDir;

}

void SetRightMotorDir(boolean bIsFwdDir)

{

if (bIsFwdDir)

{

digitalWrite(In3_Pin, LOW);

digitalWrite(In4_Pin, HIGH);

}

else //must be REV

{

digitalWrite(In3_Pin, HIGH);

digitalWrite(In4_Pin, LOW);

}

bRightMotorDirIsFwd = bIsFwdDir;

}

#pragma endregion Motor Support Routines

bool GetIMUHeadingDeg()

{

//Purpose: Get latest yaw (heading) value from IMU

//Inputs: None. This function should only be called after mpu.dmpPacketAvailable() returns TRUE

//Outputs:

// returns true if successful, otherwise false

// global_yawval updated on success

//Plan:

//Step1: check for overflow and reset the FIFO if it occurs. In this case, wait for new packet

//Step2: read all available packets to get to latest data

//Step3: update global_yawval with latest value

//Notes:

// 10/08/19 changed return type to boolean

// 10/08/19 no longer need mpuIntStatus

// 10/21/19 completely rewritten to use Homer's algorithm

bool retval = false;

int flag = GetCurrentFIFOPacket(fifoBuffer, packetSize, MAX_GETPACKET_LOOPS); //get the latest mpu packet

if (flag != 0) //0 = error exit, 1 = normal exit, 2 = recovered from an overflow

{

// display Euler angles in degrees

mpu.dmpGetQuaternion(&q, fifoBuffer);

mpu.dmpGetGravity(&gravity, &q);

mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

//compute the yaw value

global_yawval = ypr[0] * 180 / M_PI;

retval = true;

}

return retval;

}

uint8_t GetCurrentFIFOPacket(uint8_t *data, uint8_t length, uint16_t max_loops)

{

max_loops = max(10, max_loops); //added 12/04/19 to ensure we loop at least 10 times. This really IS a hard-coded number!

fifoCount = mpu.getFIFOCount();

GetPacketLoopCount = 0;

//11/10/19 I have seen this fail with fifoC > 28 on occasion, so now I loop max three times

OuterGetPacketLoopCount = 0;

while (fifoCount != packetSize && OuterGetPacketLoopCount <= 3)

{

mpu.resetFIFO();

delay(1);

fifoCount = mpu.getFIFOCount();

GetPacketLoopCount = 0;

while (fifoCount < packetSize && GetPacketLoopCount < max_loops)

{

GetPacketLoopCount++;

fifoCount = mpu.getFIFOCount();

delay(2);

}

//if we get to here, there should be exactly one packet in the FIFO

OuterGetPacketLoopCount++;

}

if (OuterGetPacketLoopCount < 3)

{

mpu.getFIFOBytes(data, packetSize);

return 1;

}

return 0; //failed to get a good packet

}

int GetLeftDistCm()

{

//Serial.print("LeftPing\t"); Serial.println(millis());

//Notes:

// 04/30/17 added limit detection/correction

int timeUsec = LeftPing.ping();

//Serial.print("Left Ping Usec = "); Serial.println(timeUsec);

int distCm = LeftPing.convert_cm(timeUsec);

//04/30/17 added limit detection/correction

distCm = (distCm > 0) ? distCm : MAX_LR_DISTANCE_CM;

return distCm;

}

//01/10/18 reverted to regular ping(). median distance function takes too long

int GetRightDistCm()

{

//Serial.print("RightPing\t"); Serial.println(millis());

//Notes:

// 04/30/17 added limit detection/correction

int timeUsec = RightPing.ping();

//Serial.print("Right Ping Usec = "); Serial.println(timeUsec);

int distCm = RightPing.convert_cm(timeUsec);

//04/30/17 added limit detection/correction

distCm = (distCm > 0) ? distCm : MAX_LR_DISTANCE_CM;

return distCm;

}

The above program was intended to help me troubleshoot the intermittent MPU6050 connection failures I have been experiencing for some time now. The purpose of the new I2C sniffer project is to create a tool to log the actual I2C traffic between this program and the IMU. The idea is that when a failure occurs, I can look back through the sniffer log to see what happened; did the Arduino Mega stop transmitting requests, or did the IMU simply stop responding, or something else entirely.

Teensy I2C Sniffer for MPU6050

1 Reply

Posted 02 January 2020

On my last post on the I2C subject, I described an Excel VBA program to parse through a 928-byte array containing the captured I2C conversation between an Arduino Mega and a MPU6050 IMU module. The Arduino was running a very simple test program that repeatedly asked the MPU6050 to report the number of bytes available in its FIFO. Then I used Kito’s I2C sniffer code to capture the SDA/SCL transitions, which I then copy/pasted into Excel.

This post describes the next step, which was to port the Excel VBA code into a Teensy sketch using the Arduino version of C++, moving toward the ultimate goal of a Teensy based, fast I2C sniffer that can be used to monitor and log long-term (hours to days) I2C bus conversations to determine what is causing the intermittent hangups I’m seeing with my Arduino Mega/MPU-6050 robot project.

The code port took a while, but mostly because of my own lack of understanding about the details of the I2C protocol and the specifics of the communication between the Arduino test program and my MPU6050 IMU module. After working through these problems, the end result was surprisingly compact – less than 500 lines, including 50 lines of test data, LOTS of comments and debugging printouts, as shown below:

/*
    Name:       Teensy_I2C_Sniffer_V5.ino
    Created:	12/31/2019 11:36:00 PM
    Author:     FRANKNEWXPS15\Frank

    This is a port of my Excel VBA code into Arduino/Teensy C++ language.
    It parses the exact same data as the Excel program, so it should produce
    the exact same I2C sequences.

    The idea here is to not only perform the port, but to figure out how to
    produce a useful human-readable output format, and to determine how much
    time is required to parse the data and output the data to the serial port

    The captured data to be parsed is held in the 'simdata' array.

    The original VBA code is saved in 'ExcelVBACode.txt'

*/

/* 'Notes:

    A typical I2C sentence when communicating with a MPU6050 IMU module goes like:
        "I2C(68) wrote 1 byte to 75 - C0 Done."
        "I2C(68) wrote 3 bytes to 72 - C0 0C 10 Done."
        "I2C(68) read 5 bytes from 6A - C0 0C 10 14 03 Done."

    To form a sentence, we need:
        Device addr: 68 in the above examples
        Read/Write direction
        To/From register address:  75, 72 and 6A in the above examples
        Data:  C0, C0 0C 10, and C0 0C 10 14 03 in the above examples
        number of bytes written/read:  1,3 & 5 in the above examples

     Each I2C communication proceeds as follows (assuming a START from an IDLE condition):
         A START or RESTART condition, denoted by SDA & SCL HIGH, followed by SDA LOW, SCL HIGH
         A 7-bit device address, MSB first (0x8/0xC = 1, 0x0/0x4 = 0)
         A R/W bit (0x8/0xC = read, 0x0/0x4 = write)
         An ACK bit (0x8/0xC = NAK, 0x0/0x4 = ACK)
         If the bus direction is WRITE, then
             A register address for read/write
             zero or more additional data bytes
         Else (the bus direction is READ)
            One or more additional data bytes
         Endif
*/
const int SIM_DATA_ARRAY_SIZE = 928;
byte simdata[SIM_DATA_ARRAY_SIZE] =
{
    0xc, 0x4, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4, 0x8, 0xc, 0x0,
    0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x8, 0xc,
    0x8, 0xc, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0xc,
    0x4, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4,
    0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4, 0x8, 0xc, 0x0,
    0x4, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0xc, 0x4, 0x8, 0xc, 0x8, 0xc,
    0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0,
    0x4, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4,
    0x0, 0x4, 0x0, 0x4, 0xc, 0x4, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4, 0x8, 0xc, 0x0,
    0x4, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4,
    0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0xc,
    0x4, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4,
    0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4, 0x8, 0xc, 0x0,
    0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4,
    0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0xc, 0x4, 0x8,
    0xc, 0x8, 0xc, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4,
    0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x8,
    0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0xc, 0x4, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4,
    0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0,
    0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4,
    0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4, 0x0,
    0x4, 0x8, 0xc, 0x0, 0x4, 0xc, 0x4, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4, 0x8, 0xc,
    0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x8,
    0xc, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4,
    0xc, 0x4, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0,
    0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4,
    0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x8,
    0xc, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0xc, 0x4,
    0x8, 0xc, 0x8, 0xc, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0,
    0x4, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4,
    0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0xc, 0x4, 0x8, 0xc, 0x8, 0xc, 0x0,
    0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4,
    0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0,
    0x4, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4,
    0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0xc, 0x4, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4, 0x8,
    0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc,
    0x8, 0xc, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0,
    0x4, 0xc, 0x4, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4,
    0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0,
    0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x8, 0xc,
    0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0xc,
    0x4, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4,
    0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4, 0x0,
    0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0xc, 0x4, 0x8, 0xc, 0x8, 0xc,
    0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0,
    0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4,
    0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x8, 0xc, 0x0,
    0x4, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0xc, 0x4, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4,
    0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x8,
    0xc, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4,
    0x0, 0x4, 0xc, 0x4, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0,
    0x4, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4,
    0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x8, 0xc, 0x0,
    0x4, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4,
    0xc, 0x4, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0,
    0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4,
    0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0xc, 0x4, 0x8, 0xc, 0x8,
    0xc, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4,
    0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0,
    0x4, 0x0, 0x4, 0x8, 0xc, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4,
    0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0xc, 0x4, 0x8, 0xc, 0x8, 0xc, 0x0,
    0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4,
    0x8, 0xc
};

//#define PARSE_LOOP_DEBUG
//#define GET_8BIT_DATABYTE_DEBUG

#pragma region VARIABLES
uint8_t devAddr;
uint8_t regAddr;

//added to handle bus direction changes
enum BUSDIR
{
    WRITE,
    READ,
    UNKNOWN = -1
} RWDir;
BUSDIR BusDir = BUSDIR::UNKNOWN;

int ACKNAKFlag; //can be negative
uint8_t databyte_array[2048]; //holds multiple databytes for later output sentence construction
uint16_t databyte_idx = 0; //index into databyte_array FIFO
uint16_t numbytes = 0; //number of data bytes extracted from data stream
#pragma endregion Variables


void setup()
{
    Serial.begin(500000); //trying higher speed with teensy
    unsigned long now = millis();
    int idx = 0;
    while (!Serial && (millis() - now) < 3000)
    {
        delay(500);
        idx++;
    }

    uint16_t dataidx = 0;
    unsigned long startUsec = micros();

    while (dataidx < SIM_DATA_ARRAY_SIZE) //outer loop only necessary for simulation runs
    {
#ifdef PARSE_LOOP_DEBUG
        Serial.printf("looptop: dataidx = %d\n", dataidx);
#endif

        //Find a START sequence (0xC followed by 0x4)
        while (!IsStart(simdata, dataidx) && dataidx < SIM_DATA_ARRAY_SIZE)
        {
            dataidx++;
        }
#ifdef PARSE_LOOP_DEBUG
        Serial.printf("Start sequence found at dataidx = %d\n", dataidx);
#endif

        if (dataidx < SIM_DATA_ARRAY_SIZE - 14)//14 entries required for 7-bit address
        {
            //Get 7-bit device address
            devAddr = Get7BitDeviceAddr(simdata, dataidx);
#ifdef PARSE_LOOP_DEBUG
            Serial.printf("devAddr = %x, dataidx = %d\n", devAddr, dataidx);
#endif

            //get read/write flag  1 = Read, 0 = Write, -1 = error
            BusDir = (BUSDIR)GetReadWriteFlag(simdata, dataidx);

#ifdef PARSE_LOOP_DEBUG
            Serial.printf("BusDir = %s\n", ((BusDir == BUSDIR::WRITE) ? "WRITE" : "READ"));
#endif

            //get ACK/NAK flag
            ACKNAKFlag = GetACKNAKFlag(simdata, dataidx);

            numbytes = GetDataBytes(simdata, dataidx, databyte_array);
#ifdef PARSE_LOOP_DEBUG
            Serial.printf("Got %d bytes from GetDataBytes()\n", numbytes);
#endif

            //If the bus direction is WRITE, then extract
            //    A register address for read / write
            //    zero or more additional data bytes
                if (BusDir == BUSDIR::WRITE)
            {
                regAddr = databyte_array[0];
#ifdef PARSE_LOOP_DEBUG
                Serial.printf("regAddr = %x, dataidx = %d\n", regAddr, dataidx);
#endif

                //check for additional data
                if (numbytes > 1)
                {
#ifdef PARSE_LOOP_DEBUG
                    Serial.printf("Additional data found!\n");
                    for (size_t i = 0; i < numbytes; i++)
                    {
                        Serial.printf("data[%d] = %x\n", i, databyte_array[i]);
                    }
#endif

                    //1st byte is register addr, subsequent bytes are data
                    OutputFormattedSentence(BusDir, devAddr, regAddr, numbytes, databyte_array, 1 );
                }
            }
            else  //all bytes are data
            {
#ifdef PARSE_LOOP_DEBUG
                Serial.printf("In data block:  got %d bytes of data\n",numbytes);
                for (size_t i = 0; i < numbytes; i++)
                {
                    Serial.printf("data[%d] = %x\n", i, databyte_array[i]);
                }
#endif
                OutputFormattedSentence(BusDir, devAddr, regAddr, numbytes, databyte_array, 0);
            }

        }//if (dataidx < SIM_DATA_ARRAY_SIZE - 14)//14 entries required for 7-bit address

    }//while (dataidx < SIM_DATA_ARRAY_SIZE) //outer loop only necessary for simulation runs

    Serial.printf("Parsing %d bytes took %lu uSec\n", SIM_DATA_ARRAY_SIZE, micros() - startUsec);

}//Setup()

void loop()
{


}
bool IsStart(byte* data, uint16_t& readidx)
{
    bool result = false;

    //Serial.printf("IsStart[%d] = %x, IsStart[%d] = %x\n",
    //    readidx, data[readidx], readidx + 1, data[readidx + 1]);

    if (data[readidx] == 0xC && data[readidx + 1] == 0x4)
    {
        result = true;
        readidx += 2; //bump to next byte pair
    }
    return result;
}

bool IsStop(byte* data, uint16_t& readidx)
{
    bool result = false;

    //Serial.printf("IsStop[%d] = %x, IsStop[%d] = %x\n",
        //readidx, data[readidx], readidx + 1, data[readidx + 1]);

    if (data[readidx] == 0x4 && data[readidx + 1] == 0xC)
    {
        result = true;
        readidx += 2; //bump to next byte pair
    }
    return result;
}

uint8_t Get7BitDeviceAddr(byte* simdata, uint16_t& readidx)
{
    //Purpose: Construct a 7-bit address starting from dataidx
    //Inputs:
    //  simdata = pointer to simulated data array
    //  readidx = starting index of 7-bit address sequence (MSB first)
    //Outputs:
    //  returns the address as an 8-bit value with the MSB = 0, or 0x0 if unsuccessful
    //  dataidx = pointer to next simdata entry
    //Plan:
    //  Step1: Convert a pair of simdata entries into a 0 or 1
    //  Step2: Add the appropriate value to an ongoing sum
    //  Step3: return the total.
    //Notes:
    //  A '0' is coded as a 0x0 followed by a 0x4
    //  A '1' is coded as a 0x8 followed by a 0xC

    uint8_t devAddr = 0x0; //failure return value

    //Serial.printf("Get7BitDeviceAddr: readidx = %d\n",readidx);

    //devAddr is exactly 7 bits long, so 8 bits with MSB = 0
    for (size_t i = 0; i < 7; i++)
    {
        if (simdata[readidx] == 0x0 && simdata[readidx + 1] == 0x4)
        {
            readidx += 2; //advance the pointer, but don't add to sum
        }

        else if (simdata[readidx] == 0x8 && simdata[readidx + 1] == 0xC)
        {
            //Serial.printf("Get7BitDeviceAddr: '1' found at i = %d, adding %x to devAddr to get %x\n",
            //    i, 1 << (7 - i), devAddr + (1 << (7-i)));

            readidx += 2; //advance the pointer
            devAddr += (1 << (7 - i)); //add 2^(7-i) to sum
        }
    }

    devAddr = devAddr >> 1; //divide result by 2 to get 7-bit addr from 8 bits
    return devAddr;
}

int Get8BitDataByte(byte* simdata, uint16_t& readidx)
{
    //Purpose: Construct a 8-bit data byte starting from dataidx
    //Inputs:
    //  simdata = pointer to simulated data array
    //  readidx = starting index of 8-bit data byte (MSB first)
    //Outputs:
    //  returns the address as an 8-bit value, or 0x0 if unsuccessful
    //  dataidx = pointer to next simdata entry
    //Plan:
    //  Step1: Convert a pair of simdata entries into a 0 or 1
    //  Step2: Add the appropriate value to an ongoing sum
    //  Step3: return the total.
    //Notes:
    //  A '0' is coded as a 0x0 followed by a 0x4
    //  A '1' is coded as a 0x8 followed by a 0xC
    //  12/29/19 - changed return val to int, so can return -1 when a 'short byte' is detected

    int dataval = 0x0; //failure return value

#ifdef GET_8BIT_DATABYTE_DEBUG
    Serial.printf("Get8BitDataByte: simdata[%d] = %x, simdata[%d] = %x\n",
        readidx, simdata[readidx], readidx + 1, simdata[readidx + 1]);
#endif

    //8 bits with MSB = 0
    int numbytes = 0;
    for (size_t i = 0; i < 8; i++)
    {
        if (simdata[readidx] == 0x0 && simdata[readidx + 1] == 0x4)
        {
            readidx += 2; //advance the pointer, but don't add to sum
            numbytes++;
        }

        else if (simdata[readidx] == 0x8 && simdata[readidx + 1] == 0xC)
        {
#ifdef GET_8BIT_DATABYTE_DEBUG
            Serial.printf("Get8BitDataByte: '1' found at i = %d, adding %x to devAddr to get %x\n",
                i, 1 << (7 - i), dataval + (1 << (7 - i)));
#endif
            readidx += 2; //advance the pointer
            dataval += (1 << (7 - i)); //add 2^(8-i) to sum
            numbytes++;
        }
    }

#ifdef GET_8BIT_DATABYTE_DEBUG
    Serial.printf("Get8BitDataByte: numbytes = %d\n", numbytes);
#endif
    if (numbytes != 8)
    {
        dataval = -1; //error return value
    }

    return dataval;
}

int GetReadWriteFlag(byte* simdata, uint16_t& readidx)
{
    //Purpose: decode R/W byte pair
    //Inputs:
    //  simdata = pointer to byte capture array
    //  readidx = index into simdata to start of R/W byte pair
    //Outputs:
    //  readidx = if successful, points to next byte pair in simdata
    //  returns 1 for Read (0x8/0xC), 0 for Write (0x0/0x4), -1 for failure
    //Notes:
    //  

    //Serial.printf("GetReadWriteFlag: readidx = %d, simdata[readidx] = %x, simdata[readidx+1]= %x\n",
    //    readidx, simdata[readidx], simdata[readidx + 1]);
    int result = 0;
    if (simdata[readidx] == 0x8 && simdata[readidx + 1] == 0xC)
    {
        result = 1; //read detected
        readidx += 2; //point to next byte pair
    }

    else if (simdata[readidx] == 0x0 && simdata[readidx + 1] == 0x4)
    {
        result = 0; //write detected
        readidx += 2; //point to next byte pair
    }
    else
    {
        result = -1; //failed to detect read or write
    }

    return result;
}

int GetACKNAKFlag(byte* simdata, uint16_t& readidx)
{
    //Purpose: decode ACK/NAK byte pair
    //Inputs:
    //  simdata = pointer to byte capture array
    //  readidx = index into simdata to start of ACK/NAK byte pair
    //Outputs:
    //  readidx = if successful, points to next byte pair in simdata
    //  returns 1 for NAK (0x8/0xC), 0 for ACK (0x0/0x4), -1 for failure
    //Notes:
    //  

    //Serial.printf("GetACKNAKFlag: readidx = %d, simdata[readidx] = %x, simdata[readidx+1]= %x\n",
    //    readidx, simdata[readidx], simdata[readidx + 1]);
    int result = 0;
    if (simdata[readidx] == 0x8 && simdata[readidx + 1] == 0xC)
    {
        result = 1; //NAK detected
        readidx += 2; //point to next byte pair
    }

    else if (simdata[readidx] == 0x0 && simdata[readidx + 1] == 0x4)
    {
        result = 0; //ACK detected
        readidx += 2; //point to next byte pair
    }
    else
    {
        result = -1; //failed to detect ACK or NAK
    }

    return result;
}

int GetDataBytes(uint8_t* data, uint16_t& readidx, uint8_t* databytes)
{
    //Notes:
    //  01/01/2020: removed databyteidx from sig - always starts at zero

    uint16_t numbytes = 0;
    uint16_t databyte_idx = 0;

    bool StartFlag = false;
    bool StopFlag = false;

    do
    {
        int dataval = Get8BitDataByte(data, readidx);

        //watch out for 'short byte' reads
        if (dataval >= 0)
        {
            uint8_t databyte = (uint8_t)dataval;
            databyte_array[databyte_idx] = databyte;
            databyte_idx++;
            numbytes++;
        }

        ACKNAKFlag = GetACKNAKFlag(data, readidx);
        StartFlag = IsStart(data, readidx);
        StopFlag = IsStop(data, readidx);

#ifdef PARSE_LOOP_DEBUG
        Serial.printf("IsStart returned %d, IsStop returned %d, dataidx = %d\n",
            StartFlag, StopFlag, readidx);
#endif

    } while (!StartFlag && !StopFlag && readidx < SIM_DATA_ARRAY_SIZE);

    
    readidx -= 2;//back readidx up so loop top is positioned correctly.

    return numbytes;
}

void OutputFormattedSentence(int RW, uint8_t dev, uint8_t reg, uint8_t numbytes, uint8_t* bytearray,uint16_t startidx)
{
    Serial.printf("I2C(%x) %s %d bytes %s %x... ",
        dev, (RW == 0 ? "writing" : "reading"), numbytes-startidx, (RW == 0 ? "to" : "from"), reg);
    for (size_t i = startidx; i < numbytes; i++)
    {
        Serial.printf("%x ", bytearray[i]);
    }

    Serial.printf(". Done\n");

}

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

189

190

191

192

193

194

195

196

197

198

199

200

201

202

203

204

205

206

207

208

209

210

211

212

213

214

215

216

217

218

219

220

221

222

223

224

225

226

227

228

229

230

231

232

233

234

235

236

237

238

239

240

241

242

243

244

245

246

247

248

249

250

251

252

253

254

255

256

257

258

259

260

261

262

263

264

265

266

267

268

269

270

271

272

273

274

275

276

277

278

279

280

281

282

283

284

285

286

287

288

289

290

291

292

293

294

295

296

297

298

299

300

301

302

303

304

305

306

307

308

309

310

311

312

313

314

315

316

317

318

319

320

321

322

323

324

325

326

327

328

329

330

331

332

333

334

335

336

337

338

339

340

341

342

343

344

345

346

347

348

349

350

351

352

353

354

355

356

357

358

359

360

361

362

363

364

365

366

367

368

369

370

371

372

373

374

375

376

377

378

379

380

381

382

383

384

385

386

387

388

389

390

391

392

393

394

395

396

397

398

399

400

401

402

403

404

405

406

407

408

409

410

411

412

413

414

415

416

417

418

419

420

421

422

423

424

425

426

427

428

429

430

431

432

433

434

435

436

437

438

439

440

441

442

443

444

445

446

447

448

449

450

451

452

453

454

455

456

457

458

459

460

461

462

463

464

465

466

467

468

469

470

471

472

473

474

475

476

477

478

479

480

481

482

483

484

485

486

487

488

489

490

Name: Teensy_I2C_Sniffer_V5.ino

Created: 12/31/2019 11:36:00 PM

Author: FRANKNEWXPS15\Frank

This is a port of my Excel VBA code into Arduino/Teensy C++ language.

It parses the exact same data as the Excel program, so it should produce

the exact same I2C sequences.

The idea here is to not only perform the port, but to figure out how to

produce a useful human-readable output format, and to determine how much

time is required to parse the data and output the data to the serial port

The captured data to be parsed is held in the 'simdata' array.

The original VBA code is saved in 'ExcelVBACode.txt'

/* 'Notes:

A typical I2C sentence when communicating with a MPU6050 IMU module goes like:

"I2C(68) wrote 1 byte to 75 - C0 Done."

"I2C(68) wrote 3 bytes to 72 - C0 0C 10 Done."

"I2C(68) read 5 bytes from 6A - C0 0C 10 14 03 Done."

To form a sentence, we need:

Device addr: 68 in the above examples

Read/Write direction

To/From register address: 75, 72 and 6A in the above examples

Data: C0, C0 0C 10, and C0 0C 10 14 03 in the above examples

number of bytes written/read: 1,3 & 5 in the above examples

Each I2C communication proceeds as follows (assuming a START from an IDLE condition):

A START or RESTART condition, denoted by SDA & SCL HIGH, followed by SDA LOW, SCL HIGH

A 7-bit device address, MSB first (0x8/0xC = 1, 0x0/0x4 = 0)

A R/W bit (0x8/0xC = read, 0x0/0x4 = write)

An ACK bit (0x8/0xC = NAK, 0x0/0x4 = ACK)

If the bus direction is WRITE, then

A register address for read/write

zero or more additional data bytes

Else (the bus direction is READ)

One or more additional data bytes

Endif

const int SIM_DATA_ARRAY_SIZE = 928;

byte simdata[SIM_DATA_ARRAY_SIZE] =

{

0xc, 0x4, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4, 0x8, 0xc, 0x0,

0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x8, 0xc,

0x8, 0xc, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0xc,

0x4, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4,

0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4, 0x8, 0xc, 0x0,

0x4, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0xc, 0x4, 0x8, 0xc, 0x8, 0xc,

0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0,

0x4, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4,

0x0, 0x4, 0x0, 0x4, 0xc, 0x4, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4, 0x8, 0xc, 0x0,

0x4, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4,

0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0xc,

0x4, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4,

0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4, 0x8, 0xc, 0x0,

0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4,

0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0xc, 0x4, 0x8,

0xc, 0x8, 0xc, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4,

0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x8,

0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0xc, 0x4, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4,

0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0,

0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4,

0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4, 0x0,

0x4, 0x8, 0xc, 0x0, 0x4, 0xc, 0x4, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4, 0x8, 0xc,

0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x8,

0xc, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4,

0xc, 0x4, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0,

0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4,

0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x8,

0xc, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0xc, 0x4,

0x8, 0xc, 0x8, 0xc, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0,

0x4, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4,

0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0xc, 0x4, 0x8, 0xc, 0x8, 0xc, 0x0,

0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4,

0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0,

0x4, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4,

0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0xc, 0x4, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4, 0x8,

0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc,

0x8, 0xc, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0,

0x4, 0xc, 0x4, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4,

0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0,

0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x8, 0xc,

0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0xc,

0x4, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4,

0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4, 0x0,

0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0xc, 0x4, 0x8, 0xc, 0x8, 0xc,

0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0,

0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4,

0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x8, 0xc, 0x0,

0x4, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0xc, 0x4, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4,

0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x8,

0xc, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4,

0x0, 0x4, 0xc, 0x4, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0,

0x4, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4,

0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x8, 0xc, 0x0,

0x4, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4,

0xc, 0x4, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0,

0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4,

0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0xc, 0x4, 0x8, 0xc, 0x8,

0xc, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4,

0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0,

0x4, 0x0, 0x4, 0x8, 0xc, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4,

0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0xc, 0x4, 0x8, 0xc, 0x8, 0xc, 0x0,

0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4,

0x8, 0xc

};

//#define PARSE_LOOP_DEBUG

//#define GET_8BIT_DATABYTE_DEBUG

#pragma region VARIABLES

uint8_t devAddr;

uint8_t regAddr;

//added to handle bus direction changes

enum BUSDIR

{

WRITE,

READ,

UNKNOWN = -1

} RWDir;

BUSDIR BusDir = BUSDIR::UNKNOWN;

int ACKNAKFlag; //can be negative

uint8_t databyte_array[2048]; //holds multiple databytes for later output sentence construction

uint16_t databyte_idx = 0; //index into databyte_array FIFO

uint16_t numbytes = 0; //number of data bytes extracted from data stream

#pragma endregion Variables

void setup()

{

Serial.begin(500000); //trying higher speed with teensy

unsigned long now = millis();

int idx = 0;

while (!Serial && (millis() - now) < 3000)

{

delay(500);

idx++;

}

uint16_t dataidx = 0;

unsigned long startUsec = micros();

while (dataidx < SIM_DATA_ARRAY_SIZE) //outer loop only necessary for simulation runs

{

#ifdef PARSE_LOOP_DEBUG

Serial.printf("looptop: dataidx = %d\n", dataidx);

#endif

//Find a START sequence (0xC followed by 0x4)

while (!IsStart(simdata, dataidx) && dataidx < SIM_DATA_ARRAY_SIZE)

{

dataidx++;

}

#ifdef PARSE_LOOP_DEBUG

Serial.printf("Start sequence found at dataidx = %d\n", dataidx);

#endif

if (dataidx < SIM_DATA_ARRAY_SIZE - 14)//14 entries required for 7-bit address

{

//Get 7-bit device address

devAddr = Get7BitDeviceAddr(simdata, dataidx);

#ifdef PARSE_LOOP_DEBUG

Serial.printf("devAddr = %x, dataidx = %d\n", devAddr, dataidx);

#endif

//get read/write flag 1 = Read, 0 = Write, -1 = error

BusDir = (BUSDIR)GetReadWriteFlag(simdata, dataidx);

#ifdef PARSE_LOOP_DEBUG

Serial.printf("BusDir = %s\n", ((BusDir == BUSDIR::WRITE) ? "WRITE" : "READ"));

#endif

//get ACK/NAK flag

ACKNAKFlag = GetACKNAKFlag(simdata, dataidx);

numbytes = GetDataBytes(simdata, dataidx, databyte_array);

#ifdef PARSE_LOOP_DEBUG

Serial.printf("Got %d bytes from GetDataBytes()\n", numbytes);

#endif

//If the bus direction is WRITE, then extract

// A register address for read / write

// zero or more additional data bytes

if (BusDir == BUSDIR::WRITE)

{

regAddr = databyte_array[0];

#ifdef PARSE_LOOP_DEBUG

Serial.printf("regAddr = %x, dataidx = %d\n", regAddr, dataidx);

#endif

//check for additional data

if (numbytes > 1)

{

#ifdef PARSE_LOOP_DEBUG

Serial.printf("Additional data found!\n");

for (size_t i = 0; i < numbytes; i++)

{

Serial.printf("data[%d] = %x\n", i, databyte_array[i]);

}

#endif

//1st byte is register addr, subsequent bytes are data

OutputFormattedSentence(BusDir, devAddr, regAddr, numbytes, databyte_array, 1 );

}

else //all bytes are data

{

#ifdef PARSE_LOOP_DEBUG

Serial.printf("In data block: got %d bytes of data\n",numbytes);

for (size_t i = 0; i < numbytes; i++)

{

Serial.printf("data[%d] = %x\n", i, databyte_array[i]);

}

#endif

OutputFormattedSentence(BusDir, devAddr, regAddr, numbytes, databyte_array, 0);

}

}//if (dataidx < SIM_DATA_ARRAY_SIZE - 14)//14 entries required for 7-bit address

}//while (dataidx < SIM_DATA_ARRAY_SIZE) //outer loop only necessary for simulation runs

Serial.printf("Parsing %d bytes took %lu uSec\n", SIM_DATA_ARRAY_SIZE, micros() - startUsec);

}//Setup()

void loop()

{

}

bool IsStart(byte* data, uint16_t& readidx)

{

bool result = false;

//Serial.printf("IsStart[%d] = %x, IsStart[%d] = %x\n",

// readidx, data[readidx], readidx + 1, data[readidx + 1]);

if (data[readidx] == 0xC && data[readidx + 1] == 0x4)

{

result = true;

readidx += 2; //bump to next byte pair

}

return result;

}

bool IsStop(byte* data, uint16_t& readidx)

{

bool result = false;

//Serial.printf("IsStop[%d] = %x, IsStop[%d] = %x\n",

//readidx, data[readidx], readidx + 1, data[readidx + 1]);

if (data[readidx] == 0x4 && data[readidx + 1] == 0xC)

{

result = true;

readidx += 2; //bump to next byte pair

}

return result;

}

uint8_t Get7BitDeviceAddr(byte* simdata, uint16_t& readidx)

{

//Purpose: Construct a 7-bit address starting from dataidx

//Inputs:

// simdata = pointer to simulated data array

// readidx = starting index of 7-bit address sequence (MSB first)

//Outputs:

// returns the address as an 8-bit value with the MSB = 0, or 0x0 if unsuccessful

// dataidx = pointer to next simdata entry

//Plan:

// Step1: Convert a pair of simdata entries into a 0 or 1

// Step2: Add the appropriate value to an ongoing sum

// Step3: return the total.

//Notes:

// A '0' is coded as a 0x0 followed by a 0x4

// A '1' is coded as a 0x8 followed by a 0xC

uint8_t devAddr = 0x0; //failure return value

//Serial.printf("Get7BitDeviceAddr: readidx = %d\n",readidx);

//devAddr is exactly 7 bits long, so 8 bits with MSB = 0

for (size_t i = 0; i < 7; i++)

{

if (simdata[readidx] == 0x0 && simdata[readidx + 1] == 0x4)

{

readidx += 2; //advance the pointer, but don't add to sum

}

else if (simdata[readidx] == 0x8 && simdata[readidx + 1] == 0xC)

{

//Serial.printf("Get7BitDeviceAddr: '1' found at i = %d, adding %x to devAddr to get %x\n",

// i, 1 << (7 - i), devAddr + (1 << (7-i)));

readidx += 2; //advance the pointer

devAddr += (1 << (7 - i)); //add 2^(7-i) to sum

}

devAddr = devAddr >> 1; //divide result by 2 to get 7-bit addr from 8 bits

return devAddr;

}

int Get8BitDataByte(byte* simdata, uint16_t& readidx)

{

//Purpose: Construct a 8-bit data byte starting from dataidx

//Inputs:

// simdata = pointer to simulated data array

// readidx = starting index of 8-bit data byte (MSB first)

//Outputs:

// returns the address as an 8-bit value, or 0x0 if unsuccessful

// dataidx = pointer to next simdata entry

//Plan:

// Step1: Convert a pair of simdata entries into a 0 or 1

// Step2: Add the appropriate value to an ongoing sum

// Step3: return the total.

//Notes:

// A '0' is coded as a 0x0 followed by a 0x4

// A '1' is coded as a 0x8 followed by a 0xC

// 12/29/19 - changed return val to int, so can return -1 when a 'short byte' is detected

int dataval = 0x0; //failure return value

#ifdef GET_8BIT_DATABYTE_DEBUG

Serial.printf("Get8BitDataByte: simdata[%d] = %x, simdata[%d] = %x\n",

readidx, simdata[readidx], readidx + 1, simdata[readidx + 1]);

#endif

//8 bits with MSB = 0

int numbytes = 0;

for (size_t i = 0; i < 8; i++)

{

if (simdata[readidx] == 0x0 && simdata[readidx + 1] == 0x4)

{

readidx += 2; //advance the pointer, but don't add to sum

numbytes++;

}

else if (simdata[readidx] == 0x8 && simdata[readidx + 1] == 0xC)

{

#ifdef GET_8BIT_DATABYTE_DEBUG

Serial.printf("Get8BitDataByte: '1' found at i = %d, adding %x to devAddr to get %x\n",

i, 1 << (7 - i), dataval + (1 << (7 - i)));

#endif

readidx += 2; //advance the pointer

dataval += (1 << (7 - i)); //add 2^(8-i) to sum

numbytes++;

}

#ifdef GET_8BIT_DATABYTE_DEBUG

Serial.printf("Get8BitDataByte: numbytes = %d\n", numbytes);

#endif

if (numbytes != 8)

{

dataval = -1; //error return value

}

return dataval;

}

int GetReadWriteFlag(byte* simdata, uint16_t& readidx)

{

//Purpose: decode R/W byte pair

//Inputs:

// simdata = pointer to byte capture array

// readidx = index into simdata to start of R/W byte pair

//Outputs:

// readidx = if successful, points to next byte pair in simdata

// returns 1 for Read (0x8/0xC), 0 for Write (0x0/0x4), -1 for failure

//Notes:

//Serial.printf("GetReadWriteFlag: readidx = %d, simdata[readidx] = %x, simdata[readidx+1]= %x\n",

// readidx, simdata[readidx], simdata[readidx + 1]);

int result = 0;

if (simdata[readidx] == 0x8 && simdata[readidx + 1] == 0xC)

{

result = 1; //read detected

readidx += 2; //point to next byte pair

}

else if (simdata[readidx] == 0x0 && simdata[readidx + 1] == 0x4)

{

result = 0; //write detected

readidx += 2; //point to next byte pair

}

else

{

result = -1; //failed to detect read or write

}

return result;

}

int GetACKNAKFlag(byte* simdata, uint16_t& readidx)

{

//Purpose: decode ACK/NAK byte pair

//Inputs:

// simdata = pointer to byte capture array

// readidx = index into simdata to start of ACK/NAK byte pair

//Outputs:

// readidx = if successful, points to next byte pair in simdata

// returns 1 for NAK (0x8/0xC), 0 for ACK (0x0/0x4), -1 for failure

//Notes:

//Serial.printf("GetACKNAKFlag: readidx = %d, simdata[readidx] = %x, simdata[readidx+1]= %x\n",

// readidx, simdata[readidx], simdata[readidx + 1]);

int result = 0;

if (simdata[readidx] == 0x8 && simdata[readidx + 1] == 0xC)

{

result = 1; //NAK detected

readidx += 2; //point to next byte pair

}

else if (simdata[readidx] == 0x0 && simdata[readidx + 1] == 0x4)

{

result = 0; //ACK detected

readidx += 2; //point to next byte pair

}

else

{

result = -1; //failed to detect ACK or NAK

}

return result;

}

int GetDataBytes(uint8_t* data, uint16_t& readidx, uint8_t* databytes)

{

//Notes:

// 01/01/2020: removed databyteidx from sig - always starts at zero

uint16_t numbytes = 0;

uint16_t databyte_idx = 0;

bool StartFlag = false;

bool StopFlag = false;

{

int dataval = Get8BitDataByte(data, readidx);

//watch out for 'short byte' reads

if (dataval >= 0)

{

uint8_t databyte = (uint8_t)dataval;

databyte_array[databyte_idx] = databyte;

databyte_idx++;

numbytes++;

}

ACKNAKFlag = GetACKNAKFlag(data, readidx);

StartFlag = IsStart(data, readidx);

StopFlag = IsStop(data, readidx);

#ifdef PARSE_LOOP_DEBUG

Serial.printf("IsStart returned %d, IsStop returned %d, dataidx = %d\n",

StartFlag, StopFlag, readidx);

#endif

} while (!StartFlag && !StopFlag && readidx < SIM_DATA_ARRAY_SIZE);

readidx -= 2;//back readidx up so loop top is positioned correctly.

return numbytes;

}

void OutputFormattedSentence(int RW, uint8_t dev, uint8_t reg, uint8_t numbytes, uint8_t* bytearray,uint16_t startidx)

{

Serial.printf("I2C(%x) %s %d bytes %s %x... ",

dev, (RW == 0 ? "writing" : "reading"), numbytes-startidx, (RW == 0 ? "to" : "from"), reg);

for (size_t i = startidx; i < numbytes; i++)

{

Serial.printf("%x ", bytearray[i]);

}

Serial.printf(". Done\n");

}

When I ran this program against the captured data, I got the following output:

Opening port
Port open
I2C(68) reading 1 bytes from 75... 68 . Done
I2C(68) reading 1 bytes from 6a... c0 . Done
I2C(68) writing 1 bytes to 6a... c4 . Done
I2C(68) reading 2 bytes from 72... 0 1c . Done
I2C(68) reading 2 bytes from 72... 0 38 . Done
I2C(68) reading 2 bytes from 72... 0 54 . Done
I2C(68) reading 2 bytes from 72... 0 70 . Done
I2C(68) reading 2 bytes from 72... 0 8c . Done
I2C(68) reading 2 bytes from 72... 0 c4 . Done
I2C(68) reading 2 bytes from 72... 0 e0 . Done
Parsing 928 bytes took 1081 uSec

Opening port

Port open

I2C(68) reading 1 bytes from 75... 68 . Done

I2C(68) reading 1 bytes from 6a... c0 . Done

I2C(68) writing 1 bytes to 6a... c4 . Done

I2C(68) reading 2 bytes from 72... 0 1c . Done

I2C(68) reading 2 bytes from 72... 0 38 . Done

I2C(68) reading 2 bytes from 72... 0 54 . Done

I2C(68) reading 2 bytes from 72... 0 70 . Done

I2C(68) reading 2 bytes from 72... 0 8c . Done

I2C(68) reading 2 bytes from 72... 0 c4 . Done

I2C(68) reading 2 bytes from 72... 0 e0 . Done

Parsing 928 bytes took 1081 uSec

which, when compared against the debug printout from Jeff Rowberg’s I2CDev program,

I2C (0x68) reading 1 bytes from 0x75...68. Done (1 read).
I2C (0x68) reading 1 bytes from 0x6A...C0. Done (1 read).
I2C (0x68) writing 1 bytes to 0x6A...C4. Done.
I2C (0x68) reading 2 bytes from 0x72...0 1C. Done (2 read).
I2C (0x68) reading 2 bytes from 0x72...0 38. Done (2 read).
I2C (0x68) reading 2 bytes from 0x72...0 54. Done (2 read).
I2C (0x68) reading 2 bytes from 0x72...0 70. Done (2 read).
I2C (0x68) reading 2 bytes from 0x72...0 8C. Done (2 read).
I2C (0x68) reading 2 bytes from 0x72...0 C4. Done (2 read).
I2C (0x68) reading 2 bytes from 0x72...0 E0. Done (2 read).

I2C (0x68) reading 1 bytes from 0x75...68. Done (1 read).

I2C (0x68) reading 1 bytes from 0x6A...C0. Done (1 read).

I2C (0x68) writing 1 bytes to 0x6A...C4. Done.

I2C (0x68) reading 2 bytes from 0x72...0 1C. Done (2 read).

I2C (0x68) reading 2 bytes from 0x72...0 38. Done (2 read).

I2C (0x68) reading 2 bytes from 0x72...0 54. Done (2 read).

I2C (0x68) reading 2 bytes from 0x72...0 70. Done (2 read).

I2C (0x68) reading 2 bytes from 0x72...0 8C. Done (2 read).

I2C (0x68) reading 2 bytes from 0x72...0 C4. Done (2 read).

I2C (0x68) reading 2 bytes from 0x72...0 E0. Done (2 read).

shows that my Teensy program correctly decoded the entire test dataset.

The next step in the process is to modify the above program to allow long-term real-time monitoring and logging of a live I2C bus. By ‘long-term’, I mean hours if not days, as the object of the exercise is to figure out why the I2C bus connection to the MPU6050 on my two-motor robot intermittently fails when the motors are running. A failure can occur within minutes, or only after several hours, and there doesn’t seem to be any rhyme nor reason, except that the motors have to be running.

In normal operation, my two-motor robot obtains a heading value from the MPU6050 once every 200 mSec or so. This I2C bus activity might comprise only 100 SCL/SDA transitions or so, and no other I2C bus activity takes place in the times between heading value requests. So, there will be few mSec of burst activity, followed by a 150-190 mSec idle period. To monitor and log in real time, I need some sort of FIFO arrangement, where the I2C transition data can be saved into the FIFO during the burst, and then processed and saved into a log file during the idle period.

While I was searching the web for I2C sniffer code, I also ran across this thread by tonton81 describing his template based circular buffer library for the Teensy line. The thread started a little over two years ago, but has been quite active during that period, and tonton81 has made several bugfixes, updates, and enhancements to his code. This might be just the thing for my project.

06 January 2020 Update:

After integrating tonton81’s circular buffer library into the project (thanks tonton81 for being so responsive with bugfixes!), I was able to demonstrate that the circular buffer version, when run against the same 928-byte simulated dataset, produced the same output as before, as shown below:

Opening port
Port open
Port open
Serial available after 2500 mSec
3001: cb_trans contains = 928 elements
I2C(68) reading 1 bytes from 75... 68 . Done
I2C(68) reading 1 bytes from 6a... c0 . Done
I2C(68) writing 1 bytes to 6a... c4 . Done
I2C(68) reading 2 bytes from 72... 0 1c . Done
I2C(68) reading 2 bytes from 72... 0 38 . Done
I2C(68) reading 2 bytes from 72... 0 54 . Done
I2C(68) reading 2 bytes from 72... 0 70 . Done
I2C(68) reading 2 bytes from 72... 0 8c . Done
I2C(68) reading 2 bytes from 72... 0 c4 . Done
I2C(68) reading 2 bytes from 72... 0 e0 . Done
3003: cb_trans contains = 0 elements

Opening port

Port open

Serial available after 2500 mSec

3001: cb_trans contains = 928 elements

I2C(68) reading 1 bytes from 75... 68 . Done

I2C(68) reading 1 bytes from 6a... c0 . Done

I2C(68) writing 1 bytes to 6a... c4 . Done

I2C(68) reading 2 bytes from 72... 0 1c . Done

I2C(68) reading 2 bytes from 72... 0 38 . Done

I2C(68) reading 2 bytes from 72... 0 54 . Done

I2C(68) reading 2 bytes from 72... 0 70 . Done

I2C(68) reading 2 bytes from 72... 0 8c . Done

I2C(68) reading 2 bytes from 72... 0 c4 . Done

I2C(68) reading 2 bytes from 72... 0 e0 . Done

3003: cb_trans contains = 0 elements

From the output above, it is clear that the Teensy can parse and print a typical 1000-byte burst in just a few mSec (3 in the above run), so it should have no problem keeping up with a 200 mSec data burst interval, and should be able to keep up with burst intervals down to around 10 mSec (100 bursts/sec!) I suspect that the Teensy’s major problem will be not dying of boredom waiting for the next burst to process!

Here’s the full code (not including circular_buffer.h):

/*
    Name:       Teensy_I2C_Sniffer_V6.ino
    Created:	1/4/2020 8:40:20 AM
    Author:     FRANKNEWXPS15\Frank

    This is a port of my Excel VBA code into Arduino/Teensy C++ language.
    It parses the exact same sample data as the Excel program, so it should
    produce the exact same I2C sequences.

    The captured sample data to be parsed is held in the 'simdata' array.

    The original VBA code is saved in 'ExcelVBACode.txt'
*/

/* 'Notes:

    A typical I2C sentence when communicating with a MPU6050 IMU module goes like:
        "I2C(68) wrote 1 byte to 75 - C0 Done."
        "I2C(68) wrote 3 bytes to 72 - C0 0C 10 Done."
        "I2C(68) read 5 bytes from 6A - C0 0C 10 14 03 Done."

    To form a sentence, we need:
        Device addr: 68 in the above examples
        Read/Write direction
        To/From register address:  75, 72 and 6A in the above examples
        Data:  C0, C0 0C 10, and C0 0C 10 14 03 in the above examples
        number of bytes written/read:  1,3 & 5 in the above examples

     Each I2C communication proceeds as follows (assuming a START from an IDLE condition):
         A START or RESTART condition, denoted by SDA & SCL HIGH, followed by SDA LOW, SCL HIGH
         A 7-bit device address, MSB first (0x8/0xC = 1, 0x0/0x4 = 0)
         A R/W bit (0x8/0xC = read, 0x0/0x4 = write)
         An ACK bit (0x8/0xC = NAK, 0x0/0x4 = ACK)
         If the bus direction is WRITE, then
             A register address for read/write
             zero or more additional data bytes
         Else (the bus direction is READ)
            One or more additional data bytes
         Endif
*/

/*
This version adds tonton81's circular buffer class (https://github.com/tonton81/Circular_Buffer)
to simulate real-time processing and display of I2C bus activity.  The sample data is loaded into
a FIFO by an ISR triggered every microsecond by a timer, and then the processing code pulls it out
and parses it as fast as possible.  If this works, then the next version will eliminate the sample
data and connect to a live I2C bus.
*/

#include <TimerOne.h> //needed for ISR
#include "circular_buffer.h"
Circular_Buffer<uint8_t, 2048> cb_trans; //holds transition values from ISR

#define MONITOR_OUT1 2 //so can monitor ISR activity with O'scope
#define MONITOR_OUT2 3 //so can monitor ISR activity with O'scope

const int SIM_DATA_ARRAY_SIZE = 928;
byte simdata[SIM_DATA_ARRAY_SIZE] =
{
    0xc, 0x4, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4, 0x8, 0xc, 0x0,
    0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x8, 0xc,
    0x8, 0xc, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0xc,
    0x4, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4,
    0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4, 0x8, 0xc, 0x0,
    0x4, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0xc, 0x4, 0x8, 0xc, 0x8, 0xc,
    0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0,
    0x4, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4,
    0x0, 0x4, 0x0, 0x4, 0xc, 0x4, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4, 0x8, 0xc, 0x0,
    0x4, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4,
    0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0xc,
    0x4, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4,
    0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4, 0x8, 0xc, 0x0,
    0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4,
    0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0xc, 0x4, 0x8,
    0xc, 0x8, 0xc, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4,
    0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x8,
    0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0xc, 0x4, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4,
    0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0,
    0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4,
    0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4, 0x0,
    0x4, 0x8, 0xc, 0x0, 0x4, 0xc, 0x4, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4, 0x8, 0xc,
    0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x8,
    0xc, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4,
    0xc, 0x4, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0,
    0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4,
    0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x8,
    0xc, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0xc, 0x4,
    0x8, 0xc, 0x8, 0xc, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0,
    0x4, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4,
    0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0xc, 0x4, 0x8, 0xc, 0x8, 0xc, 0x0,
    0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4,
    0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0,
    0x4, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4,
    0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0xc, 0x4, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4, 0x8,
    0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc,
    0x8, 0xc, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0,
    0x4, 0xc, 0x4, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4,
    0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0,
    0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x8, 0xc,
    0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0xc,
    0x4, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4,
    0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4, 0x0,
    0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0xc, 0x4, 0x8, 0xc, 0x8, 0xc,
    0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0,
    0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4,
    0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x8, 0xc, 0x0,
    0x4, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0xc, 0x4, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4,
    0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x8,
    0xc, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4,
    0x0, 0x4, 0xc, 0x4, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0,
    0x4, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4,
    0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x8, 0xc, 0x0,
    0x4, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4,
    0xc, 0x4, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0,
    0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4,
    0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0xc, 0x4, 0x8, 0xc, 0x8,
    0xc, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4,
    0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0,
    0x4, 0x0, 0x4, 0x8, 0xc, 0x8, 0xc, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4,
    0x0, 0x4, 0x0, 0x4, 0x8, 0xc, 0x0, 0x4, 0xc, 0x4, 0x8, 0xc, 0x8, 0xc, 0x0,
    0x4, 0x8, 0xc, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4, 0x0, 0x4,
    0x8, 0xc
};

//#define PARSE_LOOP_DEBUG
//#define GET_8BIT_DATABYTE_DEBUG
const int BUFFER_CHECK_INTERVAL_MSEC = 200;
//const int BUFFER_CHECK_INTERVAL_MSEC = 5;
#pragma region PROCESSING_VARIABLES
uint8_t devAddr;
uint8_t regAddr;

//added for bus direction labels
enum BUSDIR
{
    WRITE,
    READ,
    UNKNOWN = -1
} RWDir;
BUSDIR BusDir = BUSDIR::UNKNOWN;

int ACKNAKFlag; //can be negative
uint8_t databyte_array[2048]; //holds multiple databytes for later output sentence construction
uint16_t databyte_idx = 0; //index into databyte_array
uint16_t numbytes = 0; //number of data bytes extracted from data stream
uint8_t killbuff[2]; //used to consume start/stop bytes

#pragma endregion ProcVars

#pragma region ISR_VARS
uint16_t  write_index = 0;
uint16_t  read_index = 0;
uint8_t   current_portb = 0xFF;
uint8_t   last_portb = 0xFF;
bool bBufferFull = false;
elapsedMillis mSecSinceLastBufferCheck;
#pragma endregion ISRVars

int DecodeAndPrintFIFOContents(Circular_Buffer<uint8_t, 2048>& cb); //need forward reference here


//-------------------------------------------------------------------------------
//--------------------------------    ISR    ------------------------------------
//-------------------------------------------------------------------------------
//01/04/2020 modified to 'capture' data from simdata[]
void capture_data(void)
{
    if (!bBufferFull) //for debug - writes only one set of data to FIFO
    {
        last_portb = current_portb;
        current_portb = simdata[read_index]; //pull one transition value from simdata
        read_index++;

        if (last_portb != current_portb && !bBufferFull)
        {
            cb_trans.write(current_portb); //add current transition value to FIFO
            digitalWriteFast(MONITOR_OUT1, !digitalReadFast(MONITOR_OUT1));
        }
    
        //pause writing to cb_trans circular buffer
        if (read_index >= SIM_DATA_ARRAY_SIZE)
        {
            bBufferFull = true;
            read_index = 0;
            digitalWriteFast(MONITOR_OUT1, LOW);
        }
    }
}

void setup()
{
    Serial.begin(1); //rate value ignored
    unsigned long now = millis();
    int idx = 0;
    while (!Serial && (millis() - now) < 3000)
    {
        delay(500);
        idx++;
    }
    Serial.printf("Serial available after %lu mSec\n", millis() - now);

    pinMode(MONITOR_OUT1, OUTPUT);
    pinMode(MONITOR_OUT2, OUTPUT);
    pinMode(LED_BUILTIN, OUTPUT);
    Timer1.initialize(1); // run every mico second
    Timer1.attachInterrupt(capture_data);

    mSecSinceLastBufferCheck = 0;
}

void loop()
{
    if (mSecSinceLastBufferCheck > BUFFER_CHECK_INTERVAL_MSEC)
    {
        Serial.printf("%lu: cb_trans contains = %d elements\n", millis(), cb_trans.size());
        mSecSinceLastBufferCheck -= BUFFER_CHECK_INTERVAL_MSEC;
        DecodeAndPrintFIFOContents(cb_trans); //decode and print everything captured so far
        Serial.printf("%lu: cb_trans contains = %d elements\n", millis(), cb_trans.size());
        //cb_trans.clear();
        read_index = 0;
        bBufferFull = false;
        digitalWriteFast(MONITOR_OUT2, !digitalReadFast(MONITOR_OUT2));
        while (true)
        {

        }
    }
}

void PrintNextFIFOBytes(Circular_Buffer<uint8_t, 2048>& cb, uint8_t numbytes)
{
    Serial.printf("Next %d FIFO Bytes: ", numbytes);
    for (uint8_t i = 0; i < numbytes-1; i++)
    {
        Serial.printf("%x,", cb.peek(i));
    }
    Serial.printf("%x\n", cb.peek(numbytes-1));
}
bool IsStart(Circular_Buffer<uint8_t, 2048>& cb)
{
    bool result = false;

    uint8_t data[2]; data[0] = data[1] = 0;
    data[0] = cb.peek(0);
    data[1] = cb.peek(1);
    if (data[0] == 0xC && data[1] == 0x4)
    {
        //Serial.printf("Start detected at size = %d, data idx = %d\n", cb.size(), SIM_DATA_ARRAY_SIZE - cb.size());
        result = true;
    }

    return result;
}

bool IsStop(Circular_Buffer<uint8_t, 2048>& cb)
{
    bool result = false;

    uint8_t data[2];
    cb.peekBytes(data, 2);
    if (data[0] == 0x4 && data[1] == 0xC)
    {
        //cb.readBytes(data, 2); //consume these bytes
        result = true;
    }

    return result;

}

uint8_t Get7BitDeviceAddr(Circular_Buffer<uint8_t, 2048>& cb)
{
    //Purpose: Construct a 7-bit address starting from next FIFO element
    //Inputs:
    //  cb = reference to cb_trans FIFO
    //Outputs:
    //  returns the address as an 8-bit value with the MSB = 0, or 0x0 if unsuccessful
    //  the 14 transitions associated with the 7-bit address removed from FIFO
    //Plan:
    //  Step1: Convert a pair of FIFO elements into a 0 or 1
    //  Step2: Add the appropriate value to an ongoing sum
    //  Step3: return the total.
    //Notes:
    //  A '0' is coded as a 0x0 followed by a 0x4
    //  A '1' is coded as a 0x8 followed by a 0xC

    uint8_t devAddr = 0x0; //failure return value

    //devAddr is exactly 7 bits long, so 8 bits with MSB = 0
    uint8_t data[2]; //holds one transition pair
    for (size_t i = 0; i < 7; i++)
    {

        cb.readBytes(data, 2);
        //data[0] = cb.read();
        //data[1] = cb.read();
        //Serial.printf("data[0] = %x, data[1] = %x\n", data[0], data[1]);

        if (data[0] == 0x8 && data[1] == 0xC)
        {
            //Serial.printf("Get7BitDeviceAddr: '1' found at i = %d, adding %x to devAddr to get %x\n",
                //i, 1 << (7 - i), devAddr + (1 << (7 - i)));

            devAddr += (1 << (7 - i)); //add 2^(7-i) to sum
        }
    }

    devAddr = devAddr >> 1; //divide result by 2 to get 7-bit addr from 8 bits
    return devAddr;
}

int GetReadWriteFlag(Circular_Buffer<uint8_t, 2048>& cb)
{
    //Purpose: decode R/W byte pair
    //Inputs:
    //  cb = reference to cb_trans FIFO
    //Outputs:
    //  readidx = if successful, points to next byte pair in simdata
    //  returns 1 for Read (0x8/0xC), 0 for Write (0x0/0x4), -1 for failure
    //Notes:
    //  

    int result = 0;
    uint8_t data[2];
    data[0] = cb.read();
    data[1] = cb.read();
    //Serial.printf("data[0] = %x, data[1] = %x\n", data[0], data[1]);

    if (data[0] == 0x8 && data[1] == 0xC)
    {
        result = 1; //read detected
    }

    //else if (simdata[readidx] == 0x0 && simdata[readidx + 1] == 0x4)
    else if (data[0] == 0x0 && data[1] == 0x4)
    {
        result = 0; //write detected
    }
    else
    {
        result = -1; //failed to detect read or write
    }

    return result;
}

int GetACKNAKFlag(Circular_Buffer<uint8_t, 2048>& cb)
{
    //Purpose: decode ACK/NAK byte pair
    //Inputs:
    //  cb = reference to cb_trans circular buffer holding transition values
    //Outputs:
    //  returns 1 for NAK (0x8/0xC), 0 for ACK (0x0/0x4), -1 for failure
    //Notes:
    //  this code is identical to the code for GetReadWriteFlag, so use it instead 

     return GetReadWriteFlag(cb_trans);

}
int Get8BitDataByte(Circular_Buffer<uint8_t, 2048>& cb)
{
    //Purpose: Construct a 8-bit data byte starting from dataidx
    //Inputs:
    //  cb = reference to cb_trans circular buffer holding transition values
    //Outputs:
    //  returns the address as an 8-bit value, or -1 if unsuccessful
    //  dataidx = pointer to next simdata entry
    //Plan:
    //  Step1: Convert a pair of simdata entries into a 0 or 1
    //  Step2: Add the appropriate value to an ongoing sum
    //  Step3: return the total.
    //Notes:
    //  A '0' is coded as a 0x0 followed by a 0x4
    //  A '1' is coded as a 0x8 followed by a 0xC
    //  12/29/19 - changed return val to int, so can return -1 when a 'short byte' is detected

    int dataval = 0x0; 
    int numbits = 0;
    uint8_t data[2]{ 0x0,0x0 }; //holds one transition pair

////DEBUG!!
//    Serial.printf("Get8BitDataByte top: data[0] = %x,data[1] = %x, size = %d, idx = %d\n",
//        data[0], data[1], cb.size(), SIM_DATA_ARRAY_SIZE - cb.size());
//    PrintNextFIFOBytes(cb, 20);
////DEBUG!!

    //8 bits with MSB = 0

    for (size_t i = 0; i < 8; i++)
    {
        data[0] = cb.peek(0); data[1] = cb.peek(1);
        //Serial.printf("data[0] = %x, data[1] = %x\n", data[0], data[1]);

        if (data[0] == 0x0 && data[1] == 0x4)
        {
            cb.readBytes(data, 2);// now consume the bytes
            numbits++;
        }
        else if (data[0] == 0x8 && data[1] == 0xC)
        {
#ifdef GET_8BIT_DATABYTE_DEBUG
            Serial.printf("Get8BitDataByte: '1' found at i = %d, adding %x to devAddr to get %x\n",
                i, 1 << (7 - i), dataval + (1 << (7 - i)));
#endif
            cb.readBytes(data, 2);// now consume the bytes
            dataval += (1 << (7 - i)); //add 2^(8-i) to sum
            numbits++;
        }
        else //not a 1 or 0 - get me outa here!
        {
            break;
        }
    }

////DEBUG!!
//    Serial.printf("Get8BitDataByte bottom: numbits = %d, data[0] = %x,data[1] = %x, size = %d, idx = %d\n",
//       numbits, data[0], data[1], cb.size(), SIM_DATA_ARRAY_SIZE - cb.size());
//    PrintNextFIFOBytes(cb, 20);
////DEBUG!!

    if (numbits != 8)
    {
        dataval = -1; //error return value
    }

    return dataval;
}

int GetDataBytes(Circular_Buffer<uint8_t, 2048>& cb, uint8_t* databytes)
{
    //Notes:
    //  01/01/2020: removed databyteidx from sig - always starts at zero

    uint16_t numbytes = 0;
    uint16_t databyte_idx = 0;

    bool StartFlag = false;
    bool StopFlag = false;
    int dataval;

    do
    {
        dataval = Get8BitDataByte(cb);

////DEBUG!!
//        Serial.printf("GetDataBytes() just after Get8BitDataByte(): datval = %x, size = %d, idx = %d\n", 
//            dataval, cb.size(), SIM_DATA_ARRAY_SIZE-cb.size());
//        PrintNextFIFOBytes(cb, 10);
////DEBUG!!

        //watch out for 'short byte' reads
        if (dataval >= 0)
        {
            uint8_t databyte = (uint8_t)dataval;
            databyte_array[databyte_idx] = databyte;
            databyte_idx++;
            numbytes++;
            ACKNAKFlag = GetACKNAKFlag(cb_trans); //01/05/20 moved inside 'good byte' block
        }

        //ACKNAKFlag = GetACKNAKFlag(cb_trans);
////DEBUG!!
//        Serial.printf("GetDataBytes() just after GetACKNAKFlag(): ACKNAK = %d, size = %d, idx = %d\n", 
//            ACKNAKFlag, cb.size(), SIM_DATA_ARRAY_SIZE-cb.size());
//        PrintNextFIFOBytes(cb, 10);
////DEBUG!!
        StartFlag = IsStart(cb_trans);
////DEBUG!!
//        Serial.printf("GetDataBytes() just after IsStart(): StartFlag = %d, size = %d, idx = %d\n", 
//            StartFlag, cb.size(), SIM_DATA_ARRAY_SIZE-cb.size());
//        PrintNextFIFOBytes(cb, 10);
////DEBUG!!
        StopFlag = IsStop(cb_trans);

#ifdef PARSE_LOOP_DEBUG
        Serial.printf("IsStart %d, IsStop  %d, next two bytes are %x, %x, data idx = %d\n",
            StartFlag, StopFlag, cb.peek(0), cb.peek(1), SIM_DATA_ARRAY_SIZE-cb.size());
#endif

    } while (!StartFlag && !StopFlag && cb_trans.size() > 0);

    return numbytes;
}

void OutputFormattedSentence(int RW, uint8_t dev, uint8_t reg, uint8_t numbytes, uint8_t* bytearray, uint16_t startidx)
{
    Serial.printf("I2C(%x) %s %d bytes %s %x... ",
        dev, (RW == 0 ? "writing" : "reading"), numbytes - startidx, (RW == 0 ? "to" : "from"), reg);
    for (size_t i = startidx; i < numbytes; i++)
    {
        Serial.printf("%x ", bytearray[i]);
    }

    Serial.printf(". Done\n");

}

int DecodeAndPrintFIFOContents(Circular_Buffer<uint8_t, 2048>& cb)
{
    //Purpose:  decode and print I2C conversation held in cb_trans FIFO
    //Inputs:  
    //  cb = 2048 element FIFO
    //Outputs:
    //  returns number of bytes processed, or -1 for failure
    //  outputs structured I2C sentence to serial monitor
    //Plan:
    //  Step1: Determine if there is anything to do (have to have more than one transition in FIFO)
    //  Step2: Parse transitions into I2C sentence structure
    //  Step3: Output sentence to serial monitor

    if (cb.size() < 2)
    {
        return 0;
    }

    while (cb.size() > 0)
    {
#ifdef PARSE_LOOP_DEBUG
        Serial.printf("At top of while (cb.size() > 0): size = %d\n", cb.size());
        Serial.printf("Next two bytes in FIFO are %x, %x\n", cb.peek(0), cb.peek(1));
#endif
        //Find a START sequence (0xC followed by 0x4)
        while (!IsStart(cb))
        {
            Serial.printf("looking for start...\n");
        }
        cb.readBytes(killbuff,2); //01/05/20 moved START byte consume out of IsStart()


    #ifdef PARSE_LOOP_DEBUG
        Serial.printf("Start sequence found, FIFO size = %d, idx = %d\n", cb.size(), SIM_DATA_ARRAY_SIZE-cb.size());
        //PrintNextFIFOBytes(cb_trans, 20);
    #endif

        if (cb.size() > 14)//14 entries required for 7-bit address
        {
            //Get 7-bit device address
            devAddr = Get7BitDeviceAddr(cb);
            //Serial.printf("devAddr = %x, FIFO size = %d, idx = %d\n", devAddr, cb.size(), SIM_DATA_ARRAY_SIZE - cb.size());
        }

        //get read/write flag  1 = Read, 0 = Write, -1 = error
        BusDir = (BUSDIR)GetReadWriteFlag(cb);

    #ifdef PARSE_LOOP_DEBUG
        Serial.printf("BusDir = %s\n", ((BusDir == BUSDIR::WRITE) ? "WRITE" : "READ"));
        //PrintNextFIFOBytes(cb_trans, 20);
    #endif

        //get ACK/NAK flag
        ACKNAKFlag = GetACKNAKFlag(cb);

        numbytes = GetDataBytes(cb, databyte_array); //terminates on a START, but the start bytes are not consumed
    #ifdef PARSE_LOOP_DEBUG
        Serial.printf("Got %d bytes from GetDataBytes() --> ", numbytes);
        for (size_t i = 0; i < numbytes; i++)
        {
            Serial.printf(" %x ", databyte_array[i]);
        }
        Serial.printf("\n");

        //PrintNextFIFOBytes(cb_trans, 20);
    #endif
        //If the bus direction is WRITE, then extract
        //    A register address for read / write
        //    zero or more additional data bytes
        if (BusDir == BUSDIR::WRITE)
        {
            regAddr = databyte_array[0];
    #ifdef PARSE_LOOP_DEBUG
            Serial.printf("regAddr = %x, cb size = %d\n", regAddr, cb.size());
    #endif

            //check for additional data
            if (numbytes > 1)
            {
    #ifdef PARSE_LOOP_DEBUG
                Serial.printf("Additional data found!\n");
                for (size_t i = 0; i < numbytes; i++)
                {
                    Serial.printf("data[%d] = %x\n", i, databyte_array[i]);
                }
    #endif
                //1st byte is register addr, subsequent bytes are data
                OutputFormattedSentence(BusDir, devAddr, regAddr, numbytes, databyte_array, 1);
            }
        }
        else  //all bytes are data
        {
#ifdef PARSE_LOOP_DEBUG
            Serial.printf("In data block:  got %d bytes of data\n", numbytes);
            for (size_t i = 0; i < numbytes; i++)
            {
                Serial.printf("data[%d] = %x\n", i, databyte_array[i]);
            }
#endif
            OutputFormattedSentence(BusDir, devAddr, regAddr, numbytes, databyte_array, 0);
        }
    #ifdef PARSE_LOOP_DEBUG
        Serial.printf("At end of while (cb.size() > 0): size = %d\n", cb.size());
        Serial.printf("Next two bytes in FIFO are %x, %x\n", cb.peek(0), cb.peek(1));
    #endif

    }//while (cb.size() > 0)

    return 1;
}

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

189

190

191

192

193

194

195

196

197

198

199

200

201

202

203

204

205

206

207

208

209

210

211

212

213

214

215

216

217

218

219

220

221

222

223

224

225

226

227

228

229

230

231

232

233

234

235

236

237

238

239

240

241

242

243

244

245

246

247

248

249

250

251

252

253

254

255

256

257

258

259

260

261

262

263

264

265

266

267

268

269

270

271

272

273

274

275

276

277

278

279

280

281

282

283

284

285

286

287

288

289

290

291

292

293

294

295

296

297

298

299

300

301

302

303

304

305

306

307

308

309

310

311

312

313

314

315

316

317

318

319

320

321

322

323

324

325

326

327

328

329

330

331

332

333

334

335

336

337

338

339

340

341

342

343

344

345

346

347

348

349

350

351

352

353

354

355

356

357

358

359

360

361

362

363

364

365

366

367

368

369

370

371

372

373

374

375

376

377

378

379

380

381

382

383

384

385

386

387

388

389

390

391

392

393

394

395

396

397

398

399

400

401

402

403

404

405

406

407

408

409

410

411

412

413

414

415

416

417

418

419

420

421

422

423

424

425

426

427

428

429

430

431

432

433

434

435

436

437

438

439

440

441

442

443

444

445

446

447

448

449

450

451

452

453

454

455

456

457

458

459

460

461

462

463

464

465

466

467

468

469

470

471

472

473

474

475

476

477

478

479

480

481

482

483

484

485

486

487

488

489

490

491

492

493

494

495

496

497

498

499

500

501

502

503

504

505

506

507

508

509

510

511

512

513

514

515

516

517

518

519

520

521

522

523

524

525

526

527

528

529

530

531

532

533

534

535

536

537

538

539

540

541

542

543

544

545

546

547

548

549

550

551

552

553

554

555

556

557

558

559

560

561

562

563

564

565

566

567

568

569

570

571

572

573

574

575

576

577

578

579

580

581

582

583

584

585

586

587

588

589

590

591

592

593

594

595

596

597

598

599

600

601

602

603

604

605

606

607

Name: Teensy_I2C_Sniffer_V6.ino

Created: 1/4/2020 8:40:20 AM

Author: FRANKNEWXPS15\Frank

This is a port of my Excel VBA code into Arduino/Teensy C++ language.

It parses the exact same sample data as the Excel program, so it should

produce the exact same I2C sequences.

The captured sample data to be parsed is held in the 'simdata' array.

The original VBA code is saved in 'ExcelVBACode.txt'

/* 'Notes:

A typical I2C sentence when communicating with a MPU6050 IMU module goes like:

"I2C(68) wrote 1 byte to 75 - C0 Done."

"I2C(68) wrote 3 bytes to 72 - C0 0C 10 Done."

"I2C(68) read 5 bytes from 6A - C0 0C 10 14 03 Done."

To form a sentence, we need:

Device addr: 68 in the above examples

Read/Write direction

To/From register address: 75, 72 and 6A in the above examples

Data: C0, C0 0C 10, and C0 0C 10 14 03 in the above examples

number of bytes written/read: 1,3 & 5 in the above examples

Each I2C communication proceeds as follows (assuming a START from an IDLE condition):

A START or RESTART condition, denoted by SDA & SCL HIGH, followed by SDA LOW, SCL HIGH

A 7-bit device address, MSB first (0x8/0xC = 1, 0x0/0x4 = 0)

A R/W bit (0x8/0xC = read, 0x0/0x4 = write)

An ACK bit (0x8/0xC = NAK, 0x0/0x4 = ACK)

If the bus direction is WRITE, then

A register address for read/write

zero or more additional data bytes

Else (the bus direction is READ)

One or more additional data bytes

Endif

This version adds tonton81's circular buffer class (https://github.com/tonton81/Circular_Buffer)

to simulate real-time processing and display of I2C bus activity. The sample data is loaded into

a FIFO by an ISR triggered every microsecond by a timer, and then the processing code pulls it out

and parses it as fast as possible. If this works, then the next version will eliminate the sample

data and connect to a live I2C bus.

#include <TimerOne.h> //needed for ISR

#include "circular_buffer.h"

Circular_Buffer<uint8_t, 2048> cb_trans; //holds transition values from ISR