Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Baro: port to AP_HAL

Browse Source
This commit is contained in:
Pat Hickey 2012-10-11 10:53:21 -07:00 committed by Andrew Tridgell
parent c56c4ae240
commit 5d40074e4e
14 changed files with 166 additions and 161 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
libraries/AP_Baro/AP_Baro.cpp
View File

@ -8,9 +8,12 @@
* of the License, or (at your option) any later version. * of the License, or (at your option) any later version.
*/ */
#include <FastSerial.h> #include <math.h>
#include <AP_Common.h> #include <AP_Common.h>
#include <AP_Baro.h> #include <AP_Baro.h>
#include <AP_HAL.h>
extern const AP_HAL::HAL& hal;
// table of user settable parameters // table of user settable parameters
const AP_Param::GroupInfo AP_Baro::var_info[] PROGMEM = { const AP_Param::GroupInfo AP_Baro::var_info[] PROGMEM = {
@ -33,7 +36,7 @@ const AP_Param::GroupInfo AP_Baro::var_info[] PROGMEM = {
// calibrate the barometer. This must be called at least once before // calibrate the barometer. This must be called at least once before
// the altitude() or climb_rate() interfaces can be used // the altitude() or climb_rate() interfaces can be used
void AP_Baro::calibrate(void (*callback)(unsigned long t)) void AP_Baro::calibrate()
{ {
float ground_pressure = 0; float ground_pressure = 0;
float ground_temperature = 0; float ground_temperature = 0;
@ -42,7 +45,7 @@ void AP_Baro::calibrate(void (*callback)(unsigned long t))
read(); // Get initial data from absolute pressure sensor read(); // Get initial data from absolute pressure sensor
ground_pressure = get_pressure(); ground_pressure = get_pressure();
ground_temperature = get_temperature(); ground_temperature = get_temperature();
callback(20); hal.scheduler->delay(20);
} }
// let the barometer settle for a full second after startup // let the barometer settle for a full second after startup
// the MS5611 reads quite a long way off for the first second, // the MS5611 reads quite a long way off for the first second,
@ -53,7 +56,7 @@ void AP_Baro::calibrate(void (*callback)(unsigned long t))
} while (!healthy); } while (!healthy);
ground_pressure = get_pressure(); ground_pressure = get_pressure();
ground_temperature = get_temperature(); ground_temperature = get_temperature();
callback(100); hal.scheduler->delay(100);
} }
// now average over 5 values for the ground pressure and // now average over 5 values for the ground pressure and
@ -64,7 +67,7 @@ void AP_Baro::calibrate(void (*callback)(unsigned long t))
} while (!healthy); } while (!healthy);
ground_pressure = ground_pressure * 0.8 + get_pressure() * 0.2; ground_pressure = ground_pressure * 0.8 + get_pressure() * 0.2;
ground_temperature = ground_temperature * 0.8 + get_temperature() * 0.2; ground_temperature = ground_temperature * 0.8 + get_temperature() * 0.2;
callback(100); hal.scheduler->delay(100);
} }
_ground_pressure.set_and_save(ground_pressure); _ground_pressure.set_and_save(ground_pressure);

5
libraries/AP_Baro/AP_Baro.h
View File

