mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-25 08:36:08 +08:00
Merge pull request #17 from ethz-asl/feature/add_hall_effect_sensor
[WIP!!!] Feature/add hall effect sensor
This commit is contained in:
JaeyoungLim
GitHub
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -50,6 +50,7 @@ px4_add_board(
|
||||
roboclaw
|
||||
rpm
|
||||
safety_button
|
||||
si7210
|
||||
telemetry # all available telemetry drivers
|
||||
test_ppm
|
||||
tone_alarm
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
|
||||
@@ -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 <stdint.h>
|
||||
#include <sys/ioctl.h>
|
||||
|
||||
#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 */
|
||||
@@ -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 */
|
||||
|
||||
@@ -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 :
|
||||
@@ -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 <amir.melzer@mavt.ethz.ch>
|
||||
*
|
||||
* 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<uint8_t>(command >> 8);
|
||||
cmd[1] = static_cast<uint8_t>(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;
|
||||
}
|
||||
}
|
||||
@@ -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 <amir.melzer@mavt.ethz.ch>
|
||||
*
|
||||
* Driver for the SI7210 connected via I2C.
|
||||
*/
|
||||
|
||||
#ifndef SI7210_HPP_
|
||||
#define SI7210_HPP_
|
||||
|
||||
#include <drivers/drv_hall.h>
|
||||
#include <drivers/vane/vane.h>
|
||||
#include <math.h>
|
||||
#include <px4_platform_common/getopt.h>
|
||||
#include <px4_platform_common/module.h>
|
||||
#include <px4_platform_common/i2c_spi_buses.h>
|
||||
#include <uORB/topics/sensor_hall.h>
|
||||
|
||||
#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<SI7210>
|
||||
{
|
||||
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_ */
|
||||
@@ -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 <amir.melzer@mavt.ethz.ch>
|
||||
*/
|
||||
|
||||
|
||||
#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;
|
||||
}
|
||||
@@ -40,3 +40,4 @@ add_subdirectory(led)
|
||||
add_subdirectory(magnetometer)
|
||||
add_subdirectory(rangefinder)
|
||||
add_subdirectory(smbus)
|
||||
add_subdirectory(vane)
|
||||
|
||||
@@ -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)
|
||||
@@ -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 <simon@px4.io>
|
||||
* @author Lorenz Meier <lorenz@px4.io>
|
||||
*
|
||||
*/
|
||||
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <drivers/device/device.h>
|
||||
|
||||
#include <drivers/device/i2c.h>
|
||||
|
||||
#include <systemlib/err.h>
|
||||
#include <parameters/param.h>
|
||||
#include <perf/perf_counter.h>
|
||||
|
||||
#include <drivers/drv_airspeed.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/sensor_hall.h>
|
||||
|
||||
#include <drivers/vane/vane.h>
|
||||
|
||||
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);
|
||||
}
|
||||
}
|
||||
@@ -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 <string.h>
|
||||
#include <drivers/device/i2c.h>
|
||||
#include <drivers/drv_hall.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <px4_platform_common/defines.h>
|
||||
#include <perf/perf_counter.h>
|
||||
#include <uORB/topics/sensor_hall.h>
|
||||
#include <uORB/PublicationMulti.hpp>
|
||||
|
||||
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<sensor_hall_s> _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;
|
||||
};
|
||||
@@ -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 <v2.0/standard/mavlink.h>
|
||||
#include <v2.0/ASLUAV/mavlink.h>
|
||||
|
||||
__END_DECLS
|
||||
|
||||
|
||||
@@ -48,7 +48,7 @@
|
||||
#ifndef MAVLINK_FTP_UNIT_TEST
|
||||
#include "mavlink_main.h"
|
||||
#else
|
||||
#include <v2.0/standard/mavlink.h>
|
||||
#include <v2.0/ASLUAV/mavlink.h>
|
||||
#endif
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
@@ -46,7 +46,7 @@
|
||||
#ifndef MAVLINK_FTP_UNIT_TEST
|
||||
#include "mavlink_bridge_header.h"
|
||||
#else
|
||||
#include <v2.0/standard/mavlink.h>
|
||||
#include <v2.0/ASLUAV/mavlink.h>
|
||||
#endif
|
||||
|
||||
class MavlinkFtpTest;
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -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<MavlinkStreamScaledPressure3>(),
|
||||
#endif // SCALED_PRESSURE3
|
||||
#if defined(SENSOR_AIRFLOW_ANGLES_HPP)
|
||||
create_stream_list_item<MavlinkStreamSensorAirflowAngles>(),
|
||||
#endif // SENSOR_AIRFLOW_ANGLES
|
||||
#if defined(ACTUATOR_OUTPUT_STATUS_HPP)
|
||||
create_stream_list_item<MavlinkStreamActuatorOutputStatus>(),
|
||||
#endif // ACTUATOR_OUTPUT_STATUS_HPP
|
||||
|
||||
@@ -40,7 +40,7 @@
|
||||
#ifndef MAVLINK_FTP_UNIT_TEST
|
||||
#include "../mavlink_bridge_header.h"
|
||||
#else
|
||||
#include <v2.0/standard/mavlink.h>
|
||||
#include <v2.0/ASLUAV/mavlink.h>
|
||||
#endif
|
||||
#include "../mavlink_ftp.h"
|
||||
|
||||
|
||||
@@ -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 <uORB/topics/airflow_aoa.h>
|
||||
#include <uORB/topics/airflow_slip.h>
|
||||
|
||||
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
|
||||
@@ -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);
|
||||
@@ -61,10 +61,13 @@
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/SubscriptionCallback.hpp>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/airflow_aoa.h>
|
||||
#include <uORB/topics/airflow_slip.h>
|
||||
#include <uORB/topics/airspeed.h>
|
||||
#include <uORB/topics/differential_pressure.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/sensors_status_imu.h>
|
||||
#include <uORB/topics/sensor_hall.h>
|
||||
#include <uORB/topics/vehicle_air_data.h>
|
||||
#include <uORB/topics/vehicle_control_mode.h>
|
||||
#include <uORB/topics/vehicle_imu.h>
|
||||
@@ -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_s> _airspeed_pub{ORB_ID(airspeed)};
|
||||
uORB::Publication<sensor_combined_s> _sensor_pub{ORB_ID(sensor_combined)};
|
||||
uORB::Publication<airflow_aoa_s> _airflow_aoa_pub{ORB_ID(airflow_aoa)};
|
||||
uORB::Publication<airflow_slip_s> _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) {
|
||||
|
||||
|
||||
Reference in New Issue
Block a user