mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-05 07:28:29 -04:00
AP_Baro_MS5611: Asynchronous operation, plus c++ style changes.
Yeah, I know this was a big change to make all at once.
This commit is contained in:
parent
d026e48032
commit
f4aaa56b16
@ -17,7 +17,7 @@
|
|||||||
|
|
||||||
Methods:
|
Methods:
|
||||||
init() : Initialization and sensor reset
|
init() : Initialization and sensor reset
|
||||||
read() : Read sensor data and calculate Temperature, Pressure and Altitude
|
read() : Read sensor data and _calculate Temperature, Pressure and Altitude
|
||||||
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
|
||||||
Maximun data output frequency 100Hz
|
Maximun data output frequency 100Hz
|
||||||
@ -27,7 +27,7 @@
|
|||||||
get_altitude() : return altitude in meters
|
get_altitude() : return altitude in meters
|
||||||
|
|
||||||
Internal functions:
|
Internal functions:
|
||||||
calculate() : Calculate Temperature and Pressure (temperature compensated) in real units
|
_calculate() : Calculate Temperature and Pressure (temperature compensated) in real units
|
||||||
|
|
||||||
|
|
||||||
*/
|
*/
|
||||||
@ -51,38 +51,44 @@
|
|||||||
#define CMD_CONVERT_D1_OSR4096 0x48 // Maximun resolution
|
#define CMD_CONVERT_D1_OSR4096 0x48 // Maximun resolution
|
||||||
#define CMD_CONVERT_D2_OSR4096 0x58 // Maximun resolution
|
#define CMD_CONVERT_D2_OSR4096 0x58 // Maximun resolution
|
||||||
|
|
||||||
|
uint32_t AP_Baro_MS5611::_s_D1;
|
||||||
|
uint32_t AP_Baro_MS5611::_s_D2;
|
||||||
|
uint8_t AP_Baro_MS5611::_state;
|
||||||
|
long AP_Baro_MS5611::_timer;
|
||||||
|
bool AP_Baro_MS5611::_sync_access;
|
||||||
|
bool AP_Baro_MS5611::_updated;
|
||||||
|
|
||||||
uint8_t MS5611_SPI_read(byte reg)
|
uint8_t AP_Baro_MS5611::_spi_read(uint8_t reg)
|
||||||
{
|
{
|
||||||
byte dump;
|
uint8_t dump;
|
||||||
uint8_t return_value;
|
uint8_t return_value;
|
||||||
byte addr = reg; // | 0x80; // Set most significant bit
|
uint8_t addr = reg; // | 0x80; // Set most significant bit
|
||||||
digitalWrite(MS5611_CS, LOW);
|
digitalWrite(MS5611_CS, LOW);
|
||||||
dump = SPI.transfer(addr);
|
dump = SPI.transfer(addr);
|
||||||
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 MS5611_SPI_read_16bits(byte reg)
|
uint16_t AP_Baro_MS5611::_spi_read_16bits(uint8_t reg)
|
||||||
{
|
{
|
||||||
byte dump,byteH,byteL;
|
uint8_t dump, byteH, byteL;
|
||||||
uint16_t return_value;
|
uint16_t return_value;
|
||||||
byte addr = reg; // | 0x80; // Set most significant bit
|
uint8_t addr = reg; // | 0x80; // Set most significant bit
|
||||||
digitalWrite(MS5611_CS, LOW);
|
digitalWrite(MS5611_CS, LOW);
|
||||||
dump = SPI.transfer(addr);
|
dump = SPI.transfer(addr);
|
||||||
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 MS5611_SPI_read_ADC()
|
uint32_t AP_Baro_MS5611::_spi_read_adc()
|
||||||
{
|
{
|
||||||
byte dump,byteH,byteM,byteL;
|
uint8_t dump,byteH,byteM,byteL;
|
||||||
uint32_t return_value;
|
uint32_t return_value;
|
||||||
byte addr = 0x00;
|
uint8_t addr = 0x00;
|
||||||
digitalWrite(MS5611_CS, LOW);
|
digitalWrite(MS5611_CS, LOW);
|
||||||
dump = SPI.transfer(addr);
|
dump = SPI.transfer(addr);
|
||||||
byteH = SPI.transfer(0);
|
byteH = SPI.transfer(0);
|
||||||
@ -90,25 +96,22 @@ uint32_t MS5611_SPI_read_ADC()
|
|||||||
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 MS5611_SPI_write(byte reg)
|
void AP_Baro_MS5611::_spi_write(uint8_t reg)
|
||||||
{
|
{
|
||||||
byte dump;
|
uint8_t dump;
|
||||||
digitalWrite(MS5611_CS, LOW);
|
digitalWrite(MS5611_CS, LOW);
|
||||||
dump = SPI.transfer(reg);
|
dump = SPI.transfer(reg);
|
||||||
digitalWrite(MS5611_CS, HIGH);
|
digitalWrite(MS5611_CS, HIGH);
|
||||||
}
|
}
|
||||||
|
|
||||||
// The conversion proccess takes 8.2ms since the command
|
// The conversion proccess takes 8.2ms since the command
|
||||||
uint8_t AP_Baro_MS5611::MS5611_Ready()
|
bool AP_Baro_MS5611::_ready()
|
||||||
{
|
{
|
||||||
if ((millis()-MS5611_timer)>10) // wait for more than 10ms
|
return ( ( millis() - _timer ) > 10 ); // wait for more than 10ms
|
||||||
return(1);
|
|
||||||
else
|
|
||||||
return(0);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// Public Methods //////////////////////////////////////////////////////////////
|
// Public Methods //////////////////////////////////////////////////////////////
|
||||||
@ -119,70 +122,81 @@ void AP_Baro_MS5611::init( AP_PeriodicProcess *scheduler )
|
|||||||
digitalWrite(MS5611_CS, HIGH);
|
digitalWrite(MS5611_CS, HIGH);
|
||||||
delay(1);
|
delay(1);
|
||||||
|
|
||||||
MS5611_SPI_write(CMD_MS5611_RESET);
|
_spi_write(CMD_MS5611_RESET);
|
||||||
delay(4);
|
delay(4);
|
||||||
|
|
||||||
// We read the factory calibration
|
// We read the factory calibration
|
||||||
C1 = MS5611_SPI_read_16bits(CMD_MS5611_PROM_C1);
|
C1 = _spi_read_16bits(CMD_MS5611_PROM_C1);
|
||||||
C2 = MS5611_SPI_read_16bits(CMD_MS5611_PROM_C2);
|
C2 = _spi_read_16bits(CMD_MS5611_PROM_C2);
|
||||||
C3 = MS5611_SPI_read_16bits(CMD_MS5611_PROM_C3);
|
C3 = _spi_read_16bits(CMD_MS5611_PROM_C3);
|
||||||
C4 = MS5611_SPI_read_16bits(CMD_MS5611_PROM_C4);
|
C4 = _spi_read_16bits(CMD_MS5611_PROM_C4);
|
||||||
C5 = MS5611_SPI_read_16bits(CMD_MS5611_PROM_C5);
|
C5 = _spi_read_16bits(CMD_MS5611_PROM_C5);
|
||||||
C6 = MS5611_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
|
||||||
MS5611_SPI_write(CMD_CONVERT_D2_OSR4096);
|
_spi_write(CMD_CONVERT_D2_OSR4096);
|
||||||
MS5611_timer = millis();
|
_timer = millis();
|
||||||
MS5611_State = 1;
|
_state = 1;
|
||||||
Temp=0;
|
Temp=0;
|
||||||
Press=0;
|
Press=0;
|
||||||
|
|
||||||
|
scheduler->register_process( AP_Baro_MS5611::_update );
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
// Read the sensor. This is a state machine
|
// Read the sensor. This is a state machine
|
||||||
// We read one time Temperature (state=1) and then 4 times Pressure (states 2-5)
|
// We read one time Temperature (state=1) and then 4 times Pressure (states 2-5)
|
||||||
// temperature does not change so quickly...
|
// temperature does not change so quickly...
|
||||||
|
void AP_Baro_MS5611::_update(void)
|
||||||
|
{
|
||||||
|
if (_sync_access) return;
|
||||||
|
if (_state == 1){
|
||||||
|
if (_ready()){
|
||||||
|
_s_D2 = _spi_read_adc(); // On state 1 we read temp
|
||||||
|
_state++;
|
||||||
|
_spi_write(CMD_CONVERT_D1_OSR4096); // Command to read pressure
|
||||||
|
_timer = millis();
|
||||||
|
}
|
||||||
|
}else{
|
||||||
|
if (_state == 5){
|
||||||
|
if (_ready()){
|
||||||
|
_s_D1 = _spi_read_adc();
|
||||||
|
_state = 1; // Start again from state = 1
|
||||||
|
_spi_write(CMD_CONVERT_D2_OSR4096); // Command to read temperature
|
||||||
|
_timer = millis();
|
||||||
|
_updated = true; // New pressure reading
|
||||||
|
}
|
||||||
|
}else{
|
||||||
|
if (_ready()){
|
||||||
|
_s_D1 = _spi_read_adc();
|
||||||
|
_state++;
|
||||||
|
_spi_write(CMD_CONVERT_D1_OSR4096); // Command to read pressure
|
||||||
|
_timer = millis();
|
||||||
|
_updated = true; // New pressure reading
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
uint8_t AP_Baro_MS5611::read()
|
uint8_t AP_Baro_MS5611::read()
|
||||||
{
|
{
|
||||||
uint8_t result = 0;
|
_sync_access = true;
|
||||||
|
bool updated = _updated;
|
||||||
if (MS5611_State == 1){
|
_updated = 0;
|
||||||
if (MS5611_Ready()){
|
if (updated > 0) {
|
||||||
D2=MS5611_SPI_read_ADC(); // On state 1 we read temp
|
D1 = _s_D1;
|
||||||
|
D2 = _s_D2;
|
||||||
|
_raw_press = D1;
|
||||||
_raw_temp = D2;
|
_raw_temp = D2;
|
||||||
MS5611_State++;
|
|
||||||
MS5611_SPI_write(CMD_CONVERT_D1_OSR4096); // Command to read pressure
|
|
||||||
MS5611_timer = millis();
|
|
||||||
}
|
}
|
||||||
}else{
|
_sync_access = false;
|
||||||
if (MS5611_State == 5){
|
_calculate();
|
||||||
if (MS5611_Ready()){
|
return updated ? 1 : 0;
|
||||||
D1=MS5611_SPI_read_ADC();
|
|
||||||
_raw_press = D1;
|
|
||||||
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();
|
|
||||||
_raw_press = D1;
|
|
||||||
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).
|
// Calculate Temperature and compensated Pressure in real units (Celsius degrees*100, mbar*100).
|
||||||
void AP_Baro_MS5611::calculate()
|
void AP_Baro_MS5611::_calculate()
|
||||||
{
|
{
|
||||||
int32_t dT;
|
int32_t dT;
|
||||||
long long TEMP; // 64 bits
|
long long TEMP; // 64 bits
|
||||||
|
@ -8,9 +8,9 @@ class AP_Baro_MS5611 : public AP_Baro
|
|||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
AP_Baro_MS5611() {} // Constructor
|
AP_Baro_MS5611() {} // Constructor
|
||||||
void init();
|
|
||||||
|
|
||||||
/* AP_Baro public interface: */
|
/* AP_Baro public interface: */
|
||||||
|
void init(AP_PeriodicProcess *scheduler);
|
||||||
uint8_t read();
|
uint8_t read();
|
||||||
int32_t get_pressure(); // in mbar*100 units
|
int32_t get_pressure(); // in mbar*100 units
|
||||||
int16_t get_temperature(); // in celsius degrees * 100 units
|
int16_t get_temperature(); // in celsius degrees * 100 units
|
||||||
@ -20,6 +20,24 @@ class AP_Baro_MS5611 : public AP_Baro
|
|||||||
int32_t get_raw_temp();
|
int32_t get_raw_temp();
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
/* Asynchronous handler functions: */
|
||||||
|
static void _update(void);
|
||||||
|
static bool _ready();
|
||||||
|
/* Asynchronous state: */
|
||||||
|
static bool _updated;
|
||||||
|
static uint32_t _s_D1, _s_D2;
|
||||||
|
static uint8_t _state;
|
||||||
|
static long _timer;
|
||||||
|
/* Gates access to asynchronous state: */
|
||||||
|
static bool _sync_access;
|
||||||
|
|
||||||
|
/* Serial wrapper functions: */
|
||||||
|
static uint8_t _spi_read(uint8_t reg);
|
||||||
|
static uint16_t _spi_read_16bits(uint8_t reg);
|
||||||
|
static uint32_t _spi_read_adc();
|
||||||
|
static void _spi_write(uint8_t reg);
|
||||||
|
|
||||||
|
void _calculate();
|
||||||
|
|
||||||
int16_t Temp;
|
int16_t Temp;
|
||||||
int32_t Press;
|
int32_t Press;
|
||||||
@ -30,10 +48,6 @@ class AP_Baro_MS5611 : public AP_Baro
|
|||||||
// Internal calibration registers
|
// Internal calibration registers
|
||||||
uint16_t C1,C2,C3,C4,C5,C6;
|
uint16_t C1,C2,C3,C4,C5,C6;
|
||||||
uint32_t D1,D2;
|
uint32_t D1,D2;
|
||||||
void calculate();
|
|
||||||
uint8_t MS5611_Ready();
|
|
||||||
long MS5611_timer;
|
|
||||||
uint8_t MS5611_State;
|
|
||||||
};
|
};
|
||||||
|
|
||||||
#endif // __AP_BARO_MS5611_H__
|
#endif // __AP_BARO_MS5611_H__
|
||||||
|
Loading…
Reference in New Issue
Block a user