AP_Compass: ported to AP_HAL

libraries/AP_Compass/AP_Compass_HIL.cpp

@@ -9,14 +9,17 @@
  */
 
 
+#include <AP_HAL.h>
 #include "AP_Compass_HIL.h"
 
+extern const AP_HAL::HAL& hal;
+
 // Public Methods //////////////////////////////////////////////////////////////
 
 bool AP_Compass_HIL::read()
 {
     // values set by setHIL function
-    last_update = micros();      // record time of update
+    last_update = hal.scheduler->micros();      // record time of update
     return true;
 }
 

libraries/AP_Compass/AP_Compass_HMC5843.cpp

@@ -15,16 +15,12 @@
 
 // AVR LibC Includes
 #include <math.h>
-#include <FastSerial.h>
-#if defined(ARDUINO) && ARDUINO >= 100
- #include "Arduino.h"
-#else
- #include "WConstants.h"
-#endif
+#include <AP_HAL.h>
 
-#include <I2C.h>
 #include "AP_Compass_HMC5843.h"
 
+extern const AP_HAL::HAL& hal;
+
 #define COMPASS_ADDRESS       0x1E
 #define ConfigRegA           0x00
 #define ConfigRegB           0x01
@@ -54,7 +50,7 @@
 // read_register - read a register value
 bool AP_Compass_HMC5843::read_register(uint8_t address, uint8_t *value)
 {
-    if (I2c.read((uint8_t)COMPASS_ADDRESS, address, 1, value) != 0) {
+    if (hal.i2c->readRegister((uint8_t)COMPASS_ADDRESS, address, value) != 0) {
         healthy = false;
         return false;
     }
@@ -62,9 +58,9 @@ bool AP_Compass_HMC5843::read_register(uint8_t address, uint8_t *value)
 }
 
 // write_register - update a register value
-bool AP_Compass_HMC5843::write_register(uint8_t address, byte value)
+bool AP_Compass_HMC5843::write_register(uint8_t address, uint8_t value)
 {
-    if (I2c.write((uint8_t)COMPASS_ADDRESS, address, value) != 0) {
+    if (hal.i2c->writeRegister((uint8_t)COMPASS_ADDRESS, address, value) != 0) {
         healthy = false;
         return false;
     }
@@ -76,10 +72,7 @@ bool AP_Compass_HMC5843::read_raw()
 {
     uint8_t buff[6];
 
-    if (I2c.read(COMPASS_ADDRESS, 0x03, 6, buff) != 0) {
-        if (healthy) {
-            I2c.setSpeed(false);
-        }
+    if (hal.i2c->readRegisters(COMPASS_ADDRESS, 0x03, 6, buff) != 0) {
         healthy = false;
         return false;
     }
@@ -109,7 +102,7 @@ bool AP_Compass_HMC5843::read_raw()
 // accumulate a reading from the magnetometer
 void AP_Compass_HMC5843::accumulate(void)
 {
-   uint32_t tnow = micros();
+   uint32_t tnow = hal.scheduler->micros();
    if (healthy && _accum_count != 0 && (tnow - _last_accum_time) < 13333) {
 	  // the compass gets new data at 75Hz
 	  return;
@@ -154,12 +147,12 @@ AP_Compass_HMC5843::init()
 {
     int numAttempts = 0, good_count = 0;
     bool success = false;
-    byte calibration_gain = 0x20;
+    uint8_t calibration_gain = 0x20;
     uint16_t expected_x = 715;
     uint16_t expected_yz = 715;
     float gain_multiple = 1.0;
 
-    delay(10);
+    hal.scheduler->delay(10);
 
     // determine if we are using 5843 or 5883L
     if (!write_register(ConfigRegA, SampleAveraging_8<<5 | DataOutputRate_75HZ<<2 | NormalOperation) ||
@@ -193,7 +186,7 @@ AP_Compass_HMC5843::init()
         // force positiveBias (compass should return 715 for all channels)
         if (!write_register(ConfigRegA, PositiveBiasConfig))
             continue;      // compass not responding on the bus
-        delay(50);
+        hal.scheduler->delay(50);
 
         // set gains
         if (!write_register(ConfigRegB, calibration_gain) ||
@@ -201,11 +194,11 @@ AP_Compass_HMC5843::init()
             continue;
 
         // read values from the compass
-        delay(50);
+        hal.scheduler->delay(50);
         if (!read_raw())
             continue;      // we didn't read valid values
 
-        delay(10);
+        hal.scheduler->delay(10);
 
         float cal[3];
 
@@ -275,12 +268,12 @@ bool AP_Compass_HMC5843::read()
         return false;
     }
     if (!healthy) {
-        if (millis() < _retry_time) {
+        if (hal.scheduler->millis() < _retry_time) {
             return false;
         }
         if (!re_initialise()) {
-            _retry_time = millis() + 1000;
-			I2c.setSpeed(false);
+            _retry_time = hal.scheduler->millis() + 1000;
+			hal.i2c->setHighSpeed(false);
             return false;
         }
     }
@@ -289,8 +282,8 @@ bool AP_Compass_HMC5843::read()
 	   accumulate();
 	   if (!healthy || _accum_count == 0) {
 		  // try again in 1 second, and set I2c clock speed slower
-		  _retry_time = millis() + 1000;
-		  I2c.setSpeed(false);
+		  _retry_time = hal.scheduler->millis() + 1000;
+		  hal.i2c->setHighSpeed(false);
 		  return false;
 	   }
 	}
@@ -301,7 +294,7 @@ bool AP_Compass_HMC5843::read()
 	_accum_count = 0;
 	_mag_x_accum = _mag_y_accum = _mag_z_accum = 0;
 
-    last_update = micros(); // record time of update
+    last_update = hal.scheduler->micros(); // record time of update
 
     // rotate to the desired orientation
     Vector3f rot_mag = Vector3f(mag_x,mag_y,mag_z);

libraries/AP_Compass/AP_Compass_HMC5843.h

@@ -53,7 +53,7 @@ private:
     uint8_t             _base_config;
     virtual bool        re_initialise(void);
     bool                read_register(uint8_t address, uint8_t *value);
-    bool                write_register(uint8_t address, byte value);
+    bool                write_register(uint8_t address, uint8_t value);
     uint32_t            _retry_time; // when unhealthy the millis() value to retry at
 
     int16_t			    _mag_x;

libraries/AP_Compass/Compass.cpp

@@ -1,4 +1,5 @@
 /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+#include <AP_Progmem.h>
 #include "Compass.h"
 
 const AP_Param::GroupInfo Compass::var_info[] PROGMEM = {
@@ -61,7 +62,10 @@ Compass::set_initial_location(int32_t latitude, int32_t longitude)
     // the declination based on the initial GPS fix
     if (_auto_declination) {
         // Set the declination based on the lat/lng from GPS
-        _declination.set(radians(AP_Declination::get_declination((float)latitude / 10000000, (float)longitude / 10000000)));
+        _declination.set(radians(
+                AP_Declination::get_declination(
+                    (float)latitude / 10000000,
+                    (float)longitude / 10000000)));
     }
 }
 

libraries/AP_Compass/examples/AP_Compass_test/AP_Compass_test.pde

@@ -3,34 +3,28 @@
  *       Code by Jordi MuÒoz and Jose Julio. DIYDrones.com
  */
 
-#include <FastSerial.h>
 #include <AP_Common.h>
 #include <AP_Param.h>
-#include <AP_Compass.h> // Compass Library
-#include <AP_Math.h>            // ArduPilot Mega Vector/Matrix math Library
-#include <I2C.h>
+#include <AP_HAL.h>
+#include <AP_HAL_AVR.h>
 
-FastSerialPort0(Serial);
+#include <AP_Math.h>    // ArduPilot Mega Vector/Matrix math Library
+#include <AP_Declination.h>
+#include <AP_Compass.h> // Compass Library
 
 #define ToRad(x) (x*0.01745329252)  // *pi/180
 #define ToDeg(x) (x*57.2957795131)  // *180/pi
 
+const AP_HAL::HAL& hal = AP_HAL_AVR_APM2;
+
 AP_Compass_HMC5843 compass;
+uint32_t timer;
 
-
-unsigned long timer;
-
-void setup()
-{
-    Serial.begin(115200);
-    Serial.println("Compass library test (HMC5843 and HMC5883L)");
-    I2c.begin();
-    I2c.timeOut(20);
-
-    I2c.setSpeed(true);
+void setup() {
+    hal.console->println("Compass library test (HMC5843 and HMC5883L)");
 
     if (!compass.init()) {
-        Serial.println("compass initialisation failed!");
+        hal.console->println("compass initialisation failed!");
         while (1) ;
     }
 
@@ -38,24 +32,24 @@ void setup()
     compass.set_offsets(0,0,0); // set offsets to account for surrounding interference
     compass.set_declination(ToRad(0.0)); // set local difference between magnetic north and true north
 
-    Serial.print("Compass auto-detected as: ");
+    hal.console->print("Compass auto-detected as: ");
     switch( compass.product_id ) {
     case AP_COMPASS_TYPE_HIL:
-        Serial.println("HIL");
+        hal.console->println("HIL");
         break;
     case AP_COMPASS_TYPE_HMC5843:
-        Serial.println("HMC5843");
+        hal.console->println("HMC5843");
         break;
     case AP_COMPASS_TYPE_HMC5883L:
-        Serial.println("HMC5883L");
+        hal.console->println("HMC5883L");
         break;
     default:
-        Serial.println("unknown");
+        hal.console->println("unknown");
         break;
     }
 
-    delay(3000);
-    timer = micros();
+    hal.scheduler->delay(1000);
+    timer = hal.scheduler->micros();
 }
 
 void loop()
@@ -64,19 +58,19 @@ void loop()
 
     compass.accumulate();
 
-    if((micros()- timer) > 100000L)
+    if((hal.scheduler->micros()- timer) > 100000L)
     {
-        timer = micros();
+        timer = hal.scheduler->micros();
         compass.read();
-        unsigned long read_time = micros() - timer;
+        unsigned long read_time = hal.scheduler->micros() - timer;
         float heading;
 
         if (!compass.healthy) {
-            Serial.println("not healthy");
+            hal.console->println("not healthy");
             return;
         }
         heading = compass.calculate_heading(0,0); // roll = 0, pitch = 0 for this example
-	compass.null_offsets();
+        compass.null_offsets();
 
         // capture min
         if( compass.mag_x < min[0] )
@@ -100,19 +94,21 @@ void loop()
         offset[2] = -(max[2]+min[2])/2;
 
         // display all to user
-        Serial.printf("Heading: %.2f (%3u,%3u,%3u) I2cerr=%u ",
+        hal.console->printf("Heading: %.2f (%3u,%3u,%3u) i2c error: %u",
                       ToDeg(heading),
                       compass.mag_x,
                       compass.mag_y,
                       compass.mag_z, 
-		      I2c.lockup_count());
+		              hal.i2c->lockup_count());
 
         // display offsets
-        Serial.printf("\t offsets(%.2f, %.2f, %.2f)",
+        hal.console->printf(" offsets(%.2f, %.2f, %.2f)",
                       offset[0], offset[1], offset[2]);
 
-        Serial.printf(" t=%u", (unsigned)read_time);
+        hal.console->printf(" t=%u", (unsigned)read_time);
 
-        Serial.println();
+        hal.console->println();
     }
 }
+
+AP_HAL_MAIN();