diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 5410d28bee..b85f36e09d 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -184,6 +184,13 @@ then asp5033 start -X fi +# AUAV absolute/differential pressure sensor external I2C +if param greater -s SENS_EN_AUAVX 0 +then + auav start -D -X + auav start -A -X +fi + # SHT3x temperature and hygrometer sensor, external I2C if param compare -s SENS_EN_SHT3X 1 then diff --git a/src/drivers/differential_pressure/auav/AUAV.cpp b/src/drivers/differential_pressure/auav/AUAV.cpp new file mode 100644 index 0000000000..82f11f0799 --- /dev/null +++ b/src/drivers/differential_pressure/auav/AUAV.cpp @@ -0,0 +1,295 @@ +/**************************************************************************** + * + * Copyright (c) 2017-2024 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 "AUAV.hpp" +#include "AUAV_Absolute.hpp" +#include "AUAV_Differential.hpp" + +I2CSPIDriverBase *AUAV::instantiate(const I2CSPIDriverConfig &config, const int runtime_instance) +{ + AUAV *instance = nullptr; + + if (config.devid_driver_index == DRV_DIFF_PRESS_DEVTYPE_AUAV) { + instance = new AUAV_Differential(config); + + } else if (config.devid_driver_index == DRV_BARO_DEVTYPE_AUAV) { + instance = new AUAV_Absolute(config); + } + + if (!instance) { + PX4_ERR("alloc failed"); + return nullptr; + } + + if (instance->init() != OK) { + delete instance; + return nullptr; + } + + return instance; +} + +AUAV::AUAV(const I2CSPIDriverConfig &config) : + I2C(config), + I2CSPIDriver(config), + _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")), + _comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": comms errors")) +{ +} + +AUAV::~AUAV() +{ + perf_free(_sample_perf); + perf_free(_comms_errors); +} + +void AUAV::RunImpl() +{ + switch (_state) { + case STATE::READ_CALIBDATA: + handle_state_read_calibdata(); + break; + + case STATE::REQUEST_MEASUREMENT: + handle_state_request_measurement(); + break; + + case STATE::GATHER_MEASUREMENT: + handle_state_gather_measurement(); + break; + } +} + +void AUAV::print_status() +{ + I2CSPIDriverBase::print_status(); + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); +} + +int AUAV::init() +{ + const int ret = I2C::init(); + + if (ret != PX4_OK) { + DEVICE_DEBUG("I2C::init failed (%i)", ret); + return ret; + } + + int32_t hw_model = 0; + param_get(param_find("SENS_EN_AUAVX"), &hw_model); + + switch (hw_model) { + case 1: /* AUAV L05D (+- 5 inH20) */ + _cal_range = 10.0f; + break; + + case 2: /* AUAV L10D (+- 10 inH20) */ + _cal_range = 20.0f; + break; + + case 3: /* AUAV L30D (+- 30 inH20) */ + _cal_range = 60.0f; + break; + } + + ScheduleClear(); + ScheduleNow(); + return PX4_OK; +} + +int AUAV::probe() +{ + uint8_t res_data = 0; + int status = transfer(nullptr, 0, &res_data, sizeof(res_data)); + + /* Check that the sensor is active. Reported in bit 6 of the status byte */ + if ((res_data & 0x40) == 0) { + status = PX4_ERROR; + } + + return status; +} + +void AUAV::handle_state_read_calibdata() +{ + int status = PX4_OK; + calib_eeprom_addr_t calib_data_eeprom_addr = get_calib_eeprom_addr(); + calib_data_raw_t calib_data_raw {}; + + status = status || read_calibration_eeprom(calib_data_eeprom_addr.addr_a_hw, calib_data_raw.a_hw); + status = status || read_calibration_eeprom(calib_data_eeprom_addr.addr_a_lw, calib_data_raw.a_lw); + status = status || read_calibration_eeprom(calib_data_eeprom_addr.addr_b_hw, calib_data_raw.b_hw); + status = status || read_calibration_eeprom(calib_data_eeprom_addr.addr_b_lw, calib_data_raw.b_lw); + status = status || read_calibration_eeprom(calib_data_eeprom_addr.addr_c_hw, calib_data_raw.c_hw); + status = status || read_calibration_eeprom(calib_data_eeprom_addr.addr_c_lw, calib_data_raw.c_lw); + status = status || read_calibration_eeprom(calib_data_eeprom_addr.addr_d_hw, calib_data_raw.d_hw); + status = status || read_calibration_eeprom(calib_data_eeprom_addr.addr_d_lw, calib_data_raw.d_lw); + status = status || read_calibration_eeprom(calib_data_eeprom_addr.addr_tc50, calib_data_raw.tc50); + status = status || read_calibration_eeprom(calib_data_eeprom_addr.addr_es, calib_data_raw.es); + + if (status == PX4_OK) { + process_calib_data_raw(calib_data_raw); + _state = STATE::REQUEST_MEASUREMENT; + ScheduleNow(); + + } else { + /* In case of an error, try reading the calib data again after a backoff period */ + perf_count(_comms_errors); + ScheduleDelayed(10_ms); + } +} + +void AUAV::handle_state_request_measurement() +{ + uint8_t req_data = 0xAA; + int status = transfer(&req_data, sizeof(req_data), nullptr, 0); + + if (status == PX4_OK) { + _state = STATE::GATHER_MEASUREMENT; + ScheduleDelayed(get_conversion_interval()); + + } else { + /* In case of an error, start the next measurement after a backoff period */ + perf_count(_comms_errors); + ScheduleDelayed(10_ms); + } +} + +void AUAV::handle_state_gather_measurement() +{ + perf_begin(_sample_perf); + + uint8_t res_data[7]; + int status = transfer(nullptr, 0, res_data, sizeof(res_data)); + + /* Continue processing if the transfer was a success and bit 5 of the status is set to 0 (indicating the sensor is finished) */ + if (status == PX4_OK && (res_data[0] & 0x20) == 0) { + const hrt_abstime timestamp_sample = hrt_absolute_time(); + + const uint32_t pressure_raw = (res_data[1] << 16) | (res_data[2] << 8) | (res_data[3]); + const uint32_t temperature_raw = (res_data[4] << 16) | (res_data[5] << 8) | (res_data[6]); + + const float pressure_dig = correct_pressure(pressure_raw, temperature_raw); + const float pressure_p = process_pressure_dig(pressure_dig); + const float temperature_c = process_temperature_raw(temperature_raw); + + publish_pressure(pressure_p, temperature_c, timestamp_sample); + + } else { + /* In case of an error, ignore the results */ + perf_count(_comms_errors); + } + + _state = STATE::REQUEST_MEASUREMENT; + ScheduleNow(); + + perf_end(_sample_perf); +} + +int AUAV::read_calibration_eeprom(const uint8_t eeprom_address, uint16_t &data) +{ + const uint8_t req_data[3] = {eeprom_address, 0x0, 0x0}; + int status = transfer(req_data, sizeof(req_data), nullptr, 0); + + /* Wait for the EEPROM read access. Worst case is 2000us */ + px4_usleep(2000); + + uint8_t res_data[3]; + status = status || transfer(nullptr, 0, res_data, sizeof(res_data)); + + /* If bit 5 of the status byte is set to 1, the sensor is still busy. This read is considered invalid */ + if (res_data[0] & 0x20) { + status = PX4_ERROR; + } + + data = res_data[1] << 8 | res_data[2]; + return status; +} + +void AUAV::process_calib_data_raw(const calib_data_raw_t calib_data_raw) +{ + /* Conversion of calib data as described in the datasheet */ + _calib_data.a = (float)((calib_data_raw.a_hw << 16) | calib_data_raw.a_lw) / 0x7FFFFFFF; + _calib_data.b = (float)((calib_data_raw.b_hw << 16) | calib_data_raw.b_lw) / 0x7FFFFFFF; + _calib_data.c = (float)((calib_data_raw.c_hw << 16) | calib_data_raw.c_lw) / 0x7FFFFFFF; + _calib_data.d = (float)((calib_data_raw.d_hw << 16) | calib_data_raw.d_lw) / 0x7FFFFFFF; + + _calib_data.tc50h = (float)(calib_data_raw.tc50 >> 8) / 0x7F; + _calib_data.tc50l = (float)(calib_data_raw.tc50 & 0xFF) / 0x7F; + _calib_data.es = (float)(calib_data_raw.es & 0xFF) / 0x7F; +} + +float AUAV::correct_pressure(const uint32_t pressure_raw, const uint32_t temperature_raw) const +{ + /* Correct the pressure using the calib data as described in the datasheet */ + const int32_t p_raw = pressure_raw - 0x800000; + const float p_norm = (float) p_raw / 0x7FFFFF; + + const float ap = _calib_data.a * p_norm * p_norm * p_norm; + const float bp = _calib_data.b * p_norm * p_norm; + const float cp = _calib_data.c * p_norm; + + const float c_corr = ap + bp + cp + _calib_data.d; + const float p_corr = p_norm + c_corr; + + const int32_t t_diff = temperature_raw - 7576807; + const float p_nfso = (p_corr + 1.0f) / 2.0f; + + float tc50; + float p_diff; + + if (t_diff > 0) { + tc50 = _calib_data.tc50h; + + } else { + tc50 = _calib_data.tc50l; + } + + if (p_nfso > 0.5f) { + p_diff = p_nfso - 0.5f; + + } else { + p_diff = 0.5f - p_nfso; + } + + const float t_corr = ((1.0f - (_calib_data.es * 2.5f * p_diff)) * t_diff * tc50) / (100.0f * 100.0f * 167772.2f); + const float p_corrt = p_nfso - t_corr; + const float p_comp = (uint32_t)(p_corrt * (float)0xFFFFFF); + return p_comp; +} + +float AUAV::process_temperature_raw(const float temperature_raw) const +{ + return ((temperature_raw * 155.0f) / (1 << 24)) - 45.0f; +} diff --git a/src/drivers/differential_pressure/auav/AUAV.hpp b/src/drivers/differential_pressure/auav/AUAV.hpp new file mode 100644 index 0000000000..b6d952d813 --- /dev/null +++ b/src/drivers/differential_pressure/auav/AUAV.hpp @@ -0,0 +1,129 @@ +/**************************************************************************** + * + * Copyright (c) 2017-2024 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. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include +#include + +using namespace time_literals; + +static constexpr uint8_t I2C_ADDRESS_DIFFERENTIAL = 0x26; +static constexpr uint8_t I2C_ADDRESS_ABSOLUTE = 0x27; +static constexpr uint32_t I2C_SPEED = 100 * 1000; // 100 kHz I2C serial interface + +class AUAV : public device::I2C, public I2CSPIDriver +{ +public: + explicit AUAV(const I2CSPIDriverConfig &config); + virtual ~AUAV(); + + static I2CSPIDriverBase *instantiate(const I2CSPIDriverConfig &config, const int runtime_instance); + static void print_usage(); + + virtual void RunImpl(); + void print_status() override; + int init() override; + +protected: + enum class STATE : uint8_t { + READ_CALIBDATA, + REQUEST_MEASUREMENT, + GATHER_MEASUREMENT + }; + + struct calib_eeprom_addr_t { + uint8_t addr_a_hw; + uint8_t addr_a_lw; + uint8_t addr_b_hw; + uint8_t addr_b_lw; + uint8_t addr_c_hw; + uint8_t addr_c_lw; + uint8_t addr_d_hw; + uint8_t addr_d_lw; + uint8_t addr_tc50; + uint8_t addr_es; + }; + + struct calib_data_raw_t { + uint16_t a_hw; + uint16_t a_lw; + uint16_t b_hw; + uint16_t b_lw; + uint16_t c_hw; + uint16_t c_lw; + uint16_t d_hw; + uint16_t d_lw; + uint16_t tc50; + uint16_t es; + }; + + struct calib_data_t { + float a; + float b; + float c; + float d; + float tc50h; + float tc50l; + float es; + }; + + virtual void publish_pressure(const float pressure_p, const float temperature_c, + const hrt_abstime timestamp_sample) = 0; + virtual int64_t get_conversion_interval() const = 0; + virtual calib_eeprom_addr_t get_calib_eeprom_addr() const = 0; + virtual float process_pressure_dig(const float pressure_dig) const = 0; + + void handle_state_read_calibdata(); + void handle_state_request_measurement(); + void handle_state_gather_measurement(); + + int read_calibration_eeprom(const uint8_t eeprom_address, uint16_t &data); + void process_calib_data_raw(const calib_data_raw_t calib_data_raw); + float correct_pressure(const uint32_t pressure_raw, const uint32_t temperature_raw) const; + float process_temperature_raw(const float temperature_raw) const; + + float _cal_range{10.0f}; + STATE _state{STATE::READ_CALIBDATA}; + calib_data_t _calib_data {}; + + perf_counter_t _sample_perf; + perf_counter_t _comms_errors; + +private: + int probe() override; +}; diff --git a/src/drivers/differential_pressure/auav/AUAV_Absolute.cpp b/src/drivers/differential_pressure/auav/AUAV_Absolute.cpp new file mode 100644 index 0000000000..bded1d626d --- /dev/null +++ b/src/drivers/differential_pressure/auav/AUAV_Absolute.cpp @@ -0,0 +1,79 @@ +/**************************************************************************** + * + * Copyright (c) 2017-2024 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 "AUAV_Absolute.hpp" + +AUAV_Absolute::AUAV_Absolute(const I2CSPIDriverConfig &config) : + AUAV(config) +{ +} + +void AUAV_Absolute::publish_pressure(const float pressure_p, const float temperature_c, + const hrt_abstime timestamp_sample) +{ + sensor_baro_s sensor_baro{}; + sensor_baro.timestamp = hrt_absolute_time(); + sensor_baro.timestamp_sample = timestamp_sample; + sensor_baro.device_id = get_device_id(); + sensor_baro.pressure = pressure_p; + sensor_baro.temperature = temperature_c; + sensor_baro.error_count = perf_event_count(_comms_errors); + _sensor_baro_pub.publish(sensor_baro); +} + +int64_t AUAV_Absolute::get_conversion_interval() const +{ + return ABS_CONVERSION_INTERVAL; +} + +AUAV::calib_eeprom_addr_t AUAV_Absolute::get_calib_eeprom_addr() const +{ + return calib_eeprom_addr_t { + EEPROM_ABS_AHW, + EEPROM_ABS_ALW, + EEPROM_ABS_BHW, + EEPROM_ABS_BLW, + EEPROM_ABS_CHW, + EEPROM_ABS_CLW, + EEPROM_ABS_DHW, + EEPROM_ABS_DLW, + EEPROM_ABS_TC50, + EEPROM_ABS_ES + }; +} + +float AUAV_Absolute::process_pressure_dig(const float pressure_dig) const +{ + const float pressure_mbar = 250.f + 1.25f * ((pressure_dig - (0.1f * (1 << 24))) / (1 << 24)) * 1000.f; + return pressure_mbar * MBAR_TO_PA; +} diff --git a/src/drivers/differential_pressure/auav/AUAV_Absolute.hpp b/src/drivers/differential_pressure/auav/AUAV_Absolute.hpp new file mode 100644 index 0000000000..624ef27079 --- /dev/null +++ b/src/drivers/differential_pressure/auav/AUAV_Absolute.hpp @@ -0,0 +1,71 @@ +/**************************************************************************** + * + * Copyright (c) 2017-2024 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. + * + ****************************************************************************/ + +#pragma once + +#include "AUAV.hpp" +#include + +/* AUAV EEPROM addresses for absolute channel */ +static constexpr uint8_t EEPROM_ABS_AHW = 0x2F; +static constexpr uint8_t EEPROM_ABS_ALW = 0x30; +static constexpr uint8_t EEPROM_ABS_BHW = 0x31; +static constexpr uint8_t EEPROM_ABS_BLW = 0x32; +static constexpr uint8_t EEPROM_ABS_CHW = 0x33; +static constexpr uint8_t EEPROM_ABS_CLW = 0x34; +static constexpr uint8_t EEPROM_ABS_DHW = 0x35; +static constexpr uint8_t EEPROM_ABS_DLW = 0x36; +static constexpr uint8_t EEPROM_ABS_TC50 = 0x37; +static constexpr uint8_t EEPROM_ABS_ES = 0x38; + +/* Measurement rate is 50Hz */ +static constexpr unsigned ABS_MEAS_RATE = 50; +static constexpr int64_t ABS_CONVERSION_INTERVAL = (1000000 / ABS_MEAS_RATE); /* microseconds */ + +/* Conversions */ +static constexpr float MBAR_TO_PA = 100.0f; + +class AUAV_Absolute : public AUAV +{ +public: + explicit AUAV_Absolute(const I2CSPIDriverConfig &config); + ~AUAV_Absolute() = default; + +private: + void publish_pressure(const float pressure_p, const float temperature_c, const hrt_abstime timestamp_sample) override; + int64_t get_conversion_interval() const override; + calib_eeprom_addr_t get_calib_eeprom_addr() const override; + float process_pressure_dig(const float pressure_dig) const override; + + uORB::PublicationMulti _sensor_baro_pub{ORB_ID(sensor_baro)}; +}; diff --git a/src/drivers/differential_pressure/auav/AUAV_Differential.cpp b/src/drivers/differential_pressure/auav/AUAV_Differential.cpp new file mode 100644 index 0000000000..7b97f6e5cf --- /dev/null +++ b/src/drivers/differential_pressure/auav/AUAV_Differential.cpp @@ -0,0 +1,79 @@ +/**************************************************************************** + * + * Copyright (c) 2017-2024 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 "AUAV_Differential.hpp" + +AUAV_Differential::AUAV_Differential(const I2CSPIDriverConfig &config) : + AUAV(config) +{ +} + +void AUAV_Differential::publish_pressure(const float pressure_p, const float temperature_c, + const hrt_abstime timestamp_sample) +{ + differential_pressure_s differential_pressure{}; + differential_pressure.timestamp = hrt_absolute_time(); + differential_pressure.timestamp_sample = timestamp_sample; + differential_pressure.device_id = get_device_id(); + differential_pressure.differential_pressure_pa = pressure_p; + differential_pressure.temperature = temperature_c; + differential_pressure.error_count = perf_event_count(_comms_errors); + _differential_pressure_pub.publish(differential_pressure); +} + +int64_t AUAV_Differential::get_conversion_interval() const +{ + return DIFF_CONVERSION_INTERVAL; +} + +AUAV::calib_eeprom_addr_t AUAV_Differential::get_calib_eeprom_addr() const +{ + return calib_eeprom_addr_t { + EEPROM_DIFF_AHW, + EEPROM_DIFF_ALW, + EEPROM_DIFF_BHW, + EEPROM_DIFF_BLW, + EEPROM_DIFF_CHW, + EEPROM_DIFF_CLW, + EEPROM_DIFF_DHW, + EEPROM_DIFF_DLW, + EEPROM_DIFF_TC50, + EEPROM_DIFF_ES + }; +} + +float AUAV_Differential::process_pressure_dig(const float pressure_dig) const +{ + const float pressure_in_h = 1.25f * ((pressure_dig - (1 << 23)) / (1 << 24)) * _cal_range; + return pressure_in_h * INH_TO_PA; +} diff --git a/src/drivers/differential_pressure/auav/AUAV_Differential.hpp b/src/drivers/differential_pressure/auav/AUAV_Differential.hpp new file mode 100644 index 0000000000..412066a6ac --- /dev/null +++ b/src/drivers/differential_pressure/auav/AUAV_Differential.hpp @@ -0,0 +1,71 @@ +/**************************************************************************** + * + * Copyright (c) 2017-2024 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. + * + ****************************************************************************/ + +#pragma once + +#include "AUAV.hpp" +#include + +/* AUAV EEPROM addresses for differential channel */ +static constexpr uint8_t EEPROM_DIFF_AHW = 0x2B; +static constexpr uint8_t EEPROM_DIFF_ALW = 0x2C; +static constexpr uint8_t EEPROM_DIFF_BHW = 0x2D; +static constexpr uint8_t EEPROM_DIFF_BLW = 0x2E; +static constexpr uint8_t EEPROM_DIFF_CHW = 0x2F; +static constexpr uint8_t EEPROM_DIFF_CLW = 0x30; +static constexpr uint8_t EEPROM_DIFF_DHW = 0x31; +static constexpr uint8_t EEPROM_DIFF_DLW = 0x32; +static constexpr uint8_t EEPROM_DIFF_TC50 = 0x33; +static constexpr uint8_t EEPROM_DIFF_ES = 0x34; + +/* Measurement rate is 100Hz */ +static constexpr unsigned DIFF_MEAS_RATE = 100; +static constexpr int64_t DIFF_CONVERSION_INTERVAL = (1000000 / DIFF_MEAS_RATE); /* microseconds */ + +/* Conversions */ +static constexpr float INH_TO_PA = 249.08f; + +class AUAV_Differential : public AUAV +{ +public: + explicit AUAV_Differential(const I2CSPIDriverConfig &config); + ~AUAV_Differential() = default; + +private: + void publish_pressure(const float pressure_p, const float temperature_c, const hrt_abstime timestamp_sample) override; + int64_t get_conversion_interval() const override; + calib_eeprom_addr_t get_calib_eeprom_addr() const override; + float process_pressure_dig(const float pressure_dig) const override; + + uORB::PublicationMulti _differential_pressure_pub{ORB_ID(differential_pressure)}; +}; diff --git a/src/drivers/differential_pressure/auav/CMakeLists.txt b/src/drivers/differential_pressure/auav/CMakeLists.txt new file mode 100644 index 0000000000..9eaab94754 --- /dev/null +++ b/src/drivers/differential_pressure/auav/CMakeLists.txt @@ -0,0 +1,48 @@ +############################################################################ +# +# Copyright (c) 2015-2024 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__differential_pressure__auav + MAIN auav + COMPILE_FLAGS + SRCS + auav_main.cpp + AUAV.cpp + AUAV.hpp + AUAV_Absolute.cpp + AUAV_Absolute.hpp + AUAV_Differential.cpp + AUAV_Differential.hpp + DEPENDS + px4_work_queue + ) diff --git a/src/drivers/differential_pressure/auav/Kconfig b/src/drivers/differential_pressure/auav/Kconfig new file mode 100644 index 0000000000..d82bc68cf2 --- /dev/null +++ b/src/drivers/differential_pressure/auav/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_DIFFERENTIAL_PRESSURE_AUAV + bool "auav" + default n + ---help--- + Enable support for AUAV diff --git a/src/drivers/differential_pressure/auav/auav_main.cpp b/src/drivers/differential_pressure/auav/auav_main.cpp new file mode 100644 index 0000000000..1a00d02faf --- /dev/null +++ b/src/drivers/differential_pressure/auav/auav_main.cpp @@ -0,0 +1,100 @@ +/**************************************************************************** + * + * Copyright (c) 2017-2024 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 "AUAV.hpp" + +#include +#include + +void AUAV::print_usage() +{ + PRINT_MODULE_USAGE_NAME("auav", "driver"); + PRINT_MODULE_USAGE_SUBCATEGORY("airspeed_sensor"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAM_FLAG('D', "Differential pressure sensing", true); + PRINT_MODULE_USAGE_PARAM_FLAG('A', "Absolute pressure sensing", true); + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); + PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x26); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); +} + +extern "C" int auav_main(int argc, char *argv[]) +{ + using ThisDriver = AUAV; + + BusCLIArguments cli{true, false}; + cli.default_i2c_frequency = I2C_SPEED; + cli.i2c_address = I2C_ADDRESS_DIFFERENTIAL; + + int ch; + uint16_t device_type = 0; + const char *name = MODULE_NAME; + + while ((ch = cli.getOpt(argc, argv, "DA")) != EOF) { + switch (ch) { + case 'D': + device_type = DRV_DIFF_PRESS_DEVTYPE_AUAV; + name = MODULE_NAME "_differential"; + cli.i2c_address = I2C_ADDRESS_DIFFERENTIAL; + break; + + case 'A': + device_type = DRV_BARO_DEVTYPE_AUAV; + name = MODULE_NAME "_absolute"; + cli.i2c_address = I2C_ADDRESS_ABSOLUTE; + break; + } + } + + const char *verb = cli.optArg(); + + if (!verb || device_type == 0) { + ThisDriver::print_usage(); + return -1; + } + + BusInstanceIterator iterator(name, cli, device_type); + + if (!strcmp(verb, "start")) { + return ThisDriver::module_start(cli, iterator); + + } else if (!strcmp(verb, "stop")) { + return ThisDriver::module_stop(iterator); + + } else if (!strcmp(verb, "status")) { + return ThisDriver::module_status(iterator); + } + + ThisDriver::print_usage(); + return -1; +} diff --git a/src/drivers/differential_pressure/auav/parameters.c b/src/drivers/differential_pressure/auav/parameters.c new file mode 100644 index 0000000000..660bc4e943 --- /dev/null +++ b/src/drivers/differential_pressure/auav/parameters.c @@ -0,0 +1,44 @@ +/**************************************************************************** + * + * Copyright (c) 2021-2024 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. + * + ****************************************************************************/ + +/** + * Amphenol AUAV differential / absolute pressure sensor (external I2C) + * + * @reboot_required true + * @group Sensors + * @value 0 Sensor disabled, when explicitly started treated as AUAV L05D + * @value 1 AUAV L05D + * @value 2 AUAV L10D + * @value 3 AUAV L30D + */ +PARAM_DEFINE_INT32(SENS_EN_AUAVX, 0); diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 53eecce4be..904d63aeba 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -240,10 +240,14 @@ #define DRV_INS_DEVTYPE_VN100 0xE1 #define DRV_INS_DEVTYPE_VN200 0xE2 #define DRV_INS_DEVTYPE_VN300 0xE3 + #define DRV_DIFF_PRESS_DEVTYPE_ASP5033 0xE4 #define DRV_MAG_DEVTYPE_BMM350 0xE5 +#define DRV_DIFF_PRESS_DEVTYPE_AUAV 0xE6 +#define DRV_BARO_DEVTYPE_AUAV 0xE7 + #define DRV_DEVTYPE_UNUSED 0xff #endif /* _DRV_SENSOR_H */