@ -6,7 +6,6 @@
#include <AP_Param.h> #include <AP_Param.h>
#include <Filter.h> #include <Filter.h>
#include <DerivativeFilter.h> #include <DerivativeFilter.h>
#include "../AP_PeriodicProcess/AP_PeriodicProcess.h"
class AP_Baro class AP_Baro
{ {
@ -14,7 +13,7 @@ public:
bool healthy; bool healthy;
AP_Baro() { AP_Baro() {
} }
virtual bool init(AP_PeriodicProcess *scheduler)=0; virtual bool init()=0;
virtual uint8_t read() = 0; virtual uint8_t read() = 0;
virtual float get_pressure() = 0; virtual float get_pressure() = 0;
virtual float get_temperature() = 0; virtual float get_temperature() = 0;
@ -25,7 +24,7 @@ public:
// calibrate the barometer. This must be called on startup if the // calibrate the barometer. This must be called on startup if the
// altitude/climb_rate/acceleration interfaces are ever used // altitude/climb_rate/acceleration interfaces are ever used
// the callback is a delay() like routine // the callback is a delay() like routine
void calibrate(void (*callback)(unsigned long t)); void calibrate();
// get current altitude in meters relative to altitude at the time // get current altitude in meters relative to altitude at the time
// of the last calibrate() call // of the last calibrate() call

58
libraries/AP_Baro/AP_Baro_BMP085.cpp
View File

@ -35,22 +35,19 @@
* *
*/ */
extern "C" {
// AVR LibC Includes // AVR LibC Includes
#include <inttypes.h> #include <inttypes.h>
#include <avr/interrupt.h> #include <avr/interrupt.h>
}
#if defined(ARDUINO) && ARDUINO >= 100
#include "Arduino.h"
#else
#include "WConstants.h"
#endif
#include <AP_Common.h> #include <AP_Common.h>
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library #include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <I2C.h>
#include <AP_HAL.h>
#include "AP_Baro_BMP085.h" #include "AP_Baro_BMP085.h"
extern const AP_HAL::HAL& hal;
#define BMP085_ADDRESS 0x77 //(0xEE >> 1) #define BMP085_ADDRESS 0x77 //(0xEE >> 1)
#define BMP085_EOC 30 // End of conversion pin PC7 #define BMP085_EOC 30 // End of conversion pin PC7
@ -58,26 +55,28 @@ extern "C" {
// chip using a direct IO port // chip using a direct IO port
// On APM2 prerelease hw, the data ready port is hooked up to PE7, which // On APM2 prerelease hw, the data ready port is hooked up to PE7, which
// is not available to the arduino digitalRead function. // is not available to the arduino digitalRead function.
#define BMP_DATA_READY() (_apm2_hardware ? (PINE&0x80) : digitalRead(BMP085_EOC)) #define BMP_DATA_READY() (_apm2_hardware ? (PINE&0x80) : hal.gpio->read(BMP085_EOC))
// oversampling 3 gives highest resolution // oversampling 3 gives highest resolution
#define OVERSAMPLING 3 #define OVERSAMPLING 3
// Public Methods ////////////////////////////////////////////////////////////// // Public Methods //////////////////////////////////////////////////////////////
bool AP_Baro_BMP085::init( AP_PeriodicProcess * scheduler ) bool AP_Baro_BMP085::init()
{ {
byte buff[22]; uint8_t buff[22];
pinMode(BMP085_EOC, INPUT); // End Of Conversion (PC7) input hal.gpio->pinMode(BMP085_EOC, GPIO_INPUT);// End Of Conversion (PC7) input
BMP085_State = 0; // Initial state BMP085_State = 0; // Initial state
// We read the calibration data registers // We read the calibration data registers
if (I2c.read(BMP085_ADDRESS, 0xAA, 22, buff) != 0) { if (hal.i2c->readRegisters(BMP085_ADDRESS, 0xAA, 22, buff) != 0) {
healthy = false; healthy = false;
return false; return false;
} }
hal.console->println_P(PSTR("read bmp085 calibration regs"));
ac1 = ((int)buff[0] << 8) | buff[1]; ac1 = ((int)buff[0] << 8) | buff[1];
ac2 = ((int)buff[2] << 8) | buff[3]; ac2 = ((int)buff[2] << 8) | buff[3];
ac3 = ((int)buff[4] << 8) | buff[5]; ac3 = ((int)buff[4] << 8) | buff[5];
@ -92,6 +91,9 @@ bool AP_Baro_BMP085::init( AP_PeriodicProcess * scheduler )
//Send a command to read Temp //Send a command to read Temp
Command_ReadTemp(); Command_ReadTemp();
hal.console->println_P(PSTR("read bmp085 temp"));
BMP085_State = 1; BMP085_State = 1;
// init raw temo // init raw temo
@ -123,7 +125,7 @@ uint8_t AP_Baro_BMP085::read()
} }
} }
if (result) { if (result) {
_last_update = millis(); _last_update = hal.scheduler->millis();
} }
return(result); return(result);
} }
@ -149,7 +151,9 @@ int32_t AP_Baro_BMP085::get_raw_temp() {
// Send command to Read Pressure // Send command to Read Pressure
void AP_Baro_BMP085::Command_ReadPress() void AP_Baro_BMP085::Command_ReadPress()
{ {
if (I2c.write(BMP085_ADDRESS, 0xF4, 0x34+(OVERSAMPLING << 6)) != 0) { uint8_t res = hal.i2c->writeRegister(BMP085_ADDRESS, 0xF4,
0x34+(OVERSAMPLING << 6));
if (res != 0) {
healthy = false; healthy = false;
} }
} }
@ -159,24 +163,28 @@ void AP_Baro_BMP085::ReadPress()
{ {
uint8_t buf[3]; uint8_t buf[3];
if (!healthy && millis() < _retry_time) { if (!healthy && hal.scheduler->millis() < _retry_time) {
return; return;
} }
if (I2c.read(BMP085_ADDRESS, 0xF6, 3, buf) != 0) { if (hal.i2c->readRegisters(BMP085_ADDRESS, 0xF6, 3, buf) != 0) {
_retry_time = millis() + 1000; _retry_time = hal.scheduler->millis() + 1000;
I2c.setSpeed(false); hal.i2c->setHighSpeed(false);
healthy = false; healthy = false;
return; return;
} }
RawPress = (((uint32_t)buf[0] << 16) | ((uint32_t)buf[1] << 8) | ((uint32_t)buf[2])) >> (8 - OVERSAMPLING); RawPress = (((uint32_t)buf[0] << 16)
| ((uint32_t)buf[1] << 8)
| ((uint32_t)buf[2])) >> (8 - OVERSAMPLING);
} }
// Send Command to Read Temperature // Send Command to Read Temperature
void AP_Baro_BMP085::Command_ReadTemp() void AP_Baro_BMP085::Command_ReadTemp()
{ {
if (I2c.write(BMP085_ADDRESS, 0xF4, 0x2E) != 0) { uint8_t data[1];
data[0] = 0x2E;
if (hal.i2c->writeRegister(BMP085_ADDRESS, 0xF4, 0x2E) != 0) {
healthy = false; healthy = false;
} }
} }
@ -187,13 +195,13 @@ void AP_Baro_BMP085::ReadTemp()
uint8_t buf[2]; uint8_t buf[2];
int32_t _temp_sensor; int32_t _temp_sensor;
if (!healthy && millis() < _retry_time) { if (!healthy && hal.scheduler->millis() < _retry_time) {
return; return;
} }
if (I2c.read(BMP085_ADDRESS, 0xF6, 2, buf) != 0) { if (hal.i2c->readRegisters(BMP085_ADDRESS, 0xF6, 2, buf) != 0) {
_retry_time = millis() + 1000; _retry_time = hal.scheduler->millis() + 1000;
I2c.setSpeed(false); hal.i2c->setHighSpeed(false);
healthy = false; healthy = false;
return; return;
} }

4
libraries/AP_Baro/AP_Baro_BMP085.h
View File

@ -4,7 +4,7 @@
#define PRESS_FILTER_SIZE 2 #define PRESS_FILTER_SIZE 2
#include "AP_Baro.h" #include <AP_Baro.h>
#include <AverageFilter.h> #include <AverageFilter.h>
class AP_Baro_BMP085 : public AP_Baro class AP_Baro_BMP085 : public AP_Baro
@ -17,7 +17,7 @@ public:
/* AP_Baro public interface: */ /* AP_Baro public interface: */
bool init(AP_PeriodicProcess * scheduler); bool init();
uint8_t read(); uint8_t read();
float get_pressure(); float get_pressure();
float get_temperature(); float get_temperature();

11
libraries/AP_Baro/AP_Baro_BMP085_hil.cpp
View File

@ -1,16 +1,15 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include <FastSerial.h>
#include <AP_Baro.h> #include <AP_Baro.h>
#include "AP_Baro_BMP085_hil.h" #include "AP_Baro_BMP085_hil.h"
#include <AP_HAL.h>
extern const AP_HAL::HAL& hal;
// Constructors //////////////////////////////////////////////////////////////// // Constructors ////////////////////////////////////////////////////////////////
AP_Baro_BMP085_HIL::AP_Baro_BMP085_HIL() AP_Baro_BMP085_HIL::AP_Baro_BMP085_HIL(){}
{
}
// Public Methods ////////////////////////////////////////////////////////////// // Public Methods //////////////////////////////////////////////////////////////
bool AP_Baro_BMP085_HIL::init(AP_PeriodicProcess * scheduler) bool AP_Baro_BMP085_HIL::init()
{ {
BMP085_State=1; BMP085_State=1;
return true; return true;
@ -50,7 +49,7 @@ void AP_Baro_BMP085_HIL::setHIL(float _Temp, float _Press)
} }
healthy = true; healthy = true;
_last_update = millis(); _last_update = hal.scheduler->millis();
} }
float AP_Baro_BMP085_HIL::get_pressure() { float AP_Baro_BMP085_HIL::get_pressure() {

2
libraries/AP_Baro/AP_Baro_BMP085_hil.h
View File

@ -18,7 +18,7 @@ private:
public: public:
AP_Baro_BMP085_HIL(); // Constructor AP_Baro_BMP085_HIL(); // Constructor
bool init(AP_PeriodicProcess * scheduler); bool init();
uint8_t read(); uint8_t read();
float get_pressure(); float get_pressure();
float get_temperature(); float get_temperature();

61
libraries/AP_Baro/AP_Baro_MS5611.cpp
View File

@ -32,10 +32,11 @@
* *
*/ */
#include <FastSerial.h> #include <avr/interrupt.h>
#include <SPI.h> #include <AP_HAL.h>
#include "AP_Baro_MS5611.h" #include "AP_Baro_MS5611.h"
extern const AP_HAL::HAL& hal;
/* on APM v.24 MS5661_CS is PG1 (Arduino pin 40) */ /* on APM v.24 MS5661_CS is PG1 (Arduino pin 40) */
#define MS5611_CS 40 #define MS5611_CS 40
@ -64,10 +65,10 @@ uint8_t AP_Baro_MS5611::_spi_read(uint8_t reg)
{ {
uint8_t return_value; uint8_t return_value;
uint8_t addr = reg; // | 0x80; // Set most significant bit uint8_t addr = reg; // | 0x80; // Set most significant bit
digitalWrite(MS5611_CS, LOW); hal.gpio->write(MS5611_CS, 0);
SPI.transfer(addr); // discarded hal.spi->transfer(addr); // discarded
return_value = SPI.transfer(0); return_value = hal.spi->transfer(0);
digitalWrite(MS5611_CS, HIGH); hal.gpio->write(MS5611_CS, 1);
return return_value; return return_value;
} }
@ -76,11 +77,11 @@ uint16_t AP_Baro_MS5611::_spi_read_16bits(uint8_t reg)
uint8_t byteH, byteL; uint8_t byteH, byteL;
uint16_t return_value; uint16_t return_value;
uint8_t addr = reg; // | 0x80; // Set most significant bit uint8_t addr = reg; // | 0x80; // Set most significant bit
digitalWrite(MS5611_CS, LOW); hal.gpio->write(MS5611_CS, 0);
SPI.transfer(addr); // discarded hal.spi->transfer(addr); // discarded
byteH = SPI.transfer(0); byteH = hal.spi->transfer(0);
byteL = SPI.transfer(0); byteL = hal.spi->transfer(0);
digitalWrite(MS5611_CS, HIGH); hal.gpio->write(MS5611_CS, 1);
return_value = ((uint16_t)byteH<<8) | (byteL); return_value = ((uint16_t)byteH<<8) | (byteL);
return return_value; return return_value;
} }
@ -90,12 +91,12 @@ uint32_t AP_Baro_MS5611::_spi_read_adc()
uint8_t byteH,byteM,byteL; uint8_t byteH,byteM,byteL;
uint32_t return_value; uint32_t return_value;
uint8_t addr = 0x00; uint8_t addr = 0x00;
digitalWrite(MS5611_CS, LOW); hal.gpio->write(MS5611_CS, 0);
SPI.transfer(addr); // discarded hal.spi->transfer(addr); // discarded
byteH = SPI.transfer(0); byteH = hal.spi->transfer(0);
byteM = SPI.transfer(0); byteM = hal.spi->transfer(0);
byteL = SPI.transfer(0); byteL = hal.spi->transfer(0);
digitalWrite(MS5611_CS, HIGH); hal.gpio->write(MS5611_CS, 1);
return_value = (((uint32_t)byteH)<<16) | (((uint32_t)byteM)<<8) | (byteL); return_value = (((uint32_t)byteH)<<16) | (((uint32_t)byteM)<<8) | (byteL);
return return_value; return return_value;
} }
@ -103,23 +104,23 @@ uint32_t AP_Baro_MS5611::_spi_read_adc()
void AP_Baro_MS5611::_spi_write(uint8_t reg) void AP_Baro_MS5611::_spi_write(uint8_t reg)
{ {
digitalWrite(MS5611_CS, LOW); hal.gpio->write(MS5611_CS, 0);
SPI.transfer(reg); // discarded hal.spi->transfer(reg); // discarded
digitalWrite(MS5611_CS, HIGH); hal.gpio->write(MS5611_CS, 1);
} }
// Public Methods ////////////////////////////////////////////////////////////// // Public Methods //////////////////////////////////////////////////////////////
// SPI should be initialized externally // SPI should be initialized externally
bool AP_Baro_MS5611::init( AP_PeriodicProcess *scheduler ) bool AP_Baro_MS5611::init()
{ {
scheduler->suspend_timer(); hal.scheduler->suspend_timer_procs();
pinMode(MS5611_CS, OUTPUT); // Chip select Pin hal.gpio->pinMode(MS5611_CS, GPIO_OUTPUT); // Chip select Pin
digitalWrite(MS5611_CS, HIGH); hal.gpio->write(MS5611_CS, 1);
delay(1); hal.scheduler->delay(1);
_spi_write(CMD_MS5611_RESET); _spi_write(CMD_MS5611_RESET);
delay(4); hal.scheduler->delay(4);
// We read the factory calibration // We read the factory calibration
// The on-chip CRC is not used // The on-chip CRC is not used
@ -133,7 +134,7 @@ bool AP_Baro_MS5611::init( AP_PeriodicProcess *scheduler )
//Send a command to read Temp first //Send a command to read Temp first
_spi_write(CMD_CONVERT_D2_OSR4096); _spi_write(CMD_CONVERT_D2_OSR4096);
_timer = micros(); _timer = hal.scheduler->micros();
_state = 0; _state = 0;
Temp=0; Temp=0;
Press=0; Press=0;
@ -143,8 +144,8 @@ bool AP_Baro_MS5611::init( AP_PeriodicProcess *scheduler )
_d1_count = 0; _d1_count = 0;
_d2_count = 0; _d2_count = 0;
scheduler->resume_timer(); hal.scheduler->register_timer_process( AP_Baro_MS5611::_update, 1, 0);
scheduler->register_process( AP_Baro_MS5611::_update ); hal.scheduler->resume_timer_procs();
// wait for at least one value to be read // wait for at least one value to be read
while (!_updated) ; while (!_updated) ;
@ -229,7 +230,7 @@ uint8_t AP_Baro_MS5611::read()
} }
_calculate(); _calculate();
if (updated) { if (updated) {
_last_update = millis(); _last_update = hal.scheduler->millis();
} }
return updated ? 1 : 0; return updated ? 1 : 0;
} }

