Copter: auto and guided takeoff use postion controller

ArduCopter/mode.h

@@ -197,15 +197,23 @@ protected:
 
     virtual bool do_user_takeoff_start(float takeoff_alt_cm);
 
-    // method shared by both Guided and Auto for takeoff.  This is
-    // waypoint navigation but the user can control the yaw.
+    // method shared by both Guided and Auto for takeoff.
+    // position controller controls vehicle but the user can control the yaw.
     void auto_takeoff_run();
-    void auto_takeoff_set_start_alt(void);
+    void auto_takeoff_start(float complete_alt_cm, bool terrain_alt);
+    bool auto_takeoff_get_position(Vector3p& completion_pos);
 
     // altitude above-ekf-origin below which auto takeoff does not control horizontal position
     static bool auto_takeoff_no_nav_active;
     static float auto_takeoff_no_nav_alt_cm;
 
+    // auto takeoff variables
+    static float auto_take_off_start_alt_cm;    // start altitude expressed as cm above ekf origin
+    static float auto_take_off_complete_alt_cm; // completion altitude expressed as cm above ekf origin
+    static bool auto_takeoff_terrain_alt;       // true if altitudes are above terrain
+    static bool auto_takeoff_complete;          // true when takeoff is complete
+    static Vector3p auto_takeoff_complete_pos;  // target takeoff position as offset from ekf origin in cm
+
 public:
     // Navigation Yaw control
     class AutoYaw {
@@ -940,6 +948,8 @@ public:
 
     bool is_taking_off() const override;
 
+    // initialises position controller to implement take-off
+    // takeoff_alt_cm is interpreted as alt-above-home (in cm) or alt-above-terrain if a rangefinder is available
     bool do_user_takeoff_start(float takeoff_alt_cm) override;
 
     enum class SubMode {

ArduCopter/mode_auto.cpp

@@ -262,39 +262,43 @@ void ModeAuto::takeoff_start(const Location& dest_loc)
 {
     _mode = SubMode::TAKEOFF;
 
-    Location dest(dest_loc);
-
     if (!copter.current_loc.initialised()) {
         // vehicle doesn't know where it is ATM.  We should not
         // initialise our takeoff destination without knowing this!
         return;
     }
 
-    // set horizontal target
-    dest.lat = copter.current_loc.lat;
-    dest.lng = copter.current_loc.lng;
+    // calculate current and target altitudes
+    // by default current_alt_cm and alt_target_cm are alt-above-EKF-origin
+    int32_t alt_target_cm;
+    bool alt_target_terrain = false;
+    float current_alt_cm = inertial_nav.get_position_z_up_cm();
+    float terrain_offset;   // terrain's altitude in cm above the ekf origin
+    if ((dest_loc.get_alt_frame() == Location::AltFrame::ABOVE_TERRAIN) && wp_nav->get_terrain_offset(terrain_offset)) {
+        // subtract terrain offset to convert vehicle's alt-above-ekf-origin to alt-above-terrain
+        current_alt_cm -= terrain_offset;
 
-    // get altitude target
-    int32_t alt_target;
-    if (!dest.get_alt_cm(Location::AltFrame::ABOVE_HOME, alt_target)) {
-        // this failure could only happen if take-off alt was specified as an alt-above terrain and we have no terrain data
-        AP::logger().Write_Error(LogErrorSubsystem::TERRAIN, LogErrorCode::MISSING_TERRAIN_DATA);
-        // fall back to altitude above current altitude
-        alt_target = copter.current_loc.alt + dest.alt;
+        // specify alt_target_cm as alt-above-terrain
+        alt_target_cm = dest_loc.alt;
+        alt_target_terrain = true;
+    } else {
+        // set horizontal target
+        Location dest(dest_loc);
+        dest.lat = copter.current_loc.lat;
+        dest.lng = copter.current_loc.lng;
+
+        // get altitude target above EKF origin
+        if (!dest.get_alt_cm(Location::AltFrame::ABOVE_ORIGIN, alt_target_cm)) {
+            // this failure could only happen if take-off alt was specified as an alt-above terrain and we have no terrain data
+            AP::logger().Write_Error(LogErrorSubsystem::TERRAIN, LogErrorCode::MISSING_TERRAIN_DATA);
+            // fall back to altitude above current altitude
+            alt_target_cm = current_alt_cm + dest.alt;
+        }
     }
 
     // sanity check target
-    int32_t alt_target_min_cm = copter.current_loc.alt + (copter.ap.land_complete ? 100 : 0);
-    if (alt_target < alt_target_min_cm ) {
-        dest.set_alt_cm(alt_target_min_cm , Location::AltFrame::ABOVE_HOME);
-    }
-
-    // set waypoint controller target
-    if (!wp_nav->set_wp_destination_loc(dest)) {
-        // failure to set destination can only be because of missing terrain data
-        copter.failsafe_terrain_on_event();
-        return;
-    }
+    int32_t alt_target_min_cm = current_alt_cm + (copter.ap.land_complete ? 100 : 0);
+    alt_target_cm = MAX(alt_target_cm, alt_target_min_cm);
 
     // initialise yaw
     auto_yaw.set_mode(AUTO_YAW_HOLD);
@@ -302,13 +306,25 @@ void ModeAuto::takeoff_start(const Location& dest_loc)
     // clear i term when we're taking off
     set_throttle_takeoff();
 
-    // get initial alt for WP_NAVALT_MIN
-    auto_takeoff_set_start_alt();
+    // initialise alt for WP_NAVALT_MIN and set completion alt
+    auto_takeoff_start(alt_target_cm, alt_target_terrain);
 }
 
 // auto_wp_start - initialises waypoint controller to implement flying to a particular destination
 void ModeAuto::wp_start(const Location& dest_loc)
 {
+    // init wpnav and set origin if transitioning from takeoff
+    if (!wp_nav->is_active()) {
+        Vector3f stopping_point;
+        if (_mode == SubMode::TAKEOFF) {
+            Vector3p takeoff_complete_pos;
+            if (auto_takeoff_get_position(takeoff_complete_pos)) {
+                stopping_point = takeoff_complete_pos.tofloat();
+            }
+        }
+        wp_nav->wp_and_spline_init(0, stopping_point);
+    }
+
     // send target to waypoint controller
     if (!wp_nav->set_wp_destination_loc(dest_loc)) {
         // failure to set destination can only be because of missing terrain data
@@ -1194,6 +1210,18 @@ void ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd)
         }
     }
 
+    // init wpnav and set origin if transitioning from takeoff
+    if (!wp_nav->is_active()) {
+        Vector3f stopping_point;
+        if (_mode == SubMode::TAKEOFF) {
+            Vector3p takeoff_complete_pos;
+            if (auto_takeoff_get_position(takeoff_complete_pos)) {
+                stopping_point = takeoff_complete_pos.tofloat();
+            }
+        }
+        wp_nav->wp_and_spline_init(0, stopping_point);
+    }
+
     // get waypoint's location from command and send to wp_nav
     const Location dest_loc = loc_from_cmd(cmd, default_loc);
     if (!wp_nav->set_wp_destination_loc(dest_loc)) {
@@ -1637,18 +1665,15 @@ void ModeAuto::do_RTL(void)
 // verify_takeoff - check if we have completed the takeoff
 bool ModeAuto::verify_takeoff()
 {
-    // have we reached our target altitude?
-    const bool reached_wp_dest = copter.wp_nav->reached_wp_destination();
-
 #if LANDING_GEAR_ENABLED == ENABLED
     // if we have reached our destination
-    if (reached_wp_dest) {
+    if (auto_takeoff_complete) {
         // retract the landing gear
         copter.landinggear.retract_after_takeoff();
     }
 #endif
 
-    return reached_wp_dest;
+    return auto_takeoff_complete;
 }
 
 // verify_land - returns true if landing has been completed

ArduCopter/mode_guided.cpp

@@ -98,14 +98,15 @@ bool ModeGuided::allows_arming(AP_Arming::Method method) const
     return (copter.g2.guided_options & (uint32_t)Options::AllowArmingFromTX) != 0;
 };
 
-// do_user_takeoff_start - initialises waypoint controller to implement take-off
+// initialises position controller to implement take-off
+// takeoff_alt_cm is interpreted as alt-above-home (in cm) or alt-above-terrain if a rangefinder is available
 bool ModeGuided::do_user_takeoff_start(float takeoff_alt_cm)
 {
     guided_mode = SubMode::TakeOff;
 
-    // initialise wpnav destination
-    Location target_loc = copter.current_loc;
-    Location::AltFrame frame = Location::AltFrame::ABOVE_HOME;
+    // calculate target altitude and frame (either alt-above-ekf-origin or alt-above-terrain)
+    int32_t alt_target_cm;
+    bool alt_target_terrain = false;
     if (wp_nav->rangefinder_used_and_healthy() &&
         wp_nav->get_terrain_source() == AC_WPNav::TerrainSource::TERRAIN_FROM_RANGEFINDER &&
         takeoff_alt_cm < copter.rangefinder.max_distance_cm_orient(ROTATION_PITCH_270)) {
@@ -113,15 +114,19 @@ bool ModeGuided::do_user_takeoff_start(float takeoff_alt_cm)
         if (takeoff_alt_cm <= copter.rangefinder_state.alt_cm) {
             return false;
         }
-        frame = Location::AltFrame::ABOVE_TERRAIN;
-    }
-    target_loc.set_alt_cm(takeoff_alt_cm, frame);
+        // provide target altitude as alt-above-terrain
+        alt_target_cm = takeoff_alt_cm;
+        alt_target_terrain = true;
+    } else {
+        // interpret altitude as alt-above-home
+        Location target_loc = copter.current_loc;
+        target_loc.set_alt_cm(takeoff_alt_cm, Location::AltFrame::ABOVE_HOME);
 
-    if (!wp_nav->set_wp_destination_loc(target_loc)) {
-        // failure to set destination can only be because of missing terrain data
-        AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::FAILED_TO_SET_DESTINATION);
-        // failure is propagated to GCS with NAK
-        return false;
+        // provide target altitude as alt-above-ekf-origin
+        if (!target_loc.get_alt_cm(Location::AltFrame::ABOVE_ORIGIN, alt_target_cm)) {
+            // this should never happen but we reject the command just in case
+            return false;
+        }
     }
 
     // initialise yaw
@@ -130,8 +135,8 @@ bool ModeGuided::do_user_takeoff_start(float takeoff_alt_cm)
     // clear i term when we're taking off
     set_throttle_takeoff();
 
-    // get initial alt for WP_NAVALT_MIN
-    auto_takeoff_set_start_alt();
+    // initialise alt for WP_NAVALT_MIN and set completion alt
+    auto_takeoff_start(alt_target_cm, alt_target_terrain);
 
     // record takeoff has not completed
     takeoff_complete = false;
@@ -629,7 +634,7 @@ void ModeGuided::set_angle(const Quaternion &attitude_quat, const Vector3f &ang_
 void ModeGuided::takeoff_run()
 {
     auto_takeoff_run();
-    if (!takeoff_complete && wp_nav->reached_wp_destination()) {
+    if (auto_takeoff_complete && !takeoff_complete) {
         takeoff_complete = true;
 #if LANDING_GEAR_ENABLED == ENABLED
         // optionally retract landing gear

ArduCopter/takeoff.cpp

@@ -4,6 +4,11 @@ Mode::_TakeOff Mode::takeoff;
 
 bool Mode::auto_takeoff_no_nav_active = false;
 float Mode::auto_takeoff_no_nav_alt_cm = 0;
+float Mode::auto_take_off_start_alt_cm = 0;
+float Mode::auto_take_off_complete_alt_cm = 0;
+bool Mode::auto_takeoff_terrain_alt = false;
+bool Mode::auto_takeoff_complete = false;
+Vector3p Mode::auto_takeoff_complete_pos;
 
 // This file contains the high-level takeoff logic for Loiter, PosHold, AltHold, Sport modes.
 //   The take-off can be initiated from a GCS NAV_TAKEOFF command which includes a takeoff altitude
@@ -93,13 +98,21 @@ void Mode::_TakeOff::do_pilot_takeoff(float& pilot_climb_rate_cm)
     }
 }
 
+// auto_takeoff_run - controls the vertical position controller during the process of taking off in auto modes
+// auto_takeoff_complete set to true when target altitude is within 10% of the take off altitude and less than 50% max climb rate
 void Mode::auto_takeoff_run()
 {
     // if not armed set throttle to zero and exit immediately
     if (!motors->armed() || !copter.ap.auto_armed) {
         // do not spool down tradheli when on the ground with motor interlock enabled
         make_safe_ground_handling(copter.is_tradheli() && motors->get_interlock());
-        wp_nav->shift_wp_origin_and_destination_to_current_pos_xy();
+        return;
+    }
+
+    // get terrain offset
+    float terr_offset = 0.0f;
+    if (auto_takeoff_terrain_alt && !wp_nav->get_terrain_offset(terr_offset)) {
+        gcs().send_text(MAV_SEVERITY_CRITICAL, "auto takeoff: failed to get terrain offset");
         return;
     }
 
@@ -116,17 +129,18 @@ void Mode::auto_takeoff_run()
         }
     }
 
-    // aircraft stays in landed state until rotor speed runup has finished
+    // aircraft stays in landed state until rotor speed run up has finished
     if (motors->get_spool_state() == AP_Motors::SpoolState::THROTTLE_UNLIMITED) {
         set_land_complete(false);
     } else {
         // motors have not completed spool up yet so relax navigation and position controllers
-        wp_nav->shift_wp_origin_and_destination_to_current_pos_xy();
+        pos_control->relax_velocity_controller_xy();
+        pos_control->update_xy_controller();
         pos_control->relax_z_controller(0.0f);   // forces throttle output to decay to zero
         pos_control->update_z_controller();
         attitude_control->reset_yaw_target_and_rate();
         attitude_control->reset_rate_controller_I_terms();
-        attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f);
+        attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), auto_yaw.rate_cds());
         return;
     }
 
@@ -135,51 +149,74 @@ void Mode::auto_takeoff_run()
         // check if vehicle has reached no_nav_alt threshold
         if (inertial_nav.get_position_z_up_cm() >= auto_takeoff_no_nav_alt_cm) {
             auto_takeoff_no_nav_active = false;
-            wp_nav->shift_wp_origin_and_destination_to_stopping_point_xy();
-        } else {
-            // shift the navigation target horizontally to our current position
-            wp_nav->shift_wp_origin_and_destination_to_current_pos_xy();
         }
-        // tell the position controller that we have limited roll/pitch demand to prevent integrator buildup
-        pos_control->set_externally_limited_xy();
-    }
-
-    // run waypoint controller
-    copter.failsafe_terrain_set_status(wp_nav->update_wpnav());
-
-    Vector3f thrustvector{0, 0, -GRAVITY_MSS * 100.0f};
-    if (!auto_takeoff_no_nav_active) {
-        thrustvector = wp_nav->get_thrust_vector();
+        pos_control->relax_velocity_controller_xy();
+    } else {
+        Vector2f vel;
+        Vector2f accel;
+        pos_control->input_vel_accel_xy(vel, accel);
     }
