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
|
/****************************************************************************
*
* Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name ECL nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file ecl_l1_pos_control.h
* Implementation of L1 position control.
*
*
* Acknowledgements and References:
*
* This implementation has been built for PX4 based on the original
* publication from [1] and does include a lot of the ideas (not code)
* from [2].
*
*
* [1] S. Park, J. Deyst, and J. P. How, "A New Nonlinear Guidance Logic for Trajectory Tracking,"
* Proceedings of the AIAA Guidance, Navigation and Control
* Conference, Aug 2004. AIAA-2004-4900.
*
* [2] Paul Riseborough, Brandon Jones and Andrew Tridgell, L1 control for APM. Aug 2013.
* - Explicit control over frequency and damping
* - Explicit control over track capture angle
* - Ability to use loiter radius smaller than L1 length
* - Modified to use PD control for circle tracking to enable loiter radius less than L1 length
* - Modified to enable period and damping of guidance loop to be set explicitly
* - Modified to provide explicit control over capture angle
*
*/
#ifndef ECL_L1_POS_CONTROL_H
#define ECL_L1_POS_CONTROL_H
#include <mathlib/mathlib.h>
#include <geo/geo.h>
#include <ecl/ecl.h>
/**
* L1 Nonlinear Guidance Logic
*/
class __EXPORT ECL_L1_Pos_Control
{
public:
ECL_L1_Pos_Control() {
_L1_period = 25;
_L1_damping = 0.75f;
}
/**
* The current target bearing
*
* @return bearing angle (-pi..pi, in NED frame)
*/
float nav_bearing();
/**
* Get lateral acceleration demand.
*
* @return Lateral acceleration in m/s^2
*/
float nav_lateral_acceleration_demand();
/**
* Heading error.
*
* The heading error is either compared to the current track
* or to the tangent of the current loiter radius.
*/
float bearing_error();
/**
* Bearing from aircraft to current target.
*
* @return bearing angle (-pi..pi, in NED frame)
*/
float target_bearing();
/**
* Get roll angle setpoint for fixed wing.
*
* @return Roll angle (in NED frame)
*/
float nav_roll();
/**
* Get the current crosstrack error.
*
* @return Crosstrack error in meters.
*/
float crosstrack_error();
/**
* Returns true if the loiter waypoint has been reached
*/
bool reached_loiter_target();
/**
* Get the switch distance
*
* This is the distance at which the system will
* switch to the next waypoint. This depends on the
* period and damping
*
* @param waypoint_switch_radius The switching radius the waypoint has set.
*/
float switch_distance(float waypoint_switch_radius);
/**
* Navigate between two waypoints
*
* Calling this function with two waypoints results in the
* control outputs to fly to the line segment defined by
* the points and once captured following the line segment.
* This follows the logic in [1].
*
* @return sets _lateral_accel setpoint
*/
void navigate_waypoints(const math::Vector2f &vector_A, const math::Vector2f &vector_B, const math::Vector2f &vector_curr_position,
const math::Vector2f &ground_speed);
/**
* Navigate on an orbit around a loiter waypoint.
*
* This allow orbits smaller than the L1 length,
* this modification was introduced in [2].
*
* @return sets _lateral_accel setpoint
*/
void navigate_loiter(const math::Vector2f &vector_A, const math::Vector2f &vector_curr_position, float radius, int8_t loiter_direction,
const math::Vector2f &ground_speed_vector);
/**
* Navigate on a fixed bearing.
*
* This only holds a certain direction and does not perform cross
* track correction. Helpful for semi-autonomous modes. Introduced
* by [2].
*
* @return sets _lateral_accel setpoint
*/
void navigate_heading(float navigation_heading, float current_heading, const math::Vector2f &ground_speed);
/**
* Keep the wings level.
*
* This is typically needed for maximum-lift-demand situations,
* such as takeoff or near stall. Introduced in [2].
*/
void navigate_level_flight(float current_heading);
/**
* Set the L1 period.
*/
void set_l1_period(float period) {
_L1_period = period;
/* calculate the ratio introduced in [2] */
_L1_ratio = 1.0f / M_PI_F * _L1_damping * _L1_period;
/* calculate normalized frequency for heading tracking */
_heading_omega = sqrtf(2.0f) * M_PI_F / _L1_period;
}
/**
* Set the L1 damping factor.
*
* The original publication recommends a default of sqrt(2) / 2 = 0.707
*/
void set_l1_damping(float damping) {
_L1_damping = damping;
/* calculate the ratio introduced in [2] */
_L1_ratio = 1.0f / M_PI_F * _L1_damping * _L1_period;
/* calculate the L1 gain (following [2]) */
_K_L1 = 4.0f * _L1_damping * _L1_damping;
}
private:
float _lateral_accel; ///< Lateral acceleration setpoint in m/s^2
float _L1_distance; ///< L1 lead distance, defined by period and damping
bool _circle_mode; ///< flag for loiter mode
float _nav_bearing; ///< bearing to L1 reference point
float _bearing_error; ///< bearing error
float _crosstrack_error; ///< crosstrack error in meters
float _target_bearing; ///< the heading setpoint
float _L1_period; ///< L1 tracking period in seconds
float _L1_damping; ///< L1 damping ratio
float _L1_ratio; ///< L1 ratio for navigation
float _K_L1; ///< L1 control gain for _L1_damping
float _heading_omega; ///< Normalized frequency
/**
* Convert a 2D vector from WGS84 to planar coordinates.
*
* This converts from latitude and longitude to planar
* coordinates with (0,0) being at the position of ref and
* returns a vector in meters towards wp.
*
* @param ref The reference position in WGS84 coordinates
* @param wp The point to convert to into the local coordinates, in WGS84 coordinates
* @return The vector in meters pointing from the reference position to the coordinates
*/
math::Vector2f get_local_planar_vector(const math::Vector2f &origin, const math::Vector2f &target) const;
};
#endif /* ECL_L1_POS_CONTROL_H */
|