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
698a231dd6
commit
8f16b736a4
|
|
@ -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 <SPI.h>
|
||||||
|
#include "APM_MS5611.h"
|
||||||
|
|
||||||
/* For bringup: chip select tied to PJ0; arduino pin 63 */
|
uint8_t MS5611_SPI_read(byte reg)
|
||||||
#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()
|
|
||||||
{
|
{
|
||||||
_send_reset();
|
byte dump;
|
||||||
_start_conversion_D1();
|
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. */
|
uint16_t MS5611_SPI_read_16bits(byte reg)
|
||||||
void AP_Baro_MS5611::_send_reset()
|
|
||||||
{
|
{
|
||||||
CS_ASSERT;
|
byte dump,byteH,byteL;
|
||||||
|
uint16_t return_value;
|
||||||
byte resetcode = 0x1E;
|
byte addr = reg; // | 0x80; // Set most significant bit
|
||||||
byte garbage = SPI.transfer( resetcode );
|
digitalWrite(MS5611_CS, LOW);
|
||||||
|
dump = SPI.transfer(addr);
|
||||||
delay(3);
|
byteH = SPI.transfer(0);
|
||||||
|
byteL = SPI.transfer(0);
|
||||||
CS_RELEASE;
|
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 dump,byteH,byteM,byteL;
|
||||||
|
uint32_t return_value;
|
||||||
byte conversioncode = 0x48; // D1 OSR = 4096
|
byte addr = 0x00;
|
||||||
byte garbage = SPI.transfer( conversioncode );
|
digitalWrite(MS5611_CS, LOW);
|
||||||
|
dump = SPI.transfer(addr);
|
||||||
CS_RELEASE;
|
byteH = SPI.transfer(0);
|
||||||
}
|
byteM = SPI.transfer(0);
|
||||||
|
byteL = SPI.transfer(0);
|
||||||
void AP_Baro_MS5611::_start_conversion_D2()
|
digitalWrite(MS5611_CS, HIGH);
|
||||||
{
|
return_value = (((uint32_t)byteH)<<16) | (((uint32_t)byteM)<<8) | (byteL);
|
||||||
CS_ASSERT;
|
return(return_value);
|
||||||
|
|
||||||
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()
|
|
||||||
{
|
|
||||||
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
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 MS5611_CS A2 // Chip select pin (provisional)
|
||||||
#define __AP_BARO_MS5611_H__
|
|
||||||
|
|
||||||
#include <stdint.h>
|
#define CMD_MS5611_RESET 0x1E
|
||||||
#include "AP_Baro.h"
|
#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:
|
private:
|
||||||
AP_Baro_MS5611();
|
// Internal calibration registers
|
||||||
void init();
|
uint16_t C1,C2,C3,C4,C5,C6;
|
||||||
uint8_t update();
|
uint32_t D1,D2;
|
||||||
int32_t get_pressure();
|
void calculate();
|
||||||
float get_temp();
|
uint8_t MS5611_Ready();
|
||||||
|
long MS5611_timer;
|
||||||
|
uint8_t MS5611_State;
|
||||||
|
|
||||||
|
public:
|
||||||
|
//uint16_t C1,C2,C3,C4,C5,C6;
|
||||||
|
//uint32_t D1,D2;
|
||||||
|
int16_t Temp;
|
||||||
|
int32_t Press;
|
||||||
|
int32_t Alt;
|
||||||
|
|
||||||
|
|
||||||
void _send_reset();
|
APM_MS5611_Class(); // Constructor
|
||||||
void _start_conversion_D1();
|
void init();
|
||||||
void _start_conversion_D2();
|
uint8_t read();
|
||||||
bool _adc_read(int32_t * value);
|
uint32_t get_pressure(); // in mbar*100 units
|
||||||
|
uint16_t get_temperature(); // in celsius degrees * 100 units
|
||||||
private:
|
float get_altitude(); // in meter units
|
||||||
|
|
||||||
int32_t _raw_pres;
|
|
||||||
int32_t _raw_temp;
|
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|
||||||
#endif // __AP_BARO_MS5611_H__
|
#endif
|
||||||
|
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue