mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-02 14:08:45 -04:00
AP_Baro : Add init( AP_PeriodicProcess * ) method to interface & implementations
AP_Baro_MS5611 and AP_Baro_BMP085 implement the interface, with stubs only
This commit is contained in:
parent
a626e21e98
commit
d026e48032
@ -2,10 +2,13 @@
|
|||||||
#ifndef __AP_BARO_H__
|
#ifndef __AP_BARO_H__
|
||||||
#define __AP_BARO_H__
|
#define __AP_BARO_H__
|
||||||
|
|
||||||
|
#include "../AP_PeriodicProcess/AP_PeriodicProcess.h"
|
||||||
|
|
||||||
class AP_Baro
|
class AP_Baro
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
AP_Baro() {}
|
AP_Baro() {}
|
||||||
|
virtual void init(AP_PeriodicProcess *scheduler)=0;
|
||||||
virtual uint8_t read() = 0;
|
virtual uint8_t read() = 0;
|
||||||
virtual int32_t get_pressure() = 0;
|
virtual int32_t get_pressure() = 0;
|
||||||
virtual int16_t get_temperature() = 0;
|
virtual int16_t get_temperature() = 0;
|
||||||
|
@ -55,7 +55,7 @@ extern "C" {
|
|||||||
|
|
||||||
|
|
||||||
// Public Methods //////////////////////////////////////////////////////////////
|
// Public Methods //////////////////////////////////////////////////////////////
|
||||||
bool AP_Baro_BMP085::init(int initialiseWireLib, bool apm2_hardware)
|
void AP_Baro_BMP085::init( AP_PeriodicProcess * scheduler )
|
||||||
{
|
{
|
||||||
byte buff[22];
|
byte buff[22];
|
||||||
int i = 0;
|
int i = 0;
|
||||||
@ -69,7 +69,8 @@ bool AP_Baro_BMP085::init(int initialiseWireLib, bool apm2_hardware)
|
|||||||
Wire.beginTransmission(BMP085_ADDRESS);
|
Wire.beginTransmission(BMP085_ADDRESS);
|
||||||
Wire.send(0xAA);
|
Wire.send(0xAA);
|
||||||
if (Wire.endTransmission() != 0) {
|
if (Wire.endTransmission() != 0) {
|
||||||
return false;
|
// Error!
|
||||||
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
Wire.requestFrom(BMP085_ADDRESS, 22);
|
Wire.requestFrom(BMP085_ADDRESS, 22);
|
||||||
@ -80,7 +81,8 @@ bool AP_Baro_BMP085::init(int initialiseWireLib, bool apm2_hardware)
|
|||||||
i++;
|
i++;
|
||||||
}
|
}
|
||||||
if (i != 22) {
|
if (i != 22) {
|
||||||
return false;
|
// Error!
|
||||||
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
ac1 = ((int)buff[0] << 8) | buff[1];
|
ac1 = ((int)buff[0] << 8) | buff[1];
|
||||||
@ -98,7 +100,7 @@ bool AP_Baro_BMP085::init(int initialiseWireLib, bool apm2_hardware)
|
|||||||
//Send a command to read Temp
|
//Send a command to read Temp
|
||||||
Command_ReadTemp();
|
Command_ReadTemp();
|
||||||
BMP085_State = 1;
|
BMP085_State = 1;
|
||||||
return true;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Read the sensor. This is a state machine
|
// Read the sensor. This is a state machine
|
||||||
|
@ -16,6 +16,7 @@ class AP_Baro_BMP085 : public AP_Baro
|
|||||||
|
|
||||||
|
|
||||||
/* AP_Baro public interface: */
|
/* AP_Baro public interface: */
|
||||||
|
void init(AP_PeriodicProcess * scheduler);
|
||||||
uint8_t read();
|
uint8_t read();
|
||||||
int32_t get_pressure();
|
int32_t get_pressure();
|
||||||
int16_t get_temperature();
|
int16_t get_temperature();
|
||||||
|
@ -113,7 +113,7 @@ uint8_t AP_Baro_MS5611::MS5611_Ready()
|
|||||||
|
|
||||||
// Public Methods //////////////////////////////////////////////////////////////
|
// Public Methods //////////////////////////////////////////////////////////////
|
||||||
// SPI should be initialized externally
|
// SPI should be initialized externally
|
||||||
void AP_Baro_MS5611::init()
|
void AP_Baro_MS5611::init( AP_PeriodicProcess *scheduler )
|
||||||
{
|
{
|
||||||
pinMode(MS5611_CS, OUTPUT); // Chip select Pin
|
pinMode(MS5611_CS, OUTPUT); // Chip select Pin
|
||||||
digitalWrite(MS5611_CS, HIGH);
|
digitalWrite(MS5611_CS, HIGH);
|
||||||
|
Loading…
Reference in New Issue
Block a user