From 53432a1101cddc7fa180416bed507f30ab6d4c62 Mon Sep 17 00:00:00 2001 From: Pat Hickey Date: Thu, 11 Oct 2012 13:48:39 -0700 Subject: [PATCH] AP_Compass: ported to AP_HAL --- libraries/AP_Compass/AP_Compass_HIL.cpp | 5 +- libraries/AP_Compass/AP_Compass_HMC5843.cpp | 45 ++++++------- libraries/AP_Compass/AP_Compass_HMC5843.h | 2 +- libraries/AP_Compass/Compass.cpp | 6 +- .../AP_Compass_test/AP_Compass_test.pde | 64 +++++++++---------- .../examples/AP_Compass_test/Arduino.h | 0 .../examples/AP_Compass_test/nocore.inoflag | 0 7 files changed, 59 insertions(+), 63 deletions(-) create mode 100644 libraries/AP_Compass/examples/AP_Compass_test/Arduino.h create mode 100644 libraries/AP_Compass/examples/AP_Compass_test/nocore.inoflag diff --git a/libraries/AP_Compass/AP_Compass_HIL.cpp b/libraries/AP_Compass/AP_Compass_HIL.cpp index 6e76690532..a578511563 100644 --- a/libraries/AP_Compass/AP_Compass_HIL.cpp +++ b/libraries/AP_Compass/AP_Compass_HIL.cpp @@ -9,14 +9,17 @@ */ +#include #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; } diff --git a/libraries/AP_Compass/AP_Compass_HMC5843.cpp b/libraries/AP_Compass/AP_Compass_HMC5843.cpp index cbc9e7f60b..d0f8794bb5 100644 --- a/libraries/AP_Compass/AP_Compass_HMC5843.cpp +++ b/libraries/AP_Compass/AP_Compass_HMC5843.cpp @@ -15,16 +15,12 @@ // AVR LibC Includes #include -#include -#if defined(ARDUINO) && ARDUINO >= 100 - #include "Arduino.h" -#else - #include "WConstants.h" -#endif +#include -#include #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); diff --git a/libraries/AP_Compass/AP_Compass_HMC5843.h b/libraries/AP_Compass/AP_Compass_HMC5843.h index f3c17d71b2..f6ce2b2b9a 100644 --- a/libraries/AP_Compass/AP_Compass_HMC5843.h +++ b/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; diff --git a/libraries/AP_Compass/Compass.cpp b/libraries/AP_Compass/Compass.cpp index 1d2f673f91..e40e0ca095 100644 --- a/libraries/AP_Compass/Compass.cpp +++ b/libraries/AP_Compass/Compass.cpp @@ -1,4 +1,5 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#include #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))); } } diff --git a/libraries/AP_Compass/examples/AP_Compass_test/AP_Compass_test.pde b/libraries/AP_Compass/examples/AP_Compass_test/AP_Compass_test.pde index 4d1cd3b9aa..175e8caedd 100644 --- a/libraries/AP_Compass/examples/AP_Compass_test/AP_Compass_test.pde +++ b/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 #include #include -#include // Compass Library -#include // ArduPilot Mega Vector/Matrix math Library -#include +#include +#include -FastSerialPort0(Serial); +#include // ArduPilot Mega Vector/Matrix math Library +#include +#include // 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(); diff --git a/libraries/AP_Compass/examples/AP_Compass_test/Arduino.h b/libraries/AP_Compass/examples/AP_Compass_test/Arduino.h new file mode 100644 index 0000000000..e69de29bb2 diff --git a/libraries/AP_Compass/examples/AP_Compass_test/nocore.inoflag b/libraries/AP_Compass/examples/AP_Compass_test/nocore.inoflag new file mode 100644 index 0000000000..e69de29bb2