2017-12-26 19:09:11 -04:00
|
|
|
#include "Copter.h"
|
|
|
|
|
2018-02-06 02:16:09 -04:00
|
|
|
#if MODE_FOLLOW_ENABLED == ENABLED
|
|
|
|
|
2017-12-26 19:09:11 -04:00
|
|
|
/*
|
2018-02-05 22:44:05 -04:00
|
|
|
* mode_follow.cpp - follow another mavlink-enabled vehicle by system id
|
2017-12-26 19:09:11 -04:00
|
|
|
*
|
|
|
|
* TODO: stick control to move around on sphere
|
|
|
|
* TODO: stick control to change sphere diameter
|
|
|
|
* TODO: "channel 7 option" to lock onto "pointed at" target
|
|
|
|
* TODO: do better in terms of loitering around the moving point; may need a PID? Maybe use loiter controller somehow?
|
|
|
|
* TODO: extrapolate target vehicle position using its velocity and acceleration
|
2018-01-25 08:36:03 -04:00
|
|
|
* TODO: ensure AC_AVOID_ENABLED is true because we rely on it velocity limiting functions
|
2017-12-26 19:09:11 -04:00
|
|
|
*/
|
|
|
|
|
2018-02-06 02:16:09 -04:00
|
|
|
// initialise follow mode
|
2019-05-09 23:18:49 -03:00
|
|
|
bool ModeFollow::init(const bool ignore_checks)
|
2017-12-26 19:09:11 -04:00
|
|
|
{
|
2018-02-06 02:16:09 -04:00
|
|
|
if (!g2.follow.enabled()) {
|
2018-12-08 00:18:41 -04:00
|
|
|
gcs().send_text(MAV_SEVERITY_WARNING, "Set FOLL_ENABLE = 1");
|
2017-12-26 19:09:11 -04:00
|
|
|
return false;
|
|
|
|
}
|
2018-12-08 00:18:41 -04:00
|
|
|
// re-use guided mode
|
2019-05-09 23:18:49 -03:00
|
|
|
return ModeGuided::init(ignore_checks);
|
2017-12-26 19:09:11 -04:00
|
|
|
}
|
|
|
|
|
2019-10-03 22:24:51 -03:00
|
|
|
// perform cleanup required when leaving follow mode
|
|
|
|
void ModeFollow::exit()
|
|
|
|
{
|
|
|
|
g2.follow.clear_offsets_if_required();
|
|
|
|
}
|
|
|
|
|
2019-05-09 23:18:49 -03:00
|
|
|
void ModeFollow::run()
|
2017-12-26 19:09:11 -04:00
|
|
|
{
|
2019-02-28 05:03:23 -04:00
|
|
|
// if not armed set throttle to zero and exit immediately
|
2019-04-08 05:15:57 -03:00
|
|
|
if (is_disarmed_or_landed()) {
|
2020-10-13 20:19:42 -03:00
|
|
|
make_safe_ground_handling();
|
2017-12-26 19:09:11 -04:00
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
2019-02-28 05:03:23 -04:00
|
|
|
// set motors to full range
|
2019-04-09 09:16:58 -03:00
|
|
|
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
|
2019-02-28 05:03:23 -04:00
|
|
|
|
2017-12-26 19:09:11 -04:00
|
|
|
// re-use guided mode's velocity controller
|
|
|
|
// Note: this is safe from interference from GCSs and companion computer's whose guided mode
|
|
|
|
// position and velocity requests will be ignored while the vehicle is not in guided mode
|
|
|
|
|
2018-01-25 08:36:03 -04:00
|
|
|
// variables to be sent to velocity controller
|
2018-02-15 15:16:43 -04:00
|
|
|
Vector3f desired_velocity_neu_cms;
|
2018-01-25 08:36:03 -04:00
|
|
|
bool use_yaw = false;
|
|
|
|
float yaw_cd = 0.0f;
|
|
|
|
|
|
|
|
Vector3f dist_vec; // vector to lead vehicle
|
|
|
|
Vector3f dist_vec_offs; // vector to lead vehicle + offset
|
|
|
|
Vector3f vel_of_target; // velocity of lead vehicle
|
|
|
|
if (g2.follow.get_target_dist_and_vel_ned(dist_vec, dist_vec_offs, vel_of_target)) {
|
|
|
|
// convert dist_vec_offs to cm in NEU
|
|
|
|
const Vector3f dist_vec_offs_neu(dist_vec_offs.x * 100.0f, dist_vec_offs.y * 100.0f, -dist_vec_offs.z * 100.0f);
|
|
|
|
|
|
|
|
// calculate desired velocity vector in cm/s in NEU
|
2018-02-06 02:16:09 -04:00
|
|
|
const float kp = g2.follow.get_pos_p().kP();
|
2018-02-15 15:16:43 -04:00
|
|
|
desired_velocity_neu_cms.x = (vel_of_target.x * 100.0f) + (dist_vec_offs_neu.x * kp);
|
|
|
|
desired_velocity_neu_cms.y = (vel_of_target.y * 100.0f) + (dist_vec_offs_neu.y * kp);
|
|
|
|
desired_velocity_neu_cms.z = (-vel_of_target.z * 100.0f) + (dist_vec_offs_neu.z * kp);
|
2018-01-25 08:36:03 -04:00
|
|
|
|
|
|
|
// scale desired velocity to stay within horizontal speed limit
|
2018-02-15 15:16:43 -04:00
|
|
|
float desired_speed_xy = safe_sqrt(sq(desired_velocity_neu_cms.x) + sq(desired_velocity_neu_cms.y));
|
2021-05-11 01:42:02 -03:00
|
|
|
if (!is_zero(desired_speed_xy) && (desired_speed_xy > pos_control->get_max_speed_xy_cms())) {
|
|
|
|
const float scalar_xy = pos_control->get_max_speed_xy_cms() / desired_speed_xy;
|
2018-02-15 15:16:43 -04:00
|
|
|
desired_velocity_neu_cms.x *= scalar_xy;
|
|
|
|
desired_velocity_neu_cms.y *= scalar_xy;
|
2021-05-11 01:42:02 -03:00
|
|
|
desired_speed_xy = pos_control->get_max_speed_xy_cms();
|
2018-01-25 08:36:03 -04:00
|
|
|
}
|
2017-12-26 19:09:11 -04:00
|
|
|
|
2018-01-25 08:36:03 -04:00
|
|
|
// limit desired velocity to be between maximum climb and descent rates
|
2021-05-11 01:42:02 -03:00
|
|
|
desired_velocity_neu_cms.z = constrain_float(desired_velocity_neu_cms.z, -fabsf(pos_control->get_max_speed_down_cms()), pos_control->get_max_speed_up_cms());
|
2017-12-26 19:09:11 -04:00
|
|
|
|
2018-02-06 02:16:09 -04:00
|
|
|
// unit vector towards target position (i.e. vector to lead vehicle + offset)
|
2018-01-25 08:36:03 -04:00
|
|
|
Vector3f dir_to_target_neu = dist_vec_offs_neu;
|
|
|
|
const float dir_to_target_neu_len = dir_to_target_neu.length();
|
|
|
|
if (!is_zero(dir_to_target_neu_len)) {
|
|
|
|
dir_to_target_neu /= dir_to_target_neu_len;
|
2017-12-26 19:09:11 -04:00
|
|
|
}
|
|
|
|
|
2018-01-25 08:36:03 -04:00
|
|
|
// create horizontal desired velocity vector (required for slow down calculations)
|
2018-02-15 15:16:43 -04:00
|
|
|
Vector2f desired_velocity_xy_cms(desired_velocity_neu_cms.x, desired_velocity_neu_cms.y);
|
2017-12-26 19:09:11 -04:00
|
|
|
|
2018-01-25 08:36:03 -04:00
|
|
|
// create horizontal unit vector towards target (required for slow down calculations)
|
2018-02-15 15:16:43 -04:00
|
|
|
Vector2f dir_to_target_xy(desired_velocity_xy_cms.x, desired_velocity_xy_cms.y);
|
2018-01-25 08:36:03 -04:00
|
|
|
if (!dir_to_target_xy.is_zero()) {
|
|
|
|
dir_to_target_xy.normalize();
|
|
|
|
}
|
2017-12-26 19:09:11 -04:00
|
|
|
|
2018-01-25 08:36:03 -04:00
|
|
|
// slow down horizontally as we approach target (use 1/2 of maximum deceleration for gentle slow down)
|
|
|
|
const float dist_to_target_xy = Vector2f(dist_vec_offs_neu.x, dist_vec_offs_neu.y).length();
|
2021-05-11 01:42:02 -03:00
|
|
|
copter.avoid.limit_velocity_2D(pos_control->get_pos_xy_p().kP().get(), pos_control->get_max_accel_xy_cmss() * 0.5f, desired_velocity_xy_cms, dir_to_target_xy, dist_to_target_xy, copter.G_Dt);
|
2018-01-25 08:36:03 -04:00
|
|
|
// copy horizontal velocity limits back to 3d vector
|
2018-02-15 15:16:43 -04:00
|
|
|
desired_velocity_neu_cms.x = desired_velocity_xy_cms.x;
|
|
|
|
desired_velocity_neu_cms.y = desired_velocity_xy_cms.y;
|
2018-01-25 08:36:03 -04:00
|
|
|
|
2018-02-15 15:16:43 -04:00
|
|
|
// limit vertical desired_velocity_neu_cms to slow as we approach target (we use 1/2 of maximum deceleration for gentle slow down)
|
2021-05-11 01:42:02 -03:00
|
|
|
const float des_vel_z_max = copter.avoid.get_max_speed(pos_control->get_pos_z_p().kP().get(), pos_control->get_max_accel_z_cmss() * 0.5f, fabsf(dist_vec_offs_neu.z), copter.G_Dt);
|
2018-02-15 15:16:43 -04:00
|
|
|
desired_velocity_neu_cms.z = constrain_float(desired_velocity_neu_cms.z, -des_vel_z_max, des_vel_z_max);
|
2018-01-25 08:36:03 -04:00
|
|
|
|
2020-12-06 08:48:21 -04:00
|
|
|
// limit the velocity for obstacle/fence avoidance
|
2021-05-11 01:42:02 -03:00
|
|
|
copter.avoid.adjust_velocity(desired_velocity_neu_cms, pos_control->get_pos_xy_p().kP().get(), pos_control->get_max_accel_xy_cmss(), pos_control->get_pos_z_p().kP().get(), pos_control->get_max_accel_z_cmss(), G_Dt);
|
2018-01-25 08:36:03 -04:00
|
|
|
|
|
|
|
// calculate vehicle heading
|
|
|
|
switch (g2.follow.get_yaw_behave()) {
|
|
|
|
case AP_Follow::YAW_BEHAVE_FACE_LEAD_VEHICLE: {
|
2018-02-06 02:16:09 -04:00
|
|
|
const Vector3f dist_vec_xy(dist_vec.x, dist_vec.y, 0.0f);
|
2018-01-25 08:36:03 -04:00
|
|
|
if (dist_vec_xy.length() > 1.0f) {
|
|
|
|
yaw_cd = get_bearing_cd(Vector3f(), dist_vec_xy);
|
|
|
|
use_yaw = true;
|
|
|
|
}
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
|
|
|
|
case AP_Follow::YAW_BEHAVE_SAME_AS_LEAD_VEHICLE: {
|
2018-02-06 02:16:09 -04:00
|
|
|
float target_hdg = 0.0f;
|
2018-02-15 15:16:43 -04:00
|
|
|
if (g2.follow.get_target_heading_deg(target_hdg)) {
|
2018-01-25 08:36:03 -04:00
|
|
|
yaw_cd = target_hdg * 100.0f;
|
|
|
|
use_yaw = true;
|
|
|
|
}
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
|
|
|
|
case AP_Follow::YAW_BEHAVE_DIR_OF_FLIGHT: {
|
2018-02-15 15:16:43 -04:00
|
|
|
const Vector3f vel_vec(desired_velocity_neu_cms.x, desired_velocity_neu_cms.y, 0.0f);
|
2018-01-25 08:36:03 -04:00
|
|
|
if (vel_vec.length() > 100.0f) {
|
|
|
|
yaw_cd = get_bearing_cd(Vector3f(), vel_vec);
|
|
|
|
use_yaw = true;
|
|
|
|
}
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
|
|
|
|
case AP_Follow::YAW_BEHAVE_NONE:
|
|
|
|
default:
|
|
|
|
// do nothing
|
|
|
|
break;
|
2017-12-26 19:09:11 -04:00
|
|
|
|
2018-01-25 08:36:03 -04:00
|
|
|
}
|
2017-12-26 19:09:11 -04:00
|
|
|
}
|
|
|
|
|
2018-02-06 02:16:09 -04:00
|
|
|
// log output at 10hz
|
|
|
|
uint32_t now = AP_HAL::millis();
|
|
|
|
bool log_request = false;
|
|
|
|
if ((now - last_log_ms >= 100) || (last_log_ms == 0)) {
|
|
|
|
log_request = true;
|
|
|
|
last_log_ms = now;
|
|
|
|
}
|
2018-01-25 08:36:03 -04:00
|
|
|
// re-use guided mode's velocity controller (takes NEU)
|
2019-05-09 23:18:49 -03:00
|
|
|
ModeGuided::set_velocity(desired_velocity_neu_cms, use_yaw, yaw_cd, false, 0.0f, false, log_request);
|
2017-12-26 19:09:11 -04:00
|
|
|
|
2019-05-09 23:18:49 -03:00
|
|
|
ModeGuided::run();
|
2017-12-26 19:09:11 -04:00
|
|
|
}
|
2018-01-25 08:36:03 -04:00
|
|
|
|
2019-05-09 23:18:49 -03:00
|
|
|
uint32_t ModeFollow::wp_distance() const
|
2018-12-11 05:37:30 -04:00
|
|
|
{
|
|
|
|
return g2.follow.get_distance_to_target() * 100;
|
|
|
|
}
|
|
|
|
|
2019-05-09 23:18:49 -03:00
|
|
|
int32_t ModeFollow::wp_bearing() const
|
2018-12-11 05:37:30 -04:00
|
|
|
{
|
|
|
|
return g2.follow.get_bearing_to_target() * 100;
|
|
|
|
}
|
|
|
|
|
2018-12-30 01:55:05 -04:00
|
|
|
/*
|
|
|
|
get target position for mavlink reporting
|
|
|
|
*/
|
2021-07-06 18:02:26 -03:00
|
|
|
bool ModeFollow::get_wp(Location &loc) const
|
2018-12-30 01:55:05 -04:00
|
|
|
{
|
|
|
|
float dist = g2.follow.get_distance_to_target();
|
|
|
|
float bearing = g2.follow.get_bearing_to_target();
|
|
|
|
loc = copter.current_loc;
|
2019-04-05 03:11:27 -03:00
|
|
|
loc.offset_bearing(bearing, dist);
|
2018-12-30 01:55:05 -04:00
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
2018-02-06 02:16:09 -04:00
|
|
|
#endif // MODE_FOLLOW_ENABLED == ENABLED
|