Line data Source code
1 : #include "mode.h"
2 : #include "Plane.h"
3 :
4 : #if HAL_QUADPLANE_ENABLED
5 :
6 19 : bool ModeQRTL::_enter()
7 : {
8 : // treat QRTL as QLAND if we are in guided wait takeoff state, to cope
9 : // with failsafes during GUIDED->AUTO takeoff sequence
10 19 : if (plane.quadplane.guided_wait_takeoff_on_mode_enter) {
11 0 : plane.set_mode(plane.mode_qland, ModeReason::QLAND_INSTEAD_OF_RTL);
12 0 : return true;
13 : }
14 19 : submode = SubMode::RTL;
15 19 : approach_start.valid = false;
16 19 : plane.prev_WP_loc = plane.current_loc;
17 :
18 19 : int32_t RTL_alt_abs_cm = plane.home.alt + quadplane.qrtl_alt_m*100UL;
19 19 : if (quadplane.motors->get_desired_spool_state() == AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED) {
20 : // VTOL motors are active, either in VTOL flight or assisted flight
21 10 : Location destination = plane.calc_best_rally_or_home_location(plane.current_loc, RTL_alt_abs_cm);
22 10 : const float dist = plane.current_loc.get_distance(destination);
23 10 : const float radius = get_VTOL_return_radius();
24 :
25 : // Climb at least to a cone around home of height of QRTL alt and radius of radius
26 : // Always climb up to at least Q_RTL_ALT_MIN, constrain Q_RTL_ALT_MIN between Q_LAND_FINAL_ALT and Q_RTL_ALT
27 10 : const float min_climb = constrain_float(quadplane.qrtl_alt_min_m, quadplane.land_final_alt_m, quadplane.qrtl_alt_m);
28 10 : const float target_alt = MAX(quadplane.qrtl_alt_m * (dist / MAX(radius, dist)), min_climb);
29 :
30 :
31 : #if AP_TERRAIN_AVAILABLE
32 10 : const bool use_terrain = plane.terrain_enabled_in_mode(mode_number());
33 : #else
34 : const bool use_terrain = false;
35 : #endif
36 :
37 10 : const float dist_to_climb = target_alt - plane.relative_ground_altitude(RangeFinderUse::CLIMB, use_terrain);
38 10 : if (is_positive(dist_to_climb)) {
39 : // climb before returning, only next waypoint altitude is used
40 0 : submode = SubMode::climb;
41 0 : plane.next_WP_loc = plane.current_loc;
42 : #if AP_TERRAIN_AVAILABLE
43 : int32_t curent_alt_terrain_cm;
44 0 : if (use_terrain && plane.current_loc.get_alt_cm(Location::AltFrame::ABOVE_TERRAIN, curent_alt_terrain_cm)) {
45 0 : plane.next_WP_loc.set_alt_cm(curent_alt_terrain_cm + dist_to_climb * 100UL, Location::AltFrame::ABOVE_TERRAIN);
46 0 : return true;
47 : }
48 : #endif
49 0 : plane.next_WP_loc.set_alt_cm(plane.current_loc.alt + dist_to_climb * 100UL, plane.current_loc.get_alt_frame());
50 0 : return true;
51 :
52 10 : } else if (dist < radius) {
53 : // Above home "cone", return at current altitude if lower than QRTL alt
54 : int32_t current_alt_abs_cm;
55 9 : if (plane.current_loc.get_alt_cm(Location::AltFrame::ABSOLUTE, current_alt_abs_cm)) {
56 9 : RTL_alt_abs_cm = MIN(RTL_alt_abs_cm, current_alt_abs_cm);
57 : }
58 :
59 : // we're close to destination and already running VTOL motors, don't transition and don't climb
60 9 : gcs().send_text(MAV_SEVERITY_INFO,"VTOL position1 d=%.1f r=%.1f", dist, radius);
61 9 : poscontrol.set_state(QuadPlane::QPOS_POSITION1);
62 : }
63 : }
64 :
65 : // use do_RTL() to setup next_WP_loc
66 19 : plane.do_RTL(RTL_alt_abs_cm);
67 19 : quadplane.poscontrol_init_approach();
68 :
69 : int32_t from_alt;
70 : int32_t to_alt;
71 19 : if (plane.current_loc.get_alt_cm(Location::AltFrame::ABSOLUTE,from_alt) && plane.next_WP_loc.get_alt_cm(Location::AltFrame::ABSOLUTE,to_alt)) {
72 19 : poscontrol.slow_descent = from_alt > to_alt;
73 19 : return true;
74 : }
75 : // default back to old method
76 0 : poscontrol.slow_descent = (plane.current_loc.alt > plane.next_WP_loc.alt);
77 0 : return true;
78 : }
79 :
80 373173 : void ModeQRTL::update()
81 : {
82 373173 : plane.mode_qstabilize.update();
83 373173 : }
84 :
85 : /*
86 : handle QRTL mode
87 : */
88 373173 : void ModeQRTL::run()
89 : {
90 373173 : const uint32_t now = AP_HAL::millis();
91 373173 : if (quadplane.tailsitter.in_vtol_transition(now)) {
92 : // Tailsitters in FW pull up phase of VTOL transition run FW controllers
93 0 : Mode::run();
94 0 : return;
95 : }
96 :
97 373173 : switch (submode) {
98 0 : case SubMode::climb: {
99 : // request zero velocity
100 0 : Vector2f vel, accel;
101 0 : pos_control->input_vel_accel_NE_m(vel, accel);
102 0 : quadplane.run_xy_controller();
103 :
104 : // nav roll and pitch are controller by position controller
105 0 : plane.nav_roll_cd = pos_control->get_roll_cd();
106 0 : plane.nav_pitch_cd = pos_control->get_pitch_cd();
107 :
108 0 : plane.quadplane.assign_tilt_to_fwd_thr();
109 :
110 0 : if (quadplane.transition->set_VTOL_roll_pitch_limit(plane.nav_roll_cd, plane.nav_pitch_cd)) {
111 0 : pos_control->NE_set_externally_limited();
112 : }
113 : // weathervane with no pilot input
114 0 : quadplane.disable_yaw_rate_time_constant();
115 0 : attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_cd(plane.nav_roll_cd,
116 0 : plane.nav_pitch_cd,
117 0 : quadplane.get_weathervane_yaw_rate_cds());
118 :
119 : // climb at full WP nav speed
120 0 : quadplane.set_climb_rate_ms(quadplane.wp_nav->get_default_speed_up_ms());
121 0 : quadplane.run_z_controller();
122 :
123 : // Climb done when stopping point reaches target altitude
124 0 : Vector3p stopping_point_ned_m;
125 0 : pos_control->get_stopping_point_D_m(stopping_point_ned_m.z);
126 0 : Location stopping_loc = Location::from_ekf_offset_NED_m(stopping_point_ned_m, Location::AltFrame::ABOVE_ORIGIN);
127 :
128 : ftype alt_diff;
129 0 : if (!stopping_loc.get_height_above(plane.next_WP_loc, alt_diff) || is_positive(alt_diff)) {
130 : // climb finished or cant get alt diff, head home
131 0 : submode = SubMode::RTL;
132 0 : plane.prev_WP_loc = plane.current_loc;
133 :
134 0 : int32_t RTL_alt_abs_cm = plane.home.alt + quadplane.qrtl_alt_m*100UL;
135 0 : Location destination = plane.calc_best_rally_or_home_location(plane.current_loc, RTL_alt_abs_cm);
136 0 : const float dist = plane.current_loc.get_distance(destination);
137 0 : const float radius = get_VTOL_return_radius();
138 0 : if (dist < radius) {
139 : // if close to home return at current target altitude
140 : int32_t target_alt_abs_cm;
141 0 : if (plane.next_WP_loc.get_alt_cm(Location::AltFrame::ABSOLUTE, target_alt_abs_cm)) {
142 0 : RTL_alt_abs_cm = MIN(RTL_alt_abs_cm, target_alt_abs_cm);
143 : }
144 0 : gcs().send_text(MAV_SEVERITY_INFO,"VTOL position1 d=%.1f r=%.1f", dist, radius);
145 0 : poscontrol.set_state(QuadPlane::QPOS_POSITION1);
146 : }
147 :
148 0 : plane.do_RTL(RTL_alt_abs_cm);
149 0 : quadplane.poscontrol_init_approach();
150 0 : if (plane.current_loc.get_height_above(plane.next_WP_loc, alt_diff)) {
151 0 : poscontrol.slow_descent = is_positive(alt_diff);
152 : } else {
153 : // default back to old method
154 0 : poscontrol.slow_descent = (plane.current_loc.alt > plane.next_WP_loc.alt);
155 : }
156 : }
157 0 : break;
158 : }
159 :
160 373173 : case SubMode::RTL: {
161 373173 : quadplane.vtol_position_controller();
162 373173 : if (poscontrol.get_state() > QuadPlane::QPOS_POSITION2) {
163 : // change target altitude to home alt
164 180846 : plane.next_WP_loc.copy_alt_from(plane.home);
165 : }
166 373173 : if (poscontrol.get_state() >= QuadPlane::QPOS_POSITION2) {
167 : // start landing logic
168 198598 : quadplane.verify_vtol_land();
169 : }
170 :
171 : // when in approach allow stick mixing
172 742128 : if (quadplane.poscontrol.get_state() == QuadPlane::QPOS_AIRBRAKE ||
173 368955 : quadplane.poscontrol.get_state() == QuadPlane::QPOS_APPROACH) {
174 109235 : plane.stabilize_stick_mixing_fbw();
175 : }
176 373173 : break;
177 : }
178 : }
179 :
180 : // Stabilize with fixed wing surfaces
181 373173 : plane.stabilize_roll();
182 373173 : plane.stabilize_pitch();
183 373173 : plane.stabilize_yaw();
184 : }
185 :
186 : /*
187 : return current height above the QRTL destination altitude, in the same
188 : frame (terrain-relative if QRTL is terrain following, else absolute) the
189 : approach altitude ramp in update_target_altitude() is applied in.
190 : Returns false, leaving alt_delta_m unchanged, if no usable altitude data
191 : is available -- callers must not treat that the same as a zero delta
192 : */
193 3119 : bool ModeQRTL::calc_alt_delta_m(float &alt_delta_m) const
194 : {
195 : #if AP_TERRAIN_AVAILABLE
196 3119 : if (plane.next_WP_loc.terrain_alt) {
197 : // QRTL is terrain following: compute the delta in the same
198 : // terrain-relative frame the ramp is applied in via
199 : // change_target_altitude(), so the two stay consistent over
200 : // sloping ground. next_WP_loc.alt is already the destination's
201 : // own terrain-relative target height, in cm. Don't fall back to
202 : // the absolute-altitude path below on failure -- that would mix
203 : // an AMSL delta into a terrain-relative ramp
204 : float current_terrain_height_m;
205 552 : if (!plane.terrain.height_above_terrain(current_terrain_height_m, true)) {
206 0 : return false;
207 : }
208 552 : alt_delta_m = current_terrain_height_m - (plane.next_WP_loc.alt * 0.01);
209 552 : return true;
210 : }
211 : #endif
212 : int32_t current_alt_cm;
213 : int32_t dest_alt_cm;
214 5134 : if (plane.current_loc.get_alt_cm(Location::AltFrame::ABSOLUTE, current_alt_cm) &&
215 2567 : plane.next_WP_loc.get_alt_cm(Location::AltFrame::ABSOLUTE, dest_alt_cm)) {
216 2567 : alt_delta_m = (current_alt_cm - dest_alt_cm) * 0.01;
217 2567 : return true;
218 : }
219 : // no usable altitudes
220 0 : return false;
221 : }
222 :
223 : /*
224 : update target altitude for QRTL profile
225 : */
226 12458 : void ModeQRTL::update_target_altitude()
227 : {
228 : /*
229 : update height target in approach. This continues through the airbrake,
230 : POSITION1 and POSITION2 stages -- the same window in which TECS keeps
231 : running (see QuadPlane::should_disable_TECS()) -- so that the altitude
232 : TECS is using is never stepped while an abrupt transition out of
233 : APPROACH/AIRBRAKE (e.g. thrust loss, low airspeed, bad attitude) can
234 : happen well before the ramp has reached RTL_ALTITUDE
235 : */
236 12458 : const QuadPlane::position_control_state qpos_state = plane.quadplane.poscontrol.get_state();
237 12458 : if ((submode != SubMode::RTL) ||
238 12458 : (qpos_state < QuadPlane::QPOS_APPROACH) || (qpos_state > QuadPlane::QPOS_POSITION2)) {
239 6052 : Mode::update_target_altitude();
240 6052 : return;
241 : }
242 :
243 : /*
244 : initially approach at RTL_ALT_CM, then drop down to QRTL_ALT based on maximum sink rate from TECS,
245 : giving time to lose speed before we transition
246 : */
247 6406 : const float radius = MAX(fabsf(float(plane.aparm.loiter_radius)), fabsf(float(plane.g.rtl_radius)));
248 6406 : const float rtl_alt_delta = MAX(0, plane.g.RTL_altitude - plane.quadplane.qrtl_alt_m);
249 6406 : const float sink_time = rtl_alt_delta / MAX(0.6*plane.TECS_controller.get_max_sinkrate(), 1);
250 6406 : const float sink_dist = plane.aparm.airspeed_cruise * sink_time;
251 6406 : const float dist = plane.auto_state.wp_distance;
252 6406 : const float rad_min = 2*radius;
253 6406 : const float rad_max = 20*radius;
254 6406 : const float dist_rtl_alt_reached = MAX(rad_min, MIN(rad_max, rad_min+sink_dist));
255 :
256 6406 : if (!approach_start.valid) {
257 : // latch where the approach began, so the ramp below is driven by our
258 : // own altitude at the start of the approach rather than the fixed wing
259 : // waypoint offset, which is cleared as the destination is neared.
260 : // If the altitude query fails, leave valid false so we retry next
261 : // cycle rather than latching a bogus zero delta that would silently
262 : // disable the ramp for the rest of the approach
263 16 : if (calc_alt_delta_m(approach_start.alt_delta_m)) {
264 16 : approach_start.dist_m = dist;
265 16 : approach_start.valid = true;
266 : }
267 : }
268 :
269 : // Set the target altitude to the QRTL altitude
270 6406 : plane.set_target_altitude_location(plane.next_WP_loc);
271 :
272 : float alt;
273 6406 : if (dist > dist_rtl_alt_reached) {
274 : /*
275 : still well short of home: instead of immediately targeting RTL_ALTITUDE,
276 : gradually descend from the altitude we were at when this approach leg
277 : started down to RTL_ALTITUDE, reaching it at dist_rtl_alt_reached.
278 : As for fixed wing waypoints, ALT_SLOPE_MIN gates this: setting it to
279 : zero disables the gradual descent, and altitude changes smaller than
280 : it are made immediately
281 : */
282 3353 : const float alt_excess = approach_start.valid ? (approach_start.alt_delta_m - rtl_alt_delta) : 0;
283 6456 : if (approach_start.valid && (plane.g.alt_slope_min > 0) && (alt_excess >= plane.g.alt_slope_min) &&
284 3103 : (approach_start.dist_m > dist_rtl_alt_reached)) {
285 3103 : alt = linear_interpolate(rtl_alt_delta, approach_start.alt_delta_m,
286 : dist,
287 : dist_rtl_alt_reached, approach_start.dist_m);
288 : // The ramp above is driven purely by the latched start point and
289 : // distance, so it doesn't know if we've since fallen below its
290 : // line, e.g. QRTL was entered while already sinking briskly. In
291 : // that case don't command a climb back up to the ramp -- follow
292 : // the aircraft down instead, floored at rtl_alt_delta so this
293 : // branch hands off to the dist_rtl_alt_reached boundary below at
294 : // the same altitude that boundary itself targets (still permits
295 : // climbing back to rtl_alt_delta if we're currently below it,
296 : // same as the pre-ramp behaviour of targeting RTL_ALTITUDE outright).
297 : // If this cycle's altitude query fails, skip the clamp rather than
298 : // folding the failure into a target of rtl_alt_delta
299 : float current_alt_delta_m;
300 3103 : if (calc_alt_delta_m(current_alt_delta_m)) {
301 3103 : alt = MIN(alt, MAX(rtl_alt_delta, current_alt_delta_m));
302 : }
303 : } else {
304 : // gradual descent disabled, nothing worth ramping down from, or
305 : // the altitude query has never yet succeeded this approach
306 250 : alt = rtl_alt_delta;
307 : }
308 : } else {
309 : // Close to home, descend from RTL alt to QRTL alt
310 3053 : alt = linear_interpolate(0.0, rtl_alt_delta,
311 : dist,
312 : rad_min, dist_rtl_alt_reached);
313 : }
314 :
315 : // Adjust target altitude based on distance to home
316 6406 : plane.change_target_altitude(alt * 100);
317 : }
318 :
319 : // only nudge during approach
320 12439 : bool ModeQRTL::allows_throttle_nudging() const
321 : {
322 12439 : return (submode == SubMode::RTL) && (plane.quadplane.poscontrol.get_state() == QuadPlane::QPOS_APPROACH);
323 : }
324 :
325 : // Return the radius from destination at which pure VTOL flight should be used, no transition to FW
326 10 : float ModeQRTL::get_VTOL_return_radius() const
327 : {
328 10 : return MAX(fabsf(float(plane.aparm.loiter_radius)), fabsf(float(plane.g.rtl_radius))) * 1.5;
329 : }
330 :
331 : #endif
|
