3 #if MODE_FOLLOW_ENABLED == ENABLED 17 #define Debug(fmt, args ...) do { \ 18 gcs().send_text(MAV_SEVERITY_WARNING, fmt, ## args); \ 21 #define Debug(fmt, args ...) do { \ 22 ::fprintf(stderr, "%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); 24 #define Debug(fmt, args ...) 31 gcs().
send_text(MAV_SEVERITY_WARNING,
"Follow-mode init");
41 if (!
motors->armed() || !
ap.auto_armed || !
motors->get_interlock()) {
60 static uint16_t counter = 0;
64 Debug(
"dist to veh: %f %f %f", (
double)dist_vec.
x, (
double)dist_vec.
y, (
double)dist_vec.
z);
68 const Vector3f dist_vec_offs_neu(dist_vec_offs.
x * 100.0f, dist_vec_offs.
y * 100.0f, -dist_vec_offs.
z * 100.0f);
72 desired_velocity_neu_cms.
x = (vel_of_target.
x * 100.0f) + (dist_vec_offs_neu.
x * kp);
73 desired_velocity_neu_cms.
y = (vel_of_target.
y * 100.0f) + (dist_vec_offs_neu.
y * kp);
74 desired_velocity_neu_cms.
z = (-vel_of_target.
z * 100.0f) + (dist_vec_offs_neu.
z * kp);
77 float desired_speed_xy =
safe_sqrt(
sq(desired_velocity_neu_cms.
x) +
sq(desired_velocity_neu_cms.
y));
80 desired_velocity_neu_cms.
x *= scalar_xy;
81 desired_velocity_neu_cms.
y *= scalar_xy;
89 Vector3f dir_to_target_neu = dist_vec_offs_neu;
90 const float dir_to_target_neu_len = dir_to_target_neu.
length();
91 if (!
is_zero(dir_to_target_neu_len)) {
92 dir_to_target_neu /= dir_to_target_neu_len;
96 Vector2f desired_velocity_xy_cms(desired_velocity_neu_cms.
x, desired_velocity_neu_cms.
y);
99 Vector2f dir_to_target_xy(desired_velocity_xy_cms.
x, desired_velocity_xy_cms.
y);
100 if (!dir_to_target_xy.
is_zero()) {
105 const float dist_to_target_xy =
Vector2f(dist_vec_offs_neu.
x, dist_vec_offs_neu.
y).
length();
112 desired_velocity_neu_cms.
x = desired_velocity_xy_cms.
x;
113 desired_velocity_neu_cms.
y = desired_velocity_xy_cms.
y;
117 desired_velocity_neu_cms.
z =
constrain_float(desired_velocity_neu_cms.
z, -des_vel_z_max, des_vel_z_max);
125 const Vector3f dist_vec_xy(dist_vec.
x, dist_vec.
y, 0.0f);
126 if (dist_vec_xy.
length() > 1.0f) {
134 float target_hdg = 0.0f;
136 yaw_cd = target_hdg * 100.0f;
143 const Vector3f vel_vec(desired_velocity_neu_cms.
x, desired_velocity_neu_cms.
y, 0.0f);
144 if (vel_vec.
length() > 100.0f) {
161 bool log_request =
false;
172 #endif // MODE_FOLLOW_ENABLED == ENABLED
YAW_BEHAVE_SAME_AS_LEAD_VEHICLE
float get_speed_xy() const
void zero_throttle_and_relax_ac()
float safe_sqrt(const T v)
bool get_target_dist_and_vel_ned(Vector3f &dist_ned, Vector3f &dist_with_ofs, Vector3f &vel_ned)
float get_avoidance_adjusted_climbrate(float target_rate)
YawBehave get_yaw_behave() const
int32_t get_bearing_cd(const struct Location &loc1, const struct Location &loc2)
void 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, bool log_request=true)
float get_accel_xy() const
#define Debug(fmt, args ...)
float get_speed_up() const
bool init(bool ignore_checks) override
bool is_zero(const T fVal1)
YAW_BEHAVE_FACE_LEAD_VEHICLE
float get_accel_z() const
bool get_target_heading(float &heading) const
void send_text(MAV_SEVERITY severity, const char *fmt,...)
float get_speed_down() const
AC_PosControl *& pos_control
const AC_P & get_pos_p() const
float constrain_float(const float amt, const float low, const float high)
AP_HAL_MAIN_CALLBACKS & copter
bool init(bool ignore_checks) override