AP_Baro: port to AP_HAL

libraries/AP_Baro/AP_Baro.cpp

@@ -8,9 +8,12 @@
  *   of the License, or (at your option) any later version.
  */
 
-#include <FastSerial.h>
+#include <math.h>
 #include <AP_Common.h>
 #include <AP_Baro.h>
+#include <AP_HAL.h>
+
+extern const AP_HAL::HAL& hal;
 
 // table of user settable parameters
 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
 // 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_temperature = 0;
@@ -42,7 +45,7 @@ void AP_Baro::calibrate(void (*callback)(unsigned long t))
         read();         // Get initial data from absolute pressure sensor
         ground_pressure         = get_pressure();
         ground_temperature      = get_temperature();
-        callback(20);
+        hal.scheduler->delay(20);
     }
     // let the barometer settle for a full second after startup
     // 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);
         ground_pressure         = get_pressure();
         ground_temperature      = get_temperature();
-        callback(100);
+        hal.scheduler->delay(100);
     }
 
     // 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);
         ground_pressure         = ground_pressure * 0.8     + get_pressure() * 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);

libraries/AP_Baro/AP_Baro.h

@@ -6,7 +6,6 @@
 #include <AP_Param.h>
 #include <Filter.h>
 #include <DerivativeFilter.h>
-#include "../AP_PeriodicProcess/AP_PeriodicProcess.h"
 
 class AP_Baro
 {
@@ -14,7 +13,7 @@ public:
     bool                    healthy;
     AP_Baro() {
     }
-    virtual bool            init(AP_PeriodicProcess *scheduler)=0;
+    virtual bool            init()=0;
     virtual uint8_t         read() = 0;
     virtual float           get_pressure() = 0;
     virtual float           get_temperature() = 0;
@@ -25,7 +24,7 @@ public:
     // calibrate the barometer. This must be called on startup if the
     // altitude/climb_rate/acceleration interfaces are ever used
     // 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
     // of the last calibrate() call

libraries/AP_Baro/AP_Baro_BMP085.cpp

@@ -35,22 +35,19 @@
  *
  */
 
-extern "C" {
 // AVR LibC Includes
 #include <inttypes.h>
 #include <avr/interrupt.h>
-}
-#if defined(ARDUINO) && ARDUINO >= 100
- #include "Arduino.h"
-#else
- #include "WConstants.h"
-#endif
 
 #include <AP_Common.h>
 #include <AP_Math.h>            // ArduPilot Mega Vector/Matrix math Library
-#include <I2C.h>
+
+#include <AP_HAL.h>
+
 #include "AP_Baro_BMP085.h"
 
+extern const AP_HAL::HAL& hal;
+
 #define BMP085_ADDRESS 0x77  //(0xEE >> 1)
 #define BMP085_EOC 30        // End of conversion pin PC7
 
@@ -58,26 +55,28 @@ extern "C" {
 // chip using a direct IO port
 // On APM2 prerelease hw, the data ready port is hooked up to PE7, which
 // 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
 #define OVERSAMPLING 3
 
 // 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
 
     // 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;
         return false;
     }
 
+    hal.console->println_P(PSTR("read bmp085 calibration regs"));
+
     ac1 = ((int)buff[0] << 8) | buff[1];
     ac2 = ((int)buff[2] << 8) | buff[3];
     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
     Command_ReadTemp();
+    
+    hal.console->println_P(PSTR("read bmp085 temp"));
+
     BMP085_State = 1;
 
     // init raw temo
@@ -123,7 +125,7 @@ uint8_t AP_Baro_BMP085::read()
         }
     }
     if (result) {
-        _last_update = millis();
+        _last_update = hal.scheduler->millis();
     }
     return(result);
 }
@@ -149,7 +151,9 @@ int32_t AP_Baro_BMP085::get_raw_temp() {
 // Send command to Read Pressure
 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;
     }
 }
@@ -159,24 +163,28 @@ void AP_Baro_BMP085::ReadPress()
 {
     uint8_t buf[3];
 
-    if (!healthy && millis() < _retry_time) {
+    if (!healthy && hal.scheduler->millis() < _retry_time) {
         return;
     }
 
-    if (I2c.read(BMP085_ADDRESS, 0xF6, 3, buf) != 0) {
-        _retry_time = millis() + 1000;
-        I2c.setSpeed(false);
+    if (hal.i2c->readRegisters(BMP085_ADDRESS, 0xF6, 3, buf) != 0) {
+        _retry_time = hal.scheduler->millis() + 1000;
+        hal.i2c->setHighSpeed(false);
         healthy = false;
         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
 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;
     }
 }
@@ -187,13 +195,13 @@ void AP_Baro_BMP085::ReadTemp()
     uint8_t buf[2];
     int32_t _temp_sensor;
 
-    if (!healthy && millis() < _retry_time) {
+    if (!healthy && hal.scheduler->millis() < _retry_time) {
         return;
     }
 
-    if (I2c.read(BMP085_ADDRESS, 0xF6, 2, buf) != 0) {
-        _retry_time = millis() + 1000;
-        I2c.setSpeed(false);
+    if (hal.i2c->readRegisters(BMP085_ADDRESS, 0xF6, 2, buf) != 0) {
+        _retry_time = hal.scheduler->millis() + 1000;
+        hal.i2c->setHighSpeed(false);
         healthy = false;
         return;
     }

libraries/AP_Baro/AP_Baro_BMP085.h

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

libraries/AP_Baro/AP_Baro_BMP085_hil.cpp

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

libraries/AP_Baro/AP_Baro_BMP085_hil.h

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

libraries/AP_Baro/AP_Baro_MS5611.cpp

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

libraries/AP_Baro/AP_Baro_MS5611.h

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

libraries/AP_Baro/examples/AP_Baro_BMP085_test/AP_Baro_BMP085_test.pde

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

libraries/AP_Baro/examples/AP_Baro_MS5611_test/AP_Baro_MS5611_test.pde

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