zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_Compass: BMM150: minor refactors 

- Correctly sort includes and add missing AP_Math.h
  - Use anonymous struct for trim_registers in _load_trim_values,
    renaming its members so they don't start with underscore
  - Don't change _dig* values when we failed to read from sensor
  - Add some blank lines
  - Make _dig_* members be inside a _dig struct
  - Use constrain_int32 instead of if/else chain
  - s/time_us/time_usec/
  - Construct raw_field with a single constructor in _update()
  - Add missing copyright notice
  - Group methods together in declaration
mission-4.1.18
Lucas De Marchi 9 years ago
parent
6e53854122
commit
3ba27df405
3 changed files with 95 additions and 89 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      libraries/AP_Compass/AP_Compass.cpp
  2. 127
      libraries/AP_Compass/AP_Compass_BMM150.cpp
  3. 55
      libraries/AP_Compass/AP_Compass_BMM150.h

2
libraries/AP_Compass/AP_Compass.cpp
Unescape View File

@ -7,6 +7,7 @@ @@ -7,6 +7,7 @@
#include "AP_Compass_AK8963.h"
#include "AP_Compass_Backend.h"
#include "AP_Compass_BMM150.h"
#include "AP_Compass_HIL.h"
#include "AP_Compass_HMC5843.h"
#include "AP_Compass_LSM303D.h"
@ -14,7 +15,6 @@ @@ -14,7 +15,6 @@
#include "AP_Compass_PX4.h"
#include "AP_Compass_QURT.h"
#include "AP_Compass_qflight.h"
#include "AP_Compass_BMM150.h"
#include "AP_Compass.h"
extern AP_HAL::HAL& hal;

127
libraries/AP_Compass/AP_Compass_BMM150.cpp
Unescape View File

