Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 :
16 : /*
17 : * AP_Landing_Deepstall.cpp - Landing logic handler for ArduPlane for deepstall landings
18 : */
19 :
20 : #include "AP_Landing_config.h"
21 :
22 : #if HAL_LANDING_DEEPSTALL_ENABLED
23 :
24 : #include "AP_Landing.h"
25 :
26 : #include <GCS_MAVLink/GCS.h>
27 : #include <AP_HAL/AP_HAL.h>
28 : #include <SRV_Channel/SRV_Channel.h>
29 : #include <AP_Common/Location.h>
30 : #include <AP_AHRS/AP_AHRS.h>
31 : #include <AP_Logger/AP_Logger.h>
32 :
33 : // table of user settable parameters for deepstall
34 : const AP_Param::GroupInfo AP_Landing_Deepstall::var_info[] = {
35 :
36 : // @Param: V_FWD
37 : // @DisplayName: Deepstall forward velocity
38 : // @Description: The forward velocity of the aircraft while stalled
39 : // @Range: 0 20
40 : // @Units: m/s
41 : // @User: Advanced
42 : AP_GROUPINFO("V_FWD", 1, AP_Landing_Deepstall, forward_speed, 1),
43 :
44 : // @Param: SLOPE_A
45 : // @DisplayName: Deepstall slope a
46 : // @Description: The a component of distance = a*wind + b
47 : // @User: Advanced
48 : AP_GROUPINFO("SLOPE_A", 2, AP_Landing_Deepstall, slope_a, 1),
49 :
50 : // @Param: SLOPE_B
51 : // @DisplayName: Deepstall slope b
52 : // @Description: The a component of distance = a*wind + b
53 : // @User: Advanced
54 : AP_GROUPINFO("SLOPE_B", 3, AP_Landing_Deepstall, slope_b, 1),
55 :
56 : // @Param: APP_EXT
57 : // @DisplayName: Deepstall approach extension
58 : // @Description: The horizontal distance from which the aircraft will approach before the stall
59 : // @Range: 10 200
60 : // @Units: m
61 : // @User: Advanced
62 : AP_GROUPINFO("APP_EXT", 4, AP_Landing_Deepstall, approach_extension, 50),
63 :
64 : // @Param: V_DWN
65 : // @DisplayName: Deepstall velocity down
66 : // @Description: The downward velocity of the aircraft while stalled
67 : // @Range: 0 20
68 : // @Units: m/s
69 : // @User: Advanced
70 : AP_GROUPINFO("V_DWN", 5, AP_Landing_Deepstall, down_speed, 2),
71 :
72 : // @Param: SLEW_SPD
73 : // @DisplayName: Deepstall slew speed
74 : // @Description: The speed at which the elevator slews to deepstall
75 : // @Range: 0 2
76 : // @Units: s
77 : // @User: Advanced
78 : AP_GROUPINFO("SLEW_SPD", 6, AP_Landing_Deepstall, slew_speed, 0.5),
79 :
80 : // @Param: ELEV_PWM
81 : // @DisplayName: Deepstall elevator PWM
82 : // @Description: The PWM value in microseconds for the elevator at full deflection in deepstall
83 : // @Range: 900 2100
84 : // @Units: PWM
85 : // @User: Advanced
86 : AP_GROUPINFO("ELEV_PWM", 7, AP_Landing_Deepstall, elevator_pwm, 1500),
87 :
88 : // @Param: ARSP_MAX
89 : // @DisplayName: Deepstall enabled airspeed
90 : // @Description: The maximum aispeed where the deepstall steering controller is allowed to have control
91 : // @Range: 5 20
92 : // @Units: m/s
93 : // @User: Advanced
94 : AP_GROUPINFO("ARSP_MAX", 8, AP_Landing_Deepstall, handoff_airspeed, 15.0),
95 :
96 : // @Param: ARSP_MIN
97 : // @DisplayName: Deepstall minimum derating airspeed
98 : // @Description: Deepstall lowest airspeed where the deepstall controller isn't allowed full control
99 : // @Range: 5 20
100 : // @Units: m/s
101 : // @User: Advanced
102 : AP_GROUPINFO("ARSP_MIN", 9, AP_Landing_Deepstall, handoff_lower_limit_airspeed, 10.0),
103 :
104 : // @Param: L1
105 : // @DisplayName: Deepstall L1 period
106 : // @Description: Deepstall L1 navigational controller period
107 : // @Range: 5 50
108 : // @Units: s
109 : // @User: Advanced
110 : AP_GROUPINFO("L1", 10, AP_Landing_Deepstall, L1_period, 30.0),
111 :
112 : // @Param: L1_I
113 : // @DisplayName: Deepstall L1 I gain
114 : // @Description: Deepstall L1 integratior gain
115 : // @Range: 0 1
116 : // @User: Advanced
117 : AP_GROUPINFO("L1_I", 11, AP_Landing_Deepstall, L1_i, 0),
118 :
119 : // @Param: YAW_LIM
120 : // @DisplayName: Deepstall yaw rate limit
121 : // @Description: The yaw rate limit while navigating in deepstall
122 : // @Range: 0 90
123 : // @Units: deg/s
124 : // @User: Advanced
125 : AP_GROUPINFO("YAW_LIM", 12, AP_Landing_Deepstall, yaw_rate_limit, 10),
126 :
127 : // @Param: L1_TCON
128 : // @DisplayName: Deepstall L1 time constant
129 : // @Description: Time constant for deepstall L1 control
130 : // @Range: 0 1
131 : // @Units: s
132 : // @User: Advanced
133 : AP_GROUPINFO("L1_TCON", 13, AP_Landing_Deepstall, time_constant, 0.4),
134 :
135 : // @Param: P
136 : // @DisplayName: P gain
137 : // @Description: P gain
138 : // @User: Standard
139 :
140 : // @Param: I
141 : // @DisplayName: I gain
142 : // @Description: I gain
143 : // @User: Standard
144 :
145 : // @Param: D
146 : // @DisplayName: D gain
147 : // @Description: D gain
148 : // @User: Standard
149 :
150 : // @Param: IMAX
151 : // @DisplayName: IMax
152 : // @Description: Maximum integrator value
153 : // @User: Standard
154 : AP_SUBGROUPINFO(ds_PID, "", 14, AP_Landing_Deepstall, PID),
155 :
156 : // @Param: ABORTALT
157 : // @DisplayName: Deepstall minimum abort altitude
158 : // @Description: The minimum altitude which the aircraft must be above to abort a deepstall landing
159 : // @Range: 0 50
160 : // @Units: m
161 : // @User: Advanced
162 : AP_GROUPINFO("ABORTALT", 15, AP_Landing_Deepstall, min_abort_alt, 0.0f),
163 :
164 : // @Param: AIL_SCL
165 : // @DisplayName: Aileron landing gain scalaing
166 : // @Description: A scalar to reduce or increase the aileron control
167 : // @Range: 0 2.0
168 : // @User: Advanced
169 : AP_GROUPINFO("AIL_SCL", 16, AP_Landing_Deepstall, aileron_scalar, 1.0f),
170 :
171 : AP_GROUPEND
172 : };
173 :
174 :
175 : // if DEBUG_PRINTS is defined statustexts will be sent to the GCS for debug purposes
176 : // #define DEBUG_PRINTS
177 :
178 1 : void AP_Landing_Deepstall::do_land(const AP_Mission::Mission_Command& cmd, const float relative_altitude)
179 : {
180 1 : stage = DEEPSTALL_STAGE_FLY_TO_LANDING;
181 1 : ds_PID.reset();
182 1 : L1_xtrack_i = 0.0f;
183 1 : hold_level = false; // come out of yaw lock
184 :
185 : // load the landing point in, the rest of path building is deferred for a better wind estimate
186 1 : memcpy(&landing_point, &cmd.content.location, sizeof(Location));
187 :
188 1 : if (!landing_point.relative_alt && !landing_point.terrain_alt) {
189 0 : approach_alt_offset = cmd.p1;
190 0 : landing_point.offset_up_m(approach_alt_offset);
191 : } else {
192 1 : approach_alt_offset = 0.0f;
193 : }
194 1 : }
195 :
196 : // currently identical to the slope aborts
197 0 : void AP_Landing_Deepstall::verify_abort_landing(const Location &prev_WP_loc, Location &next_WP_loc, bool &throttle_suppressed)
198 : {
199 : // when aborting a landing, mimic the verify_takeoff with steering hold. Once
200 : // the altitude has been reached, restart the landing sequence
201 0 : throttle_suppressed = false;
202 0 : landing.nav_controller->update_heading_hold(prev_WP_loc.get_bearing_to(next_WP_loc));
203 0 : }
204 :
205 :
206 : /*
207 : update navigation for landing
208 : */
209 895 : bool AP_Landing_Deepstall::verify_land(const Location &prev_WP_loc, Location &next_WP_loc, const Location ¤t_loc,
210 : const float height, const float sink_rate, const float wp_proportion, const uint32_t last_flying_ms,
211 : const bool is_armed, const bool is_flying, const bool rangefinder_state_in_range)
212 : {
213 895 : switch (stage) {
214 37 : case DEEPSTALL_STAGE_FLY_TO_LANDING:
215 37 : if (current_loc.get_distance(landing_point) > abs(2 * landing.aparm.loiter_radius)) {
216 36 : landing.nav_controller->update_waypoint(current_loc, landing_point);
217 36 : return false;
218 : }
219 1 : stage = DEEPSTALL_STAGE_ESTIMATE_WIND;
220 1 : loiter_sum_cd = 0; // reset the loiter counter
221 : FALLTHROUGH;
222 295 : case DEEPSTALL_STAGE_ESTIMATE_WIND:
223 : {
224 295 : landing.nav_controller->update_loiter(landing_point, landing.aparm.loiter_radius, landing_point.loiter_ccw ? -1 : 1);
225 295 : if (!landing.nav_controller->reached_loiter_target() || (fabsf(height - approach_alt_offset) > DEEPSTALL_LOITER_ALT_TOLERANCE)) {
226 : // wait until the altitude is correct before considering a breakout
227 40 : return false;
228 : }
229 : // only count loiter progress when within the target altitude
230 255 : int32_t target_bearing = landing.nav_controller->target_bearing_cd();
231 255 : int32_t delta = wrap_180_cd(target_bearing - last_target_bearing);
232 255 : delta *= (landing_point.loiter_ccw ? -1 : 1);
233 255 : if (delta > 0) { // only accumulate turns in the correct direction
234 254 : loiter_sum_cd += delta;
235 : }
236 255 : last_target_bearing = target_bearing;
237 255 : if (loiter_sum_cd < 36000) {
238 : // wait until we've done at least one complete loiter at the correct altitude
239 254 : return false;
240 : }
241 1 : stage = DEEPSTALL_STAGE_WAIT_FOR_BREAKOUT;
242 1 : loiter_sum_cd = 0; // reset the loiter counter
243 : FALLTHROUGH;
244 : }
245 171 : case DEEPSTALL_STAGE_WAIT_FOR_BREAKOUT:
246 : // rebuild the approach path if we have done less then a full circle to allow it to be
247 : // more into the wind as the estimator continues to refine itself, and allow better
248 : // compensation on windy days. This is limited to a single full circle though, as on
249 : // a no wind day you could be in this loop forever otherwise.
250 171 : if (loiter_sum_cd < 36000) {
251 171 : build_approach_path(false);
252 : }
253 171 : if (!verify_breakout(current_loc, arc_entry, height - approach_alt_offset)) {
254 170 : int32_t target_bearing = landing.nav_controller->target_bearing_cd();
255 170 : int32_t delta = wrap_180_cd(target_bearing - last_target_bearing);
256 170 : if (delta > 0) { // only accumulate turns in the correct direction
257 168 : loiter_sum_cd += delta;
258 : }
259 170 : last_target_bearing = target_bearing;
260 170 : landing.nav_controller->update_loiter(landing_point, landing.aparm.loiter_radius, landing_point.loiter_ccw ? -1 : 1);
261 170 : return false;
262 : }
263 1 : stage = DEEPSTALL_STAGE_FLY_TO_ARC;
264 1 : memcpy(&breakout_location, ¤t_loc, sizeof(Location));
265 : FALLTHROUGH;
266 10 : case DEEPSTALL_STAGE_FLY_TO_ARC:
267 10 : if (current_loc.get_distance(arc_entry) > 2 * landing.aparm.loiter_radius) {
268 9 : landing.nav_controller->update_waypoint(breakout_location, arc_entry);
269 9 : return false;
270 : }
271 1 : stage = DEEPSTALL_STAGE_ARC;
272 : FALLTHROUGH;
273 198 : case DEEPSTALL_STAGE_ARC:
274 : {
275 198 : Vector2f groundspeed = landing.ahrs.groundspeed_vector();
276 395 : if (!landing.nav_controller->reached_loiter_target() ||
277 197 : (fabsf(wrap_180(target_heading_deg -
278 197 : degrees(atan2f(-groundspeed.y, -groundspeed.x) + M_PI))) >= 10.0f)) {
279 197 : landing.nav_controller->update_loiter(arc, landing.aparm.loiter_radius, landing_point.loiter_ccw ? -1 : 1);
280 197 : return false;
281 : }
282 1 : stage = DEEPSTALL_STAGE_APPROACH;
283 : }
284 : FALLTHROUGH;
285 : case DEEPSTALL_STAGE_APPROACH:
286 : {
287 32 : Location entry_point;
288 32 : landing.nav_controller->update_waypoint(arc_exit, extended_approach);
289 :
290 : float height_above_target;
291 32 : if (is_zero(approach_alt_offset)) {
292 32 : landing.ahrs.get_relative_position_D_home(height_above_target);
293 32 : height_above_target = -height_above_target;
294 : } else {
295 0 : Location position;
296 0 : if (landing.ahrs.get_location(position)) {
297 0 : height_above_target = (position.alt - landing_point.alt + approach_alt_offset * 100) * 1e-2f;
298 : } else {
299 0 : height_above_target = approach_alt_offset;
300 : }
301 : }
302 :
303 32 : Vector3f wind;
304 : // use the estimate even if it is not marked valid, to preserve
305 : // existing behaviour
306 32 : IGNORE_RETURN(landing.ahrs.get_wind(wind));
307 :
308 32 : const float travel_distance = predict_travel_distance(wind, height_above_target, false);
309 :
310 32 : memcpy(&entry_point, &landing_point, sizeof(Location));
311 32 : entry_point.offset_bearing(target_heading_deg + 180.0, travel_distance);
312 :
313 32 : if (!current_loc.past_interval_finish_line(arc_exit, entry_point)) {
314 31 : if (current_loc.past_interval_finish_line(arc_exit, extended_approach)) {
315 : // this should never happen, but prevent against an indefinite fly away
316 0 : stage = DEEPSTALL_STAGE_FLY_TO_LANDING;
317 : }
318 31 : return false;
319 : }
320 1 : predict_travel_distance(wind, height_above_target, true);
321 1 : stage = DEEPSTALL_STAGE_LAND;
322 1 : stall_entry_time = AP_HAL::millis();
323 :
324 1 : const SRV_Channel* elevator = SRV_Channels::get_channel_for(SRV_Channel::k_elevator);
325 1 : if (elevator != nullptr) {
326 : // take the last used elevator angle as the starting deflection
327 : // don't worry about bailing here if the elevator channel can't be found
328 : // that will be handled within override_servos
329 1 : initial_elevator_pwm = elevator->get_output_pwm();
330 : }
331 : }
332 : FALLTHROUGH;
333 : case DEEPSTALL_STAGE_LAND:
334 : // while in deepstall the only thing verify needs to keep the extended approach point sufficently far away
335 158 : landing.nav_controller->update_waypoint(current_loc, extended_approach);
336 158 : landing.disarm_if_autoland_complete_fn();
337 158 : return false;
338 0 : default:
339 0 : return true;
340 : }
341 : }
342 :
343 4470 : bool AP_Landing_Deepstall::override_servos(void)
344 : {
345 4470 : if (stage != DEEPSTALL_STAGE_LAND) {
346 3690 : return false;
347 : }
348 :
349 780 : SRV_Channel* elevator = SRV_Channels::get_channel_for(SRV_Channel::k_elevator);
350 :
351 780 : if (elevator == nullptr) {
352 : // deepstalls are impossible without these channels, abort the process
353 0 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Deepstall: Unable to find the elevator channels");
354 0 : request_go_around();
355 0 : return false;
356 : }
357 :
358 : // calculate the progress on slewing the elevator
359 780 : float slew_progress = 1.0f;
360 780 : if (slew_speed > 0) {
361 780 : slew_progress = (AP_HAL::millis() - stall_entry_time) / (100.0f * slew_speed);
362 : }
363 :
364 : // mix the elevator to the correct value
365 780 : elevator->set_output_pwm(linear_interpolate(initial_elevator_pwm, elevator_pwm,
366 : slew_progress, 0.0f, 1.0f));
367 :
368 : // use the current airspeed to dictate the travel limits
369 : float airspeed;
370 780 : if (!landing.ahrs.airspeed_EAS(airspeed)) {
371 0 : airspeed = 0; // safely forces control to the deepstall steering since we don't have an estimate
372 : }
373 :
374 :
375 : // only allow the deepstall steering controller to run below the handoff airspeed
376 780 : if (slew_progress >= 1.0f || airspeed <= handoff_airspeed) {
377 : // run the steering conntroller
378 778 : float pid = update_steering();
379 :
380 :
381 778 : float travel_limit = constrain_float((handoff_airspeed - airspeed) /
382 : (handoff_airspeed - handoff_lower_limit_airspeed) *
383 : 0.5f + 0.5f,
384 : 0.5f, 1.0f);
385 :
386 778 : float output = constrain_float(pid, -travel_limit, travel_limit);
387 778 : SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, output*4500*aileron_scalar);
388 778 : SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, output*4500);
389 778 : SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0); // this will normally be managed as part of landing,
390 : // but termination needs to set throttle control here
391 : }
392 :
393 : // hand off rudder control to deepstall controlled
394 780 : return true;
395 : }
396 :
397 0 : bool AP_Landing_Deepstall::request_go_around(void)
398 : {
399 : float current_altitude_d;
400 0 : landing.ahrs.get_relative_position_D_home(current_altitude_d);
401 :
402 0 : if (is_zero(min_abort_alt) || -current_altitude_d > min_abort_alt) {
403 0 : landing.flags.commanded_go_around = true;
404 0 : return true;
405 : } else {
406 0 : return false;
407 : }
408 : }
409 :
410 8940 : bool AP_Landing_Deepstall::is_throttle_suppressed(void) const
411 : {
412 8940 : return stage == DEEPSTALL_STAGE_LAND;
413 : }
414 :
415 4470 : bool AP_Landing_Deepstall::is_flying_forward(void) const
416 : {
417 4470 : return stage != DEEPSTALL_STAGE_LAND;
418 : }
419 :
420 4812 : bool AP_Landing_Deepstall::is_on_approach(void) const
421 : {
422 4812 : return stage == DEEPSTALL_STAGE_LAND;
423 : }
424 :
425 739 : bool AP_Landing_Deepstall::get_target_altitude_location(Location &location)
426 : {
427 739 : memcpy(&location, &landing_point, sizeof(Location));
428 739 : return true;
429 : }
430 :
431 895 : float AP_Landing_Deepstall::get_target_airspeed_ms(void) const
432 : {
433 895 : if (stage == DEEPSTALL_STAGE_APPROACH ||
434 864 : stage == DEEPSTALL_STAGE_LAND) {
435 188 : return landing.pre_flare_airspeed;
436 : }
437 :
438 707 : return landing.aparm.airspeed_cruise;
439 : }
440 :
441 894 : bool AP_Landing_Deepstall::send_deepstall_message(mavlink_channel_t chan) const
442 : {
443 894 : CHECK_PAYLOAD_SIZE2(DEEPSTALL);
444 3112 : mavlink_msg_deepstall_send(
445 : chan,
446 894 : landing_point.lat,
447 894 : landing_point.lng,
448 894 : stage >= DEEPSTALL_STAGE_WAIT_FOR_BREAKOUT ? arc_exit.lat : 0.0f,
449 894 : stage >= DEEPSTALL_STAGE_WAIT_FOR_BREAKOUT ? arc_exit.lng : 0.0f,
450 894 : stage >= DEEPSTALL_STAGE_WAIT_FOR_BREAKOUT ? arc_entry.lat : 0.0f,
451 563 : stage >= DEEPSTALL_STAGE_WAIT_FOR_BREAKOUT ? arc_entry.lng : 0.0f,
452 894 : landing_point.alt * 0.01,
453 894 : stage >= DEEPSTALL_STAGE_WAIT_FOR_BREAKOUT ? predicted_travel_distance : 0.0f,
454 894 : stage == DEEPSTALL_STAGE_LAND ? crosstrack_error : 0.0f,
455 894 : stage);
456 894 : return true;
457 : }
458 :
459 0 : const AP_PIDInfo& AP_Landing_Deepstall::get_pid_info(void) const
460 : {
461 0 : return ds_PID.get_pid_info();
462 : }
463 :
464 : #if HAL_LOGGING_ENABLED
465 905 : void AP_Landing_Deepstall::Log(void) const {
466 905 : const AP_PIDInfo& pid_info = ds_PID.get_pid_info();
467 905 : struct log_DSTL pkt = {
468 : LOG_PACKET_HEADER_INIT(LOG_DSTL_MSG),
469 905 : time_us : AP_HAL::micros64(),
470 905 : stage : (uint8_t)stage,
471 905 : target_heading : target_heading_deg,
472 905 : target_lat : landing_point.lat,
473 905 : target_lng : landing_point.lng,
474 905 : target_alt : landing_point.alt,
475 905 : crosstrack_error : (int16_t)(stage >= DEEPSTALL_STAGE_LAND ?
476 160 : constrain_float(crosstrack_error * 1e2f, (float)INT16_MIN, (float)INT16_MAX) : 0),
477 905 : travel_distance : (int16_t)(stage >= DEEPSTALL_STAGE_LAND ?
478 160 : constrain_float(predicted_travel_distance * 1e2f, (float)INT16_MIN, (float)INT16_MAX) : 0),
479 905 : l1_i : stage >= DEEPSTALL_STAGE_LAND ? L1_xtrack_i : 0.0f,
480 905 : loiter_sum_cd : stage >= DEEPSTALL_STAGE_ESTIMATE_WIND ? loiter_sum_cd : 0,
481 905 : desired : pid_info.target,
482 905 : P : pid_info.P,
483 905 : I : pid_info.I,
484 905 : D : pid_info.D,
485 3620 : };
486 905 : AP::logger().WriteBlock(&pkt, sizeof(pkt));
487 905 : }
488 : #endif
489 :
490 : // termination handling, expected to set the servo outputs
491 0 : bool AP_Landing_Deepstall::terminate(void) {
492 : // if we were not in a deepstall, mark us as being in one
493 0 : if(!landing.flags.in_progress || stage != DEEPSTALL_STAGE_LAND) {
494 0 : stall_entry_time = AP_HAL::millis();
495 0 : ds_PID.reset();
496 0 : L1_xtrack_i = 0.0f;
497 0 : landing.flags.in_progress = true;
498 0 : stage = DEEPSTALL_STAGE_LAND;
499 :
500 0 : if(landing.ahrs.get_location(landing_point)) {
501 0 : build_approach_path(true);
502 : } else {
503 0 : hold_level = true;
504 : }
505 : }
506 :
507 : // set the servo ouptuts, this can fail, so this is the important return value for the AFS
508 0 : return override_servos();
509 : }
510 :
511 171 : void AP_Landing_Deepstall::build_approach_path(bool use_current_heading)
512 : {
513 171 : float loiter_radius = landing.nav_controller->loiter_radius(landing.aparm.loiter_radius);
514 :
515 171 : Vector3f wind;
516 : // use the estimate even if it is not marked valid, to preserve
517 : // existing behaviour
518 171 : IGNORE_RETURN(landing.ahrs.get_wind(wind));
519 : // TODO: Support a user defined approach heading
520 171 : target_heading_deg = use_current_heading ? landing.ahrs.get_yaw_deg() : (degrees(atan2f(-wind.y, -wind.x)));
521 :
522 171 : memcpy(&extended_approach, &landing_point, sizeof(Location));
523 171 : memcpy(&arc_exit, &landing_point, sizeof(Location));
524 :
525 : //extend the approach point to 1km away so that there is always a navigational target
526 171 : extended_approach.offset_bearing(target_heading_deg, 1000.0);
527 :
528 171 : float expected_travel_distance = predict_travel_distance(wind, is_zero(approach_alt_offset) ? landing_point.alt * 0.01f : approach_alt_offset,
529 : false);
530 171 : float approach_extension_m = expected_travel_distance + approach_extension;
531 171 : float loiter_radius_m_abs = fabsf(loiter_radius);
532 : // an approach extensions must be at least half the loiter radius, or the aircraft has a
533 : // decent chance to be misaligned on final approach
534 171 : approach_extension_m = MAX(approach_extension_m, loiter_radius_m_abs * 0.5f);
535 :
536 171 : arc_exit.offset_bearing(target_heading_deg + 180, approach_extension_m);
537 171 : memcpy(&arc, &arc_exit, sizeof(Location));
538 171 : memcpy(&arc_entry, &arc_exit, sizeof(Location));
539 :
540 171 : float arc_heading_deg = target_heading_deg + (landing_point.loiter_ccw ? -90.0f : 90.0f);
541 171 : arc.offset_bearing(arc_heading_deg, loiter_radius_m_abs);
542 171 : arc_entry.offset_bearing(arc_heading_deg, loiter_radius_m_abs * 2);
543 :
544 : #ifdef DEBUG_PRINTS
545 : // TODO: Send this information via a MAVLink packet
546 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Arc: %3.8f %3.8f",
547 : (double)(arc.lat / 1e7),(double)( arc.lng / 1e7));
548 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Loiter en: %3.8f %3.8f",
549 : (double)(arc_entry.lat / 1e7), (double)(arc_entry.lng / 1e7));
550 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Loiter ex: %3.8f %3.8f",
551 : (double)(arc_exit.lat / 1e7), (double)(arc_exit.lng / 1e7));
552 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Extended: %3.8f %3.8f",
553 : (double)(extended_approach.lat / 1e7), (double)(extended_approach.lng / 1e7));
554 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Extended by: %f (%f)", (double)approach_extension_m,
555 : (double)expected_travel_distance);
556 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Target Heading: %3.1f", (double)target_heading_deg);
557 : #endif // DEBUG_PRINTS
558 :
559 171 : }
560 :
561 204 : float AP_Landing_Deepstall::predict_travel_distance(const Vector3f wind, const float height, const bool print)
562 : {
563 204 : bool reverse = false;
564 :
565 204 : float course = radians(target_heading_deg);
566 :
567 : // a forward speed of 0 will result in a divide by 0
568 204 : float forward_speed_ms = MAX(forward_speed, 0.1f);
569 :
570 204 : Vector2f wind_vec(wind.x, wind.y); // work with the 2D component of wind
571 204 : float wind_length = MAX(wind_vec.length(), 0.05f); // always assume a slight wind to avoid divide by 0
572 204 : Vector2f course_vec(cosf(course), sinf(course));
573 :
574 204 : float offset = course - atan2f(-wind.y, -wind.x);
575 :
576 : // estimator for how far the aircraft will travel while entering the stall
577 204 : float stall_distance = slope_a * wind_length * cosf(offset) + slope_b;
578 :
579 204 : float theta = acosf(constrain_float((wind_vec * course_vec) / wind_length, -1.0f, 1.0f));
580 204 : if ((course_vec % wind_vec) > 0) {
581 132 : reverse = true;
582 132 : theta *= -1;
583 : }
584 :
585 204 : float cross_component = sinf(theta) * wind_length;
586 204 : float estimated_crab_angle = asinf(constrain_float(cross_component / forward_speed_ms, -1.0f, 1.0f));
587 204 : if (reverse) {
588 132 : estimated_crab_angle *= -1;
589 : }
590 :
591 204 : float estimated_forward = cosf(estimated_crab_angle) * forward_speed_ms + cosf(theta) * wind_length;
592 :
593 204 : if (is_positive(down_speed)) {
594 204 : predicted_travel_distance = (estimated_forward * height / down_speed) + stall_distance;
595 : } else {
596 : // if we don't have a sane downward speed in a deepstall (IE not zero, and not
597 : // an ascent) then just provide the stall_distance as a reasonable approximation
598 0 : predicted_travel_distance = stall_distance;
599 : }
600 :
601 204 : if(print) {
602 : // allow printing the travel distances on the final entry as its used for tuning
603 1 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Deepstall: Entry: %0.1f (m) Travel: %0.1f (m)",
604 : (double)stall_distance, (double)predicted_travel_distance);
605 : }
606 :
607 204 : return predicted_travel_distance;
608 : }
609 :
610 171 : bool AP_Landing_Deepstall::verify_breakout(const Location ¤t_loc, const Location &target_loc,
611 : const float height_error) const
612 : {
613 171 : const Vector2f location_delta = current_loc.get_distance_NE(target_loc);
614 171 : const float heading_error = degrees(landing.ahrs.groundspeed_vector().angle(location_delta));
615 :
616 : // Check to see if the plane is heading toward the land waypoint. We use 20 degrees (+/-10 deg)
617 : // of margin so that the altitude to be within 5 meters of desired
618 :
619 171 : if (heading_error <= 10.0 && fabsf(height_error) < DEEPSTALL_LOITER_ALT_TOLERANCE) {
620 : // Want to head in a straight line from _here_ to the next waypoint instead of center of loiter wp
621 1 : return true;
622 : }
623 170 : return false;
624 : }
625 :
626 778 : float AP_Landing_Deepstall::update_steering()
627 : {
628 778 : Location current_loc;
629 778 : if ((!landing.ahrs.get_location(current_loc) || !landing.ahrs.healthy()) && !hold_level) {
630 : // panic if no position source is available
631 : // continue the stall but target just holding the wings held level as deepstall should be a minimal
632 : // energy configuration on the aircraft, and if a position isn't available aborting would be worse
633 0 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Deepstall: Invalid data from AHRS. Holding level");
634 0 : hold_level = true;
635 : }
636 :
637 778 : float desired_change = 0.0f;
638 :
639 778 : if (!hold_level) {
640 778 : uint32_t time = AP_HAL::millis();
641 778 : float dt = constrain_float(time - last_time, (uint32_t)10UL, (uint32_t)200UL) * 1e-3;
642 778 : last_time = time;
643 :
644 778 : Vector2f ab = arc_exit.get_distance_NE(extended_approach);
645 778 : ab.normalize();
646 778 : const Vector2f a_air = arc_exit.get_distance_NE(current_loc);
647 :
648 778 : crosstrack_error = a_air % ab;
649 778 : float sine_nu1 = constrain_float(crosstrack_error / MAX(L1_period, 0.1f), -0.7071f, 0.7107f);
650 778 : float nu1 = asinf(sine_nu1);
651 :
652 778 : if (L1_i > 0) {
653 0 : L1_xtrack_i += nu1 * L1_i / dt;
654 0 : L1_xtrack_i = constrain_float(L1_xtrack_i, -0.5f, 0.5f);
655 0 : nu1 += L1_xtrack_i;
656 : }
657 778 : desired_change = wrap_PI(radians(target_heading_deg) + nu1 - landing.ahrs.get_yaw_rad()) / time_constant;
658 : }
659 :
660 778 : float yaw_rate = landing.ahrs.get_gyro().z;
661 778 : float yaw_rate_limit_rps = radians(yaw_rate_limit);
662 778 : float error = wrap_PI(constrain_float(desired_change, -yaw_rate_limit_rps, yaw_rate_limit_rps) - yaw_rate);
663 :
664 : #ifdef DEBUG_PRINTS
665 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "x: %f e: %f r: %f d: %f",
666 : (double)crosstrack_error,
667 : (double)error,
668 : (double)degrees(yaw_rate),
669 : (double)current_loc.get_distance(landing_point));
670 : #endif // DEBUG_PRINTS
671 :
672 778 : return ds_PID.get_pid(error);
673 : }
674 :
675 : #endif // HAL_LANDING_DEEPSTALL_ENABLED
|
