diff --git a/libraries/AP_Baro/AP_Baro_MS5611.cpp b/libraries/AP_Baro/AP_Baro_MS5611.cpp index 584415fd18..abecafd5e9 100644 --- a/libraries/AP_Baro/AP_Baro_MS5611.cpp +++ b/libraries/AP_Baro/AP_Baro_MS5611.cpp @@ -1,96 +1,223 @@ +/* + APM_MS5611.cpp - Arduino Library for MS5611-01BA01 absolute pressure sensor + Code by Jose Julio, Pat Hickey and Jordi Muñoz. DIYDrones.com -#include "AP_Baro_MS5611.h" + 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 standard SPI port + Chip Select pin: Analog2 (provisional until Jordi defines the pin)!! + + Variables: + Temp : Calculated temperature (in Celsius degrees * 100) + Press : Calculated pressure (in mbar units * 100) + + + Methods: + init() : Initialization and sensor reset + read() : Read sensor data and calculate Temperature, Pressure and Altitude + This function is optimized so the main host don´t need to wait + You can call this function in your main loop + Maximun data output frequency 100Hz + It returns a 1 if there are new data. + get_pressure() : return pressure in mbar*100 units + get_temperature() : return temperature in celsius degrees*100 units + get_altitude() : return altitude in meters + + Internal functions: + calculate() : Calculate Temperature and Pressure (temperature compensated) in real units + + +*/ -#include "WProgram.h" #include +#include "APM_MS5611.h" -/* For bringup: chip select tied to PJ0; arduino pin 63 */ -#define CS_PORT PORTJ -#define CS_MASK 0x01; -#define CS_ASSERT do { CS_PORT |= CS_MASK; } while (0) -#define CS_RELEASE do { CS_PORT &= ~CS_MASK; } while (0) - -AP_Baro_MS5611::AP_Baro_MS5611() {} - -void AP_Baro_MS5611::init() +uint8_t MS5611_SPI_read(byte reg) { - _send_reset(); - _start_conversion_D1(); + byte dump; + uint8_t return_value; + byte addr = reg; // | 0x80; // Set most significant bit + digitalWrite(MS5611_CS, LOW); + dump = SPI.transfer(addr); + return_value = SPI.transfer(0); + digitalWrite(MS5611_CS, HIGH); + return(return_value); } -/* Send reset transaction. */ -void AP_Baro_MS5611::_send_reset() +uint16_t MS5611_SPI_read_16bits(byte reg) { - CS_ASSERT; - - byte resetcode = 0x1E; - byte garbage = SPI.transfer( resetcode ); - - delay(3); - - CS_RELEASE; + byte dump,byteH,byteL; + uint16_t return_value; + byte addr = reg; // | 0x80; // Set most significant bit + digitalWrite(MS5611_CS, LOW); + dump = SPI.transfer(addr); + byteH = SPI.transfer(0); + byteL = SPI.transfer(0); + digitalWrite(MS5611_CS, HIGH); + return_value = ((uint16_t)byteH<<8) | (byteL); + return(return_value); } -void AP_Baro_MS5611::_start_conversion_D1() +uint32_t MS5611_SPI_read_ADC() { - CS_ASSERT; - - byte conversioncode = 0x48; // D1 OSR = 4096 - byte garbage = SPI.transfer( conversioncode ); - - CS_RELEASE; -} - -void AP_Baro_MS5611::_start_conversion_D2() -{ - CS_ASSERT; - - byte conversioncode = 0x58; // D2 OSR = 4096 - byte garbage = SPI.transfer( conversioncode ); - - CS_RELEASE; -} - -bool AP_Baro_MS5611::_adc_read( int32_t * raw ) -{ - CS_ASSERT; - - byte readcode = 0x00; // Just write 0 to read adc. - byte garbage = SPI.transfer( readcode ); - byte b1 = SPI.transfer( 0 ); - byte b2 = SPI.transfer( 0 ); - byte b3 = SPI.transfer( 0 ); - - CS_RELEASE; - - int32_t result = (((int32_t) b1 ) << 16) | - (((int32_t) b2 ) << 8 ) | - ((int32_t) b3 ); - - // Result = 0 if we read before the conversion was complete - if (result != 0) { - *raw = result; - return true; - } - return false; -} - -uint8_t AP_Baro_MS5611::update() -{ - - -} - -int32_t AP_Baro_MS5611::get_pressure() -{ - - -} - -float AP_Baro_MS5611::get_temp() -{ - - + byte dump,byteH,byteM,byteL; + uint32_t return_value; + byte addr = 0x00; + digitalWrite(MS5611_CS, LOW); + dump = SPI.transfer(addr); + byteH = SPI.transfer(0); + byteM = SPI.transfer(0); + byteL = SPI.transfer(0); + digitalWrite(MS5611_CS, HIGH); + return_value = (((uint32_t)byteH)<<16) | (((uint32_t)byteM)<<8) | (byteL); + return(return_value); } +void MS5611_SPI_write(byte reg) +{ + byte dump; + digitalWrite(MS5611_CS, LOW); + dump = SPI.transfer(reg); + digitalWrite(MS5611_CS, HIGH); +} + +// The conversion proccess takes 8.2ms since the command +uint8_t APM_MS5611_Class::MS5611_Ready() +{ + if ((millis()-MS5611_timer)>10) // wait for more than 10ms + return(1); + else + return(0); +} + +// Constructors //////////////////////////////////////////////////////////////// +APM_MS5611_Class::APM_MS5611_Class() +{ +} + +// Public Methods ////////////////////////////////////////////////////////////// +// SPI should be initialized externally +void APM_MS5611_Class::init() +{ + + pinMode(MS5611_CS, OUTPUT); // Chip select Pin + + MS5611_SPI_write(CMD_MS5611_RESET); + delay(4); + + // We read the factory calibration + C1 = MS5611_SPI_read_16bits(CMD_MS5611_PROM_C1); + C2 = MS5611_SPI_read_16bits(CMD_MS5611_PROM_C2); + C3 = MS5611_SPI_read_16bits(CMD_MS5611_PROM_C3); + C4 = MS5611_SPI_read_16bits(CMD_MS5611_PROM_C4); + C5 = MS5611_SPI_read_16bits(CMD_MS5611_PROM_C5); + C6 = MS5611_SPI_read_16bits(CMD_MS5611_PROM_C6); + + + //Send a command to read Temp first + MS5611_SPI_write(CMD_CONVERT_D2_OSR4096); + MS5611_timer = millis(); + MS5611_State = 1; + Temp=0; + Press=0; +} + + +// Read the sensor. This is a state machine +// We read one time Temperature (state=1) and then 4 times Pressure (states 2-5) +// temperature does not change so quickly... +uint8_t APM_MS5611_Class::read() +{ + uint8_t result = 0; + + if (MS5611_State == 1){ + if (MS5611_Ready()){ + D2=MS5611_SPI_read_ADC(); // On state 1 we read temp + MS5611_State++; + MS5611_SPI_write(CMD_CONVERT_D1_OSR4096); // Command to read pressure + MS5611_timer = millis(); + } + }else{ + if (MS5611_State == 5){ + if (MS5611_Ready()){ + D1=MS5611_SPI_read_ADC(); + calculate(); + MS5611_State = 1; // Start again from state = 1 + MS5611_SPI_write(CMD_CONVERT_D2_OSR4096); // Command to read temperature + MS5611_timer = millis(); + result = 1; // New pressure reading + } + }else{ + if (MS5611_Ready()){ + D1=MS5611_SPI_read_ADC(); + calculate(); + MS5611_State++; + MS5611_SPI_write(CMD_CONVERT_D1_OSR4096); // Command to read pressure + MS5611_timer = millis(); + result = 1; // New pressure reading + } + } + } + return(result); +} + +// Calculate Temperature and compensated Pressure in real units (Celsius degrees*100, mbar*100). +void APM_MS5611_Class::calculate() +{ + int32_t dT; + long long TEMP; // 64 bits + long long OFF; + long long SENS; + long long P; + + // Formulas from manufacturer datasheet + // TODO: optimization with shift operations... (shift operations works well on 64 bits variables?) + // We define parameters as 64 bits to prevent overflow on operations + dT = D2-((long)C5*256); + TEMP = 2000 + ((long long)dT * C6)/8388608; + OFF = (long long)C2 * 65536 + ((long long)C4 * dT ) / 128; + SENS = (long long)C1 * 32768 + ((long long)C3 * dT) / 256; + + /* + if (TEMP < 2000){ // second order temperature compensation + long long T2 = (long long)dT*dT / 2147483648; + long long Aux_64 = (TEMP-2000)*(TEMP-2000); + long long OFF2 = 5*Aux_64/2; + long long SENS2 = 5*Aux_64/4; + TEMP = TEMP - T2; + OFF = OFF - OFF2; + SENS = SENS - SENS2; + } + */ + P = (D1*SENS/2097152 - OFF)/32768; + Temp = TEMP; + Press = P; +} + +uint32_t APM_MS5611_Class::get_pressure() +{ + return(Press); +} + +uint16_t APM_MS5611_Class::get_temperature() +{ + return(Temp); +} + +// Return altitude using the standard 1013.25 mbar at sea level reference +float APM_MS5611_Class::get_altitude() +{ + float tmp_float; + float Altitude; + + tmp_float = (Press / 101325.0); + tmp_float = pow(tmp_float, 0.190295); + Altitude = 44330 * (1.0 - tmp_float); + + return (Altitude); +} + diff --git a/libraries/AP_Baro/AP_Baro_MS5611.h b/libraries/AP_Baro/AP_Baro_MS5611.h index 26aac96011..1637fe3dd5 100644 --- a/libraries/AP_Baro/AP_Baro_MS5611.h +++ b/libraries/AP_Baro/AP_Baro_MS5611.h @@ -1,31 +1,46 @@ +#ifndef APM_MS5611_h +#define APM_MS5611_h -#ifndef __AP_BARO_MS5611_H__ -#define __AP_BARO_MS5611_H__ +#define MS5611_CS A2 // Chip select pin (provisional) -#include -#include "AP_Baro.h" +#define CMD_MS5611_RESET 0x1E +#define CMD_MS5611_PROM_Setup 0xA0 +#define CMD_MS5611_PROM_C1 0xA2 +#define CMD_MS5611_PROM_C2 0xA4 +#define CMD_MS5611_PROM_C3 0xA6 +#define CMD_MS5611_PROM_C4 0xA8 +#define CMD_MS5611_PROM_C5 0xAA +#define CMD_MS5611_PROM_C6 0xAC +#define CMD_MS5611_PROM_CRC 0xAE +#define CMD_CONVERT_D1_OSR4096 0x48 // Maximun resolution +#define CMD_CONVERT_D2_OSR4096 0x58 // Maximun resolution -class AP_Baro_MS5611 : public AP_Baro + +class APM_MS5611_Class { - public: - AP_Baro_MS5611(); - void init(); - uint8_t update(); - int32_t get_pressure(); - float get_temp(); + private: + // Internal calibration registers + uint16_t C1,C2,C3,C4,C5,C6; + uint32_t D1,D2; + void calculate(); + uint8_t MS5611_Ready(); + long MS5611_timer; + uint8_t MS5611_State; - - void _send_reset(); - void _start_conversion_D1(); - void _start_conversion_D2(); - bool _adc_read(int32_t * value); + public: + //uint16_t C1,C2,C3,C4,C5,C6; + //uint32_t D1,D2; + int16_t Temp; + int32_t Press; + int32_t Alt; - private: - - int32_t _raw_pres; - int32_t _raw_temp; + APM_MS5611_Class(); // Constructor + void init(); + uint8_t read(); + uint32_t get_pressure(); // in mbar*100 units + uint16_t get_temperature(); // in celsius degrees * 100 units + float get_altitude(); // in meter units }; -#endif // __AP_BARO_MS5611_H__ - +#endif