mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-05 15:38:29 -04:00
7a77832f45
this allows us to use the MS5611 barometer at its full 100Hz sample rate (80Hz for pressure, 20Hz for temperature). The pressure and temperature values are averaged between reads without adding any latency. Previously the driver would throw away values between readings This also fixes a race condition in reading from the SPI bus that could lead to bad values from the barometer
55 lines
1.3 KiB
C++
55 lines
1.3 KiB
C++
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
|
|
#ifndef __AP_BARO_MS5611_H__
|
|
#define __AP_BARO_MS5611_H__
|
|
|
|
#include "AP_Baro.h"
|
|
|
|
class AP_Baro_MS5611 : public AP_Baro
|
|
{
|
|
public:
|
|
AP_Baro_MS5611() {} // Constructor
|
|
|
|
/* AP_Baro public interface: */
|
|
bool init(AP_PeriodicProcess *scheduler);
|
|
uint8_t read();
|
|
int32_t get_pressure(); // in mbar*100 units
|
|
int16_t get_temperature(); // in celsius degrees * 100 units
|
|
|
|
int32_t get_raw_pressure();
|
|
int32_t get_raw_temp();
|
|
|
|
private:
|
|
/* Asynchronous handler functions: */
|
|
static void _update(uint32_t );
|
|
/* Asynchronous state: */
|
|
static volatile bool _updated;
|
|
static volatile uint8_t _d1_count;
|
|
static volatile uint8_t _d2_count;
|
|
static volatile uint32_t _s_D1, _s_D2;
|
|
static uint8_t _state;
|
|
static uint32_t _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;
|
|
int32_t Alt;
|
|
|
|
int32_t _raw_press;
|
|
int32_t _raw_temp;
|
|
// Internal calibration registers
|
|
uint16_t C1,C2,C3,C4,C5,C6;
|
|
uint32_t D1,D2;
|
|
};
|
|
|
|
#endif // __AP_BARO_MS5611_H__
|