forked from ArduPilot/ardupilot
/
autoyaw.cpp
272 lines (228 loc) · 8.34 KB
/
autoyaw.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
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
#include "Copter.h"
Mode::AutoYaw Mode::auto_yaw;
// roi_yaw - returns heading towards location held in roi
float Mode::AutoYaw::roi_yaw() const
{
return get_bearing_cd(copter.inertial_nav.get_position_xy_cm(), roi.xy());
}
float Mode::AutoYaw::look_ahead_yaw()
{
const Vector3f& vel = copter.inertial_nav.get_velocity_neu_cms();
const float speed_sq = vel.xy().length_squared();
// Commanded Yaw to automatically look ahead.
if (copter.position_ok() && (speed_sq > (YAW_LOOK_AHEAD_MIN_SPEED * YAW_LOOK_AHEAD_MIN_SPEED))) {
_look_ahead_yaw = degrees(atan2f(vel.y,vel.x))*100.0f;
}
return _look_ahead_yaw;
}
void Mode::AutoYaw::set_mode_to_default(bool rtl)
{
set_mode(default_mode(rtl));
}
// default_mode - returns auto_yaw.mode() based on WP_YAW_BEHAVIOR parameter
// set rtl parameter to true if this is during an RTL
autopilot_yaw_mode Mode::AutoYaw::default_mode(bool rtl) const
{
switch (copter.g.wp_yaw_behavior) {
case WP_YAW_BEHAVIOR_NONE:
return AUTO_YAW_HOLD;
case WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP_EXCEPT_RTL:
if (rtl) {
return AUTO_YAW_HOLD;
} else {
return AUTO_YAW_LOOK_AT_NEXT_WP;
}
case WP_YAW_BEHAVIOR_LOOK_AHEAD:
return AUTO_YAW_LOOK_AHEAD;
case WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP:
default:
return AUTO_YAW_LOOK_AT_NEXT_WP;
}
}
// set_mode - sets the yaw mode for auto
void Mode::AutoYaw::set_mode(autopilot_yaw_mode yaw_mode)
{
// return immediately if no change
if (_mode == yaw_mode) {
return;
}
_mode = yaw_mode;
// perform initialisation
switch (_mode) {
case AUTO_YAW_HOLD:
break;
case AUTO_YAW_LOOK_AT_NEXT_WP:
// wpnav will initialise heading when wpnav's set_destination method is called
break;
case AUTO_YAW_ROI:
// look ahead until we know otherwise
break;
case AUTO_YAW_FIXED:
// keep heading pointing in the direction held in fixed_yaw
// caller should set the fixed_yaw
break;
case AUTO_YAW_LOOK_AHEAD:
// Commanded Yaw to automatically look ahead.
_look_ahead_yaw = copter.ahrs.yaw_sensor;
break;
case AUTO_YAW_RESETTOARMEDYAW:
// initial_armed_bearing will be set during arming so no init required
break;
case AUTO_YAW_ANGLE_RATE:
break;
case AUTO_YAW_RATE:
// initialise target yaw rate to zero
_yaw_rate_cds = 0.0f;
break;
case AUTO_YAW_CIRCLE:
// no initialisation required
break;
}
}
// set_fixed_yaw - sets the yaw look at heading for auto mode
void Mode::AutoYaw::set_fixed_yaw(float angle_deg, float turn_rate_ds, int8_t direction, bool relative_angle)
{
_last_update_ms = millis();
_yaw_angle_cd = copter.attitude_control->get_att_target_euler_cd().z;
_yaw_rate_cds = 0.0;
// calculate final angle as relative to vehicle heading or absolute
if (relative_angle) {
_fixed_yaw_offset_cd = angle_deg * 100.0 * (direction >= 0 ? 1.0 : -1.0);
} else {
// absolute angle
_fixed_yaw_offset_cd = wrap_180_cd(angle_deg * 100.0 - _yaw_angle_cd);
if ( direction < 0 && is_positive(_fixed_yaw_offset_cd) ) {
_fixed_yaw_offset_cd -= 36000.0;
} else if ( direction > 0 && is_negative(_fixed_yaw_offset_cd) ) {
_fixed_yaw_offset_cd += 36000.0;
}
}
// get turn speed
if (!is_positive(turn_rate_ds)) {
// default to default slew rate
_fixed_yaw_slewrate_cds = copter.attitude_control->get_slew_yaw_max_degs() * 100.0;
} else {
_fixed_yaw_slewrate_cds = MIN(copter.attitude_control->get_slew_yaw_max_degs(), turn_rate_ds) * 100.0;
}
// set yaw mode
set_mode(AUTO_YAW_FIXED);
}
// set_fixed_yaw - sets the yaw look at heading for auto mode
void Mode::AutoYaw::set_yaw_angle_rate(float yaw_angle_d, float yaw_rate_ds)
{
_last_update_ms = millis();
_yaw_angle_cd = yaw_angle_d * 100.0;
_yaw_rate_cds = yaw_rate_ds * 100.0;
// set yaw mode
set_mode(AUTO_YAW_ANGLE_RATE);
}
// set_roi - sets the yaw to look at roi for auto mode
void Mode::AutoYaw::set_roi(const Location &roi_location)
{
// if location is zero lat, lon and altitude turn off ROI
if (roi_location.alt == 0 && roi_location.lat == 0 && roi_location.lng == 0) {
// set auto yaw mode back to default assuming the active command is a waypoint command. A more sophisticated method is required to ensure we return to the proper yaw control for the active command
auto_yaw.set_mode_to_default(false);
#if HAL_MOUNT_ENABLED
// switch off the camera tracking if enabled
if (copter.camera_mount.get_mode() == MAV_MOUNT_MODE_GPS_POINT) {
copter.camera_mount.set_mode_to_default();
}
#endif // HAL_MOUNT_ENABLED
} else {
#if HAL_MOUNT_ENABLED
// check if mount type requires us to rotate the quad
if (!copter.camera_mount.has_pan_control()) {
if (roi_location.get_vector_from_origin_NEU(roi)) {
auto_yaw.set_mode(AUTO_YAW_ROI);
}
}
// send the command to the camera mount
copter.camera_mount.set_roi_target(roi_location);
// TO-DO: expand handling of the do_nav_roi to support all modes of the MAVLink. Currently we only handle mode 4 (see below)
// 0: do nothing
// 1: point at next waypoint
// 2: point at a waypoint taken from WP# parameter (2nd parameter?)
// 3: point at a location given by alt, lon, lat parameters
// 4: point at a target given a target id (can't be implemented)
#else
// if we have no camera mount aim the quad at the location
if (roi_location.get_vector_from_origin_NEU(roi)) {
auto_yaw.set_mode(AUTO_YAW_ROI);
}
#endif // HAL_MOUNT_ENABLED
}
}
// set auto yaw rate in centi-degrees per second
void Mode::AutoYaw::set_rate(float turn_rate_cds)
{
set_mode(AUTO_YAW_RATE);
_yaw_rate_cds = turn_rate_cds;
}
// yaw - returns target heading depending upon auto_yaw.mode()
float Mode::AutoYaw::yaw()
{
switch (_mode) {
case AUTO_YAW_ROI:
// point towards a location held in roi
return roi_yaw();
case AUTO_YAW_FIXED: {
// keep heading pointing in the direction held in fixed_yaw
// with no pilot input allowed
const uint32_t now_ms = millis();
float dt = (now_ms - _last_update_ms) * 0.001;
_last_update_ms = now_ms;
float yaw_angle_step = constrain_float(_fixed_yaw_offset_cd, - dt * _fixed_yaw_slewrate_cds, dt * _fixed_yaw_slewrate_cds);
_fixed_yaw_offset_cd -= yaw_angle_step;
_yaw_angle_cd += yaw_angle_step;
return _yaw_angle_cd;
}
case AUTO_YAW_LOOK_AHEAD:
// Commanded Yaw to automatically look ahead.
return look_ahead_yaw();
case AUTO_YAW_RESETTOARMEDYAW:
// changes yaw to be same as when quad was armed
return copter.initial_armed_bearing;
case AUTO_YAW_CIRCLE:
#if MODE_CIRCLE_ENABLED
if (copter.circle_nav->is_active()) {
return copter.circle_nav->get_yaw();
}
#endif
// return the current attitude target
return wrap_360_cd(copter.attitude_control->get_att_target_euler_cd().z);
case AUTO_YAW_ANGLE_RATE:{
const uint32_t now_ms = millis();
float dt = (now_ms - _last_update_ms) * 0.001;
_last_update_ms = now_ms;
_yaw_angle_cd += _yaw_rate_cds * dt;
return _yaw_angle_cd;
}
case AUTO_YAW_LOOK_AT_NEXT_WP:
default:
// point towards next waypoint.
// we don't use wp_bearing because we don't want the copter to turn too much during flight
return copter.pos_control->get_yaw_cd();
}
}
// returns yaw rate normally set by SET_POSITION_TARGET mavlink
// messages (positive is clockwise, negative is counter clockwise)
float Mode::AutoYaw::rate_cds() const
{
switch (_mode) {
case AUTO_YAW_HOLD:
case AUTO_YAW_ROI:
case AUTO_YAW_FIXED:
case AUTO_YAW_LOOK_AHEAD:
case AUTO_YAW_RESETTOARMEDYAW:
case AUTO_YAW_CIRCLE:
return 0.0f;
case AUTO_YAW_ANGLE_RATE:
case AUTO_YAW_RATE:
return _yaw_rate_cds;
case AUTO_YAW_LOOK_AT_NEXT_WP:
return copter.pos_control->get_yaw_rate_cds();
}
// return zero turn rate (this should never happen)
return 0.0f;
}