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

MS5525 differential pressure driver

This commit is contained in:
Daniel Agar 2017-05-22 10:06:29 -04:00
parent 66a890d428
commit 7029be87c0
23 changed files with 852 additions and 62 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
ROMFS/px4fmu_common/init.d/rc.sensors
View File

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

39
ROMFS/px4fmu_test/init.d/rc.sensors
View File

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

1
cmake/configs/nuttx_aerocore2_default.cmake
View File

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

1
cmake/configs/nuttx_auav-x21_default.cmake
View File

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

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

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

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

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

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

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

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

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

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

@ -33,6 +33,7 @@ set(config_module_list
drivers/lsm303d
drivers/mb12xx
drivers/meas_airspeed
drivers/ms5525_airspeed
drivers/mkblctrl
drivers/mpu6000
drivers/mpu9250

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

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

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

@ -34,6 +34,7 @@ set(config_module_list
drivers/lsm303d
drivers/mb12xx
drivers/meas_airspeed
drivers/ms5525_airspeed
drivers/mkblctrl
drivers/mpu6000
drivers/mpu9250

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

@ -33,6 +33,7 @@ set(config_module_list
drivers/ll40ls
drivers/mb12xx
drivers/meas_airspeed
drivers/ms5525_airspeed
drivers/mkblctrl
drivers/mpu6000
drivers/mpu9250

1
cmake/configs/nuttx_px4nucleoF767ZI-v1_default.cmake
View File

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

1
cmake/configs/nuttx_tap-v1_default.cmake
View File

@ -25,6 +25,7 @@ set(config_module_list
drivers/gps
drivers/airspeed
drivers/meas_airspeed
drivers/ms5525_airspeed
modules/sensors
drivers/vmount

4
src/drivers/airspeed/airspeed.cpp
View File

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

4
src/drivers/airspeed/airspeed.h
View File

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

3
src/drivers/ets_airspeed/ets_airspeed.cpp
View File

@ -319,7 +319,8 @@ fail:
g_dev = nullptr;
}
errx(1, "no ETS airspeed sensor connected");
PX4_WARN("no ETS airspeed sensor connected");
exit(1);
}
/**

23
src/drivers/meas_airspeed/meas_airspeed.cpp
View File

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

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

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

331
src/drivers/ms5525_airspeed/MS5525.cpp Normal file
View File

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

138
src/drivers/ms5525_airspeed/MS5525.hpp Normal file
View File

@ -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_ */

270
src/drivers/ms5525_airspeed/MS5525_main.cpp Normal file
View File

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

1
src/modules/commander/airspeed_calibration.cpp
View File

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