From 213c6a19232633e172f2652d104c5c2b4f4ce3f5 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 30 Dec 2019 18:04:19 -0500 Subject: [PATCH] deprecate df_ms5611_wrapper and df_ms5607_wrapper (replaced with in tree ms5611 driver) --- boards/aerotenna/ocpoc/default.cmake | 1 - boards/emlid/navio2/cross.cmake | 1 - boards/emlid/navio2/native.cmake | 1 - boards/parrot/bebop/default.cmake | 2 +- posix-configs/bebop/px4.config | 2 +- posix-configs/ocpoc/px4.config | 2 +- posix-configs/rpi/px4.config | 2 +- posix-configs/rpi/px4_fw.config | 2 +- posix-configs/rpi/px4_no_shield.config | 2 +- posix-configs/rpi/px4_test.config | 2 +- .../df_ms5607_wrapper/CMakeLists.txt | 45 --- .../df_ms5607_wrapper/df_ms5607_wrapper.cpp | 294 ------------------ .../df_ms5611_wrapper/CMakeLists.txt | 46 --- .../df_ms5611_wrapper/df_ms5611_wrapper.cpp | 294 ------------------ 14 files changed, 7 insertions(+), 689 deletions(-) delete mode 100644 src/drivers/driver_framework_wrapper/df_ms5607_wrapper/CMakeLists.txt delete mode 100644 src/drivers/driver_framework_wrapper/df_ms5607_wrapper/df_ms5607_wrapper.cpp delete mode 100644 src/drivers/driver_framework_wrapper/df_ms5611_wrapper/CMakeLists.txt delete mode 100644 src/drivers/driver_framework_wrapper/df_ms5611_wrapper/df_ms5611_wrapper.cpp diff --git a/boards/aerotenna/ocpoc/default.cmake b/boards/aerotenna/ocpoc/default.cmake index 7ea7310076..180d1d750f 100644 --- a/boards/aerotenna/ocpoc/default.cmake +++ b/boards/aerotenna/ocpoc/default.cmake @@ -28,7 +28,6 @@ px4_add_board( DF_DRIVERS # NOTE: DriverFramework is migrating to intree PX4 drivers hmc5883 mpu9250 - ms5611 MODULES airspeed_selector attitude_estimator_q diff --git a/boards/emlid/navio2/cross.cmake b/boards/emlid/navio2/cross.cmake index 31bbf30bf5..84893898e9 100644 --- a/boards/emlid/navio2/cross.cmake +++ b/boards/emlid/navio2/cross.cmake @@ -29,7 +29,6 @@ px4_add_board( isl29501 lsm9ds1 mpu9250 - ms5611 trone MODULES airspeed_selector diff --git a/boards/emlid/navio2/native.cmake b/boards/emlid/navio2/native.cmake index 12e3b9bc74..43ce85f01c 100644 --- a/boards/emlid/navio2/native.cmake +++ b/boards/emlid/navio2/native.cmake @@ -27,7 +27,6 @@ px4_add_board( isl29501 lsm9ds1 mpu9250 - ms5611 trone MODULES airspeed_selector diff --git a/boards/parrot/bebop/default.cmake b/boards/parrot/bebop/default.cmake index 0a9ecb0644..aeb76118b2 100644 --- a/boards/parrot/bebop/default.cmake +++ b/boards/parrot/bebop/default.cmake @@ -7,12 +7,12 @@ px4_add_board( TOOLCHAIN arm-linux-gnueabihf DRIVERS + barometer/ms5611 gps linux_sbus pwm_out_sim DF_DRIVERS # NOTE: DriverFramework is migrating to intree PX4 drivers - ms5607 mpu6050 ak8963 bebop_bus diff --git a/posix-configs/bebop/px4.config b/posix-configs/bebop/px4.config index da56c51282..32b0dce2f9 100644 --- a/posix-configs/bebop/px4.config +++ b/posix-configs/bebop/px4.config @@ -36,7 +36,7 @@ param set MC_PITCHRATE_I 0.8 param set MC_PITCHRATE_D 0.0 param set MC_PR_INT_LIM 0.3 -df_ms5607_wrapper start +ms5611 -T 5607 -I start df_mpu6050_wrapper start -R 8 df_ak8963_wrapper start -R 32 # Rangefinder disabled for now. It was found to cause delays of more than 10ms diff --git a/posix-configs/ocpoc/px4.config b/posix-configs/ocpoc/px4.config index b707dd63df..e4d296a2e0 100644 --- a/posix-configs/ocpoc/px4.config +++ b/posix-configs/ocpoc/px4.config @@ -17,7 +17,7 @@ param set MAV_SYS_ID 1 df_mpu9250_wrapper start_without_mag -R 10 df_hmc5883_wrapper start -D /dev/i2c-4 -df_ms5611_wrapper start +ms5611 start rgbled start -b 1 ocpoc_adc start gps start -d /dev/ttyS3 -s diff --git a/posix-configs/rpi/px4.config b/posix-configs/rpi/px4.config index 86003af76d..97feab349c 100644 --- a/posix-configs/rpi/px4.config +++ b/posix-configs/rpi/px4.config @@ -20,7 +20,7 @@ dataman start df_lsm9ds1_wrapper start -R 4 #df_mpu9250_wrapper start -R 10 #df_hmc5883_wrapper start -df_ms5611_wrapper start +ms5611 start navio_rgbled start navio_adc start gps start -d /dev/spidev0.0 -i spi -p ubx diff --git a/posix-configs/rpi/px4_fw.config b/posix-configs/rpi/px4_fw.config index d6a14d471a..7089d06629 100644 --- a/posix-configs/rpi/px4_fw.config +++ b/posix-configs/rpi/px4_fw.config @@ -19,7 +19,7 @@ param set BAT_A_PER_V 15.391030303 dataman start df_lsm9ds1_wrapper start -R 4 -df_ms5611_wrapper start +ms5611 start navio_rgbled start navio_adc start gps start -d /dev/spidev0.0 -i spi -p ubx diff --git a/posix-configs/rpi/px4_no_shield.config b/posix-configs/rpi/px4_no_shield.config index 4afb0ae78b..5fa7c63317 100644 --- a/posix-configs/rpi/px4_no_shield.config +++ b/posix-configs/rpi/px4_no_shield.config @@ -15,7 +15,7 @@ param set MAV_BROADCAST 1 sleep 1 param set MAV_TYPE 2 df_mpu9250_wrapper start -R 10 -df_ms5611_wrapper start +ms5611 start gps start -d /dev/ttyACM0 -i uart -p ubx rc_update start sensors start diff --git a/posix-configs/rpi/px4_test.config b/posix-configs/rpi/px4_test.config index 6aabc6f2e2..609d737ba9 100644 --- a/posix-configs/rpi/px4_test.config +++ b/posix-configs/rpi/px4_test.config @@ -20,7 +20,7 @@ dataman start df_lsm9ds1_wrapper start -R 4 #df_mpu9250_wrapper start -R 10 #df_hmc5883_wrapper start -df_ms5611_wrapper start +ms5611 start navio_rgbled start navio_adc start gps start -d /dev/spidev0.0 -i spi -p ubx diff --git a/src/drivers/driver_framework_wrapper/df_ms5607_wrapper/CMakeLists.txt b/src/drivers/driver_framework_wrapper/df_ms5607_wrapper/CMakeLists.txt deleted file mode 100644 index f0afda81fa..0000000000 --- a/src/drivers/driver_framework_wrapper/df_ms5607_wrapper/CMakeLists.txt +++ /dev/null @@ -1,45 +0,0 @@ -############################################################################ -# -# Copyright (c) 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. -# -############################################################################ - -include_directories(../../../lib/DriverFramework/drivers) - -px4_add_module( - MODULE platforms__posix__drivers__df_ms5607_wrapper - MAIN df_ms5607_wrapper - SRCS - df_ms5607_wrapper.cpp - DEPENDS - git_driverframework - df_driver_framework - df_ms5607 - ) diff --git a/src/drivers/driver_framework_wrapper/df_ms5607_wrapper/df_ms5607_wrapper.cpp b/src/drivers/driver_framework_wrapper/df_ms5607_wrapper/df_ms5607_wrapper.cpp deleted file mode 100644 index 80b5c0148e..0000000000 --- a/src/drivers/driver_framework_wrapper/df_ms5607_wrapper/df_ms5607_wrapper.cpp +++ /dev/null @@ -1,294 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 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 df_ms5607_wrapper.cpp - * Lightweight driver to access the MS5607 of the DriverFramework. - */ - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include -#include - -#include - -#include -#include - - -extern "C" { __EXPORT int df_ms5607_wrapper_main(int argc, char *argv[]); } - -using namespace DriverFramework; - - -class DfMS5607Wrapper : public MS5607 -{ -public: - DfMS5607Wrapper(); - ~DfMS5607Wrapper(); - - - /** - * Start automatic measurement. - * - * @return 0 on success - */ - int start(); - - /** - * Stop automatic measurement. - * - * @return 0 on success - */ - int stop(); - -private: - int _publish(struct baro_sensor_data &data); - - orb_advert_t _baro_topic; - - int _baro_orb_class_instance; - - perf_counter_t _baro_sample_perf; - -}; - -DfMS5607Wrapper::DfMS5607Wrapper() : - MS5607(BARO_DEVICE_PATH), - _baro_topic(nullptr), - _baro_orb_class_instance(-1), - _baro_sample_perf(perf_alloc(PC_ELAPSED, "df_baro_read")) -{ -} - -DfMS5607Wrapper::~DfMS5607Wrapper() -{ - perf_free(_baro_sample_perf); -} - -int DfMS5607Wrapper::start() -{ - /* Init device and start sensor. */ - int ret = init(); - - if (ret != 0) { - PX4_ERR("MS5607 init fail: %d", ret); - return ret; - } - - ret = MS5607::start(); - - if (ret != 0) { - PX4_ERR("MS5607 start fail: %d", ret); - return ret; - } - - return 0; -} - -int DfMS5607Wrapper::stop() -{ - /* Stop sensor. */ - int ret = MS5607::stop(); - - if (ret != 0) { - PX4_ERR("MS5607 stop fail: %d", ret); - return ret; - } - - return 0; -} - -int DfMS5607Wrapper::_publish(struct baro_sensor_data &data) -{ - perf_begin(_baro_sample_perf); - - sensor_baro_s baro_report{}; - baro_report.timestamp = hrt_absolute_time(); - - baro_report.pressure = data.pressure_pa / 100.0f; // convert to mbar - baro_report.temperature = data.temperature_c; - baro_report.error_count = data.error_counter; - baro_report.device_id = m_id.dev_id; - - // TODO: when is this ever blocked? - if (!(m_pub_blocked)) { - - if (_baro_topic == nullptr) { - _baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &baro_report, - &_baro_orb_class_instance, ORB_PRIO_DEFAULT); - - } else { - orb_publish(ORB_ID(sensor_baro), _baro_topic, &baro_report); - } - } - - perf_end(_baro_sample_perf); - - return 0; -}; - - -namespace df_ms5607_wrapper -{ - -DfMS5607Wrapper *g_dev = nullptr; - -int start(/* enum Rotation rotation */); -int stop(); -int info(); -void usage(); - -int start(/*enum Rotation rotation*/) -{ - g_dev = new DfMS5607Wrapper(/*rotation*/); - - if (g_dev == nullptr) { - PX4_ERR("failed instantiating DfMS5607Wrapper object"); - return -1; - } - - int ret = g_dev->start(); - - if (ret != 0) { - PX4_ERR("DfMS5607Wrapper start failed"); - return ret; - } - - // Open the IMU sensor - DevHandle h; - DevMgr::getHandle(BARO_DEVICE_PATH, h); - - if (!h.isValid()) { - DF_LOG_INFO("Error: unable to obtain a valid handle for the receiver at: %s (%d)", - BARO_DEVICE_PATH, h.getError()); - return -1; - } - - DevMgr::releaseHandle(h); - - return 0; -} - -int stop() -{ - if (g_dev == nullptr) { - PX4_ERR("driver not running"); - return 1; - } - - int ret = g_dev->stop(); - - if (ret != 0) { - PX4_ERR("driver could not be stopped"); - return ret; - } - - delete g_dev; - g_dev = nullptr; - return 0; -} - -/** - * Print a little info about the driver. - */ -int -info() -{ - if (g_dev == nullptr) { - PX4_ERR("driver not running"); - return 1; - } - - PX4_DEBUG("state @ %p", g_dev); - - return 0; -} - -void -usage() -{ - PX4_WARN("Usage: df_ms5607_wrapper 'start', 'info', 'stop'"); -} - -} // namespace df_ms5607_wrapper - - -int -df_ms5607_wrapper_main(int argc, char *argv[]) -{ - int ret = 0; - int myoptind = 1; - - if (argc <= 1) { - df_ms5607_wrapper::usage(); - return 1; - } - - const char *verb = argv[myoptind]; - - - if (!strcmp(verb, "start")) { - ret = df_ms5607_wrapper::start(); - } - - else if (!strcmp(verb, "stop")) { - ret = df_ms5607_wrapper::stop(); - } - - else if (!strcmp(verb, "info")) { - ret = df_ms5607_wrapper::info(); - } - - else { - df_ms5607_wrapper::usage(); - return 1; - } - - return ret; -} diff --git a/src/drivers/driver_framework_wrapper/df_ms5611_wrapper/CMakeLists.txt b/src/drivers/driver_framework_wrapper/df_ms5611_wrapper/CMakeLists.txt deleted file mode 100644 index e18fd85679..0000000000 --- a/src/drivers/driver_framework_wrapper/df_ms5611_wrapper/CMakeLists.txt +++ /dev/null @@ -1,46 +0,0 @@ -############################################################################ -# -# Copyright (c) 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. -# -############################################################################ - -include_directories(../../../lib/DriverFramework/drivers) - -px4_add_module( - MODULE platforms__posix__drivers__df_ms5611_wrapper - MAIN df_ms5611_wrapper - SRCS - df_ms5611_wrapper.cpp - DEPENDS - git_driverframework - df_driver_framework - df_ms5611 - ) - diff --git a/src/drivers/driver_framework_wrapper/df_ms5611_wrapper/df_ms5611_wrapper.cpp b/src/drivers/driver_framework_wrapper/df_ms5611_wrapper/df_ms5611_wrapper.cpp deleted file mode 100644 index 15b76256b8..0000000000 --- a/src/drivers/driver_framework_wrapper/df_ms5611_wrapper/df_ms5611_wrapper.cpp +++ /dev/null @@ -1,294 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 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 df_ms5611_wrapper.cpp - * Lightweight driver to access the MS5611 of the DriverFramework. - */ - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include -#include - -#include - -#include -#include - - -extern "C" { __EXPORT int df_ms5611_wrapper_main(int argc, char *argv[]); } - -using namespace DriverFramework; - - -class DfMS5611Wrapper : public MS5611 -{ -public: - DfMS5611Wrapper(); - ~DfMS5611Wrapper(); - - - /** - * Start automatic measurement. - * - * @return 0 on success - */ - int start(); - - /** - * Stop automatic measurement. - * - * @return 0 on success - */ - int stop(); - -private: - int _publish(struct baro_sensor_data &data); - - orb_advert_t _baro_topic; - - int _baro_orb_class_instance; - - perf_counter_t _baro_sample_perf; - -}; - -DfMS5611Wrapper::DfMS5611Wrapper() : - MS5611(BARO_DEVICE_PATH), - _baro_topic(nullptr), - _baro_orb_class_instance(-1), - _baro_sample_perf(perf_alloc(PC_ELAPSED, "df_baro_read")) -{ -} - -DfMS5611Wrapper::~DfMS5611Wrapper() -{ - perf_free(_baro_sample_perf); -} - -int DfMS5611Wrapper::start() -{ - /* Init device and start sensor. */ - int ret = init(); - - if (ret != 0) { - PX4_ERR("MS5611 init fail: %d", ret); - return ret; - } - - ret = MS5611::start(); - - if (ret != 0) { - PX4_ERR("MS5611 start fail: %d", ret); - return ret; - } - - return 0; -} - -int DfMS5611Wrapper::stop() -{ - /* Stop sensor. */ - int ret = MS5611::stop(); - - if (ret != 0) { - PX4_ERR("MS5611 stop fail: %d", ret); - return ret; - } - - return 0; -} - -int DfMS5611Wrapper::_publish(struct baro_sensor_data &data) -{ - perf_begin(_baro_sample_perf); - - sensor_baro_s baro_report{}; - baro_report.timestamp = hrt_absolute_time(); - - baro_report.pressure = data.pressure_pa / 100.0f; // convert to mbar - baro_report.temperature = data.temperature_c; - baro_report.error_count = data.error_counter; - baro_report.device_id = m_id.dev_id; - - // TODO: when is this ever blocked? - if (!(m_pub_blocked)) { - - if (_baro_topic == nullptr) { - _baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &baro_report, - &_baro_orb_class_instance, ORB_PRIO_DEFAULT); - - } else { - orb_publish(ORB_ID(sensor_baro), _baro_topic, &baro_report); - } - } - - perf_end(_baro_sample_perf); - - return 0; -}; - - -namespace df_ms5611_wrapper -{ - -DfMS5611Wrapper *g_dev = nullptr; - -int start(/* enum Rotation rotation */); -int stop(); -int info(); -void usage(); - -int start(/*enum Rotation rotation*/) -{ - g_dev = new DfMS5611Wrapper(/*rotation*/); - - if (g_dev == nullptr) { - PX4_ERR("failed instantiating DfMS5611Wrapper object"); - return -1; - } - - int ret = g_dev->start(); - - if (ret != 0) { - PX4_ERR("DfMS5611Wrapper start failed"); - return ret; - } - - // Open the IMU sensor - DevHandle h; - DevMgr::getHandle(BARO_DEVICE_PATH, h); - - if (!h.isValid()) { - DF_LOG_INFO("Error: unable to obtain a valid handle for the receiver at: %s (%d)", - BARO_DEVICE_PATH, h.getError()); - return -1; - } - - DevMgr::releaseHandle(h); - - return 0; -} - -int stop() -{ - if (g_dev == nullptr) { - PX4_ERR("driver not running"); - return 1; - } - - int ret = g_dev->stop(); - - if (ret != 0) { - PX4_ERR("driver could not be stopped"); - return ret; - } - - delete g_dev; - g_dev = nullptr; - return 0; -} - -/** - * Print a little info about the driver. - */ -int -info() -{ - if (g_dev == nullptr) { - PX4_ERR("driver not running"); - return 1; - } - - PX4_DEBUG("state @ %p", g_dev); - - return 0; -} - -void -usage() -{ - PX4_WARN("Usage: df_ms5611_wrapper 'start', 'info', 'stop'"); -} - -} // namespace df_ms5611_wrapper - - -int -df_ms5611_wrapper_main(int argc, char *argv[]) -{ - int ret = 0; - int myoptind = 1; - - if (argc <= 1) { - df_ms5611_wrapper::usage(); - return 1; - } - - const char *verb = argv[myoptind]; - - - if (!strcmp(verb, "start")) { - ret = df_ms5611_wrapper::start(); - } - - else if (!strcmp(verb, "stop")) { - ret = df_ms5611_wrapper::stop(); - } - - else if (!strcmp(verb, "info")) { - ret = df_ms5611_wrapper::info(); - } - - else { - df_ms5611_wrapper::usage(); - return 1; - } - - return ret; -}