diff --git a/boards/nxp/fmuk66-v3/default.px4board b/boards/nxp/fmuk66-v3/default.px4board index 352fb02c0e..f7bb26d45f 100644 --- a/boards/nxp/fmuk66-v3/default.px4board +++ b/boards/nxp/fmuk66-v3/default.px4board @@ -41,11 +41,13 @@ CONFIG_DRIVERS_SMART_BATTERY_BATMON=y CONFIG_COMMON_TELEMETRY=y CONFIG_DRIVERS_TONE_ALARM=y CONFIG_DRIVERS_UAVCAN=y +CONFIG_DRIVERS_UWB_UWB_SR150=y CONFIG_MODULES_AIRSPEED_SELECTOR=y CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y CONFIG_MODULES_BATTERY_STATUS=y CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y +CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_EKF2=y CONFIG_MODULES_ESC_BATTERY=y @@ -69,7 +71,6 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y -CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_ROVER_POS_CONTROL=y diff --git a/boards/nxp/fmurt1062-v1/default.px4board b/boards/nxp/fmurt1062-v1/default.px4board index 54e0fc302f..8595e95f5e 100644 --- a/boards/nxp/fmurt1062-v1/default.px4board +++ b/boards/nxp/fmurt1062-v1/default.px4board @@ -23,6 +23,7 @@ CONFIG_DRIVERS_PWM_OUT_SIM=y CONFIG_DRIVERS_RC_INPUT=y CONFIG_DRIVERS_SAFETY_BUTTON=y CONFIG_DRIVERS_TONE_ALARM=y +CONFIG_COMMON_UWB=y CONFIG_MODULES_BATTERY_STATUS=y CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index d74348b8f5..b3d4095816 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -170,6 +170,8 @@ set(msg_files uavcan_parameter_value.msg ulog_stream.msg ulog_stream_ack.msg + uwb_grid.msg + uwb_distance.msg vehicle_acceleration.msg vehicle_air_data.msg vehicle_angular_acceleration.msg diff --git a/msg/uwb_distance.msg b/msg/uwb_distance.msg new file mode 100644 index 0000000000..8f1f7105b5 --- /dev/null +++ b/msg/uwb_distance.msg @@ -0,0 +1,15 @@ +# UWB distance contains the distance information measured by an ultra-wideband positioning system, +# such as Pozyx or NXP Rddrone. + +uint64 timestamp # time since system start (microseconds) +uint32 time_uwb_ms # Time of UWB device in ms +uint32 counter # Number of Ranges since last Start of Ranging +uint32 sessionid # UWB SessionID +uint32 time_offset # Time between Ranging Rounds in ms +uint16 status # status feedback # + +uint16[12] anchor_distance # distance in cm to each UWB Anchor (UWB Device wich takes part in Ranging) +bool[12] nlos # Visual line of sight yes/no +float32[12] aoafirst # Angle of arrival of first incomming RX msg + +float32[3] position # Position of the Landing point in relation to the Drone (x,y,z in Meters NED) diff --git a/msg/uwb_grid.msg b/msg/uwb_grid.msg new file mode 100644 index 0000000000..0862f84330 --- /dev/null +++ b/msg/uwb_grid.msg @@ -0,0 +1,25 @@ +# UWB report message contains the grid information measured by an ultra-wideband positioning system, +# such as Pozyx or NXP Rddrone. + +uint64 timestamp # time since system start (microseconds) +uint16 initator_time # time to synchronize clocks (microseconds) +uint8 num_anchors # Number of anchors + +float64[4] target_gps # GPS position of target of the UWB system (Lat / Lon / Alt / Yaw Offset to true North) + +# Grid position information +# Position in x,y,z in (x,y,z in centimeters NED) +# staring with POI and Anchor 0 up to Anchor 11 +int16[3] target_pos # Point of Interest, mostly landing Target x,y,z +int16[3] anchor_pos_0 +int16[3] anchor_pos_1 +int16[3] anchor_pos_2 +int16[3] anchor_pos_3 +int16[3] anchor_pos_4 +int16[3] anchor_pos_5 +int16[3] anchor_pos_6 +int16[3] anchor_pos_7 +int16[3] anchor_pos_8 +int16[3] anchor_pos_9 +int16[3] anchor_pos_10 +int16[3] anchor_pos_11 diff --git a/src/drivers/uwb/CMakeLists.txt b/src/drivers/uwb/CMakeLists.txt new file mode 100644 index 0000000000..cafdb1f46e --- /dev/null +++ b/src/drivers/uwb/CMakeLists.txt @@ -0,0 +1,34 @@ +############################################################################ +# +# 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. +# +############################################################################ + +add_subdirectory(uwb_sr150) diff --git a/src/drivers/uwb/Kconfig b/src/drivers/uwb/Kconfig new file mode 100644 index 0000000000..7a5248be5d --- /dev/null +++ b/src/drivers/uwb/Kconfig @@ -0,0 +1,9 @@ +menu "UWB" + menuconfig COMMON_UWB + bool "common UWB Drivers" + default n + select DRIVERS_UWB_UWB_SR150 + ---help--- + Enable support for uwb drivers + rsource "*/Kconfig" +endmenu diff --git a/src/drivers/uwb/uwb_sr150/CMakeLists.txt b/src/drivers/uwb/uwb_sr150/CMakeLists.txt new file mode 100644 index 0000000000..c119f1b6ce --- /dev/null +++ b/src/drivers/uwb/uwb_sr150/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2020-2022 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__uwb_sr150 + MAIN uwb_sr150 + SRCS + uwb_sr150.cpp + uwb_sr150.h + MODULE_CONFIG + module.yaml + DEPENDS + px4_work_queue +) diff --git a/src/drivers/uwb/uwb_sr150/Kconfig b/src/drivers/uwb/uwb_sr150/Kconfig new file mode 100644 index 0000000000..17ff2a7fec --- /dev/null +++ b/src/drivers/uwb/uwb_sr150/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_UWB_UWB_SR150 + bool "uwb_sr150" + default n + ---help--- + Enable support for uwb sr150 diff --git a/src/drivers/uwb/uwb_sr150/module.yaml b/src/drivers/uwb/uwb_sr150/module.yaml new file mode 100644 index 0000000000..3442e138be --- /dev/null +++ b/src/drivers/uwb/uwb_sr150/module.yaml @@ -0,0 +1,53 @@ +module_name: Ultrawideband position sensor driver +serial_config: + - command: uwb_sr150 start -d ${SERIAL_DEV} -b p:${BAUD_PARAM} + port_config_param: + name: UWB_PORT_CFG + group: UWB + default: "" + +parameters: + - group: UWB + definitions: + + UWB_INIT_OFF_X: + description: + short: UWB sensor X offset in body frame + long: UWB sensor positioning in relation to Drone in NED. X offset. A Positiv offset results in a Position o + type: float + unit: m + decimal: 2 + increment: 0.01 + default: 0.00 + + UWB_INIT_OFF_Y: + description: + short: UWB sensor Y offset in body frame + long: UWB sensor positioning in relation to Drone in NED. Y offset. + type: float + unit: m + decimal: 2 + increment: 0.01 + default: 0.00 + + UWB_INIT_OFF_Z: + description: + short: UWB sensor Y offset in body frame + long: UWB sensor positioning in relation to Drone in NED. Z offset. + type: float + unit: m + decimal: 2 + increment: 0.01 + default: 0.00 + + UWB_INIT_OFF_YAW: + description: + short: UWB sensor YAW offset in body frame + long: UWB sensor positioning in relation to Drone in NED. Yaw rotation in relation to direction of FMU. + type: float + unit: deg + decimal: 1 + increment: 0.1 + default: 0.00 + + diff --git a/src/drivers/uwb/uwb_sr150/uwb_sr150.cpp b/src/drivers/uwb/uwb_sr150/uwb_sr150.cpp new file mode 100644 index 0000000000..128cf415bc --- /dev/null +++ b/src/drivers/uwb/uwb_sr150/uwb_sr150.cpp @@ -0,0 +1,715 @@ +/**************************************************************************** + * + * Copyright (c) 2020-2022 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. + * + ****************************************************************************/ + + +/* This is a driver for the NXP SR150 UWB Chip on MK UWB Shield 2 + * This Driver handles the Communication to the UWB Board. + * For Information about HW and SW contact Mobile Knowledge: + * https://www.themobileknowledge.com + * */ + +#include "uwb_sr150.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// Timeout between bytes. If there is more time than this between bytes, then this driver assumes +// that it is the boundary between messages. +// See uwb_sr150::run() for more detailed explanation. +#define BYTE_TIMEOUT_US 5000 + +// Amount of time to wait for a new message. If more time than this passes between messages, then this +// driver assumes that the UWB_SR150 module is disconnected. +// (Right now it does not do anything about this) +#define MESSAGE_TIMEOUT_S 10 //wait 10 seconds. +#define MESSAGE_TIMEOUT_US 1 + +// The default baudrate of the uwb_sr150 module before configuration +#define DEFAULT_BAUD B115200 + +const uint8_t CMD_RANGING_STOP[UWB_CMD_LEN ] = {UWB_CMD, 0x00, 0x02, UWB_DRONE_CTL, UWB_CMD_STOP}; +const uint8_t CMD_RANGING_START[UWB_CMD_LEN ] = {UWB_CMD, 0x00, 0x02, UWB_DRONE_CTL, UWB_CMD_START}; +const uint8_t CMD_APP_START[UWB_CMD_LEN ] = {0x01, 0x00, 0x02, UWB_APP_START, UWB_PRECNAV_APP}; +const uint8_t CMD_APP_STOP[0x04 ] = {0x01, 0x00, 0x01, UWB_APP_STOP}; + +extern "C" __EXPORT int uwb_sr150_main(int argc, char *argv[]); + +UWB_SR150::UWB_SR150(const char *device_name, speed_t baudrate, bool uwb_pos_debug): + ModuleParams(nullptr), + _read_count_perf(perf_alloc(PC_COUNT, "uwb_sr150_count")), + _read_err_perf(perf_alloc(PC_COUNT, "uwb_sr150_err")) +{ + _uwb_pos_debug = uwb_pos_debug; + // start serial port + _uart = open(device_name, O_RDWR | O_NOCTTY); + + if (_uart < 0) { err(1, "could not open %s", device_name); } + + int ret = 0; + struct termios uart_config {}; + ret = tcgetattr(_uart, &uart_config); + + if (ret < 0) { err(1, "failed to get attr"); } + + uart_config.c_oflag &= ~ONLCR; // no CR for every LF + ret = cfsetispeed(&uart_config, baudrate); + + if (ret < 0) { err(1, "failed to set input speed"); } + + ret = cfsetospeed(&uart_config, baudrate); + + if (ret < 0) { err(1, "failed to set output speed"); } + + ret = tcsetattr(_uart, TCSANOW, &uart_config); + + if (ret < 0) { err(1, "failed to set attr"); } +} + +UWB_SR150::~UWB_SR150() +{ + printf("UWB: Ranging Stopped\t\n"); + int written = write(_uart, &CMD_APP_STOP, sizeof(CMD_APP_STOP)); + + if (written < (int) sizeof(CMD_APP_STOP)) { + PX4_ERR("Only wrote %d bytes out of %d.", written, (int) sizeof(CMD_APP_STOP)); + } + + perf_free(_read_err_perf); + perf_free(_read_count_perf); + + close(_uart); +} + +void UWB_SR150::run() +{ + // Subscribe to parameter_update message + parameters_update(); + + //TODO replace with BLE grid configuration + grid_info_read(&_uwb_grid_info.target_pos); + _uwb_grid_info.num_anchors = 4; + _uwb_grid_info.initator_time = hrt_absolute_time(); + _uwb_grid_info.mac_mode = 0x00; + + /* Grid Info Message*/ + _uwb_grid.timestamp = hrt_absolute_time(); + _uwb_grid.initator_time = _uwb_grid_info.initator_time; + _uwb_grid.num_anchors = _uwb_grid_info.num_anchors; + + memcpy(&_uwb_grid.target_pos, &_uwb_grid_info.target_pos, sizeof(position_t) * 5); //write Grid positions + + _uwb_grid_pub.publish(_uwb_grid); + + /* Ranging Command */ + int written = write(_uart, CMD_RANGING_START, UWB_CMD_LEN); + + if (written < UWB_CMD_LEN) { + PX4_ERR("Only wrote %d bytes out of %d.", written, (int) sizeof(uint8_t) * UWB_CMD_LEN); + } + + while (!should_exit()) { + written = UWB_SR150::distance(); //evaluate Ranging Messages until Stop + } + + if (!written) { printf("ERROR: Distance Failed"); } + + // Automatic Stop. This should not be reachable + written = write(_uart, &CMD_RANGING_STOP, UWB_CMD_LEN); + + if (written < (int) sizeof(CMD_RANGING_STOP)) { + PX4_ERR("Only wrote %d bytes out of %d.", written, (int) sizeof(CMD_RANGING_STOP)); + } +} + +void UWB_SR150::grid_info_read(position_t *grid) +{ + //place holder, until UWB initiator can respond with Grid info + /* This Reads the position of each Anchor in the Grid. + Right now the Grid configuration is saved on the SD card. + In the future, we would like, that the Initiator responds with the Grid Information (Including Position). */ + PX4_INFO("Reading UWB GRID from SD... \t\n"); + FILE *file; + file = fopen(UWB_GRID_CONFIG, "r"); + + int bread = 0; + + for (int i = 0; i < 5; i++) { + bread += fscanf(file, "%hd,%hd,%hd\n", &grid[i].x, &grid[i].y, &grid[i].z); + } + + if (bread == 5 * 3) { + PX4_INFO("GRID INFO READ! bytes read: %d \t\n", bread); + + } else { //use UUID from Grid survey + PX4_INFO("GRID INFO Missing! bytes read: %d \t\n Using standrd Grid \t\n", bread); + position_t grid_setup[5] = {{0x0, 0x0, 0x0}, {(int16_t)0xff68, 0x0, 0x0a}, {0x99, 0x0, 0x0a}, {0x0, 0x96, 0x64}, {0x0, (int16_t)0xff6a, 0x63}}; + memcpy(grid, &grid_setup, sizeof(grid_setup)); + PX4_INFO("Insert \"uwb_grid_config.csv\" containing gridinfo in cm at \"/fs/microsd/etc/\".\t\n"); + PX4_INFO("n + 1 Anchor Positions starting with Landing Target. Int16 Format: \"x,y,z\" \t\n"); + } + + fclose(file); +} + +int UWB_SR150::custom_command(int argc, char *argv[]) +{ + return print_usage("Unrecognized command."); +} + +int UWB_SR150::print_usage(const char *reason) +{ + if (reason) { + printf("%s\n\n", reason); + } + + PRINT_MODULE_USAGE_NAME("uwb", "driver"); + PRINT_MODULE_DESCRIPTION(R"DESC_STR( +### Description + +Driver for NXP UWB_SR150 UWB positioning system. This driver publishes a `uwb_distance` message +whenever the UWB_SR150 has a position measurement available. + +### Example + +Start the driver with a given device: + +$ uwb start -d /dev/ttyS2 + )DESC_STR"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, "", "Name of device for serial communication with UWB", false); + PRINT_MODULE_USAGE_PARAM_STRING('b', nullptr, "", "Baudrate for serial communication", false); + PRINT_MODULE_USAGE_PARAM_STRING('p', nullptr, "", "Position Debug: displays errors in Multilateration", false); + PRINT_MODULE_USAGE_COMMAND("stop"); + PRINT_MODULE_USAGE_COMMAND("status"); + return 0; +} + +int UWB_SR150::task_spawn(int argc, char *argv[]) +{ + int task_id = px4_task_spawn_cmd( + "uwb_driver", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 2048, + &run_trampoline, + argv + ); + + if (task_id < 0) { + return -errno; + + } else { + _task_id = task_id; + return 0; + } +} + +UWB_SR150 *UWB_SR150::instantiate(int argc, char *argv[]) +{ + int ch; + int option_index = 1; + const char *option_arg; + const char *device_name = nullptr; + bool error_flag = false; + int baudrate = 0; + bool uwb_pos_debug = false; // Display UWB position calculation debug Messages + + while ((ch = px4_getopt(argc, argv, "d:b:p", &option_index, &option_arg)) != EOF) { + switch (ch) { + case 'd': + device_name = option_arg; + break; + + case 'b': + px4_get_parameter_value(option_arg, baudrate); + break; + + case 'p': + + uwb_pos_debug = true; + break; + + default: + PX4_WARN("Unrecognized flag: %c", ch); + error_flag = true; + break; + } + } + + if (!error_flag && device_name == nullptr) { + print_usage("Device name not provided. Using default Device: TEL1:/dev/ttyS4 \n"); + device_name = "TEL2"; + error_flag = true; + } + + if (!error_flag && baudrate == 0) { + printf("Baudrate not provided. Using default Baud: 115200 \n"); + baudrate = B115200; + } + + if (!error_flag && uwb_pos_debug == true) { + printf("UWB Position algorithm Debugging \n"); + } + + if (error_flag) { + PX4_WARN("Failed to start UWB driver. \n"); + return nullptr; + + } else { + PX4_INFO("Constructing UWB_SR150. Device: %s", device_name); + return new UWB_SR150(device_name, baudrate, uwb_pos_debug); + } +} + +int UWB_SR150::distance() +{ + _uwb_init_offset_v3f = matrix::Vector3f(_uwb_init_off_x.get(), _uwb_init_off_y.get(), + _uwb_init_off_z.get()); //set offset at the start + uint8_t *buffer = (uint8_t *) &_distance_result_msg; + + FD_ZERO(&_uart_set); + FD_SET(_uart, &_uart_set); + _uart_timeout.tv_sec = MESSAGE_TIMEOUT_S ; + _uart_timeout.tv_usec = MESSAGE_TIMEOUT_US; + + size_t buffer_location = 0; + // There is a atleast 2000 clock cycles between 2 msg (20000/80mhz = 200uS) + // Messages are only delimited by time. There is a chance that this driver starts up in the middle + // of a message, with no way to know this other than time. There is also always the possibility of + // transmission errors causing a dropped byte. + // Here is the process for dealing with that: + // - Wait up to 1 second to start receiving a message + // - Once receiving a message, keep going until EITHER: + // - There is too large of a gap between bytes (Currently set to 5ms). + // This means the message is incomplete. Throw it out and start over. + // - 46 bytes are received (the size of the whole message). + + while (buffer_location < sizeof(_distance_result_msg) + && select(_uart + 1, &_uart_set, nullptr, nullptr, &_uart_timeout) > 0) { + + int bytes_read = read(_uart, &buffer[buffer_location], sizeof(_distance_result_msg) - buffer_location); + + if (bytes_read > 0) { + buffer_location += bytes_read; + + } else { + break; + } + + FD_ZERO(&_uart_set); + FD_SET(_uart, &_uart_set); + _uart_timeout.tv_sec = 0; + // Setting this timeout too high (> 37ms) will cause problems because the next message will start + // coming in, and overlap with the current message. + // Setting this timeout too low (< 1ms) will cause problems because there is some delay between + // the individual bytes of a message, and a too-short timeout will cause the message to be truncated. + // The current value of 5ms was found experimentally to never cut off a message prematurely. + // Strictly speaking, there are no downsides to setting this timeout as high as possible (Just under 37ms), + // because if this process is waiting, it means that the last message was incomplete, so there is no current + // data waiting to be published. But we would rather set this timeout lower in case the UWB_SR150 board is + // updated to publish data faster. + _uart_timeout.tv_usec = BYTE_TIMEOUT_US; + } + + perf_count(_read_count_perf); + + // All of the following criteria must be met for the message to be acceptable: + // - Size of message == sizeof(distance_msg_t) (51 bytes) + // - status == 0x00 + // - Values of all 3 position measurements are reasonable + // (If one or more anchors is missed, then position might be an unreasonably large number.) + bool ok = (buffer_location == sizeof(distance_msg_t) && _distance_result_msg.stop == 0x1b); //|| + //(buffer_location == sizeof(grid_msg_t) && _distance_result_msg.stop == 0x1b) + //); + + if (ok) { + + /* Ranging Message*/ + _uwb_distance.timestamp = hrt_absolute_time(); + _uwb_distance.time_uwb_ms = _distance_result_msg.time_uwb_ms; + _uwb_distance.counter = _distance_result_msg.seq_ctr; + _uwb_distance.sessionid = _distance_result_msg.sessionId; + _uwb_distance.time_offset = _distance_result_msg.range_interval; + + for (int i = 0; i < 4; i++) { + _uwb_distance.anchor_distance[i] = _distance_result_msg.measurements[i].distance; + _uwb_distance.nlos[i] = _distance_result_msg.measurements[i].nLos; + _uwb_distance.aoafirst[i] = float(_distance_result_msg.measurements[i].aoaFirst) / + 128; // Angle of Arrival has Format Q9.7; dividing by 2^7 results in the correct value + } + + // Algorithm goes here + UWB_POS_ERROR_CODES UWB_POS_ERROR = UWB_SR150::localization(); + + _uwb_distance.status = UWB_POS_ERROR; + + if (UWB_OK == UWB_POS_ERROR) { + + _uwb_distance.position[0] = _rel_pos(0); + _uwb_distance.position[1] = _rel_pos(1); + _uwb_distance.position[2] = _rel_pos(2); + + } else { + //only print the error if debug is enabled + if (_uwb_pos_debug) { + switch (UWB_POS_ERROR) { //UWB POSITION ALGORItHM Errors + case UWB_ANC_BELOW_THREE: + PX4_INFO("UWB not enough anchors for doing localization"); + break; + + case UWB_LIN_DEP_FOR_THREE: + PX4_INFO("UWB localization: linear dependant with 3 Anchors"); + break; + + case UWB_ANC_ON_ONE_LEVEL: + PX4_INFO("UWB localization: Anchors are on a X,Y Plane and there are not enought Anchors"); + break; + + case UWB_LIN_DEP_FOR_FOUR: + PX4_INFO("UWB localization: linear dependant with four or more Anchors"); + break; + + case UWB_RANK_ZERO: + PX4_INFO("UWB localization: rank is zero"); + break; + + default: + PX4_INFO("UWB localization: Unknown failure in Position Algorithm"); + break; + } + } + } + + _uwb_distance_pub.publish(_uwb_distance); + + } else { + //PX4_ERR("Read %d bytes instead of %d.", (int) buffer_location, (int) sizeof(distance_msg_t)); + perf_count(_read_err_perf); + + if (buffer_location == 0) { + PX4_WARN("UWB module is not responding."); + + //TODO add retry Ranging Start Message. Sometimes the UWB devices Crashes. (Check Power) + } + } + + return 1; +} + +UWB_POS_ERROR_CODES UWB_SR150::localization() +{ +// WIP + /****************************************************** + ****************** 3D Localization ******************* + *****************************************************/ + + /*!@brief: This function calculates the 3D position of the initiator from the anchor distances and positions using least squared errors. + * The function expects more than 4 anchors. The used equation system looks like follows:\n + \verbatim + - - + | M_11 M_12 M_13 | x b[0] + | M_12 M_22 M_23 | * y = b[1] + | M_23 M_13 M_33 | z b[2] + - - + \endverbatim + * @param distances_cm_in_pt: Pointer to array that contains the distances to the anchors in cm (including invalid results) + * @param no_distances: Number of valid distances in distance array (it's not the size of the array) + * @param anchor_pos: Pointer to array that contains anchor positions in cm (including positions related to invalid results) + * @param no_anc_positions: Number of valid anchor positions in the position array (it's not the size of the array) + * @param position_result_pt: Pointer toposition. position_t variable that holds the result of this calculation + * @return: The function returns a status code. */ + + /* Algorithm used: + * Linear Least Sqaures to solve Multilateration + * with a Special case if there are only 3 Anchors. + * Output is the Coordinates of the Initiator in relation to Anchor 0 in NEU (North-East-Up) Framing + * In cm + */ + + /* Resulting Position Vector*/ + int64_t x_pos = 0; + int64_t y_pos = 0; + int64_t z_pos = 0; + /* Matrix components (3*3 Matrix resulting from least square error method) [cm^2] */ + int64_t M_11 = 0; + int64_t M_12 = 0; // = M_21 + int64_t M_13 = 0; // = M_31 + int64_t M_22 = 0; + int64_t M_23 = 0; // = M_23 + int64_t M_33 = 0; + + /* Vector components (3*1 Vector resulting from least square error method) [cm^3] */ + int64_t b[3] = {0}; + + /* Miscellaneous variables */ + int64_t temp = 0; + int64_t temp2 = 0; + int64_t nominator = 0; + int64_t denominator = 0; + bool anchors_on_x_y_plane = true; // Is true, if all anchors are on the same height => x-y-plane + bool lin_dep = true; // All vectors are linear dependent, if this variable is true + uint8_t ind_y_indi = + 0; //numberr of independet vectors // First anchor index, for which the second row entry of the matrix [(x_1 - x_0) (x_2 - x_0) ... ; (y_1 - x_0) (y_2 - x_0) ...] is non-zero => linear independent + + /* Arrays for used distances and anchor positions (without rejected ones) */ + uint8_t no_distances = _uwb_grid_info.num_anchors; + uint32_t distances_cm[no_distances]; + position_t anchor_pos[no_distances]; //position in CM + uint8_t no_valid_distances = 0; + + /* Reject invalid distances (including related anchor position) */ + for (int i = 0; i < no_distances; i++) { + if (_distance_result_msg.measurements[i].distance != 0xFFFFu) { + //excludes any distance that is 0xFFFFU (int16 Maximum Value) + distances_cm[no_valid_distances] = _distance_result_msg.measurements[i].distance; + anchor_pos[no_valid_distances] = _uwb_grid_info.anchor_pos[i]; + no_valid_distances++; + + } + } + + /* Check, if there are enough valid results for doing the localization at all */ + if (no_valid_distances < 3) { + return UWB_ANC_BELOW_THREE; + } + + /* Check, if anchors are on the same x-y plane */ + for (int i = 1; i < no_valid_distances; i++) { + if (anchor_pos[i].z != anchor_pos[0].z) { + anchors_on_x_y_plane = false; + break; + } + } + + /**** Check, if there are enough linear independent anchor positions ****/ + + /* Check, if the matrix |(x_1 - x_0) (x_2 - x_0) ... | has rank 2 + * |(y_1 - y_0) (y_2 - y_0) ... | */ + + for (ind_y_indi = 2; ((ind_y_indi < no_valid_distances) && (lin_dep == true)); ind_y_indi++) { + temp = ((int64_t)anchor_pos[ind_y_indi].y - (int64_t)anchor_pos[0].y) * ((int64_t)anchor_pos[1].x - + (int64_t)anchor_pos[0].x); + temp2 = ((int64_t)anchor_pos[1].y - (int64_t)anchor_pos[0].y) * ((int64_t)anchor_pos[ind_y_indi].x - + (int64_t)anchor_pos[0].x); + + if ((temp - temp2) != 0) { + lin_dep = false; + break; + } + } + + /* Leave function, if rank is below 2 */ + if (lin_dep == true) { + return UWB_LIN_DEP_FOR_THREE; + } + + /* If the anchors are not on the same plane, three vectors must be independent => check */ + if (!anchors_on_x_y_plane) { + /* Check, if there are enough valid results for doing the localization */ + if (no_valid_distances < 4) { + return UWB_ANC_ON_ONE_LEVEL; + } + + /* Check, if the matrix |(x_1 - x_0) (x_2 - x_0) (x_3 - x_0) ... | has rank 3 (Rank y, y already checked) + * |(y_1 - y_0) (y_2 - y_0) (y_3 - y_0) ... | + * |(z_1 - z_0) (z_2 - z_0) (z_3 - z_0) ... | */ + lin_dep = true; + + for (int i = 2; ((i < no_valid_distances) && (lin_dep == true)); i++) { + if (i != ind_y_indi) { + /* (x_1 - x_0)*[(y_2 - y_0)(z_n - z_0) - (y_n - y_0)(z_2 - z_0)] */ + temp = ((int64_t)anchor_pos[ind_y_indi].y - (int64_t)anchor_pos[0].y) * ((int64_t)anchor_pos[i].z - + (int64_t)anchor_pos[0].z); + temp -= ((int64_t)anchor_pos[i].y - (int64_t)anchor_pos[0].y) * ((int64_t)anchor_pos[ind_y_indi].z - + (int64_t)anchor_pos[0].z); + temp2 = ((int64_t)anchor_pos[1].x - (int64_t)anchor_pos[0].x) * temp; + + /* Add (x_2 - x_0)*[(y_n - y_0)(z_1 - z_0) - (y_1 - y_0)(z_n - z_0)] */ + temp = ((int64_t)anchor_pos[i].y - (int64_t)anchor_pos[0].y) * ((int64_t)anchor_pos[1].z - (int64_t)anchor_pos[0].z); + temp -= ((int64_t)anchor_pos[1].y - (int64_t)anchor_pos[0].y) * ((int64_t)anchor_pos[i].z - (int64_t)anchor_pos[0].z); + temp2 += ((int64_t)anchor_pos[ind_y_indi].x - (int64_t)anchor_pos[0].x) * temp; + + /* Add (x_n - x_0)*[(y_1 - y_0)(z_2 - z_0) - (y_2 - y_0)(z_1 - z_0)] */ + temp = ((int64_t)anchor_pos[1].y - (int64_t)anchor_pos[0].y) * ((int64_t)anchor_pos[ind_y_indi].z - + (int64_t)anchor_pos[0].z); + temp -= ((int64_t)anchor_pos[ind_y_indi].y - (int64_t)anchor_pos[0].y) * ((int64_t)anchor_pos[1].z - + (int64_t)anchor_pos[0].z); + temp2 += ((int64_t)anchor_pos[i].x - (int64_t)anchor_pos[0].x) * temp; + + if (temp2 != 0) { lin_dep = false; } + } + } + + /* Leave function, if rank is below 3 */ + if (lin_dep == true) { + return UWB_LIN_DEP_FOR_FOUR; + } + } + + /************************************************** Algorithm ***********************************************************************/ + + /* Writing values resulting from least square error method (A_trans*A*x = A_trans*r; row 0 was used to remove x^2,y^2,z^2 entries => index starts at 1) */ + for (int i = 1; i < no_valid_distances; i++) { + /* Matrix (needed to be multiplied with 2, afterwards) */ + M_11 += (int64_t)pow((int64_t)(anchor_pos[i].x - anchor_pos[0].x), 2); + M_12 += (int64_t)((int64_t)(anchor_pos[i].x - anchor_pos[0].x) * (int64_t)(anchor_pos[i].y - anchor_pos[0].y)); + M_13 += (int64_t)((int64_t)(anchor_pos[i].x - anchor_pos[0].x) * (int64_t)(anchor_pos[i].z - anchor_pos[0].z)); + M_22 += (int64_t)pow((int64_t)(anchor_pos[i].y - anchor_pos[0].y), 2); + M_23 += (int64_t)((int64_t)(anchor_pos[i].y - anchor_pos[0].y) * (int64_t)(anchor_pos[i].z - anchor_pos[0].z)); + M_33 += (int64_t)pow((int64_t)(anchor_pos[i].z - anchor_pos[0].z), 2); + + /* Vector */ + temp = (int64_t)((int64_t)pow(distances_cm[0], 2) - (int64_t)pow(distances_cm[i], 2) + + (int64_t)pow(anchor_pos[i].x, 2) + (int64_t)pow(anchor_pos[i].y, 2) + + (int64_t)pow(anchor_pos[i].z, 2) - (int64_t)pow(anchor_pos[0].x, 2) + - (int64_t)pow(anchor_pos[0].y, 2) - (int64_t)pow(anchor_pos[0].z, 2)); + + b[0] += (int64_t)((int64_t)(anchor_pos[i].x - anchor_pos[0].x) * temp); + b[1] += (int64_t)((int64_t)(anchor_pos[i].y - anchor_pos[0].y) * temp); + b[2] += (int64_t)((int64_t)(anchor_pos[i].z - anchor_pos[0].z) * temp); + } + + M_11 = 2 * M_11; + M_12 = 2 * M_12; + M_13 = 2 * M_13; + M_22 = 2 * M_22; + M_23 = 2 * M_23; + M_33 = 2 * M_33; + + /* Calculating the z-position, if calculation is possible (at least one anchor at z != 0) */ + if (anchors_on_x_y_plane == false) { + nominator = b[0] * (M_12 * M_23 - M_13 * M_22) + b[1] * (M_12 * M_13 - M_11 * M_23) + b[2] * + (M_11 * M_22 - M_12 * M_12); // [cm^7] + denominator = M_11 * (M_33 * M_22 - M_23 * M_23) + 2 * M_12 * M_13 * M_23 - M_33 * M_12 * M_12 - M_22 * M_13 * + M_13; // [cm^6] + + /* Check, if denominator is zero (Rank of matrix not high enough) */ + if (denominator == 0) { + return UWB_RANK_ZERO; + } + + z_pos = ((nominator * 10) / denominator + 5) / 10; // [cm] + } + + /* Else prepare for different calculation approach (after x and y were calculated) */ + else { + z_pos = 0; + } + + /* Calculating the y-position */ + nominator = b[1] * M_11 - b[0] * M_12 - (z_pos * (M_11 * M_23 - M_12 * M_13)); // [cm^5] + denominator = M_11 * M_22 - M_12 * M_12;// [cm^4] + + /* Check, if denominator is zero (Rank of matrix not high enough) */ + if (denominator == 0) { + return UWB_RANK_ZERO; + } + + y_pos = ((nominator * 10) / denominator + 5) / 10; // [cm] + + /* Calculating the x-position */ + nominator = b[0] - z_pos * M_13 - y_pos * M_12; // [cm^3] + denominator = M_11; // [cm^2] + + x_pos = ((nominator * 10) / denominator + 5) / 10;// [cm] + + /* Calculate z-position form x and y coordinates, if z can't be determined by previous steps (All anchors at z_n = 0) */ + if (anchors_on_x_y_plane == true) { + /* Calculate z-positon relative to the anchor grid's height */ + for (int i = 0; i < no_distances; i++) { + /* z² = dis_meas_n² - (x - x_anc_n)² - (y - y_anc_n)² */ + temp = (int64_t)((int64_t)pow(distances_cm[i], 2) + - (int64_t)pow((x_pos - (int64_t)anchor_pos[i].x), 2) + - (int64_t)pow((y_pos - (int64_t)anchor_pos[i].y), 2)); + + /* z² must be positive, else x and y must be wrong => calculate positive sqrt and sum up all calculated heights, if positive */ + if (temp >= 0) { + z_pos += (int64_t)sqrt(temp); + + } else { + z_pos = 0; + } + } + + z_pos = z_pos / no_distances; // Divide sum by number of distances to get the average + + /* Add height of the anchor grid's height */ + z_pos += anchor_pos[0].z; + } + + //Output is the Coordinates of the Initiator in relation to 0,0,0 in NEU (North-East-Up) Framing + // The end goal of this math is to get the position relative to the landing point in the NED frame. + _current_position_uwb_init = matrix::Vector3f(x_pos, y_pos, z_pos); + + // Construct the rotation from the UWB_R4frame to the NWU frame. + // The UWB_SR150 frame is just NWU, rotated by some amount about the Z (up) axis. (Parameter Yaw offset) + // To get back to NWU, just rotate by negative this amount about Z. + _uwb_init_to_nwu = matrix::Dcmf(matrix::Eulerf(0.0f, 0.0f, + -(_uwb_init_off_yaw.get() * M_PI_F / 180.0f))); // + // The actual conversion: + // - Subtract _landing_point to get the position relative to the landing point, in UWB_R4 frame + // - Rotate by _rddrone_to_nwu to get into the NWU frame + // - Rotate by _nwu_to_ned to get into the NED frame + _current_position_ned = _nwu_to_ned * _uwb_init_to_nwu * _current_position_uwb_init; + + // Now the position is the landing point relative to the vehicle. + // so the only thing left is to convert cm to Meters and to add the Initiator offset + _rel_pos = _current_position_ned / 100 + _uwb_init_offset_v3f; + + // The UWB report contains the position of the vehicle relative to the landing point. + + return UWB_OK; +} + +int uwb_sr150_main(int argc, char *argv[]) +{ + return UWB_SR150::main(argc, argv); +} + +void UWB_SR150::parameters_update() +{ + if (_parameter_update_sub.updated()) { + parameter_update_s param_update; + _parameter_update_sub.copy(¶m_update); + + // If any parameter updated, call updateParams() to check if + // this class attributes need updating (and do so). + updateParams(); + } +} diff --git a/src/drivers/uwb/uwb_sr150/uwb_sr150.h b/src/drivers/uwb/uwb_sr150/uwb_sr150.h new file mode 100644 index 0000000000..54ef50b7af --- /dev/null +++ b/src/drivers/uwb/uwb_sr150/uwb_sr150.h @@ -0,0 +1,207 @@ +/**************************************************************************** + * + * Copyright (c) 2020-2022 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 PX4_RDDRONE_H +#define PX4_RDDRONE_H + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace time_literals; + +#define UWB_CMD 0x8e +#define UWB_CMD_START 0x01 +#define UWB_CMD_STOP 0x00 +#define UWB_CMD_RANGING 0x0A +#define STOP_B 0x0A + +#define UWB_PRECNAV_APP 0x04 +#define UWB_APP_START 0x10 +#define UWB_APP_STOP 0x11 +#define UWB_SESSION_START 0x22 +#define UWB_SESSION_STOP 0x23 +#define UWB_RANGING_START 0x01 +#define UWB_RANGING_STOP 0x00 +#define UWB_DRONE_CTL 0x0A + +#define UWB_CMD_LEN 0x05 +#define UWB_CMD_DISTANCE_LEN 0x21 +#define UWB_MAC_MODE 2 +#define MAX_ANCHORS 12 +#define UWB_GRID_CONFIG "/fs/microsd/etc/uwb_grid_config.csv" + +typedef struct { //needs higher accuracy? + float lat, lon, alt, yaw; //offset to true North +} gps_pos_t; + +typedef struct { + int16_t x, y, z; //axis in cm +} position_t; // Position of a device or target in 3D space + +enum UWB_POS_ERROR_CODES { + UWB_OK, + UWB_ANC_BELOW_THREE, + UWB_LIN_DEP_FOR_THREE, + UWB_ANC_ON_ONE_LEVEL, + UWB_LIN_DEP_FOR_FOUR, + UWB_RANK_ZERO +}; + +typedef struct { + uint8_t MAC[2]; // MAC Adress of UWB device + uint8_t status; // Status of Measurement + uint16_t distance; // Distance in cm + uint8_t nLos; // line of sight y/n + uint16_t aoaFirst; // Angle of Arrival of incoming msg +} __attribute__((packed)) UWB_range_meas_t; + +typedef struct { + uint32_t initator_time; //timestamp of init + uint32_t sessionId; // Session ID of UWB session + uint8_t num_anchors; //number of anchors + gps_pos_t target_gps; //GPS position of Landing Point + uint8_t mac_mode; // MAC adress mode, either 2 Byte or 8 Byte + uint8_t MAC[UWB_MAC_MODE][MAX_ANCHORS]; + position_t target_pos; //target position + position_t anchor_pos[MAX_ANCHORS]; // Position of each anchor + uint8_t stop; // Should be 27 +} grid_msg_t; + +typedef struct { + uint8_t cmd; // Should be 0x8E for distance result message + uint16_t len; // Should be 0x30 for distance result message + uint32_t time_uwb_ms; // Timestamp of UWB device in ms + uint32_t seq_ctr; // Number of Ranges since last Start of Ranging + uint32_t sessionId; // Session ID of UWB session + uint32_t range_interval; // Time between ranging rounds + uint8_t mac_mode; // MAC adress mode, either 2 Byte or 8 Byte + uint8_t no_measurements; // MAC adress mode, either 2 Byte or 8 Byte + UWB_range_meas_t measurements[4]; //Raw anchor_distance distances in CM 2*9 + uint8_t stop; // Should be 0x1B +} __attribute__((packed)) distance_msg_t; + +class UWB_SR150 : public ModuleBase, public ModuleParams +{ +public: + UWB_SR150(const char *device_name, speed_t baudrate, bool uwb_pos_debug); + + ~UWB_SR150(); + + /** + * @see ModuleBase::custom_command + */ + static int custom_command(int argc, char *argv[]); + + /** + * @see ModuleBase::print_usage + */ + static int print_usage(const char *reason = nullptr); + + /** + * @see ModuleBase::Multilateration + */ + UWB_POS_ERROR_CODES localization(); + + /** + * @see ModuleBase::Distance Result + */ + int distance(); + + /** + * @see ModuleBase::task_spawn + */ + static int task_spawn(int argc, char *argv[]); + + static UWB_SR150 *instantiate(int argc, char *argv[]); + + void run() override; + +private: + void parameters_update(); + + void grid_info_read(position_t *grid); + + DEFINE_PARAMETERS( + (ParamFloat) _uwb_init_off_x, + (ParamFloat) _uwb_init_off_y, + (ParamFloat) _uwb_init_off_z, + (ParamFloat) _uwb_init_off_yaw + ) + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + + int _uart; + fd_set _uart_set; + struct timeval _uart_timeout {}; + bool _uwb_pos_debug; + + uORB::Publication _uwb_grid_pub{ORB_ID(uwb_grid)}; + uwb_grid_s _uwb_grid{}; + + uORB::Publication _uwb_distance_pub{ORB_ID(uwb_distance)}; + uwb_distance_s _uwb_distance{}; + + uORB::Publication _landing_target_pub{ORB_ID(landing_target_pose)}; + landing_target_pose_s _landing_target{}; + + grid_msg_t _uwb_grid_info{}; + distance_msg_t _distance_result_msg{}; + matrix::Vector3f _rel_pos; + + matrix::Dcmf _uwb_init_to_nwu; + matrix::Dcmf _nwu_to_ned{matrix::Eulerf(M_PI_F, 0.0f, 0.0f)}; + matrix::Vector3f _current_position_uwb_init; + matrix::Vector3f _current_position_ned; + matrix::Vector3f _uwb_init_offset_v3f; + + perf_counter_t _read_count_perf; + perf_counter_t _read_err_perf; +}; + +#endif //PX4_RDDRONE_H diff --git a/src/modules/landing_target_estimator/LandingTargetEstimator.cpp b/src/modules/landing_target_estimator/LandingTargetEstimator.cpp index 37ed5c9516..ef22a14bcc 100644 --- a/src/modules/landing_target_estimator/LandingTargetEstimator.cpp +++ b/src/modules/landing_target_estimator/LandingTargetEstimator.cpp @@ -100,58 +100,21 @@ void LandingTargetEstimator::update() } } - if (!_new_irlockReport) { + if (!_new_sensorReport) { // nothing to do return; } // mark this sensor measurement as consumed - _new_irlockReport = false; + _new_sensorReport = 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; - } - - matrix::Vector sensor_ray; // ray pointing towards target in body frame - sensor_ray(0) = _irlockReport.pos_x * _params.scale_x; // forward - sensor_ray(1) = _irlockReport.pos_y * _params.scale_y; // right - sensor_ray(2) = 1.0f; - - // rotate unit ray according to sensor orientation - _S_att = get_rot_matrix(_params.sensor_yaw); - sensor_ray = _S_att * sensor_ray; - - // rotate the unit ray into the navigation 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 - _params.offset_z; - - // 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; - - // Adjust relative position according to sensor offset - _rel_pos(0) += _params.offset_x; - _rel_pos(1) += _params.offset_y; 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("Init %.2f %.2f", (double)vx_init, (double)vy_init); + _kalman_filter_x.init(_target_position_report.rel_pos_x, vx_init, _params.pos_unc_init, _params.vel_unc_init); + _kalman_filter_y.init(_target_position_report.rel_pos_y, vy_init, _params.pos_unc_init, _params.vel_unc_init); _estimator_initialized = true; _last_update = hrt_absolute_time(); @@ -159,8 +122,9 @@ 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); + const float measurement_uncertainty = _params.meas_unc * _dist_z * _dist_z; + bool update_x = _kalman_filter_x.update(_target_position_report.rel_pos_x, measurement_uncertainty); + bool update_y = _kalman_filter_y.update(_target_position_report.rel_pos_y, measurement_uncertainty); if (!update_x || !update_y) { if (!_faulty) { @@ -175,7 +139,7 @@ void LandingTargetEstimator::update() if (!_faulty) { // only publish if both measurements were good - _target_pose.timestamp = _irlockReport.timestamp; + _target_pose.timestamp = _target_position_report.timestamp; float x, xvel, y, yvel, covx, covx_v, covy, covy_v; _kalman_filter_x.getState(x, xvel); @@ -190,7 +154,7 @@ void LandingTargetEstimator::update() _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 = _target_position_report.rel_pos_z ; _target_pose.vx_rel = xvel; _target_pose.vy_rel = yvel; @@ -203,7 +167,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 = _target_position_report.rel_pos_z + _vehicleLocalPosition.z; _target_pose.abs_pos_valid = true; } else { @@ -220,7 +184,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 = _target_position_report.timestamp; _target_innovations.innov_x = innov_x; _target_innovations.innov_cov_x = innov_cov_x; _target_innovations.innov_y = innov_y; @@ -246,7 +210,76 @@ void LandingTargetEstimator::_update_topics() _vehicleAttitude_valid = _attitudeSub.update(&_vehicleAttitude); _vehicle_acceleration_valid = _vehicle_acceleration_sub.update(&_vehicle_acceleration); - _new_irlockReport = _irlockReportSub.update(&_irlockReport); + + if (_irlockReportSub.update(&_irlockReport)) { // + _new_irlockReport = true; + + 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; + } + + matrix::Vector sensor_ray; // ray pointing towards target in body frame + sensor_ray(0) = _irlockReport.pos_x * _params.scale_x; // forward + sensor_ray(1) = _irlockReport.pos_y * _params.scale_y; // right + sensor_ray(2) = 1.0f; + + // rotate unit ray according to sensor orientation + _S_att = get_rot_matrix(_params.sensor_yaw); + sensor_ray = _S_att * sensor_ray; + + // rotate the unit ray into the navigation 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; + } + + _dist_z = _vehicleLocalPosition.dist_bottom - _params.offset_z; + + // scale the ray s.t. the z component has length of _uncertainty_scale + _target_position_report.timestamp = _irlockReport.timestamp; + _target_position_report.rel_pos_x = sensor_ray(0) / sensor_ray(2) * _dist_z; + _target_position_report.rel_pos_y = sensor_ray(1) / sensor_ray(2) * _dist_z; + _target_position_report.rel_pos_z = _dist_z; + + // Adjust relative position according to sensor offset + _target_position_report.rel_pos_x += _params.offset_x; + _target_position_report.rel_pos_y += _params.offset_y; + + _new_sensorReport = true; + + } else if (_uwbDistanceSub.update(&_uwbDistance)) { + if (!_vehicleAttitude_valid || !_vehicleLocalPosition_valid) { + // don't have the data needed for an update + PX4_INFO("Attitude: %d, Local pos: %d", _vehicleAttitude_valid, _vehicleLocalPosition_valid); + return; + } + + if (!PX4_ISFINITE((float)_uwbDistance.position[0]) || !PX4_ISFINITE((float)_uwbDistance.position[1]) || + !PX4_ISFINITE((float)_uwbDistance.position[2])) { + PX4_WARN("Position is corrupt!"); + return; + } + + _new_sensorReport = true; + + // The coordinate system is NED (north-east-down) + // the uwb_distance msg contains the Position in NED, Vehicle relative to LP + // The coordinates "rel_pos_*" are the position of the landing point relative to the vehicle. + // To change POV we negate every Axis: + _target_position_report.timestamp = _uwbDistance.timestamp; + _target_position_report.rel_pos_x = -_uwbDistance.position[0]; + _target_position_report.rel_pos_y = -_uwbDistance.position[1]; + _target_position_report.rel_pos_z = -_uwbDistance.position[2]; + } } void LandingTargetEstimator::_update_params() diff --git a/src/modules/landing_target_estimator/LandingTargetEstimator.h b/src/modules/landing_target_estimator/LandingTargetEstimator.h index 91b96ad897..521a1e3763 100644 --- a/src/modules/landing_target_estimator/LandingTargetEstimator.h +++ b/src/modules/landing_target_estimator/LandingTargetEstimator.h @@ -54,6 +54,9 @@ #include #include #include +#include +#include +#include #include #include #include @@ -139,21 +142,32 @@ private: enum Rotation sensor_yaw; } _params; + struct { + hrt_abstime timestamp; + float rel_pos_x; + float rel_pos_y; + float rel_pos_z; + } _target_position_report; + uORB::Subscription _vehicleLocalPositionSub{ORB_ID(vehicle_local_position)}; uORB::Subscription _attitudeSub{ORB_ID(vehicle_attitude)}; uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)}; uORB::Subscription _irlockReportSub{ORB_ID(irlock_report)}; + uORB::Subscription _uwbDistanceSub{ORB_ID(uwb_distance)}; vehicle_local_position_s _vehicleLocalPosition{}; vehicle_attitude_s _vehicleAttitude{}; vehicle_acceleration_s _vehicle_acceleration{}; irlock_report_s _irlockReport{}; + uwb_grid_s _uwbGrid{}; + uwb_distance_s _uwbDistance{}; // keep track of which topics we have received bool _vehicleLocalPosition_valid{false}; bool _vehicleAttitude_valid{false}; bool _vehicle_acceleration_valid{false}; bool _new_irlockReport{false}; + bool _new_sensorReport{false}; bool _estimator_initialized{false}; // keep track of whether last measurement was rejected bool _faulty{false}; @@ -165,11 +179,10 @@ private: KalmanFilter _kalman_filter_y; hrt_abstime _last_predict{0}; // timestamp of last filter prediction hrt_abstime _last_update{0}; // timestamp of last filter update (used to check timeout) + float _dist_z{1.0f}; void _check_params(const bool force); void _update_state(); }; - - } // namespace landing_target_estimator 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 5cbe7eee18..bb666600d1 100644 --- a/src/modules/landing_target_estimator/landing_target_estimator_main.cpp +++ b/src/modules/landing_target_estimator/landing_target_estimator_main.cpp @@ -98,7 +98,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, + 2100, landing_target_estimator_thread_main, (argv) ? (char *const *)&argv[2] : nullptr); return 0; diff --git a/src/modules/navigator/precland.cpp b/src/modules/navigator/precland.cpp index ab65c12abf..65d8c83a22 100644 --- a/src/modules/navigator/precland.cpp +++ b/src/modules/navigator/precland.cpp @@ -256,21 +256,13 @@ PrecLand::run_state_horizontal_approach() if (hrt_absolute_time() - _point_reached_time > 2000000) { // if close enough for descent above target go to descend above target if (switch_to_state_descend_above_target()) { + return; } } } - if (hrt_absolute_time() - _state_start_time > STATE_TIMEOUT) { - PX4_ERR("Precision landing took too long during horizontal approach phase."); - - if (switch_to_state_fallback()) { - return; - } - - PX4_ERR("Can't switch to fallback landing"); - } float x = _target_pose.x_abs; float y = _target_pose.y_abs; @@ -388,6 +380,7 @@ bool PrecLand::switch_to_state_horizontal_approach() { if (check_state_conditions(PrecLandState::HorizontalApproach)) { + PX4_INFO("Precland: switching to horizontal approach!"); _approach_alt = _navigator->get_global_position()->alt; _point_reached_time = 0; @@ -404,6 +397,7 @@ bool PrecLand::switch_to_state_descend_above_target() { if (check_state_conditions(PrecLandState::DescendAboveTarget)) { + PX4_INFO("Precland: switching to descend!"); _state = PrecLandState::DescendAboveTarget; _state_start_time = hrt_absolute_time(); return true; @@ -416,6 +410,7 @@ bool PrecLand::switch_to_state_final_approach() { if (check_state_conditions(PrecLandState::FinalApproach)) { + PX4_INFO("Precland: switching ot final approach"); _state = PrecLandState::FinalApproach; _state_start_time = hrt_absolute_time(); return true;