From aedbb6615a994e2dd2c1a973c3474bb9d063a224 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Tue, 6 Mar 2018 01:58:12 +0100 Subject: [PATCH 1/3] pozyx: added pozyx serial driver for receiving position messages from an adruino pozyx: read all defined messages and check checksum, start from scratch if id is unknown --- ROMFS/px4fmu_common/init.d/rc.sensors | 18 + Tools/test_pozyx.py | 57 +++ cmake/configs/nuttx_px4fmu-v3_default.cmake | 1 + msg/CMakeLists.txt | 1 + msg/pozyx_report.msg | 11 + src/drivers/drv_pozyx.h | 82 +++ src/drivers/pozyx/CMakeLists.txt | 42 ++ src/drivers/pozyx/parameters.c | 42 ++ src/drivers/pozyx/pozyx.cpp | 535 ++++++++++++++++++++ 9 files changed, 789 insertions(+) create mode 100755 Tools/test_pozyx.py create mode 100644 msg/pozyx_report.msg create mode 100644 src/drivers/drv_pozyx.h create mode 100644 src/drivers/pozyx/CMakeLists.txt create mode 100644 src/drivers/pozyx/parameters.c create mode 100644 src/drivers/pozyx/pozyx.cpp diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index bc7787e12990..1ea5104307e9 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -437,6 +437,24 @@ then fi fi +if param compare SENS_EN_POZYX 2 +then + if pozyx start /dev/ttyS2 + then + fi +fi + +if param compare SENS_EN_POZYX 4 +then + # Serial 4 on FMUv2 + if ver hwcmp PX4FMU_V2 + then + if pozyx start /dev/ttyS6 + then + fi + fi +fi + # Wait 20 ms for sensors (because we need to wait for the HRT and work queue callbacks to fire) usleep 20000 sensors start diff --git a/Tools/test_pozyx.py b/Tools/test_pozyx.py new file mode 100755 index 000000000000..ca4cee99dd11 --- /dev/null +++ b/Tools/test_pozyx.py @@ -0,0 +1,57 @@ +#!/usr/bin/env python +############################################################################ +# +# Copyright (C) 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. +# +############################################################################ + + +# for python2.7 compatibility +from __future__ import print_function + +import sys +import serial +import struct +import time + +ser = serial.Serial('/dev/tty.usbserial-AH03H0W7', baudrate=115200) + +data = struct.pack(" + **/ + +#pragma once + +#include +#include + +#include "drv_sensor.h" // include sensor driver interfaces + +#define POZYX_BASE_DEVICE_PATH "/dev/pozyx" +#define POZYX0_DEVICE_PATH "/dev/pozyx0" + +#define POZYX_MSG_HEADER 0x01 +#define POZYX_MSGID_BEACON_CONFIG 0x02 +#define POZYX_MSGID_BEACON_DIST 0x03 +#define POZYX_MSGID_POSITION 0x04 + +#define POZYX_READ_LEN 8 +// buffer needs to be as large as the biggest package (18) plus POZYX_READ_LEN +#define POZYX_BUF_LEN 26 + +/* + * Messages used by the Arduino sketch to send data + */ +struct pozyx_beacon_config_s { + uint8_t beacon_id; + uint8_t beacon_count; + int32_t x; + int32_t y; + int32_t z; +}; + +struct pozyx_distance_s { + uint8_t beacon_id; + uint32_t distance; +}; + +struct pozyx_position_s { + int32_t x; + int32_t y; + int32_t z; + int16_t position_error; +}; diff --git a/src/drivers/pozyx/CMakeLists.txt b/src/drivers/pozyx/CMakeLists.txt new file mode 100644 index 000000000000..f6a97b6483c9 --- /dev/null +++ b/src/drivers/pozyx/CMakeLists.txt @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2016 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__pozyx + MAIN pozyx + COMPILE_FLAGS + SRCS + pozyx.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/pozyx/parameters.c b/src/drivers/pozyx/parameters.c new file mode 100644 index 000000000000..8e753b38ac75 --- /dev/null +++ b/src/drivers/pozyx/parameters.c @@ -0,0 +1,42 @@ +/**************************************************************************** + * + * Copyright (c) 2017 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. + * + ****************************************************************************/ + +/** + * Pozyx telemetry port + * + * @value -1 disabled + * @value 2 Telemetry 2 + * @value 4 Serial 4 + * @group Sensor Enable + */ +PARAM_DEFINE_INT32(SENS_EN_POZYX, -1); diff --git a/src/drivers/pozyx/pozyx.cpp b/src/drivers/pozyx/pozyx.cpp new file mode 100644 index 000000000000..564cc9c4c0b6 --- /dev/null +++ b/src/drivers/pozyx/pozyx.cpp @@ -0,0 +1,535 @@ +/**************************************************************************** + * + * Copyright (c) 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 pozyx.cpp + * @author Andreas Antener + * + * Driver for connecting the Pozyx localization system (1) via an Arduino + * over serial. The Arduino is expected to run the referenced sketch from APM (2)(3). + * + * 1) https://www.pozyx.io/ + * 2) http://ardupilot.org/copter/docs/common-pozyx.html + * 3) https://github.com/ArduPilot/ardupilot/blob/master/Tools/Pozyx/IndoorLoiter/IndoorLoiter.ino + */ + +#include +#include +#include +#include +#include + +#include + +#define DEFAULT_PORT "/dev/ttyS2" // telem2 on Pixhawk + +extern "C" __EXPORT int pozyx_main(int argc, char *argv[]); + +class Pozyx : public device::CDev +{ +public: + Pozyx(const char *port = DEFAULT_PORT); + virtual ~Pozyx(); + + virtual int init(); + + int start(); + +private: + bool _task_should_exit; + int _task_handle; + char _port[20]; + int _class_instance; + int _orb_class_instance; + orb_advert_t _pozyx_report_topic; + + unsigned _head; + unsigned _tail; + uint8_t _buf[POZYX_BUF_LEN]; + + enum ParsingState { + POZYX_HEADER = 0, + POZYX_ID, + POZYX_LEN, + POZYX_MSG, + } _state; + + uint8_t _msg_id; + uint8_t _msg_len; + uint8_t _msg_chksum; + + static void task_main_trampoline(int argc, char *argv[]); + void task_main(); + + uint8_t get_next_index(uint8_t current); + uint8_t get_next_byte(); + bool read_and_parse(uint8_t *buf, int len); + void send_pozyx_report(struct pozyx_position_s &pozyx_position); +}; + +namespace pozyx +{ +Pozyx *g_dev; +} + +Pozyx::Pozyx(const char *port) : + CDev("Pozyx", POZYX0_DEVICE_PATH), + _task_should_exit(false), + _task_handle(-1), + _class_instance(-1), + _orb_class_instance(-1), + _pozyx_report_topic(nullptr), + _head(0), + _tail(0), + _state(POZYX_HEADER), + _msg_id(0), + _msg_len(0), + _msg_chksum(0) +{ + /* store port name */ + strncpy(_port, port, sizeof(_port)); + /* enforce null termination */ + _port[sizeof(_port) - 1] = '\0'; + + // disable debug() calls + _debug_enabled = false; + + memset(&_buf[0], 0, sizeof(_buf)); +} + +Pozyx::~Pozyx() +{ + + if (_class_instance != -1) { + unregister_class_devname(POZYX_BASE_DEVICE_PATH, _class_instance); + } + + if (_task_handle != -1) { + /* task wakes up every 100ms or so at the longest */ + _task_should_exit = true; + + /* wait for a second for the task to quit at our request */ + unsigned i = 0; + + do { + /* wait 20ms */ + usleep(20000); + + /* if we have given up, kill it */ + if (++i > 50) { + px4_task_delete(_task_handle); + break; + } + } while (_task_handle != -1); + } + + if (_class_instance != -1) { + unregister_class_devname(POZYX_BASE_DEVICE_PATH, _class_instance); + } +} + +int +Pozyx::init() +{ + /* status */ + int ret = 0; + + do { /* create a scope to handle exit conditions using break */ + + /* do regular cdev init */ + ret = CDev::init(); + + if (ret != OK) { + PX4_WARN("cdev init failed"); + break; + } + + int fd = px4_open(POZYX0_DEVICE_PATH, 0); + + if (fd < 0) { + PX4_WARN("failed to open range finder device"); + ret = 1; + break; + } + + px4_close(fd); + + /* open fd */ + fd = px4_open(_port, O_RDWR | O_NOCTTY); + + if (fd < 0) { + PX4_WARN("failed to open serial device"); + ret = 1; + break; + } + + struct termios uart_config; + + int termios_state; + + /* fill the struct for the new configuration */ + tcgetattr(fd, &uart_config); + + /* clear ONLCR flag (which appends a CR for every LF) */ + uart_config.c_oflag &= ~ONLCR; + + /* no parity, one stop bit */ + uart_config.c_cflag &= ~(CSTOPB | PARENB); + + unsigned speed = B115200; + + /* set baud rate */ + if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) { + PX4_WARN("ERR CFG: %d ISPD", termios_state); + ret = 1; + break; + } + + if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) { + PX4_WARN("ERR CFG: %d OSPD\n", termios_state); + ret = 1; + break; + } + + if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) { + PX4_WARN("ERR baud %d ATTR", termios_state); + ret = 1; + break; + } + + px4_close(fd); + + _class_instance = register_class_devname(POZYX_BASE_DEVICE_PATH); + + struct pozyx_report_s pozyx_report = {}; + pozyx_report.timestamp = hrt_absolute_time(); + + _pozyx_report_topic = orb_advertise_multi(ORB_ID(pozyx_report), &pozyx_report, + &_orb_class_instance, ORB_PRIO_HIGH); + + if (_pozyx_report_topic == nullptr) { + DEVICE_LOG("failed to create pozyx_report object. Did you start uOrb?"); + ret = 1; + break; + } + + } while (0); + + return ret; +} + +void +Pozyx::task_main_trampoline(int argc, char *argv[]) +{ + pozyx::g_dev->task_main(); +} + +int +Pozyx::start() +{ + ASSERT(_task_handle == -1); + + /* start the task */ + _task_handle = px4_task_spawn_cmd("pozyx", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 30, + 800, + (px4_main_t)&Pozyx::task_main_trampoline, + nullptr); + + if (_task_handle < 0) { + PX4_WARN("task start failed"); + return -errno; + } + + return OK; +} + +uint8_t +Pozyx::get_next_index(uint8_t current) { + uint8_t ret = current + 1; + + if (ret == POZYX_BUF_LEN) { + ret = 0; + } + + return ret; +} + +uint8_t +Pozyx::get_next_byte() { + _tail = get_next_index(_tail); + return _buf[_tail]; +} + +bool +Pozyx::read_and_parse(uint8_t *buf, int len) +{ + // write new data into a ring buffer + for (int i = 0; i < len; i++) { + _head++; + + if (_head >= POZYX_BUF_LEN) { + _head = 0; + } + + if (_tail == _head) { + _tail = (_tail == POZYX_BUF_LEN - 1) ? 0 : _head + 1; + } + + _buf[_head] = buf[i]; + } + + // check how many bytes are in the buffer + int num_bytes = _head >= _tail ? (_head - _tail) : (_head + POZYX_BUF_LEN - _tail); + //PX4_WARN("read len %d, h %d, t %d, n %d", len, _head, _tail, num_bytes); + + for (; num_bytes > 0; num_bytes--) { + switch (_state) { + case ParsingState::POZYX_HEADER: { + uint8_t c = get_next_byte(); + //PX4_WARN("header %d: %d", _tail, c); + if (c == POZYX_MSG_HEADER) { + _msg_chksum = 0; + _state = ParsingState::POZYX_ID; + } + break; + } + + case ParsingState::POZYX_ID: + _msg_id = get_next_byte(); + //PX4_WARN("id %d", _msg_id); + + if (_msg_id == POZYX_MSGID_POSITION || + _msg_id == POZYX_MSGID_BEACON_CONFIG || + _msg_id == POZYX_MSGID_BEACON_DIST) { + _msg_chksum ^= _msg_id; + _state = ParsingState::POZYX_LEN; + + } else { + // not supported message + _state = ParsingState::POZYX_HEADER; + } + break; + + case ParsingState::POZYX_LEN: + _msg_len = get_next_byte(); + //PX4_WARN("len %d", _msg_len); + + if (_msg_len > 1) { + // has data and checksum + _msg_chksum ^= _msg_len; + _state = ParsingState::POZYX_MSG; + + } else { + // looks invalid + _state = ParsingState::POZYX_HEADER; + } + break; + + case ParsingState::POZYX_MSG: + //PX4_WARN("msg"); + // we want to be able to read the complete message once we know what we're looking for + if (num_bytes < _msg_len) { + return false; + } + + // start from the beginning after this, no matter if we can parse or not + _state = ParsingState::POZYX_HEADER; + + // verify checksum + int index = _tail; + for (; _msg_len > 1; _msg_len--) { + index = get_next_index(index); + _msg_chksum ^= _buf[index]; + } + + index = get_next_index(index); + if (_msg_chksum != _buf[index]) { + // failed + //PX4_WARN("wrong checksum"); + _tail = index; + return false; + } + + // consume + if (_msg_id == POZYX_MSGID_POSITION) { + struct pozyx_position_s pozyx_position = {}; + pozyx_position.x = (uint32_t)get_next_byte() << 0 | + (uint32_t)get_next_byte() << 8 | + (uint32_t)get_next_byte() << 16 | + (uint32_t)get_next_byte() << 24; + + pozyx_position.y = (uint32_t)get_next_byte() << 0 | + (uint32_t)get_next_byte() << 8 | + (uint32_t)get_next_byte() << 16 | + (uint32_t)get_next_byte() << 24; + + pozyx_position.z = (uint32_t)get_next_byte() << 0 | + (uint32_t)get_next_byte() << 8 | + (uint32_t)get_next_byte() << 16 | + (uint32_t)get_next_byte() << 24; + + pozyx_position.position_error = (uint32_t)get_next_byte() << 0 | + (uint32_t)get_next_byte() << 8; + send_pozyx_report(pozyx_position); + } + + // skip checksum + _tail = get_next_index(_tail); + + return true; + + } + } + + return false; +} + +void +Pozyx::send_pozyx_report(struct pozyx_position_s &pozyx_position) { + struct pozyx_report_s pozyx_report = {}; + pozyx_report.timestamp = hrt_absolute_time(); + pozyx_report.pos_x = 0.001f * pozyx_position.x; + pozyx_report.pos_y = 0.001f * pozyx_position.y; + pozyx_report.pos_z = 0.001f * pozyx_position.z; + pozyx_report.cov_xy = pozyx_position.position_error; + //PX4_WARN("pos %d %d %d", pozyx_report.cov_xy, pozyx_position.x, pozyx_position.y); + orb_publish(ORB_ID(pozyx_report), _pozyx_report_topic, &pozyx_report); +} + +void +Pozyx::task_main() +{ + int fd = px4_open(_port, O_RDWR | O_NOCTTY); + + // we poll on data from the serial port + px4_pollfd_struct_t fds[1]; + fds[0].fd = fd; + fds[0].events = POLLIN; + + uint8_t buf[POZYX_READ_LEN]; + + while (!_task_should_exit) { + // wait for up to 100ms for data + int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); + + // timed out + if (pret == 0) { + continue; + } + + if (pret < 0) { + PX4_DEBUG("Pozyx serial port poll error"); + // sleep a bit before next try + usleep(100000); + continue; + } + + if (fds[0].revents & POLLIN) { + memset(&buf[0], 0, sizeof(buf)); + int len = px4_read(fd, &buf[0], sizeof(buf)); + + if (len <= 0) { + PX4_DEBUG("error reading from Pozyx"); + } + + read_and_parse(&buf[0], len); + } + } + + px4_close(fd); +} + +int pozyx_main(int argc, char *argv[]) +{ + /* + * Start/load the driver. + */ + if (!strcmp(argv[1], "start")) { + if (pozyx::g_dev != nullptr) { + PX4_WARN("driver already started"); + return 0; + } + + if (argc > 2) { + pozyx::g_dev = new Pozyx(argv[2]); + + } else { + pozyx::g_dev = new Pozyx(DEFAULT_PORT); + } + + if (pozyx::g_dev == nullptr) { + PX4_ERR("failed to create instance of Pozyx"); + return 1; + } + + if (PX4_OK != pozyx::g_dev->init()) { + delete pozyx::g_dev; + pozyx::g_dev = nullptr; + return 1; + } + + if (OK != pozyx::g_dev->start()) { + delete pozyx::g_dev; + pozyx::g_dev = nullptr; + return 1; + } + + return 0; + } + + /* + * Stop the driver + */ + if (!strcmp(argv[1], "stop")) { + if (pozyx::g_dev != nullptr) { + delete pozyx::g_dev; + pozyx::g_dev = nullptr; + + } else { + PX4_WARN("driver not running"); + } + + return 0; + } + + if (!strcmp(argv[1], "info")) { + PX4_INFO("Pozyx"); + return 0; + + } + + PX4_WARN("unrecognized arguments, try: start [device_name], stop, info "); + return 1; +} From 04488d646d17a4b7ad938347b5accbb459ee4315 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Tue, 27 Feb 2018 16:21:09 +0100 Subject: [PATCH 2/3] landing_target_estimator: abstract sensor data and let irlock and pozyx feed data in landing_target_estimator: only initialize a new report when all data is there landing_target_estimator: added parameters to specify relative offset of sensor origin for landing target --- .../LandingTargetEstimator.cpp | 129 +++++++++++------- .../LandingTargetEstimator.h | 18 ++- .../landing_target_estimator_main.cpp | 2 +- .../landing_target_estimator_params.c | 22 +++ 4 files changed, 121 insertions(+), 50 deletions(-) diff --git a/src/modules/landing_target_estimator/LandingTargetEstimator.cpp b/src/modules/landing_target_estimator/LandingTargetEstimator.cpp index f533a9375ebf..63c77d0f5887 100644 --- a/src/modules/landing_target_estimator/LandingTargetEstimator.cpp +++ b/src/modules/landing_target_estimator/LandingTargetEstimator.cpp @@ -55,14 +55,16 @@ LandingTargetEstimator::LandingTargetEstimator() : _targetPosePub(nullptr), _targetInnovationsPub(nullptr), _paramHandle(), + _sensor_report(), _vehicleLocalPosition_valid(false), _vehicleAttitude_valid(false), _sensorBias_valid(false), - _new_irlockReport(false), + _new_sensorReport(false), _estimator_initialized(false), _faulty(false), _last_predict(0), - _last_update(0) + _last_update(0), + _uncertainty_scale(1.0f) { _paramHandle.acc_unc = param_find("LTEST_ACC_UNC"); _paramHandle.meas_unc = param_find("LTEST_MEAS_UNC"); @@ -71,6 +73,8 @@ LandingTargetEstimator::LandingTargetEstimator() : _paramHandle.mode = param_find("LTEST_MODE"); _paramHandle.scale_x = param_find("LTEST_SCALE_X"); _paramHandle.scale_y = param_find("LTEST_SCALE_Y"); + _paramHandle.offset_x = param_find("LTEST_OFF_X"); + _paramHandle.offset_y = param_find("LTEST_OFF_Y"); // Initialize uORB topics. _initialize_topics(); @@ -119,53 +123,20 @@ void LandingTargetEstimator::update() } } - if (!_new_irlockReport) { + if (!_new_sensorReport) { // nothing to do return; } // mark this sensor measurement as consumed - _new_irlockReport = false; - - if (!_vehicleAttitude_valid || !_vehicleLocalPosition_valid || !_vehicleLocalPosition.dist_bottom_valid) { - // don't have the data needed for an update - return; - } - - if (!PX4_ISFINITE(_irlockReport.pos_y) || !PX4_ISFINITE(_irlockReport.pos_x)) { - return; - } - - // TODO account for sensor orientation as set by parameter - // default orientation has camera x pointing in body y, camera y in body -x - - matrix::Vector sensor_ray; // ray pointing towards target in body frame - sensor_ray(0) = -_irlockReport.pos_y * _params.scale_y; // forward - sensor_ray(1) = _irlockReport.pos_x * _params.scale_x; // right - sensor_ray(2) = 1.0f; - - // rotate the unit ray into the navigation frame, assume sensor frame = body frame - matrix::Quaternion q_att(&_vehicleAttitude.q[0]); - _R_att = matrix::Dcm(q_att); - sensor_ray = _R_att * sensor_ray; - - if (fabsf(sensor_ray(2)) < 1e-6f) { - // z component of measurement unsafe, don't use this measurement - return; - } - - float dist = _vehicleLocalPosition.dist_bottom; - - // scale the ray s.t. the z component has length of dist - _rel_pos(0) = sensor_ray(0) / sensor_ray(2) * dist; - _rel_pos(1) = sensor_ray(1) / sensor_ray(2) * dist; + _new_sensorReport = false; if (!_estimator_initialized) { - PX4_INFO("Init"); float vx_init = _vehicleLocalPosition.v_xy_valid ? -_vehicleLocalPosition.vx : 0.f; float vy_init = _vehicleLocalPosition.v_xy_valid ? -_vehicleLocalPosition.vy : 0.f; - _kalman_filter_x.init(_rel_pos(0), vx_init, _params.pos_unc_init, _params.vel_unc_init); - _kalman_filter_y.init(_rel_pos(1), vy_init, _params.pos_unc_init, _params.vel_unc_init); + PX4_INFO("LTE: Init %.2f %.2f", (double)vx_init, (double)vy_init); + _kalman_filter_x.init(_sensor_report.rel_pos_x, vx_init, _params.pos_unc_init, _params.vel_unc_init); + _kalman_filter_y.init(_sensor_report.rel_pos_y, vy_init, _params.pos_unc_init, _params.vel_unc_init); _estimator_initialized = true; _last_update = hrt_absolute_time(); @@ -173,8 +144,8 @@ void LandingTargetEstimator::update() } else { // update - bool update_x = _kalman_filter_x.update(_rel_pos(0), _params.meas_unc * dist * dist); - bool update_y = _kalman_filter_y.update(_rel_pos(1), _params.meas_unc * dist * dist); + bool update_x = _kalman_filter_x.update(_sensor_report.rel_pos_x, _params.meas_unc * _uncertainty_scale * _uncertainty_scale); + bool update_y = _kalman_filter_y.update(_sensor_report.rel_pos_y, _params.meas_unc * _uncertainty_scale * _uncertainty_scale); if (!update_x || !update_y) { if (!_faulty) { @@ -189,7 +160,7 @@ void LandingTargetEstimator::update() if (!_faulty) { // only publish if both measurements were good - _target_pose.timestamp = _irlockReport.timestamp; + _target_pose.timestamp = _sensor_report.timestamp; float x, xvel, y, yvel, covx, covx_v, covy, covy_v; _kalman_filter_x.getState(x, xvel); @@ -200,11 +171,14 @@ void LandingTargetEstimator::update() _target_pose.is_static = (_params.mode == TargetMode::Stationary); + x += _params.offset_x; + y += _params.offset_y; + _target_pose.rel_pos_valid = true; _target_pose.rel_vel_valid = true; _target_pose.x_rel = x; _target_pose.y_rel = y; - _target_pose.z_rel = dist; + _target_pose.z_rel = _sensor_report.rel_pos_z; _target_pose.vx_rel = xvel; _target_pose.vy_rel = yvel; @@ -217,7 +191,7 @@ void LandingTargetEstimator::update() if (_vehicleLocalPosition_valid && _vehicleLocalPosition.xy_valid) { _target_pose.x_abs = x + _vehicleLocalPosition.x; _target_pose.y_abs = y + _vehicleLocalPosition.y; - _target_pose.z_abs = dist + _vehicleLocalPosition.z; + _target_pose.z_abs = _sensor_report.rel_pos_z + _vehicleLocalPosition.z; _target_pose.abs_pos_valid = true; } else { @@ -239,7 +213,7 @@ void LandingTargetEstimator::update() _kalman_filter_x.getInnovations(innov_x, innov_cov_x); _kalman_filter_y.getInnovations(innov_y, innov_cov_y); - _target_innovations.timestamp = _irlockReport.timestamp; + _target_innovations.timestamp = _sensor_report.timestamp; _target_innovations.innov_x = innov_x; _target_innovations.innov_cov_x = innov_cov_x; _target_innovations.innov_y = innov_y; @@ -277,6 +251,7 @@ void LandingTargetEstimator::_initialize_topics() _attitudeSub = orb_subscribe(ORB_ID(vehicle_attitude)); _sensorBiasSub = orb_subscribe(ORB_ID(sensor_bias)); _irlockReportSub = orb_subscribe(ORB_ID(irlock_report)); + _pozyxReportSub = orb_subscribe(ORB_ID(pozyx_report)); _parameterSub = orb_subscribe(ORB_ID(parameter_update)); } @@ -287,7 +262,65 @@ void LandingTargetEstimator::_update_topics() _vehicleAttitude_valid = _orb_update(ORB_ID(vehicle_attitude), _attitudeSub, &_vehicleAttitude); _sensorBias_valid = _orb_update(ORB_ID(sensor_bias), _sensorBiasSub, &_sensorBias); - _new_irlockReport = _orb_update(ORB_ID(irlock_report), _irlockReportSub, &_irlockReport); + if (_orb_update(ORB_ID(irlock_report), _irlockReportSub, &_irlockReport)) { + if (!_vehicleAttitude_valid || !_vehicleLocalPosition_valid || !_vehicleLocalPosition.dist_bottom_valid) { + // don't have the data needed for an update + return; + } + + if (!PX4_ISFINITE(_irlockReport.pos_y) || !PX4_ISFINITE(_irlockReport.pos_x)) { + return; + } + + // TODO account for sensor orientation as set by parameter + // default orientation has camera x pointing in body y, camera y in body -x + + matrix::Vector sensor_ray; // ray pointing towards target in body frame + sensor_ray(0) = -_irlockReport.pos_y * _params.scale_y; // forward + sensor_ray(1) = _irlockReport.pos_x * _params.scale_x; // right + sensor_ray(2) = 1.0f; + + // rotate the unit ray into the navigation frame, assume sensor frame = body frame + matrix::Quaternion q_att(&_vehicleAttitude.q[0]); + _R_att = matrix::Dcm(q_att); + sensor_ray = _R_att * sensor_ray; + + if (fabsf(sensor_ray(2)) < 1e-6f) { + // z component of measurement unsafe, don't use this measurement + return; + } + + _new_sensorReport = true; + _uncertainty_scale = _vehicleLocalPosition.dist_bottom; + + // scale the ray s.t. the z component has length of dist + _sensor_report.timestamp = _irlockReport.timestamp; + _sensor_report.rel_pos_x = sensor_ray(0) / sensor_ray(2) * _uncertainty_scale; + _sensor_report.rel_pos_y = sensor_ray(1) / sensor_ray(2) * _uncertainty_scale; + _sensor_report.rel_pos_z = _uncertainty_scale; + } + + if (_orb_update(ORB_ID(pozyx_report), _pozyxReportSub, &_pozyxReport)) { + if (!_vehicleAttitude_valid || !_vehicleLocalPosition_valid) { + // don't have the data needed for an update + return; + } + + if (!PX4_ISFINITE(_pozyxReport.pos_y) || !PX4_ISFINITE(_pozyxReport.pos_x) || + !PX4_ISFINITE(_pozyxReport.pos_z)) { + return; + } + + _new_sensorReport = true; + _uncertainty_scale = 1.0f; + + // we're assuming the coordinate system is NEU + // to get the position relative to use we just need to negate x and y + _sensor_report.timestamp = _pozyxReport.timestamp; + _sensor_report.rel_pos_x = -_pozyxReport.pos_x; + _sensor_report.rel_pos_y = -_pozyxReport.pos_y; + _sensor_report.rel_pos_z = _pozyxReport.pos_z; + } } @@ -322,6 +355,8 @@ void LandingTargetEstimator::_update_params() _params.mode = (TargetMode)mode; param_get(_paramHandle.scale_x, &_params.scale_x); param_get(_paramHandle.scale_y, &_params.scale_y); + param_get(_paramHandle.offset_x, &_params.offset_x); + param_get(_paramHandle.offset_y, &_params.offset_y); } diff --git a/src/modules/landing_target_estimator/LandingTargetEstimator.h b/src/modules/landing_target_estimator/LandingTargetEstimator.h index be43df3d10cf..ce19ea3df4a0 100644 --- a/src/modules/landing_target_estimator/LandingTargetEstimator.h +++ b/src/modules/landing_target_estimator/LandingTargetEstimator.h @@ -52,6 +52,7 @@ #include #include #include +#include #include #include #include @@ -126,6 +127,8 @@ class LandingTargetEstimator param_t mode; param_t scale_x; param_t scale_y; + param_t offset_x; + param_t offset_y; } _paramHandle; struct { @@ -136,33 +139,44 @@ class LandingTargetEstimator TargetMode mode; float scale_x; float scale_y; + float offset_x; + float offset_y; } _params; + struct { + hrt_abstime timestamp; + float rel_pos_x; + float rel_pos_y; + float rel_pos_z; + } _sensor_report; + int _vehicleLocalPositionSub; int _attitudeSub; int _sensorBiasSub; int _irlockReportSub; + int _pozyxReportSub; struct vehicle_local_position_s _vehicleLocalPosition; struct vehicle_attitude_s _vehicleAttitude; struct sensor_bias_s _sensorBias; struct irlock_report_s _irlockReport; + struct pozyx_report_s _pozyxReport; // keep track of which topics we have received bool _vehicleLocalPosition_valid; bool _vehicleAttitude_valid; bool _sensorBias_valid; - bool _new_irlockReport; + bool _new_sensorReport; bool _estimator_initialized; // keep track of whether last measurement was rejected bool _faulty; matrix::Dcm _R_att; - matrix::Vector _rel_pos; KalmanFilter _kalman_filter_x; KalmanFilter _kalman_filter_y; hrt_abstime _last_predict; // timestamp of last filter prediction hrt_abstime _last_update; // timestamp of last filter update (used to check timeout) + float _uncertainty_scale; void _check_params(const bool force); diff --git a/src/modules/landing_target_estimator/landing_target_estimator_main.cpp b/src/modules/landing_target_estimator/landing_target_estimator_main.cpp index 46a1c802c34c..9bf58ecf4fe6 100644 --- a/src/modules/landing_target_estimator/landing_target_estimator_main.cpp +++ b/src/modules/landing_target_estimator/landing_target_estimator_main.cpp @@ -99,7 +99,7 @@ int landing_target_estimator_main(int argc, char *argv[]) daemon_task = px4_task_spawn_cmd("landing_target_estimator", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 2000, + 2600, landing_target_estimator_thread_main, (argv) ? (char *const *)&argv[2] : nullptr); return 0; diff --git a/src/modules/landing_target_estimator/landing_target_estimator_params.c b/src/modules/landing_target_estimator/landing_target_estimator_params.c index 252fc2fdb99e..6ec36b47baa2 100644 --- a/src/modules/landing_target_estimator/landing_target_estimator_params.c +++ b/src/modules/landing_target_estimator/landing_target_estimator_params.c @@ -132,3 +132,25 @@ PARAM_DEFINE_FLOAT(LTEST_SCALE_X, 1.0f); * @group Landing target Estimator */ PARAM_DEFINE_FLOAT(LTEST_SCALE_Y, 1.0f); + +/** + * Landing target offset from estimated position in x axis + * + * @min 0.0 + * @max 100.0 + * @decimal 2 + * + * @group Landing target Estimator + */ +PARAM_DEFINE_FLOAT(LTEST_OFF_X, 0.0f); + +/** + * Landing target offset from estimated position in y axis + * + * @min 0.0 + * @max 100.0 + * @decimal 2 + * + * @group Landing target Estimator + */ +PARAM_DEFINE_FLOAT(LTEST_OFF_Y, 0.0f); From a724b297ed67bc3c0e3ceba0bad7833097d826c0 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Sat, 10 Mar 2018 22:16:18 +0100 Subject: [PATCH 3/3] landing_target_estimator: added beacon sensor offset in body frame --- .../LandingTargetEstimator.cpp | 14 +++++++++ .../LandingTargetEstimator.h | 4 +++ .../landing_target_estimator_params.c | 30 +++++++++++++++++-- 3 files changed, 46 insertions(+), 2 deletions(-) diff --git a/src/modules/landing_target_estimator/LandingTargetEstimator.cpp b/src/modules/landing_target_estimator/LandingTargetEstimator.cpp index 63c77d0f5887..8fdd7478ff76 100644 --- a/src/modules/landing_target_estimator/LandingTargetEstimator.cpp +++ b/src/modules/landing_target_estimator/LandingTargetEstimator.cpp @@ -75,6 +75,8 @@ LandingTargetEstimator::LandingTargetEstimator() : _paramHandle.scale_y = param_find("LTEST_SCALE_Y"); _paramHandle.offset_x = param_find("LTEST_OFF_X"); _paramHandle.offset_y = param_find("LTEST_OFF_Y"); + _paramHandle.sensor_offset_x = param_find("LTEST_S_OFF_X"); + _paramHandle.sensor_offset_y = param_find("LTEST_S_OFF_Y"); // Initialize uORB topics. _initialize_topics(); @@ -171,9 +173,19 @@ void LandingTargetEstimator::update() _target_pose.is_static = (_params.mode == TargetMode::Stationary); + // offset the landing target in earth frame x += _params.offset_x; y += _params.offset_y; + // offset the landing target in body frame + matrix::Vector3f off; + off(0) = _params.sensor_offset_x; + off(1) = _params.sensor_offset_y; + off(2) = 0.0f; + off = _R_att * off; + x += off(0); + y += off(1); + _target_pose.rel_pos_valid = true; _target_pose.rel_vel_valid = true; _target_pose.x_rel = x; @@ -357,6 +369,8 @@ void LandingTargetEstimator::_update_params() param_get(_paramHandle.scale_y, &_params.scale_y); param_get(_paramHandle.offset_x, &_params.offset_x); param_get(_paramHandle.offset_y, &_params.offset_y); + param_get(_paramHandle.sensor_offset_x, &_params.sensor_offset_x); + param_get(_paramHandle.sensor_offset_y, &_params.sensor_offset_y); } diff --git a/src/modules/landing_target_estimator/LandingTargetEstimator.h b/src/modules/landing_target_estimator/LandingTargetEstimator.h index ce19ea3df4a0..ff5e4ab3dca5 100644 --- a/src/modules/landing_target_estimator/LandingTargetEstimator.h +++ b/src/modules/landing_target_estimator/LandingTargetEstimator.h @@ -129,6 +129,8 @@ class LandingTargetEstimator param_t scale_y; param_t offset_x; param_t offset_y; + param_t sensor_offset_x; + param_t sensor_offset_y; } _paramHandle; struct { @@ -141,6 +143,8 @@ class LandingTargetEstimator float scale_y; float offset_x; float offset_y; + float sensor_offset_x; + float sensor_offset_y; } _params; struct { diff --git a/src/modules/landing_target_estimator/landing_target_estimator_params.c b/src/modules/landing_target_estimator/landing_target_estimator_params.c index 6ec36b47baa2..c82e0a161264 100644 --- a/src/modules/landing_target_estimator/landing_target_estimator_params.c +++ b/src/modules/landing_target_estimator/landing_target_estimator_params.c @@ -134,8 +134,9 @@ PARAM_DEFINE_FLOAT(LTEST_SCALE_X, 1.0f); PARAM_DEFINE_FLOAT(LTEST_SCALE_Y, 1.0f); /** - * Landing target offset from estimated position in x axis + * Landing target X offset from estimated position in NED * + * @unit m * @min 0.0 * @max 100.0 * @decimal 2 @@ -145,8 +146,9 @@ PARAM_DEFINE_FLOAT(LTEST_SCALE_Y, 1.0f); PARAM_DEFINE_FLOAT(LTEST_OFF_X, 0.0f); /** - * Landing target offset from estimated position in y axis + * Landing target Y offset from estimated position in NED * + * @unit m * @min 0.0 * @max 100.0 * @decimal 2 @@ -154,3 +156,27 @@ PARAM_DEFINE_FLOAT(LTEST_OFF_X, 0.0f); * @group Landing target Estimator */ PARAM_DEFINE_FLOAT(LTEST_OFF_Y, 0.0f); + +/** + * Sensor X position offset in body frame + * + * @unit m + * @min 0.0 + * @max 100.0 + * @decimal 2 + * + * @group Landing target Estimator + */ +PARAM_DEFINE_FLOAT(LTEST_S_OFF_X, 0.0f); + +/** + * Sensor Y position offset in body frame + * + * @unit m + * @min 0.0 + * @max 100.0 + * @decimal 2 + * + * @group Landing target Estimator + */ +PARAM_DEFINE_FLOAT(LTEST_S_OFF_Y, 0.0f);