From 0d71950e418fc5b7dede71a4587f02ea3b148f5a Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Fri, 6 Aug 2021 19:11:12 +0930 Subject: [PATCH] Copter: support Control: Refactor to use Jerk --- ArduCopter/Copter.cpp | 2 +- ArduCopter/mode_guided.cpp | 12 ++++++------ 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index 329f565c8a..160369b612 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -307,7 +307,7 @@ bool Copter::set_target_posvel_NED(const Vector3f& target_pos, const Vector3f& t const Vector3f pos_neu_cm(target_pos.x * 100.0f, target_pos.y * 100.0f, -target_pos.z * 100.0f); const Vector3f vel_neu_cms(target_vel.x * 100.0f, target_vel.y * 100.0f, -target_vel.z * 100.0f); - return mode_guided.set_destination_posvel(pos_neu_cm, vel_neu_cms); + return mode_guided.set_destination_posvelaccel(pos_neu_cm, vel_neu_cms, Vector3f()); } bool Copter::set_target_velocity_NED(const Vector3f& vel_ned) diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index 7bf83c7ee1..8508e185f0 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -608,8 +608,8 @@ void ModeGuided::accel_control_run() if ((auto_yaw.mode() == AUTO_YAW_RATE) || (auto_yaw.mode() == AUTO_YAW_ANGLE_RATE)) { auto_yaw.set_rate(0.0f); } - pos_control->input_vel_accel_xy(guided_vel_target_cms.xy(), guided_accel_target_cmss.xy()); - pos_control->input_vel_accel_z(guided_vel_target_cms.z, guided_accel_target_cmss.z, false); + pos_control->input_vel_accel_xy(guided_vel_target_cms.xy(), guided_accel_target_cmss.xy(), false); + pos_control->input_vel_accel_z(guided_vel_target_cms.z, guided_accel_target_cmss.z, false, false); } else { // update position controller with new target pos_control->input_accel_xy(guided_accel_target_cmss); @@ -687,7 +687,7 @@ void ModeGuided::velaccel_control_run() guided_vel_target_cms.x = pos_control->get_vel_desired_cms().x; guided_vel_target_cms.y = pos_control->get_vel_desired_cms().y; } - pos_control->input_vel_accel_xy(guided_vel_target_cms.xy(), guided_accel_target_cmss.xy()); + pos_control->input_vel_accel_xy(guided_vel_target_cms.xy(), guided_accel_target_cmss.xy(), false); if (!stabilizing_vel_xy() && !do_avoid) { // set position and velocity errors to zero pos_control->stop_vel_xy_stabilisation(); @@ -695,7 +695,7 @@ void ModeGuided::velaccel_control_run() // set position errors to zero pos_control->stop_pos_xy_stabilisation(); } - pos_control->input_vel_accel_z(guided_vel_target_cms.z, guided_accel_target_cmss.z, false); + pos_control->input_vel_accel_z(guided_vel_target_cms.z, guided_accel_target_cmss.z, false, false); // call velocity controller which includes z axis controller pos_control->update_xy_controller(); @@ -760,7 +760,7 @@ void ModeGuided::posvelaccel_control_run() guided_pos_target_cm.x = pos_control->get_pos_target_cm().x; guided_pos_target_cm.y = pos_control->get_pos_target_cm().y; } - pos_control->input_pos_vel_accel_xy(guided_pos_target_cm.xy(), guided_vel_target_cms.xy(), guided_accel_target_cmss.xy()); + pos_control->input_pos_vel_accel_xy(guided_pos_target_cm.xy(), guided_vel_target_cms.xy(), guided_accel_target_cmss.xy(), false); if (!stabilizing_vel_xy()) { // set position and velocity errors to zero pos_control->stop_vel_xy_stabilisation(); @@ -775,7 +775,7 @@ void ModeGuided::posvelaccel_control_run() } float pz = guided_pos_target_cm.z; - pos_control->input_pos_vel_accel_z(pz, guided_vel_target_cms.z, guided_accel_target_cmss.z); + pos_control->input_pos_vel_accel_z(pz, guided_vel_target_cms.z, guided_accel_target_cmss.z, false); guided_pos_target_cm.z = pz; // run position controllers