mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-28 10:46:33 +08:00
Support for NXP UWB position sensor
uwb_sr150 driver for the sensor, and some modifications in precision landing to allow landing on a platform using the UWB system.
This commit is contained in:
Hovergames
alessandro
@@ -0,0 +1,34 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2020 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
# are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# 2. Redistributions in binary form must reproduce the above copyright
|
||||
# notice, this list of conditions and the following disclaimer in
|
||||
# the documentation and/or other materials provided with the
|
||||
# distribution.
|
||||
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||
# used to endorse or promote products derived from this software
|
||||
# without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
add_subdirectory(uwb_sr150)
|
||||
@@ -0,0 +1,9 @@
|
||||
menu "UWB"
|
||||
menuconfig COMMON_UWB
|
||||
bool "common UWB Drivers"
|
||||
default n
|
||||
select DRIVERS_UWB_UWB_SR150
|
||||
---help---
|
||||
Enable support for uwb drivers
|
||||
rsource "*/Kconfig"
|
||||
endmenu
|
||||
@@ -0,0 +1,44 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
# are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# 2. Redistributions in binary form must reproduce the above copyright
|
||||
# notice, this list of conditions and the following disclaimer in
|
||||
# the documentation and/or other materials provided with the
|
||||
# distribution.
|
||||
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||
# used to endorse or promote products derived from this software
|
||||
# without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
px4_add_module(
|
||||
MODULE drivers__uwb_sr150
|
||||
MAIN uwb_sr150
|
||||
SRCS
|
||||
uwb_sr150.cpp
|
||||
uwb_sr150.h
|
||||
MODULE_CONFIG
|
||||
module.yaml
|
||||
DEPENDS
|
||||
px4_work_queue
|
||||
)
|
||||
@@ -0,0 +1,5 @@
|
||||
menuconfig DRIVERS_UWB_UWB_SR150
|
||||
bool "uwb_sr150"
|
||||
default n
|
||||
---help---
|
||||
Enable support for uwb sr150
|
||||
@@ -0,0 +1,53 @@
|
||||
module_name: Ultrawideband position sensor driver
|
||||
serial_config:
|
||||
- command: uwb_sr150 start -d ${SERIAL_DEV} -b p:${BAUD_PARAM}
|
||||
port_config_param:
|
||||
name: UWB_PORT_CFG
|
||||
group: UWB
|
||||
default: ""
|
||||
|
||||
parameters:
|
||||
- group: UWB
|
||||
definitions:
|
||||
|
||||
UWB_INIT_OFF_X:
|
||||
description:
|
||||
short: UWB sensor X offset in body frame
|
||||
long: UWB sensor positioning in relation to Drone in NED. X offset. A Positiv offset results in a Position o
|
||||
type: float
|
||||
unit: m
|
||||
decimal: 2
|
||||
increment: 0.01
|
||||
default: 0.00
|
||||
|
||||
UWB_INIT_OFF_Y:
|
||||
description:
|
||||
short: UWB sensor Y offset in body frame
|
||||
long: UWB sensor positioning in relation to Drone in NED. Y offset.
|
||||
type: float
|
||||
unit: m
|
||||
decimal: 2
|
||||
increment: 0.01
|
||||
default: 0.00
|
||||
|
||||
UWB_INIT_OFF_Z:
|
||||
description:
|
||||
short: UWB sensor Y offset in body frame
|
||||
long: UWB sensor positioning in relation to Drone in NED. Z offset.
|
||||
type: float
|
||||
unit: m
|
||||
decimal: 2
|
||||
increment: 0.01
|
||||
default: 0.00
|
||||
|
||||
UWB_INIT_OFF_YAW:
|
||||
description:
|
||||
short: UWB sensor YAW offset in body frame
|
||||
long: UWB sensor positioning in relation to Drone in NED. Yaw rotation in relation to direction of FMU.
|
||||
type: float
|
||||
unit: deg
|
||||
decimal: 1
|
||||
increment: 0.1
|
||||
default: 0.00
|
||||
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,207 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#ifndef PX4_RDDRONE_H
|
||||
#define PX4_RDDRONE_H
|
||||
|
||||
#include <termios.h>
|
||||
#include <poll.h>
|
||||
#include <sys/select.h>
|
||||
#include <sys/time.h>
|
||||
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include <px4_platform_common/module.h>
|
||||
#include <perf/perf_counter.h>
|
||||
#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/SubscriptionInterval.hpp>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <matrix/math.hpp>
|
||||
#include <matrix/Matrix.hpp>
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
#define UWB_CMD 0x8e
|
||||
#define UWB_CMD_START 0x01
|
||||
#define UWB_CMD_STOP 0x00
|
||||
#define UWB_CMD_RANGING 0x0A
|
||||
#define STOP_B 0x0A
|
||||
|
||||
#define UWB_PRECNAV_APP 0x04
|
||||
#define UWB_APP_START 0x10
|
||||
#define UWB_APP_STOP 0x11
|
||||
#define UWB_SESSION_START 0x22
|
||||
#define UWB_SESSION_STOP 0x23
|
||||
#define UWB_RANGING_START 0x01
|
||||
#define UWB_RANGING_STOP 0x00
|
||||
#define UWB_DRONE_CTL 0x0A
|
||||
|
||||
#define UWB_CMD_LEN 0x05
|
||||
#define UWB_CMD_DISTANCE_LEN 0x21
|
||||
#define UWB_MAC_MODE 2
|
||||
#define MAX_ANCHORS 12
|
||||
#define UWB_GRID_CONFIG "/fs/microsd/etc/uwb_grid_config.csv"
|
||||
|
||||
typedef struct { //needs higher accuracy?
|
||||
float lat, lon, alt, yaw; //offset to true North
|
||||
} gps_pos_t;
|
||||
|
||||
typedef struct {
|
||||
int16_t x, y, z; //axis in cm
|
||||
} position_t; // Position of a device or target in 3D space
|
||||
|
||||
enum UWB_POS_ERROR_CODES {
|
||||
UWB_OK,
|
||||
UWB_ANC_BELOW_THREE,
|
||||
UWB_LIN_DEP_FOR_THREE,
|
||||
UWB_ANC_ON_ONE_LEVEL,
|
||||
UWB_LIN_DEP_FOR_FOUR,
|
||||
UWB_RANK_ZERO
|
||||
};
|
||||
|
||||
typedef struct {
|
||||
uint8_t MAC[2]; // MAC Adress of UWB device
|
||||
uint8_t status; // Status of Measurement
|
||||
uint16_t distance; // Distance in cm
|
||||
uint8_t nLos; // line of sight y/n
|
||||
uint16_t aoaFirst; // Angle of Arrival of incoming msg
|
||||
} __attribute__((packed)) UWB_range_meas_t;
|
||||
|
||||
typedef struct {
|
||||
uint32_t initator_time; //timestamp of init
|
||||
uint32_t sessionId; // Session ID of UWB session
|
||||
uint8_t num_anchors; //number of anchors
|
||||
gps_pos_t target_gps; //GPS position of Landing Point
|
||||
uint8_t mac_mode; // MAC adress mode, either 2 Byte or 8 Byte
|
||||
uint8_t MAC[UWB_MAC_MODE][MAX_ANCHORS];
|
||||
position_t target_pos; //target position
|
||||
position_t anchor_pos[MAX_ANCHORS]; // Position of each anchor
|
||||
uint8_t stop; // Should be 27
|
||||
} grid_msg_t;
|
||||
|
||||
typedef struct {
|
||||
uint8_t cmd; // Should be 0x8E for distance result message
|
||||
uint16_t len; // Should be 0x30 for distance result message
|
||||
uint32_t time_uwb_ms; // Timestamp of UWB device in ms
|
||||
uint32_t seq_ctr; // Number of Ranges since last Start of Ranging
|
||||
uint32_t sessionId; // Session ID of UWB session
|
||||
uint32_t range_interval; // Time between ranging rounds
|
||||
uint8_t mac_mode; // MAC adress mode, either 2 Byte or 8 Byte
|
||||
uint8_t no_measurements; // MAC adress mode, either 2 Byte or 8 Byte
|
||||
UWB_range_meas_t measurements[4]; //Raw anchor_distance distances in CM 2*9
|
||||
uint8_t stop; // Should be 0x1B
|
||||
} __attribute__((packed)) distance_msg_t;
|
||||
|
||||
class UWB_SR150 : public ModuleBase<UWB_SR150>, public ModuleParams
|
||||
{
|
||||
public:
|
||||
UWB_SR150(const char *device_name, speed_t baudrate, bool uwb_pos_debug);
|
||||
|
||||
~UWB_SR150();
|
||||
|
||||
/**
|
||||
* @see ModuleBase::custom_command
|
||||
*/
|
||||
static int custom_command(int argc, char *argv[]);
|
||||
|
||||
/**
|
||||
* @see ModuleBase::print_usage
|
||||
*/
|
||||
static int print_usage(const char *reason = nullptr);
|
||||
|
||||
/**
|
||||
* @see ModuleBase::Multilateration
|
||||
*/
|
||||
UWB_POS_ERROR_CODES localization();
|
||||
|
||||
/**
|
||||
* @see ModuleBase::Distance Result
|
||||
*/
|
||||
int distance();
|
||||
|
||||
/**
|
||||
* @see ModuleBase::task_spawn
|
||||
*/
|
||||
static int task_spawn(int argc, char *argv[]);
|
||||
|
||||
static UWB_SR150 *instantiate(int argc, char *argv[]);
|
||||
|
||||
void run() override;
|
||||
|
||||
private:
|
||||
void parameters_update();
|
||||
|
||||
void grid_info_read(position_t *grid);
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamFloat<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
|
||||
)
|
||||
|
||||
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;
|
||||
|
||||
perf_counter_t _read_count_perf;
|
||||
perf_counter_t _read_err_perf;
|
||||
};
|
||||
|
||||
#endif //PX4_RDDRONE_H
|
||||
@@ -100,58 +100,21 @@ void LandingTargetEstimator::update()
|
||||
}
|
||||
}
|
||||
|
||||
if (!_new_irlockReport) {
|
||||
if (!_new_sensorReport) {
|
||||
// nothing to do
|
||||
return;
|
||||
}
|
||||
|
||||
// mark this sensor measurement as consumed
|
||||
_new_irlockReport = false;
|
||||
_new_sensorReport = false;
|
||||
|
||||
if (!_vehicleAttitude_valid || !_vehicleLocalPosition_valid || !_vehicleLocalPosition.dist_bottom_valid) {
|
||||
// don't have the data needed for an update
|
||||
return;
|
||||
}
|
||||
|
||||
if (!PX4_ISFINITE(_irlockReport.pos_y) || !PX4_ISFINITE(_irlockReport.pos_x)) {
|
||||
return;
|
||||
}
|
||||
|
||||
matrix::Vector<float, 3> sensor_ray; // ray pointing towards target in body frame
|
||||
sensor_ray(0) = _irlockReport.pos_x * _params.scale_x; // forward
|
||||
sensor_ray(1) = _irlockReport.pos_y * _params.scale_y; // right
|
||||
sensor_ray(2) = 1.0f;
|
||||
|
||||
// rotate unit ray according to sensor orientation
|
||||
_S_att = get_rot_matrix(_params.sensor_yaw);
|
||||
sensor_ray = _S_att * sensor_ray;
|
||||
|
||||
// rotate the unit ray into the navigation frame
|
||||
matrix::Quaternion<float> q_att(&_vehicleAttitude.q[0]);
|
||||
_R_att = matrix::Dcm<float>(q_att);
|
||||
sensor_ray = _R_att * sensor_ray;
|
||||
|
||||
if (fabsf(sensor_ray(2)) < 1e-6f) {
|
||||
// z component of measurement unsafe, don't use this measurement
|
||||
return;
|
||||
}
|
||||
|
||||
float dist = _vehicleLocalPosition.dist_bottom - _params.offset_z;
|
||||
|
||||
// scale the ray s.t. the z component has length of dist
|
||||
_rel_pos(0) = sensor_ray(0) / sensor_ray(2) * dist;
|
||||
_rel_pos(1) = sensor_ray(1) / sensor_ray(2) * dist;
|
||||
|
||||
// Adjust relative position according to sensor offset
|
||||
_rel_pos(0) += _params.offset_x;
|
||||
_rel_pos(1) += _params.offset_y;
|
||||
|
||||
if (!_estimator_initialized) {
|
||||
PX4_INFO("Init");
|
||||
float vx_init = _vehicleLocalPosition.v_xy_valid ? -_vehicleLocalPosition.vx : 0.f;
|
||||
float vy_init = _vehicleLocalPosition.v_xy_valid ? -_vehicleLocalPosition.vy : 0.f;
|
||||
_kalman_filter_x.init(_rel_pos(0), vx_init, _params.pos_unc_init, _params.vel_unc_init);
|
||||
_kalman_filter_y.init(_rel_pos(1), vy_init, _params.pos_unc_init, _params.vel_unc_init);
|
||||
PX4_INFO("Init %.2f %.2f", (double)vx_init, (double)vy_init);
|
||||
_kalman_filter_x.init(_target_position_report.rel_pos_x, vx_init, _params.pos_unc_init, _params.vel_unc_init);
|
||||
_kalman_filter_y.init(_target_position_report.rel_pos_y, vy_init, _params.pos_unc_init, _params.vel_unc_init);
|
||||
|
||||
_estimator_initialized = true;
|
||||
_last_update = hrt_absolute_time();
|
||||
@@ -159,8 +122,9 @@ void LandingTargetEstimator::update()
|
||||
|
||||
} else {
|
||||
// update
|
||||
bool update_x = _kalman_filter_x.update(_rel_pos(0), _params.meas_unc * dist * dist);
|
||||
bool update_y = _kalman_filter_y.update(_rel_pos(1), _params.meas_unc * dist * dist);
|
||||
const float measurement_uncertainty = _params.meas_unc * _dist_z * _dist_z;
|
||||
bool update_x = _kalman_filter_x.update(_target_position_report.rel_pos_x, measurement_uncertainty);
|
||||
bool update_y = _kalman_filter_y.update(_target_position_report.rel_pos_y, measurement_uncertainty);
|
||||
|
||||
if (!update_x || !update_y) {
|
||||
if (!_faulty) {
|
||||
@@ -175,7 +139,7 @@ void LandingTargetEstimator::update()
|
||||
if (!_faulty) {
|
||||
// only publish if both measurements were good
|
||||
|
||||
_target_pose.timestamp = _irlockReport.timestamp;
|
||||
_target_pose.timestamp = _target_position_report.timestamp;
|
||||
|
||||
float x, xvel, y, yvel, covx, covx_v, covy, covy_v;
|
||||
_kalman_filter_x.getState(x, xvel);
|
||||
@@ -190,7 +154,7 @@ void LandingTargetEstimator::update()
|
||||
_target_pose.rel_vel_valid = true;
|
||||
_target_pose.x_rel = x;
|
||||
_target_pose.y_rel = y;
|
||||
_target_pose.z_rel = dist;
|
||||
_target_pose.z_rel = _target_position_report.rel_pos_z ;
|
||||
_target_pose.vx_rel = xvel;
|
||||
_target_pose.vy_rel = yvel;
|
||||
|
||||
@@ -203,7 +167,7 @@ void LandingTargetEstimator::update()
|
||||
if (_vehicleLocalPosition_valid && _vehicleLocalPosition.xy_valid) {
|
||||
_target_pose.x_abs = x + _vehicleLocalPosition.x;
|
||||
_target_pose.y_abs = y + _vehicleLocalPosition.y;
|
||||
_target_pose.z_abs = dist + _vehicleLocalPosition.z;
|
||||
_target_pose.z_abs = _target_position_report.rel_pos_z + _vehicleLocalPosition.z;
|
||||
_target_pose.abs_pos_valid = true;
|
||||
|
||||
} else {
|
||||
@@ -220,7 +184,7 @@ void LandingTargetEstimator::update()
|
||||
_kalman_filter_x.getInnovations(innov_x, innov_cov_x);
|
||||
_kalman_filter_y.getInnovations(innov_y, innov_cov_y);
|
||||
|
||||
_target_innovations.timestamp = _irlockReport.timestamp;
|
||||
_target_innovations.timestamp = _target_position_report.timestamp;
|
||||
_target_innovations.innov_x = innov_x;
|
||||
_target_innovations.innov_cov_x = innov_cov_x;
|
||||
_target_innovations.innov_y = innov_y;
|
||||
@@ -246,7 +210,76 @@ void LandingTargetEstimator::_update_topics()
|
||||
_vehicleAttitude_valid = _attitudeSub.update(&_vehicleAttitude);
|
||||
_vehicle_acceleration_valid = _vehicle_acceleration_sub.update(&_vehicle_acceleration);
|
||||
|
||||
_new_irlockReport = _irlockReportSub.update(&_irlockReport);
|
||||
|
||||
if (_irlockReportSub.update(&_irlockReport)) { //
|
||||
_new_irlockReport = true;
|
||||
|
||||
if (!_vehicleAttitude_valid || !_vehicleLocalPosition_valid || !_vehicleLocalPosition.dist_bottom_valid) {
|
||||
// don't have the data needed for an update
|
||||
return;
|
||||
}
|
||||
|
||||
if (!PX4_ISFINITE(_irlockReport.pos_y) || !PX4_ISFINITE(_irlockReport.pos_x)) {
|
||||
return;
|
||||
}
|
||||
|
||||
matrix::Vector<float, 3> sensor_ray; // ray pointing towards target in body frame
|
||||
sensor_ray(0) = _irlockReport.pos_x * _params.scale_x; // forward
|
||||
sensor_ray(1) = _irlockReport.pos_y * _params.scale_y; // right
|
||||
sensor_ray(2) = 1.0f;
|
||||
|
||||
// rotate unit ray according to sensor orientation
|
||||
_S_att = get_rot_matrix(_params.sensor_yaw);
|
||||
sensor_ray = _S_att * sensor_ray;
|
||||
|
||||
// rotate the unit ray into the navigation frame
|
||||
matrix::Quaternion<float> q_att(&_vehicleAttitude.q[0]);
|
||||
_R_att = matrix::Dcm<float>(q_att);
|
||||
sensor_ray = _R_att * sensor_ray;
|
||||
|
||||
if (fabsf(sensor_ray(2)) < 1e-6f) {
|
||||
// z component of measurement unsafe, don't use this measurement
|
||||
return;
|
||||
}
|
||||
|
||||
_dist_z = _vehicleLocalPosition.dist_bottom - _params.offset_z;
|
||||
|
||||
// scale the ray s.t. the z component has length of _uncertainty_scale
|
||||
_target_position_report.timestamp = _irlockReport.timestamp;
|
||||
_target_position_report.rel_pos_x = sensor_ray(0) / sensor_ray(2) * _dist_z;
|
||||
_target_position_report.rel_pos_y = sensor_ray(1) / sensor_ray(2) * _dist_z;
|
||||
_target_position_report.rel_pos_z = _dist_z;
|
||||
|
||||
// Adjust relative position according to sensor offset
|
||||
_target_position_report.rel_pos_x += _params.offset_x;
|
||||
_target_position_report.rel_pos_y += _params.offset_y;
|
||||
|
||||
_new_sensorReport = true;
|
||||
|
||||
} else if (_uwbDistanceSub.update(&_uwbDistance)) {
|
||||
if (!_vehicleAttitude_valid || !_vehicleLocalPosition_valid) {
|
||||
// don't have the data needed for an update
|
||||
PX4_INFO("Attitude: %d, Local pos: %d", _vehicleAttitude_valid, _vehicleLocalPosition_valid);
|
||||
return;
|
||||
}
|
||||
|
||||
if (!PX4_ISFINITE((float)_uwbDistance.position[0]) || !PX4_ISFINITE((float)_uwbDistance.position[1]) ||
|
||||
!PX4_ISFINITE((float)_uwbDistance.position[2])) {
|
||||
PX4_WARN("Position is corrupt!");
|
||||
return;
|
||||
}
|
||||
|
||||
_new_sensorReport = true;
|
||||
|
||||
// The coordinate system is NED (north-east-down)
|
||||
// the uwb_distance msg contains the Position in NED, Vehicle relative to LP
|
||||
// The coordinates "rel_pos_*" are the position of the landing point relative to the vehicle.
|
||||
// To change POV we negate every Axis:
|
||||
_target_position_report.timestamp = _uwbDistance.timestamp;
|
||||
_target_position_report.rel_pos_x = -_uwbDistance.position[0];
|
||||
_target_position_report.rel_pos_y = -_uwbDistance.position[1];
|
||||
_target_position_report.rel_pos_z = -_uwbDistance.position[2];
|
||||
}
|
||||
}
|
||||
|
||||
void LandingTargetEstimator::_update_params()
|
||||
|
||||
@@ -54,6 +54,9 @@
|
||||
#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>
|
||||
@@ -139,21 +142,32 @@ private:
|
||||
enum Rotation sensor_yaw;
|
||||
} _params;
|
||||
|
||||
struct {
|
||||
hrt_abstime timestamp;
|
||||
float rel_pos_x;
|
||||
float rel_pos_y;
|
||||
float rel_pos_z;
|
||||
} _target_position_report;
|
||||
|
||||
uORB::Subscription _vehicleLocalPositionSub{ORB_ID(vehicle_local_position)};
|
||||
uORB::Subscription _attitudeSub{ORB_ID(vehicle_attitude)};
|
||||
uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
|
||||
uORB::Subscription _irlockReportSub{ORB_ID(irlock_report)};
|
||||
uORB::Subscription _uwbDistanceSub{ORB_ID(uwb_distance)};
|
||||
|
||||
vehicle_local_position_s _vehicleLocalPosition{};
|
||||
vehicle_attitude_s _vehicleAttitude{};
|
||||
vehicle_acceleration_s _vehicle_acceleration{};
|
||||
irlock_report_s _irlockReport{};
|
||||
uwb_grid_s _uwbGrid{};
|
||||
uwb_distance_s _uwbDistance{};
|
||||
|
||||
// keep track of which topics we have received
|
||||
bool _vehicleLocalPosition_valid{false};
|
||||
bool _vehicleAttitude_valid{false};
|
||||
bool _vehicle_acceleration_valid{false};
|
||||
bool _new_irlockReport{false};
|
||||
bool _new_sensorReport{false};
|
||||
bool _estimator_initialized{false};
|
||||
// keep track of whether last measurement was rejected
|
||||
bool _faulty{false};
|
||||
@@ -165,11 +179,10 @@ private:
|
||||
KalmanFilter _kalman_filter_y;
|
||||
hrt_abstime _last_predict{0}; // timestamp of last filter prediction
|
||||
hrt_abstime _last_update{0}; // timestamp of last filter update (used to check timeout)
|
||||
float _dist_z{1.0f};
|
||||
|
||||
void _check_params(const bool force);
|
||||
|
||||
void _update_state();
|
||||
};
|
||||
|
||||
|
||||
} // namespace landing_target_estimator
|
||||
|
||||
@@ -98,7 +98,7 @@ int landing_target_estimator_main(int argc, char *argv[])
|
||||
daemon_task = px4_task_spawn_cmd("landing_target_estimator",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_DEFAULT,
|
||||
2000,
|
||||
2100,
|
||||
landing_target_estimator_thread_main,
|
||||
(argv) ? (char *const *)&argv[2] : nullptr);
|
||||
return 0;
|
||||
|
||||
@@ -256,21 +256,13 @@ PrecLand::run_state_horizontal_approach()
|
||||
if (hrt_absolute_time() - _point_reached_time > 2000000) {
|
||||
// if close enough for descent above target go to descend above target
|
||||
if (switch_to_state_descend_above_target()) {
|
||||
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
if (hrt_absolute_time() - _state_start_time > STATE_TIMEOUT) {
|
||||
PX4_ERR("Precision landing took too long during horizontal approach phase.");
|
||||
|
||||
if (switch_to_state_fallback()) {
|
||||
return;
|
||||
}
|
||||
|
||||
PX4_ERR("Can't switch to fallback landing");
|
||||
}
|
||||
|
||||
float x = _target_pose.x_abs;
|
||||
float y = _target_pose.y_abs;
|
||||
@@ -388,6 +380,7 @@ bool
|
||||
PrecLand::switch_to_state_horizontal_approach()
|
||||
{
|
||||
if (check_state_conditions(PrecLandState::HorizontalApproach)) {
|
||||
PX4_INFO("Precland: switching to horizontal approach!");
|
||||
_approach_alt = _navigator->get_global_position()->alt;
|
||||
|
||||
_point_reached_time = 0;
|
||||
@@ -404,6 +397,7 @@ bool
|
||||
PrecLand::switch_to_state_descend_above_target()
|
||||
{
|
||||
if (check_state_conditions(PrecLandState::DescendAboveTarget)) {
|
||||
PX4_INFO("Precland: switching to descend!");
|
||||
_state = PrecLandState::DescendAboveTarget;
|
||||
_state_start_time = hrt_absolute_time();
|
||||
return true;
|
||||
@@ -416,6 +410,7 @@ bool
|
||||
PrecLand::switch_to_state_final_approach()
|
||||
{
|
||||
if (check_state_conditions(PrecLandState::FinalApproach)) {
|
||||
PX4_INFO("Precland: switching ot final approach");
|
||||
_state = PrecLandState::FinalApproach;
|
||||
_state_start_time = hrt_absolute_time();
|
||||
return true;
|
||||
|
||||
Reference in New Issue
Block a user