fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 16:28:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

Navigator: yaw error from param and pos_ctl_status.acceptence_rad checks before using

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2021-01-29 19:36:59 +01:00
parent 4d6749edc2
commit 85d8e74609
2 changed files with 10 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/mission_block.cpp
+1 -1
View File
@@ -391,7 +391,7 @@ MissionBlock::is_mission_item_reached()
}
if (fabsf(yaw_err) < 0.1f) { //accept heading for exit if below 0.1 rad error (5.7deg)
if (fabsf(yaw_err) < _navigator->get_yaw_threshold()) {
exit_heading_reached = true;
}
src/modules/navigator/navigator_main.cpp
+9 -7
View File
@@ -992,15 +992,17 @@ Navigator::get_cruising_throttle()
float
Navigator::get_acceptance_radius()
{
if (_vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
// return the value specified in the parameter NAV_ACC_RAD
return get_default_acceptance_radius();
float acceptance_radius = get_default_acceptance_radius(); // the value specified in the parameter NAV_ACC_RAD
const position_controller_status_s &pos_ctrl_status = _position_controller_status_sub.get();
} else {
// return the max of NAV_ACC_RAD and the controller acceptance radius (e.g. L1 distance)
const position_controller_status_s &pos_ctrl_status = _position_controller_status_sub.get();
return math::max(pos_ctrl_status.acceptance_radius, get_default_acceptance_radius());
// for fixed-wing and rover, return the max of NAV_ACC_RAD and the controller acceptance radius (e.g. L1 distance)
if (_vstatus.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
&& PX4_ISFINITE(pos_ctrl_status.acceptance_radius) && pos_ctrl_status.timestamp != 0) {
acceptance_radius = math::max(acceptance_radius, pos_ctrl_status.acceptance_radius);
}
return acceptance_radius;
}
float