mirror of https://github.com/ArduPilot/ardupilot
Copter: Also use fence in Guided_PosVel mode
This commit is contained in:
parent
9f6342a1d1
commit
c7a2fcdc37
|
@ -881,7 +881,7 @@ private:
|
||||||
bool guided_set_destination(const Vector3f& destination, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false);
|
bool guided_set_destination(const Vector3f& destination, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false);
|
||||||
bool guided_set_destination(const Location_Class& dest_loc, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false);
|
bool guided_set_destination(const Location_Class& dest_loc, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false);
|
||||||
void guided_set_velocity(const Vector3f& velocity, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false);
|
void guided_set_velocity(const Vector3f& velocity, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false);
|
||||||
void guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false);
|
bool guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false);
|
||||||
void guided_set_angle(const Quaternion &q, float climb_rate_cms, bool use_yaw_rate, float yaw_rate_rads);
|
void guided_set_angle(const Quaternion &q, float climb_rate_cms, bool use_yaw_rate, float yaw_rate_rads);
|
||||||
void guided_run();
|
void guided_run();
|
||||||
void guided_takeoff_run();
|
void guided_takeoff_run();
|
||||||
|
|
|
@ -260,13 +260,23 @@ void Copter::guided_set_velocity(const Vector3f& velocity, bool use_yaw, float y
|
||||||
}
|
}
|
||||||
|
|
||||||
// set guided mode posvel target
|
// set guided mode posvel target
|
||||||
void Copter::guided_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 Copter::guided_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
|
// check we are in velocity control mode
|
||||||
if (guided_mode != Guided_PosVel) {
|
if (guided_mode != Guided_PosVel) {
|
||||||
guided_posvel_control_start();
|
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
|
||||||
|
|
||||||
// set yaw state
|
// set yaw state
|
||||||
guided_set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
|
guided_set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
|
||||||
|
|
||||||
|
@ -278,6 +288,7 @@ void Copter::guided_set_destination_posvel(const Vector3f& destination, const Ve
|
||||||
|
|
||||||
// log target
|
// log target
|
||||||
Log_Write_GuidedTarget(guided_mode, destination, velocity);
|
Log_Write_GuidedTarget(guided_mode, destination, velocity);
|
||||||
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
// set guided mode angle target
|
// set guided mode angle target
|
||||||
|
|
Loading…
Reference in New Issue