forked from ArduPilot/ardupilot
-
Notifications
You must be signed in to change notification settings - Fork 26
/
Copy pathmode_follow.cpp
178 lines (149 loc) · 7.34 KB
/
mode_follow.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
#include "Copter.h"
#if MODE_FOLLOW_ENABLED == ENABLED
/*
* mode_follow.cpp - follow another mavlink-enabled vehicle by system id
*
* 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
* TODO: ensure AC_AVOID_ENABLED is true because we rely on it velocity limiting functions
*/
// initialise follow mode
bool ModeFollow::init(const bool ignore_checks)
{
if (!g2.follow.enabled()) {
gcs().send_text(MAV_SEVERITY_WARNING, "Set FOLL_ENABLE = 1");
return false;
}
// re-use guided mode
return ModeGuided::init(ignore_checks);
}
// perform cleanup required when leaving follow mode
void ModeFollow::exit()
{
g2.follow.clear_offsets_if_required();
}
void ModeFollow::run()
{
// if not armed set throttle to zero and exit immediately
if (is_disarmed_or_landed()) {
make_safe_ground_handling();
return;
}
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// 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
// variables to be sent to velocity controller
Vector3f desired_velocity_neu_cms;
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
const float kp = g2.follow.get_pos_p().kP();
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);
// scale desired velocity to stay within horizontal speed limit
float desired_speed_xy = safe_sqrt(sq(desired_velocity_neu_cms.x) + sq(desired_velocity_neu_cms.y));
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;
desired_velocity_neu_cms.x *= scalar_xy;
desired_velocity_neu_cms.y *= scalar_xy;
desired_speed_xy = pos_control->get_max_speed_xy_cms();
}
// limit desired velocity to be between maximum climb and descent rates
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());
// unit vector towards target position (i.e. vector to lead vehicle + offset)
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;
}
// create horizontal desired velocity vector (required for slow down calculations)
Vector2f desired_velocity_xy_cms(desired_velocity_neu_cms.x, desired_velocity_neu_cms.y);
// create horizontal unit vector towards target (required for slow down calculations)
Vector2f dir_to_target_xy(desired_velocity_xy_cms.x, desired_velocity_xy_cms.y);
if (!dir_to_target_xy.is_zero()) {
dir_to_target_xy.normalize();
}
// 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();
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);
// copy horizontal velocity limits back to 3d vector
desired_velocity_neu_cms.x = desired_velocity_xy_cms.x;
desired_velocity_neu_cms.y = desired_velocity_xy_cms.y;
// limit vertical desired_velocity_neu_cms to slow as we approach target (we use 1/2 of maximum deceleration for gentle slow down)
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);
desired_velocity_neu_cms.z = constrain_float(desired_velocity_neu_cms.z, -des_vel_z_max, des_vel_z_max);
// limit the velocity for obstacle/fence avoidance
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);
// calculate vehicle heading
switch (g2.follow.get_yaw_behave()) {
case AP_Follow::YAW_BEHAVE_FACE_LEAD_VEHICLE: {
if (dist_vec.xy().length_squared() > 1.0) {
yaw_cd = get_bearing_cd(Vector2f{}, dist_vec.xy());
use_yaw = true;
}
break;
}
case AP_Follow::YAW_BEHAVE_SAME_AS_LEAD_VEHICLE: {
float target_hdg = 0.0f;
if (g2.follow.get_target_heading_deg(target_hdg)) {
yaw_cd = target_hdg * 100.0f;
use_yaw = true;
}
break;
}
case AP_Follow::YAW_BEHAVE_DIR_OF_FLIGHT: {
if (desired_velocity_neu_cms.xy().length_squared() > (100.0 * 100.0)) {
yaw_cd = get_bearing_cd(Vector2f{}, desired_velocity_neu_cms.xy());
use_yaw = true;
}
break;
}
case AP_Follow::YAW_BEHAVE_NONE:
default:
// do nothing
break;
}
}
// 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;
}
// re-use guided mode's velocity controller (takes NEU)
ModeGuided::set_velocity(desired_velocity_neu_cms, use_yaw, yaw_cd, false, 0.0f, false, log_request);
ModeGuided::run();
}
uint32_t ModeFollow::wp_distance() const
{
return g2.follow.get_distance_to_target() * 100;
}
int32_t ModeFollow::wp_bearing() const
{
return g2.follow.get_bearing_to_target() * 100;
}
/*
get target position for mavlink reporting
*/
bool ModeFollow::get_wp(Location &loc) const
{
float dist = g2.follow.get_distance_to_target();
float bearing = g2.follow.get_bearing_to_target();
loc = copter.current_loc;
loc.offset_bearing(bearing, dist);
return true;
}
#endif // MODE_FOLLOW_ENABLED == ENABLED