2011-12-28 05:32:21 -04:00
|
|
|
|
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
2011-11-27 01:43:34 -04:00
|
|
|
|
/*
|
|
|
|
|
APM_MS5611.cpp - Arduino Library for MS5611-01BA01 absolute pressure sensor
|
|
|
|
|
Code by Jose Julio, Pat Hickey and Jordi Muñoz. DIYDrones.com
|
2011-11-05 22:11:25 -03:00
|
|
|
|
|
2011-11-27 01:43:34 -04:00
|
|
|
|
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)
|
2011-11-05 22:11:25 -03:00
|
|
|
|
|
|
|
|
|
|
2011-11-27 01:43:34 -04:00
|
|
|
|
Methods:
|
|
|
|
|
init() : Initialization and sensor reset
|
2011-12-09 02:35:40 -04:00
|
|
|
|
read() : Read sensor data and _calculate Temperature, Pressure and Altitude
|
2011-11-27 01:43:34 -04:00
|
|
|
|
This function is optimized so the main host don´t need to wait
|
|
|
|
|
You can call this function in your main loop
|
2012-02-12 20:00:06 -04:00
|
|
|
|
Maximum data output frequency 100Hz - this allows maximum oversampling in the chip ADC
|
2011-11-27 01:43:34 -04:00
|
|
|
|
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
|
2011-11-05 22:11:25 -03:00
|
|
|
|
|
2011-11-27 01:43:34 -04:00
|
|
|
|
Internal functions:
|
2011-12-09 02:35:40 -04:00
|
|
|
|
_calculate() : Calculate Temperature and Pressure (temperature compensated) in real units
|
2011-11-05 22:11:25 -03:00
|
|
|
|
|
2011-11-27 01:43:34 -04:00
|
|
|
|
|
|
|
|
|
*/
|
|
|
|
|
|
|
|
|
|
#include <SPI.h>
|
2011-11-27 01:49:40 -04:00
|
|
|
|
#include "AP_Baro_MS5611.h"
|
2011-11-27 01:43:34 -04:00
|
|
|
|
|
2011-11-27 01:49:17 -04:00
|
|
|
|
|
2011-11-30 00:32:42 -04:00
|
|
|
|
/* on APM v.24 MS5661_CS is PG1 (Arduino pin 40) */
|
|
|
|
|
#define MS5611_CS 40
|
2011-11-27 01:49:17 -04:00
|
|
|
|
|
|
|
|
|
#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
|
2012-02-12 20:00:06 -04:00
|
|
|
|
#define CMD_CONVERT_D1_OSR4096 0x48 // Maximum resolution (oversampling)
|
|
|
|
|
#define CMD_CONVERT_D2_OSR4096 0x58 // Maximum resolution (oversampling)
|
2011-11-27 01:49:17 -04:00
|
|
|
|
|
2011-12-09 02:35:40 -04:00
|
|
|
|
uint32_t AP_Baro_MS5611::_s_D1;
|
|
|
|
|
uint32_t AP_Baro_MS5611::_s_D2;
|
|
|
|
|
uint8_t AP_Baro_MS5611::_state;
|
2012-02-13 06:39:18 -04:00
|
|
|
|
uint32_t AP_Baro_MS5611::_timer;
|
2011-12-09 02:35:40 -04:00
|
|
|
|
bool AP_Baro_MS5611::_sync_access;
|
|
|
|
|
bool AP_Baro_MS5611::_updated;
|
2011-11-27 01:49:17 -04:00
|
|
|
|
|
2011-12-09 02:35:40 -04:00
|
|
|
|
uint8_t AP_Baro_MS5611::_spi_read(uint8_t reg)
|
2011-11-05 22:11:25 -03:00
|
|
|
|
{
|
2011-11-27 01:43:34 -04:00
|
|
|
|
uint8_t return_value;
|
2011-12-09 02:35:40 -04:00
|
|
|
|
uint8_t addr = reg; // | 0x80; // Set most significant bit
|
2011-11-27 01:43:34 -04:00
|
|
|
|
digitalWrite(MS5611_CS, LOW);
|
2012-06-19 23:25:19 -03:00
|
|
|
|
SPI.transfer(addr); // discarded
|
2011-11-27 01:43:34 -04:00
|
|
|
|
return_value = SPI.transfer(0);
|
|
|
|
|
digitalWrite(MS5611_CS, HIGH);
|
2011-12-09 02:35:40 -04:00
|
|
|
|
return return_value;
|
2011-11-05 22:11:25 -03:00
|
|
|
|
}
|
|
|
|
|
|
2011-12-09 02:35:40 -04:00
|
|
|
|
uint16_t AP_Baro_MS5611::_spi_read_16bits(uint8_t reg)
|
2011-11-05 22:11:25 -03:00
|
|
|
|
{
|
2012-06-19 23:25:19 -03:00
|
|
|
|
uint8_t byteH, byteL;
|
2011-11-27 01:43:34 -04:00
|
|
|
|
uint16_t return_value;
|
2011-12-09 02:35:40 -04:00
|
|
|
|
uint8_t addr = reg; // | 0x80; // Set most significant bit
|
2011-11-27 01:43:34 -04:00
|
|
|
|
digitalWrite(MS5611_CS, LOW);
|
2012-06-19 23:25:19 -03:00
|
|
|
|
SPI.transfer(addr); // discarded
|
2011-11-27 01:43:34 -04:00
|
|
|
|
byteH = SPI.transfer(0);
|
|
|
|
|
byteL = SPI.transfer(0);
|
|
|
|
|
digitalWrite(MS5611_CS, HIGH);
|
|
|
|
|
return_value = ((uint16_t)byteH<<8) | (byteL);
|
2011-12-09 02:35:40 -04:00
|
|
|
|
return return_value;
|
2011-11-27 01:43:34 -04:00
|
|
|
|
}
|
2011-11-05 22:11:25 -03:00
|
|
|
|
|
2011-12-09 02:35:40 -04:00
|
|
|
|
uint32_t AP_Baro_MS5611::_spi_read_adc()
|
2011-11-27 01:43:34 -04:00
|
|
|
|
{
|
2012-06-19 23:25:19 -03:00
|
|
|
|
uint8_t byteH,byteM,byteL;
|
2011-11-27 01:43:34 -04:00
|
|
|
|
uint32_t return_value;
|
2011-12-09 02:35:40 -04:00
|
|
|
|
uint8_t addr = 0x00;
|
2011-11-27 01:43:34 -04:00
|
|
|
|
digitalWrite(MS5611_CS, LOW);
|
2012-06-19 23:25:19 -03:00
|
|
|
|
SPI.transfer(addr); // discarded
|
2011-11-27 01:43:34 -04:00
|
|
|
|
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);
|
2011-12-09 02:35:40 -04:00
|
|
|
|
return return_value;
|
2011-11-27 01:43:34 -04:00
|
|
|
|
}
|
2011-11-05 22:11:25 -03:00
|
|
|
|
|
|
|
|
|
|
2011-12-09 02:35:40 -04:00
|
|
|
|
void AP_Baro_MS5611::_spi_write(uint8_t reg)
|
2011-11-27 01:43:34 -04:00
|
|
|
|
{
|
|
|
|
|
digitalWrite(MS5611_CS, LOW);
|
2012-06-19 23:25:19 -03:00
|
|
|
|
SPI.transfer(reg); // discarded
|
2011-11-27 01:43:34 -04:00
|
|
|
|
digitalWrite(MS5611_CS, HIGH);
|
2011-11-05 22:11:25 -03:00
|
|
|
|
}
|
|
|
|
|
|
2011-11-27 01:43:34 -04:00
|
|
|
|
// Public Methods //////////////////////////////////////////////////////////////
|
|
|
|
|
// SPI should be initialized externally
|
2011-12-28 05:32:21 -04:00
|
|
|
|
bool AP_Baro_MS5611::init( AP_PeriodicProcess *scheduler )
|
2011-11-05 22:11:25 -03:00
|
|
|
|
{
|
2012-04-30 03:11:55 -03:00
|
|
|
|
scheduler->suspend_timer();
|
|
|
|
|
|
2011-11-27 01:43:34 -04:00
|
|
|
|
pinMode(MS5611_CS, OUTPUT); // Chip select Pin
|
2011-12-28 05:32:21 -04:00
|
|
|
|
digitalWrite(MS5611_CS, HIGH);
|
|
|
|
|
delay(1);
|
2012-02-14 12:55:32 -04:00
|
|
|
|
|
2011-12-09 02:35:40 -04:00
|
|
|
|
_spi_write(CMD_MS5611_RESET);
|
2011-11-27 01:43:34 -04:00
|
|
|
|
delay(4);
|
2011-11-05 22:11:25 -03:00
|
|
|
|
|
2011-11-27 01:43:34 -04:00
|
|
|
|
// We read the factory calibration
|
2012-02-12 20:00:06 -04:00
|
|
|
|
// The on-chip CRC is not used
|
2011-12-09 02:35:40 -04:00
|
|
|
|
C1 = _spi_read_16bits(CMD_MS5611_PROM_C1);
|
|
|
|
|
C2 = _spi_read_16bits(CMD_MS5611_PROM_C2);
|
|
|
|
|
C3 = _spi_read_16bits(CMD_MS5611_PROM_C3);
|
|
|
|
|
C4 = _spi_read_16bits(CMD_MS5611_PROM_C4);
|
|
|
|
|
C5 = _spi_read_16bits(CMD_MS5611_PROM_C5);
|
|
|
|
|
C6 = _spi_read_16bits(CMD_MS5611_PROM_C6);
|
2011-11-27 01:43:34 -04:00
|
|
|
|
|
|
|
|
|
|
2011-12-28 05:32:21 -04:00
|
|
|
|
//Send a command to read Temp first
|
2011-12-09 02:35:40 -04:00
|
|
|
|
_spi_write(CMD_CONVERT_D2_OSR4096);
|
2011-12-21 08:22:37 -04:00
|
|
|
|
_timer = micros();
|
2011-12-09 02:35:40 -04:00
|
|
|
|
_state = 1;
|
2011-12-28 05:32:21 -04:00
|
|
|
|
Temp=0;
|
|
|
|
|
Press=0;
|
2012-02-14 12:55:32 -04:00
|
|
|
|
|
2012-04-30 03:11:55 -03:00
|
|
|
|
scheduler->resume_timer();
|
2011-12-28 05:32:21 -04:00
|
|
|
|
scheduler->register_process( AP_Baro_MS5611::_update );
|
2011-12-09 02:35:40 -04:00
|
|
|
|
|
2011-12-28 05:32:21 -04:00
|
|
|
|
healthy = true;
|
|
|
|
|
return true;
|
2011-11-05 22:11:25 -03:00
|
|
|
|
}
|
|
|
|
|
|
2011-11-27 01:43:34 -04:00
|
|
|
|
|
|
|
|
|
// 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...
|
2011-12-21 08:22:37 -04:00
|
|
|
|
void AP_Baro_MS5611::_update(uint32_t tnow)
|
2011-11-05 22:11:25 -03:00
|
|
|
|
{
|
2011-12-09 02:35:40 -04:00
|
|
|
|
if (_sync_access) return;
|
2011-12-21 08:22:37 -04:00
|
|
|
|
|
2012-02-12 20:00:06 -04:00
|
|
|
|
// Throttle read rate to 100hz maximum.
|
|
|
|
|
// note we use 9500us here not 10000us
|
|
|
|
|
// the read rate will end up at exactly 100hz because the Periodic Timer fires at 1khz
|
|
|
|
|
if (tnow - _timer < 9500) {
|
|
|
|
|
return;
|
2011-12-21 08:22:37 -04:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
_timer = tnow;
|
2012-02-14 12:55:32 -04:00
|
|
|
|
|
2011-12-21 08:22:37 -04:00
|
|
|
|
if (_state == 1) {
|
|
|
|
|
_s_D2 = _spi_read_adc(); // On state 1 we read temp
|
|
|
|
|
_state++;
|
|
|
|
|
_spi_write(CMD_CONVERT_D1_OSR4096); // Command to read pressure
|
|
|
|
|
} else if (_state == 5) {
|
|
|
|
|
_s_D1 = _spi_read_adc();
|
|
|
|
|
_state = 1; // Start again from state = 1
|
|
|
|
|
_spi_write(CMD_CONVERT_D2_OSR4096); // Command to read temperature
|
|
|
|
|
_updated = true; // New pressure reading
|
|
|
|
|
} else {
|
|
|
|
|
_s_D1 = _spi_read_adc();
|
|
|
|
|
_state++;
|
|
|
|
|
_spi_write(CMD_CONVERT_D1_OSR4096); // Command to read pressure
|
|
|
|
|
_updated = true; // New pressure reading
|
|
|
|
|
}
|
2011-12-09 02:35:40 -04:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
uint8_t AP_Baro_MS5611::read()
|
|
|
|
|
{
|
|
|
|
|
_sync_access = true;
|
|
|
|
|
bool updated = _updated;
|
|
|
|
|
_updated = 0;
|
|
|
|
|
if (updated > 0) {
|
|
|
|
|
D1 = _s_D1;
|
|
|
|
|
D2 = _s_D2;
|
|
|
|
|
_raw_press = D1;
|
|
|
|
|
_raw_temp = D2;
|
|
|
|
|
}
|
|
|
|
|
_sync_access = false;
|
|
|
|
|
_calculate();
|
2012-06-19 23:25:19 -03:00
|
|
|
|
if (updated) {
|
|
|
|
|
_last_update = millis();
|
|
|
|
|
}
|
2011-12-09 02:35:40 -04:00
|
|
|
|
return updated ? 1 : 0;
|
2011-11-05 22:11:25 -03:00
|
|
|
|
}
|
|
|
|
|
|
2011-11-27 01:43:34 -04:00
|
|
|
|
// Calculate Temperature and compensated Pressure in real units (Celsius degrees*100, mbar*100).
|
2011-12-09 02:35:40 -04:00
|
|
|
|
void AP_Baro_MS5611::_calculate()
|
2011-11-05 22:11:25 -03:00
|
|
|
|
{
|
2011-11-27 01:43:34 -04:00
|
|
|
|
int32_t dT;
|
2012-02-13 06:39:18 -04:00
|
|
|
|
int64_t TEMP; // 64 bits
|
|
|
|
|
int64_t OFF;
|
|
|
|
|
int64_t SENS;
|
|
|
|
|
int64_t P;
|
2011-11-27 01:43:34 -04:00
|
|
|
|
|
|
|
|
|
// Formulas from manufacturer datasheet
|
2012-02-12 20:00:06 -04:00
|
|
|
|
// as per data sheet some intermediate results require over 32 bits, therefore
|
|
|
|
|
// we define parameters as 64 bits to prevent overflow on operations
|
|
|
|
|
// sub -20c temperature compensation is not included
|
2011-11-27 01:43:34 -04:00
|
|
|
|
dT = D2-((long)C5*256);
|
2012-02-13 06:39:18 -04:00
|
|
|
|
TEMP = 2000 + ((int64_t)dT * C6)/8388608;
|
2012-02-14 12:55:32 -04:00
|
|
|
|
OFF = (int64_t)C2 * 65536 + ((int64_t)C4 * dT ) / 128;
|
2012-02-13 06:39:18 -04:00
|
|
|
|
SENS = (int64_t)C1 * 32768 + ((int64_t)C3 * dT) / 256;
|
2011-11-27 01:43:34 -04:00
|
|
|
|
|
|
|
|
|
if (TEMP < 2000){ // second order temperature compensation
|
2012-02-17 05:46:05 -04:00
|
|
|
|
int64_t T2 = (((int64_t)dT)*dT) >> 31;
|
2012-02-13 06:39:18 -04:00
|
|
|
|
int64_t Aux_64 = (TEMP-2000)*(TEMP-2000);
|
2012-02-17 05:46:05 -04:00
|
|
|
|
int64_t OFF2 = (5*Aux_64)>>1;
|
|
|
|
|
int64_t SENS2 = (5*Aux_64)>>2;
|
2011-11-27 01:43:34 -04:00
|
|
|
|
TEMP = TEMP - T2;
|
|
|
|
|
OFF = OFF - OFF2;
|
|
|
|
|
SENS = SENS - SENS2;
|
|
|
|
|
}
|
2012-02-14 12:55:32 -04:00
|
|
|
|
|
2011-11-27 01:43:34 -04:00
|
|
|
|
P = (D1*SENS/2097152 - OFF)/32768;
|
|
|
|
|
Temp = TEMP;
|
|
|
|
|
Press = P;
|
2011-11-05 22:11:25 -03:00
|
|
|
|
}
|
|
|
|
|
|
2011-11-30 00:32:23 -04:00
|
|
|
|
int32_t AP_Baro_MS5611::get_pressure()
|
2011-11-05 22:11:25 -03:00
|
|
|
|
{
|
2011-11-27 01:43:34 -04:00
|
|
|
|
return(Press);
|
|
|
|
|
}
|
2011-11-05 22:11:25 -03:00
|
|
|
|
|
2011-11-30 00:32:23 -04:00
|
|
|
|
int16_t AP_Baro_MS5611::get_temperature()
|
2011-11-27 01:43:34 -04:00
|
|
|
|
{
|
2011-12-16 23:57:54 -04:00
|
|
|
|
// callers want the temperature in 0.1C units
|
|
|
|
|
return(Temp/10);
|
2011-11-05 22:11:25 -03:00
|
|
|
|
}
|
|
|
|
|
|
2011-11-30 00:32:23 -04:00
|
|
|
|
int32_t AP_Baro_MS5611::get_raw_pressure() {
|
|
|
|
|
return _raw_press;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
int32_t AP_Baro_MS5611::get_raw_temp() {
|
|
|
|
|
return _raw_temp;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|