From dc52f3b2dfb2097b6a0ea810280d16c21b4128d0 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 20 Jul 2016 20:45:11 +0900 Subject: [PATCH] Copter: guided mode applies acceleration limits to velocity controller --- ArduCopter/Copter.h | 1 + ArduCopter/control_guided.cpp | 48 ++++++++++++++++++++++++++++++++--- 2 files changed, 45 insertions(+), 4 deletions(-) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index aff082240b..6ea6e2953a 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -812,6 +812,7 @@ private: void guided_vel_control_run(); void guided_posvel_control_run(); void guided_angle_control_run(); + void guided_set_desired_velocity_with_accel_limits(const Vector3f& vel_des); void guided_limit_clear(); void guided_limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_max_cm, float horiz_max_cm); void guided_limit_init_time_and_pos(); diff --git a/ArduCopter/control_guided.cpp b/ArduCopter/control_guided.cpp index 79a28eb0c9..2c20614806 100644 --- a/ArduCopter/control_guided.cpp +++ b/ArduCopter/control_guided.cpp @@ -236,11 +236,10 @@ void Copter::guided_set_velocity(const Vector3f& velocity) guided_vel_control_start(); } + // record velocity target + guided_vel_target_cms = velocity; vel_update_time_ms = millis(); - // set position controller velocity target - pos_control.set_desired_velocity(velocity); - // log target Log_Write_GuidedTarget(guided_mode, Vector3f(), velocity); } @@ -440,7 +439,9 @@ 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)); + guided_set_desired_velocity_with_accel_limits(Vector3f(0.0f,0.0f,0.0f)); + } else { + guided_set_desired_velocity_with_accel_limits(guided_vel_target_cms); } // call velocity controller which includes z axis controller @@ -586,6 +587,45 @@ void Copter::guided_angle_control_run() pos_control.update_z_controller(); } +// helper function to update position controller's desired velocity while respecting acceleration limits +void Copter::guided_set_desired_velocity_with_accel_limits(const Vector3f& vel_des) +{ + // get current desired velocity + Vector3f curr_vel_des = pos_control.get_desired_velocity(); + + // exit immediately if already equal + if (curr_vel_des == vel_des) { + return; + } + + // get change in desired velocity + Vector3f vel_delta = vel_des - curr_vel_des; + + // limit xy change + float vel_delta_xy = safe_sqrt(sq(vel_delta.x)+sq(vel_delta.y)); + float vel_delta_xy_max = G_Dt * pos_control.get_accel_xy(); + float ratio_xy = 1.0f; + if (!is_zero(vel_delta_xy) && (vel_delta_xy > vel_delta_xy_max)) { + ratio_xy = vel_delta_xy_max / vel_delta_xy; + } + + // limit z change + float vel_delta_z = fabsf(vel_delta.z); + float vel_delta_z_max = G_Dt * pos_control.get_accel_z(); + float ratio_z = 1.0f; + if (!is_zero(vel_delta_z) && (vel_delta_z > vel_delta_z_max)) { + ratio_z = vel_delta_z_max / vel_delta_z; + } + + // new target + curr_vel_des.x += (vel_delta.x * ratio_xy); + curr_vel_des.y += (vel_delta.y * ratio_xy); + curr_vel_des.z += (vel_delta.z * ratio_z); + + // update position controller with new target + pos_control.set_desired_velocity(curr_vel_des); +} + // Guided Limit code // guided_limit_clear - clear/turn off guided limits