// set guided mode posvel target
bool Sub::guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity)
{
// check we are in velocity control mode
if (guided_mode != Guided_PosVel) {
guided_posvel_control_start();
}
#if AC_FENCE == ENABLED
// reject destination if outside the fence
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;
}
#endif
posvel_update_time_ms = AP_HAL::millis();
posvel_pos_target_cm = destination;
posvel_vel_target_cms = velocity;
pos_control.set_pos_target(posvel_pos_target_cm);
// log target
Log_Write_GuidedTarget(guided_mode, destination, velocity);
return true;
}
// set guided mode posvel target
bool Copter::ModeGuided::set_destination_posvel(const Vector3f& destination, const Vector3f& velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw)
{
// check we are in velocity control mode
if (guided_mode != Guided_PosVel) {
posvel_control_start();
}
#if AC_FENCE == ENABLED
// reject destination if outside the fence
Location_Class dest_loc(destination);
if (!copter.fence.check_destination_within_fence(dest_loc)) {
copter.Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE);
// failure is propagated to GCS with NAK
return false;
}
#endif
// set yaw state
set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
posvel_update_time_ms = millis();
guided_pos_target_cm = destination;
guided_vel_target_cms = velocity;
copter.pos_control->set_pos_target(guided_pos_target_cm);
// log target
copter.Log_Write_GuidedTarget(guided_mode, destination, velocity);
return true;
}
// guided_set_destination - sets guided mode's target destination
// Returns true if the fence is enabled and guided waypoint is within the fence
// else return false if the waypoint is outside the fence
bool Copter::ModeGuided::set_destination(const Vector3f& destination, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw)
{
// ensure we are in position control mode
if (guided_mode != Guided_WP) {
pos_control_start();
}
#if AC_FENCE == ENABLED
// reject destination if outside the fence
Location_Class dest_loc(destination);
if (!copter.fence.check_destination_within_fence(dest_loc)) {
copter.Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE);
// failure is propagated to GCS with NAK
return false;
}
#endif
// set yaw state
set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
// no need to check return status because terrain data is not used
wp_nav->set_wp_destination(destination, false);
// log target
copter.Log_Write_GuidedTarget(guided_mode, destination, Vector3f());
return true;
}
// guided_set_destination - sets guided mode's target destination
// Returns true if the fence is enabled and guided waypoint is within the fence
// else return false if the waypoint is outside the fence
bool Sub::guided_set_destination(const Vector3f& destination)
{
// ensure we are in position control mode
if (guided_mode != Guided_WP) {
guided_pos_control_start();
}
#if AC_FENCE == ENABLED
// reject destination if outside the fence
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;
}
#endif
// no need to check return status because terrain data is not used
wp_nav.set_wp_destination(destination, false);
// log target
Log_Write_GuidedTarget(guided_mode, destination, Vector3f());
return true;
}
