diff --git a/libraries/AP_Baro/AP_Baro_BMP085.cpp b/libraries/AP_Baro/AP_Baro_BMP085.cpp index 840d0e5ea2..0a261e4edb 100644 --- a/libraries/AP_Baro/AP_Baro_BMP085.cpp +++ b/libraries/AP_Baro/AP_Baro_BMP085.cpp @@ -1,53 +1,53 @@ /// -*- 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 - - 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. - - Sensor is conected to I2C port - Sensor End of Conversion (EOC) pin is PC7 (30) - - Variables: - RawTemp : Raw temperature data - RawPress : Raw pressure data - - Temp : Calculated temperature (in 0.1�C units) - Press : Calculated pressure (in Pa units) - - Methods: - Init() : Initialization of I2C and read sensor calibration data - Read() : Read sensor data and calculate Temperature and Pressure - This function is optimized so the main host don�t need to wait - You can call this function in your main loop - It returns a 1 if there are new data. - - Internal functions: - Command_ReadTemp(): Send commando to read temperature - Command_ReadPress(): Send commando to read Pressure - ReadTemp() : Read temp register - ReadPress() : Read press register - Calculate() : Calculate Temperature and Pressure in real units - - -*/ + * APM_BMP085.cpp - Arduino Library for BMP085 absolute pressure sensor + * Code by Jordi Mu�oz and Jose Julio. DIYDrones.com + * + * 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. + * + * Sensor is conected to I2C port + * Sensor End of Conversion (EOC) pin is PC7 (30) + * + * Variables: + * RawTemp : Raw temperature data + * RawPress : Raw pressure data + * + * Temp : Calculated temperature (in 0.1�C units) + * Press : Calculated pressure (in Pa units) + * + * Methods: + * Init() : Initialization of I2C and read sensor calibration data + * Read() : Read sensor data and calculate Temperature and Pressure + * This function is optimized so the main host don�t need to wait + * You can call this function in your main loop + * It returns a 1 if there are new data. + * + * Internal functions: + * Command_ReadTemp(): Send commando to read temperature + * Command_ReadPress(): Send commando to read Pressure + * ReadTemp() : Read temp register + * ReadPress() : Read press register + * Calculate() : Calculate Temperature and Pressure in real units + * + * + */ extern "C" { - // AVR LibC Includes - #include - #include +// AVR LibC Includes +#include +#include } #if defined(ARDUINO) && ARDUINO >= 100 - #include "Arduino.h" + #include "Arduino.h" #else - #include "WConstants.h" + #include "WConstants.h" #endif #include -#include // ArduPilot Mega Vector/Matrix math Library +#include // ArduPilot Mega Vector/Matrix math Library #include #include "AP_Baro_BMP085.h" @@ -58,7 +58,7 @@ extern "C" { // chip using a direct IO port // On APM2 prerelease hw, the data ready port is hooked up to PE7, which // is not available to the arduino digitalRead function. -#define BMP_DATA_READY() (_apm2_hardware?(PINE&0x80):digitalRead(BMP085_EOC)) +#define BMP_DATA_READY() (_apm2_hardware ? (PINE&0x80) : digitalRead(BMP085_EOC)) // oversampling 3 gives highest resolution #define OVERSAMPLING 3 @@ -66,66 +66,66 @@ extern "C" { // Public Methods ////////////////////////////////////////////////////////////// bool AP_Baro_BMP085::init( AP_PeriodicProcess * scheduler ) { - byte buff[22]; + byte buff[22]; - pinMode(BMP085_EOC, INPUT); // End Of Conversion (PC7) input + pinMode(BMP085_EOC, INPUT); // End Of Conversion (PC7) input - BMP085_State = 0; // Initial state + BMP085_State = 0; // Initial state - // We read the calibration data registers - if (I2c.read(BMP085_ADDRESS, 0xAA, 22, buff) != 0) { - healthy = false; - return false; - } + // 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]; - ac2 = ((int)buff[2] << 8) | buff[3]; - ac3 = ((int)buff[4] << 8) | buff[5]; - ac4 = ((int)buff[6] << 8) | buff[7]; - ac5 = ((int)buff[8] << 8) | buff[9]; - ac6 = ((int)buff[10] << 8) | buff[11]; - b1 = ((int)buff[12] << 8) | buff[13]; - b2 = ((int)buff[14] << 8) | buff[15]; - mb = ((int)buff[16] << 8) | buff[17]; - mc = ((int)buff[18] << 8) | buff[19]; - md = ((int)buff[20] << 8) | buff[21]; + ac1 = ((int)buff[0] << 8) | buff[1]; + ac2 = ((int)buff[2] << 8) | buff[3]; + ac3 = ((int)buff[4] << 8) | buff[5]; + ac4 = ((int)buff[6] << 8) | buff[7]; + ac5 = ((int)buff[8] << 8) | buff[9]; + ac6 = ((int)buff[10] << 8) | buff[11]; + b1 = ((int)buff[12] << 8) | buff[13]; + b2 = ((int)buff[14] << 8) | buff[15]; + mb = ((int)buff[16] << 8) | buff[17]; + mc = ((int)buff[18] << 8) | buff[19]; + md = ((int)buff[20] << 8) | buff[21]; - //Send a command to read Temp - Command_ReadTemp(); - BMP085_State = 1; + //Send a command to read Temp + Command_ReadTemp(); + BMP085_State = 1; - // init raw temo - RawTemp = 0; + // init raw temo + RawTemp = 0; - healthy = true; - return true; + healthy = true; + return true; } // Read the sensor. This is a state machine // We read Temperature (state=1) and then Pressure (state!=1) on alternate calls uint8_t AP_Baro_BMP085::read() { - uint8_t result = 0; + uint8_t result = 0; - if (BMP085_State == 1){ - if (BMP_DATA_READY()){ - BMP085_State = 2; - ReadTemp(); // On state 1 we read temp - Command_ReadPress(); - } - }else{ - if (BMP_DATA_READY()){ - BMP085_State = 1; // Start again from state = 1 - ReadPress(); - Calculate(); - Command_ReadTemp(); // Read Temp - result = 1; // New pressure reading - } - } + if (BMP085_State == 1) { + if (BMP_DATA_READY()) { + BMP085_State = 2; + ReadTemp(); // On state 1 we read temp + Command_ReadPress(); + } + }else{ + if (BMP_DATA_READY()) { + BMP085_State = 1; // Start again from state = 1 + ReadPress(); + Calculate(); + Command_ReadTemp(); // Read Temp + result = 1; // New pressure reading + } + } if (result) { _last_update = millis(); } - return(result); + return(result); } float AP_Baro_BMP085::get_pressure() { @@ -149,94 +149,94 @@ int32_t AP_Baro_BMP085::get_raw_temp() { // Send command to Read Pressure void AP_Baro_BMP085::Command_ReadPress() { - if (I2c.write(BMP085_ADDRESS, 0xF4, 0x34+(OVERSAMPLING << 6)) != 0) { - healthy = false; - } + if (I2c.write(BMP085_ADDRESS, 0xF4, 0x34+(OVERSAMPLING << 6)) != 0) { + healthy = false; + } } // Read Raw Pressure values void AP_Baro_BMP085::ReadPress() { - uint8_t buf[3]; + uint8_t buf[3]; if (!healthy && millis() < _retry_time) { return; } - if (I2c.read(BMP085_ADDRESS, 0xF6, 3, buf) != 0) { + if (I2c.read(BMP085_ADDRESS, 0xF6, 3, buf) != 0) { _retry_time = millis() + 1000; I2c.setSpeed(false); - healthy = false; - return; - } + healthy = false; + return; + } - RawPress = (((uint32_t)buf[0] << 16) | ((uint32_t)buf[1] << 8) | ((uint32_t)buf[2])) >> (8 - OVERSAMPLING); + RawPress = (((uint32_t)buf[0] << 16) | ((uint32_t)buf[1] << 8) | ((uint32_t)buf[2])) >> (8 - OVERSAMPLING); } // Send Command to Read Temperature void AP_Baro_BMP085::Command_ReadTemp() { - if (I2c.write(BMP085_ADDRESS, 0xF4, 0x2E) != 0) { - healthy = false; - } + if (I2c.write(BMP085_ADDRESS, 0xF4, 0x2E) != 0) { + healthy = false; + } } // Read Raw Temperature values void AP_Baro_BMP085::ReadTemp() { - uint8_t buf[2]; - int32_t _temp_sensor; + uint8_t buf[2]; + int32_t _temp_sensor; if (!healthy && millis() < _retry_time) { return; } - if (I2c.read(BMP085_ADDRESS, 0xF6, 2, buf) != 0) { + if (I2c.read(BMP085_ADDRESS, 0xF6, 2, buf) != 0) { _retry_time = millis() + 1000; I2c.setSpeed(false); - healthy = false; - return; - } - _temp_sensor = buf[0]; - _temp_sensor = (_temp_sensor << 8) | buf[1]; + healthy = false; + return; + } + _temp_sensor = buf[0]; + _temp_sensor = (_temp_sensor << 8) | buf[1]; - RawTemp = _temp_filter.apply(_temp_sensor); + RawTemp = _temp_filter.apply(_temp_sensor); } // Calculate Temperature and Pressure in real units. void AP_Baro_BMP085::Calculate() { - int32_t x1, x2, x3, b3, b5, b6, p; - uint32_t b4, b7; - int32_t tmp; + int32_t x1, x2, x3, b3, b5, b6, p; + uint32_t b4, b7; + int32_t tmp; - // See Datasheet page 13 for this formulas - // Based also on Jee Labs BMP085 example code. Thanks for share. - // Temperature calculations - x1 = ((int32_t)RawTemp - ac6) * ac5 >> 15; - x2 = ((int32_t) mc << 11) / (x1 + md); - b5 = x1 + x2; - Temp = (b5 + 8) >> 4; + // See Datasheet page 13 for this formulas + // Based also on Jee Labs BMP085 example code. Thanks for share. + // Temperature calculations + x1 = ((int32_t)RawTemp - ac6) * ac5 >> 15; + x2 = ((int32_t) mc << 11) / (x1 + md); + b5 = x1 + x2; + Temp = (b5 + 8) >> 4; - // Pressure calculations - b6 = b5 - 4000; - x1 = (b2 * (b6 * b6 >> 12)) >> 11; - x2 = ac2 * b6 >> 11; - x3 = x1 + x2; - //b3 = (((int32_t) ac1 * 4 + x3)<> 2; // BAD - //b3 = ((int32_t) ac1 * 4 + x3 + 2) >> 2; //OK for OVERSAMPLING=0 - tmp = ac1; - tmp = (tmp*4 + x3)<> 13; - x2 = (b1 * (b6 * b6 >> 12)) >> 16; - x3 = ((x1 + x2) + 2) >> 2; - b4 = (ac4 * (uint32_t) (x3 + 32768)) >> 15; - b7 = ((uint32_t) RawPress - b3) * (50000 >> OVERSAMPLING); - p = b7 < 0x80000000 ? (b7 * 2) / b4 : (b7 / b4) * 2; + // Pressure calculations + b6 = b5 - 4000; + x1 = (b2 * (b6 * b6 >> 12)) >> 11; + x2 = ac2 * b6 >> 11; + x3 = x1 + x2; + //b3 = (((int32_t) ac1 * 4 + x3)<> 2; // BAD + //b3 = ((int32_t) ac1 * 4 + x3 + 2) >> 2; //OK for OVERSAMPLING=0 + tmp = ac1; + tmp = (tmp*4 + x3)<> 13; + x2 = (b1 * (b6 * b6 >> 12)) >> 16; + x3 = ((x1 + x2) + 2) >> 2; + b4 = (ac4 * (uint32_t) (x3 + 32768)) >> 15; + b7 = ((uint32_t) RawPress - b3) * (50000 >> OVERSAMPLING); + p = b7 < 0x80000000 ? (b7 * 2) / b4 : (b7 / b4) * 2; - x1 = (p >> 8) * (p >> 8); - x1 = (x1 * 3038) >> 16; - x2 = (-7357 * p) >> 16; - Press = p + ((x1 + x2 + 3791) >> 4); + x1 = (p >> 8) * (p >> 8); + x1 = (x1 * 3038) >> 16; + x2 = (-7357 * p) >> 16; + Press = p + ((x1 + x2 + 3791) >> 4); }