diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors
index d9e8d75d221e031fe0001f7d35940de6cdecbb2d..5a6506ebe2577db04a303f207fffd3b841111abf 100644
--- a/ROMFS/px4fmu_common/init.d/rc.sensors
+++ b/ROMFS/px4fmu_common/init.d/rc.sensors
@@ -8,7 +8,6 @@ then
if ms5611 start
then
fi
-
else
if ver hwcmp AEROFC_V1
then
@@ -142,11 +141,6 @@ then
if hmc5883 -C -T -S -R 8 start
then
fi
-
- if meas_airspeed start -b 2
- then
- fi
-
else
# FMUv2
if mpu6000 start
@@ -333,7 +327,6 @@ fi
if ver hwcmp PX4FMU_V4PRO
then
-
# Internal SPI bus ICM-20608-G
if mpu6000 -R 2 -T 20608 start
then
@@ -358,7 +351,6 @@ then
if hmc5883 -C -T -X start
then
fi
-
fi
if ver hwcmp PX4FMU_V5
@@ -388,12 +380,6 @@ then
if hmc5883 -C -T -X start
then
fi
-
- # Possible external airspeed sensor
- if meas_airspeed start -b 2
- then
- fi
-
fi
if ver hwcmp AEROCORE2
@@ -404,13 +390,30 @@ fi
if sdp3x_airspeed start
then
-else
- if meas_airspeed start
- then
- else
- ets_airspeed start
- ets_airspeed start -b 1
- fi
+fi
+
+if ms5525_airspeed start
+then
+fi
+
+if ms5525_airspeed start -b 2
+then
+fi
+
+if ms4525_airspeed start
+then
+fi
+
+if ms4525_airspeed start -b 2
+then
+fi
+
+if ets_airspeed start
+then
+fi
+
+if ets_airspeed start -b 1
+then
fi
# Wait 20 ms for sensors (because we need to wait for the HRT and work queue callbacks to fire)
diff --git a/ROMFS/px4fmu_test/init.d/rc.sensors b/ROMFS/px4fmu_test/init.d/rc.sensors
index fd100666acbd8d789cf06358c58c6729baf9c5e9..ad66a116102652a69302e4337eb257b40fbd95ba 100644
--- a/ROMFS/px4fmu_test/init.d/rc.sensors
+++ b/ROMFS/px4fmu_test/init.d/rc.sensors
@@ -73,11 +73,6 @@ then
if hmc5883 -C -T -S -R 8 start
then
fi
-
- if meas_airspeed start -b 2
- then
- fi
-
else
# FMUv2
if mpu6000 start
@@ -179,16 +174,32 @@ then
fi
fi
-if meas_airspeed start
+if sdp3x_airspeed start
+then
+fi
+
+if ms5525_airspeed start
+then
+fi
+
+if ms5525_airspeed start -b 2
+then
+fi
+
+if ms4525_airspeed start
+then
+fi
+
+if ms4525_airspeed start -b 2
+then
+fi
+
+if ets_airspeed start
+then
+fi
+
+if ets_airspeed start -b 1
then
-else
- if ets_airspeed start
- then
- else
- if ets_airspeed start -b 1
- then
- fi
- fi
fi
if sf1xx start
diff --git a/cmake/configs/nuttx_aerocore2_default.cmake b/cmake/configs/nuttx_aerocore2_default.cmake
index bf8e8bae0d853d7a464f8f7cab5250e55cf1a137..87e37e856545216222eb03d3e7dd31c8c4bc3bce 100644
--- a/cmake/configs/nuttx_aerocore2_default.cmake
+++ b/cmake/configs/nuttx_aerocore2_default.cmake
@@ -26,6 +26,7 @@ set(config_module_list
drivers/airspeed
drivers/ets_airspeed
drivers/meas_airspeed
+ drivers/ms5525_airspeed
#drivers/frsky_telemetry
modules/sensors
#drivers/pwm_input
diff --git a/cmake/configs/nuttx_auav-x21_default.cmake b/cmake/configs/nuttx_auav-x21_default.cmake
index 8fc302dfb8fd1a8aae2ddc02d098667c779ad907..b0ba209da457d98d631328f72cd9e533a42bc0b0 100644
--- a/cmake/configs/nuttx_auav-x21_default.cmake
+++ b/cmake/configs/nuttx_auav-x21_default.cmake
@@ -38,6 +38,7 @@ set(config_module_list
drivers/airspeed
drivers/ets_airspeed
drivers/meas_airspeed
+ drivers/ms5525_airspeed
drivers/frsky_telemetry
modules/sensors
drivers/mkblctrl
diff --git a/cmake/configs/nuttx_mindpx-v2_default.cmake b/cmake/configs/nuttx_mindpx-v2_default.cmake
index efd41d3d39fb5ea2a20463e3ea5092b1f01e1567..62d5a093be55a6d072adb7c59bed12a6e95c8dd2 100644
--- a/cmake/configs/nuttx_mindpx-v2_default.cmake
+++ b/cmake/configs/nuttx_mindpx-v2_default.cmake
@@ -41,6 +41,7 @@ set(config_module_list
drivers/airspeed
drivers/ets_airspeed
drivers/meas_airspeed
+ drivers/ms5525_airspeed
drivers/frsky_telemetry
modules/sensors
#drivers/mkblctrl
diff --git a/cmake/configs/nuttx_px4fmu-v1_default.cmake b/cmake/configs/nuttx_px4fmu-v1_default.cmake
index 00ac027a6abc68c8f7731978dfa74e35f23b9dd4..8055dca28da22b75e12420bd9d7d8c2217a60af5 100644
--- a/cmake/configs/nuttx_px4fmu-v1_default.cmake
+++ b/cmake/configs/nuttx_px4fmu-v1_default.cmake
@@ -36,6 +36,7 @@ set(config_module_list
drivers/airspeed
drivers/ets_airspeed
drivers/meas_airspeed
+ drivers/ms5525_airspeed
drivers/frsky_telemetry
modules/sensors
drivers/vmount
diff --git a/cmake/configs/nuttx_px4fmu-v2_default.cmake b/cmake/configs/nuttx_px4fmu-v2_default.cmake
index cba2cdddda50a1c7c762a2f80c69b756eb5f975c..860aa2d3e7ce086ffeca107fd7ea872f59335fad 100644
--- a/cmake/configs/nuttx_px4fmu-v2_default.cmake
+++ b/cmake/configs/nuttx_px4fmu-v2_default.cmake
@@ -40,6 +40,7 @@ set(config_module_list
drivers/sdp3x_airspeed
drivers/ets_airspeed
drivers/meas_airspeed
+ drivers/ms5525_airspeed
drivers/frsky_telemetry
modules/sensors
#drivers/mkblctrl
diff --git a/cmake/configs/nuttx_px4fmu-v2_test.cmake b/cmake/configs/nuttx_px4fmu-v2_test.cmake
index cb7df79efd4b23565687ee9210a89913c89399af..f130ef9c5183dac43dd94e2ce7b7fb30c63a03e6 100644
--- a/cmake/configs/nuttx_px4fmu-v2_test.cmake
+++ b/cmake/configs/nuttx_px4fmu-v2_test.cmake
@@ -39,6 +39,7 @@ set(config_module_list
drivers/airspeed
drivers/ets_airspeed
drivers/meas_airspeed
+ drivers/ms5525_airspeed
drivers/frsky_telemetry
modules/sensors
#drivers/mkblctrl
diff --git a/cmake/configs/nuttx_px4fmu-v3_default.cmake b/cmake/configs/nuttx_px4fmu-v3_default.cmake
index 716b0acb4e56baca604709ac24d3ee9c1425f632..a33534d7727f73bdbba5daf072e0279179b7f9bd 100644
--- a/cmake/configs/nuttx_px4fmu-v3_default.cmake
+++ b/cmake/configs/nuttx_px4fmu-v3_default.cmake
@@ -33,6 +33,7 @@ set(config_module_list
drivers/lsm303d
drivers/mb12xx
drivers/meas_airspeed
+ drivers/ms5525_airspeed
drivers/mkblctrl
drivers/mpu6000
drivers/mpu9250
diff --git a/cmake/configs/nuttx_px4fmu-v4_default.cmake b/cmake/configs/nuttx_px4fmu-v4_default.cmake
index de2238559a0df2cf655c33d5e6e3e0d208335c37..646973b14da196361a7d8b5c591d6e1c1807de38 100644
--- a/cmake/configs/nuttx_px4fmu-v4_default.cmake
+++ b/cmake/configs/nuttx_px4fmu-v4_default.cmake
@@ -38,6 +38,7 @@ set(config_module_list
drivers/airspeed
drivers/ets_airspeed
drivers/meas_airspeed
+ drivers/ms5525_airspeed
drivers/frsky_telemetry
modules/sensors
drivers/mkblctrl
diff --git a/cmake/configs/nuttx_px4fmu-v4pro_default.cmake b/cmake/configs/nuttx_px4fmu-v4pro_default.cmake
index 24dff87018a8da1e0aa9d2aedccc0f15817dbfdc..6e8f54ff86bacbfb2dfb347acec0240cca51e5bd 100644
--- a/cmake/configs/nuttx_px4fmu-v4pro_default.cmake
+++ b/cmake/configs/nuttx_px4fmu-v4pro_default.cmake
@@ -34,6 +34,7 @@ set(config_module_list
drivers/lsm303d
drivers/mb12xx
drivers/meas_airspeed
+ drivers/ms5525_airspeed
drivers/mkblctrl
drivers/mpu6000
drivers/mpu9250
diff --git a/cmake/configs/nuttx_px4fmu-v5_default.cmake b/cmake/configs/nuttx_px4fmu-v5_default.cmake
index e97a0eff5d8b5c892f56261bb95c8fc01b720a0f..4640268cf0f8fe716bf9cbce1b001fe82484c534 100644
--- a/cmake/configs/nuttx_px4fmu-v5_default.cmake
+++ b/cmake/configs/nuttx_px4fmu-v5_default.cmake
@@ -33,6 +33,7 @@ set(config_module_list
drivers/ll40ls
drivers/mb12xx
drivers/meas_airspeed
+ drivers/ms5525_airspeed
drivers/mkblctrl
drivers/mpu6000
drivers/mpu9250
diff --git a/cmake/configs/nuttx_px4nucleoF767ZI-v1_default.cmake b/cmake/configs/nuttx_px4nucleoF767ZI-v1_default.cmake
index 3a6017dab43c86bb9c29e3cf1e36a9dc1d55d68f..bce5c5589e829b7af84ff79f097eb9d29a65b8c0 100644
--- a/cmake/configs/nuttx_px4nucleoF767ZI-v1_default.cmake
+++ b/cmake/configs/nuttx_px4nucleoF767ZI-v1_default.cmake
@@ -36,6 +36,7 @@ set(config_module_list
drivers/airspeed
drivers/ets_airspeed
drivers/meas_airspeed
+ drivers/ms5525_airspeed
drivers/frsky_telemetry
modules/sensors
drivers/mkblctrl
diff --git a/cmake/configs/nuttx_tap-v1_default.cmake b/cmake/configs/nuttx_tap-v1_default.cmake
index f4334d26a9454139ad6011087f87b693cceb9651..697f0ebad289d41153d2b40ade75860b6ddfc7a6 100644
--- a/cmake/configs/nuttx_tap-v1_default.cmake
+++ b/cmake/configs/nuttx_tap-v1_default.cmake
@@ -25,6 +25,7 @@ set(config_module_list
drivers/gps
drivers/airspeed
drivers/meas_airspeed
+ drivers/ms5525_airspeed
modules/sensors
drivers/vmount
diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp
index afbe86fc9e7f7a65e4a3a0e46c12e6d0b27f1bfb..367605f72a6cc24c39e10352997ac62e60b167ac 100644
--- a/src/drivers/airspeed/airspeed.cpp
+++ b/src/drivers/airspeed/airspeed.cpp
@@ -151,7 +151,7 @@ Airspeed::init()
_airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &arp);
if (_airspeed_pub == nullptr) {
- warnx("uORB started?");
+ PX4_WARN("uORB started?");
}
}
@@ -190,7 +190,7 @@ Airspeed::ioctl(struct file *filp, int cmd, unsigned long arg)
_measure_ticks = 0;
return OK;
- /* external signalling (DRDY) not supported */
+ /* external signaling (DRDY) not supported */
case SENSOR_POLLRATE_EXTERNAL:
/* zero would be bad */
diff --git a/src/drivers/airspeed/airspeed.h b/src/drivers/airspeed/airspeed.h
index 3c7d8e58c3c753d16377dcd9e08f5d6e736c4e4c..b6f596507c7752c093d55f49661774ffdcb18a3f 100644
--- a/src/drivers/airspeed/airspeed.h
+++ b/src/drivers/airspeed/airspeed.h
@@ -81,7 +81,7 @@
#include <uORB/topics/subsystem_info.h>
/* Default I2C bus */
-#define PX4_I2C_BUS_DEFAULT PX4_I2C_BUS_EXPANSION
+static constexpr uint8_t PX4_I2C_BUS_DEFAULT = PX4_I2C_BUS_EXPANSION;
#ifndef CONFIG_SCHED_WORKQUEUE
# error This requires CONFIG_SCHED_WORKQUEUE.
@@ -130,7 +130,7 @@ protected:
float _max_differential_pressure_pa;
bool _sensor_ok;
bool _last_published_sensor_ok;
- int _measure_ticks;
+ uint32_t _measure_ticks;
bool _collect_phase;
float _diff_pres_offset;
diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp
index 6e497a4f5328e9df5d5a431712cc978c0282161b..a3604c5b202903d06fb1826d3e9366825598d3bc 100644
--- a/src/drivers/ets_airspeed/ets_airspeed.cpp
+++ b/src/drivers/ets_airspeed/ets_airspeed.cpp
@@ -319,7 +319,8 @@ fail:
g_dev = nullptr;
}
- errx(1, "no ETS airspeed sensor connected");
+ PX4_WARN("no ETS airspeed sensor connected");
+ exit(1);
}
/**
diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp
index 5ece89cd319e6b9381785af7c454904a740d761d..8fd76e396a7840f234c9d226e9c29f062038545f 100644
--- a/src/drivers/meas_airspeed/meas_airspeed.cpp
+++ b/src/drivers/meas_airspeed/meas_airspeed.cpp
@@ -43,7 +43,6 @@
* Supported sensors:
*
* - MS4525DO (http://www.meas-spec.com/downloads/MS4525DO.pdf)
- * - untested: MS5525DSO (http://www.meas-spec.com/downloads/MS5525DSO.pdf)
*
* Interface application notes:
*
@@ -94,9 +93,6 @@
/* I2C bus address is 1010001x */
#define I2C_ADDRESS_MS4525DO 0x28 /**< 7-bit address. Depends on the order code (this is for code "I") */
#define PATH_MS4525 "/dev/ms4525"
-/* The MS5525DSO address is 111011Cx, where C is the complementary value of the pin CSB */
-#define I2C_ADDRESS_MS5525DSO 0x77 //0x77/* 7-bit address, addr. pin pulled low */
-#define PATH_MS5525 "/dev/ms5525"
/* Register address */
#define ADDR_READ_MR 0x00 /* write to this address to start conversion */
@@ -440,22 +436,6 @@ start(int i2c_bus)
goto fail;
}
- /* try the MS5525DSO next if init fails */
- if (OK != g_dev->Airspeed::init()) {
- delete g_dev;
- g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS5525DSO, PATH_MS5525);
-
- /* check if the MS5525DSO was instantiated */
- if (g_dev == nullptr) {
- goto fail;
- }
-
- /* both versions failed if the init for the MS5525DSO fails, give up */
- if (OK != g_dev->Airspeed::init()) {
- goto fail;
- }
- }
-
/* set the poll rate to default, starts automatic data collection */
fd = open(PATH_MS4525, O_RDONLY);
@@ -476,7 +456,8 @@ fail:
g_dev = nullptr;
}
- errx(1, "no MS4525 airspeed sensor connected");
+ PX4_WARN("no MS4525 airspeed sensor connected");
+ exit(1);
}
/**
diff --git a/src/drivers/ms5525_airspeed/CMakeLists.txt b/src/drivers/ms5525_airspeed/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..1ed4ca661f0762577f9f1270c3463d75653fead5
--- /dev/null
+++ b/src/drivers/ms5525_airspeed/CMakeLists.txt
@@ -0,0 +1,44 @@
+############################################################################
+#
+# Copyright (c) 2015 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__ms5525_airspeed
+ MAIN ms5525_airspeed
+ STACK_MAIN 1200
+ COMPILE_FLAGS
+ SRCS
+ MS5525.cpp
+ MS5525_main.cpp
+ DEPENDS
+ platforms__common
+ )
+# vim: set noet ft=cmake fenc=utf-8 ff=unix :
diff --git a/src/drivers/ms5525_airspeed/MS5525.cpp b/src/drivers/ms5525_airspeed/MS5525.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6666c755100f5652e3a8e0788783b0821fe1dc6b
--- /dev/null
+++ b/src/drivers/ms5525_airspeed/MS5525.cpp
@@ -0,0 +1,331 @@
+/****************************************************************************
+ *
+ * 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 "MS5525.hpp"
+
+int
+MS5525::measure()
+{
+ int ret = PX4_ERROR;
+
+ if (_inited) {
+ // send the command to begin a conversion.
+ uint8_t cmd = _current_cmd;
+ ret = transfer(&cmd, 1, nullptr, 0);
+
+ if (ret != PX4_OK) {
+ perf_count(_comms_errors);
+ }
+
+ } else {
+ _inited = init_ms5525();
+
+ if (_inited) {
+ ret = PX4_OK;
+ }
+ }
+
+ return ret;
+}
+
+bool
+MS5525::init_ms5525()
+{
+ // Step 1 - reset
+ uint8_t cmd = CMD_RESET;
+ int ret = transfer(&cmd, 1, nullptr, 0);
+
+ if (ret != PX4_OK) {
+ perf_count(_comms_errors);
+ return false;
+ }
+
+ usleep(3000);
+
+ // Step 2 - read calibration coefficients from prom
+
+ // prom layout
+ // 0 factory data and the setup
+ // 1-6 calibration coefficients
+ // 7 serial code and CRC
+ uint16_t prom[8];
+
+ for (uint8_t i = 0; i < 8; i++) {
+ cmd = CMD_PROM_START + i * 2;
+
+ // request PROM value
+ ret = transfer(&cmd, 1, nullptr, 0);
+
+ if (ret != PX4_OK) {
+ perf_count(_comms_errors);
+ return false;
+ }
+
+ // read 2 byte value
+ uint8_t val[2];
+ ret = transfer(nullptr, 0, &val[0], 2);
+
+ if (ret == PX4_OK) {
+ prom[i] = (val[0] << 8) | val[1];
+
+ } else {
+ perf_count(_comms_errors);
+ return false;
+ }
+ }
+
+ // Step 3 - check CRC
+ const uint8_t crc = prom_crc4(prom);
+ const uint8_t onboard_crc = prom[7] & 0xF;
+
+ if (crc == onboard_crc) {
+ // store valid calibration coefficients
+ C1 = prom[1];
+ C2 = prom[2];
+ C3 = prom[3];
+ C4 = prom[4];
+ C5 = prom[5];
+ C6 = prom[6];
+
+ Tref = C5 * (1UL << Q5);
+
+ return true;
+
+ } else {
+ PX4_ERR("CRC mismatch");
+ return false;
+ }
+}
+
+uint8_t
+MS5525::prom_crc4(uint16_t n_prom[]) const
+{
+ // see Measurement Specialties AN520
+
+ // crc remainder
+ unsigned int n_rem = 0x00;
+
+ // original value of the crc
+ unsigned int crc_read = n_prom[7]; // save read CRC
+ n_prom[7] = (0xFF00 & (n_prom[7])); // CRC byte is replaced by 0
+
+ // operation is performed on bytes
+ for (int cnt = 0; cnt < 16; cnt++) {
+ // choose LSB or MSB
+ if (cnt % 2 == 1) {
+ n_rem ^= (unsigned short)((n_prom[cnt >> 1]) & 0x00FF);
+
+ } else {
+ n_rem ^= (unsigned short)(n_prom[cnt >> 1] >> 8);
+ }
+
+ for (uint8_t n_bit = 8; n_bit > 0; n_bit--) {
+ if (n_rem & (0x8000)) {
+ n_rem = (n_rem << 1) ^ 0x3000;
+
+ } else {
+ n_rem = (n_rem << 1);
+ }
+ }
+ }
+
+ n_rem = (0x000F & (n_rem >> 12)); // final 4-bit reminder is CRC code
+ n_prom[7] = crc_read; // restore the crc_read to its original place
+
+ return (n_rem ^ 0x00);
+}
+
+int
+MS5525::collect()
+{
+ perf_begin(_sample_perf);
+
+ // read ADC
+ uint8_t cmd = CMD_ADC_READ;
+ int ret = transfer(&cmd, 1, nullptr, 0);
+
+ if (ret != PX4_OK) {
+ perf_count(_comms_errors);
+ return ret;
+ }
+
+ // read 24 bits from the sensor
+ uint8_t val[3];
+ ret = transfer(nullptr, 0, &val[0], 3);
+
+ if (ret != PX4_OK) {
+ perf_count(_comms_errors);
+ return ret;
+ }
+
+ uint32_t adc = (val[0] << 16) | (val[1] << 8) | val[2];
+
+ // If the conversion is not executed before the ADC read command, or the ADC read command is repeated, it will give 0 as the output
+ // result. If the ADC read command is sent during conversion the result will be 0, the conversion will not stop and
+ // the final result will be wrong. Conversion sequence sent during the already started conversion process will yield
+ // incorrect result as well.
+ if (adc == 0) {
+ perf_count(_comms_errors);
+ return EAGAIN;
+ }
+
+ if (_current_cmd == CMD_CONVERT_PRES) {
+ D1 = adc;
+ _pressure_count++;
+
+ if (_pressure_count > 9) {
+ _pressure_count = 0;
+ _current_cmd = CMD_CONVERT_TEMP;
+ }
+
+ } else if (_current_cmd == CMD_CONVERT_TEMP) {
+ D2 = adc;
+ _current_cmd = CMD_CONVERT_PRES;
+
+ // only calculate and publish after new pressure readings
+ return PX4_OK;
+ }
+
+ // not ready yet
+ if (D1 == 0 || D2 == 0) {
+ return EAGAIN;
+ }
+
+ // Difference between actual and reference temperature
+ // dT = D2 - Tref
+ const int32_t dT = D2 - Tref;
+
+ // Measured temperature
+ // TEMP = 20°C + dT * TEMPSENS
+ const int32_t TEMP = 2000 + (dT * C6) / (1L << Q6);
+
+ // Offset at actual temperature
+ // OFF = OFF_T1 + TCO * dT
+ const int64_t OFF = C2 * (1L << Q2) + (C4 * dT) / (1L << Q4);
+
+ // Sensitivity at actual temperature
+ // SENS = SENS_T1 + TCS * dT
+ const int64_t SENS = C1 * (1L << Q1) + (C3 * dT) / (1L << Q3);
+
+ // Temperature Compensated Pressure (example 24996 = 2.4996 psi)
+ // P = D1 * SENS - OFF
+ const int64_t P = (((int64_t)D1 * SENS) / (1L << 21) - OFF) / (1L << 15);
+
+ const float diff_press_PSI = (float)P * 0.0001f;
+
+ // 1 PSI = 6894.76 Pascals
+ const float PSI_to_Pa = 6894.757f;
+ float diff_press_pa_raw = diff_press_PSI * PSI_to_Pa;
+
+ const float temperature_c = (float)TEMP * 0.01f;
+
+ // the raw value still should be compensated for the known offset
+ diff_press_pa_raw -= _diff_pres_offset;
+
+ /* 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;
+ }
+
+ differential_pressure_s diff_pressure = {
+ .timestamp = hrt_absolute_time(),
+ .error_count = perf_event_count(_comms_errors),
+ .differential_pressure_raw_pa = diff_press_pa_raw,
+ .differential_pressure_filtered_pa = _filter.apply(diff_press_pa_raw),
+ .max_differential_pressure_pa = _max_differential_pressure_pa,
+ .temperature = temperature_c
+ };
+
+ if (_airspeed_pub != nullptr && !(_pub_blocked)) {
+ /* publish it */
+ orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &diff_pressure);
+ }
+
+ new_report(diff_pressure);
+
+ /* notify anyone waiting for data */
+ poll_notify(POLLIN);
+
+ ret = OK;
+
+ perf_end(_sample_perf);
+
+ return ret;
+}
+
+void
+MS5525::cycle()
+{
+ int ret = PX4_ERROR;
+
+ // collection phase
+ if (_collect_phase) {
+ // perform collection
+ ret = collect();
+
+ if (OK != ret) {
+ /* restart the measurement state machine */
+ start();
+ _sensor_ok = false;
+ return;
+ }
+
+ // next phase is measurement
+ _collect_phase = false;
+
+ // is there a collect->measure gap?
+ if (_measure_ticks > USEC2TICK(CONVERSION_INTERVAL)) {
+
+ // schedule a fresh cycle call when we are ready to measure again
+ work_queue(HPWORK, &_work, (worker_t)&Airspeed::cycle_trampoline, this,
+ _measure_ticks - USEC2TICK(CONVERSION_INTERVAL));
+
+ return;
+ }
+ }
+
+ /* measurement phase */
+ ret = measure();
+
+ if (OK != ret) {
+ DEVICE_DEBUG("measure error");
+ }
+
+ _sensor_ok = (ret == OK);
+
+ // next phase is collection
+ _collect_phase = true;
+
+ // schedule a fresh cycle call when the measurement is done
+ work_queue(HPWORK, &_work, (worker_t)&Airspeed::cycle_trampoline, this, USEC2TICK(CONVERSION_INTERVAL));
+}
diff --git a/src/drivers/ms5525_airspeed/MS5525.hpp b/src/drivers/ms5525_airspeed/MS5525.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..7518bf32b63ed1acc96ecca32d6146d4f0ac19b6
--- /dev/null
+++ b/src/drivers/ms5525_airspeed/MS5525.hpp
@@ -0,0 +1,138 @@
+/****************************************************************************
+ *
+ * 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.
+ *
+ ****************************************************************************/
+
+#ifndef DRIVERS_MS5525_AIRSPEED_HPP_
+#define DRIVERS_MS5525_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>
+
+/* The MS5525DSO address is 111011Cx, where C is the complementary value of the pin CSB */
+static constexpr uint8_t I2C_ADDRESS_1_MS5525DSO = 0x76;
+static constexpr uint8_t I2C_ADDRESS_2_MS5525DSO = 0x77;
+
+static constexpr const char PATH_MS5525[] = "/dev/ms5525";
+
+/* Measurement rate is 100Hz */
+static constexpr unsigned MEAS_RATE = 100;
+static constexpr float MEAS_DRIVER_FILTER_FREQ = 1.2f;
+static constexpr uint64_t CONVERSION_INTERVAL = (1000000 / MEAS_RATE); /* microseconds */
+
+class MS5525 : public Airspeed
+{
+public:
+ MS5525(uint8_t bus, uint8_t address = I2C_ADDRESS_1_MS5525DSO, const char *path = PATH_MS5525) :
+ Airspeed(bus, address, CONVERSION_INTERVAL, path)
+ {
+ }
+
+ ~MS5525() override = default;
+
+private:
+
+ /**
+ * Perform a poll cycle; collect from the previous measurement
+ * and start a new one.
+ */
+ void cycle() override;
+
+ int measure() override;
+ int collect() override;
+
+ math::LowPassFilter2p _filter{MEAS_RATE, MEAS_DRIVER_FILTER_FREQ};
+
+ static constexpr uint8_t CMD_RESET = 0x1E; // ADC reset command
+ static constexpr uint8_t CMD_ADC_READ = 0x00; // ADC read command
+
+ static constexpr uint8_t CMD_PROM_START = 0xA0; // Prom read command (first)
+
+ // D1 - pressure convert commands
+ // Convert D1 (OSR=256) 0x40
+ // Convert D1 (OSR=512) 0x42
+ // Convert D1 (OSR=1024) 0x44
+ // Convert D1 (OSR=2048) 0x46
+ // Convert D1 (OSR=4096) 0x48
+ static constexpr uint8_t CMD_CONVERT_PRES = 0x44;
+
+ // D2 - temperature convert commands
+ // Convert D2 (OSR=256) 0x50
+ // Convert D2 (OSR=512) 0x52
+ // Convert D2 (OSR=1024) 0x54
+ // Convert D2 (OSR=2048) 0x56
+ // Convert D2 (OSR=4096) 0x58
+ static constexpr uint8_t CMD_CONVERT_TEMP = 0x54;
+
+ uint8_t _current_cmd{CMD_CONVERT_PRES};
+
+ unsigned _pressure_count{0};
+
+ // Qx Coefficients Matrix by Pressure Range
+ // 5525DSO-pp001DS (Pmin = -1, Pmax = 1)
+ static constexpr uint8_t Q1 = 15;
+ static constexpr uint8_t Q2 = 17;
+ static constexpr uint8_t Q3 = 7;
+ static constexpr uint8_t Q4 = 5;
+ static constexpr uint8_t Q5 = 7;
+ static constexpr uint8_t Q6 = 21;
+
+ // calibration coefficients from prom
+ uint16_t C1{0};
+ uint16_t C2{0};
+ uint16_t C3{0};
+ uint16_t C4{0};
+ uint16_t C5{0};
+ uint16_t C6{0};
+
+ uint32_t Tref{0};
+
+ // last readings for D1 (uncompensated pressure) and D2 (uncompensated temperature)
+ uint32_t D1{0};
+ uint32_t D2{0};
+
+ bool init_ms5525();
+ bool _inited{false};
+
+ uint8_t prom_crc4(uint16_t n_prom[]) const;
+
+};
+
+#endif /* DRIVERS_MS5525_AIRSPEED_HPP_ */
diff --git a/src/drivers/ms5525_airspeed/MS5525_main.cpp b/src/drivers/ms5525_airspeed/MS5525_main.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c06d2dc98d1ba41881a0aede28445c52377dbef1
--- /dev/null
+++ b/src/drivers/ms5525_airspeed/MS5525_main.cpp
@@ -0,0 +1,270 @@
+/****************************************************************************
+ *
+ * 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 "MS5525.hpp"
+
+// Driver 'main' command.
+extern "C" __EXPORT int ms5525_airspeed_main(int argc, char *argv[]);
+
+// Local functions in support of the shell command.
+namespace ms5525_airspeed
+{
+MS5525 *g_dev = nullptr;
+
+void start(uint8_t i2c_bus);
+void stop();
+void test();
+void reset();
+
+// Start the driver.
+// This function call only returns once the driver is up and running
+// or failed to detect the sensor.
+void
+start(uint8_t i2c_bus)
+{
+ int fd = -1;
+
+ if (g_dev != nullptr) {
+ PX4_ERR("already started");
+ exit(1);
+ }
+
+ g_dev = new MS5525(i2c_bus, I2C_ADDRESS_1_MS5525DSO, PATH_MS5525);
+
+ /* check if the MS4525DO was instantiated */
+ if (g_dev == nullptr) {
+ goto fail;
+ }
+
+ /* try the next MS5525DSO if init fails */
+ if (OK != g_dev->Airspeed::init()) {
+ delete g_dev;
+
+ PX4_WARN("trying MS5525 address 2");
+
+ g_dev = new MS5525(i2c_bus, I2C_ADDRESS_2_MS5525DSO, PATH_MS5525);
+
+ /* check if the MS5525DSO was instantiated */
+ if (g_dev == nullptr) {
+ PX4_WARN("MS5525 was not instantiated");
+ goto fail;
+ }
+
+ /* both versions failed if the init for the MS5525DSO fails, give up */
+ if (OK != g_dev->Airspeed::init()) {
+ PX4_WARN("MS5525 init fail");
+ goto fail;
+ }
+ }
+
+ /* set the poll rate to default, starts automatic data collection */
+ fd = open(PATH_MS5525, 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 MS5525 airspeed sensor connected");
+ exit(1);
+}
+
+// stop the driver
+void stop()
+{
+ if (g_dev != nullptr) {
+ delete g_dev;
+ g_dev = nullptr;
+
+ } else {
+ PX4_ERR("driver not running");
+ exit(1);
+ }
+
+ 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_MS5525, O_RDONLY);
+
+ if (fd < 0) {
+ PX4_WARN("%s open failed (try 'ms5525_airspeed start' if the driver is not running", PATH_MS5525);
+ 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) {
+ PX4_ERR("timed out");
+ }
+
+ /* now go get it */
+ sz = read(fd, &report, sizeof(report));
+
+ if (sz != sizeof(report)) {
+ PX4_ERR("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);
+ }
+}
+
+// reset the driver
+void reset()
+{
+ int fd = open(PATH_MS5525, O_RDONLY);
+
+ if (fd < 0) {
+ PX4_ERR("failed ");
+ exit(1);
+ }
+
+ if (ioctl(fd, SENSORIOCRESET, 0) < 0) {
+ PX4_ERR("driver reset failed");
+ exit(1);
+ }
+
+ if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
+ PX4_ERR("driver poll restart failed");
+ exit(1);
+ }
+
+ exit(0);
+}
+
+} // namespace ms5525_airspeed
+
+
+static void
+ms5525_airspeed_usage()
+{
+ PX4_WARN("usage: ms5525_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");
+}
+
+int
+ms5525_airspeed_main(int argc, char *argv[])
+{
+ uint8_t 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")) {
+ ms5525_airspeed::start(i2c_bus);
+ }
+
+ /*
+ * Stop the driver
+ */
+ if (!strcmp(argv[1], "stop")) {
+ ms5525_airspeed::stop();
+ }
+
+ /*
+ * Test the driver/device.
+ */
+ if (!strcmp(argv[1], "test")) {
+ ms5525_airspeed::test();
+ }
+
+ /*
+ * Reset the driver.
+ */
+ if (!strcmp(argv[1], "reset")) {
+ ms5525_airspeed::reset();
+ }
+
+ ms5525_airspeed_usage();
+ exit(0);
+}
diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp
index 654c5e5e44c9a34b349fa7e90f3eeb3cc1b582ea..e5f82413595a007a01c436251ca9b57223b411df 100644
--- a/src/modules/commander/airspeed_calibration.cpp
+++ b/src/modules/commander/airspeed_calibration.cpp
@@ -230,7 +230,6 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
calibration_log_info(mavlink_log_pub, "[cal] Swap static and dynamic ports!");
/* the user setup is wrong, wipe the calibration to force a proper re-calibration */
-
diff_pres_offset = 0.0f;
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG, 1);