401 lines
11 KiB
C++
401 lines
11 KiB
C++
/*
|
|
AK09918.cpp
|
|
A library for Grove - IMU 9DOF(ICM20600 + AK09918)
|
|
|
|
Copyright (c) 2018 seeed technology inc.
|
|
Website : www.seeed.cc
|
|
Author : Jerry Yip
|
|
Create Time: 2018-06
|
|
Version : 0.1
|
|
Change Log :
|
|
|
|
The MIT License (MIT)
|
|
|
|
Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
of this software and associated documentation files (the "Software"), to deal
|
|
in the Software without restriction, including without limitation the rights
|
|
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
|
copies of the Software, and to permit persons to whom the Software is
|
|
furnished to do so, subject to the following conditions:
|
|
|
|
The above copyright notice and this permission notice shall be included in
|
|
all copies or substantial portions of the Software.
|
|
|
|
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
|
|
THE SOFTWARE.
|
|
*/
|
|
|
|
|
|
#include "AK09918.h"
|
|
|
|
namespace {
|
|
constexpr uint8_t kI2cRetryCount = 3;
|
|
constexpr uint16_t kI2cRetryDelayUs = 250;
|
|
constexpr uint32_t kModeSettleMs = 2;
|
|
constexpr uint32_t kSingleMeasurementTimeoutMs = 20;
|
|
constexpr uint32_t kContinuousMeasurementTimeoutMs = 2;
|
|
constexpr uint32_t kSelfTestTimeoutMs = 100;
|
|
|
|
bool isContinuousMode(AK09918_mode_type_t mode) {
|
|
return mode == AK09918_CONTINUOUS_10HZ ||
|
|
mode == AK09918_CONTINUOUS_20HZ ||
|
|
mode == AK09918_CONTINUOUS_50HZ ||
|
|
mode == AK09918_CONTINUOUS_100HZ;
|
|
}
|
|
|
|
AK09918_err_type_t waitDataReady(AK09918 *sensor, uint32_t timeoutMs) {
|
|
const uint32_t startedMs = millis();
|
|
do {
|
|
const AK09918_err_type_t err = sensor->isDataReady();
|
|
if (err == AK09918_ERR_OK || err == AK09918_ERR_READ_FAILED) {
|
|
return err;
|
|
}
|
|
delay(1);
|
|
} while ((millis() - startedMs) < timeoutMs);
|
|
|
|
return AK09918_ERR_NOT_RDY;
|
|
}
|
|
} // namespace
|
|
|
|
AK09918::AK09918() {
|
|
_addr = AK09918_I2C_ADDR;
|
|
_mode = AK09918_POWER_DOWN;
|
|
}
|
|
|
|
|
|
AK09918_err_type_t AK09918::initialize(AK09918_mode_type_t mode) {
|
|
if (mode == AK09918_SELF_TEST) {
|
|
mode = AK09918_POWER_DOWN;
|
|
}
|
|
if (!AK09918::writeByte(_addr, AK09918_CNTL2, AK09918_POWER_DOWN)) {
|
|
return AK09918_ERR_WRITE_FAILED;
|
|
}
|
|
|
|
_mode = AK09918_POWER_DOWN;
|
|
delay(kModeSettleMs);
|
|
|
|
if (mode == AK09918_NORMAL) {
|
|
_mode = AK09918_NORMAL;
|
|
return AK09918_ERR_OK;
|
|
}
|
|
|
|
return AK09918::switchMode(mode);
|
|
}
|
|
|
|
AK09918_err_type_t AK09918::isDataReady() {
|
|
if (!AK09918::readByte(_addr, AK09918_ST1, _buffer)) {
|
|
return AK09918_ERR_READ_FAILED;
|
|
} else {
|
|
if (_buffer[0] & AK09918_DRDY_BIT) {
|
|
return AK09918_ERR_OK;
|
|
} else {
|
|
return AK09918_ERR_NOT_RDY;
|
|
}
|
|
}
|
|
}
|
|
|
|
AK09918_err_type_t AK09918::isDataSkip() {
|
|
if (!AK09918::readByte(_addr, AK09918_ST1, _buffer)) {
|
|
return AK09918_ERR_READ_FAILED;
|
|
} else {
|
|
if (_buffer[0] & AK09918_DOR_BIT) {
|
|
return AK09918_ERR_DOR;
|
|
} else {
|
|
return AK09918_ERR_OK;
|
|
}
|
|
}
|
|
}
|
|
|
|
AK09918_err_type_t AK09918::getData(int16_t* axis_x, int16_t* axis_y, int16_t* axis_z) {
|
|
AK09918_err_type_t err = AK09918::getRawData(axis_x, axis_y, axis_z);
|
|
if (err == AK09918_ERR_OK || err == AK09918_ERR_OVERFLOW) {
|
|
(*axis_x) = (*axis_x) * 15 / 100;
|
|
(*axis_y) = (*axis_y) * 15 / 100;
|
|
(*axis_z) = (*axis_z) * 15 / 100;
|
|
}
|
|
|
|
return err;
|
|
}
|
|
|
|
AK09918_err_type_t AK09918::getRawData(int16_t* axis_x, int16_t* axis_y, int16_t* axis_z) {
|
|
AK09918_err_type_t readyErr = AK09918_ERR_OK;
|
|
|
|
if (_mode == AK09918_NORMAL) {
|
|
const AK09918_err_type_t modeErr = AK09918::switchMode(AK09918_NORMAL);
|
|
if (modeErr != AK09918_ERR_OK) {
|
|
return modeErr;
|
|
}
|
|
|
|
readyErr = waitDataReady(this, kSingleMeasurementTimeoutMs);
|
|
} else if (isContinuousMode(_mode)) {
|
|
readyErr = waitDataReady(this, kContinuousMeasurementTimeoutMs);
|
|
} else {
|
|
return AK09918_ERR_NOT_RDY;
|
|
}
|
|
|
|
if (readyErr != AK09918_ERR_OK) {
|
|
const AK09918_err_type_t uncheckedErr = AK09918::getRawDataUnchecked(axis_x, axis_y, axis_z);
|
|
if ((uncheckedErr == AK09918_ERR_OK || uncheckedErr == AK09918_ERR_OVERFLOW) &&
|
|
(*axis_x != 0 || *axis_y != 0 || *axis_z != 0)) {
|
|
return uncheckedErr;
|
|
}
|
|
return readyErr;
|
|
}
|
|
|
|
if (!AK09918::readBytes(_addr, AK09918_HXL, 8, _buffer)) {
|
|
return AK09918_ERR_READ_FAILED;
|
|
} else {
|
|
*axis_x = (_buffer[1] << 8 | _buffer[0]);
|
|
*axis_y = (_buffer[3] << 8 | _buffer[2]);
|
|
*axis_z = (_buffer[5] << 8 | _buffer[4]);
|
|
if (_buffer[7] & AK09918_HOFL_BIT) {
|
|
return AK09918_ERR_OVERFLOW;
|
|
}
|
|
return AK09918_ERR_OK;
|
|
}
|
|
}
|
|
|
|
AK09918_err_type_t AK09918::getRawDataUnchecked(int16_t* axis_x, int16_t* axis_y, int16_t* axis_z) {
|
|
if (!AK09918::readBytes(_addr, AK09918_HXL, 8, _buffer)) {
|
|
return AK09918_ERR_READ_FAILED;
|
|
}
|
|
|
|
*axis_x = (_buffer[1] << 8 | _buffer[0]);
|
|
*axis_y = (_buffer[3] << 8 | _buffer[2]);
|
|
*axis_z = (_buffer[5] << 8 | _buffer[4]);
|
|
if (_buffer[7] & AK09918_HOFL_BIT) {
|
|
return AK09918_ERR_OVERFLOW;
|
|
}
|
|
|
|
return AK09918_ERR_OK;
|
|
}
|
|
|
|
AK09918_mode_type_t AK09918::getMode() {
|
|
return _mode;
|
|
}
|
|
|
|
AK09918_err_type_t AK09918::switchMode(AK09918_mode_type_t mode) {
|
|
if (mode == AK09918_SELF_TEST) {
|
|
return AK09918_ERR_WRITE_FAILED;
|
|
}
|
|
|
|
if (mode != AK09918_POWER_DOWN) {
|
|
if (!AK09918::writeByte(_addr, AK09918_CNTL2, AK09918_POWER_DOWN)) {
|
|
return AK09918_ERR_WRITE_FAILED;
|
|
}
|
|
delay(kModeSettleMs);
|
|
}
|
|
|
|
if (!AK09918::writeByte(_addr, AK09918_CNTL2, mode)) {
|
|
return AK09918_ERR_WRITE_FAILED;
|
|
}
|
|
|
|
_mode = mode;
|
|
return AK09918_ERR_OK;
|
|
}
|
|
|
|
// 1.Set Power-down mode. (MODE[4:0] bits = “00000”)
|
|
// 2.Set Self-test mode. (MODE[4:0] bits = “10000”)
|
|
// 3.Check Data Ready or not by polling DRDY bit of ST1 register.
|
|
// 4.When Data Ready, proceed to the next step. Read measurement data. (HXL to HZH)
|
|
AK09918_err_type_t AK09918::selfTest() {
|
|
int32_t axis_x, axis_y, axis_z;
|
|
bool is_end = false;
|
|
AK09918_err_type_t err;
|
|
if (!AK09918::writeByte(_addr, AK09918_CNTL2, AK09918_POWER_DOWN)) {
|
|
return AK09918_ERR_WRITE_FAILED;
|
|
}
|
|
delay(1);
|
|
if (!AK09918::writeByte(_addr, AK09918_CNTL2, AK09918_SELF_TEST)) {
|
|
return AK09918_ERR_WRITE_FAILED;
|
|
}
|
|
|
|
const uint32_t startedMs = millis();
|
|
while (!is_end) {
|
|
err = AK09918::isDataReady();
|
|
if (err == AK09918_ERR_OK) {
|
|
is_end = true;
|
|
} else if (err == AK09918_ERR_READ_FAILED) {
|
|
return AK09918_ERR_READ_FAILED;
|
|
} else if ((millis() - startedMs) >= kSelfTestTimeoutMs) {
|
|
return AK09918_ERR_TIMEOUT;
|
|
}
|
|
delay(1);
|
|
}
|
|
|
|
// read data and check
|
|
if (!AK09918::readBytes(_addr, AK09918_HXL, 8, _buffer)) {
|
|
return AK09918_ERR_READ_FAILED;
|
|
} else {
|
|
axis_x = (int32_t)((((int16_t)_buffer[1]) << 8) | _buffer[0]);
|
|
axis_y = (int32_t)((((int16_t)_buffer[3]) << 8) | _buffer[2]);
|
|
axis_z = (int32_t)((((int16_t)_buffer[5]) << 8) | _buffer[4]);
|
|
|
|
if ((axis_x >= -200) && (axis_x <= 200) && (axis_y >= -200) && (axis_y <= 200) && \
|
|
(axis_z >= -1000) && (axis_z <= -150)) {
|
|
return AK09918_ERR_OK;
|
|
} else {
|
|
return AK09918_ERR_SELFTEST_FAILED;
|
|
}
|
|
|
|
}
|
|
}
|
|
|
|
AK09918_err_type_t AK09918::reset() {
|
|
if (!AK09918::writeByte(_addr, AK09918_CNTL3, AK09918_SRST_BIT)) {
|
|
return AK09918_ERR_WRITE_FAILED;
|
|
}
|
|
return AK09918_ERR_OK;
|
|
}
|
|
|
|
String AK09918::strError(AK09918_err_type_t err) {
|
|
String result;
|
|
switch (err) {
|
|
case AK09918_ERR_OK:
|
|
result = "AK09918_ERR_OK: OK";
|
|
break;
|
|
|
|
case AK09918_ERR_DOR:
|
|
result = "AK09918_ERR_DOR: Data skipped";
|
|
break;
|
|
|
|
case AK09918_ERR_NOT_RDY:
|
|
result = "AK09918_ERR_NOT_RDY: Not ready";
|
|
break;
|
|
|
|
case AK09918_ERR_TIMEOUT:
|
|
result = "AK09918_ERR_TIMEOUT: Timeout";
|
|
break;
|
|
|
|
case AK09918_ERR_SELFTEST_FAILED:
|
|
result = "AK09918_ERR_SELFTEST_FAILED: Self test failed";
|
|
break;
|
|
|
|
case AK09918_ERR_OVERFLOW:
|
|
result = "AK09918_ERR_OVERFLOW: Sensor overflow";
|
|
break;
|
|
|
|
case AK09918_ERR_WRITE_FAILED:
|
|
result = "AK09918_ERR_WRITE_FAILED: Fail to write";
|
|
break;
|
|
|
|
case AK09918_ERR_READ_FAILED:
|
|
result = "AK09918_ERR_READ_FAILED: Fail to read";
|
|
break;
|
|
|
|
default:
|
|
result = "Unknown Error";
|
|
break;
|
|
}
|
|
return result;
|
|
}
|
|
|
|
uint16_t AK09918::getDeviceID() {
|
|
if (!AK09918::readBytes(_addr, AK09918_WIA1, 2, _buffer)) {
|
|
return 0xFFFF;
|
|
}
|
|
return (((uint16_t)_buffer[0]) << 8) | _buffer[1];
|
|
}
|
|
|
|
void AK09918::setAddress(uint8_t addr) {
|
|
_addr = addr;
|
|
_mode = AK09918_POWER_DOWN;
|
|
}
|
|
|
|
uint8_t AK09918::getAddress() {
|
|
return _addr;
|
|
}
|
|
|
|
uint8_t AK09918::getRawMode() {
|
|
return AK09918::_getRawMode();
|
|
}
|
|
|
|
uint8_t AK09918::readRegister(uint8_t reg) {
|
|
if (!AK09918::readByte(_addr, reg, _buffer)) {
|
|
return 0xFF;
|
|
}
|
|
return _buffer[0];
|
|
}
|
|
|
|
uint8_t AK09918::_getRawMode() {
|
|
if (!AK09918::readByte(_addr, AK09918_CNTL2, _buffer)) {
|
|
return 0xFF;
|
|
} else {
|
|
return _buffer[0];
|
|
}
|
|
}
|
|
|
|
bool AK09918::readBytes(uint8_t addr,uint8_t reg,uint8_t num,uint8_t *buf)
|
|
{
|
|
for (uint8_t retry = 0; retry < kI2cRetryCount; retry++) {
|
|
Wire.beginTransmission(addr);
|
|
Wire.write(reg);
|
|
// The Waveshare reference AK09918 code uses a STOP before requestFrom().
|
|
// Keep that transaction shape here; some AKM-compatible parts are picky.
|
|
if (Wire.endTransmission() == 0 &&
|
|
Wire.requestFrom((int)addr, (int)num) == num) {
|
|
uint8_t bytesRead = 0;
|
|
for (int i = 0; i < num; i++)
|
|
{
|
|
if (!Wire.available()) {
|
|
break;
|
|
}
|
|
buf[i] = (uint8_t)Wire.read();
|
|
bytesRead++;
|
|
}
|
|
if (bytesRead == num) {
|
|
return true;
|
|
}
|
|
}
|
|
|
|
while (Wire.available()) {
|
|
Wire.read();
|
|
}
|
|
delayMicroseconds(kI2cRetryDelayUs);
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
bool AK09918::readByte(uint8_t addr,uint8_t reg ,uint8_t *buf)
|
|
{
|
|
for (uint8_t retry = 0; retry < kI2cRetryCount; retry++) {
|
|
Wire.beginTransmission(addr);
|
|
Wire.write(reg);
|
|
if (Wire.endTransmission() == 0 &&
|
|
Wire.requestFrom((int)addr, 1) == 1 &&
|
|
Wire.available()) {
|
|
buf[0] = (uint8_t)Wire.read();
|
|
return true;
|
|
}
|
|
|
|
while (Wire.available()) {
|
|
Wire.read();
|
|
}
|
|
delayMicroseconds(kI2cRetryDelayUs);
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
bool AK09918::writeByte(uint8_t addr,uint8_t reg ,uint8_t Value)
|
|
{
|
|
for (uint8_t retry = 0; retry < kI2cRetryCount; retry++) {
|
|
Wire.beginTransmission(addr);
|
|
Wire.write(reg);
|
|
Wire.write(Value);
|
|
if (Wire.endTransmission() == 0) {
|
|
return true;
|
|
}
|
|
delayMicroseconds(kI2cRetryDelayUs);
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|