diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 6aac73a773..374e347b26 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -357,6 +357,7 @@ private: uint8_t unused3 : 1; // 25 // was compass_init_location; true when the compass's initial location has been set uint8_t unused2 : 1; // 26 // aux switch rc_override is allowed uint8_t armed_with_airmode_switch : 1; // 27 // we armed using a arming switch + uint8_t prec_land_active : 1; // 28 // true if precland is active }; uint32_t value; } ap_t; diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 792e8d1bc4..d112e37669 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -641,29 +641,47 @@ void Mode::land_run_horizontal_control() } } + // this variable will be updated if prec land target is in sight and pilot isn't trying to reposition the vehicle + bool doing_precision_landing = false; #if PRECISION_LANDING == ENABLED - bool doing_precision_landing = !copter.ap.land_repo_active && copter.precland.target_acquired(); + doing_precision_landing = !copter.ap.land_repo_active && copter.precland.target_acquired(); // run precision landing if (doing_precision_landing) { - Vector2f target_pos, target_vel_rel; + Vector2f target_pos, target_vel; if (!copter.precland.get_target_position_cm(target_pos)) { target_pos = inertial_nav.get_position_xy_cm(); } - if (!copter.precland.get_target_velocity_relative_cms(target_vel_rel)) { - target_vel_rel = -inertial_nav.get_velocity_xy_cms(); - } - pos_control->set_pos_target_xy_cm(target_pos.x, target_pos.y); - pos_control->override_vehicle_velocity_xy(-target_vel_rel); + // get the velocity of the target + copter.precland.get_target_velocity_cms(inertial_nav.get_velocity_xy_cms(), target_vel); + + Vector2f zero; + Vector2p landing_pos = target_pos.topostype(); + // target vel will remain zero if landing target is stationary + pos_control->input_pos_vel_accel_xy(landing_pos, target_vel, zero); + // run pos controller + pos_control->update_xy_controller(); } #endif - // process roll, pitch inputs - loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch); + if (!doing_precision_landing) { + 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); + } + // process roll, pitch inputs + loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch); - // run loiter controller - loiter_nav->update(); + // run loiter controller + loiter_nav->update(); + } - Vector3f thrust_vector = loiter_nav->get_thrust_vector(); + copter.ap.prec_land_active = doing_precision_landing; + + Vector3f thrust_vector = pos_control->get_thrust_vector(); if (g2.wp_navalt_min > 0) { // user has requested an altitude below which navigation diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index 25aff39732..5c5177277e 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -335,6 +335,12 @@ void ModeAuto::land_start(const Vector2f& destination) // disable the fence on landing copter.fence.auto_disable_fence_for_landing(); #endif + + // reset flag indicating if pilot has applied roll or pitch inputs during landing + copter.ap.land_repo_active = false; + + // this will be set true if prec land is later active + copter.ap.prec_land_active = false; } // auto_circle_movetoedge_start - initialise waypoint controller to move to edge of a circle with it's center at the specified location diff --git a/ArduCopter/mode_land.cpp b/ArduCopter/mode_land.cpp index 4096b7873e..e06f00d3fc 100644 --- a/ArduCopter/mode_land.cpp +++ b/ArduCopter/mode_land.cpp @@ -28,6 +28,9 @@ bool ModeLand::init(bool ignore_checks) // reset flag indicating if pilot has applied roll or pitch inputs during landing copter.ap.land_repo_active = false; + // this will be set true if prec land is later active + copter.ap.prec_land_active = false; + // initialise yaw auto_yaw.set_mode(AUTO_YAW_HOLD); diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp index 15dae4ecd8..9515334c3f 100644 --- a/ArduCopter/mode_rtl.cpp +++ b/ArduCopter/mode_rtl.cpp @@ -25,6 +25,9 @@ bool ModeRTL::init(bool ignore_checks) // reset flag indicating if pilot has applied roll or pitch inputs during landing copter.ap.land_repo_active = false; + // this will be set true if prec land is later active + copter.ap.prec_land_active = false; + #if PRECISION_LANDING == ENABLED // initialise precland state machine copter.precland_statemachine.init();