From 2bdd909b2943198c1964bb7ec7948f9df6a2282e Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Tue, 27 Mar 2018 18:30:16 +0300 Subject: [PATCH 1/4] precland: fix typos and code style --- src/modules/navigator/precland.cpp | 10 +++++----- src/modules/navigator/precland.h | 4 ++-- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/modules/navigator/precland.cpp b/src/modules/navigator/precland.cpp index b318df490dd3..b45b630ab2c9 100644 --- a/src/modules/navigator/precland.cpp +++ b/src/modules/navigator/precland.cpp @@ -69,8 +69,8 @@ void PrecLand::on_activation() { // We need to subscribe here and not in the constructor because constructor is called before the navigator task is spawned - if (!_targetPoseSub) { - _targetPoseSub = orb_subscribe(ORB_ID(landing_target_pose)); + if (!_target_pose_sub) { + _target_pose_sub = orb_subscribe(ORB_ID(landing_target_pose)); } _state = PrecLandState::Start; @@ -109,10 +109,10 @@ PrecLand::on_active() { // get new target measurement bool updated = false; - orb_check(_targetPoseSub, &updated); + orb_check(_target_pose_sub, &updated); if (updated) { - orb_copy(ORB_ID(landing_target_pose), _targetPoseSub, &_target_pose); + orb_copy(ORB_ID(landing_target_pose), _target_pose_sub, &_target_pose); _target_pose_valid = true; } @@ -212,7 +212,7 @@ PrecLand::run_state_horizontal_approach() // check if target visible, if not go to start if (!check_state_conditions(PrecLandState::HorizontalApproach)) { - PX4_WARN("Lost landing target while landig (horizontal approach)."); + PX4_WARN("Lost landing target while landing (horizontal approach)."); // Stay at current position for searching for the landing target pos_sp_triplet->current.lat = _navigator->get_global_position()->lat; diff --git a/src/modules/navigator/precland.h b/src/modules/navigator/precland.h index 0d9177e5d3d6..697fdd6e9503 100644 --- a/src/modules/navigator/precland.h +++ b/src/modules/navigator/precland.h @@ -100,8 +100,8 @@ class PrecLand : public MissionBlock, public ModuleParams landing_target_pose_s _target_pose{}; /**< precision landing target position */ - int _targetPoseSub{-1}; - bool _target_pose_valid{false}; /**< wether we have received a landing target position message */ + int _target_pose_sub{-1}; + bool _target_pose_valid{false}; /**< whether we have received a landing target position message */ struct map_projection_reference_s _map_ref {}; /**< reference for local/global projections */ From 91f5ba74f3a9e82f3994c9a6c019dd52537b8d3a Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Tue, 27 Mar 2018 19:06:57 +0300 Subject: [PATCH 2/4] precland: fix landing_target_pose subscription check --- src/modules/navigator/precland.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/navigator/precland.cpp b/src/modules/navigator/precland.cpp index b45b630ab2c9..321b9bf76a78 100644 --- a/src/modules/navigator/precland.cpp +++ b/src/modules/navigator/precland.cpp @@ -69,7 +69,7 @@ void PrecLand::on_activation() { // We need to subscribe here and not in the constructor because constructor is called before the navigator task is spawned - if (!_target_pose_sub) { + if (_target_pose_sub < 0) { _target_pose_sub = orb_subscribe(ORB_ID(landing_target_pose)); } From dc88c34b76ac759b0625b8e08d933434384f2488 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Tue, 27 Mar 2018 19:18:14 +0300 Subject: [PATCH 3/4] precland: fix incorrect converting microseconds to seconds --- src/modules/navigator/precland.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/navigator/precland.cpp b/src/modules/navigator/precland.cpp index 321b9bf76a78..ad3e0674545f 100644 --- a/src/modules/navigator/precland.cpp +++ b/src/modules/navigator/precland.cpp @@ -116,7 +116,7 @@ PrecLand::on_active() _target_pose_valid = true; } - if ((hrt_elapsed_time(&_target_pose.timestamp) / 1e-6f) > _param_timeout.get()) { + if ((hrt_elapsed_time(&_target_pose.timestamp) / 1e6f) > _param_timeout.get()) { _target_pose_valid = false; } From 02d52a81e7c83fa33e25226278bdb91ffd533085 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Tue, 27 Mar 2018 19:19:53 +0300 Subject: [PATCH 4/4] precland: support receiving LANDING_TARGET message --- src/modules/mavlink/mavlink_receiver.cpp | 28 ++++++++++++++++++++++++ src/modules/mavlink/mavlink_receiver.h | 3 +++ src/modules/navigator/precland.cpp | 17 +++++++------- src/modules/navigator/precland.h | 1 + 4 files changed, 41 insertions(+), 8 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 74d6c5ca5b95..777608179213 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -132,6 +132,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _land_detector_pub(nullptr), _time_offset_pub(nullptr), _follow_target_pub(nullptr), + _landing_target_pose_pub(nullptr), _transponder_report_pub(nullptr), _collision_report_pub(nullptr), _debug_key_value_pub(nullptr), @@ -294,6 +295,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_follow_target(msg); break; + case MAVLINK_MSG_ID_LANDING_TARGET: + handle_message_landing_target(msg); + break; + case MAVLINK_MSG_ID_ADSB_VEHICLE: handle_message_adsb_vehicle(msg); break; @@ -2059,6 +2064,29 @@ void MavlinkReceiver::handle_message_follow_target(mavlink_message_t *msg) } } +void MavlinkReceiver::handle_message_landing_target(mavlink_message_t *msg) +{ + mavlink_landing_target_t landing_target; + + mavlink_msg_landing_target_decode(msg, &landing_target); + + if (landing_target.position_valid && landing_target.frame == MAV_FRAME_LOCAL_NED) { + landing_target_pose_s landing_target_pose = {}; + landing_target_pose.timestamp = sync_stamp(landing_target.time_usec); + landing_target_pose.abs_pos_valid = true; + landing_target_pose.x_abs = landing_target.x; + landing_target_pose.y_abs = landing_target.y; + landing_target_pose.z_abs = landing_target.z; + + if (_landing_target_pose_pub == nullptr) { + _landing_target_pose_pub = orb_advertise(ORB_ID(landing_target_pose), &landing_target_pose); + + } else { + orb_publish(ORB_ID(landing_target_pose), _landing_target_pose_pub, &landing_target_pose); + } + } +} + void MavlinkReceiver::handle_message_adsb_vehicle(mavlink_message_t *msg) { mavlink_adsb_vehicle_t adsb; diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 1f3c2d127a6f..be24ef1e9405 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -74,6 +74,7 @@ #include #include #include +#include #include #include #include @@ -149,6 +150,7 @@ class MavlinkReceiver void handle_message_hil_state_quaternion(mavlink_message_t *msg); void handle_message_distance_sensor(mavlink_message_t *msg); void handle_message_follow_target(mavlink_message_t *msg); + void handle_message_landing_target(mavlink_message_t *msg); void handle_message_adsb_vehicle(mavlink_message_t *msg); void handle_message_collision(mavlink_message_t *msg); void handle_message_gps_rtcm_data(mavlink_message_t *msg); @@ -242,6 +244,7 @@ class MavlinkReceiver orb_advert_t _land_detector_pub; orb_advert_t _time_offset_pub; orb_advert_t _follow_target_pub; + orb_advert_t _landing_target_pose_pub; orb_advert_t _transponder_report_pub; orb_advert_t _collision_report_pub; orb_advert_t _debug_key_value_pub; diff --git a/src/modules/navigator/precland.cpp b/src/modules/navigator/precland.cpp index ad3e0674545f..ebf634e60c30 100644 --- a/src/modules/navigator/precland.cpp +++ b/src/modules/navigator/precland.cpp @@ -108,10 +108,9 @@ void PrecLand::on_active() { // get new target measurement - bool updated = false; - orb_check(_target_pose_sub, &updated); + orb_check(_target_pose_sub, &_target_pose_updated); - if (updated) { + if (_target_pose_updated) { orb_copy(ORB_ID(landing_target_pose), _target_pose_sub, &_target_pose); _target_pose_valid = true; } @@ -466,7 +465,7 @@ bool PrecLand::check_state_conditions(PrecLandState state) // if we're already in this state, only want to make it invalid if we reached the target but can't see it anymore if (_state == PrecLandState::HorizontalApproach) { if (fabsf(_target_pose.x_abs - vehicle_local_position->x) < _param_hacc_rad.get() - && fabsf(_target_pose.y_rel - vehicle_local_position->y) < _param_hacc_rad.get()) { + && fabsf(_target_pose.y_abs - vehicle_local_position->y) < _param_hacc_rad.get()) { // we've reached the position where we last saw the target. If we don't see it now, we need to do something return _target_pose_valid && _target_pose.abs_pos_valid; @@ -478,7 +477,7 @@ bool PrecLand::check_state_conditions(PrecLandState state) } // If we're trying to switch to this state, the target needs to be visible - return _target_pose_valid && _target_pose.abs_pos_valid; + return _target_pose_updated && _target_pose.abs_pos_valid; case PrecLandState::DescendAboveTarget: @@ -494,12 +493,14 @@ bool PrecLand::check_state_conditions(PrecLandState state) } else { // if not already in this state, need to be above target to enter it - return _target_pose_valid && _target_pose.abs_pos_valid - && fabsf(_target_pose.x_rel) < _param_hacc_rad.get() && fabsf(_target_pose.y_rel) < _param_hacc_rad.get(); + return _target_pose_updated && _target_pose.abs_pos_valid + && fabsf(_target_pose.x_abs - vehicle_local_position->x) < _param_hacc_rad.get() + && fabsf(_target_pose.y_abs - vehicle_local_position->y) < _param_hacc_rad.get(); } case PrecLandState::FinalApproach: - return _target_pose_valid && _target_pose.rel_pos_valid && _target_pose.z_rel < _param_final_approach_alt.get(); + return _target_pose_valid && _target_pose.abs_pos_valid + && (_target_pose.z_abs - vehicle_local_position->z) < _param_final_approach_alt.get(); case PrecLandState::Search: return true; diff --git a/src/modules/navigator/precland.h b/src/modules/navigator/precland.h index 697fdd6e9503..6814267f9362 100644 --- a/src/modules/navigator/precland.h +++ b/src/modules/navigator/precland.h @@ -102,6 +102,7 @@ class PrecLand : public MissionBlock, public ModuleParams int _target_pose_sub{-1}; bool _target_pose_valid{false}; /**< whether we have received a landing target position message */ + bool _target_pose_updated{false}; /**< wether the landing target position message is updated */ struct map_projection_reference_s _map_ref {}; /**< reference for local/global projections */