forked from Archive/PX4-Autopilot
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>
This commit is contained in:
Loic Fernau
GitHub
parent
715a1ff701
commit
f8c9be087b
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
@ -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)
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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, "<file:dev>", "Name of device for serial communication with UWB", false);
|
||||
PRINT_MODULE_USAGE_PARAM_STRING('b', nullptr, "<int>", "Baudrate for serial communication", false);
|
||||
PRINT_MODULE_USAGE_PARAM_STRING('p', nullptr, "<int>", "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();
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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 <poll.h>
|
||||
#include <sys/select.h>
|
||||
#include <sys/time.h>
|
||||
#include <perf/perf_counter.h>
|
||||
#include <lib/conversion/rotation.h>
|
||||
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include <px4_platform_common/module.h>
|
||||
#include <perf/perf_counter.h>
|
||||
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
|
||||
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/topics/landing_target_pose.h>
|
||||
#include <uORB/topics/uwb_grid.h>
|
||||
#include <uORB/topics/uwb_distance.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/SubscriptionCallback.hpp>
|
||||
#include <uORB/SubscriptionInterval.hpp>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/sensor_uwb.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
|
||||
#include <matrix/math.hpp>
|
||||
#include <matrix/Matrix.hpp>
|
||||
|
||||
#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<UWB_SR150>, public ModuleParams
|
||||
class UWB_SR150 : public ModuleBase<UWB_SR150>, 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<px4::params::UWB_INIT_OFF_X>) _uwb_init_off_x,
|
||||
(ParamFloat<px4::params::UWB_INIT_OFF_Y>) _uwb_init_off_y,
|
||||
(ParamFloat<px4::params::UWB_INIT_OFF_Z>) _uwb_init_off_z,
|
||||
(ParamFloat<px4::params::UWB_INIT_OFF_YAW>) _uwb_init_off_yaw
|
||||
)
|
||||
// Publications
|
||||
uORB::Publication<sensor_uwb_s> _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_s> _uwb_grid_pub{ORB_ID(uwb_grid)};
|
||||
uwb_grid_s _uwb_grid{};
|
||||
|
||||
uORB::Publication<uwb_distance_s> _uwb_distance_pub{ORB_ID(uwb_distance)};
|
||||
uwb_distance_s _uwb_distance{};
|
||||
|
||||
uORB::Publication<landing_target_pose_s> _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<px4::params::UWB_PORT_CFG>) _uwb_port_cfg,
|
||||
(ParamFloat<px4::params::UWB_INIT_OFF_X>) _offset_x,
|
||||
(ParamFloat<px4::params::UWB_INIT_OFF_Y>) _offset_y,
|
||||
(ParamFloat<px4::params::UWB_INIT_OFF_Z>) _offset_z,
|
||||
(ParamInt<px4::params::UWB_SENS_ROT>) _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
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -54,9 +54,6 @@
|
|||
#include <uORB/topics/irlock_report.h>
|
||||
#include <uORB/topics/landing_target_pose.h>
|
||||
#include <uORB/topics/landing_target_innovations.h>
|
||||
#include <uORB/topics/uwb_distance.h>
|
||||
#include <uORB/topics/uwb_grid.h>
|
||||
#include <uORB/topics/estimator_sensor_bias.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <matrix/math.hpp>
|
||||
#include <mathlib/mathlib.h>
|
||||
|
|
@ -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};
|
||||
|
|
|
|||
Loading…
Reference in New Issue