From 9e4340ba33c87e8295d0b8b7b976726d5bb04a6e Mon Sep 17 00:00:00 2001 From: bnsgeyer Date: Wed, 14 Feb 2024 19:01:40 -0500 Subject: [PATCH] Copter: change position controller method for making inputs --- ArduCopter/mode.h | 4 +++- ArduCopter/mode_systemid.cpp | 11 ++++++++++- 2 files changed, 13 insertions(+), 2 deletions(-) diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index a176f2cd9b..ab1267aaf7 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -3,6 +3,7 @@ #include "Copter.h" #include #include // TODO why is this needed if Copter.h includes this +#include class Parameters; class ParametersG2; @@ -1671,7 +1672,8 @@ private: float time_const_freq; // Time at constant frequency before chirp starts int8_t log_subsample; // Subsample multiple for logging. Vector2f target_vel; // target velocity for position controller modes - + Vector2f target_pos; // target positon + Vector2f input_vel_last; // last cycle input velocity // System ID states enum class SystemIDModeState { SYSTEMID_STATE_STOPPED, diff --git a/ArduCopter/mode_systemid.cpp b/ArduCopter/mode_systemid.cpp index bd78f7c0ae..b5de071c95 100644 --- a/ArduCopter/mode_systemid.cpp +++ b/ArduCopter/mode_systemid.cpp @@ -127,6 +127,10 @@ bool ModeSystemId::init(bool ignore_checks) if (!pos_control->is_active_z()) { pos_control->init_z_controller(); } + Vector3f curr_pos; + curr_pos = inertial_nav.get_position_neu_cm(); + target_pos.x = curr_pos.x; + target_pos.y = curr_pos.y; } att_bf_feedforward = attitude_control->get_bf_feedforward(); @@ -358,7 +362,12 @@ void ModeSystemId::run() } Vector2f accel; - pos_control->input_vel_accel_xy(input_vel, accel); + target_pos += input_vel * G_Dt; + if (is_positive(G_Dt)) { + accel = (input_vel - input_vel_last) / G_Dt; + input_vel_last = input_vel; + } + pos_control->set_pos_vel_accel_xy(target_pos.topostype(), input_vel, accel); // run pos controller pos_control->update_xy_controller();