AP_HAL_AVR: Implement I2CDriver based on libraries/I2C

* Removed a ton of code we don't need from that driver, which should make
  writing new drivers easier.

libraries/AP_HAL/I2CDriver.h

@@ -2,12 +2,26 @@
 #ifndef __AP_HAL_I2C_DRIVER_H__
 #define __AP_HAL_I2C_DRIVER_H__
 
+#include <stdint.h>
+
 #include "AP_HAL_Namespace.h"
 
 class AP_HAL::I2CDriver {
 public:
     I2CDriver() {}
-    virtual void init() = 0;
+    virtual void begin() = 0;
+    virtual void end() = 0;
+    virtual void setTimeout(uint16_t ms) = 0;
+    virtual void setHighSpeed(bool active) = 0;
+
+    virtual uint8_t writeRegister(uint8_t addr, uint8_t reg, uint8_t val) = 0;
+    virtual uint8_t writeRegisters(uint8_t addr, uint8_t reg,
+                                   uint8_t len, uint8_t* data) = 0;
+    virtual uint8_t readRegister(uint8_t addr, uint8_t reg, uint8_t* data) = 0;
+    virtual uint8_t readRegisters(uint8_t addr, uint8_t reg,
+                                  uint8_t len, uint8_t* data) = 0;
+
+    virtual uint8_t lockup_count();
 };
 
 #endif // __AP_HAL_I2C_DRIVER_H__

libraries/AP_HAL_AVR/I2CDriver.cpp

