Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

PrecLand: support LANDING_TARGET ext field

This commit is contained in:
chobitsfan 2022-07-30 17:54:25 +08:00 committed by Randy Mackay
parent 6c5bb1c5b8
commit 43105249cb
2 changed files with 24 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
libraries/AC_PrecLand/AC_PrecLand_Companion.cpp
View File

@ -1,4 +1,5 @@
#include <AP_HAL/AP_HAL.h>
#include <GCS_MAVLink/GCS.h>
#include "AC_PrecLand_Companion.h"
// perform any required initialisation of backend
@ -46,9 +47,28 @@ void AC_PrecLand_Companion::handle_msg(const mavlink_landing_target_t &packet, u
{
_distance_to_target = packet.distance;
// 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();
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;

1
libraries/AC_PrecLand/AC_PrecLand_Companion.h
View File

@ -44,4 +44,5 @@ private:
Vector3f _los_meas_body; // unit vector in body frame pointing towards target
bool _have_los_meas; // true if there is a valid measurement from the camera
uint32_t _los_meas_time_ms; // system time in milliseconds when los was measured
bool _wrong_frame_msg_sent;
};