dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Sensirion SDP3X airspeed / differential pressure sensor driver

This commit is contained in:
Lorenz Meier 2017-06-07 11:29:38 +02:00
parent 5ee79648b7
commit c6cece52d3
5 changed files with 652 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
cmake/configs/nuttx_px4fmu-v2_default.cmake
View File

@ -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

44
src/drivers/sdp3x_airspeed/CMakeLists.txt Normal file
View File

@ -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 :

208
src/drivers/sdp3x_airspeed/SDP3X.cpp Normal file
View File

@ -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<uint8_t>(command >> 8);
cmd[1] = static_cast<uint8_t>(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<float>(P) / static_cast<float>(_scale);
float temperature_c = temp / static_cast<float>(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);
}

110
src/drivers/sdp3x_airspeed/SDP3X.hpp Normal file
View File

@ -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 <drivers/airspeed/airspeed.h>
#include <drivers/device/i2c.h>
#include <drivers/drv_airspeed.h>
#include <math.h>
#include <mathlib/math/filter/LowPassFilter2p.hpp>
#include <px4_config.h>
#include <sys/types.h>
#include <systemlib/airspeed.h>
#include <systemlib/perf_counter.h>
#include <uORB/topics/differential_pressure.h>
#include <uORB/uORB.h>
#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_ */

289
src/drivers/sdp3x_airspeed/SDP3X_main.cpp Normal file
View File

@ -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);
}