diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 9fca458407..fe37275a46 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -269,6 +269,8 @@ private: uint32_t start_ms; } takeoff_state; + uint32_t precland_last_update_ms; + RCMapper rcmap; // board specific config diff --git a/ArduCopter/control_auto.cpp b/ArduCopter/control_auto.cpp index 5243634026..35902e20c3 100644 --- a/ArduCopter/control_auto.cpp +++ b/ArduCopter/control_auto.cpp @@ -420,10 +420,15 @@ void Copter::auto_land_run() // process roll, pitch inputs wp_nav.set_pilot_desired_acceleration(roll_control, pitch_control); + #if PRECISION_LANDING == ENABLED // run precision landing - if (!ap.land_repo_active) { - wp_nav.shift_loiter_target(precland.get_target_shift(wp_nav.get_loiter_target())); + if (!ap.land_repo_active && precland.target_acquired() && precland_last_update_ms != precland.last_update_ms()) { + Vector3f target_pos; + precland.get_target_position(target_pos); + pos_control.set_xy_target(target_pos.x, target_pos.y); + pos_control.freeze_ff_xy(); + precland_last_update_ms = precland.last_update_ms(); } #endif diff --git a/ArduCopter/control_land.cpp b/ArduCopter/control_land.cpp index 72cc400936..6d6bde5372 100644 --- a/ArduCopter/control_land.cpp +++ b/ArduCopter/control_land.cpp @@ -123,8 +123,12 @@ void Copter::land_gps_run() #if PRECISION_LANDING == ENABLED // run precision landing - if (!ap.land_repo_active) { - wp_nav.shift_loiter_target(precland.get_target_shift(wp_nav.get_loiter_target())); + if (!ap.land_repo_active && precland.target_acquired() && precland_last_update_ms != precland.last_update_ms()) { + Vector3f target_pos; + precland.get_target_position(target_pos); + pos_control.set_xy_target(target_pos.x, target_pos.y); + pos_control.freeze_ff_xy(); + precland_last_update_ms = precland.last_update_ms(); } #endif