@@ -0,0 +1,247 @@
+/*
+ *  AP_HAL_AVR I2C driver. derived from:
+ *  I2C.cpp - I2C library
+ *  Copyright (c) 2011 Wayne Truchsess.  All right reserved.
+ *  Rev 2.0 - September 19th, 2011
+ *         - Added support for timeout function to prevent
+ *           and recover from bus lockup (thanks to PaulS
+ *           and CrossRoads on the Arduino forum)
+ *         - Changed return type for stop() from void to
+ *           uint8_t to handle timeOut function
+ *  Rev 1.0 - August 8th, 2011
+ *
+ *  This is a modified version of the Arduino Wire/TWI
+ *  library.  Functions were rewritten to provide more functionality
+ *  and also the use of Repeated Start.  Some I2C devices will not
+ *  function correctly without the use of a Repeated Start.  The
+ *  initial version of this library only supports the Master.
+ *
+ *  This library is free software; you can redistribute it and/or
+ *  modify it under the terms of the GNU Lesser General Public
+ *  License as published by the Free Software Foundation; either
+ *  version 2.1 of the License, or (at your option) any later version.
+ *
+ *  This library is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ *  Lesser General Public License for more details.
+ *
+ *  You should have received a copy of the GNU Lesser General Public
+ *  License along with this library; if not, write to the Free Software
+ *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ */
+
+#include <inttypes.h>
+
+#include <avr/io.h>
+#include <avr/interrupt.h>
+
+#include <AP_HAL.h>
+#include "I2CDriver.h"
+using namespace AP_HAL_AVR;
+
+extern const AP_HAL::HAL& hal;
+
+#ifndef F_CPU
+#define CPU_FREQ 16000000L
+#else
+#define CPU_FREQ F_CPU
+#endif
+
+#define START           0x08
+#define REPEATED_START  0x10
+#define MT_SLA_ACK      0x18
+#define MT_DATA_ACK     0x28
+#define MR_SLA_ACK      0x40
+#define MR_DATA_ACK     0x50
+#define MR_DATA_NACK    0x58
+#define TWI_STATUS      (TWSR & 0xF8)
+
+#define SLA_W(address)  (address << 1)
+#define SLA_R(address)  ((address << 1) + 0x01)
+
+#define cbi(sfr, bit)   (_SFR_BYTE(sfr) &= ~_BV(bit))
+#define sbi(sfr, bit)   (_SFR_BYTE(sfr) |= _BV(bit))
+
+void AVRI2CDriver::begin() {
+    // activate internal pull-ups for twi
+    // as per note from atmega128 manual pg204
+    sbi(PORTD, 0);
+    sbi(PORTD, 1);
+
+    // initialize twi prescaler and bit rate
+    cbi(TWSR, TWPS0);
+    cbi(TWSR, TWPS1);
+    TWBR = ((CPU_FREQ / 100000) - 16) / 2;
+    // enable twi module, acks, and twi interrupt
+    TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA);
+}
+
+void AVRI2CDriver::end() {
+    TWCR = 0;
+}
+ 
+void AVRI2CDriver::setHighSpeed(bool active) {
+    if (active) {
+        TWBR = ((CPU_FREQ / 400000) - 16) / 2;
+    } else {
+        TWBR = ((CPU_FREQ / 100000) - 16) / 2;
+    }
+}
+
+uint8_t AVRI2CDriver::writeRegisters(uint8_t addr, uint8_t reg,
+                                    uint8_t len, uint8_t* data){
+    uint8_t stat = _start();
+    if (stat) return stat;
+    stat = _sendAddress(SLA_W(addr));
+    if (stat) return stat;
+    stat = _sendByte(reg);
+    if (stat) return stat;
+    for (uint8_t i = 0; i < len; i++)
+    {
+        stat = _sendByte(data[i]);
+        if (stat) return stat;
+    }
+    stat = _stop();
+    return stat;
+}
+
+uint8_t AVRI2CDriver::readRegisters(uint8_t addr, uint8_t reg,
+                                    uint8_t len, uint8_t* data){
+    uint8_t stat;
+    if ( len == 0)
+        len = 1;
+    uint8_t nackposition = len - 1;
+    stat = 0;
+    stat = _start();
+    if(stat) return stat;
+    stat = _sendAddress(SLA_W(addr));
+    if(stat) return stat;
+    stat = _sendByte(reg);
+    if(stat) return stat;
+    stat = _start();
+    if(stat) return stat;
+    stat = _sendAddress(SLA_R(addr));
+    if(stat) return stat;
+    for(uint8_t i = 0; i < len ; i++) {
+        if ( i == nackposition ) {
+            stat = _receiveByte(false);
+            if (stat != MR_DATA_NACK) return stat; 
+        } else {
+            stat = _receiveByte(true);
+            if (stat != MR_DATA_ACK) return stat;
+        }
+        data[i] = TWDR;
+    }
+    stat = _stop();
+    return stat;
+}
+
+uint8_t AVRI2CDriver::_start() {
+    TWCR = _BV(TWINT) | _BV(TWSTA) | _BV(TWEN);
+    uint8_t stat = _waitInterrupt();
+    if (stat) return stat;
+
+    if ((TWI_STATUS == START) || (TWI_STATUS == REPEATED_START)) {
+        return 0;
+    } else {
+        return TWI_STATUS;
+    }
+}
+
+uint8_t AVRI2CDriver::_stop() {
+    TWCR = _BV(TWINT) | _BV(TWEN) | _BV(TWSTO);
+    return _waitStop();
+}
+
+uint8_t AVRI2CDriver::_sendAddress(uint8_t addr) {
+    TWDR = addr;
+    TWCR = _BV(TWINT) | _BV(TWEN);
+    return _waitInterrupt();
+}
+
+uint8_t AVRI2CDriver::_sendByte(uint8_t data) {
+    TWDR = data;
+    TWCR = _BV(TWINT) | _BV(TWEN);
+    uint8_t stat = _waitInterrupt();
+    if (stat) return stat;
+
+    if (TWI_STATUS  == MT_DATA_ACK) {
+        return 0;
+    } else {
+        return TWI_STATUS;
+    }
+}
+
+uint8_t AVRI2CDriver::_receiveByte(bool ack) {
+    if (ack) {
+        TWCR = _BV(TWINT) | _BV(TWEN) | _BV(TWEA);
+    } else {
+        TWCR = _BV(TWINT) | _BV(TWEN);
+    }
+    uint8_t stat = _waitInterrupt();
+    if (stat) return stat;
+    return TWI_STATUS;
+}
+
+void AVRI2CDriver::_handleLockup() {
+    TWCR = 0; /* Releases SDA and SCL lines to high impedance */
+    TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA); /* Reinitialize TWI */
+    _lockup_count++;
+}
+
+uint8_t AVRI2CDriver::_waitInterrupt() {
+    uint32_t start = hal.scheduler->millis();
+    if (_timeoutDelay == 0) {
+        /* Wait indefinitely for interrupt to go off */
+        while (!(TWCR & _BV(TWINT))) { }
+    } else {
+        /* Wait while polling for timeout */
+        while (!(TWCR & _BV(TWINT))) {
+            uint32_t current = hal.scheduler->millis();
+            if ( current - start >= _timeoutDelay ) {
+                _handleLockup();
+                return 1;
+            }
+        }
+    }
+    return 0;
+}
+
+uint8_t AVRI2CDriver::_waitStop() {
+    uint32_t start = hal.scheduler->millis();
+    if (_timeoutDelay == 0) {
+        /* Wait indefinitely for stop condition */
+        while( TWCR & _BV(TWSTO) ) { }
+    } else  {
+        /* Wait while polling for timeout */
+        while( TWCR & _BV(TWSTO) ) {
+            uint32_t current = hal.scheduler->millis();
+            if (current - start >= _timeoutDelay) {
+                _handleLockup(); 
+                return 1;
+            }
+        }
+    }
+    return 0;
+}
+
+SIGNAL(TWI_vect)
+{
+    switch(TWI_STATUS) {
+    case 0x20:
+    case 0x30:
+    case 0x48:
+        TWCR = _BV(TWINT) | _BV(TWEN) | _BV(TWSTO);  // send a stop
+        break;
+    case 0x38:
+    case 0x68:
+    case 0x78:
+    case 0xB0:
+        TWCR = 0;  //releases SDA and SCL lines to high impedance
+        TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA);  //reinitialize TWI
+        break;
+    }
+}
+
+

