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/msg/CMakeLists.txt b/msg/CMakeLists.txt index 4c0634b0f1..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 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/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 index 50d72d00e7..1dc99f3c9c 100644 --- a/src/drivers/drv_hall.h +++ b/src/drivers/drv_hall.h @@ -40,7 +40,6 @@ #ifndef _DRV_HALL_H #define _DRV_HALL_H -#include #include #include @@ -53,7 +52,5 @@ #define HALL2_DEVICE_PATH "/dev/hall2" #define HALL3_DEVICE_PATH "/dev/hall3" -#include -#define si7210_report sensor_hall_s #endif /* _DRV_HALL_H */ diff --git a/src/drivers/si7210/CMakeLists.txt b/src/drivers/si7210/CMakeLists.txt index 4b6a2e5a6d..f4b744d51d 100644 --- a/src/drivers/si7210/CMakeLists.txt +++ b/src/drivers/si7210/CMakeLists.txt @@ -34,9 +34,11 @@ px4_add_module( MODULE drivers__si7210 MAIN si7210 COMPILE_FLAGS - -O0 SRCS + si7210_main.cpp si7210.cpp DEPENDS - px4_work_queue + 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 index c3351b1b72..7db8c27ff6 100644 --- a/src/drivers/si7210/si7210.cpp +++ b/src/drivers/si7210/si7210.cpp @@ -40,740 +40,7 @@ #include "si7210.h" -#include - -/** driver 'main' command */ -extern "C" { __EXPORT int si7210_main(int argc, char *argv[]); } - -namespace si7210 -{ - -SI7210 *g_dev_0; -SI7210 *g_dev_1; -SI7210 *g_dev_2; -SI7210 *g_dev_3; - -typedef struct { - float mag_T; - float temp_C; -} si7210_measurements_t; - -void start(bool, SI7210::Instance); -void test(bool, SI7210::Instance); -void reset(bool, SI7210::Instance); -void info(bool, SI7210::Instance); -void usage(); -int get_i2c_address(SI7210::Instance); -const char *get_path(SI7210::Instance); -SI7210 **get_g_dev_ptr(SI7210::Instance); - - -/** - * Start the driver. - * - * This function only returns if the driver is up and running - * or failed to detect the sensor. - */ -void start(bool external_bus, SI7210::Instance instance) -{ - if (!external_bus) { - PX4_ERR("Internal bus currently not supported"); - exit(1); - } - - int fd; - SI7210 **g_dev_ptr = get_g_dev_ptr(instance); - const char *path = get_path(instance); - - if (*g_dev_ptr != nullptr) { - /* if already started, the command still succeeded */ - PX4_ERR("driver instance %d at address 0x%x already started", instance, get_i2c_address(instance)); - exit(0); - } - - /* create the driver */ - if (external_bus) { -#if defined(PX4_I2C_BUS_EXPANSION) - *g_dev_ptr = new SI7210(PX4_I2C_BUS_EXPANSION, instance, path); -#else - PX4_ERR("External I2C not available"); - exit(0); -#endif - - } else { - PX4_ERR("Internal I2C not available"); - exit(0); - } - - if (*g_dev_ptr == nullptr) { - goto fail; - } - - - if (OK != (*g_dev_ptr)->init()) { - goto fail; - } - - /* set the poll rate to default, starts automatic data collection */ - fd = open(path, O_RDONLY); - - - if (fd < 0) { - goto fail; - } - - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - goto fail; - } - - close(fd); - - exit(0); -fail: - - if (*g_dev_ptr != nullptr) { - delete (*g_dev_ptr); - *g_dev_ptr = nullptr; - } - - PX4_ERR("driver instance %d start at address 0x%x failed", instance, get_i2c_address(instance)); - exit(1); -} - -void test(bool external_bus, SI7210::Instance instance) -{ - int fd = -1; - const char *path = get_path(instance); - - struct si7210_report p_report; - ssize_t sz; - - /* get the driver */ - fd = open(path, O_RDONLY); - - if (fd < 0) { - PX4_ERR("%s open failed (try 'si7210 start -a %d' if the driver is not running)", - path, instance); - exit(1); - } - - /* reset to Max polling rate*/ - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX) < 0) { - PX4_ERR("reset to Max polling rate"); - exit(1); - } - - /* do a simple demand read */ - sz = read(fd, &p_report, sizeof(p_report)); - - if (sz != sizeof(p_report)) { - PX4_ERR("immediate si7210 read failed"); - exit(1); - } - - PX4_INFO("single read"); - PX4_INFO("time: %lld", p_report.timestamp); - PX4_INFO("magnetic field: %10.4f", (double)p_report.mag_T); - PX4_INFO("temperature: %10.4f", (double)p_report.temp_C); - - PX4_INFO("PASS"); - exit(0); -} - -void -reset(bool external_bus, SI7210::Instance instance) -{ - const char *path = get_path(instance); - - int fd = open(path, O_RDONLY); - - if (fd < 0) { - PX4_ERR("%s open failed (try 'si7210 start -a %d' if the driver is not running)", - path, instance); - exit(1); - } - - if (ioctl(fd, SENSORIOCRESET, 0) < 0) { - PX4_ERR("driver reset failed"); - exit(1); - } - - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - PX4_ERR("driver poll restart failed"); - exit(1); - } - - exit(0); -} - -void -info(bool external_bus, SI7210::Instance instance) -{ - SI7210 **g_dev_ptr = get_g_dev_ptr(instance); - - if (*g_dev_ptr == nullptr) { - PX4_ERR("driver instance %d at address 0x%x not running", instance, get_i2c_address(instance)); - exit(1); - } - - printf("state @ %p\n", *g_dev_ptr); - (*g_dev_ptr)->print_info(); - - exit(0); -} - -void -usage() -{ - PX4_INFO("required:"); - PX4_INFO(" 'start', 'info', 'test', 'stop', 'reset' "); - PX4_INFO(" -i (sensor instance (0-3))"); - PX4_INFO("optional:"); - PX4_INFO(" -X (external bus)"); - PX4_INFO("example:"); - PX4_INFO(" si7210 start -i 0"); -} - -/* Get i2c address */ -int -get_i2c_address(SI7210::Instance instance) -{ - int i2c_address(0); - - switch (instance) { - case SI7210::Instance::ID_0: - i2c_address = SI7210_SLAVE_ADDRESS_0; - break; - - case SI7210::Instance::ID_1: - i2c_address = SI7210_SLAVE_ADDRESS_1; - break; - - case SI7210::Instance::ID_2: - i2c_address = SI7210_SLAVE_ADDRESS_2; - break; - - case SI7210::Instance::ID_3: - i2c_address = SI7210_SLAVE_ADDRESS_3; - break; - - default: - PX4_ERR("Invalid instance, defaulting to i2c address 0x%x", SI7210_SLAVE_ADDRESS_0); - } - - return i2c_address; -} - -const char * -get_path(SI7210::Instance instance) -{ - switch (instance) { - case SI7210::Instance::ID_0: - return HALL0_DEVICE_PATH; - - case SI7210::Instance::ID_1: - return HALL1_DEVICE_PATH; - - case SI7210::Instance::ID_2: - return HALL2_DEVICE_PATH; - - case SI7210::Instance::ID_3: - return HALL3_DEVICE_PATH; - - default: - PX4_ERR("Invalid instance, defaulting to instance 0"); - return HALL0_DEVICE_PATH; - } - -} - -SI7210 ** -get_g_dev_ptr(SI7210::Instance instance) -{ - switch (instance) { - case SI7210::Instance::ID_0: - return &g_dev_0; - - case SI7210::Instance::ID_1: - return &g_dev_1; - - case SI7210::Instance::ID_2: - return &g_dev_2; - - case SI7210::Instance::ID_3: - return &g_dev_3; - - default: - PX4_ERR("Invalid instance, defaulting to instance 0"); - return &g_dev_0; - } -} - -} // namespace si7210 - -SI7210 :: SI7210(int bus, SI7210::Instance instance, const char *path) : - I2C("SI7210", path, bus, si7210::get_i2c_address(instance), SI7210_BUS_SPEED), - _running(false), - _call_interval(0), - _reports(nullptr), - _collect_phase(false), - _hall_topic(nullptr), - _orb_class_instance(-1), - _sample_perf(perf_alloc(PC_ELAPSED, "si7210_read")), - _bad_transfers(perf_alloc(PC_COUNT, "si7210_bad_transfers")), - _good_transfers(perf_alloc(PC_COUNT, "si7210_good_transfers")), - _measure_perf(perf_alloc(PC_ELAPSED, "si7210_measure")), - _comms_errors(perf_alloc(PC_COUNT, "si7210_comms_errors")), - _duplicates(perf_alloc(PC_COUNT, "si7210_duplicates")), - _got_duplicate(false), - _instance(instance), - _params_sub(-1) -{ - _device_id.devid_s.devtype = DRV_HALL_DEVTYPE_SI7210; - - // work_cancel in the dtor will explode if we don't do this... - memset(&_work, 0, sizeof(_work)); - - // default scaling - initialize_parameter_handles(_parameter_handles); -} - -SI7210 :: ~SI7210() -{ - /* make sure we are truly inactive */ - stop(); - - /* free any existing reports */ - if (_reports != nullptr) { - delete _reports; - } - - if (_hall_topic != nullptr) { - orb_unadvertise(_hall_topic); - } - - /* delete the perf counter */ - perf_free(_sample_perf); - perf_free(_bad_transfers); - perf_free(_good_transfers); - perf_free(_measure_perf); - perf_free(_comms_errors); - perf_free(_duplicates); -} - -int SI7210::init() -{ - int ret = OK; - - /* do I2C init (and probe) first */ - ret = I2C::init(); - - /* if probe/setup failed, bail now */ - if (ret != OK) { - DEVICE_DEBUG("I2C setup failed"); - return ret; - } - - /* allocate basic report buffers */ - _reports = new ringbuffer::RingBuffer(2, sizeof(si7210_report)); - - if (_reports == nullptr) { - return ret; - } - - up_udelay(10000); - - if (collect()) { - return -EIO; - } - - /* advertise sensor topic, measure manually to initialize valid report */ - struct si7210_report prb; - _reports->get(&prb); - - /* measurement will have generated a report, publish */ - _hall_topic = orb_advertise_multi(ORB_ID(sensor_hall), &prb, &_orb_class_instance, ORB_PRIO_DEFAULT); - - if (_hall_topic == nullptr) { - PX4_WARN("ADVERT FAIL"); - } - - return ret; -} - -int -SI7210::probe() -{ - uint8_t reg; - - if (OK != get_regs(HREVID, ®)) { - perf_count(_comms_errors); - return -EIO; - } - - if (((reg & 0xf0) >> 4) != IDCHIPID) { - perf_count(_comms_errors); - return -EIO; - } - - if ((reg & 0x0f) != REVID) { - perf_count(_comms_errors); - return -EIO; - } - - return OK; -} - -void -SI7210::start() -{ - /* reset the report ring and state machine */ - _collect_phase = false; - _running = true; - _reports->flush(); - - /* schedule a cycle to start things */ - work_queue(HPWORK, &_work, (worker_t)&SI7210::cycle_trampoline, this, 1); -} - -void -SI7210::stop() -{ - _running = false; - work_cancel(HPWORK, &_work); -} - -ssize_t -SI7210::read(struct file *filp, char *buffer, size_t buflen) -{ - unsigned count = buflen / sizeof(si7210_report); - struct si7210_report *si7210_buf = reinterpret_cast(buffer); - int ret = 0; - - /* buffer must be large enough */ - if (count < 1) { - return -ENOSPC; - } - - /* if automatic measurement is enabled */ - if (_call_interval > 0) { - /* - * While there is space in the caller's buffer, and reports, copy them. - * Note that we may be pre-empted by the workq thread while we are doing this; - * we are careful to avoid racing with them. - */ - while (count--) { - if (_reports->get(si7210_buf)) { - ret += sizeof(struct si7210_report); - si7210_buf++; - } - } - - /* if there was no data, warn the caller */ - return ret ? ret : -EAGAIN; - } - - /* manual measurement - run one conversion */ - do { - _reports->flush(); - - /* wait for it to complete */ - usleep(SI7210_CONVERSION_INTERVAL); - - /* run the collection phase */ - if (OK != collect()) { - ret = -EIO; - break; - } - - - if (_reports->get(si7210_buf)) { - ret = sizeof(struct si7210_report); - } - } while (0); - - /* return the number of bytes transferred */ - return ret; - -} - -void -SI7210::cycle_trampoline(void *arg) -{ - SI7210 *dev = reinterpret_cast(arg); - - /* make measurement */ - dev->cycle(); -} - -void -SI7210::cycle() -{ - bool force_update = false; - bool param_updated = false; - - if (!_running) { - if (_params_sub >= 0) { - orb_unsubscribe(_params_sub); - } - - } else { - if (_params_sub < 0) { - _params_sub = orb_subscribe(ORB_ID(parameter_update)); - force_update = true; - } - } - - /* Check if any parameter changed */ - orb_check(_params_sub, ¶m_updated); - - if (param_updated || force_update) { - - struct parameter_update_s update; - orb_copy(ORB_ID(parameter_update), _params_sub, &update); - parameters_update(); - } - - if (_collect_phase) { - collect(); - unsigned wait_gap = _call_interval - USEC2TICK(SI7210_CONVERSION_INTERVAL); - - if ((wait_gap != 0) && (_running)) { - work_queue(HPWORK, &_work, (worker_t)&SI7210::cycle_trampoline, this, - wait_gap); //need to wait some time before new measurement - return; - } - - } - - measure(); - - if ((_running)) { - /* schedule a fresh cycle call when the measurement is done */ - work_queue(HPWORK, - &_work, - (worker_t)&SI7210::cycle_trampoline, - this, - USEC2TICK(SI7210_CONVERSION_INTERVAL)); - } -} - -int -SI7210::parameters_update() -{ - int ret = update_parameters(_parameter_handles, _parameters); - return ret; -} - -int -SI7210::measure() -{ - _collect_phase = true; - - return OK; -} - -int -SI7210::collect() -{ - _collect_phase = false; - bool si7210_notify = true; - si7210_report prb; - - uint8_t reg; - - uint16_t magRawData; - uint16_t tempRawData; - - int8_t otpTempOffsetData; - int8_t otpTempGainData; - - /* read the most recent measurement */ - perf_begin(_sample_perf); - - /* 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; - } - - /* generate a new report */ - prb.timestamp = hrt_absolute_time(); - prb.instance = _instance; - prb.mag_T = (float)(magRawData - MAG_BIAS) * MAG_CONV; - prb.temp_C = (float)((tempRawData & ~0x8000) >> 3); - prb.temp_C = _tempGain * (TEMP_CONV_A2 * prb.temp_C * prb.temp_C + TEMP_CONV_A1 * prb.temp_C + TEMP_CONV_A0 + VDD_CONV * - VDD) + _tempOffset; - - _reports->force(&prb); - - //PX4_INFO(": %.2f, %.2f", (double)prb.mag_T, (double)prb.temp_C); - - /* notify anyone waiting for data */ - if (si7210_notify) { - poll_notify(POLLIN); - } - - if (si7210_notify && !(_pub_blocked)) { - /* publish it */ - orb_publish(ORB_ID(sensor_hall), _hall_topic, &prb); - } - - perf_end(_sample_perf); - return OK; -} - -int -SI7210::ioctl(struct file *filp, int cmd, unsigned long arg) -{ - switch (cmd) { - case SENSORIOCSPOLLRATE: { - switch (arg) { - - /* switching to manual polling */ - case SENSOR_POLLRATE_MANUAL: - stop(); - _call_interval = 0; - return OK; - - /* external signalling (DRDY) not supported */ - case SENSOR_POLLRATE_EXTERNAL: - - /* zero would be bad */ - case 0: - return -EINVAL; - - /* set default/max polling rate */ - case SENSOR_POLLRATE_MAX: - case SENSOR_POLLRATE_DEFAULT: - return ioctl(filp, SENSORIOCSPOLLRATE, SI7210_MAX_DATA_RATE); - - /* adjust to a legal polling interval in Hz */ - default: { - /* do we need to start internal polling? */ - bool want_start = (_call_interval == 0); - - /* convert hz to tick interval via microseconds */ - unsigned ticks = USEC2TICK(1000000 / arg); - - /* check against maximum rate */ - if (ticks < USEC2TICK(SI7210_CONVERSION_INTERVAL)) { - return -EINVAL; - } - - /* update interval for next measurement */ - _call_interval = ticks; - - /* if we need to start the poll state machine, do it */ - if (want_start) { - start(); - } - - return OK; - } - } - } - - case SENSORIOCGPOLLRATE: - if (_call_interval == 0) { - return SENSOR_POLLRATE_MANUAL; - } - - return 1000000 / _call_interval; - - case SENSORIOCSQUEUEDEPTH: { - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 100)) { - return -EINVAL; - } - - irqstate_t flags = px4_enter_critical_section(); - - if (!_reports->resize(arg)) { - px4_leave_critical_section(flags); - return -ENOMEM; - } - - px4_leave_critical_section(flags); - - return OK; - } - - case SENSORIOCRESET: - return reset(); - - default: - /* give it to the superclass */ - return I2C::ioctl(filp, cmd, arg); - } -} - -int SI7210::reset() -{ - //ToDo: write a reset function - stop(); - return OK; -} - -int -SI7210::self_test() -{ - if (perf_event_count(_sample_perf) == 0) { - collect(); - } - - /* return 0 on success, 1 else */ - return (perf_event_count(_sample_perf) > 0) ? 0 : 1; -} +using namespace time_literals; /* Get registers value */ int @@ -874,87 +141,173 @@ SI7210::get_sensor_data(uint8_t otpAddr, int8_t *data) return OK; } -void -SI7210::print_info() + +int SI7210::write_command(uint16_t command) { - perf_print_counter(_sample_perf); - perf_print_counter(_bad_transfers); - perf_print_counter(_good_transfers); - _reports->print_info("si7210 queue"); + 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_main(int argc, char *argv[]) +SI7210::configure() { - const char *verb = argv[1]; + uint8_t reg; + int ret = get_regs(HREVID, ®); + bool config_failed = false; - bool external_bus = true; - SI7210::Instance instance = SI7210::Instance::INVALID; - int ch; - - int myoptind = 1; - const char *myoptarg = nullptr; - - /* jump over start/off/etc and look at options first */ - while ((ch = px4_getopt(argc, argv, "X:i:", &myoptind, &myoptarg)) != EOF) { - switch (ch) { - case 'X': - external_bus = true; - break; - - case 'i': - instance = SI7210::Instance(atoi(myoptarg)); - - // range check - if ((instance > 3) || (instance < 0)) { - PX4_ERR("invalid instance: %d (only 0 through 3 valid)", instance); - return PX4_ERROR; - } - - break; - - default: - PX4_ERR("unknown command line argument"); - si7210::usage(); - return PX4_ERROR; - } + if (((reg & 0xf0) >> 4) != IDCHIPID) { + config_failed = true; } - if (instance == SI7210::Instance::INVALID) { - PX4_ERR("the -i option is required"); - si7210::usage(); - exit(1); + if ((reg & 0x0f) != REVID) { + config_failed = true; } - /* - * Start/load the driver. - */ - if (!strcmp(verb, "start")) { - si7210::start(external_bus, instance); + if ((ret != PX4_OK) || config_failed) { + perf_count(_comms_errors); + DEVICE_DEBUG("config failed"); + _state = State::RequireConfig; + return ret; } - /* - * Test the driver/device. - */ - if (!strcmp(verb, "test")) { - si7210::test(external_bus, instance); - } + _state = State::Configuring; - /* - * Reset the driver. - */ - if (!strcmp(verb, "reset")) { - si7210::reset(external_bus, instance); - } - - /* - * Print driver information. - */ - if (!strcmp(verb, "info")) { - si7210::info(external_bus, instance); - } - - PX4_ERR("missing command"); - si7210::usage(); - exit(1); + 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 index 38e6cbe14b..c9ca59aa7c 100644 --- a/src/drivers/si7210/si7210.h +++ b/src/drivers/si7210/si7210.h @@ -41,55 +41,15 @@ #ifndef SI7210_HPP_ #define SI7210_HPP_ -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include -#include - -#include -#include - -#include -#include - -#include -#include -#include #include -#include +#include +#include +#include +#include +#include +#include -#include "parameters.h" - -using namespace si7210; - -#define SI7210_BUS PX4_I2C_BUS_EXPANSION - -#define SI7210_SLAVE_ADDRESS_0 0x30 /* SI7210 I2C address */ -#define SI7210_SLAVE_ADDRESS_1 0x31 /* SI7210 I2C address */ -#define SI7210_SLAVE_ADDRESS_2 0x32 /* SI7210 I2C address */ -#define SI7210_SLAVE_ADDRESS_3 0x33 /* SI7210 I2C address */ +#define I2C_ADDRESS_SI7210 0x30 /* Default SI7210 I2C address */ #define SI7210_MAX_DATA_RATE 50 @@ -149,104 +109,40 @@ using namespace si7210; #define MAG_BIAS 0xC000 #define MAG_CONV 0.00125F -class SI7210 : public device::I2C +class SI7210 : public Vane, public I2CSPIDriver { public: - enum Instance : int8_t { - INVALID = -1, - ID_0 = 0, - ID_1 = 1, - ID_2 = 2, - ID_3 = 3, - }; + 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} + { + } - SI7210(int bus, SI7210::Instance instance, const char *path); - virtual ~SI7210(); + virtual ~SI7210() = default; - virtual int init(); - virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance); + static void print_usage(); - /** - * Stop automatic measurement. - */ - void stop(); + void RunImpl(); - /** - * Diagnostics - print some basic information about the driver. - */ - void print_info(); - -protected: - virtual int probe(); + void start(); private: - work_s _work{}; + enum class State { + RequireConfig, + Configuring, + Running + }; - bool _running; + int measure() override { return 0; } + int collect() override; + int configure(); - /* altitude conversion calibration */ - unsigned _call_interval; - - si7210_report _report {}; - ringbuffer::RingBuffer *_reports; - - bool _collect_phase; - - orb_advert_t _hall_topic; - int _orb_class_instance; - - perf_counter_t _sample_perf; - perf_counter_t _bad_transfers; - perf_counter_t _good_transfers; - perf_counter_t _measure_perf; - perf_counter_t _comms_errors; - perf_counter_t _duplicates; - - bool _got_duplicate; - - Instance _instance; /**< index of the i2c address and publisher instance */ - - int _params_sub{-1}; /**< notification of parameter updates */ - - si7210_report _last_report {}; /**< used for info() */ - - Parameters _parameters{}; /**< local copies of interesting parameters */ - ParameterHandles _parameter_handles{}; /**< handles for interesting parameters */ - - /** - * Start automatic measurement. - */ - void start(); - - int measure(); //start measure - int collect(); //get results and publish - int parameters_update(); // update parameters - - static void cycle_trampoline(void *arg); - void cycle(); //main execution - - /** - * Read the specified number of bytes from SI7210. - * - * @param reg The register to read. - * @param data Pointer to buffer for bytes read. - * @param len Number of bytes to read - * @return OK if the transfer was successful, -errno otherwise. - */ - int get_data(uint8_t reg, uint8_t *data, unsigned len); - - /** - * Resets the chip. - */ - int reset(); - - /** - * Measurement self test - * - * @return 0 on success, 1 on failure - */ - int self_test(); + bool init_si7210(); /** * Get registers values @@ -276,9 +172,15 @@ private: */ int get_sensor_data(uint8_t otpAddr, int8_t *data); - /* do not allow to copy this class due to pointer data members */ - SI7210(const SI7210 &); - SI7210 operator=(const SI7210 &); + /** + * 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) {