From 422e89b1c0accf5b5cd88e7a115992dcbdaa1521 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 2 Mar 2020 17:00:41 -0500 Subject: [PATCH] iSentek IST8308 magnetometer driver --- src/drivers/drv_sensor.h | 1 + src/drivers/magnetometer/CMakeLists.txt | 1 + .../magnetometer/isentek/CMakeLists.txt | 34 ++ .../isentek/ist8308/CMakeLists.txt | 46 +++ .../magnetometer/isentek/ist8308/IST8308.cpp | 298 ++++++++++++++++++ .../magnetometer/isentek/ist8308/IST8308.hpp | 118 +++++++ .../ist8308/iSentek_IST8308_registers.hpp | 129 ++++++++ .../isentek/ist8308/ist8308_main.cpp | 210 ++++++++++++ .../drivers/magnetometer/PX4Magnetometer.hpp | 1 + 9 files changed, 838 insertions(+) create mode 100644 src/drivers/magnetometer/isentek/CMakeLists.txt create mode 100644 src/drivers/magnetometer/isentek/ist8308/CMakeLists.txt create mode 100644 src/drivers/magnetometer/isentek/ist8308/IST8308.cpp create mode 100644 src/drivers/magnetometer/isentek/ist8308/IST8308.hpp create mode 100644 src/drivers/magnetometer/isentek/ist8308/iSentek_IST8308_registers.hpp create mode 100644 src/drivers/magnetometer/isentek/ist8308/ist8308_main.cpp diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 6a5bdc20cb..6a537ad956 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -62,6 +62,7 @@ #define DRV_MAG_DEVTYPE_QMC5883 0x08 #define DRV_MAG_DEVTYPE_AK09916 0x09 #define DRV_IMU_DEVTYPE_ICM20948 0x0A +#define DRV_MAG_DEVTYPE_IST8308 0x0B #define DRV_ACC_DEVTYPE_LSM303D 0x11 #define DRV_ACC_DEVTYPE_BMA180 0x12 diff --git a/src/drivers/magnetometer/CMakeLists.txt b/src/drivers/magnetometer/CMakeLists.txt index 8decabd5a2..ecf1ff2a71 100644 --- a/src/drivers/magnetometer/CMakeLists.txt +++ b/src/drivers/magnetometer/CMakeLists.txt @@ -35,6 +35,7 @@ add_subdirectory(ak09916) add_subdirectory(bmm150) add_subdirectory(hmc5883) add_subdirectory(qmc5883) +add_subdirectory(isentek) add_subdirectory(ist8310) add_subdirectory(lis3mdl) add_subdirectory(lsm303agr) diff --git a/src/drivers/magnetometer/isentek/CMakeLists.txt b/src/drivers/magnetometer/isentek/CMakeLists.txt new file mode 100644 index 0000000000..189cdfaa4f --- /dev/null +++ b/src/drivers/magnetometer/isentek/CMakeLists.txt @@ -0,0 +1,34 @@ +############################################################################ +# +# 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. +# +############################################################################ + +add_subdirectory(ist8308) diff --git a/src/drivers/magnetometer/isentek/ist8308/CMakeLists.txt b/src/drivers/magnetometer/isentek/ist8308/CMakeLists.txt new file mode 100644 index 0000000000..0d74541f4e --- /dev/null +++ b/src/drivers/magnetometer/isentek/ist8308/CMakeLists.txt @@ -0,0 +1,46 @@ +############################################################################ +# +# 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. +# +############################################################################ + +px4_add_module( + MODULE drivers__magnetometer__isentek__ist8308 + MAIN ist8308 + COMPILE_FLAGS + SRCS + IST8308.cpp + IST8308.hpp + ist8308_main.cpp + iSentek_IST8308_registers.hpp + DEPENDS + drivers_magnetometer + px4_work_queue + ) diff --git a/src/drivers/magnetometer/isentek/ist8308/IST8308.cpp b/src/drivers/magnetometer/isentek/ist8308/IST8308.cpp new file mode 100644 index 0000000000..9899eaf9a4 --- /dev/null +++ b/src/drivers/magnetometer/isentek/ist8308/IST8308.cpp @@ -0,0 +1,298 @@ +/**************************************************************************** + * + * 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 "IST8308.hpp" + +using namespace time_literals; + +static constexpr int16_t combine(uint8_t msb, uint8_t lsb) +{ + return (msb << 8u) | lsb; +} + +IST8308::IST8308(int bus, uint8_t address, enum Rotation rotation) : + I2C(MODULE_NAME, nullptr, bus, address, I2C_SPEED), + ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())), + _px4_mag(get_device_id(), external() ? ORB_PRIO_VERY_HIGH : ORB_PRIO_HIGH, rotation) +{ + set_device_type(DRV_MAG_DEVTYPE_IST8308); + + _px4_mag.set_device_type(DRV_MAG_DEVTYPE_IST8308); + _px4_mag.set_external(external()); +} + +IST8308::~IST8308() +{ + Stop(); + + perf_free(_transfer_perf); + perf_free(_bad_register_perf); + perf_free(_bad_transfer_perf); +} + +bool IST8308::Init() +{ + if (I2C::init() != PX4_OK) { + PX4_ERR("I2C::init failed"); + return false; + } + + return Reset(); +} + +void IST8308::Stop() +{ + // wait until stopped + while (_state.load() != STATE::STOPPED) { + _state.store(STATE::REQUEST_STOP); + ScheduleNow(); + px4_usleep(10); + } +} + +bool IST8308::Reset() +{ + _state.store(STATE::RESET); + ScheduleClear(); + ScheduleNow(); + return true; +} + +void IST8308::PrintInfo() +{ + perf_print_counter(_transfer_perf); + perf_print_counter(_bad_register_perf); + perf_print_counter(_bad_transfer_perf); + + _px4_mag.print_status(); +} + +int IST8308::probe() +{ + const uint8_t whoami = RegisterRead(Register::WAI); + + if (whoami != Device_ID) { + PX4_WARN("unexpected WAI 0x%02x", whoami); + return PX4_ERROR; + } + + return PX4_OK; +} + +void IST8308::Run() +{ + switch (_state.load()) { + case STATE::RESET: + // CNTL3: Software Reset + RegisterSetAndClearBits(Register::CNTL3, CNTL3_BIT::SRST, 0); + _reset_timestamp = hrt_absolute_time(); + _state.store(STATE::WAIT_FOR_RESET); + ScheduleDelayed(50_ms); // Power On Reset: max:50ms + break; + + case STATE::WAIT_FOR_RESET: + + // Register::CNTL3 SRST: This bit is automatically reset to zero after POR routine + if ((RegisterRead(Register::WAI) == Device_ID) + && ((RegisterRead(Register::CNTL3) & CNTL3_BIT::SRST) == 0)) { + + // if reset succeeded then configure + _state.store(STATE::CONFIGURE); + ScheduleNow(); + + } else { + // RESET not complete + if (hrt_elapsed_time(&_reset_timestamp) > 100_ms) { + PX4_ERR("Reset failed, retrying"); + _state.store(STATE::RESET); + ScheduleNow(); + + } else { + PX4_DEBUG("Reset not complete, check again in 50 ms"); + ScheduleDelayed(50_ms); + } + } + + break; + + case STATE::CONFIGURE: + if (Configure()) { + // if configure succeeded then start reading every 20 ms (50 Hz) + _state.store(STATE::READ); + ScheduleOnInterval(20_ms, 20_ms); + + } else { + PX4_DEBUG("Configure failed, retrying"); + // try again in 50 ms + ScheduleDelayed(50_ms); + } + + break; + + case STATE::READ: { + struct TransferBuffer { + uint8_t STAT; + uint8_t DATAXL; + uint8_t DATAXH; + uint8_t DATAYL; + uint8_t DATAYH; + uint8_t DATAZL; + uint8_t DATAZH; + } buffer{}; + + perf_begin(_transfer_perf); + + bool failure = false; + const hrt_abstime timestamp_sample = hrt_absolute_time(); + const uint8_t cmd = static_cast(Register::STAT); + + if (transfer(&cmd, 1, (uint8_t *)&buffer, sizeof(buffer)) != PX4_OK) { + perf_count(_bad_transfer_perf); + failure = true; + } + + perf_end(_transfer_perf); + + if (!failure && (buffer.STAT && STAT_BIT::DRDY)) { + float x = combine(buffer.DATAXH, buffer.DATAXL); + float y = combine(buffer.DATAYH, buffer.DATAYL); + float 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.update(timestamp_sample, x, y, z); + } + + if (failure || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) { + // check registers incrementally + if (RegisterCheck(_register_cfg[_checked_register], true)) { + _last_config_check_timestamp = timestamp_sample; + _checked_register = (_checked_register + 1) % size_register_cfg; + + } else { + // register check failed, force reconfigure + PX4_DEBUG("Health check failed, reconfiguring"); + _state.store(STATE::CONFIGURE); + ScheduleNow(); + } + } + } + + break; + + case STATE::REQUEST_STOP: + ScheduleClear(); + _state.store(STATE::STOPPED); + break; + + case STATE::STOPPED: + // DO NOTHING + break; + } +} + +bool IST8308::Configure() +{ + bool success = true; + + for (const auto ® : _register_cfg) { + if (!RegisterCheck(reg)) { + success = false; + } + } + + // 1 Microtesla = 0.01 Gauss + _px4_mag.set_scale(1.f / 6.6f * 0.01f); // 6.6 LSB/uT + _px4_mag.set_temperature(NAN); // temperature not available + + return success; +} + +bool IST8308::RegisterCheck(const register_config_t ®_cfg, bool notify) +{ + 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; + } + + if (!success) { + RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits); + + if (notify) { + perf_count(_bad_register_perf); + _px4_mag.increase_error_count(); + } + } + + return success; +} + +uint8_t IST8308::RegisterRead(Register reg) +{ + const uint8_t cmd = static_cast(reg); + uint8_t buffer{}; + transfer(&cmd, 1, &buffer, 1); + return buffer; +} + +void IST8308::RegisterWrite(Register reg, uint8_t value) +{ + uint8_t buffer[2] { (uint8_t)reg, value }; + transfer(buffer, sizeof(buffer), nullptr, 0); +} + +void IST8308::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits) +{ + const uint8_t orig_val = RegisterRead(reg); + uint8_t val = orig_val; + + if (setbits) { + val |= setbits; + } + + if (clearbits) { + val &= ~clearbits; + } + + RegisterWrite(reg, val); +} diff --git a/src/drivers/magnetometer/isentek/ist8308/IST8308.hpp b/src/drivers/magnetometer/isentek/ist8308/IST8308.hpp new file mode 100644 index 0000000000..1432cfb8c4 --- /dev/null +++ b/src/drivers/magnetometer/isentek/ist8308/IST8308.hpp @@ -0,0 +1,118 @@ +/**************************************************************************** + * + * 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 IST8308.hpp + * + * Driver for the iSentek IST8308 connected via I2C. + * + */ + +#pragma once + +#include "iSentek_IST8308_registers.hpp" + +#include +#include +#include +#include +#include +#include + +using namespace iSentek_IST8308; + +class IST8308 : public device::I2C, public px4::ScheduledWorkItem +{ +public: + IST8308(int bus, uint8_t address = I2C_ADDRESS_DEFAULT, enum Rotation rotation = ROTATION_NONE); + ~IST8308() override; + + bool Init(); + void Start(); + void Stop(); + bool Reset(); + void PrintInfo(); + +private: + + // Sensor Configuration + struct register_config_t { + Register reg; + uint8_t set_bits{0}; + uint8_t clear_bits{0}; + }; + + int probe() override; + + void Run() override; + + bool Configure(); + + bool RegisterCheck(const register_config_t ®_cfg, bool notify = false); + + 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 _transfer_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": transfer")}; + 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")}; + + hrt_abstime _reset_timestamp{0}; + hrt_abstime _last_config_check_timestamp{0}; + + uint8_t _checked_register{0}; + + enum class STATE : uint8_t { + RESET, + WAIT_FOR_RESET, + CONFIGURE, + READ, + REQUEST_STOP, + STOPPED, + }; + + px4::atomic _state{STATE::RESET}; + + static constexpr uint8_t size_register_cfg{5}; + register_config_t _register_cfg[size_register_cfg] { + // Register | Set bits, Clear bits + { Register::ACTR, 0, ACTR_BIT::SUSPEND_EN }, + { Register::CNTL1, CNTL1_BIT::NSF_Low, 0 }, + { Register::CNTL2, CNTL2_BIT::MODE_ODR_50Hz, 0 }, + { Register::CNTL3, 0, CNTL3_BIT::SRST }, + { Register::OSRCNTL, OSRCNTL_BIT::OSR_y_32 | OSRCNTL_BIT::OSR_xz_32, 0 }, + }; +}; diff --git a/src/drivers/magnetometer/isentek/ist8308/iSentek_IST8308_registers.hpp b/src/drivers/magnetometer/isentek/ist8308/iSentek_IST8308_registers.hpp new file mode 100644 index 0000000000..1de506b832 --- /dev/null +++ b/src/drivers/magnetometer/isentek/ist8308/iSentek_IST8308_registers.hpp @@ -0,0 +1,129 @@ +/**************************************************************************** + * + * 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_IST8308_registers.hpp + * + * iSentek IST8308 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_IST8308 +{ +static constexpr uint32_t I2C_SPEED = 400 * 1000; // 400 kHz I2C serial interface +static constexpr uint8_t I2C_ADDRESS_DEFAULT = 0x0C; + +static constexpr uint8_t Device_ID = 0x08; + +enum class Register : uint8_t { + WAI = 0x00, // Who Am I Register + + STR = 0x0C, // Self-Test Register + + STAT = 0x10, // Status Register + DATAXL = 0x11, + DATAXH = 0x12, + DATAYL = 0x13, + DATAYH = 0x14, + DATAZL = 0x15, + DATAZH = 0x16, + + ACTR = 0x20, // Action Register + + CNTL1 = 0x30, // Control Setting Register 1 + CNTL2 = 0x31, // Control Setting Register 2 + CNTL3 = 0x32, // Control Setting Register 3 + CNTL4 = 0x34, // Control Setting Register 4 + + OSRCNTL = 0x41, // Over-Sampling Ratio Control Register +}; + +// STAT +enum STAT_BIT : uint8_t { + DOR = Bit1, // Data overrun bit + DRDY = Bit0, // Data ready +}; + +// ACTR +enum ACTR_BIT : uint8_t { + SUSPEND_EN = Bit1, +}; + +// CNTL1 +enum CNTL1_BIT : uint8_t { + // 6:5 NSF[1:0]: Noise Suppression Filter setting + NSF_Low = Bit5, + NSF_Middle = Bit6, + NSF_High = Bit6 | Bit5, +}; + +// CNTL2 +enum CNTL2_BIT : uint8_t { + // 4:0 MODE [4:0]: Operation mode setting + MODE_ODR_10Hz = Bit1, // 5’h02: Continuous Measurement Mode with ODR 10Hz + MODE_ODR_20Hz = Bit2, // 5’h04: Continuous Measurement Mode with ODR 20Hz + MODE_ODR_50Hz = Bit2 | Bit1, // 5’h06: Continuous Measurement Mode with ODR 50Hz + MODE_ODR_100Hz = Bit3, // 5’h08: Continuous Measurement Mode with ODR 100Hz +}; + +// CNTL3 +enum CNTL3_BIT : uint8_t { + SRST = Bit0, // Soft reset, perform the same routine as POR +}; + +// OSRCNTL +enum OSRCNTL_BIT : uint8_t { + // 5:3 + OSR_y_16 = Bit5, // 3’b100: OSR=16 (ODRmax=100) (Default) + OSR_y_32 = Bit5 | Bit3, // 3’b101: OSR=32 (ODRmax=50) + + // 2:0 + OSR_xz_16 = Bit2, // 3’b100: OSR=16 (ODRmax=100) (Default) + OSR_xz_32 = Bit2 | Bit0, // 3’b101: OSR=32 (ODRmax=50) +}; + +} // namespace iSentek_IST8308 diff --git a/src/drivers/magnetometer/isentek/ist8308/ist8308_main.cpp b/src/drivers/magnetometer/isentek/ist8308/ist8308_main.cpp new file mode 100644 index 0000000000..acb646ad19 --- /dev/null +++ b/src/drivers/magnetometer/isentek/ist8308/ist8308_main.cpp @@ -0,0 +1,210 @@ +/**************************************************************************** + * + * 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 "IST8308.hpp" + +#include + +namespace ist8308 +{ + +enum class BUS { + ALL = 0, + I2C_EXTERNAL = 1, + I2C_EXTERNAL1 = 2, + I2C_EXTERNAL2 = 3, + I2C_EXTERNAL3 = 4, + I2C_INTERNAL = 5, +}; + +struct bus_option { + BUS busid; + uint8_t busnum; + IST8308 *dev; +} bus_options[] = { +#if defined(PX4_I2C_BUS_ONBOARD) + { BUS::I2C_INTERNAL, PX4_I2C_BUS_ONBOARD, nullptr }, +#endif +#if defined(PX4_I2C_BUS_EXPANSION) + { BUS::I2C_EXTERNAL, PX4_I2C_BUS_EXPANSION, nullptr }, +#endif +#if defined(PX4_I2C_BUS_EXPANSION1) + { BUS::I2C_EXTERNAL1, PX4_I2C_BUS_EXPANSION1, nullptr }, +#endif +#if defined(PX4_I2C_BUS_EXPANSION2) + { BUS::I2C_EXTERNAL2, PX4_I2C_BUS_EXPANSION2, nullptr }, +#endif +#if defined(PX4_I2C_BUS_EXPANSION3) + { BUS::I2C_EXTERNAL3, PX4_I2C_BUS_EXPANSION3, nullptr }, +#endif +}; + +// find a bus structure for a busid +static bus_option *find_bus(BUS busid) +{ + for (bus_option &bus_option : bus_options) { + if ((busid == BUS::ALL || busid == bus_option.busid) + && bus_option.dev != nullptr) { + + return &bus_option; + } + } + + return nullptr; +} + +static bool start_bus(bus_option &bus, enum Rotation rotation) +{ + IST8308 *dev = new IST8308(bus.busnum, I2C_ADDRESS_DEFAULT, rotation); + + if (dev == nullptr) { + PX4_ERR("alloc failed"); + return false; + } + + if (!dev->Init()) { + PX4_ERR("driver start failed"); + delete dev; + return false; + } + + bus.dev = dev; + + return true; +} + +static int start(BUS busid, enum Rotation rotation) +{ + for (bus_option &bus_option : bus_options) { + if (bus_option.dev != nullptr) { + // this device is already started + PX4_WARN("already started"); + continue; + } + + if (busid != BUS::ALL && bus_option.busid != busid) { + // not the one that is asked for + continue; + } + + if (start_bus(bus_option, rotation)) { + return 0; + } + } + + return -1; +} + +static int stop(BUS busid) +{ + bus_option *bus = find_bus(busid); + + if (bus != nullptr && bus->dev != nullptr) { + delete bus->dev; + bus->dev = nullptr; + + } else { + PX4_WARN("driver not running"); + return -1; + } + + return 0; +} + +static int status(BUS busid) +{ + bus_option *bus = find_bus(busid); + + if (bus != nullptr && bus->dev != nullptr) { + bus->dev->PrintInfo(); + return 0; + } + + PX4_WARN("driver not running"); + return -1; +} + +static int usage() +{ + PX4_INFO("missing command: try 'start', 'stop', 'status'"); + PX4_INFO("options:"); + PX4_INFO(" -R rotation"); + + return 0; +} + +} // namespace ist8308 + +extern "C" int ist8308_main(int argc, char *argv[]) +{ + enum Rotation rotation = ROTATION_NONE; + int myoptind = 1; + int ch = 0; + const char *myoptarg = nullptr; + ist8308::BUS busid = ist8308::BUS::ALL; + + // start options + while ((ch = px4_getopt(argc, argv, "R:b:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'R': + rotation = (enum Rotation)atoi(myoptarg); + break; + + case 'b': + busid = (ist8308::BUS)atoi(myoptarg); + break; + + default: + return ist8308::usage(); + } + } + + if (myoptind >= argc) { + return ist8308::usage(); + } + + const char *verb = argv[myoptind]; + + if (!strcmp(verb, "start")) { + return ist8308::start(busid, rotation); + + } else if (!strcmp(verb, "stop")) { + return ist8308::stop(busid); + + } else if (!strcmp(verb, "status")) { + return ist8308::status(busid); + + } + + return ist8308::usage(); +} diff --git a/src/lib/drivers/magnetometer/PX4Magnetometer.hpp b/src/lib/drivers/magnetometer/PX4Magnetometer.hpp index 4d2108eab7..2bfa23d6ee 100644 --- a/src/lib/drivers/magnetometer/PX4Magnetometer.hpp +++ b/src/lib/drivers/magnetometer/PX4Magnetometer.hpp @@ -52,6 +52,7 @@ public: void set_device_type(uint8_t devtype); void set_error_count(uint64_t error_count) { _sensor_mag_pub.get().error_count = error_count; } + void increase_error_count() { _sensor_mag_pub.get().error_count++; } void set_scale(float scale) { _sensor_mag_pub.get().scaling = scale; } void set_temperature(float temperature) { _sensor_mag_pub.get().temperature = temperature; } void set_external(bool external) { _sensor_mag_pub.get().is_external = external; }