Line data Source code
1 : #include "mode.h"
2 : #include "Plane.h"
3 : #include <GCS_MAVLink/GCS.h>
4 :
5 : #if MODE_AUTOLAND_ENABLED
6 :
7 : // This is added to the target altitude to make sure the true target is exceeded.
8 : // This should be larger than the expected steady state error.
9 : constexpr float fast_climb_extra_alt = 10;
10 :
11 : /*
12 : mode AutoLand parameters
13 : */
14 : const AP_Param::GroupInfo ModeAutoLand::var_info[] = {
15 : // @Param: WP_ALT
16 : // @DisplayName: Final approach WP altitude
17 : // @Description: This is the target altitude above HOME for final approach waypoint
18 : // @Range: 0 200
19 : // @Increment: 1
20 : // @Units: m
21 : // @User: Standard
22 : AP_GROUPINFO("WP_ALT", 1, ModeAutoLand, final_wp_alt, 55),
23 :
24 : // @Param: WP_DIST
25 : // @DisplayName: Final approach WP distance
26 : // @Description: This is the distance from Home that the final approach waypoint is set. The waypoint point will be in the opposite direction of takeoff (the direction the plane is facing when the plane sets its takeoff heading)
27 : // @Range: 0 700
28 : // @Increment: 1
29 : // @Units: m
30 : // @User: Standard
31 : AP_GROUPINFO("WP_DIST", 2, ModeAutoLand, final_wp_dist, 400),
32 :
33 : // @Param: DIR_OFF
34 : // @DisplayName: Landing direction offset from takeoff
35 : // @Description: The captured takeoff direction after ground course is established in autotakeoffsis offset by this amount to create a different landing direction and approach.However,if TKOFF_OPTION bit1 is set, the takeoff(landing) direction is captured immediately via compass heading upon arming, then this offset is NOT applied.
36 : // @Range: -360 360
37 : // @Increment: 1
38 : // @Units: deg
39 : // @User: Standard
40 : AP_GROUPINFO("DIR_OFF", 3, ModeAutoLand, landing_dir_off, 0),
41 :
42 : // @Param: OPTIONS
43 : // @DisplayName: Autoland mode options
44 : // @Description: Enables optional autoland mode behaviors
45 : // @Bitmask: 0: When set if there is a healthy compass in use the compass heading will be captured at arming and used for the AUTOLAND mode's initial takeoff direction instead of capturing ground course in NAV_TAKEOFF or Mode TAKEOFF or other modes.
46 : // @User: Standard
47 : AP_GROUPINFO("OPTIONS", 4, ModeAutoLand, options, 0),
48 :
49 : // @Param: CLIMB
50 : // @DisplayName: Minimum altitude above terrain before turning upon entry
51 : // @Description: Vehicle will climb with limited turn ability (LEVEL_ROLL_LIMIT) until it is at least this altitude above the terrain at the point of entry, before proceeding to loiter-to-alt and landing legs. 0 Disables.
52 : // @Range: 0 100
53 : // @Increment: 1
54 : // @Units: m
55 : // @User: Standard
56 : AP_GROUPINFO("CLIMB", 5, ModeAutoLand, terrain_alt_min, 0),
57 :
58 :
59 : AP_GROUPEND
60 : };
61 :
62 692 : ModeAutoLand::ModeAutoLand() :
63 692 : Mode()
64 : {
65 692 : AP_Param::setup_object_defaults(this, var_info);
66 692 : }
67 :
68 4 : bool ModeAutoLand::_enter()
69 : {
70 : //must be flying to enter
71 4 : if (!plane.is_flying()) {
72 0 : gcs().send_text(MAV_SEVERITY_WARNING, "Must already be flying!");
73 0 : return false;
74 : }
75 :
76 : // autoland not available for quadplanes as capture of takeoff direction
77 : // doesn't make sense
78 : #if HAL_QUADPLANE_ENABLED
79 4 : if (quadplane.available()) {
80 0 : gcs().send_text(MAV_SEVERITY_WARNING, "autoland not available");
81 0 : return false;
82 : }
83 : #endif
84 :
85 4 : if (!plane.takeoff_state.initial_direction.initialized) {
86 0 : gcs().send_text(MAV_SEVERITY_WARNING, "Takeoff initial direction not set");
87 0 : return false;
88 : }
89 :
90 4 : plane.set_target_altitude_current();
91 4 : plane.auto_state.next_wp_crosstrack = true;
92 :
93 4 : plane.prev_WP_loc = plane.current_loc;
94 :
95 : // In flight stage normal for approach
96 4 : plane.set_flight_stage(AP_FixedWing::FlightStage::NORMAL);
97 :
98 4 : const Location &home = ahrs.get_home();
99 :
100 : #ifndef HAL_LANDING_DEEPSTALL_ENABLED
101 : if (plane.landing.get_type() == AP_Landing::TYPE_DEEPSTALL) {
102 : // Deep stall landings require only a landing location, they do there own loiter to alt and approach
103 : cmd_land.id = MAV_CMD_NAV_LAND;
104 : cmd_land.content.location = home;
105 :
106 : // p1 gives the altitude from which to start the deep stall above the location alt
107 : cmd_land.p1 = final_wp_alt;
108 : plane.start_command(cmd_land);
109 :
110 : stage = AutoLandStage::LANDING;
111 : return true;
112 : }
113 : #endif // HAL_LANDING_DEEPSTALL_ENABLED
114 :
115 : /*
116 : Glide slope landing is in 3 steps:
117 : 1) a loitering to alt waypoint centered on base leg
118 : 2) exiting and proceeeing to a final approach land start WP, with crosstrack
119 : 3) the landing WP at home, with crosstrack
120 :
121 : the base leg point is 90 degrees off from the landing leg
122 : */
123 :
124 : /*
125 : first calculate the starting waypoint we will use when doing the
126 : NAV_LAND. This is offset by final_wp_dist from home, in a
127 : direction 180 degrees from takeoff direction
128 : */
129 4 : land_start = home;
130 4 : land_start.offset_bearing(plane.takeoff_state.initial_direction.heading, -final_wp_dist);
131 4 : land_start.set_alt_m(final_wp_alt, Location::AltFrame::ABOVE_HOME);
132 4 : land_start.change_alt_frame(Location::AltFrame::ABSOLUTE);
133 :
134 : /*
135 : now create the initial target waypoint for the loitering to alt centered on base leg waypoint. We
136 : choose if we will do a right or left turn onto the landing based
137 : on where we are when we enter the landing mode
138 : */
139 4 : const float bearing_to_current_deg = degrees(land_start.get_bearing(plane.current_loc));
140 4 : const float bearing_err_deg = wrap_180(plane.takeoff_state.initial_direction.heading - bearing_to_current_deg);
141 4 : const float bearing_offset_deg = (bearing_err_deg > 0) ? -90 : 90;
142 :
143 : // Try and minimize loiter radius by using the smaller of the waypoint loiter radius or 1/3 of the final WP distance
144 4 : const float loiter_radius = MIN(final_wp_dist * 0.333, abs(plane.aparm.loiter_radius));
145 :
146 : // corrected_loiter_radius is the radius the vehicle will actually fly, this gets larger as altitude increases.
147 : // Strictly this gets the loiter radius at the current altitude, really we want the loiter radius at final_wp_alt.
148 4 : const float corrected_loiter_radius = plane.nav_controller->loiter_radius(loiter_radius);
149 :
150 4 : cmd_loiter.id = MAV_CMD_NAV_LOITER_TO_ALT;
151 4 : cmd_loiter.p1 = loiter_radius;
152 4 : cmd_loiter.content.location = land_start;
153 4 : cmd_loiter.content.location.offset_bearing(plane.takeoff_state.initial_direction.heading + bearing_offset_deg, corrected_loiter_radius);
154 4 : cmd_loiter.content.location.loiter_ccw = bearing_err_deg>0? 1 :0;
155 :
156 : // May need to climb first
157 4 : bool climb_first = false;
158 4 : if (terrain_alt_min > 0) {
159 : // Work out the distance needed to climb above terrain
160 : #if AP_TERRAIN_AVAILABLE
161 1 : const bool use_terrain = plane.terrain_enabled_in_current_mode();
162 : #else
163 : const bool use_terrain = false;
164 : #endif
165 1 : const float dist_to_climb = terrain_alt_min - plane.relative_ground_altitude(RangeFinderUse::CLIMB, use_terrain);
166 1 : if (is_positive(dist_to_climb)) {
167 : // Copy loiter and update target altitude to current altitude plus climb altitude
168 1 : cmd_climb = cmd_loiter;
169 : float abs_alt;
170 1 : if (plane.current_loc.get_alt_m(Location::AltFrame::ABSOLUTE, abs_alt)) {
171 1 : cmd_climb.content.location.set_alt_m(abs_alt + dist_to_climb + fast_climb_extra_alt, Location::AltFrame::ABSOLUTE);
172 1 : climb_first = true;
173 : }
174 : }
175 : }
176 :
177 : #if AP_TERRAIN_AVAILABLE
178 : // Update loiter location to be relative terrain if enabled
179 4 : if (plane.terrain_enabled_in_current_mode()) {
180 1 : cmd_loiter.content.location.set_alt_m(final_wp_alt, Location::AltFrame::ABOVE_TERRAIN);
181 : };
182 : #endif
183 : // land WP at home
184 4 : cmd_land.id = MAV_CMD_NAV_LAND;
185 4 : cmd_land.content.location = home;
186 :
187 : // start first leg toward the base leg loiter to alt point
188 4 : if (climb_first) {
189 1 : stage = AutoLandStage::CLIMB;
190 1 : plane.start_command(cmd_climb);
191 :
192 : } else {
193 3 : stage = AutoLandStage::LOITER;
194 3 : plane.start_command(cmd_loiter);
195 : }
196 :
197 4 : return true;
198 : }
199 :
200 27866 : void ModeAutoLand::update()
201 : {
202 27866 : plane.calc_nav_roll();
203 :
204 : // Apply level roll limit in climb stage
205 27866 : if (stage == AutoLandStage::CLIMB) {
206 2223 : plane.roll_limit_cd = MIN(plane.roll_limit_cd, plane.g.level_roll_limit*100);
207 2223 : plane.nav_roll_cd = constrain_int16(plane.nav_roll_cd, -plane.roll_limit_cd, plane.roll_limit_cd);
208 : }
209 :
210 27866 : plane.calc_nav_pitch();
211 27866 : if (plane.landing.is_throttle_suppressed()) {
212 : // if landing is considered complete throttle is never allowed, regardless of landing type
213 2545 : SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0.0);
214 : } else {
215 25321 : plane.calc_throttle();
216 : }
217 27866 : }
218 :
219 5574 : void ModeAutoLand::navigate()
220 : {
221 5574 : switch (stage) {
222 445 : case AutoLandStage::CLIMB:
223 : // Update loiter, although roll limit is applied the vehicle will still navigate (slowly)
224 445 : plane.update_loiter(cmd_climb.p1);
225 :
226 : ftype dist;
227 445 : if (plane.reached_loiter_target() || !cmd_climb.content.location.get_height_above(plane.current_loc, dist) || (dist < fast_climb_extra_alt)) {
228 : // Reached destination or Climb is done, move onto loiter
229 1 : plane.auto_state.next_wp_crosstrack = true;
230 1 : stage = AutoLandStage::LOITER;
231 1 : plane.start_command(cmd_loiter);
232 1 : plane.prev_WP_loc = plane.current_loc;
233 : }
234 445 : break;
235 :
236 3960 : case AutoLandStage::LOITER:
237 : // check if we have arrived and completed loiter at base leg waypoint
238 3960 : if (plane.verify_loiter_to_alt(cmd_loiter)) {
239 3 : stage = AutoLandStage::LANDING;
240 3 : plane.start_command(cmd_land);
241 : // Crosstrack from the land start location
242 3 : plane.prev_WP_loc = land_start;
243 :
244 : }
245 3960 : break;
246 :
247 1169 : case AutoLandStage::LANDING:
248 1169 : plane.set_flight_stage(AP_FixedWing::FlightStage::LAND);
249 1169 : plane.verify_command(cmd_land);
250 : // make sure we line up
251 1169 : plane.auto_state.crosstrack = true;
252 1169 : break;
253 : }
254 5574 : }
255 :
256 : /*
257 : Takeoff direction is initialized after arm when sufficient altitude
258 : and ground speed is obtained, then captured takeoff direction +
259 : offset used as landing direction in AUTOLAND
260 : */
261 8377547 : void ModeAutoLand::check_takeoff_direction()
262 : {
263 : #if HAL_QUADPLANE_ENABLED
264 : // we don't allow fixed wing autoland for quadplanes
265 8377547 : if (quadplane.available()) {
266 6504054 : return;
267 : }
268 : #endif
269 :
270 1873493 : if (plane.takeoff_state.initial_direction.initialized) {
271 1226401 : return;
272 : }
273 : //set autoland direction to GPS course over ground
274 1284319 : if (plane.control_mode->allows_autoland_direction_capture() &&
275 643323 : plane.is_flying() &&
276 1290415 : hal.util->get_soft_armed() &&
277 4832 : plane.gps.ground_speed() > GPS_GND_CRS_MIN_SPD) {
278 144 : set_autoland_direction(plane.gps.ground_course() + landing_dir_off);
279 : }
280 : }
281 :
282 : // Sets autoland direction using ground course + offset parameter
283 144 : void ModeAutoLand::set_autoland_direction(const float heading)
284 : {
285 144 : plane.takeoff_state.initial_direction.heading = wrap_360(heading);
286 144 : plane.takeoff_state.initial_direction.initialized = true;
287 144 : gcs().send_text(MAV_SEVERITY_INFO, "Autoland direction= %u",int(plane.takeoff_state.initial_direction.heading));
288 144 : }
289 :
290 : /*
291 : return true when the LOITER_TO_ALT is lined up ready for the landing, used in commands_logic verify loiter to alt
292 : */
293 434 : bool ModeAutoLand::landing_lined_up(void)
294 : {
295 : // use the line between the center of the LOITER_TO_ALT on the base leg and the
296 : // start of the landing leg (land_start_WP)
297 434 : return plane.mode_loiter.isHeadingLinedUp(cmd_loiter.content.location, cmd_land.content.location);
298 : }
299 :
300 : // possibly capture heading on arming for autoland mode if option is set and using a compass
301 415 : void ModeAutoLand::arm_check(void)
302 : {
303 415 : if (plane.ahrs.use_compass() && autoland_option_is_set(ModeAutoLand::AutoLandOption::AUTOLAND_DIR_ON_ARM)) {
304 0 : set_autoland_direction(plane.ahrs.get_yaw_deg());
305 : }
306 415 : }
307 :
308 0 : bool ModeAutoLand::is_landing() const
309 : {
310 0 : return (plane.flight_stage == AP_FixedWing::FlightStage::LAND);
311 : }
312 :
313 :
314 : #endif // MODE_AUTOLAND_ENABLED
315 :
|