libraries/AP_HAL_AVR/I2CDriver.h

@@ -5,12 +5,46 @@
 #include <AP_HAL.h>
 #include "AP_HAL_AVR_Namespace.h"
 
+#define AVRI2CDRIVER_MAX_BUFFER_SIZE 32
+
 class AP_HAL_AVR::AVRI2CDriver : public AP_HAL::I2CDriver {
 public:
-    AVRI2CDriver(): _init(0) {}
-    void init() { _init = 1; } 
+    AVRI2CDriver() {}
+
+    void begin();
+    void end();
+    void setTimeout(uint16_t ms) { _timeoutDelay = ms; }
+    void setHighSpeed(bool active);
+
+    uint8_t writeRegister(uint8_t addr, uint8_t reg, uint8_t val) {
+        /* Sometimes avr-gcc fails at dereferencing a uint8_t arg. */
+        uint8_t data[1];
+        data[0] = val;
+        return writeRegisters(addr, reg, 1, data);
+    }
+
+    uint8_t writeRegisters(uint8_t addr, uint8_t reg,
+                           uint8_t len, uint8_t* data);
+    uint8_t readRegister(uint8_t addr, uint8_t reg, uint8_t* data) {
+        return readRegisters(addr, reg, 1, data);
+    }
+    uint8_t readRegisters(uint8_t addr, uint8_t reg,
+                          uint8_t len, uint8_t* data);
+    uint8_t lockup_count() { return _lockup_count; }
+
 private:
-    int _init;
+    uint8_t _start();
+    uint8_t _stop();
+    uint8_t _sendAddress(uint8_t addr);
+    uint8_t _sendByte(uint8_t data);
+    uint8_t _receiveByte(bool ack);
+    void    _handleLockup();
+
+    uint8_t _waitInterrupt();
+    uint8_t _waitStop();
+    
+    uint8_t _lockup_count;
+    uint16_t _timeoutDelay;
 };
 
 #endif // __AP_HAL_AVR_I2C_DRIVER_H__

libraries/AP_HAL_AVR/examples/I2CDriver_HMC5883L/I2CDriver_HMC5883L.pde

@@ -0,0 +1,61 @@
+/*******************************************
+*   Sample sketch that configures an HMC5883L 3 axis
+*   magnetometer to continuous mode and reads back
+*   the three axis of data.
+*******************************************/
+
+#include <AP_Common.h>
+#include <AP_HAL.h>
+#include <AP_HAL_AVR.h>
+
+const AP_HAL_AVR::HAL_AVR& hal = AP_HAL_AVR_APM2;
+
+#define HMC5883L  0x1E
+
+void setup() {
+    hal.uart0->begin(115200);
+    hal.uart0->printf_P(PSTR("Initializing HMC5883L at address %x\r\n"),
+                                HMC5883L);
+
+    // configure device for continuous mode
+    hal.i2c->begin();
+    hal.i2c->setTimeout(100);
+
+    uint8_t stat = hal.i2c->writeRegister(HMC5883L,0x02,0x00);
+    if (stat == 0) {
+        hal.uart0->printf_P(PSTR("successful init\r\n"));
+    } else {
+        hal.uart0->printf_P(PSTR("failed init: return status %d\r\n"),
+                (int)stat);
+        for(;;);
+    }
+}
+
+void loop() {
+    uint8_t data[6];
+    //read 6 bytes (x,y,z) from the device
+    uint8_t stat = hal.i2c->readRegisters(HMC5883L,0x03,6, data);
+
+    if (stat == 0){
+        int x, y, z;
+        x = data[0] << 8;
+        x |= data[1];
+        y = data[2] << 8;
+        y |= data[3];
+        z = data[4] << 8;
+        z |= data[5];
+        hal.uart0->printf_P(PSTR("x: %d y: %d z: %d \r\n"), x, y, z);
+    } else { 
+        hal.uart0->printf_P(PSTR("i2c error: status %d\r\n"), (int)stat);
+    }
+}
+
+
+extern "C" {
+int main (void) {
+    hal.init(NULL);
+    setup();
+    for(;;) loop();
+    return 0;
+}
+}

libraries/AP_HAL_AVR/examples/I2CDriver_HMC5883L/Makefile

@@ -0,0 +1 @@
+include ../../../AP_Common/Arduino.mk