From e232543fca838dbae7a1ae7877ef0457e8fe7462 Mon Sep 17 00:00:00 2001 From: Lucas De Marchi Date: Wed, 22 Jul 2015 16:11:20 -0300 Subject: [PATCH] AP_Compass: AK8963: change initialization and rename methods We need to check the AK8963's id before anything else Here we are reordering the calls to _calibrate() and _check_id(). After that we don't need to read and write again the AK8963_CNTL1 register. While at it do some renames as well: - _configure() -> _setup_mode(): since now there's a _bus->configure() it became confusing what actually it's doing. - make error messages say what we were actually trying to do but couldn't. Also remove PSTR since this is linux-only. - start_conversion() -> start_measurements(): We are instructing the bus to start to get the samples, not to tell the chip to start an analog->digital conversion like in other sensors. --- libraries/AP_Compass/AP_Compass_AK8963.cpp | 38 ++++++++++------------ libraries/AP_Compass/AP_Compass_AK8963.h | 10 +++--- 2 files changed, 22 insertions(+), 26 deletions(-) diff --git a/libraries/AP_Compass/AP_Compass_AK8963.cpp b/libraries/AP_Compass/AP_Compass_AK8963.cpp index 49411e0a85..07d1c9e73e 100644 --- a/libraries/AP_Compass/AP_Compass_AK8963.cpp +++ b/libraries/AP_Compass/AP_Compass_AK8963.cpp @@ -159,27 +159,27 @@ bool AP_Compass_AK8963::init() } if (!_bus->configure()) { - hal.console->printf_P(PSTR("AK8963: Bus not configured for AK8963\n")); - goto fail; - } - - if (!_configure()) { - hal.console->printf_P(PSTR("AK8963: not configured\n")); + hal.console->printf("AK8963: Could not configure bus for AK8963\n"); goto fail; } if (!_check_id()) { - hal.console->printf_P(PSTR("AK8963: wrong id\n")); + hal.console->printf("AK8963: Wrong id\n"); goto fail; } if (!_calibrate()) { - hal.console->printf_P(PSTR("AK8963: not calibrated\n")); + hal.console->printf("AK8963: Could not read calibration data\n"); goto fail; } - if (!_bus->start_conversion()) { - hal.console->printf_P(PSTR("AK8963: conversion not started\n")); + if (!_setup_mode()) { + hal.console->printf("AK8963: Could not setup mode\n"); + goto fail; + } + + if (!_bus->start_measurements()) { + hal.console->printf("AK8963: Could not start measurments\n"); goto fail; } @@ -251,7 +251,7 @@ void AP_Compass_AK8963::_update() } break; case STATE_ERROR: - if (_bus->start_conversion()) { + if (_bus->start_measurements()) { _state = STATE_SAMPLE; } break; @@ -277,7 +277,7 @@ bool AP_Compass_AK8963::_check_id() return false; } -bool AP_Compass_AK8963::_configure() { +bool AP_Compass_AK8963::_setup_mode() { _bus->register_write(AK8963_CNTL1, AK8963_CONTINUOUS_MODE2 | AK8963_16BIT_ADC); return true; } @@ -291,8 +291,6 @@ bool AP_Compass_AK8963::_reset() bool AP_Compass_AK8963::_calibrate() { - uint8_t cntl1 = _bus->register_read(AK8963_CNTL1); - /* Enable FUSE-mode in order to be able to read calibration data */ _bus->register_write(AK8963_CNTL1, AK8963_FUSE_MODE | AK8963_16BIT_ADC); @@ -305,8 +303,6 @@ bool AP_Compass_AK8963::_calibrate() error("%d: %lf\n", i, _magnetometer_ASA[i]); } - _bus->register_write(AK8963_CNTL1, cntl1); - return true; } @@ -367,9 +363,9 @@ bool AP_Compass_AK8963::_sem_take_nonblocking() if (!hal.scheduler->system_initializing() ) { _sem_failure_count++; if (_sem_failure_count > 100) { - hal.scheduler->panic(PSTR("PANIC: failed to take _bus->sem " - "100 times in a row, in " - "AP_Compass_AK8963")); + hal.scheduler->panic("PANIC: failed to take _bus->sem " + "100 times in a row, in " + "AP_Compass_AK8963"); } } @@ -401,7 +397,7 @@ AP_AK8963_SerialBus_MPU9250::AP_AK8963_SerialBus_MPU9250() _spi = hal.spi->device(AP_HAL::SPIDevice_MPU9250); if (_spi == NULL) { - hal.console->println_P(PSTR("Cannot get SPIDevice_MPU9250")); + hal.console->printf("Cannot get SPIDevice_MPU9250\n"); return; } } @@ -471,7 +467,7 @@ AP_HAL::Semaphore * AP_AK8963_SerialBus_MPU9250::get_semaphore() return _spi->get_semaphore(); } -bool AP_AK8963_SerialBus_MPU9250::start_conversion() +bool AP_AK8963_SerialBus_MPU9250::start_measurements() { const uint8_t count = sizeof(struct raw_value); diff --git a/libraries/AP_Compass/AP_Compass_AK8963.h b/libraries/AP_Compass/AP_Compass_AK8963.h index 39b8bafd61..dd2449894a 100644 --- a/libraries/AP_Compass/AP_Compass_AK8963.h +++ b/libraries/AP_Compass/AP_Compass_AK8963.h @@ -27,8 +27,8 @@ public: } virtual void register_write(uint8_t address, uint8_t value) = 0; virtual AP_HAL::Semaphore* get_semaphore() = 0; - virtual bool start_conversion() = 0; virtual bool configure() = 0; + virtual bool start_measurements() = 0; virtual void read_raw(struct raw_value *rv) = 0; virtual uint32_t get_dev_id() = 0; }; @@ -55,7 +55,7 @@ private: } state_t; bool _reset(); - bool _configure(); + bool _setup_mode(); bool _check_id(); bool _calibrate(); @@ -91,8 +91,8 @@ public: void register_read(uint8_t address, uint8_t *value, uint8_t count); void register_write(uint8_t address, uint8_t value); AP_HAL::Semaphore* get_semaphore(); - bool start_conversion(); bool configure(); + bool start_measurements(); void read_raw(struct raw_value *rv); uint32_t get_dev_id(); private: @@ -112,8 +112,8 @@ public: void register_read(uint8_t address, uint8_t *value, uint8_t count); void register_write(uint8_t address, uint8_t value); AP_HAL::Semaphore* get_semaphore(); - bool start_conversion(){return true;} - bool configure(){return true;} + bool configure(){ return true; } + bool start_measurements() { return true; } void read_raw(struct raw_value *rv); uint32_t get_dev_id(); private: