diff --git a/libraries/AP_Compass/AP_Compass_HMC5843.cpp b/libraries/AP_Compass/AP_Compass_HMC5843.cpp index 7310b1cd47..80a4c36ef8 100644 --- a/libraries/AP_Compass/AP_Compass_HMC5843.cpp +++ b/libraries/AP_Compass/AP_Compass_HMC5843.cpp @@ -1,25 +1,25 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 3; indent-tabs-mode: t -*- /* - AP_Compass_HMC5843.cpp - Arduino Library for HMC5843 I2C magnetometer - Code by Jordi Muñoz and Jose Julio. DIYDrones.com - - This library is free software; you can redistribute it and/or - modify it under the terms of the GNU Lesser General Public - License as published by the Free Software Foundation; either - version 2.1 of the License, or (at your option) any later version. - - Sensor is conected to I2C port - Sensor is initialized in Continuos mode (10Hz) - -*/ + * AP_Compass_HMC5843.cpp - Arduino Library for HMC5843 I2C magnetometer + * Code by Jordi Muñoz and Jose Julio. DIYDrones.com + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * Sensor is conected to I2C port + * Sensor is initialized in Continuos mode (10Hz) + * + */ // AVR LibC Includes #include #include #if defined(ARDUINO) && ARDUINO >= 100 - #include "Arduino.h" + #include "Arduino.h" #else - #include "WConstants.h" + #include "WConstants.h" #endif #include @@ -54,233 +54,233 @@ // 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) { - healthy = false; - return false; - } - return true; + if (I2c.read((uint8_t)COMPASS_ADDRESS, address, 1, value) != 0) { + healthy = false; + return false; + } + return true; } // write_register - update a register value bool AP_Compass_HMC5843::write_register(uint8_t address, byte value) { - if (I2c.write((uint8_t)COMPASS_ADDRESS, address, value) != 0) { - healthy = false; - return false; - } - return true; + if (I2c.write((uint8_t)COMPASS_ADDRESS, address, value) != 0) { + healthy = false; + return false; + } + return true; } // Read Sensor data bool AP_Compass_HMC5843::read_raw() { - uint8_t buff[6]; + uint8_t buff[6]; - if (I2c.read(COMPASS_ADDRESS, 0x03, 6, buff) != 0) { - healthy = false; - return false; - } + if (I2c.read(COMPASS_ADDRESS, 0x03, 6, buff) != 0) { + healthy = false; + return false; + } - int16_t rx, ry, rz; - rx = (int16_t)(buff[0] << 8) | buff[1]; - if (product_id == AP_COMPASS_TYPE_HMC5883L) { - rz = (int16_t)(buff[2] << 8) | buff[3]; - ry = (int16_t)(buff[4] << 8) | buff[5]; - } else { - ry = (int16_t)(buff[2] << 8) | buff[3]; - rz = (int16_t)(buff[4] << 8) | buff[5]; - } - if (rx == -4096 || ry == -4096 || rz == -4096) { - // no valid data available - return false; - } + int16_t rx, ry, rz; + rx = (int16_t)(buff[0] << 8) | buff[1]; + if (product_id == AP_COMPASS_TYPE_HMC5883L) { + rz = (int16_t)(buff[2] << 8) | buff[3]; + ry = (int16_t)(buff[4] << 8) | buff[5]; + } else { + ry = (int16_t)(buff[2] << 8) | buff[3]; + rz = (int16_t)(buff[4] << 8) | buff[5]; + } + if (rx == -4096 || ry == -4096 || rz == -4096) { + // no valid data available + return false; + } - mag_x = -rx; - mag_y = ry; - mag_z = -rz; - - return true; + mag_x = -rx; + mag_y = ry; + mag_z = -rz; + + return true; } /* - re-initialise after a IO error + * re-initialise after a IO error */ bool AP_Compass_HMC5843::re_initialise() { - if (! write_register(ConfigRegA, _base_config) || - ! write_register(ConfigRegB, magGain) || - ! write_register(ModeRegister, ContinuousConversion)) - return false; - return true; + if (!write_register(ConfigRegA, _base_config) || + !write_register(ConfigRegB, magGain) || + !write_register(ModeRegister, ContinuousConversion)) + return false; + return true; } // Public Methods ////////////////////////////////////////////////////////////// -bool +bool AP_Compass_HMC5843::init() { - int numAttempts = 0, good_count = 0; - bool success = false; - byte calibration_gain = 0x20; - uint16_t expected_x = 715; - uint16_t expected_yz = 715; - float gain_multiple = 1.0; + int numAttempts = 0, good_count = 0; + bool success = false; + byte calibration_gain = 0x20; + uint16_t expected_x = 715; + uint16_t expected_yz = 715; + float gain_multiple = 1.0; - delay(10); + delay(10); - // determine if we are using 5843 or 5883L - if (! write_register(ConfigRegA, SampleAveraging_8<<5 | DataOutputRate_75HZ<<2 | NormalOperation) || - ! read_register(ConfigRegA, &_base_config)) { - healthy = false; - return false; - } - if ( _base_config == (SampleAveraging_8<<5 | DataOutputRate_75HZ<<2 | NormalOperation)) { - // a 5883L supports the sample averaging config - product_id = AP_COMPASS_TYPE_HMC5883L; - calibration_gain = 0x60; - expected_x = 766; - expected_yz = 713; - gain_multiple = 660.0 / 1090; // adjustment for runtime vs calibration gain - } else if (_base_config == (NormalOperation | DataOutputRate_75HZ<<2)) { - product_id = AP_COMPASS_TYPE_HMC5843; - } else { - // not behaving like either supported compass type - return false; - } + // determine if we are using 5843 or 5883L + if (!write_register(ConfigRegA, SampleAveraging_8<<5 | DataOutputRate_75HZ<<2 | NormalOperation) || + !read_register(ConfigRegA, &_base_config)) { + healthy = false; + return false; + } + if ( _base_config == (SampleAveraging_8<<5 | DataOutputRate_75HZ<<2 | NormalOperation)) { + // a 5883L supports the sample averaging config + product_id = AP_COMPASS_TYPE_HMC5883L; + calibration_gain = 0x60; + expected_x = 766; + expected_yz = 713; + gain_multiple = 660.0 / 1090; // adjustment for runtime vs calibration gain + } else if (_base_config == (NormalOperation | DataOutputRate_75HZ<<2)) { + product_id = AP_COMPASS_TYPE_HMC5843; + } else { + // not behaving like either supported compass type + return false; + } - calibration[0] = 0; - calibration[1] = 0; - calibration[2] = 0; - - while ( success == 0 && numAttempts < 20 && good_count < 5) - { - // record number of attempts at initialisation - numAttempts++; + calibration[0] = 0; + calibration[1] = 0; + calibration[2] = 0; - // force positiveBias (compass should return 715 for all channels) - if (! write_register(ConfigRegA, PositiveBiasConfig)) - continue; // compass not responding on the bus - delay(50); + while ( success == 0 && numAttempts < 20 && good_count < 5) + { + // record number of attempts at initialisation + numAttempts++; - // set gains - if (! write_register(ConfigRegB, calibration_gain) || - ! write_register(ModeRegister, SingleConversion)) - continue; + // force positiveBias (compass should return 715 for all channels) + if (!write_register(ConfigRegA, PositiveBiasConfig)) + continue; // compass not responding on the bus + delay(50); - // read values from the compass - delay(50); - if (!read_raw()) - continue; // we didn't read valid values + // set gains + if (!write_register(ConfigRegB, calibration_gain) || + !write_register(ModeRegister, SingleConversion)) + continue; - delay(10); + // read values from the compass + delay(50); + if (!read_raw()) + continue; // we didn't read valid values - float cal[3]; + delay(10); - cal[0] = fabs(expected_x / (float)mag_x); - cal[1] = fabs(expected_yz / (float)mag_y); - cal[2] = fabs(expected_yz / (float)mag_z); + float cal[3]; - if (cal[0] > 0.7 && cal[0] < 1.3 && - cal[1] > 0.7 && cal[1] < 1.3 && - cal[2] > 0.7 && cal[2] < 1.3) { - good_count++; - calibration[0] += cal[0]; - calibration[1] += cal[1]; - calibration[2] += cal[2]; - } + cal[0] = fabs(expected_x / (float)mag_x); + cal[1] = fabs(expected_yz / (float)mag_y); + cal[2] = fabs(expected_yz / (float)mag_z); + + if (cal[0] > 0.7 && cal[0] < 1.3 && + cal[1] > 0.7 && cal[1] < 1.3 && + cal[2] > 0.7 && cal[2] < 1.3) { + good_count++; + calibration[0] += cal[0]; + calibration[1] += cal[1]; + calibration[2] += cal[2]; + } #if 0 - /* useful for debugging */ - Serial.print("mag_x: "); - Serial.print(mag_x); - Serial.print(" mag_y: "); - Serial.print(mag_y); - Serial.print(" mag_z: "); - Serial.println(mag_z); - Serial.print("CalX: "); - Serial.print(calibration[0]/good_count); - Serial.print(" CalY: "); - Serial.print(calibration[1]/good_count); - Serial.print(" CalZ: "); - Serial.println(calibration[2]/good_count); + /* useful for debugging */ + Serial.print("mag_x: "); + Serial.print(mag_x); + Serial.print(" mag_y: "); + Serial.print(mag_y); + Serial.print(" mag_z: "); + Serial.println(mag_z); + Serial.print("CalX: "); + Serial.print(calibration[0]/good_count); + Serial.print(" CalY: "); + Serial.print(calibration[1]/good_count); + Serial.print(" CalZ: "); + Serial.println(calibration[2]/good_count); #endif - } + } - if (good_count >= 5) { - calibration[0] = calibration[0] * gain_multiple / good_count; - calibration[1] = calibration[1] * gain_multiple / good_count; - calibration[2] = calibration[2] * gain_multiple / good_count; - success = true; - } else { - /* best guess */ - calibration[0] = 1.0; - calibration[1] = 1.0; - calibration[2] = 1.0; - } + if (good_count >= 5) { + calibration[0] = calibration[0] * gain_multiple / good_count; + calibration[1] = calibration[1] * gain_multiple / good_count; + calibration[2] = calibration[2] * gain_multiple / good_count; + success = true; + } else { + /* best guess */ + calibration[0] = 1.0; + calibration[1] = 1.0; + calibration[2] = 1.0; + } - // leave test mode - if (!re_initialise()) { - return false; - } + // leave test mode + if (!re_initialise()) { + return false; + } - _initialised = true; + _initialised = true; - return success; + return success; } // Read Sensor data bool AP_Compass_HMC5843::read() { - if (!_initialised) { - // someone has tried to enable a compass for the first time - // mid-flight .... we can't do that yet (especially as we won't - // have the right orientation!) - return false; - } - if (!healthy) { - if (millis() < _retry_time) { - return false; - } - if (!re_initialise()) { - _retry_time = millis() + 1000; - return false; - } - } + if (!_initialised) { + // someone has tried to enable a compass for the first time + // mid-flight .... we can't do that yet (especially as we won't + // have the right orientation!) + return false; + } + if (!healthy) { + if (millis() < _retry_time) { + return false; + } + if (!re_initialise()) { + _retry_time = millis() + 1000; + return false; + } + } - if (!read_raw()) { - // try again in 1 second, and set I2c clock speed slower - _retry_time = millis() + 1000; - I2c.setSpeed(false); - return false; - } + if (!read_raw()) { + // try again in 1 second, and set I2c clock speed slower + _retry_time = millis() + 1000; + I2c.setSpeed(false); + return false; + } - mag_x *= calibration[0]; - mag_y *= calibration[1]; - mag_z *= calibration[2]; + mag_x *= calibration[0]; + mag_y *= calibration[1]; + mag_z *= calibration[2]; - last_update = micros(); // record time of update + last_update = micros(); // record time of update - // rotate to the desired orientation - Vector3f rot_mag = Vector3f(mag_x,mag_y,mag_z); - if (product_id == AP_COMPASS_TYPE_HMC5883L) { - rot_mag.rotate(ROTATION_YAW_90); - } - rot_mag.rotate(_orientation); + // rotate to the desired orientation + Vector3f rot_mag = Vector3f(mag_x,mag_y,mag_z); + if (product_id == AP_COMPASS_TYPE_HMC5883L) { + rot_mag.rotate(ROTATION_YAW_90); + } + rot_mag.rotate(_orientation); - rot_mag += _offset.get(); - mag_x = rot_mag.x; - mag_y = rot_mag.y; - mag_z = rot_mag.z; - healthy = true; + rot_mag += _offset.get(); + mag_x = rot_mag.x; + mag_y = rot_mag.y; + mag_z = rot_mag.z; + healthy = true; - return true; + return true; } // set orientation void AP_Compass_HMC5843::set_orientation(enum Rotation rotation) { - _orientation = rotation; + _orientation = rotation; }