From 70630e97747d63b08ff988e165a33db5e9583dea Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 11 Mar 2016 17:15:47 +0900 Subject: [PATCH] Copter: auto loiter handles terrain --- ArduCopter/control_auto.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/ArduCopter/control_auto.cpp b/ArduCopter/control_auto.cpp index eecf10dad0..302d390a62 100644 --- a/ArduCopter/control_auto.cpp +++ b/ArduCopter/control_auto.cpp @@ -576,8 +576,12 @@ void Copter::auto_loiter_run() // set motors to full range motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); - // run waypoint and z-axis postion controller - wp_nav.update_wpnav(); + // run waypoint and z-axis position controller + if (!wp_nav.update_wpnav()) { + // failures to update probably caused by missing terrain data + Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_UPDATE_TARGET); + // To-Do: handle failure by triggering RTL after 5 seconds with no update? + } pos_control.update_z_controller(); attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate); }