AP_AHRS: port to AP_HAL

libraries/AP_AHRS/AP_AHRS.cpp

@@ -6,9 +6,9 @@
 	as published by the Free Software Foundation; either version 2.1
 	of the License, or (at your option) any later version.
 */
-#include <FastSerial.h>
 #include <AP_AHRS.h>
-
+#include <AP_HAL.h>
+extern const AP_HAL::HAL& hal;
 
 // table of user settable parameters
 const AP_Param::GroupInfo AP_AHRS::var_info[] PROGMEM = {
@@ -87,6 +87,7 @@ bool AP_AHRS::airspeed_estimate(float *airspeed_ret)
 		if (_wind_max > 0 && _gps && _gps->status() == GPS::GPS_OK) {
 			// constrain the airspeed by the ground speed
 			// and AHRS_WIND_MAX
+#define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
 			*airspeed_ret = constrain(*airspeed_ret, 
 						  _gps->ground_speed*0.01 - _wind_max, 
 						  _gps->ground_speed*0.01 + _wind_max);
@@ -102,7 +103,8 @@ void AP_AHRS::add_trim(float roll_in_radians, float pitch_in_radians)
     Vector3f trim = _trim.get();
 
     // debug -- remove me!
-    Serial.printf_P(PSTR("\nadd_trim before R:%4.2f P:%4.2f\n"),ToDeg(trim.x),ToDeg(trim.y));
+    hal.console->printf_P(PSTR("\nadd_trim before R:%4.2f P:%4.2f\n"),
+            ToDeg(trim.x),ToDeg(trim.y));
 
     // add new trim
     trim.x = constrain(trim.x + roll_in_radians, ToRad(-10.0), ToRad(10.0));
@@ -112,5 +114,6 @@ void AP_AHRS::add_trim(float roll_in_radians, float pitch_in_radians)
     _trim.set_and_save(trim);
 
     // debug -- remove me!
-    Serial.printf_P(PSTR("add_trim after R:%4.2f P:%4.2f\n"),ToDeg(trim.x),ToDeg(trim.y));
+    hal.console->printf_P(PSTR("add_trim after R:%4.2f P:%4.2f\n"),
+            ToDeg(trim.x),ToDeg(trim.y));
 }

libraries/AP_AHRS/AP_AHRS.h

@@ -1,5 +1,5 @@
-#ifndef AP_AHRS_H
+#ifndef __AP_AHRS_H__
-#define AP_AHRS_H
+#define __AP_AHRS_H__
 /*
  *  AHRS (Attitude Heading Reference System) interface for ArduPilot
  *
@@ -18,12 +18,6 @@
 #include <AP_Baro.h>
 #include <AP_Param.h>
 
-#if defined(ARDUINO) && ARDUINO >= 100
- #include "Arduino.h"
-#else
- #include "WProgram.h"
-#endif
-
 class AP_AHRS
 {
 public:
@@ -42,24 +36,24 @@ public:
     }
 
     // empty init
-    virtual void init( AP_PeriodicProcess * scheduler = NULL ) {
+    virtual void init() {
     };
 
     // Accessors
-    void            set_fly_forward(bool b) {
+    void set_fly_forward(bool b) {
         _fly_forward = b;
     }
-    void            set_compass(Compass *compass) {
+    void set_compass(Compass *compass) {
         _compass = compass;
     }
-    void            set_barometer(AP_Baro *barometer) {
+    void set_barometer(AP_Baro *barometer) {
         _barometer = barometer;
     }
-    void            set_airspeed(AP_Airspeed *airspeed) {
+    void set_airspeed(AP_Airspeed *airspeed) {
         _airspeed = airspeed;
     }
 
-    AP_InertialSensor*            get_ins() {
+    AP_InertialSensor* get_ins() {
 	    return _ins;
     }
 
@@ -202,4 +196,4 @@ protected:
 #include <AP_AHRS_MPU6000.h>
 #include <AP_AHRS_HIL.h>
 
-#endif // AP_AHRS_H
+#endif // __AP_AHRS_H__

libraries/AP_AHRS/AP_AHRS_DCM.cpp

@@ -13,8 +13,12 @@
  *       as published by the Free Software Foundation; either version 2.1
  *       of the License, or (at your option) any later version.
  */
-#include <FastSerial.h>
 #include <AP_AHRS.h>
+#include <AP_HAL.h>
+
+extern const AP_HAL::HAL& hal;
+
+#define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
 
 // this is the speed in cm/s above which we first get a yaw lock with
 // the GPS
@@ -428,7 +432,7 @@ AP_AHRS_DCM::drift_correction(float deltat)
         // add in wind estimate
         velocity += _wind;
 
-        last_correction_time = millis();
+        last_correction_time = hal.scheduler->millis();
         _have_gps_lock = false;
 
         // update position delta for get_position()
@@ -606,7 +610,7 @@ void AP_AHRS_DCM::estimate_wind(Vector3f &velocity)
     // See http://gentlenav.googlecode.com/files/WindEstimation.pdf
     Vector3f fuselageDirection = _dcm_matrix.colx();
     Vector3f fuselageDirectionDiff = fuselageDirection - _last_fuse;
-    uint32_t now = millis();
+    uint32_t now = hal.scheduler->millis();
 
     // scrap our data and start over if we're taking too long to get a direction change
     if (now - _last_wind_time > 10000) {

libraries/AP_AHRS/AP_AHRS_DCM.h

@@ -1,5 +1,5 @@
-#ifndef AP_AHRS_DCM_H
+#ifndef __AP_AHRS_DCM_H__
-#define AP_AHRS_DCM_H
+#define __AP_AHRS_DCM_H__
 /*
  *  DCM based AHRS (Attitude Heading Reference System) interface for
  *  ArduPilot
@@ -136,4 +136,4 @@ private:
     Vector3f _wind;
 };
 
-#endif // AP_AHRS_DCM_H
+#endif // __AP_AHRS_DCM_H__

libraries/AP_AHRS/AP_AHRS_HIL.cpp

@@ -10,7 +10,6 @@
  *       version.
  */
 
-#include <FastSerial.h>
 #include <AP_AHRS.h>
 
 /**************************************************/

libraries/AP_AHRS/AP_AHRS_HIL.h

@@ -1,13 +1,11 @@
-#ifndef AP_AHRS_HIL_H
+#ifndef __AP_AHRS_HIL_H__
-#define AP_AHRS_HIL_H
+#define __AP_AHRS_HIL_H__
 
 class AP_AHRS_HIL : public AP_AHRS
 {
 public:
     // Constructors
-    AP_AHRS_HIL(AP_InertialSensor *ins, GPS *&gps) : AP_AHRS(ins, gps)
+    AP_AHRS_HIL(AP_InertialSensor *ins, GPS *&gps) : AP_AHRS(ins, gps) {}
-    {
-    }
 
     // Accessors
     Vector3f get_gyro(void) {
@@ -21,12 +19,12 @@ public:
     }
 
     // Methods
-    void            update(void) {
+    void update(void) {
         _ins->update();
     }
     
-    void            setHil(float roll, float pitch, float yaw,
+    void setHil(float roll, float pitch, float yaw,
-                           float rollRate, float pitchRate, float yawRate);
+                float rollRate, float pitchRate, float yawRate);
 
     // return the current estimate of the gyro drift
     Vector3f get_gyro_drift(void) {
@@ -48,4 +46,4 @@ private:
     Vector3f _omega;
 };
 
-#endif
+#endif // __AP_AHRS_HIL_H__

libraries/AP_AHRS/AP_AHRS_MPU6000.cpp

@@ -10,8 +10,10 @@
  *       as published by the Free Software Foundation; either version 2.1
  *       of the License, or (at your option) any later version.
  */
-#include <FastSerial.h>
 #include <AP_AHRS.h>
+#include <AP_HAL.h>
+
+extern const AP_HAL::HAL& hal;
 
 // this is the speed in cm/s above which we first get a yaw lock with
 // the GPS
@@ -28,24 +30,20 @@
 #define SPIN_RATE_LIMIT 20
 
 void
-AP_AHRS_MPU6000::init( AP_PeriodicProcess * scheduler )
+AP_AHRS_MPU6000::init()
 {
     bool timer_running = false;
 
-    // suspend timer so interrupts on spi bus do not interfere with communication to mpu6000
+    // suspend timer so interrupts on spi bus do not interfere with
-    if( scheduler != NULL ) {
+    // communication to mpu6000
-        timer_running = scheduler->running();
+    hal.scheduler->suspend_timer_procs();
-        scheduler->suspend_timer();
-    }
 
     _mpu6000->dmp_init();
     push_gains_to_dmp();
     _mpu6000->push_gyro_offsets_to_dmp();
 
     // restart timer
-    if( timer_running ) {
+    hal.scheduler->resume_timer_procs();
-        scheduler->resume_timer();
-    }
 };
 
 // run a full MPU6000 update round
@@ -87,8 +85,9 @@ float AP_AHRS_MPU6000::wrap_PI(float angle_in_radians)
     }
 }
 
-// Function to correct the gyroX and gyroY bias (roll and pitch) using the gravity vector from accelerometers
+// Function to correct the gyroX and gyroY bias (roll and pitch) using the
-// We use the internal chip axis definition to make the bias correction because both sensors outputs (gyros and accels)
+// gravity vector from accelerometers We use the internal chip axis definition
+// to make the bias correction because both sensors outputs (gyros and accels)
 // are in chip axis definition
 void AP_AHRS_MPU6000::drift_correction( float deltat )
 {
@@ -97,40 +96,52 @@ void AP_AHRS_MPU6000::drift_correction( float deltat )
     // Get current values for gyros
     _accel_vector = _ins->get_accel();
 
-    // We take the accelerometer readings and cumulate to average them and obtain the gravity vector
+    // We take the accelerometer readings and cumulate to average them and
+    // obtain the gravity vector
     _accel_filtered += _accel_vector;
     _accel_filtered_samples++;
 
     _gyro_bias_from_gravity_counter++;
-    // We make the bias calculation and correction at a lower rate (GYRO_BIAS_FROM_GRAVITY_RATE)
+    // We make the bias calculation and correction at a lower rate
+    // (GYRO_BIAS_FROM_GRAVITY_RATE)
     if( _gyro_bias_from_gravity_counter == GYRO_BIAS_FROM_GRAVITY_RATE ) {
         _gyro_bias_from_gravity_counter = 0;
 
         _accel_filtered /= _accel_filtered_samples;          // average
 
-        // Adjust ground reference : Accel Cross Gravity to obtain the error between gravity from accels and gravity from attitude solution
+        // Adjust ground reference : Accel Cross Gravity to obtain the error
+        // between gravity from accels and gravity from attitude solution
         // errorRollPitch are in Accel LSB units
-        errorRollPitch[0] = _accel_filtered.y * _dcm_matrix.c.z + _accel_filtered.z * _dcm_matrix.c.x;
+        errorRollPitch[0] = _accel_filtered.y * _dcm_matrix.c.z
-        errorRollPitch[1] = -_accel_filtered.z * _dcm_matrix.c.y - _accel_filtered.x * _dcm_matrix.c.z;
+            + _accel_filtered.z * _dcm_matrix.c.x;
+        errorRollPitch[1] = -_accel_filtered.z * _dcm_matrix.c.y
+            - _accel_filtered.x * _dcm_matrix.c.z;
 
         errorRollPitch[0] *= deltat * 1000;
         errorRollPitch[1] *= deltat * 1000;
 
         // we limit to maximum gyro drift rate on each axis
-        float drift_limit = _mpu6000->get_gyro_drift_rate() * deltat / _gyro_bias_from_gravity_gain;          //0.65*0.04 / 0.005 = 5.2
+        // 0.65*0.04 / 0.005 = 5.2
-        errorRollPitch[0] = constrain(errorRollPitch[0], -drift_limit, drift_limit);
+        float drift_limit = _mpu6000->get_gyro_drift_rate() * deltat 
-        errorRollPitch[1] = constrain(errorRollPitch[1], -drift_limit, drift_limit);
+            / _gyro_bias_from_gravity_gain;
+#define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
+        errorRollPitch[0] = constrain(errorRollPitch[0],
+                -drift_limit, drift_limit);
+        errorRollPitch[1] = constrain(errorRollPitch[1],
+                -drift_limit, drift_limit);
 
         // We correct gyroX and gyroY bias using the error vector
         _gyro_bias[0] += errorRollPitch[0]*_gyro_bias_from_gravity_gain;
         _gyro_bias[1] += errorRollPitch[1]*_gyro_bias_from_gravity_gain;
 
         // TO-DO: fix this.  Currently it makes the roll and pitch drift more!
-        // If bias values are greater than 1 LSB we update the hardware offset registers
+        // If bias values are greater than 1 LSB we update the hardware offset
+        // registers
         if( fabs(_gyro_bias[0])>1.0 ) {
             //_mpu6000->set_gyro_offsets(-1*(int)_gyro_bias[0],0,0);
             //_mpu6000->set_gyro_offsets(0,-1*(int)_gyro_bias[0],0);
-            //_gyro_bias[0] -= (int)_gyro_bias[0];  // we remove the part that we have already corrected on registers...
+            //_gyro_bias[0] -= (int)_gyro_bias[0];  // we remove the part that
+            // we have already corrected on registers...
         }
         if (fabs(_gyro_bias[1])>1.0) {
             //_mpu6000->set_gyro_offsets(-1*(int)_gyro_bias[1],0,0);
@@ -167,14 +178,16 @@ AP_AHRS_MPU6000::reset(bool recover_eulers)
     }
 }
 
-// push offsets down from IMU to INS (required so MPU6000 can perform it's own attitude estimation)
+// push offsets down from IMU to INS (required so MPU6000 can perform it's own
+// attitude estimation)
 void
 AP_AHRS_MPU6000::push_offsets_to_ins()
 {
     // push down gyro offsets (TO-DO: why are x and y offsets are reversed?!)
     _mpu6000->push_gyro_offsets_to_dmp();
 
-    // push down accelerometer offsets (TO-DO: why are x and y offsets are reversed?!)
+    // push down accelerometer offsets
+    // (TO-DO: why are x and y offsets are reversed?!)
     _mpu6000->push_accel_offsets_to_dmp();
 }
 
@@ -220,18 +233,23 @@ AP_AHRS_MPU6000::yaw_error_compass(void)
 
 //
 // drift_correction_yaw - yaw drift correction using the compass
-//    we have no way to update the dmp with it's actual heading from our compass
+//    we have no way to update the dmp with it's actual heading from our
-//    instead we use the yaw_corrected variable to hold what we think is the real heading
+//    compass instead we use the yaw_corrected variable to hold what we think
-//    we also record what the dmp said it's last heading was in the yaw_last_uncorrected variable so that on the next iteration we can add the change in yaw to our estimate
+//    is the real heading we also record what the dmp said it's last heading
+//    was in the yaw_last_uncorrected variable so that on the next iteration we
+//    can add the change in yaw to our estimate
 //
 void
 AP_AHRS_MPU6000::drift_correction_yaw(void)
 {
     static float yaw_corrected = HEADING_UNKNOWN;
     static float last_dmp_yaw = HEADING_UNKNOWN;
-    float dmp_roll, dmp_pitch, dmp_yaw; // roll pitch and yaw values from dmp
+    // roll pitch and yaw values from dmp
-    float yaw_delta;                    // change in yaw according to dmp
+    float dmp_roll, dmp_pitch, dmp_yaw;
-    float yaw_error;                    // difference between heading and corrected yaw
+    // change in yaw according to dmp
+    float yaw_delta;
+    // difference between heading and corrected yaw
+    float yaw_error;
     static float heading;
 
     // get uncorrected yaw values from dmp
@@ -282,7 +300,8 @@ void
 AP_AHRS_MPU6000::euler_angles(void)
 {
     _dcm_matrix.to_euler(&roll, &pitch, &yaw);
-    //quaternion.to_euler(&roll, &pitch, &yaw);	// cannot use this because the quaternion is not correct for yaw drift
+    // cannot use this because the quaternion is not correct for yaw drift
+    //quaternion.to_euler(&roll, &pitch, &yaw);
 
     roll_sensor     = degrees(roll)  * 100;
     pitch_sensor    = degrees(pitch) * 100;

libraries/AP_AHRS/AP_AHRS_MPU6000.h

@@ -1,5 +1,5 @@
-#ifndef AP_AHRS_MPU6000_H
+#ifndef __AP_AHRS_MPU6000_H__
-#define AP_AHRS_MPU6000_H
+#define __AP_AHRS_MPU6000_H__
 /*
  *  DCM based AHRS (Attitude Heading Reference System) interface for
  *  ArduPilot
@@ -10,89 +10,101 @@
  *  version 2.1 of the License, or (at your option) any later version.
  */
 
-#define GYRO_BIAS_FROM_GRAVITY_RATE 4  // Rate of the gyro bias from gravity correction (200Hz/4) => 50Hz
+// Rate of the gyro bias from gravity correction (200Hz/4) => 50Hz
-#define HEADING_UNKNOWN -9999             // Initial value to detect that compass correction is not initialized
+#define GYRO_BIAS_FROM_GRAVITY_RATE 4
+// Initial value to detect that compass correction is not initialized
+#define HEADING_UNKNOWN -9999
 
 // max rate of gyro drift in gyro_LSB_units/s (16.4LSB = 1deg/s)
-static const float _MPU6000_gyro_drift_rate = 0.5; // This correspond to 0.03 degrees/s/s;
+// 0.5 corresponds to 0.03 degrees/s/s;
+static const float _MPU6000_gyro_drift_rate = 0.5;
 
 class AP_AHRS_MPU6000 : public AP_AHRS
 {
 public:
     // Constructors
-    AP_AHRS_MPU6000(AP_InertialSensor_MPU6000 *mpu6000, GPS *&gps) : AP_AHRS(mpu6000, gps), _mpu6000(mpu6000)
+    AP_AHRS_MPU6000(AP_InertialSensor_MPU6000 *mpu6000, GPS *&gps) :
+        AP_AHRS(mpu6000, gps),
+        // ki and ki_yaw are experimentally derived from the simulator
+        _ki(0.0087),
+        _ki_yaw(0.01),
+        _mpu6000(mpu6000),
+        // dmp related variable initialisation
+        _compass_bias_time(0),
+        _gyro_bias_from_gravity_gain(0.008)
     {
         _dcm_matrix.identity();
-
-        // these are experimentally derived from the simulator
-        // with large drift levels
-        _ki = 0.0087;
-        _ki_yaw = 0.01;
-
-        // dmp related variable initialisation
-        _gyro_bias_from_gravity_gain = 0.008;
-        _compass_bias_time = 0;
     }
 
     // initialisation routine to start MPU6000's dmp
-    void init( AP_PeriodicProcess * scheduler = NULL );
+    void init();
 
     // return the smoothed gyro vector corrected for drift
-    Vector3f        get_gyro(void) {
+    Vector3f get_gyro(void) {
         return _ins->get_gyro();
     }
-    Matrix3f        get_dcm_matrix(void) {
+
+    Matrix3f get_dcm_matrix(void) {
         return _dcm_matrix;
     }
 
     // return the current drift correction integrator value
-    Vector3f        get_gyro_drift(void) {
+    Vector3f get_gyro_drift(void) {
         return _omega_I;
     }
 
     // Methods
-    void            update(void);
+    void update(void);
-    void            reset(bool recover_eulers = false);
+    void reset(bool recover_eulers = false);
 
-    // push offsets down from IMU to INS (required so MPU6000 can perform it's own attitude estimation)
+    // push offsets down from IMU to INS (required so MPU6000 can perform it's
-    void            push_offsets_to_ins();
+    // own attitude estimation)
-    void            push_gains_to_dmp();
+    void push_offsets_to_ins();
+    void push_gains_to_dmp();
 
     // status reporting
-    float           get_error_rp(void);
+    float get_error_rp(void);
-    float           get_error_yaw(void);
+    float get_error_yaw(void);
 
-    // set_as_secondary - avoid running some steps twice (imu updates) if this is a secondary ahrs
+    // set_as_secondary - avoid running some steps twice (imu updates) if
-    void            set_as_secondary(bool secondary) { _secondary_ahrs = secondary; }
+    // this is a secondary ahrs
+    void set_as_secondary(bool secondary) {
+        _secondary_ahrs = secondary;
+    }
 
 private:
     float _ki;
     float _ki_yaw;
-    AP_InertialSensor_MPU6000       *_mpu6000;
+    AP_InertialSensor_MPU6000 *_mpu6000;
 
     // Methods
-    void            drift_correction(float deltat);
+    void drift_correction(float deltat);
 
     // Compass correction variables. TO-DO: move or replace?
-    float           wrap_PI(float angle_in_radians);            // TO-DO: move this to standard AP_AHRS methods
+    // TO-DO: move wrap_PI to standard AP_AHRS methods
+    float wrap_PI(float angle_in_radians);
     long _compass_bias_time;
 
-    void            drift_correction_yaw(void);
+    void  drift_correction_yaw(void);
-    float           yaw_error_compass();
+    float yaw_error_compass();
-    void            euler_angles(void);
+    void  euler_angles(void);
 
     Vector3f _accel_filtered;
     int16_t _accel_filtered_samples;
-    float _gyro_bias[3];                                                        // bias_tracking
+
-    float _gyro_bias_from_gravity_gain;                         // bias correction algorithm gain
+    // bias_tracking
+    float _gyro_bias[3];
+
+    // bias correction algorithm gain
+    float _gyro_bias_from_gravity_gain;
     uint8_t _gyro_bias_from_gravity_counter;
 
     // primary representation of attitude
     Matrix3f _dcm_matrix;
-
+    // current accel vector
-    Vector3f _accel_vector;                     // current accel vector
+    Vector3f _accel_vector;
-
+    // Omega Integrator correction
-    Vector3f _omega_I;                                  // Omega Integrator correction
+    Vector3f _omega_I;
     Vector3f _omega_I_sum;
     float _omega_I_sum_time;
 
@@ -104,4 +116,4 @@ private:
     bool _secondary_ahrs;
 };
 
-#endif // AP_AHRS_MPU6000_H
+#endif // __AP_AHRS_MPU6000_H__

libraries/AP_AHRS/examples/AHRS_Test/AHRS_Test.pde

@@ -4,57 +4,42 @@
 // Simple test for the AP_AHRS interface
 //
 
-#include <FastSerial.h>
+#include <AP_HAL.h>
-#include <SPI.h>
+#include <AP_Common.h>
-#include <I2C.h>
+#include <AP_Progmem.h>
-#include <Arduino_Mega_ISR_Registry.h>
+#include <AP_Math.h>
-#include <AP_PeriodicProcess.h>
+#include <AP_Param.h>
 #include <AP_InertialSensor.h>
 #include <AP_ADC.h>
 #include <AP_GPS.h>
 #include <AP_AHRS.h>
-#include <AP_Math.h>
-#include <AP_AnalogSource.h>
-#include <AP_AnalogSource_Arduino.h>
-#include <AP_Common.h>
-#include <AP_Param.h>
 #include <AP_Compass.h>
+#include <AP_Declination.h>
 #include <AP_Airspeed.h>
 #include <AP_Baro.h>
-#include <AP_Semaphore.h>
-#include <DataFlash.h>
-#include <APM_RC.h>
 #include <GCS_MAVLink.h>
 #include <Filter.h>
 #include <AP_Buffer.h>
 
-// uncomment this for a APM2 board
+#include <AP_HAL_AVR.h>
+
+/* Only testing with APM2 for now. */
 #define APM2_HARDWARE
 
-#define WITH_GPS 0
-
-FastSerialPort0(Serial);
-FastSerialPort1(Serial1);
-
-Arduino_Mega_ISR_Registry isr_registry;
-AP_TimerProcess scheduler;
-
-#ifdef DESKTOP_BUILD
-AP_Compass_HIL compass;
-#else
-AP_Compass_HMC5843 compass;
-#endif
-
 #ifdef APM2_HARDWARE
 AP_InertialSensor_MPU6000 ins;
+const AP_HAL::HAL& hal = AP_HAL_AVR_APM2;
 # else
 AP_ADC_ADS7844 adc;
 AP_InertialSensor_Oilpan ins( &adc );
+const AP_HAL::HAL& hal = AP_HAL_AVR_APM1;
 #endif
 
-static GPS         *g_gps;
+AP_Compass_HMC5843 compass;
 
-AP_GPS_Auto     g_gps_driver(&Serial1, &g_gps);
+GPS *g_gps;
+
+AP_GPS_Auto g_gps_driver(hal.uart1, &g_gps);
 
 // choose which AHRS system to use
 AP_AHRS_DCM  ahrs(&ins, g_gps);
@@ -63,6 +48,9 @@ AP_AHRS_DCM  ahrs(&ins, g_gps);
 AP_Baro_BMP085_HIL barometer;
 
 
+#define HIGH 1
+#define LOW 0
+
 #ifdef APM2_HARDWARE
  # define A_LED_PIN        27
  # define C_LED_PIN        25
@@ -80,46 +68,32 @@ AP_Baro_BMP085_HIL barometer;
 
 static void flash_leds(bool on)
 {
-    digitalWrite(A_LED_PIN, on ? LED_OFF : LED_ON);
+    hal.gpio->write(A_LED_PIN, on ? LED_OFF : LED_ON);
-    digitalWrite(C_LED_PIN, on ? LED_ON : LED_OFF);
+    hal.gpio->write(C_LED_PIN, on ? LED_ON : LED_OFF);
 }
 
 void setup(void)
 {
-    Serial.begin(115200);
-    Serial.println("Starting up...");
-
-#ifndef DESKTOP_BUILD
-    I2c.begin();
-    I2c.timeOut(5);
-    I2c.setSpeed(true);
-#endif
-
-    SPI.begin();
-    SPI.setClockDivider(SPI_CLOCK_DIV16);
 
 #ifdef APM2_HARDWARE
     // we need to stop the barometer from holding the SPI bus
-    pinMode(40, OUTPUT);
+    hal.gpio->pinMode(40, GPIO_OUTPUT);
-    digitalWrite(40, HIGH);
+    hal.gpio->write(40, HIGH);
 #endif
 
-    isr_registry.init();
-    scheduler.init(&isr_registry);
-
     ins.init(AP_InertialSensor::COLD_START, 
 			 AP_InertialSensor::RATE_100HZ,
-			 delay, flash_leds, &scheduler);
+			 flash_leds);
-    ins.init_accel(delay, flash_leds);
+    ins.init_accel(flash_leds);
 
     compass.set_orientation(MAG_ORIENTATION);
     ahrs.init();
 
     if( compass.init() ) {
-        Serial.printf("Enabling compass\n");
+        hal.console->printf("Enabling compass\n");
         ahrs.set_compass(&compass);
     } else {
-        Serial.printf("No compass detected\n");
+        hal.console->printf("No compass detected\n");
     }
     g_gps = &g_gps_driver;
 #if WITH_GPS
@@ -131,7 +105,7 @@ void loop(void)
 {
     static uint16_t counter;
     static uint32_t last_t, last_print, last_compass;
-    uint32_t now = micros();
+    uint32_t now = hal.scheduler->micros();
     float heading = 0;
 
     if (last_t == 0) {
@@ -153,9 +127,11 @@ void loop(void)
     ahrs.update();
     counter++;
 
-    if (now - last_print >= 0.5e6) {
+    if (now - last_print >= 100000 /* 100ms : 10hz */) {
         Vector3f drift  = ahrs.get_gyro_drift();
-        Serial.printf_P(PSTR("r:%4.1f  p:%4.1f y:%4.1f drift=(%5.1f %5.1f %5.1f) hdg=%.1f rate=%.1f\n"),
+        hal.console->printf_P(
+                PSTR("r:%4.1f  p:%4.1f y:%4.1f "
+                    "drift=(%5.1f %5.1f %5.1f) hdg=%.1f rate=%.1f\n"),
                         ToDeg(ahrs.roll),
                         ToDeg(ahrs.pitch),
                         ToDeg(ahrs.yaw),
@@ -168,3 +144,5 @@ void loop(void)
         counter = 0;
     }
 }
+
+AP_HAL_MAIN();