Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-05 23:43:58 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: fixes to guided mode target check is within fence

Browse Source 
guided altitude targets are converted to alt-above-home
remove unnecessary fence_status local variable from guided_set_destination methods
log failures to set guided target under a new failure code: ERROR_CODE_DEST_OUTSIDE_FENCE (5)
rename pv_get_home_destination_distance_mc to pv_distance_to_home_cm
This commit is contained in:
Randy Mackay 2016-05-19 17:35:44 +09:00
parent 4829dbb55d
commit e0bf08abe0
4 changed files with 22 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduCopter/Copter.h
View File

@ -783,8 +783,8 @@ private:
void guided_vel_control_start();
void guided_posvel_control_start();
void guided_angle_control_start();
bool guided_set_destination(const Location_Class& dest_loc);
bool guided_set_destination(const Vector3f& destination);
bool guided_set_destination(const Location_Class& dest_loc);
void guided_set_velocity(const Vector3f& velocity);
void guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity);
void guided_set_angle(const Quaternion &q, float climb_rate_cms);
@ -935,7 +935,7 @@ private:
float pv_alt_above_home(float alt_above_origin_cm);
float pv_get_bearing_cd(const Vector3f &origin, const Vector3f &destination);
float pv_get_horizontal_distance_cm(const Vector3f &origin, const Vector3f &destination);
float pv_get_home_destination_distance_cm(const Vector3f &destination);
float pv_distance_to_home_cm(const Vector3f &destination);
void default_dead_zones();
void init_rc_in();
void init_rc_out();

31
ArduCopter/control_guided.cpp
View File

@ -170,25 +170,21 @@ void Copter::guided_angle_control_start()
// else return false if the waypoint is outside the fence
bool Copter::guided_set_destination(const Vector3f& destination)
{
bool fence_status = true;
// ensure we are in position control mode
if (guided_mode != Guided_WP) {
guided_pos_control_start();
}
#if AC_FENCE == ENABLED
// ensure waypoint is within the fence
fence_status = fence.check_fence_location(destination.z, pv_get_home_destination_distance_cm(destination));
#endif
if (!fence_status) {
// failure to set destination can only be because waypoint is outside the fence
Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_SET_DESTINATION);
// 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)) {
Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE);
// failure is propagated to GCS with NAK
return false;
}
#endif
// no need to check return status because terrain data is not used and fence is disabled
// no need to check return status because terrain data is not used
wp_nav.set_wp_destination(destination, false);
// log target
@ -201,19 +197,26 @@ bool Copter::guided_set_destination(const Vector3f& destination)
// or if the fence is enabled and guided waypoint is outside the fence
bool Copter::guided_set_destination(const Location_Class& dest_loc)
{
bool fence_status = true;
// ensure we are in position control mode
if (guided_mode != Guided_WP) {
guided_pos_control_start();
}
#if AC_FENCE == ENABLED
// ensure waypoint is within the fence
fence_status = fence.check_fence_location((float)dest_loc.alt, (float)get_distance_cm(ahrs.get_home(),dest_loc));
// 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;
}
}
#endif
if (!fence_status || !wp_nav.set_wp_destination(dest_loc)) {
// failure to set destination can only be because of missing terrain data or waypoint is outside the fence
if (!wp_nav.set_wp_destination(dest_loc)) {
// failure to set destination can only be because of missing terrain data
Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_SET_DESTINATION);
// failure is propagated to GCS with NAK
return false;

1
ArduCopter/defines.h
View File

@ -408,6 +408,7 @@ enum ThrowModeState {
#define ERROR_CODE_FAILED_TO_SET_DESTINATION 2
#define ERROR_CODE_RESTARTED_RTL 3
#define ERROR_CODE_FAILED_CIRCLE_INIT 4
#define ERROR_CODE_DEST_OUTSIDE_FENCE 5
// parachute failed to deploy because of low altitude or landed
#define ERROR_CODE_PARACHUTE_TOO_LOW 2

4
ArduCopter/position_vector.cpp
View File

@ -67,8 +67,8 @@ float Copter::pv_get_horizontal_distance_cm(const Vector3f &origin, const Vector
return norm(destination.x-origin.x,destination.y-origin.y);
}
// pv_get_home_destination_distance_cm - return distance between destination and home in cm
float Copter::pv_get_home_destination_distance_cm(const Vector3f &destination)
// returns distance between a destination and home in cm
float Copter::pv_distance_to_home_cm(const Vector3f &destination)
{
Vector3f home = pv_location_to_vector(ahrs.get_home());
return pv_get_horizontal_distance_cm(home, destination);