From 6870cd4d3d68941945d303b707c4b05bd5d1e6e4 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 22 Aug 2014 20:15:04 +0400 Subject: UAVCAN baro driver --- src/modules/uavcan/sensors/baro.cpp | 147 +++++++++++++++++++++++++++ src/modules/uavcan/sensors/baro.hpp | 73 +++++++++++++ src/modules/uavcan/sensors/sensor_bridge.cpp | 3 + 3 files changed, 223 insertions(+) create mode 100644 src/modules/uavcan/sensors/baro.cpp create mode 100644 src/modules/uavcan/sensors/baro.hpp (limited to 'src/modules/uavcan/sensors') diff --git a/src/modules/uavcan/sensors/baro.cpp b/src/modules/uavcan/sensors/baro.cpp new file mode 100644 index 000000000..3afcc3f1c --- /dev/null +++ b/src/modules/uavcan/sensors/baro.cpp @@ -0,0 +1,147 @@ +/**************************************************************************** + * + * Copyright (C) 2014 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. + * + ****************************************************************************/ + +/** + * @author Pavel Kirienko + */ + +#include "baro.hpp" +#include + +const char *const UavcanBarometerBridge::NAME = "baro"; + +UavcanBarometerBridge::UavcanBarometerBridge(uavcan::INode& node) : +device::CDev("uavcan_baro", "/dev/uavcan/baro"), +_sub_air_data(node) +{ +} + +UavcanBarometerBridge::~UavcanBarometerBridge() +{ + if (_class_instance > 0) { + (void)unregister_class_devname(BARO_DEVICE_PATH, _class_instance); + } +} + +int UavcanBarometerBridge::init() +{ + // Init the libuavcan subscription + int res = _sub_air_data.start(AirDataCbBinder(this, &UavcanBarometerBridge::air_data_sub_cb)); + if (res < 0) { + log("failed to start uavcan sub: %d", res); + return res; + } + + // Detect our device class + _class_instance = register_class_devname(BARO_DEVICE_PATH); + switch (_class_instance) { + case CLASS_DEVICE_PRIMARY: { + _orb_id = ORB_ID(sensor_baro0); + break; + } + case CLASS_DEVICE_SECONDARY: { + _orb_id = ORB_ID(sensor_baro1); + break; + } + default: { + log("invalid class instance: %d", _class_instance); + (void)unregister_class_devname(BARO_DEVICE_PATH, _class_instance); + return -1; + } + } + + log("inited with class instance %d", _class_instance); + return 0; +} + +int UavcanBarometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + case BAROIOCSMSLPRESSURE: { + if ((arg < 80000) || (arg > 120000)) { + return -EINVAL; + } else { + log("new msl pressure %u", _msl_pressure); + _msl_pressure = arg; + return OK; + } + } + case BAROIOCGMSLPRESSURE: { + return _msl_pressure; + } + default: { + return CDev::ioctl(filp, cmd, arg); + } + } +} + +void UavcanBarometerBridge::air_data_sub_cb(const uavcan::ReceivedDataStructure &msg) +{ + auto report = ::baro_report(); + + report.timestamp = msg.getUtcTimestamp().toUSec(); + if (report.timestamp == 0) { + report.timestamp = msg.getMonotonicTimestamp().toUSec(); + } + + report.temperature = msg.static_temperature; + report.pressure = msg.static_pressure / 100.0F; // Convert to millibar + + /* + * Altitude computation + * Refer to the MS5611 driver for details + */ + const double T1 = 15.0 + 273.15; // temperature at base height in Kelvin + const double a = -6.5 / 1000; // temperature gradient in degrees per metre + const double g = 9.80665; // gravity constant in m/s/s + const double R = 287.05; // ideal gas constant in J/kg/K + + const double p1 = _msl_pressure / 1000.0; // current pressure at MSL in kPa + const double p = double(msg.static_pressure) / 1000.0; // measured pressure in kPa + + report.altitude = (((std::pow((p / p1), (-(a * R) / g))) * T1) - T1) / a; + + /* + * Publish + */ + if (_orb_advert >= 0) { + orb_publish(_orb_id, _orb_advert, &report); + } else { + _orb_advert = orb_advertise(_orb_id, &report); + if (_orb_advert < 0) { + log("ADVERT FAIL"); + } else { + log("advertised"); + } + } +} diff --git a/src/modules/uavcan/sensors/baro.hpp b/src/modules/uavcan/sensors/baro.hpp new file mode 100644 index 000000000..f6aa01216 --- /dev/null +++ b/src/modules/uavcan/sensors/baro.hpp @@ -0,0 +1,73 @@ +/**************************************************************************** + * + * Copyright (C) 2014 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. + * + ****************************************************************************/ + +/** + * @author Pavel Kirienko + */ + +#pragma once + +#include "sensor_bridge.hpp" +#include +#include + +#include + +class UavcanBarometerBridge : public IUavcanSensorBridge, public device::CDev +{ +public: + static const char *const NAME; + + UavcanBarometerBridge(uavcan::INode& node); + ~UavcanBarometerBridge() override; + + const char *get_name() const override { return NAME; } + + int init() override; + +private: + int ioctl(struct file *filp, int cmd, unsigned long arg) override; + + void air_data_sub_cb(const uavcan::ReceivedDataStructure &msg); + + typedef uavcan::MethodBinder&)> + AirDataCbBinder; + + uavcan::Subscriber _sub_air_data; + unsigned _msl_pressure = 101325; + orb_id_t _orb_id = nullptr; + orb_advert_t _orb_advert = -1; + int _class_instance = -1; +}; diff --git a/src/modules/uavcan/sensors/sensor_bridge.cpp b/src/modules/uavcan/sensors/sensor_bridge.cpp index 2bd662d5c..f826c8fd2 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.cpp +++ b/src/modules/uavcan/sensors/sensor_bridge.cpp @@ -38,6 +38,7 @@ #include "sensor_bridge.hpp" #include "gnss.hpp" #include "mag.hpp" +#include "baro.hpp" IUavcanSensorBridge* IUavcanSensorBridge::make(uavcan::INode &node, const char *bridge_name) { @@ -45,6 +46,8 @@ IUavcanSensorBridge* IUavcanSensorBridge::make(uavcan::INode &node, const char * return new UavcanGnssBridge(node); } else if (!std::strncmp(UavcanMagnetometerBridge::NAME, bridge_name, MAX_NAME_LEN)) { return new UavcanMagnetometerBridge(node); + } else if (!std::strncmp(UavcanBarometerBridge::NAME, bridge_name, MAX_NAME_LEN)) { + return new UavcanBarometerBridge(node); } else { return nullptr; } -- cgit v1.2.3