diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 8247025f8d..af52b6826d 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -114,6 +114,57 @@ then adis16448 -S start fi +# Hall effect sensors si7210 +# Potentially remove the -k option if possible and improve the startup if possible +if param greater CAL_AV_AOA_ID -1 +then + set AOA_I2C_ID 0 + if param compare CAL_AV_AOA_ID 48 + then + set AOA_I2C_ID 48 + fi + if param compare CAL_AV_AOA_ID 49 + then + set AOA_I2C_ID 49 + fi + if param compare CAL_AV_AOA_ID 50 + then + set AOA_I2C_ID 50 + fi + if param compare CAL_AV_AOA_ID 51 + then + set AOA_I2C_ID 51 + fi + + si7210 start -X -k -a ${AOA_I2C_ID} + unset AOA_I2C_ID +fi + +if param greater CAL_AV_SLIP_ID -1 +then + set SLIP_I2C_ID 0 + if param compare CAL_AV_SLIP_ID 48 + then + set SLIP_I2C_ID 48 + fi + if param compare CAL_AV_SLIP_ID 49 + then + set SLIP_I2C_ID 49 + fi + if param compare CAL_AV_SLIP_ID 50 + then + set SLIP_I2C_ID 50 + fi + if param compare CAL_AV_SLIP_ID 51 + then + set SLIP_I2C_ID 51 + fi + + si7210 start -X -k -a ${SLIP_I2C_ID} + unset SLIP_I2C_ID +fi + + # probe for optional external I2C devices if param compare SENS_EXT_I2C_PRB 1 then diff --git a/boards/px4/fmu-v3/default.cmake b/boards/px4/fmu-v3/default.cmake index 7a6c719a88..1addd15ded 100644 --- a/boards/px4/fmu-v3/default.cmake +++ b/boards/px4/fmu-v3/default.cmake @@ -41,6 +41,7 @@ px4_add_board( magnetometer # all available magnetometer drivers optical_flow # all available optical flow drivers osd + si7210 pca9685 pca9685_pwm_out #power_monitor/ina226 diff --git a/boards/px4/fmu-v5/default.cmake b/boards/px4/fmu-v5/default.cmake index abc1cef9c6..66ff7e01d4 100644 --- a/boards/px4/fmu-v5/default.cmake +++ b/boards/px4/fmu-v5/default.cmake @@ -50,6 +50,7 @@ px4_add_board( roboclaw rpm safety_button + si7210 telemetry # all available telemetry drivers test_ppm tone_alarm diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index b66c6e41b5..6455aab358 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -39,6 +39,8 @@ set(msg_files actuator_controls.msg actuator_outputs.msg adc_report.msg + airflow_aoa.msg + airflow_slip.msg airspeed.msg airspeed_validated.msg airspeed_wind.msg @@ -128,6 +130,7 @@ set(msg_files sensor_baro.msg sensor_combined.msg sensor_correction.msg + sensor_hall.msg sensor_gps.msg sensor_gyro.msg sensor_gyro_fft.msg diff --git a/msg/airflow_aoa.msg b/msg/airflow_aoa.msg new file mode 100644 index 0000000000..a799636f18 --- /dev/null +++ b/msg/airflow_aoa.msg @@ -0,0 +1,3 @@ +uint64 timestamp # time since system start (microseconds) +float32 aoa_rad # angle of attack in radians +bool valid # true if measurement is within sensor/calibration range, false otherwise diff --git a/msg/airflow_slip.msg b/msg/airflow_slip.msg new file mode 100644 index 0000000000..09dad0ddd7 --- /dev/null +++ b/msg/airflow_slip.msg @@ -0,0 +1,3 @@ +uint64 timestamp # time since system start (microseconds) +float32 slip_rad # sideslip angle in radians +bool valid # true if measurement is within sensor/calibration range, false otherwise diff --git a/msg/sensor_hall.msg b/msg/sensor_hall.msg new file mode 100644 index 0000000000..a64d07f31c --- /dev/null +++ b/msg/sensor_hall.msg @@ -0,0 +1,7 @@ +uint64 timestamp # time since system start (microseconds) + +int8 instance # sensor instance + +float32 mag_t # magnetic field measurement in Tesla + +float32 temp_c # temperature measurement in deg C diff --git a/msg/tools/uorb_rtps_message_ids.yaml b/msg/tools/uorb_rtps_message_ids.yaml index 5475d5d677..616104fe73 100644 --- a/msg/tools/uorb_rtps_message_ids.yaml +++ b/msg/tools/uorb_rtps_message_ids.yaml @@ -342,6 +342,8 @@ rtps: id: 158 - msg: estimator_event_flags id: 159 + - msg: sensor_hall + id: 160 ########## multi topics: begin ########## - msg: actuator_controls_0 id: 170 diff --git a/src/drivers/drv_hall.h b/src/drivers/drv_hall.h new file mode 100644 index 0000000000..1dc99f3c9c --- /dev/null +++ b/src/drivers/drv_hall.h @@ -0,0 +1,56 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2018 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file drv_hall.h + * + * Hall Effect Magnetic Sensor driver interface. + */ + +#ifndef _DRV_HALL_H +#define _DRV_HALL_H + +#include +#include + +#include "drv_sensor.h" +#include "drv_orb_dev.h" + +#define HALL_BASE_DEVICE_PATH "/dev/hall" +#define HALL0_DEVICE_PATH "/dev/hall0" +#define HALL1_DEVICE_PATH "/dev/hall1" +#define HALL2_DEVICE_PATH "/dev/hall2" +#define HALL3_DEVICE_PATH "/dev/hall3" + + +#endif /* _DRV_HALL_H */ diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 1af334cb74..b4be57e51a 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -177,6 +177,8 @@ #define DRV_DIST_DEVTYPE_SIM 0x9a #define DRV_DIST_DEVTYPE_SRF05 0x9b +#define DRV_HALL_DEVTYPE_SI7210 0x9c + #define DRV_DEVTYPE_UNUSED 0xff #endif /* _DRV_SENSOR_H */ diff --git a/src/drivers/si7210/CMakeLists.txt b/src/drivers/si7210/CMakeLists.txt new file mode 100644 index 0000000000..f4b744d51d --- /dev/null +++ b/src/drivers/si7210/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# 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. +# +############################################################################ +px4_add_module( + MODULE drivers__si7210 + MAIN si7210 + COMPILE_FLAGS + SRCS + si7210_main.cpp + si7210.cpp + DEPENDS + drivers__vane + mathlib + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/si7210/si7210.cpp b/src/drivers/si7210/si7210.cpp new file mode 100644 index 0000000000..7db8c27ff6 --- /dev/null +++ b/src/drivers/si7210/si7210.cpp @@ -0,0 +1,313 @@ +/**************************************************************************** + * + * Copyright (c) 2012-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. + * + ****************************************************************************/ + +/** + * @file si7210.cpp + * @author: Amir Melzer + * + * Driver for the SI7210 connected via I2C. + */ + +#include "si7210.h" + +using namespace time_literals; + +/* Get registers value */ +int +SI7210::get_regs(uint8_t ptr, uint8_t *regs) +{ + uint8_t data; + + if (OK != transfer(&ptr, 1, &data, 1)) { + perf_count(_comms_errors); + return -EIO; + } + + *regs = data; + + return OK; +} + +/* Set registers value */ +int +SI7210::set_regs(uint8_t ptr, uint8_t value) +{ + uint8_t data[2]; + + data[0] = ptr; + data[1] = value; + + if (OK != transfer(&data[0], 2, nullptr, 0)) { + perf_count(_comms_errors); + return -EIO; + } + + /* read back the reg and verify */ + + if (OK != transfer(&ptr, 1, &data[1], 1)) { + perf_count(_comms_errors); + return -EIO; + } + + if (data[1] != value) { + //return -EIO; + } + + return OK; +} + +/* Get measurement value */ +int +SI7210::get_measurement(uint8_t ptr, uint16_t *value) +{ + uint8_t data[2]; + + if (OK != transfer(&ptr, 1, &data[0], 2)) { + perf_count(_comms_errors); + return -EIO; + } + + *value = (uint16_t)((data[1] & 0xff) + ((data[0] & 0xff) << 8)); + + return OK; +} + +/* Get sensor data */ +int +SI7210::get_sensor_data(uint8_t otpAddr, int8_t *data) +{ + uint8_t optCtrl; + uint8_t reg; + + if (OK != get_regs(OTP_CTRL, &optCtrl)) { + perf_count(_comms_errors); + return -EIO; + } + + if (OK != (optCtrl & OTP_CTRL_OPT_BUSY_MASK)) { + perf_count(_comms_errors); + return -EIO; + } + + reg = otpAddr; + + if (OK != set_regs(OTP_ADDR, reg)) { + perf_count(_comms_errors); + return -EIO; + } + + reg = OTP_CTRL_OPT_READ_EN_MASK; + + if (OK != set_regs(OTP_CTRL, reg)) { + perf_count(_comms_errors); + return -EIO; + } + + if (OK != get_regs(OTP_DATA, (uint8_t *) data)) { + perf_count(_comms_errors); + return -EIO; + } + + return OK; +} + + +int SI7210::write_command(uint16_t command) +{ + uint8_t cmd[2]; + cmd[0] = static_cast(command >> 8); + cmd[1] = static_cast(command & 0xff); + return transfer(&cmd[0], 2, nullptr, 0); +} + +bool +SI7210::init_si7210() +{ + return configure() == 0; +} + +int +SI7210::configure() +{ + uint8_t reg; + int ret = get_regs(HREVID, ®); + bool config_failed = false; + + if (((reg & 0xf0) >> 4) != IDCHIPID) { + config_failed = true; + } + + if ((reg & 0x0f) != REVID) { + config_failed = true; + } + + if ((ret != PX4_OK) || config_failed) { + perf_count(_comms_errors); + DEVICE_DEBUG("config failed"); + _state = State::RequireConfig; + return ret; + } + + _state = State::Configuring; + + return ret; +} + +void SI7210::start() +{ + // make sure to wait 10ms after configuring the measurement mode + ScheduleDelayed(10_ms); +} + +int SI7210::collect() +{ + perf_begin(_sample_perf); + + _collect_phase = false; + + uint8_t reg; + + uint16_t magRawData; + uint16_t tempRawData; + + int8_t otpTempOffsetData; + int8_t otpTempGainData; + + /* capture the magnetic field measurements */ + reg = ARAUTOINC_ARAUTOINC_MASK; + + if (OK != set_regs(ARAUTOINC, reg)) { + return -EIO; + } + + reg = DSPSIGSEL_MAG_VAL_SEL; + + if (OK != set_regs(DSPSIGSEL, reg)) { + return -EIO; + } + + reg = POWER_CTRL_ONEBURST_MASK; + + if (OK != set_regs(POWER_CTRL, reg)) { + return -EIO; + } + + if (OK != get_measurement(DSPSIGM, &magRawData)) { + return -EIO; + } + + /* capture the temperate measurements */ + reg = DSPSIGSEL_TEMP_VAL_SEL; + + if (OK != set_regs(DSPSIGSEL, reg)) { + return -EIO; + } + + reg = POWER_CTRL_ONEBURST_MASK; + + if (OK != set_regs(POWER_CTRL, reg)) { + return -EIO; + } + + if (OK != get_measurement(DSPSIGM, &tempRawData)) { + return -EIO; + } + + if (OK != get_sensor_data(OTP_TEMP_OFFSET, &otpTempOffsetData)) { + return -EIO; + } + + if (OK != get_sensor_data(OTP_TEMP_GAIN, &otpTempGainData)) { + return -EIO; + } + + float _tempOffset = (float)otpTempOffsetData / 16; + float _tempGain = 1 + (float)otpTempGainData / 2048; + + if (OK == ((magRawData & 0x8000) && (tempRawData & 0x8000))) { + return -EIO; + } + + sensor_hall_s report{}; + + /* generate a new report */ + report.timestamp = hrt_absolute_time(); + report.instance = _i2c_address; + report.mag_t = (float)(magRawData - MAG_BIAS) * MAG_CONV; + report.temp_c = (float)((tempRawData & ~0x8000) >> 3); + report.temp_c = _tempGain * (TEMP_CONV_A2 * report.temp_c * report.temp_c + TEMP_CONV_A1 * report.temp_c + TEMP_CONV_A0 + + VDD_CONV * + VDD) + + _tempOffset; + + _vane_pub.publish(report); + + perf_end(_sample_perf); + + return PX4_OK; +} + +void +SI7210::RunImpl() +{ + switch (_state) { + case State::RequireConfig: + if (configure() == PX4_OK) { + ScheduleDelayed(10_ms); + + } else { + // periodically retry to configure + ScheduleDelayed(300_ms); + } + + break; + + case State::Configuring: + _state = State::Running; + + ScheduleDelayed(10_ms); + break; + + case State::Running: + int ret = collect(); + + if (ret != 0 && ret != -EAGAIN) { + _sensor_ok = false; + DEVICE_DEBUG("measure error"); + _state = State::RequireConfig; + } + + ScheduleDelayed(SI7210_CONVERSION_INTERVAL); + break; + } +} diff --git a/src/drivers/si7210/si7210.h b/src/drivers/si7210/si7210.h new file mode 100644 index 0000000000..c9ca59aa7c --- /dev/null +++ b/src/drivers/si7210/si7210.h @@ -0,0 +1,186 @@ +/**************************************************************************** + * + * Copyright (c) 2012-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. + * + ****************************************************************************/ + +/** + * @file si7210.h + * @author: Amir Melzer + * + * Driver for the SI7210 connected via I2C. + */ + +#ifndef SI7210_HPP_ +#define SI7210_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#define I2C_ADDRESS_SI7210 0x30 /* Default SI7210 I2C address */ + +#define SI7210_MAX_DATA_RATE 50 + +#define SI7210_BUS_SPEED 1000*100 + +/* This value is set based on Max output data rate value */ +#define SI7210_CONVERSION_INTERVAL (1000000 / 100) /* microseconds */ + +#define IDCHIPID 0x01 +#define REVID 0x04 + +#define ARAUTOINC_ARAUTOINC_MASK 0x01 +#define OTP_CTRL_OPT_BUSY_MASK 0x01 +#define OTP_CTRL_OPT_READ_EN_MASK 0x02 +#define POWER_CTRL_SLEEP_MASK 0x01 +#define POWER_CTRL_STOP_MASK 0x02 +#define POWER_CTRL_ONEBURST_MASK 0x04 +#define POWER_CTRL_USESTORE_MASK 0x08 +#define POWER_CTRL_MEAS_MASK 0x80 +#define DSPSIGSEL_MAG_VAL_SEL 0 +#define DSPSIGSEL_TEMP_VAL_SEL 1 + +/** I2C registers for Si72xx */ +#define OTP_TEMP_OFFSET 0x1D +#define OTP_TEMP_GAIN 0x1E +#define HREVID 0xC0 +#define DSPSIGM 0xC1 +#define DSPSIGL 0xC2 +#define DSPSIGSEL 0xC3 +#define POWER_CTRL 0xC4 +#define ARAUTOINC 0xC5 +#define CTRL1 0xC6 +#define CTRL2 0xC7 +#define SLTIME 0xC8 +#define CTRL3 0xC9 +#define A0 0xCA +#define A1 0xCB +#define A2 0xCC +#define CTRL4 0xCD +#define A3 0xCE +#define A4 0xCF +#define A5 0xD0 +#define OTP_ADDR 0xE1 +#define OTP_DATA 0xE2 +#define OTP_CTRL 0xE3 +#define TM_FG 0xE4 + +/** temperature conv for Si72xx */ +#define TEMP_CONV_A2 -3.83e-6F +#define TEMP_CONV_A1 0.16094F +#define TEMP_CONV_A0 -279.80F + +#define VDD 3.30F +#define VDD_CONV -0.222F + +/** Magnetic conv for Si72xx */ +#define MAG_BIAS 0xC000 +#define MAG_CONV 0.00125F + +class SI7210 : public Vane, public I2CSPIDriver +{ +public: + SI7210(I2CSPIBusOption bus_option, const int bus, int bus_frequency, int address = I2C_ADDRESS_SI7210, + bool keep_retrying = false) : + Vane(bus, bus_frequency, address, SI7210_CONVERSION_INTERVAL), + I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus, address), + _keep_retrying{keep_retrying}, + _i2c_address{address} + { + } + + virtual ~SI7210() = default; + + static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance); + static void print_usage(); + + void RunImpl(); + + void start(); + +private: + enum class State { + RequireConfig, + Configuring, + Running + }; + + int measure() override { return 0; } + int collect() override; + int configure(); + + bool init_si7210(); + + /** + * Get registers values + * + * @return OK if the measurement command was successful. + */ + int get_regs(uint8_t ptr, uint8_t *regs); + + /** + * Set registers values + * + * @return OK if the measurement command was successful. + */ + int set_regs(uint8_t ptr, uint8_t value); + + /** + * Get measurement values + * + * @return OK if the measurement command was successful. + */ + int get_measurement(uint8_t ptr, uint16_t *value); + + /** + * Get si7210 data + * + * @return OK if the measurement command was successful. + */ + int get_sensor_data(uint8_t otpAddr, int8_t *data); + + /** + * Write a command in Sensirion specific logic + */ + int write_command(uint16_t command); + + uint16_t _scale{0}; + const bool _keep_retrying; + State _state{State::RequireConfig}; + int _i2c_address; +}; + +#endif /* SI7210_HPP_ */ diff --git a/src/drivers/si7210/si7210_main.cpp b/src/drivers/si7210/si7210_main.cpp new file mode 100644 index 0000000000..682e9e9a1f --- /dev/null +++ b/src/drivers/si7210/si7210_main.cpp @@ -0,0 +1,114 @@ +/**************************************************************************** + * + * Copyright (c) 2012-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. + * + ****************************************************************************/ +/** + * @file si7210_main.cpp + * Driver for the SI7210 connected via I2C. + * + * Author: Amir Melzer + */ + + +#include "si7210.h" + +extern "C" __EXPORT int si7210_main(int argc, char *argv[]); + +I2CSPIDriverBase *SI7210::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance) +{ + SI7210 *instance = new SI7210(iterator.configuredBusOption(), iterator.bus(), cli.bus_frequency, cli.i2c_address, + cli.keep_running); + + if (instance == nullptr) { + PX4_ERR("alloc failed"); + return nullptr; + } + + if (instance->init() != PX4_OK) { + delete instance; + return nullptr; + } + + instance->start(); + return instance; +} + + +void +SI7210::print_usage() +{ + PRINT_MODULE_USAGE_NAME("si7210", "driver"); + PRINT_MODULE_USAGE_SUBCATEGORY("airspeed_sensor"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); + PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(I2C_ADDRESS_SI7210); + PRINT_MODULE_USAGE_PARAMS_I2C_KEEP_RUNNING_FLAG(); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); +} + +int +si7210_main(int argc, char *argv[]) +{ + using ThisDriver = SI7210; + BusCLIArguments cli{true, false}; + cli.default_i2c_frequency = 100000; + cli.i2c_address = I2C_ADDRESS_SI7210; + cli.support_keep_running = true; + + int ch; + + while ((ch = cli.getopt(argc, argv, "")) != EOF) {} + + const char *verb = cli.parseDefaultArguments(argc, argv); + + if (!verb) { + ThisDriver::print_usage(); + return -1; + } + + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_HALL_DEVTYPE_SI7210); + + if (!strcmp(verb, "start")) { + return ThisDriver::module_start(cli, iterator); + } + + if (!strcmp(verb, "stop")) { + return ThisDriver::module_stop(iterator); + } + + if (!strcmp(verb, "status")) { + return ThisDriver::module_status(iterator); + } + + ThisDriver::print_usage(); + return -1; +} diff --git a/src/lib/drivers/CMakeLists.txt b/src/lib/drivers/CMakeLists.txt index b74962c5b9..606c97e742 100644 --- a/src/lib/drivers/CMakeLists.txt +++ b/src/lib/drivers/CMakeLists.txt @@ -40,3 +40,4 @@ add_subdirectory(led) add_subdirectory(magnetometer) add_subdirectory(rangefinder) add_subdirectory(smbus) +add_subdirectory(vane) diff --git a/src/lib/drivers/vane/CMakeLists.txt b/src/lib/drivers/vane/CMakeLists.txt new file mode 100644 index 0000000000..6e6470ffe0 --- /dev/null +++ b/src/lib/drivers/vane/CMakeLists.txt @@ -0,0 +1,35 @@ +############################################################################ +# +# 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. +# +############################################################################ + +px4_add_library(drivers__vane vane.cpp) +target_link_libraries(drivers__vane PRIVATE drivers__device) diff --git a/src/lib/drivers/vane/vane.cpp b/src/lib/drivers/vane/vane.cpp new file mode 100644 index 0000000000..9c6cb86d20 --- /dev/null +++ b/src/lib/drivers/vane/vane.cpp @@ -0,0 +1,122 @@ +/**************************************************************************** + * + * Copyright (c) 2013-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. + * + ****************************************************************************/ + +/** + * @file airspeed.cpp + * @author Simon Wilks + * @author Lorenz Meier + * + */ + +#include +#include + +#include + +#include +#include +#include + +#include +#include + +#include +#include + +#include + +Vane::Vane(int bus, int bus_frequency, int address, unsigned conversion_interval) : + I2C(0, "Vane", bus, address, bus_frequency), + _sensor_ok(false), + _measure_interval(conversion_interval), + _collect_phase(false), + _vane_orb_class_instance(-1), + _class_instance(-1), + _conversion_interval(conversion_interval), + _sample_perf(perf_alloc(PC_ELAPSED, "vane_read")), + _comms_errors(perf_alloc(PC_COUNT, "vane_com_err")) +{ +} + +Vane::~Vane() +{ + if (_class_instance != -1) { + unregister_class_devname(HALL_BASE_DEVICE_PATH, _class_instance); + } + + // free perf counters + perf_free(_sample_perf); + perf_free(_comms_errors); +} + +int +Vane::init() +{ + /* do I2C init (and probe) first */ + if (I2C::init() != PX4_OK) { + return PX4_ERROR; + } + + /* register alternate interfaces if we have to */ + _class_instance = register_class_devname(HALL_BASE_DEVICE_PATH); + + /* advertise sensor topic, measure manually to initialize valid report */ + measure(); + + return PX4_OK; +} + +int +Vane::probe() +{ + /* on initial power up the device may need more than one retry + for detection. Once it is running the number of retries can + be reduced + */ + _retries = 4; + int ret = measure(); + + // drop back to 2 retries once initialised + _retries = 2; + return ret; +} + +int +Vane::ioctl(device::file_t *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + default: + /* give it to the superclass */ + return I2C::ioctl(filp, cmd, arg); + } +} diff --git a/src/lib/drivers/vane/vane.h b/src/lib/drivers/vane/vane.h new file mode 100644 index 0000000000..2358ca0a33 --- /dev/null +++ b/src/lib/drivers/vane/vane.h @@ -0,0 +1,84 @@ +/**************************************************************************** + * + * Copyright (c) 2013, 2014 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 +#include +#include +#include + +class __EXPORT Vane : public device::I2C +{ +public: + Vane(int bus, int bus_frequency, int address, unsigned conversion_interval); + virtual ~Vane(); + + int init() override; + + int ioctl(device::file_t *filp, int cmd, unsigned long arg) override; + +private: + Vane(const Vane &) = delete; + Vane &operator=(const Vane &) = delete; + +protected: + int probe() override; + + /** + * Perform a poll cycle; collect from the previous measurement + * and start a new one. + */ + virtual int measure() = 0; + virtual int collect() = 0; + + bool _sensor_ok; + int _measure_interval; + bool _collect_phase; + + uORB::PublicationMulti _vane_pub{ORB_ID(sensor_hall)}; + + int _vane_orb_class_instance; + + int _class_instance; + + unsigned _conversion_interval; + + perf_counter_t _sample_perf; + perf_counter_t _comms_errors; +}; diff --git a/src/modules/mavlink/mavlink_bridge_header.h b/src/modules/mavlink/mavlink_bridge_header.h index e4811f5628..938e7ef81e 100644 --- a/src/modules/mavlink/mavlink_bridge_header.h +++ b/src/modules/mavlink/mavlink_bridge_header.h @@ -92,7 +92,7 @@ void mavlink_end_uart_send(mavlink_channel_t chan, int length); extern mavlink_status_t *mavlink_get_channel_status(uint8_t chan); extern mavlink_message_t *mavlink_get_channel_buffer(uint8_t chan); -#include +#include __END_DECLS diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 574ec21596..5b61895fd7 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -48,7 +48,7 @@ #ifndef MAVLINK_FTP_UNIT_TEST #include "mavlink_main.h" #else -#include +#include #endif using namespace time_literals; diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index a796ce45fb..2d18aefd76 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -46,7 +46,7 @@ #ifndef MAVLINK_FTP_UNIT_TEST #include "mavlink_bridge_header.h" #else -#include +#include #endif class MavlinkFtpTest; diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 917078a824..6f7dc43d47 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1536,6 +1536,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("RAW_RPM", 2.0f); configure_stream_local("RC_CHANNELS", 5.0f); configure_stream_local("SERVO_OUTPUT_RAW_0", 1.0f); + configure_stream_local("SENSOR_AIRFLOW_ANGLES", 10.0f); configure_stream_local("SYS_STATUS", 1.0f); configure_stream_local("UTM_GLOBAL_POSITION", 0.5f); configure_stream_local("VFR_HUD", 4.0f); @@ -1739,6 +1740,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("SCALED_IMU3", 25.0f); configure_stream_local("SERVO_OUTPUT_RAW_0", 20.0f); configure_stream_local("SERVO_OUTPUT_RAW_1", 20.0f); + configure_stream_local("SENSOR_AIRFLOW_ANGLES", 20.0f); configure_stream_local("SYS_STATUS", 1.0f); configure_stream_local("SYSTEM_TIME", 1.0f); configure_stream_local("UTM_GLOBAL_POSITION", 1.0f); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index a89bbf8f6e..08c17d368b 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -99,6 +99,7 @@ #include "streams/RC_CHANNELS.hpp" #include "streams/SCALED_IMU.hpp" #include "streams/SCALED_PRESSURE.hpp" +#include "streams/SENSOR_AIRFLOW_ANGLES.hpp" #include "streams/SERVO_OUTPUT_RAW.hpp" #include "streams/STATUSTEXT.hpp" #include "streams/STORAGE_INFORMATION.hpp" @@ -346,6 +347,9 @@ static const StreamListItem streams_list[] = { #if defined(SCALED_PRESSURE3) create_stream_list_item(), #endif // SCALED_PRESSURE3 +#if defined(SENSOR_AIRFLOW_ANGLES_HPP) + create_stream_list_item(), +#endif // SENSOR_AIRFLOW_ANGLES #if defined(ACTUATOR_OUTPUT_STATUS_HPP) create_stream_list_item(), #endif // ACTUATOR_OUTPUT_STATUS_HPP diff --git a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h index 8d2868af76..da263f1ab5 100644 --- a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h +++ b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h @@ -40,7 +40,7 @@ #ifndef MAVLINK_FTP_UNIT_TEST #include "../mavlink_bridge_header.h" #else -#include +#include #endif #include "../mavlink_ftp.h" diff --git a/src/modules/mavlink/streams/SENSOR_AIRFLOW_ANGLES.hpp b/src/modules/mavlink/streams/SENSOR_AIRFLOW_ANGLES.hpp new file mode 100644 index 0000000000..5be7de3f7c --- /dev/null +++ b/src/modules/mavlink/streams/SENSOR_AIRFLOW_ANGLES.hpp @@ -0,0 +1,111 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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 SENSOR_AIRFLOW_ANGLES_HPP +#define SENSOR_AIRFLOW_ANGLES_HPP + +#include +#include + +class MavlinkStreamSensorAirflowAngles : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamSensorAirflowAngles(mavlink); } + + static constexpr const char *get_name_static() { return "SENSOR_AIRFLOW_ANGLES"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_SENSOR_AIRFLOW_ANGLES; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + if (_airflow_aoa_sub.advertised() || _airflow_slip_sub.advertised()) { + return MAVLINK_MSG_ID_SENSOR_AIRFLOW_ANGLES_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + + return 0; + } + +private: + explicit MavlinkStreamSensorAirflowAngles(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::Subscription _airflow_aoa_sub{ORB_ID(airflow_aoa)}; + uORB::Subscription _airflow_slip_sub{ORB_ID(airflow_slip)}; + + bool send() override + { + if (_airflow_aoa_sub.updated() || _airflow_slip_sub.updated()) { + mavlink_sensor_airflow_angles_t msg{}; + + struct airflow_aoa_s airflow_aoa; + + if (_airflow_aoa_sub.copy(&airflow_aoa)) { + msg.timestamp = airflow_aoa.timestamp / 1000; + msg.angleofattack = math::degrees(airflow_aoa.aoa_rad); + msg.angleofattack_valid = airflow_aoa.valid; + + } else { + msg.timestamp = 0; + msg.angleofattack = 0.0; + msg.angleofattack_valid = false; + } + + struct airflow_slip_s airflow_slip; + + if (_airflow_slip_sub.copy(&airflow_slip)) { + const uint64_t timestamp = airflow_slip.timestamp / 1000; + + if (timestamp > msg.timestamp) { + msg.timestamp = timestamp; + } + + msg.sideslip = math::degrees(airflow_slip.slip_rad); + msg.sideslip_valid = airflow_slip.valid; + + } else { + msg.timestamp = 0; + msg.sideslip = 0.0; + msg.sideslip_valid = false; + } + + mavlink_msg_sensor_airflow_angles_send_struct(_mavlink->get_channel(), &msg); + + return true; + } + + return false; + } +}; + +#endif // SENSOR_AIRFLOW_ANGLES diff --git a/src/modules/sensors/sensor_params_av.c b/src/modules/sensors/sensor_params_av.c new file mode 100644 index 0000000000..c2a4d766f4 --- /dev/null +++ b/src/modules/sensors/sensor_params_av.c @@ -0,0 +1,226 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * Driver ID of the angle of attack vane (corresponds to sensor I2C address) + * + * setting -1 disables the vane, 0-3 correspond to hall driver IDs + * + * @min -1 + * @max 3 + * @reboot_required true + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_AV_AOA_ID, -1); + +/** + * Angle of attack vane angular mounting offset in degrees + * + * This value is subtracted (as a bias) from the measured angle of attack + * + * @unit deg + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_AV_AOA_OFF, 0.0); + +/** + * Minimum calibrated angle of attack vane angle in degrees + * + * @unit deg + * @min -90.0 + * @max -1.0 + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_AV_AOA_MIN, -30.0); + +/** + * Maximum calibrated angle of attack vane angle in degrees + * + * @unit deg + * @min 1.0 + * @max 90.0 + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_AV_AOA_MAX, 30.0); + +/** + * Reverse vane direction (mounting dependent) + * + * @min -1.0 + * @max 1.0 + * @value -1.0 Reverse + * @value 1.0 Normal + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_AV_AOA_REV, 1.0); + +/** + * Calibrated polynomial coefficient 0 (*1e7) + * + * y = p0 + p1 * x + p2 * x^2 + ... + * y = vane angle in degrees + * x = magnetic field measurement in Tesla (assumes use of hall effect sensor) + * + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_AV_AOA_P0, 0); + +/** + * Calibrated polynomial coefficient 1 (*1e7) + * + * y = p0 + p1 * x + p2 * x^2 + ... + * y = angle in degrees + * x = magnetic field measurement in Tesla (assumes use of hall effect sensor) + * + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_AV_AOA_P1, 0); + +/** + * Calibrated polynomial coefficient 2 (*1e7) + * + * y = p0 + p1 * x + p2 * x^2 + ... + * y = angle in degrees + * x = magnetic field measurement in Tesla (assumes use of hall effect sensor) + * + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_AV_AOA_P2, 0); + +/** + * Calibrated polynomial coefficient 3 (*1e7) + * + * y = p0 + p1 * x + p2 * x^2 + ... + * y = angle in degrees + * x = magnetic field measurement in Tesla (assumes use of hall effect sensor) + * + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_AV_AOA_P3, 0); + +/** + * Driver ID of the sideslip vane (corresponds to sensor I2C address) + * + * setting -1 disables the vane, 0-3 correspond to hall driver IDs + * + * @min -1 + * @max 3 + * @reboot_required true + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_AV_SLIP_ID, -1); + +/** + * Sideslip vane angular mounting offset in degrees + * + * This value is subtracted (as a bias) from the measured sideslip + * + * @unit deg + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_AV_SLIP_OFF, 0.0); + +/** + * Minimum calibrated sideslip vane angle in degrees + * + * @unit deg + * @min -90.0 + * @max -1.0 + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_AV_SLIP_MIN, -30.0); + +/** + * Maximum calibrated sideslip vane angle in degrees + * + * @unit deg + * @min 1.0 + * @max 90.0 + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_AV_SLIP_MAX, 30.0); + +/** + * Reverse vane direction (mounting dependent) + * + * @min -1.0 + * @max 1.0 + * @value -1.0 Reverse + * @value 1.0 Normal + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(CAL_AV_SLIP_REV, 1.0); + +/** + * Calibrated polynomial coefficient 0 (*1e7) + * + * y = p0 + p1 * x + p2 * x^2 + ... + * y = vane angle in degrees + * x = magnetic field measurement in Tesla (assumes use of hall effect sensor) + * + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_AV_SLIP_P0, 0); + +/** + * Calibrated polynomial coefficient 1 (*1e7) + * + * y = p0 + p1 * x + p2 * x^2 + ... + * y = angle in degrees + * x = magnetic field measurement in Tesla (assumes use of hall effect sensor) + * + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_AV_SLIP_P1, 0); + +/** + * Calibrated polynomial coefficient 2 (*1e7) + * + * y = p0 + p1 * x + p2 * x^2 + ... + * y = angle in degrees + * x = magnetic field measurement in Tesla (assumes use of hall effect sensor) + * + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_AV_SLIP_P2, 0); + +/** + * Calibrated polynomial coefficient 3 (*1e7) + * + * y = p0 + p1 * x + p2 * x^2 + ... + * y = angle in degrees + * x = magnetic field measurement in Tesla (assumes use of hall effect sensor) + * + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_AV_SLIP_P3, 0); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 8cf3faa1f8..df6b8a46d3 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -61,10 +61,13 @@ #include #include #include +#include +#include #include #include #include #include +#include #include #include #include @@ -129,9 +132,20 @@ private: uORB::Subscription _diff_pres_sub{ORB_ID(differential_pressure)}; uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)}; uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)}; + uORB::Subscription _sensor_hall_subs[MAX_SENSOR_COUNT] { // could be set to ORB_MULTI_MAX_INSTANCES if needed + {ORB_ID(sensor_hall), 0}, + {ORB_ID(sensor_hall), 1}, + {ORB_ID(sensor_hall), 2}, + {ORB_ID(sensor_hall), 3}, + }; + + int _av_aoa_hall_sub_index = -1; // AoA vane index for hall effect subscription + int _av_slip_hall_sub_index = -1; // Slip vane index for hall effect subscription uORB::Publication _airspeed_pub{ORB_ID(airspeed)}; uORB::Publication _sensor_pub{ORB_ID(sensor_combined)}; + uORB::Publication _airflow_aoa_pub{ORB_ID(airflow_aoa)}; + uORB::Publication _airflow_slip_pub{ORB_ID(airflow_slip)}; perf_counter_t _loop_perf; /**< loop performance counter */ @@ -156,6 +170,26 @@ private: int32_t air_cmodel; float air_tube_length; float air_tube_diameter_mm; + + int32_t CAL_AV_AOA_ID; + float CAL_AV_AOA_OFF; + float CAL_AV_AOA_MIN; + float CAL_AV_AOA_MAX; + float CAL_AV_AOA_REV; + float CAL_AV_AOA_P0; + float CAL_AV_AOA_P1; + float CAL_AV_AOA_P2; + float CAL_AV_AOA_P3; + + int32_t CAL_AV_SLIP_ID; + float CAL_AV_SLIP_OFF; + float CAL_AV_SLIP_MIN; + float CAL_AV_SLIP_MAX; + float CAL_AV_SLIP_REV; + float CAL_AV_SLIP_P0; + float CAL_AV_SLIP_P1; + float CAL_AV_SLIP_P2; + float CAL_AV_SLIP_P3; } _parameters{}; /**< local copies of interesting parameters */ struct ParameterHandles { @@ -167,6 +201,26 @@ private: param_t air_cmodel; param_t air_tube_length; param_t air_tube_diameter_mm; + + param_t CAL_AV_AOA_ID; + param_t CAL_AV_AOA_OFF; + param_t CAL_AV_AOA_MIN; + param_t CAL_AV_AOA_MAX; + param_t CAL_AV_AOA_REV; + param_t CAL_AV_AOA_P0; + param_t CAL_AV_AOA_P1; + param_t CAL_AV_AOA_P2; + param_t CAL_AV_AOA_P3; + + param_t CAL_AV_SLIP_ID; + param_t CAL_AV_SLIP_OFF; + param_t CAL_AV_SLIP_MIN; + param_t CAL_AV_SLIP_MAX; + param_t CAL_AV_SLIP_REV; + param_t CAL_AV_SLIP_P0; + param_t CAL_AV_SLIP_P1; + param_t CAL_AV_SLIP_P2; + param_t CAL_AV_SLIP_P3; } _parameter_handles{}; /**< handles for interesting parameters */ VotedSensorsUpdate _voted_sensors_update; @@ -205,6 +259,16 @@ private: */ void adc_poll(); + /** + * Poll the hall effect sensor for updated data. + */ + void hall_poll(); + + /** + * Get the hall sensor subscription index for a given driver id. + */ + int getHallSubIndex(const int av_driver_id); + void InitializeVehicleAirData(); void InitializeVehicleGPSPosition(); void InitializeVehicleIMU(); @@ -233,6 +297,25 @@ Sensors::Sensors(bool hil_enabled) : _parameter_handles.air_cmodel = param_find("CAL_AIR_CMODEL"); _parameter_handles.air_tube_length = param_find("CAL_AIR_TUBELEN"); _parameter_handles.air_tube_diameter_mm = param_find("CAL_AIR_TUBED_MM"); + _parameter_handles.CAL_AV_AOA_ID = param_find("CAL_AV_AOA_ID"); + _parameter_handles.CAL_AV_AOA_OFF = param_find("CAL_AV_AOA_OFF"); + _parameter_handles.CAL_AV_AOA_MIN = param_find("CAL_AV_AOA_MIN"); + _parameter_handles.CAL_AV_AOA_MAX = param_find("CAL_AV_AOA_MAX"); + _parameter_handles.CAL_AV_AOA_REV = param_find("CAL_AV_AOA_REV"); + _parameter_handles.CAL_AV_AOA_P0 = param_find("CAL_AV_AOA_P0"); + _parameter_handles.CAL_AV_AOA_P1 = param_find("CAL_AV_AOA_P1"); + _parameter_handles.CAL_AV_AOA_P2 = param_find("CAL_AV_AOA_P2"); + _parameter_handles.CAL_AV_AOA_P3 = param_find("CAL_AV_AOA_P3"); + + _parameter_handles.CAL_AV_SLIP_ID = param_find("CAL_AV_SLIP_ID"); + _parameter_handles.CAL_AV_SLIP_OFF = param_find("CAL_AV_SLIP_OFF"); + _parameter_handles.CAL_AV_SLIP_MIN = param_find("CAL_AV_SLIP_MIN"); + _parameter_handles.CAL_AV_SLIP_MAX = param_find("CAL_AV_SLIP_MAX"); + _parameter_handles.CAL_AV_SLIP_REV = param_find("CAL_AV_SLIP_REV"); + _parameter_handles.CAL_AV_SLIP_P0 = param_find("CAL_AV_SLIP_P0"); + _parameter_handles.CAL_AV_SLIP_P1 = param_find("CAL_AV_SLIP_P1"); + _parameter_handles.CAL_AV_SLIP_P2 = param_find("CAL_AV_SLIP_P2"); + _parameter_handles.CAL_AV_SLIP_P3 = param_find("CAL_AV_SLIP_P3"); param_find("SYS_FAC_CAL_MODE"); @@ -306,6 +389,35 @@ int Sensors::parameters_update() param_get(_parameter_handles.air_tube_length, &_parameters.air_tube_length); param_get(_parameter_handles.air_tube_diameter_mm, &_parameters.air_tube_diameter_mm); + param_get(_parameter_handles.CAL_AV_AOA_ID, &_parameters.CAL_AV_AOA_ID); + param_get(_parameter_handles.CAL_AV_AOA_OFF, &_parameters.CAL_AV_AOA_OFF); + param_get(_parameter_handles.CAL_AV_AOA_MIN, &_parameters.CAL_AV_AOA_MIN); + param_get(_parameter_handles.CAL_AV_AOA_MAX, &_parameters.CAL_AV_AOA_MAX); + param_get(_parameter_handles.CAL_AV_AOA_REV, &_parameters.CAL_AV_AOA_REV); + int32_t temp_param{0}; + param_get(_parameter_handles.CAL_AV_AOA_P0, &temp_param); + _parameters.CAL_AV_AOA_P0 = float(temp_param) * 1e-7f; + param_get(_parameter_handles.CAL_AV_AOA_P1, &temp_param); + _parameters.CAL_AV_AOA_P1 = float(temp_param) * 1e-7f; + param_get(_parameter_handles.CAL_AV_AOA_P2, &temp_param); + _parameters.CAL_AV_AOA_P2 = float(temp_param) * 1e-7f; + param_get(_parameter_handles.CAL_AV_AOA_P3, &temp_param); + _parameters.CAL_AV_AOA_P3 = float(temp_param) * 1e-7f; + + param_get(_parameter_handles.CAL_AV_SLIP_ID, &_parameters.CAL_AV_SLIP_ID); + param_get(_parameter_handles.CAL_AV_SLIP_OFF, &_parameters.CAL_AV_SLIP_OFF); + param_get(_parameter_handles.CAL_AV_SLIP_MIN, &_parameters.CAL_AV_SLIP_MIN); + param_get(_parameter_handles.CAL_AV_SLIP_MAX, &_parameters.CAL_AV_SLIP_MAX); + param_get(_parameter_handles.CAL_AV_SLIP_REV, &_parameters.CAL_AV_SLIP_REV); + param_get(_parameter_handles.CAL_AV_SLIP_P0, &temp_param); + _parameters.CAL_AV_SLIP_P0 = float(temp_param) * 1e-7f; + param_get(_parameter_handles.CAL_AV_SLIP_P1, &temp_param); + _parameters.CAL_AV_SLIP_P1 = float(temp_param) * 1e-7f; + param_get(_parameter_handles.CAL_AV_SLIP_P2, &temp_param); + _parameters.CAL_AV_SLIP_P2 = float(temp_param) * 1e-7f; + param_get(_parameter_handles.CAL_AV_SLIP_P3, &temp_param); + _parameters.CAL_AV_SLIP_P3 = float(temp_param) * 1e-7f; + _voted_sensors_update.parametersUpdate(); return PX4_OK; @@ -475,6 +587,108 @@ void Sensors::adc_poll() #endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */ } +void Sensors::hall_poll() +{ + // process aoa measurements if enabled and valid + if ((_av_aoa_hall_sub_index >= 0) && (_av_aoa_hall_sub_index < MAX_SENSOR_COUNT)) { + // A hall sensor with selected driver ID for the AoA vane has been found/set + struct sensor_hall_s sensor_hall; + + if (_sensor_hall_subs[_av_aoa_hall_sub_index].update(&sensor_hall)) { + airflow_aoa_s airflow_aoa; + airflow_aoa.timestamp = hrt_absolute_time(); + airflow_aoa.valid = false; + + float aoa_deg = _parameters.CAL_AV_AOA_REV * (_parameters.CAL_AV_AOA_P0 + + _parameters.CAL_AV_AOA_P1 * sensor_hall.mag_t + + _parameters.CAL_AV_AOA_P2 * sensor_hall.mag_t * sensor_hall.mag_t + + _parameters.CAL_AV_AOA_P3 * sensor_hall.mag_t * sensor_hall.mag_t * sensor_hall.mag_t); + + // check if aoa measurement is in sensor/calibration range + if (aoa_deg > _parameters.CAL_AV_AOA_MAX) { + airflow_aoa.aoa_rad = math::radians(_parameters.CAL_AV_AOA_MAX); + + } else if (aoa_deg >= _parameters.CAL_AV_AOA_MIN) { + airflow_aoa.aoa_rad = math::radians(aoa_deg - _parameters.CAL_AV_AOA_OFF); + airflow_aoa.valid = true; + + } else { + airflow_aoa.aoa_rad = math::radians(_parameters.CAL_AV_AOA_MIN); + } + + _airflow_aoa_pub.publish(airflow_aoa); + } + } + + + // process sideslip measurements if enabled and valid + if ((_av_slip_hall_sub_index >= 0) && (_av_slip_hall_sub_index < MAX_SENSOR_COUNT)) { + // A hall sensor with selected driver ID for the slip vane has been found/set + struct sensor_hall_s sensor_hall; + + if (_sensor_hall_subs[_av_slip_hall_sub_index].update(&sensor_hall)) { + airflow_slip_s airflow_slip; + airflow_slip.timestamp = hrt_absolute_time(); + airflow_slip.valid = false; + + float slip_deg = _parameters.CAL_AV_SLIP_REV * (_parameters.CAL_AV_SLIP_P0 + + _parameters.CAL_AV_SLIP_P1 * sensor_hall.mag_t + + _parameters.CAL_AV_SLIP_P2 * sensor_hall.mag_t * sensor_hall.mag_t + + _parameters.CAL_AV_SLIP_P3 * sensor_hall.mag_t * sensor_hall.mag_t * sensor_hall.mag_t); + + /* check if slip measurement is in sensor/calibration range */ + if (slip_deg > _parameters.CAL_AV_SLIP_MAX) { + airflow_slip.slip_rad = math::radians(_parameters.CAL_AV_SLIP_MAX); + + } else if (slip_deg >= _parameters.CAL_AV_SLIP_MIN) { + airflow_slip.slip_rad = math::radians(slip_deg - _parameters.CAL_AV_SLIP_OFF); + airflow_slip.valid = true; + + } else { + airflow_slip.slip_rad = math::radians(_parameters.CAL_AV_SLIP_MIN); + } + + _airflow_slip_pub.publish(airflow_slip); + } + } + + // finding the indices needs to be done after processing the measurements since we are copying the + // messages and that would mess with the updated flags of the subscribers + + // find the AoA hall sensor index if not set yet + if (_parameters.CAL_AV_AOA_ID >= 0 && _av_aoa_hall_sub_index < 0) { + _av_aoa_hall_sub_index = getHallSubIndex(_parameters.CAL_AV_AOA_ID); + + if (_av_aoa_hall_sub_index >= 0) { + PX4_INFO("AoA vane using hall sensor with driver ID %d", _parameters.CAL_AV_AOA_ID); + } + } + + // find the Slip hall sensor index if not set yet + if (_parameters.CAL_AV_SLIP_ID >= 0 && _av_slip_hall_sub_index < 0) { + _av_slip_hall_sub_index = getHallSubIndex(_parameters.CAL_AV_SLIP_ID); + + if (_av_slip_hall_sub_index >= 0) { + PX4_INFO("Slip vane using hall sensor with driver ID %d", _parameters.CAL_AV_SLIP_ID); + } + } +} + +int Sensors::getHallSubIndex(const int av_driver_id) +{ + for (unsigned i = 0; i < MAX_SENSOR_COUNT; i++) { + sensor_hall_s sensor_hall; + + if (_sensor_hall_subs[i].copy(&sensor_hall)) { + if (sensor_hall.instance == av_driver_id) { + return i; + } + } + } + + return -1; +} + void Sensors::InitializeVehicleAirData() { if (_param_sys_has_baro.get()) { @@ -600,6 +814,7 @@ void Sensors::Run() adc_poll(); diff_pres_poll(); + hall_poll(); if (_sensor_combined.timestamp != _sensor_combined_prev_timestamp) {