Pat Hickey
13 years ago
5 changed files with 215 additions and 0 deletions
@ -0,0 +1,18 @@
@@ -0,0 +1,18 @@
|
||||
|
||||
#ifndef __AP_BARO_H__ |
||||
#define __AP_BARO_H__ |
||||
|
||||
class AP_Baro |
||||
{ |
||||
public: |
||||
AP_Baro() {} |
||||
virtual void init() = 0; |
||||
virtual uint8_t update() = 0; |
||||
virtual int32_t get_pressure() = 0; |
||||
virtual float get_temp() = 0; |
||||
|
||||
}; |
||||
|
||||
#include "AP_Baro_MS5611.h" |
||||
|
||||
#endif // __AP_BARO_H__
|
@ -0,0 +1,96 @@
@@ -0,0 +1,96 @@
|
||||
|
||||
#include "AP_Baro_MS5611.h" |
||||
|
||||
#include "WProgram.h" |
||||
#include <SPI.h> |
||||
|
||||
/* For bringup: chip select tied to PJ0; arduino pin 63 */ |
||||
#define CS_PORT PORTJ |
||||
#define CS_MASK 0x01; |
||||
#define CS_ASSERT do { CS_PORT |= CS_MASK; } while (0) |
||||
#define CS_RELEASE do { CS_PORT &= ~CS_MASK; } while (0) |
||||
|
||||
AP_Baro_MS5611::AP_Baro_MS5611() {} |
||||
|
||||
void AP_Baro_MS5611::init() |
||||
{ |
||||
_send_reset(); |
||||
_start_conversion_D1(); |
||||
} |
||||
|
||||
/* Send reset transaction. */ |
||||
void AP_Baro_MS5611::_send_reset() |
||||
{ |
||||
CS_ASSERT; |
||||
|
||||
byte resetcode = 0x1E; |
||||
byte garbage = SPI.transfer( resetcode ); |
||||
|
||||
delay(3); |
||||
|
||||
CS_RELEASE; |
||||
} |
||||
|
||||
void AP_Baro_MS5611::_start_conversion_D1() |
||||
{ |
||||
CS_ASSERT; |
||||
|
||||
byte conversioncode = 0x48; // D1 OSR = 4096
|
||||
byte garbage = SPI.transfer( conversioncode ); |
||||
|
||||
CS_RELEASE; |
||||
} |
||||
|
||||
void AP_Baro_MS5611::_start_conversion_D2() |
||||
{ |
||||
CS_ASSERT; |
||||
|
||||
byte conversioncode = 0x58; // D2 OSR = 4096
|
||||
byte garbage = SPI.transfer( conversioncode ); |
||||
|
||||
CS_RELEASE; |
||||
} |
||||
|
||||
bool AP_Baro_MS5611::_adc_read( int32_t * raw ) |
||||
{ |
||||
CS_ASSERT; |
||||
|
||||
byte readcode = 0x00; // Just write 0 to read adc.
|
||||
byte garbage = SPI.transfer( readcode ); |
||||
byte b1 = SPI.transfer( 0 ); |
||||
byte b2 = SPI.transfer( 0 ); |
||||
byte b3 = SPI.transfer( 0 ); |
||||
|
||||
CS_RELEASE; |
||||
|
||||
int32_t result = (((int32_t) b1 ) << 16) | |
||||
(((int32_t) b2 ) << 8 ) | |
||||
((int32_t) b3 ); |
||||
|
||||
// Result = 0 if we read before the conversion was complete
|
||||
if (result != 0) { |
||||
*raw = result; |
||||
return true;
|
||||
} |
||||
return false; |
||||
} |
||||
|
||||
uint8_t AP_Baro_MS5611::update() |
||||
{ |
||||
|
||||
|
||||
} |
||||
|
||||
int32_t AP_Baro_MS5611::get_pressure() |
||||
{ |
||||
|
||||
|
||||
} |
||||
|
||||
float AP_Baro_MS5611::get_temp() |
||||
{ |
||||
|
||||
|
||||
} |
||||
|
||||
|
@ -0,0 +1,31 @@
@@ -0,0 +1,31 @@
|
||||
|
||||
#ifndef __AP_BARO_MS5611_H__ |
||||
#define __AP_BARO_MS5611_H__ |
||||
|
||||
#include <stdint.h> |
||||
#include "AP_Baro.h" |
||||
|
||||
class AP_Baro_MS5611 : public AP_Baro |
||||
{ |
||||
public: |
||||
AP_Baro_MS5611(); |
||||
void init(); |
||||
uint8_t update(); |
||||
int32_t get_pressure(); |
||||
float get_temp(); |
||||
|
||||
|
||||
void _send_reset(); |
||||
void _start_conversion_D1(); |
||||
void _start_conversion_D2(); |
||||
bool _adc_read(int32_t * value); |
||||
|
||||
private: |
||||
|
||||
int32_t _raw_pres; |
||||
int32_t _raw_temp; |
||||
|
||||
}; |
||||
|
||||
#endif // __AP_BARO_MS5611_H__
|
||||
|
@ -0,0 +1,68 @@
@@ -0,0 +1,68 @@
|
||||
|
||||
#include <stdint.h> |
||||
#include <FastSerial.h> |
||||
#include <SPI.h> |
||||
#include <AP_Baro.h> // ArduPilot Mega ADC Library |
||||
|
||||
FastSerialPort0(Serial); |
||||
|
||||
AP_Baro_MS5611 baro; |
||||
|
||||
void setup() |
||||
{ |
||||
Serial.begin(115200, 128, 128); |
||||
Serial.println("ArduPilot Mega MeasSense Barometer library test"); |
||||
|
||||
delay(1000); |
||||
|
||||
pinMode(63, OUTPUT); |
||||
digitalWrite(63, HIGH); |
||||
SPI.begin(); |
||||
SPI.setClockDivider(SPI_CLOCK_DIV32); // 500khz for debugging, increase later |
||||
|
||||
baro.init(); |
||||
} |
||||
|
||||
void loop() |
||||
{ |
||||
int32_t pres; |
||||
int32_t temp; |
||||
|
||||
Serial.println("Start Conversions"); |
||||
|
||||
baro._start_conversion_D1(); |
||||
delay(10); |
||||
bool res1 = baro._adc_read(&pres); |
||||
baro._start_conversion_D2(); |
||||
delay(10); |
||||
bool res2 = baro._adc_read(&temp); |
||||
|
||||
if (res1) { |
||||
Serial.printf("Pressure raw value %ld\r\n",pres); |
||||
} else { |
||||
Serial.println("ADC conversion D1 unsuccessful"); |
||||
} |
||||
|
||||
if (res2) { |
||||
Serial.printf("Temp raw value %ld\r\n",pres); |
||||
} else { |
||||
Serial.println("ADC conversion D2 unsuccessful"); |
||||
} |
||||
|
||||
Serial.println("---"); |
||||
delay(250); |
||||
} |
||||
|
||||
void update_and_print() |
||||
{ |
||||
int32_t pres; |
||||
float temp; |
||||
|
||||
baro.update(); |
||||
|
||||
pres = baro.get_pressure(); |
||||
temp = baro.get_temp(); |
||||
Serial.printf("p: %ld t: %f \r\n", pres, temp); |
||||
|
||||
delay(100); |
||||
} |
Loading…
Reference in new issue