emaq
Member level 4
I am trying to interface HMC6343 magnetometer with a Raspberry Pi I2C using PX4 autopilot code. None of the read and write sequence works and I always get 0x00. The HMC6343_registers.hpp and HMC6343.cpp files are as under.
The I2C.cpp is located here https://github.com/PX4/PX4-Autopilot/bl ... ix/I2C.cpp.
I am using baseline driver template from here https://github.com/PX4/PX4-Autopilot/tr ... ek/ist8310.
The HMC6343 datasheet is available here https://aerospace.honeywell.com/content ... nload=true.
What could be the possible reason?
The I2C.cpp is located here https://github.com/PX4/PX4-Autopilot/bl ... ix/I2C.cpp.
I am using baseline driver template from here https://github.com/PX4/PX4-Autopilot/tr ... ek/ist8310.
The HMC6343 datasheet is available here https://aerospace.honeywell.com/content ... nload=true.
What could be the possible reason?
C++:
/**
* @file Honeywell_HMC6343_registers.hpp
*
* Honeywell HMC6343 registers and commands.
*
*/
#pragma once
#include <cstdint>
.
.
.
namespace Honeywell_HMC6343
{
static constexpr uint32_t I2C_SPEED = 400 * 1000; // 400 kHz I2C serial interface
static constexpr uint8_t I2C_ADDRESS_DEFAULT = 0x32; // HMC6343 default I2C address
enum class Register : uint8_t {
// HMC6343 Registers
SLAVE_ADDR = 0x00, // I2C Slave Address
SW_VERSION = 0x02, // Software Version Number
OP_MODE1 = 0x04, // Operational Mode Register 1
OP_MODE2 = 0x05, // Operational Mode Register 2
SN_LSB = 0x06, // Device Serial Number LSB
SN_MSB = 0x07, // Device Serial Number MSB
DATE_CODE_YY = 0x08, // Package Date Code: Last Two Digits of the Year
DATE_CODE_WW = 0x09, // Package Date Code: Fiscal Week
DEVIATION_LSB = 0x0A, // Deviation Angle (±1800) in tenths of a degree LSB
DEVIATION_MSB = 0x0B, // Deviation Angle (±1800) in tenths of a degree MSB
VARIATION_LSB = 0x0C, // Variation Angle (±1800) in tenths of a degree LSB
VARIATION_MSB = 0x0D, // Variation Angle (±1800) in tenths of a degree MSB
XOFFSET_LSB = 0x0E, // Hard-Iron Calibration Offset for the X-axis LSB
XOFFSET_MSB = 0x0F, // Hard-Iron Calibration Offset for the X-axis MSB
YOFFSET_LSB = 0x10, // Hard-Iron Calibration Offset for the Y-axis LSB
YOFFSET_MSB = 0x11, // Hard-Iron Calibration Offset for the Y-axis MSB
ZOFFSET_LSB = 0x12, // Hard-Iron Calibration Offset for the Z-axis LSB
ZOFFSET_MSB = 0x13, // Hard-Iron Calibration Offset for the Z-axis MSB
FILTER_LSB = 0x14, // Heading IIR Filter (0x00 to 0x0F typical) LSB
FILTER_MSB = 0x15, // Heading IIR Filter (set at zero) MSB
};
enum class Command : uint8_t {
// HMC6343 Commands
POST_ACCEL = 0x40, // Post Accel Data (MSB/LSB (6 Bytes))
POST_MAG = 0x45, // Post Mag Data (MSB/LSB (6 Bytes))
POST_HEADING = 0x50, // Post Heading Data ((MSB/LSB (6 Bytes)))
POST_TILT = 0x55, // Post Tilt Data (MSB/LSB (6 Bytes))
POST_OPMODE1 = 0x65, // Read Current Value of OP Mode 1
ENTER_CAL = 0x71, // Enter User Calibration Mode
ORIENT_LEVEL = 0x72, // Level Orientation
ORIENT_SIDEWAYS = 0x73, // Upright Sideways Orientation
ORIENT_FLATFRONT = 0x74, // Upright Flat Front Orientation
ENTER_RUN = 0x75, // Enter Run Mode
ENTER_STANDBY = 0x76, // Enter Standby Mode
EXIT_CAL = 0x7E, // Exit User Calibration Mode
RESET = 0x82, // Reset the Processor
ENTER_SLEEP = 0x83, // Enter Sleep Mode
EXIT_SLEEP = 0x84, // Exit Sleep Mode
READ_EEPROM = 0xE1, // Read from EEPROM
WRITE_EEPROM = 0xF1, // Write to EEPROM
};
} // namespace Honeywell_HMC6343
C++:
/**
* @file HMC6343.cpp
*/
#include "HMC6343.hpp"
using namespace time_literals;
static constexpr int16_t combine(uint8_t msb, uint8_t lsb)
{
return (msb << 8u) | lsb;
}
HMC6343::HMC6343(I2CSPIBusOption bus_option, int bus, uint8_t addr, int bus_frequency, enum Rotation rotation) :
I2C(DRV_MAG_DEVTYPE_HMC6343, MODULE_NAME, bus, addr, bus_frequency),
I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus, addr),
_px4_mag(get_device_id(), rotation)
{
_px4_mag.set_external(external());
}
.
.
.
bool HMC6343::Reset()
{
_state = STATE::RESET;
ScheduleClear();
ScheduleNow();
return true;
}
void HMC6343::print_status()
{
I2CSPIDriverBase::print_status();
perf_print_counter(_reset_perf);
perf_print_counter(_bad_register_perf);
perf_print_counter(_bad_transfer_perf);
}
void HMC6343::RunImpl()
{
const hrt_abstime now = hrt_absolute_time();
.
.
.
case STATE::READ: {
uint8_t i2C_address = ReadEEPROM(Register::SLAVE_ADDR);
PX4_INFO("I2C Slave Address: 0x%X", i2C_address);
struct TransferBuffer {
uint8_t MxMSB;
uint8_t MxLSB;
uint8_t MyMSB;
uint8_t MyLSB;
uint8_t MzMSB;
uint8_t MzLSB;
} buffer{};
bool success = false;
uint8_t cmd = static_cast<uint8_t>(Command::POST_MAG);
if (transfer(&cmd, 1, (uint8_t *)&buffer, sizeof(buffer)) == PX4_OK) {
int16_t mag_x = combine(buffer.MxMSB, buffer.MxLSB);
int16_t mag_y = combine(buffer.MyMSB, buffer.MyLSB);
int16_t mag_z = combine(buffer.MzMSB, buffer.MzLSB);
// sensor's frame is +x forward, +y right, +z up
// z = (z == INT16_MIN) ? INT16_MAX : -z; // flip z
_px4_mag.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf));
_px4_mag.update(now, mag_x, mag_y, mag_z);
success = true;
if (_failure_count > 0) {
_failure_count--;
}
} else {
perf_count(_bad_transfer_perf);
}
if (!success) {
_failure_count++;
// full reset if things are failing consistently
if (_failure_count > 10) {
Reset();
return;
}
}
if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) {
// check configuration registers periodically or immediately following any failure
if (RegisterCheck(_register_cfg[_checked_register])) {
_last_config_check_timestamp = now;
_checked_register = (_checked_register + 1) % size_register_cfg;
} else {
// register check failed, force reset
perf_count(_bad_register_perf);
Reset();
return;
}
}
// initiate next measurement
ScheduleDelayed(200_ms); // Wait at least 200 ms to get fresh data from the default 5 Hz update rate
}
break;
}
}
bool HMC6343::Configure()
{
// first set and clear all configured register bits
for (const auto ®_cfg : _register_cfg) {
RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
}
// now check that all are configured
bool success = true;
for (const auto ®_cfg : _register_cfg) {
if (!RegisterCheck(reg_cfg)) {
success = false;
}
}
_px4_mag.set_scale(1.f / 1320.f); // 1320 LSB/Gauss
return success;
}
bool HMC6343::RegisterCheck(const register_config_t ®_cfg)
{
bool success = true;
const uint8_t reg_value = ReadEEPROM(reg_cfg.reg);
if (reg_cfg.set_bits && ((reg_value & reg_cfg.set_bits) != reg_cfg.set_bits)) {
PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits);
success = false;
}
if (reg_cfg.clear_bits && ((reg_value & reg_cfg.clear_bits) != 0)) {
PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits);
success = false;
}
return success;
}
uint8_t HMC6343::ReadOPMode(Command cmnd)
{
const uint8_t cmd = static_cast<uint8_t>(cmnd);
uint8_t buffer{};
transfer(&cmd, 1, &buffer, 1);
return buffer;
}
void HMC6343::SendCommand(Command cmnd)
{
const uint8_t cmd = static_cast<uint8_t>(cmnd);
transfer(&cmd, 1, nullptr, 0);
}
uint8_t HMC6343::ReadEEPROM(Register reg)
{
uint8_t cmd[2] { (uint8_t)(Command::READ_EEPROM), (uint8_t)reg };
uint8_t buffer {};
transfer(cmd, sizeof(cmd), &buffer, 1);
return buffer;
}
void HMC6343::WriteEEPROM(Register reg, uint8_t value)
{
uint8_t cmd[3] { (uint8_t)(Command::WRITE_EEPROM), (uint8_t)reg, value };
transfer(cmd, sizeof(cmd), nullptr, 0);
}
void HMC6343::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits)
{
const uint8_t orig_val = ReadEEPROM(reg);
uint8_t val = (orig_val & ~clearbits) | setbits;
if (orig_val != val) {
WriteEEPROM(reg, val);
}
}