5
libraries/AP_Baro/AP_Baro_MS5611.h
View File

@ -8,11 +8,10 @@
class AP_Baro_MS5611 : public AP_Baro class AP_Baro_MS5611 : public AP_Baro
{ {
public: public:
AP_Baro_MS5611() { AP_Baro_MS5611() {}
} // Constructor
/* AP_Baro public interface: */ /* AP_Baro public interface: */
bool init(AP_PeriodicProcess *scheduler); bool init();
uint8_t read(); uint8_t read();
float get_pressure(); // in mbar*100 units float get_pressure(); // in mbar*100 units
float get_temperature(); // in celsius degrees * 100 units float get_temperature(); // in celsius degrees * 100 units

89
libraries/AP_Baro/examples/AP_Baro_BMP085_test/AP_Baro_BMP085_test.pde
View File

@ -4,6 +4,7 @@
*/ */
#include <FastSerial.h> #include <FastSerial.h>
#include <AP_Common.h>
#include <I2C.h> #include <I2C.h>
#include <SPI.h> #include <SPI.h>
#include <Filter.h> #include <Filter.h>
@ -11,71 +12,67 @@
#include <Arduino_Mega_ISR_Registry.h> #include <Arduino_Mega_ISR_Registry.h>
#include <AP_PeriodicProcess.h> #include <AP_PeriodicProcess.h>
#include <AP_InertialSensor.h> #include <AP_InertialSensor.h>
#include <AP_Math.h>
#include <AP_Common.h>
#include <AP_Param.h> #include <AP_Param.h>
#include <AverageFilter.h> #include <AP_Math.h>
#include <AP_Buffer.h> #include <AP_HAL.h>
#include <Filter.h>
#include <AP_Baro.h>
#ifndef APM2_HARDWARE #include <AP_HAL_AVR.h>
# define APM2_HARDWARE 0
#endif
AP_Baro_BMP085 APM_BMP085(APM2_HARDWARE); const AP_HAL::HAL& hal = AP_HAL_AVR_APM1;
unsigned long timer; AP_Baro_BMP085 bmp085(0);
FastSerialPort0(Serial); uint32_t timer;
Arduino_Mega_ISR_Registry isr_registry;
AP_TimerProcess scheduler;
void setup() void setup()
{ {
Serial.begin(115200); hal.console->println("ArduPilot Mega BMP085 library test");
Serial.println("ArduPilot Mega BMP085 library test"); hal.console->println("Initialising barometer...");
Serial.println("Initialising barometer..."); delay(100);
I2c.begin(); hal.scheduler->delay(100);
I2c.timeOut(20);
//I2c.setSpeed(true); if (!bmp085.init()) {
hal.console->println("Barometer initialisation FAILED\n");
isr_registry.init();
scheduler.init(&isr_registry);
if (!APM_BMP085.init(&scheduler)) {
Serial.println("Barometer initialisation FAILED\n");
} }
Serial.println("initialisation complete."); delay(100); hal.console->println("initialisation complete.");
delay(1000); hal.scheduler->delay(1000);
timer = micros(); timer = hal.scheduler->micros();
} }
void loop() void loop()
{ {
float tmp_float; float tmp_float;
float Altitude;
if((micros()- timer) > 50000L) { if((hal.scheduler->micros()- timer) > 50000L) {
timer = micros(); timer = hal.scheduler->micros();
APM_BMP085.read(); bmp085.read();
unsigned long read_time = micros() - timer; uint32_t read_time = hal.scheduler->micros() - timer;
if (!APM_BMP085.healthy) { if (! bmp085.healthy) {
Serial.println("not healthy"); hal.console->println("not healthy");
return; return;
} }
Serial.print("Pressure:"); hal.console->print("Pressure:");
Serial.print(APM_BMP085.get_pressure()); hal.console->print( bmp085.get_pressure());
Serial.print(" Temperature:"); hal.console->print(" Temperature:");
Serial.print(APM_BMP085.get_temperature()); hal.console->print( bmp085.get_temperature());
Serial.print(" Altitude:"); hal.console->print(" Altitude:");
tmp_float = (APM_BMP085.get_pressure() / 101325.0); tmp_float = ( bmp085.get_pressure() / 101325.0);
tmp_float = pow(tmp_float, 0.190295); tmp_float = pow(tmp_float, 0.190295);
Altitude = 44330.0 * (1.0 - tmp_float); float alt = 44330.0 * (1.0 - tmp_float);
Serial.print(Altitude); hal.console->print(alt);
Serial.printf(" t=%u", (unsigned)read_time); hal.console->printf(" t=%lu", read_time);
Serial.println(); hal.console->println();
} }
} }
extern "C" {
int main (void) {
hal.init(NULL);
setup();
for(;;) loop();
return 0;
}
}

0
libraries/AP_Baro/examples/AP_Baro_BMP085_test/Arduino.h Normal file
View File

0
libraries/AP_Baro/examples/AP_Baro_BMP085_test/nocore.inoflag Normal file
View File

79
libraries/AP_Baro/examples/AP_Baro_MS5611_test/AP_Baro_MS5611_test.pde
View File

@ -1,65 +1,64 @@
#include <stdint.h>
#include <FastSerial.h>
#include <AP_Common.h> #include <AP_Common.h>
#include <AP_Param.h> #include <AP_Param.h>
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library #include <AP_Math.h>
#include <I2C.h> #include <AP_HAL.h>
#include <SPI.h> #include <AP_HAL_AVR.h>
#include <Filter.h>
#include <AP_Buffer.h>
#include <Arduino_Mega_ISR_Registry.h>
#include <AP_PeriodicProcess.h>
#include <AP_Baro.h> // ArduPilot Mega ADC Library
FastSerialPort0(Serial); const AP_HAL::HAL& hal = AP_HAL_AVR_APM2;
#include <Filter.h>
#include <AP_Baro.h>
AP_Baro_MS5611 baro; AP_Baro_MS5611 baro;
Arduino_Mega_ISR_Registry isr_registry; static uint32_t timer;
AP_TimerProcess scheduler;
unsigned long timer;
void setup() void setup()
{ {
Serial.begin(115200, 128, 128); hal.console->println("APM2 MS5611 Barometer library test");
Serial.println("ArduPilot Mega MeasSense Barometer library test");
delay(1000); hal.scheduler->delay(1000);
isr_registry.init(); /* What's this for? */
scheduler.init(&isr_registry); hal.gpio->pinMode(63, GPIO_OUTPUT);
hal.gpio->write(63, 1);
baro.init();
baro.calibrate();
pinMode(63, OUTPUT); timer = hal.scheduler->micros();
digitalWrite(63, HIGH);
SPI.begin();
SPI.setClockDivider(SPI_CLOCK_DIV32); // 500khz for debugging, increase later
baro.init(&scheduler);
baro.calibrate(delay);
timer = millis();
} }
void loop() void loop()
{ {
if((micros() - timer) > 100000UL) { if((hal.scheduler->micros() - timer) > 100000UL) {
timer = micros(); timer = hal.scheduler->micros();
baro.read(); baro.read();
uint32_t read_time = micros() - timer; uint32_t read_time = hal.scheduler->micros() - timer;
if (!baro.healthy) { if (!baro.healthy) {
Serial.println("not healthy"); hal.console->println("not healthy");
return; return;
} }
Serial.print("Pressure:"); hal.console->print("Pressure:");
Serial.print(baro.get_pressure()); hal.console->print(baro.get_pressure());
Serial.print(" Temperature:"); hal.console->print(" Temperature:");
Serial.print(baro.get_temperature()); hal.console->print(baro.get_temperature());
Serial.print(" Altitude:"); hal.console->print(" Altitude:");
Serial.print(baro.get_altitude()); hal.console->print(baro.get_altitude());
Serial.printf(" climb=%.2f t=%u samples=%u", hal.console->printf(" climb=%.2f t=%u samples=%u",
baro.get_climb_rate(), baro.get_climb_rate(),
(unsigned)read_time, (unsigned)read_time,
(unsigned)baro.get_pressure_samples()); (unsigned)baro.get_pressure_samples());
Serial.println(); hal.console->println();
} }
} }
extern "C" {
int main (void) {
hal.init(NULL);
setup();
for(;;) loop();
return 0;
}
}

0
libraries/AP_Baro/examples/AP_Baro_MS5611_test/Arduino.h Normal file
View File

0
libraries/AP_Baro/examples/AP_Baro_MS5611_test/nocore.inoflag Normal file
View File