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

Copter: guided mode uses modified check_destination_within_fence

This commit is contained in:
Randy Mackay 2016-07-02 17:15:05 +09:00
parent e820506c5a
commit 2647bed484
2 changed files with 7 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/Copter.cpp
View File

@ -92,7 +92,7 @@ Copter::Copter(void) :
camera_mount(ahrs, current_loc),
#endif
#if AC_FENCE == ENABLED
fence(inertial_nav),
fence(ahrs, inertial_nav),
#endif
#if AC_RALLY == ENABLED
rally(ahrs),

14
ArduCopter/control_guided.cpp
View File

@ -177,7 +177,8 @@ bool Copter::guided_set_destination(const Vector3f& destination)
#if AC_FENCE == ENABLED
// reject destination if outside the fence
if (!fence.check_destination_within_fence(pv_alt_above_home(destination.z)*0.01f, pv_distance_to_home_cm(destination)*0.01f)) {
Location_Class dest_loc(destination);
if (!fence.check_destination_within_fence(dest_loc)) {
Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE);
// failure is propagated to GCS with NAK
return false;
@ -205,13 +206,10 @@ bool Copter::guided_set_destination(const Location_Class& dest_loc)
#if AC_FENCE == ENABLED
// reject destination outside the fence.
// Note: there is a danger that a target specified as a terrain altitude might not be checked if the conversion to alt-above-home fails
int32_t alt_target_cm;
if (dest_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_HOME, alt_target_cm)) {
if (!fence.check_destination_within_fence(alt_target_cm*0.01f, get_distance_cm(ahrs.get_home(), dest_loc)*0.01f)) {
Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE);
// failure is propagated to GCS with NAK
return false;
}
if (!fence.check_destination_within_fence(dest_loc)) {
Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE);
// failure is propagated to GCS with NAK
return false;
}
#endif