Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

precland: support receiving LANDING_TARGET message #9050

Merged
merged 4 commits into from
Mar 28, 2018
Merged
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
28 changes: 28 additions & 0 deletions src/modules/mavlink/mavlink_receiver.cpp
Original file line number Diff line number Diff line change
@@ -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;
3 changes: 3 additions & 0 deletions src/modules/mavlink/mavlink_receiver.h
Original file line number Diff line number Diff line change
@@ -74,6 +74,7 @@
#include <uORB/topics/time_offset.h>
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/follow_target.h>
#include <uORB/topics/landing_target_pose.h>
#include <uORB/topics/transponder_report.h>
#include <uORB/topics/gps_inject_data.h>
#include <uORB/topics/collision_report.h>
@@ -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;
27 changes: 14 additions & 13 deletions src/modules/navigator/precland.cpp
Original file line number Diff line number Diff line change
@@ -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()) {
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This is a bug. But the fix should be in the other direction, see my other comment.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@ndepal could you submit the correct fix as a separate PR? We are doing preclands and have been observing weird horizontal approach failures. Probably related to this.

Copy link
Contributor Author

@okalachev okalachev Mar 12, 2018

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

OK, I can do it.

@ndepal, you mean we just have to use _target_pose.x_rel instead of _target_pose.x_abs - vehicle_local_position->x?

Also, I wanted to notice that this is incorrect calculation if we're inside the desired radius. The correct would be sqrt(pow(_target_pose.x_rel, 2) + pow(_target_pose.y_rel, 2)).

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@mhkabir Sure thing. But if @okalachev does it, that's great.

you mean we just have to use _target_pose.x_rel instead of _target_pose.x_abs - vehicle_local_position->x?

Correct.

The correct would be sqrt(pow(_target_pose.x_rel, 2) + pow(_target_pose.y_rel, 2)).

That would be the L2 norm. I used the L1 norm because it's cheaper to compute and just as good in this situation.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@okalachev Is this addressed now?

// 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();
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Should use relative measurements, see my other comment.


case PrecLandState::Search:
return true;
5 changes: 3 additions & 2 deletions src/modules/navigator/precland.h
Original file line number Diff line number Diff line change
@@ -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 */