diff --git a/boards/ssrc/saluki-pi b/boards/ssrc/saluki-pi index 52a2bdd29b56..62fec3456ce9 160000 --- a/boards/ssrc/saluki-pi +++ b/boards/ssrc/saluki-pi @@ -1 +1 @@ -Subproject commit 52a2bdd29b566a70ae3f759103f515d6cdc036e6 +Subproject commit 62fec3456ce901267a74baaedb1df106b9884168 diff --git a/boards/ssrc/saluki-v2 b/boards/ssrc/saluki-v2 index d9a4d9e27ae7..6a74e0cc9aae 160000 --- a/boards/ssrc/saluki-v2 +++ b/boards/ssrc/saluki-v2 @@ -1 +1 @@ -Subproject commit d9a4d9e27ae7fa2e0b52c747cb1a61b5f8e2a117 +Subproject commit 6a74e0cc9aae9c4623f0e5743dc5393be807a10e diff --git a/boards/ssrc/saluki-v3 b/boards/ssrc/saluki-v3 index 54f9807f6888..f6f8a7c41860 160000 --- a/boards/ssrc/saluki-v3 +++ b/boards/ssrc/saluki-v3 @@ -1 +1 @@ -Subproject commit 54f9807f688834bca432a2f438de16fcaa87da55 +Subproject commit f6f8a7c418602a0c1798c34bff4b12324526f727 diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 313ccd7c3ce8..85fc0c043c3f 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -179,6 +179,7 @@ set(msg_files SensorSelection.msg SensorsStatus.msg SensorsStatusImu.msg + SensorTvs.msg SensorUwb.msg SystemPower.msg SystemVersion.msg diff --git a/msg/SensorTvs.msg b/msg/SensorTvs.msg new file mode 100644 index 000000000000..73aec5e988dd --- /dev/null +++ b/msg/SensorTvs.msg @@ -0,0 +1,4 @@ +uint64 timestamp # time since system start (microseconds) +uint32 device_id # unique device ID for the sensor that does not change between power cycles +float32 temperature # temperature in degrees Celsius +float32[3] voltage # voltage in mV diff --git a/platforms/nuttx/src/px4/microchip/mpfs/CMakeLists.txt b/platforms/nuttx/src/px4/microchip/mpfs/CMakeLists.txt index aae38f8c8234..a43f0734e3c2 100644 --- a/platforms/nuttx/src/px4/microchip/mpfs/CMakeLists.txt +++ b/platforms/nuttx/src/px4/microchip/mpfs/CMakeLists.txt @@ -39,3 +39,4 @@ add_subdirectory(version version) add_subdirectory(tone_alarm tone_alarm) add_subdirectory(../microchip_common/adc adc) add_subdirectory(px4io_serial) +add_subdirectory(tvs) diff --git a/platforms/nuttx/src/px4/microchip/mpfs/include/px4_arch/tvs.h b/platforms/nuttx/src/px4/microchip/mpfs/include/px4_arch/tvs.h new file mode 100644 index 000000000000..c924d2521acf --- /dev/null +++ b/platforms/nuttx/src/px4/microchip/mpfs/include/px4_arch/tvs.h @@ -0,0 +1,43 @@ +/**************************************************************************** + * + * Copyright (c) 2024 Technology Innovation Institute. 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. + * + ****************************************************************************/ + +#pragma once + +#include + +__BEGIN_DECLS + +int tvs_get_voltage(int idx, float *voltage); +int tvs_get_temperature(float *temperature); + +__END_DECLS diff --git a/platforms/nuttx/src/px4/microchip/mpfs/tvs/CMakeLists.txt b/platforms/nuttx/src/px4/microchip/mpfs/tvs/CMakeLists.txt new file mode 100644 index 000000000000..40c86a9fca15 --- /dev/null +++ b/platforms/nuttx/src/px4/microchip/mpfs/tvs/CMakeLists.txt @@ -0,0 +1,36 @@ +############################################################################ +# +# Copyright (c) 2024 Technology Innovation Institute. 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_library(arch_tvs + mpfs_tvs.c +) diff --git a/platforms/nuttx/src/px4/microchip/mpfs/tvs/mpfs_tvs.c b/platforms/nuttx/src/px4/microchip/mpfs/tvs/mpfs_tvs.c new file mode 100644 index 000000000000..10e34721f8a0 --- /dev/null +++ b/platforms/nuttx/src/px4/microchip/mpfs/tvs/mpfs_tvs.c @@ -0,0 +1,119 @@ +/**************************************************************************** + * + * Copyright (C) 2024 Technology Innovation Institute. 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 + +#include "mpfs.h" + +/* + * TVS Readout Register base address is 0x4B00_D000. + * + * Register | Offset | Width | Description + * | | (bits) | + * ------------------------------------------------------------------------------------ + * Voltage 1.0 | 0x0 | 32 | 1.0/1.05V Core voltage measurement value. Channel 0 + * Voltage 1.8V | 0x4 | 32 | 1.8V Core voltage measurement value. Channel 1 + * Voltage 2.5V | 0x8 | 32 | 2.5V Core voltage measurement value, Channel 2 + * Temperature | 0xC | 32 | Temperature measurement value. Channel 3 +*/ + +#define MPFS_TVS_VOLT_1_0 0x4B00D000 /* MPFS TVS base address */ +#define MPFS_TVS_VOLT_1_8 0x4B00D004 +#define MPFS_TVS_VOLT_2_5 0x4B00D008 +#define MPFS_TVS_TEMP 0x4B00D00C + +int tvs_get_voltage(int idx, float *voltage) +{ + uint32_t reg = 0; + uint16_t volt = 0; + + if (!voltage) { + return -EINVAL; + } + + switch (idx) { + case 0: + reg = getreg32(MPFS_TVS_VOLT_1_0); + break; + + case 1: + reg = getreg32(MPFS_TVS_VOLT_1_8); + break; + + case 2: + reg = getreg32(MPFS_TVS_VOLT_2_5); + break; + + default: + return -ENODEV; + } + + /* MPFS TVS Voltage Register: + * [31:16] Unused bits + * 15 Signed bit + * [14:3] Integer value of the voltage (mV) + * [2:0] Fractional value of the voltage + */ + + volt = reg & 0x7FFF; + *voltage = ((float)volt) / 8; + + /* signed bit */ + if (reg & (1 << 15)) { + *voltage *= -1; + } + + return 0; +} + +int tvs_get_temperature(float *temperature) +{ + uint16_t reg = 0; + + if (!temperature) { + return -EINVAL; + } + + /* MPFS TVS Temperature Register: + * [31:16] Unused bits + * 15 Reserved + * [14:4] Integer value of the temperature (K) + * [3:0] Fractional value of the temperature + */ + + reg = getreg32(MPFS_TVS_TEMP) & 0x7FFF; + *temperature = ((float)reg) / 16; + *temperature -= 273.15f; /* Kelvin -> Celcius */ + + return 0; +} diff --git a/src/drivers/tvs/CMakeLists.txt b/src/drivers/tvs/CMakeLists.txt new file mode 100644 index 000000000000..0c89e35cb03c --- /dev/null +++ b/src/drivers/tvs/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2024 Technology Innoavation Institute. 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__tvs + MAIN tvs + COMPILE_FLAGS + SRCS + tvs.cpp + DEPENDS + px4_work_queue + arch_tvs +) diff --git a/src/drivers/tvs/Kconfig b/src/drivers/tvs/Kconfig new file mode 100644 index 000000000000..29170bd6baa4 --- /dev/null +++ b/src/drivers/tvs/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_TVS + bool "tvs" + default n + ---help--- + Enable support for Temperature and Voltage Sensor diff --git a/src/drivers/tvs/tvs.cpp b/src/drivers/tvs/tvs.cpp new file mode 100644 index 000000000000..23dbc3c84750 --- /dev/null +++ b/src/drivers/tvs/tvs.cpp @@ -0,0 +1,147 @@ +/**************************************************************************** + * + * Copyright (c) 2024 Technology Innovation Institute. 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 "tvs.hpp" + +#include + +TempVoltSensor::TempVoltSensor() : + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default) +{ + _tvs_pub.advertise(); +} + +TempVoltSensor::~TempVoltSensor() +{ + _tvs_pub.unadvertise(); +} + +int TempVoltSensor::task_spawn(int argc, char *argv[]) +{ + TempVoltSensor *instance = new TempVoltSensor(); + + if (!instance) { + PX4_ERR("alloc failed"); + return PX4_ERROR; + } + + _object.store(instance); + _task_id = task_id_is_work_queue; + + instance->init(); + + return PX4_OK; +} + +int TempVoltSensor::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int TempVoltSensor::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION("Temperature and Voltage Sensor"); + PRINT_MODULE_USAGE_NAME("tvs", "driver"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +int TempVoltSensor::print_status() +{ + PX4_INFO("TVS voltage 1.0 V: %.3f mV", double(_volt_1_0)); + PX4_INFO("TVS voltage 1.8 V: %.3f mV", double(_volt_1_8)); + PX4_INFO("TVS voltage 2.5 V: %.3f mV", double(_volt_2_5)); + PX4_INFO("TVS temperature: %.3f Celcius", double(_temperature)); + return 0; +} + +uint32_t TempVoltSensor::get_dev_id() +{ + device::Device::DeviceId id; + id.devid_s.devtype = DRV_DEVTYPE_UNUSED; + id.devid_s.bus_type = device::Device::DeviceBusType_UNKNOWN; + id.devid_s.bus = 0; + id.devid_s.address = 1; + + return id.devid; +} + +void TempVoltSensor::init() +{ + ScheduleNow(); +} + +void TempVoltSensor::Run() +{ + if (should_exit()) { + ScheduleClear(); + exit_and_cleanup(); + return; + } + + publish_report(); + + // reschedule timeout + ScheduleDelayed(MEAS_INTERVAL); +} + +void TempVoltSensor::publish_report() +{ + tvs_get_voltage(0, &_volt_1_0); + tvs_get_voltage(1, &_volt_1_8); + tvs_get_voltage(2, &_volt_2_5); + tvs_get_temperature(&_temperature); + + sensor_tvs_s report{}; + + report.timestamp = hrt_absolute_time(); + report.device_id = get_dev_id(); + report.temperature = _temperature; + report.voltage[0] = _volt_1_0; + report.voltage[1] = _volt_1_8; + report.voltage[2] = _volt_2_5; + + _tvs_pub.publish(report); +} + +extern "C" __EXPORT int tvs_main(int argc, char *argv[]) +{ + return TempVoltSensor::main(argc, argv); +} + diff --git a/src/drivers/tvs/tvs.hpp b/src/drivers/tvs/tvs.hpp new file mode 100644 index 000000000000..eaa618a39313 --- /dev/null +++ b/src/drivers/tvs/tvs.hpp @@ -0,0 +1,80 @@ +/**************************************************************************** + * + * Copyright (c) 2024 Technology Innovation Institute. 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. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include + +#include + +#include +#include + +#include + +using namespace time_literals; + +class TempVoltSensor : public ModuleBase, public px4::ScheduledWorkItem +{ +public: + TempVoltSensor(); + ~TempVoltSensor(); + + /* From Modulebase requirements */ + static int task_spawn(int argc, char *argv[]); + static int custom_command(int argc, char *argv[]); + static int print_usage(const char *reason = nullptr); + + int print_status() override; + +private: + static constexpr int MEAS_INTERVAL = 500_ms; + + static uint32_t get_dev_id(); + + void init(); + + void Run() override; + + void publish_report(void); + + float _volt_1_0 = 0; + float _volt_1_8 = 0; + float _volt_2_5 = 0; + float _temperature = 0; + + uORB::PublicationMulti _tvs_pub{ORB_ID(sensor_tvs)}; +}; +