Line data Source code
1 : #include <AP_HAL/AP_HAL.h>
2 : #include "AP_L1_Control.h"
3 :
4 : extern const AP_HAL::HAL& hal;
5 :
6 : // table of user settable parameters
7 : const AP_Param::GroupInfo AP_L1_Control::var_info[] = {
8 : // @Param: PERIOD
9 : // @DisplayName: L1 control period
10 : // @Description: Period in seconds of L1 tracking loop. This parameter is the primary control for aggressiveness of turns in auto mode. This needs to be larger for less responsive airframes. The default is quite conservative, but for most RC aircraft will lead to reasonable flight. For smaller more agile aircraft a value closer to 15 is appropriate, or even as low as 10 for some very agile aircraft. When tuning, change this value in small increments, as a value that is much too small (say 5 or 10 below the right value) can lead to very radical turns, and a risk of stalling.
11 : // @Units: s
12 : // @Range: 1 60
13 : // @Increment: 1
14 : // @User: Standard
15 : AP_GROUPINFO("PERIOD", 0, AP_L1_Control, _L1_period, 17),
16 :
17 : // @Param: DAMPING
18 : // @DisplayName: L1 control damping ratio
19 : // @Description: Damping ratio for L1 control. Increase this in increments of 0.05 if you are getting overshoot in path tracking. You should not need a value below 0.7 or above 0.85.
20 : // @Range: 0.6 1.0
21 : // @Increment: 0.05
22 : // @User: Advanced
23 : AP_GROUPINFO("DAMPING", 1, AP_L1_Control, _L1_damping, 0.75f),
24 :
25 : // @Param: XTRACK_I
26 : // @DisplayName: L1 control crosstrack integrator gain
27 : // @Description: Crosstrack error integrator gain. This gain is applied to the crosstrack error to ensure it converges to zero. Set to zero to disable. Smaller values converge slower, higher values will cause crosstrack error oscillation.
28 : // @Range: 0 0.1
29 : // @Increment: 0.01
30 : // @User: Advanced
31 : AP_GROUPINFO("XTRACK_I", 2, AP_L1_Control, _L1_xtrack_i_gain, 0.02),
32 :
33 : // @Param: LIM_BANK
34 : // @DisplayName: Loiter Radius Bank Angle Limit
35 : // @Description: The sealevel bank angle limit for a continuous loiter. (Used to calculate airframe loading limits at higher altitudes). Setting to 0, will instead just scale the loiter radius directly
36 : // @Units: deg
37 : // @Range: 0 89
38 : // @User: Advanced
39 : AP_GROUPINFO("LIM_BANK", 3, AP_L1_Control, _loiter_bank_limit, 0.0f),
40 :
41 : AP_GROUPEND
42 : };
43 :
44 : // Bank angle command based on angle between aircraft velocity vector and reference vector to path.
45 : // S. Park, J. Deyst, and J. P. How, "A New Nonlinear Guidance Logic for Trajectory Tracking,"
46 : // Proceedings of the AIAA Guidance, Navigation and Control
47 : // Conference, Aug 2004. AIAA-2004-4900.
48 : // Modified to use PD control for circle tracking to enable loiter radius less than L1 length
49 : // Modified to enable period and damping of guidance loop to be set explicitly
50 : // Modified to provide explicit control over capture angle
51 :
52 :
53 : /*
54 : Wrap AHRS yaw if in reverse - radians
55 : */
56 1945885 : float AP_L1_Control::get_yaw() const
57 : {
58 1945885 : if (_reverse) {
59 0 : return wrap_PI(M_PI + _ahrs.get_yaw_rad());
60 : }
61 1945885 : return _ahrs.get_yaw_rad();
62 : }
63 :
64 : /*
65 : Wrap AHRS yaw sensor if in reverse - centi-degress
66 : */
67 84637 : int32_t AP_L1_Control::get_yaw_sensor() const
68 : {
69 84637 : if (_reverse) {
70 0 : return wrap_180_cd(18000 + _ahrs.yaw_sensor);
71 : }
72 84637 : return _ahrs.yaw_sensor;
73 : }
74 :
75 : /*
76 : return the bank angle needed to achieve tracking from the last
77 : update_*() operation
78 : */
79 3791334 : int32_t AP_L1_Control::nav_roll_cd(void) const
80 : {
81 : float ret;
82 : /*
83 : formula can be obtained through equations of balanced spiral:
84 : liftForce * cos(roll) = gravityForce * cos(pitch);
85 : liftForce * sin(roll) = gravityForce * lateralAcceleration / gravityAcceleration; // as mass = gravityForce/gravityAcceleration
86 : see issue 24319 [https://github.com/ArduPilot/ardupilot/issues/24319]
87 : Multiplier 100.0f is for converting degrees to centidegrees
88 : Made changes to avoid zero division as proposed by Andrew Tridgell: https://github.com/ArduPilot/ardupilot/pull/24331#discussion_r1267798397
89 : */
90 3791334 : float pitchLimL1 = radians(60); // Suggestion: constraint may be modified to pitch limits if their absolute values are less than 90 degree and more than 60 degrees.
91 3791334 : float pitchL1 = constrain_float(_ahrs.get_pitch_rad(),-pitchLimL1,pitchLimL1);
92 3791334 : ret = degrees(atanf(_latAccDem * (1.0f/(GRAVITY_MSS * cosf(pitchL1))))) * 100.0f;
93 3791334 : ret = constrain_float(ret, -9000, 9000);
94 3791334 : return ret;
95 : }
96 :
97 : /*
98 : return the lateral acceleration needed to achieve tracking from the last
99 : update_*() operation
100 : */
101 0 : float AP_L1_Control::lateral_acceleration(void) const
102 : {
103 0 : return _latAccDem;
104 : }
105 :
106 1752182 : int32_t AP_L1_Control::nav_bearing_cd(void) const
107 : {
108 1752182 : return wrap_180_cd(rad_to_cd(_nav_bearing));
109 : }
110 :
111 128952 : int32_t AP_L1_Control::bearing_error_cd(void) const
112 : {
113 128952 : return rad_to_cd(_bearing_error);
114 : }
115 :
116 2359519 : int32_t AP_L1_Control::target_bearing_cd(void) const
117 : {
118 2359519 : return wrap_180_cd(_target_bearing_cd);
119 : }
120 :
121 : /*
122 : this is the turn distance assuming a 90 degree turn
123 : */
124 145036 : float AP_L1_Control::turn_distance(float wp_radius) const
125 : {
126 145036 : wp_radius *= sq(_ahrs.get_EAS2TAS());
127 145036 : return MIN(wp_radius, _L1_dist);
128 : }
129 :
130 : /*
131 : this approximates the turn distance for a given turn angle. If the
132 : turn_angle is > 90 then a 90 degree turn distance is used, otherwise
133 : the turn distance is reduced linearly.
134 : This function allows straight ahead mission legs to avoid thinking
135 : they have reached the waypoint early, which makes things like camera
136 : trigger and ball drop at exact positions under mission control much easier
137 : */
138 145036 : float AP_L1_Control::turn_distance(float wp_radius, float turn_angle) const
139 : {
140 145036 : float distance_90 = turn_distance(wp_radius);
141 145036 : turn_angle = fabsf(turn_angle);
142 145036 : if (turn_angle >= 90) {
143 62026 : return distance_90;
144 : }
145 83010 : return distance_90 * turn_angle / 90.0f;
146 : }
147 :
148 104079 : float AP_L1_Control::loiter_radius(const float radius) const
149 : {
150 : // prevent an insane loiter bank limit
151 104079 : float sanitized_bank_limit = constrain_float(_loiter_bank_limit, 0.0f, 89.0f);
152 104079 : float lateral_accel_sea_level = tanf(radians(sanitized_bank_limit)) * GRAVITY_MSS;
153 :
154 104079 : float nominal_velocity_sea_level = 0.0f;
155 104079 : if(_tecs != nullptr) {
156 104079 : nominal_velocity_sea_level = _tecs->get_target_airspeed();
157 : }
158 :
159 104079 : float eas2tas_sq = sq(_ahrs.get_EAS2TAS());
160 :
161 113030 : if (is_zero(sanitized_bank_limit) || is_zero(nominal_velocity_sea_level) ||
162 8951 : is_zero(lateral_accel_sea_level)) {
163 : // Missing a sane input for calculating the limit, or the user has
164 : // requested a straight scaling with altitude. This will always vary
165 : // with the current altitude, but will at least protect the airframe
166 95128 : return radius * eas2tas_sq;
167 : } else {
168 8951 : float sea_level_radius = sq(nominal_velocity_sea_level) / lateral_accel_sea_level;
169 8951 : if (sea_level_radius > radius) {
170 : // If we've told the plane that its sea level radius is unachievable fallback to
171 : // straight altitude scaling
172 3146 : return radius * eas2tas_sq;
173 : } else {
174 : // select the requested radius, or the required altitude scale, whichever is safer
175 5805 : return MAX(sea_level_radius * eas2tas_sq, radius);
176 : }
177 : }
178 : }
179 :
180 1032298 : bool AP_L1_Control::reached_loiter_target(void)
181 : {
182 1032298 : return _WPcircle;
183 : }
184 :
185 : /**
186 : prevent indecision in our turning by using our previous turn
187 : decision if we are in a narrow angle band pointing away from the
188 : target and the turn angle has changed sign
189 : */
190 1254093 : void AP_L1_Control::_prevent_indecision(float &Nu)
191 : {
192 1254093 : const float Nu_limit = 0.9f*M_PI;
193 2593783 : if (fabsf(Nu) > Nu_limit &&
194 85597 : fabsf(_last_Nu) > Nu_limit &&
195 1422720 : labs(wrap_180_cd(_target_bearing_cd - get_yaw_sensor())) > 12000 &&
196 83030 : Nu * _last_Nu < 0.0f) {
197 : // we are moving away from the target waypoint and pointing
198 : // away from the waypoint (not flying backwards). The sign
199 : // of Nu has also changed, which means we are
200 : // oscillating in our decision about which way to go
201 13366 : Nu = _last_Nu;
202 : }
203 1254093 : }
204 :
205 : // update L1 control for waypoint navigation
206 1155680 : void AP_L1_Control::update_waypoint(const Location &prev_WP, const Location &next_WP, float dist_min)
207 : {
208 :
209 1155680 : Location _current_loc;
210 : float Nu;
211 : float xtrackVel;
212 : float ltrackVel;
213 :
214 1155680 : uint32_t now = AP_HAL::micros();
215 1155680 : float dt = (now - _last_update_waypoint_us) * 1.0e-6f;
216 1155680 : if (dt > 1) {
217 : // controller hasn't been called for an extended period of
218 : // time. Reinitialise it.
219 351 : _L1_xtrack_i = 0.0f;
220 : }
221 1155680 : if (dt > 0.1) {
222 10882 : dt = 0.1;
223 : }
224 1155680 : _last_update_waypoint_us = now;
225 :
226 : // Calculate L1 gain required for specified damping
227 1155680 : float K_L1 = 4.0f * _L1_damping * _L1_damping;
228 :
229 : // Get current position and velocity
230 1155680 : if (_ahrs.get_location(_current_loc) == false) {
231 : // if no GPS loc available, maintain last nav/target_bearing
232 3 : _data_is_stale = true;
233 3 : return;
234 : }
235 :
236 1155677 : Vector2f _groundspeed_vector = _ahrs.groundspeed_vector();
237 :
238 : // update _target_bearing_cd
239 1155677 : _target_bearing_cd = _current_loc.get_bearing_to(next_WP);
240 :
241 : // Calculate groundspeed
242 1155677 : float groundSpeed = _groundspeed_vector.length();
243 :
244 : // check if we are moving in the direction of the front of the vehicle
245 1155677 : const bool moving_forwards = fabsf(wrap_PI(_groundspeed_vector.angle() - get_yaw())) < M_PI_2;
246 :
247 1155677 : if (groundSpeed < 0.1f || !moving_forwards) {
248 : // use a small ground speed vector in the right direction,
249 : // allowing us to use the compass heading at zero GPS velocity
250 379542 : groundSpeed = 0.1f;
251 379542 : _groundspeed_vector = Vector2f(cosf(get_yaw()), sinf(get_yaw())) * groundSpeed;
252 : }
253 :
254 : // Calculate time varying control parameters
255 : // Calculate the L1 length required for specified period
256 : // 0.3183099 = 1/1/pipi
257 1155677 : _L1_dist = MAX(0.3183099f * _L1_damping * _L1_period * groundSpeed, dist_min);
258 :
259 : // Calculate the NE position of WP B relative to WP A
260 1155677 : Vector2f AB = prev_WP.get_distance_NE(next_WP);
261 1155677 : float AB_length = AB.length();
262 :
263 : // Check for AB zero length and track directly to the destination
264 : // if too small
265 1155677 : if (AB.length() < 1.0e-6f) {
266 15601 : AB = _current_loc.get_distance_NE(next_WP);
267 15601 : if (AB.length() < 1.0e-6f) {
268 15562 : AB = Vector2f(cosf(get_yaw()), sinf(get_yaw()));
269 : }
270 : }
271 1155677 : AB.normalize();
272 :
273 : // Calculate the NE position of the aircraft relative to WP A
274 1155677 : const Vector2f A_air = prev_WP.get_distance_NE(_current_loc);
275 :
276 : // calculate distance to target track, for reporting
277 1155677 : _crosstrack_error = A_air % AB;
278 :
279 : // Determine if the aircraft is behind a +-135 degree degree arc centred on WP A
280 : // and further than L1 distance from WP A. Then use WP A as the L1 reference point
281 : // Otherwise do normal L1 guidance
282 1155677 : float WP_A_dist = A_air.length();
283 1155677 : float alongTrackDist = A_air * AB;
284 1155677 : if (WP_A_dist > _L1_dist && alongTrackDist/MAX(WP_A_dist, 1.0f) < -0.7071f)
285 : {
286 : // Calc Nu to fly To WP A
287 984 : Vector2f A_air_unit = (A_air).normalized(); // Unit vector from WP A to aircraft
288 984 : xtrackVel = _groundspeed_vector % (-A_air_unit); // Velocity across line
289 984 : ltrackVel = _groundspeed_vector * (-A_air_unit); // Velocity along line
290 984 : Nu = atan2f(xtrackVel,ltrackVel);
291 984 : _nav_bearing = atan2f(-A_air_unit.y , -A_air_unit.x); // bearing (radians) from AC to L1 point
292 1154693 : } else if (alongTrackDist > AB_length + groundSpeed*3) {
293 : // we have passed point B by 3 seconds. Head towards B
294 : // Calc Nu to fly To WP B
295 0 : const Vector2f B_air = next_WP.get_distance_NE(_current_loc);
296 0 : Vector2f B_air_unit = (B_air).normalized(); // Unit vector from WP B to aircraft
297 0 : xtrackVel = _groundspeed_vector % (-B_air_unit); // Velocity across line
298 0 : ltrackVel = _groundspeed_vector * (-B_air_unit); // Velocity along line
299 0 : Nu = atan2f(xtrackVel,ltrackVel);
300 0 : _nav_bearing = atan2f(-B_air_unit.y , -B_air_unit.x); // bearing (radians) from AC to L1 point
301 : } else { // Calc Nu to fly along AB line
302 :
303 : // Calculate Nu2 angle (angle of velocity vector relative to line connecting waypoints)
304 1154693 : xtrackVel = _groundspeed_vector % AB; // Velocity cross track
305 1154693 : ltrackVel = _groundspeed_vector * AB; // Velocity along track
306 1154693 : float Nu2 = atan2f(xtrackVel,ltrackVel);
307 : // Calculate Nu1 angle (Angle to L1 reference point)
308 1154693 : float sine_Nu1 = _crosstrack_error/MAX(_L1_dist, 0.1f);
309 : // Limit sine of Nu1 to provide a controlled track capture angle of 45 deg
310 1154693 : sine_Nu1 = constrain_float(sine_Nu1, -0.7071f, 0.7071f);
311 1154693 : float Nu1 = asinf(sine_Nu1);
312 :
313 : // compute integral error component to converge to a crosstrack of zero when traveling
314 : // straight but reset it when disabled or if it changes. That allows for much easier
315 : // tuning by having it re-converge each time it changes.
316 1154693 : if (_L1_xtrack_i_gain <= 0 || !is_equal(_L1_xtrack_i_gain.get(), _L1_xtrack_i_gain_prev)) {
317 124 : _L1_xtrack_i = 0;
318 124 : _L1_xtrack_i_gain_prev = _L1_xtrack_i_gain;
319 1154569 : } else if (fabsf(Nu1) < radians(5)) {
320 1079854 : _L1_xtrack_i += Nu1 * _L1_xtrack_i_gain * dt;
321 :
322 : // an AHRS_TRIM_X=0.1 will drift to about 0.08 so 0.1 is a good worst-case to clip at
323 1079854 : _L1_xtrack_i = constrain_float(_L1_xtrack_i, -0.1f, 0.1f);
324 : }
325 :
326 : // to converge to zero we must push Nu1 harder
327 1154693 : Nu1 += _L1_xtrack_i;
328 :
329 1154693 : Nu = Nu1 + Nu2;
330 1154693 : _nav_bearing = wrap_PI(atan2f(AB.y, AB.x) + Nu1); // bearing (radians) from AC to L1 point
331 : }
332 :
333 1155677 : _prevent_indecision(Nu);
334 1155677 : _last_Nu = Nu;
335 :
336 : // Limit Nu to +-(pi/2)
337 1155677 : Nu = constrain_float(Nu, -1.5708f, +1.5708f);
338 1155677 : _latAccDem = K_L1 * groundSpeed * groundSpeed / _L1_dist * sinf(Nu);
339 :
340 : // Waypoint capture status is always false during waypoint following
341 1155677 : _WPcircle = false;
342 1155677 : _last_loiter.reached_loiter_target_ms = 0;
343 :
344 1155677 : _bearing_error = Nu; // bearing error angle (radians), +ve to left of track
345 :
346 1155677 : _data_is_stale = false; // status are correctly updated with current waypoint data
347 : }
348 :
349 : // update L1 control for loitering
350 98416 : void AP_L1_Control::update_loiter(const Location ¢er_WP, float radius, int8_t loiter_direction)
351 : {
352 98416 : const float radius_unscaled = radius;
353 :
354 98416 : Location _current_loc;
355 :
356 : // scale loiter radius with square of EAS2TAS to allow us to stay
357 : // stable at high altitude
358 98416 : radius = loiter_radius(fabsf(radius));
359 :
360 : // Calculate guidance gains used by PD loop (used during circle tracking)
361 98416 : float omega = (6.2832f / _L1_period);
362 98416 : float Kx = omega * omega;
363 98416 : float Kv = 2.0f * _L1_damping * omega;
364 :
365 : // Calculate L1 gain required for specified damping (used during waypoint capture)
366 98416 : float K_L1 = 4.0f * _L1_damping * _L1_damping;
367 :
368 : // Get current position and velocity
369 98416 : if (_ahrs.get_location(_current_loc) == false) {
370 : // if no GPS loc available, maintain last nav/target_bearing
371 0 : _data_is_stale = true;
372 0 : return;
373 : }
374 :
375 98416 : Vector2f _groundspeed_vector = _ahrs.groundspeed_vector();
376 :
377 : // Calculate groundspeed
378 98416 : float groundSpeed = MAX(_groundspeed_vector.length() , 1.0f);
379 :
380 :
381 : // update _target_bearing_cd
382 98416 : _target_bearing_cd = _current_loc.get_bearing_to(center_WP);
383 :
384 :
385 : // Calculate time varying control parameters
386 : // Calculate the L1 length required for specified period
387 : // 0.3183099 = 1/pi
388 98416 : _L1_dist = 0.3183099f * _L1_damping * _L1_period * groundSpeed;
389 :
390 : // Calculate the NE position of the aircraft relative to WP A
391 98416 : const Vector2f A_air = center_WP.get_distance_NE(_current_loc);
392 :
393 : // Calculate the unit vector from WP A to aircraft
394 : // protect against being on the waypoint and having zero velocity
395 : // if too close to the waypoint, use the velocity vector
396 : // if the velocity vector is too small, use the heading vector
397 98416 : Vector2f A_air_unit;
398 98416 : if (A_air.length() > 0.1f) {
399 96296 : A_air_unit = A_air.normalized();
400 : } else {
401 2120 : if (_groundspeed_vector.length() < 0.1f) {
402 1926 : A_air_unit = Vector2f(cosf(_ahrs.get_yaw_rad()), sinf(_ahrs.get_yaw_rad()));
403 : } else {
404 194 : A_air_unit = _groundspeed_vector.normalized();
405 : }
406 : }
407 :
408 : // Calculate Nu to capture center_WP
409 98416 : float xtrackVelCap = A_air_unit % _groundspeed_vector; // Velocity across line - perpendicular to radial inbound to WP
410 98416 : float ltrackVelCap = - (_groundspeed_vector * A_air_unit); // Velocity along line - radial inbound to WP
411 98416 : float Nu = atan2f(xtrackVelCap,ltrackVelCap);
412 :
413 98416 : _prevent_indecision(Nu);
414 98416 : _last_Nu = Nu;
415 :
416 98416 : Nu = constrain_float(Nu, -M_PI_2, M_PI_2); // Limit Nu to +- Pi/2
417 :
418 : // Calculate lat accln demand to capture center_WP (use L1 guidance law)
419 98416 : float latAccDemCap = K_L1 * groundSpeed * groundSpeed / _L1_dist * sinf(Nu);
420 :
421 : // Calculate radial position and velocity errors
422 98416 : float xtrackVelCirc = -ltrackVelCap; // Radial outbound velocity - reuse previous radial inbound velocity
423 98416 : float xtrackErrCirc = A_air.length() - radius; // Radial distance from the loiter circle
424 :
425 : // keep crosstrack error for reporting
426 98416 : _crosstrack_error = xtrackErrCirc;
427 :
428 : // Calculate PD control correction to circle waypoint_ahrs.roll
429 98416 : float latAccDemCircPD = (xtrackErrCirc * Kx + xtrackVelCirc * Kv);
430 :
431 : // Calculate tangential velocity
432 98416 : float velTangent = xtrackVelCap * float(loiter_direction);
433 :
434 : // Calculate centripetal acceleration demand
435 98416 : float latAccDemCircCtr = velTangent * velTangent / MAX((0.5f * radius), (radius + xtrackErrCirc));
436 :
437 : // Prevent PD demand from turning the wrong way by limiting it when flying
438 : // the wrong way, or while flying out from inside the loiter radius
439 98416 : if (ltrackVelCap < 0.0f) {
440 38086 : if (velTangent < 0.0f) {
441 3934 : latAccDemCircPD = MAX(latAccDemCircPD, 0.0f);
442 34152 : } else if (xtrackErrCirc < 0.0f) {
443 8639 : latAccDemCircPD = MAX(latAccDemCircPD, -latAccDemCircCtr);
444 : }
445 : }
446 :
447 : // Sum PD control and centripetal acceleration to calculate lateral manoeuvre demand
448 98416 : float latAccDemCirc = loiter_direction * (latAccDemCircPD + latAccDemCircCtr);
449 :
450 : // Perform switchover between 'capture' and 'circle' modes at the
451 : // point where the commands cross over to achieve a seamless transfer
452 : // Only fly 'capture' mode if outside the circle
453 98416 : const uint32_t now_ms = AP_HAL::millis();
454 98416 : if (xtrackErrCirc > 0.0f && loiter_direction * latAccDemCap < loiter_direction * latAccDemCirc) {
455 34386 : _latAccDem = latAccDemCap;
456 :
457 : /*
458 : if we were previously on the circle and the target has not
459 : changed then keep _WPcircle true. This prevents
460 : reached_loiter_target() from going false due to a gust of
461 : wind or an unachievable loiter radius
462 : */
463 72147 : if (_WPcircle &&
464 3375 : _last_loiter.reached_loiter_target_ms != 0 &&
465 3375 : now_ms - _last_loiter.reached_loiter_target_ms < 200U &&
466 6726 : loiter_direction == _last_loiter.direction &&
467 41124 : is_equal(radius_unscaled, _last_loiter.radius) &&
468 3361 : center_WP.same_loc_as(_last_loiter.center_WP)) {
469 : // same location, within 200ms, keep the _WPcircle status as true
470 3313 : _last_loiter.reached_loiter_target_ms = now_ms;
471 : } else {
472 31073 : _WPcircle = false;
473 31073 : _last_loiter.reached_loiter_target_ms = 0;
474 : }
475 :
476 34386 : _bearing_error = Nu; // angle between demanded and achieved velocity vector, +ve to left of track
477 34386 : _nav_bearing = atan2f(-A_air_unit.y , -A_air_unit.x); // bearing (radians) from AC to L1 point
478 : } else {
479 64030 : _latAccDem = latAccDemCirc;
480 64030 : _WPcircle = true;
481 64030 : _last_loiter.reached_loiter_target_ms = now_ms;
482 64030 : _bearing_error = 0.0f; // bearing error (radians), +ve to left of track
483 64030 : _nav_bearing = atan2f(-A_air_unit.y , -A_air_unit.x); // bearing (radians) from AC to L1 point
484 : }
485 :
486 98416 : _last_loiter.radius = radius_unscaled;
487 98416 : _last_loiter.direction = loiter_direction;
488 98416 : _last_loiter.center_WP = center_WP;
489 :
490 98416 : _data_is_stale = false; // status are correctly updated with current waypoint data
491 : }
492 :
493 :
494 : // update L1 control for heading hold navigation
495 3487 : void AP_L1_Control::update_heading_hold(int32_t navigation_heading_cd)
496 : {
497 : // Calculate normalised frequency for tracking loop
498 3487 : const float omegaA = 4.4428f/_L1_period; // sqrt(2)*pi/period
499 : // Calculate additional damping gain
500 :
501 : int32_t Nu_cd;
502 : float Nu;
503 :
504 : // copy to _target_bearing_cd and _nav_bearing
505 3487 : _target_bearing_cd = wrap_180_cd(navigation_heading_cd);
506 3487 : _nav_bearing = cd_to_rad(navigation_heading_cd);
507 :
508 3487 : Nu_cd = _target_bearing_cd - wrap_180_cd(_ahrs.yaw_sensor);
509 3487 : Nu_cd = wrap_180_cd(Nu_cd);
510 3487 : Nu = cd_to_rad(Nu_cd);
511 :
512 3487 : Vector2f _groundspeed_vector = _ahrs.groundspeed_vector();
513 :
514 : // Calculate groundspeed
515 3487 : float groundSpeed = _groundspeed_vector.length();
516 :
517 : // Calculate time varying control parameters
518 3487 : _L1_dist = groundSpeed / omegaA; // L1 distance is adjusted to maintain a constant tracking loop frequency
519 3487 : float VomegaA = groundSpeed * omegaA;
520 :
521 : // Waypoint capture status is always false during heading hold
522 3487 : _WPcircle = false;
523 3487 : _last_loiter.reached_loiter_target_ms = 0;
524 :
525 3487 : _crosstrack_error = 0;
526 :
527 3487 : _bearing_error = Nu; // bearing error angle (radians), +ve to left of track
528 :
529 : // Limit Nu to +-pi
530 3487 : Nu = constrain_float(Nu, -M_PI_2, M_PI_2);
531 3487 : _latAccDem = 2.0f*sinf(Nu)*VomegaA;
532 :
533 3487 : _data_is_stale = false; // status are correctly updated with current waypoint data
534 3487 : }
535 :
536 : // update L1 control for level flight on current heading
537 1827 : void AP_L1_Control::update_level_flight(void)
538 : {
539 : // copy to _target_bearing_cd and _nav_bearing
540 1827 : _target_bearing_cd = _ahrs.yaw_sensor;
541 1827 : _nav_bearing = _ahrs.get_yaw_rad();
542 1827 : _bearing_error = 0;
543 1827 : _crosstrack_error = 0;
544 :
545 : // Waypoint capture status is always false during heading hold
546 1827 : _WPcircle = false;
547 1827 : _last_loiter.reached_loiter_target_ms = 0;
548 :
549 1827 : _latAccDem = 0;
550 :
551 1827 : _data_is_stale = false; // status are correctly updated with current waypoint data
552 1827 : }
|
