diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 79460fc846..7e828d0244 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -1079,6 +1079,7 @@ private: #if PRECISION_LANDING == ENABLED bool _precision_loiter_enabled; + bool _precision_loiter_active; // true if user has switched on prec loiter #endif }; diff --git a/ArduCopter/mode_loiter.cpp b/ArduCopter/mode_loiter.cpp index edb72faa5f..938ea249b6 100644 --- a/ArduCopter/mode_loiter.cpp +++ b/ArduCopter/mode_loiter.cpp @@ -34,6 +34,10 @@ bool ModeLoiter::init(bool ignore_checks) pos_control->set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); pos_control->set_correction_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); +#if PRECISION_LANDING == ENABLED + _precision_loiter_active = false; +#endif + return true; } @@ -59,15 +63,19 @@ bool ModeLoiter::do_precision_loiter() void ModeLoiter::precision_loiter_xy() { loiter_nav->clear_pilot_desired_acceleration(); - 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 @@ -158,13 +166,24 @@ void ModeLoiter::run() motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); #if PRECISION_LANDING == ENABLED + bool precision_loiter_old_state = _precision_loiter_active; if (do_precision_loiter()) { precision_loiter_xy(); + _precision_loiter_active = true; + } else { + _precision_loiter_active = false; } -#endif - - // run loiter controller + if (precision_loiter_old_state && !_precision_loiter_active) { + // prec loiter was active, not any more, let's init again as user takes control + loiter_nav->init_target(); + } + // run loiter controller if we are not doing prec loiter + if (!_precision_loiter_active) { + loiter_nav->update(); + } +#else loiter_nav->update(); +#endif // call attitude controller attitude_control->input_thrust_vector_rate_heading(loiter_nav->get_thrust_vector(), target_yaw_rate);