Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

uncrustify libraries/AP_Baro/AP_Baro_MS5611.cpp

Browse Source
This commit is contained in:
uncrustify 2012-08-16 23:09:23 -07:00 committed by Pat Hickey
parent b39411e8d4
commit b0003c020f
1 changed files with 124 additions and 124 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

248
libraries/AP_Baro/AP_Baro_MS5611.cpp
View File

@ -1,36 +1,36 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/* /*
APM_MS5611.cpp - Arduino Library for MS5611-01BA01 absolute pressure sensor * APM_MS5611.cpp - Arduino Library for MS5611-01BA01 absolute pressure sensor
Code by Jose Julio, Pat Hickey and Jordi Muñoz. DIYDrones.com * Code by Jose Julio, Pat Hickey and Jordi Muñoz. DIYDrones.com
*
This library is free software; you can redistribute it and/or * This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public * modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either * License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version. * version 2.1 of the License, or (at your option) any later version.
*
Sensor is conected to standard SPI port * Sensor is conected to standard SPI port
Chip Select pin: Analog2 (provisional until Jordi defines the pin)!! * Chip Select pin: Analog2 (provisional until Jordi defines the pin)!!
*
Variables: * Variables:
Temp : Calculated temperature (in Celsius degrees * 100) * Temp : Calculated temperature (in Celsius degrees * 100)
Press : Calculated pressure (in mbar units * 100) * Press : Calculated pressure (in mbar units * 100)
*
*
Methods: * Methods:
init() : Initialization and sensor reset * init() : Initialization and sensor reset
read() : Read sensor data and _calculate Temperature, Pressure * read() : Read sensor data and _calculate Temperature, Pressure
This function is optimized so the main host don´t need to wait * This function is optimized so the main host don´t need to wait
You can call this function in your main loop * You can call this function in your main loop
Maximum data output frequency 100Hz - this allows maximum oversampling in the chip ADC * Maximum data output frequency 100Hz - this allows maximum oversampling in the chip ADC
It returns a 1 if there are new data. * It returns a 1 if there are new data.
get_pressure() : return pressure in mbar*100 units * get_pressure() : return pressure in mbar*100 units
get_temperature() : return temperature in celsius degrees*100 units * get_temperature() : return temperature in celsius degrees*100 units
*
Internal functions: * Internal functions:
_calculate() : Calculate Temperature and Pressure (temperature compensated) in real units * _calculate() : Calculate Temperature and Pressure (temperature compensated) in real units
*
*
*/ */
#include <FastSerial.h> #include <FastSerial.h>
#include <SPI.h> #include <SPI.h>
@ -54,59 +54,59 @@
uint32_t volatile AP_Baro_MS5611::_s_D1; uint32_t volatile AP_Baro_MS5611::_s_D1;
uint32_t volatile AP_Baro_MS5611::_s_D2; uint32_t volatile AP_Baro_MS5611::_s_D2;
uint8_t volatile AP_Baro_MS5611::_d1_count; uint8_t volatile AP_Baro_MS5611::_d1_count;
uint8_t volatile AP_Baro_MS5611::_d2_count; uint8_t volatile AP_Baro_MS5611::_d2_count;
uint8_t AP_Baro_MS5611::_state; uint8_t AP_Baro_MS5611::_state;
uint32_t AP_Baro_MS5611::_timer; uint32_t AP_Baro_MS5611::_timer;
bool AP_Baro_MS5611::_sync_access; bool AP_Baro_MS5611::_sync_access;
bool volatile AP_Baro_MS5611::_updated; bool volatile AP_Baro_MS5611::_updated;
uint8_t AP_Baro_MS5611::_spi_read(uint8_t reg) uint8_t AP_Baro_MS5611::_spi_read(uint8_t reg)
{ {
uint8_t return_value; uint8_t return_value;
uint8_t addr = reg; // | 0x80; // Set most significant bit uint8_t addr = reg; // | 0x80; // Set most significant bit
digitalWrite(MS5611_CS, LOW); digitalWrite(MS5611_CS, LOW);
SPI.transfer(addr); // discarded SPI.transfer(addr); // discarded
return_value = SPI.transfer(0); return_value = SPI.transfer(0);
digitalWrite(MS5611_CS, HIGH); digitalWrite(MS5611_CS, HIGH);
return return_value; return return_value;
} }
uint16_t AP_Baro_MS5611::_spi_read_16bits(uint8_t reg) uint16_t AP_Baro_MS5611::_spi_read_16bits(uint8_t reg)
{ {
uint8_t byteH, byteL; uint8_t byteH, byteL;
uint16_t return_value; uint16_t return_value;
uint8_t addr = reg; // | 0x80; // Set most significant bit uint8_t addr = reg; // | 0x80; // Set most significant bit
digitalWrite(MS5611_CS, LOW); digitalWrite(MS5611_CS, LOW);
SPI.transfer(addr); // discarded SPI.transfer(addr); // discarded
byteH = SPI.transfer(0); byteH = SPI.transfer(0);
byteL = SPI.transfer(0); byteL = SPI.transfer(0);
digitalWrite(MS5611_CS, HIGH); digitalWrite(MS5611_CS, HIGH);
return_value = ((uint16_t)byteH<<8) | (byteL); return_value = ((uint16_t)byteH<<8) | (byteL);
return return_value; return return_value;
} }
uint32_t AP_Baro_MS5611::_spi_read_adc() uint32_t AP_Baro_MS5611::_spi_read_adc()
{ {
uint8_t byteH,byteM,byteL; uint8_t byteH,byteM,byteL;
uint32_t return_value; uint32_t return_value;
uint8_t addr = 0x00; uint8_t addr = 0x00;
digitalWrite(MS5611_CS, LOW); digitalWrite(MS5611_CS, LOW);
SPI.transfer(addr); // discarded SPI.transfer(addr); // discarded
byteH = SPI.transfer(0); byteH = SPI.transfer(0);
byteM = SPI.transfer(0); byteM = SPI.transfer(0);
byteL = SPI.transfer(0); byteL = SPI.transfer(0);
digitalWrite(MS5611_CS, HIGH); digitalWrite(MS5611_CS, HIGH);
return_value = (((uint32_t)byteH)<<16) | (((uint32_t)byteM)<<8) | (byteL); return_value = (((uint32_t)byteH)<<16) | (((uint32_t)byteM)<<8) | (byteL);
return return_value; return return_value;
} }
void AP_Baro_MS5611::_spi_write(uint8_t reg) void AP_Baro_MS5611::_spi_write(uint8_t reg)
{ {
digitalWrite(MS5611_CS, LOW); digitalWrite(MS5611_CS, LOW);
SPI.transfer(reg); // discarded SPI.transfer(reg); // discarded
digitalWrite(MS5611_CS, HIGH); digitalWrite(MS5611_CS, HIGH);
} }
// Public Methods ////////////////////////////////////////////////////////////// // Public Methods //////////////////////////////////////////////////////////////
@ -115,29 +115,29 @@ bool AP_Baro_MS5611::init( AP_PeriodicProcess *scheduler )
{ {
scheduler->suspend_timer(); scheduler->suspend_timer();
pinMode(MS5611_CS, OUTPUT); // Chip select Pin pinMode(MS5611_CS, OUTPUT); // Chip select Pin
digitalWrite(MS5611_CS, HIGH); digitalWrite(MS5611_CS, HIGH);
delay(1); delay(1);
_spi_write(CMD_MS5611_RESET); _spi_write(CMD_MS5611_RESET);
delay(4); delay(4);
// We read the factory calibration // We read the factory calibration
// The on-chip CRC is not used // The on-chip CRC is not used
C1 = _spi_read_16bits(CMD_MS5611_PROM_C1); C1 = _spi_read_16bits(CMD_MS5611_PROM_C1);
C2 = _spi_read_16bits(CMD_MS5611_PROM_C2); C2 = _spi_read_16bits(CMD_MS5611_PROM_C2);
C3 = _spi_read_16bits(CMD_MS5611_PROM_C3); C3 = _spi_read_16bits(CMD_MS5611_PROM_C3);
C4 = _spi_read_16bits(CMD_MS5611_PROM_C4); C4 = _spi_read_16bits(CMD_MS5611_PROM_C4);
C5 = _spi_read_16bits(CMD_MS5611_PROM_C5); C5 = _spi_read_16bits(CMD_MS5611_PROM_C5);
C6 = _spi_read_16bits(CMD_MS5611_PROM_C6); C6 = _spi_read_16bits(CMD_MS5611_PROM_C6);
//Send a command to read Temp first //Send a command to read Temp first
_spi_write(CMD_CONVERT_D2_OSR4096); _spi_write(CMD_CONVERT_D2_OSR4096);
_timer = micros(); _timer = micros();
_state = 0; _state = 0;
Temp=0; Temp=0;
Press=0; Press=0;
_s_D1 = 0; _s_D1 = 0;
_s_D2 = 0; _s_D2 = 0;
@ -145,12 +145,12 @@ bool AP_Baro_MS5611::init( AP_PeriodicProcess *scheduler )
_d2_count = 0; _d2_count = 0;
scheduler->resume_timer(); 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 // wait for at least one value to be read
while (!_updated) ; while (!_updated) ;
healthy = true; healthy = true;
return true; return true;
} }
@ -166,13 +166,13 @@ void AP_Baro_MS5611::_update(uint32_t tnow)
// note we use 9500us here not 10000us // note we use 9500us here not 10000us
// the read rate will end up at exactly 100hz because the Periodic Timer fires at 1khz // the read rate will end up at exactly 100hz because the Periodic Timer fires at 1khz
if (tnow - _timer < 9500) { if (tnow - _timer < 9500) {
return; return;
} }
_timer = tnow; _timer = tnow;
if (_state == 0) { 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++; _d2_count++;
if (_d2_count == 32) { if (_d2_count == 32) {
// we have summed 32 values. This only happens // we have summed 32 values. This only happens
@ -181,10 +181,10 @@ void AP_Baro_MS5611::_update(uint32_t tnow)
_s_D2 >>= 1; _s_D2 >>= 1;
_d2_count = 16; _d2_count = 16;
} }
_state++; _state++;
_spi_write(CMD_CONVERT_D1_OSR4096); // Command to read pressure _spi_write(CMD_CONVERT_D1_OSR4096); // Command to read pressure
} else { } else {
_s_D1 += _spi_read_adc(); _s_D1 += _spi_read_adc();
_d1_count++; _d1_count++;
if (_d1_count == 128) { if (_d1_count == 128) {
// we have summed 128 values. This only happens // we have summed 128 values. This only happens
@ -193,13 +193,13 @@ void AP_Baro_MS5611::_update(uint32_t tnow)
_s_D1 >>= 1; _s_D1 >>= 1;
_d1_count = 64; _d1_count = 64;
} }
_state++; _state++;
_updated = true; // New pressure reading _updated = true; // New pressure reading
if (_state == 5) { 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; _state = 0;
} else { } 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). // Calculate Temperature and compensated Pressure in real units (Celsius degrees*100, mbar*100).
void AP_Baro_MS5611::_calculate() void AP_Baro_MS5611::_calculate()
{ {
float dT; float dT;
float TEMP; float TEMP;
float OFF; float OFF;
float SENS; float SENS;
float P; float P;
// Formulas from manufacturer datasheet // Formulas from manufacturer datasheet
// sub -20c temperature compensation is not included // sub -20c temperature compensation is not included
// we do the calculations using floating point // we do the calculations using floating point
// as this is much faster on an AVR2560, and also allows // as this is much faster on an AVR2560, and also allows
// us to take advantage of the averaging of D1 and D1 over // us to take advantage of the averaging of D1 and D1 over
// multiple samples, giving us more precision // multiple samples, giving us more precision
dT = D2-(((uint32_t)C5)<<8); dT = D2-(((uint32_t)C5)<<8);
TEMP = (dT * C6)/8388608; TEMP = (dT * C6)/8388608;
OFF = C2 * 65536.0 + (C4 * dT) / 128; OFF = C2 * 65536.0 + (C4 * dT) / 128;
SENS = C1 * 32768.0 + (C3 * dT) / 256; SENS = C1 * 32768.0 + (C3 * dT) / 256;
if (TEMP < 0) { if (TEMP < 0) {
// second order temperature compensation when under 20 degrees C // second order temperature compensation when under 20 degrees C
float T2 = (dT*dT) / 0x80000000; float T2 = (dT*dT) / 0x80000000;
float Aux = TEMP*TEMP; float Aux = TEMP*TEMP;
float OFF2 = 2.5*Aux; float OFF2 = 2.5*Aux;
float SENS2 = 1.25*Aux; float SENS2 = 1.25*Aux;
TEMP = TEMP - T2; TEMP = TEMP - T2;
OFF = OFF - OFF2; OFF = OFF - OFF2;
SENS = SENS - SENS2; SENS = SENS - SENS2;
} }
P = (D1*SENS/2097152 - OFF)/32768; P = (D1*SENS/2097152 - OFF)/32768;
Temp = TEMP + 2000; Temp = TEMP + 2000;
Press = P; Press = P;
} }
float AP_Baro_MS5611::get_pressure() float AP_Baro_MS5611::get_pressure()
{ {
return Press; return Press;
} }
float AP_Baro_MS5611::get_temperature() float AP_Baro_MS5611::get_temperature()
{ {
// callers want the temperature in 0.1C units // callers want the temperature in 0.1C units
return Temp/10; return Temp/10;
} }
int32_t AP_Baro_MS5611::get_raw_pressure() { int32_t AP_Baro_MS5611::get_raw_pressure() {
return _raw_press; return _raw_press;
} }
int32_t AP_Baro_MS5611::get_raw_temp() { int32_t AP_Baro_MS5611::get_raw_temp() {
return _raw_temp; return _raw_temp;
} }