mirror of https://github.com/ArduPilot/ardupilot
uncrustify libraries/AP_Baro/AP_Baro_MS5611.cpp
This commit is contained in:
parent
46c682454a
commit
bd4f1179b1
|
@ -1,36 +1,36 @@
|
|||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
/*
|
||||
APM_MS5611.cpp - Arduino Library for MS5611-01BA01 absolute pressure sensor
|
||||
Code by Jose Julio, Pat Hickey and Jordi Muñoz. 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 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
|
||||
This function is optimized so the main host don´t need to wait
|
||||
You can call this function in your main loop
|
||||
Maximum data output frequency 100Hz - this allows maximum oversampling in the chip ADC
|
||||
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
|
||||
|
||||
Internal functions:
|
||||
_calculate() : Calculate Temperature and Pressure (temperature compensated) in real units
|
||||
|
||||
|
||||
*/
|
||||
* APM_MS5611.cpp - Arduino Library for MS5611-01BA01 absolute pressure sensor
|
||||
* Code by Jose Julio, Pat Hickey and Jordi Muñoz. 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 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
|
||||
* This function is optimized so the main host don´t need to wait
|
||||
* You can call this function in your main loop
|
||||
* Maximum data output frequency 100Hz - this allows maximum oversampling in the chip ADC
|
||||
* 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
|
||||
*
|
||||
* Internal functions:
|
||||
* _calculate() : Calculate Temperature and Pressure (temperature compensated) in real units
|
||||
*
|
||||
*
|
||||
*/
|
||||
|
||||
#include <FastSerial.h>
|
||||
#include <SPI.h>
|
||||
|
@ -54,59 +54,59 @@
|
|||
|
||||
uint32_t volatile AP_Baro_MS5611::_s_D1;
|
||||
uint32_t volatile AP_Baro_MS5611::_s_D2;
|
||||
uint8_t volatile AP_Baro_MS5611::_d1_count;
|
||||
uint8_t volatile AP_Baro_MS5611::_d2_count;
|
||||
uint8_t AP_Baro_MS5611::_state;
|
||||
uint8_t volatile AP_Baro_MS5611::_d1_count;
|
||||
uint8_t volatile AP_Baro_MS5611::_d2_count;
|
||||
uint8_t AP_Baro_MS5611::_state;
|
||||
uint32_t AP_Baro_MS5611::_timer;
|
||||
bool AP_Baro_MS5611::_sync_access;
|
||||
bool volatile AP_Baro_MS5611::_updated;
|
||||
bool AP_Baro_MS5611::_sync_access;
|
||||
bool volatile AP_Baro_MS5611::_updated;
|
||||
|
||||
uint8_t AP_Baro_MS5611::_spi_read(uint8_t reg)
|
||||
{
|
||||
uint8_t return_value;
|
||||
uint8_t addr = reg; // | 0x80; // Set most significant bit
|
||||
digitalWrite(MS5611_CS, LOW);
|
||||
SPI.transfer(addr); // discarded
|
||||
return_value = SPI.transfer(0);
|
||||
digitalWrite(MS5611_CS, HIGH);
|
||||
return return_value;
|
||||
uint8_t return_value;
|
||||
uint8_t addr = reg; // | 0x80; // Set most significant bit
|
||||
digitalWrite(MS5611_CS, LOW);
|
||||
SPI.transfer(addr); // discarded
|
||||
return_value = SPI.transfer(0);
|
||||
digitalWrite(MS5611_CS, HIGH);
|
||||
return return_value;
|
||||
}
|
||||
|
||||
uint16_t AP_Baro_MS5611::_spi_read_16bits(uint8_t reg)
|
||||
{
|
||||
uint8_t byteH, byteL;
|
||||
uint16_t return_value;
|
||||
uint8_t addr = reg; // | 0x80; // Set most significant bit
|
||||
digitalWrite(MS5611_CS, LOW);
|
||||
SPI.transfer(addr); // discarded
|
||||
byteH = SPI.transfer(0);
|
||||
byteL = SPI.transfer(0);
|
||||
digitalWrite(MS5611_CS, HIGH);
|
||||
return_value = ((uint16_t)byteH<<8) | (byteL);
|
||||
return return_value;
|
||||
uint8_t byteH, byteL;
|
||||
uint16_t return_value;
|
||||
uint8_t addr = reg; // | 0x80; // Set most significant bit
|
||||
digitalWrite(MS5611_CS, LOW);
|
||||
SPI.transfer(addr); // discarded
|
||||
byteH = SPI.transfer(0);
|
||||
byteL = SPI.transfer(0);
|
||||
digitalWrite(MS5611_CS, HIGH);
|
||||
return_value = ((uint16_t)byteH<<8) | (byteL);
|
||||
return return_value;
|
||||
}
|
||||
|
||||
uint32_t AP_Baro_MS5611::_spi_read_adc()
|
||||
{
|
||||
uint8_t byteH,byteM,byteL;
|
||||
uint32_t return_value;
|
||||
uint8_t addr = 0x00;
|
||||
digitalWrite(MS5611_CS, LOW);
|
||||
SPI.transfer(addr); // discarded
|
||||
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;
|
||||
uint8_t byteH,byteM,byteL;
|
||||
uint32_t return_value;
|
||||
uint8_t addr = 0x00;
|
||||
digitalWrite(MS5611_CS, LOW);
|
||||
SPI.transfer(addr); // discarded
|
||||
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 AP_Baro_MS5611::_spi_write(uint8_t reg)
|
||||
{
|
||||
digitalWrite(MS5611_CS, LOW);
|
||||
SPI.transfer(reg); // discarded
|
||||
digitalWrite(MS5611_CS, HIGH);
|
||||
digitalWrite(MS5611_CS, LOW);
|
||||
SPI.transfer(reg); // discarded
|
||||
digitalWrite(MS5611_CS, HIGH);
|
||||
}
|
||||
|
||||
// Public Methods //////////////////////////////////////////////////////////////
|
||||
|
@ -115,29 +115,29 @@ bool AP_Baro_MS5611::init( AP_PeriodicProcess *scheduler )
|
|||
{
|
||||
scheduler->suspend_timer();
|
||||
|
||||
pinMode(MS5611_CS, OUTPUT); // Chip select Pin
|
||||
digitalWrite(MS5611_CS, HIGH);
|
||||
delay(1);
|
||||
pinMode(MS5611_CS, OUTPUT); // Chip select Pin
|
||||
digitalWrite(MS5611_CS, HIGH);
|
||||
delay(1);
|
||||
|
||||
_spi_write(CMD_MS5611_RESET);
|
||||
delay(4);
|
||||
_spi_write(CMD_MS5611_RESET);
|
||||
delay(4);
|
||||
|
||||
// We read the factory calibration
|
||||
// The on-chip CRC is not used
|
||||
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);
|
||||
// We read the factory calibration
|
||||
// The on-chip CRC is not used
|
||||
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);
|
||||
|
||||
|
||||
//Send a command to read Temp first
|
||||
_spi_write(CMD_CONVERT_D2_OSR4096);
|
||||
_timer = micros();
|
||||
_state = 0;
|
||||
Temp=0;
|
||||
Press=0;
|
||||
//Send a command to read Temp first
|
||||
_spi_write(CMD_CONVERT_D2_OSR4096);
|
||||
_timer = micros();
|
||||
_state = 0;
|
||||
Temp=0;
|
||||
Press=0;
|
||||
|
||||
_s_D1 = 0;
|
||||
_s_D2 = 0;
|
||||
|
@ -145,12 +145,12 @@ bool AP_Baro_MS5611::init( AP_PeriodicProcess *scheduler )
|
|||
_d2_count = 0;
|
||||
|
||||
scheduler->resume_timer();
|
||||
scheduler->register_process( AP_Baro_MS5611::_update );
|
||||
scheduler->register_process( AP_Baro_MS5611::_update );
|
||||
|
||||
// wait for at least one value to be read
|
||||
while (!_updated) ;
|
||||
|
||||
healthy = true;
|
||||
healthy = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
|
@ -166,13 +166,13 @@ void AP_Baro_MS5611::_update(uint32_t tnow)
|
|||
// 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;
|
||||
return;
|
||||
}
|
||||
|
||||
_timer = tnow;
|
||||
|
||||
if (_state == 0) {
|
||||
_s_D2 += _spi_read_adc(); // On state 0 we read temp
|
||||
_s_D2 += _spi_read_adc(); // On state 0 we read temp
|
||||
_d2_count++;
|
||||
if (_d2_count == 32) {
|
||||
// we have summed 32 values. This only happens
|
||||
|
@ -181,10 +181,10 @@ void AP_Baro_MS5611::_update(uint32_t tnow)
|
|||
_s_D2 >>= 1;
|
||||
_d2_count = 16;
|
||||
}
|
||||
_state++;
|
||||
_spi_write(CMD_CONVERT_D1_OSR4096); // Command to read pressure
|
||||
_state++;
|
||||
_spi_write(CMD_CONVERT_D1_OSR4096); // Command to read pressure
|
||||
} else {
|
||||
_s_D1 += _spi_read_adc();
|
||||
_s_D1 += _spi_read_adc();
|
||||
_d1_count++;
|
||||
if (_d1_count == 128) {
|
||||
// we have summed 128 values. This only happens
|
||||
|
@ -193,13 +193,13 @@ void AP_Baro_MS5611::_update(uint32_t tnow)
|
|||
_s_D1 >>= 1;
|
||||
_d1_count = 64;
|
||||
}
|
||||
_state++;
|
||||
_updated = true; // New pressure reading
|
||||
_state++;
|
||||
_updated = true; // New pressure reading
|
||||
if (_state == 5) {
|
||||
_spi_write(CMD_CONVERT_D2_OSR4096); // Command to read temperature
|
||||
_spi_write(CMD_CONVERT_D2_OSR4096); // Command to read temperature
|
||||
_state = 0;
|
||||
} else {
|
||||
_spi_write(CMD_CONVERT_D1_OSR4096); // Command to read pressure
|
||||
_spi_write(CMD_CONVERT_D1_OSR4096); // Command to read pressure
|
||||
}
|
||||
}
|
||||
}
|
||||
|
@ -241,57 +241,57 @@ uint8_t AP_Baro_MS5611::read()
|
|||
// Calculate Temperature and compensated Pressure in real units (Celsius degrees*100, mbar*100).
|
||||
void AP_Baro_MS5611::_calculate()
|
||||
{
|
||||
float dT;
|
||||
float TEMP;
|
||||
float OFF;
|
||||
float SENS;
|
||||
float P;
|
||||
float dT;
|
||||
float TEMP;
|
||||
float OFF;
|
||||
float SENS;
|
||||
float P;
|
||||
|
||||
// Formulas from manufacturer datasheet
|
||||
// Formulas from manufacturer datasheet
|
||||
// sub -20c temperature compensation is not included
|
||||
|
||||
// we do the calculations using floating point
|
||||
// as this is much faster on an AVR2560, and also allows
|
||||
// us to take advantage of the averaging of D1 and D1 over
|
||||
// multiple samples, giving us more precision
|
||||
dT = D2-(((uint32_t)C5)<<8);
|
||||
TEMP = (dT * C6)/8388608;
|
||||
OFF = C2 * 65536.0 + (C4 * dT) / 128;
|
||||
SENS = C1 * 32768.0 + (C3 * dT) / 256;
|
||||
dT = D2-(((uint32_t)C5)<<8);
|
||||
TEMP = (dT * C6)/8388608;
|
||||
OFF = C2 * 65536.0 + (C4 * dT) / 128;
|
||||
SENS = C1 * 32768.0 + (C3 * dT) / 256;
|
||||
|
||||
if (TEMP < 0) {
|
||||
if (TEMP < 0) {
|
||||
// second order temperature compensation when under 20 degrees C
|
||||
float T2 = (dT*dT) / 0x80000000;
|
||||
float Aux = TEMP*TEMP;
|
||||
float OFF2 = 2.5*Aux;
|
||||
float SENS2 = 1.25*Aux;
|
||||
TEMP = TEMP - T2;
|
||||
OFF = OFF - OFF2;
|
||||
SENS = SENS - SENS2;
|
||||
}
|
||||
float T2 = (dT*dT) / 0x80000000;
|
||||
float Aux = TEMP*TEMP;
|
||||
float OFF2 = 2.5*Aux;
|
||||
float SENS2 = 1.25*Aux;
|
||||
TEMP = TEMP - T2;
|
||||
OFF = OFF - OFF2;
|
||||
SENS = SENS - SENS2;
|
||||
}
|
||||
|
||||
P = (D1*SENS/2097152 - OFF)/32768;
|
||||
Temp = TEMP + 2000;
|
||||
Press = P;
|
||||
P = (D1*SENS/2097152 - OFF)/32768;
|
||||
Temp = TEMP + 2000;
|
||||
Press = P;
|
||||
}
|
||||
|
||||
float AP_Baro_MS5611::get_pressure()
|
||||
{
|
||||
return Press;
|
||||
return Press;
|
||||
}
|
||||
|
||||
float AP_Baro_MS5611::get_temperature()
|
||||
{
|
||||
// callers want the temperature in 0.1C units
|
||||
return Temp/10;
|
||||
// callers want the temperature in 0.1C units
|
||||
return Temp/10;
|
||||
}
|
||||
|
||||
int32_t AP_Baro_MS5611::get_raw_pressure() {
|
||||
return _raw_press;
|
||||
return _raw_press;
|
||||
}
|
||||
|
||||
int32_t AP_Baro_MS5611::get_raw_temp() {
|
||||
return _raw_temp;
|
||||
return _raw_temp;
|
||||
}
|
||||
|
||||
|
||||
|
|
Loading…
Reference in New Issue