mirror of https://github.com/ArduPilot/ardupilot
Copter: guided mode checks dest before changing submode
This commit is contained in:
parent
ca95e7865a
commit
ab54f8805a
|
@ -190,11 +190,6 @@ void ModeGuided::angle_control_start()
|
||||||
// else return false if the waypoint is outside the fence
|
// else return false if the waypoint is outside the fence
|
||||||
bool ModeGuided::set_destination(const Vector3f& destination, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw, bool terrain_alt)
|
bool ModeGuided::set_destination(const Vector3f& destination, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw, bool terrain_alt)
|
||||||
{
|
{
|
||||||
// ensure we are in position control mode
|
|
||||||
if (guided_mode != Guided_WP) {
|
|
||||||
pos_control_start();
|
|
||||||
}
|
|
||||||
|
|
||||||
#if AC_FENCE == ENABLED
|
#if AC_FENCE == ENABLED
|
||||||
// reject destination if outside the fence
|
// reject destination if outside the fence
|
||||||
const Location dest_loc(destination);
|
const Location dest_loc(destination);
|
||||||
|
@ -205,6 +200,11 @@ bool ModeGuided::set_destination(const Vector3f& destination, bool use_yaw, floa
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
// ensure we are in position control mode
|
||||||
|
if (guided_mode != Guided_WP) {
|
||||||
|
pos_control_start();
|
||||||
|
}
|
||||||
|
|
||||||
// set yaw state
|
// set yaw state
|
||||||
set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
|
set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
|
||||||
|
|
||||||
|
@ -229,11 +229,6 @@ bool ModeGuided::get_wp(Location& destination)
|
||||||
// or if the fence is enabled and guided waypoint is outside the fence
|
// or if the fence is enabled and guided waypoint is outside the fence
|
||||||
bool ModeGuided::set_destination(const Location& dest_loc, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw)
|
bool ModeGuided::set_destination(const Location& dest_loc, 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
|
#if AC_FENCE == ENABLED
|
||||||
// reject destination outside the fence.
|
// 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
|
// 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
|
||||||
|
@ -244,6 +239,11 @@ bool ModeGuided::set_destination(const Location& dest_loc, bool use_yaw, float y
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
// ensure we are in position control mode
|
||||||
|
if (guided_mode != Guided_WP) {
|
||||||
|
pos_control_start();
|
||||||
|
}
|
||||||
|
|
||||||
if (!wp_nav->set_wp_destination(dest_loc)) {
|
if (!wp_nav->set_wp_destination(dest_loc)) {
|
||||||
// failure to set destination can only be because of missing terrain data
|
// failure to set destination can only be because of missing terrain data
|
||||||
AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::FAILED_TO_SET_DESTINATION);
|
AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::FAILED_TO_SET_DESTINATION);
|
||||||
|
@ -283,11 +283,6 @@ void ModeGuided::set_velocity(const Vector3f& velocity, bool use_yaw, float yaw_
|
||||||
// set guided mode posvel target
|
// set guided mode posvel target
|
||||||
bool 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)
|
bool 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
|
#if AC_FENCE == ENABLED
|
||||||
// reject destination if outside the fence
|
// reject destination if outside the fence
|
||||||
const Location dest_loc(destination);
|
const Location dest_loc(destination);
|
||||||
|
@ -298,6 +293,11 @@ bool ModeGuided::set_destination_posvel(const Vector3f& destination, const Vecto
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
// check we are in velocity control mode
|
||||||
|
if (guided_mode != Guided_PosVel) {
|
||||||
|
posvel_control_start();
|
||||||
|
}
|
||||||
|
|
||||||
// set yaw state
|
// set yaw state
|
||||||
set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
|
set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue