2013-08-29 02:34:34 -03:00
|
|
|
/*
|
|
|
|
This program is free software: you can redistribute it and/or modify
|
|
|
|
it under the terms of the GNU General Public License as published by
|
|
|
|
the Free Software Foundation, either version 3 of the License, or
|
|
|
|
(at your option) any later version.
|
|
|
|
|
|
|
|
This program is distributed in the hope that it will be useful,
|
|
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
|
|
GNU General Public License for more details.
|
|
|
|
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
|
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
|
|
*/
|
|
|
|
|
2011-02-14 00:27:07 -04:00
|
|
|
/*
|
2012-08-17 03:19:22 -03:00
|
|
|
* AP_Compass_HMC5843.cpp - Arduino Library for HMC5843 I2C magnetometer
|
|
|
|
* Code by Jordi Muñoz and Jose Julio. DIYDrones.com
|
|
|
|
*
|
2016-05-12 13:56:35 -03:00
|
|
|
* Sensor is connected to I2C port
|
2012-08-17 03:19:22 -03:00
|
|
|
* Sensor is initialized in Continuos mode (10Hz)
|
|
|
|
*
|
|
|
|
*/
|
2016-03-04 13:59:06 -04:00
|
|
|
#include <AP_HAL/AP_HAL.h>
|
|
|
|
|
|
|
|
#ifdef HAL_COMPASS_HMC5843_I2C_ADDR
|
|
|
|
|
|
|
|
#include <assert.h>
|
|
|
|
#include <utility>
|
2016-10-31 22:17:13 -03:00
|
|
|
#include <stdio.h>
|
2011-02-14 00:27:07 -04:00
|
|
|
|
2015-08-11 03:28:42 -03:00
|
|
|
#include <AP_Math/AP_Math.h>
|
|
|
|
#include <AP_HAL/AP_HAL.h>
|
2016-03-04 13:59:06 -04:00
|
|
|
#include <AP_HAL/utility/sparse-endian.h>
|
2011-02-14 00:27:07 -04:00
|
|
|
|
|
|
|
#include "AP_Compass_HMC5843.h"
|
2015-08-10 20:30:32 -03:00
|
|
|
#include <AP_InertialSensor/AP_InertialSensor.h>
|
|
|
|
#include <AP_InertialSensor/AuxiliaryBus.h>
|
2011-02-14 00:27:07 -04:00
|
|
|
|
2012-10-11 17:48:39 -03:00
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
|
2016-03-07 20:10:08 -04:00
|
|
|
/*
|
|
|
|
* Defaul address: 0x1E
|
|
|
|
*/
|
|
|
|
|
|
|
|
#define HMC5843_REG_CONFIG_A 0x00
|
|
|
|
// Valid sample averaging for 5883L
|
|
|
|
#define HMC5843_SAMPLE_AVERAGING_1 (0x00 << 5)
|
|
|
|
#define HMC5843_SAMPLE_AVERAGING_2 (0x01 << 5)
|
|
|
|
#define HMC5843_SAMPLE_AVERAGING_4 (0x02 << 5)
|
|
|
|
#define HMC5843_SAMPLE_AVERAGING_8 (0x03 << 5)
|
2016-11-07 04:04:40 -04:00
|
|
|
|
|
|
|
#define HMC5843_CONF_TEMP_ENABLE (0x80)
|
|
|
|
|
2016-03-07 20:10:08 -04:00
|
|
|
// Valid data output rates for 5883L
|
|
|
|
#define HMC5843_OSR_0_75HZ (0x00 << 2)
|
|
|
|
#define HMC5843_OSR_1_5HZ (0x01 << 2)
|
|
|
|
#define HMC5843_OSR_3HZ (0x02 << 2)
|
|
|
|
#define HMC5843_OSR_7_5HZ (0x03 << 2)
|
|
|
|
#define HMC5843_OSR_15HZ (0x04 << 2)
|
|
|
|
#define HMC5843_OSR_30HZ (0x05 << 2)
|
|
|
|
#define HMC5843_OSR_75HZ (0x06 << 2)
|
|
|
|
// Sensor operation modes
|
|
|
|
#define HMC5843_OPMODE_NORMAL 0x00
|
|
|
|
#define HMC5843_OPMODE_POSITIVE_BIAS 0x01
|
|
|
|
#define HMC5843_OPMODE_NEGATIVE_BIAS 0x02
|
|
|
|
#define HMC5843_OPMODE_MASK 0x03
|
|
|
|
|
|
|
|
#define HMC5843_REG_CONFIG_B 0x01
|
|
|
|
#define HMC5883L_GAIN_0_88_GA (0x00 << 5)
|
|
|
|
#define HMC5883L_GAIN_1_30_GA (0x01 << 5)
|
|
|
|
#define HMC5883L_GAIN_1_90_GA (0x02 << 5)
|
|
|
|
#define HMC5883L_GAIN_2_50_GA (0x03 << 5)
|
|
|
|
#define HMC5883L_GAIN_4_00_GA (0x04 << 5)
|
|
|
|
#define HMC5883L_GAIN_4_70_GA (0x05 << 5)
|
|
|
|
#define HMC5883L_GAIN_5_60_GA (0x06 << 5)
|
|
|
|
#define HMC5883L_GAIN_8_10_GA (0x07 << 5)
|
|
|
|
|
|
|
|
#define HMC5843_GAIN_0_70_GA (0x00 << 5)
|
|
|
|
#define HMC5843_GAIN_1_00_GA (0x01 << 5)
|
|
|
|
#define HMC5843_GAIN_1_50_GA (0x02 << 5)
|
|
|
|
#define HMC5843_GAIN_2_00_GA (0x03 << 5)
|
|
|
|
#define HMC5843_GAIN_3_20_GA (0x04 << 5)
|
|
|
|
#define HMC5843_GAIN_3_80_GA (0x05 << 5)
|
|
|
|
#define HMC5843_GAIN_4_50_GA (0x06 << 5)
|
|
|
|
#define HMC5843_GAIN_6_50_GA (0x07 << 5)
|
|
|
|
|
|
|
|
#define HMC5843_REG_MODE 0x02
|
|
|
|
#define HMC5843_MODE_CONTINUOUS 0x00
|
|
|
|
#define HMC5843_MODE_SINGLE 0x01
|
|
|
|
|
|
|
|
#define HMC5843_REG_DATA_OUTPUT_X_MSB 0x03
|
2016-03-04 13:59:06 -04:00
|
|
|
|
2016-11-07 04:04:40 -04:00
|
|
|
#define HMC5843_REG_ID_A 0x0A
|
|
|
|
|
|
|
|
|
2016-01-22 11:30:30 -04:00
|
|
|
AP_Compass_HMC5843::AP_Compass_HMC5843(Compass &compass, AP_HMC5843_BusDriver *bus,
|
2016-11-06 06:04:48 -04:00
|
|
|
bool force_external, enum Rotation rotation)
|
2016-03-04 13:59:06 -04:00
|
|
|
: AP_Compass_Backend(compass)
|
|
|
|
, _bus(bus)
|
2016-11-06 06:04:48 -04:00
|
|
|
, _rotation(rotation)
|
2016-11-23 03:02:30 -04:00
|
|
|
, _force_external(force_external)
|
2016-03-04 13:59:06 -04:00
|
|
|
{
|
|
|
|
}
|
2014-11-15 21:58:23 -04:00
|
|
|
|
2015-08-07 19:54:58 -03:00
|
|
|
AP_Compass_HMC5843::~AP_Compass_HMC5843()
|
|
|
|
{
|
|
|
|
delete _bus;
|
|
|
|
}
|
|
|
|
|
2016-03-04 13:59:06 -04:00
|
|
|
AP_Compass_Backend *AP_Compass_HMC5843::probe(Compass &compass,
|
2016-11-04 03:57:52 -03:00
|
|
|
AP_HAL::OwnPtr<AP_HAL::Device> dev,
|
2016-11-06 06:04:48 -04:00
|
|
|
bool force_external,
|
|
|
|
enum Rotation rotation)
|
2015-08-07 19:54:58 -03:00
|
|
|
{
|
2016-11-07 23:46:39 -04:00
|
|
|
if (!dev) {
|
|
|
|
return nullptr;
|
|
|
|
}
|
2016-03-04 13:59:06 -04:00
|
|
|
AP_HMC5843_BusDriver *bus = new AP_HMC5843_BusDriver_HALDevice(std::move(dev));
|
|
|
|
if (!bus) {
|
2015-08-07 19:54:58 -03:00
|
|
|
return nullptr;
|
2016-03-04 13:59:06 -04:00
|
|
|
}
|
2015-08-07 19:54:58 -03:00
|
|
|
|
2016-11-06 06:04:48 -04:00
|
|
|
AP_Compass_HMC5843 *sensor = new AP_Compass_HMC5843(compass, bus, force_external, rotation);
|
2016-03-04 13:59:06 -04:00
|
|
|
if (!sensor || !sensor->init()) {
|
|
|
|
delete sensor;
|
2015-08-10 20:30:32 -03:00
|
|
|
return nullptr;
|
2016-03-04 13:59:06 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
return sensor;
|
2015-08-10 20:30:32 -03:00
|
|
|
}
|
|
|
|
|
2016-11-06 06:04:48 -04:00
|
|
|
AP_Compass_Backend *AP_Compass_HMC5843::probe_mpu6000(Compass &compass, enum Rotation rotation)
|
2014-11-15 21:58:23 -04:00
|
|
|
{
|
2016-03-04 13:59:06 -04:00
|
|
|
AP_InertialSensor &ins = *AP_InertialSensor::get_instance();
|
|
|
|
|
|
|
|
AP_HMC5843_BusDriver *bus =
|
|
|
|
new AP_HMC5843_BusDriver_Auxiliary(ins, HAL_INS_MPU60XX_SPI,
|
|
|
|
HAL_COMPASS_HMC5843_I2C_ADDR);
|
|
|
|
if (!bus) {
|
2015-08-07 19:54:58 -03:00
|
|
|
return nullptr;
|
2014-11-15 21:58:23 -04:00
|
|
|
}
|
2016-03-04 13:59:06 -04:00
|
|
|
|
2016-11-06 06:04:48 -04:00
|
|
|
AP_Compass_HMC5843 *sensor = new AP_Compass_HMC5843(compass, bus, false, rotation);
|
2016-03-04 13:59:06 -04:00
|
|
|
if (!sensor || !sensor->init()) {
|
2014-11-15 21:58:23 -04:00
|
|
|
delete sensor;
|
2015-08-07 19:54:58 -03:00
|
|
|
return nullptr;
|
2014-11-15 21:58:23 -04:00
|
|
|
}
|
2015-08-07 19:54:58 -03:00
|
|
|
|
2014-11-15 21:58:23 -04:00
|
|
|
return sensor;
|
|
|
|
}
|
|
|
|
|
2016-03-04 13:59:06 -04:00
|
|
|
bool AP_Compass_HMC5843::init()
|
2011-07-09 09:10:00 -03:00
|
|
|
{
|
2016-03-04 13:59:06 -04:00
|
|
|
AP_HAL::Semaphore *bus_sem = _bus->get_semaphore();
|
|
|
|
|
|
|
|
if (!bus_sem || !bus_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
|
|
|
|
hal.console->printf("HMC5843: Unable to get bus semaphore\n");
|
2016-11-03 21:27:36 -03:00
|
|
|
return false;
|
2012-08-17 03:19:22 -03:00
|
|
|
}
|
2011-07-09 09:10:00 -03:00
|
|
|
|
2016-12-01 00:00:34 -04:00
|
|
|
// high retries for init
|
|
|
|
_bus->set_retries(10);
|
|
|
|
|
2016-03-04 13:59:06 -04:00
|
|
|
if (!_bus->configure()) {
|
|
|
|
hal.console->printf("HMC5843: Could not configure the bus\n");
|
|
|
|
goto errout;
|
2012-08-17 03:19:22 -03:00
|
|
|
}
|
2011-07-09 09:10:00 -03:00
|
|
|
|
2016-11-07 04:04:40 -04:00
|
|
|
if (!_check_whoami()) {
|
|
|
|
hal.console->printf("HMC5843: not a HMC device\n");
|
2016-03-04 13:59:06 -04:00
|
|
|
goto errout;
|
|
|
|
}
|
2012-08-17 03:19:22 -03:00
|
|
|
|
2016-03-04 13:59:06 -04:00
|
|
|
if (!_calibrate()) {
|
|
|
|
hal.console->printf("HMC5843: Could not calibrate sensor\n");
|
|
|
|
goto errout;
|
2012-08-17 03:19:22 -03:00
|
|
|
}
|
|
|
|
|
2016-03-07 20:10:08 -04:00
|
|
|
if (!_setup_sampling_mode()) {
|
2016-03-04 13:59:06 -04:00
|
|
|
goto errout;
|
2012-08-17 03:19:22 -03:00
|
|
|
}
|
2016-03-04 13:59:06 -04:00
|
|
|
|
|
|
|
if (!_bus->start_measurements()) {
|
|
|
|
hal.console->printf("HMC5843: Could not start measurements on bus\n");
|
|
|
|
goto errout;
|
2012-08-17 03:19:22 -03:00
|
|
|
}
|
|
|
|
|
2016-03-04 13:59:06 -04:00
|
|
|
_initialised = true;
|
|
|
|
|
2016-12-01 00:00:34 -04:00
|
|
|
// lower retries for run
|
|
|
|
_bus->set_retries(3);
|
|
|
|
|
2016-03-04 13:59:06 -04:00
|
|
|
bus_sem->give();
|
|
|
|
|
|
|
|
// perform an initial read
|
|
|
|
read();
|
|
|
|
|
|
|
|
_compass_instance = register_compass();
|
2016-11-04 06:24:53 -03:00
|
|
|
|
2016-11-06 06:04:48 -04:00
|
|
|
set_rotation(_compass_instance, _rotation);
|
|
|
|
|
2016-11-04 21:24:21 -03:00
|
|
|
_bus->set_device_type(DEVTYPE_HMC5883);
|
2016-11-04 06:24:53 -03:00
|
|
|
set_dev_id(_compass_instance, _bus->get_bus_id());
|
2016-03-04 13:59:06 -04:00
|
|
|
|
2016-01-22 11:30:30 -04:00
|
|
|
if (_force_external) {
|
|
|
|
set_external(_compass_instance, true);
|
|
|
|
}
|
2012-08-17 03:19:22 -03:00
|
|
|
|
2016-10-31 22:17:13 -03:00
|
|
|
// read from sensor at 75Hz
|
|
|
|
_bus->register_periodic_callback(13333,
|
|
|
|
FUNCTOR_BIND_MEMBER(&AP_Compass_HMC5843::_timer, bool));
|
2016-11-08 23:57:07 -04:00
|
|
|
|
|
|
|
hal.console->printf("HMC5843 found on bus 0x%x\n", _bus->get_bus_id());
|
2016-10-31 22:17:13 -03:00
|
|
|
|
2012-08-17 03:19:22 -03:00
|
|
|
return true;
|
2011-08-13 05:17:25 -03:00
|
|
|
|
2016-03-04 13:59:06 -04:00
|
|
|
errout:
|
|
|
|
bus_sem->give();
|
|
|
|
return false;
|
|
|
|
}
|
2011-07-03 05:32:58 -03:00
|
|
|
|
2016-03-04 13:59:06 -04:00
|
|
|
/*
|
2016-10-31 22:17:13 -03:00
|
|
|
* take a reading from the magnetometer
|
2016-03-04 13:59:06 -04:00
|
|
|
*
|
2016-10-31 22:17:13 -03:00
|
|
|
* bus semaphore has been taken already by HAL
|
2016-03-04 13:59:06 -04:00
|
|
|
*/
|
2016-10-31 22:17:13 -03:00
|
|
|
bool AP_Compass_HMC5843::_timer()
|
2012-08-26 00:42:00 -03:00
|
|
|
{
|
2016-10-31 22:17:13 -03:00
|
|
|
bool result = _read_sample();
|
2016-11-07 04:04:40 -04:00
|
|
|
|
|
|
|
// always ask for a new sample
|
|
|
|
_take_sample();
|
|
|
|
|
2016-10-31 22:17:13 -03:00
|
|
|
if (!result) {
|
|
|
|
return true;
|
2013-10-07 21:10:53 -03:00
|
|
|
}
|
2015-07-23 07:53:50 -03:00
|
|
|
|
2016-11-04 06:24:53 -03:00
|
|
|
uint32_t tnow = AP_HAL::micros();
|
2016-10-31 22:17:13 -03:00
|
|
|
|
|
|
|
// the _mag_N values are in the range -2048 to 2047, so we can
|
|
|
|
// accumulate up to 15 of them in an int16_t. Let's make it 14
|
|
|
|
// for ease of calculation. We expect to do reads at 10Hz, and
|
|
|
|
// we get new data at most 75Hz, so we don't expect to
|
|
|
|
// accumulate more than 8 before a read
|
|
|
|
// get raw_field - sensor frame, uncorrected
|
|
|
|
Vector3f raw_field = Vector3f(_mag_x, _mag_y, _mag_z);
|
|
|
|
raw_field *= _gain_scale;
|
|
|
|
|
|
|
|
// rotate to the desired orientation
|
2016-11-07 04:04:40 -04:00
|
|
|
if (is_external(_compass_instance)) {
|
2016-10-31 22:17:13 -03:00
|
|
|
raw_field.rotate(ROTATION_YAW_90);
|
|
|
|
}
|
|
|
|
|
|
|
|
// rotate raw_field from sensor frame to body frame
|
|
|
|
rotate_field(raw_field, _compass_instance);
|
|
|
|
|
|
|
|
// publish raw_field (uncorrected point sample) for calibration use
|
|
|
|
publish_raw_field(raw_field, tnow, _compass_instance);
|
|
|
|
|
|
|
|
// correct raw_field for known errors
|
|
|
|
correct_field(raw_field, _compass_instance);
|
|
|
|
|
2016-11-03 21:47:43 -03:00
|
|
|
if (_sem->take(0)) {
|
2016-10-31 22:17:13 -03:00
|
|
|
_mag_x_accum += raw_field.x;
|
|
|
|
_mag_y_accum += raw_field.y;
|
|
|
|
_mag_z_accum += raw_field.z;
|
|
|
|
_accum_count++;
|
|
|
|
if (_accum_count == 14) {
|
|
|
|
_mag_x_accum /= 2;
|
|
|
|
_mag_y_accum /= 2;
|
|
|
|
_mag_z_accum /= 2;
|
|
|
|
_accum_count = 7;
|
|
|
|
}
|
2016-11-03 21:47:43 -03:00
|
|
|
_sem->give();
|
2016-10-31 22:17:13 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
return true;
|
2016-03-04 13:59:06 -04:00
|
|
|
}
|
2011-12-28 05:31:36 -04:00
|
|
|
|
|
|
|
/*
|
2016-03-04 13:59:06 -04:00
|
|
|
* Take accumulated reads from the magnetometer or try to read once if no
|
|
|
|
* valid data
|
|
|
|
*
|
|
|
|
* bus semaphore must not be locked
|
2011-12-28 05:31:36 -04:00
|
|
|
*/
|
2016-03-04 13:59:06 -04:00
|
|
|
void AP_Compass_HMC5843::read()
|
2011-12-28 05:31:36 -04:00
|
|
|
{
|
2016-03-04 13:59:06 -04:00
|
|
|
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;
|
|
|
|
}
|
|
|
|
|
2016-11-03 21:47:43 -03:00
|
|
|
if (!_sem->take_nonblocking()) {
|
2016-10-31 22:17:13 -03:00
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
2016-03-04 13:59:06 -04:00
|
|
|
if (_accum_count == 0) {
|
2016-11-03 21:47:43 -03:00
|
|
|
_sem->give();
|
2016-10-31 22:17:13 -03:00
|
|
|
return;
|
2016-03-04 13:59:06 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
Vector3f field(_mag_x_accum * _scaling[0],
|
|
|
|
_mag_y_accum * _scaling[1],
|
|
|
|
_mag_z_accum * _scaling[2]);
|
|
|
|
field /= _accum_count;
|
|
|
|
|
|
|
|
_accum_count = 0;
|
|
|
|
_mag_x_accum = _mag_y_accum = _mag_z_accum = 0;
|
|
|
|
|
2016-11-03 21:47:43 -03:00
|
|
|
_sem->give();
|
2016-10-31 22:17:13 -03:00
|
|
|
|
2016-03-04 13:59:06 -04:00
|
|
|
publish_filtered_field(field, _compass_instance);
|
|
|
|
}
|
|
|
|
|
2016-03-07 20:10:08 -04:00
|
|
|
bool AP_Compass_HMC5843::_setup_sampling_mode()
|
2016-03-04 13:59:06 -04:00
|
|
|
{
|
2016-11-07 04:04:40 -04:00
|
|
|
_gain_scale = (1.0f / 1090) * 1000;
|
|
|
|
if (!_bus->register_write(HMC5843_REG_CONFIG_A,
|
|
|
|
HMC5843_CONF_TEMP_ENABLE |
|
|
|
|
HMC5843_OSR_75HZ |
|
|
|
|
HMC5843_SAMPLE_AVERAGING_1) ||
|
|
|
|
!_bus->register_write(HMC5843_REG_CONFIG_B,
|
|
|
|
HMC5883L_GAIN_1_30_GA) ||
|
|
|
|
!_bus->register_write(HMC5843_REG_MODE,
|
|
|
|
HMC5843_MODE_SINGLE)) {
|
2012-08-17 03:19:22 -03:00
|
|
|
return false;
|
2016-03-04 13:59:06 -04:00
|
|
|
}
|
2012-08-17 03:19:22 -03:00
|
|
|
return true;
|
2011-12-28 05:31:36 -04:00
|
|
|
}
|
|
|
|
|
2016-03-04 13:59:06 -04:00
|
|
|
/*
|
|
|
|
* Read Sensor data - bus semaphore must be taken
|
|
|
|
*/
|
|
|
|
bool AP_Compass_HMC5843::_read_sample()
|
|
|
|
{
|
|
|
|
struct PACKED {
|
|
|
|
be16_t rx;
|
|
|
|
be16_t ry;
|
|
|
|
be16_t rz;
|
|
|
|
} val;
|
|
|
|
int16_t rx, ry, rz;
|
|
|
|
|
2016-03-07 20:10:08 -04:00
|
|
|
if (!_bus->block_read(HMC5843_REG_DATA_OUTPUT_X_MSB, (uint8_t *) &val, sizeof(val))){
|
2016-03-04 13:59:06 -04:00
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
|
|
rx = be16toh(val.rx);
|
2016-11-07 04:04:40 -04:00
|
|
|
ry = be16toh(val.rz);
|
|
|
|
rz = be16toh(val.ry);
|
2016-03-04 13:59:06 -04:00
|
|
|
|
|
|
|
if (rx == -4096 || ry == -4096 || rz == -4096) {
|
|
|
|
// no valid data available
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
|
|
_mag_x = -rx;
|
|
|
|
_mag_y = ry;
|
|
|
|
_mag_z = -rz;
|
|
|
|
|
|
|
|
return true;
|
|
|
|
}
|
2011-12-28 05:31:36 -04:00
|
|
|
|
2016-11-07 04:04:40 -04:00
|
|
|
|
|
|
|
/*
|
|
|
|
ask for a new oneshot sample
|
|
|
|
*/
|
|
|
|
void AP_Compass_HMC5843::_take_sample()
|
2015-07-22 12:09:25 -03:00
|
|
|
{
|
2016-11-07 04:04:40 -04:00
|
|
|
_bus->register_write(HMC5843_REG_MODE,
|
|
|
|
HMC5843_MODE_SINGLE);
|
|
|
|
}
|
2015-07-22 12:09:25 -03:00
|
|
|
|
2016-11-07 04:04:40 -04:00
|
|
|
bool AP_Compass_HMC5843::_check_whoami()
|
|
|
|
{
|
|
|
|
uint8_t id[3];
|
|
|
|
if (!_bus->block_read(HMC5843_REG_ID_A, id, 3)) {
|
|
|
|
// can't talk on bus
|
|
|
|
return false;
|
2015-07-22 12:09:25 -03:00
|
|
|
}
|
2016-11-07 04:04:40 -04:00
|
|
|
if (id[0] != 'H' ||
|
|
|
|
id[1] != '4' ||
|
|
|
|
id[2] != '3') {
|
|
|
|
// not a HMC5x83 device
|
2015-07-22 12:09:25 -03:00
|
|
|
return false;
|
|
|
|
}
|
2016-03-04 13:59:06 -04:00
|
|
|
|
|
|
|
return true;
|
2015-07-22 12:09:25 -03:00
|
|
|
}
|
|
|
|
|
2016-03-04 13:59:06 -04:00
|
|
|
bool AP_Compass_HMC5843::_calibrate()
|
2011-02-14 00:27:07 -04:00
|
|
|
{
|
2016-03-07 20:10:08 -04:00
|
|
|
uint8_t calibration_gain;
|
2016-03-04 13:59:06 -04:00
|
|
|
int numAttempts = 0, good_count = 0;
|
|
|
|
bool success = false;
|
2013-01-04 18:26:26 -04:00
|
|
|
|
2016-11-07 04:04:40 -04:00
|
|
|
calibration_gain = HMC5883L_GAIN_2_50_GA;
|
2012-08-17 03:19:22 -03:00
|
|
|
|
2016-11-07 04:04:40 -04:00
|
|
|
/*
|
|
|
|
* the expected values are based on observation of real sensors
|
|
|
|
*/
|
|
|
|
float expected[3] = { 1.16*600, 1.08*600, 1.16*600 };
|
2016-03-07 20:10:08 -04:00
|
|
|
|
2016-11-08 23:57:07 -04:00
|
|
|
uint8_t base_config = HMC5843_OSR_15HZ;
|
|
|
|
uint8_t num_samples = 0;
|
2016-11-07 04:04:40 -04:00
|
|
|
|
2016-03-04 13:59:06 -04:00
|
|
|
while (success == 0 && numAttempts < 25 && good_count < 5) {
|
2012-08-17 03:19:22 -03:00
|
|
|
numAttempts++;
|
|
|
|
|
|
|
|
// force positiveBias (compass should return 715 for all channels)
|
2016-03-07 20:10:08 -04:00
|
|
|
if (!_bus->register_write(HMC5843_REG_CONFIG_A,
|
2016-11-08 23:57:07 -04:00
|
|
|
base_config | HMC5843_OPMODE_POSITIVE_BIAS)) {
|
2016-03-07 20:10:08 -04:00
|
|
|
// compass not responding on the bus
|
|
|
|
continue;
|
|
|
|
}
|
2015-07-23 07:53:50 -03:00
|
|
|
|
2012-10-11 17:48:39 -03:00
|
|
|
hal.scheduler->delay(50);
|
2012-08-17 03:19:22 -03:00
|
|
|
|
|
|
|
// set gains
|
2016-03-07 20:10:08 -04:00
|
|
|
if (!_bus->register_write(HMC5843_REG_CONFIG_B, calibration_gain) ||
|
|
|
|
!_bus->register_write(HMC5843_REG_MODE, HMC5843_MODE_SINGLE)) {
|
2012-08-17 03:19:22 -03:00
|
|
|
continue;
|
2016-03-07 20:10:08 -04:00
|
|
|
}
|
2012-08-17 03:19:22 -03:00
|
|
|
|
|
|
|
// read values from the compass
|
2012-10-11 17:48:39 -03:00
|
|
|
hal.scheduler->delay(50);
|
2016-03-04 13:59:06 -04:00
|
|
|
if (!_read_sample()) {
|
|
|
|
// we didn't read valid values
|
|
|
|
continue;
|
|
|
|
}
|
2012-08-17 03:19:22 -03:00
|
|
|
|
2016-11-08 23:57:07 -04:00
|
|
|
num_samples++;
|
|
|
|
|
2012-08-17 03:19:22 -03:00
|
|
|
float cal[3];
|
|
|
|
|
2015-10-25 17:10:41 -03:00
|
|
|
// hal.console->printf("mag %d %d %d\n", _mag_x, _mag_y, _mag_z);
|
2015-07-23 07:53:50 -03:00
|
|
|
|
2016-11-07 04:04:40 -04:00
|
|
|
cal[0] = fabsf(expected[0] / _mag_x);
|
|
|
|
cal[1] = fabsf(expected[1] / _mag_y);
|
|
|
|
cal[2] = fabsf(expected[2] / _mag_z);
|
2012-08-17 03:19:22 -03:00
|
|
|
|
2015-10-25 17:10:41 -03:00
|
|
|
// hal.console->printf("cal=%.2f %.2f %.2f\n", cal[0], cal[1], cal[2]);
|
2014-01-22 01:07:50 -04:00
|
|
|
|
|
|
|
// we throw away the first two samples as the compass may
|
|
|
|
// still be changing its state from the application of the
|
|
|
|
// strap excitation. After that we accept values in a
|
|
|
|
// reasonable range
|
2015-07-23 10:29:39 -03:00
|
|
|
if (numAttempts <= 2) {
|
|
|
|
continue;
|
|
|
|
}
|
|
|
|
|
2015-07-23 07:52:02 -03:00
|
|
|
#define IS_CALIBRATION_VALUE_VALID(val) (val > 0.7f && val < 1.35f)
|
|
|
|
|
2015-07-23 10:29:39 -03:00
|
|
|
if (IS_CALIBRATION_VALUE_VALID(cal[0]) &&
|
2015-07-23 07:52:02 -03:00
|
|
|
IS_CALIBRATION_VALUE_VALID(cal[1]) &&
|
|
|
|
IS_CALIBRATION_VALUE_VALID(cal[2])) {
|
2015-10-25 17:10:41 -03:00
|
|
|
// hal.console->printf("car=%.2f %.2f %.2f good\n", cal[0], cal[1], cal[2]);
|
2012-08-17 03:19:22 -03:00
|
|
|
good_count++;
|
2015-07-23 10:29:39 -03:00
|
|
|
|
2015-07-23 10:21:31 -03:00
|
|
|
_scaling[0] += cal[0];
|
|
|
|
_scaling[1] += cal[1];
|
|
|
|
_scaling[2] += cal[2];
|
2012-08-17 03:19:22 -03:00
|
|
|
}
|
2011-08-13 05:17:25 -03:00
|
|
|
|
2015-07-23 07:52:02 -03:00
|
|
|
#undef IS_CALIBRATION_VALUE_VALID
|
|
|
|
|
2011-08-13 05:17:25 -03:00
|
|
|
#if 0
|
2012-08-17 03:19:22 -03:00
|
|
|
/* useful for debugging */
|
2016-11-08 23:57:07 -04:00
|
|
|
hal.console->printf("MagX: %d MagY: %d MagZ: %d\n", (int)_mag_x, (int)_mag_y, (int)_mag_z);
|
|
|
|
hal.console->printf("CalX: %.2f CalY: %.2f CalZ: %.2f\n", cal[0], cal[1], cal[2]);
|
2011-08-13 05:17:25 -03:00
|
|
|
#endif
|
2012-08-17 03:19:22 -03:00
|
|
|
}
|
|
|
|
|
2016-11-08 23:57:07 -04:00
|
|
|
_bus->register_write(HMC5843_REG_CONFIG_A, base_config);
|
|
|
|
|
2012-08-17 03:19:22 -03:00
|
|
|
if (good_count >= 5) {
|
2015-09-29 11:48:46 -03:00
|
|
|
_scaling[0] = _scaling[0] / good_count;
|
|
|
|
_scaling[1] = _scaling[1] / good_count;
|
|
|
|
_scaling[2] = _scaling[2] / good_count;
|
2012-08-17 03:19:22 -03:00
|
|
|
success = true;
|
|
|
|
} else {
|
|
|
|
/* best guess */
|
2015-07-23 10:21:31 -03:00
|
|
|
_scaling[0] = 1.0;
|
|
|
|
_scaling[1] = 1.0;
|
|
|
|
_scaling[2] = 1.0;
|
2016-11-08 23:57:07 -04:00
|
|
|
if (num_samples > 5) {
|
|
|
|
// a sensor can be broken for calibration but still
|
|
|
|
// otherwise workable, accept it if we are reading samples
|
|
|
|
success = true;
|
|
|
|
}
|
2012-08-17 03:19:22 -03:00
|
|
|
}
|
|
|
|
|
2016-11-07 04:04:40 -04:00
|
|
|
#if 0
|
|
|
|
printf("scaling: %.2f %.2f %.2f\n",
|
|
|
|
_scaling[0], _scaling[1], _scaling[2]);
|
|
|
|
#endif
|
|
|
|
|
2012-08-17 03:19:22 -03:00
|
|
|
return success;
|
2011-02-14 00:27:07 -04:00
|
|
|
}
|
|
|
|
|
2016-11-04 03:57:52 -03:00
|
|
|
/* AP_HAL::Device implementation of the HMC5843 */
|
|
|
|
AP_HMC5843_BusDriver_HALDevice::AP_HMC5843_BusDriver_HALDevice(AP_HAL::OwnPtr<AP_HAL::Device> dev)
|
2016-03-04 13:59:06 -04:00
|
|
|
: _dev(std::move(dev))
|
2011-02-14 00:27:07 -04:00
|
|
|
{
|
2016-11-06 05:57:51 -04:00
|
|
|
// set read and auto-increment flags on SPI
|
|
|
|
if (_dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI) {
|
|
|
|
_dev->set_read_flag(0xC0);
|
|
|
|
}
|
2011-02-14 00:27:07 -04:00
|
|
|
}
|
2015-08-07 19:54:58 -03:00
|
|
|
|
2016-03-04 13:59:06 -04:00
|
|
|
bool AP_HMC5843_BusDriver_HALDevice::block_read(uint8_t reg, uint8_t *buf, uint32_t size)
|
2015-08-07 19:54:58 -03:00
|
|
|
{
|
2016-03-04 13:59:06 -04:00
|
|
|
return _dev->read_registers(reg, buf, size);
|
2015-08-07 19:54:58 -03:00
|
|
|
}
|
|
|
|
|
2016-03-04 13:59:06 -04:00
|
|
|
bool AP_HMC5843_BusDriver_HALDevice::register_read(uint8_t reg, uint8_t *val)
|
2015-08-07 19:54:58 -03:00
|
|
|
{
|
2016-03-04 13:59:06 -04:00
|
|
|
return _dev->read_registers(reg, val, 1);
|
2015-08-07 19:54:58 -03:00
|
|
|
}
|
|
|
|
|
2016-03-04 13:59:06 -04:00
|
|
|
bool AP_HMC5843_BusDriver_HALDevice::register_write(uint8_t reg, uint8_t val)
|
2015-08-07 19:54:58 -03:00
|
|
|
{
|
2016-03-04 13:59:06 -04:00
|
|
|
return _dev->write_register(reg, val);
|
2015-08-07 19:54:58 -03:00
|
|
|
}
|
|
|
|
|
2016-03-04 13:59:06 -04:00
|
|
|
AP_HAL::Semaphore *AP_HMC5843_BusDriver_HALDevice::get_semaphore()
|
2015-08-07 19:54:58 -03:00
|
|
|
{
|
2016-03-04 13:59:06 -04:00
|
|
|
return _dev->get_semaphore();
|
2015-08-07 19:54:58 -03:00
|
|
|
}
|
|
|
|
|
2016-10-31 22:17:13 -03:00
|
|
|
AP_HAL::Device::PeriodicHandle AP_HMC5843_BusDriver_HALDevice::register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb cb)
|
|
|
|
{
|
|
|
|
return _dev->register_periodic_callback(period_usec, cb);
|
|
|
|
}
|
|
|
|
|
|
|
|
|
2016-03-04 13:59:06 -04:00
|
|
|
/* HMC5843 on an auxiliary bus of IMU driver */
|
|
|
|
AP_HMC5843_BusDriver_Auxiliary::AP_HMC5843_BusDriver_Auxiliary(AP_InertialSensor &ins, uint8_t backend_id,
|
|
|
|
uint8_t addr)
|
2015-08-07 19:54:58 -03:00
|
|
|
{
|
2016-03-04 13:59:06 -04:00
|
|
|
/*
|
|
|
|
* Only initialize members. Fails are handled by configure or while
|
|
|
|
* getting the semaphore
|
|
|
|
*/
|
|
|
|
_bus = ins.get_auxiliary_bus(backend_id);
|
|
|
|
if (!_bus) {
|
2015-08-10 20:30:32 -03:00
|
|
|
return;
|
2016-03-04 13:59:06 -04:00
|
|
|
}
|
|
|
|
|
2015-08-10 20:30:32 -03:00
|
|
|
_slave = _bus->request_next_slave(addr);
|
|
|
|
}
|
|
|
|
|
2016-03-04 13:59:06 -04:00
|
|
|
AP_HMC5843_BusDriver_Auxiliary::~AP_HMC5843_BusDriver_Auxiliary()
|
2015-08-10 20:30:32 -03:00
|
|
|
{
|
|
|
|
/* After started it's owned by AuxiliaryBus */
|
2016-03-04 13:59:06 -04:00
|
|
|
if (!_started) {
|
2015-08-10 20:30:32 -03:00
|
|
|
delete _slave;
|
2016-03-04 13:59:06 -04:00
|
|
|
}
|
2015-08-10 20:30:32 -03:00
|
|
|
}
|
|
|
|
|
2016-03-04 13:59:06 -04:00
|
|
|
bool AP_HMC5843_BusDriver_Auxiliary::block_read(uint8_t reg, uint8_t *buf, uint32_t size)
|
2015-08-10 20:30:32 -03:00
|
|
|
{
|
2016-03-04 13:59:06 -04:00
|
|
|
if (_started) {
|
|
|
|
/*
|
|
|
|
* We can only read a block when reading the block of sample values -
|
|
|
|
* calling with any other value is a mistake
|
|
|
|
*/
|
2016-03-07 20:10:08 -04:00
|
|
|
assert(reg == HMC5843_REG_DATA_OUTPUT_X_MSB);
|
2015-08-10 20:30:32 -03:00
|
|
|
|
2016-03-04 13:59:06 -04:00
|
|
|
int n = _slave->read(buf);
|
|
|
|
return n == static_cast<int>(size);
|
|
|
|
}
|
|
|
|
|
|
|
|
int r = _slave->passthrough_read(reg, buf, size);
|
|
|
|
|
|
|
|
return r > 0 && static_cast<uint32_t>(r) == size;
|
2015-08-10 20:30:32 -03:00
|
|
|
}
|
|
|
|
|
2016-03-04 13:59:06 -04:00
|
|
|
bool AP_HMC5843_BusDriver_Auxiliary::register_read(uint8_t reg, uint8_t *val)
|
2015-08-10 20:30:32 -03:00
|
|
|
{
|
2016-03-04 13:59:06 -04:00
|
|
|
return _slave->passthrough_read(reg, val, 1) == 1;
|
2015-08-10 20:30:32 -03:00
|
|
|
}
|
|
|
|
|
2016-03-04 13:59:06 -04:00
|
|
|
bool AP_HMC5843_BusDriver_Auxiliary::register_write(uint8_t reg, uint8_t val)
|
2015-08-10 20:30:32 -03:00
|
|
|
{
|
2016-03-04 13:59:06 -04:00
|
|
|
return _slave->passthrough_write(reg, val) == 1;
|
2015-08-10 20:30:32 -03:00
|
|
|
}
|
|
|
|
|
2016-03-04 13:59:06 -04:00
|
|
|
AP_HAL::Semaphore *AP_HMC5843_BusDriver_Auxiliary::get_semaphore()
|
2015-08-10 20:30:32 -03:00
|
|
|
{
|
2016-03-04 13:59:06 -04:00
|
|
|
return _bus->get_semaphore();
|
2015-08-10 20:30:32 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
|
2016-03-04 13:59:06 -04:00
|
|
|
bool AP_HMC5843_BusDriver_Auxiliary::configure()
|
|
|
|
{
|
|
|
|
if (!_bus || !_slave) {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
return true;
|
2015-08-10 20:30:32 -03:00
|
|
|
}
|
|
|
|
|
2016-03-04 13:59:06 -04:00
|
|
|
bool AP_HMC5843_BusDriver_Auxiliary::start_measurements()
|
2015-08-10 20:30:32 -03:00
|
|
|
{
|
2016-03-07 20:10:08 -04:00
|
|
|
if (_bus->register_periodic_read(_slave, HMC5843_REG_DATA_OUTPUT_X_MSB, 6) < 0) {
|
2015-08-10 20:30:32 -03:00
|
|
|
return false;
|
2016-03-04 13:59:06 -04:00
|
|
|
}
|
2015-08-10 20:30:32 -03:00
|
|
|
|
|
|
|
_started = true;
|
|
|
|
|
|
|
|
return true;
|
|
|
|
}
|
2016-03-04 13:59:06 -04:00
|
|
|
|
2016-10-31 22:17:13 -03:00
|
|
|
AP_HAL::Device::PeriodicHandle AP_HMC5843_BusDriver_Auxiliary::register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb cb)
|
|
|
|
{
|
|
|
|
return _bus->register_periodic_callback(period_usec, cb);
|
|
|
|
}
|
|
|
|
|
2016-11-04 06:24:53 -03:00
|
|
|
// set device type within a device class
|
|
|
|
void AP_HMC5843_BusDriver_Auxiliary::set_device_type(uint8_t devtype)
|
|
|
|
{
|
|
|
|
_bus->set_device_type(devtype);
|
|
|
|
}
|
|
|
|
|
|
|
|
// return 24 bit bus identifier
|
|
|
|
uint32_t AP_HMC5843_BusDriver_Auxiliary::get_bus_id(void) const
|
|
|
|
{
|
|
|
|
return _bus->get_bus_id();
|
|
|
|
}
|
|
|
|
|
2016-03-04 13:59:06 -04:00
|
|
|
#endif
|