diff --git a/boards/cuav/nora/init/rc.board_sensors b/boards/cuav/nora/init/rc.board_sensors index d477f85ed2..81a51ac496 100644 --- a/boards/cuav/nora/init/rc.board_sensors +++ b/boards/cuav/nora/init/rc.board_sensors @@ -16,3 +16,6 @@ ms5611 -s -b 4 start # SPI6 (internal) icm20649 -s -b 6 -R 2 start ms5611 -s -b 6 start + +# External compass on GPS1/I2C1: standard CUAV GPS/compass puck (with lights, safety button, and buzzer) +ist8310 -X -b 1 -R 10 start diff --git a/boards/cuav/x7pro/init/rc.board_sensors b/boards/cuav/x7pro/init/rc.board_sensors index b083d028fb..8aaa003e4b 100644 --- a/boards/cuav/x7pro/init/rc.board_sensors +++ b/boards/cuav/x7pro/init/rc.board_sensors @@ -17,3 +17,6 @@ ms5611 -s -b 4 start # SPI6 (internal) icm20649 -s -b 6 -R 2 start ms5611 -s -b 6 start + +# External compass on GPS1/I2C1: standard CUAV GPS/compass puck (with lights, safety button, and buzzer) +ist8310 -X -b 1 -R 10 start diff --git a/boards/holybro/durandal-v1/init/rc.board_sensors b/boards/holybro/durandal-v1/init/rc.board_sensors index 88f65ac26d..211eabf0c7 100644 --- a/boards/holybro/durandal-v1/init/rc.board_sensors +++ b/boards/holybro/durandal-v1/init/rc.board_sensors @@ -12,7 +12,10 @@ bmi088 -A -R 2 -s start bmi088 -G -R 2 -s start # internal compass -ist8310 -I start +ist8310 -I -R 10 start # Baro on internal SPI ms5611 -s start + +# External compass on GPS1/I2C1: standard Holybro GPS/compass puck (with lights, safety button, and buzzer) +ist8310 -X -b 1 -R 10 start diff --git a/boards/holybro/durandal-v1/stackcheck.cmake b/boards/holybro/durandal-v1/stackcheck.cmake index fcc3b6c019..f13f47127a 100644 --- a/boards/holybro/durandal-v1/stackcheck.cmake +++ b/boards/holybro/durandal-v1/stackcheck.cmake @@ -39,7 +39,7 @@ px4_add_board( #lights/rgbled lights/rgbled_ncp5623c #magnetometer # all available magnetometer drivers - magnetometer/ist8310 + magnetometer/isentek/ist8310 #mkblctrl #optical_flow # all available optical flow drivers #osd diff --git a/boards/holybro/pix32v5/init/rc.board_sensors b/boards/holybro/pix32v5/init/rc.board_sensors index 03663d7ae7..abffe41899 100644 --- a/boards/holybro/pix32v5/init/rc.board_sensors +++ b/boards/holybro/pix32v5/init/rc.board_sensors @@ -16,7 +16,10 @@ bmi055 -A -R 2 -s start bmi055 -G -R 2 -s start # internal compass -ist8310 -I start +ist8310 -I -R 10 start # Baro on internal SPI ms5611 -s start + +# External compass on GPS1/I2C1 (the 3rd external bus): standard Holybro GPS/compass puck (with lights, safety button, and buzzer) +ist8310 -X -b 3 -R 10 start diff --git a/boards/intel/aerofc-v1/default.cmake b/boards/intel/aerofc-v1/default.cmake index 8d0906d197..ec2574a73a 100644 --- a/boards/intel/aerofc-v1/default.cmake +++ b/boards/intel/aerofc-v1/default.cmake @@ -21,7 +21,7 @@ px4_add_board( #irlock #magnetometer # all available magnetometer drivers magnetometer/hmc5883 - magnetometer/ist8310 + magnetometer/isentek/ist8310 #optical_flow/px4flow #protocol_splitter pwm_out_sim diff --git a/boards/intel/aerofc-v1/init/rc.board_sensors b/boards/intel/aerofc-v1/init/rc.board_sensors index 7dfd1ea869..653757b35d 100644 --- a/boards/intel/aerofc-v1/init/rc.board_sensors +++ b/boards/intel/aerofc-v1/init/rc.board_sensors @@ -12,7 +12,7 @@ fi mpu9250 -s -R 0 start -ist8310 -I -R 4 start +ist8310 -I -R 14 start ll40ls start -X diff --git a/boards/intel/aerofc-v1/rtps.cmake b/boards/intel/aerofc-v1/rtps.cmake index 709a4221a2..a6b61a75f1 100644 --- a/boards/intel/aerofc-v1/rtps.cmake +++ b/boards/intel/aerofc-v1/rtps.cmake @@ -21,7 +21,7 @@ px4_add_board( #irlock #magnetometer # all available magnetometer drivers magnetometer/hmc5883 - magnetometer/ist8310 + magnetometer/isentek/ist8310 #optical_flow/px4flow protocol_splitter pwm_out_sim diff --git a/boards/nxp/fmurt1062-v1/init/rc.board_sensors b/boards/nxp/fmurt1062-v1/init/rc.board_sensors index 22630ea429..2b6a09fbe2 100644 --- a/boards/nxp/fmurt1062-v1/init/rc.board_sensors +++ b/boards/nxp/fmurt1062-v1/init/rc.board_sensors @@ -28,7 +28,7 @@ bmi055 -A -R 2 -s start bmi055 -G -R 2 -s start # internal compass -ist8310 -I start +ist8310 -I -R 10 start # Baro on internal SPI ms5611 -s start diff --git a/boards/px4/fmu-v5/init/rc.board_sensors b/boards/px4/fmu-v5/init/rc.board_sensors index a40cf1b11f..e321e7f6d9 100644 --- a/boards/px4/fmu-v5/init/rc.board_sensors +++ b/boards/px4/fmu-v5/init/rc.board_sensors @@ -15,8 +15,11 @@ icm20689 -s -R 2 start bmi055 -A -R 2 -s start bmi055 -G -R 2 -s start -# internal compass -ist8310 -I start - # Baro on internal SPI ms5611 -s start + +# internal compass +ist8310 -I -R 10 start + +# External compass on GPS1/I2C1 (the 3rd external bus): standard Holybro Pixhawk 4 or CUAV V5 GPS/compass puck (with lights, safety button, and buzzer) +ist8310 -X -b 3 -R 10 start diff --git a/boards/px4/fmu-v5/optimized.cmake b/boards/px4/fmu-v5/optimized.cmake index f96a977216..332049c100 100644 --- a/boards/px4/fmu-v5/optimized.cmake +++ b/boards/px4/fmu-v5/optimized.cmake @@ -40,7 +40,7 @@ px4_add_board( lights/rgbled_ncp5623c #lights/rgbled_pwm #magnetometer # all available magnetometer drivers - magnetometer/ist8310 + magnetometer/isentek/ist8310 optical_flow # all available optical flow drivers #pwm_input pwm_out_sim diff --git a/boards/uvify/core/default.cmake b/boards/uvify/core/default.cmake index a02a695944..490c75709d 100644 --- a/boards/uvify/core/default.cmake +++ b/boards/uvify/core/default.cmake @@ -29,7 +29,7 @@ px4_add_board( lights/rgbled_ncp5623c magnetometer/bmm150 magnetometer/lis3mdl - magnetometer/ist8310 + magnetometer/isentek/ist8310 optical_flow # all available optical flow drivers pca9685 pwm_input diff --git a/cmake/px4_add_board.cmake b/cmake/px4_add_board.cmake index cd3aa7d5b9..053d226223 100644 --- a/cmake/px4_add_board.cmake +++ b/cmake/px4_add_board.cmake @@ -101,7 +101,7 @@ # gps # imu/bmi055 # imu/mpu6000 -# magnetometer/ist8310 +# magnetometer/isentek/ist8310 # pwm_out # px4io # rgbled diff --git a/src/drivers/magnetometer/CMakeLists.txt b/src/drivers/magnetometer/CMakeLists.txt index 1366ae32f5..adffb80b36 100644 --- a/src/drivers/magnetometer/CMakeLists.txt +++ b/src/drivers/magnetometer/CMakeLists.txt @@ -36,7 +36,6 @@ add_subdirectory(bmm150) add_subdirectory(hmc5883) add_subdirectory(qmc5883) add_subdirectory(isentek) -add_subdirectory(ist8310) add_subdirectory(lis2mdl) add_subdirectory(lis3mdl) add_subdirectory(lsm303agr) diff --git a/src/drivers/magnetometer/isentek/CMakeLists.txt b/src/drivers/magnetometer/isentek/CMakeLists.txt index 189cdfaa4f..14885b686f 100644 --- a/src/drivers/magnetometer/isentek/CMakeLists.txt +++ b/src/drivers/magnetometer/isentek/CMakeLists.txt @@ -32,3 +32,4 @@ ############################################################################ add_subdirectory(ist8308) +add_subdirectory(ist8310) diff --git a/src/drivers/magnetometer/ist8310/CMakeLists.txt b/src/drivers/magnetometer/isentek/ist8310/CMakeLists.txt similarity index 89% rename from src/drivers/magnetometer/ist8310/CMakeLists.txt rename to src/drivers/magnetometer/isentek/ist8310/CMakeLists.txt index 67f24cffca..c9bc5d6a80 100644 --- a/src/drivers/magnetometer/ist8310/CMakeLists.txt +++ b/src/drivers/magnetometer/isentek/ist8310/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# Copyright (c) 2020 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 @@ -30,11 +30,16 @@ # POSSIBILITY OF SUCH DAMAGE. # ############################################################################ + px4_add_module( - MODULE drivers__ist8310 + MODULE drivers__magnetometer__isentek__ist8310 MAIN ist8310 + COMPILE_FLAGS SRCS - ist8310.cpp + IST8310.cpp + IST8310.hpp + ist8310_main.cpp + iSentek_IST8310_registers.hpp DEPENDS drivers_magnetometer px4_work_queue diff --git a/src/drivers/magnetometer/isentek/ist8310/IST8310.cpp b/src/drivers/magnetometer/isentek/ist8310/IST8310.cpp new file mode 100644 index 0000000000..364fbdf04c --- /dev/null +++ b/src/drivers/magnetometer/isentek/ist8310/IST8310.cpp @@ -0,0 +1,299 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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. + * + ****************************************************************************/ + +#include "IST8310.hpp" + +using namespace time_literals; + +static constexpr int16_t combine(uint8_t msb, uint8_t lsb) +{ + return (msb << 8u) | lsb; +} + +IST8310::IST8310(I2CSPIBusOption bus_option, int bus, int bus_frequency, enum Rotation rotation) : + I2C(DRV_MAG_DEVTYPE_IST8310, MODULE_NAME, bus, I2C_ADDRESS_DEFAULT, bus_frequency), + I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), + _px4_mag(get_device_id(), rotation) +{ + _px4_mag.set_external(external()); +} + +IST8310::~IST8310() +{ + perf_free(_reset_perf); + perf_free(_bad_register_perf); + perf_free(_bad_transfer_perf); +} + +int IST8310::init() +{ + int ret = I2C::init(); + + if (ret != PX4_OK) { + DEVICE_DEBUG("I2C::init failed (%i)", ret); + return ret; + } + + return Reset() ? 0 : -1; +} + +bool IST8310::Reset() +{ + _state = STATE::RESET; + ScheduleClear(); + ScheduleNow(); + return true; +} + +void IST8310::print_status() +{ + I2CSPIDriverBase::print_status(); + + perf_print_counter(_reset_perf); + perf_print_counter(_bad_register_perf); + perf_print_counter(_bad_transfer_perf); +} + +int IST8310::probe() +{ + const uint8_t WAI = RegisterRead(Register::WAI); + + if (WAI != Device_ID) { + DEVICE_DEBUG("unexpected WAI 0x%02x", WAI); + return PX4_ERROR; + } + + return PX4_OK; +} + +void IST8310::RunImpl() +{ + const hrt_abstime now = hrt_absolute_time(); + + switch (_state) { + case STATE::RESET: + // CNTL2: Software Reset + RegisterWrite(Register::CNTL2, CNTL2_BIT::SRST); + _reset_timestamp = now; + _failure_count = 0; + _state = STATE::WAIT_FOR_RESET; + perf_count(_reset_perf); + ScheduleDelayed(50_ms); // Power On Reset: max 50ms + break; + + case STATE::WAIT_FOR_RESET: + + // SRST: This bit is automatically reset to zero after POR routine + if ((RegisterRead(Register::WAI) == Device_ID) + && ((RegisterRead(Register::CNTL2) & CNTL2_BIT::SRST) == 0)) { + + // if reset succeeded then configure + _state = STATE::CONFIGURE; + ScheduleDelayed(10_ms); + + } else { + // RESET not complete + if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + PX4_DEBUG("Reset failed, retrying"); + _state = STATE::RESET; + ScheduleDelayed(100_ms); + + } else { + PX4_DEBUG("Reset not complete, check again in 10 ms"); + ScheduleDelayed(10_ms); + } + } + + break; + + case STATE::CONFIGURE: + if (Configure()) { + // if configure succeeded then start measurement cycle + _state = STATE::MEASURE; + ScheduleDelayed(20_ms); + + } else { + // CONFIGURE not complete + if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + PX4_DEBUG("Configure failed, resetting"); + _state = STATE::RESET; + + } else { + PX4_DEBUG("Configure failed, retrying"); + } + + ScheduleDelayed(100_ms); + } + + break; + + case STATE::MEASURE: + RegisterWrite(Register::CNTL1, CNTL1_BIT::MODE_SINGLE_MEASUREMENT); + _state = STATE::READ; + ScheduleDelayed(20_ms); // Wait at least 6ms. (minimum waiting time for 16 times internal average setup) + break; + + case STATE::READ: { + struct TransferBuffer { + uint8_t STAT1; + uint8_t DATAXL; + uint8_t DATAXH; + uint8_t DATAYL; + uint8_t DATAYH; + uint8_t DATAZL; + uint8_t DATAZH; + } buffer{}; + + bool success = false; + uint8_t cmd = static_cast(Register::STAT1); + + if (transfer(&cmd, 1, (uint8_t *)&buffer, sizeof(buffer)) == PX4_OK) { + + if (buffer.STAT1 & STAT1_BIT::DRDY) { + int16_t x = combine(buffer.DATAXH, buffer.DATAXL); + int16_t y = combine(buffer.DATAYH, buffer.DATAYL); + int16_t z = combine(buffer.DATAZH, buffer.DATAZL); + + // 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, x, y, 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 + RegisterWrite(Register::CNTL1, CNTL1_BIT::MODE_SINGLE_MEASUREMENT); + ScheduleDelayed(20_ms); // Wait at least 6ms. (minimum waiting time for 16 times internal average setup) + } + + break; + } +} + +bool IST8310::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 IST8310::RegisterCheck(const register_config_t ®_cfg) +{ + bool success = true; + + const uint8_t reg_value = RegisterRead(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 IST8310::RegisterRead(Register reg) +{ + const uint8_t cmd = static_cast(reg); + uint8_t buffer{}; + transfer(&cmd, 1, &buffer, 1); + return buffer; +} + +void IST8310::RegisterWrite(Register reg, uint8_t value) +{ + uint8_t buffer[2] { (uint8_t)reg, value }; + transfer(buffer, sizeof(buffer), nullptr, 0); +} + +void IST8310::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits) +{ + const uint8_t orig_val = RegisterRead(reg); + uint8_t val = (orig_val & ~clearbits) | setbits; + + if (orig_val != val) { + RegisterWrite(reg, val); + } +} diff --git a/src/drivers/magnetometer/isentek/ist8310/IST8310.hpp b/src/drivers/magnetometer/isentek/ist8310/IST8310.hpp new file mode 100644 index 0000000000..945ea9d0de --- /dev/null +++ b/src/drivers/magnetometer/isentek/ist8310/IST8310.hpp @@ -0,0 +1,115 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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 IST8310.hpp + * + * Driver for the iSentek IST8310 connected via I2C. + * + */ + +#pragma once + +#include "iSentek_IST8310_registers.hpp" + +#include +#include +#include +#include +#include + +using namespace iSentek_IST8310; + +class IST8310 : public device::I2C, public I2CSPIDriver +{ +public: + IST8310(I2CSPIBusOption bus_option, int bus, int bus_frequency, enum Rotation rotation = ROTATION_NONE); + ~IST8310() override; + + static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance); + static void print_usage(); + + void RunImpl(); + + int init() override; + void print_status() override; + +private: + // Sensor Configuration + struct register_config_t { + Register reg; + uint8_t set_bits{0}; + uint8_t clear_bits{0}; + }; + + int probe() override; + + bool Reset(); + + bool Configure(); + + bool RegisterCheck(const register_config_t ®_cfg); + + uint8_t RegisterRead(Register reg); + void RegisterWrite(Register reg, uint8_t value); + void RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits); + + PX4Magnetometer _px4_mag; + + perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")}; + perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")}; + perf_counter_t _reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": reset")}; + + hrt_abstime _reset_timestamp{0}; + hrt_abstime _last_config_check_timestamp{0}; + int _failure_count{0}; + + enum class STATE : uint8_t { + RESET, + WAIT_FOR_RESET, + CONFIGURE, + MEASURE, + READ, + } _state{STATE::RESET}; + + uint8_t _checked_register{0}; + static constexpr uint8_t size_register_cfg{4}; + register_config_t _register_cfg[size_register_cfg] { + // Register | Set bits, Clear bits + { Register::CNTL2, 0, CNTL2_BIT::SRST }, + { Register::CNTL3, CNTL3_BIT::Z_16BIT | CNTL3_BIT::Y_16BIT | CNTL3_BIT::X_16BIT, 0 }, + { Register::AVGCNTL, AVGCNTL_BIT::Y_16TIMES | AVGCNTL_BIT::XZ_16TIMES, 0 }, + { Register::PDCNTL, PDCNTL_BIT::PULSE_NORMAL, 0 }, + }; +}; diff --git a/src/drivers/magnetometer/isentek/ist8310/iSentek_IST8310_registers.hpp b/src/drivers/magnetometer/isentek/ist8310/iSentek_IST8310_registers.hpp new file mode 100644 index 0000000000..845bfd270b --- /dev/null +++ b/src/drivers/magnetometer/isentek/ist8310/iSentek_IST8310_registers.hpp @@ -0,0 +1,149 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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 iSentek_IST8310_registers.hpp + * + * iSentek IST8310 registers. + * + */ + +#pragma once + +#include + +// TODO: move to a central header +static constexpr uint8_t Bit0 = (1 << 0); +static constexpr uint8_t Bit1 = (1 << 1); +static constexpr uint8_t Bit2 = (1 << 2); +static constexpr uint8_t Bit3 = (1 << 3); +static constexpr uint8_t Bit4 = (1 << 4); +static constexpr uint8_t Bit5 = (1 << 5); +static constexpr uint8_t Bit6 = (1 << 6); +static constexpr uint8_t Bit7 = (1 << 7); + +namespace iSentek_IST8310 +{ +static constexpr uint32_t I2C_SPEED = 400 * 1000; // 400 kHz I2C serial interface +static constexpr uint8_t I2C_ADDRESS_DEFAULT = 0x0E; + +static constexpr uint8_t Device_ID = 0x10; + +enum class Register : uint8_t { + WAI = 0x00, // Who Am I Register + + STAT1 = 0x02, // Status Register 1 + DATAXL = 0x03, + DATAXH = 0x04, + DATAYL = 0x05, + DATAYH = 0x06, + DATAZL = 0x07, + DATAZH = 0x08, + STAT2 = 0x09, // Status Register 2 + CNTL1 = 0x0A, // Control Setting Register 1 + CNTL2 = 0x0B, // Control Setting Register 2 + STR = 0x0C, // Self-Test Register + CNTL3 = 0x0D, // Control Setting Register 3 + + TEMPL = 0x1C, // Temperature Sensor Output Register (Low Byte) + TEMPH = 0x1D, // Temperature Sensor Output Register (High Byte) + + TCCNTL = 0x40, // Temperature Compensation Control register + AVGCNTL = 0x41, // Average Control Register + PDCNTL = 0x42, // Pulse Duration Control Register + + XX_CROSS_L = 0x9C, // cross axis xx low byte + XX_CROSS_H = 0x9D, // cross axis xx high byte + XY_CROSS_L = 0x9E, // cross axis xy low byte + XY_CROSS_H = 0x9F, // cross axis xy high byte + XZ_CROSS_L = 0xA0, // cross axis xz low byte + XZ_CROSS_H = 0xA1, // cross axis xz high byte + YX_CROSS_L = 0xA2, // cross axis yx low byte + YX_CROSS_H = 0xA3, // cross axis yx high byte + YY_CROSS_L = 0xA4, // cross axis yy low byte + YY_CROSS_H = 0xA5, // cross axis yy high byte + YZ_CROSS_L = 0xA6, // cross axis yz low byte + YZ_CROSS_H = 0xA7, // cross axis yz high byte + ZX_CROSS_L = 0xA8, // cross axis zx low byte + ZX_CROSS_H = 0xA9, // cross axis zx high byte + ZY_CROSS_L = 0xAA, // cross axis zy low byte + ZY_CROSS_H = 0xAB, // cross axis zy high byte + ZZ_CROSS_L = 0xAC, // cross axis zz low byte + ZZ_CROSS_H = 0xAD, // cross axis zz high byte +}; + +// STAT1 +enum STAT1_BIT : uint8_t { + DOR = Bit1, // Data overrun bit + DRDY = Bit0, // Data ready +}; + +// CNTL1 +enum CNTL1_BIT : uint8_t { + // 3:0 Mode: Operating mode setting + MODE_SINGLE_MEASUREMENT = Bit0, +}; + +// CNTL2 +enum CNTL2_BIT : uint8_t { + SRST = Bit0, // Soft reset, perform the same routine as POR +}; + +// STR +enum STR_BIT : uint8_t { + SELF_TEST = Bit6, +}; + +// CNTL3 +enum CNTL3_BIT : uint8_t { + Z_16BIT = Bit6, // Sensor output resolution adjustment for Z axis: 16-bit (Sensitivity: 1320 LSB/Gauss) + Y_16BIT = Bit5, // Sensor output resolution adjustment for Y axis: 16-bit (Sensitivity: 1320 LSB/Gauss) + X_16BIT = Bit4, // Sensor output resolution adjustment for X axis: 16-bit (Sensitivity: 1320 LSB/Gauss) +}; + +// AVGCNTL +enum AVGCNTL_BIT : uint8_t { + // 5:3 Average times for y sensor data. Times of average will be done before switch to next channel + Y_16TIMES = Bit5, // 3’b100 average by 16 times (ODRmax=166Hz) + + // 2:0 Average times for x & z sensor data. Times of average will be done before switch to next channel + XZ_16TIMES = Bit2, // average by 16 times (ODRmax=166Hz) +}; + +// PDCNTL +enum PDCNTL_BIT : uint8_t { + // 7:6 Pulse duration + PULSE_NORMAL = Bit7 | Bit6, // Normal (please use this setting) +}; + +} // namespace iSentek_IST8310 diff --git a/src/drivers/magnetometer/isentek/ist8310/ist8310_main.cpp b/src/drivers/magnetometer/isentek/ist8310/ist8310_main.cpp new file mode 100644 index 0000000000..8e75ce233f --- /dev/null +++ b/src/drivers/magnetometer/isentek/ist8310/ist8310_main.cpp @@ -0,0 +1,106 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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. + * + ****************************************************************************/ + +#include "IST8310.hpp" + +#include +#include + +I2CSPIDriverBase *IST8310::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance) +{ + IST8310 *instance = new IST8310(iterator.configuredBusOption(), iterator.bus(), cli.bus_frequency, cli.rotation); + + if (!instance) { + PX4_ERR("alloc failed"); + return nullptr; + } + + if (instance->init() != PX4_OK) { + delete instance; + PX4_DEBUG("no device on bus %i (devid 0x%x)", iterator.bus(), iterator.devid()); + return nullptr; + } + + return instance; +} + +void IST8310::print_usage() +{ + PRINT_MODULE_USAGE_NAME("ist8310", "driver"); + PRINT_MODULE_USAGE_SUBCATEGORY("magnetometer"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); + PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); +} + +extern "C" int ist8310_main(int argc, char *argv[]) +{ + int ch; + using ThisDriver = IST8310; + BusCLIArguments cli{true, false}; + cli.default_i2c_frequency = I2C_SPEED; + + while ((ch = cli.getopt(argc, argv, "R:")) != EOF) { + switch (ch) { + case 'R': + cli.rotation = (enum Rotation)atoi(cli.optarg()); + break; + } + } + + const char *verb = cli.optarg(); + + if (!verb) { + ThisDriver::print_usage(); + return -1; + } + + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_MAG_DEVTYPE_IST8310); + + if (!strcmp(verb, "start")) { + return ThisDriver::module_start(cli, iterator); + } + + if (!strcmp(verb, "stop")) { + return ThisDriver::module_stop(iterator); + } + + if (!strcmp(verb, "status")) { + return ThisDriver::module_status(iterator); + } + + ThisDriver::print_usage(); + return -1; +} diff --git a/src/drivers/magnetometer/ist8310/ist8310.cpp b/src/drivers/magnetometer/ist8310/ist8310.cpp deleted file mode 100644 index 4eccc043c6..0000000000 --- a/src/drivers/magnetometer/ist8310/ist8310.cpp +++ /dev/null @@ -1,657 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2016 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 ist8310.cpp - * - * Driver for the IST8310 magnetometer connected via I2C. - * - * @author David Sidrane - * @author Maelok Dong - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -/* - * IST8310 internal constants and data structures. - */ - -/* Max measurement rate is 160Hz, however with 160 it will be set to 166 Hz, therefore workaround using 150 - * The datasheet gives 200Hz maximum measurement rate, but it's not true according to tech support from iSentek*/ -#define IST8310_CONVERSION_INTERVAL (1000000 / 100) /* microseconds */ - -#define IST8310_BUS_I2C_ADDR 0xE -#define IST8310_DEFAULT_BUS_SPEED 400000 - -/* - * FSR: - * x, y: +- 1600 µT - * z: +- 2500 µT - * - * Resolution according to datasheet is 0.3µT/LSB - */ -#define IST8310_RESOLUTION 0.3 - -static const int16_t IST8310_MAX_VAL_XY = (1600 / IST8310_RESOLUTION) + 1; -static const int16_t IST8310_MIN_VAL_XY = -IST8310_MAX_VAL_XY; -static const int16_t IST8310_MAX_VAL_Z = (2500 / IST8310_RESOLUTION) + 1; -static const int16_t IST8310_MIN_VAL_Z = -IST8310_MAX_VAL_Z; - -/* Hardware definitions */ - -#define ADDR_WAI 0 /* WAI means 'Who Am I'*/ -# define WAI_EXPECTED_VALUE 0x10 - -#define ADDR_STAT1 0x02 -# define STAT1_DRDY_SHFITS 0x0 -# define STAT1_DRDY (1 << STAT1_DRDY_SHFITS) -# define STAT1_DRO_SHFITS 0x1 -# define STAT1_DRO (1 << STAT1_DRO_SHFITS) - -#define ADDR_DATA_OUT_X_LSB 0x03 -#define ADDR_DATA_OUT_X_MSB 0x04 -#define ADDR_DATA_OUT_Y_LSB 0x05 -#define ADDR_DATA_OUT_Y_MSB 0x06 -#define ADDR_DATA_OUT_Z_LSB 0x07 -#define ADDR_DATA_OUT_Z_MSB 0x08 - -#define ADDR_STAT2 0x09 -# define STAT2_INT_SHFITS 3 -# define STAT2_INT (1 << STAT2_INT_SHFITS) - -#define ADDR_CTRL1 0x0a -# define CTRL1_MODE_SHFITS 0 -# define CTRL1_MODE_STDBY (0 << CTRL1_MODE_SHFITS) -# define CTRL1_MODE_SINGLE (1 << CTRL1_MODE_SHFITS) - -#define ADDR_CTRL2 0x0b -# define CTRL2_SRST_SHFITS 0 /* Begin POR (auto cleared) */ -# define CTRL2_SRST (1 << CTRL2_SRST_SHFITS) -# define CTRL2_DRP_SHIFTS 2 -# define CTRL2_DRP (1 << CTRL2_DRP_SHIFTS) -# define CTRL2_DREN_SHIFTS 3 -# define CTRL2_DREN (1 << CTRL2_DREN_SHIFTS) - -#define ADDR_CTRL3 0x41 -# define CTRL3_SAMPLEAVG_16 0x24 /* Sample Averaging 16 */ -# define CTRL3_SAMPLEAVG_8 0x1b /* Sample Averaging 8 */ -# define CTRL3_SAMPLEAVG_4 0x12 /* Sample Averaging 4 */ -# define CTRL3_SAMPLEAVG_2 0x09 /* Sample Averaging 2 */ - -#define ADDR_CTRL4 0x42 -# define CTRL4_SRPD 0xC0 /* Set Reset Pulse Duration */ - -#define ADDR_STR 0x0c -# define STR_SELF_TEST_SHFITS 6 -# define STR_SELF_TEST_ON (1 << STR_SELF_TEST_SHFITS) -# define STR_SELF_TEST_OFF (0 << STR_SELF_TEST_SHFITS) - -#define ADDR_Y11_Low 0x9c -#define ADDR_Y11_High 0x9d -#define ADDR_Y12_Low 0x9e -#define ADDR_Y12_High 0x9f -#define ADDR_Y13_Low 0xa0 -#define ADDR_Y13_High 0xa1 -#define ADDR_Y21_Low 0xa2 -#define ADDR_Y21_High 0xa3 -#define ADDR_Y22_Low 0xa4 -#define ADDR_Y22_High 0xa5 -#define ADDR_Y23_Low 0xa6 -#define ADDR_Y23_High 0xa7 -#define ADDR_Y31_Low 0xa8 -#define ADDR_Y31_High 0xa9 -#define ADDR_Y32_Low 0xaa -#define ADDR_Y32_High 0xab -#define ADDR_Y33_Low 0xac -#define ADDR_Y33_High 0xad - -#define ADDR_TEMPL 0x1c -#define ADDR_TEMPH 0x1d - -class IST8310 : public device::I2C, public I2CSPIDriver -{ -public: - IST8310(I2CSPIBusOption bus_option, int bus_number, int address, enum Rotation rotation, int bus_frequency); - virtual ~IST8310(); - - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); - static void print_usage(); - - virtual int init(); - - /** - * Initialise the automatic measurement state machine and start it. - * - * @note This function is called at open and error time. It might make sense - * to make it more aggressive about resetting the bus in case of errors. - */ - void start(); - - /** - * Reset the device - */ - int reset(); - - /** - * Perform a poll cycle; collect from the previous measurement - * and start a new one. - * - * This is the heart of the measurement state machine. This function - * alternately starts a measurement, or collects the data from the - * previous measurement. - * - * When the interval between measurements is greater than the minimum - * measurement interval, a gap is inserted between collection - * and measurement to provide the most recent measurement possible - * at the next interval. - */ - void RunImpl(); - -private: - int probe() override; - - void print_status() override; - - PX4Magnetometer _px4_mag; - - unsigned _measure_interval{IST8310_CONVERSION_INTERVAL}; - - bool _collect_phase{false}; - - perf_counter_t _sample_perf; - perf_counter_t _comms_errors; - perf_counter_t _range_errors; - perf_counter_t _conf_errors; - - uint8_t _ctl3_reg{0}; - uint8_t _ctl4_reg{0}; - - /** - * check the sensor configuration. - * - * checks that the config of the sensor is correctly set, to - * cope with communication errors causing the configuration to - * change - */ - void check_conf(); - - /** - * Write a register. - * - * @param reg The register to write. - * @param val The value to write. - * @return OK on write success. - */ - int write_reg(uint8_t reg, uint8_t val); - - /** - * Write to a register block. - * - * @param address The register address to write to. - * @param data The buffer to write from. - * @param count The number of bytes to write. - * @return OK on write success. - */ - int write(unsigned address, void *data, unsigned count); - - /** - * Read a register. - * - * @param reg The register to read. - * @param val The value read. - * @return OK on read success. - */ - int read_reg(uint8_t reg, uint8_t &val); - - /** - * read register block. - * - * @param address The register address to read from. - * @param data The buffer to read into. - * @param count The number of bytes to read. - * @return OK on write success. - */ - int read(unsigned address, void *data, unsigned count); - - /** - * Issue a measurement command. - * - * @return OK if the measurement command was successful. - */ - int measure(); - - /** - * Collect the result of the most recent measurement. - */ - int collect(); -}; - -IST8310::IST8310(I2CSPIBusOption bus_option, int bus_number, int address, enum Rotation rotation, int bus_frequency) : - I2C(DRV_MAG_DEVTYPE_IST8310, MODULE_NAME, bus_number, address, bus_frequency), - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus_number, address), - _px4_mag(get_device_id(), rotation), - _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")), - _comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": com_err")), - _range_errors(perf_alloc(PC_COUNT, MODULE_NAME": rng_err")), - _conf_errors(perf_alloc(PC_COUNT, MODULE_NAME": conf_err")) -{ - _px4_mag.set_external(external()); - - // default range scale from counts to gauss - _px4_mag.set_scale(0.003f); -} - -IST8310::~IST8310() -{ - // free perf counters - perf_free(_sample_perf); - perf_free(_comms_errors); - perf_free(_range_errors); - perf_free(_conf_errors); -} - -int IST8310::init() -{ - int ret = I2C::init(); - - if (ret != OK) { - DEVICE_DEBUG("I2C init failed"); - goto out; - } - - /* reset the device configuration */ - reset(); - - ret = OK; -out: - return ret; -} - -int IST8310::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 IST8310::read(unsigned address, void *data, unsigned count) -{ - uint8_t cmd = address; - return transfer(&cmd, 1, (uint8_t *)data, count); -} - -/** - check that the configuration register has the right value. This is - done periodically to cope with I2C bus noise causing the - configuration of the compass to change. - */ -void IST8310::check_conf() -{ - int ret; - - uint8_t ctrl_reg_in = 0; - ret = read_reg(ADDR_CTRL3, ctrl_reg_in); - - if (OK != ret) { - perf_count(_comms_errors); - return; - } - - if (ctrl_reg_in != _ctl3_reg) { - perf_count(_conf_errors); - ret = write_reg(ADDR_CTRL3, _ctl3_reg); - - if (OK != ret) { - perf_count(_comms_errors); - } - } - - ret = read_reg(ADDR_CTRL4, ctrl_reg_in); - - if (OK != ret) { - perf_count(_comms_errors); - return; - } - - if (ctrl_reg_in != _ctl4_reg) { - perf_count(_conf_errors); - ret = write_reg(ADDR_CTRL4, _ctl4_reg); - - if (OK != ret) { - perf_count(_comms_errors); - } - } -} - -void IST8310::start() -{ - /* reset the report ring and state machine */ - _collect_phase = false; - - /* schedule a cycle to start things */ - ScheduleNow(); -} - -int IST8310::reset() -{ - /* software reset */ - write_reg(ADDR_CTRL2, CTRL2_SRST); - - /* configure control register 3 */ - _ctl3_reg = CTRL3_SAMPLEAVG_16; - write_reg(ADDR_CTRL3, _ctl3_reg); - - /* configure control register 4 */ - _ctl4_reg = CTRL4_SRPD; - write_reg(ADDR_CTRL4, _ctl4_reg); - - return OK; -} - -int IST8310::probe() -{ - uint8_t data[1] = {0}; - - _retries = 10; - - if (read(ADDR_WAI, &data[0], 1)) { - DEVICE_DEBUG("read_reg fail"); - return -EIO; - } - - _retries = 2; - - if ((data[0] != WAI_EXPECTED_VALUE)) { - DEVICE_DEBUG("ID byte mismatch (%02x) expected %02x", data[0], WAI_EXPECTED_VALUE); - return -EIO; - } - - return OK; -} - -void IST8310::RunImpl() -{ - /* collection phase? */ - if (_collect_phase) { - - /* perform collection */ - if (OK != collect()) { - DEVICE_DEBUG("collection error"); - /* restart the measurement state machine */ - start(); - return; - } - - /* next phase is measurement */ - _collect_phase = false; - - /* - * Is there a collect->measure gap? - */ - if (_measure_interval > IST8310_CONVERSION_INTERVAL) { - - /* schedule a fresh cycle call when we are ready to measure again */ - ScheduleDelayed(_measure_interval - IST8310_CONVERSION_INTERVAL); - - return; - } - } - - /* measurement phase */ - if (OK != measure()) { - DEVICE_DEBUG("measure error"); - } - - /* next phase is collection */ - _collect_phase = true; - - /* schedule a fresh cycle call when the measurement is done */ - ScheduleDelayed(IST8310_CONVERSION_INTERVAL); -} - -int IST8310::measure() -{ - /* - * Send the command to begin a measurement. - */ - int ret = write_reg(ADDR_CTRL1, CTRL1_MODE_SINGLE); - - if (OK != ret) { - perf_count(_comms_errors); - } - - return ret; -} - -int IST8310::collect() -{ - struct { /* status register and data as read back from the device */ - uint8_t x[2]; - uint8_t y[2]; - uint8_t z[2]; - } report_buffer{}; - - struct { - int16_t x, y, z; - } report{}; - - int ret; - uint8_t check_counter; - - perf_begin(_sample_perf); - - float xraw_f; - float yraw_f; - float zraw_f; - - _px4_mag.set_error_count(perf_event_count(_comms_errors)); - - /* - * @note We could read the status register here, which could tell us that - * we were too early and that the output registers are still being - * written. In the common case that would just slow us down, and - * we're better off just never being early. - */ - - /* get measurements from the device */ - const hrt_abstime timestamp_sample = hrt_absolute_time(); - ret = read(ADDR_DATA_OUT_X_LSB, (uint8_t *)&report_buffer, sizeof(report_buffer)); - - if (ret != OK) { - perf_count(_comms_errors); - DEVICE_DEBUG("I2C read error"); - goto out; - } - - /* swap the data we just received */ - report.x = (((int16_t)report_buffer.x[1]) << 8) | (int16_t)report_buffer.x[0]; - report.y = (((int16_t)report_buffer.y[1]) << 8) | (int16_t)report_buffer.y[0]; - report.z = (((int16_t)report_buffer.z[1]) << 8) | (int16_t)report_buffer.z[0]; - - /* - * Check if value makes sense according to the FSR and Resolution of - * this sensor, discarding outliers - */ - if (report.x > IST8310_MAX_VAL_XY || report.x < IST8310_MIN_VAL_XY || - report.y > IST8310_MAX_VAL_XY || report.y < IST8310_MIN_VAL_XY || - report.z > IST8310_MAX_VAL_Z || report.z < IST8310_MIN_VAL_Z) { - perf_count(_range_errors); - DEVICE_DEBUG("data/status read error"); - goto out; - } - - /* - * raw outputs - * - * Sensor doesn't follow right hand rule, swap x and y to make it obey - * it. - */ - xraw_f = report.y; - yraw_f = report.x; - zraw_f = report.z; - - _px4_mag.update(timestamp_sample, xraw_f, yraw_f, zraw_f); - - /* - periodically check the range register and configuration - registers. With a bad I2C cable it is possible for the - registers to become corrupt, leading to bad readings. It - doesn't happen often, but given the poor cables some - vehicles have it is worth checking for. - */ - check_counter = perf_event_count(_sample_perf) % 256; - - if (check_counter == 128) { - check_conf(); - } - - ret = OK; - -out: - perf_end(_sample_perf); - return ret; -} - -int IST8310::write_reg(uint8_t reg, uint8_t val) -{ - uint8_t buf = val; - return write(reg, &buf, 1); -} - -int IST8310::read_reg(uint8_t reg, uint8_t &val) -{ - uint8_t buf = 0; - int ret = read(reg, &buf, 1); - val = buf; - return ret; -} - -void IST8310::print_status() -{ - I2CSPIDriverBase::print_status(); - perf_print_counter(_sample_perf); - perf_print_counter(_comms_errors); - printf("poll interval: %u interval\n", _measure_interval); -} - -I2CSPIDriverBase * -IST8310::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, int runtime_instance) -{ - IST8310 *interface = new IST8310(iterator.configuredBusOption(), iterator.bus(), cli.i2c_address, cli.rotation, - cli.bus_frequency); - - if (interface == nullptr) { - PX4_ERR("alloc failed"); - return nullptr; - } - - if (interface->init() != OK) { - delete interface; - PX4_DEBUG("no device on bus %i (devid 0x%x)", iterator.bus(), iterator.devid()); - return nullptr; - } - - interface->start(); - - return interface; -} - -void IST8310::print_usage() -{ - PRINT_MODULE_USAGE_NAME("ist8310", "driver"); - PRINT_MODULE_USAGE_SUBCATEGORY("magnetometer"); - PRINT_MODULE_USAGE_COMMAND("start"); - PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); - PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0xE); - PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true); - PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); -} - -extern "C" __EXPORT int ist8310_main(int argc, char *argv[]) -{ - int ch; - using ThisDriver = IST8310; - BusCLIArguments cli{true, false}; - cli.i2c_address = IST8310_BUS_I2C_ADDR; - cli.default_i2c_frequency = IST8310_DEFAULT_BUS_SPEED; - - while ((ch = cli.getopt(argc, argv, "R:")) != EOF) { - switch (ch) { - case 'R': - cli.rotation = (enum Rotation)atoi(cli.optarg()); - break; - } - } - - const char *verb = cli.optarg(); - - if (!verb) { - ThisDriver::print_usage(); - return -1; - } - - BusInstanceIterator iterator(MODULE_NAME, cli, DRV_MAG_DEVTYPE_IST8310); - - if (!strcmp(verb, "start")) { - return ThisDriver::module_start(cli, iterator); - } - - if (!strcmp(verb, "stop")) { - return ThisDriver::module_stop(iterator); - } - - if (!strcmp(verb, "status")) { - return ThisDriver::module_status(iterator); - } - - ThisDriver::print_usage(); - return 1; -}