aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Compass: IST8310: minor fixes and refactors

Browse Source 
- Inline code that needs to take and release the lock in init()
    function, just like other drivers
  - Minor changes to coding style and renames to be similar to other
    drivers
This commit is contained in:
Lucas De Marchi 2016-12-08 16:05:57 -08:00
parent f061572e13
commit 899fce53ff
2 changed files with 54 additions and 73 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

112
libraries/AP_Compass/AP_Compass_IST8310.cpp
View File

@ -16,14 +16,12 @@
*
* Driver by Georgii Staroselskii, Sep 2016
*/
#include "AP_Compass_IST8310.h"
#include <AP_HAL/AP_HAL.h>
#include <utility>
#include <stdio.h>
#include <utility>
#include <AP_HAL/AP_HAL.h>
#include <AP_HAL/utility/sparse-endian.h>
#include <AP_Math/AP_Math.h>
@ -47,10 +45,13 @@
extern const AP_HAL::HAL &hal;
AP_Compass_Backend *AP_Compass_IST8310::probe(Compass &compass,
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
enum Rotation rotation
)
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
enum Rotation rotation)
{
if (!dev) {
return nullptr;
}
AP_Compass_IST8310 *sensor = new AP_Compass_IST8310(compass, std::move(dev), rotation);
if (!sensor || !sensor->init()) {
delete sensor;
@ -61,78 +62,64 @@ AP_Compass_Backend *AP_Compass_IST8310::probe(Compass &compass,
}
AP_Compass_IST8310::AP_Compass_IST8310(Compass &compass,
AP_HAL::OwnPtr<AP_HAL::Device> dev,
enum Rotation _rotation)
AP_HAL::OwnPtr<AP_HAL::Device> dev,
enum Rotation rotation)
: AP_Compass_Backend(compass)
, _dev(std::move(dev))
, rotation(_rotation)
, _rotation(rotation)
{
}
bool AP_Compass_IST8310::init()
{
if (!_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
hal.console->printf("IST8310: Unable to get bus semaphore\n");
goto fail_sem;
return false;
}
if (!check_id()) {
hal.console->printf("IST8310: Not a IST device\n");
// high retries for init
_dev->set_retries(10);
uint8_t whoami;
if (!_dev->read_registers(WAI_REG, &whoami, 1) ||
whoami != DEVICE_ID) {
// not an IST8310
goto fail;
}
if (!setup_sampling()) {
if (!_dev->write_register(AVGCNTL_REG, AVERAGING_Y_BY_2 | AVERAGING_XZ_BY_4) ||
!_dev->write_register(PDCNTL_REG, NORMAL_PULSE_DURATION)) {
fprintf(stderr, "IST8310: found device but could not set it up\n");
goto fail;
}
// lower retries for run
_dev->set_retries(3);
// start state machine: request a sample
start_conversion();
_dev->get_semaphore()->give();
printf("%s found on bus %u address 0x%02x\n", name, _dev->bus_num(), _dev->get_bus_address());
_instance = register_compass();
compass_instance = register_compass();
printf("%s found on bus %u id %u address 0x%02x\n", name,
_dev->bus_num(), _dev->get_bus_id(), _dev->get_bus_address());
hal.console->printf("Found a IST8310 on 0x%x as compass %u\n", _dev->get_bus_id(), compass_instance);
set_rotation(_instance, _rotation);
set_rotation(compass_instance, rotation);
/* register the compass instance in the frontend */
_dev->set_device_type(DEVTYPE_IST8310);
set_dev_id(compass_instance, _dev->get_bus_id());
set_dev_id(_instance, _dev->get_bus_id());
_dev->register_periodic_callback(10 * USEC_PER_MSEC, FUNCTOR_BIND_MEMBER(&AP_Compass_IST8310::timer, bool));
_dev->register_periodic_callback(10 * USEC_PER_MSEC,
FUNCTOR_BIND_MEMBER(&AP_Compass_IST8310::timer, bool));
return true;
fail:
_dev->get_semaphore()->give();
fail_sem:
return false;
}
bool AP_Compass_IST8310::check_id()
{
uint8_t val = 0;
if (!_dev->read_registers(WAI_REG, &val, 1)) {
return false;
}
if (val != DEVICE_ID) {
return false;
}
return true;
}
bool AP_Compass_IST8310::setup_sampling()
{
bool ret = _dev->write_register(AVGCNTL_REG, AVERAGING_Y_BY_2 | AVERAGING_XZ_BY_4);
return ret && _dev->write_register(PDCNTL_REG, NORMAL_PULSE_DURATION);
}
void AP_Compass_IST8310::start_conversion()
{
_dev->write_register(CNTL1_REG,
@ -153,9 +140,9 @@ bool AP_Compass_IST8310::timer()
ret = _dev->read_registers(STAT1_REG, (uint8_t *) &buffer, sizeof(buffer));
if (!ret) {
return true; /* We're going to be back on the next iteration either way */
/* We're going to be back on the next iteration either way */
return true;
}
auto status = buffer.status;
@ -173,23 +160,20 @@ bool AP_Compass_IST8310::timer()
Vector3f field = Vector3f{x * 3.0f, y * 3.0f, z * 3.0f};
/* rotate raw_field from sensor frame to body frame */
rotate_field(field, compass_instance);
const auto now = AP_HAL::micros();
rotate_field(field, _instance);
/* publish raw_field (uncorrected point sample) for calibration use */
publish_raw_field(field, now, compass_instance);
publish_raw_field(field, AP_HAL::micros(), _instance);
/* correct raw_field for known errors */
correct_field(field, compass_instance);
correct_field(field, _instance);
if (_sem->take(0)) {
mag_accum += field;
accum_count++;
_accum += field;
_accum_count++;
_sem->give();
}
start_conversion();
return true;
@ -201,18 +185,18 @@ void AP_Compass_IST8310::read()
return;
}
if (accum_count == 0) {
if (_accum_count == 0) {
_sem->give();
return;
}
Vector3f field(mag_accum);
field /= accum_count;
mag_accum.zero();
accum_count = 0;
Vector3f field(_accum);
field /= _accum_count;
publish_filtered_field(field, _instance);
_accum.zero();
_accum_count = 0;
_sem->give();
publish_filtered_field(field, compass_instance);
}

15
libraries/AP_Compass/AP_Compass_IST8310.h
View File

@ -38,20 +38,17 @@ public:
private:
AP_Compass_IST8310(Compass &compass,
AP_HAL::OwnPtr<AP_HAL::Device> dev,
enum Rotation rotation);
AP_HAL::OwnPtr<AP_HAL::Device> dev,
enum Rotation rotation);
bool timer();
bool init();
bool check_id();
bool setup_sampling();
void start_conversion();
AP_HAL::OwnPtr<AP_HAL::Device> _dev;
Vector3f mag_accum = Vector3f();
uint32_t accum_count = 0;
uint8_t compass_instance;
enum Rotation rotation;
Vector3f _accum = Vector3f();
uint32_t _accum_count = 0;
enum Rotation _rotation;
uint8_t _instance;
};