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

AC_WPNav_OA: separate handlers for results from Dijkstra's and BendyRuler 

also bendy ruler uses pos controller
This commit is contained in:
Randy Mackay 2021-02-24 14:03:07 +09:00
parent c991ff4318
commit b95c6b5e5d
1 changed files with 89 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

107
libraries/AC_WPNav/AC_WPNav_OA.cpp
View File

@ -1,3 +1,5 @@
#include <AP_Math/control.h>
#include <AP_InternalError/AP_InternalError.h>
#include "AC_WPNav_OA.h"
AC_WPNav_OA::AC_WPNav_OA(const AP_InertialNav& inav, const AP_AHRS_View& ahrs, AC_PosControl& pos_control, const AC_AttitudeControl& attitude_control) :
@ -83,7 +85,8 @@ bool AC_WPNav_OA::update_wpnav()
const Location origin_loc(_origin_oabak, _terrain_alt_oabak ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN);
const Location destination_loc(_destination_oabak, _terrain_alt_oabak ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN);
Location oa_origin_new, oa_destination_new;
const AP_OAPathPlanner::OA_RetState oa_retstate = oa_ptr->mission_avoidance(current_loc, origin_loc, destination_loc, oa_origin_new, oa_destination_new);
AP_OAPathPlanner::OAPathPlannerUsed path_planner_used = AP_OAPathPlanner::OAPathPlannerUsed::None;
const AP_OAPathPlanner::OA_RetState oa_retstate = oa_ptr->mission_avoidance(current_loc, origin_loc, destination_loc, oa_origin_new, oa_destination_new, path_planner_used);
switch (oa_retstate) {
case AP_OAPathPlanner::OA_NOT_REQUIRED:
if (_oa_state != oa_retstate) {
@ -107,26 +110,94 @@ bool AC_WPNav_OA::update_wpnav()
}
break;
case AP_OAPathPlanner::OA_SUCCESS:
// if oa destination has become active or destination has changed update wpnav
if ((_oa_state != AP_OAPathPlanner::OA_SUCCESS) || !oa_destination_new.same_latlon_as(_oa_destination)) {
_oa_destination = oa_destination_new;
// convert Location to offset from EKF origin
Vector3f dest_NEU;
if (_oa_destination.get_vector_from_origin_NEU(dest_NEU)) {
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_oabak)) {
_oa_state = oa_retstate;
// handling of returned destination depends upon path planner used
switch (path_planner_used) {
case AP_OAPathPlanner::OAPathPlannerUsed::None:
// this should never happen. this means the path planner has returned success but has failed to set the path planner used
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
return false;
case AP_OAPathPlanner::OAPathPlannerUsed::Dijkstras:
// Dijkstra's. Action is only needed if path planner has just became active or the target destination's lat or lon has changed
if ((_oa_state != AP_OAPathPlanner::OA_SUCCESS) || !oa_destination_new.same_latlon_as(_oa_destination)) {
Location origin_oabak_loc(_origin_oabak, _terrain_alt_oabak ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN);
Location destination_oabak_loc(_destination_oabak, _terrain_alt_oabak ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN);
oa_destination_new.linearly_interpolate_alt(origin_oabak_loc, destination_oabak_loc);
if (!set_wp_destination_loc(oa_destination_new)) {
// trigger terrain failsafe
return false;
}
// if new target set successfully, update oa state and destination
_oa_state = oa_retstate;
_oa_destination = oa_destination_new;
}
break;
case AP_OAPathPlanner::OAPathPlannerUsed::BendyRulerHorizontal: {
_oa_state = oa_retstate;
_oa_destination = oa_destination_new;
// altitude target interpolated from current_loc's distance along the original path
Location target_alt_loc = current_loc;
target_alt_loc.linearly_interpolate_alt(origin_loc, destination_loc);
// correct target_alt_loc's alt-above-ekf-origin if using terrain altitudes
// positive terr_offset means terrain below vehicle is above ekf origin's altitude
float terr_offset = 0;
if (_terrain_alt_oabak && !get_terrain_offset(terr_offset)) {
// trigger terrain failsafe
return false;
}
// calculate final destination as an offset from EKF origin in NEU
Vector2f dest_NE;
if (!_oa_destination.get_vector_xy_from_origin_NE(dest_NE)) {
// this should never happen because we can only get here if we have an EKF origin
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
return false;
}
Vector3p dest_NEU{dest_NE.x, dest_NE.y, (float)target_alt_loc.alt};
// pass the desired position directly to the position controller
_pos_control.input_pos_xyz(dest_NEU, terr_offset, 1000.0);
// update horizontal position controller (vertical is updated in vehicle code)
_pos_control.update_xy_controller();
// return success without calling parent AC_WPNav
return true;
}
break;
}
case AP_OAPathPlanner::OAPathPlannerUsed::BendyRulerVertical:
{
_oa_state = oa_retstate;
_oa_destination = oa_destination_new;
// calculate final destination as an offset from EKF origin in NEU
Vector3f dest_NEU;
if (!_oa_destination.get_vector_from_origin_NEU(dest_NEU)) {
// this should never happen because we can only get here if we have an EKF origin
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
return false;
}
// pass the desired position directly to the position controller as an offset from EKF origin in NEU
Vector3p dest_NEU_p{dest_NEU.x, dest_NEU.y, dest_NEU.z};
_pos_control.input_pos_xyz(dest_NEU_p, 0, 1000.0);
// update horizontal position controller (vertical is updated in vehicle code)
_pos_control.update_xy_controller();
// Note: do not update yaw or yaw rate as we do for BendyRuler horizontal
// return success without calling parent AC_WPNav
return true;
}
} // case AP_OAPathPlanner::OA_SUCCESS
} // switch (oa_retstate)
}
// run the non-OA update