From f8c9be087b6653c097958a04f3a5ad4ff510b492 Mon Sep 17 00:00:00 2001 From: Loic Fernau <115560580+NXPLoic@users.noreply.github.com> Date: Wed, 12 Jul 2023 17:44:23 +0200 Subject: [PATCH] drivers: rework NXP UWB driver (#21124) * UWB driver rework that uses 2 UWB MKBoards - 1 as Controller (Initiator), one as Controllee (Anchor) Co-authored-by: NXPBrianna <108274268+NXPBrianna@users.noreply.github.com> --- boards/nxp/fmuk66-e/default.px4board | 1 + boards/nxp/fmurt1062-v1/default.px4board | 2 +- boards/nxp/ucans32k146/default.px4board | 1 + msg/CMakeLists.txt | 3 +- msg/SensorUwb.msg | 34 + msg/UwbDistance.msg | 15 - msg/UwbGrid.msg | 25 - src/drivers/uwb/uwb_sr150/module.yaml | 63 +- src/drivers/uwb/uwb_sr150/uwb_sr150.cpp | 636 +++++------------- src/drivers/uwb/uwb_sr150/uwb_sr150.h | 194 ++---- .../LandingTargetEstimator.cpp | 29 +- .../LandingTargetEstimator.h | 6 - 12 files changed, 316 insertions(+), 693 deletions(-) create mode 100644 msg/SensorUwb.msg delete mode 100644 msg/UwbDistance.msg delete mode 100644 msg/UwbGrid.msg diff --git a/boards/nxp/fmuk66-e/default.px4board b/boards/nxp/fmuk66-e/default.px4board index f53c438f88..64e18c297e 100644 --- a/boards/nxp/fmuk66-e/default.px4board +++ b/boards/nxp/fmuk66-e/default.px4board @@ -19,6 +19,7 @@ CONFIG_DRIVERS_IMU_BOSCH_BMI088=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y CONFIG_DRIVERS_IRLOCK=y +CONFIG_DRIVERS_UWB_UWB_SR150=y CONFIG_COMMON_LIGHT=y CONFIG_DRIVERS_LIGHTS_RGBLED_PWM=y CONFIG_COMMON_MAGNETOMETER=y diff --git a/boards/nxp/fmurt1062-v1/default.px4board b/boards/nxp/fmurt1062-v1/default.px4board index 2a7791db8f..331b6d36bb 100644 --- a/boards/nxp/fmurt1062-v1/default.px4board +++ b/boards/nxp/fmurt1062-v1/default.px4board @@ -22,7 +22,7 @@ CONFIG_DRIVERS_PWM_OUT=y CONFIG_DRIVERS_RC_INPUT=y CONFIG_DRIVERS_SAFETY_BUTTON=y CONFIG_DRIVERS_TONE_ALARM=y -CONFIG_COMMON_UWB=y +CONFIG_DRIVERS_UWB_UWB_SR150=y CONFIG_MODULES_BATTERY_STATUS=y CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y diff --git a/boards/nxp/ucans32k146/default.px4board b/boards/nxp/ucans32k146/default.px4board index eb28094dd9..3eb4d97fb8 100644 --- a/boards/nxp/ucans32k146/default.px4board +++ b/boards/nxp/ucans32k146/default.px4board @@ -13,6 +13,7 @@ CONFIG_COMMON_MAGNETOMETER=y CONFIG_DRIVERS_PWM_OUT=y CONFIG_BOARD_UAVCAN_INTERFACES=1 CONFIG_DRIVERS_UAVCANNODE=y +CONFIG_DRIVERS_UWB_UWB_SR150=y CONFIG_UAVCANNODE_ARMING_STATUS=y CONFIG_UAVCANNODE_BEEP_COMMAND=y CONFIG_UAVCANNODE_ESC_RAW_COMMAND=y diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 3405c37a32..c9e66dbb12 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -178,6 +178,7 @@ set(msg_files SensorSelection.msg SensorsStatus.msg SensorsStatusImu.msg + SensorUwb.msg SystemPower.msg TakeoffStatus.msg TaskStackInfo.msg @@ -194,8 +195,6 @@ set(msg_files UavcanParameterValue.msg UlogStream.msg UlogStreamAck.msg - UwbDistance.msg - UwbGrid.msg VehicleAcceleration.msg VehicleAirData.msg VehicleAngularAccelerationSetpoint.msg diff --git a/msg/SensorUwb.msg b/msg/SensorUwb.msg new file mode 100644 index 0000000000..ae889a8bdc --- /dev/null +++ b/msg/SensorUwb.msg @@ -0,0 +1,34 @@ +# 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 sessionid # UWB SessionID +uint32 time_offset # Time between Ranging Rounds in ms +uint32 counter # Number of Ranges since last Start of Ranging +uint16 mac # MAC adress of Initiator (controller) + +uint16 mac_dest # MAC adress of Responder (Controlee) +uint16 status # status feedback # +uint8 nlos # None line of site condition y/n +float32 distance # distance in m to the UWB receiver + + +#Angle of arrival, Angle in Degree -60..+60; FOV in both axis is 120 degrees +float32 aoa_azimuth_dev # Angle of arrival of first incomming RX msg +float32 aoa_elevation_dev # Angle of arrival of first incomming RX msg +float32 aoa_azimuth_resp # Angle of arrival of first incomming RX msg at the responder +float32 aoa_elevation_resp # Angle of arrival of first incomming RX msg at the responder + +# Figure of merit for the angle measurements +uint8 aoa_azimuth_fom # AOA Azimuth FOM +uint8 aoa_elevation_fom # AOA Elevation FOM +uint8 aoa_dest_azimuth_fom # AOA Azimuth FOM +uint8 aoa_dest_elevation_fom # AOA Elevation FOM + +# Initiator physical configuration +uint8 orientation # Direction the sensor faces from MAV_SENSOR_ORIENTATION enum + # Standard configuration is Antennas facing down and azimuth aligened in forward direction +float32 offset_x # UWB initiator offset in X axis (NED drone frame) +float32 offset_y # UWB initiator offset in Y axis (NED drone frame) +float32 offset_z # UWB initiator offset in Z axis (NED drone frame) diff --git a/msg/UwbDistance.msg b/msg/UwbDistance.msg deleted file mode 100644 index e496a72396..0000000000 --- a/msg/UwbDistance.msg +++ /dev/null @@ -1,15 +0,0 @@ -# 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 which takes part in Ranging) -bool[12] nlos # Visual line of sight yes/no -float32[12] aoafirst # Angle of arrival of first incoming 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/UwbGrid.msg b/msg/UwbGrid.msg deleted file mode 100644 index 0862f84330..0000000000 --- a/msg/UwbGrid.msg +++ /dev/null @@ -1,25 +0,0 @@ -# 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/uwb_sr150/module.yaml b/src/drivers/uwb/uwb_sr150/module.yaml index 68085187bf..721972ba05 100644 --- a/src/drivers/uwb/uwb_sr150/module.yaml +++ b/src/drivers/uwb/uwb_sr150/module.yaml @@ -4,12 +4,11 @@ serial_config: port_config_param: name: UWB_PORT_CFG group: UWB - default: "" + default: "TEL2" parameters: - group: UWB definitions: - UWB_INIT_OFF_X: description: short: UWB sensor X offset in body frame @@ -32,7 +31,7 @@ parameters: UWB_INIT_OFF_Z: description: - short: UWB sensor Y offset in body frame + short: UWB sensor Z offset in body frame long: UWB sensor positioning in relation to Drone in NED. Z offset. type: float unit: m @@ -40,14 +39,52 @@ parameters: increment: 0.01 default: 0.00 - UWB_INIT_OFF_YAW: + UWB_SENS_ROT: 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 - - + short: UWB sensor orientation + long: The orientation of the sensor relative to the forward direction of the body frame. Look up table in src/lib/conversion/rotation.h + Default position is the antannaes downward facing, UWB board parallel with body frame. + type: enum + values: + 0: ROTATION_NONE + 1: ROTATION_YAW_45 + 2: ROTATION_YAW_90 + 3: ROTATION_YAW_135 + 4: ROTATION_YAW_180 + 5: ROTATION_YAW_225 + 6: ROTATION_YAW_270 + 7: ROTATION_YAW_315 + 8: ROTATION_ROLL_180 + 9: ROTATION_ROLL_180_YAW_45 + 10: ROTATION_ROLL_180_YAW_90 + 11: ROTATION_ROLL_180_YAW_135 + 12: ROTATION_PITCH_180 + 13: ROTATION_ROLL_180_YAW_225 + 14: ROTATION_ROLL_180_YAW_270 + 15: ROTATION_ROLL_180_YAW_315 + 16: ROTATION_ROLL_90 + 17: ROTATION_ROLL_90_YAW_45 + 18: ROTATION_ROLL_90_YAW_90 + 19: ROTATION_ROLL_90_YAW_135 + 20: ROTATION_ROLL_270 + 21: ROTATION_ROLL_270_YAW_45 + 22: ROTATION_ROLL_270_YAW_90 + 23: ROTATION_ROLL_270_YAW_135 + 24: ROTATION_PITCH_90 + 25: ROTATION_PITCH_270 + 26: ROTATION_PITCH_180_YAW_90 + 27: ROTATION_PITCH_180_YAW_270 + 28: ROTATION_ROLL_90_PITCH_90 + 29: ROTATION_ROLL_180_PITCH_90 + 30: ROTATION_ROLL_270_PITCH_90 + 31: ROTATION_ROLL_90_PITCH_180 + 32: ROTATION_ROLL_270_PITCH_180 + 33: ROTATION_ROLL_90_PITCH_270 + 34: ROTATION_ROLL_180_PITCH_270 + 35: ROTATION_ROLL_270_PITCH_270 + 36: ROTATION_ROLL_90_PITCH_180_YAW_90 + 37: ROTATION_ROLL_90_YAW_270 + 38: ROTATION_ROLL_90_PITCH_68_YAW_293 + 39: ROTATION_PITCH_315 + 40: ROTATION_ROLL_90_PITCH_315 + default: 0 diff --git a/src/drivers/uwb/uwb_sr150/uwb_sr150.cpp b/src/drivers/uwb/uwb_sr150/uwb_sr150.cpp index 7f917adbc0..ec7b8f69ae 100644 --- a/src/drivers/uwb/uwb_sr150/uwb_sr150.cpp +++ b/src/drivers/uwb/uwb_sr150/uwb_sr150.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020-2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2023 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 @@ -63,128 +63,130 @@ // 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): +UWB_SR150::UWB_SR150(const char *port): ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)), _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"); } + /* store port name */ + strncpy(_port, port, sizeof(_port) - 1); + /* enforce null termination */ + _port[sizeof(_port) - 1] = '\0'; } 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)); - } + // stop{}; will be implemented when this is changed to a scheduled work task perf_free(_read_err_perf); perf_free(_read_count_perf); close(_uart); } -void UWB_SR150::run() +bool UWB_SR150::init() { - // 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); + // execute Run() on every sensor_accel publication + if (!_sensor_uwb_sub.registerCallback()) { + PX4_ERR("callback registration failed"); + return false; } - while (!should_exit()) { - written = UWB_SR150::distance(); //evaluate Ranging Messages until Stop - } + // alternatively, Run on fixed interval + // ScheduleOnInterval(5000_us); // 2000 us interval, 200 Hz rate - 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)); - } + return true; } -void UWB_SR150::grid_info_read(position_t *grid) +void UWB_SR150::start() { - //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"); + /* schedule a cycle to start things */ + ScheduleNow(); +} - int bread = 0; +void UWB_SR150::stop() +{ + ScheduleClear(); +} - for (int i = 0; i < 5; i++) { - bread += fscanf(file, "%hd,%hd,%hd\n", &grid[i].x, &grid[i].y, &grid[i].z); +void UWB_SR150::Run() +{ + if (should_exit()) { + ScheduleClear(); + exit_and_cleanup(); + return; } - if (bread == 5 * 3) { - PX4_INFO("GRID INFO READ! bytes read: %d \t\n", bread); + if (_uart < 0) { + /* open fd */ + _uart = ::open(_port, O_RDWR | O_NOCTTY | O_NONBLOCK); - } 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"); + if (_uart < 0) { + PX4_ERR("open failed (%i)", errno); + return; + } + + struct termios uart_config; + + int termios_state; + + /* fill the struct for the new configuration */ + tcgetattr(_uart, &uart_config); + + /* clear ONLCR flag (which appends a CR for every LF) */ + uart_config.c_oflag &= ~ONLCR; + + //TODO: should I keep this? + /* no parity, one stop bit */ + uart_config.c_cflag &= ~(CSTOPB | PARENB); + + unsigned speed = DEFAULT_BAUD; + + /* set baud rate */ + if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) { + PX4_ERR("CFG: %d ISPD", termios_state); + } + + if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) { + PX4_ERR("CFG: %d OSPD", termios_state); + } + + if ((termios_state = tcsetattr(_uart, TCSANOW, &uart_config)) < 0) { + PX4_ERR("baud %d ATTR", termios_state); + } } - fclose(file); + /* perform collection */ + int collect_ret = collectData(); + + if (collect_ret == -EAGAIN) { + /* reschedule to grab the missing bits, time to transmit 8 bytes @ 9600 bps */ + ScheduleDelayed(1042 * 8); + + return; + } + + if (OK != collect_ret) { + + /* we know the sensor needs about four seconds to initialize */ + if (hrt_absolute_time() > 5 * 1000 * 1000LL && _consecutive_fail_count < 5) { + PX4_ERR("collection error #%u", _consecutive_fail_count); + } + + _consecutive_fail_count++; + + /* restart the measurement state machine */ + start(); + return; + + } else { + /* apparently success */ + _consecutive_fail_count = 0; + } } int UWB_SR150::custom_command(int argc, char *argv[]) @@ -214,43 +216,20 @@ $ uwb start -d /dev/ttyS2 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; + const char *device_name = UWB_DEFAULT_PORT; 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) { + while ((ch = px4_getopt(argc, argv, "d:b", &option_index, &option_arg)) != EOF) { switch (ch) { case 'd': device_name = option_arg; @@ -260,47 +239,54 @@ UWB_SR150 *UWB_SR150::instantiate(int argc, char *argv[]) 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; - } + UWB_SR150 *instance = new UWB_SR150(device_name); - if (!error_flag && baudrate == 0) { - printf("Baudrate not provided. Using default Baud: 115200 \n"); - baudrate = B115200; - } + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; - if (!error_flag && uwb_pos_debug == true) { - printf("UWB Position algorithm Debugging \n"); - } + instance->ScheduleOnInterval(5000_us); - if (error_flag) { - PX4_WARN("Failed to start UWB driver. \n"); - return nullptr; + if (instance->init()) { + return PX4_OK; + } } else { - PX4_INFO("Constructing UWB_SR150. Device: %s", device_name); - return new UWB_SR150(device_name, baudrate, uwb_pos_debug); + PX4_ERR("alloc failed"); + } + + delete instance; + _object.store(nullptr); + _task_id = -1; + + return PX4_ERROR; +} + +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(); } } -int UWB_SR150::distance() +int UWB_SR150::collectData() { - _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); @@ -350,366 +336,54 @@ int UWB_SR150::distance() 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) + // - Size of message == sizeof(distance_msg_t) (36 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) - //); + bool ok = (buffer_location == sizeof(distance_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; + _sensor_uwb.timestamp = hrt_absolute_time(); - 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 - } + _sensor_uwb.sessionid = _distance_result_msg.sessionId; + _sensor_uwb.time_offset = _distance_result_msg.range_interval; + _sensor_uwb.counter = _distance_result_msg.seq_ctr; + _sensor_uwb.mac = _distance_result_msg.MAC; - // Algorithm goes here - UWB_POS_ERROR_CODES UWB_POS_ERROR = UWB_SR150::localization(); + _sensor_uwb.mac_dest = _distance_result_msg.measurements.MAC; + _sensor_uwb.status = _distance_result_msg.measurements.status; + _sensor_uwb.nlos = _distance_result_msg.measurements.nLos; + _sensor_uwb.distance = double(_distance_result_msg.measurements.distance) / 100; - _uwb_distance.status = UWB_POS_ERROR; - if (UWB_OK == UWB_POS_ERROR) { + /*Angle of Arrival has Format Q9.7; dividing by 2^7 and negating results in the correct value*/ + _sensor_uwb.aoa_azimuth_dev = - double(_distance_result_msg.measurements.aoa_azimuth) / 128; + _sensor_uwb.aoa_elevation_dev = - double(_distance_result_msg.measurements.aoa_elevation) / 128; + _sensor_uwb.aoa_azimuth_resp = - double(_distance_result_msg.measurements.aoa_dest_azimuth) / 128; + _sensor_uwb.aoa_elevation_resp = - double(_distance_result_msg.measurements.aoa_dest_elevation) / 128; - _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; + /*Angle of Arrival has Format Q9.7; dividing by 2^7 and negating results in the correct value*/ + _sensor_uwb.aoa_azimuth_fom = - double(_distance_result_msg.measurements.aoa_azimuth) / 128; + _sensor_uwb.aoa_elevation_fom = - double(_distance_result_msg.measurements.aoa_elevation) / 128; + _sensor_uwb.aoa_dest_azimuth_fom = - double(_distance_result_msg.measurements.aoa_dest_azimuth) / 128; + _sensor_uwb.aoa_dest_elevation_fom = - double(_distance_result_msg.measurements.aoa_dest_elevation) / 128; - case UWB_LIN_DEP_FOR_THREE: - PX4_INFO("UWB localization: linear dependent with 3 Anchors"); - break; + /* Sensor physical offset*/ //for now we propagate the physical configuration via Uorb + _sensor_uwb.orientation = _sensor_rot.get(); + _sensor_uwb.offset_x = _offset_x.get(); + _sensor_uwb.offset_y = _offset_y.get(); + _sensor_uwb.offset_z = _offset_z.get(); - 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 dependent 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); + _sensor_uwb_pub.publish(_sensor_uwb); } 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 += sq((int64_t)(anchor_pos[i].x - anchor_pos[0].x)); - 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 += sq((int64_t)(anchor_pos[i].y - anchor_pos[0].y)); - 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 += sq((int64_t)(anchor_pos[i].z - anchor_pos[0].z)); - - /* Vector */ - temp = sq(distances_cm[0]) - sq(distances_cm[i]) - + sq(anchor_pos[i].x) + sq(anchor_pos[i].y) - + sq(anchor_pos[i].z) - sq(anchor_pos[0].x) - - sq(anchor_pos[0].y) - sq(anchor_pos[0].z); - - 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 index 5b42b50e16..a405c361ea 100644 --- a/src/drivers/uwb/uwb_sr150/uwb_sr150.h +++ b/src/drivers/uwb/uwb_sr150/uwb_sr150.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020-2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2023 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 @@ -38,101 +38,63 @@ #include #include #include +#include +#include #include #include -#include +#include + #include -#include -#include -#include -#include #include +#include #include -#include +#include +#include + #include -#include + +#define UWB_DEFAULT_PORT "/dev/ttyS1" 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 + uint16_t MAC; // MAC address of UWB device + uint8_t status; // Status of Measurement + uint16_t distance; // Distance in cm + uint8_t nLos; // line of sight y/n + int16_t aoa_azimuth; // AOA of incoming msg for Azimuth antenna pairing + int16_t aoa_elevation; // AOA of incoming msg for Altitude antenna pairing + int16_t aoa_dest_azimuth; // AOA destination Azimuth + int16_t aoa_dest_elevation; // AOA destination elevation + uint8_t aoa_azimuth_FOM; // AOA Azimuth FOM + uint8_t aoa_elevation_FOM; // AOA Elevation FOM + uint8_t aoa_dest_azimuth_FOM; // AOA Azimuth FOM + uint8_t aoa_dest_elevation_FOM; // AOA Elevation FOM } __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 + uint8_t cmd; // Should be 0x8E for distance result message + uint16_t len; // Should be 0x30 for distance result message + 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 + uint16_t MAC; // MAC address of UWB device + UWB_range_meas_t measurements; //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 +class UWB_SR150 : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { public: - UWB_SR150(const char *device_name, speed_t baudrate, bool uwb_pos_debug); - + UWB_SR150(const char *port); ~UWB_SR150(); + /** + * @see ModuleBase::task_spawn + */ + static int task_spawn(int argc, char *argv[]); + /** * @see ModuleBase::custom_command */ @@ -143,67 +105,51 @@ public: */ static int print_usage(const char *reason = nullptr); - /** - * @see ModuleBase::Multilateration - */ - UWB_POS_ERROR_CODES localization(); + bool init(); - /** - * @see ModuleBase::Distance Result - */ - int distance(); + void start(); - /** - * @see ModuleBase::task_spawn - */ - static int task_spawn(int argc, char *argv[]); + void stop(); - static UWB_SR150 *instantiate(int argc, char *argv[]); - - void run() override; + int collectData(); private: - static constexpr int64_t sq(int64_t x) { return x * x; } void parameters_update(); - void grid_info_read(position_t *grid); + void Run() override; - DEFINE_PARAMETERS( - (ParamFloat) _uwb_init_off_x, - (ParamFloat) _uwb_init_off_y, - (ParamFloat) _uwb_init_off_z, - (ParamFloat) _uwb_init_off_yaw - ) + // Publications + uORB::Publication _sensor_uwb_pub{ORB_ID(sensor_uwb)}; + // Subscriptions + uORB::SubscriptionCallbackWorkItem _sensor_uwb_sub{this, ORB_ID(sensor_uwb)}; 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; - + // Parameters + DEFINE_PARAMETERS( + (ParamInt) _uwb_port_cfg, + (ParamFloat) _offset_x, + (ParamFloat) _offset_y, + (ParamFloat) _offset_z, + (ParamInt) _sensor_rot + ) + // Performance (perf) counters perf_counter_t _read_count_perf; perf_counter_t _read_err_perf; -}; + sensor_uwb_s _sensor_uwb{}; + + char _port[20] {}; + hrt_abstime param_timestamp{0}; + + int _uart{-1}; + fd_set _uart_set; + struct timeval _uart_timeout {}; + + unsigned _consecutive_fail_count; + int _interval{100000}; + + distance_msg_t _distance_result_msg{}; +}; #endif //PX4_RDDRONE_H diff --git a/src/modules/landing_target_estimator/LandingTargetEstimator.cpp b/src/modules/landing_target_estimator/LandingTargetEstimator.cpp index 3783c3d852..5e09de57d7 100644 --- a/src/modules/landing_target_estimator/LandingTargetEstimator.cpp +++ b/src/modules/landing_target_estimator/LandingTargetEstimator.cpp @@ -100,13 +100,13 @@ void LandingTargetEstimator::update() } } - if (!_new_sensorReport) { + if (!_new_irlockReport) { // nothing to do return; } // mark this sensor measurement as consumed - _new_sensorReport = false; + _new_irlockReport = false; if (!_estimator_initialized) { @@ -254,30 +254,7 @@ void LandingTargetEstimator::_update_topics() _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 (!matrix::Vector3f(_uwbDistance.position).isAllFinite()) { - PX4_WARN("Marker position reading invalid!"); - 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]; + _new_irlockReport = true; } } diff --git a/src/modules/landing_target_estimator/LandingTargetEstimator.h b/src/modules/landing_target_estimator/LandingTargetEstimator.h index 521a1e3763..5c78147329 100644 --- a/src/modules/landing_target_estimator/LandingTargetEstimator.h +++ b/src/modules/landing_target_estimator/LandingTargetEstimator.h @@ -54,9 +54,6 @@ #include #include #include -#include -#include -#include #include #include #include @@ -153,14 +150,11 @@ private: 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};