diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 4e7054c888..f475e7ffc8 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -696,14 +696,6 @@ void Mode::land_run_horizontal_control() #endif if (!copter.ap.prec_land_active) { - if (copter.ap.prec_land_active) { - // precland isn't active anymore but was active a while back - // lets run an init again - // set target to stopping point - Vector2f stopping_point; - loiter_nav->get_stopping_point_xy(stopping_point); - loiter_nav->init_target(stopping_point); - } Vector2f accel; pos_control->input_vel_accel_xy(vel_correction, accel); } diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index ea51472f42..3becb06234 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -424,7 +424,6 @@ public: void takeoff_start(const Location& dest_loc); void wp_start(const Location& dest_loc); void land_start(); - void land_start(const Vector2f& destination); void circle_movetoedge_start(const Location &circle_center, float radius_m); void circle_start(); void nav_guided_start(); @@ -495,7 +494,6 @@ private: Location loc_from_cmd(const AP_Mission::Mission_Command& cmd, const Location& default_loc) const; - void payload_place_start(const Vector2f& destination); void payload_place_run(); bool payload_place_run_should_run(); void payload_place_run_loiter(); diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index a58b025b6a..fef897556d 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -343,22 +343,21 @@ void ModeAuto::wp_start(const Location& dest_loc) // auto_land_start - initialises controller to implement a landing void ModeAuto::land_start() -{ - // set target to stopping point - Vector2f stopping_point; - loiter_nav->get_stopping_point_xy(stopping_point); - - // call location specific land start function - land_start(stopping_point); -} - -// auto_land_start - initialises controller to implement a landing -void ModeAuto::land_start(const Vector2f& destination) { _mode = SubMode::LAND; - // initialise loiter target destination - loiter_nav->init_target(destination); + // set horizontal speed and acceleration limits + pos_control->set_max_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration()); + pos_control->set_correction_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration()); + + // initialise the vertical position controller + if (!pos_control->is_active_xy()) { + pos_control->init_xy_controller(); + } + + // set vertical speed and acceleration limits + pos_control->set_max_speed_accel_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up(), wp_nav->get_accel_z()); + pos_control->set_correction_speed_accel_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up(), wp_nav->get_accel_z()); // initialise the vertical position controller if (!pos_control->is_active_z()) { @@ -485,12 +484,29 @@ bool ModeAuto::is_taking_off() const // auto_payload_place_start - initialises controller to implement a placing void ModeAuto::payload_place_start() { - // set target to stopping point - Vector2f stopping_point; - loiter_nav->get_stopping_point_xy(stopping_point); + _mode = SubMode::NAV_PAYLOAD_PLACE; + nav_payload_place.state = PayloadPlaceStateType_Calibrating_Hover_Start; - // call location specific place start function - payload_place_start(stopping_point); + // set horizontal speed and acceleration limits + pos_control->set_max_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration()); + pos_control->set_correction_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration()); + + // initialise the vertical position controller + if (!pos_control->is_active_xy()) { + pos_control->init_xy_controller(); + } + + // set vertical speed and acceleration limits + pos_control->set_max_speed_accel_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up(), wp_nav->get_accel_z()); + pos_control->set_correction_speed_accel_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up(), wp_nav->get_accel_z()); + + // initialise the vertical position controller + if (!pos_control->is_active_z()) { + pos_control->init_z_controller(); + } + + // initialise yaw + auto_yaw.set_mode(AUTO_YAW_HOLD); } // returns true if pilot's yaw input should be used to adjust vehicle's heading @@ -908,8 +924,6 @@ void ModeAuto::land_run() // if not armed set throttle to zero and exit immediately if (is_disarmed_or_landed()) { make_safe_ground_handling(); - loiter_nav->clear_pilot_desired_acceleration(); - loiter_nav->init_target(); return; } @@ -1015,8 +1029,14 @@ void ModeAuto::loiter_to_alt_run() } if (!loiter_to_alt.loiter_start_done) { - loiter_nav->clear_pilot_desired_acceleration(); - loiter_nav->init_target(); + // set horizontal speed and acceleration limits + pos_control->set_max_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration()); + pos_control->set_correction_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration()); + + if (!pos_control->is_active_xy()) { + pos_control->init_xy_controller(); + } + _mode = SubMode::LOITER_TO_ALT; loiter_to_alt.loiter_start_done = true; } @@ -1054,31 +1074,12 @@ void ModeAuto::loiter_to_alt_run() pos_control->update_z_controller(); } -// auto_payload_place_start - initialises controller to implement placement of a load -void ModeAuto::payload_place_start(const Vector2f& destination) -{ - _mode = SubMode::NAV_PAYLOAD_PLACE; - nav_payload_place.state = PayloadPlaceStateType_Calibrating_Hover_Start; - - // initialise loiter target destination - loiter_nav->init_target(destination); - - // initialise the vertical position controller - pos_control->init_z_controller(); - - // initialise yaw - auto_yaw.set_mode(AUTO_YAW_HOLD); -} - // auto_payload_place_run - places an object in auto mode // called by auto_run at 100hz or more void ModeAuto::payload_place_run() { if (!payload_place_run_should_run()) { zero_throttle_and_relax_ac(); - // set target to current position - loiter_nav->clear_pilot_desired_acceleration(); - loiter_nav->init_target(); return; } @@ -1409,6 +1410,7 @@ void ModeAuto::do_loiter_to_alt(const AP_Mission::Mission_Command& cmd) // set vertical speed and acceleration limits pos_control->set_max_speed_accel_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up(), wp_nav->get_accel_z()); + pos_control->set_correction_speed_accel_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up(), wp_nav->get_accel_z()); } // do_spline_wp - initiate move to next waypoint @@ -1685,11 +1687,8 @@ bool ModeAuto::verify_land() case State::FlyToLocation: // check if we've reached the location if (copter.wp_nav->reached_wp_destination()) { - // get destination so we can use it for loiter target - const Vector2f& dest = copter.wp_nav->get_wp_destination().xy(); - // initialise landing controller - land_start(dest); + land_start(); // advance to next state state = State::Descending; diff --git a/ArduCopter/mode_land.cpp b/ArduCopter/mode_land.cpp index e06f00d3fc..2f2c3f278a 100644 --- a/ArduCopter/mode_land.cpp +++ b/ArduCopter/mode_land.cpp @@ -5,17 +5,19 @@ bool ModeLand::init(bool ignore_checks) { // check if we have GPS and decide which LAND we're going to do control_position = copter.position_ok(); - if (control_position) { - // set target to stopping point - Vector2f stopping_point; - loiter_nav->get_stopping_point_xy(stopping_point); - loiter_nav->init_target(stopping_point); + + // set horizontal speed and acceleration limits + pos_control->set_max_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration()); + pos_control->set_correction_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration()); + + // initialise the horizontal position controller + if (control_position && !pos_control->is_active_xy()) { + pos_control->init_xy_controller(); } // set vertical speed and acceleration limits pos_control->set_max_speed_accel_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up(), wp_nav->get_accel_z()); pos_control->set_correction_speed_accel_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up(), wp_nav->get_accel_z()); - pos_control->set_max_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration()); // initialise the vertical position controller if (!pos_control->is_active_z()) { @@ -76,8 +78,6 @@ void ModeLand::gps_run() // Land State Machine Determination if (is_disarmed_or_landed()) { make_safe_ground_handling(); - loiter_nav->clear_pilot_desired_acceleration(); - loiter_nav->init_target(); } else { // set motors to full range motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp index 6c148e57d0..e0fdebce10 100644 --- a/ArduCopter/mode_rtl.cpp +++ b/ArduCopter/mode_rtl.cpp @@ -273,9 +273,6 @@ void ModeRTL::descent_start() _state = SubMode::FINAL_DESCENT; _state_complete = false; - // Set wp navigation target to above home - loiter_nav->init_target(wp_nav->get_wp_destination().xy()); - // initialise altitude target to stopping point pos_control->init_z_controller_stopping_point(); @@ -363,8 +360,14 @@ void ModeRTL::land_start() _state = SubMode::LAND; _state_complete = false; - // Set wp navigation target to above home - loiter_nav->init_target(wp_nav->get_wp_destination().xy()); + // set horizontal speed and acceleration limits + pos_control->set_max_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration()); + pos_control->set_correction_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration()); + + // initialise loiter target destination + if (!pos_control->is_active_xy()) { + pos_control->init_xy_controller(); + } // initialise the vertical position controller if (!pos_control->is_active_z()) { @@ -405,8 +408,6 @@ void ModeRTL::land_run(bool disarm_on_land) // if not armed set throttle to zero and exit immediately if (is_disarmed_or_landed()) { make_safe_ground_handling(); - loiter_nav->clear_pilot_desired_acceleration(); - loiter_nav->init_target(); return; }