+    pos_control->update_xy_controller();
 
-    // WP_Nav has set the vertical position control targets
+    // command the aircraft to the take off altitude
+    float pos_z = auto_take_off_complete_alt_cm + terr_offset;
+    float vel_z = 0.0;
+    copter.pos_control->input_pos_vel_accel_z(pos_z, vel_z, 0.0);
+    
     // run the vertical position controller and set output throttle
-    copter.pos_control->update_z_controller();
+    pos_control->update_z_controller();
 
     // call attitude controller
     if (auto_yaw.mode() == AUTO_YAW_HOLD) {
         // roll & pitch from position controller, yaw rate from pilot
-        attitude_control->input_thrust_vector_rate_heading(thrustvector, target_yaw_rate);
+        attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), target_yaw_rate);
     } else if (auto_yaw.mode() == AUTO_YAW_RATE) {
         // roll & pitch from position controller, yaw rate from mavlink command or mission item
-        attitude_control->input_thrust_vector_rate_heading(thrustvector, auto_yaw.rate_cds());
+        attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), auto_yaw.rate_cds());
     } else {
         // roll & pitch from position controller, yaw heading from GCS or auto_heading()
-        attitude_control->input_thrust_vector_heading(thrustvector, auto_yaw.yaw(), auto_yaw.rate_cds());
+        attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.yaw(), auto_yaw.rate_cds());
+    }
+
+    // handle takeoff completion
+    bool reached_altitude = (copter.pos_control->get_pos_target_z_cm() - auto_take_off_start_alt_cm) >= ((auto_take_off_complete_alt_cm  - auto_take_off_start_alt_cm) * 0.90);
+    bool reached_climb_rate = copter.pos_control->get_vel_desired_cms().z < copter.pos_control->get_max_speed_up_cms() * 0.5;
+    auto_takeoff_complete = reached_altitude && reached_climb_rate;
+
+    // calculate completion for location in case it is needed for a smooth transition to wp_nav
+    if (auto_takeoff_complete) {
+        const Vector3p& complete_pos = copter.pos_control->get_pos_target_cm();
+        auto_takeoff_complete_pos = Vector3p{complete_pos.x, complete_pos.y, pos_z};
     }
 }
 
-void Mode::auto_takeoff_set_start_alt(void)
+void Mode::auto_takeoff_start(float complete_alt_cm, bool terrain_alt)
 {
+    auto_take_off_start_alt_cm = inertial_nav.get_position_z_up_cm();
+    auto_take_off_complete_alt_cm = complete_alt_cm;
+    auto_takeoff_terrain_alt = terrain_alt;
+    auto_takeoff_complete = false;
     if ((g2.wp_navalt_min > 0) && (is_disarmed_or_landed() || !motors->get_interlock())) {
         // we are not flying, climb with no navigation to current alt-above-ekf-origin + wp_navalt_min
-        auto_takeoff_no_nav_alt_cm = inertial_nav.get_position_z_up_cm() + g2.wp_navalt_min * 100;
+        auto_takeoff_no_nav_alt_cm = auto_take_off_start_alt_cm + g2.wp_navalt_min * 100;
         auto_takeoff_no_nav_active = true;
     } else {
         auto_takeoff_no_nav_active = false;
     }
 }
 
+// return takeoff final position if takeoff has completed successfully
+bool Mode::auto_takeoff_get_position(Vector3p& complete_pos)
+{
+    // only provide location if takeoff has completed
+    if (!auto_takeoff_complete) {
+        return false;
+    }
+
+    complete_pos = auto_takeoff_complete_pos;
+    return true;
+}
+
 bool Mode::is_taking_off() const
 {
     if (!has_user_takeoff(false)) {