ardupilot/libraries/AP_Compass/AP_Compass_MAG3110.cpp

268 lines
8.2 KiB
C++
Raw Normal View History

2018-02-02 05:25:24 -04: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/>.
*/
#include <utility>
#include <AP_HAL/AP_HAL.h>
#include <AP_Math/AP_Math.h>
#include <stdio.h>
#include "AP_Compass_MAG3110.h"
extern const AP_HAL::HAL &hal;
/*
EN:
at first glance, the magnetometer MAG3110 consists only of flaws:
    * noisy, with a bad characteristic, with very large difference on axes
    * it can't be calibrated in any way, you just have to believe what he has been measured
    * There are no adjustments and settings, it just sends data with some unknown sensitivity. Or does not sends :)
    * One and a half setup registers, in which only the frequency of operation and the number of averagings are specified
    
    This is a device, wooden to the waist. But of all these shortcomings, its sole and basic virtue arises:
    * He will never comes buggy or clevers.
    
    And since we do not need much from a magnetometer and it is required to calibrate the Ardupilot itself, the device
    appears in a completely new light - as a reliable info "north is there." What we really need.
RUS:
на первый взгляд, магнитометр MAG3110 состоит из одних лишь недостатков:
* шумный, с кривой характеристикой,
* никак не калибруется, приходится просто верить тому что он намерял
* нет никаких регулировок и настроек, он просто выдает данные с некой неизвестной чувствительностью. Или не выдает :)
* полтора настроечных регистра, в которых задается только частота работы и количество усреднений
Такой вот девайс, по пояс деревянный. Но из всех этих недостатков проистекает его единственное и основное достоинство:
* он никогда не глючит и не умничает.
А так как нам от магнитометра особо много и не требуется, а калибровать Ардупилот и сам умеет, то девайс
предстает в совсем новом свете - как надежный указатель "север там". Что нам собственно и надо.
*/
/*
the vector length filter can help with noise on the bus, but may
interfere with higher level processing. It should really be moved
into the AP_Compass_backend code, with a parameter to enable it.
*/
#ifndef MAG3110_ENABLE_LEN_FILTER
#define MAG3110_ENABLE_LEN_FILTER 0
#endif
2018-02-02 05:25:24 -04:00
// Registers
#define MAG3110_MAG_REG_STATUS 0x00
#define MAG3110_MAG_REG_HXL 0x01
#define MAG3110_MAG_REG_HXH 0x02
#define MAG3110_MAG_REG_HYL 0x03
#define MAG3110_MAG_REG_HYH 0x04
#define MAG3110_MAG_REG_HZL 0x05
#define MAG3110_MAG_REG_HZH 0x06
#define MAG3110_MAG_REG_WHO_AM_I 0x07
#define MAG3110_MAG_REG_SYSMODE 0x08
#define MAG3110_MAG_REG_CTRL_REG1 0x10
#define MAG3110_MAG_REG_CTRL_REG2 0x11
#define BIT_STATUS_REG_DATA_READY (1 << 3)
AP_Compass_MAG3110::AP_Compass_MAG3110(Compass &compass, AP_HAL::OwnPtr<AP_HAL::Device> dev)
: AP_Compass_Backend(compass)
, _dev(std::move(dev))
{
}
AP_Compass_Backend *AP_Compass_MAG3110::probe(Compass &compass,
AP_HAL::OwnPtr<AP_HAL::Device> dev,
enum Rotation rotation)
{
if (!dev) {
return nullptr;
}
AP_Compass_MAG3110 *sensor = new AP_Compass_MAG3110(compass, std::move(dev));
if (!sensor || !sensor->init(rotation)) {
delete sensor;
return nullptr;
}
return sensor;
}
bool AP_Compass_MAG3110::init(enum Rotation rotation)
{
bool success = _hardware_init();
if (!success) {
return false;
}
_initialised = true;
2018-05-04 03:29:33 -03:00
// perform an initial read
read();
2018-02-02 05:25:24 -04:00
/* register the compass instance in the frontend */
_compass_instance = register_compass();
set_rotation(_compass_instance, rotation);
_dev->set_device_type(DEVTYPE_MAG3110);
set_dev_id(_compass_instance, _dev->get_bus_id());
2018-05-04 03:29:33 -03:00
set_external(_compass_instance, true);
2018-02-02 05:25:24 -04:00
// read at 75Hz
_dev->register_periodic_callback(13333, FUNCTOR_BIND_MEMBER(&AP_Compass_MAG3110::_update, void));
return true;
}
bool AP_Compass_MAG3110::_hardware_init()
{
AP_HAL::Semaphore *bus_sem = _dev->get_semaphore();
if (!bus_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
AP_HAL::panic("MAG3110: Unable to get semaphore");
}
// initially run the bus at low speed
_dev->set_speed(AP_HAL::Device::SPEED_LOW);
bool ret=false;
_dev->set_retries(5);
uint8_t sig = 0;
bool ack = _dev->read_registers(MAG3110_MAG_REG_WHO_AM_I, &sig, 1);
if (!ack || sig != 0xC4) goto exit;
ack = _dev->write_register(MAG3110_MAG_REG_CTRL_REG1, 0x01); // active mode 80 Hz ODR with OSR = 1
if (!ack) goto exit;
hal.scheduler->delay(20);
ack = _dev->write_register(MAG3110_MAG_REG_CTRL_REG2, 0xA0); // AUTO_MRST_EN + RAW
if (!ack) goto exit;
ret = true;
_dev->set_retries(3);
printf("MAG3110 found on bus 0x%x\n", (uint16_t)_dev->get_bus_id());
exit:
_dev->set_speed(AP_HAL::Device::SPEED_HIGH);
bus_sem->give();
return ret;
}
// Read Sensor data
bool AP_Compass_MAG3110::_read_sample()
{
{
uint8_t status;
bool ack = _dev->read_registers(MAG3110_MAG_REG_STATUS, &status, 1);
if (!ack || (status & BIT_STATUS_REG_DATA_READY) == 0) {
return false;
}
}
uint8_t buf[6];
bool ack = _dev->read_registers(MAG3110_MAG_REG_HXL, buf, 6);
if (!ack) {
return false;
}
_mag_x = (int16_t)(buf[0] << 8 | buf[1]);
_mag_y = (int16_t)(buf[2] << 8 | buf[3]);
_mag_z = (int16_t)(buf[4] << 8 | buf[5]);
return true;
}
2018-05-04 03:29:33 -03:00
#define MAG_SCALE (1.0f/10000 / 0.0001f * 1000) // 1 Tesla full scale of +-10000, 1 Gauss = 0,0001 Tesla, library needs milliGauss
2018-02-02 05:25:24 -04:00
void AP_Compass_MAG3110::_update()
{
if (!_read_sample()) {
return;
}
2018-05-04 03:29:33 -03:00
Vector3f raw_field = Vector3f((float)_mag_x, (float)_mag_y, (float)_mag_z) * MAG_SCALE;
2018-02-02 05:25:24 -04:00
// 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, _compass_instance);
// correct raw_field for known errors
correct_field(raw_field, _compass_instance);
2018-02-02 05:25:24 -04:00
if (!field_ok(raw_field)) {
return;
2018-02-02 05:25:24 -04:00
}
if (_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
_mag_x_accum += raw_field.x;
_mag_y_accum += raw_field.y;
_mag_z_accum += raw_field.z;
_accum_count++;
if (_accum_count == 10) {
_mag_x_accum /= 2;
_mag_y_accum /= 2;
_mag_z_accum /= 2;
_accum_count /= 2;
2018-02-02 05:25:24 -04:00
}
_sem->give();
2018-02-02 05:25:24 -04:00
}
}
// Read Sensor data
void AP_Compass_MAG3110::read()
{
if (!_initialised) {
return;
}
if (!_sem->take_nonblocking()) {
return;
}
if (_accum_count == 0) {
/* We're not ready to publish*/
_sem->give();
return;
}
2018-05-04 03:29:33 -03:00
Vector3f field = Vector3f(_mag_x_accum, _mag_y_accum, _mag_z_accum);
2018-02-02 05:25:24 -04:00
field /= _accum_count;
_accum_count = 0;
_mag_x_accum = _mag_y_accum = _mag_z_accum = 0;
_sem->give();
publish_filtered_field(field, _compass_instance);
}