diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 7ecf42bbbb..6db7642f05 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -54,7 +54,7 @@ http://code.google.com/p/ardupilot-mega/downloads/list #include #include // ArduPilot Mega RC Library #include // ArduPilot GPS library -#include // Arduino I2C lib +#include // Arduino I2C lib #include // Arduino SPI lib #include // ArduPilot Mega Flash Memory Library #include // ArduPilot Mega Analog to Digital Converter Library @@ -703,9 +703,10 @@ static void medium_loop() #if HIL_MODE != HIL_MODE_ATTITUDE // don't execute in HIL mode if(g.compass_enabled){ - compass.read(); // Read magnetometer - compass.calculate(dcm.get_dcm_matrix()); // Calculate heading - compass.null_offsets(dcm.get_dcm_matrix()); + if (compass.read()) { + compass.calculate(dcm.get_dcm_matrix()); // Calculate heading + compass.null_offsets(dcm.get_dcm_matrix()); + } } #endif diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 692be9c537..a1ed9a528d 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -52,6 +52,7 @@ # define CONFIG_RELAY DISABLED # define MAG_ORIENTATION AP_COMPASS_APM2_SHIELD # define CONFIG_SONAR_SOURCE SONAR_SOURCE_ANALOG_PIN +# define MAGNETOMETER ENABLED # ifdef APM2_BETA_HARDWARE # define CONFIG_BARO AP_BARO_BMP085 # else // APM2 Production Hardware (default) diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index 02de32f048..2ef482272e 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -107,7 +107,8 @@ static void init_ardupilot() // Initialize Wire and SPI libraries // #ifndef DESKTOP_BUILD - Wire.begin(); + I2c.begin(); + I2c.timeOut(20); #endif SPI.begin(); SPI.setClockDivider(SPI_CLOCK_DIV16); // 1MHZ SPI rate diff --git a/ArduCopter/test.pde b/ArduCopter/test.pde index 4a63b5dcbe..dc37cf23f4 100644 --- a/ArduCopter/test.pde +++ b/ArduCopter/test.pde @@ -903,14 +903,17 @@ test_mag(uint8_t argc, const Menu::arg *argv) while(1){ delay(100); - compass.read(); - compass.calculate(dcm.get_dcm_matrix()); - Vector3f maggy = compass.get_offsets(); - Serial.printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d\n"), - (wrap_360(ToDeg(compass.heading) * 100)) /100, - compass.mag_x, - compass.mag_y, - compass.mag_z); + if (compass.read()) { + compass.calculate(dcm.get_dcm_matrix()); + Vector3f maggy = compass.get_offsets(); + Serial.printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d\n"), + (wrap_360(ToDeg(compass.heading) * 100)) /100, + compass.mag_x, + compass.mag_y, + compass.mag_z); + } else { + Serial.println_P(PSTR("not healthy")); + } if(Serial.available() > 0){ return (0); diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index e36be9d7ed..f81c1efe4e 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -29,7 +29,7 @@ version 2.1 of the License, or (at your option) any later version. #include #include // ArduPilot Mega RC Library #include // ArduPilot GPS library -#include // Arduino I2C lib +#include // Wayne Truchsess I2C lib #include // Arduino SPI lib #include // ArduPilot Mega Flash Memory Library #include // ArduPilot Mega Analog to Digital Converter Library @@ -621,11 +621,10 @@ static void medium_loop() } #if HIL_MODE != HIL_MODE_ATTITUDE - if(g.compass_enabled){ - compass.read(); // Read magnetometer - compass.calculate(dcm.get_dcm_matrix()); // Calculate heading - compass.null_offsets(dcm.get_dcm_matrix()); - } + if (g.compass_enabled && compass.read()) { + compass.calculate(dcm.get_dcm_matrix()); // Calculate heading + compass.null_offsets(dcm.get_dcm_matrix()); + } #endif /*{ Serial.print(dcm.roll_sensor, DEC); Serial.printf_P(PSTR("\t")); @@ -979,8 +978,12 @@ static void update_alt() // this function is in place to potentially add a sonar sensor in the future //altitude_sensor = BARO; - current_loc.alt = (1 - g.altitude_mix) * g_gps->altitude; // alt_MSL centimeters (meters * 100) - current_loc.alt += g.altitude_mix * (read_barometer() + home.alt); + if (barometer.healthy) { + current_loc.alt = (1 - g.altitude_mix) * g_gps->altitude; // alt_MSL centimeters (meters * 100) + current_loc.alt += g.altitude_mix * (read_barometer() + home.alt); + } else if (g_gps->fix) { + current_loc.alt = g_gps->altitude; // alt_MSL centimeters (meters * 100) + } #endif geofence_check(true); diff --git a/ArduPlane/geofence.pde b/ArduPlane/geofence.pde index 088ce6bb60..41223ae1e5 100644 --- a/ArduPlane/geofence.pde +++ b/ArduPlane/geofence.pde @@ -235,13 +235,12 @@ static void geofence_check(bool altitude_check_only) } // we are outside the fence - if (geofence_state->fence_triggered) { + if (geofence_state->fence_triggered && control_mode == GUIDED) { // we have already triggered, don't trigger again until the // user disables/re-enables using the fence channel switch return; } - // we are outside, and have not previously triggered. geofence_state->fence_triggered = true; geofence_state->breach_count++; diff --git a/ArduPlane/sensors.pde b/ArduPlane/sensors.pde index b144db8b4f..cb96f16e2b 100644 --- a/ArduPlane/sensors.pde +++ b/ArduPlane/sensors.pde @@ -11,7 +11,7 @@ static void init_barometer(void) long ground_pressure = 0; int ground_temperature; - while(ground_pressure == 0){ + while (ground_pressure == 0 || !barometer.healthy) { barometer.read(); // Get initial data from absolute pressure sensor ground_pressure = barometer.get_pressure(); ground_temperature = barometer.get_temperature(); @@ -22,10 +22,12 @@ static void init_barometer(void) for(int i = 0; i < 30; i++){ // We take some readings... #if HIL_MODE == HIL_MODE_SENSORS - gcs_update(); // look for inbound hil packets + gcs_update(); // look for inbound hil packets #endif - - barometer.read(); // Get initial data from absolute pressure sensor + + do { + barometer.read(); // Get pressure sensor + } while (!barometer.healthy); ground_pressure = (ground_pressure * 9l + barometer.get_pressure()) / 10l; ground_temperature = (ground_temperature * 9 + barometer.get_temperature()) / 10; @@ -59,7 +61,6 @@ static long read_barometer(void) barometer.read(); // Get new data from absolute pressure sensor - //abs_pressure = (abs_pressure + barometer.get_pressure()) >> 1; // Small filtering abs_pressure = ((float)abs_pressure * .7) + ((float)barometer.get_pressure() * .3); // large filtering scaling = (float)g.ground_pressure / (float)abs_pressure; diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde index e6513cfe8a..b3a30b7a5e 100644 --- a/ArduPlane/system.pde +++ b/ArduPlane/system.pde @@ -107,7 +107,8 @@ static void init_ardupilot() // Initialize Wire and SPI libraries // #ifndef DESKTOP_BUILD - Wire.begin(); + I2c.begin(); + I2c.timeOut(20); #endif SPI.begin(); SPI.setClockDivider(SPI_CLOCK_DIV16); // 1MHZ SPI rate diff --git a/ArduPlane/test.pde b/ArduPlane/test.pde index 11e1fc0a34..f55604454e 100644 --- a/ArduPlane/test.pde +++ b/ArduPlane/test.pde @@ -547,9 +547,10 @@ test_imu(uint8_t argc, const Menu::arg *argv) if(g.compass_enabled) { medium_loopCounter++; if(medium_loopCounter == 5){ - compass.read(); // Read magnetometer - compass.calculate(dcm.get_dcm_matrix()); // Calculate heading - medium_loopCounter = 0; + if (compass.read()) { + compass.calculate(dcm.get_dcm_matrix()); // Calculate heading + } + medium_loopCounter = 0; } } @@ -610,23 +611,28 @@ test_mag(uint8_t argc, const Menu::arg *argv) medium_loopCounter++; if(medium_loopCounter == 5){ - compass.read(); // Read magnetometer - compass.calculate(dcm.get_dcm_matrix()); // Calculate heading - compass.null_offsets(dcm.get_dcm_matrix()); + if (compass.read()) { + compass.calculate(dcm.get_dcm_matrix()); // Calculate heading + compass.null_offsets(dcm.get_dcm_matrix()); + } medium_loopCounter = 0; } counter++; if (counter>20) { - Vector3f maggy = compass.get_offsets(); - Serial.printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d,\tXYZoff: %6.2f, %6.2f, %6.2f\n"), - (wrap_360(ToDeg(compass.heading) * 100)) /100, - compass.mag_x, - compass.mag_y, - compass.mag_z, - maggy.x, - maggy.y, - maggy.z); + if (compass.healthy) { + Vector3f maggy = compass.get_offsets(); + Serial.printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d,\tXYZoff: %6.2f, %6.2f, %6.2f\n"), + (wrap_360(ToDeg(compass.heading) * 100)) /100, + compass.mag_x, + compass.mag_y, + compass.mag_z, + maggy.x, + maggy.y, + maggy.z); + } else { + Serial.println_P(PSTR("compass not healthy")); + } counter=0; } } @@ -694,9 +700,13 @@ test_pressure(uint8_t argc, const Menu::arg *argv) delay(100); current_loc.alt = read_barometer() + home.alt; - Serial.printf_P(PSTR("Alt: %0.2fm, Raw: %ld Temperature: %.1f\n"), - current_loc.alt / 100.0, - abs_pressure, 0.1*barometer.get_temperature()); + if (!barometer.healthy) { + Serial.println_P(PSTR("not healthy")); + } else { + Serial.printf_P(PSTR("Alt: %0.2fm, Raw: %ld Temperature: %.1f\n"), + current_loc.alt / 100.0, + abs_pressure, 0.1*barometer.get_temperature()); + } if(Serial.available() > 0){ return (0); diff --git a/libraries/AP_Baro/AP_Baro.h b/libraries/AP_Baro/AP_Baro.h index 871ab3a8bc..49b4b00075 100644 --- a/libraries/AP_Baro/AP_Baro.h +++ b/libraries/AP_Baro/AP_Baro.h @@ -1,3 +1,4 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- #ifndef __AP_BARO_H__ #define __AP_BARO_H__ @@ -7,15 +8,16 @@ class AP_Baro { public: - AP_Baro() {} - virtual void init(AP_PeriodicProcess *scheduler)=0; - virtual uint8_t read() = 0; - virtual int32_t get_pressure() = 0; - virtual int16_t get_temperature() = 0; - virtual float get_altitude() = 0; - - virtual int32_t get_raw_pressure() = 0; - virtual int32_t get_raw_temp() = 0; + bool healthy; + AP_Baro() {} + virtual bool init(AP_PeriodicProcess *scheduler)=0; + virtual uint8_t read() = 0; + virtual int32_t get_pressure() = 0; + virtual int16_t get_temperature() = 0; + virtual float get_altitude() = 0; + + virtual int32_t get_raw_pressure() = 0; + virtual int32_t get_raw_temp() = 0; }; #include "AP_Baro_MS5611.h" diff --git a/libraries/AP_Baro/AP_Baro_BMP085.cpp b/libraries/AP_Baro/AP_Baro_BMP085.cpp index 60480ec82e..e88016f362 100644 --- a/libraries/AP_Baro/AP_Baro_BMP085.cpp +++ b/libraries/AP_Baro/AP_Baro_BMP085.cpp @@ -1,3 +1,4 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- /* APM_BMP085.cpp - Arduino Library for BMP085 absolute pressure sensor Code by Jordi Mu�oz and Jose Julio. DIYDrones.com @@ -41,7 +42,9 @@ extern "C" { #include "WConstants.h" } -#include +#include +#include // ArduPilot Mega Vector/Matrix math Library +#include #include "AP_Baro_BMP085.h" #define BMP085_ADDRESS 0x77 //(0xEE >> 1) @@ -55,34 +58,19 @@ extern "C" { // Public Methods ////////////////////////////////////////////////////////////// -void AP_Baro_BMP085::init( AP_PeriodicProcess * scheduler ) +bool AP_Baro_BMP085::init( AP_PeriodicProcess * scheduler ) { byte buff[22]; - int i = 0; - pinMode(BMP085_EOC, INPUT); // End Of Conversion (PC7) input + pinMode(BMP085_EOC, INPUT); // End Of Conversion (PC7) input oss = 3; // Over Sampling setting 3 = High resolution BMP085_State = 0; // Initial state - // We read the calibration data registers - Wire.beginTransmission(BMP085_ADDRESS); - Wire.send(0xAA); - if (Wire.endTransmission() != 0) { - // Error! - return; - } - - Wire.requestFrom(BMP085_ADDRESS, 22); - - //Wire.endTransmission(); - while(Wire.available()){ - buff[i] = Wire.receive(); // receive one byte - i++; - } - if (i != 22) { - // Error! - return; + // We read the calibration data registers + if (I2c.read(BMP085_ADDRESS, 0xAA, 22, buff) != 0) { + healthy = false; + return false; } ac1 = ((int)buff[0] << 8) | buff[1]; @@ -97,10 +85,12 @@ void AP_Baro_BMP085::init( AP_PeriodicProcess * scheduler ) mc = ((int)buff[18] << 8) | buff[19]; md = ((int)buff[20] << 8) | buff[21]; - //Send a command to read Temp + //Send a command to read Temp Command_ReadTemp(); BMP085_State = 1; - return; + + healthy = true; + return true; } // Read the sensor. This is a state machine @@ -152,48 +142,27 @@ int32_t AP_Baro_BMP085::get_raw_temp() { // Send command to Read Pressure void AP_Baro_BMP085::Command_ReadPress() { - Wire.beginTransmission(BMP085_ADDRESS); - Wire.send(0xF4); - Wire.send(0x34+(oss << 6)); // write_register(0xF4, 0x34+(oversampling_setting << 6)); - Wire.endTransmission(); + if (I2c.write(BMP085_ADDRESS, 0xF4, 0x34+(oss << 6)) != 0) { + healthy = false; + } } // Read Raw Pressure values void AP_Baro_BMP085::ReadPress() { - byte msb; - byte lsb; - byte xlsb; + uint8_t buf[3]; - Wire.beginTransmission(BMP085_ADDRESS); - Wire.send(0xF6); - Wire.endTransmission(); - - Wire.requestFrom(BMP085_ADDRESS, 3); // read a byte - - while(!Wire.available()) { - // waiting + if (I2c.read(BMP085_ADDRESS, 0xF6, 3, buf) != 0) { + healthy = false; + return; } - msb = Wire.receive(); - - while(!Wire.available()) { - // waiting - } - - lsb = Wire.receive(); - - while(!Wire.available()) { - // waiting - } - - xlsb = Wire.receive(); - RawPress = (((long)msb << 16) | ((long)lsb << 8) | ((long)xlsb)) >> (8 - oss); + RawPress = (((long)buf[0] << 16) | ((long)buf[1] << 8) | ((long)buf[2])) >> (8 - oss); if(_offset_press == 0){ _offset_press = RawPress; RawPress = 0; - }else{ + } else{ RawPress -= _offset_press; } // filter @@ -203,8 +172,9 @@ void AP_Baro_BMP085::ReadPress() _press_index = 0; RawPress = 0; + // sum our filter - for(uint8_t i = 0; i < PRESS_FILTER_SIZE; i++){ + for (uint8_t i = 0; i < PRESS_FILTER_SIZE; i++){ RawPress += _press_filter[i]; } @@ -217,35 +187,27 @@ void AP_Baro_BMP085::ReadPress() // Send Command to Read Temperature void AP_Baro_BMP085::Command_ReadTemp() { - Wire.beginTransmission(BMP085_ADDRESS); - Wire.send(0xF4); - Wire.send(0x2E); - Wire.endTransmission(); + if (I2c.write(BMP085_ADDRESS, 0xF4, 0x2E) != 0) { + healthy = false; + } } // Read Raw Temperature values void AP_Baro_BMP085::ReadTemp() { - byte tmp; - Wire.beginTransmission(BMP085_ADDRESS); - Wire.send(0xF6); - Wire.endTransmission(); + uint8_t buf[2]; - Wire.beginTransmission(BMP085_ADDRESS); - Wire.requestFrom(BMP085_ADDRESS,2); + if (I2c.read(BMP085_ADDRESS, 0xF6, 2, buf) != 0) { + healthy = false; + return; + } + RawTemp = buf[0]; + RawTemp = (RawTemp << 8) | buf[1]; - while(!Wire.available()); // wait - RawTemp = Wire.receive(); - - while(!Wire.available()); // wait - tmp = Wire.receive(); - - RawTemp = RawTemp << 8 | tmp; - - if(_offset_temp == 0){ + if (_offset_temp == 0){ _offset_temp = RawTemp; RawTemp = 0; - }else{ + } else { RawTemp -= _offset_temp; } diff --git a/libraries/AP_Baro/AP_Baro_BMP085.h b/libraries/AP_Baro/AP_Baro_BMP085.h index f894ab4395..ea9e5742b0 100644 --- a/libraries/AP_Baro/AP_Baro_BMP085.h +++ b/libraries/AP_Baro/AP_Baro_BMP085.h @@ -1,3 +1,4 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- #ifndef __AP_BARO_BMP085_H__ #define __AP_BARO_BMP085_H__ @@ -16,7 +17,7 @@ class AP_Baro_BMP085 : public AP_Baro /* AP_Baro public interface: */ - void init(AP_PeriodicProcess * scheduler); + bool init(AP_PeriodicProcess * scheduler); uint8_t read(); int32_t get_pressure(); int16_t get_temperature(); diff --git a/libraries/AP_Baro/AP_Baro_BMP085_hil.cpp b/libraries/AP_Baro/AP_Baro_BMP085_hil.cpp index 2af3da2d3d..795326cb4f 100644 --- a/libraries/AP_Baro/AP_Baro_BMP085_hil.cpp +++ b/libraries/AP_Baro/AP_Baro_BMP085_hil.cpp @@ -1,3 +1,4 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- extern "C" { // AVR LibC Includes @@ -14,9 +15,10 @@ AP_Baro_BMP085_HIL::AP_Baro_BMP085_HIL() } // Public Methods ////////////////////////////////////////////////////////////// -void AP_Baro_BMP085_HIL::init(AP_PeriodicProcess * scheduler) +bool AP_Baro_BMP085_HIL::init(AP_PeriodicProcess * scheduler) { - BMP085_State=1; + BMP085_State=1; + return true; } @@ -46,6 +48,7 @@ void AP_Baro_BMP085_HIL::setHIL(float _Temp, float _Press) // TODO: map floats to raw Temp = _Temp; Press = _Press; + healthy = true; } int32_t AP_Baro_BMP085_HIL::get_pressure() { diff --git a/libraries/AP_Baro/AP_Baro_BMP085_hil.h b/libraries/AP_Baro/AP_Baro_BMP085_hil.h index fbcef71086..d7dbfd7618 100644 --- a/libraries/AP_Baro/AP_Baro_BMP085_hil.h +++ b/libraries/AP_Baro/AP_Baro_BMP085_hil.h @@ -1,3 +1,4 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- #ifndef __AP_BARO_BMP085_HIL_H__ #define __AP_BARO_BMP085_HIL_H__ @@ -6,16 +7,18 @@ class AP_Baro_BMP085_HIL { - private: - uint8_t BMP085_State; +private: + uint8_t BMP085_State; int16_t Temp; int32_t Press; - - public: + +public: AP_Baro_BMP085_HIL(); // Constructor //int Altitude; + bool healthy; + uint8_t oss; - void init(AP_PeriodicProcess * scheduler); + bool init(AP_PeriodicProcess * scheduler); uint8_t read(); int32_t get_pressure(); int16_t get_temperature(); diff --git a/libraries/AP_Baro/AP_Baro_MS5611.cpp b/libraries/AP_Baro/AP_Baro_MS5611.cpp index ab424c7de8..497fb11d45 100644 --- a/libraries/AP_Baro/AP_Baro_MS5611.cpp +++ b/libraries/AP_Baro/AP_Baro_MS5611.cpp @@ -1,3 +1,4 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- /* APM_MS5611.cpp - Arduino Library for MS5611-01BA01 absolute pressure sensor Code by Jose Julio, Pat Hickey and Jordi Muñoz. DIYDrones.com @@ -110,12 +111,12 @@ void AP_Baro_MS5611::_spi_write(uint8_t reg) // Public Methods ////////////////////////////////////////////////////////////// // SPI should be initialized externally -void AP_Baro_MS5611::init( AP_PeriodicProcess *scheduler ) +bool AP_Baro_MS5611::init( AP_PeriodicProcess *scheduler ) { pinMode(MS5611_CS, OUTPUT); // Chip select Pin - digitalWrite(MS5611_CS, HIGH); - delay(1); - + digitalWrite(MS5611_CS, HIGH); + delay(1); + _spi_write(CMD_MS5611_RESET); delay(4); @@ -128,14 +129,17 @@ void AP_Baro_MS5611::init( AP_PeriodicProcess *scheduler ) C6 = _spi_read_16bits(CMD_MS5611_PROM_C6); - //Send a command to read Temp first + //Send a command to read Temp first _spi_write(CMD_CONVERT_D2_OSR4096); _timer = micros(); _state = 1; - Temp=0; - Press=0; + Temp=0; + Press=0; + + scheduler->register_process( AP_Baro_MS5611::_update ); - scheduler->register_process( AP_Baro_MS5611::_update ); + healthy = true; + return true; } diff --git a/libraries/AP_Baro/AP_Baro_MS5611.h b/libraries/AP_Baro/AP_Baro_MS5611.h index 92cec87c26..f1a72e2681 100644 --- a/libraries/AP_Baro/AP_Baro_MS5611.h +++ b/libraries/AP_Baro/AP_Baro_MS5611.h @@ -1,3 +1,4 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- #ifndef __AP_BARO_MS5611_H__ #define __AP_BARO_MS5611_H__ @@ -10,7 +11,7 @@ class AP_Baro_MS5611 : public AP_Baro AP_Baro_MS5611() {} // Constructor /* AP_Baro public interface: */ - void init(AP_PeriodicProcess *scheduler); + bool init(AP_PeriodicProcess *scheduler); uint8_t read(); int32_t get_pressure(); // in mbar*100 units int16_t get_temperature(); // in celsius degrees * 100 units diff --git a/libraries/AP_Baro/examples/AP_Baro_BMP085_test/AP_Baro_BMP085_test.pde b/libraries/AP_Baro/examples/AP_Baro_BMP085_test/AP_Baro_BMP085_test.pde index ffbd13587e..af37cdd837 100644 --- a/libraries/AP_Baro/examples/AP_Baro_BMP085_test/AP_Baro_BMP085_test.pde +++ b/libraries/AP_Baro/examples/AP_Baro_BMP085_test/AP_Baro_BMP085_test.pde @@ -4,52 +4,74 @@ */ #include -#include -#include // ArduPilot Mega BMP085 Library +#include +#include +#include // ArduPilot Mega BMP085 Library +#include +#include +#include +#include +#include -APM_BMP085_Class APM_BMP085; +#ifndef APM2_HARDWARE +# define APM2_HARDWARE 0 +#endif + +AP_Baro_BMP085 APM_BMP085(APM2_HARDWARE); unsigned long timer; FastSerialPort0(Serial); -#ifdef APM2_HARDWARE -static bool apm2_hardware = true; -#else -static bool apm2_hardware = false; -#endif + +Arduino_Mega_ISR_Registry isr_registry; +AP_TimerProcess scheduler; void setup() { Serial.begin(115200); Serial.println("ArduPilot Mega BMP085 library test"); Serial.println("Initialising barometer..."); delay(100); - if (!APM_BMP085.Init(1, apm2_hardware)) { + + I2c.begin(); + I2c.timeOut(20); + + //I2c.setSpeed(true); + + isr_registry.init(); + scheduler.init(&isr_registry); + + if (!APM_BMP085.init(&scheduler)) { Serial.println("Barometer initialisation FAILED\n"); } Serial.println("initialisation complete."); delay(100); delay(1000); - timer = millis(); + timer = micros(); } void loop() { - int ch; float tmp_float; float Altitude; - if((millis()- timer) > 50){ - timer = millis(); - APM_BMP085.Read(); + if((micros()- timer) > 50000L){ + timer = micros(); + APM_BMP085.read(); + unsigned long read_time = micros() - timer; + if (!APM_BMP085.healthy) { + Serial.println("not healthy"); + return; + } Serial.print("Pressure:"); - Serial.print(APM_BMP085.Press); + Serial.print(APM_BMP085.get_pressure()); Serial.print(" Temperature:"); - Serial.print(APM_BMP085.Temp / 10.0); + Serial.print(APM_BMP085.get_temperature()); Serial.print(" Altitude:"); - tmp_float = (APM_BMP085.Press / 101325.0); + tmp_float = (APM_BMP085.get_pressure() / 101325.0); tmp_float = pow(tmp_float, 0.190295); Altitude = 44330 * (1.0 - tmp_float); Serial.print(Altitude); + Serial.printf(" t=%u", (unsigned)read_time); Serial.println(); } } diff --git a/libraries/AP_Baro/examples/AP_Baro_MS5611_test/AP_Baro_MS5611_test.pde b/libraries/AP_Baro/examples/AP_Baro_MS5611_test/AP_Baro_MS5611_test.pde index 76c367fada..a1ff95ac38 100644 --- a/libraries/AP_Baro/examples/AP_Baro_MS5611_test/AP_Baro_MS5611_test.pde +++ b/libraries/AP_Baro/examples/AP_Baro_MS5611_test/AP_Baro_MS5611_test.pde @@ -1,12 +1,21 @@ #include +#include +#include // ArduPilot Mega Vector/Matrix math Library #include +#include #include +#include +#include #include // ArduPilot Mega ADC Library FastSerialPort0(Serial); AP_Baro_MS5611 baro; +Arduino_Mega_ISR_Registry isr_registry; +AP_TimerProcess scheduler; + +unsigned long timer; void setup() { @@ -15,54 +24,41 @@ void setup() delay(1000); - pinMode(63, OUTPUT); - digitalWrite(63, HIGH); - SPI.begin(); - SPI.setClockDivider(SPI_CLOCK_DIV32); // 500khz for debugging, increase later + isr_registry.init(); + scheduler.init(&isr_registry); - baro.init(); + pinMode(63, OUTPUT); + digitalWrite(63, HIGH); + SPI.begin(); + SPI.setClockDivider(SPI_CLOCK_DIV32); // 500khz for debugging, increase later + + baro.init(&scheduler); + timer = micros(); } void loop() { - int32_t pres; - int32_t temp; - - Serial.println("Start Conversions"); - - baro._start_conversion_D1(); - delay(10); - bool res1 = baro._adc_read(&pres); - baro._start_conversion_D2(); - delay(10); - bool res2 = baro._adc_read(&temp); - - if (res1) { - Serial.printf("Pressure raw value %ld\r\n",pres); - } else { - Serial.println("ADC conversion D1 unsuccessful"); - } - - if (res2) { - Serial.printf("Temp raw value %ld\r\n",pres); - } else { - Serial.println("ADC conversion D2 unsuccessful"); - } - - Serial.println("---"); - delay(250); -} - -void update_and_print() -{ - int32_t pres; - float temp; - - baro.update(); - - pres = baro.get_pressure(); - temp = baro.get_temp(); - Serial.printf("p: %ld t: %f \r\n", pres, temp); - - delay(100); + float tmp_float; + float Altitude; + + if((micros()- timer) > 50000L){ + timer = micros(); + baro.read(); + unsigned long read_time = micros() - timer; + if (!baro.healthy) { + Serial.println("not healthy"); + return; + } + Serial.print("Pressure:"); + Serial.print(baro.get_pressure()); + Serial.print(" Temperature:"); + Serial.print(baro.get_temperature()); + Serial.print(" Altitude:"); + tmp_float = (baro.get_pressure() / 101325.0); + tmp_float = pow(tmp_float, 0.190295); + Altitude = 44330 * (1.0 - tmp_float); + Serial.print(Altitude); + Serial.printf(" t=%u", (unsigned)read_time); + Serial.println(); + } } diff --git a/libraries/AP_Compass/AP_Compass_HIL.cpp b/libraries/AP_Compass/AP_Compass_HIL.cpp index a8554da41b..2eb7a069b5 100644 --- a/libraries/AP_Compass/AP_Compass_HIL.cpp +++ b/libraries/AP_Compass/AP_Compass_HIL.cpp @@ -13,9 +13,10 @@ // Public Methods ////////////////////////////////////////////////////////////// -void AP_Compass_HIL::read() +bool AP_Compass_HIL::read() { - // values set by setHIL function + // values set by setHIL function + return true; } // Update raw magnetometer values from HIL data @@ -26,4 +27,5 @@ void AP_Compass_HIL::setHIL(float _mag_x, float _mag_y, float _mag_z) mag_x = _mag_x; mag_y = _mag_y; mag_z = _mag_z; + healthy = true; } diff --git a/libraries/AP_Compass/AP_Compass_HIL.h b/libraries/AP_Compass/AP_Compass_HIL.h index 3949fc9750..b3cb985d91 100644 --- a/libraries/AP_Compass/AP_Compass_HIL.h +++ b/libraries/AP_Compass/AP_Compass_HIL.h @@ -1,3 +1,4 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- #ifndef AP_Compass_HIL_H #define AP_Compass_HIL_H @@ -7,9 +8,8 @@ class AP_Compass_HIL : public Compass { public: AP_Compass_HIL(AP_Var::Key key = AP_Var::k_key_none) : Compass(key) { product_id = AP_COMPASS_TYPE_HIL; } - - void read(); - void setHIL(float Mag_X, float Mag_Y, float Mag_Z); + bool read(void); + void setHIL(float Mag_X, float Mag_Y, float Mag_Z); }; #endif diff --git a/libraries/AP_Compass/AP_Compass_HMC5843.cpp b/libraries/AP_Compass/AP_Compass_HMC5843.cpp index e3e7233d36..8096275623 100644 --- a/libraries/AP_Compass/AP_Compass_HMC5843.cpp +++ b/libraries/AP_Compass/AP_Compass_HMC5843.cpp @@ -16,8 +16,9 @@ // AVR LibC Includes #include #include "WConstants.h" +#include -#include +#include #include "AP_Compass_HMC5843.h" #define COMPASS_ADDRESS 0x1E @@ -47,40 +48,23 @@ #define DataOutputRate_75HZ 0x06 // read_register - read a register value -static bool -read_register(int address, byte *value) +bool AP_Compass_HMC5843::read_register(uint8_t address, uint8_t *value) { - bool ret = false; - - *value = 0; - - Wire.beginTransmission(COMPASS_ADDRESS); - Wire.send(address); //sends address to read from - if (0 != Wire.endTransmission()) - return false; - - Wire.requestFrom(COMPASS_ADDRESS, 1); // request 1 byte from device - if( Wire.available() ) { - *value = Wire.receive(); // receive one byte - ret = true; - } - if (0 != Wire.endTransmission()) - return false; - - return ret; + if (I2c.read((uint8_t)COMPASS_ADDRESS, address, 1, value) != 0) { + healthy = false; + return false; + } + return true; } // write_register - update a register value -static bool -write_register(int address, byte value) +bool AP_Compass_HMC5843::write_register(uint8_t address, byte value) { - Wire.beginTransmission(COMPASS_ADDRESS); - Wire.send(address); - Wire.send(value); - if (0 != Wire.endTransmission()) - return false; - delay(10); - return true; + if (I2c.write((uint8_t)COMPASS_ADDRESS, address, value) != 0) { + healthy = false; + return false; + } + return true; } /* @@ -96,25 +80,10 @@ static void rotate_for_5883L(AP_VarS *_orientation_matrix) // Read Sensor data bool AP_Compass_HMC5843::read_raw() { - int i = 0; - byte buff[6]; + uint8_t buff[6]; - Wire.beginTransmission(COMPASS_ADDRESS); - Wire.send(0x03); //sends address to read from - if (0 != Wire.endTransmission()) - return false; - - Wire.requestFrom(COMPASS_ADDRESS, 6); // request 6 bytes from device - while (Wire.available()) { - buff[i] = Wire.receive(); // receive one byte - i++; - } - - if (0 != Wire.endTransmission()) - return false; - - if (i != 6) { - /* we didn't get enough bytes */ + if (I2c.read(COMPASS_ADDRESS, 0x03, 6, buff) != 0) { + healthy = false; return false; } @@ -140,13 +109,26 @@ bool AP_Compass_HMC5843::read_raw() } + +/* + re-initialise after a IO error + */ +bool AP_Compass_HMC5843::re_initialise() +{ + if (! write_register(ConfigRegA, _base_config) || + ! write_register(ConfigRegB, magGain) || + ! write_register(ModeRegister, ContinuousConversion)) + return false; + return true; +} + + // Public Methods ////////////////////////////////////////////////////////////// bool AP_Compass_HMC5843::init() { int numAttempts = 0, good_count = 0; bool success = false; - byte base_config; // used to test compass type byte calibration_gain = 0x20; uint16_t expected_x = 715; uint16_t expected_yz = 715; @@ -156,10 +138,11 @@ AP_Compass_HMC5843::init() // determine if we are using 5843 or 5883L if (! write_register(ConfigRegA, SampleAveraging_8<<5 | DataOutputRate_75HZ<<2 | NormalOperation) || - ! read_register(ConfigRegA, &base_config)) { + ! read_register(ConfigRegA, &_base_config)) { + healthy = false; return false; } - if ( base_config == (SampleAveraging_8<<5 | DataOutputRate_75HZ<<2 | NormalOperation)) { + if ( _base_config == (SampleAveraging_8<<5 | DataOutputRate_75HZ<<2 | NormalOperation)) { // a 5883L supports the sample averaging config int old_product_id = product_id; @@ -175,7 +158,7 @@ AP_Compass_HMC5843::init() */ rotate_for_5883L(&_orientation_matrix); } - } else if (base_config == (NormalOperation | DataOutputRate_75HZ<<2)) { + } else if (_base_config == (NormalOperation | DataOutputRate_75HZ<<2)) { product_id = AP_COMPASS_TYPE_HMC5843; } else { // not behaving like either supported compass type @@ -253,23 +236,22 @@ AP_Compass_HMC5843::init() } // leave test mode - if (! write_register(ConfigRegA, base_config)) + if (!re_initialise()) { return false; - delay(50); - if (! write_register(ConfigRegB, magGain) || - ! write_register(ModeRegister, ContinuousConversion)) - return false; - delay(50); + } return success; } // Read Sensor data -void -AP_Compass_HMC5843::read() +bool AP_Compass_HMC5843::read() { + if (!healthy && !re_initialise()) { + return false; + } + if (!read_raw()) { - return; + return false; } mag_x *= calibration[0]; @@ -285,6 +267,9 @@ AP_Compass_HMC5843::read() mag_x = rot_mag.x; mag_y = rot_mag.y; mag_z = rot_mag.z; + healthy = true; + + return true; } // set orientation diff --git a/libraries/AP_Compass/AP_Compass_HMC5843.h b/libraries/AP_Compass/AP_Compass_HMC5843.h index ec77fe753e..3dbd747eef 100644 --- a/libraries/AP_Compass/AP_Compass_HMC5843.h +++ b/libraries/AP_Compass/AP_Compass_HMC5843.h @@ -1,3 +1,4 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- #ifndef AP_Compass_HMC5843_H #define AP_Compass_HMC5843_H @@ -47,11 +48,16 @@ class AP_Compass_HMC5843 : public Compass { private: float calibration[3]; + virtual bool read_raw(void); + uint8_t _base_config; + virtual bool re_initialise(void); + bool read_register(uint8_t address, uint8_t *value); + bool write_register(uint8_t address, byte value); + public: AP_Compass_HMC5843(AP_Var::Key key = AP_Var::k_key_none) : Compass(key) {} - virtual bool init(); - virtual void read(); - virtual bool read_raw(); + virtual bool init(void); + virtual bool read(void); virtual void set_orientation(const Matrix3f &rotation_matrix); }; diff --git a/libraries/AP_Compass/Compass.cpp b/libraries/AP_Compass/Compass.cpp index 86af0c21ce..d67c900ed0 100644 --- a/libraries/AP_Compass/Compass.cpp +++ b/libraries/AP_Compass/Compass.cpp @@ -1,3 +1,4 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- #include "Compass.h" // Default constructor. diff --git a/libraries/AP_Compass/Compass.h b/libraries/AP_Compass/Compass.h index b16614fc01..a2550f2518 100644 --- a/libraries/AP_Compass/Compass.h +++ b/libraries/AP_Compass/Compass.h @@ -1,3 +1,4 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- #ifndef Compass_h #define Compass_h @@ -40,6 +41,7 @@ public: float heading_x; ///< compass vector X magnitude float heading_y; ///< compass vector Y magnitude unsigned long last_update; ///< millis() time of last update + bool healthy; ///< true if last read OK /// Constructor /// @@ -56,7 +58,7 @@ public: /// Read the compass and update the mag_ variables. /// - virtual void read() = 0; + virtual bool read(void) = 0; /// Calculate the tilt-compensated heading_ variables. /// @@ -98,21 +100,21 @@ public: /// virtual Vector3f &get_offsets(); - /// Program new offset values. - /// - /// XXX DEPRECATED - /// - /// @param x Offset to the raw mag_x value. - /// @param y Offset to the raw mag_y value. - /// @param z Offset to the raw mag_z value. - /// - void set_offsets(int x, int y, int z) { set_offsets(Vector3f(x, y, z)); } + /// Program new offset values. + /// + /// XXX DEPRECATED + /// + /// @param x Offset to the raw mag_x value. + /// @param y Offset to the raw mag_y value. + /// @param z Offset to the raw mag_z value. + /// + void set_offsets(int x, int y, int z) { set_offsets(Vector3f(x, y, z)); } - /// Perform automatic offset updates using the results of the DCM matrix. - /// - /// @param dcm_matrix The DCM result matrix. - /// - void null_offsets(const Matrix3f &dcm_matrix); + /// Perform automatic offset updates using the results of the DCM matrix. + /// + /// @param dcm_matrix The DCM result matrix. + /// + void null_offsets(const Matrix3f &dcm_matrix); /// Sets the local magnetic field declination. @@ -129,7 +131,7 @@ protected: AP_Float _declination; bool _null_init_done; ///< first-time-around flag used by offset nulling - Matrix3f _last_dcm_matrix; ///< previous DCM matrix used by offset nulling - Vector3f _mag_body_last; ///< ?? used by offset nulling + Matrix3f _last_dcm_matrix; ///< previous DCM matrix used by offset nulling + Vector3f _mag_body_last; ///< ?? used by offset nulling }; #endif diff --git a/libraries/AP_Compass/examples/AP_Compass_test/AP_Compass_test.pde b/libraries/AP_Compass/examples/AP_Compass_test/AP_Compass_test.pde index f1e7e3b05f..fae4dd6124 100644 --- a/libraries/AP_Compass/examples/AP_Compass_test/AP_Compass_test.pde +++ b/libraries/AP_Compass/examples/AP_Compass_test/AP_Compass_test.pde @@ -7,7 +7,7 @@ #include #include // Compass Library #include // ArduPilot Mega Vector/Matrix math Library -#include +#include FastSerialPort0(Serial); @@ -23,7 +23,11 @@ void setup() { Serial.begin(115200); Serial.println("Compass library test (HMC5843 and HMC5883L)"); - Wire.begin(); + I2c.begin(); + I2c.timeOut(20); + + // I2c.setSpeed(true); + if (!compass.init()) { Serial.println("compass initialisation failed!"); while (1) ; @@ -50,17 +54,23 @@ void setup() } delay(3000); - timer = millis(); + timer = micros(); } void loop() { static float min[3], max[3], offset[3]; - if((millis()- timer) > 100) + if((micros()- timer) > 100000L) { - timer = millis(); + timer = micros(); compass.read(); + unsigned long read_time = micros() - timer; + + if (!compass.healthy) { + Serial.println("not healthy"); + return; + } compass.calculate(0,0); // roll = 0, pitch = 0 for this example // capture min @@ -85,25 +95,18 @@ void loop() offset[2] = -(max[2]+min[2])/2; // display all to user - Serial.print("Heading:"); - Serial.print(ToDeg(compass.heading)); - Serial.print(" ("); - Serial.print(compass.mag_x); - Serial.print(","); - Serial.print(compass.mag_y); - Serial.print(","); - Serial.print(compass.mag_z); - Serial.print(")"); + Serial.printf("Heading: %.2f (%3u,%3u,%3u) ", + ToDeg(compass.heading), + compass.mag_x, + compass.mag_y, + compass.mag_z); // display offsets - Serial.print("\t offsets("); - Serial.print(offset[0]); - Serial.print(","); - Serial.print(offset[1]); - Serial.print(","); - Serial.print(offset[2]); - Serial.print(")"); + Serial.printf("\t offsets(%.2f, %.2f, %.2f)", + offset[0], offset[1], offset[2]); + + Serial.printf(" t=%u", (unsigned)read_time); Serial.println(); } -} +} diff --git a/libraries/AP_Compass/examples/AP_Compass_test/Makefile b/libraries/AP_Compass/examples/AP_Compass_test/Makefile index 85b4d8856b..d1f40fd90f 100644 --- a/libraries/AP_Compass/examples/AP_Compass_test/Makefile +++ b/libraries/AP_Compass/examples/AP_Compass_test/Makefile @@ -1,2 +1 @@ -BOARD = mega include ../../../AP_Common/Arduino.mk diff --git a/libraries/AP_DCM/AP_DCM.cpp b/libraries/AP_DCM/AP_DCM.cpp index 513001b0d6..ceedb1055f 100644 --- a/libraries/AP_DCM/AP_DCM.cpp +++ b/libraries/AP_DCM/AP_DCM.cpp @@ -309,7 +309,7 @@ AP_DCM::drift_correction(void) //*****YAW*************** - if (_compass) { + if (_compass && _compass->healthy) { // We make the gyro YAW drift correction based on compass magnetic heading error_course = (_dcm_matrix.a.x * _compass->heading_y) - (_dcm_matrix.b.x * _compass->heading_x); // Equation 23, Calculating YAW error diff --git a/libraries/AP_InertialSensor/examples/MPU6000/MPU6000.pde b/libraries/AP_InertialSensor/examples/MPU6000/MPU6000.pde index c67db6b62f..40067b4500 100644 --- a/libraries/AP_InertialSensor/examples/MPU6000/MPU6000.pde +++ b/libraries/AP_InertialSensor/examples/MPU6000/MPU6000.pde @@ -5,7 +5,6 @@ // #include -#include #include #include #include @@ -24,7 +23,6 @@ void setup(void) Serial.begin(115200); Serial.println("Doing INS startup..."); - Wire.begin(); SPI.begin(); SPI.setClockDivider(SPI_CLOCK_DIV16); // 1MHZ SPI rate diff --git a/libraries/Desktop/Desktop.mk b/libraries/Desktop/Desktop.mk index 6aba97f647..4cd7c795e3 100644 --- a/libraries/Desktop/Desktop.mk +++ b/libraries/Desktop/Desktop.mk @@ -185,7 +185,7 @@ else endif # these are library objects we don't want in the desktop build (maybe we'll add them later) -NODESKTOP := DataFlash/DataFlash_APM1.cpp FastSerial/FastSerial.cpp AP_Compass/AP_Compass_HMC5843.cpp AP_Baro/AP_Baro_BMP085.cpp +NODESKTOP := I2C/I2C.cpp DataFlash/DataFlash_APM1.cpp FastSerial/FastSerial.cpp AP_Compass/AP_Compass_HMC5843.cpp AP_Baro/AP_Baro_BMP085.cpp # # Find sketchbook libraries referenced by the sketch. diff --git a/libraries/I2C/I2C.cpp b/libraries/I2C/I2C.cpp new file mode 100644 index 0000000000..52fb607f87 --- /dev/null +++ b/libraries/I2C/I2C.cpp @@ -0,0 +1,557 @@ +/* + 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 "WProgram.h" +#include +#include "I2C.h" + + + +uint8_t I2C::bytesAvailable = 0; +uint8_t I2C::bufferIndex = 0; +uint8_t I2C::totalBytes = 0; +uint16_t I2C::timeOutDelay = 0; + +I2C::I2C() +{ +} + + +////////////// Public Methods //////////////////////////////////////// + + + +void I2C::begin() +{ + #if defined(__AVR_ATmega168__) || defined(__AVR_ATmega8__) || defined(__AVR_ATmega328P__) + // activate internal pull-ups for twi + // as per note from atmega8 manual pg167 + sbi(PORTC, 4); + sbi(PORTC, 5); + #else + // activate internal pull-ups for twi + // as per note from atmega128 manual pg204 + sbi(PORTD, 0); + sbi(PORTD, 1); + #endif + // 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 I2C::end() +{ + TWCR = 0; +} + +void I2C::timeOut(uint16_t _timeOut) +{ + timeOutDelay = _timeOut; +} + +void I2C::setSpeed(boolean _fast) +{ + if(!_fast) + { + TWBR = ((CPU_FREQ / 100000) - 16) / 2; + } + else + { + TWBR = ((CPU_FREQ / 400000) - 16) / 2; + } +} + +void I2C::pullup(boolean activate) +{ + if(activate) + { + #if defined(__AVR_ATmega168__) || defined(__AVR_ATmega8__) || defined(__AVR_ATmega328P__) + // activate internal pull-ups for twi + // as per note from atmega8 manual pg167 + sbi(PORTC, 4); + sbi(PORTC, 5); + #else + // activate internal pull-ups for twi + // as per note from atmega128 manual pg204 + sbi(PORTD, 0); + sbi(PORTD, 1); + #endif + } + else + { + #if defined(__AVR_ATmega168__) || defined(__AVR_ATmega8__) || defined(__AVR_ATmega328P__) + // deactivate internal pull-ups for twi + // as per note from atmega8 manual pg167 + cbi(PORTC, 4); + cbi(PORTC, 5); + #else + // deactivate internal pull-ups for twi + // as per note from atmega128 manual pg204 + cbi(PORTD, 0); + cbi(PORTD, 1); + #endif + } +} + +/////////////carry over from Wire library /////////// + +uint8_t I2C::beginTransmission(uint8_t address) +{ + returnStatusWire = 0; + returnStatus = 0; + returnStatus = start(); + returnStatusWire = returnStatus; + if(returnStatus){return(returnStatus);} + returnStatus = sendAddress(SLA_W(address)); + returnStatusWire = returnStatus; + return(returnStatus); +} + +uint8_t I2C::beginTransmission(int address) +{ + return(beginTransmission((uint8_t) address)); +} + +uint8_t I2C::send(uint8_t databyte) +{ + if(returnStatusWire) + { + return(returnStatusWire); + } + returnStatus = 0; + returnStatus = sendByte(databyte); + returnStatusWire = returnStatus; + return(returnStatus); +} + +uint8_t I2C::send(int databyte) +{ + return(send((uint8_t) databyte)); +} + +uint8_t I2C::endTransmission() +{ + stop(); + return(returnStatusWire); +} + +uint8_t I2C::requestFrom(int address, int numberBytes) +{ + return(requestFrom((uint8_t) address, (uint8_t) numberBytes)); +} + +uint8_t I2C::requestFrom(uint8_t address, uint8_t numberBytes) +{ + returnStatus = 0; + returnStatus = read(address,numberBytes); + if(!returnStatus) + { + return(numberBytes); + } + return(0); +} + +uint8_t I2C::available() +{ + return(bytesAvailable); +} + +uint8_t I2C::receive() +{ + bufferIndex = totalBytes - bytesAvailable; + if(!bytesAvailable) + { + bufferIndex = 0; + return(0); + } + bytesAvailable--; + return(data[bufferIndex]); +} + + + + + +///////////////////////////////////////////////////// + +uint8_t I2C::write(uint8_t address, uint8_t registerAddress) +{ + returnStatus = 0; + returnStatus = start(); + if(returnStatus){return(returnStatus);} + returnStatus = sendAddress(SLA_W(address)); + if(returnStatus){return(returnStatus);} + returnStatus = sendByte(registerAddress); + if(returnStatus){return(returnStatus);} + returnStatus = stop(); + return(returnStatus); +} + +uint8_t I2C::write(int address, int registerAddress) +{ + return(write((uint8_t) address, (uint8_t) registerAddress)); +} + +uint8_t I2C::write(uint8_t address, uint8_t registerAddress, uint8_t databyte) +{ + returnStatus = 0; + returnStatus = start(); + if(returnStatus){return(returnStatus);} + returnStatus = sendAddress(SLA_W(address)); + if(returnStatus){return(returnStatus);} + returnStatus = sendByte(registerAddress); + if(returnStatus){return(returnStatus);} + returnStatus = sendByte(databyte); + if(returnStatus){return(returnStatus);} + returnStatus = stop(); + return(returnStatus); +} + +uint8_t I2C::write(int address, int registerAddress, int databyte) +{ + return(write((uint8_t) address, (uint8_t) registerAddress, (uint8_t) databyte)); +} + +uint8_t I2C::write(uint8_t address, uint8_t registerAddress, char *databytes) +{ + uint8_t bufferLength = strlen(databytes); + returnStatus = 0; + returnStatus = write(address, registerAddress, (uint8_t*)databytes, bufferLength); + return(returnStatus); +} + +uint8_t I2C::write(uint8_t address, uint8_t registerAddress, uint8_t *databytes, uint8_t numberBytes) +{ + returnStatus = 0; + returnStatus = start(); + if(returnStatus){return(returnStatus);} + returnStatus = sendAddress(SLA_W(address)); + if(returnStatus){return(returnStatus);} + returnStatus = sendByte(registerAddress); + if(returnStatus){return(returnStatus);} + for (uint8_t i = 0; i < numberBytes; i++) + { + returnStatus = sendByte(databytes[i]); + if(returnStatus){return(returnStatus);} + } + returnStatus = stop(); + return(returnStatus); +} + +uint8_t I2C::read(int address, int numberBytes) +{ + return(read((uint8_t) address, (uint8_t) numberBytes)); +} + +uint8_t I2C::read(uint8_t address, uint8_t numberBytes) +{ + bytesAvailable = 0; + bufferIndex = 0; + if(numberBytes == 0){numberBytes++;} + nack = numberBytes - 1; + returnStatus = 0; + returnStatus = start(); + if(returnStatus){return(returnStatus);} + returnStatus = sendAddress(SLA_R(address)); + if(returnStatus){return(returnStatus);} + for(uint8_t i = 0; i < numberBytes; i++) + { + if( i == nack ) + { + returnStatus = receiveByte(0); + if(returnStatus != MR_DATA_NACK){return(returnStatus);} + } + else + { + returnStatus = receiveByte(1); + if(returnStatus != MR_DATA_ACK){return(returnStatus);} + } + data[i] = TWDR; + bytesAvailable = i+1; + totalBytes = i+1; + } + returnStatus = stop(); + return(returnStatus); +} + +uint8_t I2C::read(int address, int registerAddress, int numberBytes) +{ + return(read((uint8_t) address, (uint8_t) registerAddress, (uint8_t) numberBytes)); +} + +uint8_t I2C::read(uint8_t address, uint8_t registerAddress, uint8_t numberBytes) +{ + bytesAvailable = 0; + bufferIndex = 0; + if(numberBytes == 0){numberBytes++;} + nack = numberBytes - 1; + returnStatus = 0; + returnStatus = start(); + if(returnStatus){return(returnStatus);} + returnStatus = sendAddress(SLA_W(address)); + if(returnStatus){return(returnStatus);} + returnStatus = sendByte(registerAddress); + if(returnStatus){return(returnStatus);} + returnStatus = start(); + if(returnStatus){return(returnStatus);} + returnStatus = sendAddress(SLA_R(address)); + if(returnStatus){return(returnStatus);} + for(uint8_t i = 0; i < numberBytes; i++) + { + if( i == nack ) + { + returnStatus = receiveByte(0); + if(returnStatus != MR_DATA_NACK){return(returnStatus);} + } + else + { + returnStatus = receiveByte(1); + if(returnStatus != MR_DATA_ACK){return(returnStatus);} + } + data[i] = TWDR; + bytesAvailable = i+1; + totalBytes = i+1; + } + returnStatus = stop(); + return(returnStatus); +} + +uint8_t I2C::read(uint8_t address, uint8_t numberBytes, uint8_t *dataBuffer) +{ + bytesAvailable = 0; + bufferIndex = 0; + if(numberBytes == 0){numberBytes++;} + nack = numberBytes - 1; + returnStatus = 0; + returnStatus = start(); + if(returnStatus){return(returnStatus);} + returnStatus = sendAddress(SLA_R(address)); + if(returnStatus){return(returnStatus);} + for(uint8_t i = 0; i < numberBytes; i++) + { + if( i == nack ) + { + returnStatus = receiveByte(0); + if(returnStatus != MR_DATA_NACK){return(returnStatus);} + } + else + { + returnStatus = receiveByte(1); + if(returnStatus != MR_DATA_ACK){return(returnStatus);} + } + dataBuffer[i] = TWDR; + bytesAvailable = i+1; + totalBytes = i+1; + } + returnStatus = stop(); + return(returnStatus); +} + +uint8_t I2C::read(uint8_t address, uint8_t registerAddress, uint8_t numberBytes, uint8_t *dataBuffer) +{ + bytesAvailable = 0; + bufferIndex = 0; + if(numberBytes == 0){numberBytes++;} + nack = numberBytes - 1; + returnStatus = 0; + returnStatus = start(); + if(returnStatus){return(returnStatus);} + returnStatus = sendAddress(SLA_W(address)); + if(returnStatus){return(returnStatus);} + returnStatus = sendByte(registerAddress); + if(returnStatus){return(returnStatus);} + returnStatus = start(); + if(returnStatus){return(returnStatus);} + returnStatus = sendAddress(SLA_R(address)); + if(returnStatus){return(returnStatus);} + for(uint8_t i = 0; i < numberBytes; i++) + { + if( i == nack ) + { + returnStatus = receiveByte(0); + if(returnStatus != MR_DATA_NACK){return(returnStatus);} + } + else + { + returnStatus = receiveByte(1); + if(returnStatus != MR_DATA_ACK){return(returnStatus);} + } + dataBuffer[i] = TWDR; + bytesAvailable = i+1; + totalBytes = i+1; + } + returnStatus = stop(); + return(returnStatus); +} + + +/////////////// Private Methods //////////////////////////////////////// + + +uint8_t I2C::start() +{ + unsigned long startingTime = millis(); + TWCR = (1<= timeOutDelay) + { + lockUp(); + return(1); + } + + } + if ((TWI_STATUS == START) || (TWI_STATUS == REPEATED_START)) + { + return(0); + } + return(TWI_STATUS); +} + +uint8_t I2C::sendAddress(uint8_t i2cAddress) +{ + TWDR = i2cAddress; + unsigned long startingTime = millis(); + TWCR = (1<= timeOutDelay) + { + lockUp(); + return(1); + } + + } + if ((TWI_STATUS == MT_SLA_ACK) || (TWI_STATUS == MR_SLA_ACK)) + { + return(0); + } + return(TWI_STATUS); +} + +uint8_t I2C::sendByte(uint8_t i2cData) +{ + TWDR = i2cData; + unsigned long startingTime = millis(); + TWCR = (1<= timeOutDelay) + { + lockUp(); + return(1); + } + + } + if (TWI_STATUS == MT_DATA_ACK) + { + return(0); + } + return(TWI_STATUS); +} + +uint8_t I2C::receiveByte(boolean ack) +{ + unsigned long startingTime = millis(); + if(ack) + { + TWCR = (1<= timeOutDelay) + { + lockUp(); + return(1); + } + + } + return(TWI_STATUS); +} + +uint8_t I2C::stop() +{ + unsigned long startingTime = millis(); + TWCR = (1<= timeOutDelay) + { + lockUp(); + return(1); + } + + } + return(0); +} + +void I2C::lockUp() +{ + TWCR = 0; //releases SDA and SCL lines to high impedance + TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA); //reinitialize TWI +} + +SIGNAL(TWI_vect) +{ + switch(TWI_STATUS){ + case 0x20: + case 0x30: + case 0x48: + TWCR = (1< + +#ifndef I2C_h +#define I2C_h + + +#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)) + +#define MAX_BUFFER_SIZE 32 + + +#ifndef CPU_FREQ +#define CPU_FREQ 16000000L +#endif + + + +class I2C +{ + public: + I2C(); + void begin(); + void end(); + void timeOut(uint16_t); + void setSpeed(boolean); + void pullup(boolean); + ///////carry over from Wire library//////// + uint8_t returnStatusWire; + uint8_t beginTransmission(uint8_t); + uint8_t beginTransmission(int); + uint8_t send(uint8_t); + uint8_t send(int); + uint8_t endTransmission(); + uint8_t requestFrom(uint8_t, uint8_t); + uint8_t requestFrom(int, int); + uint8_t available(); + /////////////////////////////////////////// + uint8_t write(uint8_t, uint8_t); + uint8_t write(int, int); + uint8_t write(uint8_t, uint8_t, uint8_t); + uint8_t write(int, int, int); + uint8_t write(uint8_t, uint8_t, char*); + uint8_t write(uint8_t, uint8_t, uint8_t*, uint8_t); + uint8_t read(uint8_t, uint8_t); + uint8_t read(int, int); + uint8_t read(uint8_t, uint8_t, uint8_t); + uint8_t read(int, int, int); + uint8_t read(uint8_t, uint8_t, uint8_t*); + uint8_t read(uint8_t, uint8_t, uint8_t, uint8_t*); + uint8_t receive(); + + private: + uint8_t start(); + uint8_t sendAddress(uint8_t); + uint8_t sendByte(uint8_t); + uint8_t receiveByte(boolean); + uint8_t stop(); + void lockUp(); + uint8_t returnStatus; + uint8_t nack; + uint8_t data[MAX_BUFFER_SIZE]; + static uint8_t bytesAvailable; + static uint8_t bufferIndex; + static uint8_t totalBytes; + static uint16_t timeOutDelay; + +}; + +extern I2C I2c; + +#endif diff --git a/libraries/I2C/examples/HMC5883L/HMC5883L.pde b/libraries/I2C/examples/HMC5883L/HMC5883L.pde new file mode 100644 index 0000000000..806e749795 --- /dev/null +++ b/libraries/I2C/examples/HMC5883L/HMC5883L.pde @@ -0,0 +1,70 @@ +/******************************************* + Sample sketch that configures an HMC5883L 3 axis + magnetometer to continuous mode and reads back + the three axis of data. + Code compiles to a size of 1500 bytes + Equivalent Wire Library code compiles to 2032 bytes + *******************************************/ + +#include + +#define HMC5883L 0x1E + +int x = 0; +int y = 0; +int z = 0; + + +void setup() +{ + I2c.begin(); + I2c.write(HMC5883L,0x02,0x00); //configure device for continuous mode +} + +void loop() +{ + I2c.read(HMC5883L,0x03,6); //read 6 bytes (x,y,z) from the device + x = I2c.receive() << 8; + x |= I2c.receive(); + y = I2c.receive() << 8; + y |= I2c.receive(); + z = I2c.receive() << 8; + z |= I2c.receive(); +} + + +/* Wire library equivalent would be this + +//#include + +#define HMC5883L 0x1E + +int x = 0; +int y = 0; +int z = 0; + + +void setup() +{ + Wire.begin(); + Wire.beginTransmission(HMC5883L); + Wire.send(0x02); + Wire.send(0x00); + Wire.endTransmission(); +} + +void loop() +{ + Wire.beginTransmission(HMC5883L); + Wire.send(0x03); + Wire.endTransmission(); + Wire.requestFrom(HMC5883L,6); + x = Wire.receive() << 8; + x |= Wire.receive(); + y = Wire.receive() << 8; + y |= Wire.receive(); + z = Wire.receive() << 8; + z |= Wire.receive(); +} + +********************************************/ diff --git a/libraries/I2C/keywords.txt b/libraries/I2C/keywords.txt new file mode 100644 index 0000000000..61ffa82ac3 --- /dev/null +++ b/libraries/I2C/keywords.txt @@ -0,0 +1,36 @@ +####################################### +# Syntax Coloring Map For I2C +####################################### + +####################################### +# Datatypes (KEYWORD1) +####################################### + + +####################################### +# Methods and Functions (KEYWORD2) +####################################### + +begin KEYWORD2 +end KEYWORD2 +timeOut KEYWORD2 +setSpeed KEYWORD2 +pullup KEYWORD2 +write KEYWORD2 +read KEYWORD2 +beginTransmission KEYWORD2 +send KEYWORD2 +endTransmission KEYWORD2 +requestFrom KEYWORD2 +available KEYWORD2 +receive KEYWORD2 + +####################################### +# Instances (KEYWORD2) +####################################### + +I2c KEYWORD2 + +####################################### +# Constants (LITERAL1) +####################################### \ No newline at end of file