From dd165100fb8c0943c7518b40ae67c36b408baa0d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 1 Jan 2015 15:47:07 +0100 Subject: Support for HMC5983, which can also be attached via SPI --- src/drivers/hmc5883/hmc5883.cpp | 237 ++++++++++++++++++------------------ src/drivers/hmc5883/hmc5883.h | 52 ++++++++ src/drivers/hmc5883/hmc5883_i2c.cpp | 175 ++++++++++++++++++++++++++ src/drivers/hmc5883/hmc5883_spi.cpp | 189 ++++++++++++++++++++++++++++ src/drivers/hmc5883/module.mk | 4 +- 5 files changed, 539 insertions(+), 118 deletions(-) create mode 100644 src/drivers/hmc5883/hmc5883.h create mode 100644 src/drivers/hmc5883/hmc5883_i2c.cpp create mode 100644 src/drivers/hmc5883/hmc5883_spi.cpp (limited to 'src/drivers') diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index d4dbf3778..5989703dd 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -34,7 +34,7 @@ /** * @file hmc5883.cpp * - * Driver for the HMC5883 magnetometer connected via I2C. + * Driver for the HMC5883 / HMC5983 magnetometer connected via I2C or SPI. */ #include @@ -74,11 +74,12 @@ #include #include +#include "hmc5883.h" + /* * HMC5883 internal constants and data structures. */ -#define HMC5883L_ADDRESS PX4_I2C_OBDEV_HMC5883 #define HMC5883L_DEVICE_PATH_INT "/dev/hmc5883_int" #define HMC5883L_DEVICE_PATH_EXT "/dev/hmc5883_ext" @@ -95,9 +96,6 @@ #define ADDR_DATA_OUT_Y_MSB 0x07 #define ADDR_DATA_OUT_Y_LSB 0x08 #define ADDR_STATUS 0x09 -#define ADDR_ID_A 0x0a -#define ADDR_ID_B 0x0b -#define ADDR_ID_C 0x0c /* modes not changeable outside of driver */ #define HMC5883L_MODE_NORMAL (0 << 0) /* default */ @@ -115,10 +113,11 @@ #define STATUS_REG_DATA_OUT_LOCK (1 << 1) /* page 16: set if data is only partially read, read device to reset */ #define STATUS_REG_DATA_READY (1 << 0) /* page 16: set if all axes have valid measurements */ -#define ID_A_WHO_AM_I 'H' -#define ID_B_WHO_AM_I '4' -#define ID_C_WHO_AM_I '3' - +enum HMC5883_BUS { + HMC5883_BUS_ALL, + HMC5883_BUS_INTERNAL, + HMC5883_BUS_EXTERNAL +}; /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -130,10 +129,10 @@ static const int ERROR = -1; # error This requires CONFIG_SCHED_WORKQUEUE. #endif -class HMC5883 : public device::I2C +class HMC5883 : public device::CDev { public: - HMC5883(int bus, const char *path, enum Rotation rotation); + HMC5883(device::Device *interface, const char *path, enum Rotation rotation); virtual ~HMC5883(); virtual int init(); @@ -147,7 +146,7 @@ public: void print_info(); protected: - virtual int probe(); + Device *_interface; private: work_s _work; @@ -174,7 +173,6 @@ private: bool _sensor_ok; /**< sensor was found and reports ok */ bool _calibrated; /**< the calibration is valid */ - int _bus; /**< the bus the device is connected to */ enum Rotation _rotation; struct mag_report _last_report; /**< used for info() */ @@ -182,15 +180,6 @@ private: uint8_t _range_bits; uint8_t _conf_reg; - /** - * Test whether the device supported by the driver is present at a - * specific address. - * - * @param address The I2C bus address to probe. - * @return True if the device is present. - */ - int probe_address(uint8_t address); - /** * Initialise the automatic measurement state machine and start it. * @@ -349,8 +338,9 @@ private: extern "C" __EXPORT int hmc5883_main(int argc, char *argv[]); -HMC5883::HMC5883(int bus, const char *path, enum Rotation rotation) : - I2C("HMC5883", path, bus, HMC5883L_ADDRESS, 400000), +HMC5883::HMC5883(device::Device *interface, const char *path, enum Rotation rotation) : + CDev("HMC5883", path), + _interface(interface), _work{}, _measure_ticks(0), _reports(nullptr), @@ -369,7 +359,6 @@ HMC5883::HMC5883(int bus, const char *path, enum Rotation rotation) : _conf_errors(perf_alloc(PC_COUNT, "hmc5883_conf_errors")), _sensor_ok(false), _calibrated(false), - _bus(bus), _rotation(rotation), _last_report{0}, _range_bits(0), @@ -416,9 +405,11 @@ HMC5883::init() { int ret = ERROR; - /* do I2C init (and probe) first */ - if (I2C::init() != OK) + ret = CDev::init(); + if (ret != OK) { + debug("CDev init failed"); goto out; + } /* allocate basic report buffers */ _reports = new RingBuffer(2, sizeof(mag_report)); @@ -429,20 +420,7 @@ HMC5883::init() reset(); _class_instance = register_class_devname(MAG_DEVICE_PATH); - - switch (_class_instance) { - case CLASS_DEVICE_PRIMARY: - _mag_orb_id = ORB_ID(sensor_mag0); - break; - - case CLASS_DEVICE_SECONDARY: - _mag_orb_id = ORB_ID(sensor_mag1); - break; - - case CLASS_DEVICE_TERTIARY: - _mag_orb_id = ORB_ID(sensor_mag2); - break; - } + _mag_orb_id = ORB_ID_TRIPLE(sensor_mag, _class_instance); ret = OK; /* sensor is ok, but not calibrated */ @@ -559,30 +537,6 @@ void HMC5883::check_conf(void) } } -int -HMC5883::probe() -{ - uint8_t data[3] = {0, 0, 0}; - - _retries = 10; - - if (read_reg(ADDR_ID_A, data[0]) || - read_reg(ADDR_ID_B, data[1]) || - read_reg(ADDR_ID_C, data[2])) - debug("read_reg fail"); - - _retries = 2; - - if ((data[0] != ID_A_WHO_AM_I) || - (data[1] != ID_B_WHO_AM_I) || - (data[2] != ID_C_WHO_AM_I)) { - debug("ID byte mismatch (%02x,%02x,%02x)", data[0], data[1], data[2]); - return -EIO; - } - - return OK; -} - ssize_t HMC5883::read(struct file *filp, char *buffer, size_t buflen) { @@ -643,6 +597,8 @@ HMC5883::read(struct file *filp, char *buffer, size_t buflen) int HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) { + unsigned dummy = arg; + switch (cmd) { case SENSORIOCSPOLLRATE: { switch (arg) { @@ -768,14 +724,12 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) return check_calibration(); case MAGIOCGEXTERNAL: - if (_bus == PX4_I2C_BUS_EXPANSION) - return 1; - else - return 0; + debug("MAGIOCGEXTERNAL in main driver"); + return _interface->ioctl(cmd, dummy); default: /* give it to the superclass */ - return I2C::ioctl(filp, cmd, arg); + return CDev::ioctl(filp, cmd, arg); } } @@ -890,7 +844,6 @@ HMC5883::collect() } report; int ret; - uint8_t cmd; uint8_t check_counter; perf_begin(_sample_perf); @@ -908,8 +861,7 @@ HMC5883::collect() */ /* get measurements from the device */ - cmd = ADDR_DATA_OUT_X_MSB; - ret = transfer(&cmd, 1, (uint8_t *)&hmc_report, sizeof(hmc_report)); + ret = _interface->read(ADDR_DATA_OUT_X_MSB, (uint8_t *)&hmc_report, sizeof(hmc_report)); if (ret != OK) { perf_count(_comms_errors); @@ -946,14 +898,14 @@ HMC5883::collect() /* scale values for output */ -#ifdef PX4_I2C_BUS_ONBOARD - if (_bus == PX4_I2C_BUS_ONBOARD) { + // XXX revisit for SPI part, might require a bus type IOCTL + unsigned dummy; + if (!_interface->ioctl(MAGIOCGEXTERNAL, dummy)) { // convert onboard so it matches offboard for the // scaling below report.y = -report.y; report.x = -report.x; } -#endif /* the standard external mag by 3DR has x pointing to the * right, y pointing backwards, and z down, therefore switch x @@ -1291,15 +1243,17 @@ int HMC5883::set_excitement(unsigned enable) int HMC5883::write_reg(uint8_t reg, uint8_t val) { - uint8_t cmd[] = { reg, val }; - - return transfer(&cmd[0], 2, nullptr, 0); + uint8_t buf = val; + return _interface->write(reg, &buf, 1); } int HMC5883::read_reg(uint8_t reg, uint8_t &val) { - return transfer(®, 1, &val, 1); + uint8_t buf = val; + int ret = _interface->read(reg, &buf, 1); + val = buf; + return ret; } float @@ -1351,6 +1305,7 @@ void test(int bus); void reset(int bus); int info(int bus); int calibrate(int bus); +const char* get_path(int bus); void usage(); /** @@ -1360,43 +1315,87 @@ void usage(); * is either successfully up and running or failed to start. */ void -start(int bus, enum Rotation rotation) +start(int external_bus, enum Rotation rotation) { int fd; /* create the driver, attempt expansion bus first */ - if (bus == -1 || bus == PX4_I2C_BUS_EXPANSION) { - if (g_dev_ext != nullptr) + if (external_bus == HMC5883_BUS_ALL || external_bus == HMC5883_BUS_EXTERNAL) { + if (g_dev_ext != nullptr) { errx(0, "already started external"); - g_dev_ext = new HMC5883(PX4_I2C_BUS_EXPANSION, HMC5883L_DEVICE_PATH_EXT, rotation); - if (g_dev_ext != nullptr && OK != g_dev_ext->init()) { - delete g_dev_ext; - g_dev_ext = nullptr; + } + + device::Device *interface = nullptr; + + /* create the driver, only attempt I2C for the external bus */ + if (interface == nullptr && (HMC5883_I2C_interface != nullptr)) { + interface = HMC5883_I2C_interface(PX4_I2C_BUS_EXPANSION); + } + + if (interface == nullptr) { + warnx("failed to allocate an interface"); + } + + if (interface->init() != OK) { + delete interface; + warnx("interface init failed"); + } else { + + g_dev_ext = new HMC5883(interface, HMC5883L_DEVICE_PATH_EXT, rotation); + if (g_dev_ext != nullptr && OK != g_dev_ext->init()) { + delete g_dev_ext; + g_dev_ext = nullptr; + } + } } -#ifdef PX4_I2C_BUS_ONBOARD /* if this failed, attempt onboard sensor */ - if (bus == -1 || bus == PX4_I2C_BUS_ONBOARD) { - if (g_dev_int != nullptr) + if (external_bus == HMC5883_BUS_ALL || external_bus == HMC5883_BUS_INTERNAL) { + if (g_dev_int != nullptr) { errx(0, "already started internal"); - g_dev_int = new HMC5883(PX4_I2C_BUS_ONBOARD, HMC5883L_DEVICE_PATH_INT, rotation); - if (g_dev_int != nullptr && OK != g_dev_int->init()) { + } + + device::Device *interface = nullptr; + + /* create the driver, try SPI first, fall back to I2C if unsuccessful */ +#ifdef PX4_SPIDEV_HMC + if (HMC5883_SPI_interface != nullptr) { + interface = HMC5883_SPI_interface(PX4_SPI_BUS_SENSORS); + } +#endif + +#ifdef PX4_I2C_BUS_ONBOARD + if (interface == nullptr && (HMC5883_I2C_interface != nullptr)) { + interface = HMC5883_I2C_interface(PX4_I2C_BUS_ONBOARD); + } +#endif + if (interface == nullptr) { + warnx("failed to allocate an interface"); + } - /* tear down the failing onboard instance */ - delete g_dev_int; - g_dev_int = nullptr; + if (interface->init() != OK) { + delete interface; + warnx("interface init failed"); + } else { + + g_dev_int = new HMC5883(interface, HMC5883L_DEVICE_PATH_INT, rotation); + if (g_dev_int != nullptr && OK != g_dev_int->init()) { - if (bus == PX4_I2C_BUS_ONBOARD) { + /* tear down the failing onboard instance */ + delete g_dev_int; + g_dev_int = nullptr; + + if (external_bus == HMC5883_BUS_INTERNAL) { + goto fail; + } + } + if (g_dev_int == nullptr && external_bus == HMC5883_BUS_INTERNAL) { goto fail; } } - if (g_dev_int == nullptr && bus == PX4_I2C_BUS_ONBOARD) { - goto fail; - } } -#endif if (g_dev_int == nullptr && g_dev_ext == nullptr) goto fail; @@ -1425,11 +1424,11 @@ start(int bus, enum Rotation rotation) exit(0); fail: - if (g_dev_int != nullptr && (bus == -1 || bus == PX4_I2C_BUS_ONBOARD)) { + if (g_dev_int != nullptr && (external_bus == HMC5883_BUS_ALL || external_bus == HMC5883_BUS_INTERNAL)) { delete g_dev_int; g_dev_int = nullptr; } - if (g_dev_ext != nullptr && (bus == -1 || bus == PX4_I2C_BUS_EXPANSION)) { + if (g_dev_ext != nullptr && (external_bus == HMC5883_BUS_ALL || external_bus == HMC5883_BUS_EXTERNAL)) { delete g_dev_ext; g_dev_ext = nullptr; } @@ -1448,7 +1447,7 @@ test(int bus) struct mag_report report; ssize_t sz; int ret; - const char *path = (bus==PX4_I2C_BUS_ONBOARD?HMC5883L_DEVICE_PATH_INT:HMC5883L_DEVICE_PATH_EXT); + const char *path = get_path(bus); int fd = open(path, O_RDONLY); @@ -1549,7 +1548,7 @@ test(int bus) int calibrate(int bus) { int ret; - const char *path = (bus==PX4_I2C_BUS_ONBOARD?HMC5883L_DEVICE_PATH_INT:HMC5883L_DEVICE_PATH_EXT); + const char *path = get_path(bus); int fd = open(path, O_RDONLY); @@ -1576,7 +1575,7 @@ int calibrate(int bus) void reset(int bus) { - const char *path = (bus==PX4_I2C_BUS_ONBOARD?HMC5883L_DEVICE_PATH_INT:HMC5883L_DEVICE_PATH_EXT); + const char *path = get_path(bus); int fd = open(path, O_RDONLY); @@ -1600,12 +1599,12 @@ info(int bus) { int ret = 1; - HMC5883 *g_dev = (bus == PX4_I2C_BUS_ONBOARD ? g_dev_int : g_dev_ext); + HMC5883 *g_dev = (bus == HMC5883_BUS_INTERNAL ? g_dev_int : g_dev_ext); if (g_dev == nullptr) { warnx("not running on bus %d", bus); } else { - warnx("running on bus: %d (%s)\n", bus, ((PX4_I2C_BUS_ONBOARD) ? "onboard" : "offboard")); + warnx("running on bus: %d (%s)\n", bus, ((HMC5883_BUS_INTERNAL) ? "onboard" : "offboard")); g_dev->print_info(); ret = 0; @@ -1614,6 +1613,12 @@ info(int bus) return ret; } +const char* +get_path(int bus) +{ + return ((bus == HMC5883_BUS_INTERNAL) ? HMC5883L_DEVICE_PATH_INT : HMC5883L_DEVICE_PATH_EXT); +} + void usage() { @@ -1622,7 +1627,7 @@ usage() warnx(" -R rotation"); warnx(" -C calibrate on start"); warnx(" -X only external bus"); -#ifdef PX4_I2C_BUS_ONBOARD +#if (PX4_I2C_BUS_ONBOARD || PX4_SPIDEV_HMC) warnx(" -I only internal bus"); #endif } @@ -1633,7 +1638,7 @@ int hmc5883_main(int argc, char *argv[]) { int ch; - int bus = -1; + int bus = HMC5883_BUS_ALL; enum Rotation rotation = ROTATION_NONE; bool calibrate = false; @@ -1642,13 +1647,13 @@ hmc5883_main(int argc, char *argv[]) case 'R': rotation = (enum Rotation)atoi(optarg); break; -#ifdef PX4_I2C_BUS_ONBOARD +#if (PX4_I2C_BUS_ONBOARD || PX4_SPIDEV_HMC) case 'I': - bus = PX4_I2C_BUS_ONBOARD; + bus = HMC5883_BUS_INTERNAL; break; #endif case 'X': - bus = PX4_I2C_BUS_EXPANSION; + bus = HMC5883_BUS_EXTERNAL; break; case 'C': calibrate = true; @@ -1692,13 +1697,13 @@ hmc5883_main(int argc, char *argv[]) * Print driver information. */ if (!strcmp(verb, "info") || !strcmp(verb, "status")) { - if (bus == -1) { + if (bus == HMC5883_BUS_ALL) { int ret = 0; - if (hmc5883::info(PX4_I2C_BUS_ONBOARD)) { + if (hmc5883::info(HMC5883_BUS_INTERNAL)) { ret = 1; } - if (hmc5883::info(PX4_I2C_BUS_EXPANSION)) { + if (hmc5883::info(HMC5883_BUS_EXTERNAL)) { ret = 1; } exit(ret); diff --git a/src/drivers/hmc5883/hmc5883.h b/src/drivers/hmc5883/hmc5883.h new file mode 100644 index 000000000..0eb773629 --- /dev/null +++ b/src/drivers/hmc5883/hmc5883.h @@ -0,0 +1,52 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file hmc5883.h + * + * Shared defines for the hmc5883 driver. + */ + +#pragma once + +#define ADDR_ID_A 0x0a +#define ADDR_ID_B 0x0b +#define ADDR_ID_C 0x0c + +#define ID_A_WHO_AM_I 'H' +#define ID_B_WHO_AM_I '4' +#define ID_C_WHO_AM_I '3' + +/* interface factories */ +extern device::Device *HMC5883_SPI_interface(int bus) weak_function; +extern device::Device *HMC5883_I2C_interface(int bus) weak_function; diff --git a/src/drivers/hmc5883/hmc5883_i2c.cpp b/src/drivers/hmc5883/hmc5883_i2c.cpp new file mode 100644 index 000000000..2d3e8fa08 --- /dev/null +++ b/src/drivers/hmc5883/hmc5883_i2c.cpp @@ -0,0 +1,175 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + + /** + * @file HMC5883_I2C.cpp + * + * I2C interface for HMC5883 / HMC 5983 + */ + +/* XXX trim includes */ +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include + +#include "hmc5883.h" + +#include "board_config.h" + +#ifdef PX4_I2C_OBDEV_HMC5883 + +#define HMC5883L_ADDRESS PX4_I2C_OBDEV_HMC5883 + +device::Device *HMC5883_I2C_interface(int bus); + +class HMC5883_I2C : public device::I2C +{ +public: + HMC5883_I2C(int bus); + virtual ~HMC5883_I2C(); + + virtual int init(); + virtual int read(unsigned address, void *data, unsigned count); + virtual int write(unsigned address, void *data, unsigned count); + + virtual int ioctl(unsigned operation, unsigned &arg); + +protected: + virtual int probe(); + +}; + +device::Device * +HMC5883_I2C_interface(int bus) +{ + return new HMC5883_I2C(bus); +} + +HMC5883_I2C::HMC5883_I2C(int bus) : + I2C("HMC5883_I2C", nullptr, bus, 0, 400000) +{ +} + +HMC5883_I2C::~HMC5883_I2C() +{ +} + +int +HMC5883_I2C::init() +{ + /* this will call probe() */ + return I2C::init(); +} + +int +HMC5883_I2C::ioctl(unsigned operation, unsigned &arg) +{ + int ret; + + switch (operation) { + + case MAGIOCGEXTERNAL: + if (_bus == PX4_I2C_BUS_EXPANSION) { + return 1; + } else { + return 0; + } + + default: + ret = -EINVAL; + } + + return ret; +} + +int +HMC5883_I2C::probe() +{ + uint8_t data[3] = {0, 0, 0}; + + _retries = 10; + + if (read(ADDR_ID_A, &data[0], 1) || + read(ADDR_ID_B, &data[1], 1) || + read(ADDR_ID_C, &data[2], 1)) { + debug("read_reg fail"); + return -EIO; + } + + _retries = 2; + + if ((data[0] != ID_A_WHO_AM_I) || + (data[1] != ID_B_WHO_AM_I) || + (data[2] != ID_C_WHO_AM_I)) { + debug("ID byte mismatch (%02x,%02x,%02x)", data[0], data[1], data[2]); + return -EIO; + } + + return OK; +} + +int +HMC5883_I2C::write(unsigned address, void *data, unsigned count) +{ + uint8_t buf[32]; + + if (sizeof(buf) < (count + 1)) { + return -EIO; + } + + buf[0] = address; + memcpy(&buf[1], data, count); + + return transfer(&buf[0], count + 1, nullptr, 0); +} + +int +HMC5883_I2C::read(unsigned address, void *data, unsigned count) +{ + uint8_t cmd = address; + return transfer(&cmd, 1, (uint8_t *)data, count); +} + +#endif /* PX4_I2C_OBDEV_HMC5883 */ diff --git a/src/drivers/hmc5883/hmc5883_spi.cpp b/src/drivers/hmc5883/hmc5883_spi.cpp new file mode 100644 index 000000000..25a2f2b40 --- /dev/null +++ b/src/drivers/hmc5883/hmc5883_spi.cpp @@ -0,0 +1,189 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + + /** + * @file HMC5883_SPI.cpp + * + * SPI interface for HMC5983 + */ + +/* XXX trim includes */ +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include + +#include "hmc5883.h" +#include + +#ifdef PX4_SPIDEV_HMC + +/* SPI protocol address bits */ +#define DIR_READ (1<<7) +#define DIR_WRITE (0<<7) +#define ADDR_INCREMENT (1<<6) + +#define HMC_MAX_SEND_LEN 4 +#define HMC_MAX_RCV_LEN 8 + +device::Device *HMC5883_SPI_interface(int bus); + +class HMC5883_SPI : public device::SPI +{ +public: + HMC5883_SPI(int bus, spi_dev_e device); + virtual ~HMC5883_SPI(); + + virtual int init(); + virtual int read(unsigned address, void *data, unsigned count); + virtual int write(unsigned address, void *data, unsigned count); + + virtual int ioctl(unsigned operation, unsigned &arg); + +}; + +device::Device * +HMC5883_SPI_interface(int bus) +{ + return new HMC5883_SPI(bus, (spi_dev_e)PX4_SPIDEV_HMC); +} + +HMC5883_SPI::HMC5883_SPI(int bus, spi_dev_e device) : + SPI("HMC5883_SPI", nullptr, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz */) +{ +} + +HMC5883_SPI::~HMC5883_SPI() +{ +} + +int +HMC5883_SPI::init() +{ + int ret; + + ret = SPI::init(); + if (ret != OK) { + debug("SPI init failed"); + return -EIO; + } + + // read WHO_AM_I value + uint8_t data[3] = {0, 0, 0}; + + if (read(ADDR_ID_A, &data[0], 1) || + read(ADDR_ID_B, &data[1], 1) || + read(ADDR_ID_C, &data[2], 1)) { + debug("read_reg fail"); + } + + if ((data[0] != ID_A_WHO_AM_I) || + (data[1] != ID_B_WHO_AM_I) || + (data[2] != ID_C_WHO_AM_I)) { + debug("ID byte mismatch (%02x,%02x,%02x)", data[0], data[1], data[2]); + return -EIO; + } + + return OK; +} + +int +HMC5883_SPI::ioctl(unsigned operation, unsigned &arg) +{ + int ret; + + switch (operation) { + + case MAGIOCGEXTERNAL: + +#ifdef PX4_SPI_BUS_EXT + if (_bus == PX4_SPI_BUS_EXT) { + return 1; + } else +#endif + { + return 0; + } + + default: + { + ret = -EINVAL; + } + } + + return ret; +} + +int +HMC5883_SPI::write(unsigned address, void *data, unsigned count) +{ + uint8_t buf[32]; + + if (sizeof(buf) < (count + 1)) { + return -EIO; + } + + buf[0] = address | DIR_WRITE; + memcpy(&buf[1], data, count); + + return transfer(&buf[0], &buf[0], count + 1); +} + +int +HMC5883_SPI::read(unsigned address, void *data, unsigned count) +{ + uint8_t buf[32]; + + if (sizeof(buf) < (count + 1)) { + return -EIO; + } + + buf[0] = address | DIR_READ | ADDR_INCREMENT; + + int ret = transfer(&buf[0], &buf[0], count + 1); + memcpy(data, &buf[1], count); + return ret; +} + +#endif /* PX4_SPIDEV_HMC */ diff --git a/src/drivers/hmc5883/module.mk b/src/drivers/hmc5883/module.mk index be2ee7276..d4028b511 100644 --- a/src/drivers/hmc5883/module.mk +++ b/src/drivers/hmc5883/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -37,7 +37,7 @@ MODULE_COMMAND = hmc5883 -SRCS = hmc5883.cpp +SRCS = hmc5883_i2c.cpp hmc5883_spi.cpp hmc5883.cpp MODULE_STACKSIZE = 1200 -- cgit v1.2.3