Line data Source code
1 : #include "AP_Soaring.h"
2 :
3 : #if HAL_SOARING_ENABLED
4 :
5 : #include <AP_AHRS/AP_AHRS.h>
6 : #include <AP_Logger/AP_Logger.h>
7 : #include <AP_TECS/AP_TECS.h>
8 : #include <GCS_MAVLink/GCS.h>
9 : #include <stdint.h>
10 :
11 : #if HAL_SOARING_NVF_EKF_ENABLED
12 : static constexpr uint32_t NVF_PUBLISHER_DELAY_MS = 250;
13 : #endif // HAL_SOARING_NVF_EKF_ENABLED
14 :
15 : // ArduSoar parameters
16 : const AP_Param::GroupInfo SoaringController::var_info[] = {
17 : // @Param: ENABLE
18 : // @DisplayName: Is the soaring mode enabled or not
19 : // @Description: Toggles the soaring mode on and off
20 : // @Values: 0:Disable,1:Enable
21 : // @User: Advanced
22 : AP_GROUPINFO_FLAGS("ENABLE", 1, SoaringController, soar_active, 0, AP_PARAM_FLAG_ENABLE),
23 :
24 : // @Param: VSPEED
25 : // @DisplayName: Vertical v-speed
26 : // @Description: Rate of climb to trigger themalling speed
27 : // @Units: m/s
28 : // @Range: 0 10
29 : // @User: Advanced
30 : AP_GROUPINFO("VSPEED", 2, SoaringController, thermal_vspeed, 0.7f),
31 :
32 : // @Param: Q1
33 : // @DisplayName: Process noise
34 : // @Description: Standard deviation of noise in process for strength
35 : // @Range: 0.0001 0.01
36 : // @User: Advanced
37 : AP_GROUPINFO("Q1", 3, SoaringController, thermal_q1, 0.001f),
38 :
39 : // @Param: Q2
40 : // @DisplayName: Process noise
41 : // @Description: Standard deviation of noise in process for position and radius
42 : // @Range: 0.01 1
43 : // @User: Advanced
44 : AP_GROUPINFO("Q2", 4, SoaringController, thermal_q2, 0.03f),
45 :
46 : // @Param: R
47 : // @DisplayName: Measurement noise
48 : // @Description: Standard deviation of noise in measurement
49 : // @Range: 0.01 1
50 : // @User: Advanced
51 :
52 : AP_GROUPINFO("R", 5, SoaringController, thermal_r, 0.45f),
53 :
54 : // @Param: DIST_AHEAD
55 : // @DisplayName: Distance to thermal center
56 : // @Description: Initial guess of the distance to the thermal center
57 : // @Units: m
58 : // @Range: 0 100
59 : // @User: Advanced
60 : AP_GROUPINFO("DIST_AHEAD", 6, SoaringController, thermal_distance_ahead, 5.0f),
61 :
62 : // @Param: MIN_THML_S
63 : // @DisplayName: Minimum thermalling time
64 : // @Description: Minimum number of seconds to spend thermalling
65 : // @Units: s
66 : // @Range: 0 600
67 : // @User: Advanced
68 : AP_GROUPINFO("MIN_THML_S", 7, SoaringController, min_thermal_s, 20),
69 :
70 : // @Param: MIN_CRSE_S
71 : // @DisplayName: Minimum cruising time
72 : // @Description: Minimum number of seconds to spend cruising
73 : // @Units: s
74 : // @Range: 0 600
75 : // @User: Advanced
76 : AP_GROUPINFO("MIN_CRSE_S", 8, SoaringController, min_cruise_s, 10),
77 :
78 : // @Param: POLAR_CD0
79 : // @DisplayName: Zero lift drag coef.
80 : // @Description: Zero lift drag coefficient
81 : // @Range: 0.005 0.5
82 : // @User: Advanced
83 : AP_GROUPINFO("POLAR_CD0", 9, SoaringController, _polarParams.CD0, 0.027),
84 :
85 : // @Param: POLAR_B
86 : // @DisplayName: Induced drag coeffient
87 : // @Description: Induced drag coeffient
88 : // @Range: 0.005 0.05
89 : // @User: Advanced
90 : AP_GROUPINFO("POLAR_B", 10, SoaringController, _polarParams.B, 0.031),
91 :
92 : // @Param: POLAR_K
93 : // @DisplayName: Cl factor
94 : // @Description: Cl factor 2*m*g/(rho*S)
95 : // @Units: m.m/s/s
96 : // @Range: 20 400
97 : // @User: Advanced
98 : AP_GROUPINFO("POLAR_K", 11, SoaringController, _polarParams.K, 25.6),
99 :
100 : // @Param: ALT_MAX
101 : // @DisplayName: Maximum soaring altitude, relative to the home location
102 : // @Description: Don't thermal any higher than this.
103 : // @Units: m
104 : // @Range: 0 5000.0
105 : // @User: Advanced
106 : AP_GROUPINFO("ALT_MAX", 12, SoaringController, alt_max, 350.0),
107 :
108 : // @Param: ALT_MIN
109 : // @DisplayName: Minimum soaring altitude, relative to the home location
110 : // @Description: Don't get any lower than this.
111 : // @Units: m
112 : // @Range: 0 1000.0
113 : // @User: Advanced
114 : AP_GROUPINFO("ALT_MIN", 13, SoaringController, alt_min, 50.0),
115 :
116 : // @Param: ALT_CUTOFF
117 : // @DisplayName: Maximum power altitude, relative to the home location
118 : // @Description: Cut off throttle at this alt.
119 : // @Units: m
120 : // @Range: 0 5000.0
121 : // @User: Advanced
122 : AP_GROUPINFO("ALT_CUTOFF", 14, SoaringController, alt_cutoff, 250.0),
123 :
124 : // 15 was SOAR_ENABLE_CH, now RCX_OPTION
125 :
126 : // @Param: MAX_DRIFT
127 : // @DisplayName: (Optional) Maximum drift distance to allow when thermalling.
128 : // @Description: The previous mode will be restored if the horizontal distance to the thermalling start location exceeds this value. -1 to disable.
129 : // @Range: 0 1000
130 : // @User: Advanced
131 : AP_GROUPINFO("MAX_DRIFT", 16, SoaringController, max_drift, -1),
132 :
133 : // @Param: MAX_RADIUS
134 : // @DisplayName: (Optional) Maximum distance from home
135 : // @Description: RTL will be entered when a thermal is exited and the plane is more than this distance from home. -1 to disable.
136 : // @Range: 0 1000
137 : // @User: Advanced
138 : AP_GROUPINFO("MAX_RADIUS", 17, SoaringController, max_radius, -1),
139 :
140 : // @Param: THML_BANK
141 : // @DisplayName: Thermalling bank angle
142 : // @Description: This parameter sets the bank angle to use when thermalling. Typically 30 - 45 degrees works well.
143 : // @Range: 20 50
144 : // @User: Advanced
145 : // @Units: deg
146 : AP_GROUPINFO("THML_BANK", 18, SoaringController, thermal_bank, 30.0),
147 :
148 : // 19 reserved for POLAR_LEARN.
149 :
150 : // @Param: THML_ARSPD
151 : // @DisplayName: Specific setting for airspeed when soaring in THERMAL mode.
152 : // @Description: If non-zero this airspeed will be used when thermalling. A value of 0 will use AIRSPEED_CRUISE.
153 : // @Range: 0 50
154 : // @User: Advanced
155 : AP_GROUPINFO("THML_ARSPD", 20, SoaringController, soar_thermal_airspeed, 0),
156 :
157 : // @Param: CRSE_ARSPD
158 : // @DisplayName: Specific setting for airspeed when soaring in AUTO mode.
159 : // @Description: If non-zero this airspeed will be used when cruising between thermals in AUTO. If set to -1, airspeed will be selected based on speed-to-fly theory. If set to 0, then AIRSPEED_CRUISE will be used while cruising between thermals.
160 : // @Range: -1 50
161 : // @User: Advanced
162 : AP_GROUPINFO("CRSE_ARSPD", 21, SoaringController, soar_cruise_airspeed, 0),
163 :
164 : // @Param: THML_FLAP
165 : // @DisplayName: Flap percent to be used during thermalling flight.
166 : // @Description: This sets the flap when in LOITER with soaring active. Overrides the usual auto flap behaviour.
167 : // @Range: 0 100
168 : // @User: Advanced
169 : AP_GROUPINFO("THML_FLAP", 22, SoaringController, soar_thermal_flap, 0),
170 :
171 : AP_GROUPEND
172 : };
173 :
174 692 : SoaringController::SoaringController(AP_TECS &tecs, const AP_FixedWing &parms) :
175 692 : _tecs(tecs),
176 692 : _vario(parms,_polarParams),
177 692 : _speedToFly(_polarParams),
178 692 : _aparm(parms),
179 1384 : _throttle_suppressed(true)
180 : {
181 692 : AP_Param::setup_object_defaults(this, var_info);
182 692 : }
183 :
184 13618 : void SoaringController::get_target(Location &wp)
185 : {
186 13618 : wp = AP::ahrs().get_home();
187 13618 : wp.offset(_position_x_filter.get(), _position_y_filter.get());
188 13618 : }
189 :
190 51738 : bool SoaringController::suppress_throttle()
191 : {
192 51738 : float alt = _vario.alt;
193 :
194 51738 : if (_throttle_suppressed && (alt < alt_min)) {
195 : // Time to throttle up
196 6 : set_throttle_suppressed(false);
197 51732 : } else if ((!_throttle_suppressed) && (alt > alt_cutoff)) {
198 : // Start glide
199 5 : set_throttle_suppressed(true);
200 :
201 : // Zero the pitch integrator - the nose is currently raised to climb, we need to go back to glide.
202 5 : _tecs.reset_pitch_I();
203 :
204 5 : _cruise_start_time_us = AP_HAL::micros64();
205 :
206 : // Reset the filtered vario rate - it is currently elevated due to the climb rate and would otherwise take a while to fall again,
207 : // leading to false positives.
208 5 : _vario.reset_trigger_filter(0.0f);
209 : }
210 :
211 51738 : return _throttle_suppressed;
212 : }
213 :
214 34129 : bool SoaringController::check_thermal_criteria()
215 : {
216 34129 : return (_last_update_status == ActiveStatus::AUTO_MODE_CHANGE
217 24748 : && ((AP_HAL::micros64() - _cruise_start_time_us) > ((unsigned)min_cruise_s * 1e6))
218 22462 : && (_vario.get_trigger_value() - _vario.get_exp_thermalling_sink()) > thermal_vspeed
219 1 : && _vario.alt < alt_max
220 58877 : && _vario.alt > alt_min);
221 : }
222 :
223 :
224 13618 : SoaringController::LoiterStatus SoaringController::check_cruise_criteria(Vector2f prev_wp, Vector2f next_wp)
225 : {
226 : // Check conditions for re-entering cruise. Note that the aircraft needs to also be aligned with the appropriate
227 : // heading before some of these conditions will actually trigger.
228 : // The GCS messages are emitted in mode_thermal.cpp. Update these if the logic here is changed.
229 :
230 13618 : if (_last_update_status == ActiveStatus::SOARING_DISABLED) {
231 0 : return LoiterStatus::DISABLED;
232 : }
233 :
234 13618 : LoiterStatus result = LoiterStatus::GOOD_TO_KEEP_LOITERING;
235 13618 : const float alt = _vario.alt;
236 :
237 13618 : if (_exit_commanded) {
238 0 : result = LoiterStatus::EXIT_COMMANDED;
239 13618 : } else if (alt > alt_max) {
240 1 : result = LoiterStatus::ALT_TOO_HIGH;
241 13617 : } else if (alt < alt_min) {
242 0 : result = LoiterStatus::ALT_TOO_LOW;
243 13617 : } else if ((AP_HAL::micros64() - _thermal_start_time_us) > ((unsigned)min_thermal_s * 1e6)) {
244 12367 : const float mcCreadyAlt = McCready(alt);
245 12367 : if (_thermalability < mcCreadyAlt) {
246 0 : result = LoiterStatus::THERMAL_WEAK;
247 12367 : } else if (alt < (-_thermal_start_pos.z) || _vario.get_filtered_climb() < 0.0) {
248 0 : result = LoiterStatus::ALT_LOST;
249 12367 : } else if (check_drift(prev_wp, next_wp)) {
250 0 : result = LoiterStatus::DRIFT_EXCEEDED;
251 : }
252 : }
253 :
254 13618 : return result;
255 : }
256 :
257 1 : void SoaringController::init_thermalling()
258 : {
259 : // Calc filter matrices - so that changes to parameters can be updated by switching in and out of thermal mode
260 1 : float r = powf(thermal_r, 2); // Measurement noise
261 1 : float cov_q1 = powf(thermal_q1, 2); // Process noise for strength
262 1 : float cov_q2 = powf(thermal_q2, 2); // Process noise for position and radius
263 :
264 : const float init_q[4] = {cov_q1,
265 : cov_q2,
266 : cov_q2,
267 1 : cov_q2};
268 :
269 1 : const MatrixN<float,4> q{init_q};
270 :
271 1 : const float init_p[4] = {INITIAL_STRENGTH_COVARIANCE,
272 : INITIAL_RADIUS_COVARIANCE,
273 : INITIAL_POSITION_COVARIANCE,
274 : INITIAL_POSITION_COVARIANCE};
275 :
276 1 : const MatrixN<float,4> p{init_p};
277 :
278 1 : Vector3f position;
279 :
280 1 : const AP_AHRS &_ahrs = AP::ahrs();
281 1 : if (!_ahrs.get_relative_position_NED_home(position)) {
282 0 : return;
283 : }
284 :
285 : // New state vector filter will be reset. Thermal location is placed in front of a/c
286 1 : const float init_xr[4] = {_vario.get_trigger_value(),
287 : INITIAL_THERMAL_RADIUS,
288 1 : position.x + thermal_distance_ahead * cosf(_ahrs.get_yaw_rad()),
289 2 : position.y + thermal_distance_ahead * sinf(_ahrs.get_yaw_rad())};
290 :
291 1 : const VectorN<float,4> xr{init_xr};
292 :
293 : // Also reset covariance matrix p so filter is not affected by previous data
294 1 : _ekf.reset(xr, p, q, r);
295 :
296 1 : _prev_update_time = AP_HAL::micros64();
297 1 : _thermal_start_time_us = AP_HAL::micros64();
298 1 : _thermal_start_pos = position;
299 :
300 1 : _vario.reset_climb_filter(0.0);
301 :
302 1 : _position_x_filter.reset(_ekf.X[2]);
303 1 : _position_y_filter.reset(_ekf.X[3]);
304 :
305 1 : _exit_commanded = false;
306 : }
307 :
308 266 : void SoaringController::init_cruising()
309 : {
310 266 : if (_last_update_status >= ActiveStatus::MANUAL_MODE_CHANGE) {
311 5 : _cruise_start_time_us = AP_HAL::micros64();
312 : // Start glide. Will be updated on the next loop.
313 5 : set_throttle_suppressed(true);
314 : }
315 266 : }
316 :
317 13618 : void SoaringController::update_thermalling()
318 : {
319 13618 : float deltaT = (AP_HAL::micros64() - _prev_update_time) * 1e-6;
320 :
321 13618 : Vector3f current_position;
322 :
323 13618 : const AP_AHRS &_ahrs = AP::ahrs();
324 13618 : if (!_ahrs.get_relative_position_NED_home(current_position)) {
325 0 : return;
326 : }
327 :
328 13618 : Vector3f wind;
329 : // use the estimate even if it is not marked valid, to preserve
330 : // existing behaviour
331 13618 : IGNORE_RETURN(_ahrs.get_wind(wind));
332 :
333 13618 : Vector3f wind_drift = wind*deltaT*_vario.get_filtered_climb()/_ekf.X[0];
334 :
335 : // update the filter
336 13618 : _ekf.update(_vario.reading, current_position.x, current_position.y, wind_drift.x, wind_drift.y);
337 :
338 :
339 13618 : _thermalability = (_ekf.X[0]*expf(-powf(get_thermalling_radius()/_ekf.X[1], 2))) - _vario.get_exp_thermalling_sink();
340 :
341 13618 : _prev_update_time = AP_HAL::micros64();
342 :
343 : // Compute smoothed estimate of position
344 13618 : _position_x_filter.set_cutoff_frequency(1/(3*_vario.tau));
345 13618 : _position_y_filter.set_cutoff_frequency(1/(3*_vario.tau));
346 :
347 13618 : _position_x_filter.apply(_ekf.X[2], deltaT);
348 13618 : _position_y_filter.apply(_ekf.X[3], deltaT);
349 :
350 : #if HAL_LOGGING_ENABLED
351 : // write log - save the data.
352 : // @LoggerMessage: SOAR
353 : // @Vehicles: Plane
354 : // @Description: Logged data from soaring feature
355 : // @URL: https://ardupilot.club/plane/docs/soaring.html
356 : // @Field: TimeUS: microseconds since system startup
357 : // @Field: nettorate: Estimate of vertical speed of surrounding airmass
358 : // @Field: x0: Thermal strength estimate
359 : // @Field: x1: Thermal radius estimate
360 : // @Field: x2: Thermal position estimate north from home
361 : // @Field: x3: Thermal position estimate east from home
362 : // @Field: north: Aircraft position north from home
363 : // @Field: east: Aircraft position east from home
364 : // @Field: alt: Aircraft altitude
365 : // @Field: dx_w: Wind speed north
366 : // @Field: dy_w: Wind speed east
367 : // @Field: th: Estimate of achievable climbrate in thermal
368 27236 : AP::logger().WriteStreaming("SOAR", "TimeUS,nettorate,x0,x1,x2,x3,north,east,alt,dx_w,dy_w,th", "Qfffffffffff",
369 : AP_HAL::micros64(),
370 13618 : (double)_vario.reading,
371 13618 : (double)_ekf.X[0],
372 13618 : (double)_ekf.X[1],
373 13618 : (double)_ekf.X[2],
374 13618 : (double)_ekf.X[3],
375 13618 : current_position.x,
376 13618 : current_position.y,
377 13618 : (double)_vario.alt,
378 13618 : (double)wind_drift.x,
379 13618 : (double)wind_drift.y,
380 13618 : (double)_thermalability);
381 : #if HAL_SOARING_NVF_EKF_ENABLED
382 : auto const now_ms = AP_HAL::millis();
383 : if (now_ms - _prev_nvf_pub_time_ms > NVF_PUBLISHER_DELAY_MS) {
384 : gcs().send_named_float("SOAREKFX0", (float)_ekf.X[0]);
385 : gcs().send_named_float("SOAREKFX1", (float)_ekf.X[1]);
386 : gcs().send_named_float("SOAREKFX2", (float)_ekf.X[2]);
387 : gcs().send_named_float("SOAREKFX3", (float)_ekf.X[3]);
388 : _prev_nvf_pub_time_ms = now_ms;
389 : }
390 :
391 : #endif // HAL_SOARING_NVF_EKF_ENABLED
392 : #endif
393 : }
394 :
395 34129 : void SoaringController::update_cruising()
396 : {
397 : // Calculate the optimal airspeed for the current conditions of wind along current direction,
398 : // expected lift in next thermal and filtered sink rate.
399 :
400 34129 : Vector3f wind;
401 : // use the estimate even if it is not marked valid, to preserve
402 : // existing behaviour
403 34129 : IGNORE_RETURN(AP::ahrs().get_wind(wind));
404 34129 : Vector3f wind_bf = AP::ahrs().earth_to_body(wind);
405 :
406 34129 : const float wx = wind_bf.x;
407 :
408 34129 : const float wz = _vario.get_stf_value();
409 :
410 : // Constraints on the airspeed calculation.
411 34129 : const float CLmin = _polarParams.K/(_aparm.airspeed_max*_aparm.airspeed_max);
412 34129 : const float CLmax = _polarParams.K/(_aparm.airspeed_min*_aparm.airspeed_min);
413 :
414 : // Update the calculation.
415 34129 : _speedToFly.update(wx, wz, thermal_vspeed, CLmin, CLmax);
416 :
417 : #if HAL_LOGGING_ENABLED
418 : // @LoggerMessage: SORC
419 : // @Vehicles: Plane
420 : // @Description: Soaring Cruise-phase data
421 : // @URL: https://ardupilot.club/plane/docs/soaring.html
422 : // @Field: TimeUS: Time since system startup
423 : // @Field: wx: body-frame wind estimate, x-axis
424 : // @Field: wz: body-frame wind estimate, z-axis
425 : // @Field: wexp: estimated thermal vertical speed
426 : // @Field: CLmin: expected climb-rate lower-limit
427 : // @Field: CLmax: expected climb-rate upper-limit
428 : // @Field: Vopt: calculated optimal speed to fly
429 68258 : AP::logger().WriteStreaming("SORC", "TimeUS,wx,wz,wexp,CLmin,CLmax,Vopt", "Qffffff",
430 : AP_HAL::micros64(),
431 : (double)wx,
432 : (double)wz,
433 : (double)thermal_vspeed,
434 : (double)CLmin,
435 : (double)CLmax,
436 34129 : (double)_speedToFly.speed_to_fly());
437 : #endif
438 34129 : }
439 :
440 68767 : void SoaringController::update_vario()
441 : {
442 68767 : _vario.update(thermal_bank);
443 68767 : }
444 :
445 :
446 12367 : float SoaringController::McCready(float alt)
447 : {
448 : // A method shell to be filled in later
449 12367 : return thermal_vspeed;
450 : }
451 :
452 2736341 : SoaringController::ActiveStatus SoaringController::active_state(bool override_disable) const
453 : {
454 2736341 : if (override_disable || !soar_active) {
455 2658379 : return ActiveStatus::SOARING_DISABLED;
456 : }
457 :
458 77962 : return _pilot_desired_state;
459 : }
460 :
461 2736341 : void SoaringController::update_active_state(bool override_disable)
462 : {
463 2736341 : ActiveStatus status = active_state(override_disable);
464 2736341 : bool state_changed = !(status == _last_update_status);
465 :
466 2736341 : if (state_changed) {
467 14 : switch (status) {
468 4 : case ActiveStatus::SOARING_DISABLED:
469 : // It's not enabled, but was enabled on the last loop.
470 4 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Soaring: Disabled.");
471 4 : set_throttle_suppressed(false);
472 4 : break;
473 8 : case ActiveStatus::MANUAL_MODE_CHANGE:
474 : // It's enabled, but wasn't on the last loop.
475 8 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Soaring: Enabled, manual mode changes.");
476 8 : break;
477 2 : case ActiveStatus::AUTO_MODE_CHANGE:
478 2 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Soaring: Enabled, automatic mode changes.");
479 2 : break;
480 : }
481 :
482 14 : if (_last_update_status == ActiveStatus::SOARING_DISABLED) {
483 : // We have switched from disabled into an active mode, start cruising.
484 9 : init_cruising();
485 5 : } else if (status != ActiveStatus::SOARING_DISABLED) {
486 : // We switched between active modes. If we're in THERMAL this means we should exit gracefully.
487 : // This has no effect if we're cruising as it is reset on thermal entry.
488 1 : _exit_commanded = true;
489 : }
490 : }
491 :
492 2736341 : _last_update_status = status;
493 2736341 : }
494 :
495 :
496 17049 : void SoaringController::set_throttle_suppressed(bool suppressed)
497 : {
498 17049 : _throttle_suppressed = suppressed;
499 :
500 : // Let the TECS know.
501 17049 : _tecs.set_gliding_requested_flag(suppressed);
502 17049 : }
503 :
504 12367 : bool SoaringController::check_drift(Vector2f prev_wp, Vector2f next_wp)
505 : {
506 : // Check for -1 (disabled)
507 12367 : if (max_drift<0) {
508 12367 : return false;
509 : }
510 :
511 : // Check against the estimated thermal.
512 0 : Vector2f position(_ekf.X[2], _ekf.X[3]);
513 :
514 0 : Vector2f start_pos(_thermal_start_pos.x, _thermal_start_pos.y);
515 :
516 0 : Vector2f mission_leg = next_wp - prev_wp;
517 :
518 0 : if (prev_wp.is_zero() || mission_leg.length() < 0.1) {
519 : // Simple check of distance from initial start point.
520 0 : return (position - start_pos).length() > max_drift;
521 : } else {
522 : // Regard the effective start point as projected onto mission leg.
523 : // Calculate drift parallel and perpendicular to mission leg.
524 : // Drift parallel and in direction of mission leg is acceptable.
525 0 : Vector2f effective_start, vec1, vec2;
526 :
527 : // Calculate effective start point (on mission leg).
528 0 : vec1 = (start_pos - prev_wp).projected(mission_leg);
529 0 : effective_start = prev_wp + vec1;
530 :
531 : // Calculate parallel and perpendicular offsets.
532 0 : vec2 = position - effective_start;
533 :
534 0 : float parallel = vec2 * mission_leg.normalized();
535 0 : float perpendicular = (vec2 - mission_leg.normalized()*parallel).length();
536 :
537 : // Check if we've drifted beyond the next wp.
538 0 : if (parallel>(next_wp - effective_start).length()) {
539 0 : return true;
540 : }
541 :
542 : // Check if we've drifted too far laterally or backwards. We don't count positive parallel offsets
543 : // as these are favourable (towards next wp)
544 0 : parallel = parallel>0 ? 0 : parallel;
545 :
546 0 : return (powf(parallel,2)+powf(perpendicular,2)) > powf(max_drift,2);
547 : }
548 : }
549 :
550 16342 : float SoaringController::get_thermalling_radius() const
551 : {
552 : // Thermalling radius is controlled by parameter SOAR_THML_BANK and true target airspeed.
553 16342 : const float target_aspd = _tecs.get_target_airspeed() * AP::ahrs().get_EAS2TAS();
554 16342 : const float radius = (target_aspd*target_aspd) / (GRAVITY_MSS * tanf(thermal_bank*DEG_TO_RAD));
555 :
556 16342 : return radius;
557 : }
558 :
559 2724 : float SoaringController::get_thermalling_target_airspeed()
560 : {
561 2724 : return soar_thermal_airspeed;
562 : }
563 :
564 5458 : float SoaringController::get_cruising_target_airspeed()
565 : {
566 5458 : if (soar_cruise_airspeed<0) {
567 623 : return _speedToFly.speed_to_fly();
568 : }
569 4835 : return soar_cruise_airspeed;
570 : }
571 :
572 : #endif // HAL_SOARING_ENABLED
|
