diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 4c9ee573ae..ea51472f42 100644 --- a/ArduCopter/mode.h +++ b/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 { diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index 9d449ef770..a58b025b6a 100644 --- a/ArduCopter/mode_auto.cpp +++ b/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 diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index 0a26f0aa14..cc8263d0fa 100644 --- a/ArduCopter/mode_guided.cpp +++ b/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 diff --git a/ArduCopter/takeoff.cpp b/ArduCopter/takeoff.cpp index 687287336b..4f12db1256 100644 --- a/ArduCopter/takeoff.cpp +++ b/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(); + pos_control->relax_velocity_controller_xy(); + } else { + Vector2f vel; + Vector2f accel; + pos_control->input_vel_accel_xy(vel, accel); } + pos_control->update_xy_controller(); - // 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(); - } - - // 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)) {