@ -15,15 +15,16 @@ @@ -15,15 +15,16 @@
* 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_Compass_BMM150.h"
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
#include <AP_HAL/utility/sparse-endian.h>
#include <utility>
#include "AP_Compass_BMM150.h"
#include <AP_HAL/utility/sparse-endian.h>
#include <AP_Math/AP_Math.h>
#define CHIP_ID_REG 0x40
#define CHIP_ID_VAL 0x32
@ -85,38 +86,40 @@ AP_Compass_BMM150::AP_Compass_BMM150(Compass &compass, @@ -85,38 +86,40 @@ AP_Compass_BMM150::AP_Compass_BMM150(Compass &compass,
bool AP_Compass_BMM150::_load_trim_values()
{
struct trim_registers {
int8_t _dig_x1;
int8_t _dig_y1;
uint8_t _rsv[3];
le16_t _dig_z4;
int8_t _dig_x2;
int8_t _dig_y2;
uint8_t _rsv2[2];
le16_t _dig_z2;
le16_t _dig_z1;
le16_t _dig_xyz1;
le16_t _dig_z3;
int8_t _dig_xy2;
uint8_t _dig_xy1;
struct {
int8_t dig_x1;
int8_t dig_y1;
uint8_t rsv[3];
le16_t dig_z4;
int8_t dig_x2;
int8_t dig_y2;
uint8_t rsv2[2];
le16_t dig_z2;
le16_t dig_z1;
le16_t dig_xyz1;
le16_t dig_z3;
int8_t dig_xy2;
uint8_t dig_xy1;
} PACKED trim_registers;
bool ret;
ret = _dev->read_registers(DIG_X1_REG, (uint8_t *)&trim_registers, sizeof(trim_registers));
_dig_x1 = trim_registers._dig_x1;
_dig_x2 = trim_registers._dig_x2;
_dig_xy1 = trim_registers._dig_xy1;
_dig_xy2 = trim_registers._dig_xy2;
_dig_xyz1 = le16toh(trim_registers._dig_xyz1);
_dig_y1 = trim_registers._dig_y1;
_dig_y2 = trim_registers._dig_y2;
_dig_z1 = le16toh(trim_registers._dig_z1);
_dig_z2 = le16toh(trim_registers._dig_z2);
_dig_z3 = le16toh(trim_registers._dig_z3);
_dig_z4 = le16toh(trim_registers._dig_z4);
return ret;
if (!_dev->read_registers(DIG_X1_REG, (uint8_t *)&trim_registers,
sizeof(trim_registers))) {
return false;
}
_dig.x1 = trim_registers.dig_x1;
_dig.x2 = trim_registers.dig_x2;
_dig.xy1 = trim_registers.dig_xy1;
_dig.xy2 = trim_registers.dig_xy2;
_dig.xyz1 = le16toh(trim_registers.dig_xyz1);
_dig.y1 = trim_registers.dig_y1;
_dig.y2 = trim_registers.dig_y2;
_dig.z1 = le16toh(trim_registers.dig_z1);
_dig.z2 = le16toh(trim_registers.dig_z2);
_dig.z3 = le16toh(trim_registers.dig_z3);
_dig.z4 = le16toh(trim_registers.dig_z4);
return true;
}
bool AP_Compass_BMM150::init()
@ -190,6 +193,7 @@ bool AP_Compass_BMM150::init() @@ -190,6 +193,7 @@ bool AP_Compass_BMM150::init()
hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_Compass_BMM150::_update, void));
return true;
bus_error:
hal.console->printf("BMM150: Bus communication error\n");
fail:
@ -205,12 +209,12 @@ fail_sem: @@ -205,12 +209,12 @@ fail_sem:
*/
int16_t AP_Compass_BMM150::_compensate_xy(int16_t xy, uint32_t rhall, int32_t txy1, int32_t txy2)
{
int32_t inter = ((int32_t)_dig_xyz1) << 14;
int32_t inter = ((int32_t)_dig.xyz1) << 14;
inter /= rhall;
inter -= 0x4000;
int32_t val = _dig_xy2 * ((inter * inter) >> 7);
val += (inter * (((uint32_t)_dig_xy1) << 7));
int32_t val = _dig.xy2 * ((inter * inter) >> 7);
val += (inter * (((uint32_t)_dig.xy1) << 7));
val >>= 9;
val += 0x100000;
val *= (txy2 + 0xA0);
@ -224,30 +228,22 @@ int16_t AP_Compass_BMM150::_compensate_xy(int16_t xy, uint32_t rhall, int32_t tx @@ -224,30 +228,22 @@ int16_t AP_Compass_BMM150::_compensate_xy(int16_t xy, uint32_t rhall, int32_t tx
int16_t AP_Compass_BMM150::_compensate_z(int16_t z, uint32_t rhall)
{
int32_t dividend = ((int32_t)(z - _dig_z4)) << 15;
dividend -= (_dig_z3 * (rhall - _dig_xyz1)) >> 2;
int32_t dividend = ((int32_t)(z - _dig.z4)) << 15;
dividend -= (_dig.z3 * (rhall - _dig.xyz1)) >> 2;
int32_t divisor = ((int32_t)_dig_z1) * (rhall << 1);
int32_t divisor = ((int32_t)_dig.z1) * (rhall << 1);
divisor += 0x8000;
divisor >>= 16;
divisor += _dig_z2;
int32_t ret = dividend / divisor;
if (ret > 0x8000 || ret < -0x8000) {
if (ret > 0x8000) {
ret = 0x8000;
} else {
ret = -0x8000;
}
}
return ret;
divisor += _dig.z2;
return constrain_int32(dividend / divisor, -0x8000, 0x8000);
}
void AP_Compass_BMM150::_update()
{
const uint32_t time_us = AP_HAL::micros();
const uint32_t time_usec = AP_HAL::micros();
if (time_us - _last_update_timestamp < MEASURE_TIME_USEC) {
if (time_usec - _last_update_timestamp < MEASURE_TIME_USEC) {
return;
}
@ -255,7 +251,7 @@ void AP_Compass_BMM150::_update() @@ -255,7 +251,7 @@ void AP_Compass_BMM150::_update()
return;
}
uint16_t data[4];
le16_t data[4];
bool ret = _dev->read_registers(DATA_X_LSB_REG, (uint8_t *) &data, sizeof(data));
_dev->get_semaphore()->give();
@ -265,19 +261,13 @@ void AP_Compass_BMM150::_update() @@ -265,19 +261,13 @@ void AP_Compass_BMM150::_update()
}
const uint16_t rhall = le16toh(data[3] >> 2);
Vector3f raw_field = Vector3f();
int16_t val = le16toh(((int16_t)data[0]) >> 3);
val = _compensate_xy(val, rhall, _dig_x1, _dig_x2);
raw_field.x = val;
val = le16toh(((int16_t)data[1]) >> 3);
val = _compensate_xy(val, rhall, _dig_y1, _dig_y2);
raw_field.y = val;
val = le16toh(((int16_t)data[2]) >> 1);
val = _compensate_z(val, rhall);
raw_field.z = val;
Vector3f raw_field = Vector3f{
(float) _compensate_xy(((int16_t)le16toh(data[0])) >> 3,
rhall, _dig.x1, _dig.x2),
(float) _compensate_xy(((int16_t)le16toh(data[1])) >> 3,
rhall, _dig.y1, _dig.y2),
(float) _compensate_z(((int16_t)le16toh(data[2])) >> 1, rhall)};
/* apply sensitivity scale 16 LSB/uT */
raw_field /= 16;
@ -288,7 +278,7 @@ void AP_Compass_BMM150::_update() @@ -288,7 +278,7 @@ void AP_Compass_BMM150::_update()
rotate_field(raw_field, _compass_instance);
/* publish raw_field (uncorrected point sample) for calibration use */
publish_raw_field(raw_field, time_us, _compass_instance);
publish_raw_field(raw_field, time_usec, _compass_instance);
/* correct raw_field for known errors */
correct_field(raw_field, _compass_instance);
@ -300,7 +290,7 @@ void AP_Compass_BMM150::_update() @@ -300,7 +290,7 @@ void AP_Compass_BMM150::_update()
_accum_count = 5;
}
_last_update_timestamp = time_us;
_last_update_timestamp = time_usec;
}
void AP_Compass_BMM150::read()
@ -310,13 +300,12 @@ void AP_Compass_BMM150::read() @@ -310,13 +300,12 @@ void AP_Compass_BMM150::read()
}
hal.scheduler->suspend_timer_procs();
Vector3f field(_mag_accum);
field /= _accum_count;
_mag_accum.zero();
_accum_count = 0;
hal.scheduler->resume_timer_procs();
publish_filtered_field(field, _compass_instance);
}

55
libraries/AP_Compass/AP_Compass_BMM150.h
Unescape View File

@ -1,9 +1,26 @@ @@ -1,9 +1,26 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
* Copyright (C) 2016 Intel Corporation. All rights reserved.
*
* This file 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 file 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/>.
*/
#pragma once
#include <AP_Common/AP_Common.h>
#include <AP_HAL/AP_HAL.h>
#include <AP_HAL/I2CDevice.h>
#include <AP_HAL/SPIDevice.h>
#include <AP_Math/AP_Math.h>
#include "AP_Compass.h"
#include "AP_Compass_Backend.h"
@ -20,9 +37,13 @@ public: @@ -20,9 +37,13 @@ public:
static constexpr const char *name = "BMM150";
private:
AP_Compass_BMM150(Compass &compass, AP_HAL::OwnPtr<AP_HAL::Device> dev);
void _update();
bool _load_trim_values();
int16_t _compensate_xy(int16_t xy, uint32_t rhall, int32_t txy1, int32_t txy2);
int16_t _compensate_z(int16_t z, uint32_t rhall);
AP_HAL::OwnPtr<AP_HAL::Device> _dev;
Vector3f _mag_accum = Vector3f();
@ -31,21 +52,17 @@ private: @@ -31,21 +52,17 @@ private:
uint8_t _compass_instance;
void _update();
int8_t _dig_x1;
int8_t _dig_y1;
int8_t _dig_x2;
int8_t _dig_y2;
uint16_t _dig_z1;
int16_t _dig_z2;
int16_t _dig_z3;
int16_t _dig_z4;
uint8_t _dig_xy1;
int8_t _dig_xy2;
uint16_t _dig_xyz1;
bool _load_trim_values();
int16_t _compensate_xy(int16_t xy, uint32_t rhall, int32_t txy1, int32_t txy2);
int16_t _compensate_z(int16_t z, uint32_t rhall);
struct {
int8_t x1;
int8_t y1;
int8_t x2;
int8_t y2;
uint16_t z1;
int16_t z2;
int16_t z3;
int16_t z4;
uint8_t xy1;
int8_t xy2;
uint16_t xyz1;
} _dig;
};

Write Preview
Loading…
Cancel
Save