mirror of https://github.com/ArduPilot/ardupilot
AP_Baro_MS5611: Overwrote with Jose Julio's latest files.
This commit is contained in:
Pat Hickey
Pat Hickey
parent
de6507a47d
commit
34cebd8a3e
|
|
@ -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 <SPI.h>
|
||||
#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);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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 <stdint.h>
|
||||
#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
|
||||
|
|
|
|||
Loading…
Reference in New Issue