diff --git a/libraries/AP_Baro/AP_Baro_MS5611.cpp b/libraries/AP_Baro/AP_Baro_MS5611.cpp index bea1b83046..4b3f47b387 100644 --- a/libraries/AP_Baro/AP_Baro_MS5611.cpp +++ b/libraries/AP_Baro/AP_Baro_MS5611.cpp @@ -17,7 +17,7 @@ Methods: 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 You can call this function in your main loop Maximun data output frequency 100Hz @@ -27,7 +27,7 @@ get_altitude() : return altitude in meters 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_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; - byte addr = reg; // | 0x80; // Set most significant bit + uint8_t 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); + 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; - byte addr = reg; // | 0x80; // Set most significant bit + uint8_t 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); + 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; - byte addr = 0x00; + uint8_t addr = 0x00; digitalWrite(MS5611_CS, LOW); dump = SPI.transfer(addr); byteH = SPI.transfer(0); @@ -90,25 +96,22 @@ uint32_t MS5611_SPI_read_ADC() byteL = SPI.transfer(0); digitalWrite(MS5611_CS, HIGH); 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); dump = SPI.transfer(reg); digitalWrite(MS5611_CS, HIGH); } // 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(1); - else - return(0); + return ( ( millis() - _timer ) > 10 ); // wait for more than 10ms } // Public Methods ////////////////////////////////////////////////////////////// @@ -119,70 +122,81 @@ void AP_Baro_MS5611::init( AP_PeriodicProcess *scheduler ) digitalWrite(MS5611_CS, HIGH); delay(1); - MS5611_SPI_write(CMD_MS5611_RESET); + _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); + 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 - MS5611_SPI_write(CMD_CONVERT_D2_OSR4096); - MS5611_timer = millis(); - MS5611_State = 1; + _spi_write(CMD_CONVERT_D2_OSR4096); + _timer = millis(); + _state = 1; Temp=0; Press=0; + + scheduler->register_process( AP_Baro_MS5611::_update ); } // 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 AP_Baro_MS5611::read() +void AP_Baro_MS5611::_update(void) { - uint8_t result = 0; - - if (MS5611_State == 1){ - if (MS5611_Ready()){ - D2=MS5611_SPI_read_ADC(); // On state 1 we read temp - _raw_temp = D2; - MS5611_State++; - MS5611_SPI_write(CMD_CONVERT_D1_OSR4096); // Command to read pressure - MS5611_timer = millis(); + 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 (MS5611_State == 5){ - if (MS5611_Ready()){ - 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 + 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 (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 + 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 } } } - return(result); +} + +uint8_t AP_Baro_MS5611::read() +{ + _sync_access = true; + bool updated = _updated; + _updated = 0; + if (updated > 0) { + D1 = _s_D1; + D2 = _s_D2; + _raw_press = D1; + _raw_temp = D2; + } + _sync_access = false; + _calculate(); + return updated ? 1 : 0; } // 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; long long TEMP; // 64 bits diff --git a/libraries/AP_Baro/AP_Baro_MS5611.h b/libraries/AP_Baro/AP_Baro_MS5611.h index d3e65ca31f..6aa9d28a7c 100644 --- a/libraries/AP_Baro/AP_Baro_MS5611.h +++ b/libraries/AP_Baro/AP_Baro_MS5611.h @@ -8,9 +8,9 @@ class AP_Baro_MS5611 : public AP_Baro { public: AP_Baro_MS5611() {} // Constructor - void init(); /* AP_Baro public interface: */ + void init(AP_PeriodicProcess *scheduler); uint8_t read(); int32_t get_pressure(); // in mbar*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(); 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; int32_t Press; @@ -30,10 +48,6 @@ class AP_Baro_MS5611 : public AP_Baro // 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; }; #endif // __AP_BARO_MS5611_H__