aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AC_WPNav: Navigation based on type of BendyRuler

Browse Source
This commit is contained in:
Rishabh 2020-07-01 11:41:55 +05:30 committed by Randy Mackay
parent ad3a7e4d49
commit db57e0831b
2 changed files with 7 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AC_WPNav/AC_WPNav_OA.cpp
View File

@ -112,10 +112,12 @@ bool AC_WPNav_OA::update_wpnav()
// convert Location to offset from EKF origin
Vector3f dest_NEU;
if (_oa_destination.get_vector_from_origin_NEU(dest_NEU)) {
// calculate target altitude by calculating OA adjusted destination's distance along the original track
// and then linear interpolate using the original track's origin and destination altitude
const float dist_along_path = constrain_float(oa_destination_new.line_path_proportion(origin_loc, destination_loc), 0.0f, 1.0f);
dest_NEU.z = linear_interpolate(_origin_oabak.z, _destination_oabak.z, dist_along_path, 0.0f, 1.0f);
if (oa_ptr -> get_bendy_type() == AP_OABendyRuler::OABendyType::OA_BENDY_HORIZONTAL || oa_ptr -> get_bendy_type() == AP_OABendyRuler::OABendyType::OA_BENDY_DISABLED) {
// calculate target altitude by calculating OA adjusted destination's distance along the original track
// and then linear interpolate using the original track's origin and destination altitude
const float dist_along_path = constrain_float(oa_destination_new.line_path_proportion(origin_loc, destination_loc), 0.0f, 1.0f);
dest_NEU.z = linear_interpolate(_origin_oabak.z, _destination_oabak.z, dist_along_path, 0.0f, 1.0f);
}
if (set_wp_destination(dest_NEU, _terrain_alt)) {
_oa_state = oa_retstate;
}

1
libraries/AC_WPNav/AC_WPNav_OA.h
View File

@ -2,6 +2,7 @@
#include <AC_WPNav/AC_WPNav.h>
#include <AC_Avoidance/AP_OAPathPlanner.h>
#include <AC_Avoidance/AP_OABendyRuler.h>
class AC_WPNav_OA : public AC_WPNav
{