From c6cece52d3e9ba37cfa3868c896c845f2aa4cd36 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 7 Jun 2017 11:29:38 +0200 Subject: [PATCH] Sensirion SDP3X airspeed / differential pressure sensor driver --- cmake/configs/nuttx_px4fmu-v2_default.cmake | 1 + src/drivers/sdp3x_airspeed/CMakeLists.txt | 44 +++ src/drivers/sdp3x_airspeed/SDP3X.cpp | 208 ++++++++++++++ src/drivers/sdp3x_airspeed/SDP3X.hpp | 110 ++++++++ src/drivers/sdp3x_airspeed/SDP3X_main.cpp | 289 ++++++++++++++++++++ 5 files changed, 652 insertions(+) create mode 100644 src/drivers/sdp3x_airspeed/CMakeLists.txt create mode 100644 src/drivers/sdp3x_airspeed/SDP3X.cpp create mode 100644 src/drivers/sdp3x_airspeed/SDP3X.hpp create mode 100644 src/drivers/sdp3x_airspeed/SDP3X_main.cpp diff --git a/cmake/configs/nuttx_px4fmu-v2_default.cmake b/cmake/configs/nuttx_px4fmu-v2_default.cmake index 6de846f567..5ed26e5a30 100644 --- a/cmake/configs/nuttx_px4fmu-v2_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v2_default.cmake @@ -37,6 +37,7 @@ set(config_module_list #drivers/hott/hott_sensors #drivers/blinkm drivers/airspeed + drivers/sdp3x_airspeed drivers/ets_airspeed drivers/meas_airspeed drivers/frsky_telemetry diff --git a/src/drivers/sdp3x_airspeed/CMakeLists.txt b/src/drivers/sdp3x_airspeed/CMakeLists.txt new file mode 100644 index 0000000000..f67ec91802 --- /dev/null +++ b/src/drivers/sdp3x_airspeed/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2017 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__sdp3x_airspeed + MAIN sdp3x_airspeed + STACK_MAIN 1200 + COMPILE_FLAGS + SRCS + SDP3X.cpp + SDP3X_main.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/sdp3x_airspeed/SDP3X.cpp b/src/drivers/sdp3x_airspeed/SDP3X.cpp new file mode 100644 index 0000000000..d44f8a9123 --- /dev/null +++ b/src/drivers/sdp3x_airspeed/SDP3X.cpp @@ -0,0 +1,208 @@ +/**************************************************************************** + * + * Copyright (c) 2017 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 "SDP3X.hpp" + +/** + * @file SDP3X.hpp + * + * Driver for Sensirion SDP3X Differential Pressure Sensor + * + */ + +int +SDP3X::probe() +{ + return !init_sdp3x(); +} + +int SDP3X::write_command(uint16_t command) +{ + uint8_t cmd[2]; + cmd[0] = static_cast(command >> 8); + cmd[1] = static_cast(command & 0xff); + return transfer(&cmd[0], 2, nullptr, 0); +} + +bool +SDP3X::init_sdp3x() +{ + // step 1 - reset on broadcast + uint16_t prev_addr = get_address(); + set_address(SDP3X_RESET_ADDR); + uint8_t reset_cmd = SDP3X_RESET_CMD; + int ret = transfer(&reset_cmd, 1, nullptr, 0); + set_address(prev_addr); + + if (ret != PX4_OK) { + perf_count(_comms_errors); + PX4_ERR("reset failed"); + return false; + } + + // wait until sensor is ready + usleep(20000); + + // step 2 - configure + ret = write_command(SDP3X_CONT_MEAS_AVG_MODE); + + if (ret != PX4_OK) { + perf_count(_comms_errors); + PX4_ERR("config failed"); + return false; + } + + usleep(10000); + + // step 3 - get scale + uint8_t val[9]; + ret = transfer(nullptr, 0, &val[0], 9); + + if (ret != PX4_OK) { + perf_count(_comms_errors); + PX4_ERR("get scale failed"); + return false; + } + + // Check the CRC + if (!crc(&val[0], 2, val[2]) || !crc(&val[3], 2, val[5]) || !crc(&val[6], 2, val[8])) { + perf_count(_comms_errors); + return false; + } + + _scale = (((uint16_t)val[6]) << 8) | val[7]; + + return true; +} + +int +SDP3X::collect() +{ + perf_begin(_sample_perf); + + // read 6 bytes from the sensor + uint8_t val[6]; + int ret = transfer(nullptr, 0, &val[0], 6); + + if (ret != PX4_OK) { + perf_count(_comms_errors); + return ret; + } + + // Check the CRC + if (!crc(&val[0], 2, val[2]) || !crc(&val[3], 2, val[5])) { + perf_count(_comms_errors); + return EAGAIN; + + } else { + ret = 0; + } + + int16_t P = (((uint16_t)val[0]) << 8) | val[1]; + int16_t temp = (((uint16_t)val[3]) << 8) | val[4]; + + float diff_press_pa_raw = static_cast(P) / static_cast(_scale); + float temperature_c = temp / static_cast(SDP3X_SCALE_TEMPERATURE); + + // the raw value still should be compensated for the known offset + diff_press_pa_raw -= _diff_pres_offset; + + differential_pressure_s report; + + // track maximum differential pressure measured (so we can work out top speed). + if (diff_press_pa_raw > _max_differential_pressure_pa) { + _max_differential_pressure_pa = diff_press_pa_raw; + } + + report.timestamp = hrt_absolute_time(); + report.error_count = perf_event_count(_comms_errors); + report.temperature = temperature_c; + report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa_raw); + report.differential_pressure_raw_pa = diff_press_pa_raw; + report.max_differential_pressure_pa = _max_differential_pressure_pa; + + if (_airspeed_pub != nullptr && !(_pub_blocked)) { + // publish it + orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report); + } + + new_report(report); + + // notify anyone waiting for data + poll_notify(POLLIN); + + ret = OK; + + perf_end(_sample_perf); + + return ret; +} + +void +SDP3X::cycle() +{ + int ret = PX4_ERROR; + + // measurement phase + ret = collect(); + + if (PX4_OK != ret) { + _sensor_ok = false; + DEVICE_DEBUG("measure error"); + } + + // schedule a fresh cycle call when the measurement is done + work_queue(HPWORK, &_work, (worker_t)&Airspeed::cycle_trampoline, this, USEC2TICK(CONVERSION_INTERVAL)); +} + +bool SDP3X::crc(const uint8_t data[], unsigned size, uint8_t checksum) +{ + uint8_t crc = 0xff; + + // calculate 8-bit checksum with polynomial 0x31 (x^8 + x^5 + x^4 + 1) + for (unsigned i = 0; i < size; i++) { + crc ^= (data[i]); + + for (int bit = 8; bit > 0; --bit) { + if (crc & 0x80) { + crc = (crc << 1) ^ 0x31; + + } else { + crc = (crc << 1); + } + } + } + + // verify checksum + return (crc == checksum); +} diff --git a/src/drivers/sdp3x_airspeed/SDP3X.hpp b/src/drivers/sdp3x_airspeed/SDP3X.hpp new file mode 100644 index 0000000000..2d0ffcf2c8 --- /dev/null +++ b/src/drivers/sdp3x_airspeed/SDP3X.hpp @@ -0,0 +1,110 @@ +/**************************************************************************** + * + * Copyright (c) 2017 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 SDP3X.hpp + * + * Driver for Sensirion SDP3X Differential Pressure Sensor + * + * Datasheet: https://www.sensirion.com/fileadmin/user_upload/customers/sensirion/Dokumente/8_Differential_Pressure/Sensirion_Differential_Pressure_Sensors_SDP3x_Digital_Datasheet_V0.8.pdf + */ + +#ifndef DRIVERS_SDP3X_AIRSPEED_HPP_ +#define DRIVERS_SDP3X_AIRSPEED_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define I2C_ADDRESS_1_SDP3X 0x21 +#define I2C_ADDRESS_2_SDP3X 0x22 +#define I2C_ADDRESS_3_SDP3X 0x23 + +#define SDP3X_SCALE_TEMPERATURE 200.0f +#define SDP3X_RESET_ADDR 0x00 +#define SDP3X_RESET_CMD 0x06 +#define SDP3X_CONT_MEAS_AVG_MODE 0x3615 + +#define PATH_SDP3X "/dev/sdp3x" + +// Measurement rate is 20Hz +#define SPD3X_MEAS_RATE 100 +#define SDP3X_MEAS_DRIVER_FILTER_FREQ 1.5f +#define CONVERSION_INTERVAL (1000000 / SPD3X_MEAS_RATE) /* microseconds */ + +class SDP3X : public Airspeed +{ +public: + SDP3X(int bus, int address = I2C_ADDRESS_1_SDP3X, const char *path = PATH_SDP3X) : + Airspeed(bus, address, CONVERSION_INTERVAL, path) + { + } + +private: + + /** + * Perform a poll cycle; collect from the previous measurement + * and start a new one. + */ + void cycle() override; + int measure() override { return 0; } + int collect() override; + int probe() override; + + math::LowPassFilter2p _filter{SPD3X_MEAS_RATE, SDP3X_MEAS_DRIVER_FILTER_FREQ}; + + bool init_sdp3x(); + + /** + * Calculate the CRC8 for the sensor payload data + */ + bool crc(const uint8_t data[], unsigned size, uint8_t checksum); + + /** + * Write a command in Sensirion specific logic + */ + int write_command(uint16_t command); + + bool _inited{false}; + int16_t _scale{0}; +}; + +#endif /* DRIVERS_SDP3X_AIRSPEED_HPP_ */ diff --git a/src/drivers/sdp3x_airspeed/SDP3X_main.cpp b/src/drivers/sdp3x_airspeed/SDP3X_main.cpp new file mode 100644 index 0000000000..11c6b731da --- /dev/null +++ b/src/drivers/sdp3x_airspeed/SDP3X_main.cpp @@ -0,0 +1,289 @@ +/**************************************************************************** + * + * Copyright (c) 2017 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 "SDP3X.hpp" + +// Driver 'main' command. +extern "C" __EXPORT int sdp3x_airspeed_main(int argc, char *argv[]); + +// Local functions in support of the shell command. +namespace sdp3x_airspeed +{ +SDP3X *g_dev = nullptr; + +void start(int i2c_bus); +void stop(); +void test(); +void reset(); +void info(); + +// Start the driver. +// This function call only returns once the driver is up and running +// or failed to detect the sensor. +void +start(int i2c_bus) +{ + int fd = -1; + + if (g_dev != nullptr) { + errx(1, "already started"); + } + + g_dev = new SDP3X(i2c_bus, I2C_ADDRESS_1_SDP3X, PATH_SDP3X); + + /* check if the MS4525DO was instantiated */ + if (g_dev == nullptr) { + goto fail; + } + + /* try the next SDP3XDSO if init fails */ + if (OK != g_dev->Airspeed::init()) { + delete g_dev; + + PX4_WARN("trying SDP3X 2"); + + g_dev = new SDP3X(i2c_bus, I2C_ADDRESS_2_SDP3X, PATH_SDP3X); + + /* check if the SDP3XDSO was instantiated */ + if (g_dev == nullptr) { + PX4_WARN("SDP3X was not instantiated"); + goto fail; + } + + /* both versions failed if the init for the SDP3XDSO fails, give up */ + if (OK != g_dev->Airspeed::init()) { + PX4_WARN("SDP3X init fail"); + goto fail; + } + } + + /* set the poll rate to default, starts automatic data collection */ + fd = open(PATH_SDP3X, O_RDONLY); + + if (fd < 0) { + goto fail; + } + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + goto fail; + } + + exit(0); + +fail: + + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + } + + PX4_WARN("no SDP3X airspeed sensor connected"); + exit(1); +} + +// stop the driver +void stop() +{ + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + + } else { + errx(1, "driver not running"); + } + + exit(0); +} + +// perform some basic functional tests on the driver; +// make sure we can collect data from the sensor in polled +// and automatic modes. +void test() +{ + int fd = open(PATH_SDP3X, O_RDONLY); + + if (fd < 0) { + PX4_WARN("%s open failed (try 'sdp3x_airspeed start' if the driver is not running", PATH_SDP3X); + exit(1); + } + + // do a simple demand read + differential_pressure_s report; + ssize_t sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) { + PX4_WARN("immediate read failed"); + exit(1); + } + + PX4_WARN("single read"); + PX4_WARN("diff pressure: %d pa", (int)report.differential_pressure_filtered_pa); + + /* start the sensor polling at 2Hz */ + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { + PX4_WARN("failed to set 2Hz poll rate"); + exit(1); + } + + /* read the sensor 5x and report each value */ + for (unsigned i = 0; i < 5; i++) { + struct pollfd fds; + + /* wait for data to be ready */ + fds.fd = fd; + fds.events = POLLIN; + int ret = poll(&fds, 1, 2000); + + if (ret != 1) { + errx(1, "timed out"); + } + + /* now go get it */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) { + err(1, "periodic read failed"); + } + + PX4_WARN("periodic read %u", i); + PX4_WARN("diff pressure: %d pa", (int)report.differential_pressure_filtered_pa); + PX4_WARN("temperature: %d C (0x%02x)", (int)report.temperature, (unsigned) report.temperature); + } + + /* reset the sensor polling to its default rate */ + if (PX4_OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) { + PX4_WARN("failed to set default rate"); + exit(1); + } + + exit(0); +} + +// reset the driver +void reset() +{ + int fd = open(PATH_SDP3X, O_RDONLY); + + if (fd < 0) { + err(1, "failed "); + } + + if (ioctl(fd, SENSORIOCRESET, 0) < 0) { + err(1, "driver reset failed"); + } + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + err(1, "driver poll restart failed"); + } + + exit(0); +} + +// print a little info about the driver +void +info() +{ + if (g_dev == nullptr) { + errx(1, "driver not running"); + } + + printf("state @ %p\n", g_dev); + g_dev->print_info(); + + exit(0); +} + +} // namespace sdp3x_airspeed + + +static void +sdp3x_airspeed_usage() +{ + PX4_WARN("usage: sdp3x_airspeed command [options]"); + PX4_WARN("options:"); + PX4_WARN("\t-b --bus i2cbus (%d)", PX4_I2C_BUS_DEFAULT); + PX4_WARN("command:"); + PX4_WARN("\tstart|stop|reset|test|info"); +} + +int +sdp3x_airspeed_main(int argc, char *argv[]) +{ + int i2c_bus = PX4_I2C_BUS_DEFAULT; + + for (int i = 1; i < argc; i++) { + if (strcmp(argv[i], "-b") == 0 || strcmp(argv[i], "--bus") == 0) { + if (argc > i + 1) { + i2c_bus = atoi(argv[i + 1]); + } + } + } + + /* + * Start/load the driver. + */ + if (!strcmp(argv[1], "start")) { + sdp3x_airspeed::start(i2c_bus); + } + + /* + * Stop the driver + */ + if (!strcmp(argv[1], "stop")) { + sdp3x_airspeed::stop(); + } + + /* + * Test the driver/device. + */ + if (!strcmp(argv[1], "test")) { + sdp3x_airspeed::test(); + } + + /* + * Reset the driver. + */ + if (!strcmp(argv[1], "reset")) { + sdp3x_airspeed::reset(); + } + + /* + * Print driver information. + */ + if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) { + sdp3x_airspeed::info(); + } + + sdp3x_airspeed_usage(); + exit(0); +}