From 3f172dbfa7b35e0be4ce7cb6544b33a35ebd9531 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 27 Jun 2021 21:38:17 -0400 Subject: [PATCH] ROMFS: new parameters for starting differential pressure sensors --- ROMFS/px4fmu_common/init.d/rc.sensors | 53 ++++++++++--------- .../differential_pressure/ets/parameters.c | 41 ++++++++++++++ .../differential_pressure/ms4525/parameters.c | 41 ++++++++++++++ .../differential_pressure/ms5525/parameters.c | 41 ++++++++++++++ .../differential_pressure/sdp3x/parameters.c | 41 ++++++++++++++ 5 files changed, 193 insertions(+), 24 deletions(-) create mode 100644 src/drivers/differential_pressure/ets/parameters.c create mode 100644 src/drivers/differential_pressure/ms4525/parameters.c create mode 100644 src/drivers/differential_pressure/ms5525/parameters.c create mode 100644 src/drivers/differential_pressure/sdp3x/parameters.c diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index f718d02d75..20038f4b5d 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -127,6 +127,35 @@ then fi fi +# Eagle Tree airspeed sensor external I2C +if param compare -s SENS_EN_ETSASPD 1 +then + ets_airspeed start -X +fi + +# Sensirion SDP3X differential pressure sensor external I2C +if param compare -s SENS_EN_SDP3X 1 +then + if sdp3x_airspeed start -X + then + else + # try another common address + sdp3x_airspeed start -X -a 0x22 + fi +fi + +# TE MS4525 differential pressure sensor external I2C +if param compare -s SENS_EN_MS4525 1 +then + ms4525_airspeed start -X +fi + +# TE MS5525 differential pressure sensor external I2C +if param compare -s SENS_EN_MS5525 1 +then + ms5525_airspeed start -X +fi + # probe for optional external I2C devices if param compare SENS_EXT_I2C_PRB 1 then @@ -143,30 +172,6 @@ then # start last (wait for possible icm20948 passthrough mode) ak09916 -X -q start - - # differential pressure sensors - if [ ${VEHICLE_TYPE} = fw -o ${VEHICLE_TYPE} = vtol ] - then - # Always try to start the airspeed sensors - # even if their usage might be disabled - sdp3x_airspeed start -X -q - sdp3x_airspeed start -X -a 0x22 -q - - # Pixhawk 2.1 has a MS5611 on I2C which gets wrongly - # detected as MS5525 because the chip manufacturer was so - # clever to assign the same I2C address and skip a WHO_AM_I - # register. - if [ $BOARD_FMUV3 = 21 ] - then - ms5525_airspeed start -X -b 2 -q - else - ms5525_airspeed start -X -q - fi - - ms4525_airspeed start -X -q - - ets_airspeed start -X -q - fi fi ############################################################################### diff --git a/src/drivers/differential_pressure/ets/parameters.c b/src/drivers/differential_pressure/ets/parameters.c new file mode 100644 index 0000000000..5a0f7728ff --- /dev/null +++ b/src/drivers/differential_pressure/ets/parameters.c @@ -0,0 +1,41 @@ +/**************************************************************************** + * + * Copyright (c) 2021 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. + * + ****************************************************************************/ + +/** + * Eagle Tree airspeed sensor (external I2C) + * + * @reboot_required true + * @group Sensors + * @boolean + */ +PARAM_DEFINE_INT32(SENS_EN_ETSASPD, 0); diff --git a/src/drivers/differential_pressure/ms4525/parameters.c b/src/drivers/differential_pressure/ms4525/parameters.c new file mode 100644 index 0000000000..c2ea85334b --- /dev/null +++ b/src/drivers/differential_pressure/ms4525/parameters.c @@ -0,0 +1,41 @@ +/**************************************************************************** + * + * Copyright (c) 2021 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. + * + ****************************************************************************/ + +/** + * TE MS4525 differential pressure sensor (external I2C) + * + * @reboot_required true + * @group Sensors + * @boolean + */ +PARAM_DEFINE_INT32(SENS_EN_MS4525, 0); diff --git a/src/drivers/differential_pressure/ms5525/parameters.c b/src/drivers/differential_pressure/ms5525/parameters.c new file mode 100644 index 0000000000..6b7eae2aca --- /dev/null +++ b/src/drivers/differential_pressure/ms5525/parameters.c @@ -0,0 +1,41 @@ +/**************************************************************************** + * + * Copyright (c) 2021 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. + * + ****************************************************************************/ + +/** + * TE MS5525 differential pressure sensor (external I2C) + * + * @reboot_required true + * @group Sensors + * @boolean + */ +PARAM_DEFINE_INT32(SENS_EN_MS5525, 0); diff --git a/src/drivers/differential_pressure/sdp3x/parameters.c b/src/drivers/differential_pressure/sdp3x/parameters.c new file mode 100644 index 0000000000..47f26836ce --- /dev/null +++ b/src/drivers/differential_pressure/sdp3x/parameters.c @@ -0,0 +1,41 @@ +/**************************************************************************** + * + * Copyright (c) 2021 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. + * + ****************************************************************************/ + +/** + * Sensirion SDP3X differential pressure sensor (external I2C) + * + * @reboot_required true + * @group Sensors + * @boolean + */ +PARAM_DEFINE_INT32(SENS_EN_SDP3X, 0);