Line data    Source code

       1             : #include "mode.h"
       2             : #include "Plane.h"
       3             : #include <GCS_MAVLink/GCS.h>
       4             : 
       5             : /*
       6             :   mode takeoff parameters
       7             :  */
       8             : const AP_Param::GroupInfo ModeTakeoff::var_info[] = {
       9             :     // @Param: ALT
      10             :     // @DisplayName: Takeoff mode altitude
      11             :     // @Description: This is the target altitude for TAKEOFF mode
      12             :     // @Range: 0 200
      13             :     // @Increment: 1
      14             :     // @Units: m
      15             :     // @User: Standard
      16             :     AP_GROUPINFO("ALT", 1, ModeTakeoff, target_alt, 50),
      17             : 
      18             :     // @Param: LVL_ALT
      19             :     // @DisplayName: Takeoff mode altitude level altitude
      20             :     // @Description: This is the altitude below which the wings are held level for TAKEOFF and AUTO modes. Below this altitude, roll demand is restricted to LEVEL_ROLL_LIMIT. Normal-flight roll restriction resumes above TKOFF_LVL_ALT*3 or TKOFF_ALT, whichever is lower. Roll limits are scaled while between TKOFF_LVL_ALT and those altitudes for a smooth transition.
      21             :     // @Range: 0 50
      22             :     // @Increment: 1
      23             :     // @Units: m
      24             :     // @User: Standard
      25             :     AP_GROUPINFO("LVL_ALT", 2, ModeTakeoff, level_alt, 10),
      26             : 
      27             :     // @Param: LVL_PITCH
      28             :     // @DisplayName: Takeoff mode altitude initial pitch
      29             :     // @Description: This is the target pitch during the takeoff.
      30             :     // @Range: 0 30
      31             :     // @Increment: 1
      32             :     // @Units: deg
      33             :     // @User: Standard
      34             :     AP_GROUPINFO("LVL_PITCH", 3, ModeTakeoff, level_pitch, 15),
      35             : 
      36             :     // @Param: DIST
      37             :     // @DisplayName: Takeoff mode distance
      38             :     // @Description: This is the distance from the takeoff location where the plane will loiter. The loiter point will be in the direction of takeoff (the direction the plane is facing when the plane begins takeoff)
      39             :     // @Range: 0 500
      40             :     // @Increment: 1
      41             :     // @Units: m
      42             :     // @User: Standard
      43             :     AP_GROUPINFO("DIST", 4, ModeTakeoff, target_dist, 200),
      44             : 
      45             :     // @Param: GND_PITCH
      46             :     // @DisplayName: Takeoff run pitch demand
      47             :     // @Description: Degrees of pitch angle demanded during the takeoff run before speed reaches TKOFF_ROTATE_SPD. For taildraggers set to 3-point ground pitch angle and use TKOFF_TDRAG_ELEV to prevent nose tipover. For nose-wheel steer aircraft set to the ground pitch angle and if a reduction in nose-wheel load is required as speed rises, use a positive offset in TKOFF_GND_PITCH of up to 5 degrees above the angle on ground, starting at the measured pitch angle and incrementing in 1 degree steps whilst checking for premature rotation and takeoff with each increment. To increase nose-wheel load, use a negative TKOFF_TDRAG_ELEV and refer to notes on TKOFF_TDRAG_ELEV before making adjustments.
      48             :     // @Units: deg
      49             :     // @Range: -5.0 10.0
      50             :     // @Increment: 0.1
      51             :     // @User: Standard
      52             :     AP_GROUPINFO("GND_PITCH", 5, ModeTakeoff, ground_pitch, 5),
      53             : 
      54             :     AP_GROUPEND
      55             : };
      56             : 
      57         692 : ModeTakeoff::ModeTakeoff() :
      58         692 :     Mode()
      59             : {
      60         692 :     AP_Param::setup_object_defaults(this, var_info);
      61         692 : }
      62             : 
      63          53 : bool ModeTakeoff::_enter()
      64             : {
      65          53 :     takeoff_mode_setup = false;
      66          53 :     have_autoenabled_fences = false;
      67             : 
      68          53 :     return true;
      69             : }
      70             : 
      71      138339 : void ModeTakeoff::update()
      72             : {
      73             :     // don't setup waypoints if we dont have a valid position and home!
      74      138339 :     if (!(plane.current_loc.initialised() && AP::ahrs().home_is_set())) {
      75        8934 :         plane.calc_nav_roll();
      76        8934 :         plane.calc_nav_pitch();
      77        8934 :         SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0.0);
      78        8934 :         return;
      79             :     }
      80             : 
      81      129405 :     const float alt = target_alt;
      82      129405 :     const float dist = target_dist;
      83      129405 :     if (!takeoff_mode_setup) {
      84       23043 :         plane.auto_state.takeoff_altitude_rel_cm = alt * 100;
      85       23043 :         const uint16_t altitude = plane.relative_ground_altitude(RangeFinderUse::NONE,true);
      86       23043 :         const Vector2f &groundspeed2d = ahrs.groundspeed_vector();
      87       23043 :         const float direction = wrap_360(degrees(groundspeed2d.angle()));
      88       23043 :         const float groundspeed = groundspeed2d.length();
      89             : 
      90             :         // see if we will skip takeoff as already flying
      91       23043 :         if (plane.is_flying() && (millis() - plane.started_flying_ms > 10000U) && groundspeed > 3) {
      92           0 :             if (altitude >= alt) {
      93           0 :                 gcs().send_text(MAV_SEVERITY_INFO, "Above TKOFF alt - loitering");
      94           0 :                 plane.next_WP_loc = plane.current_loc;
      95           0 :                 takeoff_mode_setup = true;
      96           0 :                 plane.set_flight_stage(AP_FixedWing::FlightStage::NORMAL);
      97             :             } else {
      98           0 :                 gcs().send_text(MAV_SEVERITY_INFO, "Climbing to TKOFF alt then loitering");
      99           0 :                 start_loc = plane.current_loc;
     100           0 :                 plane.next_WP_loc = plane.current_loc;
     101           0 :                 plane.next_WP_loc.offset_up_m(alt - altitude);
     102           0 :                 plane.next_WP_loc.offset_bearing(direction, dist);
     103           0 :                 takeoff_mode_setup = true;
     104           0 :                 plane.set_flight_stage(AP_FixedWing::FlightStage::TAKEOFF);
     105             :             }
     106             :             // not flying so do a full takeoff sequence
     107             :         } else {
     108             :             // setup target waypoint and alt for loiter at dist and alt from start
     109       23043 :             start_loc = plane.current_loc;
     110       23043 :             plane.prev_WP_loc = plane.current_loc;
     111       23043 :             plane.next_WP_loc = plane.current_loc;
     112       23043 :             plane.next_WP_loc.offset_up_m(alt);
     113       23043 :             plane.next_WP_loc.offset_bearing(direction, dist);
     114             : 
     115       23043 :             plane.crash_state.is_crashed = false;
     116             : 
     117       23043 :             plane.auto_state.takeoff_pitch_cd = level_pitch * 100;
     118             : 
     119       23043 :             plane.set_flight_stage(AP_FixedWing::FlightStage::TAKEOFF);
     120             : 
     121             :             /*
     122             :               don't lock in the takeoff loiter location until we reach
     123             :               a ground speed of GPS_GND_CRS_MIN_SPD to cope with aircraft
     124             :               with no compass or poor compass. If flying in a very
     125             :               strong headwind then the is_flying() check above will
     126             :               eventually trigger
     127             :              */
     128       23043 :             if (!plane.throttle_suppressed &&
     129             :                 groundspeed > GPS_GND_CRS_MIN_SPD) {
     130          51 :                 gcs().send_text(MAV_SEVERITY_INFO, "Takeoff to %.0fm for %.1fm heading %.1f deg",
     131             :                                 alt, dist, direction);
     132          51 :                 plane.takeoff_state.start_time_ms = millis();
     133          51 :                 plane.takeoff_state.level_off_start_time_ms = 0;
     134          51 :                 plane.takeoff_state.throttle_max_timer_ms = millis();
     135          51 :                 takeoff_mode_setup = true;
     136          51 :                 plane.steer_state.hold_course_cd = wrap_360_cd(direction*100); // Necessary to allow Plane::takeoff_calc_roll() to function.
     137             :             }
     138             :         }
     139             :     }
     140             :     // check for optional takeoff timeout
     141      129405 :     if (plane.check_takeoff_timeout()) {
     142           0 :         plane.set_flight_stage(AP_FixedWing::FlightStage::NORMAL);
     143           0 :         takeoff_mode_setup = false;
     144             :     }
     145             :     // We update the waypoint to follow once we're past TKOFF_LVL_ALT or we
     146             :     // pass the target location. The check for target location prevents us
     147             :     // flying towards a wrong location if we can't climb.
     148             :     // This will correct any yaw estimation errors (caused by EKF reset
     149             :     // or compass interference from max throttle), since it's using position bearing.
     150      129405 :     const float altitude_cm = plane.current_loc.alt - start_loc.alt;
     151      258810 :     if (plane.flight_stage == AP_FixedWing::FlightStage::TAKEOFF
     152       43490 :         && plane.steer_state.hold_course_cd == -1 // This is the enter-once flag.
     153      172895 :         && (altitude_cm >= (level_alt * 100.0f) || start_loc.get_distance(plane.current_loc) >= dist)
     154             :     ) {
     155           0 :         const float direction = start_loc.get_bearing_to(plane.current_loc) * 0.01;
     156           0 :         plane.next_WP_loc = start_loc;
     157           0 :         plane.next_WP_loc.offset_bearing(direction, dist);
     158           0 :         plane.next_WP_loc.offset_up_m(alt);
     159           0 :         plane.steer_state.hold_course_cd = wrap_360_cd(direction*100); // Necessary to allow Plane::takeoff_calc_roll() to function.
     160             :     }
     161             :         
     162             :     // We finish the initial level takeoff if we climb past
     163             :     // TKOFF_ALT or we pass the target location. The check for
     164             :     // target location prevents us flying forever if we can't climb.
     165      172895 :     if (plane.flight_stage == AP_FixedWing::FlightStage::TAKEOFF &&
     166       43490 :         (altitude_cm >= (alt*100 - 200) ||
     167       43465 :         start_loc.get_distance(plane.current_loc) >= dist)) {
     168          44 :         plane.set_flight_stage(AP_FixedWing::FlightStage::NORMAL);
     169             :     }
     170             : 
     171             :     // If we have timed-out on the attempt to close the final few meters
     172             :     // during pitch level-off, continue to NORMAL flight stage.
     173      129405 :     if (plane.check_takeoff_timeout_level_off()) {
     174       68414 :         plane.set_flight_stage(AP_FixedWing::FlightStage::NORMAL);
     175             :     }
     176             : 
     177      129405 :     if (plane.flight_stage == AP_FixedWing::FlightStage::TAKEOFF) {
     178             :         //below TKOFF_ALT
     179       43438 :         plane.takeoff_calc_roll();
     180       43438 :         plane.takeoff_calc_pitch();
     181       43438 :         plane.takeoff_calc_throttle();
     182             :     } else {
     183             : #if AP_FENCE_ENABLED
     184       85967 :         if (!have_autoenabled_fences) {
     185          47 :             plane.fence.auto_enable_fence_after_takeoff();
     186          47 :             have_autoenabled_fences = true;
     187             :         }
     188             : #endif
     189       85967 :         plane.calc_nav_roll();
     190       85967 :         plane.calc_nav_pitch();
     191       85967 :         plane.calc_throttle();
     192             :         
     193             :         //check if in long failsafe due to being in initial TAKEOFF stage; if it is, recall long failsafe now to get fs action via events call
     194       85967 :         if (plane.long_failsafe_pending) {
     195           0 :             plane.long_failsafe_pending = false;
     196           0 :             plane.failsafe_long_on_event(FAILSAFE_LONG, ModeReason::MODE_TAKEOFF_FAILSAFE);
     197             :         }
     198             :     }
     199             : }
     200             : 
     201       16287 : void ModeTakeoff::navigate()
     202             : {
     203             :     // Zero indicates to use WP_LOITER_RAD
     204       16287 :     plane.update_loiter(0);
     205       16287 : }
     206             :