From cca6b2f98c90279b87c41b2a2b18fe5d127e02ea Mon Sep 17 00:00:00 2001 From: Pat Hickey Date: Sat, 5 Nov 2011 18:11:25 -0700 Subject: [PATCH] Added AP_Baro and AP_Baro_MS5611 classes. Incomplete and does not work. --- libraries/AP_Baro/AP_Baro.h | 18 ++++ libraries/AP_Baro/AP_Baro_MS5611.cpp | 96 +++++++++++++++++++ libraries/AP_Baro/AP_Baro_MS5611.h | 31 ++++++ .../AP_Baro_MS5611_test.pde | 68 +++++++++++++ .../examples/AP_Baro_MS5611_test/Makefile | 2 + 5 files changed, 215 insertions(+) create mode 100644 libraries/AP_Baro/AP_Baro.h create mode 100644 libraries/AP_Baro/AP_Baro_MS5611.cpp create mode 100644 libraries/AP_Baro/AP_Baro_MS5611.h create mode 100644 libraries/AP_Baro/examples/AP_Baro_MS5611_test/AP_Baro_MS5611_test.pde create mode 100644 libraries/AP_Baro/examples/AP_Baro_MS5611_test/Makefile diff --git a/libraries/AP_Baro/AP_Baro.h b/libraries/AP_Baro/AP_Baro.h new file mode 100644 index 0000000000..41b6a718f1 --- /dev/null +++ b/libraries/AP_Baro/AP_Baro.h @@ -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__ diff --git a/libraries/AP_Baro/AP_Baro_MS5611.cpp b/libraries/AP_Baro/AP_Baro_MS5611.cpp new file mode 100644 index 0000000000..584415fd18 --- /dev/null +++ b/libraries/AP_Baro/AP_Baro_MS5611.cpp @@ -0,0 +1,96 @@ + +#include "AP_Baro_MS5611.h" + +#include "WProgram.h" +#include + +/* 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() +{ + + +} + + diff --git a/libraries/AP_Baro/AP_Baro_MS5611.h b/libraries/AP_Baro/AP_Baro_MS5611.h new file mode 100644 index 0000000000..26aac96011 --- /dev/null +++ b/libraries/AP_Baro/AP_Baro_MS5611.h @@ -0,0 +1,31 @@ + +#ifndef __AP_BARO_MS5611_H__ +#define __AP_BARO_MS5611_H__ + +#include +#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__ + diff --git a/libraries/AP_Baro/examples/AP_Baro_MS5611_test/AP_Baro_MS5611_test.pde b/libraries/AP_Baro/examples/AP_Baro_MS5611_test/AP_Baro_MS5611_test.pde new file mode 100644 index 0000000000..76c367fada --- /dev/null +++ b/libraries/AP_Baro/examples/AP_Baro_MS5611_test/AP_Baro_MS5611_test.pde @@ -0,0 +1,68 @@ + +#include +#include +#include +#include // 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); +} diff --git a/libraries/AP_Baro/examples/AP_Baro_MS5611_test/Makefile b/libraries/AP_Baro/examples/AP_Baro_MS5611_test/Makefile new file mode 100644 index 0000000000..a5d5e7918b --- /dev/null +++ b/libraries/AP_Baro/examples/AP_Baro_MS5611_test/Makefile @@ -0,0 +1,2 @@ +BOARD = mega2560 +include ../../../AP_Common/Arduino.mk