diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 83e45406b5..e8ea172b75 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -130,6 +130,15 @@ then fi fi +# SHT3x temperature and hygrometer sensor, external I2C +if param compare -s SENS_EN_SHT3X 1 +then + if ! sht3x start -X + then + sht3x start -X -a 0x45 + fi +fi + # TE MS4525 differential pressure sensor external I2C if param compare -s SENS_EN_MS4525 1 then diff --git a/boards/px4/fmu-v5/default.px4board b/boards/px4/fmu-v5/default.px4board index f420af13af..0d116706c1 100644 --- a/boards/px4/fmu-v5/default.px4board +++ b/boards/px4/fmu-v5/default.px4board @@ -15,6 +15,7 @@ CONFIG_COMMON_DISTANCE_SENSOR=y CONFIG_DRIVERS_DSHOT=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_HEATER=y +CONFIG_HYGROMETERS=y CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=y CONFIG_DRIVERS_IMU_BOSCH_BMI055=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y @@ -81,8 +82,8 @@ CONFIG_MODULES_UUV_ATT_CONTROL=y CONFIG_MODULES_UUV_POS_CONTROL=y CONFIG_MODULES_VMOUNT=y CONFIG_MODULES_VTOL_ATT_CONTROL=y -CONFIG_SYSTEMCMDS_BL_UPDATE=y CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_BL_UPDATE=y CONFIG_SYSTEMCMDS_DMESG=y CONFIG_SYSTEMCMDS_DUMPFILE=y CONFIG_SYSTEMCMDS_GPIO=y diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index c136a44c88..eafa5dc1f0 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -198,6 +198,7 @@ #define DRV_GPS_DEVTYPE_SIM 0xAF +#define DRV_HYGRO_DEVTYPE_SHT3X 0xB1 #define DRV_DEVTYPE_UNUSED 0xff diff --git a/src/drivers/hygrometer/Kconfig b/src/drivers/hygrometer/Kconfig new file mode 100644 index 0000000000..578fcff768 --- /dev/null +++ b/src/drivers/hygrometer/Kconfig @@ -0,0 +1,9 @@ +menu "Hygrometers" + menuconfig HYGROMETERS + bool "Hygrometer sensors" + default n + select DRIVERS_HYGROMETER_SHT3x + ---help--- + Enable default set of hygrometer sensors + rsource "*/Kconfig" +endmenu #Hygrometers diff --git a/src/drivers/hygrometer/sht3x/CMakeLists.txt b/src/drivers/hygrometer/sht3x/CMakeLists.txt new file mode 100644 index 0000000000..2edf899151 --- /dev/null +++ b/src/drivers/hygrometer/sht3x/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2021 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__hygrometer__sht3x + MAIN sht3x + COMPILE_FLAGS + SRCS + sht3x.cpp + sht3x.h + DEPENDS + px4_work_queue + ) diff --git a/src/drivers/hygrometer/sht3x/Kconfig b/src/drivers/hygrometer/sht3x/Kconfig new file mode 100644 index 0000000000..424ec79481 --- /dev/null +++ b/src/drivers/hygrometer/sht3x/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_HYGROMETER_SHT3x + bool "SHT3x temperature and humidity sensor" + default n + ---help--- + Enable support SHT35 temperature/humidity sensor diff --git a/src/drivers/hygrometer/sht3x/sht3x.cpp b/src/drivers/hygrometer/sht3x/sht3x.cpp new file mode 100644 index 0000000000..ff68cd62c3 --- /dev/null +++ b/src/drivers/hygrometer/sht3x/sht3x.cpp @@ -0,0 +1,349 @@ +/**************************************************************************** + * + * Copyright (c) 2021 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 sht3x.c + * + * I2C driver for temperature/humidity sensor SHT3x by senserion + * + * @author Roman Dvorak + * + */ + +#include "sht3x.h" + +SHT3X::SHT3X(const I2CSPIDriverConfig &config) : + I2C(config), + ModuleParams(nullptr), + I2CSPIDriver(config) +{ +} + +uint8_t SHT3X::calc_crc(uint8_t data[2]) +{ + uint8_t crc = 0xFF; + + for (int i = 0; i < 2; i++) { + crc ^= data[i]; + + for (uint8_t bit = 8; bit > 0; --bit) { + if (crc & 0x80) { + crc = (crc << 1) ^ 0x31u; + + } else { + crc = (crc << 1); + } + } + } + + return crc; +} + + +int SHT3X::set_pointer(uint16_t command) +{ + if (_last_command != command) { + uint8_t cmd[2]; + cmd[0] = static_cast(command >> 8); + cmd[1] = static_cast(command & 0xff); + _last_command = command; + return transfer(&cmd[0], 2, nullptr, 0); + + } else { + return 0; + } +} + +int SHT3X::read_data(uint16_t command, uint8_t *data_ptr, uint8_t length) +{ + set_pointer(command); + + uint8_t raw_data[length]; + transfer(nullptr, 0, &raw_data[0], length); + + uint8_t crc_err = 0; + + for (int i = 0; i < length / 3; ++i) { + uint8_t crc_data[2] = {raw_data[i * 3], raw_data[i * 3 + 1]}; + + if (raw_data[i * 3 + 2] != calc_crc(crc_data)) { + crc_err ++; + } + + *(data_ptr + i * 2) = raw_data[i * 3]; + *(data_ptr + i * 2 + 1) = raw_data[i * 3 + 1]; + } + + return crc_err; +} + +int SHT3X::write_data(uint16_t command, uint8_t buffer[], uint8_t length) +{ + _last_command = command; + + uint8_t cmd[2 + 3 * length / 2]; + cmd[0] = static_cast(command >> 8); + cmd[1] = static_cast(command & 0xff); + + for (int i = 0; i < length / 2; ++i) { + cmd[2 + 3 * i] = buffer[2 * i]; + cmd[2 + 3 * i + 1] = buffer[2 * i + 1]; + uint8_t crc_data[2] = {buffer[2 * i], buffer[2 * i + 1]}; + cmd[2 + 3 * i + 2] = calc_crc(crc_data); + } + + return transfer(&cmd[0], sizeof(cmd), nullptr, 0); +} + + +void SHT3X::sensor_compouse_msg(bool send) +{ + uint8_t data[4]; + int error = read_data(SHT3x_CMD_FETCH_DATA, &data[0], 6); + + if (error == PX4_OK) { + measurement_time = hrt_absolute_time(); + measurement_index ++; + + measured_temperature = (float) 175 * (data[0] << 8 | data[1]) / 65535 - 45; + measured_humidity = (float) 100 * (data[2] << 8 | data[3]) / 65535; + + if (send) { + sensor_hygrometer_s msg{}; + msg.timestamp = hrt_absolute_time(); + msg.timestamp_sample = measurement_time; + msg.temperature = measured_temperature; + msg.humidity = measured_humidity; + msg.device_id = _sht_info.serial_number; + _sensor_hygrometer_pub.publish(msg); + } + } +} + + +int +SHT3X::probe() +{ + uint8_t type[2]; + uint8_t nvalid; + + nvalid = read_data(SHT3X_CMD_READ_STATUS, &type[0], 3); + return (!(nvalid == PX4_OK)); + // 0 means I can see sensor +} + +int SHT3X::init() +{ + if (I2C::init() != PX4_OK) { + return PX4_ERROR; + } + + _sensor_hygrometer_pub.advertise(); + ScheduleOnInterval(50000); + return PX4_OK; +} + + + +int SHT3X::init_sensor() +{ + set_pointer(SHT3X_CMD_CLEAR_STATUS); + px4_usleep(2000); + + // Get sensor serial number + uint8_t serial[4]; + read_data(SHT3x_CMD_READ_SN, &serial[0], 6); + + _sht_info.serial_number = ((uint32_t)serial[0] << 24 + | (uint32_t)serial[1] << 16 + | (uint32_t)serial[2] << 8 + | (uint32_t)serial[3]); + + set_pointer(SHT3x_CMD_PERIODIC_2HZ_MEDIUM); + px4_usleep(2000); + probe(); + + PX4_INFO("Connected to SHT3x sensor, SN: %ld", _sht_info.serial_number); + + return PX4_OK; +} + +void SHT3X::RunImpl() +{ + switch (_state) { + case sht3x_state::INIT: + probe(); + init_sensor(); + _state = sht3x_state::MEASUREMENT; + break; + + case sht3x_state::MEASUREMENT: + if ((hrt_absolute_time() - measurement_time) > 200000) { + sensor_compouse_msg(1); + } + + if ((hrt_absolute_time() - measurement_time) > 3000000) { + _state = sht3x_state::ERROR_READOUT; + } + + break; + + case sht3x_state::ERROR_GENERAL: + case sht3x_state::ERROR_READOUT: { + if (_last_state != _state) { + PX4_INFO("I cant get new data. The sensor may be disconnected."); + } + + if (probe() == PX4_OK) { + _state = sht3x_state::INIT; + } + } + break; + } + + if (_last_state != _state) { + _time_in_state = hrt_absolute_time(); + _last_state = _state; + } +} + + +void +SHT3X::custom_method(const BusCLIArguments &cli) +{ + switch (cli.custom1) { + + case 1: { + PX4_INFO("Last measured values (%.3fs ago, #%d)", (double)(hrt_absolute_time() - measurement_time) / 1000000.0, + measurement_index); + PX4_INFO("Temp: %.3f, Hum: %.3f", (double)measured_temperature, (double)measured_humidity); + + } + break; + + case 2: { + _state = sht3x_state::INIT; + } + break; + } +} + + +void SHT3X::print_status() +{ + PX4_INFO("SHT3X sensor"); + I2CSPIDriverBase::print_status(); + PX4_INFO("SN: %ld", _sht_info.serial_number); + PX4_INFO("Status: %s", sht_state_names[_state]); +} + + +void SHT3X::print_usage() +{ + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +SHT3x Temperature and Humidity Sensor Driver by Senserion. + +### Examples +CLI usage example: +$ sht3x start -X + Start the sensor driver on the external bus + +$ sht3x status + Print driver status + +$ sht3x values + Print last measured values + +$ sht3x reset + Reinitialize senzor, reset flags + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("sht3x", "driver"); + + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); + PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x44); + + PRINT_MODULE_USAGE_PARAMS_I2C_KEEP_RUNNING_FLAG(); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + PRINT_MODULE_USAGE_COMMAND_DESCR("values", "Print actual data"); + PRINT_MODULE_USAGE_COMMAND_DESCR("reset", "Reinitialize sensor"); + +} + +int sht3x_main(int argc, char *argv[]) +{ + using ThisDriver = SHT3X; + BusCLIArguments cli{true, false}; + cli.default_i2c_frequency = 400000; + cli.i2c_address = 0x44; + cli.support_keep_running = true; + + const char *verb = cli.parseDefaultArguments(argc, argv); + + if (!verb) { + ThisDriver::print_usage(); + return -1; + } + + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_HYGRO_DEVTYPE_SHT3X); + + 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); + } + + if (!strcmp(verb, "values")) { + cli.custom1 = 1; + return ThisDriver::module_custom_method(cli, iterator, false); + } + + if (!strcmp(verb, "reset")) { + cli.custom1 = 2; + return ThisDriver::module_custom_method(cli, iterator, false); + } + + ThisDriver::print_usage(); + return -1; +} diff --git a/src/drivers/hygrometer/sht3x/sht3x.h b/src/drivers/hygrometer/sht3x/sht3x.h new file mode 100644 index 0000000000..8f2bbc1548 --- /dev/null +++ b/src/drivers/hygrometer/sht3x/sht3x.h @@ -0,0 +1,122 @@ +/**************************************************************************** + * + * Copyright (c) 2021 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 sht3x.h + * + * Header file for SHT3x driver + * + * @author Roman Dvorak + * + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +#define SHT3X_CMD_READ_STATUS 0xF32D +#define SHT3X_CMD_CLEAR_STATUS 0x3041 +#define SHT3x_CMD_PERIODIC_2HZ_MEDIUM 0x2220 +#define SHT3x_CMD_FETCH_DATA 0xE000 +#define SHT3x_CMD_READ_SN 0x3780 + + +using namespace time_literals; + +extern "C" __EXPORT int sht3x_main(int argc, char *argv[]); + +enum sht3x_state { + ERROR_GENERAL, + ERROR_READOUT, + INIT, + MEASUREMENT +}; +const char *sht_state_names[] = {"General error", "Readout error", "Initialization", + "Measurement" + }; + + +struct sht_info { + uint32_t serial_number; +}; + + +class SHT3X : public device::I2C, public ModuleParams, public I2CSPIDriver +{ +public: + SHT3X(const I2CSPIDriverConfig &config); + ~SHT3X() = default; + + static void print_usage(); + void RunImpl(); + + int init() override; + int probe() override; + int init_sensor(); + void print_status() override; + + void custom_method(const BusCLIArguments &cli); + + int set_pointer(uint16_t command); + int read_data(uint16_t command, uint8_t *data_ptr, uint8_t length); + int write_data(uint16_t command, uint8_t buffer[], uint8_t length); + + void sensor_compouse_msg(bool send); + + uint8_t calc_crc(uint8_t data[2]); + +private: + + float measured_temperature = 0; + float measured_humidity = 0; + uint32_t measurement_time = 0; + uint16_t measurement_index = 0; + + sht_info _sht_info; + int _state = sht3x_state::INIT; + int _last_state = sht3x_state::INIT; + uint32_t _time_in_state = hrt_absolute_time(); + uint16_t _last_command = 0; + uORB::Publication _sensor_hygrometer_pub{ORB_ID(sensor_hygrometer)}; + + DEFINE_PARAMETERS( + (ParamInt) _param_sens_en_sht3x + ) +}; diff --git a/src/drivers/hygrometer/sht3x/sht3x_params.c b/src/drivers/hygrometer/sht3x/sht3x_params.c new file mode 100644 index 0000000000..2d54886f4a --- /dev/null +++ b/src/drivers/hygrometer/sht3x/sht3x_params.c @@ -0,0 +1,41 @@ +/**************************************************************************** + * + * Copyright (c) 2021 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. + * + ****************************************************************************/ + +/** + * SHT3x temperature and hygrometer + * + * @reboot_required true + * @group Sensors + * @boolean + */ +PARAM_DEFINE_INT32(SENS_EN_SHT3X, 0); diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 89633391a7..6107edece6 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -87,6 +87,7 @@ void LoggedTopics::add_default_topics() add_topic("rtl_time_estimate", 1000); add_topic("safety"); add_topic("sensor_combined"); + add_topic("sensor_hygrometer", 500); add_optional_topic("sensor_correction"); add_optional_topic("sensor_gyro_fft", 50); add_topic("sensor_selection");