Teensy 3.2 as I2C slave, emulating PCA9685 PWM driver

[jwatte]

I'm using a Raspberry Pi with Adafruit_PCA9685 library to talk to the I2C bus.
I'm using 2 kOhm pull-ups.
I'm using a Teensy3.2 to emulate the PCA9685 PWM controller part, by forwarding the first two channels of this part to two Servo instances.

The Raspberry Pi occasionally generates I/O Errors:

Traceback (most recent call last):
  File "test_pca.py", line 13, in <module>
    pwm.set_pwm(0, 0, i)
  File "build/bdist.linux-armv7l/egg/Adafruit_PCA9685/PCA9685.py", line 107, in set_pwm
  File "build/bdist.linux-armv7l/egg/Adafruit_GPIO/I2C.py", line 115, in write8
  File "build/bdist.linux-armv7l/egg/Adafruit_PureIO/smbus.py", line 236, in write_byte_data
IOError: [Errno 121] Remote I/O error

import time
import Adafruit_PCA9685 as P

pwm = P.PCA9685()
pwm.set_pwm_freq(60)

while True:
  for i in range(300, 400):
    pwm.set_pwm(0, 0, i)
    pwm.set_pwm(1, 0, 380)
    time.sleep(0.05)
  for i in range(400, 300, -1):
    pwm.set_pwm(0, 0, i)
    pwm.set_pwm(1, 0, 390)
    time.sleep(0.05)

Looking at the bus with a scope, it seems as if someone (Pi or Teensy) is holding the SDA signal of the bus low for several milliseconds, and this causes the I/O error.

What can cause this error? How can I fix it?

Here's my sketch and PCAEmulation files:

#include "PCA9685Emulator.h"
#include <Servo.h>

PCA9685Emulator pwmEmulation;

Servo svoSteer;
Servo svoThrottle;

void setup() {
  pinMode(PIN_STEER_OUT, OUTPUT);
  svoSteer.attach(PIN_STEER_OUT);
  pinMode(PIN_THROTTLE_OUT, OUTPUT);
  svoThrottle.attach(PIN_THROTTLE_OUT);
  pwmEmulation.begin(PCA9685_I2C_ADDRESS);
}

void loop() {
  pwmEmulation.step(millis());
  svoSteer.writeMicroseconds(pwmEmulation.readChannelUs(0));
  svoThrottle.writeMicroseconds(pwmEmulation.readChannelUs(1));
}

#define PCA9685_I2C_ADDRESS 0x40

class PCA9685Emulator {
public:
  PCA9685Emulator();
  void begin(uint8_t address);
  //  Return true if there are new values
  bool step(uint32_t now);
  //  Return servo PWM value in microseconds
  uint16_t readChannelUs(uint16_t ch);

  void onRequest2();
  void onReceive2(int n);
  
  static void onRequest();
  static void onReceive(int n);
  static PCA9685Emulator *active; //  only one can be active at a time

  enum {
    NUM_CHANNELS = 16
  };
  uint8_t mem[6+NUM_CHANNELS*4];
  uint8_t wptr;
  bool gotwrite;
};

#include <Arduino.h>
#include <Wire.h>
#include "PCA9685Emulator.h"


static const uint8_t MODE1              = 0x00;
static const uint8_t MODE2              = 0x01;
static const uint8_t SUBADR1            = 0x02;
static const uint8_t SUBADR2            = 0x03;
static const uint8_t SUBADR3            = 0x04;
static const uint8_t FAKE_PRESCALE      = 0x05;
static const uint8_t LED0_ON_L          = 0x06;
static const uint8_t LED0_ON_H          = 0x07;
static const uint8_t LED0_OFF_L         = 0x08;
static const uint8_t LED0_OFF_H         = 0x09;
static const uint8_t ALL_LED_ON_L       = 0xFA;
static const uint8_t ALL_LED_ON_H       = 0xFB;
static const uint8_t ALL_LED_OFF_L      = 0xFC;
static const uint8_t ALL_LED_OFF_H      = 0xFD;
static const uint8_t PRESCALE           = 0xFE;

static const uint8_t SWRESET            = 0x06;

static const uint8_t RESTART            = 0x80;
static const uint8_t SLEEP              = 0x10;
static const uint8_t ALLCALL            = 0x01;
static const uint8_t INVRT              = 0x10;
static const uint8_t OUTDRV             = 0x04;


PCA9685Emulator *PCA9685Emulator::active;

PCA9685Emulator::PCA9685Emulator()
{
  wptr = 0;
  gotwrite = false;
}

void PCA9685Emulator::begin(uint8_t address)
{
  active = this;
  Wire.begin(address);
  Wire.onRequest(onRequest);
  Wire.onReceive(onReceive);
}

bool PCA9685Emulator::step(uint32_t now)
{
  bool ret;
  cli();
  ret = gotwrite;
  gotwrite = false;
  sei();
  return ret;
}

uint16_t PCA9685Emulator::readChannelUs(uint16_t ch) {
  if (ch >= NUM_CHANNELS) {
    return 0xffff;
  }
  uint16_t ret;
  cli();
  if ((mem[MODE1] & SLEEP) || !(mem[MODE2] & OUTDRV)) {
    ret = 0;
    sei();
  } else {
    uint32_t l_on_0 = mem[LED0_ON_L+4*ch];
    uint32_t h_on_0 = mem[LED0_ON_H+4*ch];
    uint32_t l_off_0 = mem[LED0_OFF_L+4*ch];
    uint32_t h_off_0 = mem[LED0_OFF_H+4*ch];
    uint32_t ps = mem[FAKE_PRESCALE] + 1;
    sei();
    ret = ((l_off_0 + (h_off_0 << 8)) - (l_on_0 + (h_on_0 << 8))) * ps / 25ul;
  }
  return ret;
}

void PCA9685Emulator::onRequest()
{
  active->onRequest2();
}

void PCA9685Emulator::onRequest2()
{
  //  only support returning a single byte
  if (wptr < sizeof(mem)) {
    Wire.send(mem[wptr&0xf]);
  } else {
    Wire.send(0xff);
  }
}

void PCA9685Emulator::onReceive(int received)
{
  active->onReceive2(received);
}

void PCA9685Emulator::onReceive2(int received)
{
  //  Empty the pipe within the interrupt request, to 
  //  make sure I don't race with the step() function.
  if (received != 0) {
    if (received == 1) {
      //  a command
      wptr = Wire.read();
      if (wptr == SWRESET) {
        //  software reset -- I don't care?
      }
    } else {
      //  a write
      gotwrite = true;
      wptr = Wire.read();
      --received;
      while (received > 0) {
        unsigned char rb = Wire.read();
        if (wptr < sizeof(mem)) {
          mem[wptr] = rb;
        } else if (wptr == PRESCALE) {
          mem[FAKE_PRESCALE] = rb;
        } else {
          (void)rb;
        }
        ++wptr;
        --received;
      }
    }
  }
}