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

AP_PrecLand: consume distance_to_target as alternative to rangefinder_alt 

Resolves issue #5636
This commit is contained in:
fnoop 2017-01-27 11:02:39 +00:00 committed by Randy Mackay
parent b77941c4f2
commit 438ffd870a
4 changed files with 19 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AC_PrecLand/AC_PrecLand.cpp
View File

@ -163,8 +163,13 @@ void AC_PrecLand::update(float rangefinder_alt_cm, bool rangefinder_alt_valid)
bool target_vec_valid = target_vec_unit_ned.z > 0.0f;
if (target_vec_valid && rangefinder_alt_valid && rangefinder_alt_cm > 0.0f) {
float alt = MAX(rangefinder_alt_cm*0.01f, 0.0f);
if (target_vec_valid && ( (rangefinder_alt_valid && rangefinder_alt_cm > 0.0f) || ((enum PrecLandType)(_type.get()) == PRECLAND_TYPE_COMPANION && _backend->distance_to_target()) ) ) {
float alt;
if ((enum PrecLandType)(_type.get()) == PRECLAND_TYPE_COMPANION && _backend->distance_to_target()) {
alt = _backend->distance_to_target();
} else {
alt = MAX(rangefinder_alt_cm*0.01f, 0.0f);
}
float dist = alt/target_vec_unit_ned.z;
Vector3f targetPosRelMeasNED = Vector3f(target_vec_unit_ned.x*dist, target_vec_unit_ned.y*dist, alt);

3
libraries/AC_PrecLand/AC_PrecLand_Backend.h
View File

@ -33,6 +33,9 @@ public:
// return true if there is a valid los measurement available
virtual bool have_los_meas() = 0;
// return distance to target
virtual float distance_to_target() = 0;
// parses a mavlink message from the companion computer
virtual void handle_msg(mavlink_message_t* msg) {};

6
libraries/AC_PrecLand/AC_PrecLand_Companion.cpp
View File

@ -48,6 +48,12 @@ bool AC_PrecLand_Companion::have_los_meas()
return _have_los_meas;
}
// return distance to target
float AC_PrecLand_Companion::distance_to_target()
{
return _distance_to_target;
}
void AC_PrecLand_Companion::handle_msg(mavlink_message_t* msg)
{
// parse mavlink message

3
libraries/AC_PrecLand/AC_PrecLand_Companion.h
View File

@ -31,6 +31,9 @@ public:
// return true if there is a valid los measurement available
bool have_los_meas();
// return distance to target
float distance_to_target();
// parses a mavlink message from the companion computer
void handle_msg(mavlink_message_t* msg);