ArduPlane/mode_qrtl.cpp

   1 #include "mode.h"
   2 #include "Plane.h"
   3
   4 #if HAL_QUADPLANE_ENABLED
   5
   6 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     if (plane.quadplane.guided_wait_takeoff_on_mode_enter) {
  11        plane.set_mode(plane.mode_qland, ModeReason::QLAND_INSTEAD_OF_RTL);
  12        return true;
  13     }
  14     submode = SubMode::RTL;
  15     plane.prev_WP_loc = plane.current_loc;
  16
  17     int32_t RTL_alt_abs_cm = plane.home.alt + quadplane.qrtl_alt*100UL;
  18     if (quadplane.motors->get_desired_spool_state() == AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED) {
  19         // VTOL motors are active, either in VTOL flight or assisted flight
  20         Location destination = plane.calc_best_rally_or_home_location(plane.current_loc, RTL_alt_abs_cm);
  21         const float dist = plane.current_loc.get_distance(destination);
  22         const float radius = get_VTOL_return_radius();
  23
  24         // Climb at least to a cone around home of hight of QRTL alt and radius of radius
  25         // Always climb up to at least Q_RTL_ALT_MIN, constrain Q_RTL_ALT_MIN between Q_LAND_FINAL_ALT and Q_RTL_ALT
  26         const float min_climb = constrain_float(quadplane.qrtl_alt_min, quadplane.land_final_alt, quadplane.qrtl_alt);
  27         const float target_alt = MAX(quadplane.qrtl_alt * (dist / MAX(radius, dist)), min_climb);
  28
  29
  30 #if AP_TERRAIN_AVAILABLE
  31         const bool use_terrain = plane.terrain_enabled_in_mode(mode_number());
  32 #else
  33         const bool use_terrain = false;
  34 #endif
  35
  36         const float dist_to_climb = target_alt - plane.relative_ground_altitude(plane.g.rangefinder_landing, use_terrain);
  37         if (is_positive(dist_to_climb)) {
  38             // climb before returning, only next waypoint altitude is used
  39             submode = SubMode::climb;
  40             plane.next_WP_loc = plane.current_loc;
  41 #if AP_TERRAIN_AVAILABLE
  42             int32_t curent_alt_terrain_cm;
  43             if (use_terrain && plane.current_loc.get_alt_cm(Location::AltFrame::ABOVE_TERRAIN, curent_alt_terrain_cm)) {
  44                 plane.next_WP_loc.set_alt_cm(curent_alt_terrain_cm + dist_to_climb * 100UL, Location::AltFrame::ABOVE_TERRAIN);
  45                 return true;
  46             }
  47 #endif
  48             plane.next_WP_loc.set_alt_cm(plane.current_loc.alt + dist_to_climb * 100UL, plane.current_loc.get_alt_frame());
  49             return true;
  50
  51         } else if (dist < radius) {
  52             // Above home "cone", return at curent altitude if lower than QRTL alt
  53             int32_t current_alt_abs_cm;
  54             if (plane.current_loc.get_alt_cm(Location::AltFrame::ABSOLUTE, current_alt_abs_cm)) {
  55                 RTL_alt_abs_cm = MIN(RTL_alt_abs_cm, current_alt_abs_cm);
  56             }
  57
  58             // we're close to destination and already running VTOL motors, don't transition and don't climb
  59             gcs().send_text(MAV_SEVERITY_INFO,"VTOL position1 d=%.1f r=%.1f", dist, radius);
  60             poscontrol.set_state(QuadPlane::QPOS_POSITION1);
  61         }
  62     }
  63
  64     // use do_RTL() to setup next_WP_loc
  65     plane.do_RTL(RTL_alt_abs_cm);
  66     quadplane.poscontrol_init_approach();
  67
  68     int32_t from_alt;
  69     int32_t to_alt;
  70     if (plane.current_loc.get_alt_cm(Location::AltFrame::ABSOLUTE,from_alt) && plane.next_WP_loc.get_alt_cm(Location::AltFrame::ABSOLUTE,to_alt)) {
  71         poscontrol.slow_descent = from_alt > to_alt;
  72         return true;
  73     }
  74     // default back to old method
  75     poscontrol.slow_descent = (plane.current_loc.alt > plane.next_WP_loc.alt);
  76     return true;
  77 }
  78
  79 void ModeQRTL::update()
  80 {
  81     plane.mode_qstabilize.update();
  82 }
  83
  84 /*
  85   handle QRTL mode
  86  */
  87 void ModeQRTL::run()
  88 {
  89     const uint32_t now = AP_HAL::millis();
  90     if (quadplane.tailsitter.in_vtol_transition(now)) {
  91         // Tailsitters in FW pull up phase of VTOL transition run FW controllers
  92         Mode::run();
  93         return;
  94     }
  95
  96     switch (submode) {
  97         case SubMode::climb: {
  98             // request zero velocity
  99             Vector2f vel, accel;
 100             pos_control->input_vel_accel_xy(vel, accel);
 101             quadplane.run_xy_controller();
 102
 103             // nav roll and pitch are controller by position controller
 104             plane.nav_roll_cd = pos_control->get_roll_cd();
 105             plane.nav_pitch_cd = pos_control->get_pitch_cd();
 106
 107             plane.quadplane.assign_tilt_to_fwd_thr();
 108
 109             if (quadplane.transition->set_VTOL_roll_pitch_limit(plane.nav_roll_cd, plane.nav_pitch_cd)) {
 110                 pos_control->set_externally_limited_xy();
 111             }
 112             // weathervane with no pilot input
 113             quadplane.disable_yaw_rate_time_constant();
 114             attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(plane.nav_roll_cd,
 115                                                                           plane.nav_pitch_cd,
 116                                                                           quadplane.get_weathervane_yaw_rate_cds());
 117
 118             // climb at full WP nav speed
 119             quadplane.set_climb_rate_cms(quadplane.wp_nav->get_default_speed_up());
 120             quadplane.run_z_controller();
 121
 122             // Climb done when stopping point reaches target altitude
 123             Vector3p stopping_point;
 124             pos_control->get_stopping_point_z_cm(stopping_point.z);
 125             Location stopping_loc = Location(stopping_point.tofloat(), Location::AltFrame::ABOVE_ORIGIN);
 126
 127             ftype alt_diff;
 128             if (!stopping_loc.get_alt_distance(plane.next_WP_loc, alt_diff) || is_positive(alt_diff)) {
 129                 // climb finished or cant get alt diff, head home
 130                 submode = SubMode::RTL;
 131                 plane.prev_WP_loc = plane.current_loc;
 132
 133                 int32_t RTL_alt_abs_cm = plane.home.alt + quadplane.qrtl_alt*100UL;
 134                 Location destination = plane.calc_best_rally_or_home_location(plane.current_loc, RTL_alt_abs_cm);
 135                 const float dist = plane.current_loc.get_distance(destination);
 136                 const float radius = get_VTOL_return_radius();
 137                 if (dist < radius) {
 138                     // if close to home return at current target altitude
 139                     int32_t target_alt_abs_cm;
 140                     if (plane.next_WP_loc.get_alt_cm(Location::AltFrame::ABSOLUTE, target_alt_abs_cm)) {
 141                         RTL_alt_abs_cm = MIN(RTL_alt_abs_cm, target_alt_abs_cm);
 142                     }
 143                     gcs().send_text(MAV_SEVERITY_INFO,"VTOL position1 d=%.1f r=%.1f", dist, radius);
 144                     poscontrol.set_state(QuadPlane::QPOS_POSITION1);
 145                 }
 146
 147                 plane.do_RTL(RTL_alt_abs_cm);
 148                 quadplane.poscontrol_init_approach();
 149                 if (plane.current_loc.get_alt_distance(plane.next_WP_loc, alt_diff)) {
 150                     poscontrol.slow_descent = is_positive(alt_diff);
 151                 } else {
 152                     // default back to old method
 153                     poscontrol.slow_descent = (plane.current_loc.alt > plane.next_WP_loc.alt);
 154                 }
 155             }
 156             break;
 157         }
 158
 159         case SubMode::RTL: {
 160             quadplane.vtol_position_controller();
 161             if (poscontrol.get_state() > QuadPlane::QPOS_POSITION2) {
 162                 // change target altitude to home alt
 163                 plane.next_WP_loc.alt = plane.home.alt;
 164             }
 165             if (poscontrol.get_state() >= QuadPlane::QPOS_POSITION2) {
 166                 // start landing logic
 167                 quadplane.verify_vtol_land();
 168             }
 169
 170             // when in approach allow stick mixing
 171             if (quadplane.poscontrol.get_state() == QuadPlane::QPOS_AIRBRAKE ||
 172                 quadplane.poscontrol.get_state() == QuadPlane::QPOS_APPROACH) {
 173                 plane.stabilize_stick_mixing_fbw();
 174             }
 175             break;
 176         }
 177     }
 178
 179     // Stabilize with fixed wing surfaces
 180     plane.stabilize_roll();
 181     plane.stabilize_pitch();
 182 }
 183
 184 /*
 185   update target altitude for QRTL profile
 186  */
 187 void ModeQRTL::update_target_altitude()
 188 {
 189     /*
 190       update height target in approach
 191      */
 192     if ((submode != SubMode::RTL) || (plane.quadplane.poscontrol.get_state() != QuadPlane::QPOS_APPROACH)) {
 193         Mode::update_target_altitude();
 194         return;
 195     }
 196
 197     /*
 198       initially approach at RTL_ALT_CM, then drop down to QRTL_ALT based on maximum sink rate from TECS,
 199       giving time to lose speed before we transition
 200      */
 201     const float radius = MAX(fabsf(float(plane.aparm.loiter_radius)), fabsf(float(plane.g.rtl_radius)));
 202     const float rtl_alt_delta = MAX(0, plane.g.RTL_altitude - plane.quadplane.qrtl_alt);
 203     const float sink_time = rtl_alt_delta / MAX(0.6*plane.TECS_controller.get_max_sinkrate(), 1);
 204     const float sink_dist = plane.aparm.airspeed_cruise * sink_time;
 205     const float dist = plane.auto_state.wp_distance;
 206     const float rad_min = 2*radius;
 207     const float rad_max = 20*radius;
 208     float alt = linear_interpolate(0, rtl_alt_delta,
 209                                    dist,
 210                                    rad_min, MAX(rad_min, MIN(rad_max, rad_min+sink_dist)));
 211     Location loc = plane.next_WP_loc;
 212     loc.alt += alt*100;
 213     plane.set_target_altitude_location(loc);
 214 }
 215
 216 // only nudge during approach
 217 bool ModeQRTL::allows_throttle_nudging() const
 218 {
 219     return (submode == SubMode::RTL) && (plane.quadplane.poscontrol.get_state() == QuadPlane::QPOS_APPROACH);
 220 }
 221
 222 // Return the radius from destination at which pure VTOL flight should be used, no transition to FW
 223 float ModeQRTL::get_VTOL_return_radius() const
 224 {
 225     return MAX(fabsf(float(plane.aparm.loiter_radius)), fabsf(float(plane.g.rtl_radius))) * 1.5;
 226 }
 227
 228 #endif