diff --git a/ArduCopter/control_guided.cpp b/ArduCopter/control_guided.cpp index 22ae354830..6c24bf8f2c 100644 --- a/ArduCopter/control_guided.cpp +++ b/ArduCopter/control_guided.cpp @@ -15,6 +15,7 @@ static Vector3f posvel_pos_target_cm; static Vector3f posvel_vel_target_cms; static uint32_t posvel_update_time_ms; +static uint32_t vel_update_time_ms; struct Guided_Limit { uint32_t timeout_ms; // timeout (in seconds) from the time that guided is invoked @@ -138,6 +139,8 @@ void Copter::guided_set_velocity(const Vector3f& velocity) guided_vel_control_start(); } + vel_update_time_ms = millis(); + // set position controller velocity target pos_control.set_desired_velocity(velocity); } @@ -276,6 +279,12 @@ void Copter::guided_vel_control_run() } } + // set velocity to zero if no updates received for 3 seconds + uint32_t tnow = millis(); + if (tnow - vel_update_time_ms > GUIDED_POSVEL_TIMEOUT_MS && !pos_control.get_desired_velocity().is_zero()) { + pos_control.set_desired_velocity(Vector3f(0,0,0)); + } + // calculate dt float dt = pos_control.time_since_last_xy_update();