Its working now . Seems like it needs rangefinder in order to work the precision land. I have enabled mavlink rangefinder. But I need to decent it at a constant velocity. Is there is any way to disable the rangefinder and do the precision landing?
Hello, after checking the source code of AC_PrecLand, if the PLND_TYPE = 1 (Companion)
void AC_PrecLand_Companion::handle_msg(const mavlink_landing_target_t &packet, uint32_t timestamp_ms)
{
_distance_to_target = packet.distance;
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "handle msg %.3f", _distance_to_target);
if (packet.position_valid == 1) {
if (packet.frame == MAV_FRAME_BODY_FRD) {
if (_distance_to_target > 0) {
_los_meas_body = Vector3f(packet.x, packet.y, packet.z);
_los_meas_body /= _distance_to_target;
} else {
// distance to target must be positive
return;
}
} else {
//we do not support this frame
if (!_wrong_frame_msg_sent) {
_wrong_frame_msg_sent = true;
gcs().send_text(MAV_SEVERITY_INFO,"Plnd: Frame not supported ");
}
return;
}
} else {
// compute unit vector towards target
_los_meas_body = Vector3f(-tanf(packet.angle_y), tanf(packet.angle_x), 1.0f);
_los_meas_body /= _los_meas_body.length();
}
_los_meas_time_ms = timestamp_ms;
_have_los_meas = true;
}
the code show that your frame type should be MAV_FRAME_BODY_FRD distance >0 x y z >0 Position_valid == 1, and
void AC_PrecLand::check_ekf_init_timeout()
{
if (!target_acquired() && _estimator_initialized) {
// we have just got the target in sight
if (AP_HAL::millis()-_last_update_ms > EKF_INIT_SENSOR_MIN_UPDATE_MS) {
// we have lost the target, not enough readings to initialize the EKF
_estimator_initialized = false;
gcs().send_text(MAV_SEVERITY_CRITICAL, "PrecLand: Init Failed");
} else if (AP_HAL::millis()-_estimator_init_ms > EKF_INIT_TIME_MS) {
// the target has been visible for a while, EKF should now have initialized to a good value
_target_acquired = true;
gcs().send_text(MAV_SEVERITY_INFO, "PrecLand: Init Complete");
}
}
}
static const uint32_t EKF_INIT_SENSOR_MIN_UPDATE_MS = 500;
my mavlink_landing_target_t like that
mavlink_landing_target_t req = new mavlink_landing_target_t
{
x = 1,
y = -1,
z = 5,
position_valid = 1,
type = 0,
distance = 30,
frame = 12
};
fast send the packet in LAND mode the your vehicle will move