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 b318df490dd3..ebf634e60c30 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 < 0) { + _target_pose_sub = orb_subscribe(ORB_ID(landing_target_pose)); } _state = PrecLandState::Start; @@ -108,15 +108,14 @@ void PrecLand::on_active() { // get new target measurement - bool updated = false; - orb_check(_targetPoseSub, &updated); + orb_check(_target_pose_sub, &_target_pose_updated); - if (updated) { - orb_copy(ORB_ID(landing_target_pose), _targetPoseSub, &_target_pose); + if (_target_pose_updated) { + orb_copy(ORB_ID(landing_target_pose), _target_pose_sub, &_target_pose); _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; } @@ -212,7 +211,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; @@ -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 0d9177e5d3d6..6814267f9362 100644 --- a/src/modules/navigator/precland.h +++ b/src/modules/navigator/precland.h @@ -100,8 +100,9 @@ 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 */ + 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 */