diff --git a/libraries/AP_HAL/I2CDriver.h b/libraries/AP_HAL/I2CDriver.h index d5c23bd71c..720f2b849b 100644 --- a/libraries/AP_HAL/I2CDriver.h +++ b/libraries/AP_HAL/I2CDriver.h @@ -2,12 +2,26 @@ #ifndef __AP_HAL_I2C_DRIVER_H__ #define __AP_HAL_I2C_DRIVER_H__ +#include + #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__ diff --git a/libraries/AP_HAL_AVR/I2CDriver.cpp b/libraries/AP_HAL_AVR/I2CDriver.cpp new file mode 100644 index 0000000000..d22463a18b --- /dev/null +++ b/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 + +#include +#include + +#include +#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; + } +} + + diff --git a/libraries/AP_HAL_AVR/I2CDriver.h b/libraries/AP_HAL_AVR/I2CDriver.h index 840a1947a7..2239017b42 100644 --- a/libraries/AP_HAL_AVR/I2CDriver.h +++ b/libraries/AP_HAL_AVR/I2CDriver.h @@ -5,12 +5,46 @@ #include #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__ diff --git a/libraries/AP_HAL_AVR/examples/I2CDriver_HMC5883L/Arduino.h b/libraries/AP_HAL_AVR/examples/I2CDriver_HMC5883L/Arduino.h new file mode 100644 index 0000000000..e69de29bb2 diff --git a/libraries/AP_HAL_AVR/examples/I2CDriver_HMC5883L/I2CDriver_HMC5883L.pde b/libraries/AP_HAL_AVR/examples/I2CDriver_HMC5883L/I2CDriver_HMC5883L.pde new file mode 100644 index 0000000000..efbffddad5 --- /dev/null +++ b/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 +#include +#include + +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; +} +} diff --git a/libraries/AP_HAL_AVR/examples/I2CDriver_HMC5883L/Makefile b/libraries/AP_HAL_AVR/examples/I2CDriver_HMC5883L/Makefile new file mode 100644 index 0000000000..d1f40fd90f --- /dev/null +++ b/libraries/AP_HAL_AVR/examples/I2CDriver_HMC5883L/Makefile @@ -0,0 +1 @@ +include ../../../AP_Common/Arduino.mk diff --git a/libraries/AP_HAL_AVR/examples/I2CDriver_HMC5883L/nocore.inoflag b/libraries/AP_HAL_AVR/examples/I2CDriver_HMC5883L/nocore.inoflag new file mode 100644 index 0000000000..e69de29bb2