forked from ArduPilot/ardupilot
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathmode.cpp
176 lines (151 loc) · 7.28 KB
/
mode.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
#include "mode.h"
#include "Tracker.h"
void Mode::update_auto(void)
{
struct Tracker::NavStatus &nav_status = tracker.nav_status;
Parameters &g = tracker.g;
float yaw = wrap_180_cd((nav_status.bearing+g.yaw_trim)*100); // target yaw in centidegrees
float pitch = constrain_float(nav_status.pitch+g.pitch_trim, g.pitch_min, g.pitch_max) * 100; // target pitch in centidegrees
bool direction_reversed = get_ef_yaw_direction();
calc_angle_error(pitch, yaw, direction_reversed);
float bf_pitch;
float bf_yaw;
convert_ef_to_bf(pitch, yaw, bf_pitch, bf_yaw);
// only move servos if target is at least distance_min away if we have a target
if ((g.distance_min <= 0) || (nav_status.distance >= g.distance_min) || !tracker.vehicle.location_valid) {
tracker.update_pitch_servo(bf_pitch);
tracker.update_yaw_servo(bf_yaw);
}
}
void Mode::update_scan(void)
{
struct Tracker::NavStatus &nav_status = tracker.nav_status;
Parameters &g = tracker.g;
if (!nav_status.manual_control_yaw) {
float yaw_delta = g.scan_speed_yaw * 0.02f;
nav_status.bearing += yaw_delta * (nav_status.scan_reverse_yaw?-1:1);
if (nav_status.bearing < 0 && nav_status.scan_reverse_yaw) {
nav_status.scan_reverse_yaw = false;
}
if (nav_status.bearing > 360 && !nav_status.scan_reverse_yaw) {
nav_status.scan_reverse_yaw = true;
}
nav_status.bearing = constrain_float(nav_status.bearing, 0, 360);
}
if (!nav_status.manual_control_pitch) {
const float pitch_delta = g.scan_speed_pitch * 0.02f;
if (nav_status.scan_reverse_pitch) {
nav_status.pitch -= pitch_delta;
if (nav_status.pitch < g.pitch_min) {
nav_status.scan_reverse_pitch = false;
}
} else {
nav_status.pitch += pitch_delta;
if (nav_status.pitch > g.pitch_max) {
nav_status.scan_reverse_pitch = true;
}
}
nav_status.pitch = constrain_float(nav_status.pitch, g.pitch_min, g.pitch_max);
}
update_auto();
}
void Mode::calc_angle_error(float pitch, float yaw, bool direction_reversed)
{
// Pitch angle error in centidegrees
// Positive error means the target is above current pitch
// Negative error means the target is below current pitch
const AP_AHRS &ahrs = AP::ahrs();
float ahrs_pitch = ahrs.pitch_sensor;
int32_t ef_pitch_angle_error = pitch - ahrs_pitch;
// Yaw angle error in centidegrees
// Positive error means the target is right of current yaw
// Negative error means the target is left of current yaw
int32_t ahrs_yaw_cd = wrap_180_cd(ahrs.yaw_sensor);
int32_t ef_yaw_angle_error = wrap_180_cd(yaw - ahrs_yaw_cd);
if (direction_reversed) {
if (ef_yaw_angle_error > 0) {
ef_yaw_angle_error = (yaw - ahrs_yaw_cd) - 36000;
} else {
ef_yaw_angle_error = 36000 + (yaw - ahrs_yaw_cd);
}
}
// earth frame to body frame angle error conversion
float bf_pitch_err;
float bf_yaw_err;
convert_ef_to_bf(ef_pitch_angle_error, ef_yaw_angle_error, bf_pitch_err, bf_yaw_err);
struct Tracker::NavStatus &nav_status = tracker.nav_status;
nav_status.angle_error_pitch = bf_pitch_err;
nav_status.angle_error_yaw = bf_yaw_err;
// set actual and desired for logging, note we are using angles not rates
Parameters &g = tracker.g;
g.pidPitch2Srv.set_target_rate(pitch * 0.01);
g.pidPitch2Srv.set_actual_rate(ahrs_pitch * 0.01);
g.pidYaw2Srv.set_target_rate(yaw * 0.01);
g.pidYaw2Srv.set_actual_rate(ahrs_yaw_cd * 0.01);
}
void Mode::convert_ef_to_bf(float pitch, float yaw, float& bf_pitch, float& bf_yaw)
{
// earth frame to body frame pitch and yaw conversion
const AP_AHRS &ahrs = AP::ahrs();
bf_pitch = ahrs.cos_roll() * pitch + ahrs.sin_roll() * ahrs.cos_pitch() * yaw;
bf_yaw = -ahrs.sin_roll() * pitch + ahrs.cos_pitch() * ahrs.cos_roll() * yaw;
}
bool Mode::convert_bf_to_ef(float pitch, float yaw, float& ef_pitch, float& ef_yaw)
{
const AP_AHRS &ahrs = AP::ahrs();
// avoid divide by zero
if (is_zero(ahrs.cos_pitch())) {
return false;
}
// convert earth frame angle or rates to body frame
ef_pitch = ahrs.cos_roll() * pitch - ahrs.sin_roll() * yaw;
ef_yaw = (ahrs.sin_roll() / ahrs.cos_pitch()) * pitch + (ahrs.cos_roll() / ahrs.cos_pitch()) * yaw;
return true;
}
// return value is true if taking the long road to the target, false if normal, shortest direction should be used
bool Mode::get_ef_yaw_direction()
{
// calculating distances from current pitch/yaw to lower and upper limits in centi-degrees
Parameters &g = tracker.g;
float yaw_angle_limit_lower = (-g.yaw_range * 100.0f / 2.0f) - tracker.yaw_servo_out_filt.get();
float yaw_angle_limit_upper = (g.yaw_range * 100.0f / 2.0f) - tracker.yaw_servo_out_filt.get();
float pitch_angle_limit_lower = (g.pitch_min * 100.0f) - tracker.pitch_servo_out_filt.get();
float pitch_angle_limit_upper = (g.pitch_max * 100.0f) - tracker.pitch_servo_out_filt.get();
// distances to earthframe angle limits in centi-degrees
float ef_yaw_limit_lower = yaw_angle_limit_lower;
float ef_yaw_limit_upper = yaw_angle_limit_upper;
float ef_pitch_limit_lower = pitch_angle_limit_lower;
float ef_pitch_limit_upper = pitch_angle_limit_upper;
convert_bf_to_ef(pitch_angle_limit_lower, yaw_angle_limit_lower, ef_pitch_limit_lower, ef_yaw_limit_lower);
convert_bf_to_ef(pitch_angle_limit_upper, yaw_angle_limit_upper, ef_pitch_limit_upper, ef_yaw_limit_upper);
const AP_AHRS &ahrs = AP::ahrs();
float ef_yaw_current = wrap_180_cd(ahrs.yaw_sensor);
struct Tracker::NavStatus &nav_status = tracker.nav_status;
float ef_yaw_target = wrap_180_cd((nav_status.bearing+g.yaw_trim)*100);
float ef_yaw_angle_error = wrap_180_cd(ef_yaw_target - ef_yaw_current);
// calculate angle error to target in both directions (i.e. moving up/right or lower/left)
float yaw_angle_error_upper;
float yaw_angle_error_lower;
if (ef_yaw_angle_error >= 0) {
yaw_angle_error_upper = ef_yaw_angle_error;
yaw_angle_error_lower = ef_yaw_angle_error - 36000;
} else {
yaw_angle_error_upper = 36000 + ef_yaw_angle_error;
yaw_angle_error_lower = ef_yaw_angle_error;
}
// checks that the vehicle is outside the tracker's range
if ((yaw_angle_error_lower < ef_yaw_limit_lower) && (yaw_angle_error_upper > ef_yaw_limit_upper)) {
// if the tracker is trying to move clockwise to reach the vehicle,
// but the tracker could get closer to the vehicle by moving counter-clockwise then set direction_reversed to true
if (ef_yaw_angle_error>0 && ((ef_yaw_limit_lower - yaw_angle_error_lower) < (yaw_angle_error_upper - ef_yaw_limit_upper))) {
return true;
}
// if the tracker is trying to move counter-clockwise to reach the vehicle,
// but the tracker could get closer to the vehicle by moving then set direction_reversed to true
if (ef_yaw_angle_error<0 && ((ef_yaw_limit_lower - yaw_angle_error_lower) > (yaw_angle_error_upper - ef_yaw_limit_upper))) {
return true;
}
}
// if we get this far we can use the regular, shortest path to the target
return false;
}