mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-25 16:56:25 +08:00
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
@@ -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
|
||||
|
||||
+1
-2
@@ -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
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -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};
|
||||
|
||||
Reference in New Issue
